Robotics Fundamentals 1, Chapter 9: Challenge 1 solution (forward and inverse kinematics)

Formulas: x = L1 cos t1 + L2 cos( t1 + t2 ) ,  y = L1 sin t1 + L2 sin( t1 + t2 )
          reachable only if | L1 - L2 | <= r <= L1 + L2 ,  r = sqrt( x^2 + y^2 )
          cos t2 = ( r^2 - L1^2 - L2^2 ) / ( 2 L1 L2 ) ,  t2 = +acos (elbow-down) or -acos (elbow-up)
          t1 = atan2( y, x ) - atan2( L2 sin t2 , L1 + L2 cos t2 )
          with a wrist link L3 and hand orientation phi:  W = ( x - L3 cos phi , y - L3 sin phi ) , solve the 2-link problem for W, then t3 = phi - t1 - t2

L1 = 0.25 m,  L2 = 0.15 m.

(a) t1 = 50 deg,  t2 = -70 deg  (so link 2 points at 50 - 70 = -20 deg).
        cos 50 = 0.6428   sin 50 = 0.7660   cos( -20 ) = 0.9397   sin( -20 ) = -0.3420
        x = 0.25 x 0.6428 + 0.15 x 0.9397 = 0.16070 + 0.14095 = 0.3017 m
        y = 0.25 x 0.7660 + 0.15 x ( -0.3420 ) = 0.19151 - 0.05130 = 0.1402 m
    The hand is at ( 0.302, 0.140 ) m, a distance sqrt( 0.0910 + 0.0197 ) = 0.333 m from the shoulder.
    t2 is NEGATIVE, so the elbow bends clockwise and sits above the line from the shoulder to the hand: an ELBOW-UP pose.

(b) Reach:  | 0.25 - 0.15 | = 0.10 m  to  0.25 + 0.15 = 0.40 m.
        ( 0.30, 0.20 ):  r = sqrt( 0.09 + 0.04 ) = 0.3606 m.  0.10 <= 0.3606 <= 0.40:  REACHABLE.
        ( 0.45, 0.05 ):  r = sqrt( 0.2025 + 0.0025 ) = 0.4528 m.  More than 0.40:  UNREACHABLE (too far; the arm is too short).
        ( 0.06, 0.04 ):  r = sqrt( 0.0036 + 0.0016 ) = 0.0721 m.  Less than 0.10:  UNREACHABLE (inside the hole; the arm cannot fold that tightly).

(c) Target ( 0.30, 0.20 ):   r^2 = 0.13.
        cos t2 = ( 0.13 - 0.0625 - 0.0225 ) / ( 2 x 0.25 x 0.15 ) = 0.045 / 0.075 = 0.6       so  t2 = +/- 53.13 deg  (a 3-4-5 triangle:  sin t2 = +/- 0.8)
        atan2( 0.20, 0.30 ) = 33.69 deg.

    ELBOW-DOWN  ( t2 = +53.13 deg ):
        atan2( 0.15 x 0.8 , 0.25 + 0.15 x 0.6 ) = atan2( 0.12 , 0.34 ) = 19.44 deg        t1 = 33.69 - 19.44 = 14.25 deg
        Check by FK:  elbow = ( 0.25 cos 14.25 , 0.25 sin 14.25 ) = ( 0.2423 , 0.0615 );   link 2 points at 14.25 + 53.13 = 67.38 deg:  ( 0.15 cos 67.38 , 0.15 sin 67.38 ) = ( 0.0577 , 0.1385 )
        Hand = ( 0.2423 + 0.0577 , 0.0615 + 0.1385 ) = ( 0.3000 , 0.2000 )   correct.

    ELBOW-UP  ( t2 = -53.13 deg ):
        atan2( 0.15 x ( -0.8 ) , 0.34 ) = atan2( -0.12 , 0.34 ) = -19.44 deg              t1 = 33.69 + 19.44 = 53.13 deg
        Check by FK:  elbow = ( 0.25 cos 53.13 , 0.25 sin 53.13 ) = ( 0.1500 , 0.2000 );   link 2 points at 53.13 - 53.13 = 0 deg:  ( +0.1500 , 0.0000 )
        Hand = ( 0.3000 , 0.2000 )   correct.

(d) L3 = 0.08 m,  hand at ( 0.32, 0.10 ),  phi = -30 deg.
    Wrist point:   W = ( 0.32 - 0.08 cos( -30 ) , 0.10 - 0.08 sin( -30 ) ) = ( 0.32 - 0.0693 , 0.10 + 0.04 ) = ( 0.2507 , 0.1400 ) m
    r^2 = 0.2507^2 + 0.14^2 = 0.06285 + 0.0196 = 0.08245,  r = 0.2872 m;   0.10 <= 0.2872 <= 0.40: reachable.
    cos t2 = ( 0.08245 - 0.0625 - 0.0225 ) / 0.075 = -0.00255 / 0.075 = -0.0339       t2 = +91.94 deg   (elbow-down: the positive solution)
    atan2( 0.14 , 0.2507 ) = 29.18 deg.     atan2( 0.15 sin 91.94 , 0.25 + 0.15 cos 91.94 ) = atan2( 0.14992 , 0.24491 ) = 31.47 deg
    t1 = 29.18 - 31.47 = -2.29 deg
    t3 = phi - t1 - t2 = -30 - ( -2.29 ) - 91.94 = -119.65 deg
    Check by FK:  elbow = ( 0.25 cos( -2.29 ) , 0.25 sin( -2.29 ) ) = ( 0.2498 , -0.0100 );   link 2 points at -2.29 + 91.94 = 89.65 deg:  ( +0.0009 , +0.1500 ), so the wrist is at ( 0.2507 , 0.1400 ) as required.
          link 3 points at 89.65 - 119.65 = -30 deg:  ( 0.08 cos( -30 ) , 0.08 sin( -30 ) ) = ( +0.0693 , -0.0400 ).    Hand = ( 0.3200 , 0.1000 )   correct,  orientation phi = -30 deg.
    ( -2.29 + 91.94 - 119.65 = -30 deg: the three joint angles add up to the wanted orientation.)