Ubuntu20.04，ROS Neotic。Move It！（8）运动规划管道-Motion Planning Pipeline--走向真实控制第一步

运动规划管道

在MoveIt中，设置了运动计划器以计划路径。但是，有时我们可能需要预处理运动计划请求或后处理计划的路径（例如，用于时间参数化）。在这种情况下，我们使用计划管道，该管道将运动计划器与预处理和后处理阶段链接在一起。可以通过ROS参数服务器中的名称来配置称为计划请求适配器的预处理和后处理阶段。在本教程中，我们将引导您完成C ++代码以实例化并调用这样的计划管道。

运行代码

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

source ~/ws_moveit/devel/setup.bash
roslaunch panda_moveit_config demo.launch
1
2

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

source ~/ws_moveit/devel/setup.bash
roslaunch moveit_tutorials motion_planning_pipeline_tutorial.launch
1
2

开始

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

我们将从实例化RobotModelLoader对象开始，该对象将在ROS参数服务器上查找机器人说明并构造供我们使用的RobotModel。

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

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

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

/* 接收topic/XXX上的计划场景消息，并将其应用于内部计划场景相应的内部计划场景 */
psm->startSceneMonitor();
/* 接收world geometry, collision objects, and (optionally) octomaps world geometry, collision objects and optionally octomaps */
psm->startWorldGeometryMonitor();
/* 接收关节状态更新以及附加碰撞对象中的更改，并相应地更新内部规划场景*/
psm->startStateMonitor();

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

/* 通过锁定内部规划场景进行读取，我们可以从规划场景监视器获得最新的机器人状态。这个锁确保在我们读取底层场景的状态时，它不会被更新。
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");
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

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

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

可视化

软件包MoveItVisualTools提供了许多功能，可用于可视化RViz中的对象，机械手和轨迹以及调试工具，例如脚本的逐步自省。

namespace rvt = rviz_visual_tools;
moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
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.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

目标位姿

现在，我们将为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;
1
2
3
4
5
6
7
8

位置公差为0.01 m，方向公差为0.01弧度

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

我们将使用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);
1
2
3
4

在计划之前，我们需要在规划场景上具有只读锁，以便在计划时不会修改世界表示。

{
  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;
}
1
2
3
4
5
6
7
8
9
10
11
12

可视化结果

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

/* Visualize the 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();

/* Wait for user input */
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

关节空间目标

/* 首先，将计划场景中的状态设置为上一个计划的最终状态 */
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);
1
2
3
4

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

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);
1
2
3
4
5
6
7

在计划之前，我们需要在计划场景上具有只读锁，以便在计划时不会修改世界表示。

{
  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);
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

现在您应该看到两个计划的轨迹

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");
1
2
3
4
5
6

使用计划请求适配器

通过计划请求适配器，我们可以指定在计划发生之前或在结果路径上完成计划之后应该发生的一系列操作

/* 首先，将计划场景中的状态设置为上一个计划的最终状态 */
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);
1
2
3
4

现在，将其中一个关节设置为稍微超出其上限

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);
1
2
3
4
5
6
7
8

在计划之前，我们需要在计划场景上具有只读锁，以便在计划时不会修改世界表示。

{
    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;
  }
  /* Visualize the trajectory */
  ROS_INFO("Visualizing the trajectory");
  res.getMessage(response);
  display_trajectory.trajectory_start = response.trajectory_start;
  display_trajectory.trajectory.push_back(response.trajectory);
  /* Now you should see three planned trajectories in series*/
  display_publisher.publish(display_trajectory);
  visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group);
  visual_tools.trigger();

  /* Wait for user input */
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to finish the demo");

  ROS_INFO("Done");
  return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26