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 <Eigen/Core>
50 
51 namespace ompl
52 {
53  namespace base
54  {
59  {
60  public:
62  typedef Eigen::MatrixXd Matrix;
63 
80  static Matrix ComputeRandom(unsigned int from, unsigned int to, const std::vector<double> &scale);
81 
91  static Matrix ComputeRandom(unsigned int from, unsigned int to);
92 
94  void computeRandom(unsigned int from, unsigned int to, const std::vector<double> &scale);
95 
97  void computeRandom(unsigned int from, unsigned int to);
98 
100  void project(const double *from, Eigen::Ref<Eigen::VectorXd> to) const;
101 
103  void print(std::ostream &out = std::cout) const;
104 
107  };
108 
110  OMPL_CLASS_FORWARD(StateSpace);
112 
114 
115  OMPL_CLASS_FORWARD(ProjectionEvaluator);
117 
128  {
129  public:
130  // non-copyable
131  ProjectionEvaluator(const ProjectionEvaluator &) = delete;
132  ProjectionEvaluator &operator=(const ProjectionEvaluator &) = delete;
133 
135  ProjectionEvaluator(const StateSpace *space);
136 
138  ProjectionEvaluator(const StateSpacePtr &space);
139 
140  virtual ~ProjectionEvaluator();
141 
143  virtual unsigned int getDimension() const = 0;
144 
146  virtual void project(const State *state, Eigen::Ref<Eigen::VectorXd> projection) const = 0;
147 
155  virtual void setCellSizes(const std::vector<double> &cellSizes);
156 
161  void setCellSizes(unsigned int dim, double cellSize);
162 
169  void mulCellSizes(double factor);
170 
173  bool userConfigured() const;
174 
176  const std::vector<double> &getCellSizes() const
177  {
178  return cellSizes_;
179  }
180 
182  double getCellSizes(unsigned int dim) const;
183 
185  void checkCellSizes() const;
186 
192  void inferCellSizes();
193 
198  virtual void defaultCellSizes();
199 
201  void checkBounds() const;
202 
204  bool hasBounds() const
205  {
206  return !bounds_.low.empty();
207  }
208 
212  void setBounds(const RealVectorBounds &bounds);
213 
216  {
217  return bounds_;
218  }
219 
222  void inferBounds();
223 
225  virtual void setup();
226 
228  void computeCoordinates(const Eigen::Ref<Eigen::VectorXd> &projection,
229  Eigen::Ref<Eigen::VectorXi> coord) const;
230 
232  void computeCoordinates(const State *state, Eigen::Ref<Eigen::VectorXi> coord) const
233  {
234  Eigen::VectorXd projection(getDimension());
235  project(state, projection);
236  computeCoordinates(projection, coord);
237  }
238 
241  {
242  return params_;
243  }
244 
246  const ParamSet &params() const
247  {
248  return params_;
249  }
250 
252  virtual void printSettings(std::ostream &out = std::cout) const;
253 
255  virtual void printProjection(const Eigen::Ref<Eigen::VectorXd> &projection,
256  std::ostream &out = std::cout) const;
257 
258  protected:
260  void estimateBounds();
261 
264 
268  std::vector<double> cellSizes_;
269 
272 
277 
283 
287 
290  };
291 
297  {
298  public:
305  SubspaceProjectionEvaluator(const StateSpace *space, unsigned int index,
307 
308  void setup() override;
309 
310  unsigned int getDimension() const override;
311 
312  void project(const State *state, Eigen::Ref<Eigen::VectorXd> projection) const override;
313 
314  protected:
316  unsigned int index_;
317 
323 
327  };
328  } // namespace base
329 } // namespace ompl
330 
331 #endif
void computeCoordinates(const Eigen::Ref< Eigen::VectorXd > &projection, Eigen::Ref< Eigen::VectorXi > coord) const
Compute integer coordinates for a projection.
virtual unsigned int getDimension() const =0
Return the dimension of the projection defined by this evaluator.
std::vector< double > cellSizes_
The size of a cell, in every dimension of the projected space, in the implicitly defined integer grid...
void print(std::ostream &out=std::cout) const
Print the contained projection matrix to a stram.
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.
const RealVectorBounds & getBounds() const
Get the bounds computed/set for this projection.
bool cellSizesWereInferred_
Flag indicating whether projection cell sizes were automatically inferred.
void computeRandom(unsigned int from, unsigned int to, const std::vector< double > &scale)
Wrapper for ComputeRandom(from, to, scale)
A shared pointer wrapper for ompl::base::StateSpace.
void project(const double *from, Eigen::Ref< Eigen::VectorXd > to) const
Multiply the vector from by the contained projection matrix to obtain the vector to.
void checkBounds() const
Check if the projection dimension matched the dimension of the bounds.
void inferBounds()
Compute an approximation of the bounds for this projection space. getBounds() will then report the co...
virtual void project(const State *state, Eigen::Ref< Eigen::VectorXd > projection) const =0
Compute the projection as an array of double values.
SubspaceProjectionEvaluator(const StateSpace *space, unsigned int index, ProjectionEvaluatorPtr projToUse=ProjectionEvaluatorPtr())
The constructor states that for space space, the projection to use is the same as the component at po...
ProjectionEvaluatorPtr specifiedProj_
The projection that is optionally specified by the user in the constructor argument (projToUse) ...
void inferCellSizes()
Sample the state space and decide on default cell sizes. This function is called by setup() if no cel...
RealVectorBounds estimatedBounds_
An approximate bounding box for projected state values; This is the cached result of estimateBounds()...
ParamSet params_
The set of parameters for this projection.
Maintain a set of parameters.
Definition: GenericParam.h:226
unsigned int getDimension() const override
Return the dimension of the projection defined by this evaluator.
const std::vector< double > & getCellSizes() const
Get the size (each dimension) of a grid cell.
const StateSpace * space_
The state space this projection operates on.
ParamSet & params()
Get the parameters for this projection.
bool defaultCellSizes_
Flag indicating whether cell sizes have been set by the user, or whether they were inferred automatic...
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
virtual void defaultCellSizes()
Set the default cell dimensions for this projection. The default implementation of this function is e...
Eigen::MatrixXd Matrix
Datatype for projection matrices.
unsigned int index_
The index of the subspace from which to project.
Matrix mat
Projection matrix.
void computeCoordinates(const State *state, Eigen::Ref< Eigen::VectorXi > coord) const
Compute integer coordinates for a state.
void mulCellSizes(double factor)
Multiply the cell sizes in each dimension by a specified factor factor. This function does nothing if...
A shared pointer wrapper for ompl::base::ProjectionEvaluator.
Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.
Definition: StateSpace.h:70
const ParamSet & params() const
Get the parameters for this projection.
Definition of an abstract state.
Definition: State.h:49
A projection matrix – it allows multiplication of real vectors by a specified matrix. The matrix can also be randomly generated.
The lower and upper bounds for an Rn space.
void estimateBounds()
Fill estimatedBounds_ with an approximate bounding box for the projection space (via sampling) ...
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 printSettings(std::ostream &out=std::cout) const
Print settings about this projection.
virtual void printProjection(const Eigen::Ref< Eigen::VectorXd > &projection, std::ostream &out=std::cout) const
Print a euclidean projection.
bool userConfigured() const
Return true if any user configuration has been done to this projection evaluator (setCellSizes() was ...
void project(const State *state, Eigen::Ref< Eigen::VectorXd > projection) const override
Compute the projection as an array of double values.
RealVectorBounds bounds_
A bounding box for projected state values.
void checkCellSizes() const
Check if cell dimensions match projection dimension.
bool hasBounds() const
Check if bounds were specified for this projection.
void setup() override
Perform configuration steps, if needed.
virtual void setup()
Perform configuration steps, if needed.
static Matrix ComputeRandom(unsigned int from, 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 ...
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...