double ChLcpIterativePCG::Solve(
ChLcpSystemDescriptor& sysd ///< system description with constraints and variables
)
{
std::vector<ChLcpConstraint*>& mconstraints = sysd.GetConstraintsList();
std::vector<ChLcpVariables*>& mvariables = sysd.GetVariablesList();
tot_iterations = 0;
double maxviolation = 0.;
// Update auxiliary data in all constraints before starting,
// that is: g_i=[Cq_i]*[invM_i]*[Cq_i]' and [Eq_i]=[invM_i]*[Cq_i]'
for (unsigned int ic = 0; ic< mconstraints.size(); ic++)
mconstraints[ic]->Update_auxiliary();
// Allocate auxiliary vectors;
int nc = sysd.CountActiveConstraints();
if (verbose) GetLog() <<"\n-----Projected CG, solving nc=" << nc << "unknowns \n";
ChMatrixDynamic<> ml(nc,1);
ChMatrixDynamic<> mb(nc,1);
ChMatrixDynamic<> mu(nc,1);
ChMatrixDynamic<> mp(nc,1);
ChMatrixDynamic<> mw(nc,1);
ChMatrixDynamic<> mz(nc,1);
ChMatrixDynamic<> mNp(nc,1);
ChMatrixDynamic<> mtmp(nc,1);
double graddiff= 0.00001; // explorative search step for gradient
// ***TO DO*** move the following thirty lines in a short function ChLcpSystemDescriptor::ShurBvectorCompute() ?
// Compute the b_shur vector in the Shur complement equation N*l = b_shur
// with
// N_shur = D'* (M^-1) * D
// b_shur = - c + D'*(M^-1)*k = b_i + D'*(M^-1)*k
// but flipping the sign of lambdas, b_shur = - b_i - D'*(M^-1)*k
// Do this in three steps:
// Put (M^-1)*k in q sparse vector of each variable..
for (unsigned int iv = 0; iv< mvariables.size(); iv++)
if (mvariables[iv]->IsActive())
mvariables[iv]->Compute_invMb_v(mvariables[iv]->Get_qb(), mvariables[iv]->Get_fb()); // q = [M]'*fb
// ...and now do b_shur = - D' * q ..
int s_i = 0;
for (unsigned int ic = 0; ic< mconstraints.size(); ic++)
if (mconstraints[ic]->IsActive())
{
mb(s_i, 0) = - mconstraints[ic]->Compute_Cq_q();
++s_i;
}
// ..and finally do b_shur = b_shur - c
sysd.BuildBiVector(mtmp); // b_i = -c = phi/h
mb.MatrDec(mtmp);
// Optimization: backup the q sparse data computed above,
// because (M^-1)*k will be needed at the end when computing primals.
ChMatrixDynamic<> mq;
sysd.FromVariablesToVector(mq, true);
// Initialize lambdas
if (warm_start)
sysd.FromConstraintsToVector(ml);
else
ml.FillElem(0);
// Initial projection of ml ***TO DO***?
// ...
std::vector<bool> en_l(nc);
// Initially all constraints are enabled
for (int ie= 0; ie < nc; ie++)
en_l[ie] = true;
// u = -N*l+b
sysd.ShurComplementProduct(mu, &ml, &en_l); // 1) u = N*l ... #### MATR.MULTIPLICATION!!!###
mu.MatrNeg(); // 2) u =-N*l
mu.MatrInc(mb); // 3) u =-N*l+b
mp = mu;
//
// THE LOOP
//
std::vector<double> f_hist;
for (int iter = 0; iter < max_iterations; iter++)
{
// alpha = u'*p / p'*N*p
sysd.ShurComplementProduct(mNp, &mp, &en_l);// 1) Np = N*p ... #### MATR.MULTIPLICATION!!!###
double pNp = mp.MatrDot(&mp,&mNp); // 2) pNp = p'*N*p
double up = mu.MatrDot(&mu,&mp); // 3) up = u'*p
double alpha = up/pNp; // 4) alpha = u'*p / p'*N*p
if (fabs(pNp)<10e-10) GetLog() << "Rayleygh quotient pNp breakdown \n";
// l = l + alpha * p;
mtmp.CopyFromMatrix(mp);
mtmp.MatrScale(alpha);
ml.MatrInc(mtmp);
double maxdeltalambda = mtmp.NormInf();
// l = Proj(l)
sysd.ConstraintsProject(ml); // 5) l = P(l)
// u = -N*l+b
sysd.ShurComplementProduct(mu, &ml, 0); // 6) u = N*l ... #### MATR.MULTIPLICATION!!!###
mu.MatrNeg(); // 7) u =-N*l
mu.MatrInc(mb); // 8) u =-N*l+b
// w = (Proj(l+lambda*u) -l) /lambda;
mw.CopyFromMatrix(mu);
mw.MatrScale(graddiff);
mw.MatrInc(ml);
sysd.ConstraintsProject(mw); // 9) w = P(l+lambda*u) ...
mw.MatrDec(ml);
mw.MatrScale(1.0/graddiff); //10) w = (P(l+lambda*u)-l)/lambda ...
// z = (Proj(l+lambda*p) -l) /lambda;
mz.CopyFromMatrix(mp);
mz.MatrScale(graddiff);
mz.MatrInc(ml);
sysd.ConstraintsProject(mz); //11) z = P(l+lambda*u) ...
mz.MatrDec(ml);
mz.MatrScale(1.0/graddiff); //12) z = (P(l+lambda*u)-l)/lambda ...
// beta = w'*Np / pNp;
double wNp = mw.MatrDot(&mw, &mNp);
double beta = wNp / pNp;
// p = w + beta * z;
mp.CopyFromMatrix(mz);
mp.MatrScale(beta);
mp.MatrInc(mw);
// METRICS - convergence, plots, etc
double maxd = mu.NormInf(); // ***TO DO*** should be max violation, but just for test...
// For recording into correction/residuals/violation history, if debugging
if (this->record_violation_history)
AtIterationEnd(maxd, maxdeltalambda, iter);
tot_iterations++;
}
// Resulting DUAL variables:
// store ml temporary vector into ChLcpConstraint 'l_i' multipliers
sysd.FromVectorToConstraints(ml);
// Resulting PRIMAL variables:
// compute the primal variables as v = (M^-1)(k + D*l)
// v = (M^-1)*k ... (by rewinding to the backup vector computed ad the beginning)
sysd.FromVectorToVariables(mq);
// ... + (M^-1)*D*l (this increment and also stores 'qb' in the ChLcpVariable items)
for (unsigned int ic = 0; ic < mconstraints.size(); ic++)
{
if (mconstraints[ic]->IsActive())
mconstraints[ic]->Increment_q( mconstraints[ic]->Get_l_i() );
}
if (verbose) GetLog() <<"-----\n";
return maxviolation;
}
double ChLcpInteriorPoint::Solve(
ChLcpSystemDescriptor& sysd ///< system description with constraints and variables
)
{
std::cout << "-------using interior point solver!!------" << std::endl;
std::vector<ChLcpConstraint*>& mconstraints = sysd.GetConstraintsList();
std::vector<ChLcpVariables*>& mvariables = sysd.GetVariablesList();
ChMatrixDynamic <double> mv0;
ChSparseMatrix mM;
ChSparseMatrix mCq;
ChSparseMatrix mE;
ChMatrixDynamic <double> mf;
ChMatrixDynamic <double> mb;
ChMatrixDynamic <double> mfric;
sysd.ConvertToMatrixForm(&mCq, &mM, &mE, &mf, &mb, &mfric);
sysd.FromVariablesToVector(mv0);
ChStreamOutAsciiFile file_V0( "dump_V_old.dat" ) ;
mv0.StreamOUTdenseMatlabFormat(file_V0) ;
ChStreamOutAsciiFile file_M ( "dump_M.dat" ) ;
mM.StreamOUTsparseMatlabFormat ( file_M ) ;
ChStreamOutAsciiFile file_Cq ( "dump_Cq.dat" ) ;
mCq.StreamOUTsparseMatlabFormat ( file_Cq ) ;
ChStreamOutAsciiFile file_E ( "dump_E.dat" ) ;
mE.StreamOUTsparseMatlabFormat ( file_E ) ;
ChStreamOutAsciiFile file_f ( "dump_f.dat" ) ;
mf.StreamOUTdenseMatlabFormat ( file_f ) ;
ChStreamOutAsciiFile file_b ( "dump_b.dat" ) ;
mb.StreamOUTdenseMatlabFormat ( file_b ) ;
ChStreamOutAsciiFile file_fric ( "dump_fric.dat" ) ;
mfric.StreamOUTdenseMatlabFormat ( file_fric ) ;
printf("Successfully writing chickenbutt files!\n");
/* file_f.GetFstream().close();
file_fric.GetFstream().close();
file_V0.GetFstream().close();
file_M.GetFstream().close();
file_Cq.GetFstream().close();
file_b.GetFstream().close();
*/
int nBodies = mM.GetColumns()/6;
size_t nVariables = mvariables.size();
size_t nConstraints = sysd.CountActiveConstraints();
int numContacts = nConstraints/3;
size_t nOfConstraints = mconstraints.size();
/* ALWYAS DO THIS IN THE LCP SOLVER!!!*/
for (unsigned int ic = 0; ic < nConstraints; ic++)
mconstraints[ic]->Update_auxiliary();
//Get sparse info for contact jacobian and Minv matrix to pass on to Ang's solver
std::vector<int> index_i_Cq;
std::vector<int> index_j_Cq;
std::vector<double> val_Cq;
double val;
// fprintf(stderr, "------------Cq(from C::E)----------\n");
for (int ii = 0; ii < mCq.GetRows(); ii++){
for (int jj = 0; jj < mCq.GetColumns(); jj++){
val = mCq.GetElement(ii,jj);
if (val){
index_i_Cq.push_back(jj);
index_j_Cq.push_back(ii);
val_Cq.push_back(val);
// fprintf(stderr, "%d %d %.20g\n", ii, jj, val);
}
}
}
/* for (int iv = 0; iv < mvariables.size(); iv++)
if (mvariables[iv]->IsActive())
mvariables[iv]->Compute_invMb_v(mvariables[iv]->Get_qb(), mvariables[iv]->Get_fb());
*/
// int count = 0;
// for (std::vector<int>::iterator it = index_i_Cq.begin(); it != index_i_Cq.end(); it ++){
// std::cout << "(" << index_i_Cq[count] <<"," << index_j_Cq[count] <<"):" << val_Cq[count] << std::endl;
// count ++;
// }
// Minv matrix
std::vector<int> index_i_Minv;
std::vector<int> index_j_Minv;
std::vector<double> val_Minv;
for (int i = 0; i < nBodies*6; i++){
index_i_Minv.push_back(i);
index_j_Minv.push_back(i);
val_Minv.push_back(1.0/mM.GetElement(i,i));
}
// create reference to pass on to SPIKE
int *Cq_i = &index_i_Cq[0];
int *Cq_j = &index_j_Cq[0];
int Cq_nnz = val_Cq.size();
double *Cq_val = &val_Cq[0];
int *Minv_i = &index_i_Minv[0];
int *Minv_j = &index_j_Minv[0];
double *Minv_val = &val_Minv[0];
// formulate rhs of optimization problem f(x) = 1/2 *x'*N*x + r'*x
ChMatrixDynamic <double> opt_r_tmp(nConstraints,1);
// assemble r vector
/** 1. put [M^-1]*k in q sparse vector of each variable **/
for (unsigned int iv = 0; iv < nVariables; iv ++)
if (mvariables[iv]->IsActive()){
mvariables[iv]->Compute_invMb_v(mvariables[iv]->Get_qb(), mvariables[iv]->Get_fb());
ChMatrix<double> k = mvariables[iv]->Get_fb();
ChMatrix<double> Mk = mvariables[iv]->Get_qb();
// fprintf(stderr, "Body %d k: %.12f %.12f %.12f\n", iv, k(0,0), k(1,0), k(2,0));
// fprintf(stderr, "Body %d M^[-1]*k: %.12f %.12f %.12f\n", iv, Mk(0,0), Mk(1,0), Mk(2,0));
}
/** 2. now do rhs = D'*q = D'*(M^-1)*k **/
int s_i = 0;
opt_r.Resize(nConstraints,1);
for (unsigned int ic = 0; ic < nConstraints; ic ++)
if (mconstraints[ic]->IsActive()){
opt_r(s_i,0) = mconstraints[ic]->Compute_Cq_q();
++s_i;
}
// fprintf(stderr, "------D'*M^(-1)*k-------\n");
// for (int i = 0; i < opt_r.GetRows(); i++)
// fprintf(stderr, "%.16f\n", opt_r(i,0));
/** 3. rhs = rhs + c **/
sysd.BuildBiVector(opt_r_tmp);
opt_r.MatrInc(opt_r_tmp);
// fprintf(stderr, "------opt_r-------\n");
// for (int i = 0; i < opt_r.GetRows(); i++)
// fprintf(stderr, "%.12f\n", opt_r(i,0));
///////////////////
//velocity update//
///////////////////
ChMatrixDynamic<> mq;
sysd.FromVariablesToVector(mq, true);
for (int i = 0; i < mq.GetRows(); i++){
// mq.SetElementN(i, mf.GetElementN(i)/mM.GetElement(i,i) + mv0.GetElementN(i));
// mq.SetElementN(i, mf.GetElementN(i)/mM.GetElement(i,i));
// fprintf(stderr, "%d: %g / %g + %g = %g\n",i, mf.GetElementN(i), mM.GetElement(i,i), mv0.GetElementN(i), mq.GetElementN(i));
// fprintf(stderr, "%g\n", mq.GetElementN(i));
}
////////////////////////////
//assign solver parameters//
////////////////////////////
double barrier_t = 1;
double eta_hat;
int numStages = 500;
int mu1 = 10;
double b1 = 0.5;
double a1 = 0.01;
// assign vectors here
ff.Resize(numContacts*2,1);
lambda_k.Resize(numContacts*2,1); /*initialize lambda_k*/
xk.Resize(numContacts*3,1);
r_dual.Resize(numContacts*3,1);
r_cent.Resize(numContacts*2,1);
d_x.Resize(numContacts*3,1);
d_lambda.Resize(numContacts*2,1);
Schur_rhs.Resize(3*numContacts,1);
grad_f.Resize(3*numContacts,1);
if (mconstraints.size() == 0){
sysd.FromVectorToConstraints(xk);
sysd.FromVectorToVariables(mq);
for (size_t ic = 0; ic < mconstraints.size(); ic ++){
if (mconstraints[ic]->IsActive())
mconstraints[ic]->Increment_q(mconstraints[ic]->Get_l_i());
}
return 1e-8;
}
double *BlockDiagonal_val = new double[9*numContacts];
int *BlockDiagonal_i = new int[9*numContacts];
int *BlockDiagonal_j = new int[9*numContacts];
double *spike_rhs = new double[3*numContacts];
int tmp0, tmp1, tmp2;
for (int i = 0; i < numContacts; i ++){
tmp0 = 3*i;
tmp1 = 3*i+1;
tmp2 = 3*i+2;
*(BlockDiagonal_i + 9*i) = tmp0;
*(BlockDiagonal_i + 9*i+1) = tmp0;
*(BlockDiagonal_i + 9*i+2) = tmp0;
*(BlockDiagonal_i + 9*i+3) = tmp1;
*(BlockDiagonal_i + 9*i+4) = tmp1;
*(BlockDiagonal_i + 9*i+5) = tmp1;
*(BlockDiagonal_i + 9*i+6) = tmp2;
*(BlockDiagonal_i + 9*i+7) = tmp2;
*(BlockDiagonal_i + 9*i+8) = tmp2;
*(BlockDiagonal_j + 9*i) = tmp0;
*(BlockDiagonal_j + 9*i+1) = tmp1;
*(BlockDiagonal_j + 9*i+2) = tmp2;
*(BlockDiagonal_j + 9*i+3) = tmp0;
*(BlockDiagonal_j + 9*i+4) = tmp1;
*(BlockDiagonal_j + 9*i+5) = tmp2;
*(BlockDiagonal_j + 9*i+6) = tmp0;
*(BlockDiagonal_j + 9*i+7) = tmp1;
*(BlockDiagonal_j + 9*i+8) = tmp2;
}
// initialize xk
for (int i = 0; i < numContacts; i ++){
xk(3*i, 0) = 1;
xk(3*i+1, 0) = 0;
xk(3*i+2, 0) = 0;
}
evaluateConstraints(mfric.GetAddress(), numContacts, false);
//initialize lambda
for (int i = 0; i < lambda_k.GetRows(); i++)
lambda_k(i,0) = -1/(barrier_t * ff(i,0));
/////////////////////////////
////GO THROUGH EACH STAGE////
/////////////////////////////
for (int stage = 0; stage < numStages; stage++){
eta_hat = - lambda_k.MatrDot(&lambda_k, &ff);
barrier_t = mu1 * (2*numContacts)/eta_hat;
// assemble grad_f = N*x + r
sysd.ShurComplementProduct(grad_f, &xk, 0);
// fprintf(stderr, "----------N*x----------\n");
// for (int i = 0; i < grad_f.GetRows(); i++)
// fprintf(stderr, "%.20f\n", grad_f.GetElementN(i));
grad_f.MatrInc(opt_r);
// fprintf(stderr, "----------grad_f----------\n");
// for (int i = 0; i < grad_f.GetRows(); i++)
// fprintf(stderr, "%.20f\n", grad_f.GetElementN(i));
// compute r_d and r_c for schur implementation
computeSchurRHS(grad_f.GetAddress(), mfric.GetAddress(), numContacts, barrier_t);
// fprintf(stderr, "----------r_dual----------\n");
// for (int i = 0; i < r_dual.GetRows(); i++)
// fprintf(stderr, "%.16f\n", r_dual.GetElementN(i));
// fprintf(stderr, "----------r_cent----------\n");
// for (int i = 0; i < r_cent.GetRows(); i++)
// fprintf(stderr, "%.16f\n", r_cent.GetElementN(i));
// assemble block diagonal matrix
computeBlockDiagonal(BlockDiagonal_val, mfric.GetAddress(), numContacts, barrier_t);
// assemble rhs vector for spike solver
computeSpikeRHS(spike_rhs, mfric.GetAddress(), numContacts, barrier_t);
// fprintf(stderr, "----------spike_rhs----------\n");
// for (int i = 0; i < 3*numContacts; i++){
// fprintf(stderr, "%.16f\n", *(spike_rhs + i));
// }
double *spike_dx = new double [3*numContacts];
//call ang's solver here....
bool solveSuc = solveSPIKE(nBodies, numContacts, Cq_i, Cq_j, Cq_nnz, Cq_val, Minv_i, Minv_j, Minv_val, BlockDiagonal_i, BlockDiagonal_j, BlockDiagonal_val, spike_dx, spike_rhs);
if (solveSuc == false)
std::cerr << "Solve Failed!" << std::endl;
// assume d_x is calculated perfectly!
for (int i = 0; i < numContacts; i++){
d_x(3*i,0) = *(spike_dx + 3*i);
d_x(3*i+1,0) = *(spike_dx + 3*i + 1);
d_x(3*i+2,0) = *(spike_dx + 3*i + 2);
}
/* fprintf(stderr, "-------d_x---------\n");
for (int i = 0; i < d_x.GetRows(); i++){
fprintf(stderr, "%.20f\n", d_x(i,0));
}
*/
// free the heap!
delete [] spike_dx;
// evaluate d_lambda
for (int i = 0; i < numContacts; i++){
d_lambda(i) = lambda_k(i,0)/ff(i,0) * (pow(mfric(3*i,0),2)*xk(3*i,0)*d_x(3*i,0) - xk(3*i+1,0)*d_x(3*i+1,0) -xk(3*i+2,0)*d_x(3*i+2,0) - r_cent(i,0) );
d_lambda(i + numContacts) = lambda_k(i+numContacts,0)/ff(i+numContacts)*(d_x(3*i) - r_cent(i + numContacts));
}
/* fprintf(stderr, "----------d_lambda----------\n");
for (int i = 0; i < 2*numContacts; i++){
fprintf(stderr, "%.16f\n", d_lambda(i,0));
}
*/
///////////////
//LINE SEARCH//
///////////////
double s_max = 1;
double tmp;
for (int i = 0; i < 2*numContacts; i ++){
if (d_lambda(i,0) < 0){
tmp = -lambda_k(i,0)/d_lambda(i,0);
// fprintf(stderr, "i = %d, tmp = %.20f\n", i, tmp);
if (tmp < s_max){
s_max = tmp;
}
}
}
double bla = 0.99;
double ss = bla * s_max;
// fprintf(stderr, "s_max = %.20g\n", s_max);
ff_tmp.Resize(2*numContacts,1);
lambda_k_tmp.Resize(2*numContacts,1);
xk_tmp.Resize(3*numContacts,1);;
r_dual_tmp.Resize(3*numContacts,1);;
r_cent_tmp.Resize(3*numContacts,1);;
bool DO = true;
int count = 0;
// fprintf(stderr, "----line search----\n");
while (DO){
xk_tmp = d_x;
// fprintf(stderr, "-----d_x----\n");
// for (int i = 0; i < 3*numContacts; i ++)
// fprintf(stderr, "%.20g\n", xk_tmp(i,0));
xk_tmp.MatrScale(ss);
// fprintf(stderr, "-----ss*d_x----\n");
// for (int i = 0; i < 3*numContacts; i ++)
// fprintf(stderr, "%.20g\n", xk_tmp(i,0));
xk_tmp.MatrAdd(xk,xk_tmp);
// fprintf(stderr, "-----xk+ss*d_x----\n");
// for (int i = 0; i < 3*numContacts; i ++)
// fprintf(stderr, "%.20g\n", xk_tmp(i,0));
evaluateConstraints(mfric.GetAddress(), numContacts, true);
// fprintf(stderr, "-----tmp_ff----\n");
// for (int i = 0; i < 2*numContacts; i ++)
// fprintf(stderr, "%.20g\n", ff_tmp(i,0));
// fprintf(stderr, "max_ff = %.20g\n", ff_tmp.Max());
if (ff_tmp.Max()<0){
DO = false;
}
else{
count++;
ss = b1 * ss;
// fprintf(stderr,"ss[%d] = %.20g\n", count, ss);
}
}
DO = true;
double norm_r_t = sqrt(pow(r_dual.NormTwo(),2) + pow(r_cent.NormTwo(),2));
double norm_r_t_ss;
count = 0;
while (DO){
xk_tmp = d_x;
xk_tmp.MatrScale(ss);
xk_tmp.MatrAdd(xk,xk_tmp);
lambda_k_tmp = d_lambda;
lambda_k_tmp.MatrScale(ss);
lambda_k_tmp.MatrAdd(lambda_k, lambda_k_tmp);
evaluateConstraints(mfric.GetAddress(),numContacts,true);
sysd.ShurComplementProduct(grad_f, &xk_tmp, 0);
grad_f.MatrInc(opt_r);
computeSchurKKT(grad_f.GetAddress(), mfric.GetAddress(), numContacts, barrier_t, true);
norm_r_t_ss = sqrt(pow(r_dual_tmp.NormTwo(),2) + pow(r_cent_tmp.NormTwo(),2));
if (norm_r_t_ss < (1 - a1*ss)*norm_r_t)
DO = false;
else{
count ++;
ss = b1*ss;
// fprintf(stderr,"ss[%d] = %.20g\n", count, ss);
}
}
// upadate xk and lambda_k
d_x.MatrScale(ss);
// fprintf(stderr, "-------ss*d_x---------\n");
// for (int i = 0; i < d_x.GetRows(); i++)
// fprintf(stderr, "%.20f\n", d_x(i,0));
// fprintf(stderr, "----------xk = xk + ss*d_x--------\n");
xk.MatrInc(d_x);
// for (int i = 0; i < xk.GetRows(); i++)
// fprintf(stderr, "%.20f\n", xk(i,0));
d_lambda.MatrScale(ss);
lambda_k.MatrInc(d_lambda);
// fprintf(stderr, "-------lambda_k------\n");
// for (int i = 0; i < lambda_k.GetRows(); i++)
// fprintf(stderr, "%.20f\n", lambda_k(i,0));
sysd.ShurComplementProduct(grad_f, &xk, 0);
grad_f.MatrInc(opt_r);
evaluateConstraints(mfric.GetAddress(), numContacts, false);
computeSchurKKT(grad_f.GetAddress(), mfric.GetAddress(), numContacts, barrier_t, false);
// std::cout << "----r_dual-----" << std::endl;
// for (int i = 0; i < r_dual.GetRows(); i++)
// std::cout << r_dual(i,0) << std::endl;
// std::cout << "-----r_cent-----" << std::endl;
// for (int i = 0; i < r_cent.GetRows(); i++)
// std::cout << r_cent(i,0) << std::endl;
fprintf(stderr, "stage[%d], rd = %e, rg = %e, s = %f, t = %f\n", stage+1, r_dual.NormInf(), r_cent.NormInf(), ss, barrier_t);
if (r_cent.NormInf() < 1e-10 ||stage == (numStages - 1)){
fprintf(stderr, "solution found after %d stages!\n", stage+1);
// fprintf(stderr, "stage[%d], rd = %e, rg = %e, s = %f, t = %f\n", stage+1, r_dual.NormInf(), r_cent.NormInf(), ss, barrier_t);
delete [] BlockDiagonal_val;
delete [] BlockDiagonal_i;
delete [] BlockDiagonal_j;
delete [] spike_rhs;
/////////////////////////////////////////////
//set small-magnitude contact force to zero//
/////////////////////////////////////////////
// for (int i = 0; i < numContacts; i++){
// if (sqrt(pow(xk(3*i,0),2) + pow(xk(3*i+1,0),2) + pow(xk(3*i+2,0),2)) < 1e-6){
/// xk(3*i,0) = 0;
// xk(3*i+1,0) = 0;
// xk(3*i+2, 0) = 0;
// }
// }
sysd.FromVectorToConstraints(xk);
sysd.FromVectorToVariables(mq);
for (size_t ic = 0; ic < mconstraints.size(); ic ++){
if (mconstraints[ic]->IsActive())
mconstraints[ic]->Increment_q(mconstraints[ic]->Get_l_i());
}
// return r_cent.NormInf();
return 1e-8;
}
evaluateConstraints(mfric.GetAddress(), numContacts, false);
}
}
double ChLcpSimplexSolver::Solve(
ChLcpSystemDescriptor& sysd ///< system description with constraints and variables
)
{
std::vector<ChLcpConstraint*>& mconstraints = sysd.GetConstraintsList();
std::vector<ChLcpVariables*>& mvariables = sysd.GetVariablesList();
std::vector<ChLcpKblock*>& mstiffness = sysd.GetKblocksList();
double maxviolation = 0.;
// --
// Count active linear constraints..
int n_c=0;
int n_d=0;
for (unsigned int ic = 0; ic< mconstraints.size(); ic++)
{
if (mconstraints[ic]->IsActive())
if (mconstraints[ic]->IsLinear())
if (mconstraints[ic]->IsUnilateral())
n_d++;
else
n_c++;
}
// --
// Count active variables, by scanning through all variable blocks..
int n_q=0;
for (unsigned int iv = 0; iv< mvariables.size(); iv++)
{
if (mvariables[iv]->IsActive())
{
mvariables[iv]->SetOffset(n_q); // also store offsets in state and MC matrix
n_q += mvariables[iv]->Get_ndof();
}
}
if (n_q==0) return 0;
int n_docs = n_c + n_d;
int n_vars = n_q + n_docs;
int nv = sysd.CountActiveVariables(); // also sets offsets
int nc = sysd.CountActiveConstraints();
int nx = nv+nc; // total scalar unknowns, in x vector for full KKT system Z*x-d=0
// --
// Reset and resize (if needed) auxiliary vectors
MC->Reset(n_vars,n_vars); // fast! Reset() method does not realloc if size doesn't change
B->Reset(n_vars,1);
X->Reset(n_vars,1);
if (unilaterals != 0)
{delete[]unilaterals; unilaterals = 0;}
if (n_d > 0)
{
unilaterals = new ChUnilateralData[n_d];
}
// --
// Fills the MC matrix and B vector, to pass to the sparse LCP simplex solver.
// The original problem, stated as
// | M -Cq'|*|q|- | f|= |0| , c>=0, l>=0, l*c=0;
// | Cq 0 | |l| |-b| |c|
// will be used with a small modification as:
// | M Cq'|*| q|- | f|= |0| , c>=0, l>=0, l*c=0;
// | Cq 0 | |-l| |-b| |c|
// so that it uses a symmetric MC matrix (the LDL factorization at each
// pivot is happier:)
// .. fills M submasses and 'f' part of B
int s_q=0;
for (unsigned int iv = 0; iv< mvariables.size(); iv++)
{
if (mvariables[iv]->IsActive())
{
mvariables[iv]->Build_M(*MC, s_q, s_q); // .. fills MC (M part)
B->PasteMatrix(&mvariables[iv]->Get_fb(),s_q, 0); // .. fills B (f part)
s_q += mvariables[iv]->Get_ndof();
}
}
// ..if some stiffness / hessian matrix has been added to M ,
// also add it to the sparse M
int s_k=0;
for (unsigned int ik = 0; ik< mstiffness.size(); ik++)
{
mstiffness[ik]->Build_K(*MC, true);
}
// .. fills M jacobians (only lower part) and 'b' part of B
int s_c=0;
int s_d=0;
for (unsigned int ic = 0; ic< mconstraints.size(); ic++)
{
if (mconstraints[ic]->IsActive())
if (mconstraints[ic]->IsLinear())
if (mconstraints[ic]->IsUnilateral())
{
mconstraints[ic]->Build_Cq (*MC, n_q + n_c + s_d);// .. fills MC (Cq part)
mconstraints[ic]->Build_CqT(*MC, n_q + n_c + s_d);// .. fills MC (Cq' part)
B->SetElement(n_q + n_c + s_d, 0,
-mconstraints[ic]->Get_b_i() ); // .. fills B (c part)
unilaterals[s_d].status = CONSTR_UNILATERAL_OFF;
s_d++;
}
else
{
mconstraints[ic]->Build_Cq (*MC, n_q + s_c);
mconstraints[ic]->Build_CqT(*MC, n_q + s_c);
B->SetElement(n_q + s_c, 0,
-mconstraints[ic]->Get_b_i() );
s_c++;
}
}
//***DEBUG***
double max_err=0; int err_r = -1; int err_c = -1;
for (int row = 0; row < MC->GetRows(); ++row)
for (int col = 0; col < MC->GetColumns(); ++col)
{
double diff = fabs (MC->GetElement(row,col)-MC->GetElement(col,row));
if (diff > max_err)
{
max_err = diff;
err_r = row;
err_c = col;
}
}
if (max_err > 1e-10)
GetLog() << "simplex solver: NONSYMMETRIC MC! error " << max_err << " at " << err_r << "," << err_c << "\n";
// --
// Solve the LCP
MC->SolveLCP(B, X,
n_c, n_d,
truncation_step,
false,
unilaterals);
// --
// Update results into variable-interface objects
s_q=0;
for (unsigned int iv = 0; iv< mvariables.size(); iv++)
{
if (mvariables[iv]->IsActive())
{
mvariables[iv]->Get_qb().PasteClippedMatrix(X,
s_q,0,
mvariables[iv]->Get_ndof(),1,
0,0);
s_q += mvariables[iv]->Get_ndof();
}
}
// --
// Update results into constraint-interface objects
s_c=0;
s_d=0;
for (unsigned int ic = 0; ic< mconstraints.size(); ic++)
{
if (mconstraints[ic]->IsActive())
if (mconstraints[ic]->IsLinear())
if (mconstraints[ic]->IsUnilateral())
{ //(change sign of multipliers!)
mconstraints[ic]->Set_l_i( -X->GetElement(n_q + n_c + s_d , 0));
s_d++;
}
else
{ //(change sign of multipliers!)
mconstraints[ic]->Set_l_i( -X->GetElement(n_q + s_c , 0));
s_c++;
}
}
return maxviolation;
}
double ChLcpIterativeAPGD::Solve(
ChLcpSystemDescriptor& sysd ///< system description with constraints and variables
)
{
std::vector<ChLcpConstraint*>& mconstraints = sysd.GetConstraintsList();
std::vector<ChLcpVariables*>& mvariables = sysd.GetVariablesList();
double gdiff= 0.000001;
double maxviolation = 0.;
int i_friction_comp = 0;
double theta_k=1.0;
double theta_k1=theta_k;
double beta_k1=0.0;
double L_k=0.0;
double t_k=0.0;
tot_iterations = 0;
// Allocate auxiliary vectors;
int nc = sysd.CountActiveConstraints();
if (verbose) GetLog() <<"\n-----Accelerated Projected Gradient Descent, solving nc=" << nc << "unknowns \n";
//ChMatrixDynamic<> ml(nc,1); //I made this into a class variable so I could print it easier -Hammad
ml.Resize(nc,1);
ChMatrixDynamic<> mx(nc,1);
ChMatrixDynamic<> ms(nc,1);
ChMatrixDynamic<> my(nc,1);
ChMatrixDynamic<> ml_candidate(nc,1);
ChMatrixDynamic<> mg(nc,1);
ChMatrixDynamic<> mg_tmp(nc,1);
ChMatrixDynamic<> mg_tmp1(nc,1);
ChMatrixDynamic<> mg_tmp2(nc,1);
//ChMatrixDynamic<> mb(nc,1); //I made this into a class variable so I could print it easier -Hammad
mb.Resize(nc,1);
ChMatrixDynamic<> mb_tmp(nc,1);
// Update auxiliary data in all constraints before starting,
// that is: g_i=[Cq_i]*[invM_i]*[Cq_i]' and [Eq_i]=[invM_i]*[Cq_i]'
for (unsigned int ic = 0; ic< mconstraints.size(); ic++)
mconstraints[ic]->Update_auxiliary();
// Average all g_i for the triplet of contact constraints n,u,v.
// Can be used for the fixed point phase and/or by preconditioner.
int j_friction_comp = 0;
double gi_values[3];
for (unsigned int ic = 0; ic< mconstraints.size(); ic++)
{
if (mconstraints[ic]->GetMode() == CONSTRAINT_FRIC)
{
gi_values[j_friction_comp] = mconstraints[ic]->Get_g_i();
j_friction_comp++;
if (j_friction_comp==3)
{
double average_g_i = (gi_values[0]+gi_values[1]+gi_values[2])/3.0;
mconstraints[ic-2]->Set_g_i(average_g_i);
mconstraints[ic-1]->Set_g_i(average_g_i);
mconstraints[ic-0]->Set_g_i(average_g_i);
j_friction_comp=0;
}
}
}
// ***TO DO*** move the following thirty lines in a short function ChLcpSystemDescriptor::ShurBvectorCompute() ?
// Compute the b_shur vector in the Shur complement equation N*l = b_shur
// with
// N_shur = D'* (M^-1) * D
// b_shur = - c + D'*(M^-1)*k = b_i + D'*(M^-1)*k
// but flipping the sign of lambdas, b_shur = - b_i - D'*(M^-1)*k
// Do this in three steps:
// Put (M^-1)*k in q sparse vector of each variable..
for (unsigned int iv = 0; iv< mvariables.size(); iv++)
if (mvariables[iv]->IsActive())
mvariables[iv]->Compute_invMb_v(mvariables[iv]->Get_qb(), mvariables[iv]->Get_fb()); // q = [M]'*fb
// ...and now do b_shur = - D'*q = - D'*(M^-1)*k ..
int s_i = 0;
for (unsigned int ic = 0; ic< mconstraints.size(); ic++)
if (mconstraints[ic]->IsActive())
{
mb(s_i, 0) = - mconstraints[ic]->Compute_Cq_q();
++s_i;
}
// ..and finally do b_shur = b_shur - c
sysd.BuildBiVector(mb_tmp); // b_i = -c = phi/h
mb.MatrDec(mb_tmp);
// Optimization: backup the q sparse data computed above,
// because (M^-1)*k will be needed at the end when computing primals.
ChMatrixDynamic<> mq;
sysd.FromVariablesToVector(mq, true);
// Initialize lambdas
if (warm_start)
sysd.FromConstraintsToVector(ml);
else
ml.FillElem(0);
// Initial projection of ml ***TO DO***?
sysd.ConstraintsProject(ml);
// Fallback solution
double lastgoodres = 10e30;
double lastgoodfval = 10e30;
ml_candidate.CopyFromMatrix(ml);
// g = gradient of 0.5*l'*N*l-l'*b
// g = N*l-b
sysd.ShurComplementProduct(mg, &ml, 0); // 1) g = N*l ... #### MATR.MULTIPLICATION!!!###
mg.MatrDec(mb); // 2) g = N*l - b_shur ...
//
// THE LOOP
//
double mf_p =0;
mb_tmp.FillElem(-1.0);
mb_tmp.MatrInc(ml);
sysd.ShurComplementProduct(mg_tmp,&mb_tmp,0); // 1) g = N*l ... #### MATR.MULTIPLICATION!!!###
if(mb_tmp.NormTwo()==0){
L_k=1;
}else{
L_k=mg_tmp.NormTwo()/mb_tmp.NormTwo();
}
t_k=1/L_k;
double obj1=0;
double obj2=0;
my.CopyFromMatrix(ml);
mx.CopyFromMatrix(ml);
for (int iter = 0; iter < max_iterations; iter++)
{
sysd.ShurComplementProduct(mg_tmp1, &my, 0); // 1) g_tmp1 = N*yk ... #### MATR.MULTIPLICATION!!!###
mg.MatrSub(mg_tmp1,mb); // 2) g = N*yk - b_shur ...
mx.CopyFromMatrix(mg); // 1) xk1=g
mx.MatrScale(-t_k); // 2) xk1=-tk*g
mx.MatrInc(my); // 3) xk1=y-tk*g
sysd.ConstraintsProject(mx); // 4) xk1=P(y-tk*g)
//Now do backtracking for the steplength
sysd.ShurComplementProduct(mg_tmp, &mx, 0); // 1) g_tmp = N*xk1 ... #### MATR.MULTIPLICATION!!!###
mg_tmp2.MatrSub(mg_tmp,mb); // 2) g_tmp2 = N*xk1 - b_shur ...
mg_tmp.MatrScale(0.5); // 3) g_tmp=0.5*N*xk1
mg_tmp.MatrDec(mb); // 4) g_tmp=0.5*N*xk1-b_shur
obj1 = mx.MatrDot(&mx,&mg_tmp); // 5) obj1=xk1'*(0.5*N*x_k1-b_shur)
mg_tmp1.MatrScale(0.5); // 1) g_tmp1 = 0.5*N*yk
mg_tmp1.MatrDec(mb); // 2) g_tmp1 = 0.5*N*yk-b_shur
obj2 = my.MatrDot(&my,&mg_tmp1); // 3) obj2 = yk'*(0.5*N*yk-b_shur)
ms.MatrSub(mx,my); // 1) s=xk1-yk
while(obj1>obj2+mg.MatrDot(&mg,&ms)+0.5*L_k*pow(ms.NormTwo(),2.0))
{
L_k=2*L_k;
t_k=1/L_k;
mx.CopyFromMatrix(mg); // 1) xk1=g
mx.MatrScale(-t_k); // 2) xk1=-tk*g
mx.MatrInc(my); // 3) xk1=yk-tk*g
sysd.ConstraintsProject(mx); // 4) xk1=P(yk-tk*g)
sysd.ShurComplementProduct(mg_tmp, &mx, 0); // 1) g_tmp = N*xk1 ... #### MATR.MULTIPLICATION!!!###
mg_tmp2.MatrSub(mg_tmp,mb); // 2) g_tmp2 = N*xk1 - b_shur ...
mg_tmp.MatrScale(0.5); // 3) g_tmp=0.5*N*xk1
mg_tmp.MatrDec(mb); // 4) g_tmp=0.5*N*xk1-b_shur
obj1 = mx.MatrDot(&mx,&mg_tmp); // 5) obj1=xk1'*(0.5*N*x_k1-b_shur)
ms.MatrSub(mx,my); // 1) s=xk1-yk
if (verbose) GetLog() << "APGD halving stepsize at it " << iter << "\n";
}
theta_k1=(-pow(theta_k,2)+theta_k*sqrt(pow(theta_k,2)+4))/2.0;
beta_k1=theta_k*(1.0-theta_k)/(pow(theta_k,2)+theta_k1);
my.CopyFromMatrix(mx); // 1) y=xk1;
my.MatrDec(ml); // 2) y=xk1-xk;
my.MatrScale(beta_k1); // 3) y=beta_k1*(xk1-xk);
my.MatrInc(mx); // 4) y=xk1+beta_k1*(xk1-xk);
ms.MatrSub(mx,ml); // 0) s = xk1 - xk;
// Restarting logic if momentum is not appropriate
if (mg.MatrDot(&mg,&ms)>0)
{
my.CopyFromMatrix(mx); // 1) y=xk1
theta_k1=1.0; // 2) theta_k=1
if (verbose) GetLog() << "Restarting APGD at it " << iter << "\n";
}
//Allow the step to grow...
L_k=0.9*L_k;
t_k=1/L_k;
ml.CopyFromMatrix(mx); // 1) xk=xk1;
theta_k=theta_k1; // 2) theta_k=theta_k1;
//****METHOD 1 for residual, same as ChLcpIterativeBB
// Project the gradient (for rollback strategy)
// g_proj = (l-project_orthogonal(l - gdiff*g, fric))/gdiff;
mb_tmp.CopyFromMatrix(mg_tmp2);
mb_tmp.MatrScale(-gdiff);
mb_tmp.MatrInc(ml);
sysd.ConstraintsProject(mb_tmp);
mb_tmp.MatrDec(ml);
mb_tmp.MatrDivScale(-gdiff);
double g_proj_norm = mb_tmp.NormTwo(); // NormInf() is faster..
//****End of METHOD 1 for residual, same as ChLcpIterativeBB
//****METHOD 2 for residual, same as ChLcpIterativeSOR
maxviolation = 0;
i_friction_comp = 0;
for (unsigned int ic = 0; ic< mconstraints.size(); ic++)
{
if (mconstraints[ic]->IsActive())
{
// true constraint violation may be different from 'mresidual' (ex:clamped if unilateral)
double candidate_violation = fabs(mconstraints[ic]->Violation(mg_tmp2.ElementN(ic)));
if (mconstraints[ic]->GetMode() == CONSTRAINT_FRIC)
{
candidate_violation = 0;
i_friction_comp++;
if (i_friction_comp==1)
candidate_violation = fabs(ChMin(0.0,mg_tmp2.ElementN(ic)));
if (i_friction_comp==3)
i_friction_comp =0;
}
else
{
}
maxviolation = ChMax(maxviolation, fabs(candidate_violation));
}
}
g_proj_norm=maxviolation;
//****End of METHOD 2 for residual, same as ChLcpIterativeSOR
// Rollback solution: the last best candidate ('l' with lowest projected gradient)
// in fact the method is not monotone and it is quite 'noisy', if we do not
// do this, a prematurely truncated iteration might give a crazy result.
if(g_proj_norm < lastgoodres)
{
lastgoodres = g_proj_norm;
ml_candidate = ml;
}
// METRICS - convergence, plots, etc
if (verbose)
{
// f_p = 0.5*l_candidate'*N*l_candidate - l_candidate'*b = l_candidate'*(0.5*Nl_candidate - b);
sysd.ShurComplementProduct(mg_tmp, &ml_candidate, 0); // 1) g_tmp = N*l_candidate ... #### MATR.MULTIPLICATION!!!###
mg_tmp.MatrScale(0.5); // 2) g_tmp = 0.5*N*l_candidate
mg_tmp.MatrDec(mb); // 3) g_tmp = 0.5*N*l_candidate-b_shur
mf_p = ml_candidate.MatrDot(&ml_candidate,&mg_tmp); // 4) mf_p = l_candidate'*(0.5*N*l_candidate-b_shur)
}
double maxdeltalambda = ms.NormInf();
double maxd = lastgoodres;
// For recording into correction/residuals/violation history, if debugging
if (this->record_violation_history)
AtIterationEnd(maxd, maxdeltalambda, iter);
if (verbose) GetLog() << " iter=" << iter << " f=" << mf_p << " |d|=" << maxd << " |s|=" << maxdeltalambda << "\n";
tot_iterations++;
// Terminate the loop if violation in constraints has been succesfully limited.
// ***TO DO*** a reliable termination creterion..
///*
if (maxd < this->tolerance)
{
if (verbose) GetLog() <<"APGD premature converged at i=" << iter << "\n";
break;
}
//*/
}
// Fallback to best found solution (might be useful because of nonmonotonicity)
ml.CopyFromMatrix(ml_candidate);
// Resulting DUAL variables:
// store ml temporary vector into ChLcpConstraint 'l_i' multipliers
sysd.FromVectorToConstraints(ml);
// Resulting PRIMAL variables:
// compute the primal variables as v = (M^-1)(k + D*l)
// v = (M^-1)*k ... (by rewinding to the backup vector computed ad the beginning)
sysd.FromVectorToVariables(mq);
// ... + (M^-1)*D*l (this increment and also stores 'qb' in the ChLcpVariable items)
for (unsigned int ic = 0; ic < mconstraints.size(); ic++)
{
if (mconstraints[ic]->IsActive())
mconstraints[ic]->Increment_q( mconstraints[ic]->Get_l_i() );
}
if (verbose) GetLog() <<"-----\n";
current_residual = lastgoodres;
return lastgoodres;
}