Fawkes API  Fawkes Development Version
manipulator.h
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