├── .gitattributes ├── .gitignore ├── LICENSE ├── PyDy Scripts ├── 00 - Basic │ ├── Quad_3D_flu_ENU_Euler.py │ ├── Quad_3D_flu_ENU_Quat.py │ ├── Quad_3D_frd_NED_Euler.py │ ├── Quad_3D_frd_NED_Quat.py │ └── Quad_3D_uvw │ │ ├── Quad_3D_flu_ENU_Euler_uvw.py │ │ ├── Quad_3D_flu_ENU_Quat_uvw.py │ │ ├── Quad_3D_frd_NED_Euler_uvw.py │ │ └── Quad_3D_frd_NED_Quat_uvw.py ├── 01 - Added Gyroscopic Precession │ ├── Quad_3D_flu_ENU_Euler.py │ ├── Quad_3D_flu_ENU_Quat.py │ ├── Quad_3D_frd_NED_Euler.py │ ├── Quad_3D_frd_NED_Quat.py │ └── Quad_3D_frd_NED_Quat_RotorBodies.py └── 02 - Added Wind and Aero Drag │ ├── Quad_3D_flu_ENU_Quat.py │ └── Quad_3D_frd_NED_Quat.py ├── README.md └── Simulation ├── config.py ├── ctrl.py ├── quadFiles ├── __init__.py ├── initQuad.py └── quad.py ├── run_3D_simulation.py ├── trajectory.py ├── utils ├── __init__.py ├── animation.py ├── display.py ├── mixer.py ├── quaternionFunctions.py ├── rotationConversion.py ├── stateConversions.py └── windModel.py └── waypoints.py /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | # *.gif filter=lfs diff=lfs merge=lfs -text 4 | # *Gifs/ filter=lfs diff=lfs merge=lfs -text 5 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled source # 2 | ################### 3 | *.com 4 | *.class 5 | *.dll 6 | *.exe 7 | *.o 8 | *.so 9 | 10 | # Packages # 11 | ############ 12 | # it's better to unpack these files and commit the raw source 13 | # git has its own built in compression methods 14 | *.7z 15 | *.dmg 16 | *.gz 17 | *.iso 18 | *.jar 19 | *.rar 20 | *.tar 21 | *.zip 22 | 23 | # Logs and databases # 24 | ###################### 25 | *.log 26 | *.sql 27 | *.sqlite 28 | 29 | # Applications # 30 | ################ 31 | .vscode/ 32 | .idea/ 33 | __pycache__/ 34 | Gifs/ 35 | Gifs/Raw/ 36 | 37 | # OS generated files # 38 | ###################### 39 | .DS_Store 40 | .DS_Store? 41 | ._* 42 | .Spotlight-V100 43 | .Trashes 44 | ehthumbs.db 45 | Thumbs.db -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 John Bass 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /PyDy Scripts/00 - Basic/Quad_3D_flu_ENU_Euler.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | author: John Bass 4 | email: john.bobzwik@gmail.com 5 | license: MIT 6 | Please feel free to use and modify this, but keep the above information. Thanks! 7 | """ 8 | 9 | """ 10 | Using PyDy and Sympy, this script generates the equations for the state derivatives 11 | of a quadcopter in a 3-dimensional space. The states in this particular script are: 12 | 13 | x, y, z : Position of the drone's center of mass in the inertial frame, 14 | expressed in the inertial frame 15 | xdot, ydot, zdot : Velocity of the drone's center of mass in the inertial frame, 16 | expressed in the inertial frame 17 | phi, theta, psi : Orientation (roll, pitch, yaw angles) of the drone in the 18 | inertial frame, following the order ZYX (yaw, pitch, roll) 19 | p, q, r : Angular velocity of the drone in the inertial frame, 20 | expressed in the drone's frame 21 | 22 | Important note : This script uses a flu body orientation (front-left-up) and 23 | a ENU world orientation (East-North-Up). 24 | 25 | Other note : In the resulting state derivatives, there are still simplifications 26 | that can be made that SymPy cannot simplify (factoring). 27 | 28 | author: John Bass 29 | email: 30 | """ 31 | 32 | from sympy import symbols 33 | from sympy.physics.mechanics import * 34 | 35 | # Reference frames and Points 36 | # --------------------------- 37 | N = ReferenceFrame('N') # Inertial Frame 38 | B = ReferenceFrame('B') # Drone after X (roll) rotation (Final rotation) 39 | C = ReferenceFrame('C') # Drone after Y (pitch) rotation 40 | D = ReferenceFrame('D') # Drone after Z (yaw) rotation (First rotation) 41 | 42 | No = Point('No') 43 | Bcm = Point('Bcm') # Drone's center of mass 44 | M1 = Point('M1') # Motor 1 is front left, then the rest increments CW (Drone is in X configuration, not +) 45 | M2 = Point('M2') 46 | M3 = Point('M3') 47 | M4 = Point('M4') 48 | 49 | # Variables 50 | # --------------------------- 51 | # x, y and z are the drone's coordinates in the inertial frame, expressed with the inertial frame 52 | # xdot, ydot and zdot are the drone's velocities in the inertial frame, expressed with the inertial frame 53 | # phi, theta and psi represents the drone's orientation in the inertial frame, expressed with a ZYX Body rotation 54 | # p, q and r are the drone's angular velocities in the inertial frame, expressed with the drone's frame 55 | 56 | x, y, z, xdot, ydot, zdot = dynamicsymbols('x y z xdot ydot zdot') 57 | phi, theta, psi, p, q, r = dynamicsymbols('phi theta psi p q r') 58 | 59 | # First derivatives of the variables 60 | xd, yd, zd, xdotd, ydotd, zdotd = dynamicsymbols('x y z xdot ydot zdot', 1) 61 | phid, thetad, psid, pd, qd, rd = dynamicsymbols('phi theta psi p q r', 1) 62 | 63 | # Constants 64 | # --------------------------- 65 | mB, g, dxm, dym, dzm, IBxx, IByy, IBzz = symbols('mB g dxm dym dzm IBxx IByy IBzz') 66 | ThrM1, ThrM2, ThrM3, ThrM4, TorM1, TorM2, TorM3, TorM4 = symbols('ThrM1 ThrM2 ThrM3 ThrM4 TorM1 TorM2 TorM3 TorM4') 67 | 68 | # Rotation ZYX Body 69 | # --------------------------- 70 | D.orient(N, 'Axis', [psi, N.z]) 71 | C.orient(D, 'Axis', [theta, D.y]) 72 | B.orient(C, 'Axis', [phi, C.x]) 73 | 74 | # Origin 75 | # --------------------------- 76 | No.set_vel(N, 0) 77 | 78 | # Translation 79 | # --------------------------- 80 | Bcm.set_pos(No, x*N.x + y*N.y + z*N.z) 81 | Bcm.set_vel(N, Bcm.pos_from(No).dt(N)) 82 | 83 | # Motor placement 84 | # M1 is front left, then clockwise numbering 85 | # dzm is positive for motors above center of mass 86 | # --------------------------- 87 | M1.set_pos(Bcm, dxm*B.x + dym*B.y + dzm*B.z) 88 | M2.set_pos(Bcm, dxm*B.x - dym*B.y + dzm*B.z) 89 | M3.set_pos(Bcm, -dxm*B.x - dym*B.y + dzm*B.z) 90 | M4.set_pos(Bcm, -dxm*B.x + dym*B.y + dzm*B.z) 91 | M1.v2pt_theory(Bcm, N, B) 92 | M2.v2pt_theory(Bcm, N, B) 93 | M3.v2pt_theory(Bcm, N, B) 94 | M4.v2pt_theory(Bcm, N, B) 95 | 96 | # Inertia Dyadic 97 | # --------------------------- 98 | IB = inertia(B, IBxx, IByy, IBzz) 99 | 100 | # Create Bodies 101 | # --------------------------- 102 | BodyB = RigidBody('BodyB', Bcm, B, mB, (IB, Bcm)) 103 | BodyList = [BodyB] 104 | 105 | # Forces and Torques 106 | # --------------------------- 107 | Grav_Force = (Bcm, -mB*g*N.z) 108 | FM1 = (M1, ThrM1*B.z) 109 | FM2 = (M2, ThrM2*B.z) 110 | FM3 = (M3, ThrM3*B.z) 111 | FM4 = (M4, ThrM4*B.z) 112 | 113 | TM1 = (B, TorM1*B.z) 114 | TM2 = (B, -TorM2*B.z) 115 | TM3 = (B, TorM3*B.z) 116 | TM4 = (B, -TorM4*B.z) 117 | ForceList = [Grav_Force, FM1, FM2, FM3, FM4, TM1, TM2, TM3, TM4] 118 | 119 | # Kinematic Differential Equations 120 | # --------------------------- 121 | 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)] 122 | 123 | # Kane's Method 124 | # --------------------------- 125 | KM = KanesMethod(N, q_ind=[x, y, z, phi, theta, psi], u_ind=[xdot, ydot, zdot, p, q, r], kd_eqs=kd) 126 | (fr, frstar) = KM.kanes_equations(BodyList, ForceList) 127 | 128 | # Equations of Motion 129 | # --------------------------- 130 | MM = KM.mass_matrix_full 131 | kdd = KM.kindiffdict() 132 | rhs = KM.forcing_full 133 | rhs = rhs.subs(kdd) 134 | 135 | MM.simplify() 136 | print('Mass Matrix') 137 | print('-----------') 138 | mprint(MM) 139 | print() 140 | 141 | rhs.simplify() 142 | print('Right Hand Side') 143 | print('---------------') 144 | mprint(rhs) 145 | print() 146 | 147 | # So, MM*x = rhs, where x is the State Derivatives 148 | # Solve for x 149 | stateDot = MM.inv()*rhs 150 | print('State Derivatives') 151 | print('-----------------------------------') 152 | mprint(stateDot) 153 | print() 154 | 155 | # POST-PROCESSING 156 | # --------------------------- 157 | 158 | # Useful Numbers 159 | # --------------------------- 160 | # p, q, r are the drone's angular velocities in the inertial frame, expressed in the drone's frame. 161 | # These calculations are not relevant to the ODE, but might be used for control. 162 | print('P, Q, R (Angular velocities in drone frame)') 163 | print('-------------------------------------------') 164 | mprint(dot(B.ang_vel_in(N), B.x)) 165 | print() 166 | mprint(dot(B.ang_vel_in(N), B.y)) 167 | print() 168 | mprint(dot(B.ang_vel_in(N), B.z)) 169 | print() 170 | 171 | # u, v, w are the drone's velocities in the inertial frame, expressed in the drone's frame. 172 | # These calculations are not relevant to the ODE, but might be used for control. 173 | u = dot(Bcm.vel(N).subs(kdd), B.x) 174 | v = dot(Bcm.vel(N).subs(kdd), B.y) 175 | w = dot(Bcm.vel(N).subs(kdd), B.z) 176 | print('u, v, w (Velocities in drone frame)') 177 | print('-----------------------------------') 178 | mprint(u) 179 | print() 180 | mprint(v) 181 | print() 182 | mprint(w) 183 | print() 184 | 185 | # uFlat, vFlat, wFlat are the drone's velocities in the inertial frame, expressed in a frame 186 | # parallel to the ground, but with the drone's heading (so only after the YAW rotation). 187 | # These calculations are not relevant to the ODE, but might be used for control. 188 | uFlat = dot(Bcm.vel(N).subs(kdd), D.x) 189 | vFlat = dot(Bcm.vel(N).subs(kdd), D.y) 190 | wFlat = dot(Bcm.vel(N).subs(kdd), D.z) 191 | print('uFlat, vFlat, wFlat (Velocities in drone frame (after Yaw))') 192 | print('-----------------------------------------------------------') 193 | mprint(uFlat) 194 | print() 195 | mprint(vFlat) 196 | print() 197 | mprint(wFlat) 198 | print() 199 | -------------------------------------------------------------------------------- /PyDy Scripts/00 - Basic/Quad_3D_flu_ENU_Quat.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | author: John Bass 4 | email: john.bobzwik@gmail.com 5 | license: MIT 6 | Please feel free to use and modify this, but keep the above information. Thanks! 7 | """ 8 | 9 | """ 10 | Using PyDy and Sympy, this script generates the equations for the state derivatives 11 | of a quadcopter in a 3-dimensional space. The states in this particular script are: 12 | 13 | x, y, z : Position of the drone's center of mass in the inertial frame, 14 | expressed in the inertial frame 15 | xdot, ydot, zdot : Velocity of the drone's center of mass in the inertial frame, 16 | expressed in the inertial frame 17 | q0, q1, q2, q3 : Orientation of the drone in the inertial frame using quaternions 18 | p, q, r : Angular velocity of the drone in the inertial frame, 19 | expressed in the drone's frame 20 | 21 | Important note : This script uses a flu body orientation (front-left-up) and 22 | a ENU world orientation (East-North-Up) 23 | 24 | Other note : In the resulting state derivatives, there are still simplifications 25 | that can be made that SymPy cannot simplify (factoring). 26 | 27 | author: John Bass 28 | email: 29 | """ 30 | 31 | from sympy import symbols, Matrix 32 | from sympy.physics.mechanics import * 33 | 34 | # Reference frames and Points 35 | # --------------------------- 36 | N = ReferenceFrame('N') # Inertial Frame 37 | B = ReferenceFrame('B') # Drone after X (roll) rotation (Final rotation) 38 | 39 | No = Point('No') 40 | Bcm = Point('Bcm') # Drone's center of mass 41 | M1 = Point('M1') # Motor 1 is front left, then the rest increments CW (Drone is in X configuration, not +) 42 | M2 = Point('M2') 43 | M3 = Point('M3') 44 | M4 = Point('M4') 45 | 46 | # Variables 47 | # --------------------------- 48 | # x, y and z are the drone's coordinates in the inertial frame, expressed with the inertial frame 49 | # xdot, ydot and zdot are the drone's speeds in the inertial frame, expressed with the inertial frame 50 | # q0, q1, q2, q3 represents the drone's orientation in the inertial frame, using quaternions 51 | # p, q and r are the drone's angular velocities in the inertial frame, expressed with the drone's frame 52 | 53 | x, y, z, xdot, ydot, zdot = dynamicsymbols('x y z xdot ydot zdot') 54 | q0, q1, q2, q3, p, q, r = dynamicsymbols('q0 q1 q2 q3 p q r') 55 | 56 | # First derivatives of the variables 57 | xd, yd, zd, xdotd, ydotd, zdotd = dynamicsymbols('x y z xdot ydot zdot', 1) 58 | q0d, q1d, q2d, q3d, pd, qd, rd = dynamicsymbols('q0 q1 q2 q3 p q r', 1) 59 | 60 | # Constants 61 | # --------------------------- 62 | mB, g, dxm, dym, dzm, IBxx, IByy, IBzz = symbols('mB g dxm dym dzm IBxx IByy IBzz') 63 | ThrM1, ThrM2, ThrM3, ThrM4, TorM1, TorM2, TorM3, TorM4 = symbols('ThrM1 ThrM2 ThrM3 ThrM4 TorM1 TorM2 TorM3 TorM4') 64 | 65 | # Rotation Quaternion 66 | # --------------------------- 67 | B.orient(N, 'Quaternion', [q0, q1, q2, q3]) 68 | B.set_ang_vel(N, p*B.x + q*B.y + r*B.z) 69 | 70 | # Origin 71 | # --------------------------- 72 | No.set_vel(N, 0) 73 | 74 | # Translation 75 | # --------------------------- 76 | Bcm.set_pos(No, x*N.x + y*N.y + z*N.z) 77 | Bcm.set_vel(N, Bcm.pos_from(No).dt(N)) 78 | 79 | # Motor placement 80 | # M1 is front left, then clockwise numbering 81 | # dzm is positive for motors above center of mass 82 | # --------------------------- 83 | M1.set_pos(Bcm, dxm*B.x + dym*B.y + dzm*B.z) 84 | M2.set_pos(Bcm, dxm*B.x - dym*B.y + dzm*B.z) 85 | M3.set_pos(Bcm, -dxm*B.x - dym*B.y + dzm*B.z) 86 | M4.set_pos(Bcm, -dxm*B.x + dym*B.y + dzm*B.z) 87 | M1.v2pt_theory(Bcm, N, B) 88 | M2.v2pt_theory(Bcm, N, B) 89 | M3.v2pt_theory(Bcm, N, B) 90 | M4.v2pt_theory(Bcm, N, B) 91 | 92 | # Inertia Dyadic 93 | # --------------------------- 94 | IB = inertia(B, IBxx, IByy, IBzz) 95 | 96 | # Create Bodies 97 | # --------------------------- 98 | BodyB = RigidBody('BodyB', Bcm, B, mB, (IB, Bcm)) 99 | BodyList = [BodyB] 100 | 101 | # Forces and Torques 102 | # --------------------------- 103 | Grav_Force = (Bcm, -mB*g*N.z) 104 | FM1 = (M1, ThrM1*B.z) 105 | FM2 = (M2, ThrM2*B.z) 106 | FM3 = (M3, ThrM3*B.z) 107 | FM4 = (M4, ThrM4*B.z) 108 | 109 | TM1 = (B, TorM1*B.z) 110 | TM2 = (B, -TorM2*B.z) 111 | TM3 = (B, TorM3*B.z) 112 | TM4 = (B, -TorM4*B.z) 113 | ForceList = [Grav_Force, FM1, FM2, FM3, FM4, TM1, TM2, TM3, TM4] 114 | 115 | # Calculate Quaternion Derivative 116 | # --------------------------- 117 | Gquat = Matrix([[-q1, q0, q3, -q2], 118 | [-q2, -q3, q0, q1], 119 | [-q3, q2, -q1, q0]]) 120 | 121 | angVel = Matrix([[dot(B.ang_vel_in(N), B.x)],[dot(B.ang_vel_in(N), B.y)],[dot(B.ang_vel_in(N), B.z)]]) # Basically the same as: angVel = Matrix([[p],[q],[r]]) 122 | quat_dot = 1.0/2*Gquat.T*angVel 123 | 124 | # Kinematic Differential Equations 125 | # --------------------------- 126 | kd = [xdot - xd, ydot - yd, zdot - zd, q0d - quat_dot[0], q1d - quat_dot[1], q2d - quat_dot[2], q3d - quat_dot[3]] 127 | 128 | # Kane's Method 129 | # --------------------------- 130 | KM = KanesMethod(N, q_ind=[x, y, z, q0, q1, q2, q3], u_ind=[xdot, ydot, zdot, p, q, r], kd_eqs=kd) 131 | (fr, frstar) = KM.kanes_equations(BodyList, ForceList) 132 | 133 | # Equations of Motion 134 | # --------------------------- 135 | MM = KM.mass_matrix_full 136 | kdd = KM.kindiffdict() 137 | rhs = KM.forcing_full 138 | MM = MM.subs(kdd) 139 | rhs = rhs.subs(kdd) 140 | 141 | MM.simplify() 142 | # MM = MM.subs(q0**2 + q1**2 + q2**2 + q3**2, 1) 143 | print() 144 | print('Mass Matrix') 145 | print('-----------') 146 | mprint(MM) 147 | 148 | rhs.simplify() 149 | # rhs = rhs.subs(q0**2 + q1**2 + q2**2 + q3**2, 1) 150 | print('Right Hand Side') 151 | print('---------------') 152 | mprint(rhs) 153 | print() 154 | 155 | # So, MM*x = rhs, where x is the State Derivatives 156 | # Solve for x 157 | stateDot = MM.inv()*rhs 158 | print('State Derivatives') 159 | print('-----------------------------------') 160 | mprint(stateDot) 161 | print() 162 | 163 | # POST-PROCESSING 164 | # --------------------------- 165 | 166 | # Useful Numbers 167 | # --------------------------- 168 | # p, q, r are the drone's angular velocities in the inertial frame, expressed in the drone's frame. 169 | # These calculations are not relevant to the ODE, but might be used for control. 170 | quat_dot_2 = Matrix([[q0d],[q1d],[q2d],[q3d]]) 171 | angVel_2 = 2.0*Gquat*quat_dot_2 172 | print('P, Q, R (Angular velocities in drone frame)') 173 | print('-------------------------------------------') 174 | mprint(angVel_2[0]) 175 | print() 176 | mprint(angVel_2[1]) 177 | print() 178 | mprint(angVel_2[2]) 179 | print() 180 | 181 | # u, v, w are the drone's velocities in the inertial frame, expressed in the drone's frame. 182 | # These calculations are not relevant to the ODE, but might be used for control. 183 | u = dot(Bcm.vel(N).subs(kdd), B.x).simplify() 184 | v = dot(Bcm.vel(N).subs(kdd), B.y).simplify() 185 | w = dot(Bcm.vel(N).subs(kdd), B.z).simplify() 186 | print('u, v, w (Velocities in drone frame)') 187 | print('-----------------------------------') 188 | mprint(u) 189 | print() 190 | mprint(v) 191 | print() 192 | mprint(w) 193 | print() 194 | 195 | # uFlat, vFlat, wFlat are the drone's velocities in the inertial frame, expressed in a frame 196 | # parallel to the ground, but with the drone's heading. 197 | # These calculations are not relevant to the ODE, but might be used for control. 198 | uFlat = dot(Bcm.vel(N).subs(kdd), cross(B.y, N.z)).simplify() 199 | vFlat = dot(Bcm.vel(N).subs(kdd), -cross(B.x, N.z)).simplify() 200 | wFlat = dot(Bcm.vel(N).subs(kdd), N.z).simplify() 201 | print('uFlat, vFlat, wFlat (Velocities in drone frame (after Yaw))') 202 | print('-----------------------------------------------------------') 203 | mprint(uFlat) 204 | print() 205 | mprint(vFlat) 206 | print() 207 | mprint(wFlat) 208 | print() 209 | -------------------------------------------------------------------------------- /PyDy Scripts/00 - Basic/Quad_3D_frd_NED_Euler.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | author: John Bass 4 | email: john.bobzwik@gmail.com 5 | license: MIT 6 | Please feel free to use and modify this, but keep the above information. Thanks! 7 | """ 8 | 9 | """ 10 | Using PyDy and Sympy, this script generates the equations for the state derivatives 11 | of a quadcopter in a 3-dimensional space. The states in this particular script are: 12 | 13 | x, y, z : Position of the drone's center of mass in the inertial frame, 14 | expressed in the inertial frame 15 | xdot, ydot, zdot : Velocity of the drone's center of mass in the inertial frame, 16 | expressed in the inertial frame 17 | phi, theta, psi : Orientation (roll, pitch, yaw angles) of the drone in the 18 | inertial frame, following the order ZYX (yaw, pitch, roll) 19 | p, q, r : Angular velocity of the drone in the inertial frame, 20 | expressed in the drone's frame 21 | 22 | Important note : This script uses a frd body orientation (front-right-down) and 23 | a NED world orientation (North-East-Down). The drone's altitude is -z. 24 | 25 | Other note : In the resulting state derivatives, there are still simplifications 26 | that can be made that SymPy cannot simplify (factoring). 27 | 28 | author: John Bass 29 | email: 30 | """ 31 | 32 | from sympy import symbols 33 | from sympy.physics.mechanics import * 34 | 35 | # Reference frames and Points 36 | # --------------------------- 37 | N = ReferenceFrame('N') # Inertial Frame 38 | B = ReferenceFrame('B') # Drone after X (roll) rotation (Final rotation) 39 | C = ReferenceFrame('C') # Drone after Y (pitch) rotation 40 | D = ReferenceFrame('D') # Drone after Z (yaw) rotation (First rotation) 41 | 42 | No = Point('No') 43 | Bcm = Point('Bcm') # Drone's center of mass 44 | M1 = Point('M1') # Motor 1 is front left, then the rest increments CW (Drone is in X configuration, not +) 45 | M2 = Point('M2') 46 | M3 = Point('M3') 47 | M4 = Point('M4') 48 | 49 | # Variables 50 | # --------------------------- 51 | # x, y and z are the drone's coordinates in the inertial frame, expressed with the inertial frame 52 | # xdot, ydot and zdot are the drone's velocities in the inertial frame, expressed with the inertial frame 53 | # phi, theta and psi represents the drone's orientation in the inertial frame, expressed with a ZYX Body rotation 54 | # p, q and r are the drone's angular velocities in the inertial frame, expressed with the drone's frame 55 | 56 | x, y, z, xdot, ydot, zdot = dynamicsymbols('x y z xdot ydot zdot') 57 | phi, theta, psi, p, q, r = dynamicsymbols('phi theta psi p q r') 58 | 59 | # First derivatives of the variables 60 | xd, yd, zd, xdotd, ydotd, zdotd = dynamicsymbols('x y z xdot ydot zdot', 1) 61 | phid, thetad, psid, pd, qd, rd = dynamicsymbols('phi theta psi p q r', 1) 62 | 63 | # Constants 64 | # --------------------------- 65 | mB, g, dxm, dym, dzm, IBxx, IByy, IBzz = symbols('mB g dxm dym dzm IBxx IByy IBzz') 66 | ThrM1, ThrM2, ThrM3, ThrM4, TorM1, TorM2, TorM3, TorM4 = symbols('ThrM1 ThrM2 ThrM3 ThrM4 TorM1 TorM2 TorM3 TorM4') 67 | 68 | # Rotation ZYX Body 69 | # --------------------------- 70 | D.orient(N, 'Axis', [psi, N.z]) 71 | C.orient(D, 'Axis', [theta, D.y]) 72 | B.orient(C, 'Axis', [phi, C.x]) 73 | 74 | # Origin 75 | # --------------------------- 76 | No.set_vel(N, 0) 77 | 78 | # Translation 79 | # --------------------------- 80 | Bcm.set_pos(No, x*N.x + y*N.y + z*N.z) 81 | Bcm.set_vel(N, Bcm.pos_from(No).dt(N)) 82 | 83 | # Motor placement 84 | # M1 is front left, then clockwise numbering 85 | # dzm is positive for motors above center of mass 86 | # --------------------------- 87 | M1.set_pos(Bcm, dxm*B.x - dym*B.y - dzm*B.z) 88 | M2.set_pos(Bcm, dxm*B.x + dym*B.y - dzm*B.z) 89 | M3.set_pos(Bcm, -dxm*B.x + dym*B.y - dzm*B.z) 90 | M4.set_pos(Bcm, -dxm*B.x - dym*B.y - dzm*B.z) 91 | M1.v2pt_theory(Bcm, N, B) 92 | M2.v2pt_theory(Bcm, N, B) 93 | M3.v2pt_theory(Bcm, N, B) 94 | M4.v2pt_theory(Bcm, N, B) 95 | 96 | # Inertia Dyadic 97 | # --------------------------- 98 | IB = inertia(B, IBxx, IByy, IBzz) 99 | 100 | # Create Bodies 101 | # --------------------------- 102 | BodyB = RigidBody('BodyB', Bcm, B, mB, (IB, Bcm)) 103 | BodyList = [BodyB] 104 | 105 | # Forces and Torques 106 | # --------------------------- 107 | Grav_Force = (Bcm, mB*g*N.z) 108 | FM1 = (M1, -ThrM1*B.z) 109 | FM2 = (M2, -ThrM2*B.z) 110 | FM3 = (M3, -ThrM3*B.z) 111 | FM4 = (M4, -ThrM4*B.z) 112 | 113 | TM1 = (B, -TorM1*B.z) 114 | TM2 = (B, TorM2*B.z) 115 | TM3 = (B, -TorM3*B.z) 116 | TM4 = (B, TorM4*B.z) 117 | ForceList = [Grav_Force, FM1, FM2, FM3, FM4, TM1, TM2, TM3, TM4] 118 | 119 | # Kinematic Differential Equations 120 | # --------------------------- 121 | 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)] 122 | 123 | # Kane's Method 124 | # --------------------------- 125 | KM = KanesMethod(N, q_ind=[x, y, z, phi, theta, psi], u_ind=[xdot, ydot, zdot, p, q, r], kd_eqs=kd) 126 | (fr, frstar) = KM.kanes_equations(BodyList, ForceList) 127 | 128 | # Equations of Motion 129 | # --------------------------- 130 | MM = KM.mass_matrix_full 131 | kdd = KM.kindiffdict() 132 | rhs = KM.forcing_full 133 | rhs = rhs.subs(kdd) 134 | 135 | MM.simplify() 136 | print('Mass Matrix') 137 | print('-----------') 138 | mprint(MM) 139 | print() 140 | 141 | rhs.simplify() 142 | print('Right Hand Side') 143 | print('---------------') 144 | mprint(rhs) 145 | print() 146 | 147 | # So, MM*x = rhs, where x is the State Derivatives 148 | # Solve for x 149 | stateDot = MM.inv()*rhs 150 | print('State Derivatives') 151 | print('-----------------------------------') 152 | mprint(stateDot) 153 | print() 154 | 155 | # POST-PROCESSING 156 | # --------------------------- 157 | 158 | # Useful Numbers 159 | # --------------------------- 160 | # p, q, r are the drone's angular velocities in the inertial frame, expressed in the drone's frame. 161 | # These calculations are not relevant to the ODE, but might be used for control. 162 | print('P, Q, R (Angular velocities in drone frame)') 163 | print('-------------------------------------------') 164 | mprint(dot(B.ang_vel_in(N), B.x)) 165 | print() 166 | mprint(dot(B.ang_vel_in(N), B.y)) 167 | print() 168 | mprint(dot(B.ang_vel_in(N), B.z)) 169 | print() 170 | 171 | # u, v, w are the drone's velocities in the inertial frame, expressed in the drone's frame. 172 | # These calculations are not relevant to the ODE, but might be used for control. 173 | u = dot(Bcm.vel(N).subs(kdd), B.x) 174 | v = dot(Bcm.vel(N).subs(kdd), B.y) 175 | w = dot(Bcm.vel(N).subs(kdd), B.z) 176 | print('u, v, w (Velocities in drone frame)') 177 | print('-----------------------------------') 178 | mprint(u) 179 | print() 180 | mprint(v) 181 | print() 182 | mprint(w) 183 | print() 184 | 185 | # uFlat, vFlat, wFlat are the drone's velocities in the inertial frame, expressed in a frame 186 | # parallel to the ground, but with the drone's heading (so only after the YAW rotation). 187 | # These calculations are not relevant to the ODE, but might be used for control. 188 | uFlat = dot(Bcm.vel(N).subs(kdd), D.x) 189 | vFlat = dot(Bcm.vel(N).subs(kdd), D.y) 190 | wFlat = dot(Bcm.vel(N).subs(kdd), D.z) 191 | print('uFlat, vFlat, wFlat (Velocities in drone frame (after Yaw))') 192 | print('-----------------------------------------------------------') 193 | mprint(uFlat) 194 | print() 195 | mprint(vFlat) 196 | print() 197 | mprint(wFlat) 198 | print() 199 | -------------------------------------------------------------------------------- /PyDy Scripts/00 - Basic/Quad_3D_frd_NED_Quat.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | author: John Bass 4 | email: john.bobzwik@gmail.com 5 | license: MIT 6 | Please feel free to use and modify this, but keep the above information. Thanks! 7 | """ 8 | 9 | """ 10 | Using PyDy and Sympy, this script generates the equations for the state derivatives 11 | of a quadcopter in a 3-dimensional space. The states in this particular script are: 12 | 13 | x, y, z : Position of the drone's center of mass in the inertial frame, 14 | expressed in the inertial frame 15 | xdot, ydot, zdot : Velocity of the drone's center of mass in the inertial frame, 16 | expressed in the inertial frame 17 | q0, q1, q2, q3 : Orientation of the drone in the inertial frame using quaternions 18 | p, q, r : Angular velocity of the drone in the inertial frame, 19 | expressed in the drone's frame 20 | 21 | Important note : This script uses a frd body orientation (front-right-down) and 22 | a NED world orientation (North-East-Down). The drone's altitude is -z. 23 | 24 | Other note : In the resulting state derivatives, there are still simplifications 25 | that can be made that SymPy cannot simplify (factoring). 26 | 27 | author: John Bass 28 | email: 29 | """ 30 | 31 | from sympy import symbols, Matrix 32 | from sympy.physics.mechanics import * 33 | 34 | # Reference frames and Points 35 | # --------------------------- 36 | N = ReferenceFrame('N') # Inertial Frame 37 | B = ReferenceFrame('B') # Drone after X (roll) rotation (Final rotation) 38 | 39 | No = Point('No') 40 | Bcm = Point('Bcm') # Drone's center of mass 41 | M1 = Point('M1') # Motor 1 is front left, then the rest increments CW (Drone is in X configuration, not +) 42 | M2 = Point('M2') 43 | M3 = Point('M3') 44 | M4 = Point('M4') 45 | 46 | # Variables 47 | # --------------------------- 48 | # x, y and z are the drone's coordinates in the inertial frame, expressed with the inertial frame 49 | # xdot, ydot and zdot are the drone's speeds in the inertial frame, expressed with the inertial frame 50 | # q0, q1, q2, q3 represents the drone's orientation in the inertial frame, using quaternions 51 | # p, q and r are the drone's angular velocities in the inertial frame, expressed with the drone's frame 52 | 53 | x, y, z, xdot, ydot, zdot = dynamicsymbols('x y z xdot ydot zdot') 54 | q0, q1, q2, q3, p, q, r = dynamicsymbols('q0 q1 q2 q3 p q r') 55 | 56 | # First derivatives of the variables 57 | xd, yd, zd, xdotd, ydotd, zdotd = dynamicsymbols('x y z xdot ydot zdot', 1) 58 | q0d, q1d, q2d, q3d, pd, qd, rd = dynamicsymbols('q0 q1 q2 q3 p q r', 1) 59 | 60 | # Constants 61 | # --------------------------- 62 | mB, g, dxm, dym, dzm, IBxx, IByy, IBzz = symbols('mB g dxm dym dzm IBxx IByy IBzz') 63 | ThrM1, ThrM2, ThrM3, ThrM4, TorM1, TorM2, TorM3, TorM4 = symbols('ThrM1 ThrM2 ThrM3 ThrM4 TorM1 TorM2 TorM3 TorM4') 64 | 65 | # Rotation Quaternion 66 | # --------------------------- 67 | B.orient(N, 'Quaternion', [q0, q1, q2, q3]) 68 | B.set_ang_vel(N, p*B.x + q*B.y + r*B.z) 69 | 70 | # Origin 71 | # --------------------------- 72 | No.set_vel(N, 0) 73 | 74 | # Translation 75 | # --------------------------- 76 | Bcm.set_pos(No, x*N.x + y*N.y + z*N.z) 77 | Bcm.set_vel(N, Bcm.pos_from(No).dt(N)) 78 | 79 | # Motor placement 80 | # M1 is front left, then clockwise numbering 81 | # dzm is positive for motors above center of mass 82 | # --------------------------- 83 | M1.set_pos(Bcm, dxm*B.x - dym*B.y - dzm*B.z) 84 | M2.set_pos(Bcm, dxm*B.x + dym*B.y - dzm*B.z) 85 | M3.set_pos(Bcm, -dxm*B.x + dym*B.y - dzm*B.z) 86 | M4.set_pos(Bcm, -dxm*B.x - dym*B.y - dzm*B.z) 87 | M1.v2pt_theory(Bcm, N, B) 88 | M2.v2pt_theory(Bcm, N, B) 89 | M3.v2pt_theory(Bcm, N, B) 90 | M4.v2pt_theory(Bcm, N, B) 91 | 92 | # Inertia Dyadic 93 | # --------------------------- 94 | IB = inertia(B, IBxx, IByy, IBzz) 95 | 96 | # Create Bodies 97 | # --------------------------- 98 | BodyB = RigidBody('BodyB', Bcm, B, mB, (IB, Bcm)) 99 | BodyList = [BodyB] 100 | 101 | # Forces and Torques 102 | # --------------------------- 103 | Grav_Force = (Bcm, mB*g*N.z) 104 | FM1 = (M1, -ThrM1*B.z) 105 | FM2 = (M2, -ThrM2*B.z) 106 | FM3 = (M3, -ThrM3*B.z) 107 | FM4 = (M4, -ThrM4*B.z) 108 | 109 | TM1 = (B, -TorM1*B.z) 110 | TM2 = (B, TorM2*B.z) 111 | TM3 = (B, -TorM3*B.z) 112 | TM4 = (B, TorM4*B.z) 113 | ForceList = [Grav_Force, FM1, FM2, FM3, FM4, TM1, TM2, TM3, TM4] 114 | 115 | # Calculate Quaternion Derivative 116 | # --------------------------- 117 | Gquat = Matrix([[-q1, q0, q3, -q2], 118 | [-q2, -q3, q0, q1], 119 | [-q3, q2, -q1, q0]]) 120 | 121 | angVel = Matrix([[dot(B.ang_vel_in(N), B.x)],[dot(B.ang_vel_in(N), B.y)],[dot(B.ang_vel_in(N), B.z)]]) # Basically the same as: angVel = Matrix([[p],[q],[r]]) 122 | quat_dot = 1.0/2*Gquat.T*angVel 123 | 124 | # Kinematic Differential Equations 125 | # --------------------------- 126 | kd = [xdot - xd, ydot - yd, zdot - zd, q0d - quat_dot[0], q1d - quat_dot[1], q2d - quat_dot[2], q3d - quat_dot[3]] 127 | 128 | # Kane's Method 129 | # --------------------------- 130 | KM = KanesMethod(N, q_ind=[x, y, z, q0, q1, q2, q3], u_ind=[xdot, ydot, zdot, p, q, r], kd_eqs=kd) 131 | (fr, frstar) = KM.kanes_equations(BodyList, ForceList) 132 | 133 | # Equations of Motion 134 | # --------------------------- 135 | MM = KM.mass_matrix_full 136 | kdd = KM.kindiffdict() 137 | rhs = KM.forcing_full 138 | MM = MM.subs(kdd) 139 | rhs = rhs.subs(kdd) 140 | 141 | MM.simplify() 142 | # MM = MM.subs(q0**2 + q1**2 + q2**2 + q3**2, 1) 143 | print() 144 | print('Mass Matrix') 145 | print('-----------') 146 | mprint(MM) 147 | 148 | rhs.simplify() 149 | # rhs = rhs.subs(q0**2 + q1**2 + q2**2 + q3**2, 1) 150 | print('Right Hand Side') 151 | print('---------------') 152 | mprint(rhs) 153 | print() 154 | 155 | # So, MM*x = rhs, where x is the State Derivatives 156 | # Solve for x 157 | stateDot = MM.inv()*rhs 158 | print('State Derivatives') 159 | print('-----------------------------------') 160 | mprint(stateDot) 161 | print() 162 | 163 | # POST-PROCESSING 164 | # --------------------------- 165 | 166 | # Useful Numbers 167 | # --------------------------- 168 | # p, q, r are the drone's angular velocities in the inertial frame, expressed in the drone's frame. 169 | # These calculations are not relevant to the ODE, but might be used for control. 170 | quat_dot_2 = Matrix([[q0d],[q1d],[q2d],[q3d]]) 171 | angVel_2 = 2.0*Gquat*quat_dot_2 172 | print('P, Q, R (Angular velocities in drone frame)') 173 | print('-------------------------------------------') 174 | mprint(angVel_2[0]) 175 | print() 176 | mprint(angVel_2[1]) 177 | print() 178 | mprint(angVel_2[2]) 179 | print() 180 | 181 | # u, v, w are the drone's velocities in the inertial frame, expressed in the drone's frame. 182 | # These calculations are not relevant to the ODE, but might be used for control. 183 | u = dot(Bcm.vel(N).subs(kdd), B.x).simplify() 184 | v = dot(Bcm.vel(N).subs(kdd), B.y).simplify() 185 | w = dot(Bcm.vel(N).subs(kdd), B.z).simplify() 186 | print('u, v, w (Velocities in drone frame)') 187 | print('-----------------------------------') 188 | mprint(u) 189 | print() 190 | mprint(v) 191 | print() 192 | mprint(w) 193 | print() 194 | 195 | # uFlat, vFlat, wFlat are the drone's velocities in the inertial frame, expressed in a frame 196 | # parallel to the ground, but with the drone's heading. 197 | # These calculations are not relevant to the ODE, but might be used for control. 198 | uFlat = dot(Bcm.vel(N).subs(kdd), cross(B.y, N.z)).simplify() 199 | vFlat = dot(Bcm.vel(N).subs(kdd), -cross(B.x, N.z)).simplify() 200 | wFlat = dot(Bcm.vel(N).subs(kdd), N.z).simplify() 201 | print('uFlat, vFlat, wFlat (Velocities in drone frame (after Yaw))') 202 | print('-----------------------------------------------------------') 203 | mprint(uFlat) 204 | print() 205 | mprint(vFlat) 206 | print() 207 | mprint(wFlat) 208 | print() 209 | -------------------------------------------------------------------------------- /PyDy Scripts/00 - Basic/Quad_3D_uvw/Quad_3D_flu_ENU_Euler_uvw.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | author: John Bass 4 | email: john.bobzwik@gmail.com 5 | license: MIT 6 | Please feel free to use and modify this, but keep the above information. Thanks! 7 | """ 8 | 9 | """ 10 | Using PyDy and Sympy, this script generates the equations for the state derivatives 11 | of a quadcopter in a 3-dimensional space. The states in this particular script are: 12 | 13 | x, y, z : Position of the drone's center of mass in the inertial frame, 14 | expressed in the inertial frame 15 | u, v, w : Velocity of the drone's center of mass in the inertial frame, 16 | expressed in the drone's frame 17 | phi, theta, psi : Orientation (roll, pitch, yaw angles) of the drone in the 18 | inertial frame, following the order ZYX (yaw, pitch, roll) 19 | p, q, r : Angular velocity of the drone in the inertial frame, 20 | expressed in the drone's frame 21 | 22 | Important note : This script uses a flu body orientation (front-left-up) and 23 | a ENU world orientation (East-North-Up) 24 | 25 | Other note : In the resulting state derivatives, there are still simplifications 26 | that can be made that SymPy cannot simplify (factoring). 27 | 28 | author: John Bass 29 | email: 30 | """ 31 | 32 | from sympy import symbols 33 | from sympy.physics.mechanics import * 34 | 35 | # Reference frames and Points 36 | # --------------------------- 37 | N = ReferenceFrame('N') # Inertial Frame 38 | B = ReferenceFrame('B') # Drone after X (roll) rotation (Final rotation) 39 | C = ReferenceFrame('C') # Drone after Y (pitch) rotation 40 | D = ReferenceFrame('D') # Drone after Z (yaw) rotation (First rotation) 41 | 42 | No = Point('No') 43 | Bcm = Point('Bcm') # Drone's center of mass 44 | M1 = Point('M1') # Motor 1 is front left, then the rest increments CW (Drone is in X configuration, not +) 45 | M2 = Point('M2') 46 | M3 = Point('M3') 47 | M4 = Point('M4') 48 | 49 | # Variables 50 | # --------------------------- 51 | # x, y and z are the drone's coordinates in the inertial frame, expressed with the inertial frame 52 | # u, v and w are the drone's velocities in the inertial frame, expressed with the drone's frame 53 | # phi, theta and psi represents the drone's orientation in the inertial frame, expressed with a ZYX Body rotation 54 | # p, q and r are the drone's angular velocities in the inertial frame, expressed with the drone's frame 55 | 56 | x, y, z, u, v, w = dynamicsymbols('x y z u v w') 57 | phi, theta, psi, p, q, r = dynamicsymbols('phi theta psi p q r') 58 | 59 | # First derivatives of the variables 60 | xd, yd, zd, ud, vd, wd = dynamicsymbols('x y z u v w', 1) 61 | phid, thetad, psid, pd, qd, rd = dynamicsymbols('phi theta psi p q r', 1) 62 | 63 | # Constants 64 | # --------------------------- 65 | mB, g, dxm, dym, dzm, IBxx, IByy, IBzz = symbols('mB g dxm dym dzm IBxx IByy IBzz') 66 | ThrM1, ThrM2, ThrM3, ThrM4, TorM1, TorM2, TorM3, TorM4 = symbols('ThrM1 ThrM2 ThrM3 ThrM4 TorM1 TorM2 TorM3 TorM4') 67 | 68 | # Rotation ZYX Body 69 | # --------------------------- 70 | D.orient(N, 'Axis', [psi, N.z]) 71 | C.orient(D, 'Axis', [theta, D.y]) 72 | B.orient(C, 'Axis', [phi, C.x]) 73 | 74 | # Origin 75 | # --------------------------- 76 | No.set_vel(N, 0) 77 | 78 | # Translation 79 | # --------------------------- 80 | Bcm.set_pos(No, x*N.x + y*N.y + z*N.z) 81 | Bcm.set_vel(N, u*B.x + v*B.y + w*B.z) 82 | 83 | # Motor placement 84 | # M1 is front left, then clockwise numbering 85 | # dzm is positive for motors above center of mass 86 | # --------------------------- 87 | M1.set_pos(Bcm, dxm*B.x + dym*B.y + dzm*B.z) 88 | M2.set_pos(Bcm, dxm*B.x - dym*B.y + dzm*B.z) 89 | M3.set_pos(Bcm, -dxm*B.x - dym*B.y + dzm*B.z) 90 | M4.set_pos(Bcm, -dxm*B.x + dym*B.y + dzm*B.z) 91 | M1.v2pt_theory(Bcm, N, B) 92 | M2.v2pt_theory(Bcm, N, B) 93 | M3.v2pt_theory(Bcm, N, B) 94 | M4.v2pt_theory(Bcm, N, B) 95 | 96 | # Inertia Dyadic 97 | # --------------------------- 98 | IB = inertia(B, IBxx, IByy, IBzz) 99 | 100 | # Create Bodies 101 | # --------------------------- 102 | BodyB = RigidBody('BodyB', Bcm, B, mB, (IB, Bcm)) 103 | BodyList = [BodyB] 104 | 105 | # Forces and Torques 106 | # --------------------------- 107 | Grav_Force = (Bcm, -mB*g*N.z) 108 | FM1 = (M1, ThrM1*B.z) 109 | FM2 = (M2, ThrM2*B.z) 110 | FM3 = (M3, ThrM3*B.z) 111 | FM4 = (M4, ThrM4*B.z) 112 | 113 | TM1 = (B, TorM1*B.z) 114 | TM2 = (B, -TorM2*B.z) 115 | TM3 = (B, TorM3*B.z) 116 | TM4 = (B, -TorM4*B.z) 117 | ForceList = [Grav_Force, FM1, FM2, FM3, FM4, TM1, TM2, TM3, TM4] 118 | 119 | # Kinematic Differential Equations 120 | # --------------------------- 121 | kd = [xd - dot(Bcm.vel(N), N.x), yd - dot(Bcm.vel(N), N.y), zd - dot(Bcm.vel(N), N.z), 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)] 122 | 123 | # Kane's Method 124 | # --------------------------- 125 | KM = KanesMethod(N, q_ind=[x, y, z, phi, theta, psi], u_ind=[u, v, w, p, q, r], kd_eqs=kd) 126 | (fr, frstar) = KM.kanes_equations(BodyList, ForceList) 127 | 128 | # Equations of Motion 129 | # --------------------------- 130 | MM = KM.mass_matrix_full 131 | kdd = KM.kindiffdict() 132 | rhs = KM.forcing_full 133 | rhs = rhs.subs(kdd) 134 | 135 | MM.simplify() 136 | print('Mass Matrix') 137 | print('-----------') 138 | mprint(MM) 139 | print() 140 | 141 | rhs.simplify() 142 | print('Right Hand Side') 143 | print('---------------') 144 | mprint(rhs) 145 | print() 146 | 147 | # So, MM*x = rhs, where x is the State Derivatives 148 | # Solve for x 149 | stateDot = MM.inv()*rhs 150 | print('State Derivatives') 151 | print('-----------------------------------') 152 | mprint(stateDot) 153 | print() 154 | 155 | # POST-PROCESSING 156 | # --------------------------- 157 | 158 | # Useful Numbers 159 | # --------------------------- 160 | # p, q, r are the drone's angular velocities in the inertial frame, expressed in the drone's frame. 161 | # These calculations are not relevant to the ODE, but might be used for control. 162 | print('P, Q, R (Angular velocities in drone frame)') 163 | print('-------------------------------------------') 164 | mprint(dot(B.ang_vel_in(N), B.x)) 165 | print() 166 | mprint(dot(B.ang_vel_in(N), B.y)) 167 | print() 168 | mprint(dot(B.ang_vel_in(N), B.z)) 169 | print() 170 | 171 | # xdot, ydot, zdot are the drone's velocities in the inertial frame, expressed in the inertial frame. 172 | # These calculations are not relevant to the ODE, but might be used for control 173 | xdot = dot(Bcm.vel(N).subs(kdd), N.x) 174 | ydot = dot(Bcm.vel(N).subs(kdd), N.y) 175 | zdot = dot(Bcm.vel(N).subs(kdd), N.z) 176 | print('xdot, ydot, zdot (Velocities in inertial frame)') 177 | print('-----------------------------------') 178 | mprint(xdot) 179 | print() 180 | mprint(ydot) 181 | print() 182 | mprint(zdot) 183 | print() 184 | 185 | # uFlat, vFlat, wFlat are the drone's velocities in the inertial frame, expressed in a frame 186 | # parallel to the ground, but with the drone's heading (so only after the YAW rotation). 187 | # These calculations are not relevant to the ODE, but might be used for control. 188 | uFlat = dot(Bcm.vel(N).subs(kdd), D.x) 189 | vFlat = dot(Bcm.vel(N).subs(kdd), D.y) 190 | wFlat = dot(Bcm.vel(N).subs(kdd), D.z) 191 | print('uFlat, vFlat, wFlat (Velocities in drone frame (after Yaw))') 192 | print('-----------------------------------------------------------') 193 | mprint(uFlat) 194 | print() 195 | mprint(vFlat) 196 | print() 197 | mprint(wFlat) 198 | print() 199 | -------------------------------------------------------------------------------- /PyDy Scripts/00 - Basic/Quad_3D_uvw/Quad_3D_flu_ENU_Quat_uvw.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | author: John Bass 4 | email: john.bobzwik@gmail.com 5 | license: MIT 6 | Please feel free to use and modify this, but keep the above information. Thanks! 7 | """ 8 | 9 | """ 10 | Using PyDy and Sympy, this script generates the equations for the state derivatives 11 | of a quadcopter in a 3-dimensional space. The states in this particular script are: 12 | 13 | x, y, z : Position of the drone's center of mass in the inertial frame, 14 | expressed in the inertial frame 15 | u, v, w : Velocity of the drone's center of mass in the inertial frame, 16 | expressed in the drone's frame 17 | q0, q1, q2, q3 : Orientation of the drone in the inertial frame using quaternions 18 | p, q, r : Angular velocity of the drone in the inertial frame, 19 | expressed in the drone's frame 20 | 21 | Important note : This script uses a flu body orientation (front-left-up) and 22 | a ENU world orientation (East-North-Up) 23 | 24 | Other note : In the resulting state derivatives, there are still simplifications 25 | that can be made that SymPy cannot simplify (factoring). 26 | 27 | author: John Bass 28 | email: 29 | """ 30 | 31 | from sympy import symbols, Matrix 32 | from sympy.physics.mechanics import * 33 | 34 | # Reference frames and Points 35 | # --------------------------- 36 | N = ReferenceFrame('N') # Inertial Frame 37 | B = ReferenceFrame('B') # Drone after X (roll) rotation (Final rotation) 38 | 39 | No = Point('No') 40 | Bcm = Point('Bcm') # Drone's center of mass 41 | M1 = Point('M1') # Motor 1 is front left, then the rest increments CW (Drone is in X configuration, not +) 42 | M2 = Point('M2') 43 | M3 = Point('M3') 44 | M4 = Point('M4') 45 | 46 | # Variables 47 | # --------------------------- 48 | # x, y and z are the drone's coordinates in the inertial frame, expressed with the inertial frame 49 | # u, v and w are the drone's velocities in the inertial frame, expressed with the drone's frame 50 | # q0, q1, q2, q3 represents the drone's orientation in the inertial frame, using quaternions 51 | # p, q and r are the drone's angular velocities in the inertial frame, expressed with the drone's frame 52 | 53 | x, y, z, u, v, w = dynamicsymbols('x y z u v w') 54 | q0, q1, q2, q3, p, q, r = dynamicsymbols('q0 q1 q2 q3 p q r') 55 | 56 | # First derivatives of the variables 57 | xd, yd, zd, ud, vd, wd = dynamicsymbols('x y z u v w', 1) 58 | q0d, q1d, q2d, q3d, pd, qd, rd = dynamicsymbols('q0 q1 q2 q3 p q r', 1) 59 | 60 | # Constants 61 | # --------------------------- 62 | mB, g, dxm, dym, dzm, IBxx, IByy, IBzz = symbols('mB g dxm dym dzm IBxx IByy IBzz') 63 | ThrM1, ThrM2, ThrM3, ThrM4, TorM1, TorM2, TorM3, TorM4 = symbols('ThrM1 ThrM2 ThrM3 ThrM4 TorM1 TorM2 TorM3 TorM4') 64 | 65 | # Rotation Quaternion 66 | # --------------------------- 67 | B.orient(N, 'Quaternion', [q0, q1, q2, q3]) 68 | B.set_ang_vel(N, p*B.x + q*B.y + r*B.z) 69 | 70 | # Origin 71 | # --------------------------- 72 | No.set_vel(N, 0) 73 | 74 | # Translation 75 | # --------------------------- 76 | Bcm.set_pos(No, x*N.x + y*N.y + z*N.z) 77 | Bcm.set_vel(N, u*B.x + v*B.y + w*B.z) 78 | 79 | # Motor placement 80 | # M1 is front left, then clockwise numbering 81 | # dzm is positive for motors above center of mass 82 | # --------------------------- 83 | M1.set_pos(Bcm, dxm*B.x + dym*B.y + dzm*B.z) 84 | M2.set_pos(Bcm, dxm*B.x - dym*B.y + dzm*B.z) 85 | M3.set_pos(Bcm, -dxm*B.x - dym*B.y + dzm*B.z) 86 | M4.set_pos(Bcm, -dxm*B.x + dym*B.y + dzm*B.z) 87 | M1.v2pt_theory(Bcm, N, B) 88 | M2.v2pt_theory(Bcm, N, B) 89 | M3.v2pt_theory(Bcm, N, B) 90 | M4.v2pt_theory(Bcm, N, B) 91 | 92 | # Inertia Dyadic 93 | # --------------------------- 94 | IB = inertia(B, IBxx, IByy, IBzz) 95 | 96 | # Create Bodies 97 | # --------------------------- 98 | BodyB = RigidBody('BodyB', Bcm, B, mB, (IB, Bcm)) 99 | BodyList = [BodyB] 100 | 101 | # Forces and Torques 102 | # --------------------------- 103 | Grav_Force = (Bcm, -mB*g*N.z) 104 | FM1 = (M1, ThrM1*B.z) 105 | FM2 = (M2, ThrM2*B.z) 106 | FM3 = (M3, ThrM3*B.z) 107 | FM4 = (M4, ThrM4*B.z) 108 | 109 | TM1 = (B, TorM1*B.z) 110 | TM2 = (B, -TorM2*B.z) 111 | TM3 = (B, TorM3*B.z) 112 | TM4 = (B, -TorM4*B.z) 113 | ForceList = [Grav_Force, FM1, FM2, FM3, FM4, TM1, TM2, TM3, TM4] 114 | 115 | # Calculate Quaternion Derivative 116 | # --------------------------- 117 | Gquat = Matrix([[-q1, q0, q3, -q2], 118 | [-q2, -q3, q0, q1], 119 | [-q3, q2, -q1, q0]]) 120 | 121 | angVel = Matrix([[dot(B.ang_vel_in(N), B.x)],[dot(B.ang_vel_in(N), B.y)],[dot(B.ang_vel_in(N), B.z)]]) # Basically the same as: angVel = Matrix([[p],[q],[r]]) 122 | quat_dot = 1.0/2*Gquat.T*angVel 123 | 124 | # Kinematic Differential Equations 125 | # --------------------------- 126 | kd = [xd - dot(Bcm.vel(N), N.x), yd - dot(Bcm.vel(N), N.y), zd - dot(Bcm.vel(N), N.z), q0d - quat_dot[0], q1d - quat_dot[1], q2d - quat_dot[2], q3d - quat_dot[3]] 127 | 128 | # Kane's Method 129 | # --------------------------- 130 | KM = KanesMethod(N, q_ind=[x, y, z, q0, q1, q2, q3], u_ind=[u, v, w, p, q, r], kd_eqs=kd) 131 | (fr, frstar) = KM.kanes_equations(BodyList, ForceList) 132 | 133 | # Equations of Motion 134 | # --------------------------- 135 | MM = KM.mass_matrix_full 136 | kdd = KM.kindiffdict() 137 | rhs = KM.forcing_full 138 | MM = MM.subs(kdd) 139 | rhs = rhs.subs(kdd) 140 | 141 | MM.simplify() 142 | # MM = MM.subs(q0**2 + q1**2 + q2**2 + q3**2, 1) 143 | print() 144 | print('Mass Matrix') 145 | print('-----------') 146 | mprint(MM) 147 | 148 | rhs.simplify() 149 | # rhs = rhs.subs(q0**2 + q1**2 + q2**2 + q3**2, 1) 150 | print('Right Hand Side') 151 | print('---------------') 152 | mprint(rhs) 153 | print() 154 | 155 | # So, MM*x = rhs, where x is the State Derivatives 156 | # Solve for x 157 | stateDot = MM.inv()*rhs 158 | print('State Derivatives') 159 | print('-----------------------------------') 160 | mprint(stateDot) 161 | print() 162 | 163 | # POST-PROCESSING 164 | # --------------------------- 165 | 166 | # Useful Numbers 167 | # --------------------------- 168 | # p, q, r are the drone's angular velocities in the inertial frame, expressed in the drone's frame. 169 | # These calculations are not relevant to the ODE, but might be used for control. 170 | quat_dot_2 = Matrix([[q0d],[q1d],[q2d],[q3d]]) 171 | angVel_2 = 2.0*Gquat*quat_dot_2 172 | print('P, Q, R (Angular velocities in drone frame)') 173 | print('-------------------------------------------') 174 | mprint(angVel_2[0]) 175 | print() 176 | mprint(angVel_2[1]) 177 | print() 178 | mprint(angVel_2[2]) 179 | print() 180 | 181 | # xdot, ydot, zdot are the drone's velocities in the inertial frame, expressed in the inertial frame. 182 | # These calculations are not relevant to the ODE, but might be used for control 183 | xdot = dot(Bcm.vel(N).subs(kdd), N.x).simplify() 184 | ydot = dot(Bcm.vel(N).subs(kdd), N.y).simplify() 185 | zdot = dot(Bcm.vel(N).subs(kdd), N.z).simplify() 186 | print('xdot, ydot, zdot (Velocities in inertial frame)') 187 | print('-----------------------------------') 188 | mprint(xdot) 189 | print() 190 | mprint(ydot) 191 | print() 192 | mprint(zdot) 193 | print() 194 | 195 | # uFlat, vFlat, wFlat are the drone's velocities in the inertial frame, expressed in a frame 196 | # parallel to the ground, but with the drone's heading. 197 | # These calculations are not relevant to the ODE, but might be used for control. 198 | uFlat = dot(Bcm.vel(N).subs(kdd), cross(B.y, N.z)).simplify() 199 | vFlat = dot(Bcm.vel(N).subs(kdd), -cross(B.x, N.z)).simplify() 200 | wFlat = dot(Bcm.vel(N).subs(kdd), N.z).simplify() 201 | print('uFlat, vFlat, wFlat (Velocities in drone frame (after Yaw))') 202 | print('-----------------------------------------------------------') 203 | mprint(uFlat) 204 | print() 205 | mprint(vFlat) 206 | print() 207 | mprint(wFlat) 208 | print() 209 | -------------------------------------------------------------------------------- /PyDy Scripts/00 - Basic/Quad_3D_uvw/Quad_3D_frd_NED_Euler_uvw.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | author: John Bass 4 | email: john.bobzwik@gmail.com 5 | license: MIT 6 | Please feel free to use and modify this, but keep the above information. Thanks! 7 | """ 8 | 9 | """ 10 | Using PyDy and Sympy, this script generates the equations for the state derivatives 11 | of a quadcopter in a 3-dimensional space. The states in this particular script are: 12 | 13 | x, y, z : Position of the drone's center of mass in the inertial frame, 14 | expressed in the inertial frame 15 | u, v, w : Velocity of the drone's center of mass in the inertial frame, 16 | expressed in the drone's frame 17 | phi, theta, psi : Orientation (roll, pitch, yaw angles) of the drone in the 18 | inertial frame, following the order ZYX (yaw, pitch, roll) 19 | p, q, r : Angular velocity of the drone in the inertial frame, 20 | expressed in the drone's frame 21 | 22 | Important note : This script uses a frd body orientation (front-right-down) and 23 | a NED world orientation (North-East-Down). The drone's altitude is -z. 24 | 25 | Other note : In the resulting state derivatives, there are still simplifications 26 | that can be made that SymPy cannot simplify (factoring). 27 | 28 | author: John Bass 29 | email: 30 | """ 31 | 32 | from sympy import symbols 33 | from sympy.physics.mechanics import * 34 | 35 | # Reference frames and Points 36 | # --------------------------- 37 | N = ReferenceFrame('N') # Inertial Frame 38 | B = ReferenceFrame('B') # Drone after X (roll) rotation (Final rotation) 39 | C = ReferenceFrame('C') # Drone after Y (pitch) rotation 40 | D = ReferenceFrame('D') # Drone after Z (yaw) rotation (First rotation) 41 | 42 | No = Point('No') 43 | Bcm = Point('Bcm') # Drone's center of mass 44 | M1 = Point('M1') # Motor 1 is front left, then the rest increments CW (Drone is in X configuration, not +) 45 | M2 = Point('M2') 46 | M3 = Point('M3') 47 | M4 = Point('M4') 48 | 49 | # Variables 50 | # --------------------------- 51 | # x, y and z are the drone's coordinates in the inertial frame, expressed with the inertial frame 52 | # u, v and w are the drone's velocities in the inertial frame, expressed with the drone's frame 53 | # phi, theta and psi represents the drone's orientation in the inertial frame, expressed with a ZYX Body rotation 54 | # p, q and r are the drone's angular velocities in the inertial frame, expressed with the drone's frame 55 | 56 | x, y, z, u, v, w = dynamicsymbols('x y z u v w') 57 | phi, theta, psi, p, q, r = dynamicsymbols('phi theta psi p q r') 58 | 59 | # First derivatives of the variables 60 | xd, yd, zd, ud, vd, wd = dynamicsymbols('x y z u v w', 1) 61 | phid, thetad, psid, pd, qd, rd = dynamicsymbols('phi theta psi p q r', 1) 62 | 63 | # Constants 64 | # --------------------------- 65 | mB, g, dxm, dym, dzm, IBxx, IByy, IBzz = symbols('mB g dxm dym dzm IBxx IByy IBzz') 66 | ThrM1, ThrM2, ThrM3, ThrM4, TorM1, TorM2, TorM3, TorM4 = symbols('ThrM1 ThrM2 ThrM3 ThrM4 TorM1 TorM2 TorM3 TorM4') 67 | 68 | # Rotation ZYX Body 69 | # --------------------------- 70 | D.orient(N, 'Axis', [psi, N.z]) 71 | C.orient(D, 'Axis', [theta, D.y]) 72 | B.orient(C, 'Axis', [phi, C.x]) 73 | 74 | # Origin 75 | # --------------------------- 76 | No.set_vel(N, 0) 77 | 78 | # Translation 79 | # --------------------------- 80 | Bcm.set_pos(No, x*N.x + y*N.y + z*N.z) 81 | Bcm.set_vel(N, u*B.x + v*B.y + w*B.z) 82 | 83 | # Motor placement 84 | # M1 is front left, then clockwise numbering 85 | # dzm is positive for motors above center of mass 86 | # --------------------------- 87 | M1.set_pos(Bcm, dxm*B.x - dym*B.y - dzm*B.z) 88 | M2.set_pos(Bcm, dxm*B.x + dym*B.y - dzm*B.z) 89 | M3.set_pos(Bcm, -dxm*B.x + dym*B.y - dzm*B.z) 90 | M4.set_pos(Bcm, -dxm*B.x - dym*B.y - dzm*B.z) 91 | M1.v2pt_theory(Bcm, N, B) 92 | M2.v2pt_theory(Bcm, N, B) 93 | M3.v2pt_theory(Bcm, N, B) 94 | M4.v2pt_theory(Bcm, N, B) 95 | 96 | # Inertia Dyadic 97 | # --------------------------- 98 | IB = inertia(B, IBxx, IByy, IBzz) 99 | 100 | # Create Bodies 101 | # --------------------------- 102 | BodyB = RigidBody('BodyB', Bcm, B, mB, (IB, Bcm)) 103 | BodyList = [BodyB] 104 | 105 | # Forces and Torques 106 | # --------------------------- 107 | Grav_Force = (Bcm, mB*g*N.z) 108 | FM1 = (M1, -ThrM1*B.z) 109 | FM2 = (M2, -ThrM2*B.z) 110 | FM3 = (M3, -ThrM3*B.z) 111 | FM4 = (M4, -ThrM4*B.z) 112 | 113 | TM1 = (B, -TorM1*B.z) 114 | TM2 = (B, TorM2*B.z) 115 | TM3 = (B, -TorM3*B.z) 116 | TM4 = (B, TorM4*B.z) 117 | ForceList = [Grav_Force, FM1, FM2, FM3, FM4, TM1, TM2, TM3, TM4] 118 | 119 | # Kinematic Differential Equations 120 | # --------------------------- 121 | kd = [xd - dot(Bcm.vel(N), N.x), yd - dot(Bcm.vel(N), N.y), zd - dot(Bcm.vel(N), N.z), 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)] 122 | 123 | # Kane's Method 124 | # --------------------------- 125 | KM = KanesMethod(N, q_ind=[x, y, z, phi, theta, psi], u_ind=[u, v, w, p, q, r], kd_eqs=kd) 126 | (fr, frstar) = KM.kanes_equations(BodyList, ForceList) 127 | 128 | # Equations of Motion 129 | # --------------------------- 130 | MM = KM.mass_matrix_full 131 | kdd = KM.kindiffdict() 132 | rhs = KM.forcing_full 133 | rhs = rhs.subs(kdd) 134 | 135 | MM.simplify() 136 | print('Mass Matrix') 137 | print('-----------') 138 | mprint(MM) 139 | print() 140 | 141 | rhs.simplify() 142 | print('Right Hand Side') 143 | print('---------------') 144 | mprint(rhs) 145 | print() 146 | 147 | # So, MM*x = rhs, where x is the State Derivatives 148 | # Solve for x 149 | stateDot = MM.inv()*rhs 150 | print('State Derivatives') 151 | print('-----------------------------------') 152 | mprint(stateDot) 153 | print() 154 | 155 | # POST-PROCESSING 156 | # --------------------------- 157 | 158 | # Useful Numbers 159 | # --------------------------- 160 | # p, q, r are the drone's angular velocities in the inertial frame, expressed in the drone's frame. 161 | # These calculations are not relevant to the ODE, but might be used for control. 162 | print('P, Q, R (Angular velocities in drone frame)') 163 | print('-------------------------------------------') 164 | mprint(dot(B.ang_vel_in(N), B.x)) 165 | print() 166 | mprint(dot(B.ang_vel_in(N), B.y)) 167 | print() 168 | mprint(dot(B.ang_vel_in(N), B.z)) 169 | print() 170 | 171 | # xdot, ydot, zdot are the drone's velocities in the inertial frame, expressed in the inertial frame. 172 | # These calculations are not relevant to the ODE, but might be used for control 173 | xdot = dot(Bcm.vel(N).subs(kdd), N.x) 174 | ydot = dot(Bcm.vel(N).subs(kdd), N.y) 175 | zdot = dot(Bcm.vel(N).subs(kdd), N.z) 176 | print('xdot, ydot, zdot (Velocities in inertial frame)') 177 | print('-----------------------------------') 178 | mprint(xdot) 179 | print() 180 | mprint(ydot) 181 | print() 182 | mprint(zdot) 183 | print() 184 | 185 | # uFlat, vFlat, wFlat are the drone's velocities in the inertial frame, expressed in a frame 186 | # parallel to the ground, but with the drone's heading (so only after the YAW rotation). 187 | # These calculations are not relevant to the ODE, but might be used for control. 188 | uFlat = dot(Bcm.vel(N).subs(kdd), D.x) 189 | vFlat = dot(Bcm.vel(N).subs(kdd), D.y) 190 | wFlat = dot(Bcm.vel(N).subs(kdd), D.z) 191 | print('uFlat, vFlat, wFlat (Velocities in drone frame (after Yaw))') 192 | print('-----------------------------------------------------------') 193 | mprint(uFlat) 194 | print() 195 | mprint(vFlat) 196 | print() 197 | mprint(wFlat) 198 | print() 199 | -------------------------------------------------------------------------------- /PyDy Scripts/00 - Basic/Quad_3D_uvw/Quad_3D_frd_NED_Quat_uvw.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | author: John Bass 4 | email: john.bobzwik@gmail.com 5 | license: MIT 6 | Please feel free to use and modify this, but keep the above information. Thanks! 7 | """ 8 | 9 | """ 10 | Using PyDy and Sympy, this script generates the equations for the state derivatives 11 | of a quadcopter in a 3-dimensional space. The states in this particular script are: 12 | 13 | x, y, z : Position of the drone's center of mass in the inertial frame, 14 | expressed in the inertial frame 15 | u, v, w : Velocity of the drone's center of mass in the inertial frame, 16 | expressed in the drone's frame 17 | q0, q1, q2, q3 : Orientation of the drone in the inertial frame using quaternions 18 | p, q, r : Angular velocity of the drone in the inertial frame, 19 | expressed in the drone's frame 20 | 21 | Important note : This script uses a frd body orientation (front-right-down) and 22 | a NED world orientation (North-East-Down). The drone's altitude is -z. 23 | 24 | Other note : In the resulting state derivatives, there are still simplifications 25 | that can be made that SymPy cannot simplify (factoring). 26 | 27 | author: John Bass 28 | email: 29 | """ 30 | 31 | from sympy import symbols, Matrix 32 | from sympy.physics.mechanics import * 33 | 34 | # Reference frames and Points 35 | # --------------------------- 36 | N = ReferenceFrame('N') # Inertial Frame 37 | B = ReferenceFrame('B') # Drone after X (roll) rotation (Final rotation) 38 | 39 | No = Point('No') 40 | Bcm = Point('Bcm') # Drone's center of mass 41 | M1 = Point('M1') # Motor 1 is front left, then the rest increments CW (Drone is in X configuration, not +) 42 | M2 = Point('M2') 43 | M3 = Point('M3') 44 | M4 = Point('M4') 45 | 46 | # Variables 47 | # --------------------------- 48 | # x, y and z are the drone's coordinates in the inertial frame, expressed with the inertial frame 49 | # u, v and w are the drone's velocities in the inertial frame, expressed with the drone's frame 50 | # q0, q1, q2, q3 represents the drone's orientation in the inertial frame, using quaternions 51 | # p, q and r are the drone's angular velocities in the inertial frame, expressed with the drone's frame 52 | 53 | x, y, z, u, v, w = dynamicsymbols('x y z u v w') 54 | q0, q1, q2, q3, p, q, r = dynamicsymbols('q0 q1 q2 q3 p q r') 55 | 56 | # First derivatives of the variables 57 | xd, yd, zd, ud, vd, wd = dynamicsymbols('x y z u v w', 1) 58 | q0d, q1d, q2d, q3d, pd, qd, rd = dynamicsymbols('q0 q1 q2 q3 p q r', 1) 59 | 60 | # Constants 61 | # --------------------------- 62 | mB, g, dxm, dym, dzm, IBxx, IByy, IBzz = symbols('mB g dxm dym dzm IBxx IByy IBzz') 63 | ThrM1, ThrM2, ThrM3, ThrM4, TorM1, TorM2, TorM3, TorM4 = symbols('ThrM1 ThrM2 ThrM3 ThrM4 TorM1 TorM2 TorM3 TorM4') 64 | 65 | # Rotation Quaternion 66 | # --------------------------- 67 | B.orient(N, 'Quaternion', [q0, q1, q2, q3]) 68 | B.set_ang_vel(N, p*B.x + q*B.y + r*B.z) 69 | 70 | # Origin 71 | # --------------------------- 72 | No.set_vel(N, 0) 73 | 74 | # Translation 75 | # --------------------------- 76 | Bcm.set_pos(No, x*N.x + y*N.y + z*N.z) 77 | Bcm.set_vel(N, u*B.x + v*B.y + w*B.z) 78 | 79 | # Motor placement 80 | # M1 is front left, then clockwise numbering 81 | # dzm is positive for motors above center of mass 82 | # --------------------------- 83 | M1.set_pos(Bcm, dxm*B.x - dym*B.y - dzm*B.z) 84 | M2.set_pos(Bcm, dxm*B.x + dym*B.y - dzm*B.z) 85 | M3.set_pos(Bcm, -dxm*B.x + dym*B.y - dzm*B.z) 86 | M4.set_pos(Bcm, -dxm*B.x - dym*B.y - dzm*B.z) 87 | M1.v2pt_theory(Bcm, N, B) 88 | M2.v2pt_theory(Bcm, N, B) 89 | M3.v2pt_theory(Bcm, N, B) 90 | M4.v2pt_theory(Bcm, N, B) 91 | 92 | # Inertia Dyadic 93 | # --------------------------- 94 | IB = inertia(B, IBxx, IByy, IBzz) 95 | 96 | # Create Bodies 97 | # --------------------------- 98 | BodyB = RigidBody('BodyB', Bcm, B, mB, (IB, Bcm)) 99 | BodyList = [BodyB] 100 | 101 | # Forces and Torques 102 | # --------------------------- 103 | Grav_Force = (Bcm, mB*g*N.z) 104 | FM1 = (M1, -ThrM1*B.z) 105 | FM2 = (M2, -ThrM2*B.z) 106 | FM3 = (M3, -ThrM3*B.z) 107 | FM4 = (M4, -ThrM4*B.z) 108 | 109 | TM1 = (B, -TorM1*B.z) 110 | TM2 = (B, TorM2*B.z) 111 | TM3 = (B, -TorM3*B.z) 112 | TM4 = (B, TorM4*B.z) 113 | ForceList = [Grav_Force, FM1, FM2, FM3, FM4, TM1, TM2, TM3, TM4] 114 | 115 | # Calculate Quaternion Derivative 116 | # --------------------------- 117 | Gquat = Matrix([[-q1, q0, q3, -q2], 118 | [-q2, -q3, q0, q1], 119 | [-q3, q2, -q1, q0]]) 120 | 121 | angVel = Matrix([[dot(B.ang_vel_in(N), B.x)],[dot(B.ang_vel_in(N), B.y)],[dot(B.ang_vel_in(N), B.z)]]) # Basically the same as: angVel = Matrix([[p],[q],[r]]) 122 | quat_dot = 1.0/2*Gquat.T*angVel 123 | 124 | # Kinematic Differential Equations 125 | # --------------------------- 126 | kd = [xd - dot(Bcm.vel(N), N.x), yd - dot(Bcm.vel(N), N.y), zd - dot(Bcm.vel(N), N.z), q0d - quat_dot[0], q1d - quat_dot[1], q2d - quat_dot[2], q3d - quat_dot[3]] 127 | 128 | # Kane's Method 129 | # --------------------------- 130 | KM = KanesMethod(N, q_ind=[x, y, z, q0, q1, q2, q3], u_ind=[u, v, w, p, q, r], kd_eqs=kd) 131 | (fr, frstar) = KM.kanes_equations(BodyList, ForceList) 132 | 133 | # Equations of Motion 134 | # --------------------------- 135 | MM = KM.mass_matrix_full 136 | kdd = KM.kindiffdict() 137 | rhs = KM.forcing_full 138 | MM = MM.subs(kdd) 139 | rhs = rhs.subs(kdd) 140 | 141 | MM.simplify() 142 | # MM = MM.subs(q0**2 + q1**2 + q2**2 + q3**2, 1) 143 | print() 144 | print('Mass Matrix') 145 | print('-----------') 146 | mprint(MM) 147 | 148 | rhs.simplify() 149 | # rhs = rhs.subs(q0**2 + q1**2 + q2**2 + q3**2, 1) 150 | print('Right Hand Side') 151 | print('---------------') 152 | mprint(rhs) 153 | print() 154 | 155 | # So, MM*x = rhs, where x is the State Derivatives 156 | # Solve for x 157 | stateDot = MM.inv()*rhs 158 | print('State Derivatives') 159 | print('-----------------------------------') 160 | mprint(stateDot) 161 | print() 162 | 163 | # POST-PROCESSING 164 | # --------------------------- 165 | 166 | # Useful Numbers 167 | # --------------------------- 168 | # p, q, r are the drone's angular velocities in the inertial frame, expressed in the drone's frame. 169 | # These calculations are not relevant to the ODE, but might be used for control. 170 | quat_dot_2 = Matrix([[q0d],[q1d],[q2d],[q3d]]) 171 | angVel_2 = 2.0*Gquat*quat_dot_2 172 | print('P, Q, R (Angular velocities in drone frame)') 173 | print('-------------------------------------------') 174 | mprint(angVel_2[0]) 175 | print() 176 | mprint(angVel_2[1]) 177 | print() 178 | mprint(angVel_2[2]) 179 | print() 180 | 181 | # xdot, ydot, zdot are the drone's velocities in the inertial frame, expressed in the inertial frame. 182 | # These calculations are not relevant to the ODE, but might be used for control 183 | xdot = dot(Bcm.vel(N).subs(kdd), N.x).simplify() 184 | ydot = dot(Bcm.vel(N).subs(kdd), N.y).simplify() 185 | zdot = dot(Bcm.vel(N).subs(kdd), N.z).simplify() 186 | print('xdot, ydot, zdot (Velocities in inertial frame)') 187 | print('-----------------------------------') 188 | mprint(xdot) 189 | print() 190 | mprint(ydot) 191 | print() 192 | mprint(zdot) 193 | print() 194 | 195 | # uFlat, vFlat, wFlat are the drone's velocities in the inertial frame, expressed in a frame 196 | # parallel to the ground, but with the drone's heading. 197 | # These calculations are not relevant to the ODE, but might be used for control. 198 | uFlat = dot(Bcm.vel(N).subs(kdd), cross(B.y, N.z)).simplify() 199 | vFlat = dot(Bcm.vel(N).subs(kdd), -cross(B.x, N.z)).simplify() 200 | wFlat = dot(Bcm.vel(N).subs(kdd), N.z).simplify() 201 | print('uFlat, vFlat, wFlat (Velocities in drone frame (after Yaw))') 202 | print('-----------------------------------------------------------') 203 | mprint(uFlat) 204 | print() 205 | mprint(vFlat) 206 | print() 207 | mprint(wFlat) 208 | print() 209 | -------------------------------------------------------------------------------- /PyDy Scripts/01 - Added Gyroscopic Precession/Quad_3D_flu_ENU_Euler.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | author: John Bass 4 | email: john.bobzwik@gmail.com 5 | license: MIT 6 | Please feel free to use and modify this, but keep the above information. Thanks! 7 | """ 8 | 9 | """ 10 | Using PyDy and Sympy, this script generates the equations for the state derivatives 11 | of a quadcopter in a 3-dimensional space. The states in this particular script are: 12 | 13 | x, y, z : Position of the drone's center of mass in the inertial frame, 14 | expressed in the inertial frame 15 | xdot, ydot, zdot : Velocity of the drone's center of mass in the inertial frame, 16 | expressed in the inertial frame 17 | phi, theta, psi : Orientation (roll, pitch, yaw angles) of the drone in the 18 | inertial frame, following the order ZYX (yaw, pitch, roll) 19 | p, q, r : Angular velocity of the drone in the inertial frame, 20 | expressed in the drone's frame 21 | 22 | Important note : This script uses a flu body orientation (front-left-up) and 23 | a ENU world orientation (East-North-Up). 24 | 25 | Other note : In the resulting state derivatives, there are still simplifications 26 | that can be made that SymPy cannot simplify (factoring). 27 | 28 | author: John Bass 29 | email: 30 | """ 31 | 32 | from sympy import symbols 33 | from sympy.physics.mechanics import * 34 | 35 | # Reference frames and Points 36 | # --------------------------- 37 | N = ReferenceFrame('N') # Inertial Frame 38 | B = ReferenceFrame('B') # Drone after X (roll) rotation (Final rotation) 39 | C = ReferenceFrame('C') # Drone after Y (pitch) rotation 40 | D = ReferenceFrame('D') # Drone after Z (yaw) rotation (First rotation) 41 | 42 | No = Point('No') 43 | Bcm = Point('Bcm') # Drone's center of mass 44 | M1 = Point('M1') # Motor 1 is front left, then the rest increments CW (Drone is in X configuration, not +) 45 | M2 = Point('M2') 46 | M3 = Point('M3') 47 | M4 = Point('M4') 48 | 49 | # Variables 50 | # --------------------------- 51 | # x, y and z are the drone's coordinates in the inertial frame, expressed with the inertial frame 52 | # xdot, ydot and zdot are the drone's velocities in the inertial frame, expressed with the inertial frame 53 | # phi, theta and psi represents the drone's orientation in the inertial frame, expressed with a ZYX Body rotation 54 | # p, q and r are the drone's angular velocities in the inertial frame, expressed with the drone's frame 55 | 56 | x, y, z, xdot, ydot, zdot = dynamicsymbols('x y z xdot ydot zdot') 57 | phi, theta, psi, p, q, r = dynamicsymbols('phi theta psi p q r') 58 | 59 | # First derivatives of the variables 60 | xd, yd, zd, xdotd, ydotd, zdotd = dynamicsymbols('x y z xdot ydot zdot', 1) 61 | phid, thetad, psid, pd, qd, rd = dynamicsymbols('phi theta psi p q r', 1) 62 | 63 | # Constants 64 | # --------------------------- 65 | mB, g, dxm, dym, dzm, IBxx, IByy, IBzz, IRzz, wM1, wM2, wM3, wM4 = symbols('mB g dxm dym dzm IBxx IByy IBzz IRzz wM1 wM2 wM3 wM4') 66 | ThrM1, ThrM2, ThrM3, ThrM4, TorM1, TorM2, TorM3, TorM4 = symbols('ThrM1 ThrM2 ThrM3 ThrM4 TorM1 TorM2 TorM3 TorM4') 67 | 68 | # Rotation ZYX Body 69 | # --------------------------- 70 | D.orient(N, 'Axis', [psi, N.z]) 71 | C.orient(D, 'Axis', [theta, D.y]) 72 | B.orient(C, 'Axis', [phi, C.x]) 73 | 74 | # Origin 75 | # --------------------------- 76 | No.set_vel(N, 0) 77 | 78 | # Translation 79 | # --------------------------- 80 | Bcm.set_pos(No, x*N.x + y*N.y + z*N.z) 81 | Bcm.set_vel(N, Bcm.pos_from(No).dt(N)) 82 | 83 | # Motor placement 84 | # M1 is front left, then clockwise numbering 85 | # dzm is positive for motors above center of mass 86 | # --------------------------- 87 | M1.set_pos(Bcm, dxm*B.x + dym*B.y + dzm*B.z) 88 | M2.set_pos(Bcm, dxm*B.x - dym*B.y + dzm*B.z) 89 | M3.set_pos(Bcm, -dxm*B.x - dym*B.y + dzm*B.z) 90 | M4.set_pos(Bcm, -dxm*B.x + dym*B.y + dzm*B.z) 91 | M1.v2pt_theory(Bcm, N, B) 92 | M2.v2pt_theory(Bcm, N, B) 93 | M3.v2pt_theory(Bcm, N, B) 94 | M4.v2pt_theory(Bcm, N, B) 95 | 96 | # Inertia Dyadic 97 | # --------------------------- 98 | IB = inertia(B, IBxx, IByy, IBzz) 99 | 100 | # Create Bodies 101 | # --------------------------- 102 | BodyB = RigidBody('BodyB', Bcm, B, mB, (IB, Bcm)) 103 | BodyList = [BodyB] 104 | 105 | # Forces and Torques 106 | # --------------------------- 107 | Grav_Force = (Bcm, -mB*g*N.z) 108 | FM1 = (M1, ThrM1*B.z) 109 | FM2 = (M2, ThrM2*B.z) 110 | FM3 = (M3, ThrM3*B.z) 111 | FM4 = (M4, ThrM4*B.z) 112 | 113 | TM1 = (B, TorM1*B.z) 114 | TM2 = (B, -TorM2*B.z) 115 | TM3 = (B, TorM3*B.z) 116 | TM4 = (B, -TorM4*B.z) 117 | 118 | gyro = (B, -IRzz*cross(B.ang_vel_in(N), (-wM1 + wM2 - wM3 + wM4)*B.z)) 119 | 120 | ForceList = [Grav_Force, FM1, FM2, FM3, FM4, TM1, TM2, TM3, TM4, gyro] 121 | 122 | # Kinematic Differential Equations 123 | # --------------------------- 124 | 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)] 125 | 126 | # Kane's Method 127 | # --------------------------- 128 | KM = KanesMethod(N, q_ind=[x, y, z, phi, theta, psi], u_ind=[xdot, ydot, zdot, p, q, r], kd_eqs=kd) 129 | (fr, frstar) = KM.kanes_equations(BodyList, ForceList) 130 | 131 | # Equations of Motion 132 | # --------------------------- 133 | MM = KM.mass_matrix_full 134 | kdd = KM.kindiffdict() 135 | rhs = KM.forcing_full 136 | rhs = rhs.subs(kdd) 137 | 138 | MM.simplify() 139 | print('Mass Matrix') 140 | print('-----------') 141 | mprint(MM) 142 | print() 143 | 144 | rhs.simplify() 145 | print('Right Hand Side') 146 | print('---------------') 147 | mprint(rhs) 148 | print() 149 | 150 | # So, MM*x = rhs, where x is the State Derivatives 151 | # Solve for x 152 | stateDot = MM.inv()*rhs 153 | print('State Derivatives') 154 | print('-----------------------------------') 155 | mprint(stateDot) 156 | print() 157 | 158 | # POST-PROCESSING 159 | # --------------------------- 160 | 161 | # Useful Numbers 162 | # --------------------------- 163 | # p, q, r are the drone's angular velocities in the inertial frame, expressed in the drone's frame. 164 | # These calculations are not relevant to the ODE, but might be used for control. 165 | print('P, Q, R (Angular velocities in drone frame)') 166 | print('-------------------------------------------') 167 | mprint(dot(B.ang_vel_in(N), B.x)) 168 | print() 169 | mprint(dot(B.ang_vel_in(N), B.y)) 170 | print() 171 | mprint(dot(B.ang_vel_in(N), B.z)) 172 | print() 173 | 174 | # u, v, w are the drone's velocities in the inertial frame, expressed in the drone's frame. 175 | # These calculations are not relevant to the ODE, but might be used for control. 176 | u = dot(Bcm.vel(N).subs(kdd), B.x) 177 | v = dot(Bcm.vel(N).subs(kdd), B.y) 178 | w = dot(Bcm.vel(N).subs(kdd), B.z) 179 | print('u, v, w (Velocities in drone frame)') 180 | print('-----------------------------------') 181 | mprint(u) 182 | print() 183 | mprint(v) 184 | print() 185 | mprint(w) 186 | print() 187 | 188 | # uFlat, vFlat, wFlat are the drone's velocities in the inertial frame, expressed in a frame 189 | # parallel to the ground, but with the drone's heading (so only after the YAW rotation). 190 | # These calculations are not relevant to the ODE, but might be used for control. 191 | uFlat = dot(Bcm.vel(N).subs(kdd), D.x) 192 | vFlat = dot(Bcm.vel(N).subs(kdd), D.y) 193 | wFlat = dot(Bcm.vel(N).subs(kdd), D.z) 194 | print('uFlat, vFlat, wFlat (Velocities in drone frame (after Yaw))') 195 | print('-----------------------------------------------------------') 196 | mprint(uFlat) 197 | print() 198 | mprint(vFlat) 199 | print() 200 | mprint(wFlat) 201 | print() 202 | -------------------------------------------------------------------------------- /PyDy Scripts/01 - Added Gyroscopic Precession/Quad_3D_flu_ENU_Quat.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | author: John Bass 4 | email: john.bobzwik@gmail.com 5 | license: MIT 6 | Please feel free to use and modify this, but keep the above information. Thanks! 7 | """ 8 | 9 | """ 10 | Using PyDy and Sympy, this script generates the equations for the state derivatives 11 | of a quadcopter in a 3-dimensional space. The states in this particular script are: 12 | 13 | x, y, z : Position of the drone's center of mass in the inertial frame, 14 | expressed in the inertial frame 15 | xdot, ydot, zdot : Velocity of the drone's center of mass in the inertial frame, 16 | expressed in the inertial frame 17 | q0, q1, q2, q3 : Orientation of the drone in the inertial frame using quaternions 18 | p, q, r : Angular velocity of the drone in the inertial frame, 19 | expressed in the drone's frame 20 | 21 | Important note : This script uses a flu body orientation (front-left-up) and 22 | a ENU world orientation (East-North-Up) 23 | 24 | Other note : In the resulting state derivatives, there are still simplifications 25 | that can be made that SymPy cannot simplify (factoring). 26 | 27 | author: John Bass 28 | email: 29 | """ 30 | 31 | from sympy import symbols, Matrix 32 | from sympy.physics.mechanics import * 33 | 34 | # Reference frames and Points 35 | # --------------------------- 36 | N = ReferenceFrame('N') # Inertial Frame 37 | B = ReferenceFrame('B') # Drone after X (roll) rotation (Final rotation) 38 | 39 | No = Point('No') 40 | Bcm = Point('Bcm') # Drone's center of mass 41 | M1 = Point('M1') # Motor 1 is front left, then the rest increments CW (Drone is in X configuration, not +) 42 | M2 = Point('M2') 43 | M3 = Point('M3') 44 | M4 = Point('M4') 45 | 46 | # Variables 47 | # --------------------------- 48 | # x, y and z are the drone's coordinates in the inertial frame, expressed with the inertial frame 49 | # xdot, ydot and zdot are the drone's speeds in the inertial frame, expressed with the inertial frame 50 | # q0, q1, q2, q3 represents the drone's orientation in the inertial frame, using quaternions 51 | # p, q and r are the drone's angular velocities in the inertial frame, expressed with the drone's frame 52 | 53 | x, y, z, xdot, ydot, zdot = dynamicsymbols('x y z xdot ydot zdot') 54 | q0, q1, q2, q3, p, q, r = dynamicsymbols('q0 q1 q2 q3 p q r') 55 | 56 | # First derivatives of the variables 57 | xd, yd, zd, xdotd, ydotd, zdotd = dynamicsymbols('x y z xdot ydot zdot', 1) 58 | q0d, q1d, q2d, q3d, pd, qd, rd = dynamicsymbols('q0 q1 q2 q3 p q r', 1) 59 | 60 | # Constants 61 | # --------------------------- 62 | mB, g, dxm, dym, dzm, IBxx, IByy, IBzz, IRzz, wM1, wM2, wM3, wM4 = symbols('mB g dxm dym dzm IBxx IByy IBzz IRzz wM1 wM2 wM3 wM4') 63 | ThrM1, ThrM2, ThrM3, ThrM4, TorM1, TorM2, TorM3, TorM4 = symbols('ThrM1 ThrM2 ThrM3 ThrM4 TorM1 TorM2 TorM3 TorM4') 64 | 65 | # Rotation Quaternion 66 | # --------------------------- 67 | B.orient(N, 'Quaternion', [q0, q1, q2, q3]) 68 | B.set_ang_vel(N, p*B.x + q*B.y + r*B.z) 69 | 70 | # Origin 71 | # --------------------------- 72 | No.set_vel(N, 0) 73 | 74 | # Translation 75 | # --------------------------- 76 | Bcm.set_pos(No, x*N.x + y*N.y + z*N.z) 77 | Bcm.set_vel(N, Bcm.pos_from(No).dt(N)) 78 | 79 | # Motor placement 80 | # M1 is front left, then clockwise numbering 81 | # dzm is positive for motors above center of mass 82 | # --------------------------- 83 | M1.set_pos(Bcm, dxm*B.x + dym*B.y + dzm*B.z) 84 | M2.set_pos(Bcm, dxm*B.x - dym*B.y + dzm*B.z) 85 | M3.set_pos(Bcm, -dxm*B.x - dym*B.y + dzm*B.z) 86 | M4.set_pos(Bcm, -dxm*B.x + dym*B.y + dzm*B.z) 87 | M1.v2pt_theory(Bcm, N, B) 88 | M2.v2pt_theory(Bcm, N, B) 89 | M3.v2pt_theory(Bcm, N, B) 90 | M4.v2pt_theory(Bcm, N, B) 91 | 92 | # Inertia Dyadic 93 | # --------------------------- 94 | IB = inertia(B, IBxx, IByy, IBzz) 95 | 96 | # Create Bodies 97 | # --------------------------- 98 | BodyB = RigidBody('BodyB', Bcm, B, mB, (IB, Bcm)) 99 | BodyList = [BodyB] 100 | 101 | # Forces and Torques 102 | # --------------------------- 103 | Grav_Force = (Bcm, -mB*g*N.z) 104 | FM1 = (M1, ThrM1*B.z) 105 | FM2 = (M2, ThrM2*B.z) 106 | FM3 = (M3, ThrM3*B.z) 107 | FM4 = (M4, ThrM4*B.z) 108 | 109 | TM1 = (B, TorM1*B.z) 110 | TM2 = (B, -TorM2*B.z) 111 | TM3 = (B, TorM3*B.z) 112 | TM4 = (B, -TorM4*B.z) 113 | 114 | gyro = (B, -IRzz*cross(B.ang_vel_in(N), (-wM1 + wM2 - wM3 + wM4)*B.z)) 115 | 116 | ForceList = [Grav_Force, FM1, FM2, FM3, FM4, TM1, TM2, TM3, TM4, gyro] 117 | 118 | # Calculate Quaternion Derivative 119 | # --------------------------- 120 | Gquat = Matrix([[-q1, q0, q3, -q2], 121 | [-q2, -q3, q0, q1], 122 | [-q3, q2, -q1, q0]]) 123 | 124 | angVel = Matrix([[dot(B.ang_vel_in(N), B.x)],[dot(B.ang_vel_in(N), B.y)],[dot(B.ang_vel_in(N), B.z)]]) # Basically the same as: angVel = Matrix([[p],[q],[r]]) 125 | quat_dot = 1.0/2*Gquat.T*angVel 126 | 127 | # Kinematic Differential Equations 128 | # --------------------------- 129 | kd = [xdot - xd, ydot - yd, zdot - zd, q0d - quat_dot[0], q1d - quat_dot[1], q2d - quat_dot[2], q3d - quat_dot[3]] 130 | 131 | # Kane's Method 132 | # --------------------------- 133 | KM = KanesMethod(N, q_ind=[x, y, z, q0, q1, q2, q3], u_ind=[xdot, ydot, zdot, p, q, r], kd_eqs=kd) 134 | (fr, frstar) = KM.kanes_equations(BodyList, ForceList) 135 | 136 | # Equations of Motion 137 | # --------------------------- 138 | MM = KM.mass_matrix_full 139 | kdd = KM.kindiffdict() 140 | rhs = KM.forcing_full 141 | MM = MM.subs(kdd) 142 | rhs = rhs.subs(kdd) 143 | 144 | MM.simplify() 145 | # MM = MM.subs(q0**2 + q1**2 + q2**2 + q3**2, 1) 146 | print() 147 | print('Mass Matrix') 148 | print('-----------') 149 | mprint(MM) 150 | 151 | rhs.simplify() 152 | # rhs = rhs.subs(q0**2 + q1**2 + q2**2 + q3**2, 1) 153 | print('Right Hand Side') 154 | print('---------------') 155 | mprint(rhs) 156 | print() 157 | 158 | # So, MM*x = rhs, where x is the State Derivatives 159 | # Solve for x 160 | stateDot = MM.inv()*rhs 161 | print('State Derivatives') 162 | print('-----------------------------------') 163 | mprint(stateDot) 164 | print() 165 | 166 | # POST-PROCESSING 167 | # --------------------------- 168 | 169 | # Useful Numbers 170 | # --------------------------- 171 | # p, q, r are the drone's angular velocities in the inertial frame, expressed in the drone's frame. 172 | # These calculations are not relevant to the ODE, but might be used for control. 173 | quat_dot_2 = Matrix([[q0d],[q1d],[q2d],[q3d]]) 174 | angVel_2 = 2.0*Gquat*quat_dot_2 175 | print('P, Q, R (Angular velocities in drone frame)') 176 | print('-------------------------------------------') 177 | mprint(angVel_2[0]) 178 | print() 179 | mprint(angVel_2[1]) 180 | print() 181 | mprint(angVel_2[2]) 182 | print() 183 | 184 | # u, v, w are the drone's velocities in the inertial frame, expressed in the drone's frame. 185 | # These calculations are not relevant to the ODE, but might be used for control. 186 | u = dot(Bcm.vel(N).subs(kdd), B.x).simplify() 187 | v = dot(Bcm.vel(N).subs(kdd), B.y).simplify() 188 | w = dot(Bcm.vel(N).subs(kdd), B.z).simplify() 189 | print('u, v, w (Velocities in drone frame)') 190 | print('-----------------------------------') 191 | mprint(u) 192 | print() 193 | mprint(v) 194 | print() 195 | mprint(w) 196 | print() 197 | 198 | # uFlat, vFlat, wFlat are the drone's velocities in the inertial frame, expressed in a frame 199 | # parallel to the ground, but with the drone's heading. 200 | # These calculations are not relevant to the ODE, but might be used for control. 201 | uFlat = dot(Bcm.vel(N).subs(kdd), cross(B.y, N.z)).simplify() 202 | vFlat = dot(Bcm.vel(N).subs(kdd), -cross(B.x, N.z)).simplify() 203 | wFlat = dot(Bcm.vel(N).subs(kdd), N.z).simplify() 204 | print('uFlat, vFlat, wFlat (Velocities in drone frame (after Yaw))') 205 | print('-----------------------------------------------------------') 206 | mprint(uFlat) 207 | print() 208 | mprint(vFlat) 209 | print() 210 | mprint(wFlat) 211 | print() 212 | -------------------------------------------------------------------------------- /PyDy Scripts/01 - Added Gyroscopic Precession/Quad_3D_frd_NED_Euler.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | author: John Bass 4 | email: john.bobzwik@gmail.com 5 | license: MIT 6 | Please feel free to use and modify this, but keep the above information. Thanks! 7 | """ 8 | 9 | """ 10 | Using PyDy and Sympy, this script generates the equations for the state derivatives 11 | of a quadcopter in a 3-dimensional space. The states in this particular script are: 12 | 13 | x, y, z : Position of the drone's center of mass in the inertial frame, 14 | expressed in the inertial frame 15 | xdot, ydot, zdot : Velocity of the drone's center of mass in the inertial frame, 16 | expressed in the inertial frame 17 | phi, theta, psi : Orientation (roll, pitch, yaw angles) of the drone in the 18 | inertial frame, following the order ZYX (yaw, pitch, roll) 19 | p, q, r : Angular velocity of the drone in the inertial frame, 20 | expressed in the drone's frame 21 | 22 | Important note : This script uses a frd body orientation (front-right-down) and 23 | a NED world orientation (North-East-Down). The drone's altitude is -z. 24 | 25 | Other note : In the resulting state derivatives, there are still simplifications 26 | that can be made that SymPy cannot simplify (factoring). 27 | 28 | author: John Bass 29 | email: 30 | """ 31 | 32 | from sympy import symbols 33 | from sympy.physics.mechanics import * 34 | 35 | # Reference frames and Points 36 | # --------------------------- 37 | N = ReferenceFrame('N') # Inertial Frame 38 | B = ReferenceFrame('B') # Drone after X (roll) rotation (Final rotation) 39 | C = ReferenceFrame('C') # Drone after Y (pitch) rotation 40 | D = ReferenceFrame('D') # Drone after Z (yaw) rotation (First rotation) 41 | 42 | No = Point('No') 43 | Bcm = Point('Bcm') # Drone's center of mass 44 | M1 = Point('M1') # Motor 1 is front left, then the rest increments CW (Drone is in X configuration, not +) 45 | M2 = Point('M2') 46 | M3 = Point('M3') 47 | M4 = Point('M4') 48 | 49 | # Variables 50 | # --------------------------- 51 | # x, y and z are the drone's coordinates in the inertial frame, expressed with the inertial frame 52 | # xdot, ydot and zdot are the drone's velocities in the inertial frame, expressed with the inertial frame 53 | # phi, theta and psi represents the drone's orientation in the inertial frame, expressed with a ZYX Body rotation 54 | # p, q and r are the drone's angular velocities in the inertial frame, expressed with the drone's frame 55 | 56 | x, y, z, xdot, ydot, zdot = dynamicsymbols('x y z xdot ydot zdot') 57 | phi, theta, psi, p, q, r = dynamicsymbols('phi theta psi p q r') 58 | 59 | # First derivatives of the variables 60 | xd, yd, zd, xdotd, ydotd, zdotd = dynamicsymbols('x y z xdot ydot zdot', 1) 61 | phid, thetad, psid, pd, qd, rd = dynamicsymbols('phi theta psi p q r', 1) 62 | 63 | # Constants 64 | # --------------------------- 65 | mB, g, dxm, dym, dzm, IBxx, IByy, IBzz, IRzz, wM1, wM2, wM3, wM4 = symbols('mB g dxm dym dzm IBxx IByy IBzz IRzz wM1 wM2 wM3 wM4') 66 | ThrM1, ThrM2, ThrM3, ThrM4, TorM1, TorM2, TorM3, TorM4 = symbols('ThrM1 ThrM2 ThrM3 ThrM4 TorM1 TorM2 TorM3 TorM4') 67 | 68 | # Rotation ZYX Body 69 | # --------------------------- 70 | D.orient(N, 'Axis', [psi, N.z]) 71 | C.orient(D, 'Axis', [theta, D.y]) 72 | B.orient(C, 'Axis', [phi, C.x]) 73 | 74 | # Origin 75 | # --------------------------- 76 | No.set_vel(N, 0) 77 | 78 | # Translation 79 | # --------------------------- 80 | Bcm.set_pos(No, x*N.x + y*N.y + z*N.z) 81 | Bcm.set_vel(N, Bcm.pos_from(No).dt(N)) 82 | 83 | # Motor placement 84 | # M1 is front left, then clockwise numbering 85 | # dzm is positive for motors above center of mass 86 | # --------------------------- 87 | M1.set_pos(Bcm, dxm*B.x - dym*B.y - dzm*B.z) 88 | M2.set_pos(Bcm, dxm*B.x + dym*B.y - dzm*B.z) 89 | M3.set_pos(Bcm, -dxm*B.x + dym*B.y - dzm*B.z) 90 | M4.set_pos(Bcm, -dxm*B.x - dym*B.y - dzm*B.z) 91 | M1.v2pt_theory(Bcm, N, B) 92 | M2.v2pt_theory(Bcm, N, B) 93 | M3.v2pt_theory(Bcm, N, B) 94 | M4.v2pt_theory(Bcm, N, B) 95 | 96 | # Inertia Dyadic 97 | # --------------------------- 98 | IB = inertia(B, IBxx, IByy, IBzz) 99 | 100 | # Create Bodies 101 | # --------------------------- 102 | BodyB = RigidBody('BodyB', Bcm, B, mB, (IB, Bcm)) 103 | BodyList = [BodyB] 104 | 105 | # Forces and Torques 106 | # --------------------------- 107 | Grav_Force = (Bcm, mB*g*N.z) 108 | FM1 = (M1, -ThrM1*B.z) 109 | FM2 = (M2, -ThrM2*B.z) 110 | FM3 = (M3, -ThrM3*B.z) 111 | FM4 = (M4, -ThrM4*B.z) 112 | 113 | TM1 = (B, -TorM1*B.z) 114 | TM2 = (B, TorM2*B.z) 115 | TM3 = (B, -TorM3*B.z) 116 | TM4 = (B, TorM4*B.z) 117 | 118 | gyro = (B, -IRzz*cross(B.ang_vel_in(N), (wM1 - wM2 + wM3 - wM4)*B.z)) 119 | 120 | ForceList = [Grav_Force, FM1, FM2, FM3, FM4, TM1, TM2, TM3, TM4, gyro] 121 | 122 | # Kinematic Differential Equations 123 | # --------------------------- 124 | 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)] 125 | 126 | # Kane's Method 127 | # --------------------------- 128 | KM = KanesMethod(N, q_ind=[x, y, z, phi, theta, psi], u_ind=[xdot, ydot, zdot, p, q, r], kd_eqs=kd) 129 | (fr, frstar) = KM.kanes_equations(BodyList, ForceList) 130 | 131 | # Equations of Motion 132 | # --------------------------- 133 | MM = KM.mass_matrix_full 134 | kdd = KM.kindiffdict() 135 | rhs = KM.forcing_full 136 | rhs = rhs.subs(kdd) 137 | 138 | MM.simplify() 139 | print('Mass Matrix') 140 | print('-----------') 141 | mprint(MM) 142 | print() 143 | 144 | rhs.simplify() 145 | print('Right Hand Side') 146 | print('---------------') 147 | mprint(rhs) 148 | print() 149 | 150 | # So, MM*x = rhs, where x is the State Derivatives 151 | # Solve for x 152 | stateDot = MM.inv()*rhs 153 | print('State Derivatives') 154 | print('-----------------------------------') 155 | mprint(stateDot) 156 | print() 157 | 158 | # POST-PROCESSING 159 | # --------------------------- 160 | 161 | # Useful Numbers 162 | # --------------------------- 163 | # p, q, r are the drone's angular velocities in the inertial frame, expressed in the drone's frame. 164 | # These calculations are not relevant to the ODE, but might be used for control. 165 | print('P, Q, R (Angular velocities in drone frame)') 166 | print('-------------------------------------------') 167 | mprint(dot(B.ang_vel_in(N), B.x)) 168 | print() 169 | mprint(dot(B.ang_vel_in(N), B.y)) 170 | print() 171 | mprint(dot(B.ang_vel_in(N), B.z)) 172 | print() 173 | 174 | # u, v, w are the drone's velocities in the inertial frame, expressed in the drone's frame. 175 | # These calculations are not relevant to the ODE, but might be used for control. 176 | u = dot(Bcm.vel(N).subs(kdd), B.x) 177 | v = dot(Bcm.vel(N).subs(kdd), B.y) 178 | w = dot(Bcm.vel(N).subs(kdd), B.z) 179 | print('u, v, w (Velocities in drone frame)') 180 | print('-----------------------------------') 181 | mprint(u) 182 | print() 183 | mprint(v) 184 | print() 185 | mprint(w) 186 | print() 187 | 188 | # uFlat, vFlat, wFlat are the drone's velocities in the inertial frame, expressed in a frame 189 | # parallel to the ground, but with the drone's heading (so only after the YAW rotation). 190 | # These calculations are not relevant to the ODE, but might be used for control. 191 | uFlat = dot(Bcm.vel(N).subs(kdd), D.x) 192 | vFlat = dot(Bcm.vel(N).subs(kdd), D.y) 193 | wFlat = dot(Bcm.vel(N).subs(kdd), D.z) 194 | print('uFlat, vFlat, wFlat (Velocities in drone frame (after Yaw))') 195 | print('-----------------------------------------------------------') 196 | mprint(uFlat) 197 | print() 198 | mprint(vFlat) 199 | print() 200 | mprint(wFlat) 201 | print() 202 | -------------------------------------------------------------------------------- /PyDy Scripts/01 - Added Gyroscopic Precession/Quad_3D_frd_NED_Quat.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | author: John Bass 4 | email: john.bobzwik@gmail.com 5 | license: MIT 6 | Please feel free to use and modify this, but keep the above information. Thanks! 7 | """ 8 | 9 | """ 10 | Using PyDy and Sympy, this script generates the equations for the state derivatives 11 | of a quadcopter in a 3-dimensional space. The states in this particular script are: 12 | 13 | x, y, z : Position of the drone's center of mass in the inertial frame, 14 | expressed in the inertial frame 15 | xdot, ydot, zdot : Velocity of the drone's center of mass in the inertial frame, 16 | expressed in the inertial frame 17 | q0, q1, q2, q3 : Orientation of the drone in the inertial frame using quaternions 18 | p, q, r : Angular velocity of the drone in the inertial frame, 19 | expressed in the drone's frame 20 | 21 | Important note : This script uses a frd body orientation (front-right-down) and 22 | a NED world orientation (North-East-Down). The drone's altitude is -z. 23 | 24 | Other note : In the resulting state derivatives, there are still simplifications 25 | that can be made that SymPy cannot simplify (factoring). 26 | 27 | author: John Bass 28 | email: 29 | """ 30 | 31 | from sympy import symbols, Matrix 32 | from sympy.physics.mechanics import * 33 | 34 | # Reference frames and Points 35 | # --------------------------- 36 | N = ReferenceFrame('N') # Inertial Frame 37 | B = ReferenceFrame('B') # Drone after X (roll) rotation (Final rotation) 38 | 39 | No = Point('No') 40 | Bcm = Point('Bcm') # Drone's center of mass 41 | M1 = Point('M1') # Motor 1 is front left, then the rest increments CW (Drone is in X configuration, not +) 42 | M2 = Point('M2') 43 | M3 = Point('M3') 44 | M4 = Point('M4') 45 | 46 | # Variables 47 | # --------------------------- 48 | # x, y and z are the drone's coordinates in the inertial frame, expressed with the inertial frame 49 | # xdot, ydot and zdot are the drone's speeds in the inertial frame, expressed with the inertial frame 50 | # q0, q1, q2, q3 represents the drone's orientation in the inertial frame, using quaternions 51 | # p, q and r are the drone's angular velocities in the inertial frame, expressed with the drone's frame 52 | 53 | x, y, z, xdot, ydot, zdot = dynamicsymbols('x y z xdot ydot zdot') 54 | q0, q1, q2, q3, p, q, r = dynamicsymbols('q0 q1 q2 q3 p q r') 55 | 56 | # First derivatives of the variables 57 | xd, yd, zd, xdotd, ydotd, zdotd = dynamicsymbols('x y z xdot ydot zdot', 1) 58 | q0d, q1d, q2d, q3d, pd, qd, rd = dynamicsymbols('q0 q1 q2 q3 p q r', 1) 59 | 60 | # Constants 61 | # --------------------------- 62 | mB, g, dxm, dym, dzm, IBxx, IByy, IBzz, IRzz, wM1, wM2, wM3, wM4 = symbols('mB g dxm dym dzm IBxx IByy IBzz IRzz wM1 wM2 wM3 wM4') 63 | ThrM1, ThrM2, ThrM3, ThrM4, TorM1, TorM2, TorM3, TorM4 = symbols('ThrM1 ThrM2 ThrM3 ThrM4 TorM1 TorM2 TorM3 TorM4') 64 | 65 | # Rotation Quaternion 66 | # --------------------------- 67 | B.orient(N, 'Quaternion', [q0, q1, q2, q3]) 68 | B.set_ang_vel(N, p*B.x + q*B.y + r*B.z) 69 | 70 | # Origin 71 | # --------------------------- 72 | No.set_vel(N, 0) 73 | 74 | # Translation 75 | # --------------------------- 76 | Bcm.set_pos(No, x*N.x + y*N.y + z*N.z) 77 | Bcm.set_vel(N, Bcm.pos_from(No).dt(N)) 78 | 79 | # Motor placement 80 | # M1 is front left, then clockwise numbering 81 | # dzm is positive for motors above center of mass 82 | # --------------------------- 83 | M1.set_pos(Bcm, dxm*B.x - dym*B.y - dzm*B.z) 84 | M2.set_pos(Bcm, dxm*B.x + dym*B.y - dzm*B.z) 85 | M3.set_pos(Bcm, -dxm*B.x + dym*B.y - dzm*B.z) 86 | M4.set_pos(Bcm, -dxm*B.x - dym*B.y - dzm*B.z) 87 | M1.v2pt_theory(Bcm, N, B) 88 | M2.v2pt_theory(Bcm, N, B) 89 | M3.v2pt_theory(Bcm, N, B) 90 | M4.v2pt_theory(Bcm, N, B) 91 | 92 | # Inertia Dyadic 93 | # --------------------------- 94 | IB = inertia(B, IBxx, IByy, IBzz) 95 | 96 | # Create Bodies 97 | # --------------------------- 98 | BodyB = RigidBody('BodyB', Bcm, B, mB, (IB, Bcm)) 99 | BodyList = [BodyB] 100 | 101 | # Forces and Torques 102 | # --------------------------- 103 | Grav_Force = (Bcm, mB*g*N.z) 104 | FM1 = (M1, -ThrM1*B.z) 105 | FM2 = (M2, -ThrM2*B.z) 106 | FM3 = (M3, -ThrM3*B.z) 107 | FM4 = (M4, -ThrM4*B.z) 108 | 109 | TM1 = (B, -TorM1*B.z) 110 | TM2 = (B, TorM2*B.z) 111 | TM3 = (B, -TorM3*B.z) 112 | TM4 = (B, TorM4*B.z) 113 | 114 | gyro = (B, -IRzz*cross(B.ang_vel_in(N), (wM1 - wM2 + wM3 - wM4)*B.z)) 115 | 116 | ForceList = [Grav_Force, FM1, FM2, FM3, FM4, TM1, TM2, TM3, TM4, gyro] 117 | 118 | # Calculate Quaternion Derivative 119 | # --------------------------- 120 | Gquat = Matrix([[-q1, q0, q3, -q2], 121 | [-q2, -q3, q0, q1], 122 | [-q3, q2, -q1, q0]]) 123 | 124 | angVel = Matrix([[dot(B.ang_vel_in(N), B.x)],[dot(B.ang_vel_in(N), B.y)],[dot(B.ang_vel_in(N), B.z)]]) # Basically the same as: angVel = Matrix([[p],[q],[r]]) 125 | quat_dot = 1.0/2*Gquat.T*angVel 126 | 127 | # Kinematic Differential Equations 128 | # --------------------------- 129 | kd = [xdot - xd, ydot - yd, zdot - zd, q0d - quat_dot[0], q1d - quat_dot[1], q2d - quat_dot[2], q3d - quat_dot[3]] 130 | 131 | # Kane's Method 132 | # --------------------------- 133 | KM = KanesMethod(N, q_ind=[x, y, z, q0, q1, q2, q3], u_ind=[xdot, ydot, zdot, p, q, r], kd_eqs=kd) 134 | (fr, frstar) = KM.kanes_equations(BodyList, ForceList) 135 | 136 | # Equations of Motion 137 | # --------------------------- 138 | MM = KM.mass_matrix_full 139 | kdd = KM.kindiffdict() 140 | rhs = KM.forcing_full 141 | MM = MM.subs(kdd) 142 | rhs = rhs.subs(kdd) 143 | 144 | MM.simplify() 145 | # MM = MM.subs(q0**2 + q1**2 + q2**2 + q3**2, 1) 146 | print() 147 | print('Mass Matrix') 148 | print('-----------') 149 | mprint(MM) 150 | 151 | rhs.simplify() 152 | # rhs = rhs.subs(q0**2 + q1**2 + q2**2 + q3**2, 1) 153 | print('Right Hand Side') 154 | print('---------------') 155 | mprint(rhs) 156 | print() 157 | 158 | # So, MM*x = rhs, where x is the State Derivatives 159 | # Solve for x 160 | stateDot = MM.inv()*rhs 161 | print('State Derivatives') 162 | print('-----------------------------------') 163 | mprint(stateDot) 164 | print() 165 | 166 | # POST-PROCESSING 167 | # --------------------------- 168 | 169 | # Useful Numbers 170 | # --------------------------- 171 | # p, q, r are the drone's angular velocities in the inertial frame, expressed in the drone's frame. 172 | # These calculations are not relevant to the ODE, but might be used for control. 173 | quat_dot_2 = Matrix([[q0d],[q1d],[q2d],[q3d]]) 174 | angVel_2 = 2.0*Gquat*quat_dot_2 175 | print('P, Q, R (Angular velocities in drone frame)') 176 | print('-------------------------------------------') 177 | mprint(angVel_2[0]) 178 | print() 179 | mprint(angVel_2[1]) 180 | print() 181 | mprint(angVel_2[2]) 182 | print() 183 | 184 | # u, v, w are the drone's velocities in the inertial frame, expressed in the drone's frame. 185 | # These calculations are not relevant to the ODE, but might be used for control. 186 | u = dot(Bcm.vel(N).subs(kdd), B.x).simplify() 187 | v = dot(Bcm.vel(N).subs(kdd), B.y).simplify() 188 | w = dot(Bcm.vel(N).subs(kdd), B.z).simplify() 189 | print('u, v, w (Velocities in drone frame)') 190 | print('-----------------------------------') 191 | mprint(u) 192 | print() 193 | mprint(v) 194 | print() 195 | mprint(w) 196 | print() 197 | 198 | # uFlat, vFlat, wFlat are the drone's velocities in the inertial frame, expressed in a frame 199 | # parallel to the ground, but with the drone's heading. 200 | # These calculations are not relevant to the ODE, but might be used for control. 201 | uFlat = dot(Bcm.vel(N).subs(kdd), cross(B.y, N.z)).simplify() 202 | vFlat = dot(Bcm.vel(N).subs(kdd), -cross(B.x, N.z)).simplify() 203 | wFlat = dot(Bcm.vel(N).subs(kdd), N.z).simplify() 204 | print('uFlat, vFlat, wFlat (Velocities in drone frame (after Yaw))') 205 | print('-----------------------------------------------------------') 206 | mprint(uFlat) 207 | print() 208 | mprint(vFlat) 209 | print() 210 | mprint(wFlat) 211 | print() 212 | -------------------------------------------------------------------------------- /PyDy Scripts/01 - Added Gyroscopic Precession/Quad_3D_frd_NED_Quat_RotorBodies.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | author: John Bass 4 | email: john.bobzwik@gmail.com 5 | license: MIT 6 | Please feel free to use and modify this, but keep the above information. Thanks! 7 | """ 8 | 9 | """ 10 | SCRIPT NOT FUNCTIONAL 11 | 12 | 13 | Using PyDy and Sympy, this script generates the equations for the state derivatives 14 | of a quadcopter in a 3-dimensional space. The states in this particular script are: 15 | 16 | x, y, z : Position of the drone's center of mass in the inertial frame, 17 | expressed in the inertial frame 18 | xdot, ydot, zdot : Velocity of the drone's center of mass in the inertial frame, 19 | expressed in the inertial frame 20 | q0, q1, q2, q3 : Orientation of the drone in the inertial frame using quaternions 21 | p, q, r : Angular velocity of the drone in the inertial frame, 22 | expressed in the drone's frame 23 | 24 | Important note : This script uses a frd body orientation (front-right-down) and 25 | a NED world orientation (North-East-Down). The drone's altitude is -z. 26 | 27 | Other note : In the resulting state derivatives, there are still simplifications 28 | that can be made that SymPy cannot simplify (factoring). 29 | 30 | author: John Bass 31 | email: 32 | """ 33 | 34 | from sympy import symbols, Matrix 35 | from sympy.physics.mechanics import * 36 | 37 | # Reference frames and Points 38 | # --------------------------- 39 | N = ReferenceFrame('N') # Inertial Frame 40 | B = ReferenceFrame('B') # Drone after X (roll) rotation (Final rotation) 41 | R1 = ReferenceFrame('R1') 42 | R2 = ReferenceFrame('R2') 43 | R3 = ReferenceFrame('R3') 44 | R4 = ReferenceFrame('R4') 45 | 46 | No = Point('No') 47 | Bcm = Point('Bcm') # Drone's center of mass 48 | M1 = Point('M1') # Motor 1 is front left, then the rest increments CW (Drone is in X configuration, not +) 49 | M2 = Point('M2') 50 | M3 = Point('M3') 51 | M4 = Point('M4') 52 | 53 | # Variables 54 | # --------------------------- 55 | # x, y and z are the drone's coordinates in the inertial frame, expressed with the inertial frame 56 | # xdot, ydot and zdot are the drone's speeds in the inertial frame, expressed with the inertial frame 57 | # q0, q1, q2, q3 represents the drone's orientation in the inertial frame, using quaternions 58 | # p, q and r are the drone's angular velocities in the inertial frame, expressed with the drone's frame 59 | 60 | x, y, z, xdot, ydot, zdot = dynamicsymbols('x y z xdot ydot zdot') 61 | q0, q1, q2, q3, p, q, r = dynamicsymbols('q0 q1 q2 q3 p q r') 62 | 63 | # First derivatives of the variables 64 | xd, yd, zd, xdotd, ydotd, zdotd = dynamicsymbols('x y z xdot ydot zdot', 1) 65 | q0d, q1d, q2d, q3d, pd, qd, rd = dynamicsymbols('q0 q1 q2 q3 p q r', 1) 66 | 67 | qR1, qR2, qR3, qR4 = dynamicsymbols('qR1 qR2 qR3 qR4') 68 | 69 | # Constants 70 | # --------------------------- 71 | mB, g, dxm, dym, dzm, IBxx, IByy, IBzz, mR, IRzz, wM1, wM2, wM3, wM4 = symbols('mB g dxm dym dzm IBxx IByy mR IBzz IRzz wM1 wM2 wM3 wM4') 72 | ThrM1, ThrM2, ThrM3, ThrM4, TorM1, TorM2, TorM3, TorM4 = symbols('ThrM1 ThrM2 ThrM3 ThrM4 TorM1 TorM2 TorM3 TorM4') 73 | 74 | # Rotation Quaternion 75 | # --------------------------- 76 | B.orient(N, 'Quaternion', [q0, q1, q2, q3]) 77 | B.set_ang_vel(N, p*B.x + q*B.y + r*B.z) 78 | 79 | R1.orient(B, 'Axis', [qR1, B.z]) 80 | R2.orient(B, 'Axis', [qR2, B.z]) 81 | R3.orient(B, 'Axis', [qR3, B.z]) 82 | R4.orient(B, 'Axis', [qR4, B.z]) 83 | 84 | R1.set_ang_vel(B, wM1*B.z) 85 | R2.set_ang_vel(B, -wM2*B.z) 86 | R3.set_ang_vel(B, wM3*B.z) 87 | R4.set_ang_vel(B, -wM4*B.z) 88 | 89 | # Origin 90 | # --------------------------- 91 | No.set_vel(N, 0) 92 | 93 | # Translation 94 | # --------------------------- 95 | Bcm.set_pos(No, x*N.x + y*N.y + z*N.z) 96 | Bcm.set_vel(N, Bcm.pos_from(No).dt(N)) 97 | 98 | # Motor placement 99 | # M1 is front left, then clockwise numbering 100 | # dzm is positive for motors above center of mass 101 | # --------------------------- 102 | M1.set_pos(Bcm, dxm*B.x - dym*B.y - dzm*B.z) 103 | M2.set_pos(Bcm, dxm*B.x + dym*B.y - dzm*B.z) 104 | M3.set_pos(Bcm, -dxm*B.x + dym*B.y - dzm*B.z) 105 | M4.set_pos(Bcm, -dxm*B.x - dym*B.y - dzm*B.z) 106 | M1.v2pt_theory(Bcm, N, B) 107 | M2.v2pt_theory(Bcm, N, B) 108 | M3.v2pt_theory(Bcm, N, B) 109 | M4.v2pt_theory(Bcm, N, B) 110 | 111 | # Inertia Dyadic 112 | # --------------------------- 113 | IB = inertia(B, IBxx, IByy, IBzz) 114 | 115 | IR1 = inertia(R1, 0, 0, IRzz) 116 | IR2 = inertia(R2, 0, 0, IRzz) 117 | IR3 = inertia(R3, 0, 0, IRzz) 118 | IR4 = inertia(R4, 0, 0, IRzz) 119 | 120 | # Create Bodies 121 | # --------------------------- 122 | BodyB = RigidBody('BodyB', Bcm, B, mB, (IB, Bcm)) 123 | BodyR1 = RigidBody('BodyR1', M1, R1, mR, (IR1, M1)) 124 | BodyR2 = RigidBody('BodyR2', M2, R1, mR, (IR2, M2)) 125 | BodyR3 = RigidBody('BodyR3', M3, R1, mR, (IR3, M3)) 126 | BodyR4 = RigidBody('BodyR4', M4, R1, mR, (IR4, M4)) 127 | BodyList = [BodyB, BodyR1, BodyR2, BodyR3, BodyR4] 128 | 129 | # Forces and Torques 130 | # --------------------------- 131 | Grav_Force = (Bcm, mB*g*N.z) 132 | FM1 = (M1, -ThrM1*B.z) 133 | FM2 = (M2, -ThrM2*B.z) 134 | FM3 = (M3, -ThrM3*B.z) 135 | FM4 = (M4, -ThrM4*B.z) 136 | 137 | TM1 = (B, -TorM1*B.z) 138 | TM2 = (B, TorM2*B.z) 139 | TM3 = (B, -TorM3*B.z) 140 | TM4 = (B, TorM4*B.z) 141 | 142 | # Gyro = (B, -IRzz*cross(B.ang_vel_in(N),(wM1 - wM2 + wM3 - wM4)*B.z)) 143 | 144 | ForceList = [Grav_Force, FM1, FM2, FM3, FM4, TM1, TM2, TM3, TM4] 145 | 146 | # Calculate Quaternion Derivative 147 | # --------------------------- 148 | Gquat = Matrix([[-q1, q0, q3, -q2], 149 | [-q2, -q3, q0, q1], 150 | [-q3, q2, -q1, q0]]) 151 | 152 | angVel = Matrix([[dot(B.ang_vel_in(N), B.x)],[dot(B.ang_vel_in(N), B.y)],[dot(B.ang_vel_in(N), B.z)]]) # Basically the same as: angVel = Matrix([[p],[q],[r]]) 153 | quat_dot = 1.0/2*Gquat.T*angVel 154 | 155 | # Kinematic Differential Equations 156 | # --------------------------- 157 | kd = [xdot - xd, ydot - yd, zdot - zd, q0d - quat_dot[0], q1d - quat_dot[1], q2d - quat_dot[2], q3d - quat_dot[3]] 158 | 159 | # Kane's Method 160 | # --------------------------- 161 | KM = KanesMethod(N, q_ind=[x, y, z, q0, q1, q2, q3], u_ind=[xdot, ydot, zdot, p, q, r], kd_eqs=kd) 162 | (fr, frstar) = KM.kanes_equations(BodyList, ForceList) 163 | 164 | # Equations of Motion 165 | # --------------------------- 166 | MM = KM.mass_matrix_full 167 | kdd = KM.kindiffdict() 168 | rhs = KM.forcing_full 169 | MM = MM.subs(kdd) 170 | rhs = rhs.subs(kdd) 171 | 172 | MM.simplify() 173 | # MM = MM.subs(q0**2 + q1**2 + q2**2 + q3**2, 1) 174 | print() 175 | print('Mass Matrix') 176 | print('-----------') 177 | mprint(MM) 178 | 179 | rhs.simplify() 180 | # rhs = rhs.subs(q0**2 + q1**2 + q2**2 + q3**2, 1) 181 | print('Right Hand Side') 182 | print('---------------') 183 | mprint(rhs) 184 | print() 185 | 186 | # So, MM*x = rhs, where x is the State Derivatives 187 | # Solve for x 188 | stateDot = MM.inv()*rhs 189 | print('State Derivatives') 190 | print('-----------------------------------') 191 | mprint(stateDot) 192 | print() 193 | 194 | # POST-PROCESSING 195 | # --------------------------- 196 | 197 | # Useful Numbers 198 | # --------------------------- 199 | # p, q, r are the drone's angular velocities in the inertial frame, expressed in the drone's frame. 200 | # These calculations are not relevant to the ODE, but might be used for control. 201 | quat_dot_2 = Matrix([[q0d],[q1d],[q2d],[q3d]]) 202 | angVel_2 = 2.0*Gquat*quat_dot_2 203 | print('P, Q, R (Angular velocities in drone frame)') 204 | print('-------------------------------------------') 205 | mprint(angVel_2[0]) 206 | print() 207 | mprint(angVel_2[1]) 208 | print() 209 | mprint(angVel_2[2]) 210 | print() 211 | 212 | # u, v, w are the drone's velocities in the inertial frame, expressed in the drone's frame. 213 | # These calculations are not relevant to the ODE, but might be used for control. 214 | u = dot(Bcm.vel(N).subs(kdd), B.x).simplify() 215 | v = dot(Bcm.vel(N).subs(kdd), B.y).simplify() 216 | w = dot(Bcm.vel(N).subs(kdd), B.z).simplify() 217 | print('u, v, w (Velocities in drone frame)') 218 | print('-----------------------------------') 219 | mprint(u) 220 | print() 221 | mprint(v) 222 | print() 223 | mprint(w) 224 | print() 225 | 226 | # uFlat, vFlat, wFlat are the drone's velocities in the inertial frame, expressed in a frame 227 | # parallel to the ground, but with the drone's heading. 228 | # These calculations are not relevant to the ODE, but might be used for control. 229 | uFlat = dot(Bcm.vel(N).subs(kdd), cross(B.y, N.z)).simplify() 230 | vFlat = dot(Bcm.vel(N).subs(kdd), -cross(B.x, N.z)).simplify() 231 | wFlat = dot(Bcm.vel(N).subs(kdd), N.z).simplify() 232 | print('uFlat, vFlat, wFlat (Velocities in drone frame (after Yaw))') 233 | print('-----------------------------------------------------------') 234 | mprint(uFlat) 235 | print() 236 | mprint(vFlat) 237 | print() 238 | mprint(wFlat) 239 | print() 240 | -------------------------------------------------------------------------------- /PyDy Scripts/02 - Added Wind and Aero Drag/Quad_3D_flu_ENU_Quat.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | author: John Bass 4 | email: john.bobzwik@gmail.com 5 | license: MIT 6 | Please feel free to use and modify this, but keep the above information. Thanks! 7 | """ 8 | 9 | """ 10 | Using PyDy and Sympy, this script generates the equations for the state derivatives 11 | of a quadcopter in a 3-dimensional space. The states in this particular script are: 12 | 13 | x, y, z : Position of the drone's center of mass in the inertial frame, 14 | expressed in the inertial frame 15 | xdot, ydot, zdot : Velocity of the drone's center of mass in the inertial frame, 16 | expressed in the inertial frame 17 | q0, q1, q2, q3 : Orientation of the drone in the inertial frame using quaternions 18 | p, q, r : Angular velocity of the drone in the inertial frame, 19 | expressed in the drone's frame 20 | 21 | Important note : This script uses a flu body orientation (front-left-up) and 22 | a ENU world orientation (East-North-Up) 23 | 24 | Other note : In the resulting state derivatives, there are still simplifications 25 | that can be made that SymPy cannot simplify (factoring). 26 | 27 | author: John Bass 28 | email: 29 | """ 30 | 31 | from sympy import symbols, Matrix, sign 32 | from sympy.physics.mechanics import * 33 | 34 | # Reference frames and Points 35 | # --------------------------- 36 | N = ReferenceFrame('N') # Inertial Frame 37 | B = ReferenceFrame('B') # Drone after X (roll) rotation (Final rotation) 38 | W = ReferenceFrame('W') # Wind reference frame 39 | 40 | No = Point('No') 41 | Bcm = Point('Bcm') # Drone's center of mass 42 | M1 = Point('M1') # Motor 1 is front left, then the rest increments CW (Drone is in X configuration, not +) 43 | M2 = Point('M2') 44 | M3 = Point('M3') 45 | M4 = Point('M4') 46 | Wo = Point('Wo') # Arbitrary point in the Wind Frame 47 | 48 | # Variables 49 | # --------------------------- 50 | # x, y and z are the drone's coordinates in the inertial frame, expressed with the inertial frame 51 | # xdot, ydot and zdot are the drone's speeds in the inertial frame, expressed with the inertial frame 52 | # q0, q1, q2, q3 represents the drone's orientation in the inertial frame, using quaternions 53 | # p, q and r are the drone's angular velocities in the inertial frame, expressed with the drone's frame 54 | 55 | x, y, z, xdot, ydot, zdot = dynamicsymbols('x y z xdot ydot zdot') 56 | q0, q1, q2, q3, p, q, r = dynamicsymbols('q0 q1 q2 q3 p q r') 57 | 58 | # First derivatives of the variables 59 | xd, yd, zd, xdotd, ydotd, zdotd = dynamicsymbols('x y z xdot ydot zdot', 1) 60 | q0d, q1d, q2d, q3d, pd, qd, rd = dynamicsymbols('q0 q1 q2 q3 p q r', 1) 61 | 62 | # Constants 63 | # --------------------------- 64 | mB, g, dxm, dym, dzm, IBxx, IByy, IBzz, IRzz, wM1, wM2, wM3, wM4 = symbols('mB g dxm dym dzm IBxx IByy IBzz IRzz wM1 wM2 wM3 wM4') 65 | ThrM1, ThrM2, ThrM3, ThrM4, TorM1, TorM2, TorM3, TorM4 = symbols('ThrM1 ThrM2 ThrM3 ThrM4 TorM1 TorM2 TorM3 TorM4') 66 | qW1, qW2, velW, Cd = symbols('qW1 qW2 velW Cd') 67 | 68 | # Rotation Quaternion 69 | # --------------------------- 70 | B.orient(N, 'Quaternion', [q0, q1, q2, q3]) 71 | B.set_ang_vel(N, p*B.x + q*B.y + r*B.z) 72 | 73 | # Origin 74 | # --------------------------- 75 | No.set_vel(N, 0) 76 | 77 | # Translation 78 | # --------------------------- 79 | Bcm.set_pos(No, x*N.x + y*N.y + z*N.z) 80 | Bcm.set_vel(N, Bcm.pos_from(No).dt(N)) 81 | 82 | # Motor placement 83 | # M1 is front left, then clockwise numbering 84 | # dzm is positive for motors above center of mass 85 | # --------------------------- 86 | M1.set_pos(Bcm, dxm*B.x + dym*B.y + dzm*B.z) 87 | M2.set_pos(Bcm, dxm*B.x - dym*B.y + dzm*B.z) 88 | M3.set_pos(Bcm, -dxm*B.x - dym*B.y + dzm*B.z) 89 | M4.set_pos(Bcm, -dxm*B.x + dym*B.y + dzm*B.z) 90 | M1.v2pt_theory(Bcm, N, B) 91 | M2.v2pt_theory(Bcm, N, B) 92 | M3.v2pt_theory(Bcm, N, B) 93 | M4.v2pt_theory(Bcm, N, B) 94 | 95 | # Inertia Dyadic 96 | # --------------------------- 97 | IB = inertia(B, IBxx, IByy, IBzz) 98 | 99 | # Create Bodies 100 | # --------------------------- 101 | BodyB = RigidBody('BodyB', Bcm, B, mB, (IB, Bcm)) 102 | BodyList = [BodyB] 103 | 104 | # Wind 105 | # --------------------------- 106 | W.orient(N, 'Body', [qW1, qW2, 0], 'ZYX') 107 | # W.orient(N, 'Quaternion', [qW0, qW1, qW2, qW3]) 108 | Wo.set_vel(N, velW*W.x) 109 | airVel = Bcm.vel(N) - Wo.vel(N) 110 | 111 | # Forces and Torques 112 | # --------------------------- 113 | Grav_Force = (Bcm, -mB*g*N.z) 114 | FM1 = (M1, ThrM1*B.z) 115 | FM2 = (M2, ThrM2*B.z) 116 | FM3 = (M3, ThrM3*B.z) 117 | FM4 = (M4, ThrM4*B.z) 118 | 119 | TM1 = (B, TorM1*B.z) 120 | TM2 = (B, -TorM2*B.z) 121 | TM3 = (B, TorM3*B.z) 122 | TM4 = (B, -TorM4*B.z) 123 | 124 | # drag = (Bcm, -Cd*airVel.normalize()*(airVel.magnitude())**2) 125 | dragX = (Bcm, -Cd*sign(dot(airVel, N.x))*(dot(airVel, N.x)**2)*N.x) 126 | dragY = (Bcm, -Cd*sign(dot(airVel, N.y))*(dot(airVel, N.y)**2)*N.y) 127 | dragZ = (Bcm, -Cd*sign(dot(airVel, N.z))*(dot(airVel, N.z)**2)*N.z) 128 | 129 | gyro = (B, -IRzz*cross(B.ang_vel_in(N), (-wM1 + wM2 - wM3 + wM4)*B.z)) 130 | 131 | ForceList = [Grav_Force, FM1, FM2, FM3, FM4, TM1, TM2, TM3, TM4, dragX, dragY, dragZ, gyro] 132 | 133 | # Calculate Quaternion Derivative 134 | # --------------------------- 135 | Gquat = Matrix([[-q1, q0, q3, -q2], 136 | [-q2, -q3, q0, q1], 137 | [-q3, q2, -q1, q0]]) 138 | 139 | angVel = Matrix([[dot(B.ang_vel_in(N), B.x)],[dot(B.ang_vel_in(N), B.y)],[dot(B.ang_vel_in(N), B.z)]]) # Basically the same as: angVel = Matrix([[p],[q],[r]]) 140 | quat_dot = 1.0/2*Gquat.T*angVel 141 | 142 | # Kinematic Differential Equations 143 | # --------------------------- 144 | kd = [xdot - xd, ydot - yd, zdot - zd, q0d - quat_dot[0], q1d - quat_dot[1], q2d - quat_dot[2], q3d - quat_dot[3]] 145 | 146 | # Kane's Method 147 | # --------------------------- 148 | KM = KanesMethod(N, q_ind=[x, y, z, q0, q1, q2, q3], u_ind=[xdot, ydot, zdot, p, q, r], kd_eqs=kd) 149 | (fr, frstar) = KM.kanes_equations(BodyList, ForceList) 150 | 151 | # Equations of Motion 152 | # --------------------------- 153 | MM = KM.mass_matrix_full 154 | kdd = KM.kindiffdict() 155 | rhs = KM.forcing_full 156 | MM = MM.subs(kdd) 157 | rhs = rhs.subs(kdd) 158 | 159 | MM.simplify() 160 | # MM = MM.subs(q0**2 + q1**2 + q2**2 + q3**2, 1) 161 | print() 162 | print('Mass Matrix') 163 | print('-----------') 164 | mprint(MM) 165 | 166 | rhs.simplify() 167 | # rhs = rhs.subs(q0**2 + q1**2 + q2**2 + q3**2, 1) 168 | print('Right Hand Side') 169 | print('---------------') 170 | mprint(rhs) 171 | print() 172 | 173 | # So, MM*x = rhs, where x is the State Derivatives 174 | # Solve for x 175 | stateDot = MM.inv()*rhs 176 | print('State Derivatives') 177 | print('-----------------------------------') 178 | mprint(stateDot) 179 | print() 180 | 181 | # POST-PROCESSING 182 | # --------------------------- 183 | 184 | # Useful Numbers 185 | # --------------------------- 186 | # p, q, r are the drone's angular velocities in the inertial frame, expressed in the drone's frame. 187 | # These calculations are not relevant to the ODE, but might be used for control. 188 | quat_dot_2 = Matrix([[q0d],[q1d],[q2d],[q3d]]) 189 | angVel_2 = 2.0*Gquat*quat_dot_2 190 | print('P, Q, R (Angular velocities in drone frame)') 191 | print('-------------------------------------------') 192 | mprint(angVel_2[0]) 193 | print() 194 | mprint(angVel_2[1]) 195 | print() 196 | mprint(angVel_2[2]) 197 | print() 198 | 199 | # u, v, w are the drone's velocities in the inertial frame, expressed in the drone's frame. 200 | # These calculations are not relevant to the ODE, but might be used for control. 201 | u = dot(Bcm.vel(N).subs(kdd), B.x).simplify() 202 | v = dot(Bcm.vel(N).subs(kdd), B.y).simplify() 203 | w = dot(Bcm.vel(N).subs(kdd), B.z).simplify() 204 | print('u, v, w (Velocities in drone frame)') 205 | print('-----------------------------------') 206 | mprint(u) 207 | print() 208 | mprint(v) 209 | print() 210 | mprint(w) 211 | print() 212 | 213 | # uFlat, vFlat, wFlat are the drone's velocities in the inertial frame, expressed in a frame 214 | # parallel to the ground, but with the drone's heading. 215 | # These calculations are not relevant to the ODE, but might be used for control. 216 | uFlat = dot(Bcm.vel(N).subs(kdd), cross(B.y, N.z)).simplify() 217 | vFlat = dot(Bcm.vel(N).subs(kdd), -cross(B.x, N.z)).simplify() 218 | wFlat = dot(Bcm.vel(N).subs(kdd), N.z).simplify() 219 | print('uFlat, vFlat, wFlat (Velocities in drone frame (after Yaw))') 220 | print('-----------------------------------------------------------') 221 | mprint(uFlat) 222 | print() 223 | mprint(vFlat) 224 | print() 225 | mprint(wFlat) 226 | print() 227 | -------------------------------------------------------------------------------- /PyDy Scripts/02 - Added Wind and Aero Drag/Quad_3D_frd_NED_Quat.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | author: John Bass 4 | email: john.bobzwik@gmail.com 5 | license: MIT 6 | Please feel free to use and modify this, but keep the above information. Thanks! 7 | """ 8 | 9 | """ 10 | Using PyDy and Sympy, this script generates the equations for the state derivatives 11 | of a quadcopter in a 3-dimensional space. The states in this particular script are: 12 | 13 | x, y, z : Position of the drone's center of mass in the inertial frame, 14 | expressed in the inertial frame 15 | xdot, ydot, zdot : Velocity of the drone's center of mass in the inertial frame, 16 | expressed in the inertial frame 17 | q0, q1, q2, q3 : Orientation of the drone in the inertial frame using quaternions 18 | p, q, r : Angular velocity of the drone in the inertial frame, 19 | expressed in the drone's frame 20 | 21 | Important note : This script uses a frd body orientation (front-right-down) and 22 | a NED world orientation (North-East-Down). The drone's altitude is -z. 23 | 24 | Other note : In the resulting state derivatives, there are still simplifications 25 | that can be made that SymPy cannot simplify (factoring). 26 | 27 | author: John Bass 28 | email: 29 | """ 30 | 31 | from sympy import symbols, Matrix, sign 32 | from sympy.physics.mechanics import * 33 | 34 | # Reference frames and Points 35 | # --------------------------- 36 | N = ReferenceFrame('N') # Inertial Frame 37 | B = ReferenceFrame('B') # Drone after X (roll) rotation (Final rotation) 38 | W = ReferenceFrame('W') # Wind reference frame 39 | 40 | No = Point('No') 41 | Bcm = Point('Bcm') # Drone's center of mass 42 | M1 = Point('M1') # Motor 1 is front left, then the rest increments CW (Drone is in X configuration, not +) 43 | M2 = Point('M2') 44 | M3 = Point('M3') 45 | M4 = Point('M4') 46 | Wo = Point('Wo') # Arbitrary point in the Wind Frame 47 | 48 | # Variables 49 | # --------------------------- 50 | # x, y and z are the drone's coordinates in the inertial frame, expressed with the inertial frame 51 | # xdot, ydot and zdot are the drone's speeds in the inertial frame, expressed with the inertial frame 52 | # q0, q1, q2, q3 represents the drone's orientation in the inertial frame, using quaternions 53 | # p, q and r are the drone's angular velocities in the inertial frame, expressed with the drone's frame 54 | 55 | x, y, z, xdot, ydot, zdot = dynamicsymbols('x y z xdot ydot zdot') 56 | q0, q1, q2, q3, p, q, r = dynamicsymbols('q0 q1 q2 q3 p q r') 57 | 58 | # First derivatives of the variables 59 | xd, yd, zd, xdotd, ydotd, zdotd = dynamicsymbols('x y z xdot ydot zdot', 1) 60 | q0d, q1d, q2d, q3d, pd, qd, rd = dynamicsymbols('q0 q1 q2 q3 p q r', 1) 61 | 62 | # Constants 63 | # --------------------------- 64 | mB, g, dxm, dym, dzm, IBxx, IByy, IBzz, IRzz, wM1, wM2, wM3, wM4 = symbols('mB g dxm dym dzm IBxx IByy IBzz IRzz wM1 wM2 wM3 wM4') 65 | ThrM1, ThrM2, ThrM3, ThrM4, TorM1, TorM2, TorM3, TorM4 = symbols('ThrM1 ThrM2 ThrM3 ThrM4 TorM1 TorM2 TorM3 TorM4') 66 | qW1, qW2, velW, Cd = symbols('qW1 qW2 velW Cd') 67 | 68 | # Rotation Quaternion 69 | # --------------------------- 70 | B.orient(N, 'Quaternion', [q0, q1, q2, q3]) 71 | B.set_ang_vel(N, p*B.x + q*B.y + r*B.z) 72 | 73 | # Origin 74 | # --------------------------- 75 | No.set_vel(N, 0) 76 | 77 | # Translation 78 | # --------------------------- 79 | Bcm.set_pos(No, x*N.x + y*N.y + z*N.z) 80 | Bcm.set_vel(N, Bcm.pos_from(No).dt(N)) 81 | 82 | # Motor placement 83 | # M1 is front left, then clockwise numbering 84 | # dzm is positive for motors above center of mass 85 | # --------------------------- 86 | M1.set_pos(Bcm, dxm*B.x - dym*B.y - dzm*B.z) 87 | M2.set_pos(Bcm, dxm*B.x + dym*B.y - dzm*B.z) 88 | M3.set_pos(Bcm, -dxm*B.x + dym*B.y - dzm*B.z) 89 | M4.set_pos(Bcm, -dxm*B.x - dym*B.y - dzm*B.z) 90 | M1.v2pt_theory(Bcm, N, B) 91 | M2.v2pt_theory(Bcm, N, B) 92 | M3.v2pt_theory(Bcm, N, B) 93 | M4.v2pt_theory(Bcm, N, B) 94 | 95 | # Inertia Dyadic 96 | # --------------------------- 97 | IB = inertia(B, IBxx, IByy, IBzz) 98 | 99 | # Create Bodies 100 | # --------------------------- 101 | BodyB = RigidBody('BodyB', Bcm, B, mB, (IB, Bcm)) 102 | BodyList = [BodyB] 103 | 104 | # Wind 105 | # --------------------------- 106 | W.orient(N, 'Body', [qW1, qW2, 0], 'ZYX') 107 | # W.orient(N, 'Quaternion', [qW0, qW1, qW2, qW3]) 108 | Wo.set_vel(N, velW*W.x) 109 | airVel = Bcm.vel(N) - Wo.vel(N) 110 | 111 | # Forces and Torques 112 | # --------------------------- 113 | Grav_Force = (Bcm, mB*g*N.z) 114 | FM1 = (M1, -ThrM1*B.z) 115 | FM2 = (M2, -ThrM2*B.z) 116 | FM3 = (M3, -ThrM3*B.z) 117 | FM4 = (M4, -ThrM4*B.z) 118 | 119 | TM1 = (B, -TorM1*B.z) 120 | TM2 = (B, TorM2*B.z) 121 | TM3 = (B, -TorM3*B.z) 122 | TM4 = (B, TorM4*B.z) 123 | 124 | # drag = (Bcm, -Cd*airVel.normalize()*(airVel.magnitude())**2) 125 | dragX = (Bcm, -Cd*sign(dot(airVel, N.x))*(dot(airVel, N.x)**2)*N.x) 126 | dragY = (Bcm, -Cd*sign(dot(airVel, N.y))*(dot(airVel, N.y)**2)*N.y) 127 | dragZ = (Bcm, -Cd*sign(dot(airVel, N.z))*(dot(airVel, N.z)**2)*N.z) 128 | 129 | gyro = (B, -IRzz*cross(B.ang_vel_in(N), (wM1 - wM2 + wM3 - wM4)*B.z)) 130 | 131 | ForceList = [Grav_Force, FM1, FM2, FM3, FM4, TM1, TM2, TM3, TM4, dragX, dragY, dragZ, gyro] 132 | 133 | # Calculate Quaternion Derivative 134 | # --------------------------- 135 | Gquat = Matrix([[-q1, q0, q3, -q2], 136 | [-q2, -q3, q0, q1], 137 | [-q3, q2, -q1, q0]]) 138 | 139 | angVel = Matrix([[dot(B.ang_vel_in(N), B.x)],[dot(B.ang_vel_in(N), B.y)],[dot(B.ang_vel_in(N), B.z)]]) # Basically the same as: angVel = Matrix([[p],[q],[r]]) 140 | quat_dot = 1.0/2*Gquat.T*angVel 141 | 142 | # Kinematic Differential Equations 143 | # --------------------------- 144 | kd = [xdot - xd, ydot - yd, zdot - zd, q0d - quat_dot[0], q1d - quat_dot[1], q2d - quat_dot[2], q3d - quat_dot[3]] 145 | 146 | # Kane's Method 147 | # --------------------------- 148 | KM = KanesMethod(N, q_ind=[x, y, z, q0, q1, q2, q3], u_ind=[xdot, ydot, zdot, p, q, r], kd_eqs=kd) 149 | (fr, frstar) = KM.kanes_equations(BodyList, ForceList) 150 | 151 | # Equations of Motion 152 | # --------------------------- 153 | MM = KM.mass_matrix_full 154 | kdd = KM.kindiffdict() 155 | rhs = KM.forcing_full 156 | MM = MM.subs(kdd) 157 | rhs = rhs.subs(kdd) 158 | 159 | MM.simplify() 160 | # MM = MM.subs(q0**2 + q1**2 + q2**2 + q3**2, 1) 161 | print() 162 | print('Mass Matrix') 163 | print('-----------') 164 | mprint(MM) 165 | 166 | rhs.simplify() 167 | # rhs = rhs.subs(q0**2 + q1**2 + q2**2 + q3**2, 1) 168 | print('Right Hand Side') 169 | print('---------------') 170 | mprint(rhs) 171 | print() 172 | 173 | # So, MM*x = rhs, where x is the State Derivatives 174 | # Solve for x 175 | stateDot = MM.inv()*rhs 176 | print('State Derivatives') 177 | print('-----------------------------------') 178 | mprint(stateDot) 179 | print() 180 | 181 | # POST-PROCESSING 182 | # --------------------------- 183 | 184 | # Useful Numbers 185 | # --------------------------- 186 | # p, q, r are the drone's angular velocities in the inertial frame, expressed in the drone's frame. 187 | # These calculations are not relevant to the ODE, but might be used for control. 188 | quat_dot_2 = Matrix([[q0d],[q1d],[q2d],[q3d]]) 189 | angVel_2 = 2.0*Gquat*quat_dot_2 190 | print('P, Q, R (Angular velocities in drone frame)') 191 | print('-------------------------------------------') 192 | mprint(angVel_2[0]) 193 | print() 194 | mprint(angVel_2[1]) 195 | print() 196 | mprint(angVel_2[2]) 197 | print() 198 | 199 | # u, v, w are the drone's velocities in the inertial frame, expressed in the drone's frame. 200 | # These calculations are not relevant to the ODE, but might be used for control. 201 | u = dot(Bcm.vel(N).subs(kdd), B.x).simplify() 202 | v = dot(Bcm.vel(N).subs(kdd), B.y).simplify() 203 | w = dot(Bcm.vel(N).subs(kdd), B.z).simplify() 204 | print('u, v, w (Velocities in drone frame)') 205 | print('-----------------------------------') 206 | mprint(u) 207 | print() 208 | mprint(v) 209 | print() 210 | mprint(w) 211 | print() 212 | 213 | # uFlat, vFlat, wFlat are the drone's velocities in the inertial frame, expressed in a frame 214 | # parallel to the ground, but with the drone's heading. 215 | # These calculations are not relevant to the ODE, but might be used for control. 216 | uFlat = dot(Bcm.vel(N).subs(kdd), cross(B.y, N.z)).simplify() 217 | vFlat = dot(Bcm.vel(N).subs(kdd), -cross(B.x, N.z)).simplify() 218 | wFlat = dot(Bcm.vel(N).subs(kdd), N.z).simplify() 219 | print('uFlat, vFlat, wFlat (Velocities in drone frame (after Yaw))') 220 | print('-----------------------------------------------------------') 221 | mprint(uFlat) 222 | print() 223 | mprint(vFlat) 224 | print() 225 | mprint(wFlat) 226 | print() 227 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Quadcopter Simulation and Control (Quad_SimCon) 2 | 3 | [Quadcopter Exploration Project](https://github.com/bobzwik/Quad_Exploration) 4 | 5 | This project serves 2 purposes. The first is to provide a PyDy template for creating quadcopter dynamics and outputing the relevant equations of motion. 6 | 7 | The second purpose is to provide a simple working simulation of the quadcopter's dynamics and a simple controller that can handle position control and supports minimum snap (but also minimum velocity, acceleration and jerk) trajectory generation. 8 | 9 |

10 | 11 |

12 | 13 | ## PyDy Quadcopter 14 | 15 | [PyDy](https://pypi.org/project/pydy/), short for Python Dynamics, is a tool kit made to enable the study of multibody dynamics. At it's core is the SymPy [mechanics package](https://docs.sympy.org/latest/modules/physics/mechanics/index.html#vector), which provides an API for building models and generating the symbolic equations of motion for complex multibody systems. 16 | 17 | In the *PyDy Scripts* folder, you will find multiple different scripts. Some express the drone's orientation in the NED frame, and some in the ENU frame. 18 | 19 | __NED frame__ : The world is oriented in such a way that the *X* direction is **North**, *Y* is **East** and *Z* is **Down**. The drone's orientation in this frame is **front-right-down (frd)**. This is a conventional/classic frame used in aeronautics, and also the frame used for the PX4 multicopter controller. 20 | 21 | __ENU frame__ : The world is oriented in such a way that the *X* direction is **East**, *Y* is **North** and *Z* is **Up**. The drone's orientation in this frame is **front-left-up (flu)**. This frame is widely used for its vizualizing simplicity (*z* is up), however it possesses a vizualizing complexity where "pitching up" actually results in a negative pitch angle. 22 | 23 | The other difference in the provided scripts is that some use Euler angles *phi* (*φ*), *theta* (*θ*), *psi* (*ψ*) (roll, pitch, yaw) to describes the drone's orientation, while the other scripts uses a quaternion. 24 | 25 | __Euler angles__ : In the Euler angle scripts, the drone is first rotated about its *Z* axis by a certain yaw angle (heading), then about its new *Y* axis by a certain pitch angle (elevation) and then finaly about its new *X* axis by a certain roll angle (bank). The rotation order is thus a **Body ZYX** rotation. Using Euler angles, the resulting equations of motion possesses many sine and cosine functions, meaning that it requires more time to calculate. One must remember that these equations of motion are to be integrated in order to simulated the quadcopter's motion (using an ODE function for example). This means that the equations of motion are computed many time during a single timestep of the simulation. 26 | 27 | __Quaternion__ : The use of a quaternion to describe the drone's rotation significantly decreases computing time, because of the absence of sine and cosine functions in the equations of motion. The quaternion is formed with the angle value first, followed by the 3 axis values, like so : `q = [q0, q1, q2, q3] = [qw, qx, qy, qz]`. While it is sometimes complex to understand the rotation expressed by a quaternion, the quadcopter attitude control provided in this project uses quaternions (sets a desired quaternion, computes a quaternion error, ... ). 28 | 29 | The quadcopter states are the following : 30 | 31 | * Position (*x*, *y*, *z*) 32 | * Rotation (*φ*, *θ*, *ψ*) or (*q0*, *q1*, *q2*, *q3*) 33 | * Linear Velocity (*x_dot*, *y_dot*, *z_dot*) 34 | * Angular Velocity (*p*, *q*, *r*) (The drone's angular velocity described in its own body frame, also known as *Ω*. This is not equivalent to *phi_dot*, *theta_dot*, *psi_dot*) 35 | 36 | The PyDy scripts use the Kane Method to derive the system's equations and output a Mass Matrix (*MM*) and a right-hand-side vector (*RHS*). These outputs are used to obtain the state derivative vector *s_dot* in the equation `MM*s_dot = RHS`. To solve for *s_dot*, one must first calculate the inverse of the Mass Matrix, to be used in the equation `s_dot = inv(MM)*RHS`. Fortunately, for the quadcopter in this project, the Mass Matrix is a diagonal matrix and inverses quite easily. One can numerically solve for *s_dot* during the integration, but PyDy is able to analytically solve the state derivatives, which then can easily be copied into the ODE function. 37 | 38 | Currently, I have seperated the PyDy scripts into 3 folders. The first is just a basic quadcopter. In the second, I have added gyroscopic precession of the rotors. And in the third, wind and aerodynamic drag was added. 39 | 40 | **NOTE**: In my scripts, Motor 1 is the front left motor, and the rest are numbered clockwise. This is not really conventional, but is simple enough. 41 | 42 | ### PyDy Installation 43 | To be able to run the PyDy scripts of this project, you need to first install PyDy and its dependancies. 44 | 45 | If you have the pip package manager installed you can simply type: 46 | 47 | `$ pip install pydy` 48 | 49 | Or if you have conda you can type: 50 | 51 | `$ conda install -c conda-forge pydy` 52 | 53 | ## Simulation and Control 54 | First off, the world and body orientation can be switch between a NED or ENU frame in the `config.py` file. The other scripts then handle which equations to use, depending on the chosen orientation. It also has to be mentioned that both the PyDy scripts and the simulation aim to simulate the behaviour of a **X configuration** quadcopter (not a **+ configuration**). 55 | 56 | The only packages needed for the simulation part of this project are Numpy and Matplotlib. 57 | 58 | ### Simulation 59 | In `quad.py`, I've defined a Quadcopter Class and its methods are relatively simple : initialize the quadcopter with various parameters and initial conditions, update the states, calculate the state derivatives, and calculate other useful information. The simulation uses a quaternion in the state vector to express the drone's rotation, and the state derivative vector is copied from the corresponding PyDy script. However, 8 other states were added to simulate each motors dynamics ([2nd Order System](https://apmonitor.com/pdc/index.php/Main/SecondOrderSystems)) : 60 | 61 | * Motor Angular Velocities (*wM1*, *wM2*, *wM3*, *wM4*) 62 | * Motor Angular Acceleration (*wdotM1*, *wdotM2*, *wdotM3*, *wdotM4*) 63 | 64 | The current parameters are set to roughly match the characteristics of a DJI F450 that I have in my lab, and the rotor thrust and torque coefficients have been measured. 65 | 66 | ### Trajectory Generation 67 | Different trajectories can be selected, for both position and heading. In `waypoints.py`, you can set the desired position and heading waypoints, and the time for each waypoint. You can select to use each waypoint as a step, or to interpolate between waypoints, or to generate a minimum velocity, acceleration, jerk or snap trajectory. Code from [Peter Huang](https://github.com/hbd730/quadcopter-simulation) was modified to allow for these 4 types of trajectories and to allow for segments between waypoints of different durations. There is also the possibility to have the desired heading follow the direction of the desired velocity. 68 | 69 | ### Control 70 | There are currently 3 controllers coded in this project. One to control XYZ positions, one to control XY velocities and Z position, and one to control XYZ velocities. In all 3 current controllers, it is also possible to set a Yaw angle (heading) setpoint. There are plans to add more ways of controlling the quadcopter. 71 | 72 | The control algorithm is strongly inspired by the PX4 multicopter control algorithm. It is a cascade controller, where the position error (difference between the desired position and the current position) generates a velocity setpoint, the velocity error then creates a desired thrust magnitude and orientation, which is then interpreted as a desired rotation (expressed as a quaternion). The quaternion error then generates angular rate setpoints, which then creates desired moments. The states are controlled using a PID control. Position and Attitude control uses a simple Proportional (P) gain, while Velocity and Rate uses Proportional and Derivative (D) gains. Velocity also has an optional Integral (I) gain if wind is activated in the simulation. 73 | 74 | There are multiple wind models implemented. One were the wind velocity, heading and elevation remain constant, one where they vary using a sine function, and one where they vary using a sine function with a random average value. 75 | 76 | The mixer (not based from PX4) allows to find the exact RPM of each motor given the desired thrust magnitude and desired moments. 77 | 78 | 79 | ### Useful links 80 | * [PX4 "Position and Velocity Control" Source Code](https://github.com/PX4/Firmware/blob/master/src/modules/mc_pos_control/PositionControl.cpp) 81 | * [PX4 "Desired Thrust to Desired Attitude" Source Code](https://github.com/PX4/Firmware/blob/master/src/modules/mc_pos_control/Utility/ControlMath.cpp) 82 | * [PX4 "Attitude Control" Source Code](https://github.com/PX4/Firmware/blob/master/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp) --- [Article](https://www.research-collection.ethz.ch/bitstream/handle/20.500.11850/154099/eth-7387-01.pdf) 83 | * [PX4 "Rate Control" Source Code](https://github.com/PX4/Firmware/blob/master/src/modules/mc_att_control/mc_att_control_main.cpp) 84 | * [Minimum Snap Trajectory](https://github.com/hbd730/quadcopter-simulation) --- [Article](http://www-personal.acfr.usyd.edu.au/spns/cdm/papers/Mellinger.pdf) 85 | 86 | ## Gif Gallery 87 | 88 |

89 | 93 | 94 | 95 | 96 | 97 | 98 | 99 |

100 | 101 | ## To-Do 102 | * Add Perlin noise wind model 103 | * Develop method to find gains for best response 104 | * Add the possibility for more controllers (Attitude/Stabilize and Rate/Acro) 105 | * Add scheduler to simulate and display animation in realtime simultaneously 106 | * Simulate sensors and sensor noise, calculate states from sensor data (Maybe somehow running simultaneously? I'll have to figure out how.) 107 | -------------------------------------------------------------------------------- /Simulation/config.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | author: John Bass 4 | email: john.bobzwik@gmail.com 5 | license: MIT 6 | Please feel free to use and modify this, but keep the above information. Thanks! 7 | """ 8 | 9 | # Select Orientation of Quadcopter and Reference Frame 10 | # --------------------------- 11 | # "NED" for front-right-down (frd) and North-East-Down 12 | # "ENU" for front-left-up (flu) and East-North-Up 13 | orient = "NED" 14 | 15 | # Select whether to use gyroscopic precession of the rotors in the quadcopter dynamics 16 | # --------------------------- 17 | # Set to False if rotor inertia isn't known (gyro precession has negigeable effect on drone dynamics) 18 | usePrecession = bool(False) -------------------------------------------------------------------------------- /Simulation/ctrl.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | author: John Bass 4 | email: john.bobzwik@gmail.com 5 | license: MIT 6 | Please feel free to use and modify this, but keep the above information. Thanks! 7 | """ 8 | 9 | # Position and Velocity Control based on https://github.com/PX4/Firmware/blob/master/src/modules/mc_pos_control/PositionControl.cpp 10 | # Desired Thrust to Desired Attitude based on https://github.com/PX4/Firmware/blob/master/src/modules/mc_pos_control/Utility/ControlMath.cpp 11 | # Attitude Control based on https://github.com/PX4/Firmware/blob/master/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp 12 | # and https://www.research-collection.ethz.ch/bitstream/handle/20.500.11850/154099/eth-7387-01.pdf 13 | # Rate Control based on https://github.com/PX4/Firmware/blob/master/src/modules/mc_att_control/mc_att_control_main.cpp 14 | 15 | import numpy as np 16 | from numpy import pi 17 | from numpy import sin, cos, tan, sqrt 18 | from numpy.linalg import norm 19 | import utils 20 | import config 21 | 22 | rad2deg = 180.0/pi 23 | deg2rad = pi/180.0 24 | 25 | # Set PID Gains and Max Values 26 | # --------------------------- 27 | 28 | # Position P gains 29 | Py = 1.0 30 | Px = Py 31 | Pz = 1.0 32 | 33 | pos_P_gain = np.array([Px, Py, Pz]) 34 | 35 | # Velocity P-D gains 36 | Pxdot = 5.0 37 | Dxdot = 0.5 38 | Ixdot = 5.0 39 | 40 | Pydot = Pxdot 41 | Dydot = Dxdot 42 | Iydot = Ixdot 43 | 44 | Pzdot = 4.0 45 | Dzdot = 0.5 46 | Izdot = 5.0 47 | 48 | vel_P_gain = np.array([Pxdot, Pydot, Pzdot]) 49 | vel_D_gain = np.array([Dxdot, Dydot, Dzdot]) 50 | vel_I_gain = np.array([Ixdot, Iydot, Izdot]) 51 | 52 | # Attitude P gains 53 | Pphi = 8.0 54 | Ptheta = Pphi 55 | Ppsi = 1.5 56 | PpsiStrong = 8 57 | 58 | att_P_gain = np.array([Pphi, Ptheta, Ppsi]) 59 | 60 | # Rate P-D gains 61 | Pp = 1.5 62 | Dp = 0.04 63 | 64 | Pq = Pp 65 | Dq = Dp 66 | 67 | Pr = 1.0 68 | Dr = 0.1 69 | 70 | rate_P_gain = np.array([Pp, Pq, Pr]) 71 | rate_D_gain = np.array([Dp, Dq, Dr]) 72 | 73 | # Max Velocities 74 | uMax = 5.0 75 | vMax = 5.0 76 | wMax = 5.0 77 | 78 | velMax = np.array([uMax, vMax, wMax]) 79 | velMaxAll = 5.0 80 | 81 | saturateVel_separetely = False 82 | 83 | # Max tilt 84 | tiltMax = 50.0*deg2rad 85 | 86 | # Max Rate 87 | pMax = 200.0*deg2rad 88 | qMax = 200.0*deg2rad 89 | rMax = 150.0*deg2rad 90 | 91 | rateMax = np.array([pMax, qMax, rMax]) 92 | 93 | 94 | class Control: 95 | 96 | def __init__(self, quad, yawType): 97 | self.sDesCalc = np.zeros(16) 98 | self.w_cmd = np.ones(4)*quad.params["w_hover"] 99 | self.thr_int = np.zeros(3) 100 | if (yawType == 0): 101 | att_P_gain[2] = 0 102 | self.setYawWeight() 103 | self.pos_sp = np.zeros(3) 104 | self.vel_sp = np.zeros(3) 105 | self.acc_sp = np.zeros(3) 106 | self.thrust_sp = np.zeros(3) 107 | self.eul_sp = np.zeros(3) 108 | self.pqr_sp = np.zeros(3) 109 | self.yawFF = np.zeros(3) 110 | 111 | 112 | def controller(self, traj, quad, sDes, Ts): 113 | 114 | # Desired State (Create a copy, hence the [:]) 115 | # --------------------------- 116 | self.pos_sp[:] = traj.sDes[0:3] 117 | self.vel_sp[:] = traj.sDes[3:6] 118 | self.acc_sp[:] = traj.sDes[6:9] 119 | self.thrust_sp[:] = traj.sDes[9:12] 120 | self.eul_sp[:] = traj.sDes[12:15] 121 | self.pqr_sp[:] = traj.sDes[15:18] 122 | self.yawFF[:] = traj.sDes[18] 123 | 124 | # Select Controller 125 | # --------------------------- 126 | if (traj.ctrlType == "xyz_vel"): 127 | self.saturateVel() 128 | self.z_vel_control(quad, Ts) 129 | self.xy_vel_control(quad, Ts) 130 | self.thrustToAttitude(quad, Ts) 131 | self.attitude_control(quad, Ts) 132 | self.rate_control(quad, Ts) 133 | elif (traj.ctrlType == "xy_vel_z_pos"): 134 | self.z_pos_control(quad, Ts) 135 | self.saturateVel() 136 | self.z_vel_control(quad, Ts) 137 | self.xy_vel_control(quad, Ts) 138 | self.thrustToAttitude(quad, Ts) 139 | self.attitude_control(quad, Ts) 140 | self.rate_control(quad, Ts) 141 | elif (traj.ctrlType == "xyz_pos"): 142 | self.z_pos_control(quad, Ts) 143 | self.xy_pos_control(quad, Ts) 144 | self.saturateVel() 145 | self.z_vel_control(quad, Ts) 146 | self.xy_vel_control(quad, Ts) 147 | self.thrustToAttitude(quad, Ts) 148 | self.attitude_control(quad, Ts) 149 | self.rate_control(quad, Ts) 150 | 151 | # Mixer 152 | # --------------------------- 153 | self.w_cmd = utils.mixerFM(quad, norm(self.thrust_sp), self.rateCtrl) 154 | 155 | # Add calculated Desired States 156 | # --------------------------- 157 | self.sDesCalc[0:3] = self.pos_sp 158 | self.sDesCalc[3:6] = self.vel_sp 159 | self.sDesCalc[6:9] = self.thrust_sp 160 | self.sDesCalc[9:13] = self.qd 161 | self.sDesCalc[13:16] = self.rate_sp 162 | 163 | 164 | def z_pos_control(self, quad, Ts): 165 | 166 | # Z Position Control 167 | # --------------------------- 168 | pos_z_error = self.pos_sp[2] - quad.pos[2] 169 | self.vel_sp[2] += pos_P_gain[2]*pos_z_error 170 | 171 | 172 | def xy_pos_control(self, quad, Ts): 173 | 174 | # XY Position Control 175 | # --------------------------- 176 | pos_xy_error = (self.pos_sp[0:2] - quad.pos[0:2]) 177 | self.vel_sp[0:2] += pos_P_gain[0:2]*pos_xy_error 178 | 179 | 180 | def saturateVel(self): 181 | 182 | # Saturate Velocity Setpoint 183 | # --------------------------- 184 | # Either saturate each velocity axis separately, or total velocity (prefered) 185 | if (saturateVel_separetely): 186 | self.vel_sp = np.clip(self.vel_sp, -velMax, velMax) 187 | else: 188 | totalVel_sp = norm(self.vel_sp) 189 | if (totalVel_sp > velMaxAll): 190 | self.vel_sp = self.vel_sp/totalVel_sp*velMaxAll 191 | 192 | 193 | def z_vel_control(self, quad, Ts): 194 | 195 | # Z Velocity Control (Thrust in D-direction) 196 | # --------------------------- 197 | # Hover thrust (m*g) is sent as a Feed-Forward term, in order to 198 | # allow hover when the position and velocity error are nul 199 | vel_z_error = self.vel_sp[2] - quad.vel[2] 200 | if (config.orient == "NED"): 201 | thrust_z_sp = vel_P_gain[2]*vel_z_error - vel_D_gain[2]*quad.vel_dot[2] + quad.params["mB"]*(self.acc_sp[2] - quad.params["g"]) + self.thr_int[2] 202 | elif (config.orient == "ENU"): 203 | thrust_z_sp = vel_P_gain[2]*vel_z_error - vel_D_gain[2]*quad.vel_dot[2] + quad.params["mB"]*(self.acc_sp[2] + quad.params["g"]) + self.thr_int[2] 204 | 205 | # Get thrust limits 206 | if (config.orient == "NED"): 207 | # The Thrust limits are negated and swapped due to NED-frame 208 | uMax = -quad.params["minThr"] 209 | uMin = -quad.params["maxThr"] 210 | elif (config.orient == "ENU"): 211 | uMax = quad.params["maxThr"] 212 | uMin = quad.params["minThr"] 213 | 214 | # Apply Anti-Windup in D-direction 215 | stop_int_D = (thrust_z_sp >= uMax and vel_z_error >= 0.0) or (thrust_z_sp <= uMin and vel_z_error <= 0.0) 216 | 217 | # Calculate integral part 218 | if not (stop_int_D): 219 | self.thr_int[2] += vel_I_gain[2]*vel_z_error*Ts * quad.params["useIntergral"] 220 | # Limit thrust integral 221 | self.thr_int[2] = min(abs(self.thr_int[2]), quad.params["maxThr"])*np.sign(self.thr_int[2]) 222 | 223 | # Saturate thrust setpoint in D-direction 224 | self.thrust_sp[2] = np.clip(thrust_z_sp, uMin, uMax) 225 | 226 | 227 | def xy_vel_control(self, quad, Ts): 228 | 229 | # XY Velocity Control (Thrust in NE-direction) 230 | # --------------------------- 231 | vel_xy_error = self.vel_sp[0:2] - quad.vel[0:2] 232 | thrust_xy_sp = vel_P_gain[0:2]*vel_xy_error - vel_D_gain[0:2]*quad.vel_dot[0:2] + quad.params["mB"]*(self.acc_sp[0:2]) + self.thr_int[0:2] 233 | 234 | # Max allowed thrust in NE based on tilt and excess thrust 235 | thrust_max_xy_tilt = abs(self.thrust_sp[2])*np.tan(tiltMax) 236 | thrust_max_xy = sqrt(quad.params["maxThr"]**2 - self.thrust_sp[2]**2) 237 | thrust_max_xy = min(thrust_max_xy, thrust_max_xy_tilt) 238 | 239 | # Saturate thrust in NE-direction 240 | self.thrust_sp[0:2] = thrust_xy_sp 241 | if (np.dot(self.thrust_sp[0:2].T, self.thrust_sp[0:2]) > thrust_max_xy**2): 242 | mag = norm(self.thrust_sp[0:2]) 243 | self.thrust_sp[0:2] = thrust_xy_sp/mag*thrust_max_xy 244 | 245 | # Use tracking Anti-Windup for NE-direction: during saturation, the integrator is used to unsaturate the output 246 | # see Anti-Reset Windup for PID controllers, L.Rundqwist, 1990 247 | arw_gain = 2.0/vel_P_gain[0:2] 248 | vel_err_lim = vel_xy_error - (thrust_xy_sp - self.thrust_sp[0:2])*arw_gain 249 | self.thr_int[0:2] += vel_I_gain[0:2]*vel_err_lim*Ts * quad.params["useIntergral"] 250 | 251 | def thrustToAttitude(self, quad, Ts): 252 | 253 | # Create Full Desired Quaternion Based on Thrust Setpoint and Desired Yaw Angle 254 | # --------------------------- 255 | yaw_sp = self.eul_sp[2] 256 | 257 | # Desired body_z axis direction 258 | body_z = -utils.vectNormalize(self.thrust_sp) 259 | if (config.orient == "ENU"): 260 | body_z = -body_z 261 | 262 | # Vector of desired Yaw direction in XY plane, rotated by pi/2 (fake body_y axis) 263 | y_C = np.array([-sin(yaw_sp), cos(yaw_sp), 0.0]) 264 | 265 | # Desired body_x axis direction 266 | body_x = np.cross(y_C, body_z) 267 | body_x = utils.vectNormalize(body_x) 268 | 269 | # Desired body_y axis direction 270 | body_y = np.cross(body_z, body_x) 271 | 272 | # Desired rotation matrix 273 | R_sp = np.array([body_x, body_y, body_z]).T 274 | 275 | # Full desired quaternion (full because it considers the desired Yaw angle) 276 | self.qd_full = utils.RotToQuat(R_sp) 277 | 278 | 279 | def attitude_control(self, quad, Ts): 280 | 281 | # Current thrust orientation e_z and desired thrust orientation e_z_d 282 | e_z = quad.dcm[:,2] 283 | e_z_d = -utils.vectNormalize(self.thrust_sp) 284 | if (config.orient == "ENU"): 285 | e_z_d = -e_z_d 286 | 287 | # Quaternion error between the 2 vectors 288 | qe_red = np.zeros(4) 289 | qe_red[0] = np.dot(e_z, e_z_d) + sqrt(norm(e_z)**2 * norm(e_z_d)**2) 290 | qe_red[1:4] = np.cross(e_z, e_z_d) 291 | qe_red = utils.vectNormalize(qe_red) 292 | 293 | # Reduced desired quaternion (reduced because it doesn't consider the desired Yaw angle) 294 | self.qd_red = utils.quatMultiply(qe_red, quad.quat) 295 | 296 | # Mixed desired quaternion (between reduced and full) and resulting desired quaternion qd 297 | q_mix = utils.quatMultiply(utils.inverse(self.qd_red), self.qd_full) 298 | q_mix = q_mix*np.sign(q_mix[0]) 299 | q_mix[0] = np.clip(q_mix[0], -1.0, 1.0) 300 | q_mix[3] = np.clip(q_mix[3], -1.0, 1.0) 301 | self.qd = utils.quatMultiply(self.qd_red, np.array([cos(self.yaw_w*np.arccos(q_mix[0])), 0, 0, sin(self.yaw_w*np.arcsin(q_mix[3]))])) 302 | 303 | # Resulting error quaternion 304 | self.qe = utils.quatMultiply(utils.inverse(quad.quat), self.qd) 305 | 306 | # Create rate setpoint from quaternion error 307 | self.rate_sp = (2.0*np.sign(self.qe[0])*self.qe[1:4])*att_P_gain 308 | 309 | # Limit yawFF 310 | self.yawFF = np.clip(self.yawFF, -rateMax[2], rateMax[2]) 311 | 312 | # Add Yaw rate feed-forward 313 | self.rate_sp += utils.quat2Dcm(utils.inverse(quad.quat))[:,2]*self.yawFF 314 | 315 | # Limit rate setpoint 316 | self.rate_sp = np.clip(self.rate_sp, -rateMax, rateMax) 317 | 318 | 319 | def rate_control(self, quad, Ts): 320 | 321 | # Rate Control 322 | # --------------------------- 323 | rate_error = self.rate_sp - quad.omega 324 | self.rateCtrl = rate_P_gain*rate_error - rate_D_gain*quad.omega_dot # Be sure it is right sign for the D part 325 | 326 | 327 | def setYawWeight(self): 328 | 329 | # Calculate weight of the Yaw control gain 330 | roll_pitch_gain = 0.5*(att_P_gain[0] + att_P_gain[1]) 331 | self.yaw_w = np.clip(att_P_gain[2]/roll_pitch_gain, 0.0, 1.0) 332 | 333 | att_P_gain[2] = roll_pitch_gain 334 | 335 | -------------------------------------------------------------------------------- /Simulation/quadFiles/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bobzwik/Quadcopter_SimCon/428ebd570789278da28777297549c660cae1a0f9/Simulation/quadFiles/__init__.py -------------------------------------------------------------------------------- /Simulation/quadFiles/initQuad.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | author: John Bass 4 | email: john.bobzwik@gmail.com 5 | license: MIT 6 | Please feel free to use and modify this, but keep the above information. Thanks! 7 | """ 8 | 9 | import numpy as np 10 | from numpy import pi 11 | from numpy.linalg import inv 12 | import utils 13 | import config 14 | 15 | 16 | def sys_params(): 17 | mB = 1.2 # mass (kg) 18 | g = 9.81 # gravity (m/s/s) 19 | dxm = 0.16 # arm length (m) 20 | dym = 0.16 # arm length (m) 21 | dzm = 0.05 # motor height (m) 22 | IB = np.array([[0.0123, 0, 0 ], 23 | [0, 0.0123, 0 ], 24 | [0, 0, 0.0224]]) # Inertial tensor (kg*m^2) 25 | IRzz = 2.7e-5 # Rotor moment of inertia (kg*m^2) 26 | 27 | 28 | params = {} 29 | params["mB"] = mB 30 | params["g"] = g 31 | params["dxm"] = dxm 32 | params["dym"] = dym 33 | params["dzm"] = dzm 34 | params["IB"] = IB 35 | params["invI"] = inv(IB) 36 | params["IRzz"] = IRzz 37 | params["useIntergral"] = bool(False) # Include integral gains in linear velocity control 38 | # params["interpYaw"] = bool(False) # Interpolate Yaw setpoints in waypoint trajectory 39 | 40 | params["Cd"] = 0.1 41 | params["kTh"] = 1.076e-5 # thrust coeff (N/(rad/s)^2) (1.18e-7 N/RPM^2) 42 | params["kTo"] = 1.632e-7 # torque coeff (Nm/(rad/s)^2) (1.79e-9 Nm/RPM^2) 43 | params["mixerFM"] = makeMixerFM(params) # Make mixer that calculated Thrust (F) and moments (M) as a function on motor speeds 44 | params["mixerFMinv"] = inv(params["mixerFM"]) 45 | params["minThr"] = 0.1*4 # Minimum total thrust 46 | params["maxThr"] = 9.18*4 # Maximum total thrust 47 | params["minWmotor"] = 75 # Minimum motor rotation speed (rad/s) 48 | params["maxWmotor"] = 925 # Maximum motor rotation speed (rad/s) 49 | params["tau"] = 0.015 # Value for second order system for Motor dynamics 50 | params["kp"] = 1.0 # Value for second order system for Motor dynamics 51 | params["damp"] = 1.0 # Value for second order system for Motor dynamics 52 | 53 | params["motorc1"] = 8.49 # w (rad/s) = cmd*c1 + c0 (cmd in %) 54 | params["motorc0"] = 74.7 55 | params["motordeadband"] = 1 56 | # params["ifexpo"] = bool(False) 57 | # if params["ifexpo"]: 58 | # params["maxCmd"] = 100 # cmd (%) min and max 59 | # params["minCmd"] = 0.01 60 | # else: 61 | # params["maxCmd"] = 100 62 | # params["minCmd"] = 1 63 | 64 | return params 65 | 66 | def makeMixerFM(params): 67 | dxm = params["dxm"] 68 | dym = params["dym"] 69 | kTh = params["kTh"] 70 | kTo = params["kTo"] 71 | 72 | # Motor 1 is front left, then clockwise numbering. 73 | # A mixer like this one allows to find the exact RPM of each motor 74 | # given a desired thrust and desired moments. 75 | # Inspiration for this mixer (or coefficient matrix) and how it is used : 76 | # https://link.springer.com/article/10.1007/s13369-017-2433-2 (https://sci-hub.tw/10.1007/s13369-017-2433-2) 77 | if (config.orient == "NED"): 78 | mixerFM = np.array([[ kTh, kTh, kTh, kTh], 79 | [dym*kTh, -dym*kTh, -dym*kTh, dym*kTh], 80 | [dxm*kTh, dxm*kTh, -dxm*kTh, -dxm*kTh], 81 | [ -kTo, kTo, -kTo, kTo]]) 82 | elif (config.orient == "ENU"): 83 | mixerFM = np.array([[ kTh, kTh, kTh, kTh], 84 | [ dym*kTh, -dym*kTh, -dym*kTh, dym*kTh], 85 | [-dxm*kTh, -dxm*kTh, dxm*kTh, dxm*kTh], 86 | [ kTo, -kTo, kTo, -kTo]]) 87 | 88 | 89 | return mixerFM 90 | 91 | def init_cmd(params): 92 | mB = params["mB"] 93 | g = params["g"] 94 | kTh = params["kTh"] 95 | kTo = params["kTo"] 96 | c1 = params["motorc1"] 97 | c0 = params["motorc0"] 98 | 99 | # w = cmd*c1 + c0 and m*g/4 = kTh*w^2 and torque = kTo*w^2 100 | thr_hover = mB*g/4.0 101 | w_hover = np.sqrt(thr_hover/kTh) 102 | tor_hover = kTo*w_hover*w_hover 103 | cmd_hover = (w_hover-c0)/c1 104 | return [cmd_hover, w_hover, thr_hover, tor_hover] 105 | 106 | def init_state(params): 107 | 108 | x0 = 0. # m 109 | y0 = 0. # m 110 | z0 = 0. # m 111 | phi0 = 0. # rad 112 | theta0 = 0. # rad 113 | psi0 = 0. # rad 114 | 115 | quat = utils.YPRToQuat(psi0, theta0, phi0) 116 | 117 | if (config.orient == "ENU"): 118 | z0 = -z0 119 | 120 | s = np.zeros(21) 121 | s[0] = x0 # x 122 | s[1] = y0 # y 123 | s[2] = z0 # z 124 | s[3] = quat[0] # q0 125 | s[4] = quat[1] # q1 126 | s[5] = quat[2] # q2 127 | s[6] = quat[3] # q3 128 | s[7] = 0. # xdot 129 | s[8] = 0. # ydot 130 | s[9] = 0. # zdot 131 | s[10] = 0. # p 132 | s[11] = 0. # q 133 | s[12] = 0. # r 134 | 135 | w_hover = params["w_hover"] # Hovering motor speed 136 | wdot_hover = 0. # Hovering motor acc 137 | 138 | s[13] = w_hover 139 | s[14] = wdot_hover 140 | s[15] = w_hover 141 | s[16] = wdot_hover 142 | s[17] = w_hover 143 | s[18] = wdot_hover 144 | s[19] = w_hover 145 | s[20] = wdot_hover 146 | 147 | return s -------------------------------------------------------------------------------- /Simulation/quadFiles/quad.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | author: John Bass 4 | email: john.bobzwik@gmail.com 5 | license: MIT 6 | Please feel free to use and modify this, but keep the above information. Thanks! 7 | """ 8 | 9 | import numpy as np 10 | from numpy import sin, cos, tan, pi, sign 11 | from scipy.integrate import ode 12 | 13 | from quadFiles.initQuad import sys_params, init_cmd, init_state 14 | import utils 15 | import config 16 | 17 | deg2rad = pi/180.0 18 | 19 | class Quadcopter: 20 | 21 | def __init__(self, Ti): 22 | 23 | # Quad Params 24 | # --------------------------- 25 | self.params = sys_params() 26 | 27 | # Command for initial stable hover 28 | # --------------------------- 29 | ini_hover = init_cmd(self.params) 30 | self.params["FF"] = ini_hover[0] # Feed-Forward Command for Hover 31 | self.params["w_hover"] = ini_hover[1] # Motor Speed for Hover 32 | self.params["thr_hover"] = ini_hover[2] # Motor Thrust for Hover 33 | self.thr = np.ones(4)*ini_hover[2] 34 | self.tor = np.ones(4)*ini_hover[3] 35 | 36 | # Initial State 37 | # --------------------------- 38 | self.state = init_state(self.params) 39 | 40 | self.pos = self.state[0:3] 41 | self.quat = self.state[3:7] 42 | self.vel = self.state[7:10] 43 | self.omega = self.state[10:13] 44 | self.wMotor = np.array([self.state[13], self.state[15], self.state[17], self.state[19]]) 45 | self.vel_dot = np.zeros(3) 46 | self.omega_dot = np.zeros(3) 47 | self.acc = np.zeros(3) 48 | 49 | self.extended_state() 50 | self.forces() 51 | 52 | # Set Integrator 53 | # --------------------------- 54 | self.integrator = ode(self.state_dot).set_integrator('dopri5', first_step='0.00005', atol='10e-6', rtol='10e-6') 55 | self.integrator.set_initial_value(self.state, Ti) 56 | 57 | 58 | def extended_state(self): 59 | 60 | # Rotation Matrix of current state (Direct Cosine Matrix) 61 | self.dcm = utils.quat2Dcm(self.quat) 62 | 63 | # Euler angles of current state 64 | YPR = utils.quatToYPR_ZYX(self.quat) 65 | self.euler = YPR[::-1] # flip YPR so that euler state = phi, theta, psi 66 | self.psi = YPR[0] 67 | self.theta = YPR[1] 68 | self.phi = YPR[2] 69 | 70 | 71 | def forces(self): 72 | 73 | # Rotor thrusts and torques 74 | self.thr = self.params["kTh"]*self.wMotor*self.wMotor 75 | self.tor = self.params["kTo"]*self.wMotor*self.wMotor 76 | 77 | def state_dot(self, t, state, cmd, wind): 78 | 79 | # Import Params 80 | # --------------------------- 81 | mB = self.params["mB"] 82 | g = self.params["g"] 83 | dxm = self.params["dxm"] 84 | dym = self.params["dym"] 85 | IB = self.params["IB"] 86 | IBxx = IB[0,0] 87 | IByy = IB[1,1] 88 | IBzz = IB[2,2] 89 | Cd = self.params["Cd"] 90 | 91 | kTh = self.params["kTh"] 92 | kTo = self.params["kTo"] 93 | tau = self.params["tau"] 94 | kp = self.params["kp"] 95 | damp = self.params["damp"] 96 | minWmotor = self.params["minWmotor"] 97 | maxWmotor = self.params["maxWmotor"] 98 | 99 | IRzz = self.params["IRzz"] 100 | if (config.usePrecession): 101 | uP = 1 102 | else: 103 | uP = 0 104 | 105 | # Import State Vector 106 | # --------------------------- 107 | x = state[0] 108 | y = state[1] 109 | z = state[2] 110 | q0 = state[3] 111 | q1 = state[4] 112 | q2 = state[5] 113 | q3 = state[6] 114 | xdot = state[7] 115 | ydot = state[8] 116 | zdot = state[9] 117 | p = state[10] 118 | q = state[11] 119 | r = state[12] 120 | wM1 = state[13] 121 | wdotM1 = state[14] 122 | wM2 = state[15] 123 | wdotM2 = state[16] 124 | wM3 = state[17] 125 | wdotM3 = state[18] 126 | wM4 = state[19] 127 | wdotM4 = state[20] 128 | 129 | # Motor Dynamics and Rotor forces (Second Order System: https://apmonitor.com/pdc/index.php/Main/SecondOrderSystems) 130 | # --------------------------- 131 | 132 | uMotor = cmd 133 | wddotM1 = (-2.0*damp*tau*wdotM1 - wM1 + kp*uMotor[0])/(tau**2) 134 | wddotM2 = (-2.0*damp*tau*wdotM2 - wM2 + kp*uMotor[1])/(tau**2) 135 | wddotM3 = (-2.0*damp*tau*wdotM3 - wM3 + kp*uMotor[2])/(tau**2) 136 | wddotM4 = (-2.0*damp*tau*wdotM4 - wM4 + kp*uMotor[3])/(tau**2) 137 | 138 | wMotor = np.array([wM1, wM2, wM3, wM4]) 139 | wMotor = np.clip(wMotor, minWmotor, maxWmotor) 140 | thrust = kTh*wMotor*wMotor 141 | torque = kTo*wMotor*wMotor 142 | 143 | ThrM1 = thrust[0] 144 | ThrM2 = thrust[1] 145 | ThrM3 = thrust[2] 146 | ThrM4 = thrust[3] 147 | TorM1 = torque[0] 148 | TorM2 = torque[1] 149 | TorM3 = torque[2] 150 | TorM4 = torque[3] 151 | 152 | # Wind Model 153 | # --------------------------- 154 | [velW, qW1, qW2] = wind.randomWind(t) 155 | # velW = 0 156 | 157 | # velW = 5 # m/s 158 | # qW1 = 0*deg2rad # Wind heading 159 | # qW2 = 60*deg2rad # Wind elevation (positive = upwards wind in NED, positive = downwards wind in ENU) 160 | 161 | # State Derivatives (from PyDy) This is already the analytically solved vector of MM*x = RHS 162 | # --------------------------- 163 | if (config.orient == "NED"): 164 | DynamicsDot = np.array([ 165 | [ xdot], 166 | [ ydot], 167 | [ zdot], 168 | [ -0.5*p*q1 - 0.5*q*q2 - 0.5*q3*r], 169 | [ 0.5*p*q0 - 0.5*q*q3 + 0.5*q2*r], 170 | [ 0.5*p*q3 + 0.5*q*q0 - 0.5*q1*r], 171 | [ -0.5*p*q2 + 0.5*q*q1 + 0.5*q0*r], 172 | [ (Cd*sign(velW*cos(qW1)*cos(qW2) - xdot)*(velW*cos(qW1)*cos(qW2) - xdot)**2 - 2*(q0*q2 + q1*q3)*(ThrM1 + ThrM2 + ThrM3 + ThrM4))/mB], 173 | [ (Cd*sign(velW*sin(qW1)*cos(qW2) - ydot)*(velW*sin(qW1)*cos(qW2) - ydot)**2 + 2*(q0*q1 - q2*q3)*(ThrM1 + ThrM2 + ThrM3 + ThrM4))/mB], 174 | [ (-Cd*sign(velW*sin(qW2) + zdot)*(velW*sin(qW2) + zdot)**2 - (ThrM1 + ThrM2 + ThrM3 + ThrM4)*(q0**2 - q1**2 - q2**2 + q3**2) + g*mB)/mB], 175 | [ ((IByy - IBzz)*q*r - uP*IRzz*(wM1 - wM2 + wM3 - wM4)*q + ( ThrM1 - ThrM2 - ThrM3 + ThrM4)*dym)/IBxx], # uP activates or deactivates the use of gyroscopic precession. 176 | [ ((IBzz - IBxx)*p*r + uP*IRzz*(wM1 - wM2 + wM3 - wM4)*p + ( ThrM1 + ThrM2 - ThrM3 - ThrM4)*dxm)/IByy], # Set uP to False if rotor inertia is not known (gyro precession has negigeable effect on drone dynamics) 177 | [ ((IBxx - IByy)*p*q - TorM1 + TorM2 - TorM3 + TorM4)/IBzz]]) 178 | elif (config.orient == "ENU"): 179 | DynamicsDot = np.array([ 180 | [ xdot], 181 | [ ydot], 182 | [ zdot], 183 | [ -0.5*p*q1 - 0.5*q*q2 - 0.5*q3*r], 184 | [ 0.5*p*q0 - 0.5*q*q3 + 0.5*q2*r], 185 | [ 0.5*p*q3 + 0.5*q*q0 - 0.5*q1*r], 186 | [ -0.5*p*q2 + 0.5*q*q1 + 0.5*q0*r], 187 | [ (Cd*sign(velW*cos(qW1)*cos(qW2) - xdot)*(velW*cos(qW1)*cos(qW2) - xdot)**2 + 2*(q0*q2 + q1*q3)*(ThrM1 + ThrM2 + ThrM3 + ThrM4))/mB], 188 | [ (Cd*sign(velW*sin(qW1)*cos(qW2) - ydot)*(velW*sin(qW1)*cos(qW2) - ydot)**2 - 2*(q0*q1 - q2*q3)*(ThrM1 + ThrM2 + ThrM3 + ThrM4))/mB], 189 | [ (-Cd*sign(velW*sin(qW2) + zdot)*(velW*sin(qW2) + zdot)**2 + (ThrM1 + ThrM2 + ThrM3 + ThrM4)*(q0**2 - q1**2 - q2**2 + q3**2) - g*mB)/mB], 190 | [ ((IByy - IBzz)*q*r + uP*IRzz*(wM1 - wM2 + wM3 - wM4)*q + ( ThrM1 - ThrM2 - ThrM3 + ThrM4)*dym)/IBxx], # uP activates or deactivates the use of gyroscopic precession. 191 | [ ((IBzz - IBxx)*p*r - uP*IRzz*(wM1 - wM2 + wM3 - wM4)*p + (-ThrM1 - ThrM2 + ThrM3 + ThrM4)*dxm)/IByy], # Set uP to False if rotor inertia is not known (gyro precession has negigeable effect on drone dynamics) 192 | [ ((IBxx - IBzz)*p*q + TorM1 - TorM2 + TorM3 - TorM4)/IBzz]]) 193 | 194 | 195 | # State Derivative Vector 196 | # --------------------------- 197 | sdot = np.zeros([21]) 198 | sdot[0] = DynamicsDot[0] 199 | sdot[1] = DynamicsDot[1] 200 | sdot[2] = DynamicsDot[2] 201 | sdot[3] = DynamicsDot[3] 202 | sdot[4] = DynamicsDot[4] 203 | sdot[5] = DynamicsDot[5] 204 | sdot[6] = DynamicsDot[6] 205 | sdot[7] = DynamicsDot[7] 206 | sdot[8] = DynamicsDot[8] 207 | sdot[9] = DynamicsDot[9] 208 | sdot[10] = DynamicsDot[10] 209 | sdot[11] = DynamicsDot[11] 210 | sdot[12] = DynamicsDot[12] 211 | sdot[13] = wdotM1 212 | sdot[14] = wddotM1 213 | sdot[15] = wdotM2 214 | sdot[16] = wddotM2 215 | sdot[17] = wdotM3 216 | sdot[18] = wddotM3 217 | sdot[19] = wdotM4 218 | sdot[20] = wddotM4 219 | 220 | self.acc = sdot[7:10] 221 | 222 | return sdot 223 | 224 | def update(self, t, Ts, cmd, wind): 225 | 226 | prev_vel = self.vel 227 | prev_omega = self.omega 228 | 229 | self.integrator.set_f_params(cmd, wind) 230 | self.state = self.integrator.integrate(t, t+Ts) 231 | 232 | self.pos = self.state[0:3] 233 | self.quat = self.state[3:7] 234 | self.vel = self.state[7:10] 235 | self.omega = self.state[10:13] 236 | self.wMotor = np.array([self.state[13], self.state[15], self.state[17], self.state[19]]) 237 | 238 | self.vel_dot = (self.vel - prev_vel)/Ts 239 | self.omega_dot = (self.omega - prev_omega)/Ts 240 | 241 | self.extended_state() 242 | self.forces() 243 | -------------------------------------------------------------------------------- /Simulation/run_3D_simulation.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | author: John Bass 4 | email: john.bobzwik@gmail.com 5 | license: MIT 6 | Please feel free to use and modify this, but keep the above information. Thanks! 7 | """ 8 | 9 | import numpy as np 10 | import matplotlib.pyplot as plt 11 | import time 12 | import cProfile 13 | 14 | from trajectory import Trajectory 15 | from ctrl import Control 16 | from quadFiles.quad import Quadcopter 17 | from utils.windModel import Wind 18 | import utils 19 | import config 20 | 21 | def quad_sim(t, Ts, quad, ctrl, wind, traj): 22 | 23 | # Dynamics (using last timestep's commands) 24 | # --------------------------- 25 | quad.update(t, Ts, ctrl.w_cmd, wind) 26 | t += Ts 27 | 28 | # Trajectory for Desired States 29 | # --------------------------- 30 | sDes = traj.desiredState(t, Ts, quad) 31 | 32 | # Generate Commands (for next iteration) 33 | # --------------------------- 34 | ctrl.controller(traj, quad, sDes, Ts) 35 | 36 | return t 37 | 38 | 39 | def main(): 40 | start_time = time.time() 41 | 42 | # Simulation Setup 43 | # --------------------------- 44 | Ti = 0 45 | Ts = 0.005 46 | Tf = 20 47 | ifsave = 0 48 | 49 | # Choose trajectory settings 50 | # --------------------------- 51 | ctrlOptions = ["xyz_pos", "xy_vel_z_pos", "xyz_vel"] 52 | trajSelect = np.zeros(3) 53 | 54 | # Select Control Type (0: xyz_pos, 1: xy_vel_z_pos, 2: xyz_vel) 55 | ctrlType = ctrlOptions[0] 56 | # Select Position Trajectory Type (0: hover, 1: pos_waypoint_timed, 2: pos_waypoint_interp, 57 | # 3: minimum velocity 4: minimum accel, 5: minimum jerk, 6: minimum snap 58 | # 7: minimum accel_stop 8: minimum jerk_stop 9: minimum snap_stop 59 | # 10: minimum jerk_full_stop 11: minimum snap_full_stop 60 | # 12: pos_waypoint_arrived 13: pos_waypoint_arrived_wait 61 | trajSelect[0] = 5 62 | # Select Yaw Trajectory Type (0: none 1: yaw_waypoint_timed, 2: yaw_waypoint_interp 3: follow 4: zero) 63 | trajSelect[1] = 3 64 | # Select if waypoint time is used, or if average speed is used to calculate waypoint time (0: waypoint time, 1: average speed) 65 | trajSelect[2] = 1 66 | print("Control type: {}".format(ctrlType)) 67 | 68 | # Initialize Quadcopter, Controller, Wind, Result Matrixes 69 | # --------------------------- 70 | quad = Quadcopter(Ti) 71 | traj = Trajectory(quad, ctrlType, trajSelect) 72 | ctrl = Control(quad, traj.yawType) 73 | wind = Wind('None', 2.0, 90, -15) 74 | 75 | # Trajectory for First Desired States 76 | # --------------------------- 77 | sDes = traj.desiredState(0, Ts, quad) 78 | 79 | # Generate First Commands 80 | # --------------------------- 81 | ctrl.controller(traj, quad, sDes, Ts) 82 | 83 | # Initialize Result Matrixes 84 | # --------------------------- 85 | numTimeStep = int(Tf/Ts+1) 86 | 87 | t_all = np.zeros(numTimeStep) 88 | s_all = np.zeros([numTimeStep, len(quad.state)]) 89 | pos_all = np.zeros([numTimeStep, len(quad.pos)]) 90 | vel_all = np.zeros([numTimeStep, len(quad.vel)]) 91 | quat_all = np.zeros([numTimeStep, len(quad.quat)]) 92 | omega_all = np.zeros([numTimeStep, len(quad.omega)]) 93 | euler_all = np.zeros([numTimeStep, len(quad.euler)]) 94 | sDes_traj_all = np.zeros([numTimeStep, len(traj.sDes)]) 95 | sDes_calc_all = np.zeros([numTimeStep, len(ctrl.sDesCalc)]) 96 | w_cmd_all = np.zeros([numTimeStep, len(ctrl.w_cmd)]) 97 | wMotor_all = np.zeros([numTimeStep, len(quad.wMotor)]) 98 | thr_all = np.zeros([numTimeStep, len(quad.thr)]) 99 | tor_all = np.zeros([numTimeStep, len(quad.tor)]) 100 | 101 | t_all[0] = Ti 102 | s_all[0,:] = quad.state 103 | pos_all[0,:] = quad.pos 104 | vel_all[0,:] = quad.vel 105 | quat_all[0,:] = quad.quat 106 | omega_all[0,:] = quad.omega 107 | euler_all[0,:] = quad.euler 108 | sDes_traj_all[0,:] = traj.sDes 109 | sDes_calc_all[0,:] = ctrl.sDesCalc 110 | w_cmd_all[0,:] = ctrl.w_cmd 111 | wMotor_all[0,:] = quad.wMotor 112 | thr_all[0,:] = quad.thr 113 | tor_all[0,:] = quad.tor 114 | 115 | # Run Simulation 116 | # --------------------------- 117 | t = Ti 118 | i = 1 119 | while round(t,3) < Tf: 120 | 121 | t = quad_sim(t, Ts, quad, ctrl, wind, traj) 122 | 123 | # print("{:.3f}".format(t)) 124 | t_all[i] = t 125 | s_all[i,:] = quad.state 126 | pos_all[i,:] = quad.pos 127 | vel_all[i,:] = quad.vel 128 | quat_all[i,:] = quad.quat 129 | omega_all[i,:] = quad.omega 130 | euler_all[i,:] = quad.euler 131 | sDes_traj_all[i,:] = traj.sDes 132 | sDes_calc_all[i,:] = ctrl.sDesCalc 133 | w_cmd_all[i,:] = ctrl.w_cmd 134 | wMotor_all[i,:] = quad.wMotor 135 | thr_all[i,:] = quad.thr 136 | tor_all[i,:] = quad.tor 137 | 138 | i += 1 139 | 140 | end_time = time.time() 141 | print("Simulated {:.2f}s in {:.6f}s.".format(t, end_time - start_time)) 142 | 143 | # View Results 144 | # --------------------------- 145 | 146 | # utils.fullprint(sDes_traj_all[:,3:6]) 147 | utils.makeFigures(quad.params, t_all, pos_all, vel_all, quat_all, omega_all, euler_all, w_cmd_all, wMotor_all, thr_all, tor_all, sDes_traj_all, sDes_calc_all) 148 | ani = utils.sameAxisAnimation(t_all, traj.wps, pos_all, quat_all, sDes_traj_all, Ts, quad.params, traj.xyzType, traj.yawType, ifsave) 149 | plt.show() 150 | 151 | if __name__ == "__main__": 152 | if (config.orient == "NED" or config.orient == "ENU"): 153 | main() 154 | # cProfile.run('main()') 155 | else: 156 | raise Exception("{} is not a valid orientation. Verify config.py file.".format(config.orient)) -------------------------------------------------------------------------------- /Simulation/trajectory.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | author: John Bass 4 | email: john.bobzwik@gmail.com 5 | license: MIT 6 | Please feel free to use and modify this, but keep the above information. Thanks! 7 | """ 8 | # Functions get_poly_cc, minSomethingTraj, pos_waypoint_min are derived from Peter Huang's work: 9 | # https://github.com/hbd730/quadcopter-simulation 10 | # author: Peter Huang 11 | # email: hbd730@gmail.com 12 | # license: BSD 13 | # Please feel free to use and modify this, but keep the above information. Thanks! 14 | 15 | 16 | 17 | import numpy as np 18 | from numpy import pi 19 | from numpy.linalg import norm 20 | from waypoints import makeWaypoints 21 | import config 22 | 23 | class Trajectory: 24 | 25 | def __init__(self, quad, ctrlType, trajSelect): 26 | 27 | self.ctrlType = ctrlType 28 | self.xyzType = trajSelect[0] 29 | self.yawType = trajSelect[1] 30 | self.averVel = trajSelect[2] 31 | 32 | t_wps, wps, y_wps, v_wp = makeWaypoints() 33 | self.t_wps = t_wps 34 | self.wps = wps 35 | self.y_wps = y_wps 36 | self.v_wp = v_wp 37 | 38 | self.end_reached = 0 39 | 40 | if (self.ctrlType == "xyz_pos"): 41 | self.T_segment = np.diff(self.t_wps) 42 | 43 | if (self.averVel == 1): 44 | distance_segment = self.wps[1:] - self.wps[:-1] 45 | self.T_segment = np.sqrt(distance_segment[:,0]**2 + distance_segment[:,1]**2 + distance_segment[:,2]**2)/self.v_wp 46 | self.t_wps = np.zeros(len(self.T_segment) + 1) 47 | self.t_wps[1:] = np.cumsum(self.T_segment) 48 | 49 | if (self.xyzType >= 3 and self.xyzType <= 6): 50 | self.deriv_order = int(self.xyzType-2) # Looking to minimize which derivative order (eg: Minimum velocity -> first order) 51 | 52 | # Calculate coefficients 53 | self.coeff_x = minSomethingTraj(self.wps[:,0], self.T_segment, self.deriv_order) 54 | self.coeff_y = minSomethingTraj(self.wps[:,1], self.T_segment, self.deriv_order) 55 | self.coeff_z = minSomethingTraj(self.wps[:,2], self.T_segment, self.deriv_order) 56 | 57 | elif (self.xyzType >= 7 and self.xyzType <= 9): 58 | self.deriv_order = int(self.xyzType-5) # Looking to minimize which derivative order (eg: Minimum accel -> second order) 59 | 60 | # Calculate coefficients 61 | self.coeff_x = minSomethingTraj_stop(self.wps[:,0], self.T_segment, self.deriv_order) 62 | self.coeff_y = minSomethingTraj_stop(self.wps[:,1], self.T_segment, self.deriv_order) 63 | self.coeff_z = minSomethingTraj_stop(self.wps[:,2], self.T_segment, self.deriv_order) 64 | 65 | elif (self.xyzType >= 10 and self.xyzType <= 11): 66 | self.deriv_order = int(self.xyzType-7) # Looking to minimize which derivative order (eg: Minimum jerk -> third order) 67 | 68 | # Calculate coefficients 69 | self.coeff_x = minSomethingTraj_faststop(self.wps[:,0], self.T_segment, self.deriv_order) 70 | self.coeff_y = minSomethingTraj_faststop(self.wps[:,1], self.T_segment, self.deriv_order) 71 | self.coeff_z = minSomethingTraj_faststop(self.wps[:,2], self.T_segment, self.deriv_order) 72 | 73 | if (self.yawType == 4): 74 | self.y_wps = np.zeros(len(self.t_wps)) 75 | 76 | # Get initial heading 77 | self.current_heading = quad.psi 78 | 79 | # Initialize trajectory setpoint 80 | self.desPos = np.zeros(3) # Desired position (x, y, z) 81 | self.desVel = np.zeros(3) # Desired velocity (xdot, ydot, zdot) 82 | self.desAcc = np.zeros(3) # Desired acceleration (xdotdot, ydotdot, zdotdot) 83 | self.desThr = np.zeros(3) # Desired thrust in N-E-D directions (or E-N-U, if selected) 84 | self.desEul = np.zeros(3) # Desired orientation in the world frame (phi, theta, psi) 85 | self.desPQR = np.zeros(3) # Desired angular velocity in the body frame (p, q, r) 86 | self.desYawRate = 0. # Desired yaw speed 87 | self.sDes = np.hstack((self.desPos, self.desVel, self.desAcc, self.desThr, self.desEul, self.desPQR, self.desYawRate)).astype(float) 88 | 89 | 90 | def desiredState(self, t, Ts, quad): 91 | 92 | self.desPos = np.zeros(3) # Desired position (x, y, z) 93 | self.desVel = np.zeros(3) # Desired velocity (xdot, ydot, zdot) 94 | self.desAcc = np.zeros(3) # Desired acceleration (xdotdot, ydotdot, zdotdot) 95 | self.desThr = np.zeros(3) # Desired thrust in N-E-D directions (or E-N-U, if selected) 96 | self.desEul = np.zeros(3) # Desired orientation in the world frame (phi, theta, psi) 97 | self.desPQR = np.zeros(3) # Desired angular velocity in the body frame (p, q, r) 98 | self.desYawRate = 0. # Desired yaw speed 99 | 100 | def pos_waypoint_timed(): 101 | 102 | if not (len(self.t_wps) == self.wps.shape[0]): 103 | raise Exception("Time array and waypoint array not the same size.") 104 | elif (np.diff(self.t_wps) <= 0).any(): 105 | raise Exception("Time array isn't properly ordered.") 106 | 107 | if (t == 0): 108 | self.t_idx = 0 109 | elif (t >= self.t_wps[-1]): 110 | self.t_idx = -1 111 | else: 112 | self.t_idx = np.where(t <= self.t_wps)[0][0] - 1 113 | 114 | self.desPos = self.wps[self.t_idx,:] 115 | 116 | 117 | def pos_waypoint_interp(): 118 | 119 | if not (len(self.t_wps) == self.wps.shape[0]): 120 | raise Exception("Time array and waypoint array not the same size.") 121 | elif (np.diff(self.t_wps) <= 0).any(): 122 | raise Exception("Time array isn't properly ordered.") 123 | 124 | if (t == 0): 125 | self.t_idx = 0 126 | self.desPos = self.wps[0,:] 127 | elif (t >= self.t_wps[-1]): 128 | self.t_idx = -1 129 | self.desPos = self.wps[-1,:] 130 | else: 131 | self.t_idx = np.where(t <= self.t_wps)[0][0] - 1 132 | scale = (t - self.t_wps[self.t_idx])/self.T_segment[self.t_idx] 133 | self.desPos = (1 - scale) * self.wps[self.t_idx,:] + scale * self.wps[self.t_idx + 1,:] 134 | 135 | def pos_waypoint_min(): 136 | """ The function takes known number of waypoints and time, then generates a 137 | minimum velocity, acceleration, jerk or snap trajectory which goes through each waypoint. 138 | The output is the desired state associated with the next waypoint for the time t. 139 | """ 140 | if not (len(self.t_wps) == self.wps.shape[0]): 141 | raise Exception("Time array and waypoint array not the same size.") 142 | 143 | nb_coeff = self.deriv_order*2 144 | 145 | # Hover at t=0 146 | if t == 0: 147 | self.t_idx = 0 148 | self.desPos = self.wps[0,:] 149 | # Stay hover at the last waypoint position 150 | elif (t >= self.t_wps[-1]): 151 | self.t_idx = -1 152 | self.desPos = self.wps[-1,:] 153 | else: 154 | self.t_idx = np.where(t <= self.t_wps)[0][0] - 1 155 | 156 | # Scaled time (between 0 and duration of segment) 157 | scale = (t - self.t_wps[self.t_idx]) 158 | 159 | # Which coefficients to use 160 | start = nb_coeff * self.t_idx 161 | end = nb_coeff * (self.t_idx + 1) 162 | 163 | # Set desired position, velocity and acceleration 164 | t0 = get_poly_cc(nb_coeff, 0, scale) 165 | self.desPos = np.array([self.coeff_x[start:end].dot(t0), self.coeff_y[start:end].dot(t0), self.coeff_z[start:end].dot(t0)]) 166 | 167 | t1 = get_poly_cc(nb_coeff, 1, scale) 168 | self.desVel = np.array([self.coeff_x[start:end].dot(t1), self.coeff_y[start:end].dot(t1), self.coeff_z[start:end].dot(t1)]) 169 | 170 | t2 = get_poly_cc(nb_coeff, 2, scale) 171 | self.desAcc = np.array([self.coeff_x[start:end].dot(t2), self.coeff_y[start:end].dot(t2), self.coeff_z[start:end].dot(t2)]) 172 | 173 | def pos_waypoint_arrived(): 174 | 175 | dist_consider_arrived = 0.2 # Distance to waypoint that is considered as "arrived" 176 | if (t == 0): 177 | self.t_idx = 0 178 | self.end_reached = 0 179 | elif not(self.end_reached): 180 | distance_to_next_wp = ((self.wps[self.t_idx,0]-quad.pos[0])**2 + (self.wps[self.t_idx,1]-quad.pos[1])**2 + (self.wps[self.t_idx,2]-quad.pos[2])**2)**(0.5) 181 | if (distance_to_next_wp < dist_consider_arrived): 182 | self.t_idx += 1 183 | if (self.t_idx >= len(self.wps[:,0])): # if t_idx has reached the end of planned waypoints 184 | self.end_reached = 1 185 | self.t_idx = -1 186 | 187 | self.desPos = self.wps[self.t_idx,:] 188 | 189 | def pos_waypoint_arrived_wait(): 190 | 191 | dist_consider_arrived = 0.2 # Distance to waypoint that is considered as "arrived" 192 | if (t == 0): 193 | self.t_idx = 0 # Index of waypoint to go to 194 | self.t_arrived = 0 # Time when arrived at first waypoint ([0, 0, 0]) 195 | self.arrived = True # Bool to confirm arrived at first waypoint 196 | self.end_reached = 0 # End is not reached yet 197 | 198 | # If end is not reached, calculate distance to next waypoint 199 | elif not(self.end_reached): 200 | distance_to_next_wp = ((self.wps[self.t_idx,0]-quad.pos[0])**2 + (self.wps[self.t_idx,1]-quad.pos[1])**2 + (self.wps[self.t_idx,2]-quad.pos[2])**2)**(0.5) 201 | 202 | # If waypoint distance is below a threshold, specify the arrival time and confirm arrival 203 | if (distance_to_next_wp < dist_consider_arrived) and not self.arrived: 204 | self.t_arrived = t 205 | self.arrived = True 206 | 207 | # If arrived for more than xx seconds, increment waypoint index (t_idx) 208 | # Replace 'self.t_wps[self.t_idx]' by any number to have a fixed waiting time for all waypoints 209 | elif self.arrived and (t-self.t_arrived > self.t_wps[self.t_idx]): 210 | self.t_idx += 1 211 | self.arrived = False 212 | 213 | # If t_idx has reached the end of planned waypoints 214 | if (self.t_idx >= len(self.wps[:,0])): 215 | self.end_reached = 0 # set to 1 to stop looping 216 | self.t_idx = 0 # set to -1 to stop looping 217 | 218 | self.desPos = self.wps[self.t_idx,:] 219 | 220 | def yaw_waypoint_timed(): 221 | 222 | if not (len(self.t_wps) == len(self.y_wps)): 223 | raise Exception("Time array and waypoint array not the same size.") 224 | 225 | self.desEul[2] = self.y_wps[self.t_idx] 226 | 227 | 228 | def yaw_waypoint_interp(): 229 | 230 | if not (len(self.t_wps) == len(self.y_wps)): 231 | raise Exception("Time array and waypoint array not the same size.") 232 | 233 | if (t == 0) or (t >= self.t_wps[-1]): 234 | self.desEul[2] = self.y_wps[self.t_idx] 235 | else: 236 | scale = (t - self.t_wps[self.t_idx])/self.T_segment[self.t_idx] 237 | self.desEul[2] = (1 - scale)*self.y_wps[self.t_idx] + scale*self.y_wps[self.t_idx + 1] 238 | 239 | # Angle between current vector with the next heading vector 240 | delta_psi = self.desEul[2] - self.current_heading 241 | 242 | # Set Yaw rate 243 | self.desYawRate = delta_psi / Ts 244 | 245 | # Prepare next iteration 246 | self.current_heading = self.desEul[2] 247 | 248 | 249 | def yaw_follow(): 250 | 251 | if (self.xyzType == 1 or self.xyzType == 2 or self.xyzType == 12): 252 | if (t == 0): 253 | self.desEul[2] = 0 254 | else: 255 | # Calculate desired Yaw 256 | self.desEul[2] = np.arctan2(self.desPos[1]-quad.pos[1], self.desPos[0]-quad.pos[0]) 257 | 258 | elif (self.xyzType == 13): 259 | if (t == 0): 260 | self.desEul[2] = 0 261 | self.prevDesYaw = self.desEul[2] 262 | else: 263 | if not (self.arrived): 264 | # Calculate desired Yaw 265 | self.desEul[2] = np.arctan2(self.desPos[1]-quad.pos[1], self.desPos[0]-quad.pos[0]) 266 | self.prevDesYaw = self.desEul[2] 267 | else: 268 | self.desEul[2] = self.prevDesYaw 269 | 270 | else: 271 | if (t == 0) or (t >= self.t_wps[-1]): 272 | self.desEul[2] = self.y_wps[self.t_idx] 273 | else: 274 | # Calculate desired Yaw 275 | self.desEul[2] = np.arctan2(self.desVel[1], self.desVel[0]) 276 | 277 | # Dirty hack, detect when desEul[2] switches from -pi to pi (or vice-versa) and switch manualy current_heading 278 | if (np.sign(self.desEul[2]) - np.sign(self.current_heading) and abs(self.desEul[2]-self.current_heading) >= 2*pi-0.1): 279 | self.current_heading = self.current_heading + np.sign(self.desEul[2])*2*pi 280 | 281 | # Angle between current vector with the next heading vector 282 | delta_psi = self.desEul[2] - self.current_heading 283 | 284 | # Set Yaw rate 285 | self.desYawRate = delta_psi / Ts 286 | 287 | # Prepare next iteration 288 | self.current_heading = self.desEul[2] 289 | 290 | 291 | if (self.ctrlType == "xyz_vel"): 292 | if (self.xyzType == 1): 293 | self.sDes = testVelControl(t) 294 | 295 | elif (self.ctrlType == "xy_vel_z_pos"): 296 | if (self.xyzType == 1): 297 | self.sDes = testVelControl(t) 298 | 299 | elif (self.ctrlType == "xyz_pos"): 300 | # Hover at [0, 0, 0] 301 | if (self.xyzType == 0): 302 | pass 303 | # For simple testing 304 | elif (self.xyzType == 99): 305 | self.sDes = testXYZposition(t) 306 | else: 307 | # List of possible position trajectories 308 | # --------------------------- 309 | # Set desired positions at every t_wps[i] 310 | if (self.xyzType == 1): 311 | pos_waypoint_timed() 312 | # Interpolate position between every waypoint, to arrive at desired position every t_wps[i] 313 | elif (self.xyzType == 2): 314 | pos_waypoint_interp() 315 | # Calculate a minimum velocity, acceleration, jerk or snap trajectory 316 | elif (self.xyzType >= 3 and self.xyzType <= 11): 317 | pos_waypoint_min() 318 | # Go to next waypoint when arrived at waypoint 319 | elif (self.xyzType == 12): 320 | pos_waypoint_arrived() 321 | # Go to next waypoint when arrived at waypoint after waiting x seconds 322 | elif (self.xyzType == 13): 323 | pos_waypoint_arrived_wait() 324 | 325 | # List of possible yaw trajectories 326 | # --------------------------- 327 | # Set desired yaw at every t_wps[i] 328 | if (self.yawType == 0): 329 | pass 330 | elif (self.yawType == 1): 331 | yaw_waypoint_timed() 332 | # Interpolate yaw between every waypoint, to arrive at desired yaw every t_wps[i] 333 | elif (self.yawType == 2): 334 | yaw_waypoint_interp() 335 | # Have the drone's heading match its desired velocity direction 336 | elif (self.yawType == 3): 337 | yaw_follow() 338 | 339 | self.sDes = np.hstack((self.desPos, self.desVel, self.desAcc, self.desThr, self.desEul, self.desPQR, self.desYawRate)).astype(float) 340 | 341 | return self.sDes 342 | 343 | 344 | def get_poly_cc(n, k, t): 345 | """ This is a helper function to get the coeffitient of coefficient for n-th 346 | order polynomial with k-th derivative at time t. 347 | """ 348 | assert (n > 0 and k >= 0), "order and derivative must be positive." 349 | 350 | cc = np.ones(n) 351 | D = np.linspace(n-1, 0, n) 352 | 353 | for i in range(n): 354 | for j in range(k): 355 | cc[i] = cc[i] * D[i] 356 | D[i] = D[i] - 1 357 | if D[i] == -1: 358 | D[i] = 0 359 | 360 | for i, c in enumerate(cc): 361 | cc[i] = c * np.power(t, D[i]) 362 | 363 | return cc 364 | 365 | 366 | def minSomethingTraj(waypoints, times, order): 367 | """ This function takes a list of desired waypoint i.e. [x0, x1, x2...xN] and 368 | time, returns a [M*N,1] coeffitients matrix for the N+1 waypoints (N segments), 369 | where M is the number of coefficients per segment and is equal to (order)*2. If one 370 | desires to create a minimum velocity, order = 1. Minimum snap would be order = 4. 371 | 372 | 1.The Problem 373 | Generate a full trajectory across N+1 waypoint is made of N polynomial line segment. 374 | Each segment is defined as a (2*order-1)-th order polynomial defined as follow: 375 | Minimum velocity: Pi = ai_0 + ai1*t 376 | Minimum acceleration: Pi = ai_0 + ai1*t + ai2*t^2 + ai3*t^3 377 | Minimum jerk: Pi = ai_0 + ai1*t + ai2*t^2 + ai3*t^3 + ai4*t^4 + ai5*t^5 378 | Minimum snap: Pi = ai_0 + ai1*t + ai2*t^2 + ai3*t^3 + ai4*t^4 + ai5*t^5 + ai6*t^6 + ai7*t^7 379 | 380 | Each polynomial has M unknown coefficients, thus we will have M*N unknown to 381 | solve in total, so we need to come up with M*N constraints. 382 | 383 | 2.The constraints 384 | In general, the constraints is a set of condition which define the initial 385 | and final state, continuity between each piecewise function. This includes 386 | specifying continuity in higher derivatives of the trajectory at the 387 | intermediate waypoints. 388 | 389 | 3.Matrix Design 390 | Since we have M*N unknown coefficients to solve, and if we are given M*N 391 | equations(constraints), then the problem becomes solving a linear equation. 392 | 393 | A * Coeff = B 394 | 395 | Let's look at B matrix first, B matrix is simple because it is just some constants 396 | on the right hand side of the equation. There are M*N constraints, 397 | so B matrix will be [M*N, 1]. 398 | 399 | Coeff is the final output matrix consists of M*N elements. 400 | Since B matrix is only one column, Coeff matrix must be [M*N, 1]. 401 | 402 | A matrix is tricky, we then can think of A matrix as a coeffient-coeffient matrix. 403 | We are no longer looking at a particular polynomial Pi, but rather P1, P2...PN 404 | as a whole. Since now our Coeff matrix is [M*N, 1], and B is [M*N, 1], thus 405 | A matrix must have the form [M*N, M*N]. 406 | 407 | A = [A10 A11 ... A1M A20 A21 ... A2M ... AN0 AN1 ... ANM 408 | ... 409 | ] 410 | 411 | Each element in a row represents the coefficient of coeffient aij under 412 | a certain constraint, where aij is the jth coeffient of Pi with i = 1...N, j = 0...(M-1). 413 | """ 414 | 415 | n = len(waypoints) - 1 416 | nb_coeff = order*2 417 | 418 | # initialize A, and B matrix 419 | A = np.zeros([nb_coeff*n, nb_coeff*n]) 420 | B = np.zeros(nb_coeff*n) 421 | 422 | # populate B matrix. 423 | for i in range(n): 424 | B[i] = waypoints[i] 425 | B[i + n] = waypoints[i+1] 426 | 427 | # Constraint 1 - Starting position for every segment 428 | for i in range(n): 429 | A[i][nb_coeff*i:nb_coeff*(i+1)] = get_poly_cc(nb_coeff, 0, 0) 430 | 431 | # Constraint 2 - Ending position for every segment 432 | for i in range(n): 433 | A[i+n][nb_coeff*i:nb_coeff*(i+1)] = get_poly_cc(nb_coeff, 0, times[i]) 434 | 435 | # Constraint 3 - Starting position derivatives (up to order) are null 436 | for k in range(1, order): 437 | A[2*n+k-1][:nb_coeff] = get_poly_cc(nb_coeff, k, 0) 438 | 439 | # Constraint 4 - Ending position derivatives (up to order) are null 440 | for k in range(1, order): 441 | A[2*n+(order-1)+k-1][-nb_coeff:] = get_poly_cc(nb_coeff, k, times[i]) 442 | 443 | # Constraint 5 - All derivatives are continuous at each waypint transition 444 | for i in range(n-1): 445 | for k in range(1, nb_coeff-1): 446 | A[2*n+2*(order-1) + i*2*(order-1)+k-1][i*nb_coeff : (i*nb_coeff+nb_coeff*2)] = np.concatenate((get_poly_cc(nb_coeff, k, times[i]), -get_poly_cc(nb_coeff, k, 0))) 447 | 448 | # solve for the coefficients 449 | Coeff = np.linalg.solve(A, B) 450 | return Coeff 451 | 452 | 453 | # Minimum acceleration/jerk/snap Trajectory, but with null velocity, accel and jerk at each waypoint 454 | def minSomethingTraj_stop(waypoints, times, order): 455 | """ This function takes a list of desired waypoint i.e. [x0, x1, x2...xN] and 456 | time, returns a [M*N,1] coeffitients matrix for the N+1 waypoints (N segments), 457 | where M is the number of coefficients per segment and is equal to (order)*2. If one 458 | desires to create a minimum acceleration, order = 2. Minimum snap would be order = 4. 459 | 460 | 1.The Problem 461 | Generate a full trajectory across N+1 waypoint is made of N polynomial line segment. 462 | Each segment is defined as a (2*order-1)-th order polynomial defined as follow: 463 | Minimum velocity: Pi = ai_0 + ai1*t 464 | Minimum acceleration: Pi = ai_0 + ai1*t + ai2*t^2 + ai3*t^3 465 | Minimum jerk: Pi = ai_0 + ai1*t + ai2*t^2 + ai3*t^3 + ai4*t^4 + ai5*t^5 466 | Minimum snap: Pi = ai_0 + ai1*t + ai2*t^2 + ai3*t^3 + ai4*t^4 + ai5*t^5 + ai6*t^6 + ai7*t^7 467 | 468 | Each polynomial has M unknown coefficients, thus we will have M*N unknown to 469 | solve in total, so we need to come up with M*N constraints. 470 | 471 | Unlike the function minSomethingTraj, where continuous equations for velocity, jerk and snap are generated, 472 | this function generates trajectories with null velocities, accelerations and jerks at each waypoints. 473 | This will make the drone stop for an instant at each waypoint. 474 | """ 475 | 476 | n = len(waypoints) - 1 477 | nb_coeff = order*2 478 | 479 | # initialize A, and B matrix 480 | A = np.zeros([nb_coeff*n, nb_coeff*n]) 481 | B = np.zeros(nb_coeff*n) 482 | 483 | # populate B matrix. 484 | for i in range(n): 485 | B[i] = waypoints[i] 486 | B[i + n] = waypoints[i+1] 487 | 488 | # Constraint 1 - Starting position for every segment 489 | for i in range(n): 490 | A[i][nb_coeff*i:nb_coeff*(i+1)] = get_poly_cc(nb_coeff, 0, 0) 491 | 492 | # Constraint 2 - Ending position for every segment 493 | for i in range(n): 494 | A[i+n][nb_coeff*i:nb_coeff*(i+1)] = get_poly_cc(nb_coeff, 0, times[i]) 495 | 496 | # Constraint 3 - Starting position derivatives (up to order) for each segment are null 497 | for i in range(n): 498 | for k in range(1, order): 499 | A[2*n + k-1 + i*(order-1)][nb_coeff*i:nb_coeff*(i+1)] = get_poly_cc(nb_coeff, k, 0) 500 | 501 | # Constraint 4 - Ending position derivatives (up to order) for each segment are null 502 | for i in range(n): 503 | for k in range(1, order): 504 | A[2*n+(order-1)*n + k-1 + i*(order-1)][nb_coeff*i:nb_coeff*(i+1)] = get_poly_cc(nb_coeff, k, times[i]) 505 | 506 | # solve for the coefficients 507 | Coeff = np.linalg.solve(A, B) 508 | return Coeff 509 | 510 | # Minimum acceleration/jerk/snap Trajectory, but with null velocity only at each waypoint 511 | def minSomethingTraj_faststop(waypoints, times, order): 512 | """ This function takes a list of desired waypoint i.e. [x0, x1, x2...xN] and 513 | time, returns a [M*N,1] coeffitients matrix for the N+1 waypoints (N segments), 514 | where M is the number of coefficients per segment and is equal to (order)*2. If one 515 | desires to create a minimum acceleration, order = 2. Minimum snap would be order = 4. 516 | 517 | 1.The Problem 518 | Generate a full trajectory across N+1 waypoint is made of N polynomial line segment. 519 | Each segment is defined as a (2*order-1)-th order polynomial defined as follow: 520 | Minimum velocity: Pi = ai_0 + ai1*t 521 | Minimum acceleration: Pi = ai_0 + ai1*t + ai2*t^2 + ai3*t^3 522 | Minimum jerk: Pi = ai_0 + ai1*t + ai2*t^2 + ai3*t^3 + ai4*t^4 + ai5*t^5 523 | Minimum snap: Pi = ai_0 + ai1*t + ai2*t^2 + ai3*t^3 + ai4*t^4 + ai5*t^5 + ai6*t^6 + ai7*t^7 524 | 525 | Each polynomial has M unknown coefficients, thus we will have M*N unknown to 526 | solve in total, so we need to come up with M*N constraints. 527 | 528 | Unlike the function minSomethingTraj, where continuous equations for velocity, jerk and snap are generated, 529 | and unlike the function minSomethingTraj_stop, where velocities, accelerations and jerks are equal to 0 at each waypoint, 530 | this function generates trajectories with only null velocities. Accelerations and above derivatives are continuous. 531 | This will make the drone stop for an instant at each waypoint, and then leave in the same direction it came from. 532 | """ 533 | 534 | n = len(waypoints) - 1 535 | nb_coeff = order*2 536 | 537 | # initialize A, and B matrix 538 | A = np.zeros([nb_coeff*n, nb_coeff*n]) 539 | B = np.zeros(nb_coeff*n) 540 | 541 | # populate B matrix. 542 | for i in range(n): 543 | B[i] = waypoints[i] 544 | B[i + n] = waypoints[i+1] 545 | 546 | # Constraint 1 - Starting position for every segment 547 | for i in range(n): 548 | # print(i) 549 | A[i][nb_coeff*i:nb_coeff*(i+1)] = get_poly_cc(nb_coeff, 0, 0) 550 | 551 | # Constraint 2 - Ending position for every segment 552 | for i in range(n): 553 | # print(i+n) 554 | A[i+n][nb_coeff*i:nb_coeff*(i+1)] = get_poly_cc(nb_coeff, 0, times[i]) 555 | 556 | # Constraint 3 - Starting velocity for every segment is null 557 | for i in range(n): 558 | # print(i+2*n) 559 | A[i+2*n][nb_coeff*i:nb_coeff*(i+1)] = get_poly_cc(nb_coeff, 1, 0) 560 | 561 | # Constraint 4 - Ending velocity for every segment is null 562 | for i in range(n): 563 | # print(i+3*n) 564 | A[i+3*n][nb_coeff*i:nb_coeff*(i+1)] = get_poly_cc(nb_coeff, 1, times[i]) 565 | 566 | # Constraint 5 - Starting position derivatives (above velocity and up to order) are null 567 | for k in range(2, order): 568 | # print(4*n + k-2) 569 | A[4*n+k-2][:nb_coeff] = get_poly_cc(nb_coeff, k, 0) 570 | 571 | # Constraint 6 - Ending position derivatives (above velocity and up to order) are null 572 | for k in range(2, order): 573 | # print(4*n+(order-2) + k-2) 574 | A[4*n+k-2+(order-2)][-nb_coeff:] = get_poly_cc(nb_coeff, k, times[i]) 575 | 576 | # Constraint 7 - All derivatives above velocity are continuous at each waypint transition 577 | for i in range(n-1): 578 | for k in range(2, nb_coeff-2): 579 | # print(4*n+2*(order-2)+k-2+i*(nb_coeff-4)) 580 | A[4*n+2*(order-2)+k-2+i*(nb_coeff-4)][i*nb_coeff : (i*nb_coeff+nb_coeff*2)] = np.concatenate((get_poly_cc(nb_coeff, k, times[i]), -get_poly_cc(nb_coeff, k, 0))) 581 | 582 | 583 | # solve for the coefficients 584 | Coeff = np.linalg.solve(A, B) 585 | return Coeff 586 | 587 | 588 | 589 | ## Testing scripts 590 | 591 | def testXYZposition(t): 592 | desPos = np.array([0., 0., 0.]) 593 | desVel = np.array([0., 0., 0.]) 594 | desAcc = np.array([0., 0., 0.]) 595 | desThr = np.array([0., 0., 0.]) 596 | desEul = np.array([0., 0., 0.]) 597 | desPQR = np.array([0., 0., 0.]) 598 | desYawRate = 30.0*pi/180 599 | 600 | if t >= 1 and t < 4: 601 | desPos = np.array([2, 2, 1]) 602 | elif t >= 4: 603 | desPos = np.array([2, -2, -2]) 604 | desEul = np.array([0, 0, pi/3]) 605 | 606 | sDes = np.hstack((desPos, desVel, desAcc, desThr, desEul, desPQR, desYawRate)).astype(float) 607 | 608 | return sDes 609 | 610 | 611 | def testVelControl(t): 612 | desPos = np.array([0., 0., 0.]) 613 | desVel = np.array([0., 0., 0.]) 614 | desAcc = np.array([0., 0., 0.]) 615 | desThr = np.array([0., 0., 0.]) 616 | desEul = np.array([0., 0., 0.]) 617 | desPQR = np.array([0., 0., 0.]) 618 | desYawRate = 0. 619 | 620 | if t >= 1 and t < 4: 621 | desVel = np.array([3, 2, 0]) 622 | elif t >= 4: 623 | desVel = np.array([3, -1, 0]) 624 | 625 | sDes = np.hstack((desPos, desVel, desAcc, desThr, desEul, desPQR, desYawRate)).astype(float) 626 | 627 | return sDes 628 | -------------------------------------------------------------------------------- /Simulation/utils/__init__.py: -------------------------------------------------------------------------------- 1 | from .rotationConversion import * 2 | from .stateConversions import * 3 | from .mixer import * 4 | from .display import * 5 | from .animation import * 6 | from .quaternionFunctions import * -------------------------------------------------------------------------------- /Simulation/utils/animation.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | author: John Bass 4 | email: john.bobzwik@gmail.com 5 | license: MIT 6 | Please feel free to use and modify this, but keep the above information. Thanks! 7 | """ 8 | 9 | import numpy as np 10 | from numpy import pi 11 | import matplotlib.pyplot as plt 12 | import mpl_toolkits.mplot3d.axes3d as p3 13 | from matplotlib import animation 14 | 15 | import utils 16 | import config 17 | 18 | numFrames = 8 19 | 20 | def sameAxisAnimation(t_all, waypoints, pos_all, quat_all, sDes_tr_all, Ts, params, xyzType, yawType, ifsave): 21 | 22 | x = pos_all[:,0] 23 | y = pos_all[:,1] 24 | z = pos_all[:,2] 25 | 26 | xDes = sDes_tr_all[:,0] 27 | yDes = sDes_tr_all[:,1] 28 | zDes = sDes_tr_all[:,2] 29 | 30 | x_wp = waypoints[:,0] 31 | y_wp = waypoints[:,1] 32 | z_wp = waypoints[:,2] 33 | 34 | if (config.orient == "NED"): 35 | z = -z 36 | zDes = -zDes 37 | z_wp = -z_wp 38 | 39 | fig = plt.figure() 40 | ax = p3.Axes3D(fig,auto_add_to_figure=False) 41 | fig.add_axes(ax) 42 | line1, = ax.plot([], [], [], lw=2, color='red') 43 | line2, = ax.plot([], [], [], lw=2, color='blue') 44 | line3, = ax.plot([], [], [], '--', lw=1, color='blue') 45 | 46 | # Setting the axes properties 47 | extraEachSide = 0.5 48 | maxRange = 0.5*np.array([x.max()-x.min(), y.max()-y.min(), z.max()-z.min()]).max() + extraEachSide 49 | mid_x = 0.5*(x.max()+x.min()) 50 | mid_y = 0.5*(y.max()+y.min()) 51 | mid_z = 0.5*(z.max()+z.min()) 52 | 53 | ax.set_xlim3d([mid_x-maxRange, mid_x+maxRange]) 54 | ax.set_xlabel('X') 55 | if (config.orient == "NED"): 56 | ax.set_ylim3d([mid_y+maxRange, mid_y-maxRange]) 57 | elif (config.orient == "ENU"): 58 | ax.set_ylim3d([mid_y-maxRange, mid_y+maxRange]) 59 | ax.set_ylabel('Y') 60 | ax.set_zlim3d([mid_z-maxRange, mid_z+maxRange]) 61 | ax.set_zlabel('Altitude') 62 | 63 | titleTime = ax.text2D(0.05, 0.95, "", transform=ax.transAxes) 64 | 65 | trajType = '' 66 | yawTrajType = '' 67 | 68 | if (xyzType == 0): 69 | trajType = 'Hover' 70 | else: 71 | ax.scatter(x_wp, y_wp, z_wp, color='green', alpha=1, marker = 'o', s = 25) 72 | if (xyzType == 1 or xyzType == 12): 73 | trajType = 'Simple Waypoints' 74 | else: 75 | ax.plot(xDes, yDes, zDes, ':', lw=1.3, color='green') 76 | if (xyzType == 2): 77 | trajType = 'Simple Waypoint Interpolation' 78 | elif (xyzType == 3): 79 | trajType = 'Minimum Velocity Trajectory' 80 | elif (xyzType == 4): 81 | trajType = 'Minimum Acceleration Trajectory' 82 | elif (xyzType == 5): 83 | trajType = 'Minimum Jerk Trajectory' 84 | elif (xyzType == 6): 85 | trajType = 'Minimum Snap Trajectory' 86 | elif (xyzType == 7): 87 | trajType = 'Minimum Acceleration Trajectory - Stop' 88 | elif (xyzType == 8): 89 | trajType = 'Minimum Jerk Trajectory - Stop' 90 | elif (xyzType == 9): 91 | trajType = 'Minimum Snap Trajectory - Stop' 92 | elif (xyzType == 10): 93 | trajType = 'Minimum Jerk Trajectory - Fast Stop' 94 | elif (xyzType == 1): 95 | trajType = 'Minimum Snap Trajectory - Fast Stop' 96 | 97 | if (yawType == 0): 98 | yawTrajType = 'None' 99 | elif (yawType == 1): 100 | yawTrajType = 'Waypoints' 101 | elif (yawType == 2): 102 | yawTrajType = 'Interpolation' 103 | elif (yawType == 3): 104 | yawTrajType = 'Follow' 105 | elif (yawType == 4): 106 | yawTrajType = 'Zero' 107 | 108 | 109 | 110 | titleType1 = ax.text2D(0.95, 0.95, trajType, transform=ax.transAxes, horizontalalignment='right') 111 | titleType2 = ax.text2D(0.95, 0.91, 'Yaw: '+ yawTrajType, transform=ax.transAxes, horizontalalignment='right') 112 | 113 | def updateLines(i): 114 | 115 | time = t_all[i*numFrames] 116 | pos = pos_all[i*numFrames] 117 | x = pos[0] 118 | y = pos[1] 119 | z = pos[2] 120 | 121 | x_from0 = pos_all[0:i*numFrames,0] 122 | y_from0 = pos_all[0:i*numFrames,1] 123 | z_from0 = pos_all[0:i*numFrames,2] 124 | 125 | dxm = params["dxm"] 126 | dym = params["dym"] 127 | dzm = params["dzm"] 128 | 129 | quat = quat_all[i*numFrames] 130 | 131 | if (config.orient == "NED"): 132 | z = -z 133 | z_from0 = -z_from0 134 | quat = np.array([quat[0], -quat[1], -quat[2], quat[3]]) 135 | 136 | R = utils.quat2Dcm(quat) 137 | motorPoints = np.array([[dxm, -dym, dzm], [0, 0, 0], [dxm, dym, dzm], [-dxm, dym, dzm], [0, 0, 0], [-dxm, -dym, dzm]]) 138 | motorPoints = np.dot(R, np.transpose(motorPoints)) 139 | motorPoints[0,:] += x 140 | motorPoints[1,:] += y 141 | motorPoints[2,:] += z 142 | 143 | line1.set_data(motorPoints[0,0:3], motorPoints[1,0:3]) 144 | line1.set_3d_properties(motorPoints[2,0:3]) 145 | line2.set_data(motorPoints[0,3:6], motorPoints[1,3:6]) 146 | line2.set_3d_properties(motorPoints[2,3:6]) 147 | line3.set_data(x_from0, y_from0) 148 | line3.set_3d_properties(z_from0) 149 | titleTime.set_text(u"Time = {:.2f} s".format(time)) 150 | 151 | return line1, line2 152 | 153 | 154 | def ini_plot(): 155 | 156 | line1.set_data(np.empty([1]), np.empty([1])) 157 | line1.set_3d_properties(np.empty([1])) 158 | line2.set_data(np.empty([1]), np.empty([1])) 159 | line2.set_3d_properties(np.empty([1])) 160 | line3.set_data(np.empty([1]), np.empty([1])) 161 | line3.set_3d_properties(np.empty([1])) 162 | 163 | return line1, line2, line3 164 | 165 | 166 | # Creating the Animation object 167 | line_ani = animation.FuncAnimation(fig, updateLines, init_func=ini_plot, frames=len(t_all[0:-2:numFrames]), interval=(Ts*1000*numFrames), blit=False) 168 | 169 | if (ifsave): 170 | line_ani.save('Gifs/Raw/animation_{0:.0f}_{1:.0f}.gif'.format(xyzType,yawType), dpi=80, writer='imagemagick', fps=25) 171 | 172 | plt.show() 173 | return line_ani -------------------------------------------------------------------------------- /Simulation/utils/display.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | author: John Bass 4 | email: john.bobzwik@gmail.com 5 | license: MIT 6 | Please feel free to use and modify this, but keep the above information. Thanks! 7 | """ 8 | 9 | import numpy as np 10 | from numpy import pi 11 | import matplotlib.pyplot as plt 12 | import utils 13 | 14 | rad2deg = 180.0/pi 15 | deg2rad = pi/180.0 16 | rads2rpm = 60.0/(2.0*pi) 17 | rpm2rads = 2.0*pi/60.0 18 | 19 | # Print complete vector or matrices 20 | def fullprint(*args, **kwargs): 21 | opt = np.get_printoptions() 22 | np.set_printoptions(threshold=np.inf) 23 | print(*args, **kwargs) 24 | np.set_printoptions(opt) 25 | 26 | 27 | def makeFigures(params, time, pos_all, vel_all, quat_all, omega_all, euler_all, commands, wMotor_all, thrust, torque, sDes_traj, sDes_calc): 28 | x = pos_all[:,0] 29 | y = pos_all[:,1] 30 | z = pos_all[:,2] 31 | q0 = quat_all[:,0] 32 | q1 = quat_all[:,1] 33 | q2 = quat_all[:,2] 34 | q3 = quat_all[:,3] 35 | xdot = vel_all[:,0] 36 | ydot = vel_all[:,1] 37 | zdot = vel_all[:,2] 38 | p = omega_all[:,0]*rad2deg 39 | q = omega_all[:,1]*rad2deg 40 | r = omega_all[:,2]*rad2deg 41 | 42 | wM1 = wMotor_all[:,0]*rads2rpm 43 | wM2 = wMotor_all[:,1]*rads2rpm 44 | wM3 = wMotor_all[:,2]*rads2rpm 45 | wM4 = wMotor_all[:,3]*rads2rpm 46 | 47 | phi = euler_all[:,0]*rad2deg 48 | theta = euler_all[:,1]*rad2deg 49 | psi = euler_all[:,2]*rad2deg 50 | 51 | x_sp = sDes_calc[:,0] 52 | y_sp = sDes_calc[:,1] 53 | z_sp = sDes_calc[:,2] 54 | Vx_sp = sDes_calc[:,3] 55 | Vy_sp = sDes_calc[:,4] 56 | Vz_sp = sDes_calc[:,5] 57 | x_thr_sp = sDes_calc[:,6] 58 | y_thr_sp = sDes_calc[:,7] 59 | z_thr_sp = sDes_calc[:,8] 60 | q0Des = sDes_calc[:,9] 61 | q1Des = sDes_calc[:,10] 62 | q2Des = sDes_calc[:,11] 63 | q3Des = sDes_calc[:,12] 64 | pDes = sDes_calc[:,13]*rad2deg 65 | qDes = sDes_calc[:,14]*rad2deg 66 | rDes = sDes_calc[:,15]*rad2deg 67 | 68 | x_tr = sDes_traj[:,0] 69 | y_tr = sDes_traj[:,1] 70 | z_tr = sDes_traj[:,2] 71 | Vx_tr = sDes_traj[:,3] 72 | Vy_tr = sDes_traj[:,4] 73 | Vz_tr = sDes_traj[:,5] 74 | Ax_tr = sDes_traj[:,6] 75 | Ay_tr = sDes_traj[:,7] 76 | Az_tr = sDes_traj[:,8] 77 | yaw_tr = sDes_traj[:,14]*rad2deg 78 | 79 | uM1 = commands[:,0]*rads2rpm 80 | uM2 = commands[:,1]*rads2rpm 81 | uM3 = commands[:,2]*rads2rpm 82 | uM4 = commands[:,3]*rads2rpm 83 | 84 | x_err = x_sp - x 85 | y_err = y_sp - y 86 | z_err = z_sp - z 87 | 88 | psiDes = np.zeros(q0Des.shape[0]) 89 | thetaDes = np.zeros(q0Des.shape[0]) 90 | phiDes = np.zeros(q0Des.shape[0]) 91 | for ii in range(q0Des.shape[0]): 92 | YPR = utils.quatToYPR_ZYX(sDes_calc[ii,9:13]) 93 | psiDes[ii] = YPR[0]*rad2deg 94 | thetaDes[ii] = YPR[1]*rad2deg 95 | phiDes[ii] = YPR[2]*rad2deg 96 | 97 | plt.show() 98 | 99 | plt.figure() 100 | plt.plot(time, x, time, y, time, z) 101 | plt.plot(time, x_sp, '--', time, y_sp, '--', time, z_sp, '--') 102 | plt.grid(True) 103 | plt.legend(['x','y','z','x_sp','y_sp','z_sp']) 104 | plt.xlabel('Time (s)') 105 | plt.ylabel('Position (m)') 106 | plt.draw() 107 | 108 | 109 | 110 | plt.figure() 111 | plt.plot(time, xdot, time, ydot, time, zdot) 112 | plt.plot(time, Vx_sp, '--', time, Vy_sp, '--', time, Vz_sp, '--') 113 | plt.grid(True) 114 | plt.legend(['Vx','Vy','Vz','Vx_sp','Vy_sp','Vz_sp']) 115 | plt.xlabel('Time (s)') 116 | plt.ylabel('Velocity (m/s)') 117 | plt.draw() 118 | 119 | plt.figure() 120 | plt.plot(time, x_thr_sp, time, y_thr_sp, time, z_thr_sp) 121 | plt.grid(True) 122 | plt.legend(['x_thr_sp','y_thr_sp','z_thr_sp']) 123 | plt.xlabel('Time (s)') 124 | plt.ylabel('Desired Thrust (N)') 125 | plt.draw() 126 | 127 | plt.figure() 128 | plt.plot(time, phi, time, theta, time, psi) 129 | plt.plot(time, phiDes, '--', time, thetaDes, '--', time, psiDes, '--') 130 | plt.plot(time, yaw_tr, '-.') 131 | plt.grid(True) 132 | plt.legend(['roll','pitch','yaw','roll_sp','pitch_sp','yaw_sp','yaw_tr']) 133 | plt.xlabel('Time (s)') 134 | plt.ylabel('Euler Angle (°)') 135 | plt.draw() 136 | 137 | plt.figure() 138 | plt.plot(time, p, time, q, time, r) 139 | plt.plot(time, pDes, '--', time, qDes, '--', time, rDes, '--') 140 | plt.grid(True) 141 | plt.legend(['p','q','r','p_sp','q_sp','r_sp']) 142 | plt.xlabel('Time (s)') 143 | plt.ylabel('Angular Velocity (°/s)') 144 | plt.draw() 145 | 146 | plt.figure() 147 | plt.plot(time, wM1, time, wM2, time, wM3, time, wM4) 148 | plt.plot(time, uM1, '--', time, uM2, '--', time, uM3, '--', time, uM4, '--') 149 | plt.grid(True) 150 | plt.legend(['w1','w2','w3','w4']) 151 | plt.xlabel('Time (s)') 152 | plt.ylabel('Motor Angular Velocity (RPM)') 153 | plt.draw() 154 | 155 | plt.figure() 156 | plt.subplot(2,1,1) 157 | plt.plot(time, thrust[:,0], time, thrust[:,1], time, thrust[:,2], time, thrust[:,3]) 158 | plt.grid(True) 159 | plt.legend(['thr1','thr2','thr3','thr4'], loc='upper right') 160 | plt.xlabel('Time (s)') 161 | plt.ylabel('Rotor Thrust (N)') 162 | plt.draw() 163 | 164 | plt.subplot(2,1,2) 165 | plt.plot(time, torque[:,0], time, torque[:,1], time, torque[:,2], time, torque[:,3]) 166 | plt.grid(True) 167 | plt.legend(['tor1','tor2','tor3','tor4'], loc='upper right') 168 | plt.xlabel('Time (s)') 169 | plt.ylabel('Rotor Torque (N*m)') 170 | plt.draw() 171 | 172 | plt.figure() 173 | plt.subplot(3,1,1) 174 | plt.title('Trajectory Setpoints') 175 | plt.plot(time, x_tr, time, y_tr, time, z_tr) 176 | plt.grid(True) 177 | plt.legend(['x','y','z'], loc='upper right') 178 | plt.xlabel('Time (s)') 179 | plt.ylabel('Position (m)') 180 | 181 | plt.subplot(3,1,2) 182 | plt.plot(time, Vx_tr, time, Vy_tr, time, Vz_tr) 183 | plt.grid(True) 184 | plt.legend(['Vx','Vy','Vz'], loc='upper right') 185 | plt.xlabel('Time (s)') 186 | plt.ylabel('Velocity (m/s)') 187 | 188 | plt.subplot(3,1,3) 189 | plt.plot(time, Ax_tr, time, Ay_tr, time, Az_tr) 190 | plt.grid(True) 191 | plt.legend(['Ax','Ay','Az'], loc='upper right') 192 | plt.xlabel('Time (s)') 193 | plt.ylabel('Acceleration (m/s^2)') 194 | plt.draw() 195 | 196 | plt.figure() 197 | plt.plot(time, x_err, time, y_err, time, z_err) 198 | plt.grid(True) 199 | plt.legend(['Pos x error','Pos y error','Pos z error']) 200 | plt.xlabel('Time (s)') 201 | plt.ylabel('Position Error (m)') 202 | plt.draw() -------------------------------------------------------------------------------- /Simulation/utils/mixer.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | author: John Bass 4 | email: john.bobzwik@gmail.com 5 | license: MIT 6 | Please feel free to use and modify this, but keep the above information. Thanks! 7 | """ 8 | 9 | import numpy as np 10 | import config 11 | 12 | 13 | def mixerFM(quad, thr, moment): 14 | t = np.array([thr, moment[0], moment[1], moment[2]]) 15 | w_cmd = np.sqrt(np.clip(np.dot(quad.params["mixerFMinv"], t), quad.params["minWmotor"]**2, quad.params["maxWmotor"]**2)) 16 | 17 | return w_cmd 18 | 19 | 20 | ## Under here is the conventional type of mixer 21 | 22 | # def mixer(throttle, pCmd, qCmd, rCmd, quad): 23 | # maxCmd = quad.params["maxCmd"] 24 | # minCmd = quad.params["minCmd"] 25 | 26 | # cmd = np.zeros([4, 1]) 27 | # cmd[0] = throttle + pCmd + qCmd - rCmd 28 | # cmd[1] = throttle - pCmd + qCmd + rCmd 29 | # cmd[2] = throttle - pCmd - qCmd - rCmd 30 | # cmd[3] = throttle + pCmd - qCmd + rCmd 31 | 32 | # cmd[0] = min(max(cmd[0], minCmd), maxCmd) 33 | # cmd[1] = min(max(cmd[1], minCmd), maxCmd) 34 | # cmd[2] = min(max(cmd[2], minCmd), maxCmd) 35 | # cmd[3] = min(max(cmd[3], minCmd), maxCmd) 36 | 37 | # # Add Exponential to command 38 | # # --------------------------- 39 | # cmd = expoCmd(quad.params, cmd) 40 | 41 | # return cmd 42 | 43 | # def expoCmd(params, cmd): 44 | # if params["ifexpo"]: 45 | # cmd = np.sqrt(cmd)*10 46 | 47 | # return cmd 48 | 49 | # def expoCmdInv(params, cmd): 50 | # if params["ifexpo"]: 51 | # cmd = (cmd/10)**2 52 | 53 | # return cmd 54 | -------------------------------------------------------------------------------- /Simulation/utils/quaternionFunctions.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | author: John Bass 4 | email: john.bobzwik@gmail.com 5 | license: MIT 6 | Please feel free to use and modify this, but keep the above information. Thanks! 7 | """ 8 | 9 | import numpy as np 10 | from numpy import sin, cos 11 | from numpy.linalg import norm 12 | 13 | 14 | # Normalize quaternion, or any vector 15 | def vectNormalize(q): 16 | return q/norm(q) 17 | 18 | 19 | # Quaternion multiplication 20 | def quatMultiply(q, p): 21 | Q = np.array([[q[0], -q[1], -q[2], -q[3]], 22 | [q[1], q[0], -q[3], q[2]], 23 | [q[2], q[3], q[0], -q[1]], 24 | [q[3], -q[2], q[1], q[0]]]) 25 | return Q@p 26 | 27 | 28 | # Inverse quaternion 29 | def inverse(q): 30 | qinv = np.array([q[0], -q[1], -q[2], -q[3]])/norm(q) 31 | return qinv -------------------------------------------------------------------------------- /Simulation/utils/rotationConversion.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | import numpy as np 4 | from numpy import sin, cos 5 | from numpy.linalg import norm 6 | 7 | def quatToYPR_ZYX(q): 8 | # [q0 q1 q2 q3] = [w x y z] 9 | q0 = q[0] 10 | q1 = q[1] 11 | q2 = q[2] 12 | q3 = q[3] 13 | 14 | YPR = threeaxisrot( 2.0*(q1*q2 + q0*q3), \ 15 | q0**2 + q1**2 - q2**2 - q3**2, \ 16 | -2.0*(q1*q3 - q0*q2), \ 17 | 2.0*(q2*q3 + q0*q1), \ 18 | q0**2 - q1**2 - q2**2 + q3**2) 19 | 20 | # YPR = [Yaw, pitch, roll] = [psi, theta, phi] 21 | return YPR 22 | 23 | def threeaxisrot(r11, r12, r21, r31, r32): 24 | r1 = np.arctan2(r11, r12) 25 | r2 = np.arcsin(r21) 26 | r3 = np.arctan2(r31, r32) 27 | 28 | return np.array([r1, r2, r3]) 29 | 30 | def YPRToQuat(r1, r2, r3): 31 | # For ZYX, Yaw-Pitch-Roll 32 | # psi = RPY[0] = r1 33 | # theta = RPY[1] = r2 34 | # phi = RPY[2] = r3 35 | 36 | cr1 = cos(0.5*r1) 37 | cr2 = cos(0.5*r2) 38 | cr3 = cos(0.5*r3) 39 | sr1 = sin(0.5*r1) 40 | sr2 = sin(0.5*r2) 41 | sr3 = sin(0.5*r3) 42 | 43 | q0 = cr1*cr2*cr3 + sr1*sr2*sr3 44 | q1 = cr1*cr2*sr3 - sr1*sr2*cr3 45 | q2 = cr1*sr2*cr3 + sr1*cr2*sr3 46 | q3 = sr1*cr2*cr3 - cr1*sr2*sr3 47 | 48 | # e0,e1,e2,e3 = qw,qx,qy,qz 49 | q = np.array([q0,q1,q2,q3]) 50 | # q = q*np.sign(e0) 51 | 52 | q = q/norm(q) 53 | 54 | return q 55 | 56 | def quat2Dcm(q): 57 | dcm = np.zeros([3,3]) 58 | 59 | dcm[0,0] = q[0]**2 + q[1]**2 - q[2]**2 - q[3]**2 60 | dcm[0,1] = 2.0*(q[1]*q[2] - q[0]*q[3]) 61 | dcm[0,2] = 2.0*(q[1]*q[3] + q[0]*q[2]) 62 | dcm[1,0] = 2.0*(q[1]*q[2] + q[0]*q[3]) 63 | dcm[1,1] = q[0]**2 - q[1]**2 + q[2]**2 - q[3]**2 64 | dcm[1,2] = 2.0*(q[2]*q[3] - q[0]*q[1]) 65 | dcm[2,0] = 2.0*(q[1]*q[3] - q[0]*q[2]) 66 | dcm[2,1] = 2.0*(q[2]*q[3] + q[0]*q[1]) 67 | dcm[2,2] = q[0]**2 - q[1]**2 - q[2]**2 + q[3]**2 68 | 69 | return dcm 70 | 71 | def RotToQuat(R): 72 | 73 | R11 = R[0, 0] 74 | R12 = R[0, 1] 75 | R13 = R[0, 2] 76 | R21 = R[1, 0] 77 | R22 = R[1, 1] 78 | R23 = R[1, 2] 79 | R31 = R[2, 0] 80 | R32 = R[2, 1] 81 | R33 = R[2, 2] 82 | # From page 68 of MotionGenesis book 83 | tr = R11 + R22 + R33 84 | 85 | if tr > R11 and tr > R22 and tr > R33: 86 | e0 = 0.5 * np.sqrt(1 + tr) 87 | r = 0.25 / e0 88 | e1 = (R32 - R23) * r 89 | e2 = (R13 - R31) * r 90 | e3 = (R21 - R12) * r 91 | elif R11 > R22 and R11 > R33: 92 | e1 = 0.5 * np.sqrt(1 - tr + 2*R11) 93 | r = 0.25 / e1 94 | e0 = (R32 - R23) * r 95 | e2 = (R12 + R21) * r 96 | e3 = (R13 + R31) * r 97 | elif R22 > R33: 98 | e2 = 0.5 * np.sqrt(1 - tr + 2*R22) 99 | r = 0.25 / e2 100 | e0 = (R13 - R31) * r 101 | e1 = (R12 + R21) * r 102 | e3 = (R23 + R32) * r 103 | else: 104 | e3 = 0.5 * np.sqrt(1 - tr + 2*R33) 105 | r = 0.25 / e3 106 | e0 = (R21 - R12) * r 107 | e1 = (R13 + R31) * r 108 | e2 = (R23 + R32) * r 109 | 110 | # e0,e1,e2,e3 = qw,qx,qy,qz 111 | q = np.array([e0,e1,e2,e3]) 112 | q = q*np.sign(e0) 113 | 114 | q = q/np.sqrt(np.sum(q[0]**2 + q[1]**2 + q[2]**2 + q[3]**2)) 115 | 116 | return q 117 | 118 | 119 | # def RPYtoRot_ZYX(RPY): 120 | 121 | # phi = RPY[0] 122 | # theta = RPY[1] 123 | # psi = RPY[2] 124 | 125 | # # R = np.array([[cos(psi)*cos(theta) - sin(phi)*sin(psi)*sin(theta), 126 | # # cos(theta)*sin(psi) + cos(psi)*sin(phi)*sin(theta), 127 | # # -cos(phi)*sin(theta)], 128 | # # [-cos(phi)*sin(psi), 129 | # # cos(phi)*cos(psi), 130 | # # sin(phi)], 131 | # # [cos(psi)*sin(theta) + cos(theta)*sin(phi)*sin(psi), 132 | # # sin(psi)*sin(theta) - cos(psi)*cos(theta)*sin(phi), 133 | # # cos(phi)*cos(theta)]]) 134 | 135 | # r1 = psi 136 | # r2 = theta 137 | # r3 = phi 138 | # # Rotation ZYX from page 277 of MotionGenesis book 139 | # R = np.array([[cos(r1)*cos(r2), 140 | # -sin(r1)*cos(r3) + sin(r2)*sin(r3)*cos(r1), 141 | # sin(r1)*sin(r3) + sin(r2)*cos(r1)*cos(r3)], 142 | # [sin(r1)*cos(r2), 143 | # cos(r1)*cos(r3) + sin(r1)*sin(r2)*sin(r3), 144 | # -sin(r3)*cos(r1) + sin(r1)*sin(r2)*cos(r3)], 145 | # [-sin(r2), 146 | # sin(r3)*cos(r2), 147 | # cos(r2)*cos(r3)]]) 148 | 149 | # return R 150 | 151 | -------------------------------------------------------------------------------- /Simulation/utils/stateConversions.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | author: John Bass 4 | email: john.bobzwik@gmail.com 5 | license: MIT 6 | Please feel free to use and modify this, but keep the above information. Thanks! 7 | """ 8 | 9 | import numpy as np 10 | from numpy import sin, cos, tan 11 | 12 | def phiThetaPsiDotToPQR(phi, theta, psi, phidot, thetadot, psidot): 13 | 14 | p = -sin(theta)*psidot + phidot 15 | 16 | q = sin(phi)*cos(theta)*psidot + cos(phi)*thetadot 17 | 18 | r = -sin(phi)*thetadot + cos(phi)*cos(theta)*psidot 19 | 20 | return np.array([p, q, r]) 21 | 22 | 23 | def xyzDotToUVW_euler(phi, theta, psi, xdot, ydot, zdot): 24 | u = xdot*cos(psi)*cos(theta) + ydot*sin(psi)*cos(theta) - zdot*sin(theta) 25 | 26 | v = (sin(phi)*sin(psi)*sin(theta) + cos(phi)*cos(psi))*ydot + (sin(phi)*sin(theta)*cos(psi) - sin(psi)*cos(phi))*xdot + zdot*sin(phi)*cos(theta) 27 | 28 | w = (sin(phi)*sin(psi) + sin(theta)*cos(phi)*cos(psi))*xdot + (-sin(phi)*cos(psi) + sin(psi)*sin(theta)*cos(phi))*ydot + zdot*cos(phi)*cos(theta) 29 | 30 | return np.array([u, v, w]) 31 | 32 | 33 | def xyzDotToUVW_Flat_euler(phi, theta, psi, xdot, ydot, zdot): 34 | uFlat = xdot * cos(psi) + ydot * sin(psi) 35 | 36 | vFlat = -xdot * sin(psi) + ydot * cos(psi) 37 | 38 | wFlat = zdot 39 | 40 | return np.array([uFlat, vFlat, wFlat]) 41 | 42 | def xyzDotToUVW_Flat_quat(q, xdot, ydot, zdot): 43 | q0 = q[0] 44 | q1 = q[1] 45 | q2 = q[2] 46 | q3 = q[3] 47 | 48 | uFlat = 2*(q0*q3 - q1*q2)*ydot + (q0**2 - q1**2 + q2**2 - q3**2)*xdot 49 | 50 | vFlat = -2*(q0*q3 + q1*q2)*xdot + (q0**2 + q1**2 - q2**2 - q3**2)*ydot 51 | 52 | wFlat = zdot 53 | 54 | return np.array([uFlat, vFlat, wFlat]) 55 | -------------------------------------------------------------------------------- /Simulation/utils/windModel.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | author: John Bass 4 | email: john.bobzwik@gmail.com 5 | license: MIT 6 | Please feel free to use and modify this, but keep the above information. Thanks! 7 | """ 8 | 9 | import numpy as np 10 | from numpy import sin, cos, pi 11 | import random as rd 12 | import config 13 | 14 | deg2rad = pi/180.0 15 | 16 | class Wind: 17 | 18 | def __init__(self, *args): 19 | 20 | if (len(args) == 0): 21 | self.windType = 'NONE' 22 | elif not isinstance(args[0], str): 23 | raise Exception('Not a valid wind type.') 24 | else: 25 | self.windType = args[0].upper() 26 | 27 | if (self.windType == 'SINE') or (self.windType == 'RANDOMSINE'): 28 | 29 | if (self.windType == 'SINE'): 30 | 31 | self.velW_med = args[1] 32 | self.qW1_med = args[2]*deg2rad 33 | self.qW2_med = args[3]*deg2rad 34 | 35 | elif (self.windType == 'RANDOMSINE'): 36 | 37 | # Wind Velocity 38 | velW_max = args[1] # m/s 39 | velW_min = args[2] # m/s 40 | # Wind Heading 41 | qW1_max = args[3] # deg 42 | qW1_min = args[4] # deg 43 | # Wind Elevation (positive = upwards wind in NED, positive = downwards wind in ENU) 44 | qW2_max = args[5] # deg 45 | qW2_min = args[6] # deg 46 | 47 | # Median values 48 | self.velW_med = (velW_max - velW_min)*rd.random() + velW_min 49 | self.qW1_med = ((qW1_max - qW1_min)*rd.random() + qW1_min)*deg2rad 50 | self.qW2_med = ((qW2_max - qW2_min)*rd.random() + qW2_min)*deg2rad 51 | 52 | # Wind Velocity 53 | self.velW_a1 = 1.5 # Wind velocity amplitude 1 54 | self.velW_f1 = 0.7 # Wind velocity frequency 1 55 | self.velW_d1 = 0 # Wind velocity delay (offset) 1 56 | self.velW_a2 = 1.1 # Wind velocity amplitude 2 57 | self.velW_f2 = 1.2 # Wind velocity frequency 2 58 | self.velW_d2 = 1.3 # Wind velocity delay (offset) 2 59 | self.velW_a3 = 0.8 # Wind velocity amplitude 3 60 | self.velW_f3 = 2.3 # Wind velocity frequency 3 61 | self.velW_d3 = 2.0 # Wind velocity delay (offset) 3 62 | 63 | # Wind Heading 64 | self.qW1_a1 = 15.0*deg2rad # Wind heading amplitude 1 65 | self.qW1_f1 = 0.1 # Wind heading frequency 1 66 | self.qW1_d1 = 0 # Wind heading delay (offset) 1 67 | self.qW1_a2 = 3.0*deg2rad # Wind heading amplitude 2 68 | self.qW1_f2 = 0.54 # Wind heading frequency 2 69 | self.qW1_d2 = 0 # Wind heading delay (offset) 2 70 | 71 | # Wind Elevation 72 | self.qW2_a1 = 4.0*deg2rad # Wind elevation amplitude 1 73 | self.qW2_f1 = 0.1 # Wind elevation frequency 1 74 | self.qW2_d1 = 0 # Wind elevation delay (offset) 1 75 | self.qW2_a2 = 0.8*deg2rad # Wind elevation amplitude 2 76 | self.qW2_f2 = 0.54 # Wind elevation frequency 2 77 | self.qW2_d2 = 0 # Wind elevation delay (offset) 2 78 | 79 | elif (self.windType == 'FIXED'): 80 | 81 | self.velW_med = args[1] 82 | self.qW1_med = args[2]*deg2rad 83 | self.qW2_med = args[3]*deg2rad 84 | 85 | elif (self.windType == 'NONE'): 86 | 87 | self.velW_med = 0 88 | self.qW1_med = 0 89 | self.qW2_med = 0 90 | 91 | else: 92 | 93 | raise Exception('Not a valid wind type.') 94 | 95 | 96 | def randomWind(self, t): 97 | if (self.windType == 'SINE') or (self.windType == 'RANDOMSINE'): 98 | 99 | velW = self.velW_a1*sin(self.velW_f1*t - self.velW_d1) + self.velW_a2*sin(self.velW_f2*t - self.velW_d2) + self.velW_a3*sin(self.velW_f3*t - self.velW_d3) + self.velW_med 100 | qW1 = self.qW1_a1*sin(self.qW1_f1*t - self.qW1_d1) + self.qW1_a2*sin(self.qW1_f2*t - self.qW1_d2) + self.qW1_med 101 | qW2 = self.qW2_a1*sin(self.qW2_f1*t - self.qW2_d1) + self.qW2_a2*sin(self.qW2_f2*t - self.qW2_d2) + self.qW2_med 102 | 103 | velW = max([0, velW]) 104 | 105 | elif (self.windType == 'FIXED') or (self.windType == 'NONE'): 106 | velW = self.velW_med 107 | qW1 = self.qW1_med 108 | qW2 = self.qW2_med 109 | 110 | return velW, qW1, qW2 -------------------------------------------------------------------------------- /Simulation/waypoints.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | author: John Bass 4 | email: john.bobzwik@gmail.com 5 | license: MIT 6 | Please feel free to use and modify this, but keep the above information. Thanks! 7 | """ 8 | 9 | import numpy as np 10 | from numpy import pi 11 | import config 12 | 13 | deg2rad = pi/180.0 14 | 15 | def makeWaypoints(): 16 | 17 | v_average = 1.6 18 | 19 | t_ini = 3 20 | t = np.array([2, 0, 2, 0]) 21 | 22 | wp_ini = np.array([0, 0, 0]) 23 | wp = np.array([[2, 2, 1], 24 | [-2, 3, -3], 25 | [-2, -1, -3], 26 | [3, -2, 1], 27 | wp_ini]) 28 | 29 | yaw_ini = 0 30 | yaw = np.array([20, -90, 120, 45]) 31 | 32 | t = np.hstack((t_ini, t)).astype(float) 33 | wp = np.vstack((wp_ini, wp)).astype(float) 34 | yaw = np.hstack((yaw_ini, yaw)).astype(float)*deg2rad 35 | 36 | return t, wp, yaw, v_average 37 | --------------------------------------------------------------------------------