Kiru Lab  /  Embodied Control: Kinematics and the Inverse Jacobian  /  Frames and Forward Kinematics

Forward Kinematics: The Easy Direction

Joint angles in, hand position out. One answer, always, computed by walking the chain.

Code Walkthrough  ·  about 35 minutes

Forward kinematics evaluates a function from joint space to task space. For a chain of `n` joints, the input is an `n`-vector of angles and the output is the pose of the end effector. There is exactly one answer, and computing it is a loop over the links.

f: R^n -> R^m joint angles -> end-effector pose Planar 2-link arm: x = L1 cos(q1) + L2 cos(q1 + q2) y = L1 sin(q1) + L2 sin(q1 + q2)
A planar chain of any length
import numpy as np

def forward_kinematics(q: np.ndarray, lengths: np.ndarray) -> np.ndarray:
    """End-effector (x, y) for a planar revolute chain."""
    angles = np.cumsum(q)                       # each joint angle is relative to the last link
    x = float(np.sum(lengths * np.cos(angles)))
    y = float(np.sum(lengths * np.sin(angles)))
    return np.array([x, y])

def link_positions(q, lengths):
    """Every joint position, for drawing the arm."""
    angles = np.cumsum(q)
    xs = np.concatenate([[0.0], np.cumsum(lengths * np.cos(angles))])
    ys = np.concatenate([[0.0], np.cumsum(lengths * np.sin(angles))])
    return xs, ys

Why the inverse is hard

  • It may have no solution: the target is outside the reachable workspace.
  • It may have many solutions: a two-link arm generally reaches a point with an elbow-up and an elbow-down configuration.
  • It may have infinitely many: a redundant arm with more joints than task dimensions has a continuum of answers.
  • It is nonlinear: trigonometric functions of sums do not rearrange into a clean closed form beyond simple chains.
The pattern to notice

An easy forward map and a hard inverse is one of the recurring shapes in this curriculum. Rendering an image is easy, inferring the scene is hard. Generating text from weights is easy, inferring which weights produced a behavior is hard. Mechanistic interpretability is an inverse problem, and the methods in the next track resemble the methods here for that reason.

Hold on to

  • Forward kinematics is a well-defined function with one answer
  • The inverse may have zero, several, or infinitely many solutions
  • Easy forward, hard inverse is a structural pattern across the whole field

Work through

Try each one before opening the solution. Getting it wrong first is most of where the learning happens.

  1. Implement and plot a three-link arm at several configurations.
    Hint

    Use `link_positions` to get every joint, then plot the polyline.

    Solution

    Plotting the whole chain rather than only the end effector is what makes kinematics debuggable — a wrong angle convention is obvious in a picture and invisible in a coordinate pair. Keep this plotting function; you will use it for every exercise in the rest of the track.

    import numpy as np, matplotlib.pyplot as plt

lengths = np.array([1.0, 0.8, 0.6])
configs = [np.array([0.0, 0.0, 0.0]),
           np.array([0.5, 0.5, 0.5]),
           np.array([1.2, -0.8, 0.9])]

for q in configs:
    xs, ys = link_positions(q, lengths)
    plt.plot(xs, ys, "o-", label=str(q.round(1)))
plt.gca().set_aspect("equal"); plt.legend(); plt.show()
  2. Sample 10,000 random joint vectors and scatter the reachable end-effector positions. Describe the workspace boundary.
    Hint

    Sample uniformly in joint space, not in task space, and scatter the results.

    Solution

    The workspace is an annulus: an outer boundary at the sum of the link lengths and — when no single link dominates — an inner hole the arm cannot reach into. The density is strikingly non-uniform even though the joint sampling was uniform, piling up near the boundary. That non-uniformity is the Jacobian's determinant varying across the workspace, which is exactly the manipulability measure the next module defines.

    import numpy as np, matplotlib.pyplot as plt

rng = np.random.default_rng(0)
lengths = np.array([1.0, 0.8, 0.6])
qs = rng.uniform(-np.pi, np.pi, size=(10_000, 3))
points = np.array([forward_kinematics(q, lengths) for q in qs])

plt.scatter(points[:, 0], points[:, 1], s=1, alpha=0.2)
plt.gca().set_aspect("equal"); plt.show()
print("max reach:", lengths.sum(), "observed:", np.linalg.norm(points, axis=1).max())
    Check your work

    Paste this after your own code. If it runs without raising, you have it. 

    import numpy as np
assert np.linalg.norm(points, axis=1).max() <= lengths.sum() + 1e-9
print("ok")

In the Bio Mirror

Motor Cortex and the Degrees-of-Freedom Problem Proprioception: The Sense That Makes Control Possible

Sign in to track your progress through the lab.

← Coordinate Frames and Homogeneous Transforms Back to track →