bool InterpolateConstrainedPath(Robot& robot,const vector<Config>& milestones,const vector<IKGoal>& ikGoals,vector<Config>& path,Real xtol) { if(ikGoals.empty()) { path = milestones; return true; } RobotSmoothConstrainedInterpolator interp(robot,ikGoals); interp.ftol = xtol*gConstraintToleranceScale; interp.xtol = xtol; GeneralizedCubicBezierSpline spline; if(!MultiSmoothInterpolate(interp,milestones,spline)) return false; path.resize(spline.segments.size()+1); path[0] = spline.segments[0].x0; for(size_t i=0;i<spline.segments.size();i++) path[i+1] = spline.segments[i].x3; return true; }
bool InterpolateAndTimeOptimize(const vector<Config>& configs, GeodesicManifold* space, VectorFieldFunction* constraint, Real constraintTolerance, const Vector& vmin,const Vector& vmax, const Vector& amin,const Vector& amax, TimeScaledBezierCurve& output) { if(constraint) { SmoothConstrainedInterpolator interp(space,constraint); interp.xtol = constraintTolerance; if(!MultiSmoothInterpolate(interp,configs,output.path)) return false; } else { GeneralizedCubicBezierSpline path; SplineInterpolate(configs,path.segments,space); path.durations.resize(path.segments.size()); for(size_t i=0;i+1<path.segments.size();i++) path.durations[i] = 1.0/path.durations.size(); //treat constraintTolerance as a resolution, discretize the curves into //n pieces int n=(int)Ceil(1.0/constraintTolerance); output.path.segments.resize(n); output.path.durations.resize(n); Config xp,vp,xn,vn; path.Eval(0,xp); path.Deriv(0,vp); for(int i=0;i<n;i++) { Real u2=Real(i+1)/Real(n); path.Eval(u2,xn); path.Deriv(u2,vn); output.path.segments[i].x0 = xp; output.path.segments[i].x3 = xn; output.path.segments[i].SetNaturalTangents(vp,vn); output.path.durations[i] = 1.0/Real(n); } swap(xp,xn); swap(vp,vn); } if(!output.OptimizeTimeScaling(vmin,vmax,amin,amax)) return false; return true; }
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; }