Inverse kinematics: numerical methods

Closed-form inverse kinematics is fast and complete, but it exists only for arms with special geometry, and every new design needs a new derivation. Redundant arms with seven joints, humanoids, mobile manipulators, legged robots and arms with unusual wrists all need a more general approach. Numerical IK treats the problem as finding the root of an equation or the minimum of an error, and improves an initial guess step by step until the tool reaches the target. These methods work for almost any robot, handle redundancy and joint limits naturally, and power the default solvers in tools such as MoveIt. This lesson develops Newton–Raphson IK, the pseudoinverse, damped least squares, gradient methods, and the practical tricks that make them reliable.

IK as root finding

Write the forward kinematics as a function \( \mathbf{x} = f(\mathbf{q}) \) from joint variables to a task-space description of the tool. For the planar 3R arm, \( \mathbf{x} = (x, y, \phi) \). Inverse kinematics asks for \( \mathbf{q} \) such that \( f(\mathbf{q}) = \mathbf{x}_d \), the desired pose, or equivalently a root of the error \( \mathbf{e}(\mathbf{q}) = \mathbf{x}_d - f(\mathbf{q}) \). In one dimension, Newton's method finds roots by following the tangent line: \( q_{k+1} = q_k + e(q_k)/f'(q_k) \). In many dimensions the derivative becomes a matrix of partial derivatives, the Jacobian:

Each column of J says how the tool moves when one joint moves a little. Lesson 10 studies the Jacobian in depth; here we need only that, near the current configuration, a small joint change produces an approximately linear tool change, \( \Delta\mathbf{x} \approx J\,\Delta\mathbf{q} \). For the planar 3R arm the Jacobian follows by differentiating the forward kinematics of lesson 5:

The Newton–Raphson iteration

Setting the linear approximation equal to the error gives the Newton step: solve \( J\,\Delta\mathbf{q} = \mathbf{e} \) and update \( \mathbf{q} \leftarrow \mathbf{q} + \Delta\mathbf{q} \). Repeat until the error is below a tolerance, typically a fraction of a millimetre and a fraction of a degree. When J is square and invertible, \( \Delta\mathbf{q} = J^{-1}\mathbf{e} \). Near a solution, Newton's method converges quadratically: the number of correct digits roughly doubles with each iteration, so a handful of iterations usually suffices.

The algorithm in outline is:

• Start from an initial guess \( \mathbf{q}_0 \), usually the robot's current configuration.
• Compute the forward kinematics \( f(\mathbf{q}_k) \) and the error \( \mathbf{e}_k = \mathbf{x}_d - f(\mathbf{q}_k) \).
• If \( \lVert \mathbf{e}_k \rVert \) is below tolerance, stop and return \( \mathbf{q}_k \).
• Compute the Jacobian \( J(\mathbf{q}_k) \) and solve for the step \( \Delta\mathbf{q} \).
• Update \( \mathbf{q}_{k+1} = \mathbf{q}_k + \Delta\mathbf{q} \), optionally limiting the step size, and repeat up to a maximum number of iterations.

For orientation in three dimensions the error cannot simply be a difference of Euler angles, which would suffer from their singularities and wrap-around. Instead the orientation error is taken as the rotation vector of \( R_d R^\mathsf{T} \), the matrix logarithm from lesson 7, giving a six-dimensional pose error \( (\mathbf{e}_\omega, \mathbf{e}_p) \) that pairs naturally with the six-row geometric Jacobian of lesson 10.

Non-square Jacobians: the pseudoinverse

J is square only when the number of joints equals the number of task variables. A redundant arm, such as a seven-joint arm controlling a six-dimensional pose, has more columns than rows and infinitely many steps that achieve the same tool motion. A task that specifies more than the robot can control, such as asking a five-joint arm for a full six-dimensional pose, has more rows than columns and usually no exact step. The Moore–Penrose pseudoinverse handles both cases:

For a redundant arm, \( \Delta\mathbf{q} = J^{+}\mathbf{e} \) is the smallest joint step, in the least-squares sense, that achieves the desired tool motion. For an over-specified task, it is the step that reduces the error as much as possible. In software, the pseudoinverse is computed from the singular value decomposition, which is numerically robust; never form \( (JJ^\mathsf{T})^{-1} \) explicitly near singular configurations.

Redundancy is an opportunity as well as a complication. Any joint motion in the null space of J, the set of joint velocities that produce no tool motion, can be added without disturbing the tool. The projector \( (I - J^{+}J) \) maps any desired joint motion into that null space, so the full update

reaches the target while also moving towards a secondary goal \( \Delta\mathbf{q}_0 \), such as staying away from joint limits, avoiding an obstacle with the elbow, or keeping a comfortable posture. This is how seven-axis arms keep their elbows out of the way while the tool follows a path.

Damped least squares

Near a singularity, J becomes nearly rank-deficient: some tool motions require enormous joint motions, and the inverse or pseudoinverse produces huge, erratic steps. Damped least squares, also called the Levenberg–Marquardt method, trades a little accuracy for much better behaviour by adding a small damping term \( \lambda \):

Far from singularities, with \( \lambda \) small relative to J's singular values, this is almost the pseudoinverse step. Near singularities, the damping caps the step size: in terms of the singular values \( \sigma_i \), the pseudoinverse divides by \( \sigma_i \), while damped least squares multiplies by \( \sigma_i/(\sigma_i^2 + \lambda^2) \), which never exceeds \( 1/(2\lambda) \). The cost is that the tool approaches the target a little more slowly and, at an exact singularity, may stop short of it. Many solvers adapt \( \lambda \) automatically, increasing it when steps make the error worse and reducing it when they succeed.

Gradient methods and optimization

Another view of IK is minimization: find \( \mathbf{q} \) that minimizes \( \tfrac{1}{2}\lVert \mathbf{e}(\mathbf{q}) \rVert^2 \). The gradient of this cost is \( -J^\mathsf{T}\mathbf{e} \), so gradient descent gives the Jacobian transpose method, \( \Delta\mathbf{q} = \alpha J^\mathsf{T}\mathbf{e} \). It needs no matrix inversion and is stable, but it converges slowly, especially in narrow valleys of the cost. Its appeal is simplicity, and it has a physical interpretation: \( J^\mathsf{T} \) also maps a force at the tool to joint torques, so the method behaves like pulling the tool towards the target with a spring.

Framing IK as optimization makes it easy to add other objectives and constraints: joint limits as bounds, collision avoidance as inequality constraints, preferences for posture or manipulability as extra cost terms. TRAC-IK, a popular ROS solver, runs a Newton-style solver with random restarts in parallel with a sequential quadratic programming (SQP) solver and returns whichever finds a valid solution first. MoveIt's default KDL solver uses a damped Newton method; other plugins include BioIK, a genetic and gradient hybrid, and the analytical IKFast solvers of lesson 8. Whole-body controllers for humanoids solve a quadratic program every control cycle, combining many weighted or prioritized tasks.

Practical issues

Numerical IK finds one solution, the one its iteration happens to converge to from the initial guess. Starting from the current configuration usually gives the nearest solution, which is what a controller wants for smooth motion. To find a different solution, or to recover when the solver gets stuck in a local minimum, restart from other guesses, often random ones within joint limits. Solvers also need a maximum iteration count and a time limit, typically a few milliseconds, so they fail gracefully when a target is unreachable rather than looping forever.

Joint limits need explicit handling: clamping the updated angles to their limits after each step is simple and often adequate, while optimization-based solvers treat limits as constraints. Units matter too: a pose error mixes metres and radians, so it is common to weight the position and orientation errors, deciding for example that 1 mm of position error is as important as 0.1° of orientation error. And because numerical IK can return angles that differ by multiples of 360° from a nearby solution, wrap continuous joints into a consistent range before comparing or commanding them.

import numpy as np
L = np.array([0.4, 0.3, 0.1])
def fk(q):
    a = np.cumsum(q)
    return np.array([L @ np.cos(a), L @ np.sin(a), a[-1]])
def jac(q):
    a = np.cumsum(q)
    J = np.zeros((3, 3))
    for j in range(3):
        J[0, j] = -L[j:] @ np.sin(a[j:])
        J[1, j] =  L[j:] @ np.cos(a[j:])
        J[2, j] = 1.0
    return J
def ik_dls(xd, q0, lam=0.01, tol=1e-6, iters=50):
    q = np.array(q0, float)
    for k in range(iters):
        e = xd - fk(q)
        e[2] = (e[2] + np.pi) % (2*np.pi) - np.pi          # wrap heading error
        if np.linalg.norm(e) < tol: return q, k
        J = jac(q)
        q += J.T @ np.linalg.solve(J @ J.T + lam**2*np.eye(3), e)
    return q, iters
xd = np.array([0.5206, 0.5157, np.radians(15)])
q, k = ik_dls(xd, np.radians([0, 20, 0]))
print(f"converged in {k} iterations: q = {np.round(np.degrees(q), 2)} deg")
q, k = ik_dls(xd, np.radians([90, -30, -40]))
print(f"from another start, {k} iterations: q = {np.round(np.degrees(q), 2)} deg")

converged in 6 iterations: q = [ 30.01  45.   -60.01] deg
from another start, 5 iterations: q = [ 68.23 -45.    -8.23] deg

The two runs illustrate the central property of numerical IK: different starting points converge to different solutions. The first finds the elbow-down configuration of lessons 5 and 8; the second, starting with the elbow bent the other way, finds the elbow-up mirror solution. Both put the tool on the target.