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
