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 
30 #include <vector>
31 
32 namespace fawkes {
33 
34 class Logger;
35 
36 /** OpenRAVE Robot class */
38 {
39 public:
40  OpenRaveRobot(fawkes::Logger *logger = 0);
41  OpenRaveRobot(const std::string & filename,
43  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);
53  bool display_movements = false);
54  virtual void update_manipulator();
55  virtual void update_model();
56 
57  virtual bool attach_object(OpenRAVE::KinBodyPtr object, const char *manip_name = NULL);
58  virtual bool attach_object(const char * name,
60  const char * manip_name = NULL);
61  virtual bool release_object(OpenRAVE::KinBodyPtr object);
62  virtual bool release_object(const std::string &name, fawkes::OpenRaveEnvironmentPtr &env);
63  virtual bool release_all_objects();
64 
65  virtual bool
66  set_target_rel(float trans_x, float trans_y, float trans_z, bool is_extension = false);
67  virtual bool set_target_straight(float trans_x, float trans_y, float trans_z);
68  virtual bool set_target_quat(float trans_x,
69  float trans_y,
70  float trans_z,
71  float quat_w,
72  float quat_x,
73  float quat_y,
74  float quat_z,
75  OpenRAVE::IkFilterOptions filter = OpenRAVE::IKFO_CheckEnvCollisions,
76  bool no_offset = false);
77  virtual bool
78  set_target_axis_angle(float trans_x,
79  float trans_y,
80  float trans_z,
81  float angle,
82  float axisX,
83  float axisY,
84  float axisZ,
85  OpenRAVE::IkFilterOptions filter = OpenRAVE::IKFO_CheckEnvCollisions,
86  bool no_offset = false);
87  virtual bool
89  float trans_x,
90  float trans_y,
91  float trans_z,
92  float phi,
93  float theta,
94  float psi,
95  OpenRAVE::IkFilterOptions filter = OpenRAVE::IKFO_CheckEnvCollisions,
96  bool no_offset = false);
97  virtual bool
98  set_target_object_position(float trans_x,
99  float trans_y,
100  float trans_z,
101  float rot_x,
102  OpenRAVE::IkFilterOptions filter = OpenRAVE::IKFO_CheckEnvCollisions);
103  virtual bool
104  set_target_ikparam(OpenRAVE::IkParameterization ik_param,
105  OpenRAVE::IkFilterOptions filter = OpenRAVE::IKFO_CheckEnvCollisions);
106  virtual void set_target_plannerparams(std::string &params);
107  virtual void set_target_plannerparams(const char *params);
108  virtual void set_target_raw(std::string &cmd);
109  virtual void set_target_raw(const char *cmd);
110  virtual void set_target_angles(std::vector<float> &angles);
111 
112  virtual void enable_ik_comparison(bool enable);
113 
114  virtual OpenRAVE::RobotBasePtr get_robot_ptr() const;
115  virtual target_t get_target() const;
116  virtual OpenRaveManipulatorPtr get_manipulator() const;
117  virtual OpenRAVE::PlannerBase::PlannerParametersPtr get_planner_params() const;
118  virtual std::vector<std::vector<OpenRAVE::dReal>> * get_trajectory() const;
119  virtual std::vector<std::vector<float>> * get_trajectory_device() const;
120 
121  virtual bool display_planned_movements() const;
122 
123  virtual OpenRAVE::ModuleBasePtr get_basemanip() const;
124 
125 private:
126  void init();
127  void build_name_str();
128  const char *name() const;
129 
130  bool set_target_transform(OpenRAVE::Vector & trans,
131  OpenRAVE::Vector & rotQuat,
132  OpenRAVE::IkFilterOptions filter,
133  bool no_offset = false);
134  bool set_target_euler(OpenRAVE::Vector & trans,
135  std::vector<float> & rotations,
136  OpenRAVE::IkFilterOptions filter,
137  bool no_offset = false);
138  OpenRAVE::IkParameterization get_5dof_ikparam(OpenRAVE::Transform &trans);
139  bool solve_ik(OpenRAVE::IkFilterOptions filter);
140 
141  fawkes::Logger *logger_;
142 
143  std::string name_;
144  std::string name_str_;
145  OpenRAVE::RobotBasePtr robot_;
146  OpenRAVE::RobotBase::ManipulatorPtr arm_;
147  OpenRaveManipulatorPtr manip_;
148  target_t target_;
149 
150  OpenRAVE::ModuleBasePtr mod_basemanip_;
151 
152  OpenRAVE::PlannerBase::PlannerParametersPtr planner_params_;
153  std::vector<std::vector<OpenRAVE::dReal>> * traj_;
154 
155  float trans_offset_x_;
156  float trans_offset_y_;
157  float trans_offset_z_;
158 
159  bool display_planned_movements_;
160  bool find_best_ik_;
161 };
162 
163 } // end of namespace fawkes
164 
165 #endif
Interface for logging.
Definition: logger.h:42
OpenRAVE Robot class.
Definition: robot.h:38
virtual void set_target_plannerparams(std::string &params)
Set additional planner parameters.
Definition: robot.cpp:638
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:258
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 format.
Definition: robot.cpp:739
virtual void set_target_raw(std::string &cmd)
Set raw command for BaseManipulation module.
Definition: robot.cpp:662
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:618
virtual void update_manipulator()
Update motor values from OpenRAVE model.
Definition: robot.cpp:309
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:357
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:411
virtual void set_manipulator(fawkes::OpenRaveManipulatorPtr &manip, bool display_movements=false)
Set pointer to OpenRaveManipulator object.
Definition: robot.cpp:298
virtual target_t get_target() const
Get target.
Definition: robot.cpp:701
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:384
virtual bool release_all_objects()
Release all grabbed kinbodys from the robot.
Definition: robot.cpp:871
OpenRaveRobot(fawkes::Logger *logger=0)
Constructor.
Definition: robot.cpp:47
virtual void load(const std::string &filename, fawkes::OpenRaveEnvironmentPtr &env)
Load robot from xml file.
Definition: robot.cpp:188
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:470
virtual void set_ready()
Set robot ready for usage.
Definition: robot.cpp:211
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:440
virtual OpenRAVE::PlannerBase::PlannerParametersPtr get_planner_params() const
Updates planner parameters and return pointer to it.
Definition: robot.cpp:719
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:547
virtual bool release_object(OpenRAVE::KinBodyPtr object)
Release a kinbody from the robot.
Definition: robot.cpp:837
virtual bool display_planned_movements() const
Getter for display_planned_movements_.
Definition: robot.cpp:331
virtual ~OpenRaveRobot()
Destructor.
Definition: robot.cpp:128
virtual bool attach_object(OpenRAVE::KinBodyPtr object, const char *manip_name=NULL)
Attach a kinbody to the robot.
Definition: robot.cpp:780
virtual OpenRAVE::RobotBasePtr get_robot_ptr() const
Returns RobotBasePtr for uses in other classes.
Definition: robot.cpp:692
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:749
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:274
virtual void enable_ik_comparison(bool enable)
Activate/Deactive IK comparison.
Definition: robot.cpp:343
virtual OpenRAVE::ModuleBasePtr get_basemanip() const
Return BaseManipulation Module-Pointer.
Definition: robot.cpp:767
virtual OpenRaveManipulatorPtr get_manipulator() const
Get manipulator.
Definition: robot.cpp:710
virtual void set_target_angles(std::vector< float > &angles)
Set target angles directly.
Definition: robot.cpp:680
virtual void update_model()
Update/Set OpenRAVE motor angles.
Definition: robot.cpp:319
RefPtr<> is a reference-counting shared smartpointer.
Definition: refptr.h:50
Fawkes library namespace.
euler_rotation_t
Euler rotations.
Definition: types.h:44
Struct containing information about the current target.
Definition: types.h:72