A sorting 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 gripper (the hand), lengths in millimetres. 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
The joint encoders are not perfect: the true shoulder angle is t1 plus an error with mean 0 and standard deviation s1, and the true elbow angle is t2 plus an independent error with mean 0 and standard deviation s2 (both in radians). The errors are small, so it is accurate enough to treat the hand position as changing linearly with them, with the slopes given by the partial derivatives at (t1, t2) (the first-order approximation).
Write hand_uncertainty(l1, l2, t1, t2, s1, s2) that returns a tuple (sx, sy, cxy) under that approximation:
sx,sy: the standard deviations of the hand's x and y (mm);cxy: the covariance of x and y (mm²): the mean of the product of their deviations from their means. A negative value means that when x comes out too large, y tends to come out too small.
Examples
Input: l1 = 200, l2 = 100, t1 = 0, t2 = math.pi / 2, s1 = 0.01, s2 = 0.02
Output: (2.23606798, 2.0, -2.0)
Explanation: near this pose a shoulder error e1 moves the hand by (-100, 200) * e1,
an elbow error e2 by (-100, 0) * e2. So the variance of x is 100² * 0.01² + 100² * 0.02² = 5.
Input: l1 = 400, l2 = 300, t1 = 0.5, t2 = -1.2, s1 = 0.002, s2 = 0.003
Output: (0.57980363, 1.34970041, 0.40257866)
Constraints
- answers are compared with a tolerance of
1e-6; do not round
Goals
- Propagate independent joint-angle errors to the hand through the Jacobian
- Add variances, not standard deviations, of independent contributions
- Compute the covariance between the hand's $x$ and $y$ errors