Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * controller_openrave.cpp - OpenRAVE Controller class for katana arm 00004 * 00005 * Created: Sat Jan 07 16:10:54 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_openrave.h" 00024 #include "exception.h" 00025 00026 #include <core/exceptions/software.h> 00027 00028 #ifdef HAVE_OPENRAVE 00029 #include <plugins/openrave/environment.h> 00030 #include <plugins/openrave/robot.h> 00031 #include <plugins/openrave/manipulator.h> 00032 00033 #include <cmath> 00034 00035 using namespace OpenRAVE; 00036 #endif 00037 00038 namespace fawkes { 00039 #if 0 /* just to make Emacs auto-indent happy */ 00040 } 00041 #endif 00042 00043 /** @class KatanaControllerOpenrave <plugins/katana/controller_kni.h> 00044 * Controller class for a Neuronics Katana, using libkni to interact 00045 * with the real Katana arm. 00046 * @author Bahram Maleki-Fard 00047 */ 00048 00049 #ifdef HAVE_OPENRAVE 00050 00051 /** Constructor. 00052 * @param openrave pointer to OpenRaveConnector aspect. 00053 */ 00054 KatanaControllerOpenrave::KatanaControllerOpenrave(fawkes::OpenRaveConnector* openrave) 00055 { 00056 __openrave = openrave; 00057 __OR_env = NULL; 00058 __OR_robot = NULL; 00059 __OR_manip = NULL; 00060 __initialized = false; 00061 } 00062 00063 /** Destructor. */ 00064 KatanaControllerOpenrave::~KatanaControllerOpenrave() 00065 { 00066 // Setting to NULL also deletes instance (RefPtr) 00067 00068 __openrave = NULL; 00069 __OR_env = NULL; 00070 __OR_robot = NULL; 00071 __OR_manip = NULL; 00072 } 00073 00074 00075 void 00076 KatanaControllerOpenrave::init() 00077 { 00078 try { 00079 __OR_env = __openrave->get_environment(); 00080 __OR_robot = __openrave->get_active_robot(); 00081 00082 if( __OR_robot == NULL) { 00083 throw fawkes::Exception("Cannot access OpenRaveRobot in current OpenRaveEnvironment."); 00084 } 00085 // TODO: get robot string and name, compare to this! 00086 // __robot->GetName(); 00087 00088 __OR_manip = __OR_robot->get_manipulator(); 00089 __env = __OR_env->get_env_ptr(); 00090 __robot = __OR_robot->get_robot_ptr(); 00091 __manip = __robot->GetActiveManipulator(); 00092 __initialized = true; 00093 00094 } catch (OpenRAVE::openrave_exception &e) { 00095 throw fawkes::Exception("OpenRAVE Exception:%s", e.what()); 00096 } 00097 } 00098 00099 void 00100 KatanaControllerOpenrave::set_max_velocity(unsigned int vel) 00101 { 00102 check_init(); 00103 try { 00104 EnvironmentMutex::scoped_lock lock(__env->GetMutex()); 00105 std::vector<dReal> v; 00106 __OR_manip->get_angles(v); 00107 v.assign(v.size(), (dReal)(vel / 100.0)); 00108 00109 __robot->SetActiveDOFVelocities(v); 00110 } catch( /*OpenRAVE*/::openrave_exception &e) { 00111 throw fawkes::Exception("OpenRAVE Exception:%s", e.what()); 00112 } 00113 } 00114 00115 00116 bool 00117 KatanaControllerOpenrave::final() 00118 { 00119 check_init(); 00120 return __robot->GetController()->IsDone(); 00121 } 00122 00123 bool 00124 KatanaControllerOpenrave::joint_angles() 00125 { 00126 return true; 00127 } 00128 bool 00129 KatanaControllerOpenrave::joint_encoders() 00130 { 00131 return false; 00132 } 00133 00134 void 00135 KatanaControllerOpenrave::calibrate() 00136 { 00137 // do nothing, arm in OpenRAVE does not need calibration 00138 } 00139 00140 void 00141 KatanaControllerOpenrave::stop() 00142 { 00143 check_init(); 00144 try { 00145 EnvironmentMutex::scoped_lock lock(__env->GetMutex()); 00146 std::vector<dReal> v; 00147 __robot->GetActiveDOFValues(v); 00148 __robot->SetActiveDOFValues(v); 00149 } catch( /*OpenRAVE*/::openrave_exception &e) { 00150 throw fawkes::Exception("OpenRAVE Exception:%s", e.what()); 00151 } 00152 } 00153 00154 void 00155 KatanaControllerOpenrave::turn_on() 00156 { 00157 } 00158 00159 void 00160 KatanaControllerOpenrave::turn_off() 00161 { 00162 } 00163 00164 void 00165 KatanaControllerOpenrave::read_coordinates(bool refresh) 00166 { 00167 check_init(); 00168 try { 00169 update_manipulator(); 00170 EnvironmentMutex::scoped_lock lock(__env->GetMutex()); 00171 Transform tf = __manip->GetEndEffectorTransform(); 00172 __x = tf.trans[0]; 00173 __y = tf.trans[1]; 00174 __z = tf.trans[2]; 00175 //transform quat to euler. 00176 TransformMatrix m = matrixFromQuat(tf.rot); 00177 std::vector<dReal> v; 00178 __OR_manip->get_angles_device(v); 00179 __phi = v.at(0) - 0.5*M_PI; //phi is directly derivable from joint0 00180 __psi = 0.5*M_PI - v.at(4); //psi is directly derivable from joint4 00181 __theta = acos(m.m[10]); 00182 //theta has correct range from 0-360°, but need to check if sign is also correct. use sinus for that 00183 if( asin(m.m[2] / sin(__phi)) < 0.0 ) 00184 __theta *= -1.0; 00185 } catch( /*OpenRAVE*/::openrave_exception &e ) { 00186 throw fawkes::Exception("OpenRAVE Exception:%s", e.what()); 00187 } 00188 } 00189 00190 void 00191 KatanaControllerOpenrave::read_motor_data() 00192 { 00193 //no need, simulation loop should always be running 00194 } 00195 00196 void 00197 KatanaControllerOpenrave::read_sensor_data() 00198 { 00199 //no need, simulation loop should always be running 00200 } 00201 00202 void 00203 KatanaControllerOpenrave::gripper_open(bool blocking) 00204 { 00205 00206 } 00207 00208 void 00209 KatanaControllerOpenrave::gripper_close(bool blocking) 00210 { 00211 00212 } 00213 00214 void 00215 KatanaControllerOpenrave::move_to(float x, float y, float z, float phi, float theta, float psi, bool blocking) 00216 { 00217 // This method is only here for conveniance, used by KNI 00218 throw fawkes::KatanaUnsupportedException("OpenRAVE Controller does not accept Euler Rotation"); 00219 } 00220 00221 void 00222 KatanaControllerOpenrave::move_to(std::vector<int> encoders, bool blocking) 00223 { 00224 throw fawkes::KatanaUnsupportedException("OpenRAVE Controller does not accept encoders"); 00225 } 00226 00227 void 00228 KatanaControllerOpenrave::move_to(std::vector<float> angles, bool blocking) 00229 { 00230 check_init(); 00231 try { 00232 std::vector<dReal> v; 00233 __OR_manip->set_angles_device(angles); 00234 00235 __OR_manip->get_angles(v); 00236 EnvironmentMutex::scoped_lock lock(__env->GetMutex()); 00237 __robot->SetActiveDOFValues(v); 00238 usleep(2000); 00239 } catch( /*OpenRAVE*/::openrave_exception &e) { 00240 throw fawkes::Exception("OpenRAVE Exception:%s", e.what()); 00241 } 00242 00243 if( blocking ) { 00244 wait_finished(); 00245 } 00246 } 00247 00248 void 00249 KatanaControllerOpenrave::move_motor_to(unsigned short id, int enc, bool blocking) 00250 { 00251 throw fawkes::KatanaUnsupportedException("OpenRAVE Controller does not accept encoders"); 00252 } 00253 00254 void 00255 KatanaControllerOpenrave::move_motor_to(unsigned short id, float angle, bool blocking) 00256 { 00257 check_init(); 00258 try { 00259 std::vector<dReal> v; 00260 __OR_manip->get_angles_device(v); 00261 v.at(id) = (dReal)angle; 00262 __OR_manip->set_angles_device(v); 00263 00264 __OR_manip->get_angles(v); 00265 EnvironmentMutex::scoped_lock lock(__env->GetMutex()); 00266 __robot->SetActiveDOFValues(v); 00267 usleep(2000); 00268 } catch( /*OpenRAVE*/::openrave_exception &e) { 00269 throw fawkes::Exception("OpenRAVE Exception:%s", e.what()); 00270 } 00271 00272 if( blocking ) { 00273 wait_finished(); 00274 } 00275 } 00276 00277 void 00278 KatanaControllerOpenrave::move_motor_by(unsigned short id, int enc, bool blocking) 00279 { 00280 throw fawkes::KatanaUnsupportedException("OpenRAVE Controller does not accept encoders"); 00281 } 00282 00283 void 00284 KatanaControllerOpenrave::move_motor_by(unsigned short id, float angle, bool blocking) 00285 { 00286 check_init(); 00287 try { 00288 std::vector<dReal> v; 00289 __OR_manip->get_angles_device(v); 00290 v.at(id) += (dReal)angle; 00291 __OR_manip->set_angles_device(v); 00292 00293 __OR_manip->get_angles(v); 00294 EnvironmentMutex::scoped_lock lock(__env->GetMutex()); 00295 __robot->SetActiveDOFValues(v); 00296 usleep(2000); 00297 } catch( /*OpenRAVE*/::openrave_exception &e) { 00298 throw fawkes::Exception("OpenRAVE Exception:%s", e.what()); 00299 } 00300 00301 if( blocking ) { 00302 wait_finished(); 00303 } 00304 } 00305 00306 00307 // getters 00308 double 00309 KatanaControllerOpenrave::x() 00310 { 00311 return __x; 00312 } 00313 00314 double 00315 KatanaControllerOpenrave::y() 00316 { 00317 return __y; 00318 } 00319 00320 double 00321 KatanaControllerOpenrave::z() 00322 { 00323 return __z; 00324 } 00325 00326 double 00327 KatanaControllerOpenrave::phi() 00328 { 00329 return __phi; 00330 } 00331 00332 double 00333 KatanaControllerOpenrave::theta() 00334 { 00335 return __theta; 00336 } 00337 00338 double 00339 KatanaControllerOpenrave::psi() 00340 { 00341 return __psi; 00342 } 00343 00344 void 00345 KatanaControllerOpenrave::get_sensors(std::vector<int>& to, bool refresh) 00346 { 00347 check_init(); 00348 to.clear(); 00349 to.resize(0); 00350 } 00351 00352 void 00353 KatanaControllerOpenrave::get_encoders(std::vector<int>& to, bool refresh) 00354 { 00355 throw fawkes::KatanaUnsupportedException("OpenRAVE Controller does not accept encoders"); 00356 } 00357 00358 void 00359 KatanaControllerOpenrave::get_angles(std::vector<float>& to, bool refresh) 00360 { 00361 check_init(); 00362 try { 00363 EnvironmentMutex::scoped_lock lock(__env->GetMutex()); 00364 std::vector<dReal> v; 00365 __robot->GetActiveDOFValues(v); 00366 __OR_manip->set_angles(v); 00367 00368 __OR_manip->get_angles_device(to); 00369 } catch( /*OpenRAVE*/::openrave_exception &e) { 00370 throw fawkes::Exception("OpenRAVE Exception:%s", e.what()); 00371 } 00372 } 00373 00374 00375 void 00376 KatanaControllerOpenrave::update_manipulator() 00377 { 00378 EnvironmentMutex::scoped_lock lock(__env->GetMutex()); 00379 __manip = __robot->GetActiveManipulator(); 00380 } 00381 00382 void 00383 KatanaControllerOpenrave::wait_finished() 00384 { 00385 while( !final() ) { 00386 usleep(1000); 00387 } 00388 } 00389 00390 bool 00391 KatanaControllerOpenrave::motor_oor(unsigned short id) 00392 { 00393 check_init(); 00394 std::vector<dReal> v; 00395 __OR_manip->get_angles_device(v); 00396 00397 return id > v.size(); 00398 } 00399 00400 void 00401 KatanaControllerOpenrave::check_init() 00402 { 00403 if( !__initialized ) { 00404 init(); 00405 // init() will throw an exception if it fails 00406 } 00407 } 00408 00409 #endif // HAVE_OPENRAVE 00410 00411 } // end of namespace fawkes