All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
ProjectionEvaluator.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2010, Rice University
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
37 #ifndef OMPL_BASE_PROJECTION_EVALUATOR_
38 #define OMPL_BASE_PROJECTION_EVALUATOR_
39 
40 #include "ompl/base/State.h"
41 #include "ompl/util/ClassForward.h"
42 #include "ompl/util/Console.h"
43 #include "ompl/base/GenericParam.h"
44 #include "ompl/base/spaces/RealVectorBounds.h"
45 
46 #include <vector>
47 #include <valarray>
48 #include <iostream>
49 #include <boost/noncopyable.hpp>
50 #include <boost/numeric/ublas/matrix.hpp>
51 
52 namespace ompl
53 {
54 
55  namespace base
56  {
57 
59  typedef std::vector<int> ProjectionCoordinates;
60 
62  typedef boost::numeric::ublas::vector<double> EuclideanProjection;
63 
64 
69  {
70  public:
71 
73  typedef boost::numeric::ublas::matrix<double> Matrix;
74 
91  static Matrix ComputeRandom(const unsigned int from, const unsigned int to, const std::vector<double> &scale);
92 
102  static Matrix ComputeRandom(const unsigned int from, const unsigned int to);
103 
105  void computeRandom(const unsigned int from, const unsigned int to, const std::vector<double> &scale);
106 
108  void computeRandom(const unsigned int from, const unsigned int to);
109 
111  void project(const double *from, EuclideanProjection& to) const;
112 
114  void print(std::ostream &out = std::cout) const;
115 
118  };
119 
121  OMPL_CLASS_FORWARD(StateSpace);
123 
125 
126  OMPL_CLASS_FORWARD(ProjectionEvaluator);
128 
138  class ProjectionEvaluator : private boost::noncopyable
139  {
140  public:
141 
143  ProjectionEvaluator(const StateSpace *space);
144 
146  ProjectionEvaluator(const StateSpacePtr &space);
147 
148  virtual ~ProjectionEvaluator(void);
149 
151  virtual unsigned int getDimension(void) const = 0;
152 
154  virtual void project(const State *state, EuclideanProjection &projection) const = 0;
155 
163  virtual void setCellSizes(const std::vector<double> &cellSizes);
164 
167  void setCellSizes(unsigned int dim, double cellSize);
168 
172  void mulCellSizes(double factor);
173 
175  bool userConfigured(void) const;
176 
178  const std::vector<double>& getCellSizes(void) const
179  {
180  return cellSizes_;
181  }
182 
184  double getCellSizes(unsigned int dim) const;
185 
187  void checkCellSizes(void) const;
188 
194  void inferCellSizes(void);
195 
200  virtual void defaultCellSizes(void);
201 
203  void checkBounds(void) const;
204 
206  bool hasBounds(void) const
207  {
208  return !bounds_.low.empty();
209  }
210 
214  void setBounds(const RealVectorBounds &bounds);
215 
218  {
219  return bounds_;
220  }
221 
223  void inferBounds(void);
224 
226  virtual void setup(void);
227 
229  void computeCoordinates(const EuclideanProjection &projection, ProjectionCoordinates &coord) const;
230 
232  void computeCoordinates(const State *state, ProjectionCoordinates &coord) const
233  {
234  EuclideanProjection projection(getDimension());
235  project(state, projection);
236  computeCoordinates(projection, coord);
237  }
238 
241  {
242  return params_;
243  }
244 
246  const ParamSet& params(void) const
247  {
248  return params_;
249  }
250 
252  virtual void printSettings(std::ostream &out = std::cout) const;
253 
255  virtual void printProjection(const EuclideanProjection &projection, std::ostream &out = std::cout) const;
256 
257  protected:
258 
260  void estimateBounds(void);
261 
264 
268  std::vector<double> cellSizes_;
269 
272 
277 
283 
287 
290  };
291 
297  {
298  public:
299 
306  SubspaceProjectionEvaluator(const StateSpace *space, unsigned int index, const ProjectionEvaluatorPtr &projToUse = ProjectionEvaluatorPtr());
307 
308  virtual void setup(void);
309 
310  virtual unsigned int getDimension(void) const;
311 
312  virtual void project(const State *state, EuclideanProjection &projection) const;
313 
314  protected:
315 
317  unsigned int index_;
318 
324 
327  };
328 
329  }
330 
331 }
332 
333 #endif
void estimateBounds(void)
Fill estimatedBounds_ with an approximate bounding box for the projection space (via sampling) ...
virtual void printSettings(std::ostream &out=std::cout) const
Print settings about this projection.
std::vector< double > cellSizes_
The size of a cell, in every dimension of the projected space, in the implicitly defined integer grid...
If the projection for a CompoundStateSpace is supposed to be the same as the one for one of its inclu...
std::vector< double > low
Lower bound.
bool cellSizesWereInferred_
Flag indicating whether projection cell sizes were automatically inferred.
A boost shared pointer wrapper for ompl::base::StateSpace.
void computeCoordinates(const State *state, ProjectionCoordinates &coord) const
Compute integer coordinates for a state.
void checkBounds(void) const
Check if the projection dimension matched the dimension of the bounds.
ProjectionEvaluator(const StateSpace *space)
Construct a projection evaluator for a specific state space.
virtual void setup(void)
Perform configuration steps, if needed.
static Matrix ComputeRandom(const unsigned int from, const unsigned int to, const std::vector< double > &scale)
Compute a random projection matrix with from columns and to rows. A vector with from elements can be ...
virtual unsigned int getDimension(void) const =0
Return the dimension of the projection defined by this evaluator.
bool hasBounds(void) const
Check if bounds were specified for this projection.
ProjectionEvaluatorPtr specifiedProj_
The projection that is optionally specified by the user in the constructor argument (projToUse) ...
RealVectorBounds estimatedBounds_
An approximate bounding box for projected state values; This is the cached result of estimateBounds()...
void print(std::ostream &out=std::cout) const
Print the contained projection matrix to a stram.
ParamSet params_
The set of parameters for this projection.
Maintain a set of parameters.
Definition: GenericParam.h:216
void computeRandom(const unsigned int from, const unsigned int to, const std::vector< double > &scale)
Wrapper for ComputeRandom(from, to, scale)
void inferBounds(void)
Compute an approximation of the bounds for this projection space. getBounds() will then report the co...
virtual unsigned int getDimension(void) const
Return the dimension of the projection defined by this evaluator.
const StateSpace * space_
The state space this projection operates on.
bool defaultCellSizes_
Flag indicating whether cell sizes have been set by the user, or whether they were inferred automatic...
SubspaceProjectionEvaluator(const StateSpace *space, unsigned int index, const ProjectionEvaluatorPtr &projToUse=ProjectionEvaluatorPtr())
The constructor states that for space space, the projection to use is the same as the component at po...
bool userConfigured(void) const
Return true if any user configuration has been done to this projection evaluator (setCellSizes() was ...
unsigned int index_
The index of the subspace from which to project.
virtual void project(const State *state, EuclideanProjection &projection) const
Compute the projection as an array of double values.
Matrix mat
Projection matrix.
virtual void printProjection(const EuclideanProjection &projection, std::ostream &out=std::cout) const
Print a euclidean projection.
const ParamSet & params(void) const
Get the parameters for this projection.
void mulCellSizes(double factor)
Multiply the cell sizes in each dimension by a specified factor factor. This function does nothing if...
A boost shared pointer wrapper for ompl::base::ProjectionEvaluator.
std::vector< int > ProjectionCoordinates
Grid cells corresponding to a projection value are described in terms of their coordinates.
Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.
Definition: StateSpace.h:73
virtual void setup(void)
Perform configuration steps, if needed.
const std::vector< double > & getCellSizes(void) const
Get the size (each dimension) of a grid cell.
boost::numeric::ublas::vector< double > EuclideanProjection
The datatype for state projections. This class contains a real vector.
Definition of an abstract state.
Definition: State.h:50
A projection matrix – it allows multiplication of real vectors by a specified matrix. The matrix can also be randomly generated.
void checkCellSizes(void) const
Check if cell dimensions match projection dimension.
virtual void project(const State *state, EuclideanProjection &projection) const =0
Compute the projection as an array of double values.
void inferCellSizes(void)
Sample the state space and decide on default cell sizes. This function is called by setup() if no cel...
ParamSet & params(void)
Get the parameters for this projection.
The lower and upper bounds for an Rn space.
void setBounds(const RealVectorBounds &bounds)
Set bounds on the projection. The PDST planner needs to known the bounds on the projection. Default bounds are automatically computed by inferCellSizes().
virtual void setCellSizes(const std::vector< double > &cellSizes)
Define the size (in each dimension) of a grid cell. The number of sizes set here must be the same as ...
virtual void defaultCellSizes(void)
Set the default cell dimensions for this projection. The default implementation of this function is e...
void project(const double *from, EuclideanProjection &to) const
Multiply the vector from by the contained projection matrix to obtain the vector to.
const RealVectorBounds & getBounds(void)
Get the bounds computed/set for this projection.
boost::numeric::ublas::matrix< double > Matrix
Datatype for projection matrices.
RealVectorBounds bounds_
A bounding box for projected state values.
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...
ProjectionEvaluatorPtr proj_
The projection to use. This is either the same as specifiedProj_ or, if specifiedProj_ is not initial...
void computeCoordinates(const EuclideanProjection &projection, ProjectionCoordinates &coord) const
Compute integer coordinates for a projection.