24 #ifndef __PLUGINS_KATANA_CONVERSION_H
25 #define __PLUGINS_KATANA_CONVERSION_H
27 #include <utils/math/angle.h>
28 #include <common/MathHelperFunctions.h>
31 using namespace KNI_MHF;
40 radToEnc(std::vector<float>& rad, std::vector<int>& enc)
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));
56 encToRad(std::vector<int>& enc, std::vector<float>& rad)
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));
void radToEnc(std::vector< float > &rad, std::vector< int > &enc)
Convert radian vaulues of katana arm to encoder values.
void encToRad(std::vector< int > &enc, std::vector< float > &rad)
Convert encoder vaulues of katana arm to radian angles.
float deg2rad(float deg)
Convert an angle given in degrees to radians.