23 #ifndef __PLUGINS_OPENRAVE_MANIPULATOR_H_ 24 #define __PLUGINS_OPENRAVE_MANIPULATOR_H_ 46 void add_motor(
unsigned int number,
unsigned int number_device);
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;
50 template <
typename T>
void get_angles_device(std::vector<T>& to)
const;
52 template <
typename T>
void set_angles(std::vector<T>& angles);
53 template <
typename T>
void set_angles_device(std::vector<T>& angles);
62 virtual float angle_OR_to_device(
unsigned int number,
float angle)
const = 0;
69 virtual float angle_device_to_OR(
unsigned int number,
float angle)
const = 0;
83 OpenRaveManipulator::get_angles(std::vector<T>& to)
const 86 for (
unsigned int i=0; i<__motors.size(); i++) {
87 to[__motors[i].no] = (T)__motors[i].angle;
96 OpenRaveManipulator::get_angles_device(std::vector<T>& to)
const 98 std::vector<float> tmp;
100 angles_or_to_device(tmp, to);
108 template <
typename T_from,
typename T_to>
110 OpenRaveManipulator::angles_or_to_device(std::vector<T_from>& from, std::vector<T_to>&to)
const 112 to.resize(__cnt_device);
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]);
124 template <
typename T>
126 OpenRaveManipulator::set_angles(std::vector<T>& angles)
128 if( angles.size() < __motors.size() ) {
129 angles.reserve(__motors.size());
131 for (
unsigned int i=0; i<__motors.size(); i++) {
132 __motors[i].angle = (float)angles[__motors[i].no];
139 template <
typename T>
141 OpenRaveManipulator::set_angles_device(std::vector<T>& angles)
143 if( angles.size() < __motors.size() ) {
144 angles.reserve(__motors.size());
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]);
Fawkes library namespace.
unsigned int __cnt_device
number of motors on real device
unsigned int __cnt
number of motors on OpenRAVE model
Class containing information about all manipulator motors.
std::vector< motor_t > __motors
vector of motors