\(\(alpha0\  = \ 0; \ a0\  = \ 0; \ d1\  = \ 0; 
alpha1\  = \ \(-Pi\)/2; 
    \ a1\  = \ 0; \ d2\  = \ 0; 
alpha2\  = \ 0; \ a2\  = \ 2; \ 
    d3\  = \  .5; 
alpha3\  = \ \(-Pi\)/2; \ a3\  = \  .16666; \ d4\  = \ 2; 
    
alpha4\  = \ Pi/2; \ a4\  = \ 0; \ d5 = \ 0; 
alpha5\  = \ \(-Pi\)/2; 
    \ a5\  = \ 0; \ d6\  = \ 0; 

x\  = \ 1; 
y\  = \ 1; 
z\  = \ \(-1\); 
    
r\  = \ 3; 
\)\)

\(\(

A3\  = \ \(-2\)\ a2\ d4; 
B3\  = \ 2\ a2\ a3; 

    C3\  = \ r\  - \ a3^2\  - \ d4^2\  - \ a2^2\  - \ d3^2; 


    u31 = \ \((A3\  + Sqrt[A3^2\  - \ C3^2\  + \ B3^2])\)/\((C3\  + \ B3)\); 
    
u32 = \ \((A3\  - Sqrt[A3^2\  - \ C3^2\  + \ B3^2])\)/
        \((C3\  + \ B3)\); 

t31\  = \ 2\ ArcTan[u31]; 

    t32\  = \ 2\ ArcTan[u32]; 

t3\  = \ t31; 


    f1\  = \ a3\ Cos[t3]\  + \ d4\ Sin[alpha3]\ Sin[t3]\  + \ a2; 

    f2\  = \ a3\ Cos[alpha2]\ Sin[t3]\  - \ 
        d4\ Sin[alpha3]\ Cos[alpha2]\ Cos[t3]\  - \ 
        d4\ Sin[alpha2]\ Cos[alpha2]\  - \ d3\ Sin[alpha2]; 

    f3\  = \ a3\ Sin[alpha2]\ Sin[t3]\  - \ 
        d4\ Sin[alpha3]\ Sin[alpha2]\ Cos[t3]\  + \ 
        d4\ Cos[alpha2]\ Cos[alpha3]\  + \ d3\ Cos[alpha2]; 

k1\  = \ f1; 
    
k2\  = \ \(-f2\); 

    k3\  = \ f1^2\  + \ f2^2\  + \ f3^2\  + \ a1^2\  + \ d2^2\  + \ 
        2\ d2\ f3; 
k4\  = \ f3\ Cos[alpha1]\  + \ d2\ Cos[alpha1]; 

    A2\  = \ \(-k1\); 
B2\  = \ k2; 
C2\  = \ z; 


    u21\  = \ \((A2\  + \ Sqrt[A2^2\  - \ C2^2\  + \ B2^2])\)/
        \((C2\  + \ B2)\); 

    u22\  = \ \((A2\  - \ Sqrt[A2^2\  - \ C2^2\  + \ B2^2])\)/
        \((C2\  + \ B2)\); 
t21\  = \ 2\ ArcTan[u21]; 

    t22\  = \ 2\ ArcTan[u22]; 

t2\  = \ t21; 

    g1\  = \ Cos[t2]\ f1\  - \ Sin[t2]\ f2\  + \ a1; 

    g2\  = \ Sin[t2]\ Cos[alpha1]\ f1\  + \ Cos[t2]\ Cos[alpha1]\ f2\  - \ 
        Sin[alpha1]\ f3\  - \ d2\ Sin[alpha1]; 

    g3\  = \ Sin[t2]\ Sin[alpha1]\ f1\  + \ Cos[t2]\ Sin[alpha1]\ f2\  + 
        Cos[alpha1]\ f3\  + \ d2\ Cos[alpha1]; 


    t1\  = \ ArcTan[\((x\ g1\  + \ y\ g2)\)/\((g1^2\  + \ g2^2)\), 
        \((y\ g1\  - \ x\ g2)\)/\((g1^2\  + \ g2^2)\)]; 

    Print["For theta3 = ", \ t3*180/Pi, \ " Theta2 = ", \ t2*180/Pi, 
      \ " Theta1 = ", \ t1*180/Pi]; 

t2\  = \ t22; 

    g1\  = \ Cos[t2]\ f1\  - \ Sin[t2]\ f2\  + \ a1; 

    g2\  = \ Sin[t2]\ Cos[alpha1]\ f1\  + \ Cos[t2]\ Cos[alpha1]\ f2\  - \ 
        Sin[alpha1]\ f3\  - \ d2\ Sin[alpha1]; 

    g3\  = \ Sin[t2]\ Sin[alpha1]\ f1\  + \ Cos[t2]\ Sin[alpha1]\ f2\  + 
        Cos[alpha1]\ f3\  + \ d2\ Cos[alpha1]; 


    t1\  = \ ArcTan[\((x\ g1\  + \ y\ g2)\)/\((g1^2\  + \ g2^2)\), 
        \((y\ g1\  - \ x\ g2)\)/\((g1^2\  + \ g2^2)\)]; 

    Print["For theta3 = ", \ t3*180/Pi, \ " Theta2 = ", \ t2*180/Pi, 
      \ " Theta1 = ", \ t1*180/Pi]; 


t3\  = \ t32; 


    f1\  = \ a3\ Cos[t3]\  + \ d4\ Sin[alpha3]\ Sin[t3]\  + \ a2; 

    f2\  = \ a3\ Cos[alpha2]\ Sin[t3]\  - \ 
        d4\ Sin[alpha3]\ Cos[alpha2]\ Cos[t3]\  - \ 
        d4\ Sin[alpha2]\ Cos[alpha2]\  - \ d3\ Sin[alpha2]; 

    f3\  = \ a3\ Sin[alpha2]\ Sin[t3]\  - \ 
        d4\ Sin[alpha3]\ Sin[alpha2]\ Cos[t3]\  + \ 
        d4\ Cos[alpha2]\ Cos[alpha3]\  + \ d3\ Cos[alpha2]; 




    k1\  = \ f1; 
k2\  = \ \(-f2\); 

    k3\  = \ f1^2\  + \ f2^2\  + \ f3^2\  + \ a1^2\  + \ d2^2\  + \ 
        2\ d2\ f3; 
k4\  = \ f3\ Cos[alpha1]\  + \ d2\ Cos[alpha1]; 

    A2\  = \ \(-k1\); 
B2\  = \ k2; 
C2\  = \ z; 


    u21\  = \ \((A2\  + \ Sqrt[A2^2\  - \ C2^2\  + \ B2^2])\)/
        \((C2\  + \ B2)\); 

    u22\  = \ \((A2\  - \ Sqrt[A2^2\  - \ C2^2\  + \ B2^2])\)/
        \((C2\  + \ B2)\); 
t21\  = \ 2\ ArcTan[u21]; 

    t22\  = \ 2\ ArcTan[u22]; 
t2\  = \ t21; 

    g1\  = \ Cos[t2]\ f1\  - \ Sin[t2]\ f2\  + \ a1; 

    g2\  = \ Sin[t2]\ Cos[alpha1]\ f1\  + \ Cos[t2]\ Cos[alpha1]\ f2\  - \ 
        Sin[alpha1]\ f3\  - \ d2\ Sin[alpha1]; 

    g3\  = \ Sin[t2]\ Sin[alpha1]\ f1\  + \ Cos[t2]\ Sin[alpha1]\ f2\  + 
        Cos[alpha1]\ f3\  + \ d2\ Cos[alpha1]; 


    t1\  = \ ArcTan[\((x\ g1\  + \ y\ g2)\)/\((g1^2\  + \ g2^2)\), 
        \((y\ g1\  - \ x\ g2)\)/\((g1^2\  + \ g2^2)\)]; 

    Print["For theta3 = ", \ t3*180/Pi, \ " Theta2 = ", \ t2*180/Pi, 
      \ " Theta1 = ", \ t1*180/Pi]; 

t2\  = \ t22; 

    g1\  = \ Cos[t2]\ f1\  - \ Sin[t2]\ f2\  + \ a1; 

    g2\  = \ Sin[t2]\ Cos[alpha1]\ f1\  + \ Cos[t2]\ Cos[alpha1]\ f2\  - \ 
        Sin[alpha1]\ f3\  - \ d2\ Sin[alpha1]; 

    g3\  = \ Sin[t2]\ Sin[alpha1]\ f1\  + \ Cos[t2]\ Sin[alpha1]\ f2\  + 
        Cos[alpha1]\ f3\  + \ d2\ Cos[alpha1]; 


    t1\  = \ 
      ArcTan[\((x\ g1\  + \ y\ g2)\)/\((g1^2\  + \ g2^2)\), 
        \((y\ g1\  - \ x\ g2)\)/\((g1^2\  + \ g2^2)\)]; 

    Print["For theta3 = ", \ t3*180/Pi, \ " Theta2 = ", \ t2*180/Pi, 
      \ " Theta1 = ", \ t1*180/Pi]; \)\)

\(\ << forward.m; 
t1\  = \ 24.2*Pi/180; 
t2\  = \ 102.9*Pi/180; 

    t3\  = \ 143.7*Pi/180; 
 (*\ Clear[t1, t2, t3]; \ *) 

    R03\  = \ Drop[
        Transpose[
          Drop[Transpose[
              T[alpha0, a0, d1, t1]\  . \ T[alpha1, a1, d2, t2]\ \  . 
                T[alpha2, a2, d3, t3]\ ], \(-1\)]], \(-1\)]\)\(R06\  = 
      \ {{\(-1\)/Sqrt[2.0], 0, 1/Sqrt[2.0]}, {0, 1, 0}, {1/Sqrt[2.0], 0, 
          1/Sqrt[2.0]}}; 
R36\  = \ Transpose[R03]\  . \ R06
\)t5  =  ArcCos[R36[[2]][[3]]]*180/Pit4  =  
      ArcTan[R36[[3]][[3]]/Sin[t5*Pi/180], 
          -R36[[1]][[3]]/Sin[t5*Pi/180]]*180/Pi\(t6\  = \ 
      ArcTan[\(-\(R36[\([2]\)]\)[\([2]\)]\)/Sin[t5*Pi/180], 
          \(R36[\([2]\)]\)[\([1]\)]/Sin[t5*Pi/180]]*180/Pi
\)