Problem 310875 · medium · Robotics · Level 03 Reaching and Sensing

The Smallest Move to the Next Bolt

inverse kinematics · angle wrapping · distance in joint space · tie-breaking

A two-link arm tightens bolts on a car door. Every time it moves to the next bolt, it should turn its joints as little as possible: big swings waste time and can whip the cable harness.

The arm is planar: shoulder at the origin, links l1 and l2, hand at the end; x right, y up. 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 target (x, y) 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;
  • one, t2 = 0.0, if r is within 10^{-9} of l1 + l2, or one, t2 = math.pi, 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]. The target is never the shoulder itself.

A joint can turn either way round, so the turn from angle a to angle b is the wrapped difference: b - a plus or minus whole turns of 2\pi, brought into (-\pi, \pi]. The cost of moving from the current angles current = (c1, c2) to a solution (t1, t2) is

|\mathrm{wrap}(t_1 - c_1)| + |\mathrm{wrap}(t_2 - c_2)|

The current angles may be any real numbers (a joint that has turned more than once is not wrapped).

Write nearest_solution(l1, l2, x, y, current) that returns the solution (t1, t2) with the smallest cost, exactly as defined above (with t1 wrapped), or None if the target is out of reach. If the two costs differ by less than 10^{-9}, return elbow-up.

Examples

Input:  l1 = 1, l2 = 1, x = 1, y = 1, current = (0.2, 1.0)
Output: (0.0, 1.57079633)
Explanation: elbow-up (pi/2, -pi/2) costs 1.371 + 2.571 = 3.942;
elbow-down (0, pi/2) costs 0.2 + 0.571 = 0.771.

Input:  l1 = 0.4, l2 = 0.25, x = -0.5, y = -0.2, current = (-3.0, 0.1)
Output: (3.06983906, 1.22653653)
Explanation: elbow-up (-2.309, -1.227) costs 0.691 + 1.327 = 2.018. Elbow-down
(3.070, 1.227) looks far from -3.0, but turning the shoulder the short way round is
only 0.213, so it costs 0.213 + 1.127 = 1.340.

Input:  l1 = 1, l2 = 1, x = 1, y = 1, current = (math.pi / 4, 0)
Output: (1.57079633, -1.57079633)
Explanation: both cost pi/4 + pi/2: a tie, so elbow-up.

Constraints

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

Goals

  • Measure how far the joints must turn, using wrapped angle differences
  • Choose, among the inverse-kinematics solutions, the one nearest the current pose
  • Break near-ties with a stated tolerance and order
Starting Python…