24#include "environment.h"
26#include "manipulator.h"
29#include <core/exceptions/software.h>
30#include <logging/logger.h>
31#include <openrave/planningutils.h>
34#include <boost/bind/bind.hpp>
35#include <boost/thread/thread.hpp>
37#include <openrave-core.h>
40using namespace OpenRAVE;
50run_viewer(OpenRAVE::EnvironmentBasePtr env,
const std::string &viewername,
bool *running)
52 ViewerBasePtr viewer = RaveCreateViewer(env, viewername);
72: logger_(logger), viewer_thread_(0), viewer_running_(0)
83: logger_(src.logger_), viewer_thread_(0), viewer_running_(0)
85 env_ = src.env_->CloneSelf(OpenRAVE::Clone_Bodies);
91 planner_ = RaveCreatePlanner(env_,
"birrt");
93 throw fawkes::Exception(
"%s: Could not create planner. Error in OpenRAVE.", name());
96 mod_ikfast_ = RaveCreateModule(env_,
"ikfast");
97 env_->AddModule(mod_ikfast_,
"");
100 logger_->
log_debug(name(),
"Environment cloned from %s", src.name_.c_str());
114 env_ = RaveCreateEnvironment();
116 throw fawkes::Exception(
"%s: Could not create environment. Error in OpenRAVE.", name());
121 logger_->
log_debug(name(),
"Environment created");
124 EnvironmentMutex::scoped_lock(env_->GetMutex());
127 planner_ = RaveCreatePlanner(env_,
"birrt");
129 throw fawkes::Exception(
"%s: Could not create planner. Error in OpenRAVE.", name());
132 mod_ikfast_ = RaveCreateModule(env_,
"ikfast");
133 env_->AddModule(mod_ikfast_,
"");
140 if (viewer_thread_) {
141 viewer_thread_->detach();
142 viewer_thread_->join();
143 delete viewer_thread_;
144 viewer_thread_ = NULL;
150 logger_->
log_debug(name(),
"Environment destroyed");
151 }
catch (
const openrave_exception &e) {
153 logger_->
log_warn(name(),
"Could not destroy Environment. Ex:%s", e.what());
159OpenRaveEnvironment::name()
const
161 return name_str_.c_str();
172 n <<
"OpenRaveEnvironment"
175 n << RaveGetEnvironmentId(env_) <<
":";
180 logger_->
log_debug(name_str_.c_str(),
"Set environment name (previously '%s')", name_.c_str());
190 RaveSetDebugLevel(level);
197 RaveSetDebugLevel(Level_Fatal);
207 EnvironmentMutex::scoped_lock(env_->GetMutex());
210 logger_->
log_debug(name(),
"Robot '%s' added to environment.", robot->GetName().c_str());
211 }
catch (openrave_exception &e) {
214 "Could not add robot '%s' to environment. OpenRAVE error:%s",
215 robot->GetName().c_str(),
216 e.message().c_str());
229 EnvironmentMutex::scoped_lock(env_->GetMutex());
230 robot = env_->ReadRobotXMLFile(filename);
237 "%s: Robot '%s' could not be loaded. Check xml file/path.", name(), filename.c_str());
239 logger_->
log_debug(name(),
"Robot '%s' loaded.", robot->GetName().c_str());
256OpenRAVE::EnvironmentBasePtr
272 if (viewer_thread_) {
273 viewer_thread_->join();
274 delete viewer_thread_;
275 viewer_thread_ = NULL;
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;
287 logger_->
log_error(name(),
"Could not load viewr. Ex:%s", e.what());
298 OpenRAVE::IkParameterizationType iktype)
300 EnvironmentMutex::scoped_lock(env_->GetMutex());
302 RobotBasePtr robotBase = robot->get_robot_ptr();
304 std::stringstream ssin, ssout;
305 ssin <<
"LoadIKFastSolver " << robotBase->GetName() <<
" " << (int)iktype;
308 if (!mod_ikfast_->SendCommand(ssout, ssin))
321 EnvironmentMutex::scoped_lock lock(env_->GetMutex());
323 robot->get_planner_params();
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);
342 Transform t = robot->get_robot_ptr()->GetActiveManipulator()->GetEndEffectorTransform();
345 Vector dir(target.
y, target.
z, target.
x);
346 TransformMatrix mat = matrixFromQuat(t.rot);
347 dir = mat.rotate(dir);
354 switch (target.
type) {
358 cmdin <<
"MoveActiveJoints goal";
360 std::vector<dReal> v;
362 for (
size_t i = 0; i < v.size(); ++i) {
363 cmdin <<
" " << v[i];
369 cmdin <<
"MoveToHandPosition ikparam";
370 cmdin <<
" " << target.
ikparam;
374 cmdin <<
"MoveToHandPosition pose";
375 cmdin <<
" " << target.
qw <<
" " << target.
qx <<
" " << target.
qy <<
" " << target.
qz;
376 cmdin <<
" " << target.
x <<
" " << target.
y <<
" " << target.
z;
381 cmdin <<
"MoveHandStraight direction";
382 cmdin <<
" " << target.
x <<
" " << target.
y <<
" " << target.
z;
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);
390 cmdin <<
" stepsize " << stepsize;
391 cmdin <<
" minsteps " << minsteps;
392 cmdin <<
" maxsteps " << (int)(length / stepsize);
403 cmdin <<
" execute 0";
404 cmdin <<
" outputtraj";
406 logger_->
log_debug(name(),
"Planner: basemanip cmdin:%s", cmdin.str().c_str());
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());
416 logger_->
log_debug(name(),
"Planner: path planned");
419 logger_->
log_debug(name(),
"Planner: planned. cmdout:%s", cmdout.str().c_str());
422 TrajectoryBasePtr traj = RaveCreateTrajectory(env_,
"");
423 traj->Init(robot->get_robot_ptr()->GetActiveConfigurationSpecification());
424 if (!traj->deserialize(cmdout))
428 std::vector<std::vector<dReal>> *trajRobot = robot->get_trajectory();
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);
437 if (viewer_running_) {
439 graph_handle_.clear();
441 RobotBasePtr tmp_robot = robot->get_robot_ptr();
442 RobotBase::RobotStateSaver saver(
444 for (std::vector<std::vector<dReal>>::iterator it = trajRobot->begin();
445 it != trajRobot->end();
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)));
456 if (robot->display_planned_movements()) {
457 robot->get_robot_ptr()->GetController()->SetPath(traj);
476 std::string filename = SRCDIR
"/python/graspplanning.py";
477 std::string funcname =
"runGrasp";
479 TrajectoryBasePtr traj = RaveCreateTrajectory(env_,
"");
480 traj->Init(robot->get_robot_ptr()->GetActiveConfigurationSpecification());
482 FILE *py_file = fopen(filename.c_str(),
"r");
484 throw fawkes::Exception(
"%s: Graspplanning: opening python file failed", name());
489 PyGILState_STATE gil_state = PyGILState_Ensure();
490 PyThreadState * cur_state =
492 PyThreadState *int_state = Py_NewInterpreter();
498 PyObject *py_main = PyImport_AddModule(
"__main__");
502 Py_EndInterpreter(int_state);
503 PyThreadState_Swap(cur_state);
504 PyGILState_Release(gil_state);
506 throw fawkes::Exception(
"%s: Graspplanning: Python reference 'main__'_ does not exist.",
509 PyObject *py_dict = PyModule_GetDict(py_main);
515 "%s: Graspplanning: Python reference 'main__'_ does not have a dictionary.", name());
519 int py_module = PyRun_SimpleFile(py_file, filename.c_str());
523 PyObject *py_func = PyDict_GetItemString(py_dict, funcname.c_str());
524 if (py_func && PyCallable_Check(py_func)) {
526 PyObject *py_args = PyTuple_New(3);
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,
535 PyTuple_SetItem(py_args,
537 py_value_robot_name);
538 PyTuple_SetItem(py_args,
540 py_value_target_name);
542 PyObject *py_value = PyObject_CallObject(py_func, py_args);
545 if (py_value != NULL) {
546 if (!PyString_Check(py_value)) {
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)) {
559 throw fawkes::Exception(
"%s: Graspplanning: Reading trajectory data failed.", name());
569 if (PyErr_Occurred())
579 throw fawkes::Exception(
"%s: Graspplanning: Loading python file failed.", name());
582 Py_EndInterpreter(int_state);
583 PyThreadState_Swap(cur_state);
584 PyGILState_Release(gil_state);
589 logger_->
log_debug(name(),
"Graspplanning: path planned");
592 planningutils::RetimeActiveDOFTrajectory(traj, robot->get_robot_ptr());
595 std::vector<std::vector<dReal>> *trajRobot = robot->get_trajectory();
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);
604 if (viewer_running_) {
606 graph_handle_.clear();
608 RobotBasePtr tmp_robot = robot->get_robot_ptr();
609 RobotBase::RobotStateSaver saver(
611 for (std::vector<std::vector<dReal>>::iterator it = trajRobot->begin();
612 it != trajRobot->end();
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)));
623 if (robot->display_planned_movements()) {
624 robot->get_robot_ptr()->GetController()->SetPath(traj);
638 EnvironmentMutex::scoped_lock lock(env_->GetMutex());
639 KinBodyPtr kb = env_->ReadKinBodyXMLFile(filename);
642 }
catch (
const OpenRAVE::openrave_exception &e) {
644 logger_->
log_warn(this->name(),
"Could not add Object '%s'. Ex:%s", name.c_str(), e.what());
659 EnvironmentMutex::scoped_lock lock(env_->GetMutex());
660 KinBodyPtr kb = env_->GetKinBody(name);
662 }
catch (
const OpenRAVE::openrave_exception &e) {
665 "Could not delete Object '%s'. Ex:%s",
681 EnvironmentMutex::scoped_lock lock(env_->GetMutex());
682 std::vector<KinBodyPtr> bodies;
683 env_->GetBodies(bodies);
685 for (std::vector<KinBodyPtr>::iterator it = bodies.begin(); it != bodies.end(); ++it) {
686 if (!(*it)->IsRobot())
689 }
catch (
const OpenRAVE::openrave_exception &e) {
691 logger_->
log_warn(this->name(),
"Could not delete all objects. Ex:%s", e.what());
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) {
713 "Could not rename Object '%s' to '%s'. Ex:%s",
738 EnvironmentMutex::scoped_lock lock(env_->GetMutex());
739 KinBodyPtr kb = env_->GetKinBody(name);
741 Transform transform = kb->GetTransform();
742 transform.trans = Vector(trans_x, trans_y, trans_z);
744 kb->SetTransform(transform);
745 }
catch (
const OpenRAVE::openrave_exception &e) {
747 logger_->
log_warn(this->name(),
"Could not move Object '%s'. Ex:%s", name.c_str(), e.what());
773 EnvironmentMutex::scoped_lock(env_->GetMutex());
774 t = robot->get_robot_ptr()->GetTransform();
776 return move_object(name, trans_x + t.trans[1], trans_y + t.trans[2], trans_z + t.trans[3]);
795 EnvironmentMutex::scoped_lock lock(env_->GetMutex());
796 KinBodyPtr kb = env_->GetKinBody(name);
798 Vector quat(quat_w, quat_x, quat_y, quat_z);
800 Transform transform = kb->GetTransform();
801 transform.rot = quat;
803 kb->SetTransform(transform);
804 }
catch (
const OpenRAVE::openrave_exception &e) {
807 "Could not rotate Object '%s'. Ex:%s",
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));
831 Vector q12 = quatMultiply(q1, q2);
832 Vector quat = quatMultiply(q12, q3);
834 return rotate_object(name, quat[1], quat[2], quat[3], quat[0]);
846 EnvironmentMutex::scoped_lock lockold(env->get_env_ptr()->GetMutex());
847 EnvironmentMutex::scoped_lock lock(env_->GetMutex());
850 std::vector<KinBodyPtr> old_bodies, bodies;
851 env->get_env_ptr()->GetBodies(old_bodies);
852 env_->GetBodies(bodies);
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())
861 for (body = bodies.begin(); body != bodies.end(); ++body) {
862 if ((*body)->IsRobot())
865 if ((*body)->GetName() == (*old_body)->GetName()
866 && (*body)->GetKinematicsGeometryHash() == (*old_body)->GetKinematicsGeometryHash()) {
872 if (body != bodies.end()) {
883 new_body = env_->ReadKinBodyData(empty,
"<KinBody></KinBody>");
884 new_body->Clone(*old_body, 0);
894 KinBody::KinBodyStateSaver saver(*old_body,
895 KinBody::Save_LinkTransformation | KinBody::Save_LinkEnable
896 | KinBody::Save_LinkVelocities);
897 saver.Restore(new_body);
901 KinBody::KinBodyStateSaver saver(*old_body, 0xffffffff);
902 saver.Restore(new_body);
907 for (body = bodies.begin(); body != bodies.end(); ++body) {
908 if ((*body)->IsRobot())
Base class for exceptions in Fawkes.
Expected parameter is missing.
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.
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.
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.
RefPtr<> is a reference-counting shared smartpointer.
void clear()
Set underlying instance to 0, decrementing reference count of existing instance appropriately.
Fawkes library namespace.
@ TARGET_TRANSFORM
Target: absolute endeffector translation and rotation.
@ TARGET_IKPARAM
Target: OpenRAVE::IkParameterization string.
@ TARGET_RELATIVE
Target: relative endeffector translation, based on robot's coordinate system.
@ TARGET_RAW
Target: Raw string, passed to OpenRAVE's BaseManipulation module.
@ TARGET_JOINTS
Target: motor joint values.
@ TARGET_RELATIVE_EXT
Target: relative endeffector translation, based on arm extension.
void run_viewer(OpenRAVE::EnvironmentBasePtr env, const std::string &viewername, bool *running)
Sets and loads a viewer for OpenRAVE.
Struct containing information about the current target.
float qz
z value of quaternion
float x
translation on x-axis
float qy
y value of quaternion
OpenRaveManipulatorPtr manip
target manipulator configuration
OpenRAVE::IkParameterization ikparam
OpenRAVE::IkParameterization; each target is implicitly transformed to one by OpenRAVE.
float qw
w value of quaternion
target_type_t type
target type
float z
translation on z-axis
std::string raw_cmd
raw command passed to the BaseManipulator module, e.g.
float y
translation on y-axis
std::string plannerparams
additional string to be passed to planner, i.e.
float qx
x value of quaternion