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