Fawkes API  Fawkes Development Version
katana6M180.cpp
1 
2 /***************************************************************************
3  * katana6M180.cpp - Fawkes to OpenRAVE Katana6M180 Manipulator Data
4  *
5  * Created: Thu Sep 16 14:50:34 2010
6  * Copyright 2010 Bahram Maleki-Fard, AllemaniACs RoboCup Team
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "katana6M180.h"
24 #include "../manipulator.h"
25 
26 #include <cmath>
27 #include <cstdio>
28 
29  namespace fawkes {
30 #if 0 /* just to make Emacs auto-indent happy */
31 }
32 #endif
33 
34 /** @class OpenRaveManipulatorKatana6M180 <plugins/openrave/manipulators/katana6M180.h>
35  * Class containing information about all katana6M180 motors.
36  * @author Bahram Maleki-Fard
37  */
38 
39 /** Constructor.
40  * @param count number of motors of OpenRAVE model
41  * @param countDevice number of motors of real device
42  */
43 OpenRaveManipulatorKatana6M180::OpenRaveManipulatorKatana6M180(unsigned int count, unsigned int countDevice) :
44  OpenRaveManipulator( count, countDevice )
45 {
46 }
47 
48 /** Destructor. */
50 {
51 }
52 
53 
54 
55 
56 /* ########## various ######### */
57 /** Transform single OpenRAVE motor angle to real device angle
58  * @param number motor number of real device
59  * @param angle motor angle of OpenRAVE model
60  * @return transformed angle
61  */
62 float
63 OpenRaveManipulatorKatana6M180::angle_OR_to_device(unsigned int number, float angle) const
64 {
65  float _angle;
66 
67  switch( number ) {
68  case 0:
69  _angle = angle + M_PI;
70  break;
71  case 1:
72  _angle = angle + M_PI/2;
73  break;
74  case 2:
75  _angle = angle + M_PI;
76  break;
77  case 3:
78  _angle = M_PI - angle;
79  break;
80  case 4:
81  _angle = 1.5*M_PI - angle;
82  break;
83  default:
84  _angle = angle;
85  break;
86  }
87 
88  return _angle;
89 }
90 
91 /** Transform single device motor angle to OpenRAVE angle
92  * @param number motor number of real device
93  * @param angle motor angle of real device
94  * @return transformed angle
95  */
96 float
97 OpenRaveManipulatorKatana6M180::angle_device_to_OR(unsigned int number, float angle) const
98 {
99  float _angle;
100 
101  switch( number ) {
102  case 0:
103  _angle = angle - M_PI;
104  break;
105  case 1:
106  _angle = angle - M_PI/2;
107  break;
108  case 2:
109  _angle = angle - M_PI;
110  break;
111  case 3:
112  _angle = M_PI - angle;
113  break;
114  case 4:
115  _angle = 1.5*M_PI - angle;
116  break;
117  default:
118  _angle = angle;
119  break;
120  }
121 
122  return _angle;
123 }
124 } // end namespace fawkes
Fawkes library namespace.
Class containing information about all manipulator motors.
Definition: manipulator.h:35
virtual ~OpenRaveManipulatorKatana6M180()
Destructor.
Definition: katana6M180.cpp:49