效果:
一,需求
基于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 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);
}
参考:
Flexible Collision Library(FCL)简介及使用流程_yht9306的博客-CSDN博客_fcl碰撞检测
GitHub - mccdo/osgbullet: Bullet physics and OpenSceneGraph integration
GitHub - flexible-collision-library/fcl: Flexible Collision Library