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>
41 using namespace OpenRAVE;
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);