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  virtual bool hasSymmetricDistance(void) const;
185 
187  virtual bool hasSymmetricInterpolate(void) const;
188 
190  const std::string& getName(void) const;
191 
193  void setName(const std::string &name);
194 
198  int getType(void) const
199  {
200  return type_;
201  }
202 
204  bool includes(const StateSpacePtr &other) const;
205 
207  bool includes(const StateSpace *other) const;
208 
211  bool covers(const StateSpacePtr &other) const;
212 
215  bool covers(const StateSpace *other) const;
216 
219  {
220  return params_;
221  }
222 
224  const ParamSet& params(void) const
225  {
226  return params_;
227  }
228 
234  virtual double getLongestValidSegmentFraction(void) const;
235 
246  virtual void setLongestValidSegmentFraction(double segmentFraction);
247 
249  virtual unsigned int validSegmentCount(const State *state1, const State *state2) const;
250 
257  void setValidSegmentCountFactor(unsigned int factor);
258 
260  unsigned int getValidSegmentCountFactor(void) const;
261 
264  void computeSignature(std::vector<int> &signature) const;
265 
272  virtual unsigned int getDimension(void) const = 0;
273 
280  virtual double getMaximumExtent(void) const = 0;
281 
284  virtual void enforceBounds(State *state) const = 0;
285 
288  virtual bool satisfiesBounds(const State *state) const = 0;
289 
292  virtual void copyState(State *destination, const State *source) const = 0;
293 
296  virtual double distance(const State *state1, const State *state2) const = 0;
297 
299  virtual unsigned int getSerializationLength(void) const;
300 
302  virtual void serialize(void *serialization, const State *state) const;
303 
305  virtual void deserialize(State *state, const void *serialization) const;
306 
308  virtual bool equalStates(const State *state1, const State *state2) const = 0;
309 
313  virtual void interpolate(const State *from, const State *to, const double t, State *state) const = 0;
314 
316  virtual StateSamplerPtr allocDefaultStateSampler(void) const = 0;
317 
321  virtual StateSamplerPtr allocStateSampler(void) const;
322 
325 
327  void clearStateSamplerAllocator(void);
328 
330  virtual State* allocState(void) const = 0;
331 
333  virtual void freeState(State *state) const = 0;
334 
352  virtual double* getValueAddressAtIndex(State *state, const unsigned int index) const;
353 
355  const double* getValueAddressAtIndex(const State *state, const unsigned int index) const;
356 
359  const std::vector<ValueLocation>& getValueLocations(void) const;
360 
363  const std::map<std::string, ValueLocation>& getValueLocationsByName(void) const;
364 
366  double* getValueAddressAtLocation(State *state, const ValueLocation &loc) const;
367 
369  const double* getValueAddressAtLocation(const State *state, const ValueLocation &loc) const;
370 
372  double* getValueAddressAtName(State *state, const std::string &name) const;
373 
375  const double* getValueAddressAtName(const State *state, const std::string &name) const;
376 
378  void copyToReals(std::vector<double> &reals, const State *source) const;
379 
381  void copyFromReals(State *destination, const std::vector<double> &reals) const;
382 
389  void registerProjection(const std::string &name, const ProjectionEvaluatorPtr &projection);
390 
392  void registerDefaultProjection(const ProjectionEvaluatorPtr &projection);
393 
396  virtual void registerProjections(void);
397 
399  ProjectionEvaluatorPtr getProjection(const std::string &name) const;
400 
403 
405  bool hasProjection(const std::string &name) const;
406 
408  bool hasDefaultProjection(void) const;
409 
411  const std::map<std::string, ProjectionEvaluatorPtr>& getRegisteredProjections(void) const;
412 
419  virtual void printState(const State *state, std::ostream &out) const;
420 
422  virtual void printSettings(std::ostream &out) const;
423 
425  virtual void printProjections(std::ostream &out) const;
426 
429  virtual void sanityChecks(double zero, double eps, unsigned int flags) const;
430 
433  virtual void sanityChecks(void) const;
434 
436  void diagram(std::ostream &out) const;
437 
439  void list(std::ostream &out) const;
440 
442  static void Diagram(std::ostream &out);
443 
445  static void List(std::ostream &out);
446 
454 
456  virtual StateSamplerPtr allocSubspaceStateSampler(const StateSpace *subspace) const;
457 
459  State* getSubstateAtLocation(State *state, const SubstateLocation &loc) const;
460 
462  const State* getSubstateAtLocation(const State *state, const SubstateLocation &loc) const;
463 
465  const std::map<std::string, SubstateLocation>& getSubstateLocationsByName(void) const;
466 
469  void getCommonSubspaces(const StateSpacePtr &other, std::vector<std::string> &subspaces) const;
470 
473  void getCommonSubspaces(const StateSpace *other, std::vector<std::string> &subspaces) const;
474 
477  virtual void computeLocations(void);
478 
489  virtual void setup(void);
490 
491  protected:
492 
494  static const std::string DEFAULT_PROJECTION_NAME;
495 
497  int type_;
498 
501 
503  double maxExtent_;
504 
507 
510 
513 
515  std::map<std::string, ProjectionEvaluatorPtr> projections_;
516 
519 
522  std::vector<ValueLocation> valueLocationsInOrder_;
523 
526  std::map<std::string, ValueLocation> valueLocationsByName_;
527 
529  std::map<std::string, SubstateLocation> substateLocationsByName_;
530 
531  private:
532 
534  std::string name_;
535  };
536 
539  {
540  public:
541 
544 
546  CompoundStateSpace(void);
547 
549  CompoundStateSpace(const std::vector<StateSpacePtr> &components, const std::vector<double> &weights);
550 
551  virtual ~CompoundStateSpace(void)
552  {
553  }
554 
556  template<class T>
557  T* as(const unsigned int index) const
558  {
560  BOOST_CONCEPT_ASSERT((boost::Convertible<T*, StateSpace*>));
561 
562  return static_cast<T*>(getSubspace(index).get());
563  }
564 
566  template<class T>
567  T* as(const std::string &name) const
568  {
570  BOOST_CONCEPT_ASSERT((boost::Convertible<T*, StateSpace*>));
571 
572  return static_cast<T*>(getSubspace(name).get());
573  }
574 
575  virtual bool isCompound(void) const;
576 
577  virtual bool isHybrid(void) const;
578 
584  void addSubspace(const StateSpacePtr &component, double weight);
585 
587  unsigned int getSubspaceCount(void) const;
588 
590  const StateSpacePtr& getSubspace(const unsigned int index) const;
591 
593  const StateSpacePtr& getSubspace(const std::string& name) const;
594 
596  unsigned int getSubspaceIndex(const std::string& name) const;
597 
599  bool hasSubspace(const std::string &name) const;
600 
602  double getSubspaceWeight(const unsigned int index) const;
603 
605  double getSubspaceWeight(const std::string &name) const;
606 
608  void setSubspaceWeight(const unsigned int index, double weight);
609 
611  void setSubspaceWeight(const std::string &name, double weight);
612 
614  const std::vector<StateSpacePtr>& getSubspaces(void) const;
615 
617  const std::vector<double>& getSubspaceWeights(void) const;
618 
622  bool isLocked(void) const;
623 
629  void lock(void);
635  virtual StateSamplerPtr allocSubspaceStateSampler(const StateSpace *subspace) const;
636 
642  virtual unsigned int getDimension(void) const;
643 
644  virtual double getMaximumExtent(void) const;
645 
646  virtual void enforceBounds(State *state) const;
647 
648  virtual bool satisfiesBounds(const State *state) const;
649 
650  virtual void copyState(State *destination, const State *source) const;
651 
652  virtual unsigned int getSerializationLength(void) const;
653 
654  virtual void serialize(void *serialization, const State *state) const;
655 
656  virtual void deserialize(State *state, const void *serialization) const;
657 
658  virtual double distance(const State *state1, const State *state2) const;
659 
665  virtual void setLongestValidSegmentFraction(double segmentFraction);
666 
669  virtual unsigned int validSegmentCount(const State *state1, const State *state2) const;
670 
671  virtual bool equalStates(const State *state1, const State *state2) const;
672 
673  virtual void interpolate(const State *from, const State *to, const double t, State *state) const;
674 
675  virtual StateSamplerPtr allocDefaultStateSampler(void) const;
676 
677  virtual State* allocState(void) const;
678 
679  virtual void freeState(State *state) const;
680 
681  virtual double* getValueAddressAtIndex(State *state, const unsigned int index) const;
682 
685  virtual void printState(const State *state, std::ostream &out) const;
686 
687  virtual void printSettings(std::ostream &out) const;
688 
689  virtual void computeLocations(void);
690 
691  virtual void setup(void);
692 
693  protected:
694 
696  void allocStateComponents(CompoundState *state) const;
697 
699  std::vector<StateSpacePtr> components_;
700 
702  unsigned int componentCount_;
703 
705  std::vector<double> weights_;
706 
708  double weightSum_;
709 
711  bool locked_;
712 
713  };
714 
728 
736 
739  StateSpacePtr operator-(const StateSpacePtr &a, const std::string &name);
740 
753  {
756 
759 
762  };
763 
771  AdvancedStateCopyOperation copyStateData(const StateSpacePtr &destS, State *dest,
772  const StateSpacePtr &sourceS, const State *source);
773 
781  AdvancedStateCopyOperation copyStateData(const StateSpace *destS, State *dest,
782  const StateSpace *sourceS, const State *source);
783 
789  AdvancedStateCopyOperation copyStateData(const StateSpacePtr &destS, State *dest,
790  const StateSpacePtr &sourceS, const State *source,
791  const std::vector<std::string> &subspaces);
792 
798  AdvancedStateCopyOperation copyStateData(const StateSpace *destS, State *dest,
799  const StateSpace *sourceS, const State *source,
800  const std::vector<std::string> &subspaces);
803  }
804 }
805 
806 #endif
virtual bool isCompound(void) const
Check if the state space is compound.
Definition: StateSpace.cpp:851
State * getSubstateAtLocation(State *state, const SubstateLocation &loc) const
Get the substate of state that is pointed to by loc.
Definition: StateSpace.cpp:271
void setName(const std::string &name)
Set the name of the state space.
Definition: StateSpace.cpp:192
bool isLocked(void) const
Return true if the state space is locked. A value of true means that no further spaces can be added a...
boost::function< StateSamplerPtr(const StateSpace *)> StateSamplerAllocator
Definition of a function that can allocate a state sampler.
Definition: StateSampler.h:184
virtual void deserialize(State *state, const void *serialization) const
Read the binary representation of a state from serialization and write it to state.
int type_
A type assigned for this state space.
Definition: StateSpace.h:497
ParamSet params_
The set of parameters for this space.
Definition: StateSpace.h:518
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:818
Definition of a compound state.
Definition: State.h:95
virtual void copyState(State *destination, const State *source) const
Copy a state to another. The memory of source and destination should NOT overlap. ...
Definition: StateSpace.cpp:989
virtual double * getValueAddressAtIndex(State *state, const 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:287
virtual unsigned int getSerializationLength(void) const
Get the number of chars in the serialization of a state in this space.
Definition: StateSpace.cpp:997
static const std::string DEFAULT_PROJECTION_NAME
The name used for the default projection.
Definition: StateSpace.h:494
std::size_t index
The index of the value to be accessed, within the substate location above.
Definition: StateSpace.h:126
unsigned int getSubspaceIndex(const std::string &name) const
Get the index of a specific subspace from the compound state space.
Definition: StateSpace.cpp:893
const std::vector< double > & getSubspaceWeights(void) const
Get the list of component weights.
Definition: StateSpace.cpp:951
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:455
static void Diagram(std::ostream &out)
Print a Graphviz digraph that represents the containment diagram for all the instantiated state space...
Definition: StateSpace.cpp:557
virtual void printProjections(std::ostream &out) const
Print the list of registered projections. This function is also called by printSettings() ...
Definition: StateSpace.cpp:374
A boost shared pointer wrapper for ompl::base::StateSpace.
const T * as(void) const
Cast this instance to a desired type.
Definition: StateSpace.h:97
double getSubspaceWeight(const unsigned int index) const
Get the weight of a subspace from the compound state space (used in distance computation) ...
Definition: StateSpace.cpp:906
A boost shared pointer wrapper for ompl::base::StateSampler.
const ParamSet & params(void) const
Get the parameters for this space.
Definition: StateSpace.h:224
Check whether the StateSpace::distance() is bounded by StateSpace::getExtent()
Definition: StateSpace.h:147
virtual unsigned int getSerializationLength(void) const
Get the number of chars in the serialization of a state in this space.
Definition: StateSpace.cpp:350
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:113
virtual unsigned int getDimension(void) const =0
Get the dimension of the space (not the dimension of the surrounding ambient space) ...
virtual bool isHybrid(void) const
Check if this is a hybrid state space (i.e., both discrete and continuous components exist) ...
Definition: StateSpace.cpp:856
Check whether sampled states are always within bounds.
Definition: StateSpace.h:150
Representation of the address of a value in a state. This structure stores the indexing information n...
Definition: StateSpace.h:120
No data was copied.
Definition: StateSpace.h:755
virtual bool isHybrid(void) const
Check if this is a hybrid state space (i.e., both discrete and continuous components exist) ...
Definition: StateSpace.cpp:749
virtual State * allocState(void) const
Allocate a state that can store a point in the described space.
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...
T * as(const unsigned int index) const
Cast a component of this instance to a desired type.
Definition: StateSpace.h:557
virtual double getLongestValidSegmentFraction(void) const
When performing discrete validation of motions, the length of the longest segment that does not requi...
Definition: StateSpace.cpp:813
const std::map< std::string, ValueLocation > & getValueLocationsByName(void) const
Get the named locations of values of type double contained in a state from this space. The setup() function must have been previously called.
Definition: StateSpace.cpp:303
CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:543
virtual StateSamplerPtr allocSubspaceStateSampler(const StateSpace *subspace) const
Allocate a sampler that actually samples only components that are part of subspace.
void diagram(std::ostream &out) const
Print a Graphviz digraph that represents the containment diagram for the state space.
Definition: StateSpace.cpp:530
void registerProjection(const std::string &name, const ProjectionEvaluatorPtr &projection)
Register a projection for this state space under a specified name.
Definition: StateSpace.cpp:731
void copyFromReals(State *destination, const std::vector< double > &reals) const
Copy the values from reals to the state destination using getValueAddressAtLocation() ...
Definition: StateSpace.cpp:315
void computeSignature(std::vector< int > &signature) const
Compute an array of ints that uniquely identifies the structure of the state space. The first element of the signature is the number of integers that follow.
Definition: StateSpace.cpp:209
virtual void printSettings(std::ostream &out) const
Print the settings for this state space to a stream.
Definition: StateSpace.cpp:368
virtual StateSamplerPtr allocDefaultStateSampler(void) const
Allocate an instance of the default uniform state sampler for this space.
virtual void setLongestValidSegmentFraction(double segmentFraction)
When performing discrete validation of motions, the length of the longest segment that does not requi...
void registerDefaultProjection(const ProjectionEvaluatorPtr &projection)
Register the default projection for this state space.
Definition: StateSpace.cpp:726
unsigned int longestValidSegmentCountFactor_
The factor to multiply the value returned by validSegmentCount()
Definition: StateSpace.h:512
void allocStateComponents(CompoundState *state) const
Allocate the state components. Called by allocState(). Usually called by derived state spaces...
StateSamplerAllocator ssa_
An optional state sampler allocator.
Definition: StateSpace.h:500
virtual double * getValueAddressAtIndex(State *state, const unsigned int index) const
Many states contain a number of double values. This function provides a means to get the memory addre...
unsigned int componentCount_
The number of components.
Definition: StateSpace.h:702
Maintain a set of parameters.
Definition: GenericParam.h:216
const StateSpacePtr & getSubspace(const unsigned int index) const
Get a specific subspace from the compound state space.
Definition: StateSpace.cpp:877
State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:78
All data was copied.
Definition: StateSpace.h:761
virtual void registerProjections(void)
Register the projections for this state space. Usually, this is at least the default projection...
Definition: StateSpace.cpp:216
virtual void setLongestValidSegmentFraction(double segmentFraction)
When performing discrete validation of motions, the length of the longest segment that does not requi...
Definition: StateSpace.cpp:801
const std::vector< ValueLocation > & getValueLocations(void) const
Get the locations of values of type double contained in a state from this space. The order of the val...
Definition: StateSpace.cpp:298
Check whether the distances between non-equal states is strictly positive (StateSpace::distance()) ...
Definition: StateSpace.h:135
virtual bool hasSymmetricInterpolate(void) const
Check if the interpolation function on this state space is symmetric, i.e. interpolate(from, to, t, state) = interpolate(to, from, 1-t, state). Default implementation returns true.
Definition: StateSpace.cpp:759
virtual void interpolate(const State *from, const State *to, const double t, State *state) const
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state...
StateSpace(void)
Constructor. Assigns a unique name to the space.
Definition: StateSpace.cpp:71
A space to allow the composition of state spaces.
Definition: StateSpace.h:538
Representation of the address of a substate in a state. This structure stores the indexing informatio...
Definition: StateSpace.h:106
std::vector< double > weights_
The weight assigned to each component of the state space when computing the compound distance...
Definition: StateSpace.h:705
const std::string & getName(void) const
Get the name of the state space.
Definition: StateSpace.cpp:187
virtual double getMaximumExtent(void) const =0
Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces...
unsigned int getSubspaceCount(void) const
Get the number of state spaces that make up the compound state space.
Definition: StateSpace.cpp:872
void setSubspaceWeight(const unsigned int index, double weight)
Set the weight of a subspace in the compound state space (used in distance computation) ...
Definition: StateSpace.cpp:922
virtual double getMaximumExtent(void) const
Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces...
Definition: StateSpace.cpp:964
virtual void printState(const State *state, std::ostream &out) const
Print a state to a stream.
A boost shared pointer wrapper for ompl::base::ProjectionEvaluator.
StateSpacePtr operator-(const StateSpacePtr &a, const StateSpacePtr &b)
Construct a compound state space that contains subspaces only from a. If a is compound, b (or the components from b, if b is compound) are removed and the remaining components are returned as a compound state space. If the compound space would end up containing solely one component, that component is returned instead.
Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.
Definition: StateSpace.h:73
ParamSet & params(void)
Get the parameters for this space.
Definition: StateSpace.h:218
virtual void printSettings(std::ostream &out) const
Print the settings for this state space to a stream.
ProjectionEvaluatorPtr getDefaultProjection(void) const
Get the default projection.
Definition: StateSpace.cpp:698
virtual void freeState(State *state) const
Free the memory of the allocated state.
Definition of an abstract state.
Definition: State.h:50
virtual bool equalStates(const State *state1, const State *state2) const
Checks whether two states are equal.
Check whether the triangle inequality holds when using StateSpace::interpolate() and StateSpace::dist...
Definition: StateSpace.h:144
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 setValidSegmentCountFactor(unsigned int factor)
Set factor to be the value to multiply the return value of validSegmentCount(). By default...
Definition: StateSpace.cpp:794
OptimizationObjectivePtr operator+(const OptimizationObjectivePtr &a, const OptimizationObjectivePtr &b)
Given two optimization objectives, returns a MultiOptimizationObjective that combines the two objecti...
virtual double distance(const State *state1, const State *state2) const
Computes distance between two states. This function satisfies the properties of a metric if isMetricS...
CompoundStateSpace(void)
Construct an empty compound state space.
Definition: StateSpace.cpp:823
StateSamplerPtr allocSubspaceStateSampler(const StateSpacePtr &subspace) const
Allocate a sampler that actually samples only components that are part of subspace.
Definition: StateSpace.cpp:782
void lock(void)
Lock this state space. This means no further spaces can be added as components. This function can be ...
void setStateSamplerAllocator(const StateSamplerAllocator &ssa)
Set the sampler allocator to use.
Definition: StateSpace.cpp:764
const std::map< std::string, ProjectionEvaluatorPtr > & getRegisteredProjections(void) const
Get all the registered projections.
Definition: StateSpace.cpp:721
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:450
Some data was copied.
Definition: StateSpace.h:758
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:839
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:522
double maxExtent_
The extent of this space at the time setup() was called.
Definition: StateSpace.h:503
SanityChecks
Flags to use in a bit mask for state space sanity checks. Some basic checks do not have flags associa...
Definition: StateSpace.h:131
std::map< std::string, ProjectionEvaluatorPtr > projections_
List of available projections.
Definition: StateSpace.h:515
virtual void setup(void)
Perform final setup steps. This function is automatically called by the SpaceInformation. If any default projections are to be registered, this call will set them and call their setup() functions. It is safe to call this function multiple times. At a subsequent call, projections that have been previously user configured are not re-instantiated, but their setup() method is still called.
virtual State * allocState(void) const =0
Allocate a state that can store a point in the described space.
virtual StateSamplerPtr allocStateSampler(void) const
Allocate an instance of the state sampler for this space. This sampler will be allocated with the sam...
Definition: StateSpace.cpp:774
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-...
virtual bool isDiscrete(void) const
Check if the set of states is discrete.
Definition: StateSpace.cpp:744
bool hasSubspace(const std::string &name) const
Check if a specific subspace is contained in this state space.
Definition: StateSpace.cpp:885
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:470
double weightSum_
The sum of all the weights in weights_.
Definition: StateSpace.h:708
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:359
virtual bool hasSymmetricDistance(void) const
Check if the distance function on this state space is symmetric, i.e. distance(s1,s2) = distance(s2,s1). Default implementation returns true.
Definition: StateSpace.cpp:754
SubstateLocation stateLocation
Location of the substate that contains the pointed to value.
Definition: StateSpace.h:123
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...
virtual void sanityChecks(void) const
Convenience function that allows derived state spaces to choose which checks should pass (see SanityC...
Definition: StateSpace.cpp:580
Check whether the distance function is symmetric (StateSpace::distance())
Definition: StateSpace.h:138
double longestValidSegmentFraction_
The fraction of the longest valid segment.
Definition: StateSpace.h:506
virtual bool equalStates(const State *state1, const State *state2) const =0
Checks whether two states are equal.
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...
bool locked_
Flag indicating whether adding further components is allowed or not.
Definition: StateSpace.h:711
unsigned int getValidSegmentCountFactor(void) const
Get the value used to multiply the return value of validSegmentCount().
Definition: StateSpace.cpp:808
bool hasProjection(const std::string &name) const
Check if a projection with a specified name is available.
Definition: StateSpace.cpp:693
static void List(std::ostream &out)
Print the list of available state space instances.
Definition: StateSpace.cpp:504
virtual void serialize(void *serialization, const State *state) const
Write the binary representation of state to serialization.
Definition: StateSpace.cpp:355
std::map< std::string, SubstateLocation > substateLocationsByName_
All the known substat locations, by name.
Definition: StateSpace.h:529
virtual bool isCompound(void) const
Check if the state space is compound.
Definition: StateSpace.cpp:739
Check whether the StateSpace::serialize() and StateSpace::deserialize() work as expected.
Definition: StateSpace.h:156
Check whether calling StateSpace::interpolate() works as expected.
Definition: StateSpace.h:141
Check that enforceBounds() does not modify the contents of states that are within bounds...
Definition: StateSpace.h:153
double longestValidSegment_
The longest valid segment at the time setup() was called.
Definition: StateSpace.h:509
virtual void computeLocations(void)
Compute the location information for various components of the state space. Either this function or s...
Definition: StateSpace.cpp:204
bool hasDefaultProjection(void) const
Check if a default projection is available.
Definition: StateSpace.cpp:688
virtual void interpolate(const State *from, const State *to, const 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...
virtual void freeState(State *state) const =0
Free the memory of the allocated state.
virtual void enforceBounds(State *state) const
Bring the state within the bounds of the state space. For unbounded spaces this function can be a no-...
Definition: StateSpace.cpp:973
double * getValueAddressAtLocation(State *state, const ValueLocation &loc) const
Get a pointer to the double value in state that loc points to.
Definition: StateSpace.cpp:322
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:338
virtual void copyState(State *destination, const State *source) const =0
Copy a state to another. The memory of source and destination should NOT overlap. ...
virtual bool isMetricSpace(void) const
Return true if the distance function associated with the space is a metric.
Definition: StateSpace.h:178
virtual unsigned int getDimension(void) const
Get the dimension of the space (not the dimension of the surrounding ambient space) ...
Definition: StateSpace.cpp:956
void clearStateSamplerAllocator(void)
Clear the state sampler allocator (reset to default)
Definition: StateSpace.cpp:769
const StateSpace * space
The space that is reached if the chain above is followed on the state space.
Definition: StateSpace.h:116
AdvancedStateCopyOperation
The possible outputs for an advanced copy operation.
Definition: StateSpace.h:752
virtual void serialize(void *serialization, const State *state) const
Write the binary representation of state to serialization.
virtual void setup(void)
Perform final setup steps. This function is automatically called by the SpaceInformation. If any default projections are to be registered, this call will set them and call their setup() functions. It is safe to call this function multiple times. At a subsequent call, projections that have been previously user configured are not re-instantiated, but their setup() method is still called.
Definition: StateSpace.cpp:220
const std::vector< StateSpacePtr > & getSubspaces(void) const
Get the list of components.
Definition: StateSpace.cpp:946
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:308
ProjectionEvaluatorPtr getProjection(const std::string &name) const
Get the projection registered under a specific name.
Definition: StateSpace.cpp:709
virtual void printState(const State *state, std::ostream &out) const
Print a state to a stream.
Definition: StateSpace.cpp:363
T * as(void)
Cast this instance to a desired type.
Definition: StateSpace.h:87
virtual StateSamplerPtr allocDefaultStateSampler(void) const =0
Allocate an instance of the default uniform state sampler for this space.
virtual void computeLocations(void)
Compute the location information for various components of the state space. Either this function or s...
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:526
std::vector< StateSpacePtr > components_
The state spaces that make up the compound state space.
Definition: StateSpace.h:699
int getType(void) 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:198
T * as(const std::string &name) const
Cast a component of this instance to a desired type.
Definition: StateSpace.h:567
OptimizationObjectivePtr operator*(double w, const OptimizationObjectivePtr &a)
Given a weighing factor and an optimization objective, returns a MultiOptimizationObjective containin...
const std::map< std::string, SubstateLocation > & getSubstateLocationsByName(void) const
Get the list of known substate locations (keys of the map corrspond to names of subspaces) ...
Definition: StateSpace.cpp:266
virtual bool satisfiesBounds(const State *state) const
Check if a state is inside the bounding box. For unbounded spaces this function can always return tru...
Definition: StateSpace.cpp:980
void list(std::ostream &out) const
Print the list of all contained state space instances.
Definition: StateSpace.cpp:512