Level 1 · Robotics: Poses and Frames

Before a robot can do anything useful (mow a lawn, fetch a pallet, plug itself into a charger) it has to answer two questions: where am I, and which way am I facing? On a flat floor the answer is three numbers, the robot's pose (x, y, \theta): a position and a heading. Every pose is measured in some frame, a choice of origin and axes: the map's, the robot's own, a camera's. A surprising share of robot software is nothing more than moving numbers from one frame into another without dropping a sign.

This level has two topics. Frames: angles in radians, keeping a heading in one range, the shortest turn between two headings, compass headings against the maths convention, and moving a point between the robot's frame (x ahead, y to its left) and the world's. Poses: following a script of turns and drives, finding the range and bearing to a target, planning a turn-drive-turn move, and composing poses: "the pallet is 2 m ahead of me" plus "I am here, facing there" gives "the pallet is there".

The other paths meet this one early. Turning a point is multiplying by a unit complex number on the Quantum Computing path; lengths, unit arrows and the dot product are the vectors of Machine Learning; adding up a gyroscope's readings is the Euler step of Signals & Control. A pose is a tuple, as in Python Fluency. Everything here is plain Python with the math module.

A pose in a frame

The world frame of this path: x points to the right, y points up, distances are in metres. A heading \theta is an angle in radians, measured counter-clockwise from the +x axis: 0 faces right, \pi/2 faces up, \pi faces left and -\pi/2 faces down. A pose is the tuple (x, y, theta).

The robot carries its own frame, the robot frame: the origin at the robot's centre, x pointing straight forward, y pointing to the robot's left. In world coordinates the robot's two axes are unit arrows:

The left arrow is the forward arrow turned a quarter turn counter-clockwise. Picture a robot at (2, 1) facing up the page: its forward is world +y and its left is world −x.

import math
x, y, theta = (2.0, 1.0, math.pi / 2)        # at (2, 1), facing up
forward = (math.cos(theta), math.sin(theta))
left = (-math.sin(theta), math.cos(theta))
print("forward", [round(v, 3) for v in forward])
print("left   ", [round(v, 3) for v in left])
plot([x, x + forward[0]], [y, y + forward[1]], label="forward")
plot([x, x + left[0]], [y, y + left[1]], label="left", title="The robot's axes in the world")

forward [0.0, 1.0]
left    [-1.0, 0.0]

math.cos(math.pi / 2) is not exactly 0 but 6.1e-17: math.pi is only the closest float to \pi. Round before you print, and compare floats with a tolerance, never with ==.

Common mistakes: measuring the heading clockwise or from north (that is the compass, a later section). Putting y downwards, as screen coordinates do. Taking "left" as (\sin\theta, -\cos\theta), which is the robot's right.

Radians, degrees and arcs

A full turn is 2\pi radians, 360 degrees. math.radians and math.degrees convert; math.sin, math.cos and math.atan2 work in radians only. The radian is defined so that an arc is easy: a circle of radius r turned through \theta radians lays down an arc of length

A cable drum on a winch shows it: each turn of the drum winds one circumference of cable, and a turn backwards pays cable out.

import math
r = 0.04                                  # drum radius in metres
for deg in [90, 180, 360, -45, 450]:
    rad = math.radians(deg)
    print(deg, "degrees =", round(rad, 4), "rad, winds", round(r * rad, 4), "m")

90 degrees = 1.5708 rad, winds 0.0628 m
180 degrees = 3.1416 rad, winds 0.1257 m
360 degrees = 6.2832 rad, winds 0.2513 m
-45 degrees = -0.7854 rad, winds -0.0314 m
450 degrees = 7.854 rad, winds 0.3142 m

The sign carries through: a negative angle winds a negative length. With degrees in the formula the answer is too large by a factor 180/\pi \approx 57.3, a mistake that is easy to make and easy to spot by checking one full turn against 2\pi r.

Common mistakes: r * deg instead of r * math.radians(deg). Using the diameter for r. Dropping the sign of a backwards turn with abs.

Practice: How Far Did the Wheel Roll?

Wrapping an angle into (−π, π]

Turning left by 1 radian seven times leaves a robot with a heading of 7, but it faces the same way as a heading of 7 - 2\pi = 0.717. Adding or subtracting a whole turn never changes a direction, so software keeps every heading in one window of width 2\pi. On this path that window is

called wrapping the angle. Exactly one of the two ends is inside: straight left is written +\pi, never -\pi. Two ways to wrap, the loop for clarity and the formula for speed with huge angles:

import math

def wrap(a):
    """Bring an angle into (-pi, pi]."""
    while a > math.pi:
        a -= 2 * math.pi
    while a <= -math.pi:
        a += 2 * math.pi
    return a

def wrap_fast(a):
    w = (a + math.pi) % (2 * math.pi) - math.pi     # lands in [-pi, pi)
    return math.pi if w == -math.pi else w

for a in [5.0, -5.0, 20.0, math.pi, -math.pi]:
    print(round(a, 4), "->", round(wrap(a), 4), round(wrap_fast(a), 4))
print(math.atan2(math.sin(-math.pi), math.cos(-math.pi)))

5.0 -> -1.2832 -1.2832
-5.0 -> 1.2832 1.2832
20.0 -> 1.1504 1.1504
3.1416 -> 3.1416 3.1416
-3.1416 -> 3.1416 3.1416
-3.141592653589793

The <= in the second loop sends -\pi to +\pi; with < it would stay at -\pi. The formula lands in [-\pi, \pi), the wrong end, so it needs the one-line fix. The last line shows a popular shortcut, math.atan2(math.sin(a), math.cos(a)), getting the boundary wrong: it answers -\pi for a heading that should read \pi. Python's % with a positive right side is never negative, which is why the formula needs no extra case for negative angles; math.fmod keeps the sign of the left side and does.

Common mistakes: < instead of <=. Wrapping with atan2(sin, cos) and reporting -\pi. Wrapping in degrees with radian constants, or the reverse.

Practice: The Gyro That Never Forgets

Adding up a gyroscope

A gyroscope does not say which way the robot faces. It says how fast it is turning: a rate \omega in radians per second, positive counter-clockwise. A rate times a time is an angle, so over one reading interval \Delta t the robot turns \omega\,\Delta t, and the software keeps its heading by adding these up:

This is the Euler step of Signals & Control: new value = old value + rate × time step. A camera gimbal logs its rate every 0.4 s:

import math

def wrap(a):
    while a > math.pi:
        a -= 2 * math.pi
    while a <= -math.pi:
        a += 2 * math.pi
    return a

heading, dt = 2.5, 0.4
headings = []
for rate in [1.0, 1.0, 1.0, -0.5, -2.0]:
    heading = wrap(heading + rate * dt)
    headings.append(round(heading, 4))
print(headings)
plot(headings, kind="step", title="Gimbal heading", ylabel="rad")

[2.9, -2.9832, -2.5832, -2.7832, 2.7]

The second reading pushes the heading past \pi, and it reappears near -\pi: the same direction, the other name. Press Run to see the jump in the chart. Then the gimbal turns back and the heading crosses the boundary again.

Integration has a cost. A gyro whose rate is off by a tiny constant bias adds that error at every reading, and the heading error grows without limit:

bias, dt = 0.002, 0.01            # rad/s of error, 100 readings per second
error = 0.0
for _ in range(60 * 100 * 10):    # ten minutes
    error += bias * dt
print(round(error, 4), "rad, about", round(error * 180 / 3.141592653589793), "degrees")

1.2 rad, about 69 degrees

A rate that is wrong by about a tenth of a degree per second points the robot 69 degrees off after ten minutes. That is why robots correct their heading with outside references (a compass, a camera, a map) at later levels of this path.

Common mistakes: adding the rate itself instead of rate * dt. Wrapping only the final value when every intermediate heading is wanted. Reading a negative rate as a left turn.

Practice: Adding Up the Gyro

The shortest turn between two headings

A robot facing current that must face target subtracts: target - current. That raw difference is correct as a direction but may be the long way round. Wrapping it gives the shortest signed turn: positive turns left (counter-clockwise), negative turns right, and a target exactly behind gives +\pi by the wrapping rule.

import math

def wrap(a):
    while a > math.pi:
        a -= 2 * math.pi
    while a <= -math.pi:
        a += 2 * math.pi
    return a

for current, target in [(-0.5, 2.0), (2.8, -2.9), (0.3, -2.9), (1.0, 1.0 - math.pi)]:
    raw = target - current
    print(current, "->", round(target, 4), " raw", round(raw, 4), " shortest", round(wrap(raw), 4))

-0.5 -> 2.0  raw 2.5  shortest 2.5
2.8 -> -2.9  raw -5.7  shortest 0.5832
0.3 -> -2.9  raw -3.2  shortest 3.0832
1.0 -> -2.1416  raw -3.1416  shortest 3.1416

From 2.8 to −2.9 the raw difference says "turn right by 5.7", most of a full circle; the wrapped value says "turn left by 0.58", across the \pm\pi line. From 0.3 to −2.9 the left turn of 3.08 beats the right turn of 3.2, though the target's number is smaller. The last pair is exactly opposite and comes out as +\pi.

When a motor may only turn a little per control tick, recompute the shortest turn from the current heading on every tick; a turn computed once at the start goes stale as soon as the heading crosses \pm\pi.

Common mistakes: comparing target - current without wrapping. Taking abs too early and losing the direction. Computing the turn once instead of from the latest heading.

Practice: One Notch per Tick

Driving along a heading

A robot that drives d metres straight ahead along heading \theta moves by d times its forward arrow:

A negative d reverses: the robot moves backwards and still faces the same way. A turn on the spot changes only \theta; a drive changes only x and y. Applying a list of such moves in order is dead reckoning: the robot works out where it is from what it did, with no outside measurement. Here a robot follows a list of legs, each (distance, turn afterwards), and records its track:

import math
x, y, theta = 0.0, 0.0, 0.0
xs, ys = [x], [y]
for d, turn in [(3, math.pi / 2), (2, math.pi / 4), (-1, -math.pi), (1.5, 0)]:
    x += d * math.cos(theta)
    y += d * math.sin(theta)
    theta += turn
    xs.append(x)
    ys.append(y)
    print(round(x, 4), round(y, 4), round(theta, 4))
plot(xs, ys, title="Dead reckoning")

3.0 0.0 1.5708
3.0 2.0 2.3562
3.7071 1.2929 -0.7854
4.7678 0.2322 -0.7854

The third leg reverses 1 m along the heading 3\pi/4 (up and to the left), so it moves down and to the right. Then the robot turns by -\pi to face down and to the right, and drives forwards. The heading is a running sum of turns; wrap it when you report it. Wrapping can wait until the end, because \cos and \sin do not care about whole turns. Press Run to see the track.

The same step written with complex numbers, as on the Quantum Computing path, is the arrow d\,e^{i\theta}, d * cmath.exp(1j * theta) (with theta in radians), and a whole path is the sum of its steps.

Common mistakes: degrees in math.cos. Swapping cos and sin, which measures the heading from +y. Turning before the drive when the move says "drive, then turn". Treating a negative distance as a turn round.

Practice: The Lawnmower Follows Its Script, Paces and Bearings on a Treasure Map

How far, and which way: hypot and atan2

The arrow from a point P to a point Q is (\Delta x, \Delta y) = (Q_x - P_x,\ Q_y - P_y). Its length is \sqrt{\Delta x^2 + \Delta y^2}, math.hypot(dx, dy) (or math.dist(P, Q)). Its direction, the heading a robot at P needs to face Q, is math.atan2(dy, dx): the angle from +x to the arrow, in (-\pi, \pi], correct in all four quadrants.

The tempting math.atan(dy / dx) is not: it divides by zero for a vertical arrow, and it gives the same answer for an arrow and its opposite, because \frac{-1}{-1} = \frac{1}{1}.

import math
for dx, dy in [(1, 1), (-1, -1), (-3, 0.5), (0, 2), (0, -2)]:
    a2 = math.atan2(dy, dx)
    a1 = math.atan(dy / dx) if dx != 0 else "division by zero"
    print((dx, dy), " length", round(math.hypot(dx, dy), 4), " atan2", round(a2, 4),
          " atan", a1 if isinstance(a1, str) else round(a1, 4))
print(math.atan2(0, 0))

(1, 1)  length 1.4142  atan2 0.7854  atan 0.7854
(-1, -1)  length 1.4142  atan2 -2.3562  atan 0.7854
(-3, 0.5)  length 3.0414  atan2 2.9764  atan -0.1651
(0, 2)  length 2.0  atan2 1.5708  atan division by zero
(0, -2)  length 2.0  atan2 -1.5708  atan division by zero
0.0

atan2 takes dy first. The last line is a trap: math.atan2(0, 0) returns 0.0 without complaint, though an arrow of length zero has no direction. A robot already standing on its goal must be handled before atan2, by an if.

The Quantum Computing path stores the same arrow as one complex number z = dx + dy*1j: abs(z) is its length and cmath.phase(z) its direction, the same value as math.atan2(dy, dx), in the same range (-\pi, \pi]. The difference of two such directions is a turn like any other, and it needs the same wrap.

Common mistakes: math.atan2(dx, dy) (the arguments swapped measure from +y). Using atan2 for a zero arrow. Forgetting that the result is in radians.

Practice: Distance and Direction on a Dial, Which Way Did the Weather Vane Turn?, Odometer and Compass for a Delivery Round

Range and bearing

A robot describes a target by two numbers. The range is the straight-line distance. The bearing is the angle from the robot's forward direction to the target: 0 is dead ahead, positive is to its left, negative to its right. It is the target's world direction minus the robot's heading, wrapped:

A tour robot at (4, 2) facing 2.5 rad (up and to the left) looks at three signs:

import math

def wrap(a):
    while a > math.pi:
        a -= 2 * math.pi
    while a <= -math.pi:
        a += 2 * math.pi
    return a

x, y, theta = 4, 2, 2.5
for sx, sy in [(1, 4), (6, 2), (3, -1)]:
    dx, dy = sx - x, sy - y
    rng = math.hypot(dx, dy)
    bearing = wrap(math.atan2(dy, dx) - theta)
    side = "left" if bearing > 0 else "right"
    print((sx, sy), "range", round(rng, 4), "bearing", round(bearing, 4), side)

(1, 4) range 3.6056 bearing 0.0536 left
(6, 2) range 2.0 bearing -2.5 right
(3, -1) range 3.1623 bearing 1.8906 left

The first sign is nearly dead ahead. The second lies along world +x, behind the robot's right shoulder: the world direction 0 minus the heading 2.5 is −2.5, a right turn. The third is behind on the left: its world direction is -1.89, the raw difference -4.39 wraps to +1.89, and turning left is shorter. A bearing is always relative to the robot's nose; the same sign seen by a robot facing elsewhere has a different bearing and the same range.

Common mistakes: forgetting to subtract the heading, which gives the world direction. Skipping the wrap. Swapping the arguments of atan2.

Practice: Turn How Far, Drive How Far?

Turn, drive, turn

A robot that can only turn on the spot and drive straight reaches any pose in three moves: turn to face the goal position, drive there, turn to the goal heading. The straight leg fixes everything: its length d and its heading \varphi = \operatorname{atan2}(\Delta y, \Delta x). Both turns are then "from one heading to another":

The second turn is measured from the leg's heading \varphi, not from the start heading: that is where the robot faces when it arrives. A plan is easy to check, by carrying it out with the dead-reckoning update and comparing the end with the goal:

import math

def wrap(a):
    while a > math.pi:
        a -= 2 * math.pi
    while a <= -math.pi:
        a += 2 * math.pi
    return a

xa, ya, ta = 1.0, 3.0, -1.0          # start pose
xb, yb, tb = 5.0, 0.0, 2.0           # goal pose
dx, dy = xb - xa, yb - ya
phi = math.atan2(dy, dx)
alpha, d, beta = wrap(phi - ta), math.hypot(dx, dy), wrap(tb - phi)
print("turn", round(alpha, 4), " drive", round(d, 4), " turn", round(beta, 4))

x, y, t = xa, ya, ta                 # carry out the plan
t += alpha
x += d * math.cos(t)
y += d * math.sin(t)
t = wrap(t + beta)
print("arrive at", round(x, 4), round(y, 4), round(t, 4))

turn 0.3565  drive 5.0  turn 2.6435
arrive at 5.0 0.0 2.0

When the two positions are exactly the same, there is no leg and no \varphi; atan2(0, 0) would return 0 and invent one. Check for that case first: the only move is the turn \text{wrap}(\theta_b - \theta_a).

Common mistakes: measuring the second turn from the start heading. Wrapping the first turn and forgetting the second, which is the one that most often goes the long way round. Calling atan2(0, 0).

Practice: Three Moves to the Charging Dock

Compass headings and maths headings

Ships, pilots and phone apps use the compass convention: degrees, 0 is north, angles grow clockwise (90 is east), always in [0, 360). Robot software uses the maths convention of this path: radians, 0 is east (+x, with +y north), counter-clockwise. Both name the same arrows. Flipping the direction of growth is a minus sign and moving the zero from east to north is 90 degrees, so in degrees

The same formula works both ways. Then each result is wrapped into its own range: maths into (-180, 180] degrees before converting to radians, compass with % 360.

import math
for c in [0, 45, 200, 300]:
    m = (90 - c) % 360
    if m > 180:
        m -= 360
    print("compass", c, "-> maths", m, "degrees =", round(math.radians(m), 4), "rad")
t = -2.0                                       # a maths heading in radians
print("maths", t, "-> compass", round((90 - math.degrees(t)) % 360, 4))

compass 0 -> maths 90 degrees = 1.5708 rad
compass 45 -> maths 45 degrees = 0.7854 rad
compass 200 -> maths -110 degrees = -1.9199 rad
compass 300 -> maths 150 degrees = 2.618 rad
maths -2.0 -> compass 204.5916

A compass heading c points along the unit arrow (\text{east}, \text{north}) = (\sin c, \cos c), the maths arrow with sine and cosine swapped. Arrows also answer a question plain numbers cannot: the average of several directions. Headings 340 and 30 are 25 degrees either side of 5, but their plain mean is 185, nearly south. The mean of their unit arrows points the right way:

import math
hs = [340, 30]
east = sum(math.sin(math.radians(h)) for h in hs) / len(hs)
north = sum(math.cos(math.radians(h)) for h in hs) / len(hs)
print("plain mean", sum(hs) / len(hs))
print("arrow mean", round(math.degrees(math.atan2(east, north)) % 360, 4), "length", round(math.hypot(east, north), 4))

plain mean 185.0
arrow mean 5.0 length 0.9063

With compass arrows, atan2(east, north) measures from north towards east, so the result is a compass heading directly. The mean arrow is shorter than 1 when the directions disagree; its length says how much they agree.

Common mistakes: compass - 90 (east and west swap). Converting to radians before wrapping in degrees, then wrapping with the wrong constant. Averaging headings as plain numbers.

Practice: North Is Zero on the Boat, The Average Wind Direction

From the robot's frame to the world

A point fixed to the robot (a bumper corner, a camera, a box on its tray) is known in the robot frame: x_r ahead, y_r to the left. Its world position is the robot's position plus x_r forward arrows plus y_r left arrows:

Read the formula as two steps: rotate the point by \theta about the origin, as if the robot stood there, then shift it by the robot's position. The order matters: shifting first and rotating second would swing the robot's position about the world origin too.

import math

def to_world(pose, point):
    x, y, theta = pose
    c, s = math.cos(theta), math.sin(theta)
    xr, yr = point
    return (x + c * xr - s * yr, y + s * xr + c * yr)

camera = (0.4, -0.1)                     # 0.4 m ahead, 0.1 m to the right
for pose in [(3, -1, 0.0), (3, -1, math.pi / 2), (3, -1, math.pi), (3, -1, 2.0)]:
    wx, wy = to_world(pose, camera)
    print(round(pose[2], 4), "->", round(wx, 4), round(wy, 4))

0.0 -> 3.4 -1.1
1.5708 -> 3.1 -0.6
3.1416 -> 2.6 -0.9
2.0 -> 2.9245 -0.5947

Check a frame change with quarter turns, where the answer can be seen by eye. Facing up (\pi/2), "0.4 ahead" is 0.4 up and "0.1 to the right" is 0.1 towards +x: (3.1, -0.6). Facing left (\pi), ahead is −x and the right side is +y: (2.6, -0.9).

The rotation alone, (x\cos\theta - y\sin\theta,\ x\sin\theta + y\cos\theta), turns a point about the origin. To turn about another point, a pivot, shift the pivot to the origin, rotate, and shift back. The Quantum Computing path does the same turn as a multiplication by the complex number e^{i\theta}.

Common mistakes: shifting before rotating. Swapping the signs of the sin terms, which rotates clockwise (facing up, "left" would land on +x). Forgetting that the robot's y is its left: a point on its right has a negative y_r.

Practice: Where Are the Bumper Corners?, Turning a Sketch About a Pin

From the world into the robot's frame

To steer, a robot wants a world point the other way round: how far ahead and how far to the left is it? Undo the two steps in reverse order: shift by -(x, y), then rotate by -\theta. There is a shorter way to say the same thing. The arrow (\Delta x, \Delta y) from the robot to the point is in world coordinates; how far it goes along the robot's forward arrow is a dot product, and so is how far it goes along the left arrow:

(The dot product is taught on the Machine Learning path: multiply component by component and add.) A round trip, world to robot and back, must return the point you started with:

import math

def to_robot(pose, point):
    x, y, theta = pose
    c, s = math.cos(theta), math.sin(theta)
    dx, dy = point[0] - x, point[1] - y
    return (c * dx + s * dy, -s * dx + c * dy)

def to_world(pose, point):
    x, y, theta = pose
    c, s = math.cos(theta), math.sin(theta)
    return (x + c * point[0] - s * point[1], y + s * point[0] + c * point[1])

pose = (1.0, 2.0, 2.4)
for p in [(0.5, 3), (2, 2), (1, -1)]:
    r = to_robot(pose, p)
    back = to_world(pose, r)
    print(p, "-> ahead", round(r[0], 4), "left", round(r[1], 4), "-> back", (round(back[0], 4), round(back[1], 4)))

(0.5, 3) -> ahead 1.0442 left -0.3997 -> back (0.5, 3.0)
(2, 2) -> ahead -0.7374 left -0.6755 -> back (2.0, 2.0)
(1, -1) -> ahead -2.0264 left 2.2122 -> back (1.0, -1.0)

The point (0.5, 3) is ahead and slightly right; (2, 2) is behind and to the right; (1, -1), straight down, is behind and to the left.

The same arithmetic answers "how does another robot look from here?". The second robot's position goes into the first robot's frame as above, and its heading, measured from the first robot's heading, is \text{wrap}(\theta_b - \theta_a). A special case is the inverse of a pose: if a robot's pose (x, y, \theta) is given in a base station's frame, then the station, the origin (0, 0) facing 0, seen from the robot is to_robot(pose, (0, 0)) with heading \text{wrap}(-\theta). It is not simply (-x, -y, -\theta), except when \theta = 0.

Common mistakes: rotating by +\theta (the sign pattern of to_world, the wrong direction here). Rotating before subtracting the position. Reading a negative left as left.

Practice: The Map Says Here, the Robot Says Ahead, The Platoon Leader's View, Where Is the Base Station From Here?

Composing poses, and chaining frames

A camera reports a parcel's pose relative to the robot: so far ahead, so far to the left, and its heading measured from the robot's heading. To put it on the map, compose the two poses. The position is the point moved into the world with the robot's pose; the headings simply add:

Composition is not symmetric. "Walk 2 m, then turn left" and "turn left, then walk 2 m" end in different places:

import math

def wrap(a):
    while a > math.pi:
        a -= 2 * math.pi
    while a <= -math.pi:
        a += 2 * math.pi
    return a

def oplus(a, b):
    xa, ya, ta = a
    xb, yb, tb = b
    c, s = math.cos(ta), math.sin(ta)
    return (xa + c * xb - s * yb, ya + s * xb + c * yb, wrap(ta + tb))

walk = (2.0, 0.0, 0.0)
left = (0.0, 0.0, math.pi / 2)
print([round(v, 4) for v in oplus(walk, left)])
print([round(v, 4) for v in oplus(left, walk)])

[2.0, 0.0, 1.5708]
[0.0, 2.0, 1.5708]

Composition is how odometry works: a robot reports each small step (\Delta x, \Delta y, \Delta\theta) in its own frame at the start of that step, and the world pose is the start pose composed with every step in turn. The step's displacement uses the heading before the step; only then does \Delta\theta change the heading. Updating the heading first places every step one turn late.

The same idea chains frames. A sensor bolted to a robot has its own pose in the robot frame, its mount. A point the sensor sees is first moved into the robot frame with the mount, then into the world with the robot's pose: the inner frame first.

import math

def to_world(pose, point):
    x, y, theta = pose
    c, s = math.cos(theta), math.sin(theta)
    return (x + c * point[0] - s * point[1], y + s * point[0] + c * point[1])

mount = (0.2, 0.1, math.pi / 6)         # sensor 0.2 m ahead, 0.1 m left, turned 30 degrees left
pose = (5.0, 1.0, math.pi / 2)          # robot at (5, 1), facing up
seen = (2.0, 0.0)                       # 2 m straight ahead of the sensor
in_robot = to_world(mount, seen)
in_world = to_world(pose, in_robot)
print("robot frame", [round(v, 4) for v in in_robot])
print("world frame", [round(v, 4) for v in in_world])

robot frame [1.9321, 1.1]
world frame [3.9, 2.9321]

In the robot frame the point is 1.93 m ahead and 1.1 m to the left (2 m along a line turned 30 degrees left, plus the mount's offset). The robot faces up, so ahead is +y and left is −x: the point lands at (3.9, 2.93).

Common mistakes: adding the relative offset to the position without rotating it, which is right only when the robot faces +x. Rotating the offset by the relative heading \theta_b instead of the robot's \theta_a. Applying the robot's pose before the mount.

Practice: Where the Forklift Sees the Pallet, Each Step in Its Own Words, A Laser on the Back Bumper

Ahead or behind, left or right: two signs

Many robot decisions need only a side, not an angle. Two products of arrows give the side directly, with no trigonometry. For a direction u and an arrow w:

• the dot product u_x w_x + u_y w_y is positive when w points more with u than against it (ahead), negative when it points against it (behind), zero at a right angle;
• the cross product u_x w_y - u_y w_x is positive when w is a counter-clockwise turn from u (to the left), negative for a clockwise turn (to the right), zero when w lies along the line of u, ahead or behind.

The cross product is |u|\,|w|\sin\varphi and the dot product |u|\,|w|\cos\varphi, where \varphi is the angle from u to w; their signs are the signs of the sine and the cosine. A security robot at (2, 2) drives along the direction (1, 2) and sorts what it sees into quadrants:

robot, u = (2, 2), (1, 2)
for p in [(3, 6), (6, 1), (0, 3), (1, 0), (4, 6)]:
    wx, wy = p[0] - robot[0], p[1] - robot[1]
    dot = u[0] * wx + u[1] * wy
    cross = u[0] * wy - u[1] * wx
    fb = "ahead" if dot > 0 else "behind" if dot < 0 else "abeam"
    lr = "left" if cross > 0 else "right" if cross < 0 else "on the line"
    print(p, "dot", dot, "cross", cross, fb, lr)

(3, 6) dot 9 cross 2 ahead left
(6, 1) dot 2 cross -9 ahead right
(0, 3) dot 0 cross 5 abeam left
(1, 0) dot -5 cross 0 behind on the line
(4, 6) dot 10 cross 0 ahead on the line

With whole-number coordinates both products are exact, so == 0 is a fair test: (4, 6) is exactly on the line of travel. Going through atan2 would bring rounding into a question that has an exact answer.

Common mistakes: writing the cross product the other way round, u_y w_x - u_x w_y, which swaps left and right. Measuring the arrow from the wrong point. Forgetting the zero arrow: its dot and cross products are both 0, so it looks "abeam" and "on the line" at once; handle it first.

Practice: Stakes Left and Right of the Furrow, Tailwind, Headwind or Crosswind

From formula to Python

Robotics formulas turn into Python the same way as everywhere else on the site: a sum is a loop, usually a generator expression inside sum, and a formula with inputs is a function. The index under \sum becomes the for, the term becomes the expression in front of for, and \sum_{k=0}^{n-2} becomes range(n - 1), which stops before n - 1.

• The end of a dead-reckoned path from legs (d_k, \theta_k), each driven along its own heading: x_n = x_0 + \sum_k d_k\cos\theta_k is x0 + sum(d * math.cos(t) for d, t in legs).
• The length of a path through points p_0, \dots, p_{n-1}: L = \sum_{k=0}^{n-2} \lVert p_{k+1} - p_k \rVert is sum(math.dist(pts[k], pts[k + 1]) for k in range(len(pts) - 1)).
• The total turning between headings \theta_0, \dots, \theta_{n-1}: T = \sum_{k=0}^{n-2} \lvert \text{wrap}(\theta_{k+1} - \theta_k) \rvert is sum(abs(wrap(hs[k + 1] - hs[k])) for k in range(len(hs) - 1)).
• How far a grid robot that moves along one axis at a time must go for a move v: \sum_i \lvert v_i \rvert is sum(abs(a) for a in v). A robot moving in a straight line covers \sqrt{\sum_i v_i^2}, math.sqrt(sum(a * a for a in v)).
• The rotation R(\theta)\,p = (p_x\cos\theta - p_y\sin\theta,\ p_x\sin\theta + p_y\cos\theta) is a function of two inputs, def rotate(p, theta): ..., written once and called everywhere.

import math

def wrap(a):
    while a > math.pi:
        a -= 2 * math.pi
    while a <= -math.pi:
        a += 2 * math.pi
    return a

legs = [(2.0, 0.0), (1.0, 1.2), (2.5, -0.4)]
pts = [(0, 0), (3, 4), (3, 0), (6, 4)]
hs = [3.0, -3.0, -1.5, 2.0]
v = [5, -2]
print("end x", round(0.0 + sum(d * math.cos(t) for d, t in legs), 4))
print("path length", sum(math.dist(pts[k], pts[k + 1]) for k in range(len(pts) - 1)))
print("turning", round(sum(abs(wrap(hs[k + 1] - hs[k])) for k in range(len(hs) - 1)), 4))
print("grid", sum(abs(a) for a in v), " straight", round(math.sqrt(sum(a * a for a in v)), 4))

end x 4.665
path length 14.0
turning 4.5664
grid 7  straight 5.3852

The turning sum shows why the wrap sits inside the sum: from 3.0 to −3.0 is a turn of 0.28 across \pm\pi, not 6. Reading the other way works too: when a description gives a formula, name each symbol in code first (\theta_k is hs[k], n is len(hs)), then write the sum as a generator.

Practice: Three Ways to Measure a Move

Is it the same pose? Tolerances and ties

A robot driving a 1 m square with four quarter turns ends where it began, in theory. In floats the position is off by about 10^{-16}, and the heading is 2\pi, not 0. So two poses are "the same" when

• their positions are within a distance tolerance: math.hypot(x1 - x2, y1 - y2) <= pos_tol, and
• their headings are within an angle tolerance after wrapping the difference: abs(wrap(t1 - t2)) <= angle_tol.

import math

def wrap(a):
    while a > math.pi:
        a -= 2 * math.pi
    while a <= -math.pi:
        a += 2 * math.pi
    return a

x, y, t = 0.0, 0.0, 0.0
for _ in range(4):
    x += math.cos(t)
    y += math.sin(t)
    t += math.pi / 2
print(x, y, t)
print("same position:", math.hypot(x, y) <= 1e-9, " same heading:", abs(wrap(t - 0.0)) <= 1e-9, " raw t == 0:", t == 0)

-1.8369701987210297e-16 2.220446049250313e-16 6.283185307179586
same position: True  same heading: True  raw t == 0: False

Choosing among candidates (the nearest charger, the nearest parcel) raises the same issue for ties. Rules such as "nearest first, then the smaller turn, then the earlier in the list" work in one pass: keep the best so far and replace it only when a new candidate is strictly better by the rules in order. With whole-number positions, compare squared distances, which are exact; two math.hypot values can differ in the last digit. Floats like turns are equal when they differ by less than a tolerance.

import math
robot = (0, 0, 0.0)
parcels = [(4, 3), (-5, 0), (0, 5), (4, -3)]
best = None
for i, (px, py) in enumerate(parcels):
    d2 = (px - robot[0]) ** 2 + (py - robot[1]) ** 2
    turn = abs(math.atan2(py - robot[1], px - robot[0]) - robot[2])   # heading 0: no wrap needed here
    if best is None or d2 < best[0] or (d2 == best[0] and turn < best[1] - 1e-9):
        best = (d2, turn, i)
print("parcel", best[2], "squared distance", best[0], "turn", round(best[1], 4))

parcel 0 squared distance 25 turn 0.6435

All four parcels are 5 m away. The smallest turn wins, and two parcels need the same turn (0.6435 left and right), so the earlier one in the list stays: turn < best[1] - 1e-9 is false for an equal turn. Writing <= would let a later candidate win a full tie.

Common mistakes: testing only the position, or comparing raw headings. Comparing floats with ==. Replacing the best on <=. Calling atan2 for a candidate the robot already stands on.

Practice: Is the Floor Cleaner Home Yet?, Which Rover Answers the Call?, The Sampling Rover Picks the Nearest Rock

Record the poses once

A robot's log of commands gives one trajectory. Questions about parts of the log, "how far did it get between command 4 and command 9?", do not need the part simulated again from scratch: the poses before and after the part are already on that trajectory. Simulate once, store the pose before the first command and after every command, and every question about a block of commands becomes a pair of stored poses. With poses[k] the pose after k commands, the block of commands i to j (inclusive) starts at poses[i] and ends at poses[j + 1]. This is the prefix-sum idea of the Algorithms path, applied to poses.

import math
log = [("drive", 2), ("turn", math.pi / 2), ("drive", 1), ("turn", math.pi / 2), ("drive", 2), ("drive", -0.5)]
x, y, t = 0.0, 0.0, 0.0
poses = [(x, y, t)]
for kind, value in log:
    if kind == "turn":
        t += value
    else:
        x += value * math.cos(t)
        y += value * math.sin(t)
    poses.append((x, y, t))
for i, j in [(0, 5), (2, 4), (1, 3)]:
    (x0, y0, _), (x1, y1, _) = poses[i], poses[j + 1]
    print("commands", i, "to", j, "moved", round(math.hypot(x1 - x0, y1 - y0), 4), "m")

commands 0 to 5 moved 1.118 m
commands 2 to 4 moved 2.2361 m
commands 1 to 3 moved 1.0 m

The stored list costs one pass; each question afterwards costs one subtraction, however long the block. Another record a robot keeps is a set of grid cells it has visited, each a tuple (x, y): tuples can go into a set, and cell in visited is answered at once, without scanning the history.

visited = {(0, 0)}
x = y = 0
for step in "NNEESWWS":
    dx, dy = {"N": (0, 1), "S": (0, -1), "E": (1, 0), "W": (-1, 0)}[step]
    x, y = x + dx, y + dy
    print(step, (x, y), "seen before" if (x, y) in visited else "new")
    visited.add((x, y))

N (0, 1) new
N (0, 2) new
E (1, 2) new
E (2, 2) new
S (2, 1) new
W (1, 1) new
W (0, 1) seen before
S (0, 0) seen before

Common mistakes: an off-by-one between command indices and pose indices (poses has one more entry than the log). Re-simulating every block, which turns one pass into a cubic search. Storing cells as lists, which cannot go into a set.

Practice: The Stretch That Went Nowhere, Where the Rover Crosses Its Track

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.

• Adding Up the Gyro
• Where Are the Bumper Corners?
• Turn How Far, Drive How Far?
• The Lawnmower Follows Its Script
• Where the Forklift Sees the Pallet
• Three Moves to the Charging Dock

Checklist

You can now:

• Describe where a robot is with a pose (x, y, theta) in a frame, and write the robot's forward and left arrows in world coordinates.
• Convert degrees and radians, and turn an angle into an arc length with s = r\theta.
• Wrap any angle into (-\pi, \pi], with -\pi written as +\pi, and know why atan2(sin, cos) gets the boundary wrong.
• Integrate a gyroscope's rate into a heading, and explain why a small bias grows without limit.
• Find the shortest signed turn between two headings, and translate between compass headings and maths headings.
• Follow a script of turns and drives by dead reckoning, and plan a turn-drive-turn move between two poses.
• Find the range and bearing to a target with math.hypot and math.atan2, handling a zero arrow first.
• Move a point from the robot's frame into the world (rotate, then shift) and back (shift, then rotate the other way, or two dot products).
• Compose poses, invert one, and chain a sensor's mount with the robot's pose, inner frame first.
• Tell ahead from behind and left from right with the signs of the dot and cross products, and compare poses and candidates with tolerances and stated tie-breaks.