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 
25 #include "arm.h"
26 #include "types.h"
27 
28 #include <core/threading/mutex.h>
29 
30 #include <algorithm>
31 #include <cmath>
32 #include <cstring>
33 #include <stdio.h>
34 #include <unistd.h>
35 
36 #ifdef HAVE_OPENRAVE
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;
43 #endif
44 
45 using namespace fawkes;
46 using namespace std;
47 
48 /** @class JacoBimanualOpenraveThread "bimanual_openrave_thread.h"
49  * Jaco Arm thread for dual-arm setup, integrating OpenRAVE
50  *
51  * @author Bahram Maleki-Fard
52  */
53 
54 /** Constructor.
55  * @param arms pointer to jaco_dual_arm_t struct, to be used in this thread
56  */
58 : JacoOpenraveBaseThread("JacoBimanualOpenraveThread")
59 {
60  arms_.left.arm = arms->left;
61  arms_.right.arm = arms->right;
62 #ifdef HAVE_OPENRAVE
63  planner_env_.env = NULL;
64  planner_env_.robot = NULL;
65  planner_env_.manip = NULL;
66 #endif
67 
68  __constrained = false;
69 }
70 
71 void
72 JacoBimanualOpenraveThread::_init()
73 {
74 #ifdef HAVE_OPENRAVE
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");
77 #endif
78 }
79 
80 void
81 JacoBimanualOpenraveThread::_load_robot()
82 {
83 #ifdef HAVE_OPENRAVE
84  cfg_OR_robot_file_ = config->get_string("/hardware/jaco/openrave/robot_dual_file");
85 
86  try {
87  //viewer_env_.robot = openrave->add_robot(cfg_OR_robot_file_, false);
88  // manually add robot; the automatic needs to be altered
89  viewer_env_.robot = new OpenRaveRobot(logger);
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);
94  } catch (Exception &e) {
95  throw fawkes::Exception("Could not add robot '%s' to openrave environment. (Error: %s)",
96  cfg_OR_robot_file_.c_str(),
97  e.what_no_backtrace());
98  }
99 
100  try {
101  viewer_env_.manip = new OpenRaveManipulatorKinovaJaco(6, 6);
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);
108 
109  // Set manipulator and offsets.
110  openrave->set_manipulator(viewer_env_.robot, viewer_env_.manip, 0.f, 0.f, 0.f);
111 
112  EnvironmentMutex::scoped_lock lock(viewer_env_.env->get_env_ptr()->GetMutex());
113 
114  arms_.right.manip =
115  viewer_env_.robot->get_robot_ptr()->SetActiveManipulator(arms_.right.manipname);
116  if (cfg_OR_auto_load_ik_) {
117  logger->log_debug(name(), "load IK for right arm");
118  viewer_env_.env->load_IK_solver(viewer_env_.robot, OpenRAVE::IKP_Transform6D);
119  }
120 
121  arms_.left.manip =
122  viewer_env_.robot->get_robot_ptr()->SetActiveManipulator(arms_.left.manipname);
123  if (cfg_OR_auto_load_ik_) {
124  logger->log_debug(name(), "load IK for left arm");
125  viewer_env_.env->load_IK_solver(viewer_env_.robot, OpenRAVE::IKP_Transform6D);
126  }
127 
128  } catch (Exception &e) {
129  finalize();
130  throw;
131  }
132 
133  // create cloned environment for planning
134  logger->log_debug(name(), "Clone environment for planning");
135  openrave->clone(planner_env_.env, planner_env_.robot, planner_env_.manip);
136 
137  if (!planner_env_.env || !planner_env_.robot || !planner_env_.manip) {
138  throw fawkes::Exception("Could not clone properly, received a NULL pointer");
139  }
140 
141  // set name of env
142  planner_env_.env->set_name("Planner_Bimanual");
143 
144  // set manips to those of planner env
145  arms_.right.manip =
146  planner_env_.robot->get_robot_ptr()->SetActiveManipulator(arms_.right.manipname);
147  arms_.left.manip =
148  planner_env_.robot->get_robot_ptr()->SetActiveManipulator(arms_.left.manipname);
149 
150  // initial modules for dualmanipulation
151  _init_dualmanipulation();
152 #endif
153 }
154 
155 void
156 JacoBimanualOpenraveThread::_init_dualmanipulation()
157 {
158 #ifdef HAVE_OPENRAVE
159  // load dualmanipulation module
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_,
163  true,
164  planner_env_.robot->get_robot_ptr()->GetName());
165 
166  // load MultiManipIkSolver stuff
167  // Get all the links that are affecte by left/right manipulator
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());
176 
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();
180  ++link) {
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())) {
185  // this link is affected by left manipulator
186  links_left_.insert(*link);
187  arm_idx_l.erase(idx); // no need to check this one again
188  affected = true;
189  break;
190  }
191  }
192 
193  if (affected)
194  continue;
195 
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())) {
199  // this link is affected by right manipulator
200  links_right_.insert(*link);
201  arm_idx_r.erase(idx); // no need to check this one again
202  break;
203  }
204  }
205  }
206 #endif
207 }
208 
209 void
211 {
212  arms_.left.arm = NULL;
213  arms_.right.arm = NULL;
214 #ifdef HAVE_OPENRAVE
215  openrave->set_active_robot(NULL);
216 
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;
221 #endif
222 
224 }
225 
226 void
228 {
229 #ifndef HAVE_OPENRAVE
230  usleep(30e3);
231 #else
232  if (arms_.left.arm == NULL || arms_.right.arm == NULL) {
233  usleep(30e3);
234  return;
235  }
236 
237  // get first target in queues
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();
243  }
244  arms_.left.arm->target_mutex->unlock();
245  arms_.right.arm->target_mutex->unlock();
246 
247  if (!arms_.left.target || !arms_.right.target || !arms_.left.target->coord
248  || !arms_.right.target->coord || arms_.left.target->trajec_state != TRAJEC_WAITING
249  || arms_.right.target->trajec_state != TRAJEC_WAITING) {
250  //no new target in queue, or target is not meant for coordinated bimanual manipulation
251  usleep(30e3);
252  return;
253  }
254 
255  // copy environment first
256  _copy_env();
257 
258  // get suiting IK solutions
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();
263  if (!solvable) {
264  arms_.left.target->trajec_state = TRAJEC_IK_ERROR;
265  arms_.right.target->trajec_state = TRAJEC_IK_ERROR;
266  arms_.left.arm->target_mutex->unlock();
267  arms_.right.arm->target_mutex->unlock();
268  usleep(30e3);
269  return;
270  } else {
271  arms_.left.target->type = TARGET_ANGULAR;
272  arms_.left.target->pos = sol_l;
273  arms_.right.target->type = TARGET_ANGULAR;
274  arms_.right.target->pos = sol_r;
275  arms_.left.arm->target_mutex->unlock();
276  arms_.right.arm->target_mutex->unlock();
277 
278  // run path planner
279  _plan_path();
280  }
281 
282 #endif
283 }
284 
285 /** Enable/Disable constrained planning.
286  * Enabling it will constrain the movement, which means it is tried to
287  * maintain the distance of the grippers to each other. This should be
288  * activated when moving an object with both hands, but disabled in
289  * situations when the arms do not need to hold the object simultaneously
290  * at all times.
291  *
292  * @param enable Enables/Disables the state.
293  */
294 void
296 {
297  __constrained = enable;
298 }
299 
300 /** Add target for coordinated manipulation to the queue.
301  *
302  * This adds targets to the queues for both left and right arms. It sets
303  * the target->coord flag to "true", which means it will not be processed
304  * by the threads for uncoordinated manipulation!
305  *
306  * @param l_x x-coordinate of target position of left arm
307  * @param l_y y-coordinate of target position of left arm
308  * @param l_z z-coordinate of target position of left arm
309  * @param l_e1 1st euler rotation of target orientation of left arm
310  * @param l_e2 2nd euler rotation of target orientation of left arm
311  * @param l_e3 3rd euler rotation of target orientation of left arm
312  * @param r_x x-coordinate of target position of right arm
313  * @param r_y y-coordinate of target position of right arm
314  * @param r_z z-coordinate of target position of right arm
315  * @param r_e1 1st euler rotation of target orientation of right arm
316  * @param r_e2 2nd euler rotation of target orientation of right arm
317  * @param r_e3 3rd euler rotation of target orientation of right arm
318  * @return "true", if target could be added to queue.
319  */
320 bool
322  float l_y,
323  float l_z,
324  float l_e1,
325  float l_e2,
326  float l_e3,
327  float r_x,
328  float r_y,
329  float r_z,
330  float r_e1,
331  float r_e2,
332  float r_e3)
333 {
334 #ifdef HAVE_OPENRAVE
335  // no IK checking yet, just enqueue until they can be processed
336  // create new targets for the queues
337  RefPtr<jaco_target_t> target_l(new jaco_target_t());
338  target_l->type = TARGET_CARTESIAN;
339  target_l->trajec_state = TRAJEC_WAITING;
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);
347 
348  RefPtr<jaco_target_t> target_r(new jaco_target_t());
349  target_r->type = TARGET_CARTESIAN;
350  target_r->trajec_state = TRAJEC_WAITING;
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);
358 
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();
365 
366  return true;
367 #else
368  return false;
369 #endif
370 }
371 
372 void
374 {
375  // do nothing, this thread is only for plannning!
376 }
377 
378 void
380 {
381 }
382 
383 void
384 JacoBimanualOpenraveThread::_set_trajec_state(jaco_trajec_state_t state)
385 {
386 #ifdef HAVE_OPENRAVE
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();
393 #endif
394 }
395 
396 void
397 JacoBimanualOpenraveThread::_copy_env()
398 {
399 #ifdef HAVE_OPENRAVE
400  // Update bodies in planner-environment
401  // clone robot state, ignoring grabbed bodies
402  {
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();
407 
408  /*
409  // Old method. Somehow we encountered problems. OpenRAVE internal bug?
410  RobotBase::RobotStateSaver saver(viewer_env_.robot->get_robot_ptr(),
411  0xffffffff&~KinBody::Save_GrabbedBodies&~KinBody::Save_ActiveManipulator&~KinBody::Save_ActiveDOF);
412  saver.Restore( planner_env_.robot->get_robot_ptr() );
413  */
414  // New method. Simply set the DOF values as they are in viewer_env_
415  vector<dReal> dofs;
416  viewer_env_.robot->get_robot_ptr()->GetDOFValues(dofs);
417  planner_env_.robot->get_robot_ptr()->SetDOFValues(dofs);
418  }
419 
420  // then clone all objects
421  planner_env_.env->clone_objects(viewer_env_.env);
422 
423  // update robot state with attached objects
424  {
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());
427  /*
428  // Old method. Somehow we encountered problems. OpenRAVE internal bug?
429  RobotBase::RobotStateSaver saver(viewer_env_.robot->get_robot_ptr(),
430  KinBody::Save_LinkTransformation|KinBody::Save_LinkEnable|KinBody::Save_GrabbedBodies);
431  saver.Restore( planner_env_.robot->get_robot_ptr() );
432  */
433  // New method. Grab all bodies in planner_env_ that are grabbed in viewer_env_ by this manipulator
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();
437  ++it) {
438  logger->log_debug(name(),
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()) {
444  logger->log_debug(name(),
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(),
449  planner_env_.env,
450  arms_.left.manipname.c_str());
451 
452  } else if ((*it)->_robotlinkname == arms_.right.manip->GetEndEffector()->GetName()) {
453  logger->log_debug(name(),
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(),
458  planner_env_.env,
459  arms_.right.manipname.c_str());
460  }
461  }
462  }
463 #endif
464 }
465 
466 bool
467 JacoBimanualOpenraveThread::_plan_path()
468 {
469 #ifdef HAVE_OPENRAVE
470  _set_trajec_state(TRAJEC_PLANNING);
471 
472  EnvironmentMutex::scoped_lock lock(planner_env_.env->get_env_ptr()->GetMutex());
473 
474  // Set active DOFs
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);
480 
481  // setup command for dualmanipulation module
482  stringstream cmdin, cmdout;
483  cmdin << std::setprecision(numeric_limits<dReal>::digits10 + 1);
484  cmdout << std::setprecision(numeric_limits<dReal>::digits10 + 1);
485 
486  vector<dReal> sol;
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];
492  }
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];
497  }
498 
499  //add additional planner parameters
500  if (!plannerparams_.empty()) {
501  cmdin << " " << plannerparams_;
502  }
503 
504  //constrain planning if required
505  if (__constrained) {
506  cmdin << " constrainterrorthresh 1";
507  }
508 
509  cmdin << " execute 0";
510  cmdin << " outputtraj";
511  //logger->log_debug(name(), "Planner: dualmanip cmdin:%s", cmdin.str().c_str());
512 
513  // plan path
514  bool success = false;
515  try {
516  success = mod_dualmanip_->SendCommand(cmdout, cmdin);
517  } catch (openrave_exception &e) {
518  logger->log_debug(name(), "Planner: dualmanip command failed. Ex:%s", e.what());
519  }
520 
521  if (!success) {
522  logger->log_warn(name(), "Planner: planning failed");
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();
528  return false;
529 
530  } else {
531  //logger->log_debug(name(), "Planner: path planned. cmdout:%s", cmdout.str().c_str());
532 
533  // read returned trajectory
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)) {
539  logger->log_warn(name(), "Planner: Cannot read trajectory data.");
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();
545  return false;
546 
547  } else {
548  // sampling trajectory and setting target trajectory
549  jaco_trajec_t * trajec_l = new jaco_trajec_t();
550  jaco_trajec_t * trajec_r = new jaco_trajec_t();
551  jaco_trajec_point_t p; // point we will add to trajectories
552  vector<dReal> tmp_p;
553  int arm_dof = cfg_spec.GetDOF() / 2;
554 
555  for (dReal time = 0; time <= traj->GetDuration(); time += (dReal)cfg_OR_sampling_) {
556  vector<dReal> point;
557  traj->Sample(point, time);
558 
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);
562 
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);
566  }
567 
568  arms_.left.arm->target_mutex->lock();
569  arms_.right.arm->target_mutex->lock();
570  arms_.left.target->trajec = RefPtr<jaco_trajec_t>(trajec_l);
571  arms_.right.target->trajec = RefPtr<jaco_trajec_t>(trajec_r);
572  // update target.
573  // set target->pos accordingly. This makes final-checking in goto_thread much easier
574  arms_.left.target->pos = trajec_l->back();
575  arms_.right.target->pos = trajec_r->back();
576  arms_.left.target->trajec_state = TRAJEC_READY;
577  arms_.right.target->trajec_state = TRAJEC_READY;
578  arms_.left.arm->target_mutex->unlock();
579  arms_.right.arm->target_mutex->unlock();
580 
581  return true;
582  }
583  }
584 #endif
585 
586  return false;
587 }
588 
589 bool
590 JacoBimanualOpenraveThread::_solve_multi_ik(vector<float> &left, vector<float> &right)
591 {
592 #ifndef HAVE_OPENRAVE
593  return false;
594 #else
595  EnvironmentMutex::scoped_lock plan_lock(planner_env_.env->get_env_ptr()->GetMutex());
596 
597  // robot ptr for convenienc
598  RobotBasePtr robot = planner_env_.robot->get_robot_ptr();
599 
600  // get grabbed bodies
601  vector<KinBodyPtr> grabbed;
602  robot->GetGrabbed(grabbed);
603 
604  // save state of grabbed bodies
605  vector<KinBody::KinBodyStateSaver> statesaver;
606  for (vector<KinBodyPtr>::iterator body = grabbed.begin(); body != grabbed.end(); ++body) {
607  statesaver.push_back(KinBody::KinBodyStateSaver(*body));
608  }
609 
610  // get IK solutions for both arms
611  vector<vector<dReal>> solutions_l, solutions_r;
612  {
613  // save state of robot
614  RobotBase::RobotStateSaver robot_saver(robot);
615 
616  // Find IK solutions for left arm
617  // Disable all links of right manipulator
618  for (set<KinBody::LinkPtr>::iterator body = links_right_.begin(); body != links_right_.end();
619  ++body) {
620  (*body)->Enable(false);
621  }
622  // Enable only grabbed bodies of this manipulator
623  for (vector<KinBodyPtr>::iterator body = grabbed.begin(); body != grabbed.end(); ++body) {
624  (*body)->Enable(arms_.left.manip->IsGrabbing(*body));
625  }
626  // Get Ik Solutions.
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()) {
639  logger->log_warn(name(), "No IK solutions found for left arm");
640  return false;
641  } else {
642  logger->log_debug(name(), "IK solution found for left arm");
643  }
644 
645  // now same for right arm. but enable links of right manipulator first
646  for (set<KinBody::LinkPtr>::iterator body = links_right_.begin(); body != links_right_.end();
647  ++body) {
648  (*body)->Enable(true);
649  }
650  // Disable all links of left manipulator
651  for (set<KinBody::LinkPtr>::iterator body = links_left_.begin(); body != links_left_.end();
652  ++body) {
653  (*body)->Enable(false);
654  }
655  // Enable only grabbed bodies of this manipulator
656  for (vector<KinBodyPtr>::iterator body = grabbed.begin(); body != grabbed.end(); ++body) {
657  (*body)->Enable(arms_.right.manip->IsGrabbing(*body));
658  }
659  // Get Ik Solutions.
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()) {
672  logger->log_warn(name(), "No IK solutions found for right arm");
673  return false;
674  } else {
675  logger->log_debug(name(), "IK solution found for right arm");
676  }
677  } // robot state-saver destroyed
678 
679  // restore kinbody states
680  for (vector<KinBody::KinBodyStateSaver>::iterator s = statesaver.begin(); s != statesaver.end();
681  ++s) {
682  (*s).Restore();
683  }
684 
685  // finally find the closest solutions without collision and store them
686  bool solution_found = false;
687  {
688  // save state of robot
689  RobotBase::RobotStateSaver robot_saver(robot);
690  vector<vector<dReal>>::iterator sol_l, sol_r;
691 
692  float dist = 100.f;
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);
697 
698  // try each combination to find closest non-colliding
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) {
701  // set joints for robot model
702  robot->SetDOFValues(*sol_l, 1, arms_.left.manip->GetArmIndices());
703  robot->SetDOFValues(*sol_r, 1, arms_.right.manip->GetArmIndices());
704 
705  // check for collisions
706  if (!robot->CheckSelfCollision() && !robot->GetEnv()->CheckCollision(robot)) {
707  //logger->log_debug(name(), "Collision-free solution found!");
708  // calculate distance
709  float dist_l = 0.f;
710  float dist_r = 0.f;
711  diff_l = cur_l;
712  diff_r = cur_r;
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]);
717  // use cur+diff instead of sol, to have better angles
718  // for circular joints. Otherwise planner might have problems
719  (*sol_l)[i] = cur_l[i] - diff_l[i];
720  }
721  //logger->log_debug(name(), "Distance left: %f", dist_l);
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];
725  }
726  //logger->log_debug(name(), "Distance right: %f", dist_r);
727 
728  if (dist_l + dist_r < dist) {
729  //logger->log_debug(name(), "Dist %f is closer that previous one (%f). Take this!", dist_l+dist_r, dist);
730  dist = dist_l + dist_r;
731  solution_found = true;
732  left.clear();
733  right.clear();
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);
738  }
739  } else {
740  //logger->log_debug(name(), "Skipping solution because of collision!");
741  }
742  }
743  }
744  } // robot state-saver destroyed
745 
746  return solution_found;
747 #endif
748 }
JacoBimanualOpenraveThread::update_openrave
virtual void update_openrave()
Update the openrave environment to represent the current situation.
Definition: bimanual_openrave_thread.cpp:373
JacoOpenraveBaseThread::finalize
virtual void finalize()
Finalize the thread.
Definition: openrave_base_thread.cpp:111
fawkes::jaco_dual_arm_struct::right
jaco_arm_t * right
the struct with all the data for the right arm.
Definition: types.h:115
fawkes::RefPtr
RefPtr<> is a reference-counting shared smartpointer.
Definition: refptr.h:50
fawkes::jaco_target_struct_t::type
jaco_target_type_t type
target type.
Definition: types.h:80
JacoBimanualOpenraveThread::plot_first
virtual void plot_first()
Plot the first target of the target_queue, if it is a trajectory.
Definition: bimanual_openrave_thread.cpp:379
JacoBimanualOpenraveThread::JacoBimanualOpenraveThread
JacoBimanualOpenraveThread(fawkes::jaco_dual_arm_t *arms)
Constructor.
Definition: bimanual_openrave_thread.cpp:57
fawkes::Thread::name
const char * name() const
Get name of thread.
Definition: thread.h:100
fawkes::OpenRaveRobot
OpenRAVE Robot class.
Definition: robot.h:38
JacoBimanualOpenraveThread::add_target
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.
Definition: bimanual_openrave_thread.cpp:321
fawkes::jaco_trajec_state_t
enum fawkes::jaco_trajec_state_enum jaco_trajec_state_t
The state of a trajectory.
fawkes::TARGET_ANGULAR
@ TARGET_ANGULAR
target with angular coordinates.
Definition: types.h:60
fawkes::jaco_target_struct_t::pos
jaco_trajec_point_t pos
target position (interpreted depending on target type).
Definition: types.h:81
JacoBimanualOpenraveThread::set_constrained
virtual void set_constrained(bool enable)
Enable/Disable constrained planning.
Definition: bimanual_openrave_thread.cpp:295
fawkes::jaco_target_struct_t::trajec_state
jaco_trajec_state_t trajec_state
state of the trajectory, if target is TARGET_TRAJEC.
Definition: types.h:84
JacoBimanualOpenraveThread::finalize
virtual void finalize()
Finalize the thread.
Definition: bimanual_openrave_thread.cpp:210
fawkes::LoggingAspect::logger
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
fawkes::jaco_dual_arm_struct
Jaco struct containing all components required for a dual-arm setup.
Definition: types.h:113
JacoOpenraveBaseThread
Base Jaco Arm thread, integrating OpenRAVE.
Definition: openrave_base_thread.h:62
fawkes
Fawkes library namespace.
fawkes::OpenRaveManipulatorKinovaJaco
Class containing information about all Kinova Jaco motors.
Definition: kinova_jaco.h:31
fawkes::jaco_target_struct_t
Jaco target struct, holding information on a target.
Definition: types.h:79
fawkes::TARGET_CARTESIAN
@ TARGET_CARTESIAN
target with cartesian coordinates.
Definition: types.h:59
fawkes::TRAJEC_WAITING
@ TRAJEC_WAITING
new trajectory target, wait for planner to process.
Definition: types.h:69
fawkes::Logger::log_warn
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
fawkes::jaco_dual_arm_struct::left
jaco_arm_t * left
the struct with all the data for the left arm.
Definition: types.h:114
fawkes::ConfigurableAspect::config
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
fawkes::jaco_trajec_t
std::vector< jaco_trajec_point_t > jaco_trajec_t
A trajectory.
Definition: types.h:48
fawkes::Exception::what_no_backtrace
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
Definition: exception.cpp:663
fawkes::Configuration::get_string
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
fawkes::jaco_trajec_point_t
std::vector< float > jaco_trajec_point_t
A trajectory point.
Definition: types.h:43
fawkes::jaco_target_struct_t::coord
bool coord
this target needs to be coordinated with targets of other arms.
Definition: types.h:85
fawkes::TRAJEC_READY
@ TRAJEC_READY
trajectory has been planned and is ready for execution.
Definition: types.h:71
fawkes::Logger::log_debug
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
JacoBimanualOpenraveThread::loop
virtual void loop()
Code to execute in the thread.
Definition: bimanual_openrave_thread.cpp:227
fawkes::TRAJEC_IK_ERROR
@ TRAJEC_IK_ERROR
planner could not find IK solution for target
Definition: types.h:73
fawkes::Exception
Base class for exceptions in Fawkes.
Definition: exception.h:36