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 "OneStepNSProblem.hpp"
19 #include "SolverOptions.h"
20 #include "NonSmoothDynamicalSystem.hpp"
21 //#include "Interaction.hpp"
22 #include "Interaction.hpp"
23 #include "Topology.hpp"
24 #include "Simulation.hpp"
25 #include "EulerMoreauOSI.hpp"
26 #include "MoreauJeanOSI.hpp"
27 #include "MoreauJeanBilbaoOSI.hpp"
28 #include "SchatzmanPaoliOSI.hpp"
29 #include "NewMarkAlphaOSI.hpp"
30 #include "LagrangianDS.hpp"
31 #include "NewtonEulerDS.hpp"
32 #include "ZeroOrderHoldOSI.hpp"
33 #include "NonSmoothLaw.hpp"
34 #include "Simulation.hpp"
35
36 // #define DEBUG_STDOUT
37 // #define DEBUG_MESSAGES
38 #include "siconos_debug.h"
39 #include "numerics_verbose.h" // numerics to set verbose mode ...
40
41 // --- CONSTRUCTORS/DESTRUCTOR ---
42
43
hasInteractions() const44 bool OneStepNSProblem::hasInteractions() const
45 {
46 return _simulation->nonSmoothDynamicalSystem()->topology()->indexSet(_indexSetLevel)->size() > 0 ;
47 }
48
updateInteractionBlocks()49 void OneStepNSProblem::updateInteractionBlocks()
50 {
51 DEBUG_PRINT("OneStepNSProblem::updateInteractionBlocks() starts\n");
52 // The present functions checks various conditions and possibly
53 // compute interactionBlocks matrices.
54 //
55 // Let interi and interj be two Interactions.
56 //
57 // Things to be checked are:
58 // 1 - is the topology time invariant?
59 // 2 - does interactionBlocks[interi][interj] already exists (ie has been
60 // computed in a previous time step)?
61 // 3 - do we need to compute this interactionBlock? An interactionBlock has
62 // to be computed if interi and interj are in IndexSet1 AND if interi and
63 // interj have common DynamicalSystems.
64 //
65
66 //
67 // - If 1 and 2 are true then it does nothing. 3 is not checked.
68 // - If 1 == true, 2 == false, 3 == false, it does nothing.
69 // - If 1 == true, 2 == false, 3 == true, it computes the interactionBlock.
70 // - If 1==false, 2 is not checked, and the interactionBlock is computed if 3==true.
71 //
72
73 // Get index set from Simulation
74 SP::InteractionsGraph indexSet = simulation()->indexSet(indexSetLevel());
75
76 bool isLinear = simulation()->nonSmoothDynamicalSystem()->isLinear();
77
78 // we put diagonal information on vertices
79 // self loops with bgl are a *nightmare* at the moment
80 // (patch 65198 on standard boost install)
81 if(indexSet->properties().symmetric)
82 {
83 DEBUG_PRINT("OneStepNSProblem::updateInteractionBlocks(). Symmetric case");
84 InteractionsGraph::VIterator vi, viend;
85 for(std::tie(vi, viend) = indexSet->vertices();
86 vi != viend; ++vi)
87 {
88 SP::Interaction inter = indexSet->bundle(*vi);
89 unsigned int nslawSize = inter->nonSmoothLaw()->size();
90 if(! indexSet->properties(*vi).block)
91 {
92 indexSet->properties(*vi).block.reset(new SimpleMatrix(nslawSize, nslawSize));
93 }
94
95 if(!isLinear || !_hasBeenUpdated)
96 {
97 computeDiagonalInteractionBlock(*vi);
98 }
99 }
100
101 /* interactionBlock must be zeroed at init */
102 std::vector<bool> initialized;
103 initialized.resize(indexSet->edges_number());
104 std::fill(initialized.begin(), initialized.end(), false);
105
106 InteractionsGraph::EIterator ei, eiend;
107 for(std::tie(ei, eiend) = indexSet->edges();
108 ei != eiend; ++ei)
109 {
110 SP::Interaction inter1 = indexSet->bundle(indexSet->source(*ei));
111 SP::Interaction inter2 = indexSet->bundle(indexSet->target(*ei));
112
113 /* on adjoint graph there is at most 2 edges between source and target */
114 InteractionsGraph::EDescriptor ed1, ed2;
115 std::tie(ed1, ed2) = indexSet->edges(indexSet->source(*ei), indexSet->target(*ei));
116
117 assert(*ei == ed1 || *ei == ed2);
118
119 /* the first edge has the lower index */
120 assert(indexSet->index(ed1) <= indexSet->index(ed2));
121
122 // Memory allocation if needed
123 unsigned int nslawSize1 = inter1->nonSmoothLaw()->size();
124 unsigned int nslawSize2 = inter2->nonSmoothLaw()->size();
125 unsigned int isrc = indexSet->index(indexSet->source(*ei));
126 unsigned int itar = indexSet->index(indexSet->target(*ei));
127
128 SP::SiconosMatrix currentInteractionBlock;
129
130 if(itar > isrc) // upper block
131 {
132 if(! indexSet->properties(ed1).upper_block)
133 {
134 indexSet->properties(ed1).upper_block.reset(new SimpleMatrix(nslawSize1, nslawSize2));
135 if(ed2 != ed1)
136 indexSet->properties(ed2).upper_block = indexSet->properties(ed1).upper_block;
137 }
138 currentInteractionBlock = indexSet->properties(ed1).upper_block;
139 }
140 else // lower block
141 {
142 if(! indexSet->properties(ed1).lower_block)
143 {
144 indexSet->properties(ed1).lower_block.reset(new SimpleMatrix(nslawSize1, nslawSize2));
145 if(ed2 != ed1)
146 indexSet->properties(ed2).lower_block = indexSet->properties(ed1).lower_block;
147 }
148 currentInteractionBlock = indexSet->properties(ed1).lower_block;
149 }
150
151 if(!initialized[indexSet->index(ed1)])
152 {
153 initialized[indexSet->index(ed1)] = true;
154 currentInteractionBlock->zero();
155 }
156 if(!isLinear || !_hasBeenUpdated)
157 {
158 {
159 computeInteractionBlock(*ei);
160 }
161
162 // allocation for transposed block
163 // should be avoided
164
165 if(itar > isrc) // upper block has been computed
166 {
167 if(!indexSet->properties(ed1).lower_block)
168 {
169 indexSet->properties(ed1).lower_block.
170 reset(new SimpleMatrix(indexSet->properties(ed1).upper_block->size(1),
171 indexSet->properties(ed1).upper_block->size(0)));
172 }
173 indexSet->properties(ed1).lower_block->trans(*indexSet->properties(ed1).upper_block);
174 indexSet->properties(ed2).lower_block = indexSet->properties(ed1).lower_block;
175 }
176 else
177 {
178 assert(itar < isrc); // lower block has been computed
179 if(!indexSet->properties(ed1).upper_block)
180 {
181 indexSet->properties(ed1).upper_block.
182 reset(new SimpleMatrix(indexSet->properties(ed1).lower_block->size(1),
183 indexSet->properties(ed1).lower_block->size(0)));
184 }
185 indexSet->properties(ed1).upper_block->trans(*indexSet->properties(ed1).lower_block);
186 indexSet->properties(ed2).upper_block = indexSet->properties(ed1).upper_block;
187 }
188 }
189 }
190 }
191 else // not symmetric => follow out_edges for each vertices
192 {
193 DEBUG_PRINT("OneStepNSProblem::updateInteractionBlocks(). Non symmetric case\n");
194
195 InteractionsGraph::VIterator vi, viend;
196 for(std::tie(vi, viend) = indexSet->vertices();
197 vi != viend; ++vi)
198 {
199 DEBUG_PRINT("OneStepNSProblem::updateInteractionBlocks(). Computation of diaganal block\n");
200 SP::Interaction inter = indexSet->bundle(*vi);
201 unsigned int nslawSize = inter->nonSmoothLaw()->size();
202 if(! indexSet->properties(*vi).block)
203 {
204 indexSet->properties(*vi).block.reset(new SimpleMatrix(nslawSize, nslawSize));
205 }
206
207 if(!isLinear || !_hasBeenUpdated)
208 {
209 computeDiagonalInteractionBlock(*vi);
210 }
211
212 /* on a undirected graph, out_edges gives all incident edges */
213 InteractionsGraph::OEIterator oei, oeiend;
214 /* interactionBlock must be zeroed at init */
215 std::map<SP::SiconosMatrix, bool> initialized;
216 for(std::tie(oei, oeiend) = indexSet->out_edges(*vi);
217 oei != oeiend; ++oei)
218 {
219 /* on adjoint graph there is at most 2 edges between source and target */
220 InteractionsGraph::EDescriptor ed1, ed2;
221 std::tie(ed1, ed2) = indexSet->edges(indexSet->source(*oei), indexSet->target(*oei));
222 if(indexSet->properties(ed1).upper_block)
223 {
224 initialized[indexSet->properties(ed1).upper_block] = false;
225 }
226 // if(indexSet->properties(ed2).upper_block)
227 // {
228 // initialized[indexSet->properties(ed2).upper_block] = false;
229 // }
230
231 if(indexSet->properties(ed1).lower_block)
232 {
233 initialized[indexSet->properties(ed1).lower_block] = false;
234 }
235 // if(indexSet->properties(ed2).lower_block)
236 // {
237 // initialized[indexSet->properties(ed2).lower_block] = false;
238 // }
239
240 }
241
242 for(std::tie(oei, oeiend) = indexSet->out_edges(*vi);
243 oei != oeiend; ++oei)
244 {
245 DEBUG_PRINT("OneStepNSProblem::updateInteractionBlocks(). Computation of extra-diaganal block\n");
246
247 /* on adjoint graph there is at most 2 edges between source and target */
248 InteractionsGraph::EDescriptor ed1, ed2;
249 std::tie(ed1, ed2) = indexSet->edges(indexSet->source(*oei), indexSet->target(*oei));
250
251 assert(*oei == ed1 || *oei == ed2);
252
253 /* the first edge as the lower index */
254 assert(indexSet->index(ed1) <= indexSet->index(ed2));
255
256 SP::Interaction inter1 = indexSet->bundle(indexSet->source(*oei));
257 SP::Interaction inter2 = indexSet->bundle(indexSet->target(*oei));
258
259 // Memory allocation if needed
260 unsigned int nslawSize1 = inter1->nonSmoothLaw()->size();
261 unsigned int nslawSize2 = inter2->nonSmoothLaw()->size();
262 unsigned int isrc = indexSet->index(indexSet->source(*oei));
263 unsigned int itar = indexSet->index(indexSet->target(*oei));
264
265 SP::SiconosMatrix currentInteractionBlock;
266
267 if(itar > isrc) // upper block
268 {
269 if(! indexSet->properties(ed1).upper_block)
270 {
271 indexSet->properties(ed1).upper_block.reset(new SimpleMatrix(nslawSize1, nslawSize2));
272 initialized[indexSet->properties(ed1).upper_block] = false;
273 if(ed2 != ed1)
274 indexSet->properties(ed2).upper_block = indexSet->properties(ed1).upper_block;
275 }
276 currentInteractionBlock = indexSet->properties(ed1).upper_block;
277
278 }
279 else // lower block
280 {
281 if(! indexSet->properties(ed1).lower_block)
282 {
283 indexSet->properties(ed1).lower_block.reset(new SimpleMatrix(nslawSize1, nslawSize2));
284 initialized[indexSet->properties(ed1).lower_block] = false;
285 if(ed2 != ed1)
286 indexSet->properties(ed2).lower_block = indexSet->properties(ed1).lower_block;
287 }
288 currentInteractionBlock = indexSet->properties(ed1).lower_block;
289 }
290
291
292 if(!initialized[currentInteractionBlock])
293 {
294 initialized[currentInteractionBlock] = true;
295 currentInteractionBlock->zero();
296 }
297
298 if(!isLinear || !_hasBeenUpdated)
299 {
300 if(isrc != itar)
301 computeInteractionBlock(*oei);
302 }
303
304 }
305 }
306 }
307
308
309 DEBUG_EXPR(displayBlocks(indexSet););
310
311 DEBUG_PRINT("OneStepNSProblem::updateInteractionBlocks() ends\n");
312
313
314 }
315
displayBlocks(SP::InteractionsGraph indexSet)316 void OneStepNSProblem::displayBlocks(SP::InteractionsGraph indexSet)
317 {
318
319 std::cout << "OneStepNSProblem::displayBlocks(SP::InteractionsGraph indexSet) " << std::endl;
320 InteractionsGraph::VIterator vi, viend;
321 for(std::tie(vi, viend) = indexSet->vertices();
322 vi != viend; ++vi)
323 {
324 SP::Interaction inter = indexSet->bundle(*vi);
325 if(indexSet->properties(*vi).block)
326 {
327 indexSet->properties(*vi).block->display();
328 }
329
330 InteractionsGraph::OEIterator oei, oeiend;
331 for(std::tie(oei, oeiend) = indexSet->out_edges(*vi);
332 oei != oeiend; ++oei)
333 {
334 InteractionsGraph::EDescriptor ed1, ed2;
335 std::tie(ed1, ed2) = indexSet->edges(indexSet->source(*oei), indexSet->target(*oei));
336
337 if(indexSet->properties(ed1).upper_block)
338 {
339 indexSet->properties(ed1).upper_block->display();
340 }
341 if(indexSet->properties(ed1).lower_block)
342 {
343 indexSet->properties(ed1).lower_block->display();
344 }
345 if(indexSet->properties(ed2).upper_block)
346 {
347 indexSet->properties(ed2).upper_block->display();
348 }
349 if(indexSet->properties(ed2).lower_block)
350 {
351 indexSet->properties(ed2).lower_block->display();
352 }
353 }
354
355 }
356 }
357
initialize(SP::Simulation sim)358 void OneStepNSProblem::initialize(SP::Simulation sim)
359 {
360 // Link with the simulation that owns this osnsp
361
362 assert(sim && "OneStepNSProblem::initialize(sim), sim is null.");
363
364 _simulation = sim;
365
366 // The maximum size of the problem (for example, the dim. of M in
367 // LCP or Friction problems). Set to the number of possible scalar
368 // constraints declared in the topology.
369 if(_maxSize == 0) // if maxSize not set explicitely by user before
370 _maxSize = simulation()->nonSmoothDynamicalSystem()->topology()->numberOfConstraints();
371 }
372
getOSIMatrix(OneStepIntegrator & Osi,SP::DynamicalSystem ds)373 SP::SimpleMatrix OneStepNSProblem::getOSIMatrix(OneStepIntegrator& Osi, SP::DynamicalSystem ds)
374 {
375 // Returns the integration matrix from one-step integrator and dynamical system.
376
377 // Matrix depends on OSI type.
378 SP::SimpleMatrix block;
379 OSI::TYPES osiType; // type of the current one step integrator
380 Type::Siconos dsType; // type of the current Dynamical System
381
382 osiType = Osi.getType();
383 dsType = Type::value(*ds);
384
385 if(osiType == OSI::MOREAUJEANOSI
386 || osiType == OSI::MOREAUDIRECTPROJECTIONOSI)
387 {
388 block = (static_cast<MoreauJeanOSI&>(Osi)).W(ds); // get its W matrix ( pointer link!)
389 }
390 else if(osiType == OSI::MOREAUJEANBILBAOOSI)
391 {
392 block = (static_cast<MoreauJeanBilbaoOSI&>(Osi)).iteration_matrix(ds); // get its W matrix ( pointer link!)
393 }
394 else if(osiType == OSI::SCHATZMANPAOLIOSI)
395 {
396 block = (static_cast<SchatzmanPaoliOSI&>(Osi)).W(ds); // get its W matrix ( pointer link!)
397 }
398 else if(osiType == OSI::EULERMOREAUOSI)
399 {
400 block = (static_cast<EulerMoreauOSI&>(Osi)).W(ds); // get its W matrix ( pointer link!)
401 }
402 else if(osiType == OSI::LSODAROSI) // Warning: LagrangianDS only at the time !!!
403 {
404 if(dsType != Type::LagrangianDS && dsType != Type::LagrangianLinearTIDS)
405 THROW_EXCEPTION("OneStepNSProblem::getOSIMatrix not yet implemented for LsodarOSI Integrator with dynamical system of type " + std::to_string(dsType));
406
407 // get lu-factorized mass
408 block = (std::static_pointer_cast<LagrangianDS>(ds))->inverseMass();
409 }
410 else if(osiType == OSI::NEWMARKALPHAOSI)
411 {
412 if(dsType != Type::LagrangianDS && dsType != Type::LagrangianLinearTIDS)
413 {
414 THROW_EXCEPTION("OneStepNSProblem::getOSIMatrix not yet implemented for NewmarkAlphaOSI Integrator with dynamical system of type " + std::to_string(dsType));
415 }
416 //
417 SP::OneStepNSProblems allOSNS = Osi.simulation()->oneStepNSProblems();
418 // If LCP at acceleration level
419 if(((*allOSNS)[SICONOS_OSNSP_ED_SMOOTH_ACC]).get() == this)
420 {
421 block = (std::static_pointer_cast<LagrangianDS>(ds))->inverseMass();
422 }
423 else // It LCP at position level
424 {
425 block = (static_cast<NewMarkAlphaOSI&>(Osi)).W(ds);
426 }
427 } // End Newmark OSI
428 else if(osiType == OSI::D1MINUSLINEAROSI)
429 {
430 DEBUG_PRINT("OneStepNSProblem::getOSIMatrix for osiType OSI::D1MINUSLINEAR\n");
431 /** \warning V.A. 30/052013 for implicit D1Minus it will not be the mass matrix for all OSNSP*/
432 if(dsType == Type::LagrangianDS || dsType == Type::LagrangianLinearTIDS)
433 {
434 // SP::SimpleMatrix Mold;
435 // Mold.reset(new SimpleMatrix(*(std::static_pointer_cast<LagrangianDS>(ds))->mass()));
436 // DEBUG_EXPR(Mold->display(););
437 // DEBUG_EXPR_WE(std::cout << std::boolalpha << " Mold->isFactorized() = "<< Mold->isFactorized() << std::endl;);
438 //(std::static_pointer_cast<LagrangianDS>(ds))->computeMass();
439 SP::SiconosMatrix Mass = ((std::static_pointer_cast<LagrangianDS>(ds))->mass()) ;
440 DEBUG_EXPR(Mass->display(););
441 DEBUG_EXPR_WE(std::cout << std::boolalpha << " Mass->isFactorized() = "<< Mass->isFactorized() << std::endl;);
442
443 //DEBUG_EXPR(std::cout << (*Mass-*Mold).normInf() << std::endl;);
444 /*Copy of the current mass matrix. */
445 block.reset(new SimpleMatrix(*Mass));
446 }
447 else if(dsType == Type::NewtonEulerDS)
448 {
449 SP::NewtonEulerDS d = std::static_pointer_cast<NewtonEulerDS> (ds);
450 // d->computeMass();
451 // d->mass()->resetFactorizationFlags();
452 DEBUG_EXPR(d->mass()->display(););
453 block.reset(new SimpleMatrix(*(d->mass())));
454 }
455 else
456 THROW_EXCEPTION("OneStepNSProblem::getOSIMatrix not yet implemented for D1MinusLinearOSI integrator with dynamical system of type " + std::to_string(dsType));
457 }
458 // for ZeroOrderHoldOSI, the central block is Ad = \int exp{As} ds over t_k, t_{k+1}
459 else if(osiType == OSI::ZOHOSI)
460 {
461 if(!block)
462 block.reset(new SimpleMatrix((static_cast<ZeroOrderHoldOSI&>(Osi)).Ad(ds)));
463 else
464 *block = (static_cast<ZeroOrderHoldOSI&>(Osi)).Ad(ds);
465 }
466 else
467 THROW_EXCEPTION("OneStepNSProblem::getOSIMatrix not yet implemented for Integrator of type " + std::to_string(osiType));
468 return block;
469 }
470
setSolverId(int solverId)471 void OneStepNSProblem::setSolverId(int solverId)
472 {
473 // And create a new one, with default parameters values.
474 _numerics_solver_options.reset(solver_options_create(solverId),
475 solver_options_delete);
476 }
477
setNumericsVerboseMode(bool vMode)478 void OneStepNSProblem::setNumericsVerboseMode(bool vMode)
479 {
480 numerics_set_verbose(vMode);
481 }
482
setNumericsVerboseLevel(int level)483 void OneStepNSProblem::setNumericsVerboseLevel(int level)
484 {
485 numerics_set_verbose(level);
486 }
487