Fawkes API
Fawkes Development Version
|
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 }