ROS下通过MoveIt控制UR5机器人的运动

一.MoveIt简介

MOVEit！是目前针对移动操作最先进的软件。
它结合了运动规划，操纵，三维感知，运动学，控制和导航的最新进展
它提供了一个易于使用的平台，开发先进的机器人应用程序，评估新的机器人设计和建筑集成的机器人产品
它广泛应用于工业，商业，研发和其他领域。
MOVEit！是最广泛使用的开源软件的操作，并已被用于超过65个机器人

其框架如下图所示:

具体解析可以见：http://www.ncnynl.com/archives/201610/1028.html

二.代码实现

在这个代码中，主要实现的是操控UR5在俩个pose之间做直线运动，由于我们使用的MoveIt在实现点到点的时候并不能完全按照固定的轨迹进行运动，所以我们只能将俩个Pose进行离散，如下图所示：如果我们需要通过MoveIt把UR5从Pose1移动到Pose2，如果要走直线，那么需要将Pose1和Pose2之间进行离散（正方形表示）

代码如下：

#include   //MoveIt的头文件，API接口
#include 
#include 
#include 
#include 
#include 
#include 

using namespace std;
void dispersed(double pt1[3],double pt2[3]);        

int main(int argc, char **argv)
{
    ros::init(argc, argv, "move_group_interface_demo", ros::init_options::AnonymousName);
    // 创建一个异步的自旋线程（spinning thread）
    ros::AsyncSpinner spinner(1);
    spinner.start();
    // 连接move_group节点中的机械臂实例组，这里的组名arm是我们之前在setup assistant中设置的

    // 假设我有俩个点pt1和pt2，我们需要将机械臂的末端从pt1移动到pt2
    double pt1[3]={0.60,-0.379,1.02};  
    double pt2[3]={0.70,-0.379,1.02};
    dispersed(pt1,pt2);
  ros::shutdown();
  return 0;
}

void dispersed(double pt1[3],double pt2[3])
{
    move_group_interface::MoveGroup group("manipulator");

    double Xf;
    double org_x,org_y,org_z;
    double crt_x;
    org_y=pt1[1];
    org_z=pt1[2];
    org_x = pt1[0];
    int xtimes=50;          //X轴直线离散化倍率
    Xf=(pt2[0]-pt1[0])/xtimes;   //X轴进给量
    cout << " xtimes = " << xtimes << endl;
    for(int x=0;x<=xtimes;x++)
    {
        cout << "x=" << x << endl;
        crt_x = org_x + x*Xf;
        geometry_msgs::Pose target_pose;
        target_pose.position.x = crt_x; //位姿
        target_pose.position.y = org_y;
        target_pose.position.z = org_z;
        cout << "target_pose.position.x = " << target_pose.position.x << endl;
        target_pose.orientation.w = 0;   //四元素
        target_pose.orientation.x = 1;
        target_pose.orientation.y = 0;
        target_pose.orientation.z = 0;
        group.setPoseTarget(target_pose);  
        //group.move();
        moveit::planning_interface::MoveGroup::Plan planner;
        bool is_success=group.plan(planner);
        if(is_success)
        {
            group.move();
            sleep(1);
        }
       else
        {
            cout<<"Planning fail!"<

参考：http://docs.ros.org/indigo/api/moveit_tutorials/html