1 /* Siconos is a program dedicated to modeling, simulation and control
2 * of non smooth dynamical systems.
3 *
4 * Copyright 2021 INRIA.
5 *
6 * Licensed under the Apache License, Version 2.0 (the "License");
7 * you may not use this file except in compliance with the License.
8 * You may obtain a copy of the License at
9 *
10 * http://www.apache.org/licenses/LICENSE-2.0
11 *
12 * Unless required by applicable law or agreed to in writing, software
13 * distributed under the License is distributed on an "AS IS" BASIS,
14 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15 * See the License for the specific language governing permissions and
16 * limitations under the License.
17 */
18 #include "SiconosPointers.hpp"
19 #include "NumericsMatrix.h"
20 #include "GlobalFrictionContact.hpp"
21 #include "Simulation.hpp"
22 //#include "Interaction.hpp"
23 #include "NonSmoothDynamicalSystem.hpp"
24 #include "Relation.hpp"
25 #include "NewtonImpactFrictionNSL.hpp"
26 #include "MoreauJeanGOSI.hpp" // Numerics Header
27 #include "LagrangianDS.hpp"
28 #include "NewtonEulerDS.hpp"
29 #include "NewtonImpactNSL.hpp"
30 #include "OSNSMatrix.hpp"
31
32 #include "TypeName.hpp"
33
34 // --- Numerics headers ---
35 #include "NonSmoothDrivers.h"
36 #include "gfc3d_Solvers.h"
37 #include "NumericsSparseMatrix.h"
38 // #define DEBUG_NOCOLOR
39 // #define DEBUG_STDOUT
40 // #define DEBUG_MESSAGES
41 #include "siconos_debug.h"
42
43 // Constructor from solver id - Uses delegated constructor
GlobalFrictionContact(int dimPb,const int numericsSolverId)44 GlobalFrictionContact::GlobalFrictionContact(int dimPb, const int numericsSolverId):
45 GlobalFrictionContact(dimPb, SP::SolverOptions(solver_options_create(numericsSolverId),
46 solver_options_delete))
47 {}
48
49 // Constructor based on a pre-defined solver options set.
GlobalFrictionContact(int dimPb,SP::SolverOptions options)50 GlobalFrictionContact::GlobalFrictionContact(int dimPb, SP::SolverOptions options):
51 LinearOSNS(options), _contactProblemDim(dimPb), _gfc_driver(&gfc3d_driver)
52 {
53 // // Only fc3d for the moment.
54 // if(_contactProblemDim != 3)
55 // THROW_EXCEPTION("GlobalFrictionContact No solver for 2 dimensional problems");
56
57 //Reset default storage type for numerics matrices.
58 _numericsMatrixStorageType = NM_SPARSE;
59 }
60
61
initVectorsMemory()62 void GlobalFrictionContact::initVectorsMemory()
63 {
64 // Memory allocation for reaction, and velocity
65 LinearOSNS::initVectorsMemory();
66
67 if(!_globalVelocities)
68 _globalVelocities.reset(new SiconosVector(_maxSize));
69 else
70 {
71 if(_globalVelocities->size() != _maxSize)
72 _globalVelocities->resize(_maxSize);
73 }
74
75 if(!_b)
76 _b.reset(new SiconosVector(_maxSize));
77 else
78 {
79 if(_b->size() != _maxSize)
80 _b->resize(_maxSize);
81 }
82 }
83
84
initOSNSMatrix()85 void GlobalFrictionContact::initOSNSMatrix()
86 {
87 // Default size for M = _maxSize
88 if(!_M)
89 {
90 // if (_numericsMatrixStorageType == NM_DENSE)
91 // _M.reset(new OSNSMatrix(_maxSize, NM_DENSE));
92 // else // if(MStorageType == 1) size = number of DSBlocks = number of DS in the largest considered graph of ds
93 // _M.reset(new OSNSMatrix(simulation()->nonSmoothDynamicalSystem()->dynamicalSystems()->size(), 1));
94
95 switch(_numericsMatrixStorageType)
96 {
97 case NM_DENSE:
98 {
99 _M.reset(new OSNSMatrix(_maxSize, NM_DENSE));
100 break;
101 }
102 case NM_SPARSE:
103 {
104 _M.reset(new OSNSMatrix(simulation()->nonSmoothDynamicalSystem()->dynamicalSystems()->size(), NM_SPARSE));
105 break;
106 }
107 case NM_SPARSE_BLOCK:
108 {
109 _M.reset(new OSNSMatrix(simulation()->nonSmoothDynamicalSystem()->dynamicalSystems()->size(), NM_SPARSE_BLOCK));
110 break;
111 }
112 {
113 default:
114 THROW_EXCEPTION("GlobalFrictionContact::initOSNSMatrix unknown _storageType");
115 }
116 }
117 }
118
119
120 if(!_H)
121 {
122
123 switch(_numericsMatrixStorageType)
124 {
125 case NM_DENSE:
126 {
127 _H.reset(new OSNSMatrix(_maxSize, NM_DENSE));
128 break;
129 }
130 case NM_SPARSE:
131 {
132 _H.reset(new OSNSMatrix(simulation()->nonSmoothDynamicalSystem()->dynamicalSystems()->size(), simulation()->indexSet(_indexSetLevel)->size(), NM_SPARSE));
133 break;
134 }
135 case NM_SPARSE_BLOCK:
136 {
137 _H.reset(new OSNSMatrix(simulation()->nonSmoothDynamicalSystem()->dynamicalSystems()->size(), simulation()->indexSet(_indexSetLevel)->size(), NM_SPARSE_BLOCK));
138 break;
139 }
140 {
141 default:
142 THROW_EXCEPTION("GlobalFrictionContact::initOSNSMatrix unknown _storageType");
143 }
144 }
145 }
146
147 }
148
initialize(SP::Simulation sim)149 void GlobalFrictionContact::initialize(SP::Simulation sim)
150 {
151 // - Checks memory allocation for main variables (M,q,w,z)
152 // - Formalizes the problem if the topology is time-invariant
153
154 // This function performs all steps that are time-invariant
155
156 // General initialize for OneStepNSProblem
157 OneStepNSProblem::initialize(sim);
158
159 initVectorsMemory();
160
161 // get topology
162 SP::Topology topology = simulation()->nonSmoothDynamicalSystem()->topology();
163
164 // Note that interactionBlocks is up to date since updateInteractionBlocks has been called during OneStepNSProblem::initialize()
165
166 // Fill vector of friction coefficients
167 SP::InteractionsGraph I0 = topology->indexSet0();
168 _mu.reset(new MuStorage());
169 _mu->reserve(I0->size());
170
171
172 initOSNSMatrix();
173
174
175 }
176
globalFrictionContactProblem()177 SP::GlobalFrictionContactProblem GlobalFrictionContact::globalFrictionContactProblem()
178 {
179 SP::GlobalFrictionContactProblem numerics_problem(globalFrictionContactProblem_new());
180 numerics_problem->M = &*_M->numericsMatrix();
181 numerics_problem->H = &*_H->numericsMatrix();
182 numerics_problem->q = _q->getArray();
183 numerics_problem->b = _b->getArray();
184 numerics_problem->numberOfContacts = _sizeOutput / _contactProblemDim;
185 numerics_problem->mu = _mu->data();
186 numerics_problem->dimension = 3;
187 return numerics_problem;
188 }
189
globalFrictionContactProblemPtr()190 GlobalFrictionContactProblem *GlobalFrictionContact::globalFrictionContactProblemPtr()
191 {
192 GlobalFrictionContactProblem *numerics_problem = &_numerics_problem;
193 numerics_problem->M = &*_M->numericsMatrix();
194 numerics_problem->H = &*_H->numericsMatrix();
195 numerics_problem->q = _q->getArray();
196 numerics_problem->b = _b->getArray();
197 numerics_problem->numberOfContacts = _sizeOutput / _contactProblemDim;
198 numerics_problem->mu = _mu->data();
199 numerics_problem->dimension = 3;
200 return numerics_problem;
201 }
202
203
checkCompatibleNSLaw(NonSmoothLaw & nslaw)204 bool GlobalFrictionContact::checkCompatibleNSLaw(NonSmoothLaw& nslaw)
205 {
206
207 float type_number= (float) (Type::value(nslaw) + 0.1 * nslaw.size());
208 _nslawtype.insert(type_number);
209
210 if (Type::value(nslaw) != Type::NewtonImpactFrictionNSL)
211 {
212 THROW_EXCEPTION("\nGlobalFrictionContact::checkCompatibleNSLaw - \n\
213 The chosen nonsmooth law is not compatible with FrictionalContact one step nonsmooth problem. \n\
214 Compatible NonSmoothLaw is NewtonImpactFrictionNSL (3D) \n");
215 return false;
216 }
217 if (_nslawtype.size() > 1)
218 {
219 THROW_EXCEPTION("\nFrictionContact::checkCompatibleNSLaw - \n\
220 Compatible NonSmoothLaw is : NewtonImpactFrictionNSL (3D), but you cannot mix them \n");
221 return false;
222 }
223
224 return true;
225 }
226
227
228
preCompute(double time)229 bool GlobalFrictionContact::preCompute(double time)
230 {
231 DEBUG_BEGIN("GlobalFrictionContact::preCompute(double time)\n");
232 // This function is used to prepare data for the GlobalFrictionContact problem
233 // - computation of M, H _tildeLocalVelocity and q
234 // - set _sizeOutput, sizeLocalOutput
235
236 // If the topology is time-invariant, only q needs to be computed at each time step.
237 // M, _sizeOutput have been computed in initialize and are uptodate.
238
239 // Get topology
240 SP::Topology topology = simulation()->nonSmoothDynamicalSystem()->topology();
241 DEBUG_PRINTF("indexSetLevel = %i\n", indexSetLevel());
242 if(indexSetLevel() == LEVELMAX)
243 {
244 DEBUG_END("GlobalFrictionContact::preCompute(double time)\n");
245 return false;
246 }
247 if(!_hasBeenUpdated)
248 {
249 InteractionsGraph& indexSet = *simulation()->nonSmoothDynamicalSystem()->topology()->indexSet(_indexSetLevel);
250 DynamicalSystemsGraph& DSG0 = *simulation()->nonSmoothDynamicalSystem()->dynamicalSystems();
251
252
253
254 // compute size and nnz of M and collect all matrices
255 // compute nnz of H and collect H blocks
256 // fill mu
257
258 // if (_sizeOutput == 0)
259 // {
260 // DEBUG_END("GlobalFrictionContact::preCompute(double time)\n");
261 // return false; }
262
263 _mu->clear();
264 // _mu.reserve(indexSet.size())
265
266 #if !defined(SICONOS_USE_MAP_FOR_HASH)
267 typedef std::unordered_map<SP::DynamicalSystem, SiconosMatrix*> dsMatMap;
268 typedef std::unordered_map<SP::DynamicalSystem, size_t> dsPosMap;
269 #else
270 typedef std::map<SP::DynamicalSystem, SiconosMatrix*> dsMatMap;
271 typedef std::map<SP::DynamicalSystem, size_t> dsPosMap;
272 #endif
273 dsMatMap dsMat;
274 dsPosMap absPosDS;
275
276 size_t sizeM = 0;
277
278
279 // fill _M
280 _M->fillM(DSG0);
281 sizeM = _M->size();
282 _sizeGlobalOutput = sizeM;
283 DEBUG_PRINTF("sizeM = %lu \n", sizeM);
284
285
286 // fill _q
287 if(_q->size() != _sizeGlobalOutput)
288 _q->resize(_sizeGlobalOutput);
289
290 size_t offset = 0;
291 DynamicalSystemsGraph::VIterator dsi, dsend;
292 for(std::tie(dsi, dsend) = DSG0.vertices(); dsi != dsend; ++dsi)
293 {
294 SP::DynamicalSystem ds = DSG0.bundle(*dsi);
295 Type::Siconos dsType = Type::value(*ds);
296 size_t dss = ds->dimension();
297 DEBUG_PRINTF("offset = %lu \n", offset);
298
299 OneStepIntegrator& Osi = *DSG0.properties(DSG0.descriptor(ds)).osi;
300 OSI::TYPES osiType = Osi.getType();
301 if(osiType == OSI::MOREAUJEANGOSI)
302 {
303 VectorOfVectors& ds_work_vectors = *DSG0.properties(DSG0.descriptor(ds)).workVectors;
304
305 if(dsType == Type::LagrangianDS || dsType == Type::LagrangianLinearTIDS)
306 {
307 SiconosVector& vfree = *ds_work_vectors[MoreauJeanGOSI::FREE];
308 setBlock(vfree, _q, dss, 0, offset);
309 }
310 else if(dsType == Type::NewtonEulerDS)
311 {
312 SiconosVector& vfree = *ds_work_vectors[MoreauJeanGOSI::FREE];
313 setBlock(vfree, _q, dss, 0, offset);
314 }
315 }
316 else
317 {
318 THROW_EXCEPTION("GlobalFrictionContact::computeq. Not yet implemented for Integrator type : " + std::to_string(osiType));
319 }
320 offset += dss;
321 }
322 DEBUG_EXPR(_q->display(););
323
324 /************************************/
325
326
327 // fill H
328 _H->fillH(DSG0, indexSet);
329 DEBUG_EXPR(NM_display(_H->numericsMatrix().get()););
330
331 _sizeOutput =_H->sizeColumn();
332 DEBUG_PRINTF("_sizeOutput = %i\n ", _sizeOutput);
333
334
335 //fill _b
336 if(_b->size() != _sizeOutput)
337 _b->resize(_sizeOutput);
338
339 size_t pos = 0;
340 InteractionsGraph::VIterator ui, uiend;
341 for(std::tie(ui, uiend) = indexSet.vertices(); ui != uiend; ++ui)
342 {
343 SP::Interaction inter = indexSet.bundle(*ui);
344
345 assert(Type::value(*(inter->nonSmoothLaw())) == Type::NewtonImpactFrictionNSL);
346 _mu->push_back(std::static_pointer_cast<NewtonImpactFrictionNSL>(inter->nonSmoothLaw())->mu());
347
348 SP::DynamicalSystem ds1 = indexSet.properties(*ui).source;
349 SP::DynamicalSystem ds2 = indexSet.properties(*ui).target;
350 OneStepIntegrator& Osi1 = *DSG0.properties(DSG0.descriptor(ds1)).osi;
351 OneStepIntegrator& Osi2 = *DSG0.properties(DSG0.descriptor(ds2)).osi;
352
353 OSI::TYPES osi1Type = Osi1.getType();
354 OSI::TYPES osi2Type = Osi2.getType();
355 if(osi1Type == OSI::MOREAUJEANGOSI && osi2Type == OSI::MOREAUJEANGOSI)
356 {
357 static_cast<MoreauJeanGOSI&>(Osi1).NSLcontrib(inter, *this);
358 }
359 else
360 {
361 THROW_EXCEPTION("GlobalFrictionContact::computeq. Not yet implemented for Integrator type : " + std::to_string(osi1Type));
362 }
363 SiconosVector& osnsp_rhs = *(*indexSet.properties(*ui).workVectors)[MoreauJeanGOSI::OSNSP_RHS];
364 pos = indexSet.properties(*ui).absolute_position;
365 size_t sizeY = inter->dimension();
366 setBlock(osnsp_rhs, _b, sizeY, 0, pos);
367 }
368 DEBUG_EXPR(_b->display(););
369 // Checks z and w sizes and reset if necessary
370 if(_z->size() != _sizeOutput)
371 {
372 _z->resize(_sizeOutput, false);
373 _z->zero();
374 }
375
376 if(_w->size() != _sizeOutput)
377 {
378 _w->resize(_sizeOutput);
379 _w->zero();
380 }
381 if(_globalVelocities->size() != _sizeGlobalOutput)
382 {
383 _globalVelocities->resize(_sizeGlobalOutput);
384 _globalVelocities->zero();
385 }
386
387 }
388 DEBUG_END("GlobalFrictionContact::preCompute(double time)\n");
389 return true;
390 }
391
compute(double time)392 int GlobalFrictionContact::compute(double time)
393 {
394 int info = 0;
395 // --- Prepare data for GlobalFrictionContact computing ---
396 bool cont = preCompute(time);
397 if(!cont)
398 return info;
399 updateMu();
400 // --- Call Numerics solver ---
401 if(_sizeGlobalOutput != 0)
402 {
403 info= solve();
404 DEBUG_EXPR(display(););
405 postCompute();
406 }
407 return info;
408 }
409
410
411
solve(SP::GlobalFrictionContactProblem problem)412 int GlobalFrictionContact::solve(SP::GlobalFrictionContactProblem problem)
413 {
414 if(!problem)
415 {
416 problem = globalFrictionContactProblem();
417 }
418 return (*_gfc_driver)(&*problem,
419 _z->getArray(),
420 _w->getArray(),
421 _globalVelocities->getArray(),
422 &*_numerics_solver_options);
423 }
424
425
426
postCompute()427 void GlobalFrictionContact::postCompute()
428 {
429 DEBUG_BEGIN("GlobalFrictionContact::postCompute(double time)\n");
430
431 // This function is used to set y/lambda values using output from primalfrictioncontact_driver
432 // Only Interactions (ie Interactions) of indexSet(leveMin) are concerned.
433
434 // === Get index set from Topology ===
435 InteractionsGraph& indexSet = *simulation()->nonSmoothDynamicalSystem()->topology()->indexSet(_indexSetLevel);
436 // y and lambda vectors
437 SP::SiconosVector y, lambda;
438
439 // // === Loop through "active" Interactions (ie present in indexSets[1]) ===
440
441 size_t pos = 0;
442
443 InteractionsGraph::VIterator ui, uiend;
444 for(std::tie(ui, uiend) = indexSet.vertices(); ui != uiend; ++ui, pos += 3)
445 {
446 Interaction& inter = *indexSet.bundle(*ui);
447 // Get Y and Lambda for the current Interaction
448 y = inter.y(inputOutputLevel());
449 lambda = inter.lambda(inputOutputLevel());
450 // Copy _w/_z values, starting from index pos into y/lambda.
451
452 //setBlock(*_w, y, y->size(), pos, 0);// Warning: yEquivalent is
453 // saved in y !!
454 setBlock(*_z, lambda, lambda->size(), pos, 0);
455 DEBUG_EXPR(lambda->display(););
456 }
457 DynamicalSystemsGraph& DSG0 = *simulation()->nonSmoothDynamicalSystem()->dynamicalSystems();
458
459 unsigned int sizeDS;
460 SP::OneStepIntegrator Osi;
461 DynamicalSystemsGraph::VIterator dsi, dsend;
462 pos=0;
463 for(std::tie(dsi, dsend) = DSG0.vertices(); dsi != dsend; ++dsi)
464 {
465 DynamicalSystem& ds = *DSG0.bundle(*dsi);
466 Type::Siconos dsType = Type::value(ds);
467
468 if(dsType == Type::LagrangianDS || dsType == Type::LagrangianLinearTIDS || dsType == Type::LagrangianLinearDiagonalDS)
469 {
470 LagrangianDS& d = static_cast<LagrangianDS&>(ds);
471 sizeDS = d.dimension();
472 SP::SiconosVector velocity = d.velocity();
473 DEBUG_PRINTF("ds.number() : %i \n",ds.number());
474 DEBUG_EXPR(velocity->display(););
475 DEBUG_EXPR(_globalVelocities->display(););
476 pos = DSG0.properties(*dsi).absolute_position;
477 setBlock(*_globalVelocities, velocity, sizeDS, pos, 0);
478 DEBUG_EXPR(velocity->display(););
479 }
480 else if(dsType == Type::NewtonEulerDS)
481 {
482 NewtonEulerDS& neds = static_cast<NewtonEulerDS&>(ds);;
483 sizeDS = neds.dimension();
484 SP::SiconosVector twist = neds.twist();
485 DEBUG_PRINTF("ds.number() : %i \n",ds.number());
486 DEBUG_EXPR(twist->display(););
487 DEBUG_EXPR(_globalVelocities->display(););
488 pos = DSG0.properties(*dsi).absolute_position;
489 setBlock(*_globalVelocities, twist, sizeDS, pos, 0);
490 DEBUG_EXPR(twist->display(););
491 }
492 else THROW_EXCEPTION("GlobalFrictionContact::postCompute() - not yet implemented for Dynamical system of type: " + Type::name(ds));
493
494 }
495
496 DEBUG_END("GlobalFrictionContact::postCompute(double time)\n");
497
498 }
499
display() const500 void GlobalFrictionContact::display() const
501 {
502
503 std::cout << "===== " << _contactProblemDim << "D Primal Friction Contact Problem " <<std::endl;
504 std::cout << "size (_sizeOutput) " << _sizeOutput << "(ie " << _sizeOutput / _contactProblemDim << " contacts)."<<std::endl;
505 std::cout << "and size (_sizeGlobalOutput) " << _sizeGlobalOutput <<std::endl;
506 std::cout << "_numericsMatrixStorageType" << _numericsMatrixStorageType<< std::endl;
507 std::cout << " - Matrix M : " <<std::endl;
508 // if (_M) _M->display();
509 // else std::cout << "-> nullptr" <<std::endl;
510 NumericsMatrix* M_NM = _M->numericsMatrix().get();
511 if(M_NM)
512 {
513 NM_display(M_NM);
514 }
515 std::cout << " - Matrix H : " <<std::endl;
516 // if (_H) _H->display();
517 // else std::cout << "-> nullptr" <<std::endl;
518 NumericsMatrix* H_NM = _H->numericsMatrix().get();
519 if(H_NM)
520 {
521 NM_display(H_NM);
522 }
523
524 std::cout << " - Vector q : " <<std::endl;
525 if(_q) _q->display();
526 else std::cout << "-> nullptr" <<std::endl;
527 std::cout << " - Vector b : " <<std::endl;
528 if(_b) _b->display();
529 else std::cout << "-> nullptr" <<std::endl;
530
531 std::cout << " - Vector z (reaction) : " <<std::endl;
532 if(_z) _z->display();
533 else std::cout << "-> nullptr" <<std::endl;
534
535 std::cout << " - Vector w (local velocities): " <<std::endl;
536 if(_w) _w->display();
537 else std::cout << "-> nullptr" <<std::endl;
538
539 std::cout << " - Vector globalVelocities : " <<std::endl;
540 if(_globalVelocities) _globalVelocities->display();
541 else std::cout << "-> nullptr" <<std::endl;
542
543 std::cout << "============================================================" <<std::endl;
544 }
545
updateMu()546 void GlobalFrictionContact::updateMu()
547 {
548 _mu->clear();
549 SP::InteractionsGraph indexSet = simulation()->indexSet(indexSetLevel());
550 InteractionsGraph::VIterator ui, uiend;
551 for(std::tie(ui, uiend) = indexSet->vertices(); ui != uiend; ++ui)
552 {
553 _mu->push_back(std::static_pointer_cast<NewtonImpactFrictionNSL>
554 (indexSet->bundle(*ui)->nonSmoothLaw())->mu());
555 }
556 }
557