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.hpp> 35 #include <boost/thread/thread.hpp> 37 #include <openrave-core.h> 40 using namespace OpenRAVE;
50 run_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());
159 OpenRaveEnvironment::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);
208 EnvironmentMutex::scoped_lock(env_->GetMutex());
211 logger_->
log_debug(name(),
"Robot '%s' added to environment.", robot->GetName().c_str());
212 }
catch (openrave_exception &e) {
215 "Could not add robot '%s' to environment. OpenRAVE error:%s",
216 robot->GetName().c_str(),
217 e.message().c_str());
231 EnvironmentMutex::scoped_lock(env_->GetMutex());
232 robot = env_->ReadRobotXMLFile(filename);
239 "%s: Robot '%s' could not be loaded. Check xml file/path.", name(), filename.c_str());
241 logger_->
log_debug(name(),
"Robot '%s' loaded.", robot->GetName().c_str());
259 OpenRAVE::EnvironmentBasePtr
275 if (viewer_thread_) {
276 viewer_thread_->join();
277 delete viewer_thread_;
278 viewer_thread_ = NULL;
285 viewer_running_ =
true;
286 viewer_thread_ =
new boost::thread(boost::bind(
run_viewer, env_,
"qtcoin", &viewer_running_));
287 }
catch (
const openrave_exception &e) {
288 viewer_running_ =
false;
290 logger_->
log_error(name(),
"Could not load viewr. Ex:%s", e.what());
301 OpenRAVE::IkParameterizationType iktype)
303 EnvironmentMutex::scoped_lock(env_->GetMutex());
305 RobotBasePtr robotBase = robot->get_robot_ptr();
307 std::stringstream ssin, ssout;
308 ssin <<
"LoadIKFastSolver " << robotBase->GetName() <<
" " << (int)iktype;
311 if (!mod_ikfast_->SendCommand(ssout, ssin))
324 EnvironmentMutex::scoped_lock lock(env_->GetMutex());
326 robot->get_planner_params();
338 ModuleBasePtr basemanip = robot->get_basemanip();
339 target_t target = robot->get_target();
340 std::stringstream cmdin, cmdout;
341 cmdin << std::setprecision(std::numeric_limits<dReal>::digits10 + 1);
342 cmdout << std::setprecision(std::numeric_limits<dReal>::digits10 + 1);
345 Transform t = robot->get_robot_ptr()->GetActiveManipulator()->GetEndEffectorTransform();
348 Vector dir(target.
y, target.
z, target.
x);
349 TransformMatrix mat = matrixFromQuat(t.rot);
350 dir = mat.rotate(dir);
357 switch (target.
type) {
361 cmdin <<
"MoveActiveJoints goal";
363 std::vector<dReal> v;
365 for (
size_t i = 0; i < v.size(); ++i) {
366 cmdin <<
" " << v[i];
372 cmdin <<
"MoveToHandPosition ikparam";
373 cmdin <<
" " << target.
ikparam;
377 cmdin <<
"MoveToHandPosition pose";
378 cmdin <<
" " << target.
qw <<
" " << target.
qx <<
" " << target.
qy <<
" " << target.
qz;
379 cmdin <<
" " << target.
x <<
" " << target.
y <<
" " << target.
z;
384 cmdin <<
"MoveHandStraight direction";
385 cmdin <<
" " << target.
x <<
" " << target.
y <<
" " << target.
z;
387 dReal stepsize = 0.005;
388 dReal length = sqrt(target.
x * target.
x + target.
y * target.
y + target.
z * target.
z);
389 int minsteps = (int)(length / stepsize);
393 cmdin <<
" stepsize " << stepsize;
394 cmdin <<
" minsteps " << minsteps;
395 cmdin <<
" maxsteps " << (int)(length / stepsize);
406 cmdin <<
" execute 0";
407 cmdin <<
" outputtraj";
409 logger_->
log_debug(name(),
"Planner: basemanip cmdin:%s", cmdin.str().c_str());
412 success = basemanip->SendCommand(cmdout, cmdin);
413 }
catch (openrave_exception &e) {
414 throw fawkes::Exception(
"%s: Planner: basemanip command failed. Ex%s", name(), e.what());
419 logger_->
log_debug(name(),
"Planner: path planned");
422 logger_->
log_debug(name(),
"Planner: planned. cmdout:%s", cmdout.str().c_str());
425 TrajectoryBasePtr traj = RaveCreateTrajectory(env_,
"");
426 traj->Init(robot->get_robot_ptr()->GetActiveConfigurationSpecification());
427 if (!traj->deserialize(cmdout))
431 std::vector<std::vector<dReal>> *trajRobot = robot->get_trajectory();
433 for (dReal time = 0; time <= traj->GetDuration(); time += (dReal)sampling) {
434 std::vector<dReal> point;
435 traj->Sample(point, time);
436 trajRobot->push_back(point);
440 if (viewer_running_) {
442 graph_handle_.clear();
444 RobotBasePtr tmp_robot = robot->get_robot_ptr();
445 RobotBase::RobotStateSaver saver(
447 for (std::vector<std::vector<dReal>>::iterator it = trajRobot->begin();
448 it != trajRobot->end();
450 tmp_robot->SetActiveDOFValues((*it));
451 const OpenRAVE::Vector &trans =
452 tmp_robot->GetActiveManipulator()->GetEndEffectorTransform().trans;
453 float transa[4] = {(float)trans.x, (
float)trans.y, (float)trans.z, (
float)trans.w};
454 graph_handle_.push_back(env_->plot3(transa, 1, 0, 2.f, Vector(1.f, 0.f, 0.f, 1.f)));
459 if (robot->display_planned_movements()) {
460 robot->get_robot_ptr()->GetController()->SetPath(traj);
479 std::string filename = SRCDIR
"/python/graspplanning.py";
480 std::string funcname =
"runGrasp";
482 TrajectoryBasePtr traj = RaveCreateTrajectory(env_,
"");
483 traj->Init(robot->get_robot_ptr()->GetActiveConfigurationSpecification());
485 FILE *py_file = fopen(filename.c_str(),
"r");
487 throw fawkes::Exception(
"%s: Graspplanning: opening python file failed", name());
492 PyGILState_STATE gil_state = PyGILState_Ensure();
493 PyThreadState * cur_state =
495 PyThreadState *int_state = Py_NewInterpreter();
501 PyObject *py_main = PyImport_AddModule(
"main___");
505 Py_EndInterpreter(int_state);
506 PyThreadState_Swap(cur_state);
507 PyGILState_Release(gil_state);
509 throw fawkes::Exception(
"%s: Graspplanning: Python reference 'main__'_ does not exist.",
512 PyObject *py_dict = PyModule_GetDict(py_main);
518 "%s: Graspplanning: Python reference 'main__'_ does not have a dictionary.", name());
522 int py_module = PyRun_SimpleFile(py_file, filename.c_str());
526 PyObject *py_func = PyDict_GetItemString(py_dict, funcname.c_str());
527 if (py_func && PyCallable_Check(py_func)) {
529 PyObject *py_args = PyTuple_New(3);
531 PyObject *py_value_env_id = PyInt_FromLong(RaveGetEnvironmentId(env_));
532 PyObject *py_value_robot_name =
533 PyString_FromString(robot->get_robot_ptr()->GetName().c_str());
534 PyObject *py_value_target_name = PyString_FromString(target_name.c_str());
535 PyTuple_SetItem(py_args,
538 PyTuple_SetItem(py_args,
540 py_value_robot_name);
541 PyTuple_SetItem(py_args,
543 py_value_target_name);
545 PyObject *py_value = PyObject_CallObject(py_func, py_args);
548 if (py_value != NULL) {
549 if (!PyString_Check(py_value)) {
555 std::stringstream resval;
556 resval << std::setprecision(std::numeric_limits<dReal>::digits10 + 1);
557 resval << PyString_AsString(py_value);
558 if (!traj->deserialize(resval)) {
562 throw fawkes::Exception(
"%s: Graspplanning: Reading trajectory data failed.", name());
572 if (PyErr_Occurred())
582 throw fawkes::Exception(
"%s: Graspplanning: Loading python file failed.", name());
585 Py_EndInterpreter(int_state);
586 PyThreadState_Swap(cur_state);
587 PyGILState_Release(gil_state);
592 logger_->
log_debug(name(),
"Graspplanning: path planned");
595 planningutils::RetimeActiveDOFTrajectory(traj, robot->get_robot_ptr());
598 std::vector<std::vector<dReal>> *trajRobot = robot->get_trajectory();
600 for (dReal time = 0; time <= traj->GetDuration(); time += (dReal)sampling) {
601 std::vector<dReal> point;
602 traj->Sample(point, time);
603 trajRobot->push_back(point);
607 if (viewer_running_) {
609 graph_handle_.clear();
611 RobotBasePtr tmp_robot = robot->get_robot_ptr();
612 RobotBase::RobotStateSaver saver(
614 for (std::vector<std::vector<dReal>>::iterator it = trajRobot->begin();
615 it != trajRobot->end();
617 tmp_robot->SetActiveDOFValues((*it));
618 const OpenRAVE::Vector &trans =
619 tmp_robot->GetActiveManipulator()->GetEndEffectorTransform().trans;
620 float transa[4] = {(float)trans.x, (
float)trans.y, (float)trans.z, (
float)trans.w};
621 graph_handle_.push_back(env_->plot3(transa, 1, 0, 2.f, Vector(1.f, 0.f, 0.f, 1.f)));
626 if (robot->display_planned_movements()) {
627 robot->get_robot_ptr()->GetController()->SetPath(traj);
641 EnvironmentMutex::scoped_lock lock(env_->GetMutex());
642 KinBodyPtr kb = env_->ReadKinBodyXMLFile(filename);
645 }
catch (
const OpenRAVE::openrave_exception &e) {
647 logger_->
log_warn(this->name(),
"Could not add Object '%s'. Ex:%s", name.c_str(), e.what());
662 EnvironmentMutex::scoped_lock lock(env_->GetMutex());
663 KinBodyPtr kb = env_->GetKinBody(name);
665 }
catch (
const OpenRAVE::openrave_exception &e) {
668 "Could not delete Object '%s'. Ex:%s",
684 EnvironmentMutex::scoped_lock lock(env_->GetMutex());
685 std::vector<KinBodyPtr> bodies;
686 env_->GetBodies(bodies);
688 for (std::vector<KinBodyPtr>::iterator it = bodies.begin(); it != bodies.end(); ++it) {
689 if (!(*it)->IsRobot())
692 }
catch (
const OpenRAVE::openrave_exception &e) {
694 logger_->
log_warn(this->name(),
"Could not delete all objects. Ex:%s", e.what());
710 EnvironmentMutex::scoped_lock lock(env_->GetMutex());
711 KinBodyPtr kb = env_->GetKinBody(name);
712 kb->SetName(new_name);
713 }
catch (
const OpenRAVE::openrave_exception &e) {
716 "Could not rename Object '%s' to '%s'. Ex:%s",
741 EnvironmentMutex::scoped_lock lock(env_->GetMutex());
742 KinBodyPtr kb = env_->GetKinBody(name);
744 Transform transform = kb->GetTransform();
745 transform.trans = Vector(trans_x, trans_y, trans_z);
747 kb->SetTransform(transform);
748 }
catch (
const OpenRAVE::openrave_exception &e) {
750 logger_->
log_warn(this->name(),
"Could not move Object '%s'. Ex:%s", name.c_str(), e.what());
776 EnvironmentMutex::scoped_lock(env_->GetMutex());
777 t = robot->get_robot_ptr()->GetTransform();
779 return move_object(name, trans_x + t.trans[1], trans_y + t.trans[2], trans_z + t.trans[3]);
798 EnvironmentMutex::scoped_lock lock(env_->GetMutex());
799 KinBodyPtr kb = env_->GetKinBody(name);
801 Vector quat(quat_w, quat_x, quat_y, quat_z);
803 Transform transform = kb->GetTransform();
804 transform.rot = quat;
806 kb->SetTransform(transform);
807 }
catch (
const OpenRAVE::openrave_exception &e) {
810 "Could not rotate Object '%s'. Ex:%s",
830 Vector q1 = quatFromAxisAngle(Vector(rot_x, 0.f, 0.f));
831 Vector q2 = quatFromAxisAngle(Vector(0.f, rot_y, 0.f));
832 Vector q3 = quatFromAxisAngle(Vector(0.f, 0.f, rot_z));
834 Vector q12 = quatMultiply(q1, q2);
835 Vector quat = quatMultiply(q12, q3);
837 return rotate_object(name, quat[1], quat[2], quat[3], quat[0]);
849 EnvironmentMutex::scoped_lock lockold(env->get_env_ptr()->GetMutex());
850 EnvironmentMutex::scoped_lock lock(env_->GetMutex());
853 std::vector<KinBodyPtr> old_bodies, bodies;
854 env->get_env_ptr()->GetBodies(old_bodies);
855 env_->GetBodies(bodies);
858 std::vector<KinBodyPtr>::iterator old_body, body;
859 for (old_body = old_bodies.begin(); old_body != old_bodies.end(); ++old_body) {
860 if ((*old_body)->IsRobot())
864 for (body = bodies.begin(); body != bodies.end(); ++body) {
865 if ((*body)->IsRobot())
868 if ((*body)->GetName() == (*old_body)->GetName()
869 && (*body)->GetKinematicsGeometryHash() == (*old_body)->GetKinematicsGeometryHash()) {
875 if (body != bodies.end()) {
886 new_body = env_->ReadKinBodyData(empty,
"<KinBody></KinBody>");
887 new_body->Clone(*old_body, 0);
897 KinBody::KinBodyStateSaver saver(*old_body,
898 KinBody::Save_LinkTransformation | KinBody::Save_LinkEnable
899 | KinBody::Save_LinkVelocities);
900 saver.Restore(new_body);
904 KinBody::KinBodyStateSaver saver(*old_body, 0xffffffff);
905 saver.Restore(new_body);
910 for (body = bodies.begin(); body != bodies.end(); ++body) {
911 if ((*body)->IsRobot())
virtual bool move_object(const std::string &name, float trans_x, float trans_y, float trans_z)
Move object in the environment.
Target: OpenRAVE::IkParameterization string.
virtual void start_viewer()
Starts the qt viewer in a separate thread.
float qx
x value of quaternion
virtual ~OpenRaveEnvironment()
Destructor.
OpenRaveEnvironment(fawkes::Logger *logger=0)
Constructor.
virtual OpenRAVE::EnvironmentBasePtr get_env_ptr() const
Get EnvironmentBasePtr.
float x
translation on x-axis
Target: relative endeffector translation, based on arm extension.
Fawkes library namespace.
float qz
z value of quaternion
virtual bool rename_object(const std::string &name, const std::string &new_name)
Rename object.
target_type_t type
target type
Target: absolute endeffector translation and rotation.
virtual void set_name(const char *name)
Set name of environment.
std::string raw_cmd
raw command passed to the BaseManipulator module, e.g.
std::string plannerparams
additional string to be passed to planner, i.e.
virtual bool delete_object(const std::string &name)
Remove object from environment.
virtual void create()
Create and lock the environment.
float z
translation on z-axis
virtual bool add_object(const std::string &name, const std::string &filename)
Add an object to the environment.
Target: Raw string, passed to OpenRAVE's BaseManipulation module.
float y
translation on y-axis
virtual bool rotate_object(const std::string &name, float quat_x, float quat_y, float quat_z, float quat_w)
Rotate object by a quaternion.
void clear()
Set underlying instance to 0, decrementing reference count of existing instance appropriately.
Base class for exceptions in Fawkes.
OpenRaveEnvironment class.
Target: relative endeffector translation, based on robot's coordinate system.
OpenRAVE::IkParameterization ikparam
OpenRAVE::IkParameterization; each target is implicitly transformed to one by OpenRAVE.
virtual void load_IK_solver(OpenRaveRobotPtr &robot, OpenRAVE::IkParameterizationType iktype=OpenRAVE::IKP_Transform6D)
Autogenerate IKfast IK solver for robot.
virtual void run_graspplanning(const std::string &target_name, OpenRaveRobotPtr &robot, float sampling=0.01f)
Run graspplanning script for a given target.
Struct containing information about the current target.
virtual bool delete_all_objects()
Remove all objects from environment.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
virtual void enable_debug(OpenRAVE::DebugLevel level=OpenRAVE::Level_Debug)
Enable debugging messages of OpenRAVE.
void run_viewer(OpenRAVE::EnvironmentBasePtr env, const std::string &viewername, bool *running)
Sets and loads a viewer for OpenRAVE.
virtual void add_robot(const std::string &filename)
Add a robot into the scene.
float qw
w value of quaternion
virtual void run_planner(OpenRaveRobotPtr &robot, float sampling=0.01f)
Plan collision-free path for current and target manipulator configuration of a OpenRaveRobot robot.
RefPtr<> is a reference-counting shared smartpointer.
OpenRaveManipulatorPtr manip
target manipulator configuration
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void destroy()
Destroy the environment.
void get_angles(std::vector< T > &to) const
Get motor angles of OpenRAVE model.
virtual void clone_objects(OpenRaveEnvironmentPtr &env)
Clone all non-robot objects from a referenced OpenRaveEnvironment to this one.
Expected parameter is missing.
virtual void disable_debug()
Disable debugging messages of OpenRAVE.
Target: motor joint values.
float qy
y value of quaternion