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