Fawkes API  Fawkes Development Version
neuronics_katana.cpp
00001 
00002 /***************************************************************************
00003  *  neuronics_katana.cpp - Fawkes to OpenRAVE Neuronics Katana6M180 Manipulator Data
00004  *
00005  *  Created: Thu Sep 08 15:34:52 2011
00006  *  Copyright  2011  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 "neuronics_katana.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 OpenRaveManipulatorNeuronicsKatana <plugins/openrave/manipulators/neuronics_katana.h>
00035  * Class containing information about all neuronics-katana motors.
00036  * It is the Katana6M180 type. Model is provided by OpenRAVE.
00037  * @author Bahram Maleki-Fard
00038  */
00039 
00040 /** Constructor.
00041  * @param count number of motors of OpenRAVE model
00042  * @param countDevice number of motors of real device
00043  */
00044 OpenRaveManipulatorNeuronicsKatana::OpenRaveManipulatorNeuronicsKatana(unsigned int count, unsigned int countDevice) :
00045   OpenRaveManipulator( count, countDevice )
00046 {
00047 }
00048 
00049 /** Destructor. */
00050 OpenRaveManipulatorNeuronicsKatana::~OpenRaveManipulatorNeuronicsKatana()
00051 {
00052 }
00053 
00054 
00055 
00056 
00057 /* ########## various ######### */
00058 /** Transform single OpenRAVE motor angle to real device angle
00059  * @param number motor number of real device
00060  * @param angle motor angle of OpenRAVE model
00061  * @return transformed angle
00062  */
00063 float
00064 OpenRaveManipulatorNeuronicsKatana::angle_OR_to_device(unsigned int number, float angle) const
00065 {
00066   float _angle;
00067 
00068   switch( number ) {
00069     case 0:
00070       _angle = M_PI - angle;
00071       break;
00072     case 1:
00073       _angle = M_PI/2 - angle;
00074       break;
00075     case 2:
00076       _angle = M_PI - angle;
00077       break;
00078     case 3:
00079       _angle = M_PI - angle;
00080       break;
00081     case 4:
00082       _angle = M_PI - angle;
00083       break;
00084     default:
00085       _angle = angle;
00086       break;
00087   }
00088 
00089   return _angle;
00090 }
00091 
00092 /** Transform single device motor angle to OpenRAVE angle
00093  * @param number motor number of real device
00094  * @param angle motor angle of real device
00095  * @return transformed angle
00096  */
00097 float
00098 OpenRaveManipulatorNeuronicsKatana::angle_device_to_OR(unsigned int number, float angle) const
00099 {
00100   float _angle;
00101 
00102   switch( number ) {
00103     case 0:
00104       _angle = M_PI - angle;
00105       break;
00106     case 1:
00107       _angle = M_PI/2 - angle;
00108       break;
00109     case 2:
00110       _angle = M_PI - angle;
00111       break;
00112     case 3:
00113       _angle = M_PI - angle;
00114       break;
00115     case 4:
00116       _angle = M_PI - angle;
00117       break;
00118     default:
00119       _angle = angle;
00120       break;
00121   }
00122 
00123   return _angle;
00124 }
00125 } // end namespace fawkes