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 PARABOLIC_RAMP_H
00032 #define PARABOLIC_RAMP_H
00033
00034 #include <math.h>
00035 #include "constraint_aware_spline_smoother/ParabolicPathSmooth/Math.h"
00036
00037 namespace ParabolicRamp {
00038
00050 class ParabolicRamp1D
00051 {
00052 public:
00054 void SetConstant(Real x,Real t=0);
00056 void SetLinear(Real x0,Real x1,Real t);
00058 bool SolveMinTime(Real amax,Real vmax);
00060 bool SolveMinTime2(Real amax,Real vmax,Real tLowerBound);
00062 bool SolveMinAccel(Real endTime,Real vmax);
00064 Real SolveMinAccel2(Real endTime,Real vmax);
00066 void SolveBraking(Real amax);
00068 Real Evaluate(Real t) const;
00070 Real Derivative(Real t) const;
00072 Real Accel(Real t) const;
00074 Real EndTime() const { return ttotal; }
00076 void Dilate(Real timeScale);
00078 void TrimFront(Real tcut);
00080 void TrimBack(Real tcut);
00082 void Bounds(Real& xmin,Real& xmax) const;
00084 void Bounds(Real ta,Real tb,Real& xmin,Real& xmax) const;
00086 void DerivBounds(Real& vmin,Real& vmax) const;
00088 void DerivBounds(Real ta,Real tb,Real& vmin,Real& vmax) const;
00090 bool IsValid() const;
00091
00093 Real x0,dx0;
00094 Real x1,dx1;
00095
00097 Real tswitch1,tswitch2;
00098 Real ttotal;
00099 Real a1,v,a2;
00100 };
00101
00106 class ParabolicRampND
00107 {
00108 public:
00109 void SetConstant(const Vector& x,Real t=0);
00110 void SetLinear(const Vector& x0,const Vector& x1,Real t);
00111 bool SolveMinTimeLinear(const Vector& amax,const Vector& vmax);
00112 bool SolveMinTime(const Vector& amax,const Vector& vmax);
00113 bool SolveMinAccel(const Vector& vmax,Real time);
00114 bool SolveMinAccelLinear(const Vector& vmax,Real time);
00115 void SolveBraking(const Vector& amax);
00116 void Evaluate(Real t,Vector& x) const;
00117 void Derivative(Real t,Vector& dx) const;
00118 void Accel(Real t,Vector& ddx) const;
00119 void Output(Real dt,std::vector<Vector>& path) const;
00120 void Dilate(Real timeScale);
00121 void TrimFront(Real tcut);
00122 void TrimBack(Real tcut);
00123 void Bounds(Vector& xmin,Vector& xmax) const;
00124 void Bounds(Real ta,Real tb,Vector& xmin,Vector& xmax) const;
00125 void DerivBounds(Vector& vmin,Vector& vmax) const;
00126 void DerivBounds(Real ta,Real tb,Vector& vmin,Vector& vmax) const;
00127 bool IsValid() const;
00128
00130 Vector x0,dx0;
00131 Vector x1,dx1;
00132
00134 Real endTime;
00135 std::vector<ParabolicRamp1D> ramps;
00136 };
00137
00140 bool SolveMinTimeBounded(Real x0,Real v0,Real x1,Real v1,
00141 Real amax,Real vmax,Real xmin,Real xmax,
00142 ParabolicRamp1D& ramp);
00143
00147 bool SolveMinAccelBounded(Real x0,Real v0,Real x1,Real v1,
00148 Real endTime,Real vmax,Real xmin,Real xmax,
00149 std::vector<ParabolicRamp1D>& ramps);
00150
00153 Real SolveMinTimeBounded(const Vector& x0,const Vector& v0,const Vector& x1,const Vector& v1,
00154 const Vector& amax,const Vector& vmax,const Vector& xmin,const Vector& xmax,
00155 std::vector<std::vector<ParabolicRamp1D> >& ramps);
00156
00159 bool SolveMinAccelBounded(const Vector& x0,const Vector& v0,const Vector& x1,const Vector& v1,
00160 Real endTime,const Vector& vmax,const Vector& xmin,const Vector& xmax,
00161 std::vector<std::vector<ParabolicRamp1D> >& ramps);
00162
00164 void CombineRamps(const std::vector<std::vector<ParabolicRamp1D> >& ramps,std::vector<ParabolicRampND>& ndramps);
00165
00166 }
00167
00168 #endif