23 #include "bimanual_openrave_thread.h" 27 #include <core/threading/mutex.h> 36 #include <openrave/openrave.h> 37 #include <plugins/openrave/environment.h> 38 #include <plugins/openrave/robot.h> 39 #include <plugins/openrave/manipulator.h> 40 #include <plugins/openrave/manipulators/kinova_jaco.h> 59 __arms.left.arm = arms->
left;
60 __arms.right.arm = arms->
right;
62 __planner_env.env = NULL;
63 __planner_env.robot = NULL;
64 __planner_env.manip = NULL;
67 __constrained =
false;
71 JacoBimanualOpenraveThread::_init()
74 __arms.left.manipname =
config->
get_string(
"/hardware/jaco/openrave/manipname/dual_left");
75 __arms.right.manipname =
config->
get_string(
"/hardware/jaco/openrave/manipname/dual_right");
80 JacoBimanualOpenraveThread::_load_robot()
83 __cfg_OR_robot_file =
config->
get_string(
"/hardware/jaco/openrave/robot_dual_file");
89 __viewer_env.robot->load(__cfg_OR_robot_file, __viewer_env.env);
90 __viewer_env.env->add_robot(__viewer_env.robot);
91 __viewer_env.robot->set_ready();
92 openrave->set_active_robot(__viewer_env.robot);
99 __viewer_env.manip->add_motor(0,0);
100 __viewer_env.manip->add_motor(1,1);
101 __viewer_env.manip->add_motor(2,2);
102 __viewer_env.manip->add_motor(3,3);
103 __viewer_env.manip->add_motor(4,4);
104 __viewer_env.manip->add_motor(5,5);
107 openrave->set_manipulator(__viewer_env.robot, __viewer_env.manip, 0.f, 0.f, 0.f);
109 EnvironmentMutex::scoped_lock lock(__viewer_env.env->get_env_ptr()->GetMutex());
111 __arms.right.manip = __viewer_env.robot->get_robot_ptr()->SetActiveManipulator(__arms.right.manipname);
112 if( __cfg_OR_auto_load_ik ) {
114 __viewer_env.env->load_IK_solver(__viewer_env.robot, OpenRAVE::IKP_Transform6D);
117 __arms.left.manip = __viewer_env.robot->get_robot_ptr()->SetActiveManipulator(__arms.left.manipname);
118 if( __cfg_OR_auto_load_ik ) {
120 __viewer_env.env->load_IK_solver(__viewer_env.robot, OpenRAVE::IKP_Transform6D);
130 openrave->clone(__planner_env.env, __planner_env.robot, __planner_env.manip);
132 if( !__planner_env.env || !__planner_env.robot || !__planner_env.manip) {
137 __planner_env.env->set_name(
"Planner_Bimanual");
140 __arms.right.manip = __planner_env.robot->get_robot_ptr()->SetActiveManipulator(__arms.right.manipname);
141 __arms.left.manip = __planner_env.robot->get_robot_ptr()->SetActiveManipulator(__arms.left.manipname);
144 _init_dualmanipulation();
150 JacoBimanualOpenraveThread::_init_dualmanipulation()
154 EnvironmentMutex::scoped_lock lock(__planner_env.env->get_env_ptr()->GetMutex());
155 __mod_dualmanip = RaveCreateModule(__planner_env.env->get_env_ptr(),
"dualmanipulation");
156 __planner_env.env->get_env_ptr()->Add( __mod_dualmanip,
true, __planner_env.robot->get_robot_ptr()->GetName());
160 vector<int> arm_idx_l = __arms.left.manip->GetArmIndices();
161 vector<int> arm_idx_r = __arms.right.manip->GetArmIndices();
162 vector<int> grp_idx = __arms.left.manip->GetGripperIndices();
163 arm_idx_l.reserve( arm_idx_l.size() + grp_idx.size() );
164 arm_idx_l.insert( arm_idx_l.end(), grp_idx.begin(), grp_idx.end() );
165 grp_idx = __arms.right.manip->GetGripperIndices();
166 arm_idx_r.reserve( arm_idx_r.size() + grp_idx.size() );
167 arm_idx_r.insert( arm_idx_r.end(), grp_idx.begin(), grp_idx.end() );
169 RobotBasePtr robot = __planner_env.robot->get_robot_ptr();
170 vector<KinBody::LinkPtr> all_links = robot->GetLinks();
171 for( vector<KinBody::LinkPtr>::iterator link=all_links.begin(); link!=all_links.end(); ++link ) {
172 bool affected =
false;
173 for( vector<int>::iterator idx=arm_idx_l.begin(); idx!=arm_idx_l.end(); ++idx ) {
174 if( robot->DoesAffect(robot->GetJointFromDOFIndex(*idx)->GetJointIndex(),(*link)->GetIndex()) ) {
176 links_left_.insert(*link);
177 arm_idx_l.erase(idx);
186 for( vector<int>::iterator idx=arm_idx_r.begin(); idx!=arm_idx_r.end(); ++idx ) {
187 if( robot->DoesAffect(robot->GetJointFromDOFIndex(*idx)->GetJointIndex(),(*link)->GetIndex()) ) {
189 links_right_.insert(*link);
190 arm_idx_r.erase(idx);
200 __arms.left.arm = NULL;
201 __arms.right.arm = NULL;
203 openrave->set_active_robot( NULL );
205 __planner_env.env->get_env_ptr()->Remove( __mod_dualmanip );
206 __planner_env.robot = NULL;
207 __planner_env.manip = NULL;
208 __planner_env.env = NULL;
218 #ifndef HAVE_OPENRAVE 221 if( __arms.left.arm == NULL || __arms.right.arm == NULL ) {
227 __arms.left.arm->target_mutex->lock();
228 __arms.right.arm->target_mutex->lock();
229 if( !__arms.left.arm->target_queue->empty() && !__arms.right.arm->target_queue->empty() ) {
230 __arms.left.target = __arms.left.arm->target_queue->front();
231 __arms.right.target = __arms.right.arm->target_queue->front();
233 __arms.left.arm->target_mutex->unlock();
234 __arms.right.arm->target_mutex->unlock();
236 if( !__arms.left.target || !__arms.right.target
237 || !__arms.left.target->coord || !__arms.right.target->coord
249 vector<float> sol_l, sol_r;
250 bool solvable = _solve_multi_ik(sol_l, sol_r);
251 __arms.left.arm->target_mutex->lock();
252 __arms.right.arm->target_mutex->lock();
256 __arms.left.arm->target_mutex->unlock();
257 __arms.right.arm->target_mutex->unlock();
262 __arms.left.target->pos = sol_l;
264 __arms.right.target->pos = sol_r;
265 __arms.left.arm->target_mutex->unlock();
266 __arms.right.arm->target_mutex->unlock();
287 __constrained = enable;
312 float r_x,
float r_y,
float r_z,
float r_e1,
float r_e2,
float r_e3)
320 target_l->
coord=
true;
321 target_l->
pos.push_back(l_x);
322 target_l->
pos.push_back(l_y);
323 target_l->
pos.push_back(l_z);
324 target_l->
pos.push_back(l_e1);
325 target_l->
pos.push_back(l_e2);
326 target_l->
pos.push_back(l_e3);
331 target_r->
coord=
true;
332 target_r->
pos.push_back(r_x);
333 target_r->
pos.push_back(r_y);
334 target_r->
pos.push_back(r_z);
335 target_r->
pos.push_back(r_e1);
336 target_r->
pos.push_back(r_e2);
337 target_r->
pos.push_back(r_e3);
339 __arms.left.arm->target_mutex->lock();
340 __arms.right.arm->target_mutex->lock();
341 __arms.left.arm->target_queue->push_back(target_l);
342 __arms.right.arm->target_queue->push_back(target_r);
343 __arms.left.arm->target_mutex->unlock();
344 __arms.right.arm->target_mutex->unlock();
367 __arms.left.arm->target_mutex->lock();
368 __arms.right.arm->target_mutex->lock();
369 __arms.left.target->trajec_state=state;
370 __arms.right.target->trajec_state=state;
371 __arms.left.arm->target_mutex->unlock();
372 __arms.right.arm->target_mutex->unlock();
377 JacoBimanualOpenraveThread::_copy_env()
383 EnvironmentMutex::scoped_lock view_lock(__viewer_env.env->get_env_ptr()->GetMutex());
384 EnvironmentMutex::scoped_lock plan_lock(__planner_env.env->get_env_ptr()->GetMutex());
385 __planner_env.robot->get_robot_ptr()->ReleaseAllGrabbed();
386 __planner_env.env->delete_all_objects();
396 __viewer_env.robot->get_robot_ptr()->GetDOFValues(dofs);
397 __planner_env.robot->get_robot_ptr()->SetDOFValues(dofs);
401 __planner_env.env->clone_objects( __viewer_env.env );
405 EnvironmentMutex::scoped_lock lock(__planner_env.env->get_env_ptr()->GetMutex());
406 EnvironmentMutex::scoped_lock view_lock(__viewer_env.env->get_env_ptr()->GetMutex());
414 vector<RobotBase::GrabbedInfoPtr> grabbed;
415 __viewer_env.robot->get_robot_ptr()->GetGrabbedInfo(grabbed);
416 for( vector<RobotBase::GrabbedInfoPtr>::iterator it=grabbed.begin(); it!=grabbed.end(); ++it ) {
417 logger->
log_debug(
name(),
"compare _robotlinkname '%s' with our manip links '%s' and '%s'",
418 (*it)->_robotlinkname.c_str(),
419 __arms.left.manip->GetEndEffector()->GetName().c_str(),
420 __arms.right.manip->GetEndEffector()->GetName().c_str());
421 if( (*it)->_robotlinkname == __arms.left.manip->GetEndEffector()->GetName() ) {
422 logger->
log_debug(
name(),
"attach '%s' to '%s'!", (*it)->_grabbedname.c_str(), __arms.left.manip->GetEndEffector()->GetName().c_str());
423 __planner_env.robot->attach_object((*it)->_grabbedname.c_str(), __planner_env.env, __arms.left.manipname.c_str());
425 }
else if( (*it)->_robotlinkname == __arms.right.manip->GetEndEffector()->GetName() ) {
426 logger->
log_debug(
name(),
"attach '%s' to '%s'!", (*it)->_grabbedname.c_str(), __arms.right.manip->GetEndEffector()->GetName().c_str());
427 __planner_env.robot->attach_object((*it)->_grabbedname.c_str(), __planner_env.env, __arms.right.manipname.c_str());
435 JacoBimanualOpenraveThread::_plan_path()
440 EnvironmentMutex::scoped_lock lock(__planner_env.env->get_env_ptr()->GetMutex());
443 vector<int> dofs = __arms.left.manip->GetArmIndices();
444 vector<int> dofs_r = __arms.right.manip->GetArmIndices();
445 dofs.reserve(dofs.size() + dofs_r.size());
446 dofs.insert(dofs.end(), dofs_r.begin(), dofs_r.end());
447 __planner_env.robot->get_robot_ptr()->SetActiveDOFs(dofs);
450 stringstream cmdin,cmdout;
451 cmdin << std::setprecision(numeric_limits<dReal>::digits10+1);
452 cmdout << std::setprecision(numeric_limits<dReal>::digits10+1);
455 cmdin <<
"MoveAllJoints goal";
456 __planner_env.manip->set_angles_device(__arms.left.target->pos);
457 __planner_env.manip->get_angles(sol);
458 for(
size_t i = 0; i < sol.size(); ++i) {
459 cmdin <<
" " << sol[i];
461 __planner_env.manip->set_angles_device(__arms.right.target->pos);
462 __planner_env.manip->get_angles(sol);
463 for(
size_t i = 0; i < sol.size(); ++i) {
464 cmdin <<
" " << sol[i];
468 if( !__plannerparams.empty() ) {
469 cmdin <<
" " << __plannerparams;
473 if( __constrained ) {
474 cmdin <<
" constrainterrorthresh 1";
477 cmdin <<
" execute 0";
478 cmdin <<
" outputtraj";
482 bool success =
false;
484 success = __mod_dualmanip->SendCommand(cmdout,cmdin);
485 }
catch(openrave_exception &e) {
492 __arms.left.arm->target_mutex->lock();
493 __arms.right.arm->target_mutex->lock();
494 __arms.left.arm->target_mutex->unlock();
495 __arms.right.arm->target_mutex->unlock();
502 ConfigurationSpecification cfg_spec = __planner_env.robot->get_robot_ptr()->GetActiveConfigurationSpecification();
503 TrajectoryBasePtr traj = RaveCreateTrajectory(__planner_env.env->get_env_ptr(),
"");
504 traj->Init(cfg_spec);
505 if( !traj->deserialize(cmdout) ) {
508 __arms.left.arm->target_mutex->lock();
509 __arms.right.arm->target_mutex->lock();
510 __arms.left.arm->target_mutex->unlock();
511 __arms.right.arm->target_mutex->unlock();
520 int arm_dof = cfg_spec.GetDOF() / 2;
522 for(dReal time = 0; time <= traj->GetDuration(); time += (dReal)__cfg_OR_sampling) {
524 traj->Sample(point,time);
526 tmp_p = vector<dReal>(point.begin(), point.begin()+arm_dof);
527 __planner_env.manip->angles_or_to_device( tmp_p, p);
528 trajec_l->push_back(p);
530 tmp_p = vector<dReal>(point.begin()+arm_dof, point.begin()+2*arm_dof);
531 __planner_env.manip->angles_or_to_device( tmp_p, p);
532 trajec_r->push_back(p);
535 __arms.left.arm->target_mutex->lock();
536 __arms.right.arm->target_mutex->lock();
541 __arms.left.target->pos = trajec_l->back();
542 __arms.right.target->pos = trajec_r->back();
545 __arms.left.arm->target_mutex->unlock();
546 __arms.right.arm->target_mutex->unlock();
557 JacoBimanualOpenraveThread::_solve_multi_ik(vector<float> &left, vector<float> &right)
559 #ifndef HAVE_OPENRAVE 562 EnvironmentMutex::scoped_lock plan_lock(__planner_env.env->get_env_ptr()->GetMutex());
565 RobotBasePtr robot = __planner_env.robot->get_robot_ptr();
568 vector<KinBodyPtr> grabbed;
569 robot->GetGrabbed(grabbed);
572 vector<KinBody::KinBodyStateSaver> statesaver;
573 for( vector<KinBodyPtr>::iterator body=grabbed.begin(); body!=grabbed.end(); ++body ) {
574 statesaver.push_back(KinBody::KinBodyStateSaver(*body));
578 vector< vector<dReal> > solutions_l, solutions_r;
581 RobotBase::RobotStateSaver robot_saver(robot);
583 vector<KinBody::LinkPtr> all_links = robot->GetLinks();
586 for( set<KinBody::LinkPtr>::iterator body=links_right_.begin(); body!=links_right_.end(); ++body) {
587 (*body)->Enable(
false);
590 for( vector<KinBodyPtr>::iterator body=grabbed.begin(); body!=grabbed.end(); ++body) {
591 (*body)->Enable(__arms.left.manip->IsGrabbing(*body));
594 robot->SetActiveManipulator(__arms.left.manip);
595 robot->SetActiveDOFs(__arms.left.manip->GetArmIndices());
596 __planner_env.robot->set_target_euler(
EULER_ZXZ,
597 __arms.left.target->pos.at(0), __arms.left.target->pos.at(1), __arms.left.target->pos.at(2),
598 __arms.left.target->pos.at(3), __arms.left.target->pos.at(4), __arms.left.target->pos.at(5));
599 IkParameterization param = __planner_env.robot->get_target().ikparam;
600 __arms.left.manip->FindIKSolutions(param, solutions_l, IKFO_CheckEnvCollisions);
601 if( solutions_l.empty() ) {
609 for( set<KinBody::LinkPtr>::iterator body=links_right_.begin(); body!=links_right_.end(); ++body) {
610 (*body)->Enable(
true);
613 for( set<KinBody::LinkPtr>::iterator body=links_left_.begin(); body!=links_left_.end(); ++body) {
614 (*body)->Enable(
false);
617 for( vector<KinBodyPtr>::iterator body=grabbed.begin(); body!=grabbed.end(); ++body) {
618 (*body)->Enable(__arms.right.manip->IsGrabbing(*body));
621 robot->SetActiveManipulator(__arms.right.manip);
622 robot->SetActiveDOFs(__arms.right.manip->GetArmIndices());
623 __planner_env.robot->set_target_euler(
EULER_ZXZ,
624 __arms.right.target->pos.at(0), __arms.right.target->pos.at(1), __arms.right.target->pos.at(2),
625 __arms.right.target->pos.at(3), __arms.right.target->pos.at(4), __arms.right.target->pos.at(5));
626 param = __planner_env.robot->get_target().ikparam;
627 __arms.right.manip->FindIKSolutions(param, solutions_r, IKFO_CheckEnvCollisions);
628 if( solutions_r.empty() ) {
637 for( vector<KinBody::KinBodyStateSaver>::iterator s=statesaver.begin(); s!=statesaver.end(); ++s ) {
642 bool solution_found =
false;
645 RobotBase::RobotStateSaver robot_saver(robot);
646 vector< vector<dReal> >::iterator sol_l, sol_r;
649 vector<dReal> cur_l, cur_r;
650 vector<dReal> diff_l, diff_r;
651 __arms.left.manip->GetArmDOFValues(cur_l);
652 __arms.right.manip->GetArmDOFValues(cur_r);
655 for( sol_l=solutions_l.begin(); sol_l!=solutions_l.end(); ++sol_l ) {
656 for( sol_r=solutions_r.begin(); sol_r!=solutions_r.end(); ++sol_r ) {
658 robot->SetDOFValues(*sol_l, 1, __arms.left.manip->GetArmIndices());
659 robot->SetDOFValues(*sol_r, 1, __arms.right.manip->GetArmIndices());
662 if( !robot->CheckSelfCollision() && !robot->GetEnv()->CheckCollision(robot) ) {
669 robot->SubtractDOFValues(diff_l, (*sol_l), __arms.left.manip->GetArmIndices());
670 robot->SubtractDOFValues(diff_r, (*sol_r), __arms.right.manip->GetArmIndices());
671 for(
unsigned int i=0; i<diff_l.size(); ++i) {
672 dist_l += fabs(diff_l[i]);
675 (*sol_l)[i] = cur_l[i] - diff_l[i];
678 for(
unsigned int i=0; i<diff_r.size(); ++i) {
679 dist_r += fabs(diff_r[i]);
680 (*sol_r)[i] = cur_r[i] - diff_r[i];
684 if( dist_l+dist_r < dist ) {
686 dist = dist_l + dist_r;
687 solution_found =
true;
690 __planner_env.manip->set_angles(*sol_l);
691 __planner_env.manip->get_angles_device(left);
692 __planner_env.manip->set_angles(*sol_r);
693 __planner_env.manip->get_angles_device(right);
703 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.
planner could not plan a collision-free trajectory.
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.
planner is planning the trajectory.
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.