实践三次样条插补
以前有做过moveit控制舵机机械臂的测试，记了些水文。当时觉得moveit规划的点有点少，运行时有抖动，觉得反正是玩玩，实际应用应该插值处理，于是没有深入。前不久有网友看了我的水文，做跳舞的机器人手臂用，涉及到运动抖动这个问题。我就查查如何能平滑的插值，于是看到了前辈古月老师的文章.http://www.guyuehome.com/752?replytocom=48315
从文章可以看出，三次样条插补能得到平滑的路径，平滑的速度。
看数据图，x方向是时间，y方向是位置
给定离散的点，能拟合出平滑的路径。
重点是还有源码。
输入moveit的规划所得离散的轨迹点，输出一个平滑的曲线来控制我又吃了半年土的超级无敌舵机机械臂，想必效果不错。
不实践一下实在手痒痒。
复习一下，看看moveit 输出的轨迹数据消息
http://docs.ros.org/api/trajectory_msgs/html/msg/JointTrajectory.html
std_msgs/Header header
string[] joint_names
trajectory_msgs/JointTrajectoryPoint[] points包含了关节名称数组 和轨迹点的数组。
看看轨迹点数组的内涵：
http://docs.ros.org/jade/api/trajectory_msgs/html/msg/JointTrajectoryPoint.html
File: trajectory_msgs/JointTrajectoryPoint.msg
Raw Message Definition
float64[] positions
float64[] velocities
float64[] accelerations
float64[] effort
duration time_from_start说每个轨迹点都有 positions[, velocities[, accelerations]] or positions[, effort]
排序与JointTrajectory.msg中joint names的顺序一致。
时段 time_from_start ，
有些含糊
https://answers.ros.org/question/50610/the-meaning-of-velocities-accelerations-and-time_from_start-in-jointtrajectorypointmsg/
time_from_start 相对于 trajectory.header.stamp
我猜是说以当前数据中的加速度运行相应时段的时间，就能达到速度和指定位置？或者以数据中的速度运行相应时间段就能到指定位置？要到达指定的位置是必须的啦。
拿以前最简单的实例进行修补试试。
https://blog.csdn.net/qq_38288618/article/details/78078514
这个简单的下发测试实例，actionserver回调处理中使用const control_msgs::FollowJointTrajectoryGoalConstPtr msg 获取轨迹数据。
void goalCB(const control_msgs::FollowJointTrajectoryGoalConstPtr msg)
{
//cout<<((*msg))<<endl;
std::vector<std::string> joint_names=(*msg).trajectory.joint_names;
//
setJointStateName( joint_names);
std::vector<std::string>::iterator it;
int ids [joint_names.size()];
int i=0;
for ( it = joint_names.begin(); it != joint_names.end(); it++,i++){
ids[i]=MotoName_Id[(*it)];
cout <<MotoName_Id[(*it)] <<endl;
}
//goal=(*msg);goal.trajectory.points;//c++ how to use this style??
std::vector<trajectory_msgs::JointTrajectoryPoint> points = (*msg).trajectory.points;
std::vector<trajectory_msgs::JointTrajectoryPoint>::iterator pointsit;
ros::Rate rate(10);//10hz
size_t t=points.size();
ROS_INFO("%s: goalCB ", action_name_.c_str());
for ( pointsit = points.begin(); pointsit != points.end(); pointsit++){
//cout<<(*pointsit)<<endl;
//cout <<endl ;
//here send datamsg to hardware node,command motors run.
//
publishMotorState(ids,(*pointsit).positions);
//wait
rate.sleep();
//then update joinstates an publish
setJointStatePosition((*pointsit).positions);
publishJointState();
//feedback_.
//as_.publishFeedback(feedback_);
ROS_INFO("left position :%d", (int)t);
t--;
}
// accept the new goal
//as_.acceptNewGoal();
if(as_.isActive())as_.setSucceeded();
}
就从这里改了
首先需要把数据格式整理一下
古月老师源码要输入的是 两个数组，x时间，y位置
因此需要把轨迹中每个关节的所有位置取出来放到相应的数组里。
时间点的数组可以共用一个。
写一个类来处理。
cubicSplineData.h
扫描二维码关注公众号，回复：
8873346 查看本文章
#ifndef _CUBIC_SPLINE_DATA_H_
#define _CUBIC_SPLINE_DATA_H_
#include <vector>
#include <zzz_arm_2_control_driver/cubicSpline.h>
class cubicSplineData
{
public:
cubicSplineData();
~cubicSplineData();
void initParam();
void releaseMem();
bool loadData(void **pointData,int row_count,int col_count,double *timeData);
bool getPosiByTime(double &x_in,std::vector< std::vector<double> >&result);
protected:
std::vector<cubicSpline *> cs;
int row_count,col_count;
};
#endif /* _CUBIC_SPLINE_DATA_H_ */
cubicSplineData.cpp
#include <string.h>
#include <stddef.h>
#include <stdio.h>
#include <zzz_arm_2_control_driver/cubicSplineData.h>
#include <zzz_arm_2_control_driver/cubicSpline.h>
#include <iostream>
using namespace std ;
cubicSplineData::cubicSplineData()
{
}
cubicSplineData::~cubicSplineData()
{
releaseMem();
std::vector<cubicSpline *>::iterator it_cs;
for ( it_cs = cs.begin(); it_cs != cs.end(); it_cs++){
delete (*it_cs);
}
}
void cubicSplineData::initParam()
{
row_count = row_count = 0;
}
void cubicSplineData::releaseMem()
{
initParam();
}
bool cubicSplineData::loadData(void **pointData,int row_count,int col_count,double *timeData){
for(int i=0;i<row_count;i++){
double jointData[col_count];
cout<<i<<": ";
for(int j=0;j<col_count;j++){
jointData[j]=*((double *)pointData+i*col_count+j);//pointData[i][j];
cout<<jointData[j]<<" ,";
}
cout<<"\n";
cubicSpline *spline =new cubicSpline;
spline->loadData(timeData, jointData, col_count, 0, 0, cubicSpline::BoundType_First_Derivative);
cs.push_back(spline);
cout<<"zzz-----------------"<<i<<"\n";
}
return true;
}
bool cubicSplineData::getPosiByTime(double &x_in, std::vector< std::vector< double > >&result){
//std::vector<std::vector<double>> result;
std::vector<cubicSpline *>::iterator it_cs;
for ( it_cs = cs.begin(); it_cs != cs.end(); it_cs++){
double p,v,a;
std::vector<double> point;
(*it_cs)->getYbyX(x_in,p,v,a);
point.push_back(p);
point.push_back(v);
point.push_back(a);
result.push_back(point);
}
return true;
}
#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <trajectory_msgs/JointTrajectory.h>
#include <std_msgs/Float64.h>
#include <iostream>
#include <vector>
#include <string>
#include <sensor_msgs/JointState.h>
#include <map>
//#include "zzz_arm_2_control_driver/joint_msg.h"
#include <zzz_arm_2_control_driver_msgs/joint_msg.h>
#include <zzz_arm_2_control_driver/cubicSpline.h>
#include <zzz_arm_2_control_driver/cubicSplineData.h>
using namespace std ;
typedef actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> Server;
class RobotTrajectoryFollower
{
protected:
ros::NodeHandle nh_;
// NodeHandle instance must be created before this line. Otherwise strange error may occur.
//actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> as_;
std::string action_name_;
ros::Publisher joint_pub ;
ros::Publisher joint_motor_pub ;
sensor_msgs::JointState joint_state;
zzz_arm_2_control_driver_msgs::joint_msg joint_motor_msg;
control_msgs::FollowJointTrajectoryResult result_;
control_msgs::FollowJointTrajectoryActionGoal agoal_;
control_msgs::FollowJointTrajectoryActionFeedback afeedback_;
control_msgs::FollowJointTrajectoryFeedback feedback_;
public:
map< string, int > MotoName_Id;
RobotTrajectoryFollower(std::string name) :
nh_("~"),
as_(nh_, name,boost::bind(&RobotTrajectoryFollower::goalCB, this, _1), false),
action_name_(name)
{
/*<!--
shoulder_zhuan_joint:1
upper_arm_joint:2
fore_arm_joint:3
hand_wan_joint:4
hand_zhuan_joint:5 -->*/
nh_.param("shoulder_zhuan_joint", MotoName_Id["shoulder_zhuan_joint"], 0);
nh_.param("upper_arm_joint", MotoName_Id["upper_arm_joint"], 1);
nh_.param("fore_arm_joint", MotoName_Id["fore_arm_joint"], 2);
nh_.param("hand_wan_joint", MotoName_Id["hand_wan_joint"], 3);
nh_.param("hand_zhuan_joint", MotoName_Id["hand_zhuan_joint"], 4);
joint_pub = nh_.advertise<sensor_msgs::JointState>("/move_group/fake_controller_joint_states", 1);
joint_motor_pub = nh_.advertise<zzz_arm_2_control_driver_msgs::joint_msg>("/arm_motors", 1000);
//Register callback functions:
//as_.registerGoalCallback(boost::bind(&RobotTrajectoryFollower::goalCB, this,_1));
as_.registerPreemptCallback(boost::bind(&RobotTrajectoryFollower::preemptCB, this));
as_.start();
}
~RobotTrajectoryFollower(void)//Destructor
{
}
//
void setJointStateName(std::vector<std::string> joint_names){
joint_state.name.resize(joint_names.size());
joint_state.name.assign(joint_names.begin(), joint_names.end());
//joint_state.name[0] ="arm_1_to_arm_base";
//std::vector<std::string>::iterator it;
//for ( it = joint_state.name.begin(); it != joint_state.name.end(); it++){
// cout <<(*it) <<endl;
//}
//cout <<endl ;
}
void setJointStatePosition(std::vector<double> joint_posi){
joint_state.position.resize(joint_posi.size());
joint_state.position.assign(joint_posi.begin(), joint_posi.end());
//joint_state.position[0] = base_arm;
//std::vector<double>::iterator it;
//for ( it = joint_state.position.begin(); it != joint_state.position.end(); it++){
// cout <<(*it) <<endl;
//}
//cout <<endl ;
}
void publishJointState(){
joint_state.header.stamp = ros::Time::now();
joint_pub.publish(joint_state);
}
void publishMotorState(int ids[],std::vector<double> joint_posi){
std::vector<double>::iterator it;
int i=0;
for ( it = joint_posi.begin(); it != joint_posi.end(); it++,i++){
joint_motor_msg.id=ids[i];
joint_motor_msg.r=(*it) ;
joint_motor_pub.publish(joint_motor_msg);
cout <<joint_motor_msg <<endl;
}
}
void goalCB(const control_msgs::FollowJointTrajectoryGoalConstPtr msg)
{
//cout<<((*msg))<<endl;
std::vector<std::string> joint_names=(*msg).trajectory.joint_names;
//
setJointStateName( joint_names);
std::vector<std::string>::iterator it;
int ids [joint_names.size()];
int i=0;
for ( it = joint_names.begin(); it != joint_names.end(); it++,i++){
ids[i]=MotoName_Id[(*it)];
cout <<MotoName_Id[(*it)] <<endl;
}
double x_out = 0;
double y_out = 0;
/*//------------------
double x_data[12] = {0, 0.422955130, 0.598557636, 0.734591320, 0.850603738, 0.953558869, 1.056514000, 1.159469131, 1.274332912, 1.409208218, 1.585026197, 2};
double y_data[12] = {0, 0.14881055128822188, 0.2976136037517004, 0.4464166562151788, 0.5952197086786574, 0.7440227611421358, 0.8928258136056142, 1.0416288660690929, 1.1904319185325714, 1.3392349709960498, 1.4880380234595283, 1.6368410759230068};
double vel = 0;
double acc = 0;
cubicSpline spline;
spline.loadData(x_data, y_data, 12, 0, 0, cubicSpline::BoundType_First_Derivative);
x_out = -0.004;
for(int i=0; i<=10; i++)
{
x_out = x_out + 0.004;
spline.getYbyX(x_out, y_out,vel,acc);
//printf("%f, %0.9f \n", x_out, y_out);
}
//--------------------*/
//goal=(*msg);goal.trajectory.points;//c++ how to use this style??
std::vector<trajectory_msgs::JointTrajectoryPoint> points = (*msg).trajectory.points;
std::vector<trajectory_msgs::JointTrajectoryPoint>::iterator pointsit;
ros::Rate rate(10);//10hz
size_t t=points.size();
int h=joint_names.size();//(int)(*pointsit).positions.size();
double time_data[t];
double posi_data[h][t];
int j=0;
//points[][];
double stime;
for ( pointsit = points.begin(); pointsit != points.end(); pointsit++,j++){
ros::Duration d=(*pointsit).time_from_start;
stime= d.sec+d.nsec/1000000000.0;
time_data[j]=stime;
std::vector<double> joint_posi=(*pointsit).positions;
std::vector<double>::iterator it;
int i=0;
for ( it = joint_posi.begin(); it != joint_posi.end(); it++,i++){
posi_data[i][j]=(*it);
}
cout<<j<<" ---------------------";
cout<<"point vel acc:----------\n";
std::vector<double> joint_velo=(*pointsit).velocities;
std::vector<double>::iterator it_v;
for ( it_v = joint_velo.begin(); it_v != joint_velo.end(); it_v++){
cout<<(*it_v)<<",";
}
cout<<"\n";
std::vector<double> joint_acce=(*pointsit).accelerations;
std::vector<double>::iterator it_a;
for ( it_a = joint_acce.begin(); it_a != joint_acce.end(); it_a++){
cout<<(*it_a)<<",";
}
cout<<"\n";
}
cout<<"time:----------\n";
for(int jj=0;jj<t;jj++){
cout<<time_data[jj]<<" ,";
}
cout<<"\n";
cout<<"posi:----------\n";
for(int ii=0;ii<h;ii++){
cout<<ii<<": ";
for(int jj=0;jj<t;jj++){
cout<<posi_data[ii][jj]<<" ,";
}
cout<<"\n";
}
cubicSplineData csd;
csd.loadData((void **)posi_data,h,t,time_data);
//-------------------------------------------------------------
int steps=t*5;
double sleepTime=stime/steps;
//*
x_out = -sleepTime;
for(int i=0; i<=steps; i++)
{
x_out = x_out + sleepTime;
std::vector< std::vector<double> > result;
std::vector< std::vector<double> >::iterator ir;
cout<<"=========1\n";
csd.getPosiByTime(x_out, result);
cout<<"=========2\n";
vector< double > ps;
for ( ir = result.begin(); ir != result.end(); ir++){
ps.push_back((*ir)[0]);
cout<<x_out<<": "<< (*ir)[0]<< ","<<(*ir)[1]<< ","<<(*ir)[2]<<"\n";
}
rate.sleep();
setJointStatePosition(ps);
publishJointState();
//printf("%f, %0.9f \n", x_out, y_out);
}
//*/
ROS_INFO("%s: goalCB ", action_name_.c_str());
/* for ( pointsit = points.begin(); pointsit != points.end(); pointsit++){
//cout<<(*pointsit)<<endl;
//cout <<endl ;
//here send datamsg to hardware node,command motors run.
//
publishMotorState(ids,(*pointsit).positions);
//wait
rate.sleep();
//then update joinstates an publish
setJointStatePosition((*pointsit).positions);
publishJointState();
//feedback_.
//as_.publishFeedback(feedback_);
ROS_INFO("left position :%d", (int)t);
t--;
}*/
// accept the new goal
//as_.acceptNewGoal();
if(as_.isActive())as_.setSucceeded();
}
void preemptCB()
{
ROS_INFO("%s: Preempted", action_name_.c_str());
// set the action state to preempted
if(as_.isActive()){
as_.setPreempted();
}
}
Server as_;
};
int main(int argc, char** argv)
{
ros::init(argc, argv, "arm2_jointcontroller");
RobotTrajectoryFollower RobotTrajectoryFollower("/zzz_arm/zzz_arm_controller/follow_joint_trajectory");
;
ROS_INFO("-------------zzz joint controller is running .");
//test
/*
double x_data[12] = {0, 0.422955130, 0.598557636, 0.734591320, 0.850603738, 0.953558869, 1.056514000, 1.159469131, 1.274332912, 1.409208218, 1.585026197, 2};
double y_data[12] = {0, 0.14881055128822188, 0.2976136037517004, 0.4464166562151788, 0.5952197086786574, 0.7440227611421358, 0.8928258136056142, 1.0416288660690929, 1.1904319185325714, 1.3392349709960498, 1.4880380234595283, 1.6368410759230068};
double x_out = 0;
double y_out = 0;
cubicSpline spline;
spline.loadData(x_data, y_data, 12, 0, 0, cubicSpline::BoundType_First_Derivative);
x_out = -0.004;
for(int i=0; i<=500; i++)
{
x_out = x_out + 0.004;
spline.getYbyX(x_out, y_out);
//printf("%f, %0.9f \n", x_out, y_out);
}
*/
ros::spin();
return 0;
}
古月老师源码改动部分（只是把速度和加速度输出）
bool cubicSpline::getYbyX(double &x_in, double &y_out,double &vel,double &acc)
{
int klo,khi,k;
klo=0;
khi=sample_count_-1;
double hh,bb,aa;
// 二分法查找x所在区间段
while(khi-klo>1)
{
k=(khi+klo)>>1;
if(x_sample_[k]>x_in)
khi=k;
else
klo=k;
}
hh=x_sample_[khi]-x_sample_[klo];
aa=(x_sample_[khi]-x_in)/hh;
bb=(x_in-x_sample_[klo])/hh;
y_out=aa*y_sample_[klo]+bb*y_sample_[khi]+((aa*aa*aa-aa)*M_[klo]+(bb*bb*bb-bb)*M_[khi])*hh*hh/6.0;
//////test
//double acc = 0, vel = 0;
acc = (M_[klo]*(x_sample_[khi]-x_in) + M_[khi]*(x_in - x_sample_[klo])) / hh;
vel = M_[khi]*(x_in - x_sample_[klo]) * (x_in - x_sample_[klo]) / (2 * hh)
- M_[klo]*(x_sample_[khi]-x_in) * (x_sample_[khi]-x_in) / (2 * hh)
+ (y_sample_[khi] - y_sample_[klo])/hh
- hh*(M_[khi] - M_[klo])/6;
printf("%0.9f, %0.9f, %0.9f\n",y_out, vel, acc);
//cout<<y_out;
//////test end
return true;
}
有了这个算法就可以输出平滑细腻的曲线点了。
c++不熟练，使用vector参考的内容
关于向 std::vector 中插入自定义对象的一些问题
http://blog.sina.com.cn/s/blog_a2a6dd380102wr3y.html
https://bbs.csdn.net/topics/380173642
stl vector存放含指针的类的对象?
https://bbs.csdn.net/topics/330192997/
最终vector里存的指针。
