From 6be125814b2c1c6c7ee3fa11cd60fb07bd0cb5e5 Mon Sep 17 00:00:00 2001
From: Henry Weller <http://cfd.direct>
Date: Tue, 12 Apr 2016 16:16:57 +0100
Subject: [PATCH] rigidBodyDynamics/rigidBodySolvers: Added support for the
integration of quaternion joints
---
.../rigidBodyModelState/rigidBodyModelState.C | 4 +--
.../rigidBodyMotion/rigidBodyMotion.H | 3 ++
.../rigidBodyMotion/rigidBodyMotionI.H | 7 ++++
.../CrankNicolson/CrankNicolson.C | 2 ++
.../rigidBodySolvers/Newmark/Newmark.C | 2 ++
.../rigidBodySolver/rigidBodySolver.C | 36 +++++++++++++++++++
.../rigidBodySolver/rigidBodySolver.H | 3 ++
.../rigidBodySolvers/symplectic/symplectic.C | 2 ++
8 files changed, 57 insertions(+), 2 deletions(-)
diff --git a/src/rigidBodyDynamics/rigidBodyModelState/rigidBodyModelState.C b/src/rigidBodyDynamics/rigidBodyModelState/rigidBodyModelState.C
index cad3a5ce04a..33ea779e7fc 100644
--- a/src/rigidBodyDynamics/rigidBodyModelState/rigidBodyModelState.C
+++ b/src/rigidBodyDynamics/rigidBodyModelState/rigidBodyModelState.C
@@ -33,7 +33,7 @@ Foam::RBD::rigidBodyModelState::rigidBodyModelState
)
:
q_(model.nDoF(), Zero),
- w_(model.nw(), Zero),
+ w_(model.nw(), 1),
qDot_(model.nDoF(), Zero),
qDdot_(model.nDoF(), Zero)
{}
@@ -46,7 +46,7 @@ Foam::RBD::rigidBodyModelState::rigidBodyModelState
)
:
q_(dict.lookupOrDefault("q", scalarField(model.nDoF(), Zero))),
- w_(dict.lookupOrDefault("w", scalarField(model.nw(), Zero))),
+ w_(dict.lookupOrDefault("w", scalarField(model.nw(), 1))),
qDot_(dict.lookupOrDefault("qDot", scalarField(model.nDoF(), Zero))),
qDdot_(dict.lookupOrDefault("qDdot", scalarField(model.nDoF(), Zero)))
{}
diff --git a/src/rigidBodyDynamics/rigidBodyMotion/rigidBodyMotion.H b/src/rigidBodyDynamics/rigidBodyMotion/rigidBodyMotion.H
index 398528a3b6d..b2e4562b80f 100644
--- a/src/rigidBodyDynamics/rigidBodyMotion/rigidBodyMotion.H
+++ b/src/rigidBodyDynamics/rigidBodyMotion/rigidBodyMotion.H
@@ -137,6 +137,9 @@ public:
//- Return the motion state
inline const rigidBodyModelState& state() const;
+ //- Return the motion state for modification
+ inline rigidBodyModelState& state();
+
// Edit
diff --git a/src/rigidBodyDynamics/rigidBodyMotion/rigidBodyMotionI.H b/src/rigidBodyDynamics/rigidBodyMotion/rigidBodyMotionI.H
index e9a430320df..0de439f807e 100644
--- a/src/rigidBodyDynamics/rigidBodyMotion/rigidBodyMotionI.H
+++ b/src/rigidBodyDynamics/rigidBodyMotion/rigidBodyMotionI.H
@@ -38,6 +38,13 @@ Foam::RBD::rigidBodyMotion::state() const
}
+inline Foam::RBD::rigidBodyModelState&
+Foam::RBD::rigidBodyMotion::state()
+{
+ return motionState_;
+}
+
+
inline void Foam::RBD::rigidBodyMotion::newTime()
{
motionState0_ = motionState_;
diff --git a/src/rigidBodyDynamics/rigidBodySolvers/CrankNicolson/CrankNicolson.C b/src/rigidBodyDynamics/rigidBodySolvers/CrankNicolson/CrankNicolson.C
index 895e9b9e610..6d48971be65 100644
--- a/src/rigidBodyDynamics/rigidBodySolvers/CrankNicolson/CrankNicolson.C
+++ b/src/rigidBodyDynamics/rigidBodySolvers/CrankNicolson/CrankNicolson.C
@@ -82,6 +82,8 @@ void Foam::RBD::rigidBodySolvers::CrankNicolson::solve
// Correct position
q() = q0() + deltaT()*(voc_*qDot() + (1 - voc_)*qDot0());
+ correctQuaternionJoints();
+
// Update the body-state
model_.forwardDynamicsCorrection(state());
}
diff --git a/src/rigidBodyDynamics/rigidBodySolvers/Newmark/Newmark.C b/src/rigidBodyDynamics/rigidBodySolvers/Newmark/Newmark.C
index 72e2eecea4c..2d5e5d90eaf 100644
--- a/src/rigidBodyDynamics/rigidBodySolvers/Newmark/Newmark.C
+++ b/src/rigidBodyDynamics/rigidBodySolvers/Newmark/Newmark.C
@@ -92,6 +92,8 @@ void Foam::RBD::rigidBodySolvers::Newmark::solve
+ deltaT()*qDot0()
+ sqr(deltaT())*beta_*qDdot() + sqr(deltaT())*(0.5 - beta_)*qDdot0();
+ correctQuaternionJoints();
+
// Update the body-state
model_.forwardDynamicsCorrection(state());
}
diff --git a/src/rigidBodyDynamics/rigidBodySolvers/rigidBodySolver/rigidBodySolver.C b/src/rigidBodyDynamics/rigidBodySolvers/rigidBodySolver/rigidBodySolver.C
index 669d8c9cc35..7cfc931778e 100644
--- a/src/rigidBodyDynamics/rigidBodySolvers/rigidBodySolver/rigidBodySolver.C
+++ b/src/rigidBodyDynamics/rigidBodySolvers/rigidBodySolver/rigidBodySolver.C
@@ -52,4 +52,40 @@ Foam::RBD::rigidBodySolver::~rigidBodySolver()
{}
+// * * * * * * * * * * * * * * * Member Functions * * * * * * * * * * * * * //
+
+void Foam::RBD::rigidBodySolver::correctQuaternionJoints()
+{
+ if (model_.nw())
+ {
+ forAll (model_.joints(), i)
+ {
+ const label qi = model_.joints()[i].qIndex();
+
+ if (model_.joints()[i].nw() == 1)
+ {
+ // Calculate the change in the normalized quaternion axis
+ vector dv((q().block<vector>(qi) - q0().block<vector>(qi)));
+ scalar magDv = mag(dv);
+
+ if (magDv > SMALL)
+ {
+ // Calculate the quaternion corresponding to the change
+ quaternion dQuat(dv/magDv, cos(magDv), true);
+
+ // Transform the previous time quaternion
+ quaternion quat
+ (
+ normalize(model_.joints()[i](q0(), w0())*dQuat)
+ );
+
+ // Update the joint state
+ model_.joints()[i](quat, q(), w());
+ }
+ }
+ }
+ }
+}
+
+
// ************************************************************************* //
diff --git a/src/rigidBodyDynamics/rigidBodySolvers/rigidBodySolver/rigidBodySolver.H b/src/rigidBodyDynamics/rigidBodySolvers/rigidBodySolver/rigidBodySolver.H
index a1959991b11..0183a4be716 100644
--- a/src/rigidBodyDynamics/rigidBodySolvers/rigidBodySolver/rigidBodySolver.H
+++ b/src/rigidBodyDynamics/rigidBodySolvers/rigidBodySolver/rigidBodySolver.H
@@ -100,6 +100,9 @@ protected:
//- Acceleration damping coefficient (for steady-state simulations)
inline scalar aDamp() const;
+ //- Correct the quaternion joints based on the current change in q
+ void correctQuaternionJoints();
+
public:
diff --git a/src/rigidBodyDynamics/rigidBodySolvers/symplectic/symplectic.C b/src/rigidBodyDynamics/rigidBodySolvers/symplectic/symplectic.C
index 76807a32b22..18b78ad89f2 100644
--- a/src/rigidBodyDynamics/rigidBodySolvers/symplectic/symplectic.C
+++ b/src/rigidBodyDynamics/rigidBodySolvers/symplectic/symplectic.C
@@ -73,6 +73,8 @@ void Foam::RBD::rigidBodySolvers::symplectic::solve
qDot() = qDot0() + aDamp()*0.5*deltaT0()*qDdot();
q() = q0() + deltaT()*qDot();
+ correctQuaternionJoints();
+
// Update the body-state prior to the evaluation of the restraints
model_.forwardDynamicsCorrection(state());
--
GitLab