public override void IntLoadConstraint_C(int off_L,
ref ChVectorDynamic <double> Qc,
double c,
bool do_clamp,
double recovery_clamp)
{
int cnt = 0;
for (int i = 0; i < mask.nconstr; i++)
{
if (mask.Constr_N(i).IsActive())
{
if (do_clamp)
{
if (mask.Constr_N(i).IsUnilateral())
{
Qc.matrix[off_L + cnt] += ChMaths.ChMax(c * C.matrix.ElementN(cnt), -recovery_clamp);
}
else
{
Qc.matrix[off_L + cnt] += ChMaths.ChMin(ChMaths.ChMax(c * C.matrix.ElementN(cnt), -recovery_clamp), recovery_clamp);
}
}
else
{
Qc.matrix[off_L + cnt] += c * C.matrix.ElementN(cnt);
}
cnt++; // The Gremlin in the works was here! I had accidently locked this variable in the above else, causing joint jittering
}
}
}
public override void IntLoadConstraint_C(int off_L,
ref ChVectorDynamic <double> Qc,
double c,
bool do_clamp,
double recovery_clamp)
{
int cnt = 0;
for (int i = 0; i < mask.nconstr; i++)
{
if (mask.Constr_N(i).IsActive())
{
if (do_clamp)
{
if (mask.Constr_N(i).IsUnilateral())
{
Qc.matrix[off_L + cnt] += ChMaths.ChMax(c * C.matrix.ElementN(cnt), -recovery_clamp);
}
else
{
Qc.matrix[off_L + cnt] += ChMaths.ChMin(ChMaths.ChMax(c * C.matrix.ElementN(cnt), -recovery_clamp), recovery_clamp);
}
}
else
{
Qc.matrix[off_L + cnt] += c * C.matrix.ElementN(cnt);
}
cnt++;
}
}
}
public override void ConstraintsBiLoad_C(double factor = 1, double recovery_clamp = 0.1, bool do_clamp = false)
{
int cnt = 0;
for (int i = 0; i < mask.nconstr; i++)
{
if (mask.Constr_N(i).IsActive())
{
if (do_clamp)
{
if (mask.Constr_N(i).IsUnilateral())
{
mask.Constr_N(i).Set_b_i(mask.Constr_N(i).Get_b_i() +
ChMaths.ChMax(factor * C.matrix.ElementN(cnt), -recovery_clamp));
}
else
{
mask.Constr_N(i).Set_b_i(mask.Constr_N(i).Get_b_i() +
ChMaths.ChMin(ChMaths.ChMax(factor * C.matrix.ElementN(cnt), -recovery_clamp), recovery_clamp));
}
}
else
{
mask.Constr_N(i).Set_b_i(mask.Constr_N(i).Get_b_i() + factor * C.matrix.ElementN(cnt));
}
cnt++;
}
}
}
/// Fill the matrix with random float numbers, falling within the
/// "max"/"min" range.
public void FillRandom(double max, double min)
{
for (int i = 0; i < rows * columns; ++i)
{
SetElementN(i, min + ChMaths.ChRandom() * (max - min));
}
}
public override void ConstraintsBiLoad_C(double factor = 1, double recovery_clamp = 0.1, bool do_clamp = false)
{
if (!IsActive())
{
return;
}
if (do_clamp)
{
Cx.Set_b_i(Cx.Get_b_i() + ChMaths.ChMin(ChMaths.ChMax(factor * (curr_dist - distance), -recovery_clamp), recovery_clamp));
}
else
{
Cx.Set_b_i(Cx.Get_b_i() + factor * (curr_dist - distance));
}
}
public override void ConstraintsBiLoad_C(double factor = 1, double recovery_clamp = 0.1, bool do_clamp = false)
{
if (!this.IsActive())
{
return;
}
//***TEST***
/*
* GetLog()<< "cload: " ;
* if (this.c_x) GetLog()<< " x";
* if (this.c_y) GetLog()<< " y";
* if (this.c_z) GetLog()<< " z";
* if (this.c_rx) GetLog()<< " Rx";
* if (this.c_ry) GetLog()<< " Ry";
* if (this.c_rz) GetLog()<< " Rz";
* GetLog()<< *this.C << "\n";
*/
int cnt = 0;
for (int i = 0; i < mask.nconstr; i++)
{
if (mask.Constr_N(i).IsActive())
{
if (do_clamp)
{
if (mask.Constr_N(i).IsUnilateral())
{
mask.Constr_N(i).Set_b_i(mask.Constr_N(i).Get_b_i() +
ChMaths.ChMax(factor * C.matrix.ElementN(cnt), -recovery_clamp));
}
else
{
mask.Constr_N(i).Set_b_i(mask.Constr_N(i).Get_b_i() +
ChMaths.ChMin(ChMaths.ChMax(factor * C.matrix.ElementN(cnt), -recovery_clamp), recovery_clamp));
}
}
else
{
mask.Constr_N(i).Set_b_i(mask.Constr_N(i).Get_b_i() + factor * C.matrix.ElementN(cnt));
}
cnt++;
}
}
}
public override void ConstraintsBiLoad_C(double factor = 1, double recovery_clamp = 0.1, bool do_clamp = false)
{
if (!IsActive())
{
return;
}
double cnstr_dist_violation = do_clamp
? ChMaths.ChMin(ChMaths.ChMax(factor * (m_cur_dist - m_dist), -recovery_clamp), recovery_clamp)
: factor * (m_cur_dist - m_dist);
double cnstr_dot_violation =
do_clamp ? ChMaths.ChMin(ChMaths.ChMax(factor * m_cur_dot, -recovery_clamp), recovery_clamp) : factor * m_cur_dot;
m_cnstr_dist.Set_b_i(m_cnstr_dist.Get_b_i() + cnstr_dist_violation);
m_cnstr_dot.Set_b_i(m_cnstr_dot.Get_b_i() + cnstr_dot_violation);
}
public override void IntLoadConstraint_C(int off_L,
ref ChVectorDynamic <double> Qc,
double c,
bool do_clamp,
double recovery_clamp)
{
double res = 0; // no residual anyway! allow drifting...
double cnstr_violation = c * res;
if (do_clamp)
{
cnstr_violation = ChMaths.ChMin(ChMaths.ChMax(cnstr_violation, -recovery_clamp), recovery_clamp);
}
Qc.matrix[off_L] += cnstr_violation;
}
public override void IntLoadConstraint_C(int off_L,
ref ChVectorDynamic <double> Qc,
double c,
bool do_clamp,
double recovery_clamp)
{
if (!IsActive())
{
return;
}
double cnstr_dist_violation =
do_clamp ? ChMaths.ChMin(ChMaths.ChMax(c * (m_cur_dist - m_dist), -recovery_clamp), recovery_clamp) : c * (m_cur_dist - m_dist);
double cnstr_dot_violation =
do_clamp ? ChMaths.ChMin(ChMaths.ChMax(c * m_cur_dot, -recovery_clamp), recovery_clamp) : c * m_cur_dot;
Qc.matrix[off_L + 0] += cnstr_dist_violation;
Qc.matrix[off_L + 1] += cnstr_dot_violation;
}
public override void IntLoadConstraint_C(int off_L,
ref ChVectorDynamic <double> Qc,
double c,
bool do_clamp,
double recovery_clamp)
{
if (!IsActive())
{
return;
}
if (do_clamp)
{
Qc.matrix[off_L] += ChMaths.ChMin(ChMaths.ChMax(c * (curr_dist - distance), -recovery_clamp), recovery_clamp);
}
else
{
Qc.matrix[off_L] += c * (curr_dist - distance);
}
}
public override void ConstraintsBiLoad_C(double factor = 1, double recovery_clamp = 0.1, bool do_clamp = false)
{
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 = ChMaths.ChMin(ChMaths.ChMax(res, -recovery_clamp), recovery_clamp);
}
constraint.Set_b_i(constraint.Get_b_i() + res);
}
public override void IntLoadConstraint_C(int off_L, ref ChVectorDynamic <double> Qc, double c, bool do_clamp, double recovery_clamp)
{
// 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 = ChMaths.ChMin(ChMaths.ChMax(res, -recovery_clamp), recovery_clamp);
}
Qc.matrix[off_L] += res;
}
/// Performs the static analysis,
/// doing a linear solve.
public override void StaticAnalysis()
{
ChIntegrableIIorder mintegrable = (ChIntegrableIIorder)this.integrable;
// setup main vectors
mintegrable.StateSetup(ref X, ref V, ref A);
ChState Xnew = new ChState();
ChStateDelta Dx = new ChStateDelta();
ChVectorDynamic <double> R = new ChVectorDynamic <double>();
ChVectorDynamic <double> Qc = new ChVectorDynamic <double>();
double T = 0;
// setup auxiliary vectors
Dx.Reset(mintegrable.GetNcoords_v(), GetIntegrable());
Xnew.Reset(mintegrable.GetNcoords_x(), mintegrable);
R.Reset(mintegrable.GetNcoords_v());
Qc.Reset(mintegrable.GetNconstr());
L.Reset(mintegrable.GetNconstr());
mintegrable.StateGather(ref X, ref V, ref T); // state <- system
// Set speed to zero
V.matrix.FillElem(0);
// Extrapolate a prediction as warm start
Xnew = X;
// use Newton Raphson iteration to solve implicit Euler for v_new
//
// [ - dF/dx Cq' ] [ Dx ] = [ f ]
// [ Cq 0 ] [ L ] = [ C ]
for (int i = 0; i < this.GetMaxiters(); ++i)
{
mintegrable.StateScatter(Xnew, V, T); // state -> system
R.Reset();
Qc.Reset();
mintegrable.LoadResidual_F(ref R, 1.0);
mintegrable.LoadConstraint_C(ref Qc, 1.0);
double cfactor = ChMaths.ChMin(1.0, ((double)(i + 2) / (double)(incremental_steps + 1)));
R *= cfactor;
Qc *= cfactor;
// GetLog()<< "Non-linear statics iteration=" << i << " |R|=" << R.NormInf() << " |Qc|=" << Qc.NormInf()
//<< "\n";
if ((R.matrix.NormInf() < this.GetTolerance()) && (Qc.matrix.NormInf() < this.GetTolerance()))
{
break;
}
mintegrable.StateSolveCorrection(
ref Dx, ref L, R, Qc,
0, // factor for M
0, // factor for dF/dv
-1.0, // factor for dF/dx (the stiffness matrix)
Xnew, V, T, // not needed here
false, // do not StateScatter update to Xnew Vnew T+dt before computing correction
true // force a call to the solver's Setup() function
);
Xnew += Dx;
}
X = Xnew;
mintegrable.StateScatter(X, V, T); // state -> system
mintegrable.StateScatterReactions(L); // -> system auxiliary data
}
// Updates motion laws, marker positions, etc.
public override void UpdateTime(double mytime)
{
// First, inherit to parent class
base.UpdateTime(mytime);
// Move markers 1 and 2 to align them as gear teeth
ChMatrix33 <double> ma1 = new ChMatrix33 <double>(0);
ChMatrix33 <double> ma2 = new ChMatrix33 <double>(0);
ChMatrix33 <double> mrotma = new ChMatrix33 <double>(0);
ChMatrix33 <double> marot_beta = new ChMatrix33 <double>(0);
ChVector mx;
ChVector my;
ChVector mz;
ChVector mr;
ChVector mmark1;
ChVector mmark2;
ChVector lastX;
ChVector vrota;
ChCoordsys newmarkpos = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0));
ChFrame <double> abs_shaft1 = ChFrame <double> .FNULL; // new ChFrame<double>();
ChFrame <double> abs_shaft2 = ChFrame <double> .FNULL; //new ChFrame<double>();
((ChFrame <double>)Body1).TransformLocalToParent(local_shaft1, abs_shaft1);
((ChFrame <double>)Body2).TransformLocalToParent(local_shaft2, abs_shaft2);
ChVector vbdist = ChVector.Vsub(Get_shaft_pos1(), Get_shaft_pos2());
// ChVector Trad1 = ChVector.Vnorm(ChVector.Vcross(Get_shaft_dir1(), ChVector.Vnorm(ChVector.Vcross(Get_shaft_dir1(), vbdist))));
// ChVector Trad2 = ChVector.Vnorm(ChVector.Vcross(ChVector.Vnorm(ChVector.Vcross(Get_shaft_dir2(), vbdist)), Get_shaft_dir2()));
double dist = ChVector.Vlength(vbdist);
// compute actual rotation of the two wheels (relative to truss).
ChVector md1 = abs_shaft1.GetA().MatrT_x_Vect(-vbdist);
md1.z = 0;
md1 = ChVector.Vnorm(md1);
ChVector md2 = abs_shaft2.GetA().MatrT_x_Vect(-vbdist);
md2.z = 0;
md2 = ChVector.Vnorm(md2);
double periodic_a1 = ChMaths.ChAtan2(md1.x, md1.y);
double periodic_a2 = ChMaths.ChAtan2(md2.x, md2.y);
double old_a1 = a1;
double old_a2 = a2;
double turns_a1 = Math.Floor(old_a1 / ChMaths.CH_C_2PI);
double turns_a2 = Math.Floor(old_a2 / ChMaths.CH_C_2PI);
double a1U = turns_a1 * ChMaths.CH_C_2PI + periodic_a1 + ChMaths.CH_C_2PI;
double a1M = turns_a1 * ChMaths.CH_C_2PI + periodic_a1;
double a1L = turns_a1 * ChMaths.CH_C_2PI + periodic_a1 - ChMaths.CH_C_2PI;
a1 = a1M;
if (Math.Abs(a1U - old_a1) < Math.Abs(a1M - old_a1))
{
a1 = a1U;
}
if (Math.Abs(a1L - a1) < Math.Abs(a1M - a1))
{
a1 = a1L;
}
double a2U = turns_a2 * ChMaths.CH_C_2PI + periodic_a2 + ChMaths.CH_C_2PI;
double a2M = turns_a2 * ChMaths.CH_C_2PI + periodic_a2;
double a2L = turns_a2 * ChMaths.CH_C_2PI + periodic_a2 - ChMaths.CH_C_2PI;
a2 = a2M;
if (Math.Abs(a2U - old_a2) < Math.Abs(a2M - old_a2))
{
a2 = a2U;
}
if (Math.Abs(a2L - a2) < Math.Abs(a2M - a2))
{
a2 = a2L;
}
// compute new markers coordsystem alignment
my = ChVector.Vnorm(vbdist);
mz = Get_shaft_dir1();
mx = ChVector.Vnorm(ChVector.Vcross(my, mz));
mr = ChVector.Vnorm(ChVector.Vcross(mz, mx));
mz = ChVector.Vnorm(ChVector.Vcross(mx, my));
ChVector mz2, mx2, mr2, my2;
my2 = my;
mz2 = Get_shaft_dir2();
mx2 = ChVector.Vnorm(ChVector.Vcross(my2, mz2));
mr2 = ChVector.Vnorm(ChVector.Vcross(mz2, mx2));
ma1.Set_A_axis(mx, my, mz);
// rotate csys because of beta
vrota.x = 0.0;
vrota.y = beta;
vrota.z = 0.0;
mrotma.Set_A_Rxyz(vrota);
marot_beta.nm.matrix.MatrMultiply(ma1.nm.matrix, mrotma.nm.matrix);
// rotate csys because of alpha
vrota.x = 0.0;
vrota.y = 0.0;
vrota.z = alpha;
if (react_force.x < 0)
{
vrota.z = alpha;
}
else
{
vrota.z = -alpha;
}
mrotma.Set_A_Rxyz(vrota);
ma1.nm.matrix.MatrMultiply(marot_beta.nm.matrix, mrotma.nm.matrix);
ma2.nm.matrix.CopyFromMatrix(ma1.nm.matrix);
// is a bevel gear?
double be = Math.Acos(ChVector.Vdot(Get_shaft_dir1(), Get_shaft_dir2()));
bool is_bevel = true;
if (Math.Abs(ChVector.Vdot(Get_shaft_dir1(), Get_shaft_dir2())) > 0.96)
{
is_bevel = false;
}
// compute wheel radii so that:
// w2 = - tau * w1
if (!is_bevel)
{
double pardist = ChVector.Vdot(mr, vbdist);
double inv_tau = 1.0 / tau;
if (!epicyclic)
{
r2 = pardist - pardist / (inv_tau + 1.0);
}
else
{
r2 = pardist - (tau * pardist) / (tau - 1.0);
}
r1 = r2 * tau;
}
else
{
double gamma2;
if (!epicyclic)
{
gamma2 = be / (tau + 1.0);
}
else
{
gamma2 = be / (-tau + 1.0);
}
double al = ChMaths.CH_C_PI - Math.Acos(ChVector.Vdot(Get_shaft_dir2(), my));
double te = ChMaths.CH_C_PI - al - be;
double fd = Math.Sin(te) * (dist / Math.Sin(be));
r2 = fd * Math.Tan(gamma2);
r1 = r2 * tau;
}
// compute markers positions, supposing they
// stay on the ideal wheel contact point
mmark1 = ChVector.Vadd(Get_shaft_pos2(), ChVector.Vmul(mr2, r2));
mmark2 = mmark1;
contact_pt = mmark1;
// correct marker 1 position if phasing is not correct
if (checkphase)
{
double realtau = tau;
if (epicyclic)
{
realtau = -tau;
}
double m_delta;
m_delta = -(a2 / realtau) - a1 - phase;
if (m_delta > ChMaths.CH_C_PI)
{
m_delta -= (ChMaths.CH_C_2PI); // range -180..+180 is better than 0...360
}
if (m_delta > (ChMaths.CH_C_PI / 4.0))
{
m_delta = (ChMaths.CH_C_PI / 4.0); // phase correction only in +/- 45°
}
if (m_delta < -(ChMaths.CH_C_PI / 4.0))
{
m_delta = -(ChMaths.CH_C_PI / 4.0);
}
vrota.x = vrota.y = 0.0;
vrota.z = -m_delta;
mrotma.Set_A_Rxyz(vrota); // rotate about Z of shaft to correct
mmark1 = abs_shaft1.GetA().MatrT_x_Vect(ChVector.Vsub(mmark1, Get_shaft_pos1()));
mmark1 = mrotma.Matr_x_Vect(mmark1);
mmark1 = ChVector.Vadd(abs_shaft1.GetA().Matr_x_Vect(mmark1), Get_shaft_pos1());
}
// Move Shaft 1 along its direction if not aligned to wheel
double offset = ChVector.Vdot(Get_shaft_dir1(), (contact_pt - Get_shaft_pos1()));
ChVector moff = Get_shaft_dir1() * offset;
if (Math.Abs(offset) > 0.0001)
{
local_shaft1.SetPos(local_shaft1.GetPos() + Body1.TransformDirectionParentToLocal(moff));
}
// ! Require that the BDF routine of marker won't handle speed and acc.calculus of the moved marker 2!
marker2.SetMotionType(ChMarker.eChMarkerMotion.M_MOTION_EXTERNAL);
marker1.SetMotionType(ChMarker.eChMarkerMotion.M_MOTION_EXTERNAL);
// move marker1 in proper positions
newmarkpos.pos = mmark1;
newmarkpos.rot = ma1.Get_A_quaternion();
marker1.Impose_Abs_Coord(newmarkpos); // move marker1 into teeth position
// move marker2 in proper positions
newmarkpos.pos = mmark2;
newmarkpos.rot = ma2.Get_A_quaternion();
marker2.Impose_Abs_Coord(newmarkpos); // move marker2 into teeth position
// imposed relative positions/speeds
deltaC.pos = ChVector.VNULL;
deltaC_dt.pos = ChVector.VNULL;
deltaC_dtdt.pos = ChVector.VNULL;
deltaC.rot = ChQuaternion.QUNIT; // no relative rotations imposed!
deltaC_dt.rot = ChQuaternion.QNULL;
deltaC_dtdt.rot = ChQuaternion.QNULL;
}
public override void ContIntLoadConstraint_C(int off_L,
ref ChVectorDynamic <double> Qc,
double c,
bool do_clamp,
double recovery_clamp
)
{
bool bounced = false;
// Elastic Restitution model (use simple Newton model with coefficient e=v(+)/v(-))
// Note that this works only if the two connected items are two ChBody.
if (this.objA != null && this.objB != null)
{
var oA = (ChBody)(object)this.objA;
var oB = (ChBody)(object)this.objB;
if (this.restitution != 0)
{
// compute normal rebounce speed
ChVector V1_w = oA.GetContactPointSpeed(this.p1);
ChVector V2_w = oB.GetContactPointSpeed(this.p2);
ChVector Vrel_w = V2_w - V1_w;
ChVector Vrel_cplane = this.contact_plane.MatrT_x_Vect(Vrel_w);
double h = this.container.GetSystem().GetStep(); // = 1.0 / c; // not all steppers have c = 1/h
double neg_rebounce_speed = Vrel_cplane.x * this.restitution;
if (neg_rebounce_speed < -this.container.GetSystem().GetMinBounceSpeed())
{
if (this.norm_dist + neg_rebounce_speed * h < 0)
{
// CASE: BOUNCE
bounced = true;
Qc.matrix[off_L] += neg_rebounce_speed;
}
}
}
}
if (!bounced)
{
// CASE: SETTLE (most often, and also default if two colliding items are not two ChBody)
if (this.compliance != 0)
{
double h = 1.0 / c; // was: this->container->GetSystem()->GetStep(); note not all steppers have c = 1/h
double alpha = this.dampingf; // [R]=alpha*[K]
double inv_hpa = 1.0 / (h + alpha); // 1/(h+a)
double inv_hhpa = 1.0 / (h * (h + alpha)); // 1/(h*(h+a))
//***TODO*** move to KRMmatricesLoad() the following, and only for !bounced case
Nx.Set_cfm_i((inv_hhpa) * this.compliance);
Tu.Set_cfm_i((inv_hhpa) * this.complianceT);
Tv.Set_cfm_i((inv_hhpa) * this.complianceT);
double qc = inv_hpa * this.norm_dist; //***TODO*** see how to move this in KRMmatricesLoad()
// Note: clamping of Qc in case of compliance is questionable: it does not limit only the outbound
// speed, but also the reaction, so it might allow longer 'sinking' not related to the real compliance.
// I.e. If clamping kicks in (when using large timesteps and low compliance), it acts as a numerical damping.
if (do_clamp)
{
qc = ChMaths.ChMax(qc, -recovery_clamp);
}
Qc.matrix[off_L] += qc;
}
else
{
if (do_clamp)
{
if (this.Nx.Constraint2TuplesNall.GetCohesion() != 0)
{
Qc.matrix[off_L] += ChMaths.ChMin(0.0, ChMaths.ChMax(c * this.norm_dist, -recovery_clamp));
}
else
{
Qc.matrix[off_L] += ChMaths.ChMax(c * this.norm_dist, -recovery_clamp);
}
}
else
{
Qc.matrix[off_L] += c * this.norm_dist;
}
}
}
}
/// Performs the solution of the problem.
/// \return the maximum constraint violation after termination.
public override double Solve(ref ChSystemDescriptor sysd //< system description with constraints and variables
)
{
List <ChConstraint> mconstraints = sysd.GetConstraintsList();
List <ChVariables> mvariables = sysd.GetVariablesList();
double maxviolation = 0.0;
double maxdeltalambda = 0.0;
int i_friction_comp = 0;
double[] old_lambda_friction = new double[3];
int nConstr = mconstraints.Count;
int nVars = mvariables.Count;
// 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 (int ic = 0; ic < nConstr; 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 = new double[3];
for (int ic = 0; ic < nConstr; ic++)
{
if (mconstraints[ic].GetMode() == eChConstraintMode.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 (int iv = 0; iv < nVars; iv++)
{
if (mvariables[iv].IsActive())
{
mvariables[iv].Compute_invMb_v(mvariables[iv].Get_qb().matrix, mvariables[iv].Get_fb().matrix); // q = [M]'*fb
}
}
// 3) For all items with variables, add the effect of initial (guessed)
// lagrangian reactions of constraints, if a warm start is desired.
// Otherwise, if no warm start, simply resets initial lagrangians to zero.
if (warm_start)
{
for (int ic = 0; ic < nConstr; ic++)
{
if (mconstraints[ic].IsActive())
{
mconstraints[ic].Increment_q(mconstraints[ic].Get_l_i());
}
}
}
else
{
for (int ic = 0; ic < nConstr; ic++)
{
mconstraints[ic].Set_l_i(0.0);
}
}
// 4) Perform the iteration loops
for (int iter = 0; iter < max_iterations;)
{
//
// Forward sweep, for symmetric SOR
//
maxviolation = 0;
maxdeltalambda = 0;
i_friction_comp = 0;
int dummy = mconstraints.Count;
for (int ic = 0; ic < dummy; 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 = Math.Abs(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() == eChConstraintMode.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 = Math.Abs(ChMaths.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 = ChMaths.ChMax(maxdeltalambda, Math.Abs(true_delta_0));
maxdeltalambda = ChMaths.ChMax(maxdeltalambda, Math.Abs(true_delta_1));
maxdeltalambda = ChMaths.ChMax(maxdeltalambda, Math.Abs(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 = ChMaths.ChMax(maxdeltalambda, Math.Abs(true_delta));
}
}
maxviolation = ChMaths.ChMax(maxviolation, Math.Abs(candidate_violation));
} // end IsActive()
} // end constraint loop
// Terminate the loop if violation in constraints has been successfully limited.
// if (maxviolation < tolerance)
// break;
// For recording into violation history, if debugging
if (this.record_violation_history)
{
AtIterationEnd(maxviolation, maxdeltalambda, iter);
}
// Increment iter count (each sweep, either forward or backward, is considered
// as a complete iteration, to be fair when comparing to the non-symmetric SOR :)
iter++;
//
// Backward sweep, for symmetric SOR
//
maxviolation = 0.0;
maxdeltalambda = 0.0;
i_friction_comp = 0;
for (int ic = (nConstr - 1); ic >= 0; 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 = Math.Abs(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() == eChConstraintMode.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 == 3)
{
mconstraints[ic].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);
candidate_violation = Math.Abs(ChMaths.ChMin(0.0, mresidual));
if (this.record_violation_history)
{
maxdeltalambda = ChMaths.ChMax(maxdeltalambda, Math.Abs(true_delta_0));
maxdeltalambda = ChMaths.ChMax(maxdeltalambda, Math.Abs(true_delta_1));
maxdeltalambda = ChMaths.ChMax(maxdeltalambda, Math.Abs(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 = ChMaths.ChMax(maxdeltalambda, Math.Abs(true_delta));
}
}
maxviolation = ChMaths.ChMax(maxviolation, Math.Abs(candidate_violation));
} // end IsActive()
} // end loop on constraints
// For recording into violation history, if debugging
if (this.record_violation_history)
{
AtIterationEnd(maxviolation, maxdeltalambda, iter);
}
// Terminate the loop if violation in constraints has been successfully limited.
if (maxviolation < tolerance)
{
break;
}
iter++;
}
return(maxviolation);
}
/// Performs the solution of the problem.
/// \return the maximum constraint violation after termination.
public override double Solve(ref ChSystemDescriptor sysd //< system description with constraints and variables
)
{
List <ChConstraint> mconstraints = sysd.GetConstraintsList();
List <ChVariables> mvariables = sysd.GetVariablesList();
// 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 (int ic = 0; ic < mconstraints.Count; ic++)
{
mconstraints[ic].Update_auxiliary();
}
// 2) Compute, for all items with variables, the initial guess for
// still unconstrained system:
for (int iv = 0; iv < mvariables.Count; iv++)
{
if (mvariables[iv].IsActive())
{
mvariables[iv].Compute_invMb_v(mvariables[iv].Get_qb().matrix, mvariables[iv].Get_fb().matrix); // q = [M]'*fb
}
}
// 3) For all items with variables, add the effect of initial (guessed)
// lagrangian reactions of constraints, if a warm start is desired.
// Otherwise, if no warm start, simply resets initial lagrangians to zero.
if (warm_start)
{
for (int ic = 0; ic < mconstraints.Count; ic++)
{
if (mconstraints[ic].IsActive())
{
mconstraints[ic].Increment_q(mconstraints[ic].Get_l_i());
}
}
}
else
{
for (int ic = 0; ic < mconstraints.Count; ic++)
{
mconstraints[ic].Set_l_i(0.0);
}
}
// 4) Perform the iteration loops (if there are any constraints)
double maxviolation = 0.0;
double maxdeltalambda = 0.0;
if (mconstraints.Count == 0)
{
return(maxviolation);
}
for (int iter = 0; iter < max_iterations; iter++)
{
maxviolation = 0;
maxdeltalambda = 0;
// The iteration on all constraints
for (int ic = 0; ic < mconstraints.Count; 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 = Math.Abs(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 = ChMaths.ChMax(maxdeltalambda, Math.Abs(true_delta));
}
maxviolation = ChMaths.ChMax(maxviolation, Math.Abs(candidate_violation));
} // end IsActive()
} // end loop on constraints
// For recording into violation history, if debugging
if (this.record_violation_history)
{
AtIterationEnd(maxviolation, maxdeltalambda, iter);
}
// Terminate the loop if violation in constraints has been successfully limited.
if (maxviolation < tolerance)
{
break;
}
} // end iteration loop
return(maxviolation);
}
/// Updates motion laws, marker positions, etc.
public override void UpdateTime(double mytime)
{
// First, inherit to parent class
base.UpdateTime(mytime);
ChFrame <double> abs_shaft1 = ChFrame <double> .FNULL; //new ChFrame<double>();
ChFrame <double> abs_shaft2 = ChFrame <double> .FNULL; //new ChFrame<double>();
((ChFrame <double>)Body1).TransformLocalToParent(local_shaft1, abs_shaft1);
((ChFrame <double>)Body2).TransformLocalToParent(local_shaft2, abs_shaft2);
ChVector dcc_w = ChVector.Vsub(Get_shaft_pos2(), Get_shaft_pos1());
// compute actual rotation of the two wheels (relative to truss).
ChVector md1 = abs_shaft1.GetA().MatrT_x_Vect(dcc_w);
md1.z = 0;
md1 = ChVector.Vnorm(md1);
ChVector md2 = abs_shaft2.GetA().MatrT_x_Vect(dcc_w);
md2.z = 0;
md2 = ChVector.Vnorm(md2);
double periodic_a1 = ChMaths.ChAtan2(md1.x, md1.y);
double periodic_a2 = ChMaths.ChAtan2(md2.x, md2.y);
double old_a1 = a1;
double old_a2 = a2;
double turns_a1 = Math.Floor(old_a1 / ChMaths.CH_C_2PI);
double turns_a2 = Math.Floor(old_a2 / ChMaths.CH_C_2PI);
double a1U = turns_a1 * ChMaths.CH_C_2PI + periodic_a1 + ChMaths.CH_C_2PI;
double a1M = turns_a1 * ChMaths.CH_C_2PI + periodic_a1;
double a1L = turns_a1 * ChMaths.CH_C_2PI + periodic_a1 - ChMaths.CH_C_2PI;
a1 = a1M;
if (Math.Abs(a1U - old_a1) < Math.Abs(a1M - old_a1))
{
a1 = a1U;
}
if (Math.Abs(a1L - a1) < Math.Abs(a1M - a1))
{
a1 = a1L;
}
double a2U = turns_a2 * ChMaths.CH_C_2PI + periodic_a2 + ChMaths.CH_C_2PI;
double a2M = turns_a2 * ChMaths.CH_C_2PI + periodic_a2;
double a2L = turns_a2 * ChMaths.CH_C_2PI + periodic_a2 - ChMaths.CH_C_2PI;
a2 = a2M;
if (Math.Abs(a2U - old_a2) < Math.Abs(a2M - old_a2))
{
a2 = a2U;
}
if (Math.Abs(a2L - a2) < Math.Abs(a2M - a2))
{
a2 = a2L;
}
// correct marker positions if phasing is not correct
double m_delta = 0;
if (checkphase)
{
double realtau = tau;
m_delta = a1 - phase - (a2 / realtau);
if (m_delta > ChMaths.CH_C_PI)
{
m_delta -= (ChMaths.CH_C_2PI); // range -180..+180 is better than 0...360
}
if (m_delta > (ChMaths.CH_C_PI / 4.0))
{
m_delta = (ChMaths.CH_C_PI / 4.0); // phase correction only in +/- 45°
}
if (m_delta < -(ChMaths.CH_C_PI / 4.0))
{
m_delta = -(ChMaths.CH_C_PI / 4.0);
}
//***TODO***
}
// Move markers 1 and 2 to align them as pulley ends
ChVector d21_w = dcc_w - Get_shaft_dir1() * ChVector.Vdot(Get_shaft_dir1(), dcc_w);
ChVector D21_w = ChVector.Vnorm(d21_w);
shaft_dist = d21_w.Length();
ChVector U1_w = ChVector.Vcross(Get_shaft_dir1(), D21_w);
double gamma1 = Math.Acos((r1 - r2) / shaft_dist);
ChVector Ru_w = D21_w * Math.Cos(gamma1) + U1_w * Math.Sin(gamma1);
ChVector Rl_w = D21_w * Math.Cos(gamma1) - U1_w * Math.Sin(gamma1);
belt_up1 = Get_shaft_pos1() + Ru_w * r1;
belt_low1 = Get_shaft_pos1() + Rl_w * r1;
belt_up2 = Get_shaft_pos1() + d21_w + Ru_w * r2;
belt_low2 = Get_shaft_pos1() + d21_w + Rl_w * r2;
// marker alignment
ChMatrix33 <double> maU = new ChMatrix33 <double>(0);
ChMatrix33 <double> maL = new ChMatrix33 <double>(0);
ChVector Dxu = ChVector.Vnorm(belt_up2 - belt_up1);
ChVector Dyu = Ru_w;
ChVector Dzu = ChVector.Vnorm(ChVector.Vcross(Dxu, Dyu));
Dyu = ChVector.Vnorm(ChVector.Vcross(Dzu, Dxu));
maU.Set_A_axis(Dxu, Dyu, Dzu);
// ! Require that the BDF routine of marker won't handle speed and acc.calculus of the moved marker 2!
marker2.SetMotionType(ChMarker.eChMarkerMotion.M_MOTION_EXTERNAL);
marker1.SetMotionType(ChMarker.eChMarkerMotion.M_MOTION_EXTERNAL);
ChCoordsys newmarkpos = new ChCoordsys();
// move marker1 in proper positions
newmarkpos.pos = this.belt_up1;
newmarkpos.rot = maU.Get_A_quaternion();
marker1.Impose_Abs_Coord(newmarkpos); // move marker1 into teeth position
// move marker2 in proper positions
newmarkpos.pos = this.belt_up2;
newmarkpos.rot = maU.Get_A_quaternion();
marker2.Impose_Abs_Coord(newmarkpos); // move marker2 into teeth position
double phase_correction_up = m_delta * r1;
double phase_correction_low = -phase_correction_up;
double hU = ChVector.Vlength(belt_up2 - belt_up1) + phase_correction_up;
double hL = ChVector.Vlength(belt_low2 - belt_low1) + phase_correction_low;
// imposed relative positions/speeds
deltaC.pos = new ChVector(-hU, 0, 0);
deltaC_dt.pos = ChVector.VNULL;
deltaC_dtdt.pos = ChVector.VNULL;
deltaC.rot = ChQuaternion.QUNIT; // no relative rotations imposed!
deltaC_dt.rot = ChQuaternion.QNULL;
deltaC_dtdt.rot = ChQuaternion.QNULL;
}
/// Set the user modulation of the torque (or brake, if you use it between
/// a fixed shaft and a free shaft). The modulation must range from
/// 0 (switched off) to 1 (max torque). Default is 1, when clutch is created.
/// You can update this during integration loop to simulate the pedal pushing by the driver.
public void SetModulation(double mm)
{
modulation = ChMaths.ChMax(ChMaths.ChMin(mm, 1.0), 0.0);
}
