drawrobot.m

来自「二维机器人上楼梯运动的步态规划,以及稳定性分析,各关节的运动轨迹都可以直观的表现」· M 代码 · 共 27 行

%knee
L_ha0=sqrt((xa0-xh).^2+(za0-zh).^2);
L_ha1=sqrt((xa1-xh).^2+(za1-zh).^2);
theta01=asin((xh-xa0)./L_ha0);
theta02=acos((L_ha0.^2+L_sh^2-L_th^2)./(2*L_sh.*L_ha0));
xk0=xa0+L_sh*sin(theta01+theta02);
zk0=za0+L_sh*cos(theta01+theta02);
theta11=asin((xh-xa1)./L_ha1);
theta12=acos((L_ha1.^2+L_sh^2-L_th^2)./(2*L_sh.*L_ha1));
xk1=xa1+L_sh*sin(theta11+theta12);
zk1=za1+L_sh*cos(theta11+theta12);
%body
line([xh;xh],[zh;zh+L_bd],'Color','k');hold on;
%thigh
line([xh;xk0],[zh;zk0],'Color','k');hold on;
line([xh;xk1],[zh;zk1],'Color','k');hold on;
%shin
line([xa0;xk0],[za0;zk0],'Color','k');hold on;
line([xa1;xk1],[za1;zk1],'Color','k');hold on;
%feet
line([xa0;xa0-H_f*sin(fai)],[za0;za0-H_f*cos(fai)],'Color','k');hold on;
line([xa1;xa1],[za1;za1-H_f],'Color','k');hold on;
%sole
line([xa0-H_f*sin(fai)+L_a*cos(fai);xa0-H_f*sin(fai)-L_b*cos(fai)],...
      [za0-H_f*cos(fai)-L_a*sin(fai);za0-H_f*cos(fai)+L_b*sin(fai)],'Color','k');hold on;
line([xa1+L_a;xa1-L_b],[za1-H_f;za1-H_f],'Color','k');hold on;