DynamicPath.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/DynamicPath.h"
00032 #include "constraint_aware_spline_smoother/ParabolicPathSmooth/Timer.h"
00033 #include "constraint_aware_spline_smoother/ParabolicPathSmooth/Config.h"
00034 #include "constraint_aware_spline_smoother/ParabolicPathSmooth/Math.h"
00035 #include <assert.h>
00036 #include <stdlib.h>
00037 #include <stdio.h>
00038 #include <list>
00039 #include <algorithm>
00040 using namespace std;
00041 
00042 namespace ParabolicRamp {
00043 
00044 inline Real LInfDistance(const Vector& a,const Vector& b)
00045 {
00046   PARABOLIC_RAMP_ASSERT(a.size()==b.size());
00047   Real d=0;
00048   for(size_t i=0;i<a.size();i++)
00049     d = Max(d,Abs(a[i]-b[i]));
00050   return d;
00051 }
00052 
00053 inline bool InBounds(const Vector& x,const Vector& bmin,const Vector& bmax)
00054 {
00055   PARABOLIC_RAMP_ASSERT(x.size()==bmin.size());
00056   PARABOLIC_RAMP_ASSERT(x.size()==bmax.size());
00057   for(size_t i=0;i<x.size();i++)
00058     if(x[i] < bmin[i] || x[i] > bmax[i]) return false;
00059   return true;
00060 }
00061 
00062 inline Real MaxBBLInfDistance(const Vector& x,const Vector& bmin,const Vector& bmax)
00063 {
00064   PARABOLIC_RAMP_ASSERT(x.size()==bmin.size());
00065   PARABOLIC_RAMP_ASSERT(x.size()==bmax.size());
00066   Real d=0;
00067   for(size_t i=0;i<x.size();i++)
00068     d = Max(d,Max(Abs(x[i]-bmin[i]),Abs(x[i]-bmax[i])));
00069   return d;
00070 }
00071 
00072 inline bool SolveMinTime(const Vector& x0,const Vector& dx0,const Vector& x1,const Vector& dx1,
00073                   const Vector& accMax,const Vector& velMax,const Vector& xMin,const Vector& xMax,DynamicPath& out)
00074 {
00075   if(xMin.empty()) {
00076     out.ramps.resize(1);
00077     ParabolicRampND& temp=out.ramps[0];
00078     temp.x0=x0;
00079     temp.x1=x1;
00080     temp.dx0=dx0;
00081     temp.dx1=dx1;
00082     bool res=temp.SolveMinTime(accMax,velMax);
00083     if(!res) return false;
00084   }
00085   else {
00086     vector<std::vector<ParabolicRamp1D> > ramps;
00087     Real res=SolveMinTimeBounded(x0,dx0,x1,dx1,
00088                                  accMax,velMax,xMin,xMax,
00089                                  ramps);
00090     if(res < 0) return false;
00091     CombineRamps(ramps,out.ramps);
00092   }
00093   PARABOLIC_RAMP_ASSERT(out.IsValid());
00094   return true;
00095 }
00096 
00097 DynamicPath::DynamicPath()
00098 {}
00099 
00100 void DynamicPath::Init(const Vector& _velMax,const Vector& _accMax)
00101 {
00102   velMax = _velMax;
00103   accMax = _accMax;
00104   PARABOLIC_RAMP_ASSERT(velMax.size() == accMax.size());
00105   if(!velMax.empty() && !xMin.empty()) PARABOLIC_RAMP_ASSERT(xMin.size() == velMax.size());
00106 }
00107 
00108 void DynamicPath::SetJointLimits(const Vector& _xMin,const Vector& _xMax)
00109 {
00110   xMin = _xMin;
00111   xMax = _xMax;
00112   PARABOLIC_RAMP_ASSERT(xMin.size() == xMax.size());
00113   if(!velMax.empty() && !xMin.empty()) PARABOLIC_RAMP_ASSERT(xMin.size() == velMax.size());
00114 }
00115 
00116 Real DynamicPath::GetTotalTime() const
00117 {
00118   Real t=0;
00119   for(size_t i=0;i<ramps.size();i++) t+=ramps[i].endTime;
00120   return t;
00121 }
00122 
00123 int DynamicPath::GetSegment(Real t,Real& u) const
00124 {
00125   if(t < 0) return -1;
00126   for(size_t i=0;i<ramps.size();i++) {
00127     if(t <= ramps[i].endTime) {
00128       u = t;
00129       return (int)i;
00130     }
00131     t -= ramps[i].endTime;
00132   }
00133   u = t;
00134   return (int)ramps.size();
00135 }
00136 
00137 void DynamicPath::Evaluate(Real t,Vector& x) const
00138 {
00139   PARABOLIC_RAMP_ASSERT(!ramps.empty());
00140   if(t < 0) {
00141     x = ramps.front().x0;
00142   }
00143   else {
00144     for(size_t i=0;i<ramps.size();i++) {
00145       if(t <= ramps[i].endTime) {
00146         ramps[i].Evaluate(t,x);
00147         return;
00148       }
00149       t -= ramps[i].endTime;
00150     }
00151     x = ramps.back().x1;
00152   }
00153 }
00154 
00155 void DynamicPath::Derivative(Real t,Vector& dx) const
00156 {
00157   PARABOLIC_RAMP_ASSERT(!ramps.empty());
00158   if(t < 0) {
00159     dx = ramps.front().dx0;
00160   }
00161   else {
00162     for(size_t i=0;i<ramps.size();i++) {
00163       if(t <= ramps[i].endTime) {
00164         ramps[i].Derivative(t,dx);
00165         return;
00166       }
00167       t -= ramps[i].endTime;
00168     }
00169     dx = ramps.back().dx1;
00170   }
00171 }
00172 
00173 void DynamicPath::SetMilestones(const vector<Vector>& x)
00174 {
00175   if(x.empty()) {
00176     ramps.resize(0);
00177   }
00178   else if(x.size()==1) {
00179     ramps.resize(1);
00180     ramps[0].SetConstant(x[0]);
00181   }
00182   else {
00183     Vector zero(x[0].size(),0.0);
00184     ramps.resize(x.size()-1);
00185     for(size_t i=0;i<ramps.size();i++) {
00186       ramps[i].x0 = x[i];
00187       ramps[i].x1 = x[i+1];
00188       ramps[i].dx0 = zero;
00189       ramps[i].dx1 = zero;
00190       bool res=ramps[i].SolveMinTimeLinear(accMax,velMax);
00191       PARABOLIC_RAMP_ASSERT(res);
00192     }
00193   }
00194 }
00195 
00196 void DynamicPath::SetMilestones(const vector<Vector>& x,const vector<Vector>& dx)
00197 {
00198   if(x.empty()) {
00199     ramps.resize(0);
00200   }
00201   else if(x.size()==1) {
00202     ramps.resize(1);
00203     ramps[0].SetConstant(x[0]);
00204   }
00205   else {
00206     if(xMin.empty()) {
00207       ramps.resize(x.size()-1);
00208       for(size_t i=0;i<ramps.size();i++) {
00209         ramps[i].x0 = x[i];
00210         ramps[i].x1 = x[i+1];
00211         ramps[i].dx0 = dx[i];
00212         ramps[i].dx1 = dx[i+1];
00213         bool res=ramps[i].SolveMinTime(accMax,velMax);
00214         PARABOLIC_RAMP_ASSERT(res);
00215       }
00216     }
00217     else {
00218       //bounded solving
00219       ramps.resize(0);
00220       ramps.reserve(x.size()-1);
00221       std::vector<std::vector<ParabolicRamp1D> > tempRamps;
00222       std::vector<ParabolicRampND> tempRamps2;
00223       PARABOLIC_RAMP_ASSERT(InBounds(x[0],xMin,xMax));
00224       for(size_t i=0;i+1<x.size();i++) {
00225         PARABOLIC_RAMP_ASSERT(InBounds(x[i+1],xMin,xMax));
00226         Real res=SolveMinTimeBounded(x[i],dx[i],x[i+1],dx[i+1],
00227                                      accMax,velMax,xMin,xMax,
00228                                      tempRamps);
00229         PARABOLIC_RAMP_ASSERT(res >= 0);
00230         CombineRamps(tempRamps,tempRamps2);
00231         ramps.insert(ramps.end(),tempRamps2.begin(),tempRamps2.end());
00232       }
00233     }
00234   }
00235 }
00236 
00237 void DynamicPath::GetMilestones(vector<Vector>& x,vector<Vector>& dx) const
00238 {
00239   if(ramps.empty()) {
00240     x.resize(0);
00241     dx.resize(0);
00242     return;
00243   }
00244   x.resize(ramps.size()+1);
00245   dx.resize(ramps.size()+1);
00246   x[0] = ramps[0].x0;
00247   dx[0] = ramps[0].dx0;
00248   for(size_t i=0;i<ramps.size();i++) {
00249     x[i+1] = ramps[i].x1;
00250     dx[i+1] = ramps[i].dx1;
00251   }
00252 }
00253 
00254 void DynamicPath::Append(const Vector& x)
00255 {
00256   size_t n=ramps.size();
00257   size_t p=n-1;
00258   if(ramps.size()==0) {
00259     ramps.resize(1);
00260     ramps[0].SetConstant(x);
00261   }
00262   else {
00263     if(xMin.empty()) {
00264       ramps.resize(ramps.size()+1);
00265       ramps[n].x0 = ramps[p].x1;
00266       ramps[n].dx0 = ramps[p].dx1;
00267       ramps[n].x1 = x;
00268       ramps[n].dx1.resize(x.size());
00269       fill(ramps[n].dx1.begin(),ramps[n].dx1.end(),0);
00270       bool res=ramps[n].SolveMinTime(accMax,velMax);
00271       PARABOLIC_RAMP_ASSERT(res);
00272     }
00273     else {
00274       PARABOLIC_RAMP_ASSERT(InBounds(x,xMin,xMax));
00275       std::vector<std::vector<ParabolicRamp1D> > tempRamps;
00276       std::vector<ParabolicRampND> tempRamps2;
00277       Vector zero(x.size(),0.0);
00278       Real res=SolveMinTimeBounded(ramps[p].x1,ramps[p].dx1,x,zero,
00279                                    accMax,velMax,xMin,xMax,tempRamps);
00280       PARABOLIC_RAMP_ASSERT(res>=0);
00281       CombineRamps(tempRamps,tempRamps2);
00282       ramps.insert(ramps.end(),tempRamps2.begin(),tempRamps2.end());
00283     }
00284   }
00285 }
00286 
00287 void DynamicPath::Append(const Vector& x,const Vector& dx)
00288 {
00289   size_t n=ramps.size();
00290   size_t p=n-1;
00291   PARABOLIC_RAMP_ASSERT(ramps.size()!=0);
00292   if(xMin.empty()) {
00293     ramps.resize(ramps.size()+1);
00294     ramps[n].x0 = ramps[p].x1;
00295     ramps[n].dx0 = ramps[p].dx1;
00296     ramps[n].x1 = x;
00297     ramps[n].dx1 = dx;
00298     bool res=ramps[n].SolveMinTime(accMax,velMax);
00299     PARABOLIC_RAMP_ASSERT(res);
00300   }
00301   else {
00302     PARABOLIC_RAMP_ASSERT(InBounds(x,xMin,xMax));
00303     std::vector<std::vector<ParabolicRamp1D> > tempRamps;
00304     std::vector<ParabolicRampND> tempRamps2;
00305     Real res=SolveMinTimeBounded(ramps[p].x1,ramps[p].dx1,x,dx,
00306                                  accMax,velMax,xMin,xMax,tempRamps);
00307     PARABOLIC_RAMP_ASSERT(res>=0);
00308     CombineRamps(tempRamps,tempRamps2);
00309     ramps.insert(ramps.end(),tempRamps2.begin(),tempRamps2.end());
00310   }
00311 }
00312 
00313 void DynamicPath::Concat(const DynamicPath& suffix)
00314 {
00315   PARABOLIC_RAMP_ASSERT(&suffix != this);
00316   if(suffix.ramps.empty()) return;
00317   if(ramps.empty()) {
00318     *this=suffix;
00319     return;
00320   }
00321   //double check continuity
00322   if(ramps.back().x1 != suffix.ramps.front().x0 || 
00323      ramps.back().dx1 != suffix.ramps.front().dx0) {
00324     Real xmax=0,dxmax=0;
00325     for(size_t i=0;i<ramps.back().x1.size();i++) {
00326       xmax=Max(xmax,Abs(ramps.back().x1[i]-suffix.ramps.front().x0[i]));
00327       dxmax=Max(dxmax,Abs(ramps.back().dx1[i]-suffix.ramps.front().dx0[i]));
00328     }
00329     if(Abs(xmax) > EpsilonX || Abs(dxmax) > EpsilonV) {
00330       printf("Concat endpoint error\n");
00331       printf("x:\n");
00332       for(size_t i=0;i<ramps.back().x1.size();i++)
00333         printf("%g - %g = %g\n",ramps.back().x1[i],suffix.ramps.front().x0[i],ramps.back().x1[i]-suffix.ramps.front().x0[i]);
00334       printf("dx:\n");
00335       for(size_t i=0;i<ramps.back().x1.size();i++)
00336         printf("%g - %g = %g\n",ramps.back().dx1[i],suffix.ramps.front().dx0[i],ramps.back().dx1[i]-suffix.ramps.front().dx0[i]);
00337       getchar();
00338     }
00339     ramps.back().x1 = ramps.front().x0;
00340     ramps.back().dx1 = ramps.front().dx0;
00341     for(size_t i=0;i<ramps.back().x1.size();i++) {
00342       ramps.back().ramps[i].x1 = suffix.ramps.front().x0[i];
00343       ramps.back().ramps[i].dx1 = suffix.ramps.front().dx0[i];
00344     }
00345   }
00346   PARABOLIC_RAMP_ASSERT(ramps.back().x1 == suffix.ramps.front().x0);
00347   PARABOLIC_RAMP_ASSERT(ramps.back().dx1 == suffix.ramps.front().dx0);
00348   ramps.insert(ramps.end(),suffix.ramps.begin(),suffix.ramps.end());
00349 }
00350 
00351 void DynamicPath::Split(Real t,DynamicPath& before,DynamicPath& after) const
00352 {
00353   PARABOLIC_RAMP_ASSERT(IsValid());
00354   PARABOLIC_RAMP_ASSERT(&before != this);
00355   PARABOLIC_RAMP_ASSERT(&after != this);
00356   if(ramps.empty()) {
00357     before=*this;
00358     after=*this;
00359     return;
00360   }
00361   after.velMax = before.velMax = velMax;
00362   after.accMax = before.accMax = accMax;
00363   after.xMin = before.xMin = xMin;
00364   after.xMax = before.xMax = xMax;
00365   after.ramps.resize(0);
00366   before.ramps.resize(0);
00367   if(t < 0) {  //we're before the path starts
00368     before.ramps.resize(1);
00369     before.ramps[0].SetConstant(ramps[0].x0);
00370     //place a constant for time -t on the after path
00371     after.ramps.resize(1);
00372     after.ramps[0].SetConstant(ramps[0].x0,-t);
00373   }
00374   for(size_t i=0;i<ramps.size();i++) {
00375     if(t < 0) {
00376       after.ramps.push_back(ramps[i]);
00377     }
00378     else {
00379       if(t < ramps[i].endTime) {
00380         //cut current path
00381         ParabolicRampND temp=ramps[i];
00382         temp.TrimBack(temp.endTime-t);
00383         before.ramps.push_back(temp);
00384         temp=ramps[i];
00385         temp.TrimFront(t);
00386         if(!after.ramps.empty()) {
00387           printf("DynamicPath::Split: Uh... weird, after is not empty?\n");
00388           printf("t = %g, i = %d, endtime = %g\n",t,i,ramps[i].endTime);
00389         }
00390         PARABOLIC_RAMP_ASSERT(after.ramps.size() == 0);
00391         after.ramps.push_back(temp);
00392       }
00393       else {
00394         before.ramps.push_back(ramps[i]);
00395       }
00396       t -= ramps[i].endTime;
00397     }
00398   }
00399 
00400   if(t > 0) {  //dt is longer than path
00401     ParabolicRampND temp;
00402     temp.SetConstant(ramps.back().x1,t);
00403     before.ramps.push_back(temp);
00404   }
00405   if(t >= 0) {
00406     ParabolicRampND temp;
00407     temp.SetConstant(ramps.back().x1);
00408     after.ramps.push_back(temp);
00409   }
00410   PARABOLIC_RAMP_ASSERT(before.IsValid());
00411   PARABOLIC_RAMP_ASSERT(after.IsValid());
00412 }
00413 
00414 
00415 
00416 
00417 struct RampSection
00418 {
00419   Real ta,tb;
00420   Vector xa,xb;
00421   Real da,db;
00422 };
00423 
00424 
00425 
00426     
00427 
00428 bool CheckRamp(const ParabolicRampND& ramp,FeasibilityCheckerBase* feas,DistanceCheckerBase* distance,int maxiters)
00429 {
00430   if(!feas->ConfigFeasible(ramp.x0)) return false;
00431   if(!feas->ConfigFeasible(ramp.x1)) return false;
00432   PARABOLIC_RAMP_ASSERT(distance->ObstacleDistanceNorm()==Inf);
00433   RampSection section;
00434   section.ta = 0;
00435   section.tb = ramp.endTime;
00436   section.xa = ramp.x0;
00437   section.xb = ramp.x1;
00438   section.da = distance->ObstacleDistance(ramp.x0);
00439   section.db = distance->ObstacleDistance(ramp.x1);
00440   if(section.da <= 0.0) return false;
00441   if(section.db <= 0.0) return false;
00442   list<RampSection> queue;
00443   queue.push_back(section);
00444   int iters=0;
00445   while(!queue.empty()) {
00446     section = queue.front();
00447     queue.erase(queue.begin());
00448 
00449     //check the bounds around this section
00450     if(LInfDistance(section.xa,section.xb) <= section.da+section.db) {
00451       Vector bmin,bmax;
00452       ramp.Bounds(section.ta,section.tb,bmin,bmax);
00453       if(MaxBBLInfDistance(section.xa,bmin,bmax) < section.da && 
00454          MaxBBLInfDistance(section.xb,bmin,bmax) < section.db)
00455         //can cull out the section
00456         continue;
00457     }
00458     Real tc = (section.ta+section.tb)*0.5;
00459     Vector xc;
00460     ramp.Evaluate(tc,xc);
00461     if(!feas->ConfigFeasible(xc)) return false;  //infeasible config
00462     //subdivide
00463     Real dc = distance->ObstacleDistance(xc);
00464     RampSection sa,sb;
00465     sa.ta = section.ta;
00466     sa.xa = section.xa;
00467     sa.da = section.da;
00468     sa.tb = sb.ta = tc;
00469     sa.xb = sb.xa = xc;
00470     sa.db = sb.da = dc;
00471     sb.tb = section.tb;
00472     sb.xb = section.xb;
00473     sb.db = section.db;
00474 
00475     //recurse on segments
00476     queue.push_back(sa);
00477     queue.push_back(sb);
00478 
00479     if(iters++ >= maxiters) return false;
00480   }
00481   return true;
00482 }
00483 
00484 bool CheckRamp(const ParabolicRampND& ramp,FeasibilityCheckerBase* space,Real tol)
00485 {
00486   PARABOLIC_RAMP_ASSERT(tol > 0);
00487   if(!space->ConfigFeasible(ramp.x0)) return false;
00488   if(!space->ConfigFeasible(ramp.x1)) return false;
00489   //PARABOLIC_RAMP_ASSERT(space->ConfigFeasible(ramp.x0));
00490   //PARABOLIC_RAMP_ASSERT(space->ConfigFeasible(ramp.x1));
00491 
00492   //for a parabola of form f(x) = a x^2 + b x, and the straight line 
00493   //of form g(X,u) = u*f(X)
00494   //d^2(g(X,u),p) = |p - <f(X),p>/<f(X),f(X)> f(X)|^2 < tol^2
00495   //<p,p> - <f(X),p>^2/<f(X),f(X)>  = p^T (I-f(X)f(X)^T/f(X)^T f(X)) p
00496   //max_x d^2(f(x)) => f(x)^T (I-f(X)f(X)^T/f(X)^T f(X)) f'(x) = 0
00497   //(x^2 a^T + x b^T) A (2a x + b) = 0
00498   //(x a^T + b^T) A (2a x + b) = 0
00499   //2 x^2 a^T A a + 3 x b^T A a + b^T A b = 0
00500 
00501   //the max X for which f(x) deviates from g(X,x) by at most tol is...
00502   //max_x |g(X,x)-f(x)| = max_x x/X f(X)-f(x)
00503   //=> f(X)/X - f'(x) = 0
00504   //=>  X/2 = x 
00505   //=> max_x |g(X,x)-f(x)| = |(X/2)/X f(X)-f(X/2)|
00506   //= |1/2 (aX^2+bX) - a(X/2)^2 - b(X/2) + c |
00507   //= |a| X^2 / 4
00508   //so... max X st max_x |g(X,x)-f(x)| < tol => X = 2*sqrt(tol/|a|)
00509   vector<Real> divs;
00510   Real t=0;
00511   divs.push_back(t);
00512   while(t < ramp.endTime) {
00513     Real tnext=t;
00514     Real amax = 0;
00515     Real switchNext=ramp.endTime;
00516     for(size_t i=0;i<ramp.ramps.size();i++) {
00517       if(t < ramp.ramps[i].tswitch1) {  //ramp up
00518         switchNext =  Min(switchNext, ramp.ramps[i].tswitch1);
00519         amax = Max(amax,Max(Abs(ramp.ramps[i].a1),Abs(ramp.ramps[i].a2)));
00520       }
00521       else if(t < ramp.ramps[i].tswitch2) {  //constant vel
00522         switchNext = Min(switchNext, ramp.ramps[i].tswitch2);
00523       }
00524       else if(t < ramp.ramps[i].ttotal) {  //ramp down
00525         amax = Max(amax,Max(Abs(ramp.ramps[i].a1),Abs(ramp.ramps[i].a2)));     
00526       }
00527     }
00528     Real dt = 2.0*Sqrt(tol/amax);
00529     if(t+dt > switchNext) tnext = switchNext;
00530     else tnext = t+dt;
00531 
00532     t = tnext;
00533     divs.push_back(tnext);
00534   }
00535   divs.push_back(ramp.endTime);
00536 
00537   //do a bisection thingie
00538   list<pair<int,int> > segs;
00539   segs.push_back(pair<int,int>(0,divs.size()-1));
00540   Vector q1,q2;
00541   while(!segs.empty()) {
00542     int i=segs.front().first;
00543     int j=segs.front().second;
00544     segs.erase(segs.begin());
00545     if(j == i+1) {
00546       //check path from t to tnext
00547       ramp.Evaluate(divs[i],q1);
00548       ramp.Evaluate(divs[j],q2);
00549       if(!space->SegmentFeasible(q1,q2)) return false;
00550     }
00551     else {
00552       int k=(i+j)/2;
00553       ramp.Evaluate(divs[k],q1);
00554       if(!space->ConfigFeasible(q1)) return false;
00555       segs.push_back(pair<int,int>(i,k));
00556       segs.push_back(pair<int,int>(k,j));
00557     }
00558   }
00559   return true;
00560 }
00561 
00562 RampFeasibilityChecker::RampFeasibilityChecker(FeasibilityCheckerBase* _feas,Real _tol)
00563   :feas(_feas),tol(_tol),distance(NULL),maxiters(0)
00564 {}
00565 
00566 RampFeasibilityChecker::RampFeasibilityChecker(FeasibilityCheckerBase* _feas,DistanceCheckerBase* _distance,int _maxiters)
00567   :feas(_feas),tol(0),distance(_distance),maxiters(_maxiters)
00568 {}
00569 
00570 bool RampFeasibilityChecker::Check(const ParabolicRampND& x)
00571 {
00572   if(distance) return CheckRamp(x,feas,distance,maxiters);
00573   else return CheckRamp(x,feas,tol);
00574 }
00575 
00576 
00577 bool DynamicPath::TryShortcut(Real t1,Real t2,RampFeasibilityChecker& check)
00578 {
00579   if(t1 > t2) Swap(t1,t2);
00580   Real u1,u2;
00581   int i1 = GetSegment(t1,u1);
00582   int i2 = GetSegment(t2,u2);
00583   if(i1 == i2) return false;
00584   PARABOLIC_RAMP_ASSERT(u1 >= 0);
00585   PARABOLIC_RAMP_ASSERT(u1 <= ramps[i1].endTime+EpsilonT);
00586   PARABOLIC_RAMP_ASSERT(u2 >= 0);
00587   PARABOLIC_RAMP_ASSERT(u2 <= ramps[i2].endTime+EpsilonT);
00588   u1 = Min(u1,ramps[i1].endTime);
00589   u2 = Min(u2,ramps[i2].endTime);
00590   DynamicPath intermediate;
00591   if(xMin.empty()) {
00592     intermediate.ramps.resize(1);
00593     ParabolicRampND& test=intermediate.ramps[0];
00594     ramps[i1].Evaluate(u1,test.x0);
00595     ramps[i2].Evaluate(u2,test.x1);
00596     ramps[i1].Derivative(u1,test.dx0);
00597     ramps[i2].Derivative(u2,test.dx1);
00598     bool res=test.SolveMinTime(accMax,velMax);
00599     if(!res) return false;
00600     PARABOLIC_RAMP_ASSERT(test.endTime >= 0);
00601     PARABOLIC_RAMP_ASSERT(test.IsValid());
00602   }
00603   else {
00604     Vector x0,x1,dx0,dx1;
00605     ramps[i1].Evaluate(u1,x0);
00606     ramps[i2].Evaluate(u2,x1);
00607     ramps[i1].Derivative(u1,dx0);
00608     ramps[i2].Derivative(u2,dx1);
00609     vector<std::vector<ParabolicRamp1D> > intramps;
00610     Real res=SolveMinTimeBounded(x0,dx0,x1,dx1,
00611                       accMax,velMax,xMin,xMax,
00612                       intramps);
00613     if(res < 0) return false;
00614     CombineRamps(intramps,intermediate.ramps);
00615     intermediate.accMax = accMax;
00616     intermediate.velMax = velMax;
00617     PARABOLIC_RAMP_ASSERT(intermediate.IsValid());
00618   }
00619   for(size_t i=0;i<intermediate.ramps.size();i++)
00620     if(!check.Check(intermediate.ramps[i])) return false;
00621 
00622   //perform shortcut
00623   //crop i1 and i2
00624   ramps[i1].TrimBack(ramps[i1].endTime-u1);
00625   ramps[i1].x1 = intermediate.ramps.front().x0;
00626   ramps[i1].dx1 = intermediate.ramps.front().dx0;
00627   ramps[i2].TrimFront(u2);
00628   ramps[i2].x0 = intermediate.ramps.back().x1;
00629   ramps[i2].dx0 = intermediate.ramps.back().dx1;
00630   
00631   //replace intermediate ramps with test
00632   for(int i=0;i<i2-i1-1;i++)
00633     ramps.erase(ramps.begin()+i1+1);
00634   ramps.insert(ramps.begin()+i1+1,intermediate.ramps.begin(),intermediate.ramps.end());
00635   
00636   //check for consistency
00637   for(size_t i=0;i+1<ramps.size();i++) {
00638     PARABOLIC_RAMP_ASSERT(ramps[i].x1 == ramps[i+1].x0);
00639     PARABOLIC_RAMP_ASSERT(ramps[i].dx1 == ramps[i+1].dx0);
00640   }
00641   return true;
00642 }
00643 
00644 int DynamicPath::Shortcut(int numIters,RampFeasibilityChecker& check)
00645 {
00646   RandomNumberGeneratorBase rng;
00647   return Shortcut(numIters,check,&rng);
00648 }
00649 
00650 int DynamicPath::Shortcut(int numIters,RampFeasibilityChecker& check,RandomNumberGeneratorBase* rng)
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   Vector x0,x1,dx0,dx1;
00660   DynamicPath intermediate;
00661   intermediate.Init(velMax,accMax);
00662   intermediate.SetJointLimits(xMin,xMax);
00663   for(int iters=0;iters<numIters;iters++) {
00664     Real t1=rng->Rand()*endTime,t2=rng->Rand()*endTime;
00665     if(t1 > t2) Swap(t1,t2);
00666     int i1 = std::upper_bound(rampStartTime.begin(),rampStartTime.end(),t1)-rampStartTime.begin()-1;
00667     int i2 = std::upper_bound(rampStartTime.begin(),rampStartTime.end(),t2)-rampStartTime.begin()-1;
00668     if(i1 == i2) continue; //same ramp
00669     Real u1 = t1-rampStartTime[i1];
00670     Real u2 = t2-rampStartTime[i2];
00671     PARABOLIC_RAMP_ASSERT(u1 >= 0);
00672     PARABOLIC_RAMP_ASSERT(u1 <= ramps[i1].endTime+EpsilonT);
00673     PARABOLIC_RAMP_ASSERT(u2 >= 0);
00674     PARABOLIC_RAMP_ASSERT(u2 <= ramps[i2].endTime+EpsilonT);
00675     u1 = Min(u1,ramps[i1].endTime);
00676     u2 = Min(u2,ramps[i2].endTime);
00677     ramps[i1].Evaluate(u1,x0);
00678     ramps[i2].Evaluate(u2,x1);
00679     ramps[i1].Derivative(u1,dx0);
00680     ramps[i2].Derivative(u2,dx1);
00681     bool res=SolveMinTime(x0,dx0,x1,dx1,accMax,velMax,xMin,xMax,intermediate);
00682     if(!res) continue;
00683     bool feas=true;
00684     for(size_t i=0;i<intermediate.ramps.size();i++)
00685       if(!check.Check(intermediate.ramps[i])) {
00686         feas=false;
00687         break;
00688       }
00689     if(!feas) continue;
00690     //perform shortcut
00691     shortcuts++;
00692     ramps[i1].TrimBack(ramps[i1].endTime-u1);
00693     ramps[i1].x1 = intermediate.ramps.front().x0;
00694     ramps[i1].dx1 = intermediate.ramps.front().dx0;
00695     ramps[i2].TrimFront(u2);
00696     ramps[i2].x0 = intermediate.ramps.back().x1;
00697     ramps[i2].dx0 = intermediate.ramps.back().dx1;
00698     
00699     //replace intermediate ramps 
00700     for(int i=0;i<i2-i1-1;i++)
00701       ramps.erase(ramps.begin()+i1+1);
00702     ramps.insert(ramps.begin()+i1+1,intermediate.ramps.begin(),intermediate.ramps.end());
00703 
00704     //check for consistency
00705     for(size_t i=0;i+1<ramps.size();i++) {
00706       PARABOLIC_RAMP_ASSERT(ramps[i].x1 == ramps[i+1].x0);
00707       PARABOLIC_RAMP_ASSERT(ramps[i].dx1 == ramps[i+1].dx0);
00708     }
00709     
00710     //revise the timing
00711     rampStartTime.resize(ramps.size());
00712     endTime=0;
00713     for(size_t i=0;i<ramps.size();i++) {
00714       rampStartTime[i] = endTime;
00715       endTime += ramps[i].endTime;
00716     }
00717   }
00718   return shortcuts;
00719 }
00720 
00721 int DynamicPath::ShortCircuit(RampFeasibilityChecker& check)
00722 {
00723   int shortcuts=0;
00724   DynamicPath intermediate;
00725   for(int i=0;i+1<(int)ramps.size();i++) {
00726     bool res=SolveMinTime(ramps[i].x0,ramps[i].dx0,ramps[i].x1,ramps[i].dx1,accMax,velMax,xMin,xMax,intermediate);
00727     if(!res) continue;
00728     bool feas=true;
00729     for(size_t j=0;j<intermediate.ramps.size();j++)
00730       if(!check.Check(intermediate.ramps[j])) {
00731         feas=false;
00732         break;
00733       }
00734     if(!feas) continue;
00735 
00736     ramps.erase(ramps.begin()+i+1);
00737     ramps.insert(ramps.begin()+i+1,intermediate.ramps.begin(),intermediate.ramps.end());
00738     i += (int)intermediate.ramps.size()-2;
00739     shortcuts++;
00740   }
00741   return shortcuts;
00742 }
00743 
00744 
00745 int DynamicPath::OnlineShortcut(Real leadTime,Real padTime,RampFeasibilityChecker& check)
00746 {
00747   RandomNumberGeneratorBase rng;
00748   return OnlineShortcut(leadTime,padTime,check,&rng);
00749 }
00750 
00751 int DynamicPath::OnlineShortcut(Real leadTime,Real padTime,RampFeasibilityChecker& check,RandomNumberGeneratorBase* rng)
00752 {
00753   Timer timer;
00754   int shortcuts = 0;
00755   vector<Real> rampStartTime(ramps.size()); 
00756   Real endTime=0;
00757   for(size_t i=0;i<ramps.size();i++) {
00758     rampStartTime[i] = endTime;
00759     endTime += ramps[i].endTime;
00760   }
00761   Vector x0,x1,dx0,dx1;
00762   DynamicPath intermediate;
00763   while(1) {
00764     //can only start from here
00765     Real starttime = timer.ElapsedTime()-leadTime;
00766     if(starttime+padTime >= endTime) break;
00767     starttime = Max(0.0,starttime+padTime);
00768 
00769     Real t1=starttime+Sqr(rng->Rand())*(endTime-starttime),t2=starttime+rng->Rand()*(endTime-starttime);
00770     if(t1 > t2) Swap(t1,t2);
00771     int i1 = std::upper_bound(rampStartTime.begin(),rampStartTime.end(),t1)-rampStartTime.begin()-1;
00772     int i2 = std::upper_bound(rampStartTime.begin(),rampStartTime.end(),t2)-rampStartTime.begin()-1;
00773     if(i1 == i2) continue; //same ramp
00774     Real u1 = t1-rampStartTime[i1];
00775     Real u2 = t2-rampStartTime[i2];
00776     PARABOLIC_RAMP_ASSERT(u1 >= 0);
00777     PARABOLIC_RAMP_ASSERT(u1 <= ramps[i1].endTime+EpsilonT);
00778     PARABOLIC_RAMP_ASSERT(u2 >= 0);
00779     PARABOLIC_RAMP_ASSERT(u2 <= ramps[i2].endTime+EpsilonT);
00780     u1 = Min(u1,ramps[i1].endTime);
00781     u2 = Min(u2,ramps[i2].endTime);
00782     ramps[i1].Evaluate(u1,x0);
00783     ramps[i2].Evaluate(u2,x1);
00784     ramps[i1].Derivative(u1,dx0);
00785     ramps[i2].Derivative(u2,dx1);
00786     bool res=SolveMinTime(x0,dx0,x1,dx1,accMax,velMax,xMin,xMax,intermediate);
00787     if(!res) continue;
00788     bool feas=true;
00789     for(size_t i=0;i<intermediate.ramps.size();i++)
00790       if(!check.Check(intermediate.ramps[i])) {
00791         feas=false;
00792         break;
00793       }
00794     if(!feas) continue;
00795     //check for time elapse, otherwise can't perform this shortcut
00796     if(timer.ElapsedTime()-leadTime > t1) continue; 
00797     
00798     shortcuts++;
00799     //crop i1 and i2
00800     ramps[i1].TrimBack(ramps[i1].endTime-u1);
00801     ramps[i1].x1 = intermediate.ramps.front().x0;
00802     ramps[i1].dx1 = intermediate.ramps.front().dx0;
00803     ramps[i2].TrimFront(u2);
00804     ramps[i2].x0 = intermediate.ramps.back().x1;
00805     ramps[i2].dx0 = intermediate.ramps.back().dx1;
00806     PARABOLIC_RAMP_ASSERT(ramps[i1].IsValid());
00807     PARABOLIC_RAMP_ASSERT(ramps[i2].IsValid());
00808 
00809     //replace intermediate ramps 
00810     for(int i=0;i<i2-i1-1;i++)
00811       ramps.erase(ramps.begin()+i1+1);
00812     ramps.insert(ramps.begin()+i1+1,intermediate.ramps.begin(),intermediate.ramps.end());
00813 
00814     //check for consistency
00815     for(size_t i=0;i+1<ramps.size();i++) {
00816       PARABOLIC_RAMP_ASSERT(ramps[i].x1 == ramps[i+1].x0);
00817       PARABOLIC_RAMP_ASSERT(ramps[i].dx1 == ramps[i+1].dx0);
00818     }
00819     
00820     //revise the timing
00821     rampStartTime.resize(ramps.size());
00822     endTime=0;
00823     for(size_t i=0;i<ramps.size();i++) {
00824       rampStartTime[i] = endTime;
00825       endTime += ramps[i].endTime;
00826     }
00827   }
00828   return shortcuts;
00829 }
00830 
00831 bool DynamicPath::IsValid() const
00832 {
00833   if(ramps.empty()) {
00834     fprintf(stderr,"DynamicPath::IsValid: empty path\n");
00835     return false;
00836   }
00837   for(size_t i=0;i<ramps.size();i++) {
00838     if(!ramps[i].IsValid()) {
00839       fprintf(stderr,"DynamicPath::IsValid: ramp %d is invalid\n",i);
00840       return false;
00841     }
00842     for(size_t j=0;j<ramps[i].ramps.size();j++) {
00843       if(Abs(ramps[i].ramps[j].a1) > accMax[j]+EpsilonA ||
00844          Abs(ramps[i].ramps[j].v) > velMax[j] ||
00845          Abs(ramps[i].ramps[j].a2) > accMax[j]+EpsilonA) {
00846         fprintf(stderr,"DynamicPath::IsValid: invalid acceleration or velocity on ramp %d\n",i);
00847         fprintf(stderr,"\ta1 %g, v %g, a2 %g.  amax %g, vmax %g\n",ramps[i].ramps[j].a1,ramps[i].ramps[j].v,ramps[i].ramps[j].a2,accMax[j],velMax[j]);
00848         return false;
00849       }
00850     }
00851   }
00852   for(size_t i=1;i<ramps.size();i++) {
00853     if(ramps[i].x0 != ramps[i-1].x1) {
00854       fprintf(stderr,"DynamicPath::IsValid: discontinuity at ramp %d\n",i);
00855       for(size_t j=0;j<ramps[i].x0.size();j++)
00856         fprintf(stderr,"%g ",ramps[i].x0[j]);
00857       fprintf(stderr,"\n");
00858       for(size_t j=0;j<ramps[i-1].x1.size();j++)
00859         fprintf(stderr,"%g ",ramps[i-1].x1[j]);
00860       fprintf(stderr,"\n");
00861       return false;
00862     }
00863     if(ramps[i].dx0 != ramps[i-1].dx1) {
00864       fprintf(stderr,"DynamicPath::IsValid: derivative discontinuity at ramp %d\n",i);
00865       for(size_t j=0;j<ramps[i].dx0.size();j++)
00866         fprintf(stderr,"%g ",ramps[i].dx0[j]);
00867       fprintf(stderr,"\n");
00868       for(size_t j=0;j<ramps[i-1].dx1.size();j++)
00869         fprintf(stderr,"%g ",ramps[i-1].dx1[j]);
00870       fprintf(stderr,"\n");
00871 
00872       return false;
00873     }
00874   }
00875   return true;
00876 }
00877 
00878 } //namespace ParabolicRamp


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