A museum tour robot is driven by a list of commands. Each command (v, omega, dt) means: move forward at v metres per second while turning at omega radians per second (positive counter-clockwise), for dt seconds. It then takes the next command from wherever it ended.
The robot's pose is (x, y, theta): its position in metres (x to the right, y up) and its heading in radians, counter-clockwise from the +x axis. It moves like a unicycle: always in the direction it faces, never sideways, so in symbols
With v and omega fixed, the robot follows these equations exactly for the whole dt, so a command with omega != 0 traces an arc of a circle (or a spin on the spot when v = 0), and a command with omega == 0 a straight segment. Do not split a command into smaller steps: compute where the exact motion ends.
Write unicycle_route(start, commands) that returns the final pose (x, y, theta), with theta wrapped into (-\pi, \pi]. With no commands the robot stays at start (wrapped heading included). Collect the positions after each command and press Run with plot(xs, ys) to draw the tour.
Examples
Input: start = (0, 0, 0), commands = [(1.0, 0.0, 2.0), (0.0, 0.5, 3.0), (0.4, 0.2, 5.0)]
Output: (1.20195431, 1.74376163, 2.5)
Explanation: 2 m straight along +x to (2, 0); a spin on the spot to heading 1.5 rad;
then 2 m along an arc of radius 0.4 / 0.2 = 2 m, turning 1 rad more.
Input: start = (1.0, 2.0, 3.0), commands = [(0.5, 1.0, 1.0)]
Output: (0.55103875, 1.83182556, -2.28318531)
Explanation: the heading reaches 4.0 rad, which is -2.2832 once wrapped.
Constraints
- floats are compared with a tolerance of
1e-6
Goals
- Move a pose along a straight line or an arc of a circle for a fixed time
- Chain several motion commands, each starting from where the last one ended
- Wrap the final heading into the range $(-\pi, \pi]$