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