检查与机械手的环境碰撞

打开实时脚本

在受限的工作空间中生成无碰撞的轨迹。这个例子向用户展示了如何检查碰撞轨迹和手动设计轨迹。另一种选择是使用运动规划器选择和放置使用RRT的操作器．

定义碰撞环境

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

%创建两个平台platform1 = collionbox (0.5,0.5,0.25);platform1。Pose = trvec2tform([-0.5 0.4 0.2]);platform2 = collionbox (0.5,0.5,0.25);platform2。Pose = 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函数。

机器人=装载机器人(“kinovaGen3”，“DataFormat”，“列”，“重力”，[0 0 -9.81]);

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

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

生成一个轨迹并检查碰撞

定义平台1以上的起始关节结构和平台2以上的结束关节结构。

startConfig = [3.8906 1.1924 0.0000 0.0000 0.0001 1.9454 0.7491]';endConfig = [-0.9240 1.2703 1.9865 1.2394 1.7457 -2.0500 0.4222]';显示初始和最终位置显示(机器人,startConfig);显示(机器人,endConfig);

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

q = trapveltraj([homeConfiguration(robot)，startConfig,endConfig]，200，“EndTime”2);

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

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

初始化输出inCollision = false(长度(q)， 1);检查每个姿势是否碰撞worldCollisionPairIdx = cell(长度(q)，1);提供发生碰撞的物体为i = 1:length(q) [inCollision(i)，sepDist] = checkCollision(robot,q(:，i)，worldCollisionArray，“IgnoreSelfCollision”，“上”，“详尽”，“上”，“SkippedSelfCollisions”，“父”）;[bodyIdx,worldCollisionObjIdx] = find(isnan(sepDist));查找碰撞对worldCollidingPairs = [bodyIdx,worldCollisionObjIdx];worldCollisionPairIdx{i} = worldCollidingPairs;结束isTrajectoryInCollision = any(inCollision)

isTrajectoryInCollision =逻辑1

检查检测到的碰撞

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

collidingIdx1 = find(inCollision,1);collidingIdx2 = find(inCollision,1，“最后一次”）;识别碰撞刚体。collidingBodies1 = worldCollisionPairIdx{collidingIdx1}*[1 0]';collidingBodies2 = worldCollisionPairIdx{collidingIdx2}*[1 0]';可视化环境。ax = exampleHelperVisualizeCollisionEnvironment(worldCollisionArray);添加机器人配置并突出碰撞物体。显示(机器人,问:collidingIdx1),“父”ax,“PreservePlot”、假);exampleHelperHighlightCollisionBodies(机器人，collidingBodies1 + 1,ax);显示(机器人,问:collidingIdx2),“父””,ax);exampleHelperHighlightCollisionBodies(机器人，collidingBodies2 + 1,ax);

生成无碰撞轨迹

为了避免这些碰撞，添加中间关节配置，以确保机器人绕过障碍物。

intermediateConfig1 = [-0.3644 -1.8365 0.0430 1.6606 0.2889 0.9386 0.1271]';intermediateConfig2 =[-1.8895 0.6716 2.2225 1.4608 2.5503 -2.0500 0.1536]';显示新的中间姿势ax = exampleHelperVisualizeCollisionEnvironment(worldCollisionArray);表演(intermediateConfig1的机器人“父”ax,“PreservePlot”、假);表演(intermediateConfig2的机器人“父”、ax);

生成一个新的轨迹。

[q,qd,qdd,t] = trapveltraj([homeConfiguration(robot)，intermediateConfig1,startConfig,intermediateConfig2,endConfig]，200，“EndTime”2);

验证它没有碰撞。

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

isTrajectoryInCollision =逻辑0

可视化生成的轨迹

动画结果。

绘制环境图ax2 = exampleHelperVisualizeCollisionEnvironment(worldCollisionArray);可视化机器人在它的家庭配置表演(startConfig的机器人“父”, ax2);更新轴的大小轴平等的遍历其他位置为I = 1:长度(q) show(机器人，q(:， I)，“父”ax2,“PreservePlot”、假);%更新图形drawnow结束

画出关节位置随时间的变化。

图(t,q) xlabel(“时间”) ylabel (“联合位置”）

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