Forward kinematics

From the command to the body velocity to the path over the ground.

U2.4Kinematics and framesUniversity25 min

Do this lesson in the simulator

Print it: worksheet (PDF) · answers (PDF) · as a web page

Guide: Forward and inverse kinematics for omni wheel robots

Forward 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
vy_body = (fwd / 100) * V_MAX
omega   = (rot / 100) * W_MAX

Each line is zero when its own command is below the dead band, which is 15 percent on all three axes.

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.

Body speed against command for the forward and sideways axes, each with its own full scale and a dead band02040608010005101520command, percentspeed, cm/sdead band: nothing below 15forward20 at 100sideways15 at 100
Stage one for the two sliding axes. The same command gives 1.33 times as much speed forwards as sideways, and nothing at all below 15 percent. Turning works the same way, up to W_MAX = 120 degrees a second.

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.

One half-second step of a turning robot: Euler goes straight, the robot really follows an arcEuler: straight onat the start headingexact: an arc,turning 30° as it goes1.8 cm apart
drive(70, 0, 50) for one step of 0.5 s: 14 cm/s forwards and 60 degrees a second clockwise. Euler moves 7 cm along the heading it had at the start. The robot turns 30 degrees during the step, so it really ends on the arc, 1.8 cm away. Each step adds a gap like this.

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())

Run this in the simulator

The prediction is close and it is not exact: (21.0, 36.4) against (18.6, 33.2), 42 cm of travel predicted and 38 driven. 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. Here the lag cost about 3 cm and the coasting gave nearly as much back, so most of the gap is the gain: this robot does 13.0 cm/s at 70, not 14. A model is a tool, not a promise, and knowing its error is part of having it.

The predicted end of the run against where the robot really stopped, both on the line it was facingxypredicted (21, 36.4)actual (18.6, 33.2)heading 30
The run from the cell above. The robot went the way the model said, but not as far: of the gap, 4.0 cm is along the line it was facing and only 0.5 cm is to the side. Lag, coasting and a gain that is off all change how far it goes along that line; a mistake in the rotation would have pushed the end out to the side.

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

  1. Predict a run with a turn in it by stepping the model at 0.1 s and updating the heading each step.
  2. Compare a prediction with a 0.02 s step against one with a 0.5 s step, for a robot that is turning.
  3. Improve the prediction by allowing for the quarter second of lag at the start. How much of the error does that explain?