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;
}
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];
}
k = target;
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];
}
}
}
Assert(ccount == formation.numContactPoints());
#endif //TEST_NO_CONTACT
//fill out acceleration constraint |ddx*ds^2 + dx*dds| <= amax
for(int j=0;j<a.n;j++) {
lp.q(a.n+j) = -robot.accMax(j);
lp.p(a.n+j) = robot.accMax(j);
lp.A(a.n+j,0) = ddxs[i][j];
lp.A(a.n+j,1) = dxs[i][j];
}
//compute upper bounds from vel and acc max
lp.u(0) = Inf; lp.u(1) = Inf;
for(int j=0;j<a.n;j++) {
if(dxs[i][j] < 0)
lp.u(0) = Min(lp.u(0),Sqr(robot.velMin(j)/dxs[i][j]));
else
lp.u(0) = Min(lp.u(0),Sqr(robot.velMax(j)/dxs[i][j]));
}
//expand polytope
Geometry::PolytopeProjection2D proj(lp);
Geometry::UnboundedPolytope2D poly;
proj.Solve(poly);
if(poly.vertices.empty()) {
//problem is infeasible?
printf("Problem is infeasible at segment %d\n",i);
cout<<"x = "<<xs[i]<<endl;
cout<<"dx = "<<dxs[i]<<endl;
cout<<"ddx = "<<ddxs[i]<<endl;
lp.Print(cout);
getchar();
feasible=false;
}
/*
if(i == 49) {
cout<<"x = "<<xs[i]<<endl;
cout<<"dx = "<<dxs[i]<<endl;
cout<<"ddx = "<<ddxs[i]<<endl;
lp.Print(cout);
cout<<"Result: "<<endl;
for(size_t j=0;j<poly.planes.size();j++)
cout<<poly.planes[j].normal.x<<" ds^2 + "<<poly.planes[j].normal.y<<" dds <= "<<poly.planes[j].offset<<endl;
cout<<"Vertices: "<<endl;
for(size_t j=0;j<poly.vertices.size();j++) {
cout<<poly.vertices[j]<<endl;
}
getchar();
}
*/
ds2ddsConstraintNormals[i].resize(poly.planes.size());
ds2ddsConstraintOffsets[i].resize(poly.planes.size());
for(size_t j=0;j<poly.planes.size();j++) {
ds2ddsConstraintNormals[i][j] = poly.planes[j].normal;
ds2ddsConstraintOffsets[i][j] = poly.planes[j].offset;
if(saveConstraintNames) {
stringstream ss;
ss<<"projected_constraint_plane_"<<j;
ds2ddsConstraintNames[i].push_back(ss.str());
}
}
}
//done!
return feasible;
}
void ContactOptimizeMultipath(const char* robfile,const char* pathfile,const char* settingsfile = NULL)
{
ContactOptimizeSettings settings;
if(settingsfile != NULL) {
if(!settings.read(settingsfile)) {
printf("Unable to read settings file %s\n",settingsfile);
return;
}
}
Real xtol=Real(settings["xtol"]);
int numdivs = int(settings["numdivs"]);
bool ignoreForces = bool(settings["ignoreForces"]);
Real torqueRobustness = Real(settings["torqueRobustness"]);
Real frictionRobustness = Real(settings["frictionRobustness"]);
Real forceRobustness = Real(settings["forceRobustness"]);
string outputPath;
settings["outputPath"].as(outputPath);
Real outputDt = Real(settings["outputDt"]);
Robot robot;
if(!robot.Load(robfile)) {
printf("Unable to load robot file %s\n",robfile);
return;
}
MultiPath path;
if(!path.Load(pathfile)) {
printf("Unable to load path file %s\n",pathfile);
return;
}
//double check friction
for(size_t i=0;i<path.sections.size();i++) {
Stance s;
path.GetStance(s,i);
bool changed = false;
int k=0;
int numContacts = 0;
for(Stance::iterator h=s.begin();h!=s.end();h++,k++) {
numContacts += (int)h->second.contacts.size();
for(size_t j=0;j<h->second.contacts.size();j++)
if(h->second.contacts[j].kFriction <= 0) {
if(!changed)
printf("Warning, contact %d of hold %d has invalid friction %g, setting to 0.5\n",j,k,h->second.contacts[j].kFriction);
h->second.contacts[j].kFriction = 0.5;
changed = true;
}
}
path.SetStance(s,i);
if(numContacts == 0 && !ignoreForces && robot.joints[0].type == RobotJoint::Floating) {
printf("Warning, no contacts given in stance %d for floating-base robot\n",i);
printf("Should set ignoreForces = true in trajopt.settings if you wish\n");
printf("to ignore contact force constraints.\n");
printf("Press enter to continue...\n");
getchar();
}
}
TimeScaledBezierCurve opttraj;
if(ignoreForces) {
bool res=GenerateAndTimeOptimizeMultiPath(robot,path,xtol,outputDt);
if(!res) {
printf("Time optimization failed\n");
return;
}
Assert(path.HasTiming());
cout<<"Saving dynamically optimized path to "<<outputPath<<endl;
ofstream out(outputPath.c_str(),ios::out);
for(size_t i=0;i<path.sections.size();i++) {
for(size_t j=0;j<path.sections[i].milestones.size();j++)
out<<path.sections[i].times[j]<<"\t"<<path.sections[i].milestones[j]<<endl;
}
out.close();
}
else {
bool res=ContactOptimizeMultipath(robot,path,xtol,
numdivs,opttraj,
torqueRobustness,
frictionRobustness,
forceRobustness);
if(!res) {
printf("Time optimization failed\n");
return;
}
RobotCSpace cspace(robot);
for(size_t i=0;i<opttraj.path.segments.size();i++) {
opttraj.path.segments[i].space = &cspace;
opttraj.path.segments[i].manifold = &cspace;
}
vector<Config> milestones;
opttraj.GetDiscretizedPath(outputDt,milestones);
cout<<"Saving dynamically optimized path to "<<outputPath<<endl;
ofstream out(outputPath.c_str(),ios::out);
for(size_t i=0;i<milestones.size();i++) {
out<<i*outputDt<<"\t"<<milestones[i]<<endl;
}
out.close();
printf("Plotting vel/acc constraints to trajopt_plot.csv...\n");
opttraj.Plot("trajopt_plot.csv",robot.velMin,robot.velMax,-1.0*robot.accMax,robot.accMax);
}
}
