Klamp't  0.9.0
TimeScaling.h
1 #ifndef TIME_SCALING_H
2 #define TIME_SCALING_H
3
4 #include <KrisLibrary/planning/GeneralizedBezierCurve.h>
5 #include <KrisLibrary/spline/TimeSegmentation.h>
6 #include <KrisLibrary/spline/PiecewisePolynomial.h>
7 #include <KrisLibrary/math/vector.h>
8 #include <vector>
9 #include <utility>
10
11 namespace Klampt {
12  using namespace Math;
13  using namespace std;
14
32 {
33 public:
35  int TimeToSegment(Real t) const;
36  Real TimeToParam(Real t) const;
37  Real TimeToParam(int segment,Real t) const;
38  Real TimeToParamDeriv(int segment,Real t) const;
39  Real TimeToParamAccel(int segment,Real t) const;
40  int ParamToSegment(Real s) const;
41  Real ParamToTime(Real s) const;
42  Real ParamToTime(int segment,Real s) const;
43
64  bool SolveMinTime(const Vector& vmin,const Vector& vmax,
65  const Vector& amin,const Vector& amax,
66  const vector<Real>& paramdivs,
67  const vector<Vector>& dxMins,const vector<Vector>& dxMaxs,
68  const vector<Vector>& ddxMins,const vector<Vector>& ddxMaxs,
69  Real ds0=-1,Real dsEnd=-1,
70  vector<pair<int,int> >* velocityLimitedVariables=NULL,
71  vector<pair<int,int> >* accelerationLimitedSegments=NULL);
72
75  bool SolveMinTime(const Vector& vmin,const Vector& vmax,
76  const Vector& amin,const Vector& amax,
77  const GeneralizedCubicBezierSpline& path,
78  Real ds0=-1,Real dsEnd=-1);
79
81  bool SolveMinTime(const Vector& vmin,const Vector& vmax,
82  const Vector& amin,const Vector& amax,
83  const vector<Real>& paramdivs,
84  const vector<Vector>& dxs,
85  Real ds0=-1,Real dsEnd=-1);
86
89  void ConditionMinTime(vector<Real>& paramdivs,vector<Vector>& dxs,
90  vector<Vector>& dxMins,vector<Vector>& dxMaxs,
91  vector<Vector>& ddxMins,vector<Vector>& ddxMaxs);
92
95  bool SolveMinTimeArcLength(const Vector& vmin,const Vector& vmax,
96  const Vector& amin,const Vector& amax,
97  const vector<Real>& paramdivs,
98  const vector<Vector>& dxMins,const vector<Vector>& dxMaxs,
99  const vector<Vector>& ddxMins,const vector<Vector>& ddxMaxs,
100  Real ds0=-1,Real dsEnd=-1);
101
103  bool SolveMinTimeArcLength(const Vector& vmin,const Vector& vmax,
104  const Vector& amin,const Vector& amax,
105  const vector<Real>& paramdivs,
106  const vector<Vector>& dxs,
107  Real ds0=-1,Real dsEnd=-1);
108
110  void GetTimeToParam(Spline::PiecewisePolynomial& poly) const;
111
112  Spline::TimeSegmentation params,times;
113  vector<Real> ds;
114 };
115
116
117
131 {
132 public:
133  bool OptimizeTimeScaling(const Vector& vmin,const Vector& vmax,const Vector& amin,const Vector& amax);
134  void GetPiecewiseLinear(std::vector<Real>& times,std::vector<Config>& milestones) const;
135  void GetDiscretizedPath(Real dt,std::vector<Config>& milestones) const;
136  Real EndTime() const;
137  void Eval(Real t,Vector& x) const;
138  void Deriv(Real t,Vector& dx) const;
139  void Accel(Real t,Vector& ddx) const;
140
142  void Plot(const char* fn,const Vector& vmin,const Vector& vmax,const Vector& amin,const Vector& amax,Real res=1e-3);
143
144  GeneralizedCubicBezierSpline path;
145  Spline::TimeSegmentation pathSegments; //optionally set to the partial sums of path.durations
146  TimeScaling timeScaling;
147 };
148
156 bool OptimizeTimeScaling(const GeneralizedCubicBezierSpline& path,
157  const Vector& vmin,const Vector& vmax,
158  const Vector& amin,const Vector& amax,
159  TimeScaling& scaling);
160
161 } //namespace Klampt
162
163 #endif
A convenience class that stores a Bezier curve and its time scaling. Useful for evaluating the scaled...
Definition: TimeScaling.h:130
Maps time into a given path parameter range (e.g., [0,1]) with joint space velocity and acceleration ...
Definition: TimeScaling.h:31
Definition: ContactDistance.h:6
bool OptimizeTimeScaling(const GeneralizedCubicBezierSpline &path, const Vector &vmin, const Vector &vmax, const Vector &amin, const Vector &amax, TimeScaling &scaling)
Optimizes the given path according to velocity and acceleration bounds. The resulting time scaling is...