AnalyticalGuess/test/kinematics.cpp
Go to the documentation of this file.
1 
2 #include "kinematics6M180.h"
3 #include "kinematics6M90G.h"
4 #include "kinematics6M90T.h"
5 
6 
7 #include <iostream>
8 #include <cstdio>
9 #include <stdlib.h>
10 #include <memory>
11 #include <vector>
12 #include <string>
13 
14 #include "keyboard.h"
15 
16 std::vector<std::string> explode( const std::string & in, const std::string & delim) {
17  typedef std::string::size_type size_type;
18 
19  const size_type delim_len = delim.length();
20  std::vector<std::string> result;
21 
22  size_type i = 0, j;
23  for (;;) {
24  j = in.find(delim, i);
25  result.push_back(in.substr(i, j-i));
26  if (j == std::string::npos)
27  break;
28  i = j + delim_len;
29  }
30 
31  return result;
32 }
33 
34 void DisplayHelp() {
35  std::cout << "\n";
36  std::cout << "-------------------------------------\n";
37  std::cout << "?: Display this help\n";
38  std::cout << "e: Enc2rad\n";
39  std::cout << "r: Rad2enc\n";
40  std::cout << "d: Direct kinematics\n";
41  std::cout << "i: Inverse kinematics\n";
42  std::cout << "q: Quit\n";
43  std::cout << "Esc: Quit\n";
44  std::cout << "-------------------------------------\n";
45 }
46 
47 
48 int main(int argc, char *argv[]) {
49 
50  using namespace AnaGuess;
51 
52  if (argc != 2) {
53  std::cout << "-------------------------------------\n";
54  std::cout << "usage: kinematics [6M180|6M90G|6M90T]\n";
55  std::cout << "-------------------------------------\n";
56  return 0;
57  }
58 
59  std::cout << "-----------------------\n";
60  std::cout << "KINEMATICS DEMO STARTED\n";
61  std::cout << "-----------------------\n";
62 
63  //Kinematics objects
64  Kinematics* kinematics;
65  {
66  char* t = argv[1];
67  if ((t[0]=='6')&&(t[1]=='M')&&(t[2]=='1')&&(t[3]=='8')&&(t[4]=='0')) {
68  kinematics = (Kinematics*) new Kinematics6M180();
69  } else if ((t[0]=='6')&&(t[1]=='M')&&(t[2]=='9')&&(t[3]=='0')&&(t[4]=='G')) {
70  kinematics = (Kinematics*) new Kinematics6M90G();
71  } else if ((t[0]=='6')&&(t[1]=='M')&&(t[2]=='9')&&(t[3]=='0')&&(t[4]=='T')) {
72  kinematics = (Kinematics*) new Kinematics6M90T();
73  } else {
74  std::cout << "-------------------------------------\n";
75  std::cout << "unknown type: " << t << "\n";
76  std::cout << "usage: kinematics [6M180|6M90G|6M90T]\n";
77  std::cout << "-------------------------------------\n";
78  return 0;
79  }
80  }
81 
82  std::cout << "--------------------------------------------\n";
83  std::cout << "success: katana kinematics" << argv[1] << " initialized\n";
84  std::cout << "--------------------------------------------\n";
85 
86  DisplayHelp();
87 
88  bool loop = true;
89  int input;
90  std::string strtemp;
91  std::vector<std::string> strvectemp;
92  double doubletemp;
93  int size;
94  std::vector<double> pose;
95  pose.reserve(6);
96  std::vector<int> encoder;
97  encoder.reserve(6);
98  std::vector<double> angle;
99  angle.reserve(6);
100  std::vector<double> start;
101  start.reserve(6);
102 
103  while (loop) {
104  input = _getch();
105  try {
106  switch (input) {
107  case 27: //VK_ESCAPE
108  case 'q': //VK_Q
109  loop = false;
110  continue;
111 
112  case '?':
113  DisplayHelp();
114  break;
115 
116  case 'e': //VK_E (enc2rad)
117  encoder.clear();
118  angle.clear();
119  std::cout << "\nInsert encoder values (comma separated, without spaces): \n";
120  std::cin >> strtemp;
121 std::cout << "> strtemp " << strtemp << "\n";
122  strvectemp = explode(strtemp,",");
123  size = strvectemp.size();
124 std::cout << "> strvectemp.size " << size << "\n";
125  for (int i = 0; i < size; i++) {
126  encoder.push_back(atoi(strvectemp.at(i).c_str())); // double: atof()
127 std::cout << "> encoder.at(" << i << ") " << encoder.at(i) << "\n";
128  }
129  if (!kinematics->enc2rad(angle, encoder)) {
130  std::cout << "\nConverting encoders to angles failed.\n";
131  break;
132  }
133  std::cout << "\nAngles:\n";
134  size = angle.size();
135 std::cout << "> angle.size " << size << "\n"; // !!! is 0
136  for (int i = 0; i < size; i++) {
137  std::cout << angle.at(i);
138  if (i != size - 1)
139  std::cout << ",";
140  }
141  std::cout << "\n";
142  break;
143 
144  case 'r': //VK_R (rad2enc)
145  break;
146 
147  case 'd': //VK_D (direct kinematics)
148  std::cout << "\nInsert angle values: \n";
149  angle.clear();
150  pose.clear();
151  std::cout << "Axis 1: ";
152  std::cin >> doubletemp;
153  angle.push_back(doubletemp);
154  std::cout << "Axis 2: ";
155  std::cin >> doubletemp;
156  angle.push_back(doubletemp);
157  std::cout << "Axis 3: ";
158  std::cin >> doubletemp;
159  angle.push_back(doubletemp);
160  std::cout << "Axis 4: ";
161  std::cin >> doubletemp;
162  angle.push_back(doubletemp);
163  std::cout << "Axis 5: ";
164  std::cin >> doubletemp;
165  angle.push_back(doubletemp);
166  std::cout << "Axis 6: ";
167  std::cin >> doubletemp;
168  angle.push_back(doubletemp);
169  if (!kinematics->directKinematics(pose, angle)) {
170  std::cout << "\nCalculating direct kinematics failed.\n";
171  break;
172  }
173  std::cout.precision(6);
174  std::cout << "\n------------------------------------\n";
175  std::cout << "X: " << pose[0] << "\n";
176  std::cout << "Y: " << pose[1] << "\n";
177  std::cout << "Z: " << pose[2] << "\n";
178  std::cout << "phi: " << pose[3] << "\n";
179  std::cout << "theta: " << pose[4] << "\n";
180  std::cout << "psi: " << pose[5] << "\n";
181  std::cout << "------------------------------------\n";
182  break;
183 
184  case 'i': //VK_I (inverse kinematics)
185  std::cout << "\nInsert cartesian parameters: \n";
186  angle.clear();
187  pose.clear();
188  std::cout << "X: ";
189  std::cin >> doubletemp;
190  pose.push_back(doubletemp);
191  std::cout << "Y: ";
192  std::cin >> doubletemp;
193  pose.push_back(doubletemp);
194  std::cout << "Z: ";
195  std::cin >> doubletemp;
196  pose.push_back(doubletemp);
197  std::cout << "phi: ";
198  std::cin >> doubletemp;
199  pose.push_back(doubletemp);
200  std::cout << "theta: ";
201  std::cin >> doubletemp;
202  pose.push_back(doubletemp);
203  std::cout << "psi: ";
204  std::cin >> doubletemp;
205  pose.push_back(doubletemp);
206  angle.reserve(6);
207  for (int i = 0; i < 6; i++) {
208  start[i] = 1.5;
209  }
210  try {
211  if (!kinematics->inverseKinematics(angle, pose, start)) {
212  std::cout << "\nCalculating inverse kinematics failed.\n";
213  break;
214  }
215  } catch (NoSolutionException &nse) {
216  std::cout << "\nNo solution found.\n";
217  break;
218  }
219  std::cout << "\n------------------------------------\n";
220  std::cout << "Axis 1: " << angle[0] << "\n";
221  std::cout << "Axis 2: " << angle[1] << "\n";
222  std::cout << "Axis 3: " << angle[2] << "\n";
223  std::cout << "Axis 4: " << angle[3] << "\n";
224  std::cout << "Axis 5: " << angle[4] << "\n";
225  std::cout << "Axis 6: " << angle[5] << "\n";
226  std::cout << "------------------------------------\n";
227  break;
228 
229  default: //Error message
230  std::cout << "\n'" << input << "' is not a valid command.\n" << std::endl;
231  break;
232 
233  }
234  } catch (Exception &e) {
235  std::cout << "\nERROR: " << e.message() << "\n";
236  }
237  }
238  delete kinematics;
239  return 0;
240 }
241 
AnaGuess::Kinematics::inverseKinematics
virtual bool inverseKinematics(std::vector< double > &aAngles, const std::vector< double > aPosition, const std::vector< double > aStartingAngles)=0
AnaGuess::Kinematics::directKinematics
virtual bool directKinematics(std::vector< double > &aPosition, const std::vector< double > aAngles)=0
AnaGuess::Kinematics::enc2rad
virtual bool enc2rad(std::vector< double > &aAngles, const std::vector< int > aEncoders)=0
AnaGuess::Kinematics6M180
Implements the kinematics for the Katana6M180.
Definition: kinematics6M180.h:30
pose
FloatVector * pose
Definition: include/libKinematics.h:343
explode
std::vector< std::string > explode(const std::string &in, const std::string &delim)
Definition: AnalyticalGuess/test/kinematics.cpp:16
AnaGuess::Kinematics6M90T
Implements the kinematics for the Katana6M90T.
Definition: kinematics6M90T.h:30
AnaGuess::Kinematics
Base Class for the kinematics implementations.
Definition: AnalyticalGuess/include/kinematics.h:42
_getch
int _getch()
Definition: demo/common/keyboard.cpp:58
main
int main(int argc, char *argv[])
Definition: AnalyticalGuess/test/kinematics.cpp:48
angle
FloatVector * angle
Definition: include/libKinematics.h:323
AnaGuess::Kinematics6M90G
Implements the kinematics for the Katana6M90G.
Definition: kinematics6M90G.h:30
size
byte size
Definition: kni_wrapper.cpp:35

kni
Author(s): Martin Günther
autogenerated on Fri Jan 3 2020 04:01:16