ros moveit机械臂轨迹三次插补

作者: 瀚文文文问问 | 来源:发表于2022-03-30 20:05 被阅读0次

问题引入

马上要进入研究生阶段的学习，跟导师研究机械臂的抓取问题，但本科阶段大多数都在研究一些深度学习算法，所以我准备从零开始搭建一个自己的机械臂，通过实践来掌握机械臂抓取问题的整个体系。在软件层面上我使用ros+moveit作为机械臂开发的主体，moveit可以帮助我们轻松的完成正逆解以及轨迹的规划，是非常好的工具。
moveit的输出一般是一系列的轨迹点，包含每一个关节的位置position[],速度velocities[]以及加速度accelerations[],通过action接口进行通讯，但是moveit直接输出的轨迹点数量很少并且没有进行插补，为了方便观察我将moveit输出的轨迹保存为.txt文件，使用matplotlib进行绘图如下图所示：

front_tpva.png

通过这张图可以看到moveit输出的轨迹并不圆滑，尤其是加速度曲线，这代表电机的力矩变化很大，会造成机械臂在运动的过程中抖动。

所以我打算写一个三次轨迹插补的算法对moveit输出的轨迹进行插补，本篇文章的所有程序都脱离ros和moveit，我将moveit输出的轨迹选取一个关节保存到tractory.txt文件中，文件形式如下图:

tractory.txt.png

第一行是时间，第二行是位置，第三行是速度，第四行是加速度，至于数据具体含义以及怎么使用moveit导出这个轨迹数据我在这篇文章中不去具体阐述。

为了方便自己调试也便其他小伙伴复用，本篇文章可以脱离机械臂的具体问题，看作对tractory.txt文件中的数据进行三次插补。
在介绍具体流程和代码之前我把我的工程目录放在下面。

工程目录.png

实现流程

1.读取tractory.txt文件并转化为double数组

这一部分我感觉写的有一些麻烦，因为我本身c++编程能力并不好，我本科是机械专业，之前编成用python比较多，这里谁有简单实现可以分享以下！

struct String_data
{
    string t_str;
    string pos_str;
    string vel_str; 
    string acc_str;
};

class Deal_data
{
public:


    void load_data(const char * name)
    {
        ifstream openfile;
        openfile.open(name,ios::in);
        if (!openfile.is_open())
        {
            cout << "file can't open!" << endl;
        }
        string buf;
        int count = 0;
        while(getline(openfile,buf))
        {
            if (count == 0)
            {
                str_data.t_str = buf;
                
            }
            if(count==1)
            {
                str_data.pos_str = buf;
            }
            if(count==2)
            {
                str_data.vel_str = buf;
            }   
            if(count==3)
            {
                str_data.acc_str = buf;
            }   
            count ++;   
        }
    }

    void get_point_num()
    {
        int p = 0;
        int num = 0;
        int splite_position[100];
        string t_str_inner = str_data.t_str; 
        while(t_str_inner.find(",",p)!=string::npos)
        {
            p = t_str_inner.find(",",p);
            // int型数组用来储存所有,的位置
            splite_position[num] = p;
            p = p + 1;
            num ++;
        }
        point_num = num;

    }

    double * get_data_t()
    {
        int p = 0;
        int num = 0;
        int splite_position[100];
        string t_str_inner = str_data.t_str; 
        while(t_str_inner.find(",",p)!=string::npos)
        {
            p = t_str_inner.find(",",p);
            splite_position[num] = p;
            p = p + 1;
            num ++;
        }
        char t_char[point_num];
        double * t = new double[point_num];
        for (int i=0;i<point_num;i++)
        {
            int end = splite_position[i];
            if(i==0)
            {
                // 字符串分割
                string str = t_str_inner.substr(0,end);
                // char转double,并存入数组
                t[i] = atof(str.c_str());
                // cout << t[i] << endl;
            }
            else
            {
                int start = splite_position[i-1]+1;
                string str = t_str_inner.substr(start,end - start); 
                t[i] = atof(str.c_str());
                // cout << t[i] << endl;
            }
        }
        return t;
    }

    double * get_data_pos()
    {
        int p = 0;
        int num = 0;
        int splite_position[100];
        string pos_str_inner = str_data.pos_str; 
        while(pos_str_inner.find(",",p)!=string::npos)
        {
            p = pos_str_inner.find(",",p);
            // int型数组用来储存所有,的位置
            splite_position[num] = p;
            p = p + 1;
            num ++;
        }
        char t_char[point_num];
        double * pos = new double[point_num];
        for (int i=0;i<point_num;i++)
        {
            int end = splite_position[i];
            if(i==0)
            {
                string str = pos_str_inner.substr(0,end);
                pos[i] = atof(str.c_str());
            }
            else
            {
                int start = splite_position[i-1]+1;
                string str = pos_str_inner.substr(start,end - start); 
                pos[i] = atof(str.c_str());
            }
        }
        return pos;
    }
    double * get_data_vel()
    {
        int p = 0;
        int num = 0;
        int splite_position[100];
        string vel_str_inner = str_data.vel_str; 
        while(vel_str_inner.find(",",p)!=string::npos)
        {
            p = vel_str_inner.find(",",p);
            // int型数组用来储存所有,的位置
            splite_position[num] = p;
            p = p + 1;
            num ++;
        }
        char t_char[point_num];
        double * vel = new double[point_num];
        for (int i=0;i<point_num;i++)
        {
            int end = splite_position[i];
            if(i==0)
            {
                string str = vel_str_inner.substr(0,end);
                vel[i] = atof(str.c_str());
            }
            else
            {
                int start = splite_position[i-1]+1;
                string str = vel_str_inner.substr(start,end - start); 
                vel[i] = atof(str.c_str());
            }
        }
        return vel;
    }

    double * get_data_acc()
    {
        int p = 0;
        int num = 0;
        int splite_position[100];
        string acc_str_inner = str_data.acc_str; 
        while(acc_str_inner.find(",",p)!=string::npos)
        {
            p = acc_str_inner.find(",",p);
            // int型数组用来储存所有,的位置
            splite_position[num] = p;
            p = p + 1;
            num ++;
        }
        char t_char[point_num];
        double * acc = new double[point_num];
        for (int i=0;i<point_num;i++)
        {
            int end = splite_position[i];
            if(i==0)
            {
                string str = acc_str_inner.substr(0,end);
                acc[i] = atof(str.c_str());
            }
            else
            {
                int start = splite_position[i-1]+1;
                string str = acc_str_inner.substr(start,end - start); 
                acc[i] = atof(str.c_str());
            }
        }
        return acc;
    }
    int point_num;
private:
    struct String_data str_data;

};

1.进行轨迹三次插补

插补的具体实现我参照了古月老师的方法。边界条件都为0。
cublicSpline.h头文件

#ifndef _CUBIC_SPLINE_H_
#define _CUBIC_SPLINE_H_
 
class cubicSpline
{
public:
    typedef enum _BoundType
    {
        BoundType_First_Derivative,
        BoundType_Second_Derivative
    }BoundType;
 
public:
    cubicSpline();
    ~cubicSpline();
 
    void initParam();
    void releaseMem();
 
    bool loadData(double *x_data, double *y_data, int count, double bound1, double bound2, BoundType type);
    bool getYbyX(double &x_in, double &y_out);
 
protected:
    bool spline(BoundType type);
 
protected:
    double *x_sample_, *y_sample_;
    double *M_;
    int sample_count_;
    double bound1_, bound2_;
};
#endif /* _CUBIC_SPLINE_H_ */

cublicSpine.cpp源文件

/* 三次样条插补 */
#include <string.h>
#include <stddef.h>
#include <stdio.h>
#include "cubicSpline.h"

using namespace std;
/* 初始化输入输出速度加速度 */
double acc = 0, vel = 0;
double x_out = 0, y_out = 0;

/* 三次样条无参构造 */
cubicSpline::cubicSpline()
{
}
/* 析构 */
cubicSpline::~cubicSpline()
{
    releaseMem();
}
/* 初始化参数 */
void cubicSpline::initParam()
{
    x_sample_ = y_sample_ = M_ = NULL;
    sample_count_ = 0;
    bound1_ = bound2_ = 0;
}
/* 释放参数 */
void cubicSpline::releaseMem()
{
    delete x_sample_;
    delete y_sample_;
    delete M_;
 
    initParam();
}
/* 加载关节位置数组等信息 */
bool cubicSpline::loadData(double *x_data, double *y_data, int count, double bound1, double bound2, BoundType type)
{
    if ((NULL == x_data) || (NULL == y_data) || (count < 3) || (type > BoundType_Second_Derivative) || (type < BoundType_First_Derivative))
    {
        return false;
    }
 
    initParam();
 
    x_sample_ = new double[count];
    y_sample_ = new double[count];
    M_        = new double[count];
    sample_count_ = count;
 
    memcpy(x_sample_, x_data, sample_count_*sizeof(double));
    memcpy(y_sample_, y_data, sample_count_*sizeof(double));
 
    bound1_ = bound1;
    bound2_ = bound2;
 
    return spline(type);
}
/* 计算样条插值 */
bool cubicSpline::spline(BoundType type)
{
    if ((type < BoundType_First_Derivative) || (type > BoundType_Second_Derivative))
    {
        return false;
    }
 
    //  追赶法解方程求二阶偏导数
    double f1=bound1_, f2=bound2_;
 
    double *a=new double[sample_count_];                //  a:稀疏矩阵最下边一串数
    double *b=new double[sample_count_];                //  b:稀疏矩阵最中间一串数
    double *c=new double[sample_count_];                //  c:稀疏矩阵最上边一串数
    double *d=new double[sample_count_];
 
    double *f=new double[sample_count_];
 
    double *bt=new double[sample_count_];
    double *gm=new double[sample_count_];
 
    double *h=new double[sample_count_];
 
    for(int i=0;i<sample_count_;i++)
        b[i]=2;                                //  中间一串数为2
    for(int i=0;i<sample_count_-1;i++)
        h[i]=x_sample_[i+1]-x_sample_[i];      // 各段步长
    for(int i=1;i<sample_count_-1;i++)
        a[i]=h[i-1]/(h[i-1]+h[i]);
    a[sample_count_-1]=1;
 
    c[0]=1;
    for(int i=1;i<sample_count_-1;i++)
        c[i]=h[i]/(h[i-1]+h[i]);
 
    for(int i=0;i<sample_count_-1;i++)
        f[i]=(y_sample_[i+1]-y_sample_[i])/(x_sample_[i+1]-x_sample_[i]);
 
    for(int i=1;i<sample_count_-1;i++)
        d[i]=6*(f[i]-f[i-1])/(h[i-1]+h[i]);
 
    //  追赶法求解方程
    if(BoundType_First_Derivative == type)
    {
        d[0]=6*(f[0]-f1)/h[0];
        d[sample_count_-1]=6*(f2-f[sample_count_-2])/h[sample_count_-2];
 
        bt[0]=c[0]/b[0];
        for(int i=1;i<sample_count_-1;i++)
            bt[i]=c[i]/(b[i]-a[i]*bt[i-1]);
 
        gm[0]=d[0]/b[0];
        for(int i=1;i<=sample_count_-1;i++)
            gm[i]=(d[i]-a[i]*gm[i-1])/(b[i]-a[i]*bt[i-1]);
 
        M_[sample_count_-1]=gm[sample_count_-1];
        for(int i=sample_count_-2;i>=0;i--)
            M_[i]=gm[i]-bt[i]*M_[i+1];
    }
    else if(BoundType_Second_Derivative == type)
    {
        d[1]=d[1]-a[1]*f1;
        d[sample_count_-2]=d[sample_count_-2]-c[sample_count_-2]*f2;
 
        bt[1]=c[1]/b[1];
        for(int i=2;i<sample_count_-2;i++)
            bt[i]=c[i]/(b[i]-a[i]*bt[i-1]);
 
        gm[1]=d[1]/b[1];
        for(int i=2;i<=sample_count_-2;i++)
            gm[i]=(d[i]-a[i]*gm[i-1])/(b[i]-a[i]*bt[i-1]);
 
        M_[sample_count_-2]=gm[sample_count_-2];
        for(int i=sample_count_-3;i>=1;i--)
            M_[i]=gm[i]-bt[i]*M_[i+1];
 
        M_[0]=f1;
        M_[sample_count_-1]=f2;
    }
    else
        return false;
 
    delete a;
    delete b;
    delete c;
    delete d;
    delete gm;
    delete bt;
    delete f;
    delete h;
 
    return true;
}
/* 得到速度和加速度数组 */
bool cubicSpline::getYbyX(double &x_in, double &y_out)
{
    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
    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;
    //test end
 
    return true;
}

3.main.cpp以及CMakeLists.txt

别忘了把读取tractory.txt数据的程序，放到主函数的上面，我在上面写过了就不放上去了。

#include<iostream>
#include<stdio.h>
#include<stddef.h>
#include<string.h>
#include<fstream>
#include<vector>
#include "cubicSpline.h"
double acc = 0, vel = 0;
double x_out = 0, y_out = 0;
int main()
{
    setlocale(LC_ALL,"");
    char file_name[] = "/home/zhw/slam/learn_c++/TRY_SPLINE/src/tractory.txt";
    Deal_data opf;
    opf.load_data(file_name);
    opf.get_point_num();
    int point_num = opf.point_num;
    cout << opf.point_num << endl;
    double * time = opf.get_data_t();
    double * pos = opf.get_data_pos();
    double * vel_ = opf.get_data_vel();
    double * acc_ = opf.get_data_acc();

    cubicSpline spline;
    // lumbar test
    spline.loadData(time,pos, point_num, 0, 0, cubicSpline::BoundType_First_Derivative);
    double rate = (time[point_num-1] - time[0])/(point_num*4);
    double deal_pos[point_num*4];
    double deal_vel[point_num*4];
    double deal_acc[point_num*4];
    double time_from_start_[point_num*4];
    for (int k = 0; k <= point_num*4 ; k++) {
        printf("[---位置、速度、加速度---]");
        printf("%0.9f, %0.9f, %0.9f\n",y_out, vel, acc);
        spline.getYbyX(x_out, y_out);
        time_from_start_[k] = x_out;
        deal_pos[k] = y_out;
        deal_vel[k] = vel;
        deal_acc[k] = acc;
        x_out += rate;
    }
    FILE *f;
    f = fopen("/home/zhw/slam/learn_c++/TRY_SPLINE/deal_tractory.txt","a");
    for(int j=0;j<=point_num*4;j++)
    {
        fprintf(f,"%f,",time_from_start_[j]);//6
    }
    fprintf(f,"\n");
    for(int j=0;j<=point_num*4;j++)
    {
        fprintf(f,"%f,",deal_pos[j]);//7
    }
    fprintf(f,"\n");
    for(int j=0;j<=point_num*4;j++)
    {
        fprintf(f,"%f,",deal_vel[j]);//8
    }
    fprintf(f,"\n");
    for(int j=0;j<=point_num*4;j++)
    {
        fprintf(f,"%f,",deal_acc[j]);//9
    }
    fprintf(f,"\n");
    fclose(f);

    return 0;
}

这段程序中包含了将插值后的数据保存到deal_tractory.txt文件中。

cmake_minimum_required(VERSION 3.0.2)
include_directories(${CMAKE_SOURCE_DIR}/include)
add_library(cubicSpline SHARED cubicSpline.cpp)
add_executable(main main.cpp)
target_link_libraries(main cubicSpline)

结果

最后把插值前插值后的对比图show一下！！！

deal_tpva.png

感觉整体来看变化不是很大，就是轨迹点多了一些，我这里插值了四倍，还是放到机械臂上跑一下看一下变化，这里我的机械臂还没有搭建完成，搭建完成再测试把！

ros moveit机械臂轨迹三次插补

问题引入

实现流程

1.读取tractory.txt文件并转化为double数组

1.进行轨迹三次插补

3.main.cpp以及CMakeLists.txt

结果

相关文章

网友评论

延伸阅读

深度阅读

栏目导航

热点阅读

ros moveit机械臂轨迹 三次插补

问题引入

实现流程

1.读取tractory.txt文件并转化为double数组

1.进行轨迹三次插补

3.main.cpp以及CMakeLists.txt

结果

相关文章

网友评论

延伸阅读

深度阅读

栏目导航

热点阅读

ros moveit机械臂轨迹三次插补