MoveIt 教程【3】——Move Group C++ Interface

在MoveIt中，最简单的用户接口是MoveGroup类,提供了对于控制机器人的常用基本操作：

• 设置关节角度或机器人姿态目标
• 运动规划
• 移动机器人
• 添加物体到环境里/从环境移除
• 将物体绑定到机器人上/从机器人上解绑

这个接口通过ROS主题、服务和操作与MoveGroup节点通信。

1. 运行示例代码

roslaunch panda_moveit_config demo.launch

启动 launch 文件

roslaunch moveit_tutorials move_group_interface_tutorial.launch

2. 期望输出结果

教程中，对机械臂进行了以下几种控制：

• 手臂运动到前方的目标姿态
• 手臂运动到两侧的目标姿态
• 手臂运动到一个新的目标姿态但维持末端执行器不动
• 手臂按照一个给定的轨迹运动
• 一个长方体物体添加到环境里
• 手臂绕过物体到达新的目标姿态
• 把物体固定到手臂上
• 把物体从手臂上移除
• 把物体从环境中移除

3. 代码

我的 move_group C++接口代码路径：
/home/hjs/moveit_ws/src/moveit_tutorials/doc/move_group_interface.cpp

3.1 配置

MoveIt！对被称为“planning groups”的一组关节进行操作，并将它们存储在一个名为 JointModelGroup 的对象中。在整个 MoveIt 中术语“planning groups”和“joint model group”可互换使用。

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

设置 MoveGroupInterface 类（使用想要控制和规划的 planning group 的名字即可）

moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);

使用 PlanningSceneInterface 类在“虚拟世界”场景中添加和删除碰撞对象：

moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

为了提高性能，经常使用原始指针来引用 planning group：

const robot_state::JointModelGroup* joint_model_group =
      move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);

3.2 Visualization:

MoveItVisualTools 包提供许多功能，在 RViz 中对于可视化对象、机器人和轨迹，和调试工具，比如脚本检查。

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

远程控制是一种自省工具，允许用户通过 RViz 中的按钮和键盘快捷键逐步浏览高级脚本

visual_tools.loadRemoteControl();

RViz提供了多种类型的标记，在此演示中，使用文本，圆柱体和球体

Eigen::Affine3d text_pose = Eigen::Affine3d::Identity();
  text_pose.translation().z() = 1.75;
  visual_tools.publishText(text_pose, "MoveGroupInterface Demo", rvt::WHITE, rvt::XLARGE);

批量发布用于减少发送到RViz进行大型可视化的消息数量

visual_tools.trigger();

3.3 获得基本信息

打印机器人参考框架的名字：

ROS_INFO_NAMED("tutorial", "Reference frame: %s", move_group.getPlanningFrame().c_str());

打印末端执行器链接的名称：

ROS_INFO_NAMED("tutorial", "End effector link: %s", move_group.getEndEffectorLink().c_str());

3.4 开始 demo

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

3.5 规划一个姿态目标

指定一个末端执行器的目标姿态，调用规划运动:

  geometry_msgs::Pose target_pose1;
  target_pose1.orientation.w = 1.0;
  target_pose1.position.x = 0.28;
  target_pose1.position.y = -0.2;
  target_pose1.position.z = 0.5;
  move_group.setPoseTarget(target_pose1);

调用planner来计算计划并将其可视化（只是规划，还没有具体控制运动）：

  moveit::planning_interface::MoveGroupInterface::Plan my_plan;

  bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);

  ROS_INFO_NAMED("tutorial", "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");

3.6 可视化规划

用RViz中的标记将计划可视化为一条线:

  ROS_INFO_NAMED("tutorial", "Visualizing plan 1 as trajectory line");
  visual_tools.publishAxisLabeled(target_pose1, "pose1");
  visual_tools.publishText(text_pose, "Pose Goal", rvt::WHITE, rvt::XLARGE);
  visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
  visual_tools.trigger();
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

3.7 移动到目标姿态

调用move()执行运动:

  /* Uncomment below line when working with a real robot */
  /* move_group.move(); */

是一个阻塞函数，需要控制器处于活动状态，并报告轨迹执行成功，注释掉因为是模拟，没有真正的硬件，调用的话会阻塞。

3.8 规划到一个关节空间内的目标

通过在关节空间定义一个目标的方式进行控制。旧的目标会被覆盖。

• 创建一个引用当前robot状态的指针。RobotState是包含所有当前位置/速度/加速度数据的对象

moveit::core::RobotStatePtr current_state = move_group.getCurrentState();

• 获取当前关节位置：

  std::vector<double> joint_group_positions;
  current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);

• 修改其中一个关节，规划新的关节空间目标，并将其可视化：

  joint_group_positions[0] = -1.0;  // radians
  move_group.setJointValueTarget(joint_group_positions);

  success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
  ROS_INFO_NAMED("tutorial", "Visualizing plan 2 (joint space goal) %s", success ? "" : "FAILED");

• 在RViz中可视化：

  visual_tools.deleteAllMarkers();
  visual_tools.publishText(text_pose, "Joint Space Goal", rvt::WHITE, rvt::XLARGE);
  visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
  visual_tools.trigger();
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

3.9 路径约束规划

• 定义约束：

  moveit_msgs::OrientationConstraint ocm;
  ocm.link_name = "panda_link7";
  ocm.header.frame_id = "panda_link0";
  ocm.orientation.w = 1.0;
  ocm.absolute_x_axis_tolerance = 0.1;
  ocm.absolute_y_axis_tolerance = 0.1;
  ocm.absolute_z_axis_tolerance = 0.1;
  ocm.weight = 1.0;

• 添加到指定规划组：

  moveit_msgs::Constraints test_constraints;
  test_constraints.orientation_constraints.push_back(ocm);
  move_group.setPathConstraints(test_constraints);

• 下一步进行规划，再次使用之前定义的目标姿态target_pose1。给定一个新的起始姿态。（只有在当前状态已经满足路径约束时，此操作才有效）

  robot_state::RobotState start_state(*move_group.getCurrentState());
  geometry_msgs::Pose start_pose2;
  start_pose2.orientation.w = 1.0;
  start_pose2.position.x = 0.55;
  start_pose2.position.y = -0.05;
  start_pose2.position.z = 0.8;
  start_state.setFromIK(joint_model_group, start_pose2);
  move_group.setStartState(start_state);

• 设置目标姿态：

  move_group.setPoseTarget(target_pose1);

• 加入约束条件后规划会比较耗时，因为每次采样都需要调用逆运动求解。这里把规划时间增加到10秒，保证有足够的时间规划成功。

  move_group.setPlanningTime(10.0);

  success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
  ROS_INFO_NAMED("tutorial", "Visualizing plan 3 (constraints) %s", success ? "" : "FAILED");

• 在rviz中显示:

  visual_tools.deleteAllMarkers();
  visual_tools.publishAxisLabeled(start_pose2, "start");
  visual_tools.publishAxisLabeled(target_pose1, "goal");
  visual_tools.publishText(text_pose, "Constrained Goal", rvt::WHITE, rvt::XLARGE);
  visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
  visual_tools.trigger();
  visual_tools.prompt("next step");

• 当完成路径约束时，一定要清除约束条件:

move_group.clearPathConstraints();

• 因为设置了开始状态，所以必须在规划其它路径之前清除它:

move_group.setStartStateToCurrentState();

3.10 笛卡尔路径

通过给定一系列路径点来让末端执行器经过一个特定的路径：

• 创建一个pose的vector，当前的机器人位置是在start_pose3，起始点不是必须加进数组里（这里加进去是为了可以在rviz中显示）

  geometry_msgs::Pose target_pose3 = move_group.getCurrentPose().pose;

  std::vector<geometry_msgs::Pose> waypoints;
  waypoints.push_back(target_pose3);

  target_pose3.position.z -= 0.2;
  waypoints.push_back(target_pose3);  // down

  target_pose3.position.y -= 0.2;
  waypoints.push_back(target_pose3);  // right

  target_pose3.position.z += 0.2;
  target_pose3.position.y += 0.2;
  target_pose3.position.x -= 0.2;
  waypoints.push_back(target_pose3);  // up and left

• 笛卡尔运动经常需要较慢的动作，比如接近和后退的抓握运动。在这里，演示了如何通过每个关节的最大速度比例因子来降低机器人手臂的速度。注意这不是末端执行器的速度。

move_group.setMaxVelocityScalingFactor(0.1);

• 调用computeCartesianPath(…)按一定步长插补得到一个plan。这里的步长eef_step设置为0.01，路径的分辨率就是1cm。跳动阈值jump_threshold设置为0.0将其禁用（控制真实机械臂时会有问题）。返回值是0.0~1.0之间的一个数值，表示成功规划了给定路径点的多少（1.0表示都能成功经过）。

  moveit_msgs::RobotTrajectory trajectory;
  const double jump_threshold = 0.0;
  const double eef_step = 0.01;
  double fraction = move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
  ROS_INFO_NAMED("tutorial", "Visualizing plan 4 (Cartesian path) (%.2f%% acheived)", fraction * 100.0);

• 在RViz中可视化该计划：

  visual_tools.deleteAllMarkers();
  visual_tools.publishText(text_pose, "Joint Space Goal", rvt::WHITE, rvt::XLARGE);
  visual_tools.publishPath(waypoints, rvt::LIME_GREEN, rvt::SMALL);
  for (std::size_t i = 0; i < waypoints.size(); ++i)
    visual_tools.publishAxisLabeled(waypoints[i], "pt" + std::to_string(i), rvt::SMALL);
  visual_tools.trigger();
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

3.11 添加/删除对象和附着/分离对象

• 利用moveit_msgs::CollisionObject定义一个物体

  moveit_msgs::CollisionObject collision_object;
  collision_object.header.frame_id = move_group.getPlanningFrame();

设置一个id用于标识它：

  collision_object.id = "box1";

• 定义一个立方体：

  shape_msgs::SolidPrimitive primitive;
  primitive.type = primitive.BOX;
  primitive.dimensions.resize(3);
  primitive.dimensions[0] = 0.4;
  primitive.dimensions[1] = 0.1;
  primitive.dimensions[2] = 0.4;

• 为框定义一个位姿[指定相对于frame_id]

  geometry_msgs::Pose box_pose;
  box_pose.orientation.w = 1.0;
  box_pose.position.x = 0.4;
  box_pose.position.y = -0.2;
  box_pose.position.z = 1.0;

  collision_object.primitives.push_back(primitive);
  collision_object.primitive_poses.push_back(box_pose);
  collision_object.operation = collision_object.ADD;

  std::vector<moveit_msgs::CollisionObject> collision_objects;
  collision_objects.push_back(collision_object);

• 碰撞对象添加到世界

  ROS_INFO_NAMED("tutorial", "Add an object into the world");
  planning_scene_interface.addCollisionObjects(collision_objects);

• 显示文本在RViz的状态

  visual_tools.publishText(text_pose, "Add object", rvt::WHITE, rvt::XLARGE);
  visual_tools.trigger();

• 等待MoveGroup接收并处理collision对象消息：

visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz");

• 现在如果规划一个运动，其路径会避开这个物体：

  move_group.setStartState(*move_group.getCurrentState());
  geometry_msgs::Pose another_pose;
  another_pose.orientation.w = 1.0;
  another_pose.position.x = 0.4;
  another_pose.position.y = -0.4;
  another_pose.position.z = 0.9;
  move_group.setPoseTarget(another_pose);

  success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
  ROS_INFO_NAMED("tutorial", "Visualizing plan 5 (pose goal move around cuboid) %s", success ? "" : "FAILED");

• rivz显示结果并提示下一步操作：

  visual_tools.deleteAllMarkers();
  visual_tools.publishText(text_pose, "Obstacle Goal", rvt::WHITE, rvt::XLARGE);
  visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
  visual_tools.trigger();
  visual_tools.prompt("next step");

• 把碰撞物体连接到机器人上：

  ROS_INFO_NAMED("tutorial", "Attach the object to the robot");
  move_group.attachObject(collision_object.id);

• rivz显示结果并提示下一步操作：

  visual_tools.publishText(text_pose, "Object attached to robot", rvt::WHITE, rvt::XLARGE);
  visual_tools.trigger();

  /* Wait for MoveGroup to recieve and process the attached collision object message */
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object attaches to the "
                      "robot");

• 解绑物体：

  ROS_INFO_NAMED("tutorial", "Detach the object from the robot");
  move_group.detachObject(collision_object.id);

• rivz显示结果并提示下一步操作：

  visual_tools.publishText(text_pose, "Object dettached from robot", rvt::WHITE, rvt::XLARGE);
  visual_tools.trigger();

  /* Wait for MoveGroup to recieve and process the attached collision object message */
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object detaches to the "
                      "robot");

• 从世界中移除碰撞对象：

  ROS_INFO_NAMED("tutorial", "Remove the object from the world");
  std::vector<std::string> object_ids;
  object_ids.push_back(collision_object.id);
  planning_scene_interface.removeCollisionObjects(object_ids);

• rivz显示结果并提示下一步操作：

  visual_tools.publishText(text_pose, "Object removed", rvt::WHITE, rvt::XLARGE);
  visual_tools.trigger();

  /* Wait for MoveGroup to recieve and process the attached collision object message */
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object disapears");