Main MRPT website > C++ reference for MRPT 1.4.0
CParameterizedTrajectoryGenerator.h
Go to the documentation of this file.
1/* +---------------------------------------------------------------------------+
2 | Mobile Robot Programming Toolkit (MRPT) |
3 | http://www.mrpt.org/ |
4 | |
5 | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file |
6 | See: http://www.mrpt.org/Authors - All rights reserved. |
7 | Released under BSD License. See details in http://www.mrpt.org/License |
8 +---------------------------------------------------------------------------+ */
9#ifndef CParameterizedTrajectoryGenerator_H
10#define CParameterizedTrajectoryGenerator_H
11
13#include <mrpt/math/CPolygon.h>
14#include <mrpt/utils/CStream.h>
17#include <mrpt/utils/mrpt_stdint.h> // compiler-independent version of "stdint.h"
18#include <limits> // numeric_limits
19
20namespace mrpt
21{
22 namespace opengl { class CSetOfLines; }
23
24 namespace nav
25 {
26 /** \defgroup nav_tpspace TP-Space and PTG classes
27 * \ingroup mrpt_nav_grp
28 */
29
30 /** Trajectory points in C-Space \sa CParameterizedTrajectoryGenerator */
32 {
34 TCPoint(const float x_,const float y_,const float phi_,
35 const float t_,const float dist_,
36 const float v_,const float w_) :
37 x(x_), y(y_), phi(phi_), t(t_), dist(dist_), v(v_), w(w_)
38 {}
39 float x, y, phi,t, dist,v,w;
40 };
41 typedef std::vector<TCPoint> TCPointVector;
42
43 /** This is the base class for any user-defined PTG.
44 * The class factory interface in CParameterizedTrajectoryGenerator::CreatePTG.
45 *
46 * Papers:
47 * - J.L. Blanco, J. Gonzalez-Jimenez, J.A. Fernandez-Madrigal, "Extending Obstacle Avoidance Methods through Multiple Parameter-Space Transformations", Autonomous Robots, vol. 24, no. 1, 2008. http://ingmec.ual.es/~jlblanco/papers/blanco2008eoa_DRAFT.pdf
48 *
49 * Changes history:
50 * - 30/JUN/2004: Creation (JLBC)
51 * - 16/SEP/2004: Totally redesigned.
52 * - 15/SEP/2005: Totally rewritten again, for integration into MRPT Applications Repository.
53 * - 19/JUL/2009: Simplified to use only STL data types, and created the class factory interface.
54 * \ingroup nav_tpspace
55 */
57 {
58 public:
60 protected:
61 /** Constructor: possible values in "params":
62 * - ref_distance: The maximum distance in PTGs
63 * - resolution: The cell size
64 * - v_max, w_max: Maximum robot speeds.
65 *
66 * See docs of derived classes for additional parameters:
67 */
69
70 /** Initialized the collision grid with the given size and resolution. */
71 void initializeCollisionsGrid(float refDistance,float resolution);
72
73 public:
74 /** The class factory for creating a PTG from a list of parameters "params".
75 * Possible values in "params" are:
76 * - "PTG_type": It's an integer number such as "1" -> CPTG1, "2"-> CPTG2, etc...
77 * - Those explained in CParameterizedTrajectoryGenerator::CParameterizedTrajectoryGenerator
78 * - Those explained in the specific PTG being created (e.g. CPTG1, CPTG2, etc...)
79 *
80 * \exception std::logic_error On invalid or missing parameters.
81 */
83
84 /** Gets a short textual description of the PTG and its parameters */
85 virtual std::string getDescription() const = 0 ;
86
87 /** Destructor */
89
90 /** The main method: solves the diferential equation to generate a family of parametrical trajectories.
91 */
93 uint16_t alphaValuesCount,
94 float max_time,
95 float max_dist,
96 unsigned int max_n,
97 float diferencial_t,
98 float min_dist,
99 float *out_max_acc_v = NULL,
100 float *out_max_acc_w = NULL);
101
102 /** Saves the simulated trajectories and other parameters to a target stream */
104 /** Loads the simulated trajectories and other parameters from a target stream. \return The PTG textual description */
105 virtual std::string loadTrajectories( mrpt::utils::CStream &in );
106
107 /** Computes the closest (alpha,d) TP coordinates of the trajectory point closest to the Workspace (WS) Cartesian coordinates (x,y).
108 * \param[in] x X coordinate of the query point.
109 * \param[in] y Y coordinate of the query point.
110 * \param[out] out_k Trajectory parameter index (discretized alpha value, 0-based index).
111 * \param[out] out_d Trajectory distance, normalized such that D_max becomes 1.
112 *
113 * \return true if the distance between (x,y) and the actual trajectory point is below the given tolerance (in meters).
114 * \note The default implementation in CParameterizedTrajectoryGenerator relies on a look-up-table. Derived classes may redefine this to closed-form expressions, when they exist.
115 */
116 virtual bool inverseMap_WS2TP(float x, float y, int &out_k, float &out_d, float tolerance_dist = 0.10f) const;
117
118 /** The "lambda" function, see paper for info. It takes the (a,d) pair that is closest to a given location. */
120 "Use inverseMap_WS2TP() instead", \
121 void lambdaFunction( float x, float y, int &out_k, float &out_d ) \
122 );
123
124 /** Converts an "alpha" value (into the discrete set) into a feasible motion command.
125 */
126 void directionToMotionCommand( uint16_t k, float &out_v, float &out_w );
127
128 uint16_t getAlfaValuesCount() const { return m_alphaValuesCount; };
129 size_t getPointsCountInCPath_k(uint16_t k) const { return CPoints[k].size(); };
130
131 /** Returns the C-Space coordinates (pose) when the robot has transversed a distance \a d along trajectory index \k. Returns (0,0,0) if out of bounds. */
132 void getCPointWhen_d_Is ( float d, uint16_t k, float &x, float &y, float &phi, float &t, float *v = NULL, float *w = NULL );
133
134 float GetCPathPoint_x( uint16_t k, int n ) const { return CPoints[k][n].x; }
135 float GetCPathPoint_y( uint16_t k, int n ) const { return CPoints[k][n].y; }
136 float GetCPathPoint_phi(uint16_t k, int n ) const { return CPoints[k][n].phi; }
137 float GetCPathPoint_t( uint16_t k, int n ) const { return CPoints[k][n].t; }
138 float GetCPathPoint_d( uint16_t k, int n ) const { return CPoints[k][n].dist; }
139 float GetCPathPoint_v( uint16_t k, int n ) const { return CPoints[k][n].v; }
140 float GetCPathPoint_w( uint16_t k, int n ) const { return CPoints[k][n].w; }
141
142 float getMax_V() const { return V_MAX; }
143 float getMax_W() const { return W_MAX; }
144 float getMax_V_inTPSpace() const { return maxV_inTPSpace; }
145
146 /** Alfa value for the discrete corresponding value.
147 * \sa alpha2index
148 */
149 float index2alpha( uint16_t k ) const
150 {
151 return (float)(M_PI * (-1 + 2 * (k+0.5f) / ((float)m_alphaValuesCount) ));
152 }
153
154 /** Discrete index value for the corresponding alpha value.
155 * \sa index2alpha
156 */
157 uint16_t alpha2index( float alpha ) const
158 {
159 if (alpha>M_PI) alpha-=(float)M_2PI;
160 if (alpha<-M_PI) alpha+=(float)M_2PI;
161 return (uint16_t)(0.5f*(m_alphaValuesCount*(1+alpha/M_PI) - 1));
162 }
163
164 /** Dump PTG trajectories in a binary file "./reactivenav.logs/PTGs/PTG%i.dat", with "%i" being the user-supplied parameter "nPT",
165 * and in FIVE text files: "./reactivenav.logs/PTGs/PTG%i_{x,y,phi,t,d}.txt".
166 *
167 * Text files are loadable from MATLAB/Octave, and can be visualized with the script [MRPT_DIR]/scripts/viewPTG.m ,
168 * also online: http://mrpt.googlecode.com/svn/trunk/scripts/viewPTG.m
169 *
170 * \note The directory "./reactivenav.logs/PTGs" will be created if doesn't exist.
171 * \return false on any error writing to disk.
172 */
173 bool debugDumpInFiles(const int nPT);
174
175 /** Returns the representation of one trajectory of this PTG as a 3D OpenGL object (a simple curved line).
176 * \param[in] k The 0-based index of the selected trajectory (discrete "alpha" parameter).
177 * \param[out] gl_obj Output object.
178 * \param[in] decimate_distance Minimum distance between path points (in meters).
179 * \param[in] max_path_distance If >0, cut the path at this distance (in meters).
180 */
182 const uint16_t k,
184 const float decimate_distance = 0.1f,
185 const float max_path_distance = 0.0f) const;
186
187
188 /** A list of all the pairs (alpha,distance) such as the robot collides at that cell.
189 * - map key (uint16_t) -> alpha value (k)
190 * - map value (float) -> the MINIMUM distance (d), in meters, associated with that "k".
191 */
192 //typedef std::map<uint16_t,float> TCollisionCell;
193 typedef std::vector<std::pair<uint16_t,float> > TCollisionCell;
194
195 /** An internal class for storing the collision grid */
197 {
198 private:
200
201 public:
202 CColisionGrid(float x_min, float x_max,float y_min, float y_max, float resolution, CParameterizedTrajectoryGenerator* parent )
203 : mrpt::utils::CDynamicGrid<TCollisionCell>(x_min,x_max,y_min,y_max,resolution),
204 m_parent(parent)
205 {
206 }
207 virtual ~CColisionGrid() { }
208
209 bool saveToFile( mrpt::utils::CStream* fil, const mrpt::math::CPolygon & computed_robotShape ); //!< Save to file, true = OK
210 bool loadFromFile( mrpt::utils::CStream* fil, const mrpt::math::CPolygon & current_robotShape ); //!< Load from file, true = OK
211
212 /** For an obstacle (x,y), returns a vector with all the pairs (a,d) such as the robot collides.
213 */
214 const TCollisionCell & getTPObstacle( const float obsX, const float obsY) const;
215
216 /** 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:
217 * \param cellInfo The index of the cell
218 * \param k The path index (alpha discreet value)
219 * \param d The distance (in TP-Space, range 0..1) to collision.
220 */
221 void updateCellInfo( const unsigned int icx, const unsigned int icy, const uint16_t k, const float dist );
222
223 }; // end of class CColisionGrid
224
225 /** The collision grid */
227
228 // Save/Load from files.
229 bool SaveColGridsToFile( const std::string &filename, const mrpt::math::CPolygon & computed_robotShape ); // true = OK
230 bool LoadColGridsFromFile( const std::string &filename, const mrpt::math::CPolygon & current_robotShape ); // true = OK
231
232
234
235 /** The main method to be implemented in derived classes */
236 virtual void PTG_Generator( float alpha, float t, float x, float y, float phi, float &v, float &w) = 0;
237
238 /** To be implemented in derived classes */
239 virtual bool PTG_IsIntoDomain( float x, float y ) = 0;
240
241protected:
242 /** Protected constructor for CPTG_Dummy; does not init collision grid. Not for normal usage */
243 CParameterizedTrajectoryGenerator() : m_collisionGrid(-1,1,-1,1,0.5,this) { }
244
245 float V_MAX, W_MAX;
247 std::vector<TCPointVector> CPoints;
248
249 /** Specifies the min/max values for "k" and "n", respectively.
250 * \sa m_lambdaFunctionOptimizer
251 */
253 {
255 k_min( std::numeric_limits<uint16_t>::max() ),
256 k_max( std::numeric_limits<uint16_t>::min() ),
257 n_min( std::numeric_limits<uint32_t>::max() ),
258 n_max( std::numeric_limits<uint32_t>::min() )
259 {}
260
263
264 bool isEmpty() const { return k_min==std::numeric_limits<uint16_t>::max(); }
265 };
266
267 /** This grid will contain indexes data for speeding-up the default, brute-force lambda function.
268 */
270
271 // Computed from simulations while generating trajectories:
273
274 /** The number of discrete values for "alpha" between -PI and +PI.
275 */
277
278 /** Free all the memory buffers */
280
281 }; // end of class
283
284 typedef std::vector<mrpt::nav::CParameterizedTrajectoryGenerator*> TListPTGs; //!< A list of PTGs (bare pointers)
285 typedef std::vector<mrpt::nav::CParameterizedTrajectoryGeneratorPtr> TListPTGPtr; //!< A list of PTGs (smart pointers)
286
287
290
291
292 /** A dummy PTG, used mainly to call loadTrajectories() without knowing the exact derived PTG class and still be able to analyze the trajectories. */
294 {
295 public:
296 // See base class docs
298 virtual ~CPTG_Dummy() { }
299 virtual std::string getDescription() const { return m_text_description; }
300 virtual std::string loadTrajectories( mrpt::utils::CStream &in )
301 {
302 m_text_description = CParameterizedTrajectoryGenerator::loadTrajectories(in);
303 return m_text_description;
304 }
305 virtual void PTG_Generator( float alpha, float t, float x, float y, float phi, float &v, float &w) { throw std::runtime_error("Should not call this method in a dummy PTG!"); }
306 virtual bool PTG_IsIntoDomain( float x, float y ) { throw std::runtime_error("Should not call this method in a dummy PTG!"); }
307
308 private:
310 };
311
312 }
313 namespace utils
314 {
315 // Specialization must occur in the same namespace
317 }
318
319}
320
321
322#endif
323
#define MRPT_DECLARE_TTYPENAME_NAMESPACE(_TYPE, __NS)
Definition: TTypeName.h:64
#define M_PI
Definition: bits.h:65
A wrapper of a TPolygon2D class, implementing CSerializable.
Definition: CPolygon.h:26
A dummy PTG, used mainly to call loadTrajectories() without knowing the exact derived PTG class and s...
virtual std::string loadTrajectories(mrpt::utils::CStream &in)
Loads the simulated trajectories and other parameters from a target stream.
virtual bool PTG_IsIntoDomain(float x, float y)
To be implemented in derived classes.
virtual void PTG_Generator(float alpha, float t, float x, float y, float phi, float &v, float &w)
The main method to be implemented in derived classes.
virtual std::string getDescription() const
Gets a short textual description of the PTG and its parameters.
bool saveToFile(mrpt::utils::CStream *fil, const mrpt::math::CPolygon &computed_robotShape)
Save to file, true = OK.
const TCollisionCell & getTPObstacle(const float obsX, const float obsY) const
For an obstacle (x,y), returns a vector with all the pairs (a,d) such as the robot collides.
void updateCellInfo(const unsigned int icx, const unsigned int icy, const uint16_t k, const float dist)
Updates the info into a cell: It updates the cell only if the distance d for the path k is lower than...
bool loadFromFile(mrpt::utils::CStream *fil, const mrpt::math::CPolygon &current_robotShape)
Load from file, true = OK.
CColisionGrid(float x_min, float x_max, float y_min, float y_max, float resolution, CParameterizedTrajectoryGenerator *parent)
This is the base class for any user-defined PTG.
void saveTrajectories(mrpt::utils::CStream &out) const
Saves the simulated trajectories and other parameters to a target stream.
void simulateTrajectories(uint16_t alphaValuesCount, float max_time, float max_dist, unsigned int max_n, float diferencial_t, float min_dist, float *out_max_acc_v=NULL, float *out_max_acc_w=NULL)
The main method: solves the diferential equation to generate a family of parametrical trajectories.
bool SaveColGridsToFile(const std::string &filename, const mrpt::math::CPolygon &computed_robotShape)
uint16_t m_alphaValuesCount
The number of discrete values for "alpha" between -PI and +PI.
void FreeMemory()
Free all the memory buffers.
void renderPathAsSimpleLine(const uint16_t k, mrpt::opengl::CSetOfLines &gl_obj, const float decimate_distance=0.1f, const float max_path_distance=0.0f) const
Returns the representation of one trajectory of this PTG as a 3D OpenGL object (a simple curved line)...
virtual std::string getDescription() const =0
Gets a short textual description of the PTG and its parameters.
virtual void PTG_Generator(float alpha, float t, float x, float y, float phi, float &v, float &w)=0
The main method to be implemented in derived classes.
virtual bool PTG_IsIntoDomain(float x, float y)=0
To be implemented in derived classes.
bool LoadColGridsFromFile(const std::string &filename, const mrpt::math::CPolygon &current_robotShape)
void getCPointWhen_d_Is(float d, uint16_t k, float &x, float &y, float &phi, float &t, float *v=NULL, float *w=NULL)
Returns the C-Space coordinates (pose) when the robot has transversed a distance d along trajectory i...
void initializeCollisionsGrid(float refDistance, float resolution)
Initialized the collision grid with the given size and resolution.
CParameterizedTrajectoryGenerator(const mrpt::utils::TParameters< double > &params)
Constructor: possible values in "params":
void directionToMotionCommand(uint16_t k, float &out_v, float &out_w)
Converts an "alpha" value (into the discrete set) into a feasible motion command.
virtual bool inverseMap_WS2TP(float x, float y, int &out_k, float &out_d, float tolerance_dist=0.10f) const
Computes the closest (alpha,d) TP coordinates of the trajectory point closest to the Workspace (WS) C...
CParameterizedTrajectoryGenerator()
Protected constructor for CPTG_Dummy; does not init collision grid.
uint16_t alpha2index(float alpha) const
Discrete index value for the corresponding alpha value.
mrpt::utils::CDynamicGrid< TCellForLambdaFunction > m_lambdaFunctionOptimizer
This grid will contain indexes data for speeding-up the default, brute-force lambda function.
virtual std::string loadTrajectories(mrpt::utils::CStream &in)
Loads the simulated trajectories and other parameters from a target stream.
bool debugDumpInFiles(const int nPT)
Dump PTG trajectories in a binary file "./reactivenav.logs/PTGs/PTG%i.dat", with "%i" being the user-...
std::vector< std::pair< uint16_t, float > > TCollisionCell
A list of all the pairs (alpha,distance) such as the robot collides at that cell.
static CParameterizedTrajectoryGenerator * CreatePTG(const mrpt::utils::TParameters< double > &params)
The class factory for creating a PTG from a list of parameters "params".
float index2alpha(uint16_t k) const
Alfa value for the discrete corresponding value.
A set of independent lines (or segments), one line with its own start and end positions (X,...
Definition: CSetOfLines.h:35
A 2D grid of dynamic size which stores any kind of data at each cell.
Definition: CDynamicGrid.h:41
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:39
EIGEN_STRONG_INLINE const AdjointReturnType t() const
Transpose.
#define MRPT_MAKE_ALIGNED_OPERATOR_NEW
Definition: memory.h:112
#define M_2PI
Definition: mrpt_macros.h:363
#define MRPT_DECLARE_DEPRECATED_FUNCTION(__MSG, __FUNC)
Usage: MRPT_DECLARE_DEPRECATED_FUNCTION("Use XX instead", void myFunc(double));.
Definition: mrpt_macros.h:47
std::vector< mrpt::nav::CParameterizedTrajectoryGenerator * > TListPTGs
A list of PTGs (bare pointers)
std::vector< mrpt::nav::CParameterizedTrajectoryGeneratorPtr > TListPTGPtr
A list of PTGs (smart pointers)
stlplus::smart_ptr< CParameterizedTrajectoryGenerator > CParameterizedTrajectoryGeneratorPtr
Smart pointer to a PTG.
mrpt::utils::CStream NAV_IMPEXP & operator<<(mrpt::utils::CStream &o, const mrpt::nav::TCPoint &p)
NAV_IMPEXP::mrpt::utils::CStream & operator>>(mrpt::utils::CStream &in, CHolonomicLogFileRecordPtr &pObj)
std::vector< TCPoint > TCPointVector
class OPENGL_IMPEXP CSetOfLines
Definition: CSetOfLines.h:21
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
STL namespace.
unsigned long uint32_t
Definition: pstdint.h:216
unsigned int uint16_t
Definition: pstdint.h:170
Trajectory points in C-Space.
TCPoint(const float x_, const float y_, const float phi_, const float t_, const float dist_, const float v_, const float w_)
For usage when passing a dynamic number of (numeric) arguments to a function, by name.
Definition: TParameters.h:47



Page generated by Doxygen 1.9.5 for MRPT 1.4.0 SVN: at Sun Nov 27 02:47:40 UTC 2022