esdgfhjh

最新推荐文章于 2024-11-16 19:19:32 发布

Zenker0418

最新推荐文章于 2024-11-16 19:19:32 发布

阅读量32

点赞数

文章标签： python

本文链接：https://blog.csdn.net/Zenker0418/article/details/134435770

版权

#include “mainwindow.h”
#include <sys/time.h>
#include <eigen3/Eigen/Geometry>
#include <eigen3/Eigen/Dense>
#include
#include
#include <stdio.h>
#include
using namespace std;
void fErrorJacobian_z4(double theta[6], double d[6], double a[6], double alpha[6], double **Jacobian_Z)
{

Eigen::Matrix4d dAA;
Eigen::Matrix4d A[6 + 1];

for(int j = 1; j <= 6; j++){
dAA<<1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1;
for(int i=1; i<=6; i++){
if(i==j){
A[i](0, 0) = -sin(theta[i-1]);
A[i](0, 1) = -cos(alpha[i-1])*cos(theta[i-1]);
A[i](0, 2) = sin(alpha[i-1])*cos(theta[i-1]);
A[i](0, 3) = -a[i-1]*sin(theta[i-1]);
A[i](1, 0) = cos(theta[i-1]);
A[i](1, 1) = -cos(alpha[i-1])*sin(theta[i-1]);
A[i](1, 2) = sin(alpha[i-1])*sin(theta[i-1]);
A[i](1, 3) = a[i-1]cos(theta[i-1]);
A[i](2, 0) = 0;
A[i](2, 1) = 0;
A[i](2, 2) = 0;
A[i](2, 3) = 0;
A[i](3, 0) = 0;
A[i](3, 1) = 0;
A[i](3, 2) = 0;
A[i](3, 3) = 0;
}
else{
A[i](0, 0) = cos(theta[i-1]);
A[i](0, 1) = -sin(theta[i-1]) * cos(alpha[i-1]);
A[i](0, 2) = sin(theta[i-1]) * sin(alpha[i-1]);
A[i](0, 3) = a[i-1] * cos(theta[i-1]);
A[i](1, 0) = sin(theta[i-1]);
A[i](1, 1) = cos(theta[i-1]) * cos(alpha[i-1]);
A[i](1, 2) = -cos(theta[i-1]) * sin(alpha[i-1]);
A[i](1, 3) = a[i-1] * sin(theta[i-1]);
A[i](2, 0) = 0;
A[i](2, 1) = sin(alpha[i-1]);
A[i](2, 2) = cos(alpha[i-1]);
A[i](2, 3) = d[i-1];
A[i](3, 0) = 0;
A[i](3, 1) = 0;
A[i](3, 2) = 0;
A[i](3, 3) = 1;
}
dAA=dAAA[i];
}
Jacobian_Z[0][j-1]=dAA(0,3);
Jacobian_Z[1][j-1]=dAA(1,3);
Jacobian_Z[2][j-1]=dAA(2,3);

}
}

void kinematics(double theta[6], double d[6], double a[6], double alpha[6], double *p)
{

Eigen::Matrix4d T[6 + 1]; // 为了和theta对应，0不用
for (int i = 0; i < 6; i++)
{
    T[i+1](0, 0) = cos(theta[i]);
    T[i+1](0, 1) = -sin(theta[i]) * cos(alpha[i]);
    T[i+1](0, 2) = sin(theta[i]) * sin(alpha[i]);
    T[i+1](0, 3) = a[i] * cos(theta[i]);
    T[i+1](1, 0) = sin(theta[i]);
    T[i+1](1, 1) = cos(theta[i]) * cos(alpha[i]);
    T[i+1](1, 2) = -cos(theta[i]) * sin(alpha[i]);
    T[i+1](1, 3) = a[i] * sin(theta[i]);
    T[i+1](2, 0) = 0;
    T[i+1](2, 1) = sin(alpha[i]);
    T[i+1](2, 2) = cos(alpha[i]);
    T[i+1](2, 3) = d[i];
    T[i+1](3, 0) = 0;
    T[i+1](3, 1) = 0;
    T[i+1](3, 2) = 0;
    T[i+1](3, 3) = 1;


}
Eigen::Matrix4d T06 = T[1] * T[2] * T[3] * T[4] * T[5] * T[6];

p[0] = T06(0, 3);
p[1] = T06(1, 3);
p[2] = T06(2, 3);

}
void ErrorCompensation_1(double theta_ori[6], float DH[6], double *Theta){
//补偿前DH参数
double d[6] = {DH[0],
0.0 / 1000.0,
0.0 / 1000.0,
DH[3],
DH[4],
DH[5]};
double a[6] = {0.0 / 1000.0,
DH[1],
DH[2],
0.0 / 1000.0,
0.0 / 1000.0,
0.0 / 1000.0};
double alpha[6] = {EIGEN_PI / 2, 0.0, 0.0, EIGEN_PI / 2, -EIGEN_PI / 2, 0.0};

// 补偿后DH参数
double theta[6];
theta[0] = theta_ori[0];// + 0.0 / 180 * EIGEN_PI;
theta[1] = theta_ori[1];// - 0.200247 / 180 * EIGEN_PI;
theta[2] = theta_ori[2];// - 0.0241131 / 180 * EIGEN_PI;
theta[3] = theta_ori[3];// + 0.107099 / 180 * EIGEN_PI;
theta[4] = theta_ori[4];// - 0.535242 / 180 * EIGEN_PI;
theta[5] = theta_ori[5];// + 0 / 180 * EIGEN_PI;

double D[6] = {137.5 / 1000,
               0.0 / 1000,
               0.0 / 1000,
               120.941/ 1000,
               118.061 / 1000,/*  */
               88.5 / 1000};
double A[6] = {0.0 / 1000,
               -400.811 / 1000,
               -373.927 / 1000,
               0.0 / 1000,
               0.0 / 1000,
               0.0 / 1000};
double Alpha[6] = {90.0848 / 180 * EIGEN_PI,
                   0.499605 / 180 * EIGEN_PI,
                   0.251361 / 180 * EIGEN_PI,
                   89.9559 / 180 * EIGEN_PI,
                   -89.8948 / 180 * EIGEN_PI,
                   0.323088 / 180 * EIGEN_PI};
struct timeval tv,tv1,tv2,tv3,tv4,tv5,tv6,tv7;
gettimeofday(&tv,NULL);

double *p, *P;
p=new double[6];
P=new double[6];
kinematics(theta_ori, d, a, alpha,p);

// 最大迭代次数
int maxIter = 5;
// 阻尼系数
double u = 0.01;
// H矩阵维度
int Dim = 6;
int updateJ = 1;
double error, error_last,error_1;
double **J_ad;
J_ad = (double **)malloc(6 * sizeof(double *));
for (int i = 0; i < 6; i++)
    J_ad[i] = (double *)malloc(6 * sizeof(double));
Eigen::VectorXd delta_P(3);
Eigen::VectorXd delta_theta(3);
Eigen::VectorXd delta_theta_1(3);
Eigen::MatrixXd J(3, 6);
delta_P<<0,0,0;
delta_theta<<0,0,0;
delta_theta_1<<0,0,0;
J<<0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0;

for (int i = 1; i <= maxIter; i++)
{
    ///
    if (updateJ == 1)
    {gettimeofday(&tv2,NULL);
        qDebug() <<"k_us_b:"<<(int)(tv2.tv_usec)<<" ";
        kinematics(theta, D, A, Alpha,P);
        gettimeofday(&tv3,NULL);
        qDebug() <<" k_us_a:%d "<<(int)(tv3.tv_usec);

        delta_P(0) = p[0] - P[0];
        delta_P(1) = p[1] - P[1];
        delta_P(2) = p[2] - P[2];

        gettimeofday(&tv4,NULL);
        qDebug() <<" J_us_b:%d "<<(int)(tv4.tv_usec);
        fErrorJacobian_z4(theta, D, A, Alpha,J_ad);
        gettimeofday(&tv5,NULL);
        qDebug() <<" J_us_a:%d "<<(int)(tv5.tv_usec);
        for (int i = 0; i < 3; i++)
        {
            for (int j = 0; j < 6; j++)
            {
                J(i, j) = J_ad[i][j];
            }
        }

        error_last = (p[0] - P[0]) * (p[0] - P[0]) + (p[1] - P[1]) * (p[1] - P[1]) + (p[2] - P[2]) * (p[2] - P[2]);
    }

// /
// qDebug() << p[0] << p[1] << p[2] << endl;
// qDebug() << P[0] << P[1] << P[2] << endl;
// qDebug() << delta_P << endl;
// qDebug() << J.transpose() << endl;
gettimeofday(&tv6,NULL);
qDebug() <<" Pinv_us_b:%d "<<(int)(tv6.tv_usec);
delta_theta = (J.transpose() * J + u * Eigen::MatrixXd::Identity(Dim, Dim)).inverse() * J.transpose() * delta_P;

    gettimeofday(&tv7,NULL);
    qDebug() <<" Pinv_us_a:%d "<<(int)(tv7.tv_usec);
    // qDebug()  << delta_theta << endl;
    Theta[0] = theta[0] + delta_theta(0);
    Theta[1] = theta[1] + delta_theta(1);
    Theta[2] = theta[2] + delta_theta(2);
    Theta[3] = theta[3] + delta_theta(3);
    Theta[4] = theta[4] + delta_theta(4);
    Theta[5] = theta[5] + delta_theta(5);

    // qDebug()  << Theta[1] << Theta[2] << Theta[3] << Theta[4] << Theta[5] << Theta[6] << endl;
    kinematics(Theta, D, A, Alpha,P);

    delta_P(0) = p[0] - P[0];
    delta_P(1) = p[1] - P[1];
    delta_P(2) = p[2] - P[2];

    error = (p[0] - P[0]) * (p[0] - P[0]) + (p[1] - P[1]) * (p[1] - P[1]) + (p[2] - P[2]) * (p[2] - P[2]);
    // qDebug()  << delta_P << endl;
    // qDebug()  << P[0] << P[1] << P[2] << " " << error << endl;

    if (error < error_last)
    {
        for (int k = 0; k < 6; k++)
        {
            theta[k] = Theta[k];
        }
        u = u / 10;
        error_last = error;
        //double err = sqrt(error_last);
        updateJ = 1;
    }
    else
    {
        u = u * 10;
        updateJ = 0;
    }
    


    //qDebug()  << "Time"<<i<<"= " << (time_end - time_start) << "ms; " <<"error="<<error <<endl;
    gettimeofday(&tv1,NULL);
    //printf("end:%d  ",(int)(tv1.tv_usec));
    qDebug() <<" ed:"<<(int)(tv1.tv_usec)<<" single:"<<(int)(tv1.tv_usec)-(int)(tv2.tv_usec)<<" total:"<<(int)(tv1.tv_usec)-(int)(tv.tv_usec);
    qDebug() <<" error="<<error<<endl;
    double t1=(Theta[0]-theta_ori[0])/ EIGEN_PI *180;
    double t2=(Theta[1]-theta_ori[1])/ EIGEN_PI *180;
    double t3=(Theta[2]-theta_ori[2])/ EIGEN_PI *180;
    double t4=(Theta[3]-theta_ori[3])/ EIGEN_PI *180;
    double t5=(Theta[4]-theta_ori[4])/ EIGEN_PI *180;
    double t6=(Theta[5]-theta_ori[5])/ EIGEN_PI *180;

    qDebug() <<t1<<t2<<t3<<t4<<t5<<t6<<endl;
         if(error<0.0000000001) {
         break;
     }

}

// for (int i = 0; i < 3; i++)
// {
// for (int j = 0; j < 6; j++)
// {
// qDebug() <<J(i, j)<<" ";
// }
// qDebug() <<endl;
// }
delete[] p;
delete[] P;
free(J_ad);
}
void ErrorCompensation_3(double theta_ori[6], float DH[6], double *Theta, double Tx[16]){
qDebug() <<“ErrorCompensation_3”;
//补偿前DH参数
double d[6] = {DH[0],
0.0 / 1000.0,
0.0 / 1000.0,
DH[3],
DH[4],
DH[5]};
double a[6] = {0.0 / 1000.0,
DH[1],
DH[2],
0.0 / 1000.0,
0.0 / 1000.0,
0.0 / 1000.0};
double alpha[6] = {EIGEN_PI / 2, 0.0, 0.0, EIGEN_PI / 2, -EIGEN_PI / 2, 0.0};
// 补偿后DH参数
double theta[6];
theta[0] = theta_ori[0]+0.1;
theta[1] = theta_ori[1];
theta[2] = theta_ori[2];
theta[3] = theta_ori[3];
theta[4] = theta_ori[4];
theta[5] = theta_ori[5];

double D[6] = {137.5 / 1000,
               0.0 / 1000,
               0.0 / 1000,
               120.941/ 1000,
               118.061 / 1000,/*  */
               88.5 / 1000};
double A[6] = {0.0 / 1000,
               -400.811 / 1000,
               -373.927 / 1000,
               0.0 / 1000,
               0.0 / 1000,
               0.0 / 1000};
double Alpha[6] = {90.0848 / 180 * EIGEN_PI,
                   0.499605 / 180 * EIGEN_PI,
                   0.251361 / 180 * EIGEN_PI,
                   89.9559 / 180 * EIGEN_PI,
                   -89.8948 / 180 * EIGEN_PI,
                   0.323088 / 180 * EIGEN_PI};
struct timeval tv,tv1,tv2,tv3,tv4,tv5,tv6,tv7;
gettimeofday(&tv,NULL);

double p[6], P[6];
p[0]=Tx[3];
p[1]=Tx[7];
p[2]=Tx[11];
// 最大迭代次数
int maxIter = 5;
// 阻尼系数
double u = 0.01;
// H矩阵维度
int Dim = 6;
int updateJ = 1;
double error, error_last;
double **J_ad;
J_ad = (double **)malloc(6 * sizeof(double *));
for (int i = 0; i < 6; i++)
    J_ad[i] = (double *)malloc(6 * sizeof(double));
Eigen::VectorXd delta_P=Eigen::VectorXd::Zero(3);
Eigen::VectorXd delta_theta=Eigen::VectorXd::Zero(3);
Eigen::VectorXd delta_theta_1=Eigen::VectorXd::Zero(3);
Eigen::MatrixXd J=Eigen::MatrixXd::Zero(3, 6);

//P初始值

kinematics(theta, D, A, Alpha,P);
qDebug()<<P[0]<<P[1]<<P[2];

delta_P(0) = p[0] - P[0];
delta_P(1) = p[1] - P[1];
delta_P(2) = p[2] - P[2];

for (int i = 1; i <= maxIter; i++)
{
    if (updateJ == 1)
    {
        gettimeofday(&tv4,NULL);

        fErrorJacobian_z4(theta, D, A, Alpha,J_ad);
        gettimeofday(&tv5,NULL);

        for (int i = 0; i < 3; i++)
        {
            for (int j = 0; j < 6; j++)
            {
                J(i, j) = J_ad[i][j];
            }
                            qDebug()<<J(i, 0)<<J(i, 1)<<J(i, 2)<<J(i, 3)<<J(i, 4)<<J(i, 5);
        }

        error_last = (p[0] - P[0]) * (p[0] - P[0]) + (p[1] - P[1]) * (p[1] - P[1]) + (p[2] - P[2]) * (p[2] - P[2]);
    }

    gettimeofday(&tv6,NULL);
    delta_theta = (J.transpose() * J + u * Eigen::MatrixXd::Identity(Dim, Dim)).inverse() * J.transpose() * delta_P;
    gettimeofday(&tv7,NULL);
    Theta[0] = theta[0] + delta_theta(0);
    Theta[1] = theta[1] + delta_theta(1);
    Theta[2] = theta[2] + delta_theta(2);
    Theta[3] = theta[3] + delta_theta(3);
    Theta[4] = theta[4] + delta_theta(4);
    Theta[5] = theta[5] + delta_theta(5);
    gettimeofday(&tv2,NULL);
    kinematics(Theta, D, A, Alpha,P);
    gettimeofday(&tv3,NULL);
    delta_P(0) = p[0] - P[0];
    delta_P(1) = p[1] - P[1];
    delta_P(2) = p[2] - P[2];

    error = (p[0] - P[0]) * (p[0] - P[0]) + (p[1] - P[1]) * (p[1] - P[1]) + (p[2] - P[2]) * (p[2] - P[2]);
    if (error < error_last)
    {
        for (int k = 0; k < 6; k++)
        {
            theta[k] = Theta[k];
        }
        u = u / 10;
        error_last = error;
        //double err = sqrt(error_last);
        updateJ = 1;
    }
    else
    {
        u = u * 10;
        updateJ = 0;
    }


    gettimeofday(&tv1,NULL);

    qDebug() <<"start"<<(int)(tv.tv_usec)<<"J_us_b"<<(int)(tv4.tv_usec)<<"J_us_a"<<(int)(tv5.tv_usec)<<"Pinv_us_b"<<(int)(tv6.tv_usec)<<"Pinv_us_a"<<(int)(tv7.tv_usec)<<"k_us_b"<<(int)(tv2.tv_usec)<<"k_us_a"<<(int)(tv3.tv_usec)<<"ed"<<(int)(tv1.tv_usec);
    qDebug() <<"J_us"<<(int)(tv5.tv_usec)-(int)(tv4.tv_usec)<<"Pinv_us"<<(int)(tv7.tv_usec)-(int)(tv6.tv_usec)<<"k_us"<<(int)(tv3.tv_usec)-(int)(tv2.tv_usec);
    qDebug() <<" single:"<<(int)(tv1.tv_usec)-(int)(tv4.tv_usec)<<" total:"<<(int)(tv1.tv_usec)-(int)(tv.tv_usec);
    qDebug()  <<"Theta" << Theta[0]<< Theta[1]  << Theta[2] << Theta[3] << Theta[4] << Theta[5];
    qDebug()  <<"p"<< p[0] << p[1] << p[2]<< " "<<"P"<< P[0] << P[1] << P[2] << " " <<"error"<< error<<endl;
         if(error<0.0000000001){
         break;
     }
}


free(J_ad);

}

void kinematics_1(double theta[6], double d[6], double a[6], double alpha[6], double TT[6][16])
{
Eigen::Matrix4d T[6 + 1]; // 涓轰簡鍜宼heta瀵瑰簲锛?0涓嶇敤
Eigen::MatrixXd TTT=Eigen::MatrixXd::Identity(4, 4);
struct timeval tv,tv1;
for (int i = 0; i < 6; i++){

    T[i+1](0, 0) = cos(theta[i]);
    T[i+1](0, 1) = -sin(theta[i]) * cos(alpha[i]);
    T[i+1](0, 2) = sin(theta[i]) * sin(alpha[i]);
    T[i+1](0, 3) = a[i] * cos(theta[i]);
    T[i+1](1, 0) = sin(theta[i]);
    T[i+1](1, 1) = cos(theta[i]) * cos(alpha[i]);
    T[i+1](1, 2) = -cos(theta[i]) * sin(alpha[i]);
    T[i+1](1, 3) = a[i] * sin(theta[i]);
    T[i+1](2, 0) = 0;
    T[i+1](2, 1) = sin(alpha[i]);
    T[i+1](2, 2) = cos(alpha[i]);
    T[i+1](2, 3) = d[i];
    T[i+1](3, 0) = 0;
    T[i+1](3, 1) = 0;
    T[i+1](3, 2) = 0;
    T[i+1](3, 3) = 1;

    TTT=TTT*T[i+1];
    TT[i][0]=TTT(0, 0);
    TT[i][1]=TTT(0, 1);
    TT[i][2]=TTT(0, 2);
    TT[i][3]=TTT(0, 3);
    TT[i][4]=TTT(1, 0);
    TT[i][5]=TTT(1, 1);
    TT[i][6]=TTT(1, 2);
    TT[i][7]=TTT(1, 3);
    TT[i][8]=TTT(2, 0);
    TT[i][9]=TTT(2, 1);
    TT[i][10]=TTT(2, 2);
    TT[i][11]=TTT(2, 3);
    TT[i][12]=TTT(3, 0);
    TT[i][13]=TTT(3, 1);
    TT[i][14]=TTT(3, 2);
    TT[i][15]=TTT(3, 3);

}

}

//void kinematics_2(double theta[6], double d[6], double a[6], double alpha[6], double *p)
//{

// Eigen::Matrix4d A[6 + 1]; // 为了和theta对应，0不用
// Eigen::Matrix4d T[6 + 1];
// for (int i = 0; i < 6; i++)
// {
// A[i+1](0, 0) = cos(theta[i]);
// A[i+1](0, 1) = -sin(theta[i]) * cos(alpha[i]);
// A[i+1](0, 2) = sin(theta[i]) * sin(alpha[i]);
// A[i+1](0, 3) = a[i] * cos(theta[i]);
// A[i+1](1, 0) = sin(theta[i]);
// A[i+1](1, 1) = cos(theta[i]) * cos(alpha[i]);
// A[i+1](1, 2) = -cos(theta[i]) * sin(alpha[i]);
// A[i+1](1, 3) = a[i] * sin(theta[i]);
// A[i+1](2, 0) = 0;
// A[i+1](2, 1) = sin(alpha[i]);
// A[i+1](2, 2) = cos(alpha[i]);
// A[i+1](2, 3) = d[i];
// A[i+1](3, 0) = 0;
// A[i+1](3, 1) = 0;
// A[i+1](3, 2) = 0;
// A[i+1](3, 3) = 1;

// }
// Eigen::Matrix4d T06 = A[1] * A[2] * A[3] * A[4] * A[5] * A[6];

// p[0] = T06(0, 3);
// p[1] = T06(1, 3);
// p[2] = T06(2, 3);

// Eigen::Matrix4d T02 = A[1] * A[2];

//}
Eigen::MatrixXd JacobiMatrix_1(double theta[6], double d[6], double a[6], double alpha[6]){

Eigen::MatrixXd J(6, 6);
Eigen::VectorXd k(3);
Eigen::MatrixXd R(3,3);
Eigen::VectorXd p_im(3);
Eigen::VectorXd zi_1(3);
Eigen::VectorXd zz(3);
double T[6][16] = {{0.0}};
kinematics_1(theta, d, a, alpha,  T);


for(int i=1;i<=6;i++){
    if(i==1){
        R=Eigen::MatrixXd::Identity(3, 3);
        p_im<<T[5][3],T[5][7],T[5][11];
        //cout<<T[5][3]<<" "<<T[5][7]<<" "<<T[5][11];
    }else{
        R<<T[i-2][0],T[i-2][1],T[i-2][2],T[i-2][4],T[i-2][5],T[i-2][6],T[i-2][8],T[i-2][9],T[i-2][10];
        p_im<<T[5][3]-T[i-2][3],T[5][7]-T[i-2][7],T[5][11]-T[i-2][11];
    }
    k<<0,0,1;
    zi_1=R*k;
    J(3,i-1)=zi_1(0);
    J(4,i-1)=zi_1(1);
    J(5,i-1)=zi_1(2);

    J(0,i-1)=zi_1(1)*p_im(2)-zi_1(2)*p_im(1);
    J(1,i-1)=zi_1(2)*p_im(0)-zi_1(0)*p_im(2);
    J(2,i-1)=zi_1(0)*p_im(1)-zi_1(1)*p_im(0);
    // cout<<J(0,i-1)<<" "<<J(1,i-1)<<" "<<J(2,i-1)<<" "<<J(3,i-1)<<" "<<J(4,i-1)<<" "<<J(5,i-1)<<"  jdwfsdf"<<endl;

}
return J;

}

void ErrorCompensation_4(double theta_ori[6], float DH[6], double *Theta, double Tx[16]){
qDebug() <<“ErrorCompensation_4”;
//补偿前DH参数
double d[6] = {DH[0],
0.0 / 1000.0,
0.0 / 1000.0,
DH[3],
DH[4],
DH[5]};
double a[6] = {0.0 / 1000.0,
DH[1],
DH[2],
0.0 / 1000.0,
0.0 / 1000.0,
0.0 / 1000.0};
double alpha[6] = {EIGEN_PI / 2, 0.0, 0.0, EIGEN_PI / 2, -EIGEN_PI / 2, 0.0};
// 补偿后DH参数
double theta[6];
theta[0] = theta_ori[0]+0.1;
theta[1] = theta_ori[1];
theta[2] = theta_ori[2];
theta[3] = theta_ori[3];
theta[4] = theta_ori[4];
theta[5] = theta_ori[5];

double D[6] = {137.5 / 1000,
               0.0 / 1000,
               0.0 / 1000,
               120.941/ 1000,
               118.061 / 1000,/*  */
               88.5 / 1000};
double A[6] = {0.0 / 1000,
               -400.811 / 1000,
               -373.927 / 1000,
               0.0 / 1000,
               0.0 / 1000,
               0.0 / 1000};
double Alpha[6] = {90.0848 / 180 * EIGEN_PI,
                   0.499605 / 180 * EIGEN_PI,
                   0.251361 / 180 * EIGEN_PI,
                   89.9559 / 180 * EIGEN_PI,
                   -89.8948 / 180 * EIGEN_PI,
                   0.323088 / 180 * EIGEN_PI};
struct timeval tv,tv1,tv2,tv3,tv4,tv5,tv6,tv7;
gettimeofday(&tv,NULL);

double p[6], P[6];
p[0]=Tx[3];
p[1]=Tx[7];
p[2]=Tx[11];
// 最大迭代次数
int maxIter = 5;
// 阻尼系数
double u = 0.01;
// H矩阵维度
int Dim = 6;
int updateJ = 1;
double error, error_last;


Eigen::MatrixXd J_ad=Eigen::MatrixXd::Zero(6, 6);
Eigen::VectorXd delta_P=Eigen::VectorXd::Zero(3);
Eigen::VectorXd delta_theta=Eigen::VectorXd::Zero(3);
Eigen::VectorXd delta_theta_1=Eigen::VectorXd::Zero(3);
Eigen::MatrixXd J=Eigen::MatrixXd::Zero(3, 6);

//P初始值

kinematics(theta, D, A, Alpha,P);
qDebug()<<P[0]<<P[1]<<P[2];

delta_P(0) = p[0] - P[0];
delta_P(1) = p[1] - P[1];
delta_P(2) = p[2] - P[2];

for (int i = 1; i <= maxIter; i++)
{
    if (updateJ == 1)
    {
        gettimeofday(&tv4,NULL);


        J_ad=JacobiMatrix_1(theta, D, A, Alpha);
        gettimeofday(&tv5,NULL);

        for (int i = 0; i < 3; i++)
        {
            for (int j = 0; j < 6; j++)
            {
                J(i, j) = J_ad(i,j);
            }
            qDebug()<<J(i, 0)<<J(i, 1)<<J(i, 2)<<J(i, 3)<<J(i, 4)<<J(i, 5);
        }

        error_last = (p[0] - P[0]) * (p[0] - P[0]) + (p[1] - P[1]) * (p[1] - P[1]) + (p[2] - P[2]) * (p[2] - P[2]);
    }

    gettimeofday(&tv6,NULL);
    delta_theta = (J.transpose() * J + u * Eigen::MatrixXd::Identity(Dim, Dim)).inverse() * J.transpose() * delta_P;
    gettimeofday(&tv7,NULL);
    Theta[0] = theta[0] + delta_theta(0);
    Theta[1] = theta[1] + delta_theta(1);
    Theta[2] = theta[2] + delta_theta(2);
    Theta[3] = theta[3] + delta_theta(3);
    Theta[4] = theta[4] + delta_theta(4);
    Theta[5] = theta[5] + delta_theta(5);
    gettimeofday(&tv2,NULL);
    kinematics(Theta, D, A, Alpha,P);

    gettimeofday(&tv3,NULL);
    delta_P(0) = p[0] - P[0];
    delta_P(1) = p[1] - P[1];
    delta_P(2) = p[2] - P[2];

    error = (p[0] - P[0]) * (p[0] - P[0]) + (p[1] - P[1]) * (p[1] - P[1]) + (p[2] - P[2]) * (p[2] - P[2]);
    if (error < error_last)
    {
        for (int k = 0; k < 6; k++)
        {
            theta[k] = Theta[k];
        }
        u = u / 10;
        error_last = error;
        //double err = sqrt(error_last);
        updateJ = 1;
    }
    else
    {
        u = u * 10;
        updateJ = 0;
    }


    gettimeofday(&tv1,NULL);

    qDebug() <<"start"<<(int)(tv.tv_usec)<<"J_us_b"<<(int)(tv4.tv_usec)<<"J_us_a"<<(int)(tv5.tv_usec)<<"Pinv_us_b"<<(int)(tv6.tv_usec)<<"Pinv_us_a"<<(int)(tv7.tv_usec)<<"k_us_b"<<(int)(tv2.tv_usec)<<"k_us_a"<<(int)(tv3.tv_usec)<<"ed"<<(int)(tv1.tv_usec);
    qDebug() <<"J_us"<<(int)(tv5.tv_usec)-(int)(tv4.tv_usec)<<"Pinv_us"<<(int)(tv7.tv_usec)-(int)(tv6.tv_usec)<<"k_us"<<(int)(tv3.tv_usec)-(int)(tv2.tv_usec);
    qDebug() <<" single:"<<(int)(tv1.tv_usec)-(int)(tv4.tv_usec)<<" total:"<<(int)(tv1.tv_usec)-(int)(tv.tv_usec);
    qDebug()  <<"Theta" << Theta[0]<< Theta[1]  << Theta[2] << Theta[3] << Theta[4] << Theta[5];
    qDebug()  <<"p"<< p[0] << p[1] << p[2]<< " "<<"P"<< P[0] << P[1] << P[2] << " " <<"error"<< error<<endl;
         if(error<0.0000000001){
         break;
     }
}

}

int JacobiMatrix_2(double theta[6], double d[6], double a[6], double alpha[6], double p[3], Eigen::MatrixXd & J, double P[3], double & error, Eigen::VectorXd & delta_P){

Eigen::VectorXd k(3);
Eigen::MatrixXd R(3,3);
Eigen::VectorXd p_im(3);
Eigen::VectorXd zi_1(3);
Eigen::VectorXd zz(3);
double T[6][16] = {{0.0}};
kinematics_1(theta, d, a, alpha,  T);
P[0]=T[5][3];
P[1]=T[5][7];
P[2]=T[5][11];

delta_P(0) = p[0] - P[0];
delta_P(1) = p[1] - P[1];
delta_P(2) = p[2] - P[2];
error = (p[0] - P[0]) * (p[0] - P[0]) + (p[1] - P[1]) * (p[1] - P[1]) + (p[2] - P[2]) * (p[2] - P[2]);
if(error<0.0000000001){
    return 0;
}

for(int i=1;i<=6;i++){
    if(i==1){
        R=Eigen::MatrixXd::Identity(3, 3);
        p_im<<T[5][3],T[5][7],T[5][11];
        //cout<<T[5][3]<<" "<<T[5][7]<<" "<<T[5][11];
    }else{
        R<<T[i-2][0],T[i-2][1],T[i-2][2],T[i-2][4],T[i-2][5],T[i-2][6],T[i-2][8],T[i-2][9],T[i-2][10];
        p_im<<T[5][3]-T[i-2][3],T[5][7]-T[i-2][7],T[5][11]-T[i-2][11];
    }
    k<<0,0,1;
    zi_1=R*k;

// J(3,i-1)=zi_1(0);
// J(4,i-1)=zi_1(1);
// J(5,i-1)=zi_1(2);

    J(0,i-1)=zi_1(1)*p_im(2)-zi_1(2)*p_im(1);
    J(1,i-1)=zi_1(2)*p_im(0)-zi_1(0)*p_im(2);
    J(2,i-1)=zi_1(0)*p_im(1)-zi_1(1)*p_im(0);
    // cout<<J(0,i-1)<<" "<<J(1,i-1)<<" "<<J(2,i-1)<<" "<<J(3,i-1)<<" "<<J(4,i-1)<<" "<<J(5,i-1)<<"  jdwfsdf"<<endl;

}
return 0;

}
Eigen::MatrixXd inv_3(Eigen::MatrixXd A){
//struct timeval tv,tv1,tv2,tv3,tv4,tv5,tv6,tv7;
// gettimeofday(&tv1,NULL);
double a1=A(0,0);
double b1=A(0,1);
double c1=A(0,2);
double a2=A(1,0);
double b2=A(1,1);
double c2=A(1,2);
double a3=A(2,0);
double b3=A(2,1);
double c3=A(2,2);
//gettimeofday(&tv2,NULL);
double C=a1b2c3 - a1b3c2 - a2b1c3 + a2b3c1 + a3b1c2 - a3b2c1;
// gettimeofday(&tv3,NULL);
Eigen::MatrixXd B(3,3);//=Eigen::MatrixXd::Zero(3,3);
//gettimeofday(&tv4,NULL);
B(0,0)=(b2c3 - b3c2)/C;
B(0,1)=(b3c1 - b1c3)/C;
B(0,2)=(b1c2 - b2c1)/C;

B(1,0)=(a3*c2 - a2*c3)/C;
B(1,1)=(a1*c3 - a3*c1)/C;
B(1,2)=(a2*c1 - a1*c2)/C;

B(2,0)=(a2*b3 - a3*b2)/C;
B(2,1)=(a3*b1 - a1*b3)/C;
B(2,2)=(a1*b2 - a2*b1)/C;

// gettimeofday(&tv5,NULL);

// qDebug() <<(int)(tv1.tv_usec)<<(int)(tv2.tv_usec)<<(int)(tv3.tv_usec)<<(int)(tv4.tv_usec)<<(int)(tv5.tv_usec);
return B;
}
Eigen::MatrixXd matrix_multi_3(Eigen::MatrixXd A, Eigen::MatrixXd B){
Eigen::MatrixXd C(3,3);
double a1=A(0,0);
double b1=A(0,1);
double c1=A(0,2);
double a2=A(1,0);
double b2=A(1,1);
double c2=A(1,2);
double a3=A(2,0);
double b3=A(2,1);
double c3=A(2,2);
double d1=B(0,0);
double e1=B(0,1);
double f1=B(0,2);
double d2=B(1,0);
double e2=B(1,1);
double f2=B(1,2);
double d3=B(2,0);
double e3=B(2,1);
double f3=B(2,2);
C(0,0)= a1d1 + b1d2 + c1d3;
C(0,1)= a1e1 + b1e2 + c1e3;
C(0,2)= a1f1 + b1f2 + c1f3;
C(1,0)= a2d1 + b2d2 + c2d3;
C(1,1)= a2e1 + b2e2 + c2e3;
C(1,2)= a2f1 + b2f2 + c2f3;
C(2,0)= a3d1 + b3d2 + c3d3;
C(2,1)= a3e1 + b3e2 + c3e3;
C(2,2)= a3f1 + b3f2 + c3*f3;

return C;
}
Eigen::MatrixXd inv_6(Eigen::MatrixXd ORI){

Eigen::MatrixXd A=ORI.block(0,0,3,3);
Eigen::MatrixXd B=ORI.block(0,3,3,3);
//gettimeofday(&tv1,NULL);
Eigen::MatrixXd C=ORI.block(3,0,3,3);
Eigen::MatrixXd D=ORI.block(3,3,3,3);

//gettimeofday(&tv,NULL);
Eigen::MatrixXd A_inv=inv_3(A);
Eigen::MatrixXd B_inv=inv_3(B);

Eigen::MatrixXd A_inv_B=matrix_multi_3(A_inv,B);
Eigen::MatrixXd C_A_inv=matrix_multi_3(C,A_inv);
//gettimeofday(&tv1,NULL);
Eigen::MatrixXd INV_6_D=inv_3(D-matrix_multi_3(C,A_inv_B));
//gettimeofday(&tv2,NULL);
Eigen::MatrixXd INV_6_B=-matrix_multi_3(A_inv_B,INV_6_D);
//gettimeofday(&tv3,NULL);
Eigen::MatrixXd INV_6_A=A_inv-matrix_multi_3(INV_6_B,C_A_inv);

//gettimeofday(&tv4,NULL);
Eigen::MatrixXd INV_6_C=-matrix_multi_3(INV_6_D,C_A_inv);
    //gettimeofday(&tv5,NULL);

//gettimeofday(&tv6,NULL);

//qDebug() <<“ssadsa”<<(int)(tv.tv_usec)<<(int)(tv1.tv_usec)<<(int)(tv2.tv_usec)<<(int)(tv3.tv_usec)<<(int)(tv4.tv_usec)<<(int)(tv5.tv_usec)<<(int)(tv6.tv_usec);
Eigen::MatrixXd INV(6,6);

INV<<INV_6_A,INV_6_B,INV_6_C,INV_6_D;
return INV;

}
void ErrorCompensation_5(double theta_ori[6], double *Theta, double Tx[16]){
qDebug() <<“ErrorCompensation_5”;
// 补偿后DH参数
double theta[6];
theta[0] = theta_ori[0]+0.1;
theta[1] = theta_ori[1];
theta[2] = theta_ori[2];
theta[3] = theta_ori[3];
theta[4] = theta_ori[4];
theta[5] = theta_ori[5];

double D[6] = {137.5 / 1000,
               0.0 / 1000,
               0.0 / 1000,
               120.941/ 1000,
               118.061 / 1000,/*  */
               88.5 / 1000};
double A[6] = {0.0 / 1000,
               -400.811 / 1000,
               -373.927 / 1000,
               0.0 / 1000,
               0.0 / 1000,
               0.0 / 1000};
double Alpha[6] = {90.0848 / 180 * EIGEN_PI,
                   0.499605 / 180 * EIGEN_PI,
                   0.251361 / 180 * EIGEN_PI,
                   89.9559 / 180 * EIGEN_PI,
                   -89.8948 / 180 * EIGEN_PI,
                   0.323088 / 180 * EIGEN_PI};
struct timeval tv,tv1,tv2,tv3,tv4,tv5,tv6,tv7,tv8,tv9,tv10;
gettimeofday(&tv,NULL);

double p[6], P[6];
p[0]=Tx[3];
p[1]=Tx[7];
p[2]=Tx[11];
// 最大迭代次数
int maxIter = 5;
// 阻尼系数
double u = 0.01;
// H矩阵维度
int Dim = 6;
int updateJ = 1;
double error;

double error_last=10000000;
Eigen::MatrixXd J_ad=Eigen::MatrixXd::Zero(6, 6);
Eigen::VectorXd delta_P=Eigen::VectorXd::Zero(3);
Eigen::VectorXd delta_theta=Eigen::VectorXd::Zero(3);
Eigen::VectorXd delta_theta_1=Eigen::VectorXd::Zero(3);
Eigen::MatrixXd J=Eigen::MatrixXd::Zero(3, 6);
for (int i = 1; i <= maxIter; i++)
{
    gettimeofday(&tv4,NULL);
    JacobiMatrix_2(theta, D, A, Alpha, p, J, P, error, delta_P);
    gettimeofday(&tv5,NULL);
           //qDebug()  <<"p"<< p[0] << p[1] << p[2]<< " "<<"P"<< P[0] << P[1] << P[2] << " " <<"error"<< error<<endl;
    gettimeofday(&tv10,NULL);
    if(error<0.0000000001){

       break;
    }

// for (int i = 0; i < 3; i++)
// {
// for (int j = 0; j < 6; j++)
// {
// J(i, j) = J_ad(i,j);
// }
// qDebug()<<J(i, 0)<<J(i, 1)<<J(i, 2)<<J(i, 3)<<J(i, 4)<<J(i, 5);
// }

    gettimeofday(&tv6,NULL);
    delta_theta = (J.transpose() * J + u * Eigen::MatrixXd::Identity(Dim, Dim)).inverse() * J.transpose() * delta_P;
    gettimeofday(&tv7,NULL);
    Theta[0] = theta[0] + delta_theta(0);
    Theta[1] = theta[1] + delta_theta(1);
    Theta[2] = theta[2] + delta_theta(2);
    Theta[3] = theta[3] + delta_theta(3);
    Theta[4] = theta[4] + delta_theta(4);
    Theta[5] = theta[5] + delta_theta(5);
    if (error < error_last)
    {
        for (int k = 0; k < 6; k++)
        {
            theta[k] = Theta[k];
        }
        u = u / 10;
        error_last = error;
        //double err = sqrt(error_last);
        updateJ = 1;
    }
    else
    {
        u = u * 10;
        updateJ = 0;
    }
    gettimeofday(&tv1,NULL);

//gettimeofday(&tv8,NULL);
qDebug() <<“start”<<(int)(tv.tv_usec)<<“J_us_b”<<(int)(tv4.tv_usec)<<“J_us_a”<<(int)(tv5.tv_usec)<<“Pinv_us_b”<<(int)(tv6.tv_usec)<<“Pinv_us_a”<<(int)(tv7.tv_usec)<<“ed”<<(int)(tv1.tv_usec);
qDebug() <<“J_us”<<(int)(tv5.tv_usec)-(int)(tv4.tv_usec)<<“Pinv_us”<<(int)(tv7.tv_usec)-(int)(tv6.tv_usec);
qDebug() <<" single:“<<(int)(tv1.tv_usec)-(int)(tv4.tv_usec)<<” total:"<<(int)(tv1.tv_usec)-(int)(tv.tv_usec);
qDebug() <<“Theta” << Theta[0]<< Theta[1] << Theta[2] << Theta[3] << Theta[4] << Theta[5];

//gettimeofday(&tv9,NULL);
}

// qDebug() <<“danci”<<(int)(tv8.tv_usec)-(int)(tv.tv_usec);
// qDebug() <<“danci”<<(int)(tv9.tv_usec)-(int)(tv.tv_usec);
// qDebug() <<“danci”<<(int)(tv10.tv_usec)-(int)(tv.tv_usec);
}

void ErrorCompensation_6(double theta_ori[6], double *Theta, double Tx[16]){
qDebug() <<“ErrorCompensation_6”;
// 补偿后DH参数
double theta[6];
theta[0] = theta_ori[0]+0.1;
theta[1] = theta_ori[1];
theta[2] = theta_ori[2];
theta[3] = theta_ori[3];
theta[4] = theta_ori[4];
theta[5] = theta_ori[5];

double D[6] = {137.5 / 1000,
               0.0 / 1000,
               0.0 / 1000,
               120.941/ 1000,
               118.061 / 1000,/*  */
               88.5 / 1000};
double A[6] = {0.0 / 1000,
               -400.811 / 1000,
               -373.927 / 1000,
               0.0 / 1000,
               0.0 / 1000,
               0.0 / 1000};
double Alpha[6] = {90.0848 / 180 * EIGEN_PI,
                   0.499605 / 180 * EIGEN_PI,
                   0.251361 / 180 * EIGEN_PI,
                   89.9559 / 180 * EIGEN_PI,
                   -89.8948 / 180 * EIGEN_PI,
                   0.323088 / 180 * EIGEN_PI};
//struct timeval tv,tv1,tv2,tv3,tv4,tv5,tv6,tv7;
//gettimeofday(&tv,NULL);

double p[6], P[6];
p[0]=Tx[3];
p[1]=Tx[7];
p[2]=Tx[11];
// 最大迭代次数
int maxIter = 5;
// 阻尼系数
double u = 0.01;
// H矩阵维度
int Dim = 6;
int updateJ = 1;
double error;

double error_last=10000000;
Eigen::MatrixXd J_ad=Eigen::MatrixXd::Zero(6, 6);
Eigen::VectorXd delta_P=Eigen::VectorXd::Zero(3);
Eigen::VectorXd delta_theta=Eigen::VectorXd::Zero(3);
Eigen::VectorXd delta_theta_1=Eigen::VectorXd::Zero(3);
Eigen::MatrixXd J=Eigen::MatrixXd::Zero(3, 6);
for (int i = 1; i <= maxIter; i++)
{
    //gettimeofday(&tv4,NULL);
    JacobiMatrix_2(theta, D, A, Alpha, p, J, P, error, delta_P);
    //gettimeofday(&tv5,NULL);
           //qDebug()  <<"p"<< p[0] << p[1] << p[2]<< " "<<"P"<< P[0] << P[1] << P[2] << " " <<"error"<< error<<endl;
    if(error<0.0000000001){
        break;
    }

// for (int i = 0; i < 3; i++)
// {
// for (int j = 0; j < 6; j++)
// {
// J(i, j) = J_ad(i,j);
// }
// qDebug()<<J(i, 0)<<J(i, 1)<<J(i, 2)<<J(i, 3)<<J(i, 4)<<J(i, 5);
// }

    //gettimeofday(&tv6,NULL);
    delta_theta = (J.transpose() * J + u * Eigen::MatrixXd::Identity(Dim, Dim)).inverse() * J.transpose() * delta_P;
    //gettimeofday(&tv7,NULL);
    Theta[0] = theta[0] + delta_theta(0);
    Theta[1] = theta[1] + delta_theta(1);
    Theta[2] = theta[2] + delta_theta(2);
    Theta[3] = theta[3] + delta_theta(3);
    Theta[4] = theta[4] + delta_theta(4);
    Theta[5] = theta[5] + delta_theta(5);
    if (error < error_last)
    {
        for (int k = 0; k < 6; k++)
        {
            theta[k] = Theta[k];
        }
        u = u / 10;
        error_last = error;
        //double err = sqrt(error_last);
        updateJ = 1;
    }
    else
    {
        u = u * 10;
        updateJ = 0;
    }
    //gettimeofday(&tv1,NULL);

// qDebug() <<“start”<<(int)(tv.tv_usec)<<“J_us_b”<<(int)(tv4.tv_usec)<<“J_us_a”<<(int)(tv5.tv_usec)<<“Pinv_us_b”<<(int)(tv6.tv_usec)<<“Pinv_us_a”<<(int)(tv7.tv_usec)<<“ed”<<(int)(tv1.tv_usec);
// qDebug() <<“J_us”<<(int)(tv5.tv_usec)-(int)(tv4.tv_usec)<<“Pinv_us”<<(int)(tv7.tv_usec)-(int)(tv6.tv_usec);
// qDebug() <<" single:“<<(int)(tv1.tv_usec)-(int)(tv4.tv_usec)<<” total:"<<(int)(tv1.tv_usec)-(int)(tv.tv_usec);
// qDebug() <<“Theta” << Theta[0]<< Theta[1] << Theta[2] << Theta[3] << Theta[4] << Theta[5];

}

void ErrorCompensation_7(double theta_ori[6], double *Theta, double Tx[16]){
qDebug() <<“ErrorCompensation_7”;
// 补偿后DH参数
double theta[6];
theta[0] = theta_ori[0]+0.1;
theta[1] = theta_ori[1];
theta[2] = theta_ori[2];
theta[3] = theta_ori[3];
theta[4] = theta_ori[4];
theta[5] = theta_ori[5];

double D[6] = {137.5 / 1000,
               0.0 / 1000,
               0.0 / 1000,
               120.941/ 1000,
               118.061 / 1000,/*  */
               88.5 / 1000};
double A[6] = {0.0 / 1000,
               -400.811 / 1000,
               -373.927 / 1000,
               0.0 / 1000,
               0.0 / 1000,
               0.0 / 1000};
double Alpha[6] = {90.0848 / 180 * EIGEN_PI,
                   0.499605 / 180 * EIGEN_PI,
                   0.251361 / 180 * EIGEN_PI,
                   89.9559 / 180 * EIGEN_PI,
                   -89.8948 / 180 * EIGEN_PI,
                   0.323088 / 180 * EIGEN_PI};
struct timeval tv,tv1,tv2,tv3,tv4,tv5,tv6,tv7,tv8,tv9,tv10;
gettimeofday(&tv,NULL);

double p[6], P[6];
p[0]=Tx[3];
p[1]=Tx[7];
p[2]=Tx[11];
// 最大迭代次数
int maxIter = 5;
// 阻尼系数
double u = 0.01;
// H矩阵维度
int Dim = 6;
int updateJ = 1;
double error;

double error_last=10000000;
Eigen::MatrixXd J_ad=Eigen::MatrixXd::Zero(6, 6);
Eigen::VectorXd delta_P=Eigen::VectorXd::Zero(3);
Eigen::VectorXd delta_theta=Eigen::VectorXd::Zero(3);
Eigen::VectorXd delta_theta_1=Eigen::VectorXd::Zero(3);
Eigen::MatrixXd J=Eigen::MatrixXd::Zero(3, 6);
for (int i = 1; i <= maxIter; i++)
{
    gettimeofday(&tv4,NULL);
    JacobiMatrix_2(theta, D, A, Alpha, p, J, P, error, delta_P);
    gettimeofday(&tv5,NULL);
           qDebug()  <<"p"<< p[0] << p[1] << p[2]<< " "<<"P"<< P[0] << P[1] << P[2] << " " <<"error"<< error<<endl;
    gettimeofday(&tv10,NULL);
    if(error<0.0000000001){

       break;
    }

// for (int i = 0; i < 3; i++)
// {
// for (int j = 0; j < 6; j++)
// {
// J(i, j) = J_ad(i,j);
// }
// qDebug()<<J(i, 0)<<J(i, 1)<<J(i, 2)<<J(i, 3)<<J(i, 4)<<J(i, 5);
// }

    gettimeofday(&tv6,NULL);
    Eigen::MatrixXd temp=inv_6(J.transpose() * J + u * Eigen::MatrixXd::Identity(Dim, Dim));
    delta_theta = temp * J.transpose() * delta_P;
    gettimeofday(&tv7,NULL);
    Theta[0] = theta[0] + delta_theta(0);
    Theta[1] = theta[1] + delta_theta(1);
    Theta[2] = theta[2] + delta_theta(2);
    Theta[3] = theta[3] + delta_theta(3);
    Theta[4] = theta[4] + delta_theta(4);
    Theta[5] = theta[5] + delta_theta(5);
    if (error < error_last)
    {
        for (int k = 0; k < 6; k++)
        {
            theta[k] = Theta[k];
        }
        u = u / 10;
        error_last = error;
        //double err = sqrt(error_last);
        updateJ = 1;
    }
    else
    {
        u = u * 10;
        updateJ = 0;
    }
    gettimeofday(&tv1,NULL);

//gettimeofday(&tv9,NULL);
}

// qDebug() <<“danci”<<(int)(tv8.tv_usec)-(int)(tv.tv_usec);
// qDebug() <<“danci”<<(int)(tv9.tv_usec)-(int)(tv.tv_usec);
// qDebug() <<“danci”<<(int)(tv10.tv_usec)-(int)(tv.tv_usec);
}

void ErrorCompensation_8(double theta_ori[6], double *Theta, double Tx[16]){
qDebug() <<“ErrorCompensation_8”;
// 补偿后DH参数
double theta[6];
theta[0] = theta_ori[0]+0.1;
theta[1] = theta_ori[1];
theta[2] = theta_ori[2];
theta[3] = theta_ori[3];
theta[4] = theta_ori[4];
theta[5] = theta_ori[5];

double D[6] = {137.5 / 1000,
               0.0 / 1000,
               0.0 / 1000,
               120.941/ 1000,
               118.061 / 1000,/*  */
               88.5 / 1000};
double A[6] = {0.0 / 1000,
               -400.811 / 1000,
               -373.927 / 1000,
               0.0 / 1000,
               0.0 / 1000,
               0.0 / 1000};
double Alpha[6] = {90.0848 / 180 * EIGEN_PI,
                   0.499605 / 180 * EIGEN_PI,
                   0.251361 / 180 * EIGEN_PI,
                   89.9559 / 180 * EIGEN_PI,
                   -89.8948 / 180 * EIGEN_PI,
                   0.323088 / 180 * EIGEN_PI};
double p[6], P[6];
p[0]=Tx[3];
p[1]=Tx[7];
p[2]=Tx[11];
// 最大迭代次数
int maxIter = 5;
// 阻尼系数
double u = 0.01;
// H矩阵维度
int Dim = 6;
int updateJ = 1;
double error;
double error_last=10000000;
Eigen::MatrixXd J_ad=Eigen::MatrixXd::Zero(6, 6);
Eigen::VectorXd delta_P=Eigen::VectorXd::Zero(3);
Eigen::VectorXd delta_theta=Eigen::VectorXd::Zero(3);
Eigen::VectorXd delta_theta_1=Eigen::VectorXd::Zero(3);
Eigen::MatrixXd J=Eigen::MatrixXd::Zero(3, 6);
for (int i = 1; i <= maxIter; i++)
{
    JacobiMatrix_2(theta, D, A, Alpha, p, J, P, error, delta_P);
    if(error<0.0000000001){
       break;
    }
    Eigen::MatrixXd temp=inv_6(J.transpose() * J + u * Eigen::MatrixXd::Identity(Dim, Dim));
    delta_theta = temp * J.transpose() * delta_P;
    Theta[0] = theta[0] + delta_theta(0);
    Theta[1] = theta[1] + delta_theta(1);
    Theta[2] = theta[2] + delta_theta(2);
    Theta[3] = theta[3] + delta_theta(3);
    Theta[4] = theta[4] + delta_theta(4);
    Theta[5] = theta[5] + delta_theta(5);
    if (error < error_last)
    {
        for (int k = 0; k < 6; k++)
        {
            theta[k] = Theta[k];
        }
        u = u / 10;
        error_last = error;
        updateJ = 1;
    }
    else
    {
        u = u * 10;
        updateJ = 0;
    }

}
}
void rotationMatrixToRPY(const double m[3][3], double rpy[3])
{
float cosBeta = sqrt(m[0][0] * m[0][0] + m[1][0] * m[1][0]); //cosBeta取正根，即-90 <= beta <= 90
float epsilon = 0.000001;
if (cosBeta < epsilon && cosBeta > -epsilon) {
// 机器人导论，科大用，P43 //update 0319
rpy[1] = atan2(-m[2][0], cosBeta);
rpy[2] = 0.0;
rpy[0] = atan2(-m[1][2], m[1][1]);
} else {
rpy[1] = atan2(-m[2][0], cosBeta);
rpy[2] = atan2(m[1][0], m[0][0]);
rpy[0] = atan2(m[2][1], m[2][2]);
}
}
void ErrorCompensation_1025(double Tx[16],double DiscartesPoint[6],double pos_start[6]){
// 补偿后DH参数
double theta[6];
theta[0] = pos_start[0];// + 0.0 / 180 * EIGEN_PI;
theta[1] = pos_start[1];// - 0.200247 / 180 * EIGEN_PI;
theta[2] = pos_start[2];// - 0.0241131 / 180 * EIGEN_PI;
theta[3] = pos_start[3];// + 0.107099 / 180 * EIGEN_PI;
theta[4] = pos_start[4];// - 0.535242 / 180 * EIGEN_PI;
theta[5] = pos_start[5];// + 0 / 180 * EIGEN_PI;
double D[6] = {137.5 / 1000,
0.0 / 1000,
0.0 / 1000,
120.941/ 1000,
118.061 / 1000,/* */
88.5 / 1000};
double A[6] = {0.0 / 1000,
-400.811 / 1000,
-373.927 / 1000,
0.0 / 1000,
0.0 / 1000,
0.0 / 1000};
double Alpha[6] = {90.0848 / 180 * EIGEN_PI,
0.499605 / 180 * EIGEN_PI,
0.251361 / 180 * EIGEN_PI,
89.9559 / 180 * EIGEN_PI,
-89.8948 / 180 * EIGEN_PI,
0.323088 / 180 * EIGEN_PI};
struct timeval tv,tv1,tv2,tv3,tv4,tv5,tv6,tv7;
gettimeofday(&tv,NULL);
printf(“start:%d\n”,(int)(tv.tv_usec));
double p[6], P[6];
double rpy[3];
double RPY[3];
double T[6][16] = {{0.0}};
double Theta[6]={0};
//lilun mubiao
p[0]=Tx[3];p[1]=Tx[7];p[2]=Tx[11];
double r[3][3]={{Tx[0],Tx[1],Tx[2]},{Tx[4],Tx[5],Tx[6]},{Tx[8],Tx[9],Tx[10]}};
rotationMatrixToRPY(r, rpy);
p[3]=rpy[0];p[4]=rpy[1];p[5]=rpy[2];

// 最大迭代次数
int maxIter = 10;
// 阻尼系数
double u = 0.01;
// H矩阵维度
int Dim = 6;
int updateJ = 1;
double error, error_last,error_1;


Eigen::MatrixXd J_ad(6, 6);
Eigen::VectorXd delta_P(6);
Eigen::VectorXd delta_theta(6);
Eigen::VectorXd delta_theta_1(6);
Eigen::MatrixXd J(6, 6);

delta_P<<0,0,0,0,0,0;
delta_theta<<0,0,0,0,0,0;
delta_theta_1<<0,0,0,0,0,0;
J<<0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0;
J_ad<<0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0;

for (int i = 1; i <= maxIter; i++)
{
    ///
    if (updateJ == 1)
    {gettimeofday(&tv2,NULL);

        kinematics_1(theta, D, A, Alpha,T);
        P[0]=T[5][3];P[1]=T[5][7];P[2]=T[5][11];
        double R[3][3]={{T[5][0],T[5][1],T[5][2]},{T[5][4],T[5][5],T[5][6]},{T[5][8],T[5][9],T[5][10]}};
        rotationMatrixToRPY(R, RPY);
        P[3]=RPY[0];P[4]=RPY[1];P[5]=RPY[2];
        gettimeofday(&tv3,NULL);


        delta_P(0) = p[0] - P[0];
        delta_P(1) = p[1] - P[1];
        delta_P(2) = p[2] - P[2];
        delta_P(3) = p[3] - P[3];
        delta_P(4) = p[4] - P[4];
        delta_P(5) = p[5] - P[5];

        gettimeofday(&tv4,NULL);

        J_ad=JacobiMatrix_1(theta, D, A, Alpha);
        gettimeofday(&tv5,NULL);


        for (int i = 0; i < 6; i++)
        {
            for (int j = 0; j < 6; j++)
            {
                J(i, j) = J_ad(i,j);
            }
        }

        error_last = (p[0] - P[0]) * (p[0] - P[0]) + (p[1] - P[1]) * (p[1] - P[1]) + (p[2] - P[2]) * (p[2] - P[2]);
    }
    gettimeofday(&tv6,NULL);

    delta_theta = (J.transpose() * J + u * Eigen::MatrixXd::Identity(Dim, Dim)).inverse() * J.transpose() * delta_P;

    gettimeofday(&tv7,NULL);

    // qDebug()  << delta_theta << endl;
    Theta[0] = theta[0] + delta_theta(0);
    Theta[1] = theta[1] + delta_theta(1);
    Theta[2] = theta[2] + delta_theta(2);
    Theta[3] = theta[3] + delta_theta(3);
    Theta[4] = theta[4] + delta_theta(4);
    Theta[5] = theta[5] + delta_theta(5);
    kinematics_1(Theta, D, A, Alpha,T);
    P[0]=T[5][3];P[1]=T[5][7];P[2]=T[5][11];
    double R[3][3]={{T[5][0],T[5][1],T[5][2]},{T[5][4],T[5][5],T[5][6]},{T[5][8],T[5][9],T[5][10]}};
    rotationMatrixToRPY(R, RPY);
    P[3]=RPY[0];P[4]=RPY[1];P[5]=RPY[2];

    delta_P(0) = p[0] - P[0]; delta_P(1) = p[1] - P[1];delta_P(2) = p[2] - P[2]; delta_P(3) = p[3] - P[3]; delta_P(4) = p[4] - P[4];
    delta_P(5) = p[5] - P[5];

    error = (p[0] - P[0]) * (p[0] - P[0]) + (p[1] - P[1]) * (p[1] - P[1]) + (p[2] - P[2]) * (p[2] - P[2]);
    // qDebug()  << delta_P << endl;

    if (error < error_last)
    {
        for (int k = 0; k < 6; k++)
        {
            theta[k] = Theta[k];
        }
        u = u / 10;
        error_last = error;
        //double err = sqrt(error_last);
        updateJ = 1;
    }
    else
    {
        u = u * 10;
        updateJ = 0;
    }
    
    //qDebug()  << "Time"<<i<<"= " << (time_end - time_start) << "ms; " <<"error="<<error <<endl;
    gettimeofday(&tv1,NULL);
    //printf("end:%d  ",(int)(tv1.tv_usec));

// qDebug() <<“k_us_b:”<<(int)(tv2.tv_usec)<<" “;
// qDebug() <<” k_us_a:%d “<<(int)(tv3.tv_usec);
// qDebug() <<” J_us_b:%d “<<(int)(tv4.tv_usec);
// qDebug() <<” J_us_a:%d “<<(int)(tv5.tv_usec);
// qDebug() <<” Pinv_us_b:%d “<<(int)(tv6.tv_usec);
// qDebug() <<” Pinv_us_a:%d “<<(int)(tv7.tv_usec);
qDebug() <<“modeling”<<(int)(tv5.tv_usec)-(int)(tv2.tv_usec);
qDebug() <<“Pinv”<<(int)(tv7.tv_usec)-(int)(tv6.tv_usec);
// qDebug() << (Theta[0]-theta_ori[0])/3.1415926180 << (Theta[1]-theta_ori[1])/3.1415926180 << (Theta[2]-theta_ori[2])/3.1415926180 << (Theta[3]-theta_ori[3])/3.1415926180 << (Theta[4]-theta_ori[4])/3.1415926180 << (Theta[5]-theta_ori[5])/3.1415926180 << endl;
qDebug() <<“P”<< P[0]<< " " << P[1] << " “<< P[2]<<” “<<“p”<< p[0]<< " " << p[1] << " “<< p[2]<< " “<<“delta”<<P[0]-p[0]<< " " << P[1]-p[1] << " “<< P[2]-p[2] << " " << " error=”<<error << endl;
qDebug() <<” ed:”<<(int)(tv1.tv_usec)<<” single:”<<(int)(tv1.tv_usec)-(int)(tv2.tv_usec)<<" total:"<<(int)(tv1.tv_usec)-(int)(tv.tv_usec);

    if(error<0.0000000001){
         break;
     }

}
for(int i=0;i<6;i++){
    DiscartesPoint[i]=Theta[i];
}

}
int JacobiMatrix_3(double theta[6], double d[6], double a[6], double alpha[6], double p[6], Eigen::MatrixXd & J, double P[6], double & error, Eigen::VectorXd & delta_P){

Eigen::VectorXd k(3);
Eigen::MatrixXd R(3,3);
Eigen::VectorXd p_im(3);
Eigen::VectorXd zi_1(3);
Eigen::VectorXd zz(3);
double T[6][16] = {{0.0}};
double RPY[3];
kinematics_1(theta, d, a, alpha,  T);
P[0]=T[5][3];
P[1]=T[5][7];
P[2]=T[5][11];
double RR[3][3]={{T[5][0],T[5][1],T[5][2]},{T[5][4],T[5][5],T[5][6]},{T[5][8],T[5][9],T[5][10]}};
rotationMatrixToRPY(RR, RPY);
P[3]=RPY[0];P[4]=RPY[1];P[5]=RPY[2];

delta_P(0) = p[0] - P[0];
delta_P(1) = p[1] - P[1];
delta_P(2) = p[2] - P[2];
delta_P(3) = p[3] - P[3];
delta_P(4) = p[4] - P[4];
delta_P(5) = p[5] - P[5];
        error = (p[0] - P[0]) * (p[0] - P[0]) + (p[1] - P[1]) * (p[1] - P[1]) + (p[2] - P[2]) * (p[2] - P[2]);
if(error<0.0000000001){
    return 0;
}

for(int i=1;i<=6;i++){
    if(i==1){
        R=Eigen::MatrixXd::Identity(3, 3);
        p_im<<T[5][3],T[5][7],T[5][11];
        //cout<<T[5][3]<<" "<<T[5][7]<<" "<<T[5][11];
    }else{
        R<<T[i-2][0],T[i-2][1],T[i-2][2],T[i-2][4],T[i-2][5],T[i-2][6],T[i-2][8],T[i-2][9],T[i-2][10];
        p_im<<T[5][3]-T[i-2][3],T[5][7]-T[i-2][7],T[5][11]-T[i-2][11];
    }
    k<<0,0,1;
    zi_1=R*k;
    J(3,i-1)=zi_1(0);
    J(4,i-1)=zi_1(1);
    J(5,i-1)=zi_1(2);

    J(0,i-1)=zi_1(1)*p_im(2)-zi_1(2)*p_im(1);
    J(1,i-1)=zi_1(2)*p_im(0)-zi_1(0)*p_im(2);
    J(2,i-1)=zi_1(0)*p_im(1)-zi_1(1)*p_im(0);
    // cout<<J(0,i-1)<<" "<<J(1,i-1)<<" "<<J(2,i-1)<<" "<<J(3,i-1)<<" "<<J(4,i-1)<<" "<<J(5,i-1)<<"  jdwfsdf"<<endl;

}
return 0;

}
void ErrorCompensation_9(double theta_ori[6], double Theta, double Tx[16]){
qDebug() <<“ErrorCompensation_9”;
// 补偿后DH参数
double theta[6];
theta[0] = theta_ori[0]+0.1;
theta[1] = theta_ori[1];
theta[2] = theta_ori[2];
theta[3] = theta_ori[3];
theta[4] = theta_ori[4];
theta[5] = theta_ori[5];
double D[6] = {137.5 / 1000,
0.0 / 1000,
0.0 / 1000,
120.941/ 1000,
118.061 / 1000,/ */
88.5 / 1000};
double A[6] = {0.0 / 1000,
-400.811 / 1000,
-373.927 / 1000,
0.0 / 1000,
0.0 / 1000,
0.0 / 1000};
double Alpha[6] = {90.0848 / 180 * EIGEN_PI,
0.499605 / 180 * EIGEN_PI,
0.251361 / 180 * EIGEN_PI,
89.9559 / 180 * EIGEN_PI,
-89.8948 / 180 * EIGEN_PI,
0.323088 / 180 * EIGEN_PI};
struct timeval tv,tv1,tv2,tv3,tv4,tv5,tv6,tv7,tv8,tv9,tv10;
gettimeofday(&tv,NULL);
double p[6], P[6];
double rpy[3];
double RPY[3];
double T[6][16] = {{0.0}};
p[0]=Tx[3];
p[1]=Tx[7];
p[2]=Tx[11];
double r[3][3]={{Tx[0],Tx[1],Tx[2]},{Tx[4],Tx[5],Tx[6]},{Tx[8],Tx[9],Tx[10]}};
rotationMatrixToRPY(r, rpy);
p[3]=rpy[0];p[4]=rpy[1];p[5]=rpy[2];
// 最大迭代次数
int maxIter = 5;
// 阻尼系数
double u = 0.01;
// H矩阵维度
int Dim = 6;
int updateJ = 1;
double error;
double error_last=10000000;
Eigen::MatrixXd J_ad=Eigen::MatrixXd::Zero(6, 6);
Eigen::VectorXd delta_P=Eigen::VectorXd::Zero(6);
Eigen::VectorXd delta_theta=Eigen::VectorXd::Zero(3);
Eigen::VectorXd delta_theta_1=Eigen::VectorXd::Zero(3);
Eigen::MatrixXd J=Eigen::MatrixXd::Zero(6, 6);
for (int i = 1; i <= maxIter; i++)
{
gettimeofday(&tv4,NULL);
JacobiMatrix_3(theta, D, A, Alpha, p, J, P, error, delta_P);
gettimeofday(&tv5,NULL);
qDebug() <<“p”<< p[0] << p[1] << p[2]<< " "<<“P”<< P[0] << P[1] << P[2] << " " <<“error”<< error<<endl;
gettimeofday(&tv10,NULL);
if(error<0.0000000001){

       break;
    }
    gettimeofday(&tv6,NULL);
    Eigen::MatrixXd temp=inv_6(J.transpose() * J + u * Eigen::MatrixXd::Identity(Dim, Dim));
    delta_theta = temp * J.transpose() * delta_P;
    gettimeofday(&tv7,NULL);
    Theta[0] = theta[0] + delta_theta(0);
    Theta[1] = theta[1] + delta_theta(1);
    Theta[2] = theta[2] + delta_theta(2);
    Theta[3] = theta[3] + delta_theta(3);
    Theta[4] = theta[4] + delta_theta(4);
    Theta[5] = theta[5] + delta_theta(5);
    if (error < error_last)
    {
        for (int k = 0; k < 6; k++)
        {
            theta[k] = Theta[k];
        }
        u = u / 10;
        error_last = error;
        updateJ = 1;
    }
    else
    {
        u = u * 10;
        updateJ = 0;
    }
    gettimeofday(&tv1,NULL);
    qDebug() <<"start"<<(int)(tv.tv_usec)<<"J_us_b"<<(int)(tv4.tv_usec)<<"J_us_a"<<(int)(tv5.tv_usec)<<"Pinv_us_b"<<(int)(tv6.tv_usec)<<"Pinv_us_a"<<(int)(tv7.tv_usec)<<"ed"<<(int)(tv1.tv_usec);
    qDebug() <<"J_us"<<(int)(tv5.tv_usec)-(int)(tv4.tv_usec)<<"Pinv_us"<<(int)(tv7.tv_usec)-(int)(tv6.tv_usec);
    qDebug() <<" single:"<<(int)(tv1.tv_usec)-(int)(tv4.tv_usec)<<" total:"<<(int)(tv1.tv_usec)-(int)(tv.tv_usec);
    qDebug()  <<"Theta" << Theta[0]<< Theta[1]  << Theta[2] << Theta[3] << Theta[4] << Theta[5];
}

}
void ErrorCompensation_1025sff(double Tx[16],double DiscartesPoint[6],double pos_start[6]){
// 补偿后DH参数
double theta[6];
theta[0] = pos_start[0];// + 0.0 / 180 * EIGEN_PI;
theta[1] = pos_start[1];// - 0.200247 / 180 * EIGEN_PI;
theta[2] = pos_start[2];// - 0.0241131 / 180 * EIGEN_PI;
theta[3] = pos_start[3];// + 0.107099 / 180 * EIGEN_PI;
theta[4] = pos_start[4];// - 0.535242 / 180 * EIGEN_PI;
theta[5] = pos_start[5];// + 0 / 180 * EIGEN_PI;
double D[6] = {137.5 / 1000,
0.0 / 1000,
0.0 / 1000,
120.941/ 1000,
118.061 / 1000,/* */
88.5 / 1000};
double A[6] = {0.0 / 1000,
-400.811 / 1000,
-373.927 / 1000,
0.0 / 1000,
0.0 / 1000,
0.0 / 1000};
double Alpha[6] = {90.0848 / 180 * EIGEN_PI,
0.499605 / 180 * EIGEN_PI,
0.251361 / 180 * EIGEN_PI,
89.9559 / 180 * EIGEN_PI,
-89.8948 / 180 * EIGEN_PI,
0.323088 / 180 * EIGEN_PI};
struct timeval tv,tv1,tv2,tv3,tv4,tv5,tv6,tv7;
gettimeofday(&tv,NULL);
printf(“start:%d\n”,(int)(tv.tv_usec));
double p[6], P[6];
double rpy[3];
double RPY[3];
double T[6][16] = {{0.0}};
double Theta[6]={0};
//lilun mubiao
p[0]=Tx[3];p[1]=Tx[7];p[2]=Tx[11];
double r[3][3]={{Tx[0],Tx[1],Tx[2]},{Tx[4],Tx[5],Tx[6]},{Tx[8],Tx[9],Tx[10]}};
rotationMatrixToRPY(r, rpy);
p[3]=rpy[0];p[4]=rpy[1];p[5]=rpy[2];

// 最大迭代次数
int maxIter = 10;
// 阻尼系数
double u = 0.01;
// H矩阵维度
int Dim = 6;
int updateJ = 1;
double error, error_last,error_1;


Eigen::MatrixXd J_ad(6, 6);
Eigen::VectorXd delta_P(6);
Eigen::VectorXd delta_theta(6);
Eigen::VectorXd delta_theta_1(6);
Eigen::MatrixXd J(6, 6);

delta_P<<0,0,0,0,0,0;
delta_theta<<0,0,0,0,0,0;
delta_theta_1<<0,0,0,0,0,0;
J<<0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0;
J_ad<<0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0;

for (int i = 1; i <= maxIter; i++)
{
    ///
    if (updateJ == 1)
    {gettimeofday(&tv2,NULL);

        kinematics_1(theta, D, A, Alpha,T);
        P[0]=T[5][3];P[1]=T[5][7];P[2]=T[5][11];
        double R[3][3]={{T[5][0],T[5][1],T[5][2]},{T[5][4],T[5][5],T[5][6]},{T[5][8],T[5][9],T[5][10]}};
        rotationMatrixToRPY(R, RPY);
        P[3]=RPY[0];P[4]=RPY[1];P[5]=RPY[2];
        gettimeofday(&tv3,NULL);


        delta_P(0) = p[0] - P[0];
        delta_P(1) = p[1] - P[1];
        delta_P(2) = p[2] - P[2];
        delta_P(3) = p[3] - P[3];
        delta_P(4) = p[4] - P[4];
        delta_P(5) = p[5] - P[5];

        gettimeofday(&tv4,NULL);

        J_ad=JacobiMatrix_1(theta, D, A, Alpha);
        gettimeofday(&tv5,NULL);


        for (int i = 0; i < 6; i++)
        {
            for (int j = 0; j < 6; j++)
            {
                J(i, j) = J_ad(i,j);
            }
        }

        error_last = (p[0] - P[0]) * (p[0] - P[0]) + (p[1] - P[1]) * (p[1] - P[1]) + (p[2] - P[2]) * (p[2] - P[2]);
    }
    gettimeofday(&tv6,NULL);

    delta_theta = (J.transpose() * J + u * Eigen::MatrixXd::Identity(Dim, Dim)).inverse() * J.transpose() * delta_P;

    gettimeofday(&tv7,NULL);

    // qDebug()  << delta_theta << endl;
    Theta[0] = theta[0] + delta_theta(0);
    Theta[1] = theta[1] + delta_theta(1);
    Theta[2] = theta[2] + delta_theta(2);
    Theta[3] = theta[3] + delta_theta(3);
    Theta[4] = theta[4] + delta_theta(4);
    Theta[5] = theta[5] + delta_theta(5);
    kinematics_1(Theta, D, A, Alpha,T);
    P[0]=T[5][3];P[1]=T[5][7];P[2]=T[5][11];
    double R[3][3]={{T[5][0],T[5][1],T[5][2]},{T[5][4],T[5][5],T[5][6]},{T[5][8],T[5][9],T[5][10]}};
    rotationMatrixToRPY(R, RPY);
    P[3]=RPY[0];P[4]=RPY[1];P[5]=RPY[2];

    delta_P(0) = p[0] - P[0]; delta_P(1) = p[1] - P[1];delta_P(2) = p[2] - P[2]; delta_P(3) = p[3] - P[3]; delta_P(4) = p[4] - P[4];
    delta_P(5) = p[5] - P[5];

    error = (p[0] - P[0]) * (p[0] - P[0]) + (p[1] - P[1]) * (p[1] - P[1]) + (p[2] - P[2]) * (p[2] - P[2]);
    // qDebug()  << delta_P << endl;

    if (error < error_last)
    {
        for (int k = 0; k < 6; k++)
        {
            theta[k] = Theta[k];
        }
        u = u / 10;
        error_last = error;
        //double err = sqrt(error_last);
        updateJ = 1;
    }
    else
    {
        u = u * 10;
        updateJ = 0;
    }
    
    //qDebug()  << "Time"<<i<<"= " << (time_end - time_start) << "ms; " <<"error="<<error <<endl;
    gettimeofday(&tv1,NULL);
    //printf("end:%d  ",(int)(tv1.tv_usec));

// qDebug() <<“k_us_b:”<<(int)(tv2.tv_usec)<<" “;
// qDebug() <<” k_us_a:%d “<<(int)(tv3.tv_usec);
// qDebug() <<” J_us_b:%d “<<(int)(tv4.tv_usec);
// qDebug() <<” J_us_a:%d “<<(int)(tv5.tv_usec);
// qDebug() <<” Pinv_us_b:%d “<<(int)(tv6.tv_usec);
// qDebug() <<” Pinv_us_a:%d “<<(int)(tv7.tv_usec);
qDebug() <<“modeling”<<(int)(tv5.tv_usec)-(int)(tv2.tv_usec);
qDebug() <<“Pinv”<<(int)(tv7.tv_usec)-(int)(tv6.tv_usec);
// qDebug() << (Theta[0]-theta_ori[0])/3.1415926180 << (Theta[1]-theta_ori[1])/3.1415926180 << (Theta[2]-theta_ori[2])/3.1415926180 << (Theta[3]-theta_ori[3])/3.1415926180 << (Theta[4]-theta_ori[4])/3.1415926180 << (Theta[5]-theta_ori[5])/3.1415926180 << endl;
qDebug() << P[0]<< " " << P[1] << " “<< P[2]<<” “<< p[0]<< " " << p[1] << " “<< p[2]<< " “<<P[0]-p[0]<< " " << P[1]-p[1] << " “<< P[2]-p[2] << " " << " error=”<<error << endl;
qDebug() <<” ed:”<<(int)(tv1.tv_usec)<<” single:”<<(int)(tv1.tv_usec)-(int)(tv2.tv_usec)<<" total:"<<(int)(tv1.tv_usec)-(int)(tv.tv_usec);

    if(error<0.0000000001){
         break;
     }

}
for(int i=0;i<6;i++){
    DiscartesPoint[i]=Theta[i];
}

}
int main(int argc, char *argv[])
{
double theta_ori[6] = {15.0 / 180 * EIGEN_PI,
-100.0 / 180 * EIGEN_PI,
20.0 / 180 * EIGEN_PI,
-80.0 / 180 * EIGEN_PI,
-45.0 / 180 * EIGEN_PI,
30.0 / 180 * EIGEN_PI};
float DH[6]={0.1375,
-0.401,
-0.376,
0.1205,
0.1205,
0.0885};
double *Theta;
Theta=new double[6];

//    for(int i=0; i<6;i++){
//        Theta[i]=0;
//    }
//ErrorCompensation_1(theta_ori, DH, Theta);

double D[6] = {137.5 / 1000,
               0.0 / 1000,
               0.0 / 1000,
               120.941/ 1000,
               118.061 / 1000,/*  */
               88.5 / 1000};
double A[6] = {0.0 / 1000,
               -400.811 / 1000,
               -373.927 / 1000,
               0.0 / 1000,
               0.0 / 1000,
               0.0 / 1000};
double Alpha[6] = {90.0848 / 180 * EIGEN_PI,
                   0.499605 / 180 * EIGEN_PI,
                   0.251361 / 180 * EIGEN_PI,
                   89.9559 / 180 * EIGEN_PI,
                   -89.8948 / 180 * EIGEN_PI,
                   0.323088 / 180 * EIGEN_PI};
double p[3]={0};
kinematics(theta_ori, D, A,  Alpha, p);
double Tx[16]={0};
Tx[3]=p[0];
Tx[7]=p[1];
Tx[11]=p[2];

struct timeval tv,tv1,tv2,tv3,tv4,tv5,tv6,tv7;

// gettimeofday(&tv1,NULL);
// ErrorCompensation_3(theta_ori, DH, Theta, Tx);
// gettimeofday(&tv2,NULL);
// ErrorCompensation_4(theta_ori, DH, Theta, Tx);
// gettimeofday(&tv3,NULL);
// ErrorCompensation_5(theta_ori, Theta, Tx);
// gettimeofday(&tv4,NULL);
// ErrorCompensation_6(theta_ori, Theta, Tx);
// gettimeofday(&tv5,NULL);
// ErrorCompensation_7(theta_ori, Theta, Tx);
// gettimeofday(&tv6,NULL);
// ErrorCompensation_8(theta_ori, Theta, Tx);
// gettimeofday(&tv7,NULL);
ErrorCompensation_1025(Tx,Theta,theta_ori);
ErrorCompensation_9(Tx,Theta,theta_ori);
// qDebug() <<“ErrorCompensation_3”<<(int)(tv2.tv_usec)-(int)(tv1.tv_usec);
// qDebug() <<“ErrorCompensation_4”<<(int)(tv3.tv_usec)-(int)(tv2.tv_usec);
// qDebug() <<“ErrorCompensation_5”<<(int)(tv4.tv_usec)-(int)(tv3.tv_usec);
// qDebug() <<“ErrorCompensation_6”<<(int)(tv5.tv_usec)-(int)(tv4.tv_usec);
// qDebug() <<“ErrorCompensation_7”<<(int)(tv6.tv_usec)-(int)(tv5.tv_usec);
// for(int i=0; i<6; i++)
// {
// gettimeofday(&tv6,NULL);
// ErrorCompensation_8(theta_ori, Theta, Tx);
// gettimeofday(&tv7,NULL);
// qDebug() <<“ErrorCompensation_8”<<(int)(tv7.tv_usec)-(int)(tv6.tv_usec);

// }

delete[] Theta;
QApplication a(argc, argv);
MainWindow w;
w.show();
return a.exec();

}

Zenker0418

关注

0
点赞
踩
0

收藏

觉得还不错? 一键收藏
0
评论
复制链接

分享到 QQ

分享到新浪微博

扫一扫