Problem 494595 · hard · Robotics · Level 04 Belief and Optimisation

The Elbow Hits Its Stop

inverse kinematics · gradient descent · joint limits · clamping · stopping rules

A parcel robot's arm is a planar two-link arm: the shoulder joint at the origin, a first link of length l1, an elbow, and a second link of length l2 ending in the suction cup (the hand). The joint angles are in radians and relative: t1 is the first link's angle from the +x axis, and t2 is the second link's angle from the direction of the first link. With x to the right and y up, the hand is at

x = l_1 \cos t_1 + l_2 \cos(t_1 + t_2), \qquad y = l_1 \sin t_1 + l_2 \sin(t_1 + t_2).

Each joint has mechanical stops: limits = ((lo1, hi1), (lo2, hi2)), and t1 must stay in [lo1, hi1], t2 in [lo2, hi2]. The robot searches for the angles by gradient descent on E = \tfrac{1}{2}\left[(x - t_x)^2 + (y - t_y)^2\right], where target = (tx, ty), and clamps each angle back into its range after every step (an angle below lo becomes lo, above hi becomes hi).

Write clamped_descent(l1, l2, target, start, limits, alpha, tol, stall, max_iters). Start with the angles start (inside the limits) and k = 0, let d be the distance from the hand to the target, and repeat:

  1. If d \le tol: stop with status "reached".
  2. If k == max_iters: stop with status "out of steps".
  3. Compute \partial E / \partial t_1 and \partial E / \partial t_2 at the current angles; set t_i \leftarrow t_i - \alpha \, \partial E / \partial t_i for both at once; clamp both; add 1 to k; compute the new d.
  4. If the new d > tol and neither angle changed in this step (after clamping) by more than stall, stop with status "stalled". Otherwise go back to step 1.

Return a tuple (t1, t2, k, d, status): the angles and the distance when the search stops, the number of steps made, and the status. All limits lie in [-\pi, \pi], so the angles need no wrapping.

Examples

Input:  l1 = 1, l2 = 1, target = (1, 1), start = (0.2, 0.5), limits = ((-3, 3), (0, 2.5)),
        alpha = 0.2, tol = 1e-4, stall = 1e-7, max_iters = 1000
Output: (0.00008504, 1.57065873, 125, 0.00009997, "reached")

Input:  l1 = 1, l2 = 1, target = (1, 1), start = (1.2, -0.5), limits = ((-3, 3), (-1.0, 0)),
        alpha = 0.2, tol = 1e-4, stall = 1e-7, max_iters = 1000
Output: (1.28539807, -1.0, 35, 0.34095156, "stalled")
Explanation: the target needs the elbow at -pi/2, but it stops at -1. With the elbow
there, the hand can only be 2 * cos(0.5) = 1.7552 from the shoulder, and the target
is sqrt(2) = 1.4142 away: the best it can do is 0.3410 short.

Input:  l1 = 1, l2 = 1, target = (1, 1), start = (0.2, 0.5), limits = ((-3, 3), (0, 2.5)),
        alpha = 0.2, tol = 1e-4, stall = 1e-7, max_iters = 5
Output: (0.42422226, 0.76828195, 5, 0.44159646, "out of steps")

Constraints

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

Goals

  • Keep an iterative solver inside the joint limits by clamping after every step
  • Detect a stall: the search no longer moves although the target is not reached
  • Order three stopping rules exactly and report which one fired
Starting Python…