KrisLibrary  1.0.0
CSpace.h
1 #ifndef ROBOTICS_CSPACE_H
2 #define ROBOTICS_CSPACE_H
3 
4 
5 #include <KrisLibrary/math/vector.h>
7 #include <KrisLibrary/utils/PropertyMap.h>
8 #include <memory>
9 #include "CSet.h"
10 
11 namespace Optimization {
12  class NonlinearProgram;
13 } //namespace Math
14 using namespace Math;
15 typedef Vector Config;
16 class Interpolator;
17 class EdgePlanner;
18 class CSpace;
19 typedef std::shared_ptr<EdgePlanner> EdgePlannerPtr;
20 
21 
39 class CSpace
40 {
41 public:
42  virtual ~CSpace() {}
43  void AddConstraint(const std::string& name,CSet* constraint);
44  void AddConstraint(const std::string& name,const std::shared_ptr<CSet>& constraint);
45  void AddConstraint(const std::string& name,CSet::CPredicate test);
46  void CopyConstraints(const CSpace* space,const std::string& prefix="");
47  virtual int NumDimensions();
48  virtual std::string VariableName(int i);
49  virtual int NumConstraints() { return (int)constraints.size(); }
50  virtual std::string ConstraintName(int i) { return constraintNames[i]; }
51  virtual std::shared_ptr<CSet> Constraint(int i) { return constraints[i]; }
52  virtual void Sample(Config& x)=0;
53  virtual void SampleNeighborhood(const Config& c,Real r,Config& x);
54  virtual bool IsFeasible(const Config&);
55  virtual bool IsFeasible(const Config&,int constraint);
56  virtual EdgePlannerPtr LocalPlanner(const Config& a,const Config& b);
57  virtual EdgePlannerPtr PathChecker(const Config& a,const Config& b);
58  virtual EdgePlannerPtr PathChecker(const Config& a,const Config& b,int constraint);
59 
61  virtual Real Distance(const Config& x, const Config& y) { return Distance_L2(x,y); }
62  virtual void Interpolate(const Config& x,const Config& y,Real u,Config& out);
63  virtual void Midpoint(const Config& x,const Config& y,Config& out);
64 
67  virtual bool ProjectFeasible(Config& x);
68 
70  virtual Optimization::NonlinearProgram* FeasibleNumeric();
71 
73  virtual Real ObstacleDistance(const Config& a);
74 
98  virtual void Properties(PropertyMap&);
99 
101  virtual void CheckConstraints(const Config&,std::vector<bool>& satisfied);
102 
104  void GetFeasibleNames(const Config& q,std::vector<std::string>& names);
106  void GetInfeasibleNames(const Config& q,std::vector<std::string>& names);
108  void PrintInfeasibleNames(const Config& q,std::ostream& out=std::cout,const char* prefix="",const char* suffix="\n");
109 
110  std::vector<std::string> constraintNames;
111  std::vector<std::shared_ptr<CSet> > constraints;
112 };
113 
114 #endif
Namespace for classes and functions in the Optimization package.
Definition: CSet.h:7
Real Sample(const Interval &s)
Uniformly samples the given intervals.
Definition: sample.cpp:116
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:39
Abstract base class for an edge planner / edge checker (i.e., local planner).
Definition: EdgePlanner.h:49
virtual Real Distance(const Config &x, const Config &y)
optionally overrideable (default uses euclidean space)
Definition: CSpace.h:61
A base class for all 1D interpolators.
Definition: Interpolator.h:10
Standard vector/matrix metrics.
Contains all definitions in the Math package.
Definition: WorkspaceBound.h:12
A simple map from keys to values.
Definition: PropertyMap.h:27
A structure defining a nonlinear program.
Definition: NonlinearProgram.h:22
A subset of a CSpace, which establishes a constraint for a configuration must meet. Mathematically, this is a set S which imposes the constraint [q in S].
Definition: CSet.h:20