/// Compute all forces in a contiguous array. /// Used in finite-difference Jacobian approximation. public void CalculateQ(ChState stateA_x, //< state positions for objA ChStateDelta stateA_w, //< state velocities for objA ChState stateB_x, //< state positions for objB ChStateDelta stateB_w, //< state velocities for objB ChMaterialCompositeSMC mat, //< composite material for contact pair ref ChVectorDynamic <double> Q //< output generalized forces ) { ChBody oA = (this.objA as ChBody); ChBody oB = (this.objB as ChBody); // Express contact points in local frames. // We assume that these points remain fixed to their respective contactable objects. ChVector p1_loc = oA.GetCsysForCollisionModel().TransformPointParentToLocal(this.p1); ChVector p2_loc = oB.GetCsysForCollisionModel().TransformPointParentToLocal(this.p2); // Express the local points in global frame ChVector p1_abs = oA.GetContactPoint(p1_loc, stateA_x); ChVector p2_abs = oB.GetContactPoint(p2_loc, stateB_x); /* * Note: while this can be somewhat justified for a ChBody, it will not work * for a mesh vertex for instance... * * // Project the points onto the unperturbed normal line * p1_abs = this.p1 + Vdot(p1_abs - this.p1, this.normal) * this.normal; * p2_abs = this.p2 + Vdot(p2_abs - this.p2, this.normal) * this.normal; */ // Calculate normal direction (expressed in global frame) ChVector normal_dir = (p1_abs - p2_abs).GetNormalized(); // Calculate penetration depth double delta = (p1_abs - p2_abs).Length(); // If the normal direction flipped sign, change sign of delta if (ChVector.Vdot(normal_dir, this.normal) < 0) { delta = -delta; } // Calculate velocity of contact points (expressed in global frame) ChVector vel1 = oA.GetContactPointSpeed(p1_loc, stateA_x, stateA_w); ChVector vel2 = oB.GetContactPointSpeed(p2_loc, stateB_x, stateB_w); // Compute the contact force. ChVector force = CalculateForce(delta, normal_dir, vel1, vel2, mat); // Compute and load the generalized contact forces. oA.ContactForceLoadQ(-force, p1_abs, stateA_x, ref Q, 0); oB.ContactForceLoadQ(force, p2_abs, stateB_x, ref Q, oA.ContactableGet_ndof_w()); }
/// Initialize again this constraint. public override void Reset(Ta mobjA, //< collidable object A Tb mobjB, //< collidable object B collision.ChCollisionInfo cinfo //< data for the contact pair ) //where T1: IntInterface.IBase { // Inherit base class. base.Reset(mobjA, mobjB, cinfo); // Note: cinfo.distance is the same as this.norm_dist. // Debug.Assert(cinfo.distance < 0); ChBody oA = (this.objA as ChBody); ChBody oB = (this.objB as ChBody); // Calculate composite material properties ChMaterialCompositeSMC mat = new ChMaterialCompositeSMC( this.container.GetSystem().composition_strategy, (ChMaterialSurfaceSMC)(oA.GetMaterialSurfaceBase()), (ChMaterialSurfaceSMC)(oB.GetMaterialSurfaceBase())); // Check for a user-provided callback to modify the material if (this.container.GetAddContactCallback() != null) { this.container.GetAddContactCallback().OnAddContact(cinfo, mat); } // Calculate contact force. m_force = CalculateForce(-this.norm_dist, // overlap (here, always positive) this.normal, // normal contact direction oA.GetContactPointSpeed(this.p1), // velocity of contact point on objA oB.GetContactPointSpeed(this.p2), // velocity of contact point on objB mat // composite material for contact pair ); ChSystemSMC smc = (ChSystemSMC)this.container.GetSystem(); // Set up and compute Jacobian matrices. if (smc.GetStiffContact() == true) { CreateJacobians(); CalculateJacobians(mat); } }
/// Calculate Jacobian of generalized contact forces. public void CalculateJacobians(ChMaterialCompositeSMC mat) { // Compute a finite-difference approximations to the Jacobians of the contact forces and // load dQ/dx into m_Jac.m_K and dQ/dw into m_Jac.m_R. // Note that we only calculate these Jacobians whenever the contact force itself is calculated, // that is only once per step. The Jacobian of generalized contact forces will therefore be // constant over the time step. ChBody oA = (this.objA as ChBody); ChBody oB = (this.objB as ChBody); // Get states for objA int ndofA_x = oA.ContactableGet_ndof_x(); int ndofA_w = oA.ContactableGet_ndof_w(); ChState stateA_x = new ChState(ndofA_x, null); ChStateDelta stateA_w = new ChStateDelta(ndofA_w, null); oA.ContactableGetStateBlock_x(ref stateA_x); oA.ContactableGetStateBlock_w(ref stateA_w); // Get states for objB int ndofB_x = oB.ContactableGet_ndof_x(); int ndofB_w = oB.ContactableGet_ndof_w(); ChState stateB_x = new ChState(ndofB_x, null); ChStateDelta stateB_w = new ChStateDelta(ndofB_w, null); oB.ContactableGetStateBlock_x(ref stateB_x); oB.ContactableGetStateBlock_w(ref stateB_w); // Compute Q at current state ChVectorDynamic <double> Q0 = new ChVectorDynamic <double>(ndofA_w + ndofB_w); CalculateQ(stateA_x, stateA_w, stateB_x, stateB_w, mat, ref Q0); // Finite-difference approximation perturbation. // Note that ChState and ChStateDelta are set to 0 on construction. // To accommodate objects with quaternion states, use the method ContactableIncrementState while // calculating Jacobian columns corresponding to position states. double perturbation = 1e-5; ChState stateA_x1 = new ChState(ndofA_x, null); ChState stateB_x1 = new ChState(ndofB_x, null); ChStateDelta prtrbA = new ChStateDelta(ndofA_w, null); ChStateDelta prtrbB = new ChStateDelta(ndofB_w, null); ChVectorDynamic <double> Q1 = new ChVectorDynamic <double>(ndofA_w + ndofB_w); ChVectorDynamic <double> Jcolumn = new ChVectorDynamic <double>(ndofA_w + ndofB_w); // Jacobian w.r.t. variables of objA for (int i = 0; i < ndofA_w; i++) { prtrbA.matrix[i] += perturbation; oA.ContactableIncrementState(stateA_x, prtrbA, ref stateA_x1); CalculateQ(stateA_x1, stateA_w, stateB_x, stateB_w, mat, ref Q1); prtrbA.matrix[i] -= perturbation; Jcolumn = (Q1 - Q0.matrix) * (-1 / perturbation); // note sign change m_Jac.m_K.matrix.PasteMatrix(Jcolumn.matrix, 0, i); stateA_w.matrix[i] += perturbation; CalculateQ(stateA_x, stateA_w, stateB_x, stateB_w, mat, ref Q1); stateA_w.matrix[i] -= perturbation; Jcolumn = (Q1 - Q0.matrix) * (-1 / perturbation); // note sign change m_Jac.m_R.matrix.PasteMatrix(Jcolumn.matrix, 0, i); } // Jacobian w.r.t. variables of objB for (int i = 0; i < ndofB_w; i++) { prtrbB.matrix[i] += perturbation; oB.ContactableIncrementState(stateB_x, prtrbB, ref stateB_x1); CalculateQ(stateA_x, stateA_w, stateB_x1, stateB_w, mat, ref Q1); prtrbB.matrix[i] -= perturbation; Jcolumn = (Q1 - Q0.matrix) * (-1 / perturbation); // note sign change m_Jac.m_K.matrix.PasteMatrix(Jcolumn.matrix, 0, ndofA_w + i); stateB_w.matrix[i] += perturbation; CalculateQ(stateA_x, stateA_w, stateB_x, stateB_w, mat, ref Q1); stateB_w.matrix[i] -= perturbation; Jcolumn = (Q1 - Q0.matrix) * (-1 / perturbation); // note sign change m_Jac.m_R.matrix.PasteMatrix(Jcolumn.matrix, 0, ndofA_w + i); } }
/// Calculate contact force, expressed in absolute coordinates. public ChVector CalculateForce( double delta, //< overlap in normal direction ChVector normal_dir, //< normal contact direction (expressed in global frame) ChVector vel1, //< velocity of contact point on objA (expressed in global frame) ChVector vel2, //< velocity of contact point on objB (expressed in global frame) ChMaterialCompositeSMC mat //< composite material for contact pair ) { // Set contact force to zero if no penetration. if (delta <= 0) { return(new ChVector(0, 0, 0)); } // Extract parameters from containing system ChSystemSMC sys = (ChSystemSMC)(this.container.GetSystem()); double dT = sys.GetStep(); bool use_mat_props = sys.UsingMaterialProperties(); ChSystemSMC.ContactForceModel contact_model = sys.GetContactForceModel(); ChSystemSMC.AdhesionForceModel adhesion_model = sys.GetAdhesionForceModel(); ChSystemSMC.TangentialDisplacementModel tdispl_model = sys.GetTangentialDisplacementModel(); // Relative velocity at contact ChVector relvel = vel2 - vel1; double relvel_n_mag = relvel.Dot(normal_dir); ChVector relvel_n = relvel_n_mag * normal_dir; ChVector relvel_t = relvel - relvel_n; double relvel_t_mag = relvel_t.Length(); ChBody oA = (this.objA as ChBody); ChBody oB = (this.objB as ChBody); // Calculate effective mass double eff_mass = oA.GetContactableMass() * oB.GetContactableMass() / (oA.GetContactableMass() + oB.GetContactableMass()); // Calculate stiffness and viscous damping coefficients. // All models use the following formulas for normal and tangential forces: // Fn = kn * delta_n - gn * v_n // Ft = kt * delta_t - gt * v_t double kn = 0; double kt = 0; double gn = 0; double gt = 0; double eps = double.Epsilon; switch (contact_model) { case ChSystemSMC.ContactForceModel.Hooke: if (use_mat_props) { double tmp_k = (16.0 / 15) * Math.Sqrt(this.eff_radius) * mat.E_eff; double v2 = sys.GetCharacteristicImpactVelocity() * sys.GetCharacteristicImpactVelocity(); double loge = (mat.cr_eff < eps) ? Math.Log(eps) : Math.Log(mat.cr_eff); loge = (mat.cr_eff > 1 - eps) ? Math.Log(1 - eps) : loge; double tmp_g = 1 + Math.Pow(ChMaths.CH_C_PI / loge, 2); kn = tmp_k * Math.Pow(eff_mass * v2 / tmp_k, 1.0 / 5); kt = kn; gn = Math.Sqrt(4 * eff_mass * kn / tmp_g); gt = gn; } else { kn = mat.kn; kt = mat.kt; gn = eff_mass * mat.gn; gt = eff_mass * mat.gt; } break; case ChSystemSMC.ContactForceModel.Hertz: if (use_mat_props) { double sqrt_Rd = Math.Sqrt(this.eff_radius * delta); double Sn = 2 * mat.E_eff * sqrt_Rd; double St = 8 * mat.G_eff * sqrt_Rd; double loge = (mat.cr_eff < eps) ? Math.Log(eps) : Math.Log(mat.cr_eff); double beta = loge / Math.Sqrt(loge * loge + ChMaths.CH_C_PI * ChMaths.CH_C_PI); kn = (2.0 / 3) * Sn; kt = St; gn = -2 * Math.Sqrt(5.0 / 6) * beta * Math.Sqrt(Sn * eff_mass); gt = -2 * Math.Sqrt(5.0 / 6) * beta * Math.Sqrt(St * eff_mass); } else { double tmp = this.eff_radius * Math.Sqrt(delta); kn = tmp * mat.kn; kt = tmp * mat.kt; gn = tmp * eff_mass * mat.gn; gt = tmp * eff_mass * mat.gt; } break; case ChSystemSMC.ContactForceModel.PlainCoulomb: if (use_mat_props) { double sqrt_Rd = Math.Sqrt(delta); double Sn = 2 * mat.E_eff * sqrt_Rd; double St = 8 * mat.G_eff * sqrt_Rd; double loge = (mat.cr_eff < eps) ? Math.Log(eps) : Math.Log(mat.cr_eff); double beta = loge / Math.Sqrt(loge * loge + ChMaths.CH_C_PI * ChMaths.CH_C_PI); kn = (2.0 / 3) * Sn; gn = -2 * Math.Sqrt(5.0 / 6) * beta * Math.Sqrt(Sn * eff_mass); } else { double tmp = Math.Sqrt(delta); kn = tmp * mat.kn; gn = tmp * mat.gn; } kt = 0; gt = 0; { double forceNb = kn * delta - gn * relvel_n_mag; if (forceNb < 0) { forceNb = 0; } double forceTb = mat.mu_eff * Math.Tanh(5.0 * relvel_t_mag) * forceNb; switch (adhesion_model) { case ChSystemSMC.AdhesionForceModel.Constant: forceNb -= mat.adhesion_eff; break; case ChSystemSMC.AdhesionForceModel.DMT: forceNb -= mat.adhesionMultDMT_eff * Math.Sqrt(this.eff_radius); break; } ChVector forceb = forceNb * normal_dir; if (relvel_t_mag >= sys.GetSlipVelocityThreshold()) { forceb -= (forceTb / relvel_t_mag) * relvel_t; } return(forceb); } } // Tangential displacement (magnitude) double delta_t = 0; switch (tdispl_model) { case ChSystemSMC.TangentialDisplacementModel.OneStep: delta_t = relvel_t_mag * dT; break; case ChSystemSMC.TangentialDisplacementModel.MultiStep: //// TODO: implement proper MultiStep mode delta_t = relvel_t_mag * dT; break; default: break; } // Calculate the magnitudes of the normal and tangential contact forces double forceN = kn * delta - gn * relvel_n_mag; double forceT = kt * delta_t + gt * relvel_t_mag; // If the resulting normal contact force is negative, the two shapes are moving // away from each other so fast that no contact force is generated. if (forceN < 0) { forceN = 0; forceT = 0; } // Include adhesion force switch (adhesion_model) { case ChSystemSMC.AdhesionForceModel.Constant: forceN -= mat.adhesion_eff; break; case ChSystemSMC.AdhesionForceModel.DMT: forceN -= mat.adhesionMultDMT_eff * Math.Sqrt(this.eff_radius); break; } // Coulomb law forceT = Math.Min(forceT, mat.mu_eff * Math.Abs(forceN)); // Accumulate normal and tangential forces ChVector force = forceN * normal_dir; if (relvel_t_mag >= sys.GetSlipVelocityThreshold()) { force -= (forceT / relvel_t_mag) * relvel_t; } return(force); }