Inverse kinematics of a two-joint arm

Inverse kinematics is the calculation that finds the joint angles which put a robot hand on a chosen point. It is the question a robot actually has to answer, because a person says where the hand should go and never which angles to set.

In this lesson you solve it for a two-joint arm and find that a target can have two answers, or none.

Forward kinematics goes from angles to a hand position, and there is only ever one answer. Inverse kinematics goes the other way, from a target position back to angles.

Going backwards is harder. Many angle pairs can put the hand in the same place, and for some targets no pair works at all.

The target (x, y) is a distance d from the base. Link 1, link 2 and that distance form a triangle, so the elbow angle follows from the law of cosines.

Work out c = (x squared + y squared - l1 squared - l2 squared) divided by 2 times l1 times l2. Then the elbow angle a2 is the inverse cosine of c, in degrees.

docker exec jupyter python3 -c "import numpy as np; c = (4 ** 2 + 1 ** 2 - 9 - 4) / 12; print(round(c, 4), np.round(np.degrees(np.arccos(c)), 2))"

import math

l1 = 3
l2 = 2
x = 4
y = 1

c2 = (x * x + y * y - l1 * l1 - l2 * l2) / (2 * l1 * l2)
a2 = math.degrees(math.acos(c2))
print("cos of the elbow angle:", round(c2, 4))
print("elbow angle:", round(a2, 2), "or", round(-a2, 2))
# Change x to 6 and press Run.
# The target is too far for the arm. What does Python say?

The inverse cosine gives a positive angle, 70.53. But a bend of minus 70.53 gives the same distance, so the arm bent the other way reaches the same target. These are elbow down and elbow up.

For each, the shoulder angle is a1 = atan2(y, x) minus atan2(l2 sin a2, l1 + l2 cos a2).

import math

def hand(l1, l2, a1, a2):
    t1 = math.radians(a1)
    t2 = math.radians(a1 + a2)
    return [l1 * math.cos(t1) + l2 * math.cos(t2), l1 * math.sin(t1) + l2 * math.sin(t2)]

# One solution for the target (4, 1) with links 3 and 2.
a2 = 70.5288
a1 = math.degrees(math.atan2(1, 4) - math.atan2(2 * math.sin(math.radians(a2)), 3 + 2 * math.cos(math.radians(a2))))
print("a1:", round(a1, 2))
print("hand:", [round(n, 3) for n in hand(3, 2, a1, a2)])
# Change a2 to -70.5288 and a1 to the matching angle. Does the hand still reach (4, 1)?

If the target is further than l1 plus l2, the arm cannot stretch to it. If it is nearer than the difference of the links, it cannot fold in that far.

Then c is above 1 or below minus 1, and no angle has that cosine. A good program checks the distance first and answers "no solution" instead of failing.

In start.ipynb, take the target (0, 4) and links 3 and 2. Work out both elbow angles and both shoulder angles by hand.

Then check each pair with the hand function from the last lesson. Do both land on (0, 4)?