Fawkes API  Fawkes Development Version
conversion.h
1 
2 /***************************************************************************
3  * conversion.h - conversion between encoder and radian motor values
4  *
5  * Created: Thu Dec 02 13:51:46 2010
6  * Copyright 2010 Bahram Maleki-Fard
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. A runtime exception applies to
14  * this software (see LICENSE.GPL_WRE file mentioned below for details).
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU Library General Public License for more details.
20  *
21  * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
22  */
23 
24 #ifndef __PLUGINS_KATANA_CONVERSION_H
25 #define __PLUGINS_KATANA_CONVERSION_H
26 
27 #include <utils/math/angle.h>
28 #include <common/MathHelperFunctions.h>
29 
30 #include <vector>
31 using namespace KNI_MHF;
32 
33 namespace fawkes {
34 
35 /** Convert radian vaulues of katana arm to encoder values
36  * @param rad vector with radian angle values
37  * @param enc vector to be filled with encoder values
38  */
39 inline void
40 radToEnc(std::vector<float>& rad, std::vector<int>& enc)
41 {
42  enc.clear();
43 
44  enc.push_back(rad2enc(rad[0], deg2rad( 6.65f), 51200, 31000, 1));
45  enc.push_back(rad2enc(rad[1], deg2rad(124.25f), 94976, -31000, 1));
46  enc.push_back(rad2enc(rad[2], deg2rad( 52.70f), 81408, -31000, -1));
47  enc.push_back(rad2enc(rad[3], deg2rad( 63.50f), 51200, 31000, 1));
48  enc.push_back(rad2enc(rad[4], deg2rad( 8.50f), 51200, 31000, 1));
49 }
50 
51 /** Convert encoder vaulues of katana arm to radian angles.
52  * @param enc vector with encoder values, received from CKatana::getRobotEncoders
53  * @param rad vector to be filled with angle values
54  */
55 inline void
56 encToRad(std::vector<int>& enc, std::vector<float>& rad)
57 {
58  rad.clear();
59 
60  rad.push_back(enc2rad(enc[0], deg2rad( 6.65f), 51200, 31000, 1));
61  rad.push_back(enc2rad(enc[1], deg2rad(124.25f), 94976, -31000, 1));
62  rad.push_back(enc2rad(enc[2], deg2rad( 52.70f), 81408, -31000, -1));
63  rad.push_back(enc2rad(enc[3], deg2rad( 63.50f), 51200, 31000, 1));
64  rad.push_back(enc2rad(enc[4], deg2rad( 8.50f), 51200, 31000, 1));
65 }
66 
67 } // end namespace fawkes
68 
69 #endif
Fawkes library namespace.
void radToEnc(std::vector< float > &rad, std::vector< int > &enc)
Convert radian vaulues of katana arm to encoder values.
Definition: conversion.h:40
void encToRad(std::vector< int > &enc, std::vector< float > &rad)
Convert encoder vaulues of katana arm to radian angles.
Definition: conversion.h:56
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:37