机器人C++库(10)Robotics Library 之碰撞检测算法

news/2024/3/29 23:06:32/文章来源:https://blog.csdn.net/yohnyang/article/details/127248206

机器人C++库(10)Robotics Library 之碰撞检测算法

1.碰撞检测开源库介绍

RL库中集成了以下开源含碰撞检测功能的库:

  • 1.bullet3:https://pybullet.org/wordpress/
    在这里插入图片描述
  • 2.FCL:https://github.com/flexible-collision-library/fcl
    在这里插入图片描述
  • 3.ODE:http://www.ode.org/
    在这里插入图片描述
  • 4.PQP:http://gamma.cs.unc.edu/SSV/
    在这里插入图片描述
  • 5.SOLID:http://www.dtecta.com/

2.RL碰撞检测算法使用

RL的碰撞检测算法的话是先建立机器人和工作场景的wrl模型,然后根据运动学赋予场景中的对应模型以姿态,实时判断是否有组件发生碰撞,其算法的本质实现 是依赖于开源的bullet、FCL等库,然后rl库中添加了衔接文件是的上述方法均可使用。

  • 关于场景、机器人.wrl模型建立可以参考我的另一篇文档:Robotics Library 之机器人建模(VRML)、工作场景Scances建模(VRML)
  • 下边分别介绍RL库通过.wrl进行碰撞检测的实现:

2.1 简单场景碰撞检测

  • 简单场景指的是单一的场景、单帧场景或者可以看作是静止不动的模型场景
  • 下边仅通过 bullet 库进行碰撞检测测试
#include <iostream>
#include <rl/sg/Body.h>
#include <rl/sg/Model.h>
#include <rl/sg/SimpleScene.h>#define RL_SG_BULLET 1  //此处添加宏定义,下边的if语句方可生效#ifdef RL_SG_BULLET
#include <rl/sg/bullet/Scene.h>
#endif // RL_SG_BULLET
#ifdef RL_SG_FCL
#include <rl/sg/fcl/Scene.h>
#endif // RL_SG_FCL
#ifdef RL_SG_ODE
#include <rl/sg/ode/Scene.h>
#endif // RL_SG_ODE
#ifdef RL_SG_PQP
#include <rl/sg/pqp/Scene.h>
#endif // RL_SG_PQP
#ifdef RL_SG_SOLID
#include <rl/sg/solid/Scene.h>
#endif // RL_SG_SOLID//int
//main(int argc, char** argv)
//{
//	if (argc < 2)
//	{
//		std::cout << "Usage: rlSceneCollisionTest SCENEFILE [shouldCollide]" << std::endl;
//		return EXIT_FAILURE;
//	}
//	
//	bool shouldCollide = (argc > 2);
int main()
{bool shouldCollide = true;std::vector<rl::sg::SimpleScene*> scenes;std::vector<std::string> sceneNames;#ifdef RL_SG_BULLETscenes.push_back(new rl::sg::bullet::Scene);sceneNames.push_back("bullet");
#endif // RL_SG_BULLET
#ifdef RL_SG_FCLscenes.push_back(new rl::sg::fcl::Scene);sceneNames.push_back("fcl");
#endif // RL_SG_FCL
#ifdef RL_SG_ODEscenes.push_back(new rl::sg::ode::Scene);sceneNames.push_back("ode");
#endif // RL_SG_ODE
#ifdef RL_SG_PQPscenes.push_back(new rl::sg::pqp::Scene);sceneNames.push_back("pqp");
#endif // RL_SG_PQP
#ifdef RL_SG_SOLIDscenes.push_back(new rl::sg::solid::Scene);sceneNames.push_back("solid");
#endif // RL_SG_SOLIDfor (std::size_t i = 0; i < scenes.size(); ++i){std::cout << "Loading " << sceneNames[i] << " scene" << std::endl;scenes[i]->load("puma560/unimation-puma560_boxes.xml.xml");}std::cout << "Loading done." << std::endl;std::cout << "Loading done." << std::endl;for (int i = 0; i < scenes->getNumModels(); ++i){rl::sg::Model* mdl = scenes->getModel(i);std::cout << "model" << i << "'name" << mdl->getName() << " have" << mdl->getNumBodies() << " bodies" << std::endl;for (int j = 0; j < mdl->getNumBodies(); ++j){std::cout << "    body" << j << "'name:" << mdl->getBody(j)->getName() << std::endl;}}int errorlevel = EXIT_SUCCESS;std::cout << "Now testing, expect isColliding()=" << shouldCollide << std::endl;if (scenes->isColliding() ^ shouldCollide)//异或{ std::cerr << "Error: " << sceneNames << " reports SimpleScene::isColliding()=" << scenes->isColliding() << std::endl;errorlevel = EXIT_FAILURE;}else {std::cout << "isColliding() = " << scenes->isColliding() << std::endl;std::cout << "isColliding() = " << scenes->areColliding(scenes->getModel(0)->getBody(4), scenes->getModel(1)->getBody(0)) << std::endl;std::cout << "isColliding() = " << scenes->areColliding(scenes->getModel(0)->getBody(4), scenes->getModel(0)->getBody(3)) << std::endl;std::cout << "isColliding() = " << scenes->areColliding(scenes->getModel(0)->getBody(4), scenes->getModel(0)->getBody(2)) << std::endl;}std::cout << "Testing done." << std::endl;return 0;
}

如下可以看出:模型的body名字,还有就是场景中的模型之间的碰撞关系:
在这里插入图片描述

2.2 复杂场景碰撞检测

复杂场景指机器人连续运动过程中

#include <iostream>
#include <memory>
#include <random>
#include <stdexcept>
#include <boost/lexical_cast.hpp>#include <rl/mdl/Kinematic.h>
#include <rl/mdl/XmlFactory.h>
#include <rl/sg/Body.h>
#include <rl/sg/Model.h>
#include <rl/sg/SimpleScene.h>
#include <rl/sg/bullet/Scene.h>//bullet库用于碰撞检测// mdl/sg-only version
// mdl/sg-only version
rl::math::Vector is_ScaneColliding(rl::sg::SimpleScene* scene)
{::rl::sg::Model* robotModel = scene->getModel(0);::rl::sg::Model* EWallModel = scene->getModel(1);rl::math::Vector res(robotModel->getNumBodies());for (::std::size_t i = 0; i < robotModel->getNumBodies(); ++i){res[i] = scene->areColliding(robotModel->getBody(i), EWallModel->getBody(0));/*if (res[i]){return true;}*/}return res;
}int main()
{std::cout.precision(4);std::cerr.precision(4);std::string model_path = "./puma560/unimation-puma560.xml";std::string scane_path = "./puma560/unimation-puma560_boxes.xml";std::vector<rl::math::Real>angles{ -24,3,3,0,0,0 };读入模型rl::mdl::XmlFactory factory;std::shared_ptr<rl::mdl::Model> model(factory.create(model_path));rl::mdl::Kinematic* kinematics = dynamic_cast<rl::mdl::Kinematic*>(model.get());std::size_t dof = kinematics->getDof();rl::math::Vector q(kinematics->getDof());for (std::size_t i = 0; i < kinematics->getDof(); ++i){q(i) = angles[i] * rl::math::DEG2RAD;}std::cout << "Set joint angle [rad] " << q.transpose() << std::endl;kinematics->setPosition(q);kinematics->forwardPosition();rl::sg::SimpleScene* scenes = new rl::sg::bullet::Scene;std::string sceneNames = "bullet";//导入场景模型文件std::cout << "Loading " << sceneNames << " scene" << std::endl;scenes->load(scane_path);assert(scenes[i]->getModel(0)->getNumBodies() == kinematics->getBodies());for (std::size_t b = 0; b < kinematics->getBodies(); ++b){scenes->getModel(0)->getBody(b)->setFrame(kinematics->getFrame(b));}std::cout << "Loading done." << std::endl;std::cout << "Testing SimpleScene::isColliding() in " << sceneNames << ": ";//std::cout << "collides(scenes, kinematics) = "<<(collides(scenes, kinematics) ? "true" : "false") << std::endl;std::mt19937 randomGenerator(0);std::uniform_real_distribution<rl::math::Real> randomDistribution(-180 * rl::math::DEG2RAD, 180 * rl::math::DEG2RAD);rl::math::Vector a0(dof); //1rl::math::Vector a1(dof); //1rl::math::Vector a2(dof); //1rl::math::Vector a3(dof); //1rl::math::Vector a4(dof); //1rl::math::Vector a5(dof); //0rl::math::Vector a6(dof); //0rl::math::Vector a7(dof); //0rl::math::Vector a8(dof); //0rl::math::Vector a9(dof); //0rl::math::Vector a10(dof); //0rl::math::Vector a11(dof); //0rl::math::Vector a12(dof); //0rl::math::Vector a13(dof); //0//有碰撞a0 << -101, -97, 90, 0, 0, 0;a1 << 27, -158, 90, 0, 0, 0;a2 << -105, -184, 90, 0, 0, 0;a3 << 143, -146, 90, 0, 0, 0;a4 << 135, -223, 90, 0, 0, 0;//无碰撞a5 << 76, -169, 90, 0, 0, 0;a6 << 88, -64, 90, 0, 0, 0;a7 << -15, -50, 90, 0, 0, 0;a8 << 40, -119, 90, 10, 12, 0;a9 << 2, -39, 90, 0, 0, 0;a10 << 0, 0, 0, 0, 0, 0;a11 << 0, 0, 0, 0, 6, 0;a12 << -10, -25, 90, 0, 6, 0;a13 << -10, -25, 90, 0, 6, 0;std::vector<rl::math::Vector>angs{ a0,a1,a2,a3,a4,a5,a6,a7,a8,a9,a10,a11,a12,a13 };std::size_t j;for (j = 0; j < angs.size(); ++j){rl::math::Vector q(dof);/*for (std::size_t i = 0; i < dof; ++i){q(i) = randomDistribution(randomGenerator);}*/q = angs[j] * rl::math::DEG2RAD;kinematics->setPosition(q);kinematics->forwardPosition();bool result;for (std::size_t b = 0; b < kinematics->getBodies(); ++b){scenes->getModel(0)->getBody(b)->setFrame(kinematics->getFrame(b));}rl::math::Vector res = is_ScaneColliding(scenes);//std::cout << res.transpose() << std::endl;std::cout << j << " :   q: "<<q.transpose()*rl::math::RAD2DEG<<"       result = " << res.transpose() << std::endl;}return EXIT_SUCCESS;
}

在这里插入图片描述
通过上述检测结果可对比软件仿真结果,检测结果是正确的
在这里插入图片描述

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

如若内容造成侵权/违法违规/事实不符,请联系dt猫网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

【漏洞复现-weblogic-文件上传】vulfocus/weblogic-cve_2018_2894

目录 一、靶场环境 1.1、平台&#xff1a; 1.2、知识: 1.3、描述&#xff1a; 二、漏洞验证 2.1、分析&#xff1a; 2.4、解题&#xff1a; 一、靶场环境 1.1、平台&#xff1a; Vulfocus 漏洞威胁分析平台 123.58.224.8:43200 访问http://ip:port/console后跳转到登陆页…

java基于微信小程序的社区疫情防疫系统 uniapp小程序

用户往往因为不能及时了解到疫情的最新进展而造成许多担忧。另一方面&#xff0c;疫情信息没能进行系统的管理与维护使用户没能在系统里及时的获取到信息。而传统的疫情防控系统&#xff0c;采用的还是人工管理、手工备案、人工查询的方式。但是随之用户人数的增多这种管理方式…

Exchange Online 发送邮件

项目背景 2022年10月4日 微软更改了Exchange的验证方式,原来exchangelib的库没法继续实现邮件的发送。 实现方式 利用Microsoft Graph API 里 发送邮件 - Microsoft Graph v1.0 | Microsoft LearnPOST https://graph.microsoft.com/v1.0/me/sendMail Content-type: text/plai…

【路径规划】基于matlab遗传算法求解仓库拣货距离最短优化问题【含Matlab源码 2154期】

一、物流配送中心拣货作业简介 1 物流配送中心拣货作业 1.1 问题描述 双区型仓库一般是由一定数量的等长巷道组成&#xff0c;巷道两侧的货架上存放着需要拣取的各种物品。横向有三条过道&#xff0c;不同于单区型仓库&#xff08;single-block warehouse&#xff09;的是&…

面向对象核心概念实验

第1关:面向对象核心概念编程-简单测试 本关任务: 一、请在源代码文件“Complex.java”中实现公开的复数类“Complex”,其变量(属性)是复数的实部与虚部,实部和虚部的类型都为double,访问限制为私有。同时定义以下公开方法: (1)构造方法:提供几种构造方法,包括没有…

第 8 章 项目质量管理

手工艺阶段---质量检验阶段---统计质量阶段---全面质量管理阶段&#xff08;TQM&#xff0c;全面&#xff0c;全过程&#xff0c;全员参与&#xff09; TQM又强调5Q法&#xff0c; QS&#xff1a;遵从质量管理体系&#xff0c;公司级负责 QP&#xff1a;制定质量管理计划&…

Vue中的props配置项(父组件向子组件传数据)

目录 总结 问题1&#xff1a;如果子组件Student.vue接收到数据后&#xff0c;要对数据进行操作&#xff0c;比如说&#xff1a;让显示在页面上的年龄比接收到的年龄要大&#xff0c;怎么操作&#xff1f;--> 通过 v-bind绑定属性 ,其属性值是 运行引号里面JS表达式执行的结…

Research on Micro-Expression Spotting Method Based on Optical Flow Features

Research on Micro-Expression Spotting Method Based on Optical Flow Features 哈喽&#xff0c;大家好&#xff0c;从今天开始更《菜鸡读论文》系列&#xff0c;因为我真的很菜&#xff0c;可以说是CV白的不能再白了&#xff0c;每天都在膜拜大佬&#xff0c;感觉别人和自己…

linux内存重映射的概念及对内核虚拟地址的重映射方法分析

【摘要】本文分析了Linux设备的内存映射的相关概念和理论&#xff0c;使用例子对mmap及nopage的驱动编写方法进行了解释&#xff0c;最后对3种不同的内核虚拟空间分配方法下&#xff0c;mmap驱动编写方法进行了细致的分析和调试。 1、mmap概念 如下图所示&#xff0c;mmap是操…

学习笔记276—怎么查询哪些药属于医保报销范围?

第一个方法:登陆中国医疗保险网。 点击下方的查询中的医保目录查询: 在页面中输入你想知道的药品名称即可: 意在交流学习,欢迎点赞评论🙏, 如有谬误,请联系指正。转载请注明出处。 联系方式: e-mail: heyi9069@gmail.com QQ: 3309198330(请简要说明来意,并备注“来…

Java 方法区的演进

在 JDK7 及以前&#xff0c;习惯上把方法区&#xff0c;称为永久代。JDK8开始&#xff0c;使用元空间取代了永久代。 元空间的本质和永久代类似&#xff0c;都是对JVM规范中方法区的实现。不过元空间与永久代最大的区别在于&#xff1a;元空间不在虚拟机设置的内存中&#xff…

Python 中计算字符串中的元音个数

计算字符串中的元音个数&#xff1a; 将元音存储在一个字符串中。使用 dict 理解来迭代字符串。使用 str.count() 方法计算原始字符串中每个元音的出现次数。 vowels aeioumy_str www.jiyik.com# ✅ 计算字符串中每个元音出现的次数 vowels_count {vowel: my_str.lower().…

计算机专业研究生核心能力培养(4)——实验的流程及规范

0. 前言 今天我们讲一讲实验的流程和规范,主要包括以下7个部分: 目的清晰代码规范实验可拓展结果可比较结果可复现分析实验(常规)分析实验(有针对性)1. 目的清晰 当我们进行一个课题进行研究的时候,最重要的是要目的清晰,也就是说我们为什么要进行这个研究。做实验的…

使用cpolar建立固定的SSH隧道

一直以来&#xff0c;不同的操作系统都在相互竞争&#xff0c;而竞争的结果&#xff0c;就每种操作系统都占领了自己的一片天地。虽然现在的家用电脑大多使用Windows操作系统&#xff0c;但服务器领域大多使用占用资源更少的Linux系统&#xff0c;更不用说苹果电脑专属的iOS操作…

Cell:女性痴呆风险是男性的2倍,与基因相关,或是与生俱来的

医林研究院&#xff0c;让医学更简单 阿尔茨海默症&#xff08;AD&#xff09;&#xff0c;俗称老年痴呆症&#xff0c;是一个具有高经济和社会负担的全球健康问题。全球约有5000万痴呆症病例&#xff0c;每年新增约有1000万例&#xff0c;在中国&#xff0c;有上千万患者&…

Github每日精选(第52期):验证您的有风险的shell命令shellfirm

shellfirm shellfirm 是一个shell的拦截器&#xff0c;拦截任何有风险的shell命令&#xff08;默认或由您定义&#xff09;并提示您进行双重验证。 我如何从自己身上拯救自己&#xff1f; rm -rf *git reset --hard在按下回车键之前&#xff1f;kubectl delete ns停止&#…

【干货】10个高质量的java自学网站推荐

经常有人留言问我&#xff0c;“想学习Java编程&#xff0c;有没有学习资源推荐&#xff0c;有哪些网站可以关注”。好些同学是去网盘搜索&#xff0c;或者去某宝购买&#xff0c;搜集一堆资料&#xff0c;但是又不清楚哪些是重复的内容&#xff0c;哪些内容是不是版本已经过时…

【Bluetooth|蓝牙开发】十一、一文秒懂 | 超详细的Bluez交叉编译

个人主页&#xff1a;董哥聊技术我是董哥&#xff0c;嵌入式领域新星创作者创作理念&#xff1a;专注分享高质量嵌入式文章&#xff0c;让大家读有所得&#xff01;【 所有文章汇总】 1、 前言 前面几篇文章&#xff0c;主要讲解了蓝牙协议栈层面的内容&#xff0c;本篇来从源…

城区导航智能驾驶难在哪?写在小鹏/华为-极狐NOA释放之时

交流群 | 进“传感器群/滑板底盘群”请加微信号&#xff1a;xsh041388交流群 | 进“汽车基础软件群”请加微信号&#xff1a;ckc1087备注信息&#xff1a;群名称 真实姓名、公司、岗位【本文一万字左右&#xff0c;预计阅读时间约20分钟&#xff0c;文中有若干图片/GIF/视频&a…

猿创征文 | 国产数据库之PolarDB-X数据库详解安装和使用

文章目录1、PolarDB-X是什么&#xff1f;2、PolarDB-X架构3、PolarDB-X架构优势4、PolarDB-X核心特性5、PolarDB-X部署5.1、通过PXD部署集群5.2、通过 K8S 部署5.3、通过编译安装1、PolarDB-X是什么&#xff1f; PolarDB-X是由阿里巴巴自主研发的云原生分布式数据库&#xff0…