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