Go to a point

The first useful controller: a vector to the target, straight through the inverse kinematics.

U2.6Kinematics and framesUniversity25 min

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

Guide: Forward and inverse kinematics for omni wheel robots

With the inverse kinematics in hand, the first genuinely useful controller is short.

each tick:
    vector to the target, in the world
    speed proportional to how far away it is, capped
    that velocity, through the inverse kinematics
    stop when close enough

from bugbot import *
import math
connect()

V_MAX, V_LAT = 20.0, 15.0
TX, TY = 60.0, 70.0          # cm from where the robot started

for tick in range(300):
    x, y = position()
    dx, dy = TX - x, TY - y
    gap = math.hypot(dx, dy)
    if gap < 4:
        break
    speed = min(14.0, max(4.0, 0.5 * gap))   # proportional, with a ceiling and a floor
    wx, wy = speed * dx / gap, speed * dy / gap
    a = math.radians(heading())
    drive(100 * (wx * math.sin(a) + wy * math.cos(a)) / V_MAX,
          100 * (wx * math.cos(a) - wy * math.sin(a)) / V_LAT, 0)
    wait(0.1)
stop()
print("arrived at", position())

Run this in the simulator

What is going on

0.5 * gap is proportional control on distance, which you will meet properly in U5. Far away, it drives at its ceiling of 14 cm/s. Close in, it eases off, so the robot does not overshoot and hunt about the target. It prints arrived at (57.1, 67.7), 3.7 cm from the target, after 8.8 seconds.

The floor of 4 cm/s is the dead band from U1.2. Take it out and try: 6 cm from the target the controller asks for 3 cm/s, which split between two axes is under 15 percent on each, and the robot stops dead at (55.1, 66.2) and sits there until the loop runs out thirty seconds later. A proportional controller on a real actuator needs to know where the actuator stops responding.

Dividing dx, dy by gap makes a unit vector: direction without magnitude. Multiplying by speed puts the magnitude back. Doing it in one step, speed * dx, is a common slip that makes the gain depend on how far away the target is, so the robot behaves differently at the start of a long run than a short one.

Where the position came from

This controller reads position(), and U1.3 said that is the lab's overhead camera, not the robot's. It is fine while the subject is kinematics. From U6 onwards, position() is replaced by an estimate the robot works out for itself, and this same loop keeps running on top of it. That is why it is worth writing the loop cleanly now: everything later plugs into it.

Stopping

gap < 4 sounds arbitrary and is not. It has to be bigger than the robot's coasting distance from the speed it arrives at, or the robot will sail past, turn round, come back and hunt. Measure your coast, then pick the threshold. Alternatively, ramp the speed down far enough that it arrives slowly, which is gentler but slower.

Task: go to a point

Drive to world (140, 150) and stop within 10 cm of it. The robot starts at (60, 60) facing the wrong way, and position() counts from where it started.

from bugbot import *
import math
connect()

V_MAX, V_LAT = 20.0, 15.0
TX, TY = 80.0, 90.0        # the target, in cm from the start

Challenges

1. Add a heading term so the robot also finishes facing a chosen direction, without changing the path.
2. Feed it a list of points and drive them in order, without stopping at each one.
3. Replace position() with odometry() and run it again. How close does it get now, and how much worse does it get on a longer run?