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 setTimeInterval(self, t0, tf):
self.t0 = t0
self.tf = tf
# dynamics constraint
dt = (self.tf - self.t0)/(self.N - 1)
for k in range(self.N-1):
x0 = self._getStateVec(k)
x1 = self._getStateVec(k+1)
u0 = self._getActionVec(k)
u1 = self._getActionVec(k+1)
p = self._getParamVec()
t0 = k*dt
t1 = (k+1)*dt
xErr = x1 - self.ode.rk4Step( x0, u0, u1, p, t0, t1)
#self.addNonlcon( xErr, "==", C.MX(self.ode._Nx()*[0]))
self.addNonlcon( xErr, "==", C.SXMatrix(self.ode._Nx()*[0]))
C.horzcat([conf['cm_p'], conf['cm_q'], conf['cm_r']]),
C.horzcat([conf['cn_p'], conf['cn_q'], conf['cn_r']])
]), w_bn_b_hat)
dae['momentCoeffs_pqr'] = momentCoeffs_pqr
# with alpha beta
momentCoeffs_AB = C.mul(
C.vertcat([
C.horzcat([0, conf['cl_B'], conf['cl_AB']]),
C.horzcat([conf['cm_A'], 0, 0]),
C.horzcat([0, conf['cn_B'], conf['cn_AB']])
]), C.vertcat([alpha, beta, alpha * beta]))
dae['momentCoeffs_AB'] = momentCoeffs_AB
# with control surfaces
momentCoeffs_surf = C.SXMatrix(3, 1, 0)
momentCoeffs_surf[0] += conf['cl_ail'] * dae['aileron']
momentCoeffs_surf[1] += conf['cm_elev'] * dae['elevator']
if 'flaps' in dae:
momentCoeffs_surf[1] += conf['cm_flaps'] * dae['flaps']
if 'rudder' in dae:
momentCoeffs_surf[2] += conf['cn_rudder'] * dae['rudder']
dae['momentCoeffs_surf'] = momentCoeffs_surf
momentCoeffs = dae['momentCoeffs0'] + dae['momentCoeffs_pqr'] + \
dae['momentCoeffs_AB'] + dae['momentCoeffs_surf']
dae['cl_small'] = momentCoeffs[0]
dae['cm_small'] = momentCoeffs[1]
dae['cn_small'] = momentCoeffs[2]
# LIFT :
def setupModel(dae, conf):
'''
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
mass matrix columns:
ddt(ddelta dx dy dz w_bn_b_x w_bn_b_y w_bn_b_z) nu
rhs:
forces/torques acting on : ddt(ddelta dx dy dz w_bn_b_x w_bn_b_y w_bn_b_z) nu
'''
# Parameters
m = conf['mass']
g = conf['g']
j1 = conf['j1']
j31 = conf['j31']
j2 = conf['j2']
j3 = conf['j3']
jCarousel = conf['jCarousel']
cfric = conf['cfric']
zt = conf['zt']
rA = conf['rArm']
# Frames
# ==========================================
# World frame (n) NED North-East-Down
#
# Wind carrying frame (w)
# - Translates along the world frame's x axis with the wind speed
#
# Carousel frame (c)
# - Origin coincides with the world origin
# - Makes and angle delta
#
# Carousel tip frame (a)
# - Origin sits at tip of arm
# - e_x extends radially outwards
# - e_z points downwards
#
# Body frame (b)
# - attached to body of aircraft
# - e_x extends to the nose
# - e_z points downwards
#
# Vector naming convention
# =========================
# v_ab_c
# Velocity of point a, differentiated in frame b, expressed in frame c (Greg terminlogy)
# Velocity of point a w.r.t. frame b, expressed in frame c (Joris terminology)
# States
# Components that make up the rotation matrix from carousel frame to body frame R_c2b [-]
# e11 e12 e13
# e21 e22 e23
# e31 e32 e33
e11 = dae['e11']
e12 = dae['e12']
e13 = dae['e13']
e21 = dae['e21']
e22 = dae['e22']
e23 = dae['e23']
e31 = dae['e31']
e32 = dae['e32']
e33 = dae['e33']
x = dae['x'] # Aircraft position in carousel tip frame coordinates [m]
y = dae['y']
z = dae['z']
dx = dae['dx'] # Time derivatives of x [m/s]
dy = dae['dy']
dz = dae['dz']
w1 = dae['w_bn_b_x'] # Body angular rate w.r.t world frame, expressed in body coordinates [rad/s^2]
w2 = dae['w_bn_b_y']
w3 = dae['w_bn_b_z']
ddelta = dae['ddelta'] # Carousel turning speed [rad/s]
r = dae['r']
dr = dae['dr']
ddr = dae['ddr']
tc = dae['motor_torque'] #Carousel motor torque
if 'xt' in conf:
t_xt = dae['tether_tension'] * conf['xt']
else:
t_xt = 0.0
R_g2c = C.blockcat([[dae['cos_delta'], -dae['sin_delta'], 0.0],
[dae['sin_delta'], dae['cos_delta'], 0.0],
[0.0, 0.0, 1.0]])
# wind model
def getWind():
if 'wind_model' not in conf:
return C.veccat([0, 0, 0])
if conf['wind_model']['name'] == 'wind_shear':
# use a logarithmic wind shear model
# wind(z) = w0 * log((altitude+zt)/zt) / log(z0/zt)
# where w0 is wind at z0 altitude
# zt is surface roughness characteristic length
# altitude is -z - altitude0, where altitude0 is a user parameter
z0 = conf['wind_model']['z0']
zt_roughness = conf['wind_model']['zt_roughness']
altitude = -z - conf['wind_model']['altitude0']
wind = dae['w0']*C.log((altitude+zt_roughness)/zt_roughness)/C.log(z0/zt_roughness)
dae['wind_at_altitude'] = wind
return C.mul([R_g2c, C.veccat([wind, 0, 0])])
elif conf['wind_model']['name'] in ['constant','hardcoded']:
# constant wind
dae['wind_at_altitude'] = dae['w0']
return C.mul([R_g2c, C.veccat([dae['w0'], 0, 0])])
elif conf['wind_model']['name'] == 'random_walk':
dae.addU('delta_wind_x')
dae.addU('delta_wind_y')
dae.addU('delta_wind_z')
wind_x = dae.addX('wind_x')
wind_y = dae.addX('wind_y')
wind_z = dae.addX('wind_z')
dae['wind_at_altitude'] = wind_x
return C.veccat([wind_x, wind_y, wind_z])
wind = getWind()
# Velocity of aircraft w.r.t wind carrying frame, expressed in carousel frame
v_bw_c = C.veccat( [ dx - ddelta*y
, dy + ddelta*(rA + x)
, dz
]) - wind
# Velocity of aircraft w.r.t wind carrying frame, expressed in body frame
# (needed to compute the aero forces and torques !)
v_bw_b = C.mul( dae['R_c2b'], v_bw_c )
(f1, f2, f3, t1, t2, t3) = aeroForcesTorques(dae, conf, v_bw_c, v_bw_b,
dae['w_bn_b'],
(dae['e21'], dae['e22'], dae['e23']) # y-axis of body frame in carousel coordinates
)
# f1..f3 expressed in carrousel coordinates
# t1..t3 expressed in body coordinates
# if we are running a homotopy, add psudeo forces and moments as algebraic states
if 'runHomotopy' in conf and conf['runHomotopy']:
gamma_homotopy = dae.addP('gamma_homotopy')
f1 = f1 * gamma_homotopy + dae.addZ('f1_homotopy') * (1 - gamma_homotopy)
f2 = f2 * gamma_homotopy + dae.addZ('f2_homotopy') * (1 - gamma_homotopy)
f3 = f3 * gamma_homotopy + dae.addZ('f3_homotopy') * (1 - gamma_homotopy)
t1 = t1 * gamma_homotopy + dae.addZ('t1_homotopy') * (1 - gamma_homotopy)
t2 = t2 * gamma_homotopy + dae.addZ('t2_homotopy') * (1 - gamma_homotopy)
t3 = t3 * gamma_homotopy + dae.addZ('t3_homotopy') * (1 - gamma_homotopy)
fx_dist = 0
fy_dist = 0
fz_dist = 0
mx_dist = 0
my_dist = 0
mz_dist = 0
if 'useVirtualForces' in conf:
_v = conf[ 'useVirtualForces' ]
if isinstance(_v, str):
_type = _v
_which = True, True, True
else:
assert isinstance(_v, dict)
_type = _v["type"]
_which = _v["which"]
if _type == 'random_walk':
if _which[ 0 ]:
dae.addU('df1_disturbance')
fx_dist += dae['rho_sref_v2'] * dae.addX('f1_disturbance')
if _which[ 1 ]:
dae.addU('df2_disturbance')
fy_dist += dae['rho_sref_v2'] * dae.addX('f2_disturbance')
if _which[ 2 ]:
dae.addU('df3_disturbance')
fz_dist += dae['rho_sref_v2'] * dae.addX('f3_disturbance')
elif _type == 'parameter':
if _which[ 0 ]:
fx_dist += dae['rho_sref_v2'] * dae.addX('f1_disturbance')
if _which[ 1 ]:
fy_dist += dae['rho_sref_v2'] * dae.addX('f2_disturbance')
if _which[ 2 ]:
fz_dist += dae['rho_sref_v2'] * dae.addX('f3_disturbance')
elif _type == 'online_data':
if _which[ 0 ]:
fx_dist += dae['rho_sref_v2'] * dae.addP('f1_disturbance')
if _which[ 1 ]:
fy_dist += dae['rho_sref_v2'] * dae.addP('f2_disturbance')
if _which[ 2 ]:
fz_dist += dae['rho_sref_v2'] * dae.addP('f3_disturbance')
else:
raise ValueError("WTF?")
f1 += fx_dist
f2 += fy_dist
f3 += fz_dist
dae["aero_fx_dist"] = fx_dist
dae["aero_fy_dist"] = fy_dist
dae["aero_fz_dist"] = fz_dist
if 'useVirtualTorques' in conf:
_v = conf[ 'useVirtualTorques' ]
if isinstance(_v, str):
_type = _v
_which = True, True, True
else:
assert isinstance(_v, dict)
_type = _v["type"]
_which = _v["which"]
if _type == 'random_walk':
if _which[ 0 ]:
dae.addU('dt1_disturbance')
mx_dist += dae['rho_sref_v2'] * conf['bref'] * dae.addX('t1_disturbance')
if _which[ 1 ]:
dae.addU('dt2_disturbance')
my_dist += dae['rho_sref_v2'] * conf['cref'] * dae.addX('t2_disturbance')
if _which[ 2 ]:
dae.addU('dt3_disturbance')
mz_dist += dae['rho_sref_v2'] * conf['bref'] * dae.addX('t3_disturbance')
elif _type == 'parameter':
if _which[ 0 ]:
mx_dist += dae['rho_sref_v2'] * conf['bref'] * dae.addX('t1_disturbance')
if _which[ 1 ]:
my_dist += dae['rho_sref_v2'] * conf['cref'] * dae.addX('t2_disturbance')
if _which[ 2 ]:
mz_dist += dae['rho_sref_v2'] * conf['bref'] * dae.addX('t3_disturbance')
elif _type == 'online_data':
if _which[ 0 ]:
mx_dist += dae['rho_sref_v2'] * conf['bref'] * dae.addP('t1_disturbance')
if _which[ 1 ]:
my_dist += dae['rho_sref_v2'] * conf['cref'] * dae.addP('t2_disturbance')
if _which[ 2 ]:
mz_dist += dae['rho_sref_v2'] * conf['bref'] * dae.addP('t3_disturbance')
else:
raise ValueError("WTF?")
t1 += mx_dist
t2 += my_dist
t3 += mz_dist
dae["aero_mx_dist"] = mx_dist
dae["aero_my_dist"] = my_dist
dae["aero_mz_dist"] = mz_dist
# mass matrix
mm = C.SXMatrix(8, 8)
mm[0,0] = jCarousel + m*rA*rA + m*x*x + m*y*y + 2*m*rA*x
mm[0,1] = -m*y
mm[0,2] = m*(rA + x)
mm[0,3] = 0
mm[0,4] = 0
mm[0,5] = 0
mm[0,6] = 0
mm[0,7] = 0
mm[1,0] = -m*y
mm[1,1] = m
mm[1,2] = 0
mm[1,3] = 0
mm[1,4] = 0
mm[1,5] = 0
mm[1,6] = 0
mm[1,7] = x + zt*e31
mm[2,0] = m*(rA + x)
mm[2,1] = 0
mm[2,2] = m
mm[2,3] = 0
mm[2,4] = 0
mm[2,5] = 0
mm[2,6] = 0
mm[2,7] = y + zt*e32
mm[3,0] = 0
mm[3,1] = 0
mm[3,2] = 0
mm[3,3] = m
mm[3,4] = 0
mm[3,5] = 0
mm[3,6] = 0
mm[3,7] = z + zt*e33
mm[4,0] = 0
mm[4,1] = 0
mm[4,2] = 0
mm[4,3] = 0
mm[4,4] = j1
mm[4,5] = 0
mm[4,6] = j31
mm[4,7] = -zt*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33)
mm[5,0] = 0
mm[5,1] = 0
mm[5,2] = 0
mm[5,3] = 0
mm[5,4] = 0
mm[5,5] = j2
mm[5,6] = 0
mm[5,7] = zt*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33)
mm[6,0] = 0
mm[6,1] = 0
mm[6,2] = 0
mm[6,3] = 0
mm[6,4] = j31
mm[6,5] = 0
mm[6,6] = j3
mm[6,7] = 0
mm[7,0] = -zt*(e11*e23*x - e13*e21*x + e12*e23*y - e13*e22*y + zt*e11*e23*e31 - zt*e13*e21*e31 + zt*e12*e23*e32 - zt*e13*e22*e32)
mm[7,1] = x + zt*e31
mm[7,2] = y + zt*e32
mm[7,3] = z + zt*e33
mm[7,4] = -zt*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33)
mm[7,5] = zt*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33)
mm[7,6] = 0
mm[7,7] = 0
# right hand side
zt2 = zt*zt
rhs = C.veccat(
[ tc - cfric*ddelta - f1*y + f2*(rA + x) + dy*m*(dx - 2*ddelta*y) - dx*m*(dy + 2*ddelta*rA + 2*ddelta*x)
, f1 + ddelta*m*(dy + ddelta*rA + ddelta*x) + ddelta*dy*m
, f2 - ddelta*m*(dx - ddelta*y) - ddelta*dx*m
, f3 + g*m
, t1 - w2*(j3*w3 + j31*w1) + j2*w2*w3
, t2 + w1*(j3*w3 + j31*w1) - w3*(j1*w1 + j31*w3) + t_xt
, t3 + w2*(j1*w1 + j31*w3) - j2*w1*w2
, ddr*r-(zt*w1*(e11*x+e12*y+e13*z+zt*e11*e31+zt*e12*e32+zt*e13*e33)+zt*w2*(e21*x+e22*y+e23*z+zt*e21*e31+zt*e22*e32+zt*e23*e33))*(w3-ddelta*e33)-dx*(dx-zt*e21*(w1-ddelta*e13)+zt*e11*(w2-ddelta*e23))-dy*(dy-zt*e22*(w1-ddelta*e13)+zt*e12*(w2-ddelta*e23))-dz*(dz-zt*e23*(w1-ddelta*e13)+zt*e13*(w2-ddelta*e23))+dr*dr+(w1-ddelta*e13)*(e21*(zt*dx-zt2*e21*(w1-ddelta*e13)+zt2*e11*(w2-ddelta*e23))+e22*(zt*dy-zt2*e22*(w1-ddelta*e13)+zt2*e12*(w2-ddelta*e23))+zt*e23*(dz+zt*e13*w2-zt*e23*w1)+zt*e33*(w1*z+zt*e33*w1+ddelta*e11*x+ddelta*e12*y+zt*ddelta*e11*e31+zt*ddelta*e12*e32)+zt*e31*(x+zt*e31)*(w1-ddelta*e13)+zt*e32*(y+zt*e32)*(w1-ddelta*e13))-(w2-ddelta*e23)*(e11*(zt*dx-zt2*e21*(w1-ddelta*e13)+zt2*e11*(w2-ddelta*e23))+e12*(zt*dy-zt2*e22*(w1-ddelta*e13)+zt2*e12*(w2-ddelta*e23))+zt*e13*(dz+zt*e13*w2-zt*e23*w1)-zt*e33*(w2*z+zt*e33*w2+ddelta*e21*x+ddelta*e22*y+zt*ddelta*e21*e31+zt*ddelta*e22*e32)-zt*e31*(x+zt*e31)*(w2-ddelta*e23)-zt*e32*(y+zt*e32)*(w2-ddelta*e23))
] )
dRexp = C.SXMatrix(3,3)
dRexp[0,0] = e21*(w3 - ddelta*e33) - e31*(w2 - ddelta*e23)
dRexp[0,1] = e31*(w1 - ddelta*e13) - e11*(w3 - ddelta*e33)
dRexp[0,2] = e11*(w2 - ddelta*e23) - e21*(w1 - ddelta*e13)
dRexp[1,0] = e22*(w3 - ddelta*e33) - e32*(w2 - ddelta*e23)
dRexp[1,1] = e32*(w1 - ddelta*e13) - e12*(w3 - ddelta*e33)
dRexp[1,2] = e12*(w2 - ddelta*e23) - e22*(w1 - ddelta*e13)
dRexp[2,0] = e23*w3 - e33*w2
dRexp[2,1] = e33*w1 - e13*w3
dRexp[2,2] = e13*w2 - e23*w1
# The cable constraint
c =(x + zt*e31)**2/2 + (y + zt*e32)**2/2 + (z + zt*e33)**2/2 - r**2/2
cdot =dx*(x + zt*e31) + dy*(y + zt*e32) + dz*(z + zt*e33) + zt*(w2 - ddelta*e23)*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) - zt*(w1 - ddelta*e13)*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33) - r*dr
# ddx = dae['ddx']
# ddy = dae['ddy']
# ddz = dae['ddz']
# dw1 = dae['dw1']
# dw2 = dae['dw2']
# dddelta = dae['dddelta']
ddx = dae.ddt('dx')
ddy = dae.ddt('dy')
ddz = dae.ddt('dz')
dw1 = dae.ddt('w_bn_b_x')
dw2 = dae.ddt('w_bn_b_y')
dddelta = dae.ddt('ddelta')
cddot = -(w1-ddelta*e13)*(zt*e23*(dz+zt*e13*w2-zt*e23*w1)+zt*e33*(w1*z+zt*e33*w1+ddelta*e11*x+ddelta*e12*y+zt*ddelta*e11*e31+zt*ddelta*e12*e32)+zt*e21*(dx+zt*e11*w2-zt*e21*w1-zt*ddelta*e11*e23+zt*ddelta*e13*e21)+zt*e22*(dy+zt*e12*w2-zt*e22*w1-zt*ddelta*e12*e23+zt*ddelta*e13*e22)+zt*e31*(x+zt*e31)*(w1-ddelta*e13)+zt*e32*(y+zt*e32)*(w1-ddelta*e13))+(w2-ddelta*e23)*(zt*e13*(dz+zt*e13*w2-zt*e23*w1)-zt*e33*(w2*z+zt*e33*w2+ddelta*e21*x+ddelta*e22*y+zt*ddelta*e21*e31+zt*ddelta*e22*e32)+zt*e11*(dx+zt*e11*w2-zt*e21*w1-zt*ddelta*e11*e23+zt*ddelta*e13*e21)+zt*e12*(dy+zt*e12*w2-zt*e22*w1-zt*ddelta*e12*e23+zt*ddelta*e13*e22)-zt*e31*(x+zt*e31)*(w2-ddelta*e23)-zt*e32*(y+zt*e32)*(w2-ddelta*e23))-ddr*r+(zt*w1*(e11*x+e12*y+e13*z+zt*e11*e31+zt*e12*e32+zt*e13*e33)+zt*w2*(e21*x+e22*y+e23*z+zt*e21*e31+zt*e22*e32+zt*e23*e33))*(w3-ddelta*e33)+dx*(dx+zt*e11*w2-zt*e21*w1-zt*ddelta*e11*e23+zt*ddelta*e13*e21)+dy*(dy+zt*e12*w2-zt*e22*w1-zt*ddelta*e12*e23+zt*ddelta*e13*e22)+dz*(dz+zt*e13*w2-zt*e23*w1)+ddx*(x+zt*e31)+ddy*(y+zt*e32)+ddz*(z+zt*e33)-dr*dr+zt*(dw2-dddelta*e23)*(e11*x+e12*y+e13*z+zt*e11*e31+zt*e12*e32+zt*e13*e33)-zt*(dw1-dddelta*e13)*(e21*x+e22*y+e23*z+zt*e21*e31+zt*e22*e32+zt*e23*e33)-zt*dddelta*(e11*e23*x-e13*e21*x+e12*e23*y-e13*e22*y+zt*e11*e23*e31-zt*e13*e21*e31+zt*e12*e23*e32-zt*e13*e22*e32)
# cddot = (zt*w1*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) + zt*w2*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33))*(w3 - ddelta*e33) + dx*(dx + zt*e11*w2 - zt*e21*w1 - zt*ddelta*e11*e23 + zt*ddelta*e13*e21) + dy*(dy + zt*e12*w2 - zt*e22*w1 - zt*ddelta*e12*e23 + zt*ddelta*e13*e22) + dz*(dz + zt*e13*w2 - zt*e23*w1) + ddx*(x + zt*e31) + ddy*(y + zt*e32) + ddz*(z + zt*e33) - (w1 - ddelta*e13)*(e21*(zt*dx - zt**2*e21*(w1 - ddelta*e13) + zt**2*e11*(w2 - ddelta*e23)) + e22*(zt*dy - zt**2*e22*(w1 - ddelta*e13) + zt**2*e12*(w2 - ddelta*e23)) + zt*e33*(z*w1 + ddelta*e11*x + ddelta*e12*y + zt*e33*w1 + zt*ddelta*e11*e31 + zt*ddelta*e12*e32) + zt*e23*(dz + zt*e13*w2 - zt*e23*w1) + zt*e31*(w1 - ddelta*e13)*(x + zt*e31) + zt*e32*(w1 - ddelta*e13)*(y + zt*e32)) + (w2 - ddelta*e23)*(e11*(zt*dx - zt**2*e21*(w1 - ddelta*e13) + zt**2*e11*(w2 - ddelta*e23)) + e12*(zt*dy - zt**2*e22*(w1 - ddelta*e13) + zt**2*e12*(w2 - ddelta*e23)) - zt*e33*(z*w2 + ddelta*e21*x + ddelta*e22*y + zt*e33*w2 + zt*ddelta*e21*e31 + zt*ddelta*e22*e32) + zt*e13*(dz + zt*e13*w2 - zt*e23*w1) - zt*e31*(w2 - ddelta*e23)*(x + zt*e31) - zt*e32*(w2 - ddelta*e23)*(y + zt*e32)) + zt*(dw2 - dddelta*e23)*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) - zt*(dw1 - dddelta*e13)*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33) - zt*dddelta*(e11*e23*x - e13*e21*x + e12*e23*y - e13*e22*y + zt*e11*e23*e31 - zt*e13*e21*e31 + zt*e12*e23*e32 - zt*e13*e22*e32)
# where
# dw1 = dw @> 0
# dw2 = dw @> 1
# {-
# dw3 = dw @> 2
# -}
# ddx = ddX @> 0
# ddy = ddX @> 1
# ddz = ddX @> 2
# dddelta = dddelta' @> 0
dae['c'] = c
dae['cdot'] = cdot
dae['cddot'] = cddot
return (mm, rhs, dRexp)
def setupModel(dae, conf):
'''
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
'''
m = conf['mass']
g = conf['g']
j1 = conf['j1']
j31 = conf['j31']
j2 = conf['j2']
j3 = conf['j3']
jCarousel = conf['jCarousel']
cfric = conf['cfric']
zt = conf['zt']
rA = conf['rArm']
e11 = dae['e11']
e12 = dae['e12']
e13 = dae['e13']
e21 = dae['e21']
e22 = dae['e22']
e23 = dae['e23']
e31 = dae['e31']
e32 = dae['e32']
e33 = dae['e33']
x = dae['x']
y = dae['y']
z = dae['z']
dx = dae['dx']
dy = dae['dy']
dz = dae['dz']
w1 = dae['w_bn_b_x']
w2 = dae['w_bn_b_y']
w3 = dae['w_bn_b_z']
ddelta = dae['ddelta']
r = dae['r']
dr = dae['dr']
ddr = dae['ddr']
tc = dae['motor_torque'] #Carousel motor torque
# wind model
def getWind():
if 'wind_model' not in conf:
return 0
if conf['wind_model']['name'] == 'wind_shear':
# use a logarithmic wind shear model
# wind(z) = w0 * log((z+zt)/zt) / log(z0/zt)
# where w0 is wind at z0 altitude
# zt is surface roughness characteristic length
z0 = conf['wind_model']['z0']
zt_roughness = conf['wind_model']['zt_roughness']
zsat = 0.5 * (-z + C.sqrt(z * z))
return dae['w0'] * C.log(
(zsat + zt_roughness + 2) / zt_roughness) / C.log(
z0 / zt_roughness)
elif conf['wind_model']['name'] == 'constant':
# constant wind
return dae['w0']
wind_x = getWind()
dae['wind_at_altitude'] = wind_x
dp_carousel_frame = C.veccat([
dx - ddelta * y, dy + ddelta * (rA + x), dz
]) - C.veccat([dae['cos_delta'] * wind_x, dae['sin_delta'] * wind_x, 0])
# Aircraft velocity w.r.t. inertial frame, given in its own reference frame
# (needed to compute the aero forces and torques !)
dpE = C.mul(dae['R_c2b'], dp_carousel_frame)
(f1, f2, f3, t1, t2,
t3) = aeroForcesTorques(dae, conf, dp_carousel_frame, dpE, dae['w_bn_b'],
(dae['e21'], dae['e22'], dae['e23']))
# if we are running a homotopy, add psudeo forces and moments as algebraic states
if 'runHomotopy' in conf and conf['runHomotopy']:
gamma_homotopy = dae.addP('gamma_homotopy')
f1 = f1 * gamma_homotopy + dae.addZ('f1_homotopy') * (1 -
gamma_homotopy)
f2 = f2 * gamma_homotopy + dae.addZ('f2_homotopy') * (1 -
gamma_homotopy)
f3 = f3 * gamma_homotopy + dae.addZ('f3_homotopy') * (1 -
gamma_homotopy)
t1 = t1 * gamma_homotopy + dae.addZ('t1_homotopy') * (1 -
gamma_homotopy)
t2 = t2 * gamma_homotopy + dae.addZ('t2_homotopy') * (1 -
gamma_homotopy)
t3 = t3 * gamma_homotopy + dae.addZ('t3_homotopy') * (1 -
gamma_homotopy)
# mass matrix
mm = C.SXMatrix(8, 8)
mm[0, 0] = jCarousel + m * rA * rA + m * x * x + m * y * y + 2 * m * rA * x
mm[0, 1] = -m * y
mm[0, 2] = m * (rA + x)
mm[0, 3] = 0
mm[0, 4] = 0
mm[0, 5] = 0
mm[0, 6] = 0
mm[0, 7] = 0
mm[1, 0] = -m * y
mm[1, 1] = m
mm[1, 2] = 0
mm[1, 3] = 0
mm[1, 4] = 0
mm[1, 5] = 0
mm[1, 6] = 0
mm[1, 7] = x + zt * e31
mm[2, 0] = m * (rA + x)
mm[2, 1] = 0
mm[2, 2] = m
mm[2, 3] = 0
mm[2, 4] = 0
mm[2, 5] = 0
mm[2, 6] = 0
mm[2, 7] = y + zt * e32
mm[3, 0] = 0
mm[3, 1] = 0
mm[3, 2] = 0
mm[3, 3] = m
mm[3, 4] = 0
mm[3, 5] = 0
mm[3, 6] = 0
mm[3, 7] = z + zt * e33
mm[4, 0] = 0
mm[4, 1] = 0
mm[4, 2] = 0
mm[4, 3] = 0
mm[4, 4] = j1
mm[4, 5] = 0
mm[4, 6] = j31
mm[4, 7] = -zt * (e21 * x + e22 * y + e23 * z + zt * e21 * e31 +
zt * e22 * e32 + zt * e23 * e33)
mm[5, 0] = 0
mm[5, 1] = 0
mm[5, 2] = 0
mm[5, 3] = 0
mm[5, 4] = 0
mm[5, 5] = j2
mm[5, 6] = 0
mm[5, 7] = zt * (e11 * x + e12 * y + e13 * z + zt * e11 * e31 +
zt * e12 * e32 + zt * e13 * e33)
mm[6, 0] = 0
mm[6, 1] = 0
mm[6, 2] = 0
mm[6, 3] = 0
mm[6, 4] = j31
mm[6, 5] = 0
mm[6, 6] = j3
mm[6, 7] = 0
mm[7,
0] = -zt * (e11 * e23 * x - e13 * e21 * x + e12 * e23 * y -
e13 * e22 * y + zt * e11 * e23 * e31 - zt * e13 * e21 * e31
+ zt * e12 * e23 * e32 - zt * e13 * e22 * e32)
mm[7, 1] = x + zt * e31
mm[7, 2] = y + zt * e32
mm[7, 3] = z + zt * e33
mm[7, 4] = -zt * (e21 * x + e22 * y + e23 * z + zt * e21 * e31 +
zt * e22 * e32 + zt * e23 * e33)
mm[7, 5] = zt * (e11 * x + e12 * y + e13 * z + zt * e11 * e31 +
zt * e12 * e32 + zt * e13 * e33)
mm[7, 6] = 0
mm[7, 7] = 0
# right hand side
zt2 = zt * zt
rhs = C.veccat([
tc - cfric * ddelta - f1 * y + f2 * (rA + x) + dy * m *
(dx - 2 * ddelta * y) - dx * m *
(dy + 2 * ddelta * rA + 2 * ddelta * x),
f1 + ddelta * m * (dy + ddelta * rA + ddelta * x) + ddelta * dy * m,
f2 - ddelta * m * (dx - ddelta * y) - ddelta * dx * m, f3 + g * m,
t1 - w2 * (j3 * w3 + j31 * w1) + j2 * w2 * w3,
t2 + w1 * (j3 * w3 + j31 * w1) - w3 * (j1 * w1 + j31 * w3),
t3 + w2 * (j1 * w1 + j31 * w3) - j2 * w1 * w2,
ddr * r - (zt * w1 * (e11 * x + e12 * y + e13 * z + zt * e11 * e31 +
zt * e12 * e32 + zt * e13 * e33) + zt * w2 *
(e21 * x + e22 * y + e23 * z + zt * e21 * e31 +
zt * e22 * e32 + zt * e23 * e33)) * (w3 - ddelta * e33) -
dx * (dx - zt * e21 * (w1 - ddelta * e13) + zt * e11 *
(w2 - ddelta * e23)) - dy * (dy - zt * e22 *
(w1 - ddelta * e13) + zt * e12 *
(w2 - ddelta * e23)) - dz *
(dz - zt * e23 * (w1 - ddelta * e13) + zt * e13 *
(w2 - ddelta * e23)) + dr * dr + (w1 - ddelta * e13) *
(e21 * (zt * dx - zt2 * e21 * (w1 - ddelta * e13) + zt2 * e11 *
(w2 - ddelta * e23)) + e22 *
(zt * dy - zt2 * e22 * (w1 - ddelta * e13) + zt2 * e12 *
(w2 - ddelta * e23)) + zt * e23 *
(dz + zt * e13 * w2 - zt * e23 * w1) + zt * e33 *
(w1 * z + zt * e33 * w1 + ddelta * e11 * x + ddelta * e12 * y +
zt * ddelta * e11 * e31 + zt * ddelta * e12 * e32) + zt * e31 *
(x + zt * e31) * (w1 - ddelta * e13) + zt * e32 * (y + zt * e32) *
(w1 - ddelta * e13)) - (w2 - ddelta * e23) *
(e11 * (zt * dx - zt2 * e21 * (w1 - ddelta * e13) + zt2 * e11 *
(w2 - ddelta * e23)) + e12 *
(zt * dy - zt2 * e22 * (w1 - ddelta * e13) + zt2 * e12 *
(w2 - ddelta * e23)) + zt * e13 *
(dz + zt * e13 * w2 - zt * e23 * w1) - zt * e33 *
(w2 * z + zt * e33 * w2 + ddelta * e21 * x + ddelta * e22 * y +
zt * ddelta * e21 * e31 + zt * ddelta * e22 * e32) - zt * e31 *
(x + zt * e31) * (w2 - ddelta * e23) - zt * e32 * (y + zt * e32) *
(w2 - ddelta * e23))
])
dRexp = C.SXMatrix(3, 3)
dRexp[0, 0] = e21 * (w3 - ddelta * e33) - e31 * (w2 - ddelta * e23)
dRexp[0, 1] = e31 * (w1 - ddelta * e13) - e11 * (w3 - ddelta * e33)
dRexp[0, 2] = e11 * (w2 - ddelta * e23) - e21 * (w1 - ddelta * e13)
dRexp[1, 0] = e22 * (w3 - ddelta * e33) - e32 * (w2 - ddelta * e23)
dRexp[1, 1] = e32 * (w1 - ddelta * e13) - e12 * (w3 - ddelta * e33)
dRexp[1, 2] = e12 * (w2 - ddelta * e23) - e22 * (w1 - ddelta * e13)
dRexp[2, 0] = e23 * w3 - e33 * w2
dRexp[2, 1] = e33 * w1 - e13 * w3
dRexp[2, 2] = e13 * w2 - e23 * w1
c = (x + zt * e31)**2 / 2 + (y + zt * e32)**2 / 2 + (
z + zt * e33)**2 / 2 - r**2 / 2
cdot = dx * (x + zt * e31) + dy * (y + zt * e32) + dz * (
z + zt * e33) + zt * (w2 - ddelta * e23) * (
e11 * x + e12 * y + e13 * z + zt * e11 * e31 + zt * e12 * e32 +
zt * e13 * e33) - zt * (w1 - ddelta * e13) * (
e21 * x + e22 * y + e23 * z + zt * e21 * e31 + zt * e22 * e32 +
zt * e23 * e33) - r * dr
# ddx = dae['ddx']
# ddy = dae['ddy']
# ddz = dae['ddz']
# dw1 = dae['dw1']
# dw2 = dae['dw2']
# dddelta = dae['dddelta']
ddx = dae.ddt('dx')
ddy = dae.ddt('dy')
ddz = dae.ddt('dz')
dw1 = dae.ddt('w_bn_b_x')
dw2 = dae.ddt('w_bn_b_y')
dddelta = dae.ddt('ddelta')
cddot = -(w1 - ddelta * e13) * (
zt * e23 * (dz + zt * e13 * w2 - zt * e23 * w1) + zt * e33 *
(w1 * z + zt * e33 * w1 + ddelta * e11 * x + ddelta * e12 * y +
zt * ddelta * e11 * e31 + zt * ddelta * e12 * e32) + zt * e21 *
(dx + zt * e11 * w2 - zt * e21 * w1 - zt * ddelta * e11 * e23 +
zt * ddelta * e13 * e21) + zt * e22 *
(dy + zt * e12 * w2 - zt * e22 * w1 - zt * ddelta * e12 * e23 +
zt * ddelta * e13 * e22) + zt * e31 * (x + zt * e31) *
(w1 - ddelta * e13) + zt * e32 * (y + zt * e32) * (w1 - ddelta * e13)
) + (w2 - ddelta * e23) * (
zt * e13 * (dz + zt * e13 * w2 - zt * e23 * w1) - zt * e33 *
(w2 * z + zt * e33 * w2 + ddelta * e21 * x + ddelta * e22 * y +
zt * ddelta * e21 * e31 + zt * ddelta * e22 * e32) + zt * e11 *
(dx + zt * e11 * w2 - zt * e21 * w1 - zt * ddelta * e11 * e23 +
zt * ddelta * e13 * e21) + zt * e12 *
(dy + zt * e12 * w2 - zt * e22 * w1 - zt * ddelta * e12 * e23 +
zt * ddelta * e13 * e22) - zt * e31 * (x + zt * e31) *
(w2 - ddelta * e23) - zt * e32 * (y + zt * e32) *
(w2 - ddelta * e23)) - ddr * r + (
zt * w1 *
(e11 * x + e12 * y + e13 * z + zt * e11 * e31 + zt * e12 * e32 +
zt * e13 * e33) + zt * w2 *
(e21 * x + e22 * y + e23 * z + zt * e21 * e31 + zt * e22 * e32 +
zt * e23 * e33)) * (w3 - ddelta * e33) + dx * (
dx + zt * e11 * w2 - zt * e21 * w1 - zt * ddelta * e11 * e23 +
zt * ddelta * e13 * e21) + dy * (
dy + zt * e12 * w2 - zt * e22 * w1 -
zt * ddelta * e12 * e23 + zt * ddelta * e13 * e22
) + dz * (dz + zt * e13 * w2 - zt * e23 * w1) + ddx * (
x + zt * e31) + ddy * (y + zt * e32) + ddz * (
z +
zt * e33) - dr * dr + zt * (dw2 - dddelta * e23) * (
e11 * x + e12 * y + e13 * z + zt * e11 * e31 +
zt * e12 * e32 +
zt * e13 * e33) - zt * (dw1 - dddelta * e13) * (
e21 * x + e22 * y + e23 * z + zt * e21 * e31 +
zt * e22 * e32 +
zt * e23 * e33) - zt * dddelta * (
e11 * e23 * x - e13 * e21 * x +
e12 * e23 * y - e13 * e22 * y + zt * e11 *
e23 * e31 - zt * e13 * e21 * e31 + zt *
e12 * e23 * e32 - zt * e13 * e22 * e32)
# cddot = (zt*w1*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) + zt*w2*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33))*(w3 - ddelta*e33) + dx*(dx + zt*e11*w2 - zt*e21*w1 - zt*ddelta*e11*e23 + zt*ddelta*e13*e21) + dy*(dy + zt*e12*w2 - zt*e22*w1 - zt*ddelta*e12*e23 + zt*ddelta*e13*e22) + dz*(dz + zt*e13*w2 - zt*e23*w1) + ddx*(x + zt*e31) + ddy*(y + zt*e32) + ddz*(z + zt*e33) - (w1 - ddelta*e13)*(e21*(zt*dx - zt**2*e21*(w1 - ddelta*e13) + zt**2*e11*(w2 - ddelta*e23)) + e22*(zt*dy - zt**2*e22*(w1 - ddelta*e13) + zt**2*e12*(w2 - ddelta*e23)) + zt*e33*(z*w1 + ddelta*e11*x + ddelta*e12*y + zt*e33*w1 + zt*ddelta*e11*e31 + zt*ddelta*e12*e32) + zt*e23*(dz + zt*e13*w2 - zt*e23*w1) + zt*e31*(w1 - ddelta*e13)*(x + zt*e31) + zt*e32*(w1 - ddelta*e13)*(y + zt*e32)) + (w2 - ddelta*e23)*(e11*(zt*dx - zt**2*e21*(w1 - ddelta*e13) + zt**2*e11*(w2 - ddelta*e23)) + e12*(zt*dy - zt**2*e22*(w1 - ddelta*e13) + zt**2*e12*(w2 - ddelta*e23)) - zt*e33*(z*w2 + ddelta*e21*x + ddelta*e22*y + zt*e33*w2 + zt*ddelta*e21*e31 + zt*ddelta*e22*e32) + zt*e13*(dz + zt*e13*w2 - zt*e23*w1) - zt*e31*(w2 - ddelta*e23)*(x + zt*e31) - zt*e32*(w2 - ddelta*e23)*(y + zt*e32)) + zt*(dw2 - dddelta*e23)*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) - zt*(dw1 - dddelta*e13)*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33) - zt*dddelta*(e11*e23*x - e13*e21*x + e12*e23*y - e13*e22*y + zt*e11*e23*e31 - zt*e13*e21*e31 + zt*e12*e23*e32 - zt*e13*e22*e32)
# where
# dw1 = dw @> 0
# dw2 = dw @> 1
# {-
# dw3 = dw @> 2
# -}
# ddx = ddX @> 0
# ddy = ddX @> 1
# ddz = ddX @> 2
# dddelta = dddelta' @> 0
dae['c'] = c
dae['cdot'] = cdot
dae['cddot'] = cddot
return (mm, rhs, dRexp)
def _convert_to_ode(self):
if not self._ode_conversion:
self._ode_conversion = True
self.ocp.makeExplicit()
self.update_expressions()
if len(self.ocp.z) > 0 or self.ocp.ode.empty():
raise RuntimeError("Unable to reformulate as ODE.")
t0 = self.ocp.variable('startTime').getStart()
tf = self.ocp.variable('finalTime').getStart()
self.ocp_ode_inputs = []
self.ocp_ode_inputs += list(self.p)
self.ocp_ode_inputs += list(self.x)
self.ocp_ode_inputs += list(self.u)
self.ocp_ode_inputs += [self.t]
self.ocp_ode_init_inputs = []
self.ocp_ode_init_inputs += list(self.p)
self.ocp_ode_init_inputs += list(self.x)
self.ocp_ode_init_inputs += [self.t]
self.ode_F = casadi.SXFunction(
[casadi.vertcat(self.ocp_ode_inputs)], [self.ode])
self.ode_F.init()
# The initial equations
self.ode_F0 = casadi.SXFunction(
[casadi.vertcat(self.ocp_ode_init_inputs)], [self.initial])
self.ode_F0.init()
# The Lagrange cost function
if self.lterm.numel() > 0:
self.opt_ode_L = casadi.SXFunction(
[casadi.vertcat(self.ocp_ode_inputs)],
[self.lterm[0]])
self.opt_ode_L.init()
else:
self.opt_ode_L = None
# The Mayer cost function
if self.mterm.numel() > 0:
self.ocp_ode_mterm_inputs = []
self.ocp_ode_mterm_inputs += list(self.p)
self.ocp_ode_mterm_inputs += [
x.atTime(tf, True) for x in self.ocp.x]
self.ocp_ode_mterm_inputs += [self.t]
self.opt_ode_J = casadi.SXFunction(
[casadi.vertcat(self.ocp_ode_mterm_inputs)],
[self.mterm[0]])
self.opt_ode_J.init()
else:
self.opt_ode_J = None
# Boundary Constraints
self.opt_ode_Cineq = [] #Inequality
self.opt_ode_C = [] #Equality
# Modify equality constraints to be on type g(x)=0 (instead of g(x)=a)
lb = N.array(self.ocp.point_min, dtype=N.float)
ub = N.array(self.ocp.point_max, dtype=N.float)
for i in range(len(ub)):
if lb[i] == ub[i]: #The constraint is an equality
self.opt_ode_C += [self.point[i] -
self.ocp.point_max[i]]
#self.ocp.cfcn_ub[i] = casadi.SX(0.0)
#self.ocp.cfcn_lb[i] = casadi.SX(0.0)
else: #The constraint is an inequality
if lb[i] == -N.inf:
self.opt_ode_Cineq += [(1.0) * self.point[i] -
self.ocp.point_max[i]]
elif ub[i] == N.inf:
self.opt_ode_Cineq += [(-1.0) * self.point[i] +
self.ocp.point_min[i]]
else:
self.opt_ode_Cineq += [(1.0) * self.point[i] -
self.ocp.point_max[i]]
self.opt_ode_Cineq += [(-1.0) * self.point[i] +
self.ocp.point_min[i]]
self.ocp_ode_boundary_inputs = []
self.ocp_ode_boundary_inputs += list(self.p)
self.ocp_ode_boundary_inputs += [x.atTime(t0, True) for
x in self.ocp.x]
self.ocp_ode_boundary_inputs += [x.atTime(tf, True) for
x in self.ocp.x]
self.opt_ode_C = casadi.SXFunction(
[casadi.SXMatrix(self.ocp_ode_boundary_inputs)],
[casadi.vertcat(self.opt_ode_C)])
self.opt_ode_C.init()
self.opt_ode_Cineq = casadi.SXFunction(
[casadi.SXMatrix(self.ocp_ode_boundary_inputs)],
[casadi.vertcat(self.opt_ode_Cineq)])
self.opt_ode_Cineq.init()
def setupModel(dae, conf):
# PARAMETERS OF THE KITE :
# ##############
m = conf['mass'] # mass of the kite # [ kg ]
# PHYSICAL CONSTANTS :
# ##############
g = conf['g'] # gravitational constant # [ m /s^2]
# PARAMETERS OF THE CABLE :
# ##############
#INERTIA MATRIX (Kurt's direct measurements)
j1 = conf['j1']
j31 = conf['j31']
j2 = conf['j2']
j3 = conf['j3']
#Carousel Friction & inertia
zt = conf['zt']
########### model integ ###################
e11 = dae['e11']
e12 = dae['e12']
e13 = dae['e13']
e21 = dae['e21']
e22 = dae['e22']
e23 = dae['e23']
e31 = dae['e31']
e32 = dae['e32']
e33 = dae['e33']
x = dae['x']
y = dae['y']
z = dae['z']
dx = dae['dx']
dy = dae['dy']
dz = dae['dz']
w1 = dae['w1']
w2 = dae['w2']
w3 = dae['w3']
r = dae['r']
# wind
def getWind():
z0 = conf['z0']
zt_roughness = conf['zt_roughness']
zsat = 0.5*(z+C.sqrt(z*z))
wind_x = dae['w0']*C.log((zsat+zt_roughness+2)/zt_roughness)/C.log(z0/zt_roughness)
# wind_x = dae['w0']
return wind_x
dae['wind_at_altitude'] = getWind()
v_bw_n = C.veccat( [ dx - dae['wind_at_altitude'], dy, dz ] )
R_n2b = C.veccat( [dae[n] for n in ['e11', 'e12', 'e13',
'e21', 'e22', 'e23',
'e31', 'e32', 'e33']]
).reshape((3,3))
# Aircraft velocity w.r.t. inertial frame, given in its own reference frame
# (needed to compute the aero forces and torques !)
v_bw_b = C.mul( R_n2b, v_bw_n )
(f1, f2, f3, t1, t2, t3) = aeroForcesTorques(dae, conf, v_bw_n, v_bw_b,
(dae['w1'], dae['w2'], dae['w3']),
(dae['e21'], dae['e22'], dae['e23']),
(dae['aileron'],dae['elevator'])
)
dae['prop_drag_vector'] = dae['prop_drag']*v_bw_n/dae['airspeed']
dae['prop_power'] = C.mul(dae['prop_drag_vector'].T, v_bw_n)
f1 -= dae['prop_drag_vector'][0]
f2 -= dae['prop_drag_vector'][1]
f3 -= dae['prop_drag_vector'][2]
if 'runHomotopy' in conf and conf['runHomotopy']:
gamma_homotopy = dae.addP('gamma_homotopy')
f1 = f1 * gamma_homotopy + dae.addZ('f1_homotopy') * (1 - gamma_homotopy)
f2 = f2 * gamma_homotopy + dae.addZ('f2_homotopy') * (1 - gamma_homotopy)
f3 = f3 * gamma_homotopy + (dae.addZ('f3_homotopy') - g*m) * (1 - gamma_homotopy)
t1 = t1 * gamma_homotopy + dae.addZ('t1_homotopy') * (1 - gamma_homotopy)
t2 = t2 * gamma_homotopy + dae.addZ('t2_homotopy') * (1 - gamma_homotopy)
t3 = t3 * gamma_homotopy + dae.addZ('t3_homotopy') * (1 - gamma_homotopy)
# mass matrix
mm = C.SXMatrix(7, 7)
mm[0,0] = m
mm[0,1] = 0
mm[0,2] = 0
mm[0,3] = 0
mm[0,4] = 0
mm[0,5] = 0
mm[0,6] = x + zt*e31
mm[1,0] = 0
mm[1,1] = m
mm[1,2] = 0
mm[1,3] = 0
mm[1,4] = 0
mm[1,5] = 0
mm[1,6] = y + zt*e32
mm[2,0] = 0
mm[2,1] = 0
mm[2,2] = m
mm[2,3] = 0
mm[2,4] = 0
mm[2,5] = 0
mm[2,6] = z + zt*e33
mm[3,0] = 0
mm[3,1] = 0
mm[3,2] = 0
mm[3,3] = j1
mm[3,4] = 0
mm[3,5] = j31
mm[3,6] = -zt*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33)
mm[4,0] = 0
mm[4,1] = 0
mm[4,2] = 0
mm[4,3] = 0
mm[4,4] = j2
mm[4,5] = 0
mm[4,6] = zt*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33)
mm[5,0] = 0
mm[5,1] = 0
mm[5,2] = 0
mm[5,3] = j31
mm[5,4] = 0
mm[5,5] = j3
mm[5,6] = 0
mm[6,0] = x + zt*e31
mm[6,1] = y + zt*e32
mm[6,2] = z + zt*e33
mm[6,3] = -zt*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33)
mm[6,4] = zt*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33)
mm[6,5] = 0
mm[6,6] = 0
# right hand side
zt2 = zt*zt
ddelta = 0
rA = 0
dr = 0
ddr = 0
rhs = C.veccat(
[ f1 + ddelta*m*(dy + ddelta*rA + ddelta*x) + ddelta*dy*m
, f2 - ddelta*m*(dx - ddelta*y) - ddelta*dx*m
, f3 - g*m
, t1 - w2*(j3*w3 + j31*w1) + j2*w2*w3
, t2 + w1*(j3*w3 + j31*w1) - w3*(j1*w1 + j31*w3)
, t3 + w2*(j1*w1 + j31*w3) - j2*w1*w2
, ddr*r-(zt*w1*(e11*x+e12*y+e13*z+zt*e11*e31+zt*e12*e32+zt*e13*e33)+zt*w2*(e21*x+e22*y+e23*z+zt*e21*e31+zt*e22*e32+zt*e23*e33))*(w3-ddelta*e33)-dx*(dx-zt*e21*(w1-ddelta*e13)+zt*e11*(w2-ddelta*e23))-dy*(dy-zt*e22*(w1-ddelta*e13)+zt*e12*(w2-ddelta*e23))-dz*(dz-zt*e23*(w1-ddelta*e13)+zt*e13*(w2-ddelta*e23))+dr*dr+(w1-ddelta*e13)*(e21*(zt*dx-zt2*e21*(w1-ddelta*e13)+zt2*e11*(w2-ddelta*e23))+e22*(zt*dy-zt2*e22*(w1-ddelta*e13)+zt2*e12*(w2-ddelta*e23))+zt*e23*(dz+zt*e13*w2-zt*e23*w1)+zt*e33*(w1*z+zt*e33*w1+ddelta*e11*x+ddelta*e12*y+zt*ddelta*e11*e31+zt*ddelta*e12*e32)+zt*e31*(x+zt*e31)*(w1-ddelta*e13)+zt*e32*(y+zt*e32)*(w1-ddelta*e13))-(w2-ddelta*e23)*(e11*(zt*dx-zt2*e21*(w1-ddelta*e13)+zt2*e11*(w2-ddelta*e23))+e12*(zt*dy-zt2*e22*(w1-ddelta*e13)+zt2*e12*(w2-ddelta*e23))+zt*e13*(dz+zt*e13*w2-zt*e23*w1)-zt*e33*(w2*z+zt*e33*w2+ddelta*e21*x+ddelta*e22*y+zt*ddelta*e21*e31+zt*ddelta*e22*e32)-zt*e31*(x+zt*e31)*(w2-ddelta*e23)-zt*e32*(y+zt*e32)*(w2-ddelta*e23))
] )
dRexp = C.SXMatrix(3,3)
dRexp[0,0] = e21*(w3 - ddelta*e33) - e31*(w2 - ddelta*e23)
dRexp[0,1] = e31*(w1 - ddelta*e13) - e11*(w3 - ddelta*e33)
dRexp[0,2] = e11*(w2 - ddelta*e23) - e21*(w1 - ddelta*e13)
dRexp[1,0] = e22*(w3 - ddelta*e33) - e32*(w2 - ddelta*e23)
dRexp[1,1] = e32*(w1 - ddelta*e13) - e12*(w3 - ddelta*e33)
dRexp[1,2] = e12*(w2 - ddelta*e23) - e22*(w1 - ddelta*e13)
dRexp[2,0] = e23*w3 - e33*w2
dRexp[2,1] = e33*w1 - e13*w3
dRexp[2,2] = e13*w2 - e23*w1
c =(x + zt*e31)**2/2 + (y + zt*e32)**2/2 + (z + zt*e33)**2/2 - r**2/2
cdot =dx*(x + zt*e31) + dy*(y + zt*e32) + dz*(z + zt*e33) + zt*(w2 - ddelta*e23)*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) - zt*(w1 - ddelta*e13)*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33) - r*dr
ddx = dae.ddt('dx')
ddy = dae.ddt('dy')
ddz = dae.ddt('dz')
dw1 = dae.ddt('w1')
dw2 = dae.ddt('w2')
# ddx = dae['ddx']
# ddy = dae['ddy']
# ddz = dae['ddz']
# dw1 = dae['dw1']
# dw2 = dae['dw2']
dddelta = 0
cddot = -(w1-ddelta*e13)*(zt*e23*(dz+zt*e13*w2-zt*e23*w1)+zt*e33*(w1*z+zt*e33*w1+ddelta*e11*x+ddelta*e12*y+zt*ddelta*e11*e31+zt*ddelta*e12*e32)+zt*e21*(dx+zt*e11*w2-zt*e21*w1-zt*ddelta*e11*e23+zt*ddelta*e13*e21)+zt*e22*(dy+zt*e12*w2-zt*e22*w1-zt*ddelta*e12*e23+zt*ddelta*e13*e22)+zt*e31*(x+zt*e31)*(w1-ddelta*e13)+zt*e32*(y+zt*e32)*(w1-ddelta*e13))+(w2-ddelta*e23)*(zt*e13*(dz+zt*e13*w2-zt*e23*w1)-zt*e33*(w2*z+zt*e33*w2+ddelta*e21*x+ddelta*e22*y+zt*ddelta*e21*e31+zt*ddelta*e22*e32)+zt*e11*(dx+zt*e11*w2-zt*e21*w1-zt*ddelta*e11*e23+zt*ddelta*e13*e21)+zt*e12*(dy+zt*e12*w2-zt*e22*w1-zt*ddelta*e12*e23+zt*ddelta*e13*e22)-zt*e31*(x+zt*e31)*(w2-ddelta*e23)-zt*e32*(y+zt*e32)*(w2-ddelta*e23))-ddr*r+(zt*w1*(e11*x+e12*y+e13*z+zt*e11*e31+zt*e12*e32+zt*e13*e33)+zt*w2*(e21*x+e22*y+e23*z+zt*e21*e31+zt*e22*e32+zt*e23*e33))*(w3-ddelta*e33)+dx*(dx+zt*e11*w2-zt*e21*w1-zt*ddelta*e11*e23+zt*ddelta*e13*e21)+dy*(dy+zt*e12*w2-zt*e22*w1-zt*ddelta*e12*e23+zt*ddelta*e13*e22)+dz*(dz+zt*e13*w2-zt*e23*w1)+ddx*(x+zt*e31)+ddy*(y+zt*e32)+ddz*(z+zt*e33)-dr*dr+zt*(dw2-dddelta*e23)*(e11*x+e12*y+e13*z+zt*e11*e31+zt*e12*e32+zt*e13*e33)-zt*(dw1-dddelta*e13)*(e21*x+e22*y+e23*z+zt*e21*e31+zt*e22*e32+zt*e23*e33)-zt*dddelta*(e11*e23*x-e13*e21*x+e12*e23*y-e13*e22*y+zt*e11*e23*e31-zt*e13*e21*e31+zt*e12*e23*e32-zt*e13*e22*e32)
# cddot = (zt*w1*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) + zt*w2*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33))*(w3 - ddelta*e33) + dx*(dx + zt*e11*w2 - zt*e21*w1 - zt*ddelta*e11*e23 + zt*ddelta*e13*e21) + dy*(dy + zt*e12*w2 - zt*e22*w1 - zt*ddelta*e12*e23 + zt*ddelta*e13*e22) + dz*(dz + zt*e13*w2 - zt*e23*w1) + ddx*(x + zt*e31) + ddy*(y + zt*e32) + ddz*(z + zt*e33) - (w1 - ddelta*e13)*(e21*(zt*dx - zt**2*e21*(w1 - ddelta*e13) + zt**2*e11*(w2 - ddelta*e23)) + e22*(zt*dy - zt**2*e22*(w1 - ddelta*e13) + zt**2*e12*(w2 - ddelta*e23)) + zt*e33*(z*w1 + ddelta*e11*x + ddelta*e12*y + zt*e33*w1 + zt*ddelta*e11*e31 + zt*ddelta*e12*e32) + zt*e23*(dz + zt*e13*w2 - zt*e23*w1) + zt*e31*(w1 - ddelta*e13)*(x + zt*e31) + zt*e32*(w1 - ddelta*e13)*(y + zt*e32)) + (w2 - ddelta*e23)*(e11*(zt*dx - zt**2*e21*(w1 - ddelta*e13) + zt**2*e11*(w2 - ddelta*e23)) + e12*(zt*dy - zt**2*e22*(w1 - ddelta*e13) + zt**2*e12*(w2 - ddelta*e23)) - zt*e33*(z*w2 + ddelta*e21*x + ddelta*e22*y + zt*e33*w2 + zt*ddelta*e21*e31 + zt*ddelta*e22*e32) + zt*e13*(dz + zt*e13*w2 - zt*e23*w1) - zt*e31*(w2 - ddelta*e23)*(x + zt*e31) - zt*e32*(w2 - ddelta*e23)*(y + zt*e32)) + zt*(dw2 - dddelta*e23)*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) - zt*(dw1 - dddelta*e13)*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33) - zt*dddelta*(e11*e23*x - e13*e21*x + e12*e23*y - e13*e22*y + zt*e11*e23*e31 - zt*e13*e21*e31 + zt*e12*e23*e32 - zt*e13*e22*e32)
# where
# dw1 = dw @> 0
# dw2 = dw @> 1
# {-
# dw3 = dw @> 2
# -}
# ddx = ddX @> 0
# ddy = ddX @> 1
# ddz = ddX @> 2
# dddelta = dddelta' @> 0
dae['c'] = c
dae['cdot'] = cdot
dae['cddot'] = cddot
return (mm, rhs, dRexp)
def modelInteg(state, u, p, outputs):
# PARAMETERS OF THE KITE :
# ##############
m = 0.626 # mass of the kite # [ kg ]
# PHYSICAL CONSTANTS :
# ##############
g = 9.81 # gravitational constant # [ m /s^2]
# PARAMETERS OF THE CABLE :
# ##############
#CAROUSEL ARM LENGTH
rA = 1.085 #(dixit Kurt)
zt = -0.01
#INERTIA MATRIX (Kurt's direct measurements)
j1 = 0.0163
j31 = 0.0006
j2 = 0.0078
j3 = 0.0229
#Carousel Friction & inertia
jCarousel = 1e2
# cfric = 100
########### model integ ###################
x = state['x']
y = state['y']
z = state['z']
e11 = state['e11']
e12 = state['e12']
e13 = state['e13']
e21 = state['e21']
e22 = state['e22']
e23 = state['e23']
e31 = state['e31']
e32 = state['e32']
e33 = state['e33']
dx = state['dx']
dy = state['dy']
dz = state['dz']
w1 = state['w1']
w2 = state['w2']
w3 = state['w3']
ddelta = 0
tc = 0 # = u['tc'] #Carousel motor torque
(f1, f2, f3, t1, t2, t3) = forcesTorques(state, u, p, outputs)
# ATTITUDE DYNAMICS
# #############################
# DynFile # Call DAE
# mm :: Matrix (Expr Double)
mm = C.SXMatrix(7, 7)
# mm[0,0] = jCarousel + m*rA*rA + m*x*x + m*y*y + 2*m*rA*x
# mm[0,1] = -m*y
# mm[0,2] = m*(rA + x)
# mm[0,3] = 0
# mm[0,4] = 0
# mm[0,5] = 0
# mm[0,6] = 0
## mm[0,7] = 0
# mm[1,0] = -m*y
mm[1, 1] = m
mm[1, 2] = 0
mm[1, 3] = 0
mm[1, 4] = 0
mm[1, 5] = 0
mm[1, 6] = 0
# mm[1,7] = x + zt*e31
# mm[2,0] = m*(rA + x)
mm[2, 1] = 0
mm[2, 2] = m
mm[2, 3] = 0
mm[2, 4] = 0
mm[2, 5] = 0
mm[2, 6] = 0
# mm[2,7] = y + zt*e32
# mm[3,0] = 0
mm[3, 1] = 0
mm[3, 2] = 0
mm[3, 3] = m
mm[3, 4] = 0
mm[3, 5] = 0
mm[3, 6] = 0
# mm[3,7] = z + zt*e33
# mm[4,0] = 0
mm[4, 1] = 0
mm[4, 2] = 0
mm[4, 3] = 0
mm[4, 4] = j1
mm[4, 5] = 0
mm[4, 6] = j31
# mm[4,7] = -zt*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33)
# mm[5,0] = 0
mm[5, 1] = 0
mm[5, 2] = 0
mm[5, 3] = 0
mm[5, 4] = 0
mm[5, 5] = j2
mm[5, 6] = 0
# mm[5,7] = zt*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33)
# mm[6,0] = 0
mm[6, 1] = 0
mm[6, 2] = 0
mm[6, 3] = 0
mm[6, 4] = j31
mm[6, 5] = 0
mm[6, 6] = j3
# mm[6,7] = 0
# mm[7,0] = -zt*(e11*e23*x - e13*e21*x + e12*e23*y - e13*e22*y + zt*e11*e23*e31 - zt*e13*e21*e31 + zt*e12*e23*e32 - zt*e13*e22*e32)
# mm[7,1] = x + zt*e31
# mm[7,2] = y + zt*e32
# mm[7,3] = z + zt*e33
# mm[7,4] = -zt*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33)
# mm[7,5] = zt*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33)
# mm[7,6] = 0
## mm[7,7] = 0
mm = mm[1:, 1:]
# rhs :: Vector (Expr Double)
zt2 = zt * zt
rhs = C.veccat(
[ #tc - cfric*ddelta - f1*y + f2*(rA + x) + dy*m*(dx - 2*ddelta*y) - dx*m*(dy + 2*ddelta*rA + 2*ddelta*x)
f1 + ddelta * m * (dy + ddelta * rA + ddelta * x) +
ddelta * dy * m,
f2 - ddelta * m * (dx - ddelta * y) - ddelta * dx * m, f3 - g * m,
t1 - w2 * (j3 * w3 + j31 * w1) + j2 * w2 * w3,
t2 + w1 * (j3 * w3 + j31 * w1) - w3 * (j1 * w1 + j31 * w3),
t3 + w2 * (j1 * w1 + j31 * w3) - j2 * w1 * w2
# , dr*dr + ddr*r - (zt*w1*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) + zt*w2*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33))*(w3 - ddelta*e33) - dx*(dx - zt*e21*(w1 - ddelta*e13) + zt*e11*(w2 - ddelta*e23)) - dy*(dy - zt*e22*(w1 - ddelta*e13) + zt*e12*(w2 - ddelta*e23)) - dz*(dz - zt*e23*(w1 - ddelta*e13) + zt*e13*(w2 - ddelta*e23)) + (w1 - ddelta*e13)*(e21*(zt*dx - zt2*e21*(w1 - ddelta*e13) + zt2*e11*(w2 - ddelta*e23)) + e22*(zt*dy - zt2*e22*(w1 - ddelta*e13) + zt2*e12*(w2 - ddelta*e23)) + zt*e23*(dz + zt*e13*w2 - zt*e23*w1) + zt*e33*(w1*z + zt*e33*w1 + ddelta*e11*x + ddelta*e12*y + zt*ddelta*e11*e31 + zt*ddelta*e12*e32) + zt*e31*(x + zt*e31)*(w1 - ddelta*e13) + zt*e32*(y + zt*e32)*(w1 - ddelta*e13)) - (w2 - ddelta*e23)*(e11*(zt*dx - zt2*e21*(w1 - ddelta*e13) + zt2*e11*(w2 - ddelta*e23)) + e12*(zt*dy - zt2*e22*(w1 - ddelta*e13) + zt2*e12*(w2 - ddelta*e23)) + zt*e13*(dz + zt*e13*w2 - zt*e23*w1) - zt*e33*(w2*z + zt*e33*w2 + ddelta*e21*x + ddelta*e22*y + zt*ddelta*e21*e31 + zt*ddelta*e22*e32) - zt*e31*(x + zt*e31)*(w2 - ddelta*e23) - zt*e32*(y + zt*e32)*(w2 - ddelta*e23))
])
dRexp = C.SXMatrix(3, 3)
dRexp[0, 0] = e21 * (w3 - ddelta * e33) - e31 * (w2 - ddelta * e23)
dRexp[0, 1] = e31 * (w1 - ddelta * e13) - e11 * (w3 - ddelta * e33)
dRexp[0, 2] = e11 * (w2 - ddelta * e23) - e21 * (w1 - ddelta * e13)
dRexp[1, 0] = e22 * (w3 - ddelta * e33) - e32 * (w2 - ddelta * e23)
dRexp[1, 1] = e32 * (w1 - ddelta * e13) - e12 * (w3 - ddelta * e33)
dRexp[1, 2] = e12 * (w2 - ddelta * e23) - e22 * (w1 - ddelta * e13)
dRexp[2, 0] = e23 * w3 - e33 * w2
dRexp[2, 1] = e33 * w1 - e13 * w3
dRexp[2, 2] = e13 * w2 - e23 * w1
# c =(x + zt*e31)**2/2 + (y + zt*e32)**2/2 + (z + zt*e33)**2/2 - r**2/2
# cdot =dx*(x + zt*e31) + dy*(y + zt*e32) + dz*(z + zt*e33) + zt*(w2 - ddelta*e23)*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) - zt*(w1 - ddelta*e13)*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33)
# cddot = (zt*w1*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) + zt*w2*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33))*(w3 - ddelta*e33) + dx*(dx + zt*e11*w2 - zt*e21*w1 - zt*ddelta*e11*e23 + zt*ddelta*e13*e21) + dy*(dy + zt*e12*w2 - zt*e22*w1 - zt*ddelta*e12*e23 + zt*ddelta*e13*e22) + dz*(dz + zt*e13*w2 - zt*e23*w1) + ddx*(x + zt*e31) + ddy*(y + zt*e32) + ddz*(z + zt*e33) - (w1 - ddelta*e13)*(e21*(zt*dx - zt**2*e21*(w1 - ddelta*e13) + zt**2*e11*(w2 - ddelta*e23)) + e22*(zt*dy - zt**2*e22*(w1 - ddelta*e13) + zt**2*e12*(w2 - ddelta*e23)) + zt*e33*(z*w1 + ddelta*e11*x + ddelta*e12*y + zt*e33*w1 + zt*ddelta*e11*e31 + zt*ddelta*e12*e32) + zt*e23*(dz + zt*e13*w2 - zt*e23*w1) + zt*e31*(w1 - ddelta*e13)*(x + zt*e31) + zt*e32*(w1 - ddelta*e13)*(y + zt*e32)) + (w2 - ddelta*e23)*(e11*(zt*dx - zt**2*e21*(w1 - ddelta*e13) + zt**2*e11*(w2 - ddelta*e23)) + e12*(zt*dy - zt**2*e22*(w1 - ddelta*e13) + zt**2*e12*(w2 - ddelta*e23)) - zt*e33*(z*w2 + ddelta*e21*x + ddelta*e22*y + zt*e33*w2 + zt*ddelta*e21*e31 + zt*ddelta*e22*e32) + zt*e13*(dz + zt*e13*w2 - zt*e23*w1) - zt*e31*(w2 - ddelta*e23)*(x + zt*e31) - zt*e32*(w2 - ddelta*e23)*(y + zt*e32)) + zt*(dw2 - dddelta*e23)*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) - zt*(dw1 - dddelta*e13)*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33) - zt*dddelta*(e11*e23*x - e13*e21*x + e12*e23*y - e13*e22*y + zt*e11*e23*e31 - zt*e13*e21*e31 + zt*e12*e23*e32 - zt*e13*e22*e32)
# where
# dw1 = dw @> 0
# dw2 = dw @> 1
# {-
# dw3 = dw @> 2
# -}
# ddx = ddX @> 0
# ddy = ddX @> 1
# ddz = ddX @> 2
# dddelta = dddelta' @> 0
return (mm, rhs, dRexp)
def setupModel(dae, conf):
'''
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
'''
m = conf['mass']
g = conf['g']
j1 = conf['j1']
j31 = conf['j31']
j2 = conf['j2']
j3 = conf['j3']
zt = conf['zt']
e11 = dae['e11']
e12 = dae['e12']
e13 = dae['e13']
e21 = dae['e21']
e22 = dae['e22']
e23 = dae['e23']
e31 = dae['e31']
e32 = dae['e32']
e33 = dae['e33']
x = dae['x']
y = dae['y']
z = dae['z']
dx = dae['dx']
dy = dae['dy']
dz = dae['dz']
r = dae['r']
dr = dae['dr']
ddr = dae['ddr']
# wind model
def getWind():
if conf['wind_model']['name'] == 'wind_shear':
# use a logarithmic wind shear model
# wind(z) = w0 * log((z+zt)/zt) / log(z0/zt)
# where w0 is wind at z0 altitude
# zt is surface roughness characteristic length
z0 = conf['wind_model']['z0']
zt_roughness = conf['wind_model']['zt_roughness']
return dae['w0'] * C.log(
(-z + zt_roughness) / zt_roughness) / C.log(z0 / zt_roughness)
elif conf['wind_model']['name'] == 'constant':
# constant wind
return dae['w0']
dae['wind_at_altitude'] = getWind()
# wind velocity in NED
dae['v_wn_n'] = C.veccat([dae['wind_at_altitude'], 0, 0])
# body velocity in NED
dae['v_bn_n'] = C.veccat([dx, dy, dz])
# body velocity w.r.t. wind in NED
v_bw_n = dae['v_bn_n'] - dae['v_wn_n']
# body velocity w.r.t. wind in body frame
v_bw_b = C.mul(dae['R_n2b'], v_bw_n)
# compute aerodynamic forces and moments
(f1, f2, f3, t1, t2,
t3) = aeroForcesTorques(dae, conf, v_bw_n, v_bw_b, dae['w_bn_b'],
(dae['e21'], dae['e22'], dae['e23']))
# if we are running a homotopy, add psudeo forces and moments as algebraic states
if 'runHomotopy' in conf and conf['runHomotopy']:
gamma_homotopy = dae.addP('gamma_homotopy')
f1 = f1 * gamma_homotopy + dae.addZ('f1_homotopy') * (1 -
gamma_homotopy)
f2 = f2 * gamma_homotopy + dae.addZ('f2_homotopy') * (1 -
gamma_homotopy)
f3 = f3 * gamma_homotopy + dae.addZ('f3_homotopy') * (1 -
gamma_homotopy)
t1 = t1 * gamma_homotopy + dae.addZ('t1_homotopy') * (1 -
gamma_homotopy)
t2 = t2 * gamma_homotopy + dae.addZ('t2_homotopy') * (1 -
gamma_homotopy)
t3 = t3 * gamma_homotopy + dae.addZ('t3_homotopy') * (1 -
gamma_homotopy)
# mass matrix
mm = C.SXMatrix(7, 7)
mm[0, 0] = m
mm[0, 1] = 0
mm[0, 2] = 0
mm[0, 3] = 0
mm[0, 4] = 0
mm[0, 5] = 0
mm[0, 6] = x + zt * e31
mm[1, 0] = 0
mm[1, 1] = m
mm[1, 2] = 0
mm[1, 3] = 0
mm[1, 4] = 0
mm[1, 5] = 0
mm[1, 6] = y + zt * e32
mm[2, 0] = 0
mm[2, 1] = 0
mm[2, 2] = m
mm[2, 3] = 0
mm[2, 4] = 0
mm[2, 5] = 0
mm[2, 6] = z + zt * e33
mm[3, 0] = 0
mm[3, 1] = 0
mm[3, 2] = 0
mm[3, 3] = j1
mm[3, 4] = 0
mm[3, 5] = j31
mm[3, 6] = -zt * (e21 * x + e22 * y + e23 * z + zt * e21 * e31 +
zt * e22 * e32 + zt * e23 * e33)
mm[4, 0] = 0
mm[4, 1] = 0
mm[4, 2] = 0
mm[4, 3] = 0
mm[4, 4] = j2
mm[4, 5] = 0
mm[4, 6] = zt * (e11 * x + e12 * y + e13 * z + zt * e11 * e31 +
zt * e12 * e32 + zt * e13 * e33)
mm[5, 0] = 0
mm[5, 1] = 0
mm[5, 2] = 0
mm[5, 3] = j31
mm[5, 4] = 0
mm[5, 5] = j3
mm[5, 6] = 0
mm[6, 0] = x + zt * e31
mm[6, 1] = y + zt * e32
mm[6, 2] = z + zt * e33
mm[6, 3] = -zt * (e21 * x + e22 * y + e23 * z + zt * e21 * e31 +
zt * e22 * e32 + zt * e23 * e33)
mm[6, 4] = zt * (e11 * x + e12 * y + e13 * z + zt * e11 * e31 +
zt * e12 * e32 + zt * e13 * e33)
mm[6, 5] = 0
mm[6, 6] = 0
# right hand side
w1 = dae['w_bn_b_x']
w2 = dae['w_bn_b_y']
w3 = dae['w_bn_b_z']
zt2 = zt * zt
ddelta = 0
rA = 0
rhs = C.veccat([
f1 + ddelta * m * (dy + ddelta * rA + ddelta * x) + ddelta * dy * m,
f2 - ddelta * m * (dx - ddelta * y) - ddelta * dx * m, f3 + g * m,
t1 - w2 * (j3 * w3 + j31 * w1) + j2 * w2 * w3,
t2 + w1 * (j3 * w3 + j31 * w1) - w3 * (j1 * w1 + j31 * w3),
t3 + w2 * (j1 * w1 + j31 * w3) - j2 * w1 * w2,
ddr * r - (zt * w1 * (e11 * x + e12 * y + e13 * z + zt * e11 * e31 +
zt * e12 * e32 + zt * e13 * e33) + zt * w2 *
(e21 * x + e22 * y + e23 * z + zt * e21 * e31 +
zt * e22 * e32 + zt * e23 * e33)) * (w3 - ddelta * e33) -
dx * (dx - zt * e21 * (w1 - ddelta * e13) + zt * e11 *
(w2 - ddelta * e23)) - dy * (dy - zt * e22 *
(w1 - ddelta * e13) + zt * e12 *
(w2 - ddelta * e23)) - dz *
(dz - zt * e23 * (w1 - ddelta * e13) + zt * e13 *
(w2 - ddelta * e23)) + dr * dr + (w1 - ddelta * e13) *
(e21 * (zt * dx - zt2 * e21 * (w1 - ddelta * e13) + zt2 * e11 *
(w2 - ddelta * e23)) + e22 *
(zt * dy - zt2 * e22 * (w1 - ddelta * e13) + zt2 * e12 *
(w2 - ddelta * e23)) + zt * e23 *
(dz + zt * e13 * w2 - zt * e23 * w1) + zt * e33 *
(w1 * z + zt * e33 * w1 + ddelta * e11 * x + ddelta * e12 * y +
zt * ddelta * e11 * e31 + zt * ddelta * e12 * e32) + zt * e31 *
(x + zt * e31) * (w1 - ddelta * e13) + zt * e32 * (y + zt * e32) *
(w1 - ddelta * e13)) - (w2 - ddelta * e23) *
(e11 * (zt * dx - zt2 * e21 * (w1 - ddelta * e13) + zt2 * e11 *
(w2 - ddelta * e23)) + e12 *
(zt * dy - zt2 * e22 * (w1 - ddelta * e13) + zt2 * e12 *
(w2 - ddelta * e23)) + zt * e13 *
(dz + zt * e13 * w2 - zt * e23 * w1) - zt * e33 *
(w2 * z + zt * e33 * w2 + ddelta * e21 * x + ddelta * e22 * y +
zt * ddelta * e21 * e31 + zt * ddelta * e22 * e32) - zt * e31 *
(x + zt * e31) * (w2 - ddelta * e23) - zt * e32 * (y + zt * e32) *
(w2 - ddelta * e23))
])
dRexp = C.SXMatrix(3, 3)
dRexp[0, 0] = e21 * (w3 - ddelta * e33) - e31 * (w2 - ddelta * e23)
dRexp[0, 1] = e31 * (w1 - ddelta * e13) - e11 * (w3 - ddelta * e33)
dRexp[0, 2] = e11 * (w2 - ddelta * e23) - e21 * (w1 - ddelta * e13)
dRexp[1, 0] = e22 * (w3 - ddelta * e33) - e32 * (w2 - ddelta * e23)
dRexp[1, 1] = e32 * (w1 - ddelta * e13) - e12 * (w3 - ddelta * e33)
dRexp[1, 2] = e12 * (w2 - ddelta * e23) - e22 * (w1 - ddelta * e13)
dRexp[2, 0] = e23 * w3 - e33 * w2
dRexp[2, 1] = e33 * w1 - e13 * w3
dRexp[2, 2] = e13 * w2 - e23 * w1
c = (x + zt * e31)**2 / 2 + (y + zt * e32)**2 / 2 + (
z + zt * e33)**2 / 2 - r**2 / 2
cdot = dx * (x + zt * e31) + dy * (y + zt * e32) + dz * (
z + zt * e33) + zt * (w2 - ddelta * e23) * (
e11 * x + e12 * y + e13 * z + zt * e11 * e31 + zt * e12 * e32 +
zt * e13 * e33) - zt * (w1 - ddelta * e13) * (
e21 * x + e22 * y + e23 * z + zt * e21 * e31 + zt * e22 * e32 +
zt * e23 * e33) - r * dr
ddx = dae.ddt('dx')
ddy = dae.ddt('dy')
ddz = dae.ddt('dz')
dw1 = dae.ddt('w_bn_b_x')
dw2 = dae.ddt('w_bn_b_y')
# ddx = dae['ddx']
# ddy = dae['ddy']
# ddz = dae['ddz']
# dw1 = dae['dw1']
# dw2 = dae['dw2']
dddelta = 0
cddot = -(w1 - ddelta * e13) * (
zt * e23 * (dz + zt * e13 * w2 - zt * e23 * w1) + zt * e33 *
(w1 * z + zt * e33 * w1 + ddelta * e11 * x + ddelta * e12 * y +
zt * ddelta * e11 * e31 + zt * ddelta * e12 * e32) + zt * e21 *
(dx + zt * e11 * w2 - zt * e21 * w1 - zt * ddelta * e11 * e23 +
zt * ddelta * e13 * e21) + zt * e22 *
(dy + zt * e12 * w2 - zt * e22 * w1 - zt * ddelta * e12 * e23 +
zt * ddelta * e13 * e22) + zt * e31 * (x + zt * e31) *
(w1 - ddelta * e13) + zt * e32 * (y + zt * e32) * (w1 - ddelta * e13)
) + (w2 - ddelta * e23) * (
zt * e13 * (dz + zt * e13 * w2 - zt * e23 * w1) - zt * e33 *
(w2 * z + zt * e33 * w2 + ddelta * e21 * x + ddelta * e22 * y +
zt * ddelta * e21 * e31 + zt * ddelta * e22 * e32) + zt * e11 *
(dx + zt * e11 * w2 - zt * e21 * w1 - zt * ddelta * e11 * e23 +
zt * ddelta * e13 * e21) + zt * e12 *
(dy + zt * e12 * w2 - zt * e22 * w1 - zt * ddelta * e12 * e23 +
zt * ddelta * e13 * e22) - zt * e31 * (x + zt * e31) *
(w2 - ddelta * e23) - zt * e32 * (y + zt * e32) *
(w2 - ddelta * e23)) - ddr * r + (
zt * w1 *
(e11 * x + e12 * y + e13 * z + zt * e11 * e31 + zt * e12 * e32 +
zt * e13 * e33) + zt * w2 *
(e21 * x + e22 * y + e23 * z + zt * e21 * e31 + zt * e22 * e32 +
zt * e23 * e33)) * (w3 - ddelta * e33) + dx * (
dx + zt * e11 * w2 - zt * e21 * w1 - zt * ddelta * e11 * e23 +
zt * ddelta * e13 * e21) + dy * (
dy + zt * e12 * w2 - zt * e22 * w1 -
zt * ddelta * e12 * e23 + zt * ddelta * e13 * e22
) + dz * (dz + zt * e13 * w2 - zt * e23 * w1) + ddx * (
x + zt * e31) + ddy * (y + zt * e32) + ddz * (
z +
zt * e33) - dr * dr + zt * (dw2 - dddelta * e23) * (
e11 * x + e12 * y + e13 * z + zt * e11 * e31 +
zt * e12 * e32 +
zt * e13 * e33) - zt * (dw1 - dddelta * e13) * (
e21 * x + e22 * y + e23 * z + zt * e21 * e31 +
zt * e22 * e32 +
zt * e23 * e33) - zt * dddelta * (
e11 * e23 * x - e13 * e21 * x +
e12 * e23 * y - e13 * e22 * y + zt * e11 *
e23 * e31 - zt * e13 * e21 * e31 + zt *
e12 * e23 * e32 - zt * e13 * e22 * e32)
# cddot = (zt*w1*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) + zt*w2*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33))*(w3 - ddelta*e33) + dx*(dx + zt*e11*w2 - zt*e21*w1 - zt*ddelta*e11*e23 + zt*ddelta*e13*e21) + dy*(dy + zt*e12*w2 - zt*e22*w1 - zt*ddelta*e12*e23 + zt*ddelta*e13*e22) + dz*(dz + zt*e13*w2 - zt*e23*w1) + ddx*(x + zt*e31) + ddy*(y + zt*e32) + ddz*(z + zt*e33) - (w1 - ddelta*e13)*(e21*(zt*dx - zt**2*e21*(w1 - ddelta*e13) + zt**2*e11*(w2 - ddelta*e23)) + e22*(zt*dy - zt**2*e22*(w1 - ddelta*e13) + zt**2*e12*(w2 - ddelta*e23)) + zt*e33*(z*w1 + ddelta*e11*x + ddelta*e12*y + zt*e33*w1 + zt*ddelta*e11*e31 + zt*ddelta*e12*e32) + zt*e23*(dz + zt*e13*w2 - zt*e23*w1) + zt*e31*(w1 - ddelta*e13)*(x + zt*e31) + zt*e32*(w1 - ddelta*e13)*(y + zt*e32)) + (w2 - ddelta*e23)*(e11*(zt*dx - zt**2*e21*(w1 - ddelta*e13) + zt**2*e11*(w2 - ddelta*e23)) + e12*(zt*dy - zt**2*e22*(w1 - ddelta*e13) + zt**2*e12*(w2 - ddelta*e23)) - zt*e33*(z*w2 + ddelta*e21*x + ddelta*e22*y + zt*e33*w2 + zt*ddelta*e21*e31 + zt*ddelta*e22*e32) + zt*e13*(dz + zt*e13*w2 - zt*e23*w1) - zt*e31*(w2 - ddelta*e23)*(x + zt*e31) - zt*e32*(w2 - ddelta*e23)*(y + zt*e32)) + zt*(dw2 - dddelta*e23)*(e11*x + e12*y + e13*z + zt*e11*e31 + zt*e12*e32 + zt*e13*e33) - zt*(dw1 - dddelta*e13)*(e21*x + e22*y + e23*z + zt*e21*e31 + zt*e22*e32 + zt*e23*e33) - zt*dddelta*(e11*e23*x - e13*e21*x + e12*e23*y - e13*e22*y + zt*e11*e23*e31 - zt*e13*e21*e31 + zt*e12*e23*e32 - zt*e13*e22*e32)
# where
# dw1 = dw @> 0
# dw2 = dw @> 1
# {-
# dw3 = dw @> 2
# -}
# ddx = ddX @> 0
# ddy = ddX @> 1
# ddz = ddX @> 2
# dddelta = dddelta' @> 0
dae['c'] = c
dae['cdot'] = cdot
dae['cddot'] = cddot
return (mm, rhs, dRexp)
def invskew(S): return c.SXMatrix([S[2,1],S[0,2],S[1,0]])
import numpy
from numpy import cos,sin, vstack, hstack, multiply
casadiAvailable = False
casadiTypes = set()
try:
import casadi as c
casadiAvailable = True
casadiTypes = set([type(c.SX()),type(c.SXMatrix())])
except ImportError:
pass
def TRx(a):
constr = numpy.matrix
if casadiAvailable and type(a) in casadiTypes:
constr = c.SXMatrix
return constr([[1,0,0,0],[0,cos(a),-sin(a),0],[0,sin(a),cos(a),0],[0,0,0,1]])
def TRy(a):
constr = numpy.matrix
if casadiAvailable and type(a) in casadiTypes:
constr = c.SXMatrix
return constr([[cos(a),0,sin(a),0],[0,1,0,0],[-sin(a),0,cos(a),0],[0,0,0,1]])
def TRz(a):
constr = numpy.matrix
if casadiAvailable and type(a) in casadiTypes:
constr = c.SXMatrix
return constr([[cos(a),-sin(a),0,0],[sin(a),cos(a),0,0],[0,0,1,0],[0,0,0,1]])
def tr(x,y,z):