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 #ifndef OMPL_BASE_SPACES_WRAPPER_STATE_SPACE_ 38 #define OMPL_BASE_SPACES_WRAPPER_STATE_SPACE_ 39 40 #include <utility> 41 42 #include "ompl/base/StateSpace.h" 43 44 namespace ompl 45 { 46 namespace base 47 { 48 /// @cond IGNORE 49 /** \brief Forward declaration of ompl::base::WrapperStateSpace */ 50 OMPL_CLASS_FORWARD(WrapperStateSpace); 51 /// @endcond 52 53 /** \brief A state sampler that wraps around another state sampler. */ 54 class WrapperStateSampler : public StateSampler 55 { 56 public: 57 /** \brief Constructor. Requires the wrapper state space \a space 58 * and the underlying sampler \a sampler. */ WrapperStateSampler(const StateSpace * space,StateSamplerPtr sampler)59 WrapperStateSampler(const StateSpace *space, StateSamplerPtr sampler) 60 : StateSampler(space), sampler_(std::move(sampler)) 61 { 62 } 63 64 /** \brief Sample a state using underlying sampler. */ 65 void sampleUniform(State *state) override; 66 67 /** \brief Sample a nearby state using underlying sampler. */ 68 void sampleUniformNear(State *state, const State *near, double distance) override; 69 70 /** \brief Sample a state within a Gaussian distribution using underlying sampler. */ 71 void sampleGaussian(State *state, const State *mean, double stdDev) override; 72 73 protected: 74 /** \brief Underlying state sampler. */ 75 StateSamplerPtr sampler_; 76 }; 77 78 /** \brief A projection evaluator that wraps around another projection 79 * evaluator. */ 80 class WrapperProjectionEvaluator : public ProjectionEvaluator 81 { 82 public: 83 WrapperProjectionEvaluator(const WrapperStateSpace *space); 84 85 void setup() override; 86 87 unsigned int getDimension() const override; 88 89 void project(const State *state, Eigen::Ref<Eigen::VectorXd> projection) const override; 90 91 private: 92 /** \brief Projection from wrapped space. */ 93 ProjectionEvaluatorPtr projection_; 94 }; 95 96 /** \brief State space wrapper that transparently passes state space 97 * operations through to the underlying space. Allows augmentation of 98 * state spaces with additional information. */ 99 class WrapperStateSpace : public StateSpace 100 { 101 public: 102 /** \brief Wrapper state type. Contains a reference to an underlying state. */ 103 class StateType : public State 104 { 105 public: 106 /** \brief Constructor. Takes a reference \a state to the underlying state. */ StateType(State * state)107 StateType(State *state) : state_(state) 108 { 109 } 110 111 /** \brief Get a const pointer to the underlying state. */ getState()112 const State *getState() const 113 { 114 return state_; 115 } 116 117 /** \brief Get a pointer to the underlying state. */ getState()118 State *getState() 119 { 120 return state_; 121 } 122 123 protected: 124 /** \brief Underlying state. */ 125 State *state_; 126 }; 127 WrapperStateSpace(const StateSpacePtr & space)128 WrapperStateSpace(const StateSpacePtr &space) : space_(space) 129 { 130 } 131 isCompound()132 bool isCompound() const override 133 { 134 return space_->isCompound(); 135 } 136 isDiscrete()137 bool isDiscrete() const override 138 { 139 return space_->isDiscrete(); 140 } 141 isHybrid()142 bool isHybrid() const override 143 { 144 return space_->isHybrid(); 145 } 146 isMetricSpace()147 bool isMetricSpace() const override 148 { 149 return space_->isMetricSpace(); 150 } 151 hasSymmetricDistance()152 bool hasSymmetricDistance() const override 153 { 154 return space_->hasSymmetricDistance(); 155 } 156 hasSymmetricInterpolate()157 bool hasSymmetricInterpolate() const override 158 { 159 return space_->hasSymmetricInterpolate(); 160 } 161 getName()162 const std::string &getName() const 163 { 164 return space_->getName(); 165 } 166 setName(const std::string & name)167 void setName(const std::string &name) 168 { 169 space_->setName(name); 170 } 171 getType()172 int getType() const 173 { 174 return space_->getType(); 175 } 176 includes(const StateSpacePtr & other)177 bool includes(const StateSpacePtr &other) const 178 { 179 return space_->includes(other); 180 } 181 includes(const StateSpace * other)182 bool includes(const StateSpace *other) const 183 { 184 return space_->includes(other); 185 } 186 covers(const StateSpacePtr & other)187 bool covers(const StateSpacePtr &other) const 188 { 189 return space_->covers(other); 190 } 191 covers(const StateSpace * other)192 bool covers(const StateSpace *other) const 193 { 194 return space_->covers(other); 195 } 196 params()197 ParamSet ¶ms() 198 { 199 return space_->params(); 200 } 201 202 /** \brief Get the parameters for this space */ params()203 const ParamSet ¶ms() const 204 { 205 return space_->params(); 206 } 207 getLongestValidSegmentFraction()208 double getLongestValidSegmentFraction() const override 209 { 210 return space_->getLongestValidSegmentFraction(); 211 } 212 setLongestValidSegmentFraction(double segmentFraction)213 void setLongestValidSegmentFraction(double segmentFraction) override 214 { 215 space_->setLongestValidSegmentFraction(segmentFraction); 216 } 217 validSegmentCount(const State * state1,const State * state2)218 unsigned int validSegmentCount(const State *state1, const State *state2) const override 219 { 220 return space_->validSegmentCount(state1->as<StateType>()->getState(), 221 state2->as<StateType>()->getState()); 222 } 223 setValidSegmentCountFactor(unsigned int factor)224 void setValidSegmentCountFactor(unsigned int factor) override 225 { 226 space_->setValidSegmentCountFactor(factor); 227 } 228 getValidSegmentCountFactor()229 unsigned int getValidSegmentCountFactor() const override 230 { 231 return space_->getValidSegmentCountFactor(); 232 } 233 getLongestValidSegmentLength()234 double getLongestValidSegmentLength() const override 235 { 236 return space_->getLongestValidSegmentLength(); 237 } 238 computeSignature(std::vector<int> & signature)239 void computeSignature(std::vector<int> &signature) const override 240 { 241 space_->computeSignature(signature); 242 } 243 getDimension()244 unsigned int getDimension() const override 245 { 246 return space_->getDimension(); 247 } 248 getMaximumExtent()249 double getMaximumExtent() const override 250 { 251 return space_->getMaximumExtent(); 252 } 253 getMeasure()254 double getMeasure() const override 255 { 256 return space_->getMeasure(); 257 } 258 enforceBounds(State * state)259 void enforceBounds(State *state) const override 260 { 261 space_->enforceBounds(state->as<StateType>()->getState()); 262 } 263 satisfiesBounds(const State * state)264 bool satisfiesBounds(const State *state) const override 265 { 266 return space_->satisfiesBounds(state->as<StateType>()->getState()); 267 } 268 copyState(State * destination,const State * source)269 void copyState(State *destination, const State *source) const override 270 { 271 space_->copyState(destination->as<StateType>()->getState(), source->as<StateType>()->getState()); 272 } 273 distance(const State * state1,const State * state2)274 double distance(const State *state1, const State *state2) const override 275 { 276 return space_->distance(state1->as<StateType>()->getState(), state2->as<StateType>()->getState()); 277 } 278 getSerializationLength()279 unsigned int getSerializationLength() const override 280 { 281 return space_->getSerializationLength(); 282 } 283 serialize(void * serialization,const State * state)284 void serialize(void *serialization, const State *state) const override 285 { 286 space_->serialize(serialization, state->as<StateType>()->getState()); 287 } 288 deserialize(State * state,const void * serialization)289 void deserialize(State *state, const void *serialization) const override 290 { 291 space_->deserialize(state->as<StateType>()->getState(), serialization); 292 } 293 equalStates(const State * state1,const State * state2)294 bool equalStates(const State *state1, const State *state2) const override 295 { 296 return space_->equalStates(state1->as<StateType>()->getState(), state2->as<StateType>()->getState()); 297 } 298 interpolate(const State * from,const State * to,double t,State * state)299 void interpolate(const State *from, const State *to, double t, State *state) const override 300 { 301 return space_->interpolate(from->as<StateType>()->getState(), to->as<StateType>()->getState(), t, 302 state->as<StateType>()->getState()); 303 } 304 allocDefaultStateSampler()305 StateSamplerPtr allocDefaultStateSampler() const override 306 { 307 return std::make_shared<WrapperStateSampler>(this, space_->allocDefaultStateSampler()); 308 } 309 allocState()310 State *allocState() const override 311 { 312 return new StateType(space_->allocState()); 313 } 314 freeState(State * state)315 void freeState(State *state) const override 316 { 317 auto *wstate = state->as<StateType>(); 318 space_->freeState(wstate->getState()); 319 delete wstate; 320 } 321 getValueAddressAtIndex(State * state,unsigned int index)322 double *getValueAddressAtIndex(State *state, unsigned int index) const override 323 { 324 return space_->getValueAddressAtIndex(state->as<StateType>()->getState(), index); 325 } 326 getValueAddressAtIndex(const State * state,unsigned int index)327 const double *getValueAddressAtIndex(const State *state, unsigned int index) const 328 { 329 return space_->getValueAddressAtIndex(state->as<StateType>()->getState(), index); 330 } 331 getValueLocations()332 const std::vector<ValueLocation> &getValueLocations() const 333 { 334 return space_->getValueLocations(); 335 } 336 getValueLocationsByName()337 const std::map<std::string, ValueLocation> &getValueLocationsByName() const 338 { 339 return space_->getValueLocationsByName(); 340 } 341 getValueAddressAtLocation(State * state,const ValueLocation & loc)342 double *getValueAddressAtLocation(State *state, const ValueLocation &loc) const 343 { 344 return space_->getValueAddressAtLocation(state->as<StateType>()->getState(), loc); 345 } 346 getValueAddressAtLocation(const State * state,const ValueLocation & loc)347 const double *getValueAddressAtLocation(const State *state, const ValueLocation &loc) const 348 { 349 return space_->getValueAddressAtLocation(state->as<StateType>()->getState(), loc); 350 } 351 getValueAddressAtName(State * state,const std::string & name)352 double *getValueAddressAtName(State *state, const std::string &name) const 353 { 354 return space_->getValueAddressAtName(state->as<StateType>()->getState(), name); 355 } 356 getValueAddressAtName(const State * state,const std::string & name)357 const double *getValueAddressAtName(const State *state, const std::string &name) const 358 { 359 return space_->getValueAddressAtName(state->as<StateType>()->getState(), name); 360 } 361 copyToReals(std::vector<double> & reals,const State * source)362 void copyToReals(std::vector<double> &reals, const State *source) const override 363 { 364 space_->copyToReals(reals, source->as<StateType>()->getState()); 365 } 366 copyFromReals(State * destination,const std::vector<double> & reals)367 void copyFromReals(State *destination, const std::vector<double> &reals) const override 368 { 369 space_->copyFromReals(destination->as<StateType>()->getState(), reals); 370 } 371 registerProjections()372 void registerProjections() override 373 { 374 space_->registerProjections(); 375 } 376 377 void printState(const State *state, std::ostream &out = std::cout) const override 378 { 379 space_->printState(state->as<StateType>()->getState(), out); 380 } 381 printSettings(std::ostream & out)382 void printSettings(std::ostream &out) const override 383 { 384 space_->printSettings(out); 385 } 386 printProjections(std::ostream & out)387 void printProjections(std::ostream &out) const override 388 { 389 space_->printProjections(out); 390 } 391 sanityChecks(double zero,double eps,unsigned int flags)392 void sanityChecks(double zero, double eps, unsigned int flags) const override 393 { 394 space_->sanityChecks(zero, eps, flags); 395 } 396 sanityChecks()397 void sanityChecks() const override 398 { 399 space_->sanityChecks(); 400 } 401 allocSubspaceStateSampler(const StateSpace * subspace)402 StateSamplerPtr allocSubspaceStateSampler(const StateSpace *subspace) const override 403 { 404 return space_->allocSubspaceStateSampler(subspace); 405 } 406 computeLocations()407 void computeLocations() override 408 { 409 space_->computeLocations(); 410 } 411 412 void setup() override; 413 getSpace()414 const StateSpacePtr &getSpace() const 415 { 416 return space_; 417 } 418 419 protected: 420 const StateSpacePtr space_; 421 }; 422 } 423 } 424 425 #endif 426