Programming for Robotics 1, Chapter 5: Challenge 2 solution (the track width from spins on the spot)

Idea: when the wheels turn in opposite directions by the same distance, odometry computes the angle as (distance difference) / (track it believes), and the robot really turns by (distance difference) / (true track). So odometry's angle / true angle = true track / believed track, and the corrected track is the believed track x odometry's angle / true angle. The true angle is measured the way you would measure it on the floor, by adding up every change of heading (here the simulator's heading each pass, wrapped, so that turns of more than 180 degrees are counted whole). A command of one turn and a command of four turns give the same ratio in theory; the longer test divides the small unavoidable errors (where the move stops, how the angle is read) by a larger angle. The last step repeats the test with the corrected geometry.

CODE:
TRUE_TRACK = 0.158                                       # what the robot really has; the software believes 0.150
geom = Geometry.from_config(Config)
print("the software's geometry: %s" % geom)


def spin(geometry, turns):
    """Turn on the spot by odometry. Returns (odometry's angle, the true angle), both in degrees (the true angle adds up every real heading change)."""
    sim = Simulator(World(8.0, 8.0), params=RobotParams(track_width_m=TRUE_TRACK), seed=1, pose=(4.0, 4.0, 0.0))
    base = DriveBase(sim, geometry)
    base.start(TurnAngle(turns * 2 * math.pi))
    loop = FixedRateLoop(0.02, sim.now_s, sim.advance)
    state = {"last": sim.pose()[2], "total": 0.0}

    def step(dt_s):
        base.update(dt_s)
        state["total"] += wrap_pi(sim.pose()[2] - state["last"])
        state["last"] = sim.pose()[2]
    for _ in range(int(120 / 0.02)):
        if base.move is None:
            break
        loop.run(step, 1)
    loop.run(step, 40)                                   # 0.8 s to come to rest
    return math.degrees(base.odom.turned_rad), math.degrees(state["total"])


print("(a) the estimate from one turn and from four turns: track = old track x odometry's angle / true angle")
estimates = {}
for turns in (1, 4):
    odo, true = spin(geom, turns)
    estimates[turns] = geom.track_m * odo / true
    print("  %d turn%s: odometry %7.1f deg, true %7.1f deg  ->  track %.2f mm  (error %+.2f mm)"
          % (turns, " " if turns == 1 else "s", odo, true, estimates[turns] * 1000, (estimates[turns] - TRUE_TRACK) * 1000))
print("(b) the same four-turn test with the corrected geometry (track from the four turns):")
fixed = geom.scaled(track=estimates[4] / geom.track_m)
odo, true = spin(fixed, 4)
print("  %s" % fixed)
print("  odometry %.1f deg, true %.1f deg: the robot now really turns %.2f turns for a command of 4" % (odo, true, true / 360))
print("(c) why four turns: the angle error left by stopping (the robot coasts a little, the move ends within a tolerance) is about the same in degrees")
print("    for one turn or for four, but it is divided by 4 times as large an angle: relative error of the track about %.3f %% (one) against %.3f %% (four)"
      % (100 * abs(estimates[1] - TRUE_TRACK) / TRUE_TRACK, 100 * abs(estimates[4] - TRUE_TRACK) / TRUE_TRACK))
END CODE
OUTPUT:
the software's geometry: Geometry(mpt_left=0.340339 mm, mpt_right=0.340339 mm, track=150.00 mm)
(a) the estimate from one turn and from four turns: track = old track x odometry's angle / true angle
  1 turn : odometry   360.2 deg, true   342.0 deg  ->  track 158.02 mm  (error +0.02 mm)
  4 turns: odometry  1440.3 deg, true  1367.3 deg  ->  track 158.00 mm  (error +0.00 mm)
(b) the same four-turn test with the corrected geometry (track from the four turns):
  Geometry(mpt_left=0.340339 mm, mpt_right=0.340339 mm, track=158.00 mm)
  odometry 1440.4 deg, true 1440.4 deg: the robot now really turns 4.00 turns for a command of 4
(c) why four turns: the angle error left by stopping (the robot coasts a little, the move ends within a tolerance) is about the same in degrees
    for one turn or for four, but it is divided by 4 times as large an angle: relative error of the track about 0.011 % (one) against 0.000 % (four)
END OUTPUT

Reading the output. The believed track is 150.00 mm; the robot, whose true track is 158 mm, is commanded to turn 360 degrees by odometry and really turns 342.0 degrees, so the estimate is 150 x 360.2 / 342.0 = 158.02 mm; four turns give 1440.3 / 1367.3 = 158.00 mm. Both are within a few hundredths of a millimetre of the truth because the simulator has no wheel slip and no unevenness of the floor. On a real robot the spread is far larger and the long test is the better one, for the reason in the idea; with the corrected geometry the same command of four turns gives 1440.4 degrees of true turning (the last 0.4 degrees are where the move stops, within its tolerance).

Check on method: the ways to get this wrong are (1) taking the true angle from the commanded angle instead of from measurement, which would make the ratio 1 and the correction zero; (2) multiplying where one should divide: if odometry says more than the robot did, the believed track is too small, so the corrected track is larger (the ratio odometry / true is above 1), and the ratio the other way round makes the error twice as big; (3) trusting this alone on a real robot: a spin also changes with a difference between the two wheels (Programming for Robotics 5 section 10 shows a spin test contaminated by a wheel mismatch giving 154.25 mm, 0.75 mm off, where the squares found 154.52 mm), so check the result with the square test of section 8 as well.