Inverse kinematics

The useful direction: a velocity in the world, a heading, and the command that produces it.

U2.5Kinematics and framesUniversity30 min

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

Guide: Forward and inverse kinematics for omni wheel robots

Forward kinematics is the model. Inverse kinematics is the useful one: I want to move like this; what do I send?

The problem

Given a world velocity (wx, wy) and a turn rate, and knowing the heading h, find drive(fwd, lat, rot).

Two steps, both already in hand. Rotate the world velocity into the body frame, then divide by each axis's full scale.

vx_body =  wx*cos(h) - wy*sin(h)
vy_body =  wx*sin(h) + wy*cos(h)

lat = 100 * vx_body / V_LAT
fwd = 100 * vy_body / V_MAX
rot = 100 * omega   / W_MAX

That is the whole of it, for this drive. On a four-wheel mecanum robot the second step becomes a 4 by 3 matrix, one row per wheel, and on a differential drive it becomes two equations that cannot satisfy an arbitrary sideways request at all. The structure is the same everywhere: rotate into the machine's frame, then map to whatever the machine's actuators are.

In code

from bugbot import *
import math
connect()

V_MAX, V_LAT, W_MAX = 20.0, 15.0, 120.0

def world_drive(wx, wy, spin=0.0):
    """Ask for a velocity in the world, whatever way the robot happens to be facing."""
    a = math.radians(heading())
    vx = wx * math.cos(a) - wy * math.sin(a)
    vy = wx * math.sin(a) + wy * math.cos(a)
    drive(100 * vy / V_MAX, 100 * vx / V_LAT, 100 * spin / W_MAX)

# due north at 12 cm/s for three seconds, facing east the whole time
for tick in range(30):
    world_drive(0, 12)
    wait(0.1)
stop()
print("ended", position(), "still facing", round(heading()))

Run this in the simulator

world_drive() is worth keeping. Every controller from here to the end of the course produces a velocity it wants in the world, and this is the function that turns that wish into something the motors understand.

Saturation

Ask for 30 cm/s forward and 10 cm/s sideways and the arithmetic gives fwd = 150 and lat = 67. The robot clips the first to 100 and leaves the second alone. Now you are getting two thirds of what you asked for on that axis and all of it on the other, so the robot travels in the wrong direction as well as too slowly.

The fix is to scale the whole command down together, keeping its direction:

worst = max(abs(fwd), abs(lat), abs(rot), 100.0)
fwd, lat, rot = fwd * 100 / worst, lat * 100 / worst, rot * 100 / worst

Direction preserved, magnitude reduced. This is the standard treatment, and the same idea appears again in U5 as integral windup: when the actuator cannot do what the controller asks, the controller has to be told.

Task: north while facing east

The robot faces east. Get it into the green zone to the north without turning: heading must still be within 15 degrees of 90 at the end. Use the inverse kinematics.

from bugbot import *
import math
connect()

V_MAX, V_LAT = 20.0, 15.0

Challenges

1. Add saturation handling to world_drive() and ask for 40 cm/s. Does the robot still go in the right direction?
2. Ask for a world velocity and a spin at the same time, and confirm the world path is unaffected by the spinning.
3. Write the inverse kinematics for a differential drive (left and right wheel speeds). Which requests become impossible?