A cleaning robot knows its outline only in its own frame: the origin is the centre of its wheel axle, the x axis points straight forward and the y axis points to the robot's left (metres). Its bumper corners, for example, are a fixed list in that frame.
To check for collisions with the furniture on the map, it needs those corners in the world frame (x to the right, y up). Where the robot is and which way it faces is its pose (x, y, theta): the world position of its origin, and the angle theta in radians from the world's +x axis to the robot's forward axis, counter-clockwise.
Write footprint_world(pose, corners) that returns the list of corners in world coordinates, as (x, y) tuples of floats, in the same order.
Examples
Input: pose = (2, 1, 1.5707963267948966),
corners = [(0.5, 0.25), (0.5, -0.25), (-0.3, -0.25), (-0.3, 0.25)]
Output: [(1.75, 1.5), (2.25, 1.5), (2.25, 0.7), (1.75, 0.7)]
Explanation: the robot stands at (2, 1) facing +y (up). Its front-left corner, 0.5 m ahead
and 0.25 m to its left, is 0.5 m up and 0.25 m towards -x in the world: (1.75, 1.5).
Input: pose = (-1, 3, 3.141592653589793), corners = [(1, 0), (0, 1)]
Output: [(-2.0, 3.0), (-1.0, 2.0)]
Explanation: facing -x, "forward" is towards -x and "left" is towards -y.
Constraints
- answers are compared with a tolerance of
1e-6; do not round
Goals
- Read a pose $(x, y, \theta)$ as where the robot is and which way it faces
- Move a point from the robot's frame into the world frame: rotate by $\theta$, then shift by $(x, y)$
- Check a transform with quarter turns, where the answer can be seen by eye