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 CHCONSTRAINTTWOTUPLESROLLINGN_H 16 #define CHCONSTRAINTTWOTUPLESROLLINGN_H 17 18 #include "chrono/solver/ChConstraintTwoTuplesContactN.h" 19 #include "chrono/solver/ChConstraintTwoTuplesRollingT.h" 20 21 namespace chrono { 22 23 /// This is enough to use dynamic_casting<> to detect all template types 24 /// from ChConstraintTwoTuplesRollingN 25 class ChApi ChConstraintTwoTuplesRollingNall {}; 26 27 /// This class is inherited from ChConstraintTwoTuples, 28 /// It is used to represent the normal reaction between two objects, 29 /// each represented by a tuple of ChVariables objects, 30 /// ONLY when also two ChConstraintTwoTuplesFrictionT objects are 31 /// used to represent friction. (If these two tangent constraint 32 /// are not used, for frictionless case, please use a simple ChConstraintTwo 33 /// with the CONSTRAINT_UNILATERAL mode.) 34 /// Differently from an unilateral constraint, this does not enforce 35 /// projection on positive constraint, since it will be up to the 'companion' 36 /// ChConstraintTwoTuplesFrictionT objects to call a projection on the cone, by 37 /// modifying all the three components (normal, u, v) at once. 38 /// Templates Ta and Tb are of ChVariableTupleCarrier_Nvars classes 39 40 template <class Ta, class Tb> 41 class ChApi ChConstraintTwoTuplesRollingN : public ChConstraintTwoTuples<Ta, Tb>, 42 public ChConstraintTwoTuplesRollingNall { 43 protected: 44 float rollingfriction; ///< the rolling friction coefficient 45 float spinningfriction; ///< the spinning friction coefficient 46 47 ChConstraintTwoTuplesRollingT<Ta, Tb>* constraint_U; ///< the pointer to U tangential component 48 ChConstraintTwoTuplesRollingT<Ta, Tb>* constraint_V; ///< the pointer to V tangential component 49 ChConstraintTwoTuplesContactN<Ta, Tb>* constraint_N; ///< the pointer to N component 50 51 public: 52 /// Default constructor ChConstraintTwoTuplesRollingN()53 ChConstraintTwoTuplesRollingN() 54 : rollingfriction(0), spinningfriction(0), constraint_U(NULL), constraint_V(NULL), constraint_N(NULL) { 55 this->mode = CONSTRAINT_FRIC; 56 } 57 58 /// Copy constructor ChConstraintTwoTuplesRollingN(const ChConstraintTwoTuplesRollingN & other)59 ChConstraintTwoTuplesRollingN(const ChConstraintTwoTuplesRollingN& other) : ChConstraintTwoTuples<Ta, Tb>(other) { 60 rollingfriction = other.rollingfriction; 61 spinningfriction = other.spinningfriction; 62 constraint_U = other.constraint_U; 63 constraint_V = other.constraint_V; 64 constraint_N = other.constraint_N; 65 } 66 ~ChConstraintTwoTuplesRollingN()67 virtual ~ChConstraintTwoTuplesRollingN() {} 68 69 /// "Virtual" copy constructor (covariant return type). Clone()70 virtual ChConstraintTwoTuplesRollingN* Clone() const override { return new ChConstraintTwoTuplesRollingN(*this); } 71 72 /// Assignment operator: copy from other object 73 ChConstraintTwoTuplesRollingN& operator=(const ChConstraintTwoTuplesRollingN& other) { 74 if (&other == this) 75 return *this; 76 // copy parent class data 77 ChConstraintTwoTuples<Ta, Tb>::operator=(other); 78 79 rollingfriction = other.rollingfriction; 80 spinningfriction = other.spinningfriction; 81 constraint_U = other.constraint_U; 82 constraint_V = other.constraint_V; 83 constraint_N = other.constraint_N; 84 return *this; 85 } 86 87 /// Get the rolling friction coefficient GetRollingFrictionCoefficient()88 float GetRollingFrictionCoefficient() { return rollingfriction; } 89 /// Set the rolling friction coefficient SetRollingFrictionCoefficient(float mcoeff)90 void SetRollingFrictionCoefficient(float mcoeff) { rollingfriction = mcoeff; } 91 92 /// Get the spinning friction coefficient GetSpinningFrictionCoefficient()93 float GetSpinningFrictionCoefficient() { return spinningfriction; } 94 /// Set the spinning friction coefficient SetSpinningFrictionCoefficient(float mcoeff)95 void SetSpinningFrictionCoefficient(float mcoeff) { spinningfriction = mcoeff; } 96 97 /// Get pointer to U tangential component GetRollingConstraintU()98 ChConstraintTwoTuplesRollingT<Ta, Tb>* GetRollingConstraintU() { return constraint_U; } 99 /// Get pointer to V tangential component GetRollingConstraintV()100 ChConstraintTwoTuplesRollingT<Ta, Tb>* GetRollingConstraintV() { return constraint_V; } 101 /// Get pointer to normal contact component GetNormalConstraint()102 ChConstraintTwoTuplesContactN<Ta, Tb>* GetNormalConstraint() { return constraint_N; } 103 104 /// Set pointer to U tangential component SetRollingConstraintU(ChConstraintTwoTuplesRollingT<Ta,Tb> * mconstr)105 void SetRollingConstraintU(ChConstraintTwoTuplesRollingT<Ta, Tb>* mconstr) { constraint_U = mconstr; } 106 /// Set pointer to V tangential component SetRollingConstraintV(ChConstraintTwoTuplesRollingT<Ta,Tb> * mconstr)107 void SetRollingConstraintV(ChConstraintTwoTuplesRollingT<Ta, Tb>* mconstr) { constraint_V = mconstr; } 108 /// Set pointer to normal contact component SetNormalConstraint(ChConstraintTwoTuplesContactN<Ta,Tb> * mconstr)109 void SetNormalConstraint(ChConstraintTwoTuplesContactN<Ta, Tb>* mconstr) { constraint_N = mconstr; } 110 111 /// For iterative solvers: project the value of a possible 112 /// 'l_i' value of constraint reaction onto admissible set. 113 /// This projection will also modify the l_i values of the two 114 /// tangential friction constraints (projection onto the friction cone, 115 /// as by Anitescu-Tasora theory). Project()116 virtual void Project() override { 117 if (!constraint_U) 118 return; 119 120 if (!constraint_V) 121 return; 122 123 if (!constraint_N) 124 return; 125 126 // METHOD 127 // Anitescu-Tasora projection on rolling-friction cone generator and polar cone 128 // (contractive, but performs correction on three components: normal,u,v) 129 130 double f_n = constraint_N->Get_l_i(); 131 double t_n = this->Get_l_i(); 132 double t_u = constraint_U->Get_l_i(); 133 double t_v = constraint_V->Get_l_i(); 134 double t_tang = sqrt(t_v * t_v + t_u * t_u); 135 double t_sptang = fabs(t_n); // = sqrt(t_n*t_n); 136 137 // A. Project the spinning friction (approximate - should do cone 138 // projection stuff as in B, but spinning friction is usually very low...) 139 140 if (spinningfriction) { 141 if (t_sptang < spinningfriction * f_n) { 142 // inside upper cone? keep untouched! 143 } else { 144 // inside lower cone? reset normal,u,v to zero! 145 if ((t_sptang < -(1.0 / spinningfriction) * f_n) || (fabs(f_n) < 10e-15)) { 146 constraint_N->Set_l_i(0); 147 this->Set_l_i(0); 148 } else { 149 // remaining case: project orthogonally to generator segment of upper cone (CAN BE simplified) 150 double f_n_proj = (t_sptang * spinningfriction + f_n) / (spinningfriction * spinningfriction + 1); 151 double t_tang_proj = f_n_proj * spinningfriction; 152 double tproj_div_t = t_tang_proj / t_sptang; 153 double t_n_proj = tproj_div_t * t_n; 154 155 constraint_N->Set_l_i(f_n_proj); 156 this->Set_l_i(t_n_proj); 157 } 158 } 159 } 160 161 // B. Project the rolling friction 162 163 // shortcut 164 if (!rollingfriction) { 165 constraint_U->Set_l_i(0); 166 constraint_V->Set_l_i(0); 167 if (f_n < 0) 168 constraint_N->Set_l_i(0); 169 return; 170 } 171 172 // inside upper cone? keep untouched! 173 if (t_tang < rollingfriction * f_n) 174 return; 175 176 // inside lower cone? reset normal,u,v to zero! 177 if ((t_tang < -(1.0 / rollingfriction) * f_n) || (fabs(f_n) < 10e-15)) { 178 double f_n_proj = 0; 179 double t_u_proj = 0; 180 double t_v_proj = 0; 181 182 constraint_N->Set_l_i(f_n_proj); 183 constraint_U->Set_l_i(t_u_proj); 184 constraint_V->Set_l_i(t_v_proj); 185 186 return; 187 } 188 189 // remaining case: project orthogonally to generator segment of upper cone 190 double f_n_proj = (t_tang * rollingfriction + f_n) / (rollingfriction * rollingfriction + 1); 191 double t_tang_proj = f_n_proj * rollingfriction; 192 double tproj_div_t = t_tang_proj / t_tang; 193 double t_u_proj = tproj_div_t * t_u; 194 double t_v_proj = tproj_div_t * t_v; 195 196 constraint_N->Set_l_i(f_n_proj); 197 constraint_U->Set_l_i(t_u_proj); 198 constraint_V->Set_l_i(t_v_proj); 199 } 200 }; 201 202 } // end namespace chrono 203 204 #endif 205