In previous chapters, the equations of motion of an \( n \)-DOF manipulator in local generalized coordinates \( q \in \mathbb{R}^n \) were obtained in the familiar form

\[ \mathbf{M}(q)\,\ddot q + \mathbf{C}(q,\dot q)\,\dot q + \mathbf{g}(q) = \mathbf{u} + \mathbf{J}(q)^{\top}\mathbf{f}_{\mathrm{ext}}, \]

where \( \mathbf{M}(q) \) is the inertia matrix, \( \mathbf{C}(q,\dot q)\dot q \) collects Coriolis and centrifugal terms, \( \mathbf{g}(q) \) is the gravity vector, \( \mathbf{u} \) the vector of joint torques/forces, and \( \mathbf{J}(q)^{\top}\mathbf{f}_{\mathrm{ext}} \) maps Cartesian wrenches into joint space.

Geometric mechanics interprets this equation as the coordinate expression of a deeper structure:

• a configuration manifold \( Q \) (possibly a product of Lie groups),
• its tangent bundle \( TQ \) of states \( (q,\dot q) \),
• a Riemannian metric \( g \) induced by kinetic energy, and
• a Lagrangian \( L : TQ \to \mathbb{R} \), typically \( L = T - V \).

The manipulator equations then arise from a variational principle on \( TQ \) (Lagrange–d'Alembert) plus choices of coordinates on \( Q \). This viewpoint is coordinate-free and highlights invariances, conserved quantities, and intrinsic notions of curvature and geodesics.

flowchart TD
  A["Physical mechanism (links, joints, masses)"] --> B["Configuration space Q"]
  B --> C["Tangent bundle TQ (positions, velocities)"]
  C --> D["Lagrangian L:TQ->R (T - V)"]
  D --> E["Geometric EOM on (Q,g) via variational principle"]
  E --> F["Coordinate form M(q) qddot + C(q,qdot) qdot + g(q) = u"]
        

For a robot, the configuration space \( Q \) is often a product of circles and Euclidean spaces. For a standard \( n \)-joint manipulator with revolute and prismatic joints we have schematically

\[ Q \cong S^1 \times \cdots \times S^1 \times \mathbb{R} \times \cdots \times \mathbb{R}, \]

but more generally, legs, floating bases, and rigid-body poses live on Lie groups like \( \mathrm{SO}(3) \) and \( \mathrm{SE}(3) \). From the point of view of geometric mechanics:

• \( Q \) is a smooth manifold of dimension \( n \), with local coordinates \( q = (q^1,\dots,q^n) \).
• For each \( q \in Q \), the tangent space \( T_q Q \) is a vector space of virtual velocities at \( q \).
• The tangent bundle \( TQ = \bigcup_{q \in Q} \{q\} \times T_q Q \) is the state space of positions and velocities \( (q,\dot q) \).
• The cotangent bundle \( T^{\ast}Q \) collects momenta \( p \) dual to velocities.

For a Lagrangian system we specify

\[ L : TQ \to \mathbb{R}, \quad (q,\dot q) \mapsto L(q,\dot q) = T(q,\dot q) - V(q), \]

and define generalized momenta

\[ p_i = \frac{\partial L}{\partial \dot q^i}(q,\dot q), \quad i=1,\dots,n, \]

which assemble into a covector \( p \in T_q^{\ast}Q \). In Euclidean coordinates, this reduces to the usual \( p = \mathbf{M}(q)\dot q \), but the abstract formulation is independent of the particular choice of chart on \( Q \).

Many robotic configurations naturally live on Lie groups, i.e. smooth manifolds that are also groups with smooth multiplication and inversion. For a rigid body in 3D:

• The orientation lives on \( \mathrm{SO}(3) \).
• The full pose lives on \( \mathrm{SE}(3) = \mathrm{SO}(3) \ltimes \mathbb{R}^3 \).

A multi-link mechanism can be modeled as a product of Lie groups:

\[ Q \cong G_1 \times \cdots \times G_m, \]

where each \( G_i \) is \( S^1 \) (revolute), \( \mathbb{R} \) (prismatic), \( \mathrm{SO}(3) \), or \( \mathrm{SE}(3) \). The Lie algebra \( \mathfrak{g} = T_e G \) at the identity stores angular and linear velocities in body coordinates (twists).

For a rigid body with pose \( g(t) \in \mathrm{SE}(3) \), the body twist \( \boldsymbol{\xi}_B \in \mathbb{R}^6 \) satisfies

\[ \dot g(t) = g(t)\,\widehat{\boldsymbol{\xi}_B(t)}, \]

where \( \widehat{\cdot} \) is the Lie algebra “hat” operator. This is a geometric version of the spatial/ body velocity descriptions introduced in the PoE formulation (Chapter 5). Geometric mechanics sees the full robot as evolving on a product of such Lie groups.

The kinetic energy of a robot with configuration manifold \( Q \) can be written in a coordinate-free way as a Riemannian metric on \( Q \).

A Riemannian metric is a smoothly varying inner product

\[ g_q : T_q Q \times T_q Q \to \mathbb{R}, \quad (v_q,w_q) \mapsto g_q(v_q,w_q), \]

symmetric and positive definite for each \( q \). This metric defines kinetic energy as

\[ T(q,\dot q) = \tfrac{1}{2}\,g_q(\dot q,\dot q). \]

In local coordinates \( q^1,\dots,q^n \), the metric is represented by a matrix \( \bigl(g_{ij}(q)\bigr) \) so that

\[ T(q,\dot q) = \tfrac{1}{2}\sum_{i,j=1}^{n} g_{ij}(q)\,\dot q^i \dot q^j. \]

Comparing with the standard kinetic energy expression, we identify \( \mathbf{M}(q) = \bigl(g_{ij}(q)\bigr) \). Thus, the joint-space inertia matrix is precisely the coordinate representation of a Riemannian metric on \( Q \). Under a change of coordinates \( q \mapsto \tilde q(q) \), this matrix transforms as a metric tensor.

The Levi-Civita connection of \( g \) defines how vectors are differentiated along curves. In coordinates, it is encoded in the Christoffel symbols

\[ \Gamma^{k}_{ij}(q) = \tfrac{1}{2}\sum_{\ell=1}^{n} g^{k\ell}(q)\bigl( \partial_i g_{j\ell}(q) + \partial_j g_{i\ell}(q) - \partial_{\ell} g_{ij}(q) \bigr), \]

where \( g^{k\ell} \) are the entries of the inverse metric matrix \( g(q)^{-1} \). These \( \Gamma^{k}_{ij} \) encode the same information as the Coriolis and centrifugal terms: the vector \( \mathbf{C}(q,\dot q)\dot q \) can be written as

\[ \bigl(\mathbf{C}(q,\dot q)\dot q\bigr)^k = \sum_{i,j=1}^{n} \Gamma^{k}_{ij}(q)\,\dot q^i \dot q^j. \]

Thus, geometric mechanics packages inertia and Coriolis effects into a single geometric object: the Levi-Civita connection of the kinetic-energy metric.

Given a Lagrangian \( L = T - V \) and a non-conservative generalized force covector \( Q(q,\dot q) = \bigl(Q_1,\dots,Q_n\bigr) \), the Lagrange–d'Alembert equations in coordinates read

\[ \frac{\mathrm{d}}{\mathrm{d}t}\Bigl(\frac{\partial L}{\partial \dot q^k}\Bigr) - \frac{\partial L}{\partial q^k} = Q_k(q,\dot q), \quad k = 1,\dots,n. \]

For a robotic manipulator with

\[ L(q,\dot q) = \tfrac{1}{2}\dot q^{\top} \mathbf{M}(q)\dot q - V(q), \]

a short computation using the expression of \( \Gamma^{k}_{ij} \) yields the geometric form

\[ \ddot q^k + \sum_{i,j=1}^{n} \Gamma^{k}_{ij}(q)\,\dot q^i \dot q^j + \sum_{\ell=1}^{n} g^{k\ell}(q)\,\frac{\partial V}{\partial q^{\ell}}(q) = \sum_{\ell=1}^{n} g^{k\ell}(q)\,Q_{\ell}(q,\dot q). \]

In matrix notation this is

\[ \mathbf{M}(q)\ddot q + \mathbf{C}(q,\dot q)\dot q + \nabla V(q) = Q^{\sharp}(q,\dot q), \]

which matches the familiar manipulator equation. Here, \( Q^{\sharp} \) is the metric-based identification of covectors with vectors (raising indices).

A key strength of geometric mechanics is the systematic use of symmetries. Suppose a Lie group \( G \) acts smoothly on \( Q \) by configuration transformations \( \Phi : G \times Q \to Q \). If this action lifts to \( TQ \) and leaves the Lagrangian invariant, then Noether's theorem states that there exists a conserved quantity, the momentum map \( J : TQ \to \mathfrak{g}^{\ast} \). For each \( \xi \in \mathfrak{g} \) (a Lie algebra element), we define

\[ \bigl\langle J(q,\dot q), \xi \bigr\rangle = g_q\bigl(\dot q, \xi_Q(q)\bigr), \]

where \( \xi_Q \) is the infinitesimal generator of the group action. Along any solution of the unforced equations (\( Q = 0 \)), this pairing is constant in time. For whole-body robots with a free-floating base, this reproduces the conservation of spatial linear and angular momentum when there are no external wrenches.

The Levi-Civita connection \( \nabla \) associated with the kinetic-energy metric defines geodesics: curves on \( Q \) that locally minimize distance and satisfy zero covariant acceleration. In coordinates, a curve \( q(t) \) is a geodesic if

\[ \ddot q^k + \sum_{i,j=1}^{n} \Gamma^{k}_{ij}(q)\,\dot q^i \dot q^j = 0, \quad k = 1,\dots,n. \]

For a free robot with no potential and no external forces (\( V \equiv 0,\,Q=0 \)), the equations of motion reduce to the geodesic equation. In other words, a free-floating multi-body robot moves along geodesics of its configuration manifold endowed with the kinetic-energy metric.

When potentials and generalized forces are present, the equation becomes

\[ \nabla_{\dot q}\dot q = -\operatorname{grad}_g V(q) + F^{\sharp}(q,\dot q), \]

i.e. geodesic acceleration is perturbed by the metric gradient of the potential and by external forces. This geometric viewpoint unifies “inertia-driven” motion with effects of gravity, springs, and contacts.

flowchart TD
  F0["Kinetic metric g on Q"] --> G0["Geodesics: inertial motion"]
  G0 --> E0["Coordinate form: \nqddot^k + Gamma^k_ij qdot^i qdot^j = 0"]
  V0["Potential V and forces Q"] --> P0["Lagrange-d'Alembert on (Q,g)"]
  P0 --> E1["qddot^k + Gamma^k_ij qdot^i qdot^j = \n-grad V + force term"]
  E0 --> M0["Free-floating robot motion"]
  E1 --> M1["Manipulators with gravity, contacts"]
        

We illustrate how to construct the kinetic metric, Christoffel symbols, and equations of motion of a simple 2-DOF planar manipulator directly from a Lagrangian using Python and symbolic tools. This “geometric” pipeline is conceptually similar to what advanced robotics libraries (e.g. modern_robotics, RoboticsToolbox-Python, pin bindings for Pinocchio) perform internally.

import sympy as sp

# Generalized coordinates and velocities for a 2R planar arm
q1, q2 = sp.symbols("q1 q2")
dq1, dq2 = sp.symbols("dq1 dq2")
q = sp.Matrix([q1, q2])
dq = sp.Matrix([dq1, dq2])

# Simple link parameters (masses and lengths)
m1, m2 = sp.symbols("m1 m2", positive=True)
l1, l2 = sp.symbols("l1 l2", positive=True)
g = sp.symbols("g", real=True)

# Kinetic energy (planar 2R, COM at l_i/2, scalar inertias I1, I2)
I1, I2 = sp.symbols("I1 I2", positive=True)

# Position of COMs in the plane
x1 = (l1 / 2) * sp.cos(q1)
y1 = (l1 / 2) * sp.sin(q1)

x2 = l1 * sp.cos(q1) + (l2 / 2) * sp.cos(q1 + q2)
y2 = l1 * sp.sin(q1) + (l2 / 2) * sp.sin(q1 + q2)

# Velocities of COMs via Jacobians
J1 = sp.Matrix([[sp.diff(x1, q1), sp.diff(x1, q2)],
                [sp.diff(y1, q1), sp.diff(y1, q2)]])
J2 = sp.Matrix([[sp.diff(x2, q1), sp.diff(x2, q2)],
                [sp.diff(y2, q1), sp.diff(y2, q2)]])

v1 = J1 * dq
v2 = J2 * dq

T_trans = sp.Rational(1, 2) * m1 * (v1.dot(v1)) \
        + sp.Rational(1, 2) * m2 * (v2.dot(v2))

# Angular velocities about z-axis
w1 = dq1
w2 = dq1 + dq2
T_rot = sp.Rational(1, 2) * I1 * w1**2 + sp.Rational(1, 2) * I2 * w2**2

T = sp.simplify(T_trans + T_rot)

# Potential energy (gravity in -y direction)
V = m1 * g * y1 + m2 * g * y2

L = T - V

# Extract inertia matrix M(q) from T(q, dq) = 0.5 dq^T M dq
M = sp.hessian(T, (dq1, dq2))  # Hessian of T w.r.t. velocities
M = sp.simplify(M)

print("Inertia matrix M(q):")
sp.pprint(M)

# Compute Christoffel symbols from the metric M(q)
q_symbols = (q1, q2)
M_inv = sp.simplify(M.inv())
Gamma = [[[0 for _ in range(2)] for _ in range(2)] for _ in range(2)]

for k in range(2):
    for i in range(2):
        for j in range(2):
            term = 0
            for ell in range(2):
                term += M_inv[k, ell] * (
                    sp.diff(M[j, ell], q_symbols[i]) +
                    sp.diff(M[i, ell], q_symbols[j]) -
                    sp.diff(M[i, j], q_symbols[ell])
                )
            Gamma[k][i][j] = sp.simplify(sp.Rational(1, 2) * term)

print("Christoffel symbols Gamma^k_ij(q):")
for k in range(2):
    for i in range(2):
        for j in range(2):
            print(f"Gamma[{k+1}][{i+1}][{j+1}] = {Gamma[k][i][j]}")

# Euler-Lagrange equations (Lagrange-d'Alembert with zero non-conservative forces)
ddq1, ddq2 = sp.symbols("ddq1 ddq2")
ddq = sp.Matrix([ddq1, ddq2])

EL = []
for k, qk in enumerate(q_symbols):
    dL_ddqk = sp.diff(L, (dq1, dq2)[k])
    d_dt_dL_ddqk = sp.diff(dL_ddqk, q1) * dq1 + sp.diff(dL_ddqk, q2) * dq2 \
                   + sp.diff(dL_ddqk, dq1) * ddq1 + sp.diff(dL_ddqk, dq2) * ddq2
    dL_dqk = sp.diff(L, qk)
    EL.append(sp.simplify(d_dt_dL_ddqk - dL_dqk))

print("Euler-Lagrange equations (symbolic):")
for k in range(2):
    print(f"E_{k+1}(q, dq, ddq) = {EL[k]}")
      

This script illustrates the geometric pipeline:

1. Build \( T \) and \( V \) from kinematics and inertial parameters.
2. Extract the metric \( g \equiv \mathbf{M}(q) \) via the Hessian of \( T \) in velocities.
3. Compute Christoffel symbols and Euler–Lagrange equations symbolically.

In practice, robotics libraries (e.g. pinocchio, RBDL bindings, and RoboticsToolbox-Python) implement highly optimized versions of these computations, often in Lie group coordinates.

Modern C++ libraries for robot dynamics such as RBDL, Pinocchio and Orocos KDL rely on:

• matrix and vector types (e.g. Eigen),
• SE(3) and SO(3) Lie group representations (e.g. Sophus),
• and algorithms that operate on configuration manifolds \( Q \).

8.1 C++ Example Using Eigen

The snippet below shows a minimal C++ function computing kinetic energy from an inertia matrix \( \mathbf{M}(q) \), viewed as a metric. In realistic codes, \( M(q) \) would be provided by a library such as Pinocchio, which itself implements a geometric mechanics model on the product of joint Lie groups.

#include <Eigen/Dense>

using Eigen::VectorXd;
using Eigen::MatrixXd;

// Kinetic energy T(q, dq) = 0.5 * dq^T M(q) dq
double kineticEnergy(const MatrixXd& Mq, const VectorXd& dq) {
    return 0.5 * dq.transpose() * Mq * dq;
}

// Example: call kineticEnergy inside a dynamics routine
int main() {
    const int n = 2;
    MatrixXd Mq(n, n);
    VectorXd dq(n);

    // Example numeric metric (e.g. evaluated at some configuration q)
    Mq << 2.0, 0.5,
          0.5, 1.0;

    dq << 0.3, -0.1;

    double T = kineticEnergy(Mq, dq);
    // T is the Riemannian energy 0.5 * g_q(dq, dq)
    return 0;
}
      

8.2 Java Example Using EJML

In Java, one can use matrix libraries such as EJML or Apache Commons Math. Below is a simple class that represents a configuration metric \( \mathbf{M}(q) \) and computes kinetic energy. In more sophisticated code, \( M(q) \) would be generated from a rigid-body tree description, as in Drake or other multi-body toolkits.

import org.ejml.simple.SimpleMatrix;

public class GeometricRobotModel {

    // Compute kinetic energy T(q, dq) = 0.5 * dq^T M(q) dq
    public static double kineticEnergy(SimpleMatrix Mq, SimpleMatrix dq) {
        SimpleMatrix temp = dq.transpose().mult(Mq).mult(dq);
        return 0.5 * temp.get(0, 0);
    }

    public static void main(String[] args) {
        // Example 2-DOF metric at some configuration q
        double[][] Mdata = {
            {2.0, 0.5},
            {0.5, 1.0}
        };
        double[][] dqdata = {
            {0.3},
            {-0.1}
        };

        SimpleMatrix Mq = new SimpleMatrix(Mdata);
        SimpleMatrix dq = new SimpleMatrix(dqdata);

        double T = kineticEnergy(Mq, dq);
        // T encodes the Riemannian norm of dq
    }
}
      

These short snippets isolate the metric viewpoint: the same code can be reused regardless of whether \( Q \) is a simple Euclidean space or a product of Lie groups; the geometry is hidden in how \( \mathbf{M}(q) \) is computed.

9.1 MATLAB / Simulink

MATLAB's Robotics System Toolbox and Symbolic Math Toolbox allow one to implement geometric mechanics ideas explicitly. The following script defines a simple Riemannian metric and uses it to build the manipulator equations for a 2-DOF arm; these equations can then be imported into Simulink as a custom dynamics block.

syms q1 q2 dq1 dq2 ddq1 ddq2 real
syms m1 m2 l1 l2 I1 I2 g real

q  = [q1; q2];
dq = [dq1; dq2];

% Kinetic energy of a simple 2R planar arm
x1 = (l1/2)*cos(q1);
y1 = (l1/2)*sin(q1);
x2 = l1*cos(q1) + (l2/2)*cos(q1 + q2);
y2 = l1*sin(q1) + (l2/2)*sin(q1 + q2);

J1 = jacobian([x1; y1], q);
J2 = jacobian([x2; y2], q);

v1 = J1*dq;
v2 = J2*dq;

T_trans = 0.5*m1*(v1.'*v1) + 0.5*m2*(v2.'*v2);
w1 = dq1;
w2 = dq1 + dq2;
T_rot = 0.5*I1*w1^2 + 0.5*I2*w2^2;

T = simplify(T_trans + T_rot);
V = m1*g*y1 + m2*g*y2;
L = T - V;

% Metric M(q) = d^2 T / (d dq^2)
M = simplify(hessian(T, dq));

% Euler-Lagrange equations
ddq = [ddq1; ddq2];
Q   = sym('Q', [2 1]);  % generalized forces

EL = sym(zeros(2,1));
for k = 1:2
    dL_ddqk   = functionalDerivative(L, dq(k));
    ddt_term  = jacobian(dL_ddqk, [q; dq])*[dq; ddq];
    dL_dqk    = functionalDerivative(L, q(k));
    EL(k)     = simplify(ddt_term - dL_dqk - Q(k));
end

EL = simplify(EL);

% Solve for ddq symbolically: M(q)*ddq + C(q,dq)*dq + g(q) = Q
A = jacobian(EL, ddq);     % should coincide with M(q)
b = simplify(EL - A*ddq);  % remaining terms C(q,dq)*dq + g(q) - Q

% Export A and b as MATLAB functions usable inside Simulink
matlabFunction(A, 'Vars', {q1,q2,m1,m2,l1,l2,I1,I2}, 'File', 'M_metric.m');
matlabFunction(b, 'Vars', {q1,q2,dq1,dq2,m1,m2,l1,l2,I1,I2,g,Q}, 'File', 'Cgrav_forces.m');
      

In Simulink, a typical workflow is to:

1. Create a state vector block for \( q \) and \( \dot q \).
2. Call M_metric and Cgrav_forces to evaluate the geometric terms.
3. Solve for \( \ddot q \) using a MATLAB Function block: \( \ddot q = M(q)^{-1}\bigl(Q - \text{remaining terms}\bigr) \).
4. Integrate \( \ddot q \) to obtain \( \dot q \) and \( q \).

9.2 Wolfram Mathematica

Mathematica's symbolic and differential geometry capabilities make it natural to represent configuration manifolds and metrics directly. The following snippet constructs the metric and Christoffel symbols for a generic kinetic energy function.

(* Generalized coordinates and velocities *)
Clear[q1, q2, dq1, dq2];
T[q1_, q2_, dq1_, dq2_] := 1/2 (a11[q1, q2] dq1^2 +
                               2 a12[q1, q2] dq1 dq2 +
                               a22[q1, q2] dq2^2);

(* Metric matrix g(q) *)
gmat[q1_, q2_] := { {a11[q1, q2], a12[q1, q2]},
                   {a12[q1, q2], a22[q1, q2]} };

(* Inverse metric *)
gInv[q1_, q2_] := Inverse[gmat[q1, q2]];

(* Christoffel symbols Gamma^k_ij (k,i,j from 1 to 2) *)
Gamma[k_, i_, j_][q1_, q2_] :=
 Module[{gInvMat, gij, coords, sum},
  gInvMat = gInv[q1, q2];
  gij[a_, b_] := gmat[q1, q2][[a, b]];
  coords = {q1, q2};
  sum = 0;
  Do[
   sum = sum + gInvMat[[k, ell]] (
      D[gij[j, ell], coords[[i]]] +
      D[gij[i, ell], coords[[j]]] -
      D[gij[i, j], coords[[ell]]]
    )
   ,
   {ell, 1, 2}
  ];
  1/2 sum
 ];

(* Example: specialize a simple metric *)
a11[q1_, q2_] := 1 + q2^2;
a12[q1_, q2_] := 0;
a22[q1_, q2_] := 1;

Gamma11[q1_, q2_] = Gamma[1, 1, 1][q1, q2] // Simplify
Gamma121[q1_, q2_] = Gamma[1, 2, 1][q1, q2] // Simplify
      

With these geometric primitives in place, one can implement the geodesic equation, project external forces, and symbolically analyze curvature effects in robotic mechanisms.

This lesson recast robot kinematics and dynamics within the framework of geometric mechanics. The configuration space \( Q \) of a robot was viewed as a manifold (often a product of Lie groups) with a kinetic-energy metric whose coordinate representation is the familiar inertia matrix \( \mathbf{M}(q) \). The Levi-Civita connection of this metric encodes Coriolis and centrifugal effects, and the Lagrange–d'Alembert principle yields the manipulator equations as coordinate expressions of covariant acceleration and potential gradients.

Symmetries of the robot and its environment were linked to conserved quantities through Noether's theorem, providing structural insights for floating-base and multi-contact systems. Finally, we illustrated geometric modeling pipelines in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica, bridging the abstract concepts with practical computational tools commonly used in robotics research.