末端轨迹追踪（end-effector Trajectory Tracking）

思路：

1. 插入目标端点，随机产生轨迹

2. 在轨迹上随机抽取若干点的位置

3. 利用Robotics System Toolbox的 IK Solver

4. 把 joint position 输入RigidBodyTree 并Plot出来

%% Implement Target Points and Plot Trajectory

wayPoints = [0.2 -0.2 0.02;0.25 0 0.15; 0.2 0.2 0.02];
trajectory = cscvn(wayPoints');
hold on
fnplt(trajectory,'r',1);

% eeOffset = 0;
% eeBody = robotics.RigidBody('end_effector');
% setFixedTransform(eeBody.Joint,trvec2tform([eeOffset 0 0]));
% addBody(MH,eeBody,'b7');

%% Create IK object

ik = robotics.InverseKinematics('RigidBodyTree',MH);
weights = [0 0 0 1 1 1];
initialguess = MH.homeConfiguration;

numTotalPoints = 30;
eePositions = ppval(trajectory,linspace(0,trajectory.breaks(end),numTotalPoints));
% plot3(eePositions(1,:),eePositions(2,:),eePositions(3,:))

%% Achieve Joint Position

configSoln = [];

for idx = 1:size(eePositions,2)
    tform = trvec2tform(eePositions(:,idx)');
    qs = ik('end_effector',tform,weights,initialguess);
    configSoln=[configSoln;qs];
    initialguess = qs;
end

%% Tracking Trajectory

title('Tracking Trajectory')
axis([-1 1 -1 1 -1 1]);
for idx = 1:size(eePositions,2)
    show(MH,configSoln(idx,:), 'PreservePlot', false);%,'Frames','on'
    hold on;
    pause(0.1)
end
hold off