Skip to content
LumenaryAcademy
Motion 80 min

Robot dynamics

Why kinematics is not enough — the equations of motion of an arm, derived, simulated and checked against the conservation of energy.

Code tested · 6AI-draftedAwaiting review by a roboticist

Your guides: Chloe · The Builder, Margaret · The Experimenter, Gideon · The Safety Engineer

MotivationH

Why kinematics is not enough

Everything so far has been geometry. Forward and inverse kinematics say where an arm is; the Jacobian says how fast its tool moves and what joint torques balance a steady push. None of it says what happens when the arm accelerates, or what the motors must do to hold the arm against its own weight. Those questions belong to dynamics: the relation between the torques the motors apply and the motion that results.

The answers matter at once. A palletising robot in Rotterdam swinging 30 kg boxes at two metres per second needs motors and gearboxes sized for the peak torque, which is mostly inertial, and a controller that anticipates it; a controller that only reacts to errors will always be late. A surgical or collaborative arm that knows how much torque its own motion should take can tell when something unexpected, such as a person, is pushing back. A walking robot must choose leg torques that keep it upright through each footfall. And every simulator, the place where robots are now designed, tested and trained, is a dynamics engine at its core.

This lesson derives the equations of motion of a two-link arm, simulates them and tests the simulation with the conservation of energy, adds friction and real actuators, and then explains the recursive algorithms and libraries that do all of this for robots with dozens of joints.

IntuitionH

From one link to two

Start with a single link of mass mm turning about a horizontal joint, its centre of mass a distance lcl_c from the axis, its moment of inertia about the axis IO=I+mlc2I_O = I + m l_c^2 (the parallel-axis theorem). Measure its angle qq from the horizontal. Newton's law for rotation says the motor torque must supply the angular acceleration and hold up the weight:

τ=IO q¨+mg0lccos⁡q.\tau = I_O\,\ddot q + m g_0 l_c \cos q .

Two things are already visible. The inertia term grows with acceleration, and the gravity term depends on the configuration: largest when the link is horizontal, zero when it hangs straight down.

Add a second link and three new effects appear. First, the shoulder now carries the forearm too, and how hard that is depends on the elbow: a stretched arm has more inertia about the shoulder than a folded one, just as a figure skater spins faster with her arms pulled in. So the inertia is a matrix that depends on qq. Second, the joints are coupled: accelerating the shoulder throws the forearm backwards, so even a motionless elbow must exert torque. Third, motion itself creates forces: when the shoulder spins, the forearm is flung outwards (centrifugal effects), and when both joints move, velocity-product terms appear (Coriolis effects).

m₁g₀m₂g₀q₁q₂τ₁τ₂xyhand
A free-body view of the two-link arm. Joint angles q₁ and q₂, motor torques τ₁ and τ₂ at the shoulder and elbow, and the weights of the two links acting at their centres of mass (the small circles).

Analogy, not a formal construction

Carry a full tray of tea across a room. Starting and stopping take effort even though the tray's weight never changes (inertia); swinging round a corner pushes the cups outwards (centrifugal effects); and how hard your shoulder works depends on whether your elbow is bent (a configuration-dependent inertia). A waiter's skill is an internal model of these dynamics, and a robot needs an explicit one.

DefinitionsH

Definitions

Definition— generalised coordinates and forces

The joint variables q∈Rnq \in \mathbb{R}^n are generalised coordinates: they fix the arm's configuration completely. The generalised forces τ∈Rn\tau \in \mathbb{R}^n are the joint torques (forces, for prismatic joints) that do work on them, so that the power put in by the actuators is q˙⊤τ\dot q^\top \tau.

Definition— manipulator equation

The equations of motion of a rigid open-chain arm take the form

M(q) q¨+C(q,q˙) q˙+g(q)=τ,M(q)\,\ddot q + C(q, \dot q)\,\dot q + g(q) = \tau ,

where M(q)M(q) is the n×nn \times n mass matrix (or joint-space inertia matrix), C(q,q˙)q˙C(q, \dot q)\dot q collects the Coriolis and centrifugal terms, which are quadratic in the joint velocities, and g(q)=∂V/∂qg(q) = \partial V/\partial q is the gravity vector, the gradient of the potential energy. Friction, contact forces and external loads add further terms.

Definition— Lagrangian

The Lagrangian is kinetic minus potential energy, L(q,q˙)=T(q,q˙)−V(q)\mathcal{L}(q, \dot q) = T(q, \dot q) - V(q). For a robot arm T=12q˙⊤M(q)q˙T = \tfrac12 \dot q^\top M(q) \dot q, and the equations of motion are the Euler–Lagrange equations

ddt∂L∂q˙i−∂L∂qi=τi,i=1,…,n.\frac{d}{dt}\frac{\partial \mathcal{L}}{\partial \dot q_i} - \frac{\partial \mathcal{L}}{\partial q_i} = \tau_i, \qquad i = 1, \ldots, n.

Definition— inverse and forward dynamics

Inverse dynamics computes the torques τ\tau needed for a given motion (q,q˙,q¨)(q, \dot q, \ddot q): it is used for control and for sizing motors. Forward dynamics computes the acceleration q¨=M(q)−1(τ−C(q,q˙)q˙−g(q))\ddot q = M(q)^{-1}\big(\tau - C(q, \dot q)\dot q - g(q)\big) that given torques produce: integrated over time, it is simulation.

Worked examplesH

The two-link arm, derived

Take the arm of the figure: link ii has mass mim_i, length lil_i, centre of mass a distance lcil_{ci} from its joint and moment of inertia IiI_i about its centre of mass. Angles are measured as in the earlier lessons, q1q_1 from the horizontal xx axis and q2q_2 relative to the first link, and gravity g0=9.81g_0 = 9.81 m/s² points along −y-y.

Positions and velocities. The centres of mass are at

pc1=lc1[cos⁡q1sin⁡q1],pc2=l1[cos⁡q1sin⁡q1]+lc2[cos⁡(q1+q2)sin⁡(q1+q2)],p_{c1} = l_{c1}\begin{bmatrix} \cos q_1 \\ \sin q_1 \end{bmatrix}, \qquad p_{c2} = l_1\begin{bmatrix} \cos q_1 \\ \sin q_1 \end{bmatrix} + l_{c2}\begin{bmatrix} \cos(q_1 + q_2) \\ \sin(q_1 + q_2) \end{bmatrix},

and their velocities follow by differentiation. The first link's angular velocity is q˙1\dot q_1, the second's is q˙1+q˙2\dot q_1 + \dot q_2.

Energies. The kinetic energy of each link is that of its centre of mass plus its rotation about it:

T=12m1∥p˙c1∥2+12I1q˙12+12m2∥p˙c2∥2+12I2(q˙1+q˙2)2,V=m1g0 yc1+m2g0 yc2.T = \tfrac12 m_1 \|\dot p_{c1}\|^2 + \tfrac12 I_1 \dot q_1^2 + \tfrac12 m_2 \|\dot p_{c2}\|^2 + \tfrac12 I_2 (\dot q_1 + \dot q_2)^2, \qquad V = m_1 g_0\, y_{c1} + m_2 g_0\, y_{c2}.

Expanding ∥p˙c2∥2=l12q˙12+lc22(q˙1+q˙2)2+2l1lc2cos⁡q2 q˙1(q˙1+q˙2)\|\dot p_{c2}\|^2 = l_1^2 \dot q_1^2 + l_{c2}^2(\dot q_1 + \dot q_2)^2 + 2 l_1 l_{c2} \cos q_2\, \dot q_1(\dot q_1 + \dot q_2) shows where the elbow angle enters: through the cross term between the two links' motions.

Euler–Lagrange. Applying the equations is mechanical, so let SymPy do it and print the pieces.

python
import sympy as sp

t = sp.symbols("t")
m1, m2, l1, lc1, lc2, I1, I2, g0 = sp.symbols("m1 m2 l1 lc1 lc2 I1 I2 g0", positive=True)
q1, q2 = sp.Function("q1")(t), sp.Function("q2")(t)
q, qd = sp.Matrix([q1, q2]), sp.Matrix([q1.diff(t), q2.diff(t)])

# Centres of mass of the two links (angles from the horizontal x axis; gravity along -y).
p1 = sp.Matrix([lc1 * sp.cos(q1), lc1 * sp.sin(q1)])
p2 = sp.Matrix([l1 * sp.cos(q1) + lc2 * sp.cos(q1 + q2), l1 * sp.sin(q1) + lc2 * sp.sin(q1 + q2)])
v1, v2 = p1.diff(t), p2.diff(t)

T = sp.Rational(1, 2) * (m1 * v1.dot(v1) + I1 * q1.diff(t)**2
                         + m2 * v2.dot(v2) + I2 * (q1.diff(t) + q2.diff(t))**2)
V = m1 * g0 * p1[1] + m2 * g0 * p2[1]
L = T - V

# Euler-Lagrange: d/dt dL/dqdot - dL/dq = tau
tau = sp.Matrix([sp.diff(L.diff(qd[i]), t) - L.diff(q[i]) for i in range(2)])
tau = sp.simplify(tau)

qdd = sp.Matrix([q1.diff(t, 2), q2.diff(t, 2)])
M = sp.simplify(tau.jacobian(qdd))                                # coefficients of the accelerations
G = sp.simplify(tau.subs({qdd[0]: 0, qdd[1]: 0}).subs({qd[0]: 0, qd[1]: 0}))
rest = sp.simplify(tau - M * qdd - G)                             # velocity-dependent terms

names = {q1: sp.Symbol("q1"), q2: sp.Symbol("q2")}
def show(expr):
    expr = expr.subs({q1.diff(t): sp.Symbol("qd1"), q2.diff(t): sp.Symbol("qd2")}).subs(names)
    return sp.simplify(expr)

for (i, j) in [(0, 0), (0, 1), (1, 1)]:
    print(f"M{i+1}{j+1} =", show(M[i, j]))
print("M12 == M21:", sp.simplify(M[0, 1] - M[1, 0]) == 0)
print("velocity terms, row 1:", sp.factor(show(rest[0])))
print("velocity terms, row 2:", sp.factor(show(rest[1])))
print("gravity, row 1:", show(G[0]))
print("gravity, row 2:", show(G[1]))

# The energy property: qd^T (Mdot/2) qd equals qd^T (C qd), so the velocity terms do no net work.
power_check = sp.simplify((qd.T * (M.diff(t) * qd / 2 - rest))[0])
print("qd^T (Mdot/2 - C) qd =", power_check)
Runs entirely in your browser, in an isolated worker. Nothing is sent to a server.

Written out, with h=m2l1lc2sin⁡q2h = m_2 l_1 l_{c2} \sin q_2:

M(q)=[I1+I2+m1lc12+m2(l12+lc22+2l1lc2cos⁡q2)I2+m2(lc22+l1lc2cos⁡q2)I2+m2(lc22+l1lc2cos⁡q2)I2+m2lc22],M(q) = \begin{bmatrix} I_1 + I_2 + m_1 l_{c1}^2 + m_2\left(l_1^2 + l_{c2}^2 + 2 l_1 l_{c2}\cos q_2\right) & I_2 + m_2\left(l_{c2}^2 + l_1 l_{c2}\cos q_2\right) \\ I_2 + m_2\left(l_{c2}^2 + l_1 l_{c2}\cos q_2\right) & I_2 + m_2 l_{c2}^2 \end{bmatrix},C(q,q˙) q˙=[−h q˙2 (2q˙1+q˙2)h q˙12],g(q)=g0[(m1lc1+m2l1)cos⁡q1+m2lc2cos⁡(q1+q2)m2lc2cos⁡(q1+q2)].C(q, \dot q)\,\dot q = \begin{bmatrix} -h\,\dot q_2\,(2\dot q_1 + \dot q_2) \\ h\,\dot q_1^2 \end{bmatrix}, \qquad g(q) = g_0 \begin{bmatrix} (m_1 l_{c1} + m_2 l_1)\cos q_1 + m_2 l_{c2}\cos(q_1 + q_2) \\ m_2 l_{c2}\cos(q_1 + q_2) \end{bmatrix}.

Every term has a story. M11M_{11} is largest when the arm is stretched (cos⁡q2=1\cos q_2 = 1) and smallest when folded: the figure skater. M12M_{12} is the coupling: accelerating one joint loads the other. In the velocity terms, hq˙12h\dot q_1^2 is the centrifugal torque that tries to straighten the elbow when the shoulder spins, and −2hq˙1q˙2-2h\dot q_1 \dot q_2 is the Coriolis term. The gravity vector is the static torque, exactly what the previous lesson's τ=J⊤F\tau = J^\top F gives for the weights mig0m_i g_0 acting at the centres of mass.

Example— Holding the arm out

Use uniform rods: l1=0.4l_1 = 0.4 m and m1=2m_1 = 2 kg, l2=0.3l_2 = 0.3 m and m2=1m_2 = 1 kg, so lci=li/2l_{ci} = l_i/2 and Ii=mili2/12I_i = m_i l_i^2/12. Stretched out horizontally (q=0q = 0), the holding torques are g1=9.81 (2⋅0.2+1⋅0.4+1⋅0.15)=9.32g_1 = 9.81\,(2 \cdot 0.2 + 1 \cdot 0.4 + 1 \cdot 0.15) = 9.32 N·m and g2=9.81⋅1⋅0.15=1.47g_2 = 9.81 \cdot 1 \cdot 0.15 = 1.47 N·m. The mass matrix is M11=0.4167M_{11} = 0.4167 kg·m² stretched and 0.17670.1767 kg·m² folded: the shoulder's effective inertia changes by a factor of 2.42.4 with the elbow alone, which is why a controller with fixed gains behaves differently across the workspace.

Theorem— Structure of the manipulator equation

For a rigid open-chain arm:

  1. M(q)M(q) is symmetric and positive definite for every qq.
  2. With CC built from the Christoffel symbols of MM, which for the two-link arm gives C=h[−q˙2−(q˙1+q˙2)q˙10]C = h\begin{bmatrix} -\dot q_2 & -(\dot q_1 + \dot q_2) \\ \dot q_1 & 0 \end{bmatrix}, the matrix M˙−2C\dot M - 2C is skew-symmetric; in particular q˙⊤(12M˙−C)q˙=0\dot q^\top\big(\tfrac12\dot M - C\big)\dot q = 0.
  3. The total energy E=12q˙⊤Mq˙+V(q)E = \tfrac12\dot q^\top M \dot q + V(q) changes at the rate E˙=q˙⊤τ\dot E = \dot q^\top \tau: with no torque and no friction, energy is conserved.

Proof

(1) T=12q˙⊤Mq˙T = \tfrac12\dot q^\top M \dot q is a sum of non-negative link energies, and it is zero only if every link is at rest, which for an open chain means q˙=0\dot q = 0. A quadratic form that is positive for every nonzero q˙\dot q has a positive definite matrix, and MM may be taken symmetric because only its symmetric part enters the form.

(3), assuming (2): differentiate the energy along a motion and substitute Mq¨=τ−Cq˙−gM\ddot q = \tau - C\dot q - g, remembering ∂V/∂q=g\partial V/\partial q = g:

E˙=q˙⊤Mq¨+12q˙⊤M˙q˙+q˙⊤g=q˙⊤τ+q˙⊤(12M˙−C)q˙=q˙⊤τ.\dot E = \dot q^\top M\ddot q + \tfrac12 \dot q^\top \dot M \dot q + \dot q^\top g = \dot q^\top \tau + \dot q^\top\big(\tfrac12\dot M - C\big)\dot q = \dot q^\top \tau .

(2) For the two-link arm, the last line of the SymPy output checks q˙⊤(12M˙−C)q˙=0\dot q^\top(\tfrac12\dot M - C)\dot q = 0 directly. In general it follows from writing Cij=∑k12(∂kMij+∂jMik−∂iMjk)q˙kC_{ij} = \sum_k \tfrac12\left(\partial_k M_{ij} + \partial_j M_{ik} - \partial_i M_{jk}\right)\dot q_k and checking that (M˙−2C)ij=∑k(∂iMjk−∂jMik)q˙k(\dot M - 2C)_{ij} = \sum_k (\partial_i M_{jk} - \partial_j M_{ik})\dot q_k, which changes sign when ii and jj are swapped.

∎

ComputationC

Simulating the arm, and testing the simulation

Simulation is forward dynamics integrated in time. The state is (q,q˙)(q, \dot q); at each step, compute q¨=M−1(τ−Cq˙−g)\ddot q = M^{-1}(\tau - C\dot q - g) and advance. The obvious method, explicit Euler, updates both position and velocity from the old state. Semi-implicit (symplectic) Euler first updates the velocity, then uses the new velocity to update the position; it costs the same, and it is the default in many physics engines, MuJoCo's among them. RK4, the classical fourth-order Runge–Kutta method from ordinary differential equations, evaluates the dynamics four times per step and combines the results.

Release the frictionless arm with no motor torque. Energy must then stay exactly constant, so any change is pure numerical error. Two scenarios: a gentle swing near hanging straight down (0.530.53 J of swing energy), and a violent fall from the horizontal (9.329.32 J).

python
import numpy as np

# Two uniform rods: the shoulder link 0.4 m and 2 kg, the forearm 0.3 m and 1 kg.
l1, l2, m1, m2, g0 = 0.4, 0.3, 2.0, 1.0, 9.81
lc1, lc2 = l1 / 2, l2 / 2
I1, I2 = m1 * l1**2 / 12, m2 * l2**2 / 12

def M(q):
    c2 = np.cos(q[1])
    m11 = I1 + I2 + m1 * lc1**2 + m2 * (l1**2 + lc2**2 + 2 * l1 * lc2 * c2)
    m12 = I2 + m2 * (lc2**2 + l1 * lc2 * c2)
    return np.array([[m11, m12], [m12, I2 + m2 * lc2**2]])

def Cqd(q, qd):                                   # the Coriolis and centrifugal terms C(q, qd) qd
    h = m2 * l1 * lc2 * np.sin(q[1])
    return np.array([-h * qd[1] * (2 * qd[0] + qd[1]), h * qd[0]**2])

def gvec(q):
    return g0 * np.array([(m1 * lc1 + m2 * l1) * np.cos(q[0]) + m2 * lc2 * np.cos(q[0] + q[1]),
                          m2 * lc2 * np.cos(q[0] + q[1])])

def energy(q, qd):
    y1 = lc1 * np.sin(q[0])
    y2 = l1 * np.sin(q[0]) + lc2 * np.sin(q[0] + q[1])
    return 0.5 * qd @ M(q) @ qd + g0 * (m1 * y1 + m2 * y2)

def accel(q, qd, tau=np.zeros(2)):                # forward dynamics
    return np.linalg.solve(M(q), tau - Cqd(q, qd) - gvec(q))

def explicit_euler(q, qd, h):
    return q + h * qd, qd + h * accel(q, qd)

def semi_implicit_euler(q, qd, h):                # update velocity first, then position with it
    qd_new = qd + h * accel(q, qd)
    return q + h * qd_new, qd_new

def rk4(q, qd, h):
    def f(x):
        return np.concatenate([x[2:], accel(x[:2], x[2:])])
    x = np.concatenate([q, qd])
    k1 = f(x); k2 = f(x + h / 2 * k1); k3 = f(x + h / 2 * k2); k4 = f(x + h * k3)
    x = x + h / 6 * (k1 + 2 * k2 + 2 * k3 + k4)
    return x[:2], x[2:]

scenarios = {"gentle swing": np.array([-np.pi / 2 + 0.3, 0.2]),     # hanging down, nudged
             "violent fall": np.array([0.0, 0.0])}                 # released stretched out horizontally
print("No friction and no torque: the energy should stay exactly constant for 10 s.")
print(f"{'':28}  worst energy error (J)")
print(f"{'integrator':20} {'step':>7}  {'gentle swing':>12} {'violent fall':>13}")
for name, step in [("explicit Euler", explicit_euler), ("semi-implicit Euler", semi_implicit_euler), ("RK4", rk4)]:
    for h in (0.01, 0.001):
        worst = {}
        for label, q0 in scenarios.items():
            q, qd = q0.copy(), np.zeros(2)
            E0 = energy(q, qd)
            worst[label] = 0.0
            for _ in range(int(round(10.0 / h))):
                q, qd = step(q, qd, h)
                worst[label] = max(worst[label], abs(energy(q, qd) - E0))
        print(f"{name:20} {h * 1000:4.0f} ms  {worst['gentle swing']:12.1e} {worst['violent fall']:13.1e}")
Runs entirely in your browser, in an isolated worker. Nothing is sent to a server.

The table teaches three lessons. Explicit Euler steadily pumps energy into oscillating systems and should not be used for mechanics. Semi-implicit Euler is symplectic for systems like a single pendulum, where it keeps the energy error bounded instead of letting it grow; our arm's mass matrix depends on qq, so in these variables it is only approximately so, and on the violent fall at 1010 ms its error reaches 8.38.3 J, as large as the energy itself. RK4 is far more accurate per step but not symplectic: over very long runs its tiny error drifts steadily in one direction. Physics engines choose by circumstance: small steps of semi-implicit or implicit methods where contacts and stiff springs dominate (contacts make high-order methods lose their advantage), higher-order methods for smooth free flight, and always an energy or momentum check in the test suite.

ExplanationH

Newton–Euler, and the recursive algorithms

Lagrange's method is elegant for two links and hopeless for thirty: the symbolic expressions explode. The alternative is Newton–Euler: write Newton's law f=macf = m a_c and Euler's law n=Iα+ω×Iωn = I\alpha + \omega \times I\omega for each link separately, and connect the links through the forces they exert on each other at the joints. Both methods describe the same physics, so they give identical torques (Silver proved the equivalence in 1982), but Newton–Euler organises the computation as two sweeps along the chain.

baselink 1link 2…link nforward pass: velocities and accelerations, base → tipbackward pass: forces and moments, tip → base; τᵢ at each joint
The recursive Newton–Euler algorithm. A forward pass carries velocities and accelerations from the base to the tip; a backward pass carries forces and moments from the tip to the base, and each joint torque is read off on the way.

The recursive Newton–Euler algorithm (RNEA), published by Luh, Walker and Paul in 1980, computes inverse dynamics like this:

  1. Forward pass. Starting from the base, compute each link's angular velocity and acceleration and the linear acceleration of its centre of mass from those of the previous link plus its own joint's motion. A neat trick includes gravity: give the base a fictitious upward acceleration g0g_0, and every link feels its weight automatically.
  2. Backward pass. Starting from the tip, the force a joint must transmit is what its link needs for its own acceleration plus what it passes on to the next link; likewise for moments. The torque about each joint's axis is the joint torque.

Each link is visited twice, so the cost is O(n)O(n): about as fast as a dynamics computation can be. The code implements RNEA for a planar chain in a dozen lines and checks it against the Lagrangian closed form.

python
import numpy as np

g0 = 9.81
# Link data: length, mass, centre-of-mass distance from its joint, inertia about the centre of mass.
L  = np.array([0.4, 0.3])
m  = np.array([2.0, 1.0])
lc = L / 2
I  = m * L**2 / 12

def perp(v):                                    # rotate a 2-D vector by +90 degrees (omega x v)
    return np.array([-v[1], v[0]])

def cross2(a, b):                               # z-component of a 3-D cross product
    return a[0] * b[1] - a[1] * b[0]

def rnea(q, qd, qdd):
    """Recursive Newton-Euler for a planar chain of revolute joints: returns the joint torques."""
    n = len(q)
    th, w, al = np.cumsum(q), np.cumsum(qd), np.cumsum(qdd)   # absolute angle, rate, acceleration
    u = np.column_stack([np.cos(th), np.sin(th)])              # unit vector along each link
    a_joint = np.array([0.0, g0])               # gravity, as an upward acceleration of the base
    a_com = np.zeros((n, 2))
    for i in range(n):                          # forward pass: accelerations, base to tip
        r_c, r_l = lc[i] * u[i], L[i] * u[i]
        a_com[i] = a_joint + al[i] * perp(r_c) - w[i]**2 * r_c
        a_joint = a_joint + al[i] * perp(r_l) - w[i]**2 * r_l
    tau = np.zeros(n)
    f_next, n_next = np.zeros(2), 0.0           # force and moment from the next link (none at the tip)
    for i in reversed(range(n)):                # backward pass: forces, tip to base
        f = m[i] * a_com[i] + f_next            # Newton: what joint i must supply
        r_from_joint = lc[i] * u[i]             # joint i -> centre of mass
        r_to_next = (L[i] - lc[i]) * u[i]       # centre of mass -> joint i+1
        # Euler: moments about the centre of mass
        n_i = I[i] * al[i] + n_next + cross2(r_from_joint, f) + cross2(r_to_next, f_next)
        tau[i] = n_i
        f_next, n_next = f, n_i
    return tau

def lagrange(q, qd, qdd):                       # the closed form derived with SymPy
    l1, lc1, lc2, m1, m2, I1, I2 = L[0], lc[0], lc[1], m[0], m[1], I[0], I[1]
    c2, s2 = np.cos(q[1]), np.sin(q[1])
    M = np.array([[I1 + I2 + m1 * lc1**2 + m2 * (l1**2 + lc2**2 + 2 * l1 * lc2 * c2),
                   I2 + m2 * (lc2**2 + l1 * lc2 * c2)],
                  [I2 + m2 * (lc2**2 + l1 * lc2 * c2), I2 + m2 * lc2**2]])
    h = m2 * l1 * lc2 * s2
    Cqd = np.array([-h * qd[1] * (2 * qd[0] + qd[1]), h * qd[0]**2])
    g = g0 * np.array([(m1 * lc1 + m2 * l1) * np.cos(q[0]) + m2 * lc2 * np.cos(q[0] + q[1]),
                       m2 * lc2 * np.cos(q[0] + q[1])])
    return M @ qdd + Cqd + g

rng = np.random.default_rng(0)
worst = max(np.abs(rnea(*s) - lagrange(*s)).max()
            for s in (rng.uniform(-3, 3, (3, 2)) for _ in range(1000)))
print(f"RNEA vs Lagrange on 1000 random states: largest difference {worst:.1e} N m")

q = np.radians([0.0, 0.0])
print("holding torques, arm horizontal:", np.round(rnea(q, np.zeros(2), np.zeros(2)), 3), "N m")

# The mass matrix from n calls of inverse dynamics: column j = rnea(q, 0, e_j) - rnea(q, 0, 0).
def mass_matrix(q):
    g = rnea(q, np.zeros(2), np.zeros(2))
    return np.column_stack([rnea(q, np.zeros(2), e) - g for e in np.eye(2)])
for deg in (0, 90, 180):
    print(f"M(q2 = {deg:3d} deg) =", np.round(mass_matrix(np.radians([30.0, deg])), 4).tolist())
Runs entirely in your browser, in an isolated worker. Nothing is sent to a server.

The two methods agree to about 10−1510^{-15} N·m, rounding error. The last lines show a trick from Walker and Orin (1982): since τ=Mq¨+Cq˙+g\tau = M\ddot q + C\dot q + g is linear in q¨\ddot q, calling inverse dynamics with q˙=0\dot q = 0 and q¨\ddot q a unit vector, and subtracting the gravity torques, yields one column of MM. Doing that nn times costs O(n2)O(n^2); their composite-rigid-body algorithm (CRBA) builds MM in O(n2)O(n^2) more cleverly, and forward dynamics then needs a Cholesky factorisation, O(n3)O(n^3).

For long chains Featherstone's articulated-body algorithm (ABA, 1983) does forward dynamics in O(n)O(n) directly, with three sweeps: velocities outwards, "articulated" inertias inwards (the inertia each subtree presents to its parent joint when its own joints are free to move), accelerations outwards. The practical crossover depends on the robot: for a six-axis arm CRBA plus Cholesky is often as fast, and for a humanoid with 30 or more joints or a long chain ABA wins. All three are written most compactly in Featherstone's spatial vector algebra, which packs angular and linear quantities into 6-D vectors, the twists and wrenches of the previous lesson.

ExplanationH

Friction, gears and actuators

The rigid-body model is only part of a real joint. Between the motor and the link sit bearings, a gearbox and often a torque sensor, and they add effects of their own.

Friction. The simplest useful model has a viscous part proportional to speed and a Coulomb part of constant size opposing motion, τf=b q˙+τc sign⁡(q˙)\tau_f = b\,\dot q + \tau_c\,\operatorname{sign}(\dot q). Real joints add static friction (more torque to start moving than to keep moving) and the Stribeck effect (friction that dips at low speed before rising), and their parameters change with temperature, load and wear. In geared joints friction can be several percent of the rated torque, enough to dominate slow, precise motions. The discontinuity at q˙=0\dot q = 0 is a headache for simulation; a common fix is to smooth sign⁡\operatorname{sign} with a steep tanh⁡\tanh, as in the code below.

Gears and reflected inertia. Electric motors are fast and weak, so arms use gearboxes, often of ratio 50:1 to 160:1 in industrial arms, with harmonic drives or cycloidal reducers. A gearbox of ratio NN multiplies the motor torque by NN (minus losses), divides speed by NN, and makes the motor's rotor inertia JmJ_m appear at the joint as N2JmN^2 J_m, the reflected inertia: the rotor spins NN times faster, so it stores N2N^2 times the kinetic energy per unit of joint speed. The reflected inertia adds to the diagonal of M(q)M(q):

(M(q)+N2Jm)q¨+Cq˙+g+τf=Nkt i,\big(M(q) + N^2 J_m\big)\ddot q + C\dot q + g + \tau_f = N k_t\, i ,

where ktk_t is the motor's torque constant and ii its current. The motor itself, and the power electronics that drive its current, are the subject of Lumenary EE's magnetism and motors and power electronics lessons.

python
import numpy as np

l1, l2, m1, m2, g0 = 0.4, 0.3, 2.0, 1.0, 9.81
lc1, lc2 = l1 / 2, l2 / 2
I1, I2 = m1 * l1**2 / 12, m2 * l2**2 / 12

def M(q):
    c2 = np.cos(q[1])
    m11 = I1 + I2 + m1 * lc1**2 + m2 * (l1**2 + lc2**2 + 2 * l1 * lc2 * c2)
    m12 = I2 + m2 * (lc2**2 + l1 * lc2 * c2)
    return np.array([[m11, m12], [m12, I2 + m2 * lc2**2]])

def Cqd(q, qd):
    h = m2 * l1 * lc2 * np.sin(q[1])
    return np.array([-h * qd[1] * (2 * qd[0] + qd[1]), h * qd[0]**2])

def gvec(q):
    return g0 * np.array([(m1 * lc1 + m2 * l1) * np.cos(q[0]) + m2 * lc2 * np.cos(q[0] + q[1]),
                          m2 * lc2 * np.cos(q[0] + q[1])])

def energy(q, qd):
    return 0.5 * qd @ M(q) @ qd + g0 * (m1 * lc1 * np.sin(q[0])
                                        + m2 * (l1 * np.sin(q[0]) + lc2 * np.sin(q[0] + q[1])))

def friction(qd, b=0.02, tc=0.05, v_eps=0.01):
    """Viscous plus Coulomb friction (N m); tanh smooths the jump of Coulomb friction at zero speed."""
    return b * qd + tc * np.tanh(qd / v_eps)

def rk4(q, qd, h, rotor=np.zeros(2)):
    def f(x):
        q, qd = x[:2], x[2:]
        qdd = np.linalg.solve(M(q) + np.diag(rotor), -Cqd(q, qd) - gvec(q) - friction(qd))
        return np.concatenate([qd, qdd])
    x = np.concatenate([q, qd])
    k1 = f(x); k2 = f(x + h / 2 * k1); k3 = f(x + h / 2 * k2); k4 = f(x + h * k3)
    x = x + h / 6 * (k1 + 2 * k2 + 2 * k3 + k4)
    return x[:2], x[2:]

# 1. A frictional arm released from rest, nudged away from hanging straight down.
q, qd, h = np.array([-np.pi / 2 + 0.6, 0.4]), np.zeros(2), 0.001
E_rest = energy(np.array([-np.pi / 2, 0.0]), np.zeros(2))
E, rises = [energy(q, qd) - E_rest], 0
for n in range(1, 6001):
    q, qd = rk4(q, qd, h)
    E.append(energy(q, qd) - E_rest)
    rises += E[-1] > E[-2] + 1e-12
print("energy above rest (J) at t = 0, 1, ..., 6 s:", [round(float(E[k]), 4) for k in range(0, 6001, 1000)])
print("steps in which the energy rose:", rises)

# 2. Reflected inertia: a motor rotor behind a gearbox of ratio N looks N^2 times heavier at the joint.
J_rotor, N = 3.0e-5, 100                        # kg m^2, and a 100:1 gearbox
print(f"\nrotor inertia {J_rotor:.0e} kg m^2 behind {N}:1 gearing -> {J_rotor * N**2:.2f} kg m^2 at the joint")
for deg in (0, 180):
    Mq = M(np.radians([0.0, deg]))
    print(f"q2 = {deg:3d} deg: arm M11 = {Mq[0, 0]:.4f}, M22 = {Mq[1, 1]:.4f};"
          f"  with rotors M11 = {Mq[0, 0] + J_rotor * N**2:.4f}, M22 = {Mq[1, 1] + J_rotor * N**2:.4f}")
Runs entirely in your browser, in an isolated worker. Nothing is sent to a server.

With friction the energy falls from 2.052.05 J to 0.920.92 J in six seconds and never rises in any of the 6000 steps, as the theorem predicts: E˙=−q˙⊤τf≤0\dot E = -\dot q^\top \tau_f \le 0, because friction always opposes motion. The second part is striking. A rotor of only 3×10−53 \times 10^{-5} kg·m² behind a 100:1 gearbox presents 0.300.30 kg·m² at the joint, ten times the forearm's own M22=0.03M_{22} = 0.03. With it, M11M_{11} varies between 0.480.48 and 0.720.72 (a factor of 1.51.5) instead of between 0.180.18 and 0.420.42 (a factor of 2.42.4). Heavily geared arms are therefore dominated by their motors' inertia, their joints are nearly decoupled, and simple independent-joint controllers work well on them, which is the starting point of the next level, feedback and PID.

The same effect cuts the other way for robots that must touch the world gently or absorb impacts. Reflected inertia makes a geared joint hard to push back by hand and makes collisions harsher. Legged robots therefore often use quasi-direct-drive actuators, large-diameter motors with ratios below about 10:1, as the MIT Cheetah work explains; collaborative arms add joint torque sensing and, in some designs, deliberate elasticity in series with the gearbox.

ExplanationH

Dynamics libraries and how they are used

Nobody writes RNEA by hand for a real robot. Libraries read a model description (URDF, MJCF or SDF files listing links, joints, masses and inertias) and provide the algorithms, usually with their derivatives.

  • Pinocchio implements RNEA, CRBA and ABA and, following Carpentier and Mansard (2018), their analytical derivatives, which optimisation-based controllers need.
  • Drake provides MultibodyPlant with CalcInverseDynamics, CalcMassMatrix and full simulation with contact.
  • MuJoCo computes dynamics in generalised coordinates with a soft-contact model, offering mj_forward, mj_inverse and several integrators; MJX runs it on accelerators for large batches.
  • RBDL and KDL are older, widely used C++ libraries implementing the same recursive algorithms.

With Pinocchio the three algorithms are one call each. This is shown, not run here: it needs Pinocchio and a robot model.

python
import numpy as np
import pinocchio as pin

model = pin.buildModelFromUrdf("my_arm.urdf")
data = model.createData()
q = pin.neutral(model)
v = np.zeros(model.nv)
a = np.zeros(model.nv)

tau_g = pin.rnea(model, data, q, v, a)      # inverse dynamics with v = a = 0: the gravity torques
M = pin.crba(model, data, q)                # mass matrix (check the docs: some versions fill one triangle)
ddq = pin.aba(model, data, q, v, tau_g)     # forward dynamics: holding torques give zero acceleration

What the model is used for:

  • Gravity compensation: commanding τ=g(q)\tau = g(q) makes an arm float, so it can be guided by hand. Industrial arms carry brakes that hold the arm whenever power is off, because without compensation a powerless arm falls under its own weight.
  • Computed-torque and model-based control: τ=M(q) v+Cq˙+g\tau = M(q)\,v + C\dot q + g with vv chosen by feedback cancels the nonlinear dynamics, turning the arm into a set of double integrators; the Control level, especially force and impedance control and model predictive control, builds on this.
  • Collision detection: comparing measured torques (or motor currents) with the model's prediction, often through a "momentum observer" that uses property 3, reveals unexpected external forces within milliseconds.
  • Simulation and learning: simulators integrate forward dynamics with contact thousands of times faster than real time, which is how robot policies are now trained (see simulation and sim-to-real transfer).
  • System identification: the manipulator equation is linear in the mass parameters (masses, first moments, inertia entries), τ=Y(q,q˙,q¨) π\tau = Y(q, \dot q, \ddot q)\,\pi, so the parameters of a real arm can be estimated by least squares from recorded motions, with friction terms alongside.

The mathematics beneath all of this, Lagrangian mechanics and its geometric form, is developed in Lumenary Math's classical mechanics; numerical integration in numerical methods.

Common misconceptionsH

Common misconceptions

Misconception

"The Coriolis term has to do with the Earth's rotation." In robot dynamics it is a velocity-product term of the arm's own motion: when two joints move together, the moving frame of one link makes the other experience an apparent force. The Earth's rotation is negligible for robot arms.

Misconception

"At low speed dynamics does not matter, so kinematics is enough." The velocity terms fade at low speed, but gravity does not: a slow arm still needs g(q)g(q), and getting it wrong shows up as sagging and position error. And "slow" is relative: a 30 kg payload changes the gravity torques of every joint.

Misconception

"A smaller time step, or RK4, makes a simulation trustworthy." Accuracy per step is not the same as faithful long-run behaviour. RK4 drifts slowly in energy; explicit Euler gains energy at any step size; contacts and friction introduce discontinuities that defeat high-order methods. Test a simulator with invariants (energy, momentum) and with cases whose answer you know.

Misconception

"Lagrange and Newton–Euler give different equations." They are two ways of organising the same physics and give identical torques. Lagrange gives closed-form insight for small systems; recursive Newton–Euler gives fast numerical evaluation for any size.

Exercises

Exercises

1. Robot dynamics

Warm-upHuman

A two-link arm is held still, stretched out horizontally (q1=q2=0q_1 = q_2 = 0). Link 1 has mass m1=3m_1 = 3 kg, length l1=0.5l_1 = 0.5 m and its centre of mass at lc1=0.25l_{c1} = 0.25 m; link 2 has m2=2m_2 = 2 kg and its centre of mass lc2=0.2l_{c2} = 0.2 m beyond the elbow. With g0=9.81g_0 = 9.81 m/s², what torque must the shoulder motor supply? Give the answer to the nearest N·m.

2. Robot dynamics

CoreHuman

Which statements about the mass matrix M(q)M(q) of the two-link planar arm are true? Select all that apply.

Choose all that apply

3. Robot dynamics

CoreHuman

A joint is driven by a motor through a gearbox of ratio 100:1 (the motor turns 100 times for each turn of the joint). Ignoring friction and the gearbox's own inertia, by what factor does the gearbox multiply the rotor's moment of inertia as felt at the joint?

4. Robot dynamics

CoreComputational

The starter advances a 0.50.5 m pendulum, θ¨=−(g0/l)sin⁡θ\ddot\theta = -(g_0/l)\sin\theta (with θ=0\theta = 0 hanging down), by explicit Euler. The tests swing it for 200 simulated seconds at h=0.01h = 0.01 s and check that its energy stays within 3 % of the swing energy, which explicit Euler cannot do.

Change step to semi-implicit Euler: update the angular velocity with the acceleration at the old angle, then update the angle with the new angular velocity. One evaluation of the acceleration per step, as before.

Runs entirely in your browser, in an isolated worker. Nothing is sent to a server.

5. Robot dynamics

StretchComputational

inverse_dynamics(q, qd, qdd) should return τ=M(q)q¨+C(q,q˙)q˙+g(q)\tau = M(q)\ddot q + C(q, \dot q)\dot q + g(q) for the lesson's two-link arm (l1=0.4l_1 = 0.4 m, l2=0.3l_2 = 0.3 m, m1=2m_1 = 2 kg, m2=1m_2 = 1 kg, uniform rods). The starter has MM and gg right but forgets the Coriolis and centrifugal terms, so its holding torques are correct but its torques for moving arms are not.

Complete it. The tests use the energy theorem: along any motion, E˙=q˙⊤τ\dot E = \dot q^\top \tau, which holds only if all three terms are right.

Runs entirely in your browser, in an isolated worker. Nothing is sent to a server.

6. Robot dynamics

Warm-upHuman

Which algorithm, published by Roy Featherstone in 1983, computes the forward dynamics of a chain of nn bodies in O(n)O(n) time, without forming the mass matrix? Give its name (or its usual three-letter abbreviation).

7. Robot dynamics

CoreHuman

A team needs joint torques for a 30-joint humanoid model at 1 kHz, from measured positions, velocities and desired accelerations. Which statement is right?

Choose one

Further reading and resources