Ros2 - Moveit2 - MoveItCpp

news/2024/9/20 19:00:06

MoveItCpp 教程

介绍

MoveItCpp 是一个新的高级接口，一个统一的 C++ API，不需要使用 ROS 操作、服务和消息来访问核心 MoveIt 功能，并且是现有MoveGroup API的替代方案（不是完全替代） ，我们建议需要更多实时控制或行业应用的高级用户使用此接口。PickNik Robotics 已根据我们许多商业应用的需要开发了此接口。

入门

如果您还没有这样做，请确保您已经完成入门指南中的步骤。

运行代码

打开 shell，运行启动文件：

ros2 launch moveit2_tutorials moveit_cpp_tutorial.launch.py

片刻之后，RViz 窗口应会出现，其外观与本页顶部的窗口类似。要完成每个演示步骤，请按屏幕底部RvizVisualToolsGui面板中的“下一步”按钮，或选择屏幕顶部“工具”面板中的“键工具”，然后在 RViz 处于焦点时按下键盘上的0 。

完整代码

完整代码可以在 MoveIt GitHub 项目中查看。接下来，我们将逐步介绍代码以解释其功能。

设置

static const std::string PLANNING_GROUP = "panda_arm";
static const std::string LOGNAME = "moveit_cpp_tutorial";

ros2_控制器

static const std::vector<std::string> CONTROLLERS(1, "panda_arm_controller");/* Otherwise robot with zeros joint_states */
rclcpp::sleep_for(std::chrono::seconds(1));RCLCPP_INFO(LOGGER, "Starting MoveIt Tutorials...");auto moveit_cpp_ptr = std::make_shared<moveit_cpp::MoveItCpp>(node);
moveit_cpp_ptr->getPlanningSceneMonitorNonConst()->providePlanningSceneService();auto planning_components = std::make_shared<moveit_cpp::PlanningComponent>(PLANNING_GROUP, moveit_cpp_ptr);
auto robot_model_ptr = moveit_cpp_ptr->getRobotModel();
auto robot_start_state = planning_components->getStartState();
auto joint_model_group_ptr = robot_model_ptr->getJointModelGroup(PLANNING_GROUP);

可视化

MoveItVisualTools 包提供了许多在 RViz 中可视化对象、机器人和轨迹的功能，以及调试工具，例如脚本的逐步自检

moveit_visual_tools::MoveItVisualTools visual_tools(node, "panda_link0", "moveit_cpp_tutorial",moveit_cpp_ptr->getPlanningSceneMonitorNonConst());
visual_tools.deleteAllMarkers();
visual_tools.loadRemoteControl();Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
text_pose.translation().z() = 1.75;
visual_tools.publishText(text_pose, "MoveItCpp_Demo", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();

开始演示

visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");

使用 MoveItCpp 进行规划

有多种方法可以设置计划的开始和目标状态，以下计划示例对此进行了说明

计划＃1

我们可以将计划的起始状态设置为机器人的当前状态

planning_components->setStartStateToCurrentState();

设置规划目标的第一种方法是使用 geometry_msgs::PoseStamped ROS 消息类型，如下所示:

geometry_msgs::msg::PoseStamped target_pose1;
target_pose1.header.frame_id = "panda_link0";
target_pose1.pose.orientation.w = 1.0;
target_pose1.pose.position.x = 0.28;
target_pose1.pose.position.y = -0.2;
target_pose1.pose.position.z = 0.5;
planning_components->setGoal(target_pose1, "panda_link8");

现在，我们调用 PlanningComponents 来计算计划并将其可视化。请注意，我们只是在规划:

const planning_interface::MotionPlanResponse plan_solution1 = planning_components->plan();

检查 PlanningComponents 是否成功找到该计划:

if (plan_solution1)
{

在 Rviz 中可视化起始姿势:

visual_tools.publishAxisLabeled(robot_start_state->getGlobalLinkTransform("panda_link8"), "start_pose");

在 Rviz 中可视化目标姿势

visual_tools.publishAxisLabeled(target_pose1.pose, "target_pose");
visual_tools.publishText(text_pose, "setStartStateToCurrentState", rvt::WHITE, rvt::XLARGE);

在 Rviz 中可视化轨迹

  visual_tools.publishTrajectoryLine(plan_solution1.trajectory, joint_model_group_ptr);visual_tools.trigger();/* Uncomment if you want to execute the plan *//* bool blocking = true; *//* moveit_controller_manager::ExecutionStatus result = moveit_cpp_ptr->execute(plan_solution1.trajectory, blocking, CONTROLLERS); */
}

计划＃1 可视化：

 开始下一个计划

visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
visual_tools.deleteAllMarkers();
visual_tools.trigger();

计划＃2

在这里我们将使用 moveit::core::RobotState 设置计划的当前状态

auto start_state = *(moveit_cpp_ptr->getCurrentState());
geometry_msgs::msg::Pose start_pose;
start_pose.orientation.w = 1.0;
start_pose.position.x = 0.55;
start_pose.position.y = 0.0;
start_pose.position.z = 0.6;start_state.setFromIK(joint_model_group_ptr, start_pose);planning_components->setStartState(start_state);

我们将重新使用我们原来的目标并为其制定计划。

auto plan_solution2 = planning_components->plan();
if (plan_solution2)
{moveit::core::RobotState robot_state(robot_model_ptr);moveit::core::robotStateMsgToRobotState(plan_solution2.start_state, robot_state);visual_tools.publishAxisLabeled(robot_state.getGlobalLinkTransform("panda_link8"), "start_pose");visual_tools.publishAxisLabeled(target_pose1.pose, "target_pose");visual_tools.publishText(text_pose, "moveit::core::RobotState_Start_State", rvt::WHITE, rvt::XLARGE);visual_tools.publishTrajectoryLine(plan_solution2.trajectory, joint_model_group_ptr);visual_tools.trigger();/* Uncomment if you want to execute the plan *//* bool blocking = true; *//* moveit_cpp_ptr->execute(plan_solution2.trajectory, blocking, CONTROLLERS); */
}

计划＃2可视化：

 开始下一个计划

visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
visual_tools.deleteAllMarkers();
visual_tools.trigger();

计划＃3

我们还可以使用 moveit::core::RobotState 设置计划的目标

auto target_state = *robot_start_state;
geometry_msgs::msg::Pose target_pose2;
target_pose2.orientation.w = 1.0;
target_pose2.position.x = 0.55;
target_pose2.position.y = -0.05;
target_pose2.position.z = 0.8;target_state.setFromIK(joint_model_group_ptr, target_pose2);planning_components->setGoal(target_state);

我们将重复利用以前的起点并以此为基础制定计划。

auto plan_solution3 = planning_components->plan();
if (plan_solution3)
{moveit::core::RobotState robot_state(robot_model_ptr);moveit::core::robotStateMsgToRobotState(plan_solution3.start_state, robot_state);visual_tools.publishAxisLabeled(robot_state.getGlobalLinkTransform("panda_link8"), "start_pose");visual_tools.publishAxisLabeled(target_pose2, "target_pose");visual_tools.publishText(text_pose, "moveit::core::RobotState_Goal_Pose", rvt::WHITE, rvt::XLARGE);visual_tools.publishTrajectoryLine(plan_solution3.trajectory, joint_model_group_ptr);visual_tools.trigger();/* Uncomment if you want to execute the plan *//* bool blocking = true; *//* moveit_cpp_ptr->execute(plan_solution3.trajectory, blocking, CONTROLLERS); */
}

计划＃3可视化：

开始下一个计划

visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
visual_tools.deleteAllMarkers();
visual_tools.trigger();

计划＃4

我们可以将计划的起始状态设置为机器人的当前状态。我们可以使用组状态的名称来设置计划的目标。对于熊猫机器人，我们有一个名为“panda_arm”的机器人状态计划组，名为“ready”，请参阅panda_arm.xacro

/* // Set the start state of the plan from a named robot state */
/* planning_components->setStartState("ready"); // Not implemented yet */

从命名的机器人状态设置计划的目标状态

planning_components->setGoal("ready");

我们将再次重复使用旧的起点并从中制定计划。

auto plan_solution4 = planning_components->plan();
if (plan_solution4)
{moveit::core::RobotState robot_state(robot_model_ptr);moveit::core::robotStateMsgToRobotState(plan_solution4.start_state, robot_state);visual_tools.publishAxisLabeled(robot_state.getGlobalLinkTransform("panda_link8"), "start_pose");visual_tools.publishAxisLabeled(robot_start_state->getGlobalLinkTransform("panda_link8"), "target_pose");visual_tools.publishText(text_pose, "Goal_Pose_From_Named_State", rvt::WHITE, rvt::XLARGE);visual_tools.publishTrajectoryLine(plan_solution4.trajectory, joint_model_group_ptr);visual_tools.trigger();/* Uncomment if you want to execute the plan *//* bool blocking = true; *//* moveit_cpp_ptr->execute(plan_solution4.trajectory, blocking, CONTROLLERS); */
}

计划#4 可视化：

 

开始下一个计划 

visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
visual_tools.deleteAllMarkers();
visual_tools.trigger();

计划＃5

我们还可以围绕碰撞场景中的物体生成运动计划。

首先我们创建碰撞对象

moveit_msgs::msg::CollisionObject collision_object;
collision_object.header.frame_id = "panda_link0";
collision_object.id = "box";shape_msgs::msg::SolidPrimitive box;
box.type = box.BOX;
box.dimensions = { 0.1, 0.4, 0.1 };geometry_msgs::msg::Pose box_pose;
box_pose.position.x = 0.4;
box_pose.position.y = 0.0;
box_pose.position.z = 1.0;collision_object.primitives.push_back(box);
collision_object.primitive_poses.push_back(box_pose);
collision_object.operation = collision_object.ADD;

将对象添加到规划场景

{  // Lock PlanningSceneplanning_scene_monitor::LockedPlanningSceneRW scene(moveit_cpp_ptr->getPlanningSceneMonitorNonConst());scene->processCollisionObjectMsg(collision_object);
}  // Unlock PlanningScene
planning_components->setStartStateToCurrentState();
planning_components->setGoal("extended");auto plan_solution5 = planning_components->plan();
if (plan_solution5)
{visual_tools.publishText(text_pose, "Planning_Around_Collision_Object", rvt::WHITE, rvt::XLARGE);visual_tools.publishTrajectoryLine(plan_solution5.trajectory, joint_model_group_ptr);visual_tools.trigger();/* Uncomment if you want to execute the plan *//* bool blocking = true; *//* moveit_cpp_ptr->execute(plan_solution5.trajectory, blocking, CONTROLLERS); */
}

计划＃5可视化：

 

启动文件

整个启动文件位于GitHub上。本教程中的所有代码都可以从MoveIt 设置中的moveit2_tutorials包中运行。

 

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

如若内容造成侵权/违法违规/事实不符，请联系我们进行投诉反馈，一经查实，立即删除！

相关文章

ZBLOG错误原因:数据库连接错误

ZBLOG错误原因:数据库连接错误

当您在使用 Z-Blog 时遇到“数据库连接错误”，这可能是由多种原因引起的。根据过去的经验和提供的信息，以下是一些可能的原因及解决方案：权限问题：确保数据库用户拥有足够的权限来连接数据库以及执行必要的读写操作。 检查数据库连接配置文件中的用户名和密码是否正确。文件…
阅读更多...
Z-blog上传文件时出现未知错误

Z-blog上传文件时出现未知错误

当在使用 Z-Blog 上传文件时遇到未知错误，可以尝试以下几个方面来诊断和解决问题：检查文件格式：确保上传的文件格式是 Z-Blog 支持的。常见的支持格式包括 JPEG、PNG 和 GIF。如果你上传的是其他格式的文件，可能就会导致上传失败。检查文件大小：Z-Blog 对上传文件的大小有…
阅读更多...
VMware vSphere 8.0 Update 3b 发布下载，新增功能概览

VMware vSphere 8.0 Update 3b 发布下载，新增功能概览

VMware vSphere 8.0 Update 3b 发布下载，新增功能概览VMware vSphere 8.0 Update 3b 发布下载，新增功能概览 vSphere 8.0U3 | ESXi 8.0U3 & vCenter Server 8.0U3 请访问原文链接：https://sysin.org/blog/vmware-vsphere-8-u3/，查看最新版。原创作品，转载请保留出处。…
阅读更多...
闯关提交04

闯关提交04

书生浦语大模型上海人工智能实验室推出的一系列开源AI大模型 卓越的推理能力和超长文本处理功能 包括轻量级、中量级和重量级三个类别，分别对应不同的应用场景和性能需求 支持高达一百万词元的文本输入 提供了全链路开源体系，包括数据、预训练、微调、部署和评测等环节Intern…
阅读更多...
VMware vCenter Server 8.0U3b 发布下载，新增功能概览

VMware vCenter Server 8.0U3b 发布下载，新增功能概览

VMware vCenter Server 8.0U3b 发布下载，新增功能概览VMware vCenter Server 8.0U3b 发布下载，新增功能概览 Server Management Software | vCenter 请访问原文链接：https://sysin.org/blog/vmware-vcenter-8-u3/，查看最新版。原创作品，转载请保留出处。 作者主页：sysin.…
阅读更多...
【日记】我太爱工作了，工装都发霉了（987 字）

【日记】我太爱工作了，工装都发霉了（987 字）

正文突然好想吃学校里的老麻抄手。晚上不知道为什么，忽然特别特别想吃。这附近好像从来没有那么麻辣好吃的抄手。最后还是去一家店里点了金汤的抄手。虽然店里有红汤，但感觉应该没有老麻抄手那种味道。正巧我也没有吃过酸辣口，就点了试一下。最后吃起来感觉还行。今天似乎实…
阅读更多...
VMware ESXi 8.0U3b 发布下载，新增功能概览

VMware ESXi 8.0U3b 发布下载，新增功能概览

VMware ESXi 8.0U3b 发布下载，新增功能概览VMware ESXi 8.0U3b 发布下载，新增功能概览 同步发布 Dell (戴尔)、HPE (慧与)、Lenovo (联想)、Inspur (浪潮)、Cisco (思科)、Hitachi (日立)、Fujitsu (富士通)、NEC (日电) 定制版 请访问原文链接：https://sysin.org/blog/vmwa…
阅读更多...
.net core8 使用JWT鉴权（附当前源码）

.net core8 使用JWT鉴权（附当前源码）

说明该文章是属于OverallAuth2.0系列文章，每周更新一篇该系列文章（从0到1完成系统开发）。该系统文章，我会尽量说的非常详细，做到不管新手、老手都能看懂。说明：OverallAuth2.0 是一个简单、易懂、功能强大的权限+可视化流程管理系统。 结合上一篇文章使用，味道更佳：.ne…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023

友情链接： 我的编程经验分享 一条长河网 尧图网站开发 尧图建网站 尧图建网站 广利置业