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