Equations of Motion with the Lagrange Method

Note

You can download this example as a Python script: lagrange.py or Jupyter Notebook: lagrange.ipynb.

import sympy as sm
import sympy.physics.mechanics as me
me.init_vprinting(use_latex='mathjax')

class ReferenceFrame(me.ReferenceFrame):

    def __init__(self, *args, **kwargs):

        kwargs.pop('latexs', None)

        lab = args[0].lower()
        tex = r'\hat{{{}}}_{}'

        super(ReferenceFrame, self).__init__(*args,
                                             latexs=(tex.format(lab, 'x'),
                                                     tex.format(lab, 'y'),
                                                     tex.format(lab, 'z')),
                                             **kwargs)
me.ReferenceFrame = ReferenceFrame

Learning Objectives

After completing this chapter readers will be able to:

• Derive the Lagrangian for a system of interconnected particles and rigid bodies

• Use the Euler-Lagrange equation to derive equations of motions given a Lagrangian

• Use the method of Lagrange multipliers to add constraints to the equations of motions

• Determine the generalized momenta of a system

Introduction

This book has already discussed two methods to derive the equations of motion of multibody systems: Newton-Euler and Kane’s method. This chapter will add a third: the Lagrange method, originally developed by Joseph-Louis Lagrange. These materials focus on Engineering applications for multibody systems, and therefore build the Lagrange method around the terms found earlier in Kane’s equations. In other textbooks, the Lagrange method is often derived from the Variational principles, such as virtual work or the principle of least action. A good starting point for studying the physical and mathematical background of the Lagrange approach is [Lanczos1986].

Inertial Forces from Kinetic Energy

In Kane’s method the negated generalized inertial forces equal the applied forces, see Unconstrained Equations of Motion. A large part of Kane’s method of deriving the equations of motions for a system is involved with finding the generalized inertial forces. As an alternative, the following equation also calculates the generalized inertial forces of a system, now by starting from the kinetic energy \(K (\dot{\bar{q}}, \bar{q})\) expressed as function of the generalized coordinates \(\bar{q}\), and their time derivatives. See Energy and Power for the definition of kinetic energy.

(225)\[ -\bar{F}^*_r = \frac{\mathrm{d}}{\mathrm{d}t}\left(\frac{\partial K}{\partial \dot{q}_r} \right) - \frac{\partial K}{\partial q_r}\]

Warning

Note the two minus signs in the above equation

Note

In Kane’s method, it is possible to choose generalized speeds \(\bar{u}\) that differ from the time derivatives of the generalized coordinates \(\dot{\bar{q}}\). By convention \(\bar{u} = \dot{\bar{q}}\) is assumed when using the Lagrange method. Therefore, throughout this chapter \(\dot{\bar{q}}\) is used.

The generalized inertial forces computed in this manner are the same as when following Kane’s method or the TMT method, used in the next chapter. This can be shown by carefully matching terms in these formulations, as is done for a system of point-masses in [Vallery2020].

Example: Torque Free Rigid Body

This example is largely the same as the example in Body Fixed Newton-Euler Equations. A key difference is a difference between the generalized speeds describing the rotation. In the calculation with Kane’s method, they were body-fixed angular velocities, whereas here they are the rates of change of the Euler angles.

First, set up the generalized coordinates, reference frames, and mass properties for a single rigid body located by coordinates \(x,y,z\) from point \(O\) and oriented by Euler angles \(\psi,\theta,\phi\) relative to the inertial reference frame \(N\):

t = me.dynamicsymbols._t
m, Ixx, Iyy, Izz = sm.symbols('m, I_{xx}, I_{yy}, I_{zz}')
psi, theta, phi, x, y, z = me.dynamicsymbols('psi, theta, phi, x, y, z')

q = sm.Matrix([psi, theta, phi, x, y, z])
qd = q.diff(t)
qdd = qd.diff(t)

q, qd, qdd

\[\begin{split}\displaystyle \left( \left[\begin{matrix}\psi\\\theta\\\phi\\x\\y\\z\end{matrix}\right], \ \left[\begin{matrix}\dot{\psi}\\\dot{\theta}\\\dot{\phi}\\\dot{x}\\\dot{y}\\\dot{z}\end{matrix}\right], \ \left[\begin{matrix}\ddot{\psi}\\\ddot{\theta}\\\ddot{\phi}\\\ddot{x}\\\ddot{y}\\\ddot{z}\end{matrix}\right]\right)\end{split}\]

N = me.ReferenceFrame('N')
B = me.ReferenceFrame('B')
B.orient_body_fixed(N, (psi, theta, phi), 'zxy')

I_B = me.inertia(B, Ixx, Iyy, Izz)

Then compute the kinetic energy:

N_w_B = B.ang_vel_in(N)
r_O_Bo = x*N.x + y*N.y + z*N.z
N_v_C = r_O_Bo.dt(N)
K = m*N_v_C.dot(N_v_C)/2 + N_w_B.dot(I_B.dot(N_w_B))/2
K

\[\displaystyle \frac{I_{xx} \left(- \sin{\left(\phi \right)} \cos{\left(\theta \right)} \dot{\psi} + \cos{\left(\phi \right)} \dot{\theta}\right)^{2}}{2} + \frac{I_{yy} \left(\sin{\left(\theta \right)} \dot{\psi} + \dot{\phi}\right)^{2}}{2} + \frac{I_{zz} \left(\sin{\left(\phi \right)} \dot{\theta} + \cos{\left(\phi \right)} \cos{\left(\theta \right)} \dot{\psi}\right)^{2}}{2} + \frac{m \left(\dot{x}^{2} + \dot{y}^{2} + \dot{z}^{2}\right)}{2}\]

Use the kinetic energy to find the generalized inertial forces. Here we, as an example, start with the generalized coordinate \(\psi\):

F_psi_s = K.diff(psi.diff(t)).diff(t) - K.diff(psi)
F_psi_s

\[\displaystyle I_{xx} \left(- \sin{\left(\phi \right)} \cos{\left(\theta \right)} \dot{\psi} + \cos{\left(\phi \right)} \dot{\theta}\right) \sin{\left(\phi \right)} \sin{\left(\theta \right)} \dot{\theta} - I_{xx} \left(- \sin{\left(\phi \right)} \cos{\left(\theta \right)} \dot{\psi} + \cos{\left(\phi \right)} \dot{\theta}\right) \cos{\left(\phi \right)} \cos{\left(\theta \right)} \dot{\phi} - I_{xx} \left(\sin{\left(\phi \right)} \sin{\left(\theta \right)} \dot{\psi} \dot{\theta} - \sin{\left(\phi \right)} \cos{\left(\theta \right)} \ddot{\psi} - \sin{\left(\phi \right)} \dot{\phi} \dot{\theta} - \cos{\left(\phi \right)} \cos{\left(\theta \right)} \dot{\phi} \dot{\psi} + \cos{\left(\phi \right)} \ddot{\theta}\right) \sin{\left(\phi \right)} \cos{\left(\theta \right)} + I_{yy} \left(\sin{\left(\theta \right)} \dot{\psi} + \dot{\phi}\right) \cos{\left(\theta \right)} \dot{\theta} + I_{yy} \left(\sin{\left(\theta \right)} \ddot{\psi} + \cos{\left(\theta \right)} \dot{\psi} \dot{\theta} + \ddot{\phi}\right) \sin{\left(\theta \right)} - I_{zz} \left(\sin{\left(\phi \right)} \dot{\theta} + \cos{\left(\phi \right)} \cos{\left(\theta \right)} \dot{\psi}\right) \sin{\left(\phi \right)} \cos{\left(\theta \right)} \dot{\phi} - I_{zz} \left(\sin{\left(\phi \right)} \dot{\theta} + \cos{\left(\phi \right)} \cos{\left(\theta \right)} \dot{\psi}\right) \sin{\left(\theta \right)} \cos{\left(\phi \right)} \dot{\theta} + I_{zz} \left(- \sin{\left(\phi \right)} \cos{\left(\theta \right)} \dot{\phi} \dot{\psi} + \sin{\left(\phi \right)} \ddot{\theta} - \sin{\left(\theta \right)} \cos{\left(\phi \right)} \dot{\psi} \dot{\theta} + \cos{\left(\phi \right)} \cos{\left(\theta \right)} \ddot{\psi} + \cos{\left(\phi \right)} \dot{\phi} \dot{\theta}\right) \cos{\left(\phi \right)} \cos{\left(\theta \right)}\]

A similar derivation should be made for all generalized coordinates. We could write a loop, but there there is a method to derive all the equations in one go. The vector of partial derivatives of a function, that is the gradient, can be created using the jacobian() method. The generalized inertial forces can then be found like this:

K_as_matrix = sm.Matrix([K])
Fs = (K_as_matrix.jacobian(qd).diff(t) - K_as_matrix.jacobian(q)).transpose()
Fs

\[\begin{split}\displaystyle \left[\begin{matrix}I_{xx} \left(- \sin{\left(\phi \right)} \cos{\left(\theta \right)} \dot{\psi} + \cos{\left(\phi \right)} \dot{\theta}\right) \sin{\left(\phi \right)} \sin{\left(\theta \right)} \dot{\theta} - I_{xx} \left(- \sin{\left(\phi \right)} \cos{\left(\theta \right)} \dot{\psi} + \cos{\left(\phi \right)} \dot{\theta}\right) \cos{\left(\phi \right)} \cos{\left(\theta \right)} \dot{\phi} - I_{xx} \left(\sin{\left(\phi \right)} \sin{\left(\theta \right)} \dot{\psi} \dot{\theta} - \sin{\left(\phi \right)} \cos{\left(\theta \right)} \ddot{\psi} - \sin{\left(\phi \right)} \dot{\phi} \dot{\theta} - \cos{\left(\phi \right)} \cos{\left(\theta \right)} \dot{\phi} \dot{\psi} + \cos{\left(\phi \right)} \ddot{\theta}\right) \sin{\left(\phi \right)} \cos{\left(\theta \right)} + I_{yy} \left(\sin{\left(\theta \right)} \dot{\psi} + \dot{\phi}\right) \cos{\left(\theta \right)} \dot{\theta} + I_{yy} \left(\sin{\left(\theta \right)} \ddot{\psi} + \cos{\left(\theta \right)} \dot{\psi} \dot{\theta} + \ddot{\phi}\right) \sin{\left(\theta \right)} - I_{zz} \left(\sin{\left(\phi \right)} \dot{\theta} + \cos{\left(\phi \right)} \cos{\left(\theta \right)} \dot{\psi}\right) \sin{\left(\phi \right)} \cos{\left(\theta \right)} \dot{\phi} - I_{zz} \left(\sin{\left(\phi \right)} \dot{\theta} + \cos{\left(\phi \right)} \cos{\left(\theta \right)} \dot{\psi}\right) \sin{\left(\theta \right)} \cos{\left(\phi \right)} \dot{\theta} + I_{zz} \left(- \sin{\left(\phi \right)} \cos{\left(\theta \right)} \dot{\phi} \dot{\psi} + \sin{\left(\phi \right)} \ddot{\theta} - \sin{\left(\theta \right)} \cos{\left(\phi \right)} \dot{\psi} \dot{\theta} + \cos{\left(\phi \right)} \cos{\left(\theta \right)} \ddot{\psi} + \cos{\left(\phi \right)} \dot{\phi} \dot{\theta}\right) \cos{\left(\phi \right)} \cos{\left(\theta \right)}\\- I_{xx} \left(- \sin{\left(\phi \right)} \cos{\left(\theta \right)} \dot{\psi} + \cos{\left(\phi \right)} \dot{\theta}\right) \sin{\left(\phi \right)} \sin{\left(\theta \right)} \dot{\psi} - I_{xx} \left(- \sin{\left(\phi \right)} \cos{\left(\theta \right)} \dot{\psi} + \cos{\left(\phi \right)} \dot{\theta}\right) \sin{\left(\phi \right)} \dot{\phi} + I_{xx} \left(\sin{\left(\phi \right)} \sin{\left(\theta \right)} \dot{\psi} \dot{\theta} - \sin{\left(\phi \right)} \cos{\left(\theta \right)} \ddot{\psi} - \sin{\left(\phi \right)} \dot{\phi} \dot{\theta} - \cos{\left(\phi \right)} \cos{\left(\theta \right)} \dot{\phi} \dot{\psi} + \cos{\left(\phi \right)} \ddot{\theta}\right) \cos{\left(\phi \right)} - I_{yy} \left(\sin{\left(\theta \right)} \dot{\psi} + \dot{\phi}\right) \cos{\left(\theta \right)} \dot{\psi} + I_{zz} \left(\sin{\left(\phi \right)} \dot{\theta} + \cos{\left(\phi \right)} \cos{\left(\theta \right)} \dot{\psi}\right) \sin{\left(\theta \right)} \cos{\left(\phi \right)} \dot{\psi} + I_{zz} \left(\sin{\left(\phi \right)} \dot{\theta} + \cos{\left(\phi \right)} \cos{\left(\theta \right)} \dot{\psi}\right) \cos{\left(\phi \right)} \dot{\phi} + I_{zz} \left(- \sin{\left(\phi \right)} \cos{\left(\theta \right)} \dot{\phi} \dot{\psi} + \sin{\left(\phi \right)} \ddot{\theta} - \sin{\left(\theta \right)} \cos{\left(\phi \right)} \dot{\psi} \dot{\theta} + \cos{\left(\phi \right)} \cos{\left(\theta \right)} \ddot{\psi} + \cos{\left(\phi \right)} \dot{\phi} \dot{\theta}\right) \sin{\left(\phi \right)}\\- \frac{I_{xx} \left(- 2 \sin{\left(\phi \right)} \dot{\theta} - 2 \cos{\left(\phi \right)} \cos{\left(\theta \right)} \dot{\psi}\right) \left(- \sin{\left(\phi \right)} \cos{\left(\theta \right)} \dot{\psi} + \cos{\left(\phi \right)} \dot{\theta}\right)}{2} + \frac{I_{yy} \left(2 \sin{\left(\theta \right)} \ddot{\psi} + 2 \cos{\left(\theta \right)} \dot{\psi} \dot{\theta} + 2 \ddot{\phi}\right)}{2} - \frac{I_{zz} \left(\sin{\left(\phi \right)} \dot{\theta} + \cos{\left(\phi \right)} \cos{\left(\theta \right)} \dot{\psi}\right) \left(- 2 \sin{\left(\phi \right)} \cos{\left(\theta \right)} \dot{\psi} + 2 \cos{\left(\phi \right)} \dot{\theta}\right)}{2}\\m \ddot{x}\\m \ddot{y}\\m \ddot{z}\end{matrix}\right]\end{split}\]

While these are correct generalized inertia forces, the terms, particularly the terms involving \(\ddot{q}_r\) are mangled. It is common to extract the system mass matrix \(\mathbf{M}_d\) and velocity related force vector \(\bar{g}_d\) like before:

Md = Fs.jacobian(qdd)
sm.trigsimp(Md)

\[\begin{split}\displaystyle \left[\begin{matrix}I_{xx} \sin^{2}{\left(\phi \right)} \cos^{2}{\left(\theta \right)} + I_{yy} \sin^{2}{\left(\theta \right)} + I_{zz} \cos^{2}{\left(\phi \right)} \cos^{2}{\left(\theta \right)} & \left(- I_{xx} + I_{zz}\right) \sin{\left(\phi \right)} \cos{\left(\phi \right)} \cos{\left(\theta \right)} & I_{yy} \sin{\left(\theta \right)} & 0 & 0 & 0\\\left(- I_{xx} + I_{zz}\right) \sin{\left(\phi \right)} \cos{\left(\phi \right)} \cos{\left(\theta \right)} & I_{xx} \cos^{2}{\left(\phi \right)} + I_{zz} \sin^{2}{\left(\phi \right)} & 0 & 0 & 0 & 0\\I_{yy} \sin{\left(\theta \right)} & 0 & I_{yy} & 0 & 0 & 0\\0 & 0 & 0 & m & 0 & 0\\0 & 0 & 0 & 0 & m & 0\\0 & 0 & 0 & 0 & 0 & m\end{matrix}\right]\end{split}\]

qdd_zerod = {qddr: 0 for qddr in qdd}
gd = Fs.xreplace(qdd_zerod)
sm.trigsimp(gd)

\[\begin{split}\displaystyle \left[\begin{matrix}\frac{I_{xx} \sin{\left(2 \left(\phi - \theta\right) \right)} \dot{\phi} \dot{\psi}}{4} - \frac{I_{xx} \sin{\left(2 \left(\phi - \theta\right) \right)} \dot{\psi} \dot{\theta}}{4} + \frac{I_{xx} \sin{\left(2 \left(\phi + \theta\right) \right)} \dot{\phi} \dot{\psi}}{4} + \frac{I_{xx} \sin{\left(2 \left(\phi + \theta\right) \right)} \dot{\psi} \dot{\theta}}{4} + \frac{I_{xx} \sin{\left(2 \phi \right)} \dot{\phi} \dot{\psi}}{2} - \frac{I_{xx} \sin{\left(2 \theta \right)} \dot{\psi} \dot{\theta}}{2} - \frac{I_{xx} \cos{\left(2 \phi - \theta \right)} \dot{\phi} \dot{\theta}}{2} + \frac{I_{xx} \cos{\left(2 \phi - \theta \right)} \dot{\theta}^{2}}{4} - \frac{I_{xx} \cos{\left(2 \phi + \theta \right)} \dot{\phi} \dot{\theta}}{2} - \frac{I_{xx} \cos{\left(2 \phi + \theta \right)} \dot{\theta}^{2}}{4} + I_{yy} \sin{\left(2 \theta \right)} \dot{\psi} \dot{\theta} + I_{yy} \cos{\left(\theta \right)} \dot{\phi} \dot{\theta} - \frac{I_{zz} \sin{\left(2 \left(\phi - \theta\right) \right)} \dot{\phi} \dot{\psi}}{4} + \frac{I_{zz} \sin{\left(2 \left(\phi - \theta\right) \right)} \dot{\psi} \dot{\theta}}{4} - \frac{I_{zz} \sin{\left(2 \left(\phi + \theta\right) \right)} \dot{\phi} \dot{\psi}}{4} - \frac{I_{zz} \sin{\left(2 \left(\phi + \theta\right) \right)} \dot{\psi} \dot{\theta}}{4} - \frac{I_{zz} \sin{\left(2 \phi \right)} \dot{\phi} \dot{\psi}}{2} - \frac{I_{zz} \sin{\left(2 \theta \right)} \dot{\psi} \dot{\theta}}{2} + \frac{I_{zz} \cos{\left(2 \phi - \theta \right)} \dot{\phi} \dot{\theta}}{2} - \frac{I_{zz} \cos{\left(2 \phi - \theta \right)} \dot{\theta}^{2}}{4} + \frac{I_{zz} \cos{\left(2 \phi + \theta \right)} \dot{\phi} \dot{\theta}}{2} + \frac{I_{zz} \cos{\left(2 \phi + \theta \right)} \dot{\theta}^{2}}{4}\\\frac{I_{xx} \sin{\left(2 \left(\phi - \theta\right) \right)} \dot{\psi}^{2}}{8} - \frac{I_{xx} \sin{\left(2 \left(\phi + \theta\right) \right)} \dot{\psi}^{2}}{8} - I_{xx} \sin{\left(2 \phi \right)} \dot{\phi} \dot{\theta} + \frac{I_{xx} \sin{\left(2 \theta \right)} \dot{\psi}^{2}}{4} - \frac{I_{xx} \cos{\left(2 \phi - \theta \right)} \dot{\phi} \dot{\psi}}{2} - \frac{I_{xx} \cos{\left(2 \phi + \theta \right)} \dot{\phi} \dot{\psi}}{2} - \frac{I_{yy} \sin{\left(2 \theta \right)} \dot{\psi}^{2}}{2} - I_{yy} \cos{\left(\theta \right)} \dot{\phi} \dot{\psi} - \frac{I_{zz} \sin{\left(2 \left(\phi - \theta\right) \right)} \dot{\psi}^{2}}{8} + \frac{I_{zz} \sin{\left(2 \left(\phi + \theta\right) \right)} \dot{\psi}^{2}}{8} + I_{zz} \sin{\left(2 \phi \right)} \dot{\phi} \dot{\theta} + \frac{I_{zz} \sin{\left(2 \theta \right)} \dot{\psi}^{2}}{4} + \frac{I_{zz} \cos{\left(2 \phi - \theta \right)} \dot{\phi} \dot{\psi}}{2} + \frac{I_{zz} \cos{\left(2 \phi + \theta \right)} \dot{\phi} \dot{\psi}}{2}\\I_{xx} \left(\sin{\left(\phi \right)} \dot{\theta} + \cos{\left(\phi \right)} \cos{\left(\theta \right)} \dot{\psi}\right) \left(- \sin{\left(\phi \right)} \cos{\left(\theta \right)} \dot{\psi} + \cos{\left(\phi \right)} \dot{\theta}\right) + I_{yy} \cos{\left(\theta \right)} \dot{\psi} \dot{\theta} - I_{zz} \left(\sin{\left(\phi \right)} \dot{\theta} + \cos{\left(\phi \right)} \cos{\left(\theta \right)} \dot{\psi}\right) \left(- \sin{\left(\phi \right)} \cos{\left(\theta \right)} \dot{\psi} + \cos{\left(\phi \right)} \dot{\theta}\right)\\0\\0\\0\end{matrix}\right]\end{split}\]

Conservative Forces

Recall from Energy and Power that conservative forces, can be expressed using the gradient of a scalar function of the generalized coordinates, known as the potential energy \(V(\bar{q})\), thus the conservative generalized active forces can be written as:

(226)\[\bar{F}_r^\textrm{c} = -\frac{\partial V}{\partial q_r}\]

Warning

Note the minus sign in the above equation.

Some examples of conservative forces are:

• a uniform gravitational field, for example on the surface of the earth, with potential \(V = m g h(\bar{q})\),

• gravity from Newton’s universal gravitation, with potential \(V = -G \frac{m_1m_2}{r(\bar{q})}\),

• a linear spring, with potential \(V = \frac{1}{2}k(l(\bar{q}) - l_0)^2\).

For conservative forces, it is often convenient to derive the applied forces via the potential energy.

The Lagrange Method

Both the equation for computing the inertial forces from the kinetic energy and the equation for computing the applied conservative forces from a potential energy have a term in them with the partial derivative with respect to the generalized coordinate. Furthermore, the potential energy does not depend on the generalized speeds. Therefore, the resulting (inertial and conservative applied) forces can be derived in one go, by combining the two equations.

Step 1. Compute the so called Lagrangian \(L\), the difference between the kinetic energy and potential energy:

(227)\[L = K - V\]

Step 2. Use the Euler-Lagrange equations (the name for the equation (225)) to find the equations of motion:

(228)\[\frac{\mathrm{d}}{\mathrm{d}t}\left(\frac{\partial L}{\partial \dot{q}_r} \right) - \frac{\partial L}{\partial q_r} = F_r^\textrm{nc} \textrm{ for } r = 1,\ldots,n\]

while being careful to include conservative applied forces in the potential energy \(V\) term, but not in the non-conservative generalized active force \(F_r^\textrm{nc}\).

Example: Unconstrained System

This example will use the Lagrange method to derive the equations of motion for the system introduced in Example of Kane’s Equations. The description of the system is shown again in Fig. 53.

Fig. 53 Three dimensional pendulum made up of two pinned rods and a sliding mass on rod \(B\). Each degree of freedom is resisted by a linear spring. When the generalized coordinates are all zero, the two rods are perpendicular to each other.

The first step is to define the relevant variables, constants and frames. This step is the same as for Kane’s method.

Frames and Bodies Setup

m, g, kt, kl, l = sm.symbols('m, g, k_t, k_l, l')
q1, q2, q3 = me.dynamicsymbols('q1, q2, q3')
t = me.dynamicsymbols._t

q = sm.Matrix([q1, q2, q3])
qd = q.diff(t)
qdd = qd.diff(t)

N = me.ReferenceFrame('N')
A = me.ReferenceFrame('A')
B = me.ReferenceFrame('B')

A.orient_axis(N, q1, N.z)
B.orient_axis(A, q2, A.x)

O = me.Point('O')
Ao = me.Point('A_O')
Bo = me.Point('B_O')
Q = me.Point('Q')

Ao.set_pos(O, l/2*A.x)
Bo.set_pos(O, l*A.x)
Q.set_pos(Bo, q3*B.y)

O.set_vel(N, 0)

I = m*l**2/12
I_A_Ao = I*me.outer(A.y, A.y) + I*me.outer(A.z, A.z)
I_B_Bo = I*me.outer(B.x, B.x) + I*me.outer(B.z, B.z)

Start by defining the kinetic energy for each rigid body and particle:

KA = m*Ao.vel(N).dot(Ao.vel(N))/2 + A.ang_vel_in(N).dot(I_A_Ao.dot(A.ang_vel_in(N)))/2
KA

\[\displaystyle \frac{l^{2} m \dot{q}_{1}^{2}}{6}\]

KB = m*Bo.vel(N).dot(Bo.vel(N))/2 + B.ang_vel_in(N).dot(I_B_Bo.dot(B.ang_vel_in(N)))/2
KB

\[\displaystyle \frac{l^{2} m \cos^{2}{\left(q_{2} \right)} \dot{q}_{1}^{2}}{24} + \frac{l^{2} m \dot{q}_{1}^{2}}{2} + \frac{l^{2} m \dot{q}_{2}^{2}}{24}\]

KQ = m/4*Q.vel(N).dot(Q.vel(N))/2
KQ

\[\displaystyle \frac{m \left(l^{2} \dot{q}_{1}^{2} + l \left(- q_{3} \sin{\left(q_{2} \right)} \dot{q}_{2} + \cos{\left(q_{2} \right)} \dot{q}_{3}\right) \dot{q}_{1} - l q_{3} \sin{\left(q_{2} \right)} \dot{q}_{1} \dot{q}_{2} + l \cos{\left(q_{2} \right)} \dot{q}_{1} \dot{q}_{3} + q_{3}^{2} \cos^{2}{\left(q_{2} \right)} \dot{q}_{1}^{2} + q_{3}^{2} \dot{q}_{2}^{2} + \dot{q}_{3}^{2}\right)}{8}\]

K = KA + KB + KQ

Form the potential energy from the conservative gravitational and spring forces:

V_grav = m*g*(Ao.pos_from(O) + Bo.pos_from(O)).dot(-N.x) + m/4*g*Q.pos_from(O).dot(-N.x)
V_grav

\[\displaystyle - \frac{3 g l m \cos{\left(q_{1} \right)}}{2} + \frac{g m \left(- l \cos{\left(q_{1} \right)} + q_{3} \sin{\left(q_{1} \right)} \cos{\left(q_{2} \right)}\right)}{4}\]

V_springs = kt/2*q1**2 + kt/2*q2**2 + kl/2*q3**2
V_springs

\[\displaystyle \frac{k_{l} q_{3}^{2}}{2} + \frac{k_{t} q_{1}^{2}}{2} + \frac{k_{t} q_{2}^{2}}{2}\]

V = V_grav + V_springs

The Lagrangian is then:

L = sm.Matrix([K - V])
sm.trigsimp(L)

\[\displaystyle \left[\begin{matrix}\frac{7 g l m \cos{\left(q_{1} \right)}}{4} - \frac{g m q_{3} \sin{\left(q_{1} \right)} \cos{\left(q_{2} \right)}}{4} - \frac{k_{l} q_{3}^{2}}{2} - \frac{k_{t} q_{1}^{2}}{2} - \frac{k_{t} q_{2}^{2}}{2} + \frac{l^{2} m \cos^{2}{\left(q_{2} \right)} \dot{q}_{1}^{2}}{24} + \frac{19 l^{2} m \dot{q}_{1}^{2}}{24} + \frac{l^{2} m \dot{q}_{2}^{2}}{24} - \frac{l m q_{3} \sin{\left(q_{2} \right)} \dot{q}_{1} \dot{q}_{2}}{4} + \frac{l m \cos{\left(q_{2} \right)} \dot{q}_{1} \dot{q}_{3}}{4} + \frac{m q_{3}^{2} \cos^{2}{\left(q_{2} \right)} \dot{q}_{1}^{2}}{8} + \frac{m q_{3}^{2} \dot{q}_{2}^{2}}{8} + \frac{m \dot{q}_{3}^{2}}{8}\end{matrix}\right]\]

Finally, derive the equations of motion:

fd = -(L.jacobian(qd).diff(t) - L.jacobian(q)).transpose()
qdd_zerod = {qddr: 0 for qddr in qdd}
Md = fd.jacobian(qdd)
gd = sm.trigsimp(fd.xreplace(qdd_zerod))
me.find_dynamicsymbols(Md), me.find_dynamicsymbols(gd)

\[\displaystyle \left( \left\{q_{2}, q_{3}\right\}, \ \left\{q_{1}, q_{2}, q_{3}, \dot{q}_{1}, \dot{q}_{2}, \dot{q}_{3}\right\}\right)\]

Md

\[\begin{split}\displaystyle \left[\begin{matrix}- \frac{l^{2} m \cos^{2}{\left(q_{2} \right)}}{12} - \frac{4 l^{2} m}{3} - \frac{m \left(2 l^{2} + 2 q_{3}^{2} \cos^{2}{\left(q_{2} \right)}\right)}{8} & \frac{l m q_{3} \sin{\left(q_{2} \right)}}{4} & - \frac{l m \cos{\left(q_{2} \right)}}{4}\\\frac{l m q_{3} \sin{\left(q_{2} \right)}}{4} & - \frac{l^{2} m}{12} - \frac{m q_{3}^{2}}{4} & 0\\- \frac{l m \cos{\left(q_{2} \right)}}{4} & 0 & - \frac{m}{4}\end{matrix}\right]\end{split}\]

gd

\[\begin{split}\displaystyle \left[\begin{matrix}- \frac{7 g l m \sin{\left(q_{1} \right)}}{4} - \frac{g m q_{3} \cos{\left(q_{1} - q_{2} \right)}}{8} - \frac{g m q_{3} \cos{\left(q_{1} + q_{2} \right)}}{8} - k_{t} q_{1} + \frac{l^{2} m \sin{\left(2 q_{2} \right)} \dot{q}_{1} \dot{q}_{2}}{12} + \frac{l m q_{3} \cos{\left(q_{2} \right)} \dot{q}_{2}^{2}}{4} + \frac{l m \sin{\left(q_{2} \right)} \dot{q}_{2} \dot{q}_{3}}{2} + \frac{m q_{3}^{2} \sin{\left(2 q_{2} \right)} \dot{q}_{1} \dot{q}_{2}}{4} - \frac{m q_{3} \cos{\left(2 q_{2} \right)} \dot{q}_{1} \dot{q}_{3}}{4} - \frac{m q_{3} \dot{q}_{1} \dot{q}_{3}}{4}\\\frac{g m q_{3} \cos{\left(q_{1} - q_{2} \right)}}{8} - \frac{g m q_{3} \cos{\left(q_{1} + q_{2} \right)}}{8} - k_{t} q_{2} - \frac{l^{2} m \sin{\left(2 q_{2} \right)} \dot{q}_{1}^{2}}{24} - \frac{m q_{3}^{2} \sin{\left(2 q_{2} \right)} \dot{q}_{1}^{2}}{8} - \frac{m q_{3} \dot{q}_{2} \dot{q}_{3}}{2}\\- \frac{g m \sin{\left(q_{1} \right)} \cos{\left(q_{2} \right)}}{4} - k_{l} q_{3} + \frac{m q_{3} \cos^{2}{\left(q_{2} \right)} \dot{q}_{1}^{2}}{4} + \frac{m q_{3} \dot{q}_{2}^{2}}{4}\end{matrix}\right]\end{split}\]

The mass matrix \(\mathbf{M}_d\) only depends on \(\bar{q}\), and \(\bar{g}_d\) depends on \(\dot{\bar{q}}\) and \(\bar{q}\), just as in Kane’s method. Note that \(\bar{g}_d\) now combines the effects of the velocity related force vector and the conservative forces. In this setting, \(\bar{g}_d\) is often called the dynamic bias.

Generalized Momenta

It is often useful to use a vector of intermediate variables when finding the Euler-Lagrange equations. The variables are defined as:

(229)\[p_r = \frac{\partial L}{\partial \dot{q_r}}\]

The variables are collected in a vector \(\bar{p}\). They are called the generalized momenta, as they coincide with linear momentum in the case of a Lagrangian describing a particle. Similar to the situation in the dynamics of particles, there can be conservation of generalized momentum. This is the case for the generalized momentum associated with ignorable coordinates, as defined in Equations of Motion with Nonholonomic Constraints.

For the example pendulum, the generalized momenta are calculated as:

p = L.jacobian(qd).transpose()
sm.trigsimp(p)

\[\begin{split}\displaystyle \left[\begin{matrix}\frac{m \left(l^{2} \cos^{2}{\left(q_{2} \right)} \dot{q}_{1} + 19 l^{2} \dot{q}_{1} - 3 l q_{3} \sin{\left(q_{2} \right)} \dot{q}_{2} + 3 l \cos{\left(q_{2} \right)} \dot{q}_{3} + 3 q_{3}^{2} \cos^{2}{\left(q_{2} \right)} \dot{q}_{1}\right)}{12}\\\frac{l^{2} m \dot{q}_{2}}{12} + \frac{m \left(- l \sin{\left(q_{2} \right)} \dot{q}_{1} + q_{3} \dot{q}_{2}\right) q_{3}}{4}\\\frac{m \left(l \cos{\left(q_{2} \right)} \dot{q}_{1} + \dot{q}_{3}\right)}{4}\end{matrix}\right]\end{split}\]

Constrained Equations of Motion

When using Kane’s method, constraints are handled by dividing the generalized speeds into two sets: the dependent and independent generalized speeds. Depending on the type of constraints, the dependent generalized speeds are eliminated by solving the constraint equation (for nonholonomic constraints) or the time derivative of the constraint equation (holonomic constraints). Kane’s method only gives rise to \(p = n - m\) dynamical equations, one for each independent generalized speed. The Lagrange method gives rise to \(N\) dynamical equations, one for each coordinate plus M + m additional equations to manage the constraints.

The constraints are handled using a generalized version of the approach in Exposing Noncontributing Forces. First the constraints are omitted, then a constraint force is added, with a known direction, but unknown magnitude. Finally, the (second) time derivative of the constraint equation is then appended to the equations found with the Euler-Lagrange equation.

For example, consider a particle of mass \(m\) with position \(\bar{r}^{P/O} = q_1 \hat{n}_x + q_2 \hat{n}_y + q_3\hat{n}_z\) on a slope \(q_1 = q_2\) with gravity pulling the mass down the slope. The unconstrained Lagrangian is \(L = \frac{1}{2}m(\dot{q}_1^2 + \dot{q}_2^2 + \dot{q}_3^2) - mgq_3\). The constraint force is perpendicular to the slope, so is described as \(\bar{F} = F\hat{n}_x - F\hat{n}_y\). The appended equation is the second time derivative of the constraint equation \(\ddot{q_1} - \ddot{q_2} = 0\). Combining all, gives:

(230)\[\begin{split}\begin{array}{r} m\ddot{q}_1= \phantom{-}F\\ m\ddot{q}_2= -F\\ m\ddot{q}_3 + mg = \phantom{-}0\\ \ddot{q}_1 - \ddot{q}_2\!\! = \phantom{-}0 \end{array}\end{split}\]

This can be put in matrix-form, by extracting the unknown acceleration and force magnitude:

(231)\[\begin{split}\begin{bmatrix} m & 0 & 0 &-1 \\ 0 & m & 0 & 1 \\ 0 & 0 & m & 0 \\ 1 & -1 & 0 & 0 \end{bmatrix} \begin{bmatrix} \ddot{q}_1 \\ \ddot{q}_2 \\ \ddot{q}_3 \\ F \end{bmatrix} = \begin{bmatrix} 0 \\ 0 \\ -mg \\ 0 \end{bmatrix}\end{split}\]

It can be challenging to find the direction of the constraint force from the geometry of the system directly. There is a trick, called the method of the Lagrange multipliers, to quickly find the correct generalized forces associated with the constraint forces.

Given a motion constraint (time derivatives of configuration constraint or a nonholonomic constraint) in the general form

(232)\[\sum_r a_r(\bar{q}) \dot{q}_r = 0\]

The generalized constraint force is found as:

(233)\[F_r = \lambda a_r(\bar{q})\]

Here \(\lambda\) is a variable encoding the magnitude of the constraint force. It is called the Lagrange multiplier. The same \(\lambda\) is used for each \(r\), that is, each constraint has a single associated Lagrange multiplier.

Due to how it is constructed, the power produced by the constraint force is always zero, as expected.

(234)\[P = \sum_r F_r\dot{q}_r = \sum \lambda a_r(\bar{q})\dot{q}_r = \lambda \sum a_r(\bar{q})\dot{q}_r = \lambda \cdot 0\]

For example, consider the point mass to be constrained to move in a bowl \(q_1^2 + q_2^2 + q_3^2 -1 = 0\), Fig. 54. Taking the time derivative gives: \(a_1 = 2q_1\), \(a_2 = 2q_2\), and \(a_3 = 2q_3\). This results in generalized reaction forces \(F_1 = 2\lambda q_1\), \(F_2 = 2\lambda q_2\) and \(F_3 = 2\lambda q_3\).

Fig. 54 Point mass \(P\) constrained to the surface of a spherical bowl with radius \(1\) and constraint force measure numbers \(F_1,F_2,F_3\).

Often, there are multiple constraints on the same system. For convenience, the handling of these constraints can be combined. Consider the \(m+M\) dimensional general constraint equations consisting of the time derivatives of the holonomic constraints and/or the nonholonomic constraints:

(235)\[\bar{f}_{hn}(\dot{\bar{q}}, \bar{q}) = \mathbf{M}_{hn}\dot{\bar{q}} + \bar{g}_{hn} = 0 \in \mathbb{R}^{M+m}\]

the combined constraint forces are given as:

(236)\[\bar{F}_r = \mathbf{M}_{hn}^\text{T}\bar{\lambda},\]

where \(\bar{\lambda}\) is a vector of \(m + M\) Lagrange multipliers, one for each constraint (row in \(\mathbf{M}_{hn}\)).

Example: Turn the Torque Free Rigid Body into a Rolling Sphere

The non-slip condition of the rolling sphere is split into three constraints: the velocity of the contact point (\(G\)) is zero in the \(\hat{n}_x\), the \(\hat{n}_y\), and the \(\hat{n}_z\) directions. The first two constraints are nonholonomic, the last constraint is the time derivative of a holonomic constraint. All three constraints are enforced by contact forces in their respective directions.

The contact point can be found according by \(\bar{r}^{G/B_o} = -r \hat{n}_z\). By using the Velocity Two Point Theorem, the following constraints are found.

(237)\[\begin{split}\begin{array}{l} \bar{n}_x\cdot ({}^N\bar{v}^{B_o} + {}^N\bar{\omega}^B \times (-r\hat{n}_z)) = 0 \\ \bar{n}_y\cdot ({}^N\bar{v}^{B_o} + {}^N\bar{\omega}^B \times (-r\hat{n}_z)) = 0 \\ \bar{n}_z\cdot ({}^N\bar{v}^{B_o} + {}^N\bar{\omega}^B \times (-r\hat{n}_z)) = 0 \\ \end{array}\end{split}\]

These can be used to derive the constraint force and the additional equations using the Lagrange-multiplier method as shown below. Note that here only the first time derivative of the constraint equation is used, again because the second time derivatives of the generalized coordinates appear.

Frames and Body Setup

Setting up reference frames

psi,theta, phi, x, y, z = me.dynamicsymbols('psi theta phi x y z')
N = me.ReferenceFrame('N')
B = me.ReferenceFrame('B')
B.orient_body_fixed(N, (psi, theta, phi), 'zxy')

# Mass and inertia
m, Ixx, Iyy, Izz = sm.symbols('M, I_{xx}, I_{yy}, I_{zz}')
I_B = me.inertia(B, Ixx, Iyy, Izz)

Finding the kinetic energy:

omega_B = B.ang_vel_in(N)
r_com = x*N.x + y*N.y + z*N.z
v_com = r_com.dt(N)
K = omega_B.dot(I_B.dot(omega_B))/2 + m*v_com.dot(v_com)/2

Deriving equations of motion:

t = me.dynamicsymbols._t
q = sm.Matrix([psi, theta, phi, x, y, z])
qd = q.diff(t)
qdd = qd.diff(t)

L = sm.Matrix([K])
fd = L.jacobian(qd).diff(t) - L.jacobian(q)

qdd_zerod = {qddr: 0 for qddr in qdd}
Md = fd.jacobian(qdd)
gd = fd.xreplace(qdd_zerod)

To make this free floating body a rolling wheel, three constraints are needed: the velocity of the contact point should be zero in \(\hat{n}_x\), \(\hat{n}_y\) and \(\hat{n}_x\) direction.

r = sm.symbols('r')
lambda1, lambda2, lambda3 = me.dynamicsymbols('lambda1, lambda2, lambda3')

constraint = (v_com + B.ang_vel_in(N).cross(-r*N.z)).to_matrix(N)
sm.trigsimp(constraint)

\[\begin{split}\displaystyle \left[\begin{matrix}- r \left(\sin{\left(\psi \right)} \dot{\theta} + \cos{\left(\psi \right)} \cos{\left(\theta \right)} \dot{\phi}\right) + \dot{x}\\r \left(- \sin{\left(\psi \right)} \cos{\left(\theta \right)} \dot{\phi} + \cos{\left(\psi \right)} \dot{\theta}\right) + \dot{y}\\\dot{z}\end{matrix}\right]\end{split}\]

The Jacobian of the constraints with respect to \(\dot{\bar{q}}\) is then:

Mhn = constraint.jacobian(qd)
sm.trigsimp(Mhn)

\[\begin{split}\displaystyle \left[\begin{matrix}0 & - r \sin{\left(\psi \right)} & - r \cos{\left(\psi \right)} \cos{\left(\theta \right)} & 1 & 0 & 0\\0 & r \cos{\left(\psi \right)} & - r \sin{\left(\psi \right)} \cos{\left(\theta \right)} & 0 & 1 & 0\\0 & 0 & 0 & 0 & 0 & 1\end{matrix}\right]\end{split}\]

This constraint information must then be added to the original equations. To do so, we make use of a useful fact:

diff_constraint = constraint.diff(t)
diff_constraint.jacobian(qdd) - Mhn

\[\begin{split}\displaystyle \left[\begin{matrix}0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0\end{matrix}\right]\end{split}\]

This equality is true for all constraints, as can easily be shown by taking the time derivative of the constraint equation, using the product rule.

The combined equations can now be written in a block matrix form:

(238)\[\begin{split}\begin{bmatrix} \mathbf{M}_d & \mathbf{M}_{hn}^T \\ \mathbf{ M}_{hn} & 0 \end{bmatrix} \begin{bmatrix} \ddot{\bar{q}} \\ \bar{\lambda} \end{bmatrix} + \begin{bmatrix} \bar{g}_d \\ \bar{g}_{hnd} \end{bmatrix} = \begin{bmatrix} 0 \\ 0 \end{bmatrix}\end{split}\]

where \(\bar{g}_d\) is the dynamic bias, and the last term on the right hand side, called the constraint bias, can be quickly computed as:

ghnd = diff_constraint.xreplace({qddr : 0 for qddr in qdd})
sm.trigsimp(ghnd)

\[\begin{split}\displaystyle \left[\begin{matrix}- r \left(- \sin{\left(\psi \right)} \cos{\left(\theta \right)} \dot{\phi} \dot{\psi} - \sin{\left(\theta \right)} \cos{\left(\psi \right)} \dot{\phi} \dot{\theta} + \cos{\left(\psi \right)} \dot{\psi} \dot{\theta}\right)\\r \left(\sin{\left(\psi \right)} \sin{\left(\theta \right)} \dot{\phi} \dot{\theta} - \sin{\left(\psi \right)} \dot{\psi} \dot{\theta} - \cos{\left(\psi \right)} \cos{\left(\theta \right)} \dot{\phi} \dot{\psi}\right)\\0\end{matrix}\right]\end{split}\]

We call the block matrix called the extended mass matrix and the vector on the right hand side the extended dynamic bias.

With these N + m + M equations, it is possible to solve for \(\ddot{\bar{q}}\) and \(\lambda\). It is therefore possible to integrate/simulate the system directly. However, because only the second derivative of the constraint is satisfied, numerical errors can build up due to not satisfying the actual constraint the constraint is not satisfied. It is better to use a differential algebraic solver, as discussed in Equations of Motion with Holonomic Constraints. See the scikits.ode documentation for a worked example.

The method of the Lagrange multipliers can of course also be used within Kane’s method. However, it increases the number of equations, which is why the elimination approach is often preferred there. An exception being scenarios where the constraint force itself is a useful output, for instance to check no-slip conditions in case of limited friction.

Lagrange’s vs Kane’s

The is book has now presented two alternatives to the Newton-Euler method: Kane’s method and Lagrange’s method. This raises the questions: when should each alternative method be used?

For constrained systems, Kane’s method has the advantage that the equations of motion are given for a set of independent generalized speeds only. In other words, Kane’s method gives a minimal set of equations. This can give rise to simplified equations, additional insight, and numerically more efficient simulation. This also gives the benefit that Lagrange multipliers are not needed when solving constrained systems with Kane’s method.

Furthermore, the connection from Kane’s method to vector mechanics, that is, Newton’s Laws, is clearer, which can provide additional insight, and make it easier to incorporate non-conservative forces such as friction.

On the other hand, the Lagrange method only requires energies as input, for which only the velocities of the bodies are needed. Therefore, it can be simpler to derive than the accelerations which are needed for Kane’s method.

Furthermore, the Lagrange method results in a set of equations with well understood structures and properties. These structures and properties are not studied further in these materials. A starting point for further study is Noether’s theorem, which extends the idea of ignorable coordinates to find conserved quantities like momentum and energy.