示例#1
0
void SmoothDiscretizePath(Robot& robot,const vector<Config>& oldconfigs,int n,vector<Real>& times,vector<Config>& configs)
{
  times.resize(n);
  configs.resize(n);
  RobotCSpace space(robot);
  vector<GeneralizedCubicBezierCurve> curves;
  SPLINE_INTERPOLATE_FUNC(oldconfigs,curves,&space,&space);

  times[0] = 0;
  for(int i=1;i<n;i++) 
    times[i] = Real(i)/Real(n-1);
  configs[0] = oldconfigs[0];
  configs.back() = oldconfigs.back();
  Vector temp;
  for(int i=1;i+1<n;i++) {
    Real u0 = Floor(times[i]*curves.size());
    int segind = (int)u0;
    Real u=times[i]*curves.size()-u0;
    curves[segind].Eval(u,configs[i]);
  }
}
示例#2
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);
    Matrix J;
    f.PreEval(temp);
    f.Jacobian(temp,J);
    RobustSVD<Real> svd;
    if(!svd.set(J)) {
      fprintf(stderr,"Unable to set SVD of transition constraints %d\n",i);
      return false;
    }
    svd.nullspaceComponent(dtemp,dtemp2);
    dtemp -= dtemp2;
    f.activeDofs.Map(dtemp,transitionDerivs[i]);
  }

  //start constructing path
  paths.resize(path.sections.size());   
  for(size_t i=0;i<path.sections.size();i++) {
    paths[i].segments.resize(0);
    paths[i].durations.resize(0);

    Vector dxprev,dxnext;
    if(i>0) 
      dxprev.setRef(transitionDerivs[i-1]); 
    if(i<transitionDerivs.size()) 
      dxnext.setRef(transitionDerivs[i]); 
    if(stanceConstraints[i].empty()) {
      SPLINE_INTERPOLATE_FUNC(path.sections[i].milestones,paths[i].segments,&cspace,&manifold);
      DiscretizeSpline(paths[i],xtol);

      //Note: discretizeSpline will fill in the spline durations
    }
    else {
      RobotSmoothConstrainedInterpolator interp(robot,stanceConstraints[i]);
      interp.xtol = xtol;
      if(!MultiSmoothInterpolate(interp,path.sections[i].milestones,dxprev,dxnext,paths[i])) {
	/** TEMP - test no inter-section smoothing**/
	//if(!MultiSmoothInterpolate(interp,path.sections[i].milestones,paths[i])) {
	fprintf(stderr,"Unable to interpolate section %d\n",i);
	return false;
      }
    }
    //set the time scale if the input path is timed
    if(!path.sections[i].times.empty()) {
      //printf("Time scaling section %d to duration %g\n",i,path.sections[i].times.back()-path.sections[i].times.front());
      paths[i].TimeScale(path.sections[i].times.back()-path.sections[i].times.front());
    }
  }
  return true;
}