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/