机器人碰撞检测——目标轨迹生成

根据前文的介绍，接下来可进行机器人目标轨迹的生成。

生成轨迹，检查碰撞情况

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

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]);

根据所需要的姿态，使用逆运动学来求解关节位置。

验证配置是否有效

% Use a fixed random seed to ensure repeatable results
rng(0);
ik = inverseKinematics("RigidBodyTree",robot);
weights = ones(1,6);
startConfig = ik("EndEffector_Link",startPose,weights,robot.homeConfiguration);
endConfig = ik("EndEffector_Link",endPose,weights,robot.homeConfiguration);

% Show initial and final positions
show(robot,startConfig);
show(robot,endConfig);

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

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

使用checkCollision函数检查与环境中的障碍物的碰撞。

由于机器人模型关节限制防止了大多数自碰撞，因此忽略了自碰撞。详尽检查确保函数计算所有分离距离，并在检测到第一次碰撞后继续搜索碰撞。

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

% Initialize outputs
inCollision = false(length(q), 1); % Check whether each pose is in collision
worldCollisionPairIdx = cell(length(q),1); % Provide the bodies that are in collision

for i = 1:length(q)
    
    [inCollision(i),sepDist] = checkCollision(robot,q(:,i),worldCollisionArray,"IgnoreSelfCollision","on","Exhaustive","on");
    
    [bodyIdx,worldCollisionObjIdx] = find(isnan(sepDist)); % Find collision pairs
    worldCollidingPairs = [bodyIdx,worldCollisionObjIdx]; 
    worldCollisionPairIdx{i} = worldCollidingPairs;
    
end
isTrajectoryInCollision = any(inCollision)

计算出的结果为：

isTrajectoryInCollision = logical
   1