jiaodukongzhi2.m

机器人运行轨迹计算

%SHUXUE
pp=load('data.txt');
nn=length(pp);m=0;
d1=140;d2=255;d3=255;d4=65;
for k=1:nn
    px=pp(k,1);
    py=pp(k,2);
    pz=pp(k,3);   
    for s1=0:1
        for s2=0:1
            for s3=0:1
                for s4=0:1
                     for s5=0:1
[st1,st2,st3,st4,st5]=jiaodu2(px,py,pz,s1,s2,s3,s4,s5);
if (-pi<st1&st1<pi&-125*pi/180<st2&st2<125*pi/180&-138*pi/180<st3&st3<138*pi/180 ...
    -3*pi/2<st4&st4<3*pi/2&-120*pi/180<st5&st5<133.5*pi/180)
T01=zeros(4,4);
T12=zeros(4,4);
T23=zeros(4,4);
T34=zeros(4,4);
T45=zeros(4,4);
T56=zeros(4,4);
st6=0.0;
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;

if sqrt((T06(1,4)-px)^2+(T06(2,4)-py)^2+(T06(3,4)-pz)^2)<20
    m=m+1;
    usen(m)=k;
   Pxyz(m,1)=T06(1,4);Pxyz(m,2)=T06(2,4);Pxyz(m,3)=T06(3,4);
   ST(m,1:6)=[st1,st2,st3,st4,st5,st6];
   continue
end
end
                     end
                end
            end
        end
    end
end
disp('jisuanwangbi')
save('Pxyz.txt','Pxyz','-ascii');
save('ST.txt','ST','-ascii');
save('usen.txt','usen','-ascii');