A paint robot sprays car doors. Seen from the side it 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 spray nozzle (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
At this instant the shoulder angle is changing at w1 radians per second and the elbow angle t2 at w2 radians per second (positive is counter-clockwise; w2 is the rate of the relative angle).
Paint goes on evenly only when the nozzle moves at the right speed. Write hand_velocity(l1, l2, t1, t2, w1, w2) that returns a tuple (vx, vy, speed): the rates of change of x and y at this instant (length units per second), and the speed \sqrt{v_x^2 + v_y^2}.
Examples
Input: l1 = 2, l2 = 1, t1 = 0, t2 = math.pi / 2, w1 = 0.5, w2 = -1
Output: (0.5, 1.0, 1.11803399)
Explanation: the hand is at (2, 1). The shoulder alone would move it at
0.5 * (-1, 2) = (-0.5, 1); the elbow, turning clockwise, adds (1, 0).
Input: l1 = 0.4, l2 = 0.3, t1 = 0.5, t2 = -1.2, w1 = 0.8, w2 = 1.5
Output: (0.29109403, 0.80856753, 0.85937023)
Input: l1 = 1, l2 = 1, t1 = 0.3, t2 = 0.4, w1 = 0, w2 = 0
Output: (0.0, 0.0, 0.0)
Constraints
- answers are compared with a tolerance of
1e-6; do not round
Goals
- Turn joint speeds into the hand's velocity with the Jacobian
- Multiply a $2 \times 2$ matrix by a vector by hand
- Find the speed as the length of the velocity vector