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> 53 SetViewer(OpenRAVE::EnvironmentBasePtr env,
const std::string& viewername)
55 ViewerBasePtr viewer = RaveCreateViewer(env, viewername);
58 env->AddViewer(viewer);
92 __env = RaveCreateEnvironment();
94 {
throw fawkes::Exception(
"OpenRAVE Environment: Could not create environment. Error in OpenRAVE.");}
96 {__logger->
log_debug(
"OpenRAVE Environment",
"Environment created");}
99 __planner = RaveCreatePlanner(__env,
"birrt");
101 {
throw fawkes::Exception(
"OpenRAVE Environment: Could not create planner. Error in OpenRAVE.");}
104 __mod_ikfast = RaveCreateModule(__env,
"ikfast");
105 __env->AddModule(__mod_ikfast,
"");
115 {__logger->
log_debug(
"OpenRAVE Environment",
"Environment destroyed");}
116 }
catch(
const openrave_exception& e) {
118 {__logger->
log_warn(
"OpenRAVE Environment",
"Could not destroy Environment. Ex:%s", e.what());}
126 EnvironmentMutex::scoped_lock
lock(__env->GetMutex());
135 RaveSetDebugLevel(level);
142 RaveSetDebugLevel(Level_Fatal);
155 {__logger->
log_debug(
"OpenRAVE Environment",
"Robot added to environment.");}
156 }
catch(openrave_exception &e) {
158 {__logger->
log_debug(
"OpenRAVE Environment",
"Could not add robot to environment. OpenRAVE error:%s", e.message().c_str());}
170 RobotBasePtr robot = __env->ReadRobotXMLFile(filename);
177 {__logger->
log_debug(
"OpenRAVE Environment",
"Robot loaded.");}
196 OpenRAVE::EnvironmentBasePtr
210 boost::thread thviewer(boost::bind(
SetViewer,__env,
"qtcoin"));
211 }
catch(
const openrave_exception &e) {
213 {__logger->
log_error(
"OpenRAVE Environment",
"Could not load viewr. Ex:%s", e.what());}
217 __viewer_enabled =
true;
229 std::stringstream ssin,ssout;
230 ssin <<
"LoadIKFastSolver " << robotBase->GetName() <<
" " << (int)iktype;
233 if( !__mod_ikfast->SendCommand(ssout,ssin) )
246 EnvironmentMutex::scoped_lock
lock(__env->GetMutex());
254 {__logger->
log_debug(
"OpenRAVE Environment",
"Planner: initialized");}
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);
264 Transform t = robot->
get_robot_ptr()->GetActiveManipulator()->GetEndEffectorTransform();
268 TransformMatrix mat = matrixFromQuat(t.rot);
269 dir = mat.rotate(dir);
276 switch(target.
type) {
278 cmdin <<
"MoveActiveJoints goal";
280 std::vector<dReal> v;
282 for(
size_t i = 0; i < v.size(); ++i) {
283 cmdin <<
" " << v[i];
289 cmdin <<
"MoveToHandPosition ikparam";
290 cmdin <<
" " << target.
ikparam;
294 cmdin <<
"MoveToHandPosition pose";
295 cmdin <<
" " << target.
qw <<
" " << target.
qx <<
" " << target.
qy <<
" " << target.
qz;
296 cmdin <<
" " << target.
x <<
" " << target.
y <<
" " << target.
z;
301 cmdin <<
"MoveHandStraight direction";
302 cmdin <<
" " << target.
x <<
" " << target.
y <<
" " << target.
z;
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);
310 cmdin <<
" stepsize " << stepsize;
311 cmdin <<
" minsteps " << minsteps;
312 cmdin <<
" maxsteps " << (int)(length/stepsize);
324 cmdin <<
" execute 0";
325 cmdin <<
" outputtraj";
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());
337 {__logger->
log_debug(
"OpenRAVE Environment",
"Planner: path planned");}
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.");}
347 std::vector< std::vector<dReal> >* trajRobot = robot->
get_trajectory();
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);
356 if( __viewer_enabled ) {
359 __graph_handle.clear();
362 RobotBase::RobotStateSaver saver(tmp_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)));
392 std::string filename = SRCDIR
"/python/graspplanning.py";
393 std::string funcname =
"runGrasp";
395 TrajectoryBasePtr traj = RaveCreateTrajectory(__env,
"");
396 traj->Init(robot->
get_robot_ptr()->GetActiveConfigurationSpecification());
398 FILE* py_file = fopen(filename.c_str(),
"r");
400 {
throw fawkes::Exception(
"OpenRAVE Environment: Graspplanning: opening python file failed");}
405 PyGILState_STATE gil_state = PyGILState_Ensure();
406 PyThreadState* cur_state = PyThreadState_Get();
407 PyThreadState* int_state = Py_NewInterpreter();
408 PyThreadState_Swap(int_state);
412 PyObject* py_main = PyImport_AddModule(
"__main__");
416 Py_EndInterpreter(int_state);
417 PyThreadState_Swap(cur_state);
418 PyGILState_Release(gil_state);
420 throw fawkes::Exception(
"OpenRAVE Environment: Graspplanning: Python reference '__main__' does not exist.");
422 PyObject* py_dict = PyModule_GetDict(py_main);
427 throw fawkes::Exception(
"OpenRAVE Environment: Graspplanning: Python reference '__main__' does not have a dictionary.");
431 int py_module = PyRun_SimpleFile(py_file, filename.c_str());
435 PyObject* py_func = PyDict_GetItemString(py_dict, funcname.c_str());
436 if (py_func && PyCallable_Check(py_func)) {
438 PyObject* py_args = PyTuple_New(3);
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);
444 PyTuple_SetItem(py_args, 1, py_value_robot_name);
445 PyTuple_SetItem(py_args, 2, py_value_target_name);
447 PyObject* py_value = PyObject_CallObject(py_func, py_args);
450 if (py_value != NULL) {
451 if (!PyString_Check(py_value)) {
455 throw fawkes::Exception(
"OpenRAVE Environment: Graspplanning: No grasping path found.");
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) ) {
464 throw fawkes::Exception(
"OpenRAVE Environment: Graspplanning: Reading trajectory data failed.");
471 throw fawkes::Exception(
"OpenRAVE Environment: Graspplanning: Calling function failed.");
474 if (PyErr_Occurred())
478 throw fawkes::Exception(
"OpenRAVE Environment: Graspplanning: Loading function failed.");
484 throw fawkes::Exception(
"OpenRAVE Environment: Graspplanning: Loading python file failed.");
487 Py_EndInterpreter(int_state);
488 PyThreadState_Swap(cur_state);
489 PyGILState_Release(gil_state);
494 {__logger->
log_debug(
"OpenRAVE Environment",
"Graspplanning: path planned");}
497 planningutils::RetimeActiveDOFTrajectory(traj, robot->
get_robot_ptr());
500 std::vector< std::vector<dReal> >* trajRobot = robot->
get_trajectory();
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);
509 if( __viewer_enabled ) {
512 __graph_handle.clear();
515 RobotBase::RobotStateSaver saver(tmp_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)));
541 EnvironmentMutex::scoped_lock
lock(__env->GetMutex());
542 KinBodyPtr kb = __env->ReadKinBodyXMLFile(filename);
545 }
catch(
const OpenRAVE::openrave_exception &e) {
547 __logger->
log_warn(
"OpenRAVE Environment",
"Could not add Object '%s'. Ex:%s", name.c_str(), e.what());
562 EnvironmentMutex::scoped_lock
lock(__env->GetMutex());
563 KinBodyPtr kb = __env->GetKinBody(name);
565 }
catch(
const OpenRAVE::openrave_exception &e) {
567 __logger->
log_warn(
"OpenRAVE Environment",
"Could not delete Object '%s'. Ex:%s", name.c_str(), e.what());
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) {
588 __logger->
log_warn(
"OpenRAVE Environment",
"Could not rename Object '%s' to '%s'. Ex:%s", name.c_str(), new_name.c_str(), e.what());
608 EnvironmentMutex::scoped_lock
lock(__env->GetMutex());
609 KinBodyPtr kb = __env->GetKinBody(name);
611 Transform transform = kb->GetTransform();
612 transform.trans =
Vector(trans_x, trans_y, trans_z);
615 Transform robotTrans = robot->
get_robot_ptr()->GetTransform();
616 transform.trans += robotTrans.trans;
619 kb->SetTransform(transform);
620 }
catch(
const OpenRAVE::openrave_exception &e) {
622 __logger->
log_warn(
"OpenRAVE Environment",
"Could not move Object '%s'. Ex:%s", name.c_str(), e.what());
641 EnvironmentMutex::scoped_lock
lock(__env->GetMutex());
642 KinBodyPtr kb = __env->GetKinBody(name);
644 Vector quat(quat_w, quat_x, quat_y, quat_z);
646 Transform transform = kb->GetTransform();
647 transform.rot = quat;
649 kb->SetTransform(transform);
650 }
catch(
const OpenRAVE::openrave_exception &e) {
652 __logger->
log_warn(
"OpenRAVE Environment",
"Could not rotate Object '%s'. Ex:%s", name.c_str(), e.what());
674 Vector q12 = quatMultiply (q1, q2);
675 Vector quat = quatMultiply (q12, q3);
float z() const
Convenience getter to obtain the third element.
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.
Target: OpenRAVE::IkParameterization string.
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
~OpenRaveEnvironment()
Destructor.
float x
translation on x-axis
Target: relative endeffector translation, based on arm extension.
Fawkes library namespace.
float qz
z value of quaternion
virtual OpenRAVE::RobotBasePtr get_robot_ptr() const
Returns RobotBasePtr for uses in other classes.
float x() const
Convenience getter to obtain the first element.
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.
virtual OpenRAVE::EnvironmentBasePtr get_env_ptr() const
Get EnvironmentBasePtr.
target_type_t type
target type
Target: absolute endeffector translation and rotation.
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.
virtual void load_IK_solver(OpenRaveRobot *robot, OpenRAVE::IkParameterizationType iktype=OpenRAVE::IKP_Transform6D)
Autogenerate IKfast IK solver for robot.
OpenRaveManipulator * manip
target manipulator configuration
void get_angles(std::vector< T > &to) const
Get motor angles of OpenRAVE model.
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.
virtual OpenRAVE::PlannerBase::PlannerParametersPtr get_planner_params() const
Updates planner parameters and return pointer to it.
Base class for exceptions in Fawkes.
Target: relative endeffector translation, based on robot's coordinate system.
OpenRAVE::IkParameterization ikparam
OpenRAVE::IkParameterization; each target is implicitly transformed to one by OpenRAVE.
Struct containing information about the current target.
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
void SetViewer(OpenRAVE::EnvironmentBasePtr env, const std::string &viewername)
Sets and loads a viewer for OpenRAVE.
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...
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void destroy()
Destroy the environment.
Expected parameter is missing.
virtual void disable_debug()
Disable debugging messages of OpenRAVE.
Target: motor joint values.
float qy
y value of quaternion
float y() const
Convenience getter to obtain the second element.
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.