Python-FCL 学习

Python Interface for the Flexible Collision Library

Python-FCL是（非官方的）Python接口，用于对成对的几何模型执行接近和碰撞查询。

支持三种对几何模型对的查询：

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

支持大多数FCL的对象形状，包括：
TriangleP、Box、Sphere、Ellipsoid、Capsule、 Cone、 Cylinder、Half-Space、 Plane、 Mesh、OcTree

一、安装

1，安装octomap，这是使用OcTree所必需的.

sudo apt-get install liboctomap-dev

2， Ubuntu 17.04 or newer,

sudo apt-get install libfcl-dev.
3， pip install python-fcl

二、对象Object

碰撞对象

FCL中的主要结构是CollisionObject，构成所有碰撞和距离计算的基础。
CollisionObject由两个组件组成：

1，几何形状，由CollisionGeometry 对象定义，
2，位姿，由Transform对象定义。

碰撞几何CollisionGeometry

CollisionGeometry对象主要有两种类型
1.几何图元，例如盒子和球体
2.任意三角形网格
下面说明如何实例化几何图元的示例。
（box盒, sphere球, ellipsoid椭球, capsule胶囊体, cone圆锥, and cylinder 圆柱都在原点处）

import numpy as np
import fcl

v1 = np.array([1.0, 2.0, 3.0])
v2 = np.array([2.0, 1.0, 3.0])
v3 = np.array([3.0, 2.0, 1.0])
x, y, z = 1, 2, 3
rad, lz = 1.0, 3.0
n = np.array([1.0, 0.0, 0.0])
d = 5.0

t = fcl.TriangleP(v1, v2, v3) # 三点v1, v2, v3 定义一个Triangle
b = fcl.Box(x, y, z)          # 三边长度x, y, z 定义与坐标轴轴对齐的box
s = fcl.Sphere(rad)           # 半径rad 定义Sphere
e = fcl.Ellipsoid(x, y, z)    # 三个半径x, y, z 定义与坐标轴对齐的Ellipsoid
c = fcl.Capsule(rad, lz)      # 半径rad和与z轴一致的高度lz 定义一个capsule
c = fcl.Cone(rad, lz)         # 半径rad和与z轴一致的高度lz 定义一个Cone
c = fcl.Cylinder(rad, lz)     # 半径rad和与z轴一致的高度lz 定义一个Cylinder
h = fcl.Halfspace(n, d)       # {x : <n, x> < d} 定义Halfspace
p = fcl.Plane(n, d)           # {x : <n, x> = d} 定义Plane

Triangular meshes由BVHModel类包装，并且实例化有些不同。

verts = np.array([[1.0, 1.0, 1.0],
                  [2.0, 1.0, 1.0],
                  [1.0, 2.0, 1.0],
                  [1.0, 1.0, 2.0]])
tris  = np.array([[0,2,1],
                  [0,3,2],
                  [0,1,3],
                  [1,2,3]])

m = fcl.BVHModel()
m.beginModel(len(verts), len(tris))
m.addSubModel(verts, tris)
m.endModel()

变换Transform

Transform告诉FCLCollisionGeometry在世界上的位置。这里的变换是刚性变换。
平移是3元素一维向量，而旋转可以由3x3旋转矩阵或四元数指定。

R = np.array([[0.0, -1.0, 0.0],
              [1.0,  0.0, 0.0],
              [0.0,  0.0, 1.0]])
T = np.array([1.0, 2.0, 3.0])
q = np.array([0.707, 0.0, 0.0, 0.707])

tf = fcl.Transform()     # 默认单位阵
tf = fcl.Transform(q)    # 四元数旋转，无平移
tf = fcl.Transform(R)    # 旋转矩阵，无平移
tf = fcl.Transform(T)    # 平移无旋转
tf = fcl.Transform(q, T) # 四元数旋转与平移
tf = fcl.Transform(R, T) # 旋转矩阵与无平移
tf1 = fcl.Transform(tf)  # 也可以用另一个Transform初始化

一个CollisionGeometry 和一个Transform，我们可以创建一个 CollisionObject:

t = fcl.Transform(R, T)
b = fcl.Box(x, y, z)
obj = fcl.CollisionObject(b, t)

CollisionObject的Transform可以替代性修改：不是连续叠加的变换！

t1 = fcl.Transform(R1, T1)
obj.setTransform(t1)   
obj.setRotation(R2)
obj.setTranslation(T2)
obj.setQuatRotation(q2)

三、执行操作

成对操作

给定一对碰撞对象，该库支持三种查询类型：

• 碰撞检测
• 距离计算
• 连续碰撞检测

遵循一个pipeline
1，初始化一个查询请求数据结构，加入参数。
2，初始化一个空的查询响应结构。
3，使用两个CollisionObject，请求结构和响应结构作为参数调用查询函数。
可以返回一个标量结果1/0，其他信息则存储在查询结果数据结构中

碰撞检查

g1 = fcl.Box(1,2,3)
t1 = fcl.Transform()
o1 = fcl.CollisionObject(g1, t1)

g2 = fcl.Cone(1,3)
t2 = fcl.Transform()
o2 = fcl.CollisionObject(g2, t2)

request = fcl.CollisionRequest()
result = fcl.CollisionResult()

ret = fcl.collide(o1, o2, request, result)

调用fcl.collide()之后,

ret 包含两个对象之间生成的联系数量

result 包含有关碰撞和接触的信息。

有关碰撞request 和result 的可用参数的更多信息，参见fcl/collision_data.py。

距离检查

g1 = fcl.Box(1,2,3)
t1 = fcl.Transform()
o1 = fcl.CollisionObject(g1, t1)

g2 = fcl.Cone(1,3)
t2 = fcl.Transform()
o2 = fcl.CollisionObject(g2, t2)

request = fcl.DistanceRequest()
result = fcl.DistanceResult()

ret = fcl.distance(o1, o2, request, result)

调用fcl.distance()之后, ret 包含两个对象之间的最小距离,result包含有关对象上最接近点的信息。
如果ret为负，则对象发生碰撞。
有关碰撞request 和result 的可用参数的更多信息，参见fcl/collision_data.py。

连续碰撞检查

g1 = fcl.Box(1,2,3)
t1 = fcl.Transform()
o1 = fcl.CollisionObject(g1, t1)
t1_final = fcl.Transform(np.array([1.0, 0.0, 0.0]))

g2 = fcl.Cone(1,3)
t2 = fcl.Transform()
o2 = fcl.CollisionObject(g2, t2)
t2_final = fcl.Transform(np.array([-1.0, 0.0, 0.0]))

request = fcl.ContinuousCollisionRequest()
result = fcl.ContinuousCollisionResult()

ret = fcl.continuousCollide(o1, t1_final, o2, t2_final, request, result)

调用fcl.continuousCollide()之后，ret将接触时间包含在（0,1）中，如果对象在从其初始姿势移动到最终姿势期间未发生碰撞，则将其包含在接触时间中，即1.0。
result 包含有关碰撞时间和状态的信息。
有关连续碰撞请求和结果的可用参数的更多信息，
参见fcl / collision_data.py。

Broadphase检测

FCL还支持对象组之间的Broadphase 碰撞/距离查询，并且可以避免成对计算带来的n^2的复杂度。

具体地，在执行 碰撞/距离检查 之前，将 CollisionObject项注册到DynamicAABBTreeCollisionManager中。

提供三种类型的检查：

• 一对多: 单独的 CollisionObject与Manager中所有对象之间的碰撞/距离检查。
• Manager内的多对多: Manager中所有对象对之间的成对的 碰撞/距离检查。
• Manager间的多对多: 来自两个Manager之间的成对碰撞/距离检查。

通常，碰撞检查可以返回所有接触对，而距离检查仅返回任何一对对象之间的最接近距离。

下列碰撞检查的一些示例：

这些方法具有

1回调函数，除非有特殊用例，否则请使用python-fcl中的默认值。

2封装请求/响应对的包装对象（CollisionData或DistanceData）。 该对象还有一个字段“ done”，它告诉递归冲突检查器何时退出。在重用请求之前，请务必为每个请求使用一个新的Data对象或将done属性设置为False。

objs1 = [fcl.CollisionObject(box), fcl.CollisionObject(sphere)]
objs2 = [fcl.CollisionObject(cone), fcl.CollisionObject(mesh)]

manager1 = fcl.DynamicAABBTreeCollisionManager()
manager2 = fcl.DynamicAABBTreeCollisionManager()

manager1.registerObjects(objs1)  # 将CollisionObject注册到DynamicAABBTreeCollisionManager
manager2.registerObjects(objs2)

manager1.setup()
manager2.setup()

#=====================================================================
# Managed internal (sub-n^2) collision checking
#=====================================================================
cdata = fcl.CollisionData()
manager1.collide(cdata, fcl.defaultCollisionCallback)
print 'Collision within manager 1?: {}'.format(cdata.result.is_collision)

##=====================================================================
## Managed internal (sub-n^2) distance checking
##=====================================================================
ddata = fcl.DistanceData()
manager1.distance(ddata, fcl.defaultDistanceCallback)
print ('Closest distance within manager 1?: {}'.format(ddata.result.min_distance))

#=====================================================================
# Managed one to many collision checking
#=====================================================================
req = fcl.CollisionRequest(num_max_contacts=100, enable_contact=True)
rdata = fcl.CollisionData(request = req)

manager1.collide(fcl.CollisionObject(mesh), rdata, fcl.defaultCollisionCallback)
print ('Collision between manager 1 and Mesh?: {}'.format(rdata.result.is_collision))
print ('Contacts:')
for c in rdata.result.contacts:
    print ('\tO1: {}, O2: {}'.format(c.o1, c.o2))

#=====================================================================
# Managed many to many collision checking
#=====================================================================
rdata = fcl.CollisionData(request = req)
manager1.collide(manager2, rdata, fcl.defaultCollisionCallback)
print ('Collision between manager 1 and manager 2?: {}'.format(rdata.result.is_collision))
print ('Contacts:')
for c in rdata.result.contacts:
    print ('\tO1: {}, O2: {}'.format(c.o1, c.o2))

提取哪些对象有碰撞

确定实际发生碰撞的对象，就要解析碰撞数据的contacts并使用其他外部数据结构。具体来说，传递给任何 collide()调用的fcl.CollisionData对象具有一组contacts，存储在cdata.result.contacts中。该对象是Contact 对象的简单列表，每个表示两个对象之间的接触点。每个contact 对象都有两个属性o1和o2，它们存储对原始fcl.CollisionGeometry对象的引用，该引用是为两个碰撞的fcl.CollisionObject对象创建的。

因此，做法就是从每个 fcl.CollisionGeometry 对象的id映射到其对应的实际fcl.CollisionObject或每个对象的某些字符串标识符。然后，遍历cdata.result.contacts，提取o1和o2，对每个函数应用内置的id()函数，然后在对应关系中找到所需的数据。

看示例就会懂了：

import fcl
import numpy as np

# 创建碰撞几何及其对象
geom1 = fcl.Cylinder(1.0, 1.0)
obj1 = fcl.CollisionObject(geom1)

geom2 = fcl.Cylinder(1.0, 1.0)
obj2 = fcl.CollisionObject(geom2, fcl.Transform(np.array([0.0, 0.0, 0.3])))

geom3 = fcl.Cylinder(1.0, 1.0)
obj3 = fcl.CollisionObject(geom3, fcl.Transform(np.array([0.0, 0.0, 3.0])))

geoms = [geom1, geom2, geom3]
objs = [obj1, obj2, obj3]
names = ['obj1', 'obj2', 'obj3']

# 创建几何ID和对象的对应关系 字典
geom_id_to_obj = {id(geom) : obj for geom, obj in zip(geoms, objs) }

# 创建几何ID和对象的字符串名字的对应关系 字典
geom_id_to_name = { id(geom) : name for geom, name in zip(geoms, names) }

# 创建manager 
manager = fcl.DynamicAABBTreeCollisionManager()  # 实例化
manager.registerObjects(objs)  # 注册
manager.setup()

# 创建碰撞请求结构
crequest = fcl.CollisionRequest(num_max_contacts=100, enable_contact=True)
cdata = fcl.CollisionData(crequest, fcl.CollisionResult())

# 运行碰撞请求
manager.collide(cdata, fcl.defaultCollisionCallback)

# 从contacts中提取碰撞数据，并使用该数据推断发生碰撞的对象set
objs_in_collision = set()

for contact in cdata.result.contacts:
    # 提取contacts中的碰撞几何
    coll_geom_0 = contact.o1  # 碰撞是成对出现的所有是o1,o2
    coll_geom_1 = contact.o2

    # 获取字符串名称
    coll_names = [geom_id_to_name[id(coll_geom_0)], geom_id_to_name[id(coll_geom_1)]]
    coll_names = tuple(sorted(coll_names))
    objs_in_collision.add(coll_names)

for coll_pair in objs_in_collision:
    print('Object {} in collision with object {}!'.format(coll_pair[0], coll_pair[1]))

>>> Object obj1 in collision with object obj2!

参考