Robotics System Toolbox中的机器人运动（6）-碰撞检测

CSDN话题挑战赛第2期
参赛话题：学习笔记

1、前记

       C站第5年，我还在分享机器人仿真和控制的基础内容，而且大多以MATLAB仿真为主要内容。从去年到现在为止在C站坚持学习记录的次数有所下降，现在慢慢回归到C站来，当然不排除有水的部分，不过关于机器人系统工具箱Robotics System Toolbox的学习记录在我的专栏MATLAB和机器人还是有很多介绍了。

       机器人系统工具箱包括碰撞检查、路径规划、轨迹生成、正运动学和逆运动学以及使用刚体树表示的运动学和动力学算法也越来越成熟，反观之对学习机器人来说ROS及其生态圈好像要热闹很多【可能我对其不熟，所以不太喜欢ROS，每次配环境到处都是坑很容易把人搞懵】，但这不影响对MATLAB这个工具的喜爱。我也仍然在关注每一版MATLAB更新后对机器人部分的改善。

2.内容

下面的学习例子我是参考官网代码修改的，关于如何使用系统工具箱进行环境障碍物建模，机器人规划和碰撞检测。

主要参考链接也放在这里，感兴趣的还是建议看官网说明要通透些。Check for Environmental Collisions with Manipulators

（1） 构建环境和机器人

%% 构建环境
% Create two platforms
clc
clear
platform1 = collisionBox(0.5,0.5,0.25);
platform1.Pose = trvec2tform([-0.5 0.4 0.2]);
platform2 = collisionBox(0.5,0.5,0.25);
platform2.Pose = trvec2tform([0.5 0.2 0.2]);
% Add a light fixture, modeled as a sphere
lightFixture = collisionSphere(0.1);
lightFixture.Pose = trvec2tform([.2 0 1]);
% Store in a cell array for collision-checking
worldCollisionArray = {platform1 platform2 lightFixture};%1，2，3；worldCollisionArray{1}
%% 创建figure对象显示环境：可以直接用exampleHelperVisualizeCollisionEnvironment(worldCollisionArray);
%  为了演示修改颜色做下
figure;
ax=gca;
% 显示桌子1并上色
[~, patchObj]=show(platform1,'Parent', ax);
patchObj.FaceColor = [1 0.6 0.9];
axis([-0.8,1,-0.8,1.2,0,1.4])%调整图框范围
view(141,22)%调节视角
hold on
%显示桌子2并上色
[~, patchObj]=show(platform2,'Parent', ax);
patchObj.FaceColor = [1 0.6 0.9];
% 显示灯并上色
[~, patchObj]=show(lightFixture,'Parent', ax);
patchObj.FaceColor = [0 1 0];
%% 打光
l = light;
l.Color = [1 1 1];
l.Position = [1 1 2];
%% 添加机器人模型：可更改为DH方向构建。
robot = loadrobot("kinovaGen3","DataFormat","column","Gravity",[0 0 -9.81]);
show(robot,homeConfiguration(robot),"Parent",ax, 'Frames','off');

（2）设置起始位置和目标位置，逆解出机器人两个位置的关节配置并显示

%% 给机器人添加任务起点和终点
startPose = trvec2tform([-0.5,0.5,0.6])*axang2tform([1 0 0 pi]);
endPose = trvec2tform([0.5,0.2,0.4])*axang2tform([1 0 0 pi]);
%% 使用逆运动学求解起点个终点位姿的关节构型并显示
rng(0);% Use a fixed random seed to ensure repeatable results
ik = inverseKinematics("RigidBodyTree",robot);
weights = ones(1,6);
startConfig = ik("EndEffector_Link",startPose,weights,robot.homeConfiguration);
endConfig = ik("EndEffector_Link",endPose,weights,robot.homeConfiguration);
endEffector="EndEffector_Link";
% Show initial and final positions
show(robot,startConfig);
show(robot,endConfig);
collisionArray = exampleHelperManipCollisionsFromVisuals(robot);

 （3）梯形速度规划从起点到终点的轨迹

%% 使用梯形速度规划从起点到终点的轨迹【关节空间】
[q,qd,qdd,t]= trapveltraj([homeConfiguration(robot),startConfig,endConfig],200,"EndTime",2);
hold on
show(robot,startConfig);
axis([-0.8,1,-0.8,1.2,0,1.2])%调整图框范围
view(141,22)%调节视角
axis on
hold on
title('机器人根据Home位姿、起点和终点位姿直接梯形速度的运动')
for i = 1:2:length(q)
    show(robot,q(:,i),"PreservePlot",false);%false 不留下重影
    poseNow = getTransform(robot, q(:,i), endEffector);%正运动学
    plot3(poseNow(1,4), poseNow(2,4), poseNow(3,4),'b.','MarkerSize',5)
    drawnow
end

 （4）显示机器人运动的关节位置和速度等变化

figure
subplot(3,1,1)
plot(t,q(1,:),'-r',t,q(2,:),'-b',t,q(3,:),'-c',t,q(4,:),'-.m',t,q(5,:),'.-r',t,q(6,:),'-.b',t,q(7,:),'-.y')
legend('Join1','Join2','Join3','Join4','Join5','Join6','Join7')
xlabel("Time")
ylabel("Joint Position")
grid on
subplot(3,1,2)
plot(t,qd(1,:),'-r',t,qd(2,:),'-b',t,qd(3,:),'-c',t,qd(4,:),'-.m',t,qd(5,:),'.-r',t,qd(6,:),'-.b',t,qd(7,:),'-.y')
legend('Join1','Join2','Join3','Join4','Join5','Join6','Join7')
xlabel("Time")
ylabel("Joint velocity")
grid on
subplot(3,1,3)
plot(t,qdd(1,:),'-r',t,qdd(2,:),'-b',t,qdd(3,:),'-c',t,qdd(4,:),'-.m',t,qdd(5,:),'.-r',t,qdd(6,:),'-.b',t,qdd(7,:),'-.y')
legend('Join1','Join2','Join3','Join4','Join5','Join6','Join7')
xlabel("Time")
ylabel("Joint Acceleration")
grid on

 （5）查看机器人运动过程中是否与环境发生碰撞并显示碰撞部位

%%
% 初始化输出
inCollision = false(length(q),1); %===> zeros(length(q),1)
worldCollisionPairIdx = cell(length(q),1); % 元胞数组，保存与环境碰撞的部件和关节配置的索引
for i = 1:length(q)
    [inCollision(i),sepDist] = checkCollision(robot,q(:,i),worldCollisionArray,"IgnoreSelfCollision","on","Exhaustive","on");
    % 返回发生碰撞的构型索引（200个构型中的那个19到37）和距离。
    %  sepDist为输出是机器人身体与世界碰撞对象之间的距离--->存储为矩阵
    %IgnoreSelfCollision and Exhaustivename-value on, Self collisions are
    % ignored .because the robot model joint limits prevent most self
    % collisions.免除自身碰撞的检查，因为关节限制保证了大多数自碰撞情况。只寻找构型与环境的碰撞情况
    [bodyIdx,worldCollisionObjIdx] = find(isnan(sepDist)); % 找到碰撞的部件。距离是空NaN，则发生碰撞
    worldCollidingPairs = [bodyIdx,worldCollisionObjIdx]; 
    worldCollisionPairIdx{i} = worldCollidingPairs;  %机器人部件索引第一列，环境物体索引第二列
end
isTrajectoryInCollision = any(inCollision)
%% 检测到碰撞并可视化构型，第一个和最后一个检测到的碰撞
collidingIdx1 = find(inCollision,1);%第一个：构型索引19
collidingIdx2 = find(inCollision,1,"last");%最后一个：构型索引172
% Identify the colliding rigid bodies.找到机器人中的碰撞部分
collidingBodies1 = worldCollisionPairIdx{collidingIdx1}*[1 0]';%取碰撞的机器人部件第一列的=矩阵乘以[1,0]'%具体看矩阵维度
collidingBodies2 = worldCollisionPairIdx{collidingIdx2}*[1 0]';
%  Identify the colliding world bodies.找到环境中的碰撞部分
collidingworld1 = worldCollisionPairIdx{collidingIdx1}*[0 1]';%取碰撞的机器人部件第一列的=矩阵乘以[1,0]'%具体看矩阵维度
collidingworld2 = worldCollisionPairIdx{collidingIdx2}*[0 1]';
%% 显示环境，并将碰撞进行可视化
ax = exampleHelperVisualizeCollisionEnvironment(worldCollisionArray);
% Add the robotconfigurations & highlight the colliding bodies.
show(robot,q(:,collidingIdx1),"Parent",ax,"PreservePlot",false);
exampleHelperHighlightCollisionBodies(robot,collidingBodies1 + 1,ax);
show(robot,q(:,collidingIdx2),"Parent"',ax);
exampleHelperHighlightCollisionBodies(robot,collidingBodies2 + 1,ax);
% hightlight world object
hold on
[~, patchObj]=show(worldCollisionArray{collidingworld1(1)},"Parent"',ax);
patchObj.FaceColor = [1 0.8 0];
[~, patchObj]=show(worldCollisionArray{collidingworld2},"Parent"',ax);
patchObj.FaceColor = [1 0.8 0];
title('发生碰撞的部分构型显示')

 （6）在碰撞点附近添加中间过渡点，看是否能避障【MATLAB自带的RRT可以直接实现，以后介绍】

%% 创建新的轨迹--collision free。添加中间点
intermediatePose1 = trvec2tform([-.3 -.2 .6])*axang2tform([0 1 0 -pi/4]); % 球的周围
intermediatePose2 = trvec2tform([0.2,0.2,0.6])*axang2tform([1 0 0 pi]); % 桌子2上面
intermediateConfig1 = ik("EndEffector_Link",intermediatePose1,weights,q(:,collidingIdx1));
intermediateConfig2 = ik("EndEffector_Link",intermediatePose2,weights,q(:,collidingIdx2));
% Show the new intermediate poses
ax = exampleHelperVisualizeCollisionEnvironment(worldCollisionArray);
show(robot,intermediateConfig1,"Parent",ax,"PreservePlot",false);
plot3(intermediatePose1(1,4),intermediatePose1(2,4),intermediatePose1(3,4),'bo','MarkerSize',15)
%%
hold on
show(robot,intermediateConfig2,"Parent",ax);
plot3(intermediatePose2(1,4),intermediatePose2(2,4),intermediatePose2(3,4),'bo','MarkerSize',15)

 （7）插入中间点的梯形速度规划角度和速度变化

%% 根据新的中间点完成梯形速度规划
[q,qd,qdd,t] = trapveltraj([homeConfiguration(robot),intermediateConfig1,startConfig,intermediateConfig2,endConfig],300,"EndTime",3);
%显示规划的关节角度、速度变化曲线
figure
subplot(2,1,1)
plot(t,q(1,:),'-r',t,q(2,:),'-b',t,q(3,:),'-c',t,q(4,:),'-.m',t,q(5,:),'.-r',t,q(6,:),'-.b',t,q(7,:),'-.y')
legend('Join1','Join2','Join3','Join4','Join5','Join6','Join7')
xlabel("Time")
ylabel("Joint Position")
grid on
subplot(2,1,2)
plot(t,qd(1,:),'-r',t,qd(2,:),'-b',t,qd(3,:),'-c',t,qd(4,:),'-.m',t,qd(5,:),'.-r',t,qd(6,:),'-.b',t,qd(7,:),'-.y')
legend('Join1','Join2','Join3','Join4','Join5','Join6','Join7')
xlabel("Time")
ylabel("Joint velocity")
grid on

 （8）再次检测是否碰撞checkCollision函数，并显示运动

%% 再次检查是否有碰撞
%Initialize outputs
isCollision = false(length(q),1); %初始化碰撞构型的存储空间
for i = 1:length(q)
    inCollision(i) = checkCollision(robot,q(:,i),worldCollisionArray,"IgnoreSelfCollision","on");
end
isTrajectoryInCollision = any(inCollision)
%% 
% Plot the environment
ax2 = exampleHelperVisualizeCollisionEnvironment(worldCollisionArray);
% Visualize the robot in its home configuration
show(robot,startConfig,"Parent",ax2);
axis([-0.8,1,-0.8,1.2,0,1.4])%调整图框范围
view(146,15)%调节视角
hold on
plot3(intermediatePose1(1,4),intermediatePose1(2,4),intermediatePose1(3,4),'bo','MarkerSize',10)
plot3(intermediatePose2(1,4),intermediatePose2(2,4),intermediatePose2(3,4),'bo','MarkerSize',10)
title('添加中间点的无碰撞路径')
% Loop through the other positions
for i = 1:length(q)
    show(robot,q(:,i),"Parent",ax2,"PreservePlot",false);
    poseNow = getTransform(robot, q(:,i), endEffector);%正运动学
    plot3(poseNow(1,4), poseNow(2,4), poseNow(3,4),'b.','MarkerSize',5)
    drawnow
end

 完整代码如下：上述基本按小结运行可得到每部分结果显示

%% 检测在结构环境中机器人是否与物体之间发生碰撞情况，如何避免？
% https://www.mathworks.com/help/robotics/ug/check-for-environmental-collisions-with-manipulators.html
%% 构建环境
% Create two platforms
clc
clear
platform1 = collisionBox(0.5,0.5,0.25);
platform1.Pose = trvec2tform([-0.5 0.4 0.2]);
platform2 = collisionBox(0.5,0.5,0.25);
platform2.Pose = trvec2tform([0.5 0.2 0.2]);
% Add a light fixture, modeled as a sphere
lightFixture = collisionSphere(0.1);
lightFixture.Pose = trvec2tform([.2 0 1]);
% Store in a cell array for collision-checking
worldCollisionArray = {platform1 platform2 lightFixture};%1，2，3；worldCollisionArray{1}
%% 创建figure对象显示环境：可以直接用exampleHelperVisualizeCollisionEnvironment(worldCollisionArray);
%  为了演示修改颜色做下
figure;
ax=gca;
% 显示桌子1并上色
[~, patchObj]=show(platform1,'Parent', ax);
patchObj.FaceColor = [1 0.6 0.9];
axis([-0.8,1,-0.8,1.2,0,1.4])%调整图框范围
view(141,22)%调节视角
hold on
%显示桌子2并上色
[~, patchObj]=show(platform2,'Parent', ax);
patchObj.FaceColor = [1 0.6 0.9];
% 显示灯并上色
[~, patchObj]=show(lightFixture,'Parent', ax);
patchObj.FaceColor = [0 1 0];
%% 打光
l = light;
l.Color = [1 1 1];
l.Position = [1 1 2];
%% 添加机器人模型：可更改为DH方向构建。
robot = loadrobot("kinovaGen3","DataFormat","column","Gravity",[0 0 -9.81]);
show(robot,homeConfiguration(robot),"Parent",ax, 'Frames','off');
%% 给机器人添加任务起点和终点
startPose = trvec2tform([-0.5,0.5,0.6])*axang2tform([1 0 0 pi]);
endPose = trvec2tform([0.5,0.2,0.4])*axang2tform([1 0 0 pi]);
%% 使用逆运动学求解起点个终点位姿的关节构型并显示
rng(0);% Use a fixed random seed to ensure repeatable results
ik = inverseKinematics("RigidBodyTree",robot);
weights = ones(1,6);
startConfig = ik("EndEffector_Link",startPose,weights,robot.homeConfiguration);
endConfig = ik("EndEffector_Link",endPose,weights,robot.homeConfiguration);
endEffector="EndEffector_Link";
% Show initial and final positions
show(robot,startConfig);
show(robot,endConfig);
collisionArray = exampleHelperManipCollisionsFromVisuals(robot);
%% 使用梯形速度规划从起点到终点的轨迹【关节空间】
[q,qd,qdd,t]= trapveltraj([homeConfiguration(robot),startConfig,endConfig],200,"EndTime",2);
hold on
show(robot,startConfig);
axis([-0.8,1,-0.8,1.2,0,1.2])%调整图框范围
view(141,22)%调节视角
axis on
hold on
title('机器人根据Home位姿、起点和终点位姿直接梯形速度的运动')
for i = 1:2:length(q)
    show(robot,q(:,i),"PreservePlot",false);%false 不留下重影
    poseNow = getTransform(robot, q(:,i), endEffector);%正运动学
    plot3(poseNow(1,4), poseNow(2,4), poseNow(3,4),'b.','MarkerSize',5)
    drawnow
end
%% 显示规划的关节角度、速度变化曲线
figure
subplot(3,1,1)
plot(t,q(1,:),'-r',t,q(2,:),'-b',t,q(3,:),'-c',t,q(4,:),'-.m',t,q(5,:),'.-r',t,q(6,:),'-.b',t,q(7,:),'-.y')
legend('Join1','Join2','Join3','Join4','Join5','Join6','Join7')
xlabel("Time")
ylabel("Joint Position")
grid on
subplot(3,1,2)
plot(t,qd(1,:),'-r',t,qd(2,:),'-b',t,qd(3,:),'-c',t,qd(4,:),'-.m',t,qd(5,:),'.-r',t,qd(6,:),'-.b',t,qd(7,:),'-.y')
legend('Join1','Join2','Join3','Join4','Join5','Join6','Join7')
xlabel("Time")
ylabel("Joint velocity")
grid on
subplot(3,1,3)
plot(t,qdd(1,:),'-r',t,qdd(2,:),'-b',t,qdd(3,:),'-c',t,qdd(4,:),'-.m',t,qdd(5,:),'.-r',t,qdd(6,:),'-.b',t,qdd(7,:),'-.y')
legend('Join1','Join2','Join3','Join4','Join5','Join6','Join7')
xlabel("Time")
ylabel("Joint Acceleration")
grid on
%%
% 初始化输出
inCollision = false(length(q),1); %===> zeros(length(q),1)
worldCollisionPairIdx = cell(length(q),1); % 元胞数组，保存与环境碰撞的部件和关节配置的索引
for i = 1:length(q)
    [inCollision(i),sepDist] = checkCollision(robot,q(:,i),worldCollisionArray,"IgnoreSelfCollision","on","Exhaustive","on");
    % 返回发生碰撞的构型索引（200个构型中的那个19到37）和距离。
    %  sepDist为输出是机器人身体与世界碰撞对象之间的距离--->存储为矩阵
    %IgnoreSelfCollision and Exhaustivename-value on, Self collisions are
    % ignored .because the robot model joint limits prevent most self
    % collisions.免除自身碰撞的检查，因为关节限制保证了大多数自碰撞情况。只寻找构型与环境的碰撞情况
    [bodyIdx,worldCollisionObjIdx] = find(isnan(sepDist)); % 找到碰撞的部件。距离是空NaN，则发生碰撞
    worldCollidingPairs = [bodyIdx,worldCollisionObjIdx]; 
    worldCollisionPairIdx{i} = worldCollidingPairs;  %机器人部件索引第一列，环境物体索引第二列
end
isTrajectoryInCollision = any(inCollision)
%% 检测到碰撞并可视化构型，第一个和最后一个检测到的碰撞
collidingIdx1 = find(inCollision,1);%第一个：构型索引19
collidingIdx2 = find(inCollision,1,"last");%最后一个：构型索引172
% Identify the colliding rigid bodies.找到机器人中的碰撞部分
collidingBodies1 = worldCollisionPairIdx{collidingIdx1}*[1 0]';%取碰撞的机器人部件第一列的=矩阵乘以[1,0]'%具体看矩阵维度
collidingBodies2 = worldCollisionPairIdx{collidingIdx2}*[1 0]';
%  Identify the colliding world bodies.找到环境中的碰撞部分
collidingworld1 = worldCollisionPairIdx{collidingIdx1}*[0 1]';%取碰撞的机器人部件第一列的=矩阵乘以[1,0]'%具体看矩阵维度
collidingworld2 = worldCollisionPairIdx{collidingIdx2}*[0 1]';
%% 显示环境，并将碰撞进行可视化
ax = exampleHelperVisualizeCollisionEnvironment(worldCollisionArray);
% Add the robotconfigurations & highlight the colliding bodies.
show(robot,q(:,collidingIdx1),"Parent",ax,"PreservePlot",false);
exampleHelperHighlightCollisionBodies(robot,collidingBodies1 + 1,ax);
show(robot,q(:,collidingIdx2),"Parent"',ax);
exampleHelperHighlightCollisionBodies(robot,collidingBodies2 + 1,ax);
% hightlight world object
hold on
[~, patchObj]=show(worldCollisionArray{collidingworld1(1)},"Parent"',ax);
patchObj.FaceColor = [1 0.8 0];
[~, patchObj]=show(worldCollisionArray{collidingworld2},"Parent"',ax);
patchObj.FaceColor = [1 0.8 0];
title('发生碰撞的部分构型显示')
%% 创建新的轨迹--collision free。添加中间点
intermediatePose1 = trvec2tform([-.3 -.2 .6])*axang2tform([0 1 0 -pi/4]); % 球的周围
intermediatePose2 = trvec2tform([0.2,0.2,0.6])*axang2tform([1 0 0 pi]); % 桌子2上面
intermediateConfig1 = ik("EndEffector_Link",intermediatePose1,weights,q(:,collidingIdx1));
intermediateConfig2 = ik("EndEffector_Link",intermediatePose2,weights,q(:,collidingIdx2));
% Show the new intermediate poses
ax = exampleHelperVisualizeCollisionEnvironment(worldCollisionArray);
show(robot,intermediateConfig1,"Parent",ax,"PreservePlot",false);
plot3(intermediatePose1(1,4),intermediatePose1(2,4),intermediatePose1(3,4),'bo','MarkerSize',15)
%%
hold on
show(robot,intermediateConfig2,"Parent",ax);
plot3(intermediatePose2(1,4),intermediatePose2(2,4),intermediatePose2(3,4),'bo','MarkerSize',15)
%% 根据新的中间点完成梯形速度规划
[q,qd,qdd,t] = trapveltraj([homeConfiguration(robot),intermediateConfig1,startConfig,intermediateConfig2,endConfig],300,"EndTime",3);
%显示规划的关节角度、速度变化曲线
figure
subplot(2,1,1)
plot(t,q(1,:),'-r',t,q(2,:),'-b',t,q(3,:),'-c',t,q(4,:),'-.m',t,q(5,:),'.-r',t,q(6,:),'-.b',t,q(7,:),'-.y')
legend('Join1','Join2','Join3','Join4','Join5','Join6','Join7')
xlabel("Time")
ylabel("Joint Position")
grid on
subplot(2,1,2)
plot(t,qd(1,:),'-r',t,qd(2,:),'-b',t,qd(3,:),'-c',t,qd(4,:),'-.m',t,qd(5,:),'.-r',t,qd(6,:),'-.b',t,qd(7,:),'-.y')
legend('Join1','Join2','Join3','Join4','Join5','Join6','Join7')
xlabel("Time")
ylabel("Joint velocity")
grid on
%% 再次检查是否有碰撞
%Initialize outputs
isCollision = false(length(q),1); %初始化碰撞构型的存储空间
for i = 1:length(q)
    inCollision(i) = checkCollision(robot,q(:,i),worldCollisionArray,"IgnoreSelfCollision","on");
end
isTrajectoryInCollision = any(inCollision)
%% 
% Plot the environment
ax2 = exampleHelperVisualizeCollisionEnvironment(worldCollisionArray);
% Visualize the robot in its home configuration
show(robot,startConfig,"Parent",ax2);
axis([-0.8,1,-0.8,1.2,0,1.4])%调整图框范围
view(146,15)%调节视角
hold on
plot3(intermediatePose1(1,4),intermediatePose1(2,4),intermediatePose1(3,4),'bo','MarkerSize',10)
plot3(intermediatePose2(1,4),intermediatePose2(2,4),intermediatePose2(3,4),'bo','MarkerSize',10)
title('添加中间点的无碰撞路径')
% Loop through the other positions
for i = 1:length(q)
    show(robot,q(:,i),"Parent",ax2,"PreservePlot",false);
    poseNow = getTransform(robot, q(:,i), endEffector);%正运动学
    plot3(poseNow(1,4), poseNow(2,4), poseNow(3,4),'b.','MarkerSize',5)
    drawnow
end

3、小结

这篇记录主要是使用机器人系统工具箱的函数完成环境设置和机器人运动碰撞检测，运动规划主要采用的是梯形速度规划，碰撞避免还是采用的最传统的方式----在路径中添加中间过渡点实现。后续内容待续........