MoveGroupInterface功能分析

普通运动

关节运动

给定一个目标关节值，将机械臂运动到这一个目标：

moveit::planning_interface::MoveGroupInterface move_group(groupName);
move_group.setJointValueTarget(target_position);
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
bool success=(move_group.plan(my_plan)==moveit::planning_interface::MoveitErrorCode::SUCCESS);
if(success)
    move_group.move();

Pose运动

运动到一个给定的目标Pose。若未指定末端执行器的link，则使用默认的末端执行器link，即该group的最后一个link。

moveit::planning_interface::MoveGroupInterface move_group(groupName);
move_group.setPoseTarget(target_pose, end_effector_link="");
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
bool success=(move_group.plan(my_plan)==moveit::planning_interface::MoveitErrorCode::SUCCESS);
if(success)
    move_group.move();

具有约束的路径规划

在进行路径规划时，可以为某个link或joint设置如下约束：

• JointConstraint，设置一个关节的位置约束，也就是说在规划时，必须使该关节经过该位置。
• PositionConstraint，设置一个link的位置约束
• OrientationConstraint，设置link的方向约束。该约束在开始时必须将link的方向调整为与目标方向一致，然后从起始位置移动到目标位置时，会令link的方向保持不变。该约束非常有用，例如在一个平面内进行焊接和雕刻，以及拧螺丝操作等等。
• VisibilityConstraint，

如下是以OrientationConstraint为例说明约束路径规划的使用

//定义一个方向约束
moveit_msgs::OrientationConstraint ocm;
ocm.link_name = "link6";
ocm.header.frame_id="link0";
ocm.orientation=start_pose.orientation;
ocm.absolute_x_axis_tolerance = 0.1;
ocm.absolute_y_axis_tolerance = 0.1;
ocm.absolute_z_axis_tolerance = 0.1;
//将这个约束应用于move_group
moveit_msgs::Constraints test_constraints;
test_constraints.orientation_constraints.push_back(ocm);
move_group.setPathConstraints(test_constraints);

// 注意目标的方向要与起始方向一致
move_group.setPoseTarget(target_pose);
//...

Cartesian路径规划

将机器人的手臂沿一个期望的笛卡尔坐标路径运动，这里以一个三角形（向下，向右，在左上）。可以通过指定路径点的方式来实现。在代码中采用包含pose的vector来设置路径点。通常初始的pose可以不放在里面，但添加初始pose有助于可视化。

//创建三角形的顶点，第一个顶点为机器人的起始位置点
vector<geometry_msgs::Pose> way_points;
geometry_msgs::Pose target_pose =start_pose;
way_points.push_back(target_pose);
target_pose.position.z -=0.2;
way_points.push_back(target_pose);
target_pose.position.y -=0.2;
way_points.push_back(target_pose);
target_pose.position.z +=0.2;
target_pose.position.y +=0.2;
way_points.push_back(target_pose);
//创建move_group
moveit::planning_interface::MoveGroupInterface move_group(groupName);
//move_group.setMaxVelocityScalingFactor(0.1);

//按照路径点进行轨迹规划
moveit_msgs::RobotTrajectory trajectory;
const double jump_threshold=0.0;
const double eef_step=0.01;
double fraction = move_group.computeCartesianPath(way_points, eef_step, jump_threshold, trajectory);

//在rviz中显示轨迹线
moveit_visual_tools::MoveitVisualToolsPtr visual_tools;
visual_tools.reset(new moveit_visual_tools::MoveitVisualTools("world", "/rviz_visual_tools"));
visual_tools->deleteAllMarkers();
visual_tools->publishPath(way_points, rviz_visual_tools::LIME_GREEN, rviz_visual_tools::SMALL);
for(size_t i=0; i<way_points.size(); ++i) 
    visual_tools->publishAxisLabeled(way_points[i], "pt"+std::to_string(i), rvt::SMALL);
visual_tools->trigger();

注意其中moveit_visual_tools显示marker时，要确定rviz中MarkerArray是打开并且其Topic为"/rviz_visual_tools"

Pick和Place

pick和place用的是moveit_msgs::Grasp消息，位于<moveit_msgs/Grasp.h>头文件中。其包含了如下的字段：

• trajectory_msgs/JointTrajectory pre_grasp_posture， 定义了在进行抓取之前，末端执行器关节的位置。
• trajectory_msgs/JointTrajectory grasp_posture, 定义了在进行抓取时，末端执行器group中各关节的位置。
• geometry_msgs/PoseStamped grasp_pose, 定义了末端执行器尝试抓取时的方向。
• moveit_msgs/GripperTranslation pre_grasp_approach, 定义用于接近目标物体的方向和距离
• moveit_msgs/GripperTranslation pose_grasp_retreat, 定义在抓取到物体后移动的方向和距离
• moveit_msgs/GripperTranslation pose_place_retreat, 定义在物体被放置后手臂移动的方向和距离

我们以下图中的示例来说明pick和place的应用。这是moveit中的pick和place的教程。

图中创建了两个长方体模拟台桌，第一个台桌的尺寸为(0.2,0.4,0.4),位置在(0.5,0,0.2)。第二个台桌的尺寸为(0.4,0.2,0.4)，位置在(0,0.5,0.2)。然后在第一个台桌上放置一个待移动的物件，其尺寸为(0.02,0.02,0.2)，放置在(0.5, 0, 0.4+0.1)的位置，即第一个台桌上。
（注意： 创建的长方体的自身坐标原点都为其中心点。图中显示的坐标系，红绿蓝分别指向XYZ轴。）

抓取操作

下面我们会按照顺序说明进行抓取操作需要定义的内容，实际编程时可以随意安排。
第一，确定抓取之前末端执行器接近目标物体的方向和距离。 也就是说将link_8坐标系在给定的距离上调整到与抓取方向(后面会定义)一致的方向，然后再沿设定的接近方向靠近物体，直到link_8坐标系与给定的抓取方位一致。
第二，定义在抓取前末端执行器关节名称以及状态。 在第一步中将末端调整到所需位置后，就会按照此步的定义将末端执行器关节进行调整，以便进行抓取。
第三，确定抓取时link_8的方位。 这里要确定一个事情：1，末端执行器手掌面到link_8的距离为0.058；2，末端执行器手抓要抓住长条物体的中间部位。那么link_8的位置可以确定为：
$$0.5-长条宽度/2-掌面到lin{k}_8的距离-一些padding=0.5-0.01-0.058-0.017=0.415$$

link_8的坐标方向则相对于link_0坐标系进行变换，其RPY(分别绕固定轴X,Y,Z旋转)值为$(-\pi /2,-\pi /4,-\pi /2)。$
（说明： 为什么要确定link_8的方位呢？因为link_8是arm关节组中的最后一个关节，而运动规划是针对这个关节组的，所以确定link_8的方位就知道怎么进行运动了。而末端执行器都是固连于arm关节组的最后一个关节的，如果不是，那么也可归到arm关节组去了。）
第四，确定抓取时末端执行器关节的名称以及状态。 当机器人运动到第三步中指定的抓取方位后，按此步中的定义调整末端执行器，以便夹住物体。这一步很重要，当夹住物体时会暗地里将物体依附到末端执行器关节上随之运动。
（说明： 在机器人的urdf中，必须为末端执行器定义可活动的旋转或移动关节，如果是fixed关节则会出错。就算末端执行器是一个固定的吸盘，也将关节定义成移动的，只不过编程时不让它动而已。）
第五，确定抓取后末端执行器退出动作的方向和距离。 也就是说在link_8到达指定位置并抓取目标物体后，机器人末端会安装给定的方向移动一段距离（这时末端坐标系的方向并不变）。

放置操作

放置操作的过程与抓取操作具有很多相似之处。
第一 定义放置前机器人末端如何接近目标放置位置的方向和距离。
第二 定义放置时link_8的方位。注意这里的方向是相对于抓取时的方向的，本列中是将抓取时方向再绕"base_link"的Z轴旋转90度。而位置则是物体中心的全局坐标。
第三 定义放置后机器人末端如何退出的方向和距离。退出时物体将解除于机器人的依附关系。
第四 定义放置后末端执行器关节的名称和状态。