Commit 81eec80d authored by Henry Weller's avatar Henry Weller
Browse files

rigidBodyDynamics/rigidBodyModelState: New class to hold the motion state of the rigid-body model

This is a more convenient way of maintaining the state or multiple
states (for higher-order integration), storing, retrieving and passing
between processors.
parent 0825c1b9
......@@ -32,6 +32,7 @@ Description
#include "rigidBodyModel.H"
#include "masslessBody.H"
#include "rigidBodyModelState.H"
#include "sphere.H"
#include "joints.H"
#include "IFstream.H"
......@@ -86,10 +87,11 @@ int main(int argc, char *argv[])
Info<< pendulum << endl;
// Create the joint-space state fields
scalarField q(pendulum.nDoF(), Zero);
scalarField w(pendulum.nw(), Zero);
scalarField qDot(pendulum.nDoF(), Zero);
scalarField qDdot(pendulum.nDoF(), Zero);
rigidBodyModelState pendulumState(pendulum);
scalarField& q = pendulumState.q();
scalarField& qDot = pendulumState.qDot();
scalarField& qDdot = pendulumState.qDdot();
scalarField tau(pendulum.nDoF(), Zero);
// Set the angle of the pendulum to 0.3rad
......@@ -106,15 +108,7 @@ int main(int argc, char *argv[])
qDot += 0.5*deltaT*qDdot;
q += deltaT*qDot;
pendulum.forwardDynamics
(
q,
w,
qDot,
tau,
Field<spatialVector>(),
qDdot
);
pendulum.forwardDynamics(pendulumState, tau, Field<spatialVector>());
qDot += 0.5*deltaT*qDdot;
......
......@@ -34,6 +34,7 @@ Description
#include "sphere.H"
#include "joints.H"
#include "rigidBodyRestraint.H"
#include "rigidBodyModelState.H"
#include "IFstream.H"
#include "OFstream.H"
......@@ -50,10 +51,11 @@ int main(int argc, char *argv[])
Info<< spring << endl;
// Create the joint-space state fields
scalarField q(spring.nDoF(), Zero);
scalarField w(spring.nw(), Zero);
scalarField qDot(spring.nDoF(), Zero);
scalarField qDdot(spring.nDoF(), Zero);
rigidBodyModelState springState(spring);
scalarField& q = springState.q();
scalarField& qDot = springState.qDot();
scalarField& qDdot = springState.qDdot();
scalarField tau(spring.nDoF(), Zero);
Field<spatialVector> fx(spring.nBodies(), Zero);
......@@ -68,13 +70,7 @@ int main(int argc, char *argv[])
q += deltaT*qDot;
// Update the body-state prior to the evaluation of the restraints
spring.forwardDynamicsCorrection
(
q,
w,
qDot,
qDdot
);
spring.forwardDynamicsCorrection(springState);
// Accumulate the restraint forces
fx = Zero;
......@@ -82,15 +78,7 @@ int main(int argc, char *argv[])
// Calculate the body acceleration for the given state
// and restraint forces
spring.forwardDynamics
(
q,
w,
qDot,
tau,
fx,
qDdot
);
spring.forwardDynamics(springState, tau, fx);
// Update the velocity
qDot += 0.5*deltaT*qDdot;
......
......@@ -35,4 +35,7 @@ restraints/sphericalAngularDamper/sphericalAngularDamper.C
rigidBodyModel/rigidBodyModel.C
rigidBodyModel/forwardDynamics.C
rigidBodyModelState/rigidBodyModelState.C
rigidBodyModelState/rigidBodyModelStateIO.C
LIB = $(FOAM_LIBBIN)/librigidBodyDynamics
......@@ -24,6 +24,7 @@ License
\*---------------------------------------------------------------------------*/
#include "rigidBodyModel.H"
#include "rigidBodyModelState.H"
#include "rigidBodyRestraint.H"
// * * * * * * * * * * * * * * Member Functions * * * * * * * * * * * * * * //
......@@ -47,14 +48,16 @@ void Foam::RBD::rigidBodyModel::applyRestraints(Field<spatialVector>& fx) const
void Foam::RBD::rigidBodyModel::forwardDynamics
(
const scalarField& q,
const scalarField& w,
const scalarField& qDot,
rigidBodyModelState& state,
const scalarField& tau,
const Field<spatialVector>& fx,
scalarField& qDdot
const Field<spatialVector>& fx
) const
{
const scalarField& q = state.q();
const scalarField& w = state.w();
const scalarField& qDot = state.qDot();
scalarField& qDdot = state.qDdot();
DebugInFunction
<< "q = " << q << nl
<< "qDot = " << qDot << nl
......@@ -199,14 +202,16 @@ void Foam::RBD::rigidBodyModel::forwardDynamics
void Foam::RBD::rigidBodyModel::forwardDynamicsCorrection
(
const scalarField& q,
const scalarField& w,
const scalarField& qDot,
const scalarField& qDdot
const rigidBodyModelState& state
) const
{
DebugInFunction << endl;
const scalarField& q = state.q();
const scalarField& w = state.w();
const scalarField& qDot = state.qDot();
const scalarField& qDdot = state.qDdot();
// Joint state returned by jcalc
joint::XSvc J;
......
......@@ -63,10 +63,12 @@ namespace RBD
// Forward declaration of friend functions and operators
class rigidBodyModel;
class restraint;
Ostream& operator<<(Ostream&, const rigidBodyModel&);
class rigidBodyModelState;
class restraint;
/*---------------------------------------------------------------------------*\
Class rigidBodyModel Declaration
......@@ -336,24 +338,15 @@ public:
// algorithm (Section 7.3 and Table 7.1)
void forwardDynamics
(
const scalarField& q,
const scalarField& w,
const scalarField& qDot,
rigidBodyModelState& state,
const scalarField& tau,
const Field<spatialVector>& fx,
scalarField& qDdot
const Field<spatialVector>& fx
) const;
//- Correct the velocity and acceleration of the bodies in the model
// from the given joint state fields following an integration step
// of the forwardDynamics
void forwardDynamicsCorrection
(
const scalarField& q,
const scalarField& w,
const scalarField& qDot,
const scalarField& qDdot
) const;
void forwardDynamicsCorrection(const rigidBodyModelState& state) const;
//- Write
virtual void write(Ostream&) const;
......
/*---------------------------------------------------------------------------*\
========= |
\\ / F ield | OpenFOAM: The Open Source CFD Toolbox
\\ / O peration |
\\ / A nd | Copyright (C) 2016 OpenFOAM Foundation
\\/ M anipulation |
-------------------------------------------------------------------------------
License
This file is part of OpenFOAM.
OpenFOAM is free software: you can redistribute it and/or modify it
under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
for more details.
You should have received a copy of the GNU General Public License
along with OpenFOAM. If not, see <http://www.gnu.org/licenses/>.
\*---------------------------------------------------------------------------*/
#include "rigidBodyModelState.H"
// * * * * * * * * * * * * * * * * Constructors * * * * * * * * * * * * * * //
Foam::RBD::rigidBodyModelState::rigidBodyModelState
(
const rigidBodyModel& model
)
:
q_(model.nDoF(), Zero),
w_(model.nw(), Zero),
qDot_(model.nDoF(), Zero),
qDdot_(model.nDoF(), Zero)
{}
Foam::RBD::rigidBodyModelState::rigidBodyModelState
(
const rigidBodyModel& model,
const dictionary& dict
)
:
q_(dict.lookupOrDefault("q", scalarField(model.nDoF(), Zero))),
w_(dict.lookupOrDefault("w", scalarField(model.nw(), Zero))),
qDot_(dict.lookupOrDefault("qDot", scalarField(model.nDoF(), Zero))),
qDdot_(dict.lookupOrDefault("qDdot", scalarField(model.nDoF(), Zero)))
{}
// ************************************************************************* //
/*---------------------------------------------------------------------------*\
========= |
\\ / F ield | OpenFOAM: The Open Source CFD Toolbox
\\ / O peration |
\\ / A nd | Copyright (C) 2016 OpenFOAM Foundation
\\/ M anipulation |
-------------------------------------------------------------------------------
License
This file is part of OpenFOAM.
OpenFOAM is free software: you can redistribute it and/or modify it
under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
for more details.
You should have received a copy of the GNU General Public License
along with OpenFOAM. If not, see <http://www.gnu.org/licenses/>.
Class
Foam::rigidBodyModelState
Description
Holds the motion state of rigid-body model.
SourceFiles
rigidBodyModelStateI.H
rigidBodyModelState.C
rigidBodyModelStateIO.C
\*---------------------------------------------------------------------------*/
#ifndef rigidBodyModelState_H
#define rigidBodyModelState_H
#include "rigidBodyModel.H"
#include "scalarField.H"
#include "dictionary.H"
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
namespace Foam
{
// Forward declaration of classes
class Istream;
class Ostream;
namespace RBD
{
// Forward declaration of friend functions and operators
class rigidBodyModelState;
Istream& operator>>(Istream&, rigidBodyModelState&);
Ostream& operator<<(Ostream&, const rigidBodyModelState&);
/*---------------------------------------------------------------------------*\
Class rigidBodyModelState Declaration
\*---------------------------------------------------------------------------*/
class rigidBodyModelState
{
// Private data
//- Joint position and orientation
scalarField q_;
//- Joint quaternion
scalarField w_;
//- Joint velocity
scalarField qDot_;
//- Joint acceleration
scalarField qDdot_;
public:
// Constructors
//- Construct for the given rigidBodyModel
rigidBodyModelState(const rigidBodyModel& model);
//- Construct from dictionary for the given rigidBodyModel
rigidBodyModelState
(
const rigidBodyModel& model,
const dictionary& dict
);
// Member Functions
// Access
//- Return access to the joint position and orientation
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;
//- Return access to the joint acceleration
inline const scalarField& qDdot() const;
// Edit
//- Return access to the joint position and orientation
inline scalarField& q();
//- Return access to the joint quaternion
inline scalarField& w();
//- Return access to the joint velocity
inline scalarField& qDot();
//- Return access to the joint acceleration
inline scalarField& qDdot();
//- Write to dictionary
void write(dictionary& dict) const;
//- Write to stream
void write(Ostream&) const;
// IOstream Operators
friend Istream& operator>>(Istream&, rigidBodyModelState&);
friend Ostream& operator<<(Ostream&, const rigidBodyModelState&);
};
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
} // End namespace RBD
} // End namespace Foam
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
#include "rigidBodyModelStateI.H"
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
#endif
// ************************************************************************* //
/*---------------------------------------------------------------------------*\
========= |
\\ / F ield | OpenFOAM: The Open Source CFD Toolbox
\\ / O peration |
\\ / A nd | Copyright (C) 2016 OpenFOAM Foundation
\\/ M anipulation |
-------------------------------------------------------------------------------
License
This file is part of OpenFOAM.
OpenFOAM is free software: you can redistribute it and/or modify it
under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
for more details.
You should have received a copy of the GNU General Public License
along with OpenFOAM. If not, see <http://www.gnu.org/licenses/>.
\*---------------------------------------------------------------------------*/
// * * * * * * * * * * * * * * * Member Functions * * * * * * * * * * * * * //
inline const Foam::scalarField& Foam::RBD::rigidBodyModelState::q() const
{
return q_;
}
inline const Foam::scalarField& Foam::RBD::rigidBodyModelState::w() const
{
return w_;
}
inline const Foam::scalarField& Foam::RBD::rigidBodyModelState::qDot() const
{
return qDot_;
}
inline const Foam::scalarField& Foam::RBD::rigidBodyModelState::qDdot() const
{
return qDdot_;
}
inline Foam::scalarField& Foam::RBD::rigidBodyModelState::q()
{
return q_;
}
inline Foam::scalarField& Foam::RBD::rigidBodyModelState::w()
{
return w_;
}
inline Foam::scalarField& Foam::RBD::rigidBodyModelState::qDot()
{
return qDot_;
}
inline Foam::scalarField& Foam::RBD::rigidBodyModelState::qDdot()
{
return qDdot_;
}
// ************************************************************************* //
/*---------------------------------------------------------------------------*\
========= |
\\ / F ield | OpenFOAM: The Open Source CFD Toolbox
\\ / O peration |
\\ / A nd | Copyright (C) 2016 OpenFOAM Foundation
\\/ M anipulation |
-------------------------------------------------------------------------------
License
This file is part of OpenFOAM.
OpenFOAM is free software: you can redistribute it and/or modify it
under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
for more details.
You should have received a copy of the GNU General Public License
along with OpenFOAM. If not, see <http://www.gnu.org/licenses/>.
\*---------------------------------------------------------------------------*/
#include "rigidBodyModelState.H"
#include "IOstreams.H"
// * * * * * * * * * * * * * * Member Functions * * * * * * * * * * * * * * //
void Foam::RBD::rigidBodyModelState::write(dictionary& dict) const
{
dict.add("q", q_);
dict.add("w", w_);
dict.add("qDot", qDot_);
dict.add("qDdot", qDdot_);
}
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;
}
// * * * * * * * * * * * * * * * IOstream Operators * * * * * * * * * * * * //
Foam::Istream& Foam::RBD::operator>>
(
Istream& is,
rigidBodyModelState& state
)
{
is >> state.q_
>> state.w_
>> state.qDot_
>> state.qDdot_;
// Check state of Istream
is.check
(
"Foam::Istream& Foam::operator>>"
"(Foam::Istream&, Foam::RBD::rigidBodyModelState&)"
);
return is;
}
Foam::Ostream& Foam::RBD::operator<<
(
Ostream& os,
const rigidBodyModelState& state
)
{
os << token::SPACE << state.q_
<< token::SPACE << state.w_
<< token::SPACE << state.qDot_
<< token::SPACE << state.qDdot_;
// Check state of Ostream
os.check
(
"Foam::Ostream& Foam::operator<<(Foam::Ostream&, "
"const Foam::RBD::rigidBodyModelState&)"
);
return os;
}
// ************************************************************************* //
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment