KrisLibrary  1.0.0
CSpaceHelpers.h
1 #ifndef CSPACE_HELPERS_H
2 #define CSPACE_HELPERS_H
3 
4 #include "CSpace.h"
5 #include "GeodesicSpace.h"
6 
11 class GeodesicCSpace : public CSpace, public GeodesicSpace
12 {
13 public:
14  GeodesicCSpace() {}
15  virtual ~GeodesicCSpace() {}
16 };
17 
22 {
23 public:
24  GeodesicCSpaceAdaptor(const std::shared_ptr<GeodesicSpace>& geodesic);
25  virtual int NumDimensions() { return geodesic->NumDimensions(); }
26  virtual int NumIntrinsicDimensions() { return geodesic->NumIntrinsicDimensions(); }
27  virtual Real Distance(const Config& x, const Config& y) { return geodesic->Distance(x,y); }
28  virtual void Interpolate(const Config& x,const Config& y,Real u,Config& out) { geodesic->Interpolate(x,y,u,out); }
29  virtual void Midpoint(const Config& x,const Config& y,Config& out) { geodesic->Interpolate(x,y,0.5,out); }
30 
31  virtual void InterpolateDeriv(const Config& a,const Config& b,Real u,Vector& dx) { geodesic->InterpolateDeriv(a,b,u,dx); }
32  virtual void InterpolateDerivA(const Config& a,const Config& b,Real u,const Vector& da,Vector& dx) { geodesic->InterpolateDerivA(a,b,u,da,dx); }
33  virtual void InterpolateDerivB(const Config& a,const Config& b,Real u,const Vector& db,Vector& dx) { geodesic->InterpolateDerivB(a,b,u,db,dx); }
34  virtual void InterpolateDeriv2(const Config& a,const Config& b,Real u,Vector& ddx) { geodesic->InterpolateDeriv2(a,b,u,ddx); }
35  virtual void Integrate(const Config& a,const Vector& da,Config& b) { geodesic->Integrate(a,da,b); }
36 
37  std::shared_ptr<GeodesicSpace> geodesic;
38 };
39 
45 {
46 public:
47  CartesianCSpace(int d);
48  virtual int NumDimensions() { return d; }
49  virtual void SampleNeighborhood(const Config& c,Real r,Config& x);
50  virtual Real Distance(const Config& x, const Config& y) { return CSpace::Distance(x,y); }
51  virtual void Interpolate(const Config& x,const Config& y,Real u,Config& out) { CSpace::Interpolate(x,y,u,out); }
52  virtual void Midpoint(const Config& x,const Config& y,Config& out) { out.add(x,y); out.inplaceMul(0.5); }
53 
54  virtual void Sample(Config& q) { FatalError("Not implemented"); }
55 
56  int d;
57 };
58 
63 class BoxCSpace : public CartesianCSpace
64 {
65 public:
66  BoxCSpace(Real xmin,Real xmax,int d=1);
67  BoxCSpace(const Vector& bmin,const Vector& bmax);
68  void SetDomain(const Vector& bmin,const Vector& bmax);
69  void GetDomain(Vector& bmin,Vector& bmax);
70  virtual EdgePlannerPtr PathChecker(const Config& a,const Config& b);
71  virtual void Sample(Config& x);
72  virtual void SampleNeighborhood(const Config& c,Real r,Config& x);
73  virtual void Properties(PropertyMap&);
74 
76  Vector bmin,bmax;
77 };
78 
79 
84 {
85 public:
86  MultiCSpace();
87  MultiCSpace(const std::shared_ptr<CSpace>& space1,const std::shared_ptr<CSpace>& space2);
88  MultiCSpace(const std::vector<std::shared_ptr<CSpace> >& components);
89  void Add(const std::string& name,const std::shared_ptr<CSpace>& space,Real distanceWeight=1);
90 
93  void FlattenConstraints();
95  void AddConstraint(int spaceIndex,const std::string& name,CSet* constraint);
97  void AddConstraint(int spaceIndex,const std::string& name,const std::shared_ptr<CSet>& constraint);
99  void AddConstraint(int spaceIndex,const std::string& name,CSet::CPredicate test);
100 
101  void Split(const Vector& x,std::vector<Vector>& items);
102  void SplitRef(const Vector& x,std::vector<Vector>& items);
103  void Join(const std::vector<Vector>& items,Vector& x);
104 
105  //CSpace overloads
106  virtual int NumDimensions();
107  virtual int NumIntrinsicDimensions();
108  virtual std::string VariableName(int i);
109  virtual int NumConstraints();
110  virtual std::string ConstraintName(int i);
111  virtual std::shared_ptr<CSet> Constraint(int i);
112  virtual void Sample(Config& x);
113  virtual void SampleNeighborhood(const Config& c,Real r,Config& x);
114  virtual bool IsFeasible(const Config&);
115  virtual bool ProjectFeasible(Config& x);
117  virtual EdgePlannerPtr LocalPlanner(const Config& a,const Config& b);
118  virtual EdgePlannerPtr PathChecker(const Config& a,const Config& b);
119  virtual EdgePlannerPtr PathChecker(const Config& a,const Config& b,int constraint);
120  virtual Real Distance(const Config& x, const Config& y);
121  virtual void Interpolate(const Config& x,const Config& y,Real u,Config& out);
122  virtual void Midpoint(const Config& x,const Config& y,Config& out);
123  virtual Real ObstacleDistance(const Config& a);
124  virtual void Properties(PropertyMap&);
125 
126  //GeodesicSpace overloads
127  virtual void InterpolateDeriv(const Config& a,const Config& b,Real u,Vector& dx);
128  virtual void InterpolateDerivA(const Config& a,const Config& b,Real u,const Vector& da,Vector& dx);
129  virtual void InterpolateDerivB(const Config& a,const Config& b,Real u,const Vector& db,Vector& dx);
130  virtual void InterpolateDeriv2(const Config& a,const Config& b,Real u,Vector& ddx);
131  virtual void Integrate(const Config& a,const Vector& da,Config& b);
132 
133  std::vector<std::shared_ptr<CSpace> > components;
134  std::vector<std::string> componentNames;
135  std::vector<Real> distanceWeights;
136 };
137 
148 class PiggybackCSpace : public CSpace
149 {
150 public:
151  PiggybackCSpace(CSpace* _baseSpace=NULL);
152  virtual int NumDimensions();
153  virtual std::string VariableName(int i);
154  virtual int NumConstraints();
155  virtual std::string ConstraintName(int i);
156  virtual std::shared_ptr<CSet> Constraint(int i);
157  virtual void Sample(Config& x);
158  virtual void SampleNeighborhood(const Config& c,Real r,Config& x);
159  virtual EdgePlannerPtr LocalPlanner(const Config& a,const Config& b);
160  virtual EdgePlannerPtr PathChecker(const Config& a,const Config& b);
161  virtual EdgePlannerPtr PathChecker(const Config& a,const Config& b,int obstacle);
162  virtual bool IsFeasible(const Config& x);
163  virtual bool IsFeasible(const Config&,int constraint);
164  virtual bool ProjectFeasible(Config& x);
166  virtual Real Distance(const Config& x, const Config& y);
167  virtual void Interpolate(const Config& x,const Config& y,Real u,Config& out);
168  virtual void Midpoint(const Config& x,const Config& y,Config& out);
169  virtual void Properties(PropertyMap& map);
170 
171  CSpace* baseSpace;
172 };
173 
178 {
179 public:
180  SubsetConstraintCSpace(CSpace* baseSpace,const std::vector<int>& constraints);
181  SubsetConstraintCSpace(CSpace* baseSpace,int constraints);
182  virtual bool IsFeasible(const Config& x) { return CSpace::IsFeasible(x); }
183  virtual bool IsFeasible(const Config& x,int obstacle) { return CSpace::IsFeasible(x,obstacle); }
184  virtual bool ProjectFeasible(Config& x) { return CSpace::ProjectFeasible(x); }
186  virtual EdgePlannerPtr LocalPlanner(const Config& a,const Config& b) { return CSpace::LocalPlanner(a,b); }
187  virtual EdgePlannerPtr PathChecker(const Config& a,const Config& b);
188  virtual EdgePlannerPtr PathChecker(const Config& a,const Config& b,int obstacle);
189 
190  std::vector<int> activeConstraints;
191 };
192 
193 
207 {
208 public:
209  AdaptiveCSpace(CSpace* baseSpace);
210  virtual bool IsFeasible(const Config& x);
211  virtual bool IsFeasible(const Config& x,int obstacle);
212  virtual void CheckConstraints(const Config& x,std::vector<bool>& satisfied);
213  virtual EdgePlannerPtr PathChecker(const Config& a,const Config& b);
214  virtual EdgePlannerPtr PathChecker(const Config& a,const Config& b,int obstacle);
215  bool IsFeasible_NoDeps(const Config& x,int obstacle);
216  EdgePlannerPtr PathChecker_NoDeps(const Config& a,const Config& b,int obstacle);
217  void SetupAdaptiveInfo();
218  bool AddFeasibleDependency(int constraint,int dependency);
219  bool AddVisibleDependency(int constraint,int dependency);
220  bool AddFeasibleDependency(const char* name,const char* dependency);
221  bool AddVisibleDependency(const char* name,const char* dependency);
222  void OptimizeQueryOrder();
223  void GetFeasibleDependencies(int obstacle,std::vector<int>& deps,bool recursive=true) const;
224  void GetVisibleDependencies(int obstacle,std::vector<int>& deps,bool recursive=true) const;
225  void GetStats(PropertyMap& stats) const;
226  void LoadStats(const PropertyMap& stats);
227 
229  {
230  double cost;
231  double probability;
232  double count;
233  };
234  bool adaptive;
235  std::map<std::string,int> constraintMap;
236  std::vector<PredicateStats> feasibleStats,visibleStats;
237  std::vector<std::vector<int> > feasibleTestDeps,visibleTestDeps;
238  std::vector<int> feasibleTestOrder,visibleTestOrder;
239  bool useBaseVisibleTest;
240  PredicateStats baseVisibleStats;
241 };
242 
243 
246 EdgePlannerPtr MakeSingleConstraintEpsilonChecker(CSpace* space,const Config& a,const Config& b,int obstacle,Real epsilon);
247 
250 EdgePlannerPtr MakeSingleConstraintBisectionPlanner(CSpace* space,const Config& a,const Config& b,int obstacle,Real epsilon);
251 
252 
253 #endif
Definition: CSpaceHelpers.h:83
A space with geodesics and (optionally) geodesic derivatives.
Definition: GeodesicSpace.h:31
virtual void Properties(PropertyMap &)
Returns properties of the space that might be useful for planners.
Definition: CSpace.cpp:126
virtual Real Distance(const Config &x, const Config &y)
optionally overrideable (default uses euclidean space)
Definition: CSpaceHelpers.h:27
virtual Optimization::NonlinearProgram * FeasibleNumeric()
If possible, give the feasible set as a nonlinear program.
Definition: CSpace.cpp:189
Vector Config
an alias for Vector
Definition: RobotKinematics3D.h:14
Motion planning configuration space base class. The configuration space implements an interpolation s...
Definition: CSpace.h:34
virtual Optimization::NonlinearProgram * FeasibleNumeric()
If possible, give the feasible set as a nonlinear program.
Definition: CSpaceHelpers.h:185
Definition: CSpaceHelpers.h:11
virtual void CheckConstraints(const Config &, std::vector< bool > &satisfied)
Returns a vector indicating which constraints are satisfied.
Definition: CSpace.cpp:210
virtual Real Distance(const Config &x, const Config &y)
optionally overrideable (default uses euclidean space)
Definition: CSpaceHelpers.h:50
virtual Real Distance(const Config &x, const Config &y)
optionally overrideable (default uses euclidean space)
Definition: CSpace.h:56
virtual bool ProjectFeasible(Config &x)
Definition: CSpaceHelpers.h:184
virtual Real ObstacleDistance(const Config &a)
for local planners using obstacle distance
Definition: CSpace.cpp:200
A class that optimizes constraint testing order using empirical data.
Definition: CSpaceHelpers.h:206
Converts an CSpace so that it only checks one or a subset of selected constraints.
Definition: CSpaceHelpers.h:177
Definition: CSpaceHelpers.h:228
Vector bmin
The domain. NOTE: modifing these does not directly affect the constraints! Use SetDomain instead...
Definition: CSpaceHelpers.h:76
A helper class that assists with selective overriding of another cspace&#39;s methods (similar to "monkey...
Definition: CSpaceHelpers.h:148
A simple map from keys to values.
Definition: PropertyMap.h:27
A structure defining a nonlinear program.
Definition: NonlinearProgram.h:22
Definition: CSpaceHelpers.h:63
A subset of a CSpace, which establishes a constraint for a configuration to be feasible.
Definition: CSet.h:20
Definition: CSpaceHelpers.h:21
Definition: CSpaceHelpers.h:44
virtual bool ProjectFeasible(Config &x)
Definition: CSpace.cpp:140