ParabolicRamp.cpp   ParabolicRamp.cpp 
skipping to change at line 85 skipping to change at line 85
x1 = 0.5 * (-b + det)/a; x1 = 0.5 * (-b + det)/a;
else else
x1 = 2.0 * c / (-b-det); x1 = 2.0 * c / (-b-det);
if(Abs(-b + det) < Abs(a)) if(Abs(-b + det) < Abs(a))
x2 = 0.5 * (-b-det) / a; x2 = 0.5 * (-b-det) / a;
else else
x2 = 2.0 * c / (-b+det); x2 = 2.0 * c / (-b+det);
return 2; return 2;
} }
//return a value x in [xmin,xmax] such that |a*x - b| <= epsilon*max(|a||b|
)
//for ill posed problems choose x=0
bool SafeEqSolve(Real a,Real b,Real epsilon,Real xmin,Real xmax,Real& x)
{
if(a < 0) return SafeEqSolve(-a,-b,epsilon,xmin,xmax,x);
//now we know a>=0
Real epsScaled = epsilon*Max(a,Abs(b));
//infinte range
if(IsInf(xmin)==-1 && IsInf(xmax)==1) {
if(a == 0) {
x = 0.0;
return Abs(b) <= epsScaled;
}
x = b/a;
return true;
}
Real axmin=a*xmin,axmax=a*xmax;
if(!(b + epsScaled >= axmin && b - epsScaled <= axmax)) { //ranges don
't intersect
return false;
}
if(a != 0) {
x = b/a;
if(xmin <= x && x <= xmax) {
return true;
}
}
if (Abs(0.5*(axmin+axmax) - b) <= epsScaled) {
//center of range is in epsilon range
x = 0.5*(xmin+xmax);
return true;
}
if(Abs(axmax - b) <= epsScaled) {
x = xmax;
return true;
}
assert(Abs(axmin - b) <= epsScaled);
x = xmin;
return true;
}
bool SaveRamp(const char* fn,Real x0,Real dx0,Real x1,Real dx1, bool SaveRamp(const char* fn,Real x0,Real dx0,Real x1,Real dx1,
Real a,Real v,Real t) Real a,Real v,Real t)
{ {
if(gSuppressSavingRamps) return true; if(gSuppressSavingRamps) return true;
PARABOLICLOG("Saving ramp to %s\n",fn); PARABOLIC_RAMP_PLOG("Saving ramp to %s\n",fn);
FILE* f=fopen(fn,"ab"); FILE* f=fopen(fn,"ab");
if(!f) { if(!f) {
f = fopen(fn,"wb"); f = fopen(fn,"wb");
if(!f) { if(!f) {
PARABOLICLOG("Unable to open file %s for saving\n",fn); PARABOLIC_RAMP_PLOG("Unable to open file %s for saving\n",fn);
return false; return false;
} }
} }
double vals[7]={x0,dx0,x1,dx1,a,v,t}; double vals[7]={x0,dx0,x1,dx1,a,v,t};
fwrite(vals,sizeof(double),7,f); fwrite(vals,sizeof(double),7,f);
fclose(f); fclose(f);
return true; return true;
} }
bool LoadRamp(FILE* f,Real& x0,Real& dx0,Real& x1,Real& dx1, bool LoadRamp(FILE* f,Real& x0,Real& dx0,Real& x1,Real& dx1,
skipping to change at line 138 skipping to change at line 180
{ {
FILE* f=fopen(fn,"rb"); FILE* f=fopen(fn,"rb");
if(!f) return; if(!f) return;
gSuppressSavingRamps=true; gSuppressSavingRamps=true;
ParabolicRamp1D ramp; ParabolicRamp1D ramp;
Real a,v,t; Real a,v,t;
int numRamps=0; int numRamps=0;
while(LoadRamp(f,ramp.x0,ramp.dx0,ramp.x1,ramp.dx1,a,v,t)) { while(LoadRamp(f,ramp.x0,ramp.dx0,ramp.x1,ramp.dx1,a,v,t)) {
if(t < 0) { if(t < 0) {
PARABOLIC_ASSERT( a >= 0 && v >= 0); PARABOLIC_RAMP_ASSERT( a >= 0 && v >= 0);
bool res=ramp.SolveMinTime(a,v); bool res=ramp.SolveMinTime(a,v);
PARABOLICLOG("Result %d: t=%g\n",(int)res,ramp.ttotal); PARABOLIC_RAMP_PLOG("Result %d: t=%g\n",(int)res,ramp.ttotal);
} }
else if(a < 0) { else if(a < 0) {
PARABOLIC_ASSERT( t >= 0 && v >= 0); PARABOLIC_RAMP_ASSERT( t >= 0 && v >= 0);
bool res=ramp.SolveMinAccel(t,v); bool res=ramp.SolveMinAccel(t,v);
PARABOLICLOG("Result %d: a=%g\n",(int)res,ramp.a1); PARABOLIC_RAMP_PLOG("Result %d: a=%g\n",(int)res,ramp.a1);
} }
else { else {
bool res=ramp.SolveMinTime2(a,v,t); bool res=ramp.SolveMinTime2(a,v,t);
PARABOLICLOG("Result %d: t=%g\n",(int)res,ramp.ttotal); PARABOLIC_RAMP_PLOG("Result %d: t=%g\n",(int)res,ramp.ttotal);
if(!res) { if(!res) {
bool res=ramp.SolveMinAccel(t,v); bool res=ramp.SolveMinAccel(t,v);
PARABOLICLOG("SolveMinAccel result %d: a=%g\n",(int)res,ram p.a1); PARABOLIC_RAMP_PLOG("SolveMinAccel result %d: a=%g\n",(int) res,ramp.a1);
} }
} }
PARABOLICLOG("\n"); PARABOLIC_RAMP_PLOG("\n");
numRamps++; numRamps++;
} }
fclose(f); fclose(f);
PARABOLICLOG("%d ramps loaded from file %s\n",numRamps,fn); PARABOLIC_RAMP_PLOG("%d ramps loaded from file %s\n",numRamps,fn);
gSuppressSavingRamps=false; gSuppressSavingRamps=false;
} }
class ParabolicRamp class ParabolicRamp
{ {
public: public:
Real Evaluate(Real t) const; Real Evaluate(Real t) const;
Real Derivative(Real t) const; Real Derivative(Real t) const;
Real Accel(Real t) const; Real Accel(Real t) const;
bool Solve(Real amax); bool Solve(Real amax);
skipping to change at line 253 skipping to change at line 295
return dx0 + a*t; return dx0 + a*t;
} }
Real ParabolicRamp::Accel(Real t) const Real ParabolicRamp::Accel(Real t) const
{ {
return a; return a;
} }
bool ParabolicRamp::Solve(Real amax) bool ParabolicRamp::Solve(Real amax)
{ {
if(FuzzyEquals(x0,x1,EpsilonX)) { //x0 + tv0 + t^2/2 a = x1
if(FuzzyEquals(dx0,dx1,EpsilonV)) { //v0 + ta = v1
a=0; //ta = v1-v0
ttotal = 0; //t = 2*(x1-x0)/(v1+v0)
return true; //a = (v1-v0)/t
} Real numerator = 2.0*(x1-x0);
else if(FuzzyEquals(dx0,-dx1,EpsilonV)) { //pure parabola, any acce Real denominator = dx1+dx0;
leration works bool res = SafeEqSolve(denominator,numerator,EpsilonT,0.0,Inf,ttotal);
a=amax*Sign(dx1); if(!res) {
ttotal = (dx1-dx0)/a;
return true;
}
//no parabola will work
return false; return false;
} }
a = 0.5*(Sqr(dx0)-Sqr(dx1))/(x0-x1); /*
//pick the denominator less likely to result in numerical errors if(Abs(numerator)*EpsilonT < Abs(denominator)) {
if(Abs(a) < Abs(dx0+dx1)) { ttotal = numerator/denominator;
if(Abs(dx0+dx1) < EpsilonV) { }
//danger of numerical errors else {
//dx0 = - dx1 //dx1 ~= dx0
//need x0 = x1 if(FuzzyZero(numerator,EpsilonX) && FuzzyZero(denominator,EpsilonV))
return false; {
} a=0;
else { ttotal = 0;
ttotal = 2.0*(x1-x0)/(dx0+dx1); return true;
} }
} else
else { return false;
ttotal = (dx1-dx0)/a; }
} Real numerator = 0.5*(Sqr(dx0)-Sqr(dx1));
if(ttotal < 0 && ttotal > -EpsilonT) ttotal = 0; Real denominator = x0-x1;
if(ttotal < 0) { //check potential numerical errors
ttotal = -1; if(FuzzyZero(denominator,EpsilonX)) {
if(FuzzyEquals(dx0,dx1,EpsilonV)) {
a=0; a=0;
ttotal = 0;
return true;
}
else if(FuzzyEquals(dx0,-dx1,EpsilonV)) { //pure parabola, any accel
eration works
a=amax*Sign(dx1);
ttotal = (dx1-dx0)/a;
return true;
}
if(Abs(denominator) <= Abs(numerator)) {
//no parabola will work
return false; return false;
} }
}
a = numerator/denominator;
//pick the denominator less likely to result in numerical errors
if(Abs(a) < Abs(dx0+dx1)) {
if(Abs(dx0+dx1) < EpsilonV) {
//danger of numerical errors
//dx0 = - dx1
//need x0 = x1
return false;
}
else {
ttotal = 2.0*(x1-x0)/(dx0+dx1);
}
}
else {
ttotal = (dx1-dx0)/a;
}
if(ttotal < 0 && ttotal > -EpsilonT) ttotal = 0;
if(ttotal < 0) {
ttotal = -1;
a=0;
return false;
}
//check for numerical errors
if(Abs(a) > amax && Abs(a) <= amax+EpsilonA) {
//double check if the capped version works
a = Sign(a)*amax;
}
*/
PARABOLIC_RAMP_ASSERT(ttotal >= 0);
//pick some value of a that satisfies |ta - v1-v0| < epsilonV
res = SafeEqSolve(ttotal,dx1-dx0,EpsilonV,-amax,amax,a);
if(!res) return false;
PARABOLIC_RAMP_ASSERT(Abs(a) <= amax);
//PARABOLIC_RAMP_ASSERT(FuzzyEquals(Evaluate(ttotal),x1,EpsilonX) && Fu
zzyEquals(Derivative(ttotal),dx1,EpsilonV));
//check for numerical errors
if(Abs(a) > amax && Abs(a) <= amax+EpsilonA) {
//double check if the capped version works
a = Sign(a)*amax;
}
if(FuzzyEquals(Evaluate(ttotal),x1,EpsilonX) && FuzzyEquals(Derivative( ttotal),dx1,EpsilonV)) { if(FuzzyEquals(Evaluate(ttotal),x1,EpsilonX) && FuzzyEquals(Derivative( ttotal),dx1,EpsilonV)) {
return true; return true;
} }
return false; return false;
} }
bool ParabolicRamp::SolveFixedTime(Real endTime) bool ParabolicRamp::SolveFixedTime(Real endTime)
{ {
if(!FuzzyEquals(endTime*(dx1+dx0),2.0*(x1-x0),EpsilonX)) return false;
ttotal = endTime; ttotal = endTime;
if(FuzzyEquals(x0,x1,EpsilonX)) { bool res = SafeEqSolve(ttotal,dx1-dx0,EpsilonV,-Inf,Inf,a);
if(FuzzyEquals(dx0,dx1,EpsilonV)) { if(!res) return false;
a = 0; /*
return FuzzyEquals(endTime,0.0,EpsilonT); if(FuzzyEquals(x0,x1,EpsilonX)) {
} if(FuzzyEquals(dx0,dx1,EpsilonV)) {
else if(FuzzyEquals(dx0,-dx1,EpsilonV)) { //pure parabola, any acce a = 0;
leration works return FuzzyEquals(endTime,0.0,EpsilonT);
a=(dx1-dx0)/endTime; }
return true; else if(FuzzyEquals(dx0,-dx1,EpsilonV)) { //pure parabola, any accel
} eration works
//no parabola will work a=(dx1-dx0)/endTime;
return true;
}
//no parabola will work
return false;
}
a = 0.5*(Sqr(dx0)-Sqr(dx1))/(x0-x1);
//pick the denominator less likely to result in numerical errors
if(Abs(a) < Abs(dx0+dx1)) {
if(Abs(dx0+dx1) < EpsilonV) {
//danger of numerical errors
//dx0 = - dx1
//need x0 = x1
return false; return false;
} }
a = 0.5*(Sqr(dx0)-Sqr(dx1))/(x0-x1); else {
//pick the denominator less likely to result in numerical errors ttotal = 2.0*(x1-x0)/(dx0+dx1);
if(Abs(a) < Abs(dx0+dx1)) { }
if(Abs(dx0+dx1) < EpsilonV) { }
//danger of numerical errors else {
//dx0 = - dx1 ttotal = (dx1-dx0)/a;
//need x0 = x1 }
return false; if(!FuzzyEquals(ttotal,endTime,EpsilonT)) return false;
} ttotal = endTime;
else { */
ttotal = 2.0*(x1-x0)/(dx0+dx1);
}
}
else {
ttotal = (dx1-dx0)/a;
}
if(!FuzzyEquals(ttotal,endTime,EpsilonT)) return false;
ttotal = endTime;
if(FuzzyEquals(Evaluate(ttotal),x1,EpsilonX) && FuzzyEquals(Derivative( ttotal),dx1,EpsilonV)) { if(FuzzyEquals(Evaluate(ttotal),x1,EpsilonX) && FuzzyEquals(Derivative( ttotal),dx1,EpsilonV)) {
return true; return true;
} }
return false; return false;
} }
Real ParabolicRamp::MaxVelocity() const Real ParabolicRamp::MaxVelocity() const
{ {
if(fabs(dx0) > fabs(dx1)) return dx0; if(fabs(dx0) > fabs(dx1)) return dx0;
else return dx1; else return dx1;
skipping to change at line 397 skipping to change at line 485
} }
else { else {
tswitch = -1; tswitch = -1;
ttotal = -1; ttotal = -1;
a = 0; a = 0;
return false; return false;
} }
tswitch = CalcSwitchTime(a); tswitch = CalcSwitchTime(a);
//uncomment for additional debugging //uncomment for additional debugging
if(!FuzzyEquals(x0 + 0.5*a*Sqr(tswitch) + dx0*tswitch,x1 - 0.5*a*Sqr(ts witch-ttotal) + dx1*(tswitch-ttotal),EpsilonX)) { if(!FuzzyEquals(x0 + 0.5*a*Sqr(tswitch) + dx0*tswitch,x1 - 0.5*a*Sqr(ts witch-ttotal) + dx1*(tswitch-ttotal),EpsilonX)) {
PARABOLICLOG("Error computing parabolic-parabolic min-time...\n"); PARABOLIC_RAMP_PLOG("Error computing parabolic-parabolic min-time..
PARABOLICLOG("x0=%g,%g, x1=%g,%g\n",x0,dx0,x1,dx1); .\n");
PARABOLICLOG("a = %g, tswitch = %g, ttotal = %g\n",a,tswitch,ttotal PARABOLIC_RAMP_PLOG("x0=%g,%g, x1=%g,%g\n",x0,dx0,x1,dx1);
); PARABOLIC_RAMP_PLOG("a = %g, tswitch = %g, ttotal = %g\n",a,tswitch
PARABOLICLOG("Forward %g, backward %g, diff %g\n",x0 + 0.5*a*Sqr(ts ,ttotal);
witch) + dx0*tswitch,x1 - 0.5*a*Sqr(tswitch-ttotal) + dx1*(tswitch-ttotal), PARABOLIC_RAMP_PLOG("Forward %g, backward %g, diff %g\n",x0 + 0.5*a
x0 + 0.5*a*Sqr(tswitch) + dx0*tswitch - (x1 - 0.5*a*Sqr(tswitch-ttotal) + *Sqr(tswitch) + dx0*tswitch,x1 - 0.5*a*Sqr(tswitch-ttotal) + dx1*(tswitch-t
dx1*(tswitch-ttotal))); total), x0 + 0.5*a*Sqr(tswitch) + dx0*tswitch - (x1 - 0.5*a*Sqr(tswitch-tto
tal) + dx1*(tswitch-ttotal)));
//Real b = 2.0*dx0; //2.0*(dx0-dx1); //Real b = 2.0*dx0; //2.0*(dx0-dx1);
//Real c = (Sqr(dx0)-Sqr(dx1))*0.5/a+x0-x1; //Real c = (Sqr(dx0)-Sqr(dx1))*0.5/a+x0-x1;
Real b = 2.0*a*dx0; //2.0*(dx0-dx1); Real b = 2.0*a*dx0; //2.0*(dx0-dx1);
Real c = (Sqr(dx0)-Sqr(dx1))*0.5+(x0-x1)*a; Real c = (Sqr(dx0)-Sqr(dx1))*0.5+(x0-x1)*a;
Real t1,t2; Real t1,t2;
int res=quadratic(a*a,b,c,t1,t2); int res=quadratic(a*a,b,c,t1,t2);
PARABOLICLOG("Quadratic equation %g x^2 + %g x + %g = 0\n",a*a,b,c) PARABOLIC_RAMP_PLOG("Quadratic equation %g x^2 + %g x + %g = 0\n",a
; *a,b,c);
PARABOLICLOG("%d results, %g %g\n",res,t1,t2); PARABOLIC_RAMP_PLOG("%d results, %g %g\n",res,t1,t2);
//getchar();
SaveRamp("PP_SolveMinTime_failure.dat",x0,dx0,x1,dx1,amax,Inf,-1); SaveRamp("PP_SolveMinTime_failure.dat",x0,dx0,x1,dx1,amax,Inf,-1);
return false; return false;
} }
PARABOLIC_ASSERT(FuzzyEquals(x0 + 0.5*a*Sqr(tswitch) + dx0*tswitch,x1 - 0.5*a*Sqr(tswitch-ttotal) + dx1*(tswitch-ttotal),EpsilonX)); PARABOLIC_RAMP_ASSERT(FuzzyEquals(x0 + 0.5*a*Sqr(tswitch) + dx0*tswitch ,x1 - 0.5*a*Sqr(tswitch-ttotal) + dx1*(tswitch-ttotal),EpsilonX));
return true; return true;
} }
bool PPRamp::SolveMinTime2(Real amax,Real timeLowerBound) bool PPRamp::SolveMinTime2(Real amax,Real timeLowerBound)
{ {
PARABOLIC_ASSERT(timeLowerBound >= 0); PARABOLIC_RAMP_ASSERT(timeLowerBound >= 0);
Real t1pn,t1np,t2pn,t2np; Real t1pn,t1np,t2pn,t2np;
int respn = CalcTotalTimes(amax,t1pn,t2pn); int respn = CalcTotalTimes(amax,t1pn,t2pn);
int resnp = CalcTotalTimes(-amax,t1np,t2np); int resnp = CalcTotalTimes(-amax,t1np,t2np);
ttotal = Inf; ttotal = Inf;
if(respn >= 1) { if(respn >= 1) {
if(t1pn >= timeLowerBound && t1pn < ttotal) { if(t1pn >= timeLowerBound && t1pn < ttotal) {
ttotal = t1pn; ttotal = t1pn;
a = amax; a = amax;
} }
} }
skipping to change at line 455 skipping to change at line 542
ttotal = t2np; ttotal = t2np;
a = -amax; a = -amax;
} }
} }
if(IsInf(ttotal)) { if(IsInf(ttotal)) {
a = 0; a = 0;
tswitch = ttotal = -1; tswitch = ttotal = -1;
return false; return false;
} }
PARABOLIC_RAMP_ASSERT(ttotal >= timeLowerBound);
Real ts1,ts2; Real ts1,ts2;
int res = CalcSwitchTimes(a,ts1,ts2); int res = CalcSwitchTimes(a,ts1,ts2);
PARABOLIC_ASSERT(res > 0); PARABOLIC_RAMP_ASSERT(res > 0);
if(res == 1) { if(res == 1) {
tswitch = ts1; tswitch = ts1;
PARABOLIC_ASSERT(FuzzyEquals(ttotal,ts1*2.0 - (dx1-dx0)/a,EpsilonT) ); PARABOLIC_RAMP_ASSERT(FuzzyEquals(ttotal,ts1*2.0 - (dx1-dx0)/a,Epsi lonT));
} }
else { else {
if(FuzzyEquals(ttotal,ts1*2.0 - (dx1-dx0)/a,EpsilonT)) if(FuzzyEquals(ttotal,ts1*2.0 - (dx1-dx0)/a,EpsilonT))
tswitch = ts1; tswitch = ts1;
else { else {
PARABOLIC_ASSERT(FuzzyEquals(ttotal,ts2*2.0 - (dx1-dx0)/a,Epsil onT)); PARABOLIC_RAMP_ASSERT(FuzzyEquals(ttotal,ts2*2.0 - (dx1-dx0)/a, EpsilonT));
tswitch = ts2; tswitch = ts2;
} }
} }
//uncomment for additional debugging //uncomment for additional debugging
Real eps = EpsilonX; Real eps = EpsilonX;
if(!FuzzyEquals(x0 + 0.5*a*Sqr(tswitch) + dx0*tswitch,x1 - 0.5*a*Sqr(ts witch-ttotal) + dx1*(tswitch-ttotal),eps)) { if(!FuzzyEquals(x0 + 0.5*a*Sqr(tswitch) + dx0*tswitch,x1 - 0.5*a*Sqr(ts witch-ttotal) + dx1*(tswitch-ttotal),eps)) {
PARABOLICLOG("Error computing parabola min-time...\n"); PARABOLIC_RAMP_PLOG("Error computing parabola min-time...\n");
PARABOLICLOG("x0=%g,%g, x1=%g,%g\n",x0,dx0,x1,dx1); PARABOLIC_RAMP_PLOG("x0=%g,%g, x1=%g,%g\n",x0,dx0,x1,dx1);
PARABOLICLOG("a = %g, tswitch = %g, ttotal = %g\n",a,tswitch,ttotal PARABOLIC_RAMP_PLOG("a = %g, tswitch = %g, ttotal = %g\n",a,tswitch
); ,ttotal);
PARABOLICLOG("Forward %g, backward %g, diff %g\n",x0 + 0.5*a*Sqr(ts PARABOLIC_RAMP_PLOG("Forward %g, backward %g, diff %g\n",x0 + 0.5*a
witch) + dx0*tswitch,x1 - 0.5*a*Sqr(tswitch-ttotal) + dx1*(tswitch-ttotal), *Sqr(tswitch) + dx0*tswitch,x1 - 0.5*a*Sqr(tswitch-ttotal) + dx1*(tswitch-t
x0 + 0.5*a*Sqr(tswitch) + dx0*tswitch - (x1 - 0.5*a*Sqr(tswitch-ttotal) + total), x0 + 0.5*a*Sqr(tswitch) + dx0*tswitch - (x1 - 0.5*a*Sqr(tswitch-tto
dx1*(tswitch-ttotal))); tal) + dx1*(tswitch-ttotal)));
//Real b = 2.0*dx0; //2.0*(dx0-dx1); //Real b = 2.0*dx0; //2.0*(dx0-dx1);
//Real c = (Sqr(dx0)-Sqr(dx1))*0.5/a+x0-x1; //Real c = (Sqr(dx0)-Sqr(dx1))*0.5/a+x0-x1;
Real b = 2.0*a*dx0; //2.0*(dx0-dx1); Real b = 2.0*a*dx0; //2.0*(dx0-dx1);
Real c = (Sqr(dx0)-Sqr(dx1))*0.5+(x0-x1)*a; Real c = (Sqr(dx0)-Sqr(dx1))*0.5+(x0-x1)*a;
Real t1,t2; Real t1,t2;
int res=quadratic(a*a,b,c,t1,t2); int res=quadratic(a*a,b,c,t1,t2);
PARABOLICLOG("Quadratic equation %g x^2 + %g x + %g = 0\n",a*a,b,c) PARABOLIC_RAMP_PLOG("Quadratic equation %g x^2 + %g x + %g = 0\n",a
; *a,b,c);
PARABOLICLOG("%d results, %g %g\n",res,t1,t2); PARABOLIC_RAMP_PLOG("%d results, %g %g\n",res,t1,t2);
//getchar();
SaveRamp("PP_SolveMinTime_failure.dat",x0,dx0,x1,dx1,amax,Inf,timeL owerBound); SaveRamp("PP_SolveMinTime_failure.dat",x0,dx0,x1,dx1,amax,Inf,timeL owerBound);
return false; return false;
} }
PARABOLIC_ASSERT(FuzzyEquals(x0 + 0.5*a*Sqr(tswitch) + dx0*tswitch,x1 - 0.5*a*Sqr(tswitch-ttotal) + dx1*(tswitch-ttotal),eps)); PARABOLIC_RAMP_ASSERT(FuzzyEquals(x0 + 0.5*a*Sqr(tswitch) + dx0*tswitch ,x1 - 0.5*a*Sqr(tswitch-ttotal) + dx1*(tswitch-ttotal),eps));
return true; return true;
} }
bool PPRamp::SolveMinAccel(Real endTime) bool PPRamp::SolveMinAccel(Real endTime)
{ {
Real switch1,switch2; Real switch1,switch2;
Real apn = CalcMinAccel(endTime,1.0,switch1); Real apn = CalcMinAccel(endTime,1.0,switch1);
Real anp = CalcMinAccel(endTime,-1.0,switch2); Real anp = CalcMinAccel(endTime,-1.0,switch2);
//cout<<"Accel for parabola +-: "<<apn<<", parabola -+: "<<anp<<endl; //cout<<"Accel for parabola +-: "<<apn<<", parabola -+: "<<anp<<endl;
skipping to change at line 521 skipping to change at line 608
} }
ttotal = endTime; ttotal = endTime;
if(a == apn) if(a == apn)
tswitch = switch1; tswitch = switch1;
else else
tswitch = switch2; tswitch = switch2;
//debug //debug
Real t2mT = tswitch-ttotal; Real t2mT = tswitch-ttotal;
if(!FuzzyEquals(x0 + tswitch*dx0 + 0.5*a*Sqr(tswitch),x1+t2mT*dx1-0.5*a *Sqr(t2mT),EpsilonX)) { if(!FuzzyEquals(x0 + tswitch*dx0 + 0.5*a*Sqr(tswitch),x1+t2mT*dx1-0.5*a *Sqr(t2mT),EpsilonX)) {
PARABOLICLOG("PPRamp: Error solving min-accel!\n"); PARABOLIC_RAMP_PLOG("PPRamp: Error solving min-accel!\n");
PARABOLICLOG("Forward ramp: %g, backward %g, diff %g\n",x0 + tswitc PARABOLIC_RAMP_PLOG("Forward ramp: %g, backward %g, diff %g\n",x0 +
h*dx0 + 0.5*a*Sqr(tswitch),x1+t2mT*dx1-0.5*a*Sqr(t2mT),x0 + tswitch*dx0 + 0 tswitch*dx0 + 0.5*a*Sqr(tswitch),x1+t2mT*dx1-0.5*a*Sqr(t2mT),x0 + tswitch*
.5*a*Sqr(tswitch)-(x1+t2mT*dx1-0.5*a*Sqr(t2mT))); dx0 + 0.5*a*Sqr(tswitch)-(x1+t2mT*dx1-0.5*a*Sqr(t2mT)));
PARABOLICLOG("A+ = %g, A- = %g\n",apn,anp); PARABOLIC_RAMP_PLOG("A+ = %g, A- = %g\n",apn,anp);
PARABOLICLOG("ramp %g,%g -> %g, %g\n",x0,dx0,x1,dx1); PARABOLIC_RAMP_PLOG("ramp %g,%g -> %g, %g\n",x0,dx0,x1,dx1);
PARABOLICLOG("Switch 1 %g, switch 2 %g, total %g\n",switch1,switch2 PARABOLIC_RAMP_PLOG("Switch 1 %g, switch 2 %g, total %g\n",switch1,
,ttotal); switch2,ttotal);
{ {
Real sign = 1.0; Real sign = 1.0;
Real a=Sqr(endTime); Real a=Sqr(endTime);
Real b=sign*(2.0*(dx0+dx1)*endTime+4.0*(x0-x1)); Real b=sign*(2.0*(dx0+dx1)*endTime+4.0*(x0-x1));
Real c=-Sqr(dx1-dx0); Real c=-Sqr(dx1-dx0);
PARABOLICLOG("Quadratic %g x^2 + %g x + %g = 0\n",a,b,c); PARABOLIC_RAMP_PLOG("Quadratic %g x^2 + %g x + %g = 0\n",a,b,c) ;
Real t1,t2; Real t1,t2;
int res = quadratic(a,b,c,t1,t2); int res = quadratic(a,b,c,t1,t2);
PARABOLICLOG("Solutions: %d, %g and %g\n",res,t1,t2); PARABOLIC_RAMP_PLOG("Solutions: %d, %g and %g\n",res,t1,t2);
} }
{ {
Real sign = -1.0; Real sign = -1.0;
Real a=Sqr(endTime); Real a=Sqr(endTime);
Real b=sign*(2.0*(dx0+dx1)*endTime+4.0*(x0-x1)); Real b=sign*(2.0*(dx0+dx1)*endTime+4.0*(x0-x1));
Real c=-Sqr(dx1-dx0); Real c=-Sqr(dx1-dx0);
PARABOLICLOG("Quadratic %g x^2 + %g x + %g = 0\n",a,b,c); PARABOLIC_RAMP_PLOG("Quadratic %g x^2 + %g x + %g = 0\n",a,b,c) ;
Real t1,t2; Real t1,t2;
int res = quadratic(a,b,c,t1,t2); int res = quadratic(a,b,c,t1,t2);
PARABOLICLOG("Solutions: %d, %g and %g\n",res,t1,t2); PARABOLIC_RAMP_PLOG("Solutions: %d, %g and %g\n",res,t1,t2);
} }
SaveRamp("PP_SolveMinAccel_failure.dat",x0,dx0,x1,dx1,-1,Inf,endTim e); SaveRamp("PP_SolveMinAccel_failure.dat",x0,dx0,x1,dx1,-1,Inf,endTim e);
//getchar();
} }
if(!FuzzyEquals(dx0 + a*tswitch,dx1-a*t2mT,EpsilonV)) { if(!FuzzyEquals(dx0 + a*tswitch,dx1-a*t2mT,EpsilonV)) {
PARABOLICLOG("PPRamp: Error solving min-accel!\n"); PARABOLIC_RAMP_PLOG("PPRamp: Error solving min-accel!\n");
PARABOLICLOG("Velocity error %g vs %g, err %g\n",dx0+a*tswitch,dx1- PARABOLIC_RAMP_PLOG("Velocity error %g vs %g, err %g\n",dx0+a*tswit
a*t2mT,dx0+a*tswitch-(dx1-a*t2mT)); ch,dx1-a*t2mT,dx0+a*tswitch-(dx1-a*t2mT));
PARABOLICLOG("ramp %g,%g -> %g, %g\n",x0,dx0,x1,dx1); PARABOLIC_RAMP_PLOG("ramp %g,%g -> %g, %g\n",x0,dx0,x1,dx1);
PARABOLICLOG("Accel %g\n",a); PARABOLIC_RAMP_PLOG("Accel %g\n",a);
PARABOLICLOG("Switch %g, total %g\n",tswitch,ttotal); PARABOLIC_RAMP_PLOG("Switch %g, total %g\n",tswitch,ttotal);
SaveRamp("PP_SolveMinAccel_failure.dat",x0,dx0,x1,dx1,-1,Inf,endTim e); SaveRamp("PP_SolveMinAccel_failure.dat",x0,dx0,x1,dx1,-1,Inf,endTim e);
return false; return false;
} }
return true; return true;
} }
Real PPRamp::MaxVelocity() const Real PPRamp::MaxVelocity() const
{ {
return tswitch*a+dx0; return tswitch*a+dx0;
} }
Real PPRamp::CalcTotalTime(Real a) const Real PPRamp::CalcTotalTime(Real a) const
{ {
Real tswitch = CalcSwitchTime(a); Real tswitch = CalcSwitchTime(a);
//PARABOLICLOG("a = %g, switch time %g\n",a,tswitch); //PARABOLIC_RAMP_PLOG("a = %g, switch time %g\n",a,tswitch);
if(tswitch < 0) return -1; if(tswitch < 0) return -1;
if(tswitch < (dx1-dx0)/a) return -1; if(tswitch < (dx1-dx0)/a) return -1;
return tswitch*2.0 - (dx1-dx0)/a; return tswitch*2.0 - (dx1-dx0)/a;
} }
int PPRamp::CalcTotalTimes(Real a,Real& t1,Real& t2) const int PPRamp::CalcTotalTimes(Real a,Real& t1,Real& t2) const
{ {
Real ts1,ts2; Real ts1,ts2;
int res=CalcSwitchTimes(a,ts1,ts2); int res=CalcSwitchTimes(a,ts1,ts2);
if(res == 0) return res; if(res == 0) return res;
skipping to change at line 654 skipping to change at line 740
{ {
Real t1,t2; Real t1,t2;
int res = CalcSwitchTimes(a,t1,t2); int res = CalcSwitchTimes(a,t1,t2);
if(res == 0) { if(res == 0) {
return -1; return -1;
} }
else if(res == 2) { else if(res == 2) {
//check total time //check total time
if(t2*Abs(a) < (dx1-dx0)*Sign(a)) return t1; if(t2*Abs(a) < (dx1-dx0)*Sign(a)) return t1;
else if(t1*Abs(a) < (dx1-dx0)*Sign(a)) return t2; else if(t1*Abs(a) < (dx1-dx0)*Sign(a)) return t2;
else return Min(t1,t2); //both ar e ok else return Min(t1,t2); //both are o k
} }
else return t1; else return t1;
} }
//for a PP ramp: //for a PP ramp:
//xs = x0 + ts*dx0 + 0.5*z*ts^2 //xs = x0 + ts*dx0 + 0.5*z*ts^2
//xs = x1 - (T-ts)*dx1 - 0.5*z*(T-ts)^2 //xs = x1 - (T-ts)*dx1 - 0.5*z*(T-ts)^2
//xs' = dx0 + ts*z //xs' = dx0 + ts*z
//xs' = dx1 + (T-ts)*z //xs' = dx1 + (T-ts)*z
//z = sign*a //z = sign*a
skipping to change at line 777 skipping to change at line 863
//else if(sign < 0.0 && x1 < x0+dx0*endTime) return -1; //else if(sign < 0.0 && x1 < x0+dx0*endTime) return -1;
switchTime = 0; switchTime = 0;
Real a=(dx1-dx0)/endTime; Real a=(dx1-dx0)/endTime;
if((sign > 0.0) == (a >= 0.0)) return -1; if((sign > 0.0) == (a >= 0.0)) return -1;
else return Abs(a); else return Abs(a);
} }
Real c=-Sqr(dx1-dx0)/endTime; Real c=-Sqr(dx1-dx0)/endTime;
if(FuzzyEquals(dx1,dx0,EpsilonV)) { if(FuzzyEquals(dx1,dx0,EpsilonV)) {
//one of the solutions will be very close to zero, use alt solution //one of the solutions will be very close to zero, use alt solution
Real a=-2.0*(dx0+dx1)/endTime + 4.0*(x1-x0)/Sqr(endTime); Real a=-2.0*(dx0+dx1)/endTime + 4.0*(x1-x0)/Sqr(endTime);
PARABOLICLOG("only two solutions: 0 and %g\n",a); PARABOLIC_RAMP_PLOG("only two solutions: 0 and %g\n",a);
switchTime = 0.5*endTime; switchTime = 0.5*endTime;
//try out the zero solution //try out the zero solution
PARABOLICLOG("diff at 0 solution: %g\n",x0-x1 + switchTime*(dx0+dx1) ); PARABOLIC_RAMP_PLOG("diff at 0 solution: %g\n",x0-x1 + switchTime*(d x0+dx1));
if(FuzzyEquals(x0 + switchTime*dx0,x1 - switchTime*dx1,EpsilonX)) if(FuzzyEquals(x0 + switchTime*dx0,x1 - switchTime*dx1,EpsilonX))
return 0; return 0;
PARABOLIC_ASSERT(FuzzyEquals(dx0 + switchTime*a,dx1 + switchTime*a,E PARABOLIC_RAMP_ASSERT(FuzzyEquals(dx0 + switchTime*a,dx1 + switchTim
psilonV)); e*a,EpsilonV));
PARABOLIC_ASSERT(FuzzyEquals(x0 + switchTime*dx0 + 0.5*a*Sqr(switchT PARABOLIC_RAMP_ASSERT(FuzzyEquals(x0 + switchTime*dx0 + 0.5*a*Sqr(sw
ime),x1 - switchTime*dx1-0.5*a*Sqr(switchTime),EpsilonX)); itchTime),x1 - switchTime*dx1-0.5*a*Sqr(switchTime),EpsilonX));
if((sign > 0.0) != (a >= 0.0)) return -1; if((sign > 0.0) != (a >= 0.0)) return -1;
return Abs(a); return Abs(a);
} }
if(FuzzyZero(c,EpsilonA)) { if(FuzzyZero(c,EpsilonA)) {
//need better numerical performance when dx1 ~= dx0 //need better numerical performance when dx1 ~= dx0
a = a/Abs(dx1-dx0); a = a/Abs(dx1-dx0);
b = b/Abs(dx1-dx0); b = b/Abs(dx1-dx0);
c = -Abs(dx1-dx0)/endTime; c = -Abs(dx1-dx0)/endTime;
} }
Real accel1,accel2; Real accel1,accel2;
skipping to change at line 812 skipping to change at line 898
res--; res--;
} }
Real switchTime1 = endTime*0.5+sign*0.5*(dx1-dx0)/accel1; Real switchTime1 = endTime*0.5+sign*0.5*(dx1-dx0)/accel1;
Real switchTime2 = endTime*0.5+sign*0.5*(dx1-dx0)/accel2; Real switchTime2 = endTime*0.5+sign*0.5*(dx1-dx0)/accel2;
//if(accel1 == 0 && x0 == x1) switchTime1 = 0; //if(accel1 == 0 && x0 == x1) switchTime1 = 0;
//if(accel2 == 0 && x0 == x1) switchTime2 = 0; //if(accel2 == 0 && x0 == x1) switchTime2 = 0;
if(res==0) return -1; if(res==0) return -1;
else if(res==1) { else if(res==1) {
if(!IsFinite(accel1)) { if(!IsFinite(accel1)) {
PARABOLICLOG("Error computing accelerations!\n"); PARABOLIC_RAMP_PLOG("Error computing accelerations!\n");
PARABOLICLOG("Quadratic %g x^2 + %g x + %g = 0\n",a,b,c); PARABOLIC_RAMP_PLOG("Quadratic %g x^2 + %g x + %g = 0\n",a,b,c);
PARABOLICLOG("x0 %g, dx0 %g, x1 %g, dx1 %g\n",x0,dx0,x1,dx1); PARABOLIC_RAMP_PLOG("x0 %g, dx0 %g, x1 %g, dx1 %g\n",x0,dx0,x1,dx1)
PARABOLICLOG("EndTime %g, sign %g\n",endTime,sign); ;
PARABOLICLOG("Results %g %g\n",accel1,accel2); PARABOLIC_RAMP_PLOG("EndTime %g, sign %g\n",endTime,sign);
//getchar(); PARABOLIC_RAMP_PLOG("Results %g %g\n",accel1,accel2);
} }
if(switchTime1 >= 0 && switchTime1 <= endTime) { switchTime=switchTi me1; return accel1; } if(switchTime1 >= 0 && switchTime1 <= endTime) { switchTime=switchTi me1; return accel1; }
return -1.0; return -1.0;
} }
else if(res==2) { else if(res==2) {
if(!IsFinite(accel1) || !IsFinite(accel2)) { if(!IsFinite(accel1) || !IsFinite(accel2)) {
PARABOLICLOG("Error computing accelerations!\n"); PARABOLIC_RAMP_PLOG("Error computing accelerations!\n");
PARABOLICLOG("Quadratic %g x^2 + %g x + %g = 0\n",a,b,c); PARABOLIC_RAMP_PLOG("Quadratic %g x^2 + %g x + %g = 0\n",a,b,c);
PARABOLICLOG("x0 %g, dx0 %g, x1 %g, dx1 %g\n",x0,dx0,x1,dx1); PARABOLIC_RAMP_PLOG("x0 %g, dx0 %g, x1 %g, dx1 %g\n",x0,dx0,x1,dx1)
PARABOLICLOG("EndTime %g, sign %g\n",endTime,sign); ;
PARABOLICLOG("Results %g %g\n",accel1,accel2); PARABOLIC_RAMP_PLOG("EndTime %g, sign %g\n",endTime,sign);
//getchar(); PARABOLIC_RAMP_PLOG("Results %g %g\n",accel1,accel2);
} }
if(FuzzyZero(switchTime1,EpsilonT) || FuzzyZero(switchTime2,EpsilonT )) { if(FuzzyZero(switchTime1,EpsilonT) || FuzzyZero(switchTime2,EpsilonT )) {
//need to double check for numerical instability //need to double check for numerical instability
//if sign is +, this means we're switching directly to - //if sign is +, this means we're switching directly to -
//if sign is -, this means we're switching directly to + //if sign is -, this means we're switching directly to +
if(sign > 0.0 && x1 > x0+dx0*endTime) return -1; if(sign > 0.0 && x1 > x0+dx0*endTime) return -1;
else if(sign < 0 && x1 < x0+dx0*endTime) return -1; else if(sign < 0 && x1 < x0+dx0*endTime) return -1;
switchTime = 0; switchTime = 0;
if(FuzzyZero(switchTime1,EpsilonT)) return accel1; if(FuzzyZero(switchTime1,EpsilonT)) return accel1;
else return accel2; else return accel2;
skipping to change at line 947 skipping to change at line 1031
if(!(CalcTotalTime(a,v) >= 0)) { return Inf; } if(!(CalcTotalTime(a,v) >= 0)) { return Inf; }
return a; return a;
/* /*
if(!(CalcTotalTime(a,v) >= 0)) { if(!(CalcTotalTime(a,v) >= 0)) {
//this is very strange -- does it happen because of a compiler //this is very strange -- does it happen because of a compiler
//optimization error? //optimization error?
PARABOLICWARN("PLPRamp::CalcMinAccel: some numerical error prevented computing total time\n"); PARABOLICWARN("PLPRamp::CalcMinAccel: some numerical error prevented computing total time\n");
PARABOLICWARN(" Ramp %g,%g -> %g,%g\n",x0,dx0,x1,dx1); PARABOLICWARN(" Ramp %g,%g -> %g,%g\n",x0,dx0,x1,dx1);
PARABOLICWARN(" endTime %g, accel %g, vel %g, switch times %g %g, t otal time %g\n",endTime,a,v,CalcSwitchTime1(a,v),CalcSwitchTime2(a,v),CalcT otalTime(a,v)); PARABOLICWARN(" endTime %g, accel %g, vel %g, switch times %g %g, t otal time %g\n",endTime,a,v,CalcSwitchTime1(a,v),CalcSwitchTime2(a,v),CalcT otalTime(a,v));
PARABOLIC_ASSERT(v == vold); PARABOLIC_RAMP_ASSERT(v == vold);
PARABOLICLOG("y1=%g, y2=%g, t2mt1 = %g\n",y1,y2,t2mt1); PARABOLIC_RAMP_PLOG("y1=%g, y2=%g, t2mt1 = %g\n",y1,y2,t2mt1);
Real y1_test = 0.5*(Sqr(v) - Sqr(dx0))/a + x0; Real y1_test = 0.5*(Sqr(v) - Sqr(dx0))/a + x0;
Real y2_test = 0.5*(Sqr(dx1) - Sqr(v))/a + x1; Real y2_test = 0.5*(Sqr(dx1) - Sqr(v))/a + x1;
Real t2mt1_test = (y2_test-y1_test)/v; Real t2mt1_test = (y2_test-y1_test)/v;
//Real t2mt1_test = 0.5*(Sqr(dx1) + Sqr(dx0))/(v*a) - v/a + (x1 - x0 )/v; //Real t2mt1_test = 0.5*(Sqr(dx1) + Sqr(dx0))/(v*a) - v/a + (x1 - x0 )/v;
PARABOLICLOG("y1=%g, y2=%g, t2mt1 = %g\n",y1_test,y2_test,t2mt1_test PARABOLIC_RAMP_PLOG("y1=%g, y2=%g, t2mt1 = %g\n",y1_test,y2_test,t2m
); t1_test);
PARABOLICLOG("dy1=%g, dy2=%g, dt2mt1 = %g\n",y1-y1_test,y2-y2_test,t PARABOLIC_RAMP_PLOG("dy1=%g, dy2=%g, dt2mt1 = %g\n",y1-y1_test,y2-y2
2mt1-t2mt1_test); _test,t2mt1-t2mt1_test);
//getchar();
return Inf; return Inf;
PARABOLIC_ASSERT(y1 == y1_test); PARABOLIC_RAMP_ASSERT(y1 == y1_test);
PARABOLIC_ASSERT(y2 == y2_test); PARABOLIC_RAMP_ASSERT(y2 == y2_test);
PARABOLIC_ASSERT(y2-y1 == y2_test-y1_test); PARABOLIC_RAMP_ASSERT(y2-y1 == y2_test-y1_test);
PARABOLIC_ASSERT(t2mt1 == t2mt1_test); PARABOLIC_RAMP_ASSERT(t2mt1 == t2mt1_test);
} }
PARABOLIC_ASSERT(CalcTotalTime(a,v) >= 0); PARABOLIC_RAMP_ASSERT(CalcTotalTime(a,v) >= 0);
return a; return a;
*/ */
} }
bool PLPRamp::SolveMinTime(Real amax,Real vmax) bool PLPRamp::SolveMinTime(Real amax,Real vmax)
{ {
return SolveMinTime2(amax,vmax,0); return SolveMinTime2(amax,vmax,0);
} }
bool PLPRamp::SolveMinTime2(Real amax,Real vmax,Real timeLowerBound) bool PLPRamp::SolveMinTime2(Real amax,Real vmax,Real timeLowerBound)
skipping to change at line 1004 skipping to change at line 1087
} }
if(t4 >= timeLowerBound && t4 < ttotal) { if(t4 >= timeLowerBound && t4 < ttotal) {
a = -amax; a = -amax;
v = -vmax; v = -vmax;
ttotal = t4; ttotal = t4;
} }
if(IsInf(ttotal)) { if(IsInf(ttotal)) {
a = v = 0; a = v = 0;
tswitch1 = tswitch2 = ttotal = -1; tswitch1 = tswitch2 = ttotal = -1;
//PARABOLICLOG("Times... %g %g %g %g\n",t1,t2,t3,t4); //PARABOLIC_RAMP_PLOG("Times... %g %g %g %g\n",t1,t2,t3,t4);
//PARABOLICLOG("Trying alternate MinTime2 solution technique...\n") //PARABOLIC_RAMP_PLOG("Trying alternate MinTime2 solution technique
; ...\n");
Real v1 = CalcMinTime2(timeLowerBound,amax,vmax); Real v1 = CalcMinTime2(timeLowerBound,amax,vmax);
Real v2 = CalcMinTime2(timeLowerBound,-amax,vmax); Real v2 = CalcMinTime2(timeLowerBound,-amax,vmax);
if(v1 > 0) { if(v1 > 0) {
a = amax; a = amax;
v = v1; v = v1;
tswitch1 = (v1-dx0)/a; tswitch1 = (v1-dx0)/a;
tswitch2 = timeLowerBound - (v1-dx1)/a; tswitch2 = timeLowerBound - (v1-dx1)/a;
ttotal = timeLowerBound; ttotal = timeLowerBound;
//PARABOLICLOG("Candidate 1 timing %g %g %g\n",tswitch1,tswitch //PARABOLIC_RAMP_PLOG("Candidate 1 timing %g %g %g\n",tswitch1,
2,ttotal); tswitch2,ttotal);
//PARABOLICLOG("Test 1 x %g %g\n",x1-(ttotal-tswitch2)*v1+0.5*S //PARABOLIC_RAMP_PLOG("Test 1 x %g %g\n",x1-(ttotal-tswitch2)*v
qr(ttotal-tswitch2)*a,x0+tswitch1*dx0+0.5*Sqr(tswitch1)*a+(tswitch1-tswitch 1+0.5*Sqr(ttotal-tswitch2)*a,x0+tswitch1*dx0+0.5*Sqr(tswitch1)*a+(tswitch1-
2)*v1); tswitch2)*v1);
//PARABOLICLOG("x1, v1 = %g, %g\n",x0+dx0*tswitch1+0.5*Sqr(tswi //PARABOLIC_RAMP_PLOG("x1, v1 = %g, %g\n",x0+dx0*tswitch1+0.5*S
tch1)*a,dx0+tswitch1*a); qr(tswitch1)*a,dx0+tswitch1*a);
//PARABOLICLOG("x2, v2 = %g, %g\n",x1-dx1*(ttotal-tswitch2)+0.5 //PARABOLIC_RAMP_PLOG("x2, v2 = %g, %g\n",x1-dx1*(ttotal-tswitc
*Sqr(ttotal-tswitch2)*a,dx1+(ttotal-tswitch2)*a); h2)+0.5*Sqr(ttotal-tswitch2)*a,dx1+(ttotal-tswitch2)*a);
return true; return true;
} }
if(v2 > 0) { if(v2 > 0) {
a = -amax; a = -amax;
v = v2; v = v2;
tswitch1 = (v2-dx0)/a; tswitch1 = (v2-dx0)/a;
tswitch2 = timeLowerBound - (v2-dx1)/a; tswitch2 = timeLowerBound - (v2-dx1)/a;
ttotal = timeLowerBound; ttotal = timeLowerBound;
//PARABOLICLOG("Candidate 2 timing %g %g %g\n",tswitch1,tswitch //PARABOLIC_RAMP_PLOG("Candidate 2 timing %g %g %g\n",tswitch1,
2,ttotal); tswitch2,ttotal);
//PARABOLICLOG("Test 2 x %g %g\n",x1-(ttotal-tswitch2)*v1+0.5*S //PARABOLIC_RAMP_PLOG("Test 2 x %g %g\n",x1-(ttotal-tswitch2)*v
qr(ttotal-tswitch2)*a,x0+tswitch1*dx0+0.5*Sqr(tswitch1)*a+(tswitch1-tswitch 1+0.5*Sqr(ttotal-tswitch2)*a,x0+tswitch1*dx0+0.5*Sqr(tswitch1)*a+(tswitch1-
2)*v1); tswitch2)*v1);
//PARABOLICLOG("x1, v1 = %g, %g\n",x0+dx0*tswitch1+0.5*Sqr(tswi //PARABOLIC_RAMP_PLOG("x1, v1 = %g, %g\n",x0+dx0*tswitch1+0.5*S
tch1)*a,dx0+tswitch1*a); qr(tswitch1)*a,dx0+tswitch1*a);
//PARABOLICLOG("x2, v2 = %g, %g\n",x1-dx1*(ttotal-tswitch1)+0.5 //PARABOLIC_RAMP_PLOG("x2, v2 = %g, %g\n",x1-dx1*(ttotal-tswitc
*Sqr(ttotal-tswitch1)*a,dx1+(ttotal-tswitch2)*a); h1)+0.5*Sqr(ttotal-tswitch1)*a,dx1+(ttotal-tswitch2)*a);
return true; return true;
} }
return false; return false;
} }
tswitch1 = CalcSwitchTime1(a,v); tswitch1 = CalcSwitchTime1(a,v);
tswitch2 = CalcSwitchTime2(a,v); tswitch2 = CalcSwitchTime2(a,v);
if(tswitch1 > tswitch2 && FuzzyEquals(tswitch1,tswitch2,EpsilonT)) { if(tswitch1 > tswitch2 && FuzzyEquals(tswitch1,tswitch2,EpsilonT)) {
tswitch1 = tswitch2 = 0.5*(tswitch1+tswitch2); tswitch1 = tswitch2 = 0.5*(tswitch1+tswitch2);
} }
if(tswitch2 > ttotal && FuzzyEquals(tswitch2,ttotal,EpsilonT)) { if(tswitch2 > ttotal && FuzzyEquals(tswitch2,ttotal,EpsilonT)) {
tswitch2 = ttotal; tswitch2 = ttotal;
} }
Real t2mT = tswitch2-ttotal; Real t2mT = tswitch2-ttotal;
Real xswitch = x0 + 0.5*a*Sqr(tswitch1) + dx0*tswitch1; Real xswitch = x0 + 0.5*a*Sqr(tswitch1) + dx0*tswitch1;
Real xswitch2 = xswitch + (tswitch2-tswitch1)*v; Real xswitch2 = xswitch + (tswitch2-tswitch1)*v;
if(!FuzzyEquals(xswitch2,x1 - 0.5*a*Sqr(t2mT) + dx1*t2mT,EpsilonX)) { if(!FuzzyEquals(xswitch2,x1 - 0.5*a*Sqr(t2mT) + dx1*t2mT,EpsilonX)) {
PARABOLICWARN("PLP Ramp has incorrect switch 2 position: %g vs %g\n ",xswitch2,x1 - 0.5*a*Sqr(t2mT) + dx1*t2mT); PARABOLICWARN("PLP Ramp has incorrect switch 2 position: %g vs %g\n ",xswitch2,x1 - 0.5*a*Sqr(t2mT) + dx1*t2mT);
PARABOLICLOG("Ramp %g,%g -> %g,%g\n",x0,dx0,x1,dx1); PARABOLIC_RAMP_PLOG("Ramp %g,%g -> %g,%g\n",x0,dx0,x1,dx1);
PARABOLICLOG("Acceleration %g, vel %g, deceleration %g\n",a,v,-a); PARABOLIC_RAMP_PLOG("Acceleration %g, vel %g, deceleration %g\n",a,
PARABOLICLOG("Switch times %g %g %g\n",tswitch1,tswitch2,ttotal); v,-a);
PARABOLIC_RAMP_PLOG("Switch times %g %g %g\n",tswitch1,tswitch2,tto
tal);
SaveRamp("PLP_SolveMinTime_failure.dat",x0,dx0,x1,dx1,amax,vmax,tim eLowerBound); SaveRamp("PLP_SolveMinTime_failure.dat",x0,dx0,x1,dx1,amax,vmax,tim eLowerBound);
//getchar();
//return false; //return false;
} }
return true; return true;
} }
bool PLPRamp::SolveMinAccel(Real endTime,Real vmax) bool PLPRamp::SolveMinAccel(Real endTime,Real vmax)
{ {
Real a1 = CalcMinAccel(endTime,vmax); Real a1 = CalcMinAccel(endTime,vmax);
Real a2 = CalcMinAccel(endTime,-vmax); Real a2 = CalcMinAccel(endTime,-vmax);
a = Inf; a = Inf;
skipping to change at line 1098 skipping to change at line 1180
tswitch1 = tswitch2 = 0.5*(tswitch1+tswitch2); tswitch1 = tswitch2 = 0.5*(tswitch1+tswitch2);
} }
if(tswitch2 > endTime && FuzzyEquals(tswitch2,endTime,EpsilonT)) { if(tswitch2 > endTime && FuzzyEquals(tswitch2,endTime,EpsilonT)) {
tswitch2 = endTime; tswitch2 = endTime;
} }
if(ttotal < 0) { //there was an error computing the total time if(ttotal < 0) { //there was an error computing the total time
PARABOLICWARN("PLPRamp::SolveMinAccel: some numerical error pre vented computing total time\n"); PARABOLICWARN("PLPRamp::SolveMinAccel: some numerical error pre vented computing total time\n");
PARABOLICWARN(" Ramp %g,%g -> %g,%g\n",x0,dx0,x1,dx1); PARABOLICWARN(" Ramp %g,%g -> %g,%g\n",x0,dx0,x1,dx1);
PARABOLICWARN(" endTime %g, accel %g, vel %g, switch times %g %g, total time %g\n",endTime,a,v,tswitch1,tswitch2,ttotal); PARABOLICWARN(" endTime %g, accel %g, vel %g, switch times %g %g, total time %g\n",endTime,a,v,tswitch1,tswitch2,ttotal);
SaveRamp("PLP_SolveMinAccel_failure.dat",x0,dx0,x1,dx1,-1,vmax, endTime); SaveRamp("PLP_SolveMinAccel_failure.dat",x0,dx0,x1,dx1,-1,vmax, endTime);
//getchar();
return false; return false;
} }
} }
if(ttotal > endTime + EpsilonT) { if(ttotal > endTime + EpsilonT) {
PARABOLICWARN("PLPRamp::SolveMinAccel: total time greater than endT ime!\n"); PARABOLICWARN("PLPRamp::SolveMinAccel: total time greater than endT ime!\n");
PARABOLICWARN(" endTime %g, accel %g, vel %g, switch times %g %g, total time %g\n",endTime,a,v,tswitch1,tswitch2,ttotal); PARABOLICWARN(" endTime %g, accel %g, vel %g, switch times %g %g, total time %g\n",endTime,a,v,tswitch1,tswitch2,ttotal);
SaveRamp("PLP_SolveMinAccel_failure.dat",x0,dx0,x1,dx1,-1,vmax,endT ime); SaveRamp("PLP_SolveMinAccel_failure.dat",x0,dx0,x1,dx1,-1,vmax,endT ime);
return false; return false;
} }
if(fabs(ttotal-endTime) >= EpsilonT) { if(fabs(ttotal-endTime) >= EpsilonT) {
PARABOLICWARN("PLPRamp::SolveMinAccel: total time and endTime are d ifferent!\n"); PARABOLICWARN("PLPRamp::SolveMinAccel: total time and endTime are d ifferent!\n");
PARABOLICWARN(" endTime %g, accel %g, vel %g, switch times %g %g, total time %g\n",endTime,a,v,tswitch1,tswitch2,ttotal); PARABOLICWARN(" endTime %g, accel %g, vel %g, switch times %g %g, total time %g\n",endTime,a,v,tswitch1,tswitch2,ttotal);
SaveRamp("PLP_SolveMinAccel_failure.dat",x0,dx0,x1,dx1,-1,vmax,endT ime); SaveRamp("PLP_SolveMinAccel_failure.dat",x0,dx0,x1,dx1,-1,vmax,endT ime);
} }
PARABOLIC_ASSERT(fabs(ttotal-endTime) < EpsilonT); PARABOLIC_RAMP_ASSERT(fabs(ttotal-endTime) < EpsilonT);
//fiddle with the numerical errors //fiddle with the numerical errors
ttotal = endTime; ttotal = endTime;
if(tswitch2 > ttotal) tswitch2=ttotal; if(tswitch2 > ttotal) tswitch2=ttotal;
if(tswitch1 > ttotal) tswitch1=ttotal; if(tswitch1 > ttotal) tswitch1=ttotal;
Real t2mT = tswitch2-ttotal; Real t2mT = tswitch2-ttotal;
Real xswitch = x0 + 0.5*a*Sqr(tswitch1) + dx0*tswitch1; Real xswitch = x0 + 0.5*a*Sqr(tswitch1) + dx0*tswitch1;
Real xswitch2 = xswitch + (tswitch2-tswitch1)*v; Real xswitch2 = xswitch + (tswitch2-tswitch1)*v;
if(!FuzzyEquals(xswitch2,x1 - 0.5*a*Sqr(t2mT) + dx1*t2mT,EpsilonX)) { if(!FuzzyEquals(xswitch2,x1 - 0.5*a*Sqr(t2mT) + dx1*t2mT,EpsilonX)) {
PARABOLICWARN("PLP Ramp has incorrect switch 2 position: %g vs %g\n ",xswitch2,x1 - 0.5*a*Sqr(t2mT) + dx1*t2mT); PARABOLICWARN("PLP Ramp has incorrect switch 2 position: %g vs %g\n ",xswitch2,x1 - 0.5*a*Sqr(t2mT) + dx1*t2mT);
PARABOLICLOG("Ramp %g,%g -> %g,%g\n",x0,dx0,x1,dx1); PARABOLIC_RAMP_PLOG("Ramp %g,%g -> %g,%g\n",x0,dx0,x1,dx1);
PARABOLICLOG("Acceleration %g, vel %g, deceleration %g\n",a,v,-a); PARABOLIC_RAMP_PLOG("Acceleration %g, vel %g, deceleration %g\n",a,
PARABOLICLOG("Switch times %g %g %g\n",tswitch1,tswitch2,ttotal); v,-a);
PARABOLIC_RAMP_PLOG("Switch times %g %g %g\n",tswitch1,tswitch2,tto
tal);
SaveRamp("PLP_SolveMinAccel_failure.dat",x0,dx0,x1,dx1,-1,vmax,endT ime); SaveRamp("PLP_SolveMinAccel_failure.dat",x0,dx0,x1,dx1,-1,vmax,endT ime);
//getchar();
//return false; //return false;
} }
return true; return true;
} }
Real PLPRamp::CalcMinTime2(Real endTime,Real a,Real vmax) const Real PLPRamp::CalcMinTime2(Real endTime,Real a,Real vmax) const
{ {
//Try alternate solution technique with acceleration bounded, time fixe d, velocity variable //Try alternate solution technique with acceleration bounded, time fixe d, velocity variable
Real b = -a*endTime - (dx1+dx0); Real b = -a*endTime - (dx1+dx0);
Real c = a*(x1-x0) + (Sqr(dx0)+Sqr(dx1))*0.5; Real c = a*(x1-x0) + (Sqr(dx0)+Sqr(dx1))*0.5;
Real v1,v2; Real v1,v2;
//PARABOLICLOG("Quadratic coeffs %g, %g, %g\n",1.0,b,c); //PARABOLIC_RAMP_PLOG("Quadratic coeffs %g, %g, %g\n",1.0,b,c);
int res=quadratic(1.0,b,c,v1,v2); int res=quadratic(1.0,b,c,v1,v2);
//PARABOLICLOG("Quadratic res %d, accel %g, velocities %g %g\n",res,a,v 1,v2); //PARABOLIC_RAMP_PLOG("Quadratic res %d, accel %g, velocities %g %g\n", res,a,v1,v2);
if(res >= 1) { if(res >= 1) {
Real ts1 = (v1-dx0)/a; Real ts1 = (v1-dx0)/a;
Real ts2 = endTime - (v1-dx1)/a; Real ts2 = endTime - (v1-dx1)/a;
//PARABOLICLOG("Solution 1 times %g %g %g\n",ts1,ts2,endTime); //PARABOLIC_RAMP_PLOG("Solution 1 times %g %g %g\n",ts1,ts2,endTime
if(Abs(v1) <= vmax && ts1 >= 0 && ts2 >= ts1 && ts2 <= endTime) ret );
urn v1; if(Abs(v1) <= vmax && ts1 >= 0 && ts2 >= ts1 && ts2 <= endTime) ret
//it's a valid solution! urn v1;
//it's a valid solution!
} }
if(res == 2) { if(res == 2) {
Real ts1 = (v2-dx0)/a; Real ts1 = (v2-dx0)/a;
Real ts2 = endTime - (v2-dx1)/a; Real ts2 = endTime - (v2-dx1)/a;
//PARABOLICLOG("Solution 2 times %g %g %g\n",ts1,ts2,endTime); //PARABOLIC_RAMP_PLOG("Solution 2 times %g %g %g\n",ts1,ts2,endTime
if(Abs(v2) <= vmax && ts1 >= 0 && ts2 >= ts1 && ts2 <= endTime) ret );
urn v2; if(Abs(v2) <= vmax && ts1 >= 0 && ts2 >= ts1 && ts2 <= endTime) ret
//it's a valid solution! urn v2;
//it's a valid solution!
} }
return -1; return -1;
} }
void ParabolicRamp1D::SetConstant(Real x,Real t) void ParabolicRamp1D::SetConstant(Real x,Real t)
{ {
x0 = x1 = x; x0 = x1 = x;
dx0 = dx1 = 0; dx0 = dx1 = 0;
tswitch1=tswitch2=ttotal=t; tswitch1=tswitch2=ttotal=t;
v = a1 = a2 = 0; v = a1 = a2 = 0;
} }
void ParabolicRamp1D::SetLinear(Real _x0,Real _x1,Real t) void ParabolicRamp1D::SetLinear(Real _x0,Real _x1,Real t)
{ {
PARABOLIC_ASSERT(t > 0); PARABOLIC_RAMP_ASSERT(t > 0);
x0 = _x0; x0 = _x0;
x1 = _x1; x1 = _x1;
v = dx0 = dx1 = (_x1-_x0)/t; v = dx0 = dx1 = (_x1-_x0)/t;
a1 = a2 = 0; a1 = a2 = 0;
tswitch1 = 0; tswitch1 = 0;
tswitch2 = t; tswitch2 = t;
ttotal = t; ttotal = t;
} }
Real ParabolicRamp1D::Evaluate(Real t) const Real ParabolicRamp1D::Evaluate(Real t) const
skipping to change at line 1268 skipping to change at line 1348
//some slight numerical error caused velocity to exceed maximum //some slight numerical error caused velocity to exceed maximum
a1 = pp.a; a1 = pp.a;
a2 = -pp.a; a2 = -pp.a;
v = 0; v = 0;
tswitch1 = tswitch2 = pp.tswitch; tswitch1 = tswitch2 = pp.tswitch;
ttotal = pp.ttotal; ttotal = pp.ttotal;
if(IsValid()) return true; if(IsValid()) return true;
} }
a1 = a2 = v = 0; a1 = a2 = v = 0;
tswitch1 = tswitch2 = ttotal = -1; tswitch1 = tswitch2 = ttotal = -1;
PARABOLICLOG("No ramp equation could solve for min-accel!\n"); PARABOLIC_RAMP_PLOG("No ramp equation could solve for min-accel!\n"
PARABOLICLOG("x0=%g, x1=%g, dx0=%g, dx1=%g\n",x0,x1,dx0,dx1); );
PARABOLICLOG("end time %g, vmax = %g\n",endTime,vmax); PARABOLIC_RAMP_PLOG("x0=%g, x1=%g, dx0=%g, dx1=%g\n",x0,x1,dx0,dx1)
;
PARABOLIC_RAMP_PLOG("end time %g, vmax = %g\n",endTime,vmax);
PARABOLICLOG("P=%d, PP=%d, PLP=%d\n",(int)pres,(int)ppres,(int)plpr PARABOLIC_RAMP_PLOG("P=%d, PP=%d, PLP=%d\n",(int)pres,(int)ppres,(i
es); nt)plpres);
PARABOLICLOG("p.a = %g, max vel=%g, end x=%g, end dx=%g\n",p.a,p.Ma PARABOLIC_RAMP_PLOG("p.a = %g, max vel=%g, end x=%g, end dx=%g\n",p
xVelocity(),p.Evaluate(endTime),p.Derivative(endTime)); .a,p.MaxVelocity(),p.Evaluate(endTime),p.Derivative(endTime));
PARABOLICLOG("pp.a = %g, max vel=%g\n",pp.a,pp.MaxVelocity()); PARABOLIC_RAMP_PLOG("pp.a = %g, max vel=%g\n",pp.a,pp.MaxVelocity()
PARABOLICLOG("plp.a = %g, v=%g\n",plp.a,plp.v); );
PARABOLIC_RAMP_PLOG("plp.a = %g, v=%g\n",plp.a,plp.v);
Real switch1,switch2; Real switch1,switch2;
Real apn = pp.CalcMinAccel(endTime,1.0,switch1); Real apn = pp.CalcMinAccel(endTime,1.0,switch1);
Real anp = pp.CalcMinAccel(endTime,-1.0,switch2); Real anp = pp.CalcMinAccel(endTime,-1.0,switch2);
PARABOLICLOG("PP Calcuations: +: %g %g, -: %g %g\n",apn,switch1,anp ,switch2); PARABOLIC_RAMP_PLOG("PP Calcuations: +: %g %g, -: %g %g\n",apn,swit ch1,anp,switch2);
SaveRamp("Ramp_SolveMinAccel_failure.dat",x0,dx0,x1,dx1,-1,vmax,end Time); SaveRamp("Ramp_SolveMinAccel_failure.dat",x0,dx0,x1,dx1,-1,vmax,end Time);
return false; return false;
} }
PARABOLIC_ASSERT(ttotal==endTime); PARABOLIC_RAMP_ASSERT(ttotal==endTime);
if(!IsValid()) { if(!IsValid()) {
PARABOLICLOG("Invalid min-accel!\n"); PARABOLIC_RAMP_PLOG("Invalid min-accel!\n");
PARABOLICLOG("x0=%g, x1=%g, dx0=%g, dx1=%g\n",x0,x1,dx0,dx1); PARABOLIC_RAMP_PLOG("x0=%g, x1=%g, dx0=%g, dx1=%g\n",x0,x1,dx0,dx1)
PARABOLICLOG("end time %g, vmax = %g\n",endTime,vmax); ;
PARABOLIC_RAMP_PLOG("end time %g, vmax = %g\n",endTime,vmax);
PARABOLICLOG("P=%d, PP=%d, PLP=%d\n",(int)pres,(int)ppres,(int)plpr PARABOLIC_RAMP_PLOG("P=%d, PP=%d, PLP=%d\n",(int)pres,(int)ppres,(i
es); nt)plpres);
PARABOLICLOG("p.a = %g, max vel=%g\n",p.a,p.MaxVelocity()); PARABOLIC_RAMP_PLOG("p.a = %g, max vel=%g\n",p.a,p.MaxVelocity());
PARABOLICLOG("pp.a = %g, max vel=%g\n",pp.a,pp.MaxVelocity()); PARABOLIC_RAMP_PLOG("pp.a = %g, max vel=%g\n",pp.a,pp.MaxVelocity()
PARABOLICLOG("plp.a = %g, v=%g\n",plp.a,plp.v); );
PARABOLIC_RAMP_PLOG("plp.a = %g, v=%g\n",plp.a,plp.v);
return false; return false;
} }
return true; return true;
} }
bool ParabolicRamp1D::SolveMinTime(Real amax,Real vmax) bool ParabolicRamp1D::SolveMinTime(Real amax,Real vmax)
{ {
ParabolicRamp p; ParabolicRamp p;
PPRamp pp; PPRamp pp;
PLPRamp plp; PLPRamp plp;
skipping to change at line 1349 skipping to change at line 1429
ttotal = pp.ttotal; ttotal = pp.ttotal;
} }
if(plpres && plp.ttotal < ttotal) { if(plpres && plp.ttotal < ttotal) {
a1 = plp.a; a1 = plp.a;
v = plp.v; v = plp.v;
tswitch1 = plp.tswitch1; tswitch1 = plp.tswitch1;
tswitch2 = plp.tswitch2; tswitch2 = plp.tswitch2;
ttotal = plp.ttotal; ttotal = plp.ttotal;
} }
if(IsInf(ttotal)) { if(IsInf(ttotal)) {
PARABOLICLOG("No ramp equation could solve for min-time!\n"); PARABOLIC_RAMP_PLOG("No ramp equation could solve for min-time!\n")
PARABOLICLOG("x0=%g, x1=%g, dx0=%g, dx1=%g\n",x0,x1,dx0,dx1); ;
PARABOLICLOG("vmax = %g, amax = %g\n",vmax,amax); PARABOLIC_RAMP_PLOG("x0=%g, x1=%g, dx0=%g, dx1=%g\n",x0,x1,dx0,dx1)
PARABOLICLOG("P=%d, PP=%d, PLP=%d\n",(int)pres,(int)ppres,(int)plpr ;
es); PARABOLIC_RAMP_PLOG("vmax = %g, amax = %g\n",vmax,amax);
PARABOLIC_RAMP_PLOG("P=%d, PP=%d, PLP=%d\n",(int)pres,(int)ppres,(i
nt)plpres);
if(pres) if(pres)
PARABOLICLOG(" P a=%g, ttotal=%g\n",p.a,p.ttotal); PARABOLIC_RAMP_PLOG(" P a=%g, ttotal=%g\n",p.a,p.ttotal);
if(ppres) if(ppres)
PARABOLICLOG(" PP a=%g, tswitch=%g, ttotal=%g\n",pp.a,pp.tswit ch,pp.ttotal); PARABOLIC_RAMP_PLOG(" PP a=%g, tswitch=%g, ttotal=%g\n",pp.a,p p.tswitch,pp.ttotal);
if(plpres) if(plpres)
PARABOLICLOG(" PLP a=%g, tswitch=%g, %g, ttotal=%g\n",pp.a,plp .tswitch1,plp.tswitch2,plp.ttotal); PARABOLIC_RAMP_PLOG(" PLP a=%g, tswitch=%g, %g, ttotal=%g\n",p p.a,plp.tswitch1,plp.tswitch2,plp.ttotal);
SaveRamp("Ramp_SolveMinTime_failure.dat",x0,dx0,x1,dx1,amax,vmax,-1 ); SaveRamp("Ramp_SolveMinTime_failure.dat",x0,dx0,x1,dx1,amax,vmax,-1 );
a1 = a2 = v = 0; a1 = a2 = v = 0;
tswitch1 = tswitch2 = ttotal = -1; tswitch1 = tswitch2 = ttotal = -1;
return false; return false;
} }
a2 = -a1; a2 = -a1;
//cout<<"switch time 1: "<<tswitch1<<", 2: "<<tswitch2<<", total "<<tto tal<<endl; //cout<<"switch time 1: "<<tswitch1<<", 2: "<<tswitch2<<", total "<<tto tal<<endl;
if(!IsValid()) { if(!IsValid()) {
PARABOLICLOG("Failure to find valid path!\n"); PARABOLIC_RAMP_PLOG("Failure to find valid path!\n");
PARABOLICLOG("x0=%g, x1=%g, dx0=%g, dx1=%g\n",x0,x1,dx0,dx1); PARABOLIC_RAMP_PLOG("x0=%g, x1=%g, dx0=%g, dx1=%g\n",x0,x1,dx0,dx1)
PARABOLICLOG("vmax = %g, amax = %g\n",vmax,amax); ;
PARABOLICLOG("P=%d, PP=%d, PLP=%d\n",(int)pres,(int)ppres,(int)plpr PARABOLIC_RAMP_PLOG("vmax = %g, amax = %g\n",vmax,amax);
es); PARABOLIC_RAMP_PLOG("P=%d, PP=%d, PLP=%d\n",(int)pres,(int)ppres,(i
//getchar(); nt)plpres);
} }
return true; return true;
} }
bool ParabolicRamp1D::SolveMinTime2(Real amax,Real vmax,Real tLowerBound) bool ParabolicRamp1D::SolveMinTime2(Real amax,Real vmax,Real tLowerBound)
{ {
ParabolicRamp p; ParabolicRamp p;
PPRamp pp; PPRamp pp;
PLPRamp plp; PLPRamp plp;
p.x0 = pp.x0 = plp.x0 = x0; p.x0 = pp.x0 = plp.x0 = x0;
skipping to change at line 1426 skipping to change at line 1505
ttotal = pp.ttotal; ttotal = pp.ttotal;
} }
if(plpres && plp.ttotal < ttotal) { if(plpres && plp.ttotal < ttotal) {
a1 = plp.a; a1 = plp.a;
v = plp.v; v = plp.v;
tswitch1 = plp.tswitch1; tswitch1 = plp.tswitch1;
tswitch2 = plp.tswitch2; tswitch2 = plp.tswitch2;
ttotal = plp.ttotal; ttotal = plp.ttotal;
} }
if(IsInf(ttotal)) { if(IsInf(ttotal)) {
PARABOLICLOG("No ramp equation could solve for min-time (2)!\n"); PARABOLIC_RAMP_PLOG("No ramp equation could solve for min-time (2)!
PARABOLICLOG("x0=%g, x1=%g, dx0=%g, dx1=%g\n",x0,x1,dx0,dx1); \n");
PARABOLICLOG("vmax = %g, amax = %g, tmax = %g\n",vmax,amax,tLowerBo PARABOLIC_RAMP_PLOG("x0=%g, x1=%g, dx0=%g, dx1=%g\n",x0,x1,dx0,dx1)
und); ;
PARABOLICLOG("P=%d, PP=%d, PLP=%d\n",(int)pres,(int)ppres,(int)plpr PARABOLIC_RAMP_PLOG("vmax = %g, amax = %g, tmax = %g\n",vmax,amax,t
es); LowerBound);
PARABOLIC_RAMP_PLOG("P=%d, PP=%d, PLP=%d\n",(int)pres,(int)ppres,(i
nt)plpres);
if(pres) if(pres)
PARABOLICLOG(" P a=%g, ttotal=%g\n",p.a,p.ttotal); PARABOLIC_RAMP_PLOG(" P a=%g, ttotal=%g\n",p.a,p.ttotal);
if(ppres) if(ppres)
PARABOLICLOG(" PP a=%g, tswitch=%g, ttotal=%g\n",pp.a,pp.tswit ch,pp.ttotal); PARABOLIC_RAMP_PLOG(" PP a=%g, tswitch=%g, ttotal=%g\n",pp.a,p p.tswitch,pp.ttotal);
if(plpres) if(plpres)
PARABOLICLOG(" PLP a=%g, tswitch=%g, %g, ttotal=%g\n",pp.a,plp .tswitch1,plp.tswitch2,plp.ttotal); PARABOLIC_RAMP_PLOG(" PLP a=%g, tswitch=%g, %g, ttotal=%g\n",p p.a,plp.tswitch1,plp.tswitch2,plp.ttotal);
ppres = pp.SolveMinTime(amax); ppres = pp.SolveMinTime(amax);
plpres = plp.SolveMinTime(amax,vmax); plpres = plp.SolveMinTime(amax,vmax);
PARABOLICLOG("unconstrained PP (%d): %g, PLP (%d): %g\n",(int)ppres ,pp.ttotal,(int)plpres,plp.ttotal); PARABOLIC_RAMP_PLOG("unconstrained PP (%d): %g, PLP (%d): %g\n",(in t)ppres,pp.ttotal,(int)plpres,plp.ttotal);
SaveRamp("Ramp_SolveMinTime_failure.dat",x0,dx0,x1,dx1,amax,vmax,tL owerBound); SaveRamp("Ramp_SolveMinTime_failure.dat",x0,dx0,x1,dx1,amax,vmax,tL owerBound);
a1 = a2 = v = 0; a1 = a2 = v = 0;
tswitch1 = tswitch2 = ttotal = -1; tswitch1 = tswitch2 = ttotal = -1;
return false; return false;
} }
a2 = -a1; a2 = -a1;
//cout<<"switch time 1: "<<tswitch1<<", 2: "<<tswitch2<<", total "<<tto tal<<endl; //cout<<"switch time 1: "<<tswitch1<<", 2: "<<tswitch2<<", total "<<tto tal<<endl;
if(!IsValid()) { if(!IsValid()) {
PARABOLICLOG("ParabolicRamp1D::SolveMinTime: Failure to find valid PARABOLIC_RAMP_PLOG("ParabolicRamp1D::SolveMinTime: Failure to find
path!\n"); valid path!\n");
PARABOLICLOG("x0=%g, x1=%g, dx0=%g, dx1=%g\n",x0,x1,dx0,dx1); PARABOLIC_RAMP_PLOG("x0=%g, x1=%g, dx0=%g, dx1=%g\n",x0,x1,dx0,dx1)
PARABOLICLOG("vmax = %g, amax = %g\n",vmax,amax); ;
PARABOLICLOG("P=%d, PP=%d, PLP=%d\n",(int)pres,(int)ppres,(int)plpr PARABOLIC_RAMP_PLOG("vmax = %g, amax = %g\n",vmax,amax);
es); PARABOLIC_RAMP_PLOG("P=%d, PP=%d, PLP=%d\n",(int)pres,(int)ppres,(i
nt)plpres);
SaveRamp("Ramp_SolveMinTime_failure.dat",x0,dx0,x1,dx1,amax,vmax,tL owerBound); SaveRamp("Ramp_SolveMinTime_failure.dat",x0,dx0,x1,dx1,amax,vmax,tL owerBound);
//getchar();
} }
PARABOLIC_ASSERT(ttotal >= tLowerBound); PARABOLIC_RAMP_ASSERT(ttotal >= tLowerBound);
return true; return true;
} }
void ParabolicRamp1D::SolveBraking(Real amax) void ParabolicRamp1D::SolveBraking(Real amax)
{ {
tswitch1 = 0; tswitch1 = 0;
tswitch2 = 0; tswitch2 = 0;
a1 = Sign(dx0)*amax; a1 = Sign(dx0)*amax;
v = 0; v = 0;
a2 = -Sign(dx0)*amax; a2 = -Sign(dx0)*amax;
ttotal = Abs(dx0)/amax; ttotal = Abs(dx0)/amax;
x1 = x0 + dx0*ttotal + 0.5*Sqr(ttotal)*a2; x1 = x0 + dx0*ttotal + 0.5*Sqr(ttotal)*a2;
dx1 = 0; dx1 = 0;
PARABOLIC_ASSERT(IsValid()); PARABOLIC_RAMP_ASSERT(IsValid());
} }
void ParabolicRamp1D::Dilate(Real timeScale) void ParabolicRamp1D::Dilate(Real timeScale)
{ {
tswitch1*=timeScale; tswitch1*=timeScale;
tswitch2*=timeScale; tswitch2*=timeScale;
ttotal*=timeScale; ttotal*=timeScale;
a1 *= 1.0/Sqr(timeScale); a1 *= 1.0/Sqr(timeScale);
a2 *= 1.0/Sqr(timeScale); a2 *= 1.0/Sqr(timeScale);
v *= 1.0/timeScale; Real invTimeScale = 1.0/timeScale;
v *= invTimeScale;
dx0 *= invTimeScale;
dx1 *= invTimeScale;
} }
void ParabolicRamp1D::TrimFront(Real tcut) void ParabolicRamp1D::TrimFront(Real tcut)
{ {
if(tcut > ttotal) { if(tcut > ttotal) {
PARABOLICLOG("Hmm... want to trim front of curve at time %g, end ti me %g\n",tcut,ttotal); PARABOLIC_RAMP_PLOG("Hmm... want to trim front of curve at time %g, end time %g\n",tcut,ttotal);
} }
PARABOLIC_ASSERT(tcut <= ttotal); PARABOLIC_RAMP_ASSERT(tcut <= ttotal);
x0 = Evaluate(tcut); x0 = Evaluate(tcut);
dx0 = Derivative(tcut); dx0 = Derivative(tcut);
ttotal -= tcut; ttotal -= tcut;
tswitch1 -= tcut; tswitch1 -= tcut;
tswitch2 -= tcut; tswitch2 -= tcut;
if(tswitch1 < 0) tswitch1=0; if(tswitch1 < 0) tswitch1=0;
if(tswitch2 < 0) tswitch2=0; if(tswitch2 < 0) tswitch2=0;
PARABOLIC_ASSERT(IsValid()); PARABOLIC_RAMP_ASSERT(IsValid());
} }
void ParabolicRamp1D::TrimBack(Real tcut) void ParabolicRamp1D::TrimBack(Real tcut)
{ {
//PARABOLIC_ASSERT(IsValid()); //PARABOLIC_RAMP_ASSERT(IsValid());
x1 = Evaluate(ttotal-tcut); x1 = Evaluate(ttotal-tcut);
dx1 = Derivative(ttotal-tcut); dx1 = Derivative(ttotal-tcut);
ttotal -= tcut; ttotal -= tcut;
tswitch1 = Min(tswitch1,ttotal); tswitch1 = Min(tswitch1,ttotal);
tswitch2 = Min(tswitch2,ttotal); tswitch2 = Min(tswitch2,ttotal);
PARABOLIC_ASSERT(IsValid()); PARABOLIC_RAMP_ASSERT(IsValid());
} }
void ParabolicRamp1D::Bounds(Real& xmin,Real& xmax) const void ParabolicRamp1D::Bounds(Real& xmin,Real& xmax) const
{ {
Bounds(0,ttotal,xmin,xmax); Bounds(0,ttotal,xmin,xmax);
} }
void ParabolicRamp1D::Bounds(Real ta,Real tb,Real& xmin,Real& xmax) const void ParabolicRamp1D::Bounds(Real ta,Real tb,Real& xmin,Real& xmax) const
{ {
if(ta > tb) { //orient the interval if(ta > tb) { //orient the interval
skipping to change at line 1623 skipping to change at line 1704
if(!FuzzyEquals(a2*t2mT + dx1,v,EpsilonV)) { if(!FuzzyEquals(a2*t2mT + dx1,v,EpsilonV)) {
PARABOLICWARN("Ramp has incorrect switch 2 speed: %g vs %g\n",a 2*t2mT + dx1,v); PARABOLICWARN("Ramp has incorrect switch 2 speed: %g vs %g\n",a 2*t2mT + dx1,v);
return false; return false;
} }
} }
//check switch2 //check switch2
Real xswitch = x0 + 0.5*a1*Sqr(tswitch1) + dx0*tswitch1; Real xswitch = x0 + 0.5*a1*Sqr(tswitch1) + dx0*tswitch1;
Real xswitch2 = xswitch + (tswitch2-tswitch1)*v; Real xswitch2 = xswitch + (tswitch2-tswitch1)*v;
if(!FuzzyEquals(xswitch2,x1 + 0.5*a2*Sqr(t2mT) + dx1*t2mT,EpsilonX)) { if(!FuzzyEquals(xswitch2,x1 + 0.5*a2*Sqr(t2mT) + dx1*t2mT,EpsilonX)) {
PARABOLICWARN("Ramp has incorrect switch 2 position: %g vs %g\n",xs witch2,x1 + 0.5*a2*Sqr(t2mT) + dx1*t2mT); PARABOLICWARN("Ramp has incorrect switch 2 position: %g vs %g\n",xs witch2,x1 + 0.5*a2*Sqr(t2mT) + dx1*t2mT);
PARABOLICLOG("Ramp %g,%g -> %g,%g\n",x0,dx0,x1,dx1); PARABOLIC_RAMP_PLOG("Ramp %g,%g -> %g,%g\n",x0,dx0,x1,dx1);
PARABOLICLOG("Acceleration %g, vel %g, deceleration %g\n",a1,v,a2); PARABOLIC_RAMP_PLOG("Acceleration %g, vel %g, deceleration %g\n",a1
PARABOLICLOG("Switch times %g %g %g\n",tswitch1,tswitch2,ttotal); ,v,a2);
PARABOLIC_RAMP_PLOG("Switch times %g %g %g\n",tswitch1,tswitch2,tto
tal);
return false; return false;
} }
return true; return true;
} }
void ParabolicRampND::SetConstant(const Vector& x,Real t) void ParabolicRampND::SetConstant(const Vector& x,Real t)
{ {
x0 = x1 = x; x0 = x1 = x;
dx0.resize(x.size()); dx0.resize(x.size());
dx1.resize(x.size()); dx1.resize(x.size());
fill(dx0.begin(),dx0.end(),0); fill(dx0.begin(),dx0.end(),0);
fill(dx1.begin(),dx1.end(),0); fill(dx1.begin(),dx1.end(),0);
endTime = t; endTime = t;
ramps.resize(x.size()); ramps.resize(x.size());
for(size_t i=0; i<x.size(); i++) for(size_t i=0; i<x.size(); i++)
ramps[i].SetConstant(x[i],t); ramps[i].SetConstant(x[i],t);
} }
void ParabolicRampND::SetLinear(const Vector& _x0,const Vector& _x1,Real t) void ParabolicRampND::SetLinear(const Vector& _x0,const Vector& _x1,Real t)
{ {
PARABOLIC_ASSERT(_x0.size() == _x1.size()); PARABOLIC_RAMP_ASSERT(_x0.size() == _x1.size());
PARABOLIC_ASSERT(t > 0); PARABOLIC_RAMP_ASSERT(t > 0);
x0 = _x0; x0 = _x0;
x1 = _x1; x1 = _x1;
dx0.resize(_x1.size()); dx0.resize(_x1.size());
for(size_t i=0; i<_x1.size(); i++) for(size_t i=0; i<_x1.size(); i++)
dx0[i] = (_x1[i]-_x0[i])/t; dx0[i] = (_x1[i]-_x0[i])/t;
dx1 = dx0; dx1 = dx0;
endTime = t; endTime = t;
ramps.resize(_x0.size()); ramps.resize(_x0.size());
for(size_t i=0; i<_x0.size(); i++) for(size_t i=0; i<_x0.size(); i++)
ramps[i].SetLinear(_x0[i],_x1[i],t); ramps[i].SetLinear(_x0[i],_x1[i],t);
} }
bool ParabolicRampND::SolveMinTimeLinear(const Vector& amax,const Vector& v max) bool ParabolicRampND::SolveMinTimeLinear(const Vector& amax,const Vector& v max)
{ {
PARABOLIC_ASSERT(x0.size() == dx0.size()); PARABOLIC_RAMP_ASSERT(x0.size() == dx0.size());
PARABOLIC_ASSERT(x1.size() == dx1.size()); PARABOLIC_RAMP_ASSERT(x1.size() == dx1.size());
PARABOLIC_ASSERT(x0.size() == x1.size()); PARABOLIC_RAMP_ASSERT(x0.size() == x1.size());
PARABOLIC_ASSERT(x0.size() == amax.size()); PARABOLIC_RAMP_ASSERT(x0.size() == amax.size());
PARABOLIC_ASSERT(x0.size() == vmax.size()); PARABOLIC_RAMP_ASSERT(x0.size() == vmax.size());
endTime = 0; endTime = 0;
ramps.resize(x0.size()); ramps.resize(x0.size());
ParabolicRamp1D sramp; ParabolicRamp1D sramp;
sramp.x0 = 0; sramp.x0 = 0;
sramp.x1 = 1; sramp.x1 = 1;
sramp.dx0 = 0; sramp.dx0 = 0;
sramp.dx1 = 0; sramp.dx1 = 0;
Real scale=0.0;
Real svmax=Inf,samax=Inf; Real svmax=Inf,samax=Inf;
for(size_t i=0; i<ramps.size(); i++) { for(size_t i=0; i<ramps.size(); i++) {
ramps[i].x0=x0[i]; ramps[i].x0=x0[i];
ramps[i].x1=x1[i]; ramps[i].x1=x1[i];
ramps[i].dx0=dx0[i]; ramps[i].dx0=dx0[i];
ramps[i].dx1=dx1[i]; ramps[i].dx1=dx1[i];
PARABOLIC_RAMP_ASSERT(dx0[i]==0.0);
PARABOLIC_RAMP_ASSERT(dx1[i]==0.0);
if(vmax[i]==0 || amax[i]==0) { if(vmax[i]==0 || amax[i]==0) {
if(!FuzzyEquals(x0[i],x1[i],EpsilonX)) { if(!FuzzyEquals(x0[i],x1[i],EpsilonX)) {
PARABOLICLOG("index %d vmax = %g, amax = %g, X0 != X1 (%g ! PARABOLIC_RAMP_PERROR("index %d vmax = %g, amax = %g, X0 !=
= %g)\n",i,vmax[i],amax[i],x0[i],x1[i]); X1 (%g != %g)\n",i,vmax[i],amax[i],x0[i],x1[i]);
return false; PARABOLIC_RAMP_ASSERT(0);
}
if(!FuzzyEquals(dx0[i],dx1[i],EpsilonV)) {
PARABOLICLOG("index %d vmax = %g, amax = %g, DX0 != DX1 (%g
!= %g)\n",i,vmax[i],amax[i],dx0[i],dx1[i]);
return false;
} }
ramps[i].tswitch1=ramps[i].tswitch2=ramps[i].ttotal=0; ramps[i].tswitch1=ramps[i].tswitch2=ramps[i].ttotal=0;
ramps[i].a1=ramps[i].a1=ramps[i].v=0; ramps[i].a1=ramps[i].a1=ramps[i].v=0;
continue; continue;
} }
if(vmax[i] < svmax*Abs(x1[i]-x0[i])) if(vmax[i] < svmax*Abs(x1[i]-x0[i]))
svmax = vmax[i]/Abs(x1[i]-x0[i]); svmax = vmax[i]/Abs(x1[i]-x0[i]);
if(amax[i] < samax*Abs(x1[i]-x0[i])) if(amax[i] < samax*Abs(x1[i]-x0[i]))
samax = amax[i]/Abs(x1[i]-x0[i]); samax = amax[i]/Abs(x1[i]-x0[i]);
scale = Max(scale,Abs(x1[i]-x0[i]));
} }
if(IsInf(svmax) && IsInf(samax)) { if(scale == 0.0) {
//must have equal start/end state //must have equal start/end state
SetConstant(x0); SetConstant(x0);
return true; return true;
} }
bool res=sramp.SolveMinTime(samax,svmax); //this scale factor makes the problem more numerically stable for start
/end
//locations close to one another
sramp.x1 = scale;
bool res=sramp.SolveMinTime(samax*scale,svmax*scale);
if(!res) { if(!res) {
PARABOLICWARN("Warning in straight-line parameter solve\n"); PARABOLIC_RAMP_PERROR("Warning in straight-line parameter solve\n")
//getchar(); ;
return false; return false;
} }
endTime = sramp.ttotal; endTime = sramp.ttotal;
for(size_t i=0; i<ramps.size(); i++) { for(size_t i=0; i<ramps.size(); i++) {
ramps[i].v = svmax * (x1[i]-x0[i]); ramps[i].v = svmax * (x1[i]-x0[i]);
ramps[i].a1 = samax * (x1[i]-x0[i]); ramps[i].a1 = samax * (x1[i]-x0[i]);
ramps[i].a2 = -samax * (x1[i]-x0[i]); ramps[i].a2 = -samax * (x1[i]-x0[i]);
ramps[i].tswitch1 = sramp.tswitch1; ramps[i].tswitch1 = sramp.tswitch1;
ramps[i].tswitch2 = sramp.tswitch2; ramps[i].tswitch2 = sramp.tswitch2;
ramps[i].ttotal = endTime; ramps[i].ttotal = endTime;
if(!ramps[i].IsValid()) { if(1 ) { //gValidityCheckLevel >= 2) {
PARABOLICWARN("Warning, error in straight-line path formula\n") if(!ramps[i].IsValid()) {
; PARABOLIC_RAMP_PERROR("Warning, error in straight-line path
for(size_t j=0; j<dx0.size(); j++) formula\n");
PARABOLICLOG("%g ",dx0[j]); for(size_t j=0; j<dx0.size(); j++)
for(size_t j=0; j<dx1.size(); j++) PARABOLIC_RAMP_PERROR("%g ",dx0[j]);
PARABOLICLOG("%g ",dx1[j]); for(size_t j=0; j<dx1.size(); j++)
//getchar(); PARABOLIC_RAMP_PERROR("%g ",dx1[j]);
}
} }
//correct for small numerical errors
if(Abs(ramps[i].v) > vmax[i]) { if(Abs(ramps[i].v) > vmax[i]) {
if(Abs(ramps[i].v) > vmax[i]+EpsilonV) { if(Abs(ramps[i].v) > vmax[i]+EpsilonV) {
PARABOLICWARN("Warning, numerical error in straight-line fo PARABOLIC_RAMP_PERROR("Warning, numerical error in straight
rmula?\n"); -line formula?\n");
PARABOLICWARN("velocity |%g|>%g\n",ramps[i].v,vmax[i]); PARABOLIC_RAMP_PERROR("velocity |%g|>%g\n",ramps[i].v,vmax[
//getchar(); i]);
} }
else ramps[i].v = Sign(ramps[i].v)*vmax[i]; else ramps[i].v = Sign(ramps[i].v)*vmax[i];
} }
if(Abs(ramps[i].a1) > amax[i]) { if(Abs(ramps[i].a1) > amax[i]) {
if(Abs(ramps[i].a1) > amax[i]+EpsilonA) { if(Abs(ramps[i].a1) > amax[i]+EpsilonA) {
PARABOLICWARN("Warning, numerical error in straight-line fo PARABOLIC_RAMP_PERROR("Warning, numerical error in straight
rmula?\n"); -line formula?\n");
PARABOLICWARN("accel |%g|>%g\n",ramps[i].a1,amax[i]); PARABOLIC_RAMP_PERROR("accel |%g|>%g\n",ramps[i].a1,amax[i]
//getchar(); );
} }
else ramps[i].a1 = Sign(ramps[i].a1)*amax[i]; else ramps[i].a1 = Sign(ramps[i].a1)*amax[i];
} }
if(Abs(ramps[i].a2) > amax[i]) { if(Abs(ramps[i].a2) > amax[i]) {
if(Abs(ramps[i].a2) > amax[i]+EpsilonA) { if(Abs(ramps[i].a2) > amax[i]+EpsilonA) {
PARABOLICWARN("Warning, numerical error in straight-line fo PARABOLIC_RAMP_PERROR("Warning, numerical error in straight
rmula?\n"); -line formula?\n");
PARABOLICWARN("accel |%g|>%g\n",ramps[i].a2,amax[i]); PARABOLIC_RAMP_PERROR("accel |%g|>%g\n",ramps[i].a2,amax[i]
//getchar(); );
} }
else ramps[i].a2 = Sign(ramps[i].a2)*amax[i]; else ramps[i].a2 = Sign(ramps[i].a2)*amax[i];
} }
//PARABOLIC_ASSERT(ramps[i].IsValid());
} }
return true; return true;
} }
bool ParabolicRampND::SolveMinTime(const Vector& amax,const Vector& vmax) bool ParabolicRampND::SolveMinTime(const Vector& amax,const Vector& vmax)
{ {
PARABOLIC_ASSERT(x0.size() == dx0.size()); PARABOLIC_RAMP_ASSERT(x0.size() == dx0.size());
PARABOLIC_ASSERT(x1.size() == dx1.size()); PARABOLIC_RAMP_ASSERT(x1.size() == dx1.size());
PARABOLIC_ASSERT(x0.size() == x1.size()); PARABOLIC_RAMP_ASSERT(x0.size() == x1.size());
PARABOLIC_ASSERT(x0.size() == amax.size()); PARABOLIC_RAMP_ASSERT(x0.size() == amax.size());
PARABOLIC_ASSERT(x0.size() == vmax.size()); PARABOLIC_RAMP_ASSERT(x0.size() == vmax.size());
endTime = 0; endTime = 0;
ramps.resize(x0.size()); ramps.resize(x0.size());
for(size_t i=0; i<ramps.size(); i++) { for(size_t i=0; i<ramps.size(); i++) {
ramps[i].x0=x0[i]; ramps[i].x0=x0[i];
ramps[i].x1=x1[i]; ramps[i].x1=x1[i];
ramps[i].dx0=dx0[i]; ramps[i].dx0=dx0[i];
ramps[i].dx1=dx1[i]; ramps[i].dx1=dx1[i];
if(vmax[i]==0 || amax[i]==0) { if(vmax[i]==0 || amax[i]==0) {
if(!FuzzyEquals(x0[i],x1[i],EpsilonX)) { if(!FuzzyEquals(x0[i],x1[i],EpsilonX)) {
PARABOLICLOG("index %d vmax = %g, amax = %g, X0 != X1 (%g ! = %g)\n",i,vmax[i],amax[i],x0[i],x1[i]); PARABOLIC_RAMP_PLOG("index %d vmax = %g, amax = %g, X0 != X 1 (%g != %g)\n",i,vmax[i],amax[i],x0[i],x1[i]);
return false; return false;
} }
if(!FuzzyEquals(dx0[i],dx1[i],EpsilonV)) { if(!FuzzyEquals(dx0[i],dx1[i],EpsilonV)) {
PARABOLICLOG("index %d vmax = %g, amax = %g, DX0 != DX1 (%g != %g)\n",i,vmax[i],amax[i],dx0[i],dx1[i]); PARABOLIC_RAMP_PLOG("index %d vmax = %g, amax = %g, DX0 != DX1 (%g != %g)\n",i,vmax[i],amax[i],dx0[i],dx1[i]);
return false; return false;
} }
ramps[i].tswitch1=ramps[i].tswitch2=ramps[i].ttotal=0; ramps[i].tswitch1=ramps[i].tswitch2=ramps[i].ttotal=0;
ramps[i].a1=ramps[i].a2=ramps[i].v=0; ramps[i].a1=ramps[i].a2=ramps[i].v=0;
continue; continue;
} }
if(!ramps[i].SolveMinTime(amax[i],vmax[i])) return false; if(!ramps[i].SolveMinTime(amax[i],vmax[i])) return false;
if(ramps[i].ttotal > endTime) endTime = ramps[i].ttotal; if(ramps[i].ttotal > endTime) endTime = ramps[i].ttotal;
} }
//now we have a candidate end time -- repeat looking through solutions //now we have a candidate end time -- repeat looking through solutions
//until we have solved all ramps //until we have solved all ramps
while(true) { while(true) {
bool solved = true; bool solved = true;
for(size_t i=0; i<ramps.size(); i++) { for(size_t i=0; i<ramps.size(); i++) {
if(ramps[i].ttotal == endTime) continue; if(ramps[i].ttotal == endTime) continue;
if(vmax[i]==0 || amax[i]==0) { if(vmax[i]==0 || amax[i]==0) {
ramps[i].ttotal = endTime; ramps[i].ttotal = endTime;
continue; continue;
} }
if(!ramps[i].SolveMinAccel(endTime,vmax[i])) { if(!ramps[i].SolveMinAccel(endTime,vmax[i])) {
PARABOLICLOG("Failed solving min accel for joint %d\n",i); PARABOLIC_RAMP_PLOG("Failed solving min accel for joint %d\ n",i);
ramps[i].SolveMinTime(amax[i],vmax[i]); ramps[i].SolveMinTime(amax[i],vmax[i]);
PARABOLICLOG("its min time is %g\n",ramps[i].ttotal); PARABOLIC_RAMP_PLOG("its min time is %g\n",ramps[i].ttotal) ;
if(ramps[i].tswitch1==ramps[i].tswitch2) if(ramps[i].tswitch1==ramps[i].tswitch2)
PARABOLICLOG("its type is PP\n"); PARABOLIC_RAMP_PLOG("its type is PP\n");
else if(Abs(ramps[i].v)==vmax[i]) else if(Abs(ramps[i].v)==vmax[i])
PARABOLICLOG("its type is PLP (vmax)\n"); PARABOLIC_RAMP_PLOG("its type is PLP (vmax)\n");
else else
PARABOLICLOG("its type is PLP (v=%g %%)\n",ramps[i].v/v max[i]); PARABOLIC_RAMP_PLOG("its type is PLP (v=%g %%)\n",ramps [i].v/vmax[i]);
SaveRamp("ParabolicRampND_SolveMinAccel_failure.dat",ramps[ i].x0,ramps[i].dx0,ramps[i].x1,ramps[i].dx1,-1,vmax[i],endTime); SaveRamp("ParabolicRampND_SolveMinAccel_failure.dat",ramps[ i].x0,ramps[i].dx0,ramps[i].x1,ramps[i].dx1,-1,vmax[i],endTime);
PARABOLICLOG("Saving to failed_ramps.txt\n"); PARABOLIC_RAMP_PLOG("Saving to failed_ramps.txt\n");
FILE* f=fopen("failed_ramps.txt","w+"); FILE* f=fopen("failed_ramps.txt","w+");
fprintf(f,"MinAccel T=%g, vmax=%g\n",endTime,vmax[i]); fprintf(f,"MinAccel T=%g, vmax=%g\n",endTime,vmax[i]);
fprintf(f,"x0=%g, dx0=%g\n",ramps[i].x0,ramps[i].dx0); fprintf(f,"x0=%g, dx0=%g\n",ramps[i].x0,ramps[i].dx0);
fprintf(f,"x1=%g, dx1=%g\n",ramps[i].x1,ramps[i].dx1); fprintf(f,"x1=%g, dx1=%g\n",ramps[i].x1,ramps[i].dx1);
fprintf(f,"MinTime solution v=%g, t1=%g, t2=%g, T=%g\n",ram ps[i].v,ramps[i].tswitch1,ramps[i].tswitch2,ramps[i].ttotal); fprintf(f,"MinTime solution v=%g, t1=%g, t2=%g, T=%g\n",ram ps[i].v,ramps[i].tswitch1,ramps[i].tswitch2,ramps[i].ttotal);
fprintf(f,"\n"); fprintf(f,"\n");
fclose(f); fclose(f);
return false; return false;
} }
if(Abs(ramps[i].a1) > amax[i] || Abs(ramps[i].a2) > amax[i] || Abs(ramps[i].v) > vmax[i]) { if(Abs(ramps[i].a1) > amax[i] || Abs(ramps[i].a2) > amax[i] || Abs(ramps[i].v) > vmax[i]) {
bool res=ramps[i].SolveMinTime2(amax[i],vmax[i],endTime); bool res=ramps[i].SolveMinTime2(amax[i],vmax[i],endTime);
if(!res) { if(!res) {
PARABOLICLOG("Couldn't solve min-time with lower bound! PARABOLIC_RAMP_PLOG("Couldn't solve min-time with lower
\n"); bound!\n");
//getchar();
return false; return false;
} }
PARABOLIC_ASSERT(ramps[i].ttotal > endTime); PARABOLIC_RAMP_ASSERT(ramps[i].ttotal > endTime);
endTime = ramps[i].ttotal; endTime = ramps[i].ttotal;
solved = false; solved = false;
break; //go back and re-solve break; //go back and re-solve
} }
PARABOLIC_ASSERT(Abs(ramps[i].a1) <= amax[i]+EpsilonA); PARABOLIC_RAMP_ASSERT(Abs(ramps[i].a1) <= amax[i]+EpsilonA);
PARABOLIC_ASSERT(Abs(ramps[i].a2) <= amax[i]+EpsilonA); PARABOLIC_RAMP_ASSERT(Abs(ramps[i].a2) <= amax[i]+EpsilonA);
PARABOLIC_ASSERT(Abs(ramps[i].v) <= vmax[i]+EpsilonV); PARABOLIC_RAMP_ASSERT(Abs(ramps[i].v) <= vmax[i]+EpsilonV);
PARABOLIC_ASSERT(ramps[i].ttotal==endTime); PARABOLIC_RAMP_ASSERT(ramps[i].ttotal==endTime);
} }
//done //done
if(solved) break; if(solved) break;
} }
return true; return true;
} }
bool ParabolicRampND::SolveMinAccel(const Vector& vmax,Real time) bool ParabolicRampND::SolveMinAccel(const Vector& vmax,Real time)
{ {
PARABOLIC_ASSERT(x0.size() == dx0.size()); PARABOLIC_RAMP_ASSERT(x0.size() == dx0.size());
PARABOLIC_ASSERT(x1.size() == dx1.size()); PARABOLIC_RAMP_ASSERT(x1.size() == dx1.size());
PARABOLIC_ASSERT(x0.size() == x1.size()); PARABOLIC_RAMP_ASSERT(x0.size() == x1.size());
PARABOLIC_ASSERT(x0.size() == vmax.size()); PARABOLIC_RAMP_ASSERT(x0.size() == vmax.size());
endTime = time; endTime = time;
ramps.resize(x0.size()); ramps.resize(x0.size());
for(size_t i=0; i<ramps.size(); i++) { for(size_t i=0; i<ramps.size(); i++) {
ramps[i].x0=x0[i]; ramps[i].x0=x0[i];
ramps[i].x1=x1[i]; ramps[i].x1=x1[i];
ramps[i].dx0=dx0[i]; ramps[i].dx0=dx0[i];
ramps[i].dx1=dx1[i]; ramps[i].dx1=dx1[i];
if(vmax[i]==0) { if(vmax[i]==0) {
PARABOLIC_ASSERT(FuzzyEquals(x0[i],x1[i],EpsilonX)); PARABOLIC_RAMP_ASSERT(FuzzyEquals(x0[i],x1[i],EpsilonX));
PARABOLIC_ASSERT(FuzzyEquals(dx0[i],dx1[i],EpsilonV)); PARABOLIC_RAMP_ASSERT(FuzzyEquals(dx0[i],dx1[i],EpsilonV));
ramps[i].tswitch1=ramps[i].tswitch2=ramps[i].ttotal=0; ramps[i].tswitch1=ramps[i].tswitch2=ramps[i].ttotal=0;
ramps[i].a1=ramps[i].a2=ramps[i].v=0; ramps[i].a1=ramps[i].a2=ramps[i].v=0;
continue; continue;
} }
if(!ramps[i].SolveMinAccel(endTime,vmax[i])) { if(!ramps[i].SolveMinAccel(endTime,vmax[i])) {
return false; return false;
} }
} }
return true; return true;
} }
bool ParabolicRampND::SolveMinAccelLinear(const Vector& vmax,Real time) bool ParabolicRampND::SolveMinAccelLinear(const Vector& vmax,Real time)
{ {
PARABOLIC_ASSERT(x0.size() == dx0.size()); PARABOLIC_RAMP_ASSERT(x0.size() == dx0.size());
PARABOLIC_ASSERT(x1.size() == dx1.size()); PARABOLIC_RAMP_ASSERT(x1.size() == dx1.size());
PARABOLIC_ASSERT(x0.size() == x1.size()); PARABOLIC_RAMP_ASSERT(x0.size() == x1.size());
PARABOLIC_ASSERT(x0.size() == vmax.size()); PARABOLIC_RAMP_ASSERT(x0.size() == vmax.size());
endTime = 0; endTime = 0;
ramps.resize(x0.size()); ramps.resize(x0.size());
ParabolicRamp1D sramp; ParabolicRamp1D sramp;
sramp.x0 = 0; sramp.x0 = 0;
sramp.x1 = 1; sramp.x1 = 1;
sramp.dx0 = 0; sramp.dx0 = 0;
sramp.dx1 = 0; sramp.dx1 = 0;
Real svmax=Inf; Real svmax=Inf;
for(size_t i=0; i<ramps.size(); i++) { for(size_t i=0; i<ramps.size(); i++) {
ramps[i].x0=x0[i]; ramps[i].x0=x0[i];
ramps[i].x1=x1[i]; ramps[i].x1=x1[i];
ramps[i].dx0=dx0[i]; ramps[i].dx0=dx0[i];
ramps[i].dx1=dx1[i]; ramps[i].dx1=dx1[i];
if(vmax[i]==0) { if(vmax[i]==0) {
if(!FuzzyEquals(x0[i],x1[i],EpsilonX)) { if(!FuzzyEquals(x0[i],x1[i],EpsilonX)) {
PARABOLICLOG("index %d vmax = %g, X0 != X1 (%g != %g)\n",i, vmax[i],x0[i],x1[i]); PARABOLIC_RAMP_PLOG("index %d vmax = %g, X0 != X1 (%g != %g )\n",i,vmax[i],x0[i],x1[i]);
return false; return false;
} }
if(!FuzzyEquals(dx0[i],dx1[i],EpsilonV)) { if(!FuzzyEquals(dx0[i],dx1[i],EpsilonV)) {
PARABOLICLOG("index %d vmax = %g, DX0 != DX1 (%g != %g)\n", i,vmax[i],dx0[i],dx1[i]); PARABOLIC_RAMP_PLOG("index %d vmax = %g, DX0 != DX1 (%g != %g)\n",i,vmax[i],dx0[i],dx1[i]);
return false; return false;
} }
ramps[i].tswitch1=ramps[i].tswitch2=ramps[i].ttotal=0; ramps[i].tswitch1=ramps[i].tswitch2=ramps[i].ttotal=0;
ramps[i].a1=ramps[i].a1=ramps[i].v=0; ramps[i].a1=ramps[i].a1=ramps[i].v=0;
continue; continue;
} }
if(vmax[i] < svmax*Abs(x1[i]-x0[i])) if(vmax[i] < svmax*Abs(x1[i]-x0[i]))
svmax = vmax[i]/Abs(x1[i]-x0[i]); svmax = vmax[i]/Abs(x1[i]-x0[i]);
} }
if(IsInf(svmax)) { if(IsInf(svmax)) {
//must have equal start/end state //must have equal start/end state
SetConstant(x0); SetConstant(x0);
return true; return true;
} }
bool res=sramp.SolveMinAccel(svmax,time); bool res=sramp.SolveMinAccel(svmax,time);
if(!res) { if(!res) {
PARABOLICWARN("Warning in straight-line parameter solve\n"); PARABOLICWARN("Warning in straight-line parameter solve\n");
//getchar();
return false; return false;
} }
endTime = sramp.ttotal; endTime = sramp.ttotal;
for(size_t i=0; i<ramps.size(); i++) { for(size_t i=0; i<ramps.size(); i++) {
ramps[i].v = sramp.v * (x1[i]-x0[i]); ramps[i].v = sramp.v * (x1[i]-x0[i]);
ramps[i].a1 = sramp.a1 * (x1[i]-x0[i]); ramps[i].a1 = sramp.a1 * (x1[i]-x0[i]);
ramps[i].a2 = sramp.a2 * (x1[i]-x0[i]); ramps[i].a2 = sramp.a2 * (x1[i]-x0[i]);
ramps[i].tswitch1 = sramp.tswitch1; ramps[i].tswitch1 = sramp.tswitch1;
ramps[i].tswitch2 = sramp.tswitch2; ramps[i].tswitch2 = sramp.tswitch2;
ramps[i].ttotal = endTime; ramps[i].ttotal = endTime;
if(!ramps[i].IsValid()) { if(!ramps[i].IsValid()) {
PARABOLICWARN("Warning, error in straight-line path formula\n") ; PARABOLICWARN("Warning, error in straight-line path formula\n") ;
//getchar();
res=false; res=false;
} }
} }
return res; return res;
} }
void ParabolicRampND::SolveBraking(const Vector& amax) void ParabolicRampND::SolveBraking(const Vector& amax)
{ {
PARABOLIC_ASSERT(x0.size() == dx0.size()); PARABOLIC_RAMP_ASSERT(x0.size() == dx0.size());
PARABOLIC_ASSERT(x0.size() == amax.size()); PARABOLIC_RAMP_ASSERT(x0.size() == amax.size());
x1.resize(x0.size()); x1.resize(x0.size());
dx1.resize(x0.size()); dx1.resize(x0.size());
endTime = 0; endTime = 0;
ramps.resize(x0.size()); ramps.resize(x0.size());
for(size_t i=0; i<ramps.size(); i++) { for(size_t i=0; i<ramps.size(); i++) {
if(amax[i]==0) { if(amax[i]==0) {
if(!FuzzyEquals(dx0[i],0.0,EpsilonV)) { if(!FuzzyEquals(dx0[i],0.0,EpsilonV)) {
PARABOLICLOG("index %d amax = %g, DX0 != 0 (%g != 0)\n",i,a PARABOLIC_RAMP_PLOG("index %d amax = %g, DX0 != 0 (%g != 0)
max[i],dx0[i]); \n",i,amax[i],dx0[i]);
PARABOLIC_ASSERT(0); PARABOLIC_RAMP_ASSERT(0);
} }
ramps[i].SetConstant(0); ramps[i].SetConstant(0);
continue; continue;
} }
ramps[i].x0 = x0[i]; ramps[i].x0 = x0[i];
ramps[i].dx0 = dx0[i]; ramps[i].dx0 = dx0[i];
ramps[i].SolveBraking(amax[i]); ramps[i].SolveBraking(amax[i]);
} }
for(size_t i=0; i<ramps.size(); i++) for(size_t i=0; i<ramps.size(); i++)
endTime = Max(endTime,ramps[i].ttotal); endTime = Max(endTime,ramps[i].ttotal);
for(size_t i=0; i<ramps.size(); i++) { for(size_t i=0; i<ramps.size(); i++) {
if(amax[i] != 0 && ramps[i].ttotal != endTime) { if(amax[i] != 0 && ramps[i].ttotal != endTime) {
//scale ramp acceleration to meet endTimeMax //scale ramp acceleration to meet endTimeMax
ramps[i].ttotal = endTime; ramps[i].ttotal = endTime;
//y(t) = x0 + t*dx0 + 1/2 t^2 a //y(t) = x0 + t*dx0 + 1/2 t^2 a
//y'(T) = dx0 + T a = 0 //y'(T) = dx0 + T a = 0
ramps[i].a2 = -dx0[i] / endTime; ramps[i].a2 = -dx0[i] / endTime;
ramps[i].a1 = -ramps[i].a2; ramps[i].a1 = -ramps[i].a2;
ramps[i].x1 = ramps[i].x0 + endTime*ramps[i].dx0 + 0.5*Sqr(endT ime)*ramps[i].a2; ramps[i].x1 = ramps[i].x0 + endTime*ramps[i].dx0 + 0.5*Sqr(endT ime)*ramps[i].a2;
} }
else if(amax[i] == 0.0) {
//end time of constant path
ramps[i].ttotal = endTime;
}
x1[i]=ramps[i].x1; x1[i]=ramps[i].x1;
dx1[i]=0; dx1[i]=0;
} }
PARABOLIC_ASSERT(IsValid()); PARABOLIC_RAMP_ASSERT(IsValid());
} }
void ParabolicRampND::Evaluate(Real t,Vector& x) const void ParabolicRampND::Evaluate(Real t,Vector& x) const
{ {
x.resize(ramps.size()); x.resize(ramps.size());
for(size_t j=0; j<ramps.size(); j++) for(size_t j=0; j<ramps.size(); j++)
x[j]=ramps[j].Evaluate(t); x[j]=ramps[j].Evaluate(t);
} }
void ParabolicRampND::Derivative(Real t,Vector& x) const void ParabolicRampND::Derivative(Real t,Vector& x) const
skipping to change at line 1996 skipping to change at line 2079
void ParabolicRampND::Accel(Real t,Vector& x) const void ParabolicRampND::Accel(Real t,Vector& x) const
{ {
x.resize(ramps.size()); x.resize(ramps.size());
for(size_t j=0; j<ramps.size(); j++) for(size_t j=0; j<ramps.size(); j++)
x[j]=ramps[j].Accel(t); x[j]=ramps[j].Accel(t);
} }
void ParabolicRampND::Output(Real dt,std::vector<Vector>& path) const void ParabolicRampND::Output(Real dt,std::vector<Vector>& path) const
{ {
PARABOLIC_ASSERT(!ramps.empty()); PARABOLIC_RAMP_ASSERT(!ramps.empty());
int size = (int)ceil(endTime/dt)+1; int size = (int)ceil(endTime/dt)+1;
path.resize(size); path.resize(size);
if(size == 1) { if(size == 1) {
path[0].resize(ramps.size()); path[0].resize(ramps.size());
for(size_t j=0; j<ramps.size(); j++) for(size_t j=0; j<ramps.size(); j++)
path[0][j] = ramps[j].x0; path[0][j] = ramps[j].x0;
return; return;
} }
for(int i=0; i<size; i++) { for(int i=0; i<size; i++) {
Real t=endTime*Real(i)/Real(size-1); Real t=endTime*Real(i)/Real(size-1);
skipping to change at line 2030 skipping to change at line 2113
path[i][j]=ramps[j].Evaluate(t); path[i][j]=ramps[j].Evaluate(t);
} }
path[size-1].resize(ramps.size()); path[size-1].resize(ramps.size());
for(size_t j=0;j<ramps.size();j++) for(size_t j=0;j<ramps.size();j++)
path[size-1][j] = ramps[j].x1; path[size-1][j] = ramps[j].x1;
*/ */
} }
void ParabolicRampND::Dilate(Real timeScale) void ParabolicRampND::Dilate(Real timeScale)
{ {
for(size_t i=0; i<ramps.size(); i++) for(size_t i=0; i<ramps.size(); i++) {
ramps[i].Dilate(timeScale); ramps[i].Dilate(timeScale);
dx0[i] = ramps[i].dx0;
dx1[i] = ramps[i].dx1;
}
endTime *= timeScale;
} }
void ParabolicRampND::TrimFront(Real tcut) void ParabolicRampND::TrimFront(Real tcut)
{ {
PARABOLIC_ASSERT(tcut <= endTime); PARABOLIC_RAMP_ASSERT(tcut <= endTime);
Evaluate(tcut,x0); Evaluate(tcut,x0);
Derivative(tcut,dx0); Derivative(tcut,dx0);
endTime -= tcut; endTime -= tcut;
for(size_t i=0; i<ramps.size(); i++) for(size_t i=0; i<ramps.size(); i++)
ramps[i].TrimFront(tcut); ramps[i].TrimFront(tcut);
PARABOLIC_ASSERT(IsValid()); PARABOLIC_RAMP_ASSERT(IsValid());
} }
void ParabolicRampND::TrimBack(Real tcut) void ParabolicRampND::TrimBack(Real tcut)
{ {
for(size_t i=0; i<ramps.size(); i++) for(size_t i=0; i<ramps.size(); i++)
PARABOLIC_ASSERT(endTime == ramps[i].ttotal); PARABOLIC_RAMP_ASSERT(endTime == ramps[i].ttotal);
PARABOLIC_ASSERT(tcut <= endTime); PARABOLIC_RAMP_ASSERT(tcut <= endTime);
Evaluate(endTime-tcut,x1); Evaluate(endTime-tcut,x1);
Derivative(endTime-tcut,dx1); Derivative(endTime-tcut,dx1);
endTime -= tcut; endTime -= tcut;
for(size_t i=0; i<ramps.size(); i++) for(size_t i=0; i<ramps.size(); i++)
ramps[i].TrimBack(tcut); ramps[i].TrimBack(tcut);
PARABOLIC_ASSERT(IsValid()); PARABOLIC_RAMP_ASSERT(IsValid());
} }
void ParabolicRampND::Bounds(Vector& xmin,Vector& xmax) const void ParabolicRampND::Bounds(Vector& xmin,Vector& xmax) const
{ {
xmin.resize(ramps.size()); xmin.resize(ramps.size());
xmax.resize(ramps.size()); xmax.resize(ramps.size());
for(size_t i=0; i<ramps.size(); i++) { for(size_t i=0; i<ramps.size(); i++) {
ramps[i].Bounds(xmin[i],xmax[i]); ramps[i].Bounds(xmin[i],xmax[i]);
} }
} }
skipping to change at line 2131 skipping to change at line 2218
if(!FuzzyEquals(ramps[i].dx1,dx1[i],EpsilonV)) { if(!FuzzyEquals(ramps[i].dx1,dx1[i],EpsilonV)) {
PARABOLICWARN("ParabolicRampND::IsValid(): element %d has diffe rent dx1 %g != %g\n",i,ramps[i].dx1,dx1[i]); PARABOLICWARN("ParabolicRampND::IsValid(): element %d has diffe rent dx1 %g != %g\n",i,ramps[i].dx1,dx1[i]);
return false; return false;
} }
} }
return true; return true;
} }
bool SolveMinTimeBounded(Real x0,Real v0,Real x1,Real v1,Real amax,Real vma x,Real xmin,Real xmax,ParabolicRamp1D& ramp) bool SolveMinTimeBounded(Real x0,Real v0,Real x1,Real v1,Real amax,Real vma x,Real xmin,Real xmax,ParabolicRamp1D& ramp)
{ {
PARABOLIC_ASSERT(x0 >= xmin && x0 <= xmax && x1 >= xmin && x1 <= xmax); PARABOLIC_RAMP_ASSERT(x0 >= xmin && x0 <= xmax && x1 >= xmin && x1 <= x max);
ramp.x0 = x0; ramp.x0 = x0;
ramp.dx0 = v0; ramp.dx0 = v0;
ramp.x1 = x1; ramp.x1 = x1;
ramp.dx1 = v1; ramp.dx1 = v1;
if(!ramp.SolveMinTime(amax,vmax)) return false; if(!ramp.SolveMinTime(amax,vmax)) return false;
Real bmin,bmax; Real bmin,bmax;
ramp.Bounds(bmin,bmax); ramp.Bounds(bmin,bmax);
if(bmin < xmin || bmax > xmax) return false; if(bmin < xmin || bmax > xmax) return false;
return true; return true;
} }
inline Real BrakeTime(Real x,Real v,Real xbound) inline Real BrakeTime(Real x,Real v,Real xbound)
{ {
return 2.0*(xbound-x)/v; Real t;
bool res=SafeEqSolve(v,2.0*(xbound-x),EpsilonX,0,Inf,t);
if(!res) {
PARABOLICWARN("Warning, couldn't solve brake time equation:\n");
PARABOLICWARN("%g*a = %g = 0\n",v,2.0*(xbound-x));
return 0;
}
return t;
} }
inline Real BrakeAccel(Real x,Real v,Real xbound) inline Real BrakeAccel(Real x,Real v,Real xbound)
{ {
Real tb=BrakeTime(x,v,xbound); Real a;
if(FuzzyEquals(tb,0.0,EpsilonT)) return 0; bool res=SafeEqSolve(2.0*(xbound-x),-v*v,EpsilonV,-Inf,Inf,a);
return -v/tb; if(!res) {
PARABOLICWARN("Warning, couldn't solve braking acceleration equatio
n:\n");
PARABOLICWARN("%g*a + %g = 0\n",2.0*(xbound-x),v*v);
return 0;
}
return a;
} }
bool SolveMinAccelBounded(Real x0,Real v0,Real x1,Real v1,Real endTime,Real vmax,Real xmin,Real xmax,std::vector<ParabolicRamp1D>& ramps) bool SolveMinAccelBounded(Real x0,Real v0,Real x1,Real v1,Real endTime,Real vmax,Real xmin,Real xmax,std::vector<ParabolicRamp1D>& ramps)
{ {
PARABOLIC_ASSERT(x0 >= xmin && x0 <= xmax && x1 >= xmin && x1 <= xmax); PARABOLIC_RAMP_ASSERT(x0 >= xmin && x0 <= xmax && x1 >= xmin && x1 <= x max);
ParabolicRamp1D ramp; ParabolicRamp1D ramp;
ramp.x0 = x0; ramp.x0 = x0;
ramp.dx0 = v0; ramp.dx0 = v0;
ramp.x1 = x1; ramp.x1 = x1;
ramp.dx1 = v1; ramp.dx1 = v1;
if(!ramp.SolveMinAccel(endTime,vmax)) return false; if(!ramp.SolveMinAccel(endTime,vmax)) return false;
Real bmin,bmax; Real bmin,bmax;
ramp.Bounds(bmin,bmax); ramp.Bounds(bmin,bmax);
if(bmin >= xmin && bmax <= xmax) { if(bmin >= xmin && bmax <= xmax) {
ramps.resize(1); ramps.resize(1);
skipping to change at line 2291 skipping to change at line 2390
temp[0].x1 = x1; temp[0].x1 = x1;
temp[0].dx1 = v1; temp[0].dx1 = v1;
temp[0].a1 = ba0; temp[0].a1 = ba0;
temp[0].v = 0; temp[0].v = 0;
temp[0].a2 = ba1; temp[0].a2 = ba1;
temp[0].tswitch1 = bt0; temp[0].tswitch1 = bt0;
temp[0].tswitch2 = endTime-bt1; temp[0].tswitch2 = endTime-bt1;
temp[0].ttotal = endTime; temp[0].ttotal = endTime;
ramps = temp; ramps = temp;
amax = Max(Abs(ba0),Abs(ba1)); amax = Max(Abs(ba0),Abs(ba1));
PARABOLIC_ASSERT(temp[0].IsValid()); PARABOLIC_RAMP_ASSERT(temp[0].IsValid());
} }
} }
else { else {
//type IV paths //type IV paths
if(bt0 + bt1 < endTime && Max(Abs(ba0),Abs(ba1)) < amax) { if(bt0 + bt1 < endTime && Max(Abs(ba0),Abs(ba1)) < amax) {
//first segment brakes to one side, last segment brakes to the other //first segment brakes to one side, last segment brakes to the other
//first //first
temp.resize(3); temp.resize(3);
temp[0].x0 = x0; temp[0].x0 = x0;
temp[0].dx0 = v0; temp[0].dx0 = v0;
skipping to change at line 2330 skipping to change at line 2429
temp[2].ttotal = bt1; temp[2].ttotal = bt1;
//middle section //middle section
temp[1].x0 = bx0; temp[1].x0 = bx0;
temp[1].dx0 = 0; temp[1].dx0 = 0;
temp[1].x1 = bx1; temp[1].x1 = bx1;
temp[1].dx1 = 0; temp[1].dx1 = 0;
gMinAccelQuiet = true; gMinAccelQuiet = true;
if(Abs(bx0-bx1) < (endTime-bt0-bt1)*vmax) { if(Abs(bx0-bx1) < (endTime-bt0-bt1)*vmax) {
if(temp[1].SolveMinAccel(endTime - bt0 - bt1,vmax)) { if(temp[1].SolveMinAccel(endTime - bt0 - bt1,vmax)) {
temp[1].Bounds(bmin,bmax); temp[1].Bounds(bmin,bmax);
PARABOLIC_ASSERT(bmin >= xmin && bmax <= xmax); PARABOLIC_RAMP_ASSERT(bmin >= xmin && bmax <= xmax);
if(Max(Abs(temp[1].a1),Abs(temp[1].a2)) < amax) { if(Max(Abs(temp[1].a1),Abs(temp[1].a2)) < amax) {
ramps = temp; ramps = temp;
amax = Max(Max(Abs(temp[1].a1),Abs(temp[1].a2)),Max (Abs(ba0),Abs(ba1))); amax = Max(Max(Abs(temp[1].a1),Abs(temp[1].a2)),Max (Abs(ba0),Abs(ba1)));
} }
} }
} }
gMinAccelQuiet = false; gMinAccelQuiet = false;
} }
} }
if(ramps.empty()) { if(ramps.empty()) {
PARABOLICLOG("SolveMinAccelBounded: Warning, can't find bounded tra PARABOLIC_RAMP_PLOG("SolveMinAccelBounded: Warning, can't find boun
jectory?\n"); ded trajectory?\n");
PARABOLICLOG("x0 %g v0 %g, x1 %g v1 %g\n",x0,v0,x1,v1); PARABOLIC_RAMP_PLOG("x0 %g v0 %g, x1 %g v1 %g\n",x0,v0,x1,v1);
PARABOLICLOG("endTime %g, vmax %g\n",endTime,vmax); PARABOLIC_RAMP_PLOG("endTime %g, vmax %g\n",endTime,vmax);
PARABOLICLOG("x bounds [%g,%g]\n",xmin,xmax); PARABOLIC_RAMP_PLOG("x bounds [%g,%g]\n",xmin,xmax);
//getchar();
return false; return false;
} }
for(size_t i=0; i<ramps.size(); i++) { for(size_t i=0; i<ramps.size(); i++) {
ramps[i].Bounds(bmin,bmax); ramps[i].Bounds(bmin,bmax);
if(bmin < xmin-EpsilonX || bmax > xmax+EpsilonX) { if(bmin < xmin-EpsilonX || bmax > xmax+EpsilonX) {
PARABOLICLOG("SolveMinAccelBounded: Warning, path exceeds bound PARABOLIC_RAMP_PLOG("SolveMinAccelBounded: Warning, path exceed
s?\n"); s bounds?\n");
PARABOLICLOG(" ramp[%d] bounds %g %g, limits %g %g\n",i,bmin,b PARABOLIC_RAMP_PLOG(" ramp[%d] bounds %g %g, limits %g %g\n",i
max,xmin,xmax); ,bmin,bmax,xmin,xmax);
//getchar();
return false; return false;
} }
} }
PARABOLIC_ASSERT(ramps.front().x0 == x0); PARABOLIC_RAMP_ASSERT(ramps.front().x0 == x0);
PARABOLIC_ASSERT(ramps.front().dx0 == v0); PARABOLIC_RAMP_ASSERT(ramps.front().dx0 == v0);
PARABOLIC_ASSERT(ramps.back().x1 == x1); PARABOLIC_RAMP_ASSERT(ramps.back().x1 == x1);
PARABOLIC_ASSERT(ramps.back().dx1 == v1); PARABOLIC_RAMP_ASSERT(ramps.back().dx1 == v1);
double ttotal = 0; double ttotal = 0;
for(size_t i=0; i<ramps.size(); i++) for(size_t i=0; i<ramps.size(); i++)
ttotal += ramps[i].ttotal; ttotal += ramps[i].ttotal;
for(size_t i=0; i<ramps.size(); i++) {
if(Abs(ramps[i].ttotal) == 0.0) {
ramps.erase(ramps.begin()+i);
i--;
}
}
if(!FuzzyEquals(ttotal,endTime,EpsilonT*0.1)) { if(!FuzzyEquals(ttotal,endTime,EpsilonT*0.1)) {
PARABOLICLOG("Ramp times: "); PARABOLIC_RAMP_PLOG("Ramp times: ");
for(size_t i=0; i<ramps.size(); i++) for(size_t i=0; i<ramps.size(); i++)
PARABOLICLOG("%g ",ramps[i].ttotal); PARABOLIC_RAMP_PLOG("%g ",ramps[i].ttotal);
PARABOLICLOG("\n"); PARABOLIC_RAMP_PLOG("\n");
} }
PARABOLIC_ASSERT(FuzzyEquals(ttotal,endTime,EpsilonT*0.1)); PARABOLIC_RAMP_ASSERT(FuzzyEquals(ttotal,endTime,EpsilonT*0.1));
return true; return true;
} }
Real SolveMinTimeBounded(const Vector& x0,const Vector& v0,const Vector& x1 ,const Vector& v1, Real SolveMinTimeBounded(const Vector& x0,const Vector& v0,const Vector& x1 ,const Vector& v1,
const Vector& amax,const Vector& vmax,const Vector & xmin,const Vector& xmax, const Vector& amax,const Vector& vmax,const Vector & xmin,const Vector& xmax,
vector<vector<ParabolicRamp1D> >& ramps) vector<vector<ParabolicRamp1D> >& ramps)
{ {
PARABOLIC_ASSERT(x0.size() == v0.size()); PARABOLIC_RAMP_ASSERT(x0.size() == v0.size());
PARABOLIC_ASSERT(x1.size() == v1.size()); PARABOLIC_RAMP_ASSERT(x1.size() == v1.size());
PARABOLIC_ASSERT(x0.size() == x1.size()); PARABOLIC_RAMP_ASSERT(x0.size() == x1.size());
PARABOLIC_ASSERT(x0.size() == amax.size()); PARABOLIC_RAMP_ASSERT(x0.size() == amax.size());
PARABOLIC_ASSERT(x0.size() == vmax.size()); PARABOLIC_RAMP_ASSERT(x0.size() == vmax.size());
for(size_t i=0; i<x0.size(); i++) { for(size_t i=0; i<x0.size(); i++) {
PARABOLIC_ASSERT(x0[i] >= xmin[i] && x0[i] <= xmax[i]); if(x0[i] < xmin[i] || x0[i] > xmax[i]) {
PARABOLIC_ASSERT(x1[i] >= xmin[i] && x1[i] <= xmax[i]); PARABOLICWARN("Warning, start component %d=%g out of range [%g,
PARABOLIC_ASSERT(Abs(v0[i]) <= vmax[i]); %g]\n",i,x0[i],xmin[i],xmax[i]);
PARABOLIC_ASSERT(Abs(v1[i]) <= vmax[i]); return -1;
}
if(x1[i] < xmin[i] || x1[i] > xmax[i]) {
PARABOLICWARN("Warning, goal component %d=%g out of range [%g,%
g]\n",i,x1[i],xmin[i],xmax[i]);
return -1;
}
PARABOLIC_RAMP_ASSERT(x0[i] >= xmin[i] && x0[i] <= xmax[i]);
PARABOLIC_RAMP_ASSERT(x1[i] >= xmin[i] && x1[i] <= xmax[i]);
PARABOLIC_RAMP_ASSERT(Abs(v0[i]) <= vmax[i]);
PARABOLIC_RAMP_ASSERT(Abs(v1[i]) <= vmax[i]);
} }
Real endTime = 0; Real endTime = 0;
ramps.resize(x0.size()); ramps.resize(x0.size());
for(size_t i=0; i<ramps.size(); i++) { for(size_t i=0; i<ramps.size(); i++) {
ramps[i].resize(1); ramps[i].resize(1);
ramps[i][0].x0=x0[i]; ramps[i][0].x0=x0[i];
ramps[i][0].x1=x1[i]; ramps[i][0].x1=x1[i];
ramps[i][0].dx0=v0[i]; ramps[i][0].dx0=v0[i];
ramps[i][0].dx1=v1[i]; ramps[i][0].dx1=v1[i];
if(vmax[i]==0 || amax[i]==0) { if(vmax[i]==0 || amax[i]==0) {
if(!FuzzyEquals(x0[i],x1[i],EpsilonX)) { if(!FuzzyEquals(x0[i],x1[i],EpsilonX)) {
PARABOLICLOG("index %d vmax = %g, amax = %g, X0 != X1 (%g ! = %g)\n",i,vmax[i],amax[i],x0[i],x1[i]); PARABOLIC_RAMP_PLOG("index %d vmax = %g, amax = %g, X0 != X 1 (%g != %g)\n",i,vmax[i],amax[i],x0[i],x1[i]);
return -1; return -1;
} }
if(!FuzzyEquals(v0[i],v1[i],EpsilonV)) { if(!FuzzyEquals(v0[i],v1[i],EpsilonV)) {
PARABOLICLOG("index %d vmax = %g, amax = %g, DX0 != DX1 (%g != %g)\n",i,vmax[i],amax[i],v0[i],v1[i]); PARABOLIC_RAMP_PLOG("index %d vmax = %g, amax = %g, DX0 != DX1 (%g != %g)\n",i,vmax[i],amax[i],v0[i],v1[i]);
return -1; return -1;
} }
ramps[i][0].tswitch1=ramps[i][0].tswitch2=ramps[i][0].ttotal=0; ramps[i][0].tswitch1=ramps[i][0].tswitch2=ramps[i][0].ttotal=0;
ramps[i][0].a1=ramps[i][0].a2=ramps[i][0].v=0; ramps[i][0].a1=ramps[i][0].a2=ramps[i][0].v=0;
continue; continue;
} }
if(!ramps[i][0].SolveMinTime(amax[i],vmax[i])) return -1; if(!ramps[i][0].SolveMinTime(amax[i],vmax[i])) return -1;
Real bmin,bmax; Real bmin,bmax;
ramps[i][0].Bounds(bmin,bmax); ramps[i][0].Bounds(bmin,bmax);
if(bmin < xmin[i] || bmax > xmax[i]) return -1; if(bmin < xmin[i] || bmax > xmax[i]) return -1;
if(ramps[i][0].ttotal > endTime) endTime = ramps[i][0].ttotal; if(ramps[i][0].ttotal > endTime) endTime = ramps[i][0].ttotal;
} }
//now we have a candidate end time -- repeat looking through solutions //now we have a candidate end time -- repeat looking through solutions
//until we have solved all ramps //until we have solved all ramps
while(true) { while(true) {
bool solved = true; bool solved = true;
for(size_t i=0; i<ramps.size(); i++) { for(size_t i=0; i<ramps.size(); i++) {
PARABOLIC_ASSERT(ramps[i].size() > 0); PARABOLIC_RAMP_ASSERT(ramps[i].size() > 0);
if(vmax[i]==0 || amax[i]==0) { if(vmax[i]==0 || amax[i]==0) {
ramps[i][0].ttotal = endTime; ramps[i][0].ttotal = endTime;
continue; continue;
} }
//already at maximum //already at maximum
Real ttotal = 0; Real ttotal = 0;
for(size_t j=0; j<ramps[i].size(); j++) for(size_t j=0; j<ramps[i].size(); j++)
ttotal += ramps[i][j].ttotal; ttotal += ramps[i][j].ttotal;
if(FuzzyEquals(ttotal,endTime,EpsilonT)) continue; if(FuzzyEquals(ttotal,endTime,EpsilonT)) continue;
//now solve minimum acceleration within bounds //now solve minimum acceleration within bounds
if(!SolveMinAccelBounded(x0[i],v0[i],x1[i],v1[i],endTime,vmax[i ],xmin[i],xmax[i],ramps[i])) { if(!SolveMinAccelBounded(x0[i],v0[i],x1[i],v1[i],endTime,vmax[i ],xmin[i],xmax[i],ramps[i])) {
PARABOLICLOG("Failed solving bounded min accel for joint %d \n",i); PARABOLIC_RAMP_PLOG("Failed solving bounded min accel for j oint %d\n",i);
return -1; return -1;
} }
//now check accel/velocity bounds //now check accel/velocity bounds
bool inVelBounds = true; bool inVelBounds = true;
for(size_t j=0; j<ramps[i].size(); j++) for(size_t j=0; j<ramps[i].size(); j++)
if(Abs(ramps[i][j].a1) > amax[i]+EpsilonA || Abs(ramps[i][j ].a2) > amax[i]+EpsilonA || Abs(ramps[i][j].v) > vmax[i]+EpsilonV) { if(Abs(ramps[i][j].a1) > amax[i]+EpsilonA || Abs(ramps[i][j ].a2) > amax[i]+EpsilonA || Abs(ramps[i][j].v) > vmax[i]+EpsilonV) {
//PARABOLICLOG("Ramp %d entry %d accels: %g %g, vel %g\ n",i,j,ramps[i][j].a1,ramps[i][j].a2,ramps[i][j].v); //PARABOLIC_RAMP_PLOG("Ramp %d entry %d accels: %g %g, vel %g\n",i,j,ramps[i][j].a1,ramps[i][j].a2,ramps[i][j].v);
inVelBounds = false; inVelBounds = false;
break; break;
} }
if(!inVelBounds) { if(!inVelBounds) {
ramps[i].resize(1); ramps[i].resize(1);
ramps[i][0].x0=x0[i]; ramps[i][0].x0=x0[i];
ramps[i][0].x1=x1[i]; ramps[i][0].x1=x1[i];
ramps[i][0].dx0=v0[i]; ramps[i][0].dx0=v0[i];
ramps[i][0].dx1=v1[i]; ramps[i][0].dx1=v1[i];
gMinTime2Quiet = true; gMinTime2Quiet = true;
bool res=ramps[i][0].SolveMinTime2(amax[i],vmax[i],endTime) ; bool res=ramps[i][0].SolveMinTime2(amax[i],vmax[i],endTime) ;
gMinTime2Quiet = false; gMinTime2Quiet = false;
if(!res) { if(!res) {
return -1; return -1;
} }
Real bmin,bmax; Real bmin,bmax;
ramps[i][0].Bounds(bmin,bmax); ramps[i][0].Bounds(bmin,bmax);
if(bmin < xmin[i] || bmax > xmax[i]) { if(bmin < xmin[i] || bmax > xmax[i]) {
//PARABOLICLOG("Couldn't solve min-time with lower boun //PARABOLIC_RAMP_PLOG("Couldn't solve min-time with low
d while staying in bounds\n"); er bound while staying in bounds\n");
//getchar();
return -1; return -1;
} }
//revise total time //revise total time
ttotal = 0; PARABOLIC_RAMP_ASSERT(ramps[i][0].ttotal >= endTime);
for(size_t j=0; j<ramps[i].size(); j++) endTime = ramps[i][0].ttotal;
ttotal += ramps[i][j].ttotal;
PARABOLIC_ASSERT(ttotal > endTime);
endTime = ttotal;
solved = false; solved = false;
break; //go back and re-solve break; //go back and re-solve
} }
ttotal = 0; ttotal = 0;
for(size_t j=0; j<ramps[i].size(); j++) { for(size_t j=0; j<ramps[i].size(); j++) {
PARABOLIC_ASSERT(Abs(ramps[i][j].a1) <= amax[i]+EpsilonA); PARABOLIC_RAMP_ASSERT(Abs(ramps[i][j].a1) <= amax[i]+Epsilo
PARABOLIC_ASSERT(Abs(ramps[i][j].a2) <= amax[i]+EpsilonA); nA);
PARABOLIC_ASSERT(Abs(ramps[i][j].v) <= vmax[i]+EpsilonV); PARABOLIC_RAMP_ASSERT(Abs(ramps[i][j].a2) <= amax[i]+Epsilo
nA);
PARABOLIC_RAMP_ASSERT(Abs(ramps[i][j].v) <= vmax[i]+Epsilon
V);
ttotal += ramps[i][j].ttotal; ttotal += ramps[i][j].ttotal;
} }
PARABOLIC_ASSERT(FuzzyEquals(ttotal,endTime,EpsilonT*0.1)); PARABOLIC_RAMP_ASSERT(FuzzyEquals(ttotal,endTime,EpsilonT*0.1)) ;
} }
//done //done
if(solved) break; if(solved) break;
} }
return endTime; return endTime;
} }
bool SolveMinAccelBounded(const Vector& x0,const Vector& v0,const Vector& x 1,const Vector& v1, bool SolveMinAccelBounded(const Vector& x0,const Vector& v0,const Vector& x 1,const Vector& v1,
Real endTime,const Vector& vmax,const Vector& xmi n,const Vector& xmax, Real endTime,const Vector& vmax,const Vector& xmi n,const Vector& xmax,
vector<vector<ParabolicRamp1D> >& ramps) vector<vector<ParabolicRamp1D> >& ramps)
{ {
PARABOLIC_ASSERT(x0.size() == v0.size()); PARABOLIC_RAMP_ASSERT(x0.size() == v0.size());
PARABOLIC_ASSERT(x1.size() == v1.size()); PARABOLIC_RAMP_ASSERT(x1.size() == v1.size());
PARABOLIC_ASSERT(x0.size() == x1.size()); PARABOLIC_RAMP_ASSERT(x0.size() == x1.size());
PARABOLIC_ASSERT(x0.size() == vmax.size()); PARABOLIC_RAMP_ASSERT(x0.size() == vmax.size());
for(size_t i=0; i<x0.size(); i++) { for(size_t i=0; i<x0.size(); i++) {
PARABOLIC_ASSERT(x0[i] >= xmin[i] && x0[i] <= xmax[i]); PARABOLIC_RAMP_ASSERT(x0[i] >= xmin[i] && x0[i] <= xmax[i]);
PARABOLIC_ASSERT(x1[i] >= xmin[i] && x1[i] <= xmax[i]); PARABOLIC_RAMP_ASSERT(x1[i] >= xmin[i] && x1[i] <= xmax[i]);
PARABOLIC_ASSERT(Abs(v0[i]) <= vmax[i]); PARABOLIC_RAMP_ASSERT(Abs(v0[i]) <= vmax[i]);
PARABOLIC_ASSERT(Abs(v1[i]) <= vmax[i]); PARABOLIC_RAMP_ASSERT(Abs(v1[i]) <= vmax[i]);
} }
for(size_t i=0; i<ramps.size(); i++) { for(size_t i=0; i<ramps.size(); i++) {
if(vmax[i]==0) { if(vmax[i]==0) {
ramps[i].resize(1); ramps[i].resize(1);
ramps[i][0].x0=x0[i]; ramps[i][0].x0=x0[i];
ramps[i][0].x1=x1[i]; ramps[i][0].x1=x1[i];
ramps[i][0].dx0=v0[i]; ramps[i][0].dx0=v0[i];
ramps[i][0].dx1=v1[i]; ramps[i][0].dx1=v1[i];
ramps[i][0].ttotal = endTime; ramps[i][0].ttotal = endTime;
continue; continue;
} }
//now solve minimum acceleration within bounds //now solve minimum acceleration within bounds
if(!SolveMinAccelBounded(x0[i],v0[i],x1[i],v1[i],endTime,vmax[i],xm in[i],xmax[i],ramps[i])) { if(!SolveMinAccelBounded(x0[i],v0[i],x1[i],v1[i],endTime,vmax[i],xm in[i],xmax[i],ramps[i])) {
PARABOLICLOG("Failed solving bounded min accel for joint %d\n", i); PARABOLIC_RAMP_PLOG("Failed solving bounded min accel for joint %d\n",i);
return false; return false;
} }
} }
return true; return true;
} }
void CombineRamps(const std::vector<std::vector<ParabolicRamp1D> >& ramps,s td::vector<ParabolicRampND>& ndramps) void CombineRamps(const std::vector<std::vector<ParabolicRamp1D> >& ramps,s td::vector<ParabolicRampND>& ndramps)
{ {
ndramps.resize(0); ndramps.resize(0);
vector<vector<ParabolicRamp1D>::const_iterator> indices(ramps.size()); vector<vector<ParabolicRamp1D>::const_iterator> indices(ramps.size());
for(size_t i=0; i<ramps.size(); i++) { for(size_t i=0; i<ramps.size(); i++) {
PARABOLIC_ASSERT(!ramps[i].empty()); PARABOLIC_RAMP_ASSERT(!ramps[i].empty());
indices[i] = ramps[i].begin(); indices[i] = ramps[i].begin();
} }
vector<double> timeOffsets(ramps.size(),0); //start time of current in dex vector<double> timeOffsets(ramps.size(),0); //start time of current in dex
Real t=0; Real t=0;
while(true) { while(true) {
//pick next ramp //pick next ramp
Real tnext=Inf; Real tnext=Inf;
for(size_t i=0; i<ramps.size(); i++) { for(size_t i=0; i<ramps.size(); i++) {
if(indices[i] != ramps[i].end()) if(indices[i] != ramps[i].end())
tnext = Min(tnext,timeOffsets[i]+indices[i]->ttotal); tnext = Min(tnext,timeOffsets[i]+indices[i]->ttotal);
} }
if(IsInf(tnext)) break; //done if(IsInf(tnext)) break; //done
if(!(tnext > t || t == 0)) { if(!(tnext > t || t == 0)) {
PARABOLICLOG("CombineRamps: error finding next time step?\n"); PARABOLIC_RAMP_PLOG("CombineRamps: error finding next time step
PARABOLICLOG("tnext = %g, t = %g, step = %d\n",tnext,t,ndramps. ?\n");
size()); PARABOLIC_RAMP_PLOG("tnext = %g, t = %g, step = %d\n",tnext,t,n
dramps.size());
for(size_t k=0; k<ramps.size(); k++) { for(size_t k=0; k<ramps.size(); k++) {
PARABOLICLOG("Ramp %d times: ",k); PARABOLIC_RAMP_PLOG("Ramp %d times: ",k);
Real ttotal = 0.0; Real ttotal = 0.0;
for(size_t j=0; j<ramps[k].size(); j++) { for(size_t j=0; j<ramps[k].size(); j++) {
PARABOLICLOG("%g ",ramps[k][j].ttotal); PARABOLIC_RAMP_PLOG("%g ",ramps[k][j].ttotal);
ttotal += ramps[k][j].ttotal; ttotal += ramps[k][j].ttotal;
} }
PARABOLICLOG(", total %g\n",ttotal); PARABOLIC_RAMP_PLOG(", total %g\n",ttotal);
} }
} }
PARABOLIC_ASSERT(tnext > t || t == 0); PARABOLIC_RAMP_ASSERT(tnext > t || t == 0);
if(tnext == 0) { if(tnext == 0) {
for(size_t i=0; i<ramps.size(); i++) for(size_t i=0; i<ramps.size(); i++) {
PARABOLIC_ASSERT(ramps[i].size()==1); if(ramps[i].size()!=1) {
PARABOLICWARN("Warning, some entry has multiple zeroes?
\n");
for(size_t j=0; j<ramps.size(); j++) {
PARABOLICWARN("Ramp %d: ",j);
for(size_t k=0; k<ramps[j].size(); k++)
PARABOLICWARN("%g ",ramps[j][k].ttotal);
PARABOLICWARN("\n");
}
}
PARABOLIC_RAMP_ASSERT(ramps[i].size()==1);
}
} }
ndramps.resize(ndramps.size()+1); ndramps.resize(ndramps.size()+1);
ParabolicRampND& ramp=ndramps.back(); ParabolicRampND& ramp=ndramps.back();
ramp.x0.resize(ramps.size()); ramp.x0.resize(ramps.size());
ramp.x1.resize(ramps.size()); ramp.x1.resize(ramps.size());
ramp.dx0.resize(ramps.size()); ramp.dx0.resize(ramps.size());
ramp.dx1.resize(ramps.size()); ramp.dx1.resize(ramps.size());
ramp.ramps.resize(ramps.size()); ramp.ramps.resize(ramps.size());
ramp.endTime = tnext-t; ramp.endTime = tnext-t;
skipping to change at line 2583 skipping to change at line 2700
} }
else { else {
iramp.TrimBack((timeOffsets[i]-tnext)+indices[i]->ttota l); iramp.TrimBack((timeOffsets[i]-tnext)+indices[i]->ttota l);
} }
iramp.TrimFront(t-timeOffsets[i]); iramp.TrimFront(t-timeOffsets[i]);
Real oldTotal = iramp.ttotal; Real oldTotal = iramp.ttotal;
iramp.ttotal = ramp.endTime; iramp.ttotal = ramp.endTime;
if(iramp.tswitch1 > iramp.ttotal) iramp.tswitch1 = iramp.tt otal; if(iramp.tswitch1 > iramp.ttotal) iramp.tswitch1 = iramp.tt otal;
if(iramp.tswitch2 > iramp.ttotal) iramp.tswitch2 = iramp.tt otal; if(iramp.tswitch2 > iramp.ttotal) iramp.tswitch2 = iramp.tt otal;
if(!iramp.IsValid()) { if(!iramp.IsValid()) {
PARABOLICLOG("CombineRamps: Trimming caused ramp to bec PARABOLIC_RAMP_PLOG("CombineRamps: Trimming caused ramp
ome invalid\n"); to become invalid\n");
PARABOLICLOG("Old total time %g, new total time %g\n",o PARABOLIC_RAMP_PLOG("Old total time %g, new total time
ldTotal,iramp.ttotal); %g\n",oldTotal,iramp.ttotal);
} }
PARABOLIC_ASSERT(iramp.IsValid()); PARABOLIC_RAMP_ASSERT(iramp.IsValid());
ramp.ramps[i] = iramp; ramp.ramps[i] = iramp;
ramp.x0[i] = iramp.x0; ramp.x0[i] = iramp.x0;
ramp.dx0[i] = iramp.dx0; ramp.dx0[i] = iramp.dx0;
ramp.x1[i] = iramp.x1; ramp.x1[i] = iramp.x1;
ramp.dx1[i] = iramp.dx1; ramp.dx1[i] = iramp.dx1;
if(FuzzyEquals(tnext,timeOffsets[i]+indices[i]->ttotal,Epsi lonT*0.1)) { if(FuzzyEquals(tnext,timeOffsets[i]+indices[i]->ttotal,Epsi lonT*0.1)) {
timeOffsets[i] = tnext; timeOffsets[i] = tnext;
indices[i]++; indices[i]++;
} }
PARABOLIC_ASSERT(ramp.ramps[i].ttotal == ramp.endTime); PARABOLIC_RAMP_ASSERT(ramp.ramps[i].ttotal == ramp.endTime) ;
} }
else { else {
//after the last segment, propagate a constant off the last ramp //after the last segment, propagate a constant off the last ramp
PARABOLIC_ASSERT(!ramps[i].empty()); PARABOLIC_RAMP_ASSERT(!ramps[i].empty());
ramp.x0[i] = ramps[i].back().x1; ramp.x0[i] = ramps[i].back().x1;
ramp.dx0[i] = ramps[i].back().dx1; ramp.dx0[i] = ramps[i].back().dx1;
//ramp.x1[i] = ramps[i].back().x1+ramps[i].back().dx1*(tnex t-t); //ramp.x1[i] = ramps[i].back().x1+ramps[i].back().dx1*(tnex t-t);
ramp.x1[i] = ramps[i].back().x1; ramp.x1[i] = ramps[i].back().x1;
ramp.dx1[i] = ramps[i].back().dx1; ramp.dx1[i] = ramps[i].back().dx1;
if(!FuzzyEquals(ramps[i].back().dx1*(tnext-t),0.0,EpsilonV) ) { if(!FuzzyEquals(ramps[i].back().dx1*(tnext-t),0.0,EpsilonV) ) {
PARABOLICLOG("CombineRamps: warning, propagating time % g distance %g off the back, vel %g\n",(tnext-t),ramps[i].back().dx1*(tnext- t),ramp.dx0[i]); PARABOLIC_RAMP_PLOG("CombineRamps: warning, propagating time %g distance %g off the back, vel %g\n",(tnext-t),ramps[i].back().dx1* (tnext-t),ramp.dx0[i]);
for(size_t k=0; k<ramps.size(); k++) { for(size_t k=0; k<ramps.size(); k++) {
PARABOLICLOG("Ramp %d times: ",k); PARABOLIC_RAMP_PLOG("Ramp %d times: ",k);
Real ttotal = 0.0; Real ttotal = 0.0;
for(size_t j=0; j<ramps[k].size(); j++) { for(size_t j=0; j<ramps[k].size(); j++) {
PARABOLICLOG("%g ",ramps[k][j].ttotal); PARABOLIC_RAMP_PLOG("%g ",ramps[k][j].ttotal);
ttotal += ramps[k][j].ttotal; ttotal += ramps[k][j].ttotal;
} }
PARABOLICLOG(", total %g\n",ttotal); PARABOLIC_RAMP_PLOG(", total %g\n",ttotal);
} }
//getchar();
} }
//set the 1D ramp manually //set the 1D ramp manually
ramp.ramps[i].x0 = ramp.x0[i]; ramp.ramps[i].x0 = ramp.x0[i];
ramp.ramps[i].dx0 = ramp.dx0[i]; ramp.ramps[i].dx0 = ramp.dx0[i];
ramp.ramps[i].x1 = ramp.x1[i]; ramp.ramps[i].x1 = ramp.x1[i];
ramp.ramps[i].dx1 = ramp.dx1[i]; ramp.ramps[i].dx1 = ramp.dx1[i];
ramp.ramps[i].ttotal = ramp.ramps[i].tswitch2 = (tnext-t); ramp.ramps[i].ttotal = ramp.ramps[i].tswitch2 = (tnext-t);
ramp.ramps[i].tswitch1 = 0; ramp.ramps[i].tswitch1 = 0;
ramp.ramps[i].v = ramp.dx1[i]; ramp.ramps[i].v = ramp.dx1[i];
ramp.ramps[i].a1 = ramp.ramps[i].a2 = 0; ramp.ramps[i].a1 = ramp.ramps[i].a2 = 0;
PARABOLIC_ASSERT(ramp.ramps[i].IsValid()); PARABOLIC_RAMP_ASSERT(ramp.ramps[i].IsValid());
PARABOLIC_ASSERT(ramp.ramps[i].ttotal == ramp.endTime); PARABOLIC_RAMP_ASSERT(ramp.ramps[i].ttotal == ramp.endTime)
;
} }
} }
PARABOLIC_ASSERT(ramp.IsValid()); PARABOLIC_RAMP_ASSERT(ramp.IsValid());
if(ndramps.size() > 1) { //fix up endpoints if(ndramps.size() > 1) { //fix up endpoints
ramp.x0 = ndramps[ndramps.size()-2].x1; ramp.x0 = ndramps[ndramps.size()-2].x1;
ramp.dx0 = ndramps[ndramps.size()-2].dx1; ramp.dx0 = ndramps[ndramps.size()-2].dx1;
for(size_t i=0; i<ramp.ramps.size(); i++) { for(size_t i=0; i<ramp.ramps.size(); i++) {
ramp.ramps[i].x0=ramp.x0[i]; ramp.ramps[i].x0=ramp.x0[i];
ramp.ramps[i].dx0=ramp.dx0[i]; ramp.ramps[i].dx0=ramp.dx0[i];
} }
} }
PARABOLIC_ASSERT(ramp.IsValid()); PARABOLIC_RAMP_ASSERT(ramp.IsValid());
t = tnext; t = tnext;
if(tnext == 0) //all null ramps if(tnext == 0) //all null ramps
break; break;
} }
for(size_t i=0; i<ramps.size(); i++) { for(size_t i=0; i<ramps.size(); i++) {
if(!FuzzyEquals(ramps[i].front().x0,ndramps.front().x0[i],EpsilonX) ) { if(!FuzzyEquals(ramps[i].front().x0,ndramps.front().x0[i],EpsilonX) ) {
PARABOLICLOG("CombineRamps: Error: %d start %g != %g\n",i,ramps PARABOLIC_RAMP_PLOG("CombineRamps: Error: %d start %g != %g\n",
[i].front().x0,ndramps.front().x0[i]); i,ramps[i].front().x0,ndramps.front().x0[i]);
//getchar();
} }
if(!FuzzyEquals(ramps[i].front().dx0,ndramps.front().dx0[i],Epsilon V)) { if(!FuzzyEquals(ramps[i].front().dx0,ndramps.front().dx0[i],Epsilon V)) {
PARABOLICLOG("CombineRamps: Error: %d start %g != %g\n",i,ramps PARABOLIC_RAMP_PLOG("CombineRamps: Error: %d start %g != %g\n",
[i].front().dx0,ndramps.front().dx0[i]); i,ramps[i].front().dx0,ndramps.front().dx0[i]);
//getchar();
} }
if(!FuzzyEquals(ramps[i].back().x1,ndramps.back().x1[i],EpsilonX)) { if(!FuzzyEquals(ramps[i].back().x1,ndramps.back().x1[i],EpsilonX)) {
PARABOLICLOG("CombineRamps: Error: %d back %g != %g\n",i,ramps[ PARABOLIC_RAMP_PLOG("CombineRamps: Error: %d back %g != %g\n",i
i].back().x1,ndramps.back().x1[i]); ,ramps[i].back().x1,ndramps.back().x1[i]);
//getchar();
} }
if(!FuzzyEquals(ramps[i].back().dx1,ndramps.back().dx1[i],EpsilonV) ) { if(!FuzzyEquals(ramps[i].back().dx1,ndramps.back().dx1[i],EpsilonV) ) {
PARABOLICLOG("CombineRamps: Error: %d back %g != %g\n",i,ramps[ PARABOLIC_RAMP_PLOG("CombineRamps: Error: %d back %g != %g\n",i
i].back().dx1,ndramps.back().dx1[i]); ,ramps[i].back().dx1,ndramps.back().dx1[i]);
//getchar();
} }
ndramps.front().x0[i] = ndramps.front().ramps[i].x0 = ramps[i].fron t().x0; ndramps.front().x0[i] = ndramps.front().ramps[i].x0 = ramps[i].fron t().x0;
ndramps.front().dx0[i] = ndramps.front().ramps[i].dx0 = ramps[i].fr ont().dx0; ndramps.front().dx0[i] = ndramps.front().ramps[i].dx0 = ramps[i].fr ont().dx0;
ndramps.back().x1[i] = ndramps.back().ramps[i].x1 = ramps[i].back() .x1; ndramps.back().x1[i] = ndramps.back().ramps[i].x1 = ramps[i].back() .x1;
ndramps.back().dx1[i] = ndramps.back().ramps[i].dx1 = ramps[i].back ().dx1; ndramps.back().dx1[i] = ndramps.back().ramps[i].dx1 = ramps[i].back ().dx1;
} }
} }
} //namespace ParabolicRamp } //namespace ParabolicRamp
 End of changes. 185 change blocks. 
451 lines changed or deleted 622 lines changed or added

This html diff was produced by rfcdiff 1.41. The latest version is available from http://tools.ietf.org/tools/rfcdiff/