Use FCL (The Flexible-collision-library) under windows
? ?FCL, as an open source collision detection library, supports a variety of basic geometries, supports C++ and python, and can be used on both windows and linux platforms. It is a computationally efficient collision detection tool. It is the basic collision detection algorithm in the robot arm planning control framework moveit.
Geometry types supported by FCL:
- box (cuboid)
- sphere
- ellipsoid
- capsule
- cone
- cylinder
- convex (convex hull)
- half-space
- plane
- mesh
- octree (octree)
The main functions of the FCL library (The Flexible Collision Library) are:
1. Collision detection: Detect whether two models overlap, and (optional) all overlapping triangles.
2. Distance calculation: Calculate the minimum distance between a pair of models, that is, the distance between the nearest pair of points.
3. Tolerance verification: Determine whether two models are closer or further apart than the tolerance.
4. Continuous collision detection: detect whether two motion models overlap during motion, and optional contact time.
5. Contact information: For collision detection and continuous collision detection, you can choose to return contact information (including contact normals and contact points).
Source code download and compilation
FCL source code github
In the Windows environment, there are problems when using VS studio to directly compile FCL. You need to set CMake to the Release version and block the test program. The specific operations are as follows:
- Use VS studio to open the FCO source code project, as shown in Figure 1.
figure 1
figure 2
image 3
FCL crash test demo
The test program is as follows:
//main.cpp
#include "fcl/math/constants.h"
#include "fcl/narrowphase/collision.h"
#include "fcl/narrowphase/collision_object.h"
#include "fcl/narrowphase/distance.h"
/**
* @brief Two Box collision detection tests
*/
void test1() {<!-- -->
std::shared_ptr<fcl::CollisionGeometry<double>> box1(
new fcl::Box<double>(3, 3, 3));
std::shared_ptr<fcl::CollisionGeometry<double>> box2(
new fcl::Box<double>(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_LIBCCD
fcl::collide( & amp;obj1, & amp;obj2, request, result);
std::cout << "test1 collide result:" << result.isCollision() << std::endl;
}
/**
* @brief Two collision-free Box collision detection tests
*/
void test2() {<!-- -->
std::shared_ptr<fcl::CollisionGeometry<double>> box1(
new fcl::Box<double>(3, 3, 3));
std::shared_ptr<fcl::CollisionGeometry<double>> box2(
new fcl::Box<double>(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( & amp;obj1, & amp;obj2, request, result);
std::cout << "test2 collide result:" << result.isCollision() << std::endl;
}
/**
* @brief Two collision-free Box collision detection tests and calculate the shortest distance
*/
void test3() {<!-- -->
std::shared_ptr<fcl::CollisionGeometry<double>> box1(
new fcl::Box<double>(3, 3, 3));
std::shared_ptr<fcl::CollisionGeometry<double>> box2(
new fcl::Box<double>(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( & amp;obj1, & amp;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( & amp;obj1, & amp;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 Load STL model
*/
bool loadSTLFile(const std::string & amp; filename,
std::vector<fcl::Triangle> & amp; 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); /// Position to the end of the stream, offset 0
std::streampos length = file.tellg(); /// Record the current pointer position
file.seekg(0, std::ios::beg); /// Locate the beginning of the stream, offset 0
std::vector<char> buffer(length);
file.read( & amp;buffer[0], length);
file.close();
if (length < 84) {<!-- -->
std::cerr << "Invalid STL file: " << filename << std::endl;
return false;
}
unsigned int num_triangles = *(unsigned int*) & amp;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 vertices form a triangle
float* vertex = (float*) & amp;buffer[offset + j * 12];
triangles[i][j] = (vertex[0], vertex[1], vertex[2]);
}
offset + = 50;
}
return true;
}
/**
* @brief In the STL file format, the file header part contains 80 bytes of file header information and 4 bytes of triangle number information, so the total file length is at least 84 bytes.
Therefore, in the loadSTLFile function we first check whether the file length is less than 84 bytes, and if so, the file format is considered illegal.
In the STL file, each triangle consists of 12 floats and 2 useless bytes, so each triangle takes up 50 bytes.
Therefore, in the loadSTLFile function, we iterate through each triangle through a loop and read the corresponding 12 floating point numbers from the file, and finally store the 3 vertices of the triangle in a variable of type fcl::Triangle.
After each triangle is read, the reading pointer needs to be moved forward by 50 bytes, that is, offset + = 50. Since the file header part occupies the first 84 bytes, the read pointer needs to be initialized to offset=84 before starting the loop, thereby skipping the file header part and starting to read the triangle information.
*/
void test4() {<!-- -->
std::vector<fcl::Triangle> triangles; /// Create a triangle sequence
/// Load model
if (!loadSTLFile("C:/test0.STL", triangles)) {<!-- -->
std::cout << "Error:loadSTLFile failed!" << std::endl;
return;
}
/// Create mesh and add triangles to mesh
///
std::shared_ptr<fcl::BVHModel<fcl::OBBRSSd>> mesh_geometry(
new fcl::BVHModel<fcl::OBBRSSd>());
mesh_geometry->beginModel();
for (const auto & amp; triangle : triangles) {<!-- -->
Eigen::Vector3d p1(triangle[0]), p2(triangle[1]), p3(triangle[2]);
mesh_geometry->addTriangle(p1, p2, p3);
}
mesh_geometry->endModel();
/// Create a collision object-stl, and add CollisionGeometry, coordinate position (0, 0, 0)
fcl::CollisionObjectd obj(mesh_geometry);
/// Create a collision object-box, coordinate position (0, 0, 20)
std::shared_ptr<fcl::Boxd> box1 = std::make_shared<fcl::Boxd>(2.0, 2.0, 2.0);
fcl::CollisionObjectd obj1(box1);
obj1.setTranslation(Eigen::Vector3d(0, 0, 0));
fcl::CollisionRequestd request;
fcl::CollisionResultd result;
/// Perform collision detection
fcl::collide( & amp;obj, & amp;obj1, request, result);
/// Output collision results
if (result.isCollision()) {<!-- -->
std::cout << "Collision detected!" << std::endl;
} else {<!-- -->
std::cout << "No collision detected." << std::endl;
}
/// Distance detection
fcl::DistanceRequestd requested;
fcl::DistanceResultd resultd;
fcl::distance( & amp;obj, & amp;obj1, requested, 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;
}
The CMakeList.txt file looks like this:
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})