Fawkes API  Fawkes Development Version
environment.cpp
1 
2 /***************************************************************************
3  * environment.cpp - Fawkes to OpenRAVE Environment
4  *
5  * Created: Sun Sep 19 14:50:34 2010
6  * Copyright 2010 Bahram Maleki-Fard, AllemaniACs RoboCup Team
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 // must be first because it redefines macros from standard headers
24 #include <Python.h>
25 
26 #include "environment.h"
27 #include "robot.h"
28 #include "manipulator.h"
29 
30 #include <logging/logger.h>
31 #include <core/exceptions/software.h>
32 
33 #include <openrave-core.h>
34 #include <openrave/planningutils.h>
35 #include <boost/thread/thread.hpp>
36 #include <boost/bind.hpp>
37 
38 #include <sstream>
39 #include <cstdio>
40 
41 using namespace OpenRAVE;
42 namespace fawkes {
43 #if 0 /* just to make Emacs auto-indent happy */
44 }
45 #endif
46 
47 
48 /** Sets and loads a viewer for OpenRAVE.
49  * @param env OpenRAVE environment to be attached
50  * @param viewername name of the viewr, usually "qtcoin"
51  * @param running pointer to a local variable, which will be set to "true"
52  * as long as the viewer thread runs, and "false" when the GUI closes.
53  */
54 void
55 run_viewer(OpenRAVE::EnvironmentBasePtr env, const std::string& viewername, bool* running)
56 {
57  ViewerBasePtr viewer = RaveCreateViewer(env, viewername);
58  env->Add(viewer);
59  //call the viewer's infinite loop (this is why you need a separate thread):
60  *running = true;
61  viewer->main(/*showGUI=*/true);
62  *running = false;
63  env->Remove(viewer);
64 }
65 
66 
67 /** @class OpenRaveEnvironment <plugins/openrave/environment.h>
68 * Class handling interaction with the OpenRAVE::EnvironmentBase class.
69 * This class loads a scene and handles robots/objects etc in it. All calculations
70 * in OpenRAVE (IK, planning, etc) are done based on the current scene.
71 * @author Bahram Maleki-Fard
72 */
73 
74 /** Constructor.
75  * @param logger pointer to fawkes logger
76  */
77 OpenRaveEnvironment::OpenRaveEnvironment(fawkes::Logger* logger) :
78  __logger( logger ),
79  __viewer_thread( 0 ),
80  __viewer_running( 0 )
81 {
82  set_name("");
83 }
84 
85 /** Copy constructor.
86  * This also clones the environment in OpenRAVE, including all bodies!
87  * BiRRT planner and IKFast module are also created.
88  * @param src The OpenRaveEnvironment to clone
89  */
91  : __logger( src.__logger ),
92  __viewer_thread( 0 ),
93  __viewer_running( 0 )
94 {
95  __env = src.__env->CloneSelf(OpenRAVE::Clone_Bodies);
96 
97  // update name string
98  set_name( src.__name.c_str() );
99 
100  // create planner
101  __planner = RaveCreatePlanner(__env,"birrt");
102  if(!__planner)
103  throw fawkes::Exception("%s: Could not create planner. Error in OpenRAVE.", name());
104 
105  // create ikfast module
106  __mod_ikfast = RaveCreateModule(__env,"ikfast");
107  __env->AddModule(__mod_ikfast,"");
108 
109  if (__logger)
110  __logger->log_debug(name(), "Environment cloned from %s", src.__name.c_str());
111 }
112 
113 /** Destructor. */
115 {
116  this->destroy();
117 }
118 
119 /** Create and lock the environment. */
120 void
122 {
123  // create environment
124  __env = RaveCreateEnvironment();
125  if(!__env) {
126  throw fawkes::Exception("%s: Could not create environment. Error in OpenRAVE.", name());
127  } else {
128  // update name_string
129  set_name(__name.c_str());
130  if (__logger)
131  __logger->log_debug(name(), "Environment created");
132  }
133 
134  EnvironmentMutex::scoped_lock( __env->GetMutex());
135 
136  // create planner
137  __planner = RaveCreatePlanner(__env,"birrt");
138  if(!__planner)
139  throw fawkes::Exception("%s: Could not create planner. Error in OpenRAVE.", name());
140 
141  // create ikfast module
142  __mod_ikfast = RaveCreateModule(__env,"ikfast");
143  __env->AddModule(__mod_ikfast,"");
144 }
145 
146 /** Destroy the environment. */
147 void
149 {
150  if( __viewer_thread ) {
151  __viewer_thread->detach();
152  __viewer_thread->join();
153  delete __viewer_thread;
154  __viewer_thread=NULL;
155  }
156 
157  try {
158  __env->Destroy();
159  if(__logger)
160  __logger->log_debug(name(), "Environment destroyed");
161  } catch(const openrave_exception& e) {
162  if(__logger)
163  __logger->log_warn(name(), "Could not destroy Environment. Ex:%s", e.what());
164  }
165 }
166 
167 /** Get the name string to use in logging messages etc. */
168 const char*
169 OpenRaveEnvironment::name() const
170 {
171  return __name_str.c_str();
172 }
173 
174 /** Set name of environment.
175  * Nothing important, but helpful for debugging etc.
176  * @param name The name of the environment. Can be an empty string.
177  */
178 void
180 {
181  std::stringstream n;
182  n << "OpenRaveEnvironment" << "[";
183  if( __env )
184  n << RaveGetEnvironmentId(__env) << ":";
185  n << name << "]";
186  __name_str = n.str();
187 
188  if (__logger)
189  __logger->log_debug(__name_str.c_str(), "Set environment name (previously '%s')", __name.c_str());
190  __name = name;
191 }
192 
193 /** Enable debugging messages of OpenRAVE.
194  * @param level debug level to set for OpenRAVE
195  */
196 void
197 OpenRaveEnvironment::enable_debug(OpenRAVE::DebugLevel level)
198 {
199  RaveSetDebugLevel(level);
200 }
201 
202 /** Disable debugging messages of OpenRAVE. */
203 void
205 {
206  RaveSetDebugLevel(Level_Fatal);
207 }
208 
209 /** Add a robot into the scene.
210  * @param robot RobotBasePtr of robot to add
211  * @return 1 if succeeded, 0 if not able to add robot
212  */
213 void
214 OpenRaveEnvironment::add_robot(OpenRAVE::RobotBasePtr robot)
215 {
216  try{
217  EnvironmentMutex::scoped_lock( __env->GetMutex());
218  __env->Add(robot);
219  if(__logger)
220  __logger->log_debug(name(), "Robot '%s' added to environment.", robot->GetName().c_str());
221  } catch(openrave_exception &e) {
222  if(__logger)
223  __logger->log_debug(name(), "Could not add robot '%s' to environment. OpenRAVE error:%s", robot->GetName().c_str(), e.message().c_str());
224  }
225 }
226 
227 /** Add a robot into the scene.
228  * @param filename path to robot's xml file
229  * @return 1 if succeeded, 0 if not able to load file
230  */
231 void
232 OpenRaveEnvironment::add_robot(const std::string& filename)
233 {
234  RobotBasePtr robot;
235  {
236  // load the robot
237  EnvironmentMutex::scoped_lock( __env->GetMutex());
238  robot = __env->ReadRobotXMLFile(filename);
239  }
240 
241  // if could not load robot file: Check file path, and test file itself for correct syntax and semantics
242  // by loading it directly into openrave with "openrave robotfile.xml"
243  if( !robot )
244  throw fawkes::IllegalArgumentException("%s: Robot '%s' could not be loaded. Check xml file/path.", name(), filename.c_str());
245  else if(__logger)
246  __logger->log_debug(name(), "Robot '%s' loaded.", robot->GetName().c_str());
247 
248  add_robot(robot);
249 }
250 
251 /** Add a robot into the scene.
252  * @param robot pointer to OpenRaveRobot object of robot to add
253  * @return 1 if succeeded, 0 if not able to add robot
254  */
255 void
257 {
258  add_robot(robot->get_robot_ptr());
259 }
260 
261 
262 /** Get EnvironmentBasePtr.
263  * @return EnvironmentBasePtr in use
264  */
265 OpenRAVE::EnvironmentBasePtr
267 {
268  return __env;
269 }
270 
271 /** Starts the qt viewer in a separate thread.
272  * Use this mainly for debugging purposes, as it uses
273  * a lot of CPU/GPU resources.
274  */
275 void
277 {
278  if( __viewer_running )
279  return;
280 
281  if( __viewer_thread ) {
282  __viewer_thread->join();
283  delete __viewer_thread;
284  __viewer_thread = NULL;
285  }
286 
287  try {
288  // set this variable to true here already. Otherwise we would have to wait for the upcoming
289  // boost thread to start, create viewer and add viewer to environment to get this variable set
290  // to "true". Another call to "start_viewer()" would get stuck then, waiting for "join()"!
291  __viewer_running = true;
292  __viewer_thread = new boost::thread(boost::bind(run_viewer, __env, "qtcoin", &__viewer_running));
293  } catch( const openrave_exception &e) {
294  __viewer_running = false;
295  if(__logger)
296  __logger->log_error(name(), "Could not load viewr. Ex:%s", e.what());
297  throw;
298  }
299 }
300 
301 
302 /** Autogenerate IKfast IK solver for robot.
303  * @param robot pointer to OpenRaveRobot object
304  * @param iktype IK type of solver (default: Transform6D; use TranslationDirection5D for 5DOF arms)
305  */
306 void
307 OpenRaveEnvironment::load_IK_solver(OpenRaveRobotPtr& robot, OpenRAVE::IkParameterizationType iktype)
308 {
309  EnvironmentMutex::scoped_lock( __env->GetMutex());
310 
311  RobotBasePtr robotBase = robot->get_robot_ptr();
312 
313  std::stringstream ssin,ssout;
314  ssin << "LoadIKFastSolver " << robotBase->GetName() << " " << (int)iktype;
315  // if necessary, add free inc for degrees of freedom
316  //ssin << " " << 0.04f;
317  if( !__mod_ikfast->SendCommand(ssout,ssin) )
318  throw fawkes::Exception("%s: Could not load ik solver", name());
319 }
320 
321 /** Plan collision-free path for current and target manipulator
322  * configuration of a OpenRaveRobot robot.
323  * @param robot pointer to OpenRaveRobot object of robot to use
324  * @param sampling sampling time between each trajectory point (in seconds)
325  */
326 void
328 {
329  bool success;
330  EnvironmentMutex::scoped_lock lock(__env->GetMutex()); // lock environment
331 
332  robot->get_planner_params(); // also updates internal __manip
333 
334  /*
335  // init planner. This is automatically done by BaseManipulation, but putting it here
336  // helps to identify problem source if any occurs.
337  success = __planner->InitPlan(robot->get_robot_ptr(),robot->get_planner_params());
338  if(!success)
339  {throw fawkes::Exception("%s: Planner: init failed", name());}
340  else if(__logger)
341  {__logger->log_debug(name(), "Planner: initialized");}
342  */
343  // plan path with basemanipulator
344  ModuleBasePtr basemanip = robot->get_basemanip();
345  target_t target = robot->get_target();
346  std::stringstream cmdin,cmdout;
347  cmdin << std::setprecision(std::numeric_limits<dReal>::digits10+1);
348  cmdout << std::setprecision(std::numeric_limits<dReal>::digits10+1);
349 
350  if( target.type == TARGET_RELATIVE_EXT ) {
351  Transform t = robot->get_robot_ptr()->GetActiveManipulator()->GetEndEffectorTransform();
352  //initial pose of arm looks at +z. Target values are referring to robot's coordinating system,
353  //which have this direction vector if taken as extension of manipulator (rotate -90° on y-axis)
354  Vector dir(target.y,target.z,target.x);
355  TransformMatrix mat = matrixFromQuat(t.rot);
356  dir = mat.rotate(dir);
357  target.type = TARGET_RELATIVE;
358  target.x = dir[0];
359  target.y = dir[1];
360  target.z = dir[2];
361  }
362 
363  switch(target.type) {
364  case (TARGET_RAW) :
365  cmdin << target.raw_cmd;
366  break;
367 
368  case (TARGET_JOINTS) :
369  cmdin << "MoveActiveJoints goal";
370  {
371  std::vector<dReal> v;
372  target.manip->get_angles(v);
373  for(size_t i = 0; i < v.size(); ++i) {
374  cmdin << " " << v[i];
375  }
376  }
377  break;
378 
379  case (TARGET_IKPARAM) :
380  cmdin << "MoveToHandPosition ikparam";
381  cmdin << " " << target.ikparam;
382  break;
383 
384  case (TARGET_TRANSFORM) :
385  cmdin << "MoveToHandPosition pose";
386  cmdin << " " << target.qw << " " << target.qx << " " << target.qy << " " << target.qz;
387  cmdin << " " << target.x << " " << target.y << " " << target.z;
388  break;
389 
390  case (TARGET_RELATIVE) :
391  // for now move to all relative targets in a straigt line
392  cmdin << "MoveHandStraight direction";
393  cmdin << " " << target.x << " " << target.y << " " << target.z;
394  {
395  dReal stepsize = 0.005;
396  dReal length = sqrt(target.x*target.x + target.y*target.y + target.z*target.z);
397  int minsteps = (int)(length/stepsize);
398  if( minsteps > 4 )
399  minsteps -= 4;
400 
401  cmdin << " stepsize " << stepsize;
402  cmdin << " minsteps " << minsteps;
403  cmdin << " maxsteps " << (int)(length/stepsize);
404  }
405  break;
406 
407  default :
408  throw fawkes::Exception("%s: Planner: Invalid target type", name());
409  }
410 
411  //add additional planner parameters
412  if( !target.plannerparams.empty() ) {
413  cmdin << " " << target.plannerparams;
414  }
415  cmdin << " execute 0";
416  cmdin << " outputtraj";
417  if(__logger)
418  __logger->log_debug(name(), "Planner: basemanip cmdin:%s", cmdin.str().c_str());
419 
420  try {
421  success = basemanip->SendCommand(cmdout,cmdin);
422  } catch(openrave_exception &e) {
423  throw fawkes::Exception("%s: Planner: basemanip command failed. Ex%s", name(), e.what());
424  }
425  if(!success)
426  throw fawkes::Exception("%s: Planner: planning failed", name());
427  else if(__logger)
428  __logger->log_debug(name(), "Planner: path planned");
429 
430  if(__logger)
431  __logger->log_debug(name(), "Planner: planned. cmdout:%s", cmdout.str().c_str());
432 
433  // read returned trajectory
434  TrajectoryBasePtr traj = RaveCreateTrajectory(__env, "");
435  traj->Init(robot->get_robot_ptr()->GetActiveConfigurationSpecification());
436  if( !traj->deserialize(cmdout) )
437  throw fawkes::Exception("%s: Planner: Cannot read trajectory data.", name());
438 
439  // sampling trajectory and setting robots trajectory
440  std::vector< std::vector<dReal> >* trajRobot = robot->get_trajectory();
441  trajRobot->clear();
442  for(dReal time = 0; time <= traj->GetDuration(); time += (dReal)sampling) {
443  std::vector<dReal> point;
444  traj->Sample(point,time);
445  trajRobot->push_back(point);
446  }
447 
448  // viewer options
449  if( __viewer_running ) {
450 
451  // display trajectory in viewer
452  __graph_handle.clear(); // remove all GraphHandlerPtr and currently drawn plots
453  {
454  RobotBasePtr tmp_robot = robot->get_robot_ptr();
455  RobotBase::RobotStateSaver saver(tmp_robot); // save the state, do not modifiy currently active robot!
456  for(std::vector< std::vector<dReal> >::iterator it = trajRobot->begin(); it!=trajRobot->end(); ++it) {
457  tmp_robot->SetActiveDOFValues((*it));
458  const OpenRAVE::Vector &trans = tmp_robot->GetActiveManipulator()->GetEndEffectorTransform().trans;
459  float transa[4] = { (float)trans.x, (float)trans.y, (float)trans.z, (float)trans.w };
460  __graph_handle.push_back(__env->plot3(transa, 1, 0, 2.f, Vector(1.f, 0.f, 0.f, 1.f)));
461  }
462  } // robot state is restored
463 
464  // display motion in viewer
465  if( robot->display_planned_movements()) {
466  robot->get_robot_ptr()->GetController()->SetPath(traj);
467  }
468  }
469 
470 }
471 
472 
473 /** Run graspplanning script for a given target.
474  * Script loads grasping databse, checks if target is graspable for various grasps
475  * and on success returns a string containing trajectory data.
476  * Currently the grasping databse can only be accessed via python, so we use a short
477  * python script (shortened and modified from officiel OpenRAVE graspplanning.py) to do the work.
478  * @param target_name name of targeted object (KinBody)
479  * @param robot pointer to OpenRaveRobot object of robot to use
480  * @param sampling sampling time between each trajectory point (in seconds)
481  */
482 void
483 OpenRaveEnvironment::run_graspplanning(const std::string& target_name, OpenRaveRobotPtr& robot, float sampling)
484 {
485  std::string filename = SRCDIR"/python/graspplanning.py";
486  std::string funcname = "runGrasp";
487 
488  TrajectoryBasePtr traj = RaveCreateTrajectory(__env, "");
489  traj->Init(robot->get_robot_ptr()->GetActiveConfigurationSpecification());
490 
491  FILE* py_file = fopen(filename.c_str(), "r");
492  if (py_file == NULL)
493  throw fawkes::Exception("%s: Graspplanning: opening python file failed", name());
494 
495  Py_Initialize();
496 
497  // Need to aquire global interpreter lock (GIL), create new sub-interpreter to run code in there
498  PyGILState_STATE gil_state = PyGILState_Ensure(); // aquire python GIL
499  PyThreadState* cur_state = PyThreadState_Get(); // get current ThreadState; need this to switch back to later
500  PyThreadState* int_state = Py_NewInterpreter(); // create new sub-interpreter
501  PyThreadState_Swap(int_state); // set active ThreadState; maybe not needed after calling NewInterpreter() ?
502  // Now we can safely run our python code
503 
504  // using python C API
505  PyObject* py_main = PyImport_AddModule("__main__"); // borrowed reference
506  if( !py_main ) {
507  // __main__ should always exist
508  fclose(py_file);
509  Py_EndInterpreter(int_state);
510  PyThreadState_Swap(cur_state);
511  PyGILState_Release(gil_state); // release GIL
512  Py_Finalize();
513  throw fawkes::Exception("%s: Graspplanning: Python reference '__main__' does not exist.", name());
514  }
515  PyObject* py_dict = PyModule_GetDict(py_main); // borrowed reference
516  if( !py_dict ) {
517  // __main__ should have a dictionary
518  fclose(py_file);
519  Py_Finalize();
520  throw fawkes::Exception("%s: Graspplanning: Python reference '__main__' does not have a dictionary.", name());
521  }
522 
523  // load file
524  int py_module = PyRun_SimpleFile(py_file, filename.c_str());
525  fclose(py_file);
526  if (!py_module) {
527  // load function from global dictionary
528  PyObject* py_func = PyDict_GetItemString(py_dict, funcname.c_str());
529  if (py_func && PyCallable_Check(py_func)) {
530  // create tuple for args to be passed to py_func
531  PyObject* py_args = PyTuple_New(3);
532  // fill tuple with values. We're not checking for conversion errors here atm, can be added!
533  PyObject* py_value_env_id = PyInt_FromLong(RaveGetEnvironmentId(__env));
534  PyObject* py_value_robot_name = PyString_FromString(robot->get_robot_ptr()->GetName().c_str());
535  PyObject* py_value_target_name = PyString_FromString(target_name.c_str());
536  PyTuple_SetItem(py_args, 0, py_value_env_id); //py_value reference stolen here! no need to DECREF
537  PyTuple_SetItem(py_args, 1, py_value_robot_name); //py_value reference stolen here! no need to DECREF
538  PyTuple_SetItem(py_args, 2, py_value_target_name); //py_value reference stolen here! no need to DECREF
539  // call function, get return value
540  PyObject* py_value = PyObject_CallObject(py_func, py_args);
541  Py_DECREF(py_args);
542  // check return value
543  if (py_value != NULL) {
544  if (!PyString_Check(py_value)) {
545  Py_DECREF(py_value);
546  Py_DECREF(py_func);
547  Py_Finalize();
548  throw fawkes::Exception("%s: Graspplanning: No grasping path found.", name());
549  }
550  std::stringstream resval;
551  resval << std::setprecision(std::numeric_limits<dReal>::digits10+1);
552  resval << PyString_AsString(py_value);
553  if (!traj->deserialize(resval) ) {
554  Py_DECREF(py_value);
555  Py_DECREF(py_func);
556  Py_Finalize();
557  throw fawkes::Exception("%s: Graspplanning: Reading trajectory data failed.", name());
558  }
559  Py_DECREF(py_value);
560  } else { // if calling function failed
561  Py_DECREF(py_func);
562  PyErr_Print();
563  Py_Finalize();
564  throw fawkes::Exception("%s: Graspplanning: Calling function failed.", name());
565  }
566  } else { // if loading func failed
567  if (PyErr_Occurred())
568  PyErr_Print();
569  Py_XDECREF(py_func);
570  Py_Finalize();
571  throw fawkes::Exception("%s: Graspplanning: Loading function failed.", name());
572  }
573  Py_XDECREF(py_func);
574  } else { // if loading module failed
575  PyErr_Print();
576  Py_Finalize();
577  throw fawkes::Exception("%s: Graspplanning: Loading python file failed.", name());
578  }
579 
580  Py_EndInterpreter(int_state); // close sub-interpreter
581  PyThreadState_Swap(cur_state); // re-set active state to previous one
582  PyGILState_Release(gil_state); // release GIL
583 
584  Py_Finalize(); // should be careful with that, as it closes global interpreter; Other threads running python may fail
585 
586  if(__logger)
587  __logger->log_debug(name(), "Graspplanning: path planned");
588 
589  // re-timing the trajectory with
590  planningutils::RetimeActiveDOFTrajectory(traj, robot->get_robot_ptr());
591 
592  // sampling trajectory and setting robots trajectory
593  std::vector< std::vector<dReal> >* trajRobot = robot->get_trajectory();
594  trajRobot->clear();
595  for(dReal time = 0; time <= traj->GetDuration(); time += (dReal)sampling) {
596  std::vector<dReal> point;
597  traj->Sample(point,time);
598  trajRobot->push_back(point);
599  }
600 
601  // viewer options
602  if( __viewer_running ) {
603 
604  // display trajectory in viewer
605  __graph_handle.clear(); // remove all GraphHandlerPtr and currently drawn plots
606  {
607  RobotBasePtr tmp_robot = robot->get_robot_ptr();
608  RobotBase::RobotStateSaver saver(tmp_robot); // save the state, do not modifiy currently active robot!
609  for(std::vector< std::vector<dReal> >::iterator it = trajRobot->begin(); it!=trajRobot->end(); ++it) {
610  tmp_robot->SetActiveDOFValues((*it));
611  const OpenRAVE::Vector &trans = tmp_robot->GetActiveManipulator()->GetEndEffectorTransform().trans;
612  float transa[4] = { (float)trans.x, (float)trans.y, (float)trans.z, (float)trans.w };
613  __graph_handle.push_back(__env->plot3(transa, 1, 0, 2.f, Vector(1.f, 0.f, 0.f, 1.f)));
614  }
615  } // robot state is restored
616 
617  // display motion in viewer
618  if( robot->display_planned_movements()) {
619  robot->get_robot_ptr()->GetController()->SetPath(traj);
620  }
621  }
622 }
623 
624 
625 /** Add an object to the environment.
626  * @param name name that should be given to that object
627  * @param filename path to xml file of that object (KinBody)
628  * @return true if successful
629  */
630 bool
631 OpenRaveEnvironment::add_object(const std::string& name, const std::string& filename)
632 {
633  try {
634  EnvironmentMutex::scoped_lock lock(__env->GetMutex());
635  KinBodyPtr kb = __env->ReadKinBodyXMLFile(filename);
636  kb->SetName(name);
637  __env->Add(kb);
638  } catch(const OpenRAVE::openrave_exception &e) {
639  if(__logger)
640  __logger->log_warn(this->name(), "Could not add Object '%s'. Ex:%s", name.c_str(), e.what());
641  return false;
642  }
643 
644  return true;
645 }
646 
647 /** Remove object from environment.
648  * @param name name of the object
649  * @return true if successful
650  */
651 bool
652 OpenRaveEnvironment::delete_object(const std::string& name)
653 {
654  try {
655  EnvironmentMutex::scoped_lock lock(__env->GetMutex());
656  KinBodyPtr kb = __env->GetKinBody(name);
657  __env->Remove(kb);
658  } catch(const OpenRAVE::openrave_exception &e) {
659  if(__logger)
660  __logger->log_warn(this->name(), "Could not delete Object '%s'. Ex:%s", name.c_str(), e.what());
661  return false;
662  }
663 
664  return true;
665 }
666 
667 /** Remove all objects from environment.
668  * @return true if successful
669  */
670 bool
672 {
673  try {
674  EnvironmentMutex::scoped_lock lock(__env->GetMutex());
675  std::vector<KinBodyPtr> bodies;
676  __env->GetBodies( bodies );
677 
678  for( std::vector<KinBodyPtr>::iterator it=bodies.begin(); it!=bodies.end(); ++it ) {
679  if( !(*it)->IsRobot() )
680  __env->Remove(*it);
681  }
682  } catch(const OpenRAVE::openrave_exception &e) {
683  if(__logger)
684  __logger->log_warn(this->name(), "Could not delete all objects. Ex:%s", e.what());
685  return false;
686  }
687 
688  return true;
689 }
690 
691 /** Rename object.
692  * @param name current name of the object
693  * @param new_name new name of the object
694  * @return true if successful
695  */
696 bool
697 OpenRaveEnvironment::rename_object(const std::string& name, const std::string& new_name)
698 {
699  try {
700  EnvironmentMutex::scoped_lock lock(__env->GetMutex());
701  KinBodyPtr kb = __env->GetKinBody(name);
702  kb->SetName(new_name);
703  } catch(const OpenRAVE::openrave_exception &e) {
704  if(__logger)
705  __logger->log_warn(this->name(), "Could not rename Object '%s' to '%s'. Ex:%s", name.c_str(), new_name.c_str(), e.what());
706  return false;
707  }
708 
709  return true;
710 }
711 
712 /** Move object in the environment.
713  * Distances are given in meters
714  * @param name name of the object
715  * @param trans_x transition along x-axis
716  * @param trans_y transition along y-axis
717  * @param trans_z transition along z-axis
718  * @return true if successful
719  */
720 bool
721 OpenRaveEnvironment::move_object(const std::string& name, float trans_x, float trans_y, float trans_z)
722 {
723  try {
724  EnvironmentMutex::scoped_lock lock(__env->GetMutex());
725  KinBodyPtr kb = __env->GetKinBody(name);
726 
727  Transform transform = kb->GetTransform();
728  transform.trans = Vector(trans_x, trans_y, trans_z);
729 
730  kb->SetTransform(transform);
731  } catch(const OpenRAVE::openrave_exception &e) {
732  if(__logger)
733  __logger->log_warn(this->name(), "Could not move Object '%s'. Ex:%s", name.c_str(), e.what());
734  return false;
735  }
736 
737  return true;
738 }
739 
740 /** Move object in the environment.
741  * Distances are given in meters
742  * @param name name of the object
743  * @param trans_x transition along x-axis
744  * @param trans_y transition along y-axis
745  * @param trans_z transition along z-axis
746  * @param robot move relatively to robot (in most simple cases robot is at position (0,0,0) anyway, so this has no effect)
747  * @return true if successful
748  */
749 bool
750 OpenRaveEnvironment::move_object(const std::string& name, float trans_x, float trans_y, float trans_z, OpenRaveRobotPtr& robot)
751 {
752  // remember, OpenRAVE Vector is 4-tuple (w,x,y,z)
753  Transform t;
754  {
755  EnvironmentMutex::scoped_lock( __env->GetMutex());
756  t = robot->get_robot_ptr()->GetTransform();
757  }
758  return move_object(name, trans_x+t.trans[1], trans_y+t.trans[2], trans_z+t.trans[3]);
759 }
760 
761 /** Rotate object by a quaternion.
762  * @param name name of the object
763  * @param quat_x x value of quaternion
764  * @param quat_y y value of quaternion
765  * @param quat_z z value of quaternion
766  * @param quat_w w value of quaternion
767  * @return true if successful
768  */
769 bool
770 OpenRaveEnvironment::rotate_object(const std::string& name, float quat_x, float quat_y, float quat_z, float quat_w)
771 {
772  try {
773  EnvironmentMutex::scoped_lock lock(__env->GetMutex());
774  KinBodyPtr kb = __env->GetKinBody(name);
775 
776  Vector quat(quat_w, quat_x, quat_y, quat_z);
777 
778  Transform transform = kb->GetTransform();
779  transform.rot = quat;
780 
781  kb->SetTransform(transform);
782  } catch(const OpenRAVE::openrave_exception &e) {
783  if(__logger)
784  __logger->log_warn(this->name(), "Could not rotate Object '%s'. Ex:%s", name.c_str(), e.what());
785  return false;
786  }
787 
788  return true;
789 }
790 
791 /** Rotate object along its axis.
792  * Rotation angles should be given in radians.
793  * @param name name of the object
794  * @param rot_x 1st rotation, along x-axis
795  * @param rot_y 2nd rotation, along y-axis
796  * @param rot_z 3rd rotation, along z-axis
797  * @return true if successful
798  */
799 bool
800 OpenRaveEnvironment::rotate_object(const std::string& name, float rot_x, float rot_y, float rot_z)
801 {
802  Vector q1 = quatFromAxisAngle(Vector(rot_x, 0.f, 0.f));
803  Vector q2 = quatFromAxisAngle(Vector(0.f, rot_y, 0.f));
804  Vector q3 = quatFromAxisAngle(Vector(0.f, 0.f, rot_z));
805 
806  Vector q12 = quatMultiply (q1, q2);
807  Vector quat = quatMultiply (q12, q3);
808 
809  return rotate_object(name, quat[1], quat[2], quat[3], quat[0]);
810 }
811 
812 
813 /** Clone all non-robot objects from a referenced OpenRaveEnvironment to this one.
814  * The environments should contain the same objects afterwards. Therefore objects in current
815  * environment that do not exist in the reference environment are deleted as well.
816  * @param env The reference environment
817  */
818 void
820 {
821  // lock environments
822  EnvironmentMutex::scoped_lock lockold(env->get_env_ptr()->GetMutex());
823  EnvironmentMutex::scoped_lock lock(__env->GetMutex());
824 
825  // get kinbodies
826  std::vector<KinBodyPtr> old_bodies, bodies;
827  env->get_env_ptr()->GetBodies( old_bodies );
828  __env->GetBodies( bodies );
829 
830  // check for existing bodies in this environment
831  std::vector<KinBodyPtr>::iterator old_body, body;
832  for(old_body=old_bodies.begin(); old_body!=old_bodies.end(); ++old_body ) {
833  if( (*old_body)->IsRobot() )
834  continue;
835 
836  KinBodyPtr new_body;
837  for( body=bodies.begin(); body!=bodies.end(); ++body ) {
838  if( (*body)->IsRobot() )
839  continue;
840 
841  if( (*body)->GetName() == (*old_body)->GetName() && (*body)->GetKinematicsGeometryHash() == (*old_body)->GetKinematicsGeometryHash() ) {
842  new_body = *body;
843  break;
844  }
845  }
846 
847  if( body != bodies.end() ) {
848  // remove this one from the list, to reduce checking
849  // (this one has already been found a match)
850  bodies.erase( body );
851  }
852 
853  if( !new_body ) {
854  // this is a new kinbody!
855 
856  // create new empty KinBody, then clone from old
857  KinBodyPtr empty;
858  new_body = __env->ReadKinBodyData(empty, "<KinBody></KinBody>");
859  new_body->Clone(*old_body, 0);
860 
861  // add kinbody to environment
862  __env->Add(new_body);
863 
864  // update collisison-checker and physics-engine to consider new kinbody
865  //__env->GetCollisionChecker()->InitKinBody(new_body);
866  //__env->GetPhysicsEngine()->InitKinBody(new_body);
867 
868  // clone kinbody state
869  KinBody::KinBodyStateSaver saver(*old_body, KinBody::Save_LinkTransformation|KinBody::Save_LinkEnable|KinBody::Save_LinkVelocities);
870  saver.Restore(new_body);
871 
872  } else {
873  // this kinbody already exists. just clone the state
874  KinBody::KinBodyStateSaver saver(*old_body, 0xffffffff);
875  saver.Restore(new_body);
876  }
877  }
878 
879  // remove bodies that are not in old_env anymore
880  for( body=bodies.begin(); body!=bodies.end(); ++body ) {
881  if( (*body)->IsRobot() )
882  continue;
883 
884  __env->Remove( *body );
885  }
886 }
887 
888 } // end of namespace fawkes
virtual bool move_object(const std::string &name, float trans_x, float trans_y, float trans_z)
Move object in the environment.
Target: OpenRAVE::IkParameterization string.
Definition: types.h:60
virtual void start_viewer()
Starts the qt viewer in a separate thread.
float qx
x value of quaternion
Definition: types.h:79
virtual ~OpenRaveEnvironment()
Destructor.
OpenRaveEnvironment(fawkes::Logger *logger=0)
Constructor.
Definition: environment.cpp:77
virtual OpenRAVE::EnvironmentBasePtr get_env_ptr() const
Get EnvironmentBasePtr.
float x
translation on x-axis
Definition: types.h:76
Target: relative endeffector translation, based on arm extension.
Definition: types.h:59
Fawkes library namespace.
float qz
z value of quaternion
Definition: types.h:81
virtual bool rename_object(const std::string &name, const std::string &new_name)
Rename object.
target_type_t type
target type
Definition: types.h:85
Target: absolute endeffector translation and rotation.
Definition: types.h:57
virtual void set_name(const char *name)
Set name of environment.
std::string raw_cmd
raw command passed to the BaseManipulator module, e.g.
Definition: types.h:88
std::string plannerparams
additional string to be passed to planner, i.e.
Definition: types.h:87
virtual bool delete_object(const std::string &name)
Remove object from environment.
virtual void create()
Create and lock the environment.
float z
translation on z-axis
Definition: types.h:78
virtual bool add_object(const std::string &name, const std::string &filename)
Add an object to the environment.
Target: Raw string, passed to OpenRAVE&#39;s BaseManipulation module.
Definition: types.h:61
float y
translation on y-axis
Definition: types.h:77
virtual bool rotate_object(const std::string &name, float quat_x, float quat_y, float quat_z, float quat_w)
Rotate object by a quaternion.
void clear()
Set underlying instance to 0, decrementing reference count of existing instance appropriately.
Definition: refptr.h:457
Base class for exceptions in Fawkes.
Definition: exception.h:36
OpenRaveEnvironment class.
Definition: environment.h:45
Target: relative endeffector translation, based on robot&#39;s coordinate system.
Definition: types.h:58
OpenRAVE::IkParameterization ikparam
OpenRAVE::IkParameterization; each target is implicitly transformed to one by OpenRAVE.
Definition: types.h:86
virtual void load_IK_solver(OpenRaveRobotPtr &robot, OpenRAVE::IkParameterizationType iktype=OpenRAVE::IKP_Transform6D)
Autogenerate IKfast IK solver for robot.
virtual void run_graspplanning(const std::string &target_name, OpenRaveRobotPtr &robot, float sampling=0.01f)
Run graspplanning script for a given target.
Struct containing information about the current target.
Definition: types.h:75
virtual bool delete_all_objects()
Remove all objects from environment.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
virtual void enable_debug(OpenRAVE::DebugLevel level=OpenRAVE::Level_Debug)
Enable debugging messages of OpenRAVE.
void run_viewer(OpenRAVE::EnvironmentBasePtr env, const std::string &viewername, bool *running)
Sets and loads a viewer for OpenRAVE.
Definition: environment.cpp:55
virtual void add_robot(const std::string &filename)
Add a robot into the scene.
float qw
w value of quaternion
Definition: types.h:82
virtual void run_planner(OpenRaveRobotPtr &robot, float sampling=0.01f)
Plan collision-free path for current and target manipulator configuration of a OpenRaveRobot robot...
RefPtr<> is a reference-counting shared smartpointer.
Definition: refptr.h:49
OpenRaveManipulatorPtr manip
target manipulator configuration
Definition: types.h:84
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void destroy()
Destroy the environment.
void get_angles(std::vector< T > &to) const
Get motor angles of OpenRAVE model.
Definition: manipulator.h:83
virtual void clone_objects(OpenRaveEnvironmentPtr &env)
Clone all non-robot objects from a referenced OpenRaveEnvironment to this one.
Expected parameter is missing.
Definition: software.h:82
virtual void disable_debug()
Disable debugging messages of OpenRAVE.
Target: motor joint values.
Definition: types.h:56
float qy
y value of quaternion
Definition: types.h:80
Interface for logging.
Definition: logger.h:34