1 #ifndef CSPACE_HELPERS_H 2 #define CSPACE_HELPERS_H 5 #include "GeodesicSpace.h" 25 virtual int NumDimensions() {
return geodesic->NumDimensions(); }
26 virtual int NumIntrinsicDimensions() {
return geodesic->NumIntrinsicDimensions(); }
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); }
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); }
37 std::shared_ptr<GeodesicSpace> geodesic;
48 virtual int NumDimensions() {
return d; }
49 virtual void SampleNeighborhood(
const Config& c,Real r,
Config& x);
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); }
54 virtual void Sample(
Config& q) { FatalError(
"Not implemented"); }
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);
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);
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);
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);
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&);
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);
133 std::vector<std::shared_ptr<CSpace> > components;
134 std::vector<std::string> componentNames;
135 std::vector<Real> distanceWeights;
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);
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); }
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);
190 std::vector<int> activeConstraints;
210 virtual bool IsFeasible(
const Config& x);
211 virtual bool IsFeasible(
const Config& x,
int obstacle);
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;
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;
246 EdgePlannerPtr MakeSingleConstraintEpsilonChecker(
CSpace* space,
const Config& a,
const Config& b,
int obstacle,Real epsilon);
250 EdgePlannerPtr MakeSingleConstraintBisectionPlanner(
CSpace* space,
const Config& a,
const Config& b,
int obstacle,Real epsilon);
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'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