KNIConverter.cpp
Go to the documentation of this file.
00001 /*
00002  * UOS-ROS packages - Robot Operating System code by the University of Osnabrück
00003  * Copyright (C) 2010  University of Osnabrück
00004  *
00005  * This program is free software; you can redistribute it and/or
00006  * modify it under the terms of the GNU General Public License
00007  * as published by the Free Software Foundation; either version 2
00008  * of the License, or (at your option) any later version.
00009  *
00010  * This program is distributed in the hope that it will be useful,
00011  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00012  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00013  * GNU General Public License for more details.
00014  *
00015  * You should have received a copy of the GNU General Public License
00016  * along with this program; if not, write to the Free Software
00017  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
00018  *
00019  * KNIConverter.cpp
00020  *
00021  *  Created on: 21.01.2011
00022  *      Author: Martin Günther <mguenthe@uos.de>
00023  */
00024 
00025 #include <katana/KNIConverter.h>
00026 
00027 namespace katana
00028 {
00029 
00030 KNIConverter::KNIConverter(std::string config_file_path)
00031 {
00032   bool success = config_.openFile(config_file_path.c_str());
00033 
00034   if (!success)
00035     ROS_ERROR("Could not open config file: %s", config_file_path.c_str());
00036 }
00037 
00038 KNIConverter::~KNIConverter()
00039 {
00040 }
00041 
00047 double KNIConverter::angle_rad2enc(int index, double angle)
00048 {
00049   // Attention:
00050   //   - if you get TMotInit using config_.getMotInit(index), then angleOffset etc. will be in degrees
00051   //   - if you get TMotInit using config_.getMOT().arr[index].GetInitialParameters(), all angles will be in radian.
00052   //
00053   // whoever coded the KNI has some serious issues...
00054 
00055   const TMotInit param = config_.getMotInit(index);
00056 
00057   // normalize our input to [-pi, pi)
00058   while (angle < -M_PI)
00059     angle += 2 * M_PI;
00060 
00061   while (angle >= M_PI)
00062     angle -= 2 * M_PI;
00063 
00064   // motors 0, 2, 3, 4 had to be shifted by Pi so that the range can be normalized
00065   // into a range [-Pi ... 0 ... Pi]; now shift back
00066   if (index == 0 || index == 2 || index == 3 || index == 4)
00067     angle += M_PI;
00068 
00069   if (index == (int)GRIPPER_INDEX)
00070     angle = (angle - GRIPPER_CLOSED_ANGLE) / KNI_TO_URDF_GRIPPER_FACTOR + KNI_GRIPPER_CLOSED_ANGLE;
00071 
00072   return ((deg2rad(param.angleOffset) - angle) * (double)param.encodersPerCycle * (double)param.rotationDirection)
00073       / (2.0 * M_PI) + param.encoderOffset;
00074 }
00075 
00076 double KNIConverter::angle_enc2rad(int index, int encoders)
00077 {
00078   const TMotInit param = config_.getMotInit(index);
00079 
00080   double result = deg2rad(param.angleOffset) - (((double)encoders - (double)param.encoderOffset) * 2.0 * M_PI)
00081       / ((double)param.encodersPerCycle * (double)param.rotationDirection);
00082 
00083   if (index == (int)GRIPPER_INDEX)
00084   {
00085     result = (result - KNI_GRIPPER_CLOSED_ANGLE) * KNI_TO_URDF_GRIPPER_FACTOR + GRIPPER_CLOSED_ANGLE;
00086   }
00087 
00088   // motors 0, 2, 3, 4 have to be shifted by Pi so that the range can be normalized
00089   // into a range [-Pi ... 0 ... Pi]
00090   // (the KNI normalizes these angles to [0, 2 * Pi])
00091   if (index == 0 || index == 2 || index == 3 || index == 4)
00092     result -= M_PI;
00093 
00094   // normalize our output to [-pi, pi)
00095   while (result < -M_PI)
00096     result += 2 * M_PI;
00097 
00098   while (result >= M_PI)
00099     result -= 2 * M_PI;
00100 
00101   return result;
00102 }
00103 
00108 double KNIConverter::vel_rad2enc(int index, double vel)
00109 {
00110   return vel_acc_jerk_rad2enc(index, vel) / KNI_TO_ROS_TIME;
00111 }
00112 
00113 double KNIConverter::acc_rad2enc(int index, double acc)
00114 {
00115   return vel_acc_jerk_rad2enc(index, acc) / pow(KNI_TO_ROS_TIME, 2);
00116 }
00117 
00118 double KNIConverter::jerk_rad2enc(int index, double jerk)
00119 {
00120   return vel_acc_jerk_rad2enc(index, jerk) / pow(KNI_TO_ROS_TIME, 3);
00121 }
00122 
00123 double KNIConverter::vel_enc2rad(int index, short encoders)
00124 {
00125   return vel_acc_jerk_enc2rad(index, encoders) * KNI_TO_ROS_TIME;
00126 }
00127 
00128 double KNIConverter::acc_enc2rad(int index, short encoders)
00129 {
00130   return vel_acc_jerk_enc2rad(index, encoders) * pow(KNI_TO_ROS_TIME, 2);
00131 }
00132 
00133 double KNIConverter::jerk_enc2rad(int index, short encoders)
00134 {
00135   return vel_acc_jerk_enc2rad(index, encoders) * pow(KNI_TO_ROS_TIME, 3);
00136 }
00137 
00138 double KNIConverter::vel_acc_jerk_rad2enc(int index, double vel_acc_jerk)
00139 {
00140   const TMotInit param = config_.getMotInit(index);
00141 
00142   if (index == (int)GRIPPER_INDEX)
00143     vel_acc_jerk = vel_acc_jerk / KNI_TO_URDF_GRIPPER_FACTOR;
00144 
00145   return ((-vel_acc_jerk) * (double)param.encodersPerCycle * (double)param.rotationDirection) / (2.0 * M_PI);
00146 }
00147 
00148 double KNIConverter::vel_acc_jerk_enc2rad(int index, short encoders)
00149 {
00150   const TMotInit param = config_.getMotInit(index);
00151 
00152   double result = -((double)encoders * 2.0 * M_PI) / ((double)param.encodersPerCycle * (double)param.rotationDirection);
00153 
00154   if (index == (int)GRIPPER_INDEX)
00155   {
00156     result = result * KNI_TO_URDF_GRIPPER_FACTOR;
00157   }
00158 
00159   return result;
00160 }
00161 
00162 double KNIConverter::deg2rad(const double deg)
00163 {
00164   return deg * (M_PI / 180.0);
00165 }
00166 
00167 }


katana
Author(s): Martin Günther
autogenerated on Thu Aug 27 2015 13:40:35