23 #ifndef __PLUGINS_OPENRAVE_MANIPULATOR_H_
24 #define __PLUGINS_OPENRAVE_MANIPULATOR_H_
41 virtual void add_motor(
unsigned int number,
unsigned int number_device);
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;
45 template <
typename T>
void get_angles_device(std::vector<T>& to)
const;
47 template <
typename T>
void set_angles(std::vector<T>& angles);
48 template <
typename T>
void set_angles_device(std::vector<T>& angles);
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;
67 OpenRaveManipulator::get_angles(std::vector<T>& to)
const
70 for (
unsigned int i=0; i<__motors.size(); i++) {
71 to[__motors[i].no] = (T)__motors[i].angle;
80 OpenRaveManipulator::get_angles_device(std::vector<T>& to)
const
82 std::vector<float> tmp;
84 angles_or_to_device(tmp, to);
92 template <
typename T_from,
typename T_to>
94 OpenRaveManipulator::angles_or_to_device(std::vector<T_from>& from, std::vector<T_to>&to)
const
96 to.resize(__cnt_device);
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]);
108 template <
typename T>
110 OpenRaveManipulator::set_angles(std::vector<T>& angles)
112 for (
unsigned int i=0; i<__motors.size(); i++) {
113 __motors[i].angle = (float)angles[__motors[i].no];
120 template <
typename T>
122 OpenRaveManipulator::set_angles_device(std::vector<T>& angles)
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]);
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