Fawkes API  Fawkes Development Version
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 "manipulator.h"
27 #include "robot.h"
28 
29 #include <core/exceptions/software.h>
30 
31 #include <openrave-core.h>
32 
33 using namespace fawkes;
34 
35 /** @class OpenRaveThread "openrave_thread.h"
36  * OpenRAVE Thread.
37  * This thread maintains an active connection to OpenRAVE and provides an
38  * aspect to access OpenRAVE to make it convenient for other threads to use
39  * OpenRAVE.
40  *
41  * @author Bahram Maleki-Fard
42  */
43 
44 /** Constructor. */
46 : Thread("OpenRaveThread", Thread::OPMODE_WAITFORWAKEUP),
47  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_ACT),
48  AspectProviderAspect(&or_aspectIniFin_),
49  or_aspectIniFin_(this),
50  OR_env_(0),
51  OR_robot_(0)
52 
53 {
54 }
55 
56 /** Destructor. */
58 {
59 }
60 
61 void
63 {
64  try {
65  OpenRAVE::RaveInitialize(true);
66  } catch (const OpenRAVE::openrave_exception &e) {
67  logger->log_error(name(), "Could not initialize OpenRAVE. Ex:%s", e.what());
68  }
69 
70  OR_env_ = new OpenRaveEnvironment(logger);
71 
72  OR_env_->create();
73  OR_env_->enable_debug(); // TODO: cfg
74 
75  OR_env_->set_name("viewer");
76 }
77 
78 void
80 {
81  OR_env_->destroy();
82 }
83 
84 void
86 {
87 }
88 
89 /* ##########################################################
90  * # methods form OpenRaveConnector (openrave_connector.h) #
91  * ########################################################*/
92 /** Clone basically everything
93  * We pass pointers to pointer as parameters, so the pointers we create before calling this clone()
94  * method will point to the new objects.
95  * @param env Pointer to pointer of the copied environment
96  * @param robot Pointer to pointer of the copied robot
97  * @param manip Pointer to pointer of the copied manipulator
98  */
99 void
101  OpenRaveRobotPtr & robot,
102  OpenRaveManipulatorPtr &manip) const
103 {
104  env = new OpenRaveEnvironment(**OR_env_);
105  robot = new OpenRaveRobot(**OR_robot_, env);
106  manip = robot->get_manipulator(); // cloned by OpenRaveRobot copy-ctor
107 
108  env->load_IK_solver(robot);
109 }
110 
111 /** Get pointer to OpenRaveEnvironment object.
112  * @return pointer
113  */
116 {
117  return OR_env_;
118 }
119 
120 /** Get RefPtr to currently used OpenRaveRobot object.
121  * @return RefPtr
122  */
125 {
126  return OR_robot_;
127 }
128 
129 /** Set robot to be used
130  * @param robot OpenRaveRobot that should be used implicitly in other methods
131  */
132 void
134 {
135  OR_robot_ = robot;
136 }
137 
138 /** Set robot to be used
139  * @param robot OpenRaveRobot that should be used implicitly in other methods
140  */
141 void
143 {
144  OR_robot_ = robot;
145 }
146 
147 /** Set OpenRaveManipulator object for robot, and calculate
148  * coordinate-system offsets or set them directly.
149  * Make sure to update manip angles before calibrating!
150  * @param robot pointer to OpenRaveRobot object, explicitly set
151  * @param manip pointer to OpenRAVManipulator that is set for robot
152  * @param trans_x transition offset on x-axis
153  * @param trans_y transition offset on y-axis
154  * @param trans_z transition offset on z-axis
155  * @param calibrate decides whether to calculate offset (true )or set them directly (false; default)
156  */
157 void
159  OpenRaveManipulatorPtr &manip,
160  float trans_x,
161  float trans_y,
162  float trans_z,
163  bool calibrate)
164 {
165  robot->set_manipulator(manip);
166  if (calibrate) {
167  robot->calibrate(trans_x, trans_y, trans_z);
168  } else {
169  robot->set_offset(trans_x, trans_y, trans_z);
170  }
171 }
172 /** Set OpenRaveManipulator object for robot, and calculate
173  * coordinate-system offsets or set them directly.
174  * Make sure to update manip angles before calibrating!
175  * Uses default OpenRaveRobot object.
176  * @param manip pointer to OpenRAVManipulator that is set for robot
177  * @param trans_x transition offset on x-axis
178  * @param trans_y transition offset on y-axis
179  * @param trans_z transition offset on z-axis
180  * @param calibrate decides whether to calculate offset (true )or set them directly (false; default)
181  */
182 void
184  float trans_x,
185  float trans_y,
186  float trans_z,
187  bool calibrate)
188 {
189  set_manipulator(OR_robot_, manip, trans_x, trans_y, trans_z, calibrate);
190 }
191 
192 /** Start Viewer */
193 void
195 {
196  OR_env_->start_viewer();
197 }
198 
199 /** Run planner on previously set target.
200  * @param robot robot to use planner on. If none is given, the currently used robot is taken
201  * @param sampling sampling time between each trajectory point (in seconds)
202  */
203 void
205 {
206  OR_env_->run_planner(robot, sampling);
207 }
208 
209 /** Run planner on previously set target. Uses currently active robot.
210  * @param sampling sampling time between each trajectory point (in seconds)
211  */
212 void
214 {
215  run_planner(OR_robot_, sampling);
216 }
217 
218 /** Run graspplanning script for a given target.
219  * @param target_name name of targeted object (KinBody)
220  * @param robot robot to use planner on. If none is given, the currently used robot is taken
221  */
222 void
223 OpenRaveThread::run_graspplanning(const std::string &target_name, OpenRaveRobotPtr &robot)
224 {
225  OR_env_->run_graspplanning(target_name, robot);
226 }
227 
228 /** Run graspplanning script for a given target. Uses currently active robot.
229  * @param target_name name of targeted object (KinBody)
230  */
231 void
232 OpenRaveThread::run_graspplanning(const std::string &target_name)
233 {
234  run_graspplanning(target_name, OR_robot_);
235 }
236 
237 /** Add a new robot to the environment, and set it as the currently active one.
238  * @param filename_robot path to robot's xml file
239  * @param autogenerate_IK if true: autogenerate IKfast IK solver for robot
240  * @return pointer to new OpenRaveRobot object
241  */
243 OpenRaveThread::add_robot(const std::string &filename_robot, bool autogenerate_IK)
244 {
246  robot->load(filename_robot, OR_env_);
247  OR_env_->add_robot(robot);
248  robot->set_ready();
249 
250  if (autogenerate_IK)
251  OR_env_->load_IK_solver(robot);
252 
253  set_active_robot(robot);
254 
255  return robot;
256 }
257 
258 /* ##########################################################
259  * # object handling (mainly from environment.h) #
260 * ########################################################*/
261 /** Add an object to the environment.
262  * @param name name that should be given to that object
263  * @param filename path to xml file of that object (KinBody)
264  * @return true if successful */
265 bool
266 OpenRaveThread::add_object(const std::string &name, const std::string &filename)
267 {
268  return OR_env_->add_object(name, filename);
269 }
270 
271 /** Remove object from environment.
272  * @param name name of the object
273  * @return true if successful */
274 bool
275 OpenRaveThread::delete_object(const std::string &name)
276 {
277  return OR_env_->delete_object(name);
278 }
279 
280 /** Remove all objects from environment.
281  * @return true if successful */
282 bool
284 {
285  return OR_env_->delete_all_objects();
286 }
287 
288 /** Rename object.
289  * @param name current name of the object
290  * @param new_name new name of the object
291  * @return true if successful */
292 bool
293 OpenRaveThread::rename_object(const std::string &name, const std::string &new_name)
294 {
295  return OR_env_->rename_object(name, new_name);
296 }
297 
298 /** Move object in the environment.
299  * Distances are given in meters
300  * @param name name of the object
301  * @param trans_x transition along x-axis
302  * @param trans_y transition along y-axis
303  * @param trans_z transition along z-axis
304  * @return true if successful */
305 bool
306 OpenRaveThread::move_object(const std::string &name, float trans_x, float trans_y, float trans_z)
307 {
308  return OR_env_->move_object(name, trans_x, trans_y, trans_z);
309 }
310 
311 /** Move object in the environment, relatively to robot.
312  * Distances are given in meters
313  * @param name name of the object
314  * @param trans_x transition along x-axis
315  * @param trans_y transition along y-axis
316  * @param trans_z transition along z-axis
317  * @param robot move relatively to robot (in most simple cases robot is at position (0,0,0) anyway, so this has no effect)
318  * @return true if successful */
319 bool
320 OpenRaveThread::move_object(const std::string &name,
321  float trans_x,
322  float trans_y,
323  float trans_z,
324  OpenRaveRobotPtr & robot)
325 {
326  return OR_env_->move_object(name, trans_x, trans_y, trans_z, robot);
327 }
328 
329 /** Rotate object by a quaternion.
330  * @param name name of the object
331  * @param quat_x x value of quaternion
332  * @param quat_y y value of quaternion
333  * @param quat_z z value of quaternion
334  * @param quat_w w value of quaternion
335  * @return true if successful */
336 bool
337 OpenRaveThread::rotate_object(const std::string &name,
338  float quat_x,
339  float quat_y,
340  float quat_z,
341  float quat_w)
342 {
343  return OR_env_->rotate_object(name, quat_x, quat_y, quat_z, quat_w);
344 }
345 
346 /** Rotate object along its axis.
347  * Rotation angles should be given in radians.
348  * @param name name of the object
349  * @param rot_x 1st rotation, along x-axis
350  * @param rot_y 2nd rotation, along y-axis
351  * @param rot_z 3rd rotation, along z-axis
352  * @return true if successful */
353 bool
354 OpenRaveThread::rotate_object(const std::string &name, float rot_x, float rot_y, float rot_z)
355 {
356  return OR_env_->rotate_object(name, rot_x, rot_y, rot_z);
357 }
358 
359 /** Set an object as the target.
360  * Currently the object should be cylindric, and stand upright. It may
361  * also be rotated on its x-axis, but that rotation needs to be given in an argument
362  * to calculate correct position for endeffecto. This is only temporary until
363  * proper graps planning for 5DOF in OpenRAVE is provided.
364  * @param name name of the object
365  * @param robot pointer to OpenRaveRobot that the target is set for
366  * @param rot_x rotation of object on x-axis (radians)
367  * @return true if IK solvable
368  */
369 bool
370 OpenRaveThread::set_target_object(const std::string &name, OpenRaveRobotPtr &robot, float rot_x)
371 {
372  OpenRAVE::Transform transform = OR_env_->get_env_ptr()->GetKinBody(name)->GetTransform();
373 
374  return robot->set_target_object_position(transform.trans[0],
375  transform.trans[1],
376  transform.trans[2],
377  rot_x);
378 }
379 
380 /** Attach a kinbody to the robot.
381  * @param name name of the object
382  * @param robot pointer to OpenRaveRobot that object is attached to
383  * @param manip_name name of the manipulator to attach the object to
384  * @return true if successfull
385  */
386 bool
387 OpenRaveThread::attach_object(const char *name, OpenRaveRobotPtr &robot, const char *manip_name)
388 {
389  return robot->attach_object(name, OR_env_, manip_name);
390 }
391 
392 /** Attach a kinbody to the robot. Uses currently active robot.
393  * @param name name of the object
394  * @param manip_name name of the manipulator to attach the object to
395  * @return true if successfull
396  */
397 bool
398 OpenRaveThread::attach_object(const char *name, const char *manip_name)
399 {
400  return attach_object(name, OR_robot_, manip_name);
401 }
402 
403 /** Release a kinbody from the robot.
404  * @param name name of the object
405  * @param robot pointer to OpenRaveRobot that object is released from
406  * @return true if successfull
407  */
408 bool
409 OpenRaveThread::release_object(const std::string &name, OpenRaveRobotPtr &robot)
410 {
411  return robot->release_object(name, OR_env_);
412 }
413 
414 /** Release a kinbody from the robot. Uses currently active robot.
415  * @param name name of the object
416  * @return true if successfull
417  */
418 bool
419 OpenRaveThread::release_object(const std::string &name)
420 {
421  return release_object(name, OR_robot_);
422 }
423 
424 /** Release all grabbed kinbodys from the robot.
425  * @param robot pointer to OpenRaveRobot that objects are released from
426  * @return true if successfull
427  */
428 bool
430 {
431  return robot->release_all_objects();
432 }
433 
434 /** Release all grabbed kinbodys from the robot. Uses currently active robot.
435  * @return true if successfull
436  */
437 bool
439 {
440  return release_all_objects(OR_robot_);
441 }
OpenRaveThread::set_target_object
virtual bool set_target_object(const std::string &name, fawkes::OpenRaveRobotPtr &robot, float rot_x=0)
Set an object as the target.
Definition: openrave_thread.cpp:370
OpenRaveThread::delete_all_objects
virtual bool delete_all_objects()
Remove all objects from environment.
Definition: openrave_thread.cpp:283
OpenRaveThread::run_planner
virtual void run_planner(fawkes::OpenRaveRobotPtr &robot, float sampling=0.01f)
Run planner on previously set target.
Definition: openrave_thread.cpp:204
fawkes::RefPtr
RefPtr<> is a reference-counting shared smartpointer.
Definition: refptr.h:50
fawkes::OpenRaveEnvironment
OpenRaveEnvironment class.
Definition: environment.h:44
OpenRaveThread::attach_object
virtual bool attach_object(const char *name, fawkes::OpenRaveRobotPtr &robot, const char *manip_name=NULL)
Attach a kinbody to the robot.
Definition: openrave_thread.cpp:387
fawkes::AspectProviderAspect
Thread aspect provide a new aspect.
Definition: aspect_provider.h:36
fawkes::BlockedTimingAspect
Thread aspect to use blocked timing.
Definition: blocked_timing.h:51
fawkes::Thread::name
const char * name() const
Get name of thread.
Definition: thread.h:100
OpenRaveThread::get_environment
virtual fawkes::OpenRaveEnvironmentPtr get_environment() const
Get pointer to OpenRaveEnvironment object.
Definition: openrave_thread.cpp:115
fawkes::OpenRaveRobot
OpenRAVE Robot class.
Definition: robot.h:38
OpenRaveThread::release_all_objects
virtual bool release_all_objects()
Release all grabbed kinbodys from the robot.
Definition: openrave_thread.cpp:438
OpenRaveThread::set_manipulator
virtual void set_manipulator(fawkes::OpenRaveManipulatorPtr &manip, float trans_x=0.f, float trans_y=0.f, float trans_z=0.f, bool calibrate=0)
Set OpenRaveManipulator object for robot, and calculate coordinate-system offsets or set them directl...
Definition: openrave_thread.cpp:183
OpenRaveThread::add_robot
virtual fawkes::OpenRaveRobotPtr add_robot(const std::string &filename_robot, bool autogenerate_IK)
Add a new robot to the environment, and set it as the currently active one.
Definition: openrave_thread.cpp:243
OpenRaveThread::move_object
virtual bool move_object(const std::string &name, float trans_x, float trans_y, float trans_z, fawkes::OpenRaveRobotPtr &robot)
Move object in the environment, relatively to robot.
Definition: openrave_thread.cpp:320
fawkes::LoggingAspect::logger
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
fawkes::Logger::log_error
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
fawkes
Fawkes library namespace.
OpenRaveThread::set_active_robot
virtual void set_active_robot(fawkes::OpenRaveRobotPtr robot)
Set robot to be used.
Definition: openrave_thread.cpp:133
OpenRaveThread::OpenRaveThread
OpenRaveThread()
Constructor.
Definition: openrave_thread.cpp:45
OpenRaveThread::rotate_object
virtual bool rotate_object(const std::string &name, float quat_x, float quat_y, float quat_z, float quat_w)
Rotate object by a quaternion.
Definition: openrave_thread.cpp:337
OpenRaveThread::run_graspplanning
virtual void run_graspplanning(const std::string &target_name, fawkes::OpenRaveRobotPtr &robot)
Run graspplanning script for a given target.
Definition: openrave_thread.cpp:223
OpenRaveThread::rename_object
virtual bool rename_object(const std::string &name, const std::string &new_name)
Rename object.
Definition: openrave_thread.cpp:293
fawkes::Thread
Thread class encapsulation of pthreads.
Definition: thread.h:46
OpenRaveThread::~OpenRaveThread
virtual ~OpenRaveThread()
Destructor.
Definition: openrave_thread.cpp:57
OpenRaveThread::finalize
virtual void finalize()
Finalize the thread.
Definition: openrave_thread.cpp:79
OpenRaveThread::get_active_robot
virtual fawkes::OpenRaveRobotPtr get_active_robot() const
Get RefPtr to currently used OpenRaveRobot object.
Definition: openrave_thread.cpp:124
OpenRaveThread::init
virtual void init()
Initialize the thread.
Definition: openrave_thread.cpp:62
OpenRaveThread::clone
virtual void clone(fawkes::OpenRaveEnvironmentPtr &env, fawkes::OpenRaveRobotPtr &robot, fawkes::OpenRaveManipulatorPtr &manip) const
Clone basically everything We pass pointers to pointer as parameters, so the pointers we create befor...
Definition: openrave_thread.cpp:100
OpenRaveThread::loop
virtual void loop()
Code to execute in the thread.
Definition: openrave_thread.cpp:85
OpenRaveThread::add_object
virtual bool add_object(const std::string &name, const std::string &filename)
Add an object to the environment.
Definition: openrave_thread.cpp:266
OpenRaveThread::start_viewer
virtual void start_viewer() const
Start Viewer.
Definition: openrave_thread.cpp:194
OpenRaveThread::delete_object
virtual bool delete_object(const std::string &name)
Remove object from environment.
Definition: openrave_thread.cpp:275
OpenRaveThread::release_object
virtual bool release_object(const std::string &name, fawkes::OpenRaveRobotPtr &robot)
Release a kinbody from the robot.
Definition: openrave_thread.cpp:409