clear;clf;%stairstairs(-0.2:0.4:1,fliplr(0:0.1:0.3));hold on;line([1;1.6],[0;0]);hold on;%data of the robotD_x=0.4;D_z=0.1;L_th=0.4;L_sh=0.36;L_bd=0.5;H_f=0.04;L_a=0.15;L_b=0.05;fai_b=10/180*pi;fai_e=10/180*pi;%first step%ankle    fai=[0,0,0,fai_e,0];x01=L_a*(1-cos(fai_b))+H_f*sin(fai_b);z01=3*D_z+L_a*sin(fai_b)+H_f*cos(fai_b);x02=D_x-L_a*(1-cos(fai_e))-H_f*sin(fai_e);z02=2*D_z+L_a*sin(fai_e)+H_f*cos(fai_e);xa0=[0,x01,D_x/2,x02,D_x];za0=[H_f+3*D_z,z01,H_f+4*D_z,z02,H_f+2*D_z];xa1=zeros(1,5);za1=H_f+3*D_z*ones(1,5);%hipxh=D_x/2*[0,0.1,0.4,0.83,1];zh=0.7+3*D_z-D_z/2*[0,0.1,0.4,0.83,1];drawrobot;%middle steps%ankle    for k=0:1fai=[0,fai_b,10/180*pi,fai_e,0];x01=L_a*(1-cos(fai_b))+H_f*sin(fai_b);z01=3*D_z+L_a*sin(fai_b)+H_f*cos(fai_b);x02=2*D_x-L_a*(1-cos(fai_e))-H_f*sin(fai_e);z02=D_z+L_a*sin(fai_e)+H_f*cos(fai_e);xa0=k*D_x+[0,x01,D_x,x02,2*D_x];za0=[H_f+3*D_z,z01,H_f+4*D_z,z02,H_f+D_z]-k*D_z;xa1=(k+1)*D_x*ones(1,5);za1=H_f+(2-k)*D_z*ones(1,5);%hipxh=D_x/2+k*D_x+D_x*[0,0.1,0.45,0.88,1];zh=0.7+5/2*D_z-k*D_z-D_z*[0,0.1,0.45,0.88,1];drawrobot;end%last step%ankle    x01=L_a*(1-cos(fai_b))+H_f*sin(fai_b);z01=D_z+L_a*sin(fai_b)+H_f*cos(fai_b);x02=D_x-L_a*(1-cos(fai_e))-H_f*sin(fai_e);z02=L_a*sin(fai_e)+H_f*cos(fai_e);xa0=2*D_x+[0,x01,D_x/2,x02,D_x];za0=[H_f+D_z,z01,H_f+2*D_z,z02,H_f];xa1=3*D_x*ones(1,5);za1=H_f*ones(1,5);%hipxh=5/2*D_x+D_x/2*[0,0.2,0.63,0.92,1];zh=0.7+D_z/2-D_z/2*[0,0.2,0.63,0.92,1];drawrobot;axis equal;axis([-0.2,1.6,0,1.6]);grid on;title('下樓梯桿狀圖');xlabel('Y');ylabel('Z');