【中文注释】planning_scene_tutorial.cpp
planning_scene_tutorial.cpp
#include <rclcpp/rclcpp.hpp>// MoveIt 相关头文件
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/kinematic_constraints/utils.h>// BEGIN_SUB_TUTORIAL stateFeasibilityTestExample
//
// 用户可以为 PlanningScene 类指定自定义约束
// 通过使用 setStateFeasibilityPredicate 函数来实现自定义的回调函数。
// 以下是一个简单的示例,用于检查 Panda 机器人中的 "panda_joint1" 关节是否为正角度:
bool stateFeasibilityTestExample(const moveit::core::RobotState& robot_state, bool /*verbose*/)
{const double* joint_values = robot_state.getJointPositions("panda_joint1");return (joint_values[0] > 0.0);
}
// END_SUB_TUTORIALstatic const rclcpp::Logger LOGGER = rclcpp::get_logger("planning_scene_tutorial");int main(int argc, char** argv)
{rclcpp::init(argc, argv);rclcpp::NodeOptions node_options;node_options.automatically_declare_parameters_from_overrides(true);auto planning_scene_tutorial_node = rclcpp::Node::make_shared("planning_scene_tutorial", node_options);rclcpp::executors::SingleThreadedExecutor executor;executor.add_node(planning_scene_tutorial_node);std::thread([&executor]() { executor.spin(); }).detach();// BEGIN_TUTORIAL//// 设置// ^^^^^//// PlanningScene 类可以使用 RobotModel 或 URDF 和 SRDF 轻松设置和配置。// 不过这不是推荐的方式,建议使用 PlanningSceneMonitor 来创建和维护当前的规划场景,// 使用来自机器人关节和传感器的数据来进行维护。在这个教程中,我们将直接实例化一个// PlanningScene 类,但这种实例化方式仅用于说明。robot_model_loader::RobotModelLoader robot_model_loader(planning_scene_tutorial_node, "robot_description");const moveit::core::RobotModelPtr& kinematic_model = robot_model_loader.getModel();planning_scene::PlanningScene planning_scene(kinematic_model);// 碰撞检测// ^^^^^^^^^^//// 自碰撞检测// ~~~~~~~~~~~//// 首先,我们检查机器人当前状态是否处于自碰撞状态,// 即当前的配置是否会导致机器人各部分互相碰撞。// 我们需要构造一个 CollisionRequest 对象和一个 CollisionResult 对象,// 然后将它们传入碰撞检测函数中。碰撞检测的结果会保存在 CollisionResult 对象中。// 自碰撞检测使用的是机器人的未加垫版本,直接使用 URDF 中提供的碰撞网格,未额外添加任何填充。collision_detection::CollisionRequest collision_request;collision_detection::CollisionResult collision_result;planning_scene.checkSelfCollision(collision_request, collision_result);RCLCPP_INFO_STREAM(LOGGER, "Test 1: Current state is " << (collision_result.collision ? "in" : "not in")<< " self collision");// 改变机器人状态// ~~~~~~~~~~~~~~~~//// 现在,我们来改变机器人的当前状态。规划场景会维护当前状态,// 我们可以获取一个引用来修改它,然后再次检查新配置下的碰撞情况。// 请注意,每次新的碰撞检测前都需要清除 CollisionResult。moveit::core::RobotState& current_state = planning_scene.getCurrentStateNonConst();current_state.setToRandomPositions();collision_result.clear();planning_scene.checkSelfCollision(collision_request, collision_result);RCLCPP_INFO_STREAM(LOGGER, "Test 2: Current state is " << (collision_result.collision ? "in" : "not in")<< " self collision");// 只检查某个组// ~~~~~~~~~~~~~~//// 接下来,我们只对 Panda 机器人的手部进行碰撞检测,// 即检查手部是否与机器人身体的其他部分发生碰撞。// 我们可以通过在 CollisionRequest 中添加组名 "hand" 来实现这一点。collision_request.group_name = "hand";current_state.setToRandomPositions();collision_result.clear();planning_scene.checkSelfCollision(collision_request, collision_result);RCLCPP_INFO_STREAM(LOGGER, "Test 3: Current state is " << (collision_result.collision ? "in" : "not in")<< " self collision");// 获取碰撞信息// ~~~~~~~~~~~~~//// 首先手动设置 Panda 机械臂到一个我们知道会发生自碰撞的位置。// 请注意,这个状态实际上已经超出了 Panda 关节的限制,我们可以直接检查这一点。std::vector<double> joint_values = { 0.0, 0.0, 0.0, -2.9, 0.0, 1.4, 0.0 };const moveit::core::JointModelGroup* joint_model_group = current_state.getJointModelGroup("panda_arm");current_state.setJointGroupPositions(joint_model_group, joint_values);RCLCPP_INFO_STREAM(LOGGER, "Test 4: Current state is "<< (current_state.satisfiesBounds(joint_model_group) ? "valid" : "not valid"));// 现在,我们可以获取 Panda 机械臂在某个给定配置下的所有碰撞信息。// 通过在 CollisionRequest 中填写相应的字段并指定返回的最大接触数量,// 我们可以请求这些碰撞信息。collision_request.contacts = true;collision_request.max_contacts = 1000;collision_result.clear();planning_scene.checkSelfCollision(collision_request, collision_result);RCLCPP_INFO_STREAM(LOGGER, "Test 5: Current state is " << (collision_result.collision ? "in" : "not in")<< " self collision");collision_detection::CollisionResult::ContactMap::const_iterator it;for (it = collision_result.contacts.begin(); it != collision_result.contacts.end(); ++it){RCLCPP_INFO(LOGGER, "Contact between: %s and %s", it->first.first.c_str(), it->first.second.c_str());}// 修改允许的碰撞矩阵(Allowed Collision Matrix, ACM)// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~//// Allowed Collision Matrix (ACM) 提供了一种机制,让碰撞检测器忽略某些对象之间的碰撞:// 包括机器人自身的部分之间的碰撞以及机器人与环境中的物体之间的碰撞。// 我们可以告诉碰撞检测器忽略前面提到的所有链接之间的碰撞,即使它们实际上是碰撞的,// 碰撞检测器也会忽略这些碰撞,返回该状态为“未发生碰撞”。// 在这个例子中,我们同时对 ACM 和当前状态进行了复制,并传入碰撞检测函数。collision_detection::AllowedCollisionMatrix acm = planning_scene.getAllowedCollisionMatrix();moveit::core::RobotState copied_state = planning_scene.getCurrentState();collision_detection::CollisionResult::ContactMap::const_iterator it2;for (it2 = collision_result.contacts.begin(); it2 != collision_result.contacts.end(); ++it2){acm.setEntry(it2->first.first, it2->first.second, true);}collision_result.clear();planning_scene.checkSelfCollision(collision_request, collision_result, copied_state, acm);RCLCPP_INFO_STREAM(LOGGER, "Test 6: Current state is " << (collision_result.collision ? "in" : "not in")<< " self collision");// 完整碰撞检测
// ~~~~~~~~~~~~~~~~~~~~~~~
//
// 虽然我们之前已经进行了自碰撞检测,但我们可以使用 checkCollision 函数,
// 该函数不仅会检测自碰撞,还会检测与环境(当前为空)的碰撞。
// 这是在路径规划中最常用的一组碰撞检测函数。注意,与环境的碰撞检测会使用机器人带有缓冲区的版本。
// 缓冲区可以帮助机器人在环境中与障碍物保持一定距离。
collision_result.clear();
planning_scene.checkCollision(collision_request, collision_result, copied_state, acm);
RCLCPP_INFO_STREAM(LOGGER, "测试 7: 当前状态 " << (collision_result.collision ? "发生" : "未发生") << " 自碰撞");// 约束检测
// ^^^^^^^^^^^^^^^^^^^
//
// PlanningScene 类中还包含便于使用的函数来检测约束。
// 约束可以有两种类型:
// (a) 从 :moveit_codedir:`KinematicConstraint<moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h>`
// 中选择的约束集:如关节约束(JointConstraint)、位置约束(PositionConstraint)、方向约束(OrientationConstraint)和可视性约束(VisibilityConstraint)。
// (b) 用户通过回调函数自定义的约束。我们将首先看一个简单的 KinematicConstraint 示例。
//
// 检测运动学约束
// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
// 我们将首先对熊猫机器人(Panda robot)的 panda_arm 组的末端执行器定义一个简单的位置和方向约束。
// 注意,这里使用了一些便捷的函数来填充约束(这些函数可以在
// :moveit_codedir:`utils.h<moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h>`
// 文件中找到,该文件位于 moveit_core 的 kinematic_constraints 目录下)。std::string end_effector_name = joint_model_group->getLinkModelNames().back();geometry_msgs::msg::PoseStamped desired_pose;
desired_pose.pose.orientation.w = 1.0;
desired_pose.pose.position.x = 0.3;
desired_pose.pose.position.y = -0.185;
desired_pose.pose.position.z = 0.5;
desired_pose.header.frame_id = "panda_link0";
moveit_msgs::msg::Constraints goal_constraint =kinematic_constraints::constructGoalConstraints(end_effector_name, desired_pose);// 现在,我们可以使用 PlanningScene 类中的 isStateConstrained 函数检查某个状态是否符合此约束。copied_state.setToRandomPositions();
copied_state.update();
bool constrained = planning_scene.isStateConstrained(copied_state, goal_constraint);
RCLCPP_INFO_STREAM(LOGGER, "测试 8: 随机状态 " << (constrained ? "符合" : "不符合") << " 约束");// 当需要多次检查相同约束(例如在规划器中)时,有一种更高效的方法来检测约束。
// 我们首先构建一个 KinematicConstraintSet,预处理 ROS 约束消息并为快速处理做好设置。kinematic_constraints::KinematicConstraintSet kinematic_constraint_set(kinematic_model);
kinematic_constraint_set.add(goal_constraint, planning_scene.getTransforms());
bool constrained_2 = planning_scene.isStateConstrained(copied_state, kinematic_constraint_set);
RCLCPP_INFO_STREAM(LOGGER, "测试 9: 随机状态 " << (constrained_2 ? "符合" : "不符合") << " 约束");// 也可以直接使用 KinematicConstraintSet 类来完成此操作。kinematic_constraints::ConstraintEvaluationResult constraint_eval_result =kinematic_constraint_set.decide(copied_state);
RCLCPP_INFO_STREAM(LOGGER, "测试 10: 随机状态 " << (constraint_eval_result.satisfied ? "符合" : "不符合") << " 约束");// 用户定义的约束
// ~~~~~~~~~~~~~~~~~~~~~~~~
//
// CALL_SUB_TUTORIAL stateFeasibilityTestExample// 现在,每当调用 isStateFeasible 时,就会调用这个用户定义的回调函数。planning_scene.setStateFeasibilityPredicate(stateFeasibilityTestExample);
bool state_feasible = planning_scene.isStateFeasible(copied_state);
RCLCPP_INFO_STREAM(LOGGER, "测试 11: 随机状态 " << (state_feasible ? "可行" : "不可行"));// 每当调用 isStateValid 时,将执行三个检查:
// (a) 碰撞检测 (b) 约束检测 (c) 使用用户定义回调的可行性检测。bool state_valid = planning_scene.isStateValid(copied_state, kinematic_constraint_set, "panda_arm");
RCLCPP_INFO_STREAM(LOGGER, "测试 12: 随机状态 " << (state_valid ? "有效" : "无效"));// 注意,所有通过 MoveIt 和 OMPL 可用的规划器都会执行碰撞检测、约束检测以及使用用户定义回调的可行性检测。
// 结束教程rclcpp::shutdown();
return 0;
相关文章:
【中文注释】planning_scene_tutorial.cpp
planning_scene_tutorial.cpp #include <rclcpp/rclcpp.hpp>// MoveIt 相关头文件 #include <moveit/robot_model_loader/robot_model_loader.h> #include <moveit/planning_scene/planning_scene.h> #include <moveit/kinematic_constraints/utils.h>…...

【Vue3】 h()函数的用法
目录 介绍 参数 使用案例 1.创建虚拟 DOM 元素 2. 组件的动态渲染 3. 创建功能组件 4.渲染动态属性 5. 使用插槽 6. 创建动态标签 介绍 h() 函数用于辅助创建虚拟 DOM 节点,它是 hypescript 的简称——能生成 HTML (超文本标记语言) 的 JavaScript&#x…...
Flask如何实现前后端分离项目
在现代Web开发中,前后端分离是一种常见的架构模式,其中前端和后端分别独立开发和部署,通过API进行通信。Flask作为后端框架,可以很容易地与前端框架(如React、Vue.js或Angular)配合使用来实现前后端分离。以…...

二维码生成器 1.02.41| 一站式QR码生成器和美化工具
二维码生成器是一个有用的QR码生成器应用程序。可以轻松地为网站链接、文本、WiFi、名片、短信、社交媒体账户等生成QR码。该应用支持更改QR码的颜色、码眼图案和框架,并可以添加徽标和文本,使QR码更加美观。使用此QR码生成器,可以使用设计精…...
腾讯云视立方·直播 SDK 合规使用指南
为帮助使用直播 SDK 的开发运营者(以下简称“您”)在符合个人信息保护相关法律法规、政策及标准的规定下合规接入、使用第三方SDK,深圳市腾讯计算机系统有限公司(以下简称"我们")特制定《直播 SDK 接入使用说…...

在 Spring 中使用 @EhCache 注解作为缓存
文章目录 项目概况项目设置一个简单的 RESTful Web 服务Spring 整合 EhCache第 1 步:更新依赖项以使用 EhCache Spring 注解第 2 步:设置自定义缓存管理器第 3 步:配置 EhCache第 4 步:测试缓存 刷新缓存总结推荐阅读文章 EhCache…...

npm install进度卡在 idealTree:node_global: sill idealTree buildDeps
ping一下源:ping http://registry.npm.taobao.org/ ping不通,原因:原淘宝npm永久停止服务,已更新新域名~~震惊!!! 重新安装:npm config set registry https://registry.npmmirror.c…...
力扣1031. 两个非重叠子数组的最大和
力扣1031. 两个非重叠子数组的最大和 题目解析及思路 题目要求找到两段长分别为firstLen 和 secondLen的子数组,使两段元素和最大 图解见灵神 枚举第二段区间的右端点,在左边剩余部分中找出元素和最大的第一段区间,并用前缀和优化求子数组…...

【Unity实战篇】 接入百度翻译,实现文本自动翻译功能
前言【Unity实战篇】 接入百度自动翻译,实现文本自动翻译功能一、获取百度翻译开发平台的APPID和密钥二、Unity中接入自动翻译功能三、Unity中实现自动翻译文本Text功能总结前言 日常在做项目的过程中,游戏本地化几乎已经成为必不可少的一步。本篇文章将演示怎样在Unity中接入…...
ubuntu samba
参考: 基于Ubuntu22.04的Samba服务器搭建教程(新手保姆级教程)_ubuntu samba-CSDN博客 当时按照这个不行: 主要做了这些修改 1、ufw 打开端口 这些都打开了 Samba服务使用的端口和协议如下1234: Port 137 (UDP) - NetBIOS 名…...

Linux系统和数据库常用的命令2
Linux系统和数据库常用的命令2 1、两台Linux机器ssh免密登录 client端登录server端需要免密,只需把公钥发送到server就可,会在server端生成一个authorized_keys文件 # 108机器上[rootclient ~]# ssh-keygen -t rsa // 非对称算法 Generating public/…...

Golang | Leetcode Golang题解之第468题验证IP地址
题目: 题解: func validIPAddress(queryIP string) string {if sp : strings.Split(queryIP, "."); len(sp) 4 {for _, s : range sp {if len(s) > 1 && s[0] 0 {return "Neither"}if v, err : strconv.Atoi(s); err …...
mermaid 图表相关
1.mermaid图表的代码 1.1 flowchart 流程图代码 flowchart TDA[Christmas] -->|Get money| B(Go shopping)B --> C{Let me think}C -->|One| D[Laptop]C -->|Two| E[iPhone]C -->|Three| F[fa:fa-car Car]1.2 sequece 时序图代码 sequenceDiagramAlice->&…...

Unity接入人工智能
在Unity接入人工智能中,本篇实现了接入百度智能云ai进行npc智能对话,通过http方式,并非插件,适合于所有支持Http链接的Unity版本。对于Chartgpt可以参考本篇内容的实现过程。 1-4节讲解测试,第5节讲解Unity中的实现&a…...

C语言笔记 14
函数原型 函数的先后关系 我们把自己定义的函数isPrime()写在main函数上面 是因为C的编译器自上而下顺序分析你的代码,在看到isPrime的时候,它需要知道isPrime()的样子——也就是isPrime()要几个参数,每个参数的类型如何,返回什么…...

Cpp::STL—list类的模拟实现(上)(13)
文章目录 前言一、结点类的实现二、迭代器类的实现迭代器类的存在意义迭代器类的模板参数构造函数运算符的重载--运算符的重载、!运算符的重载*运算符的重载->运算符的重载 总结 前言 注意本篇难度偏高,其主要体现在迭代器类的实现! 什么…...

ListView的Items绑定和comboBox和CheckBox组合使用实现复选框的功能
为 ListView 控件的内容指定视图模式的方法,参考官方文档。 ComboBox 样式和模板 案例说明:通过checkBox和ComboBox的组合方式实现下拉窗口的多选方式,同时说明了ListView中Items项目的两种绑定方式. 示例: 设计样式 Xaml代码…...
PetaLinux工程的常用命令——petalinux-build
petalinux-build:编译项目或指定组件。 注:有些命令我没用过,瞎翻译有可能会翻译错了。 用法: petalinux-build [options] 可选参数: -h, --help 显示函数用法。 -p, --project <PROJECT> PetaLinuxSDK项目的路径。默认…...

【Qt】窗口预览(1)—— 菜单栏
窗口预览(1) 1. QMainWindow2. QMenuBar——菜单栏2.1 创建菜单栏/将菜单栏添加到widget中2.2 addMenu——在菜单栏中添加菜单2.3 在菜单中添加选项2.4 添加快捷键2.5 支持嵌套添加菜单2.6 添加信号2.7 添加分割线 1. QMainWindow Qt窗口是通过QMainWin…...
揭秘酱香型白酒中的6大劣质酒的特点,守好你的健康与钱包
你知道吗?居然有 90%的人都喝过这 6 种劣质酱香型白酒,今天酱酒亮哥就带大家一起揭开它们的真面目,看看你中招了没有! 先说那种有很浓的生粮味的酱酒,就像刚磨出来还没烧开的豆浆味,喝起来那叫一个难受。想…...

Docker 离线安装指南
参考文章 1、确认操作系统类型及内核版本 Docker依赖于Linux内核的一些特性,不同版本的Docker对内核版本有不同要求。例如,Docker 17.06及之后的版本通常需要Linux内核3.10及以上版本,Docker17.09及更高版本对应Linux内核4.9.x及更高版本。…...

JavaSec-RCE
简介 RCE(Remote Code Execution),可以分为:命令注入(Command Injection)、代码注入(Code Injection) 代码注入 1.漏洞场景:Groovy代码注入 Groovy是一种基于JVM的动态语言,语法简洁,支持闭包、动态类型和Java互操作性,…...

装饰模式(Decorator Pattern)重构java邮件发奖系统实战
前言 现在我们有个如下的需求,设计一个邮件发奖的小系统, 需求 1.数据验证 → 2. 敏感信息加密 → 3. 日志记录 → 4. 实际发送邮件 装饰器模式(Decorator Pattern)允许向一个现有的对象添加新的功能,同时又不改变其…...
大语言模型如何处理长文本?常用文本分割技术详解
为什么需要文本分割? 引言:为什么需要文本分割?一、基础文本分割方法1. 按段落分割(Paragraph Splitting)2. 按句子分割(Sentence Splitting)二、高级文本分割策略3. 重叠分割(Sliding Window)4. 递归分割(Recursive Splitting)三、生产级工具推荐5. 使用LangChain的…...
论文解读:交大港大上海AI Lab开源论文 | 宇树机器人多姿态起立控制强化学习框架(一)
宇树机器人多姿态起立控制强化学习框架论文解析 论文解读:交大&港大&上海AI Lab开源论文 | 宇树机器人多姿态起立控制强化学习框架(一) 论文解读:交大&港大&上海AI Lab开源论文 | 宇树机器人多姿态起立控制强化…...

SpringCloudGateway 自定义局部过滤器
场景: 将所有请求转化为同一路径请求(方便穿网配置)在请求头内标识原来路径,然后在将请求分发给不同服务 AllToOneGatewayFilterFactory import lombok.Getter; import lombok.Setter; import lombok.extern.slf4j.Slf4j; impor…...

【Linux】Linux 系统默认的目录及作用说明
博主介绍:✌全网粉丝23W,CSDN博客专家、Java领域优质创作者,掘金/华为云/阿里云/InfoQ等平台优质作者、专注于Java技术领域✌ 技术范围:SpringBoot、SpringCloud、Vue、SSM、HTML、Nodejs、Python、MySQL、PostgreSQL、大数据、物…...

论文阅读笔记——Muffin: Testing Deep Learning Libraries via Neural Architecture Fuzzing
Muffin 论文 现有方法 CRADLE 和 LEMON,依赖模型推理阶段输出进行差分测试,但在训练阶段是不可行的,因为训练阶段直到最后才有固定输出,中间过程是不断变化的。API 库覆盖低,因为各个 API 都是在各种具体场景下使用。…...
redis和redission的区别
Redis 和 Redisson 是两个密切相关但又本质不同的技术,它们扮演着完全不同的角色: Redis: 内存数据库/数据结构存储 本质: 它是一个开源的、高性能的、基于内存的 键值存储数据库。它也可以将数据持久化到磁盘。 核心功能: 提供丰…...

Visual Studio Code 扩展
Visual Studio Code 扩展 change-case 大小写转换EmmyLua for VSCode 调试插件Bookmarks 书签 change-case 大小写转换 https://marketplace.visualstudio.com/items?itemNamewmaurer.change-case 选中单词后,命令 changeCase.commands 可预览转换效果 EmmyLua…...