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 
25 #include "../manipulator.h"
26 
27 #include <utils/math/angle.h>
28 
29 #include <cmath>
30 #include <cstdio>
31 
32 namespace fawkes {
33 
34 /** @class OpenRaveManipulatorKinovaJaco <plugins/openrave/manipulators/kinova_jaco.h>
35  * Class containing information about all Kinova Jaco motors.
36  * Basic model is provided by OpenRAVE.
37  * @author Bahram Maleki-Fard
38  */
39 
40 /** Constructor.
41  * @param count number of motors of OpenRAVE model
42  * @param countDevice number of motors of real device
43  */
45  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 /* ########## various ######### */
65 float
66 OpenRaveManipulatorKinovaJaco::angle_OR_to_device(unsigned int number, float angle) const
67 {
68  float _angle;
69 
70  switch (number) {
71  case 0: _angle = rad2deg(1.5 * M_PI + angle); break;
72  case 1: _angle = rad2deg(M_PI + angle); break;
73  case 2: _angle = rad2deg(M_PI + angle); break;
74  case 3: _angle = rad2deg(angle); break;
75  case 4: _angle = rad2deg(angle); break;
76  case 5: _angle = rad2deg(M_PI + angle); break;
77 
78  default: _angle = rad2deg(angle); break;
79  }
80 
81  return _angle;
82 }
83 
84 float
85 OpenRaveManipulatorKinovaJaco::angle_device_to_OR(unsigned int number, float angle) const
86 {
87  float _angle;
88 
89  switch (number) {
90  case 0: _angle = deg2rad(angle) - 1.5 * M_PI; break;
91  case 1: _angle = deg2rad(angle) - M_PI; break;
92  case 2: _angle = deg2rad(angle) - M_PI; break;
93  case 3: _angle = deg2rad(angle); break;
94  case 4: _angle = deg2rad(angle); break;
95  case 5: _angle = deg2rad(angle) - M_PI; break;
96  default: _angle = deg2rad(angle); break;
97  }
98 
99  return _angle;
100 }
101 } // end namespace fawkes
virtual ~OpenRaveManipulatorKinovaJaco()
Destructor.
Definition: kinova_jaco.cpp:51
virtual OpenRaveManipulatorPtr copy()
Create a new copy of this OpenRaveManipulator instance.
Definition: kinova_jaco.cpp:59
OpenRaveManipulatorKinovaJaco(unsigned int count, unsigned int countDevice)
Constructor.
Definition: kinova_jaco.cpp:44
Class containing information about all manipulator motors.
Definition: manipulator.h:33
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:36
float rad2deg(float rad)
Convert an angle given in radians to degrees.
Definition: angle.h:46