Fawkes API  Fawkes Development Version
openrave_thread.cpp
1 
2 /***************************************************************************
3  * openrave_thread.cpp - Jaco plugin OpenRAVE Thread for single arm
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 "openrave_thread.h"
24 
25 #include "arm.h"
26 #include "types.h"
27 
28 #include <core/threading/mutex.h>
29 #include <interfaces/JacoInterface.h>
30 #include <utils/math/angle.h>
31 
32 #include <cmath>
33 #include <cstring>
34 #include <stdio.h>
35 #include <unistd.h>
36 
37 #ifdef HAVE_OPENRAVE
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 
43 using namespace OpenRAVE;
44 #endif
45 
46 using namespace fawkes;
47 using namespace std;
48 
49 /** @class JacoOpenraveThread "openrave_thread.h"
50  * Jaco Arm thread for single-arm setup, integrating OpenRAVE
51  *
52  * @author Bahram Maleki-Fard
53  */
54 
55 /** Constructor.
56  * @param name thread name
57  * @param arm pointer to jaco_arm_t struct, to be used in this thread
58  * @param load_robot decide if this thread should load a robot. This should only
59  * be set to "true" if a separate OpenRaveRobot should be loaded (e.g. not the
60  * case when using 1 robot with 2 manipulators!)
61  */
62 JacoOpenraveThread::JacoOpenraveThread(const char *name, jaco_arm_t *arm, bool load_robot)
64 {
65  arm_ = arm;
66  load_robot_ = load_robot;
67 #ifdef HAVE_OPENRAVE
68  planner_env_.env = NULL;
69  planner_env_.robot = NULL;
70  planner_env_.manip = NULL;
71 
72  plotted_current_ = false;
73 #endif
74 }
75 
76 /** Get additional config entries. */
77 void
78 JacoOpenraveThread::_init()
79 {
80  switch (arm_->config) {
81  case CONFIG_SINGLE:
82  cfg_manipname_ = config->get_string("/hardware/jaco/openrave/manipname/single");
83  break;
84 
85  case CONFIG_LEFT:
86  cfg_manipname_ = config->get_string("/hardware/jaco/openrave/manipname/dual_left");
87  break;
88 
89  case CONFIG_RIGHT:
90  cfg_manipname_ = config->get_string("/hardware/jaco/openrave/manipname/dual_right");
91  break;
92 
93  default: throw fawkes::Exception("Could not read manipname from config."); break;
94  }
95 }
96 
97 /** Load the robot into the environment. */
98 void
99 JacoOpenraveThread::_load_robot()
100 {
101 #ifdef HAVE_OPENRAVE
102 
103  if (load_robot_) {
104  cfg_OR_robot_file_ = config->get_string("/hardware/jaco/openrave/robot_file");
105 
106  try {
107  viewer_env_.robot = openrave->add_robot(cfg_OR_robot_file_, false);
108  } catch (Exception &e) {
109  throw fawkes::Exception("Could not add robot '%s' to openrave environment. (Error: %s)",
110  cfg_OR_robot_file_.c_str(),
111  e.what_no_backtrace());
112  }
113 
114  try {
115  viewer_env_.manip = new OpenRaveManipulatorKinovaJaco(6, 6);
116  viewer_env_.manip->add_motor(0, 0);
117  viewer_env_.manip->add_motor(1, 1);
118  viewer_env_.manip->add_motor(2, 2);
119  viewer_env_.manip->add_motor(3, 3);
120  viewer_env_.manip->add_motor(4, 4);
121  viewer_env_.manip->add_motor(5, 5);
122 
123  // Set manipulator and offsets.
124  openrave->set_manipulator(viewer_env_.robot, viewer_env_.manip, 0.f, 0.f, 0.f);
125 
126  openrave->get_environment()->load_IK_solver(viewer_env_.robot, OpenRAVE::IKP_Transform6D);
127 
128  } catch (Exception &e) {
129  finalize();
130  throw;
131  }
132 
133  } else {
134  // robot was not loaded by this thread. So get them from openrave-environment now
135  try {
136  viewer_env_.robot = openrave->get_active_robot();
137  viewer_env_.manip = viewer_env_.robot->get_manipulator()->copy();
138  } catch (Exception &e) {
139  throw fawkes::Exception("%s: Could not get robot '%s' from openrave environment. (Error: %s)",
140  name(),
141  cfg_OR_robot_file_.c_str(),
142  e.what_no_backtrace());
143  }
144  }
145 
146 #endif //HAVE_OPENRAVE
147 }
148 
149 /** Get pointers to the robot and manipulator in the viewer_env, and
150  * clone the environment.
151  */
152 void
153 JacoOpenraveThread::_post_init()
154 {
155 #ifdef HAVE_OPENRAVE
156  while (!robot_) {
157  robot_ = viewer_env_.robot->get_robot_ptr();
158  usleep(100);
159  }
160  while (!manip_) {
161  EnvironmentMutex::scoped_lock lock(viewer_env_.env->get_env_ptr()->GetMutex());
162  manip_ = robot_->SetActiveManipulator(cfg_manipname_);
163  usleep(100);
164  }
165 
166  // create cloned environment for planning
167  logger->log_debug(name(), "Clone environment for planning");
168  openrave->clone(planner_env_.env, planner_env_.robot, planner_env_.manip);
169 
170  if (!planner_env_.env || !planner_env_.robot || !planner_env_.manip) {
171  throw fawkes::Exception("Could not clone properly, received a NULL pointer");
172  }
173 
174  // set name of environment
175  switch (arm_->config) {
176  case CONFIG_SINGLE: planner_env_.env->set_name("Planner"); break;
177 
178  case CONFIG_LEFT: planner_env_.env->set_name("Planner_Left"); break;
179 
180  case CONFIG_RIGHT: planner_env_.env->set_name("Planner_Right"); break;
181  }
182 
183  // set active manipulator in planning environment
184  {
185  EnvironmentMutex::scoped_lock lock(planner_env_.env->get_env_ptr()->GetMutex());
186  RobotBase::ManipulatorPtr manip =
187  planner_env_.robot->get_robot_ptr()->SetActiveManipulator(cfg_manipname_);
188  planner_env_.robot->get_robot_ptr()->SetActiveDOFs(manip->GetArmIndices());
189  }
190 
191  // Get chain of links from arm base to manipulator in viewer_env. Used for plotting joints
192  robot_->GetChain(manip_->GetBase()->GetIndex(), manip_->GetEndEffector()->GetIndex(), links_);
193 
194 #endif //HAVE_OPENRAVE
195 }
196 
197 void
199 {
200  arm_ = NULL;
201 #ifdef HAVE_OPENRAVE
202  if (load_robot_)
203  openrave->set_active_robot(NULL);
204 
205  planner_env_.robot = NULL;
206  planner_env_.manip = NULL;
207  planner_env_.env = NULL;
208 
210 #endif
211 }
212 
213 /** Mani loop.
214  * It iterates over the target_queue to find the first target that needs
215  * trajectory planning. This can be done if it is the first target,
216  * or if the previous target has a known final configuration, which can
217  * be used as the current starting configuration.
218  * The result is stored in the struct of the current target, which can
219  * then be processed by the goto_thread
220  *
221  * @see JacoGotoThread#loop to see how goto_thread processes the queue
222  */
223 void
225 {
226 #ifndef HAVE_OPENRAVE
227  usleep(30e3);
228 #else
229  if (arm_ == NULL || arm_->arm == NULL) {
230  usleep(30e3);
231  return;
232  }
233 
236  // get first target with type TARGET_TRAJEC that needs a planner
237  arm_->target_mutex->lock();
238  jaco_target_queue_t::iterator it;
239  for (it = arm_->target_queue->begin(); it != arm_->target_queue->end(); ++it) {
240  if ((*it)->trajec_state == TRAJEC_WAITING && !(*it)->coord) {
241  // have found a new target for path planning!
242  to = *it;
243  break;
244  }
245  }
246 
247  if (to) {
248  // Check if there is a prior target that can be usd as the starting position in planning.
249  // The only target-types that can be used for that are those that contain joint positions,
250  // i.e. TARGET_ANGULAR and TARGET_TRAJEC
252  while (it != arm_->target_queue->begin()) {
253  --it;
254  if ((*it)->trajec_state == TRAJEC_READY || (*it)->trajec_state == TRAJEC_EXECUTING) {
255  from = RefPtr<jaco_target_t>(new jaco_target_t());
256  from->pos = (*it)->trajec->back();
257  break;
258  } else if ((*it)->trajec_state == TRAJEC_SKIP && (*it)->type == TARGET_ANGULAR) {
259  from = *it;
260  break;
261  } else if (!((*it)->type == TARGET_GRIPPER)) {
262  // A previous target has unknown final configuration. Cannot plan for our target yet. Abort.
263  // TARGET_GRIPPER would be the only one we could skip without problems.
264  arm_->target_mutex->unlock();
265  usleep(30e3);
266  return;
267  }
268  }
269  arm_->target_mutex->unlock();
270 
271  // if there was no prior target that can be used as a starting position, create one
272  if (!from) {
273  from = RefPtr<jaco_target_t>(new jaco_target_t());
274  arm_->arm->get_joints(from->pos);
275  }
276 
277  // run planner
278  _plan_path(from, to);
280 
281  } else {
282  arm_->target_mutex->unlock();
284  usleep(30e3); // TODO: make this configurable
285  }
286 #endif
287 }
288 
289 void
291 {
292 #ifndef HAVE_OPENRAVE
293  return;
294 #else
295  if (arm_ == NULL || arm_->iface == NULL || robot_ == NULL || manip_ == NULL)
296  return;
297 
298  try {
299  joints_.clear();
300  joints_.push_back(arm_->iface->joints(0));
301  joints_.push_back(arm_->iface->joints(1));
302  joints_.push_back(arm_->iface->joints(2));
303  joints_.push_back(arm_->iface->joints(3));
304  joints_.push_back(arm_->iface->joints(4));
305  joints_.push_back(arm_->iface->joints(5));
306 
307  // get target IK values in openrave format
308  viewer_env_.manip->set_angles_device(joints_);
309  viewer_env_.manip->get_angles(joints_);
310 
311  {
312  EnvironmentMutex::scoped_lock lock(viewer_env_.env->get_env_ptr()->GetMutex());
313  robot_->SetDOFValues(joints_, 1, manip_->GetArmIndices());
314  }
315 
316  joints_.clear();
317  joints_.push_back(deg2rad(arm_->iface->finger1() - 40.f));
318  joints_.push_back(deg2rad(arm_->iface->finger2() - 40.f));
319  joints_.push_back(deg2rad(arm_->iface->finger3() - 40.f));
320  {
321  EnvironmentMutex::scoped_lock lock(viewer_env_.env->get_env_ptr()->GetMutex());
322  robot_->SetDOFValues(joints_, 1, manip_->GetGripperIndices());
323  }
324 
325  if (plot_current_) {
326  EnvironmentMutex::scoped_lock lock(viewer_env_.env->get_env_ptr()->GetMutex());
327 
328  if (!plotted_current_) {
329  // new plotting command. Erase previous plot
330  graph_current_.clear();
331  }
332 
333  if (cfg_OR_plot_cur_manip_) {
334  OpenRAVE::Vector color(0.f, 1.f, 0.f, 1.f);
335  const OpenRAVE::Vector &trans = manip_->GetEndEffectorTransform().trans;
336  float transa[4] = {(float)trans.x, (float)trans.y, (float)trans.z, (float)trans.w};
337  graph_current_.push_back(viewer_env_.env->get_env_ptr()->plot3(transa, 1, 0, 2.f, color));
338  }
339 
340  if (cfg_OR_plot_cur_joints_) {
341  OpenRAVE::Vector color(0.2f, 1.f, 0.f, 1.f);
342  for (unsigned int i = 0; i < links_.size(); ++i) {
343  const OpenRAVE::Vector &trans = links_[i]->GetTransform().trans;
344  float transa[4] = {(float)trans.x, (float)trans.y, (float)trans.z, (float)trans.w};
345  graph_current_.push_back(viewer_env_.env->get_env_ptr()->plot3(transa, 1, 0, 2.f, color));
346  }
347  }
348  }
349  plotted_current_ = plot_current_;
350 
351  } catch (openrave_exception &e) {
352  throw fawkes::Exception("OpenRAVE Exception:%s", e.what());
353  }
354 #endif
355 }
356 
357 /** Solve IK and add target to the queue.
358  *
359  * The IK is solved, ignoring collisions of the end-effector with the environment.
360  * We do this to generally decide if IK is generally solvable. Collision checking
361  * is done in a later step in JacoOpenraveThread::_plan_path .
362  *
363  * If IK is solvable, the target is enqueued in the target_queue.
364  *
365  * @param x x-coordinate of target position
366  * @param y y-coordinate of target position
367  * @param z z-coordinate of target position
368  * @param e1 1st euler rotation of target orientation
369  * @param e2 2nd euler rotation of target orientation
370  * @param e3 3rd euler rotation of target orientation
371  * @param plan decide if we want to plan a trajectory for this or not
372  * @return "true", if IK could be solved. "false" otherwise
373  */
374 bool
375 JacoOpenraveThread::add_target(float x, float y, float z, float e1, float e2, float e3, bool plan)
376 {
377  bool solvable = false; // need to define it here outside the ifdef-scope
378 
379 #ifdef HAVE_OPENRAVE
380  try {
381  // update planner params; set correct DOF and stuff
382  planner_env_.robot->get_planner_params();
383 
384  if (plan) {
385  // get IK from openrave. Ignore collisions with env though, as this is only for IK check and env might change at the
386  // time we start planning. There will be separate IK checks though for planning!
387  planner_env_.robot->enable_ik_comparison(false);
388  solvable = planner_env_.robot->set_target_euler(
389  EULER_ZXZ, x, y, z, e1, e2, e3, IKFO_IgnoreEndEffectorEnvCollisions);
390 
391  if (solvable) {
392  // add this to the target queue for planning
393  logger->log_debug(name(), "Adding to target_queue for later planning");
394 
395  // create new target for the queue
396  RefPtr<jaco_target_t> target(new jaco_target_t());
397  target->type = TARGET_CARTESIAN;
398  target->trajec_state = TRAJEC_WAITING;
399  target->coord = false;
400  target->pos.push_back(x);
401  target->pos.push_back(y);
402  target->pos.push_back(z);
403  target->pos.push_back(e1);
404  target->pos.push_back(e2);
405  target->pos.push_back(e3);
406 
407  arm_->target_mutex->lock();
408  arm_->target_queue->push_back(target);
409  arm_->target_mutex->unlock();
410  } else {
411  logger->log_warn(name(), "No IK solution found for target.");
412  }
413 
414  } else {
415  // don't plan, consider this the final configuration
416 
417  // get IK from openrave. Do not ignore collisions this time, because we skip planning
418  // and go straight to this configuration!
419  solvable = planner_env_.robot->set_target_euler(EULER_ZXZ, x, y, z, e1, e2, e3);
420 
421  if (solvable) {
422  logger->log_debug(name(), "Skip planning, add this as TARGET_ANGULAR");
423 
424  // create new target for the queue
425  RefPtr<jaco_target_t> target(new jaco_target_t());
426  target->type = TARGET_ANGULAR;
427  target->trajec_state = TRAJEC_SKIP;
428  target->coord = false;
429  // get target IK values
430  planner_env_.robot->get_target().manip->get_angles_device(target->pos);
431 
432  arm_->target_mutex->lock();
433  arm_->target_queue->push_back(target);
434  arm_->target_mutex->unlock();
435  } else {
436  logger->log_warn(name(), "No IK solution found for target.");
437  }
438  }
439 
440  } catch (openrave_exception &e) {
441  throw fawkes::Exception("OpenRAVE Exception:%s", e.what());
442  }
443 #endif
444 
445  return solvable;
446 }
447 
448 /** Add target joint values to the queue.
449  *
450  * Use this method with caution, as for now there are no checks for validity
451  * of the target joint values. This will be added soon.
452  * Collision checking with the environment is done in a later step
453  * in JacoOpenraveThread::_plan_path .
454  *
455  * @param j1 target angle of 1st joint
456  * @param j2 target angle of 2nd joint
457  * @param j3 target angle of 3rd joint
458  * @param j4 target angle of 4th joint
459  * @param j5 target angle of 5th joint
460  * @param j6 target angle of 6th joint
461  * @param plan decide if we want to plan a trajectory for this or not
462  * @return "true", if the target joints are valid and not in self-collision,
463  * "false" otherwise.
464  * CAUTION: Self-collision is not checked yet, this feature will be added soon.
465  */
466 bool
468  float j2,
469  float j3,
470  float j4,
471  float j5,
472  float j6,
473  bool plan)
474 {
475  bool joints_valid = false; // need to define it here outside the ifdef-scope
476 
477 #ifdef HAVE_OPENRAVE
478  try {
479  // update planner params; set correct DOF and stuff
480  planner_env_.robot->get_planner_params();
481 
482  //TODO: need some kind cheking for self-collision, i.e. if the joint values are "valid".
483  // For now expect the user to know what he does, when he sets joint angles directly
484  joints_valid = true;
485 
486  // create new target for the queue
487  RefPtr<jaco_target_t> target(new jaco_target_t());
488  target->type = TARGET_ANGULAR;
489  target->trajec_state = plan ? TRAJEC_WAITING : TRAJEC_SKIP;
490  target->coord = false;
491  target->pos.push_back(j1);
492  target->pos.push_back(j2);
493  target->pos.push_back(j3);
494  target->pos.push_back(j4);
495  target->pos.push_back(j5);
496  target->pos.push_back(j6);
497 
498  arm_->target_mutex->lock();
499  arm_->target_queue->push_back(target);
500  arm_->target_mutex->unlock();
501 
502  } catch (openrave_exception &e) {
503  throw fawkes::Exception("OpenRAVE Exception:%s", e.what());
504  }
505 #endif
506 
507  return joints_valid;
508 }
509 
510 /** Flush the target_queue and add this one.
511  * see JacoOpenraveThread#add_target for that.
512  *
513  * @param x x-coordinate of target position
514  * @param y y-coordinate of target position
515  * @param z z-coordinate of target position
516  * @param e1 1st euler rotation of target orientation
517  * @param e2 2nd euler rotation of target orientation
518  * @param e3 3rd euler rotation of target orientation
519  * @param plan decide if we want to plan a trajectory for this or not
520  * @return "true", if IK could be solved. "false" otherwise
521  */
522 bool
523 JacoOpenraveThread::set_target(float x, float y, float z, float e1, float e2, float e3, bool plan)
524 {
525  arm_->target_mutex->lock();
526  arm_->target_queue->clear();
527  arm_->target_mutex->unlock();
528  return add_target(x, y, z, e1, e2, e3, plan);
529 }
530 
531 /** Flush the target_queue and add this one.
532  * see JacoOpenraveThread#add_target_ang for that.
533  *
534  * @param j1 target angle of 1st joint
535  * @param j2 target angle of 2nd joint
536  * @param j3 target angle of 3rd joint
537  * @param j4 target angle of 4th joint
538  * @param j5 target angle of 5th joint
539  * @param j6 target angle of 6th joint
540  * @param plan decide if we want to plan a trajectory for this or not
541  * @return "true", if IK could be solved. "false" otherwise
542  */
543 bool
545  float j2,
546  float j3,
547  float j4,
548  float j5,
549  float j6,
550  bool plan)
551 {
552  arm_->target_mutex->lock();
553  arm_->target_queue->clear();
554  arm_->target_mutex->unlock();
555  return add_target_ang(j1, j2, j3, j4, j5, j6, plan);
556 }
557 
558 void
559 JacoOpenraveThread::_plan_path(RefPtr<jaco_target_t> &from, RefPtr<jaco_target_t> &to)
560 {
561 #ifdef HAVE_OPENRAVE
562  // update state of the trajectory
563  arm_->target_mutex->lock();
565  arm_->target_mutex->unlock();
566 
567  // Update bodies in planner-environment
568  // clone robot state, ignoring grabbed bodies
569  {
570  EnvironmentMutex::scoped_lock view_lock(viewer_env_.env->get_env_ptr()->GetMutex());
571  EnvironmentMutex::scoped_lock plan_lock(planner_env_.env->get_env_ptr()->GetMutex());
572  planner_env_.robot->get_robot_ptr()->ReleaseAllGrabbed();
573  planner_env_.env->delete_all_objects();
574 
575  /*
576  // Old method. Somehow we encountered problems. OpenRAVE internal bug?
577  RobotBase::RobotStateSaver saver(viewer_env_.robot->get_robot_ptr(),
578  0xffffffff&~KinBody::Save_GrabbedBodies&~KinBody::Save_ActiveManipulator&~KinBody::Save_ActiveDOF);
579  saver.Restore( planner_env_.robot->get_robot_ptr() );
580  */
581  // New method. Simply set the DOF values as they are in viewer_env_
582  vector<dReal> dofs;
583  viewer_env_.robot->get_robot_ptr()->GetDOFValues(dofs);
584  planner_env_.robot->get_robot_ptr()->SetDOFValues(dofs);
585  }
586 
587  // then clone all objects
588  planner_env_.env->clone_objects(viewer_env_.env);
589 
590  // restore robot state
591  {
592  EnvironmentMutex::scoped_lock lock(planner_env_.env->get_env_ptr()->GetMutex());
593 
594  // Set active manipulator and active DOFs (need for planner and IK solver!)
595  RobotBase::ManipulatorPtr manip =
596  planner_env_.robot->get_robot_ptr()->SetActiveManipulator(cfg_manipname_);
597  planner_env_.robot->get_robot_ptr()->SetActiveDOFs(manip->GetArmIndices());
598 
599  // update robot state with attached objects
600  {
601  EnvironmentMutex::scoped_lock view_lock(viewer_env_.env->get_env_ptr()->GetMutex());
602  /*
603  // Old method. Somehow we encountered problems. OpenRAVE internal bug?
604  RobotBase::RobotStateSaver saver(viewer_env_.robot->get_robot_ptr(),
605  KinBody::Save_LinkTransformation|KinBody::Save_LinkEnable|KinBody::Save_GrabbedBodies);
606  saver.Restore( planner_env_.robot->get_robot_ptr() );
607  */
608  // New method. Grab all bodies in planner_env_ that are grabbed in viewer_env_ by this manipulator
609  vector<RobotBase::GrabbedInfoPtr> grabbed;
610  viewer_env_.robot->get_robot_ptr()->GetGrabbedInfo(grabbed);
611  for (vector<RobotBase::GrabbedInfoPtr>::iterator it = grabbed.begin(); it != grabbed.end();
612  ++it) {
613  logger->log_debug(name(),
614  "compare _robotlinkname '%s' with our manip link '%s'",
615  (*it)->_robotlinkname.c_str(),
616  manip->GetEndEffector()->GetName().c_str());
617  if ((*it)->_robotlinkname == manip->GetEndEffector()->GetName()) {
618  logger->log_debug(name(), "attach '%s'!", (*it)->_grabbedname.c_str());
619  planner_env_.robot->attach_object((*it)->_grabbedname.c_str(),
620  planner_env_.env,
621  cfg_manipname_.c_str());
622  }
623  }
624  }
625  }
626 
627  // Set target point for planner. Check again for IK, avoiding collisions with the environment
628  //logger->log_debug(name(), "setting target %f %f %f %f %f %f",
629  // to->pos.at(0), to->pos.at(1), to->pos.at(2), to->pos.at(3), to->pos.at(4), to->pos.at(5));
630  planner_env_.robot->enable_ik_comparison(true);
631  if (to->type == TARGET_CARTESIAN) {
632  if (!planner_env_.robot->set_target_euler(EULER_ZXZ,
633  to->pos.at(0),
634  to->pos.at(1),
635  to->pos.at(2),
636  to->pos.at(3),
637  to->pos.at(4),
638  to->pos.at(5))) {
639  logger->log_warn(name(), "Planning failed, second IK check failed");
640  arm_->target_mutex->lock();
642  arm_->target_mutex->unlock();
643  return;
644 
645  } else {
646  // set target angles. This changes the internal target type to ANGLES (see openrave/robot.*)
647  // and will use BaseManipulation's MoveActiveJoints. Otherwise it will use MoveToHandPosition,
648  // which does not have the filtering of IK solutions for the closest one as we have.
649  vector<float> target;
650  planner_env_.robot->get_target().manip->get_angles(target);
651  planner_env_.robot->set_target_angles(target);
652  }
653 
654  } else {
655  vector<float> target;
656  //TODO: need some kind cheking for env-collision, i.e. if the target is colllision-free.
657  // For now expect the user to know what he does, when he sets joint angles directly
658  planner_env_.robot->get_target().manip->set_angles_device(to->pos);
659 
660  planner_env_.robot->get_target().manip->get_angles(target);
661  planner_env_.robot->set_target_angles(target);
662  }
663 
664  // Set starting point for planner
665  //logger->log_debug(name(), "setting start %f %f %f %f %f %f",
666  // from->pos.at(0), from->pos.at(1), from->pos.at(2), from->pos.at(3), from->pos.at(4), from->pos.at(5));
667  planner_env_.manip->set_angles_device(from->pos);
668 
669  // Set planning parameters
670  planner_env_.robot->set_target_plannerparams(plannerparams_);
671 
672  // Run planner
673  try {
674  planner_env_.env->run_planner(planner_env_.robot, cfg_OR_sampling_);
675  } catch (fawkes::Exception &e) {
676  logger->log_warn(name(), "Planning failed: %s", e.what_no_backtrace());
677  // TODO: better handling!
678  // for now just skip planning, so the target_queue can be processed
679  arm_->target_mutex->lock();
680  //to->type = TARGET_ANGULAR;
682  arm_->target_mutex->unlock();
683  return;
684  }
685 
686  // add trajectory to queue
687  //logger->log_debug(name(), "plan successful, adding to queue");
688  arm_->trajec_mutex->lock();
689  // we can do the following becaouse get_trajectory_device() returns a new object, thus
690  // can be safely deleted by RefPtr auto-deletion
691  to->trajec = RefPtr<jaco_trajec_t>(planner_env_.robot->get_trajectory_device());
692  arm_->trajec_mutex->unlock();
693 
694  // update target.
695  arm_->target_mutex->lock();
696  //change target type to ANGULAR and set target->pos accordingly. This makes final-checking
697  // in goto_thread much easier
698  to->type = TARGET_ANGULAR;
699  to->pos = to->trajec->back();
700  // update trajectory state
702  arm_->target_mutex->unlock();
703 #endif
704 }
705 
706 /** Plot the first target of the queue in the viewer_env */
707 void
709 {
710 #ifdef HAVE_OPENRAVE
711  if (!cfg_OR_use_viewer_ || (!cfg_OR_plot_traj_manip_ && !cfg_OR_plot_traj_joints_))
712  return;
713 
714  graph_handle_.clear();
715 
716  // check if there is a target to be plotted
717  arm_->target_mutex->lock();
718  if (arm_->target_queue->empty()) {
719  arm_->target_mutex->unlock();
720  return;
721  }
722 
723  // get RefPtr to first target in queue
724  RefPtr<jaco_target_t> target = arm_->target_queue->front();
725  arm_->target_mutex->unlock();
726 
727  // only plot trajectories
728  if (target->trajec_state != TRAJEC_READY && target->trajec_state != TRAJEC_EXECUTING)
729  return;
730 
731  // plot the trajectory (if possible)
732  arm_->trajec_mutex->lock();
733  if (!target->trajec) {
734  arm_->trajec_mutex->unlock();
735  return;
736  }
737 
738  // remove all GraphHandlerPtr and currently drawn plots
739  graph_handle_.clear();
740  {
741  EnvironmentMutex::scoped_lock lock(viewer_env_.env->get_env_ptr()->GetMutex());
742 
743  // save the state, do not modifiy currently active robot!
744  RobotBasePtr tmp_robot = viewer_env_.robot->get_robot_ptr();
745  RobotBase::RobotStateSaver saver(tmp_robot);
746 
747  std::vector<dReal> joints;
748  OpenRaveManipulatorPtr manip = viewer_env_.manip->copy();
749 
750  OpenRAVE::Vector color_m(arm_->trajec_color[0],
751  arm_->trajec_color[1],
752  arm_->trajec_color[2],
753  arm_->trajec_color[3]);
754  OpenRAVE::Vector color_j(arm_->trajec_color[0] / 1.4f,
755  0.2f,
756  arm_->trajec_color[2] / 1.4f,
757  arm_->trajec_color[3] / 1.4f);
758 
759  for (jaco_trajec_t::iterator it = target->trajec->begin(); it != target->trajec->end(); ++it) {
760  manip->set_angles_device((*it));
761  manip->get_angles(joints);
762 
763  tmp_robot->SetDOFValues(joints, 1, manip_->GetArmIndices());
764 
765  if (cfg_OR_plot_traj_manip_) {
766  const OpenRAVE::Vector &trans = manip_->GetEndEffectorTransform().trans;
767  float transa[4] = {(float)trans.x, (float)trans.y, (float)trans.z, (float)trans.w};
768  graph_handle_.push_back(viewer_env_.env->get_env_ptr()->plot3(transa, 1, 0, 3.f, color_m));
769  }
770 
771  if (cfg_OR_plot_traj_joints_) {
772  for (unsigned int i = 0; i < links_.size(); ++i) {
773  const OpenRAVE::Vector &trans = links_[i]->GetTransform().trans;
774  float transa[4] = {(float)trans.x, (float)trans.y, (float)trans.z, (float)trans.w};
775  graph_handle_.push_back(
776  viewer_env_.env->get_env_ptr()->plot3(transa, 1, 0, 3.f, color_j));
777  }
778  }
779  }
780  } // robot state is restored
781 
782  arm_->trajec_mutex->unlock();
783 
784 #endif //HAVE_OPENRAVE
785 }
fawkes::Mutex::lock
void lock()
Lock this mutex.
Definition: mutex.cpp:93
JacoOpenraveBaseThread::finalize
virtual void finalize()
Finalize the thread.
Definition: openrave_base_thread.cpp:111
fawkes::TRAJEC_PLANNING_ERROR
@ TRAJEC_PLANNING_ERROR
planner could not plan a collision-free trajectory.
Definition: types.h:74
fawkes::JacoInterface::finger3
float finger3() const
Get finger3 value.
Definition: JacoInterface.cpp:476
fawkes::OpenRaveManipulator::copy
virtual OpenRaveManipulatorPtr copy()=0
Create a new copy of this OpenRaveManipulator instance.
fawkes::jaco_target_struct_t::trajec
fawkes::RefPtr< jaco_trajec_t > trajec
trajectory, if target is TARGET_TRAJEC.
Definition: types.h:83
fawkes::RefPtr
RefPtr<> is a reference-counting shared smartpointer.
Definition: refptr.h:57
fawkes::jaco_target_struct_t::type
jaco_target_type_t type
target type.
Definition: types.h:80
JacoOpenraveThread::set_target
virtual bool set_target(float x, float y, float z, float e1, float e2, float e3, bool plan=true)
Flush the target_queue and add this one.
Definition: openrave_thread.cpp:523
JacoOpenraveThread::add_target
virtual bool add_target(float x, float y, float z, float e1, float e2, float e3, bool plan=true)
Solve IK and add target to the queue.
Definition: openrave_thread.cpp:375
JacoOpenraveThread::set_target_ang
virtual bool set_target_ang(float j1, float j2, float j3, float j4, float j5, float j6, bool plan=true)
Flush the target_queue and add this one.
Definition: openrave_thread.cpp:544
fawkes::EULER_ZXZ
@ EULER_ZXZ
ZXZ rotation.
Definition: types.h:51
JacoOpenraveThread::add_target_ang
virtual bool add_target_ang(float j1, float j2, float j3, float j4, float j5, float j6, bool plan=true)
Add target joint values to the queue.
Definition: openrave_thread.cpp:467
fawkes::TRAJEC_EXECUTING
@ TRAJEC_EXECUTING
trajectory is being executed.
Definition: types.h:72
fawkes::CONFIG_SINGLE
@ CONFIG_SINGLE
we only have one arm.
Definition: types.h:52
fawkes::JacoInterface::finger1
float finger1() const
Get finger1 value.
Definition: JacoInterface.cpp:414
fawkes::Thread::name
const char * name() const
Definition: thread.h:100
fawkes::JacoInterface::joints
float * joints() const
Get joints value.
Definition: JacoInterface.cpp:354
fawkes::TRAJEC_PLANNING
@ TRAJEC_PLANNING
planner is planning the trajectory.
Definition: types.h:70
fawkes::Mutex::unlock
void unlock()
Unlock the mutex.
Definition: mutex.cpp:137
fawkes::TARGET_GRIPPER
@ TARGET_GRIPPER
only gripper movement.
Definition: types.h:61
fawkes::jaco_arm_struct::trajec_color
float trajec_color[4]
the color used for plotting the trajectory.
Definition: types.h:108
fawkes::CONFIG_RIGHT
@ CONFIG_RIGHT
this arm is the right one out of two.
Definition: types.h:54
fawkes::jaco_arm_struct::iface
JacoInterface * iface
pointer to JacoInterface, assigned to this arm
Definition: types.h:96
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
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
fawkes::LoggingAspect::logger
Logger * logger
Definition: logging.h:53
fawkes::CONFIG_LEFT
@ CONFIG_LEFT
this arm is the left one out of two.
Definition: types.h:53
JacoOpenraveBaseThread
Definition: openrave_base_thread.h:55
fawkes
fawkes::jaco_arm_struct::arm
fawkes::JacoArm * arm
pointer to actual JacoArm instance, controlling this arm
Definition: types.h:95
fawkes::OpenRaveManipulatorKinovaJaco
Definition: kinova_jaco.h:36
fawkes::TRAJEC_SKIP
@ TRAJEC_SKIP
skip trajectory planning for this target.
Definition: types.h:68
JacoOpenraveThread::plot_first
virtual void plot_first()
Plot the first target of the queue in the viewer_env.
Definition: openrave_thread.cpp:708
fawkes::jaco_target_struct_t
Jaco target struct, holding information on a target.
Definition: types.h:78
fawkes::jaco_arm_struct::target_mutex
RefPtr< Mutex > target_mutex
mutex, used for accessing the target_queue
Definition: types.h:101
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
fawkes::deg2rad
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:42
JacoOpenraveThread::update_openrave
virtual void update_openrave()
Update the openrave environment to represent the current situation.
Definition: openrave_thread.cpp:290
fawkes::ConfigurableAspect::config
Configuration * config
Definition: configurable.h:53
fawkes::jaco_arm_struct::target_queue
RefPtr< jaco_target_queue_t > target_queue
queue of targets, which is processed FIFO.
Definition: types.h:105
JacoOpenraveBaseThread::planning_mutex_
fawkes::Mutex * planning_mutex_
mutex, used to lock when planning.
Definition: openrave_base_thread.h:109
JacoOpenraveThread::loop
virtual void loop()
Mani loop.
Definition: openrave_thread.cpp:224
fawkes::JacoArm::get_joints
virtual void get_joints(std::vector< float > &to) const =0
Get the joint angles of the arm.
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::OpenRaveManipulator::get_angles
void get_angles(std::vector< T > &to) const
Get motor angles of OpenRAVE model.
Definition: manipulator.h:89
fawkes::jaco_arm_struct
Jaco struct containing all components required for one arm.
Definition: types.h:92
fawkes::RefPtr::clear
void clear()
Set underlying instance to 0, decrementing reference count of existing instance appropriately.
Definition: refptr.h:453
fawkes::Configuration::get_string
virtual std::string get_string(const char *path)=0
JacoOpenraveThread::finalize
virtual void finalize()
Finalize the thread.
Definition: openrave_thread.cpp:198
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::jaco_arm_struct::config
jaco_arm_config_t config
configuration for this arm
Definition: types.h:94
fawkes::Logger::log_debug
virtual void log_debug(const char *component, const char *format,...)=0
fawkes::jaco_arm_struct::trajec_mutex
RefPtr< Mutex > trajec_mutex
mutex, used for modifying trajectory of a target.
Definition: types.h:103
fawkes::JacoInterface::finger2
float finger2() const
Get finger2 value.
Definition: JacoInterface.cpp:445
JacoOpenraveThread::JacoOpenraveThread
JacoOpenraveThread(const char *name, fawkes::jaco_arm_t *arm, bool load_robot=true)
Constructor.
Definition: openrave_thread.cpp:62
fawkes::OpenRaveManipulator::set_angles_device
void set_angles_device(std::vector< T > &angles)
Set motor angles of real device.
Definition: manipulator.h:147
fawkes::Exception
Definition: exception.h:41