Double pendulum equation of motion in python

What is a double pendulum?

Previously, in the article Equation of motion with sympy, I used sympy to derive the equation of motion of the spring-damper system. This time I would like to derive the equation of motion of the double pendulum. The double pendulum is often seen in physical mechanics, and as shown in the figure below, a pendulum is attached to the tip of the pendulum. It's just a double pendulum, but it's a complex movement that's incomparable to a single pendulum, so it's often mentioned in chaos and complex systems.

Formula

First, let's look at the equation of motion of a simple pendulum. (Imagine a single pendulum in the figure above.)

ml\ddot{\theta} + mg \sin\theta = 0

You can see that one item represents the inertial term and two items represent the gravity term, and the angular acceleration of the pendulum is proportional to the $ \ sin $ of the pendulum angle at that time. Next is the double pendulum. When it comes to deriving seriously, it will be a long story using the Lagrange function, so this time I will check the formula Wikipedia 8C% E9% 87% 8D% E6% 8C% AF% E3% 82% 8A% E5% AD% 90) is taken quickly.

(m_1+m_2)l_1\ddot{\theta}_1 + m_2l_2\ddot{\theta}_2\cos(\theta_1 - \theta_2) + m_2l_2\dot{\theta}_2^2\sin(\theta_1 - \theta_2) + (m_1 + m_2)g\sin\theta_1 = 0
\\\
l_1l_2\ddot{\theta}_1\cos(\theta_1- \theta_2) + l_2^2\ddot{\theta}_2 - l_1l_2\dot{\theta}_1^2\sin(\theta_1 - \theta_2) + gl_2\sin\theta_2 = 0

Just by looking at the equation, you can see that it is considerably more complicated than the equation of motion of a simple pendulum.

SymPyBotics Last time, I used the physics.mechanics module of SymPy directly to derive the formula, but the rigid body like this time is a clause. I would like to use SymPyBotics, which makes it easier to derive mathematical formulas for objects connected by (link). Get it from github and install it.

git clone https://github.com/cdsousa/SymPyBotics.git
cd SymPyBotics
sudo python setup.py install

Create a double pendulum using SymPyBotics. Below is the code.

import sympybotics
rbtdef = sympybotics.RobotDef('Double Pendulum',
                              [(0, 'l_1', 0, 'q-pi/2'),
                               (0, 'l_2', 0, 'q')],
                              dh_convention='standard')
from sympy import symbols, Matrix, simplify, pprint
g = symbols('g')
rbtdef.gravityacc = Matrix([[0.0],[-g],[0.0]])
rbt = sympybotics.RobotDynCode(rbtdef)
for p in rbt.geo.p:
    pprint(simplify(p), use_unicode=False)

The place where you are calling sympybotics.RobotDef is where you are creating the double pendulum. [(0,'l_1', 0,'q'), (0,'l_2', 0,'q')] is the actual double pendulum parameter, but Denavit–Hartenberg parameter is a method of giving parameters used by robots. Briefly, l_1 and l_2 indicate the length of pendulum 1 and pendulum 2, and q indicates the angle of the pendulum, which are assigned to the variables q1 and q2 after the pendulum is created. The first angle is q1-pi / 2, which is added to adjust where the angle is measured. This is because SymPyBotics measures the angle from the positive x-axis, whereas the figure measures the angle from the negative y-axis. If you want to use a triple pendulum, [(0,'l_1', 0,'q-pi / 2'), (0,'l_2', 0,'q'), (0,'l_3', 0,' q')] . The last print I wrote outputs an expression that expresses the position of the pendulum, which is as follows.

[l_1*sin(q1) ]
[            ]
[-l_1*cos(q1)]
[            ]
[     0      ]
[l_1*sin(q1) + l_2*sin(q1 + q2) ]
[                               ]
[-l_1*cos(q1) - l_2*cos(q1 + q2)]
[                               ]
[               0               ]

rbt.dyn.gen_all()
params = {'l_1x': 0.0, 'l_1y': 0.0,
          'l_2x': 0.0, 'l_2y': 0.0,
          'L_1zz': 0.0, 'L_2zz': 0.0}
mass = simplify(rbt.dyn.M.subs(params))
corioli = simplify(rbt.dyn.c.subs(params))
gravity = simplify(rbt.dyn.g.subs(params))

Next, each term (inertia term, centrifugal / Coriolis force term, gravity term) that is an element of the equation of motion is derived. Generated with rbt.dyn.gen_all () and delete unnecessary parameters (parameters related to the moment of inertia) this time.

eq = (mass * rbtdef.ddq + corioli + gravity).as_mutable()
eq[0, 0] -= eq[1, 0]
params = {rbtdef.ddq[1, 0]: rbtdef.ddq[1, 0] - rbtdef.ddq[0, 0],
          rbtdef.dq[1, 0]: rbtdef.dq[1, 0] - rbtdef.dq[0, 0],
          rbtdef.q[1, 0]: rbtdef.q[1, 0] - rbtdef.q[0, 0]}
print simplify(eq.subs(params))

Finally, we actually calculate the equation of motion. At the time of the first line, the equation of motion has been derived, but [Wikipedia](http://en.wikipedia.org/wiki/%E4%BA%8C%E9%87%8D%E6%8C% AF% E3% 82% 8A% E5% AD% 90) and the equations have been simplified and the variables have been changed to compare the results.

Matrix([
[l_1*(\ddot{q}_1*l_1*m_1 + \ddot{q}_1*l_1*m_2 + \ddot{q}_2*l_2*m_2*cos(q1 - q2) + \dot{q}_2**2*l_2*m_2*sin(q1 - q2) + g*m_1*sin(q1) + g*m_2*sin(q1))],
[                                                 l_2*m_2*(\ddot{q}_1*l_1*cos(q1 - q2) + \ddot{q}_2*l_2 - \dot{q}_1**2*l_1*sin(q1 - q2) + g*sin(q2))]])

It's a little hard to see, but if you think that the formula is equal to 0 and remove unnecessary variables, Wikipedia's equation of motion for the double pendulum E9% 87% 8D% E6% 8C% AF% E3% 82% 8A% E5% AD% 90 # .E9.81.8B.E5.8B.95.E6.96.B9.E7.A8.8B.E5. You can see that it matches BC.8F). This time it was a double pendulum, but it seems that it can also be applied to the derivation of equations of motion for multiple pendulums and robots.