16 std::vector<std::string>
explode(
const std::string & in,
const std::string & delim) {
17 typedef std::string::size_type size_type;
19 const size_type delim_len = delim.length();
20 std::vector<std::string> result;
24 j = in.find(delim, i);
25 result.push_back(in.substr(i, j-i));
26 if (j == std::string::npos)
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";
48 int main(
int argc,
char *argv[]) {
53 std::cout <<
"-------------------------------------\n";
54 std::cout <<
"usage: kinematics [6M180|6M90G|6M90T]\n";
55 std::cout <<
"-------------------------------------\n";
59 std::cout <<
"-----------------------\n";
60 std::cout <<
"KINEMATICS DEMO STARTED\n";
61 std::cout <<
"-----------------------\n";
67 if ((t[0]==
'6')&&(t[1]==
'M')&&(t[2]==
'1')&&(t[3]==
'8')&&(t[4]==
'0')) {
69 }
else if ((t[0]==
'6')&&(t[1]==
'M')&&(t[2]==
'9')&&(t[3]==
'0')&&(t[4]==
'G')) {
71 }
else if ((t[0]==
'6')&&(t[1]==
'M')&&(t[2]==
'9')&&(t[3]==
'0')&&(t[4]==
'T')) {
74 std::cout <<
"-------------------------------------\n";
75 std::cout <<
"unknown type: " << t <<
"\n";
76 std::cout <<
"usage: kinematics [6M180|6M90G|6M90T]\n";
77 std::cout <<
"-------------------------------------\n";
82 std::cout <<
"--------------------------------------------\n";
83 std::cout <<
"success: katana kinematics" << argv[1] <<
" initialized\n";
84 std::cout <<
"--------------------------------------------\n";
91 std::vector<std::string> strvectemp;
94 std::vector<double>
pose;
96 std::vector<int> encoder;
98 std::vector<double>
angle;
100 std::vector<double> start;
119 std::cout <<
"\nInsert encoder values (comma separated, without spaces): \n";
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()));
127 std::cout <<
"> encoder.at(" << i <<
") " << encoder.at(i) <<
"\n";
129 if (!kinematics->
enc2rad(angle, encoder)) {
130 std::cout <<
"\nConverting encoders to angles failed.\n";
133 std::cout <<
"\nAngles:\n";
135 std::cout <<
"> angle.size " << size <<
"\n";
136 for (
int i = 0; i <
size; i++) {
137 std::cout << angle.at(i);
148 std::cout <<
"\nInsert angle values: \n";
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);
170 std::cout <<
"\nCalculating direct kinematics failed.\n";
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";
185 std::cout <<
"\nInsert cartesian parameters: \n";
189 std::cin >> doubletemp;
190 pose.push_back(doubletemp);
192 std::cin >> doubletemp;
193 pose.push_back(doubletemp);
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);
207 for (
int i = 0; i < 6; i++) {
212 std::cout <<
"\nCalculating inverse kinematics failed.\n";
216 std::cout <<
"\nNo solution found.\n";
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";
230 std::cout <<
"\n'" << input <<
"' is not a valid command.\n" << std::endl;
235 std::cout <<
"\nERROR: " << e.
message() <<
"\n";
virtual bool inverseKinematics(std::vector< double > &aAngles, const std::vector< double > aPosition, const std::vector< double > aStartingAngles)=0
virtual bool directKinematics(std::vector< double > &aPosition, const std::vector< double > aAngles)=0
virtual bool enc2rad(std::vector< double > &aAngles, const std::vector< int > aEncoders)=0
Implements the kinematics for the Katana6M180.
std::vector< std::string > explode(const std::string &in, const std::string &delim)
Implements the kinematics for the Katana6M90T.
Base Class for the kinematics implementations.
int main(int argc, char *argv[])
std::string message() const
Implements the kinematics for the Katana6M90G.