机器人碰撞检测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距离最近的点。
运行结果如下：