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 
32 class OpenRaveManipulator
33 {
34 public:
35  OpenRaveManipulator(unsigned int count, unsigned int count_device);
36  virtual ~OpenRaveManipulator();
37 
38  /** Create a new copy of this OpenRaveManipulator instance
39  * @return RefPtr to the copied OpenRaveManipulator
40  */
41  virtual OpenRaveManipulatorPtr copy() = 0;
42 
43  void add_motor(unsigned int number, unsigned int number_device);
44 
45  template <typename T_from, typename T_to>
46  void angles_or_to_device(std::vector<T_from> &from, std::vector<T_to> &to) const;
47  template <typename T>
48  void get_angles(std::vector<T> &to) const; // angles of OpenRAVE model
49  template <typename T>
50  void get_angles_device(std::vector<T> &to) const; // angles of real device
51 
52  template <typename T>
53  void set_angles(std::vector<T> &angles);
54  template <typename T>
55  void set_angles_device(std::vector<T> &angles);
56 
57 protected:
58  /** Transform single OpenRAVE motor angle to real device angle
59  * @param number motor number of real device
60  * @param angle motor angle of OpenRAVE model
61  * @return transformed angle
62  */
63  virtual float angle_OR_to_device(unsigned int number, float angle) const = 0;
64 
65  /** Transform single device motor angle to OpenRAVE angle
66  * @param number motor number of real device
67  * @param angle motor angle of real device
68  * @return transformed angle
69  */
70  virtual float angle_device_to_OR(unsigned int number, float angle) const = 0;
71 
72  std::vector<motor_t> motors_; /**< vector of motors */
73  unsigned int cnt_; /**< number of motors on OpenRAVE model */
74  unsigned int cnt_device_; /**< number of motors on real device */
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] =
116  (T_to)angle_OR_to_device(motors_[i].no_device, (float)from[motors_[i].no]);
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
141 OpenRaveManipulator::set_angles_device(std::vector<T> &angles)
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 =
148  angle_device_to_OR(motors_[i].no_device, (float)angles[motors_[i].no_device]);
149  }
150 }
151 
152 } // end of namespace fawkes
153 
154 #endif
fawkes::OpenRaveManipulator::copy
virtual OpenRaveManipulatorPtr copy()=0
Create a new copy of this OpenRaveManipulator instance.
fawkes::OpenRaveManipulator::motors_
std::vector< motor_t > motors_
vector of motors
Definition: manipulator.h:84
fawkes::OpenRaveManipulator::cnt_device_
unsigned int cnt_device_
number of motors on real device
Definition: manipulator.h:86
fawkes::RefPtr< OpenRaveManipulator >
fawkes::OpenRaveManipulator::angle_device_to_OR
virtual float angle_device_to_OR(unsigned int number, float angle) const =0
Transform single device motor angle to OpenRAVE angle.
fawkes::OpenRaveManipulator::set_angles
void set_angles(std::vector< T > &angles)
Set motor angles of OpenRAVE model.
Definition: manipulator.h:132
fawkes
fawkes::OpenRaveManipulator::cnt_
unsigned int cnt_
number of motors on OpenRAVE model
Definition: manipulator.h:85
fawkes::OpenRaveManipulator::get_angles_device
void get_angles_device(std::vector< T > &to) const
Get motor angles of real device.
Definition: manipulator.h:102
fawkes::OpenRaveManipulator::OpenRaveManipulator
OpenRaveManipulator(unsigned int count, unsigned int count_device)
Constructor.
Definition: manipulator.cpp:44
fawkes::OpenRaveManipulator::get_angles
void get_angles(std::vector< T > &to) const
Get motor angles of OpenRAVE model.
Definition: manipulator.h:89
fawkes::OpenRaveManipulator::angle_OR_to_device
virtual float angle_OR_to_device(unsigned int number, float angle) const =0
Transform single OpenRAVE motor angle to real device angle.
fawkes::OpenRaveManipulator::add_motor
void add_motor(unsigned int number, unsigned int number_device)
Adds a motor to the list(vector) of motors.
Definition: manipulator.cpp:59
fawkes::OpenRaveManipulator::~OpenRaveManipulator
virtual ~OpenRaveManipulator()
Destructor.
Definition: manipulator.cpp:50
fawkes::OpenRaveManipulator::angles_or_to_device
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:116
fawkes::OpenRaveManipulator::set_angles_device
void set_angles_device(std::vector< T > &angles)
Set motor angles of real device.
Definition: manipulator.h:147