Bonus: Use the spare joint

A joint to spare

The grasp point has 3 coordinates but the arm has 4 joints. For most positions there's a whole curve of configurations that put the grasp point in the same place. Moving along that curve changes the arm's shape but not the grasp point. Those joint velocities form the null space of $J$: $J\,\dot q = 0$.

Resolved-rate control with $J^+$ ignores this freedom and always takes the smallest $\dot q$. As a side effect, the gripper's tilt drifts as it moves.

Adding a second goal

The matrix $I - J^+J$ projects any joint velocity onto the null space, so anything you pass through it leaves the grasp point where it is. That lets you add a second term:

The first term does the task. The second does whatever else you like without disturbing the task.

Keeping the gripper vertical

The gripper points straight down when the pitches sum to $\pi$. Penalise the deviation with $h(q) = (q_1+q_2+q_3-\pi)^2$ and move downhill on it:

A gain of $k \approx 10\ \text{s}^{-1}$ works well.

Your task

In phase("trace"), move the grasp point along the straight line from $(0.25, -0.12, 0.12)$ to $(0.25, 0.12, 0.12)$ in 3 s while keeping the gripper vertical.

The starter already traces the line with plain $J^+$ and plots the tilt: run it and watch the tilt wander. Then add the null-space term to rate_step. It works during the approach too, levelling the gripper before the trace starts.

To pass, reach both ends, stay within 5 mm of the line, and keep the tilt under 5° throughout the trace.