Fawkes API  Fawkes Development Version
robot.h
1 
2 /***************************************************************************
3  * robot.h - Fawkes to OpenRAVE Robot Handler
4  *
5  * Created: Mon Sep 20 14:50:34 2010
6  * Copyright 2010 Bahram Maleki-Fard, AllemaniACs RoboCup Team
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 #ifndef __PLUGINS_OPENRAVE_ROBOT_H
24 #define __PLUGINS_OPENRAVE_ROBOT_H
25 
26 #include "types.h"
27 
28 #include <openrave/openrave.h>
29 #include <vector>
30 
31 namespace fawkes {
32 #if 0 /* just to make Emacs auto-indent happy */
33 }
34 #endif
35 
36 class Logger;
37 
38 /** OpenRAVE Robot class */
40 {
41  public:
42  OpenRaveRobot(fawkes::Logger* logger = 0);
43  OpenRaveRobot(const std::string& filename, fawkes::OpenRaveEnvironmentPtr& env, fawkes::Logger* logger = 0);
45  virtual ~OpenRaveRobot();
46 
47  // build/load robot parts
48  virtual void load(const std::string& filename, fawkes::OpenRaveEnvironmentPtr& env);
49  virtual void set_ready();
50  virtual void set_offset(float trans_x, float trans_y, float trans_z);
51  virtual void calibrate(float device_trans_x, float device_trans_y, float device_trans_z);
52  virtual void set_manipulator(fawkes::OpenRaveManipulatorPtr& manip, bool display_movements = false);
53  virtual void update_manipulator();
54  virtual void update_model();
55 
56  virtual bool attach_object(OpenRAVE::KinBodyPtr object, const char* manip_name = NULL);
57  virtual bool attach_object(const char* name, fawkes::OpenRaveEnvironmentPtr& env, const char* manip_name = NULL);
58  virtual bool release_object(OpenRAVE::KinBodyPtr object);
59  virtual bool release_object(const std::string& name, fawkes::OpenRaveEnvironmentPtr& env);
60  virtual bool release_all_objects();
61 
62  virtual bool set_target_rel(float trans_x, float trans_y, float trans_z, bool is_extension=false);
63  virtual bool set_target_straight(float trans_x, float trans_y, float trans_z);
64  virtual bool set_target_quat(float trans_x, float trans_y, float trans_z,
65  float quat_w, float quat_x, float quat_y, float quat_z,
66  OpenRAVE::IkFilterOptions filter = OpenRAVE::IKFO_CheckEnvCollisions, bool no_offset = false);
67  virtual bool set_target_axis_angle(float trans_x, float trans_y, float trans_z,
68  float angle, float axisX, float axisY, float axisZ,
69  OpenRAVE::IkFilterOptions filter = OpenRAVE::IKFO_CheckEnvCollisions, bool no_offset = false);
70  virtual bool set_target_euler(euler_rotation_t type,
71  float trans_x, float trans_y, float trans_z,
72  float phi, float theta, float psi,
73  OpenRAVE::IkFilterOptions filter = OpenRAVE::IKFO_CheckEnvCollisions, bool no_offset = false);
74  virtual bool set_target_object_position(float trans_x, float trans_y, float trans_z, float rot_x, OpenRAVE::IkFilterOptions filter = OpenRAVE::IKFO_CheckEnvCollisions);
75  virtual bool set_target_ikparam(OpenRAVE::IkParameterization ik_param, OpenRAVE::IkFilterOptions filter = OpenRAVE::IKFO_CheckEnvCollisions);
76  virtual void set_target_plannerparams(std::string& params);
77  virtual void set_target_plannerparams(const char* params);
78  virtual void set_target_raw(std::string& cmd);
79  virtual void set_target_raw(const char* cmd);
80  virtual void set_target_angles( std::vector<float>& angles );
81 
82  virtual void enable_ik_comparison(bool enable);
83 
84  virtual OpenRAVE::RobotBasePtr get_robot_ptr() const;
85  virtual target_t get_target() const;
87  virtual OpenRAVE::PlannerBase::PlannerParametersPtr get_planner_params() const;
88  virtual std::vector< std::vector<OpenRAVE::dReal> >* get_trajectory() const;
89  virtual std::vector< std::vector<float> >* get_trajectory_device() const;
90 
91  virtual bool display_planned_movements() const;
92 
93  virtual OpenRAVE::ModuleBasePtr get_basemanip() const;
94 
95  private:
96  void init();
97  void build_name_str();
98  const char* name() const;
99 
100  bool set_target_transform(OpenRAVE::Vector& trans, OpenRAVE::Vector& rotQuat, OpenRAVE::IkFilterOptions filter, bool no_offset = false);
101  bool set_target_euler(OpenRAVE::Vector& trans, std::vector<float>& rotations, OpenRAVE::IkFilterOptions filter, bool no_offset = false);
102  OpenRAVE::IkParameterization get_5dof_ikparam(OpenRAVE::Transform& trans);
103  bool solve_ik(OpenRAVE::IkFilterOptions filter);
104 
105  fawkes::Logger* __logger;
106 
107  std::string __name;
108  std::string __name_str;
109  OpenRAVE::RobotBasePtr __robot;
110  OpenRAVE::RobotBase::ManipulatorPtr __arm;
111  OpenRaveManipulatorPtr __manip;
112  target_t __target;
113 
114 
115  OpenRAVE::ModuleBasePtr __mod_basemanip;
116 
117  OpenRAVE::PlannerBase::PlannerParametersPtr __planner_params;
118  std::vector< std::vector<OpenRAVE::dReal> >* __traj;
119 
120  float __trans_offset_x;
121  float __trans_offset_y;
122  float __trans_offset_z;
123 
124  bool __display_planned_movements;
125  bool __find_best_ik;
126 };
127 
128 } // end of namespace fawkes
129 
130 #endif
virtual void set_target_angles(std::vector< float > &angles)
Set target angles directly.
Definition: robot.cpp:623
virtual void update_manipulator()
Update motor values from OpenRAVE model.
Definition: robot.cpp:295
virtual bool set_target_euler(euler_rotation_t type, float trans_x, float trans_y, float trans_z, float phi, float theta, float psi, OpenRAVE::IkFilterOptions filter=OpenRAVE::IKFO_CheckEnvCollisions, bool no_offset=false)
Set target, given transition, and Euler-rotation.
Definition: robot.cpp:444
virtual void set_target_plannerparams(std::string &params)
Set additional planner parameters.
Definition: robot.cpp:581
Fawkes library namespace.
virtual std::vector< std::vector< float > > * get_trajectory_device() const
Return pointer to trajectory of motion from __manip to __target.manip with device angle format...
Definition: robot.cpp:695
virtual target_t get_target() const
Get target.
Definition: robot.cpp:647
virtual void set_offset(float trans_x, float trans_y, float trans_z)
Directly set transition offset between coordinate systems of real device and OpenRAVE model...
Definition: robot.cpp:244
virtual OpenRAVE::RobotBasePtr get_robot_ptr() const
Returns RobotBasePtr for uses in other classes.
Definition: robot.cpp:638
virtual bool set_target_rel(float trans_x, float trans_y, float trans_z, bool is_extension=false)
Set target, given relative transition.
Definition: robot.cpp:343
virtual bool set_target_object_position(float trans_x, float trans_y, float trans_z, float rot_x, OpenRAVE::IkFilterOptions filter=OpenRAVE::IKFO_CheckEnvCollisions)
Set target by giving position of an object.
Definition: robot.cpp:499
virtual OpenRAVE::PlannerBase::PlannerParametersPtr get_planner_params() const
Updates planner parameters and return pointer to it.
Definition: robot.cpp:665
virtual void calibrate(float device_trans_x, float device_trans_y, float device_trans_z)
Calculate transition offset between coordinate systems of real device and OpenRAVE model...
Definition: robot.cpp:260
OpenRAVE Robot class.
Definition: robot.h:39
virtual void update_model()
Update/Set OpenRAVE motor angles.
Definition: robot.cpp:305
virtual void enable_ik_comparison(bool enable)
Activate/Deactive IK comparison.
Definition: robot.cpp:329
virtual std::vector< std::vector< OpenRAVE::dReal > > * get_trajectory() const
Return pointer to trajectory of motion from __manip to __target.manip with OpenRAVE-model angle forma...
Definition: robot.cpp:685
Struct containing information about the current target.
Definition: types.h:75
virtual void set_target_raw(std::string &cmd)
Set raw command for BaseManipulation module.
Definition: robot.cpp:605
virtual bool set_target_axis_angle(float trans_x, float trans_y, float trans_z, float angle, float axisX, float axisY, float axisZ, OpenRAVE::IkFilterOptions filter=OpenRAVE::IKFO_CheckEnvCollisions, bool no_offset=false)
Set target, given transition, and rotation as axis-angle.
Definition: robot.cpp:420
OpenRaveRobot(fawkes::Logger *logger=0)
Constructor.
Definition: robot.cpp:48
virtual bool attach_object(OpenRAVE::KinBodyPtr object, const char *manip_name=NULL)
Attach a kinbody to the robot.
Definition: robot.cpp:727
virtual OpenRAVE::ModuleBasePtr get_basemanip() const
Return BaseManipulation Module-Pointer.
Definition: robot.cpp:713
virtual void set_manipulator(fawkes::OpenRaveManipulatorPtr &manip, bool display_movements=false)
Set pointer to OpenRaveManipulator object.
Definition: robot.cpp:284
euler_rotation_t
Euler rotations.
Definition: types.h:47
virtual void load(const std::string &filename, fawkes::OpenRaveEnvironmentPtr &env)
Load robot from xml file.
Definition: robot.cpp:178
RefPtr<> is a reference-counting shared smartpointer.
Definition: refptr.h:49
virtual OpenRaveManipulatorPtr get_manipulator() const
Get manipulator.
Definition: robot.cpp:656
virtual bool release_all_objects()
Release all grabbed kinbodys from the robot.
Definition: robot.cpp:814
virtual ~OpenRaveRobot()
Destructor.
Definition: robot.cpp:120
virtual void set_ready()
Set robot ready for usage.
Definition: robot.cpp:200
virtual bool set_target_straight(float trans_x, float trans_y, float trans_z)
Set target for a straight movement, given transition.
Definition: robot.cpp:370
virtual bool display_planned_movements() const
Getter for __display_planned_movements.
Definition: robot.cpp:317
virtual bool set_target_ikparam(OpenRAVE::IkParameterization ik_param, OpenRAVE::IkFilterOptions filter=OpenRAVE::IKFO_CheckEnvCollisions)
Set target by giving IkParameterizaion of target.
Definition: robot.cpp:561
virtual bool release_object(OpenRAVE::KinBodyPtr object)
Release a kinbody from the robot.
Definition: robot.cpp:780
virtual bool set_target_quat(float trans_x, float trans_y, float trans_z, float quat_w, float quat_x, float quat_y, float quat_z, OpenRAVE::IkFilterOptions filter=OpenRAVE::IKFO_CheckEnvCollisions, bool no_offset=false)
Set target, given transition, and rotation as quaternion.
Definition: robot.cpp:397
Interface for logging.
Definition: logger.h:34