int main(int argc, char** argv) { IKREAL_TYPE eerot[9], eetrans[3]; unsigned int c; std::size_t pos1, pos2; q_t q(6,0.0); int iter = 0; bool endOfDocument = false; unsigned int num_of_joints = 6; IKREAL_TYPE jointsComputed[num_of_joints]; double jointLimitMin[] = {J0_MIN, J1_MIN, J2_MIN, J3_MIN, J4_MIN, J5_MIN}; double jointLimitMax[] = {J0_MAX, J1_MAX, J2_MAX, J3_MAX, J4_MAX, J5_MAX}; bool jointViolation = false; bool newline=false; std::string line; std::ifstream robot; std::ofstream q_ikfast, t_ikfast; q_ikfast.open(TEST_DATA_PATH_Q, std::ios::app); t_ikfast.open(TEST_DATA_PATH_T, std::ios::app); robot.open(ALL_Q_PATH); // check file exists if(!robot.is_open()){ std::cout << "NO such file. Exit." << std::endl; exit(-1); } std::getline(robot, line); //check file is not empty endOfDocument = robot.eof(); if(endOfDocument){ exit(-1); } while (!endOfDocument /*&& (iter<ITER_MAX)*/) { newline=false; // for each ee pose, constraint all joint solutions while(line!="\0"){ jointViolation=false; // Parsing q_t q; pos1=0; while(pos1 != std::string::npos) { pos2 = line.find(DELIMITER, pos1+1); std::string t; if(pos1==0){ t = line.substr(pos1, pos2-pos1); } else { t = line.substr(pos1+2, pos2-pos1-1); } //std::cout << t << " " << atof(t.c_str()) << std::endl; pos1 = line.find(DELIMITER,pos1+1); q.push_back(atof(t.c_str())); } // Apply joint constraints for(std::size_t j=0;j<q.size();j++){ if(q[j]<jointLimitMin[j]){ if( (q[j]+2*M_PI)<jointLimitMax[j]){ q[j] +=2*M_PI; } else{ jointViolation=true; break; //joint violation } } else if(q[j]>jointLimitMax[j]){ if( (q[j]-2*M_PI)>jointLimitMin[j]){ q[j]-=2*M_PI; } else{ jointViolation=true; break; //joint violation } } } // If this joint violates limits, get the next joint solution if(jointViolation){ std::getline(robot, line); //get next joint solution for same ee pose endOfDocument=robot.eof(); if(endOfDocument){ newline=false; break; } continue; } // At this point, we have a joint solution beyond limits. // So when we finish processing this ee pose, we break a new line newline=true; // Save result to file q_ikfast <<q[0]<<" "<<q[1]<<" "<<q[2]<<" "<<q[3]<<" "<<q[4]<<" "<<q[5]<<" "<<"\n"; for(c=0;c<num_of_joints;c++){ jointsComputed[c]=q[c]; } ComputeFk(jointsComputed, eetrans, eerot); t_ikfast << eerot[0] << " " << eerot[1] << " " << eerot[2] << " " << eetrans[0] << " " << eerot[3] << " " << eerot[4] << " " << eerot[5] << " " << eetrans[1] << " " << eerot[6] << " " << eerot[7] << " " << eerot[8] << " " << eetrans[2] << "\n"; std::getline(robot, line); //get next joint solution for same ee pose endOfDocument = robot.eof(); if(endOfDocument){ newline=false; break; } } std::getline(robot, line); //get first joint solution for new ee pose endOfDocument = robot.eof(); if(endOfDocument){ newline=false; break; } // Separates set of joint solutions for one ee pose by a newline. if(newline){ q_ikfast << "\n"; t_ikfast << "\n"; } iter ++; } robot.close(); q_ikfast.close(); t_ikfast.close(); return 0; }
int main(int argc, char** argv) { IKREAL_TYPE eerot[9],eetrans[3]; #if IK_VERSION > 54 // for IKFast 56,61 unsigned int num_of_joints = GetNumJoints(); unsigned int num_free_parameters = GetNumFreeParameters(); #else // for IKFast 54 unsigned int num_of_joints = getNumJoints(); unsigned int num_free_parameters = getNumFreeParameters(); #endif std::string cmd; if (argv[1]) cmd = argv[1]; //printf("command: %s \n\n", cmd.c_str() ); if (cmd.compare("ik") == 0) // ik mode { if( argc == 1+7+num_free_parameters+1 ) // ik, given translation vector and quaternion pose { #if IK_VERSION > 54 // for IKFast 56,61 IkSolutionList<IKREAL_TYPE> solutions; #else // for IKFast 54 std::vector<IKSolution> vsolutions; #endif std::vector<IKREAL_TYPE> vfree(num_free_parameters); eetrans[0] = atof(argv[2]); eetrans[1] = atof(argv[3]); eetrans[2] = atof(argv[4]); // Convert input effector pose, in w x y z quaternion notation, to rotation matrix. // Must use doubles, else lose precision compared to directly inputting the rotation matrix. double qw = atof(argv[5]); double qx = atof(argv[6]); double qy = atof(argv[7]); double qz = atof(argv[8]); const double n = 1.0f/sqrt(qx*qx+qy*qy+qz*qz+qw*qw); qw *= n; qx *= n; qy *= n; qz *= n; eerot[0] = 1.0f - 2.0f*qy*qy - 2.0f*qz*qz; eerot[1] = 2.0f*qx*qy - 2.0f*qz*qw; eerot[2] = 2.0f*qx*qz + 2.0f*qy*qw; eerot[3] = 2.0f*qx*qy + 2.0f*qz*qw; eerot[4] = 1.0f - 2.0f*qx*qx - 2.0f*qz*qz; eerot[5] = 2.0f*qy*qz - 2.0f*qx*qw; eerot[6] = 2.0f*qx*qz - 2.0f*qy*qw; eerot[7] = 2.0f*qy*qz + 2.0f*qx*qw; eerot[8] = 1.0f - 2.0f*qx*qx - 2.0f*qy*qy; // For debugging, output the matrix for (unsigned char i=0; i<=8; i++) { // detect -0.0 and replace with 0.0 if ( ((int&)(eerot[i]) & 0xFFFFFFFF) == 0) eerot[i] = 0.0; } printf(" Rotation %f %f %f \n", eerot[0], eerot[1], eerot[2] ); printf(" %f %f %f \n", eerot[3], eerot[4], eerot[5] ); printf(" %f %f %f \n", eerot[6], eerot[7], eerot[8] ); printf("\n"); for(std::size_t i = 0; i < vfree.size(); ++i) vfree[i] = atof(argv[13+i]); #if IK_VERSION > 54 // for IKFast 56,61 bool bSuccess = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions); #else // for IKFast 54 bool bSuccess = ik(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, vsolutions); #endif if( !bSuccess ) { fprintf(stderr,"Failed to get ik solution\n"); //return -1; } #if IK_VERSION > 54 // for IKFast 56,61 unsigned int num_of_solutions = (int)solutions.GetNumSolutions(); #else // for IKFast 54 unsigned int num_of_solutions = (int)vsolutions.size(); #endif printf("Found %d ik solutions:\n", num_of_solutions ); std::vector<IKREAL_TYPE> solvalues(num_of_joints); for(std::size_t i = 0; i < num_of_solutions; ++i) { #if IK_VERSION > 54 // for IKFast 56,61 const IkSolutionBase<IKREAL_TYPE>& sol = solutions.GetSolution(i); int this_sol_free_params = (int)sol.GetFree().size(); #else // for IKFast 54 int this_sol_free_params = (int)vsolutions[i].GetFree().size(); #endif printf("sol%d (free=%d): ", (int)i, this_sol_free_params ); std::vector<IKREAL_TYPE> vsolfree(this_sol_free_params); #if IK_VERSION > 54 // for IKFast 56,61 sol.GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL); #else // for IKFast 54 vsolutions[i].GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL); #endif for( std::size_t j = 0; j < solvalues.size(); ++j) printf("%.15f, ", solvalues[j]); printf("\n"); } } else if( argc == 1+12+num_free_parameters+1 ) // ik, given rotation-translation matrix { #if IK_VERSION > 54 // for IKFast 56,61 IkSolutionList<IKREAL_TYPE> solutions; #else // for IKFast 54 std::vector<IKSolution> vsolutions; #endif std::vector<IKREAL_TYPE> vfree(num_free_parameters); eerot[0] = atof(argv[2]); eerot[1] = atof(argv[3]); eerot[2] = atof(argv[4]); eetrans[0] = atof(argv[5]); eerot[3] = atof(argv[6]); eerot[4] = atof(argv[7]); eerot[5] = atof(argv[8]); eetrans[1] = atof(argv[9]); eerot[6] = atof(argv[10]); eerot[7] = atof(argv[11]); eerot[8] = atof(argv[12]); eetrans[2] = atof(argv[13]); for(std::size_t i = 0; i < vfree.size(); ++i) vfree[i] = atof(argv[14+i]); printf("translation: \n"); for (unsigned int i = 0; i < 3; i++) { printf("%lf ", eetrans[i]); } printf("\n"); printf("rotation matix: \n"); for (unsigned int i = 0; i < 9; i++) { printf("%lf%s", eerot[i], (i % 3 == 2)?"\n":" "); } printf("\n"); printf("vfree:\n"); for(std::size_t i = 0; i < vfree.size(); ++i) printf("%lf ", vfree[i]); printf("\n"); #if IK_VERSION > 54 // for IKFast 56,61 bool bSuccess = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions); #else // for IKFast 54 bool bSuccess = ik(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, vsolutions); #endif if( !bSuccess ) { fprintf(stderr,"Failed to get ik solution\n"); return -1; } #if IK_VERSION > 54 // for IKFast 56,61 unsigned int num_of_solutions = (int)solutions.GetNumSolutions(); #else // for IKFast 54 unsigned int num_of_solutions = (int)vsolutions.size(); #endif printf("Found %d ik solutions:\n", num_of_solutions ); std::vector<IKREAL_TYPE> solvalues(num_of_joints); for(std::size_t i = 0; i < num_of_solutions; ++i) { #if IK_VERSION > 54 // for IKFast 56,61 const IkSolutionBase<IKREAL_TYPE>& sol = solutions.GetSolution(i); int this_sol_free_params = (int)sol.GetFree().size(); #else // for IKFast 54 int this_sol_free_params = (int)vsolutions[i].GetFree().size(); #endif printf("sol%d (free=%d): ", (int)i, this_sol_free_params ); std::vector<IKREAL_TYPE> vsolfree(this_sol_free_params); #if IK_VERSION > 54 // for IKFast 56,61 sol.GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL); #else // for IKFast 54 vsolutions[i].GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL); #endif for( std::size_t j = 0; j < solvalues.size(); ++j) printf("%.15f, ", solvalues[j]); printf("\n"); } } else { printf("\n " "Usage: \n\n " " ./compute ik t0 t1 t2 qw qi qj qk free0 ...\n\n " " Returns the ik solutions given the transformation of the end effector specified by \n " " a 3x1 translation (tX), and a 1x4 quaternion (w + i + j + k). \n " " There are %d free parameters that have to be specified.\n\n", num_free_parameters ); printf(" \n " " ./compute ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\n\n " " Returns the ik solutions given the transformation of the end effector specified by \n " " a 3x3 rotation R (rXX), and a 3x1 translation (tX). \n " " There are %d free parameters that have to be specified.\n\n", num_free_parameters ); return 1; } } // endif ik mode else if (cmd.compare("fk") == 0) // fk mode { if( argc != num_of_joints+2 ) { printf("\n " "Usage: \n\n " " ./compute fk j0 j1 ... j%d \n\n" " Returns the forward kinematic solution given the joint angles (in radians). \n\n", num_of_joints-1 ); return 1; } printf("\n\n"); // Put input joint values into array IKREAL_TYPE joints[num_of_joints]; for (unsigned int i=0; i<num_of_joints; i++) { joints[i] = atof(argv[i+2]); } #if IK_VERSION > 54 // for IKFast 56,61 ComputeFk(joints, eetrans, eerot); // void return #else // for IKFast 54 fk(joints, eetrans, eerot); // void return #endif printf("Found fk solution for end frame: \n\n"); printf(" Translation: x: %f y: %f z: %f \n", eetrans[0], eetrans[1], eetrans[2] ); printf("\n"); printf(" Rotation %f %f %f \n", eerot[0], eerot[1], eerot[2] ); printf(" Matrix: %f %f %f \n", eerot[3], eerot[4], eerot[5] ); printf(" %f %f %f \n", eerot[6], eerot[7], eerot[8] ); printf("\n"); // Display equivalent Euler angles float yaw; float pitch; float roll; if ( eerot[5] > 0.998 || eerot[5] < -0.998 ) { // singularity yaw = IKatan2( -eerot[6], eerot[0] ); pitch = 0; } else { yaw = IKatan2( eerot[2], eerot[8] ); pitch = IKatan2( eerot[3], eerot[4] ); } roll = IKasin( eerot[5] ); printf(" Euler angles: \n"); printf(" Yaw: %f ", yaw ); printf("(1st: rotation around vertical blue Z-axis in ROS Rviz) \n"); printf(" Pitch: %f \n", pitch ); printf(" Roll: %f \n", roll ); printf("\n"); // Convert rotation matrix to quaternion (Daisuke Miyazaki) float q0 = ( eerot[0] + eerot[4] + eerot[8] + 1.0f) / 4.0f; float q1 = ( eerot[0] - eerot[4] - eerot[8] + 1.0f) / 4.0f; float q2 = (-eerot[0] + eerot[4] - eerot[8] + 1.0f) / 4.0f; float q3 = (-eerot[0] - eerot[4] + eerot[8] + 1.0f) / 4.0f; if(q0 < 0.0f) q0 = 0.0f; if(q1 < 0.0f) q1 = 0.0f; if(q2 < 0.0f) q2 = 0.0f; if(q3 < 0.0f) q3 = 0.0f; q0 = sqrt(q0); q1 = sqrt(q1); q2 = sqrt(q2); q3 = sqrt(q3); if(q0 >= q1 && q0 >= q2 && q0 >= q3) { q0 *= +1.0f; q1 *= SIGN(eerot[7] - eerot[5]); q2 *= SIGN(eerot[2] - eerot[6]); q3 *= SIGN(eerot[3] - eerot[1]); } else if(q1 >= q0 && q1 >= q2 && q1 >= q3) { q0 *= SIGN(eerot[7] - eerot[5]); q1 *= +1.0f; q2 *= SIGN(eerot[3] + eerot[1]); q3 *= SIGN(eerot[2] + eerot[6]); } else if(q2 >= q0 && q2 >= q1 && q2 >= q3) { q0 *= SIGN(eerot[2] - eerot[6]); q1 *= SIGN(eerot[3] + eerot[1]); q2 *= +1.0f; q3 *= SIGN(eerot[7] + eerot[5]); } else if(q3 >= q0 && q3 >= q1 && q3 >= q2) { q0 *= SIGN(eerot[3] - eerot[1]); q1 *= SIGN(eerot[6] + eerot[2]); q2 *= SIGN(eerot[7] + eerot[5]); q3 *= +1.0f; } else { printf("Error while converting to quaternion! \n"); } float r = NORM(q0, q1, q2, q3); q0 /= r; q1 /= r; q2 /= r; q3 /= r; printf(" Quaternion: %f %f %f %f \n", q0, q1, q2, q3 ); printf(" "); // print quaternion with convention and +/- signs such that it can be copy-pasted into WolframAlpha.com printf("%f ", q0); if (q1 > 0) printf("+ %fi ", q1); else if (q1 < 0) printf("- %fi ", -q1); else printf("+ 0.00000i "); if (q2 > 0) printf("+ %fj ", q2); else if (q2 < 0) printf("- %fj ", -q2); else printf("+ 0.00000j "); if (q3 > 0) printf("+ %fk ", q3); else if (q3 < 0) printf("- %fk ", -q3); else printf("+ 0.00000k "); printf(" (alternate convention) \n"); printf("\n\n"); } else if (cmd.compare("iktiming") == 0) // generate random ik and check time performance { if( argc != 2 ) { printf("\n " "Usage: \n\n " " ./compute iktiming \n\n" " For fixed number of iterations, generates random joint angles, then \n" " calculates fk, calculates ik, measures average time taken. \n\n", num_of_joints-1 ); return 1; } printf("\n\n"); #if IK_VERSION > 54 // for IKFast 56,61 IkSolutionList<IKREAL_TYPE> solutions; #else // for IKFast 54 std::vector<IKSolution> vsolutions; #endif std::vector<IKREAL_TYPE> vfree(num_free_parameters); //for(std::size_t i = 0; i < vfree.size(); ++i) // vfree[i] = atof(argv[13+i]); srand( (unsigned)time(0) ); // seed random number generator float min = -3.14; float max = 3.14; float rnd; IKREAL_TYPE joints[num_of_joints]; timespec start_time, end_time; unsigned int elapsed_time = 0; unsigned int sum_time = 0; unsigned int fail_count = 0; #if IK_VERSION > 54 // for IKFast 56,61 unsigned int num_of_tests = 10; #else // for IKFast 54 unsigned int num_of_tests = 100000; #endif for (unsigned int i=0; i < num_of_tests; i++) { // Measure avg time for whole process //clock_gettime(CLOCK_REALTIME, &start_time); // Put random joint values into array for (unsigned int i=0; i<num_of_joints; i++) { float rnd = (float)rand() / (float)RAND_MAX; joints[i] = min + rnd * (max - min); } /* printf("Joint angles: "); for (unsigned int i=0; i<num_of_joints; i++) { printf("%f ", joints[i] ); } printf("\n"); */ #if IK_VERSION > 54 // for IKFast 56,61 ComputeFk(joints, eetrans, eerot); // void return #else // for IKFast 54 fk(joints, eetrans, eerot); // void return #endif // Measure avg time for IK clock_gettime(CLOCK_REALTIME, &start_time); #if IK_VERSION > 54 // for IKFast 56,61 vfree[0] = 5; if (!ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions)) { fail_count++; } else { printf("joint angles: \n"); for (unsigned int i=0; i<num_of_joints; i++) { printf("%lf ", joints[i]); } printf("\n"); printf("translation: \n"); for (unsigned int i = 0; i < 3; i++) { printf("%lf ", eetrans[i]); } printf("\n"); printf("rotation matix: \n"); for (unsigned int i = 0; i < 9; i++) { printf("%lf%s", eerot[i], (i % 3 == 2)?"\n":" "); } printf("\n"); /*unsigned int num_of_solutions = (int)solutions.GetNumSolutions(); printf("Found %d ik solutions:\n", num_of_solutions ); std::vector<IKREAL_TYPE> solvalues(num_of_joints); for(std::size_t i = 0; i < num_of_solutions; ++i) { const IkSolutionBase<IKREAL_TYPE>& sol = solutions.GetSolution(i); int this_sol_free_params = (int)sol.GetFree().size(); printf("sol%d (free=%d): ", (int)i, this_sol_free_params ); std::vector<IKREAL_TYPE> vsolfree(this_sol_free_params); sol.GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL); for( std::size_t j = 0; j < solvalues.size(); ++j) printf("%.15f, ", solvalues[j]); printf("\n"); }*/ } #else // for IKFast 54 ik(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, vsolutions); #endif /* #if IK_VERSION > 54 // for IKFast 56,61 unsigned int num_of_solutions = (int)solutions.GetNumSolutions(); #else // for IKFast 54 unsigned int num_of_solutions = (int)vsolutions.size(); #endif printf("Found %d ik solutions:\n", num_of_solutions ); */ clock_gettime(CLOCK_REALTIME, &end_time); elapsed_time = (unsigned int)(end_time.tv_nsec - start_time.tv_nsec); sum_time += elapsed_time; } // endfor unsigned int avg_time = (unsigned int)sum_time / (unsigned int)num_of_tests; printf("avg time: %f ms over %d tests \n", (float)avg_time/1000.0, num_of_tests ); printf("Failed of %d tests\n", fail_count); return 1; } else if (cmd.compare("iktiming2") == 0) // for fixed joint values, check time performance of ik { if( argc != 2 ) { printf("\n " "Usage: \n\n " " ./compute iktiming2 \n\n" " For fixed number of iterations, with one set of joint variables, this \n" " finds the ik solutions and measures the average time taken. \n\n", num_of_joints-1 ); return 1; } printf("\n\n"); #if IK_VERSION > 54 // for IKFast 56,61 IkSolutionList<IKREAL_TYPE> solutions; #else // for IKFast 54 std::vector<IKSolution> vsolutions; #endif std::vector<IKREAL_TYPE> vfree(num_free_parameters); //for(std::size_t i = 0; i < vfree.size(); ++i) // vfree[i] = atof(argv[13+i]); IKREAL_TYPE joints[num_of_joints]; timespec start_time, end_time; unsigned int elapsed_time = 0; unsigned int sum_time = 0; #if IK_VERSION > 54 // for IKFast 56,61 unsigned int num_of_tests = 1000000; #else // for IKFast 54 unsigned int num_of_tests = 100000; #endif // fixed rotation-translation matrix corresponding to an unusual robot pose eerot[0] = 0.002569; eerot[1] = -0.658044; eerot[2] = -0.752975; eetrans[0] = 0.121937; eerot[3] = 0.001347; eerot[4] = -0.752975; eerot[5] = 0.658048; eetrans[1] = -0.276022; eerot[6] = -0.999996; eerot[7] = -0.002705; eerot[8] = -0.001047; eetrans[2] = 0.005685; for (unsigned int i=0; i < num_of_tests; i++) { clock_gettime(CLOCK_REALTIME, &start_time); #if IK_VERSION > 54 // for IKFast 56,61 ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions); #else // for IKFast 54 ik(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, vsolutions); #endif /* #if IK_VERSION > 54 // for IKFast 56,61 unsigned int num_of_solutions = (int)solutions.GetNumSolutions(); #else // for IKFast 54 unsigned int num_of_solutions = (int)vsolutions.size(); #endif printf("Found %d ik solutions:\n", num_of_solutions ); */ clock_gettime(CLOCK_REALTIME, &end_time); elapsed_time = (unsigned int)(end_time.tv_nsec - start_time.tv_nsec); sum_time += elapsed_time; } // endfor unsigned int avg_time = (unsigned int)sum_time / (unsigned int)num_of_tests; printf("avg time: %f ms over %d tests \n", (float)avg_time/1000.0, num_of_tests ); return 1; } else { printf("\n" "Usage: \n\n"); printf(" ./compute fk j0 j1 ... j%d \n\n" " Returns the forward kinematic solution given the joint angles (in radians). \n\n", num_of_joints-1 ); printf("\n" " ./compute ik t0 t1 t2 qw qi qj qk free0 ... \n\n" " Returns the ik solutions given the transformation of the end effector specified by \n" " a 3x1 translation (tX), and a 1x4 quaternion (w + i + j + k). \n" " There are %d free parameters that have to be specified. \n\n", num_free_parameters ); printf(" \n" " ./compute ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\n\n" " Returns the ik solutions given the transformation of the end effector specified by \n" " a 3x3 rotation R (rXX), and a 3x1 translation (tX). \n" " There are %d free parameters that have to be specified. \n\n", num_free_parameters ); printf("\n" " ./compute iktiming \n\n" " For fixed number of iterations, generates random joint angles, then \n" " calculates fk, calculates ik, measures average time taken. \n\n", num_of_joints-1 ); printf("\n" " ./compute iktiming2 \n\n" " For fixed number of iterations, with one set of joint variables, this \n" " finds the ik solutions and measures the average time taken. \n\n", num_of_joints-1 ); return 1; } return 0; }