1 /* 2 * MBDyn (C) is a multibody analysis code. 3 * http://www.mbdyn.org 4 * 5 * Copyright (C) 1996-2010 6 * 7 * Pierangelo Masarati <masarati@aero.polimi.it> 8 * Paolo Mantegazza <mantegazza@aero.polimi.it> 9 * 10 * Dipartimento di Ingegneria Aerospaziale - Politecnico di Milano 11 * via La Masa, 34 - 20156 Milano, Italy 12 * http://www.aero.polimi.it 13 * 14 * Changing this copyright notice is forbidden. 15 * 16 * This program is free software; you can redistribute it and/or modify 17 * it under the terms of the GNU General Public License as published by 18 * the Free Software Foundation (version 2 of the License). 19 * 20 * 21 * This program is distributed in the hope that it will be useful, 22 * but WITHOUT ANY WARRANTY; without even the implied warranty of 23 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 24 * GNU General Public License for more details. 25 * 26 * You should have received a copy of the GNU General Public License 27 * along with this program; if not, write to the Free Software 28 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 29 */ 30 /* 31 * Author: Louis Gagnon <louis.gagnon.10@ulaval.ca> 32 * Departement de genie mecanique 33 * Universite Laval 34 * http://www.gmc.ulaval.ca 35 */ 36 37 #ifndef MODULE_WHEEL4_H 38 #define MODULE_WHEEL4_H 39 40 #include "dataman.h" 41 #include "userelem.h" 42 #include "drive_.h" // to get current time (may not be needed in the end) 43 44 //#include "joint.h" // to get information on viscoelastic elements 45 46 class Wheel4 47 : virtual public Elem, public UserDefinedElem 48 { 49 private: 50 51 // patch degrees of freedom, 52 doublereal dXx, dXy, dVx, dVy, dVPx, dVPy, dXPx, dXPy; 53 // ,patch degrees of freedom 54 55 56 // wheel node 57 StructNode *pWheel; 58 Elem *pWheelB; // body 59 Elem *pWheelE; // user loadable elem 60 61 // ring node 62 StructNode *pRing; 63 Elem *pRingB; //body 64 65 // ring node 66 StructNode *pPatch; 67 68 69 // wheel axle direction (wrt/ wheel node) 70 Vec3 WheelAxle; 71 72 // (flat) ground node 73 StructNode *pGround; 74 75 76 77 // friction data 78 bool bSlip; 79 bool bLoadedRadius; 80 DriveCaller *pMuX0; 81 DriveCaller *pMuY0; 82 DriveCaller *pTr; 83 DriveCaller *pMzr; 84 // DriveCaller *pMuY1; 85 86 // centripetal force calculation 87 doublereal rRatio; // ratio btwn portion of ring in contact with patch and the total ring 88 doublereal Krz; // vertical wheel-ring stiffness 89 90 //time variant coefficients 91 DriveCaller *pKpa; 92 doublereal dKpa; 93 Vec3 Kpatv; 94 DriveCaller *pCpa; 95 doublereal dCpa; 96 Vec3 Cpatv; 97 98 //road elevation 99 DriveCaller *pRoad; 100 doublereal dRoad; 101 doublereal dRoadAhead; // for two-point follower 102 doublereal dRoadBehind; // for two-point follower 103 doublereal dRoadInitial; // road height for simulation onset 104 doublereal dRoadPrev; 105 doublereal dRoadPrevPrev; 106 107 // thresholds 108 doublereal TRH; // prevents division by zero at null x-velocity at the price of losing validity for velocities above -TRH*1.1 and below TRH*1.1 109 doublereal TRHA; // buffer used to prevent division by zero 110 doublereal TRHT; // prevents division by zero for computation of the angle of the car (and wheels) 111 doublereal TRHTA; // buffer used on angle zero division prevention 112 doublereal TRHC; // cap on kappa 113 doublereal TdLs; // minimum value (cap) for dLs (half-length of two-point follower) 114 doublereal TdReDiv; // minimum value (cap) for divider used to find R_e (effective rolling radius) 115 doublereal TdR_e; // minimum value (cap) for R_e (effective rolling radius) 116 doublereal RDA; // x-pos prior to which road profile is null and after which is interpolated between 0 and value at RDB 117 doublereal RDB; // x-pos where road starts to be taken as fed by driver but using x=0 @ RDB 118 doublereal RDL; // after x reaches RDB+RDL, the road profile will loop over RDL 119 int RDLC; // number of times to deduct x pos from current pos for looping 120 121 // tire data, 122 Vec3 Xpa; // elongation of the contact patch spring 123 Vec3 Xpar; //relative to point on ring 124 Vec3 distM; // the real physical distance between center of ring and patch after rotation of the patch to respect the slope 125 doublereal ddistM; // the magnitude of the home-made loaded radius distM 126 doublereal dEffRad; // wheel effective radius 127 Vec3 EffRad; // physical (artificial rotation done) vector distance between wheel center and patch 128 doublereal R_e; // wheel effective radius computed only for output 129 Vec3 XparPrev; //relative to point on ring, previous timestep 130 Vec3 XpaPrev; // elongation of the contact patch spring previous timestep 131 Vec3 XparPrevPrev; //relative to point on ring, previous previous timestep 132 Vec3 XpaPrevPrev; // elongation of the contact patch spring, previous previous timestep 133 Vec3 Kpa; // stiffness 134 Vec3 Cpa; // damping 135 Vec3 XpaBC; // elongation (POSITION?) of contact patch before convergence is confirmed 136 Vec3 VpaBC; // elongation velocity of contact patch before convergence is confirmed 137 Vec3 RpaBC; // angle of contact patch before convergence is confirmed 138 Vec3 WpaBC; // angular of contact patch before convergence is confirmed 139 Vec3 Vpa; 140 Vec3 Vpar; // relative to point on ring 141 Vec3 VparWheel; // relative vel betwn patch and wheel 142 Vec3 VpaPrev; 143 Vec3 Fint; // viscoelastic element force between ring and patch 144 Vec3 Fint_old; // to keep value of Fint prior to rotation back into abs. ref. frame 145 Vec3 Fint_ring; // viscoelastic element force between ring and patch used for artificial patch rotation 146 Vec3 Fpatch; // force on patch 147 Vec3 Mint; // viscoelastic element moment between ring and patch 148 doublereal Mpa; 149 doublereal tr; // pneumatic trail 150 doublereal S_ht; // horizontal shift of pneumatic trail 151 doublereal S_hf; // horizontal shift of residual torque (equal horiz shift of lateral force) 152 doublereal M_zr; // residual torque 153 doublereal dt; //timestep 154 bool bSwift; 155 doublereal curTime; //current time 156 doublereal oldTime; //time at previous timestep 157 bool bFirstAC; // first timestep after convergence (will not reset if timestep changes) 158 bool bFirstAP; // first iter after prediction (resets if timestep changed) 159 DriveOwner tdc; // time drive 160 Vec3 zZero; // to remove a z component of a vector 161 Vec3 pcRing; // contact point of ring 162 Vec3 pcRingPrev; // contact point of ring previous timestep 163 Vec3 RingRad; // vector radius of ring 164 Vec3 RingRadPrev; // previous vector radius of ring 165 Vec3 VpcRingPrev; 166 Vec3 VpcRing; 167 Vec3 VpcRingPrevPrev; 168 Vec3 Xring; // abs pos of ring axle 169 Vec3 Vwheel; // current velocity of wheel 170 Vec3 fwd; // forward direction of wheel in absol. ref frame 171 Vec3 fwdRing; // forward direction of ring in absol. ref frame 172 Vec3 fwdRingFlat; // forward direction of ring in absol. ref frame disregarding inclination of the slope 173 Vec3 lat; // lateral direction of wheel in absol. ref frame 174 Vec3 latRing; // lateral direction of ring in absol. ref frame 175 Vec3 latRingFlat; // lateral direction of ring in absol. ref frame disregarding inclination of the slope 176 Vec3 n; // ground orientation in the absolute frame 177 Vec3 nPrev; // ground orientation in the absolute frame previous timestep 178 Vec3 fwdRingPrev; // fwd unit vector of the ring node at previous timestep 179 doublereal dRoadVel; // road velocity in the z-dir 180 Vec3 Xparp; // rotated relative displacement vector of the patch prior to multiplication by the stiffness 181 Vec3 Vparp; // rotated relative velocity vector of the patch prior to multiplication by the viscosity 182 doublereal Fn; // force on patch in z-direction 183 doublereal Fcent; // centrifugal force calculated from tire properties and velocity and "applied to patch" 184 doublereal dCt; // calculated displacement induced by centrifugal force (calculated before Fcent) 185 Vec3 Fr; // rolling resistance force vector that points in the direction of the ring 186 bool boolFn; // null if Fn < 0 to help get a proper jacobian 187 Vec3 i; // unit vector in x-dir 188 Vec3 j; // unit vector in y-dir 189 Vec3 k; // unit vector in z-dir 190 doublereal dLs; // half-width of the tandem elliptical cam follower 191 doublereal dPls; // ratio of lengths between two point follower and contact patch length 192 doublereal dR_a1; // r_a1 contact length parameter from Besselink eq. 4.85 193 doublereal dR_a2; // r_a2 contact length parameter from Besselink eq. 4.85 194 doublereal dLsProj; // half-legnth in abs x-dir of the two point follower projected on the abs x dir 195 doublereal dXxProj; // projected x-pos of patch according to road inclination 196 doublereal dXxProjPrev; // previous projected x-pos of patch, used to predict the next position 197 doublereal dLsProjPrev; // previous projected half length of contact patch, used to predict the next position 198 doublereal dt_maxF; // maximum divison of dt per timestep 199 doublereal dt_minF; // minimum division of dt per timestep 200 doublereal dLsProjRatio; // ratio of length of dLs when projected, used for timestep size calculation 201 doublereal dtPrev; // previous timestep, used to predict the next position 202 doublereal dt_maxH; // max height of step in vertical direction wanted, drive the timestep control 203 doublereal dt_adjFactor; // // factor by which the current time step is too big for the bump to come in dt_numAhead times the previous step distance 204 doublereal dt_fNow; // factor to apply now to current timestep (ie: divide current timestep by this for the next one) 205 doublereal dt_maxstep; // maximum timestep given to input file, needed by timestep driver 206 doublereal dt_minstep; // minimum timestep given to input file, needed by timestep driver 207 bool dt_On; // bool to enable or disable adjustable timestep calculation 208 doublereal dt_Res; // resolution of bump search, keep sufficiently smaller than ring radius (ie: look at every dt_Res meters for a bump in front of the tire) NOTE: will not look behind because it is assumed that the model is not made to move rearwards and that a small move rearward would not influence the timestep because it should already have been adjusted for the bump when driving towards it) 209 doublereal dtMax; // maximum recommended timestep 210 int dt_numAhead; // number of steps by which to look ahead for a bump in the road profile (timestep controller, not defined by user) 211 int dt_minStepsCycle; // minimum number of steps wanted in a force cycle (approx., influences dt) 212 doublereal dt_divF; // factor by which to divide the timestep if the force oscillates more than wanted (as determined by TminS) 213 doublereal dt_divF3; // factor if the force changes 3 times of sign in the last dt_minStepsCycle steps 214 doublereal dt_divF4; // factor if the force changes 4 or more times of sign in the last dt_minStepsCycle steps 215 std::vector< std::vector<int> > FintSignCk; 216 Vec3 FintPrev; // previous Fint for timestep control 217 Vec3 FintPrevPrev; // previous-previous Fint for timestep control 218 Vec3 XparpPrev; // previous Xpar for timestep control 219 Vec3 XparpPrevPrev; // previous-previous Xparp for timestep control 220 doublereal dn; // magnitude square of normal vector to road (n) 221 doublereal dvx; // relative speed between center of wheel and contact point on tire in the forward direction 222 doublereal dvax; // speed of axle in the forward direction 223 doublereal dvay; // speed of axle in the lateral direction 224 Vec3 va; // relative speed between wheel axle and ground 225 doublereal dXxPrev; // x-pos of patch at previous timestep 226 doublereal E; // total system kin + pot energy 227 doublereal KE; // total system kin energy 228 doublereal PE; // total system pot energy 229 // ,tire data 230 231 232 233 // variables used by the Jacobian, 234 235 doublereal derivSign; // ensures the proper derivative sign by respecting the abs(dvax) present function. 236 bool latBool; // determines whether lateral forces will contribute to the Jacobian 237 bool fwdBool; // determines whether longitudinal forces will contribute to the Jacobian 238 239 // ,variables used by the Jacobian 240 241 // output data 242 Vec3 F; 243 Vec3 M; 244 Vec3 Mz; 245 doublereal dR_0; // Rigid ring radius 246 doublereal deltaPrev; // tire deflection, from previous timestep 247 doublereal dSr; // long. slip ratio 248 doublereal dSa; // lateral slip angle 249 doublereal dAlpha; 250 doublereal dAlpha_t; 251 doublereal dAlpha_r; 252 doublereal dMuX; 253 doublereal dMuY; 254 doublereal q_sy1; // should be between 0.01 and 0.02 usually... 255 doublereal q_sy3; // 256 doublereal dvao; // reference velocity for rolling resistance velocity influence factor 257 258 bool firstRes; // this_is_the_first_residual 259 260 doublereal dDebug; // onloy used for debugging output 261 262 263 // secant of a scalar sec(doublereal x)264 doublereal sec(doublereal x) const { 265 return 1.0 / cos(x); 266 }; 267 sign(const doublereal x)268 int sign(const doublereal x) { 269 if (x >= 0.) { 270 return 1; 271 } else if (x < 0.) { 272 return -1; 273 } 274 return 0; 275 }; 276 277 #ifdef USE_NETCDF 278 NcVar *Var_Fint; 279 NcVar *Var_Xpar; 280 NcVar *Var_Xparp; 281 NcVar *Var_dXxProj; 282 NcVar *Var_dRoad; 283 NcVar *Var_F; 284 NcVar *Var_Fn; 285 NcVar *Var_debug; 286 NcVar *Var_dSr; 287 NcVar *Var_ddistM; 288 NcVar *Var_Fcent; 289 NcVar *Var_dLs; 290 NcVar *Var_R_e; 291 NcVar *Var_dSa; 292 NcVar *Var_dvax; 293 NcVar *Var_dvx; 294 NcVar *Var_dvay; 295 NcVar *Var_dMuY; 296 NcVar *Var_dMuX; 297 NcVar *Var_KE; 298 NcVar *Var_PE; 299 NcVar *Var_E; 300 NcVar *Var_dRoadAhead; 301 NcVar *Var_dRoadBehind; 302 NcVar *Var_dCt; 303 NcVar *Var_M; 304 NcVar *Var_distM; 305 NcVar *Var_n; 306 NcVar *Var_Xpa; 307 NcVar *Var_Vpa; 308 NcVar *Var_Vpar; 309 NcVar *Var_fwd; 310 NcVar *Var_fwdRing; 311 NcVar *Var_fwdRingFlat; 312 NcVar *Var_pcRing; 313 NcVar *Var_VparWheel; 314 NcVar *Var_Fr; 315 NcVar *Var_Mz; 316 317 #endif /* USE_NETCDF */ 318 319 public: 320 Wheel4(unsigned uLabel, const DofOwner *pDO, 321 DataManager* pDM, MBDynParser& HP); 322 virtual ~Wheel4(void); 323 324 virtual void OutputPrepare(OutputHandler &OH); 325 virtual void Output(OutputHandler& OH) const; 326 virtual void WorkSpaceDim(integer* piNumRows, integer* piNumCols) const; 327 VariableSubMatrixHandler& 328 AssJac(VariableSubMatrixHandler& WorkMat, 329 doublereal dCoef, 330 const VectorHandler& XCurr, 331 const VectorHandler& XPrimeCurr); 332 SubVectorHandler& 333 AssRes(SubVectorHandler& WorkVec, 334 doublereal dCoef, 335 const VectorHandler& XCurr, 336 const VectorHandler& XPrimeCurr); 337 338 // /* enables access to private data */ 339 unsigned int iGetNumPrivData(void) const; 340 unsigned int iGetPrivDataIdx(const char *s) const; 341 doublereal dGetPrivData(unsigned int i) const; 342 343 int iGetNumConnectedNodes(void) const; 344 virtual void SetInitialValue(VectorHandler& XCurr); 345 void GetConnectedNodes(std::vector<const Node *>& connectedNodes) const; 346 void SetValue(DataManager *pDM, VectorHandler& X, VectorHandler& XP, 347 SimulationEntity::Hints *ph); 348 349 350 virtual unsigned int iGetNumDof(void) const; 351 virtual DofOrder::Order GetDofType(unsigned int i) const; 352 virtual DofOrder::Order GetEqType(unsigned int i) const; 353 354 355 std::ostream& Restart(std::ostream& out) const; 356 virtual unsigned int iGetInitialNumDof(void) const; 357 virtual void 358 InitialWorkSpaceDim(integer* piNumRows, integer* piNumCols) const; 359 VariableSubMatrixHandler& 360 InitialAssJac(VariableSubMatrixHandler& WorkMat, 361 const VectorHandler& XCurr); 362 SubVectorHandler& 363 InitialAssRes(SubVectorHandler& WorkVec, const VectorHandler& XCurr); 364 365 // /* enables access to private data */ 366 // virtual unsigned int iGetNumPrivData(void) const; 367 // virtual unsigned int iGetPrivDataIdx(const char *s) const; 368 // virtual doublereal dGetPrivData(unsigned int i) const; 369 370 void AfterConvergence(const VectorHandler& X, 371 const VectorHandler& XP); // to call function after convergence of current step 372 void AfterPredict(VectorHandler& X, VectorHandler& XP); // at beginning of iteration 373 374 375 void CalculateR_e(); 376 doublereal CapLoop(doublereal Xuncapped) const; 377 // void NetCDFPrepare(OutputHandler &OH, char &buf) const; 378 379 380 }; 381 382 #endif // ! MODULE_WHEEL4_H 383 384 385 /* Wheel4 - end */ 386 387 /* TimeStep - begin */ 388 #ifndef MODULE_TIMESTEP_H 389 #define MODULE_TIMESTEP_H 390 391 class TimeStep 392 : virtual public Elem, public UserDefinedElem { 393 private: 394 395 Elem *pWheelE; // user loadable elem 396 std::vector<Elem *> pWheelsE; 397 // DriveOwner tdc; // time drive 398 399 400 public: 401 TimeStep(unsigned uLabel, const DofOwner *pDO, 402 DataManager* pDM, MBDynParser& HP); 403 virtual ~TimeStep(void); 404 405 virtual void Output(OutputHandler& OH) const; 406 virtual void WorkSpaceDim(integer* piNumRows, integer* piNumCols) const; 407 VariableSubMatrixHandler& 408 AssJac(VariableSubMatrixHandler& WorkMat, 409 doublereal dCoef, 410 const VectorHandler& XCurr, 411 const VectorHandler& XPrimeCurr); 412 SubVectorHandler& 413 AssRes(SubVectorHandler& WorkVec, 414 doublereal dCoef, 415 const VectorHandler& XCurr, 416 const VectorHandler& XPrimeCurr); 417 418 // /* enables access to private data */ 419 unsigned int iGetNumPrivData(void) const; 420 unsigned int iGetPrivDataIdx(const char *s) const; 421 doublereal dGetPrivData(unsigned int i) const; 422 423 int iGetNumConnectedNodes(void) const; 424 void GetConnectedNodes(std::vector<const Node *>& connectedNodes) const; 425 void SetValue(DataManager *pDM, VectorHandler& X, VectorHandler& XP, 426 SimulationEntity::Hints *ph); 427 std::ostream& Restart(std::ostream& out) const; 428 virtual unsigned int iGetInitialNumDof(void) const; 429 virtual void 430 InitialWorkSpaceDim(integer* piNumRows, integer* piNumCols) const; 431 VariableSubMatrixHandler& 432 InitialAssJac(VariableSubMatrixHandler& WorkMat, 433 const VectorHandler& XCurr); 434 SubVectorHandler& 435 InitialAssRes(SubVectorHandler& WorkVec, const VectorHandler& XCurr); 436 }; 437 438 #endif // ! MODULE_TIMESTEP_H 439