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

Three Joints, One Bookshelf Corner

inverse kinematics · redundancy · gradient descent · Jacobian · regularisation · angle wrapping

A library robot slides books onto shelves with a planar three-link arm: the base joint at the origin, and links of lengths lengths[0], lengths[1], lengths[2], the last ending in the gripper (the hand). The joint angles are in radians and relative: q_0 is the first link's angle from the +x axis, and each later angle is measured from the direction of the link before it. With x to the right and y up, link j points in the absolute direction \varphi_j = q_0 + \dots + q_j, and the hand is at

x = \sum_{j=0}^{2} l_j \cos \varphi_j, \qquad y = \sum_{j=0}^{2} l_j \sin \varphi_j.

Three joints for a two-coordinate target is one more than needed: the arm is redundant, and a reachable point can usually be reached in endlessly many poses. To prefer poses close to where the arm already is, the robot minimises

E(q) = \tfrac{1}{2}\left[(x - t_x)^2 + (y - t_y)^2\right] + \tfrac{\mu}{2} \sum_{i=0}^{2} (q_i - s_i)^2

where target = (tx, ty) and start = (s0, s1, s2) is the starting pose.

Write three_link_reach(lengths, target, start, alpha, mu, iters) that starts at $q = $ start and makes exactly iters gradient-descent updates. Each update computes all three partial derivatives \partial E / \partial q_i at the current angles, then changes all three at once: q_i \leftarrow q_i - \alpha \, \partial E / \partial q_i. Return a tuple (angles, distance): the list of the three final angles, each wrapped into (-\pi, \pi] (add or subtract whole turns of 2\pi), and the distance from the final hand position to the target.

Examples

Input:  lengths = [1, 0.8, 0.5], target = (1.2, 1.0), start = (0.0, 0.3, 0.3),
        alpha = 0.1, mu = 0, iters = 200
Output: ([-0.14610516, 1.17894958, 0.94781365], 0.00001215)

Input:  lengths = [1, 0.8, 0.5], target = (1.2, 1.0), start = (1.0, -0.6, -0.6),
        alpha = 0.1, mu = 0, iters = 200
Output: ([1.49357787, -1.08533434, -1.08880592], 0.00000761)
Explanation: the same shelf corner, reached in a different pose from a different start.

Input:  lengths = [1, 0.8, 0.5], target = (1.2, 1.0), start = (0.0, 0.3, 0.3),
        alpha = 0.1, mu = 0.05, iters = 200
Output: ([-0.11150763, 1.13325206, 0.88636147], 0.06281008)
Explanation: the penalty holds the arm nearer its start, so it stops a little short.

Constraints

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

Goals

  • Build the $2 \times 3$ Jacobian of a three-link arm column by column
  • Run gradient descent on a reach error plus a penalty for leaving the start pose
  • See that a redundant arm reaches the same point in many poses, and the start decides which
Starting Python…