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

Elbow Up or Elbow Down

inverse kinematics · law of cosines · atan2 · multiple solutions

A ceiling-mounted arm in a library returns books to the shelves. Seen from the side it is a planar two-link arm: shoulder at the origin, first link l1, elbow, second link l2, hand. The world frame has x to the right and y up.

The joint angles (t1, t2) are in radians, counter-clockwise, and relative: t1 is the first link's angle from the +x axis, t2 the second link's angle from the direction of the first (t2 = 0 is a straight arm).

Most reachable targets can be reached in two ways, mirror images across the line from the shoulder to the target:

  • elbow-up: t2 < 0. Walking from the shoulder towards the target, the elbow sits on your left (above the line when the target is to the right of the shoulder);
  • elbow-down: t2 > 0, the elbow on your right.

Write two_link_ik(l1, l2, x, y) that returns a list of the solutions (t1, t2) that put the hand on (x, y):

  • normally two tuples, elbow-up first, then elbow-down;
  • one tuple when the arm must be stretched straight (the target's distance r from the shoulder is within 10^{-9} of l1 + l2; then t2 = 0.0) or folded back (r within 10^{-9} of abs(l1 - l2); then t2 = math.pi);
  • an empty list when the target is out of reach (r more than 10^{-9} beyond either edge).

t1 is wrapped into (-\pi, \pi] (whole turns of 2\pi added or subtracted; -\pi is written as \pi); t2 lies in [-\pi, \pi] as described. The target is never the shoulder itself.

Examples

Input:  l1 = 1, l2 = 1, x = 1, y = 1
Output: [(1.57079633, -1.57079633), (0.0, 1.57079633)]
Explanation: elbow-up has the elbow at (0, 1), elbow-down at (1, 0).

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

Input:  l1 = 0.4, l2 = 0.25, x = 0, y = 0.65
Output: [(1.57079633, 0.0)]
Explanation: at full reach there is one way: stretched, pointing at the target.

Input:  l1 = 0.25, l2 = 0.4, x = 0, y = 0.15
Output: [(-1.57079633, 3.14159265)]
Explanation: folded, the short first link points away from the target and the
longer second link comes back past the shoulder.

Constraints

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

Goals

  • Find both joint configurations that put a two-link arm's hand on a target
  • Get the shoulder angle from the direction of the target and the elbow's offset
  • Handle the single solution at full stretch and when folded
Starting Python…