Exemplo n.º 1
0
void ChShaftsMotor::InjectConstraints(ChLcpSystemDescriptor& mdescriptor)
{
	//if (!this->IsActive())
	//	return;
	if (motor_mode != MOT_MODE_TORQUE)
		mdescriptor.InsertConstraint(&constraint);
}
Exemplo n.º 2
0
void ChLinkDistance::InjectConstraints(ChLcpSystemDescriptor& mdescriptor)
{
	if (!this->IsActive())
		return;

	mdescriptor.InsertConstraint(&Cx); 
}
Exemplo n.º 3
0
double ChLcpSolverDEM::Solve(
					ChLcpSystemDescriptor& sysd,		///< system description with constraints and variables	
					bool add_Mq_to_f 
					)
{
	std::vector<ChLcpConstraint*>& mconstraints = sysd.GetConstraintsList();
	std::vector<ChLcpVariables*>&  mvariables	= sysd.GetVariablesList();

	double maxviolation = 0.;
	double maxdeltalambda = 0.;

	// 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();

	// 2)  Compute, for all items with variables, the initial guess for
	//     still unconstrained system:

	for (unsigned int iv = 0; iv< mvariables.size(); iv++)
	{
		if (mvariables[iv]->IsActive())
		{
			if (add_Mq_to_f)
			{
				mvariables[iv]->Compute_inc_invMb_v(mvariables[iv]->Get_qb(), mvariables[iv]->Get_fb()); // q = q_old + [M]'*fb
			}
			else
			{
				mvariables[iv]->Compute_invMb_v(mvariables[iv]->Get_qb(), mvariables[iv]->Get_fb()); // q = [M]'*fb
			}
		}
	}

	// 3)  For all items with variables, add the effect of initial (guessed)
	//     lagrangian reactions of contraints, if a warm start is desired.
	//     Otherwise, if no warm start, simply resets initial lagrangians to zero.
	if (warm_start)
	{
		for (unsigned int ic = 0; ic< mconstraints.size(); ic++)
			if (mconstraints[ic]->IsActive())
				mconstraints[ic]->Increment_q(mconstraints[ic]->Get_l_i());
	}
	else
	{
		for (unsigned int ic = 0; ic< mconstraints.size(); ic++)
			mconstraints[ic]->Set_l_i(0.);
	}

	// 4)  Perform the iteration loops
	//


	for (int iter = 0; iter < max_iterations; iter++)
	{
		// The iteration on all constraints
		//

		maxviolation = 0;
		maxdeltalambda = 0;

		for (unsigned int ic = 0; ic < mconstraints.size(); ic++)
		{
			// skip computations if constraint not active.
			if (mconstraints[ic]->IsActive())
			{
				// compute residual  c_i = [Cq_i]*q + b_i
				double mresidual = mconstraints[ic]->Compute_Cq_q() + mconstraints[ic]->Get_b_i();

				// true constraint violation may be different from 'mresidual' (ex:clamped if unilateral)
				double candidate_violation = fabs(mconstraints[ic]->Violation(mresidual));

				// compute:  delta_lambda = -(omega/g_i) * ([Cq_i]*q + b_i )
				double deltal = ( omega / mconstraints[ic]->Get_g_i() ) *
								( -mresidual );

				// update:   lambda += delta_lambda;
				double old_lambda = mconstraints[ic]->Get_l_i();
				mconstraints[ic]->Set_l_i( old_lambda + deltal);

				// If new lagrangian multiplier does not satisfy inequalities, project
				// it into an admissible orthant (or, in general, onto an admissible set)
				mconstraints[ic]->Project();

				// After projection, the lambda may have changed a bit..
				double new_lambda = mconstraints[ic]->Get_l_i() ;

				// Apply the smoothing: lambda= sharpness*lambda_new_projected + (1-sharpness)*lambda_old
				if (this->shlambda!=1.0)
				{
					new_lambda = shlambda*new_lambda + (1.0-shlambda)*old_lambda;
					mconstraints[ic]->Set_l_i(new_lambda);
				}

				double true_delta = new_lambda - old_lambda;

				// For all items with variables, add the effect of incremented
				// (and projected) lagrangian reactions:
				mconstraints[ic]->Increment_q(true_delta);

				if (this->record_violation_history)
					maxdeltalambda = ChMax(maxdeltalambda, fabs(true_delta));


				maxviolation = ChMax(maxviolation, fabs(candidate_violation));

			}	// end IsActive()

		}	// end loop on constraints

		// For recording into violaiton history, if debugging
		if (this->record_violation_history)
			AtIterationEnd(maxviolation, maxdeltalambda, iter);

		// Terminate the loop if violation in constraints has been succesfully limited.
		if (maxviolation < tolerance)
			break;

	} // end iteration loop

	return maxviolation;

}
Exemplo n.º 4
0
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";

}
Exemplo n.º 5
0
void test_3()
{
	GetLog() << "\n-------------------------------------------------\n";
	GetLog() << "TEST: generic system with stiffness blocks \n\n";

	// Important: create a 'system descriptor' object that 
	// contains variables and constraints:

	ChLcpSystemDescriptor mdescriptor;

	// Now let's add variables, constraints and stiffness, as sparse data:

	mdescriptor.BeginInsertion();  // ----- system description 

		// Create C++ objects representing 'variables', set their M blocks
		// (the masses) and set their known terms 'fb'

	ChMatrix33<> minertia;
	minertia.FillDiag(6);

	ChLcpVariablesBodyOwnMass mvarA;
	mvarA.SetBodyMass(5);
	mvarA.SetBodyInertia(&minertia);
	mvarA.Get_fb().FillRandom(-3,5);

	ChLcpVariablesBodyOwnMass mvarB;
	mvarB.SetBodyMass(4);
	mvarB.SetBodyInertia(&minertia);
	mvarB.Get_fb().FillRandom(1,3);

	ChLcpVariablesBodyOwnMass mvarC;
	mvarC.SetBodyMass(5.5);
	mvarC.SetBodyInertia(&minertia);
	mvarC.Get_fb().FillRandom(-8,3);

	mdescriptor.InsertVariables(&mvarA);
	mdescriptor.InsertVariables(&mvarB);
	mdescriptor.InsertVariables(&mvarC);

		// Create two C++ objects representing 'constraints' between variables
		// and set the jacobian to random values; 
		// Also set cfm term (E diagonal = -cfm)

	ChLcpConstraintTwoBodies mca(&mvarA, &mvarB);
	mca.Set_b_i(3);
	mca.Get_Cq_a()->FillRandom(-1,1);
	mca.Get_Cq_b()->FillRandom(-1,1);
	mca.Set_cfm_i(0.2);

	ChLcpConstraintTwoBodies mcb(&mvarA, &mvarB);
	mcb.Set_b_i(5);
	mcb.Get_Cq_a()->FillRandom(-1,1);
	mcb.Get_Cq_b()->FillRandom(-1,1);
	mcb.Set_cfm_i(0.1);

	mdescriptor.InsertConstraint(&mca);
	mdescriptor.InsertConstraint(&mcb);


		// Create two C++ objects representing 'stiffness' between variables:

	ChLcpKstiffnessGeneric mKa;
	// set the affected variables (so this K is a 12x12 matrix, relative to 4 6x6 blocks)
	std::vector<ChLcpVariables*> mvarsa;
	mvarsa.push_back(&mvarA);
	mvarsa.push_back(&mvarB);
	mKa.SetVariables(mvarsa); 

	// just fill K with random values (but symmetric, by making a product of matr*matrtransposed)
	ChMatrixDynamic<> mtempA = *mKa.Get_K(); // easy init to same size of K
	mtempA.FillRandom(-0.3,0.3);
	ChMatrixDynamic<> mtempB;
	mtempB.CopyFromMatrixT(mtempA);
	*mKa.Get_K() = -mtempA*mtempB;

	mdescriptor.InsertKstiffness(&mKa);
	

	ChLcpKstiffnessGeneric mKb;
	// set the affected variables (so this K is a 12x12 matrix, relative to 4 6x6 blocks)
	std::vector<ChLcpVariables*> mvarsb;
	mvarsb.push_back(&mvarB);
	mvarsb.push_back(&mvarC);
	mKb.SetVariables(mvarsb); 

	*mKb.Get_K() =  *mKa.Get_K(); 
	
	mdescriptor.InsertKstiffness(&mKb);


	mdescriptor.EndInsertion();  // ----- system description ends here

 
	// SOLVE the problem with an iterative Krylov solver.
	// In this case we use a MINRES-like solver, that features
	// very good convergence, it supports indefinite cases (ex. 
	// redundant constraints) and also supports the presence
	// of ChStiffness blocks (other solvers cannot cope with this!)

	// .. create the solver 

	ChLcpIterativePMINRES msolver_mr(80,		// max iterations
									 false,		// don't use warm start
									 1e-12);	// termination tolerance
	
	// .. set optional parameters of solver
	msolver_mr.SetDiagonalPreconditioning(true);
	msolver_mr.SetVerbose(true);

	// .. solve the system (passing variables, constraints, stiffness
	//    blocks with the ChSystemDescriptor that we populated above)

	msolver_mr.Solve(mdescriptor);


	// .. optional: get the result as a single vector (it collects all q_i and l_i 
	//    solved values stored in variables and constraints), just for check.
	chrono::ChMatrixDynamic<double> mx;
	mdescriptor.FromUnknownsToVector(mx);		// x ={q,-l}



	// CHECK. Test if, with the solved x, we really have Z*x-d=0 ...
	// to this end do the multiplication with the special function
	// SystemProduct() that is 'sparse-friendly' and does not build Z explicitly:
	
	chrono::ChMatrixDynamic<double> md;
	mdescriptor.BuildDiVector(md);				// d={f;-b}

	chrono::ChMatrixDynamic<double> mZx;
	mdescriptor.SystemProduct(mZx, &mx);		// Zx = Z*x
	
	GetLog() << "CHECK: norm of solver residual: ||Z*x-d|| -------------------\n";
	GetLog() << (mZx - md).NormInf() << "\n";

	/*
	// Alternatively, instead of using FromUnknownsToVector, to fetch
	// result, you could just loop over the variables (q values) and 
	// over the constraints (l values), as already shown in previous examples:

	for (int im = 0; im < mdescriptor.GetVariablesList().size(); im++)
		GetLog() << "   " << mdescriptor.GetVariablesList()[im]->Get_qb()(0) << "\n";

	for (int ic = 0; ic < mdescriptor.GetConstraintsList().size(); ic++)
		GetLog() << "   " << mdescriptor.GetConstraintsList()[ic]->Get_l_i() << "\n"; 
	*/

	



	

}
Exemplo n.º 6
0
void test_2()
{
	
	GetLog() << "\n-------------------------------------------------\n";
	GetLog() << "TEST: 1D vertical pendulum - ChLcpIterativePMINRES \n\n";

	ChLcpSystemDescriptor mdescriptor;

	mdescriptor.BeginInsertion(); 	// ----- system description starts here
	
	int n_masses = 11;

	std::vector<ChLcpVariablesGeneric*> vars;
	std::vector<ChLcpConstraintTwoGeneric*> constraints;

	for (int im = 0; im < n_masses; im++)
	{
		vars.push_back (new ChLcpVariablesGeneric(1));
		vars[im]->GetMass()(0)=10;
		vars[im]->GetInvMass()(0)=1./vars[im]->GetMass()(0);
		vars[im]->Get_fb()(0)= -9.8*vars[im]->GetMass()(0)*0.01; 
		//if (im==5) vars[im]->Get_fb()(0)= 50;
		mdescriptor.InsertVariables(vars[im]);
		if (im>0)
		{
			constraints.push_back (new ChLcpConstraintTwoGeneric(vars[im], vars[im-1]) );
			constraints[im-1]->Set_b_i(0);
			constraints[im-1]->Get_Cq_a()->ElementN(0)=  1;
			constraints[im-1]->Get_Cq_b()->ElementN(0)= -1;
			//constraints[im-1]->SetMode(CONSTRAINT_UNILATERAL); // not supported by  ChLcpSimplexSolver
			mdescriptor.InsertConstraint(constraints[im-1]);
		}
	}
	
	// First variable of 1st domain is 'fixed' like in a hanging chain	
	vars[0]->SetDisabled(true);

	mdescriptor.EndInsertion();		// ----- system description is finished


			try
			{
				chrono::ChSparseMatrix mdM;
				chrono::ChSparseMatrix mdCq;
				chrono::ChSparseMatrix mdE;
				chrono::ChMatrixDynamic<double> mdf;
				chrono::ChMatrixDynamic<double> mdb;
				chrono::ChMatrixDynamic<double> mdfric;
				mdescriptor.ConvertToMatrixForm(&mdCq, &mdM, &mdE, &mdf, &mdb, &mdfric);
				chrono::ChStreamOutAsciiFile file_M("dump_M.dat");
				mdM.StreamOUTsparseMatlabFormat(file_M);
				chrono::ChStreamOutAsciiFile file_Cq("dump_Cq.dat");
				mdCq.StreamOUTsparseMatlabFormat(file_Cq);
				chrono::ChStreamOutAsciiFile file_E("dump_E.dat");
				mdE.StreamOUTsparseMatlabFormat(file_E);
				chrono::ChStreamOutAsciiFile file_f("dump_f.dat");
				mdf.StreamOUTdenseMatlabFormat(file_f);
				chrono::ChStreamOutAsciiFile file_b("dump_b.dat");
				mdb.StreamOUTdenseMatlabFormat(file_b);
				chrono::ChStreamOutAsciiFile file_fric("dump_fric.dat");
				mdfric.StreamOUTdenseMatlabFormat(file_fric);
			} 
			catch(chrono::ChException myexc)
			{
				chrono::GetLog() << myexc.what();
			}

	// Create a solver of Krylov type
	ChLcpIterativePMINRES msolver_krylov(20,		// max iterations
										false,		// warm start
										0.00001);	// tolerance  


	// .. pass the constraint and the variables to the solver 
	//    to solve - that's all.
	msolver_krylov.Solve(mdescriptor);

	// Output values
	GetLog() << "VARIABLES: \n";
	for (int im = 0; im < vars.size(); im++)
		GetLog() << "   " << vars[im]->Get_qb()(0) << "\n";

	GetLog() << "CONSTRAINTS: \n";
	for (int ic = 0; ic < constraints.size(); ic++)
		GetLog() << "   " << constraints[ic]->Get_l_i() << "\n"; 


	// Try again, for reference, using a direct solver.
	// This type of 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.

	GetLog() << "\n\nTEST: 1D vertical pendulum - ChLcpSimplexSolver \n\n";

	ChLcpSimplexSolver msolver_simpl;
	msolver_simpl.Solve(mdescriptor);

	GetLog() << "VARIABLES: \n";
	for (int im = 0; im < vars.size(); im++)
		GetLog() << "   " << vars[im]->Get_qb()(0) << "\n";

	GetLog() << "CONSTRAINTS: \n";
	for (int ic = 0; ic < constraints.size(); ic++)
		GetLog() << "   " << constraints[ic]->Get_l_i() << "\n"; 

}
Exemplo n.º 7
0
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;

}
Exemplo n.º 8
0
void ChMesh::InjectVariables(ChLcpSystemDescriptor& mdescriptor)
{
	for (unsigned int ie = 0; ie < this->vnodes.size(); ie++)
		mdescriptor.InsertVariables(&this->vnodes[ie]->Variables());
}
int main(int argc,
         char* argv[]) {
   omp_set_num_threads(8);
   ChSystemParallelDVI * system_gpu = new ChSystemParallelDVI;
   system_gpu->SetIntegrationType(ChSystem::INT_ANITESCU);

   std::stringstream ss;
   ss << "container_checkpoint_50_settled.txt";

   ChCollisionSystemBulletParallel * bullet_coll = new ChCollisionSystemBulletParallel();
   system_gpu->ChangeCollisionSystem(bullet_coll);
   system_gpu->SetStep(timestep);
   system_gpu->SetMaxPenetrationRecoverySpeed(10000);
   utils::ReadCheckpoint(system_gpu, ss.str());
   system_gpu->AssembleSystem();
   ChContactContainer* container = (ChContactContainer *) system_gpu->GetContactContainer();

   std::vector<ChLcpConstraint*>& mconstraints = system_gpu->GetLcpSystemDescriptor()->GetConstraintsList();
   std::vector<ChLcpVariables*>& mvariables = system_gpu->GetLcpSystemDescriptor()->GetVariablesList();
   size_t nOfVars = mvariables.size();
   size_t nOfConstraints = mconstraints.size();

   ChMatrixDynamic<> mb(nOfConstraints, 1);
   DynamicVector<double> rhs_vector;

//#########
   for (unsigned int ic = 0; ic < nOfConstraints; 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 < nOfConstraints; 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;
         }
      }
   }

   for (unsigned int iv = 0; iv < nOfVars; iv++) {
      if (mvariables[iv]->IsActive()) {
         mvariables[iv]->Compute_invMb_v(mvariables[iv]->Get_qb(), mvariables[iv]->Get_fb());  // q = [M]'*fb
      }
   }
   ChMatrixDynamic<> mb_tmp(nOfConstraints, 1);
   int s_i = 0;
   for (unsigned int ic = 0; ic < nOfConstraints; ic++)
      if (mconstraints[ic]->IsActive()) {
         mb(s_i, 0) = -mconstraints[ic]->Compute_Cq_q();
         ++s_i;
      }
   system_gpu->GetLcpSystemDescriptor()->BuildBiVector(mb_tmp);   // b_i   =   -c   = phi/h
   mb.MatrDec(mb_tmp);
   ChLcpSystemDescriptor* sysd = system_gpu->GetLcpSystemDescriptor();

   ChTimer<double> timer;
   timer.start();

//#########
   double max_iterations = 212;
   double SIZE = nOfConstraints;
   double lastgoodres = 10e30;
   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;

   ChMatrixDynamic<> ml(nOfConstraints, 1);
   ChMatrixDynamic<> ml_candidate(nOfConstraints, 1);
   ChMatrixDynamic<> mg(nOfConstraints, 1);
   ChMatrixDynamic<> mg_tmp(nOfConstraints, 1);
   ChMatrixDynamic<> my(nOfConstraints, 1);
   ChMatrixDynamic<> mx(nOfConstraints, 1);
   ChMatrixDynamic<> mg_tmp1(nOfConstraints, 1);
   ChMatrixDynamic<> mg_tmp2(nOfConstraints, 1);
   ChMatrixDynamic<> ms(nOfConstraints, 1);
   ml.FillElem(0);
   sysd->ConstraintsProject(ml);
   ml_candidate = ml;
   sysd->ShurComplementProduct(mg, &ml, 0);
   mg = mg - mb;

   mb_tmp.FillElem(-1.0);
   mb_tmp += 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.0 / L_k;

   double obj1 = 0;
   double obj2 = 0;

   my = ml;
   mx = ml;

   for (int iter = 0; iter < max_iterations; iter++) {
      sysd->ShurComplementProduct(mg_tmp1, &my, 0);
      mg = mg_tmp1 - mb;
      mx = mg * -t_k + my;
      sysd->ConstraintsProject(mx);
      sysd->ShurComplementProduct(mg_tmp, &mx, 0);
      mg_tmp2 = mg_tmp - mb;
      ms = mg_tmp * 0.5 - mb;
      obj1 = mx.MatrDot(&mx, &ms);
      ms = mg_tmp1 * 0.5 - mb;
      obj2 = my.MatrDot(&my, &ms);

      ms = mx - my;
      while (obj1 > obj2 + mg.MatrDot(&mg, &ms) + 0.5 * L_k * pow(ms.NormTwo(), 2.0)) {
         L_k = 2.0 * L_k;
         t_k = 1.0 / L_k;

         mx = mg * -t_k + my;
         sysd->ConstraintsProject(mx);
         sysd->ShurComplementProduct(mg_tmp, &mx, 0);
         mg_tmp2 = mg_tmp - mb;
         ms = mg_tmp * 0.5 - mb;
         obj1 = mx.MatrDot(&mx, &ms);
         ms = mx - my;
      }

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

      ms = (mx - ml);
      my = ms * beta_k1 + mx;

      if (mg.MatrDot(&mg, &ms) > 0) {
         my = mx;
         theta_k1 = 1.0;
      }

      L_k = 0.9 * L_k;
      t_k = 1.0 / L_k;

      ml = mx;
      theta_k = theta_k1;

      //========
      ChMatrixDynamic<> testres(nOfConstraints, 1);
      sysd->ShurComplementProduct(testres, &ml, 0);
      testres = testres - mb;

      double gdiff = .1;

      ChMatrixDynamic<> inside = ml - testres * gdiff;
      sysd->ConstraintsProject(inside);

      ChMatrixDynamic<> resid = (ml - inside) * (1.0 / nOfConstraints * gdiff);
      double g_proj_norm = resid.NormTwo();

      //========
      if (g_proj_norm < lastgoodres) {
         lastgoodres = g_proj_norm;
         ml_candidate = ml;
      }
      //========
      // 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 = mg_tmp * 0.5 - mb;                                   // 2)  g_tmp = 0.5*N*l_candidate                                    // 3)  g_tmp = 0.5*N*l_candidate-b_shur
      double m_objective = 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;
      //cout << "  iter=" << iter << "   f=" << m_objective << "  |d|=" << maxd << "  |s|=" << maxdeltalambda << "\n";
   }
   ml = ml_candidate;
   timer.stop();
   std::cout << timer() <<  std::endl;

//#########
   std::cout << system_gpu->GetNbodies() << " " << container->GetNcontacts() <<  std::endl;

   return 0;

}
Exemplo n.º 10
0
double ChLcpIterativeSOR::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.;
	double maxdeltalambda = 0.;
	int i_friction_comp = 0;
	double old_lambda_friction[3];


	// 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.
	//
	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;
			}
		}	
	}


	// 2)  Compute, for all items with variables, the initial guess for
	//     still unconstrained system:

	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 


	// 3)  For all items with variables, add the effect of initial (guessed)
	//     lagrangian reactions of contraints, if a warm start is desired.
	//     Otherwise, if no warm start, simply resets initial lagrangians to zero.
	if (warm_start)
	{
		for (unsigned int ic = 0; ic< mconstraints.size(); ic++)
			if (mconstraints[ic]->IsActive())
				mconstraints[ic]->Increment_q(mconstraints[ic]->Get_l_i());
	}
	else
	{
		for (unsigned int ic = 0; ic< mconstraints.size(); ic++)
			mconstraints[ic]->Set_l_i(0.);
	}

	// 4)  Perform the iteration loops
	//


	for (int iter = 0; iter < max_iterations; iter++)
	{
		// The iteration on all constraints
		//

		maxviolation = 0;
		maxdeltalambda = 0;
		i_friction_comp = 0;

		for (unsigned int ic = 0; ic < mconstraints.size(); ic++)
		{
			// skip computations if constraint not active.
			if (mconstraints[ic]->IsActive())
			{
				// compute residual  c_i = [Cq_i]*q + b_i + cfm_i*l_i
				double mresidual = mconstraints[ic]->Compute_Cq_q() + mconstraints[ic]->Get_b_i()
								 + mconstraints[ic]->Get_cfm_i() * mconstraints[ic]->Get_l_i();

				// true constraint violation may be different from 'mresidual' (ex:clamped if unilateral)
				double candidate_violation = fabs(mconstraints[ic]->Violation(mresidual));

				// compute:  delta_lambda = -(omega/g_i) * ([Cq_i]*q + b_i + cfm_i*l_i )
				double deltal = ( omega / mconstraints[ic]->Get_g_i() ) *
								( -mresidual );

				if (mconstraints[ic]->GetMode() == CONSTRAINT_FRIC)
				{
					candidate_violation = 0;

					// update:   lambda += delta_lambda;
					old_lambda_friction[i_friction_comp] = mconstraints[ic]->Get_l_i();
					mconstraints[ic]->Set_l_i( old_lambda_friction[i_friction_comp]  + deltal);
					i_friction_comp++;
					
					if (i_friction_comp==1)
						candidate_violation = fabs(ChMin(0.0,mresidual));

					if (i_friction_comp==3)
					{ 
						mconstraints[ic-2]->Project(); // the N normal component will take care of N,U,V
						double new_lambda_0 = mconstraints[ic-2]->Get_l_i() ;
						double new_lambda_1 = mconstraints[ic-1]->Get_l_i() ;
						double new_lambda_2 = mconstraints[ic-0]->Get_l_i() ;
						// Apply the smoothing: lambda= sharpness*lambda_new_projected + (1-sharpness)*lambda_old
						if (this->shlambda!=1.0)
						{
							new_lambda_0 = shlambda*new_lambda_0 + (1.0-shlambda)*old_lambda_friction[0];
							new_lambda_1 = shlambda*new_lambda_1 + (1.0-shlambda)*old_lambda_friction[1];
							new_lambda_2 = shlambda*new_lambda_2 + (1.0-shlambda)*old_lambda_friction[2];
							mconstraints[ic-2]->Set_l_i(new_lambda_0);
							mconstraints[ic-1]->Set_l_i(new_lambda_1);
							mconstraints[ic-0]->Set_l_i(new_lambda_2);
						}
						double true_delta_0 = new_lambda_0 - old_lambda_friction[0];
						double true_delta_1 = new_lambda_1 - old_lambda_friction[1];
						double true_delta_2 = new_lambda_2 - old_lambda_friction[2];
						mconstraints[ic-2]->Increment_q(true_delta_0);
						mconstraints[ic-1]->Increment_q(true_delta_1);
						mconstraints[ic-0]->Increment_q(true_delta_2);
						
						if (this->record_violation_history)
						{
							maxdeltalambda = ChMax(maxdeltalambda, fabs(true_delta_0));
							maxdeltalambda = ChMax(maxdeltalambda, fabs(true_delta_1));
							maxdeltalambda = ChMax(maxdeltalambda, fabs(true_delta_2));
						}
						i_friction_comp =0;
					}
				} 
				else
				{
					// update:   lambda += delta_lambda;
					double old_lambda = mconstraints[ic]->Get_l_i();
					mconstraints[ic]->Set_l_i( old_lambda + deltal);

					// If new lagrangian multiplier does not satisfy inequalities, project
					// it into an admissible orthant (or, in general, onto an admissible set)
					mconstraints[ic]->Project();

					// After projection, the lambda may have changed a bit..
					double new_lambda = mconstraints[ic]->Get_l_i() ;

					// Apply the smoothing: lambda= sharpness*lambda_new_projected + (1-sharpness)*lambda_old
					if (this->shlambda!=1.0)
					{
						new_lambda = shlambda*new_lambda + (1.0-shlambda)*old_lambda;
						mconstraints[ic]->Set_l_i(new_lambda);
					}

					double true_delta = new_lambda - old_lambda;

					// For all items with variables, add the effect of incremented
					// (and projected) lagrangian reactions:
					mconstraints[ic]->Increment_q(true_delta);

					if (this->record_violation_history)
						maxdeltalambda = ChMax(maxdeltalambda, fabs(true_delta)); 
				}

				maxviolation = ChMax(maxviolation, fabs(candidate_violation));

			}	// end IsActive()

		}	// end loop on constraints
 
		// For recording into violaiton history, if debugging
		if (this->record_violation_history)
			AtIterationEnd(maxviolation, maxdeltalambda, iter);

			tot_iterations++;
			// Terminate the loop if violation in constraints has been succesfully limited.
			if (maxviolation < tolerance)
				break;

	} // end iteration loop


	return maxviolation;

}
Exemplo n.º 11
0
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;

}
Exemplo n.º 12
0
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);

	}

}
Exemplo n.º 13
0
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;

}