void ChLcpKstiffnessGeneric::Build_K(ChSparseMatrix& storage, bool add)
{
if (!K)
return;
int kio =0;
for (unsigned int iv = 0; iv< this->GetNvars(); iv++)
{
int io = this->GetVariableN(iv)->GetOffset();
int in = this->GetVariableN(iv)->Get_ndof();
int kjo =0;
for (unsigned int jv = 0; jv< this->GetNvars(); jv++)
{
int jo = this->GetVariableN(jv)->GetOffset();
int jn = this->GetVariableN(jv)->Get_ndof();
if (add)
storage.PasteSumClippedMatrix(this->K, kio, kjo, in, jn, io,jo);
else
storage.PasteClippedMatrix (this->K, kio, kjo, in, jn, io,jo);
kjo += jn;
}
kio += in;
}
}
// Build the mass matrix (for these variables) scaled by c_a, storing
// it in 'storage' sparse matrix, at given column/row offset.
// Note, most iterative solvers don't need to know mass matrix explicitly.
// Optimized: doesn't fill unneeded elements except mass and 3x3 inertia.
void ChVariablesBodySharedMass::Build_M(ChSparseMatrix& storage, int insrow, int inscol, const double c_a) {
storage.SetElement(insrow + 0, inscol + 0, c_a * sharedmass->mass);
storage.SetElement(insrow + 1, inscol + 1, c_a * sharedmass->mass);
storage.SetElement(insrow + 2, inscol + 2, c_a * sharedmass->mass);
ChMatrix33<> scaledJ = sharedmass->inertia * c_a;
storage.PasteMatrix(scaledJ, insrow + 3, inscol + 3);
}
int ChSparseMatrix::DecomposeAndSolve_LDL_forLCP(ChMatrix<>* B, ChMatrix<>* X, double& mdet,
int i_D, int i_C, int n_unilaterals,
ChUnilateralData constr_data[], int from_eq)
{
ChSparseMatrix tempM;
tempM.CopyFromMatrix(this);
// deactivate the non-clamped or not yet considered unilateral constraints
for (int nd = 0; nd < n_unilaterals; nd++)
{
if (constr_data[nd].status != CONSTR_UNILATERAL_ON)
tempM.SetElement(i_D+nd, i_D+nd, INFINITE_PIVOT);
}
tempM.DecomposeAndSolve_LDL(B,X,mdet, from_eq);
return TRUE;
}
void test_1()
{
GetLog() << "\n-------------------------------------------------\n";
GetLog() << "TEST: generic system with two constraints \n\n";
// Important: create a 'system descriptor' object that
// contains variables and constraints:
ChLcpSystemDescriptor mdescriptor;
// Now let's add variables and constraints, as sparse data:
mdescriptor.BeginInsertion(); // ----- system description starts here
// create C++ objects representing 'variables':
ChLcpVariablesGeneric mvarA(3);
mvarA.GetMass().SetIdentity();
mvarA.GetMass()*=10;
ChLinearAlgebra::Invert(mvarA.GetInvMass(),&mvarA.GetMass());
mvarA.Get_fb()(0)=1;
mvarA.Get_fb()(1)=2;
ChLcpVariablesGeneric mvarB(3);
mvarB.GetMass().SetIdentity();
mvarB.GetMass()*=20;
ChLinearAlgebra::Invert(mvarB.GetInvMass(),&mvarB.GetMass());
mdescriptor.InsertVariables(&mvarA);
mdescriptor.InsertVariables(&mvarB);
// create C++ objects representing 'constraints' between variables:
ChLcpConstraintTwoGeneric mca(&mvarA, &mvarB);
mca.Set_b_i(-5);
mca.Get_Cq_a()->ElementN(0)=1;
mca.Get_Cq_a()->ElementN(1)=2;
mca.Get_Cq_a()->ElementN(2)=-1;
mca.Get_Cq_b()->ElementN(0)=1;
mca.Get_Cq_b()->ElementN(1)=-2;
mca.Get_Cq_b()->ElementN(2)=0;
ChLcpConstraintTwoGeneric mcb(&mvarA, &mvarB);
mcb.Set_b_i( 1);
mcb.Get_Cq_a()->ElementN(0)=0;
mcb.Get_Cq_a()->ElementN(1)=1;
mcb.Get_Cq_a()->ElementN(2)=0;
mcb.Get_Cq_b()->ElementN(0)=0;
mcb.Get_Cq_b()->ElementN(1)=-2;
mcb.Get_Cq_b()->ElementN(2)=0;
mdescriptor.InsertConstraint(&mca);
mdescriptor.InsertConstraint(&mcb);
mdescriptor.EndInsertion(); // ----- system description ends here
// Solve the problem with an iterative fixed-point solver, for an
// approximate (but very fast) solution:
//
// .. create the solver
ChLcpIterativeSOR msolver_iter( 1, // max iterations
false, // don't use warm start
0.0, // termination tolerance
0.8); // omega
// .. pass the constraint and the variables to the solver
// to solve - that's all.
msolver_iter.Solve(mdescriptor);
// Ok, now present the result to the user, with some
// statistical information:
double max_res, max_LCPerr;
mdescriptor.ComputeFeasabilityViolation(max_res, max_LCPerr);
// If needed, dump the full system M and Cq matrices
// on disk, in Matlab sparse format:
ChSparseMatrix matrM;
ChSparseMatrix matrCq;
mdescriptor.BuildMatrices(&matrCq, &matrM);
try
{
ChStreamOutAsciiFile fileM ("dump_M.dat");
ChStreamOutAsciiFile fileCq ("dump_Cq.dat");
matrM.StreamOUTsparseMatlabFormat(fileM);
matrCq.StreamOUTsparseMatlabFormat(fileCq);
}
catch (ChException myex)
{
GetLog() << "FILE ERROR: " << myex.what();
}
// Other checks
GetLog() << "**** Using ChLcpIterativeSOR ********** \n\n";
GetLog() << "METRICS: max residual: " << max_res << " max LCP error: " << max_LCPerr << " \n\n";
GetLog() << "vars q_a and q_b -------------------\n";
GetLog() << mvarA.Get_qb();
GetLog() << mvarB.Get_qb() << " \n";;
GetLog() << "multipliers l_1 and l_2 ------------\n\n";
GetLog() << mca.Get_l_i() << " \n";
GetLog() << mcb.Get_l_i() << " \n\n";
GetLog() << "constraint residuals c_1 and c_2 ---\n";
GetLog() << mca.Get_c_i() << " \n";
GetLog() << mcb.Get_c_i() << " \n\n\n";
// reset variables
mvarA.Get_qb().FillElem(0.);
mvarB.Get_qb().FillElem(0.);
// Now solve it again, but using the simplex solver.
// The simplex solver is much slower, and it cannot handle
// the case of unilateral constraints. This is reccomended
// only for reference or very precise solution of systems with only
// bilateral constraints, in a limited number.
ChLcpSimplexSolver msolver_simpl;
msolver_simpl.Solve(mdescriptor);
mdescriptor.ComputeFeasabilityViolation(max_res, max_LCPerr);
GetLog() << "**** Using ChLcpSimplexSolver ********* \n\n";
GetLog() << "METRICS: max residual: " << max_res << " max LCP error: " << max_LCPerr << " \n\n";
GetLog() << "vars q_a and q_b -------------------\n";
GetLog() << mvarA.Get_qb();
GetLog() << mvarB.Get_qb() << " \n";;
GetLog() << "multipliers l_1 and l_2 ------------\n\n";
GetLog() << mca.Get_l_i() << " \n";
GetLog() << mcb.Get_l_i() << " \n\n";
GetLog() << "constraint residuals c_1 and c_2 ---\n";
GetLog() << mca.Get_c_i() << " \n";
GetLog() << mcb.Get_c_i() << " \n";
}
// Build the mass matrix (for these variables) scaled by c_a, storing
// it in 'storage' sparse matrix, at given column/row offset.
// Note, most iterative solvers don't need to know mass matrix explicitly.
// Optimized: doesn't fill unneeded elements except mass.
void ChVariablesShaft::Build_M(ChSparseMatrix& storage, int insrow, int inscol, const double c_a) {
storage.SetElement(insrow + 0, inscol + 0, c_a * m_inertia);
}
// Build the mass matrix (for these variables) scaled by c_a, storing
// it in 'storage' sparse matrix, at given column/row offset.
// Note, most iterative solvers don't need to know mass matrix explicitly.
// Optimized: doesn't fill unneeded elements except mass.
void ChVariablesNode::Build_M(ChSparseMatrix& storage, int insrow, int inscol, const double c_a) {
double scaledmass = c_a * mass;
storage.SetElement(insrow + 0, inscol + 0, scaledmass);
storage.SetElement(insrow + 1, inscol + 1, scaledmass);
storage.SetElement(insrow + 2, inscol + 2, scaledmass);
}
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);
}
}
