From Lesson 1 we know that an n-DOF rigid manipulator admits equations of motion of the 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 symmetric positive definite inertia matrix, \( \mathbf{C}(\mathbf{q},\dot{\mathbf{q}}) \) collects Coriolis and centrifugal terms, \( \mathbf{g}(\mathbf{q}) \) is the gravity torque vector, and \( \boldsymbol{\tau} \) contains joint input torques.

In this lesson we specialize this general structure to:

• A 2-DOF planar R–R arm (two revolute joints in a plane).
• A 3-DOF planar R–R–R arm.

This illustrates how geometry, mass distribution, and joint coordinates appear explicitly in \( \mathbf{M},\mathbf{C},\mathbf{g} \), and prepares us for symbolic and numeric derivation workflows in Lesson 5.

flowchart TD
  A["Geometric model (links, joints, COMs)"] --> B["Kinematics: positions, velocities"]
  B --> C["Energies: T(q,qdot), V(q)"]
  C --> D["Lagrangian L = T - V"]
  D --> E["Compute d/dt(dL/dqdot) - dL/dq"]
  E --> F["Arrange into M(q) qddot + C(q,qdot) qdot + g(q) = tau"]
        

Consider a 2-DOF planar manipulator with two revolute joints, joint angles \( q_1, q_2 \), and link lengths \( \ell_1, \ell_2 \). Let each link be a rigid body with:

• Masses \( m_1, m_2 \).
• Center-of-mass (COM) distances along each link: \( c_1 \) from joint 1 to the COM of link 1, \( c_2 \) from joint 2 to the COM of link 2.
• Planar rotational inertias about the COM: \( I_1, I_2 \).

Using the base frame at joint 1, with the x-axis along the first link when \( q_1 = 0 \) and gravity pointing in the negative y-direction (for convenience), the COM positions are:

\[ \begin{aligned} x_1(q_1) &= c_1 \cos q_1, & y_1(q_1) &= c_1 \sin q_1, \\ x_2(q_1,q_2) &= \ell_1 \cos q_1 + c_2 \cos(q_1 + q_2), & y_2(q_1,q_2) &= \ell_1 \sin q_1 + c_2 \sin(q_1 + q_2). \end{aligned} \]

Differentiating with respect to time gives COM velocities \( \mathbf{v}_i = \dot{\mathbf{p}}_i(q,\dot{q}) \) as functions of \( q_1,q_2,\dot{q}_1,\dot{q}_2 \). These feed into the kinetic energy.

The total kinetic energy is the sum of translational and rotational contributions:

\[ T = \frac{1}{2} m_1 \|\mathbf{v}_1\|^2 + \frac{1}{2} I_1 \dot{q}_1^2 + \frac{1}{2} m_2 \|\mathbf{v}_2\|^2 + \frac{1}{2} I_2 (\dot{q}_1 + \dot{q}_2)^2 . \]

Using the expressions for \( \mathbf{v}_1,\mathbf{v}_2 \), one obtains the standard quadratic form:

\[ T = \tfrac{1}{2} \begin{bmatrix} \dot{q}_1 & \dot{q}_2 \end{bmatrix} \mathbf{M}(q_2) \begin{bmatrix} \dot{q}_1 \\ \dot{q}_2 \end{bmatrix}, \]

with inertia matrix entries

\[ \begin{aligned} M_{11}(q_2) &= I_1 + I_2 + m_1 c_1^2 + m_2\big(\ell_1^2 + c_2^2 + 2 \ell_1 c_2 \cos q_2\big), \\ M_{12}(q_2) &= M_{21}(q_2) = I_2 + m_2\big(c_2^2 + \ell_1 c_2 \cos q_2\big), \\ M_{22}(q_2) &= I_2 + m_2 c_2^2. \end{aligned} \]

The potential energy (with gravity magnitude \( g > 0 \) pointing in negative y) is

\[ V(q_1,q_2) = m_1 g c_1 \sin q_1 + m_2 g\big(\ell_1 \sin q_1 + c_2 \sin(q_1 + q_2)\big). \]

The Lagrangian is \( L(q,\dot{q}) = T(q,\dot{q}) - V(q) \) and feeds directly into the Euler–Lagrange equations.

Applying the Euler–Lagrange equation from Lesson 1, \( \frac{\mathrm{d}}{\mathrm{d}t}\frac{\partial L}{\partial \dot{q}_i} - \frac{\partial L}{\partial q_i} = \tau_i \), we obtain the standard dynamic model:

\[ \mathbf{M}(q_2)\,\ddot{\mathbf{q}} + \mathbf{C}(q_2,\dot{\mathbf{q}})\,\dot{\mathbf{q}} + \mathbf{g}(q_1,q_2) = \boldsymbol{\tau}, \quad \mathbf{q} = \begin{bmatrix} q_1 \\ q_2 \end{bmatrix}. \]

Define the coupling term \( h(q_2) = m_2 \ell_1 c_2 \sin q_2 \). A widely used (and energy-consistent) choice for \( \mathbf{C} \) is

\[ \mathbf{C}(q_2,\dot{\mathbf{q}}) = \begin{bmatrix} -h(q_2)\,\dot{q}_2 & -h(q_2)\,(\dot{q}_1 + \dot{q}_2) \\ h(q_2)\,\dot{q}_1 & 0 \end{bmatrix}. \]

The gravity vector is

\[ \mathbf{g}(q_1,q_2) = \begin{bmatrix} (m_1 c_1 + m_2 \ell_1) g \cos q_1 + m_2 c_2 g \cos(q_1 + q_2) \\ m_2 c_2 g \cos(q_1 + q_2) \end{bmatrix}. \]

The following structural properties (from Lesson 3) hold:

• Symmetry and positive definiteness: \( \mathbf{M}(q_2) = \mathbf{M}(q_2)^\top \) and \( \mathbf{M}(q_2) \) is positive definite for all configurations.
• Skew-symmetry: the matrix \( \dot{\mathbf{M}}(\mathbf{q}) - 2\mathbf{C}(\mathbf{q},\dot{\mathbf{q}}) \) is skew-symmetric, which underpins passivity of the manipulator dynamics.

Consider now a 3-DOF planar manipulator with revolute joints \( q_1,q_2,q_3 \), link lengths \( \ell_1,\ell_2,\ell_3 \), COM distances \( c_1,c_2,c_3 \), masses \( m_1,m_2,m_3 \), and inertias \( I_1,I_2,I_3 \) about the COMs.

The kinetic energy again has quadratic form

\[ T = \tfrac{1}{2} \dot{\mathbf{q}}^\top \mathbf{M}(\mathbf{q}) \dot{\mathbf{q}}, \quad \mathbf{q} = \begin{bmatrix} q_1 \\ q_2 \\ q_3 \end{bmatrix}, \]

with \( \mathbf{M}(\mathbf{q}) \) a \( 3\times 3 \) symmetric matrix. Instead of writing all entries explicitly (which is algebraically long), it is useful to recognize that every entry is a linear combination of a small set of trigonometric functions:

\[ \mathbf{M}(\mathbf{q}) = \mathbf{M}_0 + \mathbf{M}_1 \cos q_2 + \mathbf{M}_2 \cos q_3 + \mathbf{M}_3 \cos(q_2 + q_3), \]

where \( \mathbf{M}_0,\dots,\mathbf{M}_3 \) are constant symmetric matrices determined by \( m_i, I_i, \ell_i, c_i \). This parametric structure is heavily used in identification (Chapter 19).

The gravity vector has the generic structure

\[ \mathbf{g}(\mathbf{q}) = \mathbf{g}_1 \cos q_1 + \mathbf{g}_2 \cos(q_1 + q_2) + \mathbf{g}_3 \cos(q_1 + q_2 + q_3), \]

where the constant vectors \( \mathbf{g}_i \) encode the vertical projection of each link's COM. The Coriolis/centrifugal terms are constructed from the Christoffel symbols (Lesson 2); they can be expressed as bilinear forms in \( \dot{\mathbf{q}} \) with coefficients depending on \( \sin q_2, \sin q_3, \sin(q_2 + q_3) \).

Despite the more complex expressions, all structural properties (symmetry and positive definiteness of \( \mathbf{M} \), skew-symmetry of \( \dot{\mathbf{M}} - 2\mathbf{C} \), passivity) remain valid.

flowchart TD
  Q["q = [q1,q2,q3]"] --> T["Compute link COM speeds: v1,v2,v3"]
  T --> K["Form T = 0.5 * sum( mi * |vi|^2 + Ii * wi^2 )"]
  K --> M["Identify M(q) by collecting terms in qdot"]
  M --> Cmat["Compute Christoffel coefficients for C(q,qdot)"]
  Cmat --> G["Take V(q) from COM heights, then g(q) = dV/dq"]
  G --> FINAL["Assemble 3x3 dynamics: M(q) qddot + C(q,qdot) qdot + g(q) = tau"]
        

We now implement the 2-DOF planar dynamics explicitly in Python using numpy. Symbolic derivation can be performed with sympy, but here we assume the closed-form expressions from Sections 3 and 4.

import numpy as np

class TwoLinkParams:
    def __init__(self, l1, l2, c1, c2, m1, m2, I1, I2, g=9.81):
        self.l1 = l1
        self.l2 = l2
        self.c1 = c1
        self.c2 = c2
        self.m1 = m1
        self.m2 = m2
        self.I1 = I1
        self.I2 = I2
        self.g = g

def M_2R(q, params):
    q1, q2 = q
    l1 = params.l1
    c1 = params.c1
    c2 = params.c2
    m1 = params.m1
    m2 = params.m2
    I1 = params.I1
    I2 = params.I2

    cos2 = np.cos(q2)

    M11 = I1 + I2 + m1 * c1**2 + m2 * (l1**2 + c2**2 + 2.0 * l1 * c2 * cos2)
    M12 = I2 + m2 * (c2**2 + l1 * c2 * cos2)
    M22 = I2 + m2 * c2**2

    return np.array([[M11, M12],
                     [M12, M22]])

def C_2R(q, qdot, params):
    q1, q2 = q
    q1dot, q2dot = qdot
    l1 = params.l1
    c2 = params.c2
    m2 = params.m2

    sin2 = np.sin(q2)
    h = m2 * l1 * c2 * sin2

    C11 = -h * q2dot
    C12 = -h * (q1dot + q2dot)
    C21 = h * q1dot
    C22 = 0.0

    return np.array([[C11, C12],
                     [C21, C22]])

def g_2R(q, params):
    q1, q2 = q
    c1 = params.c1
    c2 = params.c2
    l1 = params.l1
    m1 = params.m1
    m2 = params.m2
    g = params.g

    g1 = (m1 * c1 + m2 * l1) * g * np.cos(q1) + m2 * c2 * g * np.cos(q1 + q2)
    g2 = m2 * c2 * g * np.cos(q1 + q2)

    return np.array([g1, g2])

def forward_dynamics_2R(q, qdot, tau, params):
    M = M_2R(q, params)
    C = C_2R(q, qdot, params)
    g_vec = g_2R(q, params)
    rhs = tau - C @ qdot - g_vec
    qddot = np.linalg.solve(M, rhs)
    return qddot

# Example usage
if __name__ == "__main__":
    params = TwoLinkParams(
        l1=1.0, l2=1.0,
        c1=0.5, c2=0.5,
        m1=1.0, m2=1.0,
        I1=0.1, I2=0.1,
        g=9.81
    )
    q = np.array([0.3, 0.4])
    qdot = np.array([0.2, -0.1])
    tau = np.array([0.5, 0.2])
    qddot = forward_dynamics_2R(q, qdot, tau, params)
    print("qddot =", qddot)
      

For a 3-DOF planar arm, one practical approach is to precompute constant matrices \( \mathbf{M}_0,\dots,\mathbf{M}_3 \) and implement

def M_3R(q, M0, M1, M2, M3):
    q1, q2, q3 = q
    return (M0
            + np.cos(q2) * M1
            + np.cos(q3) * M2
            + np.cos(q2 + q3) * M3)
      

where M0,...,M3 are 3x3 symmetric numpy arrays determined once from symbolic derivation (Lesson 5).

In C++, a common choice for small dense linear algebra is the Eigen library. Below we implement \( \mathbf{M}, \mathbf{C}, \mathbf{g} \) and the forward dynamics map \( (\mathbf{q},\dot{\mathbf{q}},\boldsymbol{\tau}) \mapsto \ddot{\mathbf{q}} \).

#include <Eigen/Dense>

struct TwoLinkParams {
    double l1, l2;
    double c1, c2;
    double m1, m2;
    double I1, I2;
    double g;
};

Eigen::Matrix2d M_2R(const Eigen::Vector2d& q, const TwoLinkParams& p) {
    double q2 = q(1);
    double cos2 = std::cos(q2);

    double M11 = p.I1 + p.I2
                 + p.m1 * p.c1 * p.c1
                 + p.m2 * (p.l1 * p.l1 + p.c2 * p.c2 + 2.0 * p.l1 * p.c2 * cos2);
    double M12 = p.I2 + p.m2 * (p.c2 * p.c2 + p.l1 * p.c2 * cos2);
    double M22 = p.I2 + p.m2 * p.c2 * p.c2;

    Eigen::Matrix2d M;
    M(0,0) = M11; M(0,1) = M12;
    M(1,0) = M12; M(1,1) = M22;
    return M;
}

Eigen::Matrix2d C_2R(const Eigen::Vector2d& q,
                     const Eigen::Vector2d& qdot,
                     const TwoLinkParams& p) {
    double q2 = q(1);
    double q1dot = qdot(0);
    double q2dot = qdot(1);

    double sin2 = std::sin(q2);
    double h = p.m2 * p.l1 * p.c2 * sin2;

    Eigen::Matrix2d C;
    C(0,0) = -h * q2dot;
    C(0,1) = -h * (q1dot + q2dot);
    C(1,0) =  h * q1dot;
    C(1,1) =  0.0;
    return C;
}

Eigen::Vector2d g_2R(const Eigen::Vector2d& q, const TwoLinkParams& p) {
    double q1 = q(0);
    double q2 = q(1);

    double g1 = (p.m1 * p.c1 + p.m2 * p.l1) * p.g * std::cos(q1)
                + p.m2 * p.c2 * p.g * std::cos(q1 + q2);
    double g2 = p.m2 * p.c2 * p.g * std::cos(q1 + q2);

    Eigen::Vector2d gv;
    gv(0) = g1;
    gv(1) = g2;
    return gv;
}

Eigen::Vector2d forward_dynamics_2R(const Eigen::Vector2d& q,
                                    const Eigen::Vector2d& qdot,
                                    const Eigen::Vector2d& tau,
                                    const TwoLinkParams& p) {
    Eigen::Matrix2d M = M_2R(q, p);
    Eigen::Matrix2d C = C_2R(q, qdot, p);
    Eigen::Vector2d gvec = g_2R(q, p);

    Eigen::Vector2d rhs = tau - C * qdot - gvec;
    Eigen::Vector2d qddot = M.ldlt().solve(rhs);
    return qddot;
}
      

For larger DOF counts or dynamic parameters, one typically organizes the code in classes (e.g., a RigidBody hierarchy) but the structural form remains the same.

In Java, we can implement the same model using primitive arrays without external libraries. The inertia matrix inversion is coded explicitly for the 2x2 case.

public class TwoLinkDynamics {

    public static class Params {
        public double l1, l2;
        public double c1, c2;
        public double m1, m2;
        public double I1, I2;
        public double g;

        public Params(double l1, double l2,
                      double c1, double c2,
                      double m1, double m2,
                      double I1, double I2,
                      double g) {
            this.l1 = l1;
            this.l2 = l2;
            this.c1 = c1;
            this.c2 = c2;
            this.m1 = m1;
            this.m2 = m2;
            this.I1 = I1;
            this.I2 = I2;
            this.g = g;
        }
    }

    public static double[][] M(double[] q, Params p) {
        double q2 = q[1];
        double cos2 = Math.cos(q2);

        double M11 = p.I1 + p.I2
                     + p.m1 * p.c1 * p.c1
                     + p.m2 * (p.l1 * p.l1 + p.c2 * p.c2 + 2.0 * p.l1 * p.c2 * cos2);
        double M12 = p.I2 + p.m2 * (p.c2 * p.c2 + p.l1 * p.c2 * cos2);
        double M22 = p.I2 + p.m2 * p.c2 * p.c2;

        return new double[][] {
            { M11, M12 },
            { M12, M22 }
        };
    }

    public static double[][] C(double[] q, double[] qdot, Params p) {
        double q2 = q[1];
        double q1dot = qdot[0];
        double q2dot = qdot[1];

        double sin2 = Math.sin(q2);
        double h = p.m2 * p.l1 * p.c2 * sin2;

        return new double[][] {
            { -h * q2dot,      -h * (q1dot + q2dot) },
            {  h * q1dot,       0.0 }
        };
    }

    public static double[] g(double[] q, Params p) {
        double q1 = q[0];
        double q2 = q[1];

        double g1 = (p.m1 * p.c1 + p.m2 * p.l1) * p.g * Math.cos(q1)
                    + p.m2 * p.c2 * p.g * Math.cos(q1 + q2);
        double g2 = p.m2 * p.c2 * p.g * Math.cos(q1 + q2);

        return new double[] { g1, g2 };
    }

    public static double[] forwardDynamics(double[] q,
                                           double[] qdot,
                                           double[] tau,
                                           Params p) {
        double[][] M = M(q, p);
        double[][] C = C(q, qdot, p);
        double[] g = g(q, p);

        // rhs = tau - C qdot - g
        double[] rhs = new double[2];
        rhs[0] = tau[0]
                 - (C[0][0] * qdot[0] + C[0][1] * qdot[1])
                 - g[0];
        rhs[1] = tau[1]
                 - (C[1][0] * qdot[0] + C[1][1] * qdot[1])
                 - g[1];

        // Invert 2x2 inertia matrix
        double det = M[0][0] * M[1][1] - M[0][1] * M[1][0];
        double inv00 =  M[1][1] / det;
        double inv01 = -M[0][1] / det;
        double inv10 = -M[1][0] / det;
        double inv11 =  M[0][0] / det;

        double[] qddot = new double[2];
        qddot[0] = inv00 * rhs[0] + inv01 * rhs[1];
        qddot[1] = inv10 * rhs[0] + inv11 * rhs[1];
        return qddot;
    }
}
      

For a 3-DOF arm, similar array-based code can be used with a generic 3x3 matrix inversion routine.

MATLAB offers both symbolic and numeric workflows. A compact numeric function for the 2-DOF dynamics is:

function qddot = two_link_forward_dynamics(t, state, tau, p)
% state = [q1; q2; q1dot; q2dot]
q1 = state(1); q2 = state(2);
q1dot = state(3); q2dot = state(4);

l1 = p.l1; c1 = p.c1; c2 = p.c2;
m1 = p.m1; m2 = p.m2;
I1 = p.I1; I2 = p.I2;
g = p.g;

cos2 = cos(q2);
sin2 = sin(q2);

M11 = I1 + I2 + m1 * c1^2 + m2 * (l1^2 + c2^2 + 2 * l1 * c2 * cos2);
M12 = I2 + m2 * (c2^2 + l1 * c2 * cos2);
M22 = I2 + m2 * c2^2;
M = [M11, M12; M12, M22];

h = m2 * l1 * c2 * sin2;
C = [-h * q2dot,          -h * (q1dot + q2dot);
      h * q1dot,           0];

g1 = (m1 * c1 + m2 * l1) * g * cos(q1) + m2 * c2 * g * cos(q1 + q2);
g2 = m2 * c2 * g * cos(q1 + q2);
gvec = [g1; g2];

rhs = tau - C * [q1dot; q2dot] - gvec;
qdd = M \ rhs;

qddot = qdd;
end
      

In Simulink, one typical approach is:

• Use a MATLAB Function block that encapsulates two_link_forward_dynamics.
• Integrate the joint accelerations using two cascaded Integrator blocks to obtain \( \dot{\mathbf{q}} \) and \( \mathbf{q} \).
• Feed commanded \( \boldsymbol{\tau} \) as an input signal to the function block.

For a 3-DOF arm, a 3x3 inertia matrix and 3x1 vectors are used, with the same signal-flow structure.

Wolfram Mathematica is very convenient for symbolic Lagrangian derivations. Below is a template for the 2-DOF planar arm.

(* Define symbols *)
ClearAll["Global`*"];
q1[t_]; q2[t_];
m1 = Symbol["m1"]; m2 = Symbol["m2"];
l1 = Symbol["l1"]; c1 = Symbol["c1"]; c2 = Symbol["c2"];
I1 = Symbol["I1"]; I2 = Symbol["I2"];
g  = Symbol["g"];

q1t = q1[t]; q2t = q2[t];
q1d = D[q1t, t]; q2d = D[q2t, t];

(* COM positions *)
x1 = c1 Cos[q1t];
y1 = c1 Sin[q1t];

x2 = l1 Cos[q1t] + c2 Cos[q1t + q2t];
y2 = l1 Sin[q1t] + c2 Sin[q1t + q2t];

(* Velocities *)
vx1 = D[x1, t]; vy1 = D[y1, t];
vx2 = D[x2, t]; vy2 = D[y2, t];

(* Kinetic and potential energy *)
T = 1/2 m1 (vx1^2 + vy1^2) + 1/2 I1 q1d^2
  + 1/2 m2 (vx2^2 + vy2^2) + 1/2 I2 (q1d + q2d)^2;

V = m1 g y1 + m2 g y2;

L = T - V;

(* Euler-Lagrange equations *)
eq1 = D[D[L, q1d], t] - D[L, q1t] == Symbol["tau1"];
eq2 = D[D[L, q2d], t] - D[L, q2t] == Symbol["tau2"];

(* Expand and collect with respect to accelerations q1dd, q2dd *)
q1dd = D[q1d, t]; q2dd = D[q2d, t];
eq1exp = Expand[eq1];
eq2exp = Expand[eq2];

(* Coefficients of q1dd, q2dd provide inertia matrix entries *)
M11 = Coefficient[eq1exp, q1dd];
M12 = Coefficient[eq1exp, q2dd];
M21 = Coefficient[eq2exp, q1dd];
M22 = Coefficient[eq2exp, q2dd];

Mmat = { {M11, M12}, {M21, M22} } // Simplify;

(* Residual terms yield C(q,qdot) qdot + g(q) *)
rhs1 = (Symbol["tau1"] - (eq1exp /. {q1dd -> 0, q2dd -> 0})) // Simplify;
rhs2 = (Symbol["tau2"] - (eq2exp /. {q1dd -> 0, q2dd -> 0})) // Simplify;

rhs = {rhs1, rhs2};
      

The resulting Mmat and rhs can be further processed to identify \( \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} \) and \( \mathbf{g}(\mathbf{q}) \), and exported as C or C++ code.

In this lesson we instantiated the Lagrange–Euler framework on concrete serial manipulators: a 2-DOF planar R–R arm and a 3-DOF planar R–R–R arm. We derived explicit expressions for the inertia matrix, Coriolis/centrifugal terms, and gravity vector, and emphasized the structural properties (symmetry, positive definiteness, skew-symmetry of \( \dot{\mathbf{M}} - 2\mathbf{C} \), and passivity).

We then implemented these models in several programming environments that are central in robotics: Python with numpy, C++ with Eigen, Java with primitive arrays, MATLAB/Simulink, and Wolfram Mathematica. These concrete examples will be the basis for the symbolic and numeric derivation pipelines developed in Lesson 5.