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 <algorithm>
16
17 #include "chrono/collision/ChCollisionSystemBullet.h"
18 #ifdef CHRONO_COLLISION
19 #include "chrono/collision/ChCollisionSystemChrono.h"
20 #endif
21 #include "chrono/physics/ChProximityContainer.h"
22 #include "chrono/physics/ChSystem.h"
23 #include "chrono/solver/ChSolverAPGD.h"
24 #include "chrono/solver/ChSolverBB.h"
25 #include "chrono/solver/ChSolverPJacobi.h"
26 #include "chrono/solver/ChSolverPMINRES.h"
27 #include "chrono/solver/ChSolverPSOR.h"
28 #include "chrono/solver/ChSolverPSSOR.h"
29 #include "chrono/solver/ChIterativeSolverLS.h"
30 #include "chrono/core/ChMatrix.h"
31 #include "chrono/utils/ChProfiler.h"
32
33 using namespace chrono::collision;
34
35 namespace chrono {
36
37 // -----------------------------------------------------------------------------
38 // CLASS FOR PHYSICAL SYSTEM
39 // -----------------------------------------------------------------------------
40
ChSystem()41 ChSystem::ChSystem()
42 : G_acc(ChVector<>(0, -9.8, 0)),
43 is_initialized(false),
44 is_updated(false),
45 ncoords(0),
46 ndoc(0),
47 nsysvars(0),
48 ncoords_w(0),
49 ndoc_w(0),
50 nsysvars_w(0),
51 ndof(0),
52 ndoc_w_C(0),
53 ndoc_w_D(0),
54 ch_time(0),
55 step(0.04),
56 step_min(0.002),
57 step_max(0.04),
58 tol_force(-1),
59 maxiter(6),
60 use_sleeping(false),
61 min_bounce_speed(0.15),
62 max_penetration_recovery_speed(0.6),
63 stepcount(0),
64 setupcount(0),
65 solvecount(0),
66 dump_matrices(false),
67 ncontacts(0),
68 composition_strategy(new ChMaterialCompositionStrategy),
69 nthreads_chrono(ChOMP::GetNumProcs()),
70 nthreads_eigen(1),
71 nthreads_collision(1),
72 last_err(false),
73 applied_forces_current(false) {
74 assembly.system = this;
75
76 // Set default collision engine type, collision envelope, and margin.
77 collision_system_type = collision::ChCollisionSystemType::BULLET;
78 collision::ChCollisionModel::SetDefaultSuggestedEnvelope(0.03);
79 collision::ChCollisionModel::SetDefaultSuggestedMargin(0.01);
80
81 // Set default timestepper.
82 timestepper = chrono_types::make_shared<ChTimestepperEulerImplicitLinearized>(this);
83 }
84
ChSystem(const ChSystem & other)85 ChSystem::ChSystem(const ChSystem& other) {
86 // Required by ChAssembly
87 assembly = other.assembly;
88 assembly.system = this;
89
90 G_acc = other.G_acc;
91 ncoords = other.ncoords;
92 ncoords_w = other.ncoords_w;
93 ndoc = other.ndoc;
94 ndoc_w = other.ndoc_w;
95 ndoc_w_C = other.ndoc_w_C;
96 ndoc_w_D = other.ndoc_w_D;
97 ndof = other.ndof;
98 nsysvars = other.nsysvars;
99 nsysvars_w = other.nsysvars_w;
100 ch_time = other.ch_time;
101 step = other.step;
102 step_min = other.step_min;
103 step_max = other.step_max;
104 stepcount = other.stepcount;
105 solvecount = other.solvecount;
106 setupcount = other.setupcount;
107 dump_matrices = other.dump_matrices;
108 SetTimestepperType(other.GetTimestepperType());
109 tol_force = other.tol_force;
110 nthreads_chrono = other.nthreads_chrono;
111 nthreads_eigen = other.nthreads_eigen;
112 nthreads_collision = other.nthreads_collision;
113 is_initialized = false;
114 is_updated = false;
115 applied_forces_current = false;
116 maxiter = other.maxiter;
117
118 collision_system_type = other.collision_system_type;
119
120 min_bounce_speed = other.min_bounce_speed;
121 max_penetration_recovery_speed = other.max_penetration_recovery_speed;
122 SetSolverType(other.GetSolverType());
123 use_sleeping = other.use_sleeping;
124
125 ncontacts = other.ncontacts;
126
127 collision_callbacks = other.collision_callbacks;
128
129 last_err = other.last_err;
130 }
131
~ChSystem()132 ChSystem::~ChSystem() {
133 // Before proceeding, anticipate Clear(). This would be called also by base ChAssembly destructor, anyway, but
134 // it would happen after this destructor, so the ith_body->SetSystem(0) in Clear() would not be able to remove
135 // body collision models from the collision_system. Here it is possible, since the collision_system is still alive.
136 Clear();
137 }
138
Clear()139 void ChSystem::Clear() {
140 assembly.Clear();
141
142 // contact_container->RemoveAllContacts();
143
144 // ResetTimers();
145 }
146
147 // -----------------------------------------------------------------------------
148
AddBody(std::shared_ptr<ChBody> body)149 void ChSystem::AddBody(std::shared_ptr<ChBody> body) {
150 assert(body->GetCollisionModel()->GetType() == collision_system->GetType());
151 body->SetId(static_cast<int>(Get_bodylist().size()));
152 assembly.AddBody(body);
153 }
154
AddLink(std::shared_ptr<ChLinkBase> link)155 void ChSystem::AddLink(std::shared_ptr<ChLinkBase> link) {
156 assembly.AddLink(link);
157 }
158
AddMesh(std::shared_ptr<fea::ChMesh> mesh)159 void ChSystem::AddMesh(std::shared_ptr<fea::ChMesh> mesh) {
160 assembly.AddMesh(mesh);
161 }
162
AddOtherPhysicsItem(std::shared_ptr<ChPhysicsItem> item)163 void ChSystem::AddOtherPhysicsItem(std::shared_ptr<ChPhysicsItem> item) {
164 assembly.AddOtherPhysicsItem(item);
165 }
166
167 // Add arbitrary physics item to the underlying assembly.
168 // NOTE: we cannot simply invoke ChAssembly::Add as this would not provide
169 // polymorphism!
Add(std::shared_ptr<ChPhysicsItem> item)170 void ChSystem::Add(std::shared_ptr<ChPhysicsItem> item) {
171 if (auto body = std::dynamic_pointer_cast<ChBody>(item)) {
172 AddBody(body);
173 return;
174 }
175
176 if (auto link = std::dynamic_pointer_cast<ChLinkBase>(item)) {
177 AddLink(link);
178 return;
179 }
180
181 if (auto mesh = std::dynamic_pointer_cast<fea::ChMesh>(item)) {
182 AddMesh(mesh);
183 return;
184 }
185
186 AddOtherPhysicsItem(item);
187 }
188
Remove(std::shared_ptr<ChPhysicsItem> item)189 void ChSystem::Remove(std::shared_ptr<ChPhysicsItem> item) {
190 if (auto body = std::dynamic_pointer_cast<ChBody>(item)) {
191 RemoveBody(body);
192 return;
193 }
194
195 if (auto link = std::dynamic_pointer_cast<ChLinkBase>(item)) {
196 RemoveLink(link);
197 return;
198 }
199
200 if (auto mesh = std::dynamic_pointer_cast<fea::ChMesh>(item)) {
201 RemoveMesh(mesh);
202 return;
203 }
204
205 RemoveOtherPhysicsItem(item);
206 }
207
208 // -----------------------------------------------------------------------------
209 // Set/Get routines
210 // -----------------------------------------------------------------------------
211
SetSolverMaxIterations(int max_iters)212 void ChSystem::SetSolverMaxIterations(int max_iters) {
213 if (auto iter_solver = std::dynamic_pointer_cast<ChIterativeSolver>(solver)) {
214 iter_solver->SetMaxIterations(max_iters);
215 }
216 }
217
GetSolverMaxIterations() const218 int ChSystem::GetSolverMaxIterations() const {
219 if (auto iter_solver = std::dynamic_pointer_cast<ChIterativeSolver>(solver)) {
220 return iter_solver->GetMaxIterations();
221 }
222 return 0;
223 }
224
SetSolverTolerance(double tolerance)225 void ChSystem::SetSolverTolerance(double tolerance) {
226 if (auto iter_solver = std::dynamic_pointer_cast<ChIterativeSolver>(solver)) {
227 iter_solver->SetTolerance(tolerance);
228 }
229 }
230
GetSolverTolerance() const231 double ChSystem::GetSolverTolerance() const {
232 if (auto iter_solver = std::dynamic_pointer_cast<ChIterativeSolver>(solver)) {
233 return iter_solver->GetTolerance();
234 }
235 return 0;
236 }
237
SetSolverType(ChSolver::Type type)238 void ChSystem::SetSolverType(ChSolver::Type type) {
239 // Do nothing if changing to a CUSTOM solver.
240 if (type == ChSolver::Type::CUSTOM)
241 return;
242
243 descriptor = chrono_types::make_shared<ChSystemDescriptor>();
244
245 switch (type) {
246 case ChSolver::Type::PSOR:
247 solver = chrono_types::make_shared<ChSolverPSOR>();
248 break;
249 case ChSolver::Type::PSSOR:
250 solver = chrono_types::make_shared<ChSolverPSSOR>();
251 break;
252 case ChSolver::Type::PJACOBI:
253 solver = chrono_types::make_shared<ChSolverPJacobi>();
254 break;
255 case ChSolver::Type::PMINRES:
256 solver = chrono_types::make_shared<ChSolverPMINRES>();
257 break;
258 case ChSolver::Type::BARZILAIBORWEIN:
259 solver = chrono_types::make_shared<ChSolverBB>();
260 break;
261 case ChSolver::Type::APGD:
262 solver = chrono_types::make_shared<ChSolverAPGD>();
263 break;
264 case ChSolver::Type::GMRES:
265 solver = chrono_types::make_shared<ChSolverGMRES>();
266 break;
267 case ChSolver::Type::MINRES:
268 solver = chrono_types::make_shared<ChSolverMINRES>();
269 break;
270 default:
271 GetLog() << "Solver type not supported. Use SetSolver instead.\n";
272 break;
273 }
274 }
275
GetSolver()276 std::shared_ptr<ChSolver> ChSystem::GetSolver() {
277 // In case the solver is iterative, and if the user specified a force-level tolerance,
278 // overwrite the solver's tolerance threshold.
279 if (auto iter_solver = std::dynamic_pointer_cast<ChIterativeSolver>(solver)) {
280 if (tol_force > 0) {
281 iter_solver->SetTolerance(tol_force * step);
282 }
283 }
284
285 return solver;
286 }
287
288 // -----------------------------------------------------------------------------
289
RegisterCustomCollisionCallback(std::shared_ptr<CustomCollisionCallback> callback)290 void ChSystem::RegisterCustomCollisionCallback(std::shared_ptr<CustomCollisionCallback> callback) {
291 collision_callbacks.push_back(callback);
292 }
293
UnregisterCustomCollisionCallback(std::shared_ptr<CustomCollisionCallback> callback)294 void ChSystem::UnregisterCustomCollisionCallback(std::shared_ptr<CustomCollisionCallback> callback) {
295 auto itr = std::find(std::begin(collision_callbacks), std::end(collision_callbacks), callback);
296 if (itr != collision_callbacks.end()) {
297 collision_callbacks.erase(itr);
298 }
299 }
300
301 // -----------------------------------------------------------------------------
302
SetSystemDescriptor(std::shared_ptr<ChSystemDescriptor> newdescriptor)303 void ChSystem::SetSystemDescriptor(std::shared_ptr<ChSystemDescriptor> newdescriptor) {
304 assert(newdescriptor);
305 descriptor = newdescriptor;
306 }
SetSolver(std::shared_ptr<ChSolver> newsolver)307 void ChSystem::SetSolver(std::shared_ptr<ChSolver> newsolver) {
308 assert(newsolver);
309 solver = newsolver;
310 }
311
SetCollisionSystemType(ChCollisionSystemType type)312 void ChSystem::SetCollisionSystemType(ChCollisionSystemType type) {
313 assert(assembly.GetNbodies() == 0);
314
315 collision_system_type = type;
316
317 #ifndef CHRONO_COLLISION
318 GetLog() << "Chrono was not built with Thrust support. CHRONO collision system type not available.\n";
319 collision_system_type = ChCollisionSystemType::BULLET;
320 #endif
321
322 switch (type) {
323 case ChCollisionSystemType::BULLET:
324 collision_system = chrono_types::make_shared<ChCollisionSystemBullet>();
325 break;
326 case ChCollisionSystemType::CHRONO:
327 #ifdef CHRONO_COLLISION
328 collision_system = chrono_types::make_shared<ChCollisionSystemChrono>();
329 #endif
330 break;
331 default:
332 GetLog() << "Collision system type not supported. Use SetCollisionSystem instead.\n";
333 break;
334 }
335
336 collision_system->SetNumThreads(nthreads_collision);
337 collision_system->SetSystem(this);
338 }
339
SetCollisionSystem(std::shared_ptr<ChCollisionSystem> newcollsystem)340 void ChSystem::SetCollisionSystem(std::shared_ptr<ChCollisionSystem> newcollsystem) {
341 assert(assembly.GetNbodies() == 0);
342 assert(newcollsystem);
343 collision_system = newcollsystem;
344 collision_system_type = newcollsystem->GetType();
345 collision_system->SetNumThreads(nthreads_collision);
346 collision_system->SetSystem(this);
347 }
348
SetContactContainer(std::shared_ptr<ChContactContainer> container)349 void ChSystem::SetContactContainer(std::shared_ptr<ChContactContainer> container) {
350 assert(container);
351 contact_container = container;
352 contact_container->SetSystem(this);
353 }
354
SetMaterialCompositionStrategy(std::unique_ptr<ChMaterialCompositionStrategy> && strategy)355 void ChSystem::SetMaterialCompositionStrategy(std::unique_ptr<ChMaterialCompositionStrategy>&& strategy) {
356 composition_strategy = std::move(strategy);
357 }
358
SetNumThreads(int num_threads_chrono,int num_threads_collision,int num_threads_eigen)359 void ChSystem::SetNumThreads(int num_threads_chrono, int num_threads_collision, int num_threads_eigen) {
360 nthreads_chrono = std::max(1, num_threads_chrono);
361 nthreads_collision = (num_threads_collision <= 0) ? num_threads_chrono : num_threads_collision;
362 nthreads_eigen = (num_threads_eigen <= 0) ? num_threads_chrono : num_threads_eigen;
363
364 collision_system->SetNumThreads(nthreads_collision);
365 }
366
367 // -----------------------------------------------------------------------------
368
NewBody()369 ChBody* ChSystem::NewBody() {
370 return new ChBody(collision_system_type);
371 }
372
NewBodyAuxRef()373 ChBodyAuxRef* ChSystem::NewBodyAuxRef() {
374 return new ChBodyAuxRef(collision_system_type);
375 }
376
377 // -----------------------------------------------------------------------------
378
379 // Initial system setup before analysis.
380 // This function must be called once the system construction is completed.
SetupInitial()381 void ChSystem::SetupInitial() {
382 // Set num threads for Eigen
383 Eigen::setNbThreads(nthreads_eigen);
384
385 // Set num threads for the collision system
386 if (collision_system) {
387 collision_system->SetNumThreads(nthreads_collision);
388 }
389
390 assembly.SetupInitial();
391 is_initialized = true;
392 }
393
394 // -----------------------------------------------------------------------------
395 // HIERARCHY HANDLERS
396 // -----------------------------------------------------------------------------
397
Reference_LM_byID()398 void ChSystem::Reference_LM_byID() {
399 std::vector<std::shared_ptr<ChLinkBase>> toremove;
400
401 for (auto& link : assembly.linklist) {
402 if (auto malink = std::dynamic_pointer_cast<ChLinkMarkers>(link)) {
403 std::shared_ptr<ChMarker> shm1 = assembly.SearchMarker(malink->GetMarkID1());
404 std::shared_ptr<ChMarker> shm2 = assembly.SearchMarker(malink->GetMarkID2());
405 ChMarker* mm1 = shm1.get();
406 ChMarker* mm2 = shm1.get();
407 malink->SetUpMarkers(mm1, mm2);
408 if (mm1 && mm2) {
409 malink->SetValid(true);
410 } else {
411 malink->SetValid(false);
412 malink->SetUpMarkers(0, 0); // note: marker IDs are maintained
413 toremove.push_back(malink);
414 }
415 }
416 }
417
418 for (int ir = 0; ir < toremove.size(); ++ir) {
419 assembly.RemoveLink(toremove[ir]);
420 }
421 }
422
423 // -----------------------------------------------------------------------------
424 // PREFERENCES
425 // -----------------------------------------------------------------------------
426
SetTimestepperType(ChTimestepper::Type type)427 void ChSystem::SetTimestepperType(ChTimestepper::Type type) {
428 // Do nothing if changing to a CUSTOM timestepper.
429 if (type == ChTimestepper::Type::CUSTOM)
430 return;
431
432 // Do nothing, if no change from current typestepper.
433 if (type == GetTimestepperType())
434 return;
435
436 // Plug in the new required timestepper
437 // (the previous will be automatically deallocated thanks to shared pointers)
438 switch (type) {
439 case ChTimestepper::Type::EULER_IMPLICIT:
440 timestepper = chrono_types::make_shared<ChTimestepperEulerImplicit>(this);
441 std::static_pointer_cast<ChTimestepperEulerImplicit>(timestepper)->SetMaxiters(4);
442 break;
443 case ChTimestepper::Type::EULER_IMPLICIT_LINEARIZED:
444 timestepper = chrono_types::make_shared<ChTimestepperEulerImplicitLinearized>(this);
445 break;
446 case ChTimestepper::Type::EULER_IMPLICIT_PROJECTED:
447 timestepper = chrono_types::make_shared<ChTimestepperEulerImplicitProjected>(this);
448 break;
449 case ChTimestepper::Type::TRAPEZOIDAL:
450 timestepper = chrono_types::make_shared<ChTimestepperTrapezoidal>(this);
451 std::static_pointer_cast<ChTimestepperTrapezoidal>(timestepper)->SetMaxiters(4);
452 break;
453 case ChTimestepper::Type::TRAPEZOIDAL_LINEARIZED:
454 timestepper = chrono_types::make_shared<ChTimestepperTrapezoidalLinearized>(this);
455 std::static_pointer_cast<ChTimestepperTrapezoidalLinearized>(timestepper)->SetMaxiters(4);
456 break;
457 case ChTimestepper::Type::HHT:
458 timestepper = chrono_types::make_shared<ChTimestepperHHT>(this);
459 std::static_pointer_cast<ChTimestepperHHT>(timestepper)->SetMaxiters(4);
460 break;
461 case ChTimestepper::Type::HEUN:
462 timestepper = chrono_types::make_shared<ChTimestepperHeun>(this);
463 break;
464 case ChTimestepper::Type::RUNGEKUTTA45:
465 timestepper = chrono_types::make_shared<ChTimestepperRungeKuttaExpl>(this);
466 break;
467 case ChTimestepper::Type::EULER_EXPLICIT:
468 timestepper = chrono_types::make_shared<ChTimestepperEulerExplIIorder>(this);
469 break;
470 case ChTimestepper::Type::LEAPFROG:
471 timestepper = chrono_types::make_shared<ChTimestepperLeapfrog>(this);
472 break;
473 case ChTimestepper::Type::NEWMARK:
474 timestepper = chrono_types::make_shared<ChTimestepperNewmark>(this);
475 break;
476 default:
477 throw ChException("SetTimestepperType: timestepper not supported");
478 }
479 }
480
ManageSleepingBodies()481 bool ChSystem::ManageSleepingBodies() {
482 if (!GetUseSleeping())
483 return 0;
484
485 // STEP 1:
486 // See if some body could change from no sleep-> sleep
487
488 for (auto& body : assembly.bodylist) {
489 // mark as 'could sleep' candidate
490 body->TrySleeping();
491 }
492
493 // STEP 2:
494 // See if some sleeping or potential sleeping body is touching a non sleeping one,
495 // if so, set to no sleep.
496
497 // Make this class for iterating through contacts
498
499 class _wakeup_reporter_class : public ChContactContainer::ReportContactCallback {
500 public:
501 // Callback, used to report contact points already added to the container.
502 // If returns false, the contact scanning will be stopped.
503 virtual bool OnReportContact(
504 const ChVector<>& pA, // get contact pA
505 const ChVector<>& pB, // get contact pB
506 const ChMatrix33<>& plane_coord, // get contact plane coordsystem (A column 'X' is contact normal)
507 const double& distance, // get contact distance
508 const double& eff_radius, // effective radius of curvature at contact
509 const ChVector<>& react_forces, // get react.forces (if already computed). In coordsystem 'plane_coord'
510 const ChVector<>& react_torques, // get react.torques, if rolling friction (if already computed).
511 ChContactable* contactobjA, // get model A (note: some containers may not support it and could be zero!)
512 ChContactable* contactobjB // get model B (note: some containers may not support it and could be zero!)
513 ) override {
514 if (!(contactobjA && contactobjB))
515 return true;
516 ChBody* b1 = dynamic_cast<ChBody*>(contactobjA);
517 ChBody* b2 = dynamic_cast<ChBody*>(contactobjB);
518 if (!(b1 && b2))
519 return true;
520 bool sleep1 = b1->GetSleeping();
521 bool sleep2 = b2->GetSleeping();
522 bool could_sleep1 = b1->BFlagGet(ChBody::BodyFlag::COULDSLEEP);
523 bool could_sleep2 = b2->BFlagGet(ChBody::BodyFlag::COULDSLEEP);
524 bool ground1 = b1->GetBodyFixed();
525 bool ground2 = b2->GetBodyFixed();
526 if (sleep1 && !(sleep2 || could_sleep2) && !ground2) {
527 b1->SetSleeping(false);
528 need_Setup_A = true;
529 }
530 if (sleep2 && !(sleep1 || could_sleep1) && !ground1) {
531 b2->SetSleeping(false);
532 need_Setup_A = true;
533 }
534 if (could_sleep1 && !(sleep2 || could_sleep2) && !ground2) {
535 b1->BFlagSet(ChBody::BodyFlag::COULDSLEEP, false);
536 }
537 if (could_sleep2 && !(sleep1 || could_sleep1) && !ground1) {
538 b2->BFlagSet(ChBody::BodyFlag::COULDSLEEP, false);
539 }
540 someone_sleeps = sleep1 | sleep2 | someone_sleeps;
541
542 return true; // to continue scanning contacts
543 }
544
545 // Data
546 bool someone_sleeps;
547 bool need_Setup_A;
548 };
549
550 auto my_waker = chrono_types::make_shared<_wakeup_reporter_class>();
551 my_waker->need_Setup_A = false;
552
553 bool need_Setup_L = false;
554
555 for (int i = 0; i < 1; i++) //***TO DO*** reconfigurable number of wakeup cycles
556 {
557 my_waker->someone_sleeps = false;
558
559 // scan all links and wake connected bodies
560 for (auto& link : assembly.linklist) {
561 if (auto Lpointer = std::dynamic_pointer_cast<ChLink>(link)) {
562 if (Lpointer->IsRequiringWaking()) {
563 ChBody* b1 = dynamic_cast<ChBody*>(Lpointer->GetBody1());
564 ChBody* b2 = dynamic_cast<ChBody*>(Lpointer->GetBody2());
565 if (b1 && b2) {
566 bool sleep1 = b1->GetSleeping();
567 bool sleep2 = b2->GetSleeping();
568 bool could_sleep1 = b1->BFlagGet(ChBody::BodyFlag::COULDSLEEP);
569 bool could_sleep2 = b2->BFlagGet(ChBody::BodyFlag::COULDSLEEP);
570 if (sleep1 && !(sleep2 || could_sleep2)) {
571 b1->SetSleeping(false);
572 need_Setup_L = true;
573 }
574 if (sleep2 && !(sleep1 || could_sleep1)) {
575 b2->SetSleeping(false);
576 need_Setup_L = true;
577 }
578 if (could_sleep1 && !(sleep2 || could_sleep2)) {
579 b1->BFlagSet(ChBody::BodyFlag::COULDSLEEP, false);
580 }
581 if (could_sleep2 && !(sleep1 || could_sleep1)) {
582 b2->BFlagSet(ChBody::BodyFlag::COULDSLEEP, false);
583 }
584 }
585 }
586 }
587 }
588
589 // scan all contacts and wake neighboring bodies
590 contact_container->ReportAllContacts(my_waker);
591
592 // bailout wakeup cycle prematurely, if all bodies are not sleeping
593 if (!my_waker->someone_sleeps)
594 break;
595 }
596
597 /// If some body still must change from no sleep-> sleep, do it
598 int need_Setup_B = 0;
599 for (auto& body : assembly.bodylist) {
600 if (body->BFlagGet(ChBody::BodyFlag::COULDSLEEP)) {
601 body->SetSleeping(true);
602 ++need_Setup_B;
603 }
604 }
605
606 // if some body has been activated/deactivated because of sleep state changes,
607 // the offsets and DOF counts must be updated:
608 if (my_waker->need_Setup_A || need_Setup_B || need_Setup_L) {
609 Setup();
610 return true;
611 }
612 return false;
613 }
614
615 // -----------------------------------------------------------------------------
616 // DESCRIPTOR BOOKKEEPING
617 // -----------------------------------------------------------------------------
618
DescriptorPrepareInject(ChSystemDescriptor & mdescriptor)619 void ChSystem::DescriptorPrepareInject(ChSystemDescriptor& mdescriptor) {
620 mdescriptor.BeginInsertion(); // This resets the vectors of constr. and var. pointers.
621
622 InjectConstraints(mdescriptor);
623 InjectVariables(mdescriptor);
624 InjectKRMmatrices(mdescriptor);
625
626 mdescriptor.EndInsertion();
627 }
628
629 // -----------------------------------------------------------------------------
630
631 // SETUP
632 //
633 // Set all offsets in position/speed global vectors, for all items.
634 // Count all bodies and links, etc, compute &set dof for statistics,
635 // allocates or reallocate bookkeeping data/vectors, if any,
636
Setup()637 void ChSystem::Setup() {
638 CH_PROFILE("Setup");
639
640 timer_setup.start();
641
642 ncoords = 0;
643 ncoords_w = 0;
644 ndoc = 0;
645 ndoc_w = 0;
646 ndoc_w_C = 0;
647 ndoc_w_D = 0;
648
649 // Set up the underlying assembly (compute offsets of bodies, links, etc.)
650 assembly.Setup();
651 ncoords += assembly.ncoords;
652 ncoords_w += assembly.ncoords_w;
653 ndoc_w += assembly.ndoc_w;
654 ndoc_w_C += assembly.ndoc_w_C;
655 ndoc_w_D += assembly.ndoc_w_D;
656
657 // Compute offsets for contact container
658 contact_container->SetOffset_L(assembly.offset_L + ndoc_w);
659 ndoc_w += contact_container->GetDOC();
660 ndoc_w_C += contact_container->GetDOC_c();
661 ndoc_w_D += contact_container->GetDOC_d();
662
663 ndoc = ndoc_w + assembly.nbodies; // number of constraints including quaternion constraints.
664 nsysvars = ncoords + ndoc; // total number of variables (coordinates + lagrangian multipliers)
665 nsysvars_w = ncoords_w + ndoc_w; // total number of variables (with 6 dof per body)
666
667 ndof = ncoords - ndoc; // number of degrees of freedom (approximate - does not consider constr. redundancy, etc)
668
669 timer_setup.stop();
670
671 #ifdef _DEBUG
672 // BOOKKEEPING SANITY CHECK
673 // Test if the bookkeeping is properly aligned, at least for state gather/scatters,
674 // by filling a marked vector, and see if some gaps or overlaps are remaining.
675
676 bool check_bookkeeping = false;
677 if (check_bookkeeping) {
678 ChState test_x(GetNcoords_x(), this);
679 ChStateDelta test_v(GetNcoords_w(), this);
680 ChStateDelta test_a(GetNcoords_w(), this);
681 ChVectorDynamic<> test_L(GetNconstr());
682 double poison_x = -8888.888;
683 double poison_v = -9999.999;
684 double poison_a = -7777.777;
685 double poison_L = 55555.555;
686 double test_T;
687 test_x.setConstant(poison_x); // poison x
688 test_v.setConstant(poison_v); // poison v
689 test_a.setConstant(poison_a); // poison a
690 test_L.setConstant(poison_L); // poison L
691 StateGather(test_x, test_v, test_T);
692 StateGatherAcceleration(test_a);
693 StateGatherReactions(test_L);
694 for (int i = 0; i < test_x.size(); ++i)
695 assert(test_x(i) != poison_x); // if your debugger breaks here, some ChPhysicsItem has a wrong
696 // implementation of offsets or DOFs for positions
697 for (int i = 0; i < test_v.size(); ++i)
698 assert(test_v(i) != poison_v); // if your debugger breaks here, some ChPhysicsItem has a wrong
699 // implementation of offsets or DOFs for velocities
700 for (int i = 0; i < test_a.size(); ++i)
701 assert(test_a(i) != poison_a); // if your debugger breaks here, some ChPhysicsItem has a wrong
702 // implementation of offsets or DOFs for accelerations
703 for (int i = 0; i < test_L.size(); ++i)
704 assert(test_L(i) != poison_L); // if your debugger breaks here, some ChPhysicsItem has a wrong
705 // implementation of offsets or DOFs for reaction forces
706 }
707 #endif // _DEBUG
708 }
709
710 // -----------------------------------------------------------------------------
711 // UPDATE
712 //
713 // - all physical items (bodies, links,etc.) are updated,
714 // also updating their auxiliary variables (rot.matrices, etc.).
715 // - updates all forces (automatic, as children of bodies)
716 // - updates all markers (automatic, as children of bodies).
717
Update(double mytime,bool update_assets)718 void ChSystem::Update(double mytime, bool update_assets) {
719 ch_time = mytime;
720 assembly.ChTime = mytime;
721 Update(update_assets);
722 }
723
Update(bool update_assets)724 void ChSystem::Update(bool update_assets) {
725 CH_PROFILE("Update");
726
727 if (!is_initialized)
728 SetupInitial();
729
730 timer_update.start(); // Timer for profiling
731
732 // Update underlying assembly (recursively update sub objects bodies, links, etc)
733 assembly.Update(update_assets);
734
735 // Update all contacts, if any
736 contact_container->Update(ch_time, update_assets);
737
738 timer_update.stop();
739 }
740
ForceUpdate()741 void ChSystem::ForceUpdate() {
742 is_updated = false;
743 }
744
IntToDescriptor(const unsigned int off_v,const ChStateDelta & v,const ChVectorDynamic<> & R,const unsigned int off_L,const ChVectorDynamic<> & L,const ChVectorDynamic<> & Qc)745 void ChSystem::IntToDescriptor(const unsigned int off_v,
746 const ChStateDelta& v,
747 const ChVectorDynamic<>& R,
748 const unsigned int off_L,
749 const ChVectorDynamic<>& L,
750 const ChVectorDynamic<>& Qc) {
751 // Operate on assembly sub-objects (bodies, links, etc.)
752 assembly.IntToDescriptor(off_v, v, R, off_L, L, Qc);
753
754 // Use also on contact container:
755 unsigned int displ_L = off_L - assembly.offset_L;
756 unsigned int displ_v = off_v - assembly.offset_w;
757 contact_container->IntToDescriptor(displ_v + contact_container->GetOffset_w(), v, R,
758 displ_L + contact_container->GetOffset_L(), L, Qc);
759 }
760
IntFromDescriptor(const unsigned int off_v,ChStateDelta & v,const unsigned int off_L,ChVectorDynamic<> & L)761 void ChSystem::IntFromDescriptor(const unsigned int off_v,
762 ChStateDelta& v,
763 const unsigned int off_L,
764 ChVectorDynamic<>& L) {
765 // Operate on assembly sub-objects (bodies, links, etc.)
766 assembly.IntFromDescriptor(off_v, v, off_L, L);
767
768 // Use also on contact container:
769 unsigned int displ_L = off_L - assembly.offset_L;
770 unsigned int displ_v = off_v - assembly.offset_w;
771 contact_container->IntFromDescriptor(displ_v + contact_container->GetOffset_w(), v,
772 displ_L + contact_container->GetOffset_L(), L);
773 }
774
775 // -----------------------------------------------------------------------------
776
InjectVariables(ChSystemDescriptor & mdescriptor)777 void ChSystem::InjectVariables(ChSystemDescriptor& mdescriptor) {
778 // Operate on assembly sub-objects (bodies, links, etc.)
779 assembly.InjectVariables(mdescriptor);
780
781 // Use also on contact container:
782 contact_container->InjectVariables(mdescriptor);
783 }
784
VariablesFbReset()785 void ChSystem::VariablesFbReset() {
786 // Operate on assembly sub-objects (bodies, links, etc.)
787 assembly.VariablesFbReset();
788
789 // Use also on contact container:
790 contact_container->VariablesFbReset();
791 }
792
VariablesFbLoadForces(double factor)793 void ChSystem::VariablesFbLoadForces(double factor) {
794 // Operate on assembly sub-objects (bodies, links, etc.)
795 assembly.VariablesFbLoadForces();
796
797 // Use also on contact container:
798 contact_container->VariablesFbLoadForces();
799 }
800
VariablesFbIncrementMq()801 void ChSystem::VariablesFbIncrementMq() {
802 // Operate on assembly sub-objects (bodies, links, etc.)
803 assembly.VariablesFbIncrementMq();
804
805 // Use also on contact container:
806 contact_container->VariablesFbIncrementMq();
807 }
808
VariablesQbLoadSpeed()809 void ChSystem::VariablesQbLoadSpeed() {
810 // Operate on assembly sub-objects (bodies, links, etc.)
811 assembly.VariablesQbLoadSpeed();
812
813 // Use also on contact container:
814 contact_container->VariablesQbLoadSpeed();
815 }
816
VariablesQbSetSpeed(double step)817 void ChSystem::VariablesQbSetSpeed(double step) {
818 // Operate on assembly sub-objects (bodies, links, etc.)
819 assembly.VariablesQbSetSpeed(step);
820
821 // Use also on contact container:
822 contact_container->VariablesQbSetSpeed(step);
823 }
824
VariablesQbIncrementPosition(double dt_step)825 void ChSystem::VariablesQbIncrementPosition(double dt_step) {
826 // Operate on assembly sub-objects (bodies, links, etc.)
827 assembly.VariablesQbIncrementPosition(dt_step);
828
829 // Use also on contact container:
830 contact_container->VariablesQbIncrementPosition(dt_step);
831 }
832
InjectConstraints(ChSystemDescriptor & mdescriptor)833 void ChSystem::InjectConstraints(ChSystemDescriptor& mdescriptor) {
834 // Operate on assembly sub-objects (bodies, links, etc.)
835 assembly.InjectConstraints(mdescriptor);
836
837 // Use also on contact container:
838 contact_container->InjectConstraints(mdescriptor);
839 }
840
ConstraintsBiReset()841 void ChSystem::ConstraintsBiReset() {
842 // Operate on assembly sub-objects (bodies, links, etc.)
843 assembly.ConstraintsBiReset();
844
845 // Use also on contact container:
846 contact_container->ConstraintsBiReset();
847 }
848
ConstraintsBiLoad_C(double factor,double recovery_clamp,bool do_clamp)849 void ChSystem::ConstraintsBiLoad_C(double factor, double recovery_clamp, bool do_clamp) {
850 // Operate on assembly sub-objects (bodies, links, etc.)
851 assembly.ConstraintsBiLoad_C(factor, recovery_clamp, do_clamp);
852
853 // Use also on contact container:
854 contact_container->ConstraintsBiLoad_C(factor, recovery_clamp, do_clamp);
855 }
856
ConstraintsBiLoad_Ct(double factor)857 void ChSystem::ConstraintsBiLoad_Ct(double factor) {
858 // Operate on assembly sub-objects (bodies, links, etc.)
859 assembly.ConstraintsBiLoad_Ct(factor);
860
861 // Use also on contact container:
862 contact_container->ConstraintsBiLoad_Ct(factor);
863 }
864
ConstraintsBiLoad_Qc(double factor)865 void ChSystem::ConstraintsBiLoad_Qc(double factor) {
866 // Operate on assembly sub-objects (bodies, links, etc.)
867 assembly.ConstraintsBiLoad_Qc(factor);
868
869 // Use also on contact container:
870 contact_container->ConstraintsBiLoad_Qc(factor);
871 }
872
ConstraintsFbLoadForces(double factor)873 void ChSystem::ConstraintsFbLoadForces(double factor) {
874 // Operate on assembly sub-objects (bodies, links, etc.)
875 assembly.ConstraintsFbLoadForces(factor);
876
877 // Use also on contact container:
878 contact_container->ConstraintsFbLoadForces(factor);
879 }
880
ConstraintsLoadJacobians()881 void ChSystem::ConstraintsLoadJacobians() {
882 // Operate on assembly sub-objects (bodies, links, etc.)
883 assembly.ConstraintsLoadJacobians();
884
885 // Use also on contact container:
886 contact_container->ConstraintsLoadJacobians();
887 }
888
ConstraintsFetch_react(double factor)889 void ChSystem::ConstraintsFetch_react(double factor) {
890 // Operate on assembly sub-objects (bodies, links, etc.)
891 assembly.ConstraintsFetch_react(factor);
892
893 // Use also on contact container:
894 contact_container->ConstraintsFetch_react(factor);
895 }
896
InjectKRMmatrices(ChSystemDescriptor & mdescriptor)897 void ChSystem::InjectKRMmatrices(ChSystemDescriptor& mdescriptor) {
898 // Operate on assembly sub-objects (bodies, links, etc.)
899 assembly.InjectKRMmatrices(mdescriptor);
900
901 // Use also on contact container:
902 contact_container->InjectKRMmatrices(mdescriptor);
903 }
904
KRMmatricesLoad(double Kfactor,double Rfactor,double Mfactor)905 void ChSystem::KRMmatricesLoad(double Kfactor, double Rfactor, double Mfactor) {
906 // Operate on assembly sub-objects (bodies, links, etc.)
907 assembly.KRMmatricesLoad(Kfactor, Rfactor, Mfactor);
908
909 // Use also on contact container:
910 contact_container->KRMmatricesLoad(Kfactor, Rfactor, Mfactor);
911 }
912
913 // -----------------------------------------------------------------------------
914 // TIMESTEPPER INTERFACE
915 // -----------------------------------------------------------------------------
916
917 // From system to state y={x,v}
StateGather(ChState & x,ChStateDelta & v,double & T)918 void ChSystem::StateGather(ChState& x, ChStateDelta& v, double& T) {
919 unsigned int off_x = 0;
920 unsigned int off_v = 0;
921
922 // Operate on assembly items (bodies, links, etc.)
923 assembly.IntStateGather(off_x, x, off_v, v, T);
924
925 // Use also on contact container:
926 unsigned int displ_x = off_x - assembly.offset_x;
927 unsigned int displ_v = off_v - assembly.offset_w;
928 contact_container->IntStateGather(displ_x + contact_container->GetOffset_x(), x,
929 displ_v + contact_container->GetOffset_w(), v, T);
930
931 T = ch_time;
932 }
933
934 // From state Y={x,v} to system.
StateScatter(const ChState & x,const ChStateDelta & v,const double T,bool full_update)935 void ChSystem::StateScatter(const ChState& x, const ChStateDelta& v, const double T, bool full_update) {
936 unsigned int off_x = 0;
937 unsigned int off_v = 0;
938
939 // Let each object (bodies, links, etc.) in the assembly extract its own states.
940 // Note that each object also performs an update
941 assembly.IntStateScatter(off_x, x, off_v, v, T, full_update);
942
943 // Use also on contact container:
944 unsigned int displ_x = off_x - assembly.offset_x;
945 unsigned int displ_v = off_v - assembly.offset_w;
946 contact_container->IntStateScatter(displ_x + contact_container->GetOffset_x(), x, //
947 displ_v + contact_container->GetOffset_w(), v, //
948 T, full_update);
949
950 ch_time = T;
951 }
952
953 // From system to state derivative (acceleration), some timesteppers might need last computed accel.
StateGatherAcceleration(ChStateDelta & a)954 void ChSystem::StateGatherAcceleration(ChStateDelta& a) {
955 unsigned int off_a = 0;
956
957 // Operate on assembly sub-objects (bodies, links, etc.)
958 assembly.IntStateGatherAcceleration(off_a, a);
959
960 // Use also on contact container:
961 unsigned int displ_a = off_a - assembly.offset_w;
962 contact_container->IntStateGatherAcceleration(displ_a + contact_container->GetOffset_w(), a);
963 }
964
965 // From state derivative (acceleration) to system, sometimes might be needed
StateScatterAcceleration(const ChStateDelta & a)966 void ChSystem::StateScatterAcceleration(const ChStateDelta& a) {
967 unsigned int off_a = 0;
968
969 // Operate on assembly sub-objects (bodies, links, etc.)
970 assembly.IntStateScatterAcceleration(off_a, a);
971
972 // Use also on contact container:
973 unsigned int displ_a = off_a - assembly.offset_w;
974 contact_container->IntStateScatterAcceleration(displ_a + contact_container->GetOffset_w(), a);
975 }
976
977 // From system to reaction forces (last computed) - some timestepper might need this
StateGatherReactions(ChVectorDynamic<> & L)978 void ChSystem::StateGatherReactions(ChVectorDynamic<>& L) {
979 unsigned int off_L = 0;
980
981 // Operate on assembly sub-objects (bodies, links, etc.)
982 assembly.IntStateGatherReactions(off_L, L);
983
984 // Use also on contact container:
985 unsigned int displ_L = off_L - assembly.offset_L;
986 contact_container->IntStateGatherReactions(displ_L + contact_container->GetOffset_L(), L);
987 }
988
989 // From reaction forces to system, ex. store last computed reactions in ChLink objects for plotting etc.
StateScatterReactions(const ChVectorDynamic<> & L)990 void ChSystem::StateScatterReactions(const ChVectorDynamic<>& L) {
991 unsigned int off_L = 0;
992
993 // Operate on assembly sub-objects (bodies, links, etc.)
994 assembly.IntStateScatterReactions(off_L, L);
995
996 // Use also on contact container:
997 unsigned int displ_L = off_L - assembly.offset_L;
998 contact_container->IntStateScatterReactions(displ_L + contact_container->GetOffset_L(), L);
999 }
1000
1001 // Perform x_new = x + dx for x in Y = {x, dx/dt}
1002 // It takes care of the fact that x has quaternions, dx has angular vel etc.
1003 // NOTE: the system is not updated automatically after the state increment, so one might
1004 // need to call StateScatter() if needed.
StateIncrementX(ChState & x_new,const ChState & x,const ChStateDelta & Dx)1005 void ChSystem::StateIncrementX(ChState& x_new, const ChState& x, const ChStateDelta& Dx) {
1006 unsigned int off_x = 0;
1007 unsigned int off_v = 0;
1008
1009 // Operate on assembly sub-objects (bodies, links, etc.)
1010 assembly.IntStateIncrement(off_x, x_new, x, off_v, Dx);
1011
1012 // Use also on contact container:
1013 unsigned int displ_x = off_x - assembly.offset_x;
1014 unsigned int displ_v = off_v - assembly.offset_w;
1015 contact_container->IntStateIncrement(displ_x + contact_container->GetOffset_x(), x_new, x,
1016 displ_v + contact_container->GetOffset_w(), Dx);
1017 }
1018
1019 // Assuming a DAE of the form
1020 // M*a = F(x,v,t) + Cq'*L
1021 // C(x,t) = 0
1022 // this function computes the solution of the change Du (in a or v or x) for a Newton
1023 // iteration within an implicit integration scheme.
1024 // |Du| = [ G Cq' ]^-1 * | R |
1025 // |DL| [ Cq 0 ] | Qc|
1026 // for residual R and G = [ c_a*M + c_v*dF/dv + c_x*dF/dx ]
1027 // This function returns true if successful and false otherwise.
StateSolveCorrection(ChStateDelta & Dv,ChVectorDynamic<> & L,const ChVectorDynamic<> & R,const ChVectorDynamic<> & Qc,const double c_a,const double c_v,const double c_x,const ChState & x,const ChStateDelta & v,const double T,bool force_state_scatter,bool full_update,bool force_setup)1028 bool ChSystem::StateSolveCorrection(ChStateDelta& Dv, // result: computed Dv
1029 ChVectorDynamic<>& L, // result: computed lagrangian multipliers, if any
1030 const ChVectorDynamic<>& R, // the R residual
1031 const ChVectorDynamic<>& Qc, // the Qc residual
1032 const double c_a, // the factor in c_a*M
1033 const double c_v, // the factor in c_v*dF/dv
1034 const double c_x, // the factor in c_x*dF/dx
1035 const ChState& x, // current state, x part
1036 const ChStateDelta& v, // current state, v part
1037 const double T, // current time T
1038 bool force_state_scatter, // if false, x,v and T are not scattered to the system
1039 bool full_update, // if true, perform a full update during scatter
1040 bool force_setup // if true, call the solver's Setup() function
1041 ) {
1042 CH_PROFILE("StateSolveCorrection");
1043
1044 if (force_state_scatter)
1045 StateScatter(x, v, T, full_update);
1046
1047 // R and Qc vectors --> solver sparse solver structures (also sets L and Dv to warmstart)
1048 IntToDescriptor(0, Dv, R, 0, L, Qc);
1049
1050 // If the solver's Setup() must be called or if the solver's Solve() requires it,
1051 // fill the sparse system structures with information in G and Cq.
1052 if (force_setup || GetSolver()->SolveRequiresMatrix()) {
1053 timer_jacobian.start();
1054
1055 // Cq matrix
1056 ConstraintsLoadJacobians();
1057
1058 // G matrix: M, K, R components
1059 if (c_a || c_v || c_x)
1060 KRMmatricesLoad(-c_x, -c_v, c_a);
1061
1062 // For ChVariable objects without a ChKblock, just use the 'a' coefficient
1063 descriptor->SetMassFactor(c_a);
1064
1065 timer_jacobian.stop();
1066 }
1067
1068 // Diagnostics:
1069 if (dump_matrices) {
1070 const char* numformat = "%.12g";
1071 std::string sprefix = "solve_" + std::to_string(stepcount) + "_" + std::to_string(solvecount) + "_";
1072
1073 descriptor->DumpLastMatrices(true, sprefix.c_str());
1074 descriptor->DumpLastMatrices(false, sprefix.c_str());
1075
1076 chrono::ChStreamOutAsciiFile file_x((sprefix + "x_pre.dat").c_str());
1077 file_x.SetNumFormat(numformat);
1078 StreamOUTdenseMatlabFormat(x, file_x);
1079
1080 chrono::ChStreamOutAsciiFile file_v((sprefix + "v_pre.dat").c_str());
1081 file_v.SetNumFormat(numformat);
1082 StreamOUTdenseMatlabFormat(v, file_v);
1083 }
1084
1085 // If indicated, first perform a solver setup.
1086 // Return 'false' if the setup phase fails.
1087 if (force_setup) {
1088 timer_ls_setup.start();
1089 bool success = GetSolver()->Setup(*descriptor);
1090 timer_ls_setup.stop();
1091 setupcount++;
1092 if (!success)
1093 return false;
1094 }
1095
1096 // Solve the problem
1097 // The solution is scattered in the provided system descriptor
1098 timer_ls_solve.start();
1099 GetSolver()->Solve(*descriptor);
1100 timer_ls_solve.stop();
1101
1102 // Dv and L vectors <-- sparse solver structures
1103 IntFromDescriptor(0, Dv, 0, L);
1104
1105 // Diagnostics:
1106 if (dump_matrices) {
1107 const char* numformat = "%.12g";
1108 std::string sprefix = "solve_" + std::to_string(stepcount) + "_" + std::to_string(solvecount) + "_";
1109
1110 chrono::ChStreamOutAsciiFile file_Dv((sprefix + "Dv.dat").c_str());
1111 file_Dv.SetNumFormat(numformat);
1112 StreamOUTdenseMatlabFormat(Dv, file_Dv);
1113
1114 chrono::ChStreamOutAsciiFile file_L((sprefix + "L.dat").c_str());
1115 file_L.SetNumFormat(numformat);
1116 StreamOUTdenseMatlabFormat(L, file_L);
1117
1118 // Just for diagnostic, dump also unscaled loads (forces,torques),
1119 // since the .._f.dat vector dumped in DumpLastMatrices() might contain scaled loads, and also +M*v
1120 ChVectorDynamic<> tempF(this->GetNcoords_v());
1121 tempF.setZero();
1122 LoadResidual_F(tempF, 1.0);
1123 chrono::ChStreamOutAsciiFile file_F((sprefix + "F_pre.dat").c_str());
1124 file_F.SetNumFormat(numformat);
1125 StreamOUTdenseMatlabFormat(tempF, file_F);
1126 }
1127
1128 solvecount++;
1129
1130 return true;
1131 }
1132
GetBodyAppliedForce(ChBody * body)1133 ChVector<> ChSystem::GetBodyAppliedForce(ChBody* body) {
1134 if (!is_initialized)
1135 return ChVector<>(0, 0, 0);
1136
1137 if (!applied_forces_current) {
1138 applied_forces.setZero(this->GetNcoords_v());
1139 LoadResidual_F(applied_forces, 1.0);
1140 applied_forces_current = true;
1141 }
1142 return applied_forces.segment(body->Variables().GetOffset() + 0, 3);
1143 }
1144
GetBodyAppliedTorque(ChBody * body)1145 ChVector<> ChSystem::GetBodyAppliedTorque(ChBody* body) {
1146 if (!is_initialized)
1147 return ChVector<>(0, 0, 0);
1148
1149 if (!applied_forces_current) {
1150 applied_forces.setZero(this->GetNcoords_v());
1151 LoadResidual_F(applied_forces, 1.0);
1152 applied_forces_current = true;
1153 }
1154 return applied_forces.segment(body->Variables().GetOffset() + 3, 3);
1155 }
1156
1157 // Increment a vector R with the term c*F:
1158 // R += c*F
LoadResidual_F(ChVectorDynamic<> & R,const double c)1159 void ChSystem::LoadResidual_F(ChVectorDynamic<>& R, const double c) {
1160 unsigned int off = 0;
1161
1162 // Operate on assembly sub-objects (bodies, links, etc.)
1163 assembly.IntLoadResidual_F(off, R, c);
1164
1165 // Use also on contact container:
1166 unsigned int displ_v = off - assembly.offset_w;
1167 contact_container->IntLoadResidual_F(displ_v + contact_container->GetOffset_w(), R, c);
1168 }
1169
1170 // Increment a vector R with a term that has M multiplied a given vector w:
1171 // R += c*M*w
LoadResidual_Mv(ChVectorDynamic<> & R,const ChVectorDynamic<> & w,const double c)1172 void ChSystem::LoadResidual_Mv(ChVectorDynamic<>& R, const ChVectorDynamic<>& w, const double c) {
1173 unsigned int off = 0;
1174
1175 // Operate on assembly sub-objects (bodies, links, etc.)
1176 assembly.IntLoadResidual_Mv(off, R, w, c);
1177
1178 // Use also on contact container:
1179 unsigned int displ_v = off - assembly.offset_w;
1180 contact_container->IntLoadResidual_Mv(displ_v + contact_container->GetOffset_w(), R, w, c);
1181 }
1182
1183 // Increment a vectorR with the term Cq'*L:
1184 // R += c*Cq'*L
LoadResidual_CqL(ChVectorDynamic<> & R,const ChVectorDynamic<> & L,const double c)1185 void ChSystem::LoadResidual_CqL(ChVectorDynamic<>& R, const ChVectorDynamic<>& L, const double c) {
1186 unsigned int off_L = 0;
1187
1188 // Operate on assembly sub-objects (bodies, links, etc.)
1189 assembly.IntLoadResidual_CqL(off_L, R, L, c);
1190
1191 // Use also on contact container:
1192 unsigned int displ_L = off_L - assembly.offset_L;
1193 contact_container->IntLoadResidual_CqL(displ_L + contact_container->GetOffset_L(), R, L, c);
1194 }
1195
1196 // Increment a vector Qc with the term C:
1197 // Qc += c*C
LoadConstraint_C(ChVectorDynamic<> & Qc,const double c,const bool do_clamp,const double clamp)1198 void ChSystem::LoadConstraint_C(ChVectorDynamic<>& Qc, // result: the Qc residual, Qc += c*C
1199 const double c, // a scaling factor
1200 const bool do_clamp, // enable optional clamping of Qc
1201 const double clamp // clamping value
1202 ) {
1203 unsigned int off_L = 0;
1204
1205 // Operate on assembly sub-objects (bodies, links, etc.)
1206 assembly.IntLoadConstraint_C(off_L, Qc, c, do_clamp, clamp);
1207
1208 // Use also on contact container:
1209 unsigned int displ_L = off_L - assembly.offset_L;
1210 contact_container->IntLoadConstraint_C(displ_L + contact_container->GetOffset_L(), Qc, c, do_clamp, clamp);
1211 }
1212
1213 // Increment a vector Qc with the term Ct = partial derivative dC/dt:
1214 // Qc += c*Ct
LoadConstraint_Ct(ChVectorDynamic<> & Qc,const double c)1215 void ChSystem::LoadConstraint_Ct(ChVectorDynamic<>& Qc, const double c) {
1216 unsigned int off_L = 0;
1217
1218 // Operate on assembly sub-objects (bodies, links, etc.)
1219 assembly.IntLoadConstraint_Ct(off_L, Qc, c);
1220
1221 // Use also on contact container:
1222 unsigned int displ_L = off_L - assembly.offset_L;
1223 contact_container->IntLoadConstraint_Ct(displ_L + contact_container->GetOffset_L(), Qc, c);
1224 }
1225
1226 // -----------------------------------------------------------------------------
1227 // COLLISION OPERATIONS
1228 // -----------------------------------------------------------------------------
1229
GetNcontacts()1230 int ChSystem::GetNcontacts() {
1231 return contact_container->GetNcontacts();
1232 }
1233
ComputeCollisions()1234 double ChSystem::ComputeCollisions() {
1235 CH_PROFILE("ComputeCollisions");
1236
1237 double mretC = 0.0;
1238
1239 timer_collision.start();
1240
1241 // Update all positions of collision models: delegate this to the ChAssembly
1242 assembly.SyncCollisionModels();
1243
1244 // Perform the collision detection ( broadphase and narrowphase )
1245 collision_system->PreProcess();
1246 collision_system->Run();
1247 collision_system->PostProcess();
1248
1249 // Report and store contacts and/or proximities, if there are some
1250 // containers in the physic system. The default contact container
1251 // for ChBody and ChParticles is used always.
1252 {
1253 CH_PROFILE("ReportContacts");
1254
1255 collision_system->ReportContacts(contact_container.get());
1256
1257 for (auto& item : assembly.otherphysicslist) {
1258 if (auto mcontactcontainer = std::dynamic_pointer_cast<ChContactContainer>(item)) {
1259 // collision_system->ReportContacts(mcontactcontainer.get());
1260 // ***TEST*** if one wants to populate a ChContactContainer this would clear it anyway...
1261 }
1262
1263 if (auto mproximitycontainer = std::dynamic_pointer_cast<ChProximityContainer>(item)) {
1264 collision_system->ReportProximities(mproximitycontainer.get());
1265 }
1266 }
1267 }
1268
1269 // Invoke the custom collision callbacks (if any). These can potentially add
1270 // additional contacts to the contact container.
1271 for (size_t ic = 0; ic < collision_callbacks.size(); ic++)
1272 collision_callbacks[ic]->OnCustomCollision(this);
1273
1274 // Cache the total number of contacts
1275 ncontacts = contact_container->GetNcontacts();
1276
1277 timer_collision.stop();
1278
1279 return mretC;
1280 }
1281
1282 // =============================================================================
1283 // PHYSICAL OPERATIONS
1284 // =============================================================================
1285
GetMassMatrix(ChSparseMatrix * M)1286 void ChSystem::GetMassMatrix(ChSparseMatrix* M) {
1287 // IntToDescriptor(0, Dv, R, 0, L, Qc);
1288 // ConstraintsLoadJacobians();
1289
1290 // Load all KRM matrices with the M part only
1291 KRMmatricesLoad(0, 0, 1.0);
1292 // For ChVariable objects without a ChKblock, but still with a mass:
1293 descriptor->SetMassFactor(1.0);
1294
1295 // Fill system-level M matrix
1296 this->GetSystemDescriptor()->ConvertToMatrixForm(nullptr, M, nullptr, nullptr, nullptr, nullptr, false, false);
1297 }
1298
GetStiffnessMatrix(ChSparseMatrix * K)1299 void ChSystem::GetStiffnessMatrix(ChSparseMatrix* K) {
1300 // IntToDescriptor(0, Dv, R, 0, L, Qc);
1301 // ConstraintsLoadJacobians();
1302
1303 // Load all KRM matrices with the K part only
1304 this->KRMmatricesLoad(1.0, 0, 0);
1305 // For ChVariable objects without a ChKblock, but still with a mass:
1306 descriptor->SetMassFactor(0.0);
1307
1308 // Fill system-level K matrix
1309 this->GetSystemDescriptor()->ConvertToMatrixForm(nullptr, K, nullptr, nullptr, nullptr, nullptr, false, false);
1310 }
1311
GetDampingMatrix(ChSparseMatrix * R)1312 void ChSystem::GetDampingMatrix(ChSparseMatrix* R) {
1313 // IntToDescriptor(0, Dv, R, 0, L, Qc);
1314 // ConstraintsLoadJacobians();
1315
1316 // Load all KRM matrices with the R part only
1317 this->KRMmatricesLoad(0, 1.0, 0);
1318 // For ChVariable objects without a ChKblock, but still with a mass:
1319 descriptor->SetMassFactor(0.0);
1320
1321 // Fill system-level R matrix
1322 this->GetSystemDescriptor()->ConvertToMatrixForm(nullptr, R, nullptr, nullptr, nullptr, nullptr, false, false);
1323 }
1324
GetConstraintJacobianMatrix(ChSparseMatrix * Cq)1325 void ChSystem::GetConstraintJacobianMatrix(ChSparseMatrix* Cq) {
1326 // IntToDescriptor(0, Dv, R, 0, L, Qc);
1327
1328 // Load all jacobian matrices
1329 this->ConstraintsLoadJacobians();
1330
1331 // Fill system-level R matrix
1332 this->GetSystemDescriptor()->ConvertToMatrixForm(Cq, nullptr, nullptr, nullptr, nullptr, nullptr, false, false);
1333 }
1334
DumpSystemMatrices(bool save_M,bool save_K,bool save_R,bool save_Cq,const char * path)1335 void ChSystem::DumpSystemMatrices(bool save_M, bool save_K, bool save_R, bool save_Cq, const char* path) {
1336 char filename[300];
1337 const char* numformat = "%.12g";
1338
1339 if (save_M) {
1340 ChSparseMatrix mM;
1341 this->GetMassMatrix(&mM);
1342 sprintf(filename, "%s%s", path, "_M.dat");
1343 ChStreamOutAsciiFile file_M(filename);
1344 file_M.SetNumFormat(numformat);
1345 StreamOUTsparseMatlabFormat(mM, file_M);
1346 }
1347 if (save_K) {
1348 ChSparseMatrix mK;
1349 this->GetStiffnessMatrix(&mK);
1350 sprintf(filename, "%s%s", path, "_K.dat");
1351 ChStreamOutAsciiFile file_K(filename);
1352 file_K.SetNumFormat(numformat);
1353 StreamOUTsparseMatlabFormat(mK, file_K);
1354 }
1355 if (save_R) {
1356 ChSparseMatrix mR;
1357 this->GetDampingMatrix(&mR);
1358 sprintf(filename, "%s%s", path, "_R.dat");
1359 ChStreamOutAsciiFile file_R(filename);
1360 file_R.SetNumFormat(numformat);
1361 StreamOUTsparseMatlabFormat(mR, file_R);
1362 }
1363 if (save_Cq) {
1364 ChSparseMatrix mCq;
1365 this->GetConstraintJacobianMatrix(&mCq);
1366 sprintf(filename, "%s%s", path, "_Cq.dat");
1367 ChStreamOutAsciiFile file_Cq(filename);
1368 file_Cq.SetNumFormat(numformat);
1369 StreamOUTsparseMatlabFormat(mCq, file_Cq);
1370 }
1371 }
1372
1373 // -----------------------------------------------------------------------------
1374 // PERFORM AN INTEGRATION STEP. ----
1375 //
1376 // Advances a single time step.
1377 //
1378 // Note that time step can be modified if some variable-time stepper is used.
1379 // -----------------------------------------------------------------------------
1380
DoStepDynamics(double step_size)1381 int ChSystem::DoStepDynamics(double step_size) {
1382 if (!is_initialized)
1383 SetupInitial();
1384
1385 applied_forces_current = false;
1386 step = step_size;
1387 return Integrate_Y();
1388 }
1389
1390 // -----------------------------------------------------------------------------
1391 // PERFORM INTEGRATION STEP using pluggable timestepper
1392 // -----------------------------------------------------------------------------
1393
Integrate_Y()1394 bool ChSystem::Integrate_Y() {
1395 CH_PROFILE("Integrate_Y");
1396
1397 ResetTimers();
1398
1399 timer_step.start();
1400
1401 stepcount++;
1402 solvecount = 0;
1403 setupcount = 0;
1404
1405 // Compute contacts and create contact constraints
1406 int ncontacts_old = ncontacts;
1407 ComputeCollisions();
1408
1409 // Declare an NSC system as "out of date" if there are contacts
1410 if (GetContactMethod() == ChContactMethod::NSC && (ncontacts_old != 0 || ncontacts != 0))
1411 is_updated = false;
1412
1413 // Counts dofs, number of constraints, statistics, etc.
1414 // Note: this must be invoked at all times (regardless of the flag is_updated), as various physics items may use
1415 // their own Setup to perform operations at the beginning of a step.
1416 Setup();
1417
1418 // If needed, update everything. No need to update visualization assets here.
1419 if (!is_updated) {
1420 Update(false);
1421 }
1422
1423 // Re-wake the bodies that cannot sleep because they are in contact with
1424 // some body that is not in sleep state.
1425 ManageSleepingBodies();
1426
1427 // Prepare lists of variables and constraints.
1428 DescriptorPrepareInject(*descriptor);
1429
1430 // No need to update counts and offsets, as already done by the above call (in ChSystemDescriptor::EndInsertion)
1431 ////descriptor->UpdateCountsAndOffsets();
1432
1433 // Set some settings in timestepper object
1434 timestepper->SetQcDoClamp(true);
1435 timestepper->SetQcClamping(max_penetration_recovery_speed);
1436 if (std::dynamic_pointer_cast<ChTimestepperHHT>(timestepper) ||
1437 std::dynamic_pointer_cast<ChTimestepperNewmark>(timestepper))
1438 timestepper->SetQcDoClamp(false);
1439
1440 // PERFORM TIME STEP HERE!
1441 {
1442 CH_PROFILE("Advance");
1443 timer_advance.start();
1444 timestepper->Advance(step);
1445 timer_advance.stop();
1446 }
1447
1448 // Executes custom processing at the end of step
1449 CustomEndOfStep();
1450
1451 // Call method to gather contact forces/torques in rigid bodies
1452 contact_container->ComputeContactForces();
1453
1454 // Time elapsed for step
1455 timer_step.stop();
1456
1457 // Tentatively mark system as unchanged (i.e., no updated necessary)
1458 is_updated = true;
1459
1460 return true;
1461 }
1462
1463 // -----------------------------------------------------------------------------
1464 // **** SATISFY ALL CONSTRAINT EQUATIONS WITH NEWTON
1465 // **** ITERATION, UNTIL TOLERANCE SATISFIED, THEN UPDATE
1466 // **** THE "Y" STATE WITH SetY (WHICH AUTOMATICALLY UPDATES
1467 // **** ALSO AUXILIARY MATRICES).
1468 // -----------------------------------------------------------------------------
1469
DoAssembly(int action)1470 bool ChSystem::DoAssembly(int action) {
1471 if (!is_initialized)
1472 SetupInitial();
1473
1474 applied_forces_current = false;
1475
1476 solvecount = 0;
1477 setupcount = 0;
1478
1479 Setup();
1480 Update();
1481
1482 // Overwrite various parameters
1483 int new_max_iters = 300; // if using an iterative solver
1484 double new_tolerance = 1e-10; // if using an iterative solver
1485 double new_step = 1e-6;
1486
1487 int old_max_iters = GetSolverMaxIterations();
1488 double old_tolerance = GetSolverTolerance();
1489 double old_step = GetStep();
1490
1491 SetSolverMaxIterations(std::max(old_max_iters, new_max_iters));
1492 SetSolverTolerance(new_tolerance);
1493 SetStep(new_step);
1494
1495 // Prepare lists of variables and constraints.
1496 DescriptorPrepareInject(*descriptor);
1497
1498 ChAssemblyAnalysis manalysis(*this);
1499 manalysis.SetMaxAssemblyIters(GetMaxiter());
1500
1501 // Perform analysis
1502 manalysis.AssemblyAnalysis(action, new_step);
1503
1504 // Restore parameters
1505 SetSolverMaxIterations(old_max_iters);
1506 SetSolverTolerance(old_tolerance);
1507 SetStep(old_step);
1508
1509 return true;
1510 }
1511
1512 // -----------------------------------------------------------------------------
1513 // **** PERFORM THE LINEAR STATIC ANALYSIS
1514 // -----------------------------------------------------------------------------
1515
DoStaticLinear()1516 bool ChSystem::DoStaticLinear() {
1517 if (!is_initialized)
1518 SetupInitial();
1519
1520 applied_forces_current = false;
1521
1522 solvecount = 0;
1523 setupcount = 0;
1524
1525 Setup();
1526 Update();
1527
1528 int old_maxsteps = GetSolverMaxIterations();
1529 SetSolverMaxIterations(std::max(old_maxsteps, 300));
1530
1531 // Prepare lists of variables and constraints.
1532 DescriptorPrepareInject(*descriptor);
1533
1534 ChStaticLinearAnalysis manalysis(*this);
1535
1536 // Perform analysis
1537 manalysis.StaticAnalysis();
1538
1539 SetSolverMaxIterations(old_maxsteps);
1540
1541 bool dump_data = false;
1542
1543 if (dump_data) {
1544 GetSystemDescriptor()->DumpLastMatrices();
1545
1546 // optional check for correctness in result
1547 chrono::ChVectorDynamic<double> md;
1548 GetSystemDescriptor()->BuildDiVector(md); // d={f;-b}
1549
1550 chrono::ChVectorDynamic<double> mx;
1551 GetSystemDescriptor()->FromUnknownsToVector(mx, true); // x ={q,-l}
1552 chrono::ChStreamOutAsciiFile file_x("dump_x.dat");
1553 StreamOUTdenseMatlabFormat(mx, file_x);
1554
1555 chrono::ChVectorDynamic<double> mZx;
1556 GetSystemDescriptor()->SystemProduct(mZx, mx); // Zx = Z*x
1557
1558 GetLog() << "CHECK: norm of solver residual: ||Z*x-d|| -------------------\n";
1559 GetLog() << (mZx - md).lpNorm<Eigen::Infinity>() << "\n";
1560 }
1561
1562 return true;
1563 }
1564
1565 // -----------------------------------------------------------------------------
1566 // **** PERFORM THE NONLINEAR STATIC ANALYSIS
1567 // -----------------------------------------------------------------------------
1568
DoStaticNonlinear(int nsteps,bool verbose)1569 bool ChSystem::DoStaticNonlinear(int nsteps, bool verbose) {
1570 if (!is_initialized)
1571 SetupInitial();
1572
1573 applied_forces_current = false;
1574
1575 solvecount = 0;
1576 setupcount = 0;
1577
1578 Setup();
1579 Update();
1580
1581 int old_maxsteps = GetSolverMaxIterations();
1582 SetSolverMaxIterations(std::max(old_maxsteps, 300));
1583
1584 // Prepare lists of variables and constraints.
1585 DescriptorPrepareInject(*descriptor);
1586
1587 ChStaticNonLinearAnalysis manalysis(*this);
1588 manalysis.SetMaxIterations(nsteps);
1589 manalysis.SetVerbose(verbose);
1590
1591 // Perform analysis
1592 manalysis.StaticAnalysis();
1593
1594 SetSolverMaxIterations(old_maxsteps);
1595
1596 return true;
1597 }
1598
DoStaticAnalysis(std::shared_ptr<ChStaticAnalysis> analysis)1599 bool ChSystem::DoStaticAnalysis(std::shared_ptr<ChStaticAnalysis> analysis) {
1600 if (!is_initialized)
1601 SetupInitial();
1602
1603 applied_forces_current = false;
1604
1605 solvecount = 0;
1606 setupcount = 0;
1607
1608 Setup();
1609 Update();
1610
1611 DescriptorPrepareInject(*descriptor);
1612
1613 analysis->StaticAnalysis();
1614
1615 return true;
1616 }
1617
DoStaticNonlinearRheonomic(int nsteps,bool verbose,std::shared_ptr<ChStaticNonLinearRheonomicAnalysis::IterationCallback> mcallback)1618 bool ChSystem::DoStaticNonlinearRheonomic(int nsteps, bool verbose, std::shared_ptr<ChStaticNonLinearRheonomicAnalysis::IterationCallback> mcallback) {
1619 if (!is_initialized)
1620 SetupInitial();
1621
1622 applied_forces_current = false;
1623
1624 solvecount = 0;
1625 setupcount = 0;
1626
1627 Setup();
1628 Update();
1629
1630 int old_maxsteps = GetSolverMaxIterations();
1631 SetSolverMaxIterations(std::max(old_maxsteps, 300));
1632
1633 // Prepare lists of variables and constraints.
1634 DescriptorPrepareInject(*descriptor);
1635
1636 ChStaticNonLinearRheonomicAnalysis manalysis(*this);
1637 manalysis.SetMaxIterations(nsteps);
1638 manalysis.SetVerbose(verbose);
1639 manalysis.SetCallbackIterationBegin(mcallback);
1640
1641 // Perform analysis
1642 manalysis.StaticAnalysis();
1643
1644 SetSolverMaxIterations(old_maxsteps);
1645
1646 return true;
1647 }
1648
1649 // -----------------------------------------------------------------------------
1650 // **** PERFORM THE STATIC ANALYSIS, FINDING THE STATIC
1651 // **** EQUILIBRIUM OF THE SYSTEM, WITH ITERATIVE SOLUTION
1652 // -----------------------------------------------------------------------------
1653
DoStaticRelaxing(int nsteps)1654 bool ChSystem::DoStaticRelaxing(int nsteps) {
1655 if (!is_initialized)
1656 SetupInitial();
1657
1658 applied_forces_current = false;
1659
1660 solvecount = 0;
1661 setupcount = 0;
1662
1663 int err = 0;
1664
1665 if ((ncoords > 0) && (ndof >= 0)) {
1666 for (int m_iter = 0; m_iter < nsteps; m_iter++) {
1667 for (auto& body : assembly.bodylist) {
1668 // Set no body speed and no body accel.
1669 body->SetNoSpeedNoAcceleration();
1670 }
1671 for (auto& mesh : assembly.meshlist) {
1672 mesh->SetNoSpeedNoAcceleration();
1673 }
1674 for (auto& item : assembly.otherphysicslist) {
1675 item->SetNoSpeedNoAcceleration();
1676 }
1677
1678 double undotime = GetChTime();
1679 DoFrameDynamics(undotime + (step * 1.8) * (((double)nsteps - (double)m_iter)) / (double)nsteps);
1680 ch_time = undotime;
1681 }
1682
1683 for (auto& body : assembly.bodylist) {
1684 // Set no body speed and no body accel.
1685 body->SetNoSpeedNoAcceleration();
1686 }
1687 for (auto& mesh : assembly.meshlist) {
1688 mesh->SetNoSpeedNoAcceleration();
1689 }
1690 for (auto& item : assembly.otherphysicslist) {
1691 item->SetNoSpeedNoAcceleration();
1692 }
1693 }
1694
1695 if (err) {
1696 last_err = true;
1697 GetLog() << "WARNING: some constraints may be redundant, but couldn't be eliminated \n";
1698 }
1699 return last_err;
1700 }
1701
1702 // -----------------------------------------------------------------------------
1703 // **** --- THE KINEMATIC SIMULATION ---
1704 // **** PERFORM IK (INVERSE KINEMATICS) UNTIL THE END_TIME IS
1705 // **** REACHED, STARTING FROM THE CURRENT TIME.
1706 // -----------------------------------------------------------------------------
1707
DoEntireKinematics(double end_time)1708 bool ChSystem::DoEntireKinematics(double end_time) {
1709 if (!is_initialized)
1710 SetupInitial();
1711
1712 applied_forces_current = false;
1713
1714 Setup();
1715
1716 int action = AssemblyLevel::POSITION | AssemblyLevel::VELOCITY | AssemblyLevel::ACCELERATION;
1717
1718 DoAssembly(action);
1719 // first check if there are redundant links (at least one NR cycle
1720 // even if the structure is already assembled)
1721
1722 while (ch_time < end_time) {
1723 // Newton-Raphson iteration, closing constraints
1724 DoAssembly(action);
1725
1726 if (last_err)
1727 return false;
1728
1729 // Update time and repeat.
1730 ch_time += step;
1731 }
1732
1733 return true;
1734 }
1735
1736 // -----------------------------------------------------------------------------
1737 // **** --- THE DYNAMICAL SIMULATION ---
1738 // **** PERFORM EXPLICIT OR IMPLICIT INTEGRATION TO GET
1739 // **** THE DYNAMICAL SIMULATION OF THE SYSTEM, UNTIL THE
1740 // **** END_TIME IS REACHED.
1741 // -----------------------------------------------------------------------------
1742
DoEntireDynamics(double end_time)1743 bool ChSystem::DoEntireDynamics(double end_time) {
1744 if (!is_initialized)
1745 SetupInitial();
1746
1747 applied_forces_current = false;
1748
1749 Setup();
1750
1751 // the system may have wrong layout, or too large
1752 // clearances in constraints, so it is better to
1753 // check for constraint violation each time the integration starts
1754 DoAssembly(AssemblyLevel::POSITION | AssemblyLevel::VELOCITY | AssemblyLevel::ACCELERATION);
1755
1756 // Perform the integration steps until the end
1757 // time is reached.
1758 // All the updating (of Y, Y_dt and time) is done
1759 // automatically by Integrate()
1760
1761 while (ch_time < end_time) {
1762 if (!Integrate_Y())
1763 break; // >>> 1- single integration step,
1764 // updating Y, from t to t+dt.
1765 if (last_err)
1766 return false;
1767 }
1768
1769 if (last_err)
1770 return false;
1771 return true;
1772 }
1773
1774 // Perform the dynamical integration, from current ChTime to
1775 // the specified end time, and terminating the integration exactly
1776 // on the end time. Therefore, the step of integration may get a
1777 // little increment/decrement to have the last step ending in end time.
1778 // Note that this function can be used in iterations to provide results in
1779 // a evenly spaced frames of time, even if the steps are changing.
1780 // Also note that if the time step is higher than the time increment
1781 // requested to reach end time, the step is lowered.
1782
DoFrameDynamics(double end_time)1783 bool ChSystem::DoFrameDynamics(double end_time) {
1784 if (!is_initialized)
1785 SetupInitial();
1786
1787 applied_forces_current = false;
1788
1789 double old_step = 0;
1790 double left_time;
1791 bool restore_oldstep = false;
1792 int counter = 0;
1793
1794 while (ch_time < end_time) {
1795 restore_oldstep = false;
1796 counter++;
1797
1798 left_time = end_time - ch_time;
1799
1800 if (left_time < 1e-12)
1801 break; // - no integration if backward or null frame step.
1802
1803 if (left_time < (1.3 * step)) // - step changed if too little frame step
1804 {
1805 old_step = step;
1806 step = left_time;
1807 restore_oldstep = true;
1808 }
1809
1810 if (!Integrate_Y())
1811 break; // *** Single integration step,
1812 // *** updating Y, from t to t+dt.
1813 // *** This also changes local ChTime, and may change step
1814
1815 if (last_err)
1816 break;
1817 }
1818
1819 if (restore_oldstep)
1820 step = old_step; // if timestep was changed to meet the end of frametime, restore pre-last (even for
1821 // time-varying schemes)
1822
1823 if (last_err)
1824 return false;
1825 return true;
1826 }
1827
1828 // Performs the dynamical simulation, but using "frame integration"
1829 // iteratively. The results are provided only at each frame (evenly
1830 // spaced by "frame_step") rather than at each "step" (steps can be much
1831 // more than frames, and they may be automatically changed by integrator).
1832 // Moreover, the integration results shouldn't be dependent by the
1833 // "frame_step" value (steps are performed anyway, like in normal "DoEntireDynamics"
1834 // command).
1835
DoEntireUniformDynamics(double end_time,double frame_step)1836 bool ChSystem::DoEntireUniformDynamics(double end_time, double frame_step) {
1837 if (!is_initialized)
1838 SetupInitial();
1839
1840 applied_forces_current = false;
1841
1842 // the initial system may have wrong layout, or too large clearances in constraints.
1843 Setup();
1844 DoAssembly(AssemblyLevel::POSITION | AssemblyLevel::VELOCITY | AssemblyLevel::ACCELERATION);
1845
1846 while (ch_time < end_time) {
1847 double goto_time = (ch_time + frame_step);
1848 if (!DoFrameDynamics(goto_time))
1849 return false;
1850 }
1851
1852 return true;
1853 }
1854
1855 // Like DoFrameDynamics, but performs kinematics instead of dynamics
1856
DoFrameKinematics(double end_time)1857 bool ChSystem::DoFrameKinematics(double end_time) {
1858 if (!is_initialized)
1859 SetupInitial();
1860
1861 applied_forces_current = false;
1862
1863 double old_step = 0;
1864 double left_time;
1865 int restore_oldstep;
1866 int counter = 0;
1867
1868 ////double frame_step = (end_time - ch_time);
1869
1870 while (ch_time < end_time) {
1871 restore_oldstep = false;
1872 counter++;
1873
1874 left_time = end_time - ch_time;
1875
1876 if (left_time < 0.000000001)
1877 break; // - no kinematics for backward
1878
1879 if (left_time < (1.3 * step)) // - step changed if too little frame step
1880 {
1881 old_step = step;
1882 step = left_time;
1883 restore_oldstep = true;
1884 }
1885
1886 // Newton Raphson kinematic equations solver
1887 DoAssembly(AssemblyLevel::POSITION | AssemblyLevel::VELOCITY | AssemblyLevel::ACCELERATION);
1888
1889 if (last_err)
1890 return false;
1891
1892 ch_time += step;
1893
1894 if (restore_oldstep)
1895 step = old_step; // if timestep was changed to meet the end of frametime
1896 }
1897
1898 return true;
1899 }
1900
DoStepKinematics(double step_size)1901 bool ChSystem::DoStepKinematics(double step_size) {
1902 if (!is_initialized)
1903 SetupInitial();
1904
1905 applied_forces_current = false;
1906
1907 ch_time += step_size;
1908
1909 Update();
1910
1911 // Newton Raphson kinematic equations solver
1912 DoAssembly(AssemblyLevel::POSITION | AssemblyLevel::VELOCITY | AssemblyLevel::ACCELERATION);
1913
1914 if (last_err)
1915 return false;
1916
1917 return true;
1918 }
1919
1920 // Full assembly -computes also forces-
DoFullAssembly()1921 bool ChSystem::DoFullAssembly() {
1922 DoAssembly(AssemblyLevel::POSITION | AssemblyLevel::VELOCITY | AssemblyLevel::ACCELERATION);
1923
1924 return last_err;
1925 }
1926
1927 // -----------------------------------------------------------------------------
1928 // STREAMING - FILE HANDLING
1929
ArchiveOUT(ChArchiveOut & marchive)1930 void ChSystem::ArchiveOUT(ChArchiveOut& marchive) {
1931 // version number
1932 marchive.VersionWrite<ChSystem>();
1933
1934 // serialize underlying assembly
1935 assembly.ArchiveOUT(marchive);
1936
1937 // serialize all member data:
1938
1939 marchive << CHNVP(contact_container);
1940
1941 marchive << CHNVP(G_acc);
1942 marchive << CHNVP(ch_time);
1943 marchive << CHNVP(step);
1944 marchive << CHNVP(step_min);
1945 marchive << CHNVP(step_max);
1946 marchive << CHNVP(stepcount);
1947 marchive << CHNVP(dump_matrices);
1948
1949 marchive << CHNVP(tol_force);
1950 marchive << CHNVP(maxiter);
1951 marchive << CHNVP(use_sleeping);
1952
1953 marchive << CHNVP(descriptor);
1954 marchive << CHNVP(solver);
1955
1956 marchive << CHNVP(min_bounce_speed);
1957 marchive << CHNVP(max_penetration_recovery_speed);
1958
1959 marchive << CHNVP(collision_system); // ChCollisionSystem should implement class factory for abstract create
1960
1961 marchive << CHNVP(timestepper); // ChTimestepper should implement class factory for abstract create
1962
1963 //***TODO*** complete...
1964 }
1965
1966 // Method to allow de serialization of transient data from archives.
ArchiveIN(ChArchiveIn & marchive)1967 void ChSystem::ArchiveIN(ChArchiveIn& marchive) {
1968 // version number
1969 /*int version =*/marchive.VersionRead<ChSystem>();
1970
1971 // deserialize unerlying assembly
1972 assembly.ArchiveIN(marchive);
1973
1974 // stream in all member data:
1975
1976 marchive >> CHNVP(contact_container);
1977
1978 marchive >> CHNVP(G_acc);
1979 marchive >> CHNVP(ch_time);
1980 marchive >> CHNVP(step);
1981 marchive >> CHNVP(step_min);
1982 marchive >> CHNVP(step_max);
1983 marchive >> CHNVP(stepcount);
1984 marchive >> CHNVP(dump_matrices);
1985
1986 marchive >> CHNVP(tol_force);
1987 marchive >> CHNVP(maxiter);
1988 marchive >> CHNVP(use_sleeping);
1989
1990 marchive >> CHNVP(descriptor);
1991 marchive >> CHNVP(solver);
1992
1993 marchive >> CHNVP(min_bounce_speed);
1994 marchive >> CHNVP(max_penetration_recovery_speed);
1995
1996 marchive >> CHNVP(collision_system); // ChCollisionSystem should implement class factory for abstract create
1997
1998 marchive >> CHNVP(timestepper); // ChTimestepper should implement class factory for abstract create
1999 timestepper->SetIntegrable(this);
2000
2001 //***TODO*** complete...
2002
2003 // Rebuild link pointers to markers
2004 Reference_LM_byID();
2005
2006 // Recompute statistics, offsets, etc.
2007 Setup();
2008 }
2009
2010 #define CH_CHUNK_START "Chrono binary file start"
2011 #define CH_CHUNK_END "Chrono binary file end"
2012
FileProcessChR(ChStreamInBinary & m_file)2013 int ChSystem::FileProcessChR(ChStreamInBinary& m_file) {
2014 std::string mchunk;
2015
2016 m_file >> mchunk;
2017 if (mchunk != CH_CHUNK_START)
2018 throw ChException("Not a ChR data file.");
2019
2020 // StreamINall(m_file);
2021
2022 m_file >> mchunk;
2023 if (mchunk != CH_CHUNK_END)
2024 throw ChException("The end of ChR data file is badly formatted.");
2025
2026 return 1;
2027 }
2028
FileWriteChR(ChStreamOutBinary & m_file)2029 int ChSystem::FileWriteChR(ChStreamOutBinary& m_file) {
2030 m_file << CH_CHUNK_START;
2031
2032 // StreamOUTall(m_file);
2033
2034 m_file << CH_CHUNK_END;
2035
2036 return 1;
2037 }
2038
2039 } // end namespace chrono
2040