00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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 }