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); } }