Hi
thanks for sharing.
I could not understand the way the trajectory works.
Could you please let me know how you set 1,1,1 for x, y, z as the desired trajectory?
thanks in advance.

i am doing my university project at swarming of two drones can you assist me

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?

Hi,

I want to reference your repo. If you want please let me know your full name and I can add it to the references section. Otherwise, I can only cite the repo but without your name because I am not sure if this is your last name or not.
BTW, Thanks for the code.

For python version > 3.6:

When running the current code, python raises the following warning:

\Quadcopter_SimCon-master\Simulation\utils\animation.py:40: MatplotlibDeprecationWarning: Axes3D(fig) adding itself to the figure is deprecated since 3.4. Pass the keyword argument auto_add_to_figure=False and use fig.add_axes(ax) to suppress this warning. The default value of auto_add_to_figure will change to False in mpl3.5 and True values will no longer work in 3.6. This is consistent with other Axes classes.
ax = p3.Axes3D(fig)

To solve this: open ./Simulation/utils/animation.py and replace line 40:
ax = p3.Axes3D(fig)

with the following two lines:

ax = p3.Axes3D(fig,auto_add_to_figure=False)
fig.add_axes(ax)