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
33using 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),
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
61void
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
78void
80{
81 OR_env_->destroy();
82}
83
84void
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 */
99void
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 */
132void
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 */
141void
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 */
157void
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 */
182void
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 */
193void
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 */
203void
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 */
212void
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 */
222void
223OpenRaveThread::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 */
231void
232OpenRaveThread::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 */
243OpenRaveThread::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 */
265bool
266OpenRaveThread::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 */
274bool
275OpenRaveThread::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 */
282bool
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 */
292bool
293OpenRaveThread::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 */
305bool
306OpenRaveThread::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 */
319bool
320OpenRaveThread::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 */
336bool
337OpenRaveThread::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 */
353bool
354OpenRaveThread::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 */
369bool
370OpenRaveThread::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 */
386bool
387OpenRaveThread::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 */
397bool
398OpenRaveThread::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 */
408bool
409OpenRaveThread::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 */
418bool
419OpenRaveThread::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 */
428bool
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 */
437bool
439{
440 return release_all_objects(OR_robot_);
441}
virtual void loop()
Code to execute in the thread.
OpenRaveThread()
Constructor.
virtual bool set_target_object(const std::string &name, fawkes::OpenRaveRobotPtr &robot, float rot_x=0)
Set an object as the target.
virtual void start_viewer() const
Start Viewer.
virtual bool add_object(const std::string &name, const std::string &filename)
Add an object to the 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 rotate_object(const std::string &name, float quat_x, float quat_y, float quat_z, float quat_w)
Rotate object by a quaternion.
virtual void run_graspplanning(const std::string &target_name, fawkes::OpenRaveRobotPtr &robot)
Run graspplanning script for a given target.
virtual bool release_all_objects()
Release all grabbed kinbodys from the robot.
virtual bool attach_object(const char *name, fawkes::OpenRaveRobotPtr &robot, const char *manip_name=NULL)
Attach a kinbody to the robot.
virtual void finalize()
Finalize the thread.
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.
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 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.
virtual bool release_object(const std::string &name, fawkes::OpenRaveRobotPtr &robot)
Release a kinbody from the robot.
virtual bool delete_all_objects()
Remove all objects from environment.
virtual fawkes::OpenRaveRobotPtr get_active_robot() const
Get RefPtr to currently used OpenRaveRobot object.
virtual void run_planner(fawkes::OpenRaveRobotPtr &robot, float sampling=0.01f)
Run planner on previously set target.
virtual void init()
Initialize the thread.
virtual ~OpenRaveThread()
Destructor.
virtual void set_active_robot(fawkes::OpenRaveRobotPtr robot)
Set robot to be used.
virtual bool delete_object(const std::string &name)
Remove object from environment.
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...
Thread aspect provide a new aspect.
Thread aspect to use blocked timing.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
OpenRaveEnvironment class.
Definition: environment.h:44
OpenRAVE Robot class.
Definition: robot.h:38
RefPtr<> is a reference-counting shared smartpointer.
Definition: refptr.h:50
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
Fawkes library namespace.