示例#1
0
bool CheckBounds(Robot& robot,const TimeScaledBezierCurve& traj,Real dt)
{
  Real T=traj.EndTime();
  int numdivs = (int)Ceil(T/dt);
  vector<Real> times(numdivs);
  for(int i=0;i<numdivs;i++) 
    times[i] = T*Real(i)/Real(numdivs-1);  
  return CheckBounds(robot,traj,times);
}
示例#2
0
void TimeOptimizeTest2DCircleBezier()
{
  CartesianCSpace space(2);
  SphereConstraint sphereConstraint;
  SmoothConstrainedInterpolator interp(&space,&sphereConstraint);
  interp.ftol = 1e-8;
  interp.xtol = 1e-2;
  interp.maxNewtonIters = 100;
  vector<Vector> pts;
  pts.resize(5);
  pts[0].resize(2);
  pts[1].resize(2);
  pts[2].resize(2);
  pts[3].resize(2);
  pts[4].resize(2);
  pts[0](0)=1;    pts[0](1)=0;
  pts[1](0)=0;    pts[1](1)=1;
  pts[2](0)=-1;    pts[2](1)=0;
  pts[3](0)=0;    pts[3](1)=-1;
  pts[4](0)=1;    pts[4](1)=0;
  Timer timer;
  TimeScaledBezierCurve curve;
  MultiSmoothInterpolate(interp,pts,curve.path);
  for(size_t i=0;i<curve.path.segments.size();i++)
    Assert(curve.path.segments[i].space == &space);

  printf("Num segments: %d\n",curve.path.segments.size());
  Vector vmin(2,-1.0),vmax(2,1.0);
  Vector amin(2,-1.0),amax(2,1.0);
  bool res=curve.OptimizeTimeScaling(vmin,vmax,amin,amax);
  if(!res) {
    printf("Error optimizing path\n");
    return;
  }
  printf("End time: %g\n",curve.EndTime());
  printf("Solution time: %g s\n",timer.ElapsedTime());
  Assert(curve.timeScaling.times.size()==curve.timeScaling.params.size());
  
#if PRINT_TIME_SCALING
  cout<<"t,s,ds,x,y,dx,dy:"<<endl;
  for(size_t i=0;i<curve.timeScaling.times.size();i++) {
    Vector x,v;
    curve.Eval(curve.timeScaling.times[i],x);
    curve.Deriv(curve.timeScaling.times[i],v);
    cout<<curve.timeScaling.times[i]<<","<<curve.timeScaling.params[i]<<","<<curve.timeScaling.ds[i]<<","<<x[0]<<","<<x[1]<<","<<v[0]<<","<<v[1]<<endl;
  }
#endif
}
示例#3
0
bool GenerateAndTimeOptimizeMultiPath(Robot& robot,MultiPath& multipath,Real xtol,Real dt)
{
  Timer timer;
  vector<GeneralizedCubicBezierSpline > paths;
  if(!InterpolateConstrainedMultiPath(robot,multipath,paths,xtol))
    return false;
  printf("Generated interpolating path in time %gs\n",timer.ElapsedTime());

  RobotCSpace cspace(robot);
  RobotGeodesicManifold manifold(robot);
  for(size_t i=0;i<multipath.sections.size();i++) {
    for(size_t j=0;j<paths[i].segments.size();j++) {
      paths[i].segments[j].space = &cspace;
      paths[i].segments[j].manifold = &manifold;
    }
    for(int iters=0;iters<gNumTimescaleBisectIters;iters++)
      paths[i].Bisect();
  }

#if SAVE_INTERPOLATING_CURVES
  int index=0;
  printf("Saving sections, element %d to section_x_bezier.csv\n",index);
  for(size_t i=0;i<paths.size();i++) {
    {
      stringstream ss;
      ss<<"section_"<<i<<"_bezier.csv";
      ofstream out(ss.str().c_str(),ios::out);
      out<<"duration,x0,x1,x2,x3"<<endl;
      for(size_t j=0;j<paths[i].segments.size();j++) {
	out<<paths[i].durations[j]<<","<<paths[i].segments[j].x0[index]<<","<<paths[i].segments[j].x1[index]<<","<<paths[i].segments[j].x2[index]<<","<<paths[i].segments[j].x3[index]<<endl;
      }
      out.close();
    }
    {
      stringstream ss;
      ss<<"section_"<<i<<"_bezier_vel.csv";
      ofstream out(ss.str().c_str(),ios::out);
      out<<"v(0),v(0.5),v(1)"<<endl;
      Vector temp;
      for(size_t j=0;j<paths[i].segments.size();j++) {
	paths[i].segments[j].Deriv(0,temp);
	temp /= paths[i].durations[j];
	out<<temp[index];
	paths[i].segments[j].Deriv(0.5,temp);
	temp /= paths[i].durations[j];
	out<<","<<temp[index];
	paths[i].segments[j].Deriv(1,temp);
	temp /= paths[i].durations[j];
	out<<","<<temp[index];
	out<<endl;
      }
      out.close();
    }
    {
      stringstream ss;
      ss<<"section_"<<i<<"_bezier_acc.csv";
      ofstream out(ss.str().c_str(),ios::out);
      out<<"a(0),a(1)"<<endl;
      Vector temp;
      for(size_t j=0;j<paths[i].segments.size();j++) {
	paths[i].segments[j].Accel(0,temp);
	temp /= Sqr(paths[i].durations[j]);
	out<<temp[index];
	paths[i].segments[j].Accel(1,temp);
	temp /= Sqr(paths[i].durations[j]);
	out<<","<<temp[index];
	out<<endl;
      }
      out.close();
    }
  }
#endif //SAVE_INTERPOLATING_CURVES
#if SAVE_LORES_INTERPOLATING_CURVES
  paths.clear();
  if(!InterpolateConstrainedMultiPath(robot,multipath,paths,xtol*2.0))
    return false;
  for(size_t i=0;i<multipath.sections.size();i++) {
    for(size_t j=0;j<paths[i].segments.size();j++) {
      paths[i].segments[j].space = &cspace;
      paths[i].segments[j].manifold = &manifold;
    }
  }
  for(size_t i=0;i<paths.size();i++) {
    {
      stringstream ss;
      ss<<"section_"<<i<<"_bezier_x2.csv";
      ofstream out(ss.str().c_str(),ios::out);
      out<<"duration,x0,x1,x2,x3"<<endl;
      for(size_t j=0;j<paths[i].segments.size();j++) {
	out<<paths[i].durations[j]<<","<<paths[i].segments[j].x0[index]<<","<<paths[i].segments[j].x1[index]<<","<<paths[i].segments[j].x2[index]<<","<<paths[i].segments[j].x3[index]<<endl;
      }
      out.close();
    }
    {
      stringstream ss;
      ss<<"section_"<<i<<"_bezier_vel_x2.csv";
      ofstream out(ss.str().c_str(),ios::out);
      out<<"v(0),v(0.5),v(1)"<<endl;
      Vector temp;
      for(size_t j=0;j<paths[i].segments.size();j++) {
	paths[i].segments[j].Deriv(0,temp);
	temp /= paths[i].durations[j];
	out<<temp[index];
	paths[i].segments[j].Deriv(0.5,temp);
	temp /= paths[i].durations[j];
	out<<","<<temp[index];
	paths[i].segments[j].Deriv(1,temp);
	temp /= paths[i].durations[j];
	out<<","<<temp[index];
	out<<endl;
      }
      out.close();
    }
    {
      stringstream ss;
      ss<<"section_"<<i<<"_bezier_acc_x2.csv";
      ofstream out(ss.str().c_str(),ios::out);
      out<<"a(0),a(1)"<<endl;
      Vector temp;
      for(size_t j=0;j<paths[i].segments.size();j++) {
	paths[i].segments[j].Accel(0,temp);
	temp /= Sqr(paths[i].durations[j]);
	out<<temp[index];
	paths[i].segments[j].Accel(1,temp);
	temp /= Sqr(paths[i].durations[j]);
	out<<","<<temp[index];
	out<<endl;
      }
      out.close();
    }
  }

  paths.clear();
  if(!InterpolateConstrainedMultiPath(robot,multipath,paths,xtol*4.0))
    return false;
  for(size_t i=0;i<multipath.sections.size();i++) {
    for(size_t j=0;j<paths[i].segments.size();j++) {
      paths[i].segments[j].space = &cspace;
      paths[i].segments[j].manifold = &manifold;
    }
  }
  for(size_t i=0;i<paths.size();i++) {
    {
      stringstream ss;
      ss<<"section_"<<i<<"_bezier_x4.csv";
      ofstream out(ss.str().c_str(),ios::out);
      out<<"duration,x0,x1,x2,x3"<<endl;
      for(size_t j=0;j<paths[i].segments.size();j++) {
	out<<paths[i].durations[j]<<","<<paths[i].segments[j].x0[index]<<","<<paths[i].segments[j].x1[index]<<","<<paths[i].segments[j].x2[index]<<","<<paths[i].segments[j].x3[index]<<endl;
      }
      out.close();
    }
    {
      stringstream ss;
      ss<<"section_"<<i<<"_bezier_vel_x4.csv";
      ofstream out(ss.str().c_str(),ios::out);
      out<<"v(0),v(0.5),v(1)"<<endl;
      Vector temp;
      for(size_t j=0;j<paths[i].segments.size();j++) {
	paths[i].segments[j].Deriv(0,temp);
	temp /= paths[i].durations[j];
	out<<temp[index];
	paths[i].segments[j].Deriv(0.5,temp);
	temp /= paths[i].durations[j];
	out<<","<<temp[index];
	paths[i].segments[j].Deriv(1,temp);
	temp /= paths[i].durations[j];
	out<<","<<temp[index];
	out<<endl;
      }
      out.close();
    }
    {
      stringstream ss;
      ss<<"section_"<<i<<"_bezier_acc_x4.csv";
      ofstream out(ss.str().c_str(),ios::out);
      out<<"a(0),a(1)"<<endl;
      Vector temp;
      for(size_t j=0;j<paths[i].segments.size();j++) {
	paths[i].segments[j].Accel(0,temp);
	temp /= Sqr(paths[i].durations[j]);
	out<<temp[index];
	paths[i].segments[j].Accel(1,temp);
	temp /= Sqr(paths[i].durations[j]);
	out<<","<<temp[index];
	out<<endl;
      }
      out.close();
    }
  }

  paths.clear();
  if(!InterpolateConstrainedMultiPath(robot,multipath,paths,xtol*8.0))
    return false;
  for(size_t i=0;i<multipath.sections.size();i++) {
    for(size_t j=0;j<paths[i].segments.size();j++) {
      paths[i].segments[j].space = &cspace;
      paths[i].segments[j].manifold = &manifold;
    }
  }
  for(size_t i=0;i<paths.size();i++) {
    {
      stringstream ss;
      ss<<"section_"<<i<<"_bezier_x8.csv";
      ofstream out(ss.str().c_str(),ios::out);
      out<<"duration,x0,x1,x2,x3"<<endl;
      for(size_t j=0;j<paths[i].segments.size();j++) {
	out<<paths[i].durations[j]<<","<<paths[i].segments[j].x0[index]<<","<<paths[i].segments[j].x1[index]<<","<<paths[i].segments[j].x2[index]<<","<<paths[i].segments[j].x3[index]<<endl;
      }
      out.close();
    }
    {
      stringstream ss;
      ss<<"section_"<<i<<"_bezier_vel_x8.csv";
      ofstream out(ss.str().c_str(),ios::out);
      out<<"v(0),v(0.5),v(1)"<<endl;
      Vector temp;
      for(size_t j=0;j<paths[i].segments.size();j++) {
	paths[i].segments[j].Deriv(0,temp);
	temp /= paths[i].durations[j];
	out<<temp[index];
	paths[i].segments[j].Deriv(0.5,temp);
	temp /= paths[i].durations[j];
	out<<","<<temp[index];
	paths[i].segments[j].Deriv(1,temp);
	temp /= paths[i].durations[j];
	out<<","<<temp[index];
	out<<endl;
      }
      out.close();
    }
    {
      stringstream ss;
      ss<<"section_"<<i<<"_bezier_acc_x8.csv";
      ofstream out(ss.str().c_str(),ios::out);
      out<<"a(0),a(1)"<<endl;
      Vector temp;
      for(size_t j=0;j<paths[i].segments.size();j++) {
	paths[i].segments[j].Accel(0,temp);
	temp /= Sqr(paths[i].durations[j]);
	out<<temp[index];
	paths[i].segments[j].Accel(1,temp);
	temp /= Sqr(paths[i].durations[j]);
	out<<","<<temp[index];
	out<<endl;
      }
      out.close();
    }
  }


#endif //SAVE_LORES_INTERPOLATING_CURVES

  //concatenate sections into a single curve
  TimeScaledBezierCurve traj;
  vector<int> edgeToSection,sectionEdges(1,0);
  for(size_t i=0;i<multipath.sections.size();i++) {
    traj.path.Concat(paths[i]);
    for(size_t j=0;j<paths[i].segments.size();j++)
      edgeToSection.push_back((int)i);
    sectionEdges.push_back(sectionEdges.back()+(int)paths[i].segments.size());
  }

  timer.Reset();
  bool res=traj.OptimizeTimeScaling(robot.velMin,robot.velMax,-1.0*robot.accMax,robot.accMax);
  if(!res) {
    printf("Failed to optimize time scaling\n");
    return false;
  }
  else {
    printf("Optimized into a path with duration %g, (took %gs)\n",traj.EndTime(),timer.ElapsedTime());
  }
  double T = traj.EndTime();
  int numdivs = (int)Ceil(T/dt);
  printf("Discretizing at time resolution %g\n",T/numdivs);
  numdivs++;

  Vector x,v;
  int sCur = -1;
  for(int i=0;i<numdivs;i++) {
    Real t=T*Real(i)/Real(numdivs-1);
    int trajEdge = traj.timeScaling.TimeToSegment(t);
    if(trajEdge == (int)edgeToSection.size()) trajEdge--; //end of path
    Assert(trajEdge < (int)edgeToSection.size());
    int s=edgeToSection[trajEdge];
    if(s < sCur) {
      fprintf(stderr,"Strange: edge index is going backward? %d -> %d\n",sCur,s);
      fprintf(stderr,"  time %g, division %d, traj segment %d\n",t,i,trajEdge);
    }
    Assert(s - sCur >=0);
    while(sCur < s) {
      //close off the current section and add a new one
      Real switchTime=traj.timeScaling.times[sectionEdges[sCur+1]];
      Assert(switchTime <= t);
      traj.Eval(switchTime,x);
      traj.Deriv(switchTime,v);
      if(sCur >= 0) {
	multipath.sections[sCur].times.push_back(switchTime);
	multipath.sections[sCur].milestones.push_back(x);
	multipath.sections[sCur].velocities.push_back(v);
      }
      multipath.sections[sCur+1].milestones.resize(0);
      multipath.sections[sCur+1].velocities.resize(0);
      multipath.sections[sCur+1].times.resize(0);
      multipath.sections[sCur+1].milestones.push_back(x);
      multipath.sections[sCur+1].velocities.push_back(v);
      multipath.sections[sCur+1].times.push_back(switchTime);
      sCur++;
    }
    if(t == multipath.sections[s].times.back()) continue;
    traj.Eval(t,x);
    traj.Deriv(t,v);
    multipath.sections[s].times.push_back(t);
    multipath.sections[s].milestones.push_back(x);
    multipath.sections[s].velocities.push_back(v);
  }

#if DO_TEST_TRIANGULAR
  timer.Reset();
  TimeScaledBezierCurve trajTri;
  Real Ttrap = 0;
  printf("%d paths?\n",paths.size());
  for(size_t i=0;i<paths.size();i++)
    Ttrap += OptimalTriangularTimeScaling(paths[i],robot.velMin,robot.velMax,-1.0*robot.accMax,robot.accMax,trajTri);
  printf("Optimal trapezoidal time scaling duration %g, calculated in %gs\n",Ttrap,timer.ElapsedTime());
  printf("Saving plot to opt_tri_multipath.csv\n");
  trajTri.Plot("opt_tri_multipath.csv",robot.velMin,robot.velMax,-1.0*robot.accMax,robot.accMax);
#endif // DO_TEST_TRAPEZOIDAL


#if DO_CHECK_BOUNDS  
  CheckBounds(robot,traj,dt);
#endif // DO_CHECK_BOUNDS

#if DO_SAVE_PLOT
  printf("Saving plot to opt_multipath.csv\n");
  traj.Plot("opt_multipath.csv",robot.velMin,robot.velMax,-1.0*robot.accMax,robot.accMax);
#endif //DO_SAVE_PLOT

#if DO_SAVE_LIMITS
  SaveLimits(robot,traj,dt,"opt_multipath_limits.csv");
#endif // DO_SAVE_LIMITS

  {
    multipath.settings.set("resolution",xtol);
    multipath.settings.set("program","GenerateAndTimeOptimizeMultiPath");
  }
  return true;
}
示例#4
0
bool TimeOptimizePath(Robot& robot,const vector<Real>& oldtimes,const vector<Config>& oldconfigs,Real dt,vector<Real>& newtimes,vector<Config>& newconfigs)
{
  //make a smooth interpolator
  Vector dx0,dx1,temp;
  RobotCSpace cspace(robot);
  RobotGeodesicManifold manifold(robot);
  TimeScaledBezierCurve traj;
  traj.path.durations.resize(oldconfigs.size()-1);
  for(size_t i=0;i+1<oldconfigs.size();i++) {
    traj.path.durations[i] = oldtimes[i+1]-oldtimes[i];
    if(!(traj.path.durations[i] > 0)) {
      fprintf(stderr,"TimeOptimizePath: input path does not have monotonically increasing time: segment %d range [%g,%g]\n",i,oldtimes[i],oldtimes[i+1]);
      return false;
    }
    Assert(traj.path.durations[i] > 0);
  }
  traj.path.segments.resize(oldconfigs.size()-1);
  for(size_t i=0;i+1<oldconfigs.size();i++) {
    traj.path.segments[i].space = &cspace;
    traj.path.segments[i].manifold = &manifold;
    traj.path.segments[i].x0 = oldconfigs[i];
    traj.path.segments[i].x3 = oldconfigs[i+1];
    if(i > 0) {
      InterpolateDerivative(robot,oldconfigs[i],oldconfigs[i+1],dx0);
      InterpolateDerivative(robot,oldconfigs[i],oldconfigs[i-1],temp);
      dx0 -= temp*(traj.path.durations[i]/traj.path.durations[i-1]);
      dx0 *= 0.5;
    }
    else dx0.resize(0);
    if(i+2 < oldconfigs.size()) {
      InterpolateDerivative(robot,oldconfigs[i+1],oldconfigs[i+2],dx1);
      InterpolateDerivative(robot,oldconfigs[i+1],oldconfigs[i],temp);
      dx1 *= traj.path.durations[i]/traj.path.durations[i+1];
      dx1 -= temp;
      dx1 *= 0.5;
    }
    else dx1.resize(0);
    traj.path.segments[i].SetNaturalTangents(dx0,dx1);
  }
  Timer timer;
  bool res=traj.OptimizeTimeScaling(robot.velMin,robot.velMax,-1.0*robot.accMax,robot.accMax);
  if(!res) {
    printf("Failed to optimize time scaling\n");
    return false;
  }
  else {
    printf("Optimized into a path with duration %g, (took %gs)\n",traj.EndTime(),timer.ElapsedTime());
  }
  double T = traj.EndTime();
  int numdivs = (int)Ceil(T/dt);

  printf("Discretizing at time resolution %g\n",T/numdivs);
  numdivs++;
  newtimes.resize(numdivs);
  newconfigs.resize(numdivs);

  for(int i=0;i<numdivs;i++) {
    newtimes[i] = T*Real(i)/Real(numdivs-1);
    traj.Eval(newtimes[i],newconfigs[i]);
  }
#if DO_CHECK_BOUNDS
  CheckBounds(robot,traj,newtimes);
#endif  //DO_CHECKBOUNDS
  return true;
}
示例#5
0
void SaveLimits(Robot& robot,const TimeScaledBezierCurve& traj,Real dt,const char* fn)
{
  Real T=traj.EndTime();
  int numdivs = (int)Ceil(T/dt);
  printf("Saving time-scaled values to %s\n",fn);
  ofstream out(fn,ios::out);
  out<<"t";
  for(size_t i=0;i<robot.links.size();i++)
    out<<",q["<<robot.linkNames[i]<<"]";
  for(size_t i=0;i<robot.links.size();i++)
    out<<",v["<<robot.linkNames[i]<<"]";
  for(size_t i=0;i<robot.links.size();i++)
    out<<",a["<<robot.linkNames[i]<<"]";
  out<<",activeVLimit,activeAlimit,vsaturation,asaturation";
  out<<endl;

  Vector q,v,a,maxv,maxa;
  for(int i=0;i<=numdivs;i++) {
    //double-checking velocity and acceleration bounds
    Real t = Real(i)/Real(numdivs)*T;
    traj.Eval(t,q);
    traj.Deriv(t,v);
    traj.Accel(t,a);
    out<<t;
    for(int j=0;j<q.n;j++)
      out<<","<<q(j);
    for(int j=0;j<v.n;j++)
      out<<","<<v(j);
    for(int j=0;j<a.n;j++)
      out<<","<<a(j);
    Real maxVSat=0,maxASat=0;
    int maxVSatInd=0,maxASatInd=0;
    for(int j=0;j<v.n;j++)
      if(Abs(v(j))/robot.velMax(j) > maxVSat) {
	maxVSat = Abs(v(j))/robot.velMax(j);
	maxVSatInd = j;
      }
    for(int j=0;j<a.n;j++)
      if(Abs(a(j))/robot.accMax(j) > maxASat) {
	maxASat = Abs(a(j))/robot.accMax(j);
	maxASatInd = j;
      }
    out<<","<<robot.linkNames[maxVSatInd];
    out<<","<<robot.linkNames[maxASatInd];
    out<<","<<maxVSat<<","<<maxASat<<endl;
  }
  out<<endl;
  for(size_t i=0;i<traj.timeScaling.times.size();i++) {
    //double-checking velocity and acceleration bounds
    Real t = traj.timeScaling.times[i];
    traj.Eval(t,q);
    traj.Deriv(t,v);
    traj.Accel(t,a);
    out<<t;
    for(int j=0;j<q.n;j++)
      out<<","<<q(j);
    for(int j=0;j<v.n;j++)
      out<<","<<v(j);
    for(int j=0;j<a.n;j++)
      out<<","<<a(j);
    Real maxSat=0;
    int maxSatInd=0;
    bool maxSatA=false;
    for(int j=0;j<v.n;j++)
      if(Abs(v(j))/robot.velMax(j) > maxSat) {
	maxSat = Abs(v(j))/robot.velMax(j);
	maxSatInd = j;
      }
    for(int j=0;j<a.n;j++)
      if(Abs(a(j))/robot.accMax(j) > maxSat) {
	maxSat = Abs(a(j))/robot.accMax(j);
	maxSatInd = j;
	maxSatA=true;
      }
    if(maxSatA)
      out<<",a["<<robot.linkNames[maxSatInd]<<"]";
    else
      out<<",v["<<robot.linkNames[maxSatInd]<<"]";
    out<<","<<maxSat<<endl;
  }
}
示例#6
0
int main(int argc,const char** argv)
{
  if(argc < 5) {
    printf("USAGE: timeopt spline limits gridRes dt\n");
    printf("  Optimizes the time scaling of the cubic spline 'spline' under\n");
    printf("  velocity/acceleration limits in the file 'limits'.\n");
    printf("  The time-scaling domain is split into gridRes points.\n");
    printf("  The output trajectory  is a list of time/milestone pairs\n");
    printf("  discretized at timestep dt.\n");
    return 0;
  }
  TimeScaledBezierCurve output;
  {
    ifstream in(argv[1],ios::in);
    if(!output.path.Load(in)) {
      printf("Unable to load spline file %s\n",argv[1]);
      return 1;
    }
  }
  Vector vmin,vmax,amin,amax;
  {
    ifstream in(argv[2],ios::in);
    if(!in) {
      printf("Unable to load limits file %s\n",argv[2]);
      return 1;
    }
    in >> vmin >> vmax >> amin >> amax;
    if(!in) {
      printf("Error loading limits file %s\n",argv[2]);
      return 1;
    }
  }

  for(size_t i=0;i<output.path.segments.size();i++) {
    if(output.path.segments[i].x0.n != output.path.segments[0].x0.n) {
      printf("Invalid milestone size on segment %d: %d vs %d\n",i,output.path.segments[i].x0.n,output.path.segments[0].x0.n);
      return 1;
    }
    if(output.path.segments[i].x1.n != output.path.segments[0].x0.n) {
      printf("Invalid milestone size on segment %d: %d vs %d\n",i,output.path.segments[i].x1.n,output.path.segments[0].x0.n);
      return 1;
    }
    if(output.path.segments[i].x2.n != output.path.segments[0].x0.n) {
      printf("Invalid milestone size on segment %d: %d vs %d\n",i,output.path.segments[i].x2.n,output.path.segments[0].x0.n);
      return 1;
    }
    if(output.path.segments[i].x3.n != output.path.segments[0].x0.n) {
      printf("Invalid milestone size on segment %d: %d vs %d\n",i,output.path.segments[i].x3.n,output.path.segments[0].x0.n);
      return 1;
    }
  }
  if(vmin.n != output.path.segments[0].x0.n) {
    printf("Invalid length of limits: %d vs %d\n",vmin.n,output.path.segments[0].x0.n);
    return 1;
  }
  int gridRes;
  Real dt;
  gridRes = atoi(argv[3]);
  dt = atof(argv[4]);

  if((int)output.path.segments.size() < gridRes) {
    GeneralizedCubicBezierSpline path;
    path.segments.reserve(gridRes+output.path.segments.size());
    path.durations.reserve(gridRes+output.path.segments.size());
    Config xp,vp,xn,vn;
    Real T = output.path.TotalTime();
    for(size_t i=0;i<output.path.segments.size();i++) {
      int n = (int)Ceil(gridRes*(output.path.durations[i]/T));
      if(n <= 0) {
	printf("Error: curve segment %d has nonpositive duration?\n",i);
	return 1;
      }

      //split segment[i] into n pieces
      Real divScale = 1.0/Real(n);
      xp = output.path.segments[i].x0;
      output.path.segments[i].Deriv(0,vp);
      for(int j=0;j<n;j++) {
	Real u2=Real(j+1)/Real(n);
	output.path.segments[i].Eval(u2,xn);
	output.path.segments[i].Deriv(u2,vn);
	path.segments.resize(path.segments.size()+1);
	path.durations.push_back(divScale*output.path.durations[i]);
	path.segments.back().x0 = xp;
	path.segments.back().x3 = xn;
	path.segments.back().SetNaturalTangents(vp*divScale,vn*divScale);
	swap(xp,xn);
	swap(vp,vn);
      }
    }
    output.path = path;
  }

  Timer timer;
  if(!output.OptimizeTimeScaling(vmin,vmax,amin,amax)) {
    printf("Error: OptimizeTimeScaling failed\n");
    return 1;
  }
  Real elapsedTime = timer.ElapsedTime();

  printf("Optimized duration %g, time %g\n",output.EndTime(),elapsedTime);
  vector<Vector> milestones;
  output.GetDiscretizedPath(dt,milestones);
  for(size_t i=0;i<milestones.size();i++)
    cout<<dt*i<<"\t"<<milestones[i]<<endl;;

  printf("Saving time scaling to timeopt_plot.csv\n");
  output.Plot("timeopt_plot.csv",vmin,vmax,amin,amax,1.0/Real(gridRes));
  return 0;
}