constraint_aware_spline_smoother: DynamicPath.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/DynamicPath.h"
00033 #include <list>
00034 #include <algorithm>
00035 using namespace std;
00036 
00037 const static Real EpsilonT = 1e-8;
00038 //maximum iterations for distance-based line segment checker
00039 const static int checkMaxIters = 1000;
00040 
00041 inline Real LInfDistance(const Vector& a,const Vector& b)
00042 {
00043   assert(a.size()==b.size());
00044   Real d=0;
00045   for(size_t i=0;i<a.size();i++)
00046     d = Max(d,Abs(a[i]-b[i]));
00047   return d;
00048 }
00049 
00050 
00051 inline Real MaxBBLInfDistance(const Vector& x,const Vector& bmin,const Vector& bmax)
00052 {
00053   assert(x.size()==bmin.size());
00054   assert(x.size()==bmax.size());
00055   Real d=0;
00056   for(size_t i=0;i<x.size();i++)
00057     d = Max(d,Max(Abs(x[i]-bmin[i]),Abs(x[i]-bmax[i])));
00058   return d;
00059 }
00060 
00061 DynamicPath::DynamicPath()
00062 {}
00063 
00064 void DynamicPath::Init(const Vector& _velMax,const Vector& _accMax)
00065 {
00066   velMax = _velMax;
00067   accMax = _accMax;
00068   assert(velMax.size() == accMax.size());
00069 }
00070 
00071 Real DynamicPath::GetTotalTime() const
00072 {
00073   Real t=0;
00074   for(size_t i=0;i<ramps.size();i++) t+=ramps[i].endTime;
00075   return t;
00076 }
00077 
00078 void DynamicPath::Evaluate(Real t,Vector& x)
00079 {
00080   assert(!ramps.empty());
00081   if(t < 0) {
00082     x = ramps.front().x0;
00083   }
00084   else {
00085     for(size_t i=0;i<ramps.size();i++) {
00086       if(t <= ramps[i].endTime) {
00087         ramps[i].Evaluate(t,x);
00088         return;
00089       }
00090       t -= ramps[i].endTime;
00091     }
00092     x = ramps.back().x1;
00093   }
00094 }
00095 
00096 void DynamicPath::Derivative(Real t,Vector& dx)
00097 {
00098   assert(!ramps.empty());
00099   if(t < 0) {
00100     dx = ramps.front().dx0;
00101   }
00102   else {
00103     for(size_t i=0;i<ramps.size();i++) {
00104       if(t <= ramps[i].endTime) {
00105         ramps[i].Derivative(t,dx);
00106         return;
00107       }
00108       t -= ramps[i].endTime;
00109     }
00110     dx = ramps.back().dx1;
00111   }
00112 }
00113 
00114 void DynamicPath::SetMilestones(const vector<Vector>& x)
00115 {
00116   if(x.empty()) {
00117     ramps.resize(0);
00118   }
00119   else if(x.size()==1) {
00120     ramps.resize(1);
00121     ramps[0].SetConstant(x[0]);
00122   }
00123   else {
00124     Vector zero(x[0].size(),0.0);
00125     ramps.resize(x.size()-1);
00126     for(size_t i=0;i<ramps.size();i++) {
00127       ramps[i].x0 = x[i];
00128       ramps[i].x1 = x[i+1];
00129       ramps[i].dx0 = zero;
00130       ramps[i].dx1 = zero;
00131       bool res=ramps[i].SolveMinTimeLinear(accMax,velMax);
00132       assert(res);
00133     }
00134   }
00135 }
00136 
00137 void DynamicPath::SetMilestones(const vector<Vector>& x,const vector<Vector>& dx)
00138 {
00139   if(x.empty()) {
00140     ramps.resize(0);
00141   }
00142   else if(x.size()==1) {
00143     ramps.resize(1);
00144     ramps[0].SetConstant(x[0]);
00145   }
00146   else {
00147     ramps.resize(x.size()-1);
00148     for(size_t i=0;i<ramps.size();i++) {
00149       ramps[i].x0 = x[i];
00150       ramps[i].x1 = x[i+1];
00151       ramps[i].dx0 = dx[i];
00152       ramps[i].dx1 = dx[i+1];
00153       bool res=ramps[i].SolveMinTime(accMax,velMax);
00154       assert(res);
00155     }
00156   }
00157 }
00158 
00159 void DynamicPath::GetMilestones(vector<Vector>& x,vector<Vector>& dx) const
00160 {
00161   if(ramps.empty()) {
00162     x.resize(0);
00163     dx.resize(0);
00164     return;
00165   }
00166   x.resize(ramps.size()+1);
00167   dx.resize(ramps.size()+1);
00168   x[0] = ramps[0].x0;
00169   dx[0] = ramps[0].dx0;
00170   for(size_t i=0;i<ramps.size();i++) {
00171     x[i+1] = ramps[i].x1;
00172     dx[i+1] = ramps[i].dx1;
00173   }
00174 }
00175 
00176 void DynamicPath::Append(const Vector& x)
00177 {
00178   size_t n=ramps.size();
00179   size_t p=n-1;
00180   ramps.resize(ramps.size()+1);
00181   if(ramps.size()==1) {
00182     ramps[0].SetConstant(x);
00183   }
00184   else {
00185     ramps[n].x0 = ramps[p].x1;
00186     ramps[n].dx0 = ramps[p].dx1;
00187     ramps[n].x1 = x;
00188     ramps[n].dx1.resize(x.size());
00189     fill(ramps[n].dx1.begin(),ramps[n].dx1.end(),0);
00190     bool res=ramps[n].SolveMinTime(accMax,velMax);
00191     assert(res);
00192   }
00193 }
00194 
00195 void DynamicPath::Append(const Vector& x,const Vector& dx)
00196 {
00197   size_t n=ramps.size();
00198   size_t p=n-1;
00199   ramps.resize(ramps.size()+1);
00200   if(ramps.size()==1) {
00201     fprintf(stderr,"Can't append milestone with a nonzero velocity to an empty path\n");
00202     abort();
00203   }
00204   else {
00205     ramps[n].x0 = ramps[p].x1;
00206     ramps[n].dx0 = ramps[p].dx1;
00207     ramps[n].x1 = x;
00208     ramps[n].dx1 = dx;
00209     bool res=ramps[n].SolveMinTime(accMax,velMax);
00210     assert(res);
00211   }
00212 }
00213 
00214 struct RampSection
00215 {
00216   Real ta,tb;
00217   Vector xa,xb;
00218   Real da,db;
00219 };
00220 
00221 
00222 void BoundingBox(const ParabolicRamp1D& ramp,Real ta,Real tb,Real& bmin,Real& bmax)
00223 {
00224   if(ta > tb) {  //orient the interval
00225     return BoundingBox(ramp,tb,ta,bmin,bmax);
00226   }
00227   if(ta < 0) ta = 0;
00228   if(tb <= 0) {
00229     bmin = bmax = ramp.x0;
00230     return;
00231   }
00232   if(tb > ramp.ttotal) tb=ramp.ttotal;
00233   if(ta >= ramp.ttotal) {
00234     bmin = bmax = ramp.x1;
00235     return;
00236   }
00237 
00238   bmin = ramp.Evaluate(ta);
00239   bmax = ramp.Evaluate(tb);
00240   if(bmin > bmax) Swap(bmin,bmax);
00241 
00242   Real tflip1=0,tflip2=0;
00243   if(ta < ramp.tswitch1) {
00244     //x' = a1*t + v0 = 0 => t = -v0/a1
00245     tflip1 = -ramp.dx0/ramp.a1;
00246     if(tflip1 > ramp.tswitch1) tflip1 = 0;
00247   }
00248   if(tb > ramp.tswitch2) {
00249     //x' = a2*(T-t) + v1 = 0 => (T-t) = v1/a2
00250     tflip2 = ramp.ttotal-ramp.dx1/ramp.a2;
00251     if(tflip2 < ramp.tswitch2) tflip2 = 0;
00252   }
00253   if(ta < tflip1 && tb > tflip1) {
00254     Real xflip = ramp.Evaluate(tflip1);
00255     if(xflip < bmin) bmin = xflip;
00256     else if(xflip > bmax) bmax = xflip;
00257   }
00258   if(ta < tflip2 && tb > tflip2) {
00259     Real xflip = ramp.Evaluate(tflip2);
00260     if(xflip < bmin) bmin = xflip;
00261     else if(xflip > bmax) bmax = xflip;
00262   }
00263 }
00264 
00265 void BoundingBox(const ParabolicRampND& ramp,Real ta,Real tb,Vector& bmin,Vector& bmax)
00266 {
00267   bmin.resize(ramp.ramps.size());
00268   bmax.resize(ramp.ramps.size());
00269   for(size_t i=0;i<ramp.ramps.size();i++) {
00270     BoundingBox(ramp.ramps[i],ta,tb,bmin[i],bmax[i]);
00271   }
00272 }
00273 
00274     
00275 
00276 bool CheckRamp(const ParabolicRampND& ramp,FeasibilityCheckerBase* feas,DistanceCheckerBase* distance,int maxiters)
00277 {
00278   if(!feas->ConfigFeasible(ramp.x0)) return false;
00279   if(!feas->ConfigFeasible(ramp.x1)) return false;
00280   assert(distance->ObstacleDistanceNorm()==Inf);
00281   RampSection section;
00282   section.ta = 0;
00283   section.tb = ramp.endTime;
00284   section.xa = ramp.x0;
00285   section.xb = ramp.x1;
00286   section.da = distance->ObstacleDistance(ramp.x0);
00287   section.db = distance->ObstacleDistance(ramp.x1);
00288   if(section.da <= 0.0) return false;
00289   if(section.db <= 0.0) return false;
00290   list<RampSection> queue;
00291   queue.push_back(section);
00292   int iters=0;
00293   while(!queue.empty()) {
00294     section = queue.front();
00295     queue.erase(queue.begin());
00296 
00297     //check the bounds around this section
00298     if(LInfDistance(section.xa,section.xb) <= section.da+section.db) {
00299       Vector bmin,bmax;
00300       BoundingBox(ramp,section.ta,section.tb,bmin,bmax);
00301       if(MaxBBLInfDistance(section.xa,bmin,bmax) < section.da && 
00302          MaxBBLInfDistance(section.xb,bmin,bmax) < section.db)
00303         //can cull out the section
00304         continue;
00305     }
00306     Real tc = (section.ta+section.tb)*0.5;
00307     Vector xc;
00308     ramp.Evaluate(tc,xc);
00309     if(!feas->ConfigFeasible(xc)) return false;  //infeasible config
00310     //subdivide
00311     Real dc = distance->ObstacleDistance(xc);
00312     RampSection sa,sb;
00313     sa.ta = section.ta;
00314     sa.xa = section.xa;
00315     sa.da = section.da;
00316     sa.tb = sb.ta = tc;
00317     sa.xb = sb.xa = xc;
00318     sa.db = sb.da = dc;
00319     sb.tb = section.tb;
00320     sb.xb = section.xb;
00321     sb.db = section.db;
00322 
00323     //recurse on segments
00324     queue.push_back(sa);
00325     queue.push_back(sb);
00326 
00327     if(iters++ >= maxiters) return false;
00328   }
00329   return true;
00330 }
00331 
00332 bool CheckRamp(const ParabolicRampND& ramp,FeasibilityCheckerBase* space,Real tol)
00333 {
00334   assert(tol > 0);
00335   if(!space->ConfigFeasible(ramp.x0)) return false;
00336   if(!space->ConfigFeasible(ramp.x1)) return false;
00337   //assert(space->ConfigFeasible(ramp.x0));
00338   //assert(space->ConfigFeasible(ramp.x1));
00339 
00340   //for a parabola of form f(x) = a x^2 + b x, and the straight line 
00341   //of form g(X,u) = u*f(X)
00342   //d^2(g(X,u),p) = |p - <f(X),p>/<f(X),f(X)> f(X)|^2 < tol^2
00343   //<p,p> - <f(X),p>^2/<f(X),f(X)>  = p^T (I-f(X)f(X)^T/f(X)^T f(X)) p
00344   //max_x d^2(f(x)) => f(x)^T (I-f(X)f(X)^T/f(X)^T f(X)) f'(x) = 0
00345   //(x^2 a^T + x b^T) A (2a x + b) = 0
00346   //(x a^T + b^T) A (2a x + b) = 0
00347   //2 x^2 a^T A a + 3 x b^T A a + b^T A b = 0
00348 
00349   //the max X for which f(x) deviates from g(X,x) by at most tol is...
00350   //max_x |g(X,x)-f(x)| = max_x x/X f(X)-f(x)
00351   //=> f(X)/X - f'(x) = 0
00352   //=>  X/2 = x 
00353   //=> max_x |g(X,x)-f(x)| = |(X/2)/X f(X)-f(X/2)|
00354   //= |1/2 (aX^2+bX) - a(X/2)^2 - b(X/2) + c |
00355   //= |a| X^2 / 4
00356   //so... max X st max_x |g(X,x)-f(x)| < tol => X = 2*sqrt(tol/|a|)
00357   vector<Real> divs;
00358   Real t=0;
00359   divs.push_back(t);
00360   while(t < ramp.endTime) {
00361     Real tnext=t;
00362     Real amax = 0;
00363     Real switchNext=ramp.endTime;
00364     for(size_t i=0;i<ramp.ramps.size();i++) {
00365       if(t < ramp.ramps[i].tswitch1) {  //ramp up
00366         switchNext =  Min(switchNext, ramp.ramps[i].tswitch1);
00367         amax = Max(amax,Max(Abs(ramp.ramps[i].a1),Abs(ramp.ramps[i].a2)));
00368       }
00369       else if(t < ramp.ramps[i].tswitch2) {  //constant vel
00370         switchNext = Min(switchNext, ramp.ramps[i].tswitch2);
00371       }
00372       else if(t < ramp.ramps[i].ttotal) {  //ramp down
00373         amax = Max(amax,Max(Abs(ramp.ramps[i].a1),Abs(ramp.ramps[i].a2)));     
00374       }
00375     }
00376     Real dt = 2.0*Sqrt(tol/amax);
00377     if(t+dt > switchNext) tnext = switchNext;
00378     else tnext = t+dt;
00379 
00380     t = tnext;
00381     divs.push_back(tnext);
00382   }
00383   divs.push_back(ramp.endTime);
00384 
00385   //do a bisection thingie
00386   list<pair<int,int> > segs;
00387   segs.push_back(pair<int,int>(0,divs.size()-1));
00388   Vector q1,q2;
00389   while(!segs.empty()) {
00390     int i=segs.front().first;
00391     int j=segs.front().second;
00392     segs.erase(segs.begin());
00393     if(j == i+1) {
00394       //check path from t to tnext
00395       ramp.Evaluate(divs[i],q1);
00396       ramp.Evaluate(divs[j],q2);
00397       if(!space->SegmentFeasible(q1,q2)) return false;
00398     }
00399     else {
00400       int k=(i+j)/2;
00401       ramp.Evaluate(divs[k],q1);
00402       if(!space->ConfigFeasible(q1)) return false;
00403       segs.push_back(pair<int,int>(i,k));
00404       segs.push_back(pair<int,int>(k,j));
00405     }
00406   }
00407   return true;
00408 }
00409 
00410 bool DynamicPath::TryShortcut(Real t1,Real t2,FeasibilityCheckerBase* space,Real tol)
00411 {
00412   if(t1 > t2) Swap(t1,t2);
00413   vector<Real> rampStartTime(ramps.size()); 
00414   Real endTime=0;
00415   for(size_t i=0;i<ramps.size();i++) {
00416     rampStartTime[i] = endTime;
00417     endTime += ramps[i].endTime;
00418   }
00419   int i1 = std::upper_bound(rampStartTime.begin(),rampStartTime.end(),t1)-rampStartTime.begin()-1;
00420   int i2 = std::upper_bound(rampStartTime.begin(),rampStartTime.end(),t2)-rampStartTime.begin()-1;
00421   if(i1 == i2) return false;
00422   Real u1 = t1-rampStartTime[i1];
00423   Real u2 = t2-rampStartTime[i2];
00424   assert(u1 >= 0);
00425   assert(u1 <= ramps[i1].endTime+EpsilonT);
00426   assert(u2 >= 0);
00427   assert(u2 <= ramps[i2].endTime+EpsilonT);
00428   u1 = Min(u1,ramps[i1].endTime);
00429   u2 = Min(u2,ramps[i2].endTime);
00430   ParabolicRampND test;
00431   ramps[i1].Evaluate(u1,test.x0);
00432   ramps[i2].Evaluate(u2,test.x1);
00433   ramps[i1].Derivative(u1,test.dx0);
00434   ramps[i2].Derivative(u2,test.dx1);
00435   bool res=test.SolveMinTime(accMax,velMax);
00436   if(!res) return false;
00437   assert(test.endTime >= 0);
00438   assert(test.IsValid());
00439   if(!CheckRamp(test,space,tol)) return false;
00440 
00441   //perform shortcut
00442   //crop i1 and i2
00443   ramps[i1].TrimBack(ramps[i1].endTime-u1);
00444   ramps[i1].x1 = test.x0;
00445   ramps[i1].dx1 = test.dx0;
00446   ramps[i2].TrimFront(u2);
00447   ramps[i2].x0 = test.x1;
00448   ramps[i2].dx0 = test.dx1;
00449   
00450   //test the in-between ramps
00451   ParabolicRampND temp;
00452   temp = ramps[i1];
00453   if(temp.SolveMinTime(accMax,velMax) == false) {
00454     fprintf(stderr,"Warning, truncated ramp can't be re-solved\n");
00455     fprintf(stderr,"Press enter to continue\n");
00456     getchar();
00457   }
00458   temp = ramps[i2];
00459   if(temp.SolveMinTime(accMax,velMax) == false) {
00460     fprintf(stderr,"Warning, truncated ramp can't be re-solved\n");
00461     fprintf(stderr,"Press enter to continue\n");
00462     getchar();
00463   }
00464   
00465   //replace intermediate ramps with test
00466   for(int i=0;i<i2-i1-1;i++)
00467     ramps.erase(ramps.begin()+i1+1);
00468   ramps.insert(ramps.begin()+i1+1,test);
00469   
00470   //check for consistency
00471   for(size_t i=0;i+1<ramps.size();i++) {
00472     assert(ramps[i].x1 == ramps[i+1].x0);
00473     assert(ramps[i].dx1 == ramps[i+1].dx0);
00474   }
00475   return true;
00476 }
00477 
00478 bool DynamicPath::TryShortcut(Real t1,Real t2,FeasibilityCheckerBase* feas,DistanceCheckerBase* dist)
00479 {
00480   if(t1 > t2) Swap(t1,t2);
00481   vector<Real> rampStartTime(ramps.size()); 
00482   Real endTime=0;
00483   for(size_t i=0;i<ramps.size();i++) {
00484     rampStartTime[i] = endTime;
00485     endTime += ramps[i].endTime;
00486   }
00487   int i1 = std::upper_bound(rampStartTime.begin(),rampStartTime.end(),t1)-rampStartTime.begin()-1;
00488   int i2 = std::upper_bound(rampStartTime.begin(),rampStartTime.end(),t2)-rampStartTime.begin()-1;
00489   if(i1 == i2) return false;
00490   Real u1 = t1-rampStartTime[i1];
00491   Real u2 = t2-rampStartTime[i2];
00492   assert(u1 >= 0);
00493   assert(u1 <= ramps[i1].endTime+EpsilonT);
00494   assert(u2 >= 0);
00495   assert(u2 <= ramps[i2].endTime+EpsilonT);
00496   u1 = Min(u1,ramps[i1].endTime);
00497   u2 = Min(u2,ramps[i2].endTime);
00498   ParabolicRampND test;
00499   ramps[i1].Evaluate(u1,test.x0);
00500   ramps[i2].Evaluate(u2,test.x1);
00501   ramps[i1].Derivative(u1,test.dx0);
00502   ramps[i2].Derivative(u2,test.dx1);
00503   bool res=test.SolveMinTime(accMax,velMax);
00504   if(!res) return false;
00505   assert(test.endTime >= 0);
00506   assert(test.IsValid());
00507   if(!CheckRamp(test,feas,dist,checkMaxIters)) return false;
00508 
00509   //perform shortcut
00510   //crop i1 and i2
00511   ramps[i1].TrimBack(ramps[i1].endTime-u1);
00512   ramps[i1].x1 = test.x0;
00513   ramps[i1].dx1 = test.dx0;
00514   ramps[i2].TrimFront(u2);
00515   ramps[i2].x0 = test.x1;
00516   ramps[i2].dx0 = test.dx1;
00517   
00518   //test the in-between ramps
00519   ParabolicRampND temp;
00520   temp = ramps[i1];
00521   if(temp.SolveMinTime(accMax,velMax) == false) {
00522     fprintf(stderr,"Warning, truncated ramp can't be re-solved\n");
00523     fprintf(stderr,"Press enter to continue\n");
00524     getchar();
00525   }
00526   temp = ramps[i2];
00527   if(temp.SolveMinTime(accMax,velMax) == false) {
00528     fprintf(stderr,"Warning, truncated ramp can't be re-solved\n");
00529     fprintf(stderr,"Press enter to continue\n");
00530     getchar();
00531   }
00532   
00533   //replace intermediate ramps with test
00534   for(int i=0;i<i2-i1-1;i++)
00535     ramps.erase(ramps.begin()+i1+1);
00536   ramps.insert(ramps.begin()+i1+1,test);
00537   
00538   //check for consistency
00539   for(size_t i=0;i+1<ramps.size();i++) {
00540     assert(ramps[i].x1 == ramps[i+1].x0);
00541     assert(ramps[i].dx1 == ramps[i+1].dx0);
00542   }
00543   return true;
00544 }
00545 
00546 int DynamicPath::Shortcut(int numIters,FeasibilityCheckerBase* space,Real tol)
00547 {
00548   int shortcuts = 0;
00549   vector<Real> rampStartTime(ramps.size()); 
00550   Real endTime=0;
00551   for(size_t i=0;i<ramps.size();i++) {
00552     rampStartTime[i] = endTime;
00553     endTime += ramps[i].endTime;
00554   }
00555   for(int iters=0;iters<numIters;iters++) {
00556     Real t1=Rand()*endTime,t2=Rand()*endTime;
00557     if(t1 > t2) Swap(t1,t2);
00558     int i1 = std::upper_bound(rampStartTime.begin(),rampStartTime.end(),t1)-rampStartTime.begin()-1;
00559     int i2 = std::upper_bound(rampStartTime.begin(),rampStartTime.end(),t2)-rampStartTime.begin()-1;
00560     if(i1 == i2) continue; //same ramp
00561     Real u1 = t1-rampStartTime[i1];
00562     Real u2 = t2-rampStartTime[i2];
00563     assert(u1 >= 0);
00564     assert(u1 <= ramps[i1].endTime+EpsilonT);
00565     assert(u2 >= 0);
00566     assert(u2 <= ramps[i2].endTime+EpsilonT);
00567     u1 = Min(u1,ramps[i1].endTime);
00568     u2 = Min(u2,ramps[i2].endTime);
00569     ParabolicRampND test;
00570     ramps[i1].Evaluate(u1,test.x0);
00571     ramps[i2].Evaluate(u2,test.x1);
00572     ramps[i1].Derivative(u1,test.dx0);
00573     ramps[i2].Derivative(u2,test.dx1);
00574     bool res=test.SolveMinTime(accMax,velMax);
00575     if(!res) continue;
00576     assert(test.endTime >= 0);
00577     assert(test.IsValid());
00578     if(CheckRamp(test,space,tol)) {  //perform shortcut
00579       shortcuts++;
00580       //crop i1 and i2
00581       ramps[i1].TrimBack(ramps[i1].endTime-u1);
00582       ramps[i1].x1 = test.x0;
00583       ramps[i1].dx1 = test.dx0;
00584       ramps[i2].TrimFront(u2);
00585       ramps[i2].x0 = test.x1;
00586       ramps[i2].dx0 = test.dx1;
00587 
00588       //test the in-between ramps
00589       ParabolicRampND temp;
00590       temp = ramps[i1];
00591       if(temp.SolveMinTime(accMax,velMax) == false) {
00592         fprintf(stderr,"Warning, truncated ramp can't be re-solved\n");
00593         fprintf(stderr,"Press enter to continue\n");
00594         getchar();
00595       }
00596       temp = ramps[i2];
00597       if(temp.SolveMinTime(accMax,velMax) == false) {
00598         fprintf(stderr,"Warning, truncated ramp can't be re-solved\n");
00599         fprintf(stderr,"Press enter to continue\n");
00600         getchar();
00601       }
00602 
00603       //replace intermediate ramps with test
00604       for(int i=0;i<i2-i1-1;i++)
00605         ramps.erase(ramps.begin()+i1+1);
00606       ramps.insert(ramps.begin()+i1+1,test);
00607 
00608       //check for consistency
00609       for(size_t i=0;i+1<ramps.size();i++) {
00610         assert(ramps[i].x1 == ramps[i+1].x0);
00611         assert(ramps[i].dx1 == ramps[i+1].dx0);
00612       }
00613 
00614       //revise the timing
00615       rampStartTime.resize(ramps.size());
00616       endTime=0;
00617       for(size_t i=0;i<ramps.size();i++) {
00618         rampStartTime[i] = endTime;
00619         endTime += ramps[i].endTime;
00620       }
00621     }
00622   }
00623   return shortcuts;
00624 }
00625 
00626 int DynamicPath::ShortCircuit(FeasibilityCheckerBase* space,Real tol)
00627 {
00628   int shortcuts=0;
00629   ParabolicRampND test;
00630   for(size_t i=0;i+1<ramps.size();i++) {
00631     test.x0 = ramps[i].x0;
00632     test.dx0 = ramps[i].dx0;
00633     test.x1 = ramps[i+1].x1;    
00634     test.dx1 = ramps[i+1].dx1;    
00635     bool res=test.SolveMinTime(accMax,velMax);
00636     if(!res) continue;
00637     assert(test.endTime >= 0);
00638     assert(test.IsValid());
00639     if(CheckRamp(test,space,tol)) {  //perform shortcut    
00640       ramps[i] = test;
00641       ramps.erase(ramps.begin()+i+1);
00642       i--;
00643       shortcuts++;
00644     }
00645   }
00646   return shortcuts;
00647 }
00648 
00649 
00650 int DynamicPath::Shortcut(int numIters,FeasibilityCheckerBase* space,DistanceCheckerBase* distance)
00651 {
00652   int shortcuts = 0;
00653   vector<Real> rampStartTime(ramps.size()); 
00654   Real endTime=0;
00655   for(size_t i=0;i<ramps.size();i++) {
00656     rampStartTime[i] = endTime;
00657     endTime += ramps[i].endTime;
00658   }
00659   for(int iters=0;iters<numIters;iters++) {
00660     Real t1=Rand()*endTime,t2=Rand()*endTime;
00661     if(t1 > t2) Swap(t1,t2);
00662     int i1 = std::upper_bound(rampStartTime.begin(),rampStartTime.end(),t1)-rampStartTime.begin()-1;
00663     int i2 = std::upper_bound(rampStartTime.begin(),rampStartTime.end(),t2)-rampStartTime.begin()-1;
00664     if(i1 == i2) continue; //same ramp
00665     Real u1 = t1-rampStartTime[i1];
00666     Real u2 = t2-rampStartTime[i2];
00667     assert(u1 >= 0);
00668     assert(u1 <= ramps[i1].endTime+EpsilonT);
00669     assert(u2 >= 0);
00670     assert(u2 <= ramps[i2].endTime+EpsilonT);
00671     u1 = Min(u1,ramps[i1].endTime);
00672     u2 = Min(u2,ramps[i2].endTime);
00673     ParabolicRampND test;
00674     ramps[i1].Evaluate(u1,test.x0);
00675     ramps[i2].Evaluate(u2,test.x1);
00676     ramps[i1].Derivative(u1,test.dx0);
00677     ramps[i2].Derivative(u2,test.dx1);
00678     bool res=test.SolveMinTime(accMax,velMax);
00679     if(!res) continue;
00680     assert(test.endTime >= 0);
00681     assert(test.IsValid());
00682     if(CheckRamp(test,space,distance,checkMaxIters)) {  //perform shortcut
00683       shortcuts++;
00684       //crop i1 and i2
00685       ramps[i1].TrimBack(ramps[i1].endTime-u1);
00686       ramps[i1].x1 = test.x0;
00687       ramps[i1].dx1 = test.dx0;
00688       ramps[i2].TrimFront(u2);
00689       ramps[i2].x0 = test.x1;
00690       ramps[i2].dx0 = test.dx1;
00691 
00692       //test the in-between ramps
00693       ParabolicRampND temp;
00694       temp = ramps[i1];
00695       if(temp.SolveMinTime(accMax,velMax) == false) {
00696         fprintf(stderr,"Warning, truncated ramp can't be re-solved\n");
00697         fprintf(stderr,"Press enter to continue\n");
00698         getchar();
00699       }
00700       temp = ramps[i2];
00701       if(temp.SolveMinTime(accMax,velMax) == false) {
00702         fprintf(stderr,"Warning, truncated ramp can't be re-solved\n");
00703         fprintf(stderr,"Press enter to continue\n");
00704         getchar();
00705       }
00706 
00707       //replace intermediate ramps with test
00708       for(int i=0;i<i2-i1-1;i++)
00709         ramps.erase(ramps.begin()+i1+1);
00710       ramps.insert(ramps.begin()+i1+1,test);
00711 
00712       //check for consistency
00713       for(size_t i=0;i+1<ramps.size();i++) {
00714         assert(ramps[i].x1 == ramps[i+1].x0);
00715         assert(ramps[i].dx1 == ramps[i+1].dx0);
00716       }
00717 
00718       //revise the timing
00719       rampStartTime.resize(ramps.size());
00720       endTime=0;
00721       for(size_t i=0;i<ramps.size();i++) {
00722         rampStartTime[i] = endTime;
00723         endTime += ramps[i].endTime;
00724       }
00725     }
00726   }
00727   return shortcuts;
00728 }
00729 
00730 int DynamicPath::ShortCircuit(FeasibilityCheckerBase* space,DistanceCheckerBase* dist)
00731 {
00732   int shortcuts=0;
00733   ParabolicRampND test;
00734   for(size_t i=0;i+1<ramps.size();i++) {
00735     test.x0 = ramps[i].x0;
00736     test.dx0 = ramps[i].dx0;
00737     test.x1 = ramps[i+1].x1;    
00738     test.dx1 = ramps[i+1].dx1;    
00739     bool res=test.SolveMinTime(accMax,velMax);
00740     if(!res) continue;
00741     assert(test.endTime >= 0);
00742     assert(test.IsValid());
00743     if(CheckRamp(test,space,dist,checkMaxIters)) {  //perform shortcut    
00744       ramps[i] = test;
00745       ramps.erase(ramps.begin()+i+1);
00746       i--;
00747       shortcuts++;
00748     }
00749   }
00750   return shortcuts;
00751 }
00752 
00753