Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
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  */
52 void
53 SetViewer(OpenRAVE::EnvironmentBasePtr env, const std::string& viewername)
54 {
55  ViewerBasePtr viewer = RaveCreateViewer(env, viewername);
56 
57  // attach it to the environment:
58  env->AddViewer(viewer);
59 
60  // finally you call the viewer's infinite loop (this is why you need a separate thread):
61  viewer->main(/*showGUI=*/true);
62 }
63 
64 
65 /** @class OpenRaveEnvironment <plugins/openrave/environment.h>
66 * Class handling interaction with the OpenRAVE::EnvironmentBase class.
67 * This class loads a scene and handles robots/objects etc in it. All calculations
68 * in OpenRAVE (IK, planning, etc) are done based on the current scene.
69 * @author Bahram Maleki-Fard
70 */
71 
72 /** Constructor.
73  * @param logger pointer to fawkes logger
74  */
75 OpenRaveEnvironment::OpenRaveEnvironment(fawkes::Logger* logger) :
76  __logger( logger ),
77  __viewer_enabled( 0 )
78 {
79 }
80 
81 /** Destructor. */
83 {
84  this->destroy();
85 }
86 
87 /** Create and lock the environment. */
88 void
90 {
91  // create environment
92  __env = RaveCreateEnvironment();
93  if(!__env)
94  {throw fawkes::Exception("OpenRAVE Environment: Could not create environment. Error in OpenRAVE.");}
95  else if (__logger)
96  {__logger->log_debug("OpenRAVE Environment", "Environment created");}
97 
98  // create planner
99  __planner = RaveCreatePlanner(__env,"birrt");
100  if(!__planner)
101  {throw fawkes::Exception("OpenRAVE Environment: Could not create planner. Error in OpenRAVE.");}
102 
103  // create ikfast module
104  __mod_ikfast = RaveCreateModule(__env,"ikfast");
105  __env->AddModule(__mod_ikfast,"");
106 }
107 
108 /** Destroy the environment. */
109 void
111 {
112  try {
113  __env->Destroy();
114  if(__logger)
115  {__logger->log_debug("OpenRAVE Environment", "Environment destroyed");}
116  } catch(const openrave_exception& e) {
117  if(__logger)
118  {__logger->log_warn("OpenRAVE Environment", "Could not destroy Environment. Ex:%s", e.what());}
119  }
120 }
121 
122 /** Lock the environment to prevent changes. */
123 void
125 {
126  EnvironmentMutex::scoped_lock lock(__env->GetMutex());
127 }
128 
129 /** Enable debugging messages of OpenRAVE.
130  * @param level debug level to set for OpenRAVE
131  */
132 void
133 OpenRaveEnvironment::enable_debug(OpenRAVE::DebugLevel level)
134 {
135  RaveSetDebugLevel(level);
136 }
137 
138 /** Disable debugging messages of OpenRAVE. */
139 void
141 {
142  RaveSetDebugLevel(Level_Fatal);
143 }
144 
145 /** Add a robot into the scene.
146  * @param robot RobotBasePtr of robot to add
147  * @return 1 if succeeded, 0 if not able to add robot
148  */
149 void
150 OpenRaveEnvironment::add_robot(OpenRAVE::RobotBasePtr robot)
151 {
152  try{
153  __env->Add(robot);
154  if(__logger)
155  {__logger->log_debug("OpenRAVE Environment", "Robot added to environment.");}
156  } catch(openrave_exception &e) {
157  if(__logger)
158  {__logger->log_debug("OpenRAVE Environment", "Could not add robot to environment. OpenRAVE error:%s", e.message().c_str());}
159  }
160 }
161 
162 /** Add a robot into the scene.
163  * @param filename path to robot's xml file
164  * @return 1 if succeeded, 0 if not able to load file
165  */
166 void
167 OpenRaveEnvironment::add_robot(const std::string& filename)
168 {
169  // load the robot
170  RobotBasePtr robot = __env->ReadRobotXMLFile(filename);
171 
172  // if could not load robot file: Check file path, and test file itself for correct syntax and semantics
173  // by loading it directly into openrave with "openrave robotfile.xml"
174  if( !robot )
175  {throw fawkes::IllegalArgumentException("OpenRAVE Environment: Robot could not be loaded. Check xml file/path.");}
176  else if(__logger)
177  {__logger->log_debug("OpenRAVE Environment", "Robot loaded.");}
178 
179  add_robot(robot);
180 }
181 
182 /** Add a robot into the scene.
183  * @param robot pointer to OpenRaveRobot object of robot to add
184  * @return 1 if succeeded, 0 if not able to add robot
185  */
186 void
188 {
189  add_robot(robot->get_robot_ptr());
190 }
191 
192 
193 /** Get EnvironmentBasePtr.
194  * @return EnvironmentBasePtr in use
195  */
196 OpenRAVE::EnvironmentBasePtr
198 {
199  return __env;
200 }
201 
202 /** Starts the qt viewer in a separate thread.
203  * Use this mainly for debugging purposes, as it uses
204  * a lot of CPU/GPU resources.
205  */
206 void
208 {
209  try {
210  boost::thread thviewer(boost::bind(SetViewer,__env,"qtcoin"));
211  } catch( const openrave_exception &e) {
212  if(__logger)
213  {__logger->log_error("OpenRAVE Environment", "Could not load viewr. Ex:%s", e.what());}
214  throw;
215  }
216 
217  __viewer_enabled = true;
218 }
219 
220 /** Autogenerate IKfast IK solver for robot.
221  * @param robot pointer to OpenRaveRobot object
222  * @param iktype IK type of solver (default: Transform6D; use TranslationDirection5D for 5DOF arms)
223  */
224 void
225 OpenRaveEnvironment::load_IK_solver(OpenRaveRobot* robot, OpenRAVE::IkParameterizationType iktype)
226 {
227  RobotBasePtr robotBase = robot->get_robot_ptr();
228 
229  std::stringstream ssin,ssout;
230  ssin << "LoadIKFastSolver " << robotBase->GetName() << " " << (int)iktype;
231  // if necessary, add free inc for degrees of freedom
232  //ssin << " " << 0.04f;
233  if( !__mod_ikfast->SendCommand(ssout,ssin) )
234  {throw fawkes::Exception("OpenRAVE Environment: Could not load ik solver");}
235 }
236 
237 /** Plan collision-free path for current and target manipulator
238  * configuration of a OpenRaveRobot robot.
239  * @param robot pointer to OpenRaveRobot object of robot to use
240  * @param sampling sampling time between each trajectory point (in seconds)
241  */
242 void
244 {
245  bool success;
246  EnvironmentMutex::scoped_lock lock(__env->GetMutex()); // lock environment
247 
248  // init planner. This is automatically done by BaseManipulation, but putting it here
249  // helps to identify problem source if any occurs.
250  success = __planner->InitPlan(robot->get_robot_ptr(),robot->get_planner_params());
251  if(!success)
252  {throw fawkes::Exception("OpenRAVE Environment: Planner: init failed");}
253  else if(__logger)
254  {__logger->log_debug("OpenRAVE Environment", "Planner: initialized");}
255 
256  // plan path with basemanipulator
257  ModuleBasePtr basemanip = robot->get_basemanip();
258  target_t target = robot->get_target();
259  std::stringstream cmdin,cmdout;
260  cmdin << std::setprecision(std::numeric_limits<dReal>::digits10+1);
261  cmdout << std::setprecision(std::numeric_limits<dReal>::digits10+1);
262 
263  if( target.type == TARGET_RELATIVE_EXT ) {
264  Transform t = robot->get_robot_ptr()->GetActiveManipulator()->GetEndEffectorTransform();
265  //initial pose of arm looks at +z. Target values are referring to robot's coordinating system,
266  //which have this direction vector if taken as extension of manipulator (rotate -90° on y-axis)
267  Vector dir(target.y,target.z,target.x);
268  TransformMatrix mat = matrixFromQuat(t.rot);
269  dir = mat.rotate(dir);
270  target.type = TARGET_RELATIVE;
271  target.x = dir[0];
272  target.y = dir[1];
273  target.z = dir[2];
274  }
275 
276  switch(target.type) {
277  case (TARGET_JOINTS) :
278  cmdin << "MoveActiveJoints goal";
279  {
280  std::vector<dReal> v;
281  target.manip->get_angles(v);
282  for(size_t i = 0; i < v.size(); ++i) {
283  cmdin << " " << v[i];
284  }
285  }
286  break;
287 
288  case (TARGET_IKPARAM) :
289  cmdin << "MoveToHandPosition ikparam";
290  cmdin << " " << target.ikparam;
291  break;
292 
293  case (TARGET_TRANSFORM) :
294  cmdin << "MoveToHandPosition pose";
295  cmdin << " " << target.qw << " " << target.qx << " " << target.qy << " " << target.qz;
296  cmdin << " " << target.x << " " << target.y << " " << target.z;
297  break;
298 
299  case (TARGET_RELATIVE) :
300  // for now move to all relative targets in a straigt line
301  cmdin << "MoveHandStraight direction";
302  cmdin << " " << target.x << " " << target.y << " " << target.z;
303  {
304  dReal stepsize = 0.005;
305  dReal length = sqrt(target.x*target.x + target.y*target.y + target.z*target.z);
306  int minsteps = (int)(length/stepsize);
307  if( minsteps > 4 )
308  minsteps -= 4;
309 
310  cmdin << " stepsize " << stepsize;
311  cmdin << " minsteps " << minsteps;
312  cmdin << " maxsteps " << (int)(length/stepsize);
313  }
314  break;
315 
316  default :
317  throw fawkes::Exception("OpenRAVE Environment: Planner: Invalid target type");
318  }
319 
320  //add additional planner parameters
321  if( !target.plannerparams.empty() ) {
322  cmdin << " " << target.plannerparams;
323  }
324  cmdin << " execute 0";
325  cmdin << " outputtraj";
326  //if(__logger)
327  // __logger->log_debug("OpenRAVE Environment", "Planner: basemanip cmdin:%s", cmdin.str().c_str());
328 
329  try {
330  success = basemanip->SendCommand(cmdout,cmdin);
331  } catch(openrave_exception &e) {
332  throw fawkes::Exception("OpenRAVE Environment: Planner: basemanip command failed. Ex%s", e.what());
333  }
334  if(!success)
335  {throw fawkes::Exception("OpenRAVE Environment: Planner: planning failed");}
336  else if(__logger)
337  {__logger->log_debug("OpenRAVE Environment", "Planner: path planned");}
338 
339  // read returned trajectory
340  TrajectoryBasePtr traj = RaveCreateTrajectory(__env, "");
341  traj->Init(robot->get_robot_ptr()->GetActiveConfigurationSpecification());
342  if( !traj->deserialize(cmdout) ) {
343  {throw fawkes::Exception("OpenRAVE Environment: Planner: Cannot read trajectory data.");}
344  }
345 
346  // sampling trajectory and setting robots trajectory
347  std::vector< std::vector<dReal> >* trajRobot = robot->get_trajectory();
348  trajRobot->clear();
349  for(dReal time = 0; time <= traj->GetDuration(); time += (dReal)sampling) {
350  std::vector<dReal> point;
351  traj->Sample(point,time);
352  trajRobot->push_back(point);
353  }
354 
355  // viewer options
356  if( __viewer_enabled ) {
357 
358  // display trajectory in viewer
359  __graph_handle.clear(); // remove all GraphHandlerPtr and currently drawn plots
360  {
361  RobotBasePtr tmp_robot = robot->get_robot_ptr();
362  RobotBase::RobotStateSaver saver(tmp_robot); // save the state, do not modifiy currently active robot!
363  for(std::vector< std::vector<dReal> >::iterator it = trajRobot->begin(); it!=trajRobot->end(); ++it) {
364  tmp_robot->SetActiveDOFValues((*it));
365  const OpenRAVE::Vector &trans = tmp_robot->GetActiveManipulator()->GetEndEffectorTransform().trans;
366  float transa[4] = { trans.x, trans.y, trans.z, trans.w };
367  __graph_handle.push_back(__env->plot3(transa, 1, 0, 2.f, Vector(1.f, 0.f, 0.f, 1.f)));
368  }
369  } // robot state is restored
370 
371  // display motion in viewer
372  if( robot->display_planned_movements()) {
373  robot->get_robot_ptr()->GetController()->SetPath(traj);
374  }
375  }
376 
377 }
378 
379 
380 /** Run graspplanning script for a given target.
381  * Script loads grasping databse, checks if target is graspable for various grasps
382  * and on success returns a string containing trajectory data.
383  * Currently the grasping databse can only be accessed via python, so we use a short
384  * python script (shortened and modified from officiel OpenRAVE graspplanning.py) to do the work.
385  * @param target_name name of targeted object (KinBody)
386  * @param robot pointer to OpenRaveRobot object of robot to use
387  * @param sampling sampling time between each trajectory point (in seconds)
388  */
389 void
390 OpenRaveEnvironment::run_graspplanning(const std::string& target_name, OpenRaveRobot* robot, float sampling)
391 {
392  std::string filename = SRCDIR"/python/graspplanning.py";
393  std::string funcname = "runGrasp";
394 
395  TrajectoryBasePtr traj = RaveCreateTrajectory(__env, "");
396  traj->Init(robot->get_robot_ptr()->GetActiveConfigurationSpecification());
397 
398  FILE* py_file = fopen(filename.c_str(), "r");
399  if (py_file == NULL)
400  {throw fawkes::Exception("OpenRAVE Environment: Graspplanning: opening python file failed");}
401 
402  Py_Initialize();
403 
404  // Need to aquire global interpreter lock (GIL), create new sub-interpreter to run code in there
405  PyGILState_STATE gil_state = PyGILState_Ensure(); // aquire python GIL
406  PyThreadState* cur_state = PyThreadState_Get(); // get current ThreadState; need this to switch back to later
407  PyThreadState* int_state = Py_NewInterpreter(); // create new sub-interpreter
408  PyThreadState_Swap(int_state); // set active ThreadState; maybe not needed after calling NewInterpreter() ?
409  // Now we can safely run our python code
410 
411  // using python C API
412  PyObject* py_main = PyImport_AddModule("__main__"); // borrowed reference
413  if( !py_main ) {
414  // __main__ should always exist
415  fclose(py_file);
416  Py_EndInterpreter(int_state);
417  PyThreadState_Swap(cur_state);
418  PyGILState_Release(gil_state); // release GIL
419  Py_Finalize();
420  throw fawkes::Exception("OpenRAVE Environment: Graspplanning: Python reference '__main__' does not exist.");
421  }
422  PyObject* py_dict = PyModule_GetDict(py_main); // borrowed reference
423  if( !py_dict ) {
424  // __main__ should have a dictionary
425  fclose(py_file);
426  Py_Finalize();
427  throw fawkes::Exception("OpenRAVE Environment: Graspplanning: Python reference '__main__' does not have a dictionary.");
428  }
429 
430  // load file
431  int py_module = PyRun_SimpleFile(py_file, filename.c_str());
432  fclose(py_file);
433  if (!py_module) {
434  // load function from global dictionary
435  PyObject* py_func = PyDict_GetItemString(py_dict, funcname.c_str());
436  if (py_func && PyCallable_Check(py_func)) {
437  // create tuple for args to be passed to py_func
438  PyObject* py_args = PyTuple_New(3);
439  // fill tuple with values. We're not checking for conversion errors here atm, can be added!
440  PyObject* py_value_env_id = PyInt_FromLong(RaveGetEnvironmentId(__env));
441  PyObject* py_value_robot_name = PyString_FromString(robot->get_robot_ptr()->GetName().c_str());
442  PyObject* py_value_target_name = PyString_FromString(target_name.c_str());
443  PyTuple_SetItem(py_args, 0, py_value_env_id); //py_value reference stolen here! no need to DECREF
444  PyTuple_SetItem(py_args, 1, py_value_robot_name); //py_value reference stolen here! no need to DECREF
445  PyTuple_SetItem(py_args, 2, py_value_target_name); //py_value reference stolen here! no need to DECREF
446  // call function, get return value
447  PyObject* py_value = PyObject_CallObject(py_func, py_args);
448  Py_DECREF(py_args);
449  // check return value
450  if (py_value != NULL) {
451  if (!PyString_Check(py_value)) {
452  Py_DECREF(py_value);
453  Py_DECREF(py_func);
454  Py_Finalize();
455  throw fawkes::Exception("OpenRAVE Environment: Graspplanning: No grasping path found.");
456  }
457  std::stringstream resval;
458  resval << std::setprecision(std::numeric_limits<dReal>::digits10+1);
459  resval << PyString_AsString(py_value);
460  if (!traj->deserialize(resval) ) {
461  Py_DECREF(py_value);
462  Py_DECREF(py_func);
463  Py_Finalize();
464  throw fawkes::Exception("OpenRAVE Environment: Graspplanning: Reading trajectory data failed.");
465  }
466  Py_DECREF(py_value);
467  } else { // if calling function failed
468  Py_DECREF(py_func);
469  PyErr_Print();
470  Py_Finalize();
471  throw fawkes::Exception("OpenRAVE Environment: Graspplanning: Calling function failed.");
472  }
473  } else { // if loading func failed
474  if (PyErr_Occurred())
475  PyErr_Print();
476  Py_XDECREF(py_func);
477  Py_Finalize();
478  throw fawkes::Exception("OpenRAVE Environment: Graspplanning: Loading function failed.");
479  }
480  Py_XDECREF(py_func);
481  } else { // if loading module failed
482  PyErr_Print();
483  Py_Finalize();
484  throw fawkes::Exception("OpenRAVE Environment: Graspplanning: Loading python file failed.");
485  }
486 
487  Py_EndInterpreter(int_state); // close sub-interpreter
488  PyThreadState_Swap(cur_state); // re-set active state to previous one
489  PyGILState_Release(gil_state); // release GIL
490 
491  Py_Finalize(); // should be careful with that, as it closes global interpreter; Other threads running python may fail
492 
493  if(__logger)
494  {__logger->log_debug("OpenRAVE Environment", "Graspplanning: path planned");}
495 
496  // re-timing the trajectory with
497  planningutils::RetimeActiveDOFTrajectory(traj, robot->get_robot_ptr());
498 
499  // sampling trajectory and setting robots trajectory
500  std::vector< std::vector<dReal> >* trajRobot = robot->get_trajectory();
501  trajRobot->clear();
502  for(dReal time = 0; time <= traj->GetDuration(); time += (dReal)sampling) {
503  std::vector<dReal> point;
504  traj->Sample(point,time);
505  trajRobot->push_back(point);
506  }
507 
508  // viewer options
509  if( __viewer_enabled ) {
510 
511  // display trajectory in viewer
512  __graph_handle.clear(); // remove all GraphHandlerPtr and currently drawn plots
513  {
514  RobotBasePtr tmp_robot = robot->get_robot_ptr();
515  RobotBase::RobotStateSaver saver(tmp_robot); // save the state, do not modifiy currently active robot!
516  for(std::vector< std::vector<dReal> >::iterator it = trajRobot->begin(); it!=trajRobot->end(); ++it) {
517  tmp_robot->SetActiveDOFValues((*it));
518  const OpenRAVE::Vector &trans = tmp_robot->GetActiveManipulator()->GetEndEffectorTransform().trans;
519  float transa[4] = { trans.x, trans.y, trans.z, trans.w };
520  __graph_handle.push_back(__env->plot3(transa, 1, 0, 2.f, Vector(1.f, 0.f, 0.f, 1.f)));
521  }
522  } // robot state is restored
523 
524  // display motion in viewer
525  if( robot->display_planned_movements()) {
526  robot->get_robot_ptr()->GetController()->SetPath(traj);
527  }
528  }
529 }
530 
531 
532 /** Add an object to the environment.
533  * @param name name that should be given to that object
534  * @param filename path to xml file of that object (KinBody)
535  * @return true if successful
536  */
537 bool
538 OpenRaveEnvironment::add_object(const std::string& name, const std::string& filename)
539 {
540  try {
541  EnvironmentMutex::scoped_lock lock(__env->GetMutex());
542  KinBodyPtr kb = __env->ReadKinBodyXMLFile(filename);
543  kb->SetName(name);
544  __env->Add(kb);
545  } catch(const OpenRAVE::openrave_exception &e) {
546  if(__logger)
547  __logger->log_warn("OpenRAVE Environment", "Could not add Object '%s'. Ex:%s", name.c_str(), e.what());
548  return false;
549  }
550 
551  return true;
552 }
553 
554 /** Remove object from environment.
555  * @param name name of the object
556  * @return true if successful
557  */
558 bool
559 OpenRaveEnvironment::delete_object(const std::string& name)
560 {
561  try {
562  EnvironmentMutex::scoped_lock lock(__env->GetMutex());
563  KinBodyPtr kb = __env->GetKinBody(name);
564  __env->Remove(kb);
565  } catch(const OpenRAVE::openrave_exception &e) {
566  if(__logger)
567  __logger->log_warn("OpenRAVE Environment", "Could not delete Object '%s'. Ex:%s", name.c_str(), e.what());
568  return false;
569  }
570 
571  return true;
572 }
573 
574 /** Rename object.
575  * @param name current name of the object
576  * @param new_name new name of the object
577  * @return true if successful
578  */
579 bool
580 OpenRaveEnvironment::rename_object(const std::string& name, const std::string& new_name)
581 {
582  try {
583  EnvironmentMutex::scoped_lock lock(__env->GetMutex());
584  KinBodyPtr kb = __env->GetKinBody(name);
585  kb->SetName(new_name);
586  } catch(const OpenRAVE::openrave_exception &e) {
587  if(__logger)
588  __logger->log_warn("OpenRAVE Environment", "Could not rename Object '%s' to '%s'. Ex:%s", name.c_str(), new_name.c_str(), e.what());
589  return false;
590  }
591 
592  return true;
593 }
594 
595 /** Move object in the environment.
596  * Distances are given in meters
597  * @param name name of the object
598  * @param trans_x transition along x-axis
599  * @param trans_y transition along y-axis
600  * @param trans_z transition along z-axis
601  * @param robot if given, move relatively to robot (in most simple cases robot is at position (0,0,0) anyway, so this has no effect)
602  * @return true if successful
603  */
604 bool
605 OpenRaveEnvironment::move_object(const std::string& name, float trans_x, float trans_y, float trans_z, OpenRaveRobot* robot)
606 {
607  try {
608  EnvironmentMutex::scoped_lock lock(__env->GetMutex());
609  KinBodyPtr kb = __env->GetKinBody(name);
610 
611  Transform transform = kb->GetTransform();
612  transform.trans = Vector(trans_x, trans_y, trans_z);
613 
614  if( robot ) {
615  Transform robotTrans = robot->get_robot_ptr()->GetTransform();
616  transform.trans += robotTrans.trans;
617  }
618 
619  kb->SetTransform(transform);
620  } catch(const OpenRAVE::openrave_exception &e) {
621  if(__logger)
622  __logger->log_warn("OpenRAVE Environment", "Could not move Object '%s'. Ex:%s", name.c_str(), e.what());
623  return false;
624  }
625 
626  return true;
627 }
628 
629 /** Rotate object by a quaternion.
630  * @param name name of the object
631  * @param quat_x x value of quaternion
632  * @param quat_y y value of quaternion
633  * @param quat_z z value of quaternion
634  * @param quat_w w value of quaternion
635  * @return true if successful
636  */
637 bool
638 OpenRaveEnvironment::rotate_object(const std::string& name, float quat_x, float quat_y, float quat_z, float quat_w)
639 {
640  try {
641  EnvironmentMutex::scoped_lock lock(__env->GetMutex());
642  KinBodyPtr kb = __env->GetKinBody(name);
643 
644  Vector quat(quat_w, quat_x, quat_y, quat_z);
645 
646  Transform transform = kb->GetTransform();
647  transform.rot = quat;
648 
649  kb->SetTransform(transform);
650  } catch(const OpenRAVE::openrave_exception &e) {
651  if(__logger)
652  __logger->log_warn("OpenRAVE Environment", "Could not rotate Object '%s'. Ex:%s", name.c_str(), e.what());
653  return false;
654  }
655 
656  return true;
657 }
658 
659 /** Rotate object along its axis.
660  * Rotation angles should be given in radians.
661  * @param name name of the object
662  * @param rot_x 1st rotation, along x-axis
663  * @param rot_y 2nd rotation, along y-axis
664  * @param rot_z 3rd rotation, along z-axis
665  * @return true if successful
666  */
667 bool
668 OpenRaveEnvironment::rotate_object(const std::string& name, float rot_x, float rot_y, float rot_z)
669 {
670  Vector q1 = quatFromAxisAngle(Vector(rot_x, 0.f, 0.f));
671  Vector q2 = quatFromAxisAngle(Vector(0.f, rot_y, 0.f));
672  Vector q3 = quatFromAxisAngle(Vector(0.f, 0.f, rot_z));
673 
674  Vector q12 = quatMultiply (q1, q2);
675  Vector quat = quatMultiply (q12, q3);
676 
677  return rotate_object(name, quat.x, quat.y, quat.z, quat.w);
678 }
679 
680 } // end of namespace fawkes
float z() const
Convenience getter to obtain the third element.
Definition: vector.cpp:248
virtual void run_planner(OpenRaveRobot *robot, float sampling=0.01f)
Plan collision-free path for current and target manipulator configuration of a OpenRaveRobot robot...
virtual bool display_planned_movements() const
Getter for __display_planned_movements.
Definition: robot.cpp:234
Target: OpenRAVE::IkParameterization string.
Definition: types.h:53
virtual void start_viewer()
Starts the qt viewer in a separate thread.
virtual void lock()
Lock the environment to prevent changes.
float qx
x value of quaternion
Definition: types.h:71
~OpenRaveEnvironment()
Destructor.
Definition: environment.cpp:82
float x
translation on x-axis
Definition: types.h:68
Target: relative endeffector translation, based on arm extension.
Definition: types.h:52
float qz
z value of quaternion
Definition: types.h:73
A simple column vector.
Definition: vector.h:31
virtual OpenRAVE::RobotBasePtr get_robot_ptr() const
Returns RobotBasePtr for uses in other classes.
Definition: robot.cpp:489
float x() const
Convenience getter to obtain the first element.
Definition: vector.cpp:192
virtual bool rename_object(const std::string &name, const std::string &new_name)
Rename object.
virtual OpenRAVE::ModuleBasePtr get_basemanip() const
Return BaseManipulation Module-Pointer.
Definition: robot.cpp:560
virtual OpenRAVE::EnvironmentBasePtr get_env_ptr() const
Get EnvironmentBasePtr.
target_type_t type
target type
Definition: types.h:77
Target: absolute endeffector translation and rotation.
Definition: types.h:50
std::string plannerparams
additional string to be passed to planner, i.e.
Definition: types.h:79
virtual bool delete_object(const std::string &name)
Remove object from environment.
virtual void create()
Create and lock the environment.
Definition: environment.cpp:89
float z
translation on z-axis
Definition: types.h:70
virtual bool add_object(const std::string &name, const std::string &filename)
Add an object to the environment.
virtual void load_IK_solver(OpenRaveRobot *robot, OpenRAVE::IkParameterizationType iktype=OpenRAVE::IKP_Transform6D)
Autogenerate IKfast IK solver for robot.
OpenRaveManipulator * manip
target manipulator configuration
Definition: types.h:76
void get_angles(std::vector< T > &to) const
Get motor angles of OpenRAVE model.
Definition: manipulator.h:67
float y
translation on y-axis
Definition: types.h:69
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.
virtual OpenRAVE::PlannerBase::PlannerParametersPtr get_planner_params() const
Updates planner parameters and return pointer to it.
Definition: robot.cpp:516
OpenRAVE Robot class.
Definition: robot.h:41
Base class for exceptions in Fawkes.
Definition: exception.h:36
Target: relative endeffector translation, based on robot&#39;s coordinate system.
Definition: types.h:51
OpenRAVE::IkParameterization ikparam
OpenRAVE::IkParameterization; each target is implicitly transformed to one by OpenRAVE.
Definition: types.h:78
Struct containing information about the current target.
Definition: types.h:67
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual bool move_object(const std::string &name, float trans_x, float trans_y, float trans_z, OpenRaveRobot *robot=NULL)
Move object in the environment.
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.
virtual void add_robot(const std::string &filename)
Add a robot into the scene.
float qw
w value of quaternion
Definition: types.h:74
void SetViewer(OpenRAVE::EnvironmentBasePtr env, const std::string &viewername)
Sets and loads a viewer for OpenRAVE.
Definition: environment.cpp:53
virtual std::vector< std::vector< OpenRAVE::dReal > > * get_trajectory() const
Return pointer to trajectory of motion from __manip to __target.manip with OpenRAVE-model angle forma...
Definition: robot.cpp:532
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void destroy()
Destroy the environment.
Expected parameter is missing.
Definition: software.h:82
virtual void disable_debug()
Disable debugging messages of OpenRAVE.
Target: motor joint values.
Definition: types.h:49
float qy
y value of quaternion
Definition: types.h:72
float y() const
Convenience getter to obtain the second element.
Definition: vector.cpp:220
virtual void run_graspplanning(const std::string &target_name, OpenRaveRobot *robot, float sampling=0.01f)
Run graspplanning script for a given target.
virtual target_t get_target() const
Get target.
Definition: robot.cpp:498
Interface for logging.
Definition: logger.h:34