ROS Moveit范例程序1

首先从Moveit官方的Tutorial开始。使用C++撰写机器手臂的控制。教材依据Move Group C++ Interface。
本篇描述使用Move Group去撰写。Move Group是Moveit中的一个基础的class，根据ROS文件。整体来讲功能分类:

• Group状态(取得或设定手臂名称、轴参数与名称、末端位置...等)
• 正向运动学(指令会有getPose、setPose)
• 逆向运动学(指令会有getJoint、setJoint)
• 移动路径(可以自己给路径或运动学之後抓出路径)
• 抓放工件
• 运动限制(包含set与get)
  其他的参考连结1
  程序码开头:

// 用string，指定手臂名称，这名子来自RViz内Planning Group
static const std::string PLANNING_GROUP = "panda_arm";
// 宣告一个MoveGroupInterface，也就是等等来执行运动学、抓取工件等等
moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
// 建立场景，在这环境产生障碍物
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
// 指标指向getJointModelGroup
const robot_state::JointModelGroup* joint_model_group = move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);

接着visual_tools相关的是在介面上画图写字，ROS_INFO_NAMED则是在终端机上写字，这部分可直接参考上面的参考连结1，这边不做解释。

手臂的第一个动作使用setPoseTarget这指令做移动，给一组座标位置作移动。setPoseTarget有三种型态，本范例放入Pose，Pose内结构说明

  geometry_msgs::Pose target_pose1;           // 宣告pose
  target_pose1.orientation.w = 1.0;           // 四元数旋转
  target_pose1.position.x = 0.28;             // 设定XYZ座标
  target_pose1.position.y = -0.2;
  target_pose1.position.z = 0.5;
  move_group.setPoseTarget(target_pose1);     // 执行，此时手臂开始作动

orientation这个地方使用四元数，四元数的运算公式，後续进阶篇在解释。

第二个动作是用setJoint，被关节角度移动手臂(正向运动学)，只需要vector给角度。

  moveit::core::RobotStatePtr current_state = move_group.getCurrentState();         // 
  std::vector<double> joint_group_positions;                                        // 定义一组double
  current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);// 当前位置给入joint_group_positions
  joint_group_positions[0] = -1.0;                                                 // 给角度，单位:radians
  move_group.setJointValueTarget(joint_group_positions);                           // 执行

以上是给予角度让手臂移动，可以自行定义角度，不一定要使用Current State，可以自己定义角度值。

第三个动作，给一组座标位置作移动(逆向运动学)但是给予限制条件，会用到第一组动作的指令

  moveit_msgs::OrientationConstraint ocm;                    // 宣告
  ocm.link_name = "panda_link7";                             // 要被限制的座标
  ocm.header.frame_id = "panda_link0";                       // 要限制的原始座标
  ocm.orientation.w = 1.0;                                   // 开始定义方向、xyz误差量
  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);           // 放入限制条件进入这个group内

  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);   // 逆向运动学求解，此时的joint_model_group会被计算成start_pose2的解答
  move_group.setStartState(start_state);                   // 设定起始位置，也就是说可以不用现在手臂的位置做起点

  move_group.setPoseTarget(target_pose1);                  // 有这指令才能画出Path，做出路径规划

  move_group.setPlanningTime(10.0);

这个范例，是在示范Constraints这样的运动拘束，也就是说可以用此功能做出orientation指向某方向的移动。
接着使用到setStartState定义一个起始点，需要这起始点是因为现在手臂的位置，并不好无法符合拘束条件，所以定义一个起点。
因为给的是座标所以使用setFromIK，再用setStartState写入这个位置，之後的setPoseTarget，就会做出:start_pose2到target_pose1，然後符合拘束条件的Path。(後续在运动学的文章探讨)

第四个动作是直线移动，与第一个范例一样给座标，但差别是移动的路径是直线。

    geometry_msgs::Pose target_pose3 = move_group.getCurrentPose().pose;  // 宣告一个pose，取出现在手臂的pose

    std::vector<geometry_msgs::Pose> waypoints;                          // 塞入pose用的
    waypoints.push_back(target_pose3);

    target_pose3.position.z -= 0.2;                                      // 修改pose位置
    waypoints.push_back(target_pose3);

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

    target_pose3.position.z += 0.2;
    target_pose3.position.y += 0.2;
    target_pose3.position.x -= 0.2;
    waypoints.push_back(target_pose3);

    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(waypoints, eef_step, jump_threshold, trajectory);

target_pose3这个座标是从getCurrentPose取出来的，所以这范例手臂的移动从当前的位置开始做直线移动到watpoints中的点。要注意的是这功能的pose是可以改变的，也就是说第一点可以设定pose朝下，其余的点朝其他方向，而手臂移动路径是直线，所以pose可以自行定义但要注意奇异点。
这个范例的画图功能是乱画的，只是把上述三个点连接起来，并不是依据trajectory画图，至於这个trajectory可以使用move_group.execute(trajectory);去执行所产生的路径。

第五个范例是手臂避障，在Rviz空间中产生障碍物，然後手臂会避开障碍物抵达target。逐行程序说明可参考开头的参考一这里不赘述。这里提供额外的说明。

collision_objects.push_back(collision_object)                      // 避障运算是依据这一行
planning_scene_interface.addCollisionObjects(collision_objects);   // 将障碍物画在图上

Kinetic版本避障的范例比较简陋，最新版本的程序码比较完整，留到下回合进接篇说明。

结论:这范例示范了五个手臂的动作，但是都是单一动作不是连贯的动作，要将以上的组合成一套完整的动作，还需要尝试。整体来说第一、二、四范例比较实用比较适合应用在手臂上，其余的需要了解moveit本身的运算才能熟悉。