基于碰撞网格的机器人碰撞检测算法研究

碰撞网格是通过机器人模型URDF中定义的<collision>下面的相关例子展示了如何以其他方式加载碰撞数据,以及如何检查环境碰撞:URDF捕捉机器人刚体的碰撞网格文件单独地向刚体添加碰撞对象,可以使用addCollision函数。加载所提供的带有附加碰撞对象的机器人模型iiwa = importrobot(‘iiwa14.urdf’);iiwa.DataFormat = ‘column’;生成轨迹并检测是否碰撞将开始和结束设定为一组关节位置矩阵,这些位置是无碰撞

碰撞网格是通过机器人模型URDF中定义的<collision>

下面的相关例子展示了如何以其他方式加载碰撞数据,以及如何检查环境碰撞:

URDF捕捉机器人刚体的碰撞网格文件

单独地向刚体添加碰撞对象,可以使用addCollision函数。

加载所提供的带有附加碰撞对象的机器人模型

iiwa = importrobot('iiwa14.urdf');
iiwa.DataFormat = 'column';

生成轨迹并检测是否碰撞

将开始和结束设定为一组关节位置矩阵,这些位置是无碰撞的。

startConfig = [0 -pi/4 pi 3*pi/2 0 -pi/2 pi/8]';
goalConfig = [0 -pi/4 pi 3*pi/4 0 -pi/2 pi/8]';

三秒内找到连接两个构型的关节空间轨迹:

q = trapveltraj([startConfig goalConfig],100,'EndTime',3);

为了验证输出轨迹不包含自碰撞,迭代输出样本,并使用checkCollision函数查看是否有任何点在碰撞中。

当遍历轨迹q时,在轨迹中的每个配置上调用checkCollision函数。

打开穷举check,以在检测到第一个碰撞后继续检测碰撞。

isConfigInCollision变量跟踪每个配置的冲突状态。

sepDistForConfig跟踪机器人身体之间的分离距离。

对于每个碰撞,主体索引对存储在configCollisionPairs变量中。

注意,相邻的物体不能被检测到,因为是通过连接它们的关节接触。

isConfigInCollision = false(100,1);
configCollisionPairs = cell(100,1);
sepDistForConfig = zeros(iiwa.NumBodies+1,iiwa.NumBodies+1,100);
for i = 1:length(q)
    [isColliding, sepDist] = checkCollision(iiwa,q(:,i),'Exhaustive','on');
    isConfigInCollision(i) = isColliding;
    sepDistForConfig(:,:,i) = sepDist;
end

要找出冲突中的主体的索引,请找出sepDistForConfig中的哪些条目是NaN。

septDist是一个对称矩阵,因此在索引翻转时返回相同的值。使用unique来简化列表。

for i = 1:length(q)
    sepDist = sepDistForConfig(:,:,i);
    [body1Idx,body2Idx] = find(isnan(sepDist));

    collidingPairs = unique(sort([body1Idx,body2Idx],2));
    configCollisionPairs{i} = collidingPairs;
end

通过检查输出,可看到经过一系列的碰撞规划轨迹。

可视化第一次碰撞发生的位形,并突出显示物体。

any(isConfigInCollision)
firstCollisionIdx = find(isConfigInCollision,1);

% Visualize the first configuration that is in collision.
figure;
show(iiwa,q(:,firstCollisionIdx));
exampleHelperHighlightCollisionBodies(iiwa,configCollisionPairs{firstCollisionIdx}+1,gca);

版权声明:本文为博主FL17171314原创文章,版权归属原作者,如果侵权,请联系我们删除!

原文链接:https://blog.csdn.net/weixin_51367832/article/details/122584695

共计人评分,平均

到目前为止还没有投票!成为第一位评论此文章。

(0)
xiaoxingxing的头像xiaoxingxing管理团队
上一篇 2022年1月19日
下一篇 2022年1月19日

相关推荐