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

bool ContactTimeScaling::Check(const MultiPath& path)
{
  Robot& robot = cspace.robot;
  //test at the colocation points
  Assert(traj.timeScaling.times.size()==paramDivs.size());
  Vector x,dx,ddx;
  bool feasible = true;
  for(size_t i=0;i<paramDivs.size();i++) {
    int seg=paramSections[i];
    Real t=traj.timeScaling.times[i];
    traj.Eval(t,x);
    traj.Deriv(t,dx);
    traj.Accel(t,ddx);

    for(int j=0;j<dx.n;j++)
      if(Abs(dx[j]) > robot.velMax[j]*(1+Epsilon)) {
	printf("Vel at param %d (time %g/%g) is infeasible\n",i,t,traj.timeScaling.times.back());
	printf("   |%g| > %g  at link %s\n",dx[j],robot.velMax[j],robot.LinkName(j).c_str());
	feasible = false;
      }

    for(int j=0;j<ddx.n;j++)
      if(Abs(ddx[j]) > robot.accMax[j]*(1+Epsilon)) {
	printf("Acc at param %d (time %g/%g) is infeasible\n",i,t,traj.timeScaling.times.back());
	printf("   |%g| > %g  at link %s\n",ddx[j],robot.accMax[j],robot.LinkName(j).c_str());
	feasible = false;
      }


    ContactFormation formation;
    Stance stance;
    path.GetStance(stance,seg);
    ToContactFormation(stance,formation);
    if(formation.contacts.empty()) continue;

    robot.UpdateConfig(x);
    robot.dq = dx;
    TorqueSolver solver(robot,formation);
    solver.SetGravity(Vector3(0,0,-9.8));
    solver.SetDynamics(ddx);
    bool res=solver.Solve();
    if(!res) {
      printf("TorqueSolver was not able to compute a solution at param %d (time %g/%g)\n",i,t,traj.timeScaling.times.back());
      feasible = false;
      continue;
    }
    for(int j=0;j<solver.t.n;j++) {
      if(Abs(solver.t(j)) > robot.torqueMax(j)*(1+Epsilon)) {
	printf("Torque at param %d (time %g/%g) is infeasible\n",i,t,traj.timeScaling.times.back());
	printf("   |%g| > %g  at link %s\n",solver.t(j),robot.torqueMax(j),robot.LinkName(j).c_str());
	feasible = false;
      }
    }
  }
  return feasible;
}

bool ContactTimeScaling::SetParams(const MultiPath& path,const vector<Real>& paramDivs,int numFCEdges)
{
  Robot& robot = cspace.robot;
  CustomTimeScaling::SetPath(path,paramDivs);
  CustomTimeScaling::SetDefaultBounds();
  CustomTimeScaling::SetStartStop();

  ContactFormation formation;
  int oldSection = -1;
  LinearProgram_Sparse lp;
  NewtonEulerSolver ne(robot);
  //kinetic energy, coriolis force, gravity
  Vector3 gravity(0,0,-9.8);
  Vector C,G;
  //coefficients of time scaling
  Vector a,b,c;
  bool feasible=true;
  for(size_t i=0;i<paramDivs.size();i++) {
    Assert(paramSections[i] >= 0 && paramSections[i] < (int)path.sections.size());
    if(paramSections[i] != oldSection) {
      //reconstruct LP for the contacts in this section
      Stance stance;
      path.GetStance(stance,paramSections[i]);
      ToContactFormation(stance,formation);
      for(size_t j=0;j<formation.contacts.size();j++)
	for(size_t k=0;k<formation.contacts[j].size();k++) {
	  Assert(formation.contacts[j][k].kFriction > 0);
	  Assert(frictionRobustness < 1.0);
	  formation.contacts[j][k].kFriction *= (1.0-frictionRobustness);
	}

      //now formulate the LP.  Variable 0 is dds, variable 1 is ds^2
      //rows 1-n are torque max
      //rows n+1 - 2n are acceleration max
      //rows 2n+1 + 2n+numFCEdges*nc are the force constraints
      //vel max is encoded in the velocity variable
      int n = (int)robot.links.size();
      int nc = formation.numContactPoints();
#if TEST_NO_CONTACT
      nc = 0;
#endif // TEST_NO_CONTACT
      lp.Resize(n*2+numFCEdges*nc,2+3*nc);
      lp.A.setZero();
      lp.c.setZero();
      //fill out wrench matrix FC*f <= 0
#if !TEST_NO_CONTACT
      SparseMatrix FC;
      GetFrictionConePlanes(formation,numFCEdges,FC);
      lp.A.copySubMatrix(n*2,2,FC);
      for(int j=0;j<FC.m;j++)
	lp.p(n*2+j) = -forceRobustness;
#endif // !TEST_NO_CONTACT

      lp.l(0) = 0.0;
      lp.l(1) = -Inf;

      oldSection = paramSections[i];
    }
    //configuration specific 
    robot.UpdateConfig(xs[i]);
    robot.dq = dxs[i];
    ne.CalcResidualTorques(C);
    robot.GetGravityTorques(gravity,G);
    ne.MulKineticEnergyMatrix(dxs[i],a);
    ne.MulKineticEnergyMatrix(ddxs[i],b);
    b += C;
    c = G;

    //|a dds + b ds^2 + c - Jtf| <= torquemax*scale+shift
    for(int j=0;j<a.n;j++) {
      lp.A(j,0) = b(j);
      lp.A(j,1) = a(j);
      Real tmax = robot.torqueMax(j)*torqueLimitScale+torqueLimitShift;
      if(tmax < 0) tmax=0;
      lp.p(j) = tmax-c(j);
      lp.q(j) = -tmax-c(j);
    }
#if TEST_NO_CONTACT
    lp.p.set(Inf);
    lp.q.set(-Inf);
#else
    //fill out jacobian transposes
    int ccount=0;
    for(size_t l=0;l<formation.links.size();l++) {
      int link = formation.links[l];
      int target = (formation.targets.empty() ? -1 : formation.targets[l]);
      for(size_t j=0;j<formation.contacts[l].size();j++,ccount++) {
	Vector3 p=formation.contacts[l][j].x;
	//if it's a self-contact, then transform to world
	if(target >= 0)
	  p = robot.links[target].T_World*p;
	Vector3 v;
	int k=link;
	while(k!=-1) {
	  robot.links[k].GetPositionJacobian(robot.q[k],p,v);
	  if(v.x != 0.0) lp.A(k,2+ccount*3)=-v.x;
	  if(v.y != 0.0) lp.A(k,2+ccount*3+1)=-v.y;
	  if(v.z != 0.0) lp.A(k,2+ccount*3+2)=-v.z;
	  k=robot.parents[k];
	}
//.........这里部分代码省略.........

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