Fawkes API  Fawkes Development Version
openrave_thread.cpp
00001 
00002 /***************************************************************************
00003  *  openrave_thread.cpp - OpenRAVE Thread
00004  *
00005  *  Created: Fri Feb 25 15:08:00 2011
00006  *  Copyright  2011  Bahram Maleki-Fard
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 "openrave_thread.h"
00024 
00025 #include "environment.h"
00026 #include "robot.h"
00027 #include "manipulator.h"
00028 
00029 #include <openrave-core.h>
00030 #include <core/exceptions/software.h>
00031 
00032 using namespace fawkes;
00033 
00034 /** @class OpenRaveThread "openrave_thread.h"
00035  * OpenRAVE Thread.
00036  * This thread maintains an active connection to OpenRAVE and provides an
00037  * aspect to access OpenRAVE to make it convenient for other threads to use
00038  * OpenRAVE.
00039  *
00040  * @author Bahram Maleki-Fard
00041  */
00042 
00043 /** Constructor. */
00044 OpenRaveThread::OpenRaveThread()
00045   : Thread("OpenRaveThread", Thread::OPMODE_WAITFORWAKEUP),
00046     BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_ACT),
00047     AspectProviderAspect("OpenRaveAspect", &__or_aspectIniFin),
00048   __or_aspectIniFin( this ),
00049   __OR_env( 0 ),
00050   __OR_robot( 0 )
00051 
00052 {
00053 }
00054 
00055 
00056 /** Destructor. */
00057 OpenRaveThread::~OpenRaveThread()
00058 {
00059 }
00060 
00061 
00062 void
00063 OpenRaveThread::init()
00064 {
00065   try {
00066     OpenRAVE::RaveInitialize(true);
00067   } catch(const OpenRAVE::openrave_exception &e) {
00068     logger->log_error(name(), "Could not initialize OpenRAVE. Ex:%s", e.what());
00069   }
00070 
00071   __OR_env = new OpenRaveEnvironment(logger);
00072 
00073   __OR_env->create();
00074   __OR_env->enable_debug(); // TODO: cfg
00075 
00076   __OR_env->lock();
00077 }
00078 
00079 
00080 void
00081 OpenRaveThread::finalize()
00082 {
00083   __OR_env->destroy();
00084 }
00085 
00086 
00087 void
00088 OpenRaveThread::loop()
00089 {
00090 }
00091 
00092 /* ##########################################################
00093  * #    methods form OpenRaveConnector (openrave_connector.h)     #
00094  * ########################################################*/
00095 /** Get pointer to OpenRaveEnvironment object.
00096  * @return pointer
00097  */
00098 OpenRaveEnvironment*
00099 OpenRaveThread::get_environment() const
00100 {
00101   return __OR_env;
00102 }
00103 
00104 /** Get pointer to currently used OpenRaveRobot object.
00105  * @return pointer
00106  */
00107 OpenRaveRobot*
00108 OpenRaveThread::get_active_robot() const
00109 {
00110   return __OR_robot;
00111 }
00112 
00113 /** Set robot to be used
00114  * @param robot OpenRaveRobot that should be used implicitly in other methods
00115  */
00116 void
00117 OpenRaveThread::set_active_robot(OpenRaveRobot* robot)
00118 {
00119   __OR_robot = robot;
00120 }
00121 
00122 /** Set OpenRaveManipulator object for robot, and calculate
00123  * coordinate-system offsets or set them directly.
00124  * Make sure to update manip angles before calibrating!
00125  * @param robot pointer to OpenRaveRobot object, explicitly set
00126  * @param manip pointer to OpenRAVManipulator that is set for robot
00127  * @param trans_x transition offset on x-axis
00128  * @param trans_y transition offset on y-axis
00129  * @param trans_z transition offset on z-axis
00130  * @param calibrate decides whether to calculate offset (true )or set them directly (false; default)
00131  */
00132 void
00133 OpenRaveThread::set_manipulator(OpenRaveRobot* robot, OpenRaveManipulator* manip, float trans_x, float trans_y, float trans_z, bool calibrate)
00134 {
00135   robot->set_manipulator(manip);
00136   if( calibrate )
00137     {robot->calibrate(trans_x, trans_y, trans_z);}
00138   else
00139     {robot->set_offset(trans_x, trans_y, trans_z);}
00140 }
00141 /** Set OpenRaveManipulator object for robot, and calculate
00142  * coordinate-system offsets or set them directly.
00143  * Make sure to update manip angles before calibrating!
00144  * Uses default OpenRaveRobot object.
00145  * @param manip pointer to OpenRAVManipulator that is set for robot
00146  * @param trans_x transition offset on x-axis
00147  * @param trans_y transition offset on y-axis
00148  * @param trans_z transition offset on z-axis
00149  * @param calibrate decides whether to calculate offset (true )or set them directly (false; default)
00150  */
00151 void
00152 OpenRaveThread::set_manipulator(OpenRaveManipulator* manip, float trans_x, float trans_y, float trans_z, bool calibrate)
00153 {
00154   set_manipulator(__OR_robot, manip, trans_x, trans_y, trans_z, calibrate);
00155 }
00156 
00157 
00158 /** Start Viewer */
00159 void
00160 OpenRaveThread::start_viewer() const
00161 {
00162   __OR_env->start_viewer();
00163 }
00164 
00165 /** Run planner on previously set target.
00166  * @param robot robot to use planner on. If none is given, the currently used robot is taken
00167  * @param sampling sampling time between each trajectory point (in seconds)
00168  */
00169 void
00170 OpenRaveThread::run_planner(OpenRaveRobot* robot, float sampling)
00171 {
00172   if(!robot)
00173     robot = __OR_robot;
00174 
00175   __OR_env->run_planner(robot, sampling);
00176 }
00177 
00178 /** Run graspplanning script for a given target.
00179  * @param target_name name of targeted object (KinBody)
00180  * @param robot robot to use planner on. If none is given, the currently used robot is taken
00181  */
00182 void
00183 OpenRaveThread::run_graspplanning(const std::string& target_name, OpenRaveRobot* robot)
00184 {
00185   if(!robot)
00186     robot = __OR_robot;
00187 
00188   __OR_env->run_graspplanning(target_name, robot);
00189 }
00190 
00191 /** Add a new robot to the environment, and set it as the currently active one.
00192  * @param filename_robot path to robot's xml file
00193  * @param autogenerate_IK if true: autogenerate IKfast IK solver for robot
00194  * @return pointer to new OpenRaveRobot object
00195  */
00196 OpenRaveRobot*
00197 OpenRaveThread::add_robot(const std::string& filename_robot, bool autogenerate_IK)
00198 {
00199   OpenRaveRobot* robot = new OpenRaveRobot(logger);
00200   robot->load(filename_robot, __OR_env);
00201   __OR_env->add_robot(robot);
00202   robot->set_ready();
00203 
00204   if( autogenerate_IK )
00205     __OR_env->load_IK_solver(robot);
00206 
00207   set_active_robot(robot);
00208 
00209   return robot;
00210 }
00211 
00212 /* ##########################################################
00213  * #    object handling (mainly from environment.h)        #
00214 * ########################################################*/
00215 /** Add an object to the environment.
00216  * @param name name that should be given to that object
00217  * @param filename path to xml file of that object (KinBody)
00218  * @return true if successful */
00219 bool OpenRaveThread::add_object(const std::string& name, const std::string& filename) {
00220   return __OR_env->add_object(name, filename); }
00221 
00222 /** Remove object from environment.
00223  * @param name name of the object
00224  * @return true if successful */
00225 bool OpenRaveThread::delete_object(const std::string& name) {
00226   return __OR_env->delete_object(name); }
00227 
00228 /** Rename object.
00229  * @param name current name of the object
00230  * @param new_name new name of the object
00231  * @return true if successful */
00232 bool OpenRaveThread::rename_object(const std::string& name, const std::string& new_name) {
00233   return __OR_env->rename_object(name, new_name); }
00234 
00235 /** Move object in the environment.
00236  * Distances are given in meters
00237  * @param name name of the object
00238  * @param trans_x transition along x-axis
00239  * @param trans_y transition along y-axis
00240  * @param trans_z transition along z-axis
00241  * @param robot if given, move relatively to robot (in most simple cases robot is at position (0,0,0) anyway, so this has no effect)
00242  * @return true if successful */
00243 bool OpenRaveThread::move_object(const std::string& name, float trans_x, float trans_y, float trans_z, OpenRaveRobot* robot) {
00244   return __OR_env->move_object(name, trans_x, trans_y, trans_z, robot); }
00245 
00246 /** Rotate object by a quaternion.
00247  * @param name name of the object
00248  * @param quat_x x value of quaternion
00249  * @param quat_y y value of quaternion
00250  * @param quat_z z value of quaternion
00251  * @param quat_w w value of quaternion
00252  * @return true if successful */
00253 bool OpenRaveThread::rotate_object(const std::string& name, float quat_x, float quat_y, float quat_z, float quat_w) {
00254   return __OR_env->rotate_object(name, quat_x, quat_y, quat_z, quat_w); }
00255 
00256 /** Rotate object along its axis.
00257  * Rotation angles should be given in radians.
00258  * @param name name of the object
00259  * @param rot_x 1st rotation, along x-axis
00260  * @param rot_y 2nd rotation, along y-axis
00261  * @param rot_z 3rd rotation, along z-axis
00262  * @return true if successful */
00263 bool OpenRaveThread::rotate_object(const std::string& name, float rot_x, float rot_y, float rot_z) {
00264   return __OR_env->rotate_object(name, rot_x, rot_y, rot_z); }
00265 
00266 /** Set an object as the target.
00267  * Currently the object should be cylindric, and stand upright. It may
00268  * also be rotated on its x-axis, but that rotation needs to be given in an argument
00269  * to calculate correct position for endeffecto. This is only temporary until
00270  * proper graps planning for 5DOF in OpenRAVE is provided.
00271  * @param name name of the object
00272  * @param robot pointer to OpenRaveRobot that the target is set for
00273  * @param rot_x rotation of object on x-axis (radians)
00274  * @return true if IK solvable
00275  */
00276 bool
00277 OpenRaveThread::set_target_object(const std::string& name, OpenRaveRobot* robot, float rot_x)
00278 {
00279   OpenRAVE::Transform transform = __OR_env->get_env_ptr()->GetKinBody(name)->GetTransform();
00280 
00281   return robot->set_target_object_position(transform.trans[0], transform.trans[1], transform.trans[2], rot_x);
00282 }
00283 
00284 /** Attach a kinbody to the robot.
00285  * @param name name of the object
00286  * @param robot pointer to OpenRaveRobot that object is attached to
00287  * @return true if successfull
00288  */
00289 bool
00290 OpenRaveThread::attach_object(const std::string& name, OpenRaveRobot* robot)
00291 {
00292   if( !robot )
00293     { robot = __OR_robot; }
00294 
00295   return robot->attach_object(name, __OR_env);
00296 }
00297 
00298 /** Release a kinbody from the robot.
00299  * @param name name of the object
00300  * @param robot pointer to OpenRaveRobot that object is released from
00301  * @return true if successfull
00302  */
00303 bool
00304 OpenRaveThread::release_object(const std::string& name, OpenRaveRobot* robot)
00305 {
00306   if( !robot )
00307     { robot = __OR_robot; }
00308 
00309   return robot->release_object(name, __OR_env);
00310 }
00311 
00312 /** Release all grabbed kinbodys from the robot.
00313  * @param robot pointer to OpenRaveRobot that objects are released from
00314  * @return true if successfull
00315  */
00316 bool
00317 OpenRaveThread::release_all_objects(OpenRaveRobot* robot)
00318 {
00319   if( !robot )
00320     { robot = __OR_robot; }
00321 
00322   return robot->release_all_objects();
00323 }