KNIConverter.cpp
Go to the documentation of this file.
1 /*
2  * UOS-ROS packages - Robot Operating System code by the University of Osnabrück
3  * Copyright (C) 2010 University of Osnabrück
4  *
5  * This program is free software; you can redistribute it and/or
6  * modify it under the terms of the GNU General Public License
7  * as published by the Free Software Foundation; either version 2
8  * of the License, or (at your option) any later version.
9  *
10  * This program is distributed in the hope that it will be useful,
11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program; if not, write to the Free Software
17  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
18  *
19  * KNIConverter.cpp
20  *
21  * Created on: 21.01.2011
22  * Author: Martin Günther <mguenthe@uos.de>
23  */
24 
25 #include <katana/KNIConverter.h>
26 
27 namespace katana
28 {
29 
30 KNIConverter::KNIConverter(std::string config_file_path)
31 {
32  bool success = config_.openFile(config_file_path.c_str());
33 
34  if (!success)
35  ROS_ERROR("Could not open config file: %s", config_file_path.c_str());
36 }
37 
39 {
40 }
41 
47 double KNIConverter::angle_rad2enc(int index, double angle)
48 {
49  // Attention:
50  // - if you get TMotInit using config_.getMotInit(index), then angleOffset etc. will be in degrees
51  // - if you get TMotInit using config_.getMOT().arr[index].GetInitialParameters(), all angles will be in radian.
52  //
53  // whoever coded the KNI has some serious issues...
54 
55  const TMotInit param = config_.getMotInit(index);
56 
57  // normalize our input to [-pi, pi)
58  while (angle < -M_PI)
59  angle += 2 * M_PI;
60 
61  while (angle >= M_PI)
62  angle -= 2 * M_PI;
63 
64  // motors 0, 2, 3, 4 had to be shifted by Pi so that the range can be normalized
65  // into a range [-Pi ... 0 ... Pi]; now shift back
66  if (index == 0 || index == 2 || index == 3 || index == 4)
67  angle += M_PI;
68 
69  if (index == (int)GRIPPER_INDEX)
71 
72  return ((deg2rad(param.angleOffset) - angle) * (double)param.encodersPerCycle * (double)param.rotationDirection)
73  / (2.0 * M_PI) + param.encoderOffset;
74 }
75 
76 double KNIConverter::angle_enc2rad(int index, int encoders)
77 {
78  const TMotInit param = config_.getMotInit(index);
79 
80  double result = deg2rad(param.angleOffset) - (((double)encoders - (double)param.encoderOffset) * 2.0 * M_PI)
81  / ((double)param.encodersPerCycle * (double)param.rotationDirection);
82 
83  if (index == (int)GRIPPER_INDEX)
84  {
86  }
87 
88  // motors 0, 2, 3, 4 have to be shifted by Pi so that the range can be normalized
89  // into a range [-Pi ... 0 ... Pi]
90  // (the KNI normalizes these angles to [0, 2 * Pi])
91  if (index == 0 || index == 2 || index == 3 || index == 4)
92  result -= M_PI;
93 
94  // normalize our output to [-pi, pi)
95  while (result < -M_PI)
96  result += 2 * M_PI;
97 
98  while (result >= M_PI)
99  result -= 2 * M_PI;
100 
101  return result;
102 }
103 
108 double KNIConverter::vel_rad2enc(int index, double vel)
109 {
110  return vel_acc_jerk_rad2enc(index, vel) / KNI_TO_ROS_TIME;
111 }
112 
113 double KNIConverter::acc_rad2enc(int index, double acc)
114 {
115  return vel_acc_jerk_rad2enc(index, acc) / pow(KNI_TO_ROS_TIME, 2);
116 }
117 
118 double KNIConverter::jerk_rad2enc(int index, double jerk)
119 {
120  return vel_acc_jerk_rad2enc(index, jerk) / pow(KNI_TO_ROS_TIME, 3);
121 }
122 
123 double KNIConverter::vel_enc2rad(int index, short encoders)
124 {
125  return vel_acc_jerk_enc2rad(index, encoders) * KNI_TO_ROS_TIME;
126 }
127 
128 double KNIConverter::acc_enc2rad(int index, short encoders)
129 {
130  return vel_acc_jerk_enc2rad(index, encoders) * pow(KNI_TO_ROS_TIME, 2);
131 }
132 
133 double KNIConverter::jerk_enc2rad(int index, short encoders)
134 {
135  return vel_acc_jerk_enc2rad(index, encoders) * pow(KNI_TO_ROS_TIME, 3);
136 }
137 
138 double KNIConverter::vel_acc_jerk_rad2enc(int index, double vel_acc_jerk)
139 {
140  const TMotInit param = config_.getMotInit(index);
141 
142  if (index == (int)GRIPPER_INDEX)
143  vel_acc_jerk = vel_acc_jerk / KNI_TO_URDF_GRIPPER_FACTOR;
144 
145  return ((-vel_acc_jerk) * (double)param.encodersPerCycle * (double)param.rotationDirection) / (2.0 * M_PI);
146 }
147 
148 double KNIConverter::vel_acc_jerk_enc2rad(int index, short encoders)
149 {
150  const TMotInit param = config_.getMotInit(index);
151 
152  double result = -((double)encoders * 2.0 * M_PI) / ((double)param.encodersPerCycle * (double)param.rotationDirection);
153 
154  if (index == (int)GRIPPER_INDEX)
155  {
156  result = result * KNI_TO_URDF_GRIPPER_FACTOR;
157  }
158 
159  return result;
160 }
161 
162 double KNIConverter::deg2rad(const double deg)
163 {
164  return deg * (M_PI / 180.0);
165 }
166 
167 }
double vel_enc2rad(int index, short encoders)
bool param(const std::string &param_name, T &param_val, const T &default_val)
static const double KNI_TO_ROS_TIME
KNI time is in 10 milliseconds (most of the time), ROS time is in seconds.
double acc_rad2enc(int index, double acc)
bool openFile(const char *filepath)
int encoderOffset
int encodersPerCycle
double vel_acc_jerk_rad2enc(int index, double vel_acc_jerk)
double vel_acc_jerk_enc2rad(int index, short encoders)
KNI::kmlFactory config_
Definition: KNIConverter.h:54
double deg2rad(const double deg)
double angle_rad2enc(int index, double angle)
int rotationDirection
TMotInit getMotInit(short number)
double angle_enc2rad(int index, int encoders)
double angleOffset
static const double KNI_TO_URDF_GRIPPER_FACTOR
double vel_rad2enc(int index, double vel)
KNIConverter(std::string config_file_path)
double jerk_enc2rad(int index, short encoders)
const size_t GRIPPER_INDEX
The motor index of the gripper (used in all vectors – e.g., motor_angles_)
double acc_enc2rad(int index, short encoders)
#define ROS_ERROR(...)
#define M_PI
static const double KNI_GRIPPER_CLOSED_ANGLE
constants for converting between the KNI gripper angle and the URDF gripper angle ...
double jerk_rad2enc(int index, double jerk)
static const double GRIPPER_CLOSED_ANGLE
Constants for gripper fully open or fully closed (should be equal to the value in the urdf descriptio...


katana
Author(s): Martin Günther
autogenerated on Fri Jan 3 2020 04:01:25