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  {
58  class ProjectionMatrix
59  {
60  public:
62  using Matrix = Eigen::MatrixXd;
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 
106  Matrix mat;
107 
108  public:
109  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
110  };
111 
113  OMPL_CLASS_FORWARD(StateSpace);
115 
117 
118  OMPL_CLASS_FORWARD(ProjectionEvaluator);
120 
130  class ProjectionEvaluator
131  {
132  public:
133  // non-copyable
134  ProjectionEvaluator(const ProjectionEvaluator &) = delete;
135  ProjectionEvaluator &operator=(const ProjectionEvaluator &) = delete;
136 
138  ProjectionEvaluator(const StateSpace *space);
139 
141  ProjectionEvaluator(const StateSpacePtr &space);
142 
143  virtual ~ProjectionEvaluator();
144 
146  virtual unsigned int getDimension() const = 0;
147 
149  virtual void project(const State *state, Eigen::Ref<Eigen::VectorXd> projection) const = 0;
150 
158  virtual void setCellSizes(const std::vector<double> &cellSizes);
159 
164  void setCellSizes(unsigned int dim, double cellSize);
165 
172  void mulCellSizes(double factor);
173 
176  bool userConfigured() const;
177 
179  const std::vector<double> &getCellSizes() const
180  {
181  return cellSizes_;
182  }
183 
185  double getCellSizes(unsigned int dim) const;
186 
188  void checkCellSizes() const;
189 
195  void inferCellSizes();
196 
201  virtual void defaultCellSizes();
202 
204  void checkBounds() const;
205 
207  bool hasBounds() const
208  {
209  return !bounds_.low.empty();
210  }
211 
215  void setBounds(const RealVectorBounds &bounds);
216 
218  const RealVectorBounds &getBounds() const
219  {
220  return bounds_;
221  }
222 
225  void inferBounds();
226 
228  virtual void setup();
229 
231  void computeCoordinates(const Eigen::Ref<Eigen::VectorXd> &projection,
232  Eigen::Ref<Eigen::VectorXi> coord) const;
233 
235  void computeCoordinates(const State *state, Eigen::Ref<Eigen::VectorXi> coord) const
236  {
237  Eigen::VectorXd projection(getDimension());
238  project(state, projection);
239  computeCoordinates(projection, coord);
240  }
241 
244  {
245  return params_;
246  }
247 
249  const ParamSet &params() const
250  {
251  return params_;
252  }
253 
255  virtual void printSettings(std::ostream &out = std::cout) const;
256 
258  virtual void printProjection(const Eigen::Ref<Eigen::VectorXd> &projection,
259  std::ostream &out = std::cout) const;
260 
261  protected:
263  void estimateBounds();
264 
266  const StateSpace *space_;
267 
271  std::vector<double> cellSizes_;
272 
275 
280 
285  bool defaultCellSizes_;
286 
290 
293  };
294 
300  {
301  public:
308  SubspaceProjectionEvaluator(const StateSpace *space, unsigned int index,
310 
311  void setup() override;
312 
313  unsigned int getDimension() const override;
314 
315  void project(const State *state, Eigen::Ref<Eigen::VectorXd> projection) const override;
316 
317  protected:
319  unsigned int index_;
320 
326 
330  };
331  } // namespace base
332 } // namespace ompl
333 
334 #endif
void mulCellSizes(double factor)
Multiply the cell sizes in each dimension by a specified factor factor. This function does nothing if...
void inferCellSizes()
Sample the state space and decide on default cell sizes. This function is called by setup() if no cel...
void setBounds(const RealVectorBounds &bounds)
Set bounds on the projection. The PDST planner needs to known the bounds on the projection....
Maintain a set of parameters.
Definition: GenericParam.h:289
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition: StateSpace.h:134
void computeRandom(unsigned int from, unsigned int to, const std::vector< double > &scale)
Wrapper for ComputeRandom(from, to, scale)
Definition of an abstract state.
Definition: State.h:113
virtual void printSettings(std::ostream &out=std::cout) const
Print settings about this projection.
std::vector< double > low
Lower bound.
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 ...
Eigen::MatrixXd Matrix
Datatype for projection matrices.
virtual void printProjection(const Eigen::Ref< Eigen::VectorXd > &projection, std::ostream &out=std::cout) const
Print a euclidean projection.
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 computeCoordinates(const Eigen::Ref< Eigen::VectorXd > &projection, Eigen::Ref< Eigen::VectorXi > coord) const
Compute integer coordinates for a projection.
void estimateBounds()
Fill estimatedBounds_ with an approximate bounding box for the projection space (via sampling)
bool hasBounds() const
Check if bounds were specified for this projection.
virtual unsigned int getDimension() const =0
Return the dimension of the projection defined by this evaluator.
A shared pointer wrapper for ompl::base::ProjectionEvaluator.
If the projection for a CompoundStateSpace is supposed to be the same as the one for one of its inclu...
void setup() override
Perform configuration steps, if needed.
void checkCellSizes() const
Check if cell dimensions match projection dimension.
ProjectionEvaluatorPtr proj_
The projection to use. This is either the same as specifiedProj_ or, if specifiedProj_ is not initial...
RealVectorBounds bounds_
A bounding box for projected state values.
void project(const State *state, Eigen::Ref< Eigen::VectorXd > projection) const override
Compute the projection as an array of double values.
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...
void inferBounds()
Compute an approximation of the bounds for this projection space. getBounds() will then report the co...
const RealVectorBounds & getBounds() const
Get the bounds computed/set for this projection.
virtual void setup()
Perform configuration steps, if needed.
bool userConfigured() 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.
ParamSet & params()
Get the parameters for this projection.
std::vector< double > cellSizes_
The size of a cell, in every dimension of the projected space, in the implicitly defined integer grid...
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...
unsigned int getDimension() const override
Return the dimension of the projection defined by this evaluator.
bool cellSizesWereInferred_
Flag indicating whether projection cell sizes were automatically inferred.
void checkBounds() const
Check if the projection dimension matched the dimension of the bounds.
ParamSet params_
The set of parameters for this projection.
RealVectorBounds estimatedBounds_
An approximate bounding box for projected state values; This is the cached result of estimateBounds()...
virtual void project(const State *state, Eigen::Ref< Eigen::VectorXd > projection) const =0
Compute the projection as an array of double values.
ProjectionEvaluatorPtr specifiedProj_
The projection that is optionally specified by the user in the constructor argument (projToUse)
A shared pointer wrapper for ompl::base::StateSpace.
const std::vector< double > & getCellSizes() const
Get the size (each dimension) of a grid cell
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 ...
const StateSpace * space_
The state space this projection operates on.
virtual void defaultCellSizes()
Set the default cell dimensions for this projection. The default implementation of this function is e...
void print(std::ostream &out=std::cout) const
Print the contained projection matrix to a stram.
Matrix mat
Projection matrix.
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.
The lower and upper bounds for an Rn space.