Robotics Fundamentals 1, Chapter 9: Challenge 2 solution (Jacobian, singularities and static torque)

Formulas: J = [ [ -L1 s1 - L2 s12 , -L2 s12 ] , [ L1 c1 + L2 c12 , L2 c12 ] ] ,  s1 = sin t1, s12 = sin( t1 + t2 ), c1 = cos t1, c12 = cos( t1 + t2 )
          v = J x theta_dot ;  det J = L1 L2 sin t2 ;  theta_dot = J^-1 v ;  tau = J^T F  (F = force the hand exerts on the world)
          moment check:  tau1 = x Fy - y Fx   (about the shoulder) ,   tau2 = ( x - Ex ) Fy - ( y - Ey ) Fx   (about the elbow)

L1 = 0.25 m,  L2 = 0.15 m,  t1 = 25 deg,  t2 = 70 deg  (so link 2 points at 95 deg).
cos 25 = 0.9063   sin 25 = 0.4226   cos 95 = -0.0872   sin 95 = 0.9962

(a) Elbow:  E = ( 0.25 x 0.9063 , 0.25 x 0.4226 ) = ( 0.2266 , 0.1057 ) m.
    Hand:   x = 0.2266 + 0.15 x ( -0.0872 ) = 0.2266 - 0.0131 = 0.2135 m       y = 0.1057 + 0.15 x 0.9962 = 0.1057 + 0.1494 = 0.2551 m
    The hand is at ( 0.2135 , 0.2551 ) m, 0.3326 m from the shoulder.

(b) J11 = -( 0.25 x 0.4226 + 0.15 x 0.9962 ) = -( 0.10565 + 0.14943 ) = -0.2551          J12 = -0.15 x 0.9962 = -0.1494
    J21 =    0.25 x 0.9063 + 0.15 x ( -0.0872 ) =  0.22658 - 0.01307  =  0.2135          J22 =  0.15 x ( -0.0872 ) = -0.0131
    J = [ [ -0.2551 , -0.1494 ] , [ 0.2135 , -0.0131 ] ]
    det J from the matrix:  ( -0.2551 )( -0.0131 ) - ( -0.1494 )( 0.2135 ) = 0.003335 + 0.031902 = 0.03524
    det J from the formula: L1 L2 sin t2 = 0.25 x 0.15 x sin 70 = 0.0375 x 0.9397 = 0.03524.     The two agree.   It is not near zero, so the arm is not near a singular pose.

(c) t1_dot = 0.4 rad/s,  t2_dot = 0.6 rad/s:
        vx = -0.2551 x 0.4 + ( -0.1494 ) x 0.6 = -0.10203 - 0.08966 = -0.1917 m/s
        vy =  0.2135 x 0.4 + ( -0.0131 ) x 0.6 =  0.08540 - 0.00785 =  0.0776 m/s
    Hand speed = sqrt( 0.03674 + 0.00601 ) = 0.2068 m/s, moving up and to the LEFT (the shoulder is turning the arm up while the elbow bends it further over).

(d) F = ( -2 , -6 ) N  (the hand pushes left and down).   tau = J^T F:
        tau1 = J11 Fx + J21 Fy = ( -0.2551 )( -2 ) + ( 0.2135 )( -6 ) = 0.5102 - 1.2810 = -0.7709 N m
        tau2 = J12 Fx + J22 Fy = ( -0.1494 )( -2 ) + ( -0.0131 )( -6 ) = 0.2989 + 0.0784 = +0.3773 N m
    Check by moments:
        about the shoulder:  tau1 = x Fy - y Fx = 0.2135 x ( -6 ) - 0.2551 x ( -2 ) = -1.2810 + 0.5102 = -0.7709 N m          matches.
        about the elbow:     tau2 = ( 0.2135 - 0.2266 ) x ( -6 ) - ( 0.2551 - 0.1057 ) x ( -2 ) = ( -0.0131 )( -6 ) - ( 0.1494 )( -2 ) = 0.0784 + 0.2989 = +0.3773 N m     matches.
    The shoulder must supply 0.771 N m clockwise and the elbow 0.377 N m counter-clockwise.

(e) The hand moves along the line from the shoulder at 0.04 m/s, so v = 0.04 x ( x/r , y/r ).  Solve theta_dot = J^-1 v, with  J^-1 = ( 1 / det J ) x [ [ J22 , -J12 ] , [ -J21 , J11 ] ].

    t2 = 70 deg:   r = 0.3326,  direction ( 0.6418 , 0.7668 ),  v = ( 0.02567 , 0.03067 ) m/s,  det J = 0.03524
        t1_dot = ( J22 vx - J12 vy ) / det = ( -0.0131 x 0.02567 + 0.1494 x 0.03067 ) / 0.03524 = ( -0.000336 + 0.004582 ) / 0.03524 = +0.1205 rad/s
        t2_dot = ( -J21 vx + J11 vy ) / det = ( -0.2135 x 0.02567 - 0.2551 x 0.03067 ) / 0.03524 = ( -0.005481 - 0.007824 ) / 0.03524 = -0.3776 rad/s

    t2 = 8 deg:   J = [ [ -0.1874 , -0.0817 ] , [ 0.3524 , 0.1258 ] ],  r = 0.3991,  direction ( 0.8830 , 0.4694 ),  v = ( 0.03532 , 0.01878 ) m/s,  det J = 0.0375 x sin 8 = 0.005219
        t1_dot = ( 0.1258 x 0.03532 + 0.0817 x 0.01878 ) / 0.005219 = ( 0.004443 + 0.001534 ) / 0.005219 = +1.145 rad/s
        t2_dot = ( -0.3524 x 0.03532 - 0.1874 x 0.01878 ) / 0.005219 = ( -0.012447 - 0.003519 ) / 0.005219 = -3.059 rad/s

    The same slow 0.04 m/s needs 0.38 rad/s of elbow speed at t2 = 70 deg, but 3.06 rad/s (8 times as much) at t2 = 8 deg, where det J is 7 times smaller.  Near the stretched singular pose the arm is
    almost unable to move outward, and the joints must turn very fast to move the hand slowly in that direction; at t2 = 0 exactly, the required speed would be infinite.

(f) det J = L1 L2 sin t2 is zero when sin t2 = 0:  t2 = 0 and t2 = 180 deg.
        t2 = 0:    the arm is fully STRETCHED, both links in one line, the hand at the outer edge of the workspace, r = L1 + L2 = 0.40 m.
        t2 = 180:  the arm is fully FOLDED, link 2 lying back along link 1, the hand at the inner edge, r = | L1 - L2 | = 0.10 m.
    In both poses the hand cannot move along the line of the links, however fast the joints turn (it can only move across it).