检查与机械手的环境碰撞
在受限的工作空间中生成无碰撞的轨迹。
定义碰撞环境
使用碰撞原语创建一个简单的环境。这个例子创建了一个场景，一个机器人在工作空间中，必须将对象从一个表移动到另一个表。机器人还必须避开工作区上方的圆形灯具。将表格建模为两个盒子和一个球体，并指定它们在世界中的位置。更复杂的环境可以使用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);
将机器人添加到环境中
加载Kinova操作器模型作为rigidBodyTree对象使用loadrobot函数。
机器人=装载机器人(“kinovaGen3”，“DataFormat”，“列”，“重力”，[0 0 -9.81]);
显示环境中的机器人使用与碰撞物体相同的轴。机器人基地固定在世界的起源。
表演(机器人,homeConfiguration(机器人),“父”、ax);
生成一个轨迹并检查碰撞
使用使用变换矩阵乘法组合的位置向量和方向向量定义开始和结束姿态。
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);ik =逆运动学(“RigidBodyTree”,机器人);Weights = ones(1,6);startConfig = ik(“EndEffector_Link”,重量startPose robot.homeConfiguration);endConfig = ik(“EndEffector_Link”,重量endPose robot.homeConfiguration);显示初始和最终位置显示(机器人,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”，“上”，“详尽”，“上”）;[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);
生成无碰撞轨迹
为了避免这些碰撞，添加中间路径点以确保机器人绕过障碍物。
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 = ik(“EndEffector_Link”intermediatePose1权重,q (:, collidingIdx1));intermediateConfig2 = ik(“EndEffector_Link”intermediatePose2权重,q (:, collidingIdx2));显示新的中间姿势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”，“上”）;结束isTrajectoryInCollision = any(inCollision)
isTrajectoryInCollision =逻辑1
可视化生成的轨迹
动画结果。
绘制环境图ax2 = exampleHelperVisualizeCollisionEnvironment(worldCollisionArray);可视化机器人在它的家庭配置表演(startConfig的机器人“父”, ax2);更新轴的大小轴平等的遍历其他位置为I = 1:长度(q) show(机器人，q(:， I)，“父”ax2,“PreservePlot”、假);%更新图形drawnow结束
画出关节位置随时间的变化。
图(t,q) xlabel(“时间”) ylabel (“联合位置”）
您也可以从以下列表中选择一个网站: