kni_test.cpp
Go to the documentation of this file.
1 /**********************************************************************************
2  * Katana Native Interface - A C++ interface to the robot arm Katana.
3  * Copyright (C) 2005-2009 Neuronics AG
4  * Check out the AUTHORS file for detailed contact information.
5  * This program is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12  * GNU General Public License for more details.
13  * You should have received a copy of the GNU General Public License
14  * along with this program; if not, write to the Free Software
15  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
16  **********************************************************************************/
17 
19 // test.cpp
20 // test program for KNI libraries
22 
23 #include "kniBase.h"
24 #include <iostream>
25 #include <cstdio>
26 #include <memory>
27 #include <vector>
28 #include <fstream>
30 #define LEFT false
31 #define RIGHT true
32 
34 const double PI = 3.14159265358979323846;
35 
36 struct TPoint {
37  double X, Y, Z;
38  double phi, theta, psi;
39 };
40 
41 struct TCurrentMot {
42  int idx;
43  bool running;
44  bool dir;
45 };
46 
47 struct Tpos{
48  static std::vector<int> x,y,z,u,v,w;
49  static const int xArr[], yArr[], zArr[], uArr[], vArr[], wArr[];
50 };
51 
52 //Katana obj.
53 std::unique_ptr<CLMBase> katana;
55 
56 int main(int argc, char *argv[]) {
57 
58  if (argc != 3) {
59  std::cout << "---------------------------------\n";
60  std::cout << "usage for socket connection: kni_test CONFIGFILE IP_ADDR\n";
61  std::cout << "---------------------------------\n";
62  return 0;
63  }
64 
65  std::cout << "-----------------\n";
66  std::cout << "KNI_TEST DEMO STARTED\n";
67  std::cout << "-----------------\n";
68 
69  //----------------------------------------------------------------//
70  //open device:
71  //----------------------------------------------------------------//
72 
73 
74  std::unique_ptr<CCdlSocket> device;
75  std::unique_ptr<CCplSerialCRC> protocol;
76 
77  try {
78 
79  int port = 5566;
80  device.reset(new CCdlSocket(argv[2], port));
81 
82  std::cout << "-------------------------------------------\n";
83  std::cout << "success: port " << port << " open\n";
84  std::cout << "-------------------------------------------\n";
85 
86  //--------------------------------------------------------//
87  //init protocol:
88  //--------------------------------------------------------//
89 
90  protocol.reset(new CCplSerialCRC());
91  std::cout << "-------------------------------------------\n";
92  std::cout << "success: protocol class instantiated\n";
93  std::cout << "-------------------------------------------\n";
94  protocol->init(device.get()); //fails if no response from Katana
95  std::cout << "-------------------------------------------\n";
96  std::cout << "success: communication with Katana initialized\n";
97  std::cout << "-------------------------------------------\n";
98 
99 
100  //--------------------------------------------------------//
101  //init robot:
102  //--------------------------------------------------------//
103  katana.reset(new CLMBase());
104  katana->create(argv[1], protocol.get());
105 
106 
107  } catch(Exception &e) {
108  std::cout << "ERROR: " << e.message() << std::endl;
109  return -1;
110  }
111  std::cout << "-------------------------------------------\n";
112  std::cout << "success: katana initialized\n";
113  std::cout << "-------------------------------------------\n";
114 
115  //set linear velocity to 60
116  katana->setMaximumLinearVelocity(60);
117 
118  // declare information variables
119  int nOfMot = katana->getNumberOfMotors(); // number of motors
120  int controller; // controller type: 0 for position, 1 for current
121  const char* model; // katana model name
122  std::vector<TPoint> points(0); // list of points used for motion
123 
124  // declare temporary variables
125  std::vector<int> encoders(nOfMot, 0);
126  std::vector<double> pose(6, 0.0);
127  TCurrentMot mot[6];
128  for (int i = 0; i< 6; i++){
129  mot[i].running = false;
130  mot[i].idx = i;
131  mot[i].dir = RIGHT;
132  }
133  int tmp_int;
134 
135  // calibration
136  std::cout << "- Calibrating Katana, please wait for termination..." << std::endl;
137  katana->calibrate();
138  std::cout << " ...done." << std::endl;
139 
140  // get controller information
141  std::cout << "- Check if Katana has position or current controllers..." << std::endl;
142  controller = katana->getCurrentControllerType(1);
143  for(short motorNumber = 1; motorNumber < nOfMot; ++motorNumber) {
144  tmp_int = katana->getCurrentControllerType(motorNumber+1);
145  if (tmp_int != controller) {
146  std::cout << "*** ERROR: Katana has mixed controller types on its axes! ***" << std::endl;
147  return 1;
148  }
149  }
150  std::cout << " Katana has all ";
151  controller == 0 ? std::cout << "position" : std::cout << "current";
152  std::cout << " controllers." << std::endl;
153  std::cout << " ...done." << std::endl;
154 
155  // read current force if current controller installed
156  if (controller == 1) {
157  std::cout << "- Read forces..." << std::endl;
158  for (short motorNumber = 0; motorNumber < nOfMot; ++motorNumber) {
159  std::cout << " Motor " << (motorNumber+1) << ": " << katana->getForce(motorNumber+1) << std::endl;
160  }
161  std::cout << " ...done." << std::endl;
162  }
163 
164  // get Katana model name
165  std::cout << "- Get Katana model name..." << std ::endl;
166  model = katana->GetBase()->GetGNL()->modelName;
167  std::cout << " " << model << std::endl;
168  std::cout << " ...done." << std::endl;
169 
170  // switch motors off
171  std::cout << "- Switch robot off..." << std ::endl;
172  katana->switchRobotOff();
173  std::cout << " Robot off" << std::endl;
174  std::cout << " ...done." << std::endl;
175 
176  // get robot encoders
177  std::cout << "- Read robot encoders..." << std ::endl;
178  std::cout << " Encoder values:";
179  katana->getRobotEncoders(encoders.begin(), encoders.end());
180  for (std::vector<int>::iterator i= encoders.begin(); i != encoders.end(); ++i) {
181  std::cout << " " << *i;
182  }
183  std::cout << std::endl;
184  std::cout << " ...done." << std::endl;
185 
186  // switch motors on
187  std::cout << "- Switch motors on..." << std ::endl;
188  for (short motorNumber = 0; motorNumber < nOfMot; ++motorNumber) {
189  katana->switchMotorOn((short)motorNumber);
190  std::cout << " Motor " << (motorNumber+1) << " on" << std::endl;
191  }
192  std::cout << " ...done." << std::endl;
193 
194  // move single axes (inc, dec, mov, incDegrees, decDegrees, movDegrees, moveMotorByEnc, moveMotorBy, moveMotorToEnc, moveMotorTo)
195  std::cout << "- Move single axes..." << std ::endl;
196  katana->dec(0, 10000, true);
197  std::cout << " Motor 1 decreased by 10000 encoders" << std::endl;
198  katana->inc(1, 10000, true);
199  std::cout << " Motor 2 increased by 10000 encoders" << std::endl;
200  katana->decDegrees(2, 70, true);
201  std::cout << " Motor 3 decreased by 70 degrees" << std::endl;
202  katana->mov(3, 20000, true);
203  std::cout << " " << "Motor 4 moved to encoder position 20000" << std::endl;
204  katana->movDegrees(4, 90, true);
205  std::cout << " Motor 5 moved to position 90 degrees" << std::endl;
206  katana->incDegrees(5, -35, true);
207  std::cout << " Motor 6 increased by -35 degrees" << std::endl;
208  katana->moveMotorBy(0, 0.2, true);
209  std::cout << " Motor 1 moved by 0.2 rad" << std::endl;
210  katana->moveMotorByEnc(1, -5000, true);
211  std::cout << " Motor 2 moved by -5000 encoders" << std::endl;
212  katana->moveMotorTo(2, 3.1, true);
213  std::cout << " Motor 3 moved to 3.1 rad" << std::endl;
214  katana->moveMotorToEnc(3, 10000, true);
215  std::cout << " Motor 4 moved to 10000 encoders" << std::endl;
216  std::cout << " ...done." << std::endl;
217 
218  // move all axes (moveRobotToEnc)
219  std::cout << "- Move all axes..." << std ::endl;
220  for (short motorNumber = 0; motorNumber < nOfMot; ++motorNumber) {
221  encoders[motorNumber] = 30500;
222  if (motorNumber == 1 || motorNumber == 2)
223  encoders[motorNumber] = -30500;
224  }
225  katana->moveRobotToEnc(encoders, true);
226  std::cout << " Robot moved to encoder position 30500 -30500 -30500 30500 30500 30500" << std::endl;
227  std::cout << " ...done." << std::endl;
228 
229  // get coordinates
230  std::cout << "- Get coordinates..." << std ::endl;
231  {
232  double x, y, z, phi, theta, psi;
233  katana->getCoordinates(x, y, z, phi, theta, psi);
234  std::cout << " Current coordinates: " << x << " " << y << " " << z << " " << phi << " " << theta << " " << psi << std::endl;
235  }
236  std::cout << " ...done." << std::endl;
237 
238  // get coordinates from given encoders
239  std::cout << "- Get coordinates from given encoders..." << std ::endl;
240  {
241  encoders[0] = encoders[3] = encoders[4] = encoders[5] = 25000;
242  encoders[1] = encoders[2] = -25000;
243  katana->getCoordinatesFromEncoders(pose, encoders);
244  std::cout << " Encoders: 25000 -25000 -25000 25000 25000 25000" << std::endl;
245  std::cout << " Coordinates: " << pose[0] << " " << pose[1] << " " << pose[2] << " " << pose[3] << " " << pose[4] << " " << pose[5] << std::endl;
246  }
247  std::cout << " ...done." << std::endl;
248 
249  // calculate inverse kinematics
250  std::cout << "- Calculate inverse kinematics..." << std ::endl;
251  {
252  katana->IKCalculate(pose[0], pose[1], pose[2], pose[3], pose[4], pose[5], encoders.begin());
253  std::cout << " encoders.size(): " << encoders.size() << std::endl;
254  std::cout << " Possible encoders: " << encoders[0] << " " << encoders[1] << " " << encoders[2] << " " << encoders[3] << " " << encoders[4] << " " << encoders[5] << std::endl;
255  }
256  std::cout << " ...done." << std::endl;
257 
258  // move robot to pose
259  std::cout << "- Move robot to pose..." << std ::endl;
260  katana->moveRobotTo(pose, true);
261  std::cout << " Coordinates: " << pose[0] << " " << pose[1] << " " << pose[2] << " " << pose[3] << " " << pose[4] << " " << pose[5] << std::endl;
262  std::cout << " ...done." << std::endl;
263 
264  // load points file
265  {
266  using namespace std;
267  cout << "- Load points file..." << endl;
268  ifstream listfile(model);
269  if(!listfile) {
270  cout << "*** ERROR: File '" << model << "' not found or access denied! ***" << endl;
271  return 1;
272  }
273  string line;
274  vector<string> tokens;
275  const string delimiter = ",";
276  int lines = 0;
277  while(!listfile.eof()) {
278  listfile >> line;
279  string::size_type lastPos = line.find_first_not_of(delimiter, 0);
280  string::size_type pos = line.find_first_of(delimiter, lastPos);
281  while (string::npos != pos || string::npos != lastPos) {
282  // Found a token, add it to the vector.
283  tokens.push_back(line.substr(lastPos, pos - lastPos));
284  // Skip delimiters. Note the "not_of"
285  lastPos = line.find_first_not_of(delimiter, pos);
286  // Find next "non-delimiter"
287  pos = line.find_first_of(delimiter, lastPos);
288  }
289  TPoint point;
290  point.X = atof((tokens.at(0)).data());
291  point.Y = atof((tokens.at(1)).data());
292  point.Z = atof((tokens.at(2)).data());
293  point.phi = atof((tokens.at(3)).data());
294  point.theta = atof((tokens.at(4)).data());
295  point.psi = atof((tokens.at(5)).data());
296  points.push_back( point );
297  ++lines;
298  tokens.clear();
299  }
300  cout << " " << lines << " points loaded." << endl;
301  cout << " ...done." << endl;
302  }
303 
304  // move to point in list
305  std::cout << "- Move in point list..." << std ::endl;
306  {
307  int i, j;
308  bool wait;
309  std::cout << " Single p2p movements..." << std::flush;
310  for (i = 0; i < (int)points.size(); ++i) {
311  katana->moveRobotTo(points[i].X, points[i].Y, points[i].Z, points[i].phi, points[i].theta, points[i].psi);
312  }
313  std::cout << " ...done." << std::endl;
314  std::cout << " Single linear movements..." << std::flush;
315  for (i = 0; i < (int)points.size(); ++i) {
316  katana->moveRobotLinearTo(points[i].X, points[i].Y, points[i].Z, points[i].phi, points[i].theta, points[i].psi);
317  }
318  std::cout << " ...done." << std::endl;
319  std::cout << " Concatenated p2p movements..." << std::flush;
320  for (i = 0; i < (int)points.size(); ++i) {
321  j = (i + points.size() - 1) % points.size();
322  wait = false;
323  if (i == ((int)points.size()-1)) wait = true;
324  katana->movP2P(points[j].X, points[j].Y, points[j].Z, points[j].phi, points[j].theta, points[j].psi, points[i].X, points[i].Y, points[i].Z, points[i].phi, points[i].theta, points[i].psi, true, 70.0, wait);
325  }
326  std::cout << " ...done." << std::endl;
327  std::cout << " Concatenated linear movements..." << std::flush;
328  for (i = 0; i < (int)points.size(); ++i) {
329  j = (i + points.size() - 1) % points.size();
330  wait = false;
331  if (i == ((int)points.size()-1)) wait = true;
332  katana->movLM2P(points[j].X, points[j].Y, points[j].Z, points[j].phi, points[j].theta, points[j].psi, points[i].X, points[i].Y, points[i].Z, points[i].phi, points[i].theta, points[i].psi, true, 100.0, wait);
333  }
334  std::cout << " ...done." << std::endl;
335  }
336  std::cout << " ...done." << std::endl;
337 
338  // move all axes (moveRobotToEnc)
339  std::cout << "- Move all axes..." << std ::endl;
340  for (short motorNumber = 0; motorNumber < nOfMot; ++motorNumber) {
341  encoders[motorNumber] = 30500;
342  if (motorNumber == 1 || motorNumber == 2)
343  encoders[motorNumber] = -30500;
344  }
345  katana->moveRobotToEnc(encoders, true);
346  std::cout << " Robot moved to encoder position 30500 -30500 -30500 30500 30500 30500" << std::endl;
347  std::cout << " ...done." << std::endl;
348 
349  return 0;
350 }
352 
static std::unique_ptr< CCdlSocket > device
Definition: kni_wrapper.cpp:23
double Z
Definition: control.cpp:41
static std::unique_ptr< CCplSerialCRC > protocol
Definition: kni_wrapper.cpp:24
std::string message() const
double theta
Definition: control.cpp:42
Implement the Serial-Zero protocol.
Definition: cplSerial.h:137
#define RIGHT
Definition: kni_test.cpp:31
int main(int argc, char *argv[])
Definition: kni_test.cpp:56
double phi
Definition: control.cpp:42
FloatVector * pose
Definition: control.cpp:51
const double PI
Definition: kni_test.cpp:34
double Y
Definition: control.cpp:41
std::unique_ptr< CLMBase > katana
Definition: kni_test.cpp:53
Encapsulates the socket communication device.
Definition: cdlSocket.h:65
structure for the currently active axis
Definition: control.cpp:45
double psi
Definition: control.cpp:42
Linear movement Class.
Definition: lmBase.h:73
std::vector< TPoint > points(0)
bool running
Definition: control.cpp:47
bool dir
Definition: control.cpp:48
double X
Definition: control.cpp:41
std::vector< int > encoders
Definition: kni_wrapper.cpp:29


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