kinematics.cpp
Go to the documentation of this file.
00001 
00002 #include "kinematics6M180.h"
00003 #include "kinematics6M90G.h"
00004 #include "kinematics6M90T.h"
00005 
00006 
00007 #include <iostream>
00008 #include <cstdio>
00009 #include <stdlib.h>
00010 #include <memory>
00011 #include <vector>
00012 #include <string>
00013 
00014 #include "keyboard.h"
00015 
00016 std::vector<std::string> explode( const std::string & in, const std::string & delim) {
00017         typedef std::string::size_type size_type;
00018 
00019         const size_type delim_len = delim.length();
00020         std::vector<std::string> result;
00021 
00022         size_type i = 0, j;
00023         for (;;) {
00024                 j = in.find(delim, i);
00025                 result.push_back(in.substr(i, j-i));
00026                 if (j == std::string::npos)
00027                         break;
00028                 i = j + delim_len;
00029         }
00030 
00031         return result;
00032 }
00033 
00034 void DisplayHelp() {
00035         std::cout << "\n";
00036         std::cout << "-------------------------------------\n";
00037         std::cout << "?: Display this help\n";
00038         std::cout << "e: Enc2rad\n";
00039         std::cout << "r: Rad2enc\n";
00040         std::cout << "d: Direct kinematics\n";
00041         std::cout << "i: Inverse kinematics\n";
00042         std::cout << "q: Quit\n";
00043         std::cout << "Esc: Quit\n";
00044         std::cout << "-------------------------------------\n";
00045 }
00046 
00047 
00048 int main(int argc, char *argv[]) {
00049 
00050         using namespace AnaGuess;
00051 
00052         if (argc != 2) {
00053                 std::cout << "-------------------------------------\n";
00054                 std::cout << "usage: kinematics [6M180|6M90G|6M90T]\n";
00055                 std::cout << "-------------------------------------\n";
00056                 return 0;
00057         }
00058 
00059         std::cout << "-----------------------\n";
00060         std::cout << "KINEMATICS DEMO STARTED\n";
00061         std::cout << "-----------------------\n";
00062 
00063         //Kinematics objects
00064         Kinematics* kinematics;
00065         {
00066         char* t = argv[1];
00067         if ((t[0]=='6')&&(t[1]=='M')&&(t[2]=='1')&&(t[3]=='8')&&(t[4]=='0')) {
00068                 kinematics = (Kinematics*) new Kinematics6M180();
00069         } else if ((t[0]=='6')&&(t[1]=='M')&&(t[2]=='9')&&(t[3]=='0')&&(t[4]=='G')) {
00070                 kinematics = (Kinematics*) new Kinematics6M90G();
00071         } else if ((t[0]=='6')&&(t[1]=='M')&&(t[2]=='9')&&(t[3]=='0')&&(t[4]=='T')) {
00072                 kinematics = (Kinematics*) new Kinematics6M90T();
00073         } else {
00074                 std::cout << "-------------------------------------\n";
00075                 std::cout << "unknown type: " << t << "\n";
00076                 std::cout << "usage: kinematics [6M180|6M90G|6M90T]\n";
00077                 std::cout << "-------------------------------------\n";
00078                 return 0;
00079         }
00080         }
00081 
00082         std::cout << "--------------------------------------------\n";
00083         std::cout << "success: katana kinematics" << argv[1] << " initialized\n";
00084         std::cout << "--------------------------------------------\n";
00085 
00086         DisplayHelp();
00087 
00088         bool loop = true;
00089         int input;
00090         std::string strtemp;
00091         std::vector<std::string> strvectemp;
00092         double doubletemp;
00093         int size;
00094         std::vector<double> pose;
00095         pose.reserve(6);
00096         std::vector<int> encoder;
00097         encoder.reserve(6);
00098         std::vector<double> angle;
00099         angle.reserve(6);
00100         std::vector<double> start;
00101         start.reserve(6);
00102 
00103         while (loop) {
00104                 input = _getch();
00105                 try {
00106                         switch (input) {
00107                         case 27: //VK_ESCAPE
00108                         case 'q': //VK_Q
00109                                 loop = false;
00110                                 continue;
00111 
00112                         case '?':
00113                                 DisplayHelp();
00114                                 break;
00115 
00116                         case 'e': //VK_E (enc2rad)
00117                                 encoder.clear();
00118                                 angle.clear();
00119                                 std::cout << "\nInsert encoder values (comma separated, without spaces): \n";
00120                                 std::cin >> strtemp;
00121 std::cout << "> strtemp " << strtemp << "\n";
00122                                 strvectemp = explode(strtemp,",");
00123                                 size = strvectemp.size();
00124 std::cout << "> strvectemp.size " << size << "\n";
00125                                 for (int i = 0; i < size; i++) {
00126                                         encoder.push_back(atoi(strvectemp.at(i).c_str())); // double: atof()
00127 std::cout << "> encoder.at(" << i << ") " << encoder.at(i) << "\n";
00128                                 }
00129                                 if (!kinematics->enc2rad(angle, encoder)) {
00130                                         std::cout << "\nConverting encoders to angles failed.\n";
00131                                         break;
00132                                 }
00133                                 std::cout << "\nAngles:\n";
00134                                 size = angle.size();
00135 std::cout << "> angle.size " << size << "\n"; // !!! is 0
00136                                 for (int i = 0; i < size; i++) {
00137                                         std::cout << angle.at(i);
00138                                         if (i != size - 1)
00139                                                 std::cout << ",";
00140                                 }
00141                                 std::cout << "\n";
00142                                 break;
00143 
00144                         case 'r': //VK_R (rad2enc)
00145                                 break;
00146 
00147                         case 'd': //VK_D (direct kinematics)
00148                                 std::cout << "\nInsert angle values: \n";
00149                                 angle.clear();
00150                                 pose.clear();
00151                                 std::cout << "Axis 1: ";
00152                                 std::cin >> doubletemp;
00153                                 angle.push_back(doubletemp);
00154                                 std::cout << "Axis 2: ";
00155                                 std::cin >> doubletemp;
00156                                 angle.push_back(doubletemp);
00157                                 std::cout << "Axis 3: ";
00158                                 std::cin >> doubletemp;
00159                                 angle.push_back(doubletemp);
00160                                 std::cout << "Axis 4: ";
00161                                 std::cin >> doubletemp;
00162                                 angle.push_back(doubletemp);
00163                                 std::cout << "Axis 5: ";
00164                                 std::cin >> doubletemp;
00165                                 angle.push_back(doubletemp);
00166                                 std::cout << "Axis 6: ";
00167                                 std::cin >> doubletemp;
00168                                 angle.push_back(doubletemp);
00169                                 if (!kinematics->directKinematics(pose, angle)) {
00170                                         std::cout << "\nCalculating direct kinematics failed.\n";
00171                                         break;
00172                                 }
00173                                 std::cout.precision(6);
00174                                 std::cout << "\n------------------------------------\n";
00175                                 std::cout << "X:     " << pose[0] << "\n";
00176                                 std::cout << "Y:     " << pose[1] << "\n";
00177                                 std::cout << "Z:     " << pose[2] << "\n";
00178                                 std::cout << "phi:   " << pose[3] << "\n";
00179                                 std::cout << "theta: " << pose[4] << "\n";
00180                                 std::cout << "psi:   " << pose[5] << "\n";
00181                                 std::cout << "------------------------------------\n";
00182                                 break;
00183 
00184                         case 'i':  //VK_I (inverse kinematics)
00185                                 std::cout << "\nInsert cartesian parameters: \n";
00186                                 angle.clear();
00187                                 pose.clear();
00188                                 std::cout << "X: ";
00189                                 std::cin >> doubletemp;
00190                                 pose.push_back(doubletemp);
00191                                 std::cout << "Y: ";
00192                                 std::cin >> doubletemp;
00193                                 pose.push_back(doubletemp);
00194                                 std::cout << "Z: ";
00195                                 std::cin >> doubletemp;
00196                                 pose.push_back(doubletemp);
00197                                 std::cout << "phi: ";
00198                                 std::cin >> doubletemp;
00199                                 pose.push_back(doubletemp);
00200                                 std::cout << "theta: ";
00201                                 std::cin >> doubletemp;
00202                                 pose.push_back(doubletemp);
00203                                 std::cout << "psi: ";
00204                                 std::cin >> doubletemp;
00205                                 pose.push_back(doubletemp);
00206                                 angle.reserve(6);
00207                                 for (int i = 0; i < 6; i++) {
00208                                         start[i] = 1.5;
00209                                 }
00210                                 try {
00211                                         if (!kinematics->inverseKinematics(angle, pose, start)) {
00212                                                 std::cout << "\nCalculating inverse kinematics failed.\n";
00213                                                 break;
00214                                         }
00215                                 } catch (NoSolutionException &nse) {
00216                                         std::cout << "\nNo solution found.\n";
00217                                         break;
00218                                 }
00219                                 std::cout << "\n------------------------------------\n";
00220                                 std::cout << "Axis 1: " << angle[0] << "\n";
00221                                 std::cout << "Axis 2: " << angle[1] << "\n";
00222                                 std::cout << "Axis 3: " << angle[2] << "\n";
00223                                 std::cout << "Axis 4: " << angle[3] << "\n";
00224                                 std::cout << "Axis 5: " << angle[4] << "\n";
00225                                 std::cout << "Axis 6: " << angle[5] << "\n";
00226                                 std::cout << "------------------------------------\n";
00227                                 break;
00228 
00229                         default: //Error message
00230                                 std::cout << "\n'" << input << "' is not a valid command.\n" << std::endl;
00231                                 break;
00232 
00233                         }
00234                 } catch (Exception &e) {
00235                         std::cout << "\nERROR: " << e.message() << "\n";
00236                 }
00237         }
00238         delete kinematics;
00239         return 0;
00240 }
00241 


kni
Author(s): Neuronics AG (see AUTHORS.txt); ROS wrapper by Martin Günther
autogenerated on Mon Oct 6 2014 10:45:32