Fawkes API  Fawkes Development Version
robot.cpp
1 
2 /***************************************************************************
3  * robot.cpp - Fawkes to OpenRAVE Robot Handler
4  *
5  * Created: Mon Sep 20 14:50:34 2010
6  * Copyright 2010 Bahram Maleki-Fard, AllemaniACs RoboCup Team
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 "robot.h"
24 
25 #include "environment.h"
26 #include "manipulator.h"
27 
28 #include <core/exceptions/software.h>
29 #include <logging/logger.h>
30 
31 #include <openrave-core.h>
32 
33 using namespace OpenRAVE;
34 namespace fawkes {
35 
36 /** @class OpenRaveRobot <plugins/openrave/robot.h>
37 * Class handling interaction with the OpenRAVE::RobotBase class.
38 * This class mainly handles robot specific tasks, like setting a
39 * target, looking for IK solutions and handling planning parameters
40 * for the robot.
41 * @author Bahram Maleki-Fard
42 */
43 
44 /** Constructor
45  * @param logger pointer to fawkes logger
46  */
47 OpenRaveRobot::OpenRaveRobot(fawkes::Logger *logger)
48 : logger_(logger), name_(""), manip_(0), find_best_ik_(1)
49 {
50  init();
51 }
52 /** Constructor
53  * @param filename path to robot's xml file
54  * @param env pointer to OpenRaveEnvironment object
55  * @param logger pointer to fawkes logger
56  */
57 OpenRaveRobot::OpenRaveRobot(const std::string & filename,
59  fawkes::Logger * logger)
60 : logger_(logger),
61  name_(""),
62  name_str_(""),
63  manip_(0),
64  target_(TARGET_NONE),
65  find_best_ik_(1),
66  trans_offset_x_(0),
67  trans_offset_y_(0),
68  trans_offset_z_(0),
69  display_planned_movements_(false)
70 {
71  init();
72  this->load(filename, env);
73 }
74 
75 /** Copy Constructor.
76  * @param src The OpenRaveRobot to clone
77  * @param new_env Pointer to the new OpenRaveEnvironment. We need this to set robot_
78  * to the correct robot in the new OpenRAVE-environment.
79  */
81  const fawkes::OpenRaveEnvironmentPtr &new_env)
82 : logger_(src.logger_), name_(src.name_), find_best_ik_(src.find_best_ik_)
83 {
84  build_name_str();
85  traj_ = new std::vector<std::vector<dReal>>();
86 
87  trans_offset_x_ = src.trans_offset_x_;
88  trans_offset_y_ = src.trans_offset_y_;
89  trans_offset_z_ = src.trans_offset_z_;
90 
91  // Get correct robot from environment.
92  EnvironmentMutex::scoped_lock lock(new_env->get_env_ptr()->GetMutex());
93  std::string name = src.get_robot_ptr()->GetName();
94  robot_ = new_env->get_env_ptr()->GetRobot(name);
95 
96  if (!robot_)
97  throw fawkes::IllegalArgumentException("%s: Robot '%s' could not be loaded. "
98  "Check name.",
99  this->name(),
100  name.c_str());
101 
102  build_name_str();
103  if (logger_) {
104  logger_->log_debug(this->name(), "Robot loaded.");
105  }
106 
107  // Initialize robot
108  set_ready();
109 
110  // Set the same manipulator "active" as it was in previous environment
111  // "set_ready()" just takes the first manipulator it finds
112  {
113  EnvironmentMutex::scoped_lock lock(src.get_robot_ptr()->GetEnv()->GetMutex());
114  arm_ = robot_->SetActiveManipulator(src.get_robot_ptr()->GetActiveManipulator()->GetName());
115  }
116  robot_->SetActiveDOFs(arm_->GetArmIndices());
117 
118  manip_ = src.get_manipulator()->copy();
119  target_.manip = manip_->copy();
120  display_planned_movements_ = false;
121 
122  if (logger_) {
123  logger_->log_debug(this->name(), "Robot '%s' cloned.", name.c_str());
124  }
125 }
126 
127 /** Destructor */
129 {
130  target_.manip = NULL;
131 
132  //unload everything related to this robot from environment
133  try {
134  EnvironmentBasePtr env = robot_->GetEnv();
135  EnvironmentMutex::scoped_lock lock(env->GetMutex());
136  env->Remove(mod_basemanip_);
137  env->Remove(robot_);
138  if (logger_)
139  logger_->log_warn(name(), "Robot unloaded from environment");
140  } catch (const openrave_exception &e) {
141  if (logger_)
142  logger_->log_warn(name(),
143  "Could not unload robot properly from environment. Ex:%s",
144  e.what());
145  }
146 }
147 
148 /** Build name string to use in logging messages.
149  * Nothing important, but helpful for debugging etc.
150  */
151 void
152 OpenRaveRobot::build_name_str()
153 {
154  std::stringstream n;
155  n << "OpenRaveRobot"
156  << "[";
157  if (robot_)
158  n << RaveGetEnvironmentId(robot_->GetEnv()) << ":";
159  n << name_ << "]";
160  name_str_ = n.str();
161 }
162 
163 /** Get the name string to use in logging messages etc. */
164 const char *
165 OpenRaveRobot::name() const
166 {
167  return name_str_.c_str();
168 }
169 
170 /** Inittialize object attributes */
171 void
172 OpenRaveRobot::init()
173 {
174  traj_ = new std::vector<std::vector<dReal>>();
175 
176  trans_offset_x_ = 0.f;
177  trans_offset_y_ = 0.f;
178  trans_offset_z_ = 0.f;
179 
180  build_name_str();
181 }
182 
183 /** Load robot from xml file
184  * @param filename path to robot's xml file
185  * @param env pointer to OpenRaveEnvironment object
186  */
187 void
188 OpenRaveRobot::load(const std::string &filename, fawkes::OpenRaveEnvironmentPtr &env)
189 {
190  EnvironmentMutex::scoped_lock lock(env->get_env_ptr()->GetMutex());
191 
192  // TODO: implementing without usage of 'environment'
193  // openrave_exception handling is done in OpenRAVE (see environment-core.h)
194  robot_ = env->get_env_ptr()->ReadRobotXMLFile(filename);
195 
196  if (!robot_)
198  "%s: Robot could not be loaded. Check xml file/path '%s'.", name(), filename.c_str());
199 
200  name_ = robot_->GetName();
201  build_name_str();
202 
203  if (logger_)
204  logger_->log_debug(name(), "Robot loaded.");
205 }
206 
207 /** Set robot ready for usage.
208  * Here: Set active DOFs and create plannerParameters.
209  * CAUTION: Only successful after added to environment. Otherwise no active DOF will be recognized. */
210 void
212 {
213  if (!robot_)
214  throw fawkes::Exception("%s: Robot not loaded properly yet.", name());
215 
216  EnvironmentMutex::scoped_lock lock(robot_->GetEnv()->GetMutex());
217 
218  robot_->SetActiveManipulator(robot_->GetManipulators().at(0)->GetName());
219  arm_ = robot_->GetActiveManipulator();
220  robot_->SetActiveDOFs(arm_->GetArmIndices());
221 
222  if (robot_->GetActiveDOF() == 0)
223  throw fawkes::Exception(
224  "%s: Robot not added to environment yet. Need to do that first, otherwise planner will fail.",
225  name());
226 
227  // create planner parameters
228  try {
229  PlannerBase::PlannerParametersPtr params(new PlannerBase::PlannerParameters());
230  planner_params_ = params;
231  planner_params_->_nMaxIterations = 4000; // max iterations before failure
232  planner_params_->SetRobotActiveJoints(
233  robot_); // set planning configuration space to current active dofs
234  planner_params_->vgoalconfig.resize(robot_->GetActiveDOF());
235  } catch (const openrave_exception &e) {
236  throw fawkes::Exception("%s: Could not create PlannerParameters. Ex:%s", name(), e.what());
237  }
238 
239  // create and load BaseManipulation module
240  try {
241  mod_basemanip_ = RaveCreateModule(robot_->GetEnv(), "basemanipulation");
242  robot_->GetEnv()->AddModule(mod_basemanip_, robot_->GetName());
243  } catch (const openrave_exception &e) {
244  throw fawkes::Exception("%s: Cannot load BaseManipulation Module. Ex:%s", name(), e.what());
245  }
246 
247  if (logger_)
248  logger_->log_debug(name(), "Robot ready.");
249 }
250 
251 /** Directly set transition offset between coordinate systems
252  * of real device and OpenRAVE model.
253  * @param trans_x transition offset on x-axis
254  * @param trans_y transition offset on y-axis
255  * @param trans_z transition offset on z-axis
256  */
257 void
258 OpenRaveRobot::set_offset(float trans_x, float trans_y, float trans_z)
259 {
260  trans_offset_x_ = trans_x;
261  trans_offset_y_ = trans_y;
262  trans_offset_z_ = trans_z;
263 }
264 
265 /** Calculate transition offset between coordinate systems
266  * of real device and OpenRAVE model.
267  * Sets model's angles to current device's angles (from manip_),
268  * and compares transitions.
269  * @param device_trans_x transition on x-axis (real device)
270  * @param device_trans_y transition on y-axis (real device)
271  * @param device_trans_z transition on z-axis (real device)
272  */
273 void
274 OpenRaveRobot::calibrate(float device_trans_x, float device_trans_y, float device_trans_z)
275 {
276  EnvironmentMutex::scoped_lock lock(robot_->GetEnv()->GetMutex());
277  // get device's current angles, and set them for OpenRAVE model
278  std::vector<dReal> angles;
279  manip_->get_angles(angles);
280  robot_->SetActiveDOFValues(angles);
281 
282  // get model's current transition and compare
283  arm_ = robot_->GetActiveManipulator();
284  Transform trans = arm_->GetEndEffectorTransform();
285  trans_offset_x_ = trans.trans[0] - device_trans_x;
286  trans_offset_y_ = trans.trans[1] - device_trans_y;
287  trans_offset_z_ = trans.trans[2] - device_trans_z;
288 }
289 
290 /** Set pointer to OpenRaveManipulator object.
291  * Make sure this is called AFTER all manipulator settings have
292  * been set (assures that target_.manip has the same settings).
293  * @param manip pointer to OpenRaveManipulator object
294  * @param display_movements true, if movements should be displayed in viewer.
295  * Better be "false" if want to sync OpenRAVE models with device
296  */
297 void
299 {
300  manip_ = manip;
301  target_.manip = manip_->copy();
302 
303  display_planned_movements_ = display_movements;
304 }
305 
306 /** Update motor values from OpenRAVE model.
307  * Can be used to sync real device with OpenRAVE model*/
308 void
310 {
311  EnvironmentMutex::scoped_lock lock(robot_->GetEnv()->GetMutex());
312  std::vector<dReal> angles;
313  robot_->GetActiveDOFValues(angles);
314  manip_->set_angles(angles);
315 }
316 
317 /** Update/Set OpenRAVE motor angles */
318 void
320 {
321  EnvironmentMutex::scoped_lock lock(robot_->GetEnv()->GetMutex());
322  std::vector<dReal> angles;
323  manip_->get_angles(angles);
324  robot_->SetActiveDOFValues(angles);
325 }
326 
327 /** Getter for display_planned_movements_.
328  * @return return value
329  */
330 bool
332 {
333  return display_planned_movements_;
334 }
335 
336 /** Activate/Deactive IK comparison.
337  * When activated, we don't just take the first returned IK solution, but
338  * compare them all to find the best, i.e. the one that is "closest" to our
339  * current configuration.
340  * @param enable Sets the state of the comparison. Enabled by default.
341  */
342 void
344 {
345  find_best_ik_ = enable;
346 }
347 
348 /** Set target, given relative transition.
349  * This is the prefered method to set a target for straight manipulator movement.
350  * @param trans_x x-transition
351  * @param trans_y y-transition
352  * @param trans_z z-transition
353  * @param is_extension true, if base coordination system lies in arm extension
354  * @return true if solvable, false otherwise
355  */
356 bool
357 OpenRaveRobot::set_target_rel(float trans_x, float trans_y, float trans_z, bool is_extension)
358 {
359  if (is_extension) {
360  target_.type = TARGET_RELATIVE_EXT;
361  } else {
362  target_.type = TARGET_RELATIVE;
363  }
364  target_.x = trans_x;
365  target_.y = trans_y;
366  target_.z = trans_z;
367 
368  // Not sure how to check IK solvability yet. Would be nice to have this
369  // checked before planning a path.
370  target_.solvable = true;
371 
372  return target_.solvable;
373 }
374 
375 /** Set target for a straight movement, given transition.
376  * This is the a wrapper for "set_target_rel", to be able to call for a
377  * straight arm movement by giving non-relative transition.
378  * @param trans_x x-transition
379  * @param trans_y y-transition
380  * @param trans_z z-transition
381  * @return true if solvable, false otherwise
382  */
383 bool
384 OpenRaveRobot::set_target_straight(float trans_x, float trans_y, float trans_z)
385 {
386  Transform trans;
387  {
388  EnvironmentMutex::scoped_lock lock(robot_->GetEnv()->GetMutex());
389  arm_ = robot_->GetActiveManipulator();
390  trans = arm_->GetEndEffectorTransform();
391  }
392 
393  return set_target_rel(trans_x - trans.trans[0],
394  trans_y - trans.trans[1],
395  trans_z - trans.trans[2]);
396 }
397 
398 /** Set target, given transition, and rotation as quaternion.
399  * @param trans_x x-transition
400  * @param trans_y y-transition
401  * @param trans_z z-transition
402  * @param quat_w quaternion skalar
403  * @param quat_x quaternion 1st value
404  * @param quat_y quaternion 2nd value
405  * @param quat_z quaternion 3rd value
406  * @param filter IK filter options (see OpenRAVE doc for details)
407  * @param no_offset if true, do not include manipulator offset (default: false)
408  * @return true if solvable, false otherwise
409  */
410 bool
412  float trans_y,
413  float trans_z,
414  float quat_w,
415  float quat_x,
416  float quat_y,
417  float quat_z,
418  IkFilterOptions filter,
419  bool no_offset)
420 {
421  Vector trans(trans_x, trans_y, trans_z);
422  Vector rot(quat_w, quat_x, quat_y, quat_z);
423 
424  return set_target_transform(trans, rot, filter, no_offset);
425 }
426 
427 /** Set target, given transition, and rotation as axis-angle.
428  * @param trans_x x-transition
429  * @param trans_y y-transition
430  * @param trans_z z-transition
431  * @param angle axis-angle angle
432  * @param axisX axis-angle x-axis value
433  * @param axisY axis-angle y-axis value
434  * @param axisZ axis-angle z-axis value
435  * @param filter IK filter options (see OpenRAVE doc for details)
436  * @param no_offset if true, do not include manipulator offset (default: false)
437  * @return true if solvable, false otherwise
438  */
439 bool
441  float trans_y,
442  float trans_z,
443  float angle,
444  float axisX,
445  float axisY,
446  float axisZ,
447  IkFilterOptions filter,
448  bool no_offset)
449 {
450  Vector trans(trans_x, trans_y, trans_z);
451  Vector aa(angle, axisX, axisY, axisZ);
452  Vector rot = quatFromAxisAngle(aa);
453 
454  return set_target_transform(trans, rot, filter, no_offset);
455 }
456 
457 /** Set target, given transition, and Euler-rotation.
458  * @param type Euler-rotation type (ZXZ, ZYZ, ...)
459  * @param trans_x x-transition
460  * @param trans_y y-transition
461  * @param trans_z z-transition
462  * @param phi 1st rotation
463  * @param theta 2nd rotation
464  * @param psi 3rd rotation
465  * @param filter IK filter options (see OpenRAVE doc for details)
466  * @param no_offset if true, do not include manipulator offset (default: false)
467  * @return true if solvable, false otherwise
468  */
469 bool
471  float trans_x,
472  float trans_y,
473  float trans_z,
474  float phi,
475  float theta,
476  float psi,
477  OpenRAVE::IkFilterOptions filter,
478  bool no_offset)
479 {
480  Vector trans(trans_x, trans_y, trans_z);
481  std::vector<float> rot(9, 0.f); //rotations vector
482 
483  switch (type) {
484  case (EULER_ZXZ):
485  if (logger_)
486  logger_->log_debug(
487  name(), "Target EULER_ZXZ: %f %f %f %f %f %f", trans_x, trans_y, trans_z, phi, theta, psi);
488  rot.at(2) = phi; //1st row, 3rd value; rotation on z-axis
489  rot.at(3) = theta; //2nd row, 1st value; rotation on x-axis
490  rot.at(8) = psi; //3rd row, 3rd value; rotation on z-axis
491  break;
492 
493  case (EULER_ZYZ):
494  if (logger_)
495  logger_->log_debug(name(),
496  "Target EULER_ZYZ:",
497  "%f %f %f %f %f %f",
498  trans_x,
499  trans_y,
500  trans_z,
501  phi,
502  theta,
503  psi);
504  rot.at(2) = phi; //1st row, 3rd value; rotation on z-axis
505  rot.at(4) = theta; //2nd row, 2nd value; rotation on y-axis
506  rot.at(8) = psi; //3rd row, 3rd value; rotation on z-axis
507  break;
508 
509  case (EULER_ZYX):
510  if (logger_)
511  logger_->log_debug(name(),
512  "Target EULER_ZYX:",
513  "%f %f %f %f %f %f",
514  trans_x,
515  trans_y,
516  trans_z,
517  phi,
518  theta,
519  psi);
520  rot.at(2) = phi; //1st row, 3rd value; rotation on z-axis
521  rot.at(4) = theta; //2nd row, 2nd value; rotation on y-axis
522  rot.at(6) = psi; //3rd row, 1st value; rotation on x-axis
523  break;
524 
525  default:
526  target_.type = TARGET_NONE;
527  target_.solvable = false;
528  return false;
529  }
530 
531  return set_target_euler(trans, rot, filter, no_offset);
532 }
533 
534 /** Set target by giving position of an object.
535  * Currently the object should be cylindric, and stand upright. It may
536  * also be rotated on its x-axis, but that rotation needs to be given in an argument
537  * to calculate correct position for end-effector. This is only temporary until
538  * proper grasp planning for 5DOF in OpenRAVE is provided.
539  * @param trans_x x-transition of object
540  * @param trans_y y-transition of object
541  * @param trans_z z-transition of object
542  * @param rot_x rotation of object on x-axis (radians) (default: 0.f, i.e. upright)
543  * @param filter IK filter options (see OpenRAVE doc for details)
544  * @return true if solvable, false otherwise
545  */
546 bool
548  float trans_y,
549  float trans_z,
550  float rot_x,
551  IkFilterOptions filter)
552 {
553  // This is about 2 times faster than using setTargetEuler each time, especially when it comes
554  // to the while loop (whole loop: ~56ms vs ~99ms)
555 
556  // release all attached/grabbed bodys
557  {
558  EnvironmentMutex::scoped_lock lock(robot_->GetEnv()->GetMutex());
559  robot_->ReleaseAllGrabbed();
560  }
561 
562  // quaternion defining consecutiv rotations on axis
563  float alpha =
564  atan2(trans_y - trans_offset_y_,
565  trans_x - trans_offset_x_); //angle to rotate left/right when manipulator points to +x
566  Vector quat_y =
567  quatFromAxisAngle(Vector(0.f, M_PI / 2, 0.f)); //1st, rotate down -> manipulator points to +x
568  Vector quat_x = quatFromAxisAngle(
569  Vector(-alpha, 0.f, 0.f)); //2nd, rotate left/right -> manipulator points to object
570  Vector quat_z =
571  quatFromAxisAngle(Vector(0.f, 0.f, rot_x)); //last, rotate wrist -> manipulator ready to grab
572 
573  Vector quat_xY = quatMultiply(quat_y, quat_x);
574  Vector quat_xYZ = quatMultiply(quat_xY, quat_z);
575 
576  Vector trans(trans_x, trans_y, trans_z);
577 
578  if (set_target_transform(trans, quat_xYZ, filter, true))
579  return true;
580 
581  //try varying 2nd rotation (quat_y) until a valid IK is found. Max angle: 45° (~0.79 rad)
582  Vector quatPosY = quatFromAxisAngle(Vector(0.f, 0.017f, 0.f)); //rotate up for 1°
583  Vector quatNegY = quatFromAxisAngle(Vector(0.f, -0.017f, 0.f)); //rotate down for 1°
584 
585  Vector quatPos(quat_xY); //starting position, after first 2 rotations
586  Vector quatNeg(quat_xY);
587 
588  unsigned int count = 0;
589  bool foundIK = false;
590 
591  while ((!foundIK) && (count <= 45)) {
592  count++;
593 
594  quatPos = quatMultiply(quatPos, quatPosY); //move up ~1°
595  quatNeg = quatMultiply(quatNeg, quatNegY); //move down ~1°
596 
597  quat_xYZ = quatMultiply(quatPos, quat_z); //apply wrist rotation
598  foundIK = set_target_transform(trans, quat_xYZ, filter, true);
599  if (!foundIK) {
600  quat_xYZ = quatMultiply(quatNeg, quat_z);
601  foundIK = set_target_transform(trans, quat_xYZ, filter, true);
602  }
603  }
604 
605  return foundIK;
606 }
607 
608 /** Set target by giving IkParameterizaion of target.
609  * OpenRAVE::IkParameterization is the desired type to be calculated with
610  * by OpenRAVE. Each oter type (i.e. Transform) is implicitly transformed
611  * to an IkParameterization before continuing to check for Ik solution and
612  * planning, i.e. by the BaseManipulation module.
613  * @param ik_param the OpenRAVE::IkParameterization of the target
614  * @param filter IK filter options (see OpenRAVE doc for details)
615  * @return true if solvable, false otherwise
616  */
617 bool
618 OpenRaveRobot::set_target_ikparam(IkParameterization ik_param, IkFilterOptions filter)
619 {
620  EnvironmentMutex::scoped_lock lock(robot_->GetEnv()->GetMutex());
621  arm_ = robot_->GetActiveManipulator();
622 
623  target_.ikparam = ik_param;
624  target_.type = TARGET_IKPARAM;
625  solve_ik(filter);
626 
627  return target_.solvable;
628 }
629 
630 /** Set additional planner parameters.
631  * BaseManipulation module accepts many arguments that can be passed.
632  * Planner parameters can be important to plan a path according to ones
633  * needs, e.g. set deviations, optimizer iterations, etc.
634  * Do not mistake it with the single argument "plannerparams" of BaseManipulation.
635  * @param params complete string of additional arguments.
636  */
637 void
639 {
640  target_.plannerparams = params;
641 }
642 
643 /** Set additional planner parameters.
644  * @param params complete string of additional arguments.
645  */
646 void
648 {
649  target_.plannerparams = params;
650 }
651 
652 /** Set raw command for BaseManipulation module.
653  * BaseManipulation module accepts many arguments that can be passed.
654  * Basic commands are covered by the other set_target_ methods. In case something
655  * is not covered, or you want to send a custom command, use this method.
656  * Remember that plannerparams set by "set_target_plannerparams" are still added
657  * to the planner, so make sure you don't send duplicate entries both in plannerparams
658  * and in the raw command string.
659  * @param cmd complete command string.
660  */
661 void
663 {
664  target_.raw_cmd = cmd;
665 }
666 
667 /** Set raw command for BaseManipulation module.
668  * @param cmd complete command string.
669  */
670 void
672 {
673  target_.raw_cmd = cmd;
674 }
675 
676 /** Set target angles directly.
677  * @param angles vector with angle values
678  */
679 void
680 OpenRaveRobot::set_target_angles(std::vector<float> &angles)
681 {
682  target_.manip->set_angles(angles);
683  target_.type = TARGET_JOINTS;
684  target_.solvable = true; //no IK check done though!
685 }
686 
687 /* ################### getters ##################*/
688 /** Returns RobotBasePtr for uses in other classes.
689  * @return RobotBasePtr of current robot
690  */
691 OpenRAVE::RobotBasePtr
693 {
694  return robot_;
695 }
696 
697 /** Get target.
698  * @return target struct
699  */
700 target_t
702 {
703  return target_;
704 }
705 
706 /** Get manipulator.
707  * @return pointer to currentl used OpenRaveManipulator
708  */
711 {
712  return manip_;
713 }
714 
715 /** Updates planner parameters and return pointer to it
716  * @return PlannerParametersPtr or robot's planner params
717  */
718 OpenRAVE::PlannerBase::PlannerParametersPtr
720 {
721  EnvironmentMutex::scoped_lock lock(robot_->GetEnv()->GetMutex());
722  // set planning configuration space to current active dofs
723  planner_params_->SetRobotActiveJoints(robot_);
724  planner_params_->vgoalconfig.resize(robot_->GetActiveDOF());
725 
726  manip_->get_angles(planner_params_->vinitialconfig);
727  target_.manip->get_angles(planner_params_->vgoalconfig);
728 
729  robot_->SetActiveDOFValues(planner_params_->vinitialconfig);
730 
731  return planner_params_;
732 }
733 
734 /** Return pointer to trajectory of motion from
735  * manip_ to target_.manip with OpenRAVE-model angle format
736  * @return pointer to trajectory
737  */
738 std::vector<std::vector<dReal>> *
740 {
741  return traj_;
742 }
743 
744 /** Return pointer to trajectory of motion from
745  * manip_ to target_.manip with device angle format
746  * @return pointer to trajectory
747  */
748 std::vector<std::vector<float>> *
750 {
751  std::vector<std::vector<float>> *traj = new std::vector<std::vector<float>>();
752 
753  std::vector<float> v;
754 
755  for (unsigned int i = 0; i < traj_->size(); i++) {
756  manip_->angles_or_to_device(traj_->at(i), v);
757  traj->push_back(v);
758  }
759 
760  return traj;
761 }
762 
763 /** Return BaseManipulation Module-Pointer.
764  * @return ModuleBasePtr
765  */
766 OpenRAVE::ModuleBasePtr
768 {
769  return mod_basemanip_;
770 }
771 
772 /* ###### attach / release kinbodys ###### */
773 /** Attach a kinbody to the robot.
774  * @param object KinbodyPtr of object to be attached
775  * @param manip_name name of the manipulator to attach the object to.
776  * If non given, the currently active manipulator is taken.
777  * @return true if successful
778  */
779 bool
780 OpenRaveRobot::attach_object(OpenRAVE::KinBodyPtr object, const char *manip_name)
781 {
782  EnvironmentMutex::scoped_lock lock(robot_->GetEnv()->GetMutex());
783 
784  bool success = false;
785  try {
786  if (manip_name) {
787  // try attaching to given manipulator
788  RobotBase::ManipulatorPtr manip = robot_->SetActiveManipulator(manip_name);
789  if (!manip) {
790  if (logger_)
791  logger_->log_warn(name(),
792  "Could not attach Object, could not get manipulator '%s'",
793  manip_name);
794  return false;
795 
796  } else {
797  success = robot_->Grab(object, manip->GetEndEffector());
798  }
799 
800  } else {
801  // use currently active manipulator
802  success = robot_->Grab(object);
803  }
804  } catch (const OpenRAVE::openrave_exception &e) {
805  if (logger_)
806  logger_->log_warn(name(), "Could not attach Object. Ex:%s", e.what());
807  return false;
808  }
809 
810  return success;
811 }
812 /** Attach a kinbody to the robot.
813  * @param name name of the object
814  * @param env pointer to OpenRaveEnvironment object
815  * @param manip_name name of the manipulator to attach the object to
816  * @return true if successful
817  */
818 bool
821  const char * manip_name)
822 {
823  OpenRAVE::KinBodyPtr body;
824  {
825  EnvironmentMutex::scoped_lock lock(env->get_env_ptr()->GetMutex());
826  body = env->get_env_ptr()->GetKinBody(name);
827  }
828 
829  return attach_object(body, manip_name);
830 }
831 
832 /** Release a kinbody from the robot.
833  * @param object KinbodyPtr of object to be released
834  * @return true if successful
835  */
836 bool
837 OpenRaveRobot::release_object(OpenRAVE::KinBodyPtr object)
838 {
839  try {
840  EnvironmentMutex::scoped_lock lock(robot_->GetEnv()->GetMutex());
841  robot_->Release(object);
842  } catch (const OpenRAVE::openrave_exception &e) {
843  if (logger_)
844  logger_->log_warn(name(), "Could not release Object. Ex:%s", e.what());
845  return false;
846  }
847 
848  return true;
849 }
850 /** Release a kinbody from the robot.
851  * @param name name of the object
852  * @param env pointer to OpenRaveEnvironment object
853  * @return true if successful
854  */
855 bool
857 {
858  OpenRAVE::KinBodyPtr body;
859  {
860  EnvironmentMutex::scoped_lock lock(env->get_env_ptr()->GetMutex());
861  body = env->get_env_ptr()->GetKinBody(name);
862  }
863 
864  return release_object(body);
865 }
866 
867 /** Release all grabbed kinbodys from the robot.
868  * @return true if successful
869  */
870 bool
872 {
873  try {
874  EnvironmentMutex::scoped_lock lock(robot_->GetEnv()->GetMutex());
875  robot_->ReleaseAllGrabbed();
876  } catch (const OpenRAVE::openrave_exception &e) {
877  if (logger_)
878  logger_->log_warn(name(), "Could not release all objects. Ex:%s", e.what());
879  return false;
880  }
881 
882  return true;
883 }
884 
885 /* ########################################
886  ###------------- private ------------###
887  ########################################*/
888 
889 /** Set target, given transformation (transition, and rotation as quaternion).
890  * Check IK solvability for target Transform. If solvable,
891  * then set target angles to manipulator configuration target_.manip
892  * @param trans transformation vector
893  * @param rotQuat rotation vector; a quaternion
894  * @param no_offset if true, do not include manipulator offset (default: false)
895  * @return true if solvable, false otherwise
896  */
897 bool
898 OpenRaveRobot::set_target_transform(Vector & trans,
899  OpenRAVE::Vector &rotQuat,
900  IkFilterOptions filter,
901  bool no_offset)
902 {
903  Transform target;
904  target.trans = trans;
905  target.rot = rotQuat;
906 
907  if (!no_offset) {
908  target.trans[0] += trans_offset_x_;
909  target.trans[1] += trans_offset_y_;
910  target.trans[2] += trans_offset_z_;
911  }
912 
913  target_.type = TARGET_TRANSFORM;
914  target_.x = target.trans[0];
915  target_.y = target.trans[1];
916  target_.z = target.trans[2];
917  target_.qw = target.rot[0];
918  target_.qx = target.rot[1];
919  target_.qy = target.rot[2];
920  target_.qz = target.rot[3];
921 
922  // check for supported IK types
923  EnvironmentMutex::scoped_lock lock(robot_->GetEnv()->GetMutex());
924  arm_ = robot_->GetActiveManipulator();
925  if (arm_->GetIkSolver()->Supports(IKP_Transform6D)) {
926  if (logger_)
927  logger_->log_debug(name(), "6D suppport for arm %s", arm_->GetName().c_str());
928  // arm supports 6D ik. Perfect!
929  target_.ikparam = IkParameterization(target);
930  solve_ik(filter);
931 
932  } else if (arm_->GetIkSolver()->Supports(IKP_TranslationDirection5D)) {
933  if (logger_)
934  logger_->log_debug(name(), "5D suppport");
935  // arm has only 5 DOF.
936  target_.ikparam = get_5dof_ikparam(target);
937  target_.solvable = set_target_ikparam(target_.ikparam, filter);
938 
939  } else {
940  if (logger_)
941  logger_->log_debug(name(), "No IK suppport");
942  //other IK types not supported yet
943  target_.solvable = false;
944  }
945 
946  return target_.solvable;
947 }
948 
949 /** Set target, given 3 consecutive axis rotations.
950  * Axis rotations are given as 1 vector representing a 3x3 matrix,
951  * (left to right, top to bottom) where each row represents
952  * one rotation over one axis (axis-angle notation).
953  * See public setTargetEuler methods to get a better understanding.
954  *
955  * Check IK solvability for target Transform. If solvable,
956  * then set target angles to manipulator configuration target_.manip
957  * @param rotations 3x3 matrix given as one row.
958  * @param filter IK filter options (see OpenRAVE doc for details)
959  * @param no_offset if true, do not include manipulator offset (default: false)
960  * @return true if solvable, false otherwise
961  */
962 bool
963 OpenRaveRobot::set_target_euler(OpenRAVE::Vector & trans,
964  std::vector<float> & rotations,
965  OpenRAVE::IkFilterOptions filter,
966  bool no_offset)
967 {
968  if (rotations.size() != 9) {
969  target_.type = TARGET_NONE;
970  target_.solvable = false;
971 
972  if (logger_)
973  logger_->log_error(name(),
974  "Bad size of rotations vector. Is %i, expected 9",
975  rotations.size());
976  return false;
977  }
978 
979  Vector r1(rotations.at(0), rotations.at(1), rotations.at(2));
980  Vector r2(rotations.at(3), rotations.at(4), rotations.at(5));
981  Vector r3(rotations.at(6), rotations.at(7), rotations.at(8));
982 
983  if (logger_) {
984  logger_->log_debug(name(), "TEST Rot1: %f %f %f", r1[0], r1[1], r1[2]);
985  logger_->log_debug(name(), "TEST Rot2: %f %f %f", r2[0], r2[1], r2[2]);
986  logger_->log_debug(name(), "TEST Rot3: %f %f %f", r3[0], r3[1], r3[2]);
987  }
988 
989  Vector q1 = quatFromAxisAngle(r1);
990  Vector q2 = quatFromAxisAngle(r2);
991  Vector q3 = quatFromAxisAngle(r3);
992 
993  Vector q12 = quatMultiply(q1, q2);
994  Vector quat = quatMultiply(q12, q3);
995 
996  return set_target_transform(trans, quat, filter, no_offset);
997 }
998 
999 /** Get IkParameterization for a 5DOF arm given a 6D Transform.
1000  * @param trans The 6D OpenRAVE::Transform
1001  * @return the calculated 5DOF IkParameterization
1002  */
1003 OpenRAVE::IkParameterization
1004 OpenRaveRobot::get_5dof_ikparam(OpenRAVE::Transform &trans)
1005 {
1006  /* The initial pose (that means NOT all joints=0, but the manipulator's coordinate-system
1007  matching the world-coordinate-system) of an arm in OpenRAVE has its gripper pointing to the z-axis.
1008  Imagine a tube between the grippers. That tube lies on the y-axis.
1009  For 5DOF-IK one needs another manipulator definition, that has it's z-axis lying on that
1010  'tube', i.e. it needs to be lying between the fingers. That is achieved by rotating the
1011  coordinate-system first by +-90° around z-axis, then +90° on the rotated x-axis.
1012  */
1013 
1014  // get direction vector for TranslationDirection5D
1015  /* Rotate Vector(0, +-1, 0) by target.rot. First need to figure out which of "+-"
1016  Now if the first rotation on z-axis was +90°, we need a (0,-1,0) direction vector.
1017  If it was -90°, we need (0, 1, 0). So just take the inverse of the first rotation
1018  and apply it to (1,0,0)
1019  */
1020  EnvironmentMutex::scoped_lock lock(robot_->GetEnv()->GetMutex());
1021  Vector dir(1, 0, 0);
1022  {
1023  RobotBasePtr tmp_robot = robot_;
1024  RobotBase::RobotStateSaver saver(
1025  tmp_robot); // save the state, do not modifiy currently active robot!
1026 
1027  //reset robot joints
1028  std::vector<dReal> zero_joints(tmp_robot->GetActiveDOF(), (dReal)0.0);
1029  tmp_robot->SetActiveDOFValues(zero_joints);
1030 
1031  // revert the rotations for the 5DOF manipulator specifition. See long comment above.
1032  // First rotate back -90° on x-axis (revert 2nd rotation)
1033  Transform cur_pos = arm_->GetEndEffectorTransform();
1034  Vector v1 = quatFromAxisAngle(Vector(-M_PI / 2, 0, 0));
1035  v1 = quatMultiply(cur_pos.rot, v1);
1036 
1037  // Now get the inverse of 1st rotation and get our (0, +-1, 0) direction
1038  v1 = quatInverse(v1);
1039  TransformMatrix mat = matrixFromQuat(v1);
1040  dir = mat.rotate(dir);
1041  } // robot state is restored
1042 
1043  // now rotate direction by target
1044  TransformMatrix mat = matrixFromQuat(trans.rot);
1045  dir = mat.rotate(dir);
1046 
1047  IkParameterization ikparam = arm_->GetIkParameterization(IKP_TranslationDirection5D);
1048  ikparam.SetTranslationDirection5D(RAY(trans.trans, dir));
1049 
1050  return ikparam;
1051 }
1052 
1053 /** Find IK solution that is closest to current configuration.
1054  * This method checks and updates the internal target_ variable.
1055  * @return true if solvable, false otherwise.
1056  */
1057 bool
1058 OpenRaveRobot::solve_ik(IkFilterOptions filter)
1059 {
1060  if (!find_best_ik_) {
1061  std::vector<dReal> solution;
1062  target_.solvable = arm_->FindIKSolution(target_.ikparam, solution, filter);
1063  target_.manip->set_angles(solution);
1064 
1065  } else {
1066  std::vector<std::vector<dReal>> solutions;
1067 
1068  // get all IK solutions
1069  target_.solvable = arm_->FindIKSolutions(target_.ikparam, solutions, filter);
1070  if (!target_.solvable)
1071  return false;
1072 
1073  // pick closest solution to current configuration
1074  std::vector<std::vector<dReal>>::iterator sol;
1075  std::vector<dReal> cur;
1076  std::vector<dReal> diff;
1077  float dist = 100.f;
1078  arm_->GetArmDOFValues(cur);
1079 
1080  for (sol = solutions.begin(); sol != solutions.end(); ++sol) {
1081  diff = cur;
1082  robot_->SubtractActiveDOFValues(diff, *sol);
1083 
1084  float sol_dist = 0.f;
1085  for (unsigned int i = 0; i < diff.size(); ++i) {
1086  sol_dist += fabs(diff[i]);
1087  // use cur+diff instead of sol, to have better angles
1088  // for circular joints. Otherwise planner might have problems
1089  (*sol)[i] = cur[i] - diff[i];
1090  }
1091 
1092  if (sol_dist < dist) {
1093  // found a solution that is closer
1094  dist = sol_dist;
1095  target_.manip->set_angles(*sol);
1096  }
1097  }
1098  }
1099 
1100  return target_.solvable;
1101 }
1102 
1103 } // end of namespace fawkes
virtual void set_target_angles(std::vector< float > &angles)
Set target angles directly.
Definition: robot.cpp:680
virtual OpenRaveManipulatorPtr copy()=0
Create a new copy of this OpenRaveManipulator instance.
Target: OpenRAVE::IkParameterization string.
Definition: types.h:57
virtual void update_manipulator()
Update motor values from OpenRAVE model.
Definition: robot.cpp:309
ZXZ rotation.
Definition: types.h:45
float qx
x value of quaternion
Definition: types.h:76
virtual bool set_target_euler(euler_rotation_t type, float trans_x, float trans_y, float trans_z, float phi, float theta, float psi, OpenRAVE::IkFilterOptions filter=OpenRAVE::IKFO_CheckEnvCollisions, bool no_offset=false)
Set target, given transition, and Euler-rotation.
Definition: robot.cpp:470
virtual void set_target_plannerparams(std::string &params)
Set additional planner parameters.
Definition: robot.cpp:638
float x
translation on x-axis
Definition: types.h:73
Target: relative endeffector translation, based on arm extension.
Definition: types.h:56
Fawkes library namespace.
virtual std::vector< std::vector< float > > * get_trajectory_device() const
Return pointer to trajectory of motion from manip_ to target_.manip with device angle format.
Definition: robot.cpp:749
ZYZ rotation.
Definition: types.h:46
float qz
z value of quaternion
Definition: types.h:78
virtual target_t get_target() const
Get target.
Definition: robot.cpp:701
virtual void set_offset(float trans_x, float trans_y, float trans_z)
Directly set transition offset between coordinate systems of real device and OpenRAVE model.
Definition: robot.cpp:258
virtual OpenRAVE::RobotBasePtr get_robot_ptr() const
Returns RobotBasePtr for uses in other classes.
Definition: robot.cpp:692
virtual bool set_target_rel(float trans_x, float trans_y, float trans_z, bool is_extension=false)
Set target, given relative transition.
Definition: robot.cpp:357
target_type_t type
target type
Definition: types.h:82
virtual bool set_target_object_position(float trans_x, float trans_y, float trans_z, float rot_x, OpenRAVE::IkFilterOptions filter=OpenRAVE::IKFO_CheckEnvCollisions)
Set target by giving position of an object.
Definition: robot.cpp:547
Target: absolute endeffector translation and rotation.
Definition: types.h:54
std::string raw_cmd
raw command passed to the BaseManipulator module, e.g.
Definition: types.h:88
std::string plannerparams
additional string to be passed to planner, i.e.
Definition: types.h:86
virtual OpenRAVE::PlannerBase::PlannerParametersPtr get_planner_params() const
Updates planner parameters and return pointer to it.
Definition: robot.cpp:719
float z
translation on z-axis
Definition: types.h:75
virtual void calibrate(float device_trans_x, float device_trans_y, float device_trans_z)
Calculate transition offset between coordinate systems of real device and OpenRAVE model.
Definition: robot.cpp:274
float y
translation on y-axis
Definition: types.h:74
void set_angles(std::vector< T > &angles)
Set motor angles of OpenRAVE model.
Definition: manipulator.h:126
ZYX rotation.
Definition: types.h:47
OpenRAVE Robot class.
Definition: robot.h:37
Base class for exceptions in Fawkes.
Definition: exception.h:35
virtual void update_model()
Update/Set OpenRAVE motor angles.
Definition: robot.cpp:319
Target: relative endeffector translation, based on robot's coordinate system.
Definition: types.h:55
OpenRAVE::IkParameterization ikparam
OpenRAVE::IkParameterization; each target is implicitly transformed to one by OpenRAVE.
Definition: types.h:84
No valid target.
Definition: types.h:52
virtual void enable_ik_comparison(bool enable)
Activate/Deactive IK comparison.
Definition: robot.cpp:343
virtual std::vector< std::vector< OpenRAVE::dReal > > * get_trajectory() const
Return pointer to trajectory of motion from manip_ to target_.manip with OpenRAVE-model angle format.
Definition: robot.cpp:739
Struct containing information about the current target.
Definition: types.h:71
virtual void set_target_raw(std::string &cmd)
Set raw command for BaseManipulation module.
Definition: robot.cpp:662
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
virtual bool set_target_axis_angle(float trans_x, float trans_y, float trans_z, float angle, float axisX, float axisY, float axisZ, OpenRAVE::IkFilterOptions filter=OpenRAVE::IKFO_CheckEnvCollisions, bool no_offset=false)
Set target, given transition, and rotation as axis-angle.
Definition: robot.cpp:440
float qw
w value of quaternion
Definition: types.h:79
OpenRaveRobot(fawkes::Logger *logger=0)
Constructor.
Definition: robot.cpp:47
virtual bool attach_object(OpenRAVE::KinBodyPtr object, const char *manip_name=NULL)
Attach a kinbody to the robot.
Definition: robot.cpp:780
virtual OpenRAVE::ModuleBasePtr get_basemanip() const
Return BaseManipulation Module-Pointer.
Definition: robot.cpp:767
virtual void set_manipulator(fawkes::OpenRaveManipulatorPtr &manip, bool display_movements=false)
Set pointer to OpenRaveManipulator object.
Definition: robot.cpp:298
euler_rotation_t
Euler rotations.
Definition: types.h:44
virtual void load(const std::string &filename, fawkes::OpenRaveEnvironmentPtr &env)
Load robot from xml file.
Definition: robot.cpp:188
RefPtr<> is a reference-counting shared smartpointer.
Definition: refptr.h:49
OpenRaveManipulatorPtr manip
target manipulator configuration
Definition: types.h:81
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual OpenRaveManipulatorPtr get_manipulator() const
Get manipulator.
Definition: robot.cpp:710
void get_angles(std::vector< T > &to) const
Get motor angles of OpenRAVE model.
Definition: manipulator.h:83
virtual bool release_all_objects()
Release all grabbed kinbodys from the robot.
Definition: robot.cpp:871
virtual ~OpenRaveRobot()
Destructor.
Definition: robot.cpp:128
bool solvable
target IK solvable
Definition: types.h:80
void angles_or_to_device(std::vector< T_from > &from, std::vector< T_to > &to) const
Transform OpenRAVE motor angles to real device angles.
Definition: manipulator.h:110
virtual void set_ready()
Set robot ready for usage.
Definition: robot.cpp:211
Expected parameter is missing.
Definition: software.h:79
virtual bool set_target_straight(float trans_x, float trans_y, float trans_z)
Set target for a straight movement, given transition.
Definition: robot.cpp:384
virtual bool display_planned_movements() const
Getter for display_planned_movements_.
Definition: robot.cpp:331
Target: motor joint values.
Definition: types.h:53
float qy
y value of quaternion
Definition: types.h:77
virtual bool set_target_ikparam(OpenRAVE::IkParameterization ik_param, OpenRAVE::IkFilterOptions filter=OpenRAVE::IKFO_CheckEnvCollisions)
Set target by giving IkParameterizaion of target.
Definition: robot.cpp:618
virtual bool release_object(OpenRAVE::KinBodyPtr object)
Release a kinbody from the robot.
Definition: robot.cpp:837
virtual bool set_target_quat(float trans_x, float trans_y, float trans_z, float quat_w, float quat_x, float quat_y, float quat_z, OpenRAVE::IkFilterOptions filter=OpenRAVE::IKFO_CheckEnvCollisions, bool no_offset=false)
Set target, given transition, and rotation as quaternion.
Definition: robot.cpp:411
Interface for logging.
Definition: logger.h:41