Consider an \( n \)-DOF serial manipulator with generalized joint coordinates \( \mathbf{q} = [q_1,\dots,q_n]^\top \in \mathbb{R}^n \). Each joint coordinate is either a revolute angle or a prismatic displacement (no exotic joints yet). Joint velocities and accelerations are \( \dot{\mathbf{q}}, \ddot{\mathbf{q}} \in \mathbb{R}^n \), and the corresponding joint torques/forces are collected in \( \boldsymbol{\tau} \in \mathbb{R}^n \).

In Chapter 10, you already saw that the dynamics of such a manipulator can be written in the joint-space canonical form

\[ \mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \boldsymbol{\tau}, \]

where:

• \( \mathbf{M}(\mathbf{q}) \) is the inertia (mass) matrix, symmetric and positive definite.
• \( \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} \) collects centrifugal and Coriolis terms (but we defer Christoffel-symbol details to Lesson 2).
• \( \mathbf{g}(\mathbf{q}) \) is the gravitational torque vector.

The goal of this lesson is to derive this structure using the Lagrange–Euler formalism, starting from link masses, centers of mass, and inertia tensors. The derivation proceeds mechanically and is well suited for symbolic and automatic-code-generation tools.

flowchart TD
  A["Specify robot: DH/PoE parameters, link inertias"] --> B["Define generalized coords q"]
  B --> C["Compute link velocities via Jacobians"]
  C --> D["Form kinetic energy T(q, qdot)"]
  B --> E["Compute potential energy V(q)"]
  D --> F["L = T - V"]
  E --> F
  F --> G["Apply Euler-Lagrange: d/dt(dL/dqdot) - dL/dq = tau"]
  G --> H["Rearrange to matrix form: M(q) qddot + C(q,qdot) qdot + g(q) = tau"]
        

Let link \( i \) have mass \( m_i \), center-of-mass (CoM) position \( \mathbf{p}_i(\mathbf{q}) \in \mathbb{R}^3 \) expressed in the base frame, and spatial inertia tensor \( \mathbf{I}_i \in \mathbb{R}^{3\times 3} \) expressed in a frame attached to the link. Using standard rigid-body kinematics (from earlier chapters), the linear and angular velocities of link \( i \) can be written as

\[ \mathbf{v}_i(\mathbf{q},\dot{\mathbf{q}}) = \mathbf{J}_{v,i}(\mathbf{q})\dot{\mathbf{q}}, \quad \boldsymbol{\omega}_i(\mathbf{q},\dot{\mathbf{q}}) = \mathbf{J}_{\omega,i}(\mathbf{q})\dot{\mathbf{q}}, \]

where \( \mathbf{J}_{v,i}, \mathbf{J}_{\omega,i} \) are the linear and angular velocity Jacobians of link \( i \). The kinetic energy of link \( i \) is

\[ T_i = \tfrac12 m_i \mathbf{v}_i^\top \mathbf{v}_i + \tfrac12 \boldsymbol{\omega}_i^\top \mathbf{I}_i \boldsymbol{\omega}_i. \]

Substituting the Jacobian expressions:

\[ \begin{aligned} T_i &= \tfrac12 m_i \dot{\mathbf{q}}^\top \mathbf{J}_{v,i}^\top \mathbf{J}_{v,i}\dot{\mathbf{q}} + \tfrac12 \dot{\mathbf{q}}^\top \mathbf{J}_{\omega,i}^\top \mathbf{I}_i \mathbf{J}_{\omega,i}\dot{\mathbf{q}} \\ &= \tfrac12 \dot{\mathbf{q}}^\top \Big( m_i \mathbf{J}_{v,i}^\top \mathbf{J}_{v,i} + \mathbf{J}_{\omega,i}^\top \mathbf{I}_i \mathbf{J}_{\omega,i} \Big) \dot{\mathbf{q}}. \end{aligned} \]

Summing over all links,

\[ \begin{aligned} T(\mathbf{q},\dot{\mathbf{q}}) &= \sum_{i=1}^n T_i = \tfrac12 \dot{\mathbf{q}}^\top \left[ \sum_{i=1}^n \left( m_i \mathbf{J}_{v,i}^\top \mathbf{J}_{v,i} + \mathbf{J}_{\omega,i}^\top \mathbf{I}_i \mathbf{J}_{\omega,i} \right) \right] \dot{\mathbf{q}} \\ &= \tfrac12 \dot{\mathbf{q}}^\top \mathbf{M}(\mathbf{q}) \dot{\mathbf{q}}, \end{aligned} \]

which defines the inertia matrix \( \mathbf{M}(\mathbf{q}) \).

Potential energy (for uniform gravity) is

\[ V(\mathbf{q}) = \sum_{i=1}^n m_i \mathbf{g}^\top \mathbf{p}_i(\mathbf{q}), \]

where the constant gravity vector \( \mathbf{g} \in \mathbb{R}^3 \) is typically \( \mathbf{g} = [0, 0, -g]^\top \) with \( g \approx 9.81 \) m/s\(^2\).

The Lagrangian is then defined as

\[ L(\mathbf{q},\dot{\mathbf{q}}) = T(\mathbf{q},\dot{\mathbf{q}}) - V(\mathbf{q}). \]

Everything needed for the dynamics is encoded in \( T \) and \( V \), i.e., in the link Jacobians, inertial parameters, and CoM positions.

For each generalized coordinate \( q_j \), the Lagrange equation is

\[ \frac{d}{dt}\left( \frac{\partial L}{\partial \dot{q}_j} \right) - \frac{\partial L}{\partial q_j} = \tau_j, \quad j = 1,\dots,n. \]

Using \( L = T - V \), we have \( \partial L/\partial \dot{q}_j = \partial T/\partial \dot{q}_j \) and \( \partial L/\partial q_j = \partial T/\partial q_j - \partial V/\partial q_j \). From the quadratic form of the kinetic energy \( T = \tfrac12 \dot{\mathbf{q}}^\top \mathbf{M}(\mathbf{q}) \dot{\mathbf{q}} \),

\[ \frac{\partial T}{\partial \dot{\mathbf{q}}} = \mathbf{M}(\mathbf{q})\dot{\mathbf{q}}, \]

so that

\[ \frac{d}{dt}\left( \frac{\partial T}{\partial \dot{\mathbf{q}}} \right) = \frac{d}{dt}\Big( \mathbf{M}(\mathbf{q})\dot{\mathbf{q}} \Big) = \mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \dot{\mathbf{M}}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}}. \]

On the other hand, the gradient of the potential gives the gravity term:

\[ \mathbf{g}(\mathbf{q}) = \frac{\partial V(\mathbf{q})}{\partial \mathbf{q}}. \]

Collecting terms for all joints in vector form, the Lagrange equations can be written as

\[ \mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{h}(\mathbf{q},\dot{\mathbf{q}}) + \mathbf{g}(\mathbf{q}) = \boldsymbol{\tau}, \]

where \( \mathbf{h}(\mathbf{q},\dot{\mathbf{q}}) \) collects velocity-dependent terms originating from \( \dot{\mathbf{M}}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} \) and \( \partial T/\partial \mathbf{q} \). By definition, we can introduce a matrix \( \mathbf{C}(\mathbf{q},\dot{\mathbf{q}}) \) such that

\[ \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} = \mathbf{h}(\mathbf{q},\dot{\mathbf{q}}), \]

and then the canonical manipulator equation is exactly

\[ \mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \boldsymbol{\tau}. \]

In Lesson 2, we will express the entries of \( \mathbf{C}(\mathbf{q},\dot{\mathbf{q}}) \) using partial derivatives of \( \mathbf{M}(\mathbf{q}) \) (Christoffel symbols). For now, it is enough to know that \( \mathbf{C} \) is constructed from the same building blocks (Jacobian matrices and inertial parameters) used in \( \mathbf{M} \).

As a warm-up, consider a planar pendulum of length \( \ell \) and mass \( m \), with generalized coordinate \( q \) measured from the downward vertical. The pivot is actuated by torque \( \tau \). Assume the mass is concentrated at the end of a massless rod (point mass model).

The CoM position (in the plane) is \( \mathbf{p}(q) = [\ell\sin q,\; -\ell\cos q]^\top \), so the velocity is

\[ \dot{\mathbf{p}}(q,\dot{q}) = \begin{bmatrix} \ell\cos q \\ \ell\sin q \end{bmatrix}\dot{q}, \quad \|\dot{\mathbf{p}}\|^2 = \ell^2 \dot{q}^2. \]

Thus the kinetic and potential energies are

\[ T = \tfrac12 m \ell^2 \dot{q}^2, \quad V = m g \ell (1 - \cos q). \]

The Lagrangian is

\[ L(q,\dot{q}) = \tfrac12 m \ell^2 \dot{q}^2 - m g \ell (1 - \cos q). \]

Now apply the Lagrange equation:

\[ \frac{d}{dt}\left( \frac{\partial L}{\partial \dot{q}} \right) - \frac{\partial L}{\partial q} = \tau. \]

First,

\[ \frac{\partial L}{\partial \dot{q}} = m\ell^2 \dot{q} \quad\Rightarrow\quad \frac{d}{dt}\left( \frac{\partial L}{\partial \dot{q}} \right) = m\ell^2 \ddot{q}. \]

Next,

\[ \frac{\partial L}{\partial q} = -\frac{\partial V}{\partial q} = -m g \ell \sin q. \]

Therefore,

\[ m\ell^2 \ddot{q} + m g \ell \sin q = \tau. \]

In the canonical form, this corresponds to \( \mathbf{M}(q) = [m\ell^2] \), \( \mathbf{C}(q,\dot{q}) = [0] \) (no velocity-dependent coupling in this 1-DOF system), and \( \mathbf{g}(q) = [m g \ell \sin q] \).

Python, with sympy and robotics-oriented toolboxes, is well suited for deriving EoM symbolically and generating code. Below we derive the 1-DOF pendulum EoM symbolically and then show how one would plug into a robotics library for general manipulators.

import sympy as sp

# Time variable
t = sp.symbols('t')

# Generalized coordinate and its derivatives
q = sp.Function('q')(t)
qdot  = sp.diff(q, t)
qddot = sp.diff(qdot, t)

# Parameters
m, l, g = sp.symbols('m l g', positive=True)

# Kinetic and potential energy for pendulum
T = sp.Rational(1, 2) * m * l**2 * qdot**2
V = m * g * l * (1 - sp.cos(q))

L = T - V

# Euler-Lagrange equation: d/dt(dL/dqdot) - dL/dq = tau
tau = sp.Function('tau')(t)

dL_dqdot = sp.diff(L, qdot)
d_dt_dL_dqdot = sp.diff(dL_dqdot, t)
dL_dq    = sp.diff(L, q)

EL_eq = sp.simplify(d_dt_dL_dqdot - dL_dq - tau)
print("Euler-Lagrange equation (should be 0):")
print(EL_eq)

# Solve for qddot symbolically
qddot_sol = sp.solve(sp.simplify(EL_eq), qddot)[0]
print("qddot =")
print(sp.simplify(qddot_sol))

# --------------------------------------------------
# Sketch: using roboticstoolbox-python for a general manipulator
# (after previous chapters, students have seen PoE or DH parametrizations)
# --------------------------------------------------
from roboticstoolbox import DHRobot, RevoluteDH

# Simple 1-link pendulum as a DHRobot (for illustration)
link = RevoluteDH(a=0, alpha=0, d=0)
pendulum = DHRobot([link], name="Pendulum")

import numpy as np

def eom_pendulum(q_val, qdot_val, m_val, l_val, g_val):
    # For a generic manipulator, you would normally build the full model with link masses/inertias
    # Using the toolbox, inverse dynamics is:
    M = m_val * l_val**2
    C = 0.0
    G = m_val * g_val * l_val * np.sin(q_val)
    qddot_val = (tau_func(q_val, qdot_val) - C * qdot_val - G) / M
    return qddot_val

# tau_func is a user-defined control torque function
      

For a multi-DOF manipulator, DHRobot.inverse_dynamics(q, qd, qdd) returns \( \boldsymbol{\tau} \) numerically, but internally it reflects the same Lagrange–Euler structure we have derived.

In C++, dynamic models are typically implemented numerically using a linear algebra library such as Eigen. For serial manipulators, more specialized robotics libraries (e.g., RBDL, KDL, Pinocchio) implement efficient recursive algorithms, but the underlying equations are still Lagrange–Euler or Newton–Euler.

Below is a minimal C++ function that returns the right-hand side of the pendulum dynamics \( \ddot{q} = f(q,\dot{q},\tau) \), using Eigen for vector types (this easily generalizes to multi-DOF).

#include <Eigen/Dense>

struct PendulumParams {
    double m;
    double l;
    double g;
};

// Compute qddot for a simple pendulum using Lagrange-Euler EoM
double pendulum_accel(double q, double qdot, double tau,
                      const PendulumParams& p)
{
    double M = p.m * p.l * p.l;          // inertia term
    double G = p.m * p.g * p.l * std::sin(q); // gravity torque
    double Cqdot = 0.0;                  // no Coriolis term for 1-DOF

    double qddot = (tau - Cqdot - G) / M;
    return qddot;
}

// Sketch of a multi-DOF signature:
// Eigen::VectorXd robot_accel(const Eigen::VectorXd& q,
//                             const Eigen::VectorXd& qdot,
//                             const Eigen::VectorXd& tau)
// {
//     // Compute M(q), C(q,qdot), g(q) from link inertias and Jacobians
//     // M, C, g can be assembled using a robotics library like RBDL:
//     //
//     //   RigidBodyDynamics::Model model;
//     //   // ... build model (masses, COMs, inertias, joint types)
//     //   Eigen::VectorXd qddot = Eigen::VectorXd::Zero(q.size());
//     //   RigidBodyDynamics::InverseDynamics(model, q, qdot, qddot, tau);
//     //
//     // For Lagrange-Euler, you would explicitly build M, C, g
//     // and compute qddot = M^{-1} (tau - C qdot - g).
// }
      

Robotics-focused C++ libraries such as RBDL or Pinocchio internally implement the same dynamics model, but with optimized algorithms and data structures. At the modeling level, the Lagrange–Euler derivation remains the foundation.

In Java, a common linear algebra library used in robotics is EJML (Efficient Java Matrix Library). For illustration we again implement the pendulum EoM. Multi-DOF extensions follow the same pattern but use vectors and matrices.

public class PendulumDynamics {

    public static class Params {
        public double m;
        public double l;
        public double g;

        public Params(double m, double l, double g) {
            this.m = m;
            this.l = l;
            this.g = g;
        }
    }

    // qddot = (tau - m g l sin(q)) / (m l^2)
    public static double accel(double q, double qdot, double tau, Params p) {
        double M = p.m * p.l * p.l;
        double G = p.m * p.g * p.l * Math.sin(q);
        double Cqdot = 0.0;
        return (tau - Cqdot - G) / M;
    }

    public static void main(String[] args) {
        Params p = new Params(1.0, 1.0, 9.81);
        double q = 0.5;
        double qdot = 0.0;
        double tau = 0.0;
        double qddot = accel(q, qdot, tau, p);
        System.out.println("qddot = " + qddot);
    }
}
      

For an \( n \)-DOF manipulator, you would represent \( \mathbf{M}(\mathbf{q}) \), \( \mathbf{C}(\mathbf{q},\dot{\mathbf{q}}) \), \( \mathbf{g}(\mathbf{q}) \), and \( \boldsymbol{\tau} \) as EJML matrices and vectors, and then compute

\[ \ddot{\mathbf{q}} = \mathbf{M}(\mathbf{q})^{-1} \Big( \boldsymbol{\tau} - \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} - \mathbf{g}(\mathbf{q}) \Big). \]

MATLAB with the Robotics System Toolbox provides a high-level interface for defining rigid body trees and computing inverse dynamics. For the pendulum example, we can implement the dynamics function directly or use the toolbox abstractions.

function qddot = pendulumEoM(t, x, tau, m, l, g)
% x = [q; qdot]
q    = x(1);
qdot = x(2);

M = m * l^2;
G = m * g * l * sin(q);
Cqdot = 0;   % no Coriolis term for 1-DOF

qddot = (tau - Cqdot - G) / M;
qddot = [qdot; qddot];
end

% Example numerical simulation with ODE45
m = 1.0; l = 1.0; g = 9.81;
tau_const = 0;

f = @(t, x) pendulumEoM(t, x, tau_const, m, l, g);
[t_sol, x_sol] = ode45(f, [0 10], [0.5; 0.0]);

plot(t_sol, x_sol(:,1)); xlabel('t'); ylabel('q(t)');
title('Pendulum angle using Lagrange-Euler EoM');
      

Using Robotics System Toolbox, a similar joint-space model can be implemented for a multi-DOF manipulator:

robot = rigidBodyTree('DataFormat','column','MaxNumBodies',1);
body = rigidBody('link1');
jnt  = rigidBodyJoint('joint1','revolute');
setFixedTransform(jnt, trvec2tform([0 0 0]));
jnt.JointAxis = [0 0 1];
body.Joint = jnt;
addBody(robot, body, robot.BaseName);

% Assign mass properties
body.Mass = m;
body.CenterOfMass = [0 0 -l]; % simple example
body.Inertia = [0 0 0 0 0 0];

q    = 0.5;
qdot = 0.0;
qdd  = 0.0;
tau  = inverseDynamics(robot, q, qdot, qdd); % tau from M, C, g
      

In Simulink, one typically uses a custom S-Function or MATLAB Function block to evaluate the right-hand side \( \ddot{\mathbf{q}} \) from \( \mathbf{M}, \mathbf{C}, \mathbf{g} \), then integrates to obtain \( \mathbf{q}(t) \) and \( \dot{\mathbf{q}}(t) \).

Wolfram Mathematica is very convenient for performing symbolic manipulations of Lagrangians, and for simplifying the resulting EoM. Below is a script for the 1-DOF pendulum.

(* Time, generalized coordinate, and parameters *)
t   = Symbol["t"];
q[t_] := q[t];
m = Symbol["m"];
l = Symbol["l"];
g = Symbol["g"];

qdot[t_] := D[q[t], t];

(* Energies *)
T = 1/2 m l^2 qdot[t]^2;
V = m g l (1 - Cos[q[t]]);

L = T - V;

(* Euler-Lagrange equation *)
EL = D[D[L, qdot[t]], t] - D[L, q[t]];

(* Introduce external torque tau[t] *)
tau[t_] := Symbol["tau"][t];
eq = Simplify[EL - tau[t] == 0];

(* Solve for qddot *)
qddot = D[q[t], {t, 2}];
sol = Solve[eq, qddot][[1, 1, 2]] // Simplify
      

The resulting expression matches the expected \( \ddot{q} = (\tau - m g \ell \sin q)/(m\ell^2) \), providing an independent symbolic check of the Lagrange–Euler derivation.

For real robot software stacks, the Lagrange–Euler derivation is usually performed once (symbolically or semi-symbolically), then translated into efficient numerical code. The following diagram summarizes a typical workflow spanning modeling, symbolic tools, and code generation.

flowchart TD
  MDEF["Model definition: geometry, inertial params"] --> JAC["Compute Jacobians J_v,i, J_w,i"]
  JAC --> ENERGY["Form T(q,qdot), V(q)"]
  ENERGY --> LAG["L(q,qdot) = T - V"]
  LAG --> EL["Apply Euler-Lagrange in CAS (Python, Mathematica)"]
  EL --> CANON["Obtain M(q), C(q,qdot), g(q)"]
  CANON --> GEN["Generate optimized code: C++, Java, MATLAB"]
  GEN --> SIM["Simulate and validate vs. physics intuition"]
        

In subsequent lessons we will refine \( \mathbf{C}(\mathbf{q},\dot{\mathbf{q}}) \) (via Christoffel symbols), analyze energy properties, and work out explicit models for 2-DOF and 3-DOF arms.

In this lesson we derived the Lagrange–Euler equations of motion for serial robot manipulators. Starting from link Jacobians and inertial parameters, we constructed the kinetic energy \( T = \tfrac12 \dot{\mathbf{q}}^\top \mathbf{M}(\mathbf{q})\dot{\mathbf{q}} \) and the potential energy \( V(\mathbf{q}) \), defined the Lagrangian \( L = T - V \), and applied the Euler–Lagrange equations to obtain

\[ \mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \boldsymbol{\tau}. \]

A simple 1-DOF pendulum served as a concrete example, and we illustrated how the same equations can be implemented in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica. Subsequent lessons will refine the structure of \( \mathbf{C}(\mathbf{q},\dot{\mathbf{q}}) \) via Christoffel symbols, analyze passivity and energy properties, and work through multi-DOF manipulator examples in detail.