MoveIt教程7 - Move Group (C++接口)

在MoveIt编程时最常用到的一个接口类是MoveGroupInterface
提供了对于控制机器人的常用基本操作：

设置关节角度或机器人姿态目标
运动规划
移动机器人
添加物体到环境里/从环境移除
将物体绑定到机器人上/从机器人上解绑
MoveGroupInterface通过ROS的话题topics、服务services和动作actions和MoveGroup节点通讯。

这里通过Panda机械臂的案例看一下怎么玩。

运行程序

启动panda机械臂MoveIt主程序

roslaunch panda_moveit_config demo.launch

运行MoveGroupInterface教程

roslaunch moveit_tutorials move_group_interface_tutorial.launch

RVizVisualToolsGui

通过RVizVisualToolsGui面板来逐步操作教程中的步骤。
在Panels菜单中，选择RvizVisualToolsGui。

加载完后如下

结果

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

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

代码分析

教程的完整代码https://github.com/ros-planning/moveit_tutorials/blob/kinetic-devel/doc/move_group_interface/src/move_group_interface_tutorial.cpp

下面我们分析一下代码块：

Setup

MoveIt中的控制是对规划组planning groups进行的，通过创建MoveGroupInterface对象后会把planning group保存到JointModelGroup对象。在MoveIt编程中通常会把planning group叫做joint model group。

通过SRDF中定义的planning group的名称来创建接口对象。

static const std::string PLANNING_GROUP = "panda_arm";
moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);

创建一个PlanningSceneInterface 类的对象，用于添加和移除物体。

moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

构建一个指针指向JointModelGroup对象，有些情况下会直接对joint model group进行操作。

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

Visualization

MoveItVisualTools 是一个提供可视化的包，包括物体、机器人、轨迹以及调试信息的可视化。

  // Visualization
  // ^^^^^^^^^^^^^
  //
  // The package MoveItVisualTools provides many capabilties for visualizing objects, robots,
  // and trajectories in RViz as well as debugging tools such as step-by-step introspection of a script
  namespace rvt = rviz_visual_tools;
  moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
  visual_tools.deleteAllMarkers();

  // Remote control is an introspection tool that allows users to step through a high level script
  // via buttons and keyboard shortcuts in RViz
  visual_tools.loadRemoteControl();

  // RViz provides many types of markers, in this demo we will use text, cylinders, and spheres
  Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
  text_pose.translation().z() = 1.75;
  visual_tools.publishText(text_pose, "MoveGroupInterface Demo", rvt::WHITE, rvt::XLARGE);

  // Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations
  visual_tools.trigger();

Getting Basic Information

打印一些基本的信息

  // Getting Basic Information
  // ^^^^^^^^^^^^^^^^^^^^^^^^^
  //
  // We can print the name of the reference frame for this robot.
  ROS_INFO_NAMED("tutorial", "Planning frame: %s", move_group.getPlanningFrame().c_str());

  // We can also print the name of the end-effector link for this group.
  ROS_INFO_NAMED("tutorial", "End effector link: %s", move_group.getEndEffectorLink().c_str());

  // We can get a list of all the groups in the robot:
  ROS_INFO_NAMED("tutorial", "Available Planning Groups:");
  std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
            std::ostream_iterator<std::string>(std::cout, ", "));

Start the demo

调用visual_tools的prompt提示开始。

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

Planning to a Pose goal

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

先创建一个目标姿态

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);

调用接口进行规划 (这里只是规划，还没有具体控制运动）

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");

Visualizing plans

把规划在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");

Moving to a pose goal

调用move()执行运动

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

这里注释掉因为是模拟，没有真正的硬件，调用的话会阻塞。

Planning to a joint-space goal

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

选构建一个指针指向RobotState对象，RobotState记录了规划组中各个关节的信息：位置position/速度velocity/加速度acceleration

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

获取当前关节的位置

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

改变第一个值作为目标状态，调用规划。这里用了setJointValueTarget()

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");

Planning with Path Constraints

可以规定路径规划的一些约束条件

先定义一些约束

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();

Cartesian Paths

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

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

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

geometry_msgs::Pose target_pose3 = start_pose2;

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中显示规划的plan

visual_tools.deleteAllMarkers();
visual_tools.publishText(text_pose, "Cartesian Path", 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");

Adding/Removing Objects and Attaching/Detaching Objects

添加/移除物体和绑定/解绑物体

利用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);

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

visual_tools.publishText(text_pose, "Add object", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();
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");