基于OSG 和FCL 的碰撞仿真

效果：

一，需求

基于osg 和fcl实现碰撞仿真。

前期使用osg 结合bullet 实现，但是项目中涉及到点云的碰撞，没有找到bullet如何实现点云的碰撞,因此放弃了bullet。而FCL对点云的碰撞支持很好，所以果断选择FCL。

二，实现

FCL碰撞检测流程与bullet基本相似，构造集合体，设置位姿矩阵，我们以box为例。

（1），构造几何体

void FCLManager::addBox(const std::string& name,float w, float d, float h)
{auto box_geometry = std::make_shared<fcl::Boxf>(w, d, h);auto ob = new fcl::CollisionObjectf(box_geometry);obstacleMap.emplace(name,ob);
}

（2），更新位姿矩阵

void FCLManager::updateTrans(const std::string &name, const fcl::Transform3f &trans)
{fcl::CollisionObjectf *ob=getCollisionObject(name);if(ob){ob->setTransform(trans);}
}//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;
}

三，更简单的方法

FCL支持三角面的碰撞检测，因此可以将所有OSG 的node转为三角面进行检测。

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

四，点云的碰撞

点云的碰撞使用了一种叫做八叉树的算法。

首先将点云转成pcl的点云格式，然后可以直接构造出fcl实体

fcl::CollisionObjectf* FCLManager::createPointCloudCollisionObject(const pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud_ptr, const octomap::point3d &origin_3d)
{// octomap octree settingsconst 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();
#elseint thread_id = 0;int thread_num = 1;
#endifint 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 cloudfor (auto it = free_cells.begin(); it != free_cells.end(); ++it){if (occupied_cells.find(*it) == occupied_cells.end()){octomap_octree->updateNode(*it, false);}}// occupied cellsfor (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);
}

参考：

Flexible Collision Library（FCL）简介及使用流程_yht9306的博客-CSDN博客_fcl碰撞检测

GitHub - mccdo/osgbullet: Bullet physics and OpenSceneGraph integration

GitHub - flexible-collision-library/fcl: Flexible Collision Library

基于OSG 和FCL 的碰撞仿真相关推荐

基于锁相环的调制解调仿真实现
基于锁相环的调制解调仿真实现论文+代码+实验结果下载地址:下载地址摘要随着现代集成电路技术的发展,锁相环已经成为集成电路设计中非常重要的一个部分,所以对锁相环的研究具有积极的现实意义.锁相环电路 ...
计算机联锁仿真软件设计,一种基于LabVIEW的计算机联锁仿真系统的制作方法
本发明涉及一种计算机联锁仿真系统,尤其是涉及一种基于labview的计算机联锁仿真系统. 背景技术: 计算机联锁系统的仿真系统,通常是采用c/c++等传统的编程语言实现,一方面该方式需要专业的程序员去 ...
基于FPGA实现PCIE IP功能仿真
基于FPGA实现PCIE IP功能仿真 1 开发工具 modelsim simulator或vivado simulator,本设计采用modelsim进行仿真. 2 参数配置
matlab频分复用,基于MATLAB的频分复用系统的仿真_.doc
基于MATLAB的频分复用系统的仿真_ 基于MATLAB的频分复用系统的仿真_毕业论文(设计) Abstract With the development of communication techn ...
matlab故障识别,基于Matlab的电力系统故障分析与仿真（V2.1）最新版
<基于Matlab的电力系统故障分析与仿真.doc>由会员分享,可免费在线阅读全文,更多与<基于Matlab的电力系统故障分析与仿真(V2.1)>相关文档资源请在帮帮文库(ww ...
matlab非线性系统频域标识,基于MATLAB的最小二乘法系统辨识与仿真
第 29卷第 2期许昌学院学报 Vol. 29. No. 2 2010年 3月 JOURNAL OF XUCHANG UN IVERSITY Mar. 2010 收稿日期: 2008 - 10 - ...
matlab 自激振荡,基于Simulink的非线性系统自激振荡的仿真
基于Simulink的非线性系统自激振荡的仿真研究张聚,杨马英,王万良 (浙江工业大学信息工程学院自动化系,浙江杭州310032) 摘要:针对描述函数法分析非线性系统自激振荡的局限性,提出了 ...
tcsc工作原理matlab仿真,基于Matlab的TCSC建模与仿真研究.doc
基于Matlab的TCSC建模与仿真研究基于Matlab的TCSC建模与仿真研究第17卷第5期 2006年1O月巾原T学院 JOURNALOFZHONGYUANINSTIT[ITEOFTECHN ...
【基于MATLAB的火灾疏散模拟仿真】——安全隐患提前发现，疏散方案优化
[基于MATLAB的火灾疏散模拟仿真]--安全隐患提前发现,疏散方案优化随着城市化进程的不断加速,人口密度越来越大,特别是在高层建筑中.万一发生火灾,往往会对人的生命和财产造成严重损失.因此,火灾疏 ...

基于OSG 和FCL 的碰撞仿真

基于OSG 和FCL 的碰撞仿真相关推荐

最新文章

热门文章