From c69132dd2c375da02095a37bd8134a5b8b68870e Mon Sep 17 00:00:00 2001 From: jibril-b-coulibaly Date: Sun, 23 Feb 2025 20:11:33 -0700 Subject: [PATCH 1/8] add and precompute rest length to Spring element. Refactor force calculation to limit number of expensive length calculations --- src/chrono/fea/ChElementSpring.cpp | 39 ++++++++++++++++++------------ src/chrono/fea/ChElementSpring.h | 5 ++-- 2 files changed, 27 insertions(+), 17 deletions(-) diff --git a/src/chrono/fea/ChElementSpring.cpp b/src/chrono/fea/ChElementSpring.cpp index 213f1cfa9c..a74655a0a7 100644 --- a/src/chrono/fea/ChElementSpring.cpp +++ b/src/chrono/fea/ChElementSpring.cpp @@ -13,11 +13,12 @@ // ============================================================================= #include "chrono/fea/ChElementSpring.h" +#include "chrono/core/ChVector3.h" namespace chrono { namespace fea { -ChElementSpring::ChElementSpring() : spring_k(1.0), damper_r(0.01) { +ChElementSpring::ChElementSpring() : spring_k(1.0), damper_r(0.01), length(0) { nodes.resize(2); } @@ -38,6 +39,10 @@ void ChElementSpring::GetStateBlock(ChVectorDynamic<>& mD) { mD.segment(3, 3) = this->nodes[1]->GetPos().eigen(); } +void ChElementSpring::SetupInitial(ChSystem* system) { + this->length = (nodes[1]->GetX0() - nodes[0]->GetX0()).Length(); +} + void ChElementSpring::ComputeKRMmatricesGlobal(ChMatrixRef H, double Kfactor, double Rfactor, double Mfactor) { assert((H.rows() == 6) && (H.cols() == 6)); @@ -59,7 +64,7 @@ void ChElementSpring::ComputeKRMmatricesGlobal(ChMatrixRef H, double Kfactor, do // add geometric stiffness - in future it might become an option to switch off if not needed. // See for ex. http://shodhbhagirathi.iitr.ac.in:8081/jspui/handle/123456789/8433 pag. 14-15 if (true) { - double L_ref = (nodes[1]->GetX0() - nodes[0]->GetX0()).Length(); + double L_ref = length; double L = (nodes[1]->GetPos() - nodes[0]->GetPos()).Length(); double internal_Kforce_local = this->spring_k * (L - L_ref); @@ -76,26 +81,30 @@ void ChElementSpring::ComputeKRMmatricesGlobal(ChMatrixRef H, double Kfactor, do void ChElementSpring::ComputeInternalForces(ChVectorDynamic<>& Fi) { assert(Fi.size() == 6); - ChVector3d dir = (nodes[1]->GetPos() - nodes[0]->GetPos()).GetNormalized(); - double L_ref = (nodes[1]->GetX0() - nodes[0]->GetX0()).Length(); - double L = (nodes[1]->GetPos() - nodes[0]->GetPos()).Length(); - double L_dt = Vdot((nodes[1]->GetPosDt() - nodes[0]->GetPosDt()), dir); + ChVector3d dPos = nodes[1]->GetPos() - nodes[0]->GetPos(); + double L_ref = length; + double L = dPos.Length(); + double Linv = 1.0 / L; + ChVector3d dV = nodes[1]->GetPosDt() - nodes[0]->GetPosDt(); double internal_Kforce_local = this->spring_k * (L - L_ref); - double internal_Rforce_local = this->damper_r * L_dt; - double internal_force_local = internal_Kforce_local + internal_Rforce_local; - ChVector3d int_forceA = dir * internal_force_local; - ChVector3d int_forceB = -dir * internal_force_local; + double internal_Rforce_local = this->damper_r * Vdot(dV, dPos) * Linv; + double internal_force_local = (internal_Kforce_local + internal_Rforce_local) * Linv; + ChVector3d int_forceA = dPos * internal_force_local; + ChVector3d int_forceB = -dPos * internal_force_local; Fi.segment(0, 3) = int_forceA.eigen(); Fi.segment(3, 3) = int_forceB.eigen(); } double ChElementSpring::GetCurrentForce() { - ChVector3d dir = (nodes[1]->GetPos() - nodes[0]->GetPos()).GetNormalized(); - double L_ref = (nodes[1]->GetX0() - nodes[0]->GetX0()).Length(); - double L = (nodes[1]->GetPos() - nodes[0]->GetPos()).Length(); - double L_dt = Vdot((nodes[1]->GetPosDt() - nodes[0]->GetPosDt()), dir); + // TODO: This is only used in demo_FEA_truss.cpp to mimick failure when the force it too high + // Otherwise not necessary. Think of implementing failure mechanisms / max stress/strain criterion + ChVector3d dPos = nodes[1]->GetPos() - nodes[0]->GetPos(); + double L_ref = length; + double L = dPos.Length(); + double Linv = 1.0 / L; + ChVector3d dV = nodes[1]->GetPosDt() - nodes[0]->GetPosDt(); double internal_Kforce_local = this->spring_k * (L - L_ref); - double internal_Rforce_local = this->damper_r * L_dt; + double internal_Rforce_local = this->damper_r * Vdot(dV, dPos) * Linv; return internal_Kforce_local + internal_Rforce_local; } diff --git a/src/chrono/fea/ChElementSpring.h b/src/chrono/fea/ChElementSpring.h index a1daf13c61..bea4f468c1 100644 --- a/src/chrono/fea/ChElementSpring.h +++ b/src/chrono/fea/ChElementSpring.h @@ -81,14 +81,15 @@ class ChApi ChElementSpring : public ChElementGeneric { // (***not needed, thank to bookkeeping in parent class ChElementGeneric) private: - /// Initial setup. + /// Initial setup. Precompute rest length /// No override needed for the spring element because global K is computed on-the-fly in /// ComputeAddKRmatricesGlobal() - ////virtual void SetupInitial(ChSystem* system) override {} + virtual void SetupInitial(ChSystem* system) override; std::vector > nodes; double spring_k; double damper_r; + double length; }; /// @} fea_elements From e28b12582bdd9e39bff0a698836f49307240d99c Mon Sep 17 00:00:00 2001 From: jibril-b-coulibaly Date: Sun, 23 Feb 2025 20:41:00 -0700 Subject: [PATCH 2/8] ChElementBar: store stiffness k=EA/l instead of calculating it every step. Refactor force calculations --- src/chrono/fea/ChElementBar.cpp | 42 ++++++++++++++++-------------- src/chrono/fea/ChElementBar.h | 7 ++--- src/chrono/fea/ChElementSpring.cpp | 2 +- 3 files changed, 27 insertions(+), 24 deletions(-) diff --git a/src/chrono/fea/ChElementBar.cpp b/src/chrono/fea/ChElementBar.cpp index 52f263b7a8..28225c09e7 100644 --- a/src/chrono/fea/ChElementBar.cpp +++ b/src/chrono/fea/ChElementBar.cpp @@ -17,7 +17,7 @@ namespace chrono { namespace fea { -ChElementBar::ChElementBar() : area(0.01 * 0.01), density(1000), E(0.01e9), rdamping(0.01), mass(0), length(0) { +ChElementBar::ChElementBar() : area(0.01 * 0.01), density(1000), E(0.01e9), rdamping(0.01), kstiff(0), mass(0), length(0) { nodes.resize(2); } @@ -54,10 +54,8 @@ void ChElementBar::ComputeKRMmatricesGlobal(ChMatrixRef H, double Kfactor, doubl ChVector3d dir = (nodes[1]->GetPos() - nodes[0]->GetPos()).GetNormalized(); ChVectorN dircolumn = dir.eigen(); - double Kstiffness = ((this->area * this->E) / this->length); - double Rdamping = this->rdamping * Kstiffness; // note that stiffness and damping matrices are the same, so join stuff here - double commonfactor = Kstiffness * Kfactor + Rdamping * Rfactor; + double commonfactor = this->kstiff * Kfactor + (this->rdamping * this->kstiff) * Rfactor; // TODO: the damping part depends on velocity, is 1/dt passed to Rfactor ? Or should 1/dt be inside the damping matrix calculation here? ChMatrix33<> V = dircolumn * dircolumn.transpose(); ChMatrix33<> keV = commonfactor * V; @@ -69,10 +67,9 @@ void ChElementBar::ComputeKRMmatricesGlobal(ChMatrixRef H, double Kfactor, doubl // add geometric stiffness - in future it might become an option to switch off if not needed. // See for ex. http://shodhbhagirathi.iitr.ac.in:8081/jspui/handle/123456789/8433 pag. 14-15 if (true) { - double L_ref = (nodes[1]->GetX0() - nodes[0]->GetX0()).Length(); + double L_ref = this->length; double L = (nodes[1]->GetPos() - nodes[0]->GetPos()).Length(); - double Kstiffness1 = ((this->area * this->E) / this->length); - double internal_Kforce_local = Kstiffness1 * (L - L_ref); + double internal_Kforce_local = this->kstiff * (L - L_ref); ChMatrix33<> kgV = Kfactor * (internal_Kforce_local / L_ref) * (ChMatrix33<>(1) - V); H.block(0, 0, 3, 3) += kgV; @@ -94,29 +91,34 @@ void ChElementBar::SetupInitial(ChSystem* system) { // Compute rest length, mass: this->length = (nodes[1]->GetX0() - nodes[0]->GetX0()).Length(); this->mass = this->length * this->area * this->density; + this->kstiff = this->area * this->E / this->length; } void ChElementBar::ComputeInternalForces(ChVectorDynamic<>& Fi) { assert(Fi.size() == 6); - ChVector3d dir = (nodes[1]->GetPos() - nodes[0]->GetPos()).GetNormalized(); - double internal_force_local = GetCurrentForce(); - ChVector3d int_forceA = dir * internal_force_local; - ChVector3d int_forceB = -dir * internal_force_local; - + ChVector3d dPos = nodes[1]->GetPos() - nodes[0]->GetPos(); + double L_ref = length; + double L = dPos.Length(); + double Linv = 1.0 / L; + ChVector3d dV = nodes[1]->GetPosDt() - nodes[0]->GetPosDt(); + double internal_Kforce_local = this->kstiff * (L - L_ref); + double internal_Rforce_local = (this->rdamping * this->kstiff) * Vdot(dV, dPos) * Linv; + double internal_force_local = (internal_Kforce_local + internal_Rforce_local) * Linv; + ChVector3d int_forceA = dPos * internal_force_local; + ChVector3d int_forceB = -dPos * internal_force_local; Fi.segment(0, 3) = int_forceA.eigen(); Fi.segment(3, 3) = int_forceB.eigen(); } double ChElementBar::GetCurrentForce() { - ChVector3d dir = (nodes[1]->GetPos() - nodes[0]->GetPos()).GetNormalized(); - double L_ref = (nodes[1]->GetX0() - nodes[0]->GetX0()).Length(); - double L = (nodes[1]->GetPos() - nodes[0]->GetPos()).Length(); - double L_dt = Vdot((nodes[1]->GetPosDt() - nodes[0]->GetPosDt()), dir); - double Kstiffness = ((this->area * this->E) / this->length); - double Rdamping = this->rdamping * Kstiffness; - double internal_Kforce_local = Kstiffness * (L - L_ref); - double internal_Rforce_local = Rdamping * L_dt; + ChVector3d dPos = nodes[1]->GetPos() - nodes[0]->GetPos(); + double L_ref = length; + double L = dPos.Length(); + double Linv = 1.0 / L; + ChVector3d dV = nodes[1]->GetPosDt() - nodes[0]->GetPosDt(); + double internal_Kforce_local = this->kstiff * (L - L_ref); + double internal_Rforce_local = (this->rdamping * this->kstiff) * Vdot(dV, dPos) * Linv; return internal_Kforce_local + internal_Rforce_local; } diff --git a/src/chrono/fea/ChElementBar.h b/src/chrono/fea/ChElementBar.h index 8f7fb49bbd..acb6540ba4 100644 --- a/src/chrono/fea/ChElementBar.h +++ b/src/chrono/fea/ChElementBar.h @@ -71,7 +71,7 @@ class ChApi ChElementBar : public ChElementGeneric { // Custom properties functions // - /// Set the cross sectional area of the bar (m^2) (also changes stiffness keeping same E modulus) + /// Set the cross sectional area of the bar (m^2) void SetArea(double ma) { this->area = ma; } double GetArea() { return this->area; } @@ -79,7 +79,7 @@ class ChApi ChElementBar : public ChElementGeneric { void SetDensity(double md) { this->density = md; } double GetDensity() { return this->density; } - /// Set the Young elastic modulus (N/m^2) (also sets stiffness) + /// Set the Young elastic modulus (N/m^2) void SetYoungModulus(double mE) { this->E = mE; } double GetYoungModulus() { return this->E; } @@ -111,7 +111,7 @@ class ChApi ChElementBar : public ChElementGeneric { // (***not needed, thank to bookkeeping in parent class ChElementGeneric) private: - /// Initial setup. Precompute mass and matrices that do not change during the simulation, such as the local tangent + /// Initial setup. Precompute mass, rest length, axial stiffness and matrices that do not change during the simulation, such as the local tangent /// stiffness Kl of each element, if needed, etc. virtual void SetupInitial(ChSystem* system) override; @@ -120,6 +120,7 @@ class ChApi ChElementBar : public ChElementGeneric { double density; double E; double rdamping; + double kstiff; double mass; double length; }; diff --git a/src/chrono/fea/ChElementSpring.cpp b/src/chrono/fea/ChElementSpring.cpp index a74655a0a7..ebc9b9a4db 100644 --- a/src/chrono/fea/ChElementSpring.cpp +++ b/src/chrono/fea/ChElementSpring.cpp @@ -52,7 +52,7 @@ void ChElementSpring::ComputeKRMmatricesGlobal(ChMatrixRef H, double Kfactor, do ChVectorN dircolumn = dir.eigen(); // note that stiffness and damping matrices are the same, so join stuff here - double commonfactor = this->spring_k * Kfactor + this->damper_r * Rfactor; + double commonfactor = this->spring_k * Kfactor + this->damper_r * Rfactor; // TODO: the damping part depends on velocity, is 1/dt passed to Rfactor ? Or should 1/dt be inside the damping matrix calculation here? ChMatrix33<> V = dircolumn * dircolumn.transpose(); ChMatrix33<> keV = commonfactor * V; From b7902eb8430f186843c4ea58291dfdcf4e8a19e2 Mon Sep 17 00:00:00 2001 From: jibril-b-coulibaly Date: Wed, 26 Feb 2025 17:00:40 -0700 Subject: [PATCH 3/8] define Euler beam quaternion upon building This is done for simplicity but I also think there might be a bug in SetupInitial(). When the beam is created from 2 nodes, or 1 node and 1 position, the end nodes may have different Y axes, and there is no reason why this Y axis should correspond to that of the beam element since those are determined by who created these nodes. --- src/chrono/fea/ChBuilderBeam.cpp | 6 ++++++ src/chrono/fea/ChElementBeamEuler.cpp | 13 ++++++------- src/chrono/fea/ChElementBeamEuler.h | 4 ++++ 3 files changed, 16 insertions(+), 7 deletions(-) diff --git a/src/chrono/fea/ChBuilderBeam.cpp b/src/chrono/fea/ChBuilderBeam.cpp index 4d280529a3..1080922b71 100644 --- a/src/chrono/fea/ChBuilderBeam.cpp +++ b/src/chrono/fea/ChBuilderBeam.cpp @@ -54,6 +54,8 @@ void ChBuilderBeamEuler::BuildBeam(std::shared_ptr mesh, // element->SetNodes(beam_nodes[i - 1], beam_nodes[i]); element->SetSection(sect); + + element->SetRefRotation(mrot.GetQuaternion()); } } @@ -96,6 +98,8 @@ void ChBuilderBeamEuler::BuildBeam(std::shared_ptr mesh, // element->SetNodeBreferenceRot(elrot.GetConjugate() * element->GetNodeB()->Frame().GetRot()); element->SetSection(sect); + + element->SetRefRotation(mrot.GetQuaternion()); } } @@ -135,6 +139,8 @@ void ChBuilderBeamEuler::BuildBeam(std::shared_ptr mesh, // // std::cout << " Qa=" << element->GetNodeAreferenceRot() << std::endl; // std::cout << " Qb=" << element->GetNodeBreferenceRot() << std::endl << std::endl; element->SetSection(sect); + + element->SetRefRotation(mrot.GetQuaternion()); } } diff --git a/src/chrono/fea/ChElementBeamEuler.cpp b/src/chrono/fea/ChElementBeamEuler.cpp index b49a4c1151..1a4c1e5d3e 100644 --- a/src/chrono/fea/ChElementBeamEuler.cpp +++ b/src/chrono/fea/ChElementBeamEuler.cpp @@ -15,6 +15,10 @@ // #define BEAM_VERBOSE #include +#include +#include "chrono/core/ChQuaternion.h" +#include "chrono/core/ChVector3.h" +#include "chrono/fea/ChNodeFEAxyzrot.h" #include "chrono/fea/ChElementBeamEuler.h" @@ -444,13 +448,8 @@ void ChElementBeamEuler::SetupInitial(ChSystem* system) { this->length = (nodes[1]->GetX0().GetPos() - nodes[0]->GetX0().GetPos()).Length(); this->mass = this->length * this->section->GetMassPerUnitLength(); - // Compute initial rotation - ChMatrix33<> A0; - ChVector3d mXele = nodes[1]->GetX0().GetPos() - nodes[0]->GetX0().GetPos(); - ChVector3d myele = - (nodes[0]->GetX0().GetRotMat().GetAxisY() + nodes[1]->GetX0().GetRotMat().GetAxisY()).GetNormalized(); - A0.SetFromAxisX(mXele, myele); - q_element_ref_rot = A0.GetQuaternion(); + // Set initial rotation + q_element_abs_rot = q_element_ref_rot; // Compute local stiffness matrix: ComputeStiffnessMatrix(); diff --git a/src/chrono/fea/ChElementBeamEuler.h b/src/chrono/fea/ChElementBeamEuler.h index d302c57389..161890365e 100644 --- a/src/chrono/fea/ChElementBeamEuler.h +++ b/src/chrono/fea/ChElementBeamEuler.h @@ -15,6 +15,7 @@ #ifndef CHELEMENTBEAMEULER_H #define CHELEMENTBEAMEULER_H +#include "chrono/core/ChQuaternion.h" #include "chrono/fea/ChBeamSectionEuler.h" #include "chrono/fea/ChElementBeam.h" #include "chrono/fea/ChElementCorotational.h" @@ -84,6 +85,9 @@ class ChApi ChElementBeamEuler : public ChElementBeam, /// the accumulated rotation from starting point. ChQuaternion<> GetAbsoluteRotation() { return q_element_abs_rot; } + /// Set the original reference rotation of element in space + void SetRefRotation(ChQuaternion<> q_initial) { q_element_ref_rot = q_initial; } + /// Get the original reference rotation of element in space ChQuaternion<> GetRefRotation() { return q_element_ref_rot; } From 57530fc058afe772f83d048050369b410543b760 Mon Sep 17 00:00:00 2001 From: jibril-b-coulibaly Date: Wed, 26 Feb 2025 17:08:08 -0700 Subject: [PATCH 4/8] refactor Euler beam UpdateRotation() do nothing if corotate is disabled. use cheaper quaternion representation of rotation rather than matrix multiplication --- src/chrono/fea/ChElementBeamEuler.cpp | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/src/chrono/fea/ChElementBeamEuler.cpp b/src/chrono/fea/ChElementBeamEuler.cpp index 1a4c1e5d3e..8ba53f2e8d 100644 --- a/src/chrono/fea/ChElementBeamEuler.cpp +++ b/src/chrono/fea/ChElementBeamEuler.cpp @@ -97,24 +97,23 @@ void ChElementBeamEuler::Update() { } void ChElementBeamEuler::UpdateRotation() { - ChMatrix33<> A0(this->q_element_ref_rot); + if (!this->disable_corotate) { + ChMatrix33<> Aabs; + ChQuaternion<> q_element_ref_rot_previous(q_element_ref_rot); - ChMatrix33<> Aabs; - if (this->disable_corotate) { - Aabs = A0; - q_element_abs_rot = q_element_ref_rot; - } else { ChVector3d mXele_w = nodes[1]->Frame().GetPos() - nodes[0]->Frame().GetPos(); // propose Y_w as absolute dir of the Y axis of A node, removing the effect of Aref-to-A rotation if any: // Y_w = [R Aref->w ]*[R Aref->A ]'*{0,1,0} - ChVector3d myele_wA = nodes[0]->Frame().GetRot().Rotate(q_refrotA.RotateBack(ChVector3d(0, 1, 0))); + ChVector3d myele_wA = nodes[0]->Frame().GetRot().Rotate(q_refrotA.RotateBack(ChVector3d(0, 1, 0))); // TODO: q_refrotA.RotateBack(ChVector3d(0, 1, 0)) dodes not change through the sim and is expensive to compute: store it // propose Y_w as absolute dir of the Y axis of B node, removing the effect of Bref-to-B rotation if any: // Y_w = [R Bref->w ]*[R Bref->B ]'*{0,1,0} - ChVector3d myele_wB = nodes[1]->Frame().GetRot().Rotate(q_refrotB.RotateBack(ChVector3d(0, 1, 0))); + ChVector3d myele_wB = nodes[1]->Frame().GetRot().Rotate(q_refrotB.RotateBack(ChVector3d(0, 1, 0))); // TODO: q_refrotB.RotateBack(ChVector3d(0, 1, 0)) dodes not change through the sim and is expensive to compute: store it // Average the two Y directions to have midpoint torsion (ex -30?torsion A and +30?torsion B= 0? ChVector3d myele_w = (myele_wA + myele_wB).GetNormalized(); Aabs.SetFromAxisX(mXele_w, myele_w); q_element_abs_rot = Aabs.GetQuaternion(); + + this->A.SetFromQuaternion(q_element_ref_rot_previous.GetConjugate() * q_element_abs_rot); } this->A = A0.transpose() * Aabs; From f6b06bf9a1ee52fbbbeba838a5642cda76987a10 Mon Sep 17 00:00:00 2001 From: jibril-b-coulibaly Date: Wed, 26 Feb 2025 17:12:26 -0700 Subject: [PATCH 5/8] refactor ChElementBeamEuler::GetStateBlock() with lambdas to avoid code duplication --- src/chrono/fea/ChElementBeamEuler.cpp | 58 ++++++++++++--------------- 1 file changed, 25 insertions(+), 33 deletions(-) diff --git a/src/chrono/fea/ChElementBeamEuler.cpp b/src/chrono/fea/ChElementBeamEuler.cpp index 8ba53f2e8d..a41eabd312 100644 --- a/src/chrono/fea/ChElementBeamEuler.cpp +++ b/src/chrono/fea/ChElementBeamEuler.cpp @@ -122,40 +122,32 @@ void ChElementBeamEuler::UpdateRotation() { void ChElementBeamEuler::GetStateBlock(ChVectorDynamic<>& mD) { mD.resize(12); - ChVector3d delta_rot_dir; - double delta_rot_angle; - - // Node 0, displacement (in local element frame, corotated back) - // d = [Atw]' Xt - [A0w]'X0 - ChVector3d displ = this->q_element_abs_rot.RotateBack(nodes[0]->Frame().GetPos()) - - this->q_element_ref_rot.RotateBack(nodes[0]->GetX0().GetPos()); - mD.segment(0, 3) = displ.eigen(); - - // Node 0, x,y,z small rotations (in local element frame) - ChQuaternion<> q_delta0 = q_element_abs_rot.GetConjugate() * nodes[0]->Frame().GetRot() * q_refrotA.GetConjugate(); + // Node displacement in local element frame + // d = [Atw]' Xt - [A0w]'X0 + auto getDisplacement = [&](std::shared_ptr node) { + return (q_element_abs_rot.RotateBack(node->Frame().GetPos()) - + q_element_ref_rot.RotateBack(node->GetX0().GetPos())); + }; + + // Node small rotations in local element frame // note, for small incremental rotations this is opposite of ChNodeFEAxyzrot::VariablesQbIncrementPosition - q_delta0.GetAngleAxis(delta_rot_angle, delta_rot_dir); - - if (delta_rot_angle > CH_PI) - delta_rot_angle -= CH_2PI; // no 0..360 range, use -180..+180 - - mD.segment(3, 3) = delta_rot_angle * delta_rot_dir.eigen(); - - // Node 1, displacement (in local element frame, corotated back) - // d = [Atw]' Xt - [A0w]'X0 - displ = this->q_element_abs_rot.RotateBack(nodes[1]->Frame().GetPos()) - - this->q_element_ref_rot.RotateBack(nodes[1]->GetX0().GetPos()); - mD.segment(6, 3) = displ.eigen(); - - // Node 1, x,y,z small rotations (in local element frame) - ChQuaternion<> q_delta1 = q_element_abs_rot.GetConjugate() * nodes[1]->Frame().GetRot() * q_refrotB.GetConjugate(); - // note, for small incremental rotations this is opposite of ChNodeFEAxyzrot::VariablesQbIncrementPosition - q_delta1.GetAngleAxis(delta_rot_angle, delta_rot_dir); - - if (delta_rot_angle > CH_PI) - delta_rot_angle -= CH_2PI; // no 0..360 range, use -180..+180 - - mD.segment(9, 3) = delta_rot_angle * delta_rot_dir.eigen(); + auto getRotation = [&](std::shared_ptr node, ChQuaternion<> q_elemref_to_noderef) { + ChVector3d delta_rot_dir; + double delta_rot_angle; + ChQuaternion<> q_delta = q_element_abs_rot.GetConjugate() * node->Frame().GetRot() * q_elemref_to_noderef.GetConjugate(); + // TODO: GetAngleAxis already returns in the range [-PI..PI], no need to change range + // TODO: the previous range was only shifted if delta_rot_angle > CH_PI. How about delta_rot_angle < - CH_PI ? Was that a bug? + // TODO: We may want to use GetRotVec directly, but the angle is not within [-PI .. PI] + // TODO: Consider changing GetRotVec() to return within [-PI .. PI] + q_delta.GetAngleAxis(delta_rot_angle, delta_rot_dir); + return delta_rot_angle * delta_rot_dir; + }; + + mD.segment(0, 3) = getDisplacement(nodes[0]).eigen(); + mD.segment(3, 3) = getRotation(nodes[0], q_refrotA).eigen(); + + mD.segment(6, 3) = getDisplacement(nodes[1]).eigen(); + mD.segment(9, 3) = getRotation(nodes[1], q_refrotB).eigen(); } void ChElementBeamEuler::GetFieldDt(ChVectorDynamic<>& mD_dt) { From 45037bd3a299d8c735ce21fc6e15dcdb05e06519 Mon Sep 17 00:00:00 2001 From: jibril-b-coulibaly Date: Thu, 27 Feb 2025 11:10:15 -0700 Subject: [PATCH 6/8] forgot to remove A matrix old update --- src/chrono/fea/ChElementBeamEuler.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/chrono/fea/ChElementBeamEuler.cpp b/src/chrono/fea/ChElementBeamEuler.cpp index a41eabd312..a1810b5c96 100644 --- a/src/chrono/fea/ChElementBeamEuler.cpp +++ b/src/chrono/fea/ChElementBeamEuler.cpp @@ -115,8 +115,6 @@ void ChElementBeamEuler::UpdateRotation() { this->A.SetFromQuaternion(q_element_ref_rot_previous.GetConjugate() * q_element_abs_rot); } - - this->A = A0.transpose() * Aabs; } void ChElementBeamEuler::GetStateBlock(ChVectorDynamic<>& mD) { From 64f9ce603c9dedebd7d6be97ccf6dbaac73d0fe5 Mon Sep 17 00:00:00 2001 From: jibril-b-coulibaly Date: Thu, 27 Feb 2025 11:21:05 -0700 Subject: [PATCH 7/8] rename quaternions for rotation from element reference to node reference for clarity. store unchanged values that were computed each step --- src/chrono/fea/ChBuilderBeam.cpp | 16 +++++++--------- src/chrono/fea/ChElementBeamEuler.cpp | 18 ++++++++++-------- src/chrono/fea/ChElementBeamEuler.h | 22 ++++++++++++++-------- 3 files changed, 31 insertions(+), 25 deletions(-) diff --git a/src/chrono/fea/ChBuilderBeam.cpp b/src/chrono/fea/ChBuilderBeam.cpp index 1080922b71..10632d08fe 100644 --- a/src/chrono/fea/ChBuilderBeam.cpp +++ b/src/chrono/fea/ChBuilderBeam.cpp @@ -94,12 +94,12 @@ void ChBuilderBeamEuler::BuildBeam(std::shared_ptr mesh, // element->SetNodes(beam_nodes[i - 1], beam_nodes[i]); ChQuaternion<> elrot = mrot.GetQuaternion(); - element->SetNodeAreferenceRot(elrot.GetConjugate() * element->GetNodeA()->Frame().GetRot()); - element->SetNodeBreferenceRot(elrot.GetConjugate() * element->GetNodeB()->Frame().GetRot()); + element->SetRefRotation(elrot); + element->SetElemToNodeArefRot(elrot.GetConjugate() * element->GetNodeA()->Frame().GetRot()); + element->SetElemToNodeBrefRot(elrot.GetConjugate() * element->GetNodeB()->Frame().GetRot()); element->SetSection(sect); - element->SetRefRotation(mrot.GetQuaternion()); } } @@ -133,14 +133,12 @@ void ChBuilderBeamEuler::BuildBeam(std::shared_ptr mesh, // element->SetNodes(beam_nodes[i - 1], beam_nodes[i]); ChQuaternion<> elrot = mrot.GetQuaternion(); - element->SetNodeAreferenceRot(elrot.GetConjugate() * element->GetNodeA()->Frame().GetRot()); - element->SetNodeBreferenceRot(elrot.GetConjugate() * element->GetNodeB()->Frame().GetRot()); - // std::cout << "Element n." << i << " with rotations:" << std::endl; - // std::cout << " Qa=" << element->GetNodeAreferenceRot() << std::endl; - // std::cout << " Qb=" << element->GetNodeBreferenceRot() << std::endl << std::endl; + element->SetRefRotation(elrot); + element->SetElemToNodeArefRot(elrot.GetConjugate() * element->GetNodeA()->Frame().GetRot()); + element->SetElemToNodeBrefRot(elrot.GetConjugate() * element->GetNodeB()->Frame().GetRot()); + element->SetSection(sect); - element->SetRefRotation(mrot.GetQuaternion()); } } diff --git a/src/chrono/fea/ChElementBeamEuler.cpp b/src/chrono/fea/ChElementBeamEuler.cpp index a1810b5c96..9dce573e2a 100644 --- a/src/chrono/fea/ChElementBeamEuler.cpp +++ b/src/chrono/fea/ChElementBeamEuler.cpp @@ -26,8 +26,10 @@ namespace chrono { namespace fea { ChElementBeamEuler::ChElementBeamEuler() - : q_refrotA(QUNIT), - q_refrotB(QUNIT), + : q_elemref_to_nodeAref_conj(QUNIT), + q_elemref_to_nodeBref_conj(QUNIT), + yabs_in_elemref_to_nodeAref_frame(0, 1, 0), + yabs_in_elemref_to_nodeBref_frame(0, 1, 0), q_element_abs_rot(QUNIT), q_element_ref_rot(QUNIT), disable_corotate(false), @@ -104,10 +106,10 @@ void ChElementBeamEuler::UpdateRotation() { ChVector3d mXele_w = nodes[1]->Frame().GetPos() - nodes[0]->Frame().GetPos(); // propose Y_w as absolute dir of the Y axis of A node, removing the effect of Aref-to-A rotation if any: // Y_w = [R Aref->w ]*[R Aref->A ]'*{0,1,0} - ChVector3d myele_wA = nodes[0]->Frame().GetRot().Rotate(q_refrotA.RotateBack(ChVector3d(0, 1, 0))); // TODO: q_refrotA.RotateBack(ChVector3d(0, 1, 0)) dodes not change through the sim and is expensive to compute: store it + ChVector3d myele_wA = nodes[0]->Frame().GetRot().Rotate(yabs_in_elemref_to_nodeAref_frame); // propose Y_w as absolute dir of the Y axis of B node, removing the effect of Bref-to-B rotation if any: // Y_w = [R Bref->w ]*[R Bref->B ]'*{0,1,0} - ChVector3d myele_wB = nodes[1]->Frame().GetRot().Rotate(q_refrotB.RotateBack(ChVector3d(0, 1, 0))); // TODO: q_refrotB.RotateBack(ChVector3d(0, 1, 0)) dodes not change through the sim and is expensive to compute: store it + ChVector3d myele_wB = nodes[1]->Frame().GetRot().Rotate(yabs_in_elemref_to_nodeBref_frame); // Average the two Y directions to have midpoint torsion (ex -30?torsion A and +30?torsion B= 0? ChVector3d myele_w = (myele_wA + myele_wB).GetNormalized(); Aabs.SetFromAxisX(mXele_w, myele_w); @@ -129,10 +131,10 @@ void ChElementBeamEuler::GetStateBlock(ChVectorDynamic<>& mD) { // Node small rotations in local element frame // note, for small incremental rotations this is opposite of ChNodeFEAxyzrot::VariablesQbIncrementPosition - auto getRotation = [&](std::shared_ptr node, ChQuaternion<> q_elemref_to_noderef) { + auto getRotation = [&](std::shared_ptr node, ChQuaternion<> q_elemref_to_noderef_conj) { ChVector3d delta_rot_dir; double delta_rot_angle; - ChQuaternion<> q_delta = q_element_abs_rot.GetConjugate() * node->Frame().GetRot() * q_elemref_to_noderef.GetConjugate(); + ChQuaternion<> q_delta = q_element_abs_rot.GetConjugate() * node->Frame().GetRot() * q_elemref_to_noderef_conj; // TODO: GetAngleAxis already returns in the range [-PI..PI], no need to change range // TODO: the previous range was only shifted if delta_rot_angle > CH_PI. How about delta_rot_angle < - CH_PI ? Was that a bug? // TODO: We may want to use GetRotVec directly, but the angle is not within [-PI .. PI] @@ -142,10 +144,10 @@ void ChElementBeamEuler::GetStateBlock(ChVectorDynamic<>& mD) { }; mD.segment(0, 3) = getDisplacement(nodes[0]).eigen(); - mD.segment(3, 3) = getRotation(nodes[0], q_refrotA).eigen(); + mD.segment(3, 3) = getRotation(nodes[0], q_elemref_to_nodeAref_conj).eigen(); mD.segment(6, 3) = getDisplacement(nodes[1]).eigen(); - mD.segment(9, 3) = getRotation(nodes[1], q_refrotB).eigen(); + mD.segment(9, 3) = getRotation(nodes[1], q_elemref_to_nodeBref_conj).eigen(); } void ChElementBeamEuler::GetFieldDt(ChVectorDynamic<>& mD_dt) { diff --git a/src/chrono/fea/ChElementBeamEuler.h b/src/chrono/fea/ChElementBeamEuler.h index 161890365e..0605830d54 100644 --- a/src/chrono/fea/ChElementBeamEuler.h +++ b/src/chrono/fea/ChElementBeamEuler.h @@ -72,13 +72,17 @@ class ChApi ChElementBeamEuler : public ChElementBeam, /// Get the second node (ending) std::shared_ptr GetNodeB() { return nodes[1]; } - /// Set the reference rotation of nodeA respect to the element rotation. - void SetNodeAreferenceRot(ChQuaternion<> mrot) { q_refrotA = mrot; } - ChQuaternion<> GetNodeAreferenceRot() { return q_refrotA; } + /// Set the reference rotation of nodeA with respect to the element rotation. + void SetElemToNodeArefRot(ChQuaternion<> mrot) { + q_elemref_to_nodeAref_conj = mrot.GetConjugate(); + yabs_in_elemref_to_nodeAref_frame = mrot.RotateBack(ChVector3d(0, 1, 0)); + } - /// Set the reference rotation of nodeB respect to the element rotation. - void SetNodeBreferenceRot(ChQuaternion<> mrot) { q_refrotB = mrot; } - ChQuaternion<> GetNodeBreferenceRot() { return q_refrotB; } + /// Set the reference rotation of nodeB with respect to the element rotation. + void SetElemToNodeBrefRot(ChQuaternion<> mrot) { + q_elemref_to_nodeBref_conj = mrot.GetConjugate(); + yabs_in_elemref_to_nodeBref_frame = mrot.RotateBack(ChVector3d(0, 1, 0)); + } /// Get the absolute rotation of element in space /// This is not the same of Rotation() , that expresses @@ -286,8 +290,10 @@ class ChApi ChElementBeamEuler : public ChElementBeam, ChMatrixDynamic<> Km; ///< local material stiffness matrix ChMatrixDynamic<> Kg; ///< local geometric stiffness matrix NORMALIZED by P - ChQuaternion<> q_refrotA; - ChQuaternion<> q_refrotB; + ChQuaternion<> q_elemref_to_nodeAref_conj; + ChQuaternion<> q_elemref_to_nodeBref_conj; + ChVector3d yabs_in_elemref_to_nodeAref_frame; + ChVector3d yabs_in_elemref_to_nodeBref_frame; ChQuaternion<> q_element_abs_rot; ChQuaternion<> q_element_ref_rot; From 51541761364988a62982efe77764998f0019c483 Mon Sep 17 00:00:00 2001 From: jibril-b-coulibaly Date: Tue, 4 Mar 2025 00:13:48 -0700 Subject: [PATCH 8/8] cosmetic clean up and cheaper local displacement calculation when corotational is off --- src/chrono/fea/ChElementBeamEuler.cpp | 49 +++++++++++++++------------ 1 file changed, 27 insertions(+), 22 deletions(-) diff --git a/src/chrono/fea/ChElementBeamEuler.cpp b/src/chrono/fea/ChElementBeamEuler.cpp index 9dce573e2a..699234488e 100644 --- a/src/chrono/fea/ChElementBeamEuler.cpp +++ b/src/chrono/fea/ChElementBeamEuler.cpp @@ -94,29 +94,27 @@ void ChElementBeamEuler::Update() { // parent class update: ChElementGeneric::Update(); - // always keep updated the rotation matrix A: - this->UpdateRotation(); + // Update the rotation matrix A when necessary: + if (!this->disable_corotate) + this->UpdateRotation(); } void ChElementBeamEuler::UpdateRotation() { - if (!this->disable_corotate) { - ChMatrix33<> Aabs; - ChQuaternion<> q_element_ref_rot_previous(q_element_ref_rot); - - ChVector3d mXele_w = nodes[1]->Frame().GetPos() - nodes[0]->Frame().GetPos(); - // propose Y_w as absolute dir of the Y axis of A node, removing the effect of Aref-to-A rotation if any: - // Y_w = [R Aref->w ]*[R Aref->A ]'*{0,1,0} - ChVector3d myele_wA = nodes[0]->Frame().GetRot().Rotate(yabs_in_elemref_to_nodeAref_frame); - // propose Y_w as absolute dir of the Y axis of B node, removing the effect of Bref-to-B rotation if any: - // Y_w = [R Bref->w ]*[R Bref->B ]'*{0,1,0} - ChVector3d myele_wB = nodes[1]->Frame().GetRot().Rotate(yabs_in_elemref_to_nodeBref_frame); - // Average the two Y directions to have midpoint torsion (ex -30?torsion A and +30?torsion B= 0? - ChVector3d myele_w = (myele_wA + myele_wB).GetNormalized(); - Aabs.SetFromAxisX(mXele_w, myele_w); - q_element_abs_rot = Aabs.GetQuaternion(); - - this->A.SetFromQuaternion(q_element_ref_rot_previous.GetConjugate() * q_element_abs_rot); - } + ChMatrix33<> Aabs; + + ChVector3d mXele_w = nodes[1]->Frame().GetPos() - nodes[0]->Frame().GetPos(); + // propose Y_w as absolute dir of the Y axis of A node, removing the effect of Aref-to-A rotation if any: + // Y_w = [R Aref->w ]*[R Aref->A ]'*{0,1,0} + ChVector3d myele_wA = nodes[0]->Frame().GetRot().Rotate(yabs_in_elemref_to_nodeAref_frame); + // propose Y_w as absolute dir of the Y axis of B node, removing the effect of Bref-to-B rotation if any: + // Y_w = [R Bref->w ]*[R Bref->B ]'*{0,1,0} + ChVector3d myele_wB = nodes[1]->Frame().GetRot().Rotate(yabs_in_elemref_to_nodeBref_frame); + // Average the two Y directions to have midpoint torsion (ex -30?torsion A and +30?torsion B= 0? + ChVector3d myele_w = (myele_wA + myele_wB).GetNormalized(); + Aabs.SetFromAxisX(mXele_w, myele_w); + q_element_abs_rot = Aabs.GetQuaternion(); + + this->A.SetFromQuaternion(q_element_ref_rot.GetConjugate() * q_element_abs_rot); } void ChElementBeamEuler::GetStateBlock(ChVectorDynamic<>& mD) { @@ -124,9 +122,15 @@ void ChElementBeamEuler::GetStateBlock(ChVectorDynamic<>& mD) { // Node displacement in local element frame // d = [Atw]' Xt - [A0w]'X0 + // If corotational disabled, [Atw] == [A0w] and we can do a single rotation. auto getDisplacement = [&](std::shared_ptr node) { - return (q_element_abs_rot.RotateBack(node->Frame().GetPos()) - - q_element_ref_rot.RotateBack(node->GetX0().GetPos())); + ChVector3d local_disp; + if (disable_corotate) + local_disp = q_element_ref_rot.RotateBack(node->Frame().GetPos() - node->GetX0().GetPos()); + else + local_disp = q_element_abs_rot.RotateBack(node->Frame().GetPos()) - + q_element_ref_rot.RotateBack(node->GetX0().GetPos()); + return local_disp; }; // Node small rotations in local element frame @@ -134,6 +138,7 @@ void ChElementBeamEuler::GetStateBlock(ChVectorDynamic<>& mD) { auto getRotation = [&](std::shared_ptr node, ChQuaternion<> q_elemref_to_noderef_conj) { ChVector3d delta_rot_dir; double delta_rot_angle; + // If corotational disabled, no efficiency gain by rotating qt * q0^-1 into local frame. Use corotational formula ChQuaternion<> q_delta = q_element_abs_rot.GetConjugate() * node->Frame().GetRot() * q_elemref_to_noderef_conj; // TODO: GetAngleAxis already returns in the range [-PI..PI], no need to change range // TODO: the previous range was only shifted if delta_rot_angle > CH_PI. How about delta_rot_angle < - CH_PI ? Was that a bug?