Kiru Lab / Embodied Control: Kinematics and the Inverse Jacobian / Frames and Forward Kinematics
Coordinate Frames and Homogeneous Transforms
Every rigid body carries a frame. A 4x4 matrix moves you between frames, and the algebra is matrix multiplication.
Concept · about 30 minutes
Attach a coordinate frame to every rigid part of a robot. The relationship between any two frames is a rotation plus a translation, and packing both into a single 4x4 homogeneous transform lets you compose relationships by matrix multiplication — the same composition property from track two, now describing physical space.
import numpy as np
def rot_z(theta):
c, s = np.cos(theta), np.sin(theta)
return np.array([[c, -s, 0.0], [s, c, 0.0], [0.0, 0.0, 1.0]])
def transform_2d(theta, length):
"""Rotate by theta about the joint, then translate `length` along the new x axis."""
c, s = np.cos(theta), np.sin(theta)
return np.array([
[c, -s, length * c],
[s, c, length * s],
[0.0, 0.0, 1.0],
])Rotation and translation do not commute. Rotating then translating puts you somewhere different from translating then rotating. Nearly every early kinematics bug is a composition-order bug, and the fix is always to draw the frames rather than to stare at the code.
Hold on to
- A homogeneous transform packs rotation and translation into one matrix
- Chains compose by matrix multiplication
- Rigid transforms do not commute
Work through
Try each one before opening the solution. Getting it wrong first is most of where the learning happens.
-
Compose two transforms in both orders and show the results differ. Draw both.
Hint
Take a rotation of 90 degrees and a translation of 1 along x, and apply them in both orders.
Solution
Rotate-then-translate moves along the *rotated* axis; translate-then-rotate moves along the original axis and then swings the whole thing about the origin. The two end points differ. Drawing it is worth more than the algebra: the matrices commute only when the rotation is identity or the translation is zero.
import numpy as np def rot(theta): c, s = np.cos(theta), np.sin(theta) return np.array([[c, -s, 0.0], [s, c, 0.0], [0, 0, 1.0]]) def trans(dx, dy): return np.array([[1.0, 0, dx], [0, 1.0, dy], [0, 0, 1.0]]) R, T = rot(np.pi / 2), trans(1.0, 0.0) origin = np.array([0.0, 0.0, 1.0]) print((R @ T) @ origin) # [0, 1, 1] — translate along x, then rotate print((T @ R) @ origin) # [1, 0, 1] — rotate, then translate along world x assert not np.allclose(R @ T, T @ R)Check your work
Paste this after your own code. If it runs without raising, you have it.
import numpy as np assert not np.allclose(rot(np.pi/2) @ trans(1, 0), trans(1, 0) @ rot(np.pi/2)) print("ok") -
Verify that the inverse of a rigid transform is [R^T, -R^T p] rather than a general matrix inverse.
Hint
A rotation matrix is orthogonal, so its inverse is its transpose.
Solution
Because R is orthogonal, R inverse equals R transpose — no general matrix inversion needed. The translation part becomes -R^T p, since you must undo the rotation before undoing the translation. This is not merely faster; it is exactly orthogonal, whereas a numerical inverse accumulates error that compounds along a long kinematic chain.
import numpy as np def rigid_inverse(T): R, p = T[:2, :2], T[:2, 2] out = np.eye(3) out[:2, :2] = R.T out[:2, 2] = -R.T @ p return out T = rot(0.7) @ trans(2.0, -1.0) assert np.allclose(rigid_inverse(T), np.linalg.inv(T)) assert np.allclose(rigid_inverse(T) @ T, np.eye(3))Check your work
Paste this after your own code. If it runs without raising, you have it.
import numpy as np T = rot(0.7) @ trans(2.0, -1.0) assert np.allclose(rigid_inverse(T) @ T, np.eye(3), atol=1e-12) print("ok")
Sign in to track your progress through the lab.