A platoon of delivery robots drives through a campus. A central tracker knows every robot's world pose (x, y, theta): position in metres (x to the right, y up) and heading in radians, counter-clockwise from the +x axis. The platoon leader's controller does not want world coordinates: it wants every other robot's pose as the leader sees it, in the leader's own frame:
x': how far the robot is ahead of the leader (negative means behind),y': how far it is to the leader's left (negative means to the right),t': which way it faces, counter-clockwise from the leader's heading, wrapped to (-\pi, \pi] (exactly -\pi is reported as \pi).
Write platoon_view(leader, others) that returns the list of (x', y', t') tuples, one per robot in others, in the same order.
Examples
Input: leader = (1, 1, 1.5707963267948966), others = [(1, 4, 1.5707963267948966), (0, 1, 3.141592653589793)]
Output: [(3.0, 0.0, 0.0), (0.0, 1.0, 1.5707963267948966)]
Explanation: the leader faces +y. The first robot is 3 m up the y axis, straight ahead,
facing the same way. The second is 1 m to the -x side, which is the leader's left, and
faces a quarter turn further counter-clockwise.
Input: leader = (2, -1, -2.5), others = [(-1, 2, 2.8)]
Output: [(0.6080144143289319, -4.1988472789526705, -0.9831853071795864)]
Explanation: 2.8 - (-2.5) = 5.3 rad, which wraps to 5.3 - 2*pi = -0.9832.
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
- Find the pose of one robot in another robot's frame from their world poses
- Rotate a world-frame offset into a robot's forward and left directions
- Express a heading relative to another heading, wrapped