Section 6.1: From kinematics to dynamics: forces, torques, inertia

"Kinematics maps the trajectory. Dynamics signs the invoice: so many newtons, so much inertia, paid in full at every timestep."

Section 6.1

A robot arm can trace a flawless arc through simulation and still rip its own shoulder apart on the lab floor, because the geometry that looked perfect never asked how many newton-meters the motor had to find. A kinematic planner can compute a joint-angle trajectory that avoids collisions and reaches the goal, but it says nothing about whether the robot's motors can actually execute that trajectory. Consider a robot arm commanded to lift a 5 kg payload from rest to full speed in 0.1 seconds: kinematics permits the motion, but dynamics reveals whether the shoulder joint can supply the 80 N·m peak torque the acceleration demands. If it cannot, the arm overshoots, oscillates, or triggers a protective shutdown. Dynamics is the discipline that adds forces, torques, and inertia to the geometric picture, making the difference between a path that is geometrically possible and one that is physically executable.

Kinematics grants permission; dynamics presents the bill. Figure 6.1A makes this concrete with a free-body diagram of a single link, where gravity, applied torque, and the inertia tensor together set the angular acceleration the actuator must produce.

The diagram above previews the algorithm at the heart of this transition: a forward sweep that carries velocity and acceleration outward from base to tip, followed by a backward sweep that carries forces and torques inward and reads off the joint torque at each step. The kinematics-to-dynamics transition becomes a usable computational tool. The central object is the rigid-body equation of motion \(M(q)\ddot{q} + h(q,\dot{q}) = \tau\), where \(M(q)\) is the configuration-dependent joint-space inertia matrix, \(h\) collects Coriolis, gravity, and friction terms, and \(\tau\) is the actuator torque vector. We derive how to compute \(\tau\) efficiently via the recursive Newton-Euler algorithm, connect that computation to actuator limit checking, and test the result against the closed-form manipulator equation on a 2-link arm before scaling to higher DOF.

The key practical question is: given a desired joint-space trajectory \((q(t), \dot{q}(t), \ddot{q}(t))\), can the robot's actuators supply the required torques without saturating, overheating, or violating contact constraints? On a Franka Panda, joint 1 is rated to 87 N·m continuous and 87 N·m peak; joint 4 drops to 12 N·m continuous. Aggressive Cartesian paths that look smooth in simulation can plausibly demand joint-4 peak torques of 20 N·m or more when payload inertia is not accounted for, which can cause the built-in torque monitor to trip a protective stop before the motion completes.

Theory

The kinematics-to-dynamics computation requires five inputs. You supply the joint configuration \(q\) (radians or meters, in the robot's URDF joint ordering), joint velocity \(\dot{q}\) (rad/s), desired joint acceleration \(\ddot{q}\) (rad/s²), the external wrench at the end-effector (N and N·m in the tool frame), and the URDF or rigid-body model with per-link mass, inertia tensor, and center-of-mass offsets. The algorithm produces the required actuator torque vector \(\tau\) (N·m). You then compare \(\tau\) against each joint's torque limit before accepting the trajectory for execution.

Worked Example: Recursive Newton-Euler Wrench Propagation

The cleanest way to turn kinematics into dynamics is the recursive Newton-Euler algorithm (RNEA). It runs in two sweeps. The forward sweep propagates velocity and acceleration from the base out to the end-effector. The backward sweep propagates wrenches (the stacked force-torque pairs \(\mathcal{F} = (n, f)\)) from the end-effector back to the base, and at each joint it reads off the torque the actuator must supply.

Both sweeps hinge on one per-link quantity that the backward sweep cannot do without, so before tracing the recursion we need to pin down what that quantity actually represents. The inertia tensor \(I_i\) is a 3x3 symmetric positive-definite matrix (all its eigenvalues are positive, so it never "cancels out" a rotation, only resists it by varying amounts) that encodes how a link's mass is distributed around its center of mass. Unlike scalar mass, which resists linear acceleration equally in all directions, the inertia tensor resists angular acceleration differently depending on the rotation axis. An elongated forearm link has low inertia about its own long axis but high inertia about axes perpendicular to it; the off-diagonal terms (products of inertia, the entries that couple rotation about one axis to torque about another) capture how rotation about one axis induces reaction torques about others.

In embodied AI this asymmetry has direct hardware consequences. A gripper picking along one axis may demand almost no wrist torque, while rotating the same gripper 90 degrees demands ten times more. Controllers that ignore the tensor's off-diagonal structure underestimate peak torques along coupled axes. That underestimate causes actuator saturation or protective stops precisely during the dexterous, off-axis motions that make manipulation useful. On a Franka Panda picking a 1 kg tool with a 15 cm lateral offset, zeroing the off-diagonal inertia terms produces a peak wrist-torque estimate of roughly 4 N·m. The true demand is closer to 11 N·m, and the built-in torque monitor will typically trip as a result. Every URDF link specifies the tensor entries (ixx, ixy, ixz, iyy, iyz, izz); setting them incorrectly produces a simulated robot that behaves nothing like hardware.

For link \(i\) with mass \(m_i\), center-of-mass acceleration \(a_{c,i}\), angular velocity \(\omega_i\), and angular acceleration \(\alpha_i\), the Newton and Euler balance laws are:

$$f_i = f_{i+1} + m_i\, a_{c,i}, \qquad n_i = n_{i+1} + I_i\alpha_i + \omega_i \times (I_i\omega_i) + r_{i,c}\times(m_i a_{c,i}) + r_{i,i+1}\times f_{i+1}$$

The joint torque is then the projection of the link wrench onto the joint axis \(z_i\): for a revolute joint, \(\tau_i = n_i^\top z_i\). The velocity-dependent term \(h(q,\dot q)\) introduced above splits into two named pieces once you separate its position-only part from its velocity-dependent part: \(h(q,\dot q) = C(q,\dot q)\dot q + g(q)\), where \(g(q)\) is gravity torque alone (velocity set to zero) and \(C(q,\dot q)\dot q\) is the remaining Coriolis and centrifugal torque. The worked example below computes both pieces independently to cross-check RNEA. The example below implements both sweeps for the same 2-link planar arm used in Section 6.2, then confirms that the torques RNEA produces match the manipulator-equation torques \(M\ddot{q}+C\dot{q}+g\) computed independently.

import numpy as np

m1, m2 = 1.5, 1.0      # link masses (kg)
l1, l2 = 0.5, 0.4      # link lengths (m), masses at link tips
g_acc  = 9.81

def rnea_planar_2link(q, dq, ddq):
    """Recursive Newton-Euler for a 2R planar arm, point masses at tips.
    Returns joint torques tau = [tau1, tau2]."""
    q1, q2 = q
    # Absolute link angles in the plane.
    th1 = q1
    th2 = q1 + q2
    w1  = dq[0]                 # absolute angular velocities (scalar, planar)
    w2  = dq[0] + dq[1]
    a1  = ddq[0]               # absolute angular accelerations
    a2  = ddq[0] + ddq[1]

    # Position of each point mass relative to its joint.
    p1 = l1 * np.array([np.cos(th1), np.sin(th1)])
    p2 = l2 * np.array([np.cos(th2), np.sin(th2)])

    def acc(joint_pos_accel, w, a, p):
        # Linear accel of a tip mass = joint accel + tangential + centripetal.
        perp = np.array([-p[1], p[0]])       # z x p
        return joint_pos_accel + a * perp - (w**2) * p

    def cross2(r, f):                         # scalar z-component of r x f
        return r[0] * f[1] - r[1] * f[0]

    # Forward sweep: accel of joint 1 origin includes gravity as a fictitious accel.
    grav = np.array([0.0, g_acc])            # subtract gravity => add to base accel
    a_base = grav                            # base origin "accelerates" up at g
    ac1 = acc(a_base, w1, a1, p1)            # accel of mass 1
    a_j2 = acc(a_base, w1, a1, p1)           # accel of joint-2 origin (= tip of link1)
    ac2 = acc(a_j2, w2, a2, p2)              # accel of mass 2

    # Backward sweep: forces (planar, 2D) and torques (scalar z-component).
    f2 = m2 * ac2
    n2 = cross2(p2, f2)                      # torque about joint 2
    f1 = m1 * ac1 + f2
    n1 = cross2(p1, m1 * ac1) + cross2(p1, f2) + n2

    return np.array([n1, n2])

def manipulator_torque(q, dq, ddq):
    """Independent manipulator-equation torque for cross-checking."""
    q2 = q[1]; c2, s2 = np.cos(q2), np.sin(q2)
    M = np.array([[m1*l1**2 + m2*(l1**2 + l2**2 + 2*l1*l2*c2), m2*(l2**2 + l1*l2*c2)],
                  [m2*(l2**2 + l1*l2*c2),                       m2*l2**2]])
    h = -m2*l1*l2*s2
    C = np.array([[h*dq[1], h*(dq[0]+dq[1])],
                  [-h*dq[0], 0.0]])
    c1, c12 = np.cos(q[0]), np.cos(q[0]+q[1])
    g = np.array([(m1+m2)*g_acc*l1*c1 + m2*g_acc*l2*c12, m2*g_acc*l2*c12])
    return M @ ddq + C @ dq + g

q   = np.array([0.4, -0.7])
dq  = np.array([1.2,  0.9])
ddq = np.array([0.3, -0.5])

tau_rnea = rnea_planar_2link(q, dq, ddq)
tau_eq   = manipulator_torque(q, dq, ddq)
print("RNEA torque            :", tau_rnea)
print("manipulator-eq torque  :", tau_eq)
print("agree:", np.allclose(tau_rnea, tau_eq, atol=1e-9))

The forward sweep carries kinematic quantities outward and the backward sweep carries wrenches inward, so RNEA computes inverse dynamics in \(O(n)\) time without ever forming \(M\), \(C\), or \(g\) explicitly. On a 7-DOF arm, explicit inertia-matrix construction assembles and inverts a 7x7 matrix at every control step. RNEA replaces that with link-local dot products instead, which typically cuts per-step floating-point operations by roughly an order of magnitude, though the exact ratio depends on the robot's DOF and how the inertia matrix is assembled. The cross-check against the closed-form manipulator equation is consistent with the two-sweep bookkeeping being correct on this test configuration, a necessary but not sufficient check before you scale to a 7-DOF arm where no closed form is available.

Practical Recipe

With the two-sweep mechanism verified against the closed-form equation, the remaining work is procedural: turning that verified computation into a disciplined workflow you can trust on hardware.

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.
6. Close the loop the section opened: run RNEA once per waypoint of the candidate trajectory, compare each joint's \(\tau_i\) against its continuous and peak torque rating, and reject or re-time any waypoint that exceeds either limit before the trajectory reaches hardware.

Before reading the next four directions: if a robot arm picks up an unknown payload mid-task, how does it know the new inertia without stopping to run a calibration routine?

Production Pattern

From kinematics to dynamics: forces, torques, inertia 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.

Adding mass, inertia, force, torque, and actuator limits separates kinematic prediction from dynamic cause. That gives the idea an intuitive role, a formal interface, a runnable check, and a reproducible failure mode, one treatment that serves practitioners, builders, and researchers at once.

Geometric versus physical feasibility

Kinematics can tell a planner that a joint angle can move from \(q_0\) to \(q_1\). Dynamics asks whether the actuator can create the required acceleration without exceeding torque, heating, traction, or contact limits. This distinction is the difference between geometric and physical feasibility, and it is what separates a path a planner accepts from a path a motor can actually execute. The change is not cosmetic: a path that is geometrically valid may demand an impulse the robot cannot deliver, while a slower path may be executable because inertia and actuator limits are respected.

Use this recipe when turning From kinematics to dynamics: forces, torques, inertia 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.