Robotics System Toolbox中的机器人运动(2)---圆的轨迹跟踪

       1、前记：在继Robotics System Toolbox中的机器人运动(1)，和具有逆运动学的2维轨迹跟踪（翻译--个人学习记录）这俩篇博客后，细细的研究了Robotics System Toolbox工具箱中的逆运动求解函数，发现必须要有足够的信息才能够将机器臂的各关节很好的控制起来，而足够的信息就是各关节的角度变化信息是怎样形成的。如Robotics System Toolbox中的机器人运动(1)，是从初始Home位的各关节的角度配置，以相同的幅度递增迭代而成的运动控制，是从正运动的角度去实现运动控制的。那么这里试着用逆运动的角度来实现到达空间点位各关节的角度如何配置，这里模拟具有逆运动学的2维轨迹跟踪（翻译--个人学习记录）中的例子实现。

       2、代码：

%导入机器人
clear
clc
robot=importrobot('irb_140.urdf');
show(robot);
axes.CameraPositionMode = 'auto';
%定义轨迹为半径为0.15的圆
t = (0:0.5:10)'; 
count = length(t);
center = [0.45 0 0.5];
radius = 0.15;
theta = t*(2*pi/t(end));
points =(center + radius*[cos(theta) sin(theta) zeros(size(theta))])';
%%
hold on
plot3(points(1,:),points(2,:),points(3,:),'r')%画出定义的轨迹
%逆运动学解：使用InverseKinematics对象找到机器人配置的解决方案, 以实现沿轨迹给定的最终末端位置
eeOffset = 0.01;
eeBody = robotics.RigidBody('end_effector');
setFixedTransform(eeBody.Joint,trvec2tform([eeOffset 0 0]));
addBody(robot,eeBody,'link_6');
ik = robotics.InverseKinematics('RigidBodyTree',robot);
weights = [0.1 0.1 0 1 1 1];
qInitial = robot.homeConfiguration;
%%
%通过点的轨迹循环来跟踪圆。调用每个点的ik对象以生成实现末端位置的关节配置，存储要稍后使用的配置。
for i = 1:size(points,2)
    % Solve for the configuration satisfying the desired end effector
    tform = trvec2tform(points(:,i)');
    qSol(i,:) = ik('end_effector',tform,weights,qInitial);
    % Start from prior solution
    qInitial = qSol(i,:);
end
%%
title('robot move follow the trajectory')
hold on
axis([-0.6 0.8 -0.6 0.65 0 1.3]);
for i = 1:size(points,2)
    show(robot,qSol(i,:)','PreservePlot',false);%false改为true时，留下重影。
    pause(0.3)
end
hold off

效果：

动画（1）

动画（2）