Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
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 {
53  init();
54 }
55 /** Constructor
56  * @param filename path to robot's xml file
57  * @param env pointer to OpenRaveEnvironment object
58  * @param logger pointer to fawkes logger
59  */
60 OpenRaveRobot::OpenRaveRobot(const std::string& filename, fawkes::OpenRaveEnvironment* env, fawkes::Logger* logger) :
61  __logger( logger ),
62  __name( "" ),
63  __manip( 0 )
64 {
65  init();
66  this->load(filename, env);
67 }
68 
69 /** Destructor */
71 {
72  delete __target.manip;
73 
74  //unload everything related to this robot from environment
75  try {
76  EnvironmentMutex::scoped_lock lock(__robot->GetEnv()->GetMutex());
77  __robot->GetEnv()->Remove(__mod_basemanip);
78  __robot->GetEnv()->Remove(__robot);
79  } catch(const openrave_exception &e) {
80  if(__logger)
81  {__logger->log_warn("OpenRAVE Robot", "Could not unload robot properly from environment. Ex:%s", e.what());}
82  }
83 }
84 
85 /** Inittialize object attributes */
86 void
87 OpenRaveRobot::init()
88 {
89  __traj = new std::vector< std::vector<dReal> >();
90 
91  __trans_offset_x = 0.f;
92  __trans_offset_y = 0.f;
93  __trans_offset_z = 0.f;
94 }
95 
96 
97 /** Load robot from xml file
98  * @param filename path to robot's xml file
99  * @param env pointer to OpenRaveEnvironment object
100  */
101 void
102 OpenRaveRobot::load(const std::string& filename, fawkes::OpenRaveEnvironment* env)
103 {
104  // TODO: implementing without usage of 'environment'
105  // openrave_exception handling is done in OpenRAVE (see environment-core.h)
106  __robot = env->get_env_ptr()->ReadRobotXMLFile(filename);
107 
108  if(!__robot)
109  {throw fawkes::IllegalArgumentException("OpenRAVE Robot: Robot could not be loaded. Check xml file/path.");}
110  else if(__logger)
111  {__logger->log_debug("OpenRAVE Robot", "Robot loaded.");}
112 }
113 
114 /** Set robot ready for usage.
115  * Here: Set active DOFs and create plannerParameters.
116  * CAUTION: Only successful after added to environment. Otherwise no active DOF will be recognized. */
117 void
119 {
120  if(!__robot)
121  {throw fawkes::Exception("OpenRAVE Robot: Robot not loaded properly yet.");}
122 
123  __name = __robot->GetName();
124  __robot->SetActiveManipulator(__robot->GetManipulators().at(0)->GetName());
125  __arm = __robot->GetActiveManipulator();
126  __robot->SetActiveDOFs(__arm->GetArmIndices());
127 
128  if(__robot->GetActiveDOF() == 0)
129  {throw fawkes::Exception("OpenRAVE Robot: Robot not added to environment yet. Need to do that first, otherwise planner will fail.");}
130 
131  // create planner parameters
132  try {
133  PlannerBase::PlannerParametersPtr params(new PlannerBase::PlannerParameters());
134  __planner_params = params;
135  __planner_params->_nMaxIterations = 4000; // max iterations before failure
136  __planner_params->SetRobotActiveJoints(__robot); // set planning configuration space to current active dofs
137  __planner_params->vgoalconfig.resize(__robot->GetActiveDOF());
138  } catch(const openrave_exception &e) {
139  throw fawkes::Exception("OpenRAVE Robot: Could not create PlannerParameters. Ex:%s", e.what());
140  }
141 
142  // create and load BaseManipulation module
143  try {
144  __mod_basemanip = RaveCreateModule(__robot->GetEnv(), "basemanipulation");
145  __robot->GetEnv()->AddModule( __mod_basemanip, __robot->GetName());
146  } catch(const openrave_exception &e) {
147  throw fawkes::Exception("OpenRAVE Robot: Cannot load BaseManipulation Module. Ex:%s", e.what());
148  }
149 
150  if(__logger)
151  {__logger->log_debug("OpenRAVE Robot", "Robot ready.");}
152 }
153 
154 /** Directly set transition offset between coordinate systems
155  * of real device and OpenRAVE model.
156  * @param trans_x transition offset on x-axis
157  * @param trans_y transition offset on y-axis
158  * @param trans_z transition offset on z-axis
159  */
160  void
161  OpenRaveRobot::set_offset(float trans_x, float trans_y, float trans_z)
162  {
163  __trans_offset_x = trans_x;
164  __trans_offset_y = trans_y;
165  __trans_offset_z = trans_z;
166  }
167 
168 /** Calculate transition offset between coordinate systems
169  * of real device and OpenRAVE model.
170  * Sets model's angles to current device's angles (from __manip),
171  * and compares transitions.
172  * @param device_trans_x transition on x-axis (real device)
173  * @param device_trans_y transition on y-axis (real device)
174  * @param device_trans_z transition on z-axis (real device)
175  */
176 void
177 OpenRaveRobot::calibrate(float device_trans_x, float device_trans_y, float device_trans_z)
178 {
179  EnvironmentMutex::scoped_lock lock(__robot->GetEnv()->GetMutex());
180  // get device's current angles, and set them for OpenRAVE model
181  std::vector<dReal> angles;
182  __manip->get_angles(angles);
183  __robot->SetActiveDOFValues(angles);
184 
185  // get model's current transition and compare
186  __arm = __robot->GetActiveManipulator();
187  Transform trans = __arm->GetEndEffectorTransform();
188  __trans_offset_x = trans.trans[0] - device_trans_x;
189  __trans_offset_y = trans.trans[1] - device_trans_y;
190  __trans_offset_z = trans.trans[2] - device_trans_z;
191 }
192 
193 /** Set pointer to OpenRaveManipulator object.
194  * Make sure this is called AFTER all manipulator settings have
195  * been set (assures that __target.manip has the same settings).
196  * @param manip pointer to OpenRaveManipulator object
197  * @param display_movements true, if movements should be displayed in viewer.
198  * Better be "false" if want to sync OpenRAVE models with device
199  */
200 void
202 {
203  __manip = manip;
204  __target.manip = new OpenRaveManipulator(*__manip);
205 
206  __display_planned_movements = display_movements;
207 }
208 
209 /** Update motor values from OpenRAVE model.
210  * Can be used to sync real device with OpenRAVE model*/
211 void
213 {
214  EnvironmentMutex::scoped_lock lock(__robot->GetEnv()->GetMutex());
215  std::vector<dReal> angles;
216  __robot->GetActiveDOFValues(angles);
217  __manip->set_angles(angles);
218 }
219 
220 /** Update/Set OpenRAVE motor angles */
221 void
223 {
224  EnvironmentMutex::scoped_lock lock(__robot->GetEnv()->GetMutex());
225  std::vector<dReal> angles;
226  __manip->get_angles(angles);
227  __robot->SetActiveDOFValues(angles);
228 }
229 
230 /** Getter for __display_planned_movements.
231  * @return return value
232  */
233 bool
235 {
236  return __display_planned_movements;
237 }
238 
239 /** Set target, given relative transition.
240  * This is the prefered method to set a target for straight manipulator movement.
241  * @param trans_x x-transition
242  * @param trans_y y-transition
243  * @param trans_z z-transition
244  * @param is_extension true, if base coordination system lies in arm extension
245  * @return true if solvable, false otherwise
246  */
247 bool
248 OpenRaveRobot::set_target_rel(float trans_x, float trans_y, float trans_z, bool is_extension)
249 {
250  if( is_extension ) {
251  __target.type = TARGET_RELATIVE_EXT;
252  } else {
253  __target.type = TARGET_RELATIVE;
254  }
255  __target.x = trans_x;
256  __target.y = trans_y;
257  __target.z = trans_z;
258 
259  // Not sure how to check IK solvability yet. Would be nice to have this
260  // checked before planning a path.
261  __target.solvable = true;
262 
263  return __target.solvable;
264 }
265 
266 /** Set target for a straight movement, given transition.
267  * This is the a wrapper for "set_target_rel", to be able to call for a
268  * straight arm movement by giving non-relative transition.
269  * @param trans_x x-transition
270  * @param trans_y y-transition
271  * @param trans_z z-transition
272  * @return true if solvable, false otherwise
273  */
274 bool
275 OpenRaveRobot::set_target_straight(float trans_x, float trans_y, float trans_z)
276 {
277  EnvironmentMutex::scoped_lock lock(__robot->GetEnv()->GetMutex());
278  __arm = __robot->GetActiveManipulator();
279  Transform trans = __arm->GetEndEffectorTransform();
280 
281  return set_target_rel( trans_x - trans.trans[0],
282  trans_y - trans.trans[1],
283  trans_z - trans.trans[2]);
284 }
285 
286 /** Set target, given transition, and rotation as quaternion.
287  * @param trans_x x-transition
288  * @param trans_y y-transition
289  * @param trans_z z-transition
290  * @param quat_w quaternion skalar
291  * @param quat_x quaternion 1st value
292  * @param quat_y quaternion 2nd value
293  * @param quat_z quaternion 3rd value
294  * @param no_offset if true, do not include manipulator offset (default: false)
295  * @return true if solvable, false otherwise
296  */
297 bool
298 OpenRaveRobot::set_target_quat(float trans_x, float trans_y, float trans_z, float quat_w, float quat_x, float quat_y, float quat_z, bool no_offset)
299 {
300  Vector trans(trans_x, trans_y, trans_z);
301  Vector rot(quat_w, quat_x, quat_y, quat_z);
302 
303  return set_target_transform(trans, rot, no_offset);
304 }
305 
306 /** Set target, given transition, and rotation as axis-angle.
307  * @param trans_x x-transition
308  * @param trans_y y-transition
309  * @param trans_z z-transition
310  * @param angle axis-angle angle
311  * @param axisX axis-angle x-axis value
312  * @param axisY axis-angle y-axis value
313  * @param axisZ axis-angle z-axis value
314  * @param no_offset if true, do not include manipulator offset (default: false)
315  * @return true if solvable, false otherwise
316  */
317 bool
318 OpenRaveRobot::set_target_axis_angle(float trans_x, float trans_y, float trans_z, float angle, float axisX, float axisY, float axisZ, bool no_offset)
319 {
320  Vector trans(trans_x, trans_y, trans_z);
321  Vector aa(angle, axisX, axisY, axisZ);
322  Vector rot = quatFromAxisAngle(aa);
323 
324  return set_target_transform(trans, rot, no_offset);
325 }
326 
327 /** Set target, given transition, and Euler-rotation.
328  * @param type Euler-rotation type (ZXZ, ZYZ, ...)
329  * @param trans_x x-transition
330  * @param trans_y y-transition
331  * @param trans_z z-transition
332  * @param phi 1st rotation
333  * @param theta 2nd rotation
334  * @param psi 3rd rotation
335  * @param no_offset if true, do not include manipulator offset (default: false)
336  * @return true if solvable, false otherwise
337  */
338 bool
339 OpenRaveRobot::set_target_euler(euler_rotation_t type, float trans_x, float trans_y, float trans_z, float phi, float theta, float psi, bool no_offset)
340 {
341  Vector trans(trans_x, trans_y, trans_z);
342  std::vector<float> rot(9, 0.f); //rotations vector
343 
344  switch(type) {
345  case (EULER_ZXZ) :
346  __logger->log_debug("TEST ZXZ", "%f %f %f %f %f %f", trans_x, trans_y, trans_z, phi, theta, psi);
347  rot.at(2) = phi; //1st row, 3rd value; rotation on z-axis
348  rot.at(3) = theta; //2nd row, 1st value; rotation on x-axis
349  rot.at(8) = psi; //3rd row, 3rd value; rotation on z-axis
350  break;
351 
352  case (EULER_ZYZ) :
353  __logger->log_debug("TEST ZYZ", "%f %f %f %f %f %f", trans_x, trans_y, trans_z, phi, theta, psi);
354  rot.at(2) = phi; //1st row, 3rd value; rotation on z-axis
355  rot.at(4) = theta; //2nd row, 2nd value; rotation on y-axis
356  rot.at(8) = psi; //3rd row, 3rd value; rotation on z-axis
357  break;
358 
359  case (EULER_ZYX) :
360  rot.at(2) = phi; //1st row, 3rd value; rotation on z-axis
361  rot.at(4) = theta; //2nd row, 2nd value; rotation on y-axis
362  rot.at(6) = psi; //3rd row, 1st value; rotation on x-axis
363  break;
364 
365  default :
366  __target.type = TARGET_NONE;
367  __target.solvable = false;
368  return false;
369  }
370 
371  return set_target_euler(trans, rot, no_offset);
372 }
373 
374 /** Set target by giving position of an object.
375  * Currently the object should be cylindric, and stand upright. It may
376  * also be rotated on its x-axis, but that rotation needs to be given in an argument
377  * to calculate correct position for end-effector. This is only temporary until
378  * proper grasp planning for 5DOF in OpenRAVE is provided.
379  * @param trans_x x-transition of object
380  * @param trans_y y-transition of object
381  * @param trans_z z-transition of object
382  * @param rot_x rotation of object on x-axis (radians) (default: 0.f, i.e. upright)
383  * @return true if solvable, false otherwise
384  */
385 bool
386 OpenRaveRobot::set_target_object_position(float trans_x, float trans_y, float trans_z, float rot_x)
387 {
388  // This is about 2 times faster than using setTargetEuler each time, especially when it comes
389  // to the while loop (whole loop: ~56ms vs ~99ms)
390 
391  // release all attached/grabbed bodys
392  __robot->ReleaseAllGrabbed();
393 
394  // quaternion defining consecutiv rotations on axis
395  float alpha = atan2(trans_y - __trans_offset_y, trans_x - __trans_offset_x); //angle to rotate left/right when manipulator points to +x
396  Vector quat_y = quatFromAxisAngle(Vector(0.f, M_PI/2, 0.f)); //1st, rotate down -> manipulator points to +x
397  Vector quat_x = quatFromAxisAngle(Vector(-alpha, 0.f, 0.f)); //2nd, rotate left/right -> manipulator points to object
398  Vector quat_z = quatFromAxisAngle(Vector(0.f, 0.f, rot_x)); //last, rotate wrist -> manipulator ready to grab
399 
400  Vector quat_xY = quatMultiply (quat_y, quat_x);
401  Vector quat_xYZ = quatMultiply (quat_xY, quat_z);
402 
403  Vector trans(trans_x, trans_y, trans_z);
404 
405  if( set_target_transform(trans, quat_xYZ, true) )
406  return true;
407 
408  //try varying 2nd rotation (quat_y) until a valid IK is found. Max angle: 45° (~0.79 rad)
409  Vector quatPosY=quatFromAxisAngle(Vector(0.f, 0.017f, 0.f)); //rotate up for 1°
410  Vector quatNegY=quatFromAxisAngle(Vector(0.f, -0.017f, 0.f)); //rotate down for 1°
411 
412  Vector quatPos(quat_xY); //starting position, after first 2 rotations
413  Vector quatNeg(quat_xY);
414 
415  unsigned int count = 0;
416  bool foundIK = false;
417 
418  while( (!foundIK) && (count <= 45)) {
419  count++;
420 
421  quatPos = quatMultiply(quatPos, quatPosY); //move up ~1°
422  quatNeg = quatMultiply(quatNeg, quatNegY); //move down ~1°
423 
424  quat_xYZ = quatMultiply(quatPos, quat_z); //apply wrist rotation
425  foundIK = set_target_transform(trans, quat_xYZ, true);
426  if( !foundIK ) {
427  quat_xYZ = quatMultiply(quatNeg, quat_z);
428  foundIK = set_target_transform(trans, quat_xYZ, true);
429  }
430  }
431 
432  return foundIK;
433 }
434 
435 /** Set target by giving IkParameterizaion of target.
436  * OpenRAVE::IkParameterization is the desired type to be calculated with
437  * by OpenRAVE. Each oter type (i.e. Transform) is implicitly transformed
438  * to an IkParameterization before continuing to check for Ik solution and
439  * planning, i.e. by the BaseManipulation module.
440  * @param ik_param the OpenRAVE::IkParameterization of the target
441  * @return true if solvable, false otherwise
442  */
443 bool
444 OpenRaveRobot::set_target_ikparam(OpenRAVE::IkParameterization ik_param)
445 {
446  EnvironmentMutex::scoped_lock lock(__robot->GetEnv()->GetMutex());
447  __arm = __robot->GetActiveManipulator();
448  std::vector<OpenRAVE::dReal> target_angles;
449 
450  __target.ikparam = ik_param;
451  __target.type = TARGET_IKPARAM;
452  __target.solvable = __arm->FindIKSolution(ik_param,target_angles,true);
453  __target.manip->set_angles(target_angles);
454 
455  return __target.solvable;
456 }
457 
458 /** Set additional planner parameters.
459  * BaseManipulation module accepts many arguments that can be passed.
460  * Planner parameters can be important to plan a path according to ones
461  * needs, e.g. set deviations, optimizer iterations, etc.
462  * Do not mistake it with the single argument "plannerparams" of BaseManipulation.
463  * @param params complete string of additional arguments.
464  */
465 void
467 {
468  __target.plannerparams = params;
469 }
470 
471 // just temporary! no IK check etc involved
472 /** Set target angles directly.
473  * @param angles vector with angle values
474  */
475 void
476 OpenRaveRobot::set_target_angles( std::vector<float>& angles )
477 {
478  __target.manip->set_angles(angles);
479 }
480 
481 
482 
483 
484 /* ################### getters ##################*/
485 /** Returns RobotBasePtr for uses in other classes.
486  * @return RobotBasePtr of current robot
487  */
488 OpenRAVE::RobotBasePtr
490 {
491  return __robot;
492 }
493 
494 /** Get target.
495  * @return target struct
496  */
497 target_t
499 {
500  return __target;
501 }
502 
503 /** Get manipulator.
504  * @return pointer to currentl used OpenRaveManipulator
505  */
508 {
509  return __manip;
510 }
511 
512 /** Updates planner parameters and return pointer to it
513  * @return PlannerParametersPtr or robot's planner params
514  */
515 OpenRAVE::PlannerBase::PlannerParametersPtr
517 {
518  EnvironmentMutex::scoped_lock lock(__robot->GetEnv()->GetMutex());
519  __manip->get_angles(__planner_params->vinitialconfig);
520  __target.manip->get_angles(__planner_params->vgoalconfig);
521 
522  __robot->SetActiveDOFValues(__planner_params->vinitialconfig);
523 
524  return __planner_params;
525 }
526 
527 /** Return pointer to trajectory of motion from
528  * __manip to __target.manip with OpenRAVE-model angle format
529  * @return pointer to trajectory
530  */
531 std::vector< std::vector<dReal> >*
533 {
534  return __traj;
535 }
536 
537 /** Return pointer to trajectory of motion from
538  * __manip to __target.manip with device angle format
539  * @return pointer to trajectory
540  */
541 std::vector< std::vector<float> >*
543 {
544  std::vector< std::vector<float> >* traj = new std::vector< std::vector<float> >();
545 
546  std::vector<float> v;
547 
548  for(unsigned int i=0; i<__traj->size(); i++) {
549  __manip->angles_or_to_device(__traj->at(i), v);
550  traj->push_back(v);
551  }
552 
553  return traj;
554 }
555 
556 /** Return BaseManipulation Module-Pointer.
557  * @return ModuleBasePtr
558  */
559 OpenRAVE::ModuleBasePtr
561 {
562  return __mod_basemanip;
563 }
564 
565 
566 /* ###### attach / release kinbodys ###### */
567 /** Attach a kinbody to the robot.
568  * @param object KinbodyPtr of object to be attached
569  * @return true if successful
570  */
571 bool
572 OpenRaveRobot::attach_object(OpenRAVE::KinBodyPtr object)
573 {
574  bool success = false;
575  try{
576  success = __robot->Grab(object);
577  } catch(const OpenRAVE::openrave_exception &e) {
578  if(__logger)
579  __logger->log_warn("OpenRAVE Robot", "Could not attach Object. Ex:%s", e.what());
580  return false;
581  }
582 
583  return success;
584 }
585 /** Attach a kinbody to the robot.
586  * @param name name of the object
587  * @param env pointer to OpenRaveEnvironment object
588  * @return true if successful
589  */
590 bool
592 {
593  OpenRAVE::KinBodyPtr body = env->get_env_ptr()->GetKinBody(name);
594 
595  return attach_object(body);
596 }
597 
598 /** Release a kinbody from the robot.
599  * @param object KinbodyPtr of object to be released
600  * @return true if successful
601  */
602 bool
603 OpenRaveRobot::release_object(OpenRAVE::KinBodyPtr object)
604 {
605  try{
606  __robot->Release(object);
607  } catch(const OpenRAVE::openrave_exception &e) {
608  if(__logger)
609  __logger->log_warn("OpenRAVE Robot", "Could not release Object. Ex:%s", e.what());
610  return false;
611  }
612 
613  return true;
614 }
615 /** Release a kinbody from the robot.
616  * @param name name of the object
617  * @param env pointer to OpenRaveEnvironment object
618  * @return true if successful
619  */
620 bool
622 {
623  OpenRAVE::KinBodyPtr body = env->get_env_ptr()->GetKinBody(name);
624 
625  return release_object(body);
626 }
627 
628 /** Release all grabbed kinbodys from the robot.
629  * @return true if successful
630  */
631 bool
633 {
634  try{
635  __robot->ReleaseAllGrabbed();
636  } catch(const OpenRAVE::openrave_exception &e) {
637  if(__logger)
638  __logger->log_warn("OpenRAVE Robot", "Could not release all objects. Ex:%s", e.what());
639  return false;
640  }
641 
642  return true;
643 }
644 
645 
646 
647 
648 /* ########################################
649  ###------------- private ------------###
650  ########################################*/
651 
652 /** Set target, given transformation (transition, and rotation as quaternion).
653  * Check IK solvability for target Transform. If solvable,
654  * then set target angles to manipulator configuration __target.manip
655  * @param trans transformation vector
656  * @param rotQuat rotation vector; a quaternion
657  * @param no_offset if true, do not include manipulator offset (default: false)
658  * @return true if solvable, false otherwise
659  */
660 bool
661 OpenRaveRobot::set_target_transform(OpenRAVE::Vector& trans, OpenRAVE::Vector& rotQuat, bool no_offset)
662 {
663  Transform target;
664  target.trans = trans;
665  target.rot = rotQuat;
666 
667  if( !no_offset ) {
668  target.trans[0] += __trans_offset_x;
669  target.trans[1] += __trans_offset_y;
670  target.trans[2] += __trans_offset_z;
671  }
672 
673  __target.type = TARGET_TRANSFORM;
674  __target.x = target.trans[0];
675  __target.y = target.trans[1];
676  __target.z = target.trans[2];
677  __target.qw = target.rot[0];
678  __target.qx = target.rot[1];
679  __target.qy = target.rot[2];
680  __target.qz = target.rot[3];
681 
682  // check for supported IK types
683  EnvironmentMutex::scoped_lock lock(__robot->GetEnv()->GetMutex());
684  __arm = __robot->GetActiveManipulator();
685  if( __arm->GetIkSolver()->Supports(IKP_Transform6D) ) {
686  __logger->log_debug("OR TMP", "6D suppport");
687  // arm supports 6D ik. Perfect!
688  std::vector<OpenRAVE::dReal> target_angles;
689 
690  __target.ikparam = IkParameterization(target);
691  __target.solvable = __arm->FindIKSolution(__target.ikparam,target_angles,true);
692  __target.manip->set_angles(target_angles);
693 
694  } else if( __arm->GetIkSolver()->Supports(IKP_TranslationDirection5D) ) {
695  __logger->log_debug("OR TMP", "5D suppport");
696  // arm has only 5 DOF.
697  std::vector<OpenRAVE::dReal> target_angles;
698 
699  __target.ikparam = get_5dof_ikparam(target);
700  __target.solvable = set_target_ikparam(__target.ikparam);
701 
702  } else {
703  __logger->log_debug("OR TMP", "No IK suppport");
704  //other IK types not supported yet
705  __target.solvable = false;
706  }
707 
708  return __target.solvable;
709 }
710 
711 /** Set target, given 3 consecutive axis rotations.
712  * Axis rotations are given as 1 vector representing a 3x3 matrix,
713  * (left to right, top to bottom) where each row represents
714  * one rotation over one axis (axis-angle notation).
715  * See public setTargetEuler methods to get a better understanding.
716  *
717  * Check IK solvability for target Transform. If solvable,
718  * then set target angles to manipulator configuration __target.manip
719  * @param rotations 3x3 matrix given as one row.
720  * @param no_offset if true, do not include manipulator offset (default: false)
721  * @return true if solvable, false otherwise
722  */
723 bool
724 OpenRaveRobot::set_target_euler(OpenRAVE::Vector& trans, std::vector<float>& rotations, bool no_offset)
725 {
726  if( rotations.size() != 9 ) {
727  __target.type = TARGET_NONE;
728  __target.solvable = false;
729 
730  if(__logger)
731  {__logger->log_error("OpenRAVE Robot", "Bad size of rotations vector. Is %i, expected 9", rotations.size());}
732  return false;
733  }
734 
735  Vector r1(rotations.at(0), rotations.at(1), rotations.at(2));
736  Vector r2(rotations.at(3), rotations.at(4), rotations.at(5));
737  Vector r3(rotations.at(6), rotations.at(7), rotations.at(8));
738 
739  __logger->log_debug("TEST", "Rot1: %f %f %f", r1[0], r1[1], r1[2]);
740  __logger->log_debug("TEST", "Rot2: %f %f %f", r2[0], r2[1], r2[2]);
741  __logger->log_debug("TEST", "Rot3: %f %f %f", r3[0], r3[1], r3[2]);
742 
743  Vector q1 = quatFromAxisAngle(r1);
744  Vector q2 = quatFromAxisAngle(r2);
745  Vector q3 = quatFromAxisAngle(r3);
746 
747  Vector q12 = quatMultiply (q1, q2);
748  Vector quat = quatMultiply (q12, q3);
749 
750  return set_target_transform(trans, quat, no_offset);
751 }
752 
753 /** Get IkParameterization for a 5DOF arm given a 6D Transform.
754  * @param trans The 6D OpenRAVE::Transform
755  * @return the calculated 5DOF IkParameterization
756  */
757 OpenRAVE::IkParameterization
758 OpenRaveRobot::get_5dof_ikparam(OpenRAVE::Transform& trans)
759 {
760  /* The initial pose (that means NOT all joints=0, but the manipulator's coordinate-system
761  matching the world-coordinate-system) of an arm in OpenRAVE has its gripper pointing to the z-axis.
762  Imagine a tube between the grippers. That tube lies on the y-axis.
763  For 5DOF-IK one needs another manipulator definition, that has it's z-axis lying on that
764  'tube', i.e. it needs to be lying between the fingers. That is achieved by rotating the
765  coordinate-system first by +-90° around z-axis, then +90° on the rotated x-axis.
766  */
767 
768  // get direction vector for TranslationDirection5D
769  /* Rotate Vector(0, +-1, 0) by target.rot. First need to figure out which of "+-"
770  Now if the first rotation on z-axis was +90°, we need a (0,-1,0) direction vector.
771  If it was -90°, we need (0, 1, 0). So just take the inverse of the first rotation
772  and apply it to (1,0,0)
773  */
774  EnvironmentMutex::scoped_lock lock(__robot->GetEnv()->GetMutex());
775  Vector dir(1,0,0);
776  {
777  RobotBasePtr tmp_robot = __robot;
778  RobotBase::RobotStateSaver saver(tmp_robot); // save the state, do not modifiy currently active robot!
779 
780  //reset robot joints
781  std::vector<dReal> zero_joints(tmp_robot->GetActiveDOF(), (dReal)0.0);
782  tmp_robot->SetActiveDOFValues(zero_joints);
783 
784  // revert the rotations for the 5DOF manipulator specifition. See long comment above.
785  // First rotate back -90° on x-axis (revert 2nd rotation)
786  Transform cur_pos = __arm->GetEndEffectorTransform();
787  Vector v1 = quatFromAxisAngle(Vector(-M_PI/2, 0, 0));
788  v1 = quatMultiply(cur_pos.rot, v1);
789 
790  // Now get the inverse of 1st rotation and get our (0, +-1, 0) direction
791  v1 = quatInverse(v1);
792  TransformMatrix mat = matrixFromQuat(v1);
793  dir = mat.rotate(dir);
794  } // robot state is restored
795 
796  // now rotate direction by target
797  TransformMatrix mat = matrixFromQuat(trans.rot);
798  dir = mat.rotate(dir);
799 
800  IkParameterization ikparam = __arm->GetIkParameterization(IKP_TranslationDirection5D);
801  ikparam.SetTranslationDirection5D(RAY(trans.trans, dir));
802 
803  return ikparam;
804 }
805 
806 } // end of namespace fawkes