1 // =============================================================================
2 // PROJECT CHRONO - http://projectchrono.org
3 //
4 // Copyright (c) 2014 projectchrono.org
5 // All rights reserved.
6 //
7 // Use of this source code is governed by a BSD-style license that can be found
8 // in the LICENSE file at the top level of the distribution and at
9 // http://projectchrono.org/license-chrono.txt.
10 //
11 // =============================================================================
12
13 #include <cstdlib>
14
15 #include "chrono/timestepper/ChStaticAnalysis.h"
16
17 namespace chrono {
18
ChStaticAnalysis(ChIntegrableIIorder & integrable)19 ChStaticAnalysis::ChStaticAnalysis(ChIntegrableIIorder& integrable) {
20 m_integrable = &integrable;
21 X.setZero(1, m_integrable);
22 };
23
24 // -----------------------------------------------------------------------------
25
ChStaticLinearAnalysis(ChIntegrableIIorder & integrable)26 ChStaticLinearAnalysis::ChStaticLinearAnalysis(ChIntegrableIIorder& integrable) : ChStaticAnalysis(integrable) {}
27
StaticAnalysis()28 void ChStaticLinearAnalysis::StaticAnalysis() {
29 ChIntegrableIIorder* integrable = static_cast<ChIntegrableIIorder*>(m_integrable);
30
31 // Set up main vectors
32 double T;
33 ChStateDelta V(integrable);
34 X.resize(integrable->GetNcoords_x());
35 V.resize(integrable->GetNcoords_v());
36 integrable->StateGather(X, V, T); // state <- system
37
38 // Set V speed to zero
39 V.setZero(integrable->GetNcoords_v(), integrable);
40 integrable->StateScatter(X, V, T, true); // state -> system
41
42 // Set up auxiliary vectors
43 ChStateDelta Dx;
44 ChVectorDynamic<> R;
45 ChVectorDynamic<> Qc;
46
47 Dx.setZero(integrable->GetNcoords_v(), integrable);
48 R.setZero(integrable->GetNcoords_v());
49 Qc.setZero(integrable->GetNconstr());
50 L.setZero(integrable->GetNconstr());
51
52 // Solve
53 // [-dF/dx Cq' ] [ dx ] = [ f]
54 // [ Cq 0 ] [ l ] = [-C]
55
56 integrable->LoadResidual_F(R, 1.0);
57 integrable->LoadConstraint_C(Qc, 1.0); // -C (-sign already included)
58
59 integrable->StateSolveCorrection( //
60 Dx, L, R, Qc, //
61 0, // factor for M
62 0, // factor for dF/dv
63 -1.0, // factor for dF/dx (the stiffness matrix)
64 X, V, T, // not needed here
65 false, // do not scatter Xnew Vnew T+dt before computing correction
66 false, // full update? (not used, since no scatter)
67 true // force a call to the solver's Setup() function
68 );
69
70 X += Dx;
71
72 integrable->StateScatter(X, V, T, true); // state -> system
73 integrable->StateScatterReactions(L); // -> system auxiliary data
74 }
75
76 // -----------------------------------------------------------------------------
77
ChStaticNonLinearAnalysis(ChIntegrableIIorder & integrable)78 ChStaticNonLinearAnalysis::ChStaticNonLinearAnalysis(ChIntegrableIIorder& integrable)
79 : ChStaticAnalysis(integrable),
80 m_maxiters(20),
81 m_incremental_steps(6),
82 m_use_correction_test(true),
83 m_reltol(1e-4),
84 m_abstol(1e-8),
85 m_verbose(false) {}
86
StaticAnalysis()87 void ChStaticNonLinearAnalysis::StaticAnalysis() {
88 ChIntegrableIIorder* integrable = static_cast<ChIntegrableIIorder*>(m_integrable);
89
90 if (m_verbose) {
91 GetLog() << "\nNonlinear statics\n";
92 GetLog() << " max iterations: " << m_maxiters << "\n";
93 GetLog() << " incremental steps: " << m_incremental_steps << "\n";
94 if (m_use_correction_test) {
95 GetLog() << " stopping test: correction\n";
96 GetLog() << " relative tol: " << m_reltol << "\n";
97 GetLog() << " absolute tol: " << m_abstol << "\n";
98 } else {
99 GetLog() << " stopping test: residual\n";
100 GetLog() << " tolerance: " << m_abstol << "\n";
101 }
102 GetLog() << "\n";
103 }
104
105 // Set up main vectors
106 double T;
107 ChStateDelta V(integrable);
108 X.resize(integrable->GetNcoords_x());
109 V.resize(integrable->GetNcoords_v());
110 integrable->StateGather(X, V, T); // state <- system
111
112 // Set speed to zero
113 V.setZero(integrable->GetNcoords_v(), integrable);
114 integrable->StateScatter(X, V, T, true); // state -> system
115
116 // Set up auxiliary vectors
117 ChState Xnew;
118 ChStateDelta Dx;
119 ChVectorDynamic<> R;
120 ChVectorDynamic<> Qc;
121 Xnew.setZero(integrable->GetNcoords_x(), integrable);
122 Dx.setZero(integrable->GetNcoords_v(), integrable);
123 R.setZero(integrable->GetNcoords_v());
124 Qc.setZero(integrable->GetNconstr());
125 L.setZero(integrable->GetNconstr());
126
127 // Use Newton Raphson iteration, solving for the increments
128 // [ - dF/dx Cq' ] [ Dx ] = [ f ]
129 // [ Cq 0 ] [ L ] = [-C ]
130
131 for (int i = 0; i < m_maxiters; ++i) {
132 integrable->StateScatter(X, V, T, true); // state -> system
133 R.setZero();
134 Qc.setZero();
135 integrable->LoadResidual_F(R, 1.0);
136 integrable->LoadConstraint_C(Qc, 1.0);
137
138 double cfactor = ChMin(1.0, (i + 2.0) / (m_incremental_steps + 1.0));
139 R *= cfactor;
140 Qc *= cfactor;
141
142 if (!m_use_correction_test) {
143 // Evaluate residual norms
144 double R_norm = R.lpNorm<Eigen::Infinity>();
145 double Qc_norm = Qc.lpNorm<Eigen::Infinity>();
146
147 if (m_verbose) {
148 GetLog() << "--- Nonlinear statics iteration " << i << " |R|_inf = " << R_norm
149 << " |Qc|_inf = " << Qc_norm << "\n";
150 }
151
152 // Stopping test
153 if ((R_norm < m_abstol) && (Qc_norm < m_abstol)) {
154 if (m_verbose) {
155 GetLog() << "+++ Newton procedure converged in " << i + 1 << " iterations.\n\n";
156 }
157 break;
158 }
159 }
160
161 // Solve linear system for correction
162 integrable->StateSolveCorrection( //
163 Dx, L, R, Qc, //
164 0, // factor for M
165 0, // factor for dF/dv
166 -1.0, // factor for dF/dx (the stiffness matrix)
167 X, V, T, // not needed here
168 false, // do not scatter Xnew Vnew T+dt before computing correction
169 false, // full update? (not used, since no scatter)
170 true // force a call to the solver's Setup() function
171 );
172
173 Xnew = X + Dx;
174
175 if (m_use_correction_test) {
176 // Calculate actual correction in X
177 ChState correction = Xnew - X;
178
179 // Evaluate weights and correction WRMS norm
180 ChVectorDynamic<> ewt = (m_reltol * Xnew.cwiseAbs() + m_abstol).cwiseInverse();
181 double Dx_norm = correction.wrmsNorm(ewt);
182
183 if (m_verbose) {
184 GetLog() << "--- Nonlinear statics iteration " << i << " |Dx|_wrms = " << Dx_norm << "\n";
185 }
186
187 // Stopping test
188 if (Dx_norm < 1) {
189 if (m_verbose) {
190 double R_norm = R.lpNorm<Eigen::Infinity>();
191 double Qc_norm = Qc.lpNorm<Eigen::Infinity>();
192 GetLog() << "+++ Newton procedure converged in " << i + 1 << " iterations.\n";
193 GetLog() << " |R|_inf = " << R_norm << " |Qc|_inf = " << Qc_norm << "\n\n";
194 }
195 X = Xnew;
196 break;
197 }
198 }
199
200 X = Xnew;
201 }
202
203 integrable->StateScatter(X, V, T, true); // state -> system
204 integrable->StateScatterReactions(L); // -> system auxiliary data
205 }
206
SetCorrectionTolerance(double reltol,double abstol)207 void ChStaticNonLinearAnalysis::SetCorrectionTolerance(double reltol, double abstol) {
208 m_use_correction_test = true;
209 m_reltol = reltol;
210 m_abstol = abstol;
211 }
212
SetResidualTolerance(double tol)213 void ChStaticNonLinearAnalysis::SetResidualTolerance(double tol) {
214 m_use_correction_test = false;
215 m_abstol = tol;
216 }
217
SetMaxIterations(int max_iters)218 void ChStaticNonLinearAnalysis::SetMaxIterations(int max_iters) {
219 m_maxiters = max_iters;
220 if (m_incremental_steps > m_maxiters)
221 m_incremental_steps = m_maxiters;
222 }
223
SetIncrementalSteps(int incr_steps)224 void ChStaticNonLinearAnalysis::SetIncrementalSteps(int incr_steps) {
225 m_incremental_steps = incr_steps;
226 if (m_maxiters < m_incremental_steps)
227 m_maxiters = m_incremental_steps;
228 }
229
230
231 // -----------------------------------------------------------------------------
232
ChStaticNonLinearRheonomicAnalysis(ChIntegrableIIorder & integrable)233 ChStaticNonLinearRheonomicAnalysis::ChStaticNonLinearRheonomicAnalysis(ChIntegrableIIorder& integrable)
234 : ChStaticAnalysis(integrable),
235 m_maxiters(20),
236 m_incremental_steps(6),
237 m_use_correction_test(true),
238 m_reltol(1e-4),
239 m_abstol(1e-8),
240 m_verbose(false),
241 automatic_speed_accel_computation(false) {}
242
StaticAnalysis()243 void ChStaticNonLinearRheonomicAnalysis::StaticAnalysis() {
244 ChIntegrableIIorder* integrable = static_cast<ChIntegrableIIorder*>(m_integrable);
245
246 if (m_verbose) {
247 GetLog() << "\nNonlinear static rheonomic\n";
248 GetLog() << " max iterations: " << m_maxiters << "\n";
249 GetLog() << " incremental steps: " << m_incremental_steps << "\n";
250 if (m_use_correction_test) {
251 GetLog() << " stopping test: correction\n";
252 GetLog() << " relative tol: " << m_reltol << "\n";
253 GetLog() << " absolute tol: " << m_abstol << "\n";
254 } else {
255 GetLog() << " stopping test: residual\n";
256 GetLog() << " tolerance: " << m_abstol << "\n";
257 }
258 GetLog() << "\n";
259 }
260
261 // Set up main vectors
262 double T;
263 ChStateDelta V(integrable);
264 ChStateDelta Vp(integrable);
265 ChStateDelta Vm(integrable);
266 ChStateDelta A(integrable);
267 X.resize(integrable->GetNcoords_x());
268 V.resize(integrable->GetNcoords_v());
269 Vp.resize(integrable->GetNcoords_v());
270 Vm.resize(integrable->GetNcoords_v());
271 A.resize(integrable->GetNcoords_v());
272
273
274 integrable->StateGather(X, V, T); // state <- system
275
276 // Set speed to zero
277 V.setZero(integrable->GetNcoords_v(), integrable);
278 integrable->StateScatter(X, V, T, true); // state -> system
279
280 // Set up auxiliary vectors
281 ChState Xnew;
282 ChStateDelta Dx;
283 ChVectorDynamic<> R;
284 ChVectorDynamic<> Qc;
285 ChVectorDynamic<> Dl;
286 ChVectorDynamic<> L_v;
287 Xnew.setZero(integrable->GetNcoords_x(), integrable);
288 Dx.setZero(integrable->GetNcoords_v(), integrable);
289 R.setZero(integrable->GetNcoords_v());
290 Qc.setZero(integrable->GetNconstr());
291 L.setZero(integrable->GetNconstr());
292 L_v.setZero(integrable->GetNconstr());
293 Dl.setZero(integrable->GetNconstr());
294 double dt_perturbation = 1e-5;
295
296 // Use Newton Raphson iteration
297
298 for (int i = 0; i < m_maxiters; ++i) {
299
300 integrable->StateScatter(X, V, T, true); // state -> system
301
302 // total load scaling factor
303 double cfactor = ChMin(1.0, (i + 2.0) / (m_incremental_steps + 1.0));
304
305 // Update nonzero speeds and accelerations, if any, calling
306 // the iteration callback, if any:
307 if (this->callback_iteration_begin) {
308 this->callback_iteration_begin->OnIterationBegin(cfactor, i, this); // this may modify V and A
309 integrable->StateGather(X, V, T); // state <- system
310 integrable->StateGatherAcceleration(A);
311 }
312
313 // Otherwise, if enabled, compute them automatically from rheonomic constraints:
314 // ***WARNING*** this is ok only for not too much stretched elements at the moment!
315 if (i==0 && automatic_speed_accel_computation) // btw. should happen at eeach iteration not only at first
316 {
317 // solve
318 // [ - dF/dx Cq' ] [ V ] = [ 0 ]
319 // [ Cq 0 ] [ L_v] = [-Ct ]
320
321 R.setZero(integrable->GetNcoords_v());
322 Qc.setZero(integrable->GetNconstr());
323 integrable->LoadConstraint_Ct(Qc, 1.0); // -Ct
324
325 // Solve linear system for correction
326 integrable->StateSolveCorrection( //
327 V, L_v, // unknowns
328 R, Qc, // RHS
329 0, // factor for M
330 0, // factor for dF/dv
331 -1.0, // factor for dF/dx (the stiffness matrix)
332 X, V, T, // not needed here
333 false, // do not scatter Xnew Vnew T+dt before computing correction
334 false, // full update? (not used, since no scatter)
335 true // force a call to the solver's Setup() function
336 );
337 /*
338 GetLog() << "V=\n" << V << "\n";
339 GetLog() << "Qc=\n" << Qc << "\n";
340 */
341 Xnew = X + (V * dt_perturbation); // small increment in position to recompute V and get acceleration by backward diff.
342
343 integrable->StateScatter(Xnew, V, T, true); // state -> system
344
345 // Solve linear system for correction
346 integrable->StateSolveCorrection( //
347 Vp, L_v, // unknowns
348 R, Qc, // RHS
349 0, // factor for M
350 0, // factor for dF/dv
351 -1.0, // factor for dF/dx (the stiffness matrix)
352 X, V, T, // not needed here
353 false, // do not scatter Xnew Vnew T+dt before computing correction
354 false, // full update? (not used, since no scatter)
355 true // force a call to the solver's Setup() function
356 );
357
358 Xnew = X - (V * dt_perturbation); // small increment in position to recompute V and get acceleration by backward diff.
359
360 integrable->StateScatter(Xnew, V, T, true); // state -> system
361
362 // Solve linear system for correction
363 integrable->StateSolveCorrection( //
364 Vm, L_v, // unknowns
365 R, Qc, // RHS
366 0, // factor for M
367 0, // factor for dF/dv
368 -1.0, // factor for dF/dx (the stiffness matrix)
369 X, V, T, // not needed here
370 false, // do not scatter Xnew Vnew T+dt before computing correction
371 false, // full update? (not used, since no scatter)
372 true // force a call to the solver's Setup() function
373 );
374
375 A = (Vp - Vm) / (2*dt_perturbation);
376
377 integrable->StateScatter(X, V, T, true); // state -> system
378 integrable->StateScatterAcceleration(A);
379 }
380
381
382
383
384 // B) solve for position increments, where in RHS includes all inertial forces:
385 // f automatically includes -centrifugal/gyroscopic terms at given acceleration/speed, and -M*a is added for completing inertial forces
386 //
387 // [ - dF/dx Cq' ] [ Dx ] = [ f - M*a + Cq*L]
388 // [ Cq 0 ] [ Dl ] = [ -C ]
389
390 R.setZero(integrable->GetNcoords_v());
391 Qc.setZero(integrable->GetNconstr());
392 integrable->LoadResidual_F(R, 1.0);
393 integrable->LoadResidual_CqL(R, L, 1.0);
394 integrable->LoadResidual_Mv(R, A, -1.0);
395 integrable->LoadConstraint_C(Qc, 1.0); // -C
396
397 R *= cfactor;
398 Qc *= cfactor;
399
400 if (!m_use_correction_test) {
401 // Evaluate residual norms
402 double R_norm = R.lpNorm<Eigen::Infinity>();
403 double Qc_norm = Qc.lpNorm<Eigen::Infinity>();
404
405 if (m_verbose) {
406 GetLog() << "--- Nonlinear statics iteration " << i << " |R|_inf = " << R_norm
407 << " |Qc|_inf = " << Qc_norm << "\n";
408 }
409
410 // Stopping test
411 if ((R_norm < m_abstol) && (Qc_norm < m_abstol)) {
412 if (m_verbose) {
413 GetLog() << "+++ Newton procedure converged in " << i + 1 << " iterations.\n\n";
414 }
415 break;
416 }
417 }
418
419 // Solve linear system for correction
420 integrable->StateSolveCorrection( //
421 Dx, Dl, R, Qc, //
422 0, // factor for M
423 0, // factor for dF/dv
424 -1.0, // factor for dF/dx (the stiffness matrix)
425 X, V, T, // not needed here
426 false, // do not scatter Xnew Vnew T+dt before computing correction
427 false, // full update? (not used, since no scatter)
428 true // force a call to the solver's Setup() function
429 );
430
431 Xnew = X + Dx;
432 L += Dl;
433
434 /*
435 GetLog() << "\n\n\ Iteration " << i << "\n\n";
436 GetLog() << "R=" << R << "\n";
437 GetLog() << "Qc=" << Qc << "\n";
438 GetLog() << "Dx=" << Dx << "\n";
439 */
440
441 if (m_use_correction_test) {
442 // Calculate actual correction in X
443 ChState correction = Xnew - X;
444
445 // Evaluate weights and correction WRMS norm
446 ChVectorDynamic<> ewt = (m_reltol * Xnew.cwiseAbs() + m_abstol).cwiseInverse();
447 double Dx_norm = correction.wrmsNorm(ewt);
448
449 if (m_verbose) {
450 GetLog() << "--- Nonlinear statics iteration " << i << " |Dx|_wrms = " << Dx_norm << "\n";
451 }
452
453 // Stopping test
454 if (Dx_norm < 1 && i>3) {
455 if (m_verbose) {
456 double R_norm = R.lpNorm<Eigen::Infinity>();
457 double Qc_norm = Qc.lpNorm<Eigen::Infinity>();
458 GetLog() << "+++ Newton procedure converged in " << i + 1 << " iterations.\n";
459 GetLog() << " |R|_inf = " << R_norm << " |Qc|_inf = " << Qc_norm << "\n\n";
460 }
461 X = Xnew;
462 break;
463 }
464 }
465
466 X = Xnew;
467 }
468
469 // Compute speed at the end. ***WARNING*** this is ok only for sligtly stretched elements at the moment!
470
471 if (automatic_speed_accel_computation) {
472
473 for (int i = 0; i < m_maxiters; ++i) {
474
475 integrable->StateScatter(X, V, T, true); // state -> system
476
477 // B) solve for position increments, where in RHS includes all inertial forces:
478 // f automatically includes -centrifugal/gyroscopic terms at given acceleration/speed, and -M*a is added for completing inertial forces
479 //
480 // [ - dF/dx Cq' ] [ Dx ] = [ f - M*a + Cq*L]
481 // [ Cq 0 ] [ Dl ] = [ -C ]
482
483 R.setZero(integrable->GetNcoords_v());
484 Qc.setZero(integrable->GetNconstr());
485 integrable->LoadResidual_F(R, 1.0);
486 integrable->LoadResidual_CqL(R, L, 1.0);
487 integrable->LoadResidual_Mv(R, A, -1.0);
488 //integrable->LoadConstraint_C(Qc, 1.0); // -C
489 integrable->LoadConstraint_Ct(Qc, 1.0); // -Ct
490
491 double cfactor = ChMin(1.0, (i + 2.0) / (m_incremental_steps + 1.0));
492 R *= cfactor;
493 Qc *= cfactor;
494
495 // Solve linear system for correction
496 integrable->StateSolveCorrection( //
497 V, Dl, R, Qc, //
498 0, // factor for M
499 0, // factor for dF/dv
500 -1.0, // factor for dF/dx (the stiffness matrix)
501 X, V, T, // not needed here
502 false, // do not scatter Xnew Vnew T+dt before computing correction
503 false, // full update? (not used, since no scatter)
504 true // force a call to the solver's Setup() function
505 );
506
507 //Xnew = X + Dx;
508 L += Dl;
509
510 //X = Xnew;
511 }
512
513
514
515
516
517
518 V.setZero(integrable->GetNcoords_v(), integrable);
519 R.setZero(integrable->GetNcoords_v());
520 Qc.setZero(integrable->GetNconstr());
521 L_v.setZero(integrable->GetNconstr());
522 integrable->LoadConstraint_Ct(Qc, 1.0); // -Ct
523
524 // Solve linear system for correction
525 integrable->StateSolveCorrection( //
526 V, L_v, // unknowns
527 R, Qc, // RHS
528 0, // factor for M
529 0, // factor for dF/dv
530 -1.0, // factor for dF/dx (the stiffness matrix)
531 X, V, T, // not needed here
532 false, // do not scatter Xnew Vnew T+dt before computing correction
533 false, // full update? (not used, since no scatter)
534 true // force a call to the solver's Setup() function
535 );
536
537 /*
538 GetLog() << "\n\n\ Last step " << "\n\n";
539 GetLog() << "R=" << R << "\n";
540 GetLog() << "Qc=" << Qc << "\n";
541 GetLog() << "V_last=\n" << V << "\n";
542 */
543
544 integrable->StateScatter(X, V, T, true); // state -> system
545 integrable->StateScatterAcceleration(A);
546 }
547
548 integrable->StateScatter(X, V, T, true); // state -> system
549 integrable->StateScatterReactions(L); // -> system auxiliary data
550 }
551
SetCorrectionTolerance(double reltol,double abstol)552 void ChStaticNonLinearRheonomicAnalysis::SetCorrectionTolerance(double reltol, double abstol) {
553 m_use_correction_test = true;
554 m_reltol = reltol;
555 m_abstol = abstol;
556 }
557
SetResidualTolerance(double tol)558 void ChStaticNonLinearRheonomicAnalysis::SetResidualTolerance(double tol) {
559 m_use_correction_test = false;
560 m_abstol = tol;
561 }
562
SetMaxIterations(int max_iters)563 void ChStaticNonLinearRheonomicAnalysis::SetMaxIterations(int max_iters) {
564 m_maxiters = max_iters;
565 if (m_incremental_steps > m_maxiters)
566 m_incremental_steps = m_maxiters;
567 }
568
SetIncrementalSteps(int incr_steps)569 void ChStaticNonLinearRheonomicAnalysis::SetIncrementalSteps(int incr_steps) {
570 m_incremental_steps = incr_steps;
571 if (m_maxiters < m_incremental_steps)
572 m_maxiters = m_incremental_steps;
573 }
574
575
576
577
578 } // end namespace chrono
579