A robotic desk lamp is a planar arm with n links. Its base joint sits at the origin; link i has length lengths[i] and is turned by the joint angle angles[i] (radians, counter-clockwise). The angles are relative: angles[0] is measured from the +x axis, and every later angle from the direction of the link before it. The world frame has x to the right and y up.
To draw the lamp on screen, the app needs every joint. Write joint_points(lengths, angles) that returns a tuple (points, heading):
points: a list ofn + 1points(x, y), starting with the base(0.0, 0.0), then the end of link 0, the end of link 1, ..., and last the lamp head (the hand);heading: the direction the last link points in, measured from +x and wrapped into the interval (-\pi, \pi] (add or subtract whole turns of 2\pi). The interval leaves out -\pi and keeps \pi: a link pointing along -x has heading \pi.
Examples
Input: lengths = [1, 1, 1], angles = [math.pi / 2, -math.pi / 2, -math.pi / 2]
Output: ([(0.0, 0.0), (0.0, 1.0), (1.0, 1.0), (1.0, 0.0)], -1.57079633)
Explanation: up, then turn right to point along +x, then right again to point down.
Input: lengths = [2, 1], angles = [math.pi / 2, math.pi / 2]
Output: ([(0.0, 0.0), (0.0, 2.0), (-1.0, 2.0)], 3.14159265)
Explanation: the last link points along -x, a heading of exactly pi.
Input: lengths = [0.3, 0.3, 0.2], angles = [3, 2, 1.5]
Output: ([(0.0, 0.0), (-0.29699775, 0.042336), (-0.21189909, -0.24534128),
(-0.01658157, -0.20231728)], 0.21681469)
Explanation: the angles add up to 6.5, which is 6.5 - 2 * pi = 0.2168 once wrapped.
Constraints
- answers are compared with a tolerance of
1e-6; do not round
Goals
- Walk along a chain of links, carrying the position and the heading
- Record every joint on the way, not only the end
- Wrap an angle into the interval $(-\pi, \pi]$