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

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