C++ MultiPath::Save方法代码示例

//.........这里部分代码省略.........
    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);
//.........这里部分代码省略.........

/** @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;
  }
//.........这里部分代码省略.........