A greenhouse robot drives between the rows with a small arm bolted to its deck. The robot's pose in the world is pose = (X, Y, theta): its centre is at (X, Y) and it faces the direction theta (radians, counter-clockwise from the world's +x axis; world x points right, y up).
The arm is described in the robot's frame: x forward (the direction the robot faces), y to the robot's left. Its base is at mount = (mx, my) in that frame. The arm is planar with links lengths and joint angles angles (radians, counter-clockwise). The angles are relative: angles[0] is measured from the robot's forward direction, every later one from the direction of the link before it.
Write world_hand(pose, mount, lengths, angles) that returns (x, y, heading): where the hand (the end of the last link) is in the world, and the direction of the last link in the world, wrapped into (-\pi, \pi] (whole turns of 2\pi added or subtracted; a direction along -x is \pi, never -\pi).
Examples
Input: pose = (0, 0, 0), mount = (0.2, 0), lengths = [0.5], angles = [0]
Output: (0.7, 0.0, 0.0)
Explanation: the robot sits at the origin facing +x, so its frame is the world's.
Input: pose = (2, 1, math.pi / 2), mount = (0.3, 0.1), lengths = [0.4, 0.3],
angles = [0, -math.pi / 2]
Output: (2.2, 1.7, 0.0)
Explanation: in the robot's frame the hand is at (0.7, -0.2), pointing to the
robot's right. The robot faces +y, so "forward" is world +y and "right" is
world +x: the hand is 0.7 above and 0.2 to the right of (2, 1).
Input: pose = (-1.5, 4.0, 2.5), mount = (0.25, -0.1), lengths = [0.6, 0.45],
angles = [0.9, 0.8]
Output: (-2.44113497, 3.68419864, -2.08318531)
Constraints
- answers are compared with a tolerance of
1e-6; do not round
Goals
- Compute the hand in the robot's own frame first
- Rotate and shift a point from the robot's frame into the world frame
- Add the robot's heading to the arm's to get the hand's world heading