MoveIt轨迹规划

源码

#include <iostream>       //handeye_calibration
#include <Eigen/Eigen>
#include <stdlib.h>
#include <Eigen/Geometry>
#include <Eigen/Core>
#include <moveit/move_group_interface/move_group.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>

#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>
#include <vector>
#include <math.h>
#include <control_msgs/JointTrajectoryAction.h>
#include <trajectory_msgs/JointTrajectoryPoint.h>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>

#include "ros/ros.h"
#include "std_msgs/String.h"

#include <sstream>

using namespace std;
int success_times = 0;
bool flag = true;
int i = 0;

string my_joint_trajectory = "";

void Specified_pos(float x,float y,float z,float w,float roll,float pitch,float yaw)
{
    move_group_interface::MoveGroup group("SRD6");
    geometry_msgs::Pose Specified_pose;

    Specified_pose.position.x = x;
    Specified_pose.position.y = y;
    Specified_pose.position.z = z;

    Specified_pose.orientation.w = w;
    Specified_pose.orientation.x = roll;
    Specified_pose.orientation.y = pitch;
    Specified_pose.orientation.z = yaw;

    //group.setMaxVelocityScalingFactor(0.3);  //速度系数
    //group.setMaxAccelerationScalingFactor(0.3);//加速度系数
    group.setPoseTarget(Specified_pose);

    //moveit::planning_interface::MoveGroup::Plan planner;
    moveit::planning_interface::MoveGroup::Plan planner;
    bool is_success=group.plan(planner);
    if(is_success)
    {
      moveit_msgs::RobotTrajectory msg;
      msg = planner.trajectory_;// get the trajectory from the path [1]

      std::vector<trajectory_msgs::JointTrajectoryPoint> trajectory_points;
      trajectory_points = msg.joint_trajectory.points;  //Can be seen in [2]

      std::vector<int>::size_type size1 = msg.joint_trajectory.points.size();
      ROS_INFO("The amount of points = %ld", size1);

      std::vector<float> position;
      //position.resize(vector1);
      int k = 0;
      string joint_trajectory_pt;
      for (unsigned i=0; i<size1; i++)
      {
          std::vector<int>::size_type size2 = msg.joint_trajectory.points[i].positions.size();
          for (unsigned j=0; j<size2; j++)
          {
              k++;
              position.push_back(msg.joint_trajectory.points[i]. positions[j]); // positions[j]was eerst [0]
          }

          ros::Duration duration;
          duration = msg.joint_trajectory.points[i].time_from_start;
          //ROS_INFO("time_from_start: [ %d, %d ]", duration.sec,duration.nsec);
          float nsec_to_sec;
          float time_from_start;
          nsec_to_sec = duration.nsec*0.000000001;
          time_from_start = duration.sec + nsec_to_sec;

          stringstream ss;

          ss << "point_index: " << i << endl

             << "positions: "
             << "[" << msg.joint_trajectory.points[i].positions[0]
             << "," << msg.joint_trajectory.points[i].positions[1]
             << "," << msg.joint_trajectory.points[i].positions[2]
             << "," << msg.joint_trajectory.points[i].positions[3]
             << "," << msg.joint_trajectory.points[i].positions[4]
             << "," << msg.joint_trajectory.points[i].positions[5]
             << "]" << endl

             << "velocities: "
             << "[" << msg.joint_trajectory.points[i].velocities[0]
             << "," << msg.joint_trajectory.points[i].velocities[1]
             << "," << msg.joint_trajectory.points[i].velocities[2]
             << "," << msg.joint_trajectory.points[i].velocities[3]
             << "," << msg.joint_trajectory.points[i].velocities[4]
             << "," << msg.joint_trajectory.points[i].velocities[5]
             << "]" << endl

             << "accelerations: "
             << "[" << msg.joint_trajectory.points[i].accelerations[0]
             << "," << msg.joint_trajectory.points[i].accelerations[1]
             << "," << msg.joint_trajectory.points[i].accelerations[2]
             << "," << msg.joint_trajectory.points[i].accelerations[3]
             << "," << msg.joint_trajectory.points[i].accelerations[4]
             << "," << msg.joint_trajectory.points[i].accelerations[5]
             << "]" << endl

             << "time_from_start: "
             << "[" << time_from_start
             << "]" << endl;

          joint_trajectory_pt = joint_trajectory_pt + ss.str();

      }
      cout << joint_trajectory_pt << endl;
      my_joint_trajectory = joint_trajectory_pt;
      joint_trajectory_pt = "";

      group.move();
      success_times++;
    }

    else
    {
        cout<<"Planning fail!"<<endl;
    }
}

void hand_eye_calibration(float center_x,float center_y,float center_z,float sphere_radius)
{
   move_group_interface::MoveGroup group("SRD6");

   Eigen::Matrix<double,3,1> matrix_effect;
   Eigen::Matrix<double,3,1> matrix_sphere_center;
   Eigen::Matrix<double,3,1> matrix_obj_pt;

   Eigen::Matrix<double,3,3> ee2camera;

   Eigen::Matrix<double,3,3> matrix_rotation;
   Eigen::Vector3d base_Z(0,0,1);

    int i;
    float x,y,z;
    float azimuth,pitch;
    double obj_x,obj_y,obj_z;
    //float x_ca,y_ca,z_ca;

    Specified_pos(0.2,-0.2,1.4,0,1,0,0);    //Fixed_pos

    srand(time(NULL));
    for(i = 0;i<50;i++)
    {
      LOOP:
        cout << "i = " << i << endl;
//        cout << "success_times = " << success_times << endl;

        azimuth = (rand()/(RAND_MAX+0.1))*(2*M_PI);
        pitch = (rand()/(RAND_MAX+0.1))*(M_PI/4);

        obj_x = sphere_radius * sin(pitch)*cos(azimuth) + center_x;   //球面上的点在UR5基坐标系中的x坐标
        obj_y = sphere_radius * sin(pitch)*sin(azimuth) + center_y;   //球面上的点在UR5基坐标系中的y坐标
        obj_z = sphere_radius * cos(pitch) + center_z ;   //球面上的点在UR5基坐标系中的z坐标

        matrix_sphere_center << center_x,center_y,center_z;

        matrix_obj_pt << obj_x,obj_y,obj_z;

        //求出z轴
        Eigen::Vector3d z_axiz;
        z_axiz =  matrix_sphere_center - matrix_obj_pt;
        z_axiz.normalize(); //单位化z轴

        Eigen::Vector3d x_axiz_assit;
        x_axiz_assit << sphere_radius*cos(azimuth),sphere_radius*sin(azimuth),0;
        x_axiz_assit.normalize();

        Eigen::Vector3d x_axiz(x_axiz_assit.cross(z_axiz));
        x_axiz.normalize();

        // Terminal y axis
        Eigen::Vector3d y_axiz ;
        y_axiz = (z_axiz.cross(x_axiz));
        y_axiz.normalize();

        //得到旋转矩阵
        matrix_rotation << x_axiz,y_axiz,z_axiz;
        ee2camera << 0,-1,0,0.866026,0,-0.5,0.5,0,0.866026;

        matrix_rotation =  matrix_rotation *  ee2camera ;

        Eigen::Quaterniond q = Eigen::Quaterniond(matrix_rotation);
        q.normalize();

        matrix_effect << obj_x,obj_y,obj_z;

        //cout << "quaternion =\n" << q.coeffs() << endl; // Eigen 的存储顺序为(x,y,z,w) ,实部为w                                                 // -q.coeffs()本来是没有负号的，但是求出来跟那个软件相反，所以加了个负号，就一样了
        //cout << "siyuanshu" << q.w() << endl;

        //Specified_pos(obj_x,obj_y,obj_z,q.w(),q.x(),q.y(),q.z());

        //***joint_path_command excute and export***//
        ros::NodeHandle n;
        ros::Publisher joint_trajectory_pub = n.advertise<std_msgs::String>("joint_path_command", 10000);
        ros::Rate loop_rate(10);

        //int count = 0;
        while(ros::ok())
        {
          Specified_pos(obj_x,obj_y,obj_z,q.w(),q.x(),q.y(),q.z());

          std_msgs::String joint_path_command;

          for(int n=0;n<10;n++)
          {
            joint_path_command.data = my_joint_trajectory;
            joint_trajectory_pub.publish(joint_path_command);
            ros::spinOnce();
            loop_rate.sleep();
            //++count;
          }

          if(i<50)
          {
            i++;
            goto LOOP;
          }
          else
          {
            break;
          }
        }
    }
}

void ur5_line(float org_x,float org_y,float org_z,float goal_x,float goal_y,float goal_z)
{

  const int times = 5;

  float x_add,y_add,z_add;
  float cur_x,cur_y,cur_z;

  x_add = (goal_x-org_x)/times;
  y_add = (goal_y-org_y)/times;
  z_add = (goal_z-org_z)/times;
  for(int i=0;i<times+1;i++)
  {
    cur_x = org_x + x_add*i;
    cur_y = org_y + y_add*i;
    cur_z = org_z + z_add*i;
    Specified_pos(cur_x,cur_y,cur_z,1,0,0,0);
    cout << "cur_x =" << cur_x << endl;
    cout << "cur_y =" << cur_y << endl;
    cout << "cur_z =" << cur_z << endl;
  }

}
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();

    hand_eye_calibration(0.2,0.2,0.7,0.5);

    //Specified_pos(0.5,0.3,1.0,0,1,0,0);
//    ur5_line(-0.5,0.5,1.0,0.5,0.5,1.0);

  ros::shutdown();
  return 0;
}