Fawkes API  Fawkes Development Version
katana6M180.cpp
00001 
00002 /***************************************************************************
00003  *  katana6M180.cpp - Fawkes to OpenRAVE Katana6M180 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 #include "katana6M180.h"
00024 #include "../manipulator.h"
00025 
00026 #include <cmath>
00027 #include <cstdio>
00028 
00029  namespace fawkes {
00030 #if 0 /* just to make Emacs auto-indent happy */
00031 }
00032 #endif
00033 
00034 /** @class OpenRaveManipulatorKatana6M180 <plugins/openrave/manipulators/katana6M180.h>
00035  * Class containing information about all katana6M180 motors.
00036  * @author Bahram Maleki-Fard
00037  */
00038 
00039 /** Constructor.
00040  * @param count number of motors of OpenRAVE model
00041  * @param countDevice number of motors of real device
00042  */
00043 OpenRaveManipulatorKatana6M180::OpenRaveManipulatorKatana6M180(unsigned int count, unsigned int countDevice) :
00044   OpenRaveManipulator( count, countDevice )
00045 {
00046 }
00047 
00048 /** Destructor. */
00049 OpenRaveManipulatorKatana6M180::~OpenRaveManipulatorKatana6M180()
00050 {
00051 }
00052 
00053 
00054 
00055 
00056 /* ########## various ######### */
00057 /** Transform single OpenRAVE motor angle to real device angle
00058  * @param number motor number of real device
00059  * @param angle motor angle of OpenRAVE model
00060  * @return transformed angle
00061  */
00062 float
00063 OpenRaveManipulatorKatana6M180::angle_OR_to_device(unsigned int number, float angle) const
00064 {
00065   float _angle;
00066 
00067   switch( number ) {
00068     case 0:
00069       _angle = angle + M_PI;
00070       break;
00071     case 1:
00072       _angle = angle + M_PI/2;
00073       break;
00074     case 2:
00075       _angle = angle + M_PI;
00076       break;
00077     case 3:
00078       _angle = M_PI - angle;
00079       break;
00080     case 4:
00081       _angle = 1.5*M_PI - angle;
00082       break;
00083     default:
00084       _angle = angle;
00085       break;
00086   }
00087 
00088   return _angle;
00089 }
00090 
00091 /** Transform single device motor angle to OpenRAVE angle
00092  * @param number motor number of real device
00093  * @param angle motor angle of real device
00094  * @return transformed angle
00095  */
00096 float
00097 OpenRaveManipulatorKatana6M180::angle_device_to_OR(unsigned int number, float angle) const
00098 {
00099   float _angle;
00100 
00101   switch( number ) {
00102     case 0:
00103       _angle = angle - M_PI;
00104       break;
00105     case 1:
00106       _angle = angle - M_PI/2;
00107       break;
00108     case 2:
00109       _angle = angle - M_PI;
00110       break;
00111     case 3:
00112       _angle = M_PI - angle;
00113       break;
00114     case 4:
00115       _angle = 1.5*M_PI - angle;
00116       break;
00117     default:
00118       _angle = angle;
00119       break;
00120   }
00121 
00122   return _angle;
00123 }
00124 } // end namespace fawkes