Fawkes API  Fawkes Development Version
robot.cpp
00001 
00002 /***************************************************************************
00003  *  robot.cpp - Fawkes to OpenRAVE Robot Handler
00004  *
00005  *  Created: Mon Sep 20 14:50:34 2010
00006  *  Copyright  2010  Bahram Maleki-Fard, AllemaniACs RoboCup Team
00007  *
00008  ****************************************************************************/
00009 
00010 /*  This program is free software; you can redistribute it and/or modify
00011  *  it under the terms of the GNU General Public License as published by
00012  *  the Free Software Foundation; either version 2 of the License, or
00013  *  (at your option) any later version.
00014  *
00015  *  This program is distributed in the hope that it will be useful,
00016  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018  *  GNU Library General Public License for more details.
00019  *
00020  *  Read the full text in the LICENSE.GPL file in the doc directory.
00021  */
00022 
00023 #include "robot.h"
00024 #include "manipulator.h"
00025 #include "environment.h"
00026 
00027 #include <openrave-core.h>
00028 #include <logging/logger.h>
00029 #include <core/exceptions/software.h>
00030 
00031 using namespace OpenRAVE;
00032 namespace fawkes {
00033 #if 0 /* just to make Emacs auto-indent happy */
00034 }
00035 #endif
00036 
00037 /** @class OpenRaveRobot <plugins/openrave/robot.h>
00038 * Class handling interaction with the OpenRAVE::RobotBase class.
00039 * This class mainly handles robot specific tasks, like setting a
00040 * target, looking for IK solutions and handling planning parameters
00041 * for the robot.
00042 * @author Bahram Maleki-Fard
00043 */
00044 
00045 /** Constructor
00046  * @param logger pointer to fawkes logger
00047  */
00048 OpenRaveRobot::OpenRaveRobot(fawkes::Logger* logger) :
00049   __logger( logger ),
00050   __name( "" ),
00051   __manip( 0 )
00052 {
00053   init();
00054 }
00055 /** Constructor
00056  * @param filename path to robot's xml file
00057  * @param env pointer to OpenRaveEnvironment object
00058  * @param logger pointer to fawkes logger
00059  */
00060 OpenRaveRobot::OpenRaveRobot(const std::string& filename, fawkes::OpenRaveEnvironment* env, fawkes::Logger* logger) :
00061   __logger( logger ),
00062   __name( "" ),
00063   __manip( 0 )
00064 {
00065   init();
00066   this->load(filename, env);
00067 }
00068 
00069 /** Destructor */
00070 OpenRaveRobot::~OpenRaveRobot()
00071 {
00072   delete __target.manip;
00073 
00074   //unload everything related to this robot from environment
00075   try {
00076     EnvironmentMutex::scoped_lock lock(__robot->GetEnv()->GetMutex());
00077     __robot->GetEnv()->Remove(__mod_basemanip);
00078     __robot->GetEnv()->Remove(__robot);
00079   } catch(const openrave_exception &e) {
00080     if(__logger)
00081       {__logger->log_warn("OpenRAVE Robot", "Could not unload robot properly from environment. Ex:%s", e.what());}
00082   }
00083 }
00084 
00085 /** Inittialize object attributes */
00086 void
00087 OpenRaveRobot::init()
00088 {
00089   __traj = new std::vector< std::vector<dReal> >();
00090 
00091   __trans_offset_x = 0.f;
00092   __trans_offset_y = 0.f;
00093   __trans_offset_z = 0.f;
00094 }
00095 
00096 
00097 /** Load robot from xml file
00098  * @param filename path to robot's xml file
00099  * @param env pointer to OpenRaveEnvironment object
00100  */
00101 void
00102 OpenRaveRobot::load(const std::string& filename, fawkes::OpenRaveEnvironment* env)
00103 {
00104   // TODO: implementing without usage of 'environment'
00105   // openrave_exception handling is done in OpenRAVE (see environment-core.h)
00106   __robot = env->get_env_ptr()->ReadRobotXMLFile(filename);
00107 
00108   if(!__robot)
00109     {throw fawkes::IllegalArgumentException("OpenRAVE Robot: Robot could not be loaded. Check xml file/path.");}
00110   else if(__logger)
00111     {__logger->log_debug("OpenRAVE Robot", "Robot loaded.");}
00112 }
00113 
00114 /** Set robot ready for usage.
00115  * Here: Set active DOFs and create plannerParameters.
00116  * CAUTION: Only successful after added to environment. Otherwise no active DOF will be recognized. */
00117 void
00118 OpenRaveRobot::set_ready()
00119 {
00120   if(!__robot)
00121     {throw fawkes::Exception("OpenRAVE Robot: Robot not loaded properly yet.");}
00122 
00123   __name = __robot->GetName();
00124   __robot->SetActiveManipulator(__robot->GetManipulators().at(0)->GetName());
00125   __arm = __robot->GetActiveManipulator();
00126   __robot->SetActiveDOFs(__arm->GetArmIndices());
00127 
00128   if(__robot->GetActiveDOF() == 0)
00129     {throw fawkes::Exception("OpenRAVE Robot: Robot not added to environment yet. Need to do that first, otherwise planner will fail.");}
00130 
00131   // create planner parameters
00132   try {
00133     PlannerBase::PlannerParametersPtr params(new PlannerBase::PlannerParameters());
00134     __planner_params = params;
00135     __planner_params->_nMaxIterations = 4000; // max iterations before failure
00136     __planner_params->SetRobotActiveJoints(__robot); // set planning configuration space to current active dofs
00137     __planner_params->vgoalconfig.resize(__robot->GetActiveDOF());
00138   } catch(const openrave_exception &e) {
00139     throw fawkes::Exception("OpenRAVE Robot: Could not create PlannerParameters. Ex:%s", e.what());
00140   }
00141 
00142   // create and load BaseManipulation module
00143   try {
00144     __mod_basemanip = RaveCreateModule(__robot->GetEnv(), "basemanipulation");
00145     __robot->GetEnv()->AddModule( __mod_basemanip, __robot->GetName());
00146   } catch(const openrave_exception &e) {
00147     throw fawkes::Exception("OpenRAVE Robot: Cannot load BaseManipulation Module. Ex:%s", e.what());
00148   }
00149 
00150   if(__logger)
00151     {__logger->log_debug("OpenRAVE Robot", "Robot ready.");}
00152 }
00153 
00154 /** Directly set transition offset between coordinate systems
00155  * of real device and OpenRAVE model.
00156  * @param trans_x transition offset on x-axis
00157  * @param trans_y transition offset on y-axis
00158  * @param trans_z transition offset on z-axis
00159  */
00160  void
00161  OpenRaveRobot::set_offset(float trans_x, float trans_y, float trans_z)
00162  {
00163   __trans_offset_x = trans_x;
00164   __trans_offset_y = trans_y;
00165   __trans_offset_z = trans_z;
00166  }
00167 
00168 /** Calculate transition offset between coordinate systems
00169  * of real device and OpenRAVE model.
00170  * Sets model's angles to current device's angles (from __manip),
00171  * and compares transitions.
00172  * @param device_trans_x transition on x-axis (real device)
00173  * @param device_trans_y transition on y-axis (real device)
00174  * @param device_trans_z transition on z-axis (real device)
00175  */
00176 void
00177 OpenRaveRobot::calibrate(float device_trans_x, float device_trans_y, float device_trans_z)
00178 {
00179   EnvironmentMutex::scoped_lock lock(__robot->GetEnv()->GetMutex());
00180   // get device's current angles, and set them for OpenRAVE model
00181   std::vector<dReal> angles;
00182   __manip->get_angles(angles);
00183   __robot->SetActiveDOFValues(angles);
00184 
00185   // get model's current transition and compare
00186   __arm = __robot->GetActiveManipulator();
00187   Transform trans = __arm->GetEndEffectorTransform();
00188   __trans_offset_x = trans.trans[0] - device_trans_x;
00189   __trans_offset_y = trans.trans[1] - device_trans_y;
00190   __trans_offset_z = trans.trans[2] - device_trans_z;
00191 }
00192 
00193 /** Set pointer to OpenRaveManipulator object.
00194  *  Make sure this is called AFTER all manipulator settings have
00195  *  been set (assures that __target.manip has the same settings).
00196  * @param manip pointer to OpenRaveManipulator object
00197  * @param display_movements true, if movements should be displayed in viewer.
00198  *  Better be "false" if want to sync OpenRAVE models with device
00199  */
00200 void
00201 OpenRaveRobot::set_manipulator(fawkes::OpenRaveManipulator* manip, bool display_movements)
00202 {
00203   __manip = manip;
00204   __target.manip = new OpenRaveManipulator(*__manip);
00205 
00206   __display_planned_movements = display_movements;
00207 }
00208 
00209 /** Update motor values from OpenRAVE model.
00210  * Can be used to sync real device with OpenRAVE model*/
00211 void
00212 OpenRaveRobot::update_manipulator()
00213 {
00214   EnvironmentMutex::scoped_lock lock(__robot->GetEnv()->GetMutex());
00215   std::vector<dReal> angles;
00216   __robot->GetActiveDOFValues(angles);
00217   __manip->set_angles(angles);
00218 }
00219 
00220 /** Update/Set OpenRAVE motor angles */
00221 void
00222 OpenRaveRobot::update_model()
00223 {
00224   EnvironmentMutex::scoped_lock lock(__robot->GetEnv()->GetMutex());
00225   std::vector<dReal> angles;
00226   __manip->get_angles(angles);
00227   __robot->SetActiveDOFValues(angles);
00228 }
00229 
00230 /** Getter for __display_planned_movements.
00231  * @return return value
00232  */
00233 bool
00234 OpenRaveRobot::display_planned_movements() const
00235 {
00236   return __display_planned_movements;
00237 }
00238 
00239 /** Set target, given relative transition.
00240  * This is the prefered method to set a target for straight manipulator movement.
00241  * @param trans_x x-transition
00242  * @param trans_y y-transition
00243  * @param trans_z z-transition
00244  * @param is_extension true, if base coordination system lies in arm extension
00245  * @return true if solvable, false otherwise
00246  */
00247 bool
00248 OpenRaveRobot::set_target_rel(float trans_x, float trans_y, float trans_z, bool is_extension)
00249 {
00250   if( is_extension ) {
00251     __target.type = TARGET_RELATIVE_EXT;
00252   } else {
00253     __target.type = TARGET_RELATIVE;
00254   }
00255   __target.x = trans_x;
00256   __target.y = trans_y;
00257   __target.z = trans_z;
00258 
00259   // Not sure how to check IK solvability yet. Would be nice to have this
00260   // checked before planning a path.
00261   __target.solvable = true;
00262 
00263   return __target.solvable;
00264 }
00265 
00266 /** Set target for a straight movement, given transition.
00267  * This is the a wrapper for "set_target_rel", to be able to call for a
00268  * straight arm movement by giving non-relative transition.
00269  * @param trans_x x-transition
00270  * @param trans_y y-transition
00271  * @param trans_z z-transition
00272  * @return true if solvable, false otherwise
00273  */
00274 bool
00275 OpenRaveRobot::set_target_straight(float trans_x, float trans_y, float trans_z)
00276 {
00277   EnvironmentMutex::scoped_lock lock(__robot->GetEnv()->GetMutex());
00278   __arm = __robot->GetActiveManipulator();
00279   Transform trans = __arm->GetEndEffectorTransform();
00280 
00281   return set_target_rel( trans_x - trans.trans[0],
00282                          trans_y - trans.trans[1],
00283                          trans_z - trans.trans[2]);
00284 }
00285 
00286 /** Set target, given transition, and rotation as quaternion.
00287  * @param trans_x x-transition
00288  * @param trans_y y-transition
00289  * @param trans_z z-transition
00290  * @param quat_w quaternion skalar
00291  * @param quat_x quaternion 1st value
00292  * @param quat_y quaternion 2nd value
00293  * @param quat_z quaternion 3rd value
00294  * @param no_offset if true, do not include manipulator offset (default: false)
00295  * @return true if solvable, false otherwise
00296  */
00297 bool
00298 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)
00299 {
00300   Vector trans(trans_x, trans_y, trans_z);
00301   Vector   rot(quat_w, quat_x, quat_y, quat_z);
00302 
00303   return set_target_transform(trans, rot, no_offset);
00304 }
00305 
00306 /** Set target, given transition, and rotation as axis-angle.
00307  * @param trans_x x-transition
00308  * @param trans_y y-transition
00309  * @param trans_z z-transition
00310  * @param angle axis-angle angle
00311  * @param axisX axis-angle x-axis value
00312  * @param axisY axis-angle y-axis value
00313  * @param axisZ axis-angle z-axis value
00314  * @param no_offset if true, do not include manipulator offset (default: false)
00315  * @return true if solvable, false otherwise
00316  */
00317 bool
00318 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)
00319 {
00320   Vector trans(trans_x, trans_y, trans_z);
00321   Vector aa(angle, axisX, axisY, axisZ);
00322   Vector rot = quatFromAxisAngle(aa);
00323 
00324   return set_target_transform(trans, rot, no_offset);
00325 }
00326 
00327 /** Set target, given transition, and Euler-rotation.
00328  * @param type Euler-rotation type (ZXZ, ZYZ, ...)
00329  * @param trans_x x-transition
00330  * @param trans_y y-transition
00331  * @param trans_z z-transition
00332  * @param phi 1st rotation
00333  * @param theta 2nd rotation
00334  * @param psi 3rd rotation
00335  * @param no_offset if true, do not include manipulator offset (default: false)
00336  * @return true if solvable, false otherwise
00337  */
00338 bool
00339 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)
00340 {
00341   Vector trans(trans_x, trans_y, trans_z);
00342   std::vector<float> rot(9, 0.f); //rotations vector
00343 
00344   switch(type) {
00345     case (EULER_ZXZ) :
00346         __logger->log_debug("TEST ZXZ", "%f %f %f %f %f %f", trans_x, trans_y, trans_z, phi, theta, psi);
00347         rot.at(2) = phi;   //1st row, 3rd value; rotation on z-axis
00348         rot.at(3) = theta; //2nd row, 1st value; rotation on x-axis
00349         rot.at(8) = psi;   //3rd row, 3rd value; rotation on z-axis
00350         break;
00351 
00352     case (EULER_ZYZ) :
00353         __logger->log_debug("TEST ZYZ", "%f %f %f %f %f %f", trans_x, trans_y, trans_z, phi, theta, psi);
00354         rot.at(2) = phi;   //1st row, 3rd value; rotation on z-axis
00355         rot.at(4) = theta; //2nd row, 2nd value; rotation on y-axis
00356         rot.at(8) = psi;   //3rd row, 3rd value; rotation on z-axis
00357         break;
00358 
00359     case (EULER_ZYX) :
00360         rot.at(2) = phi;   //1st row, 3rd value; rotation on z-axis
00361         rot.at(4) = theta; //2nd row, 2nd value; rotation on y-axis
00362         rot.at(6) = psi;   //3rd row, 1st value; rotation on x-axis
00363         break;
00364 
00365     default :
00366         __target.type = TARGET_NONE;
00367         __target.solvable = false;
00368         return false;
00369   }
00370 
00371   return set_target_euler(trans, rot, no_offset);
00372 }
00373 
00374 /** Set target by giving position of an object.
00375  * Currently the object should be cylindric, and stand upright. It may
00376  * also be rotated on its x-axis, but that rotation needs to be given in an argument
00377  * to calculate correct position for end-effector. This is only temporary until
00378  * proper grasp planning for 5DOF in OpenRAVE is provided.
00379  * @param trans_x x-transition of object
00380  * @param trans_y y-transition of object
00381  * @param trans_z z-transition of object
00382  * @param rot_x rotation of object on x-axis (radians) (default: 0.f, i.e. upright)
00383  * @return true if solvable, false otherwise
00384  */
00385 bool
00386 OpenRaveRobot::set_target_object_position(float trans_x, float trans_y, float trans_z, float rot_x)
00387 {
00388   // This is about 2 times faster than using setTargetEuler each time, especially when it comes
00389   // to the while loop (whole loop: ~56ms vs ~99ms)
00390 
00391   // release all attached/grabbed bodys
00392   __robot->ReleaseAllGrabbed();
00393 
00394   // quaternion defining consecutiv rotations on axis
00395   float alpha = atan2(trans_y - __trans_offset_y, trans_x - __trans_offset_x);      //angle to rotate left/right when manipulator points to +x
00396   Vector quat_y = quatFromAxisAngle(Vector(0.f, M_PI/2, 0.f));           //1st, rotate down -> manipulator points to +x
00397   Vector quat_x = quatFromAxisAngle(Vector(-alpha, 0.f, 0.f));           //2nd, rotate left/right -> manipulator points to object
00398   Vector quat_z = quatFromAxisAngle(Vector(0.f, 0.f, rot_x));             //last, rotate wrist -> manipulator ready to grab
00399 
00400   Vector quat_xY =  quatMultiply (quat_y, quat_x);
00401   Vector quat_xYZ = quatMultiply (quat_xY, quat_z);
00402 
00403   Vector trans(trans_x, trans_y, trans_z);
00404 
00405   if( set_target_transform(trans, quat_xYZ, true) )
00406     return true;
00407 
00408   //try varying 2nd rotation (quat_y) until a valid IK is found. Max angle: 45° (~0.79 rad)
00409   Vector quatPosY=quatFromAxisAngle(Vector(0.f, 0.017f, 0.f));          //rotate up for 1°
00410   Vector quatNegY=quatFromAxisAngle(Vector(0.f, -0.017f, 0.f));         //rotate down for 1°
00411 
00412   Vector quatPos(quat_xY);       //starting position, after first 2 rotations
00413   Vector quatNeg(quat_xY);
00414 
00415   unsigned int count = 0;
00416   bool foundIK = false;
00417 
00418   while( (!foundIK) && (count <= 45)) {
00419     count++;
00420 
00421     quatPos = quatMultiply(quatPos, quatPosY);  //move up ~1°
00422     quatNeg = quatMultiply(quatNeg, quatNegY);  //move down ~1°
00423 
00424     quat_xYZ = quatMultiply(quatPos, quat_z);     //apply wrist rotation
00425     foundIK = set_target_transform(trans, quat_xYZ, true);
00426     if( !foundIK ) {
00427       quat_xYZ = quatMultiply(quatNeg, quat_z);
00428       foundIK = set_target_transform(trans, quat_xYZ, true);
00429     }
00430   }
00431 
00432   return foundIK;
00433 }
00434 
00435 /** Set target by giving IkParameterizaion of target.
00436  * OpenRAVE::IkParameterization is the desired type to be calculated with
00437  * by OpenRAVE. Each oter type (i.e. Transform) is implicitly transformed
00438  * to an IkParameterization before continuing to check for Ik solution and
00439  * planning, i.e. by the BaseManipulation module.
00440  * @param ik_param the OpenRAVE::IkParameterization of the target
00441  * @return true if solvable, false otherwise
00442  */
00443 bool
00444 OpenRaveRobot::set_target_ikparam(OpenRAVE::IkParameterization ik_param)
00445 {
00446   EnvironmentMutex::scoped_lock lock(__robot->GetEnv()->GetMutex());
00447   __arm = __robot->GetActiveManipulator();
00448   std::vector<OpenRAVE::dReal> target_angles;
00449 
00450   __target.ikparam = ik_param;
00451   __target.type = TARGET_IKPARAM;
00452   __target.solvable = __arm->FindIKSolution(ik_param,target_angles,true);
00453   __target.manip->set_angles(target_angles);
00454 
00455   return __target.solvable;
00456 }
00457 
00458 /** Set additional planner parameters.
00459  * BaseManipulation module accepts many arguments that can be passed.
00460  * Planner parameters can be important to plan a path according to ones
00461  * needs, e.g. set deviations, optimizer iterations, etc.
00462  * Do not mistake it with the single argument "plannerparams" of BaseManipulation.
00463  * @param params complete string of additional arguments.
00464  */
00465 void
00466 OpenRaveRobot::set_target_plannerparams(std::string& params)
00467 {
00468   __target.plannerparams = params;
00469 }
00470 
00471 // just temporary! no IK check etc involved
00472 /** Set target angles directly.
00473  * @param angles vector with angle values
00474  */
00475 void
00476 OpenRaveRobot::set_target_angles( std::vector<float>& angles )
00477 {
00478   __target.manip->set_angles(angles);
00479 }
00480 
00481 
00482 
00483 
00484 /* ################### getters ##################*/
00485 /** Returns RobotBasePtr for uses in other classes.
00486  * @return RobotBasePtr of current robot
00487  */
00488 OpenRAVE::RobotBasePtr
00489 OpenRaveRobot::get_robot_ptr() const
00490 {
00491   return __robot;
00492 }
00493 
00494 /** Get target.
00495  * @return target struct
00496  */
00497 target_t
00498 OpenRaveRobot::get_target() const
00499 {
00500   return __target;
00501 }
00502 
00503 /** Get manipulator.
00504  * @return pointer to currentl used OpenRaveManipulator
00505  */
00506 OpenRaveManipulator*
00507 OpenRaveRobot::get_manipulator() const
00508 {
00509   return __manip;
00510 }
00511 
00512 /** Updates planner parameters and return pointer to it
00513  * @return PlannerParametersPtr or robot's planner params
00514  */
00515 OpenRAVE::PlannerBase::PlannerParametersPtr
00516 OpenRaveRobot::get_planner_params() const
00517 {
00518   EnvironmentMutex::scoped_lock lock(__robot->GetEnv()->GetMutex());
00519   __manip->get_angles(__planner_params->vinitialconfig);
00520   __target.manip->get_angles(__planner_params->vgoalconfig);
00521 
00522   __robot->SetActiveDOFValues(__planner_params->vinitialconfig);
00523 
00524   return __planner_params;
00525 }
00526 
00527 /** Return pointer to trajectory of motion from
00528  * __manip to __target.manip with OpenRAVE-model angle format
00529  * @return pointer to trajectory
00530  */
00531 std::vector< std::vector<dReal> >*
00532 OpenRaveRobot::get_trajectory() const
00533 {
00534   return __traj;
00535 }
00536 
00537 /** Return pointer to trajectory of motion from
00538  * __manip to __target.manip with device angle format
00539  * @return pointer to trajectory
00540  */
00541 std::vector< std::vector<float> >*
00542 OpenRaveRobot::get_trajectory_device() const
00543 {
00544   std::vector< std::vector<float> >* traj = new std::vector< std::vector<float> >();
00545 
00546   std::vector<float> v;
00547 
00548   for(unsigned int i=0; i<__traj->size(); i++) {
00549     __manip->angles_or_to_device(__traj->at(i), v);
00550     traj->push_back(v);
00551   }
00552 
00553   return traj;
00554 }
00555 
00556 /** Return BaseManipulation Module-Pointer.
00557  * @return ModuleBasePtr
00558  */
00559 OpenRAVE::ModuleBasePtr
00560 OpenRaveRobot::get_basemanip() const
00561 {
00562   return __mod_basemanip;
00563 }
00564 
00565 
00566 /* ###### attach / release kinbodys ###### */
00567 /** Attach a kinbody to the robot.
00568  * @param object KinbodyPtr of object to be attached
00569  * @return true if successful
00570  */
00571 bool
00572 OpenRaveRobot::attach_object(OpenRAVE::KinBodyPtr object)
00573 {
00574   bool success = false;
00575   try{
00576     success = __robot->Grab(object);
00577   } catch(const OpenRAVE::openrave_exception &e) {
00578     if(__logger)
00579       __logger->log_warn("OpenRAVE Robot", "Could not attach Object. Ex:%s", e.what());
00580     return false;
00581   }
00582 
00583   return success;
00584 }
00585 /** Attach a kinbody to the robot.
00586  * @param name name of the object
00587  * @param env pointer to OpenRaveEnvironment object
00588  * @return true if successful
00589  */
00590 bool
00591 OpenRaveRobot::attach_object(const std::string& name, fawkes::OpenRaveEnvironment* env)
00592 {
00593   OpenRAVE::KinBodyPtr body = env->get_env_ptr()->GetKinBody(name);
00594 
00595   return attach_object(body);
00596 }
00597 
00598 /** Release a kinbody from the robot.
00599  * @param object KinbodyPtr of object to be released
00600  * @return true if successful
00601  */
00602 bool
00603 OpenRaveRobot::release_object(OpenRAVE::KinBodyPtr object)
00604 {
00605   try{
00606     __robot->Release(object);
00607   } catch(const OpenRAVE::openrave_exception &e) {
00608     if(__logger)
00609       __logger->log_warn("OpenRAVE Robot", "Could not release Object. Ex:%s", e.what());
00610     return false;
00611   }
00612 
00613   return true;
00614 }
00615 /** Release a kinbody from the robot.
00616  * @param name name of the object
00617  * @param env pointer to OpenRaveEnvironment object
00618  * @return true if successful
00619  */
00620 bool
00621 OpenRaveRobot::release_object(const std::string& name, fawkes::OpenRaveEnvironment* env)
00622 {
00623   OpenRAVE::KinBodyPtr body = env->get_env_ptr()->GetKinBody(name);
00624 
00625   return release_object(body);
00626 }
00627 
00628 /** Release all grabbed kinbodys from the robot.
00629  * @return true if successful
00630  */
00631 bool
00632 OpenRaveRobot::release_all_objects()
00633 {
00634   try{
00635     __robot->ReleaseAllGrabbed();
00636   } catch(const OpenRAVE::openrave_exception &e) {
00637     if(__logger)
00638       __logger->log_warn("OpenRAVE Robot", "Could not release all objects. Ex:%s", e.what());
00639     return false;
00640   }
00641 
00642   return true;
00643 }
00644 
00645 
00646 
00647 
00648 /* ########################################
00649    ###------------- private ------------###
00650    ########################################*/
00651 
00652 /** Set target, given transformation (transition, and rotation as quaternion).
00653  * Check IK solvability for target Transform. If solvable,
00654  * then set target angles to manipulator configuration __target.manip
00655  * @param trans transformation vector
00656  * @param rotQuat rotation vector; a quaternion
00657  * @param no_offset if true, do not include manipulator offset (default: false)
00658  * @return true if solvable, false otherwise
00659  */
00660 bool
00661 OpenRaveRobot::set_target_transform(OpenRAVE::Vector& trans, OpenRAVE::Vector& rotQuat, bool no_offset)
00662 {
00663   Transform target;
00664   target.trans = trans;
00665   target.rot = rotQuat;
00666 
00667   if( !no_offset ) {
00668     target.trans[0] += __trans_offset_x;
00669     target.trans[1] += __trans_offset_y;
00670     target.trans[2] += __trans_offset_z;
00671   }
00672 
00673   __target.type = TARGET_TRANSFORM;
00674   __target.x  = target.trans[0];
00675   __target.y  = target.trans[1];
00676   __target.z  = target.trans[2];
00677   __target.qw = target.rot[0];
00678   __target.qx = target.rot[1];
00679   __target.qy = target.rot[2];
00680   __target.qz = target.rot[3];
00681 
00682   // check for supported IK types
00683   EnvironmentMutex::scoped_lock lock(__robot->GetEnv()->GetMutex());
00684   __arm = __robot->GetActiveManipulator();
00685   if( __arm->GetIkSolver()->Supports(IKP_Transform6D) ) {
00686     __logger->log_debug("OR TMP", "6D suppport");
00687     // arm supports 6D ik. Perfect!
00688     std::vector<OpenRAVE::dReal> target_angles;
00689 
00690     __target.ikparam = IkParameterization(target);
00691     __target.solvable = __arm->FindIKSolution(__target.ikparam,target_angles,true);
00692     __target.manip->set_angles(target_angles);
00693 
00694   } else if( __arm->GetIkSolver()->Supports(IKP_TranslationDirection5D) ) {
00695     __logger->log_debug("OR TMP", "5D suppport");
00696     // arm has only 5 DOF.
00697     std::vector<OpenRAVE::dReal> target_angles;
00698 
00699     __target.ikparam = get_5dof_ikparam(target);
00700     __target.solvable = set_target_ikparam(__target.ikparam);
00701 
00702   } else {
00703     __logger->log_debug("OR TMP", "No IK suppport");
00704     //other IK types not supported yet
00705     __target.solvable = false;
00706   }
00707 
00708   return __target.solvable;
00709 }
00710 
00711 /** Set target, given 3 consecutive axis rotations.
00712  * Axis rotations are given as 1 vector representing a 3x3 matrix,
00713  * (left to right, top to bottom) where each row represents
00714  * one rotation over one axis (axis-angle notation).
00715  * See public setTargetEuler methods to get a better understanding.
00716  *
00717  * Check IK solvability for target Transform. If solvable,
00718  * then set target angles to manipulator configuration __target.manip
00719  * @param rotations 3x3 matrix given as one row.
00720  * @param no_offset if true, do not include manipulator offset (default: false)
00721  * @return true if solvable, false otherwise
00722  */
00723 bool
00724 OpenRaveRobot::set_target_euler(OpenRAVE::Vector& trans, std::vector<float>& rotations, bool no_offset)
00725 {
00726   if( rotations.size() != 9 ) {
00727     __target.type = TARGET_NONE;
00728     __target.solvable = false;
00729 
00730     if(__logger)
00731       {__logger->log_error("OpenRAVE Robot", "Bad size of rotations vector. Is %i, expected 9", rotations.size());}
00732     return false;
00733   }
00734 
00735   Vector r1(rotations.at(0), rotations.at(1), rotations.at(2));
00736   Vector r2(rotations.at(3), rotations.at(4), rotations.at(5));
00737   Vector r3(rotations.at(6), rotations.at(7), rotations.at(8));
00738 
00739   __logger->log_debug("TEST", "Rot1: %f %f %f", r1[0], r1[1], r1[2]);
00740   __logger->log_debug("TEST", "Rot2: %f %f %f", r2[0], r2[1], r2[2]);
00741   __logger->log_debug("TEST", "Rot3: %f %f %f", r3[0], r3[1], r3[2]);
00742 
00743   Vector q1 = quatFromAxisAngle(r1);
00744   Vector q2 = quatFromAxisAngle(r2);
00745   Vector q3 = quatFromAxisAngle(r3);
00746 
00747   Vector q12  = quatMultiply (q1, q2);
00748   Vector quat = quatMultiply (q12, q3);
00749 
00750   return set_target_transform(trans, quat, no_offset);
00751 }
00752 
00753 /** Get IkParameterization for a 5DOF arm given a 6D Transform.
00754  * @param trans The 6D OpenRAVE::Transform
00755  * @return the calculated 5DOF IkParameterization
00756  */
00757 OpenRAVE::IkParameterization
00758 OpenRaveRobot::get_5dof_ikparam(OpenRAVE::Transform& trans)
00759 {
00760   /* The initial pose (that means NOT all joints=0, but the manipulator's coordinate-system
00761      matching the world-coordinate-system) of an arm in OpenRAVE has its gripper pointing to the z-axis.
00762      Imagine a tube between the grippers. That tube lies on the y-axis.
00763      For 5DOF-IK one needs another manipulator definition, that has it's z-axis lying on that
00764      'tube', i.e. it needs to be lying between the fingers. That is achieved by rotating the
00765      coordinate-system first by +-90° around z-axis, then +90° on the rotated x-axis.
00766   */
00767 
00768   // get direction vector for TranslationDirection5D
00769   /* Rotate Vector(0, +-1, 0) by target.rot. First need to figure out which of "+-"
00770      Now if the first rotation on z-axis was +90°, we need a (0,-1,0) direction vector.
00771      If it was -90°, we need (0, 1, 0). So just take the inverse of the first rotation
00772      and apply it to (1,0,0)
00773   */
00774   EnvironmentMutex::scoped_lock lock(__robot->GetEnv()->GetMutex());
00775   Vector dir(1,0,0);
00776   {
00777     RobotBasePtr tmp_robot = __robot;
00778     RobotBase::RobotStateSaver saver(tmp_robot); // save the state, do not modifiy currently active robot!
00779 
00780     //reset robot joints
00781     std::vector<dReal> zero_joints(tmp_robot->GetActiveDOF(), (dReal)0.0);
00782     tmp_robot->SetActiveDOFValues(zero_joints);
00783 
00784     // revert the rotations for the 5DOF manipulator specifition. See long comment above.
00785     // First rotate back -90° on x-axis (revert 2nd rotation)
00786     Transform cur_pos = __arm->GetEndEffectorTransform();
00787     Vector v1 = quatFromAxisAngle(Vector(-M_PI/2, 0, 0));
00788     v1 = quatMultiply(cur_pos.rot, v1);
00789 
00790     // Now get the inverse of 1st rotation and get our (0, +-1, 0) direction
00791     v1 = quatInverse(v1);
00792     TransformMatrix mat = matrixFromQuat(v1);
00793     dir = mat.rotate(dir);
00794   }  // robot state is restored
00795 
00796   // now rotate direction by target
00797   TransformMatrix mat = matrixFromQuat(trans.rot);
00798   dir = mat.rotate(dir);
00799 
00800   IkParameterization ikparam = __arm->GetIkParameterization(IKP_TranslationDirection5D);
00801   ikparam.SetTranslationDirection5D(RAY(trans.trans, dir));
00802 
00803   return ikparam;
00804 }
00805 
00806 } // end of namespace fawkes