WrapperStateSpace.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2017, 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: Zachary Kingston */
36 
37 #include "ompl/base/spaces/WrapperStateSpace.h"
38 
40 {
41  sampler_->sampleUniform(state->as<ompl::base::WrapperStateSpace::StateType>()->getState());
42 }
43 
44 void ompl::base::WrapperStateSampler::sampleUniformNear(State *state, const State *near, double distance)
45 {
46  sampler_->sampleUniformNear(state->as<ompl::base::WrapperStateSpace::StateType>()->getState(),
48 }
49 
50 void ompl::base::WrapperStateSampler::sampleGaussian(State *state, const State *mean, double stdDev)
51 {
52  sampler_->sampleGaussian(state->as<ompl::base::WrapperStateSpace::StateType>()->getState(),
54 }
55 
56 ompl::base::WrapperProjectionEvaluator::WrapperProjectionEvaluator(const ompl::base::WrapperStateSpace *space)
57  : ompl::base::ProjectionEvaluator(space), projection_(space->getSpace()->getDefaultProjection())
58 {
59 }
60 
62 {
63  cellSizes_ = projection_->getCellSizes();
65 }
66 
68 {
69  return projection_->getDimension();
70 }
71 
72 void ompl::base::WrapperProjectionEvaluator::project(const State *state, Eigen::Ref<Eigen::VectorXd> projection) const
73 {
74  projection_->project(state->as<ompl::base::WrapperStateSpace::StateType>()->getState(), projection);
75 }
76 
78 {
79  space_->setup();
80  maxExtent_ = space_->getMaximumExtent();
81  longestValidSegmentFraction_ = space_->getLongestValidSegmentFraction();
82  longestValidSegmentCountFactor_ = space_->getValidSegmentCountFactor();
83  longestValidSegment_ = space_->getLongestValidSegmentLength();
84  projections_ = space_->getRegisteredProjections();
85  params_ = space_->params();
86 
87  valueLocationsInOrder_ = space_->getValueLocations();
88  valueLocationsByName_ = space_->getValueLocationsByName();
89  substateLocationsByName_ = space_->getSubstateLocationsByName();
90 
91  registerDefaultProjection(std::make_shared<WrapperProjectionEvaluator>(this));
92 }
unsigned int getDimension() const override
Return the dimension of the projection defined by this evaluator.
Wrapper state type. Contains a reference to an underlying state.
Definition of an abstract state.
Definition: State.h:50
void sampleUniformNear(State *state, const State *near, double distance) override
Sample a nearby state using underlying sampler.
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
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.
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...
void sampleGaussian(State *state, const State *mean, double stdDev) override
Sample a state within a Gaussian distribution using underlying sampler.
virtual void setup()
Perform configuration steps, if needed.
const State * getState() const
Get a const pointer to the underlying state.
void setup() override
Perform final setup steps. This function is automatically called by the SpaceInformation....
StateSamplerPtr sampler_
Underlying state sampler.
State space wrapper that transparently passes state space operations through to the underlying space....
void sampleUniform(State *state) override
Sample a state using underlying sampler.
Main namespace. Contains everything in this library.
Definition: AppBase.h:22