Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * manipulator.h - Fawkes to OpenRAVE Manipulator Data 00004 * 00005 * Created: Thu Sep 16 14:50:34 2010 00006 * Copyright 2010 Bahram Maleki-Fard, AllemaniACs RoboCup Team 00007 * 00008 ****************************************************************************/ 00009 00010 /* This program is free software; you can redistribute it and/or modify 00011 * it under the terms of the GNU General Public License as published by 00012 * the Free Software Foundation; either version 2 of the License, or 00013 * (at your option) any later version. 00014 * 00015 * This program is distributed in the hope that it will be useful, 00016 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00017 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00018 * GNU Library General Public License for more details. 00019 * 00020 * Read the full text in the LICENSE.GPL file in the doc directory. 00021 */ 00022 00023 #ifndef __PLUGINS_OPENRAVE_MANIPULATOR_H_ 00024 #define __PLUGINS_OPENRAVE_MANIPULATOR_H_ 00025 00026 #include "types.h" 00027 00028 #include <vector> 00029 00030 namespace fawkes { 00031 #if 0 /* just to make Emacs auto-indent happy */ 00032 } 00033 #endif 00034 00035 class OpenRaveManipulator 00036 { 00037 public: 00038 OpenRaveManipulator(unsigned int count, unsigned int count_device); 00039 virtual ~OpenRaveManipulator(); 00040 00041 virtual void add_motor(unsigned int number, unsigned int number_device); 00042 00043 template <typename T_from, typename T_to> void angles_or_to_device(std::vector<T_from>& from, std::vector<T_to>& to) const; 00044 template <typename T> void get_angles(std::vector<T>& to) const; // angles of OpenRAVE model 00045 template <typename T> void get_angles_device(std::vector<T>& to) const; // angles of real device 00046 00047 template <typename T> void set_angles(std::vector<T>& angles); 00048 template <typename T> void set_angles_device(std::vector<T>& angles); 00049 00050 00051 protected: 00052 virtual float angle_OR_to_device(unsigned int number, float angle) const; 00053 virtual float angle_device_to_OR(unsigned int number, float angle) const; 00054 00055 std::vector<motor_t> __motors; /**< vector of motors */ 00056 unsigned int __cnt; /**< number of motors on OpenRAVE model */ 00057 unsigned int __cnt_device; /**< number of motors on real device */ 00058 }; 00059 00060 00061 /* ########## getter ########## */ 00062 /** Get motor angles of OpenRAVE model 00063 * @param to target tvector of angles 00064 */ 00065 template <typename T> 00066 void 00067 OpenRaveManipulator::get_angles(std::vector<T>& to) const 00068 { 00069 to.resize(__cnt); 00070 for (unsigned int i=0; i<__motors.size(); i++) { 00071 to[__motors[i].no] = (T)__motors[i].angle; 00072 } 00073 } 00074 00075 /** Get motor angles of real device 00076 * @param to target vector of angles 00077 */ 00078 template <typename T> 00079 void 00080 OpenRaveManipulator::get_angles_device(std::vector<T>& to) const 00081 { 00082 std::vector<float> tmp; 00083 get_angles(tmp); 00084 angles_or_to_device(tmp, to); 00085 //to = angles_or_to_device(tmp); 00086 } 00087 00088 /** Transform OpenRAVE motor angles to real device angles 00089 * @param from motor angles of OpenRAVE model 00090 * @param to motor angles of real device 00091 */ 00092 template <typename T_from, typename T_to> 00093 void 00094 OpenRaveManipulator::angles_or_to_device(std::vector<T_from>& from, std::vector<T_to>&to) const 00095 { 00096 to.resize(__cnt_device); 00097 00098 for (unsigned int i=0; i<__motors.size(); i++) { 00099 to[__motors[i].no_device] = (T_to)angle_OR_to_device(__motors[i].no_device, (float)from[__motors[i].no]); 00100 } 00101 } 00102 00103 00104 /* ########## setter ########## */ 00105 /** Set motor angles of OpenRAVE model 00106 * @param angles motor angles 00107 */ 00108 template <typename T> 00109 void 00110 OpenRaveManipulator::set_angles(std::vector<T>& angles) 00111 { 00112 for (unsigned int i=0; i<__motors.size(); i++) { 00113 __motors[i].angle = (float)angles[__motors[i].no]; 00114 } 00115 } 00116 00117 /** Set motor angles of real device 00118 * @param angles motor angles 00119 */ 00120 template <typename T> 00121 void 00122 OpenRaveManipulator::set_angles_device(std::vector<T>& angles) 00123 { 00124 for (unsigned int i=0; i<__motors.size(); i++) { 00125 __motors[i].angle = angle_device_to_OR(__motors[i].no_device, (float)angles[__motors[i].no_device]); 00126 } 00127 } 00128 00129 00130 00131 } // end of namespace fawkes 00132 00133 #endif