Unable to use System.integrate from pydy.sys to do numerical simulation of quadcopter code

[madz-mit]

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 "", line 45
x43 = # Not supported in Python with numpy:
^
SyntaxError: invalid syntax'

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
135
136 sys = System(KM,initial_conditions=initial_conditions,constants=constants,times=times)
--> 137 sys.generate_ode_function(generator='cython')
138 sys.integrate()
139

/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?