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

Straight in the Plan, Curved in the Air

inverse kinematics · forward kinematics · joint-space interpolation · point-line distance

A sign-writing robot is asked to draw a straight line with a planar two-link arm: shoulder at the origin, links l1 and l2, pen at the end; x right, y up. The planner samples the line, solves the inverse kinematics at each sample, and hands the joint angles to the motors. Between two samples, though, each motor simply turns at a steady rate from one angle to the next, and the pen does not follow a straight line. How far off does it get?

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:

  • 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);

with t1 wrapped into (-\pi, \pi]. Every sample in the tests is within reach.

Sample i, for i = 0, ..., n - 1, is \text{start} + \frac{i}{n - 1}(\text{goal} - \text{start}); at each, take the solution of the branch elbow ("up" or "down"), or the single solution. Between consecutive samples with angles (a1, a2) and (b1, b2), each joint turns the short way: by the difference wrapped into (-\pi, \pi]. Halfway, joint k is therefore at

a_k + \tfrac{1}{2}\,\mathrm{wrap}(b_k - a_k)

Write max_sag(l1, l2, start, goal, n, elbow) that returns, over the n - 1 gaps, the largest distance between the pen at the halfway moment and the straight line through start and goal (the whole line, not only the segment).

Examples

Input:  l1 = 1, l2 = 1, start = (1.5, 0), goal = (0, 1.5), n = 2, elbow = "up"
Output: 0.43933983
Explanation: both ends need the same bend, so halfway only the shoulder has
turned: the pen swings along a circle of radius 1.5 and is at (1.061, 1.061),
0.439 beyond the line x + y = 1.5.

Input:  l1 = 1, l2 = 1, start = (1.5, 0), goal = (0, 1.5), n = 5, elbow = "up"
Output: 0.03500846

Input:  l1 = 0.6, l2 = 0.4, start = (-0.8, -0.2), goal = (-0.3, 0.5), n = 5, elbow = "up"
Output: 0.01378846
Explanation: between the last two samples the shoulder goes from -3.083 to 2.800.
The short way is -0.400 rad, clockwise past the -x axis, not +5.883 rad all the way round.

Constraints

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

Goals

  • Interpolate joint angles the short way round between two samples
  • Use forward kinematics to see where the hand really goes between samples
  • Measure how far the hand strays from the intended straight line
Starting Python…