主要内容

使用碰撞网格检查机械手自身碰撞

打开实时脚本

这个例子展示了如何在执行轨迹时检查机械手的自碰撞。碰撞网格是通过< >碰撞在机器人模型的URDF中定义的标签。以下相关示例展示了如何以其他方式加载碰撞数据，以及如何检查环境碰撞:

创建机器人表示

导入KUKA®IIWA-14串行操作器的URDF文件rigidBodyTree模型。URDF捕获机器人刚体的碰撞网格文件。属性可将碰撞对象单独添加到刚体中addCollision函数。若要加载附带碰撞对象的所提供的机器人模型，请参阅loadrobot函数。

iwa = importrobot(“iiwa14.urdf”）;iiwa。DataFormat =“列”；

生成轨迹并检查碰撞

将开始和结束配置指定为一组连接位置。这些位置应该没有碰撞。

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函数。

在遍历轨迹时问，致电checkCollision对轨道上的每个构型都起作用。打开穷举检查，在检测到第一个碰撞后继续检查碰撞。

的isConfigInCollision变量跟踪每个配置的碰撞状态。的sepDistForConfig跟踪机器人主体之间的分离距离。对于每次碰撞，主体索引对存储在configCollisionPairs变量。注意，相邻的物体没有被检查，因为它们总是通过连接它们的关节保持接触。

isConfigInCollision = false(100,1);configCollisionPairs = cell(100,1);sepDistForConfig = 0 (iiwa.NumBodies+1,iiwa.NumBodies+ 1100);为i = 1:length(q) [iscollision, sepDist] = checkCollision(iiwa,q(:，i)，“详尽”，“上”，“SkippedSelfCollisions”，“父”）;isConfigInCollision(i) = iscollision;sepDistForConfig(:，:，i) = sepDist;结束

为求碰撞物体的指标，求sepDistForConfig是南．septDist是对称矩阵，因此在具有翻转索引的索引中返回相同的值。通过使用简化列表独特的．

为i = 1:长度(q) sepDist = sepDistForConfig(:，:，i);[body1Idx,body2Idx] = find(isnan(sepDist));collidingPairs = unique(sort([body1Idx,body2Idx]，2));configCollisionPairs{i} = collidingPairs;结束

通过检查输出，您可以看到计划的轨迹经过一系列碰撞。可视化第一次碰撞发生的位置，并突出显示主体。

任何(isConfigInCollision)
ans =逻辑1
firstCollisionIdx = find(isConfigInCollision,1);可视化第一个碰撞的配置。。图;表演(iiwa q (:, firstCollisionIdx));exampleHelperHighlightCollisionBodies (iiwa configCollisionPairs {firstCollisionIdx} + 1, gca);

{

生成无碰撞轨迹

第一次碰撞实际上发生在初始配置处，因为指定的关节位置超过了它的限制。调用wrapToPi限制关节的起始位置。

生成一个新的轨迹并再次检查碰撞。

newStartConfig = wrapToPi(startConfig);q = trapveltraj([newStartConfig goalConfig]，100，“EndTime”3);isConfigInCollision = false(100,1);configCollisionPairs = cell(100,1);为i = 1:length(q) iscollision = checkCollision(iiwa,q(:，i)，“详尽”，“上”，“SkippedSelfCollisions”，“父”）;isConfigInCollision(i) = iscollision;结束

经过对整个轨迹的检查，没有发现碰撞。

任何(isConfigInCollision)
ans =逻辑0
Baidu
map