Fawkes API
Fawkes Development Version
|
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