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