C++ MultiPath类代码示例

bool DiscretizeConstrainedMultiPath(Robot& robot,const MultiPath& path,MultiPath& out,Real xtol)
{
  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) {
      out = path;
      return true;
    }
  }

  vector<GeneralizedCubicBezierSpline> paths;
  if(!InterpolateConstrainedMultiPath(robot,path,paths,xtol))
    return false;

  out = path;
  {
    out.settings.set("resolution",xtol);
    out.settings.set("program","DiscretizeConstrainedMultiPath");
  }
  Real tofs = (path.HasTiming() ? path.sections[0].times.front() : 0);
  for(size_t i=0;i<out.sections.size();i++) {
    out.sections[i].velocities.resize(0);
    paths[i].GetPiecewiseLinear(out.sections[i].times,out.sections[i].milestones);
    //shift section timing
    Real tscale = (path.HasTiming() ? path.sections[i].times.back()-path.sections[i].times.front() : 1.0) / out.sections[i].times.back();
    for(size_t j=0;j<out.sections[i].times.size();j++)
      out.sections[i].times[j] = tofs + out.sections[i].times[j]*tscale;
    tofs = out.sections[i].times.back();
  }
  return true;
}

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);
    }
  }
}

    Real EuropeanMultiPathPricer::operator()(const MultiPath& multiPath)
                                                                      const {
        Size n = multiPath.pathSize();
        QL_REQUIRE(n>0, "the path cannot be empty");

        Size numAssets = multiPath.assetNumber();
        QL_REQUIRE(numAssets>0, "there must be some paths");

        Size j;
        // calculate the final price of each asset
        Array finalPrice(numAssets, 0.0);
        for (j = 0; j < numAssets; j++)
            finalPrice[j] = multiPath[j].back();
        return (*payoff_)(finalPrice) * discount_;
    }

Real EverestMultiPathPricer::operator()(const MultiPath& multiPath) const {

    Size n = multiPath.pathSize();
    QL_REQUIRE(n>0, "the path cannot be empty");

    Size numAssets = multiPath.assetNumber();
    QL_REQUIRE(numAssets>0, "there must be some paths");

    // We search the yield min
    Real minYield = multiPath[0].back() / multiPath[0].front() - 1.0;
    for (Size j=1; j<numAssets; ++j) {
        Rate yield = multiPath[j].back() / multiPath[j].front() - 1.0;
        minYield = std::min(minYield, yield);
    }
    return (1.0 + minYield + guarantee_) * notional_ * discount_;
}

    inline Real EuropeanHestonPathPricer::operator()(
                                           const MultiPath& multiPath) const {
        const Path& path = multiPath[0];
        const Size n = multiPath.pathSize();
        QL_REQUIRE(n>0, "the path cannot be empty");

        return payoff_(path.back()) * discount_;
    }

    Real PagodaMultiPathPricer::operator()(const MultiPath& multiPath) const {

        Size numAssets = multiPath.assetNumber();
        Size numSteps = multiPath.pathSize();

        Real averagePerformance = 0.0;
        for (Size i = 1; i < numSteps; i++) {
            for (Size j = 0; j < numAssets; j++) {
                averagePerformance +=
                    multiPath[j].front() *
                    (multiPath[j][i]/multiPath[j][i-1] - 1.0);
            }
        }
        averagePerformance /= numAssets;

        return discount_ * fraction_
            * std::max<Real>(0.0, std::min(roof_, averagePerformance));
    }

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;
}

    Array AmericanBasketPathPricer::state(const MultiPath& path,
                                          Size t) const {
        QL_REQUIRE(path.assetNumber() == assetNumber_, "invalid multipath");

        Array tmp(assetNumber_);
        for (Size i=0; i<assetNumber_; ++i) {
            tmp[i] = path[i][t]*scalingValue_;
        }

        return tmp;
    }

    /*
      Extract the relevant information from the whole path
     */
    LongstaffSchwartzMultiPathPricer::PathInfo 
    LongstaffSchwartzMultiPathPricer::transformPath(const MultiPath& multiPath)
    const {
        const Size numberOfAssets = multiPath.assetNumber();
        const Size numberOfTimes = timePositions_.size();

        Matrix path(numberOfAssets, numberOfTimes, Null<Real>());

        for (Size i = 0; i < numberOfTimes; ++i) {
            const Size pos = timePositions_[i];
            for (Size j = 0; j < numberOfAssets; ++j)
                path[j][i] = multiPath[j][pos];
        }
        
        PathInfo info(numberOfTimes);

        payoff_->value(path, forwardTermStructures_, info.payments, info.exercises, info.states);

        return info;
    }

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

/************************************************************************

  Expands the single multi path into all possible sequence variants.
  Since this turns out to be the time-limiting process for long de nvoo,
  this is implemented using a quick branch and bound that works on
  edge variant indices. The SeqPaths are created only for the final
  set of sequences.

*************************************************************************/
void PrmGraph::fast_expand_multi_path_for_combo_scores(
									  AllScoreModels *model,
									  const MultiPath& multi_path, 
									  vector<SeqPath>& seq_paths,
									  score_t min_score,
									  score_t forbidden_pair_penalty,
									  int max_num_paths)
{
	const vector<AA_combo>& aa_edge_comobos = config->get_aa_edge_combos();
	vector<score_t> max_attainable_scores;

	const int num_edges = multi_path.edge_idxs.size();
	max_attainable_scores.resize(num_edges+1,0);
	
	int num_path_variants=1;
	int e;
	for (e=num_edges-1; e<num_edges; e++)
	{
		const int e_idx = multi_path.edge_idxs[e];
		const MultiEdge& edge = multi_edges[e_idx];
		num_path_variants *= edge.variant_ptrs.size();
	}


	if (num_path_variants == 0)
	{
		cout << "Error: had an edge with 0 variants!" <<endl;
		exit(1);
	}

	// re-check the max attainable scores, this time compute all combo scores along the path
	// this will give a more accurate max attainable score, and also allow quick computation
	// of the expanded scores

	max_attainable_scores.clear();
	max_attainable_scores.resize(num_edges+1,0);
	vector<score_t> max_node_scores;
	max_node_scores.resize(num_edges+1,NEG_INF);

	for (e=0; e<=num_edges; e++)
	{
		const int n_edge_idx = (e>0 ? multi_path.edge_idxs[e-1] : -1);
		const int c_edge_idx = (e<num_edges ? multi_path.edge_idxs[e] : -1);
		const int node_idx = (n_edge_idx>=0 ? multi_edges[n_edge_idx].c_idx : multi_edges[c_edge_idx].n_idx);
		const int num_n_vars = (e>0 ? multi_edges[n_edge_idx].variant_ptrs.size() : 1);
		const int num_c_vars = (e<num_edges ? multi_edges[c_edge_idx].variant_ptrs.size() : 1);

		int j,k;
		for (j=0; j<num_n_vars; j++)
			for (k=0; k<num_c_vars; k++)
			{
				score_t combo_score = model->get_node_combo_score(this,node_idx,n_edge_idx,j,c_edge_idx,k);
				if (max_node_scores[e]<combo_score)
					max_node_scores[e]=combo_score;
			}
	}

	max_attainable_scores[num_edges]=max_node_scores[num_edges];
	for (e=num_edges-1; e>=0; e--)
	{
		const int e_idx = multi_path.edge_idxs[e];
		const MultiEdge& edge = multi_edges[e_idx];

		max_attainable_scores[e]= max_attainable_scores[e+1] + 
			edge.max_variant_score + max_node_scores[e]; 
	}
	score_t max_path_score =  max_attainable_scores[0] - multi_path.num_forbidden_nodes * forbidden_pair_penalty;
	score_t min_delta_allowed = min_score - max_path_score;

	if (min_delta_allowed>0)
		return;

	// in this expansion method, all edges are stored (not only ones with more than one variant)
	// perform expansion using a heap and condensed path representation
	vector<int> var_positions;				   // holds the edge idxs
	vector<int> num_vars;
	vector<int> var_edge_positions;
	vector< vector< score_t > > var_scores;
	var_scores.clear();
	var_positions.clear();
	num_vars.clear();

	int num_aas_in_multipath = 0;
	for (e=0; e<num_edges; e++)
	{
		const int e_idx = multi_path.edge_idxs[e];
		const MultiEdge& edge = multi_edges[e_idx];
		const int num_vars_in_edge = edge.variant_ptrs.size();
		vector<score_t> scores;

		num_aas_in_multipath+=edge.num_aa;
//.........这里部分代码省略.........

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

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

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

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