vcxvcb

最新推荐文章于 2024-06-14 23:48:57 发布

Zenker0418

最新推荐文章于 2024-06-14 23:48:57 发布

阅读量15

点赞数

文章标签： python

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

版权

#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);
printf("start:%d\n",(int)(tv.tv_usec));
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);

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);
}

}
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 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];

  ErrorCompensation_3(theta_ori, DH, Theta, Tx);
   ErrorCompensation_4(theta_ori, DH, Theta, Tx);
  delete[] Theta;
QApplication a(argc, argv);
MainWindow w;
w.show();
return a.exec();

}
#include “mainwindow.h”
#include
#include
#include
#include <eigen3/Eigen/Geometry>
void get_xyz(Eigen::Vector3d P1,
Eigen::Vector3d P2,
Eigen::Vector3d P3,
Eigen::Vector3d P4,
double &x, double &y, double &z)//空间四点确定球心坐标(克莱姆法则)
{
double x1=P1(0); double y1=P1(1); double z1=P1(2);
double x2=P2(0); double y2=P2(1); double z2=P2(2);
double x3=P3(0); double y3=P3(1); double z3=P3(2);
double x4=P4(0); double y4=P4(1); double z4=P4(2);
double a11,a12,a13,a21,a22,a23,a31,a32,a33,b1,b2,b3,d,d1,d2,d3;
a11=2*(x2-x1); a12=2*(y2-y1); a13=2*(z2-z1);
a21=2*(x3-x2); a22=2*(y3-y2); a23=2*(z3-z2);
a31=2*(x4-x3); a32=2*(y4-y3); a33=2*(z4-z3);
b1=x2x2-x1x1+y2y2-y1y1+z2z2-z1z1;
b2=x3x3-x2x2+y3y3-y2y2+z3z3-z2z2;
b3=x4x4-x3x3+y4y4-y3y3+z4z4-z3z3;
d=a11a22a33+a12a23a31+a13a21a32-a11a23a32-a12a21a33-a13a22a31;
d1=b1a22a33+a12a23b3+a13b2a32-b1a23a32-a12b2a33-a13a22b3;
d2=a11b2a33+b1a23a31+a13a21b3-a11a23b3-b1a21a33-a13b2a31;
d3=a11a22b3+a12b2a31+b1a21a32-a11b2a32-a12a21b3-b1a22a31;
x=d1/d;
y=d2/d;
z=d3/d;

}
void TCP_geometric_calibration(double Tx[4][16],double T_tool[16]){
int pointNum=4;
Eigen::Matrix3d RotationMatrix[4];
Eigen::Vector3d P_be[4];
Eigen::Vector3d P_bt;
Eigen::Vector3d P_et[4];
for(int i=0;i<pointNum;i++)
{
for(int m=0;m<3;m++)
{
for(int n=0;n<3;n++)
{
RotationMatrixi = Tx[i][m4+n];
}
P_bei = Tx[i][m4+3];
}
}
double x,y,z;
get_xyz(P_be[0],P_be[1],P_be[2],P_be[3],x,y,z);
P_bt(0)=x;
P_bt(1)=y;
P_bt(2)=z;
for (int i=0; i<4; i++){
P_et[i]=RotationMatrix[i].inverse()*(P_bt-P_be[i]);
qDebug()<<P_eti<<P_eti<<P_eti;
}

T_tool[3]=(P_et[0](0)+P_et[1](0)+P_et[2](0)+P_et[3](0))/4;
T_tool[7]=(P_et[0](0)+P_et[1](0)+P_et[2](0)+P_et[3](0))/4;
T_tool[11]=(P_et[0](0)+P_et[1](0)+P_et[2](0)+P_et[3](0))/4;

}
int main(int argc, char argv[])
{
// double x0=rand();
// double y0=rand();
// double z0=rand();
// double r=rand();
// qDebug()<<x0<<y0<<z0<<r;
// double theta1=rand();
// double phi1=rand();
// double x1=x0+rsin(theta1)cos(phi1);
// double y1=y0+rsin(theta1)sin(phi1);
// double z1=z0+rcos(theta1);
// double theta2=rand();
// double phi2=rand();
// double x2=x0+rsin(theta2)cos(phi2);
// double y2=y0+rsin(theta2)sin(phi2);
// double z2=z0+rcos(theta2);
// double theta3=rand();
// double phi3=rand();
// double x3=x0+rsin(theta3)cos(phi3);
// double y3=y0+rsin(theta3)sin(phi3);
// double z3=z0+rcos(theta3);
// double theta4=rand();
// double phi4=rand();
// double x4=x0+r*sin(theta4)cos(phi4);
// double y4=y0+rsin(theta4)sin(phi4);
// double z4=z0+rcos(theta4);
// double x,y,z;
// //get_xyz(x1,y1,z1,x2,y2,z2,x3,y3,z3,x4,y4,z4,x,y,z);
// qDebug()<<x<<y<<z;
double Tx[4][16]={0.121146, 0.905133, 0.407503, -0.350761, 0.968256, -0.0173308, -0.249357, -0.0694317, -0.218639, 0.424776, -0.8785, 0.299594, 0, 0, 0, 1,
-0.549799, 0.78906, 0.274054, -0.334246, 0.766886, 0.346789, 0.540022, -0.168092, 0.33107, 0.507071, -0.795783, 0.288264, 0, 0, 0, 1,
-0.155993, 0.879464, -0.449676, -0.249111, 0.915521, 0.299629, 0.268411, -0.133688, 0.370794, -0.369818, -0.851908, 0.305591, 0, 0, 0, 1,
0.0623122, 0.887835, -0.455923, -0.245275, 0.958953, -0.179874, -0.219213, -0.0703058, -0.276634, -0.423549, -0.8626, 0.310602, 0, 0, 0, 1};

double T_tool[16];
TCP_geometric_calibration(Tx,T_tool);
qDebug()<<"sd"<<T_tool[3]<<T_tool[7]<<T_tool[11];
QApplication a(argc, argv);
MainWindow w;
w.show();
return a.exec();

}

Zenker0418

关注

0
点赞
踩
0

收藏

觉得还不错? 一键收藏
0
评论
vcxvcb

double &x, double &y, double &z)//空间四点确定球心坐标(克莱姆法则)Eigen::Matrix4d T[6 + 1];// 涓轰簡鍜宼heta瀵瑰簲锛?// 补偿后DH参数。// 补偿后DH参数。
复制链接

扫一扫

vcxvcb

“相关推荐”对你有帮助么？