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

A Careful Control Loop

This section develops the technical contract for From kinematics to dynamics: forces, torques, inertia 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 From kinematics to dynamics: forces, torques, inertia 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 From kinematics to dynamics: forces, torques, inertia, 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.

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.

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 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. The cross-check against the closed-form manipulator equation is the evidence that the two-sweep bookkeeping is correct before scaling to a 7-DOF arm where no closed form is available.

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

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.

Separate kinematic prediction from dynamic cause by adding mass, inertia, force, torque, and actuator limits. 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.

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. 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.

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