大家好，今天下午写技术规划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库，还是非常的方便的，后面打算探索以下三维点云与物体之间的碰撞如何做。
今天就到这里，希望你有所收获～