A hospital delivery robot has a map of the ward with the world positions of its landmarks: the charging dock, the lift door, the nurses' station. To steer, it needs each landmark as it sees it: how far ahead and how far to the left, in its own frame.
- World frame: x to the right, y up, metres.
- Robot frame: the origin is the robot's centre, x points straight forward, y points to the robot's left.
- Pose
(x, y, theta): the robot's centre in world coordinates, and the anglethetain radians from the world's +x axis to the robot's forward axis, counter-clockwise.
Write to_robot_frame(pose, points) that takes a list of world points (x, y) and returns the same points in the robot frame, as (forward, left) tuples of floats, in the same order. A negative forward means the point is behind the robot; a negative left means it is on the robot's right.
Examples
Input: pose = (2, 1, 1.5707963267948966), points = [(1.75, 1.5), (2, 4)]
Output: [(0.5, 0.25), (3.0, 0.0)]
Explanation: the robot faces +y. The point (2, 4) is 3 m straight up, so 3 m dead ahead.
(1.75, 1.5) is 0.5 m up and 0.25 m towards -x: 0.5 m ahead and 0.25 m to the left.
Input: pose = (0, 0, 0.7853981633974483), points = [(1, 1), (-1, 1), (0, 0)]
Output: [(1.4142135623730951, 0.0), (0.0, 1.4142135623730951), (0.0, 0.0)]
Explanation: facing north-east, the point (1, 1) is straight ahead and (-1, 1) straight to the left.
Values like 1.8e-16 where the exact answer is 0 count as correct.
Constraints
- answers are compared with a tolerance of
1e-6; do not round
Goals
- Move a world point into the robot's frame: shift by $-(x, y)$, then rotate by $-\theta$
- See that undoing a transform reverses both the order and the signs of its steps
- Read the result as ahead/behind (sign of x) and left/right (sign of y)