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

Hold the Brush Level: Wrist First

inverse kinematics · decoupling · orientation · three-link arm

A glazing robot paints a stripe on pottery. Its brush must touch the pot at the right spot and point the right way. The arm is planar with three links: shoulder at the origin, links l1, l2, l3; the joint between the second and third links is the wrist, and the brush tip (the hand) is the end of the third link. x points right, y up.

The joint angles (t1, t2, t3) are in radians, counter-clockwise and relative: t1 is measured from the +x axis, t2 from the direction of the first link, t3 from the direction of the second. The hand's orientation is the direction the third link points in, measured from +x.

Write wrist_first(l1, l2, l3, x, y, phi) that returns every configuration that puts the hand on (x, y) with orientation phi. With the orientation fixed, the wrist must sit at one particular point; the shoulder and elbow then form a two-link arm reaching for the wrist. Let r be the distance from the shoulder to that wrist point. The configurations 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) first, then elbow-down (t2 > 0).

Return them as a list of tuples (t1, t2, t3), with t1 and t3 wrapped into (-\pi, \pi] (whole turns of 2\pi added or subtracted; -\pi is written as \pi). phi itself may be given outside that range. The wrist point is never the shoulder.

Examples

Input:  l1 = 0.5, l2 = 0.4, l3 = 0.2, x = 0.6, y = 0.5, phi = 0
Output: [(1.57079633, -1.57079633, 0.0), (0.22131444, 1.57079633, -1.79211077)]
Explanation: the brush points along +x, so the wrist is 0.2 to the left of the
hand, at (0.4, 0.5).

Input:  l1 = 1, l2 = 1, l3 = 0.5, x = 2.4, y = 0, phi = 0
Output: [(0.31756043, -0.63512086, 0.31756043), (-0.31756043, 0.63512086, -0.31756043)]

Input:  l1 = 1, l2 = 1, l3 = 0.5, x = 2.4, y = 0, phi = math.pi
Output: []
Explanation: the hand point is within reach, but pointing back towards the
shoulder puts the wrist at (2.9, 0), beyond 1 + 1 = 2.

Constraints

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

Goals

  • Use the required hand orientation to locate the wrist
  • Solve a three-link arm by solving a two-link arm for the wrist
  • Recover the last joint angle from the orientation, wrapped
Starting Python…