Stage 08 · Lesson 12/15

Kinematics & Inverse Kinematics · Lesson 12

Inverse Kinematics — Concept

Est. 60–80 min

Why

Why this lesson matters

Forward kinematics (Lessons 6–10) answers: *given the joint angles, where is the gripper?* Real jobs ask the opposite: *I want the gripper here — what joint angles do I need?* That is inverse kinematics (IK). It is what lets you click a point, or let a camera find an object, and have the arm go there.

By the end of this lesson you will be able to: state the IK problem; explain why it can have zero, one, two or many answers while FK has exactly one; solve a planar two-link arm with the law of cosines and atan2; split the example arm’s ‘pick from above’ problem into base yaw + a two-link problem + wrist pitch; and follow the safe IK routine — check reach, solve, verify with FK, check joint limits, then move.

A person reading at a wooden table in warm evening light, reaching out to pick up a pink cup
Reaching for a cup while reading: your brain picks shoulder, elbow and wrist angles for a target it sees — inverse kinematics, done without thinking.Photo: Shixart1985 · CC BY 2.0 · Source: Wikimedia Commons · resized, converted to WebP

Concept

Turning the question around

FK vs IK

Forward kinematicsInverse kinematics
InputJoint anglesDesired gripper position (and orientation)
OutputGripper poseJoint angles
How many answers?Exactly oneZero, one, two … or infinitely many
DifficultyMultiply matricesSolve equations (by formula or by searching)

Solving the two-link arm

For a planar arm with links L1 and L2 and a target (x, y): draw the triangle *shoulder – elbow – target*. You know all three sides: L1, L2 and d = √(x² + y²). The law of cosines (c² = a² + b² − 2ab·cos C) then gives every angle of the triangle.

2R IK (as in Modern Robotics, Chapter 6) · Text
d² = x² + y²                     (target must satisfy |L1 − L2| ≤ d ≤ L1 + L2)
β = acos((L1² + L2² − d²) / (2·L1·L2))     elbow interior angle
α = acos((d² + L1² − L2²) / (2·L1·d))      angle between L1 and the target line
γ = atan2(y, x)                             direction of the target

Answer A:  q1 = γ − α,   q2 = 180° − β
Answer B:  q1 = γ + α,   q2 = β − 180°

0, 1 or 2 answers

  • Outside the reach ring (too far or too close): no answer. No solver can fix this — move the target or the robot.
  • Exactly on the edge (fully stretched or fully folded): one answer.
  • Inside the ring: two answers. A three-link planar arm reaching only a position has infinitely many — it is ‘redundant’.

The safe IK routine

  1. 1. Check reach

    Is the target inside the workspace (Lesson 11)?

  2. 2. Solve

    Get all the answers.

  3. 3. Verify with FK

    Put each answer back into FK. The error should be almost zero; if not, the IK has a bug.

  4. 4. Check limits and collisions

    Throw away answers outside joint limits or that hit the table or the base.

  5. 5. Choose and move slowly

    Pick the answer closest to where the arm is now (Lesson 14), then move in small steps.

Visual

Visualise it: target → joints → check

Educational 2R IK: pick a target, see both elbow solutions, verify each with FK and read the error. Simplified planar solver — not full 6DOF IK.

Interactive · IK solver lab (analytical 2R — educational)

IK concept: target to jointsSingularity sketch

Workflow: target → IK → joints → FK verify → residual. Not a black box — elbow-up/down closed form. Not full 6DOF analytic IK.

Status · ok

2 analytical solution(s) (2R planar).

θ₁

-12.2°

θ₂

92.3°

FK err

0.000

Code lab

Code lab: ik_concept.py

ik_concept.py Part 1 solves the planar 2R arm for four targets and verifies every answer with FK. Part 2 solves the example arm for a pick *straight down*: J1 turns the base towards the target, the wrist centre must sit L4 straight above the tip, a 2R problem places the wrist, and J4 makes the pitch angles add up to −90°. Each answer is checked with FK and against the Lesson 11 joint limits.

ik_concept.py · Python
# ik_concept.py - inverse kinematics: target -> joint angles -> FK verify -> error.
# Part 1: planar 2R arm with the law of cosines (Modern Robotics, Section 6 intro): 0, 1 or 2 answers.
# Part 2: the example 5-joint arm picking from straight above, solved by splitting it into
#         base yaw (J1) + a 2R problem for the wrist centre + wrist pitch to keep the gripper down.
import math

# ---------- Part 1: planar 2R ----------
L1, L2 = 10.0, 8.0


def fk2(q1, q2):
    a, b = math.radians(q1), math.radians(q1 + q2)
    return L1 * math.cos(a) + L2 * math.cos(b), L1 * math.sin(a) + L2 * math.sin(b)


def ik2(x, y, l1=L1, l2=L2):
    """Return a list of (q1, q2) in degrees: [] if out of reach, else elbow-down and elbow-up."""
    d2 = x * x + y * y
    d = math.sqrt(d2)
    if d > l1 + l2 + 1e-9 or d < abs(l1 - l2) - 1e-9:
        return []
    beta = math.acos(max(-1, min(1, (l1 * l1 + l2 * l2 - d2) / (2 * l1 * l2))))   # elbow interior angle
    alpha = math.acos(max(-1, min(1, (d2 + l1 * l1 - l2 * l2) / (2 * l1 * d)))) if d > 0 else 0
    gamma = math.atan2(y, x)
    sols = [(gamma - alpha, math.pi - beta), (gamma + alpha, beta - math.pi)]   # "righty", "lefty"
    out = []
    for q1, q2 in sols:
        s = (math.degrees(q1), math.degrees(q2))
        if all(math.dist(s, o) > 1e-9 for o in out):
            out.append(s)
    return out


print(f"Part 1 - planar 2R arm, L1 = {L1}, L2 = {L2} cm (reach ring {abs(L1 - L2)} .. {L1 + L2} cm)")
for target in ((12.0, 6.0), (18.0, 0.0), (1.0, 0.5), (20.0, 5.0)):
    sols = ik2(*target)
    print(f"  target {target}: {len(sols)} solution(s)")
    for q1, q2 in sols:
        x, y = fk2(q1, q2)
        err = math.dist((x, y), target)
        print(f"     q = ({q1:7.2f}, {q2:7.2f}) deg  -> FK ({x:6.2f}, {y:6.2f})  error {err:.1e} cm")

# ---------- Part 2: example arm, pick straight down ----------
D1, A2, A3, L4 = 9.0, 10.5, 9.8, 14.0


def fk5(q):
    j1, j2, j3, j4 = (math.radians(v) for v in q[:4])
    r = A2 * math.cos(j2) + A3 * math.cos(j2 + j3) + L4 * math.cos(j2 + j3 + j4)
    z = D1 + A2 * math.sin(j2) + A3 * math.sin(j2 + j3) + L4 * math.sin(j2 + j3 + j4)
    return r * math.cos(j1), r * math.sin(j1), z


def ik_pick_down(x, y, z):
    j1 = math.degrees(math.atan2(y, x))                # 1) turn the base to face the target
    r = math.hypot(x, y)
    rw, zw = r, z + L4                                 # 2) wrist centre sits L4 straight above the tip
    sols = []
    for j2, j3 in ik2(rw, zw - D1, A2, A3):            # 3) 2R problem in the arm's vertical plane
        j4 = -90 - j2 - j3                             # 4) pitch angles must add to -90 (pointing down)
        sols.append((j1, j2, j3, j4, 0.0))
    return sols


print(f"\nPart 2 - example arm (D1 {D1}, L2 {A2}, L3 {A3}, L4 {L4} cm), gripper straight down")
for target in ((15.0, 8.0, 3.0), (18.0, 0.0, 0.0), (0.0, 22.0, 6.0), (30.0, 10.0, 2.0)):
    sols = ik_pick_down(*target)
    if not sols:
        print(f"  target {target}: NO solution - wrist centre out of reach for a straight-down pick")
        continue
    for q in sols:
        p = fk5(q)
        qs = "(" + ", ".join(f"{v:.2f}" for v in q) + ")"
        inside = 0 <= q[1] <= 180 and -150 <= q[2] <= 0 and -120 <= q[3] <= 90   # Lesson 11 example limits
        px, py, pz = (round(v, 6) + 0.0 for v in p)
        print(f"  target {target}: q = {qs}  -> FK ({px:6.2f}, {py:6.2f}, {pz:6.2f})  "
              f"error {math.dist(p, target):.0e} cm  {'within limits' if inside else 'OUTSIDE joint limits'}")

print("\nWhat to notice")
print("- FK has ONE answer; IK can have 0, 1 or 2 here (and more for longer chains)")
print("- always check IK by running FK on the answer: error should be ~0")
print("- before sending to a real arm, also check joint limits (e.g. J4 within servo travel)")

Output (run with python3 ik_concept.py)

Part 1 - planar 2R arm, L1 = 10.0, L2 = 8.0 cm (reach ring 2.0 .. 18.0 cm)
  target (12.0, 6.0): 2 solution(s)
     q = (  -9.83,   84.26) deg  -> FK ( 12.00,   6.00)  error 3.2e-15 cm
     q = (  62.96,  -84.26) deg  -> FK ( 12.00,   6.00)  error 2.7e-15 cm
  target (18.0, 0.0): 1 solution(s)
     q = (   0.00,    0.00) deg  -> FK ( 18.00,   0.00)  error 0.0e+00 cm
  target (1.0, 0.5): 0 solution(s)
  target (20.0, 5.0): 0 solution(s)

Part 2 - example arm (D1 9.0, L2 10.5, L3 9.8, L4 14.0 cm), gripper straight down
  target (15.0, 8.0, 3.0): q = (28.07, 3.74, 44.53, -138.28, 0.00)  -> FK ( 15.00,   8.00,   3.00)  error 4e-15 cm  OUTSIDE joint limits
  target (15.0, 8.0, 3.0): q = (28.07, 46.66, -44.53, -92.13, 0.00)  -> FK ( 15.00,   8.00,   3.00)  error 0e+00 cm  within limits
  target (18.0, 0.0, 0.0): q = (0.00, -6.68, 46.10, -129.41, 0.00)  -> FK ( 18.00,   0.00,   0.00)  error 1e-14 cm  OUTSIDE joint limits
  target (18.0, 0.0, 0.0): q = (0.00, 37.73, -46.10, -81.64, 0.00)  -> FK ( 18.00,   0.00,   0.00)  error 1e-14 cm  within limits
  target (0.0, 22.0, 6.0): NO solution - wrist centre out of reach for a straight-down pick
  target (30.0, 10.0, 2.0): NO solution - wrist centre out of reach for a straight-down pick

What to notice
- FK has ONE answer; IK can have 0, 1 or 2 here (and more for longer chains)
- always check IK by running FK on the answer: error should be ~0
- before sending to a real arm, also check joint limits (e.g. J4 within servo travel)

Try it: set a target, see both elbow answers, and read the FK error. Move the target outside the dashed circle and watch the solver report no solution. (Educational 2R solver — simplified.)

Interactive · IK solver lab (analytical 2R — educational)

IK concept: target to jointsSingularity sketch

Workflow: target → IK → joints → FK verify → residual. Not a black box — elbow-up/down closed form. Not full 6DOF analytic IK.

Status · ok

2 analytical solution(s) (2R planar).

θ₁

-12.2°

θ₂

92.3°

FK err

0.000

Real world

IK in the real world

  • Pick and place: a camera or conveyor gives the object’s position; IK turns it into joint angles.
  • Animation: IK places an animation rig’s hand or foot at a target — the same maths as a robot arm, and animation tools often solve it numerically (Lesson 13).
  • Palletising: a robot stacks boxes at a grid of target positions; each one is an IK problem.
A labelled product drawing of a collaborative palletiser: a UR10e cobot on a lift column with a vacuum gripper, an operator screen, a small conveyor and a wooden pallet
A collaborative palletiser built around a UR10e cobot. The conveyor delivers boxes to a known pick point and each box goes to a known place on the pallet — every move starts as an IK problem.Photo: CollaborativePalletizer · CC BY-SA 4.0 · Source: Wikimedia Commons · resized, converted to WebP

Bench build

Bench build: IK on a real two-servo arm

Reuse the two-servo arm from Lesson 6 (L1 = 10 cm, L2 = 8 cm). ik_2servo.ino takes a target ‘x y’ in cm, checks reach, solves both answers, keeps only one inside safe servo travel (−70°…70°), verifies it with FK, and then moves both servos slowly together. Tape graph paper under the arm with the shoulder shaft at (0, 0).

  1. Power first

    Servos on their own 5–6 V supply (2 A or more). Supply GND joined to Arduino GND. Shoulder signal → pin 5, elbow signal → pin 6.

  2. Zero the joints

    Find the servo commands that make each link point along +x and put them in SH_ZERO and EL_ZERO.

  3. Try targets

    Type 12 6, then 15 0, then 8 8. Mark where the tip lands and measure the error.

  4. Try a bad target

    Type 25 0. The sketch must refuse and not move.

ik_2servo.ino · Arduino C++
// ik_2servo.ino - inverse kinematics on a real 2-link servo arm (the Lesson 6 build).
// Type a target "x y" in cm (e.g. "12 6"). The sketch:
//   1) checks the target is inside the reach ring |L1 - L2| .. L1 + L2,
//   2) computes both IK answers (elbow one way / the other) with the law of cosines,
//   3) keeps only answers inside safe servo travel (-70..70 deg),
//   4) runs FK on the chosen answer and prints the error, then moves the servos SLOWLY.
// SERVOS run from their OWN 5-6 V supply (2 A or more), NEVER from the Arduino 5 V pin.
// Join the servo supply GND to Arduino GND. Signal wires: shoulder -> pin 5, elbow -> pin 6.
#include <Servo.h>
const float L1 = 10.0, L2 = 8.0;                 // cm, shaft to shaft and shaft to tip - MEASURE
const int SH_ZERO = 90, EL_ZERO = 90;            // servo command that gives 0 deg for each joint
const float QMIN = -70, QMAX = 70;               // safe travel for both joints
Servo shoulder, elbow;
float cur1 = 0, cur2 = 0;

bool ik(float x, float y, int which, float &q1, float &q2) {
  float d2 = x * x + y * y, d = sqrt(d2);
  if (d > L1 + L2 || d < fabs(L1 - L2) || d < 0.001) return false;
  float beta = acos(constrain((L1 * L1 + L2 * L2 - d2) / (2 * L1 * L2), -1, 1));
  float alpha = acos(constrain((d2 + L1 * L1 - L2 * L2) / (2 * L1 * d), -1, 1));
  float gamma = atan2(y, x);
  if (which == 0) { q1 = degrees(gamma - alpha); q2 = degrees(PI - beta); }
  else            { q1 = degrees(gamma + alpha); q2 = degrees(beta - PI); }
  return q1 >= QMIN && q1 <= QMAX && q2 >= QMIN && q2 <= QMAX;
}

void moveSlowly(float t1, float t2) {            // 1 degree per step, both joints together
  while (fabs(t1 - cur1) > 0.5 || fabs(t2 - cur2) > 0.5) {
    cur1 += constrain(t1 - cur1, -1, 1);
    cur2 += constrain(t2 - cur2, -1, 1);
    shoulder.write(SH_ZERO + (int)round(cur1));
    elbow.write(EL_ZERO + (int)round(cur2));
    delay(15);
  }
}

void setup() {
  shoulder.attach(5); elbow.attach(6);
  shoulder.write(SH_ZERO); elbow.write(EL_ZERO);
  Serial.begin(115200);
  Serial.println(F("Type target: x y   (cm, reach ring 2..18)"));
}

void loop() {
  if (!Serial.available()) return;
  float x = Serial.parseFloat(), y = Serial.parseFloat();
  while (Serial.available()) Serial.read();
  float q1, q2;
  bool ok = ik(x, y, 0, q1, q2) || ik(x, y, 1, q1, q2);   // try one elbow, then the other
  if (!ok) { Serial.println(F("No safe solution: out of reach or outside servo travel. Not moving.")); return; }
  float fx = L1 * cos(radians(q1)) + L2 * cos(radians(q1 + q2));
  float fy = L1 * sin(radians(q1)) + L2 * sin(radians(q1 + q2));
  Serial.print(F("IK q = (")); Serial.print(q1, 1); Serial.print(F(", ")); Serial.print(q2, 1);
  Serial.print(F(")  FK check (")); Serial.print(fx, 2); Serial.print(F(", ")); Serial.print(fy, 2);
  Serial.print(F(")  error ")); Serial.print(sqrt(sq(fx - x) + sq(fy - y)), 3); Serial.println(F(" cm  -> moving"));
  moveSlowly(q1, q2);
}
A pad of blue-grid graph paper and a few loose sheets with handwritten notes, with a black pen, on a wooden desk
Graph paper under the arm turns every IK test into a measurement: mark the target, mark where the tip landed, and measure the gap.Photo: Jose Camões Silva · CC BY 2.0 · Source: Wikimedia Commons · resized, converted to WebP

Debugging

Debugging IK

SymptomLikely causeFix
acos ‘math domain error’ / NaNTarget out of reach, or rounding just outside ±1Check reach first; clamp the cos value to [−1, 1]
Arm goes to the mirror-image pointatan used instead of atan2, or x/y swappedUse atan2(y, x)
FK of the IK answer is not the targetSign or formula error in IKAlways verify with FK; test with known poses
Real arm lands off by a constant angleServo zero offsetRe-zero (SH_ZERO, EL_ZERO)
Arm flips elbow between nearby targetsPicking a different answer each timeChoose the answer closest to the current pose (Lesson 14)
Servo buzzes at the targetAnswer near the end of servo travelKeep a margin inside the limits

Think

Think about it

Think

Aditi says, “IK is just FK backwards, so it should always have one answer.” Use your elbow to show why she is wrong.

Experiment

Try it in the lab

About 45 minutes with ik_concept.py, graph paper and (optionally) the two-servo arm.

Experiment

Solve, verify, choose

You will need

  • `ik_concept.py` and Python 3
  • Graph paper, ruler, protractor
  • Optional: two-servo arm from Lesson 6, separate 5–6 V servo supply, Arduino UNO

Steps

  1. Run `ik_concept.py`. Draw both answers for (12, 6) on graph paper with a protractor.
  2. Add the targets (2, 0) and (0, 18). Predict how many answers each has, then run.
  3. In Part 2, find a target on the table (z = 0) that has exactly one answer inside the limits.
  4. Change the joint-limit check to J3 between −90 and 0. Which earlier answers are now rejected?
  5. If you built the arm, test three targets with `ik_2servo.ino` and record target vs measured tip.
  6. In the IK solver lab, find the target where the two elbow answers merge into one.

Interactive · IK solver lab (analytical 2R — educational)

IK concept: target to jointsSingularity sketch

Workflow: target → IK → joints → FK verify → residual. Not a black box — elbow-up/down closed form. Not full 6DOF analytic IK.

Status · ok

2 analytical solution(s) (2R planar).

θ₁

-12.2°

θ₂

92.3°

FK err

0.000

Inside the reach ring there are two answers that differ by the elbow’s bend; on the edge they merge; outside there is none. A correct-looking answer is not enough — FK verification, joint limits and the table decide which answer the robot may use.

Knowledge check

Check your understanding

Knowledge check

1. Inverse kinematics answers which question?

2. A 2R arm (10 cm, 8 cm) and a target 12 cm away. How many IK answers?

3. The same arm and a target 20 cm away…

4. Which rule gives the elbow angle from three known side lengths?

5. Why use atan2(y, x) instead of atan(y/x)?

6. For a straight-down pick with the example arm, where must the wrist centre be?

7. IK gives an answer with zero FK error, but J3 is outside its limit. You should…

Mini assignment

Hands-on assignment

Mini assignment

IK test sheet

Make a test sheet that proves your IK works and fails safely.

Deliverables

  • Five 2R targets: inside, on the outer edge, inside the hole, outside, and on the y axis — with the number of answers predicted and found.
  • Both answers for one target drawn to scale on graph paper.
  • Three straight-down picks for the example (or your) arm with FK error and limit check.
  • If you built the arm: target vs measured tip for three targets, with the error.
  • One paragraph on how your code refuses unsafe targets.

Do this, step by step

  1. Run python3 ik_concept.py. Add five 2R targets: inside, on the outer edge (18 cm), inside the hole (under 2 cm), outside, and on the y axis. Predict the number of answers first.
  2. Pick one target with two answers and draw both arm shapes to scale on graph paper.
  3. Run Part 2 for three straight-down picks of your own and record the FK error and whether every joint is inside its limit.
  4. Try a target outside reach and confirm the code refuses it instead of giving a wrong answer.
  5. Optional bench: flash ik_2servo.ino, send three targets, and measure the real tip with a ruler.

Stuck? Hints, one at a time

NoteTry the task first. Read one hint, try again, and only then read the next one.
  1. Use the law of cosines to get the elbow angle first, then the shoulder angle.
  2. If the cosine of the elbow angle is above 1 or below −1, the target is out of reach.
  3. For straight-down picks, solve for the wrist point first: move up by the tool length from the target.

Challenges (pick by level)

  • 🟢 Easy: Solve the 2R arm for (0, 12) by hand and check with the script.
  • 🟢 Easy: Explain why (18, 0) has only one answer.
  • 🟡 Medium: Add a 45°-down pick to Part 2 (pitch angles add to −45°).
  • 🟠 Hard-ish: Make the Arduino sketch pick the answer closest to the current joint angles instead of ‘first that fits’.
  • 🔴 Hard: Add an elbow-height check: reject answers where the elbow goes below the table.
  • ⚫ Research: Find out how character animation software uses IK for feet on uneven ground.

Next step

You solved IK with a formula. That only works for some arms. In Lesson 13 — Analytical vs Numerical IK you will compare formula-based IK with iterative solvers that ‘walk’ the joints towards the target.

Look back: Lesson 11: Robot Workspace. Next: Lesson 13: Analytical vs Numerical IK. Related: Stage 1: Trigonometry for Robots and Stage 7: Saturation, Windup & Safety Limits.

Sources

Self-attested for now — keep artifacts in your lab notebook.

Complete

Today's goal

0 of 1 done today

0-day streak
Daily goal:lessons, quizzes or assignments a day

Mark this lesson complete when you have worked the experiment, knowledge check, and assignment (even roughly).

Stuck somewhere?

Ask a doubt about this lesson

Saved with this lesson's id and your email so we can reply. Privacy · or email contact@aihubstation.in