Consider a serial robot manipulator with \( n \) generalized coordinates \( \mathbf{q} \in \mathbb{R}^n \) and joint velocities \( \dot{\mathbf{q}} \in \mathbb{R}^n \). The total mechanical energy is

\[ E(q,\dot{q}) \;=\; T(q,\dot{q}) + V(q) \]

where \( T \) is the kinetic energy and \( V \) is the potential energy. For a multi-body robot under standard rigid-body assumptions, the kinetic energy can always be written in joint space as

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

with \( \mathbf{M}(q) \in \mathbb{R}^{n \times n} \) the joint-space inertia matrix. The potential energy typically consists of gravitational and elastic (e.g. joint spring) contributions:

\[ V(q) = V_{\text{grav}}(q) + V_{\text{elastic}}(q). \]

In later lessons, the Lagrangian will be defined as \( L(q,\dot{q}) = T(q,\dot{q}) - V(q) \), from which the robot equations of motion arise. In this lesson we focus exclusively on how to compute \( T \) and \( V \) for multi-body systems.

flowchart TD
  Q["Joint state (q, qdot)"] --> FK["Forward kinematics for each link frame"]
  FK --> JAC["Compute link Jacobians J_i(q)"]
  JAC --> VEL["Link velocities (v_ci, omega_i)"]
  VEL --> EN1["Per-link kinetic energies T_i"]
  Q --> H["CoM positions p_ci(q) and joint deflections"]
  H --> EN2["Per-link potentials V_i (gravity, springs)"]
  EN1 --> SUMT["Total T = sum T_i"]
  EN2 --> SUMV["Total V = sum V_i"]
  SUMT --> LAG["Define L(q,qdot) = T - V (later)"]
  SUMV --> LAG
        

Let a rigid body have mass \( m \), center of mass (CoM) position \( \mathbf{p}_C \in \mathbb{R}^3 \) and orientation \( \mathbf{R} \in \mathrm{SO}(3) \) with respect to an inertial frame. The linear velocity of the CoM is \( \mathbf{v}_C \) and the angular velocity is \( \boldsymbol{\omega} \), both expressed in some frame (e.g. base frame).

The kinetic energy is the sum of translational and rotational parts:

\[ T \;=\; T_{\text{trans}} + T_{\text{rot}} \;=\; \tfrac{1}{2} m \,\Vert \mathbf{v}_C \Vert^2 \;+\; \tfrac{1}{2} \boldsymbol{\omega}^\top \mathbf{I}_C\,\boldsymbol{\omega}, \]

where \( \mathbf{I}_C \in \mathbb{R}^{3 \times 3} \) is the inertia tensor about the CoM, expressed in the same frame as \( \boldsymbol{\omega} \). The matrix \( \mathbf{I}_C \) is symmetric positive definite for any real rigid body.

If the inertia tensor relative to another point \( O \) with position \( \mathbf{p}_O \) is needed, one uses the parallel-axis theorem. Let \( \mathbf{r}_{CO} = \mathbf{p}_C - \mathbf{p}_O \) and \( [\mathbf{r}_{CO}]_\times \) denote the skew-symmetric cross-product matrix. Then

\[ \mathbf{I}_O \;=\; \mathbf{I}_C + m\big( \Vert \mathbf{r}_{CO} \Vert^2 \mathbf{I}_3 - \mathbf{r}_{CO} \mathbf{r}_{CO}^\top \big) \;=\; \mathbf{I}_C - m[\mathbf{r}_{CO}]_\times^2. \]

This provides the foundation for assembling the kinetic energy of a multi-body robot from per-link inertial parameters (mass, CoM, inertia tensor).

Consider a serial manipulator with \( n \) joints and \( N \) rigid links (often \( N = n \) for simple chains). For link \( i \) we denote:

• \( m_i \): mass,
• \( \mathbf{p}_{C_i}(q) \): CoM position in the base frame,
• \( \mathbf{v}_{C_i}(q,\dot{q}) \): CoM linear velocity,
• \( \boldsymbol{\omega}_i(q,\dot{q}) \): angular velocity,
• \( \mathbf{I}_{C_i} \): inertia tensor about CoM in base coordinates.

The kinetic energy of link \( i \) is

\[ T_i(q,\dot{q}) = \tfrac{1}{2} m_i \Vert \mathbf{v}_{C_i} \Vert^2 + \tfrac{1}{2} \boldsymbol{\omega}_i^\top \mathbf{I}_{C_i} \boldsymbol{\omega}_i. \]

Using differential kinematics from earlier chapters, the velocities can be expressed via Jacobians:

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

where \( \mathbf{J}_{v,i}, \mathbf{J}_{\omega,i} \in \mathbb{R}^{3 \times n} \) are, respectively, the linear and angular velocity Jacobians of the link \( i \) CoM. Substituting into \( T_i \) gives

\[ \begin{aligned} T_i(q,\dot{q}) &= \tfrac{1}{2} \dot{\mathbf{q}}^\top \mathbf{J}_{v,i}(q)^\top m_i \mathbf{I}_3 \mathbf{J}_{v,i}(q)\,\dot{\mathbf{q}} + \tfrac{1}{2} \dot{\mathbf{q}}^\top \mathbf{J}_{\omega,i}(q)^\top \mathbf{I}_{C_i} \mathbf{J}_{\omega,i}(q)\,\dot{\mathbf{q}} \\ &= \tfrac{1}{2} \dot{\mathbf{q}}^\top \mathbf{M}_i(q)\,\dot{\mathbf{q}}, \end{aligned} \]

where the per-link inertia contribution is

\[ \mathbf{M}_i(q) = \mathbf{J}_{v,i}(q)^\top m_i \mathbf{I}_3 \mathbf{J}_{v,i}(q) + \mathbf{J}_{\omega,i}(q)^\top \mathbf{I}_{C_i} \mathbf{J}_{\omega,i}(q). \]

Summing over all links,

\[ T(q,\dot{q}) = \sum_{i=1}^N T_i(q,\dot{q}) = \tfrac{1}{2} \dot{\mathbf{q}}^\top \mathbf{M}(q)\,\dot{\mathbf{q}}, \qquad \mathbf{M}(q) = \sum_{i=1}^N \mathbf{M}_i(q). \]

Each term \( \mathbf{M}_i(q) = \mathbf{J}_i^\top \mathbf{H}_i \mathbf{J}_i \) is symmetric because \( \mathbf{H}_i \) is symmetric (\( m_i \mathbf{I}_3 \) and \( \mathbf{I}_{C_i} \) are symmetric). Therefore \( \mathbf{M}(q) \) is symmetric. We will see in Section 6 that it is also positive definite.

Potential energy encodes how configuration \( q \) stores energy due to external fields (e.g. gravity) and internal elements (e.g. springs). For a robot in a uniform gravitational field with \( z \)-axis pointing upward, the gravitational potential energy is

\[ V_{\text{grav}}(q) = \sum_{i=1}^N m_i g\, h_i(q) = \sum_{i=1}^N m_i g\, e_3^\top \mathbf{p}_{C_i}(q), \]

where \( g > 0 \) is the gravitational acceleration magnitude, \( h_i(q) \) is the height of the CoM of link \( i \), and \( e_3 = [0\;\;0\;\;1]^\top \).

If each joint \( j \) has a linear torsional spring with stiffness \( k_j \) and rest angle \( q_{0,j} \), the elastic energy is

\[ V_{\text{elastic}}(q) = \tfrac{1}{2} \sum_{j=1}^n k_j \big(q_j - q_{0,j}\big)^2 = \tfrac{1}{2}(\mathbf{q}-\mathbf{q}_0)^\top \mathbf{K} (\mathbf{q}-\mathbf{q}_0), \]

where \( \mathbf{K} = \mathrm{diag}(k_1,\dots,k_n) \). Additional potentials (e.g. from elastic couplings or gravity in different directions) can be added by superposition, as long as they can be expressed as scalar functions of \( q \).

flowchart TD
  Q["Joint configuration q"] --> G1["Compute CoM heights h_i(q)"]
  Q --> S1["Compute joint deflections q - q0"]
  G1 --> VG["Gravitational energy: \nV_grav = sum m_i * g * h_i"]
  S1 --> VS["Elastic energy: \nV_elastic = 0.5 * (q - q0)^T K (q - q0)"]
  VG --> VT["Total potential V(q) = V_grav + V_elastic + ..."]
  VS --> VT
        

Consider a planar 2-link arm in the vertical plane with revolute joints. Let \( q_1 \) and \( q_2 \) be the joint angles (measured from the horizontal), and define:

• \( l_1, l_2 \): link lengths,
• \( c_1, c_2 \): CoM distances from the proximal joint along each link,
• \( m_1, m_2 \): link masses,
• \( I_1, I_2 \): link planar moments of inertia (about CoM, around the out-of-plane axis).

5.1 Kinetic Energy

The CoM positions in the base frame (origin at joint 1) are

\[ \begin{aligned} \mathbf{p}_{C_1}(q) &= \begin{bmatrix} c_1 \cos q_1 \\ c_1 \sin q_1 \end{bmatrix}, \\ \mathbf{p}_{C_2}(q) &= \begin{bmatrix} l_1 \cos q_1 + c_2 \cos(q_1 + q_2) \\ l_1 \sin q_1 + c_2 \sin(q_1 + q_2) \end{bmatrix}. \end{aligned} \]

Differentiating with respect to time gives linear velocities \( \mathbf{v}_{C_1}, \mathbf{v}_{C_2} \), and the angular velocities are \( \omega_1 = \dot{q}_1 \), \( \omega_2 = \dot{q}_1 + \dot{q}_2 \) about the out-of-plane axis. After algebraic simplification (grouping terms in \( \dot{q}_1, \dot{q}_2 \)), the total kinetic energy can be written as

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

with inertia matrix

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

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

As expected, \( \mathbf{M}(q) \) is symmetric with \( M_{12} = M_{21} \). Note that \( \cos q_2 \) couples the two links, reflecting that moving one joint influences the kinetic energy stored in both links.

5.2 Potential Energy

With gravity acting downward and the \( y \)-axis chosen upward, the heights of the CoMs are

\[ \begin{aligned} h_1(q) &= c_1 \sin q_1, \\ h_2(q) &= l_1 \sin q_1 + c_2 \sin(q_1 + q_2). \end{aligned} \]

Thus the gravitational potential energy is

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

These explicit expressions for \( T(q,\dot{q}) \) and \( V(q) \) will later yield the full dynamics of the 2-DOF arm when combined into a Lagrangian. Here they serve as a concrete illustration of multi-body energy computation.

The inertia matrix \( \mathbf{M}(q) \) of a rigid-body robot possesses important structural properties that will be used throughout robot dynamics and control:

6.1 Symmetry

As seen in Section 3, the multi-body kinetic energy can be written as

\[ \mathbf{M}(q) = \sum_{i=1}^N \mathbf{J}_{v,i}(q)^\top m_i \mathbf{I}_3 \mathbf{J}_{v,i}(q) + \mathbf{J}_{\omega,i}(q)^\top \mathbf{I}_{C_i} \mathbf{J}_{\omega,i}(q). \]

Each term has the form \( \mathbf{J}^\top \mathbf{H} \mathbf{J} \) with \( \mathbf{H} \) symmetric. Therefore \( \mathbf{J}^\top \mathbf{H} \mathbf{J} \) is symmetric, and the sum of symmetric matrices is symmetric, hence \( \mathbf{M}(q) = \mathbf{M}(q)^\top \).

6.2 Positive Definiteness

For any nonzero joint velocity \( \dot{\mathbf{q}} \neq \mathbf{0} \),

\[ \begin{aligned} \dot{\mathbf{q}}^\top \mathbf{M}(q) \dot{\mathbf{q}} &= 2 T(q,\dot{\mathbf{q}}) \\ &= \sum_{i=1}^N \big( m_i \Vert \mathbf{v}_{C_i} \Vert^2 + \boldsymbol{\omega}_i^\top \mathbf{I}_{C_i} \boldsymbol{\omega}_i \big). \end{aligned} \]

Each term is nonnegative and equals zero only if \( \mathbf{v}_{C_i} = \mathbf{0} \) and \( \boldsymbol{\omega}_i = \mathbf{0} \). For a connected manipulator with independent joints, the only way all body velocities vanish is \( \dot{\mathbf{q}} = \mathbf{0} \). Hence, for any nonzero \( \dot{\mathbf{q}} \), \( \dot{\mathbf{q}}^\top \mathbf{M}(q)\dot{\mathbf{q}} > 0 \), so \( \mathbf{M}(q) \) is symmetric positive definite (SPD).

6.3 Coordinate Invariance

If we change the inertial frame by a rigid transformation, the physical velocities and inertial parameters transform so that the scalar kinetic energy \( T \) remains unchanged. This implies that \( \mathbf{M}(q) \) transforms covariantly under coordinate changes, preserving properties such as symmetry and positive definiteness.

We now implement the expressions from Section 5 in Python using numpy. For more complex robots, a robotics library such as roboticstoolbox-python can be used to obtain \( \mathbf{M}(q) \) and \( V(q) \) automatically from a kinematic model.

import numpy as np
from dataclasses import dataclass

@dataclass
class TwoLinkParams:
    m1: float
    m2: float
    l1: float
    l2: float
    c1: float
    c2: float
    I1: float
    I2: float
    g: float = 9.81  # gravitational acceleration

def inertia_matrix(q, p: TwoLinkParams):
    """
    Joint-space inertia matrix M(q) for a planar 2R arm.
    q: array-like of shape (2,)
    """
    q1, q2 = q
    cos2 = np.cos(q2)

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

    M = np.array([[M11, M12],
                  [M12, M22]], dtype=float)
    return M

def potential_energy(q, p: TwoLinkParams):
    """
    Gravitational potential V(q) for a planar 2R arm in a vertical plane.
    """
    q1, q2 = q
    V = (p.m1 * p.g * p.c1 * np.sin(q1)
         + p.m2 * p.g * (p.l1 * np.sin(q1) + p.c2 * np.sin(q1 + q2)))
    return V

def kinetic_energy(q, qd, p: TwoLinkParams):
    """
    Kinetic energy T(q, qdot) = 0.5 * qdot^T M(q) qdot
    """
    q = np.asarray(q, dtype=float).reshape(2)
    qd = np.asarray(qd, dtype=float).reshape(2)
    M = inertia_matrix(q, p)
    return 0.5 * float(qd.T @ M @ qd)

def total_energy(q, qd, p: TwoLinkParams):
    return kinetic_energy(q, qd, p) + potential_energy(q, p)

if __name__ == "__main__":
    # Example parameters (roughly human-arm sized)
    params = TwoLinkParams(
        m1=2.0, m2=1.5,
        l1=0.4, l2=0.3,
        c1=0.2, c2=0.15,
        I1=0.02, I2=0.01
    )
    q = np.array([0.5, -0.3])
    qd = np.array([0.4, 0.2])

    T = kinetic_energy(q, qd, params)
    V = potential_energy(q, params)
    E = T + V
    print("T =", T, "V =", V, "E =", E)
      

Using a robotics library. For a more general manipulator description, we can rely on roboticstoolbox-python to construct a model and query its inertia matrix:

# Requires: pip install roboticstoolbox-python spatialmath
import numpy as np
import roboticstoolbox as rtb

# Planar 2R model
arm = rtb.models.DH.Planar2()

q = np.array([0.5, -0.3])
qd = np.array([0.4, 0.2])

M = arm.inertia(q)        # inertia matrix M(q)
g_vec = arm.gravload(q)   # gravity torques g(q) (related to V(q))

T = 0.5 * float(qd.T @ M @ qd)

print("M(q) =\n", M)
print("Kinetic energy T =", T)
      

In C++, Eigen is a common linear algebra library for robotics. For full multi-body dynamics, a dedicated library such as RBDL (Rigid Body Dynamics Library) or Pinocchio is typically used. Below we show a lightweight implementation of the 2-link energies using Eigen.

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

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

Eigen::Matrix2d inertiaMatrix(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;
}

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

    double V = p.m1 * p.g * p.c1 * std::sin(q1)
             + p.m2 * p.g * (p.l1 * std::sin(q1)
                             + p.c2 * std::sin(q1 + q2));
    return V;
}

double kineticEnergy(const Eigen::Vector2d& q,
                     const Eigen::Vector2d& qdot,
                     const TwoLinkParams& p)
{
    Eigen::Matrix2d M = inertiaMatrix(q, p);
    return 0.5 * qdot.transpose() * M * qdot;
}

int main()
{
    TwoLinkParams p{2.0, 1.5, 0.4, 0.3, 0.2, 0.15, 0.02, 0.01, 9.81};

    Eigen::Vector2d q(0.5, -0.3);
    Eigen::Vector2d qdot(0.4, 0.2);

    double T = kineticEnergy(q, qdot, p);
    double V = potentialEnergy(q, p);
    double E = T + V;

    std::cout << "T = " << T
              << ", V = " << V
              << ", E = " << E << std::endl;

    return 0;
}
      

Remark. Libraries such as RBDL allow one to build a model from Denavit–Hartenberg parameters or URDF and then query M(q), potential-related quantities, and full inverse dynamics.

Java does not have a de facto standard robotics library, but linear algebra packages such as EJML or Apache Commons Math are often used. Below we implement the 2-link energies using plain arrays; in practice, you would wrap these in a matrix class.

public class TwoLinkEnergy {

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

    public static double[][] inertiaMatrix(double[] q, Params p) {
        double q1 = q[0];
        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;

        double[][] M = new double[2][2];
        M[0][0] = M11; M[0][1] = M12;
        M[1][0] = M12; M[1][1] = M22;
        return M;
    }

    public static double potentialEnergy(double[] q, Params p) {
        double q1 = q[0];
        double q2 = q[1];
        return p.m1 * p.g * p.c1 * Math.sin(q1)
             + p.m2 * p.g * (p.l1 * Math.sin(q1)
                             + p.c2 * Math.sin(q1 + q2));
    }

    public static double kineticEnergy(double[] q, double[] qdot, Params p) {
        double[][] M = inertiaMatrix(q, p);
        // qdot^T M qdot for 2x2
        double v0 = qdot[0];
        double v1 = qdot[1];
        double T = 0.5 * (v0 * (M[0][0] * v0 + M[0][1] * v1)
                        + v1 * (M[1][0] * v0 + M[1][1] * v1));
        return T;
    }

    public static void main(String[] args) {
        Params p = new Params();
        p.m1 = 2.0; p.m2 = 1.5;
        p.l1 = 0.4; p.l2 = 0.3;
        p.c1 = 0.2; p.c2 = 0.15;
        p.I1 = 0.02; p.I2 = 0.01;

        double[] q = {0.5, -0.3};
        double[] qdot = {0.4, 0.2};

        double T = kineticEnergy(q, qdot, p);
        double V = potentialEnergy(q, p);
        double E = T + V;

        System.out.println("T = " + T + ", V = " + V + ", E = " + E);
    }
}
      

Integrating this with a Java-based robotics stack (for example, a simulation environment using EJML for matrices) allows one to compute energies and later to verify dynamics implementations.

10.1 MATLAB Function (Suitable for Simulink)

MATLAB, together with the Robotics System Toolbox or Simscape Multibody, is widely used for robot dynamics. Here we implement \( \mathbf{M}(q) \), \( T \), and \( V \) as a function that can be called from a MATLAB Function block in Simulink.

function [T, V, M] = two_link_energy(q, qd, p)
% q, qd: 2x1 joint vectors
% p: struct with fields m1, m2, l1, l2, c1, c2, I1, I2, g

q1 = q(1);
q2 = q(2);

cos2 = cos(q2);

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

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

T = 0.5 * qd.' * M * qd;

V = p.m1 * p.g * p.c1 * sin(q1) ...
  + p.m2 * p.g * (p.l1 * sin(q1) + p.c2 * sin(q1 + q2));
end
      

In Simulink, one can create a MATLAB Function block, call two_link_energy, and route T and V to scopes to monitor energy conservation in simulations that use joint torques \tau computed in subsequent chapters.

10.2 Wolfram Mathematica Symbolic Derivation

Mathematica is well-suited to symbolic derivations of \( T \), \( V \), and later the full equations of motion.

ClearAll["Global`*"];

(* Parameters *)
m1 = Symbol["m1"]; m2 = Symbol["m2"];
l1 = Symbol["l1"]; l2 = Symbol["l2"];
c1 = Symbol["c1"]; c2 = Symbol["c2"];
I1 = Symbol["I1"]; I2 = Symbol["I2"];
g  = Symbol["g"];

(* Generalized coordinates *)
q1[t_] := Symbol["q1"][t];
q2[t_] := Symbol["q2"][t];

(* Positions of CoMs *)
pC1[t_] = { c1*Cos[q1[t]], c1*Sin[q1[t]] };
pC2[t_] = { l1*Cos[q1[t]] + c2*Cos[q1[t] + q2[t]],
            l1*Sin[q1[t]] + c2*Sin[q1[t] + q2[t]] };

vC1[t_] = D[pC1[t], t];
vC2[t_] = D[pC2[t], t];

(* Angular velocities (planar, scalar) *)
w1[t_] = D[q1[t], t];
w2[t_] = D[q1[t], t] + D[q2[t], t];

Ttrans = 1/2*m1*(vC1[t].vC1[t]) + 1/2*m2*(vC2[t].vC2[t]);
Trot   = 1/2*I1*w1[t]^2 + 1/2*I2*w2[t]^2;
T      = FullSimplify[Ttrans + Trot];

V = m1*g*pC1[t][[2]] + m2*g*pC2[t][[2]];

(* Express in terms of q1, q2, q1dot, q2dot *)
Tq = T /. { q1[t] -> Symbol["q1"],
           q2[t] -> Symbol["q2"],
           Derivative[1][q1][t] -> Symbol["q1dot"],
           Derivative[1][q2][t] -> Symbol["q2dot"] };

Vq = V /. { q1[t] -> Symbol["q1"],
           q2[t] -> Symbol["q2"] };

Simplify[Tq]
Simplify[Vq]
      

The resulting expressions match those in Section 5. Mathematica can further be used in later chapters to obtain symbolic equations of motion and to verify numerical implementations in MATLAB or Python.

In this lesson we built the energy description of multi-body robotic systems. Starting from single rigid-body kinetic energy, we assembled the multi-body kinetic energy via link Jacobians and inertial parameters, obtaining the joint-space inertia matrix \( \mathbf{M}(q) \) and showing it is symmetric positive definite. We then derived gravitational and elastic potential energies as functions of generalized coordinates. A detailed 2-link planar example and implementations in Python, C++, Java, MATLAB/Simulink, and Mathematica illustrated how these concepts are used in practice. These energy expressions will be the backbone for the Lagrangian formulation of robot dynamics and for understanding the structure of robot equations of motion in subsequent lessons.