Through waypoints without stopping

Stop, go, stop, go

Pick & Place moved from pose to pose and stopped at every one. When the poses in between are only there to steer the path, over a cup or around an obstacle, those stops waste time and make the arm lurch. What you want is to pass through the waypoints without slowing to a halt.

Give every waypoint a velocity

A cubic segment can also start and end moving. The cubic Hermite segment matches a position and a velocity at each end ($\tau = t/T$):

With $v_0 = v_1 = 0$, this is your Step 1 cubic. If the segments on either side of a waypoint share its velocity, both position and velocity are continuous there, and the arm flows through.

Which velocity should a waypoint get? A popular choice is the average of the slopes of the two segments that meet there (when the segments take equally long, this is the Catmull–Rom spline):

The first and last waypoints keep $v = 0$, so the arm still starts and ends at rest.

Your task

The yellow markers are world.targets(), and the arm starts on the first one. The starter:

• converts the waypoints to joint angles with arm.ik;
• sizes each segment with the cubic's peak-speed rule, $T_k = \max_i 1.5\,|\Delta_i|/v_{max,i}$, but at least 0.8 s;
• joins the segments into one spline(t) and streams it inside phase("via").

For now hermite ignores the velocities and via_velocities returns zeros, so the arm stops at every waypoint. Watch the gripper speed in Plots dip to almost nothing.

Implement hermite(q0, q1, v0, v1, T, t) and via_velocities(qs, durations). The gripper must:

• pass within 15 mm of every waypoint, in order;
• keep moving through the middle two, with a commanded speed above 5 cm/s from the second waypoint to the third;
• follow a continuous commanded velocity that stays within vmax.