26 #include "environment.h" 28 #include "manipulator.h" 30 #include <logging/logger.h> 31 #include <core/exceptions/software.h> 33 #include <openrave-core.h> 34 #include <openrave/planningutils.h> 35 #include <boost/thread/thread.hpp> 36 #include <boost/bind.hpp> 55 run_viewer(OpenRAVE::EnvironmentBasePtr env,
const std::string& viewername,
bool* running)
57 ViewerBasePtr viewer = RaveCreateViewer(env, viewername);
91 : __logger( src.__logger ),
95 __env = src.__env->CloneSelf(OpenRAVE::Clone_Bodies);
101 __planner = RaveCreatePlanner(__env,
"birrt");
103 throw fawkes::Exception(
"%s: Could not create planner. Error in OpenRAVE.", name());
106 __mod_ikfast = RaveCreateModule(__env,
"ikfast");
107 __env->AddModule(__mod_ikfast,
"");
110 __logger->
log_debug(name(),
"Environment cloned from %s", src.__name.c_str());
124 __env = RaveCreateEnvironment();
126 throw fawkes::Exception(
"%s: Could not create environment. Error in OpenRAVE.", name());
131 __logger->
log_debug(name(),
"Environment created");
134 EnvironmentMutex::scoped_lock( __env->GetMutex());
137 __planner = RaveCreatePlanner(__env,
"birrt");
139 throw fawkes::Exception(
"%s: Could not create planner. Error in OpenRAVE.", name());
142 __mod_ikfast = RaveCreateModule(__env,
"ikfast");
143 __env->AddModule(__mod_ikfast,
"");
150 if( __viewer_thread ) {
151 __viewer_thread->detach();
152 __viewer_thread->join();
153 delete __viewer_thread;
154 __viewer_thread=NULL;
160 __logger->
log_debug(name(),
"Environment destroyed");
161 }
catch(
const openrave_exception& e) {
163 __logger->
log_warn(name(),
"Could not destroy Environment. Ex:%s", e.what());
169 OpenRaveEnvironment::name()
const 171 return __name_str.c_str();
182 n <<
"OpenRaveEnvironment" <<
"[";
184 n << RaveGetEnvironmentId(__env) <<
":";
186 __name_str = n.str();
189 __logger->
log_debug(__name_str.c_str(),
"Set environment name (previously '%s')", __name.c_str());
199 RaveSetDebugLevel(level);
206 RaveSetDebugLevel(Level_Fatal);
217 EnvironmentMutex::scoped_lock( __env->GetMutex());
220 __logger->
log_debug(name(),
"Robot '%s' added to environment.", robot->GetName().c_str());
221 }
catch(openrave_exception &e) {
223 __logger->
log_debug(name(),
"Could not add robot '%s' to environment. OpenRAVE error:%s", robot->GetName().c_str(), e.message().c_str());
237 EnvironmentMutex::scoped_lock( __env->GetMutex());
238 robot = __env->ReadRobotXMLFile(filename);
246 __logger->
log_debug(name(),
"Robot '%s' loaded.", robot->GetName().c_str());
265 OpenRAVE::EnvironmentBasePtr
278 if( __viewer_running )
281 if( __viewer_thread ) {
282 __viewer_thread->join();
283 delete __viewer_thread;
284 __viewer_thread = NULL;
291 __viewer_running =
true;
292 __viewer_thread =
new boost::thread(boost::bind(
run_viewer, __env,
"qtcoin", &__viewer_running));
293 }
catch(
const openrave_exception &e) {
294 __viewer_running =
false;
296 __logger->
log_error(name(),
"Could not load viewr. Ex:%s", e.what());
309 EnvironmentMutex::scoped_lock( __env->GetMutex());
311 RobotBasePtr robotBase = robot->get_robot_ptr();
313 std::stringstream ssin,ssout;
314 ssin <<
"LoadIKFastSolver " << robotBase->GetName() <<
" " << (int)iktype;
317 if( !__mod_ikfast->SendCommand(ssout,ssin) )
330 EnvironmentMutex::scoped_lock lock(__env->GetMutex());
332 robot->get_planner_params();
344 ModuleBasePtr basemanip = robot->get_basemanip();
345 target_t target = robot->get_target();
346 std::stringstream cmdin,cmdout;
347 cmdin << std::setprecision(std::numeric_limits<dReal>::digits10+1);
348 cmdout << std::setprecision(std::numeric_limits<dReal>::digits10+1);
351 Transform t = robot->get_robot_ptr()->GetActiveManipulator()->GetEndEffectorTransform();
354 Vector dir(target.
y,target.
z,target.
x);
355 TransformMatrix mat = matrixFromQuat(t.rot);
356 dir = mat.rotate(dir);
363 switch(target.
type) {
369 cmdin <<
"MoveActiveJoints goal";
371 std::vector<dReal> v;
373 for(
size_t i = 0; i < v.size(); ++i) {
374 cmdin <<
" " << v[i];
380 cmdin <<
"MoveToHandPosition ikparam";
381 cmdin <<
" " << target.
ikparam;
385 cmdin <<
"MoveToHandPosition pose";
386 cmdin <<
" " << target.
qw <<
" " << target.
qx <<
" " << target.
qy <<
" " << target.
qz;
387 cmdin <<
" " << target.
x <<
" " << target.
y <<
" " << target.
z;
392 cmdin <<
"MoveHandStraight direction";
393 cmdin <<
" " << target.
x <<
" " << target.
y <<
" " << target.
z;
395 dReal stepsize = 0.005;
396 dReal length = sqrt(target.
x*target.
x + target.
y*target.
y + target.
z*target.
z);
397 int minsteps = (int)(length/stepsize);
401 cmdin <<
" stepsize " << stepsize;
402 cmdin <<
" minsteps " << minsteps;
403 cmdin <<
" maxsteps " << (int)(length/stepsize);
415 cmdin <<
" execute 0";
416 cmdin <<
" outputtraj";
418 __logger->
log_debug(name(),
"Planner: basemanip cmdin:%s", cmdin.str().c_str());
421 success = basemanip->SendCommand(cmdout,cmdin);
422 }
catch(openrave_exception &e) {
423 throw fawkes::Exception(
"%s: Planner: basemanip command failed. Ex%s", name(), e.what());
428 __logger->
log_debug(name(),
"Planner: path planned");
431 __logger->
log_debug(name(),
"Planner: planned. cmdout:%s", cmdout.str().c_str());
434 TrajectoryBasePtr traj = RaveCreateTrajectory(__env,
"");
435 traj->Init(robot->get_robot_ptr()->GetActiveConfigurationSpecification());
436 if( !traj->deserialize(cmdout) )
440 std::vector< std::vector<dReal> >* trajRobot = robot->get_trajectory();
442 for(dReal time = 0; time <= traj->GetDuration(); time += (dReal)sampling) {
443 std::vector<dReal> point;
444 traj->Sample(point,time);
445 trajRobot->push_back(point);
449 if( __viewer_running ) {
452 __graph_handle.clear();
454 RobotBasePtr tmp_robot = robot->get_robot_ptr();
455 RobotBase::RobotStateSaver saver(tmp_robot);
456 for(std::vector< std::vector<dReal> >::iterator it = trajRobot->begin(); it!=trajRobot->end(); ++it) {
457 tmp_robot->SetActiveDOFValues((*it));
458 const OpenRAVE::Vector &trans = tmp_robot->GetActiveManipulator()->GetEndEffectorTransform().trans;
459 float transa[4] = { (float)trans.x, (
float)trans.y, (float)trans.z, (
float)trans.w };
460 __graph_handle.push_back(__env->plot3(transa, 1, 0, 2.f, Vector(1.f, 0.f, 0.f, 1.f)));
465 if( robot->display_planned_movements()) {
466 robot->get_robot_ptr()->GetController()->SetPath(traj);
485 std::string filename = SRCDIR
"/python/graspplanning.py";
486 std::string funcname =
"runGrasp";
488 TrajectoryBasePtr traj = RaveCreateTrajectory(__env,
"");
489 traj->Init(robot->get_robot_ptr()->GetActiveConfigurationSpecification());
491 FILE* py_file = fopen(filename.c_str(),
"r");
493 throw fawkes::Exception(
"%s: Graspplanning: opening python file failed", name());
498 PyGILState_STATE gil_state = PyGILState_Ensure();
499 PyThreadState* cur_state = PyThreadState_Get();
500 PyThreadState* int_state = Py_NewInterpreter();
501 PyThreadState_Swap(int_state);
505 PyObject* py_main = PyImport_AddModule(
"__main__");
509 Py_EndInterpreter(int_state);
510 PyThreadState_Swap(cur_state);
511 PyGILState_Release(gil_state);
513 throw fawkes::Exception(
"%s: Graspplanning: Python reference '__main__' does not exist.", name());
515 PyObject* py_dict = PyModule_GetDict(py_main);
520 throw fawkes::Exception(
"%s: Graspplanning: Python reference '__main__' does not have a dictionary.", name());
524 int py_module = PyRun_SimpleFile(py_file, filename.c_str());
528 PyObject* py_func = PyDict_GetItemString(py_dict, funcname.c_str());
529 if (py_func && PyCallable_Check(py_func)) {
531 PyObject* py_args = PyTuple_New(3);
533 PyObject* py_value_env_id = PyInt_FromLong(RaveGetEnvironmentId(__env));
534 PyObject* py_value_robot_name = PyString_FromString(robot->get_robot_ptr()->GetName().c_str());
535 PyObject* py_value_target_name = PyString_FromString(target_name.c_str());
536 PyTuple_SetItem(py_args, 0, py_value_env_id);
537 PyTuple_SetItem(py_args, 1, py_value_robot_name);
538 PyTuple_SetItem(py_args, 2, py_value_target_name);
540 PyObject* py_value = PyObject_CallObject(py_func, py_args);
543 if (py_value != NULL) {
544 if (!PyString_Check(py_value)) {
550 std::stringstream resval;
551 resval << std::setprecision(std::numeric_limits<dReal>::digits10+1);
552 resval << PyString_AsString(py_value);
553 if (!traj->deserialize(resval) ) {
557 throw fawkes::Exception(
"%s: Graspplanning: Reading trajectory data failed.", name());
567 if (PyErr_Occurred())
577 throw fawkes::Exception(
"%s: Graspplanning: Loading python file failed.", name());
580 Py_EndInterpreter(int_state);
581 PyThreadState_Swap(cur_state);
582 PyGILState_Release(gil_state);
587 __logger->
log_debug(name(),
"Graspplanning: path planned");
590 planningutils::RetimeActiveDOFTrajectory(traj, robot->get_robot_ptr());
593 std::vector< std::vector<dReal> >* trajRobot = robot->get_trajectory();
595 for(dReal time = 0; time <= traj->GetDuration(); time += (dReal)sampling) {
596 std::vector<dReal> point;
597 traj->Sample(point,time);
598 trajRobot->push_back(point);
602 if( __viewer_running ) {
605 __graph_handle.clear();
607 RobotBasePtr tmp_robot = robot->get_robot_ptr();
608 RobotBase::RobotStateSaver saver(tmp_robot);
609 for(std::vector< std::vector<dReal> >::iterator it = trajRobot->begin(); it!=trajRobot->end(); ++it) {
610 tmp_robot->SetActiveDOFValues((*it));
611 const OpenRAVE::Vector &trans = tmp_robot->GetActiveManipulator()->GetEndEffectorTransform().trans;
612 float transa[4] = { (float)trans.x, (
float)trans.y, (float)trans.z, (
float)trans.w };
613 __graph_handle.push_back(__env->plot3(transa, 1, 0, 2.f, Vector(1.f, 0.f, 0.f, 1.f)));
618 if( robot->display_planned_movements()) {
619 robot->get_robot_ptr()->GetController()->SetPath(traj);
634 EnvironmentMutex::scoped_lock lock(__env->GetMutex());
635 KinBodyPtr kb = __env->ReadKinBodyXMLFile(filename);
638 }
catch(
const OpenRAVE::openrave_exception &e) {
640 __logger->
log_warn(this->name(),
"Could not add Object '%s'. Ex:%s", name.c_str(), e.what());
655 EnvironmentMutex::scoped_lock lock(__env->GetMutex());
656 KinBodyPtr kb = __env->GetKinBody(name);
658 }
catch(
const OpenRAVE::openrave_exception &e) {
660 __logger->
log_warn(this->name(),
"Could not delete Object '%s'. Ex:%s", name.c_str(), e.what());
674 EnvironmentMutex::scoped_lock lock(__env->GetMutex());
675 std::vector<KinBodyPtr> bodies;
676 __env->GetBodies( bodies );
678 for( std::vector<KinBodyPtr>::iterator it=bodies.begin(); it!=bodies.end(); ++it ) {
679 if( !(*it)->IsRobot() )
682 }
catch(
const OpenRAVE::openrave_exception &e) {
684 __logger->
log_warn(this->name(),
"Could not delete all objects. Ex:%s", e.what());
700 EnvironmentMutex::scoped_lock lock(__env->GetMutex());
701 KinBodyPtr kb = __env->GetKinBody(name);
702 kb->SetName(new_name);
703 }
catch(
const OpenRAVE::openrave_exception &e) {
705 __logger->
log_warn(this->name(),
"Could not rename Object '%s' to '%s'. Ex:%s", name.c_str(), new_name.c_str(), e.what());
724 EnvironmentMutex::scoped_lock lock(__env->GetMutex());
725 KinBodyPtr kb = __env->GetKinBody(name);
727 Transform transform = kb->GetTransform();
728 transform.trans = Vector(trans_x, trans_y, trans_z);
730 kb->SetTransform(transform);
731 }
catch(
const OpenRAVE::openrave_exception &e) {
733 __logger->
log_warn(this->name(),
"Could not move Object '%s'. Ex:%s", name.c_str(), e.what());
755 EnvironmentMutex::scoped_lock( __env->GetMutex());
756 t = robot->get_robot_ptr()->GetTransform();
758 return move_object(name, trans_x+t.trans[1], trans_y+t.trans[2], trans_z+t.trans[3]);
773 EnvironmentMutex::scoped_lock lock(__env->GetMutex());
774 KinBodyPtr kb = __env->GetKinBody(name);
776 Vector quat(quat_w, quat_x, quat_y, quat_z);
778 Transform transform = kb->GetTransform();
779 transform.rot = quat;
781 kb->SetTransform(transform);
782 }
catch(
const OpenRAVE::openrave_exception &e) {
784 __logger->
log_warn(this->name(),
"Could not rotate Object '%s'. Ex:%s", name.c_str(), e.what());
802 Vector q1 = quatFromAxisAngle(Vector(rot_x, 0.f, 0.f));
803 Vector q2 = quatFromAxisAngle(Vector(0.f, rot_y, 0.f));
804 Vector q3 = quatFromAxisAngle(Vector(0.f, 0.f, rot_z));
806 Vector q12 = quatMultiply (q1, q2);
807 Vector quat = quatMultiply (q12, q3);
809 return rotate_object(name, quat[1], quat[2], quat[3], quat[0]);
822 EnvironmentMutex::scoped_lock lockold(env->get_env_ptr()->GetMutex());
823 EnvironmentMutex::scoped_lock lock(__env->GetMutex());
826 std::vector<KinBodyPtr> old_bodies, bodies;
827 env->get_env_ptr()->GetBodies( old_bodies );
828 __env->GetBodies( bodies );
831 std::vector<KinBodyPtr>::iterator old_body, body;
832 for(old_body=old_bodies.begin(); old_body!=old_bodies.end(); ++old_body ) {
833 if( (*old_body)->IsRobot() )
837 for( body=bodies.begin(); body!=bodies.end(); ++body ) {
838 if( (*body)->IsRobot() )
841 if( (*body)->GetName() == (*old_body)->GetName() && (*body)->GetKinematicsGeometryHash() == (*old_body)->GetKinematicsGeometryHash() ) {
847 if( body != bodies.end() ) {
850 bodies.erase( body );
858 new_body = __env->ReadKinBodyData(empty,
"<KinBody></KinBody>");
859 new_body->Clone(*old_body, 0);
862 __env->Add(new_body);
869 KinBody::KinBodyStateSaver saver(*old_body, KinBody::Save_LinkTransformation|KinBody::Save_LinkEnable|KinBody::Save_LinkVelocities);
870 saver.Restore(new_body);
874 KinBody::KinBodyStateSaver saver(*old_body, 0xffffffff);
875 saver.Restore(new_body);
880 for( body=bodies.begin(); body!=bodies.end(); ++body ) {
881 if( (*body)->IsRobot() )
884 __env->Remove( *body );
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