实验二 轨迹规划

没有给轨迹时间，自己随便给了个

实验二 轨迹生成实验
一、实验目的
1、巩固机械臂轨迹的五次多项式插值方法
2、掌握通过速度控制使机械臂按预期的轨迹去运动到指定位置
二、实验内容
1、对机械臂轨迹进行插值
2、通过插值的多项式以速度控制的方式实现机械臂末端路径控制
三、实验步骤
1、给定四个机械臂坐标系中的空间路径点位姿：
第一个点：[0.2289, 0, 0.454, 1.57, 0, 0]（初始门位置）
第二个点：[0.3, 0.25, 0.322, 1.57, -1.57, 0]
第三个点：[0.3, 0.1, 0.172, 1.57, -1.57, 0]
第四个点：[0.3, -0.1, 0.122, 1.57, -1.57, 0]
（第一个点为起点、第四个点为终点，第二和第三个点为必须经过的中间点）。
2、编写轨迹生成代码，通过速度控制使机械臂末端依次经过第二和第三路径点到达第四路径点停止。经过第二和第三点时不可停顿，末端经过这两点时的速度和加速度可以自行设定。
四、评定标准
1、是否按照要求带速度经过了第二和第三点
2、能否到四个路径点准确停止，相对误差的大小。

#include <string>
#include <ros/ros.h>
#include <iostream>
#include <time.h>
#include "vector"
#include "std_msgs/Float32.h"
#include "std_msgs/Float64MultiArray.h"
#include "controller_manager_msgs/SwitchController.h"
#include "controller_manager_msgs/ListControllers.h"
#include "ikfast.h"
#include "probot_anno_manipulator_ikfast_moveit_plugin.cpp"
#include "time.h"
#include <cmath>
#include <unistd.h>

using namespace ikfast_kinematics_plugin;

int main(int argc, char * argv[])
{
	bool ret;
	ros::init(argc, argv, "path_planning");
    	ros::NodeHandle node_handle;
    	ros::AsyncSpinner spinner(1);
    	ROS_INFO_STREAM("start");

	ros::Publisher vel_pub = node_handle.advertise<std_msgs::Float64MultiArray>("/probot_anno/arm_vel_controller/command", 100);
	ikfast_kinematics_plugin::IKFastKinematicsPlugin ik;
	ret = ik.IKFastKinematicsPlugin::initialize("robot_description","manipulator","base_link","link_6",0.001);

	const double theta[4][6]={0,0,0,0,0,0,
							0.795586,-0.772686,0.205056,1.08734,2.51046,-0.842853,
							0.387524,-1.07534,-0.199874,0.404374,2.86768,-0.114923,
							-0.387371,-1.29256,-0.0658126,2.74695,0.197329,-3.06044};
	double vel[4][6] = {0};
	double acc[4][6] = {0};
	double aver_vel[3][6] = {0};
	double aver_acc[3][6] = {0};	
	double xishu[3][6][6] = {0};
	double t[3] = {5,5,5};
	double ti = 0;
	std_msgs::Float64MultiArray init_vel;	
	
	for(int i = 0;i<3;i++)
		for(int j=0;j<6;j++)
			aver_vel[i][j] = (theta[i+1][j] - theta[i][j])/t[i];		

	for(int i = 0;i<4;i++){
		for(int j =0;j<6;j++){
			if(i == 0||i==3)vel[i][j] = 0;
			else
				vel[i][j] = (aver_vel[i-1][j]+aver_vel[i][j])/2;
		}
	} 

	for(int i = 0; i < 3 ; i++)
		for(int j = 0; j<6;j++)
			aver_acc[i][j] = (vel[i+1][j] - vel[i][j])/t[i];		
		
	for(int i = 0;i<4;i++){
		for(int j =0;j<6;j++){
			if(i==0||i==3)acc[i][j]=0;
			else
				acc[i][j] = (aver_acc[i-1][j]+aver_acc[i][j])/2;			
		}	
	}

	for(int i =0;i<3;i++){
		for(int j =0;j<6;j++){
			xishu[i][j][0] = theta[i][j];
			xishu[i][j][1] = vel[i][j];
			xishu[i][j][2] = acc[i][j]/2;
			xishu[i][j][3] = (20*(theta[i+1][j]-theta[i][j]) - (8*vel[i+1][j]+12*vel[i][j])*t[i] - (3*acc[i][j] - acc[i+1][j])*pow(t[i],2))/(2*pow(t[i],3));
			xishu[i][j][4] = (-30*(theta[i+1][j]-theta[i][j]) +(14*vel[i+1][j] +16*vel[i][j])*t[i] +(3*acc[i][j] - 2*acc[i+1][j])*pow(t[i],2))/(2*pow(t[i],4));
			xishu[i][j][5] = (12*(theta[i+1][j]-theta[i][j]) - 6*(vel[i+1][j]+vel[i][j])*t[i] -(acc[i][j]-acc[i+1][j])*pow(t[i],2))/(2*pow(t[i],5));
		}	
	}

	ros::Rate r(1000);
	std_msgs::Float64MultiArray init_vel;	
	for(int i = 0;i<6;i++)init_vel.data.push_back(0);
	for(int j = 0;j<3;j++){
		while(ros::ok()){
			if(ti < t[j])
				for(int i=0;i<6;i++)
					init_vel.data.at(i ) = 5*xishu[j][i][5]*pow(ti,4)+4*xishu[j][i][4]*pow(ti,3)+3*xishu[j][i][3]*pow(ti,2)+2*xishu[j][i][2]*ti+xishu[j][i][1];
			else break;
			vel_pub.publish(init_vel);
			ti+=0.001;
			bool met = r.sleep();
		}
		ti = 0;
		vel_pub.publish(init_vel);
		//getchar();
	}
	ROS_INFO_STREAM("published");
}