Section 6.2: Rigid-body dynamics; the manipulator equation

A Careful Control Loop

This section develops the technical contract for Rigid-body dynamics; the manipulator equation into a usable mental model. First we define the object of study, then we connect it to the agent loop, then we test it with a compact implementation.

The key question in Rigid-body dynamics; the manipulator equation is practical: what must the agent know, what can it observe, what action is available, and what evidence shows that the action worked under the stated conditions?

Theory

For Rigid-body dynamics; the manipulator equation, the practical design rule is to make the interface inspectable before optimization begins: inputs, outputs, units, latency, bounds, and failure labels should all be visible in the saved artifact.

The Manipulator Equation

Every rigid robot arm with $n$ independent joints obeys one compact second-order law. It is the single most important equation in this chapter, and almost every controller, simulator, and learning method in later chapters is built on top of it:

$$M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q) = \tau$$

The terms are:

• $q \in \mathbb{R}^n$: the joint configuration (angles for revolute joints, displacements for prismatic joints), with $\dot{q}$ and $\ddot{q}$ the joint velocities and accelerations.
• $M(q) \in \mathbb{R}^{n \times n}$: the inertia matrix (also called the mass matrix). It is symmetric and positive definite for every reachable configuration. It maps joint acceleration to the inertial part of the required torque.
• $C(q,\dot{q}) \in \mathbb{R}^{n \times n}$: the Coriolis and centrifugal matrix. The product $C(q,\dot{q})\dot{q}$ collects the velocity-dependent coupling forces that appear when several joints move at once.
• $g(q) \in \mathbb{R}^n$: the gravity vector, the torque each joint must supply to hold the arm against gravity at configuration $q$.
• $\tau \in \mathbb{R}^n$: the vector of joint torques (or forces) commanded by the actuators.

The worked example below builds $M$, $C$, and $g$ in closed form for a 2-DOF planar arm (two revolute joints in the vertical plane, masses concentrated at the link tips), then verifies all three structural properties numerically with NumPy.

import numpy as np

# 2-DOF planar arm in a vertical plane, point masses at link tips.
m1, m2 = 1.5, 1.0       # link masses (kg)
l1, l2 = 0.5, 0.4       # link lengths (m)
g_acc  = 9.81           # gravity (m/s^2)

def manipulator_matrices(q, dq):
    """Return M(q), C(q,dq), g(q) for the 2-link planar arm."""
    q1, q2 = q
    dq1, dq2 = dq
    c2, s2 = np.cos(q2), np.sin(q2)

    # --- Inertia matrix M(q) (symmetric, configuration dependent) ---
    a = m1 * l1**2 + m2 * (l1**2 + l2**2 + 2 * l1 * l2 * c2)
    b = m2 * (l2**2 + l1 * l2 * c2)
    d = m2 * l2**2
    M = np.array([[a, b],
                  [b, d]])

    # --- Coriolis / centrifugal matrix C(q,dq) (Christoffel form) ---
    h = -m2 * l1 * l2 * s2
    C = np.array([[h * dq2, h * (dq1 + dq2)],
                  [-h * dq1, 0.0]])

    # --- Gravity vector g(q) ---
    c1   = np.cos(q1)
    c12  = np.cos(q1 + q2)
    g1 = (m1 + m2) * g_acc * l1 * c1 + m2 * g_acc * l2 * c12
    g2 = m2 * g_acc * l2 * c12
    g = np.array([g1, g2])
    return M, C, g

# Pick an arbitrary configuration and velocity.
q  = np.array([0.4, -0.7])
dq = np.array([1.2,  0.9])
M, C, g = manipulator_matrices(q, dq)

# Property 1: M is symmetric positive definite.
print("M symmetric :", np.allclose(M, M.T))
print("M eigvals   :", np.linalg.eigvals(M))   # all strictly > 0

# Property 3: torque to simply hold this pose equals g(q).
tau_hold = g
print("gravity hold torque tau = g(q):", tau_hold)

# Forward dynamics: given a torque, solve for joint acceleration.
tau   = np.array([2.0, 0.5])
ddq   = np.linalg.solve(M, tau - C @ dq - g)
print("forward-dynamics ddq        :", ddq)

# Inverse dynamics round-trip recovers the same torque.
tau_back = M @ ddq + C @ dq + g
print("inverse-dynamics tau        :", tau_back, "(matches input)")

# Property 2: dM/dt - 2C is skew-symmetric.
# dM/dt = (dM/dq2) * dq2 here, since M depends only on q2.
dMdq2 = np.array([[-2 * m2 * l1 * l2 * np.sin(q[1]),
                   -m2 * l1 * l2 * np.sin(q[1])],
                  [-m2 * l1 * l2 * np.sin(q[1]), 0.0]])
Mdot  = dMdq2 * dq[1]
S = Mdot - 2 * C
print("skew-symmetric (S = -S^T)  :", np.allclose(S, -S.T))
print("passivity dq^T S dq ~ 0    :", float(dq @ S @ dq))

Running this prints that $M$ is symmetric with positive eigenvalues, the hold torque equals $g(q)$, the inverse-dynamics round-trip recovers the commanded torque, and $\dot{M} - 2C$ is skew-symmetric so $\dot{q}^\top(\dot{M}-2C)\dot{q}$ is numerically zero. Those four checks are the minimum acceptance test before trusting any dynamics code, hand-built or from a library.

Practical Recipe

1. Write the observation, action, and success metric before choosing a model.
2. Build a baseline that is simple enough to debug by inspection.
3. Add the library implementation only after the baseline behavior is understood.
4. Record failures as structured cases: perception error, state error, planning error, control error, or evaluation error.
5. Run at least one perturbation test before trusting the result.

Production Pattern

Rigid-body dynamics; the manipulator equation sits inside the Part II robotics contract: geometry defines where things are, kinematics defines what motion is possible, dynamics defines what motion costs, control defines how errors are corrected, and sensing defines what the agent can know on time.

Use the manipulator equation as an accounting identity for inertia, Coriolis terms, gravity, and actuation. This makes the section useful to students, builders, and researchers at the same time: the idea has an intuitive role, a formal interface, a runnable check, and a failure mode that can be reproduced.

The manipulator equation is valuable because it separates the sources of joint torque instead of hiding them inside a simulator step. A controller can then ask a precise question: how much torque accelerates the mechanism, how much cancels gravity, how much counters velocity coupling, and how much is consumed by contact or friction?

Use this recipe when turning Rigid-body dynamics; the manipulator equation into code, a simulator experiment, or a robot diagnostic. The point is not to use every library. The point is to keep the hand-built baseline and the maintained-tool path comparable.

1. Specify mass, inertia, actuator limits, contact model, timestep, and solver tolerance before running a rollout.
2. Run one free-motion test and one contact test with logged energy, constraint violation, and penetration depth.
3. Compare the hand calculation with MuJoCo, Drake, Pinocchio, or MJX on the same model and timestep.
4. Store solver settings, random seed, initial state, trajectory, and failure labels in one artifact.
5. Scale to Isaac Lab or GPU-parallel simulation only after a small model passes deterministic checks.

For Rigid-body dynamics; the manipulator equation, distrust smooth simulation until the section-specific physical assumption has been stress-tested: timestep, contact stiffness, damping, friction, actuation, and energy behavior should each have a small diagnostic.