UR运动学正逆解
参考链接
//角度转弧度
double d2r(double degree) { return (degree/180.0) * M_PI;}
//弧度转角度
double r2d(double radian) { return (radian/M_PI) * 180.0;}
Eigen::MatrixXd kinematics(double* theta)
{
//ur5数据
// double d[6] = {0.089159, 0.00000, 0.00000, 0.109150, 0.094650, 0.082300 };
// double a[6] = {0.000000, -0.42500, -0.39225, 0.000000, 0.000000, 0.000000 };
// double alpha[6] = {1.570796, 0.00000, 0.00000, 1.570796, -1.570796, 0.000000 };
//ur5e数据
double d[6] = {0.162500, 0.00000, 0.00000, 0.133300, 0.099700, 0.099600};
double a[6] = {0.000000, -0.4250, -0.39220, 0.000000, 0.000000, 0.000000};
double alpha[6] = {1.570796, 0.00000, 0.00000, 1.570796, -1.570796, 0.000000};
Eigen::Matrix4d T[6];
for (int i = 0; i <6; ++i)
{
T[i](0, 0) = cos(theta[i]);
T[i](0, 1) = -sin(theta[i]) * cos(alpha[i]);
T[i](0, 2) = sin(theta[i]) * sin(alpha[i]);
T[i](0, 3) = a[i] * cos(theta[i]);
T[i](1, 0) = sin(theta[i]);
T[i](1, 1) = cos(theta[i]) * cos(alpha[i]);
T[i](1, 2) = -cos(theta[i]) * sin(alpha[i]);
T[i](1, 3) = a[i] * sin(theta[i]);
T[i](2, 0) = 0;
T[i](2, 1) = sin(alpha[i]);
T[i](2, 2) = cos(alpha[i]);
T[i](2, 3) = d[i];
T[i](3, 0) = 0;
T[i](3, 1) = 0;
T[i](3, 2) = 0;
T[i](3, 3) = 1;
}
Eigen::Matrix4d T06 = T[0] * T[1] * T[2] * T[3] * T[4] * T[5];
return T06;
}
int test_02()
{
//逆运动学 已知末端姿态, 求六个关节角
//正运动学 已知六个关节角, 求末端姿态
//ur5数据
// double d[6] = {0.089159, 0.00000, 0.00000, 0.109150, 0.094650, 0.082300 };
// double a[6] = {0.000000, -0.42500, -0.39225, 0.000000, 0.000000, 0.000000 };
//ur5e数据
double d[6] = {0.162500, 0.00000, 0.00000, 0.133300, 0.099700, 0.099600 };
double a[6] = {0.000000, -0.4250, -0.39220, 0.000000, 0.000000, 0.000000 };
double theta[8][6];
// double tcp[6] = {-0.117025, -0.435893, 0.151821, -0.012413, 3.111990, -0.018048};// tcp
// double joint_q[6] = {-88.235500, -104.851277, -130.634377,-33.804510, 91.673247, 1.317803};//关节角 角度
double joint_q1[6] = {-1.540, -1.830, -2.280, -0.590, 1.600, 0.023};//关节角 弧度
Eigen::Matrix4d T06 = kinematics(joint_q1);//正运动学 已知六个关节角(弧度), 求末端姿态T
//2.求解
double A, B, C, D, E, F, G, M, N;//用大写字母替代常数
//注意，由于数组下标从0开始的问题，矩阵第一行第一列的元素是(0,0)
//theta1 两个解
A = d[5] * T06(1, 2) - T06(1, 3);
B = d[5] * T06(0, 2) - T06(0, 3);
C = d[3];
//第一个解，赋给一到四组
theta[0][0] = atan2(A, B) - atan2(C, sqrt(A * A + B * B - C * C));
theta[1][0] = theta[0][0];
theta[2][0] = theta[0][0];
theta[3][0] = theta[0][0];
//第二个解，赋给五到八组
theta[4][0] = atan2(A, B) - atan2(C, -sqrt(A * A + B * B - C * C));
theta[5][0] = theta[4][0];
theta[6][0] = theta[4][0];
theta[7][0] = theta[4][0];
//theta5 四个解
//由theta[1][1]产生的第一个解，赋给一到二组
A = sin(theta[0][0]) * T06(0, 2) - cos(theta[0][0]) * T06(1, 2);
theta[0][4] = acos(A);
theta[1][4] = theta[0][4];
//由theta[1][1]产生的第二个解，赋给三到四组
theta[2][4] = -acos(A);
theta[3][4] = theta[2][4];
//由theta[5][1]产生的第一个解，赋给五到六组
A = sin(theta[4][0]) * T06(0, 2) - cos(theta[4][0]) * T06(1, 2);
theta[4][4] = acos(A);
theta[5][4] = theta[4][4];
//由theta[5][1]产生的第二个解，赋给七到八组
theta[6][4] = -acos(A);
theta[7][4] = theta[6][4];
//theta6 四个解
for (int i = 0; i <8; i = i + 2)
{
A = (sin(theta[i][0]) * T06(0, 0) - cos(theta[i][0]) * T06(1, 0));
B = (sin(theta[i][0]) * T06(0, 1) - cos(theta[i][0]) * T06(1, 1));
C = sin(theta[i][4]);
D = A * A + B * B - C * C;
if ((C <= -0.00001) || (C >= 0.000001)) {
theta[i][5] = atan2(A, B) - atan2(C, 0.00);
theta[i + 1][5] = atan2(A, B) - atan2(C, 0.00);
}
else
{
theta[i][5] = 0;
theta[i + 1][5] = 0;
}
}
//theta3 8组解
for (int i = 0; i <8; i = i + 2)
{
C = cos(theta[i][0]) * T06(0, 0) + sin(theta[i][0]) * T06(1, 0);
D = cos(theta[i][0]) * T06(0, 1) + sin(theta[i][0]) * T06(1, 1);
E = cos(theta[i][0]) * T06(0, 2) + sin(theta[i][0]) * T06(1, 2);
F = cos(theta[i][0]) * T06(0, 3) + sin(theta[i][0]) * T06(1, 3);
G = cos(theta[i][5]) * T06(2, 1) + sin(theta[i][5]) * T06(2, 0);
A = d[4] * (sin(theta[i][5]) * C + cos(theta[i][5]) * D) - d[5] * E + F;
B = T06(2, 3) - d[0] - T06(2, 2) * d[5] + d[4] * G;
//theta3
if (A * A + B * B <= (a[2 - 1] + a[3 - 1]) * (a[2 - 1] + a[3 - 1]))
{
theta[i][2] = acos((A * A + B * B - a[2 - 1] * a[2 - 1] - a[3 - 1] * a[3 - 1]) / (2 * a[2 - 1] * a[3 - 1]));
theta[i + 1][2] = -theta[i][2];
}
else
{
theta[i][2] = 0;
theta[i + 1][2] = 0;
}
}
//theta2 theta4
for (int i = 0; i < 8; ++i)
{
C = cos(theta[i][0]) * T06(0, 0) + sin(theta[i][0]) * T06(1, 0);
D = cos(theta[i][0]) * T06(0, 1) + sin(theta[i][0]) * T06(1, 1);
E = cos(theta[i][0]) * T06(0, 2) + sin(theta[i][0]) * T06(1, 2);
F = cos(theta[i][0]) * T06(0, 3) + sin(theta[i][0]) * T06(1, 3);
G = cos(theta[i][5]) * T06(2, 1) + sin(theta[i][5]) * T06(2, 0);
A = d[4] * (sin(theta[i][5]) * C + cos(theta[i][5]) * D) - d[5] * E + F;
B = T06(2, 3) - d[0] - T06(2, 2) * d[5] + d[4] * G;
M = ((a[3 - 1] * cos(theta[i][2]) + a[2 - 1]) * B - a[3 - 1] * sin(theta[i][2]) * A) / (a[2 - 1] * a[2 - 1] + a[3 - 1] * a[3 - 1] + 2 * a[2 - 1] * a[3 - 1] * cos(theta[i][2]));
N = (A + a[3 - 1] * sin(theta[i][2]) * M) / (a[3 - 1] * cos(theta[i][2]) + a[2 - 1]);
theta[i][1] = atan2(M, N);
//theta4
auto th = atan2((-sin(theta[i][5]) * C - cos(theta[i][5]) * D), G) - theta[i][1] - theta[i][2] ;
if(th > M_PI)
{
th -= 2*M_PI;
}
if(th < -M_PI)
{
th += 2*M_PI;
}
theta[i][3] = th;
}
for (int i = 0; i < 8; ++i)
{
std::cout << "[" << i << "] " ;
for (int j = 0; j <6; ++j)
{
auto th = theta[i][j];
//角度判断,如果不在[-180,180]调整
if(th >M_PI)
{
th -= 2*M_PI;
}
if(th < -M_PI)
{
th += 2*M_PI;
}
theta[i][j] = th;
std::cout << "theta" << j+1 << "=" << th *180/3.1415926 << " ";
}
kinematics(theta[i]);
std::cout<< std::endl;
}
return 0;
}