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;
}
