23 #include "openrave_thread.h"
28 #include <core/threading/mutex.h>
29 #include <interfaces/JacoInterface.h>
30 #include <utils/math/angle.h>
38 # include <plugins/openrave/environment.h>
39 # include <plugins/openrave/manipulator.h>
40 # include <plugins/openrave/manipulators/kinova_jaco.h>
41 # include <plugins/openrave/robot.h>
43 using namespace OpenRAVE;
66 load_robot_ = load_robot;
68 planner_env_.env = NULL;
69 planner_env_.robot = NULL;
70 planner_env_.manip = NULL;
72 plotted_current_ =
false;
78 JacoOpenraveThread::_init()
82 cfg_manipname_ =
config->
get_string(
"/hardware/jaco/openrave/manipname/single");
86 cfg_manipname_ =
config->
get_string(
"/hardware/jaco/openrave/manipname/dual_left");
90 cfg_manipname_ =
config->
get_string(
"/hardware/jaco/openrave/manipname/dual_right");
99 JacoOpenraveThread::_load_robot()
104 cfg_OR_robot_file_ =
config->
get_string(
"/hardware/jaco/openrave/robot_file");
107 viewer_env_.robot = openrave->add_robot(cfg_OR_robot_file_,
false);
109 throw fawkes::Exception(
"Could not add robot '%s' to openrave environment. (Error: %s)",
110 cfg_OR_robot_file_.c_str(),
116 viewer_env_.manip->add_motor(0, 0);
117 viewer_env_.manip->add_motor(1, 1);
118 viewer_env_.manip->add_motor(2, 2);
119 viewer_env_.manip->add_motor(3, 3);
120 viewer_env_.manip->add_motor(4, 4);
121 viewer_env_.manip->add_motor(5, 5);
124 openrave->set_manipulator(viewer_env_.robot, viewer_env_.manip, 0.f, 0.f, 0.f);
126 openrave->get_environment()->load_IK_solver(viewer_env_.robot, OpenRAVE::IKP_Transform6D);
136 viewer_env_.robot = openrave->get_active_robot();
137 viewer_env_.manip = viewer_env_.robot->get_manipulator()->copy();
139 throw fawkes::Exception(
"%s: Could not get robot '%s' from openrave environment. (Error: %s)",
141 cfg_OR_robot_file_.c_str(),
146 #endif //HAVE_OPENRAVE
153 JacoOpenraveThread::_post_init()
157 robot_ = viewer_env_.robot->get_robot_ptr();
161 EnvironmentMutex::scoped_lock lock(viewer_env_.env->get_env_ptr()->GetMutex());
162 manip_ = robot_->SetActiveManipulator(cfg_manipname_);
168 openrave->clone(planner_env_.env, planner_env_.robot, planner_env_.manip);
170 if (!planner_env_.env || !planner_env_.robot || !planner_env_.manip) {
176 case CONFIG_SINGLE: planner_env_.env->set_name(
"Planner");
break;
178 case CONFIG_LEFT: planner_env_.env->set_name(
"Planner_Left");
break;
180 case CONFIG_RIGHT: planner_env_.env->set_name(
"Planner_Right");
break;
185 EnvironmentMutex::scoped_lock lock(planner_env_.env->get_env_ptr()->GetMutex());
186 RobotBase::ManipulatorPtr manip =
187 planner_env_.robot->get_robot_ptr()->SetActiveManipulator(cfg_manipname_);
188 planner_env_.robot->get_robot_ptr()->SetActiveDOFs(manip->GetArmIndices());
192 robot_->GetChain(manip_->GetBase()->GetIndex(), manip_->GetEndEffector()->GetIndex(), links_);
194 #endif //HAVE_OPENRAVE
203 openrave->set_active_robot(NULL);
205 planner_env_.robot = NULL;
206 planner_env_.manip = NULL;
207 planner_env_.env = NULL;
226 #ifndef HAVE_OPENRAVE
229 if (arm_ == NULL || arm_->
arm == NULL) {
238 jaco_target_queue_t::iterator it;
256 from->
pos = (*it)->trajec->back();
278 _plan_path(from, to);
292 #ifndef HAVE_OPENRAVE
295 if (arm_ == NULL || arm_->
iface == NULL || robot_ == NULL || manip_ == NULL)
308 viewer_env_.manip->set_angles_device(joints_);
309 viewer_env_.manip->get_angles(joints_);
312 EnvironmentMutex::scoped_lock lock(viewer_env_.env->get_env_ptr()->GetMutex());
313 robot_->SetDOFValues(joints_, 1, manip_->GetArmIndices());
321 EnvironmentMutex::scoped_lock lock(viewer_env_.env->get_env_ptr()->GetMutex());
322 robot_->SetDOFValues(joints_, 1, manip_->GetGripperIndices());
326 EnvironmentMutex::scoped_lock lock(viewer_env_.env->get_env_ptr()->GetMutex());
328 if (!plotted_current_) {
330 graph_current_.clear();
333 if (cfg_OR_plot_cur_manip_) {
334 OpenRAVE::Vector color(0.f, 1.f, 0.f, 1.f);
335 const OpenRAVE::Vector &trans = manip_->GetEndEffectorTransform().trans;
336 float transa[4] = {(float)trans.x, (
float)trans.y, (float)trans.z, (
float)trans.w};
337 graph_current_.push_back(viewer_env_.env->get_env_ptr()->plot3(transa, 1, 0, 2.f, color));
340 if (cfg_OR_plot_cur_joints_) {
341 OpenRAVE::Vector color(0.2f, 1.f, 0.f, 1.f);
342 for (
unsigned int i = 0; i < links_.size(); ++i) {
343 const OpenRAVE::Vector &trans = links_[i]->GetTransform().trans;
344 float transa[4] = {(float)trans.x, (
float)trans.y, (float)trans.z, (
float)trans.w};
345 graph_current_.push_back(viewer_env_.env->get_env_ptr()->plot3(transa, 1, 0, 2.f, color));
349 plotted_current_ = plot_current_;
351 }
catch (openrave_exception &e) {
377 bool solvable =
false;
382 planner_env_.robot->get_planner_params();
387 planner_env_.robot->enable_ik_comparison(
false);
388 solvable = planner_env_.robot->set_target_euler(
389 EULER_ZXZ, x, y, z, e1, e2, e3, IKFO_IgnoreEndEffectorEnvCollisions);
399 target->
coord =
false;
400 target->
pos.push_back(x);
401 target->
pos.push_back(y);
402 target->
pos.push_back(z);
403 target->
pos.push_back(e1);
404 target->
pos.push_back(e2);
405 target->
pos.push_back(e3);
419 solvable = planner_env_.robot->set_target_euler(
EULER_ZXZ, x, y, z, e1, e2, e3);
428 target->
coord =
false;
430 planner_env_.robot->get_target().manip->get_angles_device(target->
pos);
440 }
catch (openrave_exception &e) {
475 bool joints_valid =
false;
480 planner_env_.robot->get_planner_params();
490 target->
coord =
false;
491 target->
pos.push_back(j1);
492 target->
pos.push_back(j2);
493 target->
pos.push_back(j3);
494 target->
pos.push_back(j4);
495 target->
pos.push_back(j5);
496 target->
pos.push_back(j6);
502 }
catch (openrave_exception &e) {
570 EnvironmentMutex::scoped_lock view_lock(viewer_env_.env->get_env_ptr()->GetMutex());
571 EnvironmentMutex::scoped_lock plan_lock(planner_env_.env->get_env_ptr()->GetMutex());
572 planner_env_.robot->get_robot_ptr()->ReleaseAllGrabbed();
573 planner_env_.env->delete_all_objects();
583 viewer_env_.robot->get_robot_ptr()->GetDOFValues(dofs);
584 planner_env_.robot->get_robot_ptr()->SetDOFValues(dofs);
588 planner_env_.env->clone_objects(viewer_env_.env);
592 EnvironmentMutex::scoped_lock lock(planner_env_.env->get_env_ptr()->GetMutex());
595 RobotBase::ManipulatorPtr manip =
596 planner_env_.robot->get_robot_ptr()->SetActiveManipulator(cfg_manipname_);
597 planner_env_.robot->get_robot_ptr()->SetActiveDOFs(manip->GetArmIndices());
601 EnvironmentMutex::scoped_lock view_lock(viewer_env_.env->get_env_ptr()->GetMutex());
609 vector<RobotBase::GrabbedInfoPtr> grabbed;
610 viewer_env_.robot->get_robot_ptr()->GetGrabbedInfo(grabbed);
611 for (vector<RobotBase::GrabbedInfoPtr>::iterator it = grabbed.begin(); it != grabbed.end();
614 "compare _robotlinkname '%s' with our manip link '%s'",
615 (*it)->_robotlinkname.c_str(),
616 manip->GetEndEffector()->GetName().c_str());
617 if ((*it)->_robotlinkname == manip->GetEndEffector()->GetName()) {
619 planner_env_.robot->attach_object((*it)->_grabbedname.c_str(),
621 cfg_manipname_.c_str());
630 planner_env_.robot->enable_ik_comparison(
true);
631 if (to->
type == TARGET_CARTESIAN) {
632 if (!planner_env_.robot->set_target_euler(EULER_ZXZ,
649 vector<float> target;
650 planner_env_.robot->get_target().manip->get_angles(target);
651 planner_env_.robot->set_target_angles(target);
655 vector<float> target;
658 planner_env_.robot->get_target().manip->set_angles_device(to->
pos);
660 planner_env_.robot->get_target().manip->get_angles(target);
661 planner_env_.robot->set_target_angles(target);
667 planner_env_.manip->set_angles_device(from->
pos);
670 planner_env_.robot->set_target_plannerparams(plannerparams_);
674 planner_env_.env->run_planner(planner_env_.robot, cfg_OR_sampling_);
711 if (!cfg_OR_use_viewer_ || (!cfg_OR_plot_traj_manip_ && !cfg_OR_plot_traj_joints_))
714 graph_handle_.clear();
739 graph_handle_.clear();
741 EnvironmentMutex::scoped_lock lock(viewer_env_.env->get_env_ptr()->GetMutex());
744 RobotBasePtr tmp_robot = viewer_env_.robot->get_robot_ptr();
745 RobotBase::RobotStateSaver saver(tmp_robot);
747 std::vector<dReal> joints;
759 for (jaco_trajec_t::iterator it = target->
trajec->begin(); it != target->
trajec->end(); ++it) {
763 tmp_robot->SetDOFValues(joints, 1, manip_->GetArmIndices());
765 if (cfg_OR_plot_traj_manip_) {
766 const OpenRAVE::Vector &trans = manip_->GetEndEffectorTransform().trans;
767 float transa[4] = {(float)trans.x, (
float)trans.y, (float)trans.z, (
float)trans.w};
768 graph_handle_.push_back(viewer_env_.env->get_env_ptr()->plot3(transa, 1, 0, 3.f, color_m));
771 if (cfg_OR_plot_traj_joints_) {
772 for (
unsigned int i = 0; i < links_.size(); ++i) {
773 const OpenRAVE::Vector &trans = links_[i]->GetTransform().trans;
774 float transa[4] = {(float)trans.x, (
float)trans.y, (float)trans.z, (
float)trans.w};
775 graph_handle_.push_back(
776 viewer_env_.env->get_env_ptr()->plot3(transa, 1, 0, 3.f, color_j));
784 #endif //HAVE_OPENRAVE