close all
clear
currentPath = pwd;%将URDF文件添加进路径
addpath(genpath(currentPath));
figure%可视化模型运动示意图
robot=importrobot('rh2.urdf');
robot.DataFormat='column';%数据格式为列，row为行；为‘row'时q0要转置-->q0’
show(robot,'PreservePlot',false,'Frames','off');%机械臂动画演示
axis([-1,2,-2,2,0,2]/5);
q0=0:0.01:0.5;
q1=q0;
% qq=[q0;q1;q0;q1;q0;q1;q0;q1;q0;q1;q0;q1;q0;q1;q0;q1;q0;q1;q0;q1;q0;q1;q0;q1;q0;q1;q0;q1;q0;q1]';
% qq=[q1;q0;q1;q0;q1;q0;q1;q0;q1;q0;q1;q0;q1;q0;q1;q0;q1;q0;q1;q0;q1;q0;q1;q0;q1;q0;q1;q0;q1;q0]';
qq=[q1;q0;q1;q0;q1;q0;-q1;q0;q1;q0;q1]';
m=length(q0);
camva('auto');%设置相机视角
for i=1:m
    drawnow
    robot.DataFormat='column';%数据格式为列，row为行；为‘row'时q0要转置-->q0’
    show(robot,-qq(i,:)','PreservePlot',false,'Frames','off');%机械臂动画演示
%     axis([-1,2,-2,2,0,2]/5);
%     pause(0.1)
end