Level 2 · Robotics: Motion
Level 1 put a robot somewhere: a pose (x, y, \theta), a robot frame and a world frame, and the rule for composing a small move onto a pose. This level makes things move, in the two ways robots move. First an arm: a chain of rigid links joined by motors, bolted to a base. Given the joint angles, where is the hand? That question is forward kinematics, and for an arm that lies in a plane it is Level 1's pose composition done once per link. You will find every joint of a chain, the ring of points a two-link arm can reach, joints with mechanical stops, sliding joints, an arm riding on a mobile base, joints driven over time, and the checks that keep an arm off the floor, out of a wall and away from itself.
Second, a wheeled robot with two driven wheels, the design inside most robot vacuums and warehouse carts. Two wheel speeds decide how fast it goes and how fast it turns, and back again. Wheel encoders count ticks, ticks become distances, and adding up those small motions one after another, odometry, tells the robot where it is with no GPS at all. It also drifts, and you will measure how fast.
Simulating joints and robots step by step is the Euler method of the Control lesson (Level 1 of Signals, Systems & Control), with the same first-order error and the same saturation min(hi, max(lo, value)); here it meets an exact answer it can be checked against. Odometry is a running sum, the accumulator pattern of every path. And this level sets up the next two: Level 3 runs forward kinematics backwards (which angles put the hand there?), and Level 4 asks how fast the hand moves when the joints do.
A link is a vector
Look at a planar arm from above. The base (or shoulder) sits at the origin, with x to the right and y up. Each link is a rigid bar; each joint is a motor that turns the next link. One link of length l pointing in direction \varphi (radians, counter-clockwise from +x) ends at (l\cos\varphi,\ l\sin\varphi): Level 1's "drive d along heading \theta", with the link as the drive.
Joint angles are almost always relative. The elbow motor is bolted to the first link, so it measures the second link's angle from the first link's direction, not from +x. For a two-link arm with lengths l_1, l_2 and joint angles t_1, t_2:
- the first link points along t_1 and ends at the elbow, (l_1\cos t_1,\ l_1\sin t_1);
- the second link points along t_1 + t_2, so the hand is the elbow plus that vector:
A window-cleaning arm with links of 1.2 m and 0.8 m:
import math
l1, l2 = 1.2, 0.8 # link lengths in metres
t1, t2 = 0.6, 0.9 # joint angles in radians, relative
ex, ey = l1 * math.cos(t1), l1 * math.sin(t1) # the elbow: link 1 as a vector
a = t1 + t2 # link 2's direction, measured from +x
hx, hy = ex + l2 * math.cos(a), ey + l2 * math.sin(a)
print("elbow", round(ex, 4), round(ey, 4))
print("hand ", round(hx, 4), round(hy, 4))
wx, wy = ex + l2 * math.cos(t2), ey + l2 * math.sin(t2) # the classic slip: t2 read as absolute
print("t2 read as absolute:", round(wx, 4), round(wy, 4))
plot([0, ex, hx], [0, ey, hy], xlabel="x (m)", ylabel="y (m)")
elbow 0.9904 0.6776
hand 1.047 1.4756
t2 read as absolute: 1.4877 1.3042
Press Run to see the arm: two straight segments from the base. With t_2 = 0 the arm is stretched straight; with t_2 = \pi the second link folds back over the first. A quarter turn makes math.cos(math.pi / 2) come out as 6.1e-17, not 0: rounding dust, which is why answers are compared with a tolerance and never rounded early.
Common mistakes: using t2 alone as the second link's direction. Feeding degrees to math.cos, or converting angles that already are radians. Swapping cos and sin (x goes with the cosine).
Practice: Where the Gripper Lands
Walking the chain
An arm with n links works the same way, one link at a time, like a turtle: turn by the joint angle, then walk the link's length. Two running values carry everything: the position, a running sum of link vectors, and the absolute heading, a running sum of the relative angles. Recording the position after every link gives all the joints, which is what a drawing or a collision check needs.
import math
def arm_points(lengths, angles):
x, y, heading = 0.0, 0.0, 0.0
points = [(x, y)] # the base
for length, angle in zip(lengths, angles):
heading += angle # turn first...
x += length * math.cos(heading) # ...then walk the link
y += length * math.sin(heading)
points.append((x, y))
return points, heading
points, heading = arm_points([0.9, 0.7, 0.5, 0.3], [1.2, 1.0, 0.9, 1.1])
for px, py in points:
print(round(px, 3), round(py, 3))
print("last link's heading:", round(heading, 4))
plot([p[0] for p in points], [p[1] for p in points], xlabel="x", ylabel="y")
0.0 0.0
0.326 0.839
-0.086 1.405
-0.585 1.426
-0.732 1.164
last link's heading: 4.2
Four links give five points, the base included. The heading is 4.2 rad, more than half a turn, so it points the same way as 4.2 - 2\pi. A heading that is reported is wrapped into (-\pi, \pi] with Level 1's wrap. Its while loops work at any size; a variant that never loops uses math.fmod, which keeps the sign of its argument and lands strictly between -2\pi and 2\pi, so one correction finishes the job:
import math
def wrap(a): # into (-pi, pi]; -pi itself becomes +pi
a = math.fmod(a, 2 * math.pi)
if a <= -math.pi:
a += 2 * math.pi
elif a > math.pi:
a -= 2 * math.pi
return a
print(round(wrap(4.2), 4), round(wrap(-7.0), 4), wrap(math.pi), wrap(-math.pi))
-2.0832 -0.7168 3.141592653589793 3.141592653589793
Mind the ends, as in Level 1: the interval keeps \pi and leaves out -\pi, so a link pointing along -x has heading \pi, while the one-liner (a + pi) % (2 * pi) - pi and atan2(sin a, cos a) can both give -\pi. Wrapping in the middle of the walk is harmless (cosine and sine do not care) but unnecessary: wrap once, at the end.
Common mistakes: turning after walking, so the first link always lies along +x. Leaving the base out of the list of points. Returning the unwrapped sum.
Practice: Every Joint of the Desk Lamp
The workspace is a ring
Which points can a two-link arm reach? Take a pair of joint angles and measure the hand's distance from the base. The shoulder only swings the whole arm around; the distance depends on the elbow alone:
import math
l1, l2 = 0.5, 0.3
for t2 in (0.0, 1.0, 2.0, math.pi):
dists = []
for t1 in (0.0, 2.0, -1.0):
x = l1 * math.cos(t1) + l2 * math.cos(t1 + t2)
y = l1 * math.sin(t1) + l2 * math.sin(t1 + t2)
dists.append(round(math.hypot(x, y), 4))
print("elbow", round(t2, 4), "-> distance", dists)
xs, ys = [], []
for i in range(36):
for j in range(36):
t1, t2 = i * math.pi / 18, j * math.pi / 18
xs.append(l1 * math.cos(t1) + l2 * math.cos(t1 + t2))
ys.append(l1 * math.sin(t1) + l2 * math.sin(t1 + t2))
plot(xs, ys, kind="scatter", xlabel="x", ylabel="y")
elbow 0.0 -> distance [0.8, 0.8, 0.8]
elbow 1.0 -> distance [0.7086, 0.7086, 0.7086]
elbow 2.0 -> distance [0.4638, 0.4638, 0.4638]
elbow 3.1416 -> distance [0.2, 0.2, 0.2]
Stretched (t_2 = 0) the hand is l_1 + l_2 = 0.8 away, folded (t_2 = \pi) it is |l_1 - l_2| = 0.2 away, and bending the elbow passes through every distance in between. A shoulder that turns all the way round then sweeps each distance into a full circle. Press Run: the reachable points fill a ring, an annulus, around the base. The workspace is
both edges included: the outer circle is the stretched arm, the inner one the folded arm. When l_1 = l_2 the hole shrinks to the single point at the base, which the folded arm touches.
Edges are where floats bite. With l_1 = 0.7 and l_2 = 0.1, a target exactly 0.8 away is on the outer edge, but the computer disagrees:
l1, l2 = 0.7, 0.1
print(l1 + l2, 0.8 <= l1 + l2)
l1, l2 = 70, 10 # the same arm in millimetres, as whole numbers
inner2, outer2 = (l1 - l2) ** 2, (l1 + l2) ** 2
for x, y in [(48, 64), (0, -80), (36, 48), (30, 50), (-50, 63)]:
d2 = x * x + y * y # squared distance: no square root, no rounding
print((x, y), d2, inner2 <= d2 <= outer2)
0.7999999999999999 False
(48, 64) 6400 True
(0, -80) 6400 True
(36, 48) 3600 True
(30, 50) 3400 False
(-50, 63) 6469 False
With whole numbers, compare squared distances: x * x + y * y against (l1 + l2) ** 2 is exact, so a point on an edge stays on it. Above, (48, 64) and (0, -80) sit on the outer circle and (36, 48) on the inner one; (30, 50) falls in the hole and (-50, 63) lies just beyond the outer circle. The square root is increasing, so comparing squares gives the same answers as comparing distances.
Common mistakes: a strict < at an edge. Forgetting abs in the inner radius when l2 > l1. Thinking the base is out of reach when the links are equal.
Practice: Which Wells Can the Pipette Reach?
Many poses: stops and sweeps
Real joints have stops: cables and hoses run through them, so each joint may only turn between a lowest and a highest angle (lo, hi). A pose is allowed when every joint is inside its stops, edges included. The stops apply to the angle as the motor counts it, so do not wrap it: a joint at 3.5 rad has wound its cable further than one at 3.5 - 2\pi, although the link points the same way.
A planner often tries many poses that differ in one joint: a sweep. Copy the list of angles for each try, so the others keep their values. A helper hand(lengths, q) that returns the end of the chain keeps the loop short:
import math
def hand(lengths, q):
x = y = heading = 0.0
for length, angle in zip(lengths, q):
heading += angle
x += length * math.cos(heading)
y += length * math.sin(heading)
return x, y
lengths = [1.0, 0.6, 0.4]
base = [0.4, 0.3, -0.2]
stops = [(-1.0, 1.0), (-0.5, 2.0), (-1.5, 1.5)]
values = [-0.8, -0.4, 0.0, 0.4, 0.8, 1.2, 1.6, 2.0, 2.4] # tried for joint 1
kept = []
for i, v in enumerate(values):
q = list(base) # a fresh copy: base keeps its values
q[1] = v
if all(lo <= a <= hi for a, (lo, hi) in zip(q, stops)):
kept.append((i, hand(lengths, q)))
print("allowed:", [values[i] for i, p in kept])
pts = [p for i, p in kept]
path = sum(math.dist(pts[k - 1], pts[k]) for k in range(1, len(pts)))
print("the hand travels", round(path, 4), "m in straight steps between them")
plot([p[0] for p in pts], [p[1] for p in pts], kind="scatter")
allowed: [-0.4, 0.0, 0.4, 0.8, 1.2, 1.6, 2.0]
the hand travels 2.3726 m in straight steps between them
enumerate keeps each pose's original position; zip(q, stops) pairs each angle with its own stops; all(...) needs every joint to pass; 2.0 sits exactly on its stop and is kept. The length of a path through several points is the sum of the straight steps between neighbours, math.dist(p, q). It is shorter than the curved path the hand really follows between them.
Asking for the highest of several points has a trap of its own. Two heights that are equal on paper can differ in the last digit:
import math
heights = [math.sin(a) for a in (math.pi / 6, math.pi / 3, 2 * math.pi / 3)]
print(heights)
print("max picks index", max(range(3), key=lambda k: heights[k]))
M = max(heights)
print("first within 1e-9 of the top:", next(k for k, h in enumerate(heights) if h >= M - 1e-9))
[0.49999999999999994, 0.8660254037844386, 0.8660254037844387]
max picks index 2
first within 1e-9 of the top: 1
When a specification says "heights within 1e-9 count as equal, take the first", find the maximum M first and then scan from the start for the first value at least M - 1e-9.
Common mistakes: a strict < at a stop. Wrapping before comparing with stops. Changing base itself instead of a copy. Adding a step from the last point back to the first.
Practice: Inside the Joint Stops, One Joint Sweeps the Stage
Sliding joints
Not every joint turns. A revolute joint turns, and its value is an angle; a prismatic joint slides, and its value is a length, like a telescope pulled out. In the turtle picture a prismatic joint is a walk without a turn: it adds its extension along the current heading. A drawer-reaching arm with a turning shoulder, a tube of 0.6 m that slides out by s, and a turning wrist with a 0.2 m finger:
import math
shoulder, wrist = 0.8, -1.4 # radians
for s in (0.0, 0.25, 0.5): # how far the tube is pulled out
heading = shoulder
x = (0.6 + s) * math.cos(heading) # the fixed tube and the slide, both along the shoulder's direction
y = (0.6 + s) * math.sin(heading)
heading += wrist
x += 0.2 * math.cos(heading)
y += 0.2 * math.sin(heading)
print("s =", s, " hand", round(x, 4), round(y, 4), " heading", round(heading, 4))
s = 0.0 hand 0.5831 0.3175 heading -0.6
s = 0.25 hand 0.7573 0.4968 heading -0.6
s = 0.5 hand 0.9314 0.6762 heading -0.6
Each extra 0.25 m of slide moves the hand by the same vector, 0.25(\cos 0.8, \sin 0.8), and never changes the heading. A sliding joint at the start of a chain is a rail that carries the whole arm along the base's direction.
Common mistakes: sliding along +x instead of the current heading. Letting a sliding joint reset the heading.
Practice: The Telescoping Boom
An arm on a moving base
Put the arm on a mobile robot and it rides along. Its links are described in the robot's frame (x forward, y to the robot's left), with its base at a mount point (mx, my) on the deck. Two steps, both from Level 1: compute the hand in the robot frame as for any arm, starting at the mount; then carry that point into the world by rotating it by the robot's heading \theta and shifting it by the robot's position (X, Y):
Headings add: the hand's world heading is \theta plus its heading in the robot frame, wrapped.
import math
X, Y, theta = 4.0, 2.5, -0.7 # the robot's pose in the world
mx, my = 0.15, 0.05 # the arm's base on the deck (forward, left)
lengths, angles = [0.35, 0.25], [0.5, 1.1]
x, y, h = mx, my, 0.0 # 1. the arm in the robot's frame, from the mount
for length, angle in zip(lengths, angles):
h += angle
x += length * math.cos(h)
y += length * math.sin(h)
print("robot frame:", round(x, 4), round(y, 4), round(h, 4))
c, s = math.cos(theta), math.sin(theta) # 2. rotate by theta, then shift by (X, Y)
print("world: ", round(X + c * x - s * y, 4), round(Y + s * x + c * y, 4), round(theta + h, 4))
robot frame: 0.4499 0.4677 1.6
world: 4.6454 2.5679 0.9
The rotation is a matrix times a vector: \begin{pmatrix} \cos\theta & -\sin\theta \\ \sin\theta & \cos\theta \end{pmatrix}\begin{pmatrix} x \\ y \end{pmatrix}. Rotating by a and then by b is the product of two such matrices, and the product is the rotation by a + b. That is the deeper reason relative joint angles simply add:
import math
def rot(a):
return [[math.cos(a), -math.sin(a)], [math.sin(a), math.cos(a)]]
def matmul(A, B): # entry (i, j): row i of A times column j of B
return [[sum(A[i][k] * B[k][j] for k in range(len(B))) for j in range(len(B[0]))] for i in range(len(A))]
print([[round(v, 6) for v in row] for row in matmul(rot(0.5), rot(1.1))])
print([[round(v, 6) for v in row] for row in rot(1.6)])
[[-0.0292, -0.999574], [0.999574, -0.0292]]
[[-0.0292, -0.999574], [0.999574, -0.0292]]
Robot software chains such matrices, one per joint, to place every part of a large arm.
Common mistakes: adding the mount in world coordinates without rotating it. Shifting before rotating. The signs in the rotation (x * cos + y * sin turns the wrong way). Forgetting to add \theta to the heading.
Practice: The Arm Rides Along, Multiply Two Small Matrices
Joints that move over time
A motor turns its joint at some speed in radians per second. To see what the hand does, simulate: cut time into steps of dt, move every joint by speed * dt, clamp it at its stops with min(hi, max(lo, ...)), and recompute the hand. This is the Euler step of the Control lesson with a saturation, and since the speed is constant, the joint angles are exact; only the stops bend the rule.
import math
def hand(lengths, q):
x = y = heading = 0.0
for length, angle in zip(lengths, q):
heading += angle
x += length * math.cos(heading)
y += length * math.sin(heading)
return x, y
lengths = [0.8, 0.5]
q = [0.2, 0.0] # a copy of the start: the joints we will change
speeds = [0.6, -1.5] # rad/s
stops = [(-1.0, 1.0), (-1.2, 1.2)]
dt, steps = 0.2, 8
x, y = hand(lengths, q)
highest, xs, ys = y, [x], [y] # the start pose counts too
for _ in range(steps):
for i in range(len(q)):
lo, hi = stops[i]
q[i] = min(hi, max(lo, q[i] + speeds[i] * dt))
x, y = hand(lengths, q)
highest = max(highest, y)
xs.append(x)
ys.append(y)
print("joints", [round(a, 3) for a in q])
print("hand", round(x, 4), round(y, 4), " highest", round(highest, 4))
plot(xs, ys, xlabel="x (m)", ylabel="y (m)")
joints [1.0, -1.2]
hand 0.9223 0.5738 highest 0.5738
The elbow hits its stop at -1.2 after four steps and stays; the shoulder would reach 1.04 at step 7 and is held at 1.0. Press Run to see the hand's path bend where each joint stops.
Common mistakes: forgetting the starting pose in a running minimum or maximum (with zero steps the answer is the start). Letting a joint run through its stop. Changing the caller's start list in place.
Practice: Motors Until the Stops
Which side of a line?
An arm must stay above the floor and on its own side of a wall. For a line through points p and q, the 2D cross product
is positive when P lies to the left of the direction p \to q, negative on the right, and 0 on the line. Divided by |q - p| it is the signed distance from the line. You rarely know in advance which sign is safe, so take the sign of the base, which is on the safe side by definition:
import math
p, q = (2.0, -1.0), (0.0, 3.0) # a shelf edge: the whole line through p and q
dx, dy = q[0] - p[0], q[1] - p[1]
norm = math.hypot(dx, dy)
def side(P): # signed distance, positive left of p -> q
return (dx * (P[1] - p[1]) - dy * (P[0] - p[0])) / norm
points = [(0.0, 0.0), (0.6, 0.5), (1.0, 1.0), (1.5, 0.4), (1.3, -0.2)]
sign = 1 if side(points[0]) > 0 else -1 # the base's side is the safe side
for k, P in enumerate(points):
d = sign * side(P)
print(k, round(d, 4), "clear" if d >= 1e-9 else "touches or crosses", " below the floor" if P[1] < -1e-9 else "")
0 1.3416 clear
1 0.5814 clear
2 0.0 touches or crosses
3 -0.1789 touches or crosses
4 0.2683 clear below the floor
Point 2 lies exactly on the line, point 3 beyond it, and point 4 under the floor y = 0. Two facts make checking only the joints enough. A link is straight, and the allowed side of a line (a half-plane) is convex: if both ends of a segment are inside, so is everything between. And the tolerance 1e-9 is there because a joint that should sit exactly on the floor often comes out at 1e-16 or -1e-16, rounding dust from cos(pi / 2); decide once which side of the tolerance counts as touching, and say so.
Common mistakes: assuming the base is on the positive side. Treating the line as a segment between p and q. Testing y < 0 with no tolerance.
Practice: Mind the Floor, Mind the Glass
How close are two links?
Links are thick bars, so an arm that curls up can hit itself. Model each link as the segment between its two joints and ask how close two segments get.
From a point P to a segment AB: project P onto the line, t = \frac{(P - A)\cdot(B - A)}{|B - A|^2}, where t = 0 is A and t = 1 is B; clamp t into [0, 1] to stay on the segment; measure from P to A + t(B - A). Two segments that do not cross are closest at an endpoint of one of them, so their distance is the smallest of four point-to-segment distances. They cross when C and D lie strictly on opposite sides of line AB and A and B strictly on opposite sides of line CD (cross-product signs again); then the distance is 0.
import math
def cross(o, a, b):
return (a[0] - o[0]) * (b[1] - o[1]) - (a[1] - o[1]) * (b[0] - o[0])
def point_segment(P, A, B):
dx, dy = B[0] - A[0], B[1] - A[1]
t = ((P[0] - A[0]) * dx + (P[1] - A[1]) * dy) / (dx * dx + dy * dy)
t = max(0.0, min(1.0, t)) # stay on the segment
return math.hypot(P[0] - A[0] - t * dx, P[1] - A[1] - t * dy)
def segment_distance(A, B, C, D):
if cross(A, B, C) * cross(A, B, D) < 0 and cross(C, D, A) * cross(C, D, B) < 0:
return 0.0 # they cross
return min(point_segment(A, C, D), point_segment(B, C, D),
point_segment(C, A, B), point_segment(D, A, B))
A, B = (0.0, 0.0), (3.0, 0.0)
for P in [(1.0, 2.0), (4.0, 1.0), (-1.0, -1.0)]:
print("point", P, "->", round(point_segment(P, A, B), 4))
print("an X:", segment_distance(A, B, (1.0, 1.0), (2.0, -1.0)))
print("apart:", round(segment_distance(A, B, (1.0, 0.5), (2.0, 2.0)), 4))
point (1.0, 2.0) -> 2.0
point (4.0, 1.0) -> 1.4142
point (-1.0, -1.0) -> 1.4142
an X: 0.0
apart: 0.5
For (4, 1) the projection lands at t = 4/3, beyond B; unclamped, the answer would be 1, the distance to the infinite line, but the nearest point of the segment is B itself, \sqrt 2 away. A crossing has no endpoint near the other segment, which is why it needs its own test. In an arm, neighbouring links always touch at their shared joint, so only pairs i < j with j >= i + 2 are worth checking.
Common mistakes: forgetting to clamp t. Checking only endpoint-to-endpoint distances. Missing crossings. Checking neighbouring links.
Practice: An Arm That Must Not Touch Itself
Two wheels, one motion
A differential drive robot has two driven wheels on one axle, L metres apart, and a castor or two for balance. It cannot move sideways; all it can do comes from the two wheel speeds along the floor, v_l and v_r (negative when a wheel rolls backwards). Its reference point is the middle of the axle.
While both speeds stay fixed, the whole robot turns about one point on the axle's line, the instantaneous centre of curvature, at some rate \omega. A point at distance \rho from that centre moves at \omega\rho. If the centre is R to the robot's left, the left wheel is R - L/2 from it and the right wheel R + L/2:
Subtracting gives the turning rate, adding gives the forward speed v = \omega R of the midpoint:
\omega is positive counter-clockwise, to the left, so a faster right wheel turns the robot left. The signed radius R is positive when the centre is on the left and negative on the right. Two cases are special: equal speeds give \omega = 0, a straight line with no centre at all; equal and opposite speeds give v = 0 and R = 0, a spin on the spot.
L = 0.36 # metres between the wheels
for vl, vr in [(0.5, 0.5), (0.3, 0.6), (0.6, 0.3), (-0.2, 0.2), (0.45, -0.15)]:
v = (vr + vl) / 2
w = (vr - vl) / L
R = None if w == 0 else round(v / w, 4) # straight: there is no centre
print(f"left {vl:5} right {vr:5} -> v {v:.3f} m/s omega {w:7.3f} rad/s R {R}")
left 0.5 right 0.5 -> v 0.500 m/s omega 0.000 rad/s R None
left 0.3 right 0.6 -> v 0.450 m/s omega 0.833 rad/s R 0.54
left 0.6 right 0.3 -> v 0.450 m/s omega -0.833 rad/s R -0.54
left -0.2 right 0.2 -> v 0.000 m/s omega 1.111 rad/s R 0.0
left 0.45 right -0.15 -> v 0.150 m/s omega -1.667 rad/s R -0.09
Swapping the wheels flips the turn: (0.3, 0.6) and (0.6, 0.3) have the same speed and opposite radii. The last line has |R| < L/2: the centre lies between the wheels, so the inner wheel must roll backwards.
Common mistakes: vl - vr in the difference, which makes left turns negative. Dividing by zero on a straight run. Returning abs(R), which loses the side. Mixing up the special cases: equal speeds have no centre, opposite speeds have it at the robot.
Practice: Two Wheels, One Motion
From a motion to wheel commands
A planner thinks in v and \omega; the motors want wheel speeds. Run the formulas backwards: each wheel is L/2 from the midpoint, so it rolls \omega L/2 faster or slower than v,
A wheel of radius r spinning at \varphi rad/s rolls r\varphi m/s along the floor, so a motor's spin rate is the floor speed divided by r. And to drive a circle of signed radius R at speed v, turn at \omega = v / R; one lap is 2\pi|R| metres long.
import math
L, r = 0.28, 0.035 # wheel separation and wheel radius, metres
speed = 0.4 # m/s for the midpoint
for R in (2.0, 0.5, 0.1, -0.5):
w = speed / R
vl, vr = speed - w * L / 2, speed + w * L / 2
print(f"R {R:5}: wheels {vl:6.3f} {vr:6.3f} m/s = {vl / r:6.2f} {vr / r:6.2f} rad/s, lap {2 * math.pi * abs(R) / speed:.2f} s")
R 2.0: wheels 0.372 0.428 m/s = 10.63 12.23 rad/s, lap 31.42 s
R 0.5: wheels 0.288 0.512 m/s = 8.23 14.63 rad/s, lap 7.85 s
R 0.1: wheels -0.160 0.960 m/s = -4.57 27.43 rad/s, lap 1.57 s
R -0.5: wheels 0.512 0.288 m/s = 14.63 8.23 rad/s, lap 7.85 s
On the circle of radius 0.1 m, tighter than half the wheel separation (0.14 m), the inner left wheel rolls backwards. A negative radius swaps which wheel is outside.
Common mistakes: omega * L instead of omega * L / 2. Putting the faster wheel on the inside of the turn. Multiplying by r instead of dividing. A negative lap time from forgetting abs(R).
Practice: Telling the Motors What to Do, Laps Around the Fountain
Same curve, slower
Motors have a top speed. If a command asks a wheel for more, the tempting fix, cutting that wheel down to the limit, changes the ratio of the two wheels and so the radius: the robot swings off its planned curve. Multiplying both v and \omega by one factor k multiplies both wheel speeds by k and keeps R = v/\omega. Choose k to bring the fastest wheel, by absolute value, exactly to the limit:
v, w = 0.9, 2.5 # the planner's command
L, limit = 0.3, 0.8 # metres; the top floor speed in m/s (top spin rate times r)
vl, vr = v - w * L / 2, v + w * L / 2
print("asked: ", round(vl, 4), round(vr, 4), " radius", round(v / w, 4))
cl, cr = max(-limit, min(limit, vl)), max(-limit, min(limit, vr)) # clip each wheel
print("clipped:", cl, cr, " radius", round(((cr + cl) / 2) / ((cr - cl) / L), 4))
k = limit / max(abs(vl), abs(vr)) # scale the command
print("scaled: ", round(k * vl, 4), round(k * vr, 4), " radius", round((k * v) / (k * w), 4))
asked: 0.525 1.275 radius 0.36
clipped: 0.525 0.8 radius 0.7227
scaled: 0.3294 0.8 radius 0.36
Clipping doubles the radius; scaling keeps 0.36 m and only takes longer. A wheel rolling backwards too fast counts as well, hence abs. A command already within the limit is left alone, never scaled up.
Common mistakes: comparing a spin rate in rad/s with a floor speed in m/s. Ignoring the backwards wheel. Scaling up a slow command.
Practice: Slow Down, Same Curve
Encoders: from ticks to distance
A wheel encoder is a slotted disc on the axle that sends a tick each time the wheel turns a fixed small angle: N ticks per revolution, counting down when the wheel turns backwards. One revolution rolls a wheel of radius r by its circumference 2\pi r, so one tick is 2\pi r / N metres. The differential-drive formulas hold for distances as well as speeds (a speed times a time is a distance): with wheel distances d_l and d_r,
is how far the midpoint travelled along its path and how much the heading changed.
import math
N, r, L = 2048, 0.045, 0.33 # ticks per revolution, wheel radius, wheel separation
per_tick = 2 * math.pi * r / N
left, right = 1500, 1720 # ticks counted since the last report
dl, dr = left * per_tick, right * per_tick
d, dtheta = (dr + dl) / 2, (dr - dl) / L
print("one tick:", round(per_tick * 1000, 4), "mm")
print("wheels", round(dl, 4), round(dr, 4), "m; travel", round(d, 4), "m; turn", round(dtheta, 4), "rad")
one tick: 0.1381 mm
wheels 0.2071 0.2375 m; travel 0.2223 m; turn 0.092 rad
\Delta\theta is a change, not a heading, so it is not wrapped: a long report while spinning can hold more than a full turn, and wrapping it would lose turns.
Going the other way, turning on the spot by an angle \Delta rolls each wheel along a circle of radius L/2 around the midpoint: the right wheel \Delta L/2 metres and the left the same backwards. To face a new heading, take the shorter way round, Level 1's wrap(target - current), then convert the arc to ticks, rounding only at the end:
import math
def wrap(a):
a = math.fmod(a, 2 * math.pi)
if a <= -math.pi:
a += 2 * math.pi
elif a > math.pi:
a -= 2 * math.pi
return a
N, r, L = 2048, 0.045, 0.33
current, target = 2.8, -2.9
turn = wrap(target - current) # -5.7 the long way, 0.5832 the short way
arc = turn * L / 2 # the right wheel's distance; the left's is -arc
ticks = round(arc / (2 * math.pi * r) * N) # revolutions times ticks per revolution
print(round(turn, 4), "rad:", round(arc, 4), "m per wheel:", -ticks, ticks, "ticks")
0.5832 rad: 0.0962 m per wheel: -697 697 ticks
Common mistakes: the diameter instead of the radius. // instead of /, which drops part revolutions. Wrapping \Delta\theta. L instead of L / 2 as the radius of the wheels' circle on the spot. Rounding before converting.
Practice: Counting Clicks on the Wheels, The Vacuum Turns to Face Its Dock
The unicycle and the exact arc
For working out where a robot goes, a differential drive is a unicycle: it moves in the direction it faces, never sideways, at speed v, while turning at rate \omega:
(a dot means "rate of change per second"). With v and \omega held for t seconds there is an exact answer. If \omega = 0 the robot drives straight, vt along its heading. Otherwise it runs around the centre of curvature, which is R = v/\omega along the robot's left direction (-\sin\theta, \cos\theta) (Level 1's robot frame), and turns by \Delta = \omega t. Rotating the robot around that centre by \Delta gives
import math
x, y, theta = 1.0, 0.5, 0.3
v, w, t = 0.6, -0.4, 3.0
R = v / w # -1.5: the centre is 1.5 m to the right
cx, cy = x - R * math.sin(theta), y + R * math.cos(theta)
new = theta + w * t
x2 = x + R * (math.sin(new) - math.sin(theta))
y2 = y - R * (math.cos(new) - math.cos(theta))
print("centre", round(cx, 4), round(cy, 4))
print("end ", round(x2, 4), round(y2, 4), round(new, 4))
print("distance from the centre, before and after:", round(math.dist((x, y), (cx, cy)), 4), round(math.dist((x2, y2), (cx, cy)), 4))
centre 1.4433 -0.933
end 2.6183 -0.0006 -0.9
distance from the centre, before and after: 1.5 1.5
The robot stays 1.5 m from the centre, as it must on a circle. When v = 0 the radius is 0 and only the heading changes: the spin on the spot needs no special case. A sequence of commands is the same update applied again and again, each from where the last one ended. Keep the heading as a running total and wrap it once, when you report it. To draw the route, cut each command into short pieces; the exact update gives the same end however you cut it:
import math
def drive(pose, v, w, t):
x, y, th = pose
if w == 0:
return (x + v * t * math.cos(th), y + v * t * math.sin(th), th)
R = v / w
return (x + R * (math.sin(th + w * t) - math.sin(th)), y - R * (math.cos(th + w * t) - math.cos(th)), th + w * t)
pose, xs, ys = (0.0, 0.0, 0.0), [0.0], [0.0]
for v, w, t in [(0.5, 0.0, 4.0), (0.5, 0.8, 3.0), (0.0, -1.0, 1.0), (0.4, -0.5, 5.0)]:
for _ in range(10): # ten pieces, only for the drawing
pose = drive(pose, v, w, t / 10)
xs.append(pose[0])
ys.append(pose[1])
print("end:", round(pose[0], 4), round(pose[1], 4), "heading", round(pose[2], 4))
plot(xs, ys, xlabel="x (m)", ylabel="y (m)")
end: 3.9235 1.3128 heading -1.1
The heading here ends at -1.1, already inside (-\pi, \pi]; on a long route it would not be.
Common mistakes: moving along the old heading for the whole command (that cuts the corner; see the next section). Using the heading half-way through the turn, \theta + \Delta/2: close, but not exact. Losing the sign of R for right turns or reversing. Dividing by \omega when it is 0.
Practice: Where the Tour Robot Stops
Euler steps cut corners
A general simulator does not know about circles. It cuts the time into n steps of \Delta t = T/n and takes Euler steps, exactly as for the tea in the Control lesson: move the position as if the heading stayed fixed for the whole step, then update the heading. The order matters, and is part of the method:
import math
v, w, T = 0.8, 0.5, 4.0
R = v / w
ex, ey = R * math.sin(w * T), R * (1 - math.cos(w * T)) # the exact end from (0, 0, 0)
for n in (1, 2, 4, 8, 16, 32):
dt = T / n
x = y = th = 0.0
for _ in range(n):
x += v * dt * math.cos(th) # position with the heading at the start of the step
y += v * dt * math.sin(th)
th += w * dt # then the heading
print(n, "steps: off by", round(math.hypot(x - ex, y - ey), 4), "m")
1 steps: off by 2.86 m
2 steps: off by 1.3656 m
4 steps: off by 0.6755 m
8 steps: off by 0.3369 m
16 steps: off by 0.1683 m
32 steps: off by 0.0842 m
Each step drives a straight chord along the tangent, so the polygon drifts to the outside of the circle. Doubling the number of steps roughly halves the error: Euler's method is first order. That is why odometry uses the exact arc whenever it can, and why a simulator that must use steps keeps them small.
Common mistakes: updating the heading before the position (a different method, with different answers). Forgetting to reset the pose for each run. Taking n + 1 steps.
Practice: Cutting Corners With Euler
Odometry: dead reckoning from the wheels
Odometry puts the last three sections together. Every few milliseconds the encoders report the ticks counted since the previous report. Each report is a small motion in the robot's own terms, so far forward and so much turn; added onto the pose in world coordinates, the reports trace the robot's path, the old sailors' dead reckoning. If the wheels turned at steady speeds during a report, the robot really ran along an arc of length d turning by \Delta\theta, a circle of radius R = d / \Delta\theta, and the exact arc update applies. For each report, in order:
- ticks to wheel distances, then d and \Delta\theta;
- if \Delta\theta = 0 (equal counts give exactly 0) move d straight along \theta; otherwise apply the arc update with R = d/\Delta\theta and the old \theta;
- \theta \leftarrow \theta + \Delta\theta.
import math
N, r, L = 360, 0.05, 0.4
per_tick = 2 * math.pi * r / N
readings = [(120, 120)] * 6 + [(60, 180)] * 5 + [(150, 150)] * 4 + [(-90, 90)] * 2
def step(pose, lt, rt, mode):
x, y, th = pose
dl, dr = lt * per_tick, rt * per_tick
d, dth = (dl + dr) / 2, (dr - dl) / L
if mode == "euler": # the whole report along the old heading
return (x + d * math.cos(th), y + d * math.sin(th), th + dth)
if dth == 0:
return (x + d * math.cos(th), y + d * math.sin(th), th)
R = d / dth
return (x + R * (math.sin(th + dth) - math.sin(th)), y - R * (math.cos(th + dth) - math.cos(th)), th + dth)
exact, euler, xs, ys = (0.0, 0.0, 0.0), (0.0, 0.0, 0.0), [0.0], [0.0]
for lt, rt in readings:
exact = step(exact, lt, rt, "arc")
euler = step(euler, lt, rt, "euler")
xs.append(exact[0])
ys.append(exact[1])
print("arc: ", [round(c, 4) for c in exact])
print("euler:", [round(c, 4) for c in euler], " off by", round(math.dist(exact[:2], euler[:2]), 4), "m")
plot(xs, ys, xlabel="x (m)", ylabel="y (m)")
arc: [1.1502, 0.8022, 2.0944]
euler: [1.1868, 0.75, 2.0944] off by 0.0638 m
The final heading, 2.0944 rad (120°), needs no wrapping; on a longer drive the running total would, so wrap it once, when you report it. Real reports are tiny, so the Euler shortcut is often close, but it is an approximation, and here it is already 6 cm off after 17 reports: the error adds up report after report.
Common mistakes: updating the heading before the position. Wrapping the heading after every report and then subtracting headings (wrap once, at the end, or wrap differences). Dividing by \Delta\theta when it is 0.
Practice: Dead Reckoning in the Orchard
Drift: small errors, growing gaps
Odometry has no way to notice its own mistakes. If the robot's idea of its wheel radius or wheel separation is slightly off, every report is converted slightly wrong, and the errors pile up. They do so in two different ways. A wrong radius scales every distance, and every turn with it. A wrong separation scales only the turns; on a straight run it does nothing. But a heading error, once made, swings every later segment in the wrong direction, so position errors grow with the distance driven after it.
Drive a square, three revolutions per side and a quarter turn on the spot at each corner, three times round. The true robot comes back to the start each lap; two miscalibrated copies of it do not:
import math
def track(readings, N, r, L):
x = y = th = 0.0
per_tick = 2 * math.pi * r / N
for lt, rt in readings:
dl, dr = lt * per_tick, rt * per_tick
d, dth = (dl + dr) / 2, (dr - dl) / L
if dth == 0:
x, y = x + d * math.cos(th), y + d * math.sin(th)
else:
R = d / dth
x, y = x + R * (math.sin(th + dth) - math.sin(th)), y - R * (math.cos(th + dth) - math.cos(th))
th += dth
return x, y, th
def wrap(a):
a = math.fmod(a, 2 * math.pi)
return a + 2 * math.pi if a <= -math.pi else a - 2 * math.pi if a > math.pi else a
lap = ([(360, 360)] * 3 + [(-360, 360)]) * 4 # with L = 0.4: four sides, four quarter turns
for laps in (1, 2, 3):
true = track(lap * laps, 360, 0.05, 0.4)
for name, r, L in [("separation +1 cm", 0.05, 0.41), ("radius +1%", 0.0505, 0.4)]:
est = track(lap * laps, 360, r, L)
gap = math.dist(true[:2], est[:2])
print(laps, "laps,", name, ": gap", round(gap, 4), "m, heading error", round(wrap(est[2] - true[2]), 4))
1 laps, separation +1 cm : gap 0.104 m, heading error -0.1532
1 laps, radius +1% : gap 0.042 m, heading error 0.0628
2 laps, separation +1 cm : gap 0.2075 m, heading error -0.3065
2 laps, radius +1% : gap 0.0839 m, heading error 0.1257
3 laps, separation +1 cm : gap 0.3097 m, heading error -0.4597
3 laps, radius +1% : gap 0.1257 m, heading error 0.1885
A one-centimetre error in the separation, under 3%, makes each corner about 2.2° short; after one lap the robot believes it is 10 cm from where it really is, and the gap grows with every lap. The radius error stretches every side and over-turns every corner by 1%, a smaller gap here but just as relentless. Compare headings by wrapping their difference; wrapping each heading first can turn a tiny difference into one near 2\pi. This is why a robot that relies on odometry needs something else, a landmark, a map, a beacon, to correct it from time to time: the subject of the later levels.
To report the first report after which a gap exceeds a tolerance, start with first = None and set it only while it is still None; then index 0 is a real answer, and a run that never drifts that far returns None.
Common mistakes: comparing headings instead of positions when the question is a distance. Wrapping each heading separately. Testing if not first for an index that can be 0.
Practice: A Millimetre Off, a Metre Lost
Practice
These problems are required on this path at this level; the path adds one problem from each of the level's topics, picked for you and kept.
- Where the Gripper Lands
- Every Joint of the Desk Lamp
- Which Wells Can the Pipette Reach?
- Two Wheels, One Motion
- Counting Clicks on the Wheels
- Dead Reckoning in the Orchard
Checklist
You can now:
- Compute the hand of a planar arm from relative joint angles, and every joint along the way.
- Wrap a heading into (-\pi, \pi] and know which shortcuts get the end -\pi wrong.
- Describe a two-link arm's workspace as a ring and classify a target exactly, edges included.
- Check poses against joint stops, sweep one joint, and pick a maximum with a stated tolerance.
- Handle sliding joints, and carry an arm's hand from the robot's frame into the world.
- Simulate joints moving over time with saturation at their stops.
- Decide which side of a line a point is on, and how far a point is from a segment.
- Turn two wheel speeds into v, \omega and a signed turning radius, and a planned motion back into wheel speeds and spin rates.
- Convert encoder ticks into wheel distances, a travel and a heading change, and plan a turn on the spot in ticks.
- Move a pose along an exact arc, run odometry over a list of reports, and explain why Euler steps and miscalibration make it drift.