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 <iostream>
49 #include <vector>
50 #include <string>
51 #include <map>
52 
53 namespace ompl
54 {
55  namespace base
56  {
58 
59  OMPL_CLASS_FORWARD(StateSpace);
61 
70  class StateSpace
71  {
72  public:
73  // non-copyable
74  StateSpace(const StateSpace &) = delete;
75  StateSpace &operator=(const StateSpace &) = delete;
76 
79 
81  StateSpace();
82 
83  virtual ~StateSpace();
84 
86  template <class T>
87  T *as()
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() const
98  {
100  BOOST_CONCEPT_ASSERT((boost::Convertible<T *, StateSpace *>));
101 
102  return static_cast<const T *>(this);
103  }
104 
107  struct SubstateLocation
108  {
114  std::vector<std::size_t> chain;
115 
117  const StateSpace *space;
118  };
119 
122  struct ValueLocation
123  {
125  SubstateLocation stateLocation;
126 
128  std::size_t index;
129  };
130 
135  {
136 
140 
143 
145  STATESPACE_INTERPOLATION = (1 << 3),
146 
150 
152  STATESPACE_DISTANCE_BOUND = (1 << 5),
153 
155  STATESPACE_RESPECT_BOUNDS = (1 << 6),
156 
159 
162  };
163 
168  virtual bool isCompound() const;
169 
176  virtual bool isDiscrete() const;
177 
179  virtual bool isHybrid() const;
180 
183  virtual bool isMetricSpace() const
184  {
185  return true;
186  }
187 
190  virtual bool hasSymmetricDistance() const;
191 
194  virtual bool hasSymmetricInterpolate() const;
195 
197  const std::string &getName() const;
198 
200  void setName(const std::string &name);
201 
205  int getType() const
206  {
207  return type_;
208  }
209 
211  bool includes(const StateSpacePtr &other) const;
212 
214  bool includes(const StateSpace *other) const;
215 
218  bool covers(const StateSpacePtr &other) const;
219 
222  bool covers(const StateSpace *other) const;
223 
226  {
227  return params_;
228  }
229 
231  const ParamSet &params() const
232  {
233  return params_;
234  }
235 
241  virtual double getLongestValidSegmentFraction() const;
242 
253  virtual void setLongestValidSegmentFraction(double segmentFraction);
254 
257  virtual unsigned int validSegmentCount(const State *state1, const State *state2) const;
258 
265  virtual void setValidSegmentCountFactor(unsigned int factor);
266 
268  virtual unsigned int getValidSegmentCountFactor() const;
269 
271  virtual double getLongestValidSegmentLength() const;
272 
275  virtual void computeSignature(std::vector<int> &signature) const;
276 
283  virtual unsigned int getDimension() const = 0;
284 
291  virtual double getMaximumExtent() const = 0;
292 
294  virtual double getMeasure() const = 0;
295 
298  virtual void enforceBounds(State *state) const = 0;
299 
302  virtual bool satisfiesBounds(const State *state) const = 0;
303 
307  virtual void copyState(State *destination, const State *source) const = 0;
308 
310  State *cloneState(const State *source) const;
311 
315  virtual double distance(const State *state1, const State *state2) const = 0;
316 
318  virtual unsigned int getSerializationLength() const;
319 
321  virtual void serialize(void *serialization, const State *state) const;
322 
324  virtual void deserialize(State *state, const void *serialization) const;
325 
327  virtual bool equalStates(const State *state1, const State *state2) const = 0;
328 
333  virtual void interpolate(const State *from, const State *to, double t, State *state) const = 0;
334 
336  virtual StateSamplerPtr allocDefaultStateSampler() const = 0;
337 
342  virtual StateSamplerPtr allocStateSampler() const;
343 
346 
349 
351  virtual State *allocState() const = 0;
352 
354  virtual void freeState(State *state) const = 0;
355 
376  virtual double *getValueAddressAtIndex(State *state, unsigned int index) const;
377 
379  const double *getValueAddressAtIndex(const State *state, unsigned int index) const;
380 
384  const std::vector<ValueLocation> &getValueLocations() const;
385 
388  const std::map<std::string, ValueLocation> &getValueLocationsByName() const;
389 
391  double *getValueAddressAtLocation(State *state, const ValueLocation &loc) const;
392 
394  const double *getValueAddressAtLocation(const State *state, const ValueLocation &loc) const;
395 
397  double *getValueAddressAtName(State *state, const std::string &name) const;
398 
400  const double *getValueAddressAtName(const State *state, const std::string &name) const;
401 
404  virtual void copyToReals(std::vector<double> &reals, const State *source) const;
405 
407  virtual void copyFromReals(State *destination, const std::vector<double> &reals) const;
408 
415  void registerProjection(const std::string &name, const ProjectionEvaluatorPtr &projection);
416 
418  void registerDefaultProjection(const ProjectionEvaluatorPtr &projection);
419 
423  virtual void registerProjections();
424 
426  ProjectionEvaluatorPtr getProjection(const std::string &name) const;
427 
430 
432  bool hasProjection(const std::string &name) const;
433 
435  bool hasDefaultProjection() const;
436 
438  const std::map<std::string, ProjectionEvaluatorPtr> &getRegisteredProjections() const;
439 
446  virtual void printState(const State *state, std::ostream &out = std::cout) const;
447 
449  virtual void printSettings(std::ostream &out) const;
450 
452  virtual void printProjections(std::ostream &out) const;
453 
456  virtual void sanityChecks(double zero, double eps, unsigned int flags) const;
457 
461  virtual void sanityChecks() const;
462 
464  void diagram(std::ostream &out) const;
465 
467  void list(std::ostream &out) const;
468 
471  static void Diagram(std::ostream &out);
472 
474  static void List(std::ostream &out);
475 
483 
485  virtual StateSamplerPtr allocSubspaceStateSampler(const StateSpace *subspace) const;
486 
488  State *getSubstateAtLocation(State *state, const SubstateLocation &loc) const;
489 
491  const State *getSubstateAtLocation(const State *state, const SubstateLocation &loc) const;
492 
494  const std::map<std::string, SubstateLocation> &getSubstateLocationsByName() const;
495 
500  void getCommonSubspaces(const StateSpacePtr &other, std::vector<std::string> &subspaces) const;
501 
506  void getCommonSubspaces(const StateSpace *other, std::vector<std::string> &subspaces) const;
507 
512  virtual void computeLocations();
513 
524  virtual void setup();
525 
526  protected:
528  static const std::string DEFAULT_PROJECTION_NAME;
529 
531  int type_;
532 
535 
537  double maxExtent_;
538 
541 
543  double longestValidSegment_;
544 
547  unsigned int longestValidSegmentCountFactor_;
548 
550  std::map<std::string, ProjectionEvaluatorPtr> projections_;
551 
554 
557  std::vector<ValueLocation> valueLocationsInOrder_;
558 
562  std::map<std::string, ValueLocation> valueLocationsByName_;
563 
565  std::map<std::string, SubstateLocation> substateLocationsByName_;
566 
567  private:
569  std::string name_;
570  };
571 
573  class CompoundStateSpace : public StateSpace
574  {
575  public:
578 
581 
584  CompoundStateSpace(const std::vector<StateSpacePtr> &components, const std::vector<double> &weights);
585 
586  ~CompoundStateSpace() override = default;
587 
589  template <class T>
590  T *as(const unsigned int index) const
591  {
593  BOOST_CONCEPT_ASSERT((boost::Convertible<T *, StateSpace *>));
594 
595  return static_cast<T *>(getSubspace(index).get());
596  }
597 
599  template <class T>
600  T *as(const std::string &name) const
601  {
603  BOOST_CONCEPT_ASSERT((boost::Convertible<T *, StateSpace *>));
604 
605  return static_cast<T *>(getSubspace(name).get());
606  }
607 
608  bool isCompound() const override;
609 
610  bool isHybrid() const override;
611 
618  void addSubspace(const StateSpacePtr &component, double weight);
619 
621  unsigned int getSubspaceCount() const;
622 
624  const StateSpacePtr &getSubspace(unsigned int index) const;
625 
627  const StateSpacePtr &getSubspace(const std::string &name) const;
628 
630  unsigned int getSubspaceIndex(const std::string &name) const;
631 
633  bool hasSubspace(const std::string &name) const;
634 
636  double getSubspaceWeight(unsigned int index) const;
637 
639  double getSubspaceWeight(const std::string &name) const;
640 
642  void setSubspaceWeight(unsigned int index, double weight);
643 
645  void setSubspaceWeight(const std::string &name, double weight);
646 
648  const std::vector<StateSpacePtr> &getSubspaces() const;
649 
651  const std::vector<double> &getSubspaceWeights() const;
652 
656  bool isLocked() const;
657 
663  void lock();
669  StateSamplerPtr allocSubspaceStateSampler(const StateSpace *subspace) const override;
670 
676  unsigned int getDimension() const override;
677 
678  double getMaximumExtent() const override;
679 
680  double getMeasure() const override;
681 
682  void enforceBounds(State *state) const override;
683 
684  bool satisfiesBounds(const State *state) const override;
685 
686  void copyState(State *destination, const State *source) const override;
687 
688  unsigned int getSerializationLength() const override;
689 
690  void serialize(void *serialization, const State *state) const override;
691 
692  void deserialize(State *state, const void *serialization) const override;
693 
694  double distance(const State *state1, const State *state2) const override;
695 
701  void setLongestValidSegmentFraction(double segmentFraction) override;
702 
706  unsigned int validSegmentCount(const State *state1, const State *state2) const override;
707 
708  bool equalStates(const State *state1, const State *state2) const override;
709 
710  void interpolate(const State *from, const State *to, double t, State *state) const override;
711 
712  StateSamplerPtr allocDefaultStateSampler() const override;
713 
714  State *allocState() const override;
715 
716  void freeState(State *state) const override;
717 
718  double *getValueAddressAtIndex(State *state, unsigned int index) const override;
719 
722  void printState(const State *state, std::ostream &out) const override;
723 
724  void printSettings(std::ostream &out) const override;
725 
726  void computeLocations() override;
727 
728  void setup() override;
729 
730  protected:
732  void allocStateComponents(CompoundState *state) const;
733 
735  std::vector<StateSpacePtr> components_;
736 
738  unsigned int componentCount_{0u};
739 
741  std::vector<double> weights_;
742 
744  double weightSum_{0.0};
745 
747  bool locked_{false};
748  };
749 
763 
771 
774  StateSpacePtr operator-(const StateSpacePtr &a, const std::string &name);
775 
787  {
789  NO_DATA_COPIED = 0,
790 
792  SOME_DATA_COPIED = 1,
793 
795  ALL_DATA_COPIED = 2
796  };
797 
806  const State *source);
807 
815  AdvancedStateCopyOperation copyStateData(const StateSpace *destS, State *dest, const StateSpace *sourceS,
816  const State *source);
817 
824  AdvancedStateCopyOperation copyStateData(const StateSpacePtr &destS, State *dest, const StateSpacePtr &sourceS,
825  const State *source, const std::vector<std::string> &subspaces);
826 
833  AdvancedStateCopyOperation copyStateData(const StateSpace *destS, State *dest, const StateSpace *sourceS,
834  const State *source, const std::vector<std::string> &subspaces);
836  }
837 }
838 
839 #endif
ParamSet & params()
Get the parameters for this space.
Definition: StateSpace.h:289
@ STATESPACE_DISTANCE_SYMMETRIC
Check whether the distance function is symmetric (StateSpace::distance())
Definition: StateSpace.h:206
ProjectionEvaluatorPtr getDefaultProjection() const
Get the default projection.
Definition: StateSpace.cpp:723
Definition of a compound state.
Definition: State.h:150
virtual void freeState(State *state) const =0
Free the memory of the allocated state.
const std::map< std::string, ProjectionEvaluatorPtr > & getRegisteredProjections() const
Get all the registered projections.
Definition: StateSpace.cpp:747
double maxExtent_
The extent of this space at the time setup() was called.
Definition: StateSpace.h:601
ParamSet params_
The set of parameters for this space.
Definition: StateSpace.h:617
void getCommonSubspaces(const StateSpacePtr &other, std::vector< std::string > &subspaces) const
Get the set of subspaces that this space and other have in common. The computed list of subspaces doe...
Definition: StateSpace.cpp:493
double * getValueAddressAtIndex(State *state, unsigned int index) const override
Many states contain a number of double values. This function provides a means to get the memory addre...
Maintain a set of parameters.
Definition: GenericParam.h:289
void clearStateSamplerAllocator()
Clear the state sampler allocator (reset to default)
Definition: StateSpace.cpp:795
State * cloneState(const State *source) const
Clone a state.
Definition: StateSpace.cpp:226
virtual unsigned int getSerializationLength() const
Get the number of chars in the serialization of a state in this space.
Definition: StateSpace.cpp:375
double longestValidSegment_
The longest valid segment at the time setup() was called.
Definition: StateSpace.h:607
virtual double getLongestValidSegmentFraction() const
When performing discrete validation of motions, the length of the longest segment that does not requi...
Definition: StateSpace.cpp:841
A space to allow the composition of state spaces.
Definition: StateSpace.h:637
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition: StateSpace.h:134
void enforceBounds(State *state) const override
Bring the state within the bounds of the state space. For unbounded spaces this function can be a no-...
void interpolate(const State *from, const State *to, double t, State *state) const override
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state....
void printSettings(std::ostream &out) const override
Print the settings for this state space to a stream.
AdvancedStateCopyOperation copyStateData(const StateSpacePtr &destS, State *dest, const StateSpacePtr &sourceS, const State *source)
Copy data from source (state from space sourceS) to dest (state from space destS) on a component by c...
int getType() const
Get the type of the state space. The type can be used to verify whether two space instances are of th...
Definition: StateSpace.h:269
virtual void setValidSegmentCountFactor(unsigned int factor)
Set factor to be the value to multiply the return value of validSegmentCount(). By default,...
Definition: StateSpace.cpp:820
std::vector< std::size_t > chain
In a complex state space there may be multiple compound state spaces that make up an even larger comp...
Definition: StateSpace.h:178
void registerProjection(const std::string &name, const ProjectionEvaluatorPtr &projection)
Register a projection for this state space under a specified name.
Definition: StateSpace.cpp:757
StateSamplerPtr allocDefaultStateSampler() const override
Allocate an instance of the default uniform state sampler for this space.
Definition of an abstract state.
Definition: State.h:113
State * getSubstateAtLocation(State *state, const SubstateLocation &loc) const
Get the substate of state that is pointed to by loc.
Definition: StateSpace.cpp:289
Representation of the address of a value in a state. This structure stores the indexing information n...
Definition: StateSpace.h:186
virtual void deserialize(State *state, const void *serialization) const
Read the binary representation of a state from serialization and write it to state.
Definition: StateSpace.cpp:384
StateSamplerAllocator ssa_
An optional state sampler allocator.
Definition: StateSpace.h:598
SubstateLocation stateLocation
Location of the substate that contains the pointed to value.
Definition: StateSpace.h:189
const StateSpacePtr & getSubspace(unsigned int index) const
Get a specific subspace from the compound state space.
Definition: StateSpace.cpp:909
void computeLocations() override
Compute the location information for various components of the state space. Either this function or s...
virtual void sanityChecks() const
Convenience function that allows derived state spaces to choose which checks should pass (see SanityC...
Definition: StateSpace.cpp:603
virtual void interpolate(const State *from, const State *to, double t, State *state) const =0
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state....
void lock()
Lock this state space. This means no further spaces can be added as components. This function can be ...
virtual unsigned int getValidSegmentCountFactor() const
Get the value used to multiply the return value of validSegmentCount().
Definition: StateSpace.cpp:836
virtual StateSamplerPtr allocStateSampler() const
Allocate an instance of the state sampler for this space. This sampler will be allocated with the sam...
Definition: StateSpace.cpp:800
const std::vector< ValueLocation > & getValueLocations() const
Get the locations of values of type double contained in a state from this space. The order of the val...
Definition: StateSpace.cpp:318
unsigned int getSubspaceCount() const
Get the number of state spaces that make up the compound state space.
Definition: StateSpace.cpp:904
@ STATESPACE_DISTANCE_BOUND
Check whether the StateSpace::distance() is bounded by StateSpace::getExtent()
Definition: StateSpace.h:216
unsigned int getDimension() const override
Get the dimension of the space (not the dimension of the surrounding ambient space)
Definition: StateSpace.cpp:988
bool isCompound() const override
Check if the state space is compound.
Definition: StateSpace.cpp:883
const std::vector< double > & getSubspaceWeights() const
Get the list of component weights.
Definition: StateSpace.cpp:983
StateSpace()
Constructor. Assigns a unique name to the space.
Definition: StateSpace.cpp:84
virtual void copyState(State *destination, const State *source) const =0
Copy a state to another. The memory of source and destination should NOT overlap.
double getSubspaceWeight(unsigned int index) const
Get the weight of a subspace from the compound state space (used in distance computation)
Definition: StateSpace.cpp:938
virtual void enforceBounds(State *state) const =0
Bring the state within the bounds of the state space. For unbounded spaces this function can be a no-...
@ STATESPACE_DISTANCE_DIFFERENT_STATES
Check whether the distances between non-equal states is strictly positive (StateSpace::distance())
Definition: StateSpace.h:203
double distance(const State *state1, const State *state2) const override
Computes distance between two states. This function satisfies the properties of a metric if isMetricS...
bool satisfiesBounds(const State *state) const override
Check if a state is inside the bounding box. For unbounded spaces this function can always return tru...
virtual bool isHybrid() const
Check if this is a hybrid state space (i.e., both discrete and continuous components exist)
Definition: StateSpace.cpp:775
virtual void copyToReals(std::vector< double > &reals, const State *source) const
Copy all the real values from a state source to the array reals using getValueAddressAtLocation()
Definition: StateSpace.cpp:329
std::vector< ValueLocation > valueLocationsInOrder_
The value locations for all varliables of type double contained in a state; The locations point to va...
Definition: StateSpace.h:621
double getMeasure() const override
Get a measure of the space (this can be thought of as a generalization of volume)
double * getValueAddressAtName(State *state, const std::string &name) const
Get a pointer to the double value in state that name points to.
Definition: StateSpace.cpp:361
void deserialize(State *state, const void *serialization) const override
Read the binary representation of a state from serialization and write it to state.
bool isLocked() const
Return true if the state space is locked. A value of true means that no further spaces can be added a...
virtual bool isMetricSpace() const
Return true if the distance function associated with the space is a metric.
Definition: StateSpace.h:247
A shared pointer wrapper for ompl::base::ProjectionEvaluator.
virtual unsigned int getDimension() const =0
Get the dimension of the space (not the dimension of the surrounding ambient space)
virtual double getLongestValidSegmentLength() const
Get the longest valid segment at the time setup() was called.
Definition: StateSpace.cpp:846
void setSubspaceWeight(unsigned int index, double weight)
Set the weight of a subspace in the compound state space (used in distance computation)
Definition: StateSpace.cpp:954
double longestValidSegmentFraction_
The fraction of the longest valid segment.
Definition: StateSpace.h:604
StateSamplerPtr allocSubspaceStateSampler(const StateSpacePtr &subspace) const
Allocate a sampler that actually samples only components that are part of subspace.
Definition: StateSpace.cpp:808
unsigned int componentCount_
The number of components.
Definition: StateSpace.h:802
bool equalStates(const State *state1, const State *state2) const override
Checks whether two states are equal.
void printState(const State *state, std::ostream &out) const override
Print a state to a stream.
static const std::string DEFAULT_PROJECTION_NAME
The name used for the default projection.
Definition: StateSpace.h:592
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:142
void freeState(State *state) const override
Free the memory of the allocated state.
void registerDefaultProjection(const ProjectionEvaluatorPtr &projection)
Register the default projection for this state space.
Definition: StateSpace.cpp:752
virtual void copyFromReals(State *destination, const std::vector< double > &reals) const
Copy the values from reals to the state destination using getValueAddressAtLocation()
Definition: StateSpace.cpp:337
double weightSum_
The sum of all the weights in weights_.
Definition: StateSpace.h:808
virtual bool isDiscrete() const
Check if the set of states is discrete.
Definition: StateSpace.cpp:770
virtual void registerProjections()
Register the projections for this state space. Usually, this is at least the default projection....
Definition: StateSpace.cpp:233
StateSpacePtr operator-(const StateSpacePtr &a, const StateSpacePtr &b)
Construct a compound state space that contains subspaces only from a. If a is compound,...
StateSamplerPtr allocSubspaceStateSampler(const StateSpace *subspace) const override
Allocate a sampler that actually samples only components that are part of subspace.
Representation of the address of a substate in a state. This structure stores the indexing informatio...
Definition: StateSpace.h:171
bool hasProjection(const std::string &name) const
Check if a projection with a specified name is available.
Definition: StateSpace.cpp:718
const std::map< std::string, SubstateLocation > & getSubstateLocationsByName() const
Get the list of known substate locations (keys of the map corrspond to names of subspaces)
Definition: StateSpace.cpp:284
@ STATESPACE_TRIANGLE_INEQUALITY
Check whether the triangle inequality holds when using StateSpace::interpolate() and StateSpace::dist...
Definition: StateSpace.h:213
virtual void setLongestValidSegmentFraction(double segmentFraction)
When performing discrete validation of motions, the length of the longest segment that does not requi...
Definition: StateSpace.cpp:828
State * allocState() const override
Allocate a state that can store a point in the described space.
@ NO_DATA_COPIED
No data was copied.
Definition: StateSpace.h:853
const std::map< std::string, ValueLocation > & getValueLocationsByName() const
Get the named locations of values of type double contained in a state from this space....
Definition: StateSpace.cpp:324
double getMaximumExtent() const override
Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces...
Definition: StateSpace.cpp:996
void copyState(State *destination, const State *source) const override
Copy a state to another. The memory of source and destination should NOT overlap.
std::map< std::string, ProjectionEvaluatorPtr > projections_
List of available projections.
Definition: StateSpace.h:614
const std::string & getName() const
Get the name of the state space.
Definition: StateSpace.cpp:196
virtual void printSettings(std::ostream &out) const
Print the settings for this state space to a stream.
Definition: StateSpace.cpp:393
unsigned int getSerializationLength() const override
Get the number of chars in the serialization of a state in this space.
unsigned int validSegmentCount(const State *state1, const State *state2) const override
Count how many segments of the "longest valid length" fit on the motion from state1 to state2....
virtual void computeLocations()
Compute the location information for various components of the state space. Either this function or s...
Definition: StateSpace.cpp:214
bool hasSubspace(const std::string &name) const
Check if a specific subspace is contained in this state space.
Definition: StateSpace.cpp:917
bool hasDefaultProjection() const
Check if a default projection is available.
Definition: StateSpace.cpp:713
bool includes(const StateSpacePtr &other) const
Return true if other is a space included (perhaps equal, perhaps a subspace) in this one.
Definition: StateSpace.cpp:478
@ STATESPACE_SERIALIZATION
Check whether the StateSpace::serialize() and StateSpace::deserialize() work as expected.
Definition: StateSpace.h:225
AdvancedStateCopyOperation
The possible outputs for an advanced copy operation.
Definition: StateSpace.h:850
virtual void computeSignature(std::vector< int > &signature) const
Compute an array of ints that uniquely identifies the structure of the state space....
Definition: StateSpace.cpp:219
void setName(const std::string &name)
Set the name of the state space.
Definition: StateSpace.cpp:201
int type_
A type assigned for this state space.
Definition: StateSpace.h:595
std::function< StateSamplerPtr(const StateSpace *)> StateSamplerAllocator
Definition of a function that can allocate a state sampler.
Definition: StateSampler.h:255
double * getValueAddressAtLocation(State *state, const ValueLocation &loc) const
Get a pointer to the double value in state that loc points to.
Definition: StateSpace.cpp:345
virtual bool isCompound() const
Check if the state space is compound.
Definition: StateSpace.cpp:765
void allocStateComponents(CompoundState *state) const
Allocate the state components. Called by allocState(). Usually called by derived state spaces.
std::size_t index
The index of the value to be accessed, within the substate location above.
Definition: StateSpace.h:192
ompl::base::CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:641
SanityChecks
Flags to use in a bit mask for state space sanity checks. Some basic checks do not have flags associa...
Definition: StateSpace.h:198
T * as()
Cast this instance to a desired type.
Definition: StateSpace.h:151
unsigned int longestValidSegmentCountFactor_
The factor to multiply the value returned by validSegmentCount(). Rarely used but useful for things l...
Definition: StateSpace.h:611
std::vector< double > weights_
The weight assigned to each component of the state space when computing the compound distance.
Definition: StateSpace.h:805
const StateSpace * space
The space that is reached if the chain above is followed on the state space.
Definition: StateSpace.h:181
bool locked_
Flag indicating whether adding further components is allowed or not.
Definition: StateSpace.h:811
virtual bool satisfiesBounds(const State *state) const =0
Check if a state is inside the bounding box. For unbounded spaces this function can always return tru...
void diagram(std::ostream &out) const
Print a Graphviz digraph that represents the containment diagram for the state space.
Definition: StateSpace.cpp:553
void setLongestValidSegmentFraction(double segmentFraction) override
When performing discrete validation of motions, the length of the longest segment that does not requi...
virtual double getMaximumExtent() const =0
Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces...
@ ALL_DATA_COPIED
All data was copied.
Definition: StateSpace.h:859
std::map< std::string, SubstateLocation > substateLocationsByName_
All the known substat locations, by name.
Definition: StateSpace.h:629
OptimizationObjectivePtr operator+(const OptimizationObjectivePtr &a, const OptimizationObjectivePtr &b)
Given two optimization objectives, returns a MultiOptimizationObjective that combines the two objecti...
virtual bool equalStates(const State *state1, const State *state2) const =0
Checks whether two states are equal.
@ STATESPACE_ENFORCE_BOUNDS_NO_OP
Check that enforceBounds() does not modify the contents of states that are within bounds.
Definition: StateSpace.h:222
virtual double * getValueAddressAtIndex(State *state, unsigned int index) const
Many states contain a number of double values. This function provides a means to get the memory addre...
Definition: StateSpace.cpp:306
virtual double getMeasure() const =0
Get a measure of the space (this can be thought of as a generalization of volume)
A shared pointer wrapper for ompl::base::StateSpace.
const std::vector< StateSpacePtr > & getSubspaces() const
Get the list of components.
Definition: StateSpace.cpp:978
bool covers(const StateSpacePtr &other) const
Return true if other is a space that is either included (perhaps equal, perhaps a subspace) in this o...
Definition: StateSpace.cpp:473
void setStateSamplerAllocator(const StateSamplerAllocator &ssa)
Set the sampler allocator to use.
Definition: StateSpace.cpp:790
void serialize(void *serialization, const State *state) const override
Write the binary representation of state to serialization.
@ STATESPACE_RESPECT_BOUNDS
Check whether sampled states are always within bounds.
Definition: StateSpace.h:219
virtual bool hasSymmetricDistance() const
Check if the distance function on this state space is symmetric, i.e. distance(s1,...
Definition: StateSpace.cpp:780
virtual State * allocState() const =0
Allocate a state that can store a point in the described space.
A shared pointer wrapper for ompl::base::StateSampler.
virtual void printProjections(std::ostream &out) const
Print the list of registered projections. This function is also called by printSettings()
Definition: StateSpace.cpp:399
void list(std::ostream &out) const
Print the list of all contained state space instances.
Definition: StateSpace.cpp:535
static void Diagram(std::ostream &out)
Print a Graphviz digraph that represents the containment diagram for all the instantiated state space...
Definition: StateSpace.cpp:580
@ STATESPACE_INTERPOLATION
Check whether calling StateSpace::interpolate() works as expected.
Definition: StateSpace.h:209
void addSubspace(const StateSpacePtr &component, double weight)
Adds a new state space as part of the compound state space. For computing distances within the compou...
Definition: StateSpace.cpp:871
static void List(std::ostream &out)
Print the list of available state space instances.
Definition: StateSpace.cpp:527
virtual void setup()
Perform final setup steps. This function is automatically called by the SpaceInformation....
Definition: StateSpace.cpp:237
CompoundStateSpace()
Construct an empty compound state space.
Definition: StateSpace.cpp:856
ProjectionEvaluatorPtr getProjection(const std::string &name) const
Get the projection registered under a specific name.
Definition: StateSpace.cpp:734
std::vector< StateSpacePtr > components_
The state spaces that make up the compound state space.
Definition: StateSpace.h:799
bool isHybrid() const override
Check if this is a hybrid state space (i.e., both discrete and continuous components exist)
Definition: StateSpace.cpp:888
void setup() override
Perform final setup steps. This function is automatically called by the SpaceInformation....
virtual double distance(const State *state1, const State *state2) const =0
Computes distance between two states. This function satisfies the properties of a metric if isMetricS...
virtual StateSamplerPtr allocDefaultStateSampler() const =0
Allocate an instance of the default uniform state sampler for this space.
@ SOME_DATA_COPIED
Some data was copied.
Definition: StateSpace.h:856
virtual bool hasSymmetricInterpolate() const
Check if the interpolation function on this state space is symmetric, i.e. interpolate(from,...
Definition: StateSpace.cpp:785
unsigned int getSubspaceIndex(const std::string &name) const
Get the index of a specific subspace from the compound state space.
Definition: StateSpace.cpp:925
virtual unsigned int validSegmentCount(const State *state1, const State *state2) const
Count how many segments of the "longest valid length" fit on the motion from state1 to state2.
Definition: StateSpace.cpp:851
virtual void serialize(void *serialization, const State *state) const
Write the binary representation of state to serialization.
Definition: StateSpace.cpp:380
Main namespace. Contains everything in this library.
virtual void printState(const State *state, std::ostream &out=std::cout) const
Print a state to a stream.
Definition: StateSpace.cpp:388
OptimizationObjectivePtr operator*(double weight, const OptimizationObjectivePtr &a)
Given a weighing factor and an optimization objective, returns a MultiOptimizationObjective containin...
std::map< std::string, ValueLocation > valueLocationsByName_
All the known value locations, by name. The names of state spaces access the first element of a state...
Definition: StateSpace.h:626