ParabolicRamp.cpp
Go to the documentation of this file.
00001 /*****************************************************************************
00002  *
00003  * Copyright (c) 2010-2011, the Trustees of Indiana University
00004  * All rights reserved.
00005  *
00006  * Redistribution and use in source and binary forms, with or without
00007  * modification, are permitted provided that the following conditions are met:
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of Indiana University nor the
00014  *       names of its contributors may be used to endorse or promote products
00015  *       derived from this software without specific prior written permission.
00016 
00017  * THIS SOFTWARE IS PROVIDED BY THE TRUSTEES OF INDIANA UNIVERSITY ''AS IS''
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE TRUSTEES OF INDIANA UNIVERSITY BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
00027  * THE POSSIBILITY OF SUCH DAMAGE.
00028  * 
00029  ***************************************************************************/
00030 
00031 #include "constraint_aware_spline_smoother/ParabolicPathSmooth/ParabolicRamp.h"
00032 #include "constraint_aware_spline_smoother/ParabolicPathSmooth/Config.h"
00033 #include <iostream>
00034 using namespace std;
00035 
00036 namespace ParabolicRamp {
00037 
00038 //a flag used during testing of failed ramps
00039 static bool gSuppressSavingRamps = false;
00040 
00041 //a flag used to stop SolveMinAccel from complaining during internal testing
00042 static bool gMinAccelQuiet = false;
00043 
00044 //a flag used to stop SolveMinTime2 from complaining during internal testing
00045 static bool gMinTime2Quiet = false;
00046 
00047 //solves the quadratic formula and returns the number of roots found
00048 int quadratic(Real a, Real b, Real c, Real& x1, Real& x2)
00049 {
00050   if(a == 0)
00051   {
00052           if(b == 0)
00053           {
00054                 if(c == 0)
00055                         return -1;
00056                 return 0;
00057           }
00058           x1=-c/b;
00059           return 1;
00060   }
00061   if(c == 0) { //det = b^2
00062     x1 = 0;
00063     x2 = -b/a;
00064     return 2;
00065   }
00066 
00067   Real det = b*b-4.0*a*c;
00068   if(det < 0.0)
00069     return 0;
00070   if(det == 0.0) {
00071     x1 = -b/(2.0*a);
00072     return 1;
00073   }
00074   det = sqrt(det);
00075   if(Abs(-b - det) < Abs(a))
00076     x1 = 0.5 * (-b + det)/a;
00077   else
00078     x1 = 2.0 * c / (-b-det);
00079   if(Abs(-b + det) < Abs(a)) 
00080     x2 = 0.5 * (-b-det) / a;
00081   else 
00082     x2 = 2.0 * c / (-b+det);
00083   return 2;
00084 }
00085 
00086 
00087 
00088 bool SaveRamp(const char* fn,Real x0,Real dx0,Real x1,Real dx1,
00089               Real a,Real v,Real t)
00090 {
00091   if(gSuppressSavingRamps) return true;
00092   PARABOLIC_RAMP_PLOG("Saving ramp to %s\n",fn);
00093   FILE* f=fopen(fn,"ab");
00094   if(!f) {
00095     f = fopen(fn,"wb");
00096     if(!f) {
00097       PARABOLIC_RAMP_PLOG("Unable to open file %s for saving\n",fn);
00098       return false;
00099     }
00100   }
00101   double vals[7]={x0,dx0,x1,dx1,a,v,t};
00102   fwrite(vals,sizeof(double),7,f);
00103   fclose(f);
00104   return true;
00105 }
00106 
00107 
00108 bool LoadRamp(FILE* f,Real& x0,Real& dx0,Real& x1,Real& dx1,
00109               Real& a,Real& v,Real& t)
00110 {
00111   double vals[7];
00112   int size = fread(vals,sizeof(double),7,f);
00113   if(size != 7) return false;
00114   x0=vals[0];  dx0=vals[1];
00115   x1=vals[2];  dx1=vals[3];
00116   a=vals[4];  v=vals[5];
00117   t=vals[6]; 
00118   return true;
00119 }
00120 
00121 
00122 bool LoadRamp(const char* fn,Real& x0,Real& dx0,Real& x1,Real& dx1,
00123               Real& a,Real& v,Real& t)
00124 {
00125   FILE* f=fopen(fn,"rb");
00126   if(!f) return false;
00127   bool res=LoadRamp(f,x0,dx0,x1,dx1,a,v,t);
00128   fclose(f);
00129   return res;
00130 }
00131 
00132 void TestRamps(const char* fn)
00133 {
00134   FILE* f=fopen(fn,"rb");
00135   if(!f) return;
00136 
00137   gSuppressSavingRamps=true;
00138   ParabolicRamp1D ramp;
00139   Real a,v,t;
00140   int numRamps=0;
00141   while(LoadRamp(f,ramp.x0,ramp.dx0,ramp.x1,ramp.dx1,a,v,t)) {
00142     if(t < 0) {
00143       PARABOLIC_RAMP_ASSERT( a >= 0 && v >= 0);
00144       bool res=ramp.SolveMinTime(a,v);
00145       PARABOLIC_RAMP_PLOG("Result %d: t=%g\n",(int)res,ramp.ttotal);
00146     }
00147     else if(a < 0) {
00148       PARABOLIC_RAMP_ASSERT( t >= 0 && v >= 0);
00149       bool res=ramp.SolveMinAccel(t,v);
00150       PARABOLIC_RAMP_PLOG("Result %d: a=%g\n",(int)res,ramp.a1);
00151     }
00152     else {
00153       bool res=ramp.SolveMinTime2(a,v,t);
00154       PARABOLIC_RAMP_PLOG("Result %d: t=%g\n",(int)res,ramp.ttotal);
00155 
00156       if(!res) {
00157         bool res=ramp.SolveMinAccel(t,v);
00158         PARABOLIC_RAMP_PLOG("SolveMinAccel result %d: a=%g\n",(int)res,ramp.a1);
00159       }
00160     }
00161     PARABOLIC_RAMP_PLOG("\n");
00162     numRamps++;
00163   }
00164   fclose(f);
00165   PARABOLIC_RAMP_PLOG("%d ramps loaded from file %s\n",numRamps,fn);
00166   gSuppressSavingRamps=false;
00167 }
00168 
00169 class ParabolicRamp
00170 {
00171  public:
00172   Real Evaluate(Real t) const;
00173   Real Derivative(Real t) const;
00174   Real Accel(Real t) const;
00175   bool Solve(Real amax);
00176   bool SolveFixedTime(Real endTime);
00177   Real MaxVelocity() const;
00178 
00179   //input
00180   Real x0,dx0;
00181   Real x1,dx1;
00182 
00183   //calculated
00184   Real a;
00185   Real ttotal;
00186 };
00187 
00188 
00189 class PPRamp
00190 {
00191  public:
00192   Real Evaluate(Real t) const;
00193   Real Derivative(Real t) const;
00194   Real Accel(Real t) const;
00195   bool SolveMinTime(Real amax);
00196   bool SolveMinTime2(Real amax,Real timeLowerBound);
00197   bool SolveMinAccel(Real endTime);
00198   Real SolveMinAccel2(Real endTime);
00199   Real MaxVelocity() const;
00200 
00201   Real CalcTotalTime(Real a) const;
00202   Real CalcSwitchTime(Real a) const;
00203   Real CalcMinAccel(Real endTime,Real sign,Real& switchTime) const;
00204   int CalcSwitchTimes(Real a,Real& t1,Real& t2) const;
00205   int CalcTotalTimes(Real a,Real& t1,Real& t2) const;
00206 
00207   //input
00208   Real x0,dx0;
00209   Real x1,dx1;
00210 
00211   //calculated
00212   Real a;
00213   Real tswitch,ttotal;
00214 };
00215 
00216 class PLPRamp
00217 {
00218  public:
00219   Real Evaluate(Real t) const;
00220   Real Derivative(Real t) const;
00221   Real Accel(Real t) const;
00222   bool SolveMinTime(Real amax,Real vmax);
00223   bool SolveMinTime2(Real amax,Real vmax,Real timeLowerBound);
00224   bool SolveMinAccel(Real endTime,Real vmax);
00225   Real SolveMinAccel2(Real endTime,Real vmax);
00226 
00227   Real CalcTotalTime(Real a,Real v) const;
00228   Real CalcSwitchTime1(Real a,Real v) const;
00229   Real CalcSwitchTime2(Real a,Real v) const;
00230   Real CalcMinAccel(Real endTime,Real v) const;  //variable a
00231   Real CalcMinTime2(Real endTime,Real a,Real vmax) const;  //variable v
00232 
00233   //input
00234   Real x0,dx0;
00235   Real x1,dx1;
00236 
00237   //calculated
00238   Real a,v;
00239   Real tswitch1,tswitch2,ttotal;
00240 };
00241 
00242 
00243 
00244 Real ParabolicRamp::Evaluate(Real t) const
00245 {
00246   return x0 + t*dx0 + 0.5*a*Sqr(t);
00247 }
00248 
00249 Real ParabolicRamp::Derivative(Real t) const
00250 {
00251   return dx0 + a*t;
00252 }
00253 
00254 Real ParabolicRamp::Accel(Real t) const
00255 {
00256   return a;
00257 }
00258 
00259 bool ParabolicRamp::Solve(Real amax)
00260 {
00261   if(FuzzyEquals(x0,x1,EpsilonX)) {
00262     if(FuzzyEquals(dx0,dx1,EpsilonV)) {
00263       a=0;
00264       ttotal = 0;
00265       return true;
00266     }
00267     else if(FuzzyEquals(dx0,-dx1,EpsilonV)) { //pure parabola, any acceleration works
00268       a=amax*Sign(dx1);
00269       ttotal = (dx1-dx0)/a;
00270       return true;
00271     }
00272     //no parabola will work
00273     return false;
00274   }
00275   a = 0.5*(Sqr(dx0)-Sqr(dx1))/(x0-x1);
00276   //pick the denominator less likely to result in numerical errors
00277   if(Abs(a) < Abs(dx0+dx1)) { 
00278     if(Abs(dx0+dx1) < EpsilonV) {
00279       //danger of numerical errors
00280       //dx0 = - dx1
00281       //need x0 = x1
00282       return false;
00283     }
00284     else {
00285       ttotal = 2.0*(x1-x0)/(dx0+dx1);
00286     }
00287   }
00288   else {
00289     ttotal = (dx1-dx0)/a;
00290   }
00291   if(ttotal < 0 && ttotal > -EpsilonT) ttotal = 0;
00292   if(ttotal < 0) {
00293     ttotal = -1;
00294     a=0;
00295     return false;
00296   }
00297 
00298   //check for numerical errors
00299   if(Abs(a) > amax && Abs(a) <= amax+EpsilonA) {
00300     //double check if the capped version works
00301     a = Sign(a)*amax;
00302   }
00303   if(FuzzyEquals(Evaluate(ttotal),x1,EpsilonX) && FuzzyEquals(Derivative(ttotal),dx1,EpsilonV)) {
00304     return true;
00305   }
00306   return false;
00307 }
00308 
00309 bool ParabolicRamp::SolveFixedTime(Real endTime)
00310 {
00311   ttotal = endTime;
00312   if(FuzzyEquals(x0,x1,EpsilonX)) {
00313     if(FuzzyEquals(dx0,dx1,EpsilonV)) {
00314       a = 0;
00315       return FuzzyEquals(endTime,0.0,EpsilonT);
00316     }
00317     else if(FuzzyEquals(dx0,-dx1,EpsilonV)) { //pure parabola, any acceleration works
00318       a=(dx1-dx0)/endTime;
00319       return true;
00320     }
00321     //no parabola will work
00322     return false;
00323   }
00324   a = 0.5*(Sqr(dx0)-Sqr(dx1))/(x0-x1);
00325   //pick the denominator less likely to result in numerical errors
00326   if(Abs(a) < Abs(dx0+dx1)) { 
00327     if(Abs(dx0+dx1) < EpsilonV) {
00328       //danger of numerical errors
00329       //dx0 = - dx1
00330       //need x0 = x1
00331       return false;
00332     }
00333     else {
00334       ttotal = 2.0*(x1-x0)/(dx0+dx1);
00335     }
00336   }
00337   else {
00338     ttotal = (dx1-dx0)/a;
00339   }
00340   if(!FuzzyEquals(ttotal,endTime,EpsilonT)) return false;
00341   ttotal = endTime;
00342   if(FuzzyEquals(Evaluate(ttotal),x1,EpsilonX) && FuzzyEquals(Derivative(ttotal),dx1,EpsilonV)) {
00343     return true;
00344   }
00345   return false;
00346 }
00347 
00348 Real ParabolicRamp::MaxVelocity() const
00349 {
00350   if(fabs(dx0) > fabs(dx1)) return dx0;
00351   else return dx1;
00352 }
00353 
00354 Real PPRamp::Evaluate(Real t) const
00355 {
00356   if(t < tswitch) return x0 + 0.5*a*t*t + dx0*t;
00357   else {
00358     Real tmT = t - ttotal;
00359     return x1 - 0.5*a*tmT*tmT + dx1*tmT;
00360   }
00361 }
00362 
00363 Real PPRamp::Derivative(Real t) const
00364 {
00365   if(t < tswitch) return a*t + dx0;
00366   else {
00367     Real tmT = t - ttotal;
00368     return -a*tmT + dx1;
00369   }
00370 }
00371 
00372 Real PPRamp::Accel(Real t) const
00373 {
00374   if(t < tswitch) return a;
00375   else return -a;
00376 }
00377 
00378 bool PPRamp::SolveMinTime(Real amax)
00379 {
00380   Real tpn = CalcTotalTime(amax), tnp = CalcTotalTime(-amax);
00381   //cout<<"Time for parabola +-: "<<tpn<<", parabola -+: "<<tnp<<endl;
00382   if(tpn >= 0) {
00383     if(tnp >= 0 && tnp < tpn) {
00384       a = -amax;
00385       ttotal = tnp;
00386     }
00387     else {
00388       a = amax;
00389       ttotal = tpn;
00390     }
00391   }
00392   else if(tnp >= 0) {
00393     a = -amax;
00394     ttotal = tnp;
00395   }
00396   else {
00397     tswitch = -1;
00398     ttotal = -1;
00399     a = 0;
00400     return false;
00401   }
00402   tswitch = CalcSwitchTime(a);
00403 
00404   //uncomment for additional debugging
00405   if(gValidityCheckLevel >= 1) {
00406     if(!FuzzyEquals(x0 + 0.5*a*Sqr(tswitch) + dx0*tswitch,x1 - 0.5*a*Sqr(tswitch-ttotal) + dx1*(tswitch-ttotal),EpsilonX)) {
00407       if(gVerbose >= 1)
00408         PARABOLIC_RAMP_PERROR("Numerical error computing parabolic-parabolic min-time...\n");
00409       if(gVerbose >= 2) {
00410         PARABOLIC_RAMP_PERROR("x0=%g,%g, x1=%g,%g\n",x0,dx0,x1,dx1);
00411         PARABOLIC_RAMP_PERROR("a = %g, tswitch = %g, ttotal = %g\n",a,tswitch,ttotal);
00412         PARABOLIC_RAMP_PERROR("Forward %g, backward %g, diff %g\n",x0 + 0.5*a*Sqr(tswitch) + dx0*tswitch,x1 - 0.5*a*Sqr(tswitch-ttotal) + dx1*(tswitch-ttotal), x0 + 0.5*a*Sqr(tswitch) + dx0*tswitch - (x1 - 0.5*a*Sqr(tswitch-ttotal) + dx1*(tswitch-ttotal)));
00413         //Real b = 2.0*dx0; //2.0*(dx0-dx1);
00414         //Real c = (Sqr(dx0)-Sqr(dx1))*0.5/a+x0-x1;
00415         Real b = 2.0*a*dx0; //2.0*(dx0-dx1);
00416         Real c = (Sqr(dx0)-Sqr(dx1))*0.5+(x0-x1)*a;
00417         Real t1,t2;
00418         int res=quadratic(a*a,b,c,t1,t2);
00419         PARABOLIC_RAMP_PERROR("Quadratic equation %g x^2 + %g x + %g = 0\n",a*a,b,c);
00420         PARABOLIC_RAMP_PERROR("%d results, %g %g\n",res,t1,t2);
00421         if(gErrorGetchar) getchar();
00422       }
00423       if(gErrorSave) SaveRamp("PP_SolveMinTime_failure.dat",x0,dx0,x1,dx1,amax,Inf,-1);
00424       return false;
00425     }
00426   }
00427 
00428   return true;
00429 }
00430 
00431 bool PPRamp::SolveMinTime2(Real amax,Real timeLowerBound)
00432 {
00433   PARABOLIC_RAMP_ASSERT(timeLowerBound >= 0);
00434   Real t1pn,t1np,t2pn,t2np;
00435   int respn = CalcTotalTimes(amax,t1pn,t2pn);
00436   int resnp = CalcTotalTimes(-amax,t1np,t2np);
00437   ttotal = Inf;
00438   if(respn >= 1) {
00439     if(t1pn >= timeLowerBound && t1pn < ttotal) {
00440       ttotal = t1pn;
00441       a = amax;
00442     }
00443   }
00444   if(respn >= 2) {
00445     if(t2pn >= timeLowerBound && t2pn < ttotal) {
00446       ttotal = t2pn;
00447       a = amax;
00448     }
00449   }
00450   if(resnp >= 1) {
00451     if(t1np >= timeLowerBound && t1np < ttotal) {
00452       ttotal = t1np;
00453       a = -amax;
00454     }
00455   }
00456   if(resnp >= 2) {
00457     if(t2np >= timeLowerBound && t2np < ttotal) {
00458       ttotal = t2np;
00459       a = -amax;
00460     }
00461   }
00462   if(IsInf(ttotal)) {
00463     a = 0;
00464     tswitch = ttotal = -1;
00465     return false;
00466   }
00467 
00468   Real ts1,ts2;
00469   int res = CalcSwitchTimes(a,ts1,ts2);
00470   PARABOLIC_RAMP_ASSERT(res > 0);
00471   if(res == 1) {
00472     tswitch = ts1;
00473     PARABOLIC_RAMP_ASSERT(FuzzyEquals(ttotal,ts1*2.0 - (dx1-dx0)/a,EpsilonT));
00474   }
00475   else {
00476     if(FuzzyEquals(ttotal,ts1*2.0 - (dx1-dx0)/a,EpsilonT))
00477       tswitch = ts1;
00478     else {
00479       PARABOLIC_RAMP_ASSERT(FuzzyEquals(ttotal,ts2*2.0 - (dx1-dx0)/a,EpsilonT));
00480       tswitch = ts2;
00481     }
00482   }
00483 
00484   //uncomment for additional debugging
00485   if(gValidityCheckLevel >= 1) {
00486     Real eps = EpsilonX;
00487     if(!FuzzyEquals(x0 + 0.5*a*Sqr(tswitch) + dx0*tswitch,x1 - 0.5*a*Sqr(tswitch-ttotal) + dx1*(tswitch-ttotal),eps)) {
00488       if(gVerbose >= 1)
00489         PARABOLIC_RAMP_PERROR("Numerical error in PPRamp::SolveMinTime2...\n");
00490       if(gVerbose >= 2) {
00491         PARABOLIC_RAMP_PERROR("x0=%g,%g, x1=%g,%g\n",x0,dx0,x1,dx1);
00492         PARABOLIC_RAMP_PERROR("a = %g, tswitch = %g, ttotal = %g\n",a,tswitch,ttotal);
00493         PARABOLIC_RAMP_PERROR("Forward %g, backward %g, diff %g\n",x0 + 0.5*a*Sqr(tswitch) + dx0*tswitch,x1 - 0.5*a*Sqr(tswitch-ttotal) + dx1*(tswitch-ttotal), x0 + 0.5*a*Sqr(tswitch) + dx0*tswitch - (x1 - 0.5*a*Sqr(tswitch-ttotal) + dx1*(tswitch-ttotal)));
00494         //Real b = 2.0*dx0; //2.0*(dx0-dx1);
00495         //Real c = (Sqr(dx0)-Sqr(dx1))*0.5/a+x0-x1;
00496         Real b = 2.0*a*dx0; //2.0*(dx0-dx1);
00497         Real c = (Sqr(dx0)-Sqr(dx1))*0.5+(x0-x1)*a;
00498         Real t1,t2;
00499         int res=quadratic(a*a,b,c,t1,t2);
00500         PARABOLIC_RAMP_PERROR("Quadratic equation %g x^2 + %g x + %g = 0\n",a*a,b,c);
00501         PARABOLIC_RAMP_PERROR("%d results, %g %g\n",res,t1,t2);
00502       }
00503       if(gErrorGetchar) getchar();
00504       if(gErrorSave) SaveRamp("PP_SolveMinTime_failure.dat",x0,dx0,x1,dx1,amax,Inf,timeLowerBound);
00505       return false;
00506     }
00507   }  
00508   return true;
00509 }
00510 
00511 bool PPRamp::SolveMinAccel(Real endTime)
00512 {
00513   Real switch1,switch2;
00514   Real apn = CalcMinAccel(endTime,1.0,switch1);
00515   Real anp = CalcMinAccel(endTime,-1.0,switch2);
00516   //cout<<"Accel for parabola +-: "<<apn<<", parabola -+: "<<anp<<endl;
00517   if(apn >= 0) {
00518     if(anp >= 0 && anp < apn)  a = -anp;
00519     else a = apn;
00520   }
00521   else if(anp >= 0) a = -anp;
00522   else {
00523     a=0;
00524     tswitch = -1;
00525     ttotal = -1;
00526     return false;
00527   }
00528   ttotal = endTime;
00529   if(a == apn) 
00530     tswitch = switch1;
00531   else
00532     tswitch = switch2;
00533 
00534   //debug
00535   if(gValidityCheckLevel >= 1) {
00536     Real t2mT = tswitch-ttotal;
00537     if(!FuzzyEquals(x0 + tswitch*dx0 + 0.5*a*Sqr(tswitch),x1+t2mT*dx1-0.5*a*Sqr(t2mT),EpsilonX)) {
00538       if(gVerbose >= 1)
00539         PARABOLIC_RAMP_PERROR("PPRamp::SolveMinAccel: Numerical error, x mismatch!\n");
00540       if(gVerbose >= 2) {
00541         PARABOLIC_RAMP_PERROR("Forward ramp: %g, backward %g, diff %g\n",x0 + tswitch*dx0 + 0.5*a*Sqr(tswitch),x1+t2mT*dx1-0.5*a*Sqr(t2mT),x0 + tswitch*dx0 + 0.5*a*Sqr(tswitch)-(x1+t2mT*dx1-0.5*a*Sqr(t2mT)));
00542         PARABOLIC_RAMP_PERROR("A+ = %g, A- = %g\n",apn,anp);
00543         PARABOLIC_RAMP_PERROR("ramp %g,%g -> %g, %g\n",x0,dx0,x1,dx1);
00544         PARABOLIC_RAMP_PERROR("Switch 1 %g, switch 2 %g, total %g\n",switch1,switch2,ttotal);
00545         
00546         {
00547           Real sign = 1.0;
00548           Real a=Sqr(endTime);
00549           Real b=sign*(2.0*(dx0+dx1)*endTime+4.0*(x0-x1));
00550           Real c=-Sqr(dx1-dx0);
00551           PARABOLIC_RAMP_PERROR("Quadratic %g x^2 + %g x + %g = 0\n",a,b,c);
00552           Real t1,t2;
00553           int res = quadratic(a,b,c,t1,t2);
00554           PARABOLIC_RAMP_PERROR("Solutions: %d, %g and %g\n",res,t1,t2);
00555         }
00556         {
00557           Real sign = -1.0;
00558           Real a=Sqr(endTime);
00559           Real b=sign*(2.0*(dx0+dx1)*endTime+4.0*(x0-x1));
00560           Real c=-Sqr(dx1-dx0);
00561           PARABOLIC_RAMP_PERROR("Quadratic %g x^2 + %g x + %g = 0\n",a,b,c);
00562           Real t1,t2;
00563           int res = quadratic(a,b,c,t1,t2);
00564           PARABOLIC_RAMP_PERROR("Solutions: %d, %g and %g\n",res,t1,t2);
00565         }
00566       }
00567       if(gErrorSave) SaveRamp("PP_SolveMinAccel_failure.dat",x0,dx0,x1,dx1,-1,Inf,endTime);
00568       if(gErrorGetchar) getchar();
00569     }
00570     if(!FuzzyEquals(dx0 + a*tswitch,dx1-a*t2mT,EpsilonV)) {
00571       if(gVerbose >= 1) PARABOLIC_RAMP_PERROR("PPRamp::SolveMinAccel: Numerical error, v mismatch!\n");
00572       if(gVerbose >= 2) {
00573         PARABOLIC_RAMP_PERROR("Velocity error %g vs %g, err %g\n",dx0+a*tswitch,dx1-a*t2mT,dx0+a*tswitch-(dx1-a*t2mT));
00574         PARABOLIC_RAMP_PERROR("ramp %g,%g -> %g, %g\n",x0,dx0,x1,dx1);
00575         PARABOLIC_RAMP_PERROR("Accel %g\n",a);
00576         PARABOLIC_RAMP_PERROR("Switch %g, total %g\n",tswitch,ttotal);
00577       }
00578       if(gErrorSave) SaveRamp("PP_SolveMinAccel_failure.dat",x0,dx0,x1,dx1,-1,Inf,endTime);
00579       if(gErrorGetchar) getchar();
00580       return false;
00581     }
00582   }
00583   return true;
00584 }
00585 
00586 Real PPRamp::MaxVelocity() const
00587 {
00588   return tswitch*a+dx0;
00589 }
00590 
00591 Real PPRamp::CalcTotalTime(Real a) const
00592 {
00593   Real tswitch = CalcSwitchTime(a);
00594   //printf("a = %g, switch time %g\n",a,tswitch);
00595   if(tswitch < 0) return -1;
00596   if(tswitch < (dx1-dx0)/a) return -1;
00597   return tswitch*2.0 - (dx1-dx0)/a;
00598 }
00599 
00600 int PPRamp::CalcTotalTimes(Real a,Real& t1,Real& t2) const
00601 {
00602   Real ts1,ts2;
00603   int res=CalcSwitchTimes(a,ts1,ts2);
00604   if(res == 0) return res;
00605   else if(res == 1) {
00606     if(ts1 < (dx1-dx0)/a) return 0;
00607     t1 = ts1*2.0 - (dx1-dx0)/a;
00608     return 1;
00609   }
00610   else {
00611     //check limits
00612     if(ts1 < (dx1-dx0)/a) t1=-1;
00613     else t1 = ts1*2.0 - (dx1-dx0)/a;
00614     if(ts2 < (dx1-dx0)/a) t2=-1;
00615     else t2 = ts2*2.0 - (dx1-dx0)/a;    
00616     if(t1 < 0) {
00617       if(t2 < 0) return 0;
00618       t1 = t2;
00619       return 1;
00620     }
00621     else {
00622       if(t2 < 0) return 1;
00623       else return 2;
00624     }
00625   }
00626 }
00627 
00628 int PPRamp::CalcSwitchTimes(Real a,Real& t1,Real& t2) const
00629 {
00630   int res;
00631   if(Abs(a) > 1.0) {
00632     //this may be prone to numerical errors
00633     Real b = 2.0*dx0; //2.0*(dx0-dx1);
00634     Real c = (Sqr(dx0)-Sqr(dx1))*0.5/a+x0-x1;
00635     res=quadratic(a,b,c,t1,t2);
00636   }
00637   else {
00638     Real b = 2.0*a*dx0; //2.0*(dx0-dx1);
00639     Real c = (Sqr(dx0)-Sqr(dx1))*0.5+(x0-x1)*a;
00640     res=quadratic(a*a,b,c,t1,t2);
00641   }
00642   if(res == 0) {
00643     return res;
00644   }
00645   else if(res == 2) {
00646     if(t1 < 0 && t1 > -EpsilonT*0.1) t1=0;
00647     if(t2 < 0 && t2 > -EpsilonT*0.1) t2=0;
00648     if(t1 < 0 || t1*Abs(a) < (dx1-dx0)*Sign(a)) {
00649       if(t2 < 0 || t2*Abs(a) < (dx1-dx0)*Sign(a)) return 0;
00650       t1 = t2;
00651       return 1;
00652     }
00653     else if(t2 < 0 || t2*Abs(a) < (dx1-dx0)*Sign(a)) {
00654       return 1;
00655     }
00656     else {
00657       return 2; //both are ok
00658     }
00659   }
00660   else {
00661     if(t1 < 0 && t1 > -EpsilonT) t1=0;
00662     if(t1 < 0) return 0;
00663     return 1;
00664   }
00665 }
00666 
00667 Real PPRamp::CalcSwitchTime(Real a) const
00668 {
00669   Real t1,t2;
00670   int res = CalcSwitchTimes(a,t1,t2);
00671   if(res == 0) {
00672     return -1;
00673   }
00674   else if(res == 2) {
00675     //check total time
00676     if(t2*Abs(a) < (dx1-dx0)*Sign(a)) return t1;
00677     else if(t1*Abs(a) < (dx1-dx0)*Sign(a)) return t2;
00678     else return Min(t1,t2); //both are ok
00679   }
00680   else return t1;
00681 }
00682 
00683 //for a PP ramp:
00684 //xs = x0 + ts*dx0 + 0.5*z*ts^2
00685 //xs = x1 - (T-ts)*dx1 - 0.5*z*(T-ts)^2
00686 //xs' = dx0 + ts*z
00687 //xs' = dx1 + (T-ts)*z
00688 //z = sign*a
00689 //(2ts-T)*z = dx1 - dx0
00690 //ts = 1/2* (T+(dx1 - dx0)/z)
00691 //T-ts = 1/2* (T-(dx1 - dx0)/z)
00692 //2 T(dx0+dx1) + 4(x0 - x1) - (dx1 - dx0)^2/z + z*T^2 = 0
00693 //what if z is close to 0?
00694 //suppose dx1 ~= dx0, then the other solution is 
00695 //4 T dx0 + 4(x0 - x1) + z*T^2 = 0
00696 //=>z = - 4 dx0 / T + 4(x1 - x0)/T^2
00697 //
00698 //alt: let y = (dx1 - dx0)/z, z = (dx1 - dx0)/y  (y is a time shift)
00699 //ts = 1/2* (T+y)
00700 //T-ts = 1/2* (T-y)
00701 //x0 + 1/2(T+y)*dx0 + 0.5*z*1/4(T+y)^2 = x1 - 1/2(T-y)*dx1 - 0.5*z*1/4(T-y)^2
00702 //4(x0-x1) + 2(T+y)*dx0 + 0.5*z*(T+y)^2 = - 2(T-y)*dx1 - 0.5*z*(T-y)^2
00703 //[4(x0-x1)/T + 2(dx0+dx1)] y - y^2 (dx1 - dx0)/T + (dx1 - dx0) T = 0
00704 Real PPRamp::CalcMinAccel(Real endTime,Real sign,Real& switchTime) const
00705 {
00706   Real a = -(dx1 - dx0)/endTime;
00707   Real b = (2.0*(dx0+dx1)+4.0*(x0-x1)/endTime);
00708   Real c = (dx1 - dx0)*endTime;
00709   Real rat1,rat2;
00710   int res=quadratic(a,b,c,rat1,rat2);
00711   Real accel1 = (dx1-dx0)/rat1;
00712   Real accel2 = (dx1-dx0)/rat2;
00713   Real switchTime1 = endTime*0.5+0.5*rat1;
00714   Real switchTime2 = endTime*0.5+0.5*rat2;
00715   //fix up numerical errors
00716   if(switchTime1 >  endTime && switchTime1 < endTime+EpsilonT*1e-1)
00717     switchTime1 = endTime;
00718   if(switchTime2 >  endTime && switchTime2 < endTime+EpsilonT*1e-1)
00719     switchTime2 = endTime;
00720   if(switchTime1 < 0 && switchTime1 > EpsilonT*1e-1)
00721     switchTime1 = 0;
00722   if(switchTime2 < 0 && switchTime2 > EpsilonT*1e-1)
00723     switchTime2 = 0;
00724   if(res > 0 && FuzzyZero(rat1,EpsilonT)) {
00725     //consider it as a zero, ts = T/2    
00726     //z = - 4*(x0-x1)/T^2 - 2 (dx0+dx1)/T
00727     if(!FuzzyZero(endTime,EpsilonT)) { //no good answer if endtime is small
00728       accel1=-2.0*(dx0+dx1)/endTime + 4.0*(x1-x0)/Sqr(endTime);
00729     }
00730   }
00731   if(res > 1 && FuzzyZero(rat2,EpsilonT)) {
00732     if(!FuzzyZero(endTime,EpsilonT)) { //no good answer if endtime is small
00733       accel2=-2.0*(dx0+dx1)/endTime + 4.0*(x1-x0)/Sqr(endTime);
00734     }
00735   }
00736   bool firstInfeas = false;
00737   if(res > 0 && (FuzzyZero(accel1,EpsilonA) || FuzzyZero(endTime/rat1,EpsilonA))) { //infer that accel must be small
00738     if(!FuzzyZero(dx0-dx1,EpsilonT)) { //no good answer if dx0=dx1
00739       switchTime1 = endTime*0.5;
00740     }
00741     if(!FuzzyEquals(x0 + switchTime1*dx0 + 0.5*Sqr(switchTime1)*accel1,x1 - (endTime-switchTime1)*dx1 - 0.5*Sqr(endTime-switchTime1)*accel1,EpsilonX) ||
00742        !FuzzyEquals(dx0+switchTime1*accel1,dx1+(endTime-switchTime1)*accel1,EpsilonV)) {
00743       firstInfeas = true;
00744     }
00745   }
00746   if(res > 1 && (FuzzyZero(accel2,EpsilonA) || FuzzyZero(endTime/rat2,EpsilonA))) {
00747     if(!FuzzyZero(dx0-dx1,EpsilonT)) { //no good answer if dx0=dx1
00748       switchTime2 = endTime*0.5;
00749     }
00750     if(!FuzzyEquals(x0 + switchTime2*dx0 + 0.5*Sqr(switchTime2)*accel2,x1 - (endTime-switchTime2)*dx1 - 0.5*Sqr(endTime-switchTime2)*accel2,EpsilonX) ||
00751        !FuzzyEquals(dx0+switchTime2*accel2,dx1+(endTime-switchTime2)*accel2,EpsilonV)) {
00752       res--;
00753     }
00754   }
00755   if(firstInfeas) {
00756     accel1 = accel2;
00757     rat1 = rat2;
00758     switchTime1 = switchTime2;
00759     res--;
00760   }
00761   if(res==0) return -1;
00762   else if(res==1) {
00763     if(switchTime1 >= 0 && switchTime1 <= endTime) { switchTime=switchTime1; return sign*accel1; }
00764     return -1.0;
00765   }
00766   else if(res==2) {
00767     if(switchTime1 >= 0 && switchTime1 <= endTime) {
00768       if(switchTime2 >= 0 && switchTime2 <= endTime) {
00769         if(accel1 < accel2) { switchTime=switchTime1; return sign*accel1; }
00770         else { switchTime=switchTime2; return sign*accel2; }
00771       }
00772       else { switchTime=switchTime1; return sign*accel1; }
00773     }
00774     else if(switchTime2 >= 0 && switchTime2 <= endTime) { switchTime=switchTime2; return sign*accel2; }
00775     return -1.0;
00776   }
00777   if(FuzzyZero(a,EpsilonT) && FuzzyZero(b,EpsilonT) && FuzzyZero(c,EpsilonT)) {
00778     switchTime = 0.5*endTime;
00779     return 0;
00780   }
00781   return -1.0;
00782 
00783   /*
00784   Real a=endTime;
00785   Real b=sign*(2.0*(dx0+dx1)+4.0*(x0-x1)/endTime);
00786   if(FuzzyZero(b,EpsilonX)) {
00787     //need to double check for numerical instability
00788     //if sign is +, this means we're switching directly to -
00789     //if sign is -, this means we're switching directly to +
00790     //if(sign > 0.0 && x1 > x0+dx0*endTime) return -1;
00791     //else if(sign < 0.0 && x1 < x0+dx0*endTime) return -1;
00792     switchTime = 0;
00793     Real a=(dx1-dx0)/endTime;
00794     if((sign > 0.0) == (a >= 0.0)) return -1;
00795     else return Abs(a);
00796   }
00797   Real c=-Sqr(dx1-dx0)/endTime;
00798   if(FuzzyEquals(dx1,dx0,EpsilonV)) {
00799     //one of the solutions will be very close to zero, use alt solution
00800     Real a=-2.0*(dx0+dx1)/endTime + 4.0*(x1-x0)/Sqr(endTime);
00801     printf("only two solutions: 0 and %g\n",a);
00802     switchTime = 0.5*endTime;
00803     //try out the zero solution
00804     printf("diff at 0 solution: %g\n",x0-x1 + switchTime*(dx0+dx1));
00805     if(FuzzyEquals(x0 + switchTime*dx0,x1 - switchTime*dx1,EpsilonX)) 
00806       return 0;
00807     PARABOLIC_RAMP_ASSERT(FuzzyEquals(dx0 + switchTime*a,dx1 + switchTime*a,EpsilonV));
00808     PARABOLIC_RAMP_ASSERT(FuzzyEquals(x0 + switchTime*dx0 + 0.5*a*Sqr(switchTime),x1 - switchTime*dx1-0.5*a*Sqr(switchTime),EpsilonX));
00809     if((sign > 0.0) != (a >= 0.0)) return -1;
00810     return Abs(a);
00811   }
00812   if(FuzzyZero(c,EpsilonA)) {
00813     //need better numerical performance when dx1 ~= dx0
00814     a = a/Abs(dx1-dx0);
00815     b = b/Abs(dx1-dx0);
00816     c = -Abs(dx1-dx0)/endTime;
00817   }
00818   Real accel1,accel2;
00819   int res=quadratic(a,b,c,accel1,accel2);
00820   //remove negative accelerations
00821   if(res >= 1 && accel1 < 0) {
00822     accel1 = accel2;
00823     res--;
00824   }
00825   if(res >= 2 && accel2 < 0) {
00826     res--;
00827   }
00828 
00829   Real switchTime1 = endTime*0.5+sign*0.5*(dx1-dx0)/accel1;
00830   Real switchTime2 = endTime*0.5+sign*0.5*(dx1-dx0)/accel2;
00831   //if(accel1 == 0 && x0 == x1) switchTime1 = 0;
00832   //if(accel2 == 0 && x0 == x1) switchTime2 = 0;
00833   if(res==0) return -1;
00834   else if(res==1) {
00835     if(!IsFinite(accel1)) {
00836       printf("Error computing accelerations!\n");
00837       printf("Quadratic %g x^2 + %g x + %g = 0\n",a,b,c);
00838       printf("x0 %g, dx0 %g, x1 %g, dx1 %g\n",x0,dx0,x1,dx1);
00839       printf("EndTime %g, sign %g\n",endTime,sign);
00840       printf("Results %g %g\n",accel1,accel2);
00841       getchar();
00842     }
00843     if(switchTime1 >= 0 && switchTime1 <= endTime) { switchTime=switchTime1; return accel1; }
00844     return -1.0;
00845   }
00846   else if(res==2) {
00847     if(!IsFinite(accel1) || !IsFinite(accel2)) {
00848       printf("Error computing accelerations!\n");
00849       printf("Quadratic %g x^2 + %g x + %g = 0\n",a,b,c);
00850       printf("x0 %g, dx0 %g, x1 %g, dx1 %g\n",x0,dx0,x1,dx1);
00851       printf("EndTime %g, sign %g\n",endTime,sign);
00852       printf("Results %g %g\n",accel1,accel2);
00853       getchar();
00854     }
00855     if(FuzzyZero(switchTime1,EpsilonT) || FuzzyZero(switchTime2,EpsilonT)) {
00856       //need to double check for numerical instability
00857       //if sign is +, this means we're switching directly to -
00858       //if sign is -, this means we're switching directly to +
00859       if(sign > 0.0 && x1 > x0+dx0*endTime) return -1;
00860       else if(sign < 0 && x1 < x0+dx0*endTime) return -1;
00861       switchTime = 0;
00862       if(FuzzyZero(switchTime1,EpsilonT)) return accel1;
00863       else return accel2;
00864     }
00865     if(switchTime1 >= 0 && switchTime1 <= endTime) {
00866       if(switchTime2 >= 0 && switchTime2 <= endTime) {
00867         if(switchTime1 < switchTime2) { switchTime=switchTime1; return accel1; }
00868         else { switchTime=switchTime2; return accel2; }
00869       }
00870       else { switchTime=switchTime1; return accel1; }
00871     }
00872     else if(switchTime2 >= 0 && switchTime2 <= endTime) { switchTime=switchTime2; return accel2; }
00873     return -1.0;
00874   }
00875   return -1.0;
00876   */
00877 }
00878 
00879 
00880 Real PLPRamp::Evaluate(Real t) const
00881 {
00882   Real tmT = t - ttotal;
00883   if(t < tswitch1) return x0 + 0.5*a*Sqr(t) + dx0*t;
00884   else if(t < tswitch2) {
00885     Real xswitch = x0 + 0.5*a*Sqr(tswitch1) + dx0*tswitch1;
00886     return xswitch + (t-tswitch1)*v;
00887   }
00888   else return x1 - 0.5*a*Sqr(tmT) + dx1*tmT;
00889 }
00890 
00891 Real PLPRamp::Derivative(Real t) const
00892 {
00893   Real tmT = t - ttotal;
00894   if(t < tswitch1) return a*t + dx0;
00895   else if(t < tswitch2) return v;
00896   else return -a*tmT + dx1;
00897 }
00898 
00899 Real PLPRamp::Accel(Real t) const
00900 {
00901   if(t < tswitch1) return a;
00902   else if(t < tswitch2) return 0;
00903   else return -a;
00904 }
00905 
00906 Real PLPRamp::CalcTotalTime(Real a,Real v) const
00907 {
00908   Real t1 = (v-dx0)/a;
00909   Real t2mT = (dx1-v)/a;
00910   Real y1 = 0.5*(Sqr(v) - Sqr(dx0))/a + x0;
00911   Real y2 = 0.5*(Sqr(dx1) - Sqr(v))/a + x1;
00912   Real t2mt1 = (y2-y1)/v;
00913   //Real xswitch = x0 + 0.5*a*Sqr(t1) + dx0*t1;
00914   if(t1 < 0 || t2mT > 0 || t2mt1 < 0) return -1;
00915   if(!IsFinite(t1) || !IsFinite(t2mT)) return -1;
00916   return t1 + t2mt1 - t2mT;
00917 }
00918 
00919 Real PLPRamp::CalcSwitchTime1(Real a,Real v) const
00920 {
00921   Real t1 = (v-dx0)/a;
00922   if(t1 < 0) return -1;
00923   return t1;
00924 }
00925 
00926 Real PLPRamp::CalcSwitchTime2(Real a,Real v) const
00927 {
00928   Real t1 = (v-dx0)/a;
00929   Real y1 = 0.5*(Sqr(v) - Sqr(dx0))/a + x0;
00930   Real y2 = 0.5*(Sqr(dx1) - Sqr(v))/a + x1;
00931   Real t2mt1 = (y2-y1)/v;
00932   //Real t2mt1 = 0.5*(Sqr(dx1) + Sqr(dx0))/(v*a) - v/a + (x1 - x0)/v;
00933   //Real xswitch = x0 + 0.5*a*Sqr(t1) + dx0*t1;
00934   if(t1 < 0 || t2mt1 < 0) return -1;
00935   return t1 + t2mt1;
00936 }
00937 
00938 Real PLPRamp::CalcMinAccel(Real endTime,Real v) const
00939 {
00940   Real den=endTime*v - (x1-x0);
00941   //straight line sections have den ~= 0
00942   if(FuzzyZero(den,EpsilonX)) {
00943     if(FuzzyEquals(dx0,v,EpsilonV) && FuzzyEquals(dx1,v,EpsilonV)) return 0;
00944     return Inf;
00945   }
00946   
00947   //Real a = (v - (dx0+dx1) + 0.5/v*(Sqr(dx0)+Sqr(dx1)))/(endTime - (x1-x0)/v);
00948   Real a = (Sqr(v) - v*(dx0+dx1) + 0.5*(Sqr(dx0)+Sqr(dx1)))/den;
00949   /*
00950   Real t1 = (v-dx0)/a;
00951   Real t2mT = (dx1-v)/a;
00952   Real y1 = 0.5*(Sqr(v) - Sqr(dx0))/a + x0;
00953   Real y2 = 0.5*(Sqr(dx1) - Sqr(v))/a + x1;
00954   //Real t2mt1 = 0.5*(Sqr(dx1) + Sqr(dx0))/(v*a) - v/a + (x1 - x0)/v;
00955   Real t2mt1 = (y2-y1)/v;
00956   Real vold = v;
00957   //cout<<"EndTime "<<endTime<<", v "<<v<<endl;
00958   //cout<<"a = "<<a<<", t1="<<t1<<", t2mt1="<<t2mt1<<", t2mT="<<t2mT<<endl;
00959   if(t1 < 0 || t2mT > 0 || t2mt1 < 0) return Inf;
00960   if(!IsFinite(t1) || !IsFinite(t2mT)) return Inf;
00961   */
00962   if(!(CalcTotalTime(a,v) >= 0)) { return Inf; }
00963   return a;
00964 
00965   /*
00966   if(!(CalcTotalTime(a,v) >= 0)) {
00967     //this is very strange -- does it happen because of a compiler
00968     //optimization error?
00969     fprintf(stderr,"PLPRamp::CalcMinAccel: some numerical error prevented computing total time\n");
00970     fprintf(stderr,"  Ramp %g,%g -> %g,%g\n",x0,dx0,x1,dx1);
00971     fprintf(stderr,"  endTime %g, accel %g, vel %g, switch times %g %g, total time %g\n",endTime,a,v,CalcSwitchTime1(a,v),CalcSwitchTime2(a,v),CalcTotalTime(a,v)); 
00972     PARABOLIC_RAMP_ASSERT(v == vold);
00973     printf("y1=%g, y2=%g, t2mt1 = %g\n",y1,y2,t2mt1);
00974     Real y1_test = 0.5*(Sqr(v) - Sqr(dx0))/a + x0;
00975     Real y2_test = 0.5*(Sqr(dx1) - Sqr(v))/a + x1;
00976     Real t2mt1_test = (y2_test-y1_test)/v;
00977     //Real t2mt1_test = 0.5*(Sqr(dx1) + Sqr(dx0))/(v*a) - v/a + (x1 - x0)/v;
00978     printf("y1=%g, y2=%g, t2mt1 = %g\n",y1_test,y2_test,t2mt1_test);
00979     printf("dy1=%g, dy2=%g, dt2mt1 = %g\n",y1-y1_test,y2-y2_test,t2mt1-t2mt1_test);
00980     getchar();
00981     return Inf;
00982     PARABOLIC_RAMP_ASSERT(y1 == y1_test);
00983     PARABOLIC_RAMP_ASSERT(y2 == y2_test);
00984     PARABOLIC_RAMP_ASSERT(y2-y1 == y2_test-y1_test);
00985     PARABOLIC_RAMP_ASSERT(t2mt1 == t2mt1_test);
00986   }
00987   PARABOLIC_RAMP_ASSERT(CalcTotalTime(a,v) >= 0);
00988   return a;
00989   */
00990 }
00991 
00992 
00993 bool PLPRamp::SolveMinTime(Real amax,Real vmax)
00994 {
00995   return SolveMinTime2(amax,vmax,0);
00996 }
00997 
00998 bool PLPRamp::SolveMinTime2(Real amax,Real vmax,Real timeLowerBound)
00999 {
01000   Real t1 = CalcTotalTime(amax,vmax);
01001   Real t2 = CalcTotalTime(-amax,vmax);
01002   Real t3 = CalcTotalTime(amax,-vmax);
01003   Real t4 = CalcTotalTime(-amax,-vmax);
01004   //cout<<"Time for PLP ++-: "<<t1<<", -++: "<<t2<<", +--: "<<t3<<", --+: "<<t4<<endl;
01005   ttotal = Inf;
01006   if(t1 >= timeLowerBound && t1 < ttotal) {
01007     a = amax;
01008     v = vmax;
01009     ttotal = t1;
01010   }
01011   if(t2 >= timeLowerBound && t2 < ttotal) {
01012     a = -amax;
01013     v = vmax;
01014     ttotal = t2;
01015   }
01016   if(t3 >= timeLowerBound && t3 < ttotal) {
01017     a = amax;
01018     v = -vmax;
01019     ttotal = t3;
01020   }
01021   if(t4 >= timeLowerBound && t4 < ttotal) {
01022     a = -amax;
01023     v = -vmax;
01024     ttotal = t4;
01025   }
01026   if(IsInf(ttotal)) {
01027     a = v = 0;
01028     tswitch1 = tswitch2 = ttotal = -1;
01029 
01030     //printf("Times... %g %g %g %g\n",t1,t2,t3,t4);
01031     //printf("Trying alternate MinTime2 solution technique...\n");
01032     Real v1 = CalcMinTime2(timeLowerBound,amax,vmax);
01033     Real v2 = CalcMinTime2(timeLowerBound,-amax,vmax);
01034     if(v1 > 0) {
01035       a = amax;
01036       v = v1;
01037       tswitch1 = (v1-dx0)/a;
01038       tswitch2 = timeLowerBound - (v1-dx1)/a;
01039       ttotal = timeLowerBound;
01040       //printf("Candidate 1 timing %g %g %g\n",tswitch1,tswitch2,ttotal);
01041       //printf("Test 1 x %g %g\n",x1-(ttotal-tswitch2)*v1+0.5*Sqr(ttotal-tswitch2)*a,x0+tswitch1*dx0+0.5*Sqr(tswitch1)*a+(tswitch1-tswitch2)*v1);
01042       //printf("x1, v1 = %g, %g\n",x0+dx0*tswitch1+0.5*Sqr(tswitch1)*a,dx0+tswitch1*a);
01043       //printf("x2, v2 = %g, %g\n",x1-dx1*(ttotal-tswitch2)+0.5*Sqr(ttotal-tswitch2)*a,dx1+(ttotal-tswitch2)*a);
01044       return true;
01045     }
01046     if(v2 > 0) {
01047       a = -amax;
01048       v = v2;
01049       tswitch1 = (v2-dx0)/a;
01050       tswitch2 = timeLowerBound - (v2-dx1)/a;
01051       ttotal = timeLowerBound;
01052       //printf("Candidate 2 timing %g %g %g\n",tswitch1,tswitch2,ttotal);
01053       //printf("Test 2 x %g %g\n",x1-(ttotal-tswitch2)*v1+0.5*Sqr(ttotal-tswitch2)*a,x0+tswitch1*dx0+0.5*Sqr(tswitch1)*a+(tswitch1-tswitch2)*v1);
01054       //printf("x1, v1 = %g, %g\n",x0+dx0*tswitch1+0.5*Sqr(tswitch1)*a,dx0+tswitch1*a);
01055       //printf("x2, v2 = %g, %g\n",x1-dx1*(ttotal-tswitch1)+0.5*Sqr(ttotal-tswitch1)*a,dx1+(ttotal-tswitch2)*a);
01056       return true;
01057     }
01058     return false;
01059   }
01060   tswitch1 = CalcSwitchTime1(a,v);
01061   tswitch2 = CalcSwitchTime2(a,v);
01062 
01063   if(tswitch1 > tswitch2 && FuzzyEquals(tswitch1,tswitch2,EpsilonT)) {
01064     tswitch1 = tswitch2 = 0.5*(tswitch1+tswitch2);
01065   }
01066   if(tswitch2 > ttotal && FuzzyEquals(tswitch2,ttotal,EpsilonT)) {
01067     tswitch2 = ttotal;
01068   }
01069 
01070   if(gValidityCheckLevel >= 1) {
01071     Real t2mT = tswitch2-ttotal;
01072     Real xswitch = x0 + 0.5*a*Sqr(tswitch1) + dx0*tswitch1;
01073     Real xswitch2 = xswitch + (tswitch2-tswitch1)*v;
01074     if(!FuzzyEquals(xswitch2,x1 - 0.5*a*Sqr(t2mT) + dx1*t2mT,EpsilonX)) {
01075       if(gVerbose >= 1)
01076         PARABOLIC_RAMP_PERROR("PLPRamp::SolveMinTime2: incorrect switch 2 position: %g vs %g\n",xswitch2,x1 - 0.5*a*Sqr(t2mT) + dx1*t2mT);
01077       if(gVerbose >= 2) {
01078         PARABOLIC_RAMP_PERROR("Ramp %g,%g -> %g,%g\n",x0,dx0,x1,dx1);
01079         PARABOLIC_RAMP_PERROR("Acceleration %g, vel %g, deceleration %g\n",a,v,-a);
01080         PARABOLIC_RAMP_PERROR("Switch times %g %g %g\n",tswitch1,tswitch2,ttotal);
01081       }
01082       if(gErrorSave) SaveRamp("PLP_SolveMinTime_failure.dat",x0,dx0,x1,dx1,amax,vmax,timeLowerBound);
01083       if(gErrorGetchar) getchar();
01084       return false;
01085     }
01086   }
01087   return true;
01088 }
01089 
01090 
01091 bool PLPRamp::SolveMinAccel(Real endTime,Real vmax)
01092 {
01093   Real a1 = CalcMinAccel(endTime,vmax);
01094   Real a2 = CalcMinAccel(endTime,-vmax);
01095   a = Inf;
01096   if(fabs(a1) < a) {
01097     a = a1;
01098     v = vmax;
01099   }
01100   if(fabs(a2) < a) {
01101     a = a2;
01102     v = -vmax;
01103   }
01104   if(IsInf(a)) {
01105     a = 0;
01106     tswitch1 = tswitch2 = ttotal = -1;
01107     return false;
01108   }
01109   if(fabs(a) == 0) {
01110     tswitch1 = 0;
01111     tswitch2 = endTime;
01112     ttotal = endTime;
01113   }
01114   else {
01115     ttotal = CalcTotalTime(a,v);
01116     tswitch1 = CalcSwitchTime1(a,v);
01117     tswitch2 = CalcSwitchTime2(a,v);
01118 
01119     if(tswitch1 > tswitch2 && FuzzyEquals(tswitch1,tswitch2,EpsilonT)) {
01120       tswitch1 = tswitch2 = 0.5*(tswitch1+tswitch2);
01121     }
01122     if(tswitch2 > endTime && FuzzyEquals(tswitch2,endTime,EpsilonT)) {
01123       tswitch2 = endTime;
01124     }
01125     if(ttotal < 0) {  //there was an error computing the total time
01126       if(gVerbose >= 1) PARABOLIC_RAMP_PERROR("PLPRamp::SolveMinAccel: some numerical error prevented computing total time\n");
01127       if(gVerbose >= 2) {
01128         PARABOLIC_RAMP_PERROR("  Ramp %g,%g -> %g,%g\n",x0,dx0,x1,dx1);
01129         PARABOLIC_RAMP_PERROR("  endTime %g, accel %g, vel %g, switch times %g %g, total time %g\n",endTime,a,v,tswitch1,tswitch2,ttotal); 
01130       }
01131       if(gErrorSave) SaveRamp("PLP_SolveMinAccel_failure.dat",x0,dx0,x1,dx1,-1,vmax,endTime);
01132       if(gErrorGetchar) getchar();
01133       return false;
01134     }
01135   }
01136   if(gValidityCheckLevel >= 1) {
01137     if(ttotal > endTime + EpsilonT) {
01138       if(gVerbose >= 1) 
01139         PARABOLIC_RAMP_PERROR("PLPRamp::SolveMinAccel: total time greater than endTime!\n");
01140       if(gVerbose >= 2) 
01141         PARABOLIC_RAMP_PERROR("  endTime %g, accel %g, vel %g, switch times %g %g, total time %g\n",endTime,a,v,tswitch1,tswitch2,ttotal); 
01142       if(gErrorSave) SaveRamp("PLP_SolveMinAccel_failure.dat",x0,dx0,x1,dx1,-1,vmax,endTime);
01143       if(gErrorGetchar) getchar();
01144       return false;
01145     }
01146     if(fabs(ttotal-endTime) >= EpsilonT) {
01147       if(gVerbose >= 1)
01148         PARABOLIC_RAMP_PERROR("PLPRamp::SolveMinAccel: total time and endTime are different!\n");
01149       if(gVerbose >= 2)
01150         PARABOLIC_RAMP_PERROR("  endTime %g, accel %g, vel %g, switch times %g %g, total time %g\n",endTime,a,v,tswitch1,tswitch2,ttotal); 
01151       if(gErrorSave) SaveRamp("PLP_SolveMinAccel_failure.dat",x0,dx0,x1,dx1,-1,vmax,endTime);
01152       if(gErrorGetchar) getchar();
01153     }
01154   }
01155   PARABOLIC_RAMP_ASSERT(fabs(ttotal-endTime) < EpsilonT);
01156   //fiddle with the numerical errors
01157   ttotal = endTime;
01158   if(tswitch2 > ttotal) tswitch2=ttotal;
01159   if(tswitch1 > ttotal) tswitch1=ttotal;
01160 
01161   if(gValidityCheckLevel >= 1) {
01162     Real t2mT = tswitch2-ttotal;
01163     Real xswitch = x0 + 0.5*a*Sqr(tswitch1) + dx0*tswitch1;
01164     Real xswitch2 = xswitch + (tswitch2-tswitch1)*v;
01165     if(!FuzzyEquals(xswitch2,x1 - 0.5*a*Sqr(t2mT) + dx1*t2mT,EpsilonX)) {
01166       if(gVerbose >= 1) PARABOLIC_RAMP_PERROR("PLP Ramp has incorrect switch 2 position: %g vs %g\n",xswitch2,x1 - 0.5*a*Sqr(t2mT) + dx1*t2mT);
01167       if(gVerbose >= 2) {
01168         PARABOLIC_RAMP_PERROR("Ramp %g,%g -> %g,%g\n",x0,dx0,x1,dx1);
01169         PARABOLIC_RAMP_PERROR("Acceleration %g, vel %g, deceleration %g\n",a,v,-a);
01170         PARABOLIC_RAMP_PERROR("Switch times %g %g %g\n",tswitch1,tswitch2,ttotal);
01171       }
01172       if(gErrorSave) SaveRamp("PLP_SolveMinAccel_failure.dat",x0,dx0,x1,dx1,-1,vmax,endTime);
01173       if(gErrorGetchar) getchar();
01174       return false;
01175     }
01176   }
01177   return true;
01178 }
01179 
01180 Real PLPRamp::CalcMinTime2(Real endTime,Real a,Real vmax) const
01181 {
01182   //Try alternate solution technique with acceleration bounded, time fixed, velocity variable
01183   Real b = -a*endTime - (dx1+dx0);
01184   Real c = a*(x1-x0) + (Sqr(dx0)+Sqr(dx1))*0.5;
01185   Real v1,v2;
01186   //printf("Quadratic coeffs %g, %g, %g\n",1.0,b,c);
01187   int res=quadratic(1.0,b,c,v1,v2);
01188   //printf("Quadratic res %d, accel %g, velocities %g %g\n",res,a,v1,v2);
01189   if(res >= 1) {
01190     Real ts1 = (v1-dx0)/a;
01191     Real ts2 = endTime - (v1-dx1)/a;
01192     //printf("Solution 1 times %g %g %g\n",ts1,ts2,endTime);
01193     if(Abs(v1) <= vmax && ts1 >= 0 && ts2 >= ts1 && ts2 <= endTime) return v1;  //it's a valid solution!
01194   }
01195   if(res == 2) {
01196     Real ts1 = (v2-dx0)/a;
01197     Real ts2 = endTime - (v2-dx1)/a;
01198     //printf("Solution 2 times %g %g %g\n",ts1,ts2,endTime);
01199     if(Abs(v2) <= vmax && ts1 >= 0 && ts2 >= ts1 && ts2 <= endTime) return v2;  //it's a valid solution!
01200   }
01201   return -1;
01202 }
01203 
01204 
01205 
01206 
01207 
01208 
01209 void ParabolicRamp1D::SetConstant(Real x,Real t)
01210 {
01211   x0 = x1 = x;
01212   dx0 = dx1 = 0;
01213   tswitch1=tswitch2=ttotal=t;
01214   v = a1 = a2 = 0;
01215 }
01216 
01217 void ParabolicRamp1D::SetLinear(Real _x0,Real _x1,Real t)
01218 {
01219   PARABOLIC_RAMP_ASSERT(t > 0);
01220   x0 = _x0;
01221   x1 = _x1;
01222   v = dx0 = dx1 = (_x1-_x0)/t;
01223   a1 = a2 = 0;
01224   tswitch1 = 0;
01225   tswitch2 = t;
01226   ttotal = t;
01227 }
01228 
01229 Real ParabolicRamp1D::Evaluate(Real t) const
01230 {
01231   Real tmT = t - ttotal;
01232   if(t < tswitch1) return x0 + 0.5*a1*t*t + dx0*t;
01233   else if(t < tswitch2) {
01234     Real xswitch = x0 + 0.5*a1*tswitch1*tswitch1 + dx0*tswitch1;
01235     return xswitch + (t-tswitch1)*v;
01236   }
01237   else return x1 + 0.5*a2*tmT*tmT + dx1*tmT;
01238 }
01239 
01240 Real ParabolicRamp1D::Derivative(Real t) const
01241 {
01242   if(t < tswitch1) return a1*t + dx0;
01243   else if(t < tswitch2) return v;
01244   else {
01245     Real tmT = t - ttotal;
01246     return a2*tmT + dx1;
01247   }
01248 }
01249 
01250 Real ParabolicRamp1D::Accel(Real t) const
01251 {
01252   if(t < tswitch1) return a1;
01253   else if(t < tswitch2) return 0;
01254   else return a2;
01255 }
01256 
01257 bool ParabolicRamp1D::SolveMinAccel(Real endTime,Real vmax)
01258 {
01259   ParabolicRamp p;
01260   PPRamp pp;
01261   PLPRamp plp;
01262   p.x0 = pp.x0 = plp.x0 = x0;
01263   p.x1 = pp.x1 = plp.x1 = x1;
01264   p.dx0 = pp.dx0 = plp.dx0 = dx0;
01265   p.dx1 = pp.dx1 = plp.dx1 = dx1;
01266   bool pres = p.SolveFixedTime(endTime);
01267   bool ppres = pp.SolveMinAccel(endTime);
01268   bool plpres = false;
01269   if(!IsInf(vmax))
01270     plpres = plp.SolveMinAccel(endTime,vmax);
01271   //cout<<"PP a: "<<pp.a<<", max vel "<<pp.MaxVelocity()<<endl;
01272   //cout<<"PLP a: "<<plp.a<<", vel "<<plp.v<<endl;
01273   a1 = Inf;
01274   if(pres && FuzzyEquals(endTime,p.ttotal,EpsilonT) && Abs(p.MaxVelocity()) <= vmax) {
01275     if(FuzzyEquals(p.Evaluate(endTime),x1,EpsilonX) && FuzzyEquals(p.Derivative(endTime),dx1,EpsilonV)) {
01276       a1 = p.a;
01277       v = 0;
01278       //tswitch1 = tswitch2 = p.ttotal;
01279       //ttotal = p.ttotal;
01280       tswitch1 = tswitch2 = endTime;
01281       ttotal = endTime;
01282     }
01283   }
01284   if(ppres && Abs(pp.MaxVelocity()) <= vmax && Abs(pp.a) < Abs(a1)) {
01285     a1 = pp.a;
01286     v = 0;
01287     tswitch1 = tswitch2 = pp.tswitch;
01288     ttotal = pp.ttotal;
01289   }
01290   if(plpres && Abs(plp.v) <= vmax && Abs(plp.a) < Abs(a1)) {
01291     a1 = plp.a;
01292     v = plp.v;
01293     tswitch1 = plp.tswitch1;
01294     tswitch2 = plp.tswitch2;
01295     ttotal = plp.ttotal;
01296   }
01297   a2 = -a1;
01298   
01299   if(IsInf(a1)) {
01300     if(vmax == 0) {
01301       if(FuzzyEquals(x0,x1,EpsilonX) && FuzzyEquals(dx0,dx1,EpsilonV)) {
01302         a1 = a2 = v = 0;
01303         tswitch1 = tswitch2 = ttotal = endTime;
01304         return true;
01305       }
01306     }
01307     if(ppres && Abs(pp.MaxVelocity()) <= vmax + EpsilonV) {
01308       //some slight numerical error caused velocity to exceed maximum
01309       a1 = pp.a;
01310       a2 = -pp.a;
01311       v = 0;
01312       tswitch1 = tswitch2 = pp.tswitch;
01313       ttotal = pp.ttotal;
01314       if(IsValid()) return true;
01315     }
01316     a1 = a2 = v = 0;
01317     tswitch1 = tswitch2 = ttotal = -1;
01318     if(!gMinAccelQuiet) {
01319       if(gVerbose >= 1) 
01320         PARABOLIC_RAMP_PERROR("ParabolicRamp1D::SolveMinAccel: No ramp equation was successful.\n");
01321       if(gVerbose >= 2) {
01322         PARABOLIC_RAMP_PERROR("x0=%g, x1=%g, dx0=%g, dx1=%g\n",x0,x1,dx0,dx1);
01323         PARABOLIC_RAMP_PERROR("end time %g, vmax = %g\n",endTime,vmax);
01324         
01325         PARABOLIC_RAMP_PERROR("P=%d, PP=%d, PLP=%d\n",(int)pres,(int)ppres,(int)plpres);
01326         PARABOLIC_RAMP_PERROR("p.a = %g, max vel=%g, end x=%g, end dx=%g\n",p.a,p.MaxVelocity(),p.Evaluate(endTime),p.Derivative(endTime));
01327         PARABOLIC_RAMP_PERROR("pp.a = %g, max vel=%g\n",pp.a,pp.MaxVelocity());
01328         PARABOLIC_RAMP_PERROR("plp.a = %g, v=%g\n",plp.a,plp.v);
01329         
01330         Real switch1,switch2;
01331         Real apn = pp.CalcMinAccel(endTime,1.0,switch1);
01332         Real anp = pp.CalcMinAccel(endTime,-1.0,switch2);
01333         PARABOLIC_RAMP_PERROR("PP Calcuations: +: %g %g, -: %g %g\n",apn,switch1,anp,switch2);
01334       }
01335       if(gErrorSave) SaveRamp("Ramp_SolveMinAccel_failure.dat",x0,dx0,x1,dx1,-1,vmax,endTime);
01336       if(gErrorGetchar) getchar();
01337     }
01338     return false;
01339   }
01340   if(gValidityCheckLevel >= 2) {
01341     PARABOLIC_RAMP_ASSERT(ttotal==endTime);
01342     if(!IsValid()) {
01343       if(gVerbose >= 1) {
01344         PARABOLIC_RAMP_PERROR("Invalid min-accel!\n");
01345         PARABOLIC_RAMP_PERROR("x0=%g, x1=%g, dx0=%g, dx1=%g\n",x0,x1,dx0,dx1);
01346         PARABOLIC_RAMP_PERROR("end time %g, vmax = %g\n",endTime,vmax);
01347         
01348         PARABOLIC_RAMP_PERROR("P=%d, PP=%d, PLP=%d\n",(int)pres,(int)ppres,(int)plpres);
01349         PARABOLIC_RAMP_PERROR("p.a = %g, max vel=%g\n",p.a,p.MaxVelocity());
01350         PARABOLIC_RAMP_PERROR("pp.a = %g, max vel=%g\n",pp.a,pp.MaxVelocity());
01351         PARABOLIC_RAMP_PERROR("plp.a = %g, v=%g\n",plp.a,plp.v);
01352       }
01353       if(gErrorGetchar) getchar();
01354       return false;
01355     }
01356   }
01357   return true;
01358 }
01359 
01360 bool ParabolicRamp1D::SolveMinTime(Real amax,Real vmax)
01361 {
01362   ParabolicRamp p;
01363   PPRamp pp;
01364   PLPRamp plp;
01365   p.x0 = pp.x0 = plp.x0 = x0;
01366   p.x1 = pp.x1 = plp.x1 = x1;
01367   p.dx0 = pp.dx0 = plp.dx0 = dx0;
01368   p.dx1 = pp.dx1 = plp.dx1 = dx1;
01369   bool pres = p.Solve(amax);
01370   bool ppres = pp.SolveMinTime(amax);
01371   bool plpres = false;
01372   if(!IsInf(vmax))
01373     plpres = plp.SolveMinTime(amax,vmax);
01374   //cout<<"P time: "<<p.ttotal<<", accel "<<p.a<<endl;
01375   //cout<<"PP time: "<<pp.ttotal<<", max vel "<<pp.MaxVelocity()<<endl;
01376   //cout<<"PLP time: "<<plp.ttotal<<", vel "<<plp.v<<endl;
01377   ttotal = Inf;
01378   if(pres && Abs(p.a) <= amax+EpsilonA && p.ttotal < ttotal) {
01379     if(Abs(p.a) <= amax) {
01380       a1 = p.a;
01381       v = 0;
01382       tswitch1 = tswitch2 = p.ttotal;
01383       ttotal = p.ttotal;
01384     }
01385     else {
01386       //double check
01387       p.a = Sign(p.a)*amax;
01388       if(FuzzyEquals(p.Evaluate(p.ttotal),x1,EpsilonX) && FuzzyEquals(p.Derivative(p.ttotal),dx1,EpsilonV)) {
01389         a1 = p.a;
01390         v = 0;
01391         tswitch1=tswitch2=p.ttotal;
01392         ttotal = p.ttotal;
01393       }
01394     }
01395   }
01396   if(ppres && Abs(pp.MaxVelocity()) <= vmax && pp.ttotal < ttotal) {
01397     a1 = pp.a;
01398     v = 0;
01399     tswitch1 = tswitch2 = pp.tswitch;
01400     ttotal = pp.ttotal;
01401   }
01402   if(plpres && plp.ttotal < ttotal) {
01403     a1 = plp.a;
01404     v = plp.v;
01405     tswitch1 = plp.tswitch1;
01406     tswitch2 = plp.tswitch2;
01407     ttotal = plp.ttotal;
01408   }
01409   if(IsInf(ttotal)) {
01410     if(gVerbose >= 1)
01411       PARABOLIC_RAMP_PERROR("ParabolicRamp1D::SolveMinTime: No solution found!\n");
01412     if(gVerbose >= 2) {
01413       PARABOLIC_RAMP_PERROR("x0=%g, x1=%g, dx0=%g, dx1=%g\n",x0,x1,dx0,dx1);
01414       PARABOLIC_RAMP_PERROR("vmax = %g, amax = %g\n",vmax,amax);
01415       PARABOLIC_RAMP_PERROR("P=%d, PP=%d, PLP=%d\n",(int)pres,(int)ppres,(int)plpres);
01416       if(pres) 
01417         PARABOLIC_RAMP_PERROR("  P a=%g, ttotal=%g\n",p.a,p.ttotal);
01418       if(ppres) 
01419         PARABOLIC_RAMP_PERROR("  PP a=%g, tswitch=%g, ttotal=%g\n",pp.a,pp.tswitch,pp.ttotal);
01420       if(plpres) 
01421         PARABOLIC_RAMP_PERROR("  PLP a=%g, tswitch=%g, %g, ttotal=%g\n",pp.a,plp.tswitch1,plp.tswitch2,plp.ttotal);
01422       PARABOLIC_RAMP_PERROR("\n");
01423     }
01424     if(gErrorSave) SaveRamp("Ramp_SolveMinTime_failure.dat",x0,dx0,x1,dx1,amax,vmax,-1);
01425     a1 = a2 = v = 0;
01426     tswitch1 = tswitch2 = ttotal = -1;
01427     return false;
01428   }
01429   a2 = -a1;
01430   //cout<<"switch time 1: "<<tswitch1<<", 2: "<<tswitch2<<", total "<<ttotal<<endl;
01431   if(gValidityCheckLevel >= 2) {
01432     if(!IsValid()) {
01433       if(gVerbose >= 1) {
01434         PARABOLIC_RAMP_PERROR("Solved for invalid min-time path!\n");
01435         PARABOLIC_RAMP_PERROR("x0=%g, x1=%g, dx0=%g, dx1=%g\n",x0,x1,dx0,dx1);
01436         PARABOLIC_RAMP_PERROR("vmax = %g, amax = %g\n",vmax,amax);
01437         PARABOLIC_RAMP_PERROR("P=%d, PP=%d, PLP=%d\n",(int)pres,(int)ppres,(int)plpres);
01438       }
01439       if(gErrorGetchar) getchar();
01440       return false;
01441     }
01442   }
01443   return true;
01444 }
01445 
01446 bool ParabolicRamp1D::SolveMinTime2(Real amax,Real vmax,Real tLowerBound)
01447 {
01448   ParabolicRamp p;
01449   PPRamp pp;
01450   PLPRamp plp;
01451   p.x0 = pp.x0 = plp.x0 = x0;
01452   p.x1 = pp.x1 = plp.x1 = x1;
01453   p.dx0 = pp.dx0 = plp.dx0 = dx0;
01454   p.dx1 = pp.dx1 = plp.dx1 = dx1;
01455   bool pres = p.Solve(amax);
01456   bool ppres = pp.SolveMinTime2(amax,tLowerBound);
01457   bool plpres = false;
01458   if(!IsInf(vmax))
01459     plpres = plp.SolveMinTime2(amax,vmax,tLowerBound);
01460   //cout<<"P time: "<<p.ttotal<<", accel "<<p.a<<endl;
01461   //cout<<"PP time: "<<pp.ttotal<<", max vel "<<pp.MaxVelocity()<<endl;
01462   //cout<<"PLP time: "<<plp.ttotal<<", vel "<<plp.v<<endl;
01463   ttotal = Inf;
01464   if(pres && Abs(p.a) <= amax+EpsilonA && p.ttotal < ttotal && p.ttotal >= tLowerBound) {
01465     if(Abs(p.a) <= amax) {
01466       a1 = p.a;
01467       v = 0;
01468       tswitch1 = tswitch2 = p.ttotal;
01469       ttotal = p.ttotal;
01470     }
01471     else {
01472       //double check
01473       p.a = Sign(p.a)*amax;
01474       if(FuzzyEquals(p.Evaluate(p.ttotal),x1,EpsilonX) && FuzzyEquals(p.Derivative(p.ttotal),dx1,EpsilonV)) {
01475         a1 = p.a;
01476         v = 0;
01477         tswitch1=tswitch2=p.ttotal;
01478         ttotal = p.ttotal;
01479       }
01480     }
01481   }
01482   if(ppres && Abs(pp.MaxVelocity()) <= vmax && pp.ttotal < ttotal) {
01483     a1 = pp.a;
01484     v = 0;
01485     tswitch1 = tswitch2 = pp.tswitch;
01486     ttotal = pp.ttotal;
01487   }
01488   if(plpres && plp.ttotal < ttotal) {
01489     a1 = plp.a;
01490     v = plp.v;
01491     tswitch1 = plp.tswitch1;
01492     tswitch2 = plp.tswitch2;
01493     ttotal = plp.ttotal;
01494   }
01495   if(IsInf(ttotal)) {
01496     if(!gMinTime2Quiet) {
01497       if(gVerbose >= 1) 
01498         PARABOLIC_RAMP_PERROR("ParabolicRamp1D::SolveMinTime2: No solution found!\n");
01499       if(gVerbose >= 2) {
01500         PARABOLIC_RAMP_PERROR("x0=%g, x1=%g, dx0=%g, dx1=%g\n",x0,x1,dx0,dx1);
01501         PARABOLIC_RAMP_PERROR("vmax = %g, amax = %g, tmax = %g\n",vmax,amax,tLowerBound);
01502         PARABOLIC_RAMP_PERROR("P=%d, PP=%d, PLP=%d\n",(int)pres,(int)ppres,(int)plpres);
01503         if(pres) 
01504           PARABOLIC_RAMP_PERROR("  P a=%g, ttotal=%g\n",p.a,p.ttotal);
01505         if(ppres) 
01506           PARABOLIC_RAMP_PERROR("  PP a=%g, tswitch=%g, ttotal=%g\n",pp.a,pp.tswitch,pp.ttotal);
01507         if(plpres) 
01508           PARABOLIC_RAMP_PERROR("  PLP a=%g, tswitch=%g, %g, ttotal=%g\n",pp.a,plp.tswitch1,plp.tswitch2,plp.ttotal);
01509         ppres = pp.SolveMinTime(amax);
01510         plpres = plp.SolveMinTime(amax,vmax);
01511         PARABOLIC_RAMP_PERROR("unconstrained PP (%d): %g, PLP (%d): %g\n",(int)ppres,pp.ttotal,(int)plpres,plp.ttotal);
01512         PARABOLIC_RAMP_PERROR("\n");
01513       }
01514       if(gErrorSave) SaveRamp("Ramp_SolveMinTime_failure.dat",x0,dx0,x1,dx1,amax,vmax,tLowerBound);
01515     }
01516     a1 = a2 = v = 0;
01517     tswitch1 = tswitch2 = ttotal = -1;
01518     return false;
01519   }
01520   a2 = -a1;
01521   //cout<<"switch time 1: "<<tswitch1<<", 2: "<<tswitch2<<", total "<<ttotal<<endl;
01522   if(gValidityCheckLevel >= 2) {
01523     if(!IsValid()) {
01524       if(gVerbose >= 1) {
01525         PARABOLIC_RAMP_PERROR("ParabolicRamp1D::SolveMinTime: Failure to find valid path!\n");
01526         PARABOLIC_RAMP_PERROR("x0=%g, x1=%g, dx0=%g, dx1=%g\n",x0,x1,dx0,dx1);
01527         PARABOLIC_RAMP_PERROR("vmax = %g, amax = %g\n",vmax,amax);
01528         PARABOLIC_RAMP_PERROR("P=%d, PP=%d, PLP=%d\n",(int)pres,(int)ppres,(int)plpres);
01529         if(gErrorSave) SaveRamp("Ramp_SolveMinTime_failure.dat",x0,dx0,x1,dx1,amax,vmax,tLowerBound);
01530         PARABOLIC_RAMP_PERROR("\n");
01531         if(gErrorGetchar) getchar();
01532       }
01533       return false;
01534     }
01535     PARABOLIC_RAMP_ASSERT(ttotal >= tLowerBound);
01536   }
01537   return true;
01538 }
01539 
01540 void ParabolicRamp1D::SolveBraking(Real amax)
01541 {
01542   tswitch1 = 0;
01543   tswitch2 = 0;
01544   a1 = Sign(dx0)*amax;
01545   v = 0;
01546   a2 = -Sign(dx0)*amax;
01547   ttotal = Abs(dx0)/amax;
01548   x1 = x0 + dx0*ttotal + 0.5*Sqr(ttotal)*a2;
01549   dx1 = 0;
01550   if(gValidityCheckLevel >= 2) 
01551     PARABOLIC_RAMP_ASSERT(IsValid());
01552 }
01553 
01554 void ParabolicRamp1D::Dilate(Real timeScale)
01555 {
01556   tswitch1*=timeScale;
01557   tswitch2*=timeScale;
01558   ttotal*=timeScale;
01559   a1 *= 1.0/Sqr(timeScale);
01560   a2 *= 1.0/Sqr(timeScale);
01561   v *= 1.0/timeScale;
01562 }
01563 
01564 void ParabolicRamp1D::TrimFront(Real tcut)
01565 {
01566   if(tcut > ttotal) {
01567     PARABOLIC_RAMP_PERROR("Hmm... want to trim front of curve at time %g, end time %g\n",tcut,ttotal);
01568   }
01569   PARABOLIC_RAMP_ASSERT(tcut <= ttotal);
01570   x0 = Evaluate(tcut);
01571   dx0 = Derivative(tcut);
01572   ttotal -= tcut;
01573   tswitch1 -= tcut;
01574   tswitch2 -= tcut;
01575   if(tswitch1 < 0) tswitch1=0;
01576   if(tswitch2 < 0) tswitch2=0;
01577   if(gValidityCheckLevel >= 2) 
01578     PARABOLIC_RAMP_ASSERT(IsValid());
01579 
01580 }
01581 
01582 void ParabolicRamp1D::TrimBack(Real tcut)
01583 {
01584   x1 = Evaluate(ttotal-tcut);
01585   dx1 = Derivative(ttotal-tcut);
01586   ttotal -= tcut;
01587   tswitch1 = Min(tswitch1,ttotal);
01588   tswitch2 = Min(tswitch2,ttotal);
01589   if(gValidityCheckLevel >= 2) 
01590     PARABOLIC_RAMP_ASSERT(IsValid());
01591 }
01592 
01593 void ParabolicRamp1D::Bounds(Real& xmin,Real& xmax) const
01594 {
01595   Bounds(0,ttotal,xmin,xmax);
01596 }
01597 
01598 void ParabolicRamp1D::Bounds(Real ta,Real tb,Real& xmin,Real& xmax) const
01599 {
01600   if(ta > tb) {  //orient the interval
01601     return Bounds(tb,ta,xmin,xmax);
01602   }
01603   if(ta < 0) ta = 0;
01604   if(tb <= 0) {
01605     xmin = xmax = x0;
01606     return;
01607   }
01608   if(tb > ttotal) tb=ttotal;
01609   if(ta >= ttotal) {
01610     xmin = xmax = x1;
01611     return;
01612   }
01613 
01614   xmin = Evaluate(ta);
01615   xmax = Evaluate(tb);
01616 
01617   if(xmin > xmax) Swap(xmin,xmax);
01618 
01619   Real tflip1=0,tflip2=0;
01620   if(ta < tswitch1) {
01621     //x' = a1*t + v0 = 0 => t = -v0/a1
01622     tflip1 = -dx0/a1;
01623     if(tflip1 > tswitch1) tflip1 = 0;
01624   }
01625   if(tb > tswitch2) {
01626     //x' = a2*(T-t) + v1 = 0 => (T-t) = v1/a2
01627     tflip2 = ttotal-dx1/a2;
01628     if(tflip2 < tswitch2) tflip2 = 0;
01629   }
01630   if(ta < tflip1 && tb > tflip1) {
01631     Real xflip = Evaluate(tflip1);
01632     if(xflip < xmin) xmin = xflip;
01633     else if(xflip > xmax) xmax = xflip;
01634   }
01635   if(ta < tflip2 && tb > tflip2) {
01636     Real xflip = Evaluate(tflip2);
01637     if(xflip < xmin) xmin = xflip;
01638     else if(xflip > xmax) xmax = xflip;
01639   }
01640 }
01641 
01642 void ParabolicRamp1D::DerivBounds(Real& vmin,Real& vmax) const
01643 {
01644   DerivBounds(0,ttotal,vmin,vmax);
01645 }
01646 
01647 void ParabolicRamp1D::DerivBounds(Real ta,Real tb,Real& vmin,Real& vmax) const
01648 {
01649   if(ta > tb) {  //orient the interval
01650     return DerivBounds(tb,ta,vmin,vmax);
01651   }
01652   if(ta < 0) ta = 0;
01653   if(tb <= 0) {
01654     vmin = vmax = dx0;
01655     return;
01656   }
01657   if(tb > ttotal) tb=ttotal;
01658   if(ta >= ttotal) {
01659     vmin = vmax = dx1;
01660     return;
01661   }
01662 
01663   vmin = Derivative(ta);
01664   vmax = Derivative(tb);
01665   if(vmin > vmax) Swap(vmin,vmax);
01666 
01667   if(tswitch2 > tswitch1) { //consider linear part
01668     if(ta < tswitch2 && tb > tswitch1) {
01669       vmin = Min(vmin,v);
01670       vmax = Min(vmax,v);
01671     }
01672   }
01673   else if(ta < tswitch1 && tb > tswitch1) { //PP ramp
01674     //compute bending 
01675     Real vbend = dx0 + tswitch1*a1;
01676     if(vbend < vmin) vmin = vbend;
01677     else if(vbend > vmax) vmax = vbend;
01678     vbend = dx1 + (tswitch2-ttotal)*a2;
01679     if(vbend < vmin) vmin = vbend;
01680     else if(vbend > vmax) vmax = vbend;
01681   }
01682 }
01683 
01684 bool ParabolicRamp1D::IsValid() const
01685 {
01686   if(tswitch1 < 0 || tswitch2 < tswitch1 || ttotal < tswitch2) {
01687     if(gVerbose >= 1)
01688       PARABOLIC_RAMP_PERROR("Ramp has invalid timing %g %g %g\n",tswitch1,tswitch2,ttotal);
01689     return false;
01690   }
01691 
01692   Real t2mT = tswitch2 - ttotal;
01693   if(tswitch1 != tswitch2) {
01694     if(!FuzzyEquals(a1*tswitch1 + dx0,v,EpsilonV)) {
01695       if(gVerbose >= 1)
01696         PARABOLIC_RAMP_PERROR("Ramp has incorrect switch 1 speed: %g vs %g\n",a1*tswitch1 + dx0,v);
01697       return false;
01698     }
01699     if(!FuzzyEquals(a2*t2mT + dx1,v,EpsilonV)) {
01700       if(gVerbose >= 1)
01701         PARABOLIC_RAMP_PERROR("Ramp has incorrect switch 2 speed: %g vs %g\n",a2*t2mT + dx1,v);
01702       return false;
01703     }
01704   }
01705   //check switch2
01706   Real xswitch = x0 + 0.5*a1*Sqr(tswitch1) + dx0*tswitch1;
01707   Real xswitch2 = xswitch + (tswitch2-tswitch1)*v;
01708   if(!FuzzyEquals(xswitch2,x1 + 0.5*a2*Sqr(t2mT) + dx1*t2mT,EpsilonX)) {
01709     if(gVerbose >= 1)
01710       PARABOLIC_RAMP_PERROR("Ramp has incorrect switch 2 position: %g vs %g\n",xswitch2,x1 + 0.5*a2*Sqr(t2mT) + dx1*t2mT);
01711     if(gVerbose >= 2) {
01712       PARABOLIC_RAMP_PERROR("Ramp %g,%g -> %g,%g\n",x0,dx0,x1,dx1);
01713       PARABOLIC_RAMP_PERROR("Acceleration %g, vel %g, deceleration %g\n",a1,v,a2);
01714       PARABOLIC_RAMP_PERROR("Switch times %g %g %g\n",tswitch1,tswitch2,ttotal);
01715     }
01716     return false;
01717   }
01718   return true;
01719 }
01720 
01721 
01722 
01723 
01724 
01725 void ParabolicRampND::SetConstant(const Vector& x,Real t)
01726 {
01727   x0 = x1 = x;
01728   dx0.resize(x.size());
01729   dx1.resize(x.size());
01730   fill(dx0.begin(),dx0.end(),0);
01731   fill(dx1.begin(),dx1.end(),0);
01732   endTime = t;
01733   ramps.resize(x.size());
01734   for(size_t i=0;i<x.size();i++)
01735     ramps[i].SetConstant(x[i],t);
01736 }
01737 
01738 void ParabolicRampND::SetLinear(const Vector& _x0,const Vector& _x1,Real t)
01739 {
01740   PARABOLIC_RAMP_ASSERT(_x0.size() == _x1.size());
01741   PARABOLIC_RAMP_ASSERT(t > 0);
01742   x0 = _x0;
01743   x1 = _x1;
01744   dx0.resize(_x1.size());
01745   for(size_t i=0;i<_x1.size();i++)
01746     dx0[i] = (_x1[i]-_x0[i])/t;
01747   dx1 = dx0;
01748   endTime = t;
01749   ramps.resize(_x0.size());
01750   for(size_t i=0;i<_x0.size();i++)
01751     ramps[i].SetLinear(_x0[i],_x1[i],t);
01752 }
01753 
01754 bool ParabolicRampND::SolveMinTimeLinear(const Vector& amax,const Vector& vmax)
01755 {
01756   PARABOLIC_RAMP_ASSERT(x0.size() == dx0.size());
01757   PARABOLIC_RAMP_ASSERT(x1.size() == dx1.size());
01758   PARABOLIC_RAMP_ASSERT(x0.size() == x1.size());
01759   PARABOLIC_RAMP_ASSERT(x0.size() == amax.size());
01760   PARABOLIC_RAMP_ASSERT(x0.size() == vmax.size());
01761   endTime = 0;
01762   ramps.resize(x0.size());
01763   ParabolicRamp1D sramp;
01764   sramp.x0 = 0;
01765   sramp.x1 = 1;
01766   sramp.dx0 = 0;
01767   sramp.dx1 = 0;
01768   Real svmax=Inf,samax=Inf;
01769   for(size_t i=0;i<ramps.size();i++) {
01770     ramps[i].x0=x0[i];
01771     ramps[i].x1=x1[i];
01772     ramps[i].dx0=dx0[i];
01773     ramps[i].dx1=dx1[i];
01774     if(vmax[i]==0 || amax[i]==0) {
01775       if(!FuzzyEquals(x0[i],x1[i],EpsilonX)) {
01776         if(gVerbose >= 1)
01777           PARABOLIC_RAMP_PERROR("index %d vmax = %g, amax = %g, X0 != X1 (%g != %g)\n",i,vmax[i],amax[i],x0[i],x1[i]);
01778         return false;
01779       }
01780       if(!FuzzyEquals(dx0[i],dx1[i],EpsilonV)) {
01781         if(gVerbose >= 1)
01782           PARABOLIC_RAMP_PERROR("index %d vmax = %g, amax = %g, DX0 != DX1 (%g != %g)\n",i,vmax[i],amax[i],dx0[i],dx1[i]);
01783         return false;
01784       }
01785       ramps[i].tswitch1=ramps[i].tswitch2=ramps[i].ttotal=0;
01786       ramps[i].a1=ramps[i].a1=ramps[i].v=0;
01787       continue;
01788     }
01789     if(vmax[i] < svmax*Abs(x1[i]-x0[i]))
01790       svmax = vmax[i]/Abs(x1[i]-x0[i]);
01791     if(amax[i] < samax*Abs(x1[i]-x0[i]))
01792       samax = amax[i]/Abs(x1[i]-x0[i]);
01793   }
01794 
01795   if(IsInf(svmax) && IsInf(samax)) {
01796     //must have equal start/end state
01797     SetConstant(x0);
01798     return true;
01799   }
01800 
01801   bool res=sramp.SolveMinTime(samax,svmax);
01802   if(!res) {
01803     if(gVerbose >= 1)
01804       PARABOLIC_RAMP_PERROR("Warning in straight-line parameter solve\n");
01805     if(gErrorGetchar) getchar();
01806     return false;
01807   }
01808 
01809   endTime = sramp.ttotal;
01810   for(size_t i=0;i<ramps.size();i++) {
01811     ramps[i].v = svmax * (x1[i]-x0[i]);
01812     ramps[i].a1 = samax * (x1[i]-x0[i]);
01813     ramps[i].a2 = -samax * (x1[i]-x0[i]);
01814     ramps[i].tswitch1 = sramp.tswitch1;
01815     ramps[i].tswitch2 = sramp.tswitch2;
01816     ramps[i].ttotal = endTime;
01817     if(gValidityCheckLevel >= 2) {
01818       if(!ramps[i].IsValid()) {
01819         if(gVerbose >= 1) 
01820           PARABOLIC_RAMP_PERROR("Warning, error in straight-line path formula\n");
01821         if(gVerbose >= 2) {
01822           for(size_t j=0;j<dx0.size();j++)
01823             PARABOLIC_RAMP_PERROR("%g ",dx0[j]);
01824           for(size_t j=0;j<dx1.size();j++)
01825             PARABOLIC_RAMP_PERROR("%g ",dx1[j]);
01826         }
01827         if(gErrorGetchar) getchar();
01828       }
01829     }
01830 
01831     //correct for small numerical errors
01832     if(Abs(ramps[i].v) > vmax[i]) {
01833       if(Abs(ramps[i].v) > vmax[i]+EpsilonV) {
01834         if(gVerbose >= 1) {
01835           PARABOLIC_RAMP_PERROR("Warning, numerical error in straight-line formula?\n"); 
01836           PARABOLIC_RAMP_PERROR("velocity |%g|>%g\n",ramps[i].v,vmax[i]);
01837         }
01838         if(gErrorGetchar) getchar();
01839       }
01840       else ramps[i].v = Sign(ramps[i].v)*vmax[i];
01841     }
01842     if(Abs(ramps[i].a1) > amax[i]) {
01843       if(Abs(ramps[i].a1) > amax[i]+EpsilonA) {
01844         if(gVerbose >= 1) {
01845           PARABOLIC_RAMP_PERROR("Warning, numerical error in straight-line formula?\n");
01846           PARABOLIC_RAMP_PERROR("accel |%g|>%g\n",ramps[i].a1,amax[i]);
01847         }
01848         if(gErrorGetchar) getchar();
01849       }
01850       else ramps[i].a1 = Sign(ramps[i].a1)*amax[i];
01851     }
01852     if(Abs(ramps[i].a2) > amax[i]) {
01853       if(Abs(ramps[i].a2) > amax[i]+EpsilonA) {
01854         if(gVerbose >= 1) {
01855           PARABOLIC_RAMP_PERROR("Warning, numerical error in straight-line formula?\n");
01856           PARABOLIC_RAMP_PERROR("accel |%g|>%g\n",ramps[i].a2,amax[i]);
01857         }
01858         if(gErrorGetchar) getchar();
01859       }
01860       else ramps[i].a2 = Sign(ramps[i].a2)*amax[i];
01861     }
01862   }
01863   return true;
01864 }
01865 
01866 bool ParabolicRampND::SolveMinTime(const Vector& amax,const Vector& vmax)
01867 {
01868   PARABOLIC_RAMP_ASSERT(x0.size() == dx0.size());
01869   PARABOLIC_RAMP_ASSERT(x1.size() == dx1.size());
01870   PARABOLIC_RAMP_ASSERT(x0.size() == x1.size());
01871   PARABOLIC_RAMP_ASSERT(x0.size() == amax.size());
01872   PARABOLIC_RAMP_ASSERT(x0.size() == vmax.size());
01873   endTime = 0;
01874   ramps.resize(x0.size());
01875   for(size_t i=0;i<ramps.size();i++) {
01876     ramps[i].x0=x0[i];
01877     ramps[i].x1=x1[i];
01878     ramps[i].dx0=dx0[i];
01879     ramps[i].dx1=dx1[i];
01880     if(vmax[i]==0 || amax[i]==0) {
01881       if(!FuzzyEquals(x0[i],x1[i],EpsilonX)) {
01882         if(gVerbose >= 1)
01883           PARABOLIC_RAMP_PERROR("index %d vmax = %g, amax = %g, X0 != X1 (%g != %g)\n",i,vmax[i],amax[i],x0[i],x1[i]);
01884         return false;
01885       }
01886       if(!FuzzyEquals(dx0[i],dx1[i],EpsilonV)) {
01887         if(gVerbose >= 1)
01888           PARABOLIC_RAMP_PERROR("index %d vmax = %g, amax = %g, DX0 != DX1 (%g != %g)\n",i,vmax[i],amax[i],dx0[i],dx1[i]);
01889         return false;
01890       }
01891       ramps[i].tswitch1=ramps[i].tswitch2=ramps[i].ttotal=0;
01892       ramps[i].a1=ramps[i].a2=ramps[i].v=0;
01893       continue;
01894     }
01895     if(!ramps[i].SolveMinTime(amax[i],vmax[i])) return false;
01896     if(ramps[i].ttotal > endTime) endTime = ramps[i].ttotal;
01897   }
01898   //now we have a candidate end time -- repeat looking through solutions
01899   //until we have solved all ramps
01900   while(true) {
01901     bool solved = true;
01902     for(size_t i=0;i<ramps.size();i++) {
01903       if(ramps[i].ttotal == endTime) continue;
01904       if(vmax[i]==0 || amax[i]==0) {
01905         ramps[i].ttotal = endTime;
01906         continue;
01907       }
01908       if(!ramps[i].SolveMinAccel(endTime,vmax[i])) {
01909         if(gVerbose >= 1)
01910           PARABOLIC_RAMP_PERROR("Failed solving min accel for joint %d\n",i);
01911         if(gVerbose >= 2) {
01912           ramps[i].SolveMinTime(amax[i],vmax[i]);
01913           PARABOLIC_RAMP_PERROR("its min time is %g\n",ramps[i].ttotal);
01914           if(ramps[i].tswitch1==ramps[i].tswitch2)
01915             PARABOLIC_RAMP_PERROR("its type is PP\n");
01916           else if(Abs(ramps[i].v)==vmax[i])
01917             PARABOLIC_RAMP_PERROR("its type is PLP (vmax)\n");
01918           else 
01919             PARABOLIC_RAMP_PERROR("its type is PLP (v=%g %%)\n",ramps[i].v/vmax[i]);
01920         }
01921         if(gErrorSave) SaveRamp("ParabolicRampND_SolveMinAccel_failure.dat",ramps[i].x0,ramps[i].dx0,ramps[i].x1,ramps[i].dx1,-1,vmax[i],endTime);
01922         if(gErrorSave) {
01923           PARABOLIC_RAMP_PERROR("Saving to failed_ramps.txt\n");
01924           FILE* f=fopen("failed_ramps.txt","w+");
01925           fprintf(f,"MinAccel T=%g, vmax=%g\n",endTime,vmax[i]);
01926           fprintf(f,"x0=%g, dx0=%g\n",ramps[i].x0,ramps[i].dx0);
01927           fprintf(f,"x1=%g, dx1=%g\n",ramps[i].x1,ramps[i].dx1);
01928           fprintf(f,"MinTime solution v=%g, t1=%g, t2=%g, T=%g\n",ramps[i].v,ramps[i].tswitch1,ramps[i].tswitch2,ramps[i].ttotal);
01929           fprintf(f,"\n");
01930           fclose(f);
01931         }
01932         return false;
01933       }
01934       if(Abs(ramps[i].a1) > amax[i] || Abs(ramps[i].a2) > amax[i] || Abs(ramps[i].v) > vmax[i]) {
01935         bool res=ramps[i].SolveMinTime2(amax[i],vmax[i],endTime);
01936         if(!res) {
01937           if(gVerbose >= 1) 
01938             PARABOLIC_RAMP_PERROR("Couldn't solve min-time with lower bound!\n");
01939           if(gErrorGetchar) getchar();
01940           return false;
01941         }
01942         PARABOLIC_RAMP_ASSERT(ramps[i].ttotal > endTime);
01943         endTime = ramps[i].ttotal;
01944         solved = false;
01945         break; //go back and re-solve
01946       }
01947       PARABOLIC_RAMP_ASSERT(Abs(ramps[i].a1) <= amax[i]+EpsilonA);
01948       PARABOLIC_RAMP_ASSERT(Abs(ramps[i].a2) <= amax[i]+EpsilonA);
01949       PARABOLIC_RAMP_ASSERT(Abs(ramps[i].v) <= vmax[i]+EpsilonV);
01950       PARABOLIC_RAMP_ASSERT(ramps[i].ttotal==endTime);
01951     }
01952     //done
01953     if(solved) break;
01954   }
01955   return true;
01956 }
01957 
01958 bool ParabolicRampND::SolveMinAccel(const Vector& vmax,Real time)
01959 {
01960   PARABOLIC_RAMP_ASSERT(x0.size() == dx0.size());
01961   PARABOLIC_RAMP_ASSERT(x1.size() == dx1.size());
01962   PARABOLIC_RAMP_ASSERT(x0.size() == x1.size());
01963   PARABOLIC_RAMP_ASSERT(x0.size() == vmax.size());
01964   endTime = time;
01965   ramps.resize(x0.size());
01966   for(size_t i=0;i<ramps.size();i++) {
01967     ramps[i].x0=x0[i];
01968     ramps[i].x1=x1[i];
01969     ramps[i].dx0=dx0[i];
01970     ramps[i].dx1=dx1[i];
01971     if(vmax[i]==0) {
01972       PARABOLIC_RAMP_ASSERT(FuzzyEquals(x0[i],x1[i],EpsilonX));
01973       PARABOLIC_RAMP_ASSERT(FuzzyEquals(dx0[i],dx1[i],EpsilonV));
01974       ramps[i].tswitch1=ramps[i].tswitch2=ramps[i].ttotal=0;
01975       ramps[i].a1=ramps[i].a2=ramps[i].v=0;
01976       continue;
01977     }
01978     if(!ramps[i].SolveMinAccel(endTime,vmax[i])) {
01979       return false;
01980     }
01981   }
01982   return true;
01983 }
01984 
01985 bool ParabolicRampND::SolveMinAccelLinear(const Vector& vmax,Real time)
01986 {
01987   PARABOLIC_RAMP_ASSERT(x0.size() == dx0.size());
01988   PARABOLIC_RAMP_ASSERT(x1.size() == dx1.size());
01989   PARABOLIC_RAMP_ASSERT(x0.size() == x1.size());
01990   PARABOLIC_RAMP_ASSERT(x0.size() == vmax.size());
01991   endTime = 0;
01992   ramps.resize(x0.size());
01993   ParabolicRamp1D sramp;
01994   sramp.x0 = 0;
01995   sramp.x1 = 1;
01996   sramp.dx0 = 0;
01997   sramp.dx1 = 0;
01998   Real svmax=Inf;
01999   for(size_t i=0;i<ramps.size();i++) {
02000     ramps[i].x0=x0[i];
02001     ramps[i].x1=x1[i];
02002     ramps[i].dx0=dx0[i];
02003     ramps[i].dx1=dx1[i];
02004     if(vmax[i]==0) {
02005       if(!FuzzyEquals(x0[i],x1[i],EpsilonX)) {
02006         if(gVerbose >= 1) 
02007           PARABOLIC_RAMP_PERROR("index %d vmax = %g, X0 != X1 (%g != %g)\n",i,vmax[i],x0[i],x1[i]);
02008         return false;
02009       }
02010       if(!FuzzyEquals(dx0[i],dx1[i],EpsilonV)) {
02011         if(gVerbose >= 1) 
02012           PARABOLIC_RAMP_PERROR("index %d vmax = %g, DX0 != DX1 (%g != %g)\n",i,vmax[i],dx0[i],dx1[i]);
02013         return false;
02014       }
02015       ramps[i].tswitch1=ramps[i].tswitch2=ramps[i].ttotal=0;
02016       ramps[i].a1=ramps[i].a1=ramps[i].v=0;
02017       continue;
02018     }
02019     if(vmax[i] < svmax*Abs(x1[i]-x0[i]))
02020       svmax = vmax[i]/Abs(x1[i]-x0[i]);
02021   }
02022 
02023   if(IsInf(svmax)) {
02024     //must have equal start/end state
02025     SetConstant(x0);
02026     return true;
02027   }
02028 
02029   bool res=sramp.SolveMinAccel(svmax,time);
02030   if(!res) {
02031     if(gVerbose >= 1) 
02032       PARABOLIC_RAMP_PERROR("Warning in straight-line parameter solve\n");
02033     if(gErrorGetchar) getchar();
02034     return false;
02035   }
02036 
02037   endTime = sramp.ttotal;
02038   for(size_t i=0;i<ramps.size();i++) {
02039     ramps[i].v = sramp.v * (x1[i]-x0[i]);
02040     ramps[i].a1 = sramp.a1 * (x1[i]-x0[i]);
02041     ramps[i].a2 = sramp.a2 * (x1[i]-x0[i]);
02042     ramps[i].tswitch1 = sramp.tswitch1;
02043     ramps[i].tswitch2 = sramp.tswitch2;
02044     ramps[i].ttotal = endTime;
02045     if(gValidityCheckLevel >= 2) {
02046       if(!ramps[i].IsValid()) {
02047         if(gVerbose >= 1) 
02048           PARABOLIC_RAMP_PERROR("Warning, error in straight-line path formula\n");
02049         if(gErrorGetchar) getchar();
02050         res=false;
02051       }
02052     }
02053   }
02054   return res;
02055 }
02056 
02057 void ParabolicRampND::SolveBraking(const Vector& amax)
02058 {
02059   PARABOLIC_RAMP_ASSERT(x0.size() == dx0.size());
02060   PARABOLIC_RAMP_ASSERT(x0.size() == amax.size());
02061   x1.resize(x0.size());
02062   dx1.resize(x0.size());
02063   endTime = 0;
02064   ramps.resize(x0.size());
02065   for(size_t i=0;i<ramps.size();i++) {
02066     if(amax[i]==0) {
02067       PARABOLIC_RAMP_ASSERT(FuzzyEquals(dx0[i],0.0,EpsilonV));
02068       ramps[i].SetConstant(0);
02069       continue;
02070     }
02071     ramps[i].x0 = x0[i];
02072     ramps[i].dx0 = dx0[i];
02073     ramps[i].SolveBraking(amax[i]);
02074   }
02075   for(size_t i=0;i<ramps.size();i++)
02076     endTime = Max(endTime,ramps[i].ttotal);
02077   for(size_t i=0;i<ramps.size();i++) {
02078     if(amax[i] != 0 && ramps[i].ttotal != endTime) {
02079       //scale ramp acceleration to meet endTimeMax
02080       ramps[i].ttotal = endTime;
02081       //y(t) = x0 + t*dx0 + 1/2 t^2 a
02082       //y'(T) = dx0 + T a = 0
02083       ramps[i].a2 = -dx0[i] / endTime;
02084       ramps[i].a1 = -ramps[i].a2;
02085       ramps[i].x1 = ramps[i].x0 + endTime*ramps[i].dx0 + 0.5*Sqr(endTime)*ramps[i].a2;
02086     }
02087     x1[i]=ramps[i].x1;
02088     dx1[i]=0;
02089   }
02090   if(gValidityCheckLevel >= 2) 
02091     PARABOLIC_RAMP_ASSERT(IsValid());
02092 }
02093 
02094 void ParabolicRampND::Evaluate(Real t,Vector& x) const
02095 {
02096   x.resize(ramps.size());
02097   for(size_t j=0;j<ramps.size();j++)
02098     x[j]=ramps[j].Evaluate(t);
02099 }
02100 
02101 void ParabolicRampND::Derivative(Real t,Vector& x) const
02102 {
02103   x.resize(ramps.size());
02104   for(size_t j=0;j<ramps.size();j++)
02105     x[j]=ramps[j].Derivative(t);
02106 }
02107 
02108 void ParabolicRampND::Accel(Real t,Vector& x) const
02109 {
02110   x.resize(ramps.size());
02111   for(size_t j=0;j<ramps.size();j++)
02112     x[j]=ramps[j].Accel(t);
02113 }
02114 
02115 void ParabolicRampND::Output(Real dt,std::vector<Vector>& path) const
02116 {
02117   PARABOLIC_RAMP_ASSERT(!ramps.empty());
02118   int size = (int)ceil(endTime/dt)+1;
02119   path.resize(size);
02120   if(size == 1) {
02121     path[0].resize(ramps.size());
02122     for(size_t j=0;j<ramps.size();j++)
02123       path[0][j] = ramps[j].x0;
02124     return;
02125   } 
02126   for(int i=0;i<size;i++) {
02127     Real t=endTime*Real(i)/Real(size-1);
02128     path[i].resize(ramps.size());
02129     for(size_t j=0;j<ramps.size();j++)
02130       path[i][j]=ramps[j].Evaluate(t);
02131   }
02132   
02133   /*
02134   path[0].resize(ramps.size());
02135   for(size_t j=0;j<ramps.size();j++)
02136     path[0][j] = ramps[j].x0;
02137   for(int i=1;i+1<size;i++) {
02138     Real t=endTime*Real(i)/Real(size-1);
02139     path[i].resize(ramps.size());
02140     for(size_t j=0;j<ramps.size();j++)
02141       path[i][j]=ramps[j].Evaluate(t);
02142   }
02143   path[size-1].resize(ramps.size());
02144   for(size_t j=0;j<ramps.size();j++)
02145     path[size-1][j] = ramps[j].x1;
02146   */
02147 }
02148 
02149 
02150 void ParabolicRampND::Dilate(Real timeScale)
02151 {
02152   for(size_t i=0;i<ramps.size();i++)
02153     ramps[i].Dilate(timeScale);
02154 }
02155 
02156 void ParabolicRampND::TrimFront(Real tcut)
02157 {
02158   PARABOLIC_RAMP_ASSERT(tcut <= endTime);
02159   Evaluate(tcut,x0);
02160   Derivative(tcut,dx0);
02161   endTime -= tcut;
02162   for(size_t i=0;i<ramps.size();i++)
02163     ramps[i].TrimFront(tcut);
02164   if(gValidityCheckLevel >= 2) 
02165     PARABOLIC_RAMP_ASSERT(IsValid());
02166 }
02167 
02168 void ParabolicRampND::TrimBack(Real tcut)
02169 {
02170   for(size_t i=0;i<ramps.size();i++)
02171     PARABOLIC_RAMP_ASSERT(endTime == ramps[i].ttotal);
02172   PARABOLIC_RAMP_ASSERT(tcut <= endTime);
02173   Evaluate(endTime-tcut,x1);
02174   Derivative(endTime-tcut,dx1);
02175   endTime -= tcut;
02176   for(size_t i=0;i<ramps.size();i++)
02177     ramps[i].TrimBack(tcut);
02178   if(gValidityCheckLevel >= 2) 
02179     PARABOLIC_RAMP_ASSERT(IsValid());
02180 }
02181 
02182 void ParabolicRampND::Bounds(Vector& xmin,Vector& xmax) const
02183 {
02184   xmin.resize(ramps.size());
02185   xmax.resize(ramps.size());
02186   for(size_t i=0;i<ramps.size();i++) {
02187     ramps[i].Bounds(xmin[i],xmax[i]);
02188   }
02189 }
02190 
02191 void ParabolicRampND::Bounds(Real ta,Real tb,Vector& xmin,Vector& xmax) const
02192 {
02193   xmin.resize(ramps.size());
02194   xmax.resize(ramps.size());
02195   for(size_t i=0;i<ramps.size();i++) {
02196     ramps[i].Bounds(ta,tb,xmin[i],xmax[i]);
02197   }
02198 }
02199 
02200 void ParabolicRampND::DerivBounds(Vector& vmin,Vector& vmax) const
02201 {
02202   vmin.resize(ramps.size());
02203   vmax.resize(ramps.size());
02204   for(size_t i=0;i<ramps.size();i++) {
02205     ramps[i].DerivBounds(vmin[i],vmax[i]);
02206   }
02207 }
02208 
02209 void ParabolicRampND::DerivBounds(Real ta,Real tb,Vector& vmin,Vector& vmax) const
02210 {
02211   vmin.resize(ramps.size());
02212   vmax.resize(ramps.size());
02213   for(size_t i=0;i<ramps.size();i++) {
02214     ramps[i].DerivBounds(ta,tb,vmin[i],vmax[i]);
02215   }
02216 }
02217 
02218 bool ParabolicRampND::IsValid() const
02219 {
02220   if(endTime < 0) {
02221     if(gVerbose >= 1) PARABOLIC_RAMP_PERROR("ParabolicRampND::IsValid(): endTime is negative\n");
02222     return false;
02223   }
02224   for(size_t i=0;i<ramps.size();i++) {
02225     if(!ramps[i].IsValid()) {
02226       if(gVerbose >= 1) PARABOLIC_RAMP_PERROR("ParabolicRampND::IsValid(): element %d is invalid\n",i);
02227       return false;
02228     }
02229     if(!FuzzyEquals(ramps[i].ttotal,endTime,EpsilonT)) {
02230       if(gVerbose >= 1) PARABOLIC_RAMP_PERROR("ParabolicRampND::IsValid(): element %d has different end time %g != %g\n",i,ramps[i].ttotal,endTime);
02231       return false;
02232     }
02233     if(!FuzzyEquals(ramps[i].x0,x0[i],EpsilonX)) {
02234       if(gVerbose >= 1) PARABOLIC_RAMP_PERROR("ParabolicRampND::IsValid(): element %d has different x0 %g != %g\n",i,ramps[i].x0,x0[i]);
02235       return false;
02236     }
02237     if(!FuzzyEquals(ramps[i].x1,x1[i],EpsilonX)) {
02238       if(gVerbose >= 1) PARABOLIC_RAMP_PERROR("ParabolicRampND::IsValid(): element %d has different x1 %g != %g\n",i,ramps[i].x1,x1[i]);
02239       return false;
02240     }
02241     if(!FuzzyEquals(ramps[i].dx0,dx0[i],EpsilonV)) {
02242       if(gVerbose >= 1) PARABOLIC_RAMP_PERROR("ParabolicRampND::IsValid(): element %d has different dx0 %g != %g\n",i,ramps[i].dx0,dx0[i]);
02243       return false;
02244     }
02245     if(!FuzzyEquals(ramps[i].dx1,dx1[i],EpsilonV)) {
02246       if(gVerbose >= 1) PARABOLIC_RAMP_PERROR("ParabolicRampND::IsValid(): element %d has different dx1 %g != %g\n",i,ramps[i].dx1,dx1[i]);
02247       return false;
02248     }
02249   }
02250   return true;
02251 }
02252 
02253 bool SolveMinTimeBounded(Real x0,Real v0,Real x1,Real v1,Real amax,Real vmax,Real xmin,Real xmax,ParabolicRamp1D& ramp)
02254 {
02255   PARABOLIC_RAMP_ASSERT(x0 >= xmin && x0 <= xmax && x1 >= xmin && x1 <= xmax);
02256   ramp.x0 = x0;
02257   ramp.dx0 = v0;
02258   ramp.x1 = x1;
02259   ramp.dx1 = v1;
02260   if(!ramp.SolveMinTime(amax,vmax)) return false;
02261   Real bmin,bmax;
02262   ramp.Bounds(bmin,bmax);
02263   if(bmin < xmin || bmax > xmax) return false;
02264   return true;
02265 }
02266 
02267 inline Real BrakeTime(Real x,Real v,Real xbound)
02268 {
02269   return 2.0*(xbound-x)/v;
02270 }
02271 
02272 inline Real BrakeAccel(Real x,Real v,Real xbound)
02273 {
02274   Real tb=BrakeTime(x,v,xbound);
02275   if(FuzzyEquals(tb,0.0,EpsilonT)) return 0;
02276   return -v/tb;
02277 }
02278 
02279 bool SolveMinAccelBounded(Real x0,Real v0,Real x1,Real v1,Real endTime,Real vmax,Real xmin,Real xmax,std::vector<ParabolicRamp1D>& ramps)
02280 {
02281   PARABOLIC_RAMP_ASSERT(x0 >= xmin && x0 <= xmax && x1 >= xmin && x1 <= xmax);
02282   ParabolicRamp1D ramp;
02283   ramp.x0 = x0;
02284   ramp.dx0 = v0;
02285   ramp.x1 = x1;
02286   ramp.dx1 = v1;
02287   if(!ramp.SolveMinAccel(endTime,vmax)) return false;
02288   Real bmin,bmax;
02289   ramp.Bounds(bmin,bmax);
02290   if(bmin >= xmin && bmax <= xmax) {
02291     ramps.resize(1);
02292     ramps[0] = ramp;
02293     return true;
02294   }
02295 
02296   //not within bounds, do the more complex procedure
02297   ramps.resize(0);
02298   vector<ParabolicRamp1D> temp;
02299   //Look at the IV cases
02300   Real bt0=Inf,bt1=Inf;
02301   Real ba0=Inf,ba1=Inf;
02302   Real bx0=Inf,bx1=Inf;
02303   if(v0 > 0) {
02304     bt0 = BrakeTime(x0,v0,xmax);
02305     bx0 = xmax;
02306     ba0 = BrakeAccel(x0,v0,xmax);
02307   }
02308   else if(v0 < 0) {
02309     bt0 = BrakeTime(x0,v0,xmin);
02310     bx0 = xmin;
02311     ba0 = BrakeAccel(x0,v0,xmin);
02312   }
02313   if(v1 < 0) {
02314     bt1 = BrakeTime(x1,-v1,xmax);
02315     bx1 = xmax;
02316     ba1 = BrakeAccel(x1,-v1,xmax);
02317   }
02318   else if(v1 > 0) {
02319     bt1 = BrakeTime(x1,-v1,xmin);
02320     bx1 = xmin;
02321     ba1 = BrakeAccel(x1,-v1,xmin);
02322   }
02323   Real amax=Inf;
02324   //Explore types II and III, or II and IV depending on the side
02325   //Type I path: no collision
02326   //Type II path: touches one side instantaneously
02327   //   (IIa: first segment is braking, IIb: last segment is braking)
02328   //Type III path: touches one side and remains there for some time
02329   //Type IV path: hits both top and bottom
02330   //consider braking to side, then solving to x1,v1
02331   if(bt0 < endTime && Abs(ba0) < amax) {
02332     //type IIa
02333     temp.resize(2);
02334     temp[0].x0 = x0;
02335     temp[0].dx0 = v0;
02336     temp[0].x1 = bx0;
02337     temp[0].dx1 = 0;
02338     temp[0].a1 = ba0;
02339     temp[0].v = 0;
02340     temp[0].a2 = 0;
02341     temp[0].tswitch1 = bt0;
02342     temp[0].tswitch2 = bt0;
02343     temp[0].ttotal = bt0;
02344     temp[1].x0 = bx0;
02345     temp[1].dx0 = 0;
02346     temp[1].x1 = x1;
02347     temp[1].dx1 = v1;
02348     gMinAccelQuiet = true;
02349     //first check is a quick reject
02350     if(Abs(x1-bx0) < (endTime-bt0)*vmax) {
02351       if(temp[1].SolveMinAccel(endTime-bt0,vmax)) {
02352         if(Max(Abs(temp[1].a1),Abs(temp[1].a2)) < amax) {
02353           temp[1].Bounds(bmin,bmax);
02354           if(bmin >= xmin && bmax <= xmax) {
02355             //got a better path
02356             ramps = temp;
02357             amax = Max(Abs(ba0),Max(Abs(temp[1].a1),Abs(temp[1].a2)));
02358           }
02359         }
02360       }
02361     }
02362     gMinAccelQuiet = false;
02363   }
02364   //consider reverse braking from x1,v1, then solving from x0,v0
02365   //consider braking to side, then solving to x1,v1
02366   if(bt1 < endTime && Abs(ba1) < amax) {
02367     //type IIb
02368     temp.resize(2);
02369     temp[0].x0 = x0;
02370     temp[0].dx0 = v0;
02371     temp[0].x1 = bx1;
02372     temp[0].dx1 = 0;
02373     temp[1].x0 = bx1;
02374     temp[1].dx0 = 0;
02375     temp[1].x1 = x1;
02376     temp[1].dx1 = v1;
02377     temp[1].a1 = ba1;
02378     temp[1].v = 0;
02379     temp[1].a2 = 0;
02380     temp[1].tswitch1 = bt1;
02381     temp[1].tswitch2 = bt1;
02382     temp[1].ttotal = bt1;
02383     gMinAccelQuiet = true;
02384     //first perform a quick reject
02385     if(Abs(x0-bx1) < (endTime-bt1)*vmax) {
02386       if(temp[0].SolveMinAccel(endTime-bt1,vmax)) {
02387         if(Max(Abs(temp[0].a1),Abs(temp[0].a2)) < amax) {
02388           temp[0].Bounds(bmin,bmax);
02389           if(bmin >= xmin && bmax <= xmax) {
02390             //got a better path
02391             ramps = temp;
02392             amax = Max(Abs(ba1),Max(Abs(temp[0].a1),Abs(temp[0].a2)));
02393           }
02394         }
02395       }
02396     }
02397     gMinAccelQuiet = false;
02398   }
02399   if(bx0 == bx1) {
02400     //type III: braking to side, then continuing, then accelerating to x1
02401     if(bt0 + bt1 < endTime && Max(Abs(ba0),Abs(ba1)) < amax) {
02402       temp.resize(1);
02403       temp[0].x0 = x0;
02404       temp[0].dx0 = v0;
02405       temp[0].x1 = x1;
02406       temp[0].dx1 = v1;
02407       temp[0].a1 = ba0;
02408       temp[0].v = 0;
02409       temp[0].a2 = ba1;
02410       temp[0].tswitch1 = bt0;
02411       temp[0].tswitch2 = endTime-bt1;
02412       temp[0].ttotal = endTime;
02413       ramps = temp;
02414       amax = Max(Abs(ba0),Abs(ba1));
02415       if(gValidityCheckLevel >= 2) 
02416         PARABOLIC_RAMP_ASSERT(temp[0].IsValid());
02417     }
02418   }
02419   else {
02420     //type IV paths
02421     if(bt0 + bt1 < endTime && Max(Abs(ba0),Abs(ba1)) < amax) {
02422       //first segment brakes to one side, last segment brakes to the other
02423       //first
02424       temp.resize(3);
02425       temp[0].x0 = x0;
02426       temp[0].dx0 = v0;
02427       temp[0].x1 = bx0;
02428       temp[0].dx1 = 0;
02429       temp[0].a1 = ba0;
02430       temp[0].v = 0;
02431       temp[0].a2 = 0;
02432       temp[0].tswitch1 = bt0;
02433       temp[0].tswitch2 = bt0;
02434       temp[0].ttotal = bt0;
02435       //last
02436       temp[2].x0 = bx1;
02437       temp[2].dx0 = 0;
02438       temp[2].x1 = x1;
02439       temp[2].dx1 = v1;
02440       temp[2].a1 = ba1;
02441       temp[2].v = 0;
02442       temp[2].a2 = 0;
02443       temp[2].tswitch1 = bt1;
02444       temp[2].tswitch2 = bt1;
02445       temp[2].ttotal = bt1;
02446       //middle section
02447       temp[1].x0 = bx0;
02448       temp[1].dx0 = 0;
02449       temp[1].x1 = bx1;
02450       temp[1].dx1 = 0;
02451       gMinAccelQuiet = true;
02452       if(Abs(bx0-bx1) < (endTime-bt0-bt1)*vmax) {
02453         if(temp[1].SolveMinAccel(endTime - bt0 - bt1,vmax)) {
02454           temp[1].Bounds(bmin,bmax);
02455           PARABOLIC_RAMP_ASSERT(bmin >= xmin && bmax <= xmax);
02456           if(Max(Abs(temp[1].a1),Abs(temp[1].a2)) < amax) {
02457             ramps = temp;
02458             amax = Max(Max(Abs(temp[1].a1),Abs(temp[1].a2)),Max(Abs(ba0),Abs(ba1)));
02459           }
02460         }
02461       }
02462       gMinAccelQuiet = false;
02463     }
02464   }
02465   if(ramps.empty()) {
02466     if(gVerbose >= 1) PARABOLIC_RAMP_PERROR("SolveMinAccelBounded: Warning, can't find bounded trajectory?\n");
02467     if(gVerbose >= 2) {
02468       PARABOLIC_RAMP_PERROR("x0 %g v0 %g, x1 %g v1 %g\n",x0,v0,x1,v1);
02469       PARABOLIC_RAMP_PERROR("endTime %g, vmax %g\n",endTime,vmax);
02470       PARABOLIC_RAMP_PERROR("x bounds [%g,%g]\n",xmin,xmax);
02471     }
02472     if(gErrorGetchar) getchar();
02473     return false;
02474   }
02475   if(gValidityCheckLevel >= 1) {
02476     for(size_t i=0;i<ramps.size();i++) {
02477       ramps[i].Bounds(bmin,bmax);
02478       if(bmin < xmin-EpsilonX || bmax > xmax+EpsilonX) {
02479         if(gVerbose >= 1) 
02480           PARABOLIC_RAMP_PERROR("SolveMinAccelBounded: Warning, path exceeds bounds?\n");
02481         if(gVerbose >= 2) 
02482           PARABOLIC_RAMP_PERROR("  ramp[%d] bounds %g %g, limits %g %g\n",i,bmin,bmax,xmin,xmax);
02483         if(gErrorGetchar) getchar();
02484         return false;
02485       }
02486     }
02487   }
02488 
02489   PARABOLIC_RAMP_ASSERT(ramps.front().x0 == x0);
02490   PARABOLIC_RAMP_ASSERT(ramps.front().dx0 == v0);
02491   PARABOLIC_RAMP_ASSERT(ramps.back().x1 == x1);
02492   PARABOLIC_RAMP_ASSERT(ramps.back().dx1 == v1);
02493   double ttotal = 0;
02494   for(size_t i=0;i<ramps.size();i++)
02495     ttotal += ramps[i].ttotal;
02496   if(!FuzzyEquals(ttotal,endTime,EpsilonT*0.1)) {
02497     if(gVerbose >= 1) PARABOLIC_RAMP_PERROR("SolveMinTimeBounded: Numerical timing error");
02498     if(gVerbose >= 2) {
02499       PARABOLIC_RAMP_PERROR("Ramp times: ");
02500       for(size_t i=0;i<ramps.size();i++)
02501         PARABOLIC_RAMP_PERROR("%g ",ramps[i].ttotal);
02502       PARABOLIC_RAMP_PERROR("\n");
02503     }
02504   }
02505   PARABOLIC_RAMP_ASSERT(FuzzyEquals(ttotal,endTime,EpsilonT*0.1));
02506   return true;  
02507 }
02508 
02509 Real SolveMinTimeBounded(const Vector& x0,const Vector& v0,const Vector& x1,const Vector& v1,
02510                          const Vector& amax,const Vector& vmax,const Vector& xmin,const Vector& xmax,
02511                          vector<vector<ParabolicRamp1D> >& ramps)
02512 {
02513   PARABOLIC_RAMP_ASSERT(x0.size() == v0.size());
02514   PARABOLIC_RAMP_ASSERT(x1.size() == v1.size());
02515   PARABOLIC_RAMP_ASSERT(x0.size() == x1.size());
02516   PARABOLIC_RAMP_ASSERT(x0.size() == amax.size());
02517   PARABOLIC_RAMP_ASSERT(x0.size() == vmax.size());
02518   for(size_t i=0;i<x0.size();i++) {
02519     PARABOLIC_RAMP_ASSERT(x0[i] >= xmin[i] && x0[i] <= xmax[i]);
02520     PARABOLIC_RAMP_ASSERT(x1[i] >= xmin[i] && x1[i] <= xmax[i]);
02521     PARABOLIC_RAMP_ASSERT(Abs(v0[i]) <= vmax[i]);
02522     PARABOLIC_RAMP_ASSERT(Abs(v1[i]) <= vmax[i]);
02523   }
02524   Real endTime = 0;
02525   ramps.resize(x0.size());
02526   for(size_t i=0;i<ramps.size();i++) {
02527     ramps[i].resize(1);
02528     ramps[i][0].x0=x0[i];
02529     ramps[i][0].x1=x1[i];
02530     ramps[i][0].dx0=v0[i];
02531     ramps[i][0].dx1=v1[i];
02532     if(vmax[i]==0 || amax[i]==0) {
02533       if(!FuzzyEquals(x0[i],x1[i],EpsilonX)) {
02534         if(gVerbose >= 1)
02535           PARABOLIC_RAMP_PERROR("index %d vmax = %g, amax = %g, X0 != X1 (%g != %g)\n",i,vmax[i],amax[i],x0[i],x1[i]);
02536         return -1;
02537       }
02538       if(!FuzzyEquals(v0[i],v1[i],EpsilonV)) {
02539         if(gVerbose >= 1) 
02540           PARABOLIC_RAMP_PERROR("index %d vmax = %g, amax = %g, DX0 != DX1 (%g != %g)\n",i,vmax[i],amax[i],v0[i],v1[i]);
02541         return -1;
02542       }
02543       ramps[i][0].tswitch1=ramps[i][0].tswitch2=ramps[i][0].ttotal=0;
02544       ramps[i][0].a1=ramps[i][0].a2=ramps[i][0].v=0;
02545       continue;
02546     }
02547     if(!ramps[i][0].SolveMinTime(amax[i],vmax[i])) return -1;
02548     Real bmin,bmax;
02549     ramps[i][0].Bounds(bmin,bmax);
02550     if(bmin < xmin[i] || bmax > xmax[i]) return -1;
02551     if(ramps[i][0].ttotal > endTime) endTime = ramps[i][0].ttotal;
02552   }
02553   //now we have a candidate end time -- repeat looking through solutions
02554   //until we have solved all ramps
02555   while(true) {
02556     bool solved = true;
02557     for(size_t i=0;i<ramps.size();i++) {
02558       PARABOLIC_RAMP_ASSERT(ramps[i].size() > 0);
02559       if(vmax[i]==0 || amax[i]==0) {
02560         ramps[i][0].ttotal = endTime;
02561         continue;
02562       }
02563       //already at maximum
02564       Real ttotal = 0;
02565       for(size_t j=0;j<ramps[i].size();j++)
02566         ttotal += ramps[i][j].ttotal;
02567       if(FuzzyEquals(ttotal,endTime,EpsilonT)) continue;
02568          
02569       //now solve minimum acceleration within bounds
02570       if(!SolveMinAccelBounded(x0[i],v0[i],x1[i],v1[i],endTime,vmax[i],xmin[i],xmax[i],ramps[i])) {
02571         if(gVerbose >= 1) 
02572           PARABOLIC_RAMP_PERROR("Failed solving bounded min accel for joint %d\n",i);
02573         return -1;
02574       }
02575       //now check accel/velocity bounds
02576       bool inVelBounds = true;
02577       for(size_t j=0;j<ramps[i].size();j++)
02578         if(Abs(ramps[i][j].a1) > amax[i]+EpsilonA || Abs(ramps[i][j].a2) > amax[i]+EpsilonA || Abs(ramps[i][j].v) > vmax[i]+EpsilonV) {
02579           //printf("Ramp %d entry %d accels: %g %g, vel %g\n",i,j,ramps[i][j].a1,ramps[i][j].a2,ramps[i][j].v);
02580           inVelBounds = false;
02581           break;
02582         }
02583       if(!inVelBounds) {
02584         ramps[i].resize(1);
02585         ramps[i][0].x0=x0[i];
02586         ramps[i][0].x1=x1[i];
02587         ramps[i][0].dx0=v0[i];
02588         ramps[i][0].dx1=v1[i];
02589         gMinTime2Quiet = true;
02590         bool res=ramps[i][0].SolveMinTime2(amax[i],vmax[i],endTime);
02591         gMinTime2Quiet = false;
02592         if(!res) {
02593           return -1;
02594         }
02595         Real bmin,bmax;
02596         ramps[i][0].Bounds(bmin,bmax);
02597         if(bmin < xmin[i] || bmax > xmax[i]) {
02598           //printf("Couldn't solve min-time with lower bound while staying in bounds\n");
02599           //getchar();
02600           return -1;
02601         }
02602 
02603         //revise total time
02604         ttotal = 0;
02605         for(size_t j=0;j<ramps[i].size();j++)
02606           ttotal += ramps[i][j].ttotal;
02607         PARABOLIC_RAMP_ASSERT(ttotal > endTime);
02608         endTime = ttotal;
02609         solved = false;
02610         break; //go back and re-solve
02611       }
02612       ttotal = 0;
02613       for(size_t j=0;j<ramps[i].size();j++) {
02614         PARABOLIC_RAMP_ASSERT(Abs(ramps[i][j].a1) <= amax[i]+EpsilonA);
02615         PARABOLIC_RAMP_ASSERT(Abs(ramps[i][j].a2) <= amax[i]+EpsilonA);
02616         PARABOLIC_RAMP_ASSERT(Abs(ramps[i][j].v) <= vmax[i]+EpsilonV);
02617         ttotal += ramps[i][j].ttotal;
02618       }
02619       PARABOLIC_RAMP_ASSERT(FuzzyEquals(ttotal,endTime,EpsilonT*0.1));
02620     }
02621     //done
02622     if(solved) break;
02623   }
02624   return endTime;
02625 }
02626 
02627 bool SolveMinAccelBounded(const Vector& x0,const Vector& v0,const Vector& x1,const Vector& v1,
02628                          Real endTime,const Vector& vmax,const Vector& xmin,const Vector& xmax,
02629                          vector<vector<ParabolicRamp1D> >& ramps)
02630 {
02631   PARABOLIC_RAMP_ASSERT(x0.size() == v0.size());
02632   PARABOLIC_RAMP_ASSERT(x1.size() == v1.size());
02633   PARABOLIC_RAMP_ASSERT(x0.size() == x1.size());
02634   PARABOLIC_RAMP_ASSERT(x0.size() == vmax.size());
02635   for(size_t i=0;i<x0.size();i++) {
02636     PARABOLIC_RAMP_ASSERT(x0[i] >= xmin[i] && x0[i] <= xmax[i]);
02637     PARABOLIC_RAMP_ASSERT(x1[i] >= xmin[i] && x1[i] <= xmax[i]);
02638     PARABOLIC_RAMP_ASSERT(Abs(v0[i]) <= vmax[i]);
02639     PARABOLIC_RAMP_ASSERT(Abs(v1[i]) <= vmax[i]);
02640   }
02641   for(size_t i=0;i<ramps.size();i++) {
02642     if(vmax[i]==0) {
02643       ramps[i].resize(1);
02644       ramps[i][0].x0=x0[i];
02645       ramps[i][0].x1=x1[i];
02646       ramps[i][0].dx0=v0[i];
02647       ramps[i][0].dx1=v1[i];
02648       ramps[i][0].ttotal = endTime;
02649       continue;
02650     }
02651     //now solve minimum acceleration within bounds
02652     if(!SolveMinAccelBounded(x0[i],v0[i],x1[i],v1[i],endTime,vmax[i],xmin[i],xmax[i],ramps[i])) {
02653       if(gVerbose >= 1) 
02654         PARABOLIC_RAMP_PERROR("Failed solving bounded min accel for joint %d\n",i);
02655       return false;
02656     }
02657   }
02658   return true;
02659 }
02660 
02661 void CombineRamps(const std::vector<std::vector<ParabolicRamp1D> >& ramps,std::vector<ParabolicRampND>& ndramps)
02662 {
02663   ndramps.resize(0);
02664   vector<vector<ParabolicRamp1D>::const_iterator> indices(ramps.size());
02665   for(size_t i=0;i<ramps.size();i++) {
02666     PARABOLIC_RAMP_ASSERT(!ramps[i].empty());
02667     indices[i] = ramps[i].begin();
02668   }
02669   vector<double> timeOffsets(ramps.size(),0);  //start time of current index
02670   Real t=0;
02671   while(true) {
02672     //pick next ramp
02673     Real tnext=Inf;
02674     for(size_t i=0;i<ramps.size();i++) {
02675       if(indices[i] != ramps[i].end()) 
02676         tnext = Min(tnext,timeOffsets[i]+indices[i]->ttotal);
02677     }
02678     if(IsInf(tnext)) break; //done
02679     if(! (tnext > t || t == 0)) {
02680       if(gVerbose >= 1) 
02681         PARABOLIC_RAMP_PERROR("CombineRamps: error finding next time step?\n");
02682       if(gVerbose >= 2) {
02683         PARABOLIC_RAMP_PERROR("tnext = %g, t = %g, step = %d\n",tnext,t,ndramps.size());
02684         for(size_t k=0;k<ramps.size();k++) {
02685           PARABOLIC_RAMP_PERROR("Ramp %d times: ",k);
02686           Real ttotal = 0.0;
02687           for(size_t j=0;j<ramps[k].size();j++) {
02688             PARABOLIC_RAMP_PERROR("%g ",ramps[k][j].ttotal);
02689             ttotal += ramps[k][j].ttotal;
02690           }
02691           PARABOLIC_RAMP_PERROR(", total %g\n",ttotal);
02692         }
02693       }
02694     }
02695     PARABOLIC_RAMP_ASSERT(tnext > t || t == 0);
02696     if(tnext == 0) {
02697       for(size_t i=0;i<ramps.size();i++) 
02698         PARABOLIC_RAMP_ASSERT(ramps[i].size()==1);
02699     }
02700 
02701     ndramps.resize(ndramps.size()+1);
02702     ParabolicRampND& ramp=ndramps.back();
02703     ramp.x0.resize(ramps.size());
02704     ramp.x1.resize(ramps.size());
02705     ramp.dx0.resize(ramps.size());
02706     ramp.dx1.resize(ramps.size());
02707     ramp.ramps.resize(ramps.size());
02708     ramp.endTime = tnext-t;
02709     for(size_t i=0;i<ramps.size();i++) {
02710       if(indices[i] != ramps[i].end()) {
02711         ParabolicRamp1D iramp = *indices[i];
02712         if(indices[i] == --ramps[i].end() && FuzzyEquals(tnext-timeOffsets[i],indices[i]->ttotal,EpsilonT*0.1)) {
02713           //don't trim back
02714         }
02715         else {
02716           iramp.TrimBack((timeOffsets[i]-tnext)+indices[i]->ttotal);
02717         }
02718         iramp.TrimFront(t-timeOffsets[i]);
02719         Real oldTotal = iramp.ttotal;
02720         iramp.ttotal = ramp.endTime;
02721         if(iramp.tswitch1 > iramp.ttotal) iramp.tswitch1 = iramp.ttotal;
02722         if(iramp.tswitch2 > iramp.ttotal) iramp.tswitch2 = iramp.ttotal;
02723         if(gValidityCheckLevel >= 2) {
02724           if(!iramp.IsValid()) {
02725             if(gVerbose >= 1) {
02726               PARABOLIC_RAMP_PERROR("CombineRamps: Trimming caused ramp to become invalid\n");
02727               PARABOLIC_RAMP_PERROR("Old total time %g, new total time %g\n",oldTotal,iramp.ttotal);
02728             }
02729           }
02730           PARABOLIC_RAMP_ASSERT(iramp.IsValid());
02731         }
02732         ramp.ramps[i] = iramp;
02733         ramp.x0[i] = iramp.x0;
02734         ramp.dx0[i] = iramp.dx0;
02735         ramp.x1[i] = iramp.x1;
02736         ramp.dx1[i] = iramp.dx1;
02737         if(FuzzyEquals(tnext,timeOffsets[i]+indices[i]->ttotal,EpsilonT*0.1)) {
02738           timeOffsets[i] = tnext;
02739           indices[i]++;
02740         }
02741         PARABOLIC_RAMP_ASSERT(ramp.ramps[i].ttotal == ramp.endTime);
02742       }
02743       else {
02744         //after the last segment, propagate a constant off the last ramp
02745         PARABOLIC_RAMP_ASSERT(!ramps[i].empty());
02746         ramp.x0[i] = ramps[i].back().x1;
02747         ramp.dx0[i] = ramps[i].back().dx1;
02748         //ramp.x1[i] = ramps[i].back().x1+ramps[i].back().dx1*(tnext-t);
02749         ramp.x1[i] = ramps[i].back().x1;
02750         ramp.dx1[i] = ramps[i].back().dx1;
02751         if(!FuzzyEquals(ramps[i].back().dx1*(tnext-t),0.0,EpsilonV)) {
02752           if(gVerbose >= 1) 
02753             PARABOLIC_RAMP_PERROR("CombineRamps: warning, propagating time %g distance %g off the back, vel %g\n",(tnext-t),ramps[i].back().dx1*(tnext-t),ramp.dx0[i]);
02754           if(gVerbose >= 2) {
02755             for(size_t k=0;k<ramps.size();k++) {
02756               PARABOLIC_RAMP_PERROR("Ramp %d times: ",k);
02757               Real ttotal = 0.0;
02758               for(size_t j=0;j<ramps[k].size();j++) {
02759                 PARABOLIC_RAMP_PERROR("%g ",ramps[k][j].ttotal);
02760                 ttotal += ramps[k][j].ttotal;
02761               }
02762               PARABOLIC_RAMP_PERROR(", total %g\n",ttotal);
02763             }
02764             if(gErrorGetchar) getchar();
02765           }
02766         }
02767         //set the 1D ramp manually
02768         ramp.ramps[i].x0 = ramp.x0[i];
02769         ramp.ramps[i].dx0 = ramp.dx0[i];
02770         ramp.ramps[i].x1 = ramp.x1[i];
02771         ramp.ramps[i].dx1 = ramp.dx1[i];
02772         ramp.ramps[i].ttotal = ramp.ramps[i].tswitch2 = (tnext-t);
02773         ramp.ramps[i].tswitch1 = 0;
02774         ramp.ramps[i].v = ramp.dx1[i];
02775         ramp.ramps[i].a1 = ramp.ramps[i].a2 = 0;
02776         if(gValidityCheckLevel >= 2) {
02777           PARABOLIC_RAMP_ASSERT(ramp.ramps[i].IsValid());
02778           PARABOLIC_RAMP_ASSERT(ramp.ramps[i].ttotal == ramp.endTime);
02779         }
02780       }
02781     }
02782     if(gValidityCheckLevel >= 2) 
02783       PARABOLIC_RAMP_ASSERT(ramp.IsValid());
02784     if(ndramps.size() > 1) { //fix up endpoints
02785       ramp.x0 = ndramps[ndramps.size()-2].x1;
02786       ramp.dx0 = ndramps[ndramps.size()-2].dx1;
02787       for(size_t i=0;i<ramp.ramps.size();i++) {
02788         ramp.ramps[i].x0=ramp.x0[i];
02789         ramp.ramps[i].dx0=ramp.dx0[i];
02790       }
02791     }
02792     if(gValidityCheckLevel >= 2) 
02793       PARABOLIC_RAMP_ASSERT(ramp.IsValid());
02794 
02795     t = tnext;
02796     if(tnext == 0) //all null ramps
02797       break;
02798   }
02799   for(size_t i=0;i<ramps.size();i++) {
02800     if(!FuzzyEquals(ramps[i].front().x0,ndramps.front().x0[i],EpsilonX)) {
02801       PARABOLIC_RAMP_PERROR("CombineRamps: Error: %d start %g != %g\n",i,ramps[i].front().x0,ndramps.front().x0[i]);
02802       if(gErrorGetchar) getchar();
02803     }
02804     if(!FuzzyEquals(ramps[i].front().dx0,ndramps.front().dx0[i],EpsilonV)) {
02805       PARABOLIC_RAMP_PERROR("CombineRamps: Error: %d start %g != %g\n",i,ramps[i].front().dx0,ndramps.front().dx0[i]);
02806       if(gErrorGetchar) getchar();
02807     }
02808     if(!FuzzyEquals(ramps[i].back().x1,ndramps.back().x1[i],EpsilonX)) {
02809       PARABOLIC_RAMP_PERROR("CombineRamps: Error: %d back %g != %g\n",i,ramps[i].back().x1,ndramps.back().x1[i]);
02810       if(gErrorGetchar) getchar();
02811     }
02812     if(!FuzzyEquals(ramps[i].back().dx1,ndramps.back().dx1[i],EpsilonV)) {
02813       PARABOLIC_RAMP_PERROR("CombineRamps: Error: %d back %g != %g\n",i,ramps[i].back().dx1,ndramps.back().dx1[i]);
02814       if(gErrorGetchar) getchar();
02815     }
02816     ndramps.front().x0[i] = ndramps.front().ramps[i].x0 = ramps[i].front().x0;
02817     ndramps.front().dx0[i] = ndramps.front().ramps[i].dx0 = ramps[i].front().dx0;
02818     ndramps.back().x1[i] = ndramps.back().ramps[i].x1 = ramps[i].back().x1;
02819     ndramps.back().dx1[i] = ndramps.back().ramps[i].dx1 = ramps[i].back().dx1;
02820   }
02821 }
02822 
02823 
02824 } //namespace ParabolicRamp


constraint_aware_spline_smoother
Author(s): Sachin Chitta
autogenerated on Mon Dec 2 2013 12:37:23