MoveIt编程实现关节空间机械臂运动(正运动学)

之前对MoveIt的印象一直停留在使用Rviz拖动机械臂模型，然后再点击“plan”实现轨迹的规划，点击“execute”执行机械臂的运动这种比较浅层的功能。实际我们在控制机械臂运动的时候大都是通过编程的方式控制，而不是Rviz的图形化控制。 在MoveIt中有三个主要的控制接口，如图所示，对机械臂进行控制。
这里对一个简单的成需先进行一下解析，一步步的进行难度增加学习。 使用的是probot机械臂模型，在关节空间下。首先看一下 正向运动学规划的例程，正向运动学规划简单的说就是直接给机械臂提供所有关节的目标角度从而实现的机械臂运动。 运行一下例程：  

roslaunch probot_anno_moveit_config demo.launch

rosrun probot_demo moveit_fk_demo.py

  运行效果：  

  看一下源文件：  

#moveit_fk_demo.py

import rospy, sys
import moveit_commander

class MoveItFkDemo:
    def __init__(self):

        # 初始化move_group的API,出现roscpp是因为底层是通过C++进行实现的
        moveit_commander.roscpp_initialize(sys.argv)

        # 初始化ROS节点，节点名为'moveit_fk_demo'
        rospy.init_node('moveit_fk_demo', anonymous=True)       

        # 初始化需要使用move group控制的机械臂中的arm group
        arm = moveit_commander.MoveGroupCommander('manipulator')

        # 设置机械臂运动的允许误差值，单位弧度
        arm.set_goal_joint_tolerance(0.001)

        # 设置允许的最大速度和加速度，范围0~1
        arm.set_max_acceleration_scaling_factor(0.5)
        arm.set_max_velocity_scaling_factor(0.5)

        # 控制机械臂先回到初始化位置，home是setup assistant中设置的
        arm.set_named_target('home')
        arm.go()  #让机械臂先规划，再运动，阻塞指令，直到机械臂到达home后再向下执行
        rospy.sleep(1)

        # 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）
        joint_positions = [0.391410, -0.676384, -0.376217, 0.0, 1.052834, 0.454125]
        arm.set_joint_value_target(joint_positions)  #设置关节值作为目标值

        # 控制机械臂完成运动
        arm.go()   //规划+执行
        rospy.sleep(1)

        # 控制机械臂先回到初始化位置
        arm.set_named_target('home')
        arm.go()
        rospy.sleep(1)

        # 关闭并退出moveit
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)

if __name__ == "__main__":
    try:
        MoveItFkDemo()
    except rospy.ROSInterruptException:
        pass

  其中，初始化节点：  

rospy.init_node('my_node_name', anonymous=True)

  init_node()函数需要提供一个节点名，必须要是唯一的节点名称。 如果不太关心节点的唯一性情况下，可以设置anonymous=True。 同样的程序，用C++实现以及代码解读：  

#include <ros/ros.h>
#include <moveit/move_group_interface/move_group_interface.h>

int main(int argc, char **argv)  //主函数
{
    //ros初始化节点，节点名为moveit_fk_demo
    ros::init(argc, argv, "moveit_fk_demo");
    //多线程
    ros::AsyncSpinner spinner(1);
    //开启新的线程
    spinner.start();

    //初始化需要使用move group控制的机械臂中的arm group
    moveit::planning_interface::MoveGroupInterface arm("manipulator");
    //设置机械臂运动的允许误差
    arm.setGoalJointTolerance(0.001);
    //设置允许的最大速度和加速度
    arm.setMaxAccelerationScalingFactor(0.5);
    arm.setMaxVelocityScalingFactor(0.5);

    // 控制机械臂先回到初始化位置
    arm.setNamedTarget("home");
    arm.move(); //规划+运动
    sleep(1);

    //定义一个数组，存放6个关节的信息
    double targetPose[6] = {0.391410, -0.676384, -0.376217, 0.0, 1.052834, 0.454125};
    //关节的向量，赋值
    std::vector<double> joint_group_positions(6);
    joint_group_positions[0] = targetPose[0];
    joint_group_positions[1] = targetPose[1];
    joint_group_positions[2] = targetPose[2];
    joint_group_positions[3] = targetPose[3];
    joint_group_positions[4] = targetPose[4];
    joint_group_positions[5] = targetPose[5];

    //将关节值写入
    arm.setJointValueTarget(joint_group_positions);
    arm.move(); //规划+移动
    sleep(1);

    // 控制机械臂先回到初始化位置
    arm.setNamedTarget("home");
    arm.move();
    sleep(1);

    //关闭并退出
    ros::shutdown(); 

    return 0;
}

  其中，开辟多线程：  

ros::AsyncSpinner spinner(1);

  对于一些只订阅一个话题的简单节点来说，我们使用ros::spin()进入接收循环，每当有订阅的话题发布时，进入回调函数接收和处理消息数据。但是更多的时候，一个节点往往要接收和处理不同来源的数据，并且这些数据的产生频率也各不相同，当我们在一个回调函数里耗费太多时间时，会导致其他回调函数被阻塞，导致数据丢失。这种场合需要给一个节点开辟多个线程，保证数据流的畅通。它有start() 和stop() 函数，并且在销毁的时候会自动停止。