kni: kinematics.h Source File

Go to the documentation of this file.
00001 /**************************************************************************
00002 * kinematics.h -
00003 * Kinematics Interface kinematics class for Katana4XX
00004 * Copyright (C) 2007-2008 Neuronics AG
00005 * PKE/UKE 2007, JHA 2008
00006 **************************************************************************/
00007 #ifndef _KINEMATICS_H_
00008 #define _KINEMATICS_H_
00009 
00010 
00011 #include "roboop/source/robot.h"
00012 #include "roboop/source/utils.h"
00013 #include "roboop/source/quaternion.h"
00014 
00015 #include "AnalyticalGuess/include/kinematics6M180.h"
00016 #include "AnalyticalGuess/include/kinematics6M90G.h"
00017 #include "AnalyticalGuess/include/kinematics6M90T.h"
00018 
00019 #include "katana_common.h"
00020 
00021 #include <vector>
00022 #include <string>
00023 
00024 
00025 #define mPi 3.14159265358979323846
00026 
00027 #define EDOM 33 
00028 #define ERANGE 34 
00029 
00030 #define KINLIB_VERSION_MAJOR 1
00031 #define KINLIB_VERSION_MINOR 3
00032 #define KINLIB_VERSION_REVISION 0
00033 
00035 
00036 template<typename _T> inline _T atan1(_T in1, _T in2) {
00037 
00038         if(in1==0.0 && in2 == 0.0)
00039                 return 0.0;
00040 
00041         if(in1==0.0)
00042                 return (in2<0) ? mPi*3.0/2.0 : mPi/2.0;
00043 
00044         if(in1<0.0)
00045                 return atan(in2/in1)+mPi;
00046 
00047         if( (in1>0.0) && (in2<0.0) )
00048                 return atan(in2/in1)+2.0*mPi;
00049 
00050         return atan(in2/in1);
00051 }
00052 
00054 
00055 enum katana_type {
00056 K_6M90A_F=0, // Katana 6M90A with Flange
00057 K_6M90A_G=1, // Katana 6M90A with angle Gripper
00058 K_6M180=2, // Katana 6M180 (link length with Flange)
00059 K_6M90B_F=3, // Katana 6M90B with Flange
00060 K_6M90B_G=4 // Katana 6M90B with angle Gripper
00061 };
00062 
00063 // multiply lengths to give them more weight relative to the angles in the
00064 // inverse kinematics approximation of roboop --JHA
00065 // [[in analyticalGuess the weight is more equalized, because it is calculated
00066 // in mm (range typacally +/- 200-400) and degree (range typacally +/- 180),
00067 // in roboop SI units are used: m (range typically +/- 0.2-0.4) and radian
00068 // (range typically +/- 3.0). multiplying all lengths by about 10.0 for
00069 // roboop calculations brings back a good precision in position.]]
00070 const double LENGTH_MULTIPLIER = 10.0;
00071 
00072 const int MaxDof = 10;
00073 
00074 const int Dof_90 = 6;
00075 const int Dof_180 = 5;
00076 
00078 //The matrices for the different Katana HW configurations:
00079 // 1    sigma           joint type (revolute=0, prismatic=1)
00080 // 2    theta           Denavit-Hartenberg parameter
00081 // 3    d                       Denavit-Hartenberg parameter
00082 // 4    a                       Denavit-Hartenberg parameter
00083 // 5    alpha           Denavit-Hartenberg parameter
00084 // 6    theta_{min} minimum value of joint variable
00085 // 7    theta_{max} maximum value of joint variable
00086 // 8    theta_{off} joint offset
00087 // 9    m                       mass of the link
00088 // 10   c_x             center of mass along axis $x$
00089 // 11   c_y             center of mass along axis $y$
00090 // 12   c_z             center of mass along axis $z$
00091 // 13   I_{xx}          element $xx$ of the inertia tensor matrix
00092 // 14   I_{xy}          element $xy$ of the inertia tensor matrix
00093 // 15   I_{xz}          element $xz$ of the inertia tensor matrix
00094 // 16   I_{yy}          element $yy$ of the inertia tensor matrix
00095 // 17   I_{yz}          element $yz$ of the inertia tensor matrix
00096 // 18   I_{zz}          element $zz$ of the inertia tensor matrix
00097 // 19   I_m             motor rotor inertia
00098 // 20   Gr                      motor gear ratio
00099 // 21   B                       motor viscous friction coefficient
00100 // 22   C_f             motor Coulomb friction coefficient
00101 // 23   immobile        flag for the kinematics and inverse kinematics
00102 //                                      (if true joint is locked, if false joint is free)
00103 
00104 // NewMat Real elements used in Katana6Mxxx_data[] are defined as double
00105 const Real Katana6M90A_F_data[] =
00106 {0, 0, 0, 0, 0, -3.025529, 3.013311, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
00107 0, 0, 0, 0, mPi/2.0, -0.274889, 2.168572, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
00108 0, 0, 0, 0.19, 0, -2.221804, 2.141519, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
00109 0, 0, 0, 0.139, 0, -3.551745, 0.462512, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
00110 0, 0, 0.1473, 0, -mPi/2.0, -4.546583, 1.422443, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
00111 0, 0, 0.036, 0, mPi/2.0, -2.085668, 3.656465, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
00112 
00113 const Real Katana6M90A_G_data[] =
00114 {0, 0, 0, 0, 0, -3.025529, 3.013311, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
00115 0, 0, 0, 0, mPi/2.0, -0.274889, 2.168572, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
00116 0, 0, 0, 0.19, 0, -2.221804, 2.141519, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
00117 0, 0, 0, 0.139, 0, -3.551745, 0.462512, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
00118 0, 0, 0.1473, 0, -mPi/2.0, -4.546583, 1.422443, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
00119 0, mPi/2.0, 0.1505, 0, mPi/2.0, -3.141593, 3.141593, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1};
00120 
00121 const Real Katana6M180_data[] =
00122 {0, 0, 0, 0, 0, -3.025529, 3.013311, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
00123 0, 0, 0, 0, mPi/2.0, -0.274889, 2.168572, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
00124 0, 0, 0, 0.19, 0, -2.221804, 2.141519, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
00125 0, 0, 0, 0.139, 0, -3.551745, 0.462512, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
00126 0, 0, 0.1473+0.041, 0, -mPi/2.0, -1.40499, 4.564036, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
00127 
00128 const Real Katana6M90B_F_data[] =
00129 {0, 0, 0, 0, 0, -3.025529, 3.013311, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
00130 0, 0, 0, 0, mPi/2.0, -0.274889, 2.168572, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
00131 0, 0, 0, 0.19, 0, -2.221804, 2.141519, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
00132 0, 0, 0, 0.139, 0, -3.551745, 0.462512, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
00133 0, 0, 0.1473, 0, -mPi/2.0, -1.40499, 4.564036, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
00134 0, 0, 0.036, 0, mPi/2.0, -2.160718, 3.721042, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
00135 
00136 const Real Katana6M90B_G_data[] =
00137 {0, 0, 0, 0, 0, -3.025529, 3.013311, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
00138 0, 0, 0, 0, mPi/2.0, -0.274889, 2.168572, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
00139 0, 0, 0, 0.19, 0, -2.221804, 2.141519, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
00140 0, 0, 0, 0.139, 0, -3.551745, 0.462512, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
00141 0, 0, 0.1473, 0, -mPi/2.0, -1.40499, 4.564036, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
00142 0, mPi/2.0, 0.1505, 0, mPi/2.0, -3.141593, 3.141593, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1};
00143 
00144 // angle offset data
00145 const double Angle_offset_90A_F[] = {-3.025529, 2.168572, -2.221804, 0.462512, 1.422443, 3.656465};
00146 const double Angle_offset_90A_G[] = {-3.025529, 2.168572, -2.221804, 0.462512, 1.422443, 1.570796}; //[5] immobile 1.570796
00147 const double Angle_offset_180[] = {-3.025529, 2.168572, -2.221804, 0.462512, 4.564036};
00148 const double Angle_offset_90B_F[] = {-3.025529, 2.168572, -2.221804, 0.462512, 4.564036, 3.656465};
00149 const double Angle_offset_90B_G[] = {-3.025529, 2.168572, -2.221804, 0.462512, 4.564036, 1.570796}; //[5] immobile 1.570796
00150 
00151 
00152 // angle range data
00153 const double Angle_range_90A_F[] = {6.03884, 2.443461, 4.363323, 4.014257, 5.969026, 5.742133};
00154 const double Angle_range_90A_G[] = {6.03884, 2.443461, 4.363323, 4.014257, 5.969026, 0.0};
00155 const double Angle_range_180[] = {6.03884, 2.443461, 4.363323, 4.014257, 5.969026};
00156 const double Angle_range_90B_F[] = {6.03884, 2.443461, 4.363323, 4.014257, 5.969026, 5.742133};
00157 const double Angle_range_90B_G[] = {6.03884, 2.443461, 4.363323, 4.014257, 5.969026, 0.0};
00158 
00159 // link length data
00160 const double Link_length_90A_F[] = {0.19, 0.139, 0.1473, 0.036};
00161 const double Link_length_90A_G[] = {0.19, 0.139, 0.1473, 0.1505};
00162 const double Link_length_180[] = {0.19, 0.139, 0.1473, 0.041}; // for anglegripper: 0.1555
00163 const double Link_length_90B_F[] = {0.19, 0.139, 0.1473, 0.036};
00164 const double Link_length_90B_G[] = {0.19, 0.139, 0.1473, 0.1505};
00165 
00167 const int Encoder_per_cycle[] = {51200, 94976, 47488, 51200, 51200, 51200};
00169 const int Encoder_offset[] = {31000, -31000, -31000, 31000, 31000, 31000};
00171 const int Rotation_direction[] = {-1, -1, 1, 1, 1, 1};
00172 
00173 
00175 
00178 class KinematicsLib{
00179 private:
00180 
00182 
00184         int _type;
00186         bool _matrixInit;
00187         Matrix _data;
00189         int _dof;
00191         int _dom;
00193         int _epc[MaxDof];
00195         int _encoderOffset[MaxDof];
00197         int _rotDir[MaxDof];
00199         bool _angOffInit;
00200         double _angleOffset[MaxDof];
00202         bool _angRanInit;
00203         double _angleRange[MaxDof];
00205         double _angleMin[MaxDof];
00207         double _angleMax[MaxDof];
00209         double _linkLength[4];
00211         mRobot _robot;
00213         int _immobile;
00214         double _thetaimmobile;
00216         AnaGuess::Kinematics* _anaGuess;
00218         bool _initialized;
00220         double _tcpOffset[4];
00221 
00223         
00224         int sign(int value);
00225         int sign(double value);
00226         #ifdef WIN32
00227         double round( double x);
00228         #endif // ifdef WIN32
00229         int initializeMembers();
00230         int setAngleMinMax();
00231         int initDofMat(int dof);
00232         int angleArrMDH2vecK4D(const double arr[], std::vector<double>* vec);
00233 
00234         int invKin(std::vector<double> pose, std::vector<double> prev,
00235                         std::vector<double>& angle);
00236         int invKin_bisec(std::vector<double> pose, std::vector<double> prev,
00237                         std::vector<double>& conf, int maxBisection);
00238         int anaGuess(std::vector<double> pose, std::vector<double> prev,
00239                         std::vector<double>& angle);
00240         bool checkConfig(std::vector<double> config, std::vector<double> pose,
00241                         double tol = 0.00001);
00242 
00243 public:
00244 
00246         
00248         KinematicsLib();
00250         // 3 (K_6M90B_F) or 4 (K_6M90B_G)
00251         // sets default parameters and initializes kinematics.
00252         KinematicsLib(int type);
00254         ~KinematicsLib();       
00255 
00257         
00273         int setType(int type);
00274 
00291         int setMDH(std::vector<double> theta, std::vector<double> d,
00292                         std::vector<double> a, std::vector<double> alpha, int typeNr = -1);
00293 
00305         int setLinkLen(std::vector<double> links);
00306 
00316         int setImmob(int immobile);
00317 
00326         int setEPC(std::vector<int> epc);
00327 
00336         int setEncOff(std::vector<int> encOffset);
00337 
00348         int setRotDir(std::vector<int> rotDir);
00349 
00358         int setAngOff(std::vector<double> angleOffset);
00359 
00369         int setAngRan(std::vector<double> angleRange);
00370 
00379         int setTcpOff(std::vector<double> tcpOffset);
00380 
00382 
00391         int getType();
00392 
00399         int getMaxDOF();
00400 
00407         int getDOF();
00408 
00415         int getDOM();
00416 
00426         int getMDH(std::vector<double>& theta, std::vector<double>& d,
00427                         std::vector<double>& a, std::vector<double>& alpha);
00428 
00433         int getImmob();
00434 
00440         int getEPC(std::vector<int>& epc);
00441 
00447         int getEncOff(std::vector<int>& encOffset);
00448 
00454         int getRotDir(std::vector<int>& rotDir);
00455 
00461         int getAngOff(std::vector<double>& angleOffset);
00462 
00468         int getAngRan(std::vector<double>& angleRange);
00469         
00475         int getAngStop(std::vector<double>& angleStop);
00476         
00482         int getAngMin(std::vector<double>& angleMin);
00483         
00489         int getAngMax(std::vector<double>& angleMax);
00490 
00496         int getTcpOff(std::vector<double>& tcpOffset);
00497 
00503         int getVersion(std::vector<int>& version);
00504 
00506 
00515         int init();
00516         
00518         
00527         int K4D2mDHAng(std::vector<double> angleK4D, std::vector<double>& angleMDH);
00528 
00537         int mDH2K4DAng(std::vector<double> angleMDH, std::vector<double>& angleK4D);
00538 
00540         
00547         int enc2rad(std::vector<int> encoders, std::vector<double>& angles);
00548         
00555         int rad2enc(std::vector<double> angles, std::vector<int>& encoders);
00556         
00558 
00565         int directKinematics(std::vector<double> angles, std::vector<double>& pose);
00566         
00574         int inverseKinematics(std::vector<double> pose, std::vector<double> prev,
00575                         std::vector<double>& angles, int maxBisection = 0);
00576         
00577 };
00578 
00579 #endif //_KINEMATICS_H_