首页 > 其他分享 >Ros2 - Moveit2 - MoveItCpp

Ros2 - Moveit2 - MoveItCpp

时间：2024-09-18 12:13:06浏览次数：8  
标签：moveit MoveItCpp pose ptr visual plan tools Moveit2 Ros2

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 PlanningScene
  planning_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包中运行。

 

标签：moveit,MoveItCpp,pose,ptr,visual,plan,tools,Moveit2,Ros2
From： https://www.cnblogs.com/ai-ldj/p/18418224

相关文章

  • Ros2- Moveit2- Subrame( 子坐标 )
    子坐标是在CollisionObjects上定义的坐标。它们可用于定义您放置在场景中的对象上的兴趣点，例如瓶子的开口、螺丝刀的尖端或螺丝的头部。它们可用于规划和编写机器人指令，例如“拿起瓶子，然后将开口移到水龙头的喷口下方”，或“拿起螺丝刀，然后将其放在螺丝头上方”。编写专注于机器......
  • Ros2 - Moveit2 - DeepGrasp(深度抓握)
    MoveIt深度把握本教程演示了如何在MoveIt任务构造器中使用抓握姿势检测(GPD)和 Dex-Net。GPD（左）和Dex-Net（右）用于生成拾取圆柱体的抓取姿势。https://moveit.picknik.ai/main/_images/mtc_gpd_panda.gif 入门如果您还没有这样做，请确保您已经完成入门指南中的步骤。......
  • ROS2图形化方式新建功能包工具- Turtle Nest
    提示：全文AI生成。链接：https://github.com/Jannkar/turtle_nest配置TurtleNest使用说明（中文版）一、TurtleNest简介正如海龟巢是幼海龟的诞生地，ROS2TurtleNest是ROS2包的诞生和成长之地。TurtleNest提供了一个易用的图形用户界面（GUI），简化了ROS2包的创建......
  • Ros2 - Moveit2 - Grasps（抓握）
    MoveItGrasps是一款用于抓取块或圆柱体等物体的抓取生成器，可用作MoveIt拾取和放置管道的替代品。MoveItGrasps提供基于可达性和接近、抬起和后退运动的笛卡尔规划来过滤抓取的功能。抓握生成算法基于简单的长方体形状，不考虑摩擦锥或其他抓握动力。MoveItGrasps可与平行......
  • Ros2 - Moveit2 - Pick And Place(拾取和放置)
    拾取和放置注意：本教程中使用的功能已弃用。要执行拾取和放置操作，应使用MoveIt任务构造器(MTC)（使用MoveIt任务构造器拾取和放置）。 在MoveIt中，抓取是使用MoveGroup接口完成的。为了抓取一个物体，我们需要创建moveit_msgs::Graspmsg，以便定义抓取操作中涉及的各种姿势和姿......
  • ROS2 - Moveit2 - Planning with Approximated Constraint Manifolds(使用近似约束流
    使用近似约束流形进行规划OMPL支持自定义约束，以使规划轨迹遵循所需的行为。约束可以在关节空间和笛卡尔空间中定义，后者基于方向或位置。在规划轨迹时，每个关节状态都需要遵循所有设置的约束，默认情况下，这是通过拒绝采样来执行的。然而，这可能会导致非常长的规划时间，特别是当约束非......
  • Ros2 - Moveit2 - TimeParameter(时间参数化)
    时间参数化MoveIt目前主要是一个运动规划框架-它规划关节或末端执行器的位置，但不规划速度或加速度。但是，MoveIt确实利用后处理来对速度和加速度值的运动轨迹进行时间参数化。下面我们将解释MoveIt这一部分所涉及的设置和组件。 速度控制来自文件默认情况下，MoveIt将关......
  • ROS2开发前置基础知识
    前言笔者发现现在市面上（主要是某站上）的ros2相关的教程内容大多不太基础（例如古某居（手动狗头保命），对于那些不熟悉Linux和C++甚至Python的童鞋来说不太友好，之前笔者自己在跟着学习过程中也有不少疑惑，踩了不少坑，现进行一些前置知识的总结（主要学习古某居和鱼香ros的教程，墙裂推荐），一......
  • Ros2- Moveit2 - Visualizing Collisions(可视化碰撞)
    本节将引导您了解C++示例代码，该代码可让您在RViz中移动和与机器人手臂交互时可视化机器人本身与世界之间的碰撞接触点。入门运行代码使用Roslaunch启动文件直接从moveit_tutorials运行代码：roslaunchmoveit_tutorialsvisualizing_collisions_tutorial.launch现在......
  • ROS2 - Moveit2 - 创建Moveit插件(MoveIt Plugins)
    创建MoveIt插件本节详细说明了如何在ROS中添加插件。两个必需元素是基类和插件类。插件类继承自基类并覆盖其虚拟函数。用于此目的的主要库是pluginlib。本教程包含三种不同类型的插件，即运动规划器、控制器管理器和约束采样器。运动规划器插件在本节中，我们将展示如何将新......

赞助商

阅读排行

网站主页关于我们联系我们网站地图

本网站内容转载自其他媒体，侵权联系[admin##ips99.com]。

Copyright © 2020-2023 IPS99 版权所有 IPS99