1 // ============================================================================= 2 // PROJECT CHRONO - http://projectchrono.org 3 // 4 // Copyright (c) 2014 projectchrono.org 5 // All rights reserved. 6 // 7 // Use of this source code is governed by a BSD-style license that can be found 8 // in the LICENSE file at the top level of the distribution and at 9 // http://projectchrono.org/license-chrono.txt. 10 // 11 // ============================================================================= 12 // Authors: Alessandro Tasora, Radu Serban 13 // ============================================================================= 14 15 #ifndef CH_CONTACT_NSC_H 16 #define CH_CONTACT_NSC_H 17 18 #include "chrono/core/ChFrame.h" 19 #include "chrono/solver/ChConstraintTwoTuplesContactN.h" 20 #include "chrono/solver/ChSystemDescriptor.h" 21 #include "chrono/collision/ChCollisionModel.h" 22 #include "chrono/physics/ChContactTuple.h" 23 #include "chrono/physics/ChContactContainer.h" 24 #include "chrono/physics/ChSystem.h" 25 26 namespace chrono { 27 28 /// Class for non-smooth contact between two generic ChContactable objects. 29 /// Ta and Tb are of ChContactable sub classes. 30 31 template <class Ta, class Tb> 32 class ChContactNSC : public ChContactTuple<Ta, Tb> { 33 public: 34 typedef typename ChContactTuple<Ta, Tb>::typecarr_a typecarr_a; 35 typedef typename ChContactTuple<Ta, Tb>::typecarr_b typecarr_b; 36 37 protected: 38 float* reactions_cache; ///< N,U,V reactions which might be stored in a persistent contact manifold 39 40 /// The three scalar constraints, to be fed into the system solver. 41 /// They contain jacobians data and special functions. 42 ChConstraintTwoTuplesContactN<typecarr_a, typecarr_b> Nx; 43 ChConstraintTwoTuplesFrictionT<typecarr_a, typecarr_b> Tu; 44 ChConstraintTwoTuplesFrictionT<typecarr_a, typecarr_b> Tv; 45 46 ChVector<> react_force; 47 48 double compliance; 49 double complianceT; 50 double restitution; 51 double dampingf; 52 53 public: ChContactNSC()54 ChContactNSC() { 55 Nx.SetTangentialConstraintU(&Tu); 56 Nx.SetTangentialConstraintV(&Tv); 57 } 58 ChContactNSC(ChContactContainer * mcontainer,Ta * mobjA,Tb * mobjB,const collision::ChCollisionInfo & cinfo,const ChMaterialCompositeNSC & mat)59 ChContactNSC(ChContactContainer* mcontainer, ///< contact container 60 Ta* mobjA, ///< collidable object A 61 Tb* mobjB, ///< collidable object B 62 const collision::ChCollisionInfo& cinfo, ///< data for the collision pair 63 const ChMaterialCompositeNSC& mat ///< composite material 64 ) 65 : ChContactTuple<Ta, Tb>(mcontainer, mobjA, mobjB, cinfo) { 66 Nx.SetTangentialConstraintU(&Tu); 67 Nx.SetTangentialConstraintV(&Tv); 68 69 Reset(mobjA, mobjB, cinfo, mat); 70 } 71 ~ChContactNSC()72 ~ChContactNSC() {} 73 74 /// Reinitialize this contact for reuse. Reset(Ta * mobjA,Tb * mobjB,const collision::ChCollisionInfo & cinfo,const ChMaterialCompositeNSC & mat)75 virtual void Reset(Ta* mobjA, ///< collidable object A 76 Tb* mobjB, ///< collidable object B 77 const collision::ChCollisionInfo& cinfo, ///< data for the collision pair 78 const ChMaterialCompositeNSC& mat ///< composite material 79 ) { 80 // Reset geometric information 81 this->Reset_cinfo(mobjA, mobjB, cinfo); 82 83 Nx.Get_tuple_a().SetVariables(*this->objA); 84 Nx.Get_tuple_b().SetVariables(*this->objB); 85 Tu.Get_tuple_a().SetVariables(*this->objA); 86 Tu.Get_tuple_b().SetVariables(*this->objB); 87 Tv.Get_tuple_a().SetVariables(*this->objA); 88 Tv.Get_tuple_b().SetVariables(*this->objB); 89 90 // Cache composite material properties 91 Nx.SetFrictionCoefficient(mat.static_friction); 92 Nx.SetCohesion(mat.cohesion); 93 94 this->restitution = mat.restitution; 95 this->dampingf = mat.dampingf; 96 this->compliance = mat.compliance; 97 this->complianceT = mat.complianceT; 98 99 this->reactions_cache = cinfo.reaction_cache; 100 101 // COMPUTE JACOBIANS 102 103 // delegate objA to compute its half of jacobian 104 this->objA->ComputeJacobianForContactPart(this->p1, this->contact_plane, Nx.Get_tuple_a(), Tu.Get_tuple_a(), 105 Tv.Get_tuple_a(), false); 106 107 // delegate objB to compute its half of jacobian 108 this->objB->ComputeJacobianForContactPart(this->p2, this->contact_plane, Nx.Get_tuple_b(), Tu.Get_tuple_b(), 109 Tv.Get_tuple_b(), true); 110 111 if (reactions_cache) { 112 react_force.x() = reactions_cache[0]; 113 react_force.y() = reactions_cache[1]; 114 react_force.z() = reactions_cache[2]; 115 //GetLog() << "Reset Fn=" << (double)reactions_cache[0] << " at cache address:" << (int)this->reactions_cache << "\n"; 116 } 117 else { 118 react_force = VNULL; 119 } 120 } 121 122 /// Get the contact force, if computed, in contact coordinate system GetContactForce()123 virtual ChVector<> GetContactForce() const override { return react_force; } 124 125 /// Get the contact friction coefficient GetFriction()126 virtual double GetFriction() { return Nx.GetFrictionCoefficient(); } 127 128 /// Set the contact friction coefficient SetFriction(double mf)129 virtual void SetFriction(double mf) { Nx.SetFrictionCoefficient(mf); } 130 131 // UPDATING FUNCTIONS 132 ContIntStateGatherReactions(const unsigned int off_L,ChVectorDynamic<> & L)133 virtual void ContIntStateGatherReactions(const unsigned int off_L, ChVectorDynamic<>& L) override { 134 L(off_L) = react_force.x(); 135 L(off_L + 1) = react_force.y(); 136 L(off_L + 2) = react_force.z(); 137 } 138 ContIntStateScatterReactions(const unsigned int off_L,const ChVectorDynamic<> & L)139 virtual void ContIntStateScatterReactions(const unsigned int off_L, const ChVectorDynamic<>& L) override { 140 react_force.x() = L(off_L); 141 react_force.y() = L(off_L + 1); 142 react_force.z() = L(off_L + 2); 143 144 if (reactions_cache) { 145 reactions_cache[0] = (float)L(off_L); // react_force.x(); 146 reactions_cache[1] = (float)L(off_L + 1); // react_force.y(); 147 reactions_cache[2] = (float)L(off_L + 2); // react_force.z(); 148 } 149 } 150 ContIntLoadResidual_CqL(const unsigned int off_L,ChVectorDynamic<> & R,const ChVectorDynamic<> & L,const double c)151 virtual void ContIntLoadResidual_CqL(const unsigned int off_L, 152 ChVectorDynamic<>& R, 153 const ChVectorDynamic<>& L, 154 const double c 155 ) override { 156 this->Nx.MultiplyTandAdd(R, L(off_L) * c); 157 this->Tu.MultiplyTandAdd(R, L(off_L + 1) * c); 158 this->Tv.MultiplyTandAdd(R, L(off_L + 2) * c); 159 } 160 ContIntLoadConstraint_C(const unsigned int off_L,ChVectorDynamic<> & Qc,const double c,bool do_clamp,double recovery_clamp)161 virtual void ContIntLoadConstraint_C(const unsigned int off_L, 162 ChVectorDynamic<>& Qc, 163 const double c, 164 bool do_clamp, 165 double recovery_clamp 166 ) override { 167 bool bounced = false; 168 169 // Elastic Restitution model (use simple Newton model with coefficient e=v(+)/v(-)) 170 // Note that this works only if the two connected items are two ChBody. 171 172 if (this->objA && this->objB) { 173 if (this->restitution) { 174 // compute normal rebounce speed 175 Vector V1_w = this->objA->GetContactPointSpeed(this->p1); 176 Vector V2_w = this->objB->GetContactPointSpeed(this->p2); 177 Vector Vrel_w = V2_w - V1_w; 178 Vector Vrel_cplane = this->contact_plane.transpose() * Vrel_w; 179 180 double h = this->container->GetSystem()->GetStep(); // = 1.0 / c; // not all steppers have c = 1/h 181 182 double neg_rebounce_speed = Vrel_cplane.x() * this->restitution; 183 if (neg_rebounce_speed < -this->container->GetSystem()->GetMinBounceSpeed()) 184 if (this->norm_dist + neg_rebounce_speed * h < 0) { 185 // CASE: BOUNCE 186 bounced = true; 187 Qc(off_L) += neg_rebounce_speed; 188 } 189 } 190 } 191 192 if (!bounced) { 193 // CASE: SETTLE (most often, and also default if two colliding items are not two ChBody) 194 195 if (this->compliance) { 196 double h = 1.0 / c; // was: this->container->GetSystem()->GetStep(); note not all steppers have c = 1/h 197 198 double alpha = this->dampingf; // [R]=alpha*[K] 199 double inv_hpa = 1.0 / (h + alpha); // 1/(h+a) 200 double inv_hhpa = 1.0 / (h * (h + alpha)); // 1/(h*(h+a)) 201 202 //***TODO*** move to KRMmatricesLoad() the following, and only for !bounced case 203 Nx.Set_cfm_i((inv_hhpa) * this->compliance); 204 Tu.Set_cfm_i((inv_hhpa) * this->complianceT); 205 Tv.Set_cfm_i((inv_hhpa) * this->complianceT); 206 207 double qc = inv_hpa * this->norm_dist; //***TODO*** see how to move this in KRMmatricesLoad() 208 209 // Note: clamping of Qc in case of compliance is questionable: it does not limit only the outbound 210 // speed, but also the reaction, so it might allow longer 'sinking' not related to the real compliance. 211 // I.e. If clamping kicks in (when using large timesteps and low compliance), it acts as a numerical damping. 212 if (do_clamp) { 213 qc = ChMax(qc, -recovery_clamp); 214 } 215 216 Qc(off_L) += qc; 217 218 } else { 219 if (do_clamp) 220 if (this->Nx.GetCohesion()) 221 Qc(off_L) += ChMin(0.0, ChMax(c * this->norm_dist, -recovery_clamp)); 222 else 223 Qc(off_L) += ChMax(c * this->norm_dist, -recovery_clamp); 224 else 225 Qc(off_L) += c * this->norm_dist; 226 } 227 } 228 } 229 ContIntToDescriptor(const unsigned int off_L,const ChVectorDynamic<> & L,const ChVectorDynamic<> & Qc)230 virtual void ContIntToDescriptor(const unsigned int off_L, 231 const ChVectorDynamic<>& L, 232 const ChVectorDynamic<>& Qc 233 ) override { 234 // only for solver warm start 235 Nx.Set_l_i(L(off_L)); 236 Tu.Set_l_i(L(off_L + 1)); 237 Tv.Set_l_i(L(off_L + 2)); 238 239 // solver known terms 240 Nx.Set_b_i(Qc(off_L)); 241 Tu.Set_b_i(Qc(off_L + 1)); 242 Tv.Set_b_i(Qc(off_L + 2)); 243 } 244 ContIntFromDescriptor(const unsigned int off_L,ChVectorDynamic<> & L)245 virtual void ContIntFromDescriptor(const unsigned int off_L, 246 ChVectorDynamic<>& L 247 ) override { 248 L(off_L) = Nx.Get_l_i(); 249 L(off_L + 1) = Tu.Get_l_i(); 250 L(off_L + 2) = Tv.Get_l_i(); 251 } 252 InjectConstraints(ChSystemDescriptor & mdescriptor)253 virtual void InjectConstraints(ChSystemDescriptor& mdescriptor) override { 254 mdescriptor.InsertConstraint(&Nx); 255 mdescriptor.InsertConstraint(&Tu); 256 mdescriptor.InsertConstraint(&Tv); 257 } 258 ConstraintsBiReset()259 virtual void ConstraintsBiReset() override { 260 Nx.Set_b_i(0.); 261 Tu.Set_b_i(0.); 262 Tv.Set_b_i(0.); 263 } 264 265 virtual void ConstraintsBiLoad_C(double factor = 1., double recovery_clamp = 0.1, bool do_clamp = false) override { 266 bool bounced = false; 267 268 // Elastic Restitution model (use simple Newton model with coefficient e=v(+)/v(-)) 269 // Note that this works only if the two connected items are two ChBody. 270 271 if (this->objA && this->objB) { 272 if (this->restitution) { 273 // compute normal rebounce speed 274 Vector V1_w = this->objA->GetContactPointSpeed(this->p1); 275 Vector V2_w = this->objB->GetContactPointSpeed(this->p2); 276 Vector Vrel_w = V2_w - V1_w; 277 Vector Vrel_cplane = this->contact_plane.transpose() * Vrel_w; 278 279 double h = 1.0 / factor; // inverse timestep is factor 280 281 double neg_rebounce_speed = Vrel_cplane.x() * this->restitution; 282 if (neg_rebounce_speed < -this->container->GetSystem()->GetMinBounceSpeed()) 283 if (this->norm_dist + neg_rebounce_speed * h < 0) { 284 // CASE: BOUNCE 285 bounced = true; 286 Nx.Set_b_i(Nx.Get_b_i() + neg_rebounce_speed); 287 } 288 } 289 } 290 291 if (!bounced) { 292 // CASE: SETTLE (most often, and also default if two colliding items are not two ChBody) 293 294 if (this->compliance) { 295 // inverse timestep is factor 296 double h = 1.0 / factor; 297 298 double alpha = this->dampingf; // [R]=alpha*[K] 299 double inv_hpa = 1.0 / (h + alpha); // 1/(h+a) 300 double inv_hhpa = 1.0 / (h * (h + alpha)); // 1/(h*(h+a)) 301 302 Nx.Set_cfm_i((inv_hhpa) * this->compliance); // was (inv_hh)* ... //***TEST DAMPING***// 303 Tu.Set_cfm_i((inv_hhpa) * this->complianceT); 304 Tv.Set_cfm_i((inv_hhpa) * this->complianceT); 305 306 double qc = inv_hpa * this->norm_dist; 307 308 // If clamping kicks in(when using large timesteps and low compliance), it acts as a numerical damping. 309 if (do_clamp) 310 qc = ChMax(qc, -recovery_clamp); 311 312 Nx.Set_b_i(Nx.Get_b_i() + qc); 313 314 } else { 315 // GetLog()<< "rigid " << (int)this << " recov_clamp=" << recovery_clamp << "\n"; 316 if (do_clamp) 317 if (this->Nx.GetCohesion()) 318 Nx.Set_b_i(Nx.Get_b_i() + ChMin(0.0, ChMax(factor * this->norm_dist, -recovery_clamp))); 319 else 320 Nx.Set_b_i(Nx.Get_b_i() + ChMax(factor * this->norm_dist, -recovery_clamp)); 321 else 322 Nx.Set_b_i(Nx.Get_b_i() + factor * this->norm_dist); 323 } 324 } 325 } 326 ConstraintsFetch_react(double factor)327 virtual void ConstraintsFetch_react(double factor) override { 328 // From constraints to react vector: 329 react_force.x() = Nx.Get_l_i() * factor; 330 react_force.y() = Tu.Get_l_i() * factor; 331 react_force.z() = Tv.Get_l_i() * factor; 332 } 333 }; 334 335 } // end namespace chrono 336 337 #endif 338