int main(int argc,const char** argv) { if(argc <= 3) { printf("USAGE: ContactPlan [options] world_file configs stance\n"); printf("OPTIONS:\n"); printf("-o filename: the output linear path or multipath (default contactplan.xml)\n"); printf("-p settings: set the planner configuration file\n"); printf("-opt: do optimal planning (do not terminate on the first found solution)\n"); printf("-n iters: set the default number of iterations per step (default 1000)\n"); printf("-t time: set the planning time limit (default infinity)\n"); printf("-m margin: set support polygon margin (default 0)\n"); printf("-r robotindex: set the robot index (default 0)\n"); return 0; } Srand(time(NULL)); int robot = 0; const char* outputfile = "contactplan.xml"; HaltingCondition termCond; string plannerSettings; int i; //parse command line arguments for(i=1;i<argc;i++) { if(argv[i][0]=='-') { if(0==strcmp(argv[i],"-n")) { termCond.maxIters = atoi(argv[i+1]); i++; } else if(0==strcmp(argv[i],"-t")) { termCond.timeLimit = atof(argv[i+1]); i++; } else if(0==strcmp(argv[i],"-opt")) { termCond.foundSolution = false; } else if(0==strcmp(argv[i],"-p")) { if(!GetFileContents(argv[i+1],plannerSettings)) { printf("Unable to load planner settings file %s\n",argv[i+1]); return 1; } i++; } else if(0==strcmp(argv[i],"-r")) { robot = atoi(argv[i+1]); i++; } else if(0==strcmp(argv[i],"-m")) { gSupportPolygonMargin = atof(argv[i+1]); i++; } else if(0==strcmp(argv[i],"-o")) { outputfile = argv[i+1]; i++; } else { printf("Invalid option %s\n",argv[i]); return 1; } } else break; } if(i+3 < argc) { printf("USAGE: ContactPlan [options] world_file configs stance\n"); return 1; } if(i+3 > argc) { printf("Warning: extra arguments provided\n"); } const char* worldfile = argv[i]; const char* configsfile = argv[i+1]; const char* stancefile = argv[i+2]; //Read in the world file XmlWorld xmlWorld; RobotWorld world; if(!xmlWorld.Load(worldfile)) { printf("Error loading world XML file %s\n",worldfile); return 1; } if(!xmlWorld.GetWorld(world)) { printf("Error loading world file %s\n",worldfile); return 1; } vector<Config> configs; { //Read in the configurations specified in configsfile ifstream in(configsfile); if(!in) { printf("Error opening configs file %s\n",configsfile); return false; } while(in) { Config temp; in >> temp; if(in) configs.push_back(temp); } if(configs.size() < 2) { printf("Configs file does not contain 2 or more configs\n"); return 1; } in.close(); } Stance stance; { //read in the stance specified by stancefile ifstream in(stancefile,ios::in); in >> stance; if(!in) { printf("Error loading stance file %s\n",stancefile); return 1; } in.close(); } //If the stance has no contacts, use ContactPlan. Otherwise, use StancePlan bool ignoreContactForces = false; if(NumContactPoints(stance)==0) { printf("No contact points in stance, planning without stability constraints\n"); ignoreContactForces = true; } //set up the command line, store it into the MultiPath settings string cmdline; cmdline = argv[0]; for(int i=1;i<argc;i++) { cmdline += " "; cmdline += argv[i]; } MultiPath path; path.settings["robot"] = world.robots[robot].name; path.settings["command"] = cmdline; //begin planning bool feasible = true; Config qstart = world.robots[robot].robot->q; for(size_t i=0;i+1<configs.size();i++) { MilestonePath mpath; bool res = false; if(ignoreContactForces) res = ContactPlan(world,robot,configs[i],configs[i+1],stance,mpath,termCond,plannerSettings); else res = StancePlan(world,robot,configs[i],configs[i+1],stance,mpath,termCond,plannerSettings); if(!res) { printf("Planning from stance %d to %d failed\n",i,i+1); path.sections.resize(path.sections.size()+1); path.SetStance(stance,path.sections.size()-1); path.sections.back().milestones[0] = configs[i]; path.sections.back().milestones[1] = configs[i+1]; break; } else { path.sections.resize(path.sections.size()+1); path.sections.back().milestones.resize(mpath.NumMilestones()); path.SetStance(stance,path.sections.size()-1); for(int j=0;j<mpath.NumMilestones();j++) path.sections.back().milestones[j] = mpath.GetMilestone(j); qstart = path.sections.back().milestones.back(); } } if(feasible) printf("Path planning success! Saving to %s\n",outputfile); else printf("Path planning failure. Saving placeholder path to %s\n",outputfile); const char* ext = FileExtension(outputfile); if(ext && 0==strcmp(ext,"path")) { printf("Converted to linear path format\n"); LinearPath lpath; Convert(path,lpath); ofstream f(outputfile,ios::out); lpath.Save(f); f.close(); } else path.Save(outputfile); return 0; }
void ContactOptimizeTest2(const char* robfile,const char* config1,const char* config2) { Robot robot; if(!robot.Load(robfile)) { printf("Unable to load robot file %s\n",robfile); return; } Vector a,b; ifstream ia(config1,ios::in); ifstream ib(config2,ios::in); ia >> a; ib >> b; if(!ia || !ib) { printf("Unable to load config file(s)\n"); return; } ia.close(); ib.close(); printf("Automatically detecting contacts...\n"); robot.UpdateConfig(a); ContactFormation formation; GetFlatContacts(robot,5e-3,formation); printf("Assuming friction 0.5\n"); for(size_t i=0;i<formation.contacts.size();i++) { printf("%d contacts on link %d\n",formation.contacts[i].size(),formation.links[i]); for(size_t j=0;j<formation.contacts[i].size();j++) formation.contacts[i][j].kFriction = 0.5; } Stance stance; LocalContactsToStance(formation,robot,stance); MultiPath path; path.sections.resize(1); MultiPath::PathSection& section = path.sections[0]; path.SetStance(stance,0); vector<IKGoal> ikGoals; path.GetIKProblem(ikGoals); RobotConstrainedInterpolator interp(robot,ikGoals); //Real xtol = 5e-2; Real xtol = 1e-1; interp.ftol = 1e-6; interp.xtol = xtol; if(!interp.Project(a)) { printf("Failed to project config a\n"); return; } if(!interp.Project(b)) { printf("Failed to project config b\n"); return; } cout<<"Start: "<<a<<endl; cout<<"Goal: "<<b<<endl; vector<Vector> milestones,milestones2; if(!interp.Make(a,b,milestones)) { printf("Failed to interpolate\n"); return; } if(!interp.Make(b,a,milestones2)) { printf("Failed to interpolate\n"); return; } milestones.insert(milestones.end(),++milestones2.begin(),milestones2.end()); //milestones2 = milestones; //milestones.insert(milestones.end(),++milestones2.begin(),milestones2.end()); { cout<<"Saving geometric path to temp.path"<<endl; ofstream out("temp.path",ios::out); for(size_t i=0;i<milestones.size();i++) out<<Real(i)/Real(milestones.size()-1)<<" "<<milestones[i]<<endl; out.close(); } section.milestones = milestones; vector<Real> divs(101); for(size_t i=0;i<divs.size();i++) divs[i] = Real(i)/(divs.size()-1); Timer timer; ContactTimeScaling scaling(robot); scaling.frictionRobustness = 0.25; scaling.torqueRobustness = 0.25; scaling.forceRobustness = 0.05; bool res=scaling.SetParams(path,divs); if(!res) { printf("Unable to set contact scaling, time %g\n",timer.ElapsedTime()); printf("Saving to scaling_constraints.csv\n"); ofstream outc("scaling_constraints.csv",ios::out); outc<<"collocation point,planeindex,normal x,normal y,offset"<<endl; for(size_t i=0;i<scaling.ds2ddsConstraintNormals.size();i++) for(size_t j=0;j<scaling.ds2ddsConstraintNormals[i].size();j++) outc<<i<<","<<j<<","<<scaling.ds2ddsConstraintNormals[i][j].x<<","<<scaling.ds2ddsConstraintNormals[i][j].y<<","<<scaling.ds2ddsConstraintOffsets[i][j]<<endl; return; } printf("Contact scaling init successful, time %g\n",timer.ElapsedTime()); printf("Saving to scaling_constraints.csv\n"); ofstream outc("scaling_constraints.csv",ios::out); outc<<"collocation point,planeindex,normal x,normal y,offset"<<endl; for(size_t i=0;i<scaling.ds2ddsConstraintNormals.size();i++) for(size_t j=0;j<scaling.ds2ddsConstraintNormals[i].size();j++) outc<<i<<","<<j<<","<<scaling.ds2ddsConstraintNormals[i][j].x<<","<<scaling.ds2ddsConstraintNormals[i][j].y<<","<<scaling.ds2ddsConstraintOffsets[i][j]<<endl; res=scaling.Optimize(); if(!res) { printf("Time scaling failed in time %g. Path may be dynamically infeasible.\n",timer.ElapsedTime()); return; } printf("Time scaling solved in time %g, execution time %g\n",timer.ElapsedTime(),scaling.traj.timeScaling.times.back()); scaling.Check(path); /* for(size_t i=0;i<scaling.traj.ds.size();i++) { printf("time %g: rate %g\n",scaling.traj.times[i],scaling.ds[i]); } */ { cout<<"Saving dynamically optimized path to temp_opt.path"<<endl; ofstream out("temp_opt.path",ios::out); Real dt = scaling.traj.EndTime()/100; Real t=0; for(size_t i=0;i<=100;i++) { Vector x; scaling.traj.Eval(t,x); out<<t<<"\t"<<x<<endl; t += dt; } out.close(); } }
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); } }