Fawkes API  Fawkes Development Version
kinova_jaco.cpp
1 
2 /***************************************************************************
3  * kinova_jaco.cpp - Fawkes to OpenRAVE Kinova Jaco Manipulator Data
4  *
5  * Created: Thu Sep 08 15:34:52 2011
6  * Copyright 2011 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 "kinova_jaco.h"
24 #include "../manipulator.h"
25 
26 #include <utils/math/angle.h>
27 #include <cmath>
28 #include <cstdio>
29 
30  namespace fawkes {
31 #if 0 /* just to make Emacs auto-indent happy */
32 }
33 #endif
34 
35 /** @class OpenRaveManipulatorKinovaJaco <plugins/openrave/manipulators/kinova_jaco.h>
36  * Class containing information about all Kinova Jaco motors.
37  * Basic model is provided by OpenRAVE.
38  * @author Bahram Maleki-Fard
39  */
40 
41 /** Constructor.
42  * @param count number of motors of OpenRAVE model
43  * @param countDevice number of motors of real device
44  */
45 OpenRaveManipulatorKinovaJaco::OpenRaveManipulatorKinovaJaco(unsigned int count, unsigned int countDevice) :
46  OpenRaveManipulator( count, countDevice )
47 {
48 }
49 
50 /** Destructor. */
52 {
53 }
54 
55 /** Create a new copy of this OpenRaveManipulator instance.
56  * @return A pointer to the copied instance
57  */
60 {
62 }
63 
64 
65 
66 /* ########## various ######### */
67 float
68 OpenRaveManipulatorKinovaJaco::angle_OR_to_device(unsigned int number, float angle) const
69 {
70  float _angle;
71 
72  switch( number ) {
73  case 0:
74  _angle = rad2deg(1.5*M_PI + angle);
75  break;
76  case 1:
77  _angle = rad2deg(M_PI + angle);
78  break;
79  case 2:
80  _angle = rad2deg(M_PI + angle);
81  break;
82  case 3:
83  _angle = rad2deg(angle);
84  break;
85  case 4:
86  _angle = rad2deg(angle);
87  break;
88  case 5:
89  _angle = rad2deg(M_PI + angle);
90  break;
91 
92  default:
93  _angle = rad2deg(angle);
94  break;
95  }
96 
97  return _angle;
98 }
99 
100 float
101 OpenRaveManipulatorKinovaJaco::angle_device_to_OR(unsigned int number, float angle) const
102 {
103  float _angle;
104 
105  switch( number ) {
106  case 0:
107  _angle = deg2rad(angle) - 1.5*M_PI;
108  break;
109  case 1:
110  _angle = deg2rad(angle) - M_PI;
111  break;
112  case 2:
113  _angle = deg2rad(angle) - M_PI;
114  break;
115  case 3:
116  _angle = deg2rad(angle);
117  break;
118  case 4:
119  _angle = deg2rad(angle);
120  break;
121  case 5:
122  _angle = deg2rad(angle) - M_PI;
123  break;
124  default:
125  _angle = deg2rad(angle);
126  break;
127  }
128 
129  return _angle;
130 }
131 } // end namespace fawkes
virtual OpenRaveManipulatorPtr copy()
Create a new copy of this OpenRaveManipulator instance.
Definition: kinova_jaco.cpp:59
Fawkes library namespace.
OpenRaveManipulatorKinovaJaco(unsigned int count, unsigned int countDevice)
Constructor.
Definition: kinova_jaco.cpp:45
virtual ~OpenRaveManipulatorKinovaJaco()
Destructor.
Definition: kinova_jaco.cpp:51
Class containing information about all manipulator motors.
Definition: manipulator.h:35
float rad2deg(float rad)
Convert an angle given in radians to degrees.
Definition: angle.h:48
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:37