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