机器人碰撞检测1:FCL库安装与使用

一、简介

FCL库(The Flexible Collision Library)主要的功能有:
1、碰撞检测:检测两个模型是否重叠,以及(可选)所有重叠的三角形。
2、距离计算:计算一对模型之间的最小距离,即最近的一对点之间的距离。
3、公差验证:确定两个模型是否比公差距离更近或更远。
4、连续碰撞检测:检测两个运动模型在运动过程中是否重叠,以及可选的接触时间。
5、接触信息:对于碰撞检测和连续碰撞检测,可以选择返回接触信息(包括接触法线和接触点)。
在这里插入图片描述
FCL库支持的形状类型:

  • box
  • sphere(球)
  • ellipsoid(椭球)
  • capsule(胶囊体)
  • cone(锥体)
  • cylinder(圆柱)
  • convex(凸包)
  • half-space(半空间)
  • plane(平面)
  • mesh(面片)
  • octree (八叉树)

二、FCL库安装

(使用的环境是ubuntu20.04)
第一步,把代码clone到本地:

git clone https://github.com/flexible-collision-library/fcl.git

第二步,在代码所在的目录进行编译:

cmake -S . -B build
sudo cmake --build build/ --target install

到此就可以正常使用FCL库了。
如果编译失败,一般是依赖出了问题,确保以下的库已经被正确安装:

  • Eigen (available at http://eigen.tuxfamily.org/)
  • libccd (available at http://libccd.danfis.cz/)
  • octomap (optional dependency, available at http://octomap.github.com)

三、FCL库初步使用

3.1 使用流程

以下均是README.md的搬运,强烈建议看原文

  • 设置碰撞物体的形状:
// set mesh triangles and vertice indices
std::vector<Vector3f> vertices;
std::vector<Triangle> triangles;
// code to set the vertices and triangles
...
// BVHModel is a template class for mesh geometry, for default OBBRSS template
// is used
typedef BVHModel<OBBRSSf> Model;
std::shared_ptr<Model> geom = std::make_shared<Model>();
// add the mesh data into the BVHModel structure
geom->beginModel();
geom->addSubModel(vertices, triangles);
geom->endModel();
  • 设置碰撞物体的变换(旋转和位置):
// R and T are the rotation matrix and translation vector
Matrix3f R;
Vector3f T;
// code for setting R and T
...
// transform is configured according to R and T
Transform3f pose = Transform3f::Identity();
pose.linear() = R;
pose.translation() = T;
  • 将物体与变换结合,构建碰撞实例:
//geom and tf are the geometry and the transform of the object
std::shared_ptr<BVHModel<OBBRSSf>> geom = ...
Transform3f tf = ...
//Combine them together
CollisionObjectf* obj = new CollisionObjectf(geom, tf);
  • 碰撞检测:
// Given two objects o1 and o2
CollisionObjectf* o1 = ...
CollisionObjectf* o2 = ...
// set the collision request structure, here we just use the default setting
CollisionRequest request;
// result will be returned via the collision result structure
CollisionResult result;
// perform collision test
collide(o1, o2, request, result);
  • 计算物体间距离:
// Given two objects o1 and o2
CollisionObjectf* o1 = ...
CollisionObjectf* o2 = ...
// set the distance request structure, here we just use the default setting
DistanceRequest request;
// result will be returned via the collision result structure
DistanceResult result;
// perform distance test
distance(o1, o2, request, result);
  • 连续的碰撞检测,需要设置目标位置
// Given two objects o1 and o2
CollisionObjectf* o1 = ...
CollisionObjectf* o2 = ...
// The goal transforms for o1 and o2
Transform3f tf_goal_o1 = ...
Transform3f tf_goal_o2 = ...
// set the continuous collision request structure, here we just use the default
// setting
ContinuousCollisionRequest request;
// result will be returned via the continuous collision result structure
ContinuousCollisionResult result;
// perform continuous collision test
continuousCollide(o1, tf_goal_o1, o2, tf_goal_o2, request, result);
  • Broadphase碰撞检测
    以上的操作均是Narrowphase的碰撞检测,narrowphase指的是两个物体间的碰撞检测,Broadphase指的是物体和一组物体间的碰撞检测或者两组物体间的碰撞检测,一组物体是用包围盒进行包围的,Broadphase主要用于大范围的碰撞检测,经常作为Narrowphase的前置。
// Initialize the collision manager for the first group of objects. 
// FCL provides various different implementations of CollisionManager.
// Generally, the DynamicAABBTreeCollisionManager would provide the best
// performance.
BroadPhaseCollisionManagerf* manager1 = new DynamicAABBTreeCollisionManagerf(); 
// Initialize the collision manager for the second group of objects.
BroadPhaseCollisionManagerf* manager2 = new DynamicAABBTreeCollisionManagerf();
// To add objects into the collision manager, using
// BroadPhaseCollisionManager::registerObject() function to add one object
std::vector<CollisionObjectf*> objects1 = ...
for(std::size_t i = 0; i < objects1.size(); ++i)
manager1->registerObject(objects1[i]);
// Another choose is to use BroadPhaseCollisionManager::registerObjects()
// function to add a set of objects
std::vector<CollisionObjectf*> objects2 = ...
manager2->registerObjects(objects2);
// In order to collect the information during broadphase, CollisionManager
// requires two settings:
// a) a callback to collision or distance; 
// b) an intermediate data to store the information generated during the
//    broadphase computation.
// For convenience, FCL provides default callbacks to satisfy a) and a
// corresponding call back data to satisfy b) for both collision and distance
// queries. For collision use DefaultCollisionCallback and DefaultCollisionData
// and for distance use DefaultDistanceCallback and DefaultDistanceData.
// The default collision/distance data structs are simply containers which
// include the request and distance structures for each query type as mentioned
// above.
DefaultCollisionData collision_data;
DefaultDistanceData distance_data;
// Setup the managers, which is related with initializing the broadphase
// acceleration structure according to objects input
manager1->setup();
manager2->setup();
// Examples for various queries
// 1. Collision query between two object groups and get collision numbers
manager2->collide(manager1, &collision_data, DefaultCollisionFunction);
int n_contact_num = collision_data.result.numContacts(); 
// 2. Distance query between two object groups and get the minimum distance
manager2->distance(manager1, &distance_data, DefaultDistanceFunction);
double min_distance = distance_data.result.min_distance;
// 3. Self collision query for group 1
manager1->collide(&collision_data, DefaultCollisionFunction);
// 4. Self distance query for group 1
manager1->distance(&distance_data, DefaultDistanceFunction);
// 5. Collision query between one object in group 1 and the entire group 2
manager2->collide(objects1[0], &collision_data, DefaultCollisionFunction);
// 6. Distance query between one object in group 1 and the entire group 2
manager2->distance(objects1[0], &distance_data, DefaultDistanceFunction);

3.2 FCL库使用实例

/test文件夹下存在这大量的示例,涵盖了FCL库的基本用法,是很好的FCL库学习资料,强烈建议学习一遍
在这里插入图片描述
比如运行test_fcl_collision:

./build/test/test_fcl_collision

在这里插入图片描述
以下源码地址:https://github.com/Ning-Dan/robot-collision-detect

3.2.1 Box与Box碰撞检测

文件夹结构如图:
在这里插入图片描述
main.cpp内容如下:

#include "fcl/math/constants.h"
#include "fcl/narrowphase/collision.h"
#include "fcl/narrowphase/collision_object.h"
#include "fcl/narrowphase/distance.h"

void test1(){
    std::shared_ptr<fcl::CollisionGeometry<double>> box1(new fcl::Box<double>(3,3,3));
    std::shared_ptr<fcl::CollisionGeometry<double>> box2(new fcl::Box<double>(1,1,1));

    fcl::Transform3d tf1 = fcl::Transform3d::Identity();
    fcl::CollisionObjectd obj1(box1,tf1);

    fcl::Transform3d tf2 = fcl::Transform3d::Identity();
    fcl::CollisionObjectd obj2(box2,tf2);

    fcl::CollisionRequestd request;
    fcl::CollisionResultd result;

    request.gjk_solver_type = fcl::GJKSolverType::GST_INDEP;//specify solver type with the default type is GST_LIBCCD

    fcl::collide(&obj1,&obj2,request,result);

    std::cout<<"test1 collide result:"<<result.isCollision()<<std::endl;
}

void test2(){
    std::shared_ptr<fcl::CollisionGeometry<double>> box1(new fcl::Box<double>(3,3,3));
    std::shared_ptr<fcl::CollisionGeometry<double>> box2(new fcl::Box<double>(1,1,1));

    fcl::Transform3d tf1 = fcl::Transform3d::Identity();
    fcl::CollisionObjectd obj1(box1,tf1);

    fcl::Transform3d tf2 = fcl::Transform3d::Identity();
    tf2.translation() = fcl::Vector3d{3,0,0};

    fcl::CollisionObjectd obj2(box2,tf2);

    fcl::CollisionRequestd request;
    fcl::CollisionResultd result;

    fcl::collide(&obj1,&obj2,request,result);

    std::cout<<"test2 collide result:"<<result.isCollision()<<std::endl;
}

void test3(){
    std::shared_ptr<fcl::CollisionGeometry<double>> box1(new fcl::Box<double>(3,3,3));
    std::shared_ptr<fcl::CollisionGeometry<double>> box2(new fcl::Box<double>(1,1,1));

    fcl::Transform3d tf1 = fcl::Transform3d::Identity();
    fcl::CollisionObjectd obj1(box1,tf1);

    fcl::Transform3d tf2 = fcl::Transform3d::Identity();
    tf2.translation() = fcl::Vector3d{3,0,0};

    fcl::CollisionObjectd obj2(box2,tf2);

    fcl::CollisionRequestd request;
    fcl::CollisionResultd result;

    // fcl::collide(&obj1,&obj2,request,result);

    std::cout<<"test3 collide result:"<<result.isCollision()<<std::endl;

    fcl::DistanceRequestd dist_request(true);
    dist_request.distance_tolerance = 1e-4;
    fcl::DistanceResultd dist_result;

    fcl::distance(&obj1,&obj2,dist_request,dist_result);

    std::cout<<"test3 collide distance:"<<dist_result.min_distance<<std::endl;
    std::cout<<"test3 collide point 0:"<<dist_result.nearest_points[0]<<std::endl;
    std::cout<<"test3 collide point 1:"<<dist_result.nearest_points[1]<<std::endl;
    
}

int main(int argc,char **argv){
    test1();
    test2();
    test3();
}

CMakeLists.txt内容如下:

cmake_minimum_required(VERSION 3.14)
find_package(Eigen3 REQUIRED)
find_package(FCL REQUIRED)
add_executable(use_fcl main.cpp)
target_link_libraries(use_fcl fcl Eigen3::Eigen)
target_include_directories(use_fcl PUBLIC ${EIGEN3_INCLUDE_DIRS} ${FCL_INCLUDE_DIRS})

test1中测试了两个Box碰撞的情况,test2测试了两个Box没有发生碰撞的情况,test3中测试了两个Box没有发生碰撞,计算了两个Box的最小距离,计算了两个Box距离最近的点。
运行结果如下:
在这里插入图片描述

  • 24
    点赞
  • 54
    收藏
    觉得还不错? 一键收藏
  • 8
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 8
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值