Cubic trajectories

Opening the box

In Pick & Place, arm.move_joints(q, duration) glided the arm for you. Under the hood it sent a new target to the PD controllers every 10 ms. In this lesson you generate that setpoint stream yourself, so move_joints and move_to are switched off.

Four conditions, four coefficients

A move from $q_0$ to $q_f$ that takes $T$ seconds should start and end at rest:

$$q(0)=q_0,\qquad q(T)=q_f,\qquad \dot q(0)=0,\qquad \dot q(T)=0$$

Four conditions pin down the four coefficients of a cubic. With $\tau = t/T$ and $\Delta = q_f - q_0$:

$$q(t) = q_0 + \Delta\,(3\tau^2 - 2\tau^3)$$

The formula works on the whole joint vector at once, because each joint just gets its own $\Delta$. The speed peaks halfway through, at $\dot q_{max} = 1.5\,|\Delta|/T$.

Streaming setpoints

Industrial controllers work the same way: a trajectory generator hands the servo loop a fresh setpoint every cycle.

for i in range(n + 1):              # n = T / 0.01
    arm.set_joint_targets(cubic(q0, qf, T, i * 0.01))
    sim.wait(0.01)

Open the Plots tab after a run. The measured shoulder angle trails the commanded one slightly, because the PD controller needs a moment to catch up. That's why the graders in this lesson check your commands, not the measured angles.

Your task

1. Implement cubic(q0, qf, T, t). q0 and qf hold 4 joint angles each. Clamp $\tau$ to $[0, 1]$, so times before the start return $q_0$ and times after the end return $q_f$.
2. The starter already streams the three 1.5 s moves in CONFIGS and labels them phase("move 1") to "move 3". For now cubic just returns qf. Run it and watch the targets jump.

The graders test cubic on random inputs. They also check that each move lasts 1.5 s and never commands more than the cubic's peak speed.