How Far Must the Elbow Bend? · Python problem 368268

A mural robot holds a spray nozzle on a planar two-link arm: the shoulder is fixed at the origin, a first link of length l1 runs to the elbow, and a second link of length l2 runs from the elbow to the nozzle (the hand). The world frame has x to the right and y up; all lengths are in metres.

Before it plans anything, the robot asks: to put the hand on the target (x, y), how far must the elbow bend? The bend is the angle between the direction of the first link and the direction of the second, in radians, from 0 (arm stretched straight) to \pi (arm folded back on itself). Which way the elbow bends does not matter here.

The triangle shoulder, elbow, hand has sides l1, l2 and r, the distance from the shoulder to the target. The law of cosines ties the bend b to them:

r^2 = l_1^2 + l_2^2 + 2\, l_1 l_2 \cos b

Write elbow_bend(l1, l2, x, y) that returns the bend b, or None when the target is out of reach. The rules for the edges:

the target is reachable when |l_1 - l_2| - 10^{-9} \le r \le l_1 + l_2 + 10^{-9} (a target a hair outside the ring of reachable points still counts, because coordinates carry rounding errors);
if rounding makes the cosine come out a little above 1 or below -1, clamp it to 1 or -1 before taking the arc cosine.

Examples

Input:  l1 = 0.4, l2 = 0.25, x = 0.3, y = 0.1
Output: 2.23001573
Explanation: r^2 = 0.1, so cos b = (0.1 - 0.16 - 0.0625) / (2 * 0.4 * 0.25) = -0.6125.

Input:  l1 = 0.4, l2 = 0.25, x = 0.65, y = 0
Output: 0.0
Explanation: the target is exactly at full reach: the arm is stretched straight.

Input:  l1 = 0.4, l2 = 0.25, x = 0.5, y = 0.5
Output: None
Explanation: the target is 0.707 m away, beyond 0.4 + 0.25 = 0.65.

Input:  l1 = 0.7, l2 = 0.3, x = 0.6, y = 0.8
Output: 0.0
Explanation: the target is at full reach (distance 1), but in floating point the
cosine comes out as 1.0000000000000002; clamped to 1, the bend is 0.

Constraints

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

Goals

Find the elbow angle of a two-link arm from the law of cosines
Decide first whether the target is reachable at all
Clamp a cosine that rounding pushed just outside $[-1, 1]$