Consider an \( n \)-DOF serial manipulator with generalized coordinates \( \mathbf{q} \in \mathbb{R}^n \), velocities \( \dot{\mathbf{q}} \), and accelerations \( \ddot{\mathbf{q}} \). Using the Lagrangian \( L(\mathbf{q},\dot{\mathbf{q}})=K(\mathbf{q},\dot{\mathbf{q}})-P(\mathbf{q}) \) from the previous lesson, the Euler–Lagrange equations with generalized input vector \( \mathbf{u} \in \mathbb{R}^n \) are

\[ \frac{d}{dt}\left(\frac{\partial L}{\partial \dot{q}_i}\right) - \frac{\partial L}{\partial q_i} = u_i, \quad i = 1,\dots,n. \]

For rigid-body manipulators, the kinetic energy can always be written as a quadratic form in the generalized velocities:

\[ K(\mathbf{q},\dot{\mathbf{q}}) = \tfrac{1}{2}\,\dot{\mathbf{q}}^{\mathsf{T}} \,\mathbf{M}(\mathbf{q})\,\dot{\mathbf{q}}, \]

where \( \mathbf{M}(\mathbf{q}) \in \mathbb{R}^{n \times n} \) is the joint-space inertia matrix. Substituting this expression into the Euler–Lagrange equations and rearranging yields the canonical manipulator form:

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

where:

• \( \mathbf{M}(\mathbf{q}) \) is the symmetric positive definite inertia matrix.
• \( \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\,\dot{\mathbf{q}} \) collects Coriolis and centrifugal effects (quadratic in velocities).
• \( \mathbf{g}(\mathbf{q}) \) is the gravity vector, derived from the potential energy.
• \( \mathbf{f}(\dot{\mathbf{q}}) \) models joint friction and other dissipative effects (e.g., viscous or Coulomb friction).
• \( \mathbf{u} \) is the vector of generalized input forces/torques at the joints.

flowchart TD
  Q["q, qd, qdd"] --> L["K(q,qd), P(q)"]
  L --> EL["Euler-Lagrange equations"]
  EL --> CAN["M(q) qdd + C(q,qd) qd + g(q) + f(qd) = u"]
  CAN --> PROP["Matrix properties: symmetry, SPD, skew-symmetry"]
  PROP --> APP["Used in control, simulation, identification"]
        

This matrix structure is universal for rigid-body manipulators, independent of the specific geometry or number of links. The remainder of the lesson is dedicated to understanding and proving the structural properties of each term.

Starting from \( L(\mathbf{q},\dot{\mathbf{q}})=K(\mathbf{q},\dot{\mathbf{q}})-P(\mathbf{q}) \), the Euler–Lagrange equations component-wise are

\[ \frac{d}{dt}\left(\frac{\partial K}{\partial \dot{q}_i}\right) - \frac{\partial K}{\partial q_i} + \frac{\partial P}{\partial q_i} = u_i. \]

With the quadratic kinetic energy \( K = \tfrac{1}{2}\dot{\mathbf{q}}^{\mathsf{T}}\mathbf{M}(\mathbf{q})\dot{\mathbf{q}} \), we obtain

\[ \frac{\partial K}{\partial \dot{q}_i} = \sum_{j=1}^{n} M_{ij}(\mathbf{q}) \dot{q}_j, \quad \frac{\partial K}{\partial q_i} = \tfrac{1}{2} \sum_{j,k=1}^{n} \frac{\partial M_{jk}(\mathbf{q})}{\partial q_i} \dot{q}_j \dot{q}_k. \]

Differentiating the first expression with respect to time gives

\[ \frac{d}{dt}\left(\frac{\partial K}{\partial \dot{q}_i}\right) = \sum_{j=1}^{n} M_{ij}(\mathbf{q}) \ddot{q}_j + \sum_{j,k=1}^{n} \frac{\partial M_{ij}(\mathbf{q})}{\partial q_k} \dot{q}_k \dot{q}_j. \]

Substituting into the Euler–Lagrange equations, regrouping terms, and collecting coefficients of \( \ddot{\mathbf{q}} \) yields:

\[ \sum_{j=1}^{n} M_{ij}(\mathbf{q}) \ddot{q}_j + \sum_{j,k=1}^{n} h_{ijk}(\mathbf{q}) \dot{q}_j \dot{q}_k + \frac{\partial P(\mathbf{q})}{\partial q_i} = u_i, \]

where the coefficients \( h_{ijk}(\mathbf{q}) \) are defined by

\[ h_{ijk}(\mathbf{q}) = \tfrac{1}{2} \left( \frac{\partial M_{ij}(\mathbf{q})}{\partial q_k} + \frac{\partial M_{ik}(\mathbf{q})}{\partial q_j} - \frac{\partial M_{jk}(\mathbf{q})}{\partial q_i} \right). \]

We can now define the entries of \( \mathbf{C}(\mathbf{q},\dot{\mathbf{q}}) \) as

\[ C_{ij}(\mathbf{q},\dot{\mathbf{q}}) = \sum_{k=1}^{n} h_{ijk}(\mathbf{q}) \dot{q}_k, \]

which gives the velocity-dependent term \( \sum_{j} C_{ij}\dot{q}_j \). The gravity term is

\[ g_i(\mathbf{q}) = \frac{\partial P(\mathbf{q})}{\partial q_i}. \]

Collecting all \( n \) equations into vector form yields exactly \( \mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \mathbf{u} \) for the conservative system without friction.

Note that the coefficients \( h_{ijk} \) are the classical Christoffel symbols of the first kind associated with the inertia matrix. Their geometric role will be studied in detail in the next chapter; here we use them purely as a convenient algebraic device.

Let the manipulator consist of \( n_{\ell} \) rigid links. For each link \( \ell \), denote:

• \( \mathbf{J}_{\ell}(\mathbf{q}) \in \mathbb{R}^{6 \times n} \) the spatial Jacobian mapping joint velocities to the spatial twist of link \( \ell \).
• \( \mathbf{I}_{\ell} \in \mathbb{R}^{6 \times 6} \) the spatial inertia of link \( \ell \) expressed in a common frame.

The kinetic energy can be written as

\[ K = \tfrac{1}{2} \sum_{\ell=1}^{n_{\ell}} \boldsymbol{\nu}_{\ell}^{\mathsf{T}} \mathbf{I}_{\ell} \boldsymbol{\nu}_{\ell}, \quad \boldsymbol{\nu}_{\ell} = \mathbf{J}_{\ell}(\mathbf{q}) \dot{\mathbf{q}}, \]

so that

\[ K = \tfrac{1}{2} \sum_{\ell=1}^{n_{\ell}} \dot{\mathbf{q}}^{\mathsf{T}} \mathbf{J}_{\ell}(\mathbf{q})^{\mathsf{T}} \mathbf{I}_{\ell} \mathbf{J}_{\ell}(\mathbf{q}) \dot{\mathbf{q}} = \tfrac{1}{2} \dot{\mathbf{q}}^{\mathsf{T}} \mathbf{M}(\mathbf{q}) \dot{\mathbf{q}}, \]

with

\[ \mathbf{M}(\mathbf{q}) = \sum_{\ell=1}^{n_{\ell}} \mathbf{J}_{\ell}(\mathbf{q})^{\mathsf{T}} \mathbf{I}_{\ell} \mathbf{J}_{\ell}(\mathbf{q}). \]

Since each spatial inertia \( \mathbf{I}_{\ell} \) is symmetric positive definite, it follows that:

• Symmetry: \( \mathbf{M}(\mathbf{q})^{\mathsf{T}} = \mathbf{M}(\mathbf{q}) \).
• Positive definiteness: for any nonzero \( \mathbf{z} \in \mathbb{R}^n \),

\[ \mathbf{z}^{\mathsf{T}}\mathbf{M}(\mathbf{q})\mathbf{z} = \sum_{\ell=1}^{n_{\ell}} \left(\mathbf{J}_{\ell}(\mathbf{q})\mathbf{z}\right)^{\mathsf{T}} \mathbf{I}_{\ell} \left(\mathbf{J}_{\ell}(\mathbf{q})\mathbf{z}\right) > 0 \]

unless all link twists induced by \( \mathbf{z} \) vanish. For a non-redundant manipulator with physically realizable joints, this implies that \( \mathbf{M}(\mathbf{q}) \) is positive definite for all reachable configurations.

The positive definiteness of \( \mathbf{M}(\mathbf{q}) \) guarantees that the kinetic energy defines a Riemannian metric on the configuration manifold and ensures that the dynamics are well-posed as a second-order ordinary differential equation in time for \( \mathbf{q}(t) \).

The matrix \( \mathbf{C}(\mathbf{q},\dot{\mathbf{q}}) \) is not unique; different but equivalent choices lead to different representations. Nevertheless, there is a fundamental structural property:

\[ \dot{\mathbf{M}}(\mathbf{q}) - 2\mathbf{C}(\mathbf{q},\dot{\mathbf{q}}) \text{ is skew-symmetric}, \]

in the sense that for any \( \mathbf{z} \in \mathbb{R}^n \),

\[ \mathbf{z}^{\mathsf{T}} \left(\dot{\mathbf{M}}(\mathbf{q}) - 2\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\right) \mathbf{z} = 0. \]

Sketch of proof (energy-based):

1. For the conservative system without friction and external inputs, \( \mathbf{u} = \mathbf{0}, \mathbf{f} = \mathbf{0} \), the dynamics reduce to \( \mathbf{M}\ddot{\mathbf{q}} + \mathbf{C}\dot{\mathbf{q}} + \mathbf{g} = \mathbf{0} \).
2. Define the total mechanical energy \( E(\mathbf{q},\dot{\mathbf{q}}) = K(\mathbf{q},\dot{\mathbf{q}})+P(\mathbf{q}) \).
3. Differentiating \( K = \tfrac{1}{2}\dot{\mathbf{q}}^{\mathsf{T}}\mathbf{M}\dot{\mathbf{q}} \) gives

\[ \dot{K} = \tfrac{1}{2}\dot{\mathbf{q}}^{\mathsf{T}}\dot{\mathbf{M}}\dot{\mathbf{q}} + \dot{\mathbf{q}}^{\mathsf{T}}\mathbf{M}\ddot{\mathbf{q}}. \]

4. Substitute \( \mathbf{M}\ddot{\mathbf{q}} = -\mathbf{C}\dot{\mathbf{q}} - \mathbf{g} \) and use \( \dot{P} = \dot{\mathbf{q}}^{\mathsf{T}}\mathbf{g} \) to obtain

\[ \dot{E} = \dot{K} + \dot{P} = \tfrac{1}{2}\dot{\mathbf{q}}^{\mathsf{T}}\dot{\mathbf{M}}\dot{\mathbf{q}} - \dot{\mathbf{q}}^{\mathsf{T}}\mathbf{C}\dot{\mathbf{q}}. \]

For a conservative system we require \( \dot{E} = 0 \) for all possible \( (\mathbf{q},\dot{\mathbf{q}}) \). Hence

\[ \dot{\mathbf{q}}^{\mathsf{T}} \left(\dot{\mathbf{M}}(\mathbf{q}) - 2\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\right) \dot{\mathbf{q}} = 0 \quad \text{for all } \dot{\mathbf{q}}, \]

which is exactly the skew-symmetry property. This structure is crucial for energy-based control designs, passivity analysis, and numerical integration schemes that preserve energy qualitatively.

Suppose the gravitational acceleration vector in the world frame is \( \mathbf{g}_0 \in \mathbb{R}^{3} \). Let \( \mathbf{p}_{c,\ell}(\mathbf{q}) \in \mathbb{R}^{3} \) be the position of the center of mass of link \( \ell \) of mass \( m_{\ell} \). Then the gravitational potential energy is

\[ P(\mathbf{q}) = \sum_{\ell=1}^{n_{\ell}} m_{\ell} \,\mathbf{g}_0^{\mathsf{T}}\, \mathbf{p}_{c,\ell}(\mathbf{q}). \]

The gravity vector is the gradient of this scalar potential with respect to the generalized coordinates:

\[ g_i(\mathbf{q}) = \frac{\partial P(\mathbf{q})}{\partial q_i} = \sum_{\ell=1}^{n_{\ell}} m_{\ell} \,\mathbf{g}_0^{\mathsf{T}}\, \frac{\partial \mathbf{p}_{c,\ell}(\mathbf{q})}{\partial q_i}, \quad i=1,\dots,n. \]

In matrix form, using the translational Jacobian \( \mathbf{J}_{v,\ell}(\mathbf{q}) \in \mathbb{R}^{3 \times n} \) for the CoM of link \( \ell \), we can write

\[ \mathbf{g}(\mathbf{q}) = \sum_{\ell=1}^{n_{\ell}} \mathbf{J}_{v,\ell}(\mathbf{q})^{\mathsf{T}} m_{\ell} \mathbf{g}_0. \]

This representation shows that the gravity term is linear in the masses \( m_{\ell} \) and depends only on configuration \( \mathbf{q} \), not on velocities or accelerations.

flowchart TD
  CONF["q"] --> COM["Link CoM positions p_c_l(q)"]
  COM --> P["Potential P(q) = sum m_l g0^T p_c_l(q)"]
  P --> G["g(q) = grad_q P(q)"]
  G --> EFF["Joint-space gravity torques/forces"]
        

An important structural fact is that the equations of motion are linear in the inertial parameters (masses, center-of-mass coordinates, and inertia tensor components). Let \( \boldsymbol{\pi} \) denote the stacked vector of all such parameters for all links. Then for any \( (\mathbf{q},\dot{\mathbf{q}},\ddot{\mathbf{q}}) \),

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

for some \( n \times p \) regressor matrix \( \mathbf{Y}(\mathbf{q},\dot{\mathbf{q}},\ddot{\mathbf{q}}) \), where \( p \) is the number of independent inertial parameters.

Reason: each entry of \( \mathbf{M}, \mathbf{C}, \mathbf{g} \) is a linear combination of link inertial parameters multiplied by known functions of configuration and velocities. This regressor form is fundamental for parameter identification and adaptive control (developed in later chapters), but the linearity is purely a structural property of the rigid-body dynamics.

Consider a planar 2R manipulator with joint angles \( q_1, q_2 \), link lengths \( \ell_1, \ell_2 \), center-of-mass distances \( \ell_{c1}, \ell_{c2} \), masses \( m_1, m_2 \), and scalar planar inertias \( I_1, I_2 \). Let \( c_2 = \cos(q_2), s_2 = \sin(q_2) \).

The inertia matrix has the structure

\[ \mathbf{M}(\mathbf{q}) = \begin{bmatrix} M_{11} & M_{12} \\ M_{12} & M_{22} \end{bmatrix}, \]

\[ \begin{aligned} M_{11} &= I_1 + I_2 + m_1 \ell_{c1}^2 + m_2(\ell_1^2 + \ell_{c2}^2 + 2\ell_1\ell_{c2} c_2), \\ M_{12} &= I_2 + m_2(\ell_{c2}^2 + \ell_1 \ell_{c2} c_2), \\ M_{22} &= I_2 + m_2 \ell_{c2}^2. \end{aligned} \]

A convenient representation of the Coriolis/centrifugal term uses a scalar \( h(\mathbf{q}) = -m_2 \ell_1 \ell_{c2} s_2 \). Then

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

which is one of several equivalent forms. The gravity vector (taking gravity acting in \( -y \) direction) can be written as

\[ \begin{aligned} g_1(\mathbf{q}) &= (m_1 \ell_{c1} + m_2 \ell_1) g \cos(q_1) + m_2 \ell_{c2} g \cos(q_1 + q_2), \\ g_2(\mathbf{q}) &= m_2 \ell_{c2} g \cos(q_1 + q_2), \end{aligned} \]

so that the complete EoM are

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

This example concretely illustrates how the abstract matrices \( \mathbf{M}, \mathbf{C}, \mathbf{g} \) look for a simple manipulator.

In Python, typical libraries for robot dynamics are:

• numpy for numerical linear algebra.
• sympy for symbolic derivations of \( \mathbf{M},\mathbf{C},\mathbf{g} \).
• roboticstoolbox-python or modern_robotics for higher-level robotics utilities.

Below is a minimal implementation of \( \mathbf{M}(\mathbf{q}),\mathbf{C}(\mathbf{q},\dot{\mathbf{q}}),\mathbf{g}(\mathbf{q}) \) for the planar 2R manipulator, using numpy. Note that < and > do not appear inside the code.

import numpy as np

class Planar2R:
    def __init__(self, l1, l2, lc1, lc2, m1, m2, I1, I2, g=9.81):
        self.l1 = l1
        self.l2 = l2
        self.lc1 = lc1
        self.lc2 = lc2
        self.m1 = m1
        self.m2 = m2
        self.I1 = I1
        self.I2 = I2
        self.g = g

    def M(self, q):
        q1, q2 = q
        c2 = np.cos(q2)

        m11 = (self.I1 + self.I2
               + self.m1 * self.lc1**2
               + self.m2 * (self.l1**2 + self.lc2**2 + 2.0 * self.l1 * self.lc2 * c2))
        m12 = self.I2 + self.m2 * (self.lc2**2 + self.l1 * self.lc2 * c2)
        m22 = self.I2 + self.m2 * self.lc2**2

        return np.array([[m11, m12],
                         [m12, m22]])

    def C(self, q, qd):
        q1, q2 = q
        q1d, q2d = qd
        s2 = np.sin(q2)
        h = -self.m2 * self.l1 * self.lc2 * s2

        # One common choice of C matrix
        c11 = h * q2d
        c12 = h * (q1d + q2d)
        c21 = -h * q1d
        c22 = 0.0

        return np.array([[c11, c12],
                         [c21, c22]])

    def g_vec(self, q):
        q1, q2 = q
        g1 = ((self.m1 * self.lc1 + self.m2 * self.l1) * self.g * np.cos(q1)
              + self.m2 * self.lc2 * self.g * np.cos(q1 + q2))
        g2 = self.m2 * self.lc2 * self.g * np.cos(q1 + q2)
        return np.array([g1, g2])

    def dynamics(self, q, qd, u):
        """Compute qdd = M(q)^(-1) (u - C(q,qd) qd - g(q))."""
        Mq = self.M(q)
        Cq = self.C(q, qd)
        gq = self.g_vec(q)
        rhs = u - Cq.dot(qd) - gq
        qdd = np.linalg.solve(Mq, rhs)
        return qdd
      

Symbolic derivation of \( \mathbf{M},\mathbf{C},\mathbf{g} \) can be performed in sympy, and then exported as optimized numerical code for real-time computation.

In C++, Eigen is a common linear algebra library, and modern robotics libraries such as Pinocchio or RBDL use similar structures \( \mathbf{M}(\mathbf{q}), \mathbf{C}(\mathbf{q},\dot{\mathbf{q}}), \mathbf{g}(\mathbf{q}) \). The snippet below illustrates a 2R model using Eigen::Matrix2d.

#include <Eigen/Dense>

struct Planar2R {
    double l1, l2, lc1, lc2;
    double m1, m2, I1, I2;
    double g;

    Planar2R(double l1_, double l2_, double lc1_, double lc2_,
             double m1_, double m2_, double I1_, double I2_, double g_ = 9.81)
        : l1(l1_), l2(l2_), lc1(lc1_), lc2(lc2_),
          m1(m1_), m2(m2_), I1(I1_), I2(I2_), g(g_) {}

    Eigen::Matrix2d M(const Eigen::Vector2d& q) const {
        double q2 = q(1);
        double c2 = std::cos(q2);

        double m11 = I1 + I2
                     + m1 * lc1 * lc1
                     + m2 * (l1 * l1 + lc2 * lc2 + 2.0 * l1 * lc2 * c2);
        double m12 = I2 + m2 * (lc2 * lc2 + l1 * lc2 * c2);
        double m22 = I2 + m2 * lc2 * lc2;

        Eigen::Matrix2d Mq;
        Mq(0,0) = m11; Mq(0,1) = m12;
        Mq(1,0) = m12; Mq(1,1) = m22;
        return Mq;
    }

    Eigen::Matrix2d C(const Eigen::Vector2d& q,
                      const Eigen::Vector2d& qd) const {
        double q2 = q(1);
        double q1d = qd(0);
        double q2d = qd(1);
        double s2 = std::sin(q2);
        double h = -m2 * l1 * lc2 * s2;

        Eigen::Matrix2d Cq;
        Cq(0,0) = h * q2d;
        Cq(0,1) = h * (q1d + q2d);
        Cq(1,0) = -h * q1d;
        Cq(1,1) = 0.0;
        return Cq;
    }

    Eigen::Vector2d g_vec(const Eigen::Vector2d& q) const {
        double q1 = q(0);
        double q2 = q(1);
        double g1 = ((m1 * lc1 + m2 * l1) * g * std::cos(q1)
                     + m2 * lc2 * g * std::cos(q1 + q2));
        double g2 = m2 * lc2 * g * std::cos(q1 + q2);
        return Eigen::Vector2d(g1, g2);
    }

    Eigen::Vector2d dynamics(const Eigen::Vector2d& q,
                             const Eigen::Vector2d& qd,
                             const Eigen::Vector2d& u) const {
        Eigen::Matrix2d Mq = M(q);
        Eigen::Matrix2d Cq = C(q, qd);
        Eigen::Vector2d gq = g_vec(q);
        Eigen::Vector2d rhs = u - Cq * qd - gq;
        return Mq.ldlt().solve(rhs); // qdd
    }
};
      

Full-featured libraries like Pinocchio internally provide efficient routines such as crba (composite rigid-body algorithm) for \( \mathbf{M}(\mathbf{q}) \) and rnea (recursive Newton–Euler algorithm) for inverse dynamics without requiring manual symbolic derivation.

In Java, libraries such as EJML or Apache Commons Math can be used to implement the same structure. Here is a simplified implementation using plain arrays; in practice, one would wrap these in EJML matrix classes.

public class Planar2R {
    public double l1, l2, lc1, lc2;
    public double m1, m2, I1, I2, g;

    public Planar2R(double l1, double l2, double lc1, double lc2,
                    double m1, double m2, double I1, double I2, double g) {
        this.l1 = l1; this.l2 = l2;
        this.lc1 = lc1; this.lc2 = lc2;
        this.m1 = m1; this.m2 = m2;
        this.I1 = I1; this.I2 = I2;
        this.g = g;
    }

    public double[][] M(double[] q) {
        double q1 = q[0];
        double q2 = q[1];
        double c2 = Math.cos(q2);

        double m11 = I1 + I2
                     + m1 * lc1 * lc1
                     + m2 * (l1 * l1 + lc2 * lc2 + 2.0 * l1 * lc2 * c2);
        double m12 = I2 + m2 * (lc2 * lc2 + l1 * lc2 * c2);
        double m22 = I2 + m2 * lc2 * lc2;

        return new double[][] {
            {m11, m12},
            {m12, m22}
        };
    }

    public double[][] C(double[] q, double[] qd) {
        double q1 = q[0];
        double q2 = q[1];
        double q1d = qd[0];
        double q2d = qd[1];
        double s2 = Math.sin(q2);
        double h = -m2 * l1 * lc2 * s2;

        double c11 = h * q2d;
        double c12 = h * (q1d + q2d);
        double c21 = -h * q1d;
        double c22 = 0.0;

        return new double[][] {
            {c11, c12},
            {c21, c22}
        };
    }

    public double[] gVec(double[] q) {
        double q1 = q[0];
        double q2 = q[1];

        double g1 = ((m1 * lc1 + m2 * l1) * g * Math.cos(q1)
                     + m2 * lc2 * g * Math.cos(q1 + q2));
        double g2 = m2 * lc2 * g * Math.cos(q1 + q2);

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

    public double[] dynamics(double[] q, double[] qd, double[] u) {
        double[][] Mq = M(q);
        double[][] Cq = C(q, qd);
        double[] gq = gVec(q);

        double[] Cqd = new double[2];
        Cqd[0] = Cq[0][0] * qd[0] + Cq[0][1] * qd[1];
        Cqd[1] = Cq[1][0] * qd[0] + Cq[1][1] * qd[1];

        double[] rhs = new double[2];
        rhs[0] = u[0] - Cqd[0] - gq[0];
        rhs[1] = u[1] - Cqd[1] - gq[1];

        // Solve 2x2 linear system Mq * qdd = rhs analytically
        double det = Mq[0][0] * Mq[1][1] - Mq[0][1] * Mq[1][0];
        double inv00 =  Mq[1][1] / det;
        double inv01 = -Mq[0][1] / det;
        double inv10 = -Mq[1][0] / det;
        double inv11 =  Mq[0][0] / det;

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

Replacing the manual 2x2 solver with an EJML or Apache Commons Math linear solver yields a scalable implementation for higher-DOF manipulators while preserving the same matrix structure.

MATLAB provides native support for matrix operations and the Robotics System Toolbox, which stores the dynamics in a rigidBodyTree object. The standard function inverseDynamics internally uses the structured EoM: \( \mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) \).

function planar2R_demo()
    % Parameters
    l1  = 1.0;  l2  = 1.0;
    lc1 = 0.5;  lc2 = 0.5;
    m1  = 1.0;  m2  = 1.0;
    I1  = 0.1;  I2  = 0.1;
    g   = 9.81;

    % Example state and input
    q   = [pi/4; pi/6];
    qd  = [0.2; -0.1];
    qdd = [0.5; 0.1];

    [Mq, Cq, gq] = planar2R_dynamics_matrices(q, qd, ...
                                              l1, l2, lc1, lc2, ...
                                              m1, m2, I1, I2, g);
    u = Mq * qdd + Cq * qd + gq;

    disp('Inertia matrix M(q):');
    disp(Mq);
    disp('Coriolis/centrifugal matrix C(q,qd):');
    disp(Cq);
    disp('Gravity vector g(q):');
    disp(gq);
    disp('Required joint inputs u:');
    disp(u);
end

function [Mq, Cq, gq] = planar2R_dynamics_matrices(q, qd, ...
                                                   l1, l2, lc1, lc2, ...
                                                   m1, m2, I1, I2, g)
    q1  = q(1);  q2  = q(2);
    q1d = qd(1); q2d = qd(2);

    c2 = cos(q2);
    s2 = sin(q2);

    % Inertia matrix
    M11 = I1 + I2 + m1*lc1^2 + m2*(l1^2 + lc2^2 + 2*l1*lc2*c2);
    M12 = I2 + m2*(lc2^2 + l1*lc2*c2);
    M22 = I2 + m2*lc2^2;
    Mq  = [M11, M12;
           M12, M22];

    % Coriolis/centrifugal
    h   = -m2*l1*lc2*s2;
    C11 = h*q2d;
    C12 = h*(q1d + q2d);
    C21 = -h*q1d;
    C22 = 0;
    Cq  = [C11, C12;
           C21, C22];

    % Gravity
    g1  = ((m1*lc1 + m2*l1)*g*cos(q1) + m2*lc2*g*cos(q1+q2));
    g2  = m2*lc2*g*cos(q1+q2);
    gq  = [g1; g2];
end
      

In Simulink, one can encapsulate planar2R_dynamics_matrices in a MATLAB Function block to compute \( \mathbf{M},\mathbf{C},\mathbf{g} \) at each time step and implement \( \ddot{\mathbf{q}} = \mathbf{M}(\mathbf{q})^{-1} (\mathbf{u} - \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} - \mathbf{g}(\mathbf{q})) \) inside an ODE integrator structure.

Wolfram Mathematica is particularly suited to symbolic derivation of \( \mathbf{M}(\mathbf{q}), \mathbf{C}(\mathbf{q},\dot{\mathbf{q}}), \mathbf{g}(\mathbf{q}) \). The following code symbolically constructs the Lagrangian for the 2R manipulator and derives the matrix form:

(* Define symbols *)
ClearAll["Global`*"];
q1[t_]; q2[t_];
q1t = q1[t]; q2t = q2[t];
q1d = D[q1t, t]; q2d = D[q2t, t];

l1  = Symbol["l1"];  l2  = Symbol["l2"];
lc1 = Symbol["lc1"]; lc2 = Symbol["lc2"];
m1  = Symbol["m1"];  m2  = Symbol["m2"];
I1  = Symbol["I1"];  I2  = Symbol["I2"];
g   = Symbol["g"];

(* CoM positions (planar) *)
x1 = lc1*Cos[q1t];
y1 = lc1*Sin[q1t];

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

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

(* Kinetic energy: translational + rotational *)
K1 = 1/2*m1*(vx1^2 + vy1^2) + 1/2*I1*q1d^2;
K2 = 1/2*m2*(vx2^2 + vy2^2) + 1/2*I2*(q1d + q2d)^2;
K  = Simplify[K1 + K2];

(* Potential energy *)
P1 = m1*g*y1;
P2 = m2*g*y2;
P  = Simplify[P1 + P2];

L  = K - P;

(* Euler-Lagrange equations *)
eom1 = D[D[L, q1d], t] - D[L, q1t];
eom2 = D[D[L, q2d], t] - D[L, q2t];

(* Replace second derivatives with symbols q1dd, q2dd *)
q1dd = Symbol["q1dd"];
q2dd = Symbol["q2dd"];
subs = {D[q1t, {t, 2}] -> q1dd, D[q2t, {t, 2}] -> q2dd};

eom1a = Simplify[eom1 /. subs];
eom2a = Simplify[eom2 /. subs];

(* Collect terms in q1dd, q2dd to build M(q), C(q,qd), g(q) *)
varsAcc = {q1dd, q2dd};
M11 = Coefficient[eom1a, q1dd];
M12 = Coefficient[eom1a, q2dd];
M21 = Coefficient[eom2a, q1dd];
M22 = Coefficient[eom2a, q2dd];

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

Further algebraic manipulation (using Collect and Coefficient with respect to q1d and q2d) yields explicit \( \mathbf{C}(\mathbf{q},\dot{ \mathbf{q} }) \) and \( \mathbf{g}(\mathbf{q}) \) consistent with the formulas derived earlier.

In this lesson we examined the canonical structure of robot equations of motion derived from the Lagrangian formulation. The inertia matrix \( \mathbf{M}(\mathbf{q}) \) was shown to be symmetric and positive definite, with a physically meaningful decomposition in terms of link spatial inertias and Jacobians. The Coriolis and centrifugal terms were collected into \( \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} \), and we proved the crucial skew-symmetry property of \( \dot{\mathbf{M}} - 2\mathbf{C} \) using energy conservation.

We also expressed the gravity vector \( \mathbf{g}(\mathbf{q}) \) as the gradient of the potential energy and highlighted the global linearity of the dynamics in inertial parameters via the regressor form \( \mathbf{Y}(\mathbf{q},\dot{\mathbf{q}},\ddot{\mathbf{q}})\boldsymbol{\pi} \). A concrete 2R planar example illustrated these matrices explicitly, and we implemented the structured dynamics in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica.

These structural insights will be used in the next chapter when we derive explicit expressions for \( \mathbf{M},\mathbf{C},\mathbf{g} \) via Lagrange–Euler and analyze their geometric meaning in terms of Christoffel symbols and passivity.