void ErrorCompensation_14(double Tx[16], double DiscartesPoint[6], double pos_start[6])
{
std::cout << endl;
std::cout << “ErrorCompensation_14”;
// 补偿后DH参数
double theta[6];
theta[0] = pos_start[0]; //;
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]; // - 5.535242 / 180 * EIGEN_PI;
theta[5] = pos_start[5]; // 1 + 0 / 180 * EIGEN_PI;
double D[6] = {137.5,
0.0,
0.0,
120.941,
118.061, /* */
88.5};
double A[6] = {0.0,
-400.811,
-373.927,
0.0,
0.0,
0.0};
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;
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] * 1000;
p[1] = Tx[7] * 1000;
p[2] = Tx[11] * 1000;
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, error_r;
Eigen::MatrixXd J_ad = Eigen::MatrixXd::Zero(6, 6);
Eigen::VectorXd delta_P = Eigen::VectorXd::Zero(6);
Eigen::VectorXd delta_theta = Eigen::VectorXd::Zero(6);
Eigen::VectorXd delta_theta_1 = Eigen::VectorXd::Zero(6);
Eigen::MatrixXd J = Eigen::MatrixXd::Zero(6, 6);
for (int i = 1; i <= maxIter; i++)
{
///
if (updateJ == 1)
{
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];
J_ad = JacobiMatrix_1(theta, D, A, Alpha);
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]); // + (p[3] - P[3]) * (p[3] - P[3]) + (p[4] - P[4]) * (p[4] - P[4]) + (p[5] - P[5]) * (p[5] - P[5]);
}
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);
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]); // + (p[3] - P[3]) * (p[3] - P[3]) + (p[4] - P[4]) * (p[4] - P[4]) + (p[5] - P[5]) * (p[5] - P[5]);
// std::cout << 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;
}
if (error < 0.0001)
{
break;
}
}
for (int i = 0; i < 6; i++)
{
DiscartesPoint[i] = Theta[i];
}
cout << endl;
}