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

Ruling a Straight Line

inverse kinematics · interpolation · sampling · trajectory

A drawing robot rules straight lines on a whiteboard with a planar two-link arm: shoulder at the origin, links l1 and l2, pen at the end; x right, y up. A joint controller cannot follow "a straight line"; it follows a list of joint angles. So the planner samples the line and converts each sample into joint angles.

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 point 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 line runs from start to goal. Sample i, for i = 0, 1, ..., n - 1, is the point

p_i = \text{start} + \frac{i}{n - 1}\,(\text{goal} - \text{start})

so the first sample is start and the last is goal. Write line_angles(l1, l2, start, goal, n, elbow) that returns a list of n entries, one per sample: the solution of the requested branch (elbow is "up" or "down"), the single solution where there is only one, or None where the sample is out of reach. No sample is the shoulder itself.

Press Run with plot([q[1] for q in result if q]) to see how the elbow angle changes along the line.

Examples

Input:  l1 = 1, l2 = 1, start = (1.5, 0), goal = (0, 1.5), n = 3, elbow = "up"
Output: [(0.72273425, -1.4454685), (1.79720462, -2.02361292), (2.29353057, -1.4454685)]
Explanation: the middle sample (0.75, 0.75) is closer to the shoulder, so the
elbow bends further there.

Input:  l1 = 1, l2 = 1, start = (1, 0), goal = (3, 0), n = 5, elbow = "down"
Output: [(-1.04719755, 2.0943951), (-0.72273425, 1.4454685), (0.0, 0.0), None, None]
Explanation: the samples are at x = 1, 1.5, 2, 2.5, 3. At x = 2 the arm is
stretched (one solution); beyond it the pen cannot reach.

Constraints

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

Goals

  • Sample a straight segment at evenly spaced points
  • Solve the inverse kinematics at every sample with a fixed elbow choice
  • Mark the samples the arm cannot reach
Starting Python…