Problem 351297 · easy · Robotics · Level 03 Reaching and Sensing

Solutions the Stops Allow

inverse kinematics · joint limits · filtering · tolerance

A dental-lab arm polishes crowns. It is a planar two-link arm: 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).

The inverse-kinematics solutions for a target (x, y) at distance r from the shoulder 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 (stretched), or one, t2 = math.pi, if within 10^{-9} of abs(l1 - l2) (folded);
  • otherwise two: elbow-up (t2 < 0) first, then elbow-down (t2 > 0).

In every solution t1 is wrapped into (-\pi, \pi]. The target is never the shoulder itself.

Hoses run through the joints, so each joint has mechanical stops: limits = ((lo1, hi1), (lo2, hi2)). A solution is allowed when lo1 <= t1 <= hi1 and lo2 <= t2 <= hi2, where an angle within 10^{-9} outside a stop still counts as on it (angles come out of trigonometry with rounding errors).

Write ik_with_stops(l1, l2, x, y, limits) that returns the list of allowed solutions (t1, t2), in the order above, or None when there is none (because the target is out of reach or every solution hits a stop).

Examples

Input:  l1 = 1, l2 = 1, x = 1, y = 1, limits = ((-math.pi / 2, math.pi / 2), (0, math.pi))
Output: [(0.0, 1.57079633)]
Explanation: elbow-up is (pi/2, -pi/2): t1 sits exactly on its stop, which is
fine, but t2 < 0 is not allowed. Elbow-down (0, pi/2) is.

Input:  l1 = 0.4, l2 = 0.25, x = 0.3, y = 0.1, limits = ((-1.5, 1.5), (-2.5, 2.5))
Output: [(0.996782, -2.23001573), (-0.3532809, 2.23001573)]

Input:  l1 = 0.4, l2 = 0.25, x = 0.3, y = 0.1, limits = ((0, 0.5), (-2.5, 2.5))
Output: None
Explanation: the shoulder would have to turn to 0.997 or to -0.353, both outside [0, 0.5].

Constraints

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

Goals

  • Compute both inverse-kinematics solutions of a two-link arm
  • Keep only the solutions whose joints sit inside their stops
  • Return `None` when no solution is allowed
Starting Python…