def compute_mass_matrix(dae, conf, f1, f2, f3, t1, t2, t3):
'''
take the dae that has x/z/u/p added to it already and return
the states added to it and return mass matrix and rhs of the dae residual
'''
R_b2n = dae['R_n2b'].T
r_n2b_n = C.veccat([dae['r_n2b_n_x'], dae['r_n2b_n_y'], dae['r_n2b_n_z']])
r_b2bridle_b = C.veccat([0,0,conf['zt']])
v_bn_n = C.veccat([dae['v_bn_n_x'],dae['v_bn_n_y'],dae['v_bn_n_z']])
r_n2bridle_n = r_n2b_n + C.mul(R_b2n, r_b2bridle_b)
mm00 = C.diag([1,1,1]) * (conf['mass'] + conf['tether_mass']/3.0)
mm01 = C.SXMatrix(3,3)
mm10 = mm01.T
mm02 = r_n2bridle_n
mm20 = mm02.T
J = C.blockcat([[ conf['j1'], 0, conf['j31']],
[ 0, conf['j2'], 0],
[conf['j31'], 0, conf['j3']]])
mm11 = J
mm12 = C.cross(r_b2bridle_b, C.mul(dae['R_n2b'], r_n2b_n))
mm21 = mm12.T
mm22 = C.SXMatrix(1,1)
mm = C.blockcat([[mm00,mm01,mm02],
[mm10,mm11,mm12],
[mm20,mm21,mm22]])
# right hand side
rhs0 = C.veccat([f1,f2,f3 + conf['g']*(conf['mass'] + conf['tether_mass']*0.5)])
rhs1 = C.veccat([t1,t2,t3]) - C.cross(dae['w_bn_b'], C.mul(J, dae['w_bn_b']))
# last element of RHS
R_n2b = dae['R_n2b']
w_bn_b = dae['w_bn_b']
grad_r_cdot = v_bn_n + C.mul(R_b2n, C.cross(dae['w_bn_b'], r_b2bridle_b))
tPR = - C.cross(C.mul(R_n2b, r_n2b_n), C.cross(w_bn_b, r_b2bridle_b)) - \
C.cross(C.mul(R_n2b, v_bn_n), r_b2bridle_b)
rhs2 = -C.mul(grad_r_cdot.T, v_bn_n) - C.mul(tPR.T, w_bn_b) + dae['dr']**2 + dae['r']*dae['ddr']
rhs = C.veccat([rhs0,rhs1,rhs2])
c = 0.5*(C.mul(r_n2bridle_n.T, r_n2bridle_n) - dae['r']**2)
v_bridlen_n = v_bn_n + C.mul(R_b2n, C.cross(w_bn_b, r_b2bridle_b))
cdot = C.mul(r_n2bridle_n.T, v_bridlen_n) - dae['r']*dae['dr']
a_bn_n = C.veccat([dae.ddt(name) for name in ['v_bn_n_x','v_bn_n_y','v_bn_n_z']])
dw_bn_b = C.veccat([dae.ddt(name) for name in ['w_bn_b_x','w_bn_b_y','w_bn_b_z']])
a_bridlen_n = a_bn_n + C.mul(R_b2n, C.cross(dw_bn_b, r_b2bridle_b) + C.cross(w_bn_b, C.cross(w_bn_b, r_b2bridle_b)))
cddot = C.mul(v_bridlen_n.T, v_bridlen_n) + C.mul(r_n2bridle_n.T, a_bridlen_n) - \
dae['dr']**2 - dae['r']*dae['ddr']
dae['c'] = c
dae['cdot'] = cdot
dae['cddot'] = cddot
return (mm, rhs)
def compute_mass_matrix(dae, conf, f1, f2, f3, t1, t2, t3):
'''
take the dae that has x/z/u/p added to it already and return
the states added to it and return mass matrix and rhs of the dae residual
'''
R_b2n = dae['R_n2b'].T
r_n2b_n = C.veccat([dae['r_n2b_n_x'], dae['r_n2b_n_y'], dae['r_n2b_n_z']])
r_b2bridle_b = C.veccat([0,0,conf['zt']])
v_bn_n = C.veccat([dae['v_bn_n_x'],dae['v_bn_n_y'],dae['v_bn_n_z']])
r_n2bridle_n = r_n2b_n + C.mul(R_b2n, r_b2bridle_b)
mm00 = C.diag([1,1,1]) * (conf['mass'] + conf['tether_mass']/3.0)
mm01 = C.SXMatrix(3,3)
mm10 = mm01.T
mm02 = r_n2bridle_n
mm20 = mm02.T
J = C.vertcat([C.horzcat([ conf['j1'], 0, conf['j31']]),
C.horzcat([ 0, conf['j2'], 0]),
C.horzcat([conf['j31'], 0, conf['j3']])])
mm11 = J
mm12 = C.cross(r_b2bridle_b, C.mul(dae['R_n2b'], r_n2b_n))
mm21 = mm12.T
mm22 = C.SXMatrix(1,1)
mm = C.vertcat([C.horzcat([mm00,mm01,mm02]),
C.horzcat([mm10,mm11,mm12]),
C.horzcat([mm20,mm21,mm22])])
# right hand side
rhs0 = C.veccat([f1,f2,f3 + conf['g']*(conf['mass'] + conf['tether_mass']*0.5)])
rhs1 = C.veccat([t1,t2,t3]) - C.cross(dae['w_bn_b'], C.mul(J, dae['w_bn_b']))
# last element of RHS
R_n2b = dae['R_n2b']
w_bn_b = dae['w_bn_b']
grad_r_cdot = v_bn_n + C.mul(R_b2n, C.cross(dae['w_bn_b'], r_b2bridle_b))
tPR = - C.cross(C.mul(R_n2b, r_n2b_n), C.cross(w_bn_b, r_b2bridle_b)) - \
C.cross(C.mul(R_n2b, v_bn_n), r_b2bridle_b)
rhs2 = -C.mul(grad_r_cdot.T, v_bn_n) - C.mul(tPR.T, w_bn_b) + dae['dr']**2 + dae['r']*dae['ddr']
rhs = C.veccat([rhs0,rhs1,rhs2])
c = 0.5*(C.mul(r_n2bridle_n.T, r_n2bridle_n) - dae['r']**2)
v_bridlen_n = v_bn_n + C.mul(R_b2n, C.cross(w_bn_b, r_b2bridle_b))
cdot = C.mul(r_n2bridle_n.T, v_bridlen_n) - dae['r']*dae['dr']
a_bn_n = C.veccat([dae.ddt(name) for name in ['v_bn_n_x','v_bn_n_y','v_bn_n_z']])
dw_bn_b = C.veccat([dae.ddt(name) for name in ['w_bn_b_x','w_bn_b_y','w_bn_b_z']])
a_bridlen_n = a_bn_n + C.mul(R_b2n, C.cross(dw_bn_b, r_b2bridle_b) + C.cross(w_bn_b, C.cross(w_bn_b, r_b2bridle_b)))
cddot = C.mul(v_bridlen_n.T, v_bridlen_n) + C.mul(r_n2bridle_n.T, a_bridlen_n) - \
dae['dr']**2 - dae['r']*dae['ddr']
dae['c'] = c
dae['cdot'] = cdot
dae['cddot'] = cddot
return (mm, rhs)
def add_target_orbit_constraint(mocp, p, phase_insertion):
start = lambda a: mocp.start(a)
end = lambda a: mocp.end(a)
# Target orbit - soft constraints
# 3 parameter orbit: SMA,ECC,INC
#
# h_target_magnitude - h_final_magnitude == 0
# h_target_z - h_final_z == 0
# c_target - c_final == 0
r_final_ECEF = casadi.vertcat(end(phase_insertion.rx),
end(phase_insertion.ry),
end(phase_insertion.rz))
v_final_ECEF = casadi.vertcat(end(phase_insertion.vx),
end(phase_insertion.vy),
end(phase_insertion.vz))
# Convert to ECI (z rotation is omitted, because LAN is free)
r_f = r_final_ECEF
v_f = (
v_final_ECEF +
casadi.cross(casadi.SX([0, 0, p.body_rotation_speed]), r_final_ECEF))
h_final = casadi.cross(r_f, v_f)
h_final_mag = casadi.norm_2(h_final)
h_final_z = h_final[2]
c_final = casadi.sumsqr(v_f) / 2 - 1.0 / casadi.norm_2(r_f)
defect_h_magnitude = h_final_mag - p.target_orbit_h_magnitude
defect_h_z = h_final_z - p.target_orbit_h_z
defect_c = c_final - p.target_orbit_c
slack_h_magnitude = mocp.add_variable('slack_h_magnitude', init=3.0)
slack_h_z = mocp.add_variable('slack_h_z', init=3.0)
slack_c = mocp.add_variable('slack_c', init=3.0)
mocp.add_constraint(slack_h_magnitude > 0)
mocp.add_constraint(slack_h_z > 0)
mocp.add_constraint(slack_c > 0)
mocp.add_constraint(defect_h_magnitude < slack_h_magnitude)
mocp.add_constraint(defect_h_magnitude > -slack_h_magnitude)
mocp.add_constraint(defect_h_z < slack_h_z)
mocp.add_constraint(defect_h_z > -slack_h_z)
mocp.add_constraint(defect_c < slack_c)
mocp.add_constraint(defect_c > -slack_c)
mocp.add_objective(100.0 * slack_h_magnitude)
mocp.add_objective(100.0 * slack_h_z)
mocp.add_objective(100.0 * slack_c)
def initial_guess(self, t, tf_init, params):
x_guess = self.xd()
inclination = 30.0 * ca.pi / 180.0 # the other angle than the one you're thinking of
dcmInclination = np.array(
[[ca.cos(inclination), 0.0, -ca.sin(inclination)], [0.0, 1.0, 0.0],
[ca.sin(inclination), 0.0,
ca.cos(inclination)]])
dtheta = 2.0 * ca.pi / tf_init
theta = t * dtheta
r = 0.25 * params['l']
angle = ca.SX.sym('angle')
x_cir = ca.sqrt(params['l']**2 - r**2)
y_cir = r * ca.cos(angle)
z_cir = r * ca.sin(angle)
init_pos_fun = ca.Function(
'init_pos', [angle],
[ca.mtimes(dcmInclination, ca.veccat(x_cir, y_cir, z_cir))])
init_vel_fun = init_pos_fun.jacobian()
ret = {}
ret['q'] = init_pos_fun(theta)
ret['dq'] = init_vel_fun(theta, 0) * dtheta
ret['w'] = ca.veccat(0.0, 0.0, dtheta)
norm_vel = ca.norm_2(ret['dq'])
norm_pos = ca.norm_2(ret['q'])
R0 = ret['dq'] / norm_vel
R2 = ret['q'] / norm_pos
R1 = ca.cross(ret['q'] / norm_pos, ret['dq'] / norm_vel)
ret['R'] = ca.vertcat(R0.T, R1.T, R2.T).T
return ret
def makeOrthonormal(ocp_, R):
ocp_.constrain(C.mul(R[0, :], R[0, :].T),
'==',
1,
tag=('R1[0]: e1^T * e1 == 1', None))
ocp_.constrain(C.mul(R[1, :], R[0, :].T),
'==',
0,
tag=('R1[0]: e2^T * e1 == 0', None))
ocp_.constrain(C.mul(R[1, :], R[1, :].T),
'==',
1,
tag=('R1[0]: e2^T * e2 == 1', None))
rhon = C.cross(R[0, :], R[1, :]) - R[2, :]
ocp_.constrain(rhon[0],
'==',
0,
tag=('R1[0]: ( e1^T X e2 - e3 )[0] == 0', None))
ocp_.constrain(rhon[2],
'==',
0,
tag=('R1[0]: ( e1^T X e2 - e3 )[1] == 0', None))
ocp_.constrain(rhon[1],
'==',
0,
tag=('R1[0]: ( e1^T X e2 - e3 )[2] == 0', None))
def compute_eSd(Gamma, sd, b, sym_flag = 0):
if sym_flag:
eSd = b*cs.cross(Gamma, sd)
else:
eSd = b*np.cross(Gamma, sd)
return eSd
def makeOrthonormal(g,R):
g.add(C.mul(R[0,:],R[0,:].T),'==',1, tag=('R1[0]: e1^T * e1 == 1',None))
g.add(C.mul(R[1,:],R[0,:].T),'==',0, tag=('R1[0]: e2^T * e1 == 0',None))
g.add(C.mul(R[1,:],R[1,:].T),'==',1, tag=('R1[0]: e2^T * e2 == 1',None))
rhon = C.cross(R[0,:],R[1,:]) - R[2,:]
g.add(rhon[0],'==',0, tag=('R1[0]: ( e1^T X e2 - e3 )[0] == 0',None))
g.add(rhon[2],'==',0, tag=('R1[0]: ( e1^T X e2 - e3 )[1] == 0',None))
g.add(rhon[1],'==',0, tag=('R1[0]: ( e1^T X e2 - e3 )[2] == 0',None))
def makeOrthonormal(self, g, R):
g.add(C.mul(R[0, :], R[0, :].T), '==', 1, tag = ('R1[0]: e1^T * e1 == 1', None))
g.add(C.mul(R[1, :], R[0, :].T), '==', 0, tag = ('R1[0]: e2^T * e1 == 0', None))
g.add(C.mul(R[1, :], R[1, :].T), '==', 1, tag = ('R1[0]: e2^T * e2 == 1', None))
rhon = C.cross(R[0, :], R[1, :]) - R[2, :]
g.add(rhon[0], '==', 0, tag = ('R1[0]: ( e1^T X e2 - e3 )[0] == 0', None))
g.add(rhon[2], '==', 0, tag = ('R1[0]: ( e1^T X e2 - e3 )[1] == 0', None))
g.add(rhon[1], '==', 0, tag = ('R1[0]: ( e1^T X e2 - e3 )[2] == 0', None))
def cross(a, b, axisa=-1, axisb=-1, axisc=-1, axis=None, manual=False):
"""
Return the cross product of two (arrays of) vectors.
See syntax here: https://numpy.org/doc/stable/reference/generated/numpy.cross.html
"""
if manual:
cross_x = a[1] * b[2] - a[2] * b[1]
cross_y = a[2] * b[0] - a[0] * b[2]
cross_z = a[0] * b[1] - a[1] * b[0]
return cross_x, cross_y, cross_z
if not is_casadi_type([a, b], recursive=True):
return _onp.cross(a,
b,
axisa=axisa,
axisb=axisb,
axisc=axisc,
axis=axis)
else:
if axis is not None:
if not (axis == -1 or axis == 0 or axis == 1):
raise ValueError("`axis` must be -1, 0, or 1.")
axisa = axis
axisb = axis
axisc = axis
if axisa == -1 or axisa == 1:
if not is_casadi_type(a):
a = _cas.DM(a)
a = a.T
elif axisa == 0:
pass
else:
raise ValueError("`axisa` must be -1, 0, or 1.")
if axisb == -1 or axisb == 1:
if not is_casadi_type(b):
b = _cas.DM(b)
b = b.T
elif axisb == 0:
pass
else:
raise ValueError("`axisb` must be -1, 0, or 1.")
# Compute the cross product, horizontally (along axis 1 of a 2D array)
c = _cas.cross(a, b)
if axisc == -1 or axisc == 1:
c = c.T
elif axisc == 0:
pass
else:
raise ValueError("`axisc` must be -1, 0, or 1.")
return c
def get_orthonormal_constraints(R):
ret = []
ret.append((C.mul(R[0, :], R[0, :].T) - 1, 'R1[0]: e1^T * e1 - 1 == 0'))
ret.append((C.mul(R[1, :], R[0, :].T), 'R1[0]: e2^T * e1 == 0'))
ret.append((C.mul(R[1, :], R[1, :].T) - 1, 'R1[0]: e2^T * e2 - 1 == 0'))
rhon = C.cross(R[0, :], R[1, :]) - R[2, :]
ret.append((rhon[0], 'R1[0]: ( e1^T X e2 - e3 )[0] == 0'))
ret.append((rhon[2], 'R1[0]: ( e1^T X e2 - e3 )[1] == 0'))
ret.append((rhon[1], 'R1[0]: ( e1^T X e2 - e3 )[2] == 0'))
return ret
def add_cop_constraint(self, contact, p, z, scaling=0.95):
X = scaling * contact.X
Y = scaling * contact.Y
CZ, ZG = z - contact.p, p - z
CZxZG = casadi.cross(CZ, ZG)
Dx = casadi.dot(contact.b, CZxZG) / X
Dy = casadi.dot(contact.t, CZxZG) / Y
ZGn = casadi.dot(contact.n, ZG)
slackness = Dx**2 + Dy**2 - ZGn**2
self.nlp.add_constraint(slackness, lb=[-self.nlp.infty], ub=[-0.005])
def add_linear_cop_constraints(self, contact, p, z, scaling=0.95):
GZ = z - p
for (i, v) in enumerate(contact.vertices):
v_next = contact.vertices[(i + 1) % len(contact.vertices)]
v = v + (1. - scaling) * (contact.p - v)
v_next = v_next + (1. - scaling) * (contact.p - v_next)
slackness = casadi.dot(v_next - v, casadi.cross(v - p, GZ))
self.nlp.add_constraint(slackness,
lb=[-self.nlp.infty],
ub=[-0.005])
def get_orthonormal_constraints(R):
ret = []
ret.append( (C.mul(R[0,:],R[0,:].T) - 1, 'R1[0]: e1^T * e1 - 1 == 0') )
ret.append( (C.mul(R[1,:],R[0,:].T), 'R1[0]: e2^T * e1 == 0') )
ret.append( (C.mul(R[1,:],R[1,:].T) - 1, 'R1[0]: e2^T * e2 - 1 == 0') )
rhon = C.cross(R[0,:],R[1,:]) - R[2,:]
ret.append( (rhon[0,0], 'R1[0]: ( e1^T X e2 - e3 )[0] == 0') )
ret.append( (rhon[0,2], 'R1[0]: ( e1^T X e2 - e3 )[1] == 0') )
ret.append( (rhon[0,1], 'R1[0]: ( e1^T X e2 - e3 )[2] == 0') )
return ret
def cross(u, v):
"""
:param u: 1d matrix
:type u: Matrix
:param v: 1d matrix
:type v: Matrix
:return: 1d Matrix. If u and v have length 4, it ignores the last entry and adds a zero to the result.
:rtype: Matrix
"""
return ca.cross(u, v)
def dynamics(self, aero, params):
ScalePower = params['ScalePower']
scale = 1000.
xd = self.xd
u = self.u
xa = self.xa
p = self.p
puni = self.puni
l = self.l
m = params['mK'] + 1. / 3 * params['mT'] #mass of kite and tether
g = params['g'] # gravity
A = params['sref'] # area of Kite
J = params['J'] # Kite Inertia
xddot = ca.struct_symSX(
[ca.entry('d' + name, shape=xd[name].shape) for name in xd.keys()])
[q, dq, R, w, coeff, E, Drag] = xd[...]
(Fa, M, F_tether_scaled, F_drag, F_gravity,
Tether_drag), outputs = aero(xd, xa, p, puni, l, params)
wx = skew(w[0], w[1],
w[2]) # creating a skrew matrix of the angular velocities
# Rdot = R*w (whereas w is in the skrew symmetric form)
Rconstraint = ca.reshape(xddot['dR'] - ca.mtimes(xd['R'], wx), 9,
1) # J*wdot +w x J*w = T
TorqueEq = ca.mtimes(
J, xddot['dw']) + (ca.cross(w.T,
ca.mtimes(J, w).T).T - scale *
(1 - puni['gam']) * u['T']) - puni['gam'] * M
DragForce = -Drag * R[0:3]
# ------------------------------------------------------------------------------------
# DYNAMICS of the system - Create a structure for the Differential-Algebraic Equation
# ------------------------------------------------------------------------------------
res = ca.vertcat(
xddot['dq'] - xd['dq'],\
xddot['dcoeff'] - u['dcoeff'],\
xddot['dDrag'] - u['dDrag'],\
m*(xddot['ddq'][0] + F_tether_scaled[0] - A*(1-puni['gam'])*u['u',0]) - puni['gam']*Fa[0] - F_drag[0] - Tether_drag[0], \
m*(xddot['ddq'][1] + F_tether_scaled[1] - A*(1-puni['gam'])*u['u',1]) - puni['gam']*Fa[1] - F_drag[1] - Tether_drag[1], \
m*(xddot['ddq'][2] + F_tether_scaled[2] - A*(1-puni['gam'])*u['u',2]) - puni['gam']*Fa[2] - F_drag[2] - Tether_drag[2]+ F_gravity , \
xddot['dE'] - ScalePower*ca.mtimes(F_drag.T,dq)
)
res = ca.veccat(res, Rconstraint, TorqueEq)
res = ca.veccat(res,
ca.sum1(xd['q'] * xddot['ddq']) + ca.sum1(xd['dq']**2))
# --- System dynamics function (implicit formulation)
dynamics = ca.Function('dynamics', [xd, xddot, xa, u, p, puni, l],
[res])
return dynamics
def product(cls, a, b):
assert a.shape == (4, 1) or a.shape == (4, )
assert b.shape == (4, 1) or b.shape == (4, )
r1 = a[0]
v1 = a[1:]
r2 = b[0]
v2 = b[1:]
res = ca.SX(4, 1)
res[0] = r1 * r2 - ca.dot(v1, v2)
res[1:] = r1 * v2 + r2 * v1 + ca.cross(v1, v2)
return res
def product(cls, r1, r2):
assert r1.shape == (4, 1) or r1.shape == (4, )
assert r2.shape == (4, 1) or r2.shape == (4, )
a = r1[:3]
b = r2[:3]
na_sq = ca.dot(a, a)
nb_sq = ca.dot(b, b)
res = ca.SX(4, 1)
den = (1 + na_sq * nb_sq - 2 * ca.dot(b, a))
res[:3] = ((1 - na_sq) * b +
(1 - nb_sq) * a - 2 * ca.cross(b, a)) / den
res[3] = 0 # shadow state
return res
def makeModel(conf, propertiesDir = '../properties'):
print "\n#### File '%s' is old. It does not use the NED convention you should use carouselModel in offline_mhe_stuff instead.\n" \
% inspect.getfile(inspect.currentframe())
#input("Press Enter if you wish to continue...")
dae = rawe.models.carousel(conf)
(xDotSol, zSol) = dae.solveForXDotAndZ()
ddp = C.vertcat([xDotSol['dx'],xDotSol['dy'],xDotSol['dz']])
ddt_w_bn_b = C.vertcat([xDotSol['w_bn_b_x'],xDotSol['w_bn_b_y'],xDotSol['w_bn_b_z']])
x = dae['x']
y = dae['y']
dx = dae['dx']
dy = dae['dy']
ddelta = dae['ddelta']
dddelta = xDotSol['ddelta']
R = C.veccat( [dae[n] for n in ['e11', 'e12', 'e13',
'e21', 'e22', 'e23',
'e31', 'e32', 'e33']]
).reshape((3,3))
rA = conf['rArm']
g = conf['g']
pIMU = C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'IMU/pIMU.dat')))
RIMU = C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'IMU/RIMU.dat')))
ddpIMU = C.mul(R.T,ddp) - ddelta**2*C.mul(R.T,C.vertcat([x+rA,y,0])) + 2*ddelta*C.mul(R.T,C.vertcat([-dy,dx,0])) + dddelta*C.mul(R.T,C.vertcat([-y,x+rA,0])) + C.mul(R.T,C.vertcat([0,0,g]))
aBridle = C.cross(ddt_w_bn_b,pIMU)
dae['IMU_acceleration'] = C.mul(RIMU,ddpIMU+aBridle)
dae['IMU_angular_velocity'] = C.mul(RIMU,dae['w_bn_b'])
camConf = {'PdatC1':C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'cameras/PC1.dat'))),
'PdatC2':C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'cameras/PC2.dat'))),
'RPC1':C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'cameras/RPC1.dat'))),
'RPC2':C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'cameras/RPC2.dat'))),
'pos_marker_body1':C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'markers/pos_marker_body1.dat'))),
'pos_marker_body2':C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'markers/pos_marker_body2.dat'))),
'pos_marker_body3':C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'markers/pos_marker_body3.dat')))}
dae['marker_positions'] = camModel.fullCamModel(dae,camConf)
dae['ConstR1'] = dae['e11']*dae['e11'] + dae['e12']*dae['e12'] + dae['e13']*dae['e13'] - 1
dae['ConstR2'] = dae['e11']*dae['e21'] + dae['e12']*dae['e22'] + dae['e13']*dae['e23']
dae['ConstR3'] = dae['e11']*dae['e31'] + dae['e12']*dae['e32'] + dae['e13']*dae['e33']
dae['ConstR4'] = dae['e21']*dae['e21'] + dae['e22']*dae['e22'] + dae['e23']*dae['e23'] - 1
dae['ConstR5'] = dae['e21']*dae['e31'] + dae['e22']*dae['e32'] + dae['e23']*dae['e33']
dae['ConstR6'] = dae['e31']*dae['e31'] + dae['e32']*dae['e32'] + dae['e33']*dae['e33'] - 1
#dae['ConstDelta'] = dae['cos_delta']*dae['cos_delta'] + dae['sin_delta']*dae['sin_delta'] - 1
dae['ConstDelta'] = ( dae['cos_delta']**2 + dae['sin_delta']**2 - 1 )
return dae
def compute_x_tilde(x, x_ref, state_space):
if state_space == 'longitudinal':
Vr = x[0]
alpha = x[1]
theta = x[2]
q = x[3]
Vr_ref = x_ref[0]
theta_ref = x_ref[1]
x_tilde = cs.vertcat(Vr - Vr_ref, alpha, theta - theta_ref, q)
elif state_space == 'full':
Vr_err = x[0] - x_ref[0]
q = x[3:7]
q_ref = x_ref[1:5]
# Compute reduced attitudes
R_d = cg.rotation_quaternion(q_ref)
R = cg.rotation_quaternion(q)
b = R_d[:, 0]
Gamma_d = R_d.T @ b
Gamma = R.T @ b
# Cost functions
q_err = 1 - cs.dot(cs.vertcat(1, 0, 0), Gamma)
# q_err = 1 - Gamma[0]
q_err = cs.vertcat(q_err, cs.cross(Gamma_d, Gamma))
qw = q[0]
qx = q[1]
qy = q[2]
qz = q[3]
phi = cs.arctan2(2 * (qw * qx + qy * qz), 1 - 2 * (qx**2 + qy**2))
omega_s = x[7:10]
beta = x[1]
x_tilde = cs.vertcat(Vr_err, q_err, omega_s, phi, beta)
elif state_space == 'lateral':
print('Not implemented state_space: ' + state_space)
else:
print('Unknown state_space: ' + state_space)
return x_tilde
def casadi_ode(self, t, x, u, w):
"""
Basic dynamics of xdot for unicycle
"""
tau = u[1:]
thrust = self.mass * u[0]
e3 = cs.DM([0, 0, 1])
v = x[3:6]
rpy = x[6:9]
R = self.euler2mat(rpy)
omega = x[9:12]
J_omega_dot = cs.cross(cs.mtimes(self.J, omega), omega) + tau
thrust_vec = cs.mtimes(R, e3)
acceleration = (1.0 / self.mass) * (thrust_vec * thrust + self.mg)
omega_dot = cs.mtimes(self.Jinv, J_omega_dot)
rpy_dot = self.omegaToRpyDot(rpy, omega)
return cs.vertcat(v, acceleration, rpy_dot, omega_dot) + w
def dot_x_stability(t, quat, Vr, alpha, beta, s_omega, u, model):
P = model.P
R_sb = cg.rotation_sb(alpha)
R_ws = cg.rotation_ws(beta)
R_wb = cg.rotation_wb(alpha, beta)
b_omega = R_sb.T @ s_omega
p = b_omega[0]
q = b_omega[1]
r = b_omega[2]
delta_a = u[0]
delta_e = u[1]
delta_r = u[2]
delta_t = u[3]
b_tau = compute_force(0, quat, Vr, alpha, beta, b_omega, u, model)
b_F = b_tau[:3]
b_M = b_tau[3:]
w_F = R_wb @ b_F
w_omega = R_wb @ b_omega
w_v_rel = cs.SX.zeros(3, 1)
w_v_rel[0] = Vr
# w_v_rel = np.array((Vr,0,0)) # [Vr, 0, 0].T
# rhs = w_F/P['mass'] - cg.Smtrx(w_omega)@w_v_rel
rhs = w_F / P['mass'] - cs.cross(w_omega, w_v_rel)
Vr_dot = rhs[0]
beta_dot = rhs[1] / Vr
alpha_dot = rhs[2] / (Vr * cs.cos(beta))
b_J_cg = P['I_cg']
dot_s_omega = dot_s_angvel_bn(alpha, alpha_dot, b_M, b_omega, b_J_cg)
dot_quat_nb = cg.quaternion_dot(quat, b_omega)
# dot_q_nb = dot_q_nb(:);
s_x_dot = cs.vertcat(Vr_dot, beta_dot, alpha_dot, dot_quat_nb, dot_s_omega)
return s_x_dot
def add_linear_cop_constraints(self, p, z, foothold, scaling=0.95):
"""
Constraint the COP, located between the COM `p` and the (floating-base)
ZMP `z`, to lie inside the contact area.
Parameters
----------
p : casadi.MX
Symbol of COM position variable.
z : casadi.MX
Symbol of ZMP position variable.
scaling : scalar, optional
Scaling factor between 0 and 1 applied to the contact area.
"""
GZ = z - p
nb_vert = len(foothold.vertices)
for (i, v) in enumerate(foothold.vertices):
v_next = foothold.vertices[(i + 1) % nb_vert]
v = v + (1. - scaling) * (foothold.p - v)
v_next = v_next + (1. - scaling) * (foothold.p - v_next)
slackness = casadi.dot(v_next - v, casadi.cross(v - p, GZ))
self.nlp.add_constraint(
slackness, lb=[-self.nlp.infty], ub=[-0.0005])
def solve_launch_direction_problem(p):
# Find the approximate launch direction vector.
# The launch direction vector is horizontal (orthogonal to the launch site position vector).
# The launch direction vector points (approximately) in the direction of the orbit insertion velocity.
mocp = MOCP()
rx = mocp.add_variable('rx', init=p.launch_site_x)
ry = mocp.add_variable('ry', init=p.launch_site_y)
rz = mocp.add_variable('rz', init=p.launch_site_z)
vx = mocp.add_variable('vx', init=0.01)
vy = mocp.add_variable('vy', init=0.01)
vz = mocp.add_variable('vz', init=0.01)
mocp.add_objective((rx - p.launch_site_x)**2)
mocp.add_objective((ry - p.launch_site_y)**2)
mocp.add_objective((rz - p.launch_site_z)**2)
r = casadi.vertcat(rx, ry, rz)
v = casadi.vertcat(vx, vy, vz)
h = casadi.cross(r, v)
h_mag = casadi.norm_2(h)
h_z = h[2]
c = casadi.sumsqr(v) / 2 - 1.0 / casadi.norm_2(r)
mocp.add_objective((h_mag - p.target_orbit_h_magnitude)**2)
mocp.add_objective((h_z - p.target_orbit_h_z)**2)
mocp.add_objective((c - p.target_orbit_c)**2)
mocp.solve()
v_soln = DM([mocp.get_value(vx), mocp.get_value(vy), mocp.get_value(vz)])
launch_direction = normalize(v_soln)
up_direction = normalize(
DM([p.launch_site_x, p.launch_site_y, p.launch_site_z]))
launch_direction = launch_direction - (
launch_direction.T @ up_direction) * up_direction
return launch_direction
def makeDae( conf = None ):
if conf is None:
conf = arianne_conf.makeConf()
# Make model
dae = rawe.models.carousel(conf)
(xDotSol, zSol) = dae.solveForXDotAndZ()
# Get variables and outputs from the model
ddp = C.vertcat([xDotSol['dx'],xDotSol['dy'],xDotSol['dz']])
ddt_w_bn_b = C.vertcat([xDotSol['w_bn_b_x'],xDotSol['w_bn_b_y'],xDotSol['w_bn_b_z']])
x = dae['x']
y = dae['y']
z = dae['z']
e31 = dae['e31']
e32 = dae['e32']
e33 = dae['e33']
zt = conf['zt']
dx = dae['dx']
dy = dae['dy']
ddelta = dae['ddelta']
dddelta = xDotSol['ddelta']
R = dae['R_c2b']
rA = conf['rArm']
g = conf['g']
# Rotation matrix to convert from NWU to NED frame type
R_nwu2ned = np.eye( 3 )
R_nwu2ned[1, 1] = R_nwu2ned[2, 2] = -1.0
############################################################################
#
# IMU model
#
############################################################################
# Load IMU position and orientation w.r.t. body frame
# pIMU = C.mul(R_nwu2ned, C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'IMU/pIMU.dat'))))
pIMU = C.DMatrix([0,0,0])
pIMU = C.mul(R_nwu2ned, pIMU)
#0
#0
#0
# RIMU = C.mul(R_nwu2ned, C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'IMU/RIMU.dat'))))
#9.937680e-01 6.949103e-02 8.715574e-02
#-6.975647e-02 9.975641e-01 0
#-8.694344e-02 -6.079677e-03 9.961947e-01
RIMU = C.DMatrix([[9.937680e-01, 6.949103e-02, 8.715574e-02],
[-6.975647e-02, 9.975641e-01, 0],
[-8.694344e-02, -6.079677e-03, 9.961947e-01]])
RIMU = C.mul(R_nwu2ned, RIMU)
# Define IMU measurement functions
# TODO here is omitted the term: w x w pIMU
# The sign of gravity is negative because of the NED convention (z points down!)
ddpIMU_c = ddp - ddelta ** 2 * C.vertcat([x + rA, y, 0]) + 2 * ddelta * C.vertcat([-dy, dx, 0]) + \
dddelta * C.vertcat([-y, x + rA, 0]) - C.vertcat([0, 0, g])
ddpIMU = C.mul(R, ddpIMU_c)
aBridle = C.cross(ddt_w_bn_b, pIMU)
ddpIMU += aBridle
ddpIMU = C.mul(RIMU,ddpIMU)
# You can add a parameter to conf which will give the model 3 extra states with derivative 0 (to act as parameter) for the bias in the acceleration measurements. If that is present, it should be added to the measurement of the acceleration
if 'useIMUAccelerationBias' in conf and conf['useIMUAccelerationBias']:
IMUAccelerationBias = C.vertcat([dae['IMUAccelerationBias1'],dae['IMUAccelerationBias2'],dae['IMUAccelerationBias3']])
ddpIMU += IMUAccelerationBias
# For the accelerometers
dae['IMU_acceleration'] = ddpIMU
dae['IMU_acceleration_x'] = dae['IMU_acceleration'][0]
dae['IMU_acceleration_y'] = dae['IMU_acceleration'][1]
dae['IMU_acceleration_z'] = dae['IMU_acceleration'][2]
# ... and for the gyroscopes
dae['IMU_angular_velocity'] = C.mul(RIMU, dae['w_bn_b'])
dae['IMU_angular_velocity_x'] = dae['IMU_angular_velocity'][0]
dae['IMU_angular_velocity_y'] = dae['IMU_angular_velocity'][1]
dae['IMU_angular_velocity_z'] = dae['IMU_angular_velocity'][2]
if 'kinematicIMUAccelerationModel' in conf and conf['kinematicIMUAccelerationModel']:
dae['vel_error_x'] = dae['dx']-dae['dx_IMU']
dae['vel_error_y'] = dae['dy']-dae['dy_IMU']
dae['vel_error_z'] = dae['dz']-dae['dz_IMU']
############################################################################
#
# LAS
#
############################################################################
x_tether = (x + zt*e31)
y_tether = (y + zt*e32)
z_tether = (z + zt*e33)
dae['lineAngles'] = C.vertcat([C.arctan(y_tether/x_tether),C.arctan(z_tether/x_tether)])
dae['lineAngle_hor'] = dae['lineAngles'][0]
dae['lineAngle_ver'] = dae['lineAngles'][1]
return dae
dx_bc = x_bN - x_cN
# Final velocity
v_bN = V['X',N_sim,ca.veccat,['vx_b','vy_b']]
v_cN = V['X',N_sim,ca.veccat,['vx_c','vy_c']]
dv_bc = v_bN - v_cN
# Angle between gaze and ball velocity
theta = V['X',N_sim,'theta']
phi = V['X',N_sim,'phi']
d = ca.veccat([ ca.cos(theta)*ca.cos(phi), ca.cos(theta)*ca.sin(phi), ca.sin(theta) ])
r = V['X',N_sim,ca.veccat,['vx_b','vy_b','vz_b']]
?????
delta = ca.arctan2(ca.norm_2(ca.cross()))
r_unit = r / (ca.norm_2(r) + 1e-2)
d_gaze = d - r_unit
# Regularize controls
J = 1e-1 * ca.sum_square(ca.veccat(V['U'])) * dt
# Multiple shooting constraints and non-linear control constraints
g, lbg, ubg = [], [], []
for k in range(N_sim):
# Multiple shooting
[x_next] = F([V['X',k], V['U',k], dt])
g.append(x_next - V['X',k+1])
lbg.append(ca.DMatrix.zeros(nx))
ubg.append(ca.DMatrix.zeros(nx))
def __init__(self, inertial_frame_id='world'):
Vehicle.__init__(self, inertial_frame_id)
# Declaring state variables
## Generalized position vector
self.eta = casadi.SX.sym('eta', 6)
## Generalized velocity vector
self.nu = casadi.SX.sym('nu', 6)
# Build the Coriolis matrix
self.CMatrix = casadi.SX.zeros(6, 6)
S_12 = - cross_product_operator(
casadi.mtimes(self._Mtotal[0:3, 0:3], self.nu[0:3]) +
casadi.mtimes(self._Mtotal[0:3, 3:6], self.nu[3:6]))
S_22 = - cross_product_operator(
casadi.mtimes(self._Mtotal[3:6, 0:3], self.nu[0:3]) +
casadi.mtimes(self._Mtotal[3:6, 3:6], self.nu[3:6]))
self.CMatrix[0:3, 3:6] = S_12
self.CMatrix[3:6, 0:3] = S_12
self.CMatrix[3:6, 3:6] = S_22
# Build the damping matrix (linear and nonlinear elements)
self.DMatrix = - casadi.diag(self._linear_damping)
self.DMatrix -= casadi.diag(self._linear_damping_forward_speed)
self.DMatrix -= casadi.diag(self._quad_damping * self.nu)
# Build the restoring forces vectors wrt the BODY frame
Rx = np.array([[1, 0, 0],
[0, casadi.cos(self.eta[3]), -1 * casadi.sin(self.eta[3])],
[0, casadi.sin(self.eta[3]), casadi.cos(self.eta[3])]])
Ry = np.array([[casadi.cos(self.eta[4]), 0, casadi.sin(self.eta[4])],
[0, 1, 0],
[-1 * casadi.sin(self.eta[4]), 0, casadi.cos(self.eta[4])]])
Rz = np.array([[casadi.cos(self.eta[5]), -1 * casadi.sin(self.eta[5]), 0],
[casadi.sin(self.eta[5]), casadi.cos(self.eta[5]), 0],
[0, 0, 1]])
R_n_to_b = casadi.transpose(casadi.mtimes(Rz, casadi.mtimes(Ry, Rx)))
if inertial_frame_id == 'world_ned':
Fg = casadi.SX([0, 0, -self.mass * self.gravity])
Fb = casadi.SX([0, 0, self.volume * self.gravity * self.density])
else:
Fg = casadi.SX([0, 0, self.mass * self.gravity])
Fb = casadi.SX([0, 0, -self.volume * self.gravity * self.density])
self.gVec = casadi.SX.zeros(6)
self.gVec[0:3] = -1 * casadi.mtimes(R_n_to_b, Fg + Fb)
self.gVec[3:6] = -1 * casadi.mtimes(
R_n_to_b, casadi.cross(self._cog, Fg) + casadi.cross(self._cob, Fb))
# Build Jacobian
T = 1 / casadi.cos(self.eta[4]) * np.array(
[[0, casadi.sin(self.eta[3]) * casadi.sin(self.eta[4]), casadi.cos(self.eta[3]) * casadi.sin(self.eta[4])],
[0, casadi.cos(self.eta[3]) * casadi.cos(self.eta[4]), -casadi.cos(self.eta[4]) * casadi.sin(self.eta[3])],
[0, casadi.sin(self.eta[3]), casadi.cos(self.eta[3])]])
self.eta_dot = casadi.vertcat(
casadi.mtimes(casadi.transpose(R_n_to_b), self.nu[0:3]),
casadi.mtimes(T, self.nu[3::]))
self.u = casadi.SX.sym('u', 6)
self.nu_dot = casadi.solve(
self._Mtotal,
self.u - casadi.mtimes(self.CMatrix, self.nu) - casadi.mtimes(self.DMatrix, self.nu) - self.gVec)
sref = conf['sref']
bref = conf['bref']
cref = conf['cref']
# airfoil speed in wind frame
v_bw_n_x = v_bw_n[0]
v_bw_n_y = v_bw_n[1]
v_bw_n_z = v_bw_n[2]
vKite2 = C.mul(v_bw_n.trans(), v_bw_n) #Airfoil speed^2
vKite = C.sqrt(vKite2) #Airfoil speed
dae['airspeed'] = vKite
# Lift axis, normed to airspeed
eLe_v = C.cross(C.veccat([eTe1, eTe2, eTe3]), v_bw_n)
# sideforce axis, normalized to airspeed^2
eYe_v2 = C.cross(eLe_v, -v_bw_n)
# Relative wind speed in Airfoil's referential 'E'
v_bw_b_x = v_bw_b[0]
v_bw_b_y = v_bw_b[1]
v_bw_b_z = v_bw_b[2]
if conf['alpha_beta_computation'] == 'first_order':
alpha = alpha0 + v_bw_b_z / v_bw_b_x
beta = v_bw_b_y / v_bw_b_x
# beta = v_bw_b_y / C.sqrt(v_bw_b_x*v_bw_b_x + v_bw_b_z*v_bw_b_z)
elif conf['alpha_beta_computation'] == 'closed_form':
(alpha, beta) = getWindAnglesFrom_v_bw_b(vKite, v_bw_b)
def __init__(self, inertial_frame_id='world'):
Vehicle.__init__(self, inertial_frame_id)
# Declaring state variables
## Generalized position vector
self.eta = casadi.SX.sym('eta', 6)
## Generalized velocity vector
self.nu = casadi.SX.sym('nu', 6)
# Build the Coriolis matrix
self.CMatrix = casadi.SX.zeros(6, 6)
S_12 = -cross_product_operator(
casadi.mtimes(self._Mtotal[0:3, 0:3], self.nu[0:3]) +
casadi.mtimes(self._Mtotal[0:3, 3:6], self.nu[3:6]))
S_22 = -cross_product_operator(
casadi.mtimes(self._Mtotal[3:6, 0:3], self.nu[0:3]) +
casadi.mtimes(self._Mtotal[3:6, 3:6], self.nu[3:6]))
self.CMatrix[0:3, 3:6] = S_12
self.CMatrix[3:6, 0:3] = S_12
self.CMatrix[3:6, 3:6] = S_22
# Build the damping matrix (linear and nonlinear elements)
self.DMatrix = -casadi.diag(self._linear_damping)
self.DMatrix -= casadi.diag(self._linear_damping_forward_speed)
self.DMatrix -= casadi.diag(self._quad_damping * self.nu)
# Build the restoring forces vectors wrt the BODY frame
Rx = np.array(
[[1, 0, 0],
[0, casadi.cos(self.eta[3]), -1 * casadi.sin(self.eta[3])],
[0, casadi.sin(self.eta[3]),
casadi.cos(self.eta[3])]])
Ry = np.array(
[[casadi.cos(self.eta[4]), 0,
casadi.sin(self.eta[4])], [0, 1, 0],
[-1 * casadi.sin(self.eta[4]), 0,
casadi.cos(self.eta[4])]])
Rz = np.array(
[[casadi.cos(self.eta[5]), -1 * casadi.sin(self.eta[5]), 0],
[casadi.sin(self.eta[5]),
casadi.cos(self.eta[5]), 0], [0, 0, 1]])
R_n_to_b = casadi.transpose(casadi.mtimes(Rz, casadi.mtimes(Ry, Rx)))
if inertial_frame_id == 'world_ned':
Fg = casadi.SX([0, 0, -self.mass * self.gravity])
Fb = casadi.SX([0, 0, self.volume * self.gravity * self.density])
else:
Fg = casadi.SX([0, 0, self.mass * self.gravity])
Fb = casadi.SX([0, 0, -self.volume * self.gravity * self.density])
self.gVec = casadi.SX.zeros(6)
self.gVec[0:3] = -1 * casadi.mtimes(R_n_to_b, Fg + Fb)
self.gVec[3:6] = -1 * casadi.mtimes(
R_n_to_b,
casadi.cross(self._cog, Fg) + casadi.cross(self._cob, Fb))
# Build Jacobian
T = 1 / casadi.cos(self.eta[4]) * np.array(
[[
0,
casadi.sin(self.eta[3]) * casadi.sin(self.eta[4]),
casadi.cos(self.eta[3]) * casadi.sin(self.eta[4])
],
[
0,
casadi.cos(self.eta[3]) * casadi.cos(self.eta[4]),
-casadi.cos(self.eta[4]) * casadi.sin(self.eta[3])
], [0, casadi.sin(self.eta[3]),
casadi.cos(self.eta[3])]])
self.eta_dot = casadi.vertcat(
casadi.mtimes(casadi.transpose(R_n_to_b), self.nu[0:3]),
casadi.mtimes(T, self.nu[3::]))
self.u = casadi.SX.sym('u', 6)
self.nu_dot = casadi.solve(
self._Mtotal, self.u - casadi.mtimes(self.CMatrix, self.nu) -
casadi.mtimes(self.DMatrix, self.nu) - self.gVec)
def rocket_equations(jit=True):
x = ca.SX.sym('x', 14)
u = ca.SX.sym('u', 4)
p = ca.SX.sym('p', 16)
t = ca.SX.sym('t')
dt = ca.SX.sym('dt')
# State: x
# body frame: Forward, Right, Down
omega_b = x[0:3] # inertial angular velocity expressed in body frame
r_nb = x[3:7] # modified rodrigues parameters
v_b = x[7:10] # inertial velocity expressed in body components
p_n = x[10:13] # positon in nav frame
m_fuel = x[13] # mass
# Input: u
m_dot = ca.if_else(m_fuel > 0, u[0], 0)
fin = u_to_fin(u)
# Parameters: p
g = p[0] # gravity
Jx = p[1] # moment of inertia
Jy = p[2]
Jz = p[3]
Jxz = p[4]
ve = p[5]
l_fin = p[6]
w_fin = p[7]
CL_alpha = p[8]
CL0 = p[9]
CD0 = p[10]
K = p[11]
s_fin = p[12]
rho = p[13]
m_empty = p[14]
l_motor = p[15]
# Calculations
m = m_empty + m_fuel
J_b = ca.SX.zeros(3, 3)
J_b[0, 0] = Jx + m_fuel * l_motor**2
J_b[1, 1] = Jy + m_fuel * l_motor**2
J_b[2, 2] = Jz
J_b[0, 2] = J_b[2, 0] = Jxz
C_nb = so3.Dcm.from_mrp(r_nb)
g_n = ca.vertcat(0, 0, g)
v_n = ca.mtimes(C_nb, v_b)
# aerodynamics
VT = ca.norm_2(v_b)
q = 0.5 * rho * ca.dot(v_b, v_b)
fins = {
'top': {
'fwd': [1, 0, 0],
'up': [0, 1, 0],
'angle': fin[0]
},
'left': {
'fwd': [1, 0, 0],
'up': [0, 0, -1],
'angle': fin[1]
},
'down': {
'fwd': [1, 0, 0],
'up': [0, -1, 0],
'angle': fin[2]
},
'right': {
'fwd': [1, 0, 0],
'up': [0, 0, 1],
'angle': fin[3]
},
}
rel_wind_dir = v_b / VT
# build fin lift/drag forces
vel_tol = 1e-3
FA_b = ca.vertcat(0, 0, 0)
MA_b = ca.vertcat(0, 0, 0)
for key, data in fins.items():
fwd = data['fwd']
up = data['up']
angle = data['angle']
U = ca.dot(fwd, v_b)
W = ca.dot(up, v_b)
side = ca.cross(fwd, up)
alpha = ca.if_else(ca.fabs(U) > vel_tol, -ca.atan(W / U), 0)
perp_wind_dir = ca.cross(side, rel_wind_dir)
norm_perp = ca.norm_2(perp_wind_dir)
perp_wind_dir = ca.if_else(
ca.fabs(norm_perp) > vel_tol, perp_wind_dir / norm_perp, up)
CL = CL0 + CL_alpha * (alpha + angle)
CD = CD0 + K * (CL - CL0)**2
# model stall as no lift if above 23 deg.
L = ca.if_else(ca.fabs(alpha) < 0.4, CL * q * s_fin, 0)
D = CD * q * s_fin
FAi_b = L * perp_wind_dir - D * rel_wind_dir
FA_b += FAi_b
MA_b += ca.cross(-l_fin * fwd - w_fin * side, FAi_b)
FA_b = ca.if_else(ca.fabs(VT) > vel_tol, FA_b, ca.SX.zeros(3))
MA_b = ca.if_else(ca.fabs(VT) > vel_tol, MA_b, ca.SX.zeros(3))
# propulsion
FP_b = ca.vertcat(m_dot * ve, 0, 0)
# force and momental total
F_b = FA_b + FP_b + ca.mtimes(C_nb.T, m * g_n)
M_b = MA_b
force_moment = ca.Function('force_moment', [x, u, p], [F_b, M_b],
['x', 'u', 'p'], ['F_b', 'M_b'])
# right hand side
rhs = ca.Function('rhs', [x, u, p], [
ca.vertcat(
ca.mtimes(ca.inv(J_b),
M_b - ca.cross(omega_b, ca.mtimes(J_b, omega_b))),
so3.Mrp.kinematics(r_nb, omega_b),
F_b / m - ca.cross(omega_b, v_b), ca.mtimes(C_nb, v_b), -m_dot)
], ['x', 'u', 'p'], ['rhs'], {'jit': jit})
# prediction
t0 = ca.SX.sym('t0')
h = ca.SX.sym('h')
x0 = ca.SX.sym('x', 14)
x1 = rk4(lambda t, x: rhs(x, u, p), t0, x0, h)
x1[3:7] = so3.Mrp.shadow_if_necessary(x1[3:7])
predict = ca.Function('predict', [x0, u, p, t0, h], [x1], {'jit': jit})
def schedule(t, start, ty_pairs):
val = start
for ti, yi in ty_pairs:
val = ca.if_else(t > ti, yi, val)
return val
# reference trajectory
pitch_d = 1.0
euler = so3.Euler.from_mrp(r_nb) # roll, pitch, yaw
pitch = euler[1]
# control
u_control = ca.SX.zeros(4)
# these controls are just test controls to make sure the fins are working
u_control[0] = 0.1 # mass flow rate
import control
s = control.tf([1, 0], [0, 1])
H = 140 * (s + 50) * (s + 50) / (s * (2138 * s + 208.8))
Hd = control.tf2ss(control.c2d(H, 0.01))
theta_c = (100 - p_n[2]) * (0.01) / (v_b[2] * ca.cos(p_n[2]))
x_elev = ca.SX.sym('x_elev', 2)
u_elev = ca.SX.sym('u_elev', 1)
x_1 = ca.mtimes(Hd.A, x_elev) + ca.mtimes(Hd.B, u_elev)
y_elev = ca.mtimes(Hd.C, x_elev) + ca.mtimes(Hd.D, u_elev)
elev_c = (theta_c - p_n[2]) * y_elev / (0.5 * rho * v_b[2]**2)
u_control[0] = 0.1 # mass flow rate
u_control[1] = 0
u_control[2] = elev_c
u_control[3] = 0
control = ca.Function('control', [x, p, t, dt], [u_control],
['x', 'p', 't', 'dt'], ['u'])
# initialize
pitch_deg = ca.SX.sym('pitch_deg')
omega0_b = ca.vertcat(0, 0, 0)
r0_nb = so3.Mrp.from_euler(ca.vertcat(0, pitch_deg * ca.pi / 180, 0))
v0_b = ca.vertcat(0, 0, 0)
p0_n = ca.vertcat(0, 0, 0)
m0_fuel = 0.8
# x: omega_b, r_nb, v_b, p_n, m_fuel
x0 = ca.vertcat(omega0_b, r0_nb, v0_b, p0_n, m0_fuel)
# g, Jx, Jy, Jz, Jxz, ve, l_fin, w_fin, CL_alpha, CL0, CD0, K, s, rho, m_emptpy, l_motor
p0 = [
9.8, 0.05, 1.0, 1.0, 0.0, 350, 1.0, 0.05, 2 * np.pi, 0, 0.01, 0.01,
0.05, 1.225, 0.2, 1.0
]
initialize = ca.Function('initialize', [pitch_deg], [x0, p0])
return {
'rhs': rhs,
'predict': predict,
'control': control,
'initialize': initialize,
'force_moment': force_moment,
'x': x,
'u': u,
'p': p
}
return rhs, x, u, p
def __init__(self,
nb_steps,
state_estimator,
com_target,
contact_sequence,
omega2,
swing_duration=None):
super(WrenchPredictiveController, self).__init__()
t_build_start = time()
end_com = list(com_target.p)
end_comd = list(com_target.pd)
nb_contacts = len(contact_sequence)
start_com = list(state_estimator.com)
start_comd = list(state_estimator.comd)
p_0 = self.nlp.new_constant('p_0', 3, start_com)
v_0 = self.nlp.new_constant('v_0', 3, start_comd)
p_k = p_0
v_k = v_0
T_swing = 0
T_total = 0
for k in range(nb_steps):
contact = contact_sequence[nb_contacts * k / nb_steps]
u_k = self.nlp.new_variable('u_%d' % k,
3,
init=[0., 0., 0.],
lb=self.u_min,
ub=self.u_max)
dT_k = self.nlp.new_variable('dT_%d' % k,
1,
init=[self.dT_init],
lb=[self.dT_min],
ub=[self.dT_max])
l_k = self.nlp.new_variable('l_%d' % k,
16,
init=[0.] * 16,
lb=[0.] * 16,
ub=self.l_max)
c = contact.p
w_k = casadi.mtimes(contact.wrench_span, l_k)
f_k, tau_k = w_k[:3, :], w_k[3:, :]
Ld_k = casadi.cross(c - p_k, f_k) + tau_k
m = state_estimator.pendulum.com_state.mass
self.nlp.add_equality_constraint(u_k, f_k / m + gravity)
# hard angular-momentum constraint just don't work
# nlp.add_constraint(Ld_k, lb=[0., 0., 0.], ub=[0., 0., 0.])
self.nlp.extend_cost(self.weights['Ld'] * casadi.dot(Ld_k, Ld_k) *
dT_k)
self.nlp.extend_cost(self.weights['l'] * casadi.dot(l_k, l_k) *
dT_k)
self.nlp.extend_cost(self.weights['u'] * casadi.dot(u_k, u_k) *
dT_k)
p_next = p_k + v_k * dT_k + u_k * dT_k**2 / 2
v_next = v_k + u_k * dT_k
T_total = T_total + dT_k
if 2 * k < nb_steps:
T_swing = T_swing + dT_k
p_k = self.nlp.new_variable('p_%d' % (k + 1),
3,
init=start_com,
lb=self.p_min,
ub=self.p_max)
v_k = self.nlp.new_variable('v_%d' % (k + 1),
3,
init=start_comd,
lb=self.v_min,
ub=self.v_max)
self.nlp.add_equality_constraint(p_next, p_k)
self.nlp.add_equality_constraint(v_next, v_k)
self.nlp.add_constraint(T_swing,
lb=[swing_duration],
ub=[100],
name='T_swing')
p_last, v_last = p_k, v_k
p_diff = p_last - end_com
v_diff = v_last - end_comd
self.nlp.extend_cost(self.weights['p'] * casadi.dot(p_diff, p_diff))
self.nlp.extend_cost(self.weights['v'] * casadi.dot(v_diff, v_diff))
self.nlp.extend_cost(self.weights['t'] * T_total)
self.nlp.create_solver()
#
self.build_time = time() - t_build_start
self.com_target = com_target
self.contact_sequence = contact_sequence
self.end_com = array(end_com)
self.end_comd = array(end_comd)
self.nb_contacts = nb_contacts
self.nb_steps = nb_steps
self.omega2 = omega2
self.preview = ZMPPreviewBuffer(contact_sequence)
self.start_com = array(start_com)
self.start_comd = array(start_comd)
self.state_estimator = state_estimator
self.swing_dT_min = self.dT_min
"""
eTe_f = C.veccat([eTe_f_1, eTe_f_2, eTe_f_3])
rho = conf['rho']
alpha0 = conf['alpha0deg'] * C.pi / 180
sref = conf['sref']
bref = conf['bref']
cref = conf['cref']
vKite2 = C.mul(v_bw_f.T, v_bw_f) #Airfoil speed^2
vKite = C.sqrt(vKite2) #Airfoil speed
dae['airspeed'] = vKite
# Lift axis, normed to airspeed
eLe_v_f = C.cross(eTe_f, v_bw_f)
# sideforce axis, normalized to airspeed^2
eYe_v2_f = C.cross(eLe_v_f, -v_bw_f)
if conf['alpha_beta_computation'] == 'first_order':
alpha = alpha0 + v_bw_b[2] / v_bw_b[0]
beta = v_bw_b[1] / v_bw_b[0]
# beta = v_bw_b_y / C.sqrt(v_bw_b[0]*v_bw_b[0] + v_bw_b[2]*v_bw_b[2])
elif conf['alpha_beta_computation'] == 'closed_form':
(alpha, beta) = getWindAnglesFrom_v_bw_b(vKite, v_bw_b)
alpha += alpha0
else:
raise ValueError('config "alpha_beta_compuation" value ' +
str(conf['alpha_beta_computation']) +
' not recognized, use "first_order" or "closed_form"')
def makeModel(conf,propertiesDir='../properties'):
print "Using the update carousel model"
# Make model
dae = rawe.models.carousel(conf)
(xDotSol, zSol) = dae.solveForXDotAndZ()
# Get variables and outputs from the model
ddp = C.vertcat([xDotSol['dx'],xDotSol['dy'],xDotSol['dz']])
ddt_w_bn_b = C.vertcat([xDotSol['w_bn_b_x'],xDotSol['w_bn_b_y'],xDotSol['w_bn_b_z']])
x = dae['x']
y = dae['y']
z = dae['z']
e31 = dae['e31']
e32 = dae['e32']
e33 = dae['e33']
zt = conf['zt']
dx = dae['dx']
dy = dae['dy']
ddelta = dae['ddelta']
dddelta = xDotSol['ddelta']
R = dae['R_c2b']
rA = conf['rArm']
g = conf['g']
# Rotation matrix to convert from NWU to NED frame type
R_nwu2ned = np.eye( 3 )
R_nwu2ned[1, 1] = R_nwu2ned[2, 2] = -1.0
############################################################################
#
# IMU model
#
############################################################################
# Load IMU position and orientation w.r.t. body frame
pIMU = C.mul(R_nwu2ned, C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'IMU/pIMU.dat'))))
RIMU = C.mul(R_nwu2ned, C.DMatrix(np.loadtxt(os.path.join(propertiesDir,'IMU/RIMU.dat'))))
# Define IMU measurement functions
# TODO here is omitted the term: w x w pIMU
# The sign of gravity is negative because of the NED convention (z points down!)
ddpIMU_c = ddp - ddelta ** 2 * C.vertcat([x + rA, y, 0]) + 2 * ddelta * C.vertcat([-dy, dx, 0]) + \
dddelta * C.vertcat([-y, x + rA, 0]) - C.vertcat([0, 0, g])
ddpIMU = C.mul(R, ddpIMU_c)
aBridle = C.cross(ddt_w_bn_b, pIMU)
ddpIMU += aBridle
ddpIMU = C.mul(RIMU,ddpIMU)
# You can add a parameter to conf which will give the model 3 extra states with derivative 0 (to act as parameter) for the bias in the acceleration measurements. If that is present, it should be added to the measurement of the acceleration
if 'useIMUAccelerationBias' in conf and conf['useIMUAccelerationBias']:
IMUAccelerationBias = C.vertcat([dae['IMUAccelerationBias1'],dae['IMUAccelerationBias2'],dae['IMUAccelerationBias3']])
ddpIMU += IMUAccelerationBias
# For the accelerometers
dae['IMU_acceleration'] = ddpIMU
# ... and for the gyroscopes
dae['IMU_angular_velocity'] = C.mul(RIMU, dae['w_bn_b'])
if 'kinematicIMUAccelerationModel' in conf and conf['kinematicIMUAccelerationModel']:
dae['vel_error_x'] = dae['dx']-dae['dx_IMU']
dae['vel_error_y'] = dae['dy']-dae['dy_IMU']
dae['vel_error_z'] = dae['dz']-dae['dz_IMU']
############################################################################
#
# Stereo vision subsystem modeling
#
############################################################################
# Load calibration data from configuration files
camConf = {'PdatC1':C.DMatrix(np.loadtxt(os.path.join(propertiesDir, 'cameras/PC1.dat'))),
'PdatC2':C.DMatrix(np.loadtxt(os.path.join(propertiesDir, 'cameras/PC2.dat'))),
'RPC1':C.DMatrix(np.loadtxt(os.path.join(propertiesDir, 'cameras/RPC1.dat'))),
'RPC2':C.DMatrix(np.loadtxt(os.path.join(propertiesDir, 'cameras/RPC2.dat'))),
'pos_marker_body1':C.DMatrix(np.loadtxt(os.path.join(propertiesDir, 'markers/pos_marker_body1.dat'))),
'pos_marker_body2':C.DMatrix(np.loadtxt(os.path.join(propertiesDir, 'markers/pos_marker_body2.dat'))),
'pos_marker_body3':C.DMatrix(np.loadtxt(os.path.join(propertiesDir, 'markers/pos_marker_body3.dat')))}
# Construction of the measurement functions
dae['marker_positions'] = camModel.fullCamModel(dae, camConf)
x_tether = (x + zt*e31)
y_tether = (y + zt*e32)
z_tether = (z + zt*e33)
dae['lineAngles'] = C.vertcat([C.arctan(y_tether/x_tether),C.arctan(z_tether/x_tether)])
############################################################################
#
# Constraints in the MHE
#
############################################################################
dae['ConstR1'] = dae['e11']*dae['e11'] + dae['e12']*dae['e12'] + dae['e13']*dae['e13'] - 1
dae['ConstR2'] = dae['e11']*dae['e21'] + dae['e12']*dae['e22'] + dae['e13']*dae['e23']
dae['ConstR3'] = dae['e11']*dae['e31'] + dae['e12']*dae['e32'] + dae['e13']*dae['e33']
dae['ConstR4'] = dae['e21']*dae['e21'] + dae['e22']*dae['e22'] + dae['e23']*dae['e23'] - 1
dae['ConstR5'] = dae['e21']*dae['e31'] + dae['e22']*dae['e32'] + dae['e23']*dae['e33']
dae['ConstR6'] = dae['e31']*dae['e31'] + dae['e32']*dae['e32'] + dae['e33']*dae['e33'] - 1
dae['ConstDelta'] = (dae['cos_delta'] ** 2 + dae['sin_delta'] ** 2 - 1)
return dae
def __init__(self,NR=4,debug=False,quatnorm=False):
"""
Keyword arguments:
NR -- the number of rotors
debug -- wether to print out debug info
quatnorm -- add the quaternion norm to the DAE rhs
"""
# ----------- system states and their derivatives ----
pos = struct_symSX(["x","y","z"]) # rigid body centre of mass position [m] {0}
v = struct_symSX(["vx","vy","vz"]) # rigid body centre of mass position velocity [m/s] {0}
NR = 4 # Number of rotors
states = struct_symSX([
entry("p",struct=pos),
entry("v",struct=v),
entry("q",shape=4), # quaternions {0} -> {1}
entry("w",shape=3), # rigid body angular velocity w_101 [rad/s] {1}
entry("r",shape=NR) # spin speed of rotor, wrt to platform. [rad/s] Should be positive!
# The signs are such that positive means lift generating, regardless of spin direction.
])
pos, v, q, w, r = states[...]
# ------------------------------------------------
dist = struct_symSX([
entry("Faer",shape=NR), # Disturbance on aerodynamic forcing [N]
entry("Caer",shape=NR) # Disturbance on aerodynamic torques [Nm]
])
# ----------------- Controls ---------------------
controls = struct_symSX([
entry("CR",shape=NR) # [Nm]
# Torques of the motors that drive the rotors, acting from platform on propeller
# The torque signs are always positive when putting energy in the propellor,
# regardless of spin direction.
#
])
CR = controls["CR"]
# ------------------------------------------------
# ---------------- Temporary symbols --------------
F = ssym("F",3) # Forces acting on the platform in {1} [N]
C = ssym("C",3) # Torques acting on the platform in {1} [Nm]
rotors_Faer = [ssym("Faer_%d" %i,3,1) for i in range(NR)] # Placeholder for aerodynamic force acting on propeller {1} [N]
rotors_Caer = [ssym("Caer_%d" %i,3,1) for i in range(NR)] # Placeholder for aerodynamic torques acting on propeller {1} [Nm]
# ---------------------------------------------------
# ----------------- Parameters ---------------------
rotor_model = struct_symSX([
"c", # c Cord length [m]
"R", # R Radius of propeller [m]
"CL_alpha", # CL_alpha Lift coefficient [-]
"alpha_0", # alpha_0
"CD_alpha", # CD_alpha Drag coefficient [-]
"CD_i", # CD_i Induced drag coefficient [-]
])
p = struct_symSX([
entry("rotors_model",repeat=NR,struct=rotor_model), # Parameters that describe the rotor model
entry("rotors_I",repeat=NR,shape=sp_diag(3)), # Inertias of rotors [kg.m^2]
entry("rotors_spin",repeat=NR), # Direction of spin from each rotor. 1 means rotation around positive z.
entry("rotors_p",repeat=NR,shape=3), # position of rotors in {1} [m],
entry("I",sym=casadi.diag(ssym("[Ix,Iy,Iz]"))), # Inertia of rigid body [kg.m^2]
"m", # Mass of the whole system [kg]
"g", # gravity [m/s^2]
"rho", # Air density [kg/m^3]
])
I,m,g,rho = p[["I","m","g","rho"]]
# --------------------------------------------------
# ----------------- Parameters fillin's ---------------------
p_ = p()
p_["rotors_spin"] = [1,-1,1,-1]
p_["rotors_model",:,{}] = { "c": 0.01, "R" : 0.127, "CL_alpha": 6.0, "alpha_0": 0.15, "CD_alpha": 0.02, "CD_i": 0.05} # c Cord length [m]
p_["m"] = 0.5 # [kg]
p_["g"] = 9.81 # [N/kg]
p_["rho"] = 1.225 # [kg/m^3]
L = 0.25
I_max = p_["m"] * L**2 # Inertia of a point mass at a distance L
I_ref = I_max/5
p_["I"] = casadi.diag([I_ref/2,I_ref/2,I_ref]) # [N.m^2]
p_["rotors_p",0] = DM([L,0,0])
p_["rotors_p",1] = DM([0,L,0])
p_["rotors_p",2] = DM([-L,0,0])
p_["rotors_p",3] = DM([0,-L,0])
for i in range(NR):
R_ = p_["rotors_model",i,"R"] # Radius of propeller [m]
m_ = 0.01 # Mass of a propeller [kg]
I_max = m_ * R_**2 # Inertia of a point mass
I_ref = I_max/5
p_["rotors_I",i] = casadi.diag([I_ref/2,I_ref/2,I_ref])
if debug:
print p.vecNZcat()
dist_ = dist(0)
# ----------------- Scaling ---------------------
scaling_states = states(1)
scaling_controls = controls(1)
scaling_states["r"] = 500
scaling_controls["CR"] = 0.005
scaling_dist = dist()
scaling_dist["Faer"] = float(p_["m"]*p_["g"]/NR)
scaling_dist["Caer"] = 0.0026
# ----------- Frames ------------------
T_10 = mul(tr(*pos),Tquat(*q))
T_01 = kin_inv(T_10)
R_10 = T2R(T_10)
R_01 = R_10.T
# -------------------------------------
dstates = struct_symSX(states)
dp,dv,dq,dw,dr = dstates[...]
res = struct_SX(states) # DAE residual hand side
# ----------- Dynamics of the body ----
res["p"] = v - dp
# Newton, but transform the force F from {1} to {0}
res["v"] = mul(R_10,F) - m*dv
# Kinematics of the quaterion.
res["q"] = mul(quatDynamics(*q),w)-dq
# This is a trick by Sebastien Gros to stabilize the quaternion evolution equation
res["q"] += -q*(sumAll(q**2)-1)
# Agular impulse H_1011
H = mul(p["I"],w) # Due to platform
for i in range(NR):
H+= mul(p["rotors_I",i], w + vertcat([0,0,p["rotors_spin",i]*r[i]])) # Due to rotor i
dH = mul(jacobian(H,w),dw) + mul(jacobian(H,q),dq) + mul(jacobian(H,r),dr) + casadi.cross(w,H)
res["w"] = C - dH
for i in range(NR):
res["r",i] = CR[i] + p["rotors_spin",i]*rotors_Caer[i][2] - p["rotors_I",i][2]*(dr[i]+dw[2]) # Dynamics of rotor i
# ---------------------------------
# Make a vector of f ?
#if quatnorm:
# f = vertcat(f+[sumAll(q**2)-1])
#else:
# f = vertcat(f)
# ------------ Force model ------------
Fg = mul(R_01,vertcat([0,0,-g*m]))
F_total = Fg + sum(rotors_Faer) # Total force acting on the platform
C_total = SX([0,0,0]) # Total torque acting on the platform
for i in range(NR):
C_total[:2] += rotors_Caer[i][:2] # The x and y components propagate
C_total[2] -= p["rotors_spin",i]*CR[i] # the z compent moves through a serparate system
C_total += casadi.cross(p["rotors_p",i],rotors_Faer[i]) # Torques due to thrust
res = substitute(res,F,F_total)
res = substitute(res,C,C_total)
subs_before = []
subs_after = []
v_global = mul(R_01,v)
u_z = SX([0,0,1])
# Now fill in the aerodynamic forces
for i in range(NR):
c,R,CL_alpha,alpha_0, CD_alpha, CD_i = p["rotors_model",i,...]
#Bristeau P-jean, Martin P, Salaun E, Petit N. The role of propeller aerodynamics in the model of a quadrotor UAV. In: Proceedings of the European Control Conference 2009.; 2009:683-688.
v_local = v_global + (casadi.cross(w,p["rotors_p",i])) # Velocity at rotor i
rotors_Faer_physics = (rho*c*R**3*r[i]**2*CL_alpha*(alpha_0/3.0-v_local[2]/(2.0*R*r[i]))) * u_z
subs_before.append(rotors_Faer[i])
subs_after.append(rotors_Faer_physics + dist["Faer",i])
rotors_Caer_physics = -p["rotors_spin",i]*rho*c*R**4*r[i]**2*(CD_alpha/4.0+CD_i*alpha_0**2*(alpha_0/4.0-2.0*v_local[2]/(3.0*r[i]*R))-CL_alpha*v_local[2]/(r[i]*R)*(alpha_0/3.0-v_local[2]/(2.0*r[i]*R))) * u_z
subs_before.append(rotors_Caer[i])
subs_after.append(rotors_Caer_physics + dist["Caer",i])
res = substitute(res,veccat(subs_before),veccat(subs_after))
# Make an explicit ode
rhs = - casadi.solve(jacobian(res,dstates),substitute(res,dstates,0))
# --------------------------------------
self.res_w = res
self.res = substitute(res,dist,dist_)
self.res_ = substitute(self.res,p,p_)
resf = SXFunction([dstates, states, controls ],[self.res_])
resf.init()
self.resf = resf
self.rhs_w = rhs
self.rhs = substitute(rhs,dist,dist_)
self.rhs_ = substitute(self.rhs,p,p_)
t = SX("t")
# We end up with a DAE that captures the system dynamics
dae = SXFunction(daeIn(t=t,x=states,p=controls),daeOut(ode=self.rhs_))
dae.init()
self.dae = dae
cdae = SXFunction(controldaeIn(t=t, x=states, u= controls,p=p),daeOut(ode=self.rhs))
cdae.init()
self.cdae = cdae
self.states = states
self.dstates = dstates
self.p = p
self.p_ = p_
self.controls = controls
self.NR = NR
self.w = dist
self.w_ = dist_
self.t = t
self.states_ = states()
self.dstates_ = states()
self.controls_ = controls()
self.scaling_states = scaling_states
self.scaling_controls = scaling_controls
self.scaling_dist = scaling_dist
def create_rocket_stage_phase(mocp, phase_name, p, **kwargs):
is_powered = kwargs['is_powered']
has_heating = kwargs['has_heating']
drag_area = kwargs['drag_area']
maxQ = kwargs['maxQ']
init_mass = kwargs['init_mass']
init_position = kwargs['init_position']
init_velocity = kwargs['init_velocity']
if is_powered:
init_steering = kwargs['init_steering']
engine_type = kwargs['engine_type']
engine_count = kwargs['engine_count']
mdot_max = kwargs['mdot_max']
mdot_min = kwargs['mdot_min']
if engine_type == 'vacuum':
effective_exhaust_velocity = kwargs['effective_exhaust_velocity']
else:
exhaust_velocity = kwargs['exhaust_velocity']
exit_area = kwargs['exit_area']
exit_pressure = kwargs['exit_pressure']
duration = mocp.create_phase(phase_name, init=0.001, n_intervals=2)
# Total vessel mass
mass = mocp.add_trajectory(phase_name, 'mass', init=init_mass)
# ECEF position vector
rx = mocp.add_trajectory(phase_name, 'rx', init=init_position[0])
ry = mocp.add_trajectory(phase_name, 'ry', init=init_position[1])
rz = mocp.add_trajectory(phase_name, 'rz', init=init_position[2])
r_vec = casadi.vertcat(rx, ry, rz)
# ECEF velocity vector
vx = mocp.add_trajectory(phase_name, 'vx', init=init_velocity[0])
vy = mocp.add_trajectory(phase_name, 'vy', init=init_velocity[1])
vz = mocp.add_trajectory(phase_name, 'vz', init=init_velocity[2])
v_vec = casadi.vertcat(vx, vy, vz)
if is_powered:
# ECEF steering unit vector
ux = mocp.add_trajectory(phase_name, 'ux', init=init_steering[0])
uy = mocp.add_trajectory(phase_name, 'uy', init=init_steering[1])
uz = mocp.add_trajectory(phase_name, 'uz', init=init_steering[2])
u_vec = casadi.vertcat(ux, uy, uz)
# Engine mass flow for a SINGLE engine
mdot = mocp.add_trajectory(phase_name, 'mdot', init=mdot_max)
mdot_rate = mocp.add_trajectory(phase_name, 'mdot_rate', init=0.0)
if has_heating:
heat = mocp.add_trajectory(phase_name, 'heat', init=0.0)
## Dynamics
r_squared = rx**2 + ry**2 + rz**2 + 1e-8
v_squared = vx**2 + vy**2 + vz**2 + 1e-8
airspeed = v_squared**0.5
r = r_squared**0.5
altitude = r - 1
r3_inv = r_squared**(-1.5)
mocp.add_path_output(
'downrange_distance',
casadi.asin(
casadi.norm_2(
casadi.cross(
r_vec / r,
casadi.SX(
[p.launch_site_x, p.launch_site_y,
p.launch_site_z])))))
mocp.add_path_output('altitude', altitude)
mocp.add_path_output('airspeed', airspeed)
mocp.add_path_output('vertical_speed', (r_vec.T / r) @ v_vec)
mocp.add_path_output(
'horizontal_speed_ECEF',
casadi.norm_2(v_vec - ((r_vec.T / r) @ v_vec) * (r_vec / r)))
# Gravitational acceleration (mu=1 is omitted)
gx = -r3_inv * rx
gy = -r3_inv * ry
gz = -r3_inv * rz
# Atmosphere
atmosphere_fraction = casadi.exp(-altitude / p.scale_height)
air_pressure = p.air_pressure_MSL * atmosphere_fraction
air_density = p.air_density_MSL * atmosphere_fraction
dynamic_pressure = 0.5 * air_density * v_squared
mocp.add_path_output('dynamic_pressure', dynamic_pressure)
if is_powered:
lateral_airspeed = casadi.sqrt(
casadi.sumsqr(v_vec - ((v_vec.T @ u_vec) * u_vec)) + 1e-8)
lateral_dynamic_pressure = 0.5 * air_density * lateral_airspeed * airspeed # Same as Q * sin(alpha), but without trigonometry
mocp.add_path_output('lateral_dynamic_pressure',
lateral_dynamic_pressure)
mocp.add_path_output(
'alpha',
casadi.acos((v_vec.T @ u_vec) / airspeed) * 180.0 / pi)
mocp.add_path_output(
'pitch', 90.0 - casadi.acos((r_vec.T / r) @ u_vec) * 180.0 / pi)
# Thrust force
if is_powered:
if engine_type == 'vacuum':
engine_thrust = effective_exhaust_velocity * mdot
else:
engine_thrust = exhaust_velocity * mdot + exit_area * (
exit_pressure - air_pressure)
total_thrust = engine_thrust * engine_count
mocp.add_path_output('total_thrust', total_thrust)
Tx = total_thrust * ux
Ty = total_thrust * uy
Tz = total_thrust * uz
else:
Tx = 0.0
Ty = 0.0
Tz = 0.0
# Drag force
drag_factor = (-0.5 * drag_area) * air_density * airspeed
Dx = drag_factor * vx
Dy = drag_factor * vy
Dz = drag_factor * vz
# Coriolis Acceleration
ax_Coriolis = 2 * vy * p.body_rotation_speed
ay_Coriolis = -2 * vx * p.body_rotation_speed
az_Coriolis = 0.0
# Centrifugal Acceleration
ax_Centrifugal = rx * p.body_rotation_speed**2
ay_Centrifugal = ry * p.body_rotation_speed**2
az_Centrifugal = 0.0
# Acceleration
ax = gx + ax_Coriolis + ax_Centrifugal + (Dx + Tx) / mass
ay = gy + ay_Coriolis + ay_Centrifugal + (Dy + Ty) / mass
az = gz + az_Coriolis + az_Centrifugal + (Dz + Tz) / mass
if is_powered:
mocp.set_derivative(mass, -engine_count * mdot)
mocp.set_derivative(mdot, mdot_rate)
else:
mocp.set_derivative(mass, 0.0)
mocp.set_derivative(rx, vx)
mocp.set_derivative(ry, vy)
mocp.set_derivative(rz, vz)
mocp.set_derivative(vx, ax)
mocp.set_derivative(vy, ay)
mocp.set_derivative(vz, az)
# Heat flow
heat_flow = 1e-4 * (air_density**0.5) * (airspeed**3.15)
mocp.add_path_output('heat_flow', heat_flow)
if has_heating:
mocp.set_derivative(heat, heat_flow)
## Constraints
# Duration is positive and bounded
mocp.add_constraint(duration > 0.006)
mocp.add_constraint(duration < 10.0)
# Mass is positive
mocp.add_path_constraint(mass > 1e-6)
# maxQ soft constraint
weight_dynamic_pressure_penalty = mocp.get_parameter(
'weight_dynamic_pressure_penalty')
slack_maxQ = mocp.add_trajectory(phase_name, 'slack_maxQ', init=10.0)
mocp.add_path_constraint(slack_maxQ > 0.0)
mocp.add_mean_objective(weight_dynamic_pressure_penalty * slack_maxQ)
mocp.add_path_constraint(dynamic_pressure / maxQ < 1.0 + slack_maxQ)
if is_powered:
# u is a unit vector
mocp.add_path_constraint(ux**2 + uy**2 + uz**2 == 1.0)
# Do not point down
mocp.add_path_constraint((r_vec / r).T @ u_vec > -0.2)
# Engine flow limits
mocp.add_path_constraint(mdot_min < mdot)
mocp.add_path_constraint(mdot < mdot_max)
# Throttle rate reduction
mocp.add_mean_objective(1e-6 * mdot_rate**2)
# Lateral dynamic pressure penalty
weight_lateral_dynamic_pressure_penalty = mocp.get_parameter(
'weight_lateral_dynamic_pressure_penalty')
mocp.add_mean_objective(
weight_lateral_dynamic_pressure_penalty *
(lateral_dynamic_pressure / p.maxQ_sin_alpha)**8)
variables = locals().copy()
RocketStagePhase = collections.namedtuple('RocketStagePhase',
sorted(list(variables.keys())))
return RocketStagePhase(**variables)
def aero(xd, xa, p, puni, l, params):
[q, dq, R, w, coeff, E, Drag] = xd[...]
# --- Build current wind shear profile
#
Lagrange_polynomial_x = fcts.Lagrange_poly(puni['altitude'], puni['tau_x'])
Lagrange_polynomial_y = fcts.Lagrange_poly(puni['altitude'], puni['tau_y'])
# --- wind vector & apparent velocity
xwind = Lagrange_polynomial_x(q[-1])
ywind = Lagrange_polynomial_y(q[-1])
v_app = dq - np.array([xwind, ywind, 0])
# xwind = puni['wind']*(q[-1]/params['windShearRefAltitude'])**0.15 #NOTE:AUTOMATE THAT!!!!!!
# v_app = dq - np.array([xwind,0,0])
# --- calculate angle of attack and side slip (convert apparent velocity to body frame)
AoA = -ca.mtimes(R[6:9].T, (v_app)) / ca.mtimes(R[0:3].T, (v_app))
sslip = ca.mtimes(R[3:6].T, (v_app)) / ca.mtimes(R[0:3].T, (v_app))
params['rho'] = fcts.rho(q[-1])
CF, CM, Cl, Cm, Cn = aero_coeffs(AoA, sslip, v_app, w, coeff, params)
F_aero = 0.5 * params['rho'] * params['sref'] * ca.norm_2(v_app) * (
ca.cross(CF[2] * v_app, R[3:6]) + CF[0] * (v_app))
reference_lengths = ca.vertcat(
[params['bref'], params['cref'], params['bref']])
M_aero = 0.5 * params['rho'] * params['sref'] * ca.norm_2(
v_app)**2 * CM * reference_lengths
F_drag = -Drag * R[0:3] * params[
'eff'] # Drag in opposite x-direction of kite (Props)
F_tether = q * xa # Force of the tether at the kite
# m = params['mK'] + 1./3*params['mT']
m = params[
'mK'] + 1. / 3 * params['tether_density'] * l #mass of kite and tether
F_gravity = m * params['g']
# --- Tether Drag, defined in + x direction , therefore minus sign
C_tet = 0.4
Tether_drag = -1. / 8 * params['rho'] * params[
'tether_diameter'] * C_tet * l * ca.norm_2(v_app)**2 * R[0:3]
# Tether_drag = - 1./6 * params['rho'] * params['tether_diameter'] * C_tet * l * ca.norm_2(v_app)**2 * R[0:3]
outputs = {}
outputs['v_app'] = v_app
# outputs['rho'] = params['rho']
outputs['speed'] = ca.norm_2(v_app)
outputs['windspeed_shear'] = xwind
# outputs['windspeedy_shear'] = ywind
outputs['momentconst'] = ([Cl, Cm, Cn])
outputs['AoA'] = AoA
outputs['sslip'] = sslip
outputs['AoA_deg'] = AoA * 180 / pi
outputs['sslip_deg'] = sslip * 180 / pi
outputs['CL'] = CF[2]
outputs['CD'] = -CF[0]
outputs['F_aero'] = F_aero
outputs['F_drag'] = F_drag
outputs['F_tether_scaled'] = F_tether # This is scaled so Force/kg
outputs['F_tether'] = F_tether * m
outputs['tethercstr'] = (xa * l * m) - pi / 3. * (
params['tether_diameter'] * 1000 /
2.)**2 * 3600 # tdiameter in m, l*xa = F_tether instead of norm (q*xa)
# Tensile strength of aramid: 3600N/mm^2, while dyneema has only 1700?
outputs['M_aero'] = M_aero
outputs['power'] = ca.mtimes(F_drag.T, dq)
outputs['Tether_drag'] = Tether_drag
return (F_aero, M_aero, F_tether, F_drag, F_gravity, Tether_drag), outputs
"""
eTe_f = C.veccat([eTe_f_1, eTe_f_2, eTe_f_3])
rho = conf["rho"]
alpha0 = conf["alpha0deg"] * C.pi / 180
sref = conf["sref"]
bref = conf["bref"]
cref = conf["cref"]
vKite2 = C.mul(v_bw_f.T, v_bw_f) # Airfoil speed^2
vKite = C.sqrt(vKite2) # Airfoil speed
dae["airspeed"] = vKite
# Lift axis, normed to airspeed
eLe_v_f = C.cross(eTe_f, v_bw_f)
# sideforce axis, normalized to airspeed^2
eYe_v2_f = C.cross(eLe_v_f, -v_bw_f)
if conf["alpha_beta_computation"] == "first_order":
alpha = alpha0 + v_bw_b[2] / v_bw_b[0]
beta = v_bw_b[1] / v_bw_b[0]
# beta = v_bw_b_y / C.sqrt(v_bw_b[0]*v_bw_b[0] + v_bw_b[2]*v_bw_b[2])
elif conf["alpha_beta_computation"] == "closed_form":
(alpha, beta) = getWindAnglesFrom_v_bw_b(vKite, v_bw_b)
alpha += alpha0
else:
raise ValueError(
'config "alpha_beta_compuation" value '
+ str(conf["alpha_beta_computation"])
def design_reference(phi_0,
A_phi,
f_phi,
theta_0,
A_theta,
f_theta,
time,
run_test=False):
"""TODO: Docstring for design_reference.
:phi_0: TODO
:A_phi: TODO
:f_phi: TODO
:theta_0: TODO
:A_theta: TODO
:f_theta: TODO
:returns: TODO
"""
def evaluate_scalar_casadi_function(fun, var):
"""TODO: Docstring for evaluate_scalar_casadi_function.
:fun: TODO
:var: TODO
:returns: TODO
"""
return np.squeeze(np.double(fun(var)))
def f_eval_cs_vec(fun, var):
"""TODO: Docstring for f_eval_cs_vec.
:fun: TODO
:var: TODO
:returns: TODO
"""
return np.double(fun(var).reshape((3, 1))).reshape(3)
# phi_0 = 0
# A_phi = np.pi/6
# f_phi = 0.5
# theta_0 = 0
# A_theta = np.pi/8
# f_theta = 0.5
# Symbolics
t = cs.SX.sym('t', 1, 1)
phi = phi_0 + A_phi * cs.sin(2 * cs.pi * f_phi * t)
dphi = cs.jacobian(phi, t)
ddphi = cs.jacobian(dphi, t)
theta = theta_0 + A_theta * cs.cos(2 * cs.pi * f_theta * t)
dtheta = cs.jacobian(theta, t)
ddtheta = cs.jacobian(dtheta, t)
Gamma = Geometric_controller.compute_sym_Gamma_rp(phi, theta)
# Gamma_dot = cg.orthogonal_projector(Gamma)@cs.jacobian(Gamma, t)
Gamma_dot = cs.jacobian(Gamma, t)
omega = cs.cross(Gamma_dot, Gamma)
omega_dot = cs.jacobian(omega, t)
f_phi = cs.Function('phi', [t], [phi])
f_theta = cs.Function('theta', [t], [theta])
f_Gamma = cs.Function('Gamma', [t], [Gamma])
f_Gamma_dot = cs.Function('Gamma_dot', [t], [Gamma_dot])
f_omega = cs.Function('omega', [t], [omega])
f_omega_dot = cs.Function('omega_dot', [t], [omega_dot])
# Gamma_dot = cg.compute_perpendicular_projector(Gamma)@
# Generate numeric values
t = time
t0 = t[0]
dt = t[1] - t[0]
fs = 1 / dt
T = t[-1] + dt
N = len(t)
phi = np.zeros(N)
theta = np.zeros(N)
Gamma = np.zeros((3, N))
Gamma_dot = np.zeros((3, N))
omega = np.zeros((3, N))
omega_dot = np.zeros((3, N))
phi = np.squeeze(np.double(f_phi(t)))
theta = np.squeeze(np.double(f_theta(t)))
is_orthogonal_Gamma_omega = np.zeros(N)
test_orthogonal_Gamma_omega = 0
test_orthogonal_Gamma_Gamma_dot = 0
for k in range(0, N):
Gamma[:, k] = f_eval_cs_vec(f_Gamma, t[k])
Gamma_dot[:, k] = f_eval_cs_vec(f_Gamma_dot, t[k])
omega[:, k] = f_eval_cs_vec(f_omega, t[k])
omega_dot[:, k] = f_eval_cs_vec(f_omega_dot, t[k])
test_orthogonal_Gamma_omega += np.dot(Gamma[:, k], omega[:, k])**2
test_orthogonal_Gamma_Gamma_dot += np.dot(Gamma[:, k], Gamma_dot[:,
k])**2
if run_test:
print('Design reference')
print('# Tests failed:' + str(
test_eval(test_orthogonal_Gamma_omega) +
test_eval(test_orthogonal_Gamma_Gamma_dot)))
# Integration Test
Gamma_integrated = np.zeros((3, N))
omega_integrated = np.zeros((3, N))
x_Gamma = Gamma[:,
0] #Geometric_controller.compute_Gamma_rp(phi[0], theta[0])
x_omega = omega[:, 0]
for k in range(0, N):
Gamma_dot = np.cross(Gamma[:, k], omega[:, k])
x_Gamma += dt * Gamma_dot
x_Gamma = x_Gamma / np.linalg.norm(x_Gamma)
Gamma_integrated[:, k] = x_Gamma
x_omega += dt * omega_dot[:, k]
# x_omega += Geometric_controller.compute_perpendicular_projector(Gamma[:, k])@x_omega
omega_integrated[:, k] = x_omega
fig, ax = plt.subplots(2, 1)
color = ['r', 'g', 'b']
for i in range(0, 3):
ax[0].plot(t, Gamma[i, :].T, color=color[i])
ax[0].plot(t,
Gamma_integrated[i, :].T,
color=color[i],
linestyle='--')
ax[1].plot(t, omega[i, :].T, color=color[i])
ax[1].plot(t,
omega_integrated[i, :].T,
color=color[i],
linestyle='--')
plt.show(block=False)
return [Gamma, omega, omega_dot]
def aero_drag_ampyx(xd, xa, p, puni, l, params):
[q, dq, R, w, coeff, E, Drag] = xd[...]
# --- Build current wind shear profile
Lagrange_polynomial_x = fcts.Lagrange_poly(puni['altitude'], puni['tau_x'])
Lagrange_polynomial_y = fcts.Lagrange_poly(puni['altitude'], puni['tau_y'])
# --- wind vector & apparent velocity
xwind = Lagrange_polynomial_x(q[-1])
ywind = Lagrange_polynomial_y(q[-1])
v_app = dq - np.array([xwind, ywind, 0])
# xwind = puni['wind']*(q[-1]/params['windShearRefAltitude'])**0.15 #NOTE:AUTOMATE THAT!!!!!!
# v_app = dq - np.array([xwind,0,0])
v_app_body = ca.mtimes(R.T, v_app)
# calculate angle of attack and side slip (convert apparent velocity to body frame)
AoA = -ca.mtimes(R[6:9].T, (v_app)) / ca.mtimes(R[0:3].T, (v_app))
sslip = ca.mtimes(R[3:6].T, (v_app)) / ca.mtimes(R[0:3].T, (v_app))
CF, CM, CD, CL = aero_coeffs_ampyx(AoA, sslip, v_app, w, coeff, params)
W0 = v_app / ca.norm_2(v_app)
W2 = ca.cross(v_app, R[3:6]) / ca.norm_2(v_app)
W1 = ca.cross(W2, W0)
W = ca.vertcat(W0.T, W1.T, W2.T).T
#NED to zUp
#NED2zup = [ [1, 0, 0], [0, -1, 0], [0, 0, -1]]
#NED2zup = np.reshape(NED2zup, (3,3))
#CF = ca.mtimes(NED2zup,CF)
F_aero = ca.mtimes(W, CF)
M_aero = CM
# F_aero = 0.5 * params['rho'] * params['sref'] * ca.norm_2(v_app) * (ca.cross(-CF[2]*v_app,R[3:6]) + CF[0]*(v_app) )
# reference_lengths = ca.vertcat([params['bref'], params['cref'], params['bref']])
# M_aero = 0.5 * params['rho'] * params['sref'] * ca.norm_2(v_app)**2 * CM # * reference_lengths
F_drag = -Drag * R[0:3] * params[
'eff'] # Drag in opposite x-direction of kite (Props)
F_tether = q * xa # Force of the tether at the kite
m = params['mK'] + 1. / 3 * params['tether_density'] * l
# m = params['mK'] + 1./3*params['tether_density']*ltet #mass of kite and tether
F_gravity = m * params['g']
# Tether Drag
# defined in + x direction , therefore minus sign
C_tet = 1.2
# Tether_drag = - 1./8 * params['rho'] * params['tether_diameter'] * C_tet * ltet * ca.norm_2(v_app)**2 * ca.veccat(ca.cos(AoA), 0, ca.sin(AoA))
Tether_drag = -1. / 8 * params['rho'] * params[
'tether_diameter'] * C_tet * l * ca.norm_2(v_app) * v_app
Tether_drag_body = Tether_drag #!!!!----NOTE: It should be nav frame from the beginning, since we use v_app_nav, right?
outputs = {}
outputs['v_app'] = v_app
outputs['speed'] = ca.norm_2(v_app)
outputs['windspeed_shear'] = xwind
outputs['AoA'] = AoA
outputs['sslip'] = sslip
outputs['AoA_deg'] = AoA * 180 / pi
outputs['sslip_deg'] = sslip * 180 / pi
outputs['CL'] = CL[0][0]
outputs['CD'] = -CD[0][0]
outputs['F_aero'] = F_aero
outputs['F_aero_b'] = ca.mtimes(R.T, F_aero)
outputs['F_drag'] = F_drag
outputs['F_tether_scaled'] = F_tether # This is scaled so Force/kg
outputs['F_tether'] = F_tether * m
outputs['tethercstr'] = (xa * l * m) - pi / 3. * (
params['tether_diameter'] * 1000 /
2.)**2 * 3600 # tdiameter in m, l*xa = F_tether instead of norm (q*xa)
outputs['M_aero'] = M_aero
outputs['power'] = ca.mtimes(F_drag.T, dq)
outputs['Tether_drag'] = Tether_drag
return (F_aero, M_aero, F_tether, F_drag, F_gravity, Tether_drag), outputs
def generate_figures():
import matplotlib
import matplotlib.pyplot as plt
import numpy as np
import json
with open('trajectory.json', 'r') as f:
data = json.load(f)
phases = data['phases']
scale = data['scale']
# Profile plot
e1 = normalize(
DM([
phases['ascent']['trajectories']['rx'][0],
phases['ascent']['trajectories']['ry'][0],
phases['ascent']['trajectories']['rz'][0]
]))
e2 = -normalize(
DM([
phases['target']['trajectories']['rx'][0],
phases['target']['trajectories']['ry'][0],
phases['target']['trajectories']['rz'][0]
]))
e3 = normalize(cross(e1, e2))
e2 = normalize(cross(e3, e1))
R = casadi.horzcat(e1, e2, e3).T
fig, ax = plt.subplots()
fig.set_size_inches(fig.get_size_inches() * 0.6)
to_km = scale['length'] / 1000
a = np.linspace(0, 2 * pi, 200)
ax.plot(np.cos(a) * to_km, np.sin(a) * to_km, 'k')
for phase_name in phases:
position_trajectory = R @ DM([
phases[phase_name]['trajectories']['rx'],
phases[phase_name]['trajectories']['ry'],
phases[phase_name]['trajectories']['rz']
])
position_trajectory = np.array(position_trajectory)
ax.plot(position_trajectory[1] * to_km, position_trajectory[0] * to_km)
ax.set(xlabel='y (km)', ylabel='x (km)', title='Path Profile')
ax.axis('equal')
ax.grid()
fig.tight_layout()
fig.savefig('fig/trajectory_profile.png', dpi=288)
ax.set_xlim(-300, 300)
ax.set_ylim(-300 + 600, 300 + 600)
fig.tight_layout()
fig.savefig('fig/trajectory_profile_zoomed.png', dpi=288)
plt.close('all')
fig, ax = plt.subplots()
fig.set_size_inches(fig.get_size_inches() * 0.6)
to_km = scale['length'] / 1000
a = np.linspace(0, 2 * pi, 200)
ax.plot(np.cos(a) * to_km, np.sin(a) * to_km, 'k')
for phase_name in phases:
position_trajectory = R @ DM([
phases[phase_name]['trajectories']['rx'],
phases[phase_name]['trajectories']['ry'],
phases[phase_name]['trajectories']['rz']
])
position_trajectory = np.array(position_trajectory)
ax.plot(position_trajectory[2] * to_km, position_trajectory[0] * to_km)
ax.set(xlabel='z (km)', ylabel='x (km)', title='Path Parallel')
ax.axis('equal')
ax.grid()
fig.tight_layout()
fig.savefig('fig/trajectory_parallel.png', dpi=288)
ax.set_xlim(-300, 300)
ax.set_ylim(-300 + 600, 300 + 600)
fig.tight_layout()
fig.savefig('fig/trajectory_parallel_zoomed.png', dpi=288)
plt.close('all')
# Timeseries plots
paths = \
sorted(list(set([('trajectories',k) for e in phases.values() for k in e['trajectories'].keys()])))+\
sorted(list(set([('outputs',k) for e in phases.values() for k in e['outputs'].keys()])))
for path in paths:
fig, ax = plt.subplots()
fig.set_size_inches(fig.get_size_inches() * 0.6)
for phase_name in phases:
path_scale = get_scale_factor(path[1], scale)
ax.set(xlabel='Time (s)', ylabel=path[1], title=path[1])
if path[1] in phases[phase_name][path[0]]:
if path[1] in ['mass', 'rx', 'ry', 'rz', 'altitude']:
path_scale /= 1000.0
elif 'pressure' in path[1]:
path_scale /= 1000.0
t_grid = phases[phase_name]['start_time'] + phases[phase_name][
'duration'] * np.linspace(
0.0, 1.0, len(phases[phase_name][path[0]][path[1]]))
ax.plot(
scale['time'] * t_grid,
np.array(phases[phase_name][path[0]][path[1]]) *
path_scale)
ax.grid()
fig.tight_layout()
fig.savefig('fig/trajectory_' + path[1] + '.png', dpi=288)
plt.close('all')
sref = conf['sref']
bref = conf['bref']
cref = conf['cref']
# airfoil speed in wind frame
v_bw_n_x = v_bw_n[0]
v_bw_n_y = v_bw_n[1]
v_bw_n_z = v_bw_n[2]
vKite2 = C.mul(v_bw_n.trans(), v_bw_n) #Airfoil speed^2
vKite = C.sqrt(vKite2) #Airfoil speed
dae['airspeed'] = vKite
# Lift axis, normed to airspeed
eLe_v = C.cross(C.veccat([eTe1, eTe2, eTe3]), v_bw_n)
# sideforce axis, normalized to airspeed^2
eYe_v2 = C.cross(eLe_v, -v_bw_n)
# Relative wind speed in Airfoil's referential 'E'
v_bw_b_x = v_bw_b[0]
v_bw_b_y = v_bw_b[1]
v_bw_b_z = v_bw_b[2]
if conf['alpha_beta_computation'] == 'first_order':
alpha = alpha0 + v_bw_b_z / v_bw_b_x
beta = v_bw_b_y / v_bw_b_x
# beta = v_bw_b_y / C.sqrt(v_bw_b_x*v_bw_b_x + v_bw_b_z*v_bw_b_z)
elif conf['alpha_beta_computation'] == 'closed_form':
(alpha, beta) = getWindAnglesFrom_v_bw_b(vKite, v_bw_b)
def angular_acceleration(self, inputs, omega):
tau = self.torques(inputs)
omegadot = mtimes(self.I_inv,
tau - cross(omega, mtimes(self.I, omega)))
return omegadot