使用碰撞网格检查操纵器自我冲突

打开实时脚本

此示例显示了如何在执行轨迹时检查操作器自我收集。碰撞网络通过<碰撞>在机器人模型的URDF中定义的标签。以下相关示例显示了如何以其他方式加载碰撞数据，以及如何检查环境碰撞：

创建机器人表示

导入KUKA®IIWA-14串行操纵器的乌尔德文件作为一个刚性木板模型。乌尔德F捕获了机器人中刚体的碰撞网格文件。要单独将碰撞对象添加到刚体中，您可以使用addCollision功能。要加载带有碰撞对象的提供的机器人模型，请参阅LOADROBOT功能。

iiwa = Importrobot（'iiwa14.urdf'）；iiwa.dataformat ='柱子';

生成轨迹并检查碰撞

将开始和结束配置指定为一组关节位置。这些位置应无碰撞。

startConfig = [0 -pi/4 pi 3*pi/2 0 -pi/2 pi/8]';守门员= [0 -pi/4 pi 3*pi/4 0 -pi/2 pi/8]';

找到一个在三秒钟内连接两个配置的关节空间轨迹。

q = trapveltraj（[[startConfig pargenconfig]，100，'时间结束'，3）;

要验证此输出轨迹不包含自我收集，请迭代输出样本，并使用该点查看是否有任何点相撞checkCollision功能。

在轨迹迭代时问，致电checkCollision在轨迹中的每个配置上函数。在检测到第一次后，打开详尽的检查以继续检查碰撞。

这isconfigincollision可变跟踪每种配置的碰撞状态。这sepdistforconfig跟踪机器人身体之间的分离距离。对于每次碰撞，一对身体索引都存储在ConfigCollisionPairs多变的。请注意，没有检查相邻的身体，因为它们始终通过连接它们的关节接触。

isConfiginCollision = false（100,1）;ConfigCollisionPairs =单元格（100,1）;sepdistforConfig = zeros（iiwa.numbodies+1，iiwa.numbodies+1,100）;为了i = 1：长度（q）[iscolliding，sepdist] = checkCollision（iiwa，q（：，i），，“详尽”，，，，'上'）；isConfiginCollision（i）= isColliding;sepdistforConfig（：，：，i）= sepdist;结尾

要找出碰撞中尸体的索引，请查找哪些条目sepdistforconfig是南。七月是对称矩阵，因此在带有翻转索引的索引中返回相同的值。通过使用简化列表独特的。

为了i = 1：长度（q）sepdist = sepdistforConfig（：，：：，i）;[body1idx，body2idx] = find（isnan（sepdist））;collingpairs = unique（stort（[[Body1idx，body2idx]，2））;ConfigCollisionPairs {i} = CollidingPairs;结尾

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

任何（isConfiginCollision）

ans =逻辑1

firstCollisionIdx = find（isConfiginCollision，1）;％可视化碰撞中的第一种配置。数字;show（iiwa，q（：，firstCollisionIdx））;exampleHelperhighlighlightCollisionBodies（iiwa，configCollisionPairs {firstCollisionIdx}+1，gca）;

图包含一个轴对象。轴对象包含29个类型补丁的对象。These objects represent world, iiwa_link_0, iiwa_link_1, iiwa_link_2, iiwa_link_3, iiwa_link_4, iiwa_link_5, iiwa_link_6, iiwa_link_7, iiwa_link_ee, iiwa_link_ee_kuka, iiwa_link_0_mesh, iiwa_link_1_mesh, iiwa_link_2_mesh, iiwa_link_3_mesh, iiwa_link_4_mesh, iiwa_link_5_mesh, iiwa_link_6_mesh, iiwa_link_7_mesh.

产生无碰撞轨迹

第一次碰撞实际上发生在起始配置处，因为关节位置已超过其极限。称呼环保限制关节的起始位置。

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

newstartconfig = wrataptopi（startConfig）;q = trapveltraj（[[newstartconfig pargenconfig]，100，'时间结束'，3）;isConfiginCollision = false（100,1）;ConfigCollisionPairs =单元格（100,1）;为了i = 1：长度（q）iscolliding = checkCollision（iiwa，q（：，i），，，“详尽”，，，，'上'）；isConfiginCollision（i）= isColliding;结尾

检查整个轨迹后，找不到碰撞。

任何（isConfiginCollision）

ans =逻辑0