开源推荐：碰撞检测算法fcl实现python-fcl

大家好，今天下午写技术规划PPT，其中提到了如何解决机械臂的在空间中避障的问题，所以就把之前使用的pyhton-fcl库重新来讲讲。

本节主要介绍下FCL以及Python-FCL的安装和基本使用。

1.FCL介绍

FCL是一个非常灵活的碰撞检测库，主要用于对几何模型执行接近和碰撞的查询检测。

FCL非常有名，著名机械臂控制框架moveit、OpenRave等所使用的碰撞检测库就是它，有C++和Python两个版本的代码。
作者集成了几种快速准确的碰撞检测和距离计算库，提供离散碰撞检测、连续的碰撞检测、距离计算、穿透深度估算四种功能。

python-fcl提供三种计算：

碰撞检测：检测两个模型是否重叠（以及可选的位置）。
距离计算：计算一对模型之间的最小距离。
连续碰撞检测：检测两个模型在运动过程中是否重叠（以及可选的接触时间）。

关于模型的形状也有非常多的选择：TriangleP、Box、Sphere、Ellipsoid、Capsule、 Cone、 Cylinder、Half-Space、 Plane、 Mesh、OcTree

开源地址：https://github.com/flexible-collision-library/fcl
Python版本开源地址：https://github.com/BerkeleyAutomation/python-fcl
文档地址：https://flexible-collision-library.github.io/
论文名称：FCL: A General Purpose Library for Collision and Proximity Queries（小鱼发在qq交流群：139707339）

2.Python-FCL安装

python-fcl是基于fcl官方的C++版本库进行的二次封装，这样我们如果使用python做算法开发时就可以很方便的调用到。

2.1 安装

sudo apt install liboctomap-dev libfcl-dev python3-pip
sudo pip3 install python-fcl

可以看到fcl是依赖与numpy的，安装fcl库，numpy也会自动安装。

3.基本使用

3.1 概念辨析

在正式使用fcl之前要搞清楚两个概念，第一个概念是碰撞对象，第二个概念是碰撞操作。

碰撞对象

要想检测两个物体之间有没有碰撞，我们肯定要告知fcl这两体的形状和位置，fcl根据我们的描述来判断两个物体之间的距离，从而判断是否发生碰撞，所以碰撞对象就是由对象几何形状和对象位姿两者组成的。

在fcl中对应名称如下：

碰撞对象：CollisionObject
几何形状：CollisionGeometry
位姿：Transform

碰撞操作

碰撞操作就是对我们定义的碰撞对象执行一些查询操作，目前支持的有三种：1.碰撞检测、2.距离计算、3.连续碰撞检测

接着小鱼用程序在jupyter-notebook中来体验下。

3.2 导入fcl

同时把numpy也导入下

import fcl
import numpy as np

3.3 构造碰撞对象

# 定义两个形状
box = fcl.Box(1, 1, 1)          # 三边长度1, 1, 1 定义与坐标轴轴对齐的box
sphere = fcl.Sphere(1)           # 半径1 定义Sphere# 定义两个位姿,并使其保持一致
T = np.array([1.0, 2.0, 3.0]) # 平移矩阵表示位置
q = np.array([0.707, 0.0, 0.0, 0.707]) # 四元数，表示姿态
box_tf = fcl.Transform(q, T) # 四元数旋转与平移
sphere_tf = fcl.Transform(tf)  # 也可以用另一个Transform初始化# 通过形状和位置，生成碰撞对象
box_obj = fcl.CollisionObject(box,box_tf)
sphere_obj = fcl.CollisionObject(sphere,sphere_tf)

3.4 碰撞检测

结果为True

# 碰撞检测
request = fcl.CollisionRequest()
result = fcl.CollisionResult()
ret = fcl.collide(box_obj, sphere_obj, request, result)
print("箱子和球是否发生了碰撞",result.is_collision)

3.5 距离检测

直接输出两者之间最小距离。

# 距离检测
request = fcl.DistanceRequest()
result = fcl.DistanceResult()
ret = fcl.distance(box_obj, sphere_obj, request, result)
print("两者之间最小距离",result.min_distance)

3.6 连续碰撞检测

request = fcl.ContinuousCollisionRequest()
result = fcl.ContinuousCollisionResult()
ret = fcl.continuousCollide(sphere_obj, sphere_tf, box_obj, box_tf, request, result)
print("是否连续碰撞",result.is_collide)

3.7 对比

修改两个TransForm的T的数值，再次运行程序，可以看到对比。

4.总结

今天正式的使用了以下python版本的fcl库，还是非常的方便的，后面打算探索以下三维点云与物体之间的碰撞如何做。

今天就到这里，希望你有所收获～