Main MRPT website > C++ reference
MRPT logo

CParameterizedTrajectoryGenerator.h

Go to the documentation of this file.
00001 /* +---------------------------------------------------------------------------+
00002    |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
00003    |                                                                           |
00004    |                   http://mrpt.sourceforge.net/                            |
00005    |                                                                           |
00006    |   Copyright (C) 2005-2011  University of Malaga                           |
00007    |                                                                           |
00008    |    This software was written by the Machine Perception and Intelligent    |
00009    |      Robotics Lab, University of Malaga (Spain).                          |
00010    |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
00011    |                                                                           |
00012    |  This file is part of the MRPT project.                                   |
00013    |                                                                           |
00014    |     MRPT is free software: you can redistribute it and/or modify          |
00015    |     it under the terms of the GNU General Public License as published by  |
00016    |     the Free Software Foundation, either version 3 of the License, or     |
00017    |     (at your option) any later version.                                   |
00018    |                                                                           |
00019    |   MRPT is distributed in the hope that it will be useful,                 |
00020    |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
00021    |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
00022    |     GNU General Public License for more details.                          |
00023    |                                                                           |
00024    |     You should have received a copy of the GNU General Public License     |
00025    |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
00026    |                                                                           |
00027    +---------------------------------------------------------------------------+ */
00028 #ifndef CParameterizedTrajectoryGenerator_H
00029 #define CParameterizedTrajectoryGenerator_H
00030 
00031 #include <mrpt/utils/CDynamicGrid.h>
00032 #include <mrpt/utils/CStream.h>
00033 #include <mrpt/reactivenav/link_pragmas.h>
00034 
00035 namespace mrpt
00036 {
00037   namespace reactivenav
00038   {
00039           using namespace mrpt::utils;
00040 
00041         /** This is the base class for any user defined PTG.
00042          *   The class factory interface in CParameterizedTrajectoryGenerator::CreatePTG.
00043          *
00044          * Changes history:
00045          *              - 30/JUN/2004: Creation (JLBC)
00046      *          - 16/SEP/2004: Totally redesigned.
00047          *              - 15/SEP/2005: Totally rewritten again, for integration into MRPT Applications Repository.
00048          *              - 19/JUL/2009: Simplified to use only STL data types, and created the class factory interface.
00049          */
00050         class REACTIVENAV_IMPEXP  CParameterizedTrajectoryGenerator
00051         {
00052         protected:
00053         /** Constructor: possible values in "params":
00054                  *   - ref_distance: The maximum distance in PTGs
00055                  *   - resolution: The cell size
00056                  *   - v_max, w_max: Maximum robot speeds.
00057                  *   - system_TAU, system_DELAY (Optional): Robot dynamics
00058                  */
00059         CParameterizedTrajectoryGenerator(const TParameters<double> &params);
00060 
00061                 /** Initialized the collision grid with the given size and resolution. */
00062         void initializeCollisionsGrid(float refDistance,float resolution);
00063 
00064         public:
00065                 /** The class factory for creating a PTG from a list of parameters "params".
00066                   *  Possible values in "params" are:
00067                   *       - "PTG_type": It's an integer number such as "1" -> CPTG1, "2"-> CPTG2, etc...
00068                   *       - Those explained in CParameterizedTrajectoryGenerator::CParameterizedTrajectoryGenerator
00069                   *       - Those explained in the specific PTG being created (e.g. CPTG1, CPTG2, etc...)
00070                   *
00071                   * \exception std::logic_error On invalid or missing parameters.
00072                   */
00073                 static CParameterizedTrajectoryGenerator * CreatePTG(const TParameters<double> &params);
00074 
00075                 /** Gets a short textual description of the PTG and its parameters.
00076                   */
00077                 virtual std::string getDescription() = 0;
00078 
00079         /** Destructor */
00080         virtual ~CParameterizedTrajectoryGenerator() {  }
00081 
00082         /** The main method: solves the diferential equation to generate a family of parametrical trajectories.
00083                  */
00084         void simulateTrajectories(
00085                                 uint16_t        alfaValuesCount,
00086                                 float                   max_time,
00087                                 float                   max_dist,
00088                                 unsigned int    max_n,
00089                                 float                   diferencial_t,
00090                                 float                   min_dist,
00091                                 float                   *out_max_acc_v = NULL,
00092                                 float                   *out_max_acc_w = NULL);
00093 
00094         /** The "lambda" function, see paper for info. It takes the (a,d) pair that is closest to a given location.
00095                  */
00096         virtual void lambdaFunction( float x, float y, int &out_k, float &out_d );
00097 
00098         /** Converts an "alfa" value (into the discrete set) into a feasible motion command.
00099                  */
00100         void directionToMotionCommand( uint16_t k, float &out_v, float &out_w );
00101 
00102         size_t getAlfaValuesCount() { return alfaValuesCount; };
00103         size_t getPointsCountInCPath_k(uint16_t k) { return CPoints[k].size(); };
00104 
00105         void   getCPointWhen_d_Is ( float d, uint16_t k, float &x, float &y, float &phi, float &t, float *v = NULL, float *w = NULL );
00106 
00107         float  GetCPathPoint_x( uint16_t k, int n ){ return CPoints[k][n].x; }
00108         float  GetCPathPoint_y( uint16_t k, int n ){ return CPoints[k][n].y; }
00109         float  GetCPathPoint_phi(uint16_t k, int n ){ return CPoints[k][n].phi; }
00110         float  GetCPathPoint_t( uint16_t k, int n ){ return CPoints[k][n].t; }
00111         float  GetCPathPoint_d( uint16_t k, int n ){ return CPoints[k][n].dist; }
00112         float  GetCPathPoint_v( uint16_t k, int n ){ return CPoints[k][n].v; }
00113         float  GetCPathPoint_w( uint16_t k, int n ){ return CPoints[k][n].w; }
00114 
00115         void    allocMemForVerticesData( int nVertices );
00116 
00117         void    setVertex_xy( uint16_t k, int n, int m, float x, float y )
00118         {
00119                         vertexPoints_x[k][ n*nVertices + m ] = x;
00120             vertexPoints_y[k][ n*nVertices + m ] = y;
00121         }
00122 
00123         float  getVertex_x( uint16_t k, int n, int m )
00124         {
00125                 int idx = n*nVertices + m;
00126 //                assert( idx>=0);assert(idx<nVertices * nPointsInEachPath[k] );
00127                                 return vertexPoints_x[k][idx];
00128         }
00129 
00130         float  getVertex_y( uint16_t k, int n, int m )
00131         {
00132                 int idx = n*nVertices + m;
00133 //                assert( idx>=0);assert(idx<nVertices * nPointsInEachPath[k] );
00134                                 return vertexPoints_y[k][idx];
00135         }
00136 
00137                 float*  getVertixesArray_x( uint16_t k, int n )
00138                 {
00139                 int idx = n*nVertices;
00140                                 return &vertexPoints_x[k][idx];
00141                 }
00142 
00143                 float*  getVertixesArray_y( uint16_t k, int n )
00144                 {
00145                 int idx = n*nVertices;
00146                                 return &vertexPoints_y[k][idx];
00147                 }
00148 
00149                 unsigned int getVertixesCount() { return nVertices; }
00150 
00151         float   getMax_V() { return V_MAX; }
00152         float   getMax_W() { return W_MAX; }
00153         float   getMax_V_inTPSpace() { return maxV_inTPSpace; }
00154 
00155                 /** Alfa value for the discrete corresponding value.
00156                  * \sa alfa2index
00157                  */
00158                 float  index2alfa( uint16_t k )
00159                 {
00160                         return (float)(M_PI * (-1 + 2 * (k+0.5f) / ((float)alfaValuesCount) ));
00161                 }
00162 
00163                 /** Discrete index value for the corresponding alfa value.
00164                  * \sa index2alfa
00165                  */
00166                 uint16_t  alfa2index( float alfa )
00167                 {
00168                         if (alfa>M_PI)  alfa-=(float)M_2PI;
00169                         if (alfa<-M_PI) alfa+=(float)M_2PI;
00170                         return (uint16_t)(0.5f*(alfaValuesCount*(1+alfa/M_PI) - 1));
00171                 }
00172 
00173                 /** Dump PTG trajectories in files in directory "./PTGs/".
00174                  */
00175         void    debugDumpInFiles(int nPT);
00176 
00177                 /**  A list of all the pairs (alpha,distance) such as the robot collides at that cell.
00178                   *  - map key   (uint16_t) -> alpha value (k)
00179                   *      - map value (float)    -> the MINIMUM distance (d), in meters, associated with that "k".
00180                   */
00181                 typedef std::map<uint16_t,float> TCollisionCell;
00182         
00183                 /** An internal class for storing the collision grid  */
00184                 class REACTIVENAV_IMPEXP CColisionGrid : public mrpt::utils::CDynamicGrid<TCollisionCell>
00185         {
00186           public:
00187                                 CColisionGrid(float x_min, float x_max,float y_min, float y_max, float resolution ) 
00188                                   : mrpt::utils::CDynamicGrid<TCollisionCell>(x_min,x_max,y_min,y_max,resolution)
00189                                 {
00190                                 }
00191                                 virtual ~CColisionGrid() { }
00192 
00193                                 bool    saveToFile( mrpt::utils::CStream* fil );        //!< Save to file, true = OK
00194                 bool    loadFromFile( mrpt::utils::CStream* fil );      //!< Load from file,  true = OK
00195 
00196                 /** For an obstacle (x,y), returns a vector with all the pairs (a,d) such as the robot collides.
00197                                   */
00198                                 const TCollisionCell & getTPObstacle( const float obsX, const float obsY) const;
00199 
00200                 /** Updates the info into a cell: It updates the cell only if the distance d for the path k is lower than the previous value:
00201                                   *     \param cellInfo The index of the cell
00202                                   * \param k The path index (alfa discreet value)
00203                                   * \param d The distance (in TP-Space, range 0..1) to collision.
00204                                   */
00205                 void updateCellInfo( const unsigned int icx, const unsigned int icy, const uint16_t k, const float dist );
00206         };
00207 
00208                 /** The collision grid */
00209                 CColisionGrid   m_collisionGrid;
00210 
00211         // Save/Load from files.
00212                 bool    SaveColGridsToFile( const std::string &filename );      // true = OK
00213         bool    LoadColGridsFromFile( const std::string &filename ); // true = OK
00214 
00215 
00216                 float   refDistance;
00217 
00218                 /** The main method to be implemented in derived classes.
00219                  */
00220         virtual void PTG_Generator( float alfa, float t, float x, float y, float phi, float &v, float &w) = 0;
00221 
00222                 /** To be implemented in derived classes:
00223                   */
00224                 virtual bool PTG_IsIntoDomain( float x, float y ) = 0;
00225 
00226 protected:
00227                 // Given a-priori:
00228         float                   V_MAX, W_MAX;
00229                 float                   TAU, DELAY;
00230 
00231                 float                   turningRadiusReference;
00232 
00233                 /** Specifies the min/max values for "k" and "n", respectively.
00234                   * \sa m_lambdaFunctionOptimizer
00235                   */
00236                 struct TCellForLambdaFunction
00237                 {
00238                         TCellForLambdaFunction()
00239                         {
00240                                 k_min=k_max=n_min=n_max=-1;
00241                         }
00242 
00243                         int             k_min,k_max,n_min,n_max;
00244                 };
00245 
00246                 /** This grid will contain indexes data for speeding-up the default, brute-force lambda function.
00247                   */
00248                 CDynamicGrid<TCellForLambdaFunction>    m_lambdaFunctionOptimizer;
00249 
00250                 // Computed from simulations while generating trajectories:
00251                 float   maxV_inTPSpace;
00252 
00253         bool    flag1, flag2;
00254 
00255                 /** The number of discrete values for ALFA between -PI and +PI.
00256                   */
00257         unsigned int     alfaValuesCount;
00258 
00259                 /** The trajectories in the C-Space:
00260                   */
00261         struct TCPoint
00262         {
00263                         TCPoint(        float   x,
00264                                                 float   y,
00265                                                 float   phi,
00266                                                 float   t,
00267                                                 float   dist,
00268                                                 float   v,
00269                                                 float   w)
00270                         {
00271                                 this->x = x;
00272                                 this->y = y;
00273                                 this->phi = phi;
00274                                 this->t = t;
00275                                 this->dist = dist;
00276                                 this->v = v;
00277                                 this->w = w;
00278                         };
00279 
00280                         float  x, y, phi,t, dist,v,w;
00281         };
00282                 typedef std::vector<TCPoint> TCPointVector;
00283                 std::vector<TCPointVector>      CPoints;
00284 
00285                 /** The shape of the robot along the trajectories:
00286                   */
00287                 std::vector<vector_float> vertexPoints_x,vertexPoints_y;
00288         int     nVertices;
00289 
00290                 /** Free all the memory buffers:
00291                   */
00292         void    FreeMemory();
00293 
00294         }; // end of class
00295 
00296         /** A type for lists of PTGs */
00297         typedef std::vector<mrpt::reactivenav::CParameterizedTrajectoryGenerator*>  TListPTGs;
00298 
00299   }
00300 }
00301 
00302 
00303 #endif
00304 



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:40:17 UTC 2011