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/bind.hpp>
35#include <boost/thread/thread.hpp>
36#include <cstdio>
37#include <openrave-core.h>
38#include <sstream>
39
40using namespace OpenRAVE;
41namespace 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 */
49void
50run_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 */
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. */
110void
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. */
137void
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. */
158const char *
159OpenRaveEnvironment::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 */
168void
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 */
187void
188OpenRaveEnvironment::enable_debug(OpenRAVE::DebugLevel level)
189{
190 RaveSetDebugLevel(level);
191}
192
193/** Disable debugging messages of OpenRAVE. */
194void
196{
197 RaveSetDebugLevel(Level_Fatal);
198}
199
200/** Add a robot into the scene.
201 * @param robot RobotBasePtr of robot to add
202 */
203void
204OpenRaveEnvironment::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 */
223void
224OpenRaveEnvironment::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 */
247void
249{
250 add_robot(robot->get_robot_ptr());
251}
252
253/** Get EnvironmentBasePtr.
254 * @return EnvironmentBasePtr in use
255 */
256OpenRAVE::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 */
266void
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 */
296void
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 */
317void
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 */
471void
472OpenRaveEnvironment::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 */
634bool
635OpenRaveEnvironment::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 */
655bool
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 */
677bool
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 */
703bool
704OpenRaveEnvironment::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 */
731bool
732OpenRaveEnvironment::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 */
763bool
764OpenRaveEnvironment::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 */
787bool
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 */
824bool
825OpenRaveEnvironment::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 */
842void
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
Base class for exceptions in Fawkes.
Definition: exception.h:36
Expected parameter is missing.
Definition: software.h:80
Interface for logging.
Definition: logger.h:42
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
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.
OpenRaveEnvironment class.
Definition: environment.h:44
virtual void create()
Create and lock the environment.
virtual bool move_object(const std::string &name, float trans_x, float trans_y, float trans_z)
Move object in the environment.
virtual void clone_objects(OpenRaveEnvironmentPtr &env)
Clone all non-robot objects from a referenced OpenRaveEnvironment to this one.
virtual void run_graspplanning(const std::string &target_name, OpenRaveRobotPtr &robot, float sampling=0.01f)
Run graspplanning script for a given target.
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 void add_robot(const std::string &filename)
Add a robot into the scene.
virtual bool delete_all_objects()
Remove all objects from environment.
virtual ~OpenRaveEnvironment()
Destructor.
virtual void load_IK_solver(OpenRaveRobotPtr &robot, OpenRAVE::IkParameterizationType iktype=OpenRAVE::IKP_Transform6D)
Autogenerate IKfast IK solver for robot.
virtual void enable_debug(OpenRAVE::DebugLevel level=OpenRAVE::Level_Debug)
Enable debugging messages of OpenRAVE.
virtual bool delete_object(const std::string &name)
Remove object from environment.
virtual OpenRAVE::EnvironmentBasePtr get_env_ptr() const
Get EnvironmentBasePtr.
virtual void disable_debug()
Disable debugging messages of OpenRAVE.
virtual void run_planner(OpenRaveRobotPtr &robot, float sampling=0.01f)
Plan collision-free path for current and target manipulator configuration of a OpenRaveRobot robot.
virtual void start_viewer()
Starts the qt viewer in a separate thread.
OpenRaveEnvironment(fawkes::Logger *logger=0)
Constructor.
Definition: environment.cpp:71
virtual void set_name(const char *name)
Set name of environment.
virtual bool rename_object(const std::string &name, const std::string &new_name)
Rename object.
virtual void destroy()
Destroy the environment.
virtual bool add_object(const std::string &name, const std::string &filename)
Add an object to the environment.
void get_angles(std::vector< T > &to) const
Get motor angles of OpenRAVE model.
Definition: manipulator.h:83
RefPtr<> is a reference-counting shared smartpointer.
Definition: refptr.h:50
void clear()
Set underlying instance to 0, decrementing reference count of existing instance appropriately.
Definition: refptr.h:447
Fawkes library namespace.
@ TARGET_TRANSFORM
Target: absolute endeffector translation and rotation.
Definition: types.h:54
@ TARGET_IKPARAM
Target: OpenRAVE::IkParameterization string.
Definition: types.h:57
@ TARGET_RELATIVE
Target: relative endeffector translation, based on robot's coordinate system.
Definition: types.h:55
@ TARGET_RAW
Target: Raw string, passed to OpenRAVE's BaseManipulation module.
Definition: types.h:58
@ TARGET_JOINTS
Target: motor joint values.
Definition: types.h:53
@ TARGET_RELATIVE_EXT
Target: relative endeffector translation, based on arm extension.
Definition: types.h:56
void run_viewer(OpenRAVE::EnvironmentBasePtr env, const std::string &viewername, bool *running)
Sets and loads a viewer for OpenRAVE.
Definition: environment.cpp:50
Struct containing information about the current target.
Definition: types.h:72
float qz
z value of quaternion
Definition: types.h:78
float x
translation on x-axis
Definition: types.h:73
float qy
y value of quaternion
Definition: types.h:77
OpenRaveManipulatorPtr manip
target manipulator configuration
Definition: types.h:81
OpenRAVE::IkParameterization ikparam
OpenRAVE::IkParameterization; each target is implicitly transformed to one by OpenRAVE.
Definition: types.h:84
float qw
w value of quaternion
Definition: types.h:79
target_type_t type
target type
Definition: types.h:82
float z
translation on z-axis
Definition: types.h:75
std::string raw_cmd
raw command passed to the BaseManipulator module, e.g.
Definition: types.h:88
float y
translation on y-axis
Definition: types.h:74
std::string plannerparams
additional string to be passed to planner, i.e.
Definition: types.h:86
float qx
x value of quaternion
Definition: types.h:76