def export_quad_ode_model(): model_name = 'quad_ode' # set up states & controls q0 = SX.sym('q0') q1 = SX.sym('q1') q2 = SX.sym('q2') q3 = SX.sym('q3') omegax = SX.sym('omegax') omegay = SX.sym('omegay') omegaz = SX.sym('omegaz') x = vertcat(q0, q1, q2, q3, omegax, omegay, omegaz) # controls w1 = SX.sym('w1') w2 = SX.sym('w2') w3 = SX.sym('w3') w4 = SX.sym('w4') u = vertcat(w1, w2, w3, w4) # xdot q0_dot = SX.sym('q0_dot') q1_dot = SX.sym('q1_dot') q2_dot = SX.sym('q2_dot') q3_dot = SX.sym('q3_dot') omegax_dot = SX.sym('omegax_dot') omegay_dot = SX.sym('omegay_dot') omegaz_dot = SX.sym('omegaz_dot') xdot = vertcat(q0_dot, q1_dot, q2_dot, q3_dot, omegax_dot, omegay_dot, omegaz_dot) # algebraic variables # z = None # parameters # set up parameters rho = SX.sym('rho') # air density A = SX.sym('A') # propeller area Cl = SX.sym('Cl') # lift coefficient Cd = SX.sym('Cd') # drag coefficient m = SX.sym('m') # mass of quad g = SX.sym('g') # gravity J1 = SX.sym('J1') # mom inertia J2 = SX.sym('J2') # mom inertia J3 = SX.sym('J3') # mom inertia J = diag(vertcat(J1, J2, J3)) # This comes from ref[6] pg 449. # dq = 1/2 * G(q)' * Ω S = SX.zeros(3, 4) S[:, 0] = vertcat(-q1, -q2, -q3) S[:, 1:4] = q0 * np.eye(3) - skew(vertcat(q1, q2, q3)) print("S:", S, S.shape) #Define angular velocity vector OMG = vertcat(omegax, omegay, omegaz) p = vertcat(rho, A, Cl, Cd, m, g, J1, J2, J3) #Define torque applied to the system T = SX(3, 1) T[0, 0] = 0.5 * A * Cl * rho * (w2 * w2 - w4 * w4) T[1, 0] = 0.5 * A * Cl * rho * (w1 * w1 - w3 * w3) T[2, 0] = 0.5 * A * Cd * rho * (w1 * w1 - w2 * w2 + w3 * w3 - w4 * w4) # Reference generation (quaternion to eul) used in cost function. Eul = SX(3, 1) Eul[0, 0] = arctan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1**2 + q2**2)) Eul[1, 0] = arcsin(2 * (q0 * q2 - q3 * q1)) Eul[2, 0] = arctan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2**2 + q3**2)) # dynamics f_expl = vertcat(0.5 * mtimes(transpose(S), OMG), mtimes(inv(J), (T - cross(OMG, mtimes(J, OMG))))) f_impl = xdot - f_expl print("dynamics shape: ", f_expl.shape) model = AcadosModel() model.f_impl_expr = f_impl model.f_expl_expr = f_expl model.x = x model.xdot = xdot model.u = u # model.z = [] model.p = p model.name = model_name # add the nonlinear cost. # Cost defined in paper # --- --- # | roll(x) - roll_ref | # | pitch(x) - pitch_ref | # l(x,u,z) = | yaw(x) - yaw_ref | # | x - x_ref | # | u - u_ref | # --- --- 14x1 # --- --- # | roll(x) - roll_ref | # | pitch(x) - pitch_ref | # m(x) = | yaw(x) - yaw_ref | # | x - x_ref | # --- --- 10x1 model.cost_y_expr = vertcat( Eul, x, u) #: CasADi expression for nonlinear least squares model.cost_y_expr_e = vertcat( Eul, x) #: CasADi expression for nonlinear least squares, terminal model.con_h_expr = q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3 # # yReference checking. # refCheck = Function('y_ref', [vertcat(x, u)], [model.cost_y_expr]) # print(refCheck) # y_ref = refCheck(np.array([0.566, 0.571, 0.168, 0.571, 0.1, 0.2, 0.3, 1, 2, 3, 4])) # print(y_ref) # # dynCheck = Function('sys_dyn', [vertcat(x, u),p], [f_expl]) # print(dynCheck) # dx_ref = dynCheck(np.array([0.566, 0.571, 0.168, 0.571, 0.1, 0.2, 0.3, 1, 2, 3, 4]),np.array([1, 1, 1, 1, 1, 1, 1, 1, 1])) # print(dx_ref) return model
def export_quaternion_ode_model(): model_name = 'quaternion_ode' # set up states & controls q0 = SX.sym('q0') q1 = SX.sym('q1') q2 = SX.sym('q2') q3 = SX.sym('q3') omegax = SX.sym('omegax') omegay = SX.sym('omegay') omegaz = SX.sym('omegaz') x = vertcat(q0, q1, q2, q3, omegax, omegay, omegaz) # controls omegax_cont = SX.sym('omegax_cont') omegay_cont = SX.sym('omegay_cont') omegaz_cont = SX.sym('omegaz_cont') u = vertcat(omegax_cont, omegay_cont, omegaz_cont) # xdot q0_dot = SX.sym('q0_dot') q1_dot = SX.sym('q1_dot') q2_dot = SX.sym('q2_dot') q3_dot = SX.sym('q3_dot') omegax_dot = SX.sym('omegax_dot') omegay_dot = SX.sym('omegay_dot') omegaz_dot = SX.sym('omegaz_dot') xdot = vertcat(q0_dot, q1_dot, q2_dot, q3_dot, omegax_dot, omegay_dot, omegaz_dot) # algebraic variables # z = None # parameters p = [] # This comes from ref[6] pg 449. # dq = 1/2 * G(q)' * Ω S = SX.zeros(3, 4) S[:, 0] = vertcat(-q1, -q2, -q3) S[:, 1:4] = q0 * np.eye(3) - skew(vertcat(q1, q2, q3)) print("S:", S, S.shape) OMG = vertcat(omegax, omegay, omegaz) # dynamics f_expl = vertcat(0.5 * mtimes(S.T, OMG), u) f_impl = xdot - f_expl model = AcadosModel() model.f_impl_expr = f_impl model.f_expl_expr = f_expl model.x = x model.xdot = xdot model.u = u # model.z = [] model.p = p model.name = model_name # Reference generation (quaternion to eul) used in cost function. Eul = SX(3, 1) Eul[0, 0] = arctan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1**2 + q2**2)) Eul[1, 0] = arcsin(2 * (q0 * q2 - q3 * q1)) Eul[2, 0] = arctan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2**2 + q3**2)) quat_to_eul = Function("quat_to_eul", [x, u], [Eul]) model.cost_y_expr = vertcat( Eul, x[4:, :], u) #: CasADi expression for nonlinear least squares model.cost_y_expr_e = vertcat( Eul, x[4:, :]) #: CasADi expression for nonlinear least squares, terminal model.con_h_expr = q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3 return model