Main MRPT website > C++ reference
MRPT logo
CParameterizedTrajectoryGenerator.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 #ifndef CParameterizedTrajectoryGenerator_H
29 #define CParameterizedTrajectoryGenerator_H
30 
31 #include <mrpt/utils/CDynamicGrid.h>
32 #include <mrpt/utils/CStream.h>
33 #include <mrpt/utils/TParameters.h>
34 #include <mrpt/reactivenav/link_pragmas.h>
35 
36 namespace mrpt
37 {
38  namespace reactivenav
39  {
40  using namespace mrpt::utils;
41 
42  /** This is the base class for any user defined PTG.
43  * The class factory interface in CParameterizedTrajectoryGenerator::CreatePTG.
44  *
45  * Changes history:
46  * - 30/JUN/2004: Creation (JLBC)
47  * - 16/SEP/2004: Totally redesigned.
48  * - 15/SEP/2005: Totally rewritten again, for integration into MRPT Applications Repository.
49  * - 19/JUL/2009: Simplified to use only STL data types, and created the class factory interface.
50  * \ingroup mrpt_reactivenav_grp
51  */
52  class REACTIVENAV_IMPEXP CParameterizedTrajectoryGenerator
53  {
54  public:
55  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
56  protected:
57  /** Constructor: possible values in "params":
58  * - ref_distance: The maximum distance in PTGs
59  * - resolution: The cell size
60  * - v_max, w_max: Maximum robot speeds.
61  * - system_TAU, system_DELAY (Optional): Robot dynamics
62  */
63  CParameterizedTrajectoryGenerator(const TParameters<double> &params);
64 
65  /** Initialized the collision grid with the given size and resolution. */
66  void initializeCollisionsGrid(float refDistance,float resolution);
67 
68  public:
69  /** The class factory for creating a PTG from a list of parameters "params".
70  * Possible values in "params" are:
71  * - "PTG_type": It's an integer number such as "1" -> CPTG1, "2"-> CPTG2, etc...
72  * - Those explained in CParameterizedTrajectoryGenerator::CParameterizedTrajectoryGenerator
73  * - Those explained in the specific PTG being created (e.g. CPTG1, CPTG2, etc...)
74  *
75  * \exception std::logic_error On invalid or missing parameters.
76  */
77  static CParameterizedTrajectoryGenerator * CreatePTG(const TParameters<double> &params);
78 
79  /** Gets a short textual description of the PTG and its parameters.
80  */
81  virtual std::string getDescription() const = 0 ;
82 
83  /** Destructor */
84  virtual ~CParameterizedTrajectoryGenerator() { }
85 
86  /** The main method: solves the diferential equation to generate a family of parametrical trajectories.
87  */
88  void simulateTrajectories(
89  uint16_t alfaValuesCount,
90  float max_time,
91  float max_dist,
92  unsigned int max_n,
93  float diferencial_t,
94  float min_dist,
95  float *out_max_acc_v = NULL,
96  float *out_max_acc_w = NULL);
97 
98  /** The "lambda" function, see paper for info. It takes the (a,d) pair that is closest to a given location.
99  */
100  virtual void lambdaFunction( float x, float y, int &out_k, float &out_d );
101 
102  /** Converts an "alfa" value (into the discrete set) into a feasible motion command.
103  */
104  void directionToMotionCommand( uint16_t k, float &out_v, float &out_w );
105 
106  size_t getAlfaValuesCount() { return alfaValuesCount; };
107  size_t getPointsCountInCPath_k(uint16_t k) { return CPoints[k].size(); };
108 
109  void getCPointWhen_d_Is ( float d, uint16_t k, float &x, float &y, float &phi, float &t, float *v = NULL, float *w = NULL );
110 
111  float GetCPathPoint_x( uint16_t k, int n ){ return CPoints[k][n].x; }
112  float GetCPathPoint_y( uint16_t k, int n ){ return CPoints[k][n].y; }
113  float GetCPathPoint_phi(uint16_t k, int n ){ return CPoints[k][n].phi; }
114  float GetCPathPoint_t( uint16_t k, int n ){ return CPoints[k][n].t; }
115  float GetCPathPoint_d( uint16_t k, int n ){ return CPoints[k][n].dist; }
116  float GetCPathPoint_v( uint16_t k, int n ){ return CPoints[k][n].v; }
117  float GetCPathPoint_w( uint16_t k, int n ){ return CPoints[k][n].w; }
118 
119  void allocMemForVerticesData( int nVertices );
120 
121  void setVertex_xy( uint16_t k, int n, int m, float x, float y )
122  {
123  vertexPoints_x[k][ n*nVertices + m ] = x;
124  vertexPoints_y[k][ n*nVertices + m ] = y;
125  }
126 
127  float getVertex_x( uint16_t k, int n, int m )
128  {
129  int idx = n*nVertices + m;
130 // assert( idx>=0);assert(idx<nVertices * nPointsInEachPath[k] );
131  return vertexPoints_x[k][idx];
132  }
133 
134  float getVertex_y( uint16_t k, int n, int m )
135  {
136  int idx = n*nVertices + m;
137 // assert( idx>=0);assert(idx<nVertices * nPointsInEachPath[k] );
138  return vertexPoints_y[k][idx];
139  }
140 
141  float* getVertixesArray_x( uint16_t k, int n )
142  {
143  int idx = n*nVertices;
144  return &vertexPoints_x[k][idx];
145  }
146 
147  float* getVertixesArray_y( uint16_t k, int n )
148  {
149  int idx = n*nVertices;
150  return &vertexPoints_y[k][idx];
151  }
152 
153  unsigned int getVertixesCount() { return nVertices; }
154 
155  float getMax_V() { return V_MAX; }
156  float getMax_W() { return W_MAX; }
157  float getMax_V_inTPSpace() { return maxV_inTPSpace; }
158 
159  /** Alfa value for the discrete corresponding value.
160  * \sa alfa2index
161  */
162  float index2alfa( uint16_t k )
163  {
164  return (float)(M_PI * (-1 + 2 * (k+0.5f) / ((float)alfaValuesCount) ));
165  }
166 
167  /** Discrete index value for the corresponding alfa value.
168  * \sa index2alfa
169  */
170  uint16_t alfa2index( float alfa )
171  {
172  if (alfa>M_PI) alfa-=(float)M_2PI;
173  if (alfa<-M_PI) alfa+=(float)M_2PI;
174  return (uint16_t)(0.5f*(alfaValuesCount*(1+alfa/M_PI) - 1));
175  }
176 
177  /** Dump PTG trajectories in files in directory "./PTGs/".
178  */
179  void debugDumpInFiles(int nPT);
180 
181  /** A list of all the pairs (alpha,distance) such as the robot collides at that cell.
182  * - map key (uint16_t) -> alpha value (k)
183  * - map value (float) -> the MINIMUM distance (d), in meters, associated with that "k".
184  */
185  typedef std::map<uint16_t,float> TCollisionCell;
186 
187  /** An internal class for storing the collision grid */
188  class REACTIVENAV_IMPEXP CColisionGrid : public mrpt::utils::CDynamicGrid<TCollisionCell>
189  {
190  private:
191  CParameterizedTrajectoryGenerator const * m_parent;
192 
193  public:
194  CColisionGrid(float x_min, float x_max,float y_min, float y_max, float resolution, CParameterizedTrajectoryGenerator* parent )
195  : mrpt::utils::CDynamicGrid<TCollisionCell>(x_min,x_max,y_min,y_max,resolution),
196  m_parent(parent)
197  {
198  }
199  virtual ~CColisionGrid() { }
200 
201  bool saveToFile( mrpt::utils::CStream* fil ); //!< Save to file, true = OK
202  bool loadFromFile( mrpt::utils::CStream* fil ); //!< Load from file, true = OK
203 
204  /** For an obstacle (x,y), returns a vector with all the pairs (a,d) such as the robot collides.
205  */
206  const TCollisionCell & getTPObstacle( const float obsX, const float obsY) const;
207 
208  /** 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:
209  * \param cellInfo The index of the cell
210  * \param k The path index (alfa discreet value)
211  * \param d The distance (in TP-Space, range 0..1) to collision.
212  */
213  void updateCellInfo( const unsigned int icx, const unsigned int icy, const uint16_t k, const float dist );
214  };
215 
216  /** The collision grid */
217  CColisionGrid m_collisionGrid;
218 
219  // Save/Load from files.
220  bool SaveColGridsToFile( const std::string &filename ); // true = OK
221  bool LoadColGridsFromFile( const std::string &filename ); // true = OK
222 
223 
224  float refDistance;
225 
226  /** The main method to be implemented in derived classes.
227  */
228  virtual void PTG_Generator( float alfa, float t, float x, float y, float phi, float &v, float &w) = 0;
229 
230  /** To be implemented in derived classes:
231  */
232  virtual bool PTG_IsIntoDomain( float x, float y ) = 0;
233 
234 protected:
235  // Given a-priori:
236  float V_MAX, W_MAX;
237  float TAU, DELAY;
238 
239  float turningRadiusReference;
240 
241  /** Specifies the min/max values for "k" and "n", respectively.
242  * \sa m_lambdaFunctionOptimizer
243  */
244  struct TCellForLambdaFunction
245  {
246  TCellForLambdaFunction()
247  {
248  k_min=k_max=n_min=n_max=-1;
249  }
250 
251  int k_min,k_max,n_min,n_max;
252  };
253 
254  /** This grid will contain indexes data for speeding-up the default, brute-force lambda function.
255  */
256  CDynamicGrid<TCellForLambdaFunction> m_lambdaFunctionOptimizer;
257 
258  // Computed from simulations while generating trajectories:
259  float maxV_inTPSpace;
260 
261  bool flag1, flag2;
262 
263  /** The number of discrete values for ALFA between -PI and +PI.
264  */
265  unsigned int alfaValuesCount;
266 
267  /** The trajectories in the C-Space:
268  */
269  struct TCPoint
270  {
271  TCPoint( float x,
272  float y,
273  float phi,
274  float t,
275  float dist,
276  float v,
277  float w)
278  {
279  this->x = x;
280  this->y = y;
281  this->phi = phi;
282  this->t = t;
283  this->dist = dist;
284  this->v = v;
285  this->w = w;
286  };
287 
288  float x, y, phi,t, dist,v,w;
289  };
290  typedef std::vector<TCPoint> TCPointVector;
291  std::vector<TCPointVector> CPoints;
292 
293  /** The shape of the robot along the trajectories:
294  */
295  std::vector<vector_float> vertexPoints_x,vertexPoints_y;
296  int nVertices;
297 
298  /** Free all the memory buffers:
299  */
300  void FreeMemory();
301 
302  }; // end of class
303 
304  /** A type for lists of PTGs */
305  typedef std::vector<mrpt::reactivenav::CParameterizedTrajectoryGenerator*> TListPTGs;
306 
307  }
308 }
309 
310 
311 #endif
312 


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013