Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
manipulator.h
1 
2 /***************************************************************************
3  * manipulator.h - Fawkes to OpenRAVE Manipulator Data
4  *
5  * Created: Thu Sep 16 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_MANIPULATOR_H_
24 #define __PLUGINS_OPENRAVE_MANIPULATOR_H_
25 
26 #include "types.h"
27 
28 #include <vector>
29 
30 namespace fawkes {
31 #if 0 /* just to make Emacs auto-indent happy */
32 }
33 #endif
34 
36 {
37  public:
38  OpenRaveManipulator(unsigned int count, unsigned int count_device);
39  virtual ~OpenRaveManipulator();
40 
41  virtual void add_motor(unsigned int number, unsigned int number_device);
42 
43  template <typename T_from, typename T_to> void angles_or_to_device(std::vector<T_from>& from, std::vector<T_to>& to) const;
44  template <typename T> void get_angles(std::vector<T>& to) const; // angles of OpenRAVE model
45  template <typename T> void get_angles_device(std::vector<T>& to) const; // angles of real device
46 
47  template <typename T> void set_angles(std::vector<T>& angles);
48  template <typename T> void set_angles_device(std::vector<T>& angles);
49 
50 
51  protected:
52  virtual float angle_OR_to_device(unsigned int number, float angle) const;
53  virtual float angle_device_to_OR(unsigned int number, float angle) const;
54 
55  std::vector<motor_t> __motors; /**< vector of motors */
56  unsigned int __cnt; /**< number of motors on OpenRAVE model */
57  unsigned int __cnt_device; /**< number of motors on real device */
58 };
59 
60 
61 /* ########## getter ########## */
62 /** Get motor angles of OpenRAVE model
63  * @param to target tvector of angles
64  */
65 template <typename T>
66 void
67 OpenRaveManipulator::get_angles(std::vector<T>& to) const
68 {
69  to.resize(__cnt);
70  for (unsigned int i=0; i<__motors.size(); i++) {
71  to[__motors[i].no] = (T)__motors[i].angle;
72  }
73 }
74 
75 /** Get motor angles of real device
76  * @param to target vector of angles
77  */
78 template <typename T>
79 void
80 OpenRaveManipulator::get_angles_device(std::vector<T>& to) const
81 {
82  std::vector<float> tmp;
83  get_angles(tmp);
84  angles_or_to_device(tmp, to);
85  //to = angles_or_to_device(tmp);
86 }
87 
88 /** Transform OpenRAVE motor angles to real device angles
89  * @param from motor angles of OpenRAVE model
90  * @param to motor angles of real device
91  */
92 template <typename T_from, typename T_to>
93 void
94 OpenRaveManipulator::angles_or_to_device(std::vector<T_from>& from, std::vector<T_to>&to) const
95 {
96  to.resize(__cnt_device);
97 
98  for (unsigned int i=0; i<__motors.size(); i++) {
99  to[__motors[i].no_device] = (T_to)angle_OR_to_device(__motors[i].no_device, (float)from[__motors[i].no]);
100  }
101 }
102 
103 
104 /* ########## setter ########## */
105 /** Set motor angles of OpenRAVE model
106  * @param angles motor angles
107  */
108 template <typename T>
109 void
110 OpenRaveManipulator::set_angles(std::vector<T>& angles)
111 {
112  for (unsigned int i=0; i<__motors.size(); i++) {
113  __motors[i].angle = (float)angles[__motors[i].no];
114  }
115 }
116 
117 /** Set motor angles of real device
118  * @param angles motor angles
119  */
120 template <typename T>
121 void
122 OpenRaveManipulator::set_angles_device(std::vector<T>& angles)
123 {
124  for (unsigned int i=0; i<__motors.size(); i++) {
125  __motors[i].angle = angle_device_to_OR(__motors[i].no_device, (float)angles[__motors[i].no_device]);
126  }
127 }
128 
129 
130 
131 } // end of namespace fawkes
132 
133 #endif