All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
StateSpace.h
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 #ifndef OMPL_BASE_STATE_SPACE_
38 #define OMPL_BASE_STATE_SPACE_
39 
40 #include "ompl/base/State.h"
41 #include "ompl/base/StateSpaceTypes.h"
42 #include "ompl/base/StateSampler.h"
43 #include "ompl/base/ProjectionEvaluator.h"
44 #include "ompl/base/GenericParam.h"
45 #include "ompl/util/Console.h"
46 #include "ompl/util/ClassForward.h"
47 #include <boost/concept_check.hpp>
48 #include <boost/noncopyable.hpp>
49 #include <iostream>
50 #include <vector>
51 #include <string>
52 #include <map>
53 
54 namespace ompl
55 {
56  namespace base
57  {
58 
60 
61  OMPL_CLASS_FORWARD(StateSpace);
63 
73  class StateSpace : private boost::noncopyable
74  {
75  public:
76 
78  typedef State StateType;
79 
81  StateSpace(void);
82 
83  virtual ~StateSpace(void);
84 
86  template<class T>
87  T* as(void)
88  {
90  BOOST_CONCEPT_ASSERT((boost::Convertible<T*, StateSpace*>));
91 
92  return static_cast<T*>(this);
93  }
94 
96  template<class T>
97  const T* as(void) const
98  {
100  BOOST_CONCEPT_ASSERT((boost::Convertible<T*, StateSpace*>));
101 
102  return static_cast<const T*>(this);
103  }
104 
107  {
113  std::vector<std::size_t> chain;
114 
117  };
118 
121  {
124 
126  std::size_t index;
127  };
128 
132  {
133 
136 
139 
142 
145 
148 
151 
154 
157  };
158 
163  virtual bool isCompound(void) const;
164 
171  virtual bool isDiscrete(void) const;
172 
174  virtual bool isHybrid(void) const;
175 
178  virtual bool isMetricSpace(void) const
179  {
180  return true;
181  }
182 
184  const std::string& getName(void) const;
185 
187  void setName(const std::string &name);
188 
192  int getType(void) const
193  {
194  return type_;
195  }
196 
198  bool includes(const StateSpacePtr &other) const;
199 
201  bool includes(const StateSpace *other) const;
202 
205  bool covers(const StateSpacePtr &other) const;
206 
209  bool covers(const StateSpace *other) const;
210 
213  {
214  return params_;
215  }
216 
218  const ParamSet& params(void) const
219  {
220  return params_;
221  }
222 
228  virtual double getLongestValidSegmentFraction(void) const;
229 
240  virtual void setLongestValidSegmentFraction(double segmentFraction);
241 
243  virtual unsigned int validSegmentCount(const State *state1, const State *state2) const;
244 
251  void setValidSegmentCountFactor(unsigned int factor);
252 
254  unsigned int getValidSegmentCountFactor(void) const;
255 
258  void computeSignature(std::vector<int> &signature) const;
259 
266  virtual unsigned int getDimension(void) const = 0;
267 
274  virtual double getMaximumExtent(void) const = 0;
275 
278  virtual void enforceBounds(State *state) const = 0;
279 
282  virtual bool satisfiesBounds(const State *state) const = 0;
283 
286  virtual void copyState(State *destination, const State *source) const = 0;
287 
290  virtual double distance(const State *state1, const State *state2) const = 0;
291 
293  virtual unsigned int getSerializationLength(void) const;
294 
296  virtual void serialize(void *serialization, const State *state) const;
297 
299  virtual void deserialize(State *state, const void *serialization) const;
300 
302  virtual bool equalStates(const State *state1, const State *state2) const = 0;
303 
307  virtual void interpolate(const State *from, const State *to, const double t, State *state) const = 0;
308 
310  virtual StateSamplerPtr allocDefaultStateSampler(void) const = 0;
311 
315  virtual StateSamplerPtr allocStateSampler(void) const;
316 
319 
321  void clearStateSamplerAllocator(void);
322 
324  virtual State* allocState(void) const = 0;
325 
327  virtual void freeState(State *state) const = 0;
328 
346  virtual double* getValueAddressAtIndex(State *state, const unsigned int index) const;
347 
349  const double* getValueAddressAtIndex(const State *state, const unsigned int index) const;
350 
353  const std::vector<ValueLocation>& getValueLocations(void) const;
354 
357  const std::map<std::string, ValueLocation>& getValueLocationsByName(void) const;
358 
360  double* getValueAddressAtLocation(State *state, const ValueLocation &loc) const;
361 
363  const double* getValueAddressAtLocation(const State *state, const ValueLocation &loc) const;
364 
366  double* getValueAddressAtName(State *state, const std::string &name) const;
367 
369  const double* getValueAddressAtName(const State *state, const std::string &name) const;
370 
372  void copyToReals(std::vector<double> &reals, const State *source) const;
373 
375  void copyFromReals(State *destination, const std::vector<double> &reals) const;
376 
383  void registerProjection(const std::string &name, const ProjectionEvaluatorPtr &projection);
384 
386  void registerDefaultProjection(const ProjectionEvaluatorPtr &projection);
387 
390  virtual void registerProjections(void);
391 
393  ProjectionEvaluatorPtr getProjection(const std::string &name) const;
394 
397 
399  bool hasProjection(const std::string &name) const;
400 
402  bool hasDefaultProjection(void) const;
403 
405  const std::map<std::string, ProjectionEvaluatorPtr>& getRegisteredProjections(void) const;
406 
413  virtual void printState(const State *state, std::ostream &out) const;
414 
416  virtual void printSettings(std::ostream &out) const;
417 
419  virtual void printProjections(std::ostream &out) const;
420 
423  virtual void sanityChecks(double zero, double eps, unsigned int flags) const;
424 
427  virtual void sanityChecks(void) const;
428 
430  void diagram(std::ostream &out) const;
431 
433  void list(std::ostream &out) const;
434 
436  static void Diagram(std::ostream &out);
437 
439  static void List(std::ostream &out);
440 
448 
450  virtual StateSamplerPtr allocSubspaceStateSampler(const StateSpace *subspace) const;
451 
453  State* getSubstateAtLocation(State *state, const SubstateLocation &loc) const;
454 
456  const State* getSubstateAtLocation(const State *state, const SubstateLocation &loc) const;
457 
459  const std::map<std::string, SubstateLocation>& getSubstateLocationsByName(void) const;
460 
463  void getCommonSubspaces(const StateSpacePtr &other, std::vector<std::string> &subspaces) const;
464 
467  void getCommonSubspaces(const StateSpace *other, std::vector<std::string> &subspaces) const;
468 
471  virtual void computeLocations(void);
472 
483  virtual void setup(void);
484 
485  protected:
486 
488  static const std::string DEFAULT_PROJECTION_NAME;
489 
491  int type_;
492 
495 
497  double maxExtent_;
498 
501 
504 
507 
509  std::map<std::string, ProjectionEvaluatorPtr> projections_;
510 
513 
516  std::vector<ValueLocation> valueLocationsInOrder_;
517 
520  std::map<std::string, ValueLocation> valueLocationsByName_;
521 
523  std::map<std::string, SubstateLocation> substateLocationsByName_;
524 
525  private:
526 
528  std::string name_;
529  };
530 
533  {
534  public:
535 
538 
540  CompoundStateSpace(void);
541 
543  CompoundStateSpace(const std::vector<StateSpacePtr> &components, const std::vector<double> &weights);
544 
545  virtual ~CompoundStateSpace(void)
546  {
547  }
548 
550  template<class T>
551  T* as(const unsigned int index) const
552  {
554  BOOST_CONCEPT_ASSERT((boost::Convertible<T*, StateSpace*>));
555 
556  return static_cast<T*>(getSubspace(index).get());
557  }
558 
560  template<class T>
561  T* as(const std::string &name) const
562  {
564  BOOST_CONCEPT_ASSERT((boost::Convertible<T*, StateSpace*>));
565 
566  return static_cast<T*>(getSubspace(name).get());
567  }
568 
569  virtual bool isCompound(void) const;
570 
571  virtual bool isHybrid(void) const;
572 
578  void addSubspace(const StateSpacePtr &component, double weight);
579 
581  unsigned int getSubspaceCount(void) const;
582 
584  const StateSpacePtr& getSubspace(const unsigned int index) const;
585 
587  const StateSpacePtr& getSubspace(const std::string& name) const;
588 
590  unsigned int getSubspaceIndex(const std::string& name) const;
591 
593  bool hasSubspace(const std::string &name) const;
594 
596  double getSubspaceWeight(const unsigned int index) const;
597 
599  double getSubspaceWeight(const std::string &name) const;
600 
602  void setSubspaceWeight(const unsigned int index, double weight);
603 
605  void setSubspaceWeight(const std::string &name, double weight);
606 
608  const std::vector<StateSpacePtr>& getSubspaces(void) const;
609 
611  const std::vector<double>& getSubspaceWeights(void) const;
612 
616  bool isLocked(void) const;
617 
623  void lock(void);
629  virtual StateSamplerPtr allocSubspaceStateSampler(const StateSpace *subspace) const;
630 
636  virtual unsigned int getDimension(void) const;
637 
638  virtual double getMaximumExtent(void) const;
639 
640  virtual void enforceBounds(State *state) const;
641 
642  virtual bool satisfiesBounds(const State *state) const;
643 
644  virtual void copyState(State *destination, const State *source) const;
645 
646  virtual unsigned int getSerializationLength(void) const;
647 
648  virtual void serialize(void *serialization, const State *state) const;
649 
650  virtual void deserialize(State *state, const void *serialization) const;
651 
652  virtual double distance(const State *state1, const State *state2) const;
653 
659  virtual void setLongestValidSegmentFraction(double segmentFraction);
660 
663  virtual unsigned int validSegmentCount(const State *state1, const State *state2) const;
664 
665  virtual bool equalStates(const State *state1, const State *state2) const;
666 
667  virtual void interpolate(const State *from, const State *to, const double t, State *state) const;
668 
669  virtual StateSamplerPtr allocDefaultStateSampler(void) const;
670 
671  virtual State* allocState(void) const;
672 
673  virtual void freeState(State *state) const;
674 
675  virtual double* getValueAddressAtIndex(State *state, const unsigned int index) const;
676 
679  virtual void printState(const State *state, std::ostream &out) const;
680 
681  virtual void printSettings(std::ostream &out) const;
682 
683  virtual void computeLocations(void);
684 
685  virtual void setup(void);
686 
687  protected:
688 
690  void allocStateComponents(CompoundState *state) const;
691 
693  std::vector<StateSpacePtr> components_;
694 
696  unsigned int componentCount_;
697 
699  std::vector<double> weights_;
700 
702  double weightSum_;
703 
705  bool locked_;
706 
707  };
708 
722 
730 
733  StateSpacePtr operator-(const StateSpacePtr &a, const std::string &name);
734 
747  {
750 
753 
756  };
757 
765  AdvancedStateCopyOperation copyStateData(const StateSpacePtr &destS, State *dest,
766  const StateSpacePtr &sourceS, const State *source);
767 
775  AdvancedStateCopyOperation copyStateData(const StateSpace *destS, State *dest,
776  const StateSpace *sourceS, const State *source);
777 
783  AdvancedStateCopyOperation copyStateData(const StateSpacePtr &destS, State *dest,
784  const StateSpacePtr &sourceS, const State *source,
785  const std::vector<std::string> &subspaces);
786 
792  AdvancedStateCopyOperation copyStateData(const StateSpace *destS, State *dest,
793  const StateSpace *sourceS, const State *source,
794  const std::vector<std::string> &subspaces);
797  }
798 }
799 
800 #endif