检查与操纵器的环境碰撞
在受限的工作空间中生成无碰撞轨迹。这个例子向用户展示了如何检查碰撞轨迹和手动设计轨迹。另一种选择是使用运动计划器对操纵器使用RRT进行选择和放置．
定义碰撞环境
使用碰撞原语创建一个简单的环境。这个例子创建了一个场景，其中一个机器人在一个工作区中，必须将对象从一个表移动到另一个表。机器人还必须避免工作区上方的圆形灯具。将表格建模为两个盒子和一个球体，并指定它们在世界中的位置。可以使用创建更复杂的环境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。Pose = trvec2tform([0.5 0.2 0.2]);添加一个灯具，建模为一个球体lightFixture = collisionSphere (0.1);lightFixture。= trvec2tform([。2 0 1]);%存储在单元格数组中以进行碰撞检查worldCollisionArray = {platform1 platform2 lightFixture};
使用迭代冲突数组的助手函数可视化环境。
ax = exampleHelperVisualizeCollisionEnvironment (worldCollisionArray);
将机器人添加到环境中
加载一个Kinova操纵器模型作为rigidBodyTree对象使用loadrobot函数。
机器人= 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(机器人),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”，“上”，“详尽”，“上”，“SkippedSelfCollisions”，“父”）;[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);添加机器人配置并突出显示碰撞的身体。。显示(机器人,问: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(机器人),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 =任何(inCollision)
isTrajectoryInCollision =逻辑0
可视化生成的轨迹
有生命的结果。
绘制环境图ax2 = exampleHelperVisualizeCollisionEnvironment (worldCollisionArray);可视化机器人在它的家庭配置表演(startConfig的机器人“父”, ax2);更新轴的大小轴平等的遍历其他位置为I = 1:length(q) show(robot,q(:， I)，“父”ax2,“PreservePlot”、假);%更新图drawnow结束
画出关节位置随时间的变化。
图绘制(t, q)包含(“时间”) ylabel (“联合位置”）
您也可以从以下列表中选择网站: