检查与操纵器的环境碰撞

打开直播脚本

在约束的工作区中生成无碰撞轨迹。

定义碰撞环境

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

创建两个平台platform1 = CollisionBox（0.5,0.5,0.25）;platform1.pose = trvec2tform（[ -  0.5 0.4 0.2]）;platform2 = CollisionBox（0.5,0.5,0.25）;platform2.plate = trvec2tform（[0.5 0.2 0.2]）;％添加灯具，建模为球体LightRuxture = CollisionSphere（0.1）;lightfixture.pose = trvec2tform（[。2 0 1]）;％存储在用于碰撞检查的单元格数组中worldcollisionArray = {platform1 platform2 lightduxture};

使用辅助函数可视化环境，该功能通过碰撞阵列迭代。

Ax = examplehelpervisualizeCollisionEn环境（WorldCollisionArray）;

图包含轴对象。轴对象包含3个类型的贴片对象。

将机器人添加到environment

装载Kinova机械手模型作为一个刚性小组细胞对象使用loadrobot函数。

机器人= loadrobot (“kinovaGen3”，“DataFormat”，“柱子”，“重力”，[0 0 -9.81]）;

使用与碰撞对象相同的轴在环境中显示机器人。机器人基座固定在世界的原点上。

展示（机器人，homeconfiguration（机器人），“父母”，斧头）;

图包含轴对象。轴对象包含28型贴片，线条。这些对象代表base_link，hompent_link，falmararm1_link，forearm_link，wrist1_link，wrist2_link，bracket_link，endeffectr_link，hompent_link_mesh，fallarm2_link_mesh，forearm_link_mesh，wrist1_link_mesh，wrist2_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”，封闭，重量，机器人.Homeconfiguration）;％显示初始和最终位置展示（机器人，StartConfig）;展示（机器人，endconfig）;

图包含轴对象。轴对象包含78个类型的补丁，行。这些对象代表base_link，hompent_link，falmararm1_link，forearm_link，wrist1_link，wrist2_link，bracket_link，endeffectr_link，hompent_link_mesh，fallarm2_link_mesh，forearm_link_mesh，wrist1_link_mesh，wrist2_link_mesh，bracelet_link_mesh，base_link_mesh。

使用梯形速度曲线曲线从原始位置生成平滑轨迹到起始位置，然后到结束位置。

q = trapveltraj（[homeconfiguration（机器人），startconfig，endconfig]，200，“EndTime”，2）;

检查使用环境中的障碍的碰撞核对机构函数。启用IgnoreSelfCollision河畔穷名称值参数。忽略自碰撞，因为机器人模型关节限制可以防止大多数自我冲突。呼道检查确保功能计算所有分离距离，并在检测到第一次碰撞后继续搜索碰撞。

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

％初始化输出infollision = false（长度（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 =任何（infollision）

isTrajectoryInCollision =逻辑1

检查发现碰撞

从最后一步开始，检测到两个碰撞。可视化这些配置以进一步研究。使用exampleHelperHighlightCollisionBodies函数根据索引突出显示主体。你可以看到球和桌子发生了碰撞。

collingidx1 = find（incollision，1）;collingidx2 = find（incollision，1，“最后的”）;识别碰撞刚体。collidingBodies1 = worldCollisionPairIdx{collidingIdx1}*[1 0]';collidingBodies2 = worldCollisionPairIdx{collidingIdx2}*[1 0]';％可视化环境。Ax = examplehelpervisualizeCollisionEn环境（WorldCollisionArray）;%添加robotconfigurations并突出碰撞的物体。显示(机器人,问:collidingIdx1),“父母”，斧头，“PreservePlot”、假);exampleHelperHighlightCollisionBodies(机器人,collidingBodies1 + 1, ax);显示(机器人,问:collidingIdx2),“父母”'，斧头）;examplehelperhighlightCollisionBodies（机器人，CollingBodies2 + 1，AX）;

图包含轴对象。轴对象包含53个类型的贴片物体，线。这些对象代表base_link，hompent_link，falmararm1_link，forearm_link，wrist1_link，wrist2_link，bracket_link，endeffectr_link，hompent_link_mesh，fallarm2_link_mesh，forearm_link_mesh，wrist1_link_mesh，wrist2_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”，中间epose1，重量，q（：，collingiddx1））;intermediateConfig2 = IK（“EndEffector_Link”intermediatePose2权重,q (:, collidingIdx2));显示新的中间姿势Ax = examplehelpervisualizeCollisionEn环境（WorldCollisionArray）;展示（机器人，intermediateConfig1，“父母”，斧头，“PreservePlot”、假);表演(intermediateConfig2的机器人“父母”，斧头）;

生成一个新的轨迹。

[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 =任何（infollision）

isTrajectoryInCollision =逻辑1

可视化生成的轨迹

有生命的结果。

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

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

图绘制(t, q)包含(“时间”）ylabel（“联合立场”）

图包含轴对象。轴对象包含7个类型的线。