1 /* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/mbdyn/struct/brake.cc,v 1.26 2017/01/12 14:46:43 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 /* Continuano i vincoli di rotazione piani */
33
34 #include "mbconfig.h" /* This goes first in every *.c,*.cc file */
35
36 #include "brake.h"
37
38 /* Brake - begin */
39
40 const unsigned int Brake::NumSelfDof(0);
41 const unsigned int Brake::NumDof(12);
42
43 /* Costruttore non banale */
Brake(unsigned int uL,const DofOwner * pDO,const StructNode * pN1,const StructNode * pN2,const Vec3 & dTmp1,const Vec3 & dTmp2,const Mat3x3 & R1hTmp,const Mat3x3 & R2hTmp,flag fOut,const doublereal rr,const doublereal pref,BasicShapeCoefficient * const sh,BasicFriction * const f,DriveCaller * pdc)44 Brake::Brake(unsigned int uL, const DofOwner* pDO,
45 const StructNode* pN1, const StructNode* pN2,
46 const Vec3& dTmp1, const Vec3& dTmp2,
47 const Mat3x3& R1hTmp, const Mat3x3& R2hTmp,
48 flag fOut,
49 const doublereal rr,
50 const doublereal pref,
51 BasicShapeCoefficient *const sh,
52 BasicFriction *const f,
53 #if 0
54 bool isforce,
55 const Vec3& dir,
56 #endif
57 DriveCaller *pdc)
58 : Elem(uL, fOut),
59 Joint(uL, pDO, fOut),
60 pNode1(pN1), pNode2(pN2),
61 d1(dTmp1), R1h(R1hTmp), d2(dTmp2), R2h(R2hTmp), /* F(Zero3), */ M(Zero3), dTheta(0.),
62 Sh_c(sh), fc(f), preF(pref), r(rr), brakeForce(pdc) /* ,
63 isForce(isforce), Dir(dir) */
64 {
65 NO_OP;
66 }
67
68
69 /* Distruttore banale */
~Brake(void)70 Brake::~Brake(void)
71 {
72 NO_OP;
73 };
74
75 void
SetValue(DataManager * pDM,VectorHandler & X,VectorHandler & XP,SimulationEntity::Hints * ph)76 Brake::SetValue(DataManager *pDM,
77 VectorHandler& X, VectorHandler& XP,
78 SimulationEntity::Hints *ph)
79 {
80 Mat3x3 RTmp((pNode1->GetRCurr()*R1h).Transpose()*(pNode2->GetRCurr()*R2h));
81 Vec3 v(MatR2EulerAngles(RTmp));
82
83 dTheta = v.dGet(3);
84
85 fc->SetValue(pDM, X, XP, ph, iGetFirstIndex() + NumSelfDof);
86 }
87
88 void
AfterConvergence(const VectorHandler & X,const VectorHandler & XP)89 Brake::AfterConvergence(const VectorHandler& X,
90 const VectorHandler& XP)
91 {
92
93 Mat3x3 RTmp(((pNode1->GetRCurr()*R1h).Transpose()
94 *pNode1->GetRPrev()*R1h).Transpose()
95 *((pNode2->GetRCurr()*R2h).Transpose()
96 *pNode2->GetRPrev()*R2h));
97 Vec3 V(MatR2EulerAngles(RTmp.Transpose()));
98
99 dTheta += V.dGet(3);
100
101 Mat3x3 R1(pNode1->GetRCurr());
102 Mat3x3 R1hTmp(R1*R1h);
103 Vec3 e3a(R1hTmp.GetVec(3));
104 Vec3 Omega1(pNode1->GetWCurr());
105 Vec3 Omega2(pNode2->GetWCurr());
106 //relative velocity
107 doublereal v = (Omega1-Omega2).Dot(e3a)*r;
108 //reaction norm
109 doublereal modF = std::max(brakeForce.dGet(), preF);
110 fc->AfterConvergence(modF, v, X, XP, iGetFirstIndex() + NumSelfDof);
111 }
112
113
114 /* Contributo al file di restart */
Restart(std::ostream & out) const115 std::ostream& Brake::Restart(std::ostream& out) const
116 {
117 Joint::Restart(out) << ", plane hinge, "
118 << pNode1->GetLabel() << ", reference, node, ",
119 d1.Write(out, ", ")
120 << ", hinge, reference, node, 1, ", (R1h.GetVec(1)).Write(out, ", ")
121 << ", 2, ", (R1h.GetVec(2)).Write(out, ", ") << ", "
122 << pNode2->GetLabel() << ", reference, node, ",
123 d2.Write(out, ", ")
124 << ", hinge, reference, node, 1, ", (R2h.GetVec(1)).Write(out, ", ")
125 << ", 2, ", (R2h.GetVec(2)).Write(out, ", ") << ';' << std::endl;
126
127 return out;
128 }
129
130
131 /* Assemblaggio jacobiano */
132 VariableSubMatrixHandler&
AssJac(VariableSubMatrixHandler & WorkMat,doublereal dCoef,const VectorHandler & XCurr,const VectorHandler & XPrimeCurr)133 Brake::AssJac(VariableSubMatrixHandler& WorkMat,
134 doublereal dCoef,
135 const VectorHandler& XCurr,
136 const VectorHandler& XPrimeCurr)
137 {
138 DEBUGCOUT("Entering Brake::AssJac()" << std::endl);
139
140 /* Setta la sottomatrice come piena (e' un po' dispersivo, ma lo jacobiano
141 * e' complicato */
142 FullSubMatrixHandler& WM = WorkMat.SetFull();
143
144 /* Ridimensiona la sottomatrice in base alle esigenze */
145 integer iNumRows = 0;
146 integer iNumCols = 0;
147 WorkSpaceDim(&iNumRows, &iNumCols);
148 WM.ResizeReset(iNumRows, iNumCols);
149
150 /* Recupera gli indici delle varie incognite */
151 integer iNode1FirstPosIndex = pNode1->iGetFirstPositionIndex();
152 integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
153 integer iNode2FirstPosIndex = pNode2->iGetFirstPositionIndex();
154 integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
155 integer iFirstReactionIndex = iGetFirstIndex();
156
157 /* Setta gli indici delle equazioni */
158 for (unsigned int iCnt = 1; iCnt <= 6; iCnt++) {
159 WM.PutRowIndex(iCnt, iNode1FirstMomIndex+iCnt);
160 WM.PutColIndex(iCnt, iNode1FirstPosIndex+iCnt);
161 WM.PutRowIndex(6+iCnt, iNode2FirstMomIndex+iCnt);
162 WM.PutColIndex(6+iCnt, iNode2FirstPosIndex+iCnt);
163 }
164
165 for (unsigned int iCnt = 1; iCnt <= iGetNumDof(); iCnt++) {
166 WM.PutRowIndex(12+iCnt, iFirstReactionIndex+iCnt);
167 WM.PutColIndex(12+iCnt, iFirstReactionIndex+iCnt);
168 }
169
170 /* Recupera i dati che servono */
171 Mat3x3 R1(pNode1->GetRRef());
172 Mat3x3 R2(pNode2->GetRRef());
173 Vec3 d1Tmp(R1*d1);
174 Vec3 d2Tmp(R2*d2);
175 Mat3x3 R1hTmp(R1*R1h);
176 Mat3x3 R2hTmp(R2*R2h);
177
178 /* Suppongo che le reazioni F, M siano gia' state aggiornate da AssRes;
179 * ricordo che la forza F e' nel sistema globale, mentre la coppia M
180 * e' nel sistema locale ed il terzo termine, M(3), e' nullo in quanto
181 * diretto come l'asse attorno al quale la rotazione e' consentita */
182
183
184 /*
185 * La cerniera piana ha le prime 3 equazioni uguali alla cerniera sferica;
186 * inoltre ha due equazioni che affermano la coincidenza dell'asse 3 del
187 * riferimento solidale con la cerniera visto dai due nodi.
188 *
189 * (R1 * R1h * e1)^T * (R2 * R2h * e3) = 0
190 * (R1 * R1h * e2)^T * (R2 * R2h * e3) = 0
191 *
192 * A queste equazioni corrisponde una reazione di coppia agente attorno
193 * agli assi 1 e 2 del riferimento della cerniera. La coppia attorno
194 * all'asse 3 e' nulla per definizione. Quindi la coppia nel sistema
195 * globale e':
196 * -R1 * R1h * M per il nodo 1,
197 * R2 * R2h * M per il nodo 2
198 *
199 *
200 * xa ga xb gb F M
201 * Qa | 0 0 0 0 I 0 | | xa | | -F |
202 * Ga | 0 c*(F/\da/\-(Sa*M)/\) 0 0 da/\ Sa | | ga | | -da/\F-Sa*M |
203 * Qb | 0 0 0 0 -I 0 | | xb | = | F |
204 * Gb | 0 0 0 -c*(F/\db/\-(Sb*M)/\) -db/\ -Sb | | gb | | db/\F+Sb*M |
205 * F | -c*I c*da/\ c*I -c*db/\ 0 0 | | F | | xa+da-xb-db |
206 * M1 | 0 c*Tb1^T*Ta3/\ 0 c*Ta3^T*Tb1/\ 0 0 | | M | | Sb^T*Ta3 |
207 * M2 | 0 c*Tb2^T*Ta3/\ 0 c*Ta3^T*Tb2/\ 0 0 |
208 *
209 * con da = R1*d01, db = R2*d02, c = dCoef,
210 * Sa = R1*R1h*[e1,e2], Sb = R2*R2h*[e1, e2],
211 * Ta3 = R1*R1h*e3, Tb1 = R2*R2h*e1, Tb2 = R2*R2h*e2
212 */
213
214
215 /* Moltiplica la forza ed il momento per il coefficiente
216 * del metodo */
217 Vec3 MTmp = M*dCoef;
218
219 Vec3 e3a(R1hTmp.GetVec(3));
220 Vec3 e1b(R2hTmp.GetVec(1));
221 Vec3 e2b(R2hTmp.GetVec(2));
222 MTmp = e2b*MTmp.dGet(1)-e1b*MTmp.dGet(2);
223
224 Mat3x3 MWedgee3aWedge(MatCrossCross, MTmp, e3a);
225 Mat3x3 e3aWedgeMWedge(MatCrossCross, e3a, MTmp);
226
227
228 /* Contributo del momento alle equazioni di equilibrio dei nodi */
229 Vec3 Tmp1(e2b.Cross(e3a));
230 Vec3 Tmp2(e3a.Cross(e1b));
231
232
233 /* Modifica: divido le equazioni di vincolo per dCoef */
234
235 /* Equazioni di vincolo degli spostamenti */
236
237 /* Equazione di vincolo del momento
238 *
239 * Attenzione: bisogna scrivere il vettore trasposto
240 * (Sb[1]^T*(Sa[3]/\))*dCoef
241 * Questo pero' e' uguale a:
242 * (-Sa[3]/\*Sb[1])^T*dCoef,
243 * che puo' essere ulteriormente semplificato:
244 * (Sa[3].Cross(Sb[1])*(-dCoef))^T;
245 */
246
247
248 //retrive
249 //friction coef
250 doublereal f = fc->fc();
251 //shape function
252 doublereal shc = Sh_c->Sh_c();
253 //omega and omega rif
254 Vec3 Omega1(pNode1->GetWCurr());
255 Vec3 Omega2(pNode2->GetWCurr());
256 Vec3 Omega1r(pNode1->GetWRef());
257 Vec3 Omega2r(pNode2->GetWRef());
258 //compute
259 //relative velocity
260 doublereal v = (Omega1-Omega2).Dot(e3a)*r;
261 //reaction norm
262 doublereal modF = std::max(brakeForce.dGet(), preF);
263 //reaction moment
264 //doublereal M3 = shc*modF*r;
265
266 ExpandableRowVector dfc;
267 ExpandableRowVector dF;
268 ExpandableRowVector dv;
269 //variation of reaction force
270 dF.ReDim(0);
271 //variation of relative velocity
272 dv.ReDim(6);
273
274 /* new (approximate: assume constant triads orientations)
275 * relative velocity linearization */
276
277 /* FIXME: why *1. ??? */
278 dv.Set((e3a(1)*1.)*r, 0 + 1, 3 + 1);
279 dv.Set((e3a(2)*1.)*r, 0 + 2, 3 + 2);
280 dv.Set((e3a(3)*1.)*r, 0 + 3, 3 + 3);
281
282 dv.Set(-(e3a(1)*1.)*r, 3 + 1, 9 + 1);
283 dv.Set(-(e3a(2)*1.)*r, 3 + 2, 9 + 2);
284 dv.Set(-(e3a(3)*1.)*r, 3 + 3, 9 + 3);
285
286
287 //assemble friction states
288 fc->AssJac(WM,dfc, 12 + NumSelfDof,
289 iFirstReactionIndex + NumSelfDof, dCoef, modF, v,
290 XCurr, XPrimeCurr, dF, dv);
291 ExpandableRowVector dM3;
292 ExpandableRowVector dShc;
293 //compute
294 //variation of shape function
295 Sh_c->dSh_c(dShc,f,modF,v,dfc,dF,dv);
296 //variation of moment component
297 dM3.ReDim(2);
298 dM3.Set(shc*r, 1); dM3.Link(1, &dF);
299 dM3.Set(modF*r, 2); dM3.Link(2, &dShc);
300
301 //assemble first node
302 //variation of moment component
303 dM3.Add(WM, 3 + 1, e3a(1));
304 dM3.Add(WM, 3 + 2, e3a(2));
305 dM3.Add(WM, 3 + 3, e3a(3));
306 //assemble second node
307 //variation of moment component
308 dM3.Sub(WM, 9 + 1, e3a(1));
309 dM3.Sub(WM, 9 + 2, e3a(2));
310 dM3.Sub(WM, 9 + 3, e3a(3));
311
312 return WorkMat;
313 }
314
315
316 /* Assemblaggio residuo */
AssRes(SubVectorHandler & WorkVec,doublereal dCoef,const VectorHandler & XCurr,const VectorHandler & XPrimeCurr)317 SubVectorHandler& Brake::AssRes(SubVectorHandler& WorkVec,
318 doublereal dCoef,
319 const VectorHandler& XCurr,
320 const VectorHandler& XPrimeCurr)
321 {
322 DEBUGCOUT("Entering Brake::AssRes()" << std::endl);
323
324 /* Dimensiona e resetta la matrice di lavoro */
325 integer iNumRows = 0;
326 integer iNumCols = 0;
327 WorkSpaceDim(&iNumRows, &iNumCols);
328 WorkVec.ResizeReset(iNumRows);
329
330 /* Indici */
331 integer iNode1FirstMomIndex = pNode1->iGetFirstMomentumIndex();
332 integer iNode2FirstMomIndex = pNode2->iGetFirstMomentumIndex();
333 integer iFirstReactionIndex = iGetFirstIndex();
334
335 /* Indici dei nodi */
336 for (int iCnt = 1; iCnt <= 6; iCnt++) {
337 WorkVec.PutRowIndex(iCnt, iNode1FirstMomIndex+iCnt);
338 WorkVec.PutRowIndex(6+iCnt, iNode2FirstMomIndex+iCnt);
339 }
340
341 /* Indici del vincolo */
342 for (unsigned int iCnt = 1; iCnt <= iGetNumDof(); iCnt++) {
343 WorkVec.PutRowIndex(12+iCnt, iFirstReactionIndex+iCnt);
344 }
345
346 /* Aggiorna i dati propri */
347 //FIXME
348 //F = Vec3(Zero3);
349 M = Vec3(Zero3);
350
351 /*
352 * FIXME: provare a mettere "modificatori" di forza/momento sui gdl
353 * residui: attrito, rigidezze e smorzamenti, ecc.
354 */
355
356 /* Recupera i dati */
357 Vec3 x1(pNode1->GetXCurr());
358 Vec3 x2(pNode2->GetXCurr());
359 Mat3x3 R1(pNode1->GetRCurr());
360 Mat3x3 R2(pNode2->GetRCurr());
361
362 /* Costruisce i dati propri nella configurazione corrente */
363 Vec3 dTmp1(R1*d1);
364 Vec3 dTmp2(R2*d2);
365 Mat3x3 R1hTmp(R1*R1h);
366 Mat3x3 R2hTmp(R2*R2h);
367
368 Vec3 e3a(R1hTmp.GetVec(3));
369 Vec3 e1b(R2hTmp.GetVec(1));
370 Vec3 e2b(R2hTmp.GetVec(2));
371
372 Vec3 MTmp(e2b.Cross(e3a)*M.dGet(1)+e3a.Cross(e1b)*M.dGet(2));
373
374 /* Equazioni di equilibrio, nodo 1 */
375
376 /* Equazioni di equilibrio, nodo 2 */
377
378 /* Modifica: divido le equazioni di vincolo per dCoef */
379
380 bool ChangeJac(false);
381 Vec3 Omega1(pNode1->GetWCurr());
382 Vec3 Omega2(pNode2->GetWCurr());
383 doublereal v = (Omega1-Omega2).Dot(e3a)*r;
384 doublereal modF = std::max(brakeForce.dGet(), preF);
385 try {
386 fc->AssRes(WorkVec, 12 + NumSelfDof,
387 iFirstReactionIndex + NumSelfDof,
388 modF, v, XCurr, XPrimeCurr);
389 }
390 catch (Elem::ChangedEquationStructure) {
391 ChangeJac = true;
392 }
393 doublereal f = fc->fc();
394 doublereal shc = Sh_c->Sh_c(f, modF, v);
395 M(3) = r*shc*modF;
396 WorkVec.Sub(4, e3a*M(3));
397 WorkVec.Add(10, e3a*M(3));
398 //!!!!!!!!!!!!!!
399 // M += e3a*M3;
400 if (ChangeJac) {
401 throw Elem::ChangedEquationStructure(MBDYN_EXCEPT_ARGS);
402 }
403
404 return WorkVec;
405 }
406
iGetNumDof(void) const407 unsigned int Brake::iGetNumDof(void) const {
408 unsigned int i = NumSelfDof;
409 if (fc) {
410 i+=fc->iGetNumDof();
411 }
412 return i;
413 };
414
415
416 DofOrder::Order
GetDofType(unsigned int i) const417 Brake::GetDofType(unsigned int i) const {
418 ASSERT(i >= 0 && i < iGetNumDof());
419 if (i<NumSelfDof) {
420 return DofOrder::ALGEBRAIC;
421 } else {
422 return fc->GetDofType(i-NumSelfDof);
423 }
424 };
425
426 DofOrder::Order
GetEqType(unsigned int i) const427 Brake::GetEqType(unsigned int i) const
428 {
429 ASSERTMSGBREAK(i < iGetNumDof(),
430 "INDEX ERROR in Brake::GetEqType");
431 if (i<NumSelfDof) {
432 return DofOrder::ALGEBRAIC;
433 } else {
434 return fc->GetEqType(i-NumSelfDof);
435 }
436 }
437
438 /* Output (da mettere a punto) */
Output(OutputHandler & OH) const439 void Brake::Output(OutputHandler& OH) const
440 {
441 if (bToBeOutput()) {
442 Mat3x3 R2Tmp(pNode2->GetRCurr()*R2h);
443 Mat3x3 RTmp((pNode1->GetRCurr()*R1h).Transpose()*R2Tmp);
444 Mat3x3 R2TmpT(R2Tmp.Transpose());
445
446 std::ostream &of = Joint::Output(OH.Joints(), "PlaneHinge", GetLabel(),
447 /* R2TmpT*F*/ Zero3, M, /* F */ Zero3, R2Tmp*M)
448 << " " << MatR2EulerAngles(RTmp)*dRaDegr
449 << " " << R2TmpT*(pNode2->GetWCurr()-pNode1->GetWCurr());
450 if (fc) {
451 of << " " << fc->fc() << " " << brakeForce.dGet();
452 }
453 of << std::endl;
454 }
455 }
456
457
458 /* Contributo allo jacobiano durante l'assemblaggio iniziale */
459 VariableSubMatrixHandler&
InitialAssJac(VariableSubMatrixHandler & WorkMat,const VectorHandler & XCurr)460 Brake::InitialAssJac(VariableSubMatrixHandler& WorkMat,
461 const VectorHandler& XCurr)
462 {
463 /* Per ora usa la matrice piena; eventualmente si puo'
464 * passare a quella sparsa quando si ottimizza */
465 WorkMat.SetNullMatrix();
466
467 return WorkMat;
468 }
469
470
471 /* Contributo al residuo durante l'assemblaggio iniziale */
472 SubVectorHandler&
InitialAssRes(SubVectorHandler & WorkVec,const VectorHandler & XCurr)473 Brake::InitialAssRes(SubVectorHandler& WorkVec,
474 const VectorHandler& XCurr)
475 {
476 DEBUGCOUT("Entering Brake::InitialAssRes()" << std::endl);
477
478 /* Dimensiona e resetta la matrice di lavoro */
479 WorkVec.ResizeReset(0);
480
481 return WorkVec;
482 }
483
484
485 unsigned int
iGetNumPrivData(void) const486 Brake::iGetNumPrivData(void) const
487 {
488 /* FIXME: add access to friction priv data... */
489 return 2;
490 }
491
492 unsigned int
iGetPrivDataIdx(const char * s) const493 Brake::iGetPrivDataIdx(const char *s) const
494 {
495 ASSERT(s != NULL);
496
497 if (strcmp(s, "rz") == 0) {
498 return 1;
499 }
500
501 if (strcmp(s, "wz") == 0) {
502 return 2;
503 }
504
505
506 return 0;
507 }
508
dGetPrivData(unsigned int i) const509 doublereal Brake::dGetPrivData(unsigned int i) const
510 {
511 ASSERT(i >= 1 && i <= iGetNumPrivData());
512
513 switch (i) {
514 case 1: {
515 return dTheta;
516 }
517
518 case 2: {
519 Mat3x3 R2TmpT((pNode2->GetRCurr()*R2h).Transpose());
520 Vec3 v(R2TmpT*(pNode2->GetWCurr()-pNode1->GetWCurr()));
521
522 return v.dGet(3);
523 }
524
525 default:
526 silent_cerr("Brake(" << GetLabel() << "): "
527 "illegal private data " << i << std::endl);
528 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
529 }
530 }
531
532 /* Brake - end */
533
534
535