def rpy_quaternion(quat):
""" Returns roll, pitch, yaw angle (ZYX-Convention) from a quaternion."""
qw, qx, qy, qz = quat[0], quat[1], quat[2], quat[3]
roll = cs.arctan2(2.0*(qw*qx + qy*qz), 1 - 2*(qx**2+qy**2))
pitch = cs.arcsin(2*(qw*qy-qz*qx))
yaw = cs.arctan2(2*(qw*qz+qx*qy), 1 - 2*(qy**2 + qz**2))
return roll, pitch, yaw
def fk_rpy_casadi(self, q):
T = self.fk_casadi(q)
s = ca.sqrt(T[0, 0] * T[0, 0] + T[0, 1] * T[0, 1])
r_x = ca.arctan2(-T[1, 2], T[2, 2])
r_y = ca.arctan2(T[0, 2], s)
r_z = ca.arctan2(-T[0, 1], T[2, 2])
return ca.vcat([T[:3, 3], r_x, r_y, r_z])
def getEuler(ocp, k):
r11 = ocp.lookup('e11',timestep=k)
r12 = ocp.lookup('e12',timestep=k)
mr13 = -ocp.lookup('e13',timestep=k)
# mr13 -- nan protect
# | mr13' > 1 = 1
# | mr13' < -1 = -1
# | otherwise = mr13'
r23 = ocp.lookup('e23',timestep=k)
r33 = ocp.lookup('e33',timestep=k)
yaw = C.arctan2(r12,r11)
pitch = C.arcsin(mr13)
roll = C.arctan2(r23,r33)
return (yaw,pitch,roll)
def getEuler(ocp, k):
r11 = ocp.lookup('e11', timestep=k)
r12 = ocp.lookup('e12', timestep=k)
mr13 = -ocp.lookup('e13', timestep=k)
# mr13 -- nan protect
# | mr13' > 1 = 1
# | mr13' < -1 = -1
# | otherwise = mr13'
r23 = ocp.lookup('e23', timestep=k)
r33 = ocp.lookup('e33', timestep=k)
yaw = C.arctan2(r12, r11)
pitch = C.arcsin(mr13)
roll = C.arctan2(r23, r33)
return (yaw, pitch, roll)
def new_model(
# Initial conditions
x_b0=0, y_b0=0, z_b0=0, vx_b0=10, vy_b0=5, vz_b0=15,
x_c0=20, y_c0=5, vx_c0=0, vy_c0=0,
# Initial covariance
S0=ca.diagcat([0.1, 0.1, 0, 0.1, 0.1, 0,
1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-2]) * 0.25,
# Hypercovariance weight
L0_weight=1e-5,
# Mass of the ball
mass=0.15,
# Discretization time step
dt=0.1,
# Number of Runge-Kutta integration intervals per time step
n_rk=10,
# Reaction time (in units of dt)
n_delay=1,
# System noise weight
M_weight=1e-3,
# Observation noise
N_min=1e-2, # when looking directly at the ball
N_max=1e0, # when the ball is 90 degrees from the gaze direction
# Final cost: w_cl * distance_between_ball_and_catcher
w_cl=1e3,
# Running cost on controls: u.T * R * u
R=1e0 * ca.diagcat([1e1, 1e0, 1e0, 1e-1]),
# Final cost of uncertainty: w_Sl * tr(S)
w_Sl=1e3,
# Running cost of uncertainty: w_S * tr(S)
w_S=1e2,
# Control limits
F_c1=7.5, F_c2=2.5,
w_max=2 * ca.pi,
psi_max=0.8 * ca.pi/2,
):
phi0 = ca.arctan2(y_b0-y_c0, x_b0-x_c0) # direction towards the ball
if phi0 < 0:
phi0 += 2 * ca.pi
psi0 = 0
# Initial mean
m0 = ca.DMatrix([x_b0, y_b0, z_b0, vx_b0, vy_b0, vz_b0,
x_c0, y_c0, vx_c0, vy_c0, phi0, psi0])
# Hypercovariance
L0 = ca.DMatrix.eye(m0.size()) * L0_weight
# System noise matrix
M = ca.DMatrix.eye(m0.size()) * M_weight
# Catcher dynamics is less noisy
M[-6:, -6:] = ca.DMatrix.eye(6) * 1e-5
return Model((m0, S0, L0), dt, n_rk, n_delay, (M, N_min, N_max),
(w_cl, R, w_Sl, w_S), (F_c1, F_c2, w_max, psi_max))
def compute_x_tilde(x, x_ref, state_space):
if state_space == 'longitudinal':
Vr = x[0]
alpha = x[1]
theta = x[2]
q = x[3]
Vr_ref = x_ref[0]
theta_ref = x_ref[1]
x_tilde = cs.vertcat(Vr - Vr_ref, alpha, theta - theta_ref, q)
elif state_space == 'full':
Vr_err = x[0] - x_ref[0]
q = x[3:7]
q_ref = x_ref[1:5]
# Compute reduced attitudes
R_d = cg.rotation_quaternion(q_ref)
R = cg.rotation_quaternion(q)
b = R_d[:, 0]
Gamma_d = R_d.T @ b
Gamma = R.T @ b
# Cost functions
q_err = 1 - cs.dot(cs.vertcat(1, 0, 0), Gamma)
# q_err = 1 - Gamma[0]
q_err = cs.vertcat(q_err, cs.cross(Gamma_d, Gamma))
qw = q[0]
qx = q[1]
qy = q[2]
qz = q[3]
phi = cs.arctan2(2 * (qw * qx + qy * qz), 1 - 2 * (qx**2 + qy**2))
omega_s = x[7:10]
beta = x[1]
x_tilde = cs.vertcat(Vr_err, q_err, omega_s, phi, beta)
elif state_space == 'lateral':
print('Not implemented state_space: ' + state_space)
else:
print('Unknown state_space: ' + state_space)
return x_tilde
__author__ = 'belousov'
# ============================================================================
# Initialization
# ============================================================================
# Initial condition
x_b0 = y_b0 = z_b0 = 0
vx_b0 = 10
vy_b0 = 4
vz_b0 = 15
x_c0 = 22
y_c0 = 7
vx_c0 = vy_c0 = 0
phi0 = ca.arctan2(y_b0-y_c0, x_b0-x_c0) # direction towards the ball
if phi0 < 0:
phi0 += 2 * ca.pi
psi0 = 0
# Initial mean
m0 = ca.DMatrix([x_b0, y_b0, z_b0, vx_b0, vy_b0, vz_b0,
x_c0, y_c0, vx_c0, vy_c0, phi0, psi0])
# Initial covariance
S0 = ca.diagcat([0.2, 0.2, 0, 0.5, 0.5, 0,
1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-2]) * 0.25
# Hypercovariance
L0 = ca.DMatrix.eye(m0.size()) * 1e-5
# Discretization step
dt = 0.1
# Number of Runge-Kutta integration intervals per time step
def getWindAnglesFrom_v_bw_b(airspeed, v_bw_b):
alpha = C.arctan2(v_bw_b[2], v_bw_b[0])
beta = C.arcsin(v_bw_b[1] / airspeed)
return (alpha, beta)
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
u[name] = steadyState[name]
p = {}
for name in dae.pNames():
p[name] = steadyState[name]
# set up the sim timer
timer = rawe.sim.Timer(dt)
timer.start()
# loop through and simulate, if there's an error close the communicator and throw exception
try:
while True:
# for k in range(100):
# sleep for dt
timer.sleep()
# time.sleep(0.1)
# send message to visualizer/plotter
outs = sim.getOutputs(x,u,p)
outs['delta'] = C.arctan2(x['sin_delta'], x['cos_delta'])
communicator.sendKite(x,u,p,outs,conf)
# try to take a simulation step of dt
try:
x = sim.step(x,u,p)
except RuntimeError:
# problem simulating, close the communicator
communicator.close()
raise Exception('OH NOES, IDAS CHOKED')
except KeyboardInterrupt:
print "closing..."
communicator.close()
pass
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
dx_bc = x_bN - x_cN
# Final velocity
v_bN = V['X',N_sim,ca.veccat,['vx_b','vy_b']]
v_cN = V['X',N_sim,ca.veccat,['vx_c','vy_c']]
dv_bc = v_bN - v_cN
# Angle between gaze and ball velocity
theta = V['X',N_sim,'theta']
phi = V['X',N_sim,'phi']
d = ca.veccat([ ca.cos(theta)*ca.cos(phi), ca.cos(theta)*ca.sin(phi), ca.sin(theta) ])
r = V['X',N_sim,ca.veccat,['vx_b','vy_b','vz_b']]
?????
delta = ca.arctan2(ca.norm_2(ca.cross()))
r_unit = r / (ca.norm_2(r) + 1e-2)
d_gaze = d - r_unit
# Regularize controls
J = 1e-1 * ca.sum_square(ca.veccat(V['U'])) * dt
# Multiple shooting constraints and non-linear control constraints
g, lbg, ubg = [], [], []
for k in range(N_sim):
# Multiple shooting
[x_next] = F([V['X',k], V['U',k], dt])
g.append(x_next - V['X',k+1])
lbg.append(ca.DMatrix.zeros(nx))
ubg.append(ca.DMatrix.zeros(nx))
# Lift axis ** Normed to we @>@> **
eLe1 = - eTe2*we3 + eTe3*we2
eLe2 = - eTe3*we1 + eTe1*we3
eLe3 = - eTe1*we2 + eTe2*we1
# AERODYNAMIC COEEFICIENTS
# #################
vT1 = wE1
vT2 = -lT*w3 + wE2
vT3 = lT*w2 + wE3
# alpha = alpha0-wE3/wE1
# beta = wE2/C.sqrt(wE1*wE1 + wE3*wE3)
alpha = alpha0 + C.arctan2(-wE3,wE1)
beta = C.arcsin(wE2/vKite)
#NOTE: beta & alphaTail are compensated for the tail motion induced by
#omega @>@>
# alphaTail = alpha0-vT3/vT1
# betaTail = vT2/C.sqrt(vT1*vT1 + vT3*vT3)
alphaTail = alpha0 + C.arctan2(-vT3,vT1)
betaTail = C.arcsin(vT2/vKite)
dae.addOutput('alpha(deg)', alpha*180/C.pi)
dae.addOutput('alphaTail(deg)', alphaTail*180/C.pi)
dae.addOutput('beta(deg)', beta*180/C.pi)
dae.addOutput('betaTail(deg)', betaTail*180/C.pi)
# cL = cLA*alpha + cLe*elevator + cL0
def aoa(vel_r):
""" Returns the angle of attack."""
return cs.arctan2(vel_r[2], vel_r[0])
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 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 get_flat_maps(ntrailers):
'''
returns expressions for state variables [x, y, phi, theta...]
as functions of outputs [y0, y1, Dy0, Dy1, ...]
'''
out = SX.zeros(ntrailers + 3, 2)
out[0, 0] = SX.sym(f'x{ntrailers - 1}')
out[0, 1] = SX.sym(f'y{ntrailers - 1}')
for i in range(1, ntrailers + 3):
out[i, 0] = SX.sym(f'D{i}x{ntrailers-1}')
out[i, 1] = SX.sym(f'D{i}y{ntrailers-1}')
args = out[1:-1, :].T.reshape((-1, 1))
dargs = out[2:, :].T.reshape((-1, 1))
# 1. find all thetas
p = out[0, :].T
v = sxvec(args[0], args[1])
theta = arctan2(v[1], v[0])
dtheta = jtimes(theta, args, dargs)
thetas = [theta]
velocities = [v]
positions = [p]
dthetas = [dtheta]
for i in range(ntrailers - 1):
p_next = p
v_next = v
theta_next = theta
dtheta_next = dtheta
p = p_next + sxvec(cos(theta_next), sin(theta_next))
v = v_next + sxvec(-sin(theta_next), cos(theta_next)) * dtheta_next
theta = arctan2(v[1], v[0])
dtheta = jtimes(theta, args, dargs)
thetas += [theta]
velocities += [v]
dthetas += [dtheta]
positions += [p]
positions = positions[::-1]
velocities = velocities[::-1]
dthetas = dthetas[::-1]
thetas = thetas[::-1]
# 2. find phi
v0 = velocities[0]
dtheta0 = dthetas[0]
theta0 = thetas[0]
phi = arctan2(dtheta0, cos(theta0) * v0[0] + sin(theta0) * v0[1])
# 3. find controls
u1 = v0.T @ sxvec(cos(theta0), sin(theta0))
u2 = jtimes(phi, args, dargs)
return make_tuple('FlatMaps',
flat_out=out[0],
flat_derivs=out,
u1=u1,
u2=u2,
phi=phi,
theta=thetas,
velocities=velocities,
positions=positions)
def ssa(vel_r):
""" Returns the sideslip angle."""
return cs.arctan2(vel_r[1], vel_r[0])
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 courseAngle(vel_n):
""" Returns the course angle."""
return cs.arctan2(vel_n[1], vel_n[0])
def getWindAnglesFrom_v_bw_b(airspeed, v_bw_b):
alpha = C.arctan2(v_bw_b[2], v_bw_b[0])
beta = C.arcsin(v_bw_b[1] / airspeed)
return (alpha, beta)
def setupOcp(dae,conf,nk,nicp=1,deg=4):
ocp = rawe.collocation.Coll(dae, nk=nk,nicp=nicp,deg=deg)
print "setting up collocation..."
ocp.setupCollocation(ocp('endTime'))
# constrain line angle
for k in range(0,nk):
ocp.constrain(ocp.lookup('cos_line_angle',timestep=k),'>=',C.cos(65*pi/180), tag=('line angle',k))
constrainAirspeedAlphaBeta(ocp)
constrainTetherForce(ocp)
realMotorConstraints(ocp)
# bounds
ocp.bound('aileron', (numpy.radians(-10),numpy.radians(10)))
ocp.bound('elevator',(numpy.radians(-10),numpy.radians(10)))
ocp.bound('rudder', (numpy.radians(-10),numpy.radians(10)))
ocp.bound('flaps', (numpy.radians(0),numpy.radians(0)))
ocp.bound('flaps', (-1,1), timestep=0, quiet=True) # LICQ, because of IC
ocp.bound('daileron', (numpy.radians(-40), numpy.radians(40)))
ocp.bound('delevator', (numpy.radians(-40), numpy.radians(40)))
ocp.bound('drudder', (numpy.radians(-40), numpy.radians(40)))
ocp.bound('dflaps', (numpy.radians(-40), numpy.radians(40)))
ocp.bound('ddelta',(-2*pi, 2*pi))
ocp.bound('x',(-2000,2000))
ocp.bound('y',(-2000,2000))
ocp.bound('z',(-2000,0))
ocp.bound('r',(2,300))
ocp.bound('dr',(-100,100))
ocp.bound('ddr',(-150,150))
ocp.bound('dddr',(-200,200))
# ocp.bound('dr',(-1000,1000))
# ocp.bound('ddr',(-1500,1500))
# ocp.bound('dddr',(-500,500))
ocp.bound('motor_torque',(-300,300))
ocp.bound('dmotor_torque',(-10000,10000))
ocp.bound('cos_delta',(-1.5,1.5))
ocp.bound('sin_delta',(-1.5,1.5))
for e in ['e11','e21','e31','e12','e22','e32','e13','e23','e33']:
ocp.bound(e,(-1.1,1.1))
for d in ['dx','dy','dz']:
ocp.bound(d,(-1000,1000))
for w in ['w_bn_b_x',
'w_bn_b_y',
'w_bn_b_z']:
ocp.bound(w,(-4*pi,4*pi))
ocp.bound('endTime',(1,17))
ocp.bound('w0',(10,10))
# boundary conditions
# constrain invariants
def constrainInvariantErrs():
rawekite.kiteutils.makeOrthonormal(ocp, ocp.lookup('R_c2b',timestep=0))
ocp.constrain(ocp.lookup('c',timestep=0), '==', 0, tag=('initial c 0',None))
ocp.constrain(ocp.lookup('cdot',timestep=0), '==', 0, tag=('initial cdot 0',None))
ocp.constrain(ocp('sin_delta',timestep=0)**2 + ocp('cos_delta',timestep=0)**2,
'==', 1, tag=('sin**2 + cos**2 == 1',None))
constrainInvariantErrs()
def getFourierFit(filename,phase):
# load the fourier fit
f=open(filename,'r')
trajFits = pickle.load(f)
f.close()
trajFits.setPhase(phase)
return trajFits
def get_fourier_dcm(fourier_traj):
return C.vertcat([C.horzcat([fourier_traj['e11'], fourier_traj['e12'], fourier_traj['e13']]),
C.horzcat([fourier_traj['e21'], fourier_traj['e22'], fourier_traj['e23']]),
C.horzcat([fourier_traj['e31'], fourier_traj['e32'], fourier_traj['e33']])])
startup = getFourierFit("data/carousel_homotopy_fourier.dat",ocp('phase0'))
for name in [ 'x','y','z',
'dx','dy','dz',
'w_bn_b_x','w_bn_b_y','w_bn_b_z',
'ddr',
'ddelta',
'aileron','elevator','rudder','flaps',
'motor_torque'
]:
ocp.constrain(ocp(name,timestep=0),'==',startup[name],tag=('initial '+name,None))
ocp.constrain(C.arctan2(ocp('sin_delta',timestep=0), ocp('cos_delta',timestep=0)),
'==',
C.arctan2(startup['sin_delta'], startup['cos_delta']),
tag=('startup delta matching',None))
dcm0 = get_fourier_dcm(startup)
dcm1 = rawekite.kiteutils.getDcm(ocp, 0, prefix='e')
rawekite.kiteutils.matchDcms(ocp, dcm0, dcm1, tag='startup')
crosswind = getFourierFit('../pumping_mode/data/crosswind_opt_mechanical_6_loops_fourier.dat',
ocp('phase1'))
correspondingNames = {'x':'r_n2b_n_x',
'y':'r_n2b_n_y',
'z':'r_n2b_n_z',
'dx':'v_bn_n_x',
'dy':'v_bn_n_y',
'dz':'v_bn_n_z'}
obj = 0
for name in [ 'x','y','z',
'dx','dy','dz',
# 'ddr',
# 'w_bn_b_x','w_bn_b_y','w_bn_b_z',
# 'aileron','elevator','rudder','flaps',
'e11','e12','e13','e21','e22','e23','e31','e32','e33'
]:
if name in correspondingNames:
name_ = correspondingNames[name]
else:
name_ = name
obj += (ocp(name,timestep=-1)-crosswind[name_])**2
ocp.bound('ddelta',(0,0),timestep=-1,force=True,quiet=True)
ocp.bound('sin_delta',(0,0),timestep=-1,force=True,quiet=True)
ocp.bound('cos_delta',(0.5,1.5),timestep=-1,force=True,quiet=True)
# dcm0 = rawekite.kiteutils.getDcm(traj, -1, prefix='e')
# dcm1 = rawekite.kiteutils.getDcm(ocp, -1, prefix='e')
# rawekite.kiteutils.matchDcms(ocp, dcm0, dcm1, tag='crosswind')
return (ocp,obj)
