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);
}
}
}
void ContactOptimizeTest()
{
Robot robot;
if(!robot.Load("data/simple_2d_biped.rob")) {
printf("Unable to load data/simple_2d_biped.rob\n");
return;
}
MultiPath path;
path.sections.resize(1);
MultiPath::PathSection& section = path.sections[0];
section.holds.resize(2);
section.holds[0].contacts.resize(2);
section.holds[0].contacts[0].x.set(-0.4,-0.1,0);
section.holds[0].contacts[1].x.set(-0.4,0.1,0);
section.holds[0].contacts[0].n.set(0,0,1);
section.holds[0].contacts[1].n.set(0,0,1);
section.holds[0].contacts[0].kFriction = 0.5;
section.holds[0].contacts[1].kFriction = 0.5;
section.holds[1].contacts.resize(2);
section.holds[1].contacts[0].x.set(0.4,-0.1,0);
section.holds[1].contacts[1].x.set(0.4,0.1,0);
section.holds[1].contacts[0].n.set(0,0,1);
section.holds[1].contacts[1].n.set(0,0,1);
section.holds[1].contacts[0].kFriction = 0.5;
section.holds[1].contacts[1].kFriction = 0.5;
section.holds[0].link = 7;
section.holds[1].link = 9;
section.holds[0].SetupIKConstraint(Vector3(0.5,-0.1,0),Vector3(Zero));
section.holds[1].SetupIKConstraint(Vector3(0.5,-0.1,0),Vector3(Zero));
vector<IKGoal> ikGoals;
path.GetIKProblem(ikGoals);
RobotConstrainedInterpolator interp(robot,ikGoals);
Real xtol = 5e-2;
interp.ftol = 1e-4;
interp.xtol = xtol;
Vector a(7),b(7);
ifstream ia("simple_2d_biped/a.config",ios::in);
ifstream ib("simple_2d_biped/b.config",ios::in);
ia >> a;
ib >> b;
ia.close();
ib.close();
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(401);
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.1;
bool res=scaling.SetParams(path,divs);
if(!res) {
printf("Unable to set contact scaling, time %g\n",timer.ElapsedTime());
return;
}
printf("Contact scaling init successful, time %g\n",timer.ElapsedTime());
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;
res = scaling.Optimize();
if(!res) {
printf("Time scaling failed in time %g. Path may be dynamically infeasible.\n",timer.ElapsedTime());
return;
}
printf("Time scaling solved in time %g, execution time %g\n",timer.ElapsedTime(),scaling.traj.timeScaling.times.back());
printf("\n");
printf("Checking path for feasibility...\n");
scaling.Check(path);
/*
for(size_t i=0;i<scaling.traj.ds.size();i++) {
printf("time %g: rate %g\n",scaling.traj.times[i],scaling.ds[i]);
}
*/
{
cout<<"Saving dynamically optimized path to temp_opt.path"<<endl;
ofstream out("temp_opt.path",ios::out);
int numdivs = 200;
Real dt = scaling.traj.EndTime()/numdivs;
Real t=0;
for(size_t i=0;i<=numdivs;i++) {
Vector x;
scaling.traj.Eval(t,x);
out<<t<<"\t"<<x<<endl;
t += dt;
}
out.close();
}
}
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;
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;
res=scaling.Optimize();
if(!res) {
printf("Time scaling failed in time %g. Path may be dynamically infeasible.\n",timer.ElapsedTime());
return;
}
printf("Time scaling solved in time %g, execution time %g\n",timer.ElapsedTime(),scaling.traj.timeScaling.times.back());
scaling.Check(path);
/*
for(size_t i=0;i<scaling.traj.ds.size();i++) {
printf("time %g: rate %g\n",scaling.traj.times[i],scaling.ds[i]);
}
*/
{
cout<<"Saving dynamically optimized path to temp_opt.path"<<endl;
ofstream out("temp_opt.path",ios::out);
Real dt = scaling.traj.EndTime()/100;
Real t=0;
for(size_t i=0;i<=100;i++) {
Vector x;
scaling.traj.Eval(t,x);
out<<t<<"\t"<<x<<endl;
t += dt;
}
out.close();
}
}
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;
}
