public override void ConstraintsBiLoad_C(double factor = 1, double recovery_clamp = 0.1, bool do_clamp = false)
{
// if (!IsActive())
// return;
double res = 0; // no residual
constraint.Set_b_i(constraint.Get_b_i() + factor * res);
}
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);
}