ROS下工业机器人的运动规划及轨迹规划

仿真过程中若rviz窗口抖动，则点击窗口最大化按钮使其窗口非最大话即可消除窗口抖动

1、利用moveit结合industrial_robot_client包提供的joint_trajectory_action发布joint_path_command数据，由于moveit规划的路径加速度存在较大抖动，故需利用三次样条曲线进行平滑处理后，连同各个关节位置、速度、加速度打包发送给控制器；最后利用控制器控制机械臂完成各个点之间插补；

2、启动launch文件

3、脉络框架

4、脉络详细

#include        //handeye_calibration
#include 
#include 
#include 
#include 
#include 
#include 

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

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

#include 

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("SRF6");
    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_points;
      trajectory_points = msg.joint_trajectory.points;  //Can be seen in [2]

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

      std::vector position;
      //position.resize(vector1);
      int k = 0;
      string joint_trajectory_pt;
      for (unsigned i=0; i::size_type size2 = msg.joint_trajectory.points[i].positions.size();
          for (unsigned j=0; j matrix_effect;
   Eigen::Matrix matrix_sphere_center;
   Eigen::Matrix matrix_obj_pt;

   Eigen::Matrix ee2camera;

   Eigen::Matrix 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("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 = 20;

  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