$search

kinematics.h

Go to the documentation of this file.

/**************************************************************************
* kinematics.h -
* Kinematics Interface kinematics class for Katana4XX
* Copyright (C) 2007-2008 Neuronics AG
* PKE/UKE 2007, JHA 2008
**************************************************************************/
#ifndef _KINEMATICS_H_
#define _KINEMATICS_H_


#include "roboop/source/robot.h"
#include "roboop/source/utils.h"
#include "roboop/source/quaternion.h"

#include "AnalyticalGuess/include/kinematics6M180.h"
#include "AnalyticalGuess/include/kinematics6M90G.h"
#include "AnalyticalGuess/include/kinematics6M90T.h"

#include "katana_common.h"

#include <vector>
#include <string>


#define mPi 3.14159265358979323846

#define EDOM 33 
#define ERANGE 34 

#define KINLIB_VERSION_MAJOR 1
#define KINLIB_VERSION_MINOR 3
#define KINLIB_VERSION_REVISION 0


template<typename _T> inline _T atan1(_T in1, _T in2) {

        if(in1==0.0 && in2 == 0.0)
                return 0.0;

        if(in1==0.0)
                return (in2<0) ? mPi*3.0/2.0 : mPi/2.0;

        if(in1<0.0)
                return atan(in2/in1)+mPi;

        if( (in1>0.0) && (in2<0.0) )
                return atan(in2/in1)+2.0*mPi;

        return atan(in2/in1);
}


enum katana_type {
K_6M90A_F=0, // Katana 6M90A with Flange
K_6M90A_G=1, // Katana 6M90A with angle Gripper
K_6M180=2, // Katana 6M180 (link length with Flange)
K_6M90B_F=3, // Katana 6M90B with Flange
K_6M90B_G=4 // Katana 6M90B with angle Gripper
};

// multiply lengths to give them more weight relative to the angles in the
// inverse kinematics approximation of roboop --JHA
// [[in analyticalGuess the weight is more equalized, because it is calculated
// in mm (range typacally +/- 200-400) and degree (range typacally +/- 180),
// in roboop SI units are used: m (range typically +/- 0.2-0.4) and radian
// (range typically +/- 3.0). multiplying all lengths by about 10.0 for
// roboop calculations brings back a good precision in position.]]
const double LENGTH_MULTIPLIER = 10.0;

const int MaxDof = 10;

const int Dof_90 = 6;
const int Dof_180 = 5;

//The matrices for the different Katana HW configurations:
// 1    sigma           joint type (revolute=0, prismatic=1)
// 2    theta           Denavit-Hartenberg parameter
// 3    d                       Denavit-Hartenberg parameter
// 4    a                       Denavit-Hartenberg parameter
// 5    alpha           Denavit-Hartenberg parameter
// 6    theta_{min} minimum value of joint variable
// 7    theta_{max} maximum value of joint variable
// 8    theta_{off} joint offset
// 9    m                       mass of the link
// 10   c_x             center of mass along axis $x$
// 11   c_y             center of mass along axis $y$
// 12   c_z             center of mass along axis $z$
// 13   I_{xx}          element $xx$ of the inertia tensor matrix
// 14   I_{xy}          element $xy$ of the inertia tensor matrix
// 15   I_{xz}          element $xz$ of the inertia tensor matrix
// 16   I_{yy}          element $yy$ of the inertia tensor matrix
// 17   I_{yz}          element $yz$ of the inertia tensor matrix
// 18   I_{zz}          element $zz$ of the inertia tensor matrix
// 19   I_m             motor rotor inertia
// 20   Gr                      motor gear ratio
// 21   B                       motor viscous friction coefficient
// 22   C_f             motor Coulomb friction coefficient
// 23   immobile        flag for the kinematics and inverse kinematics
//                                      (if true joint is locked, if false joint is free)

// NewMat Real elements used in Katana6Mxxx_data[] are defined as double
const Real Katana6M90A_F_data[] =
{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,
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,
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,
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,
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,
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};

const Real Katana6M90A_G_data[] =
{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,
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,
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,
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,
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,
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};

const Real Katana6M180_data[] =
{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,
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,
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,
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,
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};

const Real Katana6M90B_F_data[] =
{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,
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,
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,
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,
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,
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};

const Real Katana6M90B_G_data[] =
{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,
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,
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,
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,
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,
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};

// angle offset data
const double Angle_offset_90A_F[] = {-3.025529, 2.168572, -2.221804, 0.462512, 1.422443, 3.656465};
const double Angle_offset_90A_G[] = {-3.025529, 2.168572, -2.221804, 0.462512, 1.422443, 1.570796}; //[5] immobile 1.570796
const double Angle_offset_180[] = {-3.025529, 2.168572, -2.221804, 0.462512, 4.564036};
const double Angle_offset_90B_F[] = {-3.025529, 2.168572, -2.221804, 0.462512, 4.564036, 3.656465};
const double Angle_offset_90B_G[] = {-3.025529, 2.168572, -2.221804, 0.462512, 4.564036, 1.570796}; //[5] immobile 1.570796


// angle range data
const double Angle_range_90A_F[] = {6.03884, 2.443461, 4.363323, 4.014257, 5.969026, 5.742133};
const double Angle_range_90A_G[] = {6.03884, 2.443461, 4.363323, 4.014257, 5.969026, 0.0};
const double Angle_range_180[] = {6.03884, 2.443461, 4.363323, 4.014257, 5.969026};
const double Angle_range_90B_F[] = {6.03884, 2.443461, 4.363323, 4.014257, 5.969026, 5.742133};
const double Angle_range_90B_G[] = {6.03884, 2.443461, 4.363323, 4.014257, 5.969026, 0.0};

// link length data
const double Link_length_90A_F[] = {0.19, 0.139, 0.1473, 0.036};
const double Link_length_90A_G[] = {0.19, 0.139, 0.1473, 0.1505};
const double Link_length_180[] = {0.19, 0.139, 0.1473, 0.041}; // for anglegripper: 0.1555
const double Link_length_90B_F[] = {0.19, 0.139, 0.1473, 0.036};
const double Link_length_90B_G[] = {0.19, 0.139, 0.1473, 0.1505};

const int Encoder_per_cycle[] = {51200, 94976, 47488, 51200, 51200, 51200};
const int Encoder_offset[] = {31000, -31000, -31000, 31000, 31000, 31000};
const int Rotation_direction[] = {-1, -1, 1, 1, 1, 1};



class KinematicsLib{
private:


        int _type;
        bool _matrixInit;
        Matrix _data;
        int _dof;
        int _dom;
        int _epc[MaxDof];
        int _encoderOffset[MaxDof];
        int _rotDir[MaxDof];
        bool _angOffInit;
        double _angleOffset[MaxDof];
        bool _angRanInit;
        double _angleRange[MaxDof];
        double _angleMin[MaxDof];
        double _angleMax[MaxDof];
        double _linkLength[4];
        mRobot _robot;
        int _immobile;
        double _thetaimmobile;
        AnaGuess::Kinematics* _anaGuess;
        bool _initialized;
        double _tcpOffset[4];

        
        int sign(int value);
        int sign(double value);
        #ifdef WIN32
        double round( double x);
        #endif // ifdef WIN32
        int initializeMembers();
        int setAngleMinMax();
        int initDofMat(int dof);
        int angleArrMDH2vecK4D(const double arr[], std::vector<double>* vec);

        int invKin(std::vector<double> pose, std::vector<double> prev,
                        std::vector<double>& angle);
        int invKin_bisec(std::vector<double> pose, std::vector<double> prev,
                        std::vector<double>& conf, int maxBisection);
        int anaGuess(std::vector<double> pose, std::vector<double> prev,
                        std::vector<double>& angle);
        bool checkConfig(std::vector<double> config, std::vector<double> pose,
                        double tol = 0.00001);

public:

        
        KinematicsLib();
        // 3 (K_6M90B_F) or 4 (K_6M90B_G)
        // sets default parameters and initializes kinematics.
        KinematicsLib(int type);
        ~KinematicsLib();       

        
        int setType(int type);

        int setMDH(std::vector<double> theta, std::vector<double> d,
                        std::vector<double> a, std::vector<double> alpha, int typeNr = -1);

        int setLinkLen(std::vector<double> links);

        int setImmob(int immobile);

        int setEPC(std::vector<int> epc);

        int setEncOff(std::vector<int> encOffset);

        int setRotDir(std::vector<int> rotDir);

        int setAngOff(std::vector<double> angleOffset);

        int setAngRan(std::vector<double> angleRange);

        int setTcpOff(std::vector<double> tcpOffset);


        int getType();

        int getMaxDOF();

        int getDOF();

        int getDOM();

        int getMDH(std::vector<double>& theta, std::vector<double>& d,
                        std::vector<double>& a, std::vector<double>& alpha);

        int getImmob();

        int getEPC(std::vector<int>& epc);

        int getEncOff(std::vector<int>& encOffset);

        int getRotDir(std::vector<int>& rotDir);

        int getAngOff(std::vector<double>& angleOffset);

        int getAngRan(std::vector<double>& angleRange);
        
        int getAngStop(std::vector<double>& angleStop);
        
        int getAngMin(std::vector<double>& angleMin);
        
        int getAngMax(std::vector<double>& angleMax);

        int getTcpOff(std::vector<double>& tcpOffset);

        int getVersion(std::vector<int>& version);


        int init();
        
        
        int K4D2mDHAng(std::vector<double> angleK4D, std::vector<double>& angleMDH);

        int mDH2K4DAng(std::vector<double> angleMDH, std::vector<double>& angleK4D);

        
        int enc2rad(std::vector<int> encoders, std::vector<double>& angles);
        
        int rad2enc(std::vector<double> angles, std::vector<int>& encoders);
        

        int directKinematics(std::vector<double> angles, std::vector<double>& pose);
        
        int inverseKinematics(std::vector<double> pose, std::vector<double> prev,
                        std::vector<double>& angles, int maxBisection = 0);
        
};

#endif //_KINEMATICS_H_

---

kni
Author(s): Neuronics AG (see AUTHORS.txt); ROS wrapper by Martin Günther
autogenerated on Tue Mar 5 12:32:59 2013