RealVectorStateProjections.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_SPACES_REAL_VECTOR_STATE_PROJECTIONS_
38 #define OMPL_BASE_SPACES_REAL_VECTOR_STATE_PROJECTIONS_
39 
40 #include "ompl/base/ProjectionEvaluator.h"
41 #include "ompl/base/spaces/RealVectorStateSpace.h"
42 
43 namespace ompl
44 {
45  namespace base
46  {
52  class RealVectorLinearProjectionEvaluator : public ProjectionEvaluator
53  {
54  public:
57  RealVectorLinearProjectionEvaluator(const StateSpace *space, const std::vector<double> &cellSizes,
58  const ProjectionMatrix::Matrix &projection);
59 
62  RealVectorLinearProjectionEvaluator(const StateSpacePtr &space, const std::vector<double> &cellSizes,
63  const ProjectionMatrix::Matrix &projection);
64 
67  RealVectorLinearProjectionEvaluator(const StateSpace *space, const ProjectionMatrix::Matrix &projection);
68 
72 
73  unsigned int getDimension() const override;
74 
75  void project(const State *state, Eigen::Ref<Eigen::VectorXd> projection) const override;
76 
77  protected:
79  ProjectionMatrix projection_;
80  };
81 
83  class RealVectorRandomLinearProjectionEvaluator : public RealVectorLinearProjectionEvaluator
84  {
85  public:
88  RealVectorRandomLinearProjectionEvaluator(const StateSpace *space, const std::vector<double> &cellSizes)
90  space, cellSizes, ProjectionMatrix::ComputeRandom(space->getDimension(), cellSizes.size()))
91  {
92  }
93 
96  RealVectorRandomLinearProjectionEvaluator(const StateSpacePtr &space, const std::vector<double> &cellSizes)
98  space, cellSizes, ProjectionMatrix::ComputeRandom(space->getDimension(), cellSizes.size()))
99  {
100  }
101 
105  RealVectorRandomLinearProjectionEvaluator(const StateSpace *space, unsigned int dim)
107  space,
108  ProjectionMatrix::ComputeRandom(space->getDimension(), dim,
109  space->as<RealVectorStateSpace>()->getBounds().getDifference()))
110  {
111  }
112 
118  space,
119  ProjectionMatrix::ComputeRandom(space->getDimension(), dim,
120  space->as<RealVectorStateSpace>()->getBounds().getDifference()))
121  {
122  }
123  };
124 
126  class RealVectorOrthogonalProjectionEvaluator : public ProjectionEvaluator
127  {
128  public:
131  RealVectorOrthogonalProjectionEvaluator(const StateSpace *space, const std::vector<double> &cellSizes,
132  std::vector<unsigned int> components);
133 
136  RealVectorOrthogonalProjectionEvaluator(const StateSpacePtr &space, const std::vector<double> &cellSizes,
137  std::vector<unsigned int> components);
138 
142  RealVectorOrthogonalProjectionEvaluator(const StateSpace *space, std::vector<unsigned int> components);
143 
147  RealVectorOrthogonalProjectionEvaluator(const StateSpacePtr &space, std::vector<unsigned int> components);
148 
149  unsigned int getDimension() const override;
150 
151  void defaultCellSizes() override;
152 
153  void project(const State *state, Eigen::Ref<Eigen::VectorXd> projection) const override;
154 
155  protected:
157  void copyBounds();
158 
160  std::vector<unsigned int> components_;
161  };
162 
165  {
166  public:
169  RealVectorIdentityProjectionEvaluator(const StateSpace *space, const std::vector<double> &cellSizes);
170 
173  RealVectorIdentityProjectionEvaluator(const StateSpacePtr &space, const std::vector<double> &cellSizes);
174 
179 
184 
185  unsigned int getDimension() const override;
186 
187  void defaultCellSizes() override;
188 
189  void setup() override;
190 
191  void project(const State *state, Eigen::Ref<Eigen::VectorXd> projection) const override;
192 
193  private:
195  void copyBounds();
196 
198  std::size_t copySize_;
199  };
200  } // namespace base
201 } // namespace ompl
202 #endif
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition: StateSpace.h:134
unsigned int getDimension() const override
Return the dimension of the projection defined by this evaluator.
ProjectionMatrix projection_
The projection matrix.
Definition for a class computing linear projections (multiplication of a k-by-n matrix to the the Rn ...
Definition of an abstract state.
Definition: State.h:113
Eigen::MatrixXd Matrix
Datatype for projection matrices.
A projection matrix – it allows multiplication of real vectors by a specified matrix....
void project(const State *state, Eigen::Ref< Eigen::VectorXd > projection) const override
Compute the projection as an array of double values.
RealVectorLinearProjectionEvaluator(const StateSpace *space, const std::vector< double > &cellSizes, const ProjectionMatrix::Matrix &projection)
Initialize a linear projection evaluator for state space space. The used projection matrix is project...
unsigned int getDimension() const override
Return the dimension of the projection defined by this evaluator.
void project(const State *state, Eigen::Ref< Eigen::VectorXd > projection) const override
Compute the projection as an array of double values.
void defaultCellSizes() override
Set the default cell dimensions for this projection. The default implementation of this function is e...
A state space representing Rn. The distance function is the L2 norm.
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...
const RealVectorBounds & getBounds() const
Get the bounds computed/set for this projection.
RealVectorRandomLinearProjectionEvaluator(const StateSpace *space, const std::vector< double > &cellSizes)
Initialize a linear projection evaluator for state space space. The used projection matrix is sampled...
unsigned int getDimension() const override
Return the dimension of the projection defined by this evaluator.
void setup() override
Perform configuration steps, if needed.
void project(const State *state, Eigen::Ref< Eigen::VectorXd > projection) const override
Compute the projection as an array of double values.
void defaultCellSizes() override
Set the default cell dimensions for this projection. The default implementation of this function is e...
std::vector< unsigned int > components_
The set of components selected by the projection.
A shared pointer wrapper for ompl::base::StateSpace.
RealVectorOrthogonalProjectionEvaluator(const StateSpace *space, const std::vector< double > &cellSizes, std::vector< unsigned int > components)
Initialize an orthogonal projection evaluator for state space space. The indices of the kept componen...
void copyBounds()
Fill bounds_ with bounds from the state space.
RealVectorIdentityProjectionEvaluator(const StateSpace *space, const std::vector< double > &cellSizes)
Initialize the identity projection evaluator for state space space. The indices of the kept component...
Main namespace. Contains everything in this library.