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 "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(&__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->set_name("viewer");
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 /** Clone basically everything
96  * We pass pointers to pointer as parameters, so the pointers we create before calling this clone()
97  * method will point to the new objects.
98  * @param env Pointer to pointer of the copied environment
99  * @param robot Pointer to pointer of the copied robot
100  * @param manip Pointer to pointer of the copied manipulator
101  */
102 void
104 {
105  env = new OpenRaveEnvironment(**__OR_env);
106  robot = new OpenRaveRobot(**__OR_robot, env);
107  manip = robot->get_manipulator(); // cloned by OpenRaveRobot copy-ctor
108 
109  env->load_IK_solver(robot);
110 }
111 
112 /** Get pointer to OpenRaveEnvironment object.
113  * @return pointer
114  */
117 {
118  return __OR_env;
119 }
120 
121 /** Get RefPtr to currently used OpenRaveRobot object.
122  * @return RefPtr
123  */
126 {
127  return __OR_robot;
128 }
129 
130 /** Set robot to be used
131  * @param robot OpenRaveRobot that should be used implicitly in other methods
132  */
133 void
135 {
136  __OR_robot = robot;
137 }
138 
139 /** Set robot to be used
140  * @param robot OpenRaveRobot that should be used implicitly in other methods
141  */
142 void
144 {
145  __OR_robot = robot;
146 }
147 
148 /** Set OpenRaveManipulator object for robot, and calculate
149  * coordinate-system offsets or set them directly.
150  * Make sure to update manip angles before calibrating!
151  * @param robot pointer to OpenRaveRobot object, explicitly set
152  * @param manip pointer to OpenRAVManipulator that is set for robot
153  * @param trans_x transition offset on x-axis
154  * @param trans_y transition offset on y-axis
155  * @param trans_z transition offset on z-axis
156  * @param calibrate decides whether to calculate offset (true )or set them directly (false; default)
157  */
158 void
159 OpenRaveThread::set_manipulator(OpenRaveRobotPtr& robot, OpenRaveManipulatorPtr& manip, float trans_x, float trans_y, float trans_z, bool calibrate)
160 {
161  robot->set_manipulator(manip);
162  if( calibrate )
163  {robot->calibrate(trans_x, trans_y, trans_z);}
164  else
165  {robot->set_offset(trans_x, trans_y, trans_z);}
166 }
167 /** Set OpenRaveManipulator object for robot, and calculate
168  * coordinate-system offsets or set them directly.
169  * Make sure to update manip angles before calibrating!
170  * Uses default OpenRaveRobot object.
171  * @param manip pointer to OpenRAVManipulator that is set for robot
172  * @param trans_x transition offset on x-axis
173  * @param trans_y transition offset on y-axis
174  * @param trans_z transition offset on z-axis
175  * @param calibrate decides whether to calculate offset (true )or set them directly (false; default)
176  */
177 void
178 OpenRaveThread::set_manipulator(OpenRaveManipulatorPtr& manip, float trans_x, float trans_y, float trans_z, bool calibrate)
179 {
180  set_manipulator(__OR_robot, manip, trans_x, trans_y, trans_z, calibrate);
181 }
182 
183 
184 /** Start Viewer */
185 void
187 {
188  __OR_env->start_viewer();
189 }
190 
191 /** Run planner on previously set target.
192  * @param robot robot to use planner on. If none is given, the currently used robot is taken
193  * @param sampling sampling time between each trajectory point (in seconds)
194  */
195 void
197 {
198  __OR_env->run_planner(robot, sampling);
199 }
200 
201 /** Run planner on previously set target. Uses currently active robot.
202  * @param sampling sampling time between each trajectory point (in seconds)
203  */
204 void
206 {
207  run_planner(__OR_robot, sampling);
208 }
209 
210 /** Run graspplanning script for a given target.
211  * @param target_name name of targeted object (KinBody)
212  * @param robot robot to use planner on. If none is given, the currently used robot is taken
213  */
214 void
215 OpenRaveThread::run_graspplanning(const std::string& target_name, OpenRaveRobotPtr& robot)
216 {
217  __OR_env->run_graspplanning(target_name, robot);
218 }
219 
220 /** Run graspplanning script for a given target. Uses currently active robot.
221  * @param target_name name of targeted object (KinBody)
222  */
223 void
224 OpenRaveThread::run_graspplanning(const std::string& target_name)
225 {
226  run_graspplanning(target_name, __OR_robot);
227 }
228 
229 /** Add a new robot to the environment, and set it as the currently active one.
230  * @param filename_robot path to robot's xml file
231  * @param autogenerate_IK if true: autogenerate IKfast IK solver for robot
232  * @return pointer to new OpenRaveRobot object
233  */
235 OpenRaveThread::add_robot(const std::string& filename_robot, bool autogenerate_IK)
236 {
238  robot->load(filename_robot, __OR_env);
239  __OR_env->add_robot(robot);
240  robot->set_ready();
241 
242  if( autogenerate_IK )
243  __OR_env->load_IK_solver(robot);
244 
245  set_active_robot(robot);
246 
247  return robot;
248 }
249 
250 /* ##########################################################
251  * # object handling (mainly from environment.h) #
252 * ########################################################*/
253 /** Add an object to the environment.
254  * @param name name that should be given to that object
255  * @param filename path to xml file of that object (KinBody)
256  * @return true if successful */
257 bool OpenRaveThread::add_object(const std::string& name, const std::string& filename) {
258  return __OR_env->add_object(name, filename); }
259 
260 /** Remove object from environment.
261  * @param name name of the object
262  * @return true if successful */
263 bool OpenRaveThread::delete_object(const std::string& name) {
264  return __OR_env->delete_object(name); }
265 
266 /** Remove all objects from environment.
267  * @return true if successful */
269  return __OR_env->delete_all_objects(); }
270 
271 /** Rename object.
272  * @param name current name of the object
273  * @param new_name new name of the object
274  * @return true if successful */
275 bool OpenRaveThread::rename_object(const std::string& name, const std::string& new_name) {
276  return __OR_env->rename_object(name, new_name); }
277 
278 /** Move object in the environment.
279  * Distances are given in meters
280  * @param name name of the object
281  * @param trans_x transition along x-axis
282  * @param trans_y transition along y-axis
283  * @param trans_z transition along z-axis
284  * @return true if successful */
285 bool OpenRaveThread::move_object(const std::string& name, float trans_x, float trans_y, float trans_z) {
286  return __OR_env->move_object(name, trans_x, trans_y, trans_z); }
287 
288 /** Move object in the environment, relatively to robot.
289  * Distances are given in meters
290  * @param name name of the object
291  * @param trans_x transition along x-axis
292  * @param trans_y transition along y-axis
293  * @param trans_z transition along z-axis
294  * @param robot move relatively to robot (in most simple cases robot is at position (0,0,0) anyway, so this has no effect)
295  * @return true if successful */
296 bool OpenRaveThread::move_object(const std::string& name, float trans_x, float trans_y, float trans_z, OpenRaveRobotPtr& robot) {
297  return __OR_env->move_object(name, trans_x, trans_y, trans_z, robot); }
298 
299 /** Rotate object by a quaternion.
300  * @param name name of the object
301  * @param quat_x x value of quaternion
302  * @param quat_y y value of quaternion
303  * @param quat_z z value of quaternion
304  * @param quat_w w value of quaternion
305  * @return true if successful */
306 bool OpenRaveThread::rotate_object(const std::string& name, float quat_x, float quat_y, float quat_z, float quat_w) {
307  return __OR_env->rotate_object(name, quat_x, quat_y, quat_z, quat_w); }
308 
309 /** Rotate object along its axis.
310  * Rotation angles should be given in radians.
311  * @param name name of the object
312  * @param rot_x 1st rotation, along x-axis
313  * @param rot_y 2nd rotation, along y-axis
314  * @param rot_z 3rd rotation, along z-axis
315  * @return true if successful */
316 bool OpenRaveThread::rotate_object(const std::string& name, float rot_x, float rot_y, float rot_z) {
317  return __OR_env->rotate_object(name, rot_x, rot_y, rot_z); }
318 
319 /** Set an object as the target.
320  * Currently the object should be cylindric, and stand upright. It may
321  * also be rotated on its x-axis, but that rotation needs to be given in an argument
322  * to calculate correct position for endeffecto. This is only temporary until
323  * proper graps planning for 5DOF in OpenRAVE is provided.
324  * @param name name of the object
325  * @param robot pointer to OpenRaveRobot that the target is set for
326  * @param rot_x rotation of object on x-axis (radians)
327  * @return true if IK solvable
328  */
329 bool
330 OpenRaveThread::set_target_object(const std::string& name, OpenRaveRobotPtr& robot, float rot_x)
331 {
332  OpenRAVE::Transform transform = __OR_env->get_env_ptr()->GetKinBody(name)->GetTransform();
333 
334  return robot->set_target_object_position(transform.trans[0], transform.trans[1], transform.trans[2], rot_x);
335 }
336 
337 /** Attach a kinbody to the robot.
338  * @param name name of the object
339  * @param robot pointer to OpenRaveRobot that object is attached to
340  * @param manip_name name of the manipulator to attach the object to
341  * @return true if successfull
342  */
343 bool
344 OpenRaveThread::attach_object(const char* name, OpenRaveRobotPtr& robot, const char* manip_name)
345 {
346  return robot->attach_object(name, __OR_env, manip_name);
347 }
348 
349 /** Attach a kinbody to the robot. Uses currently active robot.
350  * @param name name of the object
351  * @param manip_name name of the manipulator to attach the object to
352  * @return true if successfull
353  */
354 bool
355 OpenRaveThread::attach_object(const char* name, const char* manip_name)
356 {
357  return attach_object(name, __OR_robot, manip_name);
358 }
359 
360 /** Release a kinbody from the robot.
361  * @param name name of the object
362  * @param robot pointer to OpenRaveRobot that object is released from
363  * @return true if successfull
364  */
365 bool
367 {
368  return robot->release_object(name, __OR_env);
369 }
370 
371 /** Release a kinbody from the robot. Uses currently active robot.
372  * @param name name of the object
373  * @return true if successfull
374  */
375 bool
377 {
378  return release_object(name, __OR_robot);
379 }
380 
381 /** Release all grabbed kinbodys from the robot.
382  * @param robot pointer to OpenRaveRobot that objects are released from
383  * @return true if successfull
384  */
385 bool
387 {
388  return robot->release_all_objects();
389 }
390 
391 /** Release all grabbed kinbodys from the robot. Uses currently active robot.
392  * @return true if successfull
393  */
394 bool
396 {
397  return release_all_objects(__OR_robot);
398 }
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...
OpenRaveThread()
Constructor.
virtual void set_active_robot(fawkes::OpenRaveRobotPtr robot)
Set robot to be used.
Fawkes library namespace.
virtual void init()
Initialize the thread.
virtual bool attach_object(const char *name, fawkes::OpenRaveRobotPtr &robot, const char *manip_name=NULL)
Attach a kinbody to the robot.
Thread class encapsulation of pthreads.
Definition: thread.h:42
virtual bool delete_all_objects()
Remove all objects from environment.
virtual void finalize()
Finalize the thread.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
virtual ~OpenRaveThread()
Destructor.
virtual bool release_object(const std::string &name, fawkes::OpenRaveRobotPtr &robot)
Release a kinbody from the robot.
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.
Thread aspect to use blocked timing.
virtual bool delete_object(const std::string &name)
Remove object from environment.
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...
virtual bool release_all_objects()
Release all grabbed kinbodys from the robot.
OpenRAVE Robot class.
Definition: robot.h:39
virtual fawkes::OpenRaveRobotPtr get_active_robot() const
Get RefPtr to currently used OpenRaveRobot object.
OpenRaveEnvironment class.
Definition: environment.h:45
virtual bool set_target_object(const std::string &name, fawkes::OpenRaveRobotPtr &robot, float rot_x=0)
Set an object as the target.
virtual void run_planner(fawkes::OpenRaveRobotPtr &robot, float sampling=0.01f)
Run planner on previously set target.
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.
const char * name() const
Get name of thread.
Definition: thread.h:95
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
Thread aspect provide a new aspect.
virtual void run_graspplanning(const std::string &target_name, fawkes::OpenRaveRobotPtr &robot)
Run graspplanning script for a given target.
RefPtr<> is a reference-counting shared smartpointer.
Definition: refptr.h:49
virtual void loop()
Code to execute in the thread.
virtual void start_viewer() const
Start Viewer.
virtual bool rename_object(const std::string &name, const std::string &new_name)
Rename object.
virtual fawkes::OpenRaveEnvironmentPtr get_environment() const
Get pointer to OpenRaveEnvironment object.
virtual bool add_object(const std::string &name, const std::string &filename)
Add an object to the environment.
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.