运动规划管道 Motion Planning Pipeline¶

在 MoveIt 中，运动规划器 motion planner 用于规划路径。然而，我们经常需要对运动规划请求进行预处理或后处理(例如用于时间参数化)。在这种情况下，我们使用规划管道 planning pipeline ，其将一个运动规划器与预处理和后处理阶段连接起来。所谓的前处理和后处理阶段（称为规划请求适配器 planning request adapter ），可以在 ROS 的参数服务器上按名称配置。在本教程中，我们将让您运行 c++ 代码来实例化和调用这样的 Planning Pipeline 。

开始¶

请先确保已经完成了入门里的步骤。

运行代码¶

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

roslaunch panda_moveit_config demo.launch

在第二个 shell 里，运行 launch 文件：

roslaunch moveit_tutorials motion_planning_pipeline_tutorial.launch

注意: 本教程使用 RvizVisualToolsGui 面板来逐步完成演示。要将此面板添加到 RViz ，请参考可视化教程。

RViz 窗口过一会儿就会出现，看起来和本页面顶部那张截图差不多。想要依次查看每个演示步骤，要么按下窗口底部 RvizVisualToolsGui 面板里的 Next 按钮，或者在 RViz 窗口聚焦状态下，选择窗口顶部 Tools 面板下的 Key Tool ，然后按下键盘上的 N 。

预期效果¶

在 RViz 里，我们最终应该能看到以下三个效果：

机器人移动它的右臂到其前方的一个目标位姿处，

机器人移动它的右臂到其一边的一个目标关节位置处，

机器人移动它的右臂到其前方的初始位姿处。

整个代码¶

全部代码可以在 MoveIt GitHub project 里找到。

开始¶

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

我们将从实例化一个 RobotModelLoader 对象开始，该对象将在 ROS 参数服务器上查找 robot description ，并构建一个供我们使用的 RobotModel 。

robot_model_loader::RobotModelLoaderPtr robot_model_loader(
    new robot_model_loader::RobotModelLoader("robot_description"));

通过使用 RobotModelLoader ，我们可以构造一个规划场景监视器（planing scene monitor），该监视器将创建一个规划场景，监视规划场景变更，并将变更应用于其内部的规划场景对象（planning scene）。然后，我们调用 startSceneMonitor ，startWorldGeometryMonitor 和 startStateMonitor 以完全初始化规划场景监视器。

planning_scene_monitor::PlanningSceneMonitorPtr psm(
    new planning_scene_monitor::PlanningSceneMonitor(robot_model_loader));

/* 监听主题 /XXX 上的规划场景消息，并将其应用于对应的内部规划场景对象 */
psm->startSceneMonitor();
/* 监听世界几何体，碰撞对象和 octomaps（可选）*/
psm->startWorldGeometryMonitor();
/* 监听关节状态更新以及附加的碰撞对象的更改，并相应地更新内部规划场景对象 */
psm->startStateMonitor();

/* 我们还可以使用 RobotModelLoader 获得包含机器人运动学信息的机器人模型 ，即 RobotModel 对象 */
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 来跟踪当前的机器人位姿和 planning group。 Joint Model
   group 可用于一次处理一组关节，例如处理左臂或末端执行器 */
const moveit::core::JointModelGroup* joint_model_group = robot_state->getJointModelGroup("panda_arm");

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

planning_pipeline::PlanningPipelinePtr planning_pipeline(
    new planning_pipeline::PlanningPipeline(robot_model, node_handle, "planning_plugin", "request_adapters"));

可视化¶

MoveItVisualTools 包提供了 RViz 里许多将物体、机器人和轨迹可视化的功能，还提供了一些调试工具，如脚本的逐步运行。

namespace rvt = rviz_visual_tools;
moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
visual_tools.deleteAllMarkers();

/* 远程控制是一种自我审查工具，允许用户通过 RViz 中的按钮和快捷键来逐步地执行高级脚本。 */
visual_tools.loadRemoteControl();

/* RViz 提供了许多类型的 markers，在本 demo 中，我们将使用文本，圆柱体和球体 */
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;
planning_interface::MotionPlanResponse res;
geometry_msgs::PoseStamped pose;
pose.header.frame_id = "panda_link0";
pose.pose.position.x = 0.3;
pose.pose.position.y = 0.0;
pose.pose.position.z = 0.75;
pose.pose.orientation.w = 1.0;

位置容许误差为 0.01 m，姿态容许误差为 0.01 弧度。

  std::vector<double> tolerance_pose(3, 0.01);
  std::vector<double> tolerance_angle(3, 0.01);


我们使用 `kinematic_constraints`_ 包中提供的一个辅助函数来创建带约束的规划请求。

req.group_name = "panda_arm";
moveit_msgs::Constraints pose_goal =
    kinematic_constraints::constructGoalConstraints("panda_link8", pose, tolerance_pose, tolerance_angle);
req.goal_constraints.push_back(pose_goal);

在执行规划之前，我们需要在规划场景上加只读锁，以便在规划时不会被修改 world representation 。

{
  planning_scene_monitor::LockedPlanningSceneRO lscene(psm);
  /* 现在，调用规划管道并检查规划是否成功。 */
  planning_pipeline->generatePlan(lscene, req, res);
}
/* 现在，调用规划管道并检查规划是否成功。 */
/* 检查规划是否成功 */
if (res.error_code_.val != res.error_code_.SUCCESS)
{
  ROS_ERROR("Could not compute plan successfully");
  return 0;
}

可视化结果¶

ros::Publisher display_publisher =
    node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
moveit_msgs::DisplayTrajectory display_trajectory;

/* 可视化运动轨迹 */
ROS_INFO("Visualizing the trajectory");
moveit_msgs::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::Constraints joint_goal = kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group);

req.goal_constraints.clear();
req.goal_constraints.push_back(joint_goal);

在执行规划之前，我们需要在规划场景上加只读锁，以便在规划时不会被修改 world representation 。

{
  planning_scene_monitor::LockedPlanningSceneRO lscene(psm);
  /* 现在, 调用规划管道并检查规划是否成功。 */
  planning_pipeline->generatePlan(lscene, req, res);
}
/* 检查规划是否成功。 */
if (res.error_code_.val != res.error_code_.SUCCESS)
{
  ROS_ERROR("Could not compute plan successfully");
  return 0;
}
/* 可视化轨迹 */
ROS_INFO("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");

使用规划请求适配器 Planning Request Adapter¶

通过规划请求适配器 Planning Request Adapter ，我们可以指定在进行规划之前或完成规划之后在结果路径上应该执行的一系列操作。

/* 首先, 将规划场景中的状态设置为上一次运动规划的最终状态 */
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);

在执行规划之前，我们需要在规划场景上加只读锁，以便在规划时不会被修改 world representation 。

  {
    planning_scene_monitor::LockedPlanningSceneRO lscene(psm);
    /* 现在, 调用规划管道并检查规划是否成功 */
    planning_pipeline->generatePlan(lscene, req, res);
  }
  if (res.error_code_.val != res.error_code_.SUCCESS)
  {
    ROS_ERROR("Could not compute plan successfully");
    return 0;
  }
  /* 可视化轨迹 */
  ROS_INFO("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");

  ROS_INFO("Done");
  return 0;
}

Launch 文件¶

整个 launch 文件在 GitHub 上的这里可见。本教程中的所有代码都可从 moveit_tutorials 包中运行，这个包是 MoveIt 安装的一部分。

开源反馈

有地方需要改进？请在 GitHub page 新开一个 pull request。