예제 #1
0
double ChFunction_Operation::Get_y      (double x)
{
	double res;

	switch (op_type)
	{
	case ChOP_ADD:
		res = fa->Get_y(x) + fb->Get_y(x); break;
	case ChOP_SUB:
		res = fa->Get_y(x) - fb->Get_y(x); break;
	case ChOP_MUL:
		res = fa->Get_y(x) * fb->Get_y(x); break;
	case ChOP_DIV:
		res = fa->Get_y(x) / fb->Get_y(x); break;
	case ChOP_POW:
		res = pow (fa->Get_y(x), fb->Get_y(x)); break;
	case ChOP_MAX:
		res = ChMax (fa->Get_y(x), fb->Get_y(x)); break;
	case ChOP_MIN:
		res = ChMin (fa->Get_y(x), fb->Get_y(x)); break;
	case ChOP_MODULO:
		res = fmod (fa->Get_y(x), fb->Get_y(x)); break;
	case ChOP_FABS :
		res = fabs (fa->Get_y(x)); break;
	case ChOP_FUNCT :
		res = fa->Get_y(fb->Get_y(x)); break;
	default:
		res = 0; break;
	}
	return res;
}
예제 #2
0
/*
double ChFunction_Operation::Get_y_dx   (double x)
{
	double res = 0;
	res = ChFunction::Get_y_dx(x); // default: numerical differentiation
	return res;
}

double ChFunction_Operation::Get_y_dxdx (double x)
{
	double res = 0;
	res = ChFunction::Get_y_dxdx(x); // default: numerical differentiation
	return res;
}
*/
void ChFunction_Operation::Extimate_x_range (double& xmin, double& xmax)
{
	double amin, amax, bmin, bmax;
	fa->Extimate_x_range(amin,amax);
	fb->Extimate_x_range(bmin,bmax);
	xmin = ChMin(amin, bmin);
	xmax = ChMax(amax, bmax);
}
예제 #3
0
void ChLinkDistance::ConstraintsBiLoad_C(double factor, double recovery_clamp, bool do_clamp) {
    if (!IsActive())
        return;

    if (do_clamp)
        Cx.Set_b_i(Cx.Get_b_i() + ChMin(ChMax(factor * (curr_dist - distance), -recovery_clamp), recovery_clamp));
    else
        Cx.Set_b_i(Cx.Get_b_i() + factor * (curr_dist - distance));
}
예제 #4
0
void ChLinkDistance::IntLoadConstraint_C(const unsigned int off_L,  ///< offset in Qc residual
                                         ChVectorDynamic<>& Qc,     ///< result: the Qc residual, Qc += c*C
                                         const double c,            ///< a scaling factor
                                         bool do_clamp,             ///< apply clamping to c*C?
                                         double recovery_clamp      ///< value for min/max clamping of c*C
                                         ) {
    if (!IsActive())
        return;

    if (do_clamp)
        Qc(off_L) += ChMin(ChMax(c * (curr_dist - distance), -recovery_clamp), recovery_clamp);
    else
        Qc(off_L) += c * (curr_dist - distance);
}
예제 #5
0
void ChShaftsMotorSpeed::ConstraintsBiLoad_C(double factor, double recovery_clamp, bool do_clamp) {

    double C;
    if (this->avoid_angle_drift) 
        C = this->GetMotorRot()  - aux_dt - this->rot_offset; 
    else
        C = 0.0;

    double res = factor * C;

    if (do_clamp) {
        res = ChMin(ChMax(res, -recovery_clamp), recovery_clamp);
    }

    constraint.Set_b_i(constraint.Get_b_i() + res);
}
예제 #6
0
void ChShaftsMotor::IntLoadConstraint_C(const unsigned int off_L,  // offset in Qc residual
                                        ChVectorDynamic<>& Qc,     // result: the Qc residual, Qc += c*C
                                        const double c,            // a scaling factor
                                        bool do_clamp,             // apply clamping to c*C?
                                        double recovery_clamp      // value for min/max clamping of c*C
                                        ) {
    if (motor_mode != MOT_MODE_TORQUE) {
        double res;

        if (motor_mode == MOT_MODE_SPEED)
            res = 0;  // no need to stabilize positions

        if (motor_mode == MOT_MODE_ROTATION)
            res = c * (GetMotorRot() - motor_set_rot);

        if (do_clamp) {
            res = ChMin(ChMax(res, -recovery_clamp), recovery_clamp);
        }

        Qc(off_L) += res;
    }
}
예제 #7
0
void ChShaftsMotorSpeed::IntLoadConstraint_C(const unsigned int off_L,  // offset in Qc residual
                                        ChVectorDynamic<>& Qc,     // result: the Qc residual, Qc += c*C
                                        const double c,            // a scaling factor
                                        bool do_clamp,             // apply clamping to c*C?
                                        double recovery_clamp      // value for min/max clamping of c*C
                                        ) {
    // Add the time-dependent term in residual C as 
    //   C = d_error - d_setpoint - d_offset
    // with d_error = x_pos_A-x_pos_B, and d_setpoint = x(t)
    double C;
    if (this->avoid_angle_drift) 
        C = this->GetMotorRot()  - aux_dt - this->rot_offset; 
    else
        C = 0.0;

    double res = c * C;

    if (do_clamp) {
        res = ChMin(ChMax(res, -recovery_clamp), recovery_clamp);
    }
    Qc(off_L) += res;
}
예제 #8
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;

}
예제 #9
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;

}
예제 #10
0
void ChNodeSPH::SetCollisionRadius(double mr) {
    coll_rad = mr;
    double aabb_rad = h_rad / 2;  // to avoid too many pairs: bounding boxes hemisizes will sum..  __.__--*--
    ((ChModelBullet*)collision_model)->SetSphereRadius(coll_rad, ChMax(0.0, aabb_rad - coll_rad));
}
예제 #11
0
int ChIntegrator::ODEintegrate_step (
					int n_eq,				// number of equations
					ChMatrix<>* Y,				// the current state (also will contain resulting state after integration)
					double& t,				// the starting time (also will contain final time ater integration)
					int getdy(ChMatrix<>* m_Y, double m_t, ChMatrix<>* m_dYdt, void* m_userdata), // function which computes dy/dt=f(y,t), returning TRUE if OK, FALSE if error
					double h,				// step lenght.
					double& int_error,		// if possible, returns here the local error extimation
					void* userdata)			// additional user data
{
	//
	// initialize stuff..
	//

	int_error = 0.0;
	
	int i;
	int m_ok = TRUE;

	double a2=0.2,a3=0.3,a4=0.6,a5=1.0,a6=0.875,b21=0.2,
		b31=3.0/40.0,b32=9.0/40.0,b41=0.3,b42 = -0.9,b43=1.2,
		b51 = -11.0/54.0, b52=2.5,b53 = -70.0/27.0,b54=35.0/27.0,
		b61=1631.0/55296.0,b62=175.0/512.0,b63=575.0/13824.0,
		b64=44275.0/110592.0,b65=253.0/4096.0,c1=37.0/378.0,
		c3=250.0/621.0,c4=125.0/594.0,c6=512.0/1771.0,
		dc5 = -277.00/14336.0;
	double dc1=c1-2825.0/27648.0,dc3=c3-18575.0/48384.0,
		dc4=c4-13525.0/55296.0,dc6=c6-0.25;

	//
	// get number of equations...
	//

	int nequations = Y->GetRows();
	if (nequations != n_eq) 
				return FALSE;

	//
	// Initialize and reset temporary vectors, to match number
	// of equations
	//

	ChMatrixDynamic<> Ydt;
	ChMatrixDynamic<> Ytemp;
	ChMatrixDynamic<> Ynew;
	ChMatrixDynamic<> Yk1;
	ChMatrixDynamic<> Yk2;
	ChMatrixDynamic<> Yk3;
	ChMatrixDynamic<> Yk4;
	ChMatrixDynamic<> Yk5;
	ChMatrixDynamic<> Yk6;

	Ydt.Reset	(nequations, 1);
	Ytemp.Reset (nequations, 1);
	Yk1.Reset	(nequations, 1);
	Yk2.Reset	(nequations, 1);
	Yk3.Reset	(nequations, 1);
	Yk4.Reset	(nequations, 1);
	Yk5.Reset	(nequations, 1);
	Yk6.Reset	(nequations, 1);
	Ynew.Reset	(nequations, 1);


	// 
	// Perform step integration
	//

	switch (this->method)
	{


	case INTEG_EULEROSTD_EXP:

				// 1-- dY/dt   at current time: t0 

		m_ok=(*getdy)(Y, t,			&Yk1, userdata);	// >>>>>>>>>>>>>>>>>>>>>>> Yk1

		Yk1.MatrScale(h);

		Ynew.MatrAdd (*Y, Yk1);
		
		break;



	case INTEG_EULEROMOD_EXP:

				// 1- Yk1 = Ydt (Y, t0)
		m_ok=(*getdy)(Y, t,		&Yk1, userdata);		// >>>>>>>>>>>>>>>>>>>>>>> Yk1


				// 2- Yk2 = Ydt (Y+(dt/2)*Ydt,  t+dt/2)
		for (i = 0; i < nequations; i++)
			Ytemp.SetElement(i,0, 
						Y->GetElement(i,0) +
						h*0.5* Ydt.GetElement(i,0)  );

		m_ok=(*getdy)(&Ytemp, t+h/2,	&Yk2, userdata);	// >>>>>>>>>>>>>>>>>>>>>>> Yk2 


				// 3- Ynew = Y + (Yk2 * dt)
		for (i = 0; i < nequations; i++)
			Ynew.SetElement(i,0, 
						Y->GetElement(i,0) +
						h* Yk2.GetElement(i,0)  );
	
		break;



	case INTEG_HEUN_EXP:

				// 1- Yk1 = Ydt (Y, t0)
		m_ok=(*getdy)(Y, t,		&Yk1, userdata);			// >>>>>>>>>>>>>>>>>>>>>>> Yk1


				// 2- Yk2 = Ydt (Y+ dt*Ydt,  t+dt)
		for (i = 0; i < nequations; i++)
			Ytemp.SetElement(i,0, 
						Y->GetElement(i,0) +
						h * Yk1.GetElement(i,0)  );

		m_ok=(*getdy)(&Ytemp, t+h,	&Yk2, userdata);		// >>>>>>>>>>>>>>>>>>>>>>> Yk2 

				// 3- Ynew= Y + (Yk1 * dt/2) + (Yk1 * dt/2)
		for (i = 0; i < nequations; i++)
			Ynew.SetElement(i,0, 
						Y->GetElement(i,0) +
						h*0.5* Yk1.GetElement(i,0) +
						h*0.5* Yk2.GetElement(i,0) );

		break;



	case INTEG_KUTTA_EXP:

		// 1-- dY/dt   at current time: t0 
		m_ok=(*getdy)(Y, t,			&Yk1, userdata);	// >>>>>>>>>>>>>>>>>>>>>>> Yk1

		// 2-- dY/dt   at time: t0 +a2*h
		for (i = 0; i < nequations; i++)
			Ytemp.SetElement(i,0, 
						Y->GetElement(i,0) +
						b21*h* Yk1.GetElement(i,0)  );

		m_ok=(*getdy)(&Ytemp, t +a2*h,	&Yk2, userdata);	// >>>>>>>>>>>>>>>>>>>>>>> Yk2

		// 3-- dY/dt   at time: t0 +a3*h
		for (i = 0; i < nequations; i++)
			Ytemp.SetElement(i,0, 
						Y->GetElement(i,0) +
						h*( b31*Yk1.GetElement(i,0) +
						    b32*Yk2.GetElement(i,0)  )   );

		m_ok=(*getdy)(&Ytemp, t +a3*h,	&Yk3, userdata);	// >>>>>>>>>>>>>>>>>>>>>>> Yk3


		// 4-- dY/dt   at time: t0 +a4*h
		for (i = 0; i < nequations; i++)
			Ytemp.SetElement(i,0, 
						Y->GetElement(i,0) +
						h*( b41*Yk1.GetElement(i,0) +
						    b42*Yk2.GetElement(i,0) +
							b43*Yk3.GetElement(i,0)  )   );
		
		m_ok=(*getdy)(&Ytemp, t +a4*h,	&Yk4, userdata);	// >>>>>>>>>>>>>>>>>>>>>>> Yk4
		

		// 5-- dY/dt   at time: t0 +a5*h
		for (i = 0; i < nequations; i++)
			Ytemp.SetElement(i,0, 
						Y->GetElement(i,0) +
						h*( b51*Yk1.GetElement(i,0) +
						    b52*Yk2.GetElement(i,0) +
						    b53*Yk3.GetElement(i,0) +
							b54*Yk4.GetElement(i,0)  )   );

		m_ok=(*getdy)(&Ytemp, t +a5*h,	&Yk5, userdata);	// >>>>>>>>>>>>>>>>>>>>>>> Yk5


		// 6-- dY/dt   at time: t0 +a6*h
		for (i = 0; i < nequations; i++)
			Ytemp.SetElement(i,0, 
						Y->GetElement(i,0) +
						h*( b61*Yk1.GetElement(i,0) +
						    b62*Yk2.GetElement(i,0) +
						    b63*Yk3.GetElement(i,0) +
							b64*Yk4.GetElement(i,0) +
							b65*Yk5.GetElement(i,0)  )   );

		m_ok=(*getdy)(&Ytemp, t +a6*h,	&Yk6, userdata);	// >>>>>>>>>>>>>>>>>>>>>>> Yk6


		// Ynew = ..  acumulate to set incremented new Y state 
		for (i = 0; i < nequations; i++)
			Ynew.SetElement(i,0,
						Y->GetElement(i,0) +
						h*( c1*Yk1.GetElement(i,0) +
							c3*Yk3.GetElement(i,0) +
							c4*Yk4.GetElement(i,0) +
							c6*Yk6.GetElement(i,0)	)	);

				// -- compute local error
		for (i = 0; i < nequations; i++)
			int_error += ChMax(int_error, fabs(
					    h*( dc1*Yk1.GetElement(i,0) +
							dc3*Yk3.GetElement(i,0) +
							dc4*Yk4.GetElement(i,0) +
							dc5*Yk5.GetElement(i,0) +
							dc6*Yk6.GetElement(i,0)	 )    
							));
		break;



	case INTEG_RK_EXP:

		// 1-- dY/dt   at current time: t0 

		m_ok=(*getdy)(Y, t,			&Yk1, userdata);	// >>>>>>>>>>>>>>>>>>>>>>> Yk1


		// 2-- dY/dt   at time: t0 + h/2
		for (i = 0; i < nequations; i++)
			Ytemp.SetElement(i,0, 
						Y->GetElement(i,0) +
						0.5 * h * Yk1.GetElement(i,0)  );

		m_ok=(*getdy)(&Ytemp, t +h/2,	&Yk2, userdata);	// >>>>>>>>>>>>>>>>>>>>>>> Yk2


		// 3-- dY/dt   at time: t0 + h/2
		for (i = 0; i < nequations; i++)
			Ytemp.SetElement(i,0, 
						Y->GetElement(i,0) +
						0.5 * h * Yk2.GetElement(i,0)  );

		m_ok=(*getdy)(&Ytemp, t +h/2,	&Yk3, userdata);	// >>>>>>>>>>>>>>>>>>>>>>> Yk3


		// 4-- dY/dt   at time: t0 + h
		for (i = 0; i < nequations; i++)
			Ytemp.SetElement(i,0, 
						Y->GetElement(i,0) +
							  h * Yk3.GetElement(i,0)  );
				
		m_ok=(*getdy)(&Ytemp, t +h,		&Yk4, userdata);	// >>>>>>>>>>>>>>>>>>>>>>> Yk4



		// Ynew = ... acumulate to set incremented new Y state
		for (i = 0; i < nequations; i++)
			Ynew.SetElement(i,0,
						Y->GetElement(i,0) +
						h*( (1.0/6.0)*Yk1.GetElement(i,0) +
							(1.0/3.0)*Yk2.GetElement(i,0) +
							(1.0/3.0)*Yk3.GetElement(i,0) +
							(1.0/6.0)*Yk4.GetElement(i,0)	)	);

		break;





	default:
		
		m_ok = FALSE;
		break;
	}

	//
	// Go forward.. (UPDATE TIME AND STATE)
	//

	Y->CopyFromMatrix(Ynew);
	t = t+h;


	return (m_ok);
}
예제 #12
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;

}