All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
DiscreteStateSpace.cpp
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2011, Rice University
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Rice University nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Author: Elizabeth Fudge */
00036 
00037 #include "ompl/base/spaces/DiscreteStateSpace.h"
00038 #include "ompl/util/Exception.h"
00039 #include <limits>
00040 #include <cstdlib>
00041 
00042 void ompl::base::DiscreteStateSampler::sampleUniform(State *state)
00043 {
00044     state->as<DiscreteStateSpace::StateType>()->value =
00045         rng_.uniformInt(space_->as<DiscreteStateSpace>()->getLowerBound(),
00046                         space_->as<DiscreteStateSpace>()->getUpperBound());
00047 }
00048 
00049 void ompl::base::DiscreteStateSampler::sampleUniformNear(State *state, const State *near, const double distance)
00050 {
00051     const int d = (int)floor(distance + 0.5);
00052     state->as<DiscreteStateSpace::StateType>()->value =
00053         rng_.uniformInt(near->as<DiscreteStateSpace::StateType>()->value - d,
00054                         near->as<DiscreteStateSpace::StateType>()->value + d);
00055     space_->enforceBounds(state);
00056 }
00057 
00058 void ompl::base::DiscreteStateSampler::sampleGaussian(State *state, const State *mean, const double stdDev)
00059 {
00060     state->as<DiscreteStateSpace::StateType>()->value =
00061         (int)floor(rng_.gaussian(mean->as<DiscreteStateSpace::StateType>()->value, stdDev) + 0.5);
00062     space_->enforceBounds(state);
00063 }
00064 
00065 bool ompl::base::DiscreteStateSpace::isDiscrete(void) const
00066 {
00067     return true;
00068 }
00069 
00070 unsigned int ompl::base::DiscreteStateSpace::getDimension(void) const
00071 {
00072     return 1;
00073 }
00074 
00075 double ompl::base::DiscreteStateSpace::getMaximumExtent(void) const
00076 {
00077     return upperBound_ - lowerBound_;
00078 }
00079 
00080 void ompl::base::DiscreteStateSpace::enforceBounds(State *state) const
00081 {
00082     if (state->as<StateType>()->value < lowerBound_)
00083         state->as<StateType>()->value = lowerBound_;
00084     else
00085         if (state->as<StateType>()->value > upperBound_)
00086             state->as<StateType>()->value = upperBound_;
00087 }
00088 
00089 bool ompl::base::DiscreteStateSpace::satisfiesBounds(const State *state) const
00090 {
00091     return state->as<StateType>()->value >= lowerBound_ && state->as<StateType>()->value <= upperBound_;
00092 }
00093 
00094 void ompl::base::DiscreteStateSpace::copyState(State *destination, const State *source) const
00095 {
00096     destination->as<StateType>()->value = source->as<StateType>()->value;
00097 }
00098 
00099 double ompl::base::DiscreteStateSpace::distance(const State *state1, const State *state2) const
00100 {
00101     return abs(state1->as<StateType>()->value - state2->as<StateType>()->value);
00102 }
00103 
00104 bool ompl::base::DiscreteStateSpace::equalStates(const State *state1, const State *state2) const
00105 {
00106     return state1->as<StateType>()->value == state2->as<StateType>()->value;
00107 }
00108 
00109 void ompl::base::DiscreteStateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
00110 {
00111     state->as<StateType>()->value = (int)floor(from->as<StateType>()->value +
00112                                                (to->as<StateType>()->value - from->as<StateType>()->value) * t + 0.5);
00113 }
00114 
00115 ompl::base::StateSamplerPtr ompl::base::DiscreteStateSpace::allocStateSampler(void) const
00116 {
00117     return StateSamplerPtr(new DiscreteStateSampler(this));
00118 }
00119 
00120 ompl::base::State* ompl::base::DiscreteStateSpace::allocState(void) const
00121 {
00122     return new StateType();
00123 }
00124 
00125 void ompl::base::DiscreteStateSpace::freeState(State *state) const
00126 {
00127     delete static_cast<StateType*>(state);
00128 }
00129 
00130 void ompl::base::DiscreteStateSpace::registerProjections(void)
00131 {
00132     class DiscreteDefaultProjection : public ProjectionEvaluator
00133     {
00134     public:
00135 
00136         DiscreteDefaultProjection(const StateSpace *space) : ProjectionEvaluator(space)
00137         {
00138         }
00139 
00140         virtual unsigned int getDimension(void) const
00141         {
00142             return 1;
00143         }
00144 
00145         virtual void defaultCellSizes(void)
00146         {
00147             cellSizes_.resize(1);
00148             cellSizes_[0] = 1.0;
00149         }
00150 
00151         virtual void project(const State *state, EuclideanProjection &projection) const
00152         {
00153             projection(0) = state->as<DiscreteStateSpace::StateType>()->value;
00154         }
00155     };
00156 
00157     registerDefaultProjection(ProjectionEvaluatorPtr(dynamic_cast<ProjectionEvaluator*>(new DiscreteDefaultProjection(this))));
00158 }
00159 
00160 void ompl::base::DiscreteStateSpace::setup(void)
00161 {
00162     if (lowerBound_ > upperBound_)
00163         throw Exception("Lower bound cannot be larger than upper bound for a discrete space");
00164     StateSpace::setup();
00165 }
00166 
00167 void ompl::base::DiscreteStateSpace::printState(const State *state, std::ostream &out) const
00168 {
00169     out << "DiscreteState [";
00170     if (state)
00171         out << state->as<StateType>()->value;
00172     else
00173         out << "NULL";
00174     out << ']' << std::endl;
00175 }
00176 
00177 void ompl::base::DiscreteStateSpace::printSettings(std::ostream &out) const
00178 {
00179     out << "Discrete state space '" << getName() << "' with bounds [" << lowerBound_ << ", " << upperBound_ << "]" << std::endl;
00180 }