Forward kinematics
From the command to the body velocity to the path over the ground.
Do this lesson in the simulatorForward kinematics answers: given the commands, where does the robot end up? It is the model of the machine, and it runs in three stages.
command (percent) -> body velocity (cm/s) -> world velocity -> position over time
Stage one: command to body velocity
From the data sheet in U1, each axis has its own full-scale speed and its own dead band:
vx_body = (lat / 100) * V_LAT if |lat| is above the dead band, else 0
vy_body = (fwd / 100) * V_MAX
omega = (rot / 100) * W_MAX
with V_MAX about 20 cm/s, V_LAT about 15 cm/s and W_MAX about 120 deg/s on this robot. Yours are the ones you measured.
Stage two: body to world
That is the rotation matrix from U2.2, with the heading you have now:
wx = vx_body*cos(h) + vy_body*sin(h)
wy = -vx_body*sin(h) + vy_body*cos(h)
Stage three: integrate
Position is the integral of velocity. In code that is a sum over small steps:
x += wx * dt
y += wy * dt
h += omega * dt
If the robot is turning, the heading changes inside the step too, so the path over that step is an arc rather than a straight line. For a step of 0.02 s this hardly matters; for a step of 0.5 s it matters a great deal. That is the difference between Euler integration and the exact arc, and it is the same choice you will meet again in U6.
Predicting a run
from bugbot import *
import math
connect()
V_MAX = 20.0
fwd, seconds = 70, 3.0
h = math.radians(heading())
v = (fwd / 100) * V_MAX
print("prediction:", round(v * math.sin(h) * seconds, 1), round(v * math.cos(h) * seconds, 1))
forward(fwd)
wait(seconds)
stop()
wait(0.6)
print("actual: ", position())
The prediction is close and it is not exact. The gap is everything the model leaves out: the lag at the start, the coasting at the end, the sideways leak, the gain that is not quite what the data sheet says. A model is a tool, not a promise, and knowing its error is part of having it.
Task: predict where it stops
Before driving, print where the forward kinematics say the robot will end up, as predicted x: and predicted y:. Then drive it and see. The robot does not start facing along an axis, so the rotation matters.
from bugbot import *
import math
connect()
# predict first, then drive
Challenges
- Predict a run with a turn in it by stepping the model at 0.1 s and updating the heading each step.
- Compare a prediction with a 0.02 s step against one with a 0.5 s step, for a robot that is turning.
- Improve the prediction by allowing for the quarter second of lag at the start. How much of the error does that explain?