Skip to content
LumenaryAcademy
Control 75 min

Trajectories and tracking

Plan how a joint should move through time, within its speed and torque limits — then make the arm follow, using its model as well as its errors.

Code tested · 6AI-draftedAwaiting review by a roboticist

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

MotivationH

From 'go there' to 'go there like this'

In feedback and PID we asked a joint to jump from one angle to another by changing its reference in a single step. The controller responded with everything it had: the voltage saturated, the integrator wound up, the derivative kicked. For a small hobby joint that is a nuisance. For a 20 kg arm in a car factory in Puebla, or a surgical arm in Seoul, it is unacceptable: steps excite vibrations in the links, shake the payload, wear the gearboxes and make the motion unpredictable to anyone nearby.

Real robots never receive steps. A trajectory generator turns "go from here to there" into a smooth reference that says where each joint should be at every instant, how fast it should be moving, and how fast that speed should change, all within the limits of the motors. Then a tracking controller makes the robot follow that moving reference. This lesson does both: first the classic ways to plan a motion in time, then the key idea of tracking, which is to use the robot's model to predict the torque each instant needs, and to leave feedback only the small errors that prediction misses.

IntuitionH

Paths, trajectories and timing

A path is a geometric curve: the set of configurations the robot passes through, with no clock. A trajectory is a path with timing: a configuration for every instant. The same path can be traversed slowly and gently or quickly and violently, and separating the two questions, where and when, is one of the most useful ideas in motion planning. We write a trajectory as q(t)=path(s(t))q(t) = \text{path}(s(t)), where the path is parameterised by s∈[0,1]s \in [0, 1] and the time scaling s(t)s(t) says how far along the path the robot is at time tt.

Why not just feed a moving reference to the PID loop and let it follow? Because feedback acts on errors, so it must first have an error. A joint asked to move at a steady 1 rad/s under P control will lag behind its reference by exactly the error needed to produce the voltage for 1 rad/s. The lag grows with speed and with load. The fix is the one every skilled person uses without thinking.

Analogy, not a formal construction

When you lift a suitcase you expect to be heavy, your muscles tense before it leaves the ground; if it turns out to be empty, it flies up. Your brain predicted the force the motion would need and sent it in advance: that is feedforward. Feedback, from your eyes and the stretch sensors in your muscles, then corrects whatever the prediction got wrong.

DefinitionsH

Definitions

Definition— point-to-point trajectory

A point-to-point trajectory moves from q0q_0 to qfq_f in a duration TT, usually rest-to-rest: zero velocity (and often zero acceleration) at both ends. A via-point trajectory passes through intermediate waypoints without stopping.

Definition— smoothness

A trajectory is CkC^k if it and its first kk derivatives are continuous. A trajectory that is C1C^1 but not C2C^2 has jumps in acceleration, so the motor torque jumps; a jump in acceleration is an infinite jerk (the derivative of acceleration), which excites vibration.

Definition— feedforward

A feedforward term computes, from the desired trajectory and a model, the input the plant needs to follow it, before any error occurs. A tracking controller combines feedforward with feedback on the tracking error e=qd−qe = q_d - q.

Worked examplesH

Point to point: polynomials and trapezoids

Write the move as q(t)=q0+Δ s(t)q(t) = q_0 + \Delta\, s(t), with Δ=qf−q0\Delta = q_f - q_0 and ss going from 0 to 1. Everything is about choosing s(t)s(t) on [0,T][0, T]; let τ=t/T\tau = t/T.

Cubic. Four conditions, s(0)=0s(0) = 0, s(T)=1s(T) = 1, s˙(0)=s˙(T)=0\dot s(0) = \dot s(T) = 0, fix a cubic: s=3τ2−2τ3s = 3\tau^2 - 2\tau^3. Its peak velocity is 1.5 Δ/T1.5\,\Delta/T at mid-move, and its acceleration Δ(6−12τ)/T2\Delta(6 - 12\tau)/T^2 is largest at the ends, 6Δ/T26\Delta/T^2. That is the problem: the robot is at rest before t=0t = 0 with zero acceleration, and at t=0t = 0 the acceleration jumps to its maximum.

Quintic. Add s¨(0)=s¨(T)=0\ddot s(0) = \ddot s(T) = 0, six conditions in all, and the unique quintic is

s(τ)=10τ3−15τ4+6τ5.s(\tau) = 10\tau^3 - 15\tau^4 + 6\tau^5 .

It starts and stops gently, with continuous acceleration. The price is a higher peak velocity, 1.875 Δ/T1.875\,\Delta/T, and peak acceleration 103Δ/T2≈5.77 Δ/T2\tfrac{10}{\sqrt 3}\Delta/T^2 \approx 5.77\,\Delta/T^2 at τ=12∓123\tau = \tfrac12 \mp \tfrac{1}{2\sqrt 3}.

Trapezoidal velocity. Accelerate at the limit amax⁡a_{\max}, cruise at vmax⁡v_{\max}, decelerate at amax⁡a_{\max}. The ramps take ta=vmax⁡/amax⁡t_a = v_{\max}/a_{\max} and the whole move T=Δ/vmax⁡+vmax⁡/amax⁡T = \Delta/v_{\max} + v_{\max}/a_{\max}. If Δ<vmax⁡2/amax⁡\Delta < v_{\max}^2/a_{\max}, the joint never reaches vmax⁡v_{\max} and the profile becomes a triangle with peak velocity Δ amax⁡\sqrt{\Delta\,a_{\max}}. Industrial controllers have used this profile for decades because it is the fastest possible motion under velocity and acceleration limits.

01200.51.0 svelocity (rad/s)quinticcubictrapezoid
Velocity against time for a 1.2 rad move in 1 s, three ways. The cubic (solid) and the trapezoid (dashed) start with a jump in acceleration; the quintic (gold) starts and ends with zero acceleration but needs the highest peak velocity, 2.25 rad/s. The area under each curve is the same 1.2 rad.

Now turn the question round, as a robot controller must: given limits of 2 rad/s and 8 rad/s² on a joint, what is the shortest duration for a 1.2 rad move with each profile? For the polynomials, the peaks scale as Δ/T\Delta/T and Δ/T2\Delta/T^2, so TT must satisfy both limits.

python
import numpy as np

def cubic(d, T, t):
    s = t / T
    return d * (3 * s**2 - 2 * s**3), d * (6 * s - 6 * s**2) / T, d * (6 - 12 * s) / T**2

def quintic(d, T, t):
    s = t / T
    return (d * (10 * s**3 - 15 * s**4 + 6 * s**5), d * (30 * s**2 - 60 * s**3 + 30 * s**4) / T,
            d * (60 * s - 180 * s**2 + 120 * s**3) / T**2)

def trapezoid(d, v, a, t):
    """Accelerate at a, cruise at v, decelerate at a. Falls back to a triangle if d < v^2/a."""
    if d < v * v / a:
        v = np.sqrt(d * a)                          # never reaches v: triangular profile
    ta, T = v / a, d / v + v / a
    if t < ta:
        return 0.5 * a * t * t, a * t, a
    if t < T - ta:
        return v * (t - ta / 2), v, 0.0
    r = T - t
    return d - 0.5 * a * r * r, a * r, -a

d, v_max, a_max = 1.2, 2.0, 8.0                     # a 1.2 rad move; joint limits 2 rad/s, 8 rad/s^2
T_cubic = max(1.5 * d / v_max, np.sqrt(6 * d / a_max))
T_quintic = max(1.875 * d / v_max, np.sqrt(10 / np.sqrt(3) * d / a_max))
T_trap = d / v_max + v_max / a_max

print(f"{'profile':10} {'shortest T':>10} {'peak vel':>9} {'peak acc':>9} {'acc at start':>12}")
for name, T, prof in [("cubic", T_cubic, lambda t, T=T_cubic: cubic(d, T, t)),
                      ("quintic", T_quintic, lambda t, T=T_quintic: quintic(d, T, t)),
                      ("trapezoid", T_trap, lambda t: trapezoid(d, v_max, a_max, t))]:
    ts = np.linspace(0, T, 2001)
    q, v, a = np.array([prof(t) for t in ts]).T
    assert abs(q[-1] - d) < 1e-9 and abs(v[-1]) < 1e-9          # it arrives, at rest
    print(f"{name:10} {T:9.3f}s {np.abs(v).max():8.3f} {np.abs(a).max():9.3f} {a[0]:12.3f}")
Runs entirely in your browser, in an isolated worker. Nothing is sent to a server.

The trapezoid is fastest (0.85 s) because it runs at a limit the whole time. The quintic is slowest (1.125 s), limited by its high peak velocity, but it is the only one whose acceleration starts at zero. The cubic and the trapezoid both demand 8 rad/s² instantly at t=0t = 0.

Industrial controllers usually go one step further and limit jerk too: the S-curve profile replaces each corner of the trapezoid with a short ramp of constant jerk, giving seven phases (jerk up, constant acceleration, jerk down, cruise, and the mirror image). It is nearly as fast as the trapezoid and nearly as gentle as the quintic.

ExplanationH

Through waypoints: splines

A task often specifies several waypoints: lift the part, move above the bin, lower it. Stopping at each is slow and jerky. Instead, pass through them with a cubic spline: a separate cubic on each interval, joined so that position, velocity and acceleration are continuous at every waypoint. Given the waypoint times, the unknowns are the velocities viv_i at the waypoints, and requiring equal accelerations on both sides of each interior waypoint gives one linear equation per waypoint, a tridiagonal system that is cheap to solve.

python
import numpy as np

def knot_velocities(ts, qs, v_start=0.0, v_end=0.0):
    """Velocities at the waypoints that make piecewise-cubic motion continuous in acceleration."""
    n, h = len(ts), np.diff(ts)
    A, b = np.zeros((n, n)), np.zeros(n)
    A[0, 0], b[0], A[-1, -1], b[-1] = 1.0, v_start, 1.0, v_end
    for i in range(1, n - 1):                     # acceleration continuity at waypoint i
        A[i, i - 1], A[i, i], A[i, i + 1] = 1 / h[i - 1], 2 / h[i - 1] + 2 / h[i], 1 / h[i]
        b[i] = 3 * ((qs[i] - qs[i - 1]) / h[i - 1]**2 + (qs[i + 1] - qs[i]) / h[i]**2)
    return np.linalg.solve(A, b)

def hermite(ts, qs, vs, t):
    """Evaluate the piecewise cubic with positions qs and velocities vs at the knots ts."""
    i = min(np.searchsorted(ts, t, side="right") - 1, len(ts) - 2)
    h, s = ts[i + 1] - ts[i], (t - ts[i]) / (ts[i + 1] - ts[i])
    p0, p1, m0, m1 = qs[i], qs[i + 1], vs[i] * h, vs[i + 1] * h
    q = (2*s**3 - 3*s**2 + 1) * p0 + (s**3 - 2*s**2 + s) * m0 + (-2*s**3 + 3*s**2) * p1 + (s**3 - s**2) * m1
    dq = ((6*s**2 - 6*s) * p0 + (3*s**2 - 4*s + 1) * m0 + (-6*s**2 + 6*s) * p1 + (3*s**2 - 2*s) * m1) / h
    ddq = ((12*s - 6) * p0 + (6*s - 4) * m0 + (-12*s + 6) * p1 + (6*s - 2) * m1) / h**2
    return q, dq, ddq

ts = np.array([0.0, 1.0, 2.0, 3.0, 4.0])          # when to pass each waypoint (s)
qs = np.array([0.0, 0.8, 0.5, 1.2, 1.0])          # one joint's waypoints (rad)
vs = knot_velocities(ts, qs)
print("velocities at the waypoints:", np.round(vs, 3))

grid = np.linspace(0, 4, 4001)
for name, v_knots in [("spline (C2)", vs), ("stop at each", np.zeros_like(qs))]:
    q, dq, ddq = np.array([hermite(ts, qs, v_knots, t) for t in grid]).T
    jumps = [abs(hermite(ts, qs, v_knots, tk - 1e-9)[2] - hermite(ts, qs, v_knots, tk + 1e-9)[2])
             for tk in ts[1:-1]]
    print(f"{name:13} peak |v| {np.abs(dq).max():.3f} rad/s   peak |a| {np.abs(ddq).max():.3f} rad/s^2"
          f"   largest jump in a at a waypoint {max(jumps):.3f}")

# Too fast for a 0.8 rad/s limit? Stretch time by c: velocities scale by 1/c, accelerations by 1/c^2.
c = np.abs(np.array([hermite(ts, qs, vs, t)[1] for t in grid])).max() / 0.8
print(f"to respect 0.8 rad/s, stretch every interval by {c:.3f}: total {4 * c:.2f} s instead of 4 s")
q = np.array([hermite(ts, qs, vs, t)[0] for t in grid])
print(f"highest point of the spline: {q.max():.3f} rad at t = {grid[q.argmax()]:.2f} s (highest waypoint {qs.max()})")
Runs entirely in your browser, in an isolated worker. Nothing is sent to a server.

Passing through the waypoints with the spline's velocities lowers both peaks and makes acceleration continuous everywhere; stopping at each waypoint makes acceleration jump, by up to 3 rad/s², at the stops. The last lines show the time-scaling trick: if the spline is too fast for the joint, stretch every interval by the same factor cc. Velocities fall by cc and accelerations by c2c^2, and the path is unchanged.

For a robot with several joints, every joint uses the same waypoint times so that they arrive together, and the time scaling is set by whichever joint is most constrained. Splines in task space (the tool's position and orientation) work the same way, with each sample then converted to joint angles by inverse kinematics.

ComputationC

Time scaling against torque limits

Velocity and acceleration limits are simple stand-ins for the real constraint: what the motors can deliver. Along a trajectory, the torque each joint needs follows from the dynamics, the inverse dynamics

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

where MM is the mass matrix, cc the Coriolis and centrifugal terms and gg gravity. For the rest of the lesson we use a planar two-link arm moving in a vertical plane, with links of 0.5 m and 0.4 m and, for simplicity, each link's mass (2 kg and 1 kg) concentrated at its far end. Its drives can deliver 40 N·m at the shoulder and 15 N·m at the elbow. The task: swing from q=(−0.5,0.8)q = (-0.5, 0.8) rad to (1.0,−0.4)(1.0, -0.4) rad, lifting the hand from about (0.82,−0.12)(0.82, -0.12) m to (0.60,0.65)(0.60, 0.65) m, along a quintic.

python
import numpy as np

L1, L2, G = 0.5, 0.4, 9.81
TAU_MAX = np.array([40.0, 15.0])

def dynamics(q, dq, m1=2.0, m2=1.0):
    c1, c2, s2, c12 = np.cos(q[0]), np.cos(q[1]), np.sin(q[1]), np.cos(q[0] + q[1])
    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 * (2 * dq[0] * dq[1] + dq[1]**2), h * dq[0]**2])
    g = np.array([(m1 + m2) * G * L1 * c1 + m2 * G * L2 * c12, m2 * G * L2 * c12])
    return M, c, g

def quintic(q0, qf, T, t):
    s, d = np.clip(t / T, 0, 1), qf - q0
    return (q0 + d * (10 * s**3 - 15 * s**4 + 6 * s**5), d * (30 * s**2 - 60 * s**3 + 30 * s**4) / T,
            d * (60 * s - 180 * s**2 + 120 * s**3) / T**2)

def peak_torques(q0, qf, T, samples=400):
    """Inverse dynamics along the trajectory: tau = M(q) ddq + c(q, dq) + g(q)."""
    peak = np.zeros(2)
    for t in np.linspace(0, T, samples):
        q, dq, ddq = quintic(q0, qf, T, t)
        M, c, g = dynamics(q, dq)
        peak = np.maximum(peak, np.abs(M @ ddq + c + g))
    return peak

q0, qf = np.array([-0.5, 0.8]), np.array([1.0, -0.4])     # a 1.5 rad swing of the shoulder
print(f"{'T (s)':>6} {'peak tau1':>10} {'peak tau2':>10}   limits {TAU_MAX[0]:.0f}, {TAU_MAX[1]:.0f} N m")
for T in [1.5, 1.0, 0.8, 0.6, 0.5, 0.4]:
    p = peak_torques(q0, qf, T)
    print(f"{T:6.2f} {p[0]:10.1f} {p[1]:10.1f}   {'ok' if np.all(p <= TAU_MAX) else 'TOO FAST'}")

lo, hi = 0.3, 1.5                                          # bisection on the duration
for _ in range(40):
    mid = 0.5 * (lo + hi)
    lo, hi = (lo, mid) if np.all(peak_torques(q0, qf, mid) <= TAU_MAX) else (mid, hi)
print(f"shortest duration within the torque limits: {hi:.3f} s")
print("gravity alone at the start needs", np.round(dynamics(q0, np.zeros(2))[2], 1), "N m, at any speed")
Runs entirely in your browser, in an isolated worker. Nothing is sent to a server.

Note how the torques scale. Stretching time by cc divides q˙\dot q by cc and q¨\ddot q by c2c^2, so the inertial and Coriolis terms fall by c2c^2, but gravity does not change at all. Halving the duration from 1.0 s to 0.5 s roughly doubles the shoulder's peak torque, from 26.0 to 51.6 N·m, not quadruples it, because about 17 N·m of it is gravity. And if gravity alone ever exceeded a joint's limit, no amount of slowing down would help: the arm simply cannot hold that pose.

Uniform scaling is conservative, since it slows the whole move because of its worst instant. Time-optimal path parameterisation (TOPP), developed by Bobrow, Dubowsky and Gibson and by Shin and McKay in 1985, chooses the speed s˙\dot s separately at every point of the path, running each instant at the limit of some joint. Modern implementations such as TOPP-RA do this in milliseconds, and motion-planning frameworks such as MoveIt run such a step after planning every path.

ExplanationH

Tracking: feedforward plus feedback

Now make the arm follow the trajectory. Three controllers, in order of how much they know about the arm:

  1. Independent-joint PD. Each joint has its own PD loop on its own error, with gains scaled by the inertia that joint feels at the start. It knows nothing about gravity or about how the joints push on each other.
  2. PD plus gravity compensation. The same, plus the gravity torque g(q)g(q) computed from the model, so the PD loops no longer have to hold the arm up.
  3. Computed-torque control. Use the whole model.

Definition— computed-torque control

Given the desired trajectory (qd,q˙d,q¨d)(q_d, \dot q_d, \ddot q_d) and gains Kp,KdK_p, K_d, computed-torque control (also called inverse-dynamics control) applies

τ=M(q)(q¨d+Kd e˙+Kp e)+c(q,q˙)+g(q),e=qd−q.\tau = M(q)\big(\ddot q_d + K_d\,\dot e + K_p\,e\big) + c(q, \dot q) + g(q), \qquad e = q_d - q .

Theorem— Exact linearisation by computed torque

If the model is exact and M(q)M(q) is invertible, computed-torque control makes the tracking error obey e¨+Kd e˙+Kp e=0\ddot e + K_d\,\dot e + K_p\,e = 0 for every joint, independently of the configuration. With diagonal positive-definite KpK_p and KdK_d, the error decays exponentially to zero from any initial error.

Proof

Substitute the control into the arm's dynamics Mq¨+c+g=τM\ddot q + c + g = \tau: the cc and gg terms cancel, leaving Mq¨=M(q¨d+Kde˙+Kpe)M\ddot q = M(\ddot q_d + K_d\dot e + K_p e). Since MM is invertible, q¨=q¨d+Kde˙+Kpe\ddot q = \ddot q_d + K_d\dot e + K_p e, which is e¨+Kde˙+Kpe=0\ddot e + K_d \dot e + K_p e = 0. With diagonal gains this is one linear second-order equation per joint, whose characteristic roots λ2+kdλ+kp=0\lambda^2 + k_d\lambda + k_p = 0 have negative real parts whenever kp,kd>0k_p, k_d > 0.

∎

The controller has two parts. The feedforward M(q)q¨d+c+gM(q)\ddot q_d + c + g supplies the torque the motion needs; the feedback M(q)(Kde˙+Kpe)M(q)(K_d\dot e + K_p e) corrects errors, scaled by the inertia so that the same gains give the same response in every configuration. With Kp=400K_p = 400 s⁻² and Kd=40K_d = 40 s⁻¹, every joint's error behaves like a critically damped oscillator with natural frequency 20 rad/s.

trajectorygeneratorouter PDq̈d + Kd ė + Kp einverse dynamicsM(q) v + c + garmqd, q̇d, q̈dvτmeasured q, q̇
Computed-torque control. The trajectory generator supplies desired position, velocity and acceleration; an outer PD loop turns the tracking error into a corrected acceleration; the inverse-dynamics block, using the arm's model and its measured state, converts that acceleration into joint torques.

The simulation runs each controller at 1 kHz on the true arm, following the 0.8 s quintic from the last section. The fourth row gives computed torque a wrong model: it believes the forearm is 20 % lighter than it is, a realistic error once a gripper or payload is attached.

python
import numpy as np

# Planar 2-link arm in a vertical plane, each link's mass at its far end.
L1, L2, G = 0.5, 0.4, 9.81
TAU_MAX = np.array([40.0, 15.0])           # N m, what the joint drives can deliver

def dynamics(q, dq, m1=2.0, m2=1.0):
    """M(q), the Coriolis/centrifugal vector c(q, dq) and gravity g(q):  M ddq + c + g = tau."""
    c1, c2, s2 = np.cos(q[0]), np.cos(q[1]), np.sin(q[1])
    c12 = np.cos(q[0] + q[1])
    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 * (2 * dq[0] * dq[1] + dq[1]**2), h * dq[0]**2])
    g = np.array([(m1 + m2) * G * L1 * c1 + m2 * G * L2 * c12, m2 * G * L2 * c12])
    return M, c, g

def accel(x, tau):
    q, dq = x[:2], x[2:]
    M, c, g = dynamics(q, dq)                       # the TRUE arm
    return np.concatenate([dq, np.linalg.solve(M, tau - c - g)])

def rk4(x, tau, h):
    k1 = accel(x, tau); k2 = accel(x + h / 2 * k1, tau)
    k3 = accel(x + h / 2 * k2, tau); k4 = accel(x + h * k3, tau)
    return x + h / 6 * (k1 + 2 * k2 + 2 * k3 + k4)

def quintic(q0, qf, T, t):
    """Rest-to-rest quintic: position, velocity, acceleration at time t."""
    s = np.clip(t / T, 0, 1)
    d = qf - q0
    return (q0 + d * (10 * s**3 - 15 * s**4 + 6 * s**5),
            d * (30 * s**2 - 60 * s**3 + 30 * s**4) / T,
            d * (60 * s - 180 * s**2 + 120 * s**3) / T**2)

q0, qf, T_move = np.array([-0.5, 0.8]), np.array([1.0, -0.4]), 0.8
DT = 0.001                                           # 1 kHz control
KP, KD = np.array([400.0, 400.0]), np.array([40.0, 40.0])   # "stiffness" in rad/s^2 per rad
M_nom = np.diag(dynamics(q0, np.zeros(2))[0])        # a constant inertia guess per joint

def track(controller, seconds=1.2):
    x = np.concatenate([q0, np.zeros(2)])
    errs, taus = [], []
    for k in range(int(seconds / DT)):
        qd, dqd, ddqd = quintic(q0, qf, T_move, k * DT)
        e, de = qd - x[:2], dqd - x[2:]
        tau = np.clip(controller(x[:2], x[2:], qd, dqd, ddqd, e, de), -TAU_MAX, TAU_MAX)
        errs.append(e); taus.append(tau)
        x = rk4(x, tau, DT)
    return np.abs(np.array(errs)).max(axis=0), np.abs(np.array(errs[-1])), np.abs(np.array(taus)).max(axis=0)

def pd(q, dq, qd, dqd, ddqd, e, de):
    return M_nom * (KP * e + KD * de)
def pd_gravity(q, dq, qd, dqd, ddqd, e, de):
    return M_nom * (KP * e + KD * de) + dynamics(q, dq)[2]
def computed_torque(q, dq, qd, dqd, ddqd, e, de, m2=1.0):
    M, c, g = dynamics(q, dq, m2=m2)
    return M @ (ddqd + KD * de + KP * e) + c + g
def computed_torque_wrong(*args):
    return computed_torque(*args, m2=0.8)            # model thinks the forearm is 20 % lighter

print(f"{'controller':26} {'max error (mrad)':>17} {'final error':>12} {'peak torque (N m)':>18}")
for name, ctl in [("independent PD", pd), ("PD + gravity", pd_gravity),
                  ("computed torque", computed_torque), ("computed torque, m2 -20%", computed_torque_wrong)]:
    emax, efin, tmax = track(ctl)
    print(f"{name:26} {emax[0]*1e3:8.1f} {emax[1]*1e3:8.1f} {efin[0]*1e3:6.2f} {efin[1]*1e3:5.2f} {tmax[0]:9.1f} {tmax[1]:8.1f}")
Runs entirely in your browser, in an isolated worker. Nothing is sent to a server.

The last row is the practical lesson. A model is never exact: payloads change, friction drifts with temperature, cables pull. Practical tracking controllers therefore combine the model with something that removes its residual errors: an integral term, a disturbance observer that estimates the unmodelled torque the way the observer lesson estimated velocities, adaptive control that updates the masses online (the dynamics are linear in the inertial parameters, which makes this tractable), or a model learned from data. And the feedforward is only as good as the reference: a step has infinite q¨d\ddot q_d, which is one more reason to plan smooth trajectories first.

ExplanationH

How it is done in practice

  • Industrial arms generate jerk-limited S-curve or spline trajectories in their controller and track them with cascaded drive loops (feedback and PID showed the cascade) plus model-based feedforward of velocity, acceleration and gravity torque, computed for every joint at the controller's interpolation rate, typically every millisecond or faster. Users rarely see it; they see the "motion profile" and "payload" settings, and the payload setting exists because the feedforward needs it.
  • Online trajectory generation. When the target can change at any moment (a conveyor, a person's hand, a visual-servoing loop), the trajectory must be replanned every control cycle. Libraries such as Ruckig compute the time-optimal jerk-limited motion from any current state to a new target in microseconds, which is what makes this possible.
  • Torque-controlled research and collaborative arms accept joint torques directly over a 1 kHz real-time interface, so computed-torque and the impedance controllers of the next lesson run on the robot's own computer. Libraries such as Pinocchio compute inverse dynamics for a seven-joint arm in a few microseconds with the recursive Newton–Euler algorithm, whose cost grows only linearly with the number of joints.
  • ROS 2. The joint-trajectory controller of ros2_control takes a list of timed waypoints, interpolates them with splines and tracks them with feedforward plus PID; MoveIt produces those waypoints with a time-parameterisation step after planning.
  • Legged robots and humanoids use the same idea, generalised: the "inverse dynamics" is a small optimisation that finds joint torques realising desired accelerations of the body and feet while respecting contact forces and friction (whole-body control), run at around 1 kHz under a slower planner.
Common misconceptionsH

Common misconceptions

Misconception

"Smoother is always slower." A quintic is slower than a trapezoid under the same velocity limit, but a jerk-limited S-curve loses very little time, and on a real arm a smooth motion often finishes sooner, because it does not leave the tool vibrating for a settling time at the end.

Misconception

"With a good enough feedback loop, feedforward is unnecessary." Feedback needs an error to act, so a pure feedback controller following a moving reference always lags, and raising the gains to shrink the lag runs into noise, saturation and stability limits. Feedforward removes the predictable part of the error without raising any gain.

Misconception

"Computed-torque control cancels the dynamics, so the robot becomes linear." Only if the model is exact. In practice it removes most of the nonlinearity, and the feedback and any integral or adaptive terms handle the rest; the 20 % mass error in the table shows what is left over.

Misconception

"If every waypoint is within the limits, the trajectory is too." Interpolation can overshoot waypoints (the spline above did), and the peak velocity or torque usually occurs between waypoints. Check limits on a fine time grid, or use a time-parameterisation method that enforces them.

Exercises

Exercises

1. Trajectories and tracking

Warm-upHuman

A cubic and a quintic polynomial both move a joint from rest to rest in the same time. What does the quintic gain, and what does it pay?

Choose one

2. Trajectories and tracking

CoreHuman

A wrist joint must turn 1.01.0 rad, rest to rest, along a quintic profile. Its limits are 1.51.5 rad/s and 44 rad/s². What is the shortest duration in milliseconds? (The quintic's peak velocity is 1.875 Δ/T1.875\,\Delta/T and its peak acceleration 103 Δ/T2≈5.774 Δ/T2\tfrac{10}{\sqrt3}\,\Delta/T^2 \approx 5.774\,\Delta/T^2.)

3. Trajectories and tracking

CoreHuman

A two-link arm moving in a horizontal plane (so gravity does no work) needs a peak shoulder torque of 66 N·m to follow a trajectory that takes 1.01.0 s. The same path is now to be followed in 0.50.5 s, uniformly time-scaled. What peak shoulder torque, in N·m, does it need? Ignore friction.

4. Trajectories and tracking

CoreComputational

Write the trapezoidal velocity profile. trapezoid_duration(d, v_max, a_max) returns the duration of the fastest rest-to-rest move of length d>0d > 0, and trapezoid(d, v_max, a_max, t) returns (q, v, a) at time tt (clamped to [0,T][0, T]).

Remember the triangular case: if d<vmax⁡2/amax⁡d < v_{\max}^2/a_{\max}, the joint never reaches vmax⁡v_{\max}; it accelerates for half the move and decelerates for the other half, peaking at d amax⁡\sqrt{d\,a_{\max}}.

The starter moves at constant velocity, which jumps from zero instantly and breaks the acceleration limit.

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

5. Trajectories and tracking

StretchComputational

A single motor-driven joint swings a pendulum (bob 1.5 kg at 0.3 m, rotor inertia 0.02 kg·m², viscous friction 0.1 N·m·s/rad), with dynamics

I q¨+b q˙+mglsin⁡q=τ,I=ml2+Irotor.I\,\ddot q + b\,\dot q + m g l \sin q = \tau, \qquad I = m l^2 + I_{\text{rotor}} .

It must follow qd(t)=1+0.8sin⁡(πt)q_d(t) = 1 + 0.8\sin(\pi t) rad. The starter's PD law lags by tens of milliradians. Replace it with computed-torque control with Kp=400K_p = 400 s⁻² and Kd=40K_d = 40 s⁻¹, so that after the first half-second the error stays below 2 mrad.

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

6. Trajectories and tracking

CoreHuman

A computed-torque controller tracks well until the robot picks up a 1 kg tool that the model does not know about. Afterwards the arm still follows trajectories closely, but sits a few milliradians low whenever it stops. What is the best explanation and remedy?

Choose one

7. Trajectories and tracking

Warm-upHuman

Sending a motor the torque that the model says the next instant of the trajectory will need, before any tracking error has appeared, is called ______ control. (One word.)

Further reading and resources

  • Modern Robotics: Mechanics, Planning, and Control · Kevin M. Lynch and Frank C. Park · 2017Chapter 9 covers trajectory generation and time scaling, chapter 11 robot control including computed torque; free preprint and videos.
  • Time-optimal control of robotic manipulators along specified paths · James E. Bobrow, Steven Dubowsky and John S. Gibson · 1985With the work of Kang Shin and Neil McKay the same year, the origin of time-optimal path parameterisation. International Journal of Robotics Research 4(3).
  • TOPP-RA: time-optimal path parameterisation by reachability analysis · Hung Pham and Quang-Cuong PhamOpen-source implementation of a fast, robust method for the fastest timing of a path under velocity, acceleration and torque limits.
  • Ruckig · Lars Berscheid and Torsten KrögerOpen-source online trajectory generation with velocity, acceleration and jerk limits, computed in microseconds.
  • The coordination of arm movements: an experimentally confirmed mathematical model · Tamar Flash and Neville Hogan · 1985Human reaching motions are close to minimum-jerk trajectories, which are quintic polynomials. Journal of Neuroscience 5(7).
  • PinocchioFast inverse dynamics (the recursive Newton–Euler algorithm) and its derivatives, used for computed-torque and whole-body control.
  • ros2_controlIts joint-trajectory controller interpolates a planned trajectory with splines and tracks it with PID plus feedforward.
  • MoveItThe ROS motion-planning framework; after planning a path it time-parameterises it within the robot's limits.