vcxvcb

最新推荐文章于 2024-11-16 19:19:32 发布
最新推荐文章于 2024-11-16 19:19:32 发布
阅读量26 收藏
点赞数
文章标签： python
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：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
关注
ks8 本地化部署 F5-TTS
陈锐的技术笔记
11-13 876
此外，F5-TTS 还创新性地引入了推理时的 Sway Sampling 策略，这一策略可以在推理阶段优先处理早期的流步骤，从而提高生成语音与输入文本的对齐效果。在 LibriSpeech-PC 数据集上，该模型的字错误率（WER）达到了2.42，并且在推理时的实时因子(RTF)为0.15，显著优于之前的扩散模型 E2TTS，后者在处理速度和鲁棒性上存在短板。传统的 TTS 模型往往需要进行复杂的持续时间建模、音素对齐和专门的文本编码，这些都增加了合成过程的复杂性。继续TSS的启动，进入pod 输入命令。
什么是Python模块化编程
大数据实战派
11-13 465
模块化编程是一种编程方法论，它将程序的功能划分为多个独立的模块，每个模块负责程序中的一部分功能。通过这种方式，程序的代码被分割成更小、更易管理的部分，每个模块都可以独立开发、调试和测试，模块之间通过接口进行交互。模块化编程是将一个复杂的程序拆分为多个独立、功能明确的小模块的编程方法。这不仅提高了代码的复用性和可维护性，还促进了团队协作开发，增强了程序的扩展性。在实际开发中，模块化编程让代码结构更加清晰，逻辑更加简洁，为程序的长久维护和扩展打下了坚实的基础。
外星人入侵
weixin_60243362的博客
11-12 2617
外星人入侵项目实践
使用`nohup`后台运行Python服务不输出日志避坑
A15216110998的专栏
11-14 293
这里的应替换为你的Python脚本名称。错误日志未输出这里使用了2>&1来将错误输出重定向到与标准输出相同的文件。代码中的print()语句未输出到日志有时候，由于Python的缓冲输出机制，print()到屏幕的内容可能不会立即显示在日志中。要解决这个问题，可以使用-u现在，print()语句应该会立即出现在日志文件中。其他注意事项如果你的脚本依赖于环境变量，确保在启动脚本之前已经设置了这些变量，或者在脚本中显式导入它们。考虑使用第三方库如logging。
pytorch基础-数据集导入
gudao07的博客
11-14 983
pytorch的数据导入，包括dataset,tensorboard,transform,dataloder等
Django前后端跨域问题解决
最新发布
一夜白头催人泪
11-16 193
在settings的中间件配置路径。在utils的cors下配置。
【python基础算法编程】
weixin_42333261的博客
11-12 588
这个函数使用两个指针，一个从字符串的开头开始，另一个从字符串的结尾开始。否则，函数递归调用自身，传入去掉首尾字符的子字符串。递归是一种直接的方法来计算斐波那契数列，但对于较大的 N 值，它会非常慢，因为它会重复计算相同的子问题。然后，它比较字符串的第一个和最后一个字符是否相等。如果它们不相等，函数返回。函数是一个生成器函数，用于生成从给定序列中选择指定长度的元素的所有排列。可以使用循环从最后一个元素开始，逐个访问每个元素，直到第一个元素。斐波那契数列是一个经典的数学序列，其中每个数字都是前两个数字的和。
YOLOv11改进，YOLOv11添加GnConv递归门控卷积，二次创新C3k2结构
weixin_44779079的博客
11-15 194
gnConv 是一种高效的空间交互操作，通过递归门控卷积实现长距离和高阶的空间交互。其核心思想是通过使用标准卷积、线性投影和逐元素乘法，模拟自注意力中的输入自适应空间混合，但具有更低的计算复杂度。输入自适应的空间交互在传统的卷积神经网络中，卷积操作是通过固定的卷积核对邻域进行加权求和，从而聚合邻域特征。这种方法的缺点是，卷积核是固定的，不具备自适应的能力。与此不同，gnConv 引入了自适应的空间交互，类似于 Transformer 中的自注意力机制。在自注意力机制中，通过多头自注意力（MHSA）
Python爬虫学习路线精简大纲！！！
qq_40835367的博客
11-13 400
Python爬虫学习路线精简大纲！！！
python设置两个软件定时启动
车轮的博客
11-15 661
软件名_path = os.getenv('软件名_PATH', r'软件路径.exe')软件名_path = os.getenv('软件名_PATH', r'软件路径.exe')# 创建调度器并设置时区为Asia/Shanghai。# 使用环境变量获取软件路径。# 启动 xxxxx。# 添加任务，每天下午x点x分执行。请自行替换软件路径及软件名字。
sklearn.datasets中make_classification函数
u013172930的博客
11-13 426
make_classification 是 sklearn.datasets 模块中的一个函数，用于生成一个用于分类任务的虚拟数据集。它允许你生成具有指定特征、类别和噪声等属性的人工数据集，通常用于测试和演示机器学习算法。通过from sklearn.datasets import make_classification导入。
Python 小高考篇（6）常见错误及排查
jzwalliser的博客
11-16 1048
提前搞清楚函数有哪些参数，以及参数个数，否则很容易犯这类错。若在一个变量定义之前就去读其它，则会出现此错误。
Scala中set和case class的特点和例题
2301_80781830的博客
11-12 419
///case class 的定义语法：case class 类名(属性名：类型,属性名：类型)// println(book1)//println会自动去调用对象的toString方法。////设置一个Book。有三个属性：书名，作者，价格。// //判断一个类的两个对象是否相等。// //判断依据是书名相同。////case class的特点。////3.它可以实现了一些方法。////Set:特点去重。////1.数据是只读的。////2.可以省略new。
text-embedding-ada-002
hshpy的博客
11-14 591
The。
【Python】使用PyTorch构建神经网络：从基础到实战
萧鼎的博客
11-13 825
PyTorch是一个由Facebook AI Research团队开发的开源深度学习框架，以其易用性和强大的动态计算图功能，逐渐成为研究和开发深度学习模型的首选工具之一。torch：提供张量（Tensor）运算，类似于NumPy，但可以在GPU上加速。torch.nn：专门用于构建神经网络的模块，包含大量的神经网络层和损失函数。在深入神经网络之前，让我们快速了解一些PyTorch的基础知识。PyTorch的是构建神经网络的基类。通过继承nn.Module并实现__init__和forward。
关于Python中主要容器类型（列表、元组、字符串、集合、字典），包括它们的特点、常用函数以及相互之间的区别。
2301_77058976的博客
11-13 178
Python中的容器是一种可以存储其他对象的数据结构。主要的容器类型包括列表（list）、元组（tuple）、字符串（String）集合（set）和字典（dictionary）。下面是每种容器的基本语法和一些常用函数。
P5719 【深基4.例3】分类平均 python
Morantkk的博客
11-13 124
注意：要熟练应用python保留小数位的用法 使用f“{num：.1f}”或者用format之类的。思路：分别得到A类数字和B类数然后求和求平均即可。
selenium元素定位校验以及遇到的元素操作问题记录
weixin_45531218的博客
11-15 601
selenium元素定位表达式的校验 几个元素定位或点击报错的问题结局方法记录
flask基础知识记录
wh柒八九的博客
11-14 228
本文记录下flask框架的一些基础知识与内容 文章目录flask框架概述 flask框架概述
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值