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