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