SE3StateSpace.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/SE3StateSpace.h"
38 #include "ompl/tools/config/MagicConstants.h"
39 #include <cstring>
40 
42 {
43  auto *state = new StateType();
44  allocStateComponents(state);
45  return state;
46 }
47 
49 {
51 }
52 
54 {
55  class SE3DefaultProjection : public ProjectionEvaluator
56  {
57  public:
58  SE3DefaultProjection(const StateSpace *space) : ProjectionEvaluator(space)
59  {
60  }
61 
62  unsigned int getDimension() const override
63  {
64  return 3;
65  }
66 
67  void defaultCellSizes() override
68  {
69  cellSizes_.resize(3);
70  bounds_ = space_->as<SE3StateSpace>()->getBounds();
71  cellSizes_[0] = (bounds_.high[0] - bounds_.low[0]) / magic::PROJECTION_DIMENSION_SPLITS;
72  cellSizes_[1] = (bounds_.high[1] - bounds_.low[1]) / magic::PROJECTION_DIMENSION_SPLITS;
73  cellSizes_[2] = (bounds_.high[2] - bounds_.low[2]) / magic::PROJECTION_DIMENSION_SPLITS;
74  }
75 
76  void project(const State *state, EuclideanProjection &projection) const override
77  {
78  memcpy(&projection(0),
80  3 * sizeof(double));
81  }
82  };
83 
84  registerDefaultProjection(std::make_shared<SE3DefaultProjection>(this));
85 }
State * allocState() const override
Allocate a state that can store a point in the described space.
void freeState(State *state) const override
Free the memory of the allocated state.
unsigned int getDimension() const override
Get the dimension of the space (not the dimension of the surrounding ambient space) ...
const RealVectorBounds & getBounds() const
Get the bounds for this state space.
A state in SE(3): position = (x, y, z), quaternion = (x, y, z, w)
Definition: SE3StateSpace.h:53
void registerDefaultProjection(const ProjectionEvaluatorPtr &projection)
Register the default projection for this state space.
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
ompl::base::CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:577
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:70
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:49
void registerProjections() override
Register the projections for this state space. Usually, this is at least the default projection...
double * values
The value of the actual vector in Rn
void freeState(State *state) const override
Free the memory of the allocated state.
const T * as(unsigned int index) const
Cast a component of this instance to a desired type.
Definition: State.h:95
A state space representing SE(3)
Definition: SE3StateSpace.h:49
void allocStateComponents(CompoundState *state) const
Allocate the state components. Called by allocState(). Usually called by derived state spaces...
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...