Inverse kinematics: solving for the angles

Running forward kinematics backwards: the closed form for a two-link arm, why it always has two answers, and the damped iteration that works when no closed form exists.

Where you are. You can compute where the hand is from the joint angles, build the Jacobian at any pose, and say what a singularity does to it. This lesson runs the whole thing backwards, which is the direction every real task needs.

Pin your fingertip to the table

Put your index fingertip on a spot on the table and press lightly, enough that you would feel it slide.

Now swing your elbow out to the side, then tuck it back against your ribs. Your shoulder rolls, your forearm turns, your elbow sweeps through most of a foot of arc, and the fingertip does not move. Nothing about that spot on the table told your arm what to do with the elbow.

Now slide the fingertip straight away from you until the arm is straight, and try to keep going. Nothing you do at the shoulder or the elbow buys another millimetre. Notice the last part of that journey too: in the final centimetre of reach, the elbow had to swing a long way to buy very little.

Three things happened in twenty seconds. One fingertip position, many arm configurations. A place where the configurations run out. And a neighbourhood just before it where the exchange rate between joint motion and hand motion goes bad. Every difficulty in this lesson is one of those three.

The idea in one paragraph

Forward kinematics is a function: angles in, exactly one pose out, no way to fail. Inverse kinematics is the same question backwards, and it is not a function. A pose can have several sets of joint angles that reach it, none at all, or a continuum. Two tools cover the ground. For a simple geometry like the two-link arm, trigonometry gives a closed form that hands you both answers exactly. For everything else you iterate: measure how far off you are, ask the Jacobian which joint motion closes the gap, take a fraction of the step it suggests, go round again. That loop is six lines long and knows nothing about the arm it drives, which is why it is the default. It has one dangerous failure mode - asking a nearly singular Jacobian for something it cannot deliver - and one extra term fixes it.

Forward is evaluation; backward is a solve

Those two words are the whole difficulty gap. $\mathrm{FK}(\theta)$ is something you evaluate. $\mathrm{FK}(\theta) = \mathbf{p}^{*}$ is something you solve, and solving is a different class of problem.

The asymmetry shows in the return type. FK is typed angles -> pose. IK cannot be, because it has to be able to hand back nothing, or a set.

What you asked for	What comes back	Why
A reachable pose	Several answers	Different configurations put the hand in the same place; two for the planar two-link arm, up to eight for a typical six-axis industrial arm
A pose outside the workspace	Nothing	No configuration exists, and the solver has to say so rather than return its best wrong guess
A reachable pose on a redundant arm	Infinitely many	More joints than the task constrains; your own seven-joint arm is the one you just felt at the table

Two routes from a target to joint angles. The closed form runs the target and the link lengths through the law of cosines to two exact answers, for this geometry only. The iteration starts from a guess and runs a damped loop, returning one answer on whichever branch the guess was on, for any arm with a forward-kinematics function and a Jacobian.

Wider than the screen; scroll it sideways.

The two-link arm is a triangle

Three points: the base, the elbow, and the target. Two of the distances between them are fixed by the hardware - $l_1$ base to elbow, $l_2$ elbow to target - and the third, $r$ , is fixed by where the target is. Three side lengths pin down a triangle up to a reflection, and that is the whole closed form. The rest is extracting angles.

Start with the law of cosines on the interior angle at the elbow, $\gamma$ :

r^2 = l_1^2 + l_2^2 - 2\,l_1 l_2 \cos\gamma

In words: the distance across a triangle grows as the angle facing it opens up. Rearrange for the angle, minding one detail. $\gamma$ is the interior angle, but $\theta_2$ is measured from link 1 extended, so $\theta_2 = 180^{\circ} - \gamma$ and $\cos\theta_2 = -\cos\gamma$ :

\cos\theta_2 = \frac{r^2 - l_1^2 - l_2^2}{2\,l_1 l_2}

Read it as a sentence: how far away the target is decides how bent the elbow must be. Direction does not appear. With $l_1 = 1.0$ and $l_2 = 0.7$ metres, every target at distance 1.1 needs $\theta_2 = 101.5370^{\circ}$ .

The base, the elbow and the target form a triangle with sides l1, l2 and r, so the law of cosines fixes the elbow angle from the distance alone. The shoulder angle is the direction to the target minus the angle link 2 steals. Mirroring the triangle about r gives the second solution.

Wider than the screen; scroll it sideways.

Now the shoulder. Let $\beta = \operatorname{atan2}(y, x)$ be the direction of the target from the base. Pointing link 1 straight along $\beta$ would put the hand off to one side, because link 2 leaves the elbow at an angle. So aim at the target, then back off by the angle link 2 steals:

\begin{aligned} \psi &= \operatorname{atan2}\left(l_2\sin\theta_2,\; l_1 + l_2\cos\theta_2\right) && \text{the angle from link 1 round to the hand} \\ \theta_1 &= \beta - \psi && \text{aim at the target, then back off} \end{aligned}

Worked through for the target $(1.20, 0.30)$ : $r = 1.2369$ , so $\cos\theta_2 = 0.0286$ and $\theta_2 = 88.36^{\circ}$ ; $\beta = 14.04^{\circ}$ and $\psi = 34.45^{\circ}$ , so $\theta_1 = -20.41^{\circ}$ . Forward kinematics on those two angles lands on $(1.20, 0.30)$ to within 2.5e-16.

Why there are always two

$\arccos$ returns the non-negative angle, but $\cos(-\theta_2) = \cos(\theta_2)$ , so $-88.36^{\circ}$ satisfies the same equation, and flipping the sign of $\theta_2$ also flips $\psi$ , moving $\theta_1$ to $+48.49^{\circ}$ . Two complete answers: the elbow sits at $(0.937, -0.349)$ in one and $(0.663, 0.749)$ in the other, fingertip on the target in both, agreeing to within 1.2e-15 across 20,000 random reachable targets. Geometrically it is the triangle mirrored about the line from base to target, the paper version of what you felt with your fingertip pinned.

The branches are called elbow-up and elbow-down, and the names describe the sign of $\theta_2$ , not which way the elbow points. Positive $\theta_2$ always puts the elbow on the same side of that line, but for a target in front of the robot that side looks like down, and for one behind it, up.

Reachability comes free

$\cos\theta_2$ has to lie in $[-1, 1]$ . Outside that range no triangle exists with those side lengths, which is the algebraic form of “you cannot reach it”. At $r = 1.71$ the formula gives $\cos\theta_2 = +1.0244$ , at $r = 0.29$ it gives $-1.0042$ . Both are refusals, and together they bound the reachable ring between $|l_1 - l_2| = 0.30$ and $l_1 + l_2 = 1.70$ , the same annulus drawn in workspace and reachability.

Why the closed form runs out

Every step above used the fact that this arm is two links in a plane. Add a third and the base, two elbows and the target form a quadrilateral, which three side lengths do not pin down. Offset one joint axis by a centimetre and the derivation is scrap. So the general method has to know nothing about the geometry, and may assume only that you can compute $\mathrm{FK}(\theta)$ and $J(\theta)$ .

Run the Jacobian backwards

You are at $\theta$ , the hand is at $\mathrm{FK}(\theta)$ , you want it at $\mathbf{p}^{*}$ . The miss is a vector in hand space:

\mathbf{e} = \mathbf{p}^{*} - \mathrm{FK}(\theta)

The Jacobian says a small joint step $\Delta\theta$ moves the hand by about $J(\theta)\,\Delta\theta$ . You want that motion to be $\mathbf{e}$ , so solve $J\,\Delta\theta = \mathbf{e}$ , take part of that step, rebuild everything at the new pose, repeat.

The damped least squares loop: measure the miss in hand space, build the Jacobian at the current pose, solve the damped system for a joint step, take half of it, and feed the new angles back to the start.

Wider than the screen; scroll it sideways.

This is Newton’s method on $\mathrm{FK}(\theta) - \mathbf{p}^{*} = 0$ , in a shape you already own: forward pass, error, local linear model, parameter update. The Jacobian plays the part the gradient plays when training a network.

The step fraction $\alpha$ matters more than it looks. $J$ is a linear model of a curved map, honest only near the pose where you built it, so walking the full distance it recommends can land you somewhere it never described. Over 200 random reachable targets from one fixed seed:

Step fraction	Converged to `1e-6`	Median iterations	Worst case
$\alpha = 0.2$	200 / 200	64	73
$\alpha = 0.5$	200 / 200	22	31
$\alpha = 1.0$	187 / 200	7	never converged

The seed also chooses the branch, less reliably than folklore suggests. Seeded at $\theta_2 = +0.6$ and jumping cold to a random target, the solver landed on the $\theta_2 > 0$ branch 189 times out of 200. Tracking a target that moved 5 mm at a time, reseeded from the previous solution, it held the branch 200 out of 200. So “seed from the current configuration and keep the branch” is solid while you are following something, and only a tendency when you jump.

The damping term, and when it is actually working

Solving $J\,\Delta\theta = \mathbf{e}$ exactly means dividing by the Jacobian’s smallest singular value, and singularities is the lesson about what that costs. The short version: at $\theta_2 = 0.57^{\circ}$ , a hair off straight, the smaller singular value is 0.0038, so asking for 5 cm of hand motion along the arm’s own axis returns a joint step of 13.20 radians. At exactly $\theta_2 = 0$ the matrix is rank 1 and np.linalg.solve raises LinAlgError: Singular matrix.

So the loop does not ask for the exact answer. It asks for a nearby one:

\Delta\theta = J^{\top}\left(J J^{\top} + \lambda^2 I\right)^{-1}\mathbf{e} \qquad \text{damped least squares}

Two things about $\lambda$ are worth knowing before you tune it, and neither is obvious.

It is free until it is needed. Over the same 200 targets, $\lambda = 0$ , $0.01$ and $0.05$ all converged 200 / 200 with an identical median of 22 iterations. Only at $\lambda = 0.2$ do you pay, and barely: median 25, worst case 56. So pick $\lambda$ for the worst pose you expect to visit, not the average one, because the average one is not charging you.

A reachable target near the edge does not trigger it. Aim the solver 0.001 inside the outer rim, about as close to a singularity as a reachable target gets, and run 100 fixed steps. Undamped and damped are identical: both peak at 1.14 radians per step, both land on the target to machine precision. The shipped experiment in code/ik_2link.py sets up exactly this case and then claims in its final line that the undamped run whips the joints. It does not, and the numbers it prints say so.

What actually separates them is an impossible request. Move that same target 0.05 past the rim and the undamped run winds $\theta_2$ through more than three full turns and finishes 2.449 from the target, while the damped one stretches to the rim and stops 0.065 short - the full measurement is the table in singularities. The lesson for IK specifically is that damping is what makes a wrong request degrade instead of explode, and wrong requests are the normal case: a perception error, a policy that has not learned the workspace, an operator typing a number.

That is the argument for running the closed form’s reachability test first. $|\cos\theta_2| > 1$ is one comparison, it is exact, and it separates “cannot be done” from “not converged yet” - a distinction the iterative solver cannot make about itself, because both look like a residual that stopped shrinking.

Review

Forward is evaluation; backward is a solve

Forward kinematics is a function: angles in, exactly one pose out, no way to fail. Inverse kinematics is the same question backwards, and it is not a function. A pose can have several sets of joint angles that reach it, none at all, or a continuum. The asymmetry shows in the return type. Forward is typed angles to pose; inverse cannot be, because it has to be able to hand back nothing, or a set. A reachable pose gives several answers, two for the planar two-link arm and up to eight for a typical six-axis industrial arm. A pose outside the workspace gives nothing, and the solver has to say so rather than return its best wrong guess. A redundant arm gives infinitely many, which is what you feel when you pin a fingertip to the table and swing your elbow through a foot of arc without moving it.

The two-link arm is a triangle

Three points: the base, the elbow and the target. Two of the distances between them are fixed by the hardware, the third is fixed by where the target is, and three side lengths pin down a triangle up to a reflection. That is the whole closed form; the rest is extracting angles. The law of cosines at the elbow reads as a sentence: how far away the target is decides how bent the elbow must be, and direction does not appear in it at all. The shoulder is then the direction to the target, minus the angle the second link steals. There are always two answers because cosine is even, so flipping the sign of the elbow angle satisfies the same equation; geometrically it is the triangle mirrored about the line from the base to the target. Reachability comes free, because a cosine outside minus one to one is the algebraic form of you cannot reach it.

When there is no closed form, iterate carefully

Add a third link and the derivation is scrap, so the general method assumes only that you can compute forward kinematics and the Jacobian. Measure the miss in hand space, ask the Jacobian which joint motion closes it, take a fraction of that step, rebuild everything at the new pose and go round again. It is Newton’s method, with the Jacobian playing the part the gradient plays when training a network. The step fraction matters more than it looks, because the Jacobian is a linear model of a curved map and is honest only near where you built it. Over two hundred targets, taking the full step converged in seven iterations and failed thirteen times, and those failures sat in the comfortable middle of the workspace, so that is overshoot rather than singularity. Halving the step costs three times the iterations and buys back all thirteen. Damping is the separate fix: instead of asking for the joint step that produces exactly this hand motion, ask for the one that gets closest without being large.

Check yourself

1. cos θ2 = (r² − l1² − l2²) / (2 l1 l2) contains no reference to the direction of the target. Why not, and what does that tell you about the solution set?

Because $\theta_2$ is an interior property of the triangle formed by base, elbow and target, and a triangle is determined by its three side lengths alone. Turning the whole triangle about the base moves the target without changing any side length, so $\theta_2$ cannot depend on direction. Every target at the same distance needs the same elbow bend; with $l_1 = 1.0$ and $l_2 = 0.7$ , everything at radius 1.1 needs $\theta_2 = 101.5370^{\circ}$ . Direction is absorbed entirely by $\theta_1$ , through the $\beta = \operatorname{atan2}(y, x)$ term.

2. Where does the second solution come from algebraically, and what does it look like on the arm?

From $\arccos$ . The formula fixes $\cos\theta_2$ , and both $+\theta_2$ and $-\theta_2$ satisfy it, so you get two elbow angles. Flipping the sign of $\theta_2$ also flips $\psi$ , which moves $\theta_1$ by $2\psi$ . Geometrically it is the triangle mirrored about the line from base to target: the elbow swings to the other side of that line while the fingertip stays put. For the target $(1.20, 0.30)$ the two answers are $\theta = (-20.41^{\circ}, +88.36^{\circ})$ and $\theta = (+48.49^{\circ}, -88.36^{\circ})$ , with elbows at $(0.937, -0.349)$ and $(0.663, 0.749)$ . Forward kinematics puts the hand on the target in both, agreeing to 1.2e-15.

3. Your solver is converging on well-conditioned targets in the middle of the workspace and failing on about 6% of them. Damping is on. What do you look at first?

The step fraction, not the damping. Damping protects the near-singular case, and the middle of the workspace is not that case. Failures clustered away from the edges are overshoot: a step size near 1.0 walks past where the local linear model is valid, so the iteration wanders instead of settling. Measured: $\alpha = 1.0$ failed 13 of 200 at radii between 0.41 and 0.98, while $\alpha = 0.5$ converged 200 of 200 at the cost of going from a median of 7 iterations to 22.

4. You aim the solver at a target just inside the outer edge, and λ = 0 and λ = 0.01 print exactly the same numbers. Is your damping broken?

No. Both peak at 1.14 radians per step and both land on the target to machine precision, because the target is reachable and the iteration never has to ask for motion the arm cannot deliver. Damping only changes the answer when the requested motion has a real component in a direction whose singular value is small, and merely being near the rim is not enough - the solver has to be asking for something out there. Move the same target 0.05 past the rim and the two diverge immediately, 22.42 radians against 2.50. Being close to a singularity is not the trigger; asking a nearly singular Jacobian for something it cannot deliver is.

5. Your solver returns after 200 iterations with a residual of 0.05 and no error raised. What are the two explanations, and how do you tell them apart in one line?

Either the target is unreachable, or the iteration simply has not converged - a poor seed, too small a step, a limit in the way. The iterative solver cannot distinguish them about itself, because both look like a residual that stopped shrinking. The closed form can: compute $\cos\theta_2 = \dfrac{r^2 - l_1^2 - l_2^2}{2\,l_1 l_2}$ and check $|\cos\theta_2| \le 1$ , which for this arm is exactly the ring from 0.30 to 1.70. Run that before the solver. A system that reports “out of range” is a different system from one that reports joint angles for a pose it never actually reached.

6. If the closed form is exact and instant, why is the iterative method the one worth learning properly?

Because the closed form is a derivation, not an algorithm. It used the fact that this arm is two links in a plane; add a link, offset an axis, or use a wrist whose axes do not intersect and it has to be redone or may not exist. The iterative loop assumes only that you can compute $\mathrm{FK}(\theta)$ and $J(\theta)$ , so the same six lines drive any mechanism. Where a closed form does exist it is worth using - faster, exact, and it hands you every branch explicitly rather than whichever one the seed happened to find - which is why so many industrial arms are built with a spherical wrist that guarantees one.

Do this

Open code/ik_2link.py. It already has fk; the Jacobian functions are the ones from the Jacobian, so fill those in first if you have not.

Implement ik_dls. Three lines inside the existing loop: build $J$ , solve $\Delta\theta = J^{\top}\left(J J^{\top} + \lambda^2 I\right)^{-1}\mathbf{e}$ with np.linalg.solve, then th += alpha * dth. Run python ik_2link.py. You should see 100/100 targets converged; median iterations 21, and det J at θ2=0 printing 6.00e-17, which is a floating-point zero.
Run the shipped singularity experiment and disbelieve it. python ik_2link.py --singularity targets a point 0.001 inside the outer edge and prints max ‖Δθ‖ per step for $\lambda = 0$ and $\lambda = 0.01$ . Its closing line claims the undamped run whips the joints. Read the two numbers. Both are 1.14. The claim does not reproduce, because the target is reachable and the iteration never has to ask for anything the arm cannot give.
Find the setup that does reproduce it. Change the target in singularity_experiment to L1 + L2 + 0.05, just outside the workspace, and run again. You should get 22.42 against 2.50 on the max step, and final errors of 2.45e+00 against 6.47e-02, which is the run singularities tabulates in full. Write one sentence explaining why moving the target 5 cm further away is what made damping matter.
Add the closed form. Write ik_analytic(target, elbow_up=True) from the two formulas in this lesson, returning None when $|\cos\theta_2| > 1$ . Check three things: that forward kinematics on both branches lands on the target, that $r = 1.71$ and $r = 0.29$ both return None, and that your iterative solver’s answer matches one of the two branches. Then print the elbow position for elbow_up=True with the target at $(1.2, 0.3)$ and again at $(-1.2, 0.3)$ , and satisfy yourself about what that flag really names.
Sweep $\lambda$ where it costs nothing. Give eval_ik a lam argument and run it at $\lambda = 0$ , $0.01$ , $0.05$ and $0.2$ . The first three are indistinguishable, all 100/100 at a median of 21 iterations; only $\lambda = 0.2$ moves, to 26. That is the point: damping is insurance you are not paying for until you need it.

Solution: solutions/ik_2link.py.

What you can now do

You can solve a two-link arm in closed form from the law of cosines, explain why the answer always arrives as a mirrored pair and pick the one you want, and read $|\cos\theta_2| > 1$ as an exact reachability test. You can write the damped least squares loop for any arm whose Jacobian you can compute, choose a step fraction for a reason rather than by feel, and say when the damping term is doing something and when it is idle. Most usefully, you can tell an unreachable target apart from an unconverged solver, which is the failure that actually shows up in a running system.