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.
From one link to two
Start with a single link of mass turning about a horizontal joint, its centre of mass a distance from the axis, its moment of inertia about the axis (the parallel-axis theorem). Measure its angle from the horizontal. Newton's law for rotation says the motor torque must supply the angular acceleration and hold up the weight:
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 . 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).
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.
Definitions
Definition— generalised coordinates and forces
The joint variables are generalised coordinates: they fix the arm's configuration completely. The generalised forces are the joint torques (forces, for prismatic joints) that do work on them, so that the power put in by the actuators is .
Definition— manipulator equation
The equations of motion of a rigid open-chain arm take the form
where is the mass matrix (or joint-space inertia matrix), collects the Coriolis and centrifugal terms, which are quadratic in the joint velocities, and 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, . For a robot arm , and the equations of motion are the Euler–Lagrange equations
Definition— inverse and forward dynamics
Inverse dynamics computes the torques needed for a given motion : it is used for control and for sizing motors. Forward dynamics computes the acceleration that given torques produce: integrated over time, it is simulation.
The two-link arm, derived
Take the arm of the figure: link has mass , length , centre of mass a distance from its joint and moment of inertia about its centre of mass. Angles are measured as in the earlier lessons, from the horizontal axis and relative to the first link, and gravity m/s² points along .
Positions and velocities. The centres of mass are at
and their velocities follow by differentiation. The first link's angular velocity is , the second's is .
Energies. The kinetic energy of each link is that of its centre of mass plus its rotation about it:
Expanding 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.
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)Written out, with :
Every term has a story. is largest when the arm is stretched () and smallest when folded: the figure skater. is the coupling: accelerating one joint loads the other. In the velocity terms, is the centrifugal torque that tries to straighten the elbow when the shoulder spins, and is the Coriolis term. The gravity vector is the static torque, exactly what the previous lesson's gives for the weights acting at the centres of mass.
Example— Holding the arm out
Use uniform rods: m and kg, m and kg, so and . Stretched out horizontally (), the holding torques are N·m and N·m. The mass matrix is kg·m² stretched and kg·m² folded: the shoulder's effective inertia changes by a factor of 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:
- is symmetric and positive definite for every .
- With built from the Christoffel symbols of , which for the two-link arm gives , the matrix is skew-symmetric; in particular .
- The total energy changes at the rate : with no torque and no friction, energy is conserved.
Proof
(1) 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 . A quadratic form that is positive for every nonzero has a positive definite matrix, and may be taken symmetric because only its symmetric part enters the form.
(3), assuming (2): differentiate the energy along a motion and substitute , remembering :
(2) For the two-link arm, the last line of the SymPy output checks directly. In general it follows from writing and checking that , which changes sign when and are swapped.
∎
Simulating the arm, and testing the simulation
Simulation is forward dynamics integrated in time. The state is ; at each step, compute 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 ( J of swing energy), and a violent fall from the horizontal ( J).
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}")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 , so in these variables it is only approximately so, and on the violent fall at ms its error reaches 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.
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 and Euler's law 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.
The recursive Newton–Euler algorithm (RNEA), published by Luh, Walker and Paul in 1980, computes inverse dynamics like this:
- 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 , and every link feels its weight automatically.
- 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 : 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.
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())The two methods agree to about N·m, rounding error. The last lines show a trick from Walker and Orin (1982): since is linear in , calling inverse dynamics with and a unit vector, and subtracting the gravity torques, yields one column of . Doing that times costs ; their composite-rigid-body algorithm (CRBA) builds in more cleverly, and forward dynamics then needs a Cholesky factorisation, .
For long chains Featherstone's articulated-body algorithm (ABA, 1983) does forward dynamics in 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.
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, . 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 is a headache for simulation; a common fix is to smooth with a steep , 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 multiplies the motor torque by (minus losses), divides speed by , and makes the motor's rotor inertia appear at the joint as , the reflected inertia: the rotor spins times faster, so it stores times the kinetic energy per unit of joint speed. The reflected inertia adds to the diagonal of :
where is the motor's torque constant and 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.
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}")With friction the energy falls from J to J in six seconds and never rises in any of the 6000 steps, as the theorem predicts: , because friction always opposes motion. The second part is striking. A rotor of only kg·m² behind a 100:1 gearbox presents kg·m² at the joint, ten times the forearm's own . With it, varies between and (a factor of ) instead of between and (a factor of ). 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.
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
MultibodyPlantwithCalcInverseDynamics,CalcMassMatrixand full simulation with contact. - MuJoCo computes dynamics in generalised coordinates with a soft-contact model, offering
mj_forward,mj_inverseand 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.
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 accelerationWhat the model is used for:
- Gravity compensation: commanding 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: with 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), , 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 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 , 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
1. Robot dynamics
Warm-upHumanA two-link arm is held still, stretched out horizontally (). Link 1 has mass kg, length m and its centre of mass at m; link 2 has kg and its centre of mass m beyond the elbow. With m/s², what torque must the shoulder motor supply? Give the answer to the nearest N·m.
2. Robot dynamics
CoreHumanWhich statements about the mass matrix of the two-link planar arm are true? Select all that apply.
3. Robot dynamics
CoreHumanA 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
CoreComputationalThe starter advances a m pendulum, (with hanging down), by explicit Euler. The tests swing it for 200 simulated seconds at 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.
5. Robot dynamics
StretchComputationalinverse_dynamics(q, qd, qdd) should return for the lesson's two-link arm ( m, m, kg, kg, uniform rods). The starter has and 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, , which holds only if all three terms are right.
6. Robot dynamics
Warm-upHumanWhich algorithm, published by Roy Featherstone in 1983, computes the forward dynamics of a chain of bodies in time, without forming the mass matrix? Give its name (or its usual three-letter abbreviation).
7. Robot dynamics
CoreHumanA team needs joint torques for a 30-joint humanoid model at 1 kHz, from measured positions, velocities and desired accelerations. Which statement is right?
Further reading and resources
- Modern Robotics: Mechanics, Planning, and Control · Kevin M. Lynch and Frank C. Park · 2017Chapter 8 derives open-chain dynamics with both Lagrange and Newton–Euler, in twist and wrench notation.
- Rigid Body Dynamics Algorithms · Roy Featherstone · 2008The reference book on spatial vector algebra, RNEA, CRBA and ABA; the author's site has free notes on spatial vectors.
- Roy Featherstone's home pageTutorials on spatial vectors and the original papers.
- On-Line Computational Scheme for Mechanical Manipulators · J. Y. S. Luh, M. W. Walker and R. P. C. Paul · 1980The recursive Newton–Euler algorithm for inverse dynamics in O(n).
- Efficient Dynamic Computer Simulation of Robotic Mechanisms · M. W. Walker and D. E. Orin · 1982Forward dynamics from repeated inverse dynamics, including the composite-rigid-body method for the mass matrix.
- The Calculation of Robot Dynamics Using Articulated-Body Inertias · Roy Featherstone · 1983The articulated-body algorithm, which computes forward dynamics in O(n).
- On the Equivalence of Lagrangian and Newton-Euler Dynamics for Manipulators · William M. Silver · 1982
- Analytical Derivatives of Rigid Body Dynamics Algorithms · Justin Carpentier and Nicolas Mansard · 2018
- The Pinocchio C++ library · Justin Carpentier et al. · 2019Code at github.com/stack-of-tasks/pinocchio.
- MuJoCo: A physics engine for model-based control · Emanuel Todorov, Tom Erez and Yuval Tassa · 2012The engine is open source at mujoco.org; its documentation explains its dynamics and integrators.
- Proprioceptive Actuator Design in the MIT Cheetah · Patrick M. Wensing et al. · 2017Why legged robots use low gear ratios, and what reflected inertia does to impacts.
- Underactuated Robotics · Russ TedrakeFree online text whose early chapters treat the manipulator equation, pendulums and double pendulums.