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;
trajectory has been planned and is ready for execution.
JacoBimanualOpenraveThread(fawkes::jaco_dual_arm_t *arms)
Constructor.
Fawkes library namespace.
enum fawkes::jaco_trajec_state_enum jaco_trajec_state_t
The state of a trajectory.
virtual void finalize()
Finalize the thread.
std::vector< jaco_trajec_point_t > jaco_trajec_t
A trajectory.
Logger * logger
This is the Logger member used to access the logger.
jaco_target_type_t type
target type.
jaco_arm_t * left
the struct with all the data for the left arm.
jaco_arm_t * right
the struct with all the data for the right arm.
jaco_trajec_point_t pos
target position (interpreted depending on target type).
jaco_trajec_state_t trajec_state
state of the trajectory, if target is TARGET_TRAJEC.
std::vector< float > jaco_trajec_point_t
A trajectory point.
Class containing information about all Kinova Jaco motors.
Base class for exceptions in Fawkes.
virtual void set_constrained(bool enable)
Enable/Disable constrained planning.
target with cartesian coordinates.
virtual bool add_target(float l_x, float l_y, float l_z, float l_e1, float l_e2, float l_e3, float r_x, float r_y, float r_z, float r_e1, float r_e2, float r_e3)
Add target for coordinated manipulation to the queue.
Jaco target struct, holding information on a target.
target with angular coordinates.
const char * name() const
Get name of thread.
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void loop()
Code to execute in the thread.
bool coord
this target needs to be coordinated with targets of other arms.
planner could not find IK solution for target
RefPtr<> is a reference-counting shared smartpointer.
virtual void update_openrave()
Update the openrave environment to represent the current situation.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void finalize()
Finalize the thread.
new trajectory target, wait for planner to process.
Jaco struct containing all components required for a dual-arm setup.
Configuration * config
This is the Configuration member used to access the configuration.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
virtual void plot_first()
Plot the first target of the target_queue, if it is a trajectory.
Base Jaco Arm thread, integrating OpenRAVE.