-
Notifications
You must be signed in to change notification settings - Fork 1
/
Tether_06.py
159 lines (142 loc) · 7.06 KB
/
Tether_06.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
# -*- coding: utf-8 -*-
"""
Tutorial example simulating a 3D mass-spring system with a nonlinear spring (no spring forces
for l < l_0). It uses five tether segments. The force coupling is now implemented
correctly.
"""
import numpy as np
import pylab as plt
import math
import time
from assimulo.solvers.sundials import IDA # Imports the solver IDA from Assimulo
from assimulo.problem import Implicit_Problem # Imports the problem formulation from Assimulo
G_EARTH = np.array([0.0, 0.0, -9.81]) # gravitational acceleration
C_SPRING = 614600.0 # spring constant
DAMPING = 473*0.0045 # damping [Ns/m]
L0 = 50.0 # initial segment length [m]
ALPHA0 = math.pi/10 # initial tether angle [rad]
SEGMENTS = 5
DURATION = 10 # duration of the simulation [s]
V_RO = 2.0 # reel-out speed [m/s]
D_TETHER = 4.0 # tether diameter [mm]
RHO_TETHER = 724.0 # density of Dyneema [kg/m³]
ZEROS = np.array([0.0, 0.0, 0.0])
RESULT = np.zeros(SEGMENTS * 6 + 3).reshape((-1, 3))
mass_per_meter = RHO_TETHER * SEGMENTS * (D_TETHER/2000.0)**2
# State vector y = mass0.pos, mass1.pos, mass1.vel
# Derivative yd = mass0.vel, mass1.vel, mass1.acc
# Residual res = (yd.mass0.vel), (y.mass1.vel - yd.mass1.vel), (yd.mass1.acc - G_EARTH)
# Extend Assimulos problem definition
class ExtendedProblem(Implicit_Problem):
# Set the initial conditions
t0 = 0.0 # Initial time
pos, vel, acc = [], [], []
x, y, z0 = 0.0, 0.0, 0.0
for i in range (SEGMENTS + 1):
l0 = -i*L0/SEGMENTS
pos.append(np.array([math.sin(ALPHA0) * l0, 0.0, math.cos(ALPHA0) * l0]))
vel.append(np.array([0.0, 0.0, 0.0]))
if i == 0:
acc.append(np.array([0.0, 0.0, 0.0]))
else:
acc.append(np.array([0.0, 0.0, -9.81]))
y0, yd0 = pos[0], vel[0]
for i in range (SEGMENTS):
y0 = np.append(y0, np.append(pos[i+1], vel[i+1])) # Initial state vector
yd0 = np.append(yd0, np.append(vel[i+1], acc[i+1])) # Initial state vector derivative
def res(self, t, y, yd):
y1 = y.reshape((-1, 3)) # reshape the state vector such that we can access it per 3D-vector
yd1 = yd.reshape((-1, 3))
length = L0 + V_RO*t
c_spring1 = C_SPRING / (length/SEGMENTS)
damping = DAMPING / (length/SEGMENTS)
m_tether_particle = mass_per_meter * (length/SEGMENTS)
RESULT[0] = y1[0] # the velocity of mass0 shall be zero
last_force = ZEROS # later this shall be the kite force
for i in range(SEGMENTS-2, -1, -1): # count down from segments-2 to zero
# 1. calculate the force of the lowest spring (the spring next to the kite)
res_3 = y1[2*i+4] - yd1[2*i+3] # the derivative of the position of mass1 must be equal to its velocity
rel_vel = yd1[2*i+3] - yd1[2*i+1] # calculate the relative velocity of mass2 with respect to mass 1
segment = y1[2*i+3] - y1[2*i+1] # calculate the vector from mass1 to mass0
if np.linalg.norm(segment) > length/SEGMENTS: # if the segment is not loose, calculate spring and damping force
c_spring = c_spring1
else:
c_spring = 0.0
force = c_spring * (np.linalg.norm(segment) - length/SEGMENTS) * segment / np.linalg.norm(segment) \
+ damping * rel_vel
# 2. apply it to the lowest mass (the mass next to the kite)
spring_forces = force - last_force
last_force = force
mass = m_tether_particle
acc1 = spring_forces / mass # create the vector of the spring acceleration
res_4 = yd1[2*i+4] - (G_EARTH - acc1) # the derivative of the velocity must be equal to the total acceleration
RESULT[2*i+3] = res_3
RESULT[2*i+4] = res_4
# 3. calculate the force of the spring above
res_1 = y1[2] - yd1[1] # the derivative of the position of mass1 must be equal to its velocity
rel_vel = yd1[1] - yd1[0] # calculate the relative velocity of mass1 with respect to mass 0
segment = y1[1] - y1[0] # calculate the vector from mass1 to mass0
norm = math.sqrt(segment[0]**2 + segment[1]**2 + segment[2]**2)
if np.linalg.norm(segment) > length/SEGMENTS: # if the segment is not loose, calculate spring and damping force
c_spring = c_spring1
else:
c_spring = 0.0
force = c_spring * (norm - length/SEGMENTS) * segment / norm + damping * rel_vel
# 2. apply it to the next mass nearer to the winch
spring_forces = force - last_force
mass = m_tether_particle
acc = spring_forces / mass # create the vector of the spring acceleration
res_2 = yd1[2] - (G_EARTH - acc) # the derivative of the velocity must be equal to the total acceleration
RESULT[1] = res_1
RESULT[2] = res_2
return RESULT.flatten()
def plot2d(fig, y, reltime, segments, line, sc, txt):
index = round(reltime*50)
x, z = np.zeros(segments+1), np.zeros(segments+1)
for i in range(segments):
x[i+1] = y[index, 3+6*i]
z[i+1] = y[index, 5+6*i]
z_max = np.max(z)
if line is None:
line, = plt.plot(x,z, linewidth="1")
sc = plt.scatter(x, z, s=15, color="red")
plt.pause(0.01)
txt = plt.annotate("t="+str(reltime)+" s",
xy=(segments*L0/4.2, z_max-3.0*segments/5), fontsize = 12)
plt.show(block=False)
else:
line.set_xdata(x)
line.set_ydata(z)
sc.set_offsets(np.c_[x, z])
txt.set_text("t="+str(round(reltime,1))+" s")
fig.canvas.draw()
plt.pause(0.01)
plt.show(block=False)
return line, sc, txt
def play(duration, y):
dt = 0.151
plt.ioff()
fig = plt.figure()
plt.ylim(-1.2*(L0+V_RO*duration), 0.5)
plt.xlim(-L0/2, L0/2)
plt.grid(True, color="grey", linestyle="dotted")
plt.tight_layout(rect=(0, 0, 0.98, 0.98))
line, sc, txt = None, None, None
for t in np.linspace(0, duration, num=round(duration/dt)):
line, sc, txt = plot2d(fig, y, t, SEGMENTS, line, sc, txt)
time.sleep(dt/2)
plt.show(block=True)
def run_example():
# Create an instance of the problem
model = ExtendedProblem() # Create the problem
model.name = 'Mass-Spring' # Specifies the name of problem (optional)
sim = IDA(model) # Create the solver
sim.verbosity = 0
sim.atol = 1.0e-6
sim.rtol = 1.0e-6
sim.linear_solver="SPGMR"
time, y, yd = sim.simulate(DURATION, round(DURATION*50)+2) # 50 communications points per second
play(DURATION, y)
return
if __name__ == '__main__':
run_example()