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 Groupstatic const std::string PLANNING_GROUP = "panda_arm";// 宣告一个MoveGroupInterface，也就是等等来执行运动学、抓取工件等等moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);// 建立场景，在这环境产生障碍物moveit::planning_interface::PlanningSceneInterface planning_scene_interface;// 指标指向getJointModelGroupconst 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本身的运算才能熟悉。