constraint_aware_spline_smoother: constraint_aware_spline_smoother: ParabolicRamp.cpp Source File

00001 /*****************************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  * Copyright (c) 2009, the Trustees of Indiana University
00005  * All rights reserved.
00006  *
00007  * Redistribution and use in source and binary forms, with or without
00008  * modification, are permitted provided that the following conditions are met:
00009  *     * Redistributions of source code must retain the above copyright
00010  *       notice, this list of conditions and the following disclaimer.
00011  *     * Redistributions in binary form must reproduce the above copyright
00012  *       notice, this list of conditions and the following disclaimer in the
00013  *       documentation and/or other materials provided with the distribution.
00014  *     * Neither the name of Indiana University nor the
00015  *       names of its contributors may be used to endorse or promote products
00016  *       derived from this software without specific prior written permission.
00017 
00018  * THIS SOFTWARE IS PROVIDED BY THE TRUSTEES OF INDIANA UNIVERSITY ''AS IS''
00019  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00020  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00021  * ARE DISCLAIMED. IN NO EVENT SHALL THE TRUSTEES OF INDIANA UNIVERSITY BE
00022  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00023  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00024  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00025  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00026  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00027  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
00028  * THE POSSIBILITY OF SUCH DAMAGE.
00029  * 
00030  ***************************************************************************/
00031 
00032 #include "constraint_aware_spline_smoother/ParabolicRamp.h"
00033 #include <assert.h>
00034 #include <iostream>
00035 using namespace std;
00036 
00037 //tolerance for time
00038 const static Real EpsilonT = 1e-6;
00039 //tolerance for position
00040 const static Real EpsilonX = 1e-6;
00041 //tolerance for velocity
00042 const static Real EpsilonV = 1e-6;
00043 //tolerance for acceleration
00044 const static Real EpsilonA = 1e-6;
00045 
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 class ParabolicRamp
00089 {
00090  public:
00091   Real Evaluate(Real t) const;
00092   Real Derivative(Real t) const;
00093   Real Accel(Real t) const;
00094   bool Solve();
00095   Real MaxVelocity() const;
00096 
00097   //input
00098   Real x0,dx0;
00099   Real x1,dx1;
00100 
00101   //calculated
00102   Real a;
00103   Real ttotal;
00104 };
00105 
00106 
00107 class PPRamp
00108 {
00109  public:
00110   Real Evaluate(Real t) const;
00111   Real Derivative(Real t) const;
00112   Real Accel(Real t) const;
00113   bool SolveMinTime(Real amax);
00114   bool SolveMinAccel(Real endTime);
00115   Real MaxVelocity() const;
00116 
00117   Real CalcTotalTime(Real a) const;
00118   Real CalcSwitchTime(Real a) const;
00119   Real CalcMinAccel(Real endTime,Real sign,Real& switchTime) const;
00120 
00121   //input
00122   Real x0,dx0;
00123   Real x1,dx1;
00124 
00125   //calculated
00126   Real a;
00127   Real tswitch,ttotal;
00128 };
00129 
00130 class PLPRamp
00131 {
00132  public:
00133   Real Evaluate(Real t) const;
00134   Real Derivative(Real t) const;
00135   Real Accel(Real t) const;
00136   bool SolveMinTime(Real amax,Real vmax);
00137   bool SolveMinAccel(Real endTime,Real vmax);
00138 
00139   Real CalcTotalTime(Real a,Real v) const;
00140   Real CalcSwitchTime1(Real a,Real v) const;
00141   Real CalcSwitchTime2(Real a,Real v) const;
00142   Real CalcMinAccel(Real endTime,Real v) const;
00143 
00144   //input
00145   Real x0,dx0;
00146   Real x1,dx1;
00147 
00148   //calculated
00149   Real a,v;
00150   Real tswitch1,tswitch2,ttotal;
00151 };
00152 
00153 
00154 
00155 Real ParabolicRamp::Evaluate(Real t) const
00156 {
00157   return x0 + t*dx0 + 0.5*a*Sqr(t);
00158 }
00159 
00160 Real ParabolicRamp::Derivative(Real t) const
00161 {
00162   return dx0 + a*t;
00163 }
00164 
00165 Real ParabolicRamp::Accel(Real t) const
00166 {
00167   return a;
00168 }
00169 
00170 bool ParabolicRamp::Solve()
00171 {
00172   a = 0.5*(Sqr(dx0)-Sqr(dx1))/(x0-x1);
00173   ttotal = (dx1-dx0)/a;
00174   if(ttotal < 0) {
00175     ttotal = -1;
00176     a=0;
00177     return false;
00178   }
00179   return true;
00180 }
00181 
00182 Real ParabolicRamp::MaxVelocity() const
00183 {
00184   if(fabs(dx0) > fabs(dx1)) return dx0;
00185   else return dx1;
00186 }
00187 
00188 Real PPRamp::Evaluate(Real t) const
00189 {
00190   if(t < tswitch) return x0 + 0.5*a*t*t + dx0*t;
00191   else {
00192     Real tmT = t - ttotal;
00193     return x1 - 0.5*a*tmT*tmT + dx1*tmT;
00194   }
00195 }
00196 
00197 Real PPRamp::Derivative(Real t) const
00198 {
00199   if(t < tswitch) return a*t + dx0;
00200   else {
00201     Real tmT = t - ttotal;
00202     return -a*tmT + dx1;
00203   }
00204 }
00205 
00206 Real PPRamp::Accel(Real t) const
00207 {
00208   if(t < tswitch) return a;
00209   else return -a;
00210 }
00211 
00212 bool PPRamp::SolveMinTime(Real amax)
00213 {
00214   Real tpn = CalcTotalTime(amax), tnp = CalcTotalTime(-amax);
00215   //cout<<"Time for parabola +-: "<<tpn<<", parabola -+: "<<tnp<<endl;
00216   if(tpn >= 0) {
00217     if(tnp >= 0 && tnp < tpn) {
00218       a = -amax;
00219       ttotal = tnp;
00220     }
00221     else {
00222       a = amax;
00223       ttotal = tpn;
00224     }
00225   }
00226   else if(tnp >= 0) {
00227     a = -amax;
00228     ttotal = tnp;
00229   }
00230   else {
00231     tswitch = -1;
00232     ttotal = -1;
00233     a = 0;
00234     return false;
00235   }
00236   tswitch = CalcSwitchTime(a);
00237   /* //uncomment for additional debugging
00238   if(!FuzzyEquals(x0 + 0.5*a*Sqr(tswitch) + dx0*tswitch,x1 - 0.5*a*Sqr(tswitch-ttotal) + dx1*(tswitch-ttotal),EpsilonX)) {
00239     printf("Error computing parabola min-time...\n");
00240     printf("x0=%g,%g, x1=%g,%g\n",x0,dx0,x1,dx1);
00241     printf("a = %g, ttotal = %g, tswitch = %g\n",a,tswitch,ttotal);
00242     printf("Forward %g, backward %g\n",x0 + 0.5*a*Sqr(tswitch) + dx0*tswitch,x1 - 0.5*a*Sqr(tswitch-ttotal) + dx1*(tswitch-ttotal));
00243     getchar();
00244     return false;
00245   }
00246   assert(FuzzyEquals(x0 + 0.5*a*Sqr(tswitch) + dx0*tswitch,x1 - 0.5*a*Sqr(tswitch-ttotal) + dx1*(tswitch-ttotal),EpsilonX));
00247   */
00248   return true;
00249 }
00250 
00251 bool PPRamp::SolveMinAccel(Real endTime)
00252 {
00253   Real switch1,switch2;
00254   Real apn = CalcMinAccel(endTime,1.0,switch1);
00255   Real anp = CalcMinAccel(endTime,-1.0,switch2);
00256   //cout<<"Accel for parabola +-: "<<apn<<", parabola -+: "<<anp<<endl;
00257   if(apn >= 0) {
00258     if(anp >= 0 && anp < apn)  a = -anp;
00259     else a = apn;
00260   }
00261   else if(anp >= 0) a = -anp;
00262   else {
00263     a=0;
00264     tswitch = -1;
00265     ttotal = -1;
00266     return false;
00267   }
00268   ttotal = endTime;
00269   if(a == apn) 
00270     tswitch = switch1;
00271   else
00272     tswitch = switch2;
00273   return true;
00274 }
00275 
00276 Real PPRamp::MaxVelocity() const
00277 {
00278   return tswitch*a+dx0;
00279 }
00280 
00281 Real PPRamp::CalcTotalTime(Real a) const
00282 {
00283   Real tswitch = CalcSwitchTime(a);
00284   //printf("a = %g, switch time %g\n",a,tswitch);
00285   if(tswitch < 0) return -1;
00286   if(tswitch < (dx1-dx0)/a) return -1;
00287   return tswitch*2.0 - (dx1-dx0)/a;
00288 }
00289 
00290 Real PPRamp::CalcSwitchTime(Real a) const
00291 {
00292   Real b = 2.0*dx0; //2.0*(dx0-dx1);
00293   Real c = (Sqr(dx0)-Sqr(dx1))/(2.0*a)+x0-x1;
00294   Real t1,t2;
00295   int res=quadratic(a,b,c,t1,t2);
00296   if(res == 0) {
00297     return -1;
00298   }
00299   else if(res == 2) {
00300     if(t1 < 0 && t1 > -EpsilonT) t1=0;
00301     if(t2 < 0 && t2 > -EpsilonT) t2=0;
00302     if(t1 < 0) return t2;
00303     else if(t2 < 0) return t1;
00304     else {  //check ttotal
00305       if(t2*Abs(a) < (dx1-dx0)*Sign(a)) return t1;
00306       else if(t1*Abs(a) < (dx1-dx0)*Sign(a)) return t2;
00307       else return Min(t1,t2); //both are ok
00308     }
00309   }
00310   else return t1;
00311 }
00312 
00313 Real PPRamp::CalcMinAccel(Real endTime,Real sign,Real& switchTime) const
00314 {
00315   Real a=Sqr(endTime);
00316   Real b=sign*(2.0*(dx0+dx1)*endTime+4.0*(x0-x1));
00317   Real c=-Sqr(dx1-dx0);
00318 
00319   //Version 1.1: new code to deal with rare side cases
00320   if(FuzzyZero(b,EpsilonX)) {
00321     //need to double check for numerical instability
00322     //if sign is +, this means we're switching directly to -
00323     //if sign is -, this means we're switching directly to +
00324     //if(sign > 0.0 && x1 > x0+dx0*endTime) return -1;
00325     //else if(sign < 0.0 && x1 < x0+dx0*endTime) return -1;
00326     switchTime = 0;
00327     Real a=(dx1-dx0)/endTime;
00328     if((sign > 0.0) == (a >= 0.0)) return -1;
00329     else return Abs(a);
00330   }
00331 
00332   Real accel1,accel2;
00333   int res=quadratic(a,b,c,accel1,accel2);
00334   Real switchTime1 = endTime*0.5+0.5*(dx1-dx0)/accel1;
00335   Real switchTime2 = endTime*0.5+0.5*(dx1-dx0)/accel2;
00336   if(accel1 == 0 && x0 == x1) switchTime1 = 0;
00337   if(accel2 == 0 && x0 == x1) switchTime2 = 0;
00338   if(res==0) return -1;
00339   else if(res==1) {
00340     if(switchTime1 >= 0 && switchTime1 <= endTime) { switchTime=switchTime1; return accel1; }
00341     return -1.0;
00342   }
00343   else if(res==2) {
00344     if(switchTime1 >= 0 && switchTime1 <= endTime) {
00345       if(switchTime2 >= 0 && switchTime2 <= endTime) {
00346         if(switchTime1 < switchTime2) { switchTime=switchTime1; return accel1; }
00347         else { switchTime=switchTime2; return accel2; }
00348       }
00349       else { switchTime=switchTime1; return accel1; }
00350     }
00351     else if(switchTime2 >= 0 && switchTime2 <= endTime) { switchTime=switchTime2; return accel2; }
00352     return -1.0;
00353   }
00354   return -1.0;
00355 }
00356 
00357 
00358 Real PLPRamp::Evaluate(Real t) const
00359 {
00360   Real tmT = t - ttotal;
00361   if(t < tswitch1) return x0 + 0.5*a*Sqr(t) + dx0*t;
00362   else if(t < tswitch2) {
00363     Real xswitch = x0 + 0.5*a*Sqr(tswitch1) + dx0*tswitch1;
00364     return xswitch + (t-tswitch1)*v;
00365   }
00366   else return x1 - 0.5*a*Sqr(tmT) + dx1*tmT;
00367 }
00368 
00369 Real PLPRamp::Derivative(Real t) const
00370 {
00371   Real tmT = t - ttotal;
00372   if(t < tswitch1) return a*t + dx0;
00373   else if(t < tswitch2) return v;
00374   else return -a*tmT + dx1;
00375 }
00376 
00377 Real PLPRamp::Accel(Real t) const
00378 {
00379   if(t < tswitch1) return a;
00380   else if(t < tswitch2) return 0;
00381   else return -a;
00382 }
00383 
00384 Real PLPRamp::CalcTotalTime(Real a,Real v) const
00385 {
00386   Real t1 = (v-dx0)/a;
00387   Real t2mT = (dx1-v)/a;
00388   Real y1 = 0.5*(Sqr(v) - Sqr(dx0))/a + x0;
00389   Real y2 = 0.5*(Sqr(dx1) - Sqr(v))/a + x1;
00390   Real t2mt1 = (y2-y1)/v;
00391   //Real xswitch = x0 + 0.5*a*Sqr(t1) + dx0*t1;
00392   if(t1 < 0 || t2mT > 0 || t2mt1 < 0) return -1;
00393   return t1 + t2mt1 - t2mT;
00394 }
00395 
00396 Real PLPRamp::CalcSwitchTime1(Real a,Real v) const
00397 {
00398   Real t1 = (v-dx0)/a;
00399   if(t1 < 0) return -1;
00400   return t1;
00401 }
00402 
00403 Real PLPRamp::CalcSwitchTime2(Real a,Real v) const
00404 {
00405   Real t1 = (v-dx0)/a;
00406   Real y1 = 0.5*(Sqr(v) - Sqr(dx0))/a + x0;
00407   Real y2 = 0.5*(Sqr(dx1) - Sqr(v))/a + x1;
00408   Real t2mt1 = (y2-y1)/v;
00409   //Real xswitch = x0 + 0.5*a*Sqr(t1) + dx0*t1;
00410   if(t1 < 0 || t2mt1 < 0) return -1;
00411   return t1 + t2mt1;
00412 }
00413 
00414 Real PLPRamp::CalcMinAccel(Real endTime,Real v) const
00415 {
00416   Real a = (v - (dx0+dx1) + 0.5/v*(Sqr(dx0)+Sqr(dx1)))/(endTime - (x1-x0)/v);
00417   Real t1 = (v-dx0)/a;
00418   Real t2mT = (dx1-v)/a;
00419   Real y1 = 0.5*(Sqr(v) - Sqr(dx0))/a + x0;
00420   Real y2 = 0.5*(Sqr(dx1) - Sqr(v))/a + x1;
00421   Real t2mt1 = (y2-y1)/v;
00422   //cout<<"EndTime "<<endTime<<", v "<<v<<endl;
00423   //cout<<"a = "<<a<<", t1="<<t1<<", t2mt1="<<t2mt1<<", t2mT="<<t2mT<<endl;
00424   if(t1 < 0 || t2mT > 0 || t2mt1 < 0) return Inf;
00425   return a;
00426 }
00427 
00428 
00429 bool PLPRamp::SolveMinTime(Real amax,Real vmax)
00430 {
00431   Real t1 = CalcTotalTime(amax,vmax);
00432   Real t2 = CalcTotalTime(-amax,vmax);
00433   Real t3 = CalcTotalTime(amax,-vmax);
00434   Real t4 = CalcTotalTime(-amax,-vmax);
00435   //cout<<"Time for PLP ++-: "<<t1<<", -++: "<<t2<<", +--: "<<t3<<", --+: "<<t4<<endl;
00436   ttotal = Inf;
00437   if(t1 >= 0 && t1 < ttotal) {
00438     a = amax;
00439     v = vmax;
00440     ttotal = t1;
00441   }
00442   if(t2 >= 0 && t2 < ttotal) {
00443     a = -amax;
00444     v = vmax;
00445     ttotal = t2;
00446   }
00447   if(t3 >= 0 && t3 < ttotal) {
00448     a = amax;
00449     v = -vmax;
00450     ttotal = t3;
00451   }
00452   if(t4 >= 0 && t4 < ttotal) {
00453     a = -amax;
00454     v = -vmax;
00455     ttotal = t4;
00456   }
00457   if(IsInf(ttotal)) {
00458     a = v = 0;
00459     tswitch1 = tswitch2 = ttotal = -1;
00460     return false;
00461   }
00462   tswitch1 = CalcSwitchTime1(a,v);
00463   tswitch2 = CalcSwitchTime2(a,v);
00464   return true;
00465 }
00466 
00467 bool PLPRamp::SolveMinAccel(Real endTime,Real vmax)
00468 {
00469   Real a1 = CalcMinAccel(endTime,vmax);
00470   Real a2 = CalcMinAccel(endTime,-vmax);
00471   a = Inf;
00472   if(fabs(a1) < a) {
00473     a = a1;
00474     v = vmax;
00475   }
00476   if(fabs(a2) < a) {
00477     a = a2;
00478     v = -vmax;
00479   }
00480   if(IsInf(a)) {
00481     a = 0;
00482     tswitch1 = tswitch2 = ttotal = -1;
00483     return false;
00484   }
00485   if(fabs(a) == 0) {
00486     tswitch1 = 0;
00487     tswitch2 = endTime;
00488     ttotal = endTime;
00489   }
00490   else {
00491     ttotal = CalcTotalTime(a,v);
00492     tswitch1 = CalcSwitchTime1(a,v);
00493     tswitch2 = CalcSwitchTime2(a,v);
00494 
00495     if(ttotal < 0) {  //there was an error computing the total time
00496       fprintf(stderr,"PLPRamp::SolveMinAccel: some numerical error prevented computing total time\n");
00497       getchar();
00498       return false;
00499     }
00500   }
00501   if(ttotal > endTime + 1e-3) {
00502     fprintf(stderr,"PLPRamp::SolveMinAccel: total time greater than endTime!\n");
00503     fprintf(stderr,"  endTime %g, accel %g, vel %g, switch times %g %g, total time %g\n",endTime,a,v,tswitch1,tswitch2,ttotal); 
00504     return false;
00505   }
00506   if(fabs(ttotal-endTime) >= 1e-3) {
00507     fprintf(stderr,"PLPRamp::SolveMinAccel: total time and endTime are different!\n");
00508     fprintf(stderr,"  endTime %g, accel %g, vel %g, switch times %g %g, total time %g\n",endTime,a,v,tswitch1,tswitch2,ttotal); 
00509   }
00510   assert(fabs(ttotal-endTime) < 1e-3);
00511   return true;
00512 }
00513 
00514 
00515 
00516 
00517 
00518 
00519 
00520 void ParabolicRamp1D::SetConstant(Real x)
00521 {
00522   x0 = x1 = x;
00523   dx0 = dx1 = 0;
00524   tswitch1=tswitch2=ttotal=0;
00525   v = a1 = a2 = 0;
00526 }
00527 
00528 Real ParabolicRamp1D::Evaluate(Real t) const
00529 {
00530   Real tmT = t - ttotal;
00531   if(t < tswitch1) return x0 + 0.5*a1*t*t + dx0*t;
00532   else if(t < tswitch2) {
00533     Real xswitch = x0 + 0.5*a1*tswitch1*tswitch1 + dx0*tswitch1;
00534     return xswitch + (t-tswitch1)*v;
00535   }
00536   else return x1 + 0.5*a2*tmT*tmT + dx1*tmT;
00537 }
00538 
00539 Real ParabolicRamp1D::Derivative(Real t) const
00540 {
00541   if(t < tswitch1) return a1*t + dx0;
00542   else if(t < tswitch2) return v;
00543   else {
00544     Real tmT = t - ttotal;
00545     return a2*tmT + dx1;
00546   }
00547 }
00548 
00549 Real ParabolicRamp1D::Accel(Real t) const
00550 {
00551   if(t < tswitch1) return a1;
00552   else if(t < tswitch2) return 0;
00553   else return a2;
00554 }
00555 
00556 bool ParabolicRamp1D::SolveMinAccel(Real endTime,Real vmax)
00557 {
00558   ParabolicRamp p;
00559   PPRamp pp;
00560   PLPRamp plp;
00561   p.x0 = pp.x0 = plp.x0 = x0;
00562   p.x1 = pp.x1 = plp.x1 = x1;
00563   p.dx0 = pp.dx0 = plp.dx0 = dx0;
00564   p.dx1 = pp.dx1 = plp.dx1 = dx1;
00565   bool pres = p.Solve();
00566   bool ppres = pp.SolveMinAccel(endTime);
00567   bool plpres = plp.SolveMinAccel(endTime,vmax);
00568   //cout<<"PP a: "<<pp.a<<", max vel "<<pp.MaxVelocity()<<endl;
00569   //cout<<"PLP a: "<<plp.a<<", vel "<<plp.v<<endl;
00570   a1 = Inf;
00571   if(pres && FuzzyEquals(endTime,p.ttotal,EpsilonT) && Abs(p.MaxVelocity()) <= vmax) {
00572     a1 = p.a;
00573     v = 0;
00574     tswitch1 = tswitch2 = p.ttotal;
00575     ttotal = p.ttotal;
00576   }
00577   if(ppres && Abs(pp.MaxVelocity()) <= vmax && Abs(pp.a) < Abs(a1)) {
00578     a1 = pp.a;
00579     v = 0;
00580     tswitch1 = tswitch2 = pp.tswitch;
00581     ttotal = pp.ttotal;
00582   }
00583   if(plpres && Abs(plp.v) <= vmax && Abs(plp.a) < Abs(a1)) {
00584     a1 = plp.a;
00585     v = plp.v;
00586     tswitch1 = plp.tswitch1;
00587     tswitch2 = plp.tswitch2;
00588     ttotal = plp.ttotal;
00589   }
00590   
00591   if(IsInf(a1)) {
00592     if(vmax == 0) {
00593       if(FuzzyEquals(x0,x1,EpsilonX) && FuzzyEquals(dx0,dx1,EpsilonV)) {
00594         a1 = a2 = v = 0;
00595         tswitch1 = tswitch2 = ttotal = endTime;
00596         return true;
00597       }
00598     }
00599     a1 = a2 = v = 0;
00600     tswitch1 = tswitch2 = ttotal = -1;
00601     printf("No ramp equation could solve for min-accel!\n");
00602     printf("x0=%g, x1=%g, dx0=%g, dx1=%g\n",x0,x1,dx0,dx1);
00603     printf("end time %g, vmax = %g\n",endTime,vmax);
00604 
00605     printf("PP=%d, PLP=%d\n",(int)ppres,(int)plpres);
00606     printf("pp.a = %g, max vel=%g\n",pp.a,pp.MaxVelocity());
00607     printf("plp.a = %g, v=%g\n",plp.a,plp.v);
00608     return false;
00609   }
00610   a2 = -a1;
00611   assert(fabs(ttotal-endTime) < 1e-3);
00612   return true;
00613 }
00614 
00615 bool ParabolicRamp1D::SolveMinTime(Real amax,Real vmax)
00616 {
00617   ParabolicRamp p;
00618   PPRamp pp;
00619   PLPRamp plp;
00620   p.x0 = pp.x0 = plp.x0 = x0;
00621   p.x1 = pp.x1 = plp.x1 = x1;
00622   p.dx0 = pp.dx0 = plp.dx0 = dx0;
00623   p.dx1 = pp.dx1 = plp.dx1 = dx1;
00624   bool pres = p.Solve();
00625   bool ppres = pp.SolveMinTime(amax);
00626   bool plpres = plp.SolveMinTime(amax,vmax);
00627   //cout<<"P time: "<<p.ttotal<<", accel "<<p.a<<endl;
00628   //cout<<"PP time: "<<pp.ttotal<<", max vel "<<pp.MaxVelocity()<<endl;
00629   //cout<<"PLP time: "<<plp.ttotal<<", vel "<<plp.v<<endl;
00630   ttotal = Inf;
00631   if(pres && Abs(p.a) <= amax+EpsilonA && p.ttotal < ttotal) {
00632     if(Abs(p.a) <= amax) {
00633       a1 = p.a;
00634       v = 0;
00635       tswitch1 = tswitch2 = p.ttotal;
00636       ttotal = p.ttotal;
00637     }
00638     else {
00639       //double check
00640       p.a = Sign(p.a)*amax;
00641       if(FuzzyEquals(p.Evaluate(p.ttotal),x1,EpsilonX) && FuzzyEquals(p.Derivative(p.ttotal),dx1,EpsilonV)) {
00642         a1 = p.a;
00643         v = 0;
00644         tswitch1=tswitch2=p.ttotal;
00645         ttotal = p.ttotal;
00646       }
00647     }
00648   }
00649   if(ppres && Abs(pp.MaxVelocity()) <= vmax && pp.ttotal < ttotal) {
00650     a1 = pp.a;
00651     v = 0;
00652     tswitch1 = tswitch2 = pp.tswitch;
00653     ttotal = pp.ttotal;
00654   }
00655   if(plpres && plp.ttotal < ttotal) {
00656     a1 = plp.a;
00657     v = plp.v;
00658     tswitch1 = plp.tswitch1;
00659     tswitch2 = plp.tswitch2;
00660     ttotal = plp.ttotal;
00661   }
00662   if(IsInf(ttotal)) {
00663     printf("No ramp equation could solve for min-time!\n");
00664     printf("x0=%g, x1=%g, dx0=%g, dx1=%g\n",x0,x1,dx0,dx1);
00665     printf("vmax = %g, amax = %g\n",vmax,amax);
00666     printf("P=%d, PP=%d, PLP=%d\n",(int)pres,(int)ppres,(int)plpres);
00667     a1 = a2 = v = 0;
00668     tswitch1 = tswitch2 = ttotal = -1;
00669     return false;
00670   }
00671   a2 = -a1;
00672   //cout<<"switch time 1: "<<tswitch1<<", 2: "<<tswitch2<<", total "<<ttotal<<endl;
00673   return true;
00674 }
00675 
00676 void ParabolicRamp1D::Dilate(Real timeScale)
00677 {
00678   tswitch1*=timeScale;
00679   tswitch2*=timeScale;
00680   ttotal*=timeScale;
00681   a1 *= 1.0/Sqr(timeScale);
00682   a2 *= 1.0/Sqr(timeScale);
00683   v *= 1.0/timeScale;
00684 }
00685 
00686 void ParabolicRamp1D::TrimFront(Real tcut)
00687 {
00688   x0 = Evaluate(tcut);
00689   dx0 = Derivative(tcut);
00690   ttotal -= tcut;
00691   tswitch1 -= tcut;
00692   tswitch2 -= tcut;
00693   if(tswitch1 < 0) tswitch1=0;
00694   if(tswitch2 < 0) tswitch2=0;
00695   assert(IsValid());
00696 
00697 }
00698 
00699 void ParabolicRamp1D::TrimBack(Real tcut)
00700 {
00701   x1 = Evaluate(ttotal-tcut);
00702   dx1 = Derivative(ttotal-tcut);
00703   ttotal -= tcut;
00704   tswitch1 = Min(tswitch1,ttotal);
00705   tswitch2 = Min(tswitch2,ttotal);
00706   assert(IsValid());
00707 }
00708 
00709 bool ParabolicRamp1D::IsValid() const
00710 {
00711   if(tswitch1 < 0 || tswitch2 < tswitch1 || ttotal < tswitch2) {
00712     fprintf(stderr,"Ramp has invalid timing %g %g %g\n",tswitch1,tswitch2,ttotal);
00713     return false;
00714   }
00715 
00716   Real t2mT = tswitch2 - ttotal;
00717   if(tswitch1 != tswitch2) {
00718     if(!FuzzyEquals(a1*tswitch1 + dx0,v,EpsilonV)) {
00719       fprintf(stderr,"Ramp has incorrect switch 1 speed: %g vs %g\n",a1*tswitch1 + dx0,v);
00720       return false;
00721     }
00722     if(!FuzzyEquals(a2*t2mT + dx1,v,EpsilonV)) {
00723       fprintf(stderr,"Ramp has incorrect switch 2 speed: %g vs %g\n",a2*t2mT + dx1,v);
00724       return false;
00725     }
00726   }
00727   //check switch2
00728   Real xswitch = x0 + 0.5*a1*Sqr(tswitch1) + dx0*tswitch1;
00729   Real xswitch2 = xswitch + (tswitch2-tswitch1)*v;
00730   if(!FuzzyEquals(xswitch2,x1 + 0.5*a2*Sqr(t2mT) + dx1*t2mT,EpsilonX)) {
00731     fprintf(stderr,"Ramp has incorrect switch 2 position: %g vs %g\n",xswitch2,x1 + 0.5*a2*Sqr(t2mT) + dx1*t2mT);
00732     return false;
00733   }
00734   return true;
00735 }
00736 
00737 
00738 
00739 
00740 
00741 void ParabolicRampND::SetConstant(const Vector& x)
00742 {
00743   x0 = x1 = x;
00744   dx0.resize(x.size());
00745   dx1.resize(x.size());
00746   fill(dx0.begin(),dx0.end(),0);
00747   fill(dx1.begin(),dx1.end(),0);
00748   endTime = 0;
00749   ramps.resize(x.size());
00750   for(size_t i=0;i<x.size();i++)
00751     ramps[i].SetConstant(x[i]);
00752 }
00753 
00754 bool ParabolicRampND::SolveMinTimeLinear(const Vector& amax,const Vector& vmax)
00755 {
00756   assert(x0.size() == dx0.size());
00757   assert(x1.size() == dx1.size());
00758   assert(x0.size() == x1.size());
00759   assert(x0.size() == amax.size());
00760   assert(x0.size() == vmax.size());
00761   endTime = 0;
00762   ramps.resize(x0.size());
00763   ParabolicRamp1D sramp;
00764   sramp.x0 = 0;
00765   sramp.x1 = 1;
00766   sramp.dx0 = 0;
00767   sramp.dx1 = 0;
00768   Real svmax=Inf,samax=Inf;
00769   for(size_t i=0;i<ramps.size();i++) {
00770     ramps[i].x0=x0[i];
00771     ramps[i].x1=x1[i];
00772     ramps[i].dx0=dx0[i];
00773     ramps[i].dx1=dx1[i];
00774     if(vmax[i]==0 || amax[i]==0) {
00775       if(!FuzzyEquals(x0[i],x1[i],EpsilonX)) {
00776         printf("index %d vmax = %g, amax = %g, X0 != X1 (%g != %g)\n",(int) i,vmax[i],amax[i],x0[i],x1[i]);
00777         return false;
00778       }
00779       if(!FuzzyEquals(dx0[i],dx1[i],EpsilonV)) {
00780         printf("index %d vmax = %g, amax = %g, DX0 != DX1 (%g != %g)\n",(int) i,vmax[i],amax[i],dx0[i],dx1[i]);
00781         return false;
00782       }
00783       ramps[i].tswitch1=ramps[i].tswitch2=ramps[i].ttotal=0;
00784       ramps[i].a1=ramps[i].a1=ramps[i].v=0;
00785       continue;
00786     }
00787     if(vmax[i] < svmax*Abs(x1[i]-x0[i]))
00788       svmax = vmax[i]/Abs(x1[i]-x0[i]);
00789     if(amax[i] < samax*Abs(x1[i]-x0[i]))
00790       samax = amax[i]/Abs(x1[i]-x0[i]);
00791   }
00792 
00793   bool res=sramp.SolveMinTime(samax,svmax);
00794   if(!res) {
00795     fprintf(stderr,"Warning in straight-line parameter solve\n");
00796     getchar();
00797     return false;
00798   }
00799 
00800   endTime = sramp.ttotal;
00801   for(size_t i=0;i<ramps.size();i++) {
00802     ramps[i].v = svmax * (x1[i]-x0[i]);
00803     ramps[i].a1 = samax * (x1[i]-x0[i]);
00804     ramps[i].a2 = -samax * (x1[i]-x0[i]);
00805     ramps[i].tswitch1 = sramp.tswitch1;
00806     ramps[i].tswitch2 = sramp.tswitch2;
00807     ramps[i].ttotal = endTime;
00808     if(!ramps[i].IsValid()) {
00809       fprintf(stderr,"Warning, error in straight-line path formula\n");
00810       getchar();
00811     }
00812   }
00813   return true;
00814 }
00815 
00816 bool ParabolicRampND::SolveMinTime(const Vector& amax,const Vector& vmax)
00817 {
00818   assert(x0.size() == dx0.size());
00819   assert(x1.size() == dx1.size());
00820   assert(x0.size() == x1.size());
00821   assert(x0.size() == amax.size());
00822   assert(x0.size() == vmax.size());
00823   endTime = 0;
00824   ramps.resize(x0.size());
00825   for(size_t i=0;i<ramps.size();i++) {
00826     ramps[i].x0=x0[i];
00827     ramps[i].x1=x1[i];
00828     ramps[i].dx0=dx0[i];
00829     ramps[i].dx1=dx1[i];
00830     if(vmax[i]==0 || amax[i]==0) {
00831       if(!FuzzyEquals(x0[i],x1[i],EpsilonX)) {
00832         printf("index %d vmax = %g, amax = %g, X0 != X1 (%g != %g)\n",(int)i,vmax[i],amax[i],x0[i],x1[i]);
00833         return false;
00834       }
00835       if(!FuzzyEquals(dx0[i],dx1[i],EpsilonV)) {
00836         printf("index %d vmax = %g, amax = %g, DX0 != DX1 (%g != %g)\n",(int)i,vmax[i],amax[i],dx0[i],dx1[i]);
00837         return false;
00838       }
00839       ramps[i].tswitch1=ramps[i].tswitch2=ramps[i].ttotal=0;
00840       ramps[i].a1=ramps[i].a2=ramps[i].v=0;
00841       continue;
00842     }
00843     if(!ramps[i].SolveMinTime(amax[i],vmax[i])) return false;
00844     if(ramps[i].ttotal > endTime) endTime = ramps[i].ttotal;
00845   }
00846   for(size_t i=0;i<ramps.size();i++) {
00847     if(ramps[i].ttotal != endTime) {
00848       if(vmax[i]==0) {
00849         ramps[i].ttotal = endTime;
00850       }
00851       else if(!ramps[i].SolveMinAccel(endTime,vmax[i])) {
00852         printf("Failed solving min accel for joint %d\n",(int)i);
00853         ramps[i].SolveMinTime(amax[i],vmax[i]);
00854         printf("its min time is %g\n",ramps[i].ttotal);
00855         if(ramps[i].tswitch1==ramps[i].tswitch2)
00856           printf("its type is PP\n");
00857         else if(Abs(ramps[i].v)==vmax[i])
00858           printf("its type is PLP (vmax)\n");
00859         else 
00860           printf("its type is PLP (v=%g %%)\n",ramps[i].v/vmax[i]);
00861         printf("Saving to failed_ramps.txt\n");
00862         FILE* f=fopen("failed_ramps.txt","w+");
00863         fprintf(f,"MinAccel T=%g, vmax=%g\n",endTime,vmax[i]);
00864         fprintf(f,"x0=%g, dx0=%g\n",ramps[i].x0,ramps[i].dx0);
00865         fprintf(f,"x1=%g, dx1=%g\n",ramps[i].x1,ramps[i].dx1);
00866         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);
00867         fprintf(f,"\n");
00868         fclose(f);
00869         return false;
00870       }
00871     }
00872   }
00873   return true;
00874 }
00875 
00876 bool ParabolicRampND::SolveMinAccel(const Vector& vmax,Real time)
00877 {
00878   assert(x0.size() == dx0.size());
00879   assert(x1.size() == dx1.size());
00880   assert(x0.size() == x1.size());
00881   assert(x0.size() == vmax.size());
00882   endTime = time;
00883   ramps.resize(x0.size());
00884   for(size_t i=0;i<ramps.size();i++) {
00885     ramps[i].x0=x0[i];
00886     ramps[i].x1=x1[i];
00887     ramps[i].dx0=dx0[i];
00888     ramps[i].dx1=dx1[i];
00889     if(vmax[i]==0) {
00890       assert(FuzzyEquals(x0[i],x1[i],EpsilonX));
00891       assert(FuzzyEquals(dx0[i],dx1[i],EpsilonV));
00892       ramps[i].tswitch1=ramps[i].tswitch2=ramps[i].ttotal=0;
00893       ramps[i].a1=ramps[i].a2=ramps[i].v=0;
00894       continue;
00895     }
00896     if(!ramps[i].SolveMinAccel(endTime,vmax[i])) {
00897       return false;
00898     }
00899   }
00900   return true;
00901 }
00902 
00903 void ParabolicRampND::Evaluate(Real t,Vector& x) const
00904 {
00905   x.resize(ramps.size());
00906   for(size_t j=0;j<ramps.size();j++)
00907     x[j]=ramps[j].Evaluate(t);
00908 }
00909 
00910 void ParabolicRampND::Derivative(Real t,Vector& x) const
00911 {
00912   x.resize(ramps.size());
00913   for(size_t j=0;j<ramps.size();j++)
00914     x[j]=ramps[j].Derivative(t);
00915 }
00916 
00917 void ParabolicRampND::Output(Real dt,std::vector<Vector>& path) const
00918 {
00919   assert(!ramps.empty());
00920   int size = (int)ceil(endTime/dt)+1;
00921   path.resize(size);
00922   if(size == 1) {
00923     path[0].resize(ramps.size());
00924     for(size_t j=0;j<ramps.size();j++)
00925       path[0][j] = ramps[j].x0;
00926     return;
00927   } 
00928   for(int i=0;i<size;i++) {
00929     Real t=endTime*Real(i)/Real(size-1);
00930     path[i].resize(ramps.size());
00931     for(size_t j=0;j<ramps.size();j++)
00932       path[i][j]=ramps[j].Evaluate(t);
00933   }
00934   
00935   /*
00936   path[0].resize(ramps.size());
00937   for(size_t j=0;j<ramps.size();j++)
00938     path[0][j] = ramps[j].x0;
00939   for(int i=1;i+1<size;i++) {
00940     Real t=endTime*Real(i)/Real(size-1);
00941     path[i].resize(ramps.size());
00942     for(size_t j=0;j<ramps.size();j++)
00943       path[i][j]=ramps[j].Evaluate(t);
00944   }
00945   path[size-1].resize(ramps.size());
00946   for(size_t j=0;j<ramps.size();j++)
00947     path[size-1][j] = ramps[j].x1;
00948   */
00949 }
00950 
00951 
00952 void ParabolicRampND::Dilate(Real timeScale)
00953 {
00954   for(size_t i=0;i<ramps.size();i++)
00955     ramps[i].Dilate(timeScale);
00956 }
00957 
00958 void ParabolicRampND::TrimFront(Real tcut)
00959 {
00960   Evaluate(tcut,x0);
00961   Derivative(tcut,dx0);
00962   endTime -= tcut;
00963   for(size_t i=0;i<ramps.size();i++)
00964     ramps[i].TrimFront(tcut);
00965   assert(IsValid());
00966 }
00967 
00968 void ParabolicRampND::TrimBack(Real tcut)
00969 {
00970   Evaluate(endTime-tcut,x1);
00971   Derivative(endTime-tcut,dx1);
00972   endTime -= tcut;
00973   for(size_t i=0;i<ramps.size();i++)
00974     ramps[i].TrimBack(tcut);
00975   assert(IsValid());
00976 }
00977 
00978 bool ParabolicRampND::IsValid() const
00979 {
00980   if(endTime < 0) return false;
00981   for(size_t i=0;i<ramps.size();i++)
00982     if(!ramps[i].IsValid()) return false;
00983   return true;
00984 }