Velocity and acceleration limits

Limits instead of durations

So far you picked $T$ and hoped the joints could keep up. Real drives have hard limits: a top speed $v_{max}$ and a top acceleration $a_{max}$ for each joint. This arm publishes them as arm.vmax and arm.amax (rad/s and rad/s², in joint order). The fastest legal move turns the question around: given the limits, what is the shortest $T$?

The trapezoid

Use the limits the whole way: accelerate at $a_{max}$, cruise at $v_{max}$, then brake at $a_{max}$. Plotted against time, the velocity is a trapezoid. Reaching full speed takes $t_a = v_{max}/a_{max}$, and braking takes the same time again, so

Or a triangle

If $|\Delta| < v_{max}^2/a_{max}$, the move is over before the joint reaches $v_{max}$. The cruise disappears and the velocity becomes a triangle that peaks at $v_p = \sqrt{|\Delta|\,a_{max}}$ halfway through:

Where the joint is at time t

Let $v_p$ be the peak speed ($v_{max}$, or $\sqrt{|\Delta|\,a_{max}}$ for a triangle), and let $t_a = v_p/a_{max}$. The distance travelled after time $t$ is:

The joint angle is then $q_0 + \operatorname{sign}(\Delta)\,s(t)$.

Your task

Implement trapezoid_duration(dq, vmax, amax) and trapezoid(q0, qf, vmax, amax, t) for a single joint: scalars in and out, with $t$ clamped to $[0, T]$.

The starter moves the shoulder only, using arm.vmax[1] and arm.amax[1]. The first move is long enough to cruise; the second is a short triangle. For now the starter moves at constant speed, so its acceleration spikes at both ends of each move.

The commands are graded: speed within 1.02·vmax, acceleration within 1.05·amax, and each move within 3% of the optimal time.