Problem 411045 · easy · Robotics · Level 04 Belief and Optimisation

Nudge Each Joint, Watch the Hand

Jacobian · central difference · numerical differentiation · forward kinematics

A greenhouse robot picks tomatoes with 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 picker (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).

The Jacobian J is the 2 \times 2 matrix of partial derivatives: row 0 holds \partial x / \partial t_1 and \partial x / \partial t_2, row 1 holds \partial y / \partial t_1 and \partial y / \partial t_2.

The firmware will later run on arms whose formula nobody wrote down, so it estimates J by central differences with a step h: to estimate column j, move joint j alone by +h and by -h, compute the hand position p^{+} and p^{-} both times, and take

\text{column } j \approx \frac{p^{+} - p^{-}}{2h} \quad \text{(for } x \text{ and } y \text{ separately).}

Before trusting it, you test it on this arm. Write jacobian_check(l1, l2, t1, t2, h) that returns a tuple (estimate, error):

  • estimate: the central-difference Jacobian, as [[dx_dt1, dx_dt2], [dy_dt1, dy_dt2]];
  • error: the largest absolute difference between an entry of estimate and the same entry of the exact Jacobian.

Examples

Input:  l1 = 2, l2 = 1, t1 = 0, t2 = math.pi / 2, h = 0.1
Output: ([[-0.99833417, -0.99833417], [1.99666833, 0.0]], 0.00333167)
Explanation: the exact Jacobian is [[-1, -1], [2, 0]]; the largest gap is in row 1, column 0.

Input:  l1 = 0.4, l2 = 0.3, t1 = 0.5, t2 = -1.2, h = 0.5
Output: ([[0.00143357, 0.18531265], [0.55659932, 0.22001093]], 0.02388636)

Constraints

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

Goals

  • Estimate each column of a Jacobian by nudging one joint both ways
  • Compare a numerical estimate with the exact derivatives entry by entry
  • See the estimate's error shrink as the step gets smaller
Starting Python…