检查操作器是否与环境发生碰撞

打开生活的脚本

在受限的工作空间中生成一个无碰撞的轨迹。

定义碰撞环境

使用碰撞基元创建一个简单的环境。此示例创建一个机器人在工作区中的场景，并且必须将对象从一个表移动到另一个表。机器人还必须避免工作空间上方的圆形灯具。将表格为两个盒子和一个球体，并在世界上指定它们的姿势。可以使用更复杂的环境collisionmesh.对象。

％创建两个平台platform1 = collisionBox (0.5, 0.5, 0.25);platform1。姿势= trvec2tform([-0.5 0.4 0.2]);platform2 = collisionBox (0.5, 0.5, 0.25);platform2。姿势= trvec2tform([0.5 0.2 0.2]);添加一个灯具，模型为球体lightFixture = collisionSphere (0.1);lightFixture。= trvec2tform([。2 0 1]);存储在单元格数组中以进行冲突检查worldCollisionArray = {platform1 platform2 lightFixture};

使用遍历碰撞数组的帮助函数可视化环境。

ax = exampleHelperVisualizeCollisionEnvironment (worldCollisionArray);

图中包含一个轴对象。axis对象包含3个patch类型的对象。

将机器人添加到环境中

加载Kinova机械手模型作为rigidBodyTree对象使用loadrobot.函数。

robot = loadrobot（“kinovagen3”那“dataformat”那“列”那“重力”[0 0 -9.81]);

使用与碰撞对象相同的轴显示环境中的机器人。机器人基地固定在世界的起源。

表演(机器人,homeConfiguration(机器人),“父”、ax);

图中包含一个轴对象。axis对象包含28个类型为patch, line的对象。这些对象代表base_link, Shoulder_Link, HalfArm1_Link, HalfArm2_Link, ForeArm_Link，腕1_link，腕2_link, Bracelet_Link, EndEffector_Link, Shoulder_Link_mesh, HalfArm1_Link_mesh, HalfArm2_Link_mesh, ForeArm_Link_mesh，腕1_link_mesh，腕2_link_mesh, Bracelet_Link_mesh, base_link_mesh。

生成轨迹并检查碰撞

使用使用变换矩阵乘法组合的位置和方向向量来定义一个开始和结束姿势。

eleplings = trvec2tform（[ -  0.5,0.5,0.4]）* AXANG2TFORM（[1 0 0 PI]）;Endople = TRVEC2TFORM（[0.5,0.2,0.4]）* AXANG2TFORM（[1 0 0 PI]）;

使用underedkinematics.根据所要求的姿态求出关节位置。手动检查配置是否有效。

％使用固定的随机种子来确保可重复的结果RNG（0）;ik = undedkkkinematics（“刚性小孩”，机器人）;权重=α（1,6）;startconfig = ik（“endeffector_link”，血统，重量，机器人.Homeconfiguration）;endconfig = ik（“endeffector_link”,重量endPose robot.homeConfiguration);显示初始和最终位置显示(机器人,startConfig);显示(机器人,endConfig);

图中包含一个轴对象。axis对象包含78个类型为patch, line的对象。这些对象代表base_link, Shoulder_Link, HalfArm1_Link, HalfArm2_Link, ForeArm_Link，腕1_link，腕2_link, Bracelet_Link, EndEffector_Link, Shoulder_Link_mesh, HalfArm1_Link_mesh, HalfArm2_Link_mesh, ForeArm_Link_mesh，腕1_link_mesh，腕2_link_mesh, Bracelet_Link_mesh, base_link_mesh。

使用梯形速度剖面来生成一个从起始位置到起始位置，然后到结束位置的平滑轨迹。

q = trapveltraj ([homeConfiguration(机器人),startConfig, endConfig], 200年,“时间结束”2);

检查是否与环境中的障碍物发生碰撞checkCollision函数。启用IgnoreLeCholision.n详尽的名称-值参数。自碰撞被忽略，因为机器人模型的关节限制阻止了大多数自碰撞。详尽的检查确保该函数计算所有分离距离，并在检测到第一次碰撞后继续搜索碰撞。

这sepDist输出将机器人体与世界碰撞对象之间的距离存储为矩阵。每行对应于特定的世界碰撞对象。每列对应于机器人身体。价值观南表明碰撞。将碰撞的索引存储为单元格数组。

%初始化输出inCollision = false(length(q)， 1);％检查每个姿势是否碰撞worldcollisionpairidx = cell（长度（q），1）;%提供碰撞中的主体为了i = 1：长度（q）[infollision（i），sepdist] = checkcollision（机器人，q（：，i），worldcollisionArray，“忽略”那“在”那“详尽”那“在”）;[bodyidx，worldcollisionobjidx] =查找（Isnan（套子））;％查找碰撞对worldcollidpairs = [bodyidx，worldcollisionobjidx];worldcollisionpairidx {i} = worldcollidide;结尾isTrajectoryInCollision =任何(inCollision)

istrajectoryincollision =.逻辑1

检查检测到的碰撞

从最后一步，检测到两个碰撞。可视化这些配置以进一步调查。使用exampleHelperHighlightCollisionBodies基于索引突出显示体的功能。您可以看到碰撞发生在球体和表格中。

collidingIdx1 =找到(inCollision, 1);collidingIdx2 =找到(inCollision 1“最后一次”）;％识别碰撞刚体。collidingBodies1 = worldCollisionPairIdx{collidingIdx1}*[1 0]';collidingBodies2 = worldCollisionPairIdx{collidingIdx2}*[1 0]';想象环境。ax = exampleHelperVisualizeCollisionEnvironment (worldCollisionArray);％添加RobotConfigurations并突出显示碰撞机构。展示（机器人，q（：，collingiddx1），“父”ax,“preserveplot”、假);exampleHelperHighlightCollisionBodies(机器人,collidingBodies1 + 1, ax);显示(机器人,问:collidingIdx2),“父””,ax);exampleHelperHighlightCollisionBodies(机器人,collidingBodies2 + 1, ax);

图中包含一个轴对象。axis对象包含53个类型为patch, line的对象。这些对象代表base_link, Shoulder_Link, HalfArm1_Link, HalfArm2_Link, ForeArm_Link，腕1_link，腕2_link, Bracelet_Link, EndEffector_Link, Shoulder_Link_mesh, HalfArm1_Link_mesh, HalfArm2_Link_mesh, ForeArm_Link_mesh，腕1_link_mesh，腕2_link_mesh, Bracelet_Link_mesh, base_link_mesh。

生成无碰撞轨迹

为避免这些碰撞，添加中间航点以确保机器人导航周围的障碍物。

中间exose1 = trvec2tform（[ - 。3 -.2 .6]）* axang2tform（[0 1 0-pi / 4]）;在球的外面和周围中间介绍2 = TRVEC2TFORM（[0.2,0.2,0.6]）* AXANG2TFORM（[1 0 0 PI]）;％来自上面intermediateConfig1 = IK（“endeffector_link”intermediatePose1权重,q (:, collidingIdx1));intermediateConfig2 =动力学(“endeffector_link”，中间酶2，重量，q（：，collingiddx2））;％显示新的中间姿势ax = exampleHelperVisualizeCollisionEnvironment (worldCollisionArray);表演(intermediateConfig1的机器人“父”ax,“preserveplot”、假);展示（机器人，intermediateConfig2，“父”、ax);

生成一个新的轨迹。

[q，qd，qdd，t] = trapveltraj（[homeconfiguration（机器人），intermediateConfig1，Startconfig，IntermediateConfig2，Endconfig]，200，“时间结束”2);

确认它是无碰撞的。

％初始化输出infollision = false（长度（q），1）;％检查每个姿势是否碰撞为了i = 1：长度（q）infollision（i）= checkcollision（机器人，q（：，i），worldcollisionArray，“忽略”那“在”）;结尾isTrajectoryInCollision =任何(inCollision)

istrajectoryincollision =.逻辑1

可视化生成的轨迹

为结果设置动画。

%绘制环境ax2 = exampleHelperVisualizeCollisionEnvironment (worldCollisionArray);％在其家庭配置中可视化机器人展示（机器人，StartConfig，“父”，AX2）;%更新轴的大小轴平等的％循环通过其他位置为了i = 1：长度（q）显示（机器人，q（：，i），“父”，AX2，“preserveplot”、假);％更新图drawnow结尾

随着时间的推移绘制关节位置。

图绘制(t, q)包含(“时间”) ylabel (“联合位置”的）

图中包含一个轴对象。axis对象包含7个类型为line的对象。