1 /* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/mbdyn/base/driven.cc,v 1.43 2017/01/12 14:46:09 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 /* Driven elements:
33 * elements that are used depending on the (boolean) value
34 * of a driver. Example: a driven joint is assembled only
35 * if the driver is true, otherwise there is no joint and
36 * the reaction unknowns are set to zero
37 */
38
39 #include "mbconfig.h" /* This goes first in every *.c,*.cc file */
40
41 #include "driven.h"
42 #include "joint.h"
43
DrivenElem(DataManager * pdm,const DriveCaller * pDC,const Elem * pE,SimulationEntity::Hints * ph)44 DrivenElem::DrivenElem(DataManager *pdm,
45 const DriveCaller* pDC, const Elem* pE,
46 SimulationEntity::Hints *ph)
47 : Elem(pE->GetLabel(), pE->fToBeOutput()),
48 NestedElem(pE),
49 DriveOwner(pDC),
50 pDM(pdm),
51 pHints(ph),
52 bActive(false)
53 {
54 ASSERT(pDC != 0);
55
56 bActive = (pDC->dGet() != 0.);
57 }
58
59
~DrivenElem(void)60 DrivenElem::~DrivenElem(void)
61 {
62 ASSERT(pElem != 0);
63
64 if (pHints != 0) {
65 for (unsigned i = 0; i < pHints->size(); i++) {
66 delete (*pHints)[i];
67 }
68 delete pHints;
69 pHints = 0;
70 }
71 }
72
73 bool
bIsActive(void) const74 DrivenElem::bIsActive(void) const
75 {
76 return (dGet() != 0.);
77 }
78
79 void
Output(OutputHandler & OH) const80 DrivenElem::Output(OutputHandler& OH) const
81 {
82 ASSERT(pElem != 0);
83 if (dGet() != 0.) {
84 pElem->Output(OH);
85 }
86 }
87
88 /* Scrive il contributo dell'elemento al file di restart */
89 std::ostream&
Restart(std::ostream & out) const90 DrivenElem::Restart(std::ostream& out) const
91 {
92 ASSERT(pElem != 0);
93
94 out << "driven: " << GetLabel() << ", ",
95 pGetDriveCaller()->Restart(out) << ", ";
96
97 if (pHints) {
98 for (unsigned i = 0; i < pHints->size(); i++) {
99 out << "hint, \"";
100
101 // TODO
102
103 out << "\", ";
104 }
105 }
106
107 pElem->Restart(out);
108
109 return out;
110 }
111
112 /*
113 * Elaborazione vettori e dati prima e dopo la predizione
114 * per MultiStepIntegrator
115 */
116 void
BeforePredict(VectorHandler & X,VectorHandler & XP,VectorHandler & XPrev,VectorHandler & XPPrev) const117 DrivenElem::BeforePredict(VectorHandler& X,
118 VectorHandler& XP,
119 VectorHandler& XPrev,
120 VectorHandler& XPPrev) const
121 {
122 ASSERT(pElem != 0);
123 if (dGet() != 0.) {
124 pElem->BeforePredict(X, XP, XPrev, XPPrev);
125 }
126 }
127
128 void
AfterPredict(VectorHandler & X,VectorHandler & XP)129 DrivenElem::AfterPredict(VectorHandler& X, VectorHandler& XP)
130 {
131 ASSERT(pElem != 0);
132 if (dGet() != 0.) {
133 if (!bActive) {
134 bActive = true;
135 pElem->SetValue(pDM, X, XP, pHints);
136 }
137 pElem->AfterPredict(X, XP);
138
139 } else {
140 if (bActive) {
141 bActive = false;
142 }
143 }
144 }
145
146 void
SetValue(DataManager * pdm,VectorHandler & X,VectorHandler & XP,SimulationEntity::Hints * ph)147 DrivenElem::SetValue(DataManager *pdm,
148 VectorHandler& X, VectorHandler& XP,
149 SimulationEntity::Hints *ph)
150 {
151 ASSERT(pElem != 0);
152
153 if (dGet() != 0.) {
154 pElem->SetValue(pdm, X, XP, ph);
155 }
156 }
157
158 #if 0
159 void
160 DrivenElem::SetInitialValue(VectorHandler& X)
161 {
162 ASSERT(pElem != 0);
163 if (dGet() != 0.) {
164 ElemWithDofs* pEwD = dynamic_cast<ElemWithDofs *>(pElem);
165 if (pEwD) {
166 pEwD->SetInitialValue(X);
167 }
168 }
169 }
170 #endif
171
172 /* Aggiorna dati in base alla soluzione */
173 void
Update(const VectorHandler & XCurr,const VectorHandler & XPrimeCurr)174 DrivenElem::Update(const VectorHandler& XCurr, const VectorHandler& XPrimeCurr)
175 {
176 ASSERT(pElem != 0);
177 if (dGet() != 0.) {
178 pElem->Update(XCurr, XPrimeCurr);
179 }
180 }
181
182 /* Inverse Dynamics: */
183 void
Update(const VectorHandler & XCurr,InverseDynamics::Order iOrder)184 DrivenElem::Update(const VectorHandler& XCurr, InverseDynamics::Order iOrder)
185 {
186 ASSERT(pElem != 0);
187 if (dGet() != 0.) {
188 pElem->Update(XCurr, iOrder);
189 }
190 }
191
192 /* inverse dynamics Jacobian matrix assembly */
193 VariableSubMatrixHandler&
AssJac(VariableSubMatrixHandler & WorkMat,const VectorHandler & XCurr)194 DrivenElem::AssJac(VariableSubMatrixHandler& WorkMat,
195 const VectorHandler& XCurr)
196 {
197 ASSERT(pElem != 0);
198
199 // must be a joint
200 ASSERT(dynamic_cast<const Joint *>(pElem) != 0);
201 // must either be torque, prescribed motion or ergonomy
202 ASSERT(dynamic_cast<const Joint *>(pElem)->bIsTorque()
203 || dynamic_cast<const Joint *>(pElem)->bIsPrescribedMotion()
204 || dynamic_cast<const Joint *>(pElem)->bIsErgonomy());
205 // dGet() must not be zero, otherwise AssJac() would not be called
206 ASSERT(dGet() != 0.);
207
208 return pElem->AssJac(WorkMat, XCurr);
209 }
210
211 /* inverse dynamics residual assembly */
212 SubVectorHandler&
AssRes(SubVectorHandler & WorkVec,const VectorHandler & XCurr,const VectorHandler & XPrimeCurr,const VectorHandler & XPrimePrimeCurr,InverseDynamics::Order iOrder)213 DrivenElem::AssRes(SubVectorHandler& WorkVec,
214 const VectorHandler& XCurr,
215 const VectorHandler& XPrimeCurr,
216 const VectorHandler& XPrimePrimeCurr,
217 InverseDynamics::Order iOrder)
218 {
219 ASSERT(pElem != 0);
220
221 if (dGet() != 0.) {
222 return pElem->AssRes(WorkVec, XCurr, XPrimeCurr, XPrimePrimeCurr, iOrder);
223 }
224
225 WorkVec.Resize(0);
226
227 return WorkVec;
228 }
229
230 /* Inverse Dynamics: */
231 void
AfterConvergence(const VectorHandler & X,const VectorHandler & XP,const VectorHandler & XPP)232 DrivenElem::AfterConvergence(const VectorHandler& X,
233 const VectorHandler& XP, const VectorHandler& XPP)
234 {
235 ASSERT(pElem != 0);
236 if (dGet() != 0.) {
237 pElem->AfterConvergence(X, XP, XPP);
238 }
239 }
240
241 void
AfterConvergence(const VectorHandler & X,const VectorHandler & XP)242 DrivenElem::AfterConvergence(const VectorHandler& X, const VectorHandler& XP)
243 {
244 ASSERT(pElem != 0);
245 if (dGet() != 0.) {
246 pElem->AfterConvergence(X, XP);
247 }
248 }
249
250 /* assemblaggio jacobiano */
251 VariableSubMatrixHandler&
AssJac(VariableSubMatrixHandler & WorkMat,doublereal dCoef,const VectorHandler & XCurr,const VectorHandler & XPrimeCurr)252 DrivenElem::AssJac(VariableSubMatrixHandler& WorkMat,
253 doublereal dCoef,
254 const VectorHandler& XCurr,
255 const VectorHandler& XPrimeCurr)
256 {
257 ASSERT(pElem != 0);
258
259 if (dGet() != 0.) {
260 return pElem->AssJac(WorkMat, dCoef, XCurr, XPrimeCurr);
261 }
262
263 unsigned int iNumDofs = pElem->iGetNumDof();
264 if (iNumDofs == 0) {
265 WorkMat.SetNullMatrix();
266
267 } else {
268 SparseSubMatrixHandler& WM = WorkMat.SetSparse();
269 WM.ResizeReset(iNumDofs, 0);
270
271 /* NOTE: must not fail, since iNumDofs != 0 */
272 integer iFirstIndex = dynamic_cast<ElemWithDofs *>(pElem)->iGetFirstIndex();
273
274 for (unsigned int iCnt = 1; iCnt <= iNumDofs; iCnt++) {
275 doublereal J;
276
277 switch (pElem->GetDofType(iCnt - 1)) {
278 case DofOrder::ALGEBRAIC:
279 J = 1.;
280 break;
281 case DofOrder::DIFFERENTIAL:
282 J = dCoef;
283 break;
284 default:
285 ASSERT(0);
286 throw ErrGeneric(MBDYN_EXCEPT_ARGS);
287 }
288
289 WM.PutItem(iCnt, iFirstIndex+iCnt,
290 iFirstIndex+iCnt, J);
291 }
292 }
293
294 return WorkMat;
295 }
296
297 void
AssMats(VariableSubMatrixHandler & WorkMatA,VariableSubMatrixHandler & WorkMatB,const VectorHandler & XCurr,const VectorHandler & XPrimeCurr)298 DrivenElem::AssMats(VariableSubMatrixHandler& WorkMatA,
299 VariableSubMatrixHandler& WorkMatB,
300 const VectorHandler& XCurr,
301 const VectorHandler& XPrimeCurr)
302 {
303 ASSERT(pElem != 0);
304
305 if (dGet() != 0.) {
306 pElem->AssMats(WorkMatA, WorkMatB, XCurr, XPrimeCurr);
307 return;
308 }
309
310 WorkMatA.SetNullMatrix();
311
312 unsigned int iNumDofs = pElem->iGetNumDof();
313 if (iNumDofs == 0) {
314 WorkMatB.SetNullMatrix();
315 } else {
316 SparseSubMatrixHandler& WM = WorkMatB.SetSparse();
317 WM.ResizeReset(iNumDofs, 0);
318
319 /* NOTE: must not fail, since iNumDofs != 0 */
320 integer iFirstIndex = dynamic_cast<ElemWithDofs *>(pElem)->iGetFirstIndex();
321
322 for (unsigned int iCnt = 1; iCnt <= iNumDofs; iCnt++) {
323 WM.PutItem(iCnt, iFirstIndex+iCnt,
324 iFirstIndex+iCnt, 1.);
325 }
326 }
327 };
328
329 /* assemblaggio residuo */
330 SubVectorHandler&
AssRes(SubVectorHandler & WorkVec,doublereal dCoef,const VectorHandler & XCurr,const VectorHandler & XPrimeCurr)331 DrivenElem::AssRes(SubVectorHandler& WorkVec,
332 doublereal dCoef,
333 const VectorHandler& XCurr,
334 const VectorHandler& XPrimeCurr)
335 {
336 ASSERT(pElem != 0);
337 if (dGet() != 0.) {
338 return pElem->AssRes(WorkVec, dCoef, XCurr, XPrimeCurr);
339 }
340
341 unsigned int iNumDofs = pElem->iGetNumDof();
342 if (iNumDofs == 0) {
343 WorkVec.Resize(0);
344 } else {
345 WorkVec.ResizeReset(iNumDofs);
346
347 /* NOTE: must not fail, since iNumDofs != 0 */
348 integer iFirstIndex = dynamic_cast<ElemWithDofs *>(pElem)->iGetFirstIndex();
349
350 for (unsigned int iCnt = 1; iCnt <= iNumDofs; iCnt++) {
351 WorkVec.PutRowIndex(iCnt, iFirstIndex+iCnt);
352 WorkVec.PutCoef(iCnt, -XCurr(iFirstIndex+iCnt));
353 }
354 }
355
356 return WorkVec;
357 }
358
359 /*
360 * Returns the current value of a private data
361 * with 0 < i <= iGetNumPrivData()
362 */
363 doublereal
dGetPrivData(unsigned int i) const364 DrivenElem::dGetPrivData(unsigned int i) const
365 {
366 if (dGet() != 0.) {
367 return pElem->dGetPrivData(i);
368 }
369
370 /* safe default */
371 return 0.;
372 }
373
374 /* InitialAssemblyElem */
375 unsigned int
iGetInitialNumDof(void) const376 DrivenElem::iGetInitialNumDof(void) const
377 {
378 if (dGet() != 0.) {
379 return NestedElem::iGetInitialNumDof();
380 }
381
382 return 0;
383 }
384
385 void
InitialWorkSpaceDim(integer * piNumRows,integer * piNumCols) const386 DrivenElem::InitialWorkSpaceDim(integer* piNumRows, integer* piNumCols) const
387 {
388 if (dGet() != 0.) {
389 NestedElem::InitialWorkSpaceDim(piNumRows, piNumCols);
390 }
391 }
392
393 VariableSubMatrixHandler&
InitialAssJac(VariableSubMatrixHandler & WorkMat,const VectorHandler & XCurr)394 DrivenElem::InitialAssJac(VariableSubMatrixHandler& WorkMat,
395 const VectorHandler& XCurr)
396 {
397 if (dGet() != 0.) {
398 return NestedElem::InitialAssJac(WorkMat, XCurr);
399 }
400
401 WorkMat.SetNullMatrix();
402 return WorkMat;
403 }
404
405 SubVectorHandler&
InitialAssRes(SubVectorHandler & WorkVec,const VectorHandler & XCurr)406 DrivenElem::InitialAssRes(SubVectorHandler& WorkVec,
407 const VectorHandler& XCurr)
408 {
409 if (dGet() != 0.) {
410 return NestedElem::InitialAssRes(WorkVec, XCurr);
411 }
412
413 WorkVec.Resize(0);
414 return WorkVec;
415 }
416
417 /* ElemGravityOwner */
418 Vec3
GetS_int(void) const419 DrivenElem::GetS_int(void) const
420 {
421 if (dGet() != 0.) {
422 return NestedElem::GetS_int();
423 }
424
425 return Zero3;
426 }
427
428 Mat3x3
GetJ_int(void) const429 DrivenElem::GetJ_int(void) const
430 {
431 if (dGet() != 0.) {
432 return NestedElem::GetJ_int();
433 }
434
435 return Zero3x3;
436 }
437
438 Vec3
GetB_int(void) const439 DrivenElem::GetB_int(void) const
440 {
441 if (dGet() != 0.) {
442 return NestedElem::GetB_int();
443 }
444
445 return Zero3;
446 }
447
448 // NOTE: gravity owners must provide the momenta moment
449 // with respect to the origin of the global reference frame!
450 Vec3
GetG_int(void) const451 DrivenElem::GetG_int(void) const
452 {
453 if (dGet() != 0.) {
454 return NestedElem::GetG_int();
455 }
456
457 return Zero3;
458 }
459
460 doublereal
dGetM(void) const461 DrivenElem::dGetM(void) const
462 {
463 if (dGet() != 0.) {
464 return NestedElem::dGetM();
465 }
466
467 return 0.;
468 }
469
470 Vec3
GetS(void) const471 DrivenElem::GetS(void) const
472 {
473 if (dGet() != 0.) {
474 return NestedElem::GetS();
475 }
476
477 return Zero3;
478 }
479
480 Mat3x3
GetJ(void) const481 DrivenElem::GetJ(void) const
482 {
483 if (dGet() != 0.) {
484 return NestedElem::GetJ();
485 }
486
487 return Zero3x3;
488 }
489
490 /* ElemDofOwner */
491 void
SetInitialValue(VectorHandler & X)492 DrivenElem::SetInitialValue(VectorHandler& X)
493 {
494 if (dGet() != 0.) {
495 return NestedElem::SetInitialValue(X);
496 }
497 }
498
499