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
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:
00108 case 'q':
00109 loop = false;
00110 continue;
00111
00112 case '?':
00113 DisplayHelp();
00114 break;
00115
00116 case 'e':
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()));
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";
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':
00145 break;
00146
00147 case 'd':
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':
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:
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