KatanaNativeInterface $VERSION$

lmBase.h

Go to the documentation of this file.
00001 /*
00002  *   Katana Native Interface - A C++ interface to the robot arm Katana.
00003  *   Copyright (C) 2005 Neuronics AG
00004  *   Check out the AUTHORS file for detailed contact information.
00005  *
00006  *   This program is free software; you can redistribute it and/or modify
00007  *   it under the terms of the GNU General Public License as published by
00008  *   the Free Software Foundation; either version 2 of the License, or
00009  *   (at your option) any later version.
00010  *
00011  *   This program is distributed in the hope that it will be useful,
00012  *   but WITHOUT ANY WARRANTY; without even the implied warranty of
00013  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014  *   GNU General Public License for more details.
00015  *
00016  *   You should have received a copy of the GNU General Public License
00017  *   along with this program; if not, write to the Free Software
00018  *   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00019  */
00020 
00021 /********************************************************************************/
00022 #ifndef _LMBASE_H_
00023 #define _LMBASE_H_
00024 /********************************************************************************/
00025 
00026 #include "KNI_InvKin/ikBase.h"
00027 #include "common/exception.h"
00028 #include <vector>
00029 /********************************************************************************/
00030 
00031 
00032 //------------------------------------------------------------------------------//
00033 
00036 struct TLM_points {
00037     double pos;  
00038     double time; 
00039 };
00040 
00043 struct TLMtrajectory {
00044     double*             arr_actpos;                     
00045     double*             arr_tarpos;                     
00046     int                 distance;                       
00047     double              time;                           
00048     double              dt;                                     
00049     short               number_of_points;       
00050     TLM_points* points;                         
00051     short**             motors;                         
00052     double**    derivatives;            
00053     double***   coefficients;           
00054     short***    parameters;                     
00055 };
00056 
00059 struct TMLMIP {
00060     short       mlm_intermediate_pos[5];          
00061 };
00062 
00063 
00067 struct TPoint6D {
00068         double X;
00069         double Y;
00070         double Z;
00071         double Al;
00072         double Be;
00073         double Ga;
00074 };
00075 
00076 struct TPoint3D {
00077         double X;
00078         double Y;
00079         double Z;
00080 };
00081 
00082 
00083 struct TBlendtrace {
00084         TPoint3D        p1p2n;
00085         TPoint3D        p2p3n;
00086         TPoint3D        V1A;
00087         TPoint3D        V1B;
00088         TPoint3D        P1A;
00089         TPoint3D        P1B;
00090         TPoint3D        b1;
00091         TPoint3D        b2;
00092         TPoint3D        m1;
00093         TPoint3D        m2;
00094         double          tA;
00095         double          tB;
00096         double          distBA;
00097 };
00098 
00099 struct TSplinepoint {
00100         TPoint6D        point;
00101         double          time;
00102 };
00103 
00107 struct TBLENDtrajectory {
00108         TPoint6D*               referencepoints;
00109         short                   number_of_referencepoints;
00110         TBlendtrace*    blendtrace; 
00111         short                   number_of_blends;
00112         TSplinepoint*   splinepoints;
00113         short                   number_of_splines;
00114         short                   number_of_splinepoints;
00115         double                  tEnd;
00116 
00117 };
00118 
00119 //---------------------------------------------------------------------------//
00120 
00125 
00126 
00129 class JointSpeedException : public Exception {
00130 public:
00131     JointSpeedException() throw(): 
00132         Exception("Joint speed too high", -70) {}
00133 };
00134 
00137 class WaitParameterException : public Exception {
00138 public:
00139     WaitParameterException() throw(): 
00140         Exception("Wait parameter set to false", -71) {}
00141 };
00142 
00146 
00147 //---------------------------------------------------------------------------//
00148 
00153 class DLLDIR_LM CLMBase : public CikBase {
00154 
00155  private:
00156         double _maximumVelocity;
00157         bool   _activatePositionController;
00158         bool   _isInitialized;
00159 
00160 
00161     TLMtrajectory trajectory;
00162     TBLENDtrajectory blendtrajectory;
00163 
00164 
00165 
00166     void fillPoints(double vmax);
00167     void polDeviratives();
00168     void polCoefficients();
00169 
00170     void calcParameters(double* arr_actpos, double* arr_tarpos, double vmax);
00171 
00172 
00183         double totalTime(double distance, double acc, double dec, double vmax);
00184 
00196         double relPosition(double reltime, double distance, double acc, double dec,
00197                 double vmax);
00198         
00215         void splineCoefficients(int steps, double *timearray, double *encoderarray,
00216                 double *arr_p1, double *arr_p2, double *arr_p3, double *arr_p4);
00217         
00229         bool checkJointSpeed(std::vector<int> lastsolution,
00230                 std::vector<int> solution, double time);
00231 
00232 //     TRetBLEND blend_calculate_blendtraces(double amax, double vmax, long tightness);
00233 //     TRetBLEND blend_fill_splinepoints(double kappa, double amax, double vmax);
00234 //     double blend_Ivontacc(double tp, double dist, double amax, double vmax);
00235 //     TRetBLEND blend_Ivontdec(double tp, double dist, double amax, double vmax, double* I);
00236 
00237  public:
00238 
00239     CLMBase() : _maximumVelocity(20), _activatePositionController(true), _isInitialized(false) {}
00240 
00244     void initLM();
00245 
00247     void movLM(double X, double Y, double Z, 
00248                  double Al, double Be, double Ga,      
00249                  bool exactflag, double vmax, bool wait=true, int tolerance = 100, long timeout = TM_ENDLESS);  
00250 
00254         void movLM2PwithL(double X1, double Y1, double Z1, double Al1, double Be1,
00255                 double Ga1, double X2, double Y2, double Z2, double Al2, double Be2,
00256                 double Ga2, bool exactflag, double vmax, bool wait=false,
00257                 int tolerance = 100, long timeout = TM_ENDLESS);
00258 
00259     void movLM2P4D(double X1, double Y1, double Z1, 
00260                  double Al1, double Be1, double Ga1,      
00261                                   double X2, double Y2, double Z2, 
00262                  double Al2, double Be2, double Ga2, 
00263                                   bool exactflag, double vmax, bool wait=false, int tolerance = 100, long timeout = TM_ENDLESS);        
00264 
00291     void movLM2P(double X1, double Y1, double Z1, double Al1, double Be1,
00292         double Ga1, double X2, double Y2, double Z2, double Al2, double Be2,
00293         double Ga2, bool exactflag, double vmax, bool wait=true,
00294         int tolerance = 100, long timeout = TM_ENDLESS);
00295 
00296         void   setMaximumLinearVelocity(double maximumVelocity);
00297         double getMaximumLinearVelocity() const;
00298 
00302         void setActivatePositionController(bool activate);
00303 
00306         bool getActivatePositionController();
00307 
00309         void moveRobotLinearTo(
00310                 double x,   double y,     double z, 
00311                 double phi, double theta, double psi,
00312                 bool   waitUntilReached = true, 
00313                 int    waitTimeout = TM_ENDLESS);
00314 
00319         void moveRobotLinearTo(
00320                 std::vector<double> coordinates,
00321                 bool   waitUntilReached = true, 
00322                 int    waitTimeout = TM_ENDLESS);
00323 
00324 //     TRetBLEND movLMBlend(double X1, double Y1, double Z1, 
00325 //               double Al1, double Be1, double Ga1,      
00326 //                                double X2, double Y2, double Z2, 
00327 //               double Al2, double Be2, double Ga2,
00328 //                                double vmax, long tightness);
00329 
00330 
00331 };
00332 
00333 /********************************************************************************/
00334 #endif //_IKBASE_H_
00335 /********************************************************************************/