lmBase.h

Go to the documentation of this file.

/*
 *   Katana Native Interface - A C++ interface to the robot arm Katana.
 *   Copyright (C) 2005 Neuronics AG
 *   Check out the AUTHORS file for detailed contact information.
 *
 *   This program is free software; you can redistribute it and/or modify
 *   it under the terms of the GNU General Public License as published by
 *   the Free Software Foundation; either version 2 of the License, or
 *   (at your option) any later version.
 *
 *   This program is distributed in the hope that it will be useful,
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *   GNU General Public License for more details.
 *
 *   You should have received a copy of the GNU General Public License
 *   along with this program; if not, write to the Free Software
 *   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

/********************************************************************************/
#ifndef _LMBASE_H_
#define _LMBASE_H_
/********************************************************************************/
#include "common/dllexport.h"
#include "common/Timer.h"
#include "KNI_InvKin/ikBase.h"
#include "common/exception.h"
#include <vector>
#include <cmath>

#include "boost/numeric/ublas/matrix.hpp"
#include "boost/numeric/ublas/vector.hpp"
#include "boost/numeric/ublas/vector_proxy.hpp"
#include "boost/numeric/ublas/triangular.hpp"
#include "boost/numeric/ublas/lu.hpp"
#include "boost/numeric/ublas/io.hpp"

/********************************************************************************/


//---------------------------------------------------------------------------//



class JointSpeedException : public Exception {
public:
    JointSpeedException() throw(): 
        Exception("Joint speed too high", -70) {}
};

class WaitParameterException : public Exception {
public:
    WaitParameterException() throw(): 
        Exception("Wait parameter set to false", -71) {}
};


//---------------------------------------------------------------------------//

class DLLDIR_LM CLMBase : public CikBase {

 private:
        double _maximumVelocity;
        bool   _activatePositionController;


        double totalTime(double distance, double acc, double dec, double vmax);

        double relPosition(double reltime, double distance, double acc, double dec,
                double vmax);
        
        void splineCoefficients(int steps, double *timearray, double *encoderarray,
                double *arr_p1, double *arr_p2, double *arr_p3, double *arr_p4);
        
        bool checkJointSpeed(std::vector<int> lastsolution,
                std::vector<int> solution, double time);
        
        int getSpeed(int distance, int acceleration, int time);


 public:

    CLMBase() : _maximumVelocity(20), _activatePositionController(true) {}

    void movLM(double X, double Y, double Z, 
                 double Al, double Be, double Ga,      
                 bool exactflag, double vmax, bool wait=true, int tolerance = 100, long timeout = TM_ENDLESS);  

    void movLM2P(double X1, double Y1, double Z1, double Al1, double Be1,
        double Ga1, double X2, double Y2, double Z2, double Al2, double Be2,
        double Ga2, bool exactflag, double vmax, bool wait=true,
        int tolerance = 100, long timeout = TM_ENDLESS);

    void movP2P(double X1, double Y1, double Z1, double Ph1, double Th1,
                double Ps1, double X2, double Y2, double Z2, double Ph2, double Th2,
                double Ps2, bool exactflag, double vmax, bool wait=true,
                long timeout = TM_ENDLESS);

        void   setMaximumLinearVelocity(double maximumVelocity);
        double getMaximumLinearVelocity() const;

        void setActivatePositionController(bool activate);

        bool getActivatePositionController();

        void moveRobotLinearTo(
                double x,   double y,     double z, 
                double phi, double theta, double psi,
                bool   waitUntilReached = true, 
                int    waitTimeout = TM_ENDLESS);

    void moveRobotLinearTo(
                std::vector<double> coordinates,
                bool   waitUntilReached = true, 
                int    waitTimeout = TM_ENDLESS);

    void moveRobotTo(double x, double y, double z, 
                  double phi, double theta, double psi,
                  bool waitUntilReached = true, int waitTimeout = TM_ENDLESS);

    void moveRobotTo(std::vector<double> coordinates, bool waitUntilReached = true, int waitTimeout = TM_ENDLESS);

};
/********************************************************************************/
#endif //_IKBASE_H_
/********************************************************************************/