FCL碰撞检测算法实现过程

碰撞检测是在特定时刻使用碰撞检测算法对两个刚体进行检测，判断当前位置是否可以运动。
FCL 开源检测库提供了物体碰撞检测以及物体接近距离的计算[51]。 FCL 能够检测
传统的三角平面和包括球体，方块，柱体等基础形状体的碰撞或者距离检测。除此之
外，还能对点云进行碰撞检测。 FCL 也可以处理可变形的模型，它不对模型做任何关
于变形的假设，并且假定在每个时间点上物体的边界被表示为三角网格。FCL 认为能够被检测的几何形状都可以表示为两个接口： 1）重叠， 检查几何形状的边界体积与对应于某个其他对象的边界体积之间的重叠。 2）相交， 检查几何对象之间的精确交集。 这主要用于在低级基元之间执行精确的邻近查询。
FCL 碰撞检测库在检测两个物体之间的碰撞的时候使用的是物体的位置、姿态、
包围盒以及表面网格。包围盒碰撞检测以及网格碰撞检测方法是碰撞检测常用的两种
思路。包围盒检测首先在复杂的物体表面添加包围体，目的是利用包围体进行快速碰
撞检测。包围体类型包括球体、轴对称包围盒（AABB）、有向包围盒（OBB），
包围球碰撞检测是用一个球体包围整个几何体，执行相交测试很简单，但是紧密
性太差。当物体在每个坐标系的分布比较均匀的时候会留下很大的空隙，预处理时间
耗费较大。包围盒检测完毕之后，即使两个盒子真的存在重叠，也不能说明两个物体发生了
碰撞，因为物体的形状是不规则的。包围盒检测只能作为粗略检测的方法，优点是该
方法速度非常快，当两个包围盒之间没有重叠时，就能肯定物体间没有碰撞。
网格碰撞检测所使用的是构成障碍物体表面的网格。 FCL 会对两个物体表面上所
有的网格进行检测，任意两个网格之间是否具有重叠。与包围盒碰撞检测相对比，这
个方法更加耗费时间。因此整个碰撞检测方案是先执行包围盒检测，如果有碰撞出现，再使用网格碰撞检测作为精细碰撞检测过程；反之如果包围盒没有碰撞出现，则
判断为没有碰撞，停止碰撞检测过程。
在系统平台下实现碰撞检测功能，可以使用 rviz 可视化环境进行测试。首先我们
在可视化环境中使用代码导入障碍物，然后对障碍物体进行编辑， 通过调整障碍物的
尺寸和位置使得机械臂与障碍物能够实现碰撞。 障碍物放置好之后， 再驱动机械臂末
端使其到达障碍物附近