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;
}
ConvergenceResult ConstrainedMinimizationProblem::StepTR(Real R)
{
grad.resize(x.n);
fx=(*f)(x);
f->Gradient(x,grad);
int cn = C->NumDimensions();
int dn = D->NumDimensions();
Vector cx(cn),dx(dn);
(*C)(x,cx);
(*D)(x,dx);
cout<<"******************************************************"<<endl;
cout<<"ConstrainedMinimization::StepTR(): objective "<<fx<<", initial distance "<<cx.maxAbsElement()<<", margin "<<dx.minElement()<<endl;
//solving the linearized version of the problem for a displacement
//y such that C(x+y) ~= C(x)+J(x)*y = 0
//etc
if(!sparse) {
LinearProgram lp;
lp.minimize=true;
lp.Resize(cn+dn,x.n); //reserve enough space in A
Assert(lp.c.n == grad.n);
lp.c = grad;
Matrix Ac,Ad;
Vector bc,bd;
Ac.setRef(lp.A,0,0,1,1,cn,x.n);
Ad.setRef(lp.A,cn,0,1,1,dn,x.n);
C->Jacobian(x,Ac);
D->Jacobian(x,Ad);
bc.setRef(lp.p,0,1,cn);
bc.setNegative(cx);
bc.setRef(lp.q,0,1,cn);
bc.setNegative(cx);
bd.setRef(lp.q,cn,1,dn);
bd.setNegative(dx);
lp.l.set(-R);
lp.u.set(R);
if(bmin.n != 0) {
for(int i=0;i<x.n;i++) {
if(lp.l(i)+x(i) < bmin(i)) lp.l(i) = bmin(i)-x(i);
if(lp.u(i)+x(i) > bmax(i)) lp.u(i) = bmax(i)-x(i);
}
}
RobustLPSolver lps;
LinearProgram::Result res=lps.Solve(lp);
if(res == LinearProgram::Infeasible) {
cout<<"ConstrainedMinimization::StepTR(): trust region radius "<<R<<" too small, doing a feasibility solve step"<<endl;
//NOTE: if MaxItersReached is returned, this will return ConvergenceError
ConvergenceResult r;
SolveFeasiblePoint(x,1,&r);
if(r == MaxItersReached || r == ConvergenceF)
//if(!SolveFeasiblePoint(x,1)) return ConvergenceError;
return LocalMinimum;
else if(r == ConvergenceX)
return ConvergenceX;
else
return ConvergenceError;
}
else if(res == LinearProgram::Unbounded) {
cout<<"ConstrainedMinimization::StepTR(): uh... can't be unbounded!"<<endl;
Abort();
}
else if(res == LinearProgram::Feasible) {
//arbitrary merit function...
Real fScale=tolf/tolc;
Real origMerit = Merit(fx,cx,dx,fScale);
Real alpha = One;
Real stepNorm = lps.xopt.norm();
Vector xold=x;
int numSteps=0;
while(alpha > 1e-3) {
x=xold; x.madd(lps.xopt,alpha);
(*C)(x,cx);
(*D)(x,dx);
fx = (*f)(x);
Real newMerit = Merit(fx,cx,dx,fScale);
cout<<"ConstrainedMinimization::StepTR(): step length "<<stepNorm*alpha<<endl;
cout<<"ConstrainedMinimization::StepTR(): objective "<<fx<<", distance "<<cx.maxAbsElement()<<", margin "<<dx.minElement()<<endl;
if(newMerit < origMerit) {
if(numSteps == 0) {
if(FuzzyEquals(newMerit,origMerit,tolf)) return ConvergenceF;
if(stepNorm < tolx) return ConvergenceX;
return MaxItersReached;
}
else {
cout<<"ConstrainedMinimization::StepTR(): trust region radius "<<R<<" too large"<<endl;
cout<<"Suggest radius "<<R*alpha<<endl;
return Divergence;
}
}
numSteps++;
alpha *= 0.5;
}
cout<<"ConstrainedMinimization::StepTR(): trust region radius "<<R<<" too large"<<endl;
x = xold;
return Divergence;
}
else { //error
cout<<"ConstrainedMinimization::StepTR(): error solving linear program!"<<endl;
return ConvergenceError;
}
return MaxItersReached;
}
else { //identical, but sparse
LinearProgram_Sparse lp;
lp.minimize=true;
lp.Resize(cn+dn,x.n); //reserve enough space in A
Assert(lp.c.n == grad.n);
lp.c = grad;
SparseMatrix Ac(cn,x.n),Ad(dn,x.n);
Vector bc,bd;
SparseVectorFunction* sC,*sD;
//Note: dangerous cast here!
try {
sC=dynamic_cast<SparseVectorFunction*>(C);
sD=dynamic_cast<SparseVectorFunction*>(D);
}
catch(exception& e) {
FatalError("Could not cast C or D to sparse functions! exception: %s",e.what());
}
cout<<"Evaluating equality jacobian..."; cout.flush();
sC->Jacobian_Sparse(x,Ac);
cout<<Ac.numNonZeros()<<" nonzeros"<<endl;
cout<<"Evaluating inequality jacobian..."; cout.flush();
sD->Jacobian_Sparse(x,Ad);
cout<<Ad.numNonZeros()<<" nonzeros"<<endl;
lp.A.copySubMatrix(0,0,Ac);
lp.A.copySubMatrix(cn,0,Ad);
bc.setRef(lp.p,0,1,cn);
bc.setNegative(cx);
bc.setRef(lp.q,0,1,cn);
bc.setNegative(cx);
bd.setRef(lp.q,cn,1,dn);
bd.setNegative(dx);
lp.l.set(-R);
lp.u.set(R);
if(bmin.n != 0) {
for(int i=0;i<x.n;i++) {
if(lp.l(i)+x(i) < bmin(i)) lp.l(i) = bmin(i)-x(i);
if(lp.u(i)+x(i) > bmax(i)) lp.u(i) = bmax(i)-x(i);
}
}
RobustLPSolver lps;
LinearProgram::Result res=lps.Solve(lp);
if(res == LinearProgram::Infeasible) {
cout<<"Linear program is infeasible!"<<endl;
/*
lp.l.set(-Inf);
lp.u.set(Inf);
LinearProgram::Result res2=lps.Solve(lp);
if(res2 == LinearProgram::Infeasible) {
cout<<"Seems to be infeasible over entire space!"<<endl;
getchar();
//deactivate inequality constraints
for(int i=0;i<dn;i++) {
lp.q(cn+i)=-Inf;
lp.p(cn+i)=Inf;
}
res2 = lps.Solve(lp);
if(res2 == LinearProgram::Infeasible) {
cout<<"equalities are infeasible!"<<endl;
//cout<<"Equalities: "<<cn<<", variables "<<x.n<<endl;
//lp.Print(cout);
getchar();
cout<<"Trying LSQR of equalities..."<<endl;
LSQRInterface lsqr;
lsqr.verbose = 0;
if(lsqr.Solve(Ac,cx)) {
lsqr.x.inplaceNegative();
cout<<"LSQR solved the problem with residual "<<lsqr.residualNorm<<endl;
}
else
cout<<"LSQR failed with residual "<<lsqr.residualNorm<<endl;
return ConvergenceError;
}
else {
}
}
*/
/*
lp.A.eraseZeros();
LinearProgram_Sparse lptemp;
lptemp = lp;
lptemp.A.clear();
lptemp.p.clear();
lptemp.q.clear();
for(int i=1;i<lp.A.m;i++) {
lptemp.A.resize(i,lp.A.n);
lptemp.p.resize(i);
lptemp.q.resize(i);
for(int j=0;j<i;j++) {
lptemp.A.rows[j] = lp.A.rows[j];
lptemp.p(j) = lp.p(j);
lptemp.q(j) = lp.q(j);
}
if(lps.Solve(lptemp) == LinearProgram::Infeasible) {
cout<<"Constraint "<<i<<" caused the LP to become infeasible!"<<endl;
SparseVector tmp;
tmp.set(lptemp.A.rows[i-1]);
cout<<lptemp.q(i-1)<<" < "; tmp.print(cout); cout<<" < "<<lptemp.p(i-1)<<endl;
getchar();
break;
}
}
*/
cout<<"ConstrainedMinimization::StepTR(): trust region radius "<<R<<" too small, doing a feasibility solve step"<<endl;
rootSolver.tolf = tolc;
rootSolver.tolmin = tolx;
rootSolver.tolx = tolx;
rootSolver.bmin.setRef(bmin);
rootSolver.bmax.setRef(bmax);
rootSolver.verbose = verbose;
rootSolver.x.setRef(x);
rootSolver.sparse = sparse;
cout<<"ConstrainedMinimizationProblem::SolveFeasiblePoint..."<<endl;
int iters=1;
ConvergenceResult r=rootSolver.SolveConstrained_SLP(iters);
if(r == ConvergenceError) {
cout<<"ConstrainedMinimization::StepTR(): feasibility could not be solved!"<<endl;
return ConvergenceError;
}
else if(r == ConvergenceX) {
cout<<"ConstrainedMinimization::StepTR(): feasibility could not be solved, x tolerance has been reached"<<endl;
return ConvergenceX;
}
/*
if(!SolveFeasiblePoint(x,1)) {
cout<<"ConstrainedMinimization::StepTR(): feasibility could not be solved!"<<endl;
return ConvergenceError;
}
*/
(*C)(x,cx);
(*D)(x,dx);
fx = (*f)(x);
cout<<"ConstrainedMinimization::StepTR(): final objective "<<fx<<", distance "<<cx.maxAbsElement()<<", margin "<<dx.minElement()<<endl;
/*
//shortcut to SolveFeasiblePoint() -- just solve the least squares problem
for(int i=0;i<dn;i++)
if(dx(i) > Zero) { //remove from lp.A, lp.b
lp.A.rows[i+cn].entries.clear();
lp.q(i+cn)=Zero;
}
bool res=rootSolver.SolveUnderconstrainedLS(lp.A,lp.q,lps.xopt);
if(res) {
Real merit = Merit(Zero,cx,dx);
Real alpha = One;
Vector x0=x;
while(alpha > 1e-4) {
x = x0; x.madd(lps.xopt,alpha);
(*C)(x,cx);
(*D)(x,dx);
fx = (*f)(x);
Real newMerit = Merit(Zero,cx,dx);
if(newMerit < merit-1e-4) //sufficient decrease
break;
alpha *= 0.5;
}
if(alpha <= 1e-4) {
cout<<"ConstrainedMinimization::StepTR(): backtracking failed!"<<endl;
x=x0;
}
else {
cout<<"ConstrainedMinimization::StepTR(): final objective "<<fx<<", distance "<<cx.maxAbsElement()<<", margin "<<dx.minElement()<<endl;
}
}
*/
return LocalMinimum;
}
else if(res == LinearProgram::Unbounded) {
cout<<"ConstrainedMinimization::StepTR(): uh... can't be unbounded!"<<endl;
Abort();
}
else if(res == LinearProgram::Feasible) {
//arbitrary merit function...
Real fScale=tolf/tolc;
Real origMerit = Merit(fx,cx,dx,fScale);
cout<<"ConstrainedMinimization::StepTR():"<<endl;
cout<<" Original merit "<<origMerit<<endl;
Real alpha = One;
Real stepNorm = lps.xopt.norm();
Vector xold=x;
int numSteps=0;
while(alpha > 1e-3) {
x=xold; x.madd(lps.xopt,alpha);
(*C)(x,cx);
(*D)(x,dx);
fx = (*f)(x);
Real newMerit = Merit(fx,cx,dx,fScale);
cout<<" Step length "<<stepNorm*alpha<<", merit "<<newMerit<<endl;
cout<<" Objective "<<fx<<", distance "<<cx.maxAbsElement()<<", margin "<<dx.minElement()<<endl;
if(newMerit < origMerit) {
if(numSteps == 0) {
if(FuzzyEquals(newMerit,origMerit,tolf)) return ConvergenceF;
if(stepNorm < tolx) return ConvergenceX;
return MaxItersReached;
}
else {
cout<<"ConstrainedMinimization::StepTR(): trust region radius "<<R<<" too large"<<endl;
cout<<"Suggest radius "<<R*alpha<<endl;
return Divergence;
}
}
numSteps++;
alpha *= 0.5;
}
cout<<"ConstrainedMinimization::StepTR(): trust region radius "<<R<<" too large"<<endl;
x = xold;
return Divergence;
}
else { //error
cout<<"ConstrainedMinimization::StepTR(): error solving linear program!"<<endl;
return ConvergenceError;
}
return MaxItersReached;
}
}