Beginner · Lesson 4 of 10

Inverse kinematics: analytic and numeric

Find the joint angles that put a two-link arm's tip on a target, with the law of cosines and with a damped least-squares solver, handle unreachable targets, and see what singularities do to the numbers

Beginner45 minUpdated 14 Nov 2026Free until 31 Jan 2027

You will learn

Derive the closed-form inverse kinematics of a planar two-link arm and return both elbow solutions
Detect an unreachable target before computing angles and explain the boundary case
Solve the same problem numerically with a damped least-squares iteration and with scipy.optimize.least_squares, and show that both match the closed form
Compute the Jacobian determinant of the arm and explain why steps near a singularity blow up without damping

Before this: Forward kinematics of a two-link arm

Lesson 03 went from joint angles to the tip position. Tasks run the other way. A camera finds a part, a path planner produces a line of points to follow, and the controller needs joint angles that put the tool on each of them. That is inverse kinematics (IK). It is harder than forward kinematics because the answer may not be unique and may not exist. This lesson solves it for the same two-link arm (links of 0.4 m and 0.3 m) in two ways: an exact formula, and a numeric solver of the kind that general robot software uses. Then it looks at what goes wrong near a singularity.

Two answers, one answer or none

For a target inside the annulus found in lesson 03, there are two ways to place the arm: with the elbow on one side of the line from base to target or the other. On the inner and outer edges of the annulus the two coincide (arm fully folded or fully straight). Outside the annulus there is no solution at all.

The two inverse kinematics solutions for a target at (0.5, 0.2) m with links of 0.4 m and 0.3 m. The solid arm is the elbow-up solution, with the elbow above the line from base to target. The dashed arm is the elbow-down solution. Both put the end-effector on the same point.

Software that drives a real arm must choose a branch and stick to it. Switching branches in the middle of a move means swinging the elbow through the straight-arm position, which is a large and possibly dangerous motion for a small change at the tip.

The closed-form solution

Start with the distance from the base to the target, r² = x² + y². The two links and the line from base to target form a triangle with sides l1, l2 and r. Lesson 03 found that r² = l1² + l2² + 2 l1 l2 cos(theta2). Solve for the elbow angle:

cos(theta2) = (x^2 + y^2 - l1^2 - l2^2) / (2 l1 l2)
theta2      = +acos(...)  or  -acos(...)

The two signs are the two branches. If the right-hand side of the first line is greater than 1 or less than -1 there is no real angle, which is exactly the test for a target outside the annulus.

With theta2 known, the shoulder angle is the direction to the target minus the angle that the bent arm adds:

theta1 = atan2(y, x) - atan2(l2 sin(theta2), l1 + l2 cos(theta2))

Use atan2, not atan. The two-argument form looks at the signs of both inputs and returns an angle in the correct quadrant over the full circle. atan(y / x) cannot tell (1, 1) from (-1, -1) and fails when x is zero.

import numpy as np

L1, L2 = 0.4, 0.3   # link lengths, m (same arm as lesson 3)

def fk(q: np.ndarray) -> np.ndarray:
    t1, t2 = q
    return np.array([L1 * np.cos(t1) + L2 * np.cos(t1 + t2),
                     L1 * np.sin(t1) + L2 * np.sin(t1 + t2)])

def ik_analytic(x: float, y: float) -> list[np.ndarray]:
    """All joint solutions (theta1, theta2) that put the tip at (x, y). Empty if unreachable."""
    c2 = (x**2 + y**2 - L1**2 - L2**2) / (2 * L1 * L2)
    if abs(c2) > 1.0 + 1e-12:          # target outside the annulus
        return []
    c2 = float(np.clip(c2, -1.0, 1.0))  # absorb rounding on the boundary
    solutions = []
    for sign in (+1.0, -1.0):          # the two square roots give the two elbow branches
        t2 = sign * np.arccos(c2)
        t1 = np.arctan2(y, x) - np.arctan2(L2 * np.sin(t2), L1 + L2 * np.cos(t2))
        solutions.append(np.array([t1, t2]))
    return solutions

def elbow_side(q: np.ndarray, target: np.ndarray) -> str:
    """Is the elbow to the left (above) or right (below) of the line from base to target?"""
    elbow = np.array([L1 * np.cos(q[0]), L1 * np.sin(q[0])])
    cross = target[0] * elbow[1] - target[1] * elbow[0]
    return "elbow-up" if cross > 0 else "elbow-down"

target = np.array([0.5, 0.2])
for q in ik_analytic(*target):
    err = np.linalg.norm(fk(q) - target)
    print(f"{elbow_side(q, target):10s} theta1 {np.rad2deg(q[0]):7.2f} deg  "
          f"theta2 {np.rad2deg(q[1]):7.2f} deg  FK error {err:.1e} m")

for far in ([0.8, 0.0], [0.05, 0.0]):
    print(f"target {far}: {len(ik_analytic(*far))} solutions")
print(f"target [0.7, 0.0]: {len(ik_analytic(0.7, 0.0))} solutions (arm straight, both branches coincide)")

elbow-down theta1  -11.52 deg  theta2   80.41 deg  FK error 0.0e+00 m
elbow-up   theta1   55.12 deg  theta2  -80.41 deg  FK error 2.8e-17 m
target [0.8, 0.0]: 0 solutions
target [0.05, 0.0]: 0 solutions
target [0.7, 0.0]: 2 solutions (arm straight, both branches coincide)

Feeding each answer back through forward kinematics lands on the target to within 1e-16 m, which is the check to run on any IK code. The target at 0.8 m is beyond the 0.7 m reach and the one at 0.05 m is inside the 0.1 m hole, so both return no solutions. A target exactly on the outer edge returns two identical solutions. Without the small tolerance and the clip, rounding can push the cosine to 1.0000000000000002 and arccos returns nan for a target the arm can reach.

For which branch is “up”, the code checks which side of the base-to-target line the elbow is on. For this target, a positive theta2 (the elbow bent anticlockwise) puts the elbow below the line. Do not rely on the sign of theta2 alone to name the branch, because the relationship flips when the target is on the other side of the base.

A numeric solver

The closed form exists because this arm is simple. For a general arm, such as a 7-joint cobot or an arm with offsets that break the neat triangles, there is often no formula, and robot software solves IK numerically. The idea is to start from a guess, see how far the tip is from the target, and use the Jacobian to decide how to change the joints.

The Jacobian J(q) is the matrix of partial derivatives of the tip position with respect to the joint angles. For small joint changes dq, the tip moves by about J dq. To remove an error e, we want J dq = e. Solving that directly is Newton’s method. It converges fast near a solution and misbehaves near singularities, where J cannot be inverted. Damped least squares adds a small term that limits the step:

dq = J^T (J J^T + lambda^2 I)^-1 e

With lambda equal to zero this is the plain least-squares (pseudo-inverse) step. A small positive lambda costs a little accuracy per step and keeps the step bounded when J is nearly singular. The method is also known as Levenberg-Marquardt, and SciPy’s least_squares offers it among its methods.

import numpy as np
from scipy.optimize import least_squares

L1, L2 = 0.4, 0.3   # link lengths, m

def fk(q: np.ndarray) -> np.ndarray:
    t1, t2 = q
    return np.array([L1 * np.cos(t1) + L2 * np.cos(t1 + t2),
                     L1 * np.sin(t1) + L2 * np.sin(t1 + t2)])

def jacobian(q: np.ndarray) -> np.ndarray:
    """d(tip position) / d(joint angles), a 2x2 matrix."""
    t1, t2 = q
    s1, c1 = np.sin(t1), np.cos(t1)
    s12, c12 = np.sin(t1 + t2), np.cos(t1 + t2)
    return np.array([[-L1 * s1 - L2 * s12, -L2 * s12],
                     [ L1 * c1 + L2 * c12,  L2 * c12]])

def ik_dls(target: np.ndarray, q0: np.ndarray, damping: float = 0.01,
           tol: float = 1e-10, max_iter: int = 200) -> tuple[np.ndarray, int]:
    """Damped least-squares (Levenberg-Marquardt style) iteration on the joint angles."""
    q = q0.astype(float).copy()
    for i in range(max_iter):
        e = target - fk(q)
        if e @ e < tol**2:
            return q, i
        Jq = jacobian(q)
        dq = Jq.T @ np.linalg.solve(Jq @ Jq.T + damping**2 * np.eye(2), e)
        q += dq
    return q, max_iter

def ik_analytic(x: float, y: float) -> list[np.ndarray]:
    """Closed form from the previous block."""
    c2 = (x**2 + y**2 - L1**2 - L2**2) / (2 * L1 * L2)
    if abs(c2) > 1.0 + 1e-12:
        return []
    c2 = float(np.clip(c2, -1.0, 1.0))
    out = []
    for sign in (+1.0, -1.0):
        t2 = sign * np.arccos(c2)
        out.append(np.array([np.arctan2(y, x) - np.arctan2(L2 * np.sin(t2), L1 + L2 * np.cos(t2)), t2]))
    return out

def wrap(a: np.ndarray) -> np.ndarray:
    return (a + np.pi) % (2 * np.pi) - np.pi

target = np.array([0.5, 0.2])
closed_form = ik_analytic(*target)

for q0 in (np.array([0.0, 0.5]), np.array([1.0, -0.5])):
    q, iters = ik_dls(target, q0)
    gap = min(np.abs(wrap(q - qa)).max() for qa in closed_form)
    print(f"DLS from {q0}: theta = {np.round(np.rad2deg(wrap(q)), 2)} deg, {iters} iterations, "
          f"tip error {np.linalg.norm(fk(q) - target):.1e} m, gap to closed form {gap:.1e} rad")

res = least_squares(lambda q: fk(q) - target, x0=[0.0, 0.5], jac=lambda q: jacobian(q))
print(f"least_squares:   theta = {np.round(np.rad2deg(wrap(res.x)), 2)} deg, "
      f"evaluations {res.nfev}, error {np.linalg.norm(res.fun):.1e} m")
gap = min(np.abs(wrap(res.x - qa)).max() for qa in closed_form)
print(f"least_squares gap to closed form: {gap:.1e} rad")

DLS from [0.  0.5]: theta = [-11.52  80.41] deg, 6 iterations, tip error 1.7e-11 m, gap to closed form 7.6e-11 rad
DLS from [ 1.  -0.5]: theta = [ 55.12 -80.41] deg, 7 iterations, tip error 1.4e-12 m, gap to closed form 6.2e-12 rad
least_squares:   theta = [-11.52  80.41] deg, evaluations 6, error 6.1e-14 m
least_squares gap to closed form: 2.6e-13 rad

Both numeric solvers agree with the closed form to better than 1e-10 rad in 6 or 7 iterations. Note which solution each one finds. Started with the elbow bent anticlockwise, the iteration falls into the elbow-down branch. Started the other way, it finds elbow-up. A numeric solver returns one answer, the one nearest its starting guess in a loose sense, and has no idea that another exists. When tracking a path this is what you want: seed each point with the previous point’s solution and the arm stays on one branch. For a one-off target it means you must choose the seed deliberately.

The wrap function matters as well. Angles that differ by 2π are the same joint position for a continuous joint but different numbers, so compare angles after wrapping them into the range from -π to π. A joint with mechanical limits needs those limits checked after solving, or passed to the solver as bounds.

Singularities

The determinant of this arm’s Jacobian works out to l1 l2 sin(theta2). It is zero when the arm is straight or fully folded. At those configurations the two joints can only move the tip at right angles to the arm, and no joint velocity moves it along the arm. That is a singularity. Near one, asking for a small tip motion in the lost direction demands a huge joint motion. The block below shows the determinant shrinking, then takes one linearised step from an almost straight arm (theta2 of 0.5°) towards a point 1 cm closer to the base, with and without damping. Last, it asks for a target outside the reach.

import numpy as np

L1, L2 = 0.4, 0.3   # link lengths, m

def fk(q: np.ndarray) -> np.ndarray:
    t1, t2 = q
    return np.array([L1 * np.cos(t1) + L2 * np.cos(t1 + t2),
                     L1 * np.sin(t1) + L2 * np.sin(t1 + t2)])

def jacobian(q: np.ndarray) -> np.ndarray:
    t1, t2 = q
    s1, c1, s12, c12 = np.sin(t1), np.cos(t1), np.sin(t1 + t2), np.cos(t1 + t2)
    return np.array([[-L1 * s1 - L2 * s12, -L2 * s12],
                     [ L1 * c1 + L2 * c12,  L2 * c12]])

print("det J = L1 L2 sin(theta2):")
for deg in (90, 30, 5, 0.5, 0):
    q = np.array([0.0, np.deg2rad(deg)])
    print(f"  theta2 {deg:5.1f} deg  det J {np.linalg.det(jacobian(q)):+.5f} m^2")

# One linearised step from an almost straight arm (theta2 = 0.5 deg): pull the tip 1 cm towards the base
q = np.array([0.0, np.deg2rad(0.5)])
e = np.array([-0.01, 0.0])
Jq = jacobian(q)
for lam in (0.0, 0.01, 0.05):
    dq = Jq.T @ np.linalg.solve(Jq @ Jq.T + lam**2 * np.eye(2), e)
    moved = fk(q + dq) - fk(q)
    print(f"damping {lam:4.2f}: largest joint step {np.rad2deg(np.abs(dq)).max():7.2f} deg, "
          f"tip moved ({moved[0]:+.5f}, {moved[1]:+.5f}) m")

# A target outside the reach: iterate damped least squares with two damping values
target = np.array([0.9, 0.3])
print(f"unreachable target {target}, best possible miss {np.linalg.norm(target) - (L1 + L2):.4f} m")
for lam in (0.05, 0.2):
    q = np.array([0.3, 0.3])
    misses = []
    for _ in range(500):
        Jq = jacobian(q)
        q = q + Jq.T @ np.linalg.solve(Jq @ Jq.T + lam**2 * np.eye(2), target - fk(q))
        misses.append(np.linalg.norm(target - fk(q)))
    print(f"  damping {lam:4.2f}: theta = {np.round(np.rad2deg(q), 2) + 0.0} deg, "
          f"last three misses {np.round(misses[-3:], 4)} m")

det J = L1 L2 sin(theta2):
  theta2  90.0 deg  det J +0.12000 m^2
  theta2  30.0 deg  det J +0.06000 m^2
  theta2   5.0 deg  det J +0.01046 m^2
  theta2   0.5 deg  det J +0.00105 m^2
  theta2   0.0 deg  det J +0.00000 m^2
damping 0.00: largest joint step  382.99 deg, tip moved (-1.31672, -0.30220) m
damping 0.01: largest joint step    7.11 deg, tip moved (-0.00150, +0.00003) m
damping 0.05: largest joint step    0.29 deg, tip moved (-0.00001, +0.00004) m
unreachable target [0.9 0.3], best possible miss 0.2487 m
  damping 0.05: theta = [ 40.47 -57.03] deg, last three misses [0.3258 0.3161 0.3329] m
  damping 0.20: theta = [18.43  0.  ] deg, last three misses [0.2487 0.2487 0.2487] m

Read the results in order. At 0.5° from straight the determinant is about 1 % of its value at 90°. The undamped step asks for a 383° joint rotation to move the tip 1 cm, and because the linear model is only valid for small steps, the tip ends up 1.3 m away. A damping of 0.01 limits the step to 7°, and 0.05 limits it to 0.3°. Neither achieves the full 1 cm in one step. Along the arm, the tip only moves by an amount proportional to the square of the elbow bend, so the solver has to iterate.

The unreachable target shows the other side of the trade. The best the arm can do is point straight at the target and miss by 0.2487 m. With a damping of 0.2 the iteration settles exactly there, with theta1 aimed at the target and the elbow straight. With a damping of 0.05 it never settles. It chatters around the singular straight-arm pose, and the miss jumps between 0.32 m and 0.33 m. Small damping gives fast, accurate convergence when the target is reachable. Larger damping gives calm behaviour near singularities and outside the workspace. Samuel Buss’s tutorial in Further reading makes the same trade-off explicit. It points to schemes that choose the damping on the fly, and suggests clamping the size of the error vector so that a smaller damping still behaves well with out-of-reach targets.

Singularities are not only a solver problem. A real arm commanded to follow a straight line through a singular pose will demand very large joint speeds there, and the controller will either slow down, stop with a fault, or deviate from the path. 3D arms have several kinds of singularity, and path planners steer around them deliberately. The kinematics hub collects the theory.

Exercise

Use ik_analytic to make the arm’s tip trace a circle of radius 0.1 m centred at (0.45, 0.1) in 100 steps. Keep the elbow-up branch throughout and print the largest change in either joint angle between consecutive points.
Repeat exercise 1 with ik_dls, seeding each point with the previous solution. Compare the result with the closed form at every point.
Move the circle’s centre to (0.55, 0.1) so that it crosses outside the outer edge of the workspace. What does each method do on the unreachable part? Which behaviour would you want on a real robot, and why?
Add bounds of ±150° on theta2 to the least_squares call (its bounds argument) and find a target where the bounded solver must pick a different branch from the unbounded one.

The two-link arm project turns this exercise into an arm that draws a shape.

Summary

Inverse kinematics finds joint angles for a target pose. For a two-link arm there are two solutions inside the workspace, one on its edges and none outside.
The closed form uses the law of cosines for theta2 and atan2 for theta1. Test the cosine against ±1 with a small tolerance before calling arccos.
Numeric IK iterates dq = J^T (J J^T + lambda^2 I)^-1 e or calls a least-squares solver. It finds the solution nearest its seed and agrees with the closed form to rounding error.
At a singularity the Jacobian loses rank. Undamped steps blow up there, and damping trades a little accuracy for bounded, stable steps and sensible behaviour for unreachable targets.