All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
RealVectorStateProjections.cpp
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 #include "ompl/base/spaces/RealVectorStateProjections.h"
38 #include "ompl/util/Exception.h"
39 #include "ompl/tools/config/MagicConstants.h"
40 #include <cstring>
41 
43 namespace ompl
44 {
45  namespace base
46  {
47  static inline void checkSpaceType(const StateSpace *m)
48  {
49  if (!dynamic_cast<const RealVectorStateSpace*>(m))
50  throw Exception("Expected real vector state space for projection");
51  }
52  }
53 }
55 
57  const ProjectionMatrix::Matrix &projection) :
58  ProjectionEvaluator(space)
59 {
60  checkSpaceType(space_);
61  projection_.mat = projection;
62  setCellSizes(cellSizes);
63 }
64 
66  const ProjectionMatrix::Matrix &projection) :
67  ProjectionEvaluator(space)
68 {
69  checkSpaceType(space_);
70  projection_.mat = projection;
71  setCellSizes(cellSizes);
72 }
73 
75  const ProjectionMatrix::Matrix &projection) :
76  ProjectionEvaluator(space)
77 {
78  checkSpaceType(space_);
79  projection_.mat = projection;
80 }
81 
83  const ProjectionMatrix::Matrix &projection) :
84  ProjectionEvaluator(space)
85 {
86  checkSpaceType(space_);
87  projection_.mat = projection;
88 }
89 
91  const std::vector<unsigned int> &components) :
92  ProjectionEvaluator(space), components_(components)
93 {
94  checkSpaceType(space_);
95  setCellSizes(cellSizes);
96  copyBounds();
97 }
98 
100  const std::vector<unsigned int> &components) :
101  ProjectionEvaluator(space), components_(components)
102 {
103  checkSpaceType(space_);
104  setCellSizes(cellSizes);
105  copyBounds();
106 }
107 
109  ProjectionEvaluator(space), components_(components)
110 {
111  checkSpaceType(space_);
112 }
113 
115  ProjectionEvaluator(space), components_(components)
116 {
117  checkSpaceType(space_);
118 }
119 
121 {
122  bounds_.resize(components_.size());
123  const RealVectorBounds &bounds = space_->as<RealVectorStateSpace>()->getBounds();
124  for (unsigned int i = 0 ; i < components_.size() ; ++i)
125  {
126  bounds_.low[i] = bounds.low[components_[i]];
127  bounds_.high[i] = bounds.high[components_[i]];
128  }
129 }
130 
132 {
133  const RealVectorBounds &bounds = space_->as<RealVectorStateSpace>()->getBounds();
134  bounds_.resize(components_.size());
135  cellSizes_.resize(components_.size());
136  for (unsigned int i = 0 ; i < cellSizes_.size() ; ++i)
137  {
138  bounds_.low[i] = bounds.low[components_[i]];
139  bounds_.high[i] = bounds.high[components_[i]];
140  cellSizes_[i] = (bounds_.high[i] - bounds_.low[i]) / magic::PROJECTION_DIMENSION_SPLITS;
141  }
142 }
143 
145 {
146  return projection_.mat.size1();
147 }
148 
150 {
151  projection_.project(state->as<RealVectorStateSpace::StateType>()->values, projection);
152 }
153 
155 {
156  return components_.size();
157 }
158 
160 {
161  for (unsigned int i = 0 ; i < components_.size() ; ++i)
162  projection(i) = state->as<RealVectorStateSpace::StateType>()->values[components_[i]];
163 }
164 
166  ProjectionEvaluator(space)
167 {
168  checkSpaceType(space_);
169  setCellSizes(cellSizes);
170  copyBounds();
171 }
172 
174  ProjectionEvaluator(space)
175 {
176  checkSpaceType(space_);
177 }
178 
180  ProjectionEvaluator(space)
181 {
182  checkSpaceType(space_);
183  setCellSizes(cellSizes);
184  copyBounds();
185 }
186 
188  ProjectionEvaluator(space)
189 {
190  checkSpaceType(space_);
191 }
192 
193 void ompl::base::RealVectorIdentityProjectionEvaluator::copyBounds(void)
194 {
195  bounds_ = space_->as<RealVectorStateSpace>()->getBounds();
196 }
197 
199 {
200  bounds_ = space_->as<RealVectorStateSpace>()->getBounds();
201  cellSizes_.resize(getDimension());
202  for (unsigned int i = 0 ; i < cellSizes_.size() ; ++i)
203  cellSizes_[i] = (bounds_.high[i] - bounds_.low[i]) / magic::PROJECTION_DIMENSION_SPLITS;
204 }
205 
207 {
208  copySize_ = getDimension() * sizeof(double);
210 }
211 
213 {
214  return space_->getDimension();
215 }
216 
218 {
219  memcpy(&projection(0), state->as<RealVectorStateSpace::StateType>()->values, copySize_);
220 }
std::vector< double > low
Lower bound.
RealVectorIdentityProjectionEvaluator(const StateSpace *space, const std::vector< double > &cellSizes)
Initialize the identity projection evaluator for state space space. The indices of the kept component...
A boost shared pointer wrapper for ompl::base::StateSpace.
virtual void setup(void)
Perform configuration steps, if needed.
virtual void project(const State *state, EuclideanProjection &projection) const
Compute the projection as an array of double values.
void copyBounds()
Fill bounds_ with bounds from the state space.
virtual unsigned int getDimension(void) const
Return the dimension of the projection defined by this evaluator.
ProjectionMatrix projection_
The projection matrix.
virtual unsigned int getDimension(void) const
Return the dimension of the projection defined by this evaluator.
virtual void project(const State *state, EuclideanProjection &projection) const
Compute the projection as an array of double values.
const T * as(void) const
Cast this instance to a desired type.
Definition: State.h:74
const StateSpace * space_
The state space this projection operates on.
Matrix mat
Projection matrix.
std::vector< double > high
Upper bound.
virtual void defaultCellSizes(void)
Set the default cell dimensions for this projection. The default implementation of this function is e...
static const double PROJECTION_DIMENSION_SPLITS
When the cell sizes for a projection are automatically computed, this value defines the number of par...
A state space representing Rn. The distance function is the L2 norm.
Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.
Definition: StateSpace.h:73
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
The lower and upper bounds for an Rn space.
double * values
The value of the actual vector in Rn
virtual unsigned int getDimension(void) const
Return the dimension of the projection defined by this evaluator.
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 ...
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...
virtual void project(const State *state, EuclideanProjection &projection) const
Compute the projection as an array of double values.
RealVectorOrthogonalProjectionEvaluator(const StateSpace *space, const std::vector< double > &cellSizes, const std::vector< unsigned int > &components)
Initialize an orthogonal projection evaluator for state space space. The indices of the kept componen...
virtual void setup(void)
Perform configuration steps, if needed.
boost::numeric::ublas::matrix< double > Matrix
Datatype for projection matrices.
virtual void defaultCellSizes(void)
Set the default cell dimensions for this projection. The default implementation of this function is e...
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...