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;
float a5 = 0;
float a6 = 0;
float d1 = 0;
float d2 = 0;
float d3 = 50;
float d4 = 425;
float d5 = 0;
float d6 = 100;
Matrix4d m;
m << Poperator->m_edit_nx , Poperator->m_edit_ox , Poperator->m_edit_ax , Poperator->m_edit_px,
Poperator->m_edit_ny , Poperator->m_edit_oy , Poperator->m_edit_ay, Poperator->m_edit_py,
Poperator->m_edit_nz , Poperator->m_edit_oz , Poperator->m_edit_az ,Poperator->m_edit_pz,
0 , 0 , 0 ,1.0000;
float pax;
float pay;
float paz;
pax = m(0,3) - d6*m(0,2);
pay = m(1,3) - d6*m(1,2);
paz = m(2,3) - d6*m(2,2);
float t1;
float theta1_1;
float theta1_2;
float gg;
//float g1;
//float g2;
t1 = pax*pax + pay*pay-d3*d3;
if (t1>=0)
{
theta1_1 = atan2(pay,pax)-atan2(d3,sqrt(t1));
theta1_2 = atan2(pay,pax)-atan2(d3,-sqrt(t1));
// cout << "输出theta1_1 的值" << theta1_1*180/PI << endl;
//cout << "输出theta1_2 的值" << theta1_2*180/PI << endl;
}
//考虑复数
else
{
gg = sqrt(-t1);
/*g1 = real(gg);g2 = imag(gg);*/
theta1_1 = atan2(pay,pax)-atan2(gg,0);
theta1_2 = atan2(pay,pax)-atan2(gg,0);
}
//theta2计算
float t2_1;
float t2_2;
float t2_3;
float t2_4;
float theta2_1;
float theta2_2;
float theta2_3;
float theta2_4;
t2_1 = ((pax*cos(theta1_1)+pay*sin(theta1_1)-a1)*(pax*cos(theta1_1)+pay*sin(theta1_1)-a1) + a2*a2 + paz*paz - d4*d4)/(2*a2);
t2_2 = ((pax*cos(theta1_2)+pay*sin(theta1_2)-a1)*(pax*cos(theta1_2)+pay*sin(theta1_2)-a1) + a2*a2 + paz*paz - d4*d4)/(2*a2);
t2_3 = ((pax*cos(theta1_1)+pay*sin(theta1_1)-a1)*(pax*cos(theta1_1)+pay*sin(theta1_1)-a1) + paz*paz -t2_1*t2_1);
t2_4 = ((pax*cos(theta1_2)+pay*sin(theta1_2)-a1)*(pax*cos(theta1_2)+pay*sin(theta1_2)-a1) + paz*paz -t2_2*t2_2);
if (t2_3 >=0)
{
theta2_1 = atan2((pax*cos(theta1_1)+pay*sin(theta1_1)-a1),paz)-atan2(t2_1,sqrt(t2_3));
theta2_2 = atan2(pax*cos(theta1_1)+pay*sin(theta1_1)-a1,paz)-atan2(t2_1,-sqrt(t2_3));
}
if (t2_4 >=0)
{
theta2_3 = atan2(pax*cos(theta1_2)+pay*sin(theta1_2)-a1,paz)-atan2(t2_2,sqrt(t2_4));
theta2_4 = atan2(pax*cos(theta1_2)+pay*sin(theta1_2)-a1,paz)-atan2(t2_2,-sqrt(t2_4));
}
if (t2_3 <0)
{
gg = sqrt(-t2_3);
theta2_1 = atan2(pax*cos(theta1_1)+pay*sin(theta1_1)-a1,paz)-atan2(gg,0);
theta2_2 = atan2(pax*cos(theta1_1)+pay*sin(theta1_1)-a1,paz)-atan2(gg,0);
}
if (t2_4<0)
{
gg = sqrt(-t2_4);
theta2_3 = atan2(pax*cos(theta1_2)+pay*sin(theta1_2)-a1,paz)-atan2(gg,0);
theta2_4 = atan2(pax*cos(theta1_2)+pay*sin(theta1_2)-a1,paz)-atan2(gg,0);
}
/*cout << "输出theta2_1 的值" << theta2_1*180/PI+90 << endl;
cout << "输出theta2_2 的值" << theta2_2*180/PI+90 << endl;
cout << "输出theta2_3 的值" << theta2_3*180/PI+90 << endl;
cout << "输出theta2_4 的值" << theta2_4*180/PI+90 << endl;*/
//theta3
float theta3_1;
float theta3_2;
float theta3_3;
float theta3_4;
theta3_1 = atan2(pax*cos(theta1_1)+pay*sin(theta1_1)-a1-a2*cos(theta2_1),paz+a2*sin(theta2_1))-theta2_1;
theta3_2 = atan2(pax*cos(theta1_1)+pay*sin(theta1_1)-a1-a2*cos(theta2_2),paz+a2*sin(theta2_2))-theta2_2;
theta3_3 = atan2(pax*cos(theta1_2)+pay*sin(theta1_2)-a1-a2*cos(theta2_3),paz+a2*sin(theta2_3))-theta2_3;
theta3_4 = atan2(pax*cos(theta1_2)+pay*sin(theta1_2)-a1-a2*cos(theta2_4),paz+a2*sin(theta2_4))-theta2_4;
//cout << "输出theta3_1 的值" << theta3_1*180/PI-90 << endl;
//cout << "输出theta3_2 的值" << theta3_2*180/PI-90 << endl;
//cout << "输出theta3_3 的值" << theta3_3*180/PI-90 << endl;
//cout << "输出theta3_4 的值" << theta3_4*180/PI-90 << endl;
//腕部计算
Matrix3d ww1;
Matrix3d ww2;
Matrix3d ww3;
Matrix3d ww4;
ww1 << cos(theta2_1+theta3_1)*cos(theta1_1) , cos(theta2_1+theta3_1)*sin(theta1_1) ,-sin(theta2_1+theta3_1) ,
-sin(theta1_1) , cos(theta1_1) , 0,
sin(theta2_1+theta3_1)*cos(theta1_1) , sin(theta2_1+theta3_1)*sin(theta1_1) ,cos(theta2_1+theta3_1) ;
ww2 << cos(theta2_2+theta3_2)*cos(theta1_1) , cos(theta2_2+theta3_2)*sin(theta1_1) ,-sin(theta2_2+theta3_2) ,
-sin(theta1_1) , cos(theta1_1) , 0,
sin(theta2_2+theta3_2)*cos(theta1_1) , sin(theta2_2+theta3_2)*sin(theta1_1) ,cos(theta2_2+theta3_2) ;
ww3 << cos(theta2_3+theta3_3)*cos(theta1_2) , cos(theta2_3+theta3_3)*sin(theta1_2) ,-sin(theta2_3+theta3_3) ,
-sin(theta1_2) , cos(theta1_2) , 0,
sin(theta2_3+theta3_3)*cos(theta1_2) , sin(theta2_3+theta3_3)*sin(theta1_2) ,cos(theta2_3+theta3_3) ;
ww4 << cos(theta2_4+theta3_4)*cos(theta1_2) , cos(theta2_4+theta3_4)*sin(theta1_2) ,-sin(theta2_4+theta3_4) ,
-sin(theta1_2) , cos(theta1_2) , 0,
sin(theta2_4+theta3_4)*cos(theta1_2) , sin(theta2_4+theta3_4)*sin(theta1_2) ,cos(theta2_4+theta3_4) ;
Matrix3d pp1;
Matrix3d pp2;
Matrix3d pp3;
Matrix3d pp4;
Matrix3d mm1;
mm1= m.block(0,0, 3,3); //m.block表示提取m矩阵,具体语法百度
pp1 = ww1*mm1;
pp2 = ww2*mm1;
pp3 = ww3*mm1;
pp4 = ww4*mm1;
//cout << pp1<<endl;
//theta5
float theta5_1;
float theta5_2;
float theta5_3;
float theta5_4;
float theta5_5;
float theta5_6;
float theta5_7;
float theta5_8;
theta5_1 = atan2(sqrt(pp1(0,2)*pp1(0,2)+pp1(1,2)*pp1(1,2)), pp1(2,2));
theta5_2 = atan2(-sqrt(pp1(0,2)*pp1(0,2)+pp1(1,2)*pp1(1,2)), pp1(2,2));
theta5_3 = atan2(sqrt(pp2(0,2)*pp2(0,2)+pp2(1,2)*pp2(1,2)), pp2(2,2));
theta5_4 = atan2(-sqrt(pp2(0,2)*pp2(0,2)+pp2(1,2)*pp2(1,2)), pp2(2,2));
theta5_5 = atan2(sqrt(pp3(0,2)*pp3(0,2)+pp3(1,2)*pp3(1,2)), pp3(2,2));
theta5_6 = atan2(-sqrt(pp3(0,2)*pp3(0,2)+pp3(1,2)*pp3(1,2)), pp3(2,2));
theta5_7 = atan2(sqrt(pp4(0,2)*pp4(0,2)+pp4(1,2)*pp4(1,2)), pp4(2,2));
theta5_8 = atan2(-sqrt(pp4(0,2)*pp4(0,2)+pp4(1,2)*pp4(1,2)), pp4(2,2));
/*cout << "theta5_1 :" << theta5_1*180/PI <<endl;
cout << "theta5_2 :" << theta5_2*180/PI <<endl;
cout << "theta5_3 :" << theta5_3*180/PI <<endl;
cout << "theta5_4 :" << theta5_4*180/PI <<endl;
cout << "theta5_5 :" << theta5_5*180/PI <<endl;
cout << "theta5_6 :" << theta5_6*180/PI <<endl;
cout << "theta5_7 :" << theta5_7*180/PI <<endl;
cout << "theta5_8 :" << theta5_8*180/PI <<endl;*/
//theta4
float theta4_1;
float theta4_2;
float theta4_3;
float theta4_4;
float theta4_5;
float theta4_6;
float theta4_7;
float theta4_8;
float awx1;
float awx2;
float awx3;
float awx4;
float awy1;
float awy2;
float awy3;
float awy4;
awx1 = pp1(0,2);
awx2 = pp2(0,2);
awx3 = pp3(0,2);
awx4 = pp4(0,2);
awy1 = pp1(1,2);
awy2 = pp2(1,2);
awy3 = pp3(1,2);
awy4 = pp4(1,2);
if (abs(theta5_1)-0>0.001)
{
theta4_1 = atan2(awy1/sin(theta5_1), awx1/sin(theta5_1));
}
else
{
theta4_1 = rand()/(2*PI+1)-PI;
}
if (abs(theta5_2)-0>0.001)
{
theta4_2 = atan2(awy1/sin(theta5_2), awx1/sin(theta5_2));
}
else
{
theta4_2 = rand()/(2*PI+1)-PI;
}
if (abs(theta5_3)-0>0.001)
{
theta4_3 = atan2(awy2/sin(theta5_3), awx2/sin(theta5_3));
}
else
{
theta4_3 = rand()/(2*PI+1)-PI;
}
if (abs(theta5_4)-0>0.001)
{
theta4_4 = atan2(awy2/sin(theta5_4), awx2/sin(theta5_4));
}
else
{
theta4_4 = rand()/(2*PI+1)-PI;
}
if (abs(theta5_5)-0>0.001)
{
theta4_5 = atan2(awy3/sin(theta5_5), awx3/sin(theta5_5));
}
else
{
theta4_5 = rand()/(2*PI+1)-PI;
}
if (abs(theta5_6)-0>0.001)
{
theta4_6 = atan2(awy3/sin(theta5_6), awx3/sin(theta5_6));
}
else
{
theta4_6 = rand()/(2*PI+1)-PI;
}
if (abs(theta5_7)-0>0.001)
{
theta4_7 = atan2(awy4/sin(theta5_7), awx4/sin(theta5_7));
}
else
{
theta4_7 = rand()/(2*PI+1)-PI;
}
if (abs(theta5_8)-0>0.001)
{
theta4_8 = atan2(awy4/sin(theta5_8), awx4/sin(theta5_8));
}
else
{
theta4_8 = rand()/(2*PI+1)-PI;
}
//cout << "theta4_1 :" << theta4_1*180/PI <<endl;
//cout << "theta4_2 :" << theta4_2*180/PI <<endl;
//cout << "theta4_3 :" << theta4_3*180/PI <<endl;
//cout << "theta4_4 :" << theta4_4*180/PI <<endl;
//cout << "theta4_5 :" << theta4_5*180/PI <<endl;
//cout << "theta4_6 :" << theta4_6*180/PI <<endl;
//cout << "theta4_7 :" << theta4_7*180/PI <<endl;
//cout << "theta4_8 :" << theta4_8*180/PI <<endl;
//theta6
float theta6_1;
float theta6_2;
float theta6_3;
float theta6_4;
float theta6_5;
float theta6_6;
float theta6_7;
float theta6_8;
float owz1;
float owz2;
float owz3;
float owz4;
float nwz1;
float nwz2;
float nwz3;
float nwz4;
owz1 = pp1(2,1);
owz2 = pp2(2,1);
owz3 = pp3(2,1);
owz4 = pp4(2,1);
nwz1 = pp1(2,0);
nwz2 = pp2(2,0);
nwz3 = pp3(2,0);
nwz4 = pp4(2,0);
if (abs(theta5_1)-0>0.001)
{
theta6_1 = atan2(owz1/sin(theta5_1), -nwz1/sin(theta5_1));
}
else
{
theta6_1 = -theta4_1;
}
if (abs(theta5_2)-0>0.001)
{
theta6_2 = atan2(owz1/sin(theta5_2), -nwz1/sin(theta5_2));
}
else
{
theta6_2 = -theta4_2;
}
if (abs(theta5_3)-0>0.001)
{
theta6_3 = atan2(owz2/sin(theta5_3), -nwz2/sin(theta5_3));
}
else
{
theta6_3 = -theta4_3;
}
if (abs(theta5_4)-0>0.001)
{
theta6_4 = atan2(owz2/sin(theta5_4), -nwz2/sin(theta5_4));
}
else
{
theta6_4 = -theta4_4;
}
if (abs(theta5_5)-0>0.001)
{
theta6_5 = atan2(owz3/sin(theta5_5), -nwz3/sin(theta5_5));
}
else
{
theta6_5 = -theta4_5;
}
if (abs(theta5_6)-0>0.001)
{
theta6_6 = atan2(owz3/sin(theta5_6), -nwz3/sin(theta5_6));
}
else
{
theta6_6 = -theta4_6;
}
if (abs(theta5_7)-0>0.001)
{
theta6_7 = atan2(owz4/sin(theta5_7), -nwz4/sin(theta5_7));
}
else
{
theta6_7 = -theta4_7;
}
if (abs(theta5_8)-0>0.001)
{
theta6_8 = atan2(owz4/sin(theta5_8), -nwz4/sin(theta5_8));
}
else
{
theta6_8 = -theta4_8;
}
//cout << "theta6_1 :" << theta6_1*180/PI <<endl;
//cout << "theta6_2 :" << theta6_2*180/PI <<endl;
//cout << "theta6_3 :" << theta6_3*180/PI <<endl;
//cout << "theta6_4 :" << theta6_4*180/PI <<endl;
//cout << "theta6_5 :" << theta6_5*180/PI <<endl;
//cout << "theta6_6 :" << theta6_6*180/PI <<endl;
//cout << "theta6_7 :" << theta6_7*180/PI <<endl;
//cout << "theta6_8 :" << theta6_8*180/PI <<endl;
//生成解决方案
float theta1[6] = {0};
float theta2[6] = {0};
float theta3[6] = {0};
float theta4[6] = {0};
float theta5[6] = {0};
float theta6[6] = {0};
float theta7[6] = {0};
float theta8[6] = {0};
theta1[0] = theta1_1*180/PI; theta1[1] = theta2_1*180/PI+90; theta1[2] = theta3_1*180/PI-90;theta1[3] = theta4_1*180/PI; theta1[4] = theta5_1*180/PI; theta1[5] = theta6_1*180/PI;
theta2[0] = theta1_1*180/PI; theta2[1] = theta2_1*180/PI+90; theta2[2] = theta3_1*180/PI-90; theta2[3] = theta4_2*180/PI; theta2[4] = theta5_2*180/PI; theta2[5] = theta6_2*180/PI;
theta3[0] = theta1_1*180/PI; theta3[1] = theta2_2*180/PI+90; theta3[2] = theta3_2*180/PI-90; theta3[3] = theta4_3*180/PI; theta3[4] = theta5_3*180/PI; theta3[5] = theta6_3*180/PI;
theta4[0] = theta1_1*180/PI; theta4[1] = theta2_2*180/PI+90; theta4[2] = theta3_2*180/PI-90; theta4[3] = theta4_4*180/PI; theta4[4] = theta5_4*180/PI; theta4[5] = theta6_4*180/PI;
theta5[0] = theta1_2*180/PI; theta5[1] = theta2_3*180/PI+90; theta5[2] = theta3_3*180/PI-90; theta5[3] = theta4_5*180/PI; theta5[4] = theta5_5*180/PI; theta5[5] = theta6_5*180/PI;
theta6[0] = theta1_2*180/PI; theta6[1] = theta2_3*180/PI+90; theta6[2] = theta3_3*180/PI-90; theta6[3] = theta4_6*180/PI; theta6[4] = theta5_6*180/PI; theta6[5] = theta6_6*180/PI;
theta7[0] = theta1_2*180/PI; theta7[1] = theta2_4*180/PI+90; theta7[2] = theta3_4*180/PI-90; theta7[3] = theta4_7*180/PI; theta7[4] = theta5_7*180/PI; theta7[5] = theta6_7*180/PI;
theta8[0] = theta1_2*180/PI; theta8[1] = theta2_4*180/PI+90; theta8[2] = theta3_4*180/PI-90; theta8[3] = theta4_8*180/PI; theta8[4] = theta5_8*180/PI; theta8[5] = theta6_8*180/PI;
//以所有关节转动和最小为原则
float cha1 =0;
float cha2 =0;
float cha3 =0;
float cha4 =0;
float cha5 =0;
float cha6 =0;
float cha7 =0;
float cha8 =0;
cha1 = abs(ntheta1-theta1[0])+abs(ntheta2-theta1[1])+abs(ntheta3-theta1[2])+abs(ntheta4-theta1[3])+abs(ntheta5-theta1[4])+abs(ntheta6-theta1[5]);
cha2 = abs(ntheta1-theta2[0])+abs(ntheta2-theta2[1])+abs(ntheta3-theta2[2])+abs(ntheta4-theta2[3])+abs(ntheta5-theta2[4])+abs(ntheta6-theta2[5]);
cha3 = abs(ntheta1-theta3[0])+abs(ntheta2-theta3[1])+abs(ntheta3-theta3[2])+abs(ntheta4-theta3[3])+abs(ntheta5-theta3[4])+abs(ntheta6-theta3[5]);
cha4 = abs(ntheta1-theta4[0])+abs(ntheta2-theta4[1])+abs(ntheta3-theta4[2])+abs(ntheta4-theta4[3])+abs(ntheta5-theta4[4])+abs(ntheta6-theta4[5]);
cha5 = abs(ntheta1-theta5[0])+abs(ntheta2-theta5[1])+abs(ntheta3-theta5[2])+abs(ntheta4-theta5[3])+abs(ntheta5-theta5[4])+abs(ntheta6-theta5[5]);
cha6 = abs(ntheta1-theta6[0])+abs(ntheta2-theta6[1])+abs(ntheta3-theta6[2])+abs(ntheta4-theta6[3])+abs(ntheta5-theta6[4])+abs(ntheta6-theta6[5]);
cha7 = abs(ntheta1-theta7[0])+abs(ntheta2-theta7[1])+abs(ntheta3-theta7[2])+abs(ntheta4-theta7[3])+abs(ntheta5-theta7[4])+abs(ntheta6-theta7[5]);
cha8 = abs(ntheta1-theta8[0])+abs(ntheta2-theta8[1])+abs(ntheta3-theta8[2])+abs(ntheta4-theta8[3])+abs(ntheta5-theta8[4])+abs(ntheta6-theta8[5]);
vector<float> v(8) ;
v[0]=cha1;v[1]=cha2;v[2]=cha3;v[3]=cha4;v[4]=cha5;v[5]=cha6;v[6]=cha7;v[7]=cha8;
auto smallest = std::min_element(std::begin(v), std::end(v));
std::cout << "min element is " << *smallest<< " at position " << std::distance(std::begin(v), smallest) << std::endl;
switch(std::distance(std::begin(v), smallest))
{
case 0:
/* ntheta1 = theta1[0];
ntheta2 = theta1[1];
ntheta3 = theta1[2];
ntheta4 = theta1[3];
ntheta5 = theta1[4];
ntheta6 = theta1[5];*/
ntheta1 = floor( theta1[0] * 10000.000f + 0.5) / 10000.000f;
ntheta2 = floor(theta1[1] * 10000.000f + 0.5) / 10000.000f;
ntheta3 = floor(theta1[2] * 10000.000f + 0.5) / 10000.000f;
ntheta4 = floor(theta1[3] * 10000.000f + 0.5) / 10000.000f;
ntheta5 = floor(theta1[4] * 10000.000f + 0.5) / 10000.000f;
ntheta6 = floor(theta1[5] * 10000.000f + 0.5) / 10000.000f;
break;
case 1:
ntheta1 = floor( theta2[0] * 10000.000f + 0.5) / 10000.000f;
ntheta2 = floor(theta2[1] * 10000.000f + 0.5) / 10000.000f;
ntheta3 = floor(theta2[2] * 10000.000f + 0.5) / 10000.000f;
ntheta4 = floor(theta2[3] * 10000.000f + 0.5) / 10000.000f;
ntheta5 = floor(theta2[4] * 10000.000f + 0.5) / 10000.000f;
ntheta6 = floor(theta2[5] * 10000.000f + 0.5) / 10000.000f;
break;
case 2:
ntheta1 = floor( theta3[0] * 10000.000f + 0.5) / 10000.000f;
ntheta2 = floor(theta3[1] * 10000.000f + 0.5) / 10000.000f;
ntheta3 = floor(theta3[2] * 10000.000f + 0.5) / 10000.000f;
ntheta4 = floor(theta3[3] * 10000.000f + 0.5) / 10000.000f;
ntheta5 = floor(theta3[4] * 10000.000f + 0.5) / 10000.000f;
ntheta6 = floor(theta3[5] * 10000.000f + 0.5) / 10000.000f;
break;
case 3:
ntheta1 = floor( theta4[0] * 10000.000f + 0.5) / 10000.000f;
ntheta2 = floor(theta4[1] * 10000.000f + 0.5) / 10000.000f;
ntheta3 = floor(theta4[2] * 10000.000f + 0.5) / 10000.000f;
ntheta4 = floor(theta4[3] * 10000.000f + 0.5) / 10000.000f;
ntheta5 = floor(theta4[4] * 10000.000f + 0.5) / 10000.000f;
ntheta6 = floor(theta4[5] * 10000.000f + 0.5) / 10000.000f;
break;
case 4:
ntheta1 = floor( theta5[0] * 10000.000f + 0.5) / 10000.000f;
ntheta2 = floor(theta5[1] * 10000.000f + 0.5) / 10000.000f;
ntheta3 = floor(theta5[2] * 10000.000f + 0.5) / 10000.000f;
ntheta4 = floor(theta5[3] * 10000.000f + 0.5) / 10000.000f;
ntheta5 = floor(theta5[4] * 10000.000f + 0.5) / 10000.000f;
ntheta6 = floor(theta5[5] * 10000.000f + 0.5) / 10000.000f;
break;
case 5:
ntheta1 = floor( theta6[0] * 10000.000f + 0.5) / 10000.000f;
ntheta2 = floor(theta6[1] * 10000.000f + 0.5) / 10000.000f;
ntheta3 = floor(theta6[2] * 10000.000f + 0.5) / 10000.000f;
ntheta4 = floor(theta6[3] * 10000.000f + 0.5) / 10000.000f;
ntheta5 = floor(theta6[4] * 10000.000f + 0.5) / 10000.000f;
ntheta6 = floor(theta6[5] * 10000.000f + 0.5) / 10000.000f;
break;
case 6:
ntheta1 = floor( theta7[0] * 10000.000f + 0.5) / 10000.000f;
ntheta2 = floor(theta7[1] * 10000.000f + 0.5) / 10000.000f;
ntheta3 = floor(theta7[2] * 10000.000f + 0.5) / 10000.000f;
ntheta4 = floor(theta7[3] * 10000.000f + 0.5) / 10000.000f;
ntheta5 = floor(theta7[4] * 10000.000f + 0.5) / 10000.000f;
ntheta6 = floor(theta7[5] * 10000.000f + 0.5) / 10000.000f;
break;
case 7:
ntheta1 = floor( theta8[0] * 10000.000f + 0.5) / 10000.000f;
ntheta2 = floor(theta8[1] * 10000.000f + 0.5) / 10000.000f;
ntheta3 = floor(theta8[2] * 10000.000f + 0.5) / 10000.000f;
ntheta4 = floor(theta8[3] * 10000.000f + 0.5) / 10000.000f;
ntheta5 = floor(theta8[4] * 10000.000f + 0.5) / 10000.000f;
ntheta6 = floor(theta8[5] * 10000.000f + 0.5) / 10000.000f;
break;
}
}
相关参考链接:https://blog.csdn.net/u012428169/article/details/71169546
https://wenku.baidu.com/view/d1c5535de418964bcf84b9d528ea81c758f52e66.html
https://blog.csdn.net/yangyangyang20092010/article/details/47863477