Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * environment.cpp - Fawkes to OpenRAVE Environment 00004 * 00005 * Created: Sun Sep 19 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 // must be first because it redefines macros from standard headers 00024 #include <Python.h> 00025 00026 #include "environment.h" 00027 #include "robot.h" 00028 #include "manipulator.h" 00029 00030 #include <logging/logger.h> 00031 #include <core/exceptions/software.h> 00032 00033 #include <openrave-core.h> 00034 #include <openrave/planningutils.h> 00035 #include <boost/thread/thread.hpp> 00036 #include <boost/bind.hpp> 00037 00038 #include <sstream> 00039 #include <cstdio> 00040 00041 using namespace OpenRAVE; 00042 namespace fawkes { 00043 #if 0 /* just to make Emacs auto-indent happy */ 00044 } 00045 #endif 00046 00047 00048 /** Sets and loads a viewer for OpenRAVE. 00049 * @param env OpenRAVE environment to be attached 00050 * @param viewername name of the viewr, usually "qtcoin" 00051 */ 00052 void 00053 SetViewer(OpenRAVE::EnvironmentBasePtr env, const std::string& viewername) 00054 { 00055 ViewerBasePtr viewer = RaveCreateViewer(env, viewername); 00056 00057 // attach it to the environment: 00058 env->AddViewer(viewer); 00059 00060 // finally you call the viewer's infinite loop (this is why you need a separate thread): 00061 viewer->main(/*showGUI=*/true); 00062 } 00063 00064 00065 /** @class OpenRaveEnvironment <plugins/openrave/environment.h> 00066 * Class handling interaction with the OpenRAVE::EnvironmentBase class. 00067 * This class loads a scene and handles robots/objects etc in it. All calculations 00068 * in OpenRAVE (IK, planning, etc) are done based on the current scene. 00069 * @author Bahram Maleki-Fard 00070 */ 00071 00072 /** Constructor. 00073 * @param logger pointer to fawkes logger 00074 */ 00075 OpenRaveEnvironment::OpenRaveEnvironment(fawkes::Logger* logger) : 00076 __logger( logger ), 00077 __viewer_enabled( 0 ) 00078 { 00079 } 00080 00081 /** Destructor. */ 00082 OpenRaveEnvironment::~OpenRaveEnvironment() 00083 { 00084 this->destroy(); 00085 } 00086 00087 /** Create and lock the environment. */ 00088 void 00089 OpenRaveEnvironment::create() 00090 { 00091 // create environment 00092 __env = RaveCreateEnvironment(); 00093 if(!__env) 00094 {throw fawkes::Exception("OpenRAVE Environment: Could not create environment. Error in OpenRAVE.");} 00095 else if (__logger) 00096 {__logger->log_debug("OpenRAVE Environment", "Environment created");} 00097 00098 // create planner 00099 __planner = RaveCreatePlanner(__env,"birrt"); 00100 if(!__planner) 00101 {throw fawkes::Exception("OpenRAVE Environment: Could not create planner. Error in OpenRAVE.");} 00102 00103 // create ikfast module 00104 __mod_ikfast = RaveCreateModule(__env,"ikfast"); 00105 __env->AddModule(__mod_ikfast,""); 00106 } 00107 00108 /** Destroy the environment. */ 00109 void 00110 OpenRaveEnvironment::destroy() 00111 { 00112 try { 00113 __env->Destroy(); 00114 if(__logger) 00115 {__logger->log_debug("OpenRAVE Environment", "Environment destroyed");} 00116 } catch(const openrave_exception& e) { 00117 if(__logger) 00118 {__logger->log_warn("OpenRAVE Environment", "Could not destroy Environment. Ex:%s", e.what());} 00119 } 00120 } 00121 00122 /** Lock the environment to prevent changes. */ 00123 void 00124 OpenRaveEnvironment::lock() 00125 { 00126 EnvironmentMutex::scoped_lock lock(__env->GetMutex()); 00127 } 00128 00129 /** Enable debugging messages of OpenRAVE. 00130 * @param level debug level to set for OpenRAVE 00131 */ 00132 void 00133 OpenRaveEnvironment::enable_debug(OpenRAVE::DebugLevel level) 00134 { 00135 RaveSetDebugLevel(level); 00136 } 00137 00138 /** Disable debugging messages of OpenRAVE. */ 00139 void 00140 OpenRaveEnvironment::disable_debug() 00141 { 00142 RaveSetDebugLevel(Level_Fatal); 00143 } 00144 00145 /** Add a robot into the scene. 00146 * @param robot RobotBasePtr of robot to add 00147 * @return 1 if succeeded, 0 if not able to add robot 00148 */ 00149 void 00150 OpenRaveEnvironment::add_robot(OpenRAVE::RobotBasePtr robot) 00151 { 00152 try{ 00153 __env->Add(robot); 00154 if(__logger) 00155 {__logger->log_debug("OpenRAVE Environment", "Robot added to environment.");} 00156 } catch(openrave_exception &e) { 00157 if(__logger) 00158 {__logger->log_debug("OpenRAVE Environment", "Could not add robot to environment. OpenRAVE error:%s", e.message().c_str());} 00159 } 00160 } 00161 00162 /** Add a robot into the scene. 00163 * @param filename path to robot's xml file 00164 * @return 1 if succeeded, 0 if not able to load file 00165 */ 00166 void 00167 OpenRaveEnvironment::add_robot(const std::string& filename) 00168 { 00169 // load the robot 00170 RobotBasePtr robot = __env->ReadRobotXMLFile(filename); 00171 00172 // if could not load robot file: Check file path, and test file itself for correct syntax and semantics 00173 // by loading it directly into openrave with "openrave robotfile.xml" 00174 if( !robot ) 00175 {throw fawkes::IllegalArgumentException("OpenRAVE Environment: Robot could not be loaded. Check xml file/path.");} 00176 else if(__logger) 00177 {__logger->log_debug("OpenRAVE Environment", "Robot loaded.");} 00178 00179 add_robot(robot); 00180 } 00181 00182 /** Add a robot into the scene. 00183 * @param robot pointer to OpenRaveRobot object of robot to add 00184 * @return 1 if succeeded, 0 if not able to add robot 00185 */ 00186 void 00187 OpenRaveEnvironment::add_robot(OpenRaveRobot* robot) 00188 { 00189 add_robot(robot->get_robot_ptr()); 00190 } 00191 00192 00193 /** Get EnvironmentBasePtr. 00194 * @return EnvironmentBasePtr in use 00195 */ 00196 OpenRAVE::EnvironmentBasePtr 00197 OpenRaveEnvironment::get_env_ptr() const 00198 { 00199 return __env; 00200 } 00201 00202 /** Starts the qt viewer in a separate thread. 00203 * Use this mainly for debugging purposes, as it uses 00204 * a lot of CPU/GPU resources. 00205 */ 00206 void 00207 OpenRaveEnvironment::start_viewer() 00208 { 00209 try { 00210 boost::thread thviewer(boost::bind(SetViewer,__env,"qtcoin")); 00211 } catch( const openrave_exception &e) { 00212 if(__logger) 00213 {__logger->log_error("OpenRAVE Environment", "Could not load viewr. Ex:%s", e.what());} 00214 throw; 00215 } 00216 00217 __viewer_enabled = true; 00218 } 00219 00220 /** Autogenerate IKfast IK solver for robot. 00221 * @param robot pointer to OpenRaveRobot object 00222 * @param iktype IK type of solver (default: Transform6D; use TranslationDirection5D for 5DOF arms) 00223 */ 00224 void 00225 OpenRaveEnvironment::load_IK_solver(OpenRaveRobot* robot, OpenRAVE::IkParameterizationType iktype) 00226 { 00227 RobotBasePtr robotBase = robot->get_robot_ptr(); 00228 00229 std::stringstream ssin,ssout; 00230 ssin << "LoadIKFastSolver " << robotBase->GetName() << " " << (int)iktype; 00231 // if necessary, add free inc for degrees of freedom 00232 //ssin << " " << 0.04f; 00233 if( !__mod_ikfast->SendCommand(ssout,ssin) ) 00234 {throw fawkes::Exception("OpenRAVE Environment: Could not load ik solver");} 00235 } 00236 00237 /** Plan collision-free path for current and target manipulator 00238 * configuration of a OpenRaveRobot robot. 00239 * @param robot pointer to OpenRaveRobot object of robot to use 00240 * @param sampling sampling time between each trajectory point (in seconds) 00241 */ 00242 void 00243 OpenRaveEnvironment::run_planner(OpenRaveRobot* robot, float sampling) 00244 { 00245 bool success; 00246 EnvironmentMutex::scoped_lock lock(__env->GetMutex()); // lock environment 00247 00248 // init planner. This is automatically done by BaseManipulation, but putting it here 00249 // helps to identify problem source if any occurs. 00250 success = __planner->InitPlan(robot->get_robot_ptr(),robot->get_planner_params()); 00251 if(!success) 00252 {throw fawkes::Exception("OpenRAVE Environment: Planner: init failed");} 00253 else if(__logger) 00254 {__logger->log_debug("OpenRAVE Environment", "Planner: initialized");} 00255 00256 // plan path with basemanipulator 00257 ModuleBasePtr basemanip = robot->get_basemanip(); 00258 target_t target = robot->get_target(); 00259 std::stringstream cmdin,cmdout; 00260 cmdin << std::setprecision(std::numeric_limits<dReal>::digits10+1); 00261 cmdout << std::setprecision(std::numeric_limits<dReal>::digits10+1); 00262 00263 if( target.type == TARGET_RELATIVE_EXT ) { 00264 Transform t = robot->get_robot_ptr()->GetActiveManipulator()->GetEndEffectorTransform(); 00265 //initial pose of arm looks at +z. Target values are referring to robot's coordinating system, 00266 //which have this direction vector if taken as extension of manipulator (rotate -90° on y-axis) 00267 Vector dir(target.y,target.z,target.x); 00268 TransformMatrix mat = matrixFromQuat(t.rot); 00269 dir = mat.rotate(dir); 00270 target.type = TARGET_RELATIVE; 00271 target.x = dir[0]; 00272 target.y = dir[1]; 00273 target.z = dir[2]; 00274 } 00275 00276 switch(target.type) { 00277 case (TARGET_JOINTS) : 00278 cmdin << "MoveActiveJoints goal"; 00279 { 00280 std::vector<dReal> v; 00281 target.manip->get_angles(v); 00282 for(size_t i = 0; i < v.size(); ++i) { 00283 cmdin << " " << v[i]; 00284 } 00285 } 00286 break; 00287 00288 case (TARGET_IKPARAM) : 00289 cmdin << "MoveToHandPosition ikparam"; 00290 cmdin << " " << target.ikparam; 00291 break; 00292 00293 case (TARGET_TRANSFORM) : 00294 cmdin << "MoveToHandPosition pose"; 00295 cmdin << " " << target.qw << " " << target.qx << " " << target.qy << " " << target.qz; 00296 cmdin << " " << target.x << " " << target.y << " " << target.z; 00297 break; 00298 00299 case (TARGET_RELATIVE) : 00300 // for now move to all relative targets in a straigt line 00301 cmdin << "MoveHandStraight direction"; 00302 cmdin << " " << target.x << " " << target.y << " " << target.z; 00303 { 00304 dReal stepsize = 0.005; 00305 dReal length = sqrt(target.x*target.x + target.y*target.y + target.z*target.z); 00306 int minsteps = (int)(length/stepsize); 00307 if( minsteps > 4 ) 00308 minsteps -= 4; 00309 00310 cmdin << " stepsize " << stepsize; 00311 cmdin << " minsteps " << minsteps; 00312 cmdin << " maxsteps " << (int)(length/stepsize); 00313 } 00314 break; 00315 00316 default : 00317 throw fawkes::Exception("OpenRAVE Environment: Planner: Invalid target type"); 00318 } 00319 00320 //add additional planner parameters 00321 if( !target.plannerparams.empty() ) { 00322 cmdin << " " << target.plannerparams; 00323 } 00324 cmdin << " execute 0"; 00325 cmdin << " outputtraj"; 00326 //if(__logger) 00327 // __logger->log_debug("OpenRAVE Environment", "Planner: basemanip cmdin:%s", cmdin.str().c_str()); 00328 00329 try { 00330 success = basemanip->SendCommand(cmdout,cmdin); 00331 } catch(openrave_exception &e) { 00332 throw fawkes::Exception("OpenRAVE Environment: Planner: basemanip command failed. Ex%s", e.what()); 00333 } 00334 if(!success) 00335 {throw fawkes::Exception("OpenRAVE Environment: Planner: planning failed");} 00336 else if(__logger) 00337 {__logger->log_debug("OpenRAVE Environment", "Planner: path planned");} 00338 00339 // read returned trajectory 00340 TrajectoryBasePtr traj = RaveCreateTrajectory(__env, ""); 00341 traj->Init(robot->get_robot_ptr()->GetActiveConfigurationSpecification()); 00342 if( !traj->deserialize(cmdout) ) { 00343 {throw fawkes::Exception("OpenRAVE Environment: Planner: Cannot read trajectory data.");} 00344 } 00345 00346 // sampling trajectory and setting robots trajectory 00347 std::vector< std::vector<dReal> >* trajRobot = robot->get_trajectory(); 00348 trajRobot->clear(); 00349 for(dReal time = 0; time <= traj->GetDuration(); time += (dReal)sampling) { 00350 std::vector<dReal> point; 00351 traj->Sample(point,time); 00352 trajRobot->push_back(point); 00353 } 00354 00355 // viewer options 00356 if( __viewer_enabled ) { 00357 00358 // display trajectory in viewer 00359 __graph_handle.clear(); // remove all GraphHandlerPtr and currently drawn plots 00360 { 00361 RobotBasePtr tmp_robot = robot->get_robot_ptr(); 00362 RobotBase::RobotStateSaver saver(tmp_robot); // save the state, do not modifiy currently active robot! 00363 for(std::vector< std::vector<dReal> >::iterator it = trajRobot->begin(); it!=trajRobot->end(); ++it) { 00364 tmp_robot->SetActiveDOFValues((*it)); 00365 const OpenRAVE::Vector &trans = tmp_robot->GetActiveManipulator()->GetEndEffectorTransform().trans; 00366 float transa[4] = { trans.x, trans.y, trans.z, trans.w }; 00367 __graph_handle.push_back(__env->plot3(transa, 1, 0, 2.f, Vector(1.f, 0.f, 0.f, 1.f))); 00368 } 00369 } // robot state is restored 00370 00371 // display motion in viewer 00372 if( robot->display_planned_movements()) { 00373 robot->get_robot_ptr()->GetController()->SetPath(traj); 00374 } 00375 } 00376 00377 } 00378 00379 00380 /** Run graspplanning script for a given target. 00381 * Script loads grasping databse, checks if target is graspable for various grasps 00382 * and on success returns a string containing trajectory data. 00383 * Currently the grasping databse can only be accessed via python, so we use a short 00384 * python script (shortened and modified from officiel OpenRAVE graspplanning.py) to do the work. 00385 * @param target_name name of targeted object (KinBody) 00386 * @param robot pointer to OpenRaveRobot object of robot to use 00387 * @param sampling sampling time between each trajectory point (in seconds) 00388 */ 00389 void 00390 OpenRaveEnvironment::run_graspplanning(const std::string& target_name, OpenRaveRobot* robot, float sampling) 00391 { 00392 std::string filename = SRCDIR"/python/graspplanning.py"; 00393 std::string funcname = "runGrasp"; 00394 00395 TrajectoryBasePtr traj = RaveCreateTrajectory(__env, ""); 00396 traj->Init(robot->get_robot_ptr()->GetActiveConfigurationSpecification()); 00397 00398 FILE* py_file = fopen(filename.c_str(), "r"); 00399 if (py_file == NULL) 00400 {throw fawkes::Exception("OpenRAVE Environment: Graspplanning: opening python file failed");} 00401 00402 Py_Initialize(); 00403 00404 // Need to aquire global interpreter lock (GIL), create new sub-interpreter to run code in there 00405 PyGILState_STATE gil_state = PyGILState_Ensure(); // aquire python GIL 00406 PyThreadState* cur_state = PyThreadState_Get(); // get current ThreadState; need this to switch back to later 00407 PyThreadState* int_state = Py_NewInterpreter(); // create new sub-interpreter 00408 PyThreadState_Swap(int_state); // set active ThreadState; maybe not needed after calling NewInterpreter() ? 00409 // Now we can safely run our python code 00410 00411 // using python C API 00412 PyObject* py_main = PyImport_AddModule("__main__"); // borrowed reference 00413 if( !py_main ) { 00414 // __main__ should always exist 00415 fclose(py_file); 00416 Py_EndInterpreter(int_state); 00417 PyThreadState_Swap(cur_state); 00418 PyGILState_Release(gil_state); // release GIL 00419 Py_Finalize(); 00420 throw fawkes::Exception("OpenRAVE Environment: Graspplanning: Python reference '__main__' does not exist."); 00421 } 00422 PyObject* py_dict = PyModule_GetDict(py_main); // borrowed reference 00423 if( !py_dict ) { 00424 // __main__ should have a dictionary 00425 fclose(py_file); 00426 Py_Finalize(); 00427 throw fawkes::Exception("OpenRAVE Environment: Graspplanning: Python reference '__main__' does not have a dictionary."); 00428 } 00429 00430 // load file 00431 int py_module = PyRun_SimpleFile(py_file, filename.c_str()); 00432 fclose(py_file); 00433 if (!py_module) { 00434 // load function from global dictionary 00435 PyObject* py_func = PyDict_GetItemString(py_dict, funcname.c_str()); 00436 if (py_func && PyCallable_Check(py_func)) { 00437 // create tuple for args to be passed to py_func 00438 PyObject* py_args = PyTuple_New(3); 00439 // fill tuple with values. We're not checking for conversion errors here atm, can be added! 00440 PyObject* py_value_env_id = PyInt_FromLong(RaveGetEnvironmentId(__env)); 00441 PyObject* py_value_robot_name = PyString_FromString(robot->get_robot_ptr()->GetName().c_str()); 00442 PyObject* py_value_target_name = PyString_FromString(target_name.c_str()); 00443 PyTuple_SetItem(py_args, 0, py_value_env_id); //py_value reference stolen here! no need to DECREF 00444 PyTuple_SetItem(py_args, 1, py_value_robot_name); //py_value reference stolen here! no need to DECREF 00445 PyTuple_SetItem(py_args, 2, py_value_target_name); //py_value reference stolen here! no need to DECREF 00446 // call function, get return value 00447 PyObject* py_value = PyObject_CallObject(py_func, py_args); 00448 Py_DECREF(py_args); 00449 // check return value 00450 if (py_value != NULL) { 00451 if (!PyString_Check(py_value)) { 00452 Py_DECREF(py_value); 00453 Py_DECREF(py_func); 00454 Py_Finalize(); 00455 throw fawkes::Exception("OpenRAVE Environment: Graspplanning: No grasping path found."); 00456 } 00457 std::stringstream resval; 00458 resval << std::setprecision(std::numeric_limits<dReal>::digits10+1); 00459 resval << PyString_AsString(py_value); 00460 if (!traj->deserialize(resval) ) { 00461 Py_DECREF(py_value); 00462 Py_DECREF(py_func); 00463 Py_Finalize(); 00464 throw fawkes::Exception("OpenRAVE Environment: Graspplanning: Reading trajectory data failed."); 00465 } 00466 Py_DECREF(py_value); 00467 } else { // if calling function failed 00468 Py_DECREF(py_func); 00469 PyErr_Print(); 00470 Py_Finalize(); 00471 throw fawkes::Exception("OpenRAVE Environment: Graspplanning: Calling function failed."); 00472 } 00473 } else { // if loading func failed 00474 if (PyErr_Occurred()) 00475 PyErr_Print(); 00476 Py_XDECREF(py_func); 00477 Py_Finalize(); 00478 throw fawkes::Exception("OpenRAVE Environment: Graspplanning: Loading function failed."); 00479 } 00480 Py_XDECREF(py_func); 00481 } else { // if loading module failed 00482 PyErr_Print(); 00483 Py_Finalize(); 00484 throw fawkes::Exception("OpenRAVE Environment: Graspplanning: Loading python file failed."); 00485 } 00486 00487 Py_EndInterpreter(int_state); // close sub-interpreter 00488 PyThreadState_Swap(cur_state); // re-set active state to previous one 00489 PyGILState_Release(gil_state); // release GIL 00490 00491 Py_Finalize(); // should be careful with that, as it closes global interpreter; Other threads running python may fail 00492 00493 if(__logger) 00494 {__logger->log_debug("OpenRAVE Environment", "Graspplanning: path planned");} 00495 00496 // re-timing the trajectory with 00497 planningutils::RetimeActiveDOFTrajectory(traj, robot->get_robot_ptr()); 00498 00499 // sampling trajectory and setting robots trajectory 00500 std::vector< std::vector<dReal> >* trajRobot = robot->get_trajectory(); 00501 trajRobot->clear(); 00502 for(dReal time = 0; time <= traj->GetDuration(); time += (dReal)sampling) { 00503 std::vector<dReal> point; 00504 traj->Sample(point,time); 00505 trajRobot->push_back(point); 00506 } 00507 00508 // viewer options 00509 if( __viewer_enabled ) { 00510 00511 // display trajectory in viewer 00512 __graph_handle.clear(); // remove all GraphHandlerPtr and currently drawn plots 00513 { 00514 RobotBasePtr tmp_robot = robot->get_robot_ptr(); 00515 RobotBase::RobotStateSaver saver(tmp_robot); // save the state, do not modifiy currently active robot! 00516 for(std::vector< std::vector<dReal> >::iterator it = trajRobot->begin(); it!=trajRobot->end(); ++it) { 00517 tmp_robot->SetActiveDOFValues((*it)); 00518 const OpenRAVE::Vector &trans = tmp_robot->GetActiveManipulator()->GetEndEffectorTransform().trans; 00519 float transa[4] = { trans.x, trans.y, trans.z, trans.w }; 00520 __graph_handle.push_back(__env->plot3(transa, 1, 0, 2.f, Vector(1.f, 0.f, 0.f, 1.f))); 00521 } 00522 } // robot state is restored 00523 00524 // display motion in viewer 00525 if( robot->display_planned_movements()) { 00526 robot->get_robot_ptr()->GetController()->SetPath(traj); 00527 } 00528 } 00529 } 00530 00531 00532 /** Add an object to the environment. 00533 * @param name name that should be given to that object 00534 * @param filename path to xml file of that object (KinBody) 00535 * @return true if successful 00536 */ 00537 bool 00538 OpenRaveEnvironment::add_object(const std::string& name, const std::string& filename) 00539 { 00540 try { 00541 EnvironmentMutex::scoped_lock lock(__env->GetMutex()); 00542 KinBodyPtr kb = __env->ReadKinBodyXMLFile(filename); 00543 kb->SetName(name); 00544 __env->Add(kb); 00545 } catch(const OpenRAVE::openrave_exception &e) { 00546 if(__logger) 00547 __logger->log_warn("OpenRAVE Environment", "Could not add Object '%s'. Ex:%s", name.c_str(), e.what()); 00548 return false; 00549 } 00550 00551 return true; 00552 } 00553 00554 /** Remove object from environment. 00555 * @param name name of the object 00556 * @return true if successful 00557 */ 00558 bool 00559 OpenRaveEnvironment::delete_object(const std::string& name) 00560 { 00561 try { 00562 EnvironmentMutex::scoped_lock lock(__env->GetMutex()); 00563 KinBodyPtr kb = __env->GetKinBody(name); 00564 __env->Remove(kb); 00565 } catch(const OpenRAVE::openrave_exception &e) { 00566 if(__logger) 00567 __logger->log_warn("OpenRAVE Environment", "Could not delete Object '%s'. Ex:%s", name.c_str(), e.what()); 00568 return false; 00569 } 00570 00571 return true; 00572 } 00573 00574 /** Rename object. 00575 * @param name current name of the object 00576 * @param new_name new name of the object 00577 * @return true if successful 00578 */ 00579 bool 00580 OpenRaveEnvironment::rename_object(const std::string& name, const std::string& new_name) 00581 { 00582 try { 00583 EnvironmentMutex::scoped_lock lock(__env->GetMutex()); 00584 KinBodyPtr kb = __env->GetKinBody(name); 00585 kb->SetName(new_name); 00586 } catch(const OpenRAVE::openrave_exception &e) { 00587 if(__logger) 00588 __logger->log_warn("OpenRAVE Environment", "Could not rename Object '%s' to '%s'. Ex:%s", name.c_str(), new_name.c_str(), e.what()); 00589 return false; 00590 } 00591 00592 return true; 00593 } 00594 00595 /** Move object in the environment. 00596 * Distances are given in meters 00597 * @param name name of the object 00598 * @param trans_x transition along x-axis 00599 * @param trans_y transition along y-axis 00600 * @param trans_z transition along z-axis 00601 * @param robot if given, move relatively to robot (in most simple cases robot is at position (0,0,0) anyway, so this has no effect) 00602 * @return true if successful 00603 */ 00604 bool 00605 OpenRaveEnvironment::move_object(const std::string& name, float trans_x, float trans_y, float trans_z, OpenRaveRobot* robot) 00606 { 00607 try { 00608 EnvironmentMutex::scoped_lock lock(__env->GetMutex()); 00609 KinBodyPtr kb = __env->GetKinBody(name); 00610 00611 Transform transform = kb->GetTransform(); 00612 transform.trans = Vector(trans_x, trans_y, trans_z); 00613 00614 if( robot ) { 00615 Transform robotTrans = robot->get_robot_ptr()->GetTransform(); 00616 transform.trans += robotTrans.trans; 00617 } 00618 00619 kb->SetTransform(transform); 00620 } catch(const OpenRAVE::openrave_exception &e) { 00621 if(__logger) 00622 __logger->log_warn("OpenRAVE Environment", "Could not move Object '%s'. Ex:%s", name.c_str(), e.what()); 00623 return false; 00624 } 00625 00626 return true; 00627 } 00628 00629 /** Rotate object by a quaternion. 00630 * @param name name of the object 00631 * @param quat_x x value of quaternion 00632 * @param quat_y y value of quaternion 00633 * @param quat_z z value of quaternion 00634 * @param quat_w w value of quaternion 00635 * @return true if successful 00636 */ 00637 bool 00638 OpenRaveEnvironment::rotate_object(const std::string& name, float quat_x, float quat_y, float quat_z, float quat_w) 00639 { 00640 try { 00641 EnvironmentMutex::scoped_lock lock(__env->GetMutex()); 00642 KinBodyPtr kb = __env->GetKinBody(name); 00643 00644 Vector quat(quat_w, quat_x, quat_y, quat_z); 00645 00646 Transform transform = kb->GetTransform(); 00647 transform.rot = quat; 00648 00649 kb->SetTransform(transform); 00650 } catch(const OpenRAVE::openrave_exception &e) { 00651 if(__logger) 00652 __logger->log_warn("OpenRAVE Environment", "Could not rotate Object '%s'. Ex:%s", name.c_str(), e.what()); 00653 return false; 00654 } 00655 00656 return true; 00657 } 00658 00659 /** Rotate object along its axis. 00660 * Rotation angles should be given in radians. 00661 * @param name name of the object 00662 * @param rot_x 1st rotation, along x-axis 00663 * @param rot_y 2nd rotation, along y-axis 00664 * @param rot_z 3rd rotation, along z-axis 00665 * @return true if successful 00666 */ 00667 bool 00668 OpenRaveEnvironment::rotate_object(const std::string& name, float rot_x, float rot_y, float rot_z) 00669 { 00670 Vector q1 = quatFromAxisAngle(Vector(rot_x, 0.f, 0.f)); 00671 Vector q2 = quatFromAxisAngle(Vector(0.f, rot_y, 0.f)); 00672 Vector q3 = quatFromAxisAngle(Vector(0.f, 0.f, rot_z)); 00673 00674 Vector q12 = quatMultiply (q1, q2); 00675 Vector quat = quatMultiply (q12, q3); 00676 00677 return rotate_object(name, quat.x, quat.y, quat.z, quat.w); 00678 } 00679 00680 } // end of namespace fawkes