int RobotWorld::LoadRobot(const string& fn) { Robot* robot = new Robot; if(!robot->Load(fn.c_str())) { delete robot; return -1; } const char* justfn = GetFileName(fn.c_str()); char* buf = new char[strlen(justfn)+1]; strcpy(buf,justfn); StripExtension(buf); string name=buf; delete [] buf; int i = AddRobot(name,robot); return i; }
int main(int argc, char** argv) { if(argc < 2) { printf("USAGE: RobotTest robot_file\n"); return 0; } Robot robot; if(!robot.Load(argv[1])) { printf("Error loading robot file %s\n",argv[1]); return 1; } printf("Done loading robot file %s\n",argv[1]); JointTestProgram program(&robot,NULL); return program.Run(); }
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); } } }
void ContactOptimizeTest() { Robot robot; if(!robot.Load("data/simple_2d_biped.rob")) { printf("Unable to load data/simple_2d_biped.rob\n"); return; } MultiPath path; path.sections.resize(1); MultiPath::PathSection& section = path.sections[0]; section.holds.resize(2); section.holds[0].contacts.resize(2); section.holds[0].contacts[0].x.set(-0.4,-0.1,0); section.holds[0].contacts[1].x.set(-0.4,0.1,0); section.holds[0].contacts[0].n.set(0,0,1); section.holds[0].contacts[1].n.set(0,0,1); section.holds[0].contacts[0].kFriction = 0.5; section.holds[0].contacts[1].kFriction = 0.5; section.holds[1].contacts.resize(2); section.holds[1].contacts[0].x.set(0.4,-0.1,0); section.holds[1].contacts[1].x.set(0.4,0.1,0); section.holds[1].contacts[0].n.set(0,0,1); section.holds[1].contacts[1].n.set(0,0,1); section.holds[1].contacts[0].kFriction = 0.5; section.holds[1].contacts[1].kFriction = 0.5; section.holds[0].link = 7; section.holds[1].link = 9; section.holds[0].SetupIKConstraint(Vector3(0.5,-0.1,0),Vector3(Zero)); section.holds[1].SetupIKConstraint(Vector3(0.5,-0.1,0),Vector3(Zero)); vector<IKGoal> ikGoals; path.GetIKProblem(ikGoals); RobotConstrainedInterpolator interp(robot,ikGoals); Real xtol = 5e-2; interp.ftol = 1e-4; interp.xtol = xtol; Vector a(7),b(7); ifstream ia("simple_2d_biped/a.config",ios::in); ifstream ib("simple_2d_biped/b.config",ios::in); ia >> a; ib >> b; ia.close(); ib.close(); 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(401); 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.1; bool res=scaling.SetParams(path,divs); if(!res) { printf("Unable to set contact scaling, time %g\n",timer.ElapsedTime()); return; } printf("Contact scaling init successful, time %g\n",timer.ElapsedTime()); 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()); printf("\n"); printf("Checking path for feasibility...\n"); 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); int numdivs = 200; Real dt = scaling.traj.EndTime()/numdivs; Real t=0; for(size_t i=0;i<=numdivs;i++) { Vector x; scaling.traj.Eval(t,x); out<<t<<"\t"<<x<<endl; t += dt; } out.close(); } }
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); } }