Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * robot.cpp - Fawkes to OpenRAVE Robot Handler 00004 * 00005 * Created: Mon Sep 20 14:50:34 2010 00006 * Copyright 2010 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 "robot.h" 00024 #include "manipulator.h" 00025 #include "environment.h" 00026 00027 #include <openrave-core.h> 00028 #include <logging/logger.h> 00029 #include <core/exceptions/software.h> 00030 00031 using namespace OpenRAVE; 00032 namespace fawkes { 00033 #if 0 /* just to make Emacs auto-indent happy */ 00034 } 00035 #endif 00036 00037 /** @class OpenRaveRobot <plugins/openrave/robot.h> 00038 * Class handling interaction with the OpenRAVE::RobotBase class. 00039 * This class mainly handles robot specific tasks, like setting a 00040 * target, looking for IK solutions and handling planning parameters 00041 * for the robot. 00042 * @author Bahram Maleki-Fard 00043 */ 00044 00045 /** Constructor 00046 * @param logger pointer to fawkes logger 00047 */ 00048 OpenRaveRobot::OpenRaveRobot(fawkes::Logger* logger) : 00049 __logger( logger ), 00050 __name( "" ), 00051 __manip( 0 ) 00052 { 00053 init(); 00054 } 00055 /** Constructor 00056 * @param filename path to robot's xml file 00057 * @param env pointer to OpenRaveEnvironment object 00058 * @param logger pointer to fawkes logger 00059 */ 00060 OpenRaveRobot::OpenRaveRobot(const std::string& filename, fawkes::OpenRaveEnvironment* env, fawkes::Logger* logger) : 00061 __logger( logger ), 00062 __name( "" ), 00063 __manip( 0 ) 00064 { 00065 init(); 00066 this->load(filename, env); 00067 } 00068 00069 /** Destructor */ 00070 OpenRaveRobot::~OpenRaveRobot() 00071 { 00072 delete __target.manip; 00073 00074 //unload everything related to this robot from environment 00075 try { 00076 EnvironmentMutex::scoped_lock lock(__robot->GetEnv()->GetMutex()); 00077 __robot->GetEnv()->Remove(__mod_basemanip); 00078 __robot->GetEnv()->Remove(__robot); 00079 } catch(const openrave_exception &e) { 00080 if(__logger) 00081 {__logger->log_warn("OpenRAVE Robot", "Could not unload robot properly from environment. Ex:%s", e.what());} 00082 } 00083 } 00084 00085 /** Inittialize object attributes */ 00086 void 00087 OpenRaveRobot::init() 00088 { 00089 __traj = new std::vector< std::vector<dReal> >(); 00090 00091 __trans_offset_x = 0.f; 00092 __trans_offset_y = 0.f; 00093 __trans_offset_z = 0.f; 00094 } 00095 00096 00097 /** Load robot from xml file 00098 * @param filename path to robot's xml file 00099 * @param env pointer to OpenRaveEnvironment object 00100 */ 00101 void 00102 OpenRaveRobot::load(const std::string& filename, fawkes::OpenRaveEnvironment* env) 00103 { 00104 // TODO: implementing without usage of 'environment' 00105 // openrave_exception handling is done in OpenRAVE (see environment-core.h) 00106 __robot = env->get_env_ptr()->ReadRobotXMLFile(filename); 00107 00108 if(!__robot) 00109 {throw fawkes::IllegalArgumentException("OpenRAVE Robot: Robot could not be loaded. Check xml file/path.");} 00110 else if(__logger) 00111 {__logger->log_debug("OpenRAVE Robot", "Robot loaded.");} 00112 } 00113 00114 /** Set robot ready for usage. 00115 * Here: Set active DOFs and create plannerParameters. 00116 * CAUTION: Only successful after added to environment. Otherwise no active DOF will be recognized. */ 00117 void 00118 OpenRaveRobot::set_ready() 00119 { 00120 if(!__robot) 00121 {throw fawkes::Exception("OpenRAVE Robot: Robot not loaded properly yet.");} 00122 00123 __name = __robot->GetName(); 00124 __robot->SetActiveManipulator(__robot->GetManipulators().at(0)->GetName()); 00125 __arm = __robot->GetActiveManipulator(); 00126 __robot->SetActiveDOFs(__arm->GetArmIndices()); 00127 00128 if(__robot->GetActiveDOF() == 0) 00129 {throw fawkes::Exception("OpenRAVE Robot: Robot not added to environment yet. Need to do that first, otherwise planner will fail.");} 00130 00131 // create planner parameters 00132 try { 00133 PlannerBase::PlannerParametersPtr params(new PlannerBase::PlannerParameters()); 00134 __planner_params = params; 00135 __planner_params->_nMaxIterations = 4000; // max iterations before failure 00136 __planner_params->SetRobotActiveJoints(__robot); // set planning configuration space to current active dofs 00137 __planner_params->vgoalconfig.resize(__robot->GetActiveDOF()); 00138 } catch(const openrave_exception &e) { 00139 throw fawkes::Exception("OpenRAVE Robot: Could not create PlannerParameters. Ex:%s", e.what()); 00140 } 00141 00142 // create and load BaseManipulation module 00143 try { 00144 __mod_basemanip = RaveCreateModule(__robot->GetEnv(), "basemanipulation"); 00145 __robot->GetEnv()->AddModule( __mod_basemanip, __robot->GetName()); 00146 } catch(const openrave_exception &e) { 00147 throw fawkes::Exception("OpenRAVE Robot: Cannot load BaseManipulation Module. Ex:%s", e.what()); 00148 } 00149 00150 if(__logger) 00151 {__logger->log_debug("OpenRAVE Robot", "Robot ready.");} 00152 } 00153 00154 /** Directly set transition offset between coordinate systems 00155 * of real device and OpenRAVE model. 00156 * @param trans_x transition offset on x-axis 00157 * @param trans_y transition offset on y-axis 00158 * @param trans_z transition offset on z-axis 00159 */ 00160 void 00161 OpenRaveRobot::set_offset(float trans_x, float trans_y, float trans_z) 00162 { 00163 __trans_offset_x = trans_x; 00164 __trans_offset_y = trans_y; 00165 __trans_offset_z = trans_z; 00166 } 00167 00168 /** Calculate transition offset between coordinate systems 00169 * of real device and OpenRAVE model. 00170 * Sets model's angles to current device's angles (from __manip), 00171 * and compares transitions. 00172 * @param device_trans_x transition on x-axis (real device) 00173 * @param device_trans_y transition on y-axis (real device) 00174 * @param device_trans_z transition on z-axis (real device) 00175 */ 00176 void 00177 OpenRaveRobot::calibrate(float device_trans_x, float device_trans_y, float device_trans_z) 00178 { 00179 EnvironmentMutex::scoped_lock lock(__robot->GetEnv()->GetMutex()); 00180 // get device's current angles, and set them for OpenRAVE model 00181 std::vector<dReal> angles; 00182 __manip->get_angles(angles); 00183 __robot->SetActiveDOFValues(angles); 00184 00185 // get model's current transition and compare 00186 __arm = __robot->GetActiveManipulator(); 00187 Transform trans = __arm->GetEndEffectorTransform(); 00188 __trans_offset_x = trans.trans[0] - device_trans_x; 00189 __trans_offset_y = trans.trans[1] - device_trans_y; 00190 __trans_offset_z = trans.trans[2] - device_trans_z; 00191 } 00192 00193 /** Set pointer to OpenRaveManipulator object. 00194 * Make sure this is called AFTER all manipulator settings have 00195 * been set (assures that __target.manip has the same settings). 00196 * @param manip pointer to OpenRaveManipulator object 00197 * @param display_movements true, if movements should be displayed in viewer. 00198 * Better be "false" if want to sync OpenRAVE models with device 00199 */ 00200 void 00201 OpenRaveRobot::set_manipulator(fawkes::OpenRaveManipulator* manip, bool display_movements) 00202 { 00203 __manip = manip; 00204 __target.manip = new OpenRaveManipulator(*__manip); 00205 00206 __display_planned_movements = display_movements; 00207 } 00208 00209 /** Update motor values from OpenRAVE model. 00210 * Can be used to sync real device with OpenRAVE model*/ 00211 void 00212 OpenRaveRobot::update_manipulator() 00213 { 00214 EnvironmentMutex::scoped_lock lock(__robot->GetEnv()->GetMutex()); 00215 std::vector<dReal> angles; 00216 __robot->GetActiveDOFValues(angles); 00217 __manip->set_angles(angles); 00218 } 00219 00220 /** Update/Set OpenRAVE motor angles */ 00221 void 00222 OpenRaveRobot::update_model() 00223 { 00224 EnvironmentMutex::scoped_lock lock(__robot->GetEnv()->GetMutex()); 00225 std::vector<dReal> angles; 00226 __manip->get_angles(angles); 00227 __robot->SetActiveDOFValues(angles); 00228 } 00229 00230 /** Getter for __display_planned_movements. 00231 * @return return value 00232 */ 00233 bool 00234 OpenRaveRobot::display_planned_movements() const 00235 { 00236 return __display_planned_movements; 00237 } 00238 00239 /** Set target, given relative transition. 00240 * This is the prefered method to set a target for straight manipulator movement. 00241 * @param trans_x x-transition 00242 * @param trans_y y-transition 00243 * @param trans_z z-transition 00244 * @param is_extension true, if base coordination system lies in arm extension 00245 * @return true if solvable, false otherwise 00246 */ 00247 bool 00248 OpenRaveRobot::set_target_rel(float trans_x, float trans_y, float trans_z, bool is_extension) 00249 { 00250 if( is_extension ) { 00251 __target.type = TARGET_RELATIVE_EXT; 00252 } else { 00253 __target.type = TARGET_RELATIVE; 00254 } 00255 __target.x = trans_x; 00256 __target.y = trans_y; 00257 __target.z = trans_z; 00258 00259 // Not sure how to check IK solvability yet. Would be nice to have this 00260 // checked before planning a path. 00261 __target.solvable = true; 00262 00263 return __target.solvable; 00264 } 00265 00266 /** Set target for a straight movement, given transition. 00267 * This is the a wrapper for "set_target_rel", to be able to call for a 00268 * straight arm movement by giving non-relative transition. 00269 * @param trans_x x-transition 00270 * @param trans_y y-transition 00271 * @param trans_z z-transition 00272 * @return true if solvable, false otherwise 00273 */ 00274 bool 00275 OpenRaveRobot::set_target_straight(float trans_x, float trans_y, float trans_z) 00276 { 00277 EnvironmentMutex::scoped_lock lock(__robot->GetEnv()->GetMutex()); 00278 __arm = __robot->GetActiveManipulator(); 00279 Transform trans = __arm->GetEndEffectorTransform(); 00280 00281 return set_target_rel( trans_x - trans.trans[0], 00282 trans_y - trans.trans[1], 00283 trans_z - trans.trans[2]); 00284 } 00285 00286 /** Set target, given transition, and rotation as quaternion. 00287 * @param trans_x x-transition 00288 * @param trans_y y-transition 00289 * @param trans_z z-transition 00290 * @param quat_w quaternion skalar 00291 * @param quat_x quaternion 1st value 00292 * @param quat_y quaternion 2nd value 00293 * @param quat_z quaternion 3rd value 00294 * @param no_offset if true, do not include manipulator offset (default: false) 00295 * @return true if solvable, false otherwise 00296 */ 00297 bool 00298 OpenRaveRobot::set_target_quat(float trans_x, float trans_y, float trans_z, float quat_w, float quat_x, float quat_y, float quat_z, bool no_offset) 00299 { 00300 Vector trans(trans_x, trans_y, trans_z); 00301 Vector rot(quat_w, quat_x, quat_y, quat_z); 00302 00303 return set_target_transform(trans, rot, no_offset); 00304 } 00305 00306 /** Set target, given transition, and rotation as axis-angle. 00307 * @param trans_x x-transition 00308 * @param trans_y y-transition 00309 * @param trans_z z-transition 00310 * @param angle axis-angle angle 00311 * @param axisX axis-angle x-axis value 00312 * @param axisY axis-angle y-axis value 00313 * @param axisZ axis-angle z-axis value 00314 * @param no_offset if true, do not include manipulator offset (default: false) 00315 * @return true if solvable, false otherwise 00316 */ 00317 bool 00318 OpenRaveRobot::set_target_axis_angle(float trans_x, float trans_y, float trans_z, float angle, float axisX, float axisY, float axisZ, bool no_offset) 00319 { 00320 Vector trans(trans_x, trans_y, trans_z); 00321 Vector aa(angle, axisX, axisY, axisZ); 00322 Vector rot = quatFromAxisAngle(aa); 00323 00324 return set_target_transform(trans, rot, no_offset); 00325 } 00326 00327 /** Set target, given transition, and Euler-rotation. 00328 * @param type Euler-rotation type (ZXZ, ZYZ, ...) 00329 * @param trans_x x-transition 00330 * @param trans_y y-transition 00331 * @param trans_z z-transition 00332 * @param phi 1st rotation 00333 * @param theta 2nd rotation 00334 * @param psi 3rd rotation 00335 * @param no_offset if true, do not include manipulator offset (default: false) 00336 * @return true if solvable, false otherwise 00337 */ 00338 bool 00339 OpenRaveRobot::set_target_euler(euler_rotation_t type, float trans_x, float trans_y, float trans_z, float phi, float theta, float psi, bool no_offset) 00340 { 00341 Vector trans(trans_x, trans_y, trans_z); 00342 std::vector<float> rot(9, 0.f); //rotations vector 00343 00344 switch(type) { 00345 case (EULER_ZXZ) : 00346 __logger->log_debug("TEST ZXZ", "%f %f %f %f %f %f", trans_x, trans_y, trans_z, phi, theta, psi); 00347 rot.at(2) = phi; //1st row, 3rd value; rotation on z-axis 00348 rot.at(3) = theta; //2nd row, 1st value; rotation on x-axis 00349 rot.at(8) = psi; //3rd row, 3rd value; rotation on z-axis 00350 break; 00351 00352 case (EULER_ZYZ) : 00353 __logger->log_debug("TEST ZYZ", "%f %f %f %f %f %f", trans_x, trans_y, trans_z, phi, theta, psi); 00354 rot.at(2) = phi; //1st row, 3rd value; rotation on z-axis 00355 rot.at(4) = theta; //2nd row, 2nd value; rotation on y-axis 00356 rot.at(8) = psi; //3rd row, 3rd value; rotation on z-axis 00357 break; 00358 00359 case (EULER_ZYX) : 00360 rot.at(2) = phi; //1st row, 3rd value; rotation on z-axis 00361 rot.at(4) = theta; //2nd row, 2nd value; rotation on y-axis 00362 rot.at(6) = psi; //3rd row, 1st value; rotation on x-axis 00363 break; 00364 00365 default : 00366 __target.type = TARGET_NONE; 00367 __target.solvable = false; 00368 return false; 00369 } 00370 00371 return set_target_euler(trans, rot, no_offset); 00372 } 00373 00374 /** Set target by giving position of an object. 00375 * Currently the object should be cylindric, and stand upright. It may 00376 * also be rotated on its x-axis, but that rotation needs to be given in an argument 00377 * to calculate correct position for end-effector. This is only temporary until 00378 * proper grasp planning for 5DOF in OpenRAVE is provided. 00379 * @param trans_x x-transition of object 00380 * @param trans_y y-transition of object 00381 * @param trans_z z-transition of object 00382 * @param rot_x rotation of object on x-axis (radians) (default: 0.f, i.e. upright) 00383 * @return true if solvable, false otherwise 00384 */ 00385 bool 00386 OpenRaveRobot::set_target_object_position(float trans_x, float trans_y, float trans_z, float rot_x) 00387 { 00388 // This is about 2 times faster than using setTargetEuler each time, especially when it comes 00389 // to the while loop (whole loop: ~56ms vs ~99ms) 00390 00391 // release all attached/grabbed bodys 00392 __robot->ReleaseAllGrabbed(); 00393 00394 // quaternion defining consecutiv rotations on axis 00395 float alpha = atan2(trans_y - __trans_offset_y, trans_x - __trans_offset_x); //angle to rotate left/right when manipulator points to +x 00396 Vector quat_y = quatFromAxisAngle(Vector(0.f, M_PI/2, 0.f)); //1st, rotate down -> manipulator points to +x 00397 Vector quat_x = quatFromAxisAngle(Vector(-alpha, 0.f, 0.f)); //2nd, rotate left/right -> manipulator points to object 00398 Vector quat_z = quatFromAxisAngle(Vector(0.f, 0.f, rot_x)); //last, rotate wrist -> manipulator ready to grab 00399 00400 Vector quat_xY = quatMultiply (quat_y, quat_x); 00401 Vector quat_xYZ = quatMultiply (quat_xY, quat_z); 00402 00403 Vector trans(trans_x, trans_y, trans_z); 00404 00405 if( set_target_transform(trans, quat_xYZ, true) ) 00406 return true; 00407 00408 //try varying 2nd rotation (quat_y) until a valid IK is found. Max angle: 45° (~0.79 rad) 00409 Vector quatPosY=quatFromAxisAngle(Vector(0.f, 0.017f, 0.f)); //rotate up for 1° 00410 Vector quatNegY=quatFromAxisAngle(Vector(0.f, -0.017f, 0.f)); //rotate down for 1° 00411 00412 Vector quatPos(quat_xY); //starting position, after first 2 rotations 00413 Vector quatNeg(quat_xY); 00414 00415 unsigned int count = 0; 00416 bool foundIK = false; 00417 00418 while( (!foundIK) && (count <= 45)) { 00419 count++; 00420 00421 quatPos = quatMultiply(quatPos, quatPosY); //move up ~1° 00422 quatNeg = quatMultiply(quatNeg, quatNegY); //move down ~1° 00423 00424 quat_xYZ = quatMultiply(quatPos, quat_z); //apply wrist rotation 00425 foundIK = set_target_transform(trans, quat_xYZ, true); 00426 if( !foundIK ) { 00427 quat_xYZ = quatMultiply(quatNeg, quat_z); 00428 foundIK = set_target_transform(trans, quat_xYZ, true); 00429 } 00430 } 00431 00432 return foundIK; 00433 } 00434 00435 /** Set target by giving IkParameterizaion of target. 00436 * OpenRAVE::IkParameterization is the desired type to be calculated with 00437 * by OpenRAVE. Each oter type (i.e. Transform) is implicitly transformed 00438 * to an IkParameterization before continuing to check for Ik solution and 00439 * planning, i.e. by the BaseManipulation module. 00440 * @param ik_param the OpenRAVE::IkParameterization of the target 00441 * @return true if solvable, false otherwise 00442 */ 00443 bool 00444 OpenRaveRobot::set_target_ikparam(OpenRAVE::IkParameterization ik_param) 00445 { 00446 EnvironmentMutex::scoped_lock lock(__robot->GetEnv()->GetMutex()); 00447 __arm = __robot->GetActiveManipulator(); 00448 std::vector<OpenRAVE::dReal> target_angles; 00449 00450 __target.ikparam = ik_param; 00451 __target.type = TARGET_IKPARAM; 00452 __target.solvable = __arm->FindIKSolution(ik_param,target_angles,true); 00453 __target.manip->set_angles(target_angles); 00454 00455 return __target.solvable; 00456 } 00457 00458 /** Set additional planner parameters. 00459 * BaseManipulation module accepts many arguments that can be passed. 00460 * Planner parameters can be important to plan a path according to ones 00461 * needs, e.g. set deviations, optimizer iterations, etc. 00462 * Do not mistake it with the single argument "plannerparams" of BaseManipulation. 00463 * @param params complete string of additional arguments. 00464 */ 00465 void 00466 OpenRaveRobot::set_target_plannerparams(std::string& params) 00467 { 00468 __target.plannerparams = params; 00469 } 00470 00471 // just temporary! no IK check etc involved 00472 /** Set target angles directly. 00473 * @param angles vector with angle values 00474 */ 00475 void 00476 OpenRaveRobot::set_target_angles( std::vector<float>& angles ) 00477 { 00478 __target.manip->set_angles(angles); 00479 } 00480 00481 00482 00483 00484 /* ################### getters ##################*/ 00485 /** Returns RobotBasePtr for uses in other classes. 00486 * @return RobotBasePtr of current robot 00487 */ 00488 OpenRAVE::RobotBasePtr 00489 OpenRaveRobot::get_robot_ptr() const 00490 { 00491 return __robot; 00492 } 00493 00494 /** Get target. 00495 * @return target struct 00496 */ 00497 target_t 00498 OpenRaveRobot::get_target() const 00499 { 00500 return __target; 00501 } 00502 00503 /** Get manipulator. 00504 * @return pointer to currentl used OpenRaveManipulator 00505 */ 00506 OpenRaveManipulator* 00507 OpenRaveRobot::get_manipulator() const 00508 { 00509 return __manip; 00510 } 00511 00512 /** Updates planner parameters and return pointer to it 00513 * @return PlannerParametersPtr or robot's planner params 00514 */ 00515 OpenRAVE::PlannerBase::PlannerParametersPtr 00516 OpenRaveRobot::get_planner_params() const 00517 { 00518 EnvironmentMutex::scoped_lock lock(__robot->GetEnv()->GetMutex()); 00519 __manip->get_angles(__planner_params->vinitialconfig); 00520 __target.manip->get_angles(__planner_params->vgoalconfig); 00521 00522 __robot->SetActiveDOFValues(__planner_params->vinitialconfig); 00523 00524 return __planner_params; 00525 } 00526 00527 /** Return pointer to trajectory of motion from 00528 * __manip to __target.manip with OpenRAVE-model angle format 00529 * @return pointer to trajectory 00530 */ 00531 std::vector< std::vector<dReal> >* 00532 OpenRaveRobot::get_trajectory() const 00533 { 00534 return __traj; 00535 } 00536 00537 /** Return pointer to trajectory of motion from 00538 * __manip to __target.manip with device angle format 00539 * @return pointer to trajectory 00540 */ 00541 std::vector< std::vector<float> >* 00542 OpenRaveRobot::get_trajectory_device() const 00543 { 00544 std::vector< std::vector<float> >* traj = new std::vector< std::vector<float> >(); 00545 00546 std::vector<float> v; 00547 00548 for(unsigned int i=0; i<__traj->size(); i++) { 00549 __manip->angles_or_to_device(__traj->at(i), v); 00550 traj->push_back(v); 00551 } 00552 00553 return traj; 00554 } 00555 00556 /** Return BaseManipulation Module-Pointer. 00557 * @return ModuleBasePtr 00558 */ 00559 OpenRAVE::ModuleBasePtr 00560 OpenRaveRobot::get_basemanip() const 00561 { 00562 return __mod_basemanip; 00563 } 00564 00565 00566 /* ###### attach / release kinbodys ###### */ 00567 /** Attach a kinbody to the robot. 00568 * @param object KinbodyPtr of object to be attached 00569 * @return true if successful 00570 */ 00571 bool 00572 OpenRaveRobot::attach_object(OpenRAVE::KinBodyPtr object) 00573 { 00574 bool success = false; 00575 try{ 00576 success = __robot->Grab(object); 00577 } catch(const OpenRAVE::openrave_exception &e) { 00578 if(__logger) 00579 __logger->log_warn("OpenRAVE Robot", "Could not attach Object. Ex:%s", e.what()); 00580 return false; 00581 } 00582 00583 return success; 00584 } 00585 /** Attach a kinbody to the robot. 00586 * @param name name of the object 00587 * @param env pointer to OpenRaveEnvironment object 00588 * @return true if successful 00589 */ 00590 bool 00591 OpenRaveRobot::attach_object(const std::string& name, fawkes::OpenRaveEnvironment* env) 00592 { 00593 OpenRAVE::KinBodyPtr body = env->get_env_ptr()->GetKinBody(name); 00594 00595 return attach_object(body); 00596 } 00597 00598 /** Release a kinbody from the robot. 00599 * @param object KinbodyPtr of object to be released 00600 * @return true if successful 00601 */ 00602 bool 00603 OpenRaveRobot::release_object(OpenRAVE::KinBodyPtr object) 00604 { 00605 try{ 00606 __robot->Release(object); 00607 } catch(const OpenRAVE::openrave_exception &e) { 00608 if(__logger) 00609 __logger->log_warn("OpenRAVE Robot", "Could not release Object. Ex:%s", e.what()); 00610 return false; 00611 } 00612 00613 return true; 00614 } 00615 /** Release a kinbody from the robot. 00616 * @param name name of the object 00617 * @param env pointer to OpenRaveEnvironment object 00618 * @return true if successful 00619 */ 00620 bool 00621 OpenRaveRobot::release_object(const std::string& name, fawkes::OpenRaveEnvironment* env) 00622 { 00623 OpenRAVE::KinBodyPtr body = env->get_env_ptr()->GetKinBody(name); 00624 00625 return release_object(body); 00626 } 00627 00628 /** Release all grabbed kinbodys from the robot. 00629 * @return true if successful 00630 */ 00631 bool 00632 OpenRaveRobot::release_all_objects() 00633 { 00634 try{ 00635 __robot->ReleaseAllGrabbed(); 00636 } catch(const OpenRAVE::openrave_exception &e) { 00637 if(__logger) 00638 __logger->log_warn("OpenRAVE Robot", "Could not release all objects. Ex:%s", e.what()); 00639 return false; 00640 } 00641 00642 return true; 00643 } 00644 00645 00646 00647 00648 /* ######################################## 00649 ###------------- private ------------### 00650 ########################################*/ 00651 00652 /** Set target, given transformation (transition, and rotation as quaternion). 00653 * Check IK solvability for target Transform. If solvable, 00654 * then set target angles to manipulator configuration __target.manip 00655 * @param trans transformation vector 00656 * @param rotQuat rotation vector; a quaternion 00657 * @param no_offset if true, do not include manipulator offset (default: false) 00658 * @return true if solvable, false otherwise 00659 */ 00660 bool 00661 OpenRaveRobot::set_target_transform(OpenRAVE::Vector& trans, OpenRAVE::Vector& rotQuat, bool no_offset) 00662 { 00663 Transform target; 00664 target.trans = trans; 00665 target.rot = rotQuat; 00666 00667 if( !no_offset ) { 00668 target.trans[0] += __trans_offset_x; 00669 target.trans[1] += __trans_offset_y; 00670 target.trans[2] += __trans_offset_z; 00671 } 00672 00673 __target.type = TARGET_TRANSFORM; 00674 __target.x = target.trans[0]; 00675 __target.y = target.trans[1]; 00676 __target.z = target.trans[2]; 00677 __target.qw = target.rot[0]; 00678 __target.qx = target.rot[1]; 00679 __target.qy = target.rot[2]; 00680 __target.qz = target.rot[3]; 00681 00682 // check for supported IK types 00683 EnvironmentMutex::scoped_lock lock(__robot->GetEnv()->GetMutex()); 00684 __arm = __robot->GetActiveManipulator(); 00685 if( __arm->GetIkSolver()->Supports(IKP_Transform6D) ) { 00686 __logger->log_debug("OR TMP", "6D suppport"); 00687 // arm supports 6D ik. Perfect! 00688 std::vector<OpenRAVE::dReal> target_angles; 00689 00690 __target.ikparam = IkParameterization(target); 00691 __target.solvable = __arm->FindIKSolution(__target.ikparam,target_angles,true); 00692 __target.manip->set_angles(target_angles); 00693 00694 } else if( __arm->GetIkSolver()->Supports(IKP_TranslationDirection5D) ) { 00695 __logger->log_debug("OR TMP", "5D suppport"); 00696 // arm has only 5 DOF. 00697 std::vector<OpenRAVE::dReal> target_angles; 00698 00699 __target.ikparam = get_5dof_ikparam(target); 00700 __target.solvable = set_target_ikparam(__target.ikparam); 00701 00702 } else { 00703 __logger->log_debug("OR TMP", "No IK suppport"); 00704 //other IK types not supported yet 00705 __target.solvable = false; 00706 } 00707 00708 return __target.solvable; 00709 } 00710 00711 /** Set target, given 3 consecutive axis rotations. 00712 * Axis rotations are given as 1 vector representing a 3x3 matrix, 00713 * (left to right, top to bottom) where each row represents 00714 * one rotation over one axis (axis-angle notation). 00715 * See public setTargetEuler methods to get a better understanding. 00716 * 00717 * Check IK solvability for target Transform. If solvable, 00718 * then set target angles to manipulator configuration __target.manip 00719 * @param rotations 3x3 matrix given as one row. 00720 * @param no_offset if true, do not include manipulator offset (default: false) 00721 * @return true if solvable, false otherwise 00722 */ 00723 bool 00724 OpenRaveRobot::set_target_euler(OpenRAVE::Vector& trans, std::vector<float>& rotations, bool no_offset) 00725 { 00726 if( rotations.size() != 9 ) { 00727 __target.type = TARGET_NONE; 00728 __target.solvable = false; 00729 00730 if(__logger) 00731 {__logger->log_error("OpenRAVE Robot", "Bad size of rotations vector. Is %i, expected 9", rotations.size());} 00732 return false; 00733 } 00734 00735 Vector r1(rotations.at(0), rotations.at(1), rotations.at(2)); 00736 Vector r2(rotations.at(3), rotations.at(4), rotations.at(5)); 00737 Vector r3(rotations.at(6), rotations.at(7), rotations.at(8)); 00738 00739 __logger->log_debug("TEST", "Rot1: %f %f %f", r1[0], r1[1], r1[2]); 00740 __logger->log_debug("TEST", "Rot2: %f %f %f", r2[0], r2[1], r2[2]); 00741 __logger->log_debug("TEST", "Rot3: %f %f %f", r3[0], r3[1], r3[2]); 00742 00743 Vector q1 = quatFromAxisAngle(r1); 00744 Vector q2 = quatFromAxisAngle(r2); 00745 Vector q3 = quatFromAxisAngle(r3); 00746 00747 Vector q12 = quatMultiply (q1, q2); 00748 Vector quat = quatMultiply (q12, q3); 00749 00750 return set_target_transform(trans, quat, no_offset); 00751 } 00752 00753 /** Get IkParameterization for a 5DOF arm given a 6D Transform. 00754 * @param trans The 6D OpenRAVE::Transform 00755 * @return the calculated 5DOF IkParameterization 00756 */ 00757 OpenRAVE::IkParameterization 00758 OpenRaveRobot::get_5dof_ikparam(OpenRAVE::Transform& trans) 00759 { 00760 /* The initial pose (that means NOT all joints=0, but the manipulator's coordinate-system 00761 matching the world-coordinate-system) of an arm in OpenRAVE has its gripper pointing to the z-axis. 00762 Imagine a tube between the grippers. That tube lies on the y-axis. 00763 For 5DOF-IK one needs another manipulator definition, that has it's z-axis lying on that 00764 'tube', i.e. it needs to be lying between the fingers. That is achieved by rotating the 00765 coordinate-system first by +-90° around z-axis, then +90° on the rotated x-axis. 00766 */ 00767 00768 // get direction vector for TranslationDirection5D 00769 /* Rotate Vector(0, +-1, 0) by target.rot. First need to figure out which of "+-" 00770 Now if the first rotation on z-axis was +90°, we need a (0,-1,0) direction vector. 00771 If it was -90°, we need (0, 1, 0). So just take the inverse of the first rotation 00772 and apply it to (1,0,0) 00773 */ 00774 EnvironmentMutex::scoped_lock lock(__robot->GetEnv()->GetMutex()); 00775 Vector dir(1,0,0); 00776 { 00777 RobotBasePtr tmp_robot = __robot; 00778 RobotBase::RobotStateSaver saver(tmp_robot); // save the state, do not modifiy currently active robot! 00779 00780 //reset robot joints 00781 std::vector<dReal> zero_joints(tmp_robot->GetActiveDOF(), (dReal)0.0); 00782 tmp_robot->SetActiveDOFValues(zero_joints); 00783 00784 // revert the rotations for the 5DOF manipulator specifition. See long comment above. 00785 // First rotate back -90° on x-axis (revert 2nd rotation) 00786 Transform cur_pos = __arm->GetEndEffectorTransform(); 00787 Vector v1 = quatFromAxisAngle(Vector(-M_PI/2, 0, 0)); 00788 v1 = quatMultiply(cur_pos.rot, v1); 00789 00790 // Now get the inverse of 1st rotation and get our (0, +-1, 0) direction 00791 v1 = quatInverse(v1); 00792 TransformMatrix mat = matrixFromQuat(v1); 00793 dir = mat.rotate(dir); 00794 } // robot state is restored 00795 00796 // now rotate direction by target 00797 TransformMatrix mat = matrixFromQuat(trans.rot); 00798 dir = mat.rotate(dir); 00799 00800 IkParameterization ikparam = __arm->GetIkParameterization(IKP_TranslationDirection5D); 00801 ikparam.SetTranslationDirection5D(RAY(trans.trans, dir)); 00802 00803 return ikparam; 00804 } 00805 00806 } // end of namespace fawkes