利用FCL实现更加精准的碰撞检测

news2024/11/23 2:54:23

一,需求

利用OSG结合FCL实现实现精准的碰撞检测。

二,效果  看这里

利用FCL实现更加精准的碰撞检测 – Qt hello

三,分析

我们看如下这张图,碰撞的逻辑就是,在一个三维场景中,构造一个实体,比如下边的BoxA,然后在物理引擎比如bullet中,或者专用的碰撞检测库中也构造一个对应的实体,比如BoxB。之后在BoxA位姿改变时后,将BoB的位姿也做相应的更新。之后发生碰撞时,物理引擎或者FCL就会给出信号。而这个场景,可以是VTK或者OSG。而碰撞检测可以用Bullet也可以用FCL。

之前用bullet做个尝试,基本的图形能满足需求,比如球,盒子,但是项目中涉及到点云的碰撞,而bullet中处理点云,没有找到好的处理方式。但是FCL可以将点云转变成fcl中对应的实体,因此最终选择了FCL进行碰撞检测,这里列出FCL中大概的步骤。

1,FCL中构造实体。这里构造了一个 盒子。

auto box_geometry = std::make_shared<fcl::Boxf>(w, d, h);
auto ob = new fcl::CollisionObjectf(box_geometry);

2,更新FCL实体的位姿矩阵。

void FCLManager::updateTrans(const std::string &name, const fcl::Transform3f &trans)
{
    fcl::CollisionObjectf *ob=getCollisionObject(name);
    if(ob){
        ob->setTransform(trans);
    }
}

//OSG 矩阵 需要进行转换 才能给到FCL使用
//osg 矩阵转fcl矩阵
osg::Vec3 osgTrans = mt.getTrans(); // 获取平移分量
osg::Quat osgQuat = mt.getRotate(); // 获取旋转分量
 
fcl::Quaternionf rotation(osgQuat.w(), osgQuat.x(), osgQuat.y(), osgQuat.z());
fcl::Vector3f translation(osgTrans.x(), osgTrans.y(), osgTrans.z());
fcl::Transform3f fclTrans=fcl::Transform3f::Identity();
fclTrans.translation() = translation;
fclTrans.linear()=rotation.toRotationMatrix();
FCLManager::getInstance()->updateTrans(this->getName(),fclTrans);

3,碰撞检测

我是检测机器人和其它障碍物的碰撞,这里把机器人关节放到一个集合中,把其它障碍物放到另一个集合中

 
bool FCLManager::detectCollision()
{
    fcl::CollisionRequestf request;
    fcl::CollisionResultf result;
    for(auto &ob1:jointMap){
        for(auto &ob2:obstacleMap){
            collide(ob1.second, ob2.second, request, result);
            if(result.isCollision()){
                return true;
            }
        }
    }
    return false;
}

4,FCL支持三角面检测。因此我们在FCL中构造对应实体的时候,可以直接用三角面。这样不管OSG中构造的时盒子还是球,还是导入的stl,对应FCL中都是统一用三角面处理。

void FCLManager::addTriMesh(const std::string &name, osg::Node *node)
{
     fcl::CollisionObjectf *obj = createNodeCollisionObject(node);
     obstacleMap.emplace(name,obj);
}
 
fcl::CollisionObjectf *FCLManager::createNodeCollisionObject(osg::Node *node)
{
    MyComputeTriMeshVisitor visitor;
    node->accept( visitor );
    osg::Vec3Array* vertices = visitor.getTriMesh();
 
    typedef fcl::BVHModel<fcl::OBBRSSf> Model;
    Model* model = new Model();
    std::shared_ptr<fcl::CollisionGeometryf> m1_ptr(model);
    model->beginModel();
    osg::Vec3 p1, p2, p3;
    for( size_t i = 0; i + 2 < vertices->size(); i += 3 )
    {
        p1 = vertices->at( i );
        p2 = vertices->at( i + 1 );
        p3 = vertices->at( i + 2 );
 
        fcl::Vector3<float> pp1{p1.x(),p1.y(),p1.z()};
        fcl::Vector3<float> pp2{p2.x(),p2.y(),p2.z()};
        fcl::Vector3<float> pp3{p3.x(),p3.y(),p3.z()};
 
        model->addTriangle(pp1, pp2, pp3);
    }
    model->endModel();
    model->computeLocalAABB();
 
    return new  fcl::CollisionObjectf(m1_ptr);
}

5,点云的碰撞。点云的碰撞 使用了一种叫做八叉树的算法。首先将点云转成pcl的点云 格式,然后可以直接构造出fcl实体,这也是选用FCL的原因。

fcl::CollisionObjectf* FCLManager::createPointCloudCollisionObject(const pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud_ptr, const octomap::point3d &origin_3d)
{
    // octomap octree settings
    const double resolution = 0.01;
    const double prob_hit = 0.9;
    const double prob_miss = 0.1;
    const double clamping_thres_min = 0.12;
    const double clamping_thres_max = 0.98;
 
    std::shared_ptr<octomap::OcTree> octomap_octree = std::make_shared<octomap::OcTree>(resolution);
    octomap_octree->setProbHit(prob_hit);
    octomap_octree->setProbMiss(prob_miss);
    octomap_octree->setClampingThresMin(clamping_thres_min);
    octomap_octree->setClampingThresMax(clamping_thres_max);
 
    octomap::KeySet free_cells;
    octomap::KeySet occupied_cells;
 
#if defined(_OPENMP)
#pragma omp parallel
#endif
    {
#if defined(_OPENMP)
        auto thread_id = omp_get_thread_num();
        auto thread_num = omp_get_num_threads();
#else
        int thread_id = 0;
        int thread_num = 1;
#endif
        int start_idx = static_cast<int>(pointcloud_ptr->size() / thread_num) * thread_id;
        int end_idx = static_cast<int>(pointcloud_ptr->size() / thread_num) * (thread_id + 1);
        if (thread_id == thread_num - 1)
        {
            end_idx = pointcloud_ptr->size();
        }
 
        octomap::KeySet local_free_cells;
        octomap::KeySet local_occupied_cells;
 
        for (auto i = start_idx; i < end_idx; i++)
        {
            octomap::point3d point((*pointcloud_ptr)[i].x, (*pointcloud_ptr)[i].y, (*pointcloud_ptr)[i].z);
            octomap::KeyRay key_ray;
            if (octomap_octree->computeRayKeys(origin_3d, point, key_ray))
            {
                local_free_cells.insert(key_ray.begin(), key_ray.end());
            }
 
            octomap::OcTreeKey tree_key;
            if (octomap_octree->coordToKeyChecked(point, tree_key))
            {
                local_occupied_cells.insert(tree_key);
            }
        }
 
#if defined(_OPENMP)
#pragma omp critical
#endif
        {
            free_cells.insert(local_free_cells.begin(), local_free_cells.end());
            occupied_cells.insert(local_occupied_cells.begin(), local_occupied_cells.end());
        }
    }
 
    // free cells only if not occupied in this cloud
    for (auto it = free_cells.begin(); it != free_cells.end(); ++it)
    {
        if (occupied_cells.find(*it) == occupied_cells.end())
        {
            octomap_octree->updateNode(*it, false);
        }
    }
 
    // occupied cells
    for (auto it = occupied_cells.begin(); it != occupied_cells.end(); ++it)
    {
        octomap_octree->updateNode(*it, true);
    }
 
    auto fcl_octree = std::make_shared<fcl::OcTree<float>>(octomap_octree);
    std::shared_ptr<fcl::CollisionGeometryf> fcl_geometry = fcl_octree;
    return new fcl::CollisionObjectf(fcl_geometry);
}

四,总结  看这里

利用FCL实现更加精准的碰撞检测 – Qt hello

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.coloradmin.cn/o/1622318.html

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈,一经查实,立即删除!

相关文章

机器学习笔记(二)回归

一、线性回归 线性回归是一种用于预测的统计方法&#xff0c;特别适用于连续值预测。&#x1f4c8;线性回归通过最小化误差的平方和来寻找一个线性关系&#xff0c;用于预测一个变量&#xff08;因变量&#xff09;基于一个或多个其他变量&#xff08;自变量&#xff09;的值。…

远程控制安卓手机:便捷、高效与安全的方法

在移动设备的领域里&#xff0c;远程控制安卓手机的能力也变得越来越重要。这种技术可以让我们在远程地点方便地操作手机&#xff0c;无论是处理紧急事务、帮助他人解决问题&#xff0c;还是仅仅为了享受科技带来的便利。本文将为你介绍2种便捷、高效且安全的方法&#xff0c;让…

笔试狂刷--Day6(岛屿数量+模拟)

大家好,我是LvZi,今天带来笔试狂刷--Day6 一.在字符串中找出连续最⻓的数字串 1.题目链接 在字符串中找出连续最⻓的数字串 2.题目分析 使用双指针模拟 3.代码实现 import java.util.Scanner; // 注意类名必须为 Main, 不要有任何 package xxx 信息 public class Main {p…

Linux的学习之路:20、进程信号(2)

摘要 本章讲一下进程信号的阻塞信号和捕捉信号和可重入函数 目录 摘要 一、阻塞信号 1、阻塞信号 2、信号集操作函数 二、捕捉信号 1、内核如何实现信号的捕捉 2、代码实演 三、可重入函数 一、阻塞信号 1、阻塞信号 实际执行信号的处理动作称为信号递达(Delivery) …

文末送资料 | AI大模型接入指南:免费畅聊公众号新时代!附搭建教程

目录 今天内容有点意思&#xff01; 福利&#xff1a;拉到最后&#xff0c;免费送资料&#xff0c;你想要的全都有 我把公号接入了&#xff0c;字节跳动的云雀AI大模型&#xff01; 先给大家看几个案例 重点来了 如何将公号接入AI大模型呢&#xff1f; 1、创建AI聊天机器…

海南封关怎么看?win战略会任志雄解析

今年海南自由贸易港建设也进入了新阶段:将在2025年年底前适时启动全岛封关运作,封关后的海南将以全新姿态迎接更广泛的发展机遇。 封关在即,企业有何感受?还有哪些准备工作?封关后的海南将呈现怎样的状态?近日,红星资本局记者深入实地了解海南自贸港如何成型起势。 利好当…

快手不发作品ip地址会变吗

在数字时代&#xff0c;我们每个人的在线行为都留下了独特的痕迹。这些痕迹不仅仅是我们的言论或行为&#xff0c;还包括我们的IP地址——一个在网络世界中标识我们位置的数字标签。近年来&#xff0c;随着短视频平台的兴起&#xff0c;如快手这样的应用已经深入人们的日常生活…

sentinel-1.8.7与nacos-2.3.0实现动态规则配置、双向同步

&#x1f60a; 作者&#xff1a; 一恍过去 &#x1f496; 主页&#xff1a; https://blog.csdn.net/zhuocailing3390 &#x1f38a; 社区&#xff1a; Java技术栈交流 &#x1f389; 主题&#xff1a; sentinel-1.8.7与nacos-2.3.0实现动态规则配置、双向同步 ⏱️ 创作时…

2024年大数据应用、智能控制与软件工程国际会议(BDAICSE2024)

2024年大数据应用、智能控制与软件工程国际会议(BDAICSE2024) 会议简介 我们诚挚邀请您参加2024年大数据应用、智能控制和软件工程国际会议&#xff08;BDAICSE2024&#xff09;。这次会议将在美丽的长沙市举行。 本次大会旨在汇聚全球大数据应用、智能控制、软件工程等领…

常见大厂面试题(SQL)02

小鹏面试题: 小鹏汽车充电每辆车连续快充最大次数 原表charging_data idcharge_timecharge_typeXP10012023/11/20 8:45快充XP10012023/11/21 20:45快充XP10012023/11/22 8:45快充XP10012023/11/23 8:45慢充XP10012023/11/25 8:45快充XP10022023/11/25 8:45快充XP10022023/11/…

kubernetes中Pod资源的使用限制

一、概述 当kubernetes调度创建Pod后&#xff0c;Pod是否有足够的资源来运行容器&#xff0c;是非常重要的。资源分为两种类型——容器请求的资源和容器被限制的资源。 请求和限制是kubernetes控制集群cpu和内存等资源的重要方式&#xff0c;他们是两种不同的机制 容器请求的资…

C语言之回调函数+可变参数__VA_ARGS__:用法实例(四十八)

简介&#xff1a; CSDN博客专家&#xff0c;专注Android/Linux系统&#xff0c;分享多mic语音方案、音视频、编解码等技术&#xff0c;与大家一起成长&#xff01; 优质专栏&#xff1a;Audio工程师进阶系列【原创干货持续更新中……】&#x1f680; 优质专栏&#xff1a;多媒…

GaussDB数据库SQL系列-聚合函数

背景 在这篇文章中&#xff0c;我们将深入探讨GaussDB数据库中聚合函数的使用和优化。聚合函数是数据库查询中非常重要的工具&#xff0c;它们可以对一组值执行计算并返回单个值。例如&#xff0c;聚合函数可以用来计算平均值、总和、最大值和最小值。 这些功能在数据分析和报…

C语言 | Leetcode C语言题解之第48题旋转图像

题目&#xff1a; 题解&#xff1a; void swap(int* a, int* b) {int t *a;*a *b, *b t; }void rotate(int** matrix, int matrixSize, int* matrixColSize) {// 水平翻转for (int i 0; i < matrixSize / 2; i) {for (int j 0; j < matrixSize; j) {swap(&matr…

Linux_网络基础2_3

文章目录 一、应用层协议1.我写的socket是在干什么&#xff1f;2.再谈"协议"1.问题1 # 2.问题23.造轮子——定制应用协议4.使用json来完成应用协议1.安装库2.使用 —— 序列化3.使用 —— 反序列化 二、HTTP协议1.认识URL2.http协议的请求格式和响应格式3.编写响应 -…

大模型(e.g., ChatGPT)里面的一些技术和发展方向

文章目录 如何炼成ChatGPT如何调教ChatGPT如何武装ChatGPT一些大模型的其他方向prompt tuningInstruction tuning 这个是基于视频 https://www.bilibili.com/video/BV17t4218761&#xff0c;可以了解一下大模型里面的一些技术和最近的发展&#xff0c;基本都是2022你那以来的发…

Spring Boot整合Redisson的两种方式

项目场景 Spring Boot整合Redisson的两种方式&#xff0c;方式一直接使用yml配置&#xff0c;方式二创建RedissonConfig配置类。 前言 redisson和redis区别&#xff1a; Redis是一个开源的内存数据库&#xff0c;支持多种数据类型&#xff0c;如字符串、哈希、列表、集合和有序…

tailwindcss在使用cdn引入静态html的时候,vscode默认不会提示问题

1.首先确保vscode下载tailwind插件&#xff1a;Tailwind CSS IntelliSense 2.需要在根目录文件夹创建一个tailwind.config.js文件 export default {theme: {extend: {// 可根据需要自行配置&#xff0c;空配置项可以正常使用},}, }3.在html文件的标签中引入配置文件&#xf…

基础环境:wsl2安装Ubuntu22.04 + miniconda

服务器相关信息&#xff1a; Thinkpad p1 gen5 64G 2T 3080ti&#xff0c;自带的有nvidia-smi显卡驱动。使用wsl2安装Ubuntu22.04 miniconda目标&#xff1a;安装gpu版本的PyTorch2.1.2&#xff08;torch2.1.2/cu117 torchvision0.16.2/cu117&#xff09; 处理器 12th Gen I…

ADS8866 ADC转换芯片驱动调试

目录 ADS8866 ADC转换芯片驱动调试 开发环境&#xff1a; ADS8866功能简介 硬件连接 SPI3的软件驱动 ADS8866通讯时序分析 ADS8866驱动 ADS8866 ADC转换芯片驱动调试 开发环境&#xff1a; 主控芯片&#xff1a;STM32F103RCT6、ADS8866转换芯片、使用HAL库进行开发。 …