Leg inverse kinematics

To make a robot walk you do not think in servo angles. You think in foot positions: "put the front left foot 40 mm ahead, 120 mm below the hip". Forward kinematics turns joint angles into a foot position, which is easy. Inverse kinematics (IK) goes the other way, from a wanted foot position to the joint angles that achieve it. Each leg is a short chain of rigid links, and IK is where trigonometry earns its keep.

Splitting the leg into two problems

A 3-DOF leg looks hard, but it splits cleanly:

1. The hip pitch and knee servos move the foot inside one vertical plane. That is a planar two-link arm.
2. The hip abduction servo rotates that whole plane sideways around the body's long axis.

Solve the abduction angle first, rotate the target into the leg plane, then solve the planar problem. Set up coordinates with the origin at the hip: x forward, y sideways (outward is positive), z straight down.

The planar two-link solution

In the leg plane, call the thigh length L1 and the shin length L2. The foot target is at horizontal offset x and vertical drop zp from the hip. The straight-line distance is:

d = sqrt(x² + zp²)

The hip, knee and foot form a triangle with sides L1, L2 and d. The law of cosines gives its angles from the side lengths:

cos(knee interior) = (L1² + L2² - d²) / (2 x L1 x L2)

cos(beta) = (L1² + d² - L2²) / (2 x L1 x d)

Here beta is the angle at the hip between the thigh and the line to the foot. The line itself is tilted from vertical by alpha = atan2(x, zp). The servo values are then:

• Knee bend = 180° - knee interior (0° means a straight leg).
• Hip pitch = alpha + beta for a knee that points forward. Use alpha - beta for the other bend direction.

Why the law of cosines and not just a table of angles? Because it works for any target, it needs only two lengths, and a foot position that is out of reach shows up directly as a cosine outside the range from -1 to 1.

A worked example

Take L1 = 80 mm, L2 = 90 mm, and a foot target directly under the hip, 120 mm down: x = 0, zp = 120.

• d = 120 mm
• cos(interior) = (6400 + 8100 - 14400) / 14400 = 0.0069, so the interior angle is 89.6° and the knee bend is 180 - 89.6 = 90.4°.
• cos(beta) = (6400 + 14400 - 8100) / (2 x 80 x 120) = 0.6615, so beta = 48.6°.
• alpha = atan2(0, 120) = 0°, so the hip pitch is 48.6°.

Check by forward kinematics. The knee is at (80 sin 48.6°, 80 cos 48.6°) = (60.0, 52.9). The shin points at 48.6° - 90.4° = -41.8° from vertical, so the foot is at (60.0 + 90 sin(-41.8°), 52.9 + 90 cos(-41.8°)) = (60.0 - 60.0, 52.9 + 67.1) = (0, 120). It lands on the target.

Try it

A leg is a two-link chain, just like a robot arm. Drag the target and watch the elbow solution change. In the solver, the first link is the thigh and the second is the shin.

x110

y70

elbow: up

theta1 = 70.3°

theta2 = -87.9°

L1 = 100, L2 = 80

The third servo: hip abduction

Looking at the robot from the front, the leg swings sideways by an angle phi. With the foot at lateral offset y and vertical drop z:

phi = atan2(y, z)

After that rotation, the leg plane sees a longer vertical distance:

zp = sqrt(y² + z²)

Feed x and zp into the planar solution. For example, with y = 30 mm and z = 120 mm: phi = atan2(30, 120) = 14.0° and zp = sqrt(900 + 14400) = 123.7 mm. The planar solver then simply sees a slightly longer reach.

The Arduino function

#include <math.h>

const float L1 = 80.0;   // thigh length, mm
const float L2 = 90.0;   // shin length, mm

struct LegAngles {
  float roll;   // hip abduction, degrees
  float hip;    // hip pitch, degrees
  float knee;   // knee bend, degrees (0 = straight leg)
  bool  ok;     // false if the target was out of reach
};

LegAngles legIK(float x, float y, float z) {
  LegAngles a;

  // Step 1: abduction, then rotate the target into the leg plane
  a.roll = atan2(y, z) * RAD_TO_DEG;
  float zp = sqrt(y * y + z * z);

  // Step 2: distance from hip to foot, kept inside the reachable range
  float d    = sqrt(x * x + zp * zp);
  float dMax = L1 + L2 - 0.5;
  float dMin = fabs(L1 - L2) + 0.5;
  a.ok = (d >= dMin && d <= dMax);
  d = constrain(d, dMin, dMax);

  // Step 3: law of cosines
  float cosK = (L1 * L1 + L2 * L2 - d * d) / (2.0 * L1 * L2);
  float cosB = (L1 * L1 + d * d - L2 * L2) / (2.0 * L1 * d);
  float interior = acos(constrain(cosK, -1.0, 1.0));
  float beta     = acos(constrain(cosB, -1.0, 1.0));
  float alpha    = atan2(x, zp);

  a.knee = 180.0 - interior * RAD_TO_DEG;
  a.hip  = (alpha + beta) * RAD_TO_DEG;
  return a;
}

// Convert a joint angle to a servo angle.
// sign: +1 or -1 for mirrored servos, offset: calibration in degrees.
float toServo(float jointDeg, float sign, float offsetDeg) {
  return constrain(90.0 + sign * jointDeg + offsetDeg, 0.0, 180.0);
}

Use it like this for the front left leg (channels 0, 1, 2). The knee needs an offset of -90°, because 0° of bend (a straight leg) is placed at one end of travel and a bend of 90° should land on the servo's centre:

LegAngles a = legIK(0.0, 0.0, 120.0);
setServoAngle(0, toServo(a.roll, +1, 0));
setServoAngle(1, toServo(a.hip,  +1, 0));
setServoAngle(2, toServo(a.knee, +1, -90));

For the worked target this sends 138.6° to the hip servo and 90.4° to the knee servo.

Reachability and cost

The foot can only reach distances between |L1 - L2| = 10 mm and L1 + L2 = 170 mm from the hip. At full stretch the leg is nearly singular: a tiny change in d needs a large knee change and the servo has almost no leverage. Keep working targets between about 60% and 90% of full reach, here 100 to 150 mm.

On an 8-bit AVR, acos, atan2 and sqrt in floating point take on the order of 100 µs each, so one leg costs about 1 ms and four legs about 4 ms of a 20 ms control tick. An ESP32 does the same in a few microseconds.