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 CHCONSTRAINT_H 16 #define CHCONSTRAINT_H 17 18 #include "chrono/core/ChApiCE.h" 19 #include "chrono/core/ChClassFactory.h" 20 #include "chrono/core/ChMatrix.h" 21 22 namespace chrono { 23 24 /// Modes for constraint 25 enum eChConstraintMode { 26 CONSTRAINT_FREE = 0, ///< the constraint does not enforce anything 27 CONSTRAINT_LOCK = 1, ///< the constraint enforces c_i=0; 28 CONSTRAINT_UNILATERAL = 2, ///< the constraint enforces linear complementarity 29 /// c_i>=0, l_i>=0, l_1*c_i=0; 30 CONSTRAINT_FRIC = 3, ///< the constraint is one of three reactions in friction 31 ///(cone complementarity problem) 32 }; 33 34 /// Base class for representing constraints to be used 35 /// with variational inequality solvers, used with Linear/CCP/LCP 36 /// problems including inequalities, equalities, nonlinearities, etc. 37 /// 38 /// See ChSystemDescriptor for more information about the overall 39 /// problem and data representation. 40 /// 41 /// The jacobian matrix [Cq] is built row by row by jacobians 42 /// [Cq_i] of the constraints.\n 43 /// [E] optionally includes 'cfm_i' terms on the diagonal. 44 /// 45 /// In general, typical bilateral constraints must be solved 46 /// to have residual c_i = 0 and unilaterals: c_i>0 47 /// where the following linearization is introduced: 48 /// c_i= [Cq_i]*q + b_i 49 /// 50 /// The base class introduces just the minimum requirements 51 /// for the solver, that is the basic methods that will be 52 /// called by the solver. It is up to the derived classes 53 /// to implement these methods, and to add further features.. 54 55 class ChApi ChConstraint { 56 protected: 57 double c_i; ///< The 'c_i' residual of the constraint (if satisfied, c must be 0) 58 double l_i; ///< The 'l_i' lagrangian multiplier (reaction) 59 double b_i; ///< The 'b_i' right term in [Cq_i]*q+b_i=0 , note: c_i= [Cq_i]*q + b_i 60 61 /// The constraint force mixing, if needed (usually is zero) to add some 62 /// numerical 'compliance' in the constraint, that is the equation becomes: 63 /// c_i= [Cq_i]*q + b_i + cfm*l_i =0; 64 /// Example, it could be cfm = [k * h^2](^-1) where k is stiffness 65 double cfm_i; 66 67 private: 68 bool valid; ///< the link has no formal problems (references restored correctly, etc) 69 bool disabled; ///< the user can turn on/off the link easily 70 bool redundant; ///< the constraint is redundant or singular 71 bool broken; ///< the constraint is broken (someone pulled too much..) 72 bool active; ///< Cached active state depending on previous flags. Internal update. 73 74 protected: 75 eChConstraintMode mode; ///< mode of the constraint: free / lock / complementar 76 double g_i; ///< 'g_i' product [Cq_i]*[invM_i]*[Cq_i]' (+cfm) 77 int offset; ///< offset in global "l" state vector (needed by some solvers) 78 79 public: 80 /// Default constructor ChConstraint()81 ChConstraint() 82 : c_i(0), 83 l_i(0), 84 b_i(0), 85 cfm_i(0), 86 valid(false), 87 disabled(false), 88 redundant(false), 89 broken(false), 90 active(true), 91 mode(CONSTRAINT_LOCK), 92 g_i(0) {} 93 94 /// Copy constructor 95 ChConstraint(const ChConstraint& other); 96 ~ChConstraint()97 virtual ~ChConstraint() {} 98 99 /// "Virtual" copy constructor. 100 virtual ChConstraint* Clone() const = 0; 101 102 /// Assignment operator: copy from other object 103 ChConstraint& operator=(const ChConstraint& other); 104 105 /// Comparison (compares only flags, not the jacobians etc.) 106 bool operator==(const ChConstraint& other) const; 107 108 /// Tells if the constraint data is currently valid. IsValid()109 bool IsValid() const { return valid; } 110 111 /// Use this function to set the valid state (child class 112 /// Children classes must use this function depending on 113 /// the result of their implementations of RestoreReference(); SetValid(bool mon)114 void SetValid(bool mon) { 115 valid = mon; 116 UpdateActiveFlag(); 117 } 118 119 /// Tells if the constraint is currently turned on or off by the user. IsDisabled()120 bool IsDisabled() const { return disabled; } 121 122 /// User can use this to enable/disable the constraint as desired SetDisabled(bool mon)123 void SetDisabled(bool mon) { 124 disabled = mon; 125 UpdateActiveFlag(); 126 } 127 128 /// Tells if the constraint is redundant or singular. IsRedundant()129 bool IsRedundant() const { return redundant; } 130 131 /// Solvers may use the following to mark a constraint as redundant SetRedundant(bool mon)132 void SetRedundant(bool mon) { 133 redundant = mon; 134 UpdateActiveFlag(); 135 } 136 137 /// Tells if the constraint is broken, for excess of pulling/pushing. IsBroken()138 bool IsBroken() const { return broken; } 139 140 /// 3rd party software can set the 'broken' status via this method 141 /// (by default, constraints never break); SetBroken(bool mon)142 void SetBroken(bool mon) { 143 broken = mon; 144 UpdateActiveFlag(); 145 } 146 147 /// Tells if the constraint is unilateral (typical complementarity constraint). IsUnilateral()148 virtual bool IsUnilateral() const { return mode == CONSTRAINT_UNILATERAL; } 149 150 /// Tells if the constraint is linear (if non linear, returns false). IsLinear()151 virtual bool IsLinear() const { return true; } 152 153 /// Gets the mode of the constraint: free / lock / complementary 154 /// A typical constraint has 'lock = true' by default. GetMode()155 eChConstraintMode GetMode() const { return mode; } 156 157 /// Sets the mode of the constraint: free / lock / complementary SetMode(eChConstraintMode mmode)158 void SetMode(eChConstraintMode mmode) { 159 mode = mmode; 160 UpdateActiveFlag(); 161 } 162 163 /// Tells if the constraint is currently active, in general, 164 /// that is tells if it must be included into the system solver or not. 165 /// This method cumulates the effect of all flags (so a constraint may 166 /// be not active either because 'disabled', or 'broken', o 'redundant', or 167 /// not 'valid'.) IsActive()168 inline bool IsActive() const { return active; } 169 170 /// Set the status of the constraint to active SetActive(bool isactive)171 void SetActive(bool isactive) { active = isactive; } 172 173 /// Compute the residual of the constraint using the LINEAR expression 174 /// <pre> 175 /// c_i= [Cq_i]*q + cfm_i*l_i + b_i 176 /// </pre> 177 /// For a satisfied bilateral constraint, this residual must be near zero. Compute_c_i()178 virtual double Compute_c_i() { 179 c_i = Compute_Cq_q() + cfm_i * l_i + b_i; 180 return c_i; 181 } 182 183 /// Return the residual 'c_i' of this constraint. // CURRENTLY NOT USED Get_c_i()184 double Get_c_i() const { return c_i; } 185 186 /// Sets the known term b_i in [Cq_i]*q + b_i = 0, where: c_i = [Cq_i]*q + b_i = 0 Set_b_i(const double mb)187 void Set_b_i(const double mb) { b_i = mb; } 188 189 /// Return the known term b_i in [Cq_i]*q + b_i = 0, where: c_i= [Cq_i]*q + b_i = 0 Get_b_i()190 double Get_b_i() const { return b_i; } 191 192 /// Sets the constraint force mixing term (default=0). 193 /// Adds artificial 'elasticity' to the constraint, as c_i= [Cq_i]*q + b_i + cfm*l_i = 0 Set_cfm_i(const double mcfm)194 void Set_cfm_i(const double mcfm) { cfm_i = mcfm; } 195 196 /// Returns the constraint force mixing term. Get_cfm_i()197 double Get_cfm_i() const { return cfm_i; } 198 199 /// Sets the 'l_i' value (constraint reaction, see 'l' vector) Set_l_i(double ml_i)200 void Set_l_i(double ml_i) { l_i = ml_i; } 201 202 /// Return the 'l_i' value (constraint reaction, see 'l' vector) Get_l_i()203 double Get_l_i() const { return l_i; } 204 205 // ----- Functions often used by iterative solvers: 206 207 /// This function must update jacobians and auxiliary 208 /// data such as the 'g_i' product. This function is 209 /// often called by solvers at the beginning of the 210 /// solution process. 211 /// *** This function MUST BE OVERRIDDEN by specialized 212 /// inherited classes, which have some jacobians! Update_auxiliary()213 virtual void Update_auxiliary() {} 214 215 /// Return the 'g_i' product , that is [Cq_i]*[invM_i]*[Cq_i]' (+cfm) Get_g_i()216 double Get_g_i() const { return g_i; } 217 218 /// Usually you should not use the Set_g_i function, because g_i 219 /// should be automatically computed during the Update_auxiliary() . Set_g_i(double m_g_i)220 void Set_g_i(double m_g_i) { g_i = m_g_i; } 221 222 /// This function must computes the product between 223 /// the row-jacobian of this constraint '[Cq_i]' and the 224 /// vector of variables, 'q', that is, Cq_q=[Cq_i]*q. 225 /// This is used for some iterative solvers. 226 /// *** This function MUST BE OVERRIDDEN by specialized 227 /// inherited classes! (since it will be called frequently, 228 /// when iterative solvers are used, the implementation of 229 /// the [Cq_i]*q product must be AS FAST AS POSSIBLE!). 230 /// It returns the result of the computation. 231 virtual double Compute_Cq_q() = 0; 232 233 /// This function must increment the vector of variables 234 /// 'q' with the quantity [invM]*[Cq_i]'*deltal,that is 235 /// q+=[invM]*[Cq_i]'*deltal 236 /// This is used for some iterative solvers. 237 /// *** This function MUST BE OVERRIDDEN by specialized 238 /// inherited classes! 239 virtual void Increment_q(const double deltal) = 0; 240 241 /// Computes the product of the corresponding block in the 242 /// system matrix by 'vect', and add to 'result'. 243 /// NOTE: the 'vect' vector must already have 244 /// the size of the total variables&constraints in the system; the procedure 245 /// will use the ChVariable offsets (that must be already updated) to know the 246 /// indexes in result and vect; 247 virtual void MultiplyAndAdd(double& result, const ChVectorDynamic<double>& vect) const = 0; 248 249 /// Computes the product of the corresponding transposed block in the 250 /// system matrix (ie. the TRANSPOSED jacobian matrix C_q') by 'l', and add to 251 /// 'result'. 252 /// NOTE: the 'result' vectors must already have 253 /// the size of the total variables&constraints in the system; the procedure 254 /// will use the ChVariable offsets (that must be already updated) to know the 255 /// indexes in result and vect; 256 virtual void MultiplyTandAdd(ChVectorDynamic<double>& result, double l) = 0; 257 258 /// For iterative solvers: project the value of a possible 259 /// 'l_i' value of constraint reaction onto admissible orthant/set. 260 /// Default behavior: if constraint is unilateral and l_i<0, reset l_i=0 261 /// *** This function MAY BE OVERRIDDEN by specialized 262 /// inherited classes! For example, a bilateral constraint 263 /// can do nothing, a monolateral: l_i= ChMax(0., l_i); 264 /// a 'boxed constraint': l_i= ChMin(ChMax(min., l_i), max); etc. etc. 265 virtual void Project(); 266 267 /// Given the residual of the constraint computed as the 268 /// linear map mc_i = [Cq]*q + b_i + cfm*l_i , returns the 269 /// violation of the constraint, considering inequalities, etc. 270 /// For bilateral constraint, violation = mc_i. 271 /// For unilateral constraint, violation = min(mc_i, 0), 272 /// For boxed constraints or such, inherited class MAY OVERRIDE THIS! 273 virtual double Violation(double mc_i); 274 275 /// Puts the jacobian portions into the 'insrow' row of a sparse matrix, 276 /// where each portion of jacobian is shifted in order to match the 277 /// offset of the corresponding ChVariable. 278 virtual void Build_Cq(ChSparseMatrix& storage, int insrow) = 0; 279 280 /// Same as Build_Cq, but puts the _transposed_ jacobian row as a column. 281 virtual void Build_CqT(ChSparseMatrix& storage, int inscol) = 0; 282 283 /// Set offset in global q vector (set automatically by ChSystemDescriptor) SetOffset(int moff)284 void SetOffset(int moff) { offset = moff; } 285 286 /// Get offset in global q vector GetOffset()287 int GetOffset() const { return offset; } 288 289 /// Method to allow serialization of transient data to archives. 290 virtual void ArchiveOUT(ChArchiveOut& marchive); 291 292 /// Method to allow de-serialization of transient data from archives. 293 virtual void ArchiveIN(ChArchiveIn& marchive); 294 295 private: UpdateActiveFlag()296 void UpdateActiveFlag() { 297 this->active = (valid && !disabled && !redundant && !broken && mode != (CONSTRAINT_FREE)); 298 } 299 }; 300 301 } // end namespace chrono 302 303 #endif 304