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
30namespace fawkes {
31
33{
34public:
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 */
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
57protected:
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 */
81template <typename T>
82void
83OpenRaveManipulator::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 */
94template <typename T>
95void
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 */
108template <typename T_from, typename T_to>
109void
110OpenRaveManipulator::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 */
124template <typename T>
125void
126OpenRaveManipulator::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 */
139template <typename T>
140void
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
Class containing information about all manipulator motors.
Definition: manipulator.h:33
OpenRaveManipulator(unsigned int count, unsigned int count_device)
Constructor.
Definition: manipulator.cpp:38
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
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 float angle_device_to_OR(unsigned int number, float angle) const =0
Transform single device motor angle to OpenRAVE angle.
virtual float angle_OR_to_device(unsigned int number, float angle) const =0
Transform single OpenRAVE motor angle to real device angle.
void set_angles(std::vector< T > &angles)
Set motor angles of OpenRAVE model.
Definition: manipulator.h:126
void add_motor(unsigned int number, unsigned int number_device)
Adds a motor to the list(vector) of motors.
Definition: manipulator.cpp:53
std::vector< motor_t > motors_
vector of motors
Definition: manipulator.h:72
unsigned int cnt_
number of motors on OpenRAVE model
Definition: manipulator.h:73
virtual OpenRaveManipulatorPtr copy()=0
Create a new copy of this OpenRaveManipulator instance.
virtual ~OpenRaveManipulator()
Destructor.
Definition: manipulator.cpp:44
unsigned int cnt_device_
number of motors on real device
Definition: manipulator.h:74
void set_angles_device(std::vector< T > &angles)
Set motor angles of real device.
Definition: manipulator.h:141
Fawkes library namespace.