Fawkes API  Fawkes Development Version
goto_openrave_thread.cpp
00001 
00002 /***************************************************************************
00003  *  goto_openrave_thread.cpp - Katana goto one-time thread using openrave lib
00004  *
00005  *  Created: Wed Jun 10 11:45:31 2009
00006  *  Copyright  2006-2009  Tim Niemueller [www.niemueller.de]
00007  *                  2010  Bahram Maleki-Fard
00008  *
00009  ****************************************************************************/
00010 
00011 /*  This program is free software; you can redistribute it and/or modify
00012  *  it under the terms of the GNU General Public License as published by
00013  *  the Free Software Foundation; either version 2 of the License, or
00014  *  (at your option) any later version.
00015  *
00016  *  This program is distributed in the hope that it will be useful,
00017  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00018  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00019  *  GNU Library General Public License for more details.
00020  *
00021  *  Read the full text in the LICENSE.GPL file in the doc directory.
00022  */
00023 
00024 #include "goto_openrave_thread.h"
00025 #include "conversion.h"
00026 #include "controller.h"
00027 #include "exception.h"
00028 
00029 #include <cstdlib>
00030 
00031 #include <utils/time/time.h>
00032 
00033 #ifdef HAVE_OPENRAVE
00034 
00035 #include <plugins/openrave/robot.h>
00036 #include <plugins/openrave/environment.h>
00037 #include <plugins/openrave/manipulators/katana6M180.h>
00038 #include <plugins/openrave/manipulators/neuronics_katana.h>
00039 #include <plugins/openrave/types.h>
00040 
00041 #include <vector>
00042 #endif
00043 using namespace fawkes;
00044 
00045 /** @class KatanaGotoOpenRaveThread "goto_openrave_thread.h"
00046  * Katana collision-free goto thread.
00047  * This thread moves the arm into a specified position,
00048  * using IK and path-planning from OpenRAVE.
00049  * @author Tim Niemueller (goto_thread.h/cpp)
00050  * @author Bahram Maleki-Fard (OpenRAVE extension)
00051  */
00052 
00053 #ifdef HAVE_OPENRAVE
00054 
00055 /// @cond SELFEXPLAINING
00056 const std::string KatanaGotoOpenRaveThread::DEFAULT_PLANNERPARAMS =
00057                   "minimumgoalpaths 16 postprocessingparameters <_nmaxiterations>100</_nmaxiterations>"
00058                   "<_postprocessing planner=\"parabolicsmoother\"><_nmaxiterations>200</_nmaxiterations>"
00059                   "</_postprocessing>\n";
00060 const std::string KatanaGotoOpenRaveThread::DEFAULT_PLANNERPARAMS_STRAIGHT =
00061                   "maxdeviationangle 0.05";
00062 /// @endcond
00063 
00064 
00065 /** Constructor.
00066  * @param katana katana controller base class
00067  * @param logger logger
00068  * @param openrave pointer to OpenRaveConnector aspect
00069  * @param poll_interval_ms interval in ms between two checks if the
00070  * final position has been reached
00071  * @param robot_file path to robot's xml-file
00072  * @param arm_model arm model used in robot_file, either "5dof" or "6dof_dummy"
00073  * @param autoload_IK true, if IK databas should be automatically generated (recommended)
00074  * @param use_viewer true, if viewer should be started (default: false)
00075  */
00076 KatanaGotoOpenRaveThread::KatanaGotoOpenRaveThread(fawkes::RefPtr<fawkes::KatanaController> katana,
00077                                    fawkes::Logger *logger,
00078                                    fawkes::OpenRaveConnector* openrave,
00079                                    unsigned int poll_interval_ms,
00080                                    std::string robot_file,
00081                                    std::string arm_model,
00082                                    bool autoload_IK,
00083                                    bool use_viewer)
00084   : KatanaMotionThread("KatanaGotoOpenRaveThread", katana, logger),
00085   __OR_robot( 0 ),
00086   __OR_manip( 0 ),
00087   __target_object( "" ),
00088   __target_traj( 0 ),
00089   __cfg_robot_file( robot_file ),
00090   __cfg_arm_model( arm_model ),
00091   __cfg_autoload_IK( autoload_IK ),
00092   __cfg_use_viewer( use_viewer ),
00093   __is_target_object( 0 ),
00094   __has_target_quaternion( 0 ),
00095   __move_straight( 0 ),
00096   __is_arm_extension( 0 ),
00097   __plannerparams( "default" ),
00098   __plannerparams_straight( "default" ),
00099   _openrave( openrave )
00100 {
00101 }
00102 
00103 
00104 /** Set target position.
00105  * @param x X coordinate relative to base
00106  * @param y Y coordinate relative to base
00107  * @param z Z coordinate relative to base
00108  * @param phi Phi Euler angle of tool
00109  * @param theta Theta Euler angle of tool
00110  * @param psi Psi Euler angle of tool
00111  */
00112 void
00113 KatanaGotoOpenRaveThread::set_target(float x, float y, float z,
00114                              float phi, float theta, float psi)
00115 {
00116   __x     = x;
00117   __y     = y;
00118   __z     = z;
00119   __phi   = (phi);
00120   __theta = (theta);
00121   __psi   = (psi);
00122 
00123   __has_target_quaternion = false;
00124   __is_target_object = false;
00125   __move_straight = false;
00126   __is_arm_extension = false;
00127 }
00128 
00129 /** Set target position.
00130  * @param x X coordinate relative to base
00131  * @param y Y coordinate relative to base
00132  * @param z Z coordinate relative to base
00133  * @param quat_x x value of quaternion for tool's rotation
00134  * @param quat_y y value of quaternion for tool's rotation
00135  * @param quat_z z value of quaternion for tool's rotation
00136  * @param quat_w w value of quaternion for tool's rotation
00137  */
00138 void
00139 KatanaGotoOpenRaveThread::set_target(float x, float y, float z,
00140                              float quat_x, float quat_y, float quat_z, float quat_w)
00141 {
00142   __x      = x;
00143   __y      = y;
00144   __z      = z;
00145   __quat_x = quat_x;
00146   __quat_y = quat_y;
00147   __quat_z = quat_z;
00148   __quat_w = quat_w;
00149 
00150   __has_target_quaternion = true;
00151   __is_target_object = false;
00152   __move_straight = false;
00153   __is_arm_extension = false;
00154 }
00155 
00156 /** Set target position.
00157  * @param object_name name of the object (kinbody) in OpenRaveEnvironment
00158  */
00159 void
00160 KatanaGotoOpenRaveThread::set_target(const std::string& object_name, float rot_x)
00161 {
00162   __target_object = object_name;
00163 
00164   __is_target_object = true;
00165 }
00166 
00167 /** Set theta error
00168  * @param error error in radians
00169  */
00170 void
00171 KatanaGotoOpenRaveThread::set_theta_error(float error)
00172 {
00173   __theta_error = error;
00174 }
00175 
00176 /** Set if arm should move straight.
00177  * Make sure to call this after(!) a "set_target" method, as they
00178  * set "__move_straight" attribute to its default value.
00179  * @param move_straight true, if arm should move straight
00180  */
00181 void
00182 KatanaGotoOpenRaveThread::set_move_straight(bool move_straight)
00183 {
00184   __move_straight = move_straight;
00185 }
00186 
00187 /** Set if target is taken as arm extension.
00188  * Make sure to call this after(!) a "set_target" method, as they
00189  * set "__move_straight" attribute to its default value.
00190  * @param arm_extension true, if target is regarded as arm extension
00191  */
00192 void
00193 KatanaGotoOpenRaveThread::set_arm_extension(bool arm_extension)
00194 {
00195   __is_arm_extension = arm_extension;
00196 }
00197 
00198 /** Set plannerparams.
00199  * @param params plannerparameters. For further information, check openrave plugin, or OpenRAVE documentaiton.
00200  * @param straight true, if these params are for straight movement
00201  */
00202 void
00203 KatanaGotoOpenRaveThread::set_plannerparams(std::string& params, bool straight)
00204 {
00205   if( straight ) {
00206     __plannerparams_straight = params;
00207   } else {
00208     __plannerparams = params;
00209   }
00210 }
00211 
00212 /** Set plannerparams.
00213  * @param params plannerparameters. For further information, check openrave plugin, or OpenRAVE documentaiton.
00214  * @param straight true, if these params are for straight movement
00215  */
00216 void
00217 KatanaGotoOpenRaveThread::set_plannerparams(const char* params, bool straight)
00218 {
00219   if( straight ) {
00220     __plannerparams_straight = params;
00221   } else {
00222     __plannerparams = params;
00223   }
00224 }
00225 
00226 void
00227 KatanaGotoOpenRaveThread::init()
00228 {
00229   try {
00230     __OR_robot = _openrave->add_robot(__cfg_robot_file, false);
00231 
00232     // configure manipulator
00233     // TODO: from config parameters? neccessary?
00234     if( __cfg_arm_model == "5dof" ) {
00235       __OR_manip = new OpenRaveManipulatorNeuronicsKatana(5, 5);
00236       __OR_manip->add_motor(0,0);
00237       __OR_manip->add_motor(1,1);
00238       __OR_manip->add_motor(2,2);
00239       __OR_manip->add_motor(3,3);
00240       __OR_manip->add_motor(4,4);
00241 
00242       // Set manipulator and offsets.
00243       // offsetZ: katana.kinbody is 0.165 above ground; coordinate system of real katana has origin in intersection of j1 and j2 (i.e. start of link L2: 0.2015 on z-axis)
00244       // offsetX: katana.kinbody is setup 0.0725 on +x axis
00245       _openrave->set_manipulator(__OR_robot, __OR_manip, 0.f, 0.f, 0.f);
00246       __OR_robot->get_robot_ptr()->SetActiveManipulator("arm_kni");
00247 
00248       if( __cfg_autoload_IK ) {
00249         _openrave->get_environment()->load_IK_solver(__OR_robot, OpenRAVE::IKP_TranslationDirection5D);
00250       }
00251     } else if ( __cfg_arm_model == "6dof_dummy" ) {
00252       __OR_manip = new OpenRaveManipulatorKatana6M180(6, 5);
00253       __OR_manip->add_motor(0,0);
00254       __OR_manip->add_motor(1,1);
00255       __OR_manip->add_motor(2,2);
00256       __OR_manip->add_motor(4,3);
00257       __OR_manip->add_motor(5,4);
00258 
00259       // Set manipulator and offsets.
00260       // offsetZ: katana.kinbody is 0.165 above ground; coordinate system of real katana has origin in intersection of j1 and j2 (i.e. start of link L2: 0.2015 on z-axis)
00261       // offsetX: katana.kinbody is setup 0.0725 on +x axis
00262       _openrave->set_manipulator(__OR_robot, __OR_manip, 0.f, 0.f, 0.f);
00263 
00264       if( __cfg_autoload_IK ) {
00265         _openrave->get_environment()->load_IK_solver(__OR_robot, OpenRAVE::IKP_Transform6D);
00266       }
00267     } else {
00268       throw fawkes::Exception("Unknown entry for 'arm_model':%s", __cfg_arm_model.c_str());
00269     }
00270 
00271   } catch (Exception& e) {
00272     // TODO: not just simple throw....
00273     throw;
00274   }
00275 
00276   if( __cfg_use_viewer)
00277     _openrave->start_viewer();
00278 }
00279 
00280 void
00281 KatanaGotoOpenRaveThread::finalize()
00282 {
00283   delete(__OR_robot);
00284   __OR_robot = NULL;
00285 
00286   delete(__OR_manip);
00287   __OR_manip = NULL;
00288 }
00289 
00290 void
00291 KatanaGotoOpenRaveThread::once()
00292 {
00293 #ifndef EARLY_PLANNING
00294   if( !plan_target() ) {
00295     _finished = true;
00296     return;
00297   }
00298 #else
00299   if( _error_code != fawkes::KatanaInterface::ERROR_NONE ) {
00300     _finished = true;
00301     return;
00302   }
00303 #endif
00304 
00305   // Get trajectories and move katana along them
00306   __target_traj = __OR_robot->get_trajectory_device();
00307   Time time_now, time_last = Time();
00308   try {
00309     bool final = false;
00310     __it = __target_traj->begin();
00311     while (!final) {
00312       time_last.stamp_systime();
00313       final = move_katana();
00314       update_openrave_data();
00315       time_now.stamp_systime();
00316 
00317       // Wait before sending next command. W.it until 5ms before reached time for next traj point
00318       // CAUTION! In order for this to work correctly, you need to assure that OpenRAVE model of the
00319       //   arm and the real device have the same velocity, i.e. need the same amount of time to complete
00320       //   a movement. Otherwise sampling over time and waiting does not make much sense.
00321       //   Disable the following line if requirement not fulfilled.
00322 
00323       //usleep(1000*1000*(sampling + time_last.in_sec() - time_now.in_sec() - 0.005f));
00324     }
00325 
00326     // check if encoders are close enough to target position
00327     final = false;
00328     while (!final) {
00329       final = true;
00330       update_openrave_data();
00331       try {
00332         final = _katana->final();
00333       } catch (fawkes::KatanaMotorCrashedException &e) {
00334         _logger->log_warn("KatanaMotorControlTrhead", e.what());
00335         _error_code = fawkes::KatanaInterface::ERROR_MOTOR_CRASHED;
00336         break;
00337       }
00338     }
00339 
00340   } catch (fawkes::Exception &e) {
00341     _logger->log_warn("KatanaGotoThread", "Moving along trajectory failed (ignoring): %s", e.what());
00342     _finished = true;
00343     _error_code = fawkes::KatanaInterface::ERROR_CMD_START_FAILED;
00344     _katana->stop();
00345     return;
00346   }
00347 
00348 
00349   // TODO: Check for finished motion
00350   // Can't use current version like in goto_thread, because target is
00351   // not set in libkni!  can check endeffector position instead, or
00352   // check last angle values from __target_traj with katana-arm values
00353 
00354   _finished = true;
00355 }
00356 
00357 
00358 /** Peform path-planning on target.
00359  * @return true if ik solvable and path planned, false otherwise
00360  */
00361 bool
00362 KatanaGotoOpenRaveThread::plan_target()
00363 {
00364   // Fetch motor encoder values
00365   if( !update_motor_data() ) {
00366     _logger->log_warn("KatanaGotoThread", "Fetching current motor values failed");
00367     _error_code = fawkes::KatanaInterface::ERROR_COMMUNICATION;
00368     return false;
00369   }
00370 
00371   // Set starting point for planner, convert encoder values to angles if necessary
00372   if( !_katana->joint_angles() ) {
00373     encToRad(__motor_encoders, __motor_angles);
00374   }
00375   __OR_manip->set_angles_device(__motor_angles);
00376 
00377   // Checking if target has IK solution
00378   if( __plannerparams.compare("default") == 0 ) {
00379     __plannerparams = DEFAULT_PLANNERPARAMS;
00380   }
00381   if( __is_target_object) {
00382     _logger->log_debug(name(), "Check IK for object (%s)", __target_object.c_str());
00383 
00384     if( !_openrave->set_target_object(__target_object, __OR_robot) ) {
00385       _logger->log_warn("KatanaGotoThread", "Initiating goto failed, no IK solution found");
00386       _error_code = fawkes::KatanaInterface::ERROR_NO_SOLUTION;
00387       return false;
00388     }
00389   }
00390   else {
00391     bool success = false;
00392     try {
00393       if( __has_target_quaternion ) {
00394         _logger->log_debug(name(), "Check IK(%f,%f,%f  |  %f,%f,%f,%f)",
00395                            __x, __y, __z, __quat_x, __quat_y, __quat_z, __quat_w);
00396         success = __OR_robot->set_target_quat(__x, __y, __z, __quat_w, __quat_x, __quat_y, __quat_z);
00397       } else if( __move_straight ) {
00398         _logger->log_debug(name(), "Check IK(%f,%f,%f), straight movement",
00399                            __x, __y, __z);
00400         if( __is_arm_extension ) {
00401           success = __OR_robot->set_target_rel(__x, __y, __z, true);
00402         } else {
00403           success = __OR_robot->set_target_straight(__x, __y, __z);
00404         }
00405         if( __plannerparams_straight.compare("default") == 0 ) {
00406           __plannerparams_straight = DEFAULT_PLANNERPARAMS_STRAIGHT;
00407         }
00408       } else {
00409         float theta_error = 0.0f;
00410         while( !success && (theta_error <= __theta_error)) {
00411           _logger->log_debug(name(), "Check IK(%f,%f,%f  |  %f,%f,%f)",
00412                              __x, __y, __z, __phi, __theta+theta_error, __psi);
00413           success = __OR_robot->set_target_euler(EULER_ZXZ, __x, __y, __z, __phi, __theta+theta_error, __psi);
00414           if( !success ) {
00415             _logger->log_debug(name(), "Check IK(%f,%f,%f  |  %f,%f,%f)",
00416                                __x, __y, __z, __phi, __theta-theta_error, __psi);
00417             success = __OR_robot->set_target_euler(EULER_ZXZ, __x, __y, __z, __phi, __theta-theta_error, __psi);
00418           }
00419 
00420           theta_error += 0.01;
00421         }
00422       }
00423     } catch(OpenRAVE::openrave_exception &e) {
00424       _logger->log_debug(name(), "OpenRAVE exception:%s", e.what());
00425     }
00426 
00427     if( !success ) {
00428       _logger->log_warn("KatanaGotoThread", "Initiating goto failed, no IK solution found");
00429       _error_code = fawkes::KatanaInterface::ERROR_NO_SOLUTION;
00430       return false;
00431     }
00432   }
00433   if( __move_straight ) {
00434     __OR_robot->set_target_plannerparams(__plannerparams_straight);
00435   } else {
00436     __OR_robot->set_target_plannerparams(__plannerparams);
00437   }
00438 
00439   // Run planner
00440   float sampling = 0.04f; //maybe catch from config? or "learning" depending on performance?
00441   try {
00442     _openrave->run_planner(__OR_robot, sampling);
00443   } catch (fawkes::Exception &e) {
00444     _logger->log_warn("KatanaGotoThread", "Planner failed (ignoring): %s", e.what());
00445     _error_code = fawkes::KatanaInterface::ERROR_UNSPECIFIC;
00446     return false;
00447   }
00448 
00449   _error_code = fawkes::KatanaInterface::ERROR_NONE;
00450   return true;
00451 }
00452 
00453 /** Update data of arm in OpenRAVE model */
00454 void
00455 KatanaGotoOpenRaveThread::update_openrave_data()
00456 {
00457   // Fetch motor encoder values
00458   if( !update_motor_data() ) {
00459     _logger->log_warn("KatanaGotoThread", "Fetching current motor values failed");
00460     _finished = true;
00461     return;
00462   }
00463 
00464   // Convert encoder values to angles if necessary
00465   if( !_katana->joint_angles() ) {
00466     encToRad(__motor_encoders, __motor_angles);
00467   }
00468   __OR_manip->set_angles_device(__motor_angles);
00469 
00470   std::vector<OpenRAVE::dReal> angles;
00471   __OR_manip->get_angles(angles);
00472 
00473   {
00474     OpenRAVE::EnvironmentMutex::scoped_lock lock(__OR_robot->get_robot_ptr()->GetEnv()->GetMutex());
00475     __OR_robot->get_robot_ptr()->SetActiveDOFValues(angles);
00476   }
00477 }
00478 
00479 /** Update motors and fetch current encoder values.
00480  * @return true if succesful, false otherwise
00481  */
00482 bool
00483 KatanaGotoOpenRaveThread::update_motor_data()
00484 {
00485   short num_errors  = 0;
00486 
00487   // update motors
00488   while (1) {
00489     //usleep(__poll_interval_usec);
00490     try {
00491       _katana->read_sensor_data(); // update sensor data
00492       _katana->read_motor_data(); // update motor data
00493     } catch (Exception &e) {
00494       if (++num_errors <= 10) {
00495         _logger->log_warn("KatanaGotoThread", "Receiving motor data failed, retrying");
00496         continue;
00497       } else {
00498         _logger->log_warn("KatanaGotoThread", "Receiving motor data failed too often, aborting");
00499         _error_code = fawkes::KatanaInterface::ERROR_COMMUNICATION;
00500         return 0;
00501       }
00502     }
00503     break;
00504   }
00505 
00506   // fetch joint values
00507   num_errors = 0;
00508   while (1) {
00509     //usleep(__poll_interval_usec);
00510     try {
00511       if( _katana->joint_angles()) {
00512         _katana->get_angles(__motor_angles, false);    //fetch encoder values, param refreshEncoders=false
00513       } else {
00514         _katana->get_encoders(__motor_encoders, false);    //fetch encoder values, param refreshEncoders=false
00515       }
00516     } catch (Exception &e) {
00517       if (++num_errors <= 10) {
00518         _logger->log_warn("KatanaGotoThread", "Receiving motor %s failed, retrying", _katana->joint_angles() ? "angles" : "encoders");
00519         continue;
00520       } else {
00521         _logger->log_warn("KatanaGotoThread", "Receiving motor %s failed, aborting", _katana->joint_angles() ? "angles" : "encoders");
00522         _error_code = fawkes::KatanaInterface::ERROR_COMMUNICATION;
00523         return 0;
00524       }
00525     }
00526     break;
00527   }
00528 
00529   return 1;
00530 }
00531 
00532 
00533 /** Realize next trajectory point.
00534  * Take the next point from the current trajectory __target_traj, set its
00535  * joint values and advance the iterator.
00536  * @return true if the trajectory is finished, i.e. there are no more points
00537  * left, false otherwise.
00538  */
00539 bool
00540 KatanaGotoOpenRaveThread::move_katana()
00541 {
00542   if( _katana->joint_angles() ) {
00543     _katana->move_to(*__it, /*blocking*/false);
00544   } else {
00545     std::vector<int> enc;
00546     _katana->get_encoders(enc);
00547     _katana->move_to(enc, /*blocking*/false);
00548   }
00549 
00550   return (++__it == __target_traj->end());
00551 }
00552 
00553 #endif