Inverse kinematics of a two-link arm

Inverse kinematics (IK) is the question you actually care about when building an arm: I want the hand at (x, y), so what angles do I send to the servos? It is harder than FK for a good reason. A given target can have two solutions, one, or none at all. Understanding why those cases appear is the difference between an arm that works and one that twitches at the edge of its reach.

Setting up

Use the same planar two-link arm as before: lengths L1 and L2, joint angles t1 and t2, and the forward equations

x = L1*cos(t1) + L2*cos(t1 + t2) y = L1*sin(t1) + L2*sin(t1 + t2)

We know x and y and want t1 and t2. These two equations are nonlinear and tangled together, but geometry untangles them.

Step 1: the elbow angle from the law of cosines

Square both equations and add them. The cross terms use cos^2 + sin^2 = 1 and the identity cos(t1)*cos(t1+t2) + sin(t1)*sin(t1+t2) = cos(t2):

x^2 + y^2 = L1^2 + L2^2 + 2*L1*L2*cos(t2)

This is exactly the law of cosines for the triangle formed by the two links and the line from base to target. Solve for cos(t2):

D = cos(t2) = (x^2 + y^2 - L1^2 - L2^2) / (2*L1*L2)

The elbow angle depends only on how far the target is from the base. The target direction does not matter yet.

Step 2: elbow-up or elbow-down

Knowing cos(t2) does not give a unique angle, because cos(t2) = cos(-t2). There are two solutions:

t2 = +acos(D) or t2 = -acos(D)

They are mirror images of each other across the line from base to target. With the arm pointing to the right, a positive t2 bends the second link counter-clockwise, which puts the elbow below the line: elbow-down. A negative t2 puts the elbow above it: elbow-up. Both reach the same point.

Step 3: the shoulder angle

In the frame of link 1, the tip sits at (L1 + L2*cos(t2), L2*sin(t2)). Link 1 is rotated by t1 from the x axis, and the target direction is rotated by beta from link 1, where

beta = atan2(L2*sin(t2), L1 + L2*cos(t2))

So the target direction is atan2(y, x) = t1 + beta, which gives

t1 = atan2(y, x) - atan2(L2*sin(t2), L1 + L2*cos(t2))

Use atan2, not atan, so the quadrant comes out right.

A worked example

Take L1 = 100 mm, L2 = 80 mm and target (120, 100) mm.

1. x^2 + y^2 = 14400 + 10000 = 24400.
2. D = (24400 - 10000 - 6400) / (2*100*80) = 8000 / 16000 = 0.5.
3. t2 = acos(0.5) = 60 degrees for elbow-down, or -60 degrees for elbow-up.
4. Target direction: atan2(100, 120) = 39.81 degrees.
5. Elbow-down: beta = atan2(80*sin 60, 100 + 80*cos 60) = atan2(69.28, 140) = 26.33 degrees, so t1 = 39.81 - 26.33 = 13.48 degrees.
6. Elbow-up: beta = -26.33 degrees, so t1 = 39.81 + 26.33 = 66.14 degrees.

Verify elbow-down with FK: x = 100*cos 13.48 + 80*cos 73.48 = 97.24 + 22.75 = 119.99 mm and y = 100*sin 13.48 + 80*sin 73.48 = 23.31 + 76.71 = 100.02 mm. It matches the target. Always run this check in your own code, because a sign error in IK is easy to make and FK will catch it instantly.

x110

y70

elbow: up

theta1 = 70.3°

theta2 = -87.9°

L1 = 100, L2 = 80

Drag the target in the widget and watch both solutions. As you pull the target out toward the edge of the ring, the elbow straightens. Drag it past the edge and there is no solution: the arm cannot reach.

Reachability and singularities

A solution exists only when the law of cosines triangle exists, that is when -1 <= D <= 1. In terms of distance r = sqrt(x^2 + y^2):

|L1 - L2| <= r <= L1 + L2

For our arm that is 20 mm to 180 mm. Outside this ring D has magnitude above 1 and acos(D) has no real value.

The boundary cases are the singularities. At D = 1 the arm is fully stretched (t2 = 0), and at D = -1 it is folded flat (t2 = 180 degrees). Here the two solutions merge into one, and the arm loses the ability to move the tip radially. The numbers behind the trouble: d(acos D)/dD = -1/sqrt(1 - D^2), which grows without bound as D approaches 1. At r = 179 mm, D = 0.9776 and t2 = 12.2 degrees. At r = 179.9 mm, t2 = 3.8 degrees. At r = 180 mm, t2 = 0. A target that moves just one millimetre changes the elbow by 12 degrees, so near the edge, tiny errors in the target become large, fast joint motions. The next lesson explains this with the Jacobian.

Code

import numpy as np

L1, L2 = 100.0, 80.0

def ik(x, y, elbow_down=True):
    D = (x*x + y*y - L1**2 - L2**2) / (2 * L1 * L2)
    if abs(D) > 1.0:
        return None                      # target out of reach
    s2 = np.sqrt(1.0 - D*D) * (1 if elbow_down else -1)
    t2 = np.arctan2(s2, D)
    t1 = np.arctan2(y, x) - np.arctan2(L2 * s2, L1 + L2 * D)
    return t1, t2

for down in (True, False):
    t1, t2 = ik(120, 100, down)
    print(down, np.degrees(t1).round(2), np.degrees(t2).round(2))
# True  13.48  60.0
# False 66.14 -60.0

On the Arduino, the same logic with a servo mapping:

#include <Servo.h>

const float L1 = 100.0, L2 = 80.0;   // mm
Servo shoulder, elbow;

bool solveIK(float x, float y, bool elbowDown, float &t1, float &t2) {
  float D = (x*x + y*y - L1*L1 - L2*L2) / (2.0 * L1 * L2);
  if (fabs(D) > 1.0) return false;           // unreachable
  float s2 = sqrt(1.0 - D*D);
  if (!elbowDown) s2 = -s2;
  t2 = atan2(s2, D);
  t1 = atan2(y, x) - atan2(L2 * s2, L1 + L2 * D);
  return true;
}

void setup() {
  shoulder.attach(10);
  elbow.attach(9);
  float t1, t2;
  if (solveIK(120, 100, true, t1, t2)) {
    // Offsets depend on how the horns were mounted: calibrate them
    shoulder.write(constrain(t1 * RAD_TO_DEG, 0, 180));
    elbow.write(constrain(90 + t2 * RAD_TO_DEG, 0, 180));
  }
}

void loop() {}

Notice the constrain(): a mathematically valid solution can still be outside what the servo or the frame allows. Always test the result against the joint limits and, if it fails, try the other elbow configuration.