Numerical inverse kinematics

What it computes

Inverse kinematics (IK) finds joint angles $q$ that put the gripper at a target $p^*$. Few arms have a closed-form answer, so general solvers iterate: start from a guess $q_0$ and repeatedly step to shrink the error $e = p^* - \mathrm{fk}(q)$.

The damped least-squares step

Near $q$, FK is linear: $\mathrm{fk}(q + \Delta q) \approx \mathrm{fk}(q) + J\,\Delta q$, where $J$ is the Jacobian. Solving $J\,\Delta q = e$ exactly with the pseudo-inverse blows up near singularities, where $J$ loses rank (a straight arm, for example). Damped least squares trades a little accuracy for stability:

With $\lambda = 0$ this is the pseudo-inverse step. With $\lambda > 0$ the step stays small even when $J$ is singular. This is the Levenberg–Marquardt method that most IK libraries use. Keep $\lambda$ small ($0.02$): heavy damping bends the path the solver takes, and it can stall against a joint limit.

The algorithm

q = q0
repeat max_iters times:
    e = target - fk(q)
    if |e| < tol: return q
    dq = J(q)^T (J J^T + lam^2 I)^-1 e
    cap |dq| at max_step, then q = clip(q + dq, joint limits)
return q        # best effort: never raise, never return NaN

When the target is out of reach, the error can't reach zero. The damped steps then settle with the arm stretched straight towards the target.

Tools

arm.fk(q) gives the grasp point and arm.jacobian(q) its $3 \times 4$ Jacobian. arm.limits holds the joint limits. arm.ik() is switched off.

See it in context

Jacobians steps 4–5 build up to this solver one piece at a time.

Your task

Implement ik(target, q0). The program touches two targets and reaches for a third that is out of range. The grader also runs your solver on 20 random targets, from a singular start, and on targets out of reach.