Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
openrave_thread.cpp
1 
2 /***************************************************************************
3  * openrave_thread.cpp - OpenRAVE Thread
4  *
5  * Created: Fri Feb 25 15:08:00 2011
6  * Copyright 2011 Bahram Maleki-Fard
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 "openrave_thread.h"
24 
25 #include "environment.h"
26 #include "robot.h"
27 #include "manipulator.h"
28 
29 #include <openrave-core.h>
30 #include <core/exceptions/software.h>
31 
32 using namespace fawkes;
33 
34 /** @class OpenRaveThread "openrave_thread.h"
35  * OpenRAVE Thread.
36  * This thread maintains an active connection to OpenRAVE and provides an
37  * aspect to access OpenRAVE to make it convenient for other threads to use
38  * OpenRAVE.
39  *
40  * @author Bahram Maleki-Fard
41  */
42 
43 /** Constructor. */
45  : Thread("OpenRaveThread", Thread::OPMODE_WAITFORWAKEUP),
46  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_ACT),
47  AspectProviderAspect("OpenRaveAspect", &__or_aspectIniFin),
48  __or_aspectIniFin( this ),
49  __OR_env( 0 ),
50  __OR_robot( 0 )
51 
52 {
53 }
54 
55 
56 /** Destructor. */
58 {
59 }
60 
61 
62 void
64 {
65  try {
66  OpenRAVE::RaveInitialize(true);
67  } catch(const OpenRAVE::openrave_exception &e) {
68  logger->log_error(name(), "Could not initialize OpenRAVE. Ex:%s", e.what());
69  }
70 
71  __OR_env = new OpenRaveEnvironment(logger);
72 
73  __OR_env->create();
74  __OR_env->enable_debug(); // TODO: cfg
75 
76  __OR_env->lock();
77 }
78 
79 
80 void
82 {
83  __OR_env->destroy();
84 }
85 
86 
87 void
89 {
90 }
91 
92 /* ##########################################################
93  * # methods form OpenRaveConnector (openrave_connector.h) #
94  * ########################################################*/
95 /** Get pointer to OpenRaveEnvironment object.
96  * @return pointer
97  */
100 {
101  return __OR_env;
102 }
103 
104 /** Get pointer to currently used OpenRaveRobot object.
105  * @return pointer
106  */
109 {
110  return __OR_robot;
111 }
112 
113 /** Set robot to be used
114  * @param robot OpenRaveRobot that should be used implicitly in other methods
115  */
116 void
118 {
119  __OR_robot = robot;
120 }
121 
122 /** Set OpenRaveManipulator object for robot, and calculate
123  * coordinate-system offsets or set them directly.
124  * Make sure to update manip angles before calibrating!
125  * @param robot pointer to OpenRaveRobot object, explicitly set
126  * @param manip pointer to OpenRAVManipulator that is set for robot
127  * @param trans_x transition offset on x-axis
128  * @param trans_y transition offset on y-axis
129  * @param trans_z transition offset on z-axis
130  * @param calibrate decides whether to calculate offset (true )or set them directly (false; default)
131  */
132 void
133 OpenRaveThread::set_manipulator(OpenRaveRobot* robot, OpenRaveManipulator* manip, float trans_x, float trans_y, float trans_z, bool calibrate)
134 {
135  robot->set_manipulator(manip);
136  if( calibrate )
137  {robot->calibrate(trans_x, trans_y, trans_z);}
138  else
139  {robot->set_offset(trans_x, trans_y, trans_z);}
140 }
141 /** Set OpenRaveManipulator object for robot, and calculate
142  * coordinate-system offsets or set them directly.
143  * Make sure to update manip angles before calibrating!
144  * Uses default OpenRaveRobot object.
145  * @param manip pointer to OpenRAVManipulator that is set for robot
146  * @param trans_x transition offset on x-axis
147  * @param trans_y transition offset on y-axis
148  * @param trans_z transition offset on z-axis
149  * @param calibrate decides whether to calculate offset (true )or set them directly (false; default)
150  */
151 void
152 OpenRaveThread::set_manipulator(OpenRaveManipulator* manip, float trans_x, float trans_y, float trans_z, bool calibrate)
153 {
154  set_manipulator(__OR_robot, manip, trans_x, trans_y, trans_z, calibrate);
155 }
156 
157 
158 /** Start Viewer */
159 void
161 {
162  __OR_env->start_viewer();
163 }
164 
165 /** Run planner on previously set target.
166  * @param robot robot to use planner on. If none is given, the currently used robot is taken
167  * @param sampling sampling time between each trajectory point (in seconds)
168  */
169 void
171 {
172  if(!robot)
173  robot = __OR_robot;
174 
175  __OR_env->run_planner(robot, sampling);
176 }
177 
178 /** Run graspplanning script for a given target.
179  * @param target_name name of targeted object (KinBody)
180  * @param robot robot to use planner on. If none is given, the currently used robot is taken
181  */
182 void
183 OpenRaveThread::run_graspplanning(const std::string& target_name, OpenRaveRobot* robot)
184 {
185  if(!robot)
186  robot = __OR_robot;
187 
188  __OR_env->run_graspplanning(target_name, robot);
189 }
190 
191 /** Add a new robot to the environment, and set it as the currently active one.
192  * @param filename_robot path to robot's xml file
193  * @param autogenerate_IK if true: autogenerate IKfast IK solver for robot
194  * @return pointer to new OpenRaveRobot object
195  */
197 OpenRaveThread::add_robot(const std::string& filename_robot, bool autogenerate_IK)
198 {
199  OpenRaveRobot* robot = new OpenRaveRobot(logger);
200  robot->load(filename_robot, __OR_env);
201  __OR_env->add_robot(robot);
202  robot->set_ready();
203 
204  if( autogenerate_IK )
205  __OR_env->load_IK_solver(robot);
206 
207  set_active_robot(robot);
208 
209  return robot;
210 }
211 
212 /* ##########################################################
213  * # object handling (mainly from environment.h) #
214 * ########################################################*/
215 /** Add an object to the environment.
216  * @param name name that should be given to that object
217  * @param filename path to xml file of that object (KinBody)
218  * @return true if successful */
219 bool OpenRaveThread::add_object(const std::string& name, const std::string& filename) {
220  return __OR_env->add_object(name, filename); }
221 
222 /** Remove object from environment.
223  * @param name name of the object
224  * @return true if successful */
225 bool OpenRaveThread::delete_object(const std::string& name) {
226  return __OR_env->delete_object(name); }
227 
228 /** Rename object.
229  * @param name current name of the object
230  * @param new_name new name of the object
231  * @return true if successful */
232 bool OpenRaveThread::rename_object(const std::string& name, const std::string& new_name) {
233  return __OR_env->rename_object(name, new_name); }
234 
235 /** Move object in the environment.
236  * Distances are given in meters
237  * @param name name of the object
238  * @param trans_x transition along x-axis
239  * @param trans_y transition along y-axis
240  * @param trans_z transition along z-axis
241  * @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)
242  * @return true if successful */
243 bool OpenRaveThread::move_object(const std::string& name, float trans_x, float trans_y, float trans_z, OpenRaveRobot* robot) {
244  return __OR_env->move_object(name, trans_x, trans_y, trans_z, robot); }
245 
246 /** Rotate object by a quaternion.
247  * @param name name of the object
248  * @param quat_x x value of quaternion
249  * @param quat_y y value of quaternion
250  * @param quat_z z value of quaternion
251  * @param quat_w w value of quaternion
252  * @return true if successful */
253 bool OpenRaveThread::rotate_object(const std::string& name, float quat_x, float quat_y, float quat_z, float quat_w) {
254  return __OR_env->rotate_object(name, quat_x, quat_y, quat_z, quat_w); }
255 
256 /** Rotate object along its axis.
257  * Rotation angles should be given in radians.
258  * @param name name of the object
259  * @param rot_x 1st rotation, along x-axis
260  * @param rot_y 2nd rotation, along y-axis
261  * @param rot_z 3rd rotation, along z-axis
262  * @return true if successful */
263 bool OpenRaveThread::rotate_object(const std::string& name, float rot_x, float rot_y, float rot_z) {
264  return __OR_env->rotate_object(name, rot_x, rot_y, rot_z); }
265 
266 /** Set an object as the target.
267  * Currently the object should be cylindric, and stand upright. It may
268  * also be rotated on its x-axis, but that rotation needs to be given in an argument
269  * to calculate correct position for endeffecto. This is only temporary until
270  * proper graps planning for 5DOF in OpenRAVE is provided.
271  * @param name name of the object
272  * @param robot pointer to OpenRaveRobot that the target is set for
273  * @param rot_x rotation of object on x-axis (radians)
274  * @return true if IK solvable
275  */
276 bool
277 OpenRaveThread::set_target_object(const std::string& name, OpenRaveRobot* robot, float rot_x)
278 {
279  OpenRAVE::Transform transform = __OR_env->get_env_ptr()->GetKinBody(name)->GetTransform();
280 
281  return robot->set_target_object_position(transform.trans[0], transform.trans[1], transform.trans[2], rot_x);
282 }
283 
284 /** Attach a kinbody to the robot.
285  * @param name name of the object
286  * @param robot pointer to OpenRaveRobot that object is attached to
287  * @return true if successfull
288  */
289 bool
290 OpenRaveThread::attach_object(const std::string& name, OpenRaveRobot* robot)
291 {
292  if( !robot )
293  { robot = __OR_robot; }
294 
295  return robot->attach_object(name, __OR_env);
296 }
297 
298 /** Release a kinbody from the robot.
299  * @param name name of the object
300  * @param robot pointer to OpenRaveRobot that object is released from
301  * @return true if successfull
302  */
303 bool
304 OpenRaveThread::release_object(const std::string& name, OpenRaveRobot* robot)
305 {
306  if( !robot )
307  { robot = __OR_robot; }
308 
309  return robot->release_object(name, __OR_env);
310 }
311 
312 /** Release all grabbed kinbodys from the robot.
313  * @param robot pointer to OpenRaveRobot that objects are released from
314  * @return true if successfull
315  */
316 bool
318 {
319  if( !robot )
320  { robot = __OR_robot; }
321 
322  return robot->release_all_objects();
323 }