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