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