Quadcopter_SimCon
Quadcopter_SimCon copied to clipboard
bobzwik
Unable to use System.integrate from pydy.sys to do numerical simulation of quadcopter code
I am trying to perform numerical simulation of a simplified version of the quadcopter code. The simplifications include removing the 4 motors and all the forces and moments associated with them while retaining gravity (So it's basically a rock in free-fall with 6 degrees of freedom). But I'm getting errors using sys.integrate(). The following is my modified code
`from sympy import symbols from sympy.physics.mechanics import * import numpy as np from pydy.system import System
Reference frames and Points
---------------------------
N = ReferenceFrame('N') # Inertial Frame B = ReferenceFrame('B') # Drone after X (roll) rotation (Final rotation) C = ReferenceFrame('C') # Drone after Y (pitch) rotation D = ReferenceFrame('D') # Drone after Z (yaw) rotation (First rotation)
No = Point('No') Bcm = Point('Bcm') # Drone's center of mass #M1 = Point('M1') # Motor 1 is front left, then the rest increments CW (Drone is in X configuration, not +) #M2 = Point('M2') #M3 = Point('M3') #M4 = Point('M4')
Variables
---------------------------
x, y and z are the drone's coordinates in the inertial frame, expressed with the inertial frame
u, v and w are the drone's velocities in the inertial frame, expressed with the drone's frame
phi, theta and psi represents the drone's orientation in the inertial frame, expressed with a ZYX Body rotation
p, q and r are the drone's angular velocities in the inertial frame, expressed with the drone's frame
x, y, z, u, v, w = dynamicsymbols('x y z u v w') phi, theta, psi, p, q, r = dynamicsymbols('phi theta psi p q r')
First derivatives of the variables
xd, yd, zd, ud, vd, wd = dynamicsymbols('x y z u v w', 1) phid, thetad, psid, pd, qd, rd = dynamicsymbols('phi theta psi p q r', 1)
Constants
---------------------------
#mB, g, dxm, dym, dzm, IBxx, IByy, IBzz = symbols('mB g dxm dym dzm IBxx IByy IBzz') #ThrM1, ThrM2, ThrM3, ThrM4, TorM1, TorM2, TorM3, TorM4 = symbols('ThrM1 ThrM2 ThrM3 ThrM4 TorM1 TorM2 TorM3 TorM4') mB, g, IBxx, IByy, IBzz = symbols('mB g IBxx IByy IBzz')
Rotation ZYX Body
---------------------------
D.orient(N, 'Axis', [psi, N.z]) C.orient(D, 'Axis', [theta, D.y]) B.orient(C, 'Axis', [phi, C.x])
Origin
---------------------------
No.set_vel(N, 0)
Translation
---------------------------
Bcm.set_pos(No, xN.x + yN.y + zN.z) Bcm.set_vel(N, uB.x + vB.y + wB.z)
Motor placement
M1 is front left, then clockwise numbering
dzm is positive for motors above center of mass
---------------------------
#M1.set_pos(Bcm, dxmB.x - dymB.y - dzmB.z) #M2.set_pos(Bcm, dxmB.x + dymB.y - dzmB.z) #M3.set_pos(Bcm, -dxmB.x + dymB.y - dzmB.z) #M4.set_pos(Bcm, -dxmB.x - dymB.y - dzmB.z) #M1.v2pt_theory(Bcm, N, B) #M2.v2pt_theory(Bcm, N, B) #M3.v2pt_theory(Bcm, N, B) #M4.v2pt_theory(Bcm, N, B)
Inertia Dyadic
---------------------------
IB = inertia(B, IBxx, IByy, IBzz)
Create Bodies
---------------------------
BodyB = RigidBody('BodyB', Bcm, B, mB, (IB, Bcm)) BodyList = [BodyB]
Forces and Torques
---------------------------
Grav_Force = (Bcm, mBgN.z) #FM1 = (M1, -ThrM1B.z) #FM2 = (M2, -ThrM2B.z) #FM3 = (M3, -ThrM3B.z) #FM4 = (M4, -ThrM4B.z)
#TM1 = (B, -TorM1B.z) #TM2 = (B, TorM2B.z) #TM3 = (B, -TorM3B.z) #TM4 = (B, TorM4B.z) #ForceList = [Grav_Force, FM1, FM2, FM3, FM4, TM1, TM2, TM3, TM4] ForceList = [Grav_Force]
Kinematic Differential Equations
---------------------------
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)]
Kane's Method
---------------------------
KM = KanesMethod(N, q_ind=[x, y, z, phi, theta, psi], u_ind=[u, v, w, p, q, r], kd_eqs=kd) (fr, frstar) = KM.kanes_equations(BodyList, ForceList)
initial_conditions = {x:0.0, y:0.0, z:-1000.0, phi:0.0, theta:0.0, psi:0.0, u:0.0, v:0.0, w:0.0, p:0.0, q:0.0, r:0.0} constants = {mB:1.0,g:9.81, IBxx:1.0, IByy:1.0, IBzz:1.0} times = np.linspace(0, 5, 100)
sys = System(KM,initial_conditions=initial_conditions,constants=constants,times=times) #sys.generate_ode_function(generator='cython') sys.integrate()`
And the error I'm getting (Running on ipython) : '%run Quad_3D_frd_NED_Euler_uvw.py Traceback (most recent call last):
File "/usr/local/lib/python3.7/dist-packages/pydy/codegen/ode_function_generators.py", line 907, in generate_ode_function g = generator(*args, **kwargs)
TypeError: 'str' object is not callable
During handling of the above exception, another exception occurred:
File "
If I uncomment the line before sys.integrate() for using the cython generator I get a different error : `--------------------------------------------------------------------------- TypeError Traceback (most recent call last) /usr/local/lib/python3.7/dist-packages/pydy/codegen/ode_function_generators.py in generate_ode_function(*args, **kwargs) 906 # See if user passed in a custom class. --> 907 g = generator(*args, **kwargs) 908 except TypeError:
TypeError: 'str' object is not callable
During handling of the above exception, another exception occurred:
ValueError Traceback (most recent call last)
~/work/pydy-master/bin/Quad_3D_frd_NED_Euler_uvw.py in
/usr/local/lib/python3.7/dist-packages/pydy/system.py in generate_ode_function(self, **kwargs) 481 self._evaluate_ode_function = generate_ode_function( 482 *self._args_for_gen_ode_func(), --> 483 **kwargs) 484 485 return self.evaluate_ode_function
/usr/local/lib/python3.7/dist-packages/pydy/codegen/ode_function_generators.py in generate_ode_function(*args, **kwargs) 915 raise NotImplementedError(msg) 916 else: --> 917 return g.generate() 918 else: 919 return g.generate()
/usr/local/lib/python3.7/dist-packages/pydy/codegen/ode_function_generators.py in generate(self) 620 self.generate_full_rhs_function() 621 elif self.system_type == 'full mass matrix': --> 622 self.generate_full_mass_matrix_function() 623 elif self.system_type == 'min mass matrix': 624 self.generate_min_mass_matrix_function()
/usr/local/lib/python3.7/dist-packages/pydy/codegen/ode_function_generators.py in generate_full_mass_matrix_function(self) 698 self.inputs)) 699 else: --> 700 self._set_eval_array(self._cythonize(outputs, self.inputs)) 701 702 def generate_min_mass_matrix_function(self):
/usr/local/lib/python3.7/dist-packages/pydy/codegen/ode_function_generators.py in _cythonize(self, outputs, inputs) 651 g = CythonMatrixGenerator(inputs, outputs, 652 prefix=self._options['prefix'], --> 653 cse=self._options['cse']) 654 return g.compile(tmp_dir=self._options['tmp_dir'], 655 verbose=self._options['verbose'])
/usr/local/lib/python3.7/dist-packages/pydy/codegen/cython_code.py in init(self, arguments, matrices, prefix, cse) 91 self.num_arguments = len(arguments) 92 self.c_matrix_generator = CMatrixGenerator(arguments, matrices, ---> 93 cse=cse) 94 95 self._generate_code_blocks()
/usr/local/lib/python3.7/dist-packages/pydy/codegen/matrix_generator.py in init(self, arguments, matrices, cse) 61 if required_arg not in all_arguments: 62 msg = "{} is missing from the argument sequences." ---> 63 raise ValueError(msg.format(required_arg)) 64 65 self.matrices = matrices
ValueError: Derivative(theta(t), t) is missing from the argument sequences. `
But I am able to run the astrobee example from pydy, which is also a 6DoF model with 6 coordinates and 6 speeds all in the same reference frame.
I am suspecting that the error is due to the combined usage of different reference frames while defining coordinates and speeds in the kane's model. For the coordinates vector, the positions (x,y,z) are in the NED frame, attitudes are euler angles (phi, theta, psi), while for the speeds vector, the velocities (u,v,w) are in the Body FRD frame and angular rates (p,q,r) are also in the Body FRD frame. Thus we don't have x'=u, y'=v and so on. Is my understanding correct? And how would I solve this problem? Note that the equations derived using Kane's method appear to be correct. Or am I missing something?
