Fawkes API  Fawkes Development Version
bimanual_openrave_thread.cpp
1 
2 /***************************************************************************
3  * bimanual_openrave_thread.cpp - Jaco plugin OpenRAVE Thread for bimanual manipulation
4  *
5  * Created: Mon Jul 28 19:43:20 2014
6  * Copyright 2014 Bahram Maleki-Fard
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "bimanual_openrave_thread.h"
24 #include "types.h"
25 #include "arm.h"
26 
27 #include <core/threading/mutex.h>
28 
29 #include <cmath>
30 #include <stdio.h>
31 #include <cstring>
32 #include <algorithm>
33 #include <unistd.h>
34 
35 #ifdef HAVE_OPENRAVE
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>
41  using namespace OpenRAVE;
42 #endif
43 
44 using namespace fawkes;
45 using namespace std;
46 
47 /** @class JacoBimanualOpenraveThread "bimanual_openrave_thread.h"
48  * Jaco Arm thread for dual-arm setup, integrating OpenRAVE
49  *
50  * @author Bahram Maleki-Fard
51  */
52 
53 /** Constructor.
54  * @param arms pointer to jaco_dual_arm_t struct, to be used in this thread
55  */
57  : JacoOpenraveBaseThread("JacoBimanualOpenraveThread")
58 {
59  __arms.left.arm = arms->left;
60  __arms.right.arm = arms->right;
61 #ifdef HAVE_OPENRAVE
62  __planner_env.env = NULL;
63  __planner_env.robot = NULL;
64  __planner_env.manip = NULL;
65 #endif
66 
67  __constrained = false;
68 }
69 
70 void
71 JacoBimanualOpenraveThread::_init()
72 {
73 #ifdef HAVE_OPENRAVE
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");
76 #endif
77 }
78 
79 void
80 JacoBimanualOpenraveThread::_load_robot()
81 {
82 #ifdef HAVE_OPENRAVE
83  __cfg_OR_robot_file = config->get_string("/hardware/jaco/openrave/robot_dual_file");
84 
85  try {
86  //__viewer_env.robot = openrave->add_robot(__cfg_OR_robot_file, false);
87  // manually add robot; the automatic needs to be altered
88  __viewer_env.robot = new OpenRaveRobot(logger);
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);
93  } catch (Exception& e) {
94  throw fawkes::Exception("Could not add robot '%s' to openrave environment. (Error: %s)", __cfg_OR_robot_file.c_str(), e.what_no_backtrace());
95  }
96 
97  try {
98  __viewer_env.manip = new OpenRaveManipulatorKinovaJaco(6, 6);
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);
105 
106  // Set manipulator and offsets.
107  openrave->set_manipulator(__viewer_env.robot, __viewer_env.manip, 0.f, 0.f, 0.f);
108 
109  EnvironmentMutex::scoped_lock lock(__viewer_env.env->get_env_ptr()->GetMutex());
110 
111  __arms.right.manip = __viewer_env.robot->get_robot_ptr()->SetActiveManipulator(__arms.right.manipname);
112  if( __cfg_OR_auto_load_ik ) {
113  logger->log_debug(name(), "load IK for right arm");
114  __viewer_env.env->load_IK_solver(__viewer_env.robot, OpenRAVE::IKP_Transform6D);
115  }
116 
117  __arms.left.manip = __viewer_env.robot->get_robot_ptr()->SetActiveManipulator(__arms.left.manipname);
118  if( __cfg_OR_auto_load_ik ) {
119  logger->log_debug(name(), "load IK for left arm");
120  __viewer_env.env->load_IK_solver(__viewer_env.robot, OpenRAVE::IKP_Transform6D);
121  }
122 
123  } catch (Exception& e) {
124  finalize();
125  throw;
126  }
127 
128  // create cloned environment for planning
129  logger->log_debug(name(), "Clone environment for planning");
130  openrave->clone(__planner_env.env, __planner_env.robot, __planner_env.manip);
131 
132  if( !__planner_env.env || !__planner_env.robot || !__planner_env.manip) {
133  throw fawkes::Exception("Could not clone properly, received a NULL pointer");
134  }
135 
136  // set name of env
137  __planner_env.env->set_name("Planner_Bimanual");
138 
139  // set manips to those of planner env
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);
142 
143  // initial modules for dualmanipulation
144  _init_dualmanipulation();
145 #endif
146 }
147 
148 
149 void
150 JacoBimanualOpenraveThread::_init_dualmanipulation()
151 {
152 #ifdef HAVE_OPENRAVE
153  // load dualmanipulation module
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());
157 
158  // load MultiManipIkSolver stuff
159  // Get all the links that are affecte by left/right manipulator
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() );
168 
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()) ) {
175  // this link is affected by left manipulator
176  links_left_.insert(*link);
177  arm_idx_l.erase(idx); // no need to check this one again
178  affected = true;
179  break;
180  }
181  }
182 
183  if( affected )
184  continue;
185 
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()) ) {
188  // this link is affected by right manipulator
189  links_right_.insert(*link);
190  arm_idx_r.erase(idx); // no need to check this one again
191  break;
192  }
193  }
194  }
195 #endif
196 }
197 
198 void
200  __arms.left.arm = NULL;
201  __arms.right.arm = NULL;
202 #ifdef HAVE_OPENRAVE
203  openrave->set_active_robot( NULL );
204 
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;
209 #endif
210 
212 }
213 
214 
215 void
217 {
218 #ifndef HAVE_OPENRAVE
219  usleep(30e3);
220 #else
221  if( __arms.left.arm == NULL || __arms.right.arm == NULL ) {
222  usleep(30e3);
223  return;
224  }
225 
226  // get first target in queues
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();
232  }
233  __arms.left.arm->target_mutex->unlock();
234  __arms.right.arm->target_mutex->unlock();
235 
236  if( !__arms.left.target || !__arms.right.target
237  || !__arms.left.target->coord || !__arms.right.target->coord
238  || __arms.left.target->trajec_state !=TRAJEC_WAITING
239  || __arms.right.target->trajec_state!=TRAJEC_WAITING ) {
240  //no new target in queue, or target is not meant for coordinated bimanual manipulation
241  usleep(30e3);
242  return;
243  }
244 
245  // copy environment first
246  _copy_env();
247 
248  // get suiting IK solutions
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();
253  if( !solvable ) {
254  __arms.left.target->trajec_state=TRAJEC_IK_ERROR;
255  __arms.right.target->trajec_state=TRAJEC_IK_ERROR;
256  __arms.left.arm->target_mutex->unlock();
257  __arms.right.arm->target_mutex->unlock();
258  usleep(30e3);
259  return;
260  } else {
261  __arms.left.target->type=TARGET_ANGULAR;
262  __arms.left.target->pos = sol_l;
263  __arms.right.target->type=TARGET_ANGULAR;
264  __arms.right.target->pos = sol_r;
265  __arms.left.arm->target_mutex->unlock();
266  __arms.right.arm->target_mutex->unlock();
267 
268  // run path planner
269  _plan_path();
270  }
271 
272 #endif
273 }
274 
275 /** Enable/Disable constrained planning.
276  * Enabling it will constrain the movement, which means it is tried to
277  * maintain the distance of the grippers to each other. This should be
278  * activated when moving an object with both hands, but disabled in
279  * situations when the arms do not need to hold the object simultaneously
280  * at all times.
281  *
282  * @param enable Enables/Disables the state.
283  */
284 void
286 {
287  __constrained = enable;
288 }
289 
290 /** Add target for coordinated manipulation to the queue.
291  *
292  * This adds targets to the queues for both left and right arms. It sets
293  * the target->coord flag to "true", which means it will not be processed
294  * by the threads for uncoordinated manipulation!
295  *
296  * @param l_x x-coordinate of target position of left arm
297  * @param l_y y-coordinate of target position of left arm
298  * @param l_z z-coordinate of target position of left arm
299  * @param l_e1 1st euler rotation of target orientation of left arm
300  * @param l_e2 2nd euler rotation of target orientation of left arm
301  * @param l_e3 3rd euler rotation of target orientation of left arm
302  * @param r_x x-coordinate of target position of right arm
303  * @param r_y y-coordinate of target position of right arm
304  * @param r_z z-coordinate of target position of right arm
305  * @param r_e1 1st euler rotation of target orientation of right arm
306  * @param r_e2 2nd euler rotation of target orientation of right arm
307  * @param r_e3 3rd euler rotation of target orientation of right arm
308  * @return "true", if target could be added to queue.
309  */
310 bool
311 JacoBimanualOpenraveThread::add_target(float l_x, float l_y, float l_z, float l_e1, float l_e2, float l_e3,
312  float r_x, float r_y, float r_z, float r_e1, float r_e2, float r_e3)
313 {
314 #ifdef HAVE_OPENRAVE
315  // no IK checking yet, just enqueue until they can be processed
316  // create new targets for the queues
317  RefPtr<jaco_target_t> target_l(new jaco_target_t());
318  target_l->type = TARGET_CARTESIAN;
319  target_l->trajec_state = TRAJEC_WAITING;
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);
327 
328  RefPtr<jaco_target_t> target_r(new jaco_target_t());
329  target_r->type = TARGET_CARTESIAN;
330  target_r->trajec_state = TRAJEC_WAITING;
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);
338 
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();
345 
346  return true;
347 #else
348  return false;
349 #endif
350 }
351 
352 void
354 {
355  // do nothing, this thread is only for plannning!
356 }
357 
358 void
360 {
361 }
362 
363 void
364 JacoBimanualOpenraveThread::_set_trajec_state(jaco_trajec_state_t state)
365 {
366 #ifdef HAVE_OPENRAVE
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();
373 #endif
374 }
375 
376 void
377 JacoBimanualOpenraveThread::_copy_env()
378 {
379 #ifdef HAVE_OPENRAVE
380  // Update bodies in planner-environment
381  // clone robot state, ignoring grabbed bodies
382  {
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();
387 
388  /*
389  // Old method. Somehow we encountered problems. OpenRAVE internal bug?
390  RobotBase::RobotStateSaver saver(__viewer_env.robot->get_robot_ptr(),
391  0xffffffff&~KinBody::Save_GrabbedBodies&~KinBody::Save_ActiveManipulator&~KinBody::Save_ActiveDOF);
392  saver.Restore( __planner_env.robot->get_robot_ptr() );
393  */
394  // New method. Simply set the DOF values as they are in __viewer_env
395  vector<dReal> dofs;
396  __viewer_env.robot->get_robot_ptr()->GetDOFValues(dofs);
397  __planner_env.robot->get_robot_ptr()->SetDOFValues(dofs);
398  }
399 
400  // then clone all objects
401  __planner_env.env->clone_objects( __viewer_env.env );
402 
403  // update robot state with attached objects
404  {
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());
407  /*
408  // Old method. Somehow we encountered problems. OpenRAVE internal bug?
409  RobotBase::RobotStateSaver saver(__viewer_env.robot->get_robot_ptr(),
410  KinBody::Save_LinkTransformation|KinBody::Save_LinkEnable|KinBody::Save_GrabbedBodies);
411  saver.Restore( __planner_env.robot->get_robot_ptr() );
412  */
413  // New method. Grab all bodies in __planner_env that are grabbed in __viewer_env by this manipulator
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());
424 
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());
428  }
429  }
430  }
431 #endif
432 }
433 
434 bool
435 JacoBimanualOpenraveThread::_plan_path()
436 {
437 #ifdef HAVE_OPENRAVE
438  _set_trajec_state(TRAJEC_PLANNING);
439 
440  EnvironmentMutex::scoped_lock lock(__planner_env.env->get_env_ptr()->GetMutex());
441 
442  // Set active DOFs
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);
448 
449  // setup command for dualmanipulation module
450  stringstream cmdin,cmdout;
451  cmdin << std::setprecision(numeric_limits<dReal>::digits10+1);
452  cmdout << std::setprecision(numeric_limits<dReal>::digits10+1);
453 
454  vector<dReal> sol;
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];
460  }
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];
465  }
466 
467  //add additional planner parameters
468  if( !__plannerparams.empty() ) {
469  cmdin << " " << __plannerparams;
470  }
471 
472  //constrain planning if required
473  if( __constrained ) {
474  cmdin << " constrainterrorthresh 1";
475  }
476 
477  cmdin << " execute 0";
478  cmdin << " outputtraj";
479  //logger->log_debug(name(), "Planner: dualmanip cmdin:%s", cmdin.str().c_str());
480 
481  // plan path
482  bool success = false;
483  try {
484  success = __mod_dualmanip->SendCommand(cmdout,cmdin);
485  } catch(openrave_exception &e) {
486  logger->log_debug(name(), "Planner: dualmanip command failed. Ex:%s", e.what());
487  }
488 
489  if(!success) {
490  logger->log_warn(name(),"Planner: planning failed");
491  _set_trajec_state(TRAJEC_PLANNING_ERROR);
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();
496  return false;
497 
498  } else {
499  //logger->log_debug(name(), "Planner: path planned. cmdout:%s", cmdout.str().c_str());
500 
501  // read returned trajectory
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) ) {
506  logger->log_warn(name(), "Planner: Cannot read trajectory data.");
507  _set_trajec_state(TRAJEC_PLANNING_ERROR);
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();
512  return false;
513 
514  } else {
515  // sampling trajectory and setting target trajectory
516  jaco_trajec_t* trajec_l = new jaco_trajec_t();
517  jaco_trajec_t* trajec_r = new jaco_trajec_t();
518  jaco_trajec_point_t p; // point we will add to trajectories
519  vector<dReal> tmp_p;
520  int arm_dof = cfg_spec.GetDOF() / 2;
521 
522  for(dReal time = 0; time <= traj->GetDuration(); time += (dReal)__cfg_OR_sampling) {
523  vector<dReal> point;
524  traj->Sample(point,time);
525 
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);
529 
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);
533  }
534 
535  __arms.left.arm->target_mutex->lock();
536  __arms.right.arm->target_mutex->lock();
537  __arms.left.target->trajec = RefPtr<jaco_trajec_t>( trajec_l );
538  __arms.right.target->trajec = RefPtr<jaco_trajec_t>( trajec_r );
539  // update target.
540  // set target->pos accordingly. This makes final-checking in goto_thread much easier
541  __arms.left.target->pos = trajec_l->back();
542  __arms.right.target->pos = trajec_r->back();
543  __arms.left.target->trajec_state=TRAJEC_READY;
544  __arms.right.target->trajec_state=TRAJEC_READY;
545  __arms.left.arm->target_mutex->unlock();
546  __arms.right.arm->target_mutex->unlock();
547 
548  return true;
549  }
550  }
551 #endif
552 
553  return false;
554 }
555 
556 bool
557 JacoBimanualOpenraveThread::_solve_multi_ik(vector<float> &left, vector<float> &right)
558 {
559 #ifndef HAVE_OPENRAVE
560  return false;
561 #else
562  EnvironmentMutex::scoped_lock plan_lock(__planner_env.env->get_env_ptr()->GetMutex());
563 
564  // robot ptr for convenienc
565  RobotBasePtr robot = __planner_env.robot->get_robot_ptr();
566 
567  // get grabbed bodies
568  vector<KinBodyPtr> grabbed;
569  robot->GetGrabbed(grabbed);
570 
571  // save state of grabbed bodies
572  vector<KinBody::KinBodyStateSaver> statesaver;
573  for( vector<KinBodyPtr>::iterator body=grabbed.begin(); body!=grabbed.end(); ++body ) {
574  statesaver.push_back(KinBody::KinBodyStateSaver(*body));
575  }
576 
577  // get IK solutions for both arms
578  vector< vector<dReal> > solutions_l, solutions_r;
579  {
580  // save state of robot
581  RobotBase::RobotStateSaver robot_saver(robot);
582 
583  vector<KinBody::LinkPtr> all_links = robot->GetLinks();
584  // Find IK solutions for left arm
585  // Disable all links of right manipulator
586  for( set<KinBody::LinkPtr>::iterator body=links_right_.begin(); body!=links_right_.end(); ++body) {
587  (*body)->Enable(false);
588  }
589  // Enable only grabbed bodies of this manipulator
590  for( vector<KinBodyPtr>::iterator body=grabbed.begin(); body!=grabbed.end(); ++body) {
591  (*body)->Enable(__arms.left.manip->IsGrabbing(*body));
592  }
593  // Get Ik Solutions.
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() ) {
602  logger->log_warn(name(), "No IK solutions found for left arm");
603  return false;
604  } else {
605  logger->log_debug(name(), "IK solution found for left arm");
606  }
607 
608  // now same for right arm. but enable links of right manipulator first
609  for( set<KinBody::LinkPtr>::iterator body=links_right_.begin(); body!=links_right_.end(); ++body) {
610  (*body)->Enable(true);
611  }
612  // Disable all links of left manipulator
613  for( set<KinBody::LinkPtr>::iterator body=links_left_.begin(); body!=links_left_.end(); ++body) {
614  (*body)->Enable(false);
615  }
616  // Enable only grabbed bodies of this manipulator
617  for( vector<KinBodyPtr>::iterator body=grabbed.begin(); body!=grabbed.end(); ++body) {
618  (*body)->Enable(__arms.right.manip->IsGrabbing(*body));
619  }
620  // Get Ik Solutions.
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() ) {
629  logger->log_warn(name(), "No IK solutions found for right arm");
630  return false;
631  } else {
632  logger->log_debug(name(), "IK solution found for right arm");
633  }
634  } // robot state-saver destroyed
635 
636  // restore kinbody states
637  for( vector<KinBody::KinBodyStateSaver>::iterator s=statesaver.begin(); s!=statesaver.end(); ++s ) {
638  (*s).Restore();
639  }
640 
641  // finally find the closest solutions without collision and store them
642  bool solution_found = false;
643  {
644  // save state of robot
645  RobotBase::RobotStateSaver robot_saver(robot);
646  vector< vector<dReal> >::iterator sol_l, sol_r;
647 
648  float dist = 100.f;
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);
653 
654  // try each combination to find closest non-colliding
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 ) {
657  // set joints for robot model
658  robot->SetDOFValues(*sol_l, 1, __arms.left.manip->GetArmIndices());
659  robot->SetDOFValues(*sol_r, 1, __arms.right.manip->GetArmIndices());
660 
661  // check for collisions
662  if( !robot->CheckSelfCollision() && !robot->GetEnv()->CheckCollision(robot) ) {
663  //logger->log_debug(name(), "Collision-free solution found!");
664  // calculate distance
665  float dist_l = 0.f;
666  float dist_r = 0.f;
667  diff_l = cur_l;
668  diff_r = cur_r;
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]);
673  // use cur+diff instead of sol, to have better angles
674  // for circular joints. Otherwise planner might have problems
675  (*sol_l)[i] = cur_l[i] - diff_l[i];
676  }
677  //logger->log_debug(name(), "Distance left: %f", dist_l);
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];
681  }
682  //logger->log_debug(name(), "Distance right: %f", dist_r);
683 
684  if( dist_l+dist_r < dist ) {
685  //logger->log_debug(name(), "Dist %f is closer that previous one (%f). Take this!", dist_l+dist_r, dist);
686  dist = dist_l + dist_r;
687  solution_found = true;
688  left.clear();
689  right.clear();
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);
694  }
695  } else {
696  //logger->log_debug(name(), "Skipping solution because of collision!");
697  }
698  }
699  }
700  } // robot state-saver destroyed
701 
702 
703  return solution_found;
704 #endif
705 }
trajectory has been planned and is ready for execution.
Definition: types.h:74
ZXZ rotation.
Definition: types.h:48
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.
STL namespace.
virtual void finalize()
Finalize the thread.
std::vector< jaco_trajec_point_t > jaco_trajec_t
A trajectory.
Definition: types.h:51
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
jaco_target_type_t type
target type.
Definition: types.h:82
planner could not plan a collision-free trajectory.
Definition: types.h:77
jaco_arm_t * left
the struct with all the data for the left arm.
Definition: types.h:113
jaco_arm_t * right
the struct with all the data for the right arm.
Definition: types.h:114
jaco_trajec_point_t pos
target position (interpreted depending on target type).
Definition: types.h:83
jaco_trajec_state_t trajec_state
state of the trajectory, if target is TARGET_TRAJEC.
Definition: types.h:86
std::vector< float > jaco_trajec_point_t
A trajectory point.
Definition: types.h:46
OpenRAVE Robot class.
Definition: robot.h:39
Class containing information about all Kinova Jaco motors.
Definition: kinova_jaco.h:33
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual void set_constrained(bool enable)
Enable/Disable constrained planning.
target with cartesian coordinates.
Definition: types.h:62
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.
Definition: types.h:81
target with angular coordinates.
Definition: types.h:63
const char * name() const
Get name of thread.
Definition: thread.h:95
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
Definition: exception.cpp:686
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
planner is planning the trajectory.
Definition: types.h:73
virtual void loop()
Code to execute in the thread.
bool coord
this target needs to be coordinated with targets of other arms.
Definition: types.h:87
planner could not find IK solution for target
Definition: types.h:76
RefPtr<> is a reference-counting shared smartpointer.
Definition: refptr.h:49
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.
Definition: types.h:72
Jaco struct containing all components required for a dual-arm setup.
Definition: types.h:112
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:44
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.