explainer
Numerical inverse kinematics: solve a tool position with local steps
Use a position Jacobian and damped least squares to refine a two-joint arm toward a target. Inspect accepted steps, compare starting guesses, and distinguish convergence, a stalled solve, and unreachable geometry.
What you will learn
- Form a position error and its local Jacobian equation.
- Compute a damped least-squares update with explicit units.
- Check actual error after a finite update and recompute the Jacobian.
- Find different joint solutions by changing the initial guess.
- Distinguish an achieved tolerance from a stalled or limited run.
Before you start
Forward kinematics tells you where a robot's tool goes at chosen joint angles. Inverse kinematics, or IK, asks which angles place the tool at a target. A numerical solver starts with a guess, builds a local approximation, and checks a sequence of updates.
We will solve a two-joint arm's position task. You will see the remaining error after each accepted step, two solutions from different seeds, and a reachable target that stalls from an exactly straight arm.
Specify the target and the arm
Fix the shoulder at the space origin. Space x points right and y points up. The links have lengths L₁ = 2 m and L₂ = 1 m.
Shoulder angle θ₁ is absolute; elbow angle θ₂ is relative to the first link. Both increase counterclockwise.
Write q = (θ₁, θ₂), using radians in the equations. The forward-kinematics lesson gives the tool position:
pₓ(q) = 2 cos θ₁ + cos(θ₁ + θ₂)
pᵧ(q) = 2 sin θ₁ + sin(θ₁ + θ₂)
The target t = (tₓ, tᵧ) uses the same space axes and metre units. Our task is p(q) = t. We do not prescribe tool orientation, model obstacles, or impose mechanical joint limits.
This ideal arm reaches positions with radius 1 ≤ ‖t‖ ≤ 3 m. The longest reach is 2 + 1 m; the shortest is 2 − 1 m. Unrestricted joint rotations cover every direction and each intermediate radius, forming an annulus with a central hole.
Turn position error into a local equation
At the current guess q, define the signed error e = t − p(q). A small angular update δ changes the position approximately by J(q)δ:
p(q + δ) ≈ p(q) + J(q)δ
J(q)δ ≈ e
This is a local Taylor approximation. The Jacobian's rows correspond to x and y outputs; its columns correspond to shoulder and elbow inputs. With s₁₂ = sin(θ₁ + θ₂) and c₁₂ = cos(θ₁ + θ₂):
J =
[−pᵧ, −s₁₂;
pₓ, c₁₂]
J has units m/rad. The update δ uses radians, so Jδ uses metres like e. These are candidate angle changes, not joint velocities or a physical duration.
For a command expressed in metres per second, differential inverse kinematics solves for joint rates. Its experiment compares the instantaneous prediction with the tool position after holding those rates for a chosen interval.
If J is invertible, δ = J⁻¹e solves the linearized equation. More generally, δ = J⁺e gives the minimum-norm least-squares update, as explained in the pseudoinverse lesson. Modern Robotics introduces numerical IK through this repeated local solve.
An exact solution to Jδ = e can still miss the nonlinear target. The arm changes direction during a finite update, and the next Jacobian generally differs from the current one.
Penalize large updates with damping
Near a kinematic singularity, a small singular value can make a pseudoinverse update large. Damped least squares balances the linearized error against update length:
minimize ‖Jδ − e‖₂² + λ²‖δ‖₂²
δ = Jᵀ(JJᵀ + λ²I)⁻¹e
Positive λ makes the penalized problem have a unique solution. It changes the optimization objective: the damped result generally leaves some linearized error even when J is invertible. Samuel Buss derives this objective and both equivalent solution formulas, in section 5 of his IK survey.
In our position coordinates λ uses m/rad, so both objective terms have units m². Changing length units requires changing λ consistently. Mixing revolute and prismatic joints or adding orientation errors requires deliberate scaling and weighting.
Along a singular-value direction, damping replaces the nonzero pseudoinverse gain 1/σ with σ/(σ² + λ²). Small gains receive less amplification, while an exactly zero gain still produces no update in that mode. The manipulability lesson makes these directional gains visible.
The experiment keeps λ between 0.05 and 1 m/rad. Larger damping often slows progress. At λ = 0, the displayed inverse formula requires full row rank; singular cases need a pseudoinverse treatment. Positive damping does not guarantee that the nonlinear iteration will reach the target.
Check each finite step before accepting it
Our solver adds two checks around the damped update. First, cap its Euclidean angular length at 0.35 rad, about 20°. This bound applies to the two-component vector, not independently to each joint.
Second, try the capped direction at scales 1, 1/2, 1/4, …, 1/256. For each trial, evaluate the actual forward position and its error norm. Accept the first trial that decreases the norm by more than 10⁻¹² m.
The full sequence is:
- Evaluate p(q), e and J(q) at the current candidate.
- Solve the damped local problem and cap δ.
- Test the nine possible backtracking scales in order.
- Accept a decreasing trial, then recompute everything at the new q.
This line search checks the nonlinear model. A rejected trial leaves the candidate unchanged. The fixed cap, nine trials and numerical thresholds are choices for this experiment; they are not universal IK settings.
Accepted errors decrease, but that fact alone does not establish convergence to zero. A local stationary point or a strict iteration budget can stop the sequence with substantial error.
Calculate the first accepted step
Start at q = (π/2, −π/2), giving p = (1, 2) m. Choose target t = (2, 1) m and λ = 0.2 m/rad. The initial error is e = (1, −1) m, with norm √2 ≈ 1.414214 m.
At this posture J has rows (−2, 0) and (1, 1). Solve A z = e, then set δ = Jᵀz:
A = JJᵀ + 0.04I
A = [4.04, −2; −2, 2.04]
z ≈ (0.009430, −0.480951)
δ ≈ (−0.499811, −0.480951) rad
The raw update has length 0.693632 rad, exceeding the cap. Multiplying by 0.35/0.693632 gives δ_cap ≈ (−0.252200, −0.242683) rad.
The full capped trial produces angles (75.550°, −103.905°) and position (1.379094, 1.461803) m. Its error norm is 0.773812 m, so the solver accepts it without halving. This is progress, with a substantial remaining error that requires another local solve.
With the default settings, five accepted steps reach error 0.000029 m, below the experiment's 0.0001 m tolerance. That iteration count belongs to this seed, target, damping and stopping rule.
Inspect the numerical sequence
Use Take one IK step to inspect the first update, then Run up to 60 steps to finish the bounded attempt. The budget includes earlier single steps. Changing a target, seed or damping value starts a new calculation.
Choose Backtracking example and take four single steps. On the fourth attempt, Last accepted scale becomes 0.500 and the accepted step length is 0.175000 rad, after the full capped trial increases the position error.
The dashed arm marks the selected seed; the solid arm marks the current numerical candidate. Blue dots record the seed and accepted tool positions. The table lists their residuals and accepted angular step lengths, showing the latest six entries when the sequence is longer.
The dotted connectors summarize successive candidates. They do not specify joint interpolation, a timing law, or a collision-checked robot path. A real robot can remain still while a computer calculates all these candidates.
The seed sliders span ±180°. Those are choices for initial guesses, while the solver's joint variables remain unrestricted. The plot tests position only, so it does not promise a desired tool heading after convergence.
Compare starting guesses and singular stalls
The target (2, 1) m has two distinct elbow branches. One exact solution is (θ₁, θ₂) = (0°, 90°). Another is (53.130102°, −90°), to the shown precision. The analytical inverse kinematics lesson derives both configurations directly from this arm's geometry and checks them with forward kinematics.
The default seed converges near the negative-elbow solution. Other elbow branch starts from (−30°, 90°) and converges near the positive-elbow solution. Each final posture meets the same position goal within tolerance. Neither branch is automatically safer or cheaper to move to.
Now select Straight seed, reachable inward target. The seed q = (0, 0) places the tool at (3, 0), while t = (2, 0) lies inside the reachable annulus. Its local data are:
J = [0, 0; 3, 1]
e = (−1, 0)
Jᵀe = (0, 0)
δ = (0, 0)
Every instantaneous point velocity at this posture is vertical. The error points inward horizontally, so this first-order model supplies no update. Damping, capping and halving cannot turn a zero direction into a useful one.
The solver reports Stalled after one attempted update, with zero accepted steps and 1 m of error. Bent seed, same inward target begins at (30°, −60°) and reaches tolerance in five steps. This pair separates a local failure from geometric unreachability.
Interpret why the solver stops
Each final status answers a different question:
- Converged: the current position error is at most 0.0001 m. An already solved seed needs no attempted step.
- Stalled: the damped direction is below 10⁻¹⁰ rad with error remaining, or none of the nine trials provides the required decrease.
- Iteration limit: 60 attempts ended before reaching tolerance. The slow boundary preset retains about 0.014910 m of error.
- Unreachable target: the target's radius lies outside [1, 3] m. This geometric check runs before iteration and concerns an exact position solution in the ideal model.
The outer preset targets (3.5, 0) m; the inner preset targets (0.5, 0) m. Both are impossible for these link lengths, even without joint limits. A stalled or limited solve inside the annulus supplies no such proof.
Convergence here verifies the forward model's position residual. Applying the result to a real arm also requires calibration, joint-limit checks, collision checking and motion planning. A learned model might propose an initial guess, but its output still needs those checks and a forward-kinematics residual check.
For full-pose IK, orientation needs a compatible rotation error. Modern Robotics constructs a body-frame pose error from log(T_current⁻¹T_target) and pairs it with the body Jacobian. Subtracting Euler-angle tuples generally does not give that twist error; the exponential and logarithm maps lesson explains the local coordinates and their branch choices.
Run the same algorithm in Python
This Python 3 example uses only the standard library. It mirrors the experiment's damping, step cap, backtracking, tolerance and iteration budget. The displayed cases use the same finite inputs as the presets.
from math import cos, sin, radians, degrees, hypot
def geometry(q):
a, b = q
x = 2*cos(a) + cos(a+b)
y = 2*sin(a) + sin(a+b)
return (x, y), ((-y, -sin(a+b)), (x, cos(a+b)))
def damped_delta(j, e, damping):
a = sum(v*v for v in j[0]) + damping*damping
b = sum(j[0][i]*j[1][i] for i in range(2))
c = sum(v*v for v in j[1]) + damping*damping
determinant = a*c - b*b
z = ((c*e[0]-b*e[1])/determinant,
(a*e[1]-b*e[0])/determinant)
return tuple(j[0][i]*z[0] + j[1][i]*z[1] for i in range(2))
def solve(seed_degrees, target, damping=0.2):
q = tuple(radians(v) for v in seed_degrees)
p, j = geometry(q)
error = hypot(target[0]-p[0], target[1]-p[1])
if not 1 <= hypot(*target) <= 3:
return 'unreachable', 0, q, error
if error <= 0.0001:
return 'converged', 0, q, error
for attempt in range(1, 61):
p, j = geometry(q)
e = (target[0]-p[0], target[1]-p[1])
delta = damped_delta(j, e, damping)
length = hypot(*delta)
if length < 1e-10:
return 'stalled', attempt, q, error
cap = min(1, 0.35/length)
for reduction in range(9):
scale = 2**(-reduction)
trial = tuple(q[i] + scale*cap*delta[i] for i in range(2))
trial_p, _ = geometry(trial)
trial_error = hypot(target[0]-trial_p[0], target[1]-trial_p[1])
if trial_error < error - 1e-12:
q, error = trial, trial_error
break
else:
return 'stalled', attempt, q, error
if error <= 0.0001:
return 'converged', attempt, q, error
return 'limit', 60, q, error
def display(q):
return '(' + ', '.join(
f'{0.0 if abs(degrees(x)) < 0.5e-6 else degrees(x):.6f}' for x in q
) + ')'
cases = [
('worked', (90, -90), (2, 1), 0.2),
('other branch', (-30, 90), (2, 1), 0.2),
('straight seed', (0, 0), (2, 0), 0.2),
('bent seed', (30, -60), (2, 0), 0.2),
('outer target', (90, -90), (3.5, 0), 0.2),
('inner target', (90, -90), (0.5, 0), 0.2),
('already solved', (90, -90), (1, 2), 0.2),
('slow boundary', (90, -90), (3, 0), 1.0),
]
for name, seed, target, damping in cases:
status, attempts, q, error = solve(seed, target, damping)
print(f'{name}: {status}, attempts={attempts}, error={error:.6f} m')
print('Joint angles (degrees):', display(q))
Expected output:
worked: converged, attempts=5, error=0.000029 m
Joint angles (degrees): (53.130550, -90.001854)
other branch: converged, attempts=5, error=0.000009 m
Joint angles (degrees): (-0.000137, 90.000571)
straight seed: stalled, attempts=1, error=1.000000 m
Joint angles (degrees): (0.000000, 0.000000)
bent seed: converged, attempts=5, error=0.000006 m
Joint angles (degrees): (28.955101, -104.477861)
outer target: unreachable, attempts=0, error=3.201562 m
Joint angles (degrees): (90.000000, -90.000000)
inner target: unreachable, attempts=0, error=2.061553 m
Joint angles (degrees): (90.000000, -90.000000)
already solved: converged, attempts=0, error=0.000000 m
Joint angles (degrees): (90.000000, -90.000000)
slow boundary: limit, attempts=60, error=0.014910 m
Joint angles (degrees): (4.038929, -12.124795)
The two-by-two linear solve stays small enough to write directly. Larger systems usually use matrix factorizations or SVD-based solvers. Six printed decimals describe rounded floating-point values.
Try it yourself
Exercise 1. Start at q = (0, 0) and target t = (2.5, 0) m. Compute e and Jᵀe. Does the damped update move the arm? Is the target geometrically unreachable?
Show the reachable-stall solution
The tip starts at (3, 0), so e = (−0.5, 0) m. J has rows (0, 0) and (3, 1), giving Jᵀe = (0, 0). In the damped normal equation, this zero right-hand side produces δ = (0, 0) for every positive λ.
The target radius is 2.5 m, inside the annulus. A finite bend can reach it, but the straight posture's first derivative offers only vertical motion. A different initial guess changes the local model. Failure from this seed does not establish failure from every seed.
Exercise 2. For a standalone local position model, let J = diag(2, 1) m/rad, e = (1, −2) m, and λ = 0.5 m/rad. Find the raw damped update and its predicted position change before applying a step cap. Compare it with the undamped inverse update.
Show the damped linear-model solution
JJᵀ + λ²I is diag(4.25, 1.25). Multiplying its inverse by e and then by Jᵀ gives δ = (8/17, −8/5) rad, approximately (0.470588, −1.6) rad.
The predicted change is Jδ = (16/17, −8/5) m. Its residual is e − Jδ = (1/17, −2/5) m. The undamped inverse gives (0.5, −2) rad and zero linearized residual, with a larger update norm.
Positive damping penalizes update length alongside residual error. Neither raw result establishes how a nonlinear arm moves through a finite update; that requires evaluating its forward map. Our experiment would also cap either of these large directions before testing it.
Sources and further study
- Lynch and Park, Modern Robotics: Numerical Inverse Kinematics, Part 1. Develops the local Jacobian and pseudoinverse iteration and explains the importance of an initial guess.
- Samuel R. Buss: Introduction to Inverse Kinematics with Jacobian Transpose, Pseudoinverse and Damped Least Squares Methods. Author's 2009 survey, hosted in Carnegie Mellon's course materials; section 5 derives damping and section 6 interprets its singular-value gains.
- Lynch and Park, Modern Robotics: Numerical Inverse Kinematics, Part 2. Uses a matrix-logarithm pose error and matching body Jacobian for full-pose iteration.