Reaching a point: inverse kinematics of a two-joint arm, Inverse Kinematics, PID and Control Basics

Given where the hand should be, find the two joint angles. The law of cosines does it in four lines, and the arm has two answers.

Forward and inverse

A robot arm is a chain of links joined by motors. Two questions come up all the time:

	You know	You want	Difficulty
forward kinematics	the joint angles	where the hand is	easy: add up the links
inverse kinematics (IK)	where the hand should be	the joint angles	harder: there may be 0, 1 or 2 answers

Programming an arm with angles is like steering a car by telling it how far to turn each wheel. IK lets you say “put the gripper here” and work out the angles yourself.

Almost every useful arm program is built on IK. A camera finds a cube at some point on the table. A slider on your phone says “10 mm further left”. A drawing program wants the pen to follow a letter. All of them speak in positions, and only IK turns positions into the angles your motors understand.

Forward first

A flat arm with a shoulder at the origin, an upper link of length $L_1$ at angle $\theta_1$ from the x axis, and a forearm of length $L_2$ bent by $\theta_2$ relative to the upper link. The tip is at

\begin{aligned} x &= L_1 \cos\theta_1 + L_2 \cos(\theta_1 + \theta_2) \\ y &= L_1 \sin\theta_1 + L_2 \sin(\theta_1 + \theta_2) \end{aligned}

Each link is an arrow; the tip is where the arrows end when you put them tip to tail. That is vector addition again.

Notice the $\theta_1 + \theta_2$ . The forearm’s direction in the world is the shoulder angle plus the elbow angle, because the elbow motor sits on the upper arm and turns with it. This “relative angle” habit is how most arms are described, and it matters later: the MeArm in chapter 4 measures its forearm differently.

A quick check with numbers. $L_1 = L_2 = 8$ cm, $\theta_1 = 90°$ (upper arm straight up), $\theta_2 = -90°$ (elbow bent right). The upper arm ends at (0, 8). The forearm points at $90° - 90° = 0°$ , straight right, so the tip is at (8, 8). The formulas agree: $x = 8\cos 90° + 8\cos 0° = 0 + 8$ , $y = 8 \sin 90° + 8 \sin 0° = 8$ .

Inverse: two steps with the law of cosines

Step 1, the elbow. The shoulder, the elbow and the target form a triangle with sides $L_1$ , $L_2$ and $d = \sqrt{x^2 + y^2}$ . The law of cosines gives the angle at the elbow directly:

\cos\theta_2 = \frac{x^2 + y^2 - L_1^2 - L_2^2}{2\,L_1 L_2}

Where does this come from? The law of cosines for the triangle says $d^2 = L_1^2 + L_2^2 - 2L_1L_2\cos\gamma$ , where $\gamma$ is the inside angle at the elbow. A straight arm has $\gamma = 180°$ and $\theta_2 = 0$ , so $\gamma = 180° - \lvert\theta_2\rvert$ and $\cos\gamma = -\cos\theta_2$ . Put that in and solve for $\cos\theta_2$ : you get the line above.

Step 2, the shoulder. Point the shoulder at the target, then correct for the bend of the elbow:

\theta_1 = \operatorname{atan2}(y, x) - \operatorname{atan2}\!\big(L_2 \sin\theta_2,\; L_1 + L_2 \cos\theta_2\big)

atan2(y, x) is the angle of the point (x, y), and unlike a plain arctangent it knows which quarter of the plane the point is in. The second atan2 is the angle between the upper arm and the line from the shoulder to the tip: it is how far the bent elbow pulls the tip away from where the upper arm points.

A worked example, all the way through

$L_1 = L_2 = 8$ cm, target (10, 6).

Elbow. $x^2 + y^2 = 100 + 36 = 136$ . $L_1^2 + L_2^2 = 128$ . $2L_1L_2 = 128$ .

\cos\theta_2 = \frac{136 - 128}{128} = 0.0625 \quad\Rightarrow\quad \theta_2 = \pm 86.4°

Shoulder, elbow-up solution ( $\theta_2 = -86.4°$ ). The direction to the target is $\operatorname{atan2}(6, 10) = 31.0°$ . The correction: $L_2\sin\theta_2 = 8 \cdot (-0.998) = -7.98$ and $L_1 + L_2\cos\theta_2 = 8 + 8 \cdot 0.0625 = 8.5$ , so $\operatorname{atan2}(-7.98, 8.5) = -43.2°$ . Then

\theta_1 = 31.0° - (-43.2°) = 74.2°

Check with forward kinematics. $\theta_1 + \theta_2 = 74.2° - 86.4° = -12.2°$ . $x = 8\cos 74.2° + 8\cos(-12.2°) = 2.18 + 7.82 = 10.0$ and $y = 8\sin 74.2° + 8 \sin(-12.2°) = 7.70 - 1.69 = 6.0$ . It lands on the target.

The other solution ( $\theta_2 = +86.4°$ ) gives $\theta_1 = -12.2°$ . The upper arm points slightly down and the elbow bends up. Same tip, very different arm.

Two answers: elbow up, elbow down

$\cos\theta_2$ is the same for $+\theta_2$ and $-\theta_2$ . Both give a valid arm: one with the elbow bent up, one with it bent down, and the tip in exactly the same place. A real arm usually picks the one that keeps the elbow away from the table, or the one closest to where the arm is now, so it does not swing through a big motion.

Notice something neat in the example: the two solutions swap their numbers. Elbow-up is (74.2°, −86.4°), elbow-down is (−12.2°, 86.4°), and $74.2 - 86.4 = -12.2$ . That is no accident: the two arms are mirror images across the line from the shoulder to the target.

In code

from math import acos, atan2, cos, sin, sqrt

def ik(x, y, l1, l2, elbow_up=True):
    """Joint angles (radians) that put the tip at (x, y), or None if out of reach."""
    c2 = (x * x + y * y - l1 * l1 - l2 * l2) / (2 * l1 * l2)
    if c2 < -1 or c2 > 1:
        return None                      # too far, or too close to the shoulder
    t2 = acos(c2)
    if elbow_up:
        t2 = -t2
    t1 = atan2(y, x) - atan2(l2 * sin(t2), l1 + l2 * cos(t2))
    return t1, t2

def fk(t1, t2, l1, l2):
    """Check: the angles back to the tip position."""
    return (l1 * cos(t1) + l2 * cos(t1 + t2),
            l1 * sin(t1) + l2 * sin(t1 + t2))

Always test IK with FK: put the angles you got back into fk() and check you land on the target. It catches sign mistakes immediately.

from math import degrees

t1, t2 = ik(10, 6, 8, 8)
print(round(degrees(t1), 1), round(degrees(t2), 1))   # 74.2 -86.4
print(fk(t1, t2, 8, 8))                               # (10.0, 6.0), give or take 1e-15

Making it robust: three small upgrades

The four-line ik() is correct, but a real arm needs a little more care.

1. Round-off at full reach. A target exactly at full reach can give $\cos\theta_2 = 1.0000000002$ because of how computers store decimals, and acos crashes. Allow a tiny margin, then clamp:

def ik_safe(x, y, l1, l2, elbow_up=True, eps=1e-9):
    c2 = (x * x + y * y - l1 * l1 - l2 * l2) / (2 * l1 * l2)
    if c2 > 1 + eps or c2 < -1 - eps:
        return None
    c2 = max(-1.0, min(1.0, c2))         # 1.0000000002 becomes 1.0
    t2 = acos(c2)
    if elbow_up:
        t2 = -t2
    t1 = atan2(y, x) - atan2(l2 * sin(t2), l1 + l2 * cos(t2))
    return t1, t2

2. Reach as far as you can. If a target is too far, you may want the arm to point at it instead of doing nothing. Scale the target back onto the edge of the reachable circle:

def reach_toward(x, y, l1, l2):
    d = sqrt(x * x + y * y)
    if d > l1 + l2:
        k = (l1 + l2) * 0.999 / d        # 0.1 % inside the edge
        x, y = x * k, y * k
    return ik_safe(x, y, l1, l2)

With $L_1 = L_2 = 8$ and the target (17, 0), this returns about (2.6°, −5.1°): an almost straight arm pointing right, with the tip at (15.98, 0). Whether “reach as far as you can” is a good idea depends on the job. For a drawing arm it is fine. For an arm that should grab something, it is better to refuse and report the problem.

3. Pick the solution nearest to where you are. Compute both, and take the one with the smallest change of joint angles:

def closest(x, y, l1, l2, current):
    """current = (t1, t2) now. Returns the solution that needs the least movement."""
    best = None
    for up in (True, False):
        s = ik_safe(x, y, l1, l2, up)
        if s is None:
            continue
        cost = abs(s[0] - current[0]) + abs(s[1] - current[1])
        if best is None or cost < best[0]:
            best = (cost, s)
    return None if best is None else best[1]

If the arm is at (0, 1 rad), closest(10, 6, 8, 8, ...) picks the elbow-down answer (−12.2°, 86.4°). If it is at (1, −1 rad), it picks elbow-up. Either way the arm never flips its elbow over without a reason.

Erreurs typiques

Degrees vs radians. Python’s sin, cos and atan2 use radians. Servos usually want degrees: degrees() converts.
atan instead of atan2. Plain atan(y / x) cannot tell the point (1, 1) from (−1, −1), and fails when x = 0.
atan2 arguments swapped. It is atan2(y, x), y first. Swapping them mirrors the arm across the diagonal and the FK check fails.
The servo’s zero is not the maths’ zero. A servo at “90°” might be the arm pointing straight up. Add an offset for each joint, and check the direction too.
Rounding just past 1. A target exactly at full reach can give $\cos\theta_2 = 1.0000000002$ and crash acos. Clamp to [−1, 1] when you are within a tiny margin.
Mixing units. Link lengths in cm and targets in mm give nonsense without an error message. Pick one unit (the Robot Arm guide uses millimetres) and stick to it.

Check yourself

Prédis d'abord

An arm has $L_1 = 10$ and $L_2 = 6$ . What is the farthest point it can reach, and what is the closest?
For a target at distance exactly $d = L_1 + L_2$ , how many IK solutions are there?
You compute IK, run FK on the result, and land at (6, 10) instead of (10, 6). What is the most likely bug?
Why does the formula for $\theta_1$ need $\theta_2$ first?

Voir la réponse

Farthest: $10 + 6 = 16$ , arm straight. Closest: $10 - 6 = 4$ , forearm folded back. Points nearer than 4 to the shoulder are out of reach too, which is why ik() also checks $\cos\theta_2 \lt -1$ .
One: elbow-up and elbow-down become the same straight arm, because $+0 = -0$ .
The arguments of atan2 are swapped somewhere (x and y exchanged), which mirrors the result across the line y = x.
The correction term is the angle between the upper arm and the line to the tip, and that depends on how much the elbow is bent.

What the full course covers

Lesson 1 is free. The other lessons come with the course.

Reaching a point: inverse kinematics of a two-joint arm (this chapter)
Workspace, singularities and joint limits
Coordinate frames and transforms
A real arm in 3D: the MeArm
The Jacobian and numerical IK
Feedback: P, I and D explained with a cruise control
Measuring a DC motor: encoder, step response and a simple model
Tuning a motor speed loop on real hardware
Position over speed: cascaded loops and motion profiles
An arm that follows a target smoothly