Twists and the exponential
A steady command is a twist, a turn about one point. The exact odometry step, the matrix exponential behind it, and the logarithm that plans one smooth arc.
Do this lesson in the simulatorPrint it: worksheet (PDF) · answers (PDF) · as a web page
In U3.2 the odometry update rotated each step's movement by the heading halfway through the step, and that one change made it noticeably better. This lesson shows why: there is an exact answer for a step, and the midpoint rule is a good approximation of it. The exact answer comes from Lynch and Park's Modern Robotics (chapter 3), written here for a robot on a flat mat.
A command held steady is a twist
Send drive(60, 40, 25) and hold it. Once the lag has died away, the robot has a steady forward speed, a steady sideways speed and a steady turn rate, all in its own body frame. Those three numbers together are a twist:
V = (w, vx, vy) w in degrees a second (clockwise here), vx right and vy forward in cm/s
Lynch and Park write the turn rate first, and so will this lesson. The twist is measured in the body frame, so it does not change as the robot turns: the robot keeps doing the same thing relative to itself, and the world sees it go round.
from bugbot import *
import math
connect()
drive(60, 40, 25)
wait(1.0) # let the lag settle, so the twist is steady
vx, vy = flow() # cm/s, in the body frame: right, forward
w = imu()[1] # deg/s, clockwise
print("twist: w =", round(w, 1), "deg/s, vx =", round(vx, 1), "vy =", round(vy, 1), "cm/s")
wr = math.radians(w)
cx, cy = vy / wr, -vx / wr # the centre, in the body frame
r = math.hypot(cx, cy)
side = "ahead" if cy >= 0 else "behind"
print("centre", round(cx, 1), "cm right,", round(abs(cy), 1), "cm", side + "; radius", round(r, 1), "cm")
# the centre on the mat: rotate it into the world frame and add the position
x, y = position()
h = math.radians(heading())
wx = x + cx * math.cos(h) + cy * math.sin(h)
wy = y - cx * math.sin(h) + cy * math.cos(h)
draw("centre", [(wx, wy)], "red", "dots", 3)
draw("circle", [(wx + r * math.cos(k * math.pi / 30), wy + r * math.sin(k * math.pi / 30)) for k in range(61)], "red", "line")
for i in range(40):
x, y = position()
if i % 10 == 0:
print("t =", round(1 + i * 0.2, 1), "s: distance from the centre", round(math.hypot(x - wx, y - wy), 1), "cm")
wait(0.2)
stop()
Every steady twist is a turn about one point
Run it and watch the mat. The red dot is worked out from one reading of the twist, before the robot has gone anywhere, and the robot then circles it: the distance stays at 22 to 24 cm for the next eight seconds. A steady twist is a rotation about a fixed point, the centre of rotation, and the twist tells you where that point is:
centre, in the body frame: cx = vy / w, cy = -vx / w (w in radians a second)
radius: |v| / |w|
This is the planar case of the screw idea that runs through Lynch and Park: any rigid motion is a rotation about some axis combined with a slide along it. On a flat mat there is nothing to slide along, so every motion is a turn about a point. The one exception is w = 0, a pure slide with no turn, where the centre has gone off to infinity and the circle has become a straight line.
It also says what happens when you mix sideways and turning. Crabbing sideways while turning does not trace a sideways line: it traces a circle whose centre is ahead of or behind the robot. Forward and turning puts the centre to the side; sideways and turning puts it in front or behind. The first cell does both at once, which is why its centre is 20 cm to the right and 11 cm behind.
The exact step
Odometry reads the twist once per tick and assumes it held for the whole tick. Given that assumption, the movement over the tick is not a guess: it is an exact arc of that circle. Adding up the world velocity over the tick, with the heading turning at a steady rate, gives it in closed form:
def exact_step(x, y, h, vx, vy, w, dt):
"""One tick of odometry, exact for a twist that is steady over the tick.
h and w in degrees (clockwise), vx and vy in cm/s in the body frame."""
a, wr = math.radians(h), math.radians(w)
if abs(wr) < 1e-9: # no turn: a straight slide
C, S = math.cos(a) * dt, math.sin(a) * dt
else: # the integrals of cos and sin of the heading over the tick
C = (math.sin(a + wr * dt) - math.sin(a)) / wr
S = (math.cos(a) - math.cos(a + wr * dt)) / wr
return x + vx * C + vy * S, y - vx * S + vy * C, h + w * dt
Compare it with the Euler step (the old-heading update in U3.2), which is the same thing with C = cos(h) * dt and S = sin(h) * dt: the heading frozen at the start of the tick. For a short tick the two agree. As the tick grows, Euler walks straight off the circle along the tangent, and every tick adds another gap.
The same thing, as Lynch and Park write it
The book keeps the pose as a 3 by 3 matrix T in SE(2), a rotation and a position packed together, and the body twist as a 3 by 3 matrix [V] in se(2). Its conventions are not this robot's, so translate first. The book measures its angle θ anticlockwise from the mat's x axis, puts the body x axis forward and the body y axis to the left, and uses radians:
θ = 90° − h h is this robot's heading, clockwise from +y
ω = −w in radians a second; the book counts anticlockwise as positive
v1 = vy, v2 = −vx body x forward, body y to the left
Then the pose, the twist and the step are:
T = [ cos θ −sin θ x ] [V] = [ 0 −ω v1 ]
[ sin θ cos θ y ] [ ω 0 v2 ]
[ 0 0 1 ] [ 0 0 0 ]
T(t + dt) = T(t) · exp([V] dt)
exp([V] dt) = [ cos ωdt −sin ωdt (v1 sin ωdt − v2 (1 − cos ωdt)) / ω ]
[ sin ωdt cos ωdt (v1 (1 − cos ωdt) + v2 sin ωdt) / ω ]
[ 0 0 1 ]
The product is multiplied on the right because the twist is in the body frame. Multiply it out, substitute the three lines of translation, and you get exact_step term for term: it is exp([V] dt) for this robot's frame, with nothing lost. The geometry does not change between the two conventions; only the labels and the sign of the turn do. Rather than trust the algebra, check it: the cell below builds [V], takes its exponential straight from the power series I + M + M²/2! + ..., and compares the result with exact_step.
import math
def matmul(A, B):
return [[sum(A[i][k] * B[k][j] for k in range(3)) for j in range(3)] for i in range(3)]
def expm(M, terms=30):
"""exp(M) straight from its power series: I + M + M^2/2! + M^3/3! + ..."""
E = [[float(i == j) for j in range(3)] for i in range(3)]
P = [row[:] for row in E]
for n in range(1, terms):
P = [[v / n for v in row] for row in matmul(P, M)]
E = [[E[i][j] + P[i][j] for j in range(3)] for i in range(3)]
return E
def exact_step(x, y, h, vx, vy, w, dt):
a, wr = math.radians(h), math.radians(w)
C = (math.sin(a + wr * dt) - math.sin(a)) / wr
S = (math.cos(a) - math.cos(a + wr * dt)) / wr
return x + vx * C + vy * S, y - vx * S + vy * C, h + w * dt
# one second of the next cell's twist, from (10, 20) facing 30
x, y, h = 10.0, 20.0, 30.0
vx, vy, w, dt = 4.5, 14.0, 48.0, 1.0
# into the book's frame: angle anticlockwise from the mat's x axis, body x forward, body y left, rad/s
th, om, v1, v2 = math.radians(90 - h), -math.radians(w), vy, -vx
T = [[math.cos(th), -math.sin(th), x], [math.sin(th), math.cos(th), y], [0.0, 0.0, 1.0]]
V = [[0.0, -om, v1], [om, 0.0, v2], [0.0, 0.0, 0.0]]
T2 = matmul(T, expm([[v * dt for v in row] for row in V]))
h2 = (90 - math.degrees(math.atan2(T2[1][0], T2[0][0]))) % 360
print("T exp([V] dt): x", round(T2[0][2], 6), " y", round(T2[1][2], 6), " heading", round(h2, 6))
xe, ye, he = exact_step(x, y, h, vx, vy, w, dt)
print("exact_step: x", round(xe, 6), " y", round(ye, 6), " heading", round(he % 360, 6))
Both lines give x 23.566275, y 24.455409 and heading 78, and scipy.linalg.expm on the same matrix agrees to every digit shown. The series is a slow way to get there, a few dozen 3 by 3 products; the closed form needs one sine and one cosine, which is why odometry uses it.
How much it matters
Here is one run, logged once and integrated three ways, at three tick lengths. The robot drives drive(70, 30, 40) for three seconds. At the design's full-scale speeds that is 48 degrees a second, 144 degrees in three seconds; this robot's turn is a little strong, and it turns 151. For each tick the cell prints two lines: each method's error against the true end point, and how far Euler and midpoint land from the exact step fed the same readings. The second line has no sensor in it at all, so it is the integration error alone.
from bugbot import *
import math
connect()
def euler(p, vx, vy, w, dt):
a = math.radians(p[2])
return p[0] + (vx * math.cos(a) + vy * math.sin(a)) * dt, p[1] + (-vx * math.sin(a) + vy * math.cos(a)) * dt, p[2] + w * dt
def midpoint(p, vx, vy, w, dt):
a = math.radians(p[2] + 0.5 * w * dt)
return p[0] + (vx * math.cos(a) + vy * math.sin(a)) * dt, p[1] + (-vx * math.sin(a) + vy * math.cos(a)) * dt, p[2] + w * dt
def exact(p, vx, vy, w, dt):
a, wr = math.radians(p[2]), math.radians(w)
C = (math.sin(a + wr * dt) - math.sin(a)) / wr
S = (math.cos(a) - math.cos(a + wr * dt)) / wr
return p[0] + vx * C + vy * S, p[1] - vx * S + vy * C, p[2] + w * dt
drive(70, 30, 40)
wait(0.8)
x0, y0 = position()
h0 = heading()
log = [] # one reading every 0.1 s: (vx, vy, w)
for i in range(30):
vx, vy = flow()
log.append((vx, vy, imu()[1]))
wait(0.1)
x, y = position()
turned = (heading() - h0) % 360
stop()
print("truth: moved", round(x - x0, 1), round(y - y0, 1), "and turned", round(turned), "degrees")
for dt in (0.1, 0.5, 1.0):
k = round(dt / 0.1) # use every k-th reading, as a slower loop would
end = {}
for name, step in (("euler", euler), ("midpoint", midpoint), ("exact", exact)):
p = (0.0, 0.0, h0)
for vx, vy, w in log[::k]:
p = step(p, vx, vy, w, dt)
end[name] = p
err = {n: math.hypot(p[0] - (x - x0), p[1] - (y - y0)) for n, p in end.items()}
gap = {n: math.hypot(p[0] - end["exact"][0], p[1] - end["exact"][1]) for n, p in end.items()}
print("tick", dt, "s error: euler", round(err["euler"], 2), " midpoint", round(err["midpoint"], 2), " exact", round(err["exact"], 2), "cm")
print(" off exact, same readings: euler", round(gap["euler"], 2), " midpoint", round(gap["midpoint"], 2), "cm")
Three things to take from it.
- Euler's error grows with the tick. Its integration error alone is 1.37, 6.64 and 13.46 cm at the three ticks, roughly in proportion to the tick. At a tenth of a second that is about the size of the sensor's own error, so against the truth it hardly shows; at a whole second it has lost over ten centimetres in three seconds of driving.
- Midpoint is wrong only in length, never in direction. Over a tick the exact step is a chord of the arc, and the chord points exactly along the middle heading. What midpoint gets wrong is how long it is: it moves
|v| dt, the length of the arc, where the chord is shorter by the factorsin(θ/2) / (θ/2), withθ = w dtin radians. That is1 − θ²/24to first order, so midpoint is too long by aboutθ²/24of each step. Here the robot turns about 50 degrees a second: at one-second ticksθis 0.88 rad and each step of about 15 cm is 3 per cent too long, half a centimetre, and three such steps (pointing different ways round the arc) come to the 1.0 cm in the output. Halve the tick and the total drops by about four, to 0.25 cm; at a tenth of a second each step is 0.03 per cent too long, and the gap is 0.01 cm. - Against the truth, midpoint and exact cannot be told apart. Both are within about a centimetre of the true end point at every tick, and which of them is closer changes from run to run: at 0.5 s midpoint is closer (0.05 against 0.19 cm), at 1 s exact is (0.48 against 1.2 cm). What is left is not the integration any more. It is the twist not being steady across the tick (the reading is one instant, and the robot's speed wanders) and the noise on the sensor, and it is as big as the gap between the two methods, so it decides the order. The exponential removes one error completely and leaves the others exactly where they were, which is what makes it useful: once the integration is exact, any error left is telling you about the robot.
Going backwards: the logarithm
The exponential answers "I hold this twist for this long: where do I end up?". The matrix logarithm answers the reverse: "I want to end up there, facing that way: what single steady twist gets me there in this time?". For a turn of TURN degrees in T seconds, ending DX right and DY forward of where it started (in the start's body frame):
w = TURN / T
C = sin(w T) / w, S = (1 - cos(w T)) / w (w in radians a second)
vx = (C DX - S DY) / (C² + S²)
vy = (S DX + C DY) / (C² + S²)
That is the exact step solved for the twist. It is a real planning tool: one steady command, one smooth arc, no stopping to turn. On a holonomic robot almost any end pose has an arc like this, because the sideways term lets the circle be placed anywhere. The "almost" is worth knowing before you use it:
- No turn. With
TURN = 0,wis zero andCandSdivide by it. The limit isC = T,S = 0, which givesvx = DX / Tandvy = DY / T: a straight slide, the centre gone to infinity. Code has to catch that case separately, asexact_stepdoes. - A whole number of turns. Facing 120 is also facing 480 or −240, and each of those is a different twist: a tighter circle that loops round before it arrives, or one that turns the other way. The logarithm has one answer for every
TURN + 360k, and you choose which by theTURNyou give it. Usually that is the smallest. At exactlyTURN = ±360(or any non-zero multiple),CandSare both zero: a full circle brings the robot back to where it started whatever the twist, so it cannot take it anywhere else. - The dead band. Each part of the command must be zero or at least 15 percent, or that motor does nothing and the arc is wrong. A longer
Tmakes every part smaller, so a slow, gentle arc can drop a small part into the dead band. - The speed limits. No part can be more than 100 percent. A shorter
Tmakes every part bigger. SoThas a window: long enough to keep every part under 100, short enough to keep every non-zero part over 15. ChangingTscales every part by the same factor, so if the biggest part, in percent, is more than 100 / 15 (about 6.7) times the smallest non-zero one, noTfits and one arc cannot do it on this robot.
Task: land on the spot in one arc
The green square is 25 cm to the robot's right and 30 cm ahead of it. The robot must arrive in it facing 120 degrees (a third of a turn clockwise), using one steady drive() command held for T = 3 seconds, then stop(). Work out the twist with the logarithm, print it as a line starting twist: with w = ..., vx = ... and vy = ... (deg/s and cm/s), turn it into a command with the full-scale speeds below, and drive it. The printed twist is checked against the logarithm, to a tenth.
from bugbot import *
import math
connect()
V_MAX, V_LAT, W_MAX = 20.0, 15.0, 120.0 # the design's full-scale speeds: cm/s forward, cm/s sideways, deg/s
DX, DY, TURN = 25.0, 30.0, 120.0 # where to end up, in the start's body frame, and how far to turn
T = 3.0 # how long to hold the command, s
Challenges
- The robot lands a little short of the arc, because of the quarter second of lag at the start. Hold the command for a little longer to make up for it. How much longer, and does the landing get better? (Watch the heading as well as the position.)
- Finite motions do not commute. Twists themselves add like vectors, but the motions they produce, their exponentials, do not:
exp(A) exp(B)is notexp(B) exp(A)in general. Turn 90 degrees and then drive 20 cm, or drive 20 cm and then turn 90 degrees: work out both end poses withexact_step, then drive both. Why are they different, and which pairs of moves give the same answer in either order? - Replace the Euler step in your U3.2 odometry with
exact_stepand repeat the long lap from the project. How much of the final error was the integration?