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
}
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);
}
bool CheckBounds(Robot& robot,const TimeScaledBezierCurve& traj,const vector<Real>& times)
{
Vector v,a,maxv,maxa;
Vector oldv,diffa,maxdiffa;
bool res=true;
for(size_t i=0;i<times.size();i++) {
//double-checking velocity and acceleration bounds
traj.Deriv(times[i],v);
traj.Accel(times[i],a);
if(i==0) { maxv=v; maxa=a; }
else {
for(int j=0;j<v.n;j++) {
maxv[j] = Max(maxv[j],Abs(v[j]));
maxa[j] = Max(maxa[j],Abs(a[j]));
}
}
for(int j=0;j<v.n;j++) {
if(Abs(v[j]) > robot.velMax[j]+1e-3) {
printf("Exceeded vel max %s=%g at time %g\n",robot.LinkName(j).c_str(),v(j),times[i]);
res = false;
}
if(Abs(a[j]) > robot.accMax[j]+1e-3) {
printf("Exceeded accel max %s=%g at time %g\n",robot.LinkName(j).c_str(),a(j),times[i]);
res = false;
}
}
if(!oldv.empty()) {
diffa = (v-oldv)/(times[i]-times[i-1]);
for(int j=0;j<v.n;j++) {
if(Abs(diffa[j]) > robot.accMax[j]+1e-3) {
printf("Diff accel max %s=%g at time %g\n",robot.LinkName(j).c_str(),diffa(j),times[i]);
res = false;
}
}
}
oldv = v;
}
cout<<"Max vel "<<maxv<<endl;
cout<<"Max accel "<<maxa<<endl;
cout<<"End vel "<<v<<endl;
cout<<"End accel "<<a<<endl;
return res;
}
bool InterpolateAndTimeOptimize(const vector<Config>& configs,
GeodesicManifold* space,
VectorFieldFunction* constraint,
Real constraintTolerance,
const Vector& vmin,const Vector& vmax,
const Vector& amin,const Vector& amax,
TimeScaledBezierCurve& output)
{
if(constraint) {
SmoothConstrainedInterpolator interp(space,constraint);
interp.xtol = constraintTolerance;
if(!MultiSmoothInterpolate(interp,configs,output.path)) return false;
}
else {
GeneralizedCubicBezierSpline path;
SplineInterpolate(configs,path.segments,space);
path.durations.resize(path.segments.size());
for(size_t i=0;i+1<path.segments.size();i++)
path.durations[i] = 1.0/path.durations.size();
//treat constraintTolerance as a resolution, discretize the curves into
//n pieces
int n=(int)Ceil(1.0/constraintTolerance);
output.path.segments.resize(n);
output.path.durations.resize(n);
Config xp,vp,xn,vn;
path.Eval(0,xp);
path.Deriv(0,vp);
for(int i=0;i<n;i++) {
Real u2=Real(i+1)/Real(n);
path.Eval(u2,xn);
path.Deriv(u2,vn);
output.path.segments[i].x0 = xp;
output.path.segments[i].x3 = xn;
output.path.segments[i].SetNaturalTangents(vp,vn);
output.path.durations[i] = 1.0/Real(n);
}
swap(xp,xn);
swap(vp,vn);
}
if(!output.OptimizeTimeScaling(vmin,vmax,amin,amax)) return false;
return true;
}
void TimeOptimizeTestNLinkPlanar()
{
const char* fn = "data/planar%d.rob";
int ns [] = {5,10,15,20,25,30,40,50,60,70,80,90,100};
Real tolscales [] = {3.95,3.55,2.73,2,2,2,1.8,1.6,1.42,1,1,1,1};
int num = 13;
char buf[256];
for(int i=0;i<num;i++) {
int n=ns[i];
printf("Testing optimize %d\n",n);
sprintf(buf,fn,n);
Robot robot;
if(!robot.Load(buf)) {
fprintf(stderr,"Unable to load robot %s\n",buf);
return;
}
int ee = robot.links.size()-1;
Vector3 localpt(1,0,0);
Real len = robot.links.size();
//make a half circle
Config a(robot.q.n,Pi/robot.links.size()),b;
a[0] *= 0.5;
robot.UpdateConfig(a);
IKGoal goal,goaltemp;
goal.link = ee;
goal.localPosition = localpt;
goal.SetFixedPosition(robot.links[ee].T_World*localpt);
//cout<<"Goal position "<<goal.endPosition<<endl;
goaltemp = goal;
goaltemp.link = ee/2;
goaltemp.SetFixedPosition(goal.endPosition*0.5);
//cout<<"Middle goal position "<<goaltemp.endPosition<<endl;
vector<IKGoal> onegoal(1),bothgoals(2);
onegoal[0] = goal;
bothgoals[0] = goal;
bothgoals[1] = goaltemp;
int iters=100;
bool res=SolveIK(robot,bothgoals,1e-3,iters);
if(!res) {
fprintf(stderr,"Couldn't solve for target robot config\n");
return;
}
b = robot.q;
Timer timer;
GeneralizedCubicBezierSpline path;
RobotSmoothConstrainedInterpolator interp(robot,onegoal);
interp.xtol = 1e-3*tolscales[i];
if(!interp.Make(a,b,path)) {
fprintf(stderr,"Couldn't interpolate for target robot config\n");
return;
}
printf("Solved for path with tol %g in time %g\n",interp.xtol,timer.ElapsedTime());
{
sprintf(buf,"trajopt_a_%d.config",n);
ofstream out(buf);
out<<a<<endl;
}
{
sprintf(buf,"trajopt_b_%d.config",n);
ofstream out(buf);
out<<b<<endl;
}
{
sprintf(buf,"trajopt_interp_%d.xml",n);
vector<Real> times;
vector<Config> configs;
path.GetPiecewiseLinear(times,configs);
MultiPath mpath;
mpath.SetTimedMilestones(times,configs);
mpath.SetIKProblem(onegoal);
mpath.Save(buf);
}
{
//unroll joints
for(size_t i=0;i<path.segments.size();i++) {
for(int j=0;j<path.segments[i].x0.n;j++) {
if(path.segments[i].x0[j] > Pi)
path.segments[i].x0[j] -= TwoPi;
if(path.segments[i].x1[j] > Pi)
path.segments[i].x1[j] -= TwoPi;
if(path.segments[i].x2[j] > Pi)
path.segments[i].x2[j] -= TwoPi;
if(path.segments[i].x3[j] > Pi)
path.segments[i].x3[j] -= TwoPi;
}
}
sprintf(buf,"trajopt_interp_%d.spline",n);
ofstream out(buf,ios::out);
out.precision(10);
path.Save(out);
}
TimeScaledBezierCurve curve;
//curve.path = path;
{
sprintf(buf,"trajopt_interp_%d.spline",n);
ifstream in(buf,ios::in);
curve.path.Load(in);
Assert(curve.path.segments.size() == path.durations.size());
for(size_t i=0;i<curve.path.durations.size();i++) {
Assert(FuzzyEquals(curve.path.durations[i],path.durations[i]));
if(!curve.path.segments[i].x0.isEqual(path.segments[i].x0,Epsilon)) {
printf("Error on segment %d\n",i);
cout<<path.segments[i].x0<<endl;
cout<<curve.path.segments[i].x0<<endl;
cout<<"Distance: "<<path.segments[i].x0.distance(curve.path.segments[i].x0)<<endl;
}
Assert(curve.path.segments[i].x0.isEqual(path.segments[i].x0,Epsilon));
Assert(curve.path.segments[i].x1.isEqual(path.segments[i].x1,Epsilon));
Assert(curve.path.segments[i].x2.isEqual(path.segments[i].x2,Epsilon));
Assert(curve.path.segments[i].x3.isEqual(path.segments[i].x3,Epsilon));
}
}
Vector vmin(robot.q.n,-Pi),vmax(robot.q.n,Pi);
Vector amin(robot.q.n,-Pi),amax(robot.q.n,Pi);
timer.Reset();
if(!curve.OptimizeTimeScaling(vmin,vmax,amin,amax)) {
fprintf(stderr,"Optimize failed in time %g\n",timer.ElapsedTime());
return;
}
printf("Solved time optimization with %d segments in time %g\n",curve.path.segments.size(),timer.ElapsedTime());
//output optimized path
{
Real dt = 0.5;
vector<Real> times;
vector<Config> configs;
/*curve.GetDiscretizedPath(dt,configs);
times.resize(configs.size());
for(size_t j=0;j<times.size();j++)
times[j] = j*dt;
*/
curve.GetPiecewiseLinear(times,configs);
MultiPath mpath;
mpath.SetIKProblem(onegoal);
mpath.SetTimedMilestones(times,configs);
sprintf(buf,"trajopt_opt_%d.xml",n);
mpath.Save(buf);
}
}
}
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;
}
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;
}
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;
}
}
void ContactOptimizeMultipath(const char* robfile,const char* pathfile,const char* settingsfile = NULL)
{
ContactOptimizeSettings settings;
if(settingsfile != NULL) {
if(!settings.read(settingsfile)) {
printf("Unable to read settings file %s\n",settingsfile);
return;
}
}
Real xtol=Real(settings["xtol"]);
int numdivs = int(settings["numdivs"]);
bool ignoreForces = bool(settings["ignoreForces"]);
Real torqueRobustness = Real(settings["torqueRobustness"]);
Real frictionRobustness = Real(settings["frictionRobustness"]);
Real forceRobustness = Real(settings["forceRobustness"]);
string outputPath;
settings["outputPath"].as(outputPath);
Real outputDt = Real(settings["outputDt"]);
Robot robot;
if(!robot.Load(robfile)) {
printf("Unable to load robot file %s\n",robfile);
return;
}
MultiPath path;
if(!path.Load(pathfile)) {
printf("Unable to load path file %s\n",pathfile);
return;
}
//double check friction
for(size_t i=0;i<path.sections.size();i++) {
Stance s;
path.GetStance(s,i);
bool changed = false;
int k=0;
int numContacts = 0;
for(Stance::iterator h=s.begin();h!=s.end();h++,k++) {
numContacts += (int)h->second.contacts.size();
for(size_t j=0;j<h->second.contacts.size();j++)
if(h->second.contacts[j].kFriction <= 0) {
if(!changed)
printf("Warning, contact %d of hold %d has invalid friction %g, setting to 0.5\n",j,k,h->second.contacts[j].kFriction);
h->second.contacts[j].kFriction = 0.5;
changed = true;
}
}
path.SetStance(s,i);
if(numContacts == 0 && !ignoreForces && robot.joints[0].type == RobotJoint::Floating) {
printf("Warning, no contacts given in stance %d for floating-base robot\n",i);
printf("Should set ignoreForces = true in trajopt.settings if you wish\n");
printf("to ignore contact force constraints.\n");
printf("Press enter to continue...\n");
getchar();
}
}
TimeScaledBezierCurve opttraj;
if(ignoreForces) {
bool res=GenerateAndTimeOptimizeMultiPath(robot,path,xtol,outputDt);
if(!res) {
printf("Time optimization failed\n");
return;
}
Assert(path.HasTiming());
cout<<"Saving dynamically optimized path to "<<outputPath<<endl;
ofstream out(outputPath.c_str(),ios::out);
for(size_t i=0;i<path.sections.size();i++) {
for(size_t j=0;j<path.sections[i].milestones.size();j++)
out<<path.sections[i].times[j]<<"\t"<<path.sections[i].milestones[j]<<endl;
}
out.close();
}
else {
bool res=ContactOptimizeMultipath(robot,path,xtol,
numdivs,opttraj,
torqueRobustness,
frictionRobustness,
forceRobustness);
if(!res) {
printf("Time optimization failed\n");
return;
}
RobotCSpace cspace(robot);
for(size_t i=0;i<opttraj.path.segments.size();i++) {
opttraj.path.segments[i].space = &cspace;
opttraj.path.segments[i].manifold = &cspace;
}
vector<Config> milestones;
opttraj.GetDiscretizedPath(outputDt,milestones);
cout<<"Saving dynamically optimized path to "<<outputPath<<endl;
ofstream out(outputPath.c_str(),ios::out);
for(size_t i=0;i<milestones.size();i++) {
out<<i*outputDt<<"\t"<<milestones[i]<<endl;
}
out.close();
printf("Plotting vel/acc constraints to trajopt_plot.csv...\n");
opttraj.Plot("trajopt_plot.csv",robot.velMin,robot.velMax,-1.0*robot.accMax,robot.accMax);
}
}
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;
}
