clear;clf;%stairstairs(-0.1:0.4:1.1,0:0.1:0.3);hold on;line([1.1;1.6],[0.3;0.3]);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=15/180*pi;fai_e=15/180*pi;%inital step%ankle    fai=[0,0,0,-fai_e,0];x01=L_a*(1-cos(fai_b))+H_f*sin(fai_b);z01=L_a*sin(fai_b)+H_f*cos(fai_b);x02=D_x-L_b*(1-cos(fai_e))-H_f*sin(fai_e);z02=D_z+L_b*sin(fai_e)+H_f*cos(fai_e);xa0=[0,x01,D_x/2,x02,D_x];za0=[H_f,z01,H_f+2*D_z,z02,H_f+D_z];xa1=zeros(1,5);za1=H_f*ones(1,5);%hipxh=D_x/2*[0,0.08,0.35,0.8,1];zh=0.7*ones(1,5);drawrobot;%middle steps%ankle    for k=0:1fai=[0,fai_b,0,-fai_e,0];x01=L_a*(1-cos(fai_b))+H_f*sin(fai_b);z01=L_a*sin(fai_b)+H_f*cos(fai_b);x02=2*D_x-L_b*(1-cos(fai_e))-H_f*sin(fai_e);z02=2*D_z+L_b*sin(fai_e)+H_f*cos(fai_e);xa0=k*D_x+[0,x01,D_x,x02,2*D_x];za0=k*D_z+[H_f,z01,H_f+3*D_z,z02,H_f+2*D_z];xa1=(k+1)*D_x*ones(1,5);za1=H_f+(k+1)*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+k*D_z+D_z*[0,0.1,0.45,0.88,1];drawrobot;end%last step%anklefai=[0,fai_b,0,0,0];x01=L_a*(1-cos(fai_b))+H_f*sin(fai_b);z01=L_a*sin(fai_b)+H_f*cos(fai_b);x02=D_x-L_b*(1-cos(fai_e))-H_f*sin(fai_e);z02=D_z+L_b*sin(fai_e)+H_f*cos(fai_e);xa0=2*D_x+[0,x01,D_x/2,x02,D_x];za0=2*D_z+[H_f,z01,H_f+2*D_z,z02,H_f+D_z];xa1=3*D_x*ones(1,5);za1=H_f+3*D_z*ones(1,5);%hipxh=5/2*D_x+D_x/2*[0,0.15,0.45,0.91,1];zh=0.7+2*D_z+D_z*[0,0.15,0.45,0.91,1];drawrobot;axis equal;axis([-0.2,1.6,0,1.6]);grid on;title('上樓梯桿狀圖');xlabel('Y');ylabel('Z');