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 // Authors: Alessandro Tasora, Radu Serban
13 // =============================================================================
14
15 #include <cmath>
16
17 #include "chrono/timestepper/ChTimestepperHHT.h"
18
19 namespace chrono {
20
21 // Register into the object factory, to enable run-time dynamic creation and persistence
CH_FACTORY_REGISTER(ChTimestepperHHT)22 CH_FACTORY_REGISTER(ChTimestepperHHT)
23
24 ChTimestepperHHT::ChTimestepperHHT(ChIntegrableIIorder* intgr)
25 : ChTimestepperIIorder(intgr),
26 ChImplicitIterativeTimestepper(),
27 mode(ACCELERATION),
28 scaling(false),
29 step_control(true),
30 maxiters_success(3),
31 req_successful_steps(5),
32 step_increase_factor(2),
33 step_decrease_factor(0.5),
34 h_min(1e-10),
35 h(1e6),
36 num_successful_steps(0),
37 modified_Newton(true) {
38 SetAlpha(-0.2); // default: some dissipation
39 }
40
SetAlpha(double malpha)41 void ChTimestepperHHT::SetAlpha(double malpha) {
42 alpha = malpha;
43 if (alpha < -1.0 / 3.0)
44 alpha = -1.0 / 3.0;
45 if (alpha > 0)
46 alpha = 0;
47 gamma = (1.0 - 2.0 * alpha) / 2.0;
48 beta = pow((1.0 - alpha), 2) / 4.0;
49 }
50
51 // Performs a step of HHT (generalized alpha) implicit for II order systems
Advance(const double dt)52 void ChTimestepperHHT::Advance(const double dt) {
53 // Downcast
54 ChIntegrableIIorder* mintegrable = (ChIntegrableIIorder*)this->integrable;
55
56 // Setup main vectors
57 mintegrable->StateSetup(X, V, A);
58
59 // Setup auxiliary vectors
60 Da.setZero(mintegrable->GetNcoords_a(), mintegrable);
61 if (mode == POSITION) {
62 Xprev.setZero(mintegrable->GetNcoords_x(), mintegrable);
63 Dx.setZero(mintegrable->GetNcoords_v(), mintegrable);
64 }
65 Dl.setZero(mintegrable->GetNconstr());
66 Xnew.setZero(mintegrable->GetNcoords_x(), mintegrable);
67 Vnew.setZero(mintegrable->GetNcoords_v(), mintegrable);
68 Anew.setZero(mintegrable->GetNcoords_a(), mintegrable);
69 R.setZero(mintegrable->GetNcoords_v());
70 Rold.setZero(mintegrable->GetNcoords_v());
71 Qc.setZero(mintegrable->GetNconstr());
72 L.setZero(mintegrable->GetNconstr());
73 Lnew.setZero(mintegrable->GetNconstr());
74
75 // State at current time T
76 mintegrable->StateGather(X, V, T); // state <- system
77 mintegrable->StateGatherAcceleration(A); // <- system
78 mintegrable->StateGatherReactions(L); // <- system
79
80 // Advance solution to time T+dt, possibly taking multiple steps
81 double tfinal = T + dt; // target final time
82 numiters = 0; // total number of NR iterations for this step
83 numsetups = 0;
84 numsolves = 0;
85
86 // If we had a streak of successful steps, consider a stepsize increase.
87 // Note that we never attempt a step larger than the specified dt value.
88 // If step size control is disabled, always use h = dt.
89 if (!step_control) {
90 h = dt;
91 num_successful_steps = 0;
92 } else if (num_successful_steps >= req_successful_steps) {
93 double new_h = ChMin(h * step_increase_factor, dt);
94 if (new_h > h + h_min) {
95 h = new_h;
96 num_successful_steps = 0;
97 if (verbose)
98 GetLog() << " +++HHT increase stepsize to " << h << "\n";
99 }
100 } else {
101 h = ChMin(h, dt);
102 }
103
104 // Monitor flags controlling whther or not the Newton matrix must be updated.
105 // If using modified Newton, a matrix update occurs:
106 // - at the beginning of a step
107 // - on a stepsize decrease
108 // - if the Newton iteration does not converge with an out-of-date matrix
109 // Otherwise, the matrix is updated at each iteration.
110 matrix_is_current = false;
111 call_setup = true;
112
113 // Loop until reaching final time
114 while (true) {
115 double scaling_factor = scaling ? beta * h * h : 1;
116 Prepare(mintegrable, scaling_factor);
117
118 // Newton-Raphson for state at T+h
119 bool converged = false;
120 int it;
121
122 for (it = 0; it < maxiters; it++) {
123 if (verbose && modified_Newton && call_setup)
124 GetLog() << " HHT call Setup.\n";
125
126 // Solve linear system and increment state
127 Increment(mintegrable, scaling_factor);
128
129 // Increment counters
130 numiters++;
131 numsolves++;
132 if (call_setup) {
133 numsetups++;
134 }
135
136 // If using modified Newton, do not call Setup again
137 call_setup = !modified_Newton;
138
139 // A flag to indicate the trend of convergence
140 if ((Rold.norm() < R.norm()) && (R.norm() > threshold_R)) {
141 convergence_trend_flag = false; // very dangerous, seems to diverge
142 break;
143 } else {
144 convergence_trend_flag = true; // normal, seems to converge
145 }
146
147 // Check convergence
148 converged = CheckConvergence(scaling_factor);
149 if (converged)
150 break;
151 }
152
153 if (!converged) { // ------ NR did not converge
154 convergence_trend_flag = false;
155 }
156
157
158 if (converged) {
159 // ------ NR converged
160
161 // if the number of iterations was low enough, increase the count of successive
162 // successful steps (for possible step increase)
163 if (it < maxiters_success)
164 num_successful_steps++;
165 else
166 num_successful_steps = 0;
167
168 if (verbose) {
169 GetLog() << " HHT NR converged (" << num_successful_steps << ").";
170 GetLog() << " T = " << T + h << " h = " << h << "\n";
171 }
172
173 // Advance time (clamp to tfinal if close enough)
174 T += h;
175 if (std::abs(T - tfinal) < std::min(h_min, 1e-6)) {
176 T = tfinal;
177 }
178
179 // Set the state
180 X = Xnew;
181 V = Vnew;
182 A = Anew;
183 L = Lnew;
184
185 /*
186 } else if (!matrix_is_current) {
187 // ------ NR did not converge but the matrix was out-of-date
188
189 // reset the count of successive successful steps
190 num_successful_steps = 0;
191
192 // re-attempt step with updated matrix
193 if (verbose) {
194 GetLog() << " HHT re-attempt step with updated matrix.\n";
195 }
196
197 call_setup = true;
198 */
199
200 } else if (!step_control) {
201 // ------ NR did not converge and we do not control stepsize
202
203 // reset the count of successive successful steps
204 num_successful_steps = 0;
205
206 // accept solution as is and complete step
207 if (verbose) {
208 GetLog() << " HHT NR terminated.";
209 GetLog() << " T = " << T + h << " h = " << h << "\n";
210 }
211
212 T += h;
213 X = Xnew;
214 V = Vnew;
215 A = Anew;
216 L = Lnew;
217
218 } else {
219 // ------ NR did not converge
220
221 // reset the count of successive successful steps
222 num_successful_steps = 0;
223
224 // decrease stepsize
225 h *= step_decrease_factor;
226
227 if (verbose)
228 GetLog() << " ---HHT reduce stepsize to " << h << "\n";
229
230 // bail out if stepsize reaches minimum allowable
231 if (h < h_min) {
232 if (verbose)
233 GetLog() << " HHT at minimum stepsize. Exiting...\n";
234 throw ChException("HHT: Reached minimum allowable step size.");
235 }
236
237 // force a matrix re-evaluation (due to change in stepsize)
238 call_setup = true;
239 }
240
241 if (T >= tfinal) {
242 break;
243 }
244
245 // Go back in the loop: scatter state and reset temporary vector
246 // Scatter state -> system
247 mintegrable->StateScatter(X, V, T, false);
248 Rold.setZero();
249 Anew.setZero(mintegrable->GetNcoords_a(), mintegrable);
250 }
251
252 // Scatter state -> system doing a full update
253 mintegrable->StateScatter(X, V, T, true);
254
255 // Scatter auxiliary data (A and L) -> system
256 mintegrable->StateScatterAcceleration(A);
257 mintegrable->StateScatterReactions(L);
258 }
259
260 // Prepare attempting a step of size h (assuming a converged state at the current time t):
261 // - Initialize residual vector with terms at current time
262 // - Obtain a prediction at T+h for NR using extrapolation from solution at current time.
263 // - For ACCELERATION mode, if not using step size control, start with zero acceleration
264 // guess (previous step not guaranteed to have converged)
265 // - Set the error weight vectors (using solution at current time)
Prepare(ChIntegrableIIorder * integrable,double scaling_factor)266 void ChTimestepperHHT::Prepare(ChIntegrableIIorder* integrable, double scaling_factor) {
267 switch (mode) {
268 case ACCELERATION:
269 if (step_control)
270 Anew = A;
271 Vnew = V + Anew * h;
272 Xnew = X + Vnew * h + Anew * (h * h);
273 integrable->LoadResidual_F(Rold, -alpha / (1.0 + alpha)); // -alpha/(1.0+alpha) * f_old
274 integrable->LoadResidual_CqL(Rold, L, -alpha / (1.0 + alpha)); // -alpha/(1.0+alpha) * Cq'*l_old
275 CalcErrorWeights(A, reltol, abstolS, ewtS);
276 break;
277 case POSITION:
278 Xnew = X;
279 Xprev = X;
280 Vnew = V * (-(gamma / beta - 1.0)) - A * (h * (gamma / (2.0 * beta) - 1.0));
281 Anew = V * (-1.0 / (beta * h)) - A * (1.0 / (2.0 * beta) - 1.0);
282 integrable->LoadResidual_F(Rold, -(alpha / (1.0 + alpha)) * scaling_factor); // -alpha/(1.0+alpha) * f_old
283 integrable->LoadResidual_CqL(Rold, L,
284 -(alpha / (1.0 + alpha)) * scaling_factor); // -alpha/(1.0+alpha) * Cq'*l_old
285 CalcErrorWeights(X, reltol, abstolS, ewtS);
286 break;
287 }
288
289 Lnew = L;
290
291 CalcErrorWeights(L, reltol, abstolL, ewtL);
292 }
293
294 // Calculate a new iterate of the new state at time T+h:
295 // - Scatter the current estimate of the new state (the state at time T+h)
296 // - Set up and solve linear system
297 // - Calculate solution increment
298 // - Update the estimate of the new state (the state at time T+h)
299 //
300 // This is one iteration of Newton-Raphson to solve for a_new
301 //
302 // [ M - dt*gamma*dF/dv - dt^2*beta*dF/dx Cq' ] [ Da ] =
303 // [ Cq 0 ] [ Dl ]
304 // [ -1/(1+alpha)*M*(a_new) + (f_new +Cq*l_new) - (alpha/(1+alpha))(f_old +Cq*l_old)]
305 // [ 1/(beta*dt^2)*C ]
306 //
Increment(ChIntegrableIIorder * integrable,double scaling_factor)307 void ChTimestepperHHT::Increment(ChIntegrableIIorder* integrable, double scaling_factor) {
308 // Scatter the current estimate of state at time T+h
309 integrable->StateScatter(Xnew, Vnew, T + h, false);
310
311 // Initialize the two segments of the RHS
312 R = Rold; // terms related to state at time T
313 Qc.setZero(); // zero
314
315 switch (mode) {
316 case ACCELERATION:
317 // Set up linear system
318 integrable->LoadResidual_F(R, 1.0); // f_new
319 integrable->LoadResidual_CqL(R, Lnew, 1.0); // Cq'*l_new
320 integrable->LoadResidual_Mv(R, Anew, -1 / (1 + alpha)); // -1/(1+alpha)*M*a_new
321 integrable->LoadConstraint_C(Qc, 1 / (beta * h * h), Qc_do_clamp, Qc_clamping); // 1/(beta*dt^2)*C
322
323 // Solve linear system
324 integrable->StateSolveCorrection(Da, Dl, R, Qc,
325 1 / (1 + alpha), // factor for M (was 1 in Negrut paper ?!)
326 -h * gamma, // factor for dF/dv
327 -h * h * beta, // factor for dF/dx
328 Xnew, Vnew, T + h, // not used here (force_scatter = false)
329 false, // do not scatter states
330 false, // full update? (not used, since no scatter)
331 call_setup // call Setup?
332 );
333
334 // Update estimate of state at t+h
335 Lnew += Dl; // not -= Dl because we assume StateSolveCorrection flips sign of Dl
336 Anew += Da;
337 Xnew = X + V * h + A * (h * h * (0.5 - beta)) + Anew * (h * h * beta);
338 Vnew = V + A * (h * (1.0 - gamma)) + Anew * (h * gamma);
339
340 break;
341
342 case POSITION:
343 // Set up linear system
344 integrable->LoadResidual_F(R, scaling_factor); // f_new
345 integrable->LoadResidual_CqL(R, Lnew, scaling_factor); // Cq'*l_new
346 integrable->LoadResidual_Mv(R, Anew, -1 / (1 + alpha) * scaling_factor); // -1/(1+alpha)*M*a_new
347 integrable->LoadConstraint_C(Qc, 1.0, Qc_do_clamp, Qc_clamping); // 1/(beta*dt^2)*C
348
349 // Solve linear system
350 integrable->StateSolveCorrection(Da, Dl, R, Qc,
351 scaling_factor / ((1 + alpha) * beta * h * h), // factor for M
352 -scaling_factor * gamma / (beta * h), // factor for dF/dv
353 -scaling_factor, // factor for dF/dx
354 Xnew, Vnew, T + h, // not used here(force_scatter = false)
355 false, // do not scatter states
356 false, // full update? (not used, since no scatter)
357 call_setup // call Setup?
358 );
359
360 // Update estimate of state at t+h
361 Lnew += Dl * (1.0 / scaling_factor); // not -= Dl because we assume StateSolveCorrection flips sign of Dl
362 Dx += Da;
363 Xnew = (X + Dx);
364 Vnew = V * (-(gamma / beta - 1.0)) - A * (h * (gamma / (2.0 * beta) - 1.0));
365 Vnew += Dx * (gamma / (beta * h));
366 Anew = -V * (1.0 / (beta * h)) - A * (1.0 / (2.0 * beta) - 1.0);
367 Anew += Dx * (1.0 / (beta * h * h));
368
369 break;
370 }
371
372 // If Setup was called at this iteration, mark the Newton matrix as up-to-date
373 matrix_is_current = call_setup;
374 }
375
376 // Convergence test
CheckConvergence(double scaling_factor)377 bool ChTimestepperHHT::CheckConvergence(double scaling_factor) {
378 bool converged = false;
379
380 switch (mode) {
381 case ACCELERATION: {
382 // Declare convergence when either the residual is below the absolute tolerance or
383 // the WRMS update norm is less than 1 (relative + absolute tolerance test)
384 // |R|_2 < atol
385 // or |D|_WRMS < 1
386 // Both states and Lagrange multipliers must converge.
387 double R_nrm = R.norm();
388 double Qc_nrm = Qc.norm();
389 double Da_nrm = Da.wrmsNorm(ewtS);
390 double Dl_nrm = Dl.wrmsNorm(ewtL);
391
392 if (verbose) {
393 GetLog() << " HHT iteration=" << numiters << " |R|=" << R_nrm << " |Qc|=" << Qc_nrm
394 << " |Da|=" << Da_nrm << " |Dl|=" << Dl_nrm << " N = " << (int)R.size()
395 << " M = " << (int)Qc.size() << "\n";
396 }
397
398 if ((R_nrm < abstolS && Qc_nrm < abstolL) || (Da_nrm < 1 && Dl_nrm < 1))
399 converged = true;
400
401 break;
402 }
403 case POSITION: {
404 // Declare convergence when the WRMS norm of the update is less than 1
405 // (relative + absolute tolerance test).
406 // Note that the scaling factor must be properly included in the update to
407 // the Lagrange multipliers.
408 double Dx_nrm = (Xnew - Xprev).wrmsNorm(ewtS);
409 Xprev = Xnew;
410
411 double Dl_nrm = Dl.wrmsNorm(ewtL);
412 Dl_nrm /= scaling_factor;
413
414 if (verbose) {
415 GetLog() << " HHT iteration=" << numiters << " |Dx|=" << Dx_nrm << " |Dl|=" << Dl_nrm << "\n";
416 }
417
418 if (Dx_nrm < 1 && Dl_nrm < 1)
419 converged = true;
420
421 break;
422 }
423 }
424
425 return converged;
426 }
427
428 // Calculate the error weight vector corresponding to the specified solution vector x,
429 // using the given relative and absolute tolerances.
CalcErrorWeights(const ChVectorDynamic<> & x,double rtol,double atol,ChVectorDynamic<> & ewt)430 void ChTimestepperHHT::CalcErrorWeights(const ChVectorDynamic<>& x, double rtol, double atol, ChVectorDynamic<>& ewt) {
431 ewt = (rtol * x.cwiseAbs() + atol).cwiseInverse();
432 }
433
434 // Trick to avoid putting the following mapper macro inside the class definition in .h file:
435 // enclose macros in local 'my_enum_mappers', just to avoid avoiding cluttering of the parent class.
436 class my_enum_mappers : public ChTimestepperHHT {
437 public:
438 CH_ENUM_MAPPER_BEGIN(HHT_Mode);
439 CH_ENUM_VAL(ACCELERATION);
440 CH_ENUM_VAL(POSITION);
441 CH_ENUM_MAPPER_END(HHT_Mode);
442 };
443
ArchiveOUT(ChArchiveOut & archive)444 void ChTimestepperHHT::ArchiveOUT(ChArchiveOut& archive) {
445 // version number
446 archive.VersionWrite<ChTimestepperHHT>();
447 // serialize parent class:
448 ChTimestepperIIorder::ArchiveOUT(archive);
449 ChImplicitIterativeTimestepper::ArchiveOUT(archive);
450 // serialize all member data:
451 archive << CHNVP(alpha);
452 archive << CHNVP(beta);
453 archive << CHNVP(gamma);
454 archive << CHNVP(scaling);
455 my_enum_mappers::HHT_Mode_mapper modemapper;
456 archive << CHNVP(modemapper(mode), "mode");
457 }
458
ArchiveIN(ChArchiveIn & archive)459 void ChTimestepperHHT::ArchiveIN(ChArchiveIn& archive) {
460 // version number
461 /*int version =*/ archive.VersionRead<ChTimestepperHHT>();
462 // deserialize parent class:
463 ChTimestepperIIorder::ArchiveIN(archive);
464 ChImplicitIterativeTimestepper::ArchiveIN(archive);
465 // stream in all member data:
466 archive >> CHNVP(alpha);
467 archive >> CHNVP(beta);
468 archive >> CHNVP(gamma);
469 archive >> CHNVP(scaling);
470 my_enum_mappers::HHT_Mode_mapper modemapper;
471 archive >> CHNVP(modemapper(mode), "mode");
472 }
473
474 } // end namespace chrono