湛江企业模板建站,推荐个临汾做网站的,公司网络维护服务,wordpress 换图片地址windows下使用FCL#xff08;The Flexible-collision-library#xff09; FCL做为一款开源的碰撞检测库#xff0c;支持多种基础的几何体#xff0c;及支持C和python#xff0c;在windows和linux平台均可以使用。是一款计算高效的碰撞检测工具。在机械臂规划控制框架movei…windows下使用FCLThe Flexible-collision-library FCL做为一款开源的碰撞检测库支持多种基础的几何体及支持C和python在windows和linux平台均可以使用。是一款计算高效的碰撞检测工具。在机械臂规划控制框架moveit中做为基础的碰撞检测算法。 FCL支持的几何体类型
box 长方体sphere球ellipsoid椭球capsule胶囊体cone锥体cylinder圆柱convex凸包half-space半空间plane平面mesh面片octree 八叉树
FCL库The Flexible Collision Library主要的功能有 1、碰撞检测检测两个模型是否重叠以及可选所有重叠的三角形。 2、距离计算计算一对模型之间的最小距离即最近的一对点之间的距离。 3、公差验证确定两个模型是否比公差距离更近或更远。 4、连续碰撞检测检测两个运动模型在运动过程中是否重叠以及可选的接触时间。 5、接触信息对于碰撞检测和连续碰撞检测可以选择返回接触信息包括接触法线和接触点。
源码下载及编译
FCL 源码github 在windows环境下使用VS studio直接编译FCL存在问题需要将CMake设置成Release版本以及屏蔽掉测试程序。具体操作如下
使用VS studio打开FCO源码工程如图1所示。 图1 2. 通过改CMakeList.txt文件屏蔽测试程序如图2所示。 图2 3. 点击“项目”再点击“fcl的CMake配置”将编译设置成Release版本如图3所示。 图3 4. 点击“生成”再点击“全部重新生成”对FCL源码进行编译。
FCL碰撞测试demo 测试程序如下所示
//main.cpp
#include fcl/math/constants.h
#include fcl/narrowphase/collision.h
#include fcl/narrowphase/collision_object.h
#include fcl/narrowphase/distance.h/*** brief 两个相互碰撞的Box碰撞检测测试*/
void test1() {std::shared_ptrfcl::CollisionGeometrydouble box1(new fcl::Boxdouble(3, 3, 3));std::shared_ptrfcl::CollisionGeometrydouble box2(new fcl::Boxdouble(1, 1, 1));fcl::Transform3d tf1 fcl::Transform3d::Identity();fcl::CollisionObjectd obj1(box1, tf1);fcl::Transform3d tf2 fcl::Transform3d::Identity();fcl::CollisionObjectd obj2(box2, tf2);fcl::CollisionRequestd request;fcl::CollisionResultd result;request.gjk_solver_type fcl::GJKSolverType::GST_INDEP; // specify solver type with the default// type is GST_LIBCCDfcl::collide(obj1, obj2, request, result);std::cout test1 collide result: result.isCollision() std::endl;
}/*** brief 两个无碰撞的Box碰撞检测测试*/
void test2() {std::shared_ptrfcl::CollisionGeometrydouble box1(new fcl::Boxdouble(3, 3, 3));std::shared_ptrfcl::CollisionGeometrydouble box2(new fcl::Boxdouble(1, 1, 1));fcl::Transform3d tf1 fcl::Transform3d::Identity();fcl::CollisionObjectd obj1(box1, tf1);fcl::Transform3d tf2 fcl::Transform3d::Identity();tf2.translation() fcl::Vector3d{3, 0, 0};fcl::CollisionObjectd obj2(box2, tf2);fcl::CollisionRequestd request;fcl::CollisionResultd result;fcl::collide(obj1, obj2, request, result);std::cout test2 collide result: result.isCollision() std::endl;
}/*** brief 两个无碰撞的Box碰撞检测测试并计算最短距离*/
void test3() {std::shared_ptrfcl::CollisionGeometrydouble box1(new fcl::Boxdouble(3, 3, 3));std::shared_ptrfcl::CollisionGeometrydouble box2(new fcl::Boxdouble(1, 1, 1));fcl::Transform3d tf1 fcl::Transform3d::Identity();fcl::CollisionObjectd obj1(box1, tf1);fcl::Transform3d tf2 fcl::Transform3d::Identity();tf2.translation() fcl::Vector3d{3, 0, 0};fcl::CollisionObjectd obj2(box2, tf2);fcl::CollisionRequestd request;fcl::CollisionResultd result;// fcl::collide(obj1,obj2,request,result);std::cout test3 collide result: result.isCollision() std::endl;fcl::DistanceRequestd dist_request(true);dist_request.distance_tolerance 1e-4;fcl::DistanceResultd dist_result;fcl::distance(obj1, obj2, dist_request, dist_result);std::cout test3 collide distance: dist_result.min_distance std::endl;std::cout test3 collide point 0: dist_result.nearest_points[0] std::endl;std::cout test3 collide point 1: dist_result.nearest_points[1] std::endl;
}/*** brief 加载STL模型*/
bool loadSTLFile(const std::string filename,std::vectorfcl::Triangle triangles) {std::ifstream file(filename, std::ios::in | std::ios::binary);if (!file) {std::cerr Failed to open STL file: filename std::endl;return false;}file.seekg(0, std::ios::end); /// 定位到流末尾的位置0偏移std::streampos length file.tellg(); /// 记录当前指针位置file.seekg(0, std::ios::beg); /// 定位到流开头的位置0偏移std::vectorchar buffer(length);file.read(buffer[0], length);file.close();if (length 84) {std::cerr Invalid STL file: filename std::endl;return false;}unsigned int num_triangles *(unsigned int*)buffer[80];triangles.resize(num_triangles);unsigned int offset 84;for (unsigned int i 0; i num_triangles; i) {for (unsigned int j 0; j 3; j) {// 3顶点构成三角形float* vertex (float*)buffer[offset j * 12];triangles[i][j] (vertex[0], vertex[1], vertex[2]);}offset 50;}return true;
}/*** brief 在STL文件格式中文件头部分包含80个字节的文件头信息和4个字节的三角形数量信息因此文件总长度至少为84个字节。
因此在loadSTLFile函数中我们首先检查文件长度是否小于84个字节如果是则认为文件格式非法。
在STL文件中每个三角形由12个浮点数和2个无用字节组成因此每个三角形占用50个字节。
因此在loadSTLFile函数中我们通过一个循环遍历每个三角形并从文件中读取对应的12个浮点数最后将三角形的3个顶点存储在一个fcl::Triangle类型的变量中。
每次读取完一个三角形后需要将读取指针向前移动50个字节即offset 50。由于文件头部分占用了前84个字节因此在开始循环前需要将读取指针初始化为offset 84从而跳过文件头部分开始读取三角形信息。*/
void test4() {std::vectorfcl::Triangle triangles; /// 创建三角片序列/// 加载模型if (!loadSTLFile(C:/test0.STL, triangles)) {std::cout Error:loadSTLFile failed! std::endl;return;}/// 创建mesh,并添加三角片到mesh///std::shared_ptrfcl::BVHModelfcl::OBBRSSd mesh_geometry(new fcl::BVHModelfcl::OBBRSSd());mesh_geometry-beginModel();for (const auto triangle : triangles) {Eigen::Vector3d p1(triangle[0]), p2(triangle[1]), p3(triangle[2]);mesh_geometry-addTriangle(p1, p2, p3);}mesh_geometry-endModel();/// 建立碰撞对象-stl ,并添加CollisionGeometry,坐标位置000fcl::CollisionObjectd obj(mesh_geometry);/// 建立碰撞对象-box ,坐标位置0020std::shared_ptrfcl::Boxd box1 std::make_sharedfcl::Boxd(2.0, 2.0, 2.0);fcl::CollisionObjectd obj1(box1);obj1.setTranslation(Eigen::Vector3d(0, 0, 0));fcl::CollisionRequestd request;fcl::CollisionResultd result;/// 进行碰撞检测fcl::collide(obj, obj1, request, result);/// 输出碰撞结果if (result.isCollision()) {std::cout Collision detected! std::endl;} else {std::cout No collision detected. std::endl;}/// 距离检测fcl::DistanceRequestd requestd;fcl::DistanceResultd resultd;fcl::distance(obj, obj1, requestd, resultd);std::cout min_distance: resultd.min_distance std::endl;
}int main(int argc, char** argv) {std::cout FCL test std::endl;test1();test2();test3();test4();std::cout end test std::endl;return 0;
}CMakeList.txt文件如下所示
cmake_minimum_required(VERSION 3.14)
find_package(Eigen3 REQUIRED)
find_package(FCL REQUIRED)
add_executable(use_fcl main.cpp)
target_link_libraries(use_fcl fcl Eigen3::Eigen)
target_include_directories(use_fcl PUBLIC ${EIGEN3_INCLUDE_DIRS} ${FCL_INCLUDE_DIRS})
