A field robot measures its pose relative to its base station: p = (x, y, t) says the robot is at (x, y) metres in the station's frame (the station's forward direction is +x, its left is +y) and faces t radians counter-clockwise from the station's forward direction.
The robot's planner works in the robot's own frame instead: x straight ahead of the robot, y to the robot's left, angles counter-clockwise from the robot's forward direction. It needs the station's pose in that frame: where the station is, as seen from the robot, and which way the station faces, measured from the robot's heading. This is the inverse of the pose p.
Write inverse_pose(p) that returns the station's pose in the robot's frame, (x', y', t'), with t' wrapped to (-\pi, \pi] (exactly -\pi is reported as \pi).
Composing a pose with its inverse must give back (0, 0, 0): place p's inverse in the station's frame by starting at p, going x' along the robot's forward direction and y' along its left, and adding t' to t, and you land on the station facing its own forward direction.
Examples
Input: p = (2, 0, 0)
Output: (-2.0, 0.0, 0.0)
Explanation: the robot is 2 m in front of the station and faces the same way,
so the station is 2 m behind the robot.
Input: p = (0, 3, 1.5707963267948966)
Output: (-3.0, 0.0, -1.5707963267948966)
Explanation: the robot is 3 m to the station's left, facing away from it; the station is
3 m straight behind the robot, and faces a quarter turn clockwise from the robot.
Input: p = (1, 2, 0.8)
Output: (-2.131418891146211, -0.676057327794808, -0.8)
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
- Invert a pose: swap which of two frames is the reference
- Express an offset in a robot's frame with its forward and left directions
- Check an inverse by composing it with the original pose