A delivery robot in a hospital corridor network starts at the pose start = (x, y, theta): position in metres (x to the right, y up) and heading theta in radians, counter-clockwise from the +x axis. It visits a list of waypoints, each a position (wx, wy), in the given order. To reach each waypoint it:
- turns on the spot until it faces the waypoint, always the shorter way round: the turn angle lies in (-\pi, \pi] (a half turn counts as \pi either way);
- drives straight to it, and keeps that heading.
There is no final turn after the last waypoint. A waypoint exactly at the robot's current position costs nothing: no turn, no drive.
Write route_cost(start, waypoints) that returns a tuple (distance, turning): the total distance driven in metres, and the total amount of turning in radians, counting every turn as positive whichever way it goes. Press Run with plot(xs, ys) on the start and the waypoints to see the round.
Examples
Input: start = (0, 0, 0), waypoints = [(3, 0), (3, 4), (0, 0)]
Output: (12.0, 4.068887871591405)
Explanation: the legs are 3, 4 and 5 m long. The robot already faces (3, 0); then it turns
pi/2 = 1.5708 to face up; to face (0, 0) from heading pi/2 it turns 2.4981 counter-clockwise
(the other way round would be 3.7851).
Input: start = (0, 0, 0), waypoints = [(0, 0), (-2, 0)]
Output: (2.0, 3.141592653589793)
Explanation: the first waypoint is where the robot already is; then a half turn and 2 m.
Input: start = (1, 1, 1.5707963267948966), waypoints = []
Output: (0.0, 0.0)
Constraints
- answers are compared with a tolerance of
1e-6; do not round
Goals
- Find the heading that points from one position to another with `math.atan2`
- Take the shorter way round for every turn by wrapping the angle difference
- Total the distance driven and the turning done along a route