Inverse kinematics: iterative (Jacobian-based) methods

For a 7-DOF arm (Franka, Kinova, Sawyer), closed-form IK doesn't exist in the general case. The extra DOF gives a "self-motion manifold" of valid solutions for any reachable target. You don't pick the right answer analytically — you iterate from the current configuration toward one. Jacobian-based IK is what every modern arm controller uses for redundant manipulators.

The setup

Forward kinematics maps joints to pose: $\mathbf{x}=f(\mathbf{q})$. Differentiating both sides:

$$\dot{\mathbf{x}}=J(\mathbf{q})\dot{\mathbf{q}}$$

$J$ is the 6×n Jacobian. To move the end-effector at velocity ${\dot{\mathbf{x}}}_{\text{des}}$, you need joint velocities $\dot{\mathbf{q}}$ that satisfy this. Solving for $\dot{\mathbf{q}}$ is the IK step.

The naive approach: pseudoinverse

$$\dot{\mathbf{q}}=J^{+}{\dot{\mathbf{x}}}_{\text{des}}$$

$J^{+}$ is the Moore-Penrose pseudoinverse. For a tall $J$ (more rows than columns, fewer DOF than 6), it solves the least-squares problem. For a wide $J$ (more DOF than 6, redundant), it picks the minimum-norm solution.

Wrap this in a loop:

for step in range(max_steps):
    x = forward_kinematics(q)
    error = target - x
    if np.linalg.norm(error) < tol:
        return q
    J = compute_jacobian(q)
    dq = np.linalg.pinv(J) @ error
    q = q + alpha * dq
return None  # didn't converge

Twenty lines. Works on simple cases. Fails on hard ones.

Why pseudoinverse fails near singularities

At a singularity, J loses rank. $J^{+}$ blows up — small Cartesian errors require huge joint velocities. The arm flails. Singularities happen at workspace boundaries, fully-extended configurations, and certain joint alignments.

Fix: damped least squares (DLS):

$$\dot{\mathbf{q}}=J^T(J{J}^T+{\lambda }^{2}I{)}^{-1}{\dot{\mathbf{x}}}_{\text{des}}$$

The damping $\lambda$ bounds the magnitude of joint velocities even when $J$ is near-singular. Trade-off: small $\lambda$ (e.g., 0.001) — accurate but unstable near singularities; large $\lambda$ (e.g., 0.1) — stable but inaccurate.

Practical pattern: variable damping. Start with $\lambda$ small. If the smallest singular value of $J$ drops below a threshold, increase $\lambda$. Standard in IKFast and TRAC-IK.

Newton-Raphson with line search

Pseudoinverse gives the descent direction; the magnitude is uncertain. Use a line search to ensure each step actually reduces the error:

for step in range(max_steps):
    x = forward_kinematics(q)
    error = target - x
    if np.linalg.norm(error) < tol: return q
    J = compute_jacobian(q)
    dq = damped_pseudoinverse(J, lambda_=0.01) @ error

    # Line search
    alpha = 1.0
    while alpha > 0.01:
        q_new = q + alpha * dq
        new_error = target - forward_kinematics(q_new)
        if np.linalg.norm(new_error) < np.linalg.norm(error):
            q = q_new
            break
        alpha *= 0.5
    else:
        return None  # stuck

Slower per iteration but converges from worse initial guesses. Used by ROS's kdl_kinematics_plugin.

Null-space projection: the redundancy bonus

For redundant arms (n > 6 DOF), there's a null space of joint velocities that don't affect the end-effector. Use it to satisfy secondary objectives — joint-limit avoidance, manipulability maximization, obstacle avoidance — while still tracking the primary task.

$$\dot{\mathbf{q}}=J^{+}{\dot{\mathbf{x}}}_{\text{des}}+(I-J^{+}J){\dot{\mathbf{q}}}_{\text{secondary}}$$

Where ${\dot{\mathbf{q}}}_{\text{secondary}}$ is your secondary motion (gradient of a cost function). The projection $(I-J^{+}J)$ keeps it from disturbing the primary task.

Concrete example: keep joint 4 near zero while moving the end-effector:

q_secondary = np.zeros(7)
q_secondary[3] = -k * (q[3] - q4_target)  # pull joint 4 toward target
N = np.eye(7) - np.linalg.pinv(J) @ J
dq = np.linalg.pinv(J) @ error + N @ q_secondary

The arm tracks the goal AND obeys the secondary preference. Magic.

Practical solver options

• KDL (Orocos Kinematics & Dynamics Library): the legacy default. Pseudoinverse-based; works but stalls near singularities.
• TRAC-IK: combines KDL with sequential quadratic programming for joint-limit handling. Default for many production setups.
• BiotIK / IK-Geometric: closed-form when possible, iterative fallback. Used in ROS for 6-DOF spherical-wrist arms.
• BIO-IK: meta-heuristic / genetic algorithm; handles obstacles + multi-objective in one solve. Slower but more flexible.
• cuRobo: NVIDIA's GPU-accelerated IK (and motion planning). 100×+ speedup on hard problems.

The convergence checklist

If your iterative IK isn't converging, in order:

1. Is the target reachable? Check workspace bounds.
2. Is the initial guess reasonable? Far guesses → far convergence; warm-start from the previous solution.
3. Is the Jacobian numerically correct? Compare analytical to finite-difference Jacobian on a test case.
4. Are step sizes small enough? Reduce learning rate.
5. Is damping large enough? Try increasing $\lambda$.
6. Are joint limits binding? Add penalty for exceeding limits.

Skip steps and you're guessing. Run them in order and you find the cause.

When iterative IK doesn't work

• Discontinuous solutions: small task changes lead to large joint reconfigurations. Pure iterative IK can flip; add a continuity constraint or use trajectory-aware IK.
• Hard joint-limit constraints: pseudoinverse can't enforce limits cleanly. Use SQP-based solvers (TRAC-IK, BiotIK).
• Real-time-critical paths: IK at 1 kHz with full optimization is hard. Cache previous solutions; do incremental updates only.
• Avoiding obstacles in arm space: classical IK ignores arm-link collisions. Add it as a secondary cost or use Bio_IK.

What modern IK looks like in 2026

The state-of-the-art is no longer one algorithm. Production stacks combine:

• Analytical IK when the arm geometry has a closed form (most 6-DOF spherical-wrist).
• Damped Jacobian iteration as a fallback or for redundant arms.
• Sampling + optimization for hard collision-aware problems.
• Learned IK (neural network warm-starts an iterative solver). Recent research; not yet production-default.

For 95% of work, pick TRAC-IK or KDL and move on. Reach for cuRobo or BIO-IK only when you hit specific limitations.

Exercise

For a 6-DOF arm in MuJoCo, implement the damped pseudoinverse IK solver above. Generate 100 random reachable target poses; measure success rate, mean iterations, max iterations. Then implement the null-space-projection version that pulls each joint toward zero while reaching the target. Watch the arm prefer "natural" configurations even when redundancy lets it.

Next

The Jacobian itself — the matrix you've been using. What it is, how to compute it, and what its singular values tell you.