Question

机械臂应在混乱的环境中移动。它的手腕上装有RGB-D传感器，可绘制周围环境的地图。先验知识是场景的某些部分（即在机器人后面-> y <0以及安装在它上面的表面上方-> z> 0）是自由空间。当手臂四处移动并将来自不同姿势的深度图像融合到octomap中时，可以获得更多知识。与MoveIt！的默认行为（该行为在未明确称为占用空间的所有octomap中进行计划）相反，我想将运动计划限制在已知没有先验知识或先前扫描的区域。

到目前为止，我研究了两种可能的行动方案：

1。）修改融合深度感应产生的octomap，然后再将其发布到Moveit的planning_scene主题。我希望内部能够用占据的细胞（p = 0.97）替换未知细胞（p = 0.5）。

for(octomap::OcTree::tree_iterator it = octomap->begin_tree(), end=octomap->end_tree(); it!= end; ++it)
    {
        if(it.getY() <= 0.1) { // behind robot --> assume free space over table
            if(it.getZ() > 0.) { // above table --> free
                it->setLogOdds(-2.);
            } else {            // below table --> occupied
                it->setLogOdds(3.5);
            }
        } else {                             // before robot only above threshold are non-occupied
            if(it->getLogOdds() > -1.9) it->setLogOdds(3.5);
        }
    }

问题似乎是，此迭代器仅遍历八叉树的单元格，而这些单元格之前至少被看到过一次。视线未触摸的区域将被忽略。因此，Moveit会将这些占用率未知的区域用于导航。

2。）更改移动碰撞检测行为。我发现moveit_core / collision_detection / src / collision_octomap_filter.cpp中的moveit似乎仅与octomap交互（在本例中为导入）。在第124行中，我测试了octree->isNodeOccupied(*it)，我认为这可能是更改行为的另一种方法，例如用it->getLogOdds() > -1.9替换此函数调用。遗憾的是，此处的更改似乎并不影响运动计划器。旁边的实际打印语句也未执行，这使我相信我的当前配置（来自Panda机器人的moveit教程）可以在另一个不太明显的moveit位置处理计划场景的octomap的碰撞检查。

欢迎提出建议，问题和进一步的想法！

限制Moveit！运动计划者，以探索先前使用Octomap编码的空间

0 个答案: