An autonomous forklift knows its own pose a = (xa, ya, ta) in the warehouse: its position in metres (x to the right, y up) and its heading ta in radians, counter-clockwise from the +x axis. Its camera spots a pallet and reports the pallet's pose relative to the forklift, b = (xb, yb, tb), in the forklift's own frame: xb metres straight ahead, yb metres to the forklift's left (negative means to its right), and the pallet's front facing tb radians counter-clockwise from the direction the forklift faces.
Write compose(a, b) that returns the pallet's pose in the warehouse frame, (x, y, theta), with theta wrapped to the range (-\pi, \pi] (whole turns of 2\pi added or subtracted; exactly -\pi is reported as \pi). This combination of two poses is called composing them.
Examples
Input: a = (2, 1, math.pi / 2), b = (3, 0, 0)
Output: (2.0, 4.0, 1.5707963267948966)
Explanation: the forklift faces +y, so "3 m ahead" is 3 m up the y axis,
and the pallet faces the same way as the forklift.
Input: a = (1, 1, 0.5), b = (2, -1, 1.0)
Output: (3.2345906623849485, 1.0812685153180333, 1.5)
Explanation: 2 m ahead along heading 0.5 is (1.7552, 0.9589); 1 m to the right
of that is a further (0.4794, -0.8776). The headings add: 0.5 + 1.0 = 1.5.
Input: a = (4, -2, 3.0), b = (0, 0, 0.5)
Output: (4.0, -2.0, -2.7831853071795862)
Explanation: the same spot, turned by 3.5 rad, which wraps to 3.5 - 2*pi.
Constraints
- answers are compared with a tolerance of
1e-6; do not round
Goals
- Turn a pose measured in a robot's own frame into a pose in the world frame
- Rotate the relative offset by the robot's heading before adding it
- Add headings and wrap the result into $(-\pi, \pi]$