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 
25 #include "exception.h"
26 
27 #include <common/MathHelperFunctions.h>
28 #include <core/exceptions/software.h>
29 
30 #include <kniBase.h>
31 
32 namespace fawkes {
33 
34 /** @class KatanaControllerKni <plugins/katana/controller_kni.h>
35  * Controller class for a Neuronics Katana, using libkni to interact
36  * with the real Katana arm.
37  * @author Bahram Maleki-Fard
38  */
39 
40 /** Constructor. */
42 : cfg_device_("/dev/ttyS0"), cfg_kni_conffile_("/etc/kni3/hd300/katana6M180.cfg")
43 {
44  cfg_read_timeout_ = 100;
45  cfg_write_timeout_ = 0;
46 
47  gripper_last_pos_.clear();
48  gripper_last_pos_.resize(2);
49 
50  x_ = y_ = z_ = 0.;
51  phi_ = theta_ = psi_ = 0.;
52 }
53 
54 /** Destructor. */
56 {
57  // Setting to NULL also deletes instance (RefPtr)
58  katana_ = NULL;
59 
60  device_.reset();
61  protocol_.reset();
62 }
63 
64 /** Setup parameters needed to initialize Katana arm with libkni.
65  * @param device device string, e.g. "/dev/ttyS0"
66  * @param kni_conffile path to kni configfile, e.g. "/etc/kni3/hd300/katana6M180.cfg"
67  * @param read_timeout timeout for read operations, in ms
68  * @param write_timeout timeout for write operations, in ms
69  */
70 void
71 KatanaControllerKni::setup(std::string &device,
72  std::string &kni_conffile,
73  unsigned int read_timeout,
74  unsigned int write_timeout)
75 {
76  cfg_device_ = device;
77  cfg_kni_conffile_ = kni_conffile;
78  cfg_read_timeout_ = read_timeout;
79  cfg_write_timeout_ = write_timeout;
80 }
81 
82 void
84 {
85  try {
86  TCdlCOMDesc ccd = {0, 57600, 8, 'N', 1, (int)cfg_read_timeout_, (int)cfg_write_timeout_};
87  device_.reset(new CCdlCOM(ccd, cfg_device_.c_str()));
88 
89  protocol_.reset(new CCplSerialCRC());
90  protocol_->init(device_.get());
91 
92  katana_ = RefPtr<CLMBase>(new CLMBase());
93  katana_->create(cfg_kni_conffile_.c_str(), protocol_.get());
94  katbase_ = katana_->GetBase();
95  sensor_ctrl_ = &katbase_->GetSCT()->arr[0];
96 
97  katbase_->recvECH();
98  } catch (/*KNI*/ ::Exception &e) {
99  throw fawkes::Exception("KNI Exception:%s", e.what());
100  }
101 
102  try {
103  motor_init_.resize(katana_->getNumberOfMotors());
104  for (unsigned int i = 0; i < motor_init_.size(); i++) {
105  motor_init_.at(i) = *(katbase_->GetMOT()->arr[i].GetInitialParameters());
106  }
107  } catch (/*KNI*/ ::Exception &e) {
108  throw fawkes::Exception("KNI Exception:%s", e.what());
109  }
110 }
111 
112 void
114 {
115  try {
116  katana_->setRobotVelocityLimit((int)vel);
117  } catch (/*KNI*/ ::Exception &e) {
118  throw fawkes::Exception("KNI Exception:%s", e.what());
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
265  float y,
266  float z,
267  float phi,
268  float theta,
269  float psi,
270  bool blocking)
271 {
272  cleanup_active_motors();
273 
274  try {
275  katana_->moveRobotTo(x_, y_, z_, phi_, theta_, psi_, blocking);
276  } catch (KNI::NoSolutionException &e) {
277  throw fawkes::KatanaNoSolutionException("KNI NoSolutionException:%s", e.what());
278  } catch (/*KNI*/ ::Exception &e) {
279  throw fawkes::Exception("KNI Exception:%s", e.what());
280  }
281 
282  for (short i = 0; i < katana_->getNumberOfMotors(); ++i) {
283  add_active_motor(i);
284  }
285 }
286 
287 void
288 KatanaControllerKni::move_to(std::vector<int> encoders, bool blocking)
289 {
290  cleanup_active_motors();
291 
292  try {
293  katana_->moveRobotToEnc(encoders);
294  } catch (/*KNI*/ ::Exception &e) {
295  throw fawkes::Exception("KNI Exception:%s", e.what());
296  }
297 
298  for (unsigned short i = 0; i < encoders.size(); ++i) {
299  add_active_motor(i);
300  }
301 }
302 
303 void
304 KatanaControllerKni::move_to(std::vector<float> angles, bool blocking)
305 {
306  std::vector<int> encoders;
307 
308  try {
309  for (unsigned int i = 0; i < angles.size(); i++) {
310  encoders.push_back(KNI_MHF::rad2enc((double)angles.at(i),
311  motor_init_.at(i).angleOffset,
312  motor_init_.at(i).encodersPerCycle,
313  motor_init_.at(i).encoderOffset,
314  motor_init_.at(i).rotationDirection));
315  }
316  } catch (/*KNI*/ ::Exception &e) {
317  throw fawkes::Exception("KNI Exception:%s", e.what());
318  }
319 
320  move_to(encoders, blocking);
321 }
322 
323 void
324 KatanaControllerKni::move_motor_to(unsigned short id, int enc, bool blocking)
325 {
326  if (motor_oor(id))
327  throw fawkes::KatanaOutOfRangeException("Motor out of range.");
328 
329  cleanup_active_motors();
330 
331  try {
332  katana_->moveMotorToEnc(id, enc);
333  } catch (/*KNI*/ ::Exception &e) {
334  throw fawkes::Exception("KNI Exception:%s", e.what());
335  }
336 
337  add_active_motor(id);
338 }
339 
340 void
341 KatanaControllerKni::move_motor_to(unsigned short id, float angle, bool blocking)
342 {
343  if (motor_oor(id))
344  throw fawkes::KatanaOutOfRangeException("Motor out of range.");
345 
346  cleanup_active_motors();
347 
348  try {
349  katana_->moveMotorTo(id, angle);
350  } catch (/*KNI*/ ::Exception &e) {
351  throw fawkes::Exception("KNI Exception:%s", e.what());
352  }
353 
354  add_active_motor(id);
355 }
356 
357 void
358 KatanaControllerKni::move_motor_by(unsigned short id, int enc, bool blocking)
359 {
360  if (motor_oor(id))
361  throw fawkes::KatanaOutOfRangeException("Motor out of range.");
362 
363  cleanup_active_motors();
364 
365  try {
366  katana_->moveMotorByEnc(id, enc);
367  } catch (/*KNI*/ ::Exception &e) {
368  throw fawkes::Exception("KNI Exception:%s", e.what());
369  }
370 
371  add_active_motor(id);
372 }
373 
374 void
375 KatanaControllerKni::move_motor_by(unsigned short id, float angle, bool blocking)
376 {
377  if (motor_oor(id))
378  throw fawkes::KatanaOutOfRangeException("Motor out of range.");
379 
380  cleanup_active_motors();
381 
382  try {
383  katana_->moveMotorBy(id, angle);
384  } catch (/*KNI*/ ::Exception &e) {
385  throw fawkes::Exception("KNI Exception:%s", e.what());
386  }
387 
388  add_active_motor(id);
389 }
390 
391 // getters
392 double
394 {
395  return x_;
396 }
397 
398 double
400 {
401  return y_;
402 }
403 
404 double
406 {
407  return z_;
408 }
409 
410 double
412 {
413  return phi_;
414 }
415 
416 double
418 {
419  return theta_;
420 }
421 
422 double
424 {
425  return psi_;
426 }
427 
428 void
429 KatanaControllerKni::get_sensors(std::vector<int> &to, bool refresh)
430 {
431  if (refresh)
433 
434  try {
435  const TSctDAT *sensor_data = sensor_ctrl_->GetDAT();
436 
437  const int num_sensors = (size_t)sensor_data->cnt;
438  to.clear();
439  to.resize(num_sensors);
440 
441  for (int i = 0; i < num_sensors; ++i) {
442  to[i] = sensor_data->arr[i];
443  }
444  } catch (/*KNI*/ ::Exception &e) {
445  throw fawkes::Exception("KNI Exception:%s", e.what());
446  }
447 }
448 
449 void
450 KatanaControllerKni::get_encoders(std::vector<int> &to, bool refresh)
451 {
452  try {
453  std::vector<int> encoders = katana_->getRobotEncoders(refresh);
454 
455  to.clear();
456  to = encoders;
457  } catch (/*KNI*/ ::Exception &e) {
458  throw fawkes::Exception("KNI Exception:%s", e.what());
459  }
460 }
461 
462 void
463 KatanaControllerKni::get_angles(std::vector<float> &to, bool refresh)
464 {
465  try {
466  std::vector<int> encoders = katana_->getRobotEncoders(refresh);
467 
468  to.clear();
469  for (unsigned int i = 0; i < encoders.size(); i++) {
470  to.push_back(KNI_MHF::enc2rad(encoders.at(i),
471  motor_init_.at(i).angleOffset,
472  motor_init_.at(i).encodersPerCycle,
473  motor_init_.at(i).encoderOffset,
474  motor_init_.at(i).rotationDirection));
475  }
476  } catch (/*KNI*/ ::Exception &e) {
477  throw fawkes::Exception("KNI Exception:%s", e.what());
478  }
479 }
480 
481 bool
482 KatanaControllerKni::motor_oor(unsigned short id)
483 {
484  return id > (unsigned short)katana_->getNumberOfMotors();
485 }
486 
487 bool
488 KatanaControllerKni::motor_final(unsigned short id)
489 {
490  CMotBase mot = katbase_->GetMOT()->arr[id];
491  if (mot.GetPVP()->msf == MSF_MOTCRASHED)
492  throw fawkes::KatanaMotorCrashedException("Motor %u crashed.", id);
493 
494  // extra check for gripper, consider final if not moved for a while
495  unsigned short gripper_not_moved = 0;
496  if (id == katbase_->GetMOT()->cnt - 1) {
497  if (gripper_last_pos_[0] == mot.GetPVP()->pos) {
498  gripper_last_pos_[1] += 1;
499  } else {
500  gripper_last_pos_[0] = mot.GetPVP()->pos;
501  gripper_last_pos_[1] = 0;
502  }
503  gripper_not_moved = gripper_last_pos_[1];
504  }
505 
506  return (std::abs(mot.GetTPS()->tarpos - mot.GetPVP()->pos) < 10) or (gripper_not_moved > 3);
507 }
508 
509 void
510 KatanaControllerKni::cleanup_active_motors()
511 {
512  for (unsigned int i = 0; i < active_motors_.size(); ++i) {
513  if (motor_final(active_motors_.at(i))) {
514  active_motors_.erase(active_motors_.begin() + i);
515  --i;
516  }
517  }
518 }
519 
520 void
521 KatanaControllerKni::add_active_motor(unsigned short id)
522 {
523  for (unsigned int i = 0; i < active_motors_.size(); ++i) {
524  if (active_motors_.at(i) == id) {
525  return;
526  }
527  }
528  active_motors_.push_back(id);
529 }
530 
531 } // 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:37
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:639
No joint configuration for desired target found.
Definition: exception.h:31
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.
Base class for exceptions in Fawkes.
Definition: exception.h:35
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:43
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.