A small robot reports its motion one step at a time, in its own words: each step is a triple (dx, dy, dtheta) measured in the robot's frame at the start of that step (x forward, y to the robot's left, metres). It means: the robot's centre ended up at the point dx ahead and dy to the left of where it was, and its heading changed by dtheta radians (positive is a left turn).
Keeping track of the world pose from such steps is called dead reckoning. A pose is (x, y, theta): the robot's centre in the world (x to the right, y up) and the angle theta in radians from the world's +x axis to the robot's forward direction, counter-clockwise, wrapped to (-\pi, \pi].
Write dead_reckon(start, moves) that returns the list of world poses after each step, in order, each as a tuple (x, y, theta) with theta wrapped to (-\pi, \pi]. The result is empty when there are no moves. Collect the x and y values and press Run with plot(xs, ys) to see the path.
Examples
Input: start = (0, 0, 0), moves = [(2, 0, 1.5707963267948966), (1, 0, 0), (0, -1, -1.5707963267948966)]
Output: [(2.0, 0.0, 1.5707963267948966), (2.0, 1.0, 1.5707963267948966), (3.0, 1.0, 0.0)]
Explanation: 2 m ahead along +x, then a left quarter turn. Now facing +y, "1 m ahead"
means 1 m up. Then 1 m to the robot's right, which is +x, and a right quarter turn.
Input: start = (1, 1, 3.0), moves = [(1, 0, 0.5)]
Output: [(0.010007503399554585, 1.1411200080598671, -2.7831853071795862)]
Explanation: the step goes 1 m along the heading 3.0; the new heading 3.5 wraps to 3.5 - 2*pi.
Constraints
- answers are compared with a tolerance of
1e-6; do not round
Goals
- Compose a pose with a motion given in the robot's own frame
- Use the heading before a step to place it, and update the heading after
- Chain many steps into a trajectory and keep the heading wrapped