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

打开生活的脚本

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

定义碰撞环境

使用碰撞原语创建一个简单的环境。这个示例创建了一个场景，在这个场景中，一个机器人在工作空间中，必须将对象从一个表移动到另一个表。机器人还必须避免工作空间上方的圆形灯具。将表格建模为两个盒子和一个球体，并指定它们在世界中的姿态。更复杂的环境可以使用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函数。

机器人= 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。

生成一个轨迹并检查碰撞

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

startPose = trvec2tform([-0.5,0.5,0.4])*axang2tform([1 0 0 pi]);endPose = trvec2tform([0.5,0.2,0.4])*axang2tform([1 0 0 pi]);

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

%使用固定的随机种子，以确保可重复的结果rng (0);本土知识= inverseKinematics (“RigidBodyTree”,机器人);重量= 1 (1,6);startConfig =动力学(“EndEffector_Link”,重量startPose robot.homeConfiguration);endConfig =动力学(“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年,“EndTime”2);

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

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

%初始化输出inCollision = false(length(q)， 1);检查每个姿势是否碰撞worldCollisionPairIdx =细胞(长度(q), 1);%提供碰撞中的主体为i = 1:length(q) [inCollision(i)，sepDist] = checkCollision(robot,q(:，i)，worldCollisionArray，“IgnoreSelfCollision”，“上”，“详尽”，“上”）;[bodyIdx, worldCollisionObjIdx] =找到(isnan (sepDist));查找碰撞对worldCollidingPairs = [bodyIdx, worldCollisionObjIdx];worldCollisionPairIdx{我}= worldCollidingPairs;结束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并突出碰撞的物体。显示(机器人,问:collidingIdx1),“父”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。

生成无碰撞轨迹

为了避免这些碰撞，可以添加中间路径点，以确保机器人绕过障碍物。

intermediatePose1 = trvec2tform([-。3 -。2 .6])*axang2tform([0 1 0 -pi/4]);在球的外面和周围intermediatePose2 = trvec2tform([0.2,0.2,0.6])*axang2tform([1 0 0 pi]);从上面进来intermediateConfig1 =动力学(“EndEffector_Link”intermediatePose1权重,q (:, collidingIdx1));intermediateConfig2 =动力学(“EndEffector_Link”intermediatePose2权重,q (:, collidingIdx2));显示新的中间姿势ax = exampleHelperVisualizeCollisionEnvironment (worldCollisionArray);表演(intermediateConfig1的机器人“父”ax,“PreservePlot”、假);表演(intermediateConfig2的机器人“父”、ax);

生成一个新的轨迹。

[q, qd qdd t] = trapveltraj ([homeConfiguration(机器人),intermediateConfig1, startConfig, intermediateConfig2, endConfig], 200年,“EndTime”2);

确认它是无碰撞的。

%初始化输出inCollision = false(长度(q), 1);检查每个姿势是否碰撞为inCollision(i) = checkCollision(robot,q(:，i)，worldCollisionArray，“IgnoreSelfCollision”，“上”）;结束isTrajectoryInCollision =任何(inCollision)

isTrajectoryInCollision =逻辑1

可视化生成的轨迹

有生命的结果。

%绘制环境ax2 = exampleHelperVisualizeCollisionEnvironment (worldCollisionArray);%在机器人的主配置中可视化表演(startConfig的机器人“父”, ax2);%更新轴的大小轴平等的%循环遍历其他位置为show(robot,q(:， I)，)“父”ax2,“PreservePlot”、假);%更新数字drawnow结束

绘制关节位置随时间的变化。

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

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