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

Steering the Icing Nozzle

Jacobian · 2x2 inverse · singularity · velocity control · saturation

A cake-decorating robot pipes icing 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 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

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).

If the joint angles change at w1 and w2 radians per second, the hand moves with velocity J \begin{pmatrix} w_1 \\ w_2 \end{pmatrix}, where J is the Jacobian: the 2 \times 2 matrix with rows (\partial x / \partial t_1, \partial x / \partial t_2) and (\partial y / \partial t_1, \partial y / \partial t_2).

The pattern needs the nozzle to move at the velocity (vx, vy) right now. Write joint_speeds(l1, l2, t1, t2, vx, vy, w_max, tol) that returns the joint speeds as a tuple (w1, w2):

  1. If |\det J| \le tol, the pose is (nearly) singular and the needed speeds would be huge or not exist: return None.
  2. Otherwise take the speeds (w1, w2) that give exactly the velocity (vx, vy).
  3. Each motor turns at most w_max rad/s in either direction. If max(|w1|, |w2|) > w_max, multiply both speeds by the same factor so that the larger one becomes exactly w_max (the nozzle keeps its direction and just moves slower). Otherwise return them unchanged.

Examples

Input:  l1 = 2, l2 = 1, t1 = 0, t2 = math.pi / 2, vx = 0.5, vy = 1, w_max = 2, tol = 1e-6
Output: (0.5, -1.0)

Input:  l1 = 2, l2 = 1, t1 = 0, t2 = math.pi / 2, vx = 3, vy = 2, w_max = 2, tol = 1e-6
Output: (0.5, -2.0)
Explanation: the velocity (3, 2) needs the speeds (1, -4). The elbow would be twice
over the limit, so both speeds are halved.

Input:  l1 = 1, l2 = 1, t1 = 0.4, t2 = 0.0, vx = 0.1, vy = 0, w_max = 5, tol = 1e-6
Output: None
Explanation: the arm is stretched straight; det J = 0.

Constraints

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

Goals

  • Solve $J \dot q = v$ for the joint speeds with the $2 \times 2$ inverse
  • Refuse to solve near a singular pose, where the determinant is almost zero
  • Scale both joint speeds together when a motor would exceed its limit
Starting Python…