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!