Newton's method for IK

IK without a formula

The closed-form IK from Pick & Place only works because this arm is simple and the gripper points straight down. Most arms have no neat formula, so general-purpose solvers work numerically: start from a guess and keep improving it.

Near $q$, the Jacobian is a linear model of FK: $\mathrm{fk}(q + \Delta q) \approx \mathrm{fk}(q) + J\,\Delta q$. To close the error $e = p^* - \mathrm{fk}(q)$, solve that model for $\Delta q$, take the step, and repeat:

This is Newton's method. Close to a solution the error roughly squares each iteration: 1 cm, then 0.1 mm, then far below anything you can measure.

Making it robust

• Redundancy. Four joints for a 3-D goal gives infinitely many solutions. $J^+$ takes the smallest step, so you land on a solution near your starting guess.
• Cap the step. Far from the goal the linear model is poor, and a full step can fling the arm somewhere useless or pin it against a joint limit. If $\lVert\Delta q\rVert > 0.3$ rad, scale $\Delta q$ down to that length.
• Respect the joint limits. Clip $q$ to arm.limits after every step.
• Know when to give up. If the error isn't below 0.1 mm after 100 iterations, the target is probably out of reach. Raise ValueError.

Your task

1. Implement numerical_ik(p, q0). It returns 4 joint angles that put the grasp point at p, starting the search from q0.
2. The program then touches the three yellow targets in order with arm.move_joints.

arm.ik() and arm.move_to() are switched off. The grader runs your solver from the home pose on 20 random targets (within 2 mm), and checks that it reports two targets that are out of reach.