Inverse kinematics: analytic, numeric, and constrained

Forward kinematics, from two lessons ago, is a clean function: joint angles in, end-effector pose out — a forward pass. But almost nothing your robot does starts from joint angles. Perception hands you a grasp pose in Cartesian space; the pi-zero-style policies of Phase 04 emit end-effector targets; every waypoint from Lesson 7's motion planner lives in $SE(3)$. Between all of them and the six joint position commands your WidowX AI's iNerve controller tracks at 500 Hz sits the inverse problem: given a pose, find joint angles that produce it. Unlike the forward pass, the inverse is not a function — a single pose can have zero, two, eight, or infinitely many answers — and the standard way to compute it is an optimization loop running inside a control tick with a hard deadline. That combination, multiplicity plus iterative solving under a latency budget, is this lesson's subject.

Foundations: The Twist, Rotation Error, and Damped Optimization

Before deriving the solver, we must rigorously define the quantities it manipulates. The geometric Jacobian $J$ maps joint velocities $\dot{\mathbf{q}} \in \mathbb{R}^n$ to the end-effector twist $\mathbf{V} \in \mathbb{R}^6$. The twist is a 6-dimensional vector stacking the linear velocity $\mathbf{v} \in \mathbb{R}^3$ (meters per second) and the angular velocity $\boldsymbol{\omega} \in \mathbb{R}^3$ (radians per second): $\mathbf{V} = [\mathbf{v}; \boldsymbol{\omega}]$. This vector lives in the tangent space of the special Euclidean group $SE(3)$, representing an infinitesimal rigid-body motion.

A critical gap in many implementations is the definition of orientation error. We cannot subtract rotation matrices directly because $SO(3)$ is a non-linear manifold. Instead, we map the rotation error into $\mathbb{R}^3$ using the logarithm map. Given a current orientation $R_c$ and a target $R_t$, the relative rotation is $R_{err} = R_t R_c^\top$. The orientation error vector is $\mathbf{e}_{rot} = \log(R_{err})$, which yields an axis-angle vector. This vector represents the rotation required to align $R_c$ with $R_t$ and is linearizable near the identity, making it suitable for Newton-Raphson methods.

The Damped Least Squares (DLS) method minimizes the objective $f(\Delta\mathbf{q}) = \|J\Delta\mathbf{q} - \mathbf{e}\|^2 + \lambda^2 \|\Delta\mathbf{q}\|^2$. To find the optimal step $\Delta\mathbf{q}$, we take the gradient with respect to $\Delta\mathbf{q}$ and set it to zero. The gradient is $\nabla_{\Delta\mathbf{q}} f = 2J^\top(J\Delta\mathbf{q} - \mathbf{e}) + 2\lambda^2 \Delta\mathbf{q}$. Setting this to zero and rearranging yields the normal equations: $(J^\top J + \lambda^2 I)\Delta\mathbf{q} = J^\top \mathbf{e}$. This derivation confirms that DLS is a regularized linear solve, where $\lambda^2$ acts as a ridge penalty to stabilize the inverse when $J$ is ill-conditioned.

Unit consistency is often overlooked. Position error is in meters, while orientation error is in radians. To make the objective function dimensionally consistent, we scale the rotation rows of the error vector and the Jacobian by a characteristic length $s$ (e.g., 0.1 m). This ensures that a 1-radian orientation error is weighted equivalently to a 0.1-meter position error, reflecting the physical displacement at the tool tip.

Worked Example: Planar 2-Link Arm

Consider a planar 2-link arm with $l_1 = l_2 = 0.3$ m. The target is $x = 0.4$ m, $y = 0.2$ m. First, check reachability: $r = \sqrt{0.4^2 + 0.2^2} \approx 0.447$ m. Since $|l_1 - l_2| \le r \le l_1 + l_2$, the target is reachable. The elbow angle is $\cos\theta_2 = \frac{r^2 - l_1^2 - l_2^2}{2 l_1 l_2} \approx 0.111$, so $\theta_2 \approx \pm 83.6^\circ$.

For the elbow-up branch ($\theta_2 > 0$), $\theta_1 \approx -17.5^\circ$. Now, suppose we start at $\mathbf{q} = [0, 0]^\top$. The Jacobian is $J = \begin{bmatrix} -0.3 & -0.6 \\ 0.3 & 0 \end{bmatrix}$. The error is $\mathbf{e} = [0.4, 0.2]^\top$. Using DLS with $\lambda = 0.1$, we solve $(J^\top J + 0.01 I)\Delta\mathbf{q} = J^\top \mathbf{e}$. The unclamped step is $\Delta\mathbf{q} \approx [0.52, 0.31]^\top$. The norm is $\|\Delta\mathbf{q}\| \approx 0.61$ rad, which exceeds the step limit of 0.3 rad. We scale the step by $0.3/0.61 \approx 0.49$, yielding a new configuration $\mathbf{q}_{new} \approx [0.25, 0.15]^\top$.

Closed form, when the geometry cooperates

Start with the smallest problem that exhibits everything interesting: a planar two-link arm with link lengths $l_1$ and $l_2$ — think of your WidowX AI viewed from the side with the base yaw frozen, roughly $l_1 \approx l_2 \approx 0.3$ m (illustrative lengths for a 0.77 m-reach arm). Forward kinematics of the tip is:

To invert, square and add both equations. The cross terms collapse via the angle-sum identities and $\theta_1$ vanishes entirely — squared distance from the base depends only on the elbow angle:

Two things fall out immediately. Reachability: a solution exists iff $|\cos\theta_2| \le 1$, i.e. $|l_1 - l_2| \le r \le l_1 + l_2$ — the workspace is an annulus (a full disk of radius 0.6 m for equal links). Multiplicity: $\theta_2 = \pm\arccos(\cdot)$ gives two branches, the familiar elbow-up and elbow-down configurations. With $\theta_2$ fixed, $\theta_1$ follows from the target bearing minus the interior angle of the triangle:

Multiplicity scales with structure. A 6-DOF arm with a spherical wrist — last three joint axes intersecting at one point — has up to 8 discrete solutions (shoulder left/right times elbow up/down times wrist flip); a fully general 6R chain has up to 16. The spherical wrist is what makes industrial arms analytically solvable, via a decomposition you should recognize as separation of concerns: the wrist-center position depends only on the first three joints, and the orientation only on the last three. Compute the wrist center by backing off from the target pose along the tool axis,

Now the practical question for your hardware: does the geometry decompose? Open the URDF in Trossen's ROS 2 description for the WidowX AI and check whether the last three axes truly intersect at a common point — real arms routinely carry millimeter-scale offsets that break the spherical-wrist condition (Pieper's condition, in the textbook framing), and Trossen publishes no closed-form kinematic parameters for this arm. Do not assume a textbook decomposition your URDF cannot certify. Closed-form IK is still worth having where it exists — it costs a microsecond, enumerates every branch where numeric solvers find one, and makes an excellent seed generator — but for your arm, the workhorse is numeric.

Numeric IK: root finding against a rank-deficient Jacobian

Numeric IK treats the problem as root finding. Define a residual $\mathbf{e}(\mathbf{q}) \in \mathbb{R}^6$ stacking the position error $\mathbf{p}_{\text{target}} - \mathbf{p}(\mathbf{q})$ and an orientation error — the axis-angle vector (rotation log) of $R_{\text{target}} R(\mathbf{q})^\top$. Linearize FK around the current guess with last lesson's geometric Jacobian: since $\mathbf{e}$ is target-minus-current, a joint step $\Delta\mathbf{q}$ reduces the residual by approximately $J\Delta\mathbf{q}$, and Newton-Raphson picks the step that zeroes the linearization:

Near a solution this converges quadratically — 3 to 6 iterations from a decent seed, each costing one FK, one Jacobian, and one 6×6 solve (about 2 µs in numpy on your workstation; a full warm solve lands well under a millisecond). The failure mode is the one last lesson set up: as the arm approaches a singularity, $\sigma_{\min}(J) \to 0$ and the Newton step $J^{-1}\mathbf{e}$ explodes along the near-null direction — a 1 mm residual can demand a 3-radian joint step. The fix is to stop asking for an exact linear solve and instead minimize a damped objective:

Set the gradient to zero — $2J^\top(J\Delta\mathbf{q} - \mathbf{e}) + 2\lambda^2\Delta\mathbf{q} = 0$ — and collect terms to get the damped least squares (DLS) solution, the Levenberg-Marquardt step specialized to this residual. The two forms below are algebraically identical; use the second, because for a 6-DOF task $JJ^\top$ is 6×6 regardless of joint count:

That SVD reading is the whole story of $\lambda$: it trades tracking accuracy near singularities for a hard bound on step size. With $\lambda = 0.1$, no direction is amplified by more than 5×, no matter how degenerate the configuration. Typical settings for a WidowX-AI-scale arm: $\lambda \in [0.05, 0.1]$, tolerances of $10^{-4}$ m and $10^{-3}$ rad (0.1 mm and 0.06° — well below the arm's ~1 mm repeatability, so the solver is never your dominant error source). Two more practicalities. Step limiting: cap $\lVert\Delta\mathbf{q}\rVert$ at 0.2–0.3 rad per iteration so the linearization stays valid — the trust-region move. Unit weighting: position error is in meters, orientation error in radians, and stacking them unweighted implicitly says 1 rad matters as much as 1 m. Scale the rotation rows of both $\mathbf{e}$ and $J$ by a characteristic length — 0.1 m, roughly your gripper length — so a radian of orientation error costs the same as the tool-tip displacement it actually causes.

Damping fixes exploding steps but not local minima. DLS is a local method: if the path from seed to solution is blocked by a joint limit, or position and orientation errors pull in opposite directions, the iterate stalls at a nonzero residual. From a random seed expect 20–50 iterations, and on a limits-heavy arm expect a few percent of solves to stall even for reachable targets. The remedy is cheap and unglamorous: detect the stall (error plateau across iterations) and restart from a new random seed inside the limits. Five restarts at under a millisecond each keeps the worst case below 5 ms.

Differential IK: track trajectories in velocity space

Pose IK answers where should the joints be; a trajectory needs how should the joints move at 50–500 Hz. You could re-run pose IK at every waypoint, but that is the wrong tool three times over. First, branch consistency: each solve is an independent optimization that may land on a different branch — elbow-up at waypoint $k$, elbow-down at $k{+}1$ — and interpolating between them swings the whole arm violently. Second, wasted work: adjacent waypoints differ by millimeters, yet each cold solve burns 20+ iterations rediscovering what the last one knew. Third, no velocity continuity: independent position solutions say nothing about smoothness. So stop solving for positions and solve for velocities — differential IK. Command a desired end-effector twist, convert it to joint velocities with one damped solve, integrate:

One 6×$n$ damped solve per tick — microseconds — so this runs comfortably at 500 Hz on a single CPU core. (Too small for the GPU, whose launch overhead exceeds the solve; batched IK on the RTX card returns in Lesson 6 for grasp-candidate ranking.) Because each tick moves continuously from the previous configuration, the solution cannot jump branches: differential IK converts a discrete, multi-valued problem into a continuous, single-valued flow. Pose IK does not disappear — you still need it for starting and goal configurations in Lesson 7's planner — but everything that moves smoothly should move through velocity space. This layer is also load-bearing for Phase 04: when a VLA policy emits end-effector waypoints, differential IK is what turns them into joint commands, and an unreported stall here is indistinguishable from a bad policy in your eval numbers.

Constraints as first-class citizens: differential IK as a QP

A real arm has joint position limits (on the WidowX AI, joint 1 travels 0–180° and joint 2 only 0–135°), velocity bounds (360°/s on the proximal joints, 540°/s at the wrist), and things it must not hit — including its own table. The lazy approach: compute $\dot{\mathbf{q}} = J^{\dagger}_{\lambda}\mathbf{V}_d$, then clip each joint velocity to its bounds. This is wrong in a specific, instructive way the quiz below probes. The right approach, developed in the MIT manipulation notes' pick-and-place chapter (opens in a new tab), is to make constraints part of the optimization itself. The key enabler: with velocities as decision variables, every constraint of interest is linear, so the problem is a tiny quadratic program. Position limits become velocity bounds through a one-tick lookahead — to guarantee $\mathbf{q}_{\min} \le \mathbf{q} + h\dot{\mathbf{q}} \le \mathbf{q}_{\max}$, bound the velocity by:

Collision margins fit the same mold. If $d(\mathbf{q})$ is the signed distance between a link and an obstacle, its time derivative is linear in joint velocity, $\dot{d} = J_d\,\dot{\mathbf{q}}$ for a distance Jacobian $J_d$ — so requiring the margin not to shrink faster than it can afford is one more linear row: $J_d\,\dot{\mathbf{q}} \ge -\gamma\,(d - d_{\text{safe}})/h$. Assemble everything:

Feel the scale: 6 decision variables, a dozen box bounds, a handful of distance rows. An off-the-shelf QP solver like OSQP dispatches this in 50–200 µs on one CPU core — inside a 2 ms control tick with an order of magnitude to spare. And the QP does something clamping never can: when the desired twist is infeasible, it returns the closest feasible motion — typically preserving the direction of end-effector motion while scaling its speed.

Redundancy and the nullspace

When the arm has more joint freedoms than the task demands, the surplus is called redundancy, and the set of joint velocities that produce zero task motion is the nullspace of $J$. You might think a 6-DOF arm has none — six joints, six pose coordinates — but redundancy is a property of the task, not the arm. Command a top-down grasp of a cylinder and the rotation about the vertical approach axis is free: the task is 5-DOF, your WidowX AI has 6, and a one-dimensional family of equally correct configurations appears. The classical tool for spending that freedom is nullspace projection:

Read the structure: the first term does the task; the second term ascends a secondary objective $w$ — stay near joint mid-range, maximize last lesson's manipulability measure, lean away from an obstacle — but only along directions the task cannot feel. In the QP the same idea is even simpler: add a small cost $\epsilon\lVert\dot{\mathbf{q}} - \dot{\mathbf{q}}_{\text{posture}}\rVert^2$ and the solver spends the leftover freedom on it. Even on a low-DOF arm the concept earns its keep: dropping one orientation coordinate from a grasp task is often the difference between a stalled solver at a joint limit and a clean reach, and knowing which coordinate the task does not need is a design decision you will make explicitly when defining grasp candidates in Lesson 6.

A solver you can trust: DLS in numpy

Here is the complete pose-IK loop, written against the FK and Jacobian functions you built in the previous two lessons. Note what makes it trustworthy rather than merely functional: unit weighting applied consistently to residual and Jacobian, a trust-region step cap, joint-limit clamping (legitimate in pose IK, where iterates are never executed), stall detection, and a return contract that never hides failure.

import numpy as np

def rotation_error(R_current, R_target):
    """Axis-angle vector of the rotation taking R_current to R_target."""
    R_err = R_target @ R_current.T
    cos_t = np.clip((np.trace(R_err) - 1.0) / 2.0, -1.0, 1.0)
    theta = np.arccos(cos_t)
    if theta < 1e-9:
        return np.zeros(3)
    axis = np.array([R_err[2, 1] - R_err[1, 2],
                     R_err[0, 2] - R_err[2, 0],
                     R_err[1, 0] - R_err[0, 1]]) / (2.0 * np.sin(theta))
    return theta * axis

def ik_dls(fk, jacobian, q0, p_target, R_target, q_min, q_max,
           lam=0.08, rot_weight=0.1, step_limit=0.3,
           pos_tol=1e-4, rot_tol=1e-3, max_iters=100):
    """fk: q -> (p, R). jacobian: q -> (6, n) geometric Jacobian, [v; w] rows.
    Always check result['converged'] before using result['q']."""
    q = np.array(q0, dtype=float)
    W = np.diag([1.0, 1.0, 1.0, rot_weight, rot_weight, rot_weight])
    best_q, best_score, prev_score = q.copy(), np.inf, np.inf
    for k in range(max_iters):
        p, R = fk(q)
        e = np.concatenate([p_target - p, rotation_error(R, R_target)])
        pos_err = np.linalg.norm(e[:3])
        rot_err = np.linalg.norm(e[3:])
        score = pos_err + rot_weight * rot_err
        if score < best_score:
            best_score, best_q = score, q.copy()
        if pos_err < pos_tol and rot_err < rot_tol:
            return {"q": q, "converged": True, "iters": k,
                    "pos_err": pos_err, "rot_err": rot_err}
        if abs(prev_score - score) < 1e-9:
            break                       # stalled: plateau, likely local minimum
        prev_score = score
        Jw = W @ jacobian(q)            # weight rotation rows of J and e alike
        ew = W @ e
        dq = Jw.T @ np.linalg.solve(Jw @ Jw.T + lam**2 * np.eye(6), ew)
        step = np.linalg.norm(dq)
        if step > step_limit:           # trust region: keep linearization valid
            dq *= step_limit / step
        q_new = np.clip(q + dq, q_min, q_max)
        if np.linalg.norm(q_new - q) < 1e-10:
            break                       # pinned against joint limits
        q = q_new
    p, R = fk(best_q)
    return {"q": best_q, "converged": False, "iters": k,
            "pos_err": float(np.linalg.norm(p_target - p)),
            "rot_err": float(np.linalg.norm(rotation_error(R, R_target)))}

def ik_with_restarts(fk, jacobian, p_target, R_target, q_min, q_max,
                     q_seed, n_restarts=5, seed=0):
    """Warm-start from q_seed (usually the current configuration),
    then random-restart inside the joint limits. Never hides failure."""
    rng = np.random.default_rng(seed)
    attempts = [np.asarray(q_seed)]
    attempts += [rng.uniform(q_min, q_max) for _ in range(n_restarts)]
    results = []
    for q0 in attempts:
        r = ik_dls(fk, jacobian, q0, p_target, R_target, q_min, q_max)
        if r["converged"]:
            r["restarts_used"] = len(results)
            return r
        results.append(r)
    worst_best = min(results, key=lambda r: r["pos_err"])
    worst_best["restarts_used"] = len(results)
    return worst_best                   # converged is False: caller must check

Two details deserve emphasis. The stall check compares successive scores, not iterates — a solver orbiting a local minimum can keep moving while making no progress, and the plateau is what reveals it. And the joint-limit np.clip inside the loop is a pose-IK privilege: fine here because only the final answer is executed, but exactly the element-wise clamping the QP section warned against for tracking. Same operation, different contract.

Where this goes next: the previous lesson, The Jacobian: velocities, forces, and singularities, built the matrix this entire lesson leaned on; you have now spent it three ways — as a Newton linearization, a damped pseudoinverse, and a QP constraint row. Next, Camera models, calibration, and hand-eye moves to the other end of the pipeline: where grasp poses actually come from. The two lessons meet at the gripper — a 5 mm IK residual and a 5 mm hand-eye calibration bias produce identical misses — and the honest solver reporting you built here is what will let you tell them apart when the full pick-and-place oracle comes together.