Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031 #ifndef DYNAMIC_PATH_H
00032 #define DYNAMIC_PATH_H
00033
00034 #include "constraint_aware_spline_smoother/ParabolicPathSmooth/ParabolicRamp.h"
00035
00036 namespace ParabolicRamp {
00037
00040 class FeasibilityCheckerBase
00041 {
00042 public:
00043 virtual ~FeasibilityCheckerBase() {}
00044 virtual bool ConfigFeasible(const Vector& x)=0;
00045 virtual bool SegmentFeasible(const Vector& a,const Vector& b)=0;
00046 };
00047
00054 class DistanceCheckerBase
00055 {
00056 public:
00057 virtual ~DistanceCheckerBase() {}
00058 virtual Real ObstacleDistanceNorm() const { return Inf; }
00059 virtual Real ObstacleDistance(const Vector& x)=0;
00060 };
00061
00063 bool CheckRamp(const ParabolicRampND& ramp,FeasibilityCheckerBase* feas,DistanceCheckerBase* distance,int maxiters);
00064
00067 bool CheckRamp(const ParabolicRampND& ramp,FeasibilityCheckerBase* space,Real tol);
00068
00069
00081 class RampFeasibilityChecker
00082 {
00083 public:
00084 RampFeasibilityChecker(FeasibilityCheckerBase* feas,Real tol);
00085 RampFeasibilityChecker(FeasibilityCheckerBase* feas,DistanceCheckerBase* distance,int maxiters);
00086 bool Check(const ParabolicRampND& x);
00087
00088 FeasibilityCheckerBase* feas;
00089 Real tol;
00090 DistanceCheckerBase* distance;
00091 int maxiters;
00092 };
00093
00097 class RandomNumberGeneratorBase
00098 {
00099 public:
00100 virtual Real Rand() { return ::ParabolicRamp::Rand(); }
00101 };
00102
00103
00113 class DynamicPath
00114 {
00115 public:
00116 DynamicPath();
00117 void Init(const Vector& velMax,const Vector& accMax);
00118 void SetJointLimits(const Vector& qMin,const Vector& qMax);
00119 inline void Clear() { ramps.clear(); }
00120 inline bool Empty() const { return ramps.empty(); }
00121 Real GetTotalTime() const;
00122 int GetSegment(Real t,Real& u) const;
00123 void Evaluate(Real t,Vector& x) const;
00124 void Derivative(Real t,Vector& dx) const;
00125 void SetMilestones(const std::vector<Vector>& x);
00126 void SetMilestones(const std::vector<Vector>& x,const std::vector<Vector>& dx);
00127 void GetMilestones(std::vector<Vector>& x,std::vector<Vector>& dx) const;
00128 void Append(const Vector& x);
00129 void Append(const Vector& x,const Vector& dx);
00130 void Concat(const DynamicPath& suffix);
00131 void Split(Real t,DynamicPath& before,DynamicPath& after) const;
00132 bool TryShortcut(Real t1,Real t2,RampFeasibilityChecker& check);
00133 int Shortcut(int numIters,RampFeasibilityChecker& check);
00134 int Shortcut(int numIters,RampFeasibilityChecker& check,RandomNumberGeneratorBase* rng);
00135 int ShortCircuit(RampFeasibilityChecker& check);
00138 int OnlineShortcut(Real leadTime,Real padTime,RampFeasibilityChecker& check);
00139 int OnlineShortcut(Real leadTime,Real padTime,RampFeasibilityChecker& check,RandomNumberGeneratorBase* rng);
00140
00141 bool IsValid() const;
00142
00144 Vector xMin,xMax,velMax,accMax;
00146 std::vector<ParabolicRampND> ramps;
00147 };
00148
00149 }
00150
00151 #endif