Section 6.2: Rigid-body dynamics; the manipulator equation

"M, C, and g split every torque into the share that accelerates, the share the velocity already spent, and the share gravity will always claim."

A Torque Accountant

Hold your arm straight out and keep it still: your shoulder is quietly burning fuel just to fight gravity, and the instant you swing it, a second, sideways force you never commanded appears out of nowhere. A robot arm feels exactly these forces, and one compact law, the manipulator equation, names every one of them. This section defines the object of study, connects it to the agent loop, and tests it with a compact implementation.

The key question 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

The generic checklist below (interface, mechanism, failure labels) is the template this section fills in concretely: the next heading, "The Manipulator Equation," is where \(M\), \(C\), and \(g\) are actually defined and used, so read this paragraph as the scaffolding and the next section as the payload.

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

Consider swinging a robot arm to a target pose. Gravity pulls link 2 down with a force that depends on the current angle of link 1. Accelerating joint 1 shifts the effective inertia seen at joint 2. Moving both joints at once creates Coriolis forces that push each joint sideways relative to the other. A controller that ignores any of these coupling terms overshoots, oscillates, or stalls, even with perfect actuators. The manipulator equation makes each contribution explicit so the controller can cancel it precisely. In representative simulation studies of this style (pre-deep-RL era, circa 2015), a PD (proportional-derivative) controller with no dynamics model typically needs on the order of tens of iterations of gain tuning to stabilize a fast two-joint swing, whereas a controller that feeds forward \(M\ddot{q} + C\dot{q} + g\) can reach comparable stability in one pass; the exact iteration count varies by benchmark and gain-tuning method, but the qualitative gap holds because the equation is a complete torque budget that leaves nothing for the gains to guess.

This law is called the manipulator equation, rather than just "the equation of motion," because it is the same Lagrangian rigid-body dynamics used across all of mechanics, specialized to the case a manipulator cares about: a serial chain of actuated joints with a torque input at each one, so that \(M\), \(C\), and \(g\) can be organized around the joint-space variables \(q\), \(\dot{q}\), \(\ddot{q}\) a controller actually commands and measures.

Every rigid robot arm with \(n\) independent joints obeys one compact second-order law, shown term by term for a 2-DOF arm in Figure 6.2A. 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$$

A controller that cannot name all three terms on the left is not controlling the robot; it is negotiating with it.

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. Why it matters: a real robot arm moving at speed carries angular momentum in each link; changing direction of that momentum demands torque even with no desired acceleration. Ignoring \(C\dot{q}\) causes systematic tracking errors that grow with joint speed, degrading grasps and making sim-to-real transfer unreliable at any velocity above slow-motion. At a moderate swing speed of 1 rad/s on a 1 kg, 0.5 m link, the omitted Coriolis torque reaches roughly 0.25 Nm, which is the same order as a 2-cm position error in gravity compensation: negligible at slow-motion joint speeds, but large enough at operating speed to cause a gripper to miss a narrow slot entirely. How it works: each entry \(C_{ij}\) is a Christoffel symbol of the inertia matrix, where a Christoffel symbol is the coefficient built from partial derivatives of \(M(q)\) that measures how the inertia felt at one joint changes as another joint moves, computed as \(C_{ij} = \tfrac{1}{2}\sum_k\!\left(\tfrac{\partial M_{ij}}{\partial q_k}+\tfrac{\partial M_{ik}}{\partial q_j}-\tfrac{\partial M_{jk}}{\partial q_i}\right)\dot{q}_k\). Because \(M(q)\) changes shape as the arm moves, these partial derivatives capture how one joint's acceleration shifts the effective inertia seen by another.
• \(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

Because the three terms must be re-evaluated every cycle rather than precomputed, putting the manipulator equation into a real system is less about the algebra and more about disciplined bookkeeping, which the following recipe makes concrete.

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

Once you can name each torque term and test it in isolation, the question becomes where this equation lives in a deployed stack, and the answer is that 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.

Treat the manipulator equation as an accounting identity across inertia, Coriolis terms, gravity, and actuation. That framing gives it what practitioners and researchers both need: an intuitive role, a formal interface, a runnable check, and a reproducible failure mode.

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? Consider a specific case: the 7-DOF KUKA LBR iiwa operating in gravity-compensation mode computes \(g(q)\) at 1 kHz and feeds it as a feed-forward term, typically reducing position error at rest from tens of millinewton-meters to under 1 mNm with no stiffness gains, though the exact figure depends on the calibrated inertial parameters and joint friction of the individual arm. This reduction is not automatic: it holds only when the identified mass and link parameters feeding \(g(q)\) match the physical arm closely enough that the residual is friction and encoder noise rather than model error. This is precisely property (3) of the manipulator equation applied in production hardware.

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.

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.