zhyang-dev/openfoam
1
0
Fork 0
mirror of https://develop.openfoam.com/Development/openfoam.git synced 2025-11-28 03:28:01 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

rigidBodyDynamics: Simplify handling of quaternions by maintaining a unit quaternion in the joint state field 'q'

Browse Source 
'w' is now obtained from 'v' using the relation w = sqrt(1 - |sqr(v)|)
and 'v' is stored in the joint state field 'q' and integrated in the
usual manner but corrected using quaternion transformations.
This commit is contained in:
Henry Weller
2016-04-12 21:44:34 +01:00
parent 2d2ecf91dc
commit 315a1baad3
41 changed files with 13 additions and 90 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
applications/test/rigidBodyDynamics/sphericalJoint/sphericalJoint.C
View File

@ -64,8 +64,7 @@ int main(int argc, char *argv[])
sphericalJoint.joints()[1]
(
quaternion(quaternion::ZYX, vector(0.3, 0, 0)),
sphericalJoint.state().q(),
sphericalJoint.state().w()
sphericalJoint.state().q()
);
// Set the gravitational acceleration
@ -90,8 +89,7 @@ int main(int argc, char *argv[])
<< t << " "
<< sphericalJoint.joints()[1]
(
sphericalJoint.state().q(),
sphericalJoint.state().w()
sphericalJoint.state().q()
).eulerAngles(quaternion::ZYX).x()
<< endl;
}

1
src/rigidBodyDynamics/joints/Pa/Pa.C
View File

@ -85,7 +85,6 @@ void Foam::RBD::joints::Pa::jcalc
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const
{

1
src/rigidBodyDynamics/joints/Pa/Pa.H
View File

@ -92,7 +92,6 @@ public:
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const;

1
src/rigidBodyDynamics/joints/Px/Px.C
View File

@ -84,7 +84,6 @@ void Foam::RBD::joints::Px::jcalc
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const
{

1
src/rigidBodyDynamics/joints/Px/Px.H
View File

@ -92,7 +92,6 @@ public:
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const;
};

1
src/rigidBodyDynamics/joints/Pxyz/Pxyz.C
View File

@ -88,7 +88,6 @@ void Foam::RBD::joints::Pxyz::jcalc
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const
{

1
src/rigidBodyDynamics/joints/Pxyz/Pxyz.H
View File

@ -92,7 +92,6 @@ public:
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const;
};

1
src/rigidBodyDynamics/joints/Py/Py.C
View File

@ -84,7 +84,6 @@ void Foam::RBD::joints::Py::jcalc
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const
{

1
src/rigidBodyDynamics/joints/Py/Py.H
View File

@ -92,7 +92,6 @@ public:
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const;
};

1
src/rigidBodyDynamics/joints/Pz/Pz.C
View File

@ -84,7 +84,6 @@ void Foam::RBD::joints::Pz::jcalc
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const
{

1
src/rigidBodyDynamics/joints/Pz/Pz.H
View File

@ -92,7 +92,6 @@ public:
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const;
};

1
src/rigidBodyDynamics/joints/Ra/Ra.C
View File

@ -85,7 +85,6 @@ void Foam::RBD::joints::Ra::jcalc
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const
{

1
src/rigidBodyDynamics/joints/Ra/Ra.H
View File

@ -92,7 +92,6 @@ public:
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const;

3
src/rigidBodyDynamics/joints/Rs/Rs.C
View File

@ -94,11 +94,10 @@ void Foam::RBD::joints::Rs::jcalc
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const
{
J.X.E() = operator()(q, w).R().T();
J.X.E() = operator()(q).R().T();
J.X.r() = Zero;
J.S = Zero;

1
src/rigidBodyDynamics/joints/Rs/Rs.H
View File

@ -97,7 +97,6 @@ public:
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const;
};

1
src/rigidBodyDynamics/joints/Rx/Rx.C
View File

@ -84,7 +84,6 @@ void Foam::RBD::joints::Rx::jcalc
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const
{

1
src/rigidBodyDynamics/joints/Rx/Rx.H
View File

@ -92,7 +92,6 @@ public:
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const;
};

1
src/rigidBodyDynamics/joints/Rxyz/Rxyz.C
View File

@ -88,7 +88,6 @@ void Foam::RBD::joints::Rxyz::jcalc
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const
{

1
src/rigidBodyDynamics/joints/Rxyz/Rxyz.H
View File

@ -93,7 +93,6 @@ public:
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const;
};

1
src/rigidBodyDynamics/joints/Ry/Ry.C
View File

@ -84,7 +84,6 @@ void Foam::RBD::joints::Ry::jcalc
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const
{

1
src/rigidBodyDynamics/joints/Ry/Ry.H
View File

@ -92,7 +92,6 @@ public:
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const;
};

1
src/rigidBodyDynamics/joints/Ryxz/Ryxz.C
View File

@ -88,7 +88,6 @@ void Foam::RBD::joints::Ryxz::jcalc
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const
{

1
src/rigidBodyDynamics/joints/Ryxz/Ryxz.H
View File

@ -93,7 +93,6 @@ public:
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const;
};

1
src/rigidBodyDynamics/joints/Rz/Rz.C
View File

@ -84,7 +84,6 @@ void Foam::RBD::joints::Rz::jcalc
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const
{

1
src/rigidBodyDynamics/joints/Rz/Rz.H
View File

@ -92,7 +92,6 @@ public:
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const;
};

1
src/rigidBodyDynamics/joints/Rzyx/Rzyx.C
View File

@ -88,7 +88,6 @@ void Foam::RBD::joints::Rzyx::jcalc
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const
{

1
src/rigidBodyDynamics/joints/Rzyx/Rzyx.H
View File

@ -93,7 +93,6 @@ public:
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const;
};

3
src/rigidBodyDynamics/joints/composite/compositeJoint.C
View File

@ -96,11 +96,10 @@ void Foam::RBD::joints::composite::jcalc
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const
{
last().jcalc(J, q, w, qDot);
last().jcalc(J, q, qDot);
}

1
src/rigidBodyDynamics/joints/composite/compositeJoint.H
View File

@ -109,7 +109,6 @@ public:
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const;

7
src/rigidBodyDynamics/joints/joint/joint.H
View File

@ -236,7 +236,6 @@ public:
(
XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const = 0;
@ -250,8 +249,7 @@ public:
// if it uses a quaternion representation for rotation
inline quaternion operator()
(
const scalarField& q,
const scalarField& w
const scalarField& q
) const;
//- Set the state quaternion for this joint
@ -259,8 +257,7 @@ public:
inline void operator()
(
const quaternion& quat,
scalarField& q,
scalarField& w
scalarField& q
) const;

9
src/rigidBodyDynamics/joints/joint/jointI.H
View File

@ -71,8 +71,7 @@ inline const Foam::List<Foam::spatialVector>& Foam::RBD::joint::S() const
inline Foam::quaternion Foam::RBD::joint::operator()
(
const scalarField& q,
const scalarField& w
const scalarField& q
) const
{
if (nw() != 1)
@ -82,15 +81,14 @@ inline Foam::quaternion Foam::RBD::joint::operator()
<< abort(FatalError);
}
return quaternion(w[wIndex_], q.block<vector>(qIndex_));
return quaternion::unit(q.block<vector>(qIndex_));
}
inline void Foam::RBD::joint::operator()
(
const quaternion& quat,
scalarField& q,
scalarField& w
scalarField& q
) const
{
if (nw() != 1)
@ -100,7 +98,6 @@ inline void Foam::RBD::joint::operator()
<< abort(FatalError);
}
w[wIndex_] = quat.w();
q[qIndex_] = quat.v().x();
q[qIndex_+1] = quat.v().y();
q[qIndex_+2] = quat.v().z();

1
src/rigidBodyDynamics/joints/null/nullJoint.C
View File

@ -80,7 +80,6 @@ void Foam::RBD::joints::null::jcalc
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const
{

1
src/rigidBodyDynamics/joints/null/nullJoint.H
View File

@ -92,7 +92,6 @@ public:
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const;
};

6
src/rigidBodyDynamics/rigidBodyModel/forwardDynamics.C
View File

@ -54,7 +54,6 @@ void Foam::RBD::rigidBodyModel::forwardDynamics
) const
{
const scalarField& q = state.q();
const scalarField& w = state.w();
const scalarField& qDot = state.qDot();
scalarField& qDdot = state.qDdot();
@ -71,7 +70,7 @@ void Foam::RBD::rigidBodyModel::forwardDynamics
for (label i=1; i<nBodies(); i++)
{
const joint& jnt = joints()[i];
jnt.jcalc(J, q, w, qDot);
jnt.jcalc(J, q, qDot);
S_[i] = J.S;
S1_[i] = J.S1;
@ -208,7 +207,6 @@ void Foam::RBD::rigidBodyModel::forwardDynamicsCorrection
DebugInFunction << endl;
const scalarField& q = state.q();
const scalarField& w = state.w();
const scalarField& qDot = state.qDot();
const scalarField& qDdot = state.qDdot();
@ -223,7 +221,7 @@ void Foam::RBD::rigidBodyModel::forwardDynamicsCorrection
const joint& jnt = joints()[i];
const label qi = jnt.qIndex();
jnt.jcalc(J, q, w, qDot);
jnt.jcalc(J, q, qDot);
S_[i] = J.S;
S1_[i] = J.S1;

2
src/rigidBodyDynamics/rigidBodyModelState/rigidBodyModelState.C
View File

@ -33,7 +33,6 @@ Foam::RBD::rigidBodyModelState::rigidBodyModelState
)
:
q_(model.nDoF(), Zero),
w_(model.nw(), 1),
qDot_(model.nDoF(), Zero),
qDdot_(model.nDoF(), Zero)
{}
@ -46,7 +45,6 @@ Foam::RBD::rigidBodyModelState::rigidBodyModelState
)
:
q_(dict.lookupOrDefault("q", scalarField(model.nDoF(), 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)))
{}

5
src/rigidBodyDynamics/rigidBodyModelState/rigidBodyModelState.H
View File

@ -70,9 +70,6 @@ class rigidBodyModelState
//- Joint position and orientation
scalarField q_;
//- Joint quaternion
scalarField w_;
//- Joint velocity
scalarField qDot_;
@ -103,7 +100,6 @@ public:
inline const scalarField& q() const;
//- Return access to the joint quaternion
inline const scalarField& w() const;
//- Return access to the joint velocity
inline const scalarField& qDot() const;
@ -118,7 +114,6 @@ public:
inline scalarField& q();
//- Return access to the joint quaternion
inline scalarField& w();
//- Return access to the joint velocity
inline scalarField& qDot();

12
src/rigidBodyDynamics/rigidBodyModelState/rigidBodyModelStateI.H
View File

@ -31,12 +31,6 @@ inline const Foam::scalarField& Foam::RBD::rigidBodyModelState::q() const
}
inline const Foam::scalarField& Foam::RBD::rigidBodyModelState::w() const
{
return w_;
}
inline const Foam::scalarField& Foam::RBD::rigidBodyModelState::qDot() const
{
return qDot_;
@ -55,12 +49,6 @@ inline Foam::scalarField& Foam::RBD::rigidBodyModelState::q()
}
inline Foam::scalarField& Foam::RBD::rigidBodyModelState::w()
{
return w_;
}
inline Foam::scalarField& Foam::RBD::rigidBodyModelState::qDot()
{
return qDot_;

4
src/rigidBodyDynamics/rigidBodyModelState/rigidBodyModelStateIO.C
View File

@ -31,7 +31,6 @@ License
void Foam::RBD::rigidBodyModelState::write(dictionary& dict) const
{
dict.add("q", q_);
dict.add("w", w_);
dict.add("qDot", qDot_);
dict.add("qDdot", qDdot_);
}
@ -40,7 +39,6 @@ void Foam::RBD::rigidBodyModelState::write(dictionary& dict) const
void Foam::RBD::rigidBodyModelState::write(Ostream& os) const
{
os.writeKeyword("q") << q_ << token::END_STATEMENT << nl;
os.writeKeyword("w") << w_ << token::END_STATEMENT << nl;
os.writeKeyword("qDot") << qDot_ << token::END_STATEMENT << nl;
os.writeKeyword("qDdot") << qDdot_ << token::END_STATEMENT << nl;
}
@ -55,7 +53,6 @@ Foam::Istream& Foam::RBD::operator>>
)
{
is >> state.q_
>> state.w_
>> state.qDot_
>> state.qDdot_;
@ -77,7 +74,6 @@ Foam::Ostream& Foam::RBD::operator<<
)
{
os << token::SPACE << state.q_
<< token::SPACE << state.w_
<< token::SPACE << state.qDot_
<< token::SPACE << state.qDdot_;

4
src/rigidBodyDynamics/rigidBodySolvers/rigidBodySolver/rigidBodySolver.C
View File

@ -76,11 +76,11 @@ void Foam::RBD::rigidBodySolver::correctQuaternionJoints()
// Transform the previous time quaternion
quaternion quat
(
normalize(model_.joints()[i](q0(), w0())*dQuat)
normalize(model_.joints()[i](q0())*dQuat)
);
// Update the joint state
model_.joints()[i](quat, q(), w());
model_.joints()[i](quat, q());
}
}
}

2
src/rigidBodyDynamics/rigidBodySolvers/rigidBodySolver/rigidBodySolver.H
View File

@ -75,7 +75,6 @@ protected:
inline scalarField& q();
//- Return the current joint quaternion
inline scalarField& w();
//- Return the current joint velocity
inline scalarField& qDot();
@ -88,7 +87,6 @@ protected:
inline const scalarField& q0() const;
//- Return the current joint quaternion
inline const scalarField& w0() const;
//- Return the current joint velocity
inline const scalarField& qDot0() const;

12
src/rigidBodyDynamics/rigidBodySolvers/rigidBodySolver/rigidBodySolverI.H
View File

@ -56,12 +56,6 @@ inline Foam::scalarField& Foam::RBD::rigidBodySolver::q()
}
inline Foam::scalarField& Foam::RBD::rigidBodySolver::w()
{
return state().w();
}
inline Foam::scalarField& Foam::RBD::rigidBodySolver::qDot()
{
return state().qDot();
@ -80,12 +74,6 @@ inline const Foam::scalarField& Foam::RBD::rigidBodySolver::q0() const
}
inline const Foam::scalarField& Foam::RBD::rigidBodySolver::w0() const
{
return state0().w();
}
inline const Foam::scalarField& Foam::RBD::rigidBodySolver::qDot0() const
{
return state0().qDot();