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 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); } } }
/** @brief Completely interpolates, optimizes, and time-scales the * given MultiPath to satisfy its contact constraints. * * Arguments * - robot: the robot * - path: the MultiPath containing the milestones / stances of the motion * - interpTol: the tolerance that must be met for contact constraints for the * interpolated path. * - numdivs: the number of grid points used for time-scaling * - traj (out): the output timed trajectory. (Warning, the spaces and manifolds in * traj.path.segments will be bogus pointers.) * - torqueRobustness: a parameter in [0,1] determining the robustness margin * added to the torque constraint. The robot's torques are scaled by a factor * of (1-torqueRobustness). * - frictionRobustness: a parameter in [0,1] determinining the robustness margin * added to the friction constraint. The friction coefficients are scaled by * a factor of (1-frictionRobustness). Helps the path avoid slipping. * - forceRobustness: a minimum normal contact force that must be applied * *at each contact point* (in Newtons). Helps the trajectory avoid contact * separation. * - savePath: if true, saves the interpolated MultiPath to disk. * - saveConstraints: if true, saves the time scaling convex program constraints * to disk. * * Return value is true if interpolation / time scaling was successful. * Failure indicates that the milestones could not be interpolated, or * the path is not dynamically feasible. For example, the milestones or * interpolated path might violate stability constraints. */ bool ContactOptimizeMultipath(Robot& robot,const MultiPath& path, Real interpTol,int numdivs, TimeScaledBezierCurve& traj, Real torqueRobustness=0.0, Real frictionRobustness=0.0, Real forceRobustness=0.0, bool savePath = true, bool saveConstraints = false) { Assert(torqueRobustness < 1.0); Assert(frictionRobustness < 1.0); Timer timer; MultiPath ipath; bool res=DiscretizeConstrainedMultiPath(robot,path,ipath,interpTol); if(!res) { printf("Could not discretize path, failing\n"); return false; } int ns = 0; for(size_t i=0;i<ipath.sections.size();i++) ns += ipath.sections[i].milestones.size()-1; printf("Interpolated at resolution %g to %d segments, time %g\n",interpTol,ns,timer.ElapsedTime()); if(savePath) { cout<<"Saving interpolated geometric path to temp.xml"<<endl; ipath.Save("temp.xml"); } vector<Real> divs(numdivs); Real T = ipath.Duration(); for(size_t i=0;i<divs.size();i++) divs[i] = T*Real(i)/(divs.size()-1); timer.Reset(); ContactTimeScaling scaling(robot); //CustomTimeScaling scaling(robot); scaling.frictionRobustness = frictionRobustness; scaling.torqueLimitScale = 1.0-torqueRobustness; scaling.forceRobustness = forceRobustness; res=scaling.SetParams(ipath,divs); /* scaling.SetPath(ipath,divs); scaling.SetDefaultBounds(); scaling.SetStartStop(); bool res=true; */ if(!res) { printf("Unable to set contact scaling, time %g\n",timer.ElapsedTime()); if(saveConstraints) { 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 false; } printf("Contact scaling init successful, time %g\n",timer.ElapsedTime()); if(saveConstraints) { 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 false; } printf("Time scaling solved in time %g, execution time %g\n",timer.ElapsedTime(),scaling.traj.timeScaling.times.back()); //scaling.Check(ipath); traj = scaling.traj; return true; }