KrisLibrary  1.0.0
MultiModalCSpace.h
1 #ifndef ROBOTICS_MULTI_MODAL_CSPACE_H
2 #define ROBOTICS_MULTI_MODAL_CSPACE_H
3 
4 #include "CSpace.h"
5 #include <KrisLibrary/graph/UndirectedGraph.h>
6 #include <KrisLibrary/errors.h>
7 #include <vector>
8 
12 template<class Mode>
14 {
15  public:
16  virtual ~MultiModalCSpace() {}
17  //must be overloaded
18  virtual bool IsValid(const Mode& m) { return true; }
19  virtual CSpace* GetModeCSpace(const Mode& m) { return NULL; }
20  virtual CSpace* GetTransitionCSpace(const Mode& m1,const Mode& m2) { return NULL; }
21 
22  //getting modes
23  virtual bool CanEnum() const { return false; }
24  virtual bool CanSample() const { return false; }
25  virtual void Enum(std::vector<Mode>& modes) { FatalError("MultiModalCSpace: Cannot enumerate all modes"); }
26  virtual void Sample(std::vector<Mode>& modes) { FatalError("MultiModalCSpace: Cannot sample modes"); }
27 
28  //getting/testing adjacencies
29  virtual bool CanEnumAdjacent() const { return false; }
30  virtual bool CanSampleAdjacent() const { return false; }
31  virtual bool CanTestAdjacent() const { return false; }
32  virtual void EnumAdjacent(const Mode& m,std::vector<Mode>& modes) { FatalError("MultiModalCSpace: Cannot enumerate adjacent modes"); }
33  virtual void SampleAdjacent(const Mode& m,std::vector<Mode>& adj) { FatalError("MultiModalCSpace: Cannot sample adjacent modes"); }
34  virtual bool TestAdjacent(const Mode& m1,const Mode& m2) { FatalError("MultiModalCSpace: Cannot test mode adjacency"); return false; }
35 };
36 
38 {
39  public:
40  typedef int Mode;
42 
43  virtual ~ExplicitMMCSpace() {}
44  void DeleteAll();
45 
46  //must be overloaded
47  virtual bool IsValid(const Mode& m) { return m >= 0 && m < modeGraph.NumNodes(); }
48  virtual CSpace* GetModeCSpace(const Mode& m) { return modeGraph.nodes[m]; }
49  virtual CSpace* GetTransitionCSpace(const Mode& m1,const Mode& m2) { return *modeGraph.FindEdge(m1,m2); }
50 
51  //getting modes
52  virtual bool CanEnum() const { return true; }
53  virtual bool CanSample() const { return true; }
54  virtual void Enum(std::vector<Mode>& modes) { modes.resize(modeGraph.nodes.size()); for(size_t i=0;i<modeGraph.nodes.size();i++) modes[i]=i; }
55  virtual void Sample(std::vector<Mode>& modes) { Enum(modes); }
56 
57  //getting/testing adjacencies
58  virtual bool CanEnumAdjacent() const { return true; }
59  virtual bool CanSampleAdjacent() const { return true; }
60  virtual bool CanTestAdjacent() const { return true; }
61  virtual void EnumAdjacent(const Mode& m,std::vector<Mode>& modes) {
63  modes.resize(0);
64  for(modeGraph.Begin(m,e);!e.end();++e) modes.push_back(e.target());
65  }
66  virtual void SampleAdjacent(const Mode& m,std::vector<Mode>& adj) { EnumAdjacent(m,adj); }
67  virtual bool TestAdjacent(const Mode& m1,const Mode& m2) { return modeGraph.HasEdge(m1,m2); }
68 
69  //must fill out this graph beforehand
70  ModeGraph modeGraph;
71 };
72 
73 
74 #endif
Motion planning configuration space base class. The configuration space implements an interpolation s...
Definition: CSpace.h:39
Multi-modal configuration space base class.
Definition: MultiModalCSpace.h:13
Definition: Edge.h:88
Definition: MultiModalCSpace.h:37