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
19 #include "TimeStepping.hpp"
20 #include "Topology.hpp"
21 #include "LCP.hpp"
22 #include "Relay.hpp"
23 #include "TimeDiscretisation.hpp"
24 #include "NonSmoothDynamicalSystem.hpp"
25 //#include "Interaction.hpp"
26 #include "OneStepIntegrator.hpp"
27 #include "EulerMoreauOSI.hpp"
28 #include "Interaction.hpp"
29 #include "EventsManager.hpp"
30 #include "FrictionContact.hpp"
31 #include "FirstOrderNonLinearDS.hpp"
32 #include "NonSmoothLaw.hpp"
33 #include "TypeName.hpp"
34 #include "Relation.hpp"
35 #include "BlockVector.hpp"
36 #include "NewtonEulerR.hpp"
37 #include "FirstOrderR.hpp"
38
39 #include <SiconosConfig.h>
40 #include <functional>
41 using namespace std::placeholders;
42
43 // #define DEBUG_BEGIN_END_ONLY
44 // #define DEBUG_STDOUT
45 // #define DEBUG_NOCOLOR
46 // #define DEBUG_MESSAGES
47 #include "siconos_debug.h"
48
49 using namespace RELATION;
50
51 /** Pointer to function, used to set the behavior of simulation when
52 ns solver failed. If equal to null, use DefaultCheckSolverOutput
53 else (set with setCheckSolverFunction) call the pointer below).
54 Note FP: (temporary) bad method to set checkSolverOutput but it
55 works ... It may be better to use plug-in?
56 */
57 static CheckSolverFPtr checkSolverOutput = nullptr;
58
TimeStepping(SP::NonSmoothDynamicalSystem nsds,SP::TimeDiscretisation td,SP::OneStepIntegrator osi,SP::OneStepNSProblem osnspb)59 TimeStepping::TimeStepping(SP::NonSmoothDynamicalSystem nsds,
60 SP::TimeDiscretisation td,
61 SP::OneStepIntegrator osi,
62 SP::OneStepNSProblem osnspb)
63 : Simulation(nsds,td), _newtonTolerance(1e-6), _newtonMaxIteration(50), _newtonNbIterations(0),
64 _newtonCumulativeNbIterations(0), _newtonOptions(SICONOS_TS_NONLINEAR),
65 _newtonResiduDSMax(0.0), _newtonResiduYMax(0.0), _newtonResiduRMax(0.0),
66 _computeResiduY(false),_computeResiduR(false),
67 _isNewtonConverge(false),
68 _newtonUpdateInteractionsPerIteration(false),_displayNewtonConvergence(false),
69 _warnOnNonConvergence(true),
70 _resetAllLambda(true)
71 {
72
73 if(osi) insertIntegrator(osi);
74 (*_allNSProblems).resize(SICONOS_NB_OSNSP_TS);
75 if(osnspb) insertNonSmoothProblem(osnspb, SICONOS_OSNSP_TS_VELOCITY);
76 SP::EulerMoreauOSI euosi(std::dynamic_pointer_cast<EulerMoreauOSI>(osi));
77 if(euosi)
78 {
79 _computeResiduY = true;
80 _computeResiduR = true;
81 }
82
83 }
84
TimeStepping(SP::NonSmoothDynamicalSystem nsds,SP::TimeDiscretisation td,int nb)85 TimeStepping::TimeStepping(SP::NonSmoothDynamicalSystem nsds, SP::TimeDiscretisation td, int nb)
86 : Simulation(nsds,td), _newtonTolerance(1e-6), _newtonMaxIteration(50), _newtonNbIterations(0),
87 _newtonCumulativeNbIterations(0), _newtonOptions(SICONOS_TS_NONLINEAR),
88 _newtonResiduDSMax(0.0), _newtonResiduYMax(0.0), _newtonResiduRMax(0.0), _computeResiduY(false),
89 _computeResiduR(false),
90 _isNewtonConverge(false),
91 _newtonUpdateInteractionsPerIteration(false),_displayNewtonConvergence(false),
92 _warnOnNonConvergence(true),
93 _resetAllLambda(true)
94 {
95 (*_allNSProblems).resize(nb);
96 }
97
98 // --- Destructor ---
~TimeStepping()99 TimeStepping::~TimeStepping()
100 {
101 }
102
insertIntegrator(SP::OneStepIntegrator osi)103 void TimeStepping::insertIntegrator(SP::OneStepIntegrator osi)
104 {
105 _allOSI->insert(osi);
106 SP::EulerMoreauOSI euosi(std::dynamic_pointer_cast<EulerMoreauOSI>(osi));
107 if(euosi)
108 {
109 _computeResiduY = true;
110 _computeResiduR = true;
111 }
112 }
113 // bool TimeStepping::predictorDeactivate(SP::Interaction inter, unsigned int i)
114 // {
115 // double h = timeStep();
116 // double y = inter->getYRef(i-1); // for i=1 it is the position -> historic notation y
117 // double yDot = inter->getYRef(i); // for i=1 it is the velocity -> historic notation yDot
118 // DEBUG_PRINTF("TS::predictorDeactivate yref=%e, yDot=%e, y_estimated=%e.\n", y, yDot, y+0.5*h*yDot);
119 // y += 0.5*h*yDot;
120 // assert(!std::isnan(y));
121 // if(y>0)
122 // DEBUG_PRINTF("TS::predictorDeactivate DEACTIVATE.\n");
123 // return (y>0);
124 // }
125
126 // bool TimeStepping::predictorActivate(SP::Interaction inter, unsigned int i)
127 // {
128 // double h = timeStep();
129 // double y = inter->getYRef(i-1); // for i=1 it is the position -> historic notation y
130 // double yDot = inter->getYRef(i); // for i=1 it is the velocity -> historic notation yDot
131 // DEBUG_PRINTF("TS::predictorActivate yref=%e, yDot=%e, y_estimated=%e.\n", y, yDot, y+0.5*h*yDot);
132 // y += 0.5*h*yDot;
133 // assert(!std::isnan(y));
134 // if(y<=0)
135 // DEBUG_PRINTF("TS::predictorActivate ACTIVATE.\n");
136 // return (y<=0);
137 // }
138
updateIndexSet(unsigned int i)139 void TimeStepping::updateIndexSet(unsigned int i)
140 {
141 // To update IndexSet i: add or remove Interactions from
142 // this set, depending on y values.
143 // boost::default_color_type is used to organize update in InteractionsGraph:
144 // - white_color : undiscovered vertex (Interaction)
145 // - gray_color : discovered vertex (Interaction) but searching descendants
146 // - black_color : discovered vertex (Interaction) together with the descendants
147
148 assert(_nsds);
149 assert(_nsds->topology());
150
151 SP::Topology topo = _nsds->topology();
152
153 assert(i < topo->indexSetsSize() &&
154 "TimeStepping::updateIndexSet(i), indexSets[i] does not exist.");
155 // IndexSets[0] must not be updated in simulation, since it belongs to Topology.
156 assert(i > 0 &&
157 "TimeStepping::updateIndexSet(i=0), indexSets[0] cannot be updated.");
158
159 // For all Interactions in indexSet[i-1], compute y[i-1] and
160 // update the indexSet[i].
161 SP::InteractionsGraph indexSet0 = topo->indexSet(0);
162 SP::InteractionsGraph indexSet1 = topo->indexSet(1);
163 assert(indexSet0);
164 assert(indexSet1);
165 DynamicalSystemsGraph& DSG0= *nonSmoothDynamicalSystem()->dynamicalSystems();
166 topo->setHasChanged(false);
167
168 DEBUG_PRINTF("TimeStepping::updateIndexSet(unsigned int i). update indexSets start : indexSet0 size : %ld\n", indexSet0->size());
169 DEBUG_PRINTF("TimeStepping::updateIndexSet(unsigned int i). update IndexSets start : indexSet1 size : %ld\n", indexSet1->size());
170
171 // Check indexSet1
172 InteractionsGraph::VIterator ui1, ui1end, v1next;
173 std::tie(ui1, ui1end) = indexSet1->vertices();
174
175 //Remove interactions from the indexSet1
176 for(v1next = ui1; ui1 != ui1end; ui1 = v1next)
177 {
178 ++v1next;
179
180 SP::Interaction inter1 = indexSet1->bundle(*ui1);
181 if(indexSet0->is_vertex(inter1))
182 {
183 InteractionsGraph::VDescriptor inter1_descr0 = indexSet0->descriptor(inter1);
184
185 assert((indexSet0->color(inter1_descr0) == boost::white_color));
186
187 indexSet0->color(inter1_descr0) = boost::gray_color;
188 if(Type::value(*(inter1->nonSmoothLaw())) != Type::EqualityConditionNSL)
189 {
190 // We assume that the integrator of the ds1 drive the update of the index set
191 //SP::OneStepIntegrator Osi = indexSet1->properties(*ui1).osi;
192 SP::DynamicalSystem ds1 = indexSet1->properties(*ui1).source;
193 OneStepIntegrator& osi = *DSG0.properties(DSG0.descriptor(ds1)).osi;
194
195 //if(predictorDeactivate(inter1,i))
196 if(osi.removeInteractionFromIndexSet(inter1, i))
197 {
198 // Interaction is not active
199 // ui1 becomes invalid
200 indexSet0->color(inter1_descr0) = boost::black_color;
201
202 indexSet1->eraseProperties(*ui1);
203
204 InteractionsGraph::OEIterator oei, oeiend;
205 for(std::tie(oei, oeiend) = indexSet1->out_edges(*ui1);
206 oei != oeiend; ++oei)
207 {
208 InteractionsGraph::EDescriptor ed1, ed2;
209 std::tie(ed1, ed2) = indexSet1->edges(indexSet1->source(*oei), indexSet1->target(*oei));
210 if(ed2 != ed1)
211 {
212 indexSet1->eraseProperties(ed1);
213 indexSet1->eraseProperties(ed2);
214 }
215 else
216 {
217 indexSet1->eraseProperties(ed1);
218 }
219 }
220 indexSet1->remove_vertex(inter1);
221 /* \warning V.A. 25/05/2012 : Multiplier lambda are only set to zero if they are removed from the IndexSet*/
222 inter1->lambda(1)->zero();
223 topo->setHasChanged(true);
224 }
225 }
226 }
227 else
228 {
229 // Interaction is not in indexSet0 anymore.
230 // ui1 becomes invalid
231 indexSet1->eraseProperties(*ui1);
232 InteractionsGraph::OEIterator oei, oeiend;
233 for(std::tie(oei, oeiend) = indexSet1->out_edges(*ui1);
234 oei != oeiend; ++oei)
235 {
236 InteractionsGraph::EDescriptor ed1, ed2;
237 std::tie(ed1, ed2) = indexSet1->edges(indexSet1->source(*oei), indexSet1->target(*oei));
238 if(ed2 != ed1)
239 {
240 indexSet1->eraseProperties(ed1);
241 indexSet1->eraseProperties(ed2);
242 }
243 else
244 {
245 indexSet1->eraseProperties(ed1);
246 }
247 }
248
249 indexSet1->remove_vertex(inter1);
250 topo->setHasChanged(true);
251 }
252 }
253
254 // indexSet0\indexSet1 scan
255 InteractionsGraph::VIterator ui0, ui0end;
256 //Add interaction in indexSet1
257 for(std::tie(ui0, ui0end) = indexSet0->vertices(); ui0 != ui0end; ++ui0)
258 {
259 if(indexSet0->color(*ui0) == boost::black_color)
260 {
261 // reset
262 indexSet0->color(*ui0) = boost::white_color ;
263 }
264 else
265 {
266 if(indexSet0->color(*ui0) == boost::gray_color)
267 {
268 // reset
269 indexSet0->color(*ui0) = boost::white_color;
270
271 assert(indexSet1->is_vertex(indexSet0->bundle(*ui0)));
272 /*assert( { !predictorDeactivate(indexSet0->bundle(*ui0),i) ||
273 Type::value(*(indexSet0->bundle(*ui0)->nonSmoothLaw())) == Type::EqualityConditionNSL ;
274 });*/
275 }
276 else
277 {
278 assert(indexSet0->color(*ui0) == boost::white_color);
279
280 SP::Interaction inter0 = indexSet0->bundle(*ui0);
281 assert(!indexSet1->is_vertex(inter0));
282 bool activate = true;
283 if(Type::value(*(inter0->nonSmoothLaw())) != Type::EqualityConditionNSL
284 && Type::value(*(inter0->nonSmoothLaw())) != Type::RelayNSL)
285 {
286 //SP::OneStepIntegrator Osi = indexSet0->properties(*ui0).osi;
287 // We assume that the integrator of the ds1 drive the update of the index set
288 SP::DynamicalSystem ds1 = indexSet1->properties(*ui0).source;
289 OneStepIntegrator& osi = *DSG0.properties(DSG0.descriptor(ds1)).osi;
290
291 activate = osi.addInteractionInIndexSet(inter0, i);
292 }
293 if(activate)
294 {
295 assert(!indexSet1->is_vertex(inter0));
296
297 // vertex and edges insertion in indexSet1
298 indexSet1->copy_vertex(inter0, *indexSet0);
299 topo->setHasChanged(true);
300 assert(indexSet1->is_vertex(inter0));
301 }
302 }
303 }
304 }
305
306 assert(indexSet1->size() <= indexSet0->size());
307
308 DEBUG_PRINTF("TimeStepping::updateIndexSet(unsigned int i). update indexSets end : indexSet0 size : %ld\n", indexSet0->size());
309 DEBUG_PRINTF("TimeStepping::updateIndexSet(unsigned int i). update IndexSets end : indexSet1 size : %ld\n", indexSet1->size());
310 }
311
312 // void TimeStepping::insertNonSmoothProblem(SP::OneStepNSProblem osns)
313 // {
314 // // A the time, a time stepping simulation can only have one non
315 // // smooth problem.
316 // if((*_allNSProblems)[SICONOS_OSNSP_TS_VELOCITY])
317 // THROW_EXCEPTION
318 // ("TimeStepping, insertNonSmoothProblem - A non smooth problem already exist. You can not have more than one.");
319
320 // (*_allNSProblems)[SICONOS_OSNSP_TS_VELOCITY] = osns;
321 // }
322
initOSNS()323 void TimeStepping::initOSNS()
324 {
325 // === creates links between work vector in OSI and work vector in
326 // Interactions
327 SP::OneStepIntegrator osi;
328
329 SP::Topology topo = _nsds->topology();
330 SP::InteractionsGraph indexSet0 = topo->indexSet(0);
331
332 InteractionsGraph::VIterator ui, uiend;
333
334 if(!_allNSProblems->empty()) // ie if some Interactions have been
335 // declared and a Non smooth problem
336 // built.
337 {
338 //if (_allNSProblems->size()>1)
339 // THROW_EXCEPTION("TimeStepping::initialize, at the time, a time stepping simulation can not have more than one non smooth problem.");
340
341 // At the time, we consider that for all systems, levelMin is
342 // equal to the minimum value of the relative degree - 1 except
343 // for degree 0 case where we keep 0.
344
345
346 // === update all index sets ===
347 updateIndexSets();
348
349 // initialization of OneStepNonSmoothProblem
350 for(OSNSIterator itOsns = _allNSProblems->begin(); itOsns != _allNSProblems->end(); ++itOsns)
351 {
352 if(*itOsns)
353 (*itOsns)->initialize(shared_from_this());
354 else
355 THROW_EXCEPTION("TimeStepping::initOSNS failed. A OneStepNSProblem has not been set. ");
356 }
357 }
358 }
359
nextStep()360 void TimeStepping::nextStep()
361 {
362 DEBUG_BEGIN("void TimeStepping::nextStep()\n");
363 processEvents();
364 DEBUG_END("void TimeStepping::nextStep()\n");
365 }
366
computeFreeState()367 void TimeStepping::computeFreeState()
368 {
369 DEBUG_BEGIN("TimeStepping::computeFreeState()\n");
370 std::for_each(_allOSI->begin(), _allOSI->end(), std::bind(&OneStepIntegrator::computeFreeState, _1));
371 DEBUG_END("TimeStepping::computeFreeState()\n");
372 }
373
374 // compute simulation between current and next event. Initial
375 // DS/interaction state is given by memory vectors and final state is
376 // the one saved in DS/Interaction at the end of this function
computeOneStep()377 void TimeStepping::computeOneStep()
378 {
379 advanceToEvent();
380 }
381
382
initializeNewtonLoop()383 void TimeStepping::initializeNewtonLoop()
384 {
385 DEBUG_BEGIN("TimeStepping::initializeNewtonLoop()\n");
386 double tkp1 = getTkp1();
387 assert(!std::isnan(tkp1));
388
389 for(OSIIterator it = _allOSI->begin(); it != _allOSI->end() ; ++it)
390 {
391 (*it)->computeInitialNewtonState();
392 (*it)->computeResidu();
393 }
394
395 // Predictive contact -- update initial contacts after updating DS positions
396 updateWorldFromDS();
397 updateInteractions();
398
399 // Changes in updateInteractions may require initialization
400 initializeNSDSChangelog();
401
402 SP::InteractionsGraph indexSet0 = _nsds->topology()->indexSet0();
403 if(indexSet0->size()>0)
404 {
405 for(OSIIterator itOSI = _allOSI->begin(); itOSI != _allOSI->end() ; ++itOSI)
406 {
407 (*itOSI)->updateOutput(nextTime());
408 (*itOSI)->updateInput(nextTime());
409 }
410 }
411
412 SP::DynamicalSystemsGraph dsGraph = _nsds->dynamicalSystems();
413 for(DynamicalSystemsGraph::VIterator vi = dsGraph->begin(); vi != dsGraph->end(); ++vi)
414 {
415 dsGraph->bundle(*vi)->updatePlugins(tkp1);
416 }
417
418 for(OSIIterator it = _allOSI->begin(); it != _allOSI->end() ; ++it)
419 (*it)->computeResidu();
420
421 if(_computeResiduY)
422 {
423 for(OSIIterator itOSI = _allOSI->begin(); itOSI != _allOSI->end() ; ++itOSI)
424 {
425 (*itOSI)->computeResiduOutput(tkp1, indexSet0);
426 }
427 }
428 DEBUG_END("TimeStepping::initializeNewtonLoop()\n");
429 }
430
run()431 void TimeStepping::run()
432 {
433 unsigned int count = 0; // events counter.
434 // do simulation while events remains in the "future events" list of
435 // events manager.
436 std::cout << " ==== Start of " << Type::name(*this) << " simulation - This may take a while ... ====" <<std::endl;
437 while(_eventsManager->hasNextEvent())
438 {
439 advanceToEvent();
440
441 processEvents();
442 count++;
443 }
444 std::cout << "===== End of " << Type::name(*this) << "simulation. " << count << " events have been processed. ==== " <<std::endl;
445 }
446
resetLambdas()447 void TimeStepping::resetLambdas()
448 {
449 if(_resetAllLambda)
450 {
451 // Initialize lambdas of all interactions.
452 SP::InteractionsGraph indexSet0 = _nsds->topology()->indexSet(0);
453 InteractionsGraph::VIterator ui, uiend, vnext;
454 std::tie(ui, uiend) = indexSet0->vertices();
455 for(vnext = ui; ui != uiend; ui = vnext)
456 {
457 ++vnext;
458 indexSet0->bundle(*ui)->resetAllLambda();
459 }
460 }
461 }
462
advanceToEvent()463 void TimeStepping::advanceToEvent()
464 {
465 DEBUG_PRINTF("TimeStepping::advanceToEvent(). Time =%f\n",getTkp1());
466 initialize();
467 resetLambdas();
468 newtonSolve(_newtonTolerance, _newtonMaxIteration);
469 }
470
471 /*update of the nabla */
472 /*discretisation of the Interactions */
prepareNewtonIteration()473 void TimeStepping::prepareNewtonIteration()
474 {
475 DEBUG_BEGIN("TimeStepping::prepareNewtonIteration()\n");
476 double tkp1 = getTkp1();
477 for(OSIIterator itosi = _allOSI->begin();
478 itosi != _allOSI->end(); ++itosi)
479 {
480 (*itosi)->prepareNewtonIteration(tkp1);
481 }
482
483 DEBUG_END("TimeStepping::prepareNewtonIteration()\n");
484 }
485
saveYandLambdaInOldVariables()486 void TimeStepping::saveYandLambdaInOldVariables()
487 {
488 // Temp FP : saveInOldVar was called for each osns and each osns call
489 // swapInOldVar for all interactions in the nsds.
490 // ==> let's do it only once, by the simu.
491
492 InteractionsGraph::VIterator ui, uiend;
493 SP::InteractionsGraph indexSet0 = _nsds->topology()->indexSet0();
494 for(std::tie(ui, uiend) = indexSet0->vertices(); ui != uiend; ++ui)
495 {
496 //indexSet0->bundle(*ui)->swapInMemory();
497 indexSet0->bundle(*ui)->swapInOldVariables();;
498 }
499 }
500
displayNewtonConvergenceInTheLoop()501 void TimeStepping::displayNewtonConvergenceInTheLoop()
502 {
503 if(_displayNewtonConvergence)
504 {
505 std::cout << "[kernel] TimeStepping::newtonSolve -- _newtonNbIterations =" << _newtonNbIterations << std::endl;
506 std::cout << "[kernel] TimeStepping::newtonSolve -- _newtonResiduDSMax =" << _newtonResiduDSMax << std::endl;
507 if(_computeResiduY)
508 {
509 std::cout << "[kernel] TimeStepping::newtonSolve -- _newtonResiduYMax =" << _newtonResiduYMax << std::endl;
510 }
511 else
512 {
513 std::cout << "[kernel] TimeStepping::newtonSolve -- _newtonResiduYMax =" << "not computed" << std::endl;
514 }
515 if(_computeResiduR)
516 {
517 std::cout << "[kernel] TimeStepping::newtonSolve -- _newtonResiduRMax =" << _newtonResiduRMax << std::endl;
518 }
519 else
520 {
521 std::cout << "[kernel] TimeStepping::newtonSolve -- _newtonResiduRMax =" << "not computed" << std::endl;
522 }
523 }
524 else
525 {
526 DEBUG_PRINTF("# _newtonNbIterations = %i\n",_newtonNbIterations);
527 DEBUG_PRINTF("# _newtonResiduDSMax = %12.8e\t",_newtonResiduDSMax);
528 DEBUG_PRINTF("# _newtonResiduYMax = %12.8e\t",_newtonResiduYMax);
529 DEBUG_PRINTF("# _newtonResiduRMax = %12.8e\n",_newtonResiduRMax);
530 }
531 }
displayNewtonConvergenceAtTheEnd(int info,unsigned int maxStep)532 void TimeStepping::displayNewtonConvergenceAtTheEnd(int info, unsigned int maxStep)
533 {
534 if(_displayNewtonConvergence)
535 {
536 std::cout << "[kernel] TimeStepping::newtonSolve -- _newtonCumulativeNbIterations =" << _newtonCumulativeNbIterations << std::endl;
537 }
538 else
539 {
540 DEBUG_PRINTF("# _newtonCumulativeNbIterations= %i\n",_newtonCumulativeNbIterations);
541 }
542
543 if(!_isNewtonConverge)
544 {
545 if(_warnOnNonConvergence)
546
547 std::cout << "[kernel][warning] TimeStepping::newtonSolve reached max. number of iterations: "
548 << maxStep
549 <<" with accuracy: "
550 << _newtonResiduDSMax
551 << std::endl ;
552
553 if(info && _warnOnNonConvergence)
554 std::cout << "[kernel] TimeStepping::newtonSolve -- nonsmooth solver failed." <<std::endl ;
555 }
556 }
newtonSolve(double criterion,unsigned int maxStep)557 void TimeStepping::newtonSolve(double criterion, unsigned int maxStep)
558 {
559
560 DEBUG_BEGIN("TimeStepping::newtonSolve(double criterion, unsigned int maxStep)\n");
561 _isNewtonConverge = false;
562 _newtonNbIterations = 0; // number of Newton iterations
563 int info = 0;
564 bool isLinear = _nsds->isLinear();
565
566 initializeNewtonLoop();
567
568 if((_newtonOptions == SICONOS_TS_LINEAR || _newtonOptions == SICONOS_TS_LINEAR_IMPLICIT)
569 || isLinear)
570 {
571 _newtonNbIterations++;
572 DEBUG_PRINTF("TimeStepping::newtonSolve(). _newtonNbIterations = %i\n", _newtonNbIterations);
573 prepareNewtonIteration();
574 computeFreeState();
575 //bool hasNSProblems = (!_allNSProblems->empty() && indexSet0.size() > 0) ? true : false;
576 bool hasNSProblems = (!_allNSProblems->empty()) ? true : false;
577 if(hasNSProblems)
578 info = computeOneStepNSProblem(SICONOS_OSNSP_TS_VELOCITY);
579 // Check output from solver (convergence or not ...)
580 if(!checkSolverOutput)
581 DefaultCheckSolverOutput(info);
582 else
583 checkSolverOutput(info, this);
584
585 update();
586
587 hasNSProblems = (!_allNSProblems->empty()) ? true : false;
588 if(hasNSProblems)
589 saveYandLambdaInOldVariables();
590 }
591
592 else if(_newtonOptions == SICONOS_TS_NONLINEAR)
593 {
594 // while((!_isNewtonConverge)&&(_newtonNbIterations < maxStep)&&(!info))
595 //_isNewtonConverge = newtonCheckConvergence(criterion);
596 while((!_isNewtonConverge) && (_newtonNbIterations < maxStep))
597 {
598 _newtonNbIterations++;
599 info=0;
600
601 prepareNewtonIteration();
602 computeFreeState();
603
604 if(info && _warnOnNonConvergence)
605 std::cout << "New Newton loop because of nonsmooth solver failed\n" <<std::endl;
606
607 // if there is not any Interaction at
608 // the beginning of the simulation _allNSProblems may not be
609 // empty here (check with SpaceFilter and one disk not on
610 // the ground : MultiBodyTest::t2)
611
612 // if((*_allNSProblems)[SICONOS_OSNSP_TS_VELOCITY]->simulation())
613 // is also relevant here.
614 //InteractionsGraph& indexSet0 = *_nsds->topology()->indexSet0();
615 //bool hasNSProblems = (!_allNSProblems->empty() && indexSet0.size() > 0) ? true : false;
616
617 bool hasNSProblems = (!_allNSProblems->empty()) ? true : false;
618 if(hasNSProblems)
619 {
620 info = computeOneStepNSProblem(SICONOS_OSNSP_TS_VELOCITY);
621 // Check output from solver (convergence or not ...)
622 if(!checkSolverOutput)
623 DefaultCheckSolverOutput(info);
624 else
625 checkSolverOutput(info, this);
626 }
627
628 updateInput();
629 updateState();
630
631 if(!_isNewtonConverge && _newtonNbIterations < maxStep)
632 {
633 updateOutput();
634 }
635
636 _isNewtonConverge = newtonCheckConvergence(criterion);
637
638 if(!_isNewtonConverge && !info)
639 {
640 if(hasNSProblems)
641 saveYandLambdaInOldVariables();
642 }
643 displayNewtonConvergenceInTheLoop();
644 } // End of the Newton Loop
645
646 _newtonCumulativeNbIterations += _newtonNbIterations;
647
648 displayNewtonConvergenceAtTheEnd(info, maxStep);
649 }
650 else
651 THROW_EXCEPTION("TimeStepping::NewtonSolve failed. Unknown newtonOptions: " + std::to_string(_newtonOptions));
652 DEBUG_END("TimeStepping::newtonSolve(double criterion, unsigned int maxStep)\n");
653 }
654
newtonCheckConvergence(double criterion)655 bool TimeStepping::newtonCheckConvergence(double criterion)
656 {
657 bool checkConvergence = true;
658 //_relativeConvergenceCriterionHeld is true means that the RCC is
659 //activated, and the relative criteron helds. In this case the
660 //newtonCheckConvergence has to return true. End of the Newton
661 //iterations
662 if(_relativeConvergenceCriterionHeld)
663 {
664 return true;
665 }
666 // get the nsds indicator of convergence
667 // We compute cvg = abs(xi+1 - xi)/xi and if cvg < criterion
668 // if (nsdsConverge < criterion )
669
670 double residu = 0.0;
671 _newtonResiduDSMax = 0.0;
672 for(OSIIterator it = _allOSI->begin(); it != _allOSI->end() ; ++it)
673 {
674 residu = (*it)->computeResidu();
675
676 if(residu > _newtonResiduDSMax) _newtonResiduDSMax = residu;
677 if(residu > criterion)
678 {
679 checkConvergence = false;
680 //break;
681 }
682 }
683
684
685
686
687 if(_computeResiduY)
688 {
689 //check residuy.
690 _newtonResiduYMax = 0.0;
691 residu = 0.0;
692
693 SP::InteractionsGraph indexSet0 = _nsds->topology()->indexSet0();
694 for(OSIIterator itOSI = _allOSI->begin(); itOSI != _allOSI->end() ; ++itOSI)
695 {
696 residu = std::max(residu,(*itOSI)->computeResiduOutput(getTkp1(), indexSet0));
697 }
698
699
700 // InteractionsGraph::VIterator ui, uiend;
701 // SP::Interaction inter;
702 // for (std::tie(ui, uiend) = indexSet0->vertices(); ui != uiend; ++ui)
703 // {
704 // inter = indexSet0->bundle(*ui);
705 // VectorOfVectors& workV = *indexSet0->properties(*ui).workVectors;
706
707 // inter->computeResiduY(, workV);
708 // inter->residuY()->norm2();
709 if(residu > _newtonResiduYMax) _newtonResiduYMax = residu;
710 if(residu > criterion)
711 checkConvergence = false;
712 }
713
714 if(_computeResiduR)
715 {
716 //check residur.
717 _newtonResiduRMax = 0.0;
718 residu = 0.0;
719 SP::InteractionsGraph indexSet0 = _nsds->topology()->indexSet0();
720
721 for(OSIIterator itOSI = _allOSI->begin(); itOSI != _allOSI->end() ; ++itOSI)
722 {
723 residu = std::max(residu,(*itOSI)->computeResiduInput(getTkp1(), indexSet0));
724 }
725
726
727 // InteractionsGraph::VIterator ui, uiend;
728 // SP::Interaction inter;
729 // for (std::tie(ui, uiend) = indexSet0->vertices(); ui != uiend; ++ui)
730 // {
731 // inter = indexSet0->bundle(*ui);
732 // VectorOfBlockVectors& DSlink = *indexSet0->properties(*ui).DSlink;
733 // VectorOfVectors& workV = *indexSet0->properties(*ui).workVectors;
734
735 // inter->computeResiduR(getTkp1(), DSlink, workV);
736 // // TODO support other DS
737 if(residu > _newtonResiduRMax) _newtonResiduRMax = residu;
738 if(residu > criterion)
739 {
740 checkConvergence = false;
741 }
742 }
743
744
745 return(checkConvergence);
746 }
747
DefaultCheckSolverOutput(int info)748 void TimeStepping::DefaultCheckSolverOutput(int info)
749 {
750 // info = 0 => ok
751 // else: depend on solver
752 if(info != 0)
753 {
754 std::cout << "[kernel] TimeStepping::DefaultCheckSolverOutput:" << std::endl;
755 std::cout << "[kernel] Non smooth solver warning : output message from numerics solver is equal to " << info << std::endl;
756 // std::cout << "=> may have failed? (See Numerics solver documentation for details on the message meaning)." <<std::endl;
757 // std::cout << "=> may have failed? (See Numerics solver documentation for details on the message meaning)." <<std::endl;
758 // THROW_EXCEPTION(" Non smooth problem, solver convergence failed ");
759 /* if(info == 1)
760 std::cout <<" reach max iterations number with solver " << solverName <<std::endl;
761 else if (info == 2)
762 {
763 if (solverName == "LexicoLemke" || solverName == "CPG" || solverName == "NLGS")
764 THROW_EXCEPTION(" negative diagonal term with solver "+solverName);
765 else if (solverName == "QP" || solverName == "NSQP" )
766 THROW_EXCEPTION(" can not satisfy convergence criteria for solver "+solverName);
767 else if (solverName == "Latin")
768 THROW_EXCEPTION(" Choleski factorisation failed with solver Latin");
769 }
770 else if (info == 3 && solverName == "CPG")
771 std::cout << "pWp null in solver CPG" <<std::endl;
772 else if (info == 3 && solverName == "Latin")
773 THROW_EXCEPTION("Null diagonal term with solver Latin");
774 else if (info == 5 && (solverName == "QP" || solverName == "NSQP"))
775 THROW_EXCEPTION("Length of working array insufficient in solver "+solverName);
776 else
777 THROW_EXCEPTION("Unknown error type in solver "+ solverName);
778 */
779 }
780 }
781
setCheckSolverFunction(CheckSolverFPtr newF)782 void TimeStepping::setCheckSolverFunction(CheckSolverFPtr newF)
783 {
784 checkSolverOutput = newF;
785 }
786