如何使用灵活的碰撞库在两个点云或点云与机器人末端执行器模型之间进行碰撞检测
How to do collision detection between two point clouds or a point cloud and a robot end effector model using flexible collision library?
我正在构建一个料框抓取演示,我需要在激光扫描仪生成的点云和机器人的末端执行器之间进行碰撞检查。
我打算用fcl(灵活的碰撞库(和pcl(点云库(来完成这项工作。 但是FCL的示例或教程非常有限。我在他们的 github 页面阅读了 fcl 源代码中的演示,并编写了一个代码示例,但我无法正确理解。以下是我编写的代码:
#include <iostream>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/ply_io.h>
#include <fcl/geometry/bvh/BVH_model.h>
#include <fcl/narrowphase/collision.h>
int main(int argc, char **argv)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr obj1_cloud(new pcl::PointCloud<pcl::PointXYZ>());
pcl::PointCloud<pcl::PointXYZ>::Ptr obj2_cloud(new pcl::PointCloud<pcl::PointXYZ>());
pcl::PLYReader ply_reader;
ply_reader.read(argv[1], *obj1_cloud);
ply_reader.read(argv[2], *obj2_cloud);
std::shared_ptr<fcl::BVHModel<fcl::AABB<double>>> model1(new fcl::BVHModel<fcl::AABB<double>>);
std::shared_ptr<fcl::BVHModel<fcl::AABB<double>>> model2(new fcl::BVHModel<fcl::AABB<double>>);
model1->beginModel();
for (int i = 0; i < obj1_cloud->points.size(); i++)
{
fcl::Vector3d point;
point(0) = obj1_cloud->points[i].x;
point(1) = obj1_cloud->points[i].y;
point(2) = obj1_cloud->points[i].z;
model1->addVertex(point);
}
model1->endModel();
model1->computeLocalAABB();
model2->beginModel();
for (int i = 0; i < obj2_cloud->points.size(); i++)
{
fcl::Vector3d point;
point(0) = obj2_cloud->points[i].x;
point(1) = obj2_cloud->points[i].y;
point(2) = obj2_cloud->points[i].z;
model2->addVertex(point);
}
model2->endModel();
model2->computeLocalAABB();
fcl::Transform3<double> pose1 = fcl::Transform3<double>::Identity();
fcl::Transform3<double> pose2 = fcl::Transform3<double>::Identity();
fcl::CollisionRequest<double> collision_request;
collision_request.gjk_solver_type = fcl::GJKSolverType::GST_INDEP;
fcl::CollisionResult<double> collision_result;
fcl::detail::MeshCollisionTraversalNode<fcl::AABB<double>> traveral_node;
if (!fcl::detail::initialize(traveral_node, *model1, pose1, *model2, pose2, collision_request, collision_result))
std::cout << "initialize error" << std::endl;
fcl::detail::collide(&traveral_node);
}
这段代码可以通过编译,但在运行时总是给我错误。
"初始化错误分段错误">
谁能帮我把事情做好?感谢您的帮助!
AFAIK 来自 fcl
的源代码,MeshCollisionTraversalNode
应该用于ModelType
BVH_MODEL_TRIANGLES
BVHModel
。在你的程序中,你只把点云的所有顶点都加到BVHModel
,所以它的ModelType
是BVH_MODEL_POINTCLOUD
的,这不符合MeshCollisionTraversalNode
的要求。
我认为您有两种方法可以解决问题:
一种是在点云上运行三角测量(参考:http://pointclouds.org/documentation/tutorials/greedy_projection.php(并构造BVH_MODEL_TRIANGLES
的BVHModel
。如果从pcl
生成的网格模型是凹的,则可能还需要在碰撞检测之前使用凸分解。
另一种方法是使用八叉树作为点云的表示。您可以参考答案以获取更多详细信息。
相关文章:
- 在执行其他功能的同时播放动画(LED矩阵和Arduino/ESP8266)
- C++,系统无法执行指定的程序
- 使用C++中的模板和运算符重载执行矩阵运算
- 创建一个函数以在输入为负数或零时输出字符串.第一次执行用户定义的函数
- 执行函数时导致崩溃的变量
- 无论条件是否为true,if总是在c++中执行
- 当函数模板参数是具有默认参数的类模板时,函数模板参数的推导如何执行
- 在C++中对T*类型执行std::move的意外行为
- 使用QProcess执行命令,并将结果存储在QStringList中
- 如何在没有信号的情况下从C++执行QML插槽
- 如何确认我的constexpr表达式实际上已经在编译时执行
- C++17中的并行执行策略
- QML按钮点击功能执行顺序
- 程序在执行程序的其余部分之前退出
- 为什么catch中的代码没有被执行
- C++从其他 constexpr 创建 lambda 不能按顺序执行 Constexpr
- 将执行、作业和WinAPI相乘
- 对字符数组中的元素执行逐位操作
- 为什么g++在未执行的代码处标记强制转换错误
- 如何使用灵活的碰撞库在两个点云或点云与机器人末端执行器模型之间进行碰撞检测