基于碰撞网格的机器人碰撞检测算法研究

碰撞网格是通过机器人模型URDF中定义的<collision>

下面的相关例子展示了如何以其他方式加载碰撞数据，以及如何检查环境碰撞:

URDF捕捉机器人刚体的碰撞网格文件

单独地向刚体添加碰撞对象，可以使用addCollision函数。

加载所提供的带有附加碰撞对象的机器人模型

iiwa = importrobot('iiwa14.urdf');
iiwa.DataFormat = 'column';

生成轨迹并检测是否碰撞

将开始和结束设定为一组关节位置矩阵，这些位置是无碰撞的。

startConfig = [0 -pi/4 pi 3*pi/2 0 -pi/2 pi/8]';
goalConfig = [0 -pi/4 pi 3*pi/4 0 -pi/2 pi/8]';

三秒内找到连接两个构型的关节空间轨迹：

q = trapveltraj([startConfig goalConfig],100,'EndTime',3);

为了验证输出轨迹不包含自碰撞，迭代输出样本，并使用checkCollision函数查看是否有任何点在碰撞中。

当遍历轨迹q时，在轨迹中的每个配置上调用checkCollision函数。

打开穷举check，以在检测到第一个碰撞后继续检测碰撞。

isConfigInCollision变量跟踪每个配置的冲突状态。

sepDistForConfig跟踪机器人身体之间的分离距离。

对于每个碰撞，主体索引对存储在configCollisionPairs变量中。

注意，相邻的物体不能被检测到，因为是通过连接它们的关节接触。

isConfigInCollision = false(100,1);
configCollisionPairs = cell(100,1);
sepDistForConfig = zeros(iiwa.NumBodies+1,iiwa.NumBodies+1,100);
for i = 1:length(q)
    [isColliding, sepDist] = checkCollision(iiwa,q(:,i),'Exhaustive','on');
    isConfigInCollision(i) = isColliding;
    sepDistForConfig(:,:,i) = sepDist;
end

要找出冲突中的主体的索引，请找出sepDistForConfig中的哪些条目是NaN。

septDist是一个对称矩阵，因此在索引翻转时返回相同的值。使用unique来简化列表。

for i = 1:length(q)
    sepDist = sepDistForConfig(:,:,i);
    [body1Idx,body2Idx] = find(isnan(sepDist));

    collidingPairs = unique(sort([body1Idx,body2Idx],2));
    configCollisionPairs{i} = collidingPairs;
end

通过检查输出，可看到经过一系列的碰撞规划轨迹。

可视化第一次碰撞发生的位形，并突出显示物体。

any(isConfigInCollision)

firstCollisionIdx = find(isConfigInCollision,1);

% Visualize the first configuration that is in collision.
figure;
show(iiwa,q(:,firstCollisionIdx));
exampleHelperHighlightCollisionBodies(iiwa,configCollisionPairs{firstCollisionIdx}+1,gca);