基於Eigen庫的TX90正逆運動學求解
阿新 • • 發佈：2019-01-12
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