bobzwik / quadcopter_simcon Goto Github PK
View Code? Open in Web Editor NEWQuadcopter Simulation and Control. Dynamics generated with PyDy.
License: MIT License
Quadcopter Simulation and Control. Dynamics generated with PyDy.
License: MIT License
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
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')
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')
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)
#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')
D.orient(N, 'Axis', [psi, N.z])
C.orient(D, 'Axis', [theta, D.y])
B.orient(C, 'Axis', [phi, C.x])
No.set_vel(N, 0)
Bcm.set_pos(No, xN.x + yN.y + zN.z)
Bcm.set_vel(N, uB.x + vB.y + wB.z)
#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)
IB = inertia(B, IBxx, IByy, IBzz)
BodyB = RigidBody('BodyB', Bcm, B, mB, (IB, Bcm))
BodyList = [BodyB]
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]
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)]
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)
A declarative, efficient, and flexible JavaScript library for building user interfaces.
๐ Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
An Open Source Machine Learning Framework for Everyone
The Web framework for perfectionists with deadlines.
A PHP framework for web artisans
Bring data to life with SVG, Canvas and HTML. ๐๐๐
JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
Some thing interesting about web. New door for the world.
A server is a program made to process requests and deliver data to clients.
Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
Some thing interesting about visualization, use data art
Some thing interesting about game, make everyone happy.
We are working to build community through open source technology. NB: members must have two-factor auth.
Open source projects and samples from Microsoft.
Google โค๏ธ Open Source for everyone.
Alibaba Open Source for everyone
Data-Driven Documents codes.
China tencent open source team.