Fawkes API  Fawkes Development Version
controller_kni.cpp
1 
2 /***************************************************************************
3  * controller_kni.cpp - KNI Controller class for katana arm
4  *
5  * Created: Tue Jan 03 11:40:31 2012
6  * Copyright 2012 Bahram Maleki-Fard, AllemaniACs RoboCup Team
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "controller_kni.h"
24 #include "exception.h"
25 
26 #include <core/exceptions/software.h>
27 
28 #include <kniBase.h>
29 #include <common/MathHelperFunctions.h>
30 
31 namespace fawkes {
32 #if 0 /* just to make Emacs auto-indent happy */
33 }
34 #endif
35 
36 /** @class KatanaControllerKni <plugins/katana/controller_kni.h>
37  * Controller class for a Neuronics Katana, using libkni to interact
38  * with the real Katana arm.
39  * @author Bahram Maleki-Fard
40  */
41 
42 /** Constructor. */
44 {
45  // setting default setup values
46  __cfg_device = "/dev/ttyS0";
47  __cfg_kni_conffile = "/etc/kni3/hd300/katana6M180.cfg";
48  __cfg_read_timeout = 100;
49  __cfg_write_timeout = 0;
50 
51  __gripper_last_pos.clear();
52  __gripper_last_pos.resize(2);
53 }
54 
55 /** Destructor. */
57 {
58  // Setting to NULL also deletes instance (RefPtr)
59  __katana = NULL;
60 
61  __device.reset();
62  __protocol.reset();
63 }
64 
65 /** Setup parameters needed to initialize Katana arm with libkni.
66  * @param device device string, e.g. "/dev/ttyS0"
67  * @param kni_conffile path to kni configfile, e.g. "/etc/kni3/hd300/katana6M180.cfg"
68  * @param read_timeout timeout for read operations, in ms
69  * @param write_timeout timeout for write operations, in ms
70  */
71 void
72 KatanaControllerKni::setup(std::string& device, std::string& kni_conffile,
73  unsigned int read_timeout, unsigned int write_timeout)
74 {
75  __cfg_device = device;
76  __cfg_kni_conffile = kni_conffile;
77  __cfg_read_timeout = read_timeout;
78  __cfg_write_timeout = write_timeout;
79 }
80 
81 void
83 {
84  try {
85  TCdlCOMDesc ccd = {0, 57600, 8, 'N', 1, (int)__cfg_read_timeout, (int)__cfg_write_timeout};
86  __device.reset(new CCdlCOM(ccd, __cfg_device.c_str()));
87 
88  __protocol.reset(new CCplSerialCRC());
89  __protocol->init(__device.get());
90 
91  __katana = RefPtr<CLMBase>(new CLMBase());
92  __katana->create(__cfg_kni_conffile.c_str(), __protocol.get());
93  __katbase = __katana->GetBase();
94  __sensor_ctrl = &__katbase->GetSCT()->arr[0];
95 
96  __katbase->recvECH();
97  } catch (/*KNI*/::Exception &e) {
98  throw fawkes::Exception("KNI Exception:%s", e.what());
99  }
100 
101  try {
102  __motor_init.resize(__katana->getNumberOfMotors());
103  for(unsigned int i=0; i<__motor_init.size(); i++) {
104  __motor_init.at(i) = *(__katbase->GetMOT()->arr[i].GetInitialParameters());
105  }
106  } catch (/*KNI*/::Exception &e) {
107  throw fawkes::Exception("KNI Exception:%s", e.what());
108  }
109 }
110 
111 void
113 {
114  try {
115  __katana->setRobotVelocityLimit((int)vel);
116  } catch (/*KNI*/::Exception &e) {
117  throw fawkes::Exception("KNI Exception:%s", e.what());
118  }
119 }
120 
121 
122 bool
124 {
125  if( __active_motors.size() < 1 )
126  return true;
127 
128  bool final = true;
129  for(unsigned int i=0; i<__active_motors.size(); i++) {
130  final &= motor_final(__active_motors.at(i));
131  }
132  cleanup_active_motors();
133  return final;
134 }
135 
136 bool
138 {
139  return true;
140 }
141 bool
143 {
144  return true;
145 }
146 
147 void
149 {
150  try {
151  __katana->calibrate();
152  } catch (/*KNI*/::Exception &e) {
153  throw fawkes::Exception("KNI Exception:%s", e.what());
154  }
155 }
156 
157 void
159 {
160  try {
161  __katana->freezeRobot();
162  } catch (/*KNI*/::Exception &e) {
163  throw fawkes::Exception("KNI Exception:%s", e.what());
164  }
165 }
166 
167 void
169 {
170  try {
171  __katana->switchRobotOn();
172  } catch (/*KNI*/::Exception &e) {
173  throw fawkes::Exception("KNI Exception:%s", e.what());
174  }
175 }
176 
177 void
179 {
180  try {
181  __katana->switchRobotOff();
182  } catch (/*KNI*/::Exception &e) {
183  throw fawkes::Exception("KNI Exception:%s", e.what());
184  }
185 }
186 
187 void
189 {
190  try {
191  __katana->getCoordinates(__x, __y, __z, __phi, __theta, __psi, refresh);
192  } catch (/*KNI*/::Exception &e) {
193  throw fawkes::Exception("KNI Exception:%s", e.what());
194  }
195 }
196 
197 void
199 {
200  try {
201  if( __active_motors.size() == (unsigned short)__katana->getNumberOfMotors() ) {
202  __katbase->recvMPS(); // get position for all motors
203  __katbase->recvGMS(); // get status flags for all motors
204  } else {
205  const TKatMOT* mot = __katbase->GetMOT();
206  for(unsigned int i=0; i<__active_motors.size(); i++) {
207  mot->arr[__active_motors.at(i)].recvPVP(); // get position data for motor
208  }
209  }
210  } catch (/*KNI*/::Exception &e) {
211  throw fawkes::Exception("KNI Exception:%s", e.what());
212  }
213 }
214 
215 void
217 {
218  try {
219  __sensor_ctrl->recvDAT();
220  } catch (/*KNI*/::ParameterReadingException &e) {
221  throw fawkes::Exception("KNI ParameterReadingException:%s", e.what());
222  } catch (/*KNI*/::Exception &e) {
223  throw fawkes::Exception("KNI Exception:%s", e.what());
224  }
225 }
226 
227 void
229 {
230  try {
231  __katana->openGripper(blocking);
232  } catch (/*KNI*/::Exception &e) {
233  throw fawkes::Exception("KNI Exception:%s", e.what());
234  }
235 
236  __active_motors.clear();
237  __active_motors.resize(1);
238  __active_motors[0] = __katbase->GetMOT()->cnt - 1;
239 
240  __gripper_last_pos.clear();
241  __gripper_last_pos[0] = __katbase->GetMOT()->arr[__active_motors[0]].GetPVP()->pos;
242  __gripper_last_pos[1] = 0; //counter
243 }
244 
245 void
247 {
248  try {
249  __katana->closeGripper(blocking);
250  } catch (/*KNI*/::Exception &e) {
251  throw fawkes::Exception("KNI Exception:%s", e.what());
252  }
253 
254  __active_motors.clear();
255  __active_motors.resize(1);
256  __active_motors[0] = __katbase->GetMOT()->cnt - 1;
257 
258  __gripper_last_pos.clear();
259  __gripper_last_pos[0] = __katbase->GetMOT()->arr[__active_motors[0]].GetPVP()->pos;
260  __gripper_last_pos[1] = 0; //counter
261 }
262 
263 void
264 KatanaControllerKni::move_to(float x, float y, float z, float phi, float theta, float psi, bool blocking)
265 {
266  cleanup_active_motors();
267 
268  try {
269  __katana->moveRobotTo(__x, __y, __z, __phi, __theta, __psi, blocking);
270  } catch (KNI::NoSolutionException &e) {
271  throw fawkes::KatanaNoSolutionException("KNI NoSolutionException:%s", e.what());
272  } catch (/*KNI*/::Exception &e) {
273  throw fawkes::Exception("KNI Exception:%s", e.what());
274  return;
275  }
276 
277  for(short i=0; i<__katana->getNumberOfMotors(); ++i) {
278  add_active_motor(i);
279  }
280 }
281 
282 void
283 KatanaControllerKni::move_to(std::vector<int> encoders, bool blocking)
284 {
285  cleanup_active_motors();
286 
287  try {
288  __katana->moveRobotToEnc(encoders);
289  } catch (/*KNI*/::Exception &e) {
290  throw fawkes::Exception("KNI Exception:%s", e.what());
291  }
292 
293  for(unsigned short i=0; i<encoders.size(); ++i) {
294  add_active_motor(i);
295  }
296 }
297 
298 void
299 KatanaControllerKni::move_to(std::vector<float> angles, bool blocking)
300 {
301  std::vector<int> encoders;
302 
303  try {
304  for(unsigned int i=0; i<angles.size(); i++) {
305  encoders.push_back(KNI_MHF::rad2enc( (double)angles.at(i), __motor_init.at(i).angleOffset, __motor_init.at(i).encodersPerCycle, __motor_init.at(i).encoderOffset, __motor_init.at(i).rotationDirection));
306  }
307  } catch (/*KNI*/::Exception &e) {
308  throw fawkes::Exception("KNI Exception:%s", e.what());
309  }
310 
311  move_to(encoders, blocking);
312 }
313 
314 void
315 KatanaControllerKni::move_motor_to(unsigned short id, int enc, bool blocking)
316 {
317  if( motor_oor(id) )
318  throw fawkes::KatanaOutOfRangeException("Motor out of range.");
319 
320  cleanup_active_motors();
321 
322  try {
323  __katana->moveMotorToEnc(id, enc);
324  } catch (/*KNI*/::Exception &e) {
325  throw fawkes::Exception("KNI Exception:%s", e.what());
326  }
327 
328  add_active_motor(id);
329 }
330 
331 void
332 KatanaControllerKni::move_motor_to(unsigned short id, float angle, bool blocking)
333 {
334  if( motor_oor(id) )
335  throw fawkes::KatanaOutOfRangeException("Motor out of range.");
336 
337  cleanup_active_motors();
338 
339  try {
340  __katana->moveMotorTo(id, angle);
341  } catch (/*KNI*/::Exception &e) {
342  throw fawkes::Exception("KNI Exception:%s", e.what());
343  }
344 
345  add_active_motor(id);
346 }
347 
348 void
349 KatanaControllerKni::move_motor_by(unsigned short id, int enc, bool blocking)
350 {
351  if( motor_oor(id) )
352  throw fawkes::KatanaOutOfRangeException("Motor out of range.");
353 
354  cleanup_active_motors();
355 
356  try {
357  __katana->moveMotorByEnc(id, enc);
358  } catch (/*KNI*/::Exception &e) {
359  throw fawkes::Exception("KNI Exception:%s", e.what());
360  }
361 
362  add_active_motor(id);
363 }
364 
365 void
366 KatanaControllerKni::move_motor_by(unsigned short id, float angle, bool blocking)
367 {
368  if( motor_oor(id) )
369  throw fawkes::KatanaOutOfRangeException("Motor out of range.");
370 
371  cleanup_active_motors();
372 
373  try {
374  __katana->moveMotorBy(id, angle);
375  } catch (/*KNI*/::Exception &e) {
376  throw fawkes::Exception("KNI Exception:%s", e.what());
377  }
378 
379  add_active_motor(id);
380 }
381 
382 
383 // getters
384 double
386 {
387  return __x;
388 }
389 
390 double
392 {
393  return __y;
394 }
395 
396 double
398 {
399  return __z;
400 }
401 
402 double
404 {
405  return __phi;
406 }
407 
408 double
410 {
411  return __theta;
412 }
413 
414 double
416 {
417  return __psi;
418 }
419 
420 void
421 KatanaControllerKni::get_sensors(std::vector<int>& to, bool refresh)
422 {
423  if( refresh )
425 
426  try {
427  const TSctDAT *sensor_data = __sensor_ctrl->GetDAT();
428 
429  const int num_sensors = (size_t)sensor_data->cnt;
430  to.clear();
431  to.resize(num_sensors);
432 
433  for (int i = 0; i < num_sensors; ++i) {
434  to[i] = sensor_data->arr[i];
435  }
436  } catch (/*KNI*/::Exception &e) {
437  throw fawkes::Exception("KNI Exception:%s", e.what());
438  }
439 }
440 
441 void
442 KatanaControllerKni::get_encoders(std::vector<int>& to, bool refresh)
443 {
444  try {
445  std::vector<int> encoders = __katana->getRobotEncoders(refresh);
446 
447  to.clear();
448  to = encoders;
449  } catch (/*KNI*/::Exception &e) {
450  throw fawkes::Exception("KNI Exception:%s", e.what());
451  }
452 }
453 
454 void
455 KatanaControllerKni::get_angles(std::vector<float>& to, bool refresh)
456 {
457  try {
458  std::vector<int> encoders = __katana->getRobotEncoders(refresh);
459 
460  to.clear();
461  for(unsigned int i=0; i<encoders.size(); i++) {
462  to.push_back(KNI_MHF::enc2rad( encoders.at(i), __motor_init.at(i).angleOffset, __motor_init.at(i).encodersPerCycle, __motor_init.at(i).encoderOffset, __motor_init.at(i).rotationDirection));
463  }
464  } catch (/*KNI*/::Exception &e) {
465  throw fawkes::Exception("KNI Exception:%s", e.what());
466  }
467 }
468 
469 
470 bool
471 KatanaControllerKni::motor_oor(unsigned short id)
472 {
473  return id > (unsigned short)__katana->getNumberOfMotors();
474 }
475 
476 bool
477 KatanaControllerKni::motor_final(unsigned short id)
478 {
479  CMotBase mot = __katbase->GetMOT()->arr[id];
480  if (mot.GetPVP()->msf == MSF_MOTCRASHED)
481  throw fawkes::KatanaMotorCrashedException("Motor %u crashed.", id);
482 
483  // extra check for gripper, consider final if not moved for a while
484  unsigned short gripper_not_moved = 0;
485  if (id == __katbase->GetMOT()->cnt - 1) {
486  if (__gripper_last_pos[0] == mot.GetPVP()->pos) {
487  __gripper_last_pos[1] += 1;
488  } else {
489  __gripper_last_pos[0] = mot.GetPVP()->pos;
490  __gripper_last_pos[1] = 0;
491  }
492  gripper_not_moved = __gripper_last_pos[1];
493  }
494 
495  return (std::abs(mot.GetTPS()->tarpos - mot.GetPVP()->pos) < 10)
496  or (gripper_not_moved > 3);
497 }
498 
499 void
500 KatanaControllerKni::cleanup_active_motors()
501 {
502  for(unsigned int i=0; i<__active_motors.size(); ++i) {
503  if( motor_final(__active_motors.at(i)) ) {
504  __active_motors.erase(__active_motors.begin()+i);
505  --i;
506  }
507  }
508 }
509 
510 void
511 KatanaControllerKni::add_active_motor(unsigned short id)
512 {
513  for(unsigned int i=0; i<__active_motors.size(); ++i) {
514  if( __active_motors.at(i) == id ) {
515  return;
516  }
517  }
518  __active_motors.push_back(id);
519 }
520 
521 } // end of namespace fawkes
virtual double z()
Get z-coordinate of latest endeffector position.
virtual bool final()
Check if movement is final.
virtual void setup(std::string &device, std::string &kni_conffile, unsigned int read_timeout, unsigned int write_timeout)
Setup parameters needed to initialize Katana arm with libkni.
virtual double theta()
Get theta-rotation of latest endeffector orientation.
virtual void read_sensor_data()
Read all sensor data from device into controller libray.
At least one motor is out of range.
Definition: exception.h:40
Fawkes library namespace.
virtual void turn_on()
Turn on arm/motors.
virtual void move_to(float x, float y, float z, float phi, float theta, float psi, bool blocking=false)
Move endeffctor to given coordinates.
virtual double phi()
Get x-coordinate of latest endeffector position.
virtual double x()
Get x-coordinate of latest endeffector position.
virtual const char * what() const
Get primary string.
Definition: exception.cpp:661
No joint configuration for desired target found.
Definition: exception.h:34
virtual void init()
Initialize controller.
virtual void move_motor_by(unsigned short id, int enc, bool blocking=false)
Move single joint/motor by encoder value (i.e.
virtual void turn_off()
Turn off arm/motors.
virtual void move_motor_to(unsigned short id, int enc, bool blocking=false)
Move single joint/motor to encoder value.
virtual void set_max_velocity(unsigned int vel)
Set maximum velocity.
virtual void gripper_close(bool blocking=false)
Close Gripper.
virtual double y()
Get y-coordinate of latest endeffector position.
virtual void read_motor_data()
Read motor data of currently active joints from device into controller libray.
virtual bool joint_angles()
Check if controller provides joint angle values.
virtual ~KatanaControllerKni()
Destructor.
virtual void calibrate()
Calibrate the arm.
KatanaControllerKni()
Constructor.
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual double psi()
Get psi-rotation of latest endeffector orientation.
virtual void get_encoders(std::vector< int > &to, bool refresh=false)
Get encoder values of joints/motors.
virtual void get_angles(std::vector< float > &to, bool refresh=false)
Get angle values of joints/motors.
virtual void get_sensors(std::vector< int > &to, bool refresh=false)
Get sensor values.
virtual bool joint_encoders()
Check if controller provides joint encoder values.
At least one motor crashed.
Definition: exception.h:45
virtual void gripper_open(bool blocking=false)
Open Gripper.
virtual void stop()
Stop movement immediately.
virtual void read_coordinates(bool refresh=false)
Store current coordinates of endeeffctor.