This repository has been archived by the owner on Feb 21, 2024. It is now read-only.
-
Notifications
You must be signed in to change notification settings - Fork 0
/
test_quad_ned.py
167 lines (143 loc) · 5.2 KB
/
test_quad_ned.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
from sympy import symbols
from sympy.physics.mechanics import *
# Reference frames and Points
# ---------------------------
N = ReferenceFrame('N') # Inertial Frame
B = ReferenceFrame('B') # Drone after X (roll) rotation (Final rotation)
C = ReferenceFrame('C') # Drone after Y (pitch) rotation
D = ReferenceFrame('D') # Drone after Z (yaw) rotation (First rotation)
No = Point('No')
Bcm = Point('Bcm') # Drone's center of mass
M1 = Point('M1') # Motor 1 is front left, then the rest increments CW (Drone is in X configuration, not +)
M2 = Point('M2')
M3 = Point('M3')
M4 = Point('M4')
# Variables
# ---------------------------
# x, y and z are the drone's coordinates in the inertial frame, expressed with the inertial frame
# xdot, ydot and zdot are the drone's velocities in the inertial frame, expressed with the inertial frame
# phi, theta and psi represents the drone's orientation in the inertial frame, expressed with a ZYX Body rotation
# p, q and r are the drone's angular velocities in the inertial frame, expressed with the drone's frame
x, y, z, xdot, ydot, zdot = dynamicsymbols('x y z xdot ydot zdot')
phi, theta, psi, p, q, r = dynamicsymbols('phi theta psi p q r')
# First derivatives of the variables
xd, yd, zd, xdotd, ydotd, zdotd = dynamicsymbols('x y z xdot ydot zdot', 1)
phid, thetad, psid, pd, qd, rd = dynamicsymbols('phi theta psi p q r', 1)
# Constants
# ---------------------------
mB, g, dxm, dym, dzm, IBxx, IByy, IBzz = symbols('mB g dxm dym dzm IBxx IByy IBzz')
ThrM1, ThrM2, ThrM3, ThrM4, TorM1, TorM2, TorM3, TorM4 = symbols('ThrM1 ThrM2 ThrM3 ThrM4 TorM1 TorM2 TorM3 TorM4')
# Rotation ZYX Body
# ---------------------------
D.orient(N, 'Axis', [psi, N.z])
C.orient(D, 'Axis', [theta, D.y])
B.orient(C, 'Axis', [phi, C.x])
# Origin
# ---------------------------
No.set_vel(N, 0)
# Translation
# ---------------------------
Bcm.set_pos(No, x*N.x + y*N.y + z*N.z)
Bcm.set_vel(N, Bcm.pos_from(No).dt(N))
# Motor placement
# M1 is front left, then clockwise numbering
# dzm is positive for motors above center of mass
# ---------------------------
M1.set_pos(Bcm, dxm*B.x - dym*B.y - dzm*B.z)
M2.set_pos(Bcm, dxm*B.x + dym*B.y - dzm*B.z)
M3.set_pos(Bcm, -dxm*B.x + dym*B.y - dzm*B.z)
M4.set_pos(Bcm, -dxm*B.x - dym*B.y - dzm*B.z)
M1.v2pt_theory(Bcm, N, B)
M2.v2pt_theory(Bcm, N, B)
M3.v2pt_theory(Bcm, N, B)
M4.v2pt_theory(Bcm, N, B)
# Inertia Dyadic
# ---------------------------
IB = inertia(B, IBxx, IByy, IBzz)
# Create Bodies
# ---------------------------
BodyB = RigidBody('BodyB', Bcm, B, mB, (IB, Bcm))
BodyList = [BodyB]
# Forces and Torques
# ---------------------------
Grav_Force = (Bcm, mB*g*N.z)
FM1 = (M1, -ThrM1*B.z)
FM2 = (M2, -ThrM2*B.z)
FM3 = (M3, -ThrM3*B.z)
FM4 = (M4, -ThrM4*B.z)
TM1 = (B, -TorM1*B.z)
TM2 = (B, TorM2*B.z)
TM3 = (B, -TorM3*B.z)
TM4 = (B, TorM4*B.z)
ForceList = [Grav_Force, FM1, FM2, FM3, FM4, TM1, TM2, TM3, TM4]
# Kinematic Differential Equations
# ---------------------------
kd = [xdot - xd, ydot - yd, zdot - zd, p - dot(B.ang_vel_in(N), B.x), q - dot(B.ang_vel_in(N), B.y), r - dot(B.ang_vel_in(N), B.z)]
# Kane's Method
# ---------------------------
KM = KanesMethod(N, q_ind=[x, y, z, phi, theta, psi], u_ind=[xdot, ydot, zdot, p, q, r], kd_eqs=kd)
(fr, frstar) = KM.kanes_equations(BodyList, ForceList)
# Equations of Motion
# ---------------------------
MM = KM.mass_matrix_full
kdd = KM.kindiffdict()
rhs = KM.forcing_full
rhs = rhs.subs(kdd)
MM.simplify()
print('Mass Matrix')
print('-----------')
mprint(MM)
print()
rhs.simplify()
print('Right Hand Side')
print('---------------')
mprint(rhs)
print()
# So, MM*x = rhs, where x is the State Derivatives
# Solve for x
stateDot = MM.inv()*rhs
print('State Derivatives')
print('-----------------------------------')
mprint(stateDot)
print()
# POST-PROCESSING
# ---------------------------
# Useful Numbers
# ---------------------------
# p, q, r are the drone's angular velocities in the inertial frame, expressed in the drone's frame.
# These calculations are not relevant to the ODE, but might be used for control.
print('P, Q, R (Angular velocities in drone frame)')
print('-------------------------------------------')
mprint(dot(B.ang_vel_in(N), B.x))
print()
mprint(dot(B.ang_vel_in(N), B.y))
print()
mprint(dot(B.ang_vel_in(N), B.z))
print()
# u, v, w are the drone's velocities in the inertial frame, expressed in the drone's frame.
# These calculations are not relevant to the ODE, but might be used for control.
u = dot(Bcm.vel(N).subs(kdd), B.x)
v = dot(Bcm.vel(N).subs(kdd), B.y)
w = dot(Bcm.vel(N).subs(kdd), B.z)
print('u, v, w (Velocities in drone frame)')
print('-----------------------------------')
mprint(u)
print()
mprint(v)
print()
mprint(w)
print()
# uFlat, vFlat, wFlat are the drone's velocities in the inertial frame, expressed in a frame
# parallel to the ground, but with the drone's heading (so only after the YAW rotation).
# These calculations are not relevant to the ODE, but might be used for control.
uFlat = dot(Bcm.vel(N).subs(kdd), D.x)
vFlat = dot(Bcm.vel(N).subs(kdd), D.y)
wFlat = dot(Bcm.vel(N).subs(kdd), D.z)
print('uFlat, vFlat, wFlat (Velocities in drone frame (after Yaw))')
print('-----------------------------------------------------------')
mprint(uFlat)
print()
mprint(vFlat)
print()
mprint(wFlat)
print()