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

void EvaluateMultiPath(Robot& robot,const MultiPath& path,Real t,Config& q,Real xtol,Real contactol,int numIKIters)
{
  RobotCSpace space(robot);;
  RobotGeodesicManifold manifold(robot);
  GeneralizedCubicBezierCurve curve(&space,&manifold);
  Real duration,param;
  int seg=path.Evaluate(t,curve,duration,param,MultiPath::InterpLinear);
  if(seg < 0) seg = 0;
  if(seg >= path.sections.size()) seg = (int)path.sections.size()-1;
  curve.Eval(param,q);

  //solve for constraints
  bool solveIK = false;
  if(!path.settings.contains("resolution"))
    solveIK = true;
  else {
    Real res = path.settings.as<Real>("resolution");
    if(res > xtol) solveIK=true;
  }
  if(solveIK) {
    vector<IKGoal> ik;
    path.GetIKProblem(ik,seg);
    if(!ik.empty()) {
      swap(q,robot.q);
      robot.UpdateFrames();

      int iters=numIKIters;
      bool res=SolveIK(robot,ik,contactol,iters,0);
      if(!res) printf("Warning, couldn't solve IK problem at sec %d, time %g\n",seg,t);
      swap(q,robot.q);
    }
  }
}

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

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

bool InterpolateConstrainedMultiPath(Robot& robot,const MultiPath& path,vector<GeneralizedCubicBezierSpline>& paths,Real xtol)
{
  //sanity check -- make sure it's a continuous path
  if(!path.IsContinuous()) {
    fprintf(stderr,"InterpolateConstrainedMultiPath: path is discontinuous\n");
    return false;
  }
  if(path.sections.empty()) {
    fprintf(stderr,"InterpolateConstrainedMultiPath: path is empty\n");
    return false;
  }

  if(path.settings.contains("resolution")) {
    //see if the resolution is high enough to just interpolate directly
    Real res=path.settings.as<Real>("resolution");
    if(res <= xtol) {
      printf("Direct interpolating trajectory with res %g\n",res);
      //just interpolate directly
      RobotCSpace space(robot);
      RobotGeodesicManifold manifold(robot);
      paths.resize(path.sections.size());
      for(size_t i=0;i<path.sections.size();i++) {
	if(path.sections[i].times.empty()) {
	  SPLINE_INTERPOLATE_FUNC(path.sections[i].milestones,paths[i].segments,
			       &space,&manifold);
	  //uniform timing
	  paths[i].durations.resize(paths[i].segments.size());
	  Real dt=1.0/Real(paths[i].segments.size());
	  for(size_t j=0;j<paths[i].segments.size();j++)
	    paths[i].durations[j] = dt;
	}
	else {
	  SPLINE_INTERPOLATE_FUNC(path.sections[i].milestones,path.sections[i].times,paths[i].segments,
			       &space,&manifold);
	  //get timing from path
	  paths[i].durations.resize(paths[i].segments.size());
	  for(size_t j=0;j<paths[i].segments.size();j++)
	    paths[i].durations[j] = path.sections[i].times[j+1]-path.sections[i].times[j];
	}
      }
      return true;
    }
  }
  printf("Discretizing constrained trajectory at res %g\n",xtol);

  RobotCSpace cspace(robot);
  RobotGeodesicManifold manifold(robot);

  //create transition constraints and derivatives
  vector<vector<IKGoal> > stanceConstraints(path.sections.size());
  vector<vector<IKGoal> > transitionConstraints(path.sections.size()-1);
  vector<Config> transitionDerivs(path.sections.size()-1);
  for(size_t i=0;i<path.sections.size();i++) 
    path.GetIKProblem(stanceConstraints[i],i);
  for(size_t i=0;i+1<path.sections.size();i++) {
    //put all nonredundant constraints into transitionConstraints[i]
    transitionConstraints[i]=stanceConstraints[i];
    for(size_t j=0;j<stanceConstraints[i+1].size();j++) {
      bool res=AddGoalNonredundant(stanceConstraints[i+1][j],transitionConstraints[i]);
      if(!res) {
	fprintf(stderr,"Conflict between goal %d of stance %d and stance %d\n",j,i+1,i);
	fprintf(stderr,"  Link %d\n",stanceConstraints[i+1][j].link);
	return false;
      }
    }

    const Config& prev=path.sections[i].milestones[path.sections[i].milestones.size()-2];
    const Config& next=path.sections[i+1].milestones[1];
    manifold.InterpolateDeriv(prev,next,0.5,transitionDerivs[i]);
    transitionDerivs[i] *= 0.5;

    //check for overshoots a la MonotonicInterpolate
    Vector inslope,outslope;
    manifold.InterpolateDeriv(prev,path.sections[i].milestones.back(),1.0,inslope);
    manifold.InterpolateDeriv(path.sections[i].milestones.back(),next,0.0,outslope);
    for(int j=0;j<transitionDerivs[i].n;j++) {
      if(Sign(transitionDerivs[i][j]) != Sign(inslope[j]) || Sign(transitionDerivs[i][j]) != Sign(outslope[j])) transitionDerivs[i][j] = 0;
      else {
	if(transitionDerivs[i][j] > 0) {
	  if(transitionDerivs[i][j] > 3.0*outslope[j])
	    transitionDerivs[i][j] = 3.0*outslope[j];
	  if(transitionDerivs[i][j] > 3.0*inslope[j])
	    transitionDerivs[i][j] = 3.0*inslope[j];
	}
	else {
	  if(transitionDerivs[i][j] < 3.0*outslope[j])
	    transitionDerivs[i][j] = 3.0*outslope[j];
	  if(transitionDerivs[i][j] < 3.0*inslope[j])
	    transitionDerivs[i][j] = 3.0*inslope[j];
	}
      }
    }

    //project "natural" derivative onto transition manifold
    RobotIKFunction f(robot);
    f.UseIK(transitionConstraints[i]);
    GetDefaultIKDofs(robot,transitionConstraints[i],f.activeDofs);
    Vector temp(f.activeDofs.Size()),dtemp(f.activeDofs.Size()),dtemp2;
    f.activeDofs.InvMap(path.sections[i].milestones.back(),temp);
    f.activeDofs.InvMap(transitionDerivs[i],dtemp);
//.........这里部分代码省略.........