For an \(n\)-DOF serial manipulator with generalized coordinates \( \mathbf{q} \in \mathbb{R}^n \), the joint-space equation of motion (EoM) derived in previous lessons has the standard form

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

Each term has a precise energetic meaning:

• Inertia matrix \( \mathbf{M}(\mathbf{q}) \) maps joint accelerations to generalized inertial torques. It encodes how the distribution of mass and inertia of all links, as seen from the joints, depends on configuration.
• Coriolis/centrifugal term \( \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} \) contains forces that depend on joint velocities. These terms redistribute kinetic energy among joints but do not create or destroy it in the absence of actuation and gravity.
• Gravity vector \( \mathbf{g}(\mathbf{q}) \) is the generalized force needed to balance the weight of the robot at configuration \( \mathbf{q} \).

From the Lagrangian viewpoint with kinetic energy \( T(\mathbf{q},\dot{\mathbf{q}}) \) and potential energy \( P(\mathbf{q}) \), one shows that

\[ \mathbf{M}(\mathbf{q}) = \frac{\partial^2 T}{\partial \dot{\mathbf{q}}\,\partial \dot{\mathbf{q}}^{\mathsf{T}}}, \quad \mathbf{g}(\mathbf{q}) = \frac{\partial P(\mathbf{q})}{\partial \mathbf{q}}, \]

while the Coriolis/centrifugal term is constructed so that the system obeys kinetic energy conservation when \( \boldsymbol{\tau} = \mathbf{0} \) and \( \mathbf{g}(\mathbf{q}) = \mathbf{0} \).

flowchart TD
  A["Link masses, inertias, geometry"] --> B["Velocities via Jacobians J(q)"]
  B --> C["Kinetic energy T(q, qdot)"]
  C --> D["Inertia matrix M(q)"]
  C --> E["Velocity coupling → C(q, qdot) qdot"]
  F["Potential energy P(q)"] --> G["Gravity vector g(q)"]
  D --> H["Joint-space dynamics"]
  E --> H
  G --> H
  H["M(q) qddot + C(q, qdot) qdot + g(q) = tau"]
        

Consider a manipulator with links indexed by \( k = 1,\dots,N \), each with mass \( m_k \), spatial inertia tensor \( \mathbf{I}_k \) about its center of mass (CoM), linear CoM velocity \( \mathbf{v}_k(\mathbf{q},\dot{\mathbf{q}}) \) and angular velocity \( \boldsymbol{\omega}_k(\mathbf{q},\dot{\mathbf{q}}) \). The total kinetic energy is

\[ T(\mathbf{q},\dot{\mathbf{q}}) \;=\; \frac{1}{2} \sum_{k=1}^N \left( m_k \,\mathbf{v}_k^{\mathsf{T}}\mathbf{v}_k \;+\; \boldsymbol{\omega}_k^{\mathsf{T}}\mathbf{I}_k \boldsymbol{\omega}_k \right). \]

From differential kinematics (previous chapters), the velocities depend linearly on joint velocities via Jacobians:

\[ \mathbf{v}_k(\mathbf{q},\dot{\mathbf{q}}) = \mathbf{J}_{v,k}(\mathbf{q})\dot{\mathbf{q}}, \quad \boldsymbol{\omega}_k(\mathbf{q},\dot{\mathbf{q}}) = \mathbf{J}_{\omega,k}(\mathbf{q})\dot{\mathbf{q}}. \]

Substituting,

\[ \begin{aligned} T(\mathbf{q},\dot{\mathbf{q}}) &= \frac{1}{2} \sum_{k=1}^N \left( m_k \dot{\mathbf{q}}^{\mathsf{T}}\mathbf{J}_{v,k}^{\mathsf{T}}\mathbf{J}_{v,k}\dot{\mathbf{q}} + \dot{\mathbf{q}}^{\mathsf{T}}\mathbf{J}_{\omega,k}^{\mathsf{T}}\mathbf{I}_k\mathbf{J}_{\omega,k}\dot{\mathbf{q}} \right) \\ &= \frac{1}{2} \dot{\mathbf{q}}^{\mathsf{T}}\mathbf{M}(\mathbf{q})\dot{\mathbf{q}}, \end{aligned} \]

with

\[ \mathbf{M}(\mathbf{q}) = \sum_{k=1}^N \left( m_k \mathbf{J}_{v,k}^{\mathsf{T}}(\mathbf{q})\mathbf{J}_{v,k}(\mathbf{q}) + \mathbf{J}_{\omega,k}^{\mathsf{T}}(\mathbf{q})\mathbf{I}_k\mathbf{J}_{\omega,k}(\mathbf{q}) \right). \]

2.1 Symmetry and Positive Definiteness

From the energy definition we can prove two central properties.

Symmetry. Component-wise, define

\[ M_{ij}(\mathbf{q}) \;=\; \frac{\partial^2 T(\mathbf{q},\dot{\mathbf{q}})}{\partial \dot{q}_i \,\partial \dot{q}_j}. \]

Since \( T \) is a smooth scalar function of \( \dot{\mathbf{q}} \), mixed partial derivatives commute:

\[ M_{ij}(\mathbf{q}) = \frac{\partial^2 T}{\partial \dot{q}_i \,\partial \dot{q}_j} = \frac{\partial^2 T}{\partial \dot{q}_j \,\partial \dot{q}_i} = M_{ji}(\mathbf{q}). \]

Hence \( \mathbf{M}(\mathbf{q}) \) is symmetric for all configurations.

Positive definiteness. For any nonzero joint-velocity vector \( \mathbf{x} \in \mathbb{R}^n \), define \( T(\mathbf{q},\mathbf{x}) = \frac{1}{2}\mathbf{x}^{\mathsf{T}}\mathbf{M}(\mathbf{q})\mathbf{x} \). This is the kinetic energy obtained if we set \( \dot{\mathbf{q}} = \mathbf{x} \). Physically,

\[ T(\mathbf{q},\mathbf{x}) = \frac{1}{2} \sum_{k=1}^N \left( m_k \|\mathbf{v}_k(\mathbf{q},\mathbf{x})\|^2 + \boldsymbol{\omega}_k^{\mathsf{T}}(\mathbf{q},\mathbf{x}) \mathbf{I}_k \boldsymbol{\omega}_k(\mathbf{q},\mathbf{x}) \right), \]

which is strictly positive whenever \( \mathbf{x} \neq \mathbf{0} \), provided all masses and inertia tensors are physically valid (nonnegative masses, positive definite inertia matrices about the CoMs). Therefore

\[ \mathbf{x}^{\mathsf{T}}\mathbf{M}(\mathbf{q})\mathbf{x} = 2T(\mathbf{q},\mathbf{x}) > 0 \quad \text{for all } \mathbf{x} \neq \mathbf{0}, \]

so \( \mathbf{M}(\mathbf{q}) \) is symmetric positive definite (SPD) for all physical configurations.

2.2 Physical Reading of Matrix Entries

• The diagonal element \( M_{ii}(\mathbf{q}) \) is the effective inertia about joint \( i \) when only that joint is moving, with all other joints locked.
• The off-diagonal element \( M_{ij}(\mathbf{q}) \) captures dynamic coupling between joints \( i \) and \( j \): accelerating joint \( j \) requires torque at joint \( i \) even if \( \ddot{q}_i = 0 \).
• The eigenvalues of \( \mathbf{M}(\mathbf{q}) \) are the principal inertias in joint space, and eigenvectors give preferred directions of motion where acceleration can be produced with minimum torque.

When a manipulator extends its arm, masses move farther from the base, and the entries of \( \mathbf{M}(\mathbf{q}) \) increase. This is the multi-DOF analogue of a figure skater extending arms to increase rotational inertia and slow down.

The term \( \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} \) collects components of the generalized forces that are quadratic in the joint velocities. They arise because the kinetic energy depends on both configuration and velocity: \( T = \tfrac{1}{2}\dot{\mathbf{q}}^{\mathsf{T}}\mathbf{M}(\mathbf{q})\dot{\mathbf{q}} \).

3.1 Energy-Based Characterization

Taking the time derivative of the kinetic energy,

\[ \begin{aligned} \dot{T} &= \frac{\mathrm{d}}{\mathrm{d}t} \left( \frac{1}{2}\dot{\mathbf{q}}^{\mathsf{T}}\mathbf{M}(\mathbf{q})\dot{\mathbf{q}} \right) \\ &= \dot{\mathbf{q}}^{\mathsf{T}}\mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \frac{1}{2}\dot{\mathbf{q}}^{\mathsf{T}}\dot{\mathbf{M}}(\mathbf{q})\dot{\mathbf{q}}, \end{aligned} \]

where \( \dot{\mathbf{M}}(\mathbf{q}) = \tfrac{\partial \mathbf{M}}{\partial \mathbf{q}}\dot{\mathbf{q}} \) is the directional derivative of the inertia matrix along the motion.

In the absence of actuation and gravity (\( \boldsymbol{\tau} = \mathbf{0} \), \( \mathbf{g}(\mathbf{q}) = \mathbf{0} \)), the EoM reduces to \( \mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} = \mathbf{0} \). Substituting \( \ddot{\mathbf{q}} = -\mathbf{M}^{-1}\mathbf{C}\dot{\mathbf{q}} \) into \( \dot{T} \) yields

\[ \dot{T} = -\dot{\mathbf{q}}^{\mathsf{T}}\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \frac{1}{2}\dot{\mathbf{q}}^{\mathsf{T}}\dot{\mathbf{M}}(\mathbf{q})\dot{\mathbf{q}} = \frac{1}{2}\dot{\mathbf{q}}^{\mathsf{T}} \big( \dot{\mathbf{M}}(\mathbf{q}) - 2\mathbf{C}(\mathbf{q},\dot{\mathbf{q}}) \big) \dot{\mathbf{q}}. \]

For a conservative system, free motion must conserve kinetic energy, so \( \dot{T} = 0 \) for all \( \dot{\mathbf{q}} \), which implies

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

This condition is satisfied if the matrix \( \dot{\mathbf{M}}(\mathbf{q}) - 2\mathbf{C}(\mathbf{q},\dot{\mathbf{q}}) \) is skew-symmetric, because \( \mathbf{x}^{\mathsf{T}}\mathbf{S}\mathbf{x} = 0 \) for all \( \mathbf{x} \) whenever \( \mathbf{S}^{\mathsf{T}} = -\mathbf{S} \).

Thus a physically meaningful choice of \( \mathbf{C}(\mathbf{q},\dot{\mathbf{q}}) \) is characterized by the property that \( \dot{\mathbf{M}} - 2\mathbf{C} \) is skew-symmetric, ensuring that Coriolis and centrifugal terms do not inject or dissipate mechanical energy; they only redistribute it.

3.2 Coriolis vs. Centrifugal Terms

• Centrifugal terms occur even for a single rotating joint; they are proportional to \( \dot{q}_i^2 \) and represent the tendency of mass to move radially outward.
• Coriolis terms couple distinct joints; in a 2-DOF arm they are typically proportional to \( \dot{q}_i\dot{q}_j \), transferring energy between joint motions as the arm changes shape.

In later chapters (Lagrange–Euler with Christoffel symbols) we will see that the elements of \( \mathbf{C}(\mathbf{q},\dot{\mathbf{q}}) \) can be expressed in terms of derivatives of \( \mathbf{M}(\mathbf{q}) \), making these geometric connections explicit.

Recall that the potential energy of the manipulator due to gravity is

\[ P(\mathbf{q}) = \sum_{k=1}^N m_k g_0 h_k(\mathbf{q}), \]

where \( g_0 \) is the magnitude of gravitational acceleration and \( h_k(\mathbf{q}) \) is the height of link \( k \)'s CoM in the world frame.

By the Lagrange equations, the generalized force associated with gravity is

\[ g_i(\mathbf{q}) = \frac{\partial P(\mathbf{q})}{\partial q_i}, \quad \mathbf{g}(\mathbf{q}) = \begin{bmatrix} g_1(\mathbf{q}) \\ \vdots \\ g_n(\mathbf{q}) \end{bmatrix}. \]

Physically, \( g_i(\mathbf{q}) \) is the torque that joint \( i \) must generate to balance gravity when the arm is held quasi-statically at configuration \( \mathbf{q} \):

\[ \dot{\mathbf{q}} = \ddot{\mathbf{q}} = \mathbf{0} \quad\Rightarrow\quad \boldsymbol{\tau} = \mathbf{g}(\mathbf{q}). \]

As the arm moves into a posture where more mass is lifted against gravity (for example, an extended horizontal configuration), the entries of \( \mathbf{g}(\mathbf{q}) \) increase in magnitude, reflecting the larger lever arm of the weight relative to each joint axis.

Consider a planar two-link revolute manipulator operating in the horizontal plane, with joint angles \( q_1 \), \( q_2 \). Let \( l_1 \) be the length of link 1, and \( l_{c1} \), \( l_{c2} \) the CoM distances of links from their proximal joint axes. Let \( m_1, m_2 \) be link masses and \( I_1, I_2 \) the planar moments of inertia about each CoM.

5.1 Inertia Matrix

A standard derivation (using planar Jacobians) yields

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

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

Interpretation:

• When \( q_2 = 0 \), the arm is fully stretched and the term \( 2 m_2 l_1 l_{c2}\cos q_2 \) maximizes the inertia seen at joint 1.
• When \( q_2 = \pi \), link 2 folds back along link 1, reducing the effective inertia about joint 1.
• \( M_{12} = M_{21} \) captures how accelerating joint 2 produces torque at joint 1 and vice versa; the magnitude depends on \( \cos q_2 \).

5.2 Coriolis/Centrifugal Term

One convenient representation of the Coriolis/centrifugal term for this arm is

\[ \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} = \begin{bmatrix} -m_2 l_1 l_{c2} \sin q_2 \big(2\dot{q}_1\dot{q}_2 + \dot{q}_2^2 \big) \\ m_2 l_1 l_{c2} \sin q_2 \,\dot{q}_1^2 \end{bmatrix}. \]

The structure is typical:

• Terms in \( \dot{q}_1^2 \) and \( \dot{q}_2^2 \) are centrifugal.
• Mixed terms in \( \dot{q}_1\dot{q}_2 \) are Coriolis, transferring energy between joints as the configuration changes.
• The factor \( \sin q_2 \) reflects that these forces vanish when the links are colinear (\( q_2 = 0 \) or \( q_2 = \pi \)).

5.3 Gravity Vector

If the arm operates in a vertical plane with gravity magnitude \( g_0 \), a common expression for the gravity torques is

\[ \mathbf{g}(\mathbf{q}) = \begin{bmatrix} (m_1 l_{c1} + m_2 l_1) g_0 \cos q_1 + m_2 l_{c2} g_0 \cos(q_1 + q_2) \\ m_2 l_{c2} g_0 \cos(q_1 + q_2) \end{bmatrix}. \]

When the arm is held horizontally (\( q_1 \approx \tfrac{\pi}{2}, q_2 \approx 0 \)), both components are large in magnitude, reflecting the need for substantial torque to support the links against gravity. When the arm is hanging straight down (\( q_1 \approx 0, q_2 \approx 0 \)), the required torques are much smaller.

We now implement the 2R dynamics \( \mathbf{M}(\mathbf{q}) \), \( \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} \) and \( \mathbf{g}(\mathbf{q}) \) in several languages. In practice, robotics libraries (e.g., roboticstoolbox in Python, Eigen-based code generators in C++, or symbolic tools) automate these expressions, but understanding the underlying formulas is crucial.

flowchart TD
  S["Define link parameters (m, I, lengths)"] --> Mq["Implement M(q) from analytic formulas"]
  Mq --> Cv["Implement C(q, qdot) qdot"]
  Cv --> Gq["Implement g(q) from potential P(q)"]
  Gq --> NUM["Use in numerical simulation or control"]
  NUM --> LANG["Wrap in Python / C++ / Java / MATLAB / Mathematica APIs"]
        

6.1 Python (NumPy)

import numpy as np

def planar_2r_dynamics(q, qdot, params):
    """
    Compute M(q), C(q, qdot) qdot, g(q) for a planar 2R arm.

    q      : array-like length 2 [q1, q2]
    qdot   : array-like length 2 [dq1, dq2]
    params : dict with keys
             m1, m2, l1, lc1, lc2, I1, I2, g0
    """
    q1, q2 = q
    dq1, dq2 = qdot
    m1 = params["m1"]
    m2 = params["m2"]
    l1 = params["l1"]
    lc1 = params["lc1"]
    lc2 = params["lc2"]
    I1 = params["I1"]
    I2 = params["I2"]
    g0 = params["g0"]

    c2 = np.cos(q2)
    s2 = np.sin(q2)

    # Inertia matrix M(q)
    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
    M = np.array([[M11, M12],
                  [M12, M22]])

    # Coriolis/centrifugal term C(q, qdot) qdot (vector)
    h1 = -m2 * l1 * lc2 * s2 * (2 * dq1 * dq2 + dq2**2)
    h2 =  m2 * l1 * lc2 * s2 * dq1**2
    Cqdot = np.array([h1, h2])

    # Gravity vector g(q)
    g1 = (m1 * lc1 + m2 * l1) * g0 * np.cos(q1) + m2 * lc2 * g0 * np.cos(q1 + q2)
    g2 = m2 * lc2 * g0 * np.cos(q1 + q2)
    g = np.array([g1, g2])

    return M, Cqdot, g

# Example usage:
if __name__ == "__main__":
    params = dict(m1=1.0, m2=1.0, l1=1.0, lc1=0.5, lc2=0.5,
                  I1=0.1, I2=0.1, g0=9.81)
    q = np.array([0.5, 0.3])
    qdot = np.array([0.1, -0.2])
    M, Cqdot, g = planar_2r_dynamics(q, qdot, params)
    print("M(q) =\n", M)
    print("C(q, qdot) qdot =", Cqdot)
    print("g(q) =", g)
      

6.2 C++ (Eigen)

#include <Eigen/Dense>
#include <cmath>

struct Planar2RParams {
    double m1, m2;
    double l1, lc1, lc2;
    double I1, I2;
    double g0;
};

void planar2R_dynamics(const Eigen::Vector2d& q,
                       const Eigen::Vector2d& qdot,
                       const Planar2RParams& p,
                       Eigen::Matrix2d& M,
                       Eigen::Vector2d& Cqdot,
                       Eigen::Vector2d& g)
{
    const double q1  = q(0);
    const double q2  = q(1);
    const double dq1 = qdot(0);
    const double dq2 = qdot(1);

    const double c2 = std::cos(q2);
    const double s2 = std::sin(q2);

    const double m1  = p.m1;
    const double m2  = p.m2;
    const double l1  = p.l1;
    const double lc1 = p.lc1;
    const double lc2 = p.lc2;
    const double I1  = p.I1;
    const double I2  = p.I2;
    const double g0  = p.g0;

    // Inertia matrix M(q)
    const double M11 = I1 + I2 + m1 * lc1 * lc1
                       + m2 * (l1 * l1 + lc2 * lc2 + 2.0 * l1 * lc2 * c2);
    const double M12 = I2 + m2 * (lc2 * lc2 + l1 * lc2 * c2);
    const double M22 = I2 + m2 * lc2 * lc2;

    M(0,0) = M11;
    M(0,1) = M12;
    M(1,0) = M12;
    M(1,1) = M22;

    // Coriolis/centrifugal term C(q, qdot) qdot
    const double h1 = -m2 * l1 * lc2 * s2 * (2.0 * dq1 * dq2 + dq2 * dq2);
    const double h2 =  m2 * l1 * lc2 * s2 * dq1 * dq1;

    Cqdot(0) = h1;
    Cqdot(1) = h2;

    // Gravity term g(q)
    const double g1 = (m1 * lc1 + m2 * l1) * g0 * std::cos(q1)
                      + m2 * lc2 * g0 * std::cos(q1 + q2);
    const double g2 = m2 * lc2 * g0 * std::cos(q1 + q2);

    g(0) = g1;
    g(1) = g2;
}
      

6.3 Java (Array-Based Implementation)

public class Planar2RDynamics {

    public static class Params {
        public double m1, m2;
        public double l1, lc1, lc2;
        public double I1, I2;
        public double g0;
    }

    /**
     * Compute M(q), C(q, qdot) qdot, g(q) for a planar 2R arm.
     *
     * q      : length-2 array {q1, q2}
     * qdot   : length-2 array {dq1, dq2}
     * params : link parameters
     *
     * Returns an object with fields M (2x2), Cqdot (2), g (2).
     */
    public static class Result {
        public double[][] M = new double[2][2];
        public double[] Cqdot = new double[2];
        public double[] g = new double[2];
    }

    public static Result compute(double[] q, double[] qdot, Params p) {
        Result r = new Result();

        double q1 = q[0];
        double q2 = q[1];
        double dq1 = qdot[0];
        double dq2 = qdot[1];

        double c2 = Math.cos(q2);
        double s2 = Math.sin(q2);

        double m1  = p.m1;
        double m2  = p.m2;
        double l1  = p.l1;
        double lc1 = p.lc1;
        double lc2 = p.lc2;
        double I1  = p.I1;
        double I2  = p.I2;
        double g0  = p.g0;

        // Inertia matrix M(q)
        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;

        r.M[0][0] = M11;
        r.M[0][1] = M12;
        r.M[1][0] = M12;
        r.M[1][1] = M22;

        // Coriolis/centrifugal vector
        double h1 = -m2 * l1 * lc2 * s2 * (2.0 * dq1 * dq2 + dq2 * dq2);
        double h2 =  m2 * l1 * lc2 * s2 * dq1 * dq1;

        r.Cqdot[0] = h1;
        r.Cqdot[1] = h2;

        // Gravity vector
        double g1 = (m1 * lc1 + m2 * l1) * g0 * Math.cos(q1)
                    + m2 * lc2 * g0 * Math.cos(q1 + q2);
        double g2 = m2 * lc2 * g0 * Math.cos(q1 + q2);

        r.g[0] = g1;
        r.g[1] = g2;

        return r;
    }
}
      

6.4 MATLAB / Simulink

function [M, Cqdot, g] = planar2R_dynamics(q, qdot, params)
%PLANAR2R_DYNAMICS Compute M(q), C(q, qdot) qdot, g(q) for planar 2R arm.
%   q      : [q1; q2]
%   qdot   : [dq1; dq2]
%   params : struct with fields m1, m2, l1, lc1, lc2, I1, I2, g0

q1  = q(1);
q2  = q(2);
dq1 = qdot(1);
dq2 = qdot(2);

m1  = params.m1;
m2  = params.m2;
l1  = params.l1;
lc1 = params.lc1;
lc2 = params.lc2;
I1  = params.I1;
I2  = params.I2;
g0  = params.g0;

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;

M = [M11, M12;
     M12, M22];

% Coriolis/centrifugal vector
h1 = -m2*l1*lc2*s2*(2*dq1*dq2 + dq2^2);
h2 =  m2*l1*lc2*s2*dq1^2;

Cqdot = [h1; h2];

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

In Simulink, you can call planar2R_dynamics from a MATLAB Function block. The inputs are the current joint states and parameters; the outputs M, Cqdot and g can be combined to compute the joint torques \( \boldsymbol{\tau} = \mathbf{M}\ddot{\mathbf{q}} + \mathbf{C}\dot{\mathbf{q}} + \mathbf{g} \) inside the block.

6.5 Wolfram Mathematica (Symbolic Derivation)

(* Parameters *)
Clear[m1, m2, l1, lc1, lc2, I1, I2, g0];
Clear[q1, q2, dq1, dq2];

q  = {q1, q2};
dq = {dq1, dq2};

(* Kinetic and potential energies for planar 2R (conceptual form) *)
(* For brevity, we use known closed-form T and P from the analytic derivation. *)
T = 1/2 (
      (I1 + I2 + m1*lc1^2 + m2*(l1^2 + lc2^2 + 2*l1*lc2*Cos[q2]))*dq1^2
    + (I2 + m2*lc2^2)*dq2^2
    + 2*(I2 + m2*(lc2^2 + l1*lc2*Cos[q2]))*dq1*dq2
    );

P = (m1*lc1 + m2*l1)*g0*Cos[q1] + m2*lc2*g0*Cos[q1 + q2];

(* Inertia matrix M(q) = d^2 T / (d dq d dq^T) *)
M = Table[D[D[T, dq[[i]]], dq[[j]]], {i, 1, 2}, {j, 1, 2}] // Simplify;

(* Gravity vector g(q) = dP/dq *)
gvec = Table[D[P, q[[i]]], {i, 1, 2}] // Simplify;

(* Christoffel symbols of first kind c_ijk, then C(q, dq) dq *)
c = Table[
      1/2 (D[M[[i, j]], q[[k]]] + D[M[[i, k]], q[[j]]] - D[M[[j, k]], q[[i]]]),
      {i, 1, 2}, {j, 1, 2}, {k, 1, 2}
    ];

Cvec = Table[
      Sum[c[[i, j, k]]*dq[[j]]*dq[[k]], {j, 1, 2}, {k, 1, 2}],
      {i, 1, 2}
    ] // Simplify;
      

The symbolic expressions M, gvec and Cvec match the analytic formulas given earlier, and can be exported or simplified automatically for use in other languages.

• The inertia matrix \( \mathbf{M}(\mathbf{q}) \) is defined by the kinetic energy \( T = \tfrac{1}{2}\dot{\mathbf{q}}^{\mathsf{T}}\mathbf{M}(\mathbf{q})\dot{\mathbf{q}} \), and is symmetric positive definite for physically realizable robots.
• Coriolis and centrifugal terms \( \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} \) represent velocity-dependent forces that conserve kinetic energy in the absence of actuation and gravity, characterized by a skew-symmetry condition on \( \dot{\mathbf{M}} - 2\mathbf{C} \).
• The gravity vector \( \mathbf{g}(\mathbf{q}) = \partial P(\mathbf{q})/\partial \mathbf{q} \) is the generalized force needed to support the robot against gravity at a fixed configuration.
• In a planar 2R arm, explicit expressions for \( \mathbf{M}, \mathbf{C}, \mathbf{g} \) reveal configuration-dependent inertia and coupling between joints, and can be implemented consistently across Python, C++, Java, MATLAB/Simulink, and Mathematica.

These interpretations will be crucial when we study Lagrange–Euler derivations, passivity, and energy-based control designs in the next chapters.