rotacion_matriz.m

clear,close, clc;

syms q1 q2 q3 q4 q5 q6 ...
     d1 d2 d3 d4 d5 d6...
     a1 a2 a3 a4 a5 a6 ...
     alp1 alp2 alp3 alp4 alp5 alp5...
     L1 L2 L3 L4 L5 L6 pi...
     R11 R12 R13 R21 R22 R23 R31 R32 R33;
 
 
 T1=q1;     T2=q2;    T3=q3;  r11=R11;  r12=R12;   r13=R13; 
 d1=L1;     d2=0;      d3=0;  r21=R21;  r22=R22;   r23=R23;
 a1=0;      a2=L2;     a3=0;  r31=R13;  r32=R23;   r33=R33;
 alp1=90;   alp2=0;  alp3=-90;
 
 R1=[cos(T1)     -cosd(alp1)*sin(T1)    sind(alp1)*sin(T1);...
    sin(T1)      cosd(alp1)*cos(T1)   -sind(alp1)*cos(T1);...
      0              sind(alp1)          cosd(alp1)     ];
  
  

R2=[cos(T2)     -cosd(alp2)*sin(T2)    sind(alp2)*sin(T2);...
    sin(T2)      cosd(alp2)*cos(T2)   -sind(alp2)*cos(T2);...
      0              sind(alp2)          cosd(alp2)     ];
 
  
R3=[cos(T3)     -cosd(alp3)*sin(T3)    sind(alp3)*sin(T3);...
    sin(T3)      cosd(alp3)*cos(T3)   -sind(alp3)*cos(T3);...
      0              sind(alp3)          cosd(alp3)     ];

R06=[r11   r12  r13; ...
     r21   r22  r23;...
     r31   r32  r33 ];
   
R30=R1*R2*R3
R30=simplify(R30)
RT=transpose(R30)
R3_0=RT*R06