jiaodukongzhi4.m

机器人运行轨迹计算

clear;
d1=140;d2=255;d3=255;d4=65;
    m=0;
for kk=5:6
if kk==1
nx=0.2689;ny=-0.2551;nz=-0.9285;
ox=-0.6870;oy=-0.7267;oz=0;
ax=-0.6747;ay=0.6379;az=-0.3714;
px=320;py=-104;pz=20.0085;
elseif kk==2
nx=-0.2404;ny=0.2831;nz=-0.9285;
ox=0.7623;oy=0.6472;oz=0;
ax=0.6009;ay=-0.7078;az=-0.3714;
px=120;py=106;pz=50.8244;
elseif kk==3
nx=-0.0587;ny=-0.3667;nz=-0.9285;
ox=-0.9874;oy=0.1580;oz=0;
ax=0.1467;ay=0.9168;az=-0.3714;
px=190;py=-125;pz=81.9846;
elseif kk==4
nx=0.1691;ny=0.3307;nz=-0.9285;
ox=0.8903;oy=-0.4553;oz=0;
ax=-0.4227;ay=-0.8267;az=-0.3714;
px=255;py=88;pz=151.3637;
elseif kk==5
nx=0;ny=1;nz=0;
ox=-1;oy=0;oz=0;
ax=0;ay=0;az=1;
px=210;py=0;pz=180;
elseif kk==6
nx=0;ny=1;nz=0;
ox=-1;oy=0;oz=0;
ax=0;ay=0;az=1;
px=250;py=0;pz=115;
end
    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]=jiaodu4(px,py,pz,s1,s2,s3,s4,s5,nx,ny,nz,...
                                       ox,oy,oz,ax,ay,az);
%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)=kk;
   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('Pxyz3.txt','Pxyz','-ascii');
save('ST3.txt','ST','-ascii');
save('usen3.txt','usen','-ascii');