def log(cls, G):
theta = ca.arccos(((G[0, 0] + G[1, 1] + G[2, 2]) - 1) / 2)
wSkew = so3.wedge(so3.Dcm.log(G[:3, :3]))
V_inv = ca.SX.eye(3) - 0.5 * wSkew + (1 / (theta**2)) * (1 - (
(cls.C1(theta)) / (2 * cls.C2(theta)))) * ca.mtimes(wSkew, wSkew)
# t is translational component vector
t = ca.SX(3, 1)
t[0, 0] = G[0, 3]
t[1, 0] = G[1, 3]
t[2, 0] = G[2, 3]
uInv = ca.mtimes(V_inv, t)
horz2 = ca.horzcat(wSkew, uInv)
lastRow2 = ca.horzcat(0, 0, 0, 0)
return ca.vertcat(horz2, lastRow2)
def carouselModel(conf):
'''
pass this a conf, and it'll return you a dae
'''
# empty Dae
dae = Dae()
# add some differential states/algebraic vars/controls/params
dae.addZ("nu")
dae.addX( [ "x"
, "y"
, "z"
, "e11"
, "e12"
, "e13"
, "e21"
, "e22"
, "e23"
, "e31"
, "e32"
, "e33"
, "dx"
, "dy"
, "dz"
, "w_bn_b_x"
, "w_bn_b_y"
, "w_bn_b_z"
, "ddelta"
, "r"
, "dr"
, "aileron"
, "elevator"
, "motor_torque"
, "ddr"
] )
if 'cn_rudder' in conf:
dae.addX('rudder')
dae.addU('drudder')
if 'cL_flaps' in conf:
dae.addX('flaps')
dae.addU('dflaps')
if conf['delta_parameterization'] == 'linear':
dae.addX('delta')
dae['cos_delta'] = C.cos(dae['delta'])
dae['sin_delta'] = C.sin(dae['delta'])
dae_delta_residual = dae.ddt('delta') - dae['ddelta']
elif conf['delta_parameterization'] == 'cos_sin':
dae.addX("cos_delta")
dae.addX("sin_delta")
norm = dae['cos_delta']**2 + dae['sin_delta']**2
if 'stabilize_invariants' in conf and conf['stabilize_invariants'] == True:
pole_delta = 0.5
else:
pole_delta = 0.0
cos_delta_dot_st = -pole_delta/2.* ( dae['cos_delta'] - dae['cos_delta'] / norm )
sin_delta_dot_st = -pole_delta/2.* ( dae['sin_delta'] - dae['sin_delta'] / norm )
dae_delta_residual = C.veccat([dae.ddt('cos_delta') - (-dae['sin_delta']*dae['ddelta'] + cos_delta_dot_st),
dae.ddt('sin_delta') - ( dae['cos_delta']*dae['ddelta'] + sin_delta_dot_st) ])
else:
raise ValueError('unrecognized delta_parameterization "'+conf['delta_parameterization']+'"')
dae.addU( [ "daileron"
, "delevator"
, "dmotor_torque"
, 'dddr'
] )
# add wind parameter if wind shear is in configuration
if 'wind_model' in conf:
if conf['wind_model']['name'] == 'hardcoded':
dae['w0'] = conf['wind_model']['hardcoded_value']
elif conf['wind_model']['name'] != 'random_walk':
dae.addP( ['w0'] )
# set some state derivatives as outputs
dae['ddx'] = dae.ddt('dx')
dae['ddy'] = dae.ddt('dy')
dae['ddz'] = dae.ddt('dz')
dae['ddt_w_bn_b_x'] = dae.ddt('w_bn_b_x')
dae['ddt_w_bn_b_y'] = dae.ddt('w_bn_b_y')
dae['ddt_w_bn_b_z'] = dae.ddt('w_bn_b_z')
dae['w_bn_b'] = C.veccat([dae['w_bn_b_x'], dae['w_bn_b_y'], dae['w_bn_b_z']])
# some outputs in for plotting
dae['carousel_RPM'] = dae['ddelta']*60/(2*C.pi)
dae['aileron_deg'] = dae['aileron']*180/C.pi
dae['elevator_deg'] = dae['elevator']*180/C.pi
dae['daileron_deg_s'] = dae['daileron']*180/C.pi
dae['delevator_deg_s'] = dae['delevator']*180/C.pi
# tether tension == radius*nu where nu is alg. var associated with x^2+y^2+z^2-r^2==0
dae['tether_tension'] = dae['r']*dae['nu']
# theoretical mechanical power
dae['mechanical_winch_power'] = -dae['tether_tension']*dae['dr']
# carousel2 motor model from thesis data
dae['rpm'] = -dae['dr']/0.1*60/(2*C.pi)
dae['torque'] = dae['tether_tension']*0.1
dae['electrical_winch_power'] = 293.5816373499238 + \
0.0003931623408*dae['rpm']*dae['rpm'] + \
0.0665919381751*dae['torque']*dae['torque'] + \
0.1078628659825*dae['rpm']*dae['torque']
dae['R_c2b'] = C.blockcat([[dae['e11'],dae['e12'],dae['e13']],
[dae['e21'],dae['e22'],dae['e23']],
[dae['e31'],dae['e32'],dae['e33']]])
dae["yaw"] = C.arctan2(dae["e12"], dae["e11"]) * 180.0 / C.pi
dae["pitch"] = C.arcsin( -dae["e13"] ) * 180.0 / C.pi
dae["roll"] = C.arctan2(dae["e23"], dae["e33"]) * 180.0 / C.pi
# line angle
dae['cos_line_angle'] = \
-(dae['e31']*dae['x'] + dae['e32']*dae['y'] + dae['e33']*dae['z']) / C.sqrt(dae['x']**2 + dae['y']**2 + dae['z']**2)
dae['line_angle_deg'] = C.arccos(dae['cos_line_angle'])*180.0/C.pi
(massMatrix, rhs, dRexp) = setupModel(dae, conf)
if 'stabilize_invariants' in conf and conf['stabilize_invariants'] == True:
RotPole = 0.5
else:
RotPole = 0.0
Rst = RotPole*C.mul( dae['R_c2b'], (C.inv(C.mul(dae['R_c2b'].T,dae['R_c2b'])) - numpy.eye(3)) )
ode = C.veccat([
C.veccat([dae.ddt(name) for name in ['x','y','z']]) - C.veccat([dae['dx'],dae['dy'],dae['dz']]),
C.veccat([dae.ddt(name) for name in ["e11","e12","e13",
"e21","e22","e23",
"e31","e32","e33"]]) - ( dRexp.T.reshape([9,1]) + Rst.reshape([9,1]) ),
dae_delta_residual,
# C.veccat([dae.ddt(name) for name in ['dx','dy','dz']]) - C.veccat([dae['ddx'],dae['ddy'],dae['ddz']]),
# C.veccat([dae.ddt(name) for name in ['w1','w2','w3']]) - C.veccat([dae['dw1'],dae['dw2'],dae['dw3']]),
# dae.ddt('ddelta') - dae['dddelta'],
dae.ddt('r') - dae['dr'],
dae.ddt('dr') - dae['ddr'],
dae.ddt('aileron') - dae['daileron'],
dae.ddt('elevator') - dae['delevator'],
dae.ddt('motor_torque') - dae['dmotor_torque'],
dae.ddt('ddr') - dae['dddr']
])
if 'rudder' in dae:
ode = C.veccat([ode, dae.ddt('rudder') - dae['drudder']])
if 'flaps' in dae:
ode = C.veccat([ode, dae.ddt('flaps') - dae['dflaps']])
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 ]:
ode = C.veccat([ode,
dae.ddt('t1_disturbance') - dae['dt1_disturbance']])
if _which[ 1 ]:
ode = C.veccat([ode,
dae.ddt('t2_disturbance') - dae['dt2_disturbance']])
if _which[ 2 ]:
ode = C.veccat([ode,
dae.ddt('t3_disturbance') - dae['dt3_disturbance']])
elif _type == 'parameter':
if _which[ 0 ]:
ode = C.veccat([ode, dae.ddt('t1_disturbance')])
if _which[ 1 ]:
ode = C.veccat([ode, dae.ddt('t2_disturbance')])
if _which[ 2 ]:
ode = C.veccat([ode, dae.ddt('t3_disturbance')])
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 is 'random_walk':
if _which[ 0 ]:
ode = C.veccat([ode,
dae.ddt('f1_disturbance') - dae['df1_disturbance']])
if _which[ 1 ]:
ode = C.veccat([ode,
dae.ddt('f2_disturbance') - dae['df2_disturbance']])
if _which[ 2 ]:
ode = C.veccat([ode,
dae.ddt('f3_disturbance') - dae['df3_disturbance']])
elif _type == 'parameter':
if _which[ 0 ]:
ode = C.veccat([ode, dae.ddt('f1_disturbance')])
if _which[ 1 ]:
ode = C.veccat([ode, dae.ddt('f2_disturbance')])
if _which[ 2 ]:
ode = C.veccat([ode, dae.ddt('f3_disturbance')])
if 'wind_model' in conf and conf['wind_model']['name'] == 'random_walk':
tau_wind = conf['wind_model']['tau_wind']
ode = C.veccat([ode,
dae.ddt('wind_x') - (-dae['wind_x'] / tau_wind + dae['delta_wind_x'])
, dae.ddt('wind_y') - (-dae['wind_y'] / tau_wind + dae['delta_wind_y'])
, dae.ddt('wind_z') - (-dae['wind_z'] / tau_wind + dae['delta_wind_z'])
])
if 'stabilize_invariants' in conf and conf['stabilize_invariants'] == True:
cPole = 0.5
else:
cPole = 0.0
rhs[-1] -= 2*cPole*dae['cdot'] + cPole*cPole*dae['c']
psuedoZVec = C.veccat([dae.ddt(name) for name in ['ddelta','dx','dy','dz','w_bn_b_x','w_bn_b_y','w_bn_b_z']]+[dae['nu']])
alg = C.mul(massMatrix, psuedoZVec) - rhs
dae.setResidual([ode,alg])
return dae
def plot_heuristics(model, x_all, u_all, n_last=2):
n_interm_points = 301
n = len(x_all[:])
t_all = np.linspace(0, (n - 1) * model.dt, n)
t_all_dense = np.linspace(t_all[0], t_all[-1], n_interm_points)
fig, ax = plt.subplots(2, 2, figsize=(10, 10))
# ---------------- Optic acceleration cancellation ----------------- #
oac = []
for k in range(n):
x_b = x_all[k, ca.veccat, ['x_b', 'y_b']]
x_c = x_all[k, ca.veccat, ['x_c', 'y_c']]
r_bc_xy = ca.norm_2(x_b - x_c)
z_b = x_all[k, 'z_b']
tan_phi = ca.arctan2(z_b, r_bc_xy)
oac.append(tan_phi)
# Fit a line for OAC
fit_oac = np.polyfit(t_all[:-n_last], oac[:-n_last], 1)
fit_oac_fn = np.poly1d(fit_oac)
# Plot OAC
ax[0, 0].plot(t_all[:-n_last], oac[:-n_last],
label='Simulation', lw=3)
ax[0, 0].plot(t_all, fit_oac_fn(t_all), '--k', label='Linear fit')
# ------------------- Constant bearing angle ----------------------- #
cba = []
d = ca.veccat([ca.cos(model.m0['phi']),
ca.sin(model.m0['phi'])])
for k in range(n):
x_b = x_all[k, ca.veccat, ['x_b', 'y_b']]
x_c = x_all[k, ca.veccat, ['x_c', 'y_c']]
r_cb = x_b - x_c
cos_gamma = ca.mul(d.T, r_cb) / ca.norm_2(r_cb)
cba.append(np.rad2deg(np.float(ca.arccos(cos_gamma))))
# Fit a const for CBA
fit_cba = np.polyfit(t_all[:-n_last], cba[:-n_last], 0)
fit_cba_fn = np.poly1d(fit_cba)
# Smoothen the trajectory
t_part_dense = np.linspace(t_all[0], t_all[-n_last-1], 301)
cba_smooth = spline(t_all[:-n_last], cba[:-n_last], t_part_dense)
ax[1, 0].plot(t_part_dense, cba_smooth, lw=3, label='Simulation')
# Plot CBA
# ax[1, 0].plot(t_all[:-n_last], cba[:-n_last],
# label='$\gamma \\approx const$')
ax[1, 0].plot(t_all, fit_cba_fn(t_all), '--k', label='Constant fit')
# ---------- Generalized optic acceleration cancellation ----------- #
goac_smooth = spline(t_all,
model.m0['phi'] - x_all[:, 'phi'],
t_all_dense)
n_many_last = n_last *\
n_interm_points / (t_all[-1] - t_all[0]) * model.dt
# Delta
ax[0, 1].plot(t_all_dense[:-n_many_last],
np.rad2deg(goac_smooth[:-n_many_last]), lw=3,
label=r'Rotation angle $\delta$')
# Gamma
ax[0, 1].plot(t_all[:-n_last], cba[:-n_last], '--', lw=2,
label=r'Bearing angle $\gamma$')
# ax[0, 1].plot([t_all[0], t_all[-1]], [30, 30], 'k--',
# label='experimental bound')
# ax[0, 1].plot([t_all[0], t_all[-1]], [-30, -30], 'k--')
# ax[0, 1].yaxis.set_ticks(range(-60, 70, 30))
# Derivative of delta
# ax0_twin = ax[0, 1].twinx()
# ax0_twin.step(t_all,
# np.rad2deg(np.array(ca.veccat([0, u_all[:, 'w_phi']]))),
# 'g-', label='derivative $\mathrm{d}\delta/\mathrm{d}t$')
# ax0_twin.set_ylim(-90, 90)
# ax0_twin.yaxis.set_ticks(range(-90, 100, 30))
# ax0_twin.set_ylabel('$\mathrm{d}\delta/\mathrm{d}t$, deg/s')
# ax0_twin.yaxis.label.set_color('g')
# ax0_twin.legend(loc='lower right')
# -------------------- Linear optic trajectory --------------------- #
lot_beta = []
x_b = model.m0[ca.veccat, ['x_b', 'y_b']]
for k in range(n):
x_c = x_all[k, ca.veccat, ['x_c', 'y_c']]
d = ca.veccat([ca.cos(x_all[k, 'phi']),
ca.sin(x_all[k, 'phi'])])
r = x_b - x_c
cos_beta = ca.mul(d.T, r) / ca.norm_2(r)
beta = ca.arccos(cos_beta)
tan_beta = ca.tan(beta)
lot_beta.append(tan_beta)
# lot_beta.append(np.rad2deg(np.float(ca.arccos(cos_beta))))
# lot_alpha = np.rad2deg(np.array(x_all[:, 'psi']))
lot_alpha = ca.tan(x_all[:, 'psi'])
# Fit a line for LOT
fit_lot = np.polyfit(lot_alpha[model.n_delay:-n_last],
lot_beta[model.n_delay:-n_last], 1)
fit_lot_fn = np.poly1d(fit_lot)
# Plot
ax[1, 1].scatter(lot_alpha[model.n_delay:-n_last],
lot_beta[model.n_delay:-n_last],
label='Simulation')
ax[1, 1].plot(lot_alpha[model.n_delay:-n_last],
fit_lot_fn(lot_alpha[model.n_delay:-n_last]),
'--k', label='Linear fit')
fig.tight_layout()
return fig, ax
def carouselModel(conf):
'''
pass this a conf, and it'll return you a dae
'''
# empty Dae
dae = Dae()
# add some differential states/algebraic vars/controls/params
dae.addZ("nu")
dae.addX( [ "x"
, "y"
, "z"
, "e11"
, "e12"
, "e13"
, "e21"
, "e22"
, "e23"
, "e31"
, "e32"
, "e33"
, "dx"
, "dy"
, "dz"
, "w_bn_b_x"
, "w_bn_b_y"
, "w_bn_b_z"
, "ddelta"
, "r"
, "dr"
, "aileron"
, "elevator"
, "motor_torque"
, "ddr"
] )
if 'cn_rudder' in conf:
dae.addX('rudder')
dae.addU('drudder')
if 'cL_flaps' in conf:
dae.addX('flaps')
dae.addU('dflaps')
if conf['delta_parameterization'] == 'linear':
dae.addX('delta')
dae['cos_delta'] = C.cos(dae['delta'])
dae['sin_delta'] = C.sin(dae['delta'])
dae_delta_residual = dae.ddt('delta') - dae['ddelta']
elif conf['delta_parameterization'] == 'cos_sin':
dae.addX("cos_delta")
dae.addX("sin_delta")
norm = dae['cos_delta']**2 + dae['sin_delta']**2
if 'stabilize_invariants' in conf and conf['stabilize_invariants'] == True:
pole_delta = 0.5
else:
pole_delta = 0.0
cos_delta_dot_st = -pole_delta/2.* ( dae['cos_delta'] - dae['cos_delta'] / norm )
sin_delta_dot_st = -pole_delta/2.* ( dae['sin_delta'] - dae['sin_delta'] / norm )
dae_delta_residual = C.veccat([dae.ddt('cos_delta') - (-dae['sin_delta']*dae['ddelta'] + cos_delta_dot_st),
dae.ddt('sin_delta') - ( dae['cos_delta']*dae['ddelta'] + sin_delta_dot_st) ])
else:
raise ValueError('unrecognized delta_parameterization "'+conf['delta_parameterization']+'"')
if "parametrizeInputsAsOnlineData" in conf and conf[ "parametrizeInputsAsOnlineData" ] is True:
dae.addP( [ "daileron",
"delevator",
"dmotor_torque",
'dddr' ] )
else:
dae.addU( [ "daileron",
"delevator",
"dmotor_torque",
'dddr' ] )
# add wind parameter if wind shear is in configuration
if 'wind_model' in conf:
if conf['wind_model']['name'] == 'hardcoded':
dae['w0'] = conf['wind_model']['hardcoded_value']
elif conf['wind_model']['name'] != 'random_walk':
dae.addP( ['w0'] )
# set some state derivatives as outputs
dae['ddx'] = dae.ddt('dx')
dae['ddy'] = dae.ddt('dy')
dae['ddz'] = dae.ddt('dz')
dae['ddt_w_bn_b_x'] = dae.ddt('w_bn_b_x')
dae['ddt_w_bn_b_y'] = dae.ddt('w_bn_b_y')
dae['ddt_w_bn_b_z'] = dae.ddt('w_bn_b_z')
dae['w_bn_b'] = C.veccat([dae['w_bn_b_x'], dae['w_bn_b_y'], dae['w_bn_b_z']])
# some outputs in for plotting
dae['carousel_RPM'] = dae['ddelta']*60/(2*C.pi)
dae['aileron_deg'] = dae['aileron']*180/C.pi
dae['elevator_deg'] = dae['elevator']*180/C.pi
dae['daileron_deg_s'] = dae['daileron']*180/C.pi
dae['delevator_deg_s'] = dae['delevator']*180/C.pi
# tether tension == radius*nu where nu is alg. var associated with x^2+y^2+z^2-r^2==0
dae['tether_tension'] = dae['r']*dae['nu']
# theoretical mechanical power
dae['mechanical_winch_power'] = -dae['tether_tension']*dae['dr']
# carousel2 motor model from thesis data
dae['rpm'] = -dae['dr']/0.1*60/(2*C.pi)
dae['torque'] = dae['tether_tension']*0.1
dae['electrical_winch_power'] = 293.5816373499238 + \
0.0003931623408*dae['rpm']*dae['rpm'] + \
0.0665919381751*dae['torque']*dae['torque'] + \
0.1078628659825*dae['rpm']*dae['torque']
dae['R_c2b'] = C.blockcat([[dae['e11'],dae['e12'],dae['e13']],
[dae['e21'],dae['e22'],dae['e23']],
[dae['e31'],dae['e32'],dae['e33']]])
dae["yaw"] = C.arctan2(dae["e12"], dae["e11"]) * 180.0 / C.pi
dae["pitch"] = C.arcsin( -dae["e13"] ) * 180.0 / C.pi
dae["roll"] = C.arctan2(dae["e23"], dae["e33"]) * 180.0 / C.pi
# line angle
dae['cos_line_angle'] = \
-(dae['e31']*dae['x'] + dae['e32']*dae['y'] + dae['e33']*dae['z']) / C.sqrt(dae['x']**2 + dae['y']**2 + dae['z']**2)
dae['line_angle_deg'] = C.arccos(dae['cos_line_angle'])*180.0/C.pi
(massMatrix, rhs, dRexp) = setupModel(dae, conf)
if 'stabilize_invariants' in conf and conf['stabilize_invariants'] == True:
RotPole = 0.5
else:
RotPole = 0.0
Rst = RotPole*C.mul( dae['R_c2b'], (C.inv(C.mul(dae['R_c2b'].T,dae['R_c2b'])) - numpy.eye(3)) )
ode = C.veccat([
C.veccat([dae.ddt(name) for name in ['x','y','z']]) - C.veccat([dae['dx'],dae['dy'],dae['dz']]),
C.veccat([dae.ddt(name) for name in ["e11","e12","e13",
"e21","e22","e23",
"e31","e32","e33"]]) - ( dRexp.T.reshape([9,1]) + Rst.reshape([9,1]) ),
dae_delta_residual,
# C.veccat([dae.ddt(name) for name in ['dx','dy','dz']]) - C.veccat([dae['ddx'],dae['ddy'],dae['ddz']]),
# C.veccat([dae.ddt(name) for name in ['w1','w2','w3']]) - C.veccat([dae['dw1'],dae['dw2'],dae['dw3']]),
# dae.ddt('ddelta') - dae['dddelta'],
dae.ddt('r') - dae['dr'],
dae.ddt('dr') - dae['ddr'],
dae.ddt('aileron') - dae['daileron'],
dae.ddt('elevator') - dae['delevator'],
dae.ddt('motor_torque') - dae['dmotor_torque'],
dae.ddt('ddr') - dae['dddr']
])
if 'rudder' in dae:
ode = C.veccat([ode, dae.ddt('rudder') - dae['drudder']])
if 'flaps' in dae:
ode = C.veccat([ode, dae.ddt('flaps') - dae['dflaps']])
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 ]:
ode = C.veccat([ode,
dae.ddt('t1_disturbance') - dae['dt1_disturbance']])
if _which[ 1 ]:
ode = C.veccat([ode,
dae.ddt('t2_disturbance') - dae['dt2_disturbance']])
if _which[ 2 ]:
ode = C.veccat([ode,
dae.ddt('t3_disturbance') - dae['dt3_disturbance']])
elif _type == 'parameter':
if _which[ 0 ]:
ode = C.veccat([ode, dae.ddt('t1_disturbance')])
if _which[ 1 ]:
ode = C.veccat([ode, dae.ddt('t2_disturbance')])
if _which[ 2 ]:
ode = C.veccat([ode, dae.ddt('t3_disturbance')])
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 is 'random_walk':
if _which[ 0 ]:
ode = C.veccat([ode,
dae.ddt('f1_disturbance') - dae['df1_disturbance']])
if _which[ 1 ]:
ode = C.veccat([ode,
dae.ddt('f2_disturbance') - dae['df2_disturbance']])
if _which[ 2 ]:
ode = C.veccat([ode,
dae.ddt('f3_disturbance') - dae['df3_disturbance']])
elif _type == 'parameter':
if _which[ 0 ]:
ode = C.veccat([ode, dae.ddt('f1_disturbance')])
if _which[ 1 ]:
ode = C.veccat([ode, dae.ddt('f2_disturbance')])
if _which[ 2 ]:
ode = C.veccat([ode, dae.ddt('f3_disturbance')])
if 'wind_model' in conf and conf['wind_model']['name'] == 'random_walk':
tau_wind = conf['wind_model']['tau_wind']
ode = C.veccat([ode,
dae.ddt('wind_x') - (-dae['wind_x'] / tau_wind + dae['delta_wind_x'])
, dae.ddt('wind_y') - (-dae['wind_y'] / tau_wind + dae['delta_wind_y'])
, dae.ddt('wind_z') - (-dae['wind_z'] / tau_wind + dae['delta_wind_z'])
])
if 'stabilize_invariants' in conf and conf['stabilize_invariants'] == True:
cPole = 0.5
else:
cPole = 0.0
rhs[-1] -= 2*cPole*dae['cdot'] + cPole*cPole*dae['c']
psuedoZVec = C.veccat([dae.ddt(name) for name in ['ddelta','dx','dy','dz','w_bn_b_x','w_bn_b_y','w_bn_b_z']]+[dae['nu']])
alg = C.mul(massMatrix, psuedoZVec) - rhs
dae.setResidual([ode,alg])
return dae
def traverse(node, casadi_syms, rootnode):
#print node
#print node.args
#print len(node.args)
if len(node.args)==0:
# Handle symbols
if(node.is_Symbol):
return casadi_syms[node.name]
# Handle numbers and constants
if node.is_Zero:
return 0
if node.is_Number:
return float(node)
trig = sympy.functions.elementary.trigonometric
if len(node.args)==1:
# Handle unary operators
child = traverse(node.args[0], casadi_syms, rootnode) # Recursion!
if type(node) == trig.cos:
return casadi.cos(child)
if type(node) == trig.sin:
return casadi.sin(child)
if type(node) == trig.tan:
return casadi.tan(child)
if type(node) == trig.cosh:
return casadi.cosh(child)
if type(node) == trig.sinh:
return casadi.sinh(child)
if type(node) == trig.tanh:
return casadi.tanh(child)
if type(node) == trig.cot:
return 1/casadi.tan(child)
if type(node) == trig.acos:
return casadi.arccos(child)
if type(node) == trig.asin:
return casadi.arcsin(child)
if type(node) == trig.atan:
return casadi.arctan(child)
if len(node.args)==2:
# Handle binary operators
left = traverse(node.args[0], casadi_syms, rootnode) # Recursion!
right = traverse(node.args[1], casadi_syms, rootnode) # Recursion!
if node.is_Pow:
return left**right
if type(node) == trig.atan2:
return casadi.arctan2(left,right)
if len(node.args)>=2:
# Handle n-ary operators
child_generator = ( traverse(arg,casadi_syms,rootnode)
for arg in node.args )
if node.is_Add:
return reduce(lambda x, y: x+y, child_generator)
if node.is_Mul:
return reduce(lambda x, y: x*y, child_generator)
if node!=rootnode:
raise Exception("No mapping to casadi for node of type "
+ str(type(node)))
def crosswind_model(conf):
'''
pass this a conf, and it'll return you a dae
'''
# empty Dae
dae = Dae()
# add some differential states/algebraic vars/controls/params
dae.addZ('nu')
dae.addX( ['r_n2b_n_x', 'r_n2b_n_y', 'r_n2b_n_z',
'e11', 'e12', 'e13',
'e21', 'e22', 'e23',
'e31', 'e32', 'e33',
'v_bn_n_x', 'v_bn_n_y', 'v_bn_n_z',
'w_bn_b_x', 'w_bn_b_y', 'w_bn_b_z',
'aileron', 'elevator', 'rudder', 'flaps', 'prop_drag'
])
dae.addU( [ 'daileron'
, 'delevator'
, 'drudder'
, 'dflaps'
, 'dprop_drag'
] )
dae.addP( ['r','w0'] )
# set some state derivatives as outputs
dae['ddx'] = dae.ddt('v_bn_n_x')
dae['ddy'] = dae.ddt('v_bn_n_y')
dae['ddz'] = dae.ddt('v_bn_n_z')
dae['ddt_w_bn_b_x'] = dae.ddt('w_bn_b_x')
dae['ddt_w_bn_b_y'] = dae.ddt('w_bn_b_y')
dae['ddt_w_bn_b_z'] = dae.ddt('w_bn_b_z')
dae['w_bn_b'] = C.veccat([dae['w_bn_b_x'], dae['w_bn_b_y'], dae['w_bn_b_z']])
# some outputs in degrees for plotting
dae['aileron_deg'] = dae['aileron']*180/C.pi
dae['elevator_deg'] = dae['elevator']*180/C.pi
dae['rudder_deg'] = dae['rudder']*180/C.pi
dae['daileron_deg_s'] = dae['daileron']*180/C.pi
dae['delevator_deg_s'] = dae['delevator']*180/C.pi
dae['drudder_deg_s'] = dae['drudder']*180/C.pi
# tether tension == radius*nu where nu is alg. var associated with x^2+y^2+z^2-r^2==0
dae['tether_tension'] = dae['r']*dae['nu']
dae['R_n2b'] = C.vertcat([C.horzcat([dae['e11'], dae['e12'], dae['e13']]),
C.horzcat([dae['e21'], dae['e22'], dae['e23']]),
C.horzcat([dae['e31'], dae['e32'], dae['e33']])])
# local wind
dae['wind_at_altitude'] = get_wind(dae, conf)
# 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( [ dae['v_bn_n_x'] , dae['v_bn_n_y'], dae['v_bn_n_z'] ] )
# 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']))
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 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)
# derivative of dcm
dRexp = C.mul(skew_mat(dae['w_bn_b']).T, dae['R_n2b'])
ddt_R_n2b = C.vertcat(\
[C.horzcat([dae.ddt(name) for name in ['e11', 'e12', 'e13']]),
C.horzcat([dae.ddt(name) for name in ['e21', 'e22', 'e23']]),
C.horzcat([dae.ddt(name) for name in ['e31', 'e32', 'e33']])])
# get mass matrix, rhs
(mass_matrix, rhs) = compute_mass_matrix(dae, conf, f1, f2, f3, t1, t2, t3)
# set up the residual
ode = C.veccat([
C.veccat([dae.ddt('r_n2b_n_'+name) - dae['v_bn_n_'+name] for name in ['x','y','z']]),
C.veccat([ddt_R_n2b - dRexp]),
dae.ddt('aileron') - dae['daileron'],
dae.ddt('elevator') - dae['delevator'],
dae.ddt('rudder') - dae['drudder'],
dae.ddt('flaps') - dae['dflaps'],
dae.ddt('prop_drag') - dae['dprop_drag']
])
# acceleration for plotting
ddx = dae['ddx']
ddy = dae['ddy']
ddz = dae['ddz']
dae['accel_g'] = C.sqrt(ddx**2 + ddy**2 + (ddz + 9.8)**2)/9.8
dae['accel_without_gravity_g'] = C.sqrt(ddx**2 + ddy**2 + ddz**2)/9.8
dae['accel'] = C.sqrt(ddx**2 + ddy**2 + (ddz+9.8)**2)
dae['accel_without_gravity'] = C.sqrt(ddx**2 + ddy**2 + ddz**2)
# line angle
dae['cos_line_angle'] = \
-(dae['e31']*dae['r_n2b_n_x'] + dae['e32']*dae['r_n2b_n_y'] + dae['e33']*dae['r_n2b_n_z']) / \
C.sqrt(dae['r_n2b_n_x']**2 + dae['r_n2b_n_y']**2 + dae['r_n2b_n_z']**2)
dae['line_angle_deg'] = C.arccos(dae['cos_line_angle'])*180.0/C.pi
# add local loyd's limit
def addLoydsLimit():
w = dae['wind_at_altitude']
cL = dae['cL']
cD = dae['cD'] + dae['cD_tether']
rho = conf['rho']
S = conf['sref']
loyds = 2/27.0*rho*S*w**3*cL**3/cD**2
loyds2 = 2/27.0*rho*S*w**3*(cL**2/cD**2 + 1)**(1.5)*cD
dae["loyds_limit"] = loyds
dae["loyds_limit_exact"] = loyds2
addLoydsLimit()
psuedo_zvec = C.veccat([dae.ddt(name) for name in \
['v_bn_n_x','v_bn_n_y','v_bn_n_z','w_bn_b_x','w_bn_b_y','w_bn_b_z']]+[dae['nu']])
alg = C.mul(mass_matrix, psuedo_zvec) - rhs
dae.setResidual( [ode, alg] )
return dae
def log(cls, R):
# TODO
theta = ca.arccos((ca.trace(R) - 1) / 2)
return vee(cls.C3(theta) * (R - R.T))
def traverse(node, casadi_syms, rootnode):
#print node
#print node.args
#print len(node.args)
if len(node.args)==0:
# Handle symbols
if(node.is_Symbol):
return casadi_syms[node.name]
# Handle numbers and constants
if node.is_Zero:
return 0
if node.is_Number:
return float(node)
trig = sympy.functions.elementary.trigonometric
if len(node.args)==1:
# Handle unary operators
child = traverse(node.args[0], casadi_syms, rootnode) # Recursion!
if type(node) == trig.cos:
return casadi.cos(child)
if type(node) == trig.sin:
return casadi.sin(child)
if type(node) == trig.tan:
return casadi.tan(child)
if type(node) == trig.cosh:
return casadi.cosh(child)
if type(node) == trig.sinh:
return casadi.sinh(child)
if type(node) == trig.tanh:
return casadi.tanh(child)
if type(node) == trig.cot:
return 1/casadi.tan(child)
if type(node) == trig.acos:
return casadi.arccos(child)
if type(node) == trig.asin:
return casadi.arcsin(child)
if type(node) == trig.atan:
return casadi.arctan(child)
if len(node.args)==2:
# Handle binary operators
left = traverse(node.args[0], casadi_syms, rootnode) # Recursion!
right = traverse(node.args[1], casadi_syms, rootnode) # Recursion!
if node.is_Pow:
return left**right
if type(node) == trig.atan2:
return casadi.arctan2(left,right)
if len(node.args)>=2:
# Handle n-ary operators
child_generator = ( traverse(arg,casadi_syms,rootnode)
for arg in node.args )
if node.is_Add:
return reduce(lambda x, y: x+y, child_generator)
if node.is_Mul:
return reduce(lambda x, y: x*y, child_generator)
if node!=rootnode:
raise Exception("No mapping to casadi for node of type "
+ str(type(node)))
def crosswindModel(conf):
'''
pass this a conf, and it'll return you a dae
'''
# empty Dae
dae = Dae()
# add some differential states/algebraic vars/controls/params
dae.addZ("nu")
dae.addX( [ "x"
, "y"
, "z"
, "e11"
, "e12"
, "e13"
, "e21"
, "e22"
, "e23"
, "e31"
, "e32"
, "e33"
, "dx"
, "dy"
, "dz"
, "w_bn_b_x"
, "w_bn_b_y"
, "w_bn_b_z"
, "r"
, "dr"
, 'ddr'
, "aileron"
, "elevator"
, "rudder"
, "flaps"
] )
dae.addU( [ "daileron"
, "delevator"
, "drudder"
, "dflaps"
, 'dddr'
] )
dae.addP( ['w0'] )
# set some state derivatives as outputs
dae['ddx'] = dae.ddt('dx')
dae['ddy'] = dae.ddt('dy')
dae['ddz'] = dae.ddt('dz')
dae['ddt_w_bn_b_x'] = dae.ddt('w_bn_b_x')
dae['ddt_w_bn_b_y'] = dae.ddt('w_bn_b_y')
dae['ddt_w_bn_b_z'] = dae.ddt('w_bn_b_z')
dae['w_bn_b'] = C.veccat([dae['w_bn_b_x'], dae['w_bn_b_y'], dae['w_bn_b_z']])
# some outputs in degrees for plotting
dae['aileron_deg'] = dae['aileron']*180/C.pi
dae['elevator_deg'] = dae['elevator']*180/C.pi
dae['rudder_deg'] = dae['rudder']*180/C.pi
dae['daileron_deg_s'] = dae['daileron']*180/C.pi
dae['delevator_deg_s'] = dae['delevator']*180/C.pi
dae['drudder_deg_s'] = dae['drudder']*180/C.pi
# tether tension == radius*nu where nu is alg. var associated with x^2+y^2+z^2-r^2==0
dae['tether_tension'] = dae['r']*dae['nu']
# theoretical mechanical power
dae['mechanical_winch_power'] = -dae['tether_tension']*dae['dr']
# carousel2 motor model from thesis data
dae['rpm'] = -dae['dr']/0.1*60/(2*C.pi)
dae['torque'] = dae['tether_tension']*0.1
dae['electrical_winch_power'] = 293.5816373499238 + \
0.0003931623408*dae['rpm']*dae['rpm'] + \
0.0665919381751*dae['torque']*dae['torque'] + \
0.1078628659825*dae['rpm']*dae['torque']
dae['R_n2b'] = C.vertcat([C.horzcat([dae['e11'],dae['e12'],dae['e13']]),
C.horzcat([dae['e21'],dae['e22'],dae['e23']]),
C.horzcat([dae['e31'],dae['e32'],dae['e33']])])
# get mass matrix, rhs
(massMatrix, rhs, dRexp) = setupModel(dae, conf)
# set up the residual
ode = C.veccat([
C.veccat([dae.ddt(name) for name in ['x','y','z']]) - C.veccat([dae['dx'],dae['dy'],dae['dz']]),
C.veccat([dae.ddt(name) for name in ["e11","e12","e13",
"e21","e22","e23",
"e31","e32","e33"]]) - dRexp.trans().reshape([9,1]),
# C.veccat([dae.ddt(name) for name in ['dx','dy','dz']]) - C.veccat([dae['ddx'],dae['ddy'],dae['ddz']]),
# C.veccat([dae.ddt(name) for name in ['w1','w2','w3']]) - C.veccat([dae['dw1'],dae['dw2'],dae['dw3']]),
dae.ddt('r') - dae['dr'],
dae.ddt('dr') - dae['ddr'],
dae.ddt('ddr') - dae['dddr'],
dae.ddt('aileron') - dae['daileron'],
dae.ddt('elevator') - dae['delevator'],
dae.ddt('rudder') - dae['drudder'],
dae.ddt('flaps') - dae['dflaps']
])
# acceleration for plotting
ddx = dae['ddx']
ddy = dae['ddy']
ddz = dae['ddz']
dae['accel_g'] = C.sqrt(ddx**2 + ddy**2 + (ddz + 9.8)**2)/9.8
dae['accel_without_gravity_g'] = C.sqrt(ddx**2 + ddy**2 + ddz**2)/9.8
dae['accel'] = C.sqrt(ddx**2 + ddy**2 + (ddz+9.8)**2)
dae['accel_without_gravity'] = C.sqrt(ddx**2 + ddy**2 + ddz**2)
# line angle
dae['cos_line_angle'] = \
-(dae['e31']*dae['x'] + dae['e32']*dae['y'] + dae['e33']*dae['z']) / C.sqrt(dae['x']**2 + dae['y']**2 + dae['z']**2)
dae['line_angle_deg'] = C.arccos(dae['cos_line_angle'])*180.0/C.pi
# add local loyd's limit
def addLoydsLimit():
w = dae['wind_at_altitude']
cL = dae['cL']
cD = dae['cD'] + dae['cD_tether']
rho = conf['rho']
S = conf['sref']
loyds = 2/27.0*rho*S*w**3*cL**3/cD**2
loyds2 = 2/27.0*rho*S*w**3*(cL**2/cD**2 + 1)**(1.5)*cD
dae["loyds_limit"] = loyds
dae["loyds_limit_exact"] = loyds2
dae['neg_electrical_winch_power'] = -dae['electrical_winch_power']
dae['neg_mechanical_winch_power'] = -dae['mechanical_winch_power']
addLoydsLimit()
psuedoZVec = C.veccat([dae.ddt(name) for name in \
['dx','dy','dz','w_bn_b_x','w_bn_b_y','w_bn_b_z']]+[dae['nu']])
alg = C.mul(massMatrix, psuedoZVec) - rhs
dae.setResidual( [ode, alg] )
return dae
def crosswind_model(conf):
'''
pass this a conf, and it'll return you a dae
'''
# empty Dae
dae = Dae()
# add some differential states/algebraic vars/controls/params
dae.addZ('nu')
dae.addX([
'r_n2b_n_x', 'r_n2b_n_y', 'r_n2b_n_z', 'e11', 'e12', 'e13', 'e21',
'e22', 'e23', 'e31', 'e32', 'e33', 'v_bn_n_x', 'v_bn_n_y', 'v_bn_n_z',
'w_bn_b_x', 'w_bn_b_y', 'w_bn_b_z', 'aileron', 'elevator', 'rudder',
'flaps', 'prop_drag'
])
dae.addU(['daileron', 'delevator', 'drudder', 'dflaps', 'dprop_drag'])
dae.addP(['r', 'w0'])
# set some state derivatives as outputs
dae['ddx'] = dae.ddt('v_bn_n_x')
dae['ddy'] = dae.ddt('v_bn_n_y')
dae['ddz'] = dae.ddt('v_bn_n_z')
dae['ddt_w_bn_b_x'] = dae.ddt('w_bn_b_x')
dae['ddt_w_bn_b_y'] = dae.ddt('w_bn_b_y')
dae['ddt_w_bn_b_z'] = dae.ddt('w_bn_b_z')
dae['w_bn_b'] = C.veccat(
[dae['w_bn_b_x'], dae['w_bn_b_y'], dae['w_bn_b_z']])
# some outputs in degrees for plotting
dae['aileron_deg'] = dae['aileron'] * 180 / C.pi
dae['elevator_deg'] = dae['elevator'] * 180 / C.pi
dae['rudder_deg'] = dae['rudder'] * 180 / C.pi
dae['daileron_deg_s'] = dae['daileron'] * 180 / C.pi
dae['delevator_deg_s'] = dae['delevator'] * 180 / C.pi
dae['drudder_deg_s'] = dae['drudder'] * 180 / C.pi
# tether tension == radius*nu where nu is alg. var associated with x^2+y^2+z^2-r^2==0
dae['tether_tension'] = dae['r'] * dae['nu']
dae['R_n2b'] = C.vertcat([
C.horzcat([dae['e11'], dae['e12'], dae['e13']]),
C.horzcat([dae['e21'], dae['e22'], dae['e23']]),
C.horzcat([dae['e31'], dae['e32'], dae['e33']])
])
# local wind
dae['wind_at_altitude'] = get_wind(dae, conf)
# 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(
[dae['v_bn_n_x'], dae['v_bn_n_y'], dae['v_bn_n_z']])
# 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']))
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 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)
# derivative of dcm
dRexp = C.mul(skew_mat(dae['w_bn_b']).T, dae['R_n2b'])
ddt_R_n2b = C.vertcat(\
[C.horzcat([dae.ddt(name) for name in ['e11', 'e12', 'e13']]),
C.horzcat([dae.ddt(name) for name in ['e21', 'e22', 'e23']]),
C.horzcat([dae.ddt(name) for name in ['e31', 'e32', 'e33']])])
# get mass matrix, rhs
(mass_matrix, rhs) = compute_mass_matrix(dae, conf, f1, f2, f3, t1, t2, t3)
# set up the residual
ode = C.veccat([
C.veccat([
dae.ddt('r_n2b_n_' + name) - dae['v_bn_n_' + name]
for name in ['x', 'y', 'z']
]),
C.veccat([ddt_R_n2b - dRexp]),
dae.ddt('aileron') - dae['daileron'],
dae.ddt('elevator') - dae['delevator'],
dae.ddt('rudder') - dae['drudder'],
dae.ddt('flaps') - dae['dflaps'],
dae.ddt('prop_drag') - dae['dprop_drag']
])
# acceleration for plotting
ddx = dae['ddx']
ddy = dae['ddy']
ddz = dae['ddz']
dae['accel_g'] = C.sqrt(ddx**2 + ddy**2 + (ddz + 9.8)**2) / 9.8
dae['accel_without_gravity_g'] = C.sqrt(ddx**2 + ddy**2 + ddz**2) / 9.8
dae['accel'] = C.sqrt(ddx**2 + ddy**2 + (ddz + 9.8)**2)
dae['accel_without_gravity'] = C.sqrt(ddx**2 + ddy**2 + ddz**2)
# line angle
dae['cos_line_angle'] = \
-(dae['e31']*dae['r_n2b_n_x'] + dae['e32']*dae['r_n2b_n_y'] + dae['e33']*dae['r_n2b_n_z']) / \
C.sqrt(dae['r_n2b_n_x']**2 + dae['r_n2b_n_y']**2 + dae['r_n2b_n_z']**2)
dae['line_angle_deg'] = C.arccos(dae['cos_line_angle']) * 180.0 / C.pi
# add local loyd's limit
def addLoydsLimit():
w = dae['wind_at_altitude']
cL = dae['cL']
cD = dae['cD'] + dae['cD_tether']
rho = conf['rho']
S = conf['sref']
loyds = 2 / 27.0 * rho * S * w**3 * cL**3 / cD**2
loyds2 = 2 / 27.0 * rho * S * w**3 * (cL**2 / cD**2 + 1)**(1.5) * cD
dae["loyds_limit"] = loyds
dae["loyds_limit_exact"] = loyds2
addLoydsLimit()
psuedo_zvec = C.veccat([dae.ddt(name) for name in \
['v_bn_n_x','v_bn_n_y','v_bn_n_z','w_bn_b_x','w_bn_b_y','w_bn_b_z']]+[dae['nu']])
alg = C.mul(mass_matrix, psuedo_zvec) - rhs
dae.setResidual([ode, alg])
return dae
def carouselModel(conf,nSteps=None,extraParams=[]):
dae = Dae()
for ep in extraParams:
dae.addP(ep)
# dae.addZ( [ "dddelta"
# , "ddx"
# , "ddy"
# , "ddz"
# , "dw1"
# , "dw2"
# , "dw3"
# , "nu"
# ] )
dae.addZ("nu")
dae.addX( [ "x"
, "y"
, "z"
, "e11"
, "e12"
, "e13"
, "e21"
, "e22"
, "e23"
, "e31"
, "e32"
, "e33"
, "dx"
, "dy"
, "dz"
, "w1"
, "w2"
, "w3"
, "ddelta"
, "r"
, "dr"
, "aileron"
, "elevator"
] )
if conf['delta_parameterization'] == 'linear':
dae.addX('delta')
dae['cos_delta'] = C.cos(dae['delta'])
dae['sin_delta'] = C.sin(dae['delta'])
dae_delta_residual = dae.ddt('delta') - dae['ddelta'],
elif conf['delta_parameterization'] == 'cos_sin':
dae.addX("cos_delta")
dae.addX("sin_delta")
dae_delta_residual = C.veccat([dae.ddt('cos_delta') - (-dae['sin_delta']*dae['ddelta']),
dae.ddt('sin_delta') - dae['cos_delta']*dae['ddelta']])
else:
raise ValueErorr('unrecognized delta_parameterization "'+conf['delta_parameterization']+'"')
dae.addU( [ "daileron"
, "delevator"
, "motor_torque"
, 'ddr'
] )
# add wind parameter if wind shear is in configuration
if 'z0' in conf and 'zt_roughness' in conf:
dae.addP( ['w0'] )
dae['RPM'] = dae['ddelta']*60/(2*C.pi)
dae['aileron(deg)'] = dae['aileron']*180/C.pi
dae['elevator(deg)'] = dae['elevator']*180/C.pi
dae['daileron(deg/s)'] = dae['daileron']*180/C.pi
dae['delevator(deg/s)'] = dae['delevator']*180/C.pi
dae['motor power'] = dae['motor_torque']*dae['ddelta']
dae['tether tension'] = dae['r']*dae['nu']
dae['winch power'] = -dae['tether tension']*dae['dr']
dae['dcm'] = C.vertcat([C.horzcat([dae['e11'],dae['e12'],dae['e13']]),
C.horzcat([dae['e21'],dae['e22'],dae['e23']]),
C.horzcat([dae['e31'],dae['e32'],dae['e33']])])
# line angle
dae['cos(line angle)'] = \
(dae['e31']*dae['x'] + dae['e32']*dae['y'] + dae['e33']*dae['z']) / C.sqrt(dae['x']**2 + dae['y']**2 + dae['z']**2)
dae['line angle (deg)'] = C.arccos(dae['cos(line angle)'])*180.0/C.pi
(massMatrix, rhs, dRexp) = setupModel(dae, conf)
ode = C.veccat([
C.veccat([dae.ddt(name) for name in ['x','y','z']]) - C.veccat([dae['dx'],dae['dy'],dae['dz']]),
C.veccat([dae.ddt(name) for name in ["e11","e12","e13",
"e21","e22","e23",
"e31","e32","e33"]]) - dRexp.trans().reshape([9,1]),
dae_delta_residual,
# C.veccat([dae.ddt(name) for name in ['dx','dy','dz']]) - C.veccat([dae['ddx'],dae['ddy'],dae['ddz']]),
# C.veccat([dae.ddt(name) for name in ['w1','w2','w3']]) - C.veccat([dae['dw1'],dae['dw2'],dae['dw3']]),
# dae.ddt('ddelta') - dae['dddelta'],
dae.ddt('r') - dae['dr'],
dae.ddt('dr') - dae['ddr'],
dae.ddt('aileron') - dae['daileron'],
dae.ddt('elevator') - dae['delevator']
])
if nSteps is not None:
dae.addP('endTime')
psuedoZVec = C.veccat([dae.ddt(name) for name in ['ddelta','dx','dy','dz','w1','w2','w3']]+[dae['nu']])
alg = C.mul(massMatrix, psuedoZVec) - rhs
dae.setResidual([ode,alg])
return dae
def crosswindModel(conf):
dae = Dae()
dae.addZ("nu")
dae.addX( [ "x"
, "y"
, "z"
, "e11"
, "e12"
, "e13"
, "e21"
, "e22"
, "e23"
, "e31"
, "e32"
, "e33"
, "dx"
, "dy"
, "dz"
, "w1"
, "w2"
, "w3"
, "aileron"
, "elevator"
] )
dae.addU( [ "daileron"
, "delevator"
, 'prop_drag'
] )
dae.addP( ['r',
'w0'] )
dae['ddx'] = dae.ddt('dx')
dae['ddy'] = dae.ddt('dy')
dae['ddz'] = dae.ddt('dz')
dae['dw1'] = dae.ddt('w1')
dae['dw2'] = dae.ddt('w2')
dae['dw3'] = dae.ddt('w3')
dae['aileron_deg'] = dae['aileron']*180/C.pi
dae['elevator_deg'] = dae['elevator']*180/C.pi
dae['daileron_deg_s'] = dae['daileron']*180/C.pi
dae['delevator_deg_s'] = dae['delevator']*180/C.pi
dae['tether_tension'] = dae['r']*dae['nu']
dae['dcm'] = C.vertcat([C.horzcat([dae['e11'],dae['e12'],dae['e13']]),
C.horzcat([dae['e21'],dae['e22'],dae['e23']]),
C.horzcat([dae['e31'],dae['e32'],dae['e33']])])
(massMatrix, rhs, dRexp) = setupModel(dae, conf)
ode = C.veccat([
C.veccat([dae.ddt(name) for name in ['x','y','z']]) - C.veccat([dae['dx'],dae['dy'],dae['dz']]),
C.veccat([dae.ddt(name) for name in ["e11","e12","e13",
"e21","e22","e23",
"e31","e32","e33"]]) - dRexp.trans().reshape([9,1]),
# C.veccat([dae.ddt(name) for name in ['dx','dy','dz']]) - C.veccat([dae['ddx'],dae['ddy'],dae['ddz']]),
# C.veccat([dae.ddt(name) for name in ['w1','w2','w3']]) - C.veccat([dae['dw1'],dae['dw2'],dae['dw3']]),
dae.ddt('aileron') - dae['daileron'],
dae.ddt('elevator') - dae['delevator']
])
# acceleration
# ddx = dae['ddx']
# ddy = dae['ddy']
# ddz = dae['ddz']
ddx = dae.ddt('dx')
ddy = dae.ddt('dy')
ddz = dae.ddt('dz')
dae['accel_g'] = C.sqrt(ddx**2 + ddy**2 + (ddz + 9.8)**2)/9.8
dae['accel_without_gravity_g'] = C.sqrt(ddx**2 + ddy**2 + ddz**2)/9.8
dae['accel'] = C.sqrt(ddx**2 + ddy**2 + (ddz+9.8)**2)
dae['accel_without_gravity'] = C.sqrt(ddx**2 + ddy**2 + ddz**2)
# line angle
dae['cos_line_angle'] = \
(dae['e31']*dae['x'] + dae['e32']*dae['y'] + dae['e33']*dae['z']) / C.sqrt(dae['x']**2 + dae['y']**2 + dae['z']**2)
dae['line_angle_deg'] = C.arccos(dae['cos_line_angle'])*180.0/C.pi
psuedoZVec = C.veccat([dae.ddt(name) for name in ['dx','dy','dz','w1','w2','w3']]+[dae['nu']])
alg = C.mul(massMatrix, psuedoZVec) - rhs
dae.setResidual( [ode, alg] )
return dae
def crosswindModel(conf):
'''
pass this a conf, and it'll return you a dae
'''
# empty Dae
dae = Dae()
# add some differential states/algebraic vars/controls/params
dae.addZ("nu")
dae.addX([
"x", "y", "z", "e11", "e12", "e13", "e21", "e22", "e23", "e31", "e32",
"e33", "dx", "dy", "dz", "w_bn_b_x", "w_bn_b_y", "w_bn_b_z", "r", "dr",
'ddr', "aileron", "elevator", "rudder", "flaps"
])
dae.addU(["daileron", "delevator", "drudder", "dflaps", 'dddr'])
dae.addP(['w0'])
# set some state derivatives as outputs
dae['ddx'] = dae.ddt('dx')
dae['ddy'] = dae.ddt('dy')
dae['ddz'] = dae.ddt('dz')
dae['ddt_w_bn_b_x'] = dae.ddt('w_bn_b_x')
dae['ddt_w_bn_b_y'] = dae.ddt('w_bn_b_y')
dae['ddt_w_bn_b_z'] = dae.ddt('w_bn_b_z')
dae['w_bn_b'] = C.veccat(
[dae['w_bn_b_x'], dae['w_bn_b_y'], dae['w_bn_b_z']])
# some outputs in degrees for plotting
dae['aileron_deg'] = dae['aileron'] * 180 / C.pi
dae['elevator_deg'] = dae['elevator'] * 180 / C.pi
dae['rudder_deg'] = dae['rudder'] * 180 / C.pi
dae['daileron_deg_s'] = dae['daileron'] * 180 / C.pi
dae['delevator_deg_s'] = dae['delevator'] * 180 / C.pi
dae['drudder_deg_s'] = dae['drudder'] * 180 / C.pi
# tether tension == radius*nu where nu is alg. var associated with x^2+y^2+z^2-r^2==0
dae['tether_tension'] = dae['r'] * dae['nu']
# theoretical mechanical power
dae['mechanical_winch_power'] = -dae['tether_tension'] * dae['dr']
# carousel2 motor model from thesis data
dae['rpm'] = -dae['dr'] / 0.1 * 60 / (2 * C.pi)
dae['torque'] = dae['tether_tension'] * 0.1
dae['electrical_winch_power'] = 293.5816373499238 + \
0.0003931623408*dae['rpm']*dae['rpm'] + \
0.0665919381751*dae['torque']*dae['torque'] + \
0.1078628659825*dae['rpm']*dae['torque']
dae['R_n2b'] = C.vertcat([
C.horzcat([dae['e11'], dae['e12'], dae['e13']]),
C.horzcat([dae['e21'], dae['e22'], dae['e23']]),
C.horzcat([dae['e31'], dae['e32'], dae['e33']])
])
# get mass matrix, rhs
(massMatrix, rhs, dRexp) = setupModel(dae, conf)
# set up the residual
ode = C.veccat([
C.veccat([dae.ddt(name) for name in ['x', 'y', 'z']]) -
C.veccat([dae['dx'], dae['dy'], dae['dz']]),
C.veccat([
dae.ddt(name) for name in
["e11", "e12", "e13", "e21", "e22", "e23", "e31", "e32", "e33"]
]) - dRexp.trans().reshape([9, 1]),
# C.veccat([dae.ddt(name) for name in ['dx','dy','dz']]) - C.veccat([dae['ddx'],dae['ddy'],dae['ddz']]),
# C.veccat([dae.ddt(name) for name in ['w1','w2','w3']]) - C.veccat([dae['dw1'],dae['dw2'],dae['dw3']]),
dae.ddt('r') - dae['dr'],
dae.ddt('dr') - dae['ddr'],
dae.ddt('ddr') - dae['dddr'],
dae.ddt('aileron') - dae['daileron'],
dae.ddt('elevator') - dae['delevator'],
dae.ddt('rudder') - dae['drudder'],
dae.ddt('flaps') - dae['dflaps']
])
# acceleration for plotting
ddx = dae['ddx']
ddy = dae['ddy']
ddz = dae['ddz']
dae['accel_g'] = C.sqrt(ddx**2 + ddy**2 + (ddz + 9.8)**2) / 9.8
dae['accel_without_gravity_g'] = C.sqrt(ddx**2 + ddy**2 + ddz**2) / 9.8
dae['accel'] = C.sqrt(ddx**2 + ddy**2 + (ddz + 9.8)**2)
dae['accel_without_gravity'] = C.sqrt(ddx**2 + ddy**2 + ddz**2)
# line angle
dae['cos_line_angle'] = \
-(dae['e31']*dae['x'] + dae['e32']*dae['y'] + dae['e33']*dae['z']) / C.sqrt(dae['x']**2 + dae['y']**2 + dae['z']**2)
dae['line_angle_deg'] = C.arccos(dae['cos_line_angle']) * 180.0 / C.pi
# add local loyd's limit
def addLoydsLimit():
w = dae['wind_at_altitude']
cL = dae['cL']
cD = dae['cD'] + dae['cD_tether']
rho = conf['rho']
S = conf['sref']
loyds = 2 / 27.0 * rho * S * w**3 * cL**3 / cD**2
loyds2 = 2 / 27.0 * rho * S * w**3 * (cL**2 / cD**2 + 1)**(1.5) * cD
dae["loyds_limit"] = loyds
dae["loyds_limit_exact"] = loyds2
dae['neg_electrical_winch_power'] = -dae['electrical_winch_power']
dae['neg_mechanical_winch_power'] = -dae['mechanical_winch_power']
addLoydsLimit()
psuedoZVec = C.veccat([dae.ddt(name) for name in \
['dx','dy','dz','w_bn_b_x','w_bn_b_y','w_bn_b_z']]+[dae['nu']])
alg = C.mul(massMatrix, psuedoZVec) - rhs
dae.setResidual([ode, alg])
return dae
def crosswindModel(conf,extraParams=[]):
dae = Dae()
for ep in extraParams:
dae.addP(ep)
# dae.addZ( [ "ddx"
# , "ddy"
# , "ddz"
# , "dw1"
# , "dw2"
# , "dw3"
# , "nu"
# ] )
dae.addZ("nu")
dae.addX( [ "x" # state 0
, "y" # state 1
, "z" # state 2
, "e11" # state 3
, "e12" # state 4
, "e13" # state 5
, "e21" # state 6
, "e22" # state 7
, "e23" # state 8
, "e31" # state 9
, "e32" # state 10
, "e33" # state 11
, "dx" # state 12
, "dy" # state 13
, "dz" # state 14
, "w1" # state 15
, "w2" # state 16
, "w3" # state 17
, "r" # state 20
, "dr" # state 21
, "aileron"
, "elevator"
] )
dae.addU( [ "daileron"
, "delevator"
, 'ddr'
] )
dae.addP( ['w0'] )
dae['ddx'] = dae.ddt('dx')
dae['ddy'] = dae.ddt('dy')
dae['ddz'] = dae.ddt('dz')
dae['dw1'] = dae.ddt('w1')
dae['dw2'] = dae.ddt('w2')
dae['dw3'] = dae.ddt('w3')
dae['aileron(deg)'] = dae['aileron']*180/C.pi
dae['elevator(deg)'] = dae['elevator']*180/C.pi
dae['daileron(deg/s)'] = dae['daileron']*180/C.pi
dae['delevator(deg/s)'] = dae['delevator']*180/C.pi
dae['tether tension'] = dae['r']*dae['nu']
dae['winch power'] = -dae['tether tension']*dae['dr']
dae['dcm'] = C.vertcat([C.horzcat([dae['e11'],dae['e12'],dae['e13']]),
C.horzcat([dae['e21'],dae['e22'],dae['e23']]),
C.horzcat([dae['e31'],dae['e32'],dae['e33']])])
(massMatrix, rhs, dRexp) = setupModel(dae, conf)
ode = C.veccat([
C.veccat([dae.ddt(name) for name in ['x','y','z']]) - C.veccat([dae['dx'],dae['dy'],dae['dz']]),
C.veccat([dae.ddt(name) for name in ["e11","e12","e13",
"e21","e22","e23",
"e31","e32","e33"]]) - dRexp.trans().reshape([9,1]),
# C.veccat([dae.ddt(name) for name in ['dx','dy','dz']]) - C.veccat([dae['ddx'],dae['ddy'],dae['ddz']]),
# C.veccat([dae.ddt(name) for name in ['w1','w2','w3']]) - C.veccat([dae['dw1'],dae['dw2'],dae['dw3']]),
dae.ddt('r') - dae['dr'],
dae.ddt('dr') - dae['ddr'],
dae.ddt('aileron') - dae['daileron'],
dae.ddt('elevator') - dae['delevator']
])
# acceleration
# ddx = dae['ddx']
# ddy = dae['ddy']
# ddz = dae['ddz']
ddx = dae.ddt('dx')
ddy = dae.ddt('dy')
ddz = dae.ddt('dz')
dae['accel (g)'] = C.sqrt(ddx**2 + ddy**2 + (ddz + 9.8)**2)/9.8
dae['accel without gravity (g)'] = C.sqrt(ddx**2 + ddy**2 + ddz**2)/9.8
dae['accel'] = C.sqrt(ddx**2 + ddy**2 + (ddz+9.8)**2)
dae['accel without gravity'] = C.sqrt(ddx**2 + ddy**2 + ddz**2)
# line angle
dae['cos(line angle)'] = \
(dae['e31']*dae['x'] + dae['e32']*dae['y'] + dae['e33']*dae['z']) / C.sqrt(dae['x']**2 + dae['y']**2 + dae['z']**2)
dae['line angle (deg)'] = C.arccos(dae['cos(line angle)'])*180.0/C.pi
# add local loyd's limit
def addLoydsLimit():
w = dae['wind at altitude']
cL = dae['cL']
cD = dae['cD']
rho = conf['rho']
S = conf['sref']
loyds = 2/27.0*rho*S*w**3*cL**3/cD**2
loyds2 = 2/27.0*rho*S*w**3*(cL**2/cD**2 + 1)**(1.5)*cD
dae["loyd's limit"] = loyds
dae["loyd's limit (exact)"] = loyds2
dae['-(winch power)'] = -dae['winch power']
addLoydsLimit()
psuedoZVec = C.veccat([dae.ddt(name) for name in ['dx','dy','dz','w1','w2','w3']]+[dae['nu']])
alg = C.mul(massMatrix, psuedoZVec) - rhs
dae.setResidual( [ode, alg] )
return dae
def rotation_to_angle(R):
# Source:
# https://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation
return cs.arccos((cs.trace(R) - 1.0) / 2.0)
