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: Radu Serban 13 // 14 // Translational spring-damper-actuator (TSDA) with force optionally specified 15 // through a user-supplied functor object (default, linear TSDA). 16 // 17 // Optionally, a ChLinkTSDA can have internal dynamics, described by a system of 18 // ODEs. The internal states are integrated simultaneous with the containing 19 // system and they can be accessed and used in the force calculation. 20 // Such objects can be used in active suspension models. 21 // ============================================================================= 22 23 #ifndef CH_LINK_TSDA_H 24 #define CH_LINK_TSDA_H 25 26 #include "chrono/physics/ChLink.h" 27 #include "chrono/physics/ChBody.h" 28 #include "chrono/solver/ChVariablesGenericDiagonalMass.h" 29 #include "chrono/solver/ChKblockGeneric.h" 30 31 namespace chrono { 32 33 /// Class for translational spring-damper-actuator (TSDA) with the force optionally specified through a functor object 34 /// (default, linear TSDA). Optionally, a ChLinkTSDA can have internal dynamics, described by a system of ODEs. The 35 /// internal states are integrated simultaneous with the containing system and they can be accessed and used in the 36 /// force calculation. ChLinkTSDA provides optional support for computing Jacobians of the generalized forces. 37 class ChApi ChLinkTSDA : public ChLink { 38 public: 39 ChLinkTSDA(); 40 ChLinkTSDA(const ChLinkTSDA& other); 41 ~ChLinkTSDA(); 42 43 /// "Virtual" copy constructor (covariant return type). 44 virtual ChLinkTSDA* Clone() const override; 45 46 /// Set spring rest (free) length. 47 /// Optionally, the free length can be calculated from the initial configuration (see #Initialize). SetRestLength(double len)48 void SetRestLength(double len) { m_rest_length = len; } 49 50 /// Set spring coefficient (default: 0). 51 /// Used only if no force functor is provided. SetSpringCoefficient(double k)52 void SetSpringCoefficient(double k) { m_k = k; } 53 54 /// Set damping coefficient (default: 0). 55 /// Used only if no force functor is provided. SetDampingCoefficient(double r)56 void SetDampingCoefficient(double r) { m_r = r; } 57 58 /// Set constant actuation force (default: 0). 59 /// Used only if no force functor is provided. SetActuatorForce(double f)60 void SetActuatorForce(double f) { m_f = f; } 61 62 /// Declare the forces generated by this spring as stiff (default: false). 63 /// If stiff, Jacobian information will be generated. IsStiff(bool val)64 void IsStiff(bool val) { m_stiff = val; } 65 66 /// Get current states. GetStates()67 const ChVectorDynamic<>& GetStates() const { return m_states; } 68 69 /// Get the spring rest (free) length. GetRestLength()70 double GetRestLength() const { return m_rest_length; } 71 72 /// Get current length. GetLength()73 double GetLength() const { return m_length; } 74 75 /// Get current deformation. GetDeformation()76 double GetDeformation() const { return m_length - m_rest_length; } 77 78 /// Get current length rate of change. GetVelocity()79 double GetVelocity() const { return m_length_dt; } 80 81 /// Get current force (in the direction of the force element). GetForce()82 double GetForce() const { return m_force; } 83 84 /// Get the endpoint location on 1st body (expressed in body coordinate system) GetPoint1Rel()85 const ChVector<>& GetPoint1Rel() const { return m_loc1; } 86 87 /// Get the endpoint location on 1st body (expressed in absolute coordinate system) GetPoint1Abs()88 const ChVector<>& GetPoint1Abs() const { return m_aloc1; } 89 90 /// Get the endpoint location on 2nd body (expressed in body coordinate system) GetPoint2Rel()91 const ChVector<>& GetPoint2Rel() const { return m_loc2; } 92 93 /// Get the endpoint location on 1st body (expressed in absolute coordinate system) GetPoint2Abs()94 const ChVector<>& GetPoint2Abs() const { return m_aloc2; } 95 96 /// Get the value of the spring coefficient. 97 /// Meaningful only if no force functor is provided. GetSpringCoefficient()98 double GetSpringCoefficient() const { return m_k; } 99 100 /// Get the value of the damping coefficient. 101 /// Meaningful only if no force functor is provided. GetDampingCoefficient()102 double GetDampingCoefficient() const { return m_r; } 103 104 /// Get the constant acutation force. 105 /// Meaningful only if no force functor is provided. GetActuatorForce()106 double GetActuatorForce() const { return m_f; } 107 108 /// Class to be used as a callback interface for calculating the general spring-damper force. 109 class ChApi ForceFunctor { 110 public: ~ForceFunctor()111 virtual ~ForceFunctor() {} 112 113 /// Calculate and return the general spring-damper force at the specified configuration. 114 /// If the link has internal ODE states, the current states can be accessed with link->GetStates(). 115 virtual double operator()(double time, ///< current time 116 double rest_length, ///< undeformed length 117 double length, ///< current length 118 double vel, ///< current velocity (positive when extending) 119 ChLinkTSDA* link ///< back-pointer to associated link 120 ) = 0; 121 }; 122 123 /// Specify the functor object for calculating the force. RegisterForceFunctor(std::shared_ptr<ForceFunctor> functor)124 void RegisterForceFunctor(std::shared_ptr<ForceFunctor> functor) { m_force_fun = functor; } 125 126 /// Class to be used as a callback interface for specifying the ODE, y' = f(t,y); y(0) = y0. 127 class ChApi ODE { 128 public: ~ODE()129 virtual ~ODE() {} 130 131 /// Specify number of states (dimension of y). 132 virtual int GetNumStates() const = 0; 133 134 /// Set initial conditions. 135 /// Must load y0 = y(0). 136 virtual void SetInitialConditions(ChVectorDynamic<>& states, ///< output initial conditions vector 137 ChLinkTSDA* link ///< back-pointer to associated link 138 ) = 0; 139 140 /// Calculate and return the ODE right-hand side at the provided time and states. 141 /// Must load f(t,y). 142 virtual void CalculateRHS(double time, ///< current time 143 const ChVectorDynamic<>& states, ///< current ODE states 144 ChVectorDynamic<>& rhs, ///< output ODE right-hand side vector 145 ChLinkTSDA* link ///< back-pointer to associated link 146 ) = 0; 147 148 /// Calculate the Jacobian of the ODE right-hand side with rerspect to the ODE states. 149 /// Only used if the link force is declared as stiff. If provided, load df/dy into the provided matrix 'jac' 150 /// (already set to zero before the call) and return 'true'. In that case, the user-provided Jacobian will 151 /// overwrite the default finite-difference approximation. CalculateJac(double time,const ChVectorDynamic<> & states,const ChVectorDynamic<> & rhs,ChMatrixDynamic<> & jac,ChLinkTSDA * link)152 virtual bool CalculateJac(double time, ///< current time 153 const ChVectorDynamic<>& states, ///< current ODE states 154 const ChVectorDynamic<>& rhs, ///< current ODE right-hand side vector 155 ChMatrixDynamic<>& jac, ///< output Jacobian matrix 156 ChLinkTSDA* link ///< back-pointer to associated link 157 ) { 158 return false; 159 } 160 }; 161 162 /// Specify the functor object for calculating the ODE right-hand side. 163 void RegisterODE(ODE* functor); 164 165 /// Initialize the spring, specifying the two bodies to be connected, the location of the two anchor points of each 166 /// body (each expressed in body or absolute coordinates), and the imposed rest length of the spring. 167 void Initialize( 168 std::shared_ptr<ChBody> body1, ///< first body to link 169 std::shared_ptr<ChBody> body2, ///< second body to link 170 bool pos_are_relative, ///< if true, point locations are relative to bodies 171 ChVector<> loc1, ///< point on 1st body (rel. or abs., see flag above) 172 ChVector<> loc2, ///< point on 2nd body (rel. or abs., see flag above) 173 bool auto_rest_length = true, ///< if true, initializes the rest length as the distance between loc1 and loc2 174 double rest_length = 0 ///< rest length (no need to define if auto_rest_length=true.) 175 ); 176 177 /// Method to allow serialization of transient data to archives. 178 virtual void ArchiveOUT(ChArchiveOut& marchive) override; 179 180 /// Method to allow deserialization of transient data from archives. 181 virtual void ArchiveIN(ChArchiveIn& marchive) override; 182 183 private: 184 virtual void Update(double mytime, bool update_assets = true) override; 185 GetDOF()186 virtual int GetDOF() override { return m_nstates; } 187 188 // Interface to solver Variables()189 ChVariables& Variables() { return *m_variables; } 190 virtual void InjectVariables(ChSystemDescriptor& descriptor) override; 191 virtual void InjectKRMmatrices(ChSystemDescriptor& descriptor) override; 192 193 virtual void IntStateGather(const unsigned int off_x, 194 ChState& x, 195 const unsigned int off_v, 196 ChStateDelta& v, 197 double& T) override; 198 virtual void IntStateScatter(const unsigned int off_x, 199 const ChState& x, 200 const unsigned int off_v, 201 const ChStateDelta& v, 202 const double T, 203 bool full_update) override; 204 virtual void IntStateGatherAcceleration(const unsigned int off_a, ChStateDelta& a) override; 205 virtual void IntStateScatterAcceleration(const unsigned int off_a, const ChStateDelta& a) override; 206 virtual void IntLoadResidual_F(const unsigned int off, ChVectorDynamic<>& R, const double c) override; 207 virtual void IntLoadResidual_Mv(const unsigned int off, 208 ChVectorDynamic<>& R, 209 const ChVectorDynamic<>& v, 210 const double c) override; 211 virtual void IntToDescriptor(const unsigned int off_v, 212 const ChStateDelta& v, 213 const ChVectorDynamic<>& R, 214 const unsigned int off_L, 215 const ChVectorDynamic<>& L, 216 const ChVectorDynamic<>& Qc) override; 217 virtual void IntFromDescriptor(const unsigned int off_v, 218 ChStateDelta& v, 219 const unsigned int off_L, 220 ChVectorDynamic<>& L) override; 221 222 virtual void KRMmatricesLoad(double Kfactor, double Rfactor, double Mfactor) override; 223 224 // Interface to the solver (old style) 225 virtual void VariablesFbReset() override; 226 virtual void VariablesFbLoadForces(double factor = 1) override; 227 virtual void VariablesQbLoadSpeed() override; 228 virtual void VariablesQbSetSpeed(double step = 0) override; 229 virtual void VariablesFbIncrementMq() override; 230 virtual void VariablesQbIncrementPosition(double step) override; 231 virtual void ConstraintsFbLoadForces(double factor = 1) override; 232 233 /// Manager class for Jacobians matrices. \n 234 /// - Variables associated with this link (order important): body1, body2, ODE states. \n 235 /// - Generalized forces for link (order important): applied force on body1, applied force on body2, ODE RHS. \n 236 /// The K and R blocks in m_KRM have the form: [A B], \n 237 /// with A of size (12+nstates) x 12 and B of size (12+nstates) x nstates. \n 238 /// These blocks are computed using finite-differences. However, if the user-provided ODE class implements 239 /// CalculateJac, that will be used to override the bottom-right (nstates x nstates) block of R. 240 class SpringJacobians { 241 public: 242 ChKblockGeneric m_KRM; ///< linear combination of K, R, M for the variables associated with this link 243 ChMatrixDynamic<> m_J; ///< Jacobian of ODE right-hand side with respect to ODE states (contributes to R term) 244 ChMatrixDynamic<> m_K; ///< K contribution from this link 245 ChMatrixDynamic<> m_R; ///< R contribution from this link 246 }; 247 248 /// Compute generalized forces Q given the packed states and state derivatives. 249 void ComputeQ(double time, ///< current time 250 const ChState& state_x, ///< state position to evaluate Q 251 const ChStateDelta& state_w, ///< state speed to evaluate Q 252 ChVectorDynamic<>& Qforce ///< output forcing vector 253 ); 254 255 /// Create the Jacobian matrices, allocate space, associate variables. 256 void CreateJacobianMatrices(); 257 258 /// Compute the Jacobian of the generalized forcing with respect to states of the two connected bodies and internal 259 /// states (as needed). Most of this information is computed using forward finite-differences. 260 void ComputeJacobians(double time, ///< current time 261 const ChState& state_x, ///< state position to evaluate jacobians 262 const ChStateDelta& state_w ///< state speed to evaluate jacobians 263 ); 264 265 ChVector<> m_loc1; ///< location of end point on body1 (relative to body1) 266 ChVector<> m_loc2; ///< location of end point on body2 (relative to body1) 267 ChVector<> m_aloc1; ///< location of end point on body1 (absolute) 268 ChVector<> m_aloc2; ///< location of end point on body2 (absolute) 269 double m_rest_length; ///< undeformed length 270 double m_length; ///< current length 271 double m_length_dt; ///< current length rate of change 272 273 bool m_stiff; ///< true if loads are stiff (triggers Jacobian calculation) 274 275 double m_k; ///< spring coefficient (if no force functor provided) 276 double m_r; ///< damping coefficient (if no force functor provided) 277 double m_f; ///< constant actuation (if no force functor provided) 278 279 std::shared_ptr<ForceFunctor> m_force_fun; ///< functor for force calculation 280 double m_force; ///< force in distance coordinates 281 282 ODE* m_ode_fun; ///< functor for ODE specification 283 int m_nstates; ///< number of internal ODE states 284 ChVectorDynamic<> m_states; ///< vector of internal ODE states 285 ChVariablesGenericDiagonalMass* m_variables; ///< carrier for internal dynamics states 286 287 ChVectorDynamic<> m_Qforce; ///< generalized forcing terms 288 SpringJacobians* m_jacobians; ///< Jacobian information 289 290 static const double m_FD_delta; ///< perturbation for finite-difference Jacobian approximation 291 292 friend class ChSystemMulticore; 293 }; 294 295 CH_CLASS_VERSION(ChLinkTSDA, 0) 296 297 } // end namespace chrono 298 299 #endif 300