Optimisation: gradient descent

Reaching as a minimisation

arm.ik is switched off. Instead of solving for the joint angles with trigonometry, turn "put the gripper on the target $p^*$" into one number to make as small as possible, the cost:

$$C(q) = \lVert \mathrm{fk}(q) - p^* \rVert^2$$

$q$ is the four joint angles, $\mathrm{fk}(q)$ is where they put the gripper (given below), and $C$ is the squared distance to the target, in m². It's zero exactly when the gripper is on the target.

Walk downhill

The gradient $\nabla C(q)$ lists how fast $C$ grows as each joint turns (one partial derivative $\partial C / \partial q_j$ per joint); it points uphill. Estimate it one joint at a time by nudging that joint a tiny angle $h$ each way (a central difference):

$$\frac{\partial C}{\partial q_j} \approx \frac{C(q + h\,e_j) - C(q - h\,e_j)}{2h}$$

$e_j$ is 1 for joint $j$ and 0 for the others. Gradient descent takes a step against the gradient, again and again:

$$q \leftarrow q - \alpha\,\nabla C(q)$$

The step size $\alpha$ matters. Too small and it crawls; too big and it overshoots, bouncing from one side of the valley to the other. $\alpha = 1$ works here: try 5 and watch the distance plot. Stop when $C < 10^{-8}$ m² (0.1 mm away) or after 1000 steps.

Valleys, walls and the unreachable

• Constraints. Joints have limits (arm.limits). After each step, clip $q$ back inside them with np.clip(q, LO, HI).
• Different starts, different answers. The heatmap shows $C$ over shoulder (across) and elbow (down), with the base and wrist held where descent finished; angles past a limit are shaded. It has two dark valleys, elbow up and elbow down, and descent slides into whichever is downhill from where it starts.
• Out of reach, the lowest cost is above zero. Descent stops with the arm stretched towards the target, as close as it can get.

Jacobians, step 4 swaps the fixed step for Newton's method and converges in a handful of steps.

Your task

1. Write cost(q, target), gradient(q, target) (central differences with H) and descend(q0, target). descend returns (q, path): the final joint angles and the list of every q it visited, start included.
2. The starter descends from the arm's current pose to each of world.targets(), plots the distance, draws the heatmap and moves the arm there with arm.move_joints.