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 courseAngle(vel_n): """ Returns the course angle.""" return cs.arctan2(vel_n[1], vel_n[0])
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)