1 /* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/modules/module-wheel2/module-wheel2.cc,v 1.75 2017/01/12 14:58:41 masarati Exp $ */
2 /*
3 * MBDyn (C) is a multibody analysis code.
4 * http://www.mbdyn.org
5 *
6 * Copyright (C) 1996-2017
7 *
8 * Pierangelo Masarati <masarati@aero.polimi.it>
9 * Paolo Mantegazza <mantegazza@aero.polimi.it>
10 *
11 * Dipartimento di Ingegneria Aerospaziale - Politecnico di Milano
12 * via La Masa, 34 - 20156 Milano, Italy
13 * http://www.aero.polimi.it
14 *
15 * Changing this copyright notice is forbidden.
16 *
17 * This program is free software; you can redistribute it and/or modify
18 * it under the terms of the GNU General Public License as published by
19 * the Free Software Foundation (version 2 of the License).
20 *
21 *
22 * This program is distributed in the hope that it will be useful,
23 * but WITHOUT ANY WARRANTY; without even the implied warranty of
24 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
25 * GNU General Public License for more details.
26 *
27 * You should have received a copy of the GNU General Public License
28 * along with this program; if not, write to the Free Software
29 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
30 */
31
32 #include "mbconfig.h" /* This goes first in every *.c,*.cc file */
33
34 #include "dataman.h"
35 #include "userelem.h"
36
37 #include <iostream>
38 #include <limits>
39 #include <cfloat>
40 #include <limits>
41
42 #include "module-wheel2.h"
43
44 class Wheel2
45 : virtual public Elem, public UserDefinedElem
46 {
47 private:
48
49 // wheel node
50 const StructNode *pWheel;
51
52 // wheel axle direction (wrt/ wheel node)
53 Vec3 WheelAxle;
54
55 // (flat) ground node
56 const StructNode *pGround;
57
58 // ground position/orientation (wrt/ ground node)
59 Vec3 GroundPosition;
60 Vec3 GroundDirection;
61
62 // wheel geometry
63 doublereal dRadius;
64 doublereal dInternalRadius;
65 doublereal dVolCoef;
66
67 doublereal dRefArea;
68 doublereal dRNP; /* R+nG'*pG */
69 doublereal dV0;
70
71 // tyre properties
72 doublereal dP0;
73 doublereal dGamma;
74 doublereal dHystVRef;
75
76 // friction data
77 bool bSlip;
78 const DriveCaller *pMuX0;
79 const DriveCaller *pMuY0;
80 const DriveCaller *pMuY1;
81 doublereal dvThreshold;
82
83 // output data
84 Vec3 F;
85 Vec3 M;
86 doublereal dInstRadius;
87 doublereal dDeltaL;
88 doublereal dVn;
89 doublereal dSr;
90 doublereal dAlpha;
91 doublereal dAlphaThreshold;
92 doublereal dMuX;
93 doublereal dMuY;
94 doublereal dVa;
95 doublereal dVc;
96
97 public:
98 Wheel2(unsigned uLabel, const DofOwner *pDO,
99 DataManager* pDM, MBDynParser& HP);
100 virtual ~Wheel2(void);
101
102 virtual void Output(OutputHandler& OH) const;
103 virtual void WorkSpaceDim(integer* piNumRows, integer* piNumCols) const;
104 VariableSubMatrixHandler&
105 AssJac(VariableSubMatrixHandler& WorkMat,
106 doublereal dCoef,
107 const VectorHandler& XCurr,
108 const VectorHandler& XPrimeCurr);
109 SubVectorHandler&
110 AssRes(SubVectorHandler& WorkVec,
111 doublereal dCoef,
112 const VectorHandler& XCurr,
113 const VectorHandler& XPrimeCurr);
114 unsigned int iGetNumPrivData(void) const;
115 int iGetNumConnectedNodes(void) const;
116 void GetConnectedNodes(std::vector<const Node *>& connectedNodes) const;
117 void SetValue(DataManager *pDM, VectorHandler& X, VectorHandler& XP,
118 SimulationEntity::Hints *ph);
119 std::ostream& Restart(std::ostream& out) const;
120 virtual unsigned int iGetInitialNumDof(void) const;
121 virtual void
122 InitialWorkSpaceDim(integer* piNumRows, integer* piNumCols) const;
123 VariableSubMatrixHandler&
124 InitialAssJac(VariableSubMatrixHandler& WorkMat,
125 const VectorHandler& XCurr);
126 SubVectorHandler&
127 InitialAssRes(SubVectorHandler& WorkVec, const VectorHandler& XCurr);
128 };
129
Wheel2(unsigned uLabel,const DofOwner * pDO,DataManager * pDM,MBDynParser & HP)130 Wheel2::Wheel2(unsigned uLabel, const DofOwner *pDO,
131 DataManager* pDM, MBDynParser& HP)
132 : Elem(uLabel, flag(0)),
133 UserDefinedElem(uLabel, pDO)
134 {
135 // help
136 if (HP.IsKeyWord("help")) {
137 silent_cout(
138 " \n"
139 "Module: wheel2 \n"
140 "Author: Stefania Gualdi <gualdi@aero.polimi.it> \n"
141 " Pierangelo Masarati <masarati@aero.polimi.it> \n"
142 "Organization: Dipartimento di Ingegneria Aerospaziale \n"
143 " Politecnico di Milano \n"
144 " http://www.aero.polimi.it \n"
145 " \n"
146 " All rights reserved \n"
147 " \n"
148 "Connects 2 structural nodes: \n"
149 " - Wheel \n"
150 " - Ground \n"
151 " \n"
152 "Note: \n"
153 " - The Axle and the Wheel structural nodes must be connected \n"
154 " by a joint that allows relative rotations only about \n"
155 " one axis (the axle) \n"
156 " - The center of the wheel is assumed coincident with \n"
157 " the position of the wheel structural node \n"
158 " - The Ground structural node supports a plane defined \n"
159 " a point and a direction orthogonal to the plane (future \n"
160 " versions might use an arbitrary, deformable surface) \n"
161 " - The forces are applied at the \"contact point\", that \n"
162 " is defined according to geometrical properties \n"
163 " of the system and according to the relative position \n"
164 " and orientation of the Wheel and Ground structural nodes \n"
165 " \n"
166 " - Input: \n"
167 " <wheel structural node label> , \n"
168 " <wheel axle direction> , \n"
169 " <ground structural node label> , \n"
170 " <reference point position of the ground plane> , \n"
171 " <direction orthogonal to the ground plane> , \n"
172 " <wheel radius> , \n"
173 " <torus radius> , \n"
174 " <volume coefficient (black magic?)> , \n"
175 " <tire pressure> , \n"
176 " <tire polytropic exponent> , \n"
177 " <reference velocity for tire hysteresis> \n"
178 " [ slip , \n"
179 " <longitudinal friction coefficient drive> \n"
180 " <lateral friction coefficient drive for s.r.=0> \n"
181 " <lateral friction coefficient drive for s.r.=1> \n"
182 " [ , threshold , <slip ratio velocity threshold> , \n"
183 " <slip angle velocity threshold> ] ] \n"
184 " \n"
185 " - Output: \n"
186 " 1) element label \n"
187 " 2-4) tire force in global reference frame \n"
188 " 5-7) tire couple in global reference frame \n"
189 " 8) effective radius \n"
190 " 9) tire radial deformation \n"
191 " 10) tire radial deformation velocity \n"
192 " 11) slip ratio \n"
193 " 12) slip angle \n"
194 " 13) longitudinal friction coefficient \n"
195 " 14) lateral friction coefficient \n"
196 " 15) axis relative tangential velocity \n"
197 " 16) point of contact relative tangential velocity \n"
198 << std::endl);
199
200 if (!HP.IsArg()) {
201 /*
202 * Exit quietly if nothing else is provided
203 */
204 throw NoErr(MBDYN_EXCEPT_ARGS);
205 }
206 }
207
208 // read wheel node
209 pWheel = dynamic_cast<const StructNode *>(pDM->ReadNode(HP, Node::STRUCTURAL));
210
211 // read wheel axle
212 ReferenceFrame RF = ReferenceFrame(pWheel);
213 WheelAxle = HP.GetVecRel(RF);
214
215 // read ground node
216 pGround = dynamic_cast<const StructNode *>(pDM->ReadNode(HP, Node::STRUCTURAL));
217
218 // read ground position/orientation
219 RF = ReferenceFrame(pGround);
220 GroundPosition = HP.GetPosRel(RF);
221 GroundDirection = HP.GetVecRel(RF);
222
223 // normalize ground orientation
224 doublereal d = GroundDirection.Dot();
225 if (d <= std::numeric_limits<doublereal>::epsilon()) {
226 silent_cerr("Wheel2(" << uLabel << "): "
227 "null direction at line " << HP.GetLineData()
228 << std::endl);
229 throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
230 }
231 GroundDirection /= sqrt(d);
232
233 // wheel geometry
234 dRadius = HP.GetReal();
235 dInternalRadius = HP.GetReal();
236 dVolCoef = HP.GetReal();
237
238 // reference area
239 dRefArea = 3.7*dInternalRadius*std::sqrt(dInternalRadius*(2.*dRadius - dInternalRadius));
240
241 // parameter required to compute Delta L
242 dRNP = dRadius + GroundPosition*GroundDirection;
243
244 // reference volume
245 dV0 = 2.*M_PI*(dRadius - dInternalRadius)
246 *M_PI*dInternalRadius*dInternalRadius*dVolCoef;
247
248 // tyre properties
249 dP0 = HP.GetReal();
250 dGamma = HP.GetReal();
251 dHystVRef = HP.GetReal();
252
253 // friction
254 bSlip = false;
255 if (HP.IsKeyWord("slip")) {
256 bSlip = true;
257
258 /*
259 * Parametri di attrito
260 */
261 pMuX0 = HP.GetDriveCaller();
262 pMuY0 = HP.GetDriveCaller();
263 pMuY1 = HP.GetDriveCaller();
264
265 dvThreshold = 0.;
266 dAlphaThreshold = 0.;
267 if (HP.IsKeyWord("threshold")) {
268 dvThreshold = HP.GetReal();
269 if (dvThreshold < 0.) {
270 silent_cerr("Wheel2(" << uLabel << "): "
271 "illegal velocity threshold " << dvThreshold
272 << " at line " << HP.GetLineData() << std::endl);
273 dvThreshold = std::abs(dvThreshold);
274 }
275
276 dAlphaThreshold = HP.GetReal();
277 if (dvThreshold < 0.) {
278 silent_cerr("Wheel2(" << uLabel << "): "
279 "illegal slip angle threshold " << dAlphaThreshold
280 << " at line " << HP.GetLineData() << std::endl);
281 dAlphaThreshold = std::abs(dAlphaThreshold);
282 }
283 }
284 }
285
286 SetOutputFlag(pDM->fReadOutput(HP, Elem::LOADABLE));
287
288 std::ostream& out = pDM->GetLogFile();
289 out << "wheel2: " << uLabel
290 << " " << pWheel->GetLabel() //node label
291 << " " << WheelAxle //wheel axle
292 << " " << pGround->GetLabel() //ground label
293 << " " << GroundDirection //ground direction
294 << " " << dRadius //wheel radius
295 << " " << dInternalRadius //wheel internal radius
296 << std::endl;
297 }
298
~Wheel2(void)299 Wheel2::~Wheel2(void)
300 {
301 NO_OP;
302 }
303
304 void
Output(OutputHandler & OH) const305 Wheel2::Output(OutputHandler& OH) const
306 {
307 if (bToBeOutput()) {
308 std::ostream& out = OH.Loadable();
309
310 out << std::setw(8) << GetLabel() // 1: label
311 << " " << F // 2-4: force
312 << " " << M // 5-7: moment
313 << " " << dInstRadius // 8: inst. radius
314 << " " << dDeltaL // 9: radial deformation
315 << " " << dVn // 10: radial deformation velocity
316 << " " << dSr // 11: slip ratio
317 << " " << 180./M_PI*dAlpha // 12: slip angle
318 << " " << dMuX // 13: longitudinal friction coefficient
319 << " " << dMuY // 14: lateral friction coefficient
320 << " " << dVa // 15: axis relative velocity
321 << " " << dVc // 16: POC relative velocity
322 << std::endl;
323 }
324 }
325
326 void
WorkSpaceDim(integer * piNumRows,integer * piNumCols) const327 Wheel2::WorkSpaceDim(integer* piNumRows, integer* piNumCols) const
328 {
329 *piNumRows = 12;
330 *piNumCols = 12;
331 }
332
333 VariableSubMatrixHandler&
AssJac(VariableSubMatrixHandler & WorkMat,doublereal dCoef,const VectorHandler & XCurr,const VectorHandler & XPrimeCurr)334 Wheel2::AssJac(VariableSubMatrixHandler& WorkMat,
335 doublereal dCoef,
336 const VectorHandler& XCurr,
337 const VectorHandler& XPrimeCurr)
338 {
339 WorkMat.SetNullMatrix();
340
341 return WorkMat;
342 }
343
344 SubVectorHandler&
AssRes(SubVectorHandler & WorkVec,doublereal dCoef,const VectorHandler & XCurr,const VectorHandler & XPrimeCurr)345 Wheel2::AssRes(SubVectorHandler& WorkVec,
346 doublereal dCoef,
347 const VectorHandler& XCurr,
348 const VectorHandler& XPrimeCurr)
349 {
350 // ground orientation in the absolute frame
351 Vec3 n = pGround->GetRCurr()*GroundDirection;
352
353 // wheel-ground distance in the absolute frame
354 Vec3 x = pWheel->GetXCurr() - pGround->GetXCurr();
355
356 // contact when dDeltaL > 0
357 dDeltaL = dRNP - x*n;
358
359 // reset output data
360 dInstRadius = dRadius - dDeltaL;
361
362 dSr = 0.;
363 dAlpha = 0.;
364
365 dMuX = 0.;
366 dMuY = 0.;
367
368 dVa = 0.;
369 dVc = 0.;
370
371 if (dDeltaL < 0.) {
372
373 F = Zero3;
374 M = Zero3;
375
376 dInstRadius = dRadius;
377 dDeltaL = 0.;
378
379 // no need to assemble residual contribution
380 WorkVec.Resize(0);
381
382 return WorkVec;
383 }
384
385 // resize residual
386 integer iNumRows = 0;
387 integer iNumCols = 0;
388 WorkSpaceDim(&iNumRows, &iNumCols);
389
390 WorkVec.ResizeReset(iNumRows);
391
392 integer iGroundFirstMomIndex = pGround->iGetFirstMomentumIndex();
393 integer iWheelFirstMomIndex = pWheel->iGetFirstMomentumIndex();
394
395 // equations indexes
396 for (int iCnt = 1; iCnt <= 6; iCnt++) {
397 WorkVec.PutRowIndex(iCnt, iGroundFirstMomIndex + iCnt);
398 WorkVec.PutRowIndex(6 + iCnt, iWheelFirstMomIndex + iCnt);
399 }
400
401 // relative speed between wheel axle and ground
402 Vec3 va = pWheel->GetVCurr() - pGround->GetVCurr()
403 - pGround->GetWCurr().Cross(pGround->GetRCurr()*GroundPosition);
404
405 dVa = (va - n*(n*va)).Norm();
406
407 // relative speed between wheel and ground at contact point
408 Vec3 v = va - (pWheel->GetWCurr()).Cross(n*dInstRadius);
409
410 // normal component of relative speed
411 // (positive when wheel departs from ground)
412 dVn = n*v;
413
414 dVc = (v - n*dVn).Norm();
415
416 // estimated contact area
417 doublereal dA = dRefArea*(dDeltaL/dInternalRadius);
418
419 // estimated intersection volume
420 doublereal dDeltaV = .5*dA*dDeltaL;
421
422 // estimated tyre pressure (polytropic)
423 doublereal dP = dP0*pow(dV0/(dV0 - dDeltaV), dGamma);
424
425 // we assume the contact point lies at the root of the normal
426 // to the ground that passes through the center of the wheel
427 // this means that the axle needs to remain parallel to the ground
428 Vec3 pc = pWheel->GetXCurr() - (n*dInstRadius);
429
430 // force
431 doublereal dFn = (dA*dP*(1. - tanh(dVn/dHystVRef)));
432 F = n*dFn;
433
434 if (bSlip) {
435 // "forward" direction: axle cross normal to ground
436 Vec3 fwd = (pWheel->GetRCurr()*WheelAxle).Cross(n);
437 doublereal d = fwd.Dot();
438 if (d < std::numeric_limits<doublereal>::epsilon()) {
439 silent_cerr("Wheel2(" << GetLabel() << "): "
440 "wheel axle is (neraly) orthogonal "
441 "to the ground" << std::endl);
442 throw DataManager::ErrGeneric(MBDYN_EXCEPT_ARGS);
443 }
444 fwd /= sqrt(d);
445
446 // slip ratio
447 doublereal dvx = fwd.Dot(v);
448 doublereal sgn = copysign(1., dvx);
449 doublereal dfvx = std::abs(dvx);
450 doublereal dvax = fwd.Dot(va);
451 doublereal dfvax = std::abs(dvax);
452
453 // FIXME: if vax ~ 0 (e.g. because the vehicle stopped)
454 // the "sleep" ratio needs to be small, right?
455 dSr = dfvx/(dfvax + dvThreshold);
456 if (dSr > 1.) {
457 dSr = 1.;
458 }
459
460 // "lateral" direction: normal to ground cross forward
461 Vec3 lat = n.Cross(fwd);
462
463 // lateral speed of wheel center
464 doublereal dvay = lat.Dot(va);
465
466 // wheel center drift angle
467 // NOTE: the angle is restricted to the first quadran
468 // the angle goes to zero when the velocity is below threshold
469 dAlpha = atan2(dvay, std::abs(dvax));
470 if (dAlphaThreshold > 0.) {
471 doublereal dtmp = tanh(sqrt(dvax*dvax + dvay*dvay)/dAlphaThreshold);
472 dAlpha *= dtmp*dtmp;
473 }
474
475 // longitudinal friction coefficient
476 doublereal dMuX0 = pMuX0->dGet(dSr);
477
478 // NOTE: -1 < alpha/(pi/2) < 1
479 dMuX = dMuX0*sgn*(1. - std::abs(dAlpha)/M_PI_2);
480
481 // force correction: the longitudinal friction coefficient
482 // is used with the sign of the contact point relative speed
483 F -= fwd*dFn*dMuX;
484
485 if (dvay != 0.) {
486 doublereal dMuY0 = pMuY0->dGet(dAlpha);
487 doublereal dMuY1 = pMuY1->dGet(dAlpha);
488
489 dMuY = dMuY0 + (dMuY1 - dMuY0)*dSr;
490
491 // force correction
492 F -= lat*dFn*dMuY;
493 }
494 }
495
496 // moment
497 M = (pc - pWheel->GetXCurr()).Cross(F);
498
499 WorkVec.Sub(1, F);
500 WorkVec.Sub(4, (pc - pGround->GetXCurr()).Cross(F));
501 WorkVec.Add(7, F);
502 WorkVec.Add(10, M);
503
504 return WorkVec;
505 }
506
507 unsigned int
iGetNumPrivData(void) const508 Wheel2::iGetNumPrivData(void) const
509 {
510 return 0;
511 }
512
513 int
iGetNumConnectedNodes(void) const514 Wheel2::iGetNumConnectedNodes(void) const
515 {
516 // wheel + ground
517 return 2;
518 }
519
520 void
GetConnectedNodes(std::vector<const Node * > & connectedNodes) const521 Wheel2::GetConnectedNodes(std::vector<const Node *>& connectedNodes) const
522 {
523 // wheel + ground
524 connectedNodes.resize(2);
525
526 connectedNodes[0] = pWheel;
527 connectedNodes[1] = pGround;
528 }
529
530 void
SetValue(DataManager * pDM,VectorHandler & X,VectorHandler & XP,SimulationEntity::Hints * ph)531 Wheel2::SetValue(DataManager *pDM,
532 VectorHandler& X, VectorHandler& XP,
533 SimulationEntity::Hints *ph)
534 {
535 NO_OP;
536 }
537
538 std::ostream&
Restart(std::ostream & out) const539 Wheel2::Restart(std::ostream& out) const
540 {
541 return out << "# not implemented yet" << std::endl;
542 }
543
544 unsigned
iGetInitialNumDof(void) const545 Wheel2::iGetInitialNumDof(void) const
546 {
547 return 0;
548 }
549
550 void
InitialWorkSpaceDim(integer * piNumRows,integer * piNumCols) const551 Wheel2::InitialWorkSpaceDim(integer* piNumRows, integer* piNumCols) const
552 {
553 *piNumRows = 0;
554 *piNumCols = 0;
555 }
556
557 VariableSubMatrixHandler&
InitialAssJac(VariableSubMatrixHandler & WorkMat,const VectorHandler & XCurr)558 Wheel2::InitialAssJac(VariableSubMatrixHandler& WorkMat,
559 const VectorHandler& XCurr)
560 {
561 WorkMat.SetNullMatrix();
562 return WorkMat;
563 }
564
565 SubVectorHandler&
InitialAssRes(SubVectorHandler & WorkVec,const VectorHandler & XCurr)566 Wheel2::InitialAssRes(SubVectorHandler& WorkVec, const VectorHandler& XCurr)
567 {
568 WorkVec.Resize(0);
569 return WorkVec;
570 }
571
572 bool
wheel2_set(void)573 wheel2_set(void)
574 {
575 UserDefinedElemRead *rf = new UDERead<Wheel2>;
576
577 if (!SetUDE("wheel2", rf)) {
578 delete rf;
579 return false;
580 }
581
582 return true;
583 }
584
585 #ifndef STATIC_MODULES
586 extern "C" int
module_init(const char * module_name,void * pdm,void * php)587 module_init(const char *module_name, void *pdm, void *php)
588 {
589 if (!wheel2_set()) {
590 silent_cerr("Wheel2: "
591 "module_init(" << module_name << ") "
592 "failed" << std::endl);
593 return -1;
594 }
595 return 0;
596 }
597 #endif // ! STATIC_MODULES
598