Fawkes API  Fawkes Development Version
controller_kni.cpp
00001 
00002 /***************************************************************************
00003  *  controller_kni.cpp - KNI Controller class for katana arm
00004  *
00005  *  Created: Tue Jan 03 11:40:31 2012
00006  *  Copyright  2012  Bahram Maleki-Fard, AllemaniACs RoboCup Team
00007  *
00008  ****************************************************************************/
00009 
00010 /*  This program is free software; you can redistribute it and/or modify
00011  *  it under the terms of the GNU General Public License as published by
00012  *  the Free Software Foundation; either version 2 of the License, or
00013  *  (at your option) any later version.
00014  *
00015  *  This program is distributed in the hope that it will be useful,
00016  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018  *  GNU Library General Public License for more details.
00019  *
00020  *  Read the full text in the LICENSE.GPL file in the doc directory.
00021  */
00022 
00023 #include "controller_kni.h"
00024 #include "exception.h"
00025 
00026 #include <core/exceptions/software.h>
00027 
00028 #include <kniBase.h>
00029 #include <common/MathHelperFunctions.h>
00030 
00031 namespace fawkes {
00032 #if 0 /* just to make Emacs auto-indent happy */
00033 }
00034 #endif
00035 
00036 /** @class KatanaControllerKni <plugins/katana/controller_kni.h>
00037  * Controller class for a Neuronics Katana, using libkni to interact
00038  * with the real Katana arm.
00039  * @author Bahram Maleki-Fard
00040  */
00041 
00042 /** Constructor. */
00043 KatanaControllerKni::KatanaControllerKni()
00044 {
00045   // setting default setup values
00046   __cfg_device = "/dev/ttyS0";
00047   __cfg_kni_conffile = "/etc/kni3/hd300/katana6M180.cfg";
00048   __cfg_read_timeout = 100;
00049   __cfg_write_timeout = 0;
00050 
00051   __gripper_last_pos.clear();
00052   __gripper_last_pos.resize(2);
00053 }
00054 
00055 /** Destructor. */
00056 KatanaControllerKni::~KatanaControllerKni()
00057 {
00058   // Setting to NULL also deletes instance (RefPtr)
00059   __katana = NULL;
00060 
00061   __device.reset();
00062   __protocol.reset();
00063 }
00064 
00065 /** Setup parameters needed to initialize Katana arm with libkni.
00066  * @param device device string, e.g. "/dev/ttyS0"
00067  * @param kni_conffile path to kni configfile, e.g. "/etc/kni3/hd300/katana6M180.cfg"
00068  * @param read_timeout timeout for read operations, in ms
00069  * @param write_timeout timeout for write operations, in ms
00070  */
00071 void
00072 KatanaControllerKni::setup(std::string& device, std::string& kni_conffile,
00073                            unsigned int read_timeout, unsigned int write_timeout)
00074 {
00075   __cfg_device = device;
00076   __cfg_kni_conffile = kni_conffile;
00077   __cfg_read_timeout = read_timeout;
00078   __cfg_write_timeout = write_timeout;
00079 }
00080 
00081 void
00082 KatanaControllerKni::init()
00083 {
00084   try {
00085     TCdlCOMDesc ccd = {0, 57600, 8, 'N', 1, (int)__cfg_read_timeout, (int)__cfg_write_timeout};
00086     __device.reset(new CCdlCOM(ccd, __cfg_device.c_str()));
00087 
00088     __protocol.reset(new CCplSerialCRC());
00089     __protocol->init(__device.get());
00090 
00091     __katana = RefPtr<CLMBase>(new CLMBase());
00092     __katana->create(__cfg_kni_conffile.c_str(), __protocol.get());
00093     __katbase = __katana->GetBase();
00094     __sensor_ctrl = &__katbase->GetSCT()->arr[0];
00095 
00096     __katbase->recvECH();
00097   } catch (/*KNI*/::Exception &e) {
00098     throw fawkes::Exception("KNI Exception:%s", e.what());
00099   }
00100 
00101   try {
00102     __motor_init.resize(__katana->getNumberOfMotors());
00103     for(unsigned int i=0; i<__motor_init.size(); i++) {
00104       __motor_init.at(i) = *(__katbase->GetMOT()->arr[i].GetInitialParameters());
00105     }
00106   } catch (/*KNI*/::Exception &e) {
00107     throw fawkes::Exception("KNI Exception:%s", e.what());
00108   }
00109 }
00110 
00111 void
00112 KatanaControllerKni::set_max_velocity(unsigned int vel)
00113 {
00114   try {
00115     __katana->setRobotVelocityLimit((int)vel);
00116   } catch (/*KNI*/::Exception &e) {
00117     throw fawkes::Exception("KNI Exception:%s", e.what());
00118   }
00119 }
00120 
00121 
00122 bool
00123 KatanaControllerKni::final()
00124 {
00125   if( __active_motors.size() < 1 )
00126     return true;
00127 
00128   bool final = true;
00129   for(unsigned int i=0; i<__active_motors.size(); i++) {
00130     final &= motor_final(__active_motors.at(i));
00131   }
00132   cleanup_active_motors();
00133   return final;
00134 }
00135 
00136 bool
00137 KatanaControllerKni::joint_angles()
00138 {
00139   return true;
00140 }
00141 bool
00142 KatanaControllerKni::joint_encoders()
00143 {
00144   return true;
00145 }
00146 
00147 void
00148 KatanaControllerKni::calibrate()
00149 {
00150   try {
00151     __katana->calibrate();
00152   } catch (/*KNI*/::Exception &e) {
00153     throw fawkes::Exception("KNI Exception:%s", e.what());
00154   }
00155 }
00156 
00157 void
00158 KatanaControllerKni::stop()
00159 {
00160   try {
00161     __katana->freezeRobot();
00162   } catch (/*KNI*/::Exception &e) {
00163     throw fawkes::Exception("KNI Exception:%s", e.what());
00164   }
00165 }
00166 
00167 void
00168 KatanaControllerKni::turn_on()
00169 {
00170   try {
00171     __katana->switchRobotOn();
00172   } catch (/*KNI*/::Exception &e) {
00173     throw fawkes::Exception("KNI Exception:%s", e.what());
00174   }
00175 }
00176 
00177 void
00178 KatanaControllerKni::turn_off()
00179 {
00180   try {
00181     __katana->switchRobotOff();
00182   } catch (/*KNI*/::Exception &e) {
00183     throw fawkes::Exception("KNI Exception:%s", e.what());
00184   }
00185 }
00186 
00187 void
00188 KatanaControllerKni::read_coordinates(bool refresh)
00189 {
00190   try {
00191     __katana->getCoordinates(__x, __y, __z, __phi, __theta, __psi, refresh);
00192   } catch (/*KNI*/::Exception &e) {
00193     throw fawkes::Exception("KNI Exception:%s", e.what());
00194   }
00195 }
00196 
00197 void
00198 KatanaControllerKni::read_motor_data()
00199 {
00200   try {
00201     if( __active_motors.size() == (unsigned short)__katana->getNumberOfMotors() ) {
00202       __katbase->recvMPS(); // get position for all motors
00203       __katbase->recvGMS(); // get status flags for all motors
00204     } else {
00205       const TKatMOT* mot = __katbase->GetMOT();
00206       for(unsigned int i=0; i<__active_motors.size(); i++) {
00207         mot->arr[__active_motors.at(i)].recvPVP(); // get position data for motor
00208       }
00209     }
00210   } catch (/*KNI*/::Exception &e) {
00211     throw fawkes::Exception("KNI Exception:%s", e.what());
00212   }
00213 }
00214 
00215 void
00216 KatanaControllerKni::read_sensor_data()
00217 {
00218   try {
00219     __sensor_ctrl->recvDAT();
00220   } catch (/*KNI*/::ParameterReadingException &e) {
00221     throw fawkes::Exception("KNI ParameterReadingException:%s", e.what());
00222   } catch (/*KNI*/::Exception &e) {
00223     throw fawkes::Exception("KNI Exception:%s", e.what());
00224   }
00225 }
00226 
00227 void
00228 KatanaControllerKni::gripper_open(bool blocking)
00229 {
00230   try {
00231     __katana->openGripper(blocking);
00232   } catch (/*KNI*/::Exception &e) {
00233     throw fawkes::Exception("KNI Exception:%s", e.what());
00234   }
00235 
00236   __active_motors.clear();
00237   __active_motors.resize(1);
00238   __active_motors[0] = __katbase->GetMOT()->cnt - 1;
00239 
00240   __gripper_last_pos.clear();
00241   __gripper_last_pos[0] = __katbase->GetMOT()->arr[__active_motors[0]].GetPVP()->pos;
00242   __gripper_last_pos[1] = 0; //counter
00243 }
00244 
00245 void
00246 KatanaControllerKni::gripper_close(bool blocking)
00247 {
00248   try {
00249     __katana->closeGripper(blocking);
00250   } catch (/*KNI*/::Exception &e) {
00251     throw fawkes::Exception("KNI Exception:%s", e.what());
00252   }
00253 
00254   __active_motors.clear();
00255   __active_motors.resize(1);
00256   __active_motors[0] = __katbase->GetMOT()->cnt - 1;
00257 
00258   __gripper_last_pos.clear();
00259   __gripper_last_pos[0] = __katbase->GetMOT()->arr[__active_motors[0]].GetPVP()->pos;
00260   __gripper_last_pos[1] = 0; //counter
00261 }
00262 
00263 void
00264 KatanaControllerKni::move_to(float x, float y, float z, float phi, float theta, float psi, bool blocking)
00265 {
00266   cleanup_active_motors();
00267 
00268   try {
00269     __katana->moveRobotTo(__x, __y, __z, __phi, __theta, __psi, blocking);
00270   } catch (KNI::NoSolutionException &e) {
00271     throw fawkes::KatanaNoSolutionException("KNI NoSolutionException:%s", e.what());
00272   } catch (/*KNI*/::Exception &e) {
00273     throw fawkes::Exception("KNI Exception:%s", e.what());
00274     return;
00275   }
00276 
00277   for(short i=0; i<__katana->getNumberOfMotors(); ++i) {
00278     add_active_motor(i);
00279   }
00280 }
00281 
00282 void
00283 KatanaControllerKni::move_to(std::vector<int> encoders, bool blocking)
00284 {
00285   cleanup_active_motors();
00286 
00287   try {
00288     __katana->moveRobotToEnc(encoders);
00289   } catch (/*KNI*/::Exception &e) {
00290     throw fawkes::Exception("KNI Exception:%s", e.what());
00291   }
00292 
00293   for(unsigned short i=0; i<encoders.size(); ++i) {
00294     add_active_motor(i);
00295   }
00296 }
00297 
00298 void
00299 KatanaControllerKni::move_to(std::vector<float> angles, bool blocking)
00300 {
00301   std::vector<int> encoders;
00302 
00303   try {
00304     for(unsigned int i=0; i<angles.size(); i++) {
00305       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));
00306     }
00307   } catch (/*KNI*/::Exception &e) {
00308     throw fawkes::Exception("KNI Exception:%s", e.what());
00309   }
00310 
00311   move_to(encoders, blocking);
00312 }
00313 
00314 void
00315 KatanaControllerKni::move_motor_to(unsigned short id, int enc, bool blocking)
00316 {
00317   if( motor_oor(id) )
00318     throw fawkes::KatanaOutOfRangeException("Motor out of range.");
00319 
00320   cleanup_active_motors();
00321 
00322   try {
00323     __katana->moveMotorToEnc(id, enc);
00324   } catch (/*KNI*/::Exception &e) {
00325     throw fawkes::Exception("KNI Exception:%s", e.what());
00326   }
00327 
00328   add_active_motor(id);
00329 }
00330 
00331 void
00332 KatanaControllerKni::move_motor_to(unsigned short id, float angle, bool blocking)
00333 {
00334   if( motor_oor(id) )
00335     throw fawkes::KatanaOutOfRangeException("Motor out of range.");
00336 
00337   cleanup_active_motors();
00338 
00339   try {
00340     __katana->moveMotorTo(id, angle);
00341   } catch (/*KNI*/::Exception &e) {
00342     throw fawkes::Exception("KNI Exception:%s", e.what());
00343   }
00344 
00345   add_active_motor(id);
00346 }
00347 
00348 void
00349 KatanaControllerKni::move_motor_by(unsigned short id, int enc, bool blocking)
00350 {
00351   if( motor_oor(id) )
00352     throw fawkes::KatanaOutOfRangeException("Motor out of range.");
00353 
00354   cleanup_active_motors();
00355 
00356   try {
00357     __katana->moveMotorByEnc(id, enc);
00358   } catch (/*KNI*/::Exception &e) {
00359     throw fawkes::Exception("KNI Exception:%s", e.what());
00360   }
00361 
00362   add_active_motor(id);
00363 }
00364 
00365 void
00366 KatanaControllerKni::move_motor_by(unsigned short id, float angle, bool blocking)
00367 {
00368   if( motor_oor(id) )
00369     throw fawkes::KatanaOutOfRangeException("Motor out of range.");
00370 
00371   cleanup_active_motors();
00372 
00373   try {
00374     __katana->moveMotorBy(id, angle);
00375   } catch (/*KNI*/::Exception &e) {
00376     throw fawkes::Exception("KNI Exception:%s", e.what());
00377   }
00378 
00379   add_active_motor(id);
00380 }
00381 
00382 
00383 // getters
00384 double
00385 KatanaControllerKni::x()
00386 {
00387   return __x;
00388 }
00389 
00390 double
00391 KatanaControllerKni::y()
00392 {
00393   return __y;
00394 }
00395 
00396 double
00397 KatanaControllerKni::z()
00398 {
00399   return __z;
00400 }
00401 
00402 double
00403 KatanaControllerKni::phi()
00404 {
00405   return __phi;
00406 }
00407 
00408 double
00409 KatanaControllerKni::theta()
00410 {
00411   return __theta;
00412 }
00413 
00414 double
00415 KatanaControllerKni::psi()
00416 {
00417   return __psi;
00418 }
00419 
00420 void
00421 KatanaControllerKni::get_sensors(std::vector<int>& to, bool refresh)
00422 {
00423   if( refresh )
00424     read_sensor_data();
00425 
00426   try {
00427     const TSctDAT *sensor_data = __sensor_ctrl->GetDAT();
00428 
00429     const int num_sensors = (size_t)sensor_data->cnt;
00430     to.clear();
00431     to.resize(num_sensors);
00432 
00433     for (int i = 0; i < num_sensors; ++i) {
00434       to[i] = sensor_data->arr[i];
00435     }
00436   } catch (/*KNI*/::Exception &e) {
00437     throw fawkes::Exception("KNI Exception:%s", e.what());
00438   }
00439 }
00440 
00441 void
00442 KatanaControllerKni::get_encoders(std::vector<int>& to, bool refresh)
00443 {
00444   try {
00445     std::vector<int> encoders = __katana->getRobotEncoders(refresh);
00446 
00447     to.clear();
00448     to = encoders;
00449   } catch (/*KNI*/::Exception &e) {
00450     throw fawkes::Exception("KNI Exception:%s", e.what());
00451   }
00452 }
00453 
00454 void
00455 KatanaControllerKni::get_angles(std::vector<float>& to, bool refresh)
00456 {
00457   try {
00458     std::vector<int> encoders = __katana->getRobotEncoders(refresh);
00459 
00460     to.clear();
00461     for(unsigned int i=0; i<encoders.size(); i++) {
00462       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));
00463     }
00464   } catch (/*KNI*/::Exception &e) {
00465     throw fawkes::Exception("KNI Exception:%s", e.what());
00466   }
00467 }
00468 
00469 
00470 bool
00471 KatanaControllerKni::motor_oor(unsigned short id)
00472 {
00473   return id > (unsigned short)__katana->getNumberOfMotors();
00474 }
00475 
00476 bool
00477 KatanaControllerKni::motor_final(unsigned short id)
00478 {
00479   CMotBase mot = __katbase->GetMOT()->arr[id];
00480   if (mot.GetPVP()->msf == MSF_MOTCRASHED)
00481     throw fawkes::KatanaMotorCrashedException("Motor %u crashed.", id);
00482 
00483   // extra check for gripper, consider final if not moved for a while
00484   unsigned short gripper_not_moved = 0;
00485   if (id == __katbase->GetMOT()->cnt - 1) {
00486     if (__gripper_last_pos[0] == mot.GetPVP()->pos) {
00487       __gripper_last_pos[1] += 1;
00488     } else {
00489       __gripper_last_pos[0] = mot.GetPVP()->pos;
00490       __gripper_last_pos[1] = 0;
00491     }
00492     gripper_not_moved = __gripper_last_pos[1];
00493   }
00494 
00495   return (std::abs(mot.GetTPS()->tarpos - mot.GetPVP()->pos) < 10)
00496       or (gripper_not_moved > 3);
00497 }
00498 
00499 void
00500 KatanaControllerKni::cleanup_active_motors()
00501 {
00502   for(unsigned int i=0; i<__active_motors.size(); ++i) {
00503     if( motor_final(__active_motors.at(i)) ) {
00504       __active_motors.erase(__active_motors.begin()+i);
00505       --i;
00506     }
00507   }
00508 }
00509 
00510 void
00511 KatanaControllerKni::add_active_motor(unsigned short id)
00512 {
00513   for(unsigned int i=0; i<__active_motors.size(); ++i) {
00514     if( __active_motors.at(i) == id ) {
00515       return;
00516     }
00517   }
00518   __active_motors.push_back(id);
00519 }
00520 
00521 } // end of namespace fawkes