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

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

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