M5.3 — Forward and Inverse Kinematics

Learning objectives

After this lesson, you should be able to:

• Compute forward kinematics for a planar arm.
• Solve inverse kinematics analytically for a two-link arm and check it with forward kinematics.
• Explain why there are two solutions or none.
• Define the Jacobian and use it for iterative IK.
• Explain singularities and why joint motions blow up near them.
• Describe how a controller chooses among IK solutions.

The one-sentence idea

Forward kinematics adds up the links to find the hand, and inverse kinematics searches for joint angles that make that sum equal a target, with the Jacobian guiding the search.

Forward is a formula. Inverse is a search.

Two questions about every arm

Forward kinematics (FK) asks: given the joint angles, where is the hand? Inverse kinematics (IK) asks the reverse: given where I want the hand, which joint angles do I need? Kinematics is about geometry only, positions and angles, not forces or time. M5.2 showed that the first question has a single answer and the second can have none, one, or many. This lesson computes both.

You will rarely write these from scratch for a real robot, because robot software provides solvers built from the robot’s description. But a policy that outputs hand poses relies on them, and knowing what they do, and where they fail, explains many robot behaviours.

Forward kinematics: add up the links

For a planar arm, forward kinematics walks along the chain. Each link points in the direction given by the sum of all joint angles up to it, and the hand position is the sum of the link vectors. The code below works for any number of links:

import numpy as np

def fk(q, lengths):
    angles = np.cumsum(q)                                   # absolute direction of each link
    return np.array([np.sum(lengths * np.cos(angles)),
                     np.sum(lengths * np.sin(angles))])

print(fk(np.array([0.0, np.pi / 2]), np.array([1.0, 1.0])).round(3))
print(fk(np.array([np.pi / 4, np.pi / 4, -np.pi / 2]), np.array([1.0, 1.0, 0.5])).round(3))

[1. 1.]
[1.207 1.707]

The first call is the two-link configuration from M5.2. The second has three links: the first two point at 45° and 90°, the third swings back to 0°, and the hand ends at (1.207, 1.707). Real 3-D arms do the same walk along the chain, but with 3-D rotations and translations in place of the angles, which is why M5.4 introduces coordinate frames and transformations next.

Inverse kinematics: solving for the angles

For the two-link arm the inverse problem can be solved exactly with the law of cosines. With the hand at (x, y) at distance r from the base, the elbow angle satisfies:

Taking the positive or negative inverse cosine gives the two elbow configurations. The shoulder angle then follows from the direction to the target, corrected for the elbow:

def ik(target, l1=1.0, l2=1.0):
    x, y = target
    c2 = (x * x + y * y - l1 * l1 - l2 * l2) / (2 * l1 * l2)
    if abs(c2) > 1:
        return []                                             # out of reach: no solution
    solutions = []
    for q2 in (np.arccos(c2), -np.arccos(c2)):                # the two elbow choices
        q1 = np.arctan2(y, x) - np.arctan2(l2 * np.sin(q2), l1 + l2 * np.cos(q2))
        solutions.append(np.array([q1, q2]))
    return solutions

for q in ik((1.0, 1.0)):
    print(q.round(3), fk(q, np.array([1.0, 1.0])).round(3))     # check: forward kinematics lands on the target
print(ik((2.5, 0.0)))

[0.    1.571] [1. 1.]
[ 1.571 -1.571] [1. 1.]
[]

The two solutions are exactly the “elbow” configurations of M5.2, (0, π/2) and (π/2, −π/2), and running each through forward kinematics returns the target, which is how an IK solver is checked. A target outside the workspace gives an empty list. Given several solutions, a controller typically picks the one that is nearest the arm’s current configuration (smallest joint motion), that respects the joint limits, and that avoids collisions:

current = np.array([0.2, 1.2])
options = ik((1.0, 1.0))
best = min(options, key=lambda q: np.linalg.norm(q - current))
print(best.round(3))

[0.    1.571]

The Jacobian: how small joint motions move the hand

Closed-form IK exists only for simple arms. The general tool is the Jacobian J(q), a matrix that says how a small change in the joint angles changes the hand position: Δx ≈ J(q) Δq. Each column is the hand’s motion per unit change of one joint. For the planar two-link arm it is:

l = np.array([1.0, 1.0])

def jacobian(q):
    s1, c1 = np.sin(q[0]), np.cos(q[0])
    s12, c12 = np.sin(q[0] + q[1]), np.cos(q[0] + q[1])
    return np.array([[-l[0] * s1 - l[1] * s12, -l[1] * s12],
                     [ l[0] * c1 + l[1] * c12,  l[1] * c12]])

q = np.array([0.3, 0.8])
eps = 1e-6
numeric = np.column_stack([(fk(q + eps * e, l) - fk(q, l)) / eps for e in np.eye(2)])   # nudge each joint
print(np.allclose(jacobian(q), numeric, atol=1e-4))
print(jacobian(q).round(3))

True
[[-1.187 -0.891]
 [ 1.409  0.454]]

The first column is how the hand moves per radian of the shoulder joint, the second per radian of the elbow. Because Δx ≈ J Δq, you can solve for the joint change that produces a desired hand motion, Δq ≈ J⁻¹ Δx (or a pseudo-inverse when J is not square). Repeating that in small steps is iterative inverse kinematics, and it works for any arm for which you can compute J:

target = np.array([1.2, 0.8])
q = np.array([0.3, 0.8])                       # initial guess
for step in range(20):
    error = target - fk(q, l)
    q = q + np.linalg.pinv(jacobian(q)) @ error      # move the joints to reduce the error
print(q.round(3))
print(np.linalg.norm(target - fk(q, l)) < 1e-6)

[-0.177  1.531]
True

Starting from a guess, each step uses the current Jacobian to correct the remaining error, and in 20 steps the hand reaches the target to within a micro-unit. This run settled on a solution with a positive elbow angle, the same family as the first configuration in M5.2. A different starting guess can converge to the other solution of the same target, which is the many-answers situation again.

Singularities

Some configurations make the Jacobian lose a direction: the hand cannot move in some direction however the joints move. Such a configuration is a singularity. For the two-link arm it is when the arm is fully stretched or fully folded, where the determinant of J is l₁ l₂ sin(q₂) and is zero. Near a singularity, a small hand motion needs very large joint motions, so velocity commands become dangerous:

for q2 in (1.0, 0.1, 0.01):
    J = jacobian(np.array([0.3, q2]))
    dq = np.linalg.solve(J, np.array([0.0, 0.1]))      # joints needed to move the hand 0.1 upward
    print(q2, round(abs(np.linalg.det(J)), 4), round(np.abs(dq).max(), 1))

1.0 0.8415 0.1
0.1 0.0998 0.7
0.01 0.01 6.0

As the elbow straightens, the determinant shrinks toward zero and the joint motion needed for the same 0.1 upward move grows from 0.1 rad to 6.0 rad. A planner or learned policy that ignores this can ask for impossible joint speeds. Solvers handle it with damping and limits, and policies that output Cartesian commands depend on those solvers behaving well.

Common misconceptions

Engineering takeaways

1. Forward kinematics maps joint angles to a hand pose and always has one answer.
2. Inverse kinematics maps a pose to joint angles and can have zero, one or many.
3. Always verify an IK solution by running forward kinematics.
4. The law of cosines solves the two-link case in closed form.
5. The Jacobian relates small joint changes to hand motion: Δx ≈ J Δq.
6. Iterative IK repeats q ← q + J⁺ error until the error is small.
7. Near a singularity the Jacobian loses a direction and joint motions explode.
8. Practical solvers pick among solutions by closeness, limits and collisions.

Kinematics is the geometry that lets “move the hand there” become “turn the joints by these amounts”.

Knowledge check

Try each question first, then reveal the explanation.

What’s next

You can now convert between joints and the hand. Next is how positions and orientations are described relative to different coordinate frames.

Back to the VLA roadmap prerequisites