1 from sympy
import Symbol, sin, cos, tan, diff
3 from sympy.matrices
import Matrix
10 theta = Symbol(
'theta')
12 x_dot = Symbol(
'x_dot')
13 y_dot = Symbol(
'y_dot')
14 z_dot = Symbol(
'z_dot')
15 phi_dot = Symbol(
'phi_dot')
16 theta_dot = Symbol(
'theta_dot')
17 psi_dot = Symbol(
'psi_dot')
25 mass_ = Symbol(
'mass_')
42 sin_theta = sin(theta)
43 cos_theta = cos(theta)
44 tan_theta = tan(theta)
51 [0, cos_phi, -sin_phi],
55 [cos_theta, 0, sin_theta],
57 [-sin_theta, 0, cos_theta]
60 [cos_psi, -sin_psi, 0],
61 [sin_psi, cos_psi, 0],
67 Gtot = Matrix([0, 0, - g_])
68 Ftot = Matrix([0, 0, F_1 + F_2 + F_3 + F_4])
70 pos_ddot = Gtot + R * Ftot / mass_
75 L_ * (F_1 - F_2), L_ * (F_1 - F_3),
77 (M_1 - M_2 + M_3 - M_4)
79 omega = Matrix([phi_dot, theta_dot, psi_dot])
82 Ix = 2 * mass_ * (radius * radius) / 5.0 + 2 * radius * radius * mass_
83 Iy = 2 * mass_ * (radius * radius) / 5.0 + 2 * radius * radius * mass_
84 Iz = 2 * mass_ * (radius * radius) / 5.0 + 4 * radius * radius * mass_
87 I = Matrix([[Ix, 0, 0], [0, Iy, 0], [0, 0, Iz]])
88 Iinv = Matrix([[1 / Ix, 0, 0], [0, 1 / Iy, 0], [0, 0, 1 / Iz]])
90 omega_dot = Iinv * (tau - omega.cross(I * omega))
95 phi_dot, theta_dot, psi_dot,
96 pos_ddot[0], pos_ddot[1], pos_ddot[2],
97 omega_dot[0], omega_dot[1], omega_dot[2]
101 x_, y_, z_, phi, theta, psi, x_dot, y_dot, z_dot, phi_dot, theta_dot, psi_dot
105 for var
in statevars:
106 fx.append(diff(state_dot, var).T)
108 fx = Matrix(np.array(fx).T.tolist())
113 print(list(fx.row(i)))
122 for var
in [u0, u1, u2, u3]:
123 fu.append(diff(state_dot, var).T)
125 fu = Matrix(np.array(fu).T.tolist())
128 print(list(fu.row(i)))