MoveIt!教程 - MoveGroup接口

最新教程见MoveIt!教程

在MoveIt!中主要的用户界面是通过MoveGroup类实现，为用户可能想要执行的大多数操作提供了易于使用的功能，特别是设置关节或姿态目标，创建运动计划，移动机器人，将对象添加到环境中，并从机器人中添加/分离对象。该接口将ROS主题、服务和动作传递给MoveGroup节点。

创建Catkin工作空间

编译示例代码

在您的catkin工作区（cd~/ws_moveit/src）中，下载本教程：

git clonehttps://github.com/ros-planning/moveit_tutorials.git

临时PR2动力学介绍

您还需要一个pr2moveitconfig包来运行本教程。目前，这是在ROS动力学中未释放的，但下面是一个临时的解决方法：

git clonehttps://github.com/PR2/pr2_common.git -b kinetic-devel
git clonehttps://github.com/davetcoleman/pr2_moveit_config.git

安装依赖关系和构建

在编译新代码之前，扫描您的catkin工作空间，寻找丢失的包：

rosdep install--from-paths . --ignore-src --rosdistro kinetic
cd ~/ws_moveit
catkin config--extend /opt/ros/kinetic --cmake-args -DCMAKE_BUILD_TYPE=Release
catkin build

启动Rviz和MoveGroup节点

确保您已经重新获得了安装文件：

source ~/ws_moveit/devel/setup.bash

启动Rviz，等待一切完成加载：

roslaunch pr2_moveit_config demo.launch

运行演示

在一个新的终端窗口中，运行move_group_interface_tutorial.launchroslaunch文件：

roslaunch moveit_tutorials move_group_interface_tutorial.launch

在短暂的片刻之后，Rviz窗口应该出现，并且看起来与页面顶部的那个窗口相似。在屏幕下方按下一个按钮，或者按下键盘上的“n”键，而Rviz则专注于在每个演示步骤中取得进展。

解释演示

整个代码位于moveit教程github的子文件夹“pr2_tutorials/planning”下。接下来，我们逐段逐段地分析代码，解释它的功能。

设置

只需使用要控制和计划的组的名称，就可以轻松设置MoveGroup类。

moveit::planning_interface::MoveGroup group("right_arm");

我们将使用PlanningSceneInterface类直接处理世界。

moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

(可选)创建用于在Rviz中可视化计划的发布程序。

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

获取基本信息

可以为这个机器人输出参考帧的名称

ROS_INFO("Reference frame: %s", group.getPlanningFrame().c_str());

还可以输出这个组的末端执行器链接的名称

ROS_INFO("Reference frame: %s", group.getEndEffectorLink().c_str());

规划到目标姿态

可以为这一组设计一个运动来达到末端执行器想要的姿态

geometry_msgs::Pose target_pose1;
target_pose1.orientation.w = 1.0;
target_pose1.position.x = 0.28;
target_pose1.position.y = -0.7;
target_pose1.position.z = 1.0;
group.setPoseTarget(target_pose1);

现在，调用规划器来计算规划并将其可视化。注意，我们只是规划，而不是要求move_group实际移动机器人

moveit::planning_interface::MoveGroup::Plan my_plan;
bool success = group.plan(my_plan);ROS_INFO("Visualizing plan 1 (pose goal) %s",success?"":"FAILED");
/* Sleep to give Rviz time to visualize the plan. */
sleep(5.0);

可视化规划

现在有了一个规划，可以在Rviz中将它可视化。这是不必要的，因为上面的group.plan()调用是自动执行的。但是，在希望可视化先前创建的规划的情况下，显式发布计划是有用的

if (1)
{ROS_INFO("Visualizing plan 1 (again)");display_trajectory.trajectory_start = my_plan.start_state_;display_trajectory.trajectory.push_back(my_plan.trajectory_);display_publisher.publish(display_trajectory);/* Sleep to give Rviz time to visualize the plan. */sleep(5.0);
}

移向目标姿态

移动到目标姿态类似于上面的步骤，只是我们现在使用move()函数。注意，之前设定的目标姿态仍然是活动的，所以机器人会尝试移动到那个目标。在本教程中，我们将不使用该函数，因为它是一个阻塞函数，需要控制器处于活动状态，并报告轨迹执行成功

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

规划到目标关节空间

让我们设定一个共同的空间目标并朝着它前进。这将替换我们上面设置的位姿目标。

首首先获取组的当前联合值集。

std::vector<double> group_variable_values;
group.getCurrentState()->copyJointGroupPositions(group.getCurrentState()->getRobotModel()->getJointModelGroup(group.getName()), group_variable_values);

现在，让我们修改其中一个关节，规划为新的关节空间目标，并可视化该计划。

group_variable_values[0] = -1.0;
group.setJointValueTarget(group_variable_values);
success = group.plan(my_plan);ROS_INFO("Visualizing plan 2 (joint space goal) %s",success?"":"FAILED");
/* Sleep to give Rviz time to visualize the plan. */
sleep(5.0);

路径约束规划

可以很容易地为机器人上的链接指定路径约束。让我们为我们的组指定一个路径约束和一个位姿目标。首先定义路径约束。

moveit_msgs::OrientationConstraint ocm;
ocm.link_name = "r_wrist_roll_link";
ocm.header.frame_id = "base_link";
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;

现在，将它设置为group的路径约束。

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

我们将重新利用我们的旧目标，并计划它。注意，只有当当前状态已经满足路径约束时，这才会工作。所以，我们需要将起始状态设置为一个新的姿态。

robot_state::RobotState start_state(*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;
const robot_state::JointModelGroup *joint_model_group =start_state.getJointModelGroup(group.getName());
start_state.setFromIK(joint_model_group, start_pose2);
group.setStartState(start_state);

现在，我们将从刚刚创建的新start状态开始计划早期的pose目标。

group.setPoseTarget(target_pose1);
success = group.plan(my_plan);ROS_INFO("Visualizing plan 3 (constraints) %s",success?"":"FAILED");
/* Sleep to give Rviz time to visualize the plan. */
sleep(10.0);

当使用路径约束时，一定要清除它。

group.clearPathConstraints();

笛卡尔路径

您可以通过指定末端执行器要经过的路径点列表来直接规划笛卡尔路径。注意，我们从上面的新start状态开始。初始姿态(开始状态)不需要添加到waypoint列表中。

std::vector<geometry_msgs::Pose> waypoints;geometry_msgs::Pose target_pose3 = start_pose2;
target_pose3.position.x += 0.2;
target_pose3.position.z += 0.2;
waypoints.push_back(target_pose3);  // up and outtarget_pose3.position.y -= 0.2;
waypoints.push_back(target_pose3);  // lefttarget_pose3.position.z -= 0.2;
target_pose3.position.y += 0.2;
target_pose3.position.x -= 0.2;
waypoints.push_back(target_pose3);  // down and right (back to start)

我们想要笛卡尔坐标路径以1cm的分辨率插值这就是为什么我们要指定0。01作为笛卡尔坐标平移的最大步长。我们将把跳转阈值指定为0.0，从而有效地禁用它。

moveit_msgs::RobotTrajectory trajectory;
double fraction = group.computeCartesianPath(waypoints,0.01,  // eef_step0.0,   // jump_thresholdtrajectory);ROS_INFO("Visualizing plan 4 (cartesian path) (%.2f%% acheived)",fraction * 100.0);
/* Sleep to give Rviz time to visualize the plan. */
sleep(15.0);

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

首先定义碰撞对象消息。

moveit_msgs::CollisionObject collision_object;
collision_object.header.frame_id = group.getPlanningFrame();/* The id of the object is used to identify it. */
collision_object.id = "box1";/* Define a box to add to the world. */
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;/* A pose for the box (specified relative to frame_id) */
geometry_msgs::Pose box_pose;
box_pose.orientation.w = 1.0;
box_pose.position.x =  0.6;
box_pose.position.y = -0.4;
box_pose.position.z =  1.2;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("Add an object into the world");
planning_scene_interface.addCollisionObjects(collision_objects);/* Sleep so we have time to see the object in RViz */
sleep(2.0);

带有碰撞检测的规划可能很慢。让我们设定规划时间，以确保计划员有足够的时间来做计划。10秒应该足够了。

group.setPlanningTime(10.0);

当我们规划轨迹时，它会避开障碍物

group.setStartState(*group.getCurrentState());
group.setPoseTarget(target_pose1);
success = group.plan(my_plan);ROS_INFO("Visualizing plan 5 (pose goal move around box) %s",success?"":"FAILED");
/* Sleep to give Rviz time to visualize the plan. */
sleep(10.0);

现在把碰撞物体附加到机器人上。

ROS_INFO("Attach the object to the robot");
group.attachObject(collision_object.id);
/* Sleep to give Rviz time to show the object attached (different color). */
sleep(4.0);

现在把碰撞物体从机器人中分离出来。

ROS_INFO("Detach the object from the robot");
group.detachObject(collision_object.id);
/* Sleep to give Rviz time to show the object detached. */
sleep(4.0);

现在将碰撞对象从世界中移除。

ROS_INFO("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);
/* Sleep to give Rviz time to show the object is no longer there. */
sleep(4.0);

双臂的目标姿态

首先定义一个新的组来处理这两个分支。然后定义两个独立的姿态目标，每个末端执行器一个。请注意，我们正在为上面的右臂重用目标

moveit::planning_interface::MoveGroup two_arms_group("arms");two_arms_group.setPoseTarget(target_pose1, "r_wrist_roll_link");geometry_msgs::Pose target_pose2;
target_pose2.orientation.w = 1.0;
target_pose2.position.x = 0.7;
target_pose2.position.y = 0.15;
target_pose2.position.z = 1.0;two_arms_group.setPoseTarget(target_pose2, "l_wrist_roll_link");

现在，我们可以规划和可视化

moveit::planning_interface::MoveGroup::Plan two_arms_plan;
two_arms_group.plan(two_arms_plan);
sleep(4.0);

运行代码

entire code Compiling the code launch file

从moveit_tutorials直接运行代码的启动文件：

roslaunch moveit_tutorials move_group_interface_tutorial.launch

过一会儿，Rviz窗口应该会出现：

可以关闭窗口右下角的运动规划部分，以便更好地查看机器人。

期望输出

在Rviz中，我们应该能够看到以下内容(每一步之间会有5-10秒的延迟)：

1. 机器人将右臂移向右前方的姿势。

2. 机器人将右臂移动到右侧的关节处。

3. 在保持末端执行器的水平的同时，机器人将右臂移回到新的姿势目标。

4. 机器人沿着理想的笛卡尔路径移动它的右手臂（一个三角形向上+向前，左，向下+后）。

5. 将一个盒子对象添加到右边的右边的环境中。

6. 该机器人将右臂移至姿势目标，避免与盒子发生碰撞。

7. 这个物体附着在手腕上（它的颜色会变成紫色/橙色/绿色）。

8. 这个物体脱离了手腕（它的颜色会变回绿色）。

9. 对象从环境中删除。

10. 这个机器人同时将两个手臂同时移动到两个不同的姿势。