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  ClassForward(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 
177  const std::string& getName(void) const;
178 
180  void setName(const std::string &name);
181 
185  int getType(void) const
186  {
187  return type_;
188  }
189 
191  bool includes(const StateSpacePtr &other) const;
192 
194  bool includes(const StateSpace *other) const;
195 
198  bool covers(const StateSpacePtr &other) const;
199 
202  bool covers(const StateSpace *other) const;
203 
206  {
207  return params_;
208  }
209 
211  const ParamSet& params(void) const
212  {
213  return params_;
214  }
215 
221  virtual double getLongestValidSegmentFraction(void) const;
222 
233  virtual void setLongestValidSegmentFraction(double segmentFraction);
234 
236  virtual unsigned int validSegmentCount(const State *state1, const State *state2) const;
237 
244  void setValidSegmentCountFactor(unsigned int factor);
245 
247  unsigned int getValidSegmentCountFactor(void) const;
248 
251  void computeSignature(std::vector<int> &signature) const;
252 
259  virtual unsigned int getDimension(void) const = 0;
260 
267  virtual double getMaximumExtent(void) const = 0;
268 
271  virtual void enforceBounds(State *state) const = 0;
272 
275  virtual bool satisfiesBounds(const State *state) const = 0;
276 
279  virtual void copyState(State *destination, const State *source) const = 0;
280 
283  virtual double distance(const State *state1, const State *state2) const = 0;
284 
286  virtual unsigned int getSerializationLength(void) const;
287 
289  virtual void serialize(void *serialization, const State *state) const;
290 
292  virtual void deserialize(State *state, const void *serialization) const;
293 
295  virtual bool equalStates(const State *state1, const State *state2) const = 0;
296 
300  virtual void interpolate(const State *from, const State *to, const double t, State *state) const = 0;
301 
303  virtual StateSamplerPtr allocDefaultStateSampler(void) const = 0;
304 
308  virtual StateSamplerPtr allocStateSampler(void) const;
309 
312 
314  void clearStateSamplerAllocator(void);
315 
317  virtual State* allocState(void) const = 0;
318 
320  virtual void freeState(State *state) const = 0;
321 
339  virtual double* getValueAddressAtIndex(State *state, const unsigned int index) const;
340 
342  const double* getValueAddressAtIndex(const State *state, const unsigned int index) const;
343 
346  const std::vector<ValueLocation>& getValueLocations(void) const;
347 
350  const std::map<std::string, ValueLocation>& getValueLocationsByName(void) const;
351 
353  double* getValueAddressAtLocation(State *state, const ValueLocation &loc) const;
354 
356  const double* getValueAddressAtLocation(const State *state, const ValueLocation &loc) const;
357 
359  double* getValueAddressAtName(State *state, const std::string &name) const;
360 
362  const double* getValueAddressAtName(const State *state, const std::string &name) const;
363 
365  void copyToReals(std::vector<double> &reals, const State *source) const;
366 
368  void copyFromReals(State *destination, const std::vector<double> &reals) const;
369 
376  void registerProjection(const std::string &name, const ProjectionEvaluatorPtr &projection);
377 
379  void registerDefaultProjection(const ProjectionEvaluatorPtr &projection);
380 
383  virtual void registerProjections(void);
384 
386  ProjectionEvaluatorPtr getProjection(const std::string &name) const;
387 
390 
392  bool hasProjection(const std::string &name) const;
393 
395  bool hasDefaultProjection(void) const;
396 
398  const std::map<std::string, ProjectionEvaluatorPtr>& getRegisteredProjections(void) const;
399 
406  virtual void printState(const State *state, std::ostream &out) const;
407 
409  virtual void printSettings(std::ostream &out) const;
410 
412  virtual void printProjections(std::ostream &out) const;
413 
416  virtual void sanityChecks(double zero, double eps, unsigned int flags) const;
417 
420  virtual void sanityChecks(void) const;
421 
423  void diagram(std::ostream &out) const;
424 
426  void list(std::ostream &out) const;
427 
429  static void Diagram(std::ostream &out);
430 
432  static void List(std::ostream &out);
433 
441 
443  virtual StateSamplerPtr allocSubspaceStateSampler(const StateSpace *subspace) const;
444 
446  State* getSubstateAtLocation(State *state, const SubstateLocation &loc) const;
447 
449  const State* getSubstateAtLocation(const State *state, const SubstateLocation &loc) const;
450 
452  const std::map<std::string, SubstateLocation>& getSubstateLocationsByName(void) const;
453 
456  void getCommonSubspaces(const StateSpacePtr &other, std::vector<std::string> &subspaces) const;
457 
460  void getCommonSubspaces(const StateSpace *other, std::vector<std::string> &subspaces) const;
461 
464  virtual void computeLocations(void);
465 
476  virtual void setup(void);
477 
478  protected:
479 
481  static const std::string DEFAULT_PROJECTION_NAME;
482 
484  int type_;
485 
488 
490  double maxExtent_;
491 
494 
497 
500 
502  std::map<std::string, ProjectionEvaluatorPtr> projections_;
503 
506 
509  std::vector<ValueLocation> valueLocationsInOrder_;
510 
513  std::map<std::string, ValueLocation> valueLocationsByName_;
514 
516  std::map<std::string, SubstateLocation> substateLocationsByName_;
517 
518  private:
519 
521  std::string name_;
522  };
523 
526  {
527  public:
528 
531 
533  CompoundStateSpace(void);
534 
536  CompoundStateSpace(const std::vector<StateSpacePtr> &components, const std::vector<double> &weights);
537 
538  virtual ~CompoundStateSpace(void)
539  {
540  }
541 
543  template<class T>
544  T* as(const unsigned int index) const
545  {
547  BOOST_CONCEPT_ASSERT((boost::Convertible<T*, StateSpace*>));
548 
549  return static_cast<T*>(getSubspace(index).get());
550  }
551 
553  template<class T>
554  T* as(const std::string &name) const
555  {
557  BOOST_CONCEPT_ASSERT((boost::Convertible<T*, StateSpace*>));
558 
559  return static_cast<T*>(getSubspace(name).get());
560  }
561 
562  virtual bool isCompound(void) const;
563 
564  virtual bool isHybrid(void) const;
565 
571  void addSubspace(const StateSpacePtr &component, double weight);
572 
574  unsigned int getSubspaceCount(void) const;
575 
577  const StateSpacePtr& getSubspace(const unsigned int index) const;
578 
580  const StateSpacePtr& getSubspace(const std::string& name) const;
581 
583  unsigned int getSubspaceIndex(const std::string& name) const;
584 
586  bool hasSubspace(const std::string &name) const;
587 
589  double getSubspaceWeight(const unsigned int index) const;
590 
592  double getSubspaceWeight(const std::string &name) const;
593 
595  void setSubspaceWeight(const unsigned int index, double weight);
596 
598  void setSubspaceWeight(const std::string &name, double weight);
599 
601  const std::vector<StateSpacePtr>& getSubspaces(void) const;
602 
604  const std::vector<double>& getSubspaceWeights(void) const;
605 
609  bool isLocked(void) const;
610 
616  void lock(void);
622  virtual StateSamplerPtr allocSubspaceStateSampler(const StateSpace *subspace) const;
623 
629  virtual unsigned int getDimension(void) const;
630 
631  virtual double getMaximumExtent(void) const;
632 
633  virtual void enforceBounds(State *state) const;
634 
635  virtual bool satisfiesBounds(const State *state) const;
636 
637  virtual void copyState(State *destination, const State *source) const;
638 
639  virtual unsigned int getSerializationLength(void) const;
640 
641  virtual void serialize(void *serialization, const State *state) const;
642 
643  virtual void deserialize(State *state, const void *serialization) const;
644 
645  virtual double distance(const State *state1, const State *state2) const;
646 
652  virtual void setLongestValidSegmentFraction(double segmentFraction);
653 
656  virtual unsigned int validSegmentCount(const State *state1, const State *state2) const;
657 
658  virtual bool equalStates(const State *state1, const State *state2) const;
659 
660  virtual void interpolate(const State *from, const State *to, const double t, State *state) const;
661 
662  virtual StateSamplerPtr allocDefaultStateSampler(void) const;
663 
664  virtual State* allocState(void) const;
665 
666  virtual void freeState(State *state) const;
667 
668  virtual double* getValueAddressAtIndex(State *state, const unsigned int index) const;
669 
672  virtual void printState(const State *state, std::ostream &out) const;
673 
674  virtual void printSettings(std::ostream &out) const;
675 
676  virtual void computeLocations(void);
677 
678  virtual void setup(void);
679 
680  protected:
681 
683  void allocStateComponents(CompoundState *state) const;
684 
686  std::vector<StateSpacePtr> components_;
687 
689  unsigned int componentCount_;
690 
692  std::vector<double> weights_;
693 
695  double weightSum_;
696 
698  bool locked_;
699 
700  };
701 
715 
723 
726  StateSpacePtr operator-(const StateSpacePtr &a, const std::string &name);
727 
740  {
743 
746 
749  };
750 
758  AdvancedStateCopyOperation copyStateData(const StateSpacePtr &destS, State *dest,
759  const StateSpacePtr &sourceS, const State *source);
760 
768  AdvancedStateCopyOperation copyStateData(const StateSpace *destS, State *dest,
769  const StateSpace *sourceS, const State *source);
770 
776  AdvancedStateCopyOperation copyStateData(const StateSpacePtr &destS, State *dest,
777  const StateSpacePtr &sourceS, const State *source,
778  const std::vector<std::string> &subspaces);
779 
785  AdvancedStateCopyOperation copyStateData(const StateSpace *destS, State *dest,
786  const StateSpace *sourceS, const State *source,
787  const std::vector<std::string> &subspaces);
790  }
791 }
792 
793 #endif