SE2StateSpace.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/SE2StateSpace.h"
38 #include "ompl/tools/config/MagicConstants.h"
39 #include <cstring>
40 
42 {
43  StateType *state = new StateType();
44  allocStateComponents(state);
45  return state;
46 }
47 
49 {
51 }
52 
54 {
55  class SE2DefaultProjection : public ProjectionEvaluator
56  {
57  public:
58 
59  SE2DefaultProjection(const StateSpace *space) : ProjectionEvaluator(space)
60  {
61  }
62 
63  virtual unsigned int getDimension() const
64  {
65  return 2;
66  }
67 
68  virtual void defaultCellSizes()
69  {
70  cellSizes_.resize(2);
71  bounds_ = space_->as<SE2StateSpace>()->getBounds();
72  cellSizes_[0] = (bounds_.high[0] - bounds_.low[0]) / magic::PROJECTION_DIMENSION_SPLITS;
73  cellSizes_[1] = (bounds_.high[1] - bounds_.low[1]) / magic::PROJECTION_DIMENSION_SPLITS;
74  }
75 
76  virtual void project(const State *state, EuclideanProjection &projection) const
77  {
78  memcpy(&projection(0), state->as<SE2StateSpace::StateType>()->as<RealVectorStateSpace::StateType>(0)->values, 2 * sizeof(double));
79  }
80  };
81 
82  registerDefaultProjection(ProjectionEvaluatorPtr(dynamic_cast<ProjectionEvaluator*>(new SE2DefaultProjection(this))));
83 }
A state in SE(2): (x, y, yaw)
Definition: SE2StateSpace.h:55
virtual void registerProjections()
Register the projections for this state space. Usually, this is at least the default projection...
CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:549
void allocStateComponents(CompoundState *state) const
Allocate the state components. Called by allocState(). Usually called by derived state spaces...
virtual void freeState(State *state) const
Free the memory of the allocated state.
A state space representing SE(2)
Definition: SE2StateSpace.h:50
A boost shared pointer wrapper for ompl::base::ProjectionEvaluator.
static const double PROJECTION_DIMENSION_SPLITS
When the cell sizes for a projection are automatically computed, this value defines the number of par...
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.
virtual void freeState(State *state) const
Free the memory of the allocated state.
Definition of an abstract state.
Definition: State.h:50
virtual State * allocState() const
Allocate a state that can store a point in the described space.
double * values
The value of the actual vector in Rn
const T * as() const
Cast this instance to a desired type.
Definition: State.h:74
const T * as(const unsigned int index) const
Cast a component of this instance to a desired type.
Definition: State.h:109
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...