23 #include "bimanual_openrave_thread.h"
28 #include <core/threading/mutex.h>
37 # include <openrave/openrave.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>
42 using namespace OpenRAVE;
60 arms_.left.arm = arms->
left;
61 arms_.right.arm = arms->
right;
63 planner_env_.env = NULL;
64 planner_env_.robot = NULL;
65 planner_env_.manip = NULL;
68 __constrained =
false;
72 JacoBimanualOpenraveThread::_init()
75 arms_.left.manipname =
config->
get_string(
"/hardware/jaco/openrave/manipname/dual_left");
76 arms_.right.manipname =
config->
get_string(
"/hardware/jaco/openrave/manipname/dual_right");
81 JacoBimanualOpenraveThread::_load_robot()
84 cfg_OR_robot_file_ =
config->
get_string(
"/hardware/jaco/openrave/robot_dual_file");
90 viewer_env_.robot->load(cfg_OR_robot_file_, viewer_env_.env);
91 viewer_env_.env->add_robot(viewer_env_.robot);
92 viewer_env_.robot->set_ready();
93 openrave->set_active_robot(viewer_env_.robot);
95 throw fawkes::Exception(
"Could not add robot '%s' to openrave environment. (Error: %s)",
96 cfg_OR_robot_file_.c_str(),
102 viewer_env_.manip->add_motor(0, 0);
103 viewer_env_.manip->add_motor(1, 1);
104 viewer_env_.manip->add_motor(2, 2);
105 viewer_env_.manip->add_motor(3, 3);
106 viewer_env_.manip->add_motor(4, 4);
107 viewer_env_.manip->add_motor(5, 5);
110 openrave->set_manipulator(viewer_env_.robot, viewer_env_.manip, 0.f, 0.f, 0.f);
112 EnvironmentMutex::scoped_lock lock(viewer_env_.env->get_env_ptr()->GetMutex());
115 viewer_env_.robot->get_robot_ptr()->SetActiveManipulator(arms_.right.manipname);
116 if (cfg_OR_auto_load_ik_) {
118 viewer_env_.env->load_IK_solver(viewer_env_.robot, OpenRAVE::IKP_Transform6D);
122 viewer_env_.robot->get_robot_ptr()->SetActiveManipulator(arms_.left.manipname);
123 if (cfg_OR_auto_load_ik_) {
125 viewer_env_.env->load_IK_solver(viewer_env_.robot, OpenRAVE::IKP_Transform6D);
135 openrave->clone(planner_env_.env, planner_env_.robot, planner_env_.manip);
137 if (!planner_env_.env || !planner_env_.robot || !planner_env_.manip) {
142 planner_env_.env->set_name(
"Planner_Bimanual");
146 planner_env_.robot->get_robot_ptr()->SetActiveManipulator(arms_.right.manipname);
148 planner_env_.robot->get_robot_ptr()->SetActiveManipulator(arms_.left.manipname);
151 _init_dualmanipulation();
156 JacoBimanualOpenraveThread::_init_dualmanipulation()
160 EnvironmentMutex::scoped_lock lock(planner_env_.env->get_env_ptr()->GetMutex());
161 mod_dualmanip_ = RaveCreateModule(planner_env_.env->get_env_ptr(),
"dualmanipulation");
162 planner_env_.env->get_env_ptr()->Add(mod_dualmanip_,
164 planner_env_.robot->get_robot_ptr()->GetName());
168 vector<int> arm_idx_l = arms_.left.manip->GetArmIndices();
169 vector<int> arm_idx_r = arms_.right.manip->GetArmIndices();
170 vector<int> grp_idx = arms_.left.manip->GetGripperIndices();
171 arm_idx_l.reserve(arm_idx_l.size() + grp_idx.size());
172 arm_idx_l.insert(arm_idx_l.end(), grp_idx.begin(), grp_idx.end());
173 grp_idx = arms_.right.manip->GetGripperIndices();
174 arm_idx_r.reserve(arm_idx_r.size() + grp_idx.size());
175 arm_idx_r.insert(arm_idx_r.end(), grp_idx.begin(), grp_idx.end());
177 RobotBasePtr robot = planner_env_.robot->get_robot_ptr();
178 vector<KinBody::LinkPtr> all_links = robot->GetLinks();
179 for (vector<KinBody::LinkPtr>::iterator link = all_links.begin(); link != all_links.end();
181 bool affected =
false;
182 for (vector<int>::iterator idx = arm_idx_l.begin(); idx != arm_idx_l.end(); ++idx) {
183 if (robot->DoesAffect(robot->GetJointFromDOFIndex(*idx)->GetJointIndex(),
184 (*link)->GetIndex())) {
186 links_left_.insert(*link);
187 arm_idx_l.erase(idx);
196 for (vector<int>::iterator idx = arm_idx_r.begin(); idx != arm_idx_r.end(); ++idx) {
197 if (robot->DoesAffect(robot->GetJointFromDOFIndex(*idx)->GetJointIndex(),
198 (*link)->GetIndex())) {
200 links_right_.insert(*link);
201 arm_idx_r.erase(idx);
212 arms_.left.arm = NULL;
213 arms_.right.arm = NULL;
215 openrave->set_active_robot(NULL);
217 planner_env_.env->get_env_ptr()->Remove(mod_dualmanip_);
218 planner_env_.robot = NULL;
219 planner_env_.manip = NULL;
220 planner_env_.env = NULL;
229 #ifndef HAVE_OPENRAVE
232 if (arms_.left.arm == NULL || arms_.right.arm == NULL) {
238 arms_.left.arm->target_mutex->lock();
239 arms_.right.arm->target_mutex->lock();
240 if (!arms_.left.arm->target_queue->empty() && !arms_.right.arm->target_queue->empty()) {
241 arms_.left.target = arms_.left.arm->target_queue->front();
242 arms_.right.target = arms_.right.arm->target_queue->front();
244 arms_.left.arm->target_mutex->unlock();
245 arms_.right.arm->target_mutex->unlock();
247 if (!arms_.left.target || !arms_.right.target || !arms_.left.target->coord
248 || !arms_.right.target->coord || arms_.left.target->trajec_state !=
TRAJEC_WAITING
259 vector<float> sol_l, sol_r;
260 bool solvable = _solve_multi_ik(sol_l, sol_r);
261 arms_.left.arm->target_mutex->lock();
262 arms_.right.arm->target_mutex->lock();
266 arms_.left.arm->target_mutex->unlock();
267 arms_.right.arm->target_mutex->unlock();
272 arms_.left.target->pos = sol_l;
274 arms_.right.target->pos = sol_r;
275 arms_.left.arm->target_mutex->unlock();
276 arms_.right.arm->target_mutex->unlock();
297 __constrained = enable;
340 target_l->
coord =
true;
341 target_l->
pos.push_back(l_x);
342 target_l->
pos.push_back(l_y);
343 target_l->
pos.push_back(l_z);
344 target_l->
pos.push_back(l_e1);
345 target_l->
pos.push_back(l_e2);
346 target_l->
pos.push_back(l_e3);
351 target_r->
coord =
true;
352 target_r->
pos.push_back(r_x);
353 target_r->
pos.push_back(r_y);
354 target_r->
pos.push_back(r_z);
355 target_r->
pos.push_back(r_e1);
356 target_r->
pos.push_back(r_e2);
357 target_r->
pos.push_back(r_e3);
359 arms_.left.arm->target_mutex->lock();
360 arms_.right.arm->target_mutex->lock();
361 arms_.left.arm->target_queue->push_back(target_l);
362 arms_.right.arm->target_queue->push_back(target_r);
363 arms_.left.arm->target_mutex->unlock();
364 arms_.right.arm->target_mutex->unlock();
387 arms_.left.arm->target_mutex->lock();
388 arms_.right.arm->target_mutex->lock();
389 arms_.left.target->trajec_state = state;
390 arms_.right.target->trajec_state = state;
391 arms_.left.arm->target_mutex->unlock();
392 arms_.right.arm->target_mutex->unlock();
397 JacoBimanualOpenraveThread::_copy_env()
403 EnvironmentMutex::scoped_lock view_lock(viewer_env_.env->get_env_ptr()->GetMutex());
404 EnvironmentMutex::scoped_lock plan_lock(planner_env_.env->get_env_ptr()->GetMutex());
405 planner_env_.robot->get_robot_ptr()->ReleaseAllGrabbed();
406 planner_env_.env->delete_all_objects();
416 viewer_env_.robot->get_robot_ptr()->GetDOFValues(dofs);
417 planner_env_.robot->get_robot_ptr()->SetDOFValues(dofs);
421 planner_env_.env->clone_objects(viewer_env_.env);
425 EnvironmentMutex::scoped_lock lock(planner_env_.env->get_env_ptr()->GetMutex());
426 EnvironmentMutex::scoped_lock view_lock(viewer_env_.env->get_env_ptr()->GetMutex());
434 vector<RobotBase::GrabbedInfoPtr> grabbed;
435 viewer_env_.robot->get_robot_ptr()->GetGrabbedInfo(grabbed);
436 for (vector<RobotBase::GrabbedInfoPtr>::iterator it = grabbed.begin(); it != grabbed.end();
439 "compare _robotlinkname '%s' with our manip links '%s' and '%s'",
440 (*it)->_robotlinkname.c_str(),
441 arms_.left.manip->GetEndEffector()->GetName().c_str(),
442 arms_.right.manip->GetEndEffector()->GetName().c_str());
443 if ((*it)->_robotlinkname == arms_.left.manip->GetEndEffector()->GetName()) {
445 "attach '%s' to '%s'!",
446 (*it)->_grabbedname.c_str(),
447 arms_.left.manip->GetEndEffector()->GetName().c_str());
448 planner_env_.robot->attach_object((*it)->_grabbedname.c_str(),
450 arms_.left.manipname.c_str());
452 }
else if ((*it)->_robotlinkname == arms_.right.manip->GetEndEffector()->GetName()) {
454 "attach '%s' to '%s'!",
455 (*it)->_grabbedname.c_str(),
456 arms_.right.manip->GetEndEffector()->GetName().c_str());
457 planner_env_.robot->attach_object((*it)->_grabbedname.c_str(),
459 arms_.right.manipname.c_str());
467 JacoBimanualOpenraveThread::_plan_path()
470 _set_trajec_state(TRAJEC_PLANNING);
472 EnvironmentMutex::scoped_lock lock(planner_env_.env->get_env_ptr()->GetMutex());
475 vector<int> dofs = arms_.left.manip->GetArmIndices();
476 vector<int> dofs_r = arms_.right.manip->GetArmIndices();
477 dofs.reserve(dofs.size() + dofs_r.size());
478 dofs.insert(dofs.end(), dofs_r.begin(), dofs_r.end());
479 planner_env_.robot->get_robot_ptr()->SetActiveDOFs(dofs);
482 stringstream cmdin, cmdout;
483 cmdin << std::setprecision(numeric_limits<dReal>::digits10 + 1);
484 cmdout << std::setprecision(numeric_limits<dReal>::digits10 + 1);
487 cmdin <<
"MoveAllJoints goal";
488 planner_env_.manip->set_angles_device(arms_.left.target->pos);
489 planner_env_.manip->get_angles(sol);
490 for (
size_t i = 0; i < sol.size(); ++i) {
491 cmdin <<
" " << sol[i];
493 planner_env_.manip->set_angles_device(arms_.right.target->pos);
494 planner_env_.manip->get_angles(sol);
495 for (
size_t i = 0; i < sol.size(); ++i) {
496 cmdin <<
" " << sol[i];
500 if (!plannerparams_.empty()) {
501 cmdin <<
" " << plannerparams_;
506 cmdin <<
" constrainterrorthresh 1";
509 cmdin <<
" execute 0";
510 cmdin <<
" outputtraj";
514 bool success =
false;
516 success = mod_dualmanip_->SendCommand(cmdout, cmdin);
517 }
catch (openrave_exception &e) {
523 _set_trajec_state(TRAJEC_PLANNING_ERROR);
524 arms_.left.arm->target_mutex->lock();
525 arms_.right.arm->target_mutex->lock();
526 arms_.left.arm->target_mutex->unlock();
527 arms_.right.arm->target_mutex->unlock();
534 ConfigurationSpecification cfg_spec =
535 planner_env_.robot->get_robot_ptr()->GetActiveConfigurationSpecification();
536 TrajectoryBasePtr traj = RaveCreateTrajectory(planner_env_.env->get_env_ptr(),
"");
537 traj->Init(cfg_spec);
538 if (!traj->deserialize(cmdout)) {
540 _set_trajec_state(TRAJEC_PLANNING_ERROR);
541 arms_.left.arm->target_mutex->lock();
542 arms_.right.arm->target_mutex->lock();
543 arms_.left.arm->target_mutex->unlock();
544 arms_.right.arm->target_mutex->unlock();
553 int arm_dof = cfg_spec.GetDOF() / 2;
555 for (dReal time = 0; time <= traj->GetDuration(); time += (dReal)cfg_OR_sampling_) {
557 traj->Sample(point, time);
559 tmp_p = vector<dReal>(point.begin(), point.begin() + arm_dof);
560 planner_env_.manip->angles_or_to_device(tmp_p, p);
561 trajec_l->push_back(p);
563 tmp_p = vector<dReal>(point.begin() + arm_dof, point.begin() + 2 * arm_dof);
564 planner_env_.manip->angles_or_to_device(tmp_p, p);
565 trajec_r->push_back(p);
568 arms_.left.arm->target_mutex->lock();
569 arms_.right.arm->target_mutex->lock();
574 arms_.left.target->pos = trajec_l->back();
575 arms_.right.target->pos = trajec_r->back();
578 arms_.left.arm->target_mutex->unlock();
579 arms_.right.arm->target_mutex->unlock();
590 JacoBimanualOpenraveThread::_solve_multi_ik(vector<float> &left, vector<float> &right)
592 #ifndef HAVE_OPENRAVE
595 EnvironmentMutex::scoped_lock plan_lock(planner_env_.env->get_env_ptr()->GetMutex());
598 RobotBasePtr robot = planner_env_.robot->get_robot_ptr();
601 vector<KinBodyPtr> grabbed;
602 robot->GetGrabbed(grabbed);
605 vector<KinBody::KinBodyStateSaver> statesaver;
606 for (vector<KinBodyPtr>::iterator body = grabbed.begin(); body != grabbed.end(); ++body) {
607 statesaver.push_back(KinBody::KinBodyStateSaver(*body));
611 vector<vector<dReal>> solutions_l, solutions_r;
614 RobotBase::RobotStateSaver robot_saver(robot);
618 for (set<KinBody::LinkPtr>::iterator body = links_right_.begin(); body != links_right_.end();
620 (*body)->Enable(
false);
623 for (vector<KinBodyPtr>::iterator body = grabbed.begin(); body != grabbed.end(); ++body) {
624 (*body)->Enable(arms_.left.manip->IsGrabbing(*body));
627 robot->SetActiveManipulator(arms_.left.manip);
628 robot->SetActiveDOFs(arms_.left.manip->GetArmIndices());
629 planner_env_.robot->set_target_euler(EULER_ZXZ,
630 arms_.left.target->pos.at(0),
631 arms_.left.target->pos.at(1),
632 arms_.left.target->pos.at(2),
633 arms_.left.target->pos.at(3),
634 arms_.left.target->pos.at(4),
635 arms_.left.target->pos.at(5));
636 IkParameterization param = planner_env_.robot->get_target().ikparam;
637 arms_.left.manip->FindIKSolutions(param, solutions_l, IKFO_CheckEnvCollisions);
638 if (solutions_l.empty()) {
646 for (set<KinBody::LinkPtr>::iterator body = links_right_.begin(); body != links_right_.end();
648 (*body)->Enable(
true);
651 for (set<KinBody::LinkPtr>::iterator body = links_left_.begin(); body != links_left_.end();
653 (*body)->Enable(
false);
656 for (vector<KinBodyPtr>::iterator body = grabbed.begin(); body != grabbed.end(); ++body) {
657 (*body)->Enable(arms_.right.manip->IsGrabbing(*body));
660 robot->SetActiveManipulator(arms_.right.manip);
661 robot->SetActiveDOFs(arms_.right.manip->GetArmIndices());
662 planner_env_.robot->set_target_euler(EULER_ZXZ,
663 arms_.right.target->pos.at(0),
664 arms_.right.target->pos.at(1),
665 arms_.right.target->pos.at(2),
666 arms_.right.target->pos.at(3),
667 arms_.right.target->pos.at(4),
668 arms_.right.target->pos.at(5));
669 param = planner_env_.robot->get_target().ikparam;
670 arms_.right.manip->FindIKSolutions(param, solutions_r, IKFO_CheckEnvCollisions);
671 if (solutions_r.empty()) {
680 for (vector<KinBody::KinBodyStateSaver>::iterator s = statesaver.begin(); s != statesaver.end();
686 bool solution_found =
false;
689 RobotBase::RobotStateSaver robot_saver(robot);
690 vector<vector<dReal>>::iterator sol_l, sol_r;
693 vector<dReal> cur_l, cur_r;
694 vector<dReal> diff_l, diff_r;
695 arms_.left.manip->GetArmDOFValues(cur_l);
696 arms_.right.manip->GetArmDOFValues(cur_r);
699 for (sol_l = solutions_l.begin(); sol_l != solutions_l.end(); ++sol_l) {
700 for (sol_r = solutions_r.begin(); sol_r != solutions_r.end(); ++sol_r) {
702 robot->SetDOFValues(*sol_l, 1, arms_.left.manip->GetArmIndices());
703 robot->SetDOFValues(*sol_r, 1, arms_.right.manip->GetArmIndices());
706 if (!robot->CheckSelfCollision() && !robot->GetEnv()->CheckCollision(robot)) {
713 robot->SubtractDOFValues(diff_l, (*sol_l), arms_.left.manip->GetArmIndices());
714 robot->SubtractDOFValues(diff_r, (*sol_r), arms_.right.manip->GetArmIndices());
715 for (
unsigned int i = 0; i < diff_l.size(); ++i) {
716 dist_l += fabs(diff_l[i]);
719 (*sol_l)[i] = cur_l[i] - diff_l[i];
722 for (
unsigned int i = 0; i < diff_r.size(); ++i) {
723 dist_r += fabs(diff_r[i]);
724 (*sol_r)[i] = cur_r[i] - diff_r[i];
728 if (dist_l + dist_r < dist) {
730 dist = dist_l + dist_r;
731 solution_found =
true;
734 planner_env_.manip->set_angles(*sol_l);
735 planner_env_.manip->get_angles_device(left);
736 planner_env_.manip->set_angles(*sol_r);
737 planner_env_.manip->get_angles_device(right);
746 return solution_found;