Where the Elbow Must Flip

A welding robot runs a seam along a straight line with a planar two-link arm: shoulder at the origin, links l1 and l2, torch at the end; x right, y up. Flipping the elbow to its mirror pose in the middle of a seam means lifting the torch, swinging the arm round and starting again, so the planner wants to know where that would happen.

The joint angles (t1, t2) are in radians, counter-clockwise and relative (t1 from the +x axis, t2 from the direction of the first link). For a point at distance r from the shoulder, the inverse-kinematics solutions are:

• none if r is more than 10^{-9} outside the range from abs(l1 - l2) to l1 + l2;
• a single solution, t2 = 0.0 (straight), if r is within 10^{-9} of l1 + l2, or t2 = math.pi (folded), if within 10^{-9} of abs(l1 - l2);
• otherwise two: elbow-up (t2 < 0) and elbow-down (t2 > 0).

In every solution t1 is wrapped into (-\pi, \pi]. A solution is allowed when lo1 <= t1 <= hi1 and lo2 <= t2 <= hi2 for the stops limits = ((lo1, hi1), (lo2, hi2)), where an angle within 10^{-9} outside a stop still counts as on it.

The seam is sampled at n points, sample i being \text{start} + \frac{i}{n - 1}(\text{goal} - \text{start}) for i = 0, ..., n - 1. The arm starts on the branch elbow ("up" or "down") and goes through the samples in order:

1. If the sample has no allowed solution, the seam cannot be welded: return None.
2. If the sample has a single solution (straight or folded), use it. Both branches meet there, so the arm may leave the next sample on either branch without a flip.
3. Otherwise, use the current branch's solution if it is allowed. If not, use the other branch's solution and switch to that branch; this is a flip at sample i, unless the previous sample had a single solution, in which case the switch is free. (Before sample 0 the arm is on its branch, not at a single solution: being forced onto the other branch at sample 0 is a flip at 0.)

Write plan_seam(l1, l2, start, goal, n, limits, elbow) that returns (flips, angles): the list of samples where the elbow flips, in increasing order, and the list of the n solutions used.

Examples

Input:  l1 = 1, l2 = 1, start = (1.2, 1.2), goal = (1.2, -1.2), n = 5,
        limits = ((-1, 2), (-math.pi, math.pi)), elbow = "down"
Output: ([3], [(0.22779934, 1.1151977), (-0.37183426, 1.6709637),
               (-0.92729522, 1.8545904), (0.37183426, -1.6709637), (-0.22779934, -1.1151977)])
Explanation: at sample 3, (1.2, -0.6), elbow-down would need t1 = -1.30, past the
stop at -1. The arm flips to elbow-up and stays there.

Input:  l1 = 1, l2 = 1, start = (1.6, 1.2), goal = (1.6, -1.2), n = 5,
        limits = ((-math.pi, math.pi), (0, math.pi)), elbow = "up"
Output: ([], [(0.64350111, 0.0), (-0.18762989, 1.0928011), (-0.64350111, 1.2870022),
              (-0.90517123, 1.0928011), (-0.64350111, 0.0)])
Explanation: the elbow can only bend counter-clockwise (t2 >= 0), so elbow-up is
never allowed. But the seam starts with the arm straight, at distance 2, so it
moves onto elbow-down for free.

Input:  l1 = 1, l2 = 1, start = (1.2, 1.2), goal = (1.2, -1.2), n = 5,
        limits = ((-math.pi, math.pi), (0, math.pi)), elbow = "up"
Output: ([0], [(0.22779934, 1.1151977), (-0.37183426, 1.6709637),
               (-0.92729522, 1.8545904), (-1.2991295, 1.6709637), (-1.342997, 1.1151977)])

Constraints

• answers are compared with a tolerance of 1e-6; do not round

Goals

• Follow one elbow branch along a sampled line while the joint stops allow it
• Detect the samples where the arm is forced to flip to the other branch
• Use the straight (or folded) pose as a free crossing between the branches