DynamicPath.cpp | DynamicPath.cpp | |||
---|---|---|---|---|
skipping to change at line 43 | skipping to change at line 43 | |||
#include <stdlib.h> | #include <stdlib.h> | |||
#include <stdio.h> | #include <stdio.h> | |||
#include <list> | #include <list> | |||
#include <algorithm> | #include <algorithm> | |||
using namespace std; | using namespace std; | |||
namespace ParabolicRamp { | namespace ParabolicRamp { | |||
inline Real LInfDistance(const Vector& a,const Vector& b) | inline Real LInfDistance(const Vector& a,const Vector& b) | |||
{ | { | |||
PARABOLIC_ASSERT(a.size()==b.size()); | PARABOLIC_RAMP_ASSERT(a.size()==b.size()); | |||
Real d=0; | Real d=0; | |||
for(size_t i=0; i<a.size(); i++) | for(size_t i=0; i<a.size(); i++) | |||
d = Max(d,Abs(a[i]-b[i])); | d = Max(d,Abs(a[i]-b[i])); | |||
return d; | return d; | |||
} | } | |||
inline bool InBounds(const Vector& x,const Vector& bmin,const Vector& bmax) | inline bool InBounds(const Vector& x,const Vector& bmin,const Vector& bmax) | |||
{ | { | |||
PARABOLIC_ASSERT(x.size()==bmin.size()); | PARABOLIC_RAMP_ASSERT(x.size()==bmin.size()); | |||
PARABOLIC_ASSERT(x.size()==bmax.size()); | PARABOLIC_RAMP_ASSERT(x.size()==bmax.size()); | |||
for(size_t i=0; i<x.size(); i++) | for(size_t i=0; i<x.size(); i++) | |||
if(x[i] < bmin[i] || x[i] > bmax[i]) return false; | if(x[i] < bmin[i] || x[i] > bmax[i]) return false; | |||
return true; | return true; | |||
} | } | |||
inline Real MaxBBLInfDistance(const Vector& x,const Vector& bmin,const Vect or& bmax) | inline Real MaxBBLInfDistance(const Vector& x,const Vector& bmin,const Vect or& bmax) | |||
{ | { | |||
PARABOLIC_ASSERT(x.size()==bmin.size()); | PARABOLIC_RAMP_ASSERT(x.size()==bmin.size()); | |||
PARABOLIC_ASSERT(x.size()==bmax.size()); | PARABOLIC_RAMP_ASSERT(x.size()==bmax.size()); | |||
Real d=0; | Real d=0; | |||
for(size_t i=0; i<x.size(); i++) | for(size_t i=0; i<x.size(); i++) | |||
d = Max(d,Max(Abs(x[i]-bmin[i]),Abs(x[i]-bmax[i]))); | d = Max(d,Max(Abs(x[i]-bmin[i]),Abs(x[i]-bmax[i]))); | |||
return d; | return d; | |||
} | } | |||
inline bool SolveMinTime(const Vector& x0,const Vector& dx0,const Vector& x 1,const Vector& dx1, | inline bool SolveMinTime(const Vector& x0,const Vector& dx0,const Vector& x 1,const Vector& dx1, | |||
const Vector& accMax,const Vector& velMax,const Ve ctor& xMin,const Vector& xMax,DynamicPath& out) | const Vector& accMax,const Vector& velMax,const Ve ctor& xMin,const Vector& xMax,DynamicPath& out) | |||
{ | { | |||
if(xMin.empty()) { | if(xMin.empty()) { | |||
skipping to change at line 92 | skipping to change at line 92 | |||
else { | else { | |||
vector<std::vector<ParabolicRamp1D> > ramps; | vector<std::vector<ParabolicRamp1D> > ramps; | |||
Real res=SolveMinTimeBounded(x0,dx0,x1,dx1, | Real res=SolveMinTimeBounded(x0,dx0,x1,dx1, | |||
accMax,velMax,xMin,xMax, | accMax,velMax,xMin,xMax, | |||
ramps); | ramps); | |||
if(res < 0) return false; | if(res < 0) return false; | |||
CombineRamps(ramps,out.ramps); | CombineRamps(ramps,out.ramps); | |||
} | } | |||
out.accMax = accMax; | out.accMax = accMax; | |||
out.velMax = velMax; | out.velMax = velMax; | |||
PARABOLIC_ASSERT(out.IsValid()); | PARABOLIC_RAMP_ASSERT(out.IsValid()); | |||
return true; | return true; | |||
} | } | |||
class RandomSamplerStd : public RandomSamplerBase | class RandomSamplerStd : public RandomSamplerBase | |||
{ | { | |||
Real Rand() | Real Rand() | |||
{ | { | |||
return Real(rand())/Real(RAND_MAX); | return Real(rand())/Real(RAND_MAX); | |||
} | } | |||
}; | }; | |||
skipping to change at line 115 | skipping to change at line 115 | |||
DynamicPath::DynamicPath() | DynamicPath::DynamicPath() | |||
{ | { | |||
sampler = &s_samplerstd; | sampler = &s_samplerstd; | |||
} | } | |||
void DynamicPath::Init(const Vector& _velMax,const Vector& _accMax) | void DynamicPath::Init(const Vector& _velMax,const Vector& _accMax) | |||
{ | { | |||
velMax = _velMax; | velMax = _velMax; | |||
accMax = _accMax; | accMax = _accMax; | |||
PARABOLIC_ASSERT(velMax.size() == accMax.size()); | PARABOLIC_RAMP_ASSERT(velMax.size() == accMax.size()); | |||
if(!velMax.empty() && !xMin.empty()) PARABOLIC_ASSERT(xMin.size() == ve | if(!velMax.empty() && !xMin.empty()) PARABOLIC_RAMP_ASSERT(xMin.size() | |||
lMax.size()); | == velMax.size()); | |||
} | } | |||
void DynamicPath::SetJointLimits(const Vector& _xMin,const Vector& _xMax) | void DynamicPath::SetJointLimits(const Vector& _xMin,const Vector& _xMax) | |||
{ | { | |||
xMin = _xMin; | xMin = _xMin; | |||
xMax = _xMax; | xMax = _xMax; | |||
PARABOLIC_ASSERT(xMin.size() == xMax.size()); | PARABOLIC_RAMP_ASSERT(xMin.size() == xMax.size()); | |||
if(!velMax.empty() && !xMin.empty()) PARABOLIC_ASSERT(xMin.size() == ve | if(!velMax.empty() && !xMin.empty()) PARABOLIC_RAMP_ASSERT(xMin.size() | |||
lMax.size()); | == velMax.size()); | |||
} | } | |||
Real DynamicPath::GetTotalTime() const | Real DynamicPath::GetTotalTime() const | |||
{ | { | |||
Real t=0; | Real t=0; | |||
for(size_t i=0; i<ramps.size(); i++) t+=ramps[i].endTime; | for(size_t i=0; i<ramps.size(); i++) t+=ramps[i].endTime; | |||
return t; | return t; | |||
} | } | |||
int DynamicPath::GetSegment(Real t,Real& u) const | int DynamicPath::GetSegment(Real t,Real& u) const | |||
skipping to change at line 150 | skipping to change at line 150 | |||
return (int)i; | return (int)i; | |||
} | } | |||
t -= ramps[i].endTime; | t -= ramps[i].endTime; | |||
} | } | |||
u = t; | u = t; | |||
return (int)ramps.size(); | return (int)ramps.size(); | |||
} | } | |||
void DynamicPath::Evaluate(Real t,Vector& x) const | void DynamicPath::Evaluate(Real t,Vector& x) const | |||
{ | { | |||
PARABOLIC_ASSERT(!ramps.empty()); | PARABOLIC_RAMP_ASSERT(!ramps.empty()); | |||
if(t < 0) { | if(t < 0) { | |||
x = ramps.front().x0; | x = ramps.front().x0; | |||
} | } | |||
else { | else { | |||
for(size_t i=0; i<ramps.size(); i++) { | for(size_t i=0; i<ramps.size(); i++) { | |||
if(t <= ramps[i].endTime) { | if(t <= ramps[i].endTime) { | |||
ramps[i].Evaluate(t,x); | ramps[i].Evaluate(t,x); | |||
return; | return; | |||
} | } | |||
t -= ramps[i].endTime; | t -= ramps[i].endTime; | |||
} | } | |||
x = ramps.back().x1; | x = ramps.back().x1; | |||
} | } | |||
} | } | |||
void DynamicPath::Derivative(Real t,Vector& dx) const | void DynamicPath::Derivative(Real t,Vector& dx) const | |||
{ | { | |||
PARABOLIC_ASSERT(!ramps.empty()); | PARABOLIC_RAMP_ASSERT(!ramps.empty()); | |||
if(t < 0) { | if(t < 0) { | |||
dx = ramps.front().dx0; | dx = ramps.front().dx0; | |||
} | } | |||
else { | else { | |||
for(size_t i=0; i<ramps.size(); i++) { | for(size_t i=0; i<ramps.size(); i++) { | |||
if(t <= ramps[i].endTime) { | if(t <= ramps[i].endTime) { | |||
ramps[i].Derivative(t,dx); | ramps[i].Derivative(t,dx); | |||
return; | return; | |||
} | } | |||
t -= ramps[i].endTime; | t -= ramps[i].endTime; | |||
skipping to change at line 202 | skipping to change at line 202 | |||
} | } | |||
else { | else { | |||
Vector zero(x[0].size(),0.0); | Vector zero(x[0].size(),0.0); | |||
ramps.resize(x.size()-1); | ramps.resize(x.size()-1); | |||
for(size_t i=0; i<ramps.size(); i++) { | for(size_t i=0; i<ramps.size(); i++) { | |||
ramps[i].x0 = x[i]; | ramps[i].x0 = x[i]; | |||
ramps[i].x1 = x[i+1]; | ramps[i].x1 = x[i+1]; | |||
ramps[i].dx0 = zero; | ramps[i].dx0 = zero; | |||
ramps[i].dx1 = zero; | ramps[i].dx1 = zero; | |||
bool res=ramps[i].SolveMinTimeLinear(accMax,velMax); | bool res=ramps[i].SolveMinTimeLinear(accMax,velMax); | |||
PARABOLIC_ASSERT(res && ramps[i].IsValid()); | PARABOLIC_RAMP_ASSERT(res && ramps[i].IsValid()); | |||
} | } | |||
} | } | |||
} | } | |||
void DynamicPath::SetMilestones(const vector<Vector>& x,const vector<Vector >& dx) | void DynamicPath::SetMilestones(const vector<Vector>& x,const vector<Vector >& dx) | |||
{ | { | |||
if(x.empty()) { | if(x.empty()) { | |||
ramps.resize(0); | ramps.resize(0); | |||
} | } | |||
else if(x.size()==1) { | else if(x.size()==1) { | |||
skipping to change at line 225 | skipping to change at line 225 | |||
} | } | |||
else { | else { | |||
if(xMin.empty()) { | if(xMin.empty()) { | |||
ramps.resize(x.size()-1); | ramps.resize(x.size()-1); | |||
for(size_t i=0; i<ramps.size(); i++) { | for(size_t i=0; i<ramps.size(); i++) { | |||
ramps[i].x0 = x[i]; | ramps[i].x0 = x[i]; | |||
ramps[i].x1 = x[i+1]; | ramps[i].x1 = x[i+1]; | |||
ramps[i].dx0 = dx[i]; | ramps[i].dx0 = dx[i]; | |||
ramps[i].dx1 = dx[i+1]; | ramps[i].dx1 = dx[i+1]; | |||
bool res=ramps[i].SolveMinTime(accMax,velMax); | bool res=ramps[i].SolveMinTime(accMax,velMax); | |||
PARABOLIC_ASSERT(res); | PARABOLIC_RAMP_ASSERT(res); | |||
PARABOLIC_ASSERT(ramps[i].IsValid()); | PARABOLIC_RAMP_ASSERT(ramps[i].IsValid()); | |||
} | } | |||
} | } | |||
else { | else { | |||
//bounded solving | //bounded solving | |||
ramps.resize(0); | ramps.resize(0); | |||
ramps.reserve(x.size()-1); | ramps.reserve(x.size()-1); | |||
std::vector<std::vector<ParabolicRamp1D> > tempRamps; | std::vector<std::vector<ParabolicRamp1D> > tempRamps; | |||
std::vector<ParabolicRampND> tempRamps2; | std::vector<ParabolicRampND> tempRamps2; | |||
PARABOLIC_ASSERT(InBounds(x[0],xMin,xMax)); | PARABOLIC_RAMP_ASSERT(InBounds(x[0],xMin,xMax)); | |||
for(size_t i=0; i+1<x.size(); i++) { | for(size_t i=0; i+1<x.size(); i++) { | |||
PARABOLIC_ASSERT(InBounds(x[i+1],xMin,xMax)); | PARABOLIC_RAMP_ASSERT(InBounds(x[i+1],xMin,xMax)); | |||
Real res=SolveMinTimeBounded(x[i],dx[i],x[i+1],dx[i+1], | Real res=SolveMinTimeBounded(x[i],dx[i],x[i+1],dx[i+1], | |||
accMax,velMax,xMin,xMax, | accMax,velMax,xMin,xMax, | |||
tempRamps); | tempRamps); | |||
PARABOLIC_ASSERT(res >= 0); | PARABOLIC_RAMP_ASSERT(res >= 0); | |||
CombineRamps(tempRamps,tempRamps2); | CombineRamps(tempRamps,tempRamps2); | |||
for(size_t j = 0; j < tempRamps2.size(); ++j) { | for(size_t j = 0; j < tempRamps2.size(); ++j) { | |||
PARABOLIC_ASSERT(tempRamps2[j].IsValid()); | PARABOLIC_RAMP_ASSERT(tempRamps2[j].IsValid()); | |||
} | } | |||
ramps.insert(ramps.end(),tempRamps2.begin(),tempRamps2.end( )); | ramps.insert(ramps.end(),tempRamps2.begin(),tempRamps2.end( )); | |||
} | } | |||
} | } | |||
} | } | |||
} | } | |||
void DynamicPath::GetMilestones(vector<Vector>& x,vector<Vector>& dx) const | void DynamicPath::GetMilestones(vector<Vector>& x,vector<Vector>& dx) const | |||
{ | { | |||
if(ramps.empty()) { | if(ramps.empty()) { | |||
skipping to change at line 286 | skipping to change at line 286 | |||
} | } | |||
else { | else { | |||
if(xMin.empty()) { | if(xMin.empty()) { | |||
ramps.resize(ramps.size()+1); | ramps.resize(ramps.size()+1); | |||
ramps[n].x0 = ramps[p].x1; | ramps[n].x0 = ramps[p].x1; | |||
ramps[n].dx0 = ramps[p].dx1; | ramps[n].dx0 = ramps[p].dx1; | |||
ramps[n].x1 = x; | ramps[n].x1 = x; | |||
ramps[n].dx1.resize(x.size()); | ramps[n].dx1.resize(x.size()); | |||
fill(ramps[n].dx1.begin(),ramps[n].dx1.end(),0); | fill(ramps[n].dx1.begin(),ramps[n].dx1.end(),0); | |||
bool res=ramps[n].SolveMinTime(accMax,velMax); | bool res=ramps[n].SolveMinTime(accMax,velMax); | |||
PARABOLIC_ASSERT(res); | PARABOLIC_RAMP_ASSERT(res); | |||
} | } | |||
else { | else { | |||
PARABOLIC_ASSERT(InBounds(x,xMin,xMax)); | PARABOLIC_RAMP_ASSERT(InBounds(x,xMin,xMax)); | |||
std::vector<std::vector<ParabolicRamp1D> > tempRamps; | std::vector<std::vector<ParabolicRamp1D> > tempRamps; | |||
std::vector<ParabolicRampND> tempRamps2; | std::vector<ParabolicRampND> tempRamps2; | |||
Vector zero(x.size(),0.0); | Vector zero(x.size(),0.0); | |||
Real res=SolveMinTimeBounded(ramps[p].x1,ramps[p].dx1,x,zero, | Real res=SolveMinTimeBounded(ramps[p].x1,ramps[p].dx1,x,zero, | |||
accMax,velMax,xMin,xMax,tempRamps) ; | accMax,velMax,xMin,xMax,tempRamps) ; | |||
PARABOLIC_ASSERT(res>=0); | PARABOLIC_RAMP_ASSERT(res>=0); | |||
CombineRamps(tempRamps,tempRamps2); | CombineRamps(tempRamps,tempRamps2); | |||
ramps.insert(ramps.end(),tempRamps2.begin(),tempRamps2.end()); | ramps.insert(ramps.end(),tempRamps2.begin(),tempRamps2.end()); | |||
} | } | |||
} | } | |||
} | } | |||
void DynamicPath::Append(const Vector& x,const Vector& dx) | void DynamicPath::Append(const Vector& x,const Vector& dx) | |||
{ | { | |||
size_t n=ramps.size(); | size_t n=ramps.size(); | |||
size_t p=n-1; | size_t p=n-1; | |||
if(ramps.size()==0) { | if(ramps.size()==0) { | |||
PARABOLICWARN("Can't append milestone with a nonzero velocity to an empty path\n"); | PARABOLICWARN("Can't append milestone with a nonzero velocity to an empty path\n"); | |||
PARABOLIC_ASSERT(0); | PARABOLIC_RAMP_ASSERT(0); | |||
} | } | |||
else { | else { | |||
if(xMin.empty()) { | if(xMin.empty()) { | |||
ramps.resize(ramps.size()+1); | ramps.resize(ramps.size()+1); | |||
ramps[n].x0 = ramps[p].x1; | ramps[n].x0 = ramps[p].x1; | |||
ramps[n].dx0 = ramps[p].dx1; | ramps[n].dx0 = ramps[p].dx1; | |||
ramps[n].x1 = x; | ramps[n].x1 = x; | |||
ramps[n].dx1 = dx; | ramps[n].dx1 = dx; | |||
bool res=ramps[n].SolveMinTime(accMax,velMax); | bool res=ramps[n].SolveMinTime(accMax,velMax); | |||
PARABOLIC_ASSERT(res); | PARABOLIC_RAMP_ASSERT(res); | |||
} | } | |||
else { | else { | |||
PARABOLIC_ASSERT(InBounds(x,xMin,xMax)); | PARABOLIC_RAMP_ASSERT(InBounds(x,xMin,xMax)); | |||
std::vector<std::vector<ParabolicRamp1D> > tempRamps; | std::vector<std::vector<ParabolicRamp1D> > tempRamps; | |||
std::vector<ParabolicRampND> tempRamps2; | std::vector<ParabolicRampND> tempRamps2; | |||
Real res=SolveMinTimeBounded(ramps[p].x1,ramps[p].dx1,x,dx, | Real res=SolveMinTimeBounded(ramps[p].x1,ramps[p].dx1,x,dx, | |||
accMax,velMax,xMin,xMax,tempRamps) ; | accMax,velMax,xMin,xMax,tempRamps) ; | |||
PARABOLIC_ASSERT(res>=0); | PARABOLIC_RAMP_ASSERT(res>=0); | |||
CombineRamps(tempRamps,tempRamps2); | CombineRamps(tempRamps,tempRamps2); | |||
ramps.insert(ramps.end(),tempRamps2.begin(),tempRamps2.end()); | ramps.insert(ramps.end(),tempRamps2.begin(),tempRamps2.end()); | |||
} | } | |||
} | } | |||
} | } | |||
void DynamicPath::Concat(const DynamicPath& suffix) | void DynamicPath::Concat(const DynamicPath& suffix) | |||
{ | { | |||
PARABOLIC_ASSERT(&suffix != this); | PARABOLIC_RAMP_ASSERT(&suffix != this); | |||
if(suffix.ramps.empty()) return; | if(suffix.ramps.empty()) return; | |||
if(ramps.empty()) { | if(ramps.empty()) { | |||
*this=suffix; | *this=suffix; | |||
return; | return; | |||
} | } | |||
//double check continuity | //double check continuity | |||
if(ramps.back().x1 != suffix.ramps.front().x0 || | if(ramps.back().x1 != suffix.ramps.front().x0 || | |||
ramps.back().dx1 != suffix.ramps.front().dx0) { | ramps.back().dx1 != suffix.ramps.front().dx0) { | |||
Real xmax=0,dxmax=0; | Real xmax=0,dxmax=0; | |||
for(size_t i=0; i<ramps.back().x1.size(); i++) { | for(size_t i=0; i<ramps.back().x1.size(); i++) { | |||
xmax=Max(xmax,Abs(ramps.back().x1[i]-suffix.ramps.front().x0[i] )); | xmax=Max(xmax,Abs(ramps.back().x1[i]-suffix.ramps.front().x0[i] )); | |||
dxmax=Max(dxmax,Abs(ramps.back().dx1[i]-suffix.ramps.front().dx 0[i])); | dxmax=Max(dxmax,Abs(ramps.back().dx1[i]-suffix.ramps.front().dx 0[i])); | |||
} | } | |||
if(Abs(xmax) > EpsilonX || Abs(dxmax) > EpsilonV) { | if(Abs(xmax) > EpsilonX || Abs(dxmax) > EpsilonV) { | |||
PARABOLICLOG("Concat endpoint error\n"); | PARABOLIC_RAMP_PLOG("Concat endpoint error\n"); | |||
PARABOLICLOG("x:\n"); | PARABOLIC_RAMP_PLOG("x:\n"); | |||
for(size_t i=0; i<ramps.back().x1.size(); i++) | for(size_t i=0; i<ramps.back().x1.size(); i++) | |||
PARABOLICLOG("%g - %g = %g\n",ramps.back().x1[i],suffix.ram | PARABOLIC_RAMP_PLOG("%g - %g = %g\n",ramps.back().x1[i],suf | |||
ps.front().x0[i],ramps.back().x1[i]-suffix.ramps.front().x0[i]); | fix.ramps.front().x0[i],ramps.back().x1[i]-suffix.ramps.front().x0[i]); | |||
PARABOLICLOG("dx:\n"); | PARABOLIC_RAMP_PLOG("dx:\n"); | |||
for(size_t i=0; i<ramps.back().x1.size(); i++) | for(size_t i=0; i<ramps.back().x1.size(); i++) | |||
PARABOLICLOG("%g - %g = %g\n",ramps.back().dx1[i],suffix.ra mps.front().dx0[i],ramps.back().dx1[i]-suffix.ramps.front().dx0[i]); | PARABOLIC_RAMP_PLOG("%g - %g = %g\n",ramps.back().dx1[i],su ffix.ramps.front().dx0[i],ramps.back().dx1[i]-suffix.ramps.front().dx0[i]); | |||
//getchar(); | //getchar(); | |||
} | } | |||
ramps.back().x1 = ramps.front().x0; | ramps.back().x1 = ramps.front().x0; | |||
ramps.back().dx1 = ramps.front().dx0; | ramps.back().dx1 = ramps.front().dx0; | |||
for(size_t i=0; i<ramps.back().x1.size(); i++) { | for(size_t i=0; i<ramps.back().x1.size(); i++) { | |||
ramps.back().ramps[i].x1 = suffix.ramps.front().x0[i]; | ramps.back().ramps[i].x1 = suffix.ramps.front().x0[i]; | |||
ramps.back().ramps[i].dx1 = suffix.ramps.front().dx0[i]; | ramps.back().ramps[i].dx1 = suffix.ramps.front().dx0[i]; | |||
} | } | |||
} | } | |||
PARABOLIC_ASSERT(ramps.back().x1 == suffix.ramps.front().x0); | PARABOLIC_RAMP_ASSERT(ramps.back().x1 == suffix.ramps.front().x0); | |||
PARABOLIC_ASSERT(ramps.back().dx1 == suffix.ramps.front().dx0); | PARABOLIC_RAMP_ASSERT(ramps.back().dx1 == suffix.ramps.front().dx0); | |||
ramps.insert(ramps.end(),suffix.ramps.begin(),suffix.ramps.end()); | ramps.insert(ramps.end(),suffix.ramps.begin(),suffix.ramps.end()); | |||
} | } | |||
void DynamicPath::Split(Real t,DynamicPath& before,DynamicPath& after) cons t | void DynamicPath::Split(Real t,DynamicPath& before,DynamicPath& after) cons t | |||
{ | { | |||
PARABOLIC_ASSERT(IsValid()); | PARABOLIC_RAMP_ASSERT(IsValid()); | |||
PARABOLIC_ASSERT(&before != this); | PARABOLIC_RAMP_ASSERT(&before != this); | |||
PARABOLIC_ASSERT(&after != this); | PARABOLIC_RAMP_ASSERT(&after != this); | |||
if(ramps.empty()) { | if(ramps.empty()) { | |||
before=*this; | before=*this; | |||
after=*this; | after=*this; | |||
return; | return; | |||
} | } | |||
after.velMax = before.velMax = velMax; | after.velMax = before.velMax = velMax; | |||
after.accMax = before.accMax = accMax; | after.accMax = before.accMax = accMax; | |||
after.xMin = before.xMin = xMin; | after.xMin = before.xMin = xMin; | |||
after.xMax = before.xMax = xMax; | after.xMax = before.xMax = xMax; | |||
after.ramps.resize(0); | after.ramps.resize(0); | |||
skipping to change at line 407 | skipping to change at line 407 | |||
} | } | |||
else { | else { | |||
if(t < ramps[i].endTime) { | if(t < ramps[i].endTime) { | |||
//cut current path | //cut current path | |||
ParabolicRampND temp=ramps[i]; | ParabolicRampND temp=ramps[i]; | |||
temp.TrimBack(temp.endTime-t); | temp.TrimBack(temp.endTime-t); | |||
before.ramps.push_back(temp); | before.ramps.push_back(temp); | |||
temp=ramps[i]; | temp=ramps[i]; | |||
temp.TrimFront(t); | temp.TrimFront(t); | |||
if(!after.ramps.empty()) { | if(!after.ramps.empty()) { | |||
PARABOLICLOG("DynamicPath::Split: Uh... weird, after is | PARABOLIC_RAMP_PLOG("DynamicPath::Split: Uh... weird, a | |||
not empty?\n"); | fter is not empty?\n"); | |||
PARABOLICLOG("t = %g, i = %d, endtime = %g\n",t,i,ramps | PARABOLIC_RAMP_PLOG("t = %g, i = %d, endtime = %g\n",t, | |||
[i].endTime); | i,ramps[i].endTime); | |||
} | } | |||
PARABOLIC_ASSERT(after.ramps.size() == 0); | PARABOLIC_RAMP_ASSERT(after.ramps.size() == 0); | |||
after.ramps.push_back(temp); | after.ramps.push_back(temp); | |||
} | } | |||
else { | else { | |||
before.ramps.push_back(ramps[i]); | before.ramps.push_back(ramps[i]); | |||
} | } | |||
t -= ramps[i].endTime; | t -= ramps[i].endTime; | |||
} | } | |||
} | } | |||
if(t > 0) { //dt is longer than path | if(t > 0) { //dt is longer than path | |||
ParabolicRampND temp; | ParabolicRampND temp; | |||
temp.SetConstant(ramps.back().x1,t); | temp.SetConstant(ramps.back().x1,t); | |||
before.ramps.push_back(temp); | before.ramps.push_back(temp); | |||
} | } | |||
if(t >= 0) { | if(t >= 0) { | |||
ParabolicRampND temp; | ParabolicRampND temp; | |||
temp.SetConstant(ramps.back().x1); | temp.SetConstant(ramps.back().x1); | |||
after.ramps.push_back(temp); | after.ramps.push_back(temp); | |||
} | } | |||
PARABOLIC_ASSERT(before.IsValid()); | PARABOLIC_RAMP_ASSERT(before.IsValid()); | |||
PARABOLIC_ASSERT(after.IsValid()); | PARABOLIC_RAMP_ASSERT(after.IsValid()); | |||
} | } | |||
struct RampSection | struct RampSection | |||
{ | { | |||
Real ta,tb; | Real ta,tb; | |||
Vector xa,xb; | Vector xa,xb; | |||
Real da,db; | Real da,db; | |||
}; | }; | |||
bool CheckRamp(const ParabolicRampND& ramp,FeasibilityCheckerBase* feas,Dis tanceCheckerBase* distance,int maxiters) | bool CheckRamp(const ParabolicRampND& ramp,FeasibilityCheckerBase* feas,Dis tanceCheckerBase* distance,int maxiters) | |||
{ | { | |||
if(!feas->ConfigFeasible(ramp.x0)) return false; | if(!feas->ConfigFeasible(ramp.x0)) return false; | |||
if(!feas->ConfigFeasible(ramp.x1)) return false; | if(!feas->ConfigFeasible(ramp.x1)) return false; | |||
PARABOLIC_ASSERT(distance->ObstacleDistanceNorm()==Inf); | PARABOLIC_RAMP_ASSERT(distance->ObstacleDistanceNorm()==Inf); | |||
RampSection section; | RampSection section; | |||
section.ta = 0; | section.ta = 0; | |||
section.tb = ramp.endTime; | section.tb = ramp.endTime; | |||
section.xa = ramp.x0; | section.xa = ramp.x0; | |||
section.xb = ramp.x1; | section.xb = ramp.x1; | |||
section.da = distance->ObstacleDistance(ramp.x0); | section.da = distance->ObstacleDistance(ramp.x0); | |||
section.db = distance->ObstacleDistance(ramp.x1); | section.db = distance->ObstacleDistance(ramp.x1); | |||
if(section.da <= 0.0) return false; | if(section.da <= 0.0) return false; | |||
if(section.db <= 0.0) return false; | if(section.db <= 0.0) return false; | |||
list<RampSection> queue; | list<RampSection> queue; | |||
skipping to change at line 474 | skipping to change at line 474 | |||
Vector bmin,bmax; | Vector bmin,bmax; | |||
ramp.Bounds(section.ta,section.tb,bmin,bmax); | ramp.Bounds(section.ta,section.tb,bmin,bmax); | |||
if(MaxBBLInfDistance(section.xa,bmin,bmax) < section.da && | if(MaxBBLInfDistance(section.xa,bmin,bmax) < section.da && | |||
MaxBBLInfDistance(section.xb,bmin,bmax) < section.db) | MaxBBLInfDistance(section.xb,bmin,bmax) < section.db) | |||
//can cull out the section | //can cull out the section | |||
continue; | continue; | |||
} | } | |||
Real tc = (section.ta+section.tb)*0.5; | Real tc = (section.ta+section.tb)*0.5; | |||
Vector xc; | Vector xc; | |||
ramp.Evaluate(tc,xc); | ramp.Evaluate(tc,xc); | |||
if(!feas->ConfigFeasible(xc)) return false; //infeasible config | if(!feas->ConfigFeasible(xc)) return false; //in feasible config | |||
//subdivide | //subdivide | |||
Real dc = distance->ObstacleDistance(xc); | Real dc = distance->ObstacleDistance(xc); | |||
RampSection sa,sb; | RampSection sa,sb; | |||
sa.ta = section.ta; | sa.ta = section.ta; | |||
sa.xa = section.xa; | sa.xa = section.xa; | |||
sa.da = section.da; | sa.da = section.da; | |||
sa.tb = sb.ta = tc; | sa.tb = sb.ta = tc; | |||
sa.xb = sb.xa = xc; | sa.xb = sb.xa = xc; | |||
sa.db = sb.da = dc; | sa.db = sb.da = dc; | |||
sb.tb = section.tb; | sb.tb = section.tb; | |||
skipping to change at line 499 | skipping to change at line 499 | |||
queue.push_back(sa); | queue.push_back(sa); | |||
queue.push_back(sb); | queue.push_back(sb); | |||
if(iters++ >= maxiters) return false; | if(iters++ >= maxiters) return false; | |||
} | } | |||
return true; | return true; | |||
} | } | |||
bool CheckRamp(const ParabolicRampND& ramp,FeasibilityCheckerBase* space,co nst ParabolicRamp::Vector& tol) | bool CheckRamp(const ParabolicRampND& ramp,FeasibilityCheckerBase* space,co nst ParabolicRamp::Vector& tol) | |||
{ | { | |||
PARABOLIC_ASSERT(tol.size() == ramp.ramps.size()); | PARABOLIC_RAMP_ASSERT(tol.size() == ramp.ramps.size()); | |||
for(size_t i = 0; i < tol.size(); ++i) { | for(size_t i = 0; i < tol.size(); ++i) { | |||
PARABOLIC_ASSERT(tol[i] > 0); | PARABOLIC_RAMP_ASSERT(tol[i] > 0); | |||
} | } | |||
if(!space->ConfigFeasible(ramp.x0)) return false; | if(!space->ConfigFeasible(ramp.x0)) return false; | |||
if(!space->ConfigFeasible(ramp.x1)) return false; | if(!space->ConfigFeasible(ramp.x1)) return false; | |||
//PARABOLIC_ASSERT(space->ConfigFeasible(ramp.x0)); | //PARABOLIC_RAMP_ASSERT(space->ConfigFeasible(ramp.x0)); | |||
//PARABOLIC_ASSERT(space->ConfigFeasible(ramp.x1)); | //PARABOLIC_RAMP_ASSERT(space->ConfigFeasible(ramp.x1)); | |||
//for a parabola of form f(x) = a x^2 + b x, and the straight line | //for a parabola of form f(x) = a x^2 + b x, and the straight line | |||
//of form g(X,u) = u*f(X) | //of form g(X,u) = u*f(X) | |||
//d^2(g(X,u),p) = |p - <f(X),p>/<f(X),f(X)> f(X)|^2 < tol^2 | //d^2(g(X,u),p) = |p - <f(X),p>/<f(X),f(X)> f(X)|^2 < tol^2 | |||
//<p,p> - <f(X),p>^2/<f(X),f(X)> = p^T (I-f(X)f(X)^T/f(X)^T f(X)) p | //<p,p> - <f(X),p>^2/<f(X),f(X)> = p^T (I-f(X)f(X)^T/f(X)^T f(X)) p | |||
//max_x d^2(f(x)) => f(x)^T (I-f(X)f(X)^T/f(X)^T f(X)) f'(x) = 0 | //max_x d^2(f(x)) => f(x)^T (I-f(X)f(X)^T/f(X)^T f(X)) f'(x) = 0 | |||
//(x^2 a^T + x b^T) A (2a x + b) = 0 | //(x^2 a^T + x b^T) A (2a x + b) = 0 | |||
//(x a^T + b^T) A (2a x + b) = 0 | //(x a^T + b^T) A (2a x + b) = 0 | |||
//2 x^2 a^T A a + 3 x b^T A a + b^T A b = 0 | //2 x^2 a^T A a + 3 x b^T A a + b^T A b = 0 | |||
skipping to change at line 603 | skipping to change at line 603 | |||
else return CheckRamp(x,feas,tol); | else return CheckRamp(x,feas,tol); | |||
} | } | |||
bool DynamicPath::TryShortcut(Real t1,Real t2,RampFeasibilityChecker& check ) | bool DynamicPath::TryShortcut(Real t1,Real t2,RampFeasibilityChecker& check ) | |||
{ | { | |||
if(t1 > t2) Swap(t1,t2); | if(t1 > t2) Swap(t1,t2); | |||
Real u1,u2; | Real u1,u2; | |||
int i1 = GetSegment(t1,u1); | int i1 = GetSegment(t1,u1); | |||
int i2 = GetSegment(t2,u2); | int i2 = GetSegment(t2,u2); | |||
if(i1 == i2) return false; | if(i1 == i2) return false; | |||
PARABOLIC_ASSERT(u1 >= 0); | PARABOLIC_RAMP_ASSERT(u1 >= 0); | |||
PARABOLIC_ASSERT(u1 <= ramps[i1].endTime+EpsilonT); | PARABOLIC_RAMP_ASSERT(u1 <= ramps[i1].endTime+EpsilonT); | |||
PARABOLIC_ASSERT(u2 >= 0); | PARABOLIC_RAMP_ASSERT(u2 >= 0); | |||
PARABOLIC_ASSERT(u2 <= ramps[i2].endTime+EpsilonT); | PARABOLIC_RAMP_ASSERT(u2 <= ramps[i2].endTime+EpsilonT); | |||
u1 = Min(u1,ramps[i1].endTime); | u1 = Min(u1,ramps[i1].endTime); | |||
u2 = Min(u2,ramps[i2].endTime); | u2 = Min(u2,ramps[i2].endTime); | |||
DynamicPath intermediate; | DynamicPath intermediate; | |||
if(xMin.empty()) { | if(xMin.empty()) { | |||
intermediate.ramps.resize(1); | intermediate.ramps.resize(1); | |||
ParabolicRampND& test=intermediate.ramps[0]; | ParabolicRampND& test=intermediate.ramps[0]; | |||
ramps[i1].Evaluate(u1,test.x0); | ramps[i1].Evaluate(u1,test.x0); | |||
ramps[i2].Evaluate(u2,test.x1); | ramps[i2].Evaluate(u2,test.x1); | |||
ramps[i1].Derivative(u1,test.dx0); | ramps[i1].Derivative(u1,test.dx0); | |||
ramps[i2].Derivative(u2,test.dx1); | ramps[i2].Derivative(u2,test.dx1); | |||
bool res=test.SolveMinTime(accMax,velMax); | bool res=test.SolveMinTime(accMax,velMax); | |||
if(!res) return false; | if(!res) return false; | |||
PARABOLIC_ASSERT(test.endTime >= 0); | PARABOLIC_RAMP_ASSERT(test.endTime >= 0); | |||
PARABOLIC_ASSERT(test.IsValid()); | PARABOLIC_RAMP_ASSERT(test.IsValid()); | |||
} | } | |||
else { | else { | |||
Vector x0,x1,dx0,dx1; | Vector x0,x1,dx0,dx1; | |||
ramps[i1].Evaluate(u1,x0); | ramps[i1].Evaluate(u1,x0); | |||
ramps[i2].Evaluate(u2,x1); | ramps[i2].Evaluate(u2,x1); | |||
ramps[i1].Derivative(u1,dx0); | ramps[i1].Derivative(u1,dx0); | |||
ramps[i2].Derivative(u2,dx1); | ramps[i2].Derivative(u2,dx1); | |||
vector<std::vector<ParabolicRamp1D> > intramps; | vector<std::vector<ParabolicRamp1D> > intramps; | |||
Real res=SolveMinTimeBounded(x0,dx0,x1,dx1, | Real res=SolveMinTimeBounded(x0,dx0,x1,dx1, | |||
accMax,velMax,xMin,xMax, | accMax,velMax,xMin,xMax, | |||
intramps); | intramps); | |||
if(res < 0) return false; | if(res < 0) return false; | |||
CombineRamps(intramps,intermediate.ramps); | CombineRamps(intramps,intermediate.ramps); | |||
intermediate.accMax = accMax; | intermediate.accMax = accMax; | |||
intermediate.velMax = velMax; | intermediate.velMax = velMax; | |||
PARABOLIC_ASSERT(intermediate.IsValid()); | PARABOLIC_RAMP_ASSERT(intermediate.IsValid()); | |||
} | } | |||
for(size_t i=0; i<intermediate.ramps.size(); i++) | for(size_t i=0; i<intermediate.ramps.size(); i++) | |||
if(!check.Check(intermediate.ramps[i])) return false; | if(!check.Check(intermediate.ramps[i])) return false; | |||
//perform shortcut | //perform shortcut | |||
//crop i1 and i2 | //crop i1 and i2 | |||
ramps[i1].TrimBack(ramps[i1].endTime-u1); | ramps[i1].TrimBack(ramps[i1].endTime-u1); | |||
ramps[i1].x1 = intermediate.ramps.front().x0; | ramps[i1].x1 = intermediate.ramps.front().x0; | |||
ramps[i1].dx1 = intermediate.ramps.front().dx0; | ramps[i1].dx1 = intermediate.ramps.front().dx0; | |||
ramps[i2].TrimFront(u2); | ramps[i2].TrimFront(u2); | |||
ramps[i2].x0 = intermediate.ramps.back().x1; | ramps[i2].x0 = intermediate.ramps.back().x1; | |||
ramps[i2].dx0 = intermediate.ramps.back().dx1; | ramps[i2].dx0 = intermediate.ramps.back().dx1; | |||
//replace intermediate ramps with test | //replace intermediate ramps with test | |||
for(int i=0; i<i2-i1-1; i++) | for(int i=0; i<i2-i1-1; i++) | |||
ramps.erase(ramps.begin()+i1+1); | ramps.erase(ramps.begin()+i1+1); | |||
ramps.insert(ramps.begin()+i1+1,intermediate.ramps.begin(),intermediate .ramps.end()); | ramps.insert(ramps.begin()+i1+1,intermediate.ramps.begin(),intermediate .ramps.end()); | |||
//check for consistency | //check for consistency | |||
for(size_t i=0; i+1<ramps.size(); i++) { | for(size_t i=0; i+1<ramps.size(); i++) { | |||
PARABOLIC_ASSERT(ramps[i].x1 == ramps[i+1].x0); | PARABOLIC_RAMP_ASSERT(ramps[i].x1 == ramps[i+1].x0); | |||
PARABOLIC_ASSERT(ramps[i].dx1 == ramps[i+1].dx0); | PARABOLIC_RAMP_ASSERT(ramps[i].dx1 == ramps[i+1].dx0); | |||
} | } | |||
return true; | return true; | |||
} | } | |||
int DynamicPath::Shortcut(int numIters,RampFeasibilityChecker& check) | int DynamicPath::Shortcut(int numIters,RampFeasibilityChecker& check) | |||
{ | { | |||
int shortcuts = 0; | int shortcuts = 0; | |||
vector<Real> rampStartTime(ramps.size()); | vector<Real> rampStartTime(ramps.size()); | |||
Real endTime=0; | Real endTime=0; | |||
for(size_t i=0; i<ramps.size(); i++) { | for(size_t i=0; i<ramps.size(); i++) { | |||
skipping to change at line 687 | skipping to change at line 687 | |||
t1 = 0; | t1 = 0; | |||
t2 = endTime; | t2 = endTime; | |||
} | } | |||
if(t1 > t2) Swap(t1,t2); | if(t1 > t2) Swap(t1,t2); | |||
int i1 = std::upper_bound(rampStartTime.begin(),rampStartTime.end() ,t1)-rampStartTime.begin()-1; | int i1 = std::upper_bound(rampStartTime.begin(),rampStartTime.end() ,t1)-rampStartTime.begin()-1; | |||
int i2 = std::upper_bound(rampStartTime.begin(),rampStartTime.end() ,t2)-rampStartTime.begin()-1; | int i2 = std::upper_bound(rampStartTime.begin(),rampStartTime.end() ,t2)-rampStartTime.begin()-1; | |||
if(i1 == i2) continue; | if(i1 == i2) continue; | |||
//same ramp | //same ramp | |||
Real u1 = t1-rampStartTime[i1]; | Real u1 = t1-rampStartTime[i1]; | |||
Real u2 = t2-rampStartTime[i2]; | Real u2 = t2-rampStartTime[i2]; | |||
PARABOLIC_ASSERT(u1 >= 0); | PARABOLIC_RAMP_ASSERT(u1 >= 0); | |||
PARABOLIC_ASSERT(u1 <= ramps[i1].endTime+EpsilonT); | PARABOLIC_RAMP_ASSERT(u1 <= ramps[i1].endTime+EpsilonT); | |||
PARABOLIC_ASSERT(u2 >= 0); | PARABOLIC_RAMP_ASSERT(u2 >= 0); | |||
PARABOLIC_ASSERT(u2 <= ramps[i2].endTime+EpsilonT); | PARABOLIC_RAMP_ASSERT(u2 <= ramps[i2].endTime+EpsilonT); | |||
u1 = Min(u1,ramps[i1].endTime); | u1 = Min(u1,ramps[i1].endTime); | |||
u2 = Min(u2,ramps[i2].endTime); | u2 = Min(u2,ramps[i2].endTime); | |||
ramps[i1].Evaluate(u1,x0); | ramps[i1].Evaluate(u1,x0); | |||
ramps[i2].Evaluate(u2,x1); | ramps[i2].Evaluate(u2,x1); | |||
ramps[i1].Derivative(u1,dx0); | ramps[i1].Derivative(u1,dx0); | |||
ramps[i2].Derivative(u2,dx1); | ramps[i2].Derivative(u2,dx1); | |||
bool res=SolveMinTime(x0,dx0,x1,dx1,accMax,velMax,xMin,xMax,interme diate); | bool res=SolveMinTime(x0,dx0,x1,dx1,accMax,velMax,xMin,xMax,interme diate); | |||
if(!res) continue; | if(!res) continue; | |||
bool feas=true; | bool feas=true; | |||
for(size_t i=0; i<intermediate.ramps.size(); i++) | for(size_t i=0; i<intermediate.ramps.size(); i++) | |||
skipping to change at line 722 | skipping to change at line 722 | |||
ramps[i2].x0 = intermediate.ramps.back().x1; | ramps[i2].x0 = intermediate.ramps.back().x1; | |||
ramps[i2].dx0 = intermediate.ramps.back().dx1; | ramps[i2].dx0 = intermediate.ramps.back().dx1; | |||
//replace intermediate ramps | //replace intermediate ramps | |||
for(int i=0; i<i2-i1-1; i++) | for(int i=0; i<i2-i1-1; i++) | |||
ramps.erase(ramps.begin()+i1+1); | ramps.erase(ramps.begin()+i1+1); | |||
ramps.insert(ramps.begin()+i1+1,intermediate.ramps.begin(),intermed iate.ramps.end()); | ramps.insert(ramps.begin()+i1+1,intermediate.ramps.begin(),intermed iate.ramps.end()); | |||
//check for consistency | //check for consistency | |||
for(size_t i=0; i+1<ramps.size(); i++) { | for(size_t i=0; i+1<ramps.size(); i++) { | |||
PARABOLIC_ASSERT(ramps[i].x1 == ramps[i+1].x0); | PARABOLIC_RAMP_ASSERT(ramps[i].x1 == ramps[i+1].x0); | |||
PARABOLIC_ASSERT(ramps[i].dx1 == ramps[i+1].dx0); | PARABOLIC_RAMP_ASSERT(ramps[i].dx1 == ramps[i+1].dx0); | |||
} | } | |||
//revise the timing | //revise the timing | |||
rampStartTime.resize(ramps.size()); | rampStartTime.resize(ramps.size()); | |||
endTime=0; | endTime=0; | |||
for(size_t i=0; i<ramps.size(); i++) { | for(size_t i=0; i<ramps.size(); i++) { | |||
rampStartTime[i] = endTime; | rampStartTime[i] = endTime; | |||
endTime += ramps[i].endTime; | endTime += ramps[i].endTime; | |||
} | } | |||
} | } | |||
skipping to change at line 782 | skipping to change at line 782 | |||
while(1) { | while(1) { | |||
//can only start from here | //can only start from here | |||
Real starttime = timer.ElapsedTime()-leadTime; | Real starttime = timer.ElapsedTime()-leadTime; | |||
if(starttime+padTime >= endTime) break; | if(starttime+padTime >= endTime) break; | |||
starttime = Max(0.0,starttime+padTime); | starttime = Max(0.0,starttime+padTime); | |||
Real t1=starttime+Sqr(sampler->Rand())*(endTime-starttime),t2=start time+sampler->Rand()*(endTime-starttime); | Real t1=starttime+Sqr(sampler->Rand())*(endTime-starttime),t2=start time+sampler->Rand()*(endTime-starttime); | |||
if(t1 > t2) Swap(t1,t2); | if(t1 > t2) Swap(t1,t2); | |||
int i1 = std::upper_bound(rampStartTime.begin(),rampStartTime.end() ,t1)-rampStartTime.begin()-1; | int i1 = std::upper_bound(rampStartTime.begin(),rampStartTime.end() ,t1)-rampStartTime.begin()-1; | |||
int i2 = std::upper_bound(rampStartTime.begin(),rampStartTime.end() ,t2)-rampStartTime.begin()-1; | int i2 = std::upper_bound(rampStartTime.begin(),rampStartTime.end() ,t2)-rampStartTime.begin()-1; | |||
if(i1 == i2) continue; //same ramp | if(i1 == i2) continue; //same ramp | |||
Real u1 = t1-rampStartTime[i1]; | Real u1 = t1-rampStartTime[i1]; | |||
Real u2 = t2-rampStartTime[i2]; | Real u2 = t2-rampStartTime[i2]; | |||
PARABOLIC_ASSERT(u1 >= 0); | PARABOLIC_RAMP_ASSERT(u1 >= 0); | |||
PARABOLIC_ASSERT(u1 <= ramps[i1].endTime+EpsilonT); | PARABOLIC_RAMP_ASSERT(u1 <= ramps[i1].endTime+EpsilonT); | |||
PARABOLIC_ASSERT(u2 >= 0); | PARABOLIC_RAMP_ASSERT(u2 >= 0); | |||
PARABOLIC_ASSERT(u2 <= ramps[i2].endTime+EpsilonT); | PARABOLIC_RAMP_ASSERT(u2 <= ramps[i2].endTime+EpsilonT); | |||
u1 = Min(u1,ramps[i1].endTime); | u1 = Min(u1,ramps[i1].endTime); | |||
u2 = Min(u2,ramps[i2].endTime); | u2 = Min(u2,ramps[i2].endTime); | |||
ramps[i1].Evaluate(u1,x0); | ramps[i1].Evaluate(u1,x0); | |||
ramps[i2].Evaluate(u2,x1); | ramps[i2].Evaluate(u2,x1); | |||
ramps[i1].Derivative(u1,dx0); | ramps[i1].Derivative(u1,dx0); | |||
ramps[i2].Derivative(u2,dx1); | ramps[i2].Derivative(u2,dx1); | |||
bool res=SolveMinTime(x0,dx0,x1,dx1,accMax,velMax,xMin,xMax,interme diate); | bool res=SolveMinTime(x0,dx0,x1,dx1,accMax,velMax,xMin,xMax,interme diate); | |||
if(!res) continue; | if(!res) continue; | |||
bool feas=true; | bool feas=true; | |||
for(size_t i=0; i<intermediate.ramps.size(); i++) | for(size_t i=0; i<intermediate.ramps.size(); i++) | |||
skipping to change at line 815 | skipping to change at line 815 | |||
if(timer.ElapsedTime()-leadTime > t1) continue; | if(timer.ElapsedTime()-leadTime > t1) continue; | |||
shortcuts++; | shortcuts++; | |||
//crop i1 and i2 | //crop i1 and i2 | |||
ramps[i1].TrimBack(ramps[i1].endTime-u1); | ramps[i1].TrimBack(ramps[i1].endTime-u1); | |||
ramps[i1].x1 = intermediate.ramps.front().x0; | ramps[i1].x1 = intermediate.ramps.front().x0; | |||
ramps[i1].dx1 = intermediate.ramps.front().dx0; | ramps[i1].dx1 = intermediate.ramps.front().dx0; | |||
ramps[i2].TrimFront(u2); | ramps[i2].TrimFront(u2); | |||
ramps[i2].x0 = intermediate.ramps.back().x1; | ramps[i2].x0 = intermediate.ramps.back().x1; | |||
ramps[i2].dx0 = intermediate.ramps.back().dx1; | ramps[i2].dx0 = intermediate.ramps.back().dx1; | |||
PARABOLIC_ASSERT(ramps[i1].IsValid()); | PARABOLIC_RAMP_ASSERT(ramps[i1].IsValid()); | |||
PARABOLIC_ASSERT(ramps[i2].IsValid()); | PARABOLIC_RAMP_ASSERT(ramps[i2].IsValid()); | |||
//replace intermediate ramps | //replace intermediate ramps | |||
for(int i=0; i<i2-i1-1; i++) | for(int i=0; i<i2-i1-1; i++) | |||
ramps.erase(ramps.begin()+i1+1); | ramps.erase(ramps.begin()+i1+1); | |||
ramps.insert(ramps.begin()+i1+1,intermediate.ramps.begin(),intermed iate.ramps.end()); | ramps.insert(ramps.begin()+i1+1,intermediate.ramps.begin(),intermed iate.ramps.end()); | |||
//check for consistency | //check for consistency | |||
for(size_t i=0; i+1<ramps.size(); i++) { | for(size_t i=0; i+1<ramps.size(); i++) { | |||
PARABOLIC_ASSERT(ramps[i].x1 == ramps[i+1].x0); | PARABOLIC_RAMP_ASSERT(ramps[i].x1 == ramps[i+1].x0); | |||
PARABOLIC_ASSERT(ramps[i].dx1 == ramps[i+1].dx0); | PARABOLIC_RAMP_ASSERT(ramps[i].dx1 == ramps[i+1].dx0); | |||
} | } | |||
//revise the timing | //revise the timing | |||
rampStartTime.resize(ramps.size()); | rampStartTime.resize(ramps.size()); | |||
endTime=0; | endTime=0; | |||
for(size_t i=0; i<ramps.size(); i++) { | for(size_t i=0; i<ramps.size(); i++) { | |||
rampStartTime[i] = endTime; | rampStartTime[i] = endTime; | |||
endTime += ramps[i].endTime; | endTime += ramps[i].endTime; | |||
} | } | |||
} | } | |||
End of changes. 45 change blocks. | ||||
77 lines changed or deleted | 77 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/ |