开源推荐:碰撞检测算法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也会自动安装。
pyhhon-fcl-install

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的数值,再次运行程序,可以看到对比。
两者在相同Transform
Transform的X改为20后

4.总结

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

今天就到这里,希望你有所收获~

评论 2
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值