shuxuefuhao.m

机器人运行轨迹计算

%SHUXUE
T01=zeros(4,4);
T12=zeros(4,4);
T23=zeros(4,4);
T34=zeros(4,4);
T45=zeros(4,4);
T56=zeros(4,4);
syms st1 st2 st3 st4 st5 st6 d1 d2 d3 d4 
T01=[cos(st1+pi/2), -sin(st1+pi/2),0,0;
     sin(st1+pi/2),cos(st1+pi/2),0,0;
     0,0,1,d1;
     0,0,0,1];
T12=[cos(st2+pi/2),-sin(st2+pi/2),0,0;
     0,0,-1,0;
     sin(st2+pi/2),cos(st2+pi/2),0,0;
     0,0,0,1]; 
T23=[cos(st3+pi/2),-sin(st3+pi/2),0,d2;
    sin(st3+pi/2),cos(st3+pi/2),0,0;
    0,0,1,0;
    0,0,0,1];
T34=[cos(st4),-sin(st4),0,0;
    0,0,1,-d3;
    -sin(st4),-cos(st4),0,0;
    0,0,0,1];
T45=[cos(st5),-sin(st5),0,0;
    0,0,-1,0;
    sin(st5),cos(st5),0,0;
    0,0,0,1];
T56=[cos(st6),-sin(st6),0,0;
    0,0,1,-d4;
    -sin(st6),-cos(st6),0,0;    
    0,0,0,1];

T06=T01*T12*T23*T34*T45*T56

syms nx ny nz ox oy oz ax ay az px py pz
TT=[nx,ox,ax,px;ny,oy,ay,py;nz,oz,az,pz;0,0,0,1];
invT01=inv(T01);
invT02=inv(T01*T12);
invT03=inv(T01*T12*T23);
st6=0;
invT56=inv(subs(T56));
TL=invT01*TT*invT56;
TR=T12*T23*T34*T45;
TL2=invT02*TT*invT56;
TR2=T23*T34*T45;
TL4=invT03*TT*invT56;
TR4=T34*T45;