Problem 489645 · medium · Robotics · Level 04 Belief and Optimisation

Downhill to the Light Switch

inverse kinematics · gradient descent · Jacobian transpose · stopping rule · angle wrapping

A home-help robot must press a light switch. Its 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 fingertip (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).

Instead of a formula for the angles, the robot searches for them. With the switch at target = (tx, ty), the error to make small is half the squared distance

E(t_1, t_2) = \tfrac{1}{2}\left[(x - t_x)^2 + (y - t_y)^2\right].

Write ik_descent(l1, l2, target, start, alpha, tol, max_iters) that runs gradient descent on E from start = (t1, t2) and returns a tuple (t1, t2, iterations, distance):

  1. Set iterations = 0.
  2. Compute the hand position and its distance d to the target (the plain Euclidean distance, not E). If d \le tol, or iterations == max_iters, stop.
  3. Otherwise compute both partial derivatives \partial E / \partial t_1 and \partial E / \partial t_2 at the current angles, then update both angles at once: t_i \leftarrow t_i - \alpha \, \partial E / \partial t_i. Add 1 to iterations and go back to step 2.

Return the final angles wrapped into (-\pi, \pi] (add or subtract whole turns of 2\pi), the number of updates made, and the distance d at the final angles. A target out of reach is allowed: the descent then runs out its budget. Collect the distances in a list and press Run with plot(dists, logy=True) to watch them fall.

Examples

Input:  l1 = 1, l2 = 1, target = (1, 1), start = (0, 0.5), alpha = 0.2, tol = 1e-4, max_iters = 1000
Output: (0.00008266, 1.57066258, 122, 0.00009717)
Explanation: the arm settles on the shoulder along +x and the elbow bent a quarter
turn; after update 122 the fingertip is within 1e-4 of the switch.

Input:  l1 = 1, l2 = 1, target = (1, 1), start = (0, 0.5), alpha = 0.2, tol = 1e-4, max_iters = 3
Output: (0.38998153, 0.79619964, 3, 0.42940689)

Input:  l1 = 1, l2 = 1, target = (3, 0), start = (0.5, 0.5), alpha = 0.2, tol = 1e-6, max_iters = 200
Output: (-0.00000001, 0.00000002, 200, 1.0)
Explanation: the switch is 3 away but the arm only reaches 2; it ends stretched toward it.

Constraints

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

Goals

  • Find joint angles by gradient descent on half the squared distance to the target
  • Compute the gradient as the Jacobian transpose times the position error
  • Stop by a tolerance or an iteration budget and wrap the final angles
Starting Python…