Level 4 · Robotics: Belief and Optimisation
Level 2's odometry drifts, and Level 3's sensors measure the world around the robot. This level puts the two together to answer "where am I?" against a map. The answer is not one guess. It is a belief: a probability for every place the robot could be. Two updates keep it current. A reading sharpens it (Bayes' rule, cell by cell); a move blurs it (each cell passes its probability on to where the robot may land). Run them in turn and you have histogram localization, a robot that starts out lost and finds itself. You will also measure how sure it is, use a range sensor with Gaussian noise, work on a 2D floor, recover from being carried off, and choose the move that should teach it the most.
The second topic returns to the arm. Level 3 found the joint angles for a target with a formula, which only short arms have. Here the question is smaller and more useful: when each joint turns a little, how does the hand move? The answer is a matrix of partial derivatives, the Jacobian. It turns joint speeds into hand velocities and back, shows where an arm loses a direction (a singular pose), says how encoder errors blur the hand, and turns inverse kinematics into optimisation: walk the angles downhill on the distance to the target.
Both topics borrow from other paths, and this lesson teaches only the robotics twist. The sense update is Bayes' rule from Statistics (Level 3), applied to every cell at once. Gradient descent and numerical gradients come from Machine Learning (Level 4); here the parameters are joint angles and the loss is a distance. Move-then-sense is also the predict-correct loop of the Kalman filter on the Control path (Level 6), with a list of probabilities in place of a Gaussian. Everything is plain Python with math.
A belief, and what a reading does to it
Cut the robot's world into n cells, say metre-long stretches of a garden path. The belief is a list belief of n probabilities that add up to 1: belief[i] is how likely the robot is to be in cell i. A robot with no idea starts uniform, 1 / n everywhere. The map says what is in each cell; here, a tree beside the path or open ground.
A sensor reading z is evidence. The sensor model gives its likelihood from each cell, P(z \mid i): how probable this reading would be if the robot stood in cell i. Bayes' rule, from the Statistics lesson, then gives the new belief in every cell:
In code: multiply each cell by its likelihood, then divide by the sum. That sum is P(z), the probability of the reading itself. A lawn robot's camera reports "tree" correctly 75% of the time beside a tree, and "open" correctly 85% of the time on open ground:
world = ["tree", "open", "open", "tree", "tree", "open", "open", "open"]
like = {("tree", "tree"): 0.75, ("open", "tree"): 0.25, # P(reading | a tree beside the cell)
("open", "open"): 0.85, ("tree", "open"): 0.15} # P(reading | open ground)
def sense(belief, reading):
weights = [b * like[(reading, side)] for b, side in zip(belief, world)]
total = sum(weights) # Bayes' denominator: P(reading)
return [w / total for w in weights], total
for prior in ([1 / 8] * 8, [0.0, 0.1, 0.3, 0.4, 0.2, 0.0, 0.0, 0.0]):
post, pz = sense(prior, "tree")
print("P(tree) =", round(pz, 4), " ->", [round(b, 4) for b in post])
plot(list(range(8)), post, kind="bar", xlabel="cell", ylabel="belief")
P(tree) = 0.375 -> [0.25, 0.05, 0.05, 0.25, 0.25, 0.05, 0.05, 0.05]
P(tree) = 0.51 -> [0.0, 0.0294, 0.0882, 0.5882, 0.2941, 0.0, 0.0, 0.0]
From a uniform start, "tree" makes each tree cell five times as likely as each open cell, the ratio 0.75 / 0.15 of the likelihoods. From the second prior, which already ruled out cell 0, the tree in cell 0 gets nothing: zero times any likelihood is zero. The prior still counts, as base rates do in Statistics: cell 3 ends twice as likely as cell 4 because it started twice as likely.
If every weight is 0, no cell can explain the reading, and the sum you would divide by is 0. Something is wrong (the map, the sensor model, or a belief that was too sure), so check for it before dividing, and do what the specification says, often a fresh uniform belief.
Common mistakes: giving a cell the chance of a correct reading when the reading disagrees with its map entry: that likelihood is 1 - p. Forgetting to normalise. Dividing by a zero sum. Confusing P(z \mid i), the sensor model, with P(i \mid z), the answer.
Practice: Was That a Door?
Moving spreads the belief
When the robot moves, its belief must move with it. If the wheels were perfect, the list would simply shift. They slip, so a command to move u cells comes with a motion model: a list of outcomes (d, p), "it really moved d cells with probability p". Each cell pushes its probability to every cell it may land in, weighted by the outcome's probability. Several cells can land in the same place, so start from a fresh list of zeros and add. This is the law of total probability:
Where it lands depends on the world. On a ring (a loop of track) cell n - 1 is followed by cell 0, so the landing cell is (i + d) % n. In a corridor with end walls, a move that would leave it stops at the end cell: min(n - 1, max(0, i + d)).
def move(belief, outcomes, ring):
n = len(belief)
new = [0.0] * n # a fresh list: add into it
for i, b in enumerate(belief):
for d, p in outcomes: # moved d cells with probability p
j = (i + d) % n if ring else min(n - 1, max(0, i + d))
new[j] += b * p
return new
slip = [(1, 0.2), (2, 0.6), (3, 0.2)] # told to move 2 cells
belief = [0.0] * 10
belief[7] = 1.0
for step in range(3):
belief = move(belief, slip, ring=True)
print("ring", step + 1, [round(b, 3) for b in belief])
print("corridor", [round(b, 3) for b in move([0, 0, 0, 0, 0, 0, 0, 0.5, 0.5, 0], slip, ring=False)])
ring 1 [0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.6]
ring 2 [0.24, 0.44, 0.24, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04]
ring 3 [0.008, 0.072, 0.24, 0.36, 0.24, 0.072, 0.008, 0.0, 0.0, 0.0]
corridor [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.9]
On the ring, the certain robot in cell 7 lands in cells 8, 9 and 0 (it wrapped). Each further move widens the bump: after three moves of 2 cells it covers seven cells, peaked at cell 3, six cells on from cell 7 round the ring. Moving never makes the robot surer. Sliding one small pattern of probabilities along a list and adding up is a convolution, the same operation that smooths a signal. In the corridor, every move that would pass the end wall piles up in cell 9: 0.9 of the probability sits against the wall.
Common mistakes: new[j] = ... instead of +=, which loses probability when two outcomes land in one cell. Moving backwards with i - d. Wrapping in a corridor, or dropping the moves that leave it (then the belief no longer adds up to 1). Changing belief while still reading from it.
Practice: Round the Greenhouse Rail, Nowhere to Go but the End Wall
How sure is the robot?
Two numbers summarise a belief. The entropy looks at its whole shape:
where cells with p_i = 0 add nothing (the limit of p \log p is 0, and math.log2(0) is an error, so skip them). A certain robot has H = 0. A belief spread evenly over 2^m cells has H = m: it would take m yes/no questions to find the robot. With log2 the unit is bits; the natural logarithm would give "nats".
The best guess is the cell with the largest probability, and a practical question is how much probability lies near it: the sum over the cells within k of the best guess, with the window clipped at the ends of a corridor.
import math
def entropy(p):
return sum(-x * math.log2(x) for x in p if x > 0) # skip the zeros
for name, p in [("certain", [0, 0, 1, 0]), ("two cells", [0.5, 0.5, 0, 0]),
("uniform over 8", [1 / 8] * 8), ("lopsided", [0.7, 0.1, 0.1, 0.1])]:
print(name, round(entropy(p), 4), "bits")
belief = [0.05, 0.1, 0.3, 0.3, 0.15, 0.1]
best = 0
for i, p in enumerate(belief):
if p > belief[best]: # strictly larger: a tie keeps the first
best = i
k = 1
lo, hi = max(0, best - k), min(len(belief) - 1, best + k)
print("best", best, " mass within", k, "cells:", round(sum(belief[lo:hi + 1]), 4))
certain 0.0 bits
two cells 1.0 bits
uniform over 8 3.0 bits
lopsided 1.3568 bits
best 2 mass within 1 cells: 0.7
Cells 2 and 3 tie at 0.3; the strict > keeps the first, cell 2, and the window holds cells 1 to 3. belief.index(max(belief)) does the same in one line. Written as -x * log2(x) inside the sum, a certain belief gives 0.0; with the minus sign outside, Python prints -0.0.
Common mistakes: math.log2(0). The natural logarithm where bits are asked for. >= in the best-guess loop, which lets a later tie win. Wrapping the window round the ends of a corridor that has walls.
Practice: How Sure Is the Robot?
Move, sense, repeat: the histogram filter
A localizing robot alternates the two updates: move (the belief spreads), then sense (it sharpens). Starting from a uniform belief, a map with distinctive stretches soon pins it down. This loop is the Bayes filter; with a list over cells it is called the histogram filter, or grid localization.
Comparing probabilities needs care. Two cells that should tie can come out of a long run of sums differing by 10^{-17}, so "the largest" is decided with a tolerance: find the top value first, then take the first cell within 1e-9 of it, as in Level 2's highest arm pose.
A delivery robot on a loop of 9 cells, each with a post or a gap beside it; its sensor is right 85% of the time, and its moves land one short or one long 10% of the time each:
import math
world = ["post", "gap", "gap", "post", "gap", "post", "post", "gap", "gap"]
n, p_correct = len(world), 0.85
def move(belief, u):
new = [0.0] * n
for i, b in enumerate(belief):
for d, p in ((u - 1, 0.1), (u, 0.8), (u + 1, 0.1)):
new[(i + d) % n] += b * p
return new
def sense(belief, reading):
w = [b * (p_correct if world[i] == reading else 1 - p_correct) for i, b in enumerate(belief)]
total = sum(w)
return [x / total for x in w]
def entropy(p):
return sum(-x * math.log2(x) for x in p if x > 0)
belief = [1 / n] * n
print("start H", round(entropy(belief), 3))
for u, reading in [(1, "post"), (1, "post"), (2, "gap"), (2, "post")]:
belief = move(belief, u)
h_moved = entropy(belief)
belief = sense(belief, reading)
top = max(belief)
best = next(i for i, b in enumerate(belief) if b >= top - 1e-9)
print(u, reading.ljust(5), "H", round(h_moved, 3), "->", round(entropy(belief), 3), " best", best, round(belief[best], 4))
plot(list(range(n)), belief, kind="bar", xlabel="cell", ylabel="belief")
start H 3.17
1 post H 3.17 -> 2.74 best 0 0.2048
1 post H 2.945 -> 2.636 best 6 0.3948
2 gap H 2.802 -> 2.254 best 8 0.466
2 post H 2.528 -> 2.325 best 0 0.4897
Read the entropy column: every move raises it (or, from uniform, leaves it), every reading lowers it. Two posts in a row point to the pair of neighbouring posts, cells 5 and 6; after a gap and another post the robot believes, at about 49%, that it is in cell 0, the post after the two gaps. Press Run to see the final belief. A map that looks the same from two places (a pattern that repeats round the loop) can never tell them apart: their probabilities stay tied however long the robot drives.
The order of the two updates is part of the method: a filter that senses first and then moves gives other numbers. The problems state the order; this lesson uses move, then sense.
Common mistakes: sensing before moving when the order is move-then-sense. Comparing floats with == in a tie-break. Forgetting to normalise, so every probability shrinks towards 0 step after step. Reusing one list for the old and the new belief inside a move.
Practice: Lost on the Ring Corridor
Readings that are numbers: a Gaussian likelihood
A door sensor gives one of two words. A range sensor gives a number, and its noise is usually modelled as Gaussian (normal). The sense update needs a likelihood per cell, built in two steps:
- Predict the reading from the map: if the robot stood in cell i, what distance d_i would it measure? This is Level 3's measurement model.
- Turn the gap z - d_i into a weight with the Gaussian density of standard deviation \sigma:
The factor in front is the same for every cell, so it cancels when you normalise; only the exponential matters. A robot in a 10-cell corridor whose sonar looks left, towards cell 0, with a wall before cell 0; it measures 2.7 cells:
import math
row = [0, 0, 1, 0, 0, 0, 1, 0, 0, 0] # 1 = blocked; the sensor looks LEFT, toward cell 0
n = len(row)
expected, last = [], -1 # the wall before cell 0 counts as blocked cell -1
for i in range(n):
expected.append(i - last) # distance to the nearest blocked cell on the left
if row[i] == 1:
last = i
print("expected", [e if row[i] == 0 else "-" for i, e in enumerate(expected)])
z, sigma = 2.7, 0.4
belief = [0.0 if b else 1 / row.count(0) for b in row] # uniform over the free cells
weights = [belief[i] * math.exp(-(z - expected[i]) ** 2 / (2 * sigma ** 2)) for i in range(n)]
total = sum(weights)
print("belief ", [round(w / total, 4) for w in weights])
expected [1, 2, '-', 1, 2, 3, '-', 1, 2, 3]
belief [0.0001, 0.1002, 0.0, 0.0001, 0.1002, 0.3497, 0.0, 0.0001, 0.1002, 0.3497]
One pass from left to right finds every expected distance: remember the last blocked cell seen, and each free cell's distance is its index minus that one. (A sensor looking right would make the pass from right to left.) Cells 5 and 9 both expect 3, a gap of 0.3, and share most of the belief; cells expecting 2 (a gap of 0.7) get under a third as much each; cells expecting 1 (a gap of 1.7, over four standard deviations) are all but ruled out. Every cell with the same prediction gets the same weight: the map, not the sensor, decides what can be told apart.
Common mistakes: counting the cells between the robot and the obstacle instead of the distance. Forgetting the end wall, which every cell sees if nothing else is in the way. sigma where the formula has sigma ** 2. Giving blocked cells a weight.
Practice: Two Metres to the Trolley
The same filter on a grid
Nothing in the filter cares that the cells form a line. On a floor plan, the belief is a list of rows, belief[r][c], with row 0 at the top as in every grid on this site, so north is r - 1. The motion update pushes each tile's probability to its landing tiles, and the sense update reweights and normalises the whole grid.
A floor of light (L) and dark (D) tiles. A command moves the robot one tile with probability 0.8; otherwise its wheels slip and it stays. A move off the floor bumps the skirting board and leaves it where it was. Its sensor reads "D" on 90% of dark tiles and on 20% of light ones:
grid = ["LLDL",
"DLLL",
"LDLD"] # L light tile, D dark tile; row 0 at the top
R, C = len(grid), len(grid[0])
STEP = {"N": (-1, 0), "S": (1, 0), "E": (0, 1), "W": (0, -1)}
p_go, p_hit, p_miss = 0.8, 0.9, 0.2 # P(reading D | dark tile), P(reading D | light tile)
belief = [[1 / (R * C)] * C for _ in range(R)]
for command, reading in [("E", "D"), ("S", "L")]:
dr, dc = STEP[command]
new = [[0.0] * C for _ in range(R)]
for r in range(R):
for c in range(C):
b = belief[r][c]
new[r][c] += b * (1 - p_go) # the wheels slipped: it stayed
rr, cc = r + dr, c + dc
if 0 <= rr < R and 0 <= cc < C:
new[rr][cc] += b * p_go
else:
new[r][c] += b * p_go # bumped the edge: it stayed too
for r in range(R):
for c in range(C):
dark = grid[r][c] == "D"
if reading == "D":
new[r][c] *= p_hit if dark else p_miss
else:
new[r][c] *= (1 - p_hit) if dark else (1 - p_miss)
total = sum(map(sum, new))
belief = [[x / total for x in line] for line in new]
for line in belief:
print([round(x, 3) for x in line])
[0.003, 0.017, 0.009, 0.03]
[0.004, 0.084, 0.319, 0.151]
[0.077, 0.056, 0.151, 0.1]
After "east, dark" and "south, light", the light tile at row 1, column 2 leads with 0.319: it sits just south of the dark tile at (0, 2), which the robot reaches by moving east from (0, 1). A dictionary from a direction letter to (drow, dcol) keeps the four directions in one place, and sum(map(sum, new)) adds up a whole grid.
Common mistakes: "N" as row + 1. Losing the probability of a blocked move instead of keeping it on the tile. Updating belief in place while other tiles still read from it. Normalising each row on its own instead of the whole grid.
Practice: Coloured Tiles in the Toy Shop
The kidnapped robot
A robot that is sure of itself has a weakness. If someone picks it up and puts it down elsewhere (the kidnapped robot problem), its belief is certain and wrong, and a filter that allows probability 0 can never recover: a cell at 0 stays at 0, whatever the readings say. Each disagreeing reading is simply blamed on sensor noise.
The cure is one extra line after each step: mix a little uniform belief back in,
as if a fresh kidnapping were possible at any time. The list still adds up to 1, and no cell ever drops to 0. A robot on a loop of 8 cells believes it is in cell 0 but stands in cell 4; it moves one cell per step and reads what is really there:
world = ["post", "gap", "gap", "post", "gap", "post", "post", "gap"]
n, p_correct = len(world), 0.9
truth = 4 # where the robot really is after the visitor
readings = []
for t in range(1, 11): # it moves one cell per step; what it really sees
readings.append(world[(truth + t) % n])
def run(eps):
belief = [1.0] + [0.0] * (n - 1) # sure, and wrong: it thinks it is in cell 0
guesses = []
for reading in readings:
belief = belief[-1:] + belief[:-1] # an exact move of one cell round the ring
w = [b * (p_correct if world[i] == reading else 1 - p_correct) for i, b in enumerate(belief)]
total = sum(w)
belief = [(1 - eps) * x / total + eps / n for x in w]
top = max(belief)
guesses.append(next(i for i, b in enumerate(belief) if b >= top - 1e-9))
return guesses
print("truth ", [(truth + t) % n for t in range(1, 11)])
print("eps 0 ", run(0.0))
print("eps 0.05", run(0.05))
truth [5, 6, 7, 0, 1, 2, 3, 4, 5, 6]
eps 0 [1, 2, 3, 4, 5, 6, 7, 0, 1, 2]
eps 0.05 [1, 2, 3, 0, 1, 2, 3, 4, 5, 6]
Without mixing, the robot marches on in its imagined loop forever. With \varepsilon = 0.05, the readings that keep disagreeing build a new peak, and from the fourth step on the best guess is the true cell. The price is sharpness: the best guess can never reach probability 1. (Rotating the list, belief[-1:] + belief[:-1], is an exact move of one cell round a ring.)
Common mistakes: mixing before the sense update when the specification mixes after it. Forgetting the factor 1 - eps, so the belief adds up to more than 1. A tie-break without tolerance.
Practice: Carried Off by a Visitor
The normalising sum is evidence
The sum you divide by in each sense update is not just bookkeeping. It is P(z_t \mid z_1, \dots, z_{t-1}): how probable this reading was, predicted from everything before it. Multiplying those sums over a run gives the probability of the whole run of readings, by the chain rule of probability:
That number, the evidence, compares maps. Which of two floor plans fits what the robot saw? Run the filter on each, multiply the sums, and apply Bayes' rule across the plans:
maps = {"old plan": ["post", "gap", "gap", "post", "gap", "gap"],
"new plan": ["post", "gap", "post", "post", "gap", "gap"]}
steps = [(1, "post"), (1, "post"), (2, "gap"), (1, "post")]
p_correct = 0.85
evidence = {}
for name, world in maps.items():
n = len(world)
belief, prob = [1 / n] * n, 1.0
for u, reading in steps:
moved = [0.0] * n
for i, b in enumerate(belief):
for d, p in ((u - 1, 0.1), (u, 0.8), (u + 1, 0.1)):
moved[(i + d) % n] += b * p
w = [b * (p_correct if world[i] == reading else 1 - p_correct) for i, b in enumerate(moved)]
total = sum(w) # P(this reading | the readings so far, this map)
prob *= total
belief = [x / total for x in w]
evidence[name] = prob
print(name, " P(all readings) =", round(prob, 6), " best cell", belief.index(max(belief)), round(max(belief), 3))
s = sum(evidence.values())
print({name: round(e / s, 4) for name, e in evidence.items()})
old plan P(all readings) = 0.037555 best cell 0 0.443
new plan P(all readings) = 0.076906 best cell 0 0.757
{'old plan': 0.3281, 'new plan': 0.6719}
Two posts in a row are unlikely on the old plan, which has no neighbouring posts, so it explains the run about half as well, and with equal priors the new plan wins two to one. Judge a map by this probability of the readings, never by how peaked its final belief is: a wrong map can still end with a confident belief. Normalise every step and multiply the sums; skipping the normalisation and multiplying anyway counts early readings twice.
Common mistakes: comparing maps by their largest final belief. Forgetting the sums (they are the evidence). Multiplying sums from unnormalised beliefs.
Practice: Which Floor Am I On?
Which move tells the most?
A robot can also use the filter in imagination, before it acts. For a candidate move, do the motion update (it does not depend on the reading), then branch on every reading z it might get. Each branch is an ordinary sense update; its normalising sum is P(z), and its result has an entropy. The expected entropy of the move weights them:
The move with the smallest E(u) is expected to leave the robot surest: the largest information gain H_{\text{now}} - E(u). A robot that is in cell 1 (60%) or cell 5 (40%) weighs three moves:
import math
world = ["gap", "post", "gap", "gap", "post", "post", "gap", "gap"]
n = len(world)
like = {("post", "post"): 0.9, ("gap", "post"): 0.1, ("gap", "gap"): 0.8, ("post", "gap"): 0.2}
belief = [0.0, 0.6, 0.0, 0.0, 0.0, 0.4, 0.0, 0.0] # cell 1 or cell 5
def entropy(p):
return sum(-x * math.log2(x) for x in p if x > 0)
for u in (1, 2, 3):
moved = [0.0] * n
for i, b in enumerate(belief):
moved[(i + u) % n] += b # an exact move, to keep it short
expected = 0.0
for z in ("post", "gap"):
w = [b * like[(z, world[i])] for i, b in enumerate(moved)]
pz = sum(w) # how likely this reading is
if pz > 0:
h = entropy([x / pz for x in w])
expected += pz * h
print(f"u={u} read {z:4} P={pz:.3f} H after={h:.4f}")
print(f"u={u} expected H = {expected:.4f} (now {entropy(belief):.4f})")
u=1 read post P=0.200 H after=0.9710
u=1 read gap P=0.800 H after=0.9710
u=1 expected H = 0.9710 (now 0.9710)
u=2 read post P=0.200 H after=0.9710
u=2 read gap P=0.800 H after=0.9710
u=2 expected H = 0.9710 (now 0.9710)
u=3 read post P=0.620 H after=0.5548
u=3 read gap P=0.380 H after=0.6292
u=3 expected H = 0.5831 (now 0.9710)
Moves of 1 and 2 put both possible positions beside a gap, so whatever the sensor says, the two keep their 60/40 split: no gain. A move of 3 puts one beside a post and one beside a gap, and either reading tells them apart. Moving alone never lowers the entropy, so a move must be scored by the entropy after the imagined reading. Skip a reading with P(z) = 0: it cannot happen, and you cannot divide by it.
Common mistakes: scoring a move by the entropy after the motion update alone. Following only the likelier reading. Forgetting to weight each branch by P(z). Dividing by P(z) = 0.
Practice: The Move That Tells the Most
How the hand answers each joint: the Jacobian
Back to the planar arm of Level 2: joint angles relative to the previous link, the hand at
A partial derivative asks how one output changes when one input changes and the others stay fixed. Four of them describe a two-link arm, and together they form the Jacobian, rows for x and y, columns for t_1 and t_2:
Two rules give every entry: \frac{d}{dt}\cos t = -\sin t and \frac{d}{dt}\sin t = \cos t, and t_1 sits inside \cos(t_1 + t_2) too, so the first column has both links' terms.
Read a column as a motion: column i is the velocity of the hand when joint i turns at 1 rad/s and the others hold still. Turning a joint swings everything beyond it in a circle around that joint, so each link j from i on adds l_j(-\sin\varphi_j, \cos\varphi_j), its own vector turned a quarter turn, where \varphi_j is its absolute direction. For the shoulder that is the whole arm: column 0 is always (-y, x). The rule works for any number of links:
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
def jacobian(lengths, q):
heads, h = [], 0.0
for angle in q:
h += angle
heads.append(h) # absolute direction of each link
cols = []
for i in range(len(q)): # turning joint i swings links i, i+1, ...
cols.append((-sum(l * math.sin(p) for l, p in zip(lengths[i:], heads[i:])),
sum(l * math.cos(p) for l, p in zip(lengths[i:], heads[i:]))))
return [[c[0] for c in cols], [c[1] for c in cols]] # row 0: x, row 1: y
lengths, q = [0.9, 0.6], [0.7, 0.8]
x, y = hand(lengths, q)
print("hand", round(x, 4), round(y, 4))
print("J ", [[round(v, 4) for v in r] for r in jacobian(lengths, q)])
print("3 links:", [[round(v, 4) for v in r] for r in jacobian([0.9, 0.6, 0.3], [0.7, 0.8, -0.5])])
hand 0.7308 1.1783
J [[-1.1783, -0.5985], [0.7308, 0.0424]]
3 links: [[-1.4307, -0.8509, -0.2524], [0.8929, 0.2045, 0.1621]]
The first column, (-1.1783, 0.7308), is (-y, x) for the hand at (0.7308, 1.1783). A three-link arm has a 2 \times 3 Jacobian, one column per joint. The layout is a convention worth keeping: rows are hand coordinates, columns are joints. The transpose holds the same numbers in the wrong places.
Common mistakes: dropping -l_2\sin(t_1+t_2) from \partial x/\partial t_1. Sign slips in the derivative of cosine. Building column i from link i alone instead of every link from i on. Returning the transpose.
Practice: How the Hand Answers Each Joint
Joint speeds become hand velocity
When both joints turn at once, at rates \dot t_1 = w_1 and \dot t_2 = w_2 rad/s, the chain rule adds their effects: each hand coordinate changes at (its sensitivity to t_1) times w_1 plus (its sensitivity to t_2) times w_2. That is a matrix times a vector:
The velocity depends on the pose (through J) and is exact at that instant. A tiny simulated time step confirms it:
import math
l1, l2, t1, t2 = 0.9, 0.6, 0.7, 0.8
w1, w2 = 0.4, -0.9 # rad/s; w2 turns the elbow's relative angle
a = t1 + t2
J = [[-l1 * math.sin(t1) - l2 * math.sin(a), -l2 * math.sin(a)],
[l1 * math.cos(t1) + l2 * math.cos(a), l2 * math.cos(a)]]
vx = J[0][0] * w1 + J[0][1] * w2
vy = J[1][0] * w1 + J[1][1] * w2
print("J q-dot: ", round(vx, 4), round(vy, 4), " speed", round(math.hypot(vx, vy), 4))
def fk(t1, t2):
return l1 * math.cos(t1) + l2 * math.cos(t1 + t2), l1 * math.sin(t1) + l2 * math.sin(t1 + t2)
dt = 1e-6 # let the motors run for a microsecond
(x0, y0), (x1, y1) = fk(t1, t2), fk(t1 + w1 * dt, t2 + w2 * dt)
print("simulated: ", round((x1 - x0) / dt, 4), round((y1 - y0) / dt, 4))
J q-dot: 0.0673 0.2541 speed 0.2629
simulated: 0.0673 0.2541
w_2 is the rate of the relative elbow angle. The second link's direction in the world turns at w_1 + w_2, which the formula already accounts for: feeding it w_1 + w_2 as well would count the shoulder twice.
Common mistakes: multiplying by J^T instead of J. Treating w_2 as the second link's absolute rate. Forgetting that the shoulder carries the second link along.
Practice: How Fast Is the Spray Nozzle Moving?
Checking a Jacobian by nudging the joints
A Jacobian worked out by hand deserves a test, and an arm whose formula nobody wrote down still needs a Jacobian. Both call for the central difference from the Machine Learning lesson: move one joint by +h and by -h, compute the hand both times, and divide the change by 2h. One joint gives one column (both x and y):
Its error shrinks like h^2; the one-sided difference (p(q + h) - p(q))/h shrinks only like h:
import math
l1, l2, t1, t2 = 0.9, 0.6, 0.7, 0.8
def x_of(t1): # the hand's x, with t2 held fixed
return l1 * math.cos(t1) + l2 * math.cos(t1 + t2)
exact = -l1 * math.sin(t1) - l2 * math.sin(t1 + t2)
for h in (0.2, 0.1, 0.05, 0.025):
central = (x_of(t1 + h) - x_of(t1 - h)) / (2 * h)
forward = (x_of(t1 + h) - x_of(t1)) / h
print(f"h={h:<6} central error {abs(central - exact):.6f} one-sided error {abs(forward - exact):.6f}")
h=0.2 central error 0.007840 one-sided error 0.064997
h=0.1 central error 0.001963 one-sided error 0.034547
h=0.05 central error 0.000491 one-sided error 0.017775
h=0.025 central error 0.000123 one-sided error 0.009012
Halving h divides the central error by 4 and the one-sided error by only 2. To compare a whole estimated Jacobian with the exact one, take the largest absolute difference over all entries.
Common mistakes: dividing by h instead of 2 * h. Nudging both joints at once. Writing column 0 into row 0. A tiny h such as 1e-12, where rounding swamps the difference.
Practice: Nudge Each Joint, Watch the Hand, Slopes of a Black Box
Singular poses: where the hand loses a direction
The two columns of J are the hand's two ways to move. Usually they point in different directions and, combined, move the hand anywhere nearby. When they line up, one direction is lost. The determinant measures this: \det J = J_{00}J_{11} - J_{01}J_{10} is the area of the parallelogram the columns span, and multiplying out, almost everything cancels:
It is zero exactly when the elbow is stretched (t_2 = 0) or folded (t_2 = \pi), wherever the shoulder points. These are the edges of Level 2's ring, where Level 3's elbow-up and elbow-down solutions meet. Stretched, both columns are perpendicular to the arm: the hand can swing sideways but cannot move further out, which is why the outer edge is the edge.
import math
l1, l2 = 0.9, 0.6
for t2 in (1.2, 0.3, 0.01, 0.0, -0.01, math.pi, 2 * math.pi):
t1 = 0.4 # any shoulder angle gives the same determinant
a = t1 + t2
J = [[-l1 * math.sin(t1) - l2 * math.sin(a), -l2 * math.sin(a)],
[l1 * math.cos(t1) + l2 * math.cos(a), l2 * math.cos(a)]]
det = J[0][0] * J[1][1] - J[0][1] * J[1][0]
kind = "regular" if abs(det) > 1e-3 else ("stretched" if math.cos(t2) > 0 else "folded")
print(f"t2 {t2:7.4f} det {round(det, 5) + 0.0:9.5f} l1*l2*sin(t2) {round(l1 * l2 * math.sin(t2), 5) + 0.0:9.5f} {kind}")
t2 1.2000 det 0.50330 l1*l2*sin(t2) 0.50330 regular
t2 0.3000 det 0.15958 l1*l2*sin(t2) 0.15958 regular
t2 0.0100 det 0.00540 l1*l2*sin(t2) 0.00540 regular
t2 0.0000 det 0.00000 l1*l2*sin(t2) 0.00000 stretched
t2 -0.0100 det -0.00540 l1*l2*sin(t2) -0.00540 regular
t2 3.1416 det 0.00000 l1*l2*sin(t2) 0.00000 folded
t2 6.2832 det 0.00000 l1*l2*sin(t2) 0.00000 stretched
The determinant from the matrix and the short formula agree. At \pi and 2\pi they come out as rounding dust (around 10^{-16}), never exactly 0, so test |\det J| \le a tolerance, never t2 == 0; then the sign of \cos t_2 names the kind, and it handles whole turns such as 2\pi for free. Near a singular pose the arm is weak in one direction, as the next section shows.
Common mistakes: comparing t2 with the tolerance instead of the determinant. t2 == 0 exactly. A strict < where the specification says "at most".
Practice: Where the Welder Loses a Direction
From a wanted hand velocity back to joint speeds
A controller usually knows the reverse: the hand must move at (v_x, v_y), so how fast should each joint turn? Solve J\,\dot q = v. A 2 \times 2 matrix has a short inverse:
Divide by the determinant only after checking it. Watch what happens as the arm straightens:
import math
l1, l2, t1 = 0.9, 0.6, 0.4
vx, vy = 0.0, 0.1 # the hand must go straight up at 0.1 m/s
def joint_rates(t2, tol=1e-9):
a = t1 + t2
A, B = -l1 * math.sin(t1) - l2 * math.sin(a), -l2 * math.sin(a)
Cc, D = l1 * math.cos(t1) + l2 * math.cos(a), l2 * math.cos(a)
det = A * D - B * Cc
if abs(det) <= tol:
return None # singular: refuse before dividing
return (D * vx - B * vy) / det, (-Cc * vx + A * vy) / det
for t2 in (1.5, 0.5, 0.1, 0.01, 0.0):
r = joint_rates(t2)
print(t2, None if r is None else (round(r[0], 4), round(r[1], 4)))
1.5 (0.1054, -0.1705)
0.5 (0.1815, -0.3169)
0.1 (0.5336, -1.1837)
0.01 (4.4291, -10.9195)
0.0 None
The same small hand velocity needs joint speeds that grow like 1/\sin t_2: at t_2 = 0.01 the elbow must spin at almost 11 rad/s, and stretched there is no answer at all. Real motors have a top speed. When a solution asks too much, scale both speeds by one factor so the faster one sits exactly at the limit, as Level 2 scaled wheel speeds to keep a curve: the hand keeps its direction and just moves more slowly. Clipping each joint on its own would bend the hand's path.
Common mistakes: dividing before checking the determinant. Using J^T where the inverse is needed. Clipping each speed separately. Scaling up a command that is already within the limit.
Practice: Steering the Icing Nozzle
Inverse kinematics by walking downhill
Level 3 found the angles that put the hand on a target with the law of cosines. An iterative method works for any arm. Make the error a function of the angles,
and run gradient descent on it, exactly as in the Machine Learning lesson. The robotics twist is the gradient. E depends on the angles only through the hand, so by the chain rule
Each joint moves in proportion to how much turning it would close the gap. The update is q \leftarrow q - \alpha J^T e, with both partial derivatives computed from the old angles before either changes. The loop needs a stopping rule: check the distance at the top of each round, and stop when it is within a tolerance or the budget of steps is spent. Joint angles may wander past \pm\pi along the way; wrap them once, 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
def descend(l1, l2, target, q, alpha, tol, budget):
t1, t2 = q
dists = []
for k in range(budget + 1):
a = t1 + t2
ex = l1 * math.cos(t1) + l2 * math.cos(a) - target[0] # e = hand - target
ey = l1 * math.sin(t1) + l2 * math.sin(a) - target[1]
dists.append(math.hypot(ex, ey))
if dists[-1] <= tol or k == budget: # stop before stepping
return wrap(t1), wrap(t2), k, dists
g1 = (-l1 * math.sin(t1) - l2 * math.sin(a)) * ex + (l1 * math.cos(t1) + l2 * math.cos(a)) * ey
g2 = -l2 * math.sin(a) * ex + l2 * math.cos(a) * ey # J^T e, both from the old angles
t1, t2 = t1 - alpha * g1, t2 - alpha * g2
t1, t2, k, dists = descend(0.9, 0.6, (0.3, 1.1), (0.0, 0.4), 0.5, 1e-6, 500)
print("reached in", k, "steps:", round(t1, 4), round(t2, 4), " distance", round(dists[-1], 8))
print("first distances", [round(d, 4) for d in dists[:6]])
t1, t2, k, dists = descend(0.9, 0.6, (2.0, 0.5), (0.0, 0.4), 0.5, 1e-6, 300)
print("out of reach:", k, "steps, distance", round(dists[-1], 4), " elbow", round(t2, 6), " shoulder", round(t1, 4), "vs", round(math.atan2(0.5, 2.0), 4))
plot(dists, logy=True, xlabel="step", ylabel="distance")
reached in 130 steps: 0.7549 1.4501 distance 9.6e-07
first distances [1.4419, 0.3893, 0.2221, 0.211, 0.2015, 0.192]
out of reach: 300 steps, distance 0.5616 elbow 0.0 shoulder 0.245 vs 0.245
The first step closes most of the gap, then progress slows to a steady crawl: with a fixed step size, gradient descent is quick far away and slow near the bottom, and a run that reaches 1e-6 takes 130 steps. The k returned counts the updates made, not the checks. The second target is 2.06 m away, beyond the arm's 1.5 m reach. The descent cannot get there, so it does the best it can: the arm stretches out (t_2 \to 0) and points straight at the target (t_1 = \operatorname{atan2}(0.5, 2.0)), 0.56 m short, until the budget runs out. Press Run to see the distance fall on a log scale.
Common mistakes: updating t1 and then using the new value for t2's derivative. Adding the gradient instead of subtracting it. Dropping the \tfrac12, which doubles every step. Stopping on E when the rule is about the distance. Counting the final check as an iteration. Returning unwrapped angles.
Practice: Downhill to the Light Switch, Descend Until the Loss Stops Falling
Damped least squares: bigger steps, kept finite
Gradient descent creeps. A bolder rule asks: which joint change \Delta q would close the gap if the arm were exactly linear, J\,\Delta q = e with now e = \text{target} - p, the way to go? The plain answer, J^{-1}e, explodes near a singular pose, as the joint speeds did. Damped least squares asks instead for the \Delta q that makes |J\Delta q - e|^2 + \lambda^2|\Delta q|^2 smallest, a good fit that does not cost huge joint changes. The answer is
JJ^T + \lambda^2 I is a 2 \times 2 symmetric matrix whose determinant is never 0 for \lambda > 0, so the inverse formula of the last section always works. With \lambda near 0 this is the full linear jump; with a large \lambda it behaves like a small transpose step.
import math
l1, l2, target, start, tol = 0.9, 0.6, (0.3, 1.1), (0.0, 0.4), 1e-6
def fk(t1, t2):
return l1 * math.cos(t1) + l2 * math.cos(t1 + t2), l1 * math.sin(t1) + l2 * math.sin(t1 + t2)
def step(t1, t2, rule, alpha=0.5, lam=0.05):
x, y = fk(t1, t2)
ex, ey = target[0] - x, target[1] - y # e = target - hand: the way to go
a = t1 + t2
A, B = -l1 * math.sin(t1) - l2 * math.sin(a), -l2 * math.sin(a)
C, D = l1 * math.cos(t1) + l2 * math.cos(a), l2 * math.cos(a)
if rule == "transpose":
return t1 + alpha * (A * ex + C * ey), t2 + alpha * (B * ex + D * ey)
m00, m01, m11 = A * A + B * B + lam ** 2, A * C + B * D, C * C + D * D + lam ** 2
det = m00 * m11 - m01 * m01 # (J J^T + lam^2 I) is never singular
ux, uy = (m11 * ex - m01 * ey) / det, (-m01 * ex + m00 * ey) / det
return t1 + A * ux + C * uy, t2 + B * ux + D * uy # J^T u
for rule in ("transpose", "dls"):
t1, t2 = start
dists = [math.dist(fk(t1, t2), target)]
while dists[-1] > tol and len(dists) < 1000:
t1, t2 = step(t1, t2, rule)
dists.append(math.dist(fk(t1, t2), target))
print(rule.ljust(9), len(dists) - 1, "steps; distances", [round(d, 4) for d in dists[:5]])
transpose 130 steps; distances [1.4419, 0.3893, 0.2221, 0.211, 0.2015]
dls 8 steps; distances [1.4419, 2.6113, 1.9927, 0.7804, 0.3227]
Same arm, target and start as the last section: 8 damped steps against 130 transpose steps. But look at the first damped step: it lands farther away (2.61 m) than it started. The rule aims straight at the target as if the arm were linear, and a real arm curves; once close, the linear picture is accurate and the steps converge fast. The sign convention differs between the two sections: here e points from the hand to the target and the step is added.
Common mistakes: using p - \text{target} as e here (the arm walks away). \lambda instead of \lambda^2. Running the second rule from where the first one ended instead of from the same start. Changing t_1 before computing t_2's change.
Practice: Two Ways to Close the Gap
Joint stops and spare joints
Two complications of real arms fit into the same descent.
Stops. Level 2's joints have limits (lo, hi). The simplest way to respect them is to clamp after every step: min(hi, max(lo, t)). While the solution lies inside the limits this changes nothing. When the descent heads for a pose beyond a stop, the joint pins there, the other joints do their best, and the steps shrink to nothing: the search has stalled. To detect it, compare each angle with its value before the step after clamping; measured before clamping, a joint pushing against its stop seems to move every step.
Spare joints. A three-link arm reaching a point in the plane has one joint more than it needs: it is redundant, and most targets can be reached in endlessly many poses. Descent then settles on whichever solution is downhill from the start. To prefer poses near the start pose s, add a penalty, E + \frac{\mu}{2}\sum_i (q_i - s_i)^2; its gradient adds \mu(q_i - s_i) to component i, a pull back towards the start that trades a little reach for a smaller change of pose.
import math
def fk(lengths, q):
x = y = h = 0.0
heads = []
for l, t in zip(lengths, q):
h += t
heads.append(h)
x, y = x + l * math.cos(h), y + l * math.sin(h)
return x, y, heads
def descend(lengths, target, q, alpha, iters, limits=None, mu=0.0):
start, q = list(q), list(q)
for _ in range(iters):
x, y, heads = fk(lengths, q)
ex, ey = x - target[0], y - target[1]
g = []
for i in range(len(q)): # column i: every link from i on
jx = -sum(l * math.sin(p) for l, p in zip(lengths[i:], heads[i:]))
jy = sum(l * math.cos(p) for l, p in zip(lengths[i:], heads[i:]))
g.append(jx * ex + jy * ey + mu * (q[i] - start[i]))
q = [t - alpha * gi for t, gi in zip(q, g)]
if limits:
q = [min(hi, max(lo, t)) for t, (lo, hi) in zip(q, limits)] # clamp after the step
x, y, _ = fk(lengths, q)
return [round(t, 4) for t in q], round(math.dist((x, y), target), 6)
L3, target = [0.7, 0.5, 0.3], (0.6, 0.9)
print("from (0, 0, 0) ", descend(L3, target, [0.0, 0.0, 0.0], 0.3, 400))
print("from (2, -1, -1) ", descend(L3, target, [2.0, -1.0, -1.0], 0.3, 400))
print("with a pull, mu=0.1", descend(L3, target, [0.0, 0.0, 0.0], 0.3, 400, mu=0.1))
print("elbow stop at -0.3 ", descend([0.9, 0.6], (1.1, -0.2), [0.5, 0.0], 0.3, 400, limits=[(-3, 3), (-0.3, 3)]))
from (0, 0, 0) ([0.2125, 1.2179, 0.6804], 0.0)
from (2, -1, -1) ([1.6941, -1.0539, -0.9567], 0.0)
with a pull, mu=0.1 ([0.3792, 0.9136, 0.4639], 0.188647)
elbow stop at -0.3 ([-0.0601, -0.3], 0.3658)
The three-link arm reaches the same point from two starts in two quite different poses, one bent one way and one the other. With the pull, it stops 0.19 m short, nearer its start pose. The last line is the trap of clamping: the two-link target is reachable with the elbow at about +1.5, well inside its stops, but from this start the descent heads for the elbow-down pose, hits the stop at -0.3 and stalls 0.37 m short. A stall is a reason to try another start, not proof that the target is out of reach.
Common mistakes: clamping only at the end. Measuring a joint's change before clamping. Building the penalty from the previous step's angles instead of the start. Changing the start list in place (the penalty needs it).
Practice: The Elbow Hits Its Stop, Three Joints, One Bookshelf Corner
How encoder errors blur the hand
Joint encoders are not perfect: each angle is off by a small error, e_1 with standard deviation s_1 and an independent e_2 with s_2. For small errors the hand moves linearly with them, \delta p \approx J\,\delta q, so each hand coordinate is a weighted sum of two independent errors. Level 3 propagated sensor errors the same way: variances add with squared weights, and the covariance (from Statistics) uses the products of the two rows' weights:
In matrix form, \Sigma_p = J\,\Sigma_q J^T with \Sigma_q = \operatorname{diag}(s_1^2, s_2^2). A simulation with random errors checks the formula:
import math, random
l1, l2, t1, t2 = 0.9, 0.6, 0.7, 0.8
s1, s2 = 0.004, 0.01 # encoder standard deviations, radians
a = t1 + t2
J = [[-l1 * math.sin(t1) - l2 * math.sin(a), -l2 * math.sin(a)],
[l1 * math.cos(t1) + l2 * math.cos(a), l2 * math.cos(a)]]
v1, v2 = s1 ** 2, s2 ** 2
var_x = J[0][0] ** 2 * v1 + J[0][1] ** 2 * v2
var_y = J[1][0] ** 2 * v1 + J[1][1] ** 2 * v2
cov = J[0][0] * J[1][0] * v1 + J[0][1] * J[1][1] * v2
print("predicted:", round(math.sqrt(var_x) * 1000, 3), round(math.sqrt(var_y) * 1000, 3), "mm, cov", round(cov * 1e6, 3), "mm^2")
def fk(t1, t2):
return l1 * math.cos(t1) + l2 * math.cos(t1 + t2), l1 * math.sin(t1) + l2 * math.sin(t1 + t2)
rng = random.Random(4)
pts = [fk(t1 + rng.gauss(0, s1), t2 + rng.gauss(0, s2)) for _ in range(100000)]
mx, my = sum(p[0] for p in pts) / len(pts), sum(p[1] for p in pts) / len(pts)
sx = math.sqrt(sum((p[0] - mx) ** 2 for p in pts) / len(pts))
sy = math.sqrt(sum((p[1] - my) ** 2 for p in pts) / len(pts))
cxy = sum((p[0] - mx) * (p[1] - my) for p in pts) / len(pts)
print("simulated:", round(sx * 1000, 3), round(sy * 1000, 3), "mm, cov", round(cxy * 1e6, 3), "mm^2")
plot([p[0] for p in pts[:2000]], [p[1] for p in pts[:2000]], kind="scatter", xlabel="x (m)", ylabel="y (m)")
predicted: 7.618 2.954 mm, cov -16.318 mm^2
simulated: 7.562 2.943 mm, cov -16.079 mm^2
The linear prediction and 100,000 simulated arms agree to within 2%. The blur is not round: press Run to see a tilted ellipse, long in x and short in y at this pose, and the negative covariance says that when x comes out too large, y tends to come out too small. Adding the standard deviations instead (|J_{00}|s_1 + |J_{01}|s_2) gives a worst case, not the spread of independent errors. The slopes are per radian, so the errors must be in radians too.
Common mistakes: adding standard deviations. Forgetting to square the Jacobian entries. Dropping the covariance's sign. Errors in degrees.
Practice: Wobbly Encoders, Blurry Gripper, Measurements That Carry Their Error
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.
- Was That a Door?
- Round the Greenhouse Rail
- Lost on the Ring Corridor
- How the Hand Answers Each Joint
- Steering the Icing Nozzle
- Downhill to the Light Switch
Checklist
You can now:
- Keep a belief over the cells of a map and update it with a reading: multiply by the likelihoods, normalise, and handle a reading no cell explains.
- Move a belief on a ring and in a corridor with end walls, adding each cell's probability into its landing cells.
- Measure a belief's entropy in bits and find its best guess with a stated tie-break.
- Run the histogram filter (move, then sense) from a uniform start, on a line or on a grid.
- Build a likelihood from a predicted range and Gaussian noise.
- Recover from kidnapping by mixing in a little uniform belief, compare maps by the evidence, and choose the move with the lowest expected entropy.
- Write an arm's Jacobian, read its columns as motions, and check it by central differences.
- Turn joint speeds into a hand velocity and back, and spot singular poses by the determinant.
- Solve inverse kinematics by gradient descent and by damped least squares, with stopping rules, joint stops and a pull towards the start pose.
- Propagate joint-angle errors to the hand with \Sigma_p = J\,\Sigma_q J^T.
Further reading: Wikipedia: Monte Carlo localization (the particle version of this level's filter) and Wikipedia: Inverse kinematics.