Eigen库可以实现matlab上类似的矩阵运算,非常方便,相关介绍参考后边链接,机器人正逆运动学求解过程可参考我之前的博客,逆运动学在一定范围内基本正确,但还需要完善,以下是相关代码可供参考:
正运动学实现代码:
//师姐的参数,TX90,使用标准DH参数
void TX90robot::forward()
{
float theta1 = ntheta1*PI/180;
float theta2 = (ntheta2-90)*PI/180;
float theta3 = (ntheta3+90)*PI/180;
float theta4 = ntheta4*PI/180;
float theta5 = ntheta5*PI/180;
float theta6 = ntheta6*PI/180;
float alpha1 =-PI/2; float alpha2 = 0; float alpha3 = PI/2; float alpha4 = -PI/2; float alpha5 = PI/2; float alpha6 = 0;
float d1 = 0;float d2 = 0; float d3 = 50;float d4 = 425;float d5 = 0;float d6 = 100;
float a1 =50;float a2 =425;float a3 =0; float a4 = 0;float a5 = 0; float a6 = 0;
Matrix4d T1;
T1 << cos(theta1) , -sin(theta1)*cos(alpha1) , sin(theta1)*sin(alpha1) , a1*cos(theta1),
sin(theta1) , cos(theta1)*cos(alpha1) , -cos(theta1)*sin(alpha1), a1*sin(theta1),
0 , sin(alpha1) , cos(alpha1) , d1 ,
0 , 0 , 0 ,1.0000;
Matrix4d T2;
T2 << cos(theta2) , -sin(theta2)*cos(alpha2) , sin(theta2)*sin(alpha2) , a2*cos(theta2),
sin(theta2) , cos(theta2)*cos(alpha2) , -cos(theta2)*sin(alpha2), a2*sin(theta2),
0 , sin(alpha2) , cos(alpha2) , d2 ,
0 , 0 , 0 ,1.0000;
Matrix4d T3;
T3 << cos(theta3) , -sin(theta3)*cos(alpha3) , sin(theta3)*sin(alpha3) , a3*cos(theta3),
sin(theta3) , cos(theta3)*cos(alpha3) , -cos(theta3)*sin(alpha3), a3*sin(theta3),
0 , sin(alpha3) , cos(alpha3) , d3 ,
0 , 0 , 0 ,1.0000;
Matrix4d T4;
T4 << cos(theta4) , -sin(theta4)*cos(alpha4) , sin(theta4)*sin(alpha4) , a4*cos(theta4),
sin(theta4) , cos(theta4)*cos(alpha4) , -cos(theta4)*sin(alpha4), a4*sin(theta4),
0 , sin(alpha4) , cos(alpha4) , d4 ,
0 , 0 , 0 ,1.0000;
Matrix4d T5;
T5 << cos(theta5) , -sin(theta5)*cos(alpha5) , sin(theta5)*sin(alpha5) , a5*cos(theta5),
sin(theta5) , cos(theta5)*cos(alpha5) , -cos(theta5)*sin(alpha5), a5*sin(theta5),
0 , sin(alpha5) , cos(alpha5) , d5 ,
0 , 0 , 0 ,1.0000;
Matrix4d T6;
T6 << cos(theta6) , -sin(theta6)*cos(alpha6) , sin(theta6)*sin(alpha6) , a6*cos(theta6),
sin(theta6) , cos(theta6)*cos(alpha6) , -cos(theta6)*sin(alpha6), a6*sin(theta6),
0 , sin(alpha6) , cos(alpha6) , d6 ,
0 , 0 , 0 ,1.0000;
Matrix4d T;
T = T1*T2*T3*T4*T5*T6;
float nx;float ny; float nz;
float ox;float oy;float oz;
float ax;float ay;float az;
float px;float py;float pz;
nx = T(0,0);
ny = T(1,0);
nz = T(2,0);
ox = T(0,1);
oy = T(1,1);
oz = T(2,1);
ax = T(0,2);
ay = T(1,2);
az = T(2,2);
px = T(0,3);
py = T(1,3);
pz = T(2,3);
//double b;
//b = floor(a * 10000.000f + 0.5) / 10000.000f; /*保留小数点后四位*/
//四舍五入,保留4位
float output[12] = {0};
output[0] = floor(nx * 10000.000f + 0.5) / 10000.000f; output[1] = floor(ny * 10000.000f + 0.5) / 10000.000f; output[2] = floor(nz * 10000.000f + 0.5) / 10000.000f;
output[3] = floor(ox * 10000.000f + 0.5) / 10000.000f; output[4] = floor(oy * 10000.000f + 0.5) / 10000.000f; output[5] = floor(oz * 10000.000f + 0.5) / 10000.000f;
output[6] = floor(ax * 10000.000f + 0.5) / 10000.000f; output[7] = floor(ay * 10000.000f + 0.5) / 10000.000f; output[8] = floor(az * 10000.000f + 0.5) / 10000.000f;
output[9] = floor(px * 10000.000f + 0.5) / 10000.000f; output[10] = floor(py * 10000.000f + 0.5) / 10000.000f; output[11] = floor(pz * 10000.000f + 0.5) / 10000.000f;
//int i;
//double epsilon = 1.0e-6;
//for (i = 0 ;i<12 ;i++ )
//{
// if ((epsilon >=output[i] )&&(-epsilon<=output[i]))
// {
// output[i] = 0;
// }
//}
Poperator->m_edit_nx = output[0];Poperator->m_edit_ny = output[1]; Poperator->m_edit_nz = output[2];
Poperator->m_edit_ox = output[3] ;Poperator->m_edit_oy =output[4]; Poperator->m_edit_oz = output[5];
Poperator->m_edit_ax = output[6];Poperator->m_edit_ay = output[7]; Poperator->m_edit_az =output[8];
Poperator->m_edit_px = output[9];Poperator->m_edit_py =output[10]; Poperator->m_edit_pz = output[11];
}
逆运动学代码如下:
void::TX90robot::inverse()
{
//连杆参数
float a1 = 50;
float a2 = 425;
float a3 = 0;
float a4 = 0;
floa