一,需求
利用OSG结合FCL实现实现精准的碰撞检测。
二,效果 看这里
利用FCL实现更加精准的碰撞检测 – Qt hello
三,分析
我们看如下这张图,碰撞的逻辑就是,在一个三维场景中,构造一个实体,比如下边的BoxA,然后在物理引擎比如bullet中,或者专用的碰撞检测库中也构造一个对应的实体,比如BoxB。之后在BoxA位姿改变时后,将BoB的位姿也做相应的更新。之后发生碰撞时,物理引擎或者FCL就会给出信号。而这个场景,可以是VTK或者OSG。而碰撞检测可以用Bullet也可以用FCL。
之前用bullet做个尝试,基本的图形能满足需求,比如球,盒子,但是项目中涉及到点云的碰撞,而bullet中处理点云,没有找到好的处理方式。但是FCL可以将点云转变成fcl中对应的实体,因此最终选择了FCL进行碰撞检测,这里列出FCL中大概的步骤。
1,FCL中构造实体。这里构造了一个 盒子。
auto box_geometry = std::make_shared<fcl::Boxf>(w, d, h); auto ob = new fcl::CollisionObjectf(box_geometry);
2,更新FCL实体的位姿矩阵。
void FCLManager::updateTrans(const std::string &name, const fcl::Transform3f &trans) { fcl::CollisionObjectf *ob=getCollisionObject(name); if(ob){ ob->setTransform(trans); } } //OSG 矩阵 需要进行转换 才能给到FCL使用 //osg 矩阵转fcl矩阵 osg::Vec3 osgTrans = mt.getTrans(); // 获取平移分量 osg::Quat osgQuat = mt.getRotate(); // 获取旋转分量 fcl::Quaternionf rotation(osgQuat.w(), osgQuat.x(), osgQuat.y(), osgQuat.z()); fcl::Vector3f translation(osgTrans.x(), osgTrans.y(), osgTrans.z()); fcl::Transform3f fclTrans=fcl::Transform3f::Identity(); fclTrans.translation() = translation; fclTrans.linear()=rotation.toRotationMatrix(); FCLManager::getInstance()->updateTrans(this->getName(),fclTrans);
3,碰撞检测
我是检测机器人和其它障碍物的碰撞,这里把机器人关节放到一个集合中,把其它障碍物放到另一个集合中
bool FCLManager::detectCollision() { fcl::CollisionRequestf request; fcl::CollisionResultf result; for(auto &ob1:jointMap){ for(auto &ob2:obstacleMap){ collide(ob1.second, ob2.second, request, result); if(result.isCollision()){ return true; } } } return false; }
4,FCL支持三角面检测。因此我们在FCL中构造对应实体的时候,可以直接用三角面。这样不管OSG中构造的时盒子还是球,还是导入的stl,对应FCL中都是统一用三角面处理。
void FCLManager::addTriMesh(const std::string &name, osg::Node *node) { fcl::CollisionObjectf *obj = createNodeCollisionObject(node); obstacleMap.emplace(name,obj); } fcl::CollisionObjectf *FCLManager::createNodeCollisionObject(osg::Node *node) { MyComputeTriMeshVisitor visitor; node->accept( visitor ); osg::Vec3Array* vertices = visitor.getTriMesh(); typedef fcl::BVHModel<fcl::OBBRSSf> Model; Model* model = new Model(); std::shared_ptr<fcl::CollisionGeometryf> m1_ptr(model); model->beginModel(); osg::Vec3 p1, p2, p3; for( size_t i = 0; i + 2 < vertices->size(); i += 3 ) { p1 = vertices->at( i ); p2 = vertices->at( i + 1 ); p3 = vertices->at( i + 2 ); fcl::Vector3<float> pp1{p1.x(),p1.y(),p1.z()}; fcl::Vector3<float> pp2{p2.x(),p2.y(),p2.z()}; fcl::Vector3<float> pp3{p3.x(),p3.y(),p3.z()}; model->addTriangle(pp1, pp2, pp3); } model->endModel(); model->computeLocalAABB(); return new fcl::CollisionObjectf(m1_ptr); }
5,点云的碰撞。点云的碰撞 使用了一种叫做八叉树的算法。首先将点云转成pcl的点云 格式,然后可以直接构造出fcl实体,这也是选用FCL的原因。
fcl::CollisionObjectf* FCLManager::createPointCloudCollisionObject(const pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud_ptr, const octomap::point3d &origin_3d) { // octomap octree settings const double resolution = 0.01; const double prob_hit = 0.9; const double prob_miss = 0.1; const double clamping_thres_min = 0.12; const double clamping_thres_max = 0.98; std::shared_ptr<octomap::OcTree> octomap_octree = std::make_shared<octomap::OcTree>(resolution); octomap_octree->setProbHit(prob_hit); octomap_octree->setProbMiss(prob_miss); octomap_octree->setClampingThresMin(clamping_thres_min); octomap_octree->setClampingThresMax(clamping_thres_max); octomap::KeySet free_cells; octomap::KeySet occupied_cells; #if defined(_OPENMP) #pragma omp parallel #endif { #if defined(_OPENMP) auto thread_id = omp_get_thread_num(); auto thread_num = omp_get_num_threads(); #else int thread_id = 0; int thread_num = 1; #endif int start_idx = static_cast<int>(pointcloud_ptr->size() / thread_num) * thread_id; int end_idx = static_cast<int>(pointcloud_ptr->size() / thread_num) * (thread_id + 1); if (thread_id == thread_num - 1) { end_idx = pointcloud_ptr->size(); } octomap::KeySet local_free_cells; octomap::KeySet local_occupied_cells; for (auto i = start_idx; i < end_idx; i++) { octomap::point3d point((*pointcloud_ptr)[i].x, (*pointcloud_ptr)[i].y, (*pointcloud_ptr)[i].z); octomap::KeyRay key_ray; if (octomap_octree->computeRayKeys(origin_3d, point, key_ray)) { local_free_cells.insert(key_ray.begin(), key_ray.end()); } octomap::OcTreeKey tree_key; if (octomap_octree->coordToKeyChecked(point, tree_key)) { local_occupied_cells.insert(tree_key); } } #if defined(_OPENMP) #pragma omp critical #endif { free_cells.insert(local_free_cells.begin(), local_free_cells.end()); occupied_cells.insert(local_occupied_cells.begin(), local_occupied_cells.end()); } } // free cells only if not occupied in this cloud for (auto it = free_cells.begin(); it != free_cells.end(); ++it) { if (occupied_cells.find(*it) == occupied_cells.end()) { octomap_octree->updateNode(*it, false); } } // occupied cells for (auto it = occupied_cells.begin(); it != occupied_cells.end(); ++it) { octomap_octree->updateNode(*it, true); } auto fcl_octree = std::make_shared<fcl::OcTree<float>>(octomap_octree); std::shared_ptr<fcl::CollisionGeometryf> fcl_geometry = fcl_octree; return new fcl::CollisionObjectf(fcl_geometry); }
四,总结 看这里
利用FCL实现更加精准的碰撞检测 – Qt hello