【MoveIt 2】直接通过 C++ API 使用 MoveIt : 运动规划管道

通过 C++ API 使用 MoveIt 运动规划管道
最新推荐文章于 2025-01-13 14:16:21 发布
原创 最新推荐文章于 2025-01-13 14:16:21 发布 · 1k 阅读 · 7 ·
CC 4.0 BY-SA版权
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
文章标签：

#c++ #开发语言

在 MoveIt 中，运动规划器被设置为规划路径。然而，有时我们可能希望对运动规划请求进行预处理或对规划的路径进行后处理（例如，进行时间参数化）。在这种情况下，我们使用规划管道，它将运动规划器与预处理和后处理阶段链接起来。预处理和后处理阶段称为规划请求适配器，可以通过 ROS 参数服务器按名称进行配置。在本教程中，我们将通过 C++ 代码向您介绍如何实例化和调用这样的规划管道。

 入门

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

 运行代码

打开两个 shell。在第一个 shell 中启动 RViz 并等待所有内容加载完成：

ros2 launch moveit2_tutorials move_group.launch.py

fbb08a28b9f21347b310bdbb3a2f4a1b.png

在第二个 shell 中，运行启动文件：

ros2 launch moveit2_tutorials motion_planning_pipeline_tutorial.launch.py

注意：本教程使用 RvizVisualToolsGui 面板逐步演示。要将此面板添加到 RViz，请按照可视化教程中的说明进行操作。

片刻之后，RViz 窗口应该会出现，并且看起来与此页面顶部的窗口类似。要通过每个演示步骤，请按屏幕底部的 RvizVisualToolsGui 面板中的 Next 按钮，或在屏幕顶部的 Tools 面板中选择 Key Tool，然后在 RViz 聚焦时按键盘上的 N 键。

预期输出

在 RViz 中，我们最终应该能够看到三个轨迹被重放：

  1. 机器人将其右臂移动到前方的目标姿势，

  2. 机器人将其右臂移动到侧面的关节目标，

  3. 机器人将其右臂移回到它前面的原始姿势目标，

整个代码

整个代码可以在 MoveIt GitHub 项目中看到。https://github.com/moveit/moveit2_tutorials/blob/main/doc/examples/motion_planning_pipeline

#include <pluginlib/class_loader.hpp> // 插件库类加载器


// MoveIt
#include <moveit/robot_model_loader/robot_model_loader.h> // 机器人模型加载器
#include <moveit/robot_state/conversions.h> // 机器人状态转换
#include <moveit/planning_pipeline/planning_pipeline.h> // 规划管道
#include <moveit/planning_interface/planning_interface.h> // 规划接口
#include <moveit/planning_scene_monitor/planning_scene_monitor.h> // 规划场景监视器
#include <moveit/kinematic_constraints/utils.h> // 运动学约束工具
#include <moveit_msgs/msg/display_trajectory.hpp> // 显示轨迹消息
#include <moveit_msgs/msg/planning_scene.hpp> // 规划场景消息
#include <moveit_visual_tools/moveit_visual_tools.h> // MoveIt可视化工具


static const rclcpp::Logger LOGGER = rclcpp::get_logger("motion_planning_pipeline"); // 日志记录器


int main(int argc, char** argv)
{
  rclcpp::init(argc, argv); // 初始化ROS
  rclcpp::NodeOptions node_options; // 节点选项
  node_options.automatically_declare_parameters_from_overrides(true); // 自动声明参数
  auto node = rclcpp::Node::make_shared("motion_planning_pipeline_tutorial", node_options); // 创建共享节点


  rclcpp::executors::SingleThreadedExecutor executor; // 单线程执行器
  executor.add_node(node); // 添加节点到执行器
  std::thread([&executor]() { executor.spin(); }).detach(); // 分离线程执行


  // BEGIN_TUTORIAL
  // 开始
  // ^^^^^
  // 设置使用规划管道非常简单。在加载规划器之前，我们需要两个对象，
  // 一个机器人模型和一个规划场景。
  //
  // 我们将通过实例化一个
  // :moveit_codedir:`RobotModelLoader<moveit_ros/planning/robot_model_loader/include/moveit/robot_model_loader/robot_model_loader.h>`
  // 对象，该对象将在ROS参数服务器上查找机器人描述并构建一个
  // :moveit_codedir:`RobotModel<moveit_core/robot_model/include/moveit/robot_model/robot_model.h>`
  // 供我们使用。
  robot_model_loader::RobotModelLoaderPtr robot_model_loader(
      new robot_model_loader::RobotModelLoader(node, "robot_description")); // 机器人模型加载器


  // 使用RobotModelLoader，我们可以构建一个规划场景监视器，
  // 它将创建一个规划场景，监视规划场景差异，并将差异应用到其内部规划场景。
  // 然后我们调用startSceneMonitor、startWorldGeometryMonitor和
  // startStateMonitor来完全初始化规划场景监视器
  planning_scene_monitor::PlanningSceneMonitorPtr psm(
      new planning_scene_monitor::PlanningSceneMonitor(node, robot_model_loader)); // 规划场景监视器


  /* 监听主题/XXX上的规划场景消息并将其应用到内部规划场景
                       相应地更新内部规划场景 */
  psm->startSceneMonitor(); // 启动场景监视器
  /* 监听世界几何、碰撞对象和（可选）八叉树地图的变化
                                世界几何、碰撞对象和可选的八叉树地图 */
  psm->startWorldGeometryMonitor(); // 启动世界几何监视器
  /* 监听关节状态更新以及附加碰撞对象的变化
                        并相应地更新内部规划场景 */
  psm->startStateMonitor(); // 启动状态监视器


  /* 我们还可以使用RobotModelLoader获取包含机器人运动学信息的机器人模型 */
  moveit::core::RobotModelPtr robot_model = robot_model_loader->getModel(); // 获取机器人模型


  /* 我们可以通过锁定内部规划场景来从PlanningSceneMonitor获取最新的机器人状态
     以进行读取。此锁定确保在我们读取其状态时，底层场景不会更新。
     RobotState在计算机器人的正向和逆向运动学等许多方面非常有用 */
  moveit::core::RobotStatePtr robot_state(
      new moveit::core::RobotState(planning_scene_monitor::LockedPlanningSceneRO(psm)->getCurrentState())); // 获取机器人状态


  /* 创建一个JointModelGroup来跟踪当前的机器人姿态和规划组。关节模型
     组在处理一组关节时非常有用，例如左臂或末端执行器 */
  const moveit::core::JointModelGroup* joint_model_group = robot_state->getJointModelGroup("panda_arm"); // 获取关节模型组


  // 我们现在可以设置PlanningPipeline对象，它将使用ROS参数服务器
  // 来确定请求适配器和规划插件的集合
  planning_pipeline::PlanningPipelinePtr planning_pipeline(
      new planning_pipeline::PlanningPipeline(robot_model, node, "ompl")); // 设置规划管道


  // 可视化
  // ^^^^^^^^^^^^^
  // MoveItVisualTools包提供了许多可视化对象、机器人和轨迹的功能，
  // 以及调试工具，例如逐步检查脚本。
  namespace rvt = rviz_visual_tools;
  moveit_visual_tools::MoveItVisualTools visual_tools(node, "panda_link0", "move_group_tutorial", psm); // MoveIt可视化工具
  visual_tools.deleteAllMarkers(); // 删除所有标记


  /* 远程控制是一个检查工具，允许用户通过按钮和键盘快捷键在RViz中逐步执行高级脚本 */
  visual_tools.loadRemoteControl(); // 加载远程控制


  /* RViz提供了多种类型的标记，在本演示中我们将使用文本、圆柱体和球体 */
  Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity(); // 文本位置
  text_pose.translation().z() = 1.75; // 设置文本位置
  visual_tools.publishText(text_pose, "Motion Planning Pipeline Demo", rvt::WHITE, rvt::XLARGE); // 发布文本


  /* 批量发布用于减少发送到RViz的大型可视化消息的数量 */
  visual_tools.trigger(); // 触发批量发布


  /* 我们还可以使用visual_tools等待用户输入 */
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo"); // 提示用户输入


  // 姿态目标
  // ^^^^^^^^^
  // 我们现在将为Panda的右臂创建一个运动计划请求
  // 指定末端执行器的期望姿态作为输入。
  planning_interface::MotionPlanRequest req; // 运动计划请求
  req.pipeline_id = "ompl"; // 管道ID
  req.planner_id = "RRTConnectkConfigDefault"; // 规划器ID
  req.allowed_planning_time = 1.0; // 允许的规划时间
  req.max_velocity_scaling_factor = 1.0; // 最大速度缩放因子
  req.max_acceleration_scaling_factor = 1.0; // 最大加速度缩放因子
  planning_interface::MotionPlanResponse res; // 运动计划响应
  geometry_msgs::msg::PoseStamped pose; // 姿态消息
  pose.header.frame_id = "panda_link0"; // 帧ID
  pose.pose.position.x = 0.3; // 位置X
  pose.pose.position.y = 0.0; // 位置Y
  pose.pose.position.z = 0.75; // 位置Z
  pose.pose.orientation.w = 1.0; // 方向W


  // 在位置上指定0.01米的公差
  // 在方向上指定0.01弧度的公差
  std::vector<double> tolerance_pose(3, 0.1); // 位置公差
  std::vector<double> tolerance_angle(3, 0.1); // 方向公差


  // 我们将使用一个助手函数将请求创建为约束
  // 该函数可从
  // :moveit_codedir:`kinematic_constraints<moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h>`
  // 包中获得。
  req.group_name = "panda_arm"; // 组名
  moveit_msgs::msg::Constraints pose_goal =
      kinematic_constraints::constructGoalConstraints("panda_link8", pose, tolerance_pose, tolerance_angle); // 构建目标约束
  req.goal_constraints.push_back(pose_goal); // 添加目标约束


  // 在规划之前，我们需要对规划场景进行只读锁定，以便在规划期间不会修改世界表示
  {
    planning_scene_monitor::LockedPlanningSceneRO lscene(psm); // 锁定规划场景
    /* 现在，调用管道并检查规划是否成功。*/
    /* 检查规划是否成功 */
    if (!planning_pipeline->generatePlan(lscene, req, res) || res.error_code.val != res.error_code.SUCCESS)
    {
      RCLCPP_ERROR(LOGGER, "Could not compute plan successfully"); // 规划失败
      rclcpp::shutdown(); // 关闭ROS
      return -1; // 返回错误代码
    }
  }
  // 可视化结果
  // ^^^^^^^^^^^^^^^^^^^^
  rclcpp::Publisher<moveit_msgs::msg::DisplayTrajectory>::SharedPtr display_publisher =
      node->create_publisher<moveit_msgs::msg::DisplayTrajectory>("/display_planned_path", 1); // 创建发布者
  moveit_msgs::msg::DisplayTrajectory display_trajectory; // 显示轨迹消息


  /* 可视化轨迹 */
  RCLCPP_INFO(LOGGER, "Visualizing the trajectory"); // 可视化轨迹
  moveit_msgs::msg::MotionPlanResponse response; // 运动计划响应
  res.getMessage(response); // 获取响应消息


  display_trajectory.trajectory_start = response.trajectory_start; // 设置轨迹起点
  display_trajectory.trajectory.push_back(response.trajectory); // 添加轨迹
  display_publisher->publish(display_trajectory); // 发布轨迹
  visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group); // 发布轨迹线
  visual_tools.trigger(); // 触发可视化


  /* 等待用户输入 */
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); // 提示用户输入


  // 关节空间目标
  // ^^^^^^^^^^^^^^^^^
  /* 首先，将规划场景中的状态设置为上一个计划的最终状态 */
  robot_state = planning_scene_monitor::LockedPlanningSceneRO(psm)->getCurrentStateUpdated(response.trajectory_start); // 更新机器人状态
  robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions); // 设置关节组位置
  moveit::core::robotStateToRobotStateMsg(*robot_state, req.start_state); // 转换机器人状态消息


  // 现在，设置一个关节空间目标
  moveit::core::RobotState goal_state(*robot_state); // 目标状态
  std::vector<double> joint_values = { -1.0, 0.7, 0.7, -1.5, -0.7, 2.0, 0.0 }; // 关节值
  goal_state.setJointGroupPositions(joint_model_group, joint_values); // 设置关节组位置
  moveit_msgs::msg::Constraints joint_goal =
      kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group); // 构建关节目标约束


  req.goal_constraints.clear(); // 清除目标约束
  req.goal_constraints.push_back(joint_goal); // 添加关节目标约束


  // 在规划之前，我们需要对规划场景进行只读锁定，以便在规划期间不会修改世界表示
  {
    planning_scene_monitor::LockedPlanningSceneRO lscene(psm); // 锁定规划场景
    /* 现在，调用管道并检查规划是否成功。*/
    if (!planning_pipeline->generatePlan(lscene, req, res) || res.error_code.val != res.error_code.SUCCESS)
    {
      RCLCPP_ERROR(LOGGER, "Could not compute plan successfully"); // 规划失败
      rclcpp::shutdown(); // 关闭ROS
      return -1; // 返回错误代码
    }
  }
  /* 可视化轨迹 */
  RCLCPP_INFO(LOGGER, "Visualizing the trajectory"); // 可视化轨迹
  res.getMessage(response); // 获取响应消息
  display_trajectory.trajectory_start = response.trajectory_start; // 设置轨迹起点
  display_trajectory.trajectory.push_back(response.trajectory); // 添加轨迹
  // 现在你应该看到两个连续的规划轨迹
  display_publisher->publish(display_trajectory); // 发布轨迹
  visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group); // 发布轨迹线
  visual_tools.trigger(); // 触发可视化


  /* 等待用户输入 */
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); // 提示用户输入


  // 使用规划请求适配器
  // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  // 规划请求适配器允许我们指定一系列操作，
  // 这些操作应在规划发生之前或规划完成后对结果路径进行处理


  /* 首先，将规划场景中的状态设置为上一个计划的最终状态 */
  robot_state = planning_scene_monitor::LockedPlanningSceneRO(psm)->getCurrentStateUpdated(response.trajectory_start); // 更新机器人状态
  robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions); // 设置关节组位置
  moveit::core::robotStateToRobotStateMsg(*robot_state, req.start_state); // 转换机器人状态消息


  // 现在，将其中一个关节稍微设置在其上限之外
  const moveit::core::JointModel* joint_model = joint_model_group->getJointModel("panda_joint3"); // 获取关节模型
  const moveit::core::JointModel::Bounds& joint_bounds = joint_model->getVariableBounds(); // 获取关节边界
  std::vector<double> tmp_values(1, 0.0); // 临时值
  tmp_values[0] = joint_bounds[0].min_position_ - 0.01; // 设置关节位置
  robot_state->setJointPositions(joint_model, tmp_values); // 设置关节位置


  req.goal_constraints.clear(); // 清除目标约束
  req.goal_constraints.push_back(pose_goal); // 添加姿态目标约束


  // 在规划之前，我们需要对规划场景进行只读锁定，以便在规划期间不会修改世界表示
  {
    planning_scene_monitor::LockedPlanningSceneRO lscene(psm); // 锁定规划场景
    /* 现在，调用管道并检查规划是否成功。*/
    if (!planning_pipeline->generatePlan(lscene, req, res) || res.error_code.val != res.error_code.SUCCESS)
    {
      RCLCPP_ERROR(LOGGER, "Could not compute plan successfully"); // 规划失败
      rclcpp::shutdown(); // 关闭ROS
      return -1; // 返回错误代码
    }
  }
  /* 可视化轨迹 */
  RCLCPP_INFO(LOGGER, "Visualizing the trajectory"); // 可视化轨迹
  res.getMessage(response); // 获取响应消息
  display_trajectory.trajectory_start = response.trajectory_start; // 设置轨迹起点
  display_trajectory.trajectory.push_back(response.trajectory); // 添加轨迹
  /* 现在你应该看到三个连续的规划轨迹 */
  display_publisher->publish(display_trajectory); // 发布轨迹
  visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group); // 发布轨迹线
  visual_tools.trigger(); // 触发可视化


  /* 等待用户输入 */
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to finish the demo"); // 提示用户输入


  RCLCPP_INFO(LOGGER, "Done"); // 完成


  rclcpp::shutdown(); // 关闭ROS
  return 0; // 返回成功代码
}

开始

设置开始使用规划管道非常容易。在我们加载规划器之前，我们需要两个对象，一个 RobotModel 和一个 PlanningScene。

我们将首先实例化一个 RobotModelLoader 对象，它将在 ROS 参数服务器上查找机器人描述并为我们构建一个 RobotModel 以供使用。

robot_model_loader::RobotModelLoaderPtr robot_model_loader(
      new robot_model_loader::RobotModelLoader(node, "robot_description")); // 机器人模型加载器

使用 RobotModelLoader，我们可以构建一个规划场景监视器，该监视器将创建一个规划场景，监视规划场景差异，并将差异应用于其内部规划场景。然后我们调用 startSceneMonitor、startWorldGeometryMonitor 和 startStateMonitor 来完全初始化规划场景监视器。

planning_scene_monitor::PlanningSceneMonitorPtr psm(
      new planning_scene_monitor::PlanningSceneMonitor(node, robot_model_loader)); // 规划场景监视器


  /* 监听主题/XXX上的规划场景消息并将其应用到内部规划场景
                       相应地更新内部规划场景 */
  psm->startSceneMonitor(); // 启动场景监视器
  /* 监听世界几何、碰撞对象和（可选）八叉树地图的变化
                                世界几何、碰撞对象和可选的八叉树地图 */
  psm->startWorldGeometryMonitor(); // 启动世界几何监视器
  /* 监听关节状态更新以及附加碰撞对象的变化
                        并相应地更新内部规划场景 */
  psm->startStateMonitor(); // 启动状态监视器


  /* 我们还可以使用RobotModelLoader获取包含机器人运动学信息的机器人模型 */
  moveit::core::RobotModelPtr robot_model = robot_model_loader->getModel(); // 获取机器人模型


  /* 我们可以通过锁定内部规划场景来从PlanningSceneMonitor获取最新的机器人状态
     以进行读取。此锁定确保在我们读取其状态时，底层场景不会更新。
     RobotState在计算机器人的正向和逆向运动学等许多方面非常有用 */
  moveit::core::RobotStatePtr robot_state(
      new moveit::core::RobotState(planning_scene_monitor::LockedPlanningSceneRO(psm)->getCurrentState())); // 获取机器人状态


  /* 创建一个JointModelGroup来跟踪当前的机器人姿态和规划组。关节模型
     组在处理一组关节时非常有用，例如左臂或末端执行器 */
  const moveit::core::JointModelGroup* joint_model_group = robot_state->getJointModelGroup("panda_arm"); // 获取关节模型组

我们现在可以设置 PlanningPipeline 对象，它将使用 ROS 参数服务器来确定请求适配器和规划插件的集合

planning_pipeline::PlanningPipelinePtr planning_pipeline(
      new planning_pipeline::PlanningPipeline(robot_model, node, "ompl")); // 设置规划管道

 可视化 

该软件包 MoveItVisualTools 提供了许多在 RViz 中可视化对象、机器人和轨迹的功能，以及诸如脚本逐步内省等调试工具。

namespace rvt = rviz_visual_tools;
  moveit_visual_tools::MoveItVisualTools visual_tools(node, "panda_link0", "move_group_tutorial", psm); // MoveIt可视化工具
  visual_tools.deleteAllMarkers(); // 删除所有标记


  /* 远程控制是一个检查工具，允许用户通过按钮和键盘快捷键在RViz中逐步执行高级脚本 */
  visual_tools.loadRemoteControl(); // 加载远程控制


  /* RViz提供了多种类型的标记，在本演示中我们将使用文本、圆柱体和球体 */
  Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity(); // 文本位置
  text_pose.translation().z() = 1.75; // 设置文本位置
  visual_tools.publishText(text_pose, "Motion Planning Pipeline Demo", rvt::WHITE, rvt::XLARGE); // 发布文本


  /* 批量发布用于减少发送到RViz的大型可视化消息的数量 */
  visual_tools.trigger(); // 触发批量发布


  /* 我们还可以使用visual_tools等待用户输入 */
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo"); // 提示用户输入

姿势目标 

我们现在将为熊猫的右臂创建一个运动计划请求，指定末端执行器的期望姿态作为输入。

planning_interface::MotionPlanRequest req; // 运动计划请求
  req.pipeline_id = "ompl"; // 管道ID
  req.planner_id = "RRTConnectkConfigDefault"; // 规划器ID
  req.allowed_planning_time = 1.0; // 允许的规划时间
  req.max_velocity_scaling_factor = 1.0; // 最大速度缩放因子
  req.max_acceleration_scaling_factor = 1.0; // 最大加速度缩放因子
  planning_interface::MotionPlanResponse res; // 运动计划响应
  geometry_msgs::msg::PoseStamped pose; // 姿态消息
  pose.header.frame_id = "panda_link0"; // 帧ID
  pose.pose.position.x = 0.3; // 位置X
  pose.pose.position.y = 0.0; // 位置Y
  pose.pose.position.z = 0.75; // 位置Z
  pose.pose.orientation.w = 1.0; // 方向W

位置公差为 0.01 米，方向公差为 0.01 弧度。

std::vector<double> tolerance_pose(3, 0.1); // 位置公差
  std::vector<double> tolerance_angle(3, 0.1); // 方向公差

我们将使用 kinematic_constraints 包中可用的辅助函数将请求创建为约束。

req.group_name = "panda_arm"; // 组名
  moveit_msgs::msg::Constraints pose_goal =
      kinematic_constraints::constructGoalConstraints("panda_link8", pose, tolerance_pose, tolerance_angle); // 构建目标约束
  req.goal_constraints.push_back(pose_goal); // 添加目标约束

在规划之前，我们需要在规划场景上设置一个只读锁，以便在规划时不会修改世界表示

{
    planning_scene_monitor::LockedPlanningSceneRO lscene(psm); // 锁定规划场景
    /* 现在，调用管道并检查规划是否成功。 */
    /* 检查规划是否成功 */
    if (!planning_pipeline->generatePlan(lscene, req, res) || res.error_code.val != res.error_code.SUCCESS)
    {
      RCLCPP_ERROR(LOGGER, "Could not compute plan successfully"); // 规划失败
      rclcpp::shutdown(); // 关闭ROS
      return -1; // 返回错误代码
    }
  }

可视化结果 

ce30503d85c56f5399c0b72f45e940d0.png

rclcpp::Publisher<moveit_msgs::msg::DisplayTrajectory>::SharedPtr display_publisher =
      node->create_publisher<moveit_msgs::msg::DisplayTrajectory>("/display_planned_path", 1); // 创建发布者
  moveit_msgs::msg::DisplayTrajectory display_trajectory; // 显示轨迹消息


  /* 可视化轨迹 */
  RCLCPP_INFO(LOGGER, "Visualizing the trajectory"); // 可视化轨迹
  moveit_msgs::msg::MotionPlanResponse response; // 运动计划响应
  res.getMessage(response); // 获取响应消息


  display_trajectory.trajectory_start = response.trajectory_start; // 设置轨迹起点
  display_trajectory.trajectory.push_back(response.trajectory); // 添加轨迹
  display_publisher->publish(display_trajectory); // 发布轨迹
  visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group); // 发布轨迹线
  visual_tools.trigger(); // 触发可视化


  /* 等待用户输入 */
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); // 提示用户输入

 关节空间目标 

robot_state = planning_scene_monitor::LockedPlanningSceneRO(psm)->getCurrentStateUpdated(response.trajectory_start); // 更新机器人状态
  robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions); // 设置关节组位置
  moveit::core::robotStateToRobotStateMsg(*robot_state, req.start_state); // 转换机器人状态消息

现在，设定一个关节空间目标

moveit::core::RobotState goal_state(*robot_state); // 目标状态
  std::vector<double> joint_values = { -1.0, 0.7, 0.7, -1.5, -0.7, 2.0, 0.0 }; // 关节值
  goal_state.setJointGroupPositions(joint_model_group, joint_values); // 设置关节组位置
  moveit_msgs::msg::Constraints joint_goal =
      kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group); // 构建关节目标约束


  req.goal_constraints.clear(); // 清除目标约束
  req.goal_constraints.push_back(joint_goal); // 添加关节目标约束

在规划之前，我们需要在规划场景上设置一个只读锁，以便在规划时不会修改世界表示

{
    planning_scene_monitor::LockedPlanningSceneRO lscene(psm); // 锁定规划场景
    /* 现在，调用管道并检查规划是否成功。 */
    if (!planning_pipeline->generatePlan(lscene, req, res) || res.error_code.val != res.error_code.SUCCESS)
    {
      RCLCPP_ERROR(LOGGER, "Could not compute plan successfully"); // 规划失败
      rclcpp::shutdown(); // 关闭ROS
      return -1; // 返回错误代码
    }
  }
  /* 可视化轨迹 */
  RCLCPP_INFO(LOGGER, "Visualizing the trajectory"); // 可视化轨迹
  res.getMessage(response); // 获取响应消息
  display_trajectory.trajectory_start = response.trajectory_start; // 设置轨迹起点
  display_trajectory.trajectory.push_back(response.trajectory); // 添加轨迹

现在你应该看到两个连续的规划轨迹

ccc7277b6ffedc34b0bfe1b3f223cdfe.png

display_publisher->publish(display_trajectory); // 发布轨迹
  visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group); // 发布轨迹线
  visual_tools.trigger(); // 触发可视化


  /* 等待用户输入 */
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); // 提示用户输入

使用规划请求适配器 

规划请求适配器允许我们指定一系列操作，这些操作应在规划发生之前或规划完成后对结果路径进行处理

/* 首先，将规划场景中的状态设置为上一个规划的最终状态 */
  robot_state = planning_scene_monitor::LockedPlanningSceneRO(psm)->getCurrentStateUpdated(response.trajectory_start); // 更新机器人状态
  robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions); // 设置关节组位置
  moveit::core::robotStateToRobotStateMsg(*robot_state, req.start_state); // 转换机器人状态消息

现在，将其中一个关节设置在其上限之外一点点

const moveit::core::JointModel* joint_model = joint_model_group->getJointModel("panda_joint3"); // 获取关节模型
  const moveit::core::JointModel::Bounds& joint_bounds = joint_model->getVariableBounds(); // 获取关节边界
  std::vector<double> tmp_values(1, 0.0); // 临时值
  tmp_values[0] = joint_bounds[0].min_position_ - 0.01; // 设置关节位置
  robot_state->setJointPositions(joint_model, tmp_values); // 设置关节位置


  req.goal_constraints.clear(); // 清除目标约束
  req.goal_constraints.push_back(pose_goal); // 添加姿态目标约束

在规划之前，我们需要在规划场景上设置一个只读锁，以便在规划时不会修改世界表示

// 在规划之前，我们需要对规划场景进行只读锁定，以便在规划期间不会修改世界表示
  {
    planning_scene_monitor::LockedPlanningSceneRO lscene(psm); // 锁定规划场景
    /* 现在，调用管道并检查规划是否成功。 */
    if (!planning_pipeline->generatePlan(lscene, req, res) || res.error_code.val != res.error_code.SUCCESS)
    {
      RCLCPP_ERROR(LOGGER, "Could not compute plan successfully"); // 规划失败
      rclcpp::shutdown(); // 关闭ROS
      return -1; // 返回错误代码
    }
  }
  /* 可视化轨迹 */
  RCLCPP_INFO(LOGGER, "Visualizing the trajectory"); // 可视化轨迹
  res.getMessage(response); // 获取响应消息
  display_trajectory.trajectory_start = response.trajectory_start; // 设置轨迹起点
  display_trajectory.trajectory.push_back(response.trajectory); // 添加轨迹
  /* 现在你应该看到三个连续的规划轨迹 */
  display_publisher->publish(display_trajectory); // 发布轨迹
  visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group); // 发布轨迹线
  visual_tools.trigger(); // 触发可视化


  /* 等待用户输入 */
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to finish the demo"); // 提示用户输入


  RCLCPP_INFO(LOGGER, "Done"); // 完成


  rclcpp::shutdown(); // 关闭ROS
  return 0; // 返回成功代码
}

3dbdabc6bf9d6fc7e3ff6cb69b64e5f5.png

启动文件 

整个启动文件在 GitHub 上。 本教程中的所有代码都可以从您作为 MoveIt 设置一部分的 moveit_tutorials 包中编译和运行。https://github.com/moveit/moveit2_tutorials/blob/main/doc/examples/motion_planning_pipeline/launch/motion_planning_pipeline_tutorial.launch.py 

import os  # 导入os模块
import yaml  # 导入yaml模块
from launch import LaunchDescription  # 从launch模块导入LaunchDescription类
from launch_ros.actions import Node  # 从launch_ros.actions模块导入Node类
from ament_index_python.packages import get_package_share_directory  # 从ament_index_python.packages模块导入get_package_share_directory函数
from moveit_configs_utils import MoveItConfigsBuilder  # 从moveit_configs_utils模块导入MoveItConfigsBuilder类




def load_file(package_name, file_path):  # 定义load_file函数，参数为package_name和file_path
    package_path = get_package_share_directory(package_name)  # 获取包的共享目录路径
    absolute_file_path = os.path.join(package_path, file_path)  # 拼接得到文件的绝对路径


    try:
        with open(absolute_file_path, "r") as file:  # 尝试以只读模式打开文件
            return file.read()  # 读取文件内容并返回
    except EnvironmentError:  # 捕获环境错误（包括IOError、OSError和WindowsError）
        return None  # 返回None




def load_yaml(package_name, file_path):  # 定义load_yaml函数，参数为package_name和file_path
    package_path = get_package_share_directory(package_name)  # 获取包的共享目录路径
    absolute_file_path = os.path.join(package_path, file_path)  # 拼接得到文件的绝对路径


    try:
        with open(absolute_file_path, "r") as file:  # 尝试以只读模式打开文件
            return yaml.safe_load(file)  # 安全地加载yaml文件内容并返回
    except EnvironmentError:  # 捕获环境错误（包括IOError、OSError和WindowsError）
        return None  # 返回None




def generate_launch_description():  # 定义generate_launch_description函数


    moveit_config = (
        MoveItConfigsBuilder("moveit_resources_panda")  # 创建MoveIt配置构建器对象
        .robot_description(file_path="config/panda.urdf.xacro")  # 设置机器人描述文件路径
        .planning_scene_monitor(  # 设置规划场景监视器
            publish_robot_description=True, publish_robot_description_semantic=True
        )
        .planning_pipelines("ompl", ["ompl"])  # 设置规划管道
        .to_moveit_configs()  # 转换为MoveIt配置
    )


    # 运动规划管道演示可执行文件
    motion_planning_pipeline_demo = Node(
        name="motion_planning_pipeline_tutorial",  # 节点名称
        package="moveit2_tutorials",  # 包名称
        executable="motion_planning_pipeline_tutorial",  # 可执行文件名称
        output="screen",  # 输出到屏幕
        parameters=[
            moveit_config.robot_description,  # 机器人描述参数
            moveit_config.robot_description_semantic,  # 机器人语义描述参数
            moveit_config.robot_description_kinematics,  # 机器人运动学描述参数
            moveit_config.planning_pipelines,  # 规划管道参数
            moveit_config.joint_limits,  # 关节限制参数
        ],
    )


    return LaunchDescription([motion_planning_pipeline_demo])  # 返回LaunchDescription对象
ros2_control 使用教程（一）
机器人控制与仿真应用
06-26 3993
包含硬件接口的头文件并添加命名空间定义，以简化进一步开发。实现 on_init 方法。在此，应初始化所有成员变量并处理 info 参数中的参数。在第一行中，通常会调用父代 on_init 来处理标准值，如 name。具体方法是：hardware_interface::(Actuator|Sensor|System)Interface::on_init(info)。
MoveIt2-humble】入门教程----第一个 C++ MoveIt 程序
ZPC8210的博客
10-07 1307
四节教程会手把手带你写一个完整的 Moveit 控制程序，包括轨迹规划、RViz可视化、添加碰撞物体、抓取和放置。
【ROS2】实操Movei2！详解：panda机械臂规避障碍物 + 物体抓取和放置
guilin
01-13 3924
如何使用Moveit2！（最新）对panda机械臂进行路径规划，实操规避障碍物、物体的抓取和放置。含详细解析。
在melodic中自定义路径规划算法（个人记录）
m0_52010566的博客
06-09 1318
sudo catkin build(编译有警告，我没管，考虑可能是因为cmake版本更新的问题，因为更新前的那次编译没有警告)sudo apt-get install python-rosdep(官网有这个命令。问题：cmake可能无法识别某些指令，需升级cmake（参考。sudo make install（不确定需不需要）roslaunch 你的包名 你的launch文件。
【MoveIt 2】围绕对象进行规划
cxyhjl的博客
08-01 777
本教程将向您介绍如何将对象插入到规划场景中并围绕它们进行规划。先决条件如果您还没有完成，请确保您已经完成了在 RViz 中可视化的步骤。该项目假设您从上一个教程结束的hello_moveit项目开始。如果您只想运行教程，可以按照 Docker 指南启动一个包含完整教程的容器。步骤1 添加包含用于规划场景接口在你的源文件顶部，将此添加到包含列表中：#include <moveit...
Moveit2极简入门（1）
weixin_42265404的博客
06-14 2249
MoveIt 2安装（ROS2-galactic）了解ROS2基本框架后就要进入具体问题的学习中了，从今天开始争取一周了解Moveit2的基本用法。MoveIt2功能 二、使用步骤 1.升级源软件 2.安装功能包 有时会缺少controller包 sudo apt-get install ros-galactic-controller-manager 3.增加工作空间 4.下载Moveit2源码并安装依赖 5.设置环境变量 总结 Moveit2安装完毕...............
【亲测免费】 MoveIt2 笛卡尔路径规划：速度约束的精细控制
gitblog_09812的博客
10-31 577
MoveIt2 笛卡尔路径规划：速度约束的精细控制 【下载地址】MoveIt2笛卡尔路径规划添加速度约束的代码 该资源文件的主要内容是MoveIt2中的笛卡尔路径规划代码，并且在此基础上添加了速度约束。通过使用该代码，用户可以在MoveIt2中实现更加精细的路径规划控制，确保机械臂在执行任务时能够满足特定的速度要求 ...
四、使用MoveGroup C++接口——案例代码分解（八）
weixin_56773716的博客
08-21 1047
,
moveit实时轨迹规划
最新发布
06-14
-MoveItC++API：如引用[2]提到的，通过API可以访问轨迹数据并进行修改。实现实时轨迹规划的代码框架示例（基于C++）：```cpp#include<moveit/move_group_interface/move_group_interface.h>#include<moveit/planning...
moveit接口
03-15
其核心组件之一是 Motion Planning Pipeline，该管道允许开发者通过 C++ 或 Python 编程语言实现复杂的机器人操作任务。 在 `motion_planning_pipeline_tutorial.cpp` 文件中展示了如何配置和执行基本的运动规划...
机器人导航与操作：从导航栈到MoveIt!应用
# 机器人导航与操作：从导航栈到MoveIt!应用 ## 1. 导航栈可视化 导航栈是机器人自主导航的核心组件，在rviz中，有多种可视化元素能帮助我们了解导航栈的工作状态： | 可视化元素 | 主题 | 类型 | 说明 | | --- | -...
【MoveIt 2】入门-安装
cxyhjl的博客
07-30 1611
入门在这里，我们将设置您的环境以最佳运行教程。这将创建一个 colcon 工作区，下载所有最新的 MoveIt 源代码，并从源代码构建所有内容，以确保您拥有最新的修复和改进。构建所有 MoveIt 的源代码可能需要 20-30 分钟，具体取决于您计算机的 CPU 速度和可用 RAM。如果您的系统性能较低，或者只是想更快地开始，请查看我们的 Docker 指南。安装 ROS 2 和 colcon...
【MoveIt 2】直接通过 C++ API 使用 MoveIt : 规划场景 ROS API
cxyhjl的博客
08-02 694
在本教程中，我们将研究使用计划场景差异来执行两个操作：将对象添加和移除到世界中将物体附加到机器人和从机器人分离入门如果您还没有这样做，请确保您已完成入门中的步骤。运行代码打开两个 shell。在第一个 shell 中启动 RViz 并等待所有内容加载完成：ros2 launch moveit2_tutorials move_group.launch.py在第二个 shell 中，运行此演示...
MoveIt2——2.MoveIt在RViz中的快速入门
Avada_C_L的博客
07-11 3424
MoveIt2官方教程翻译
【MoveIt 2】在 RViz 中 MoveIt 快速入门
cxyhjl的博客
07-31 1991
本教程将教您如何使用 RViz 和 MoveIt Display 插件在 MoveIt 中创建运动规划。Rviz 是 ROS 中的主要可视化工具，是调试机器人非常有用的工具。MoveIt Display 插件允许您设置虚拟环境（规划场景），交互式地为机器人创建起始和目标状态，测试各种运动规划器，并可视化输出。让我们开始吧！入门如果您还没有这样做，请确保您已完成入门或我们的 Docker 指南中...
【MoveIt 2】您的第一个 C++ MoveIt 项目
cxyhjl的博客
07-31 1094
您的第一个 C++ MoveIt 项目本教程将指导你编写第一个使用 MoveIt 的 C++ 应用程序。警告：由于需要额外的参数才能实现完整的 Move Group 功能，MoveIt 中的大多数功能将无法正常工作。要进行完整设置，请继续学习 Move Group C++ 接口教程。先决条件如果您还没有这样做，请确保您已完成入门中的步骤。本教程假设您了解 ROS 2 的基础知识。为了为此做好准备...
MoveIt! 学习笔记7-MotionPlanningPipeline-规划请求适配器
HarwardWu的博客
12-29 1200
此博文主要是用来记录ROS-Kinetic 中，用于机器人轨迹规划的MoveIt功能包的学习记录。 英文原版教程见此链接：http://docs.ros.org/en/kinetic/api/moveit_tutorials/html/doc/move_group_interface/move_group_interface_tutorial.html 引： Motion Planning Pipeline的背景介绍： 在MoveIt！这个工具箱中，不同的motion planne...
【附带源码】机械臂MoveIt2极简教程（一）、moveit2安装
押波张飞的博客
06-05 7946
利用ros2中的moveit2框架可以快速开发机械臂应用，使开发者更专注于具体业务实现
Moveit中Motion Planning Pipeline的理解
lyh458的博客
06-05 679
Moveit中Motion Planning Pipeline的理解
评论 3
成就一亿技术人!
拼手气红包6.0元
还能输入1000个字符
 
红包 添加红包
表情包 插入表情
表情包 代码片
 条评论被折叠 查看
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值