Problem 129969 · hard · Robotics · Level 01 Poses and Frames

The Stretch That Went Nowhere

pose · dead reckoning · angle wrapping · tolerance · prefix states · search

A warehouse robot logged every command it executed in a shift. Each command is a pair:

  • ("turn", a): turn on the spot by a radians (positive is counter-clockwise);
  • ("drive", d): drive d metres straight along the current heading (negative d reverses).

Driving d along heading \theta moves the robot by (d\cos\theta,\ d\sin\theta). The robot starts at the pose (0, 0, 0): the origin, facing the +x axis (x to the right, y up).

The fleet engineers look for wasted stretches: a block of consecutive commands, commands[first] up to and including commands[last], after which the robot is back at the pose it had just before commands[first]. "Back" means, comparing the pose just before the block with the pose just after it:

  • the two positions are at most pos_tol metres apart, and
  • the two headings differ by at most angle_tol radians, the difference being wrapped into (-\pi, \pi] first (so headings a whole turn apart do not differ).

Write wasted_stretch(commands, pos_tol, angle_tol) that returns the shortest wasted stretch as a tuple (first, last) of indices into commands. If several are equally short, return the one with the smallest first. Return None if there is no wasted stretch. A single command can be a wasted stretch on its own (("drive", 0), or a turn by a whole turn).

Examples

Input:  commands = [("drive", 1), ("turn", math.pi / 2)] * 4 + [("drive", 3)], pos_tol = 1e-6, angle_tol = 1e-6
Output: (0, 7)
Explanation: the first eight commands drive a 1 m square and end facing +x again.

Input:  commands = [("drive", 2), ("turn", 1), ("turn", -1), ("drive", -2)], pos_tol = 1e-6, angle_tol = 1e-6
Output: (1, 2)
Explanation: the whole list goes nowhere too, but the two turns in the middle are shorter.

Input:  commands = [("drive", 1), ("turn", 0.5)], pos_tol = 0.1, angle_tol = 0.1
Output: None

Input:  commands = [("turn", math.pi)] * 3, pos_tol = 1e-6, angle_tol = 1e-6
Output: (0, 1)
Explanation: (0, 1) and (1, 2) are both two half turns; the earlier one wins.

Constraints

  • len(commands) <= 500
  • no test puts any pair of poses within 1e-9 of either tolerance, so < and <= give the same answer
  • the whole check should run in well under a second: avoid re-simulating every block from scratch

Goals

  • Recognise that a block of commands goes nowhere exactly when the poses before and after it agree
  • Record the pose after every command once, and compare pairs of recorded poses
  • Search blocks shortest first, with a stated tie-break
Starting Python…