shuxue.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);
st1=90;
st2=-90;
st3=0;
st4=0;
st5=-90;
st6=0;
st1=st1*pi/180;
st2=st2*pi/180;
st3=st3*pi/180;
st4=st4*pi/180;
st5=st5*pi/180;
st6=st6*pi/180;
d1=140;
d2=255;
d3=255;
d4=65;
T01=[cos(st1+pi/2), sin(st1+pi/2),0,0;
     -sin(st1+pi/2),cos(st1+pi/2),0,0;
     0,0,1,0;
     0,0,0,1];
T12=[cos(st2),-sin(st2),0,0;
     0,0,-1,0;
     sin(st2),cos(st2),0,d1;
     0,0,0,1]; 
%T12=[1,0,0,0;
     %0,cos(st2),-sin(st2),-d1*sin(st2);
     %0,sin(st2),cos(st2),d1*cos(st2);
     %0,0,0,1];
T23=[1,0,0,0;
    0,cos(st3),-sin(st3),-d2*sin(st3);
    0,sin(st3),cos(st3),d2*cos(st3);
    0,0,0,1];
T34=[cos(st4),-sin(st4),0,0;
    sin(st4),cos(st4),0,0;
    0,0,1,d3;
    0,0,0,1];
T45=[1,0,0,0;
    0,cos(st5),-sin(st5),-d4*sin(st5);
    0,sin(st5),cos(st5),d4*cos(st5);
    0,0,0,1];
T56=[cos(st6),-sin(st6),0,0;
    sin(st6),cos(st6),0,0;
    0,0,1,0;
    0,0,0,1];
T06=T01*T12*T23*T34*T45*T56