2 MoveIt！教程 - MoveGroup接口

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

创建Catkin工作空间
您不需要构建所有的MoveIt！从源代码中，但是您确实需要一个catkin工作空间设置。如果您还没有设置工作空间，那么请遵循MoveIt的“先决条件”部分！源安装页面，并确保在“源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，等待一切完成加载：
roslaunchpr2_moveit_config demo.launch
运行演示
在一个新的终端窗口中，运行move_group_interface_tutorial.launchroslaunch文件:
roslaunchmoveit_tutorials move_group_interface_tutorial.launch
在短暂的片刻之后，Rviz窗口应该出现，并且看起来与页面顶部的那个窗口相似。在屏幕下方按下一个按钮，或者按下键盘上的“n”键，而Rviz则专注于在每个演示步骤中取得进展。
期望输出
在Rviz中，我们应该能够看到以下内容：
1.       机器人将右臂移向右前方的姿势。
2.       机器人将右臂移动到右侧的关节处。
3.       在保持末端执行器的水平的同时，机器人将右臂移回到新的姿势目标。
4.       机器人沿着理想的笛卡尔路径移动它的右手臂（一个三角形向上+向前，左，向下+后）。
5.       将一个盒子对象添加到右边的右边的环境中。


6.       该机器人将右臂移至姿势目标，避免与盒子发生碰撞。
7.       这个物体附着在手腕上（它的颜色会变成紫色/橙色/绿色）。
8.       这个物体脱离了手腕（它的颜色会变回绿色）。
9.       对象从环境中删除。
10.    这个机器人同时将两个手臂同时移动到两个不同的姿势。
解释演示
整个代码位于moveit教程github的子文件夹“pr2_tutorials/planning”下。接下来，我们逐段逐段地分析代码，解释它的功能。
设置
MoveIt !在称为“计划组”的节点上操作，并将它们存储在一个叫做JointModelGroup的对象中。在MoveIt!术语“计划组”和“联合模型组”是可以交换的。
static conststd::string PLANNING_GROUP = "right_arm";
MoveGroup类可以很容易地使用您想要控制和计划的计划组的名称来设置。
moveit::planning_interface::MoveGroupInterfacemove_group(PLANNING_GROUP);
我们将使用PlanningSceneInterface类在我们的“虚拟世界”场景中添加和删除冲突对象
moveit::planning_interface::PlanningSceneInterfaceplanning_scene_interface;
原始指针经常用于表示计划组，以提高性能。
constrobot_state::JointModelGroup *joint_model_group =
 move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
可视化
这个包MoveItVisualTools提供了许多用于可视化对象、机器人和Rviz中的轨迹的capabilties，以及调试工具，例如逐步地对脚本进行内省。
namespace rvt =rviz_visual_tools;
moveit_visual_tools::MoveItVisualToolsvisual_tools("odom_combined");
visual_tools.deleteAllMarkers();
Remote control是一种自省工具，它允许用户通过Rviz的按钮和键盘快捷方式来执行高级脚本。
visual_tools.loadRemoteControl();
Rviz提供了许多类型的标记，在这个演示中我们将使用文本、圆柱体和球体
Eigen::Affine3dtext_pose = Eigen::Affine3d::Identity();
text_pose.translation().z()= 1.75; // above head of PR2
visual_tools.publishText(text_pose,"MoveGroupInterface Demo", rvt::WHITE, rvt::XLARGE);
批量发布用于减少发送到Rviz的消息的数量，以实现大型可视化
visual_tools.trigger();
获取基本信息
我们可以打印这个机器人的参考框架的名称
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());
规划到一个目标姿态
我们可以为这个基团设计一个运动到一个理想的末端执行器的姿态。
geometry_msgs::Posetarget_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;
move_group.setPoseTarget(target_pose1);
现在，我们叫计划者来计算这个计划并把它形象化。注意，我们只是在计划，而不是让movegroup移动机器人。
moveit::planning_interface::MoveGroupInterface::Planmy_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");
可视化规划
我们也可以把这个计划看作是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("nextstep");
移向目标姿态
移动到一个姿态目标与上面的步骤类似，只是我们现在使用move（）函数。注意，我们之前设定的姿势目标仍然是活跃的，所以机器人会努力达到这个目标。在本教程中，我们不会使用这个函数，因为它是一个阻塞函数，并且要求控制器处于活动状态，并在执行轨迹时报告成功。
/* Uncommentbelow line when working with a real robot */
/*move_group.move() */
规划到关节空间
让我们设定一个共同的空间目标，然后向它靠近。这将取代我们设定的姿态目标。
首先，我们将创建一个指针来引用当前机器人的状态。RobotState对象包含所有当前位置/速度/加速度数据。
moveit::core::RobotStatePtrcurrent_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
bd69
=(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("nextstep");
路径约束的规划
可以很容易地为机器人的链接指定路径约束。让我们为我们的组指定一个路径约束和一个姿态目标。首先定义路径约束。
moveit_msgs::OrientationConstraintocm;
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::Constraintstest_constraints;
test_constraints.orientation_constraints.push_back(ocm);
move_group.setPathConstraints(test_constraints);
我们将重新使用我们已有的目标并计划它。注意，只有当当前状态满足路径约束时才会起作用。因此，我们需要将开始状态设置为一个新的姿势。
robot_state::RobotStatestart_state(*move_group.getCurrentState());
geometry_msgs::Posestart_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);
由于每个样本都必须调用逆运动学求解器，所以对约束的规划可能会很慢。让我们从默认的5秒增加计划时间，确保计划者有足够的时间成功。
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("nextstep");
当使用路径约束时，一定要清除它。
move_group.clearPathConstraints();
笛卡尔路径
您可以通过指定末端执行器的路径点的列表来直接规划笛卡尔路径。请注意，我们正从上面的新的开始状态开始。最初的姿态（启动状态）不需要添加到waypoint列表中，但是添加它可以帮助可视化
std::vector<geometry_msgs::Pose>waypoints;
waypoints.push_back(start_pose2);
 
geometry_msgs::Posetarget_pose3 = start_pose2;
 
target_pose3.position.z+= 0.2;
waypoints.push_back(target_pose3);  // up
 
target_pose3.position.y-= 0.1;
waypoints.push_back(target_pose3);  // left
 
target_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
对于像接近和后退的动作这样的动作，笛卡尔的运动常常需要慢一些。在此，我们演示了如何通过每个关节的马赫数速度来降低机器人手臂的速度。注意，这不是末端执行器点的速度。
move_group.setMaxVelocityScalingFactor(0.1);
我们想要用1cm的分辨率对笛卡尔路径进行插值，这就是为什么我们要在笛卡尔坐标中指定0。01作为最大的步骤。我们将把跳跃阈值指定为0.0，有效地禁用它。警告——在操作真正的硬件时禁用跳转阈值，可能导致大量不可预测的冗余关节活动，并可能成为安全问题。
moveit_msgs::RobotTrajectorytrajectory;
const doublejump_threshold = 0.0;
const doubleeef_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_ti = 0; i < waypoints.size(); ++i)
  visual_tools.publishAxisLabeled(waypoints[i],"pt" + std::to_string(i), rvt::SMALL);
visual_tools.trigger();
visual_tools.prompt("nextstep");
添加/移除对象和连接/分离对象
定义碰撞对象ROS消息。
moveit_msgs::CollisionObjectcollision_object;
collision_object.header.frame_id= move_group.getPlanningFrame();
对象的id用于标识它。
collision_object.id= "box1";
定义一个盒子添加到世界上。
shape_msgs::SolidPrimitiveprimitive;
primitive.type =primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[0]= 0.4;
primitive.dimensions[1]= 0.1;
primitive.dimensions[2]= 0.4;
为盒子定义一个姿态（指定相对于id）
geometry_msgs::Posebox_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_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接收并处理碰撞对象消息
ros::Duration(1.0).sleep();
现在，当我们规划一条轨迹时，它会避开障碍物
move_group.setStartState(*move_group.getCurrentState());
move_group.setPoseTarget(target_pose1);
 
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");
可视化规划
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("nextstep");
现在，让我们把碰撞对象附加到机器人上
ROS_INFO_NAMED("tutorial","Attach the object to the robot");
move_group.attachObject(collision_object.id);
在RViz中显示状态文本
visual_tools.publishText(text_pose,"Object attached to robot", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();
 
/* Sleep toallow MoveGroup to recieve and process the attached collision object message */
ros::Duration(1.0).sleep();
现在，让我们把碰撞对象从机器人中分离出来
ROS_INFO_NAMED("tutorial","Detach the object from the robot");
move_group.detachObject(collision_object.id);
在RViz中显示状态文本
visual_tools.publishText(text_pose,"Object dettached from robot", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();
 
/* Sleep toallow MoveGroup to recieve and process the detach collision object message */
ros::Duration(1.0).sleep();
现在，让我们把碰撞对象从世界上移除
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);
在RViz中显示状态文本
visual_tools.publishText(text_pose,"Object removed", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();
 
/* Sleep to giveRviz time to show the object is no longer there.*/
ros::Duration(1.0).sleep();
双臂目标姿势
首先定义一个新组来处理这两个手臂。
static conststd::string PLANNING_GROUP2 = "arms";
moveit::planning_interface::MoveGroupInterfacetwo_arms_move_group(PLANNING_GROUP2);
定义两个单独的姿势目标，一个针对每个末端执行器。请注意，我们正在重用上面右边的那个目标
two_arms_move_group.setPoseTarget(target_pose1,"r_wrist_roll_link");
 
geometry_msgs::Posetarget_pose4;
target_pose4.orientation.w= 1.0;
target_pose4.position.x= 0.7;
target_pose4.position.y= 0.15;
target_pose4.position.z= 1.0;
 
two_arms_move_group.setPoseTarget(target_pose4,"l_wrist_roll_link");
现在，我们可以规划和可视化
moveit::planning_interface::MoveGroupInterface::Plantwo_arms_plan;
 
success =(two_arms_move_group.plan(two_arms_plan) ==moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial","Visualizing plan 7 (dual arm plan) %s", success ? "" :"FAILED");
在Rviz中可视化这个计划
visual_tools.deleteAllMarkers();
visual_tools.publishAxisLabeled(target_pose1,"goal1");
visual_tools.publishAxisLabeled(target_pose4,"goal2");
visual_tools.publishText(text_pose,"Two Arm Goal", rvt::WHITE, rvt::XLARGE);
joint_model_group= move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP2);
visual_tools.publishTrajectoryLine(two_arms_plan.trajectory_,joint_model_group);
visual_tools.trigger();