A robot lawnmower knows its pose (x, y, theta): its position in metres in the garden's frame (x to the right, y up) and its heading theta, the direction it faces, in radians counter-clockwise from the +x axis. It is given a script of commands, each a pair:
("turn", a): turn on the spot byaradians (positive is counter-clockwise, negative clockwise). The position does not change.("drive", d): drivedmetres straight along the current heading without turning. A negativedmeans reversing: the mower moves backwards, still facing the same way.
Driving d along heading \theta changes the position by (d\cos\theta,\ d\sin\theta).
Write follow_script(start, commands) that returns the final pose as a tuple (x, y, theta), with theta wrapped to the range (-\pi, \pi]: add or subtract whole turns of 2\pi until it lies in that range, so a heading of exactly -\pi is reported as \pi. Collect the positions in two lists and press Run with plot(xs, ys) to see the mower's track.
Examples
Input: start = (0, 0, 0), commands = [("drive", 2), ("turn", math.pi / 2), ("drive", 1)]
Output: (2.0, 1.0, 1.5707963267948966)
Explanation: 2 m along +x, a quarter turn to the left, then 1 m along +y.
Input: start = (1, 2, 3.0), commands = [("turn", 0.5), ("drive", -2)]
Output: (2.872913374581593, 2.70156645537924, -2.7831853071795862)
Explanation: the heading becomes 3.5 rad, which is -2.7832 after subtracting 2*pi.
Reversing 2 m moves the mower by (-2 cos 3.5, -2 sin 3.5) = (1.8729, 0.7016).
Input: start = (5, 5, 0), commands = [("turn", -math.pi)]
Output: (5.0, 5.0, 3.141592653589793)
Explanation: a heading of -pi is reported as pi.
Constraints
- answers are compared with a tolerance of
1e-6; do not round
Goals
- Keep a robot's pose `(x, y, theta)` up to date while it turns and drives
- Move a distance `d` along a heading with `cos` and `sin`
- Wrap a heading into the range $(-\pi, \pi]$