Loading...
Searching...
No Matches
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
51namespace ompl
52{
53 namespace base
54 {
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
107
108 public:
109 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
110 };
111
113 OMPL_CLASS_FORWARD(StateSpace);
115
117
118 OMPL_CLASS_FORWARD(ProjectionEvaluator);
120
131 {
132 public:
133 // non-copyable
135 ProjectionEvaluator &operator=(const ProjectionEvaluator &) = delete;
136
138 ProjectionEvaluator(const StateSpace *space);
139
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
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
267
271 std::vector<double> cellSizes_;
272
275
280
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
Maintain a set of parameters.
A shared pointer wrapper for ompl::base::ProjectionEvaluator.
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...
bool hasBounds() const
Check if bounds were specified for this projection.
bool userConfigured() const
Return true if any user configuration has been done to this projection evaluator (setCellSizes() was ...
void setBounds(const RealVectorBounds &bounds)
Set bounds on the projection. The PDST planner needs to known the bounds on the projection....
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()
Set the default cell dimensions for this projection. The default implementation of this function is e...
virtual unsigned int getDimension() const =0
Return the dimension of the projection defined by this evaluator.
void mulCellSizes(double factor)
Multiply the cell sizes in each dimension by a specified factor factor. This function does nothing if...
virtual void printSettings(std::ostream &out=std::cout) const
Print settings about this projection.
RealVectorBounds bounds_
A bounding box for projected state values.
bool defaultCellSizes_
Flag indicating whether cell sizes have been set by the user, or whether they were inferred automatic...
const StateSpace * space_
The state space this projection operates on.
void inferCellSizes()
Sample the state space and decide on default cell sizes. This function is called by setup() if no cel...
virtual void printProjection(const Eigen::Ref< Eigen::VectorXd > &projection, std::ostream &out=std::cout) const
Print a euclidean projection.
ParamSet params_
The set of parameters for this projection.
void estimateBounds()
Fill estimatedBounds_ with an approximate bounding box for the projection space (via sampling)
const std::vector< double > & getCellSizes() const
Get the size (each dimension) of a grid cell
void checkCellSizes() const
Check if cell dimensions match projection dimension.
void checkBounds() const
Check if the projection dimension matched the dimension of the bounds.
virtual void project(const State *state, Eigen::Ref< Eigen::VectorXd > projection) const =0
Compute the projection as an array of double values.
const RealVectorBounds & getBounds() const
Get the bounds computed/set for this projection.
void inferBounds()
Compute an approximation of the bounds for this projection space. getBounds() will then report the co...
const ParamSet & params() const
Get the parameters for this projection.
ParamSet & params()
Get the parameters for this projection.
bool cellSizesWereInferred_
Flag indicating whether projection cell sizes were automatically inferred.
void computeCoordinates(const Eigen::Ref< Eigen::VectorXd > &projection, Eigen::Ref< Eigen::VectorXi > coord) const
Compute integer coordinates for a projection.
RealVectorBounds estimatedBounds_
An approximate bounding box for projected state values; This is the cached result of estimateBounds()...
virtual void setup()
Perform configuration steps, if needed.
std::vector< double > cellSizes_
The size of a cell, in every dimension of the projected space, in the implicitly defined integer grid...
void computeCoordinates(const State *state, Eigen::Ref< Eigen::VectorXi > coord) const
Compute integer coordinates for a state.
A projection matrix – it allows multiplication of real vectors by a specified matrix....
Matrix mat
Projection matrix.
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 computeRandom(unsigned int from, unsigned int to, const std::vector< double > &scale)
Wrapper for ComputeRandom(from, to, scale)
void print(std::ostream &out=std::cout) const
Print the contained projection matrix to a stram.
Eigen::MatrixXd Matrix
Datatype for projection matrices.
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 ...
The lower and upper bounds for an Rn space.
std::vector< double > low
Lower bound.
A shared pointer wrapper for ompl::base::StateSpace.
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition StateSpace.h:71
Definition of an abstract state.
Definition State.h:50
If the projection for a CompoundStateSpace is supposed to be the same as the one for one of its inclu...
unsigned int index_
The index of the subspace from which to project.
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.
ProjectionEvaluatorPtr proj_
The projection to use. This is either the same as specifiedProj_ or, if specifiedProj_ is not initial...
ProjectionEvaluatorPtr specifiedProj_
The projection that is optionally specified by the user in the constructor argument (projToUse)
unsigned int getDimension() const override
Return the dimension of the projection defined by this evaluator.
Main namespace. Contains everything in this library.