Fawkes API  Fawkes Development Version
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  /** Create a new copy of this OpenRaveManipulator instance
42  * @return RefPtr to the copied OpenRaveManipulator
43  */
44  virtual OpenRaveManipulatorPtr copy() = 0;
45 
46  void add_motor(unsigned int number, unsigned int number_device);
47 
48  template <typename T_from, typename T_to> void angles_or_to_device(std::vector<T_from>& from, std::vector<T_to>& to) const;
49  template <typename T> void get_angles(std::vector<T>& to) const; // angles of OpenRAVE model
50  template <typename T> void get_angles_device(std::vector<T>& to) const; // angles of real device
51 
52  template <typename T> void set_angles(std::vector<T>& angles);
53  template <typename T> void set_angles_device(std::vector<T>& angles);
54 
55 
56  protected:
57  /** Transform single OpenRAVE motor angle to real device angle
58  * @param number motor number of real device
59  * @param angle motor angle of OpenRAVE model
60  * @return transformed angle
61  */
62  virtual float angle_OR_to_device(unsigned int number, float angle) const = 0;
63 
64  /** Transform single device motor angle to OpenRAVE angle
65  * @param number motor number of real device
66  * @param angle motor angle of real device
67  * @return transformed angle
68  */
69  virtual float angle_device_to_OR(unsigned int number, float angle) const = 0;
70 
71  std::vector<motor_t> __motors; /**< vector of motors */
72  unsigned int __cnt; /**< number of motors on OpenRAVE model */
73  unsigned int __cnt_device; /**< number of motors on real device */
74 };
75 
76 
77 /* ########## getter ########## */
78 /** Get motor angles of OpenRAVE model
79  * @param to target tvector of angles
80  */
81 template <typename T>
82 void
83 OpenRaveManipulator::get_angles(std::vector<T>& to) const
84 {
85  to.resize(__cnt);
86  for (unsigned int i=0; i<__motors.size(); i++) {
87  to[__motors[i].no] = (T)__motors[i].angle;
88  }
89 }
90 
91 /** Get motor angles of real device
92  * @param to target vector of angles
93  */
94 template <typename T>
95 void
96 OpenRaveManipulator::get_angles_device(std::vector<T>& to) const
97 {
98  std::vector<float> tmp;
99  get_angles(tmp);
100  angles_or_to_device(tmp, to);
101  //to = angles_or_to_device(tmp);
102 }
103 
104 /** Transform OpenRAVE motor angles to real device angles
105  * @param from motor angles of OpenRAVE model
106  * @param to motor angles of real device
107  */
108 template <typename T_from, typename T_to>
109 void
110 OpenRaveManipulator::angles_or_to_device(std::vector<T_from>& from, std::vector<T_to>&to) const
111 {
112  to.resize(__cnt_device);
113 
114  for (unsigned int i=0; i<__motors.size(); i++) {
115  to[__motors[i].no_device] = (T_to)angle_OR_to_device(__motors[i].no_device, (float)from[__motors[i].no]);
116  }
117 }
118 
119 
120 /* ########## setter ########## */
121 /** Set motor angles of OpenRAVE model
122  * @param angles motor angles
123  */
124 template <typename T>
125 void
126 OpenRaveManipulator::set_angles(std::vector<T>& angles)
127 {
128  if( angles.size() < __motors.size() ) {
129  angles.reserve(__motors.size());
130  }
131  for (unsigned int i=0; i<__motors.size(); i++) {
132  __motors[i].angle = (float)angles[__motors[i].no];
133  }
134 }
135 
136 /** Set motor angles of real device
137  * @param angles motor angles
138  */
139 template <typename T>
140 void
142 {
143  if( angles.size() < __motors.size() ) {
144  angles.reserve(__motors.size());
145  }
146  for (unsigned int i=0; i<__motors.size(); i++) {
147  __motors[i].angle = angle_device_to_OR(__motors[i].no_device, (float)angles[__motors[i].no_device]);
148  }
149 }
150 
151 
152 
153 } // end of namespace fawkes
154 
155 #endif
void set_angles_device(std::vector< T > &angles)
Set motor angles of real device.
Definition: manipulator.h:141
virtual OpenRaveManipulatorPtr copy()=0
Create a new copy of this OpenRaveManipulator instance.
Fawkes library namespace.
virtual float angle_device_to_OR(unsigned int number, float angle) const =0
Transform single device motor angle to OpenRAVE angle.
unsigned int __cnt_device
number of motors on real device
Definition: manipulator.h:73
unsigned int __cnt
number of motors on OpenRAVE model
Definition: manipulator.h:72
void set_angles(std::vector< T > &angles)
Set motor angles of OpenRAVE model.
Definition: manipulator.h:126
OpenRaveManipulator(unsigned int count, unsigned int count_device)
Constructor.
Definition: manipulator.cpp:41
Class containing information about all manipulator motors.
Definition: manipulator.h:35
void get_angles_device(std::vector< T > &to) const
Get motor angles of real device.
Definition: manipulator.h:96
void get_angles(std::vector< T > &to) const
Get motor angles of OpenRAVE model.
Definition: manipulator.h:83
virtual float angle_OR_to_device(unsigned int number, float angle) const =0
Transform single OpenRAVE motor angle to real device angle.
void angles_or_to_device(std::vector< T_from > &from, std::vector< T_to > &to) const
Transform OpenRAVE motor angles to real device angles.
Definition: manipulator.h:110
virtual ~OpenRaveManipulator()
Destructor.
Definition: manipulator.cpp:48
void add_motor(unsigned int number, unsigned int number_device)
Adds a motor to the list(vector) of motors.
Definition: manipulator.cpp:58
std::vector< motor_t > __motors
vector of motors
Definition: manipulator.h:71