zhyang-dev/openfoam
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

rigidBodyDynamics: New library providing support for the dynamics of n-DoF articulated bodies

Browse Source 
Based on the principles, algorithms, data structures and notation
presented in the book:

    Featherstone, R. (2008).
    Rigid body dynamics algorithms.
    Springer.

This development is sponsored by Carnegie Wave Energy Ltd.
This commit is contained in:
Henry Weller
2016-04-03 15:48:59 +01:00
parent 9d0ab178b0
commit 55b457a0ca
48 changed files with 5580 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
src/rigidBodyDynamics/Make/files Normal file
View File

@ -0,0 +1,25 @@
bodies/rigidBody/rigidBody.C
bodies/masslessBody/masslessBody.C
joints/joint/joint.C
joints/nullJoint/nullJoint.C
joints/Rx/Rx.C
joints/Ry/Ry.C
joints/Rz/Rz.C
joints/Ra/Ra.C
joints/Rs/Rs.C
joints/Rzyx/Rzyx.C
joints/Rxyz/Rxyz.C
joints/Ryxz/Ryxz.C
joints/Px/Px.C
joints/Py/Py.C
joints/Pz/Pz.C
joints/Pa/Pa.C
joints/Pxyz/Pxyz.C
rigidBodyModel/rigidBodyModel.C
rigidBodyModel/forwardDynamics.C
LIB = $(FOAM_LIBBIN)/librigidBodyDynamics

3
src/rigidBodyDynamics/Make/options Normal file
View File

@ -0,0 +1,3 @@
EXE_INC =
LIB_LIBS =

42
src/rigidBodyDynamics/bodies/masslessBody/masslessBody.C Normal file
View File

@ -0,0 +1,42 @@
/*---------------------------------------------------------------------------*\
========= |
\\ / 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 "masslessBody.H"
// * * * * * * * * * * * * * * * * Destructor * * * * * * * * * * * * * * * //
Foam::RBD::masslessBody::~masslessBody()
{}
// * * * * * * * * * * * * * * Member Functions * * * * * * * * * * * * * * //
bool Foam::RBD::masslessBody::massless() const
{
return true;
}
// ************************************************************************* //

106
src/rigidBodyDynamics/bodies/masslessBody/masslessBody.H Normal file
View File

@ -0,0 +1,106 @@
/*---------------------------------------------------------------------------*\3
========= |
\\ / 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::masslessBody
Description
SourceFiles
masslessBodyI.H
masslessBody.C
\*---------------------------------------------------------------------------*/
#ifndef RBD_masslessBody_H
#define RBD_masslessBody_H
#include "rigidBody.H"
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
namespace Foam
{
// Forward declaration of Foam classes
class Istream;
class Ostream;
namespace RBD
{
// Forward declaration of friend functions and operators
class masslessBody;
Ostream& operator<<(Ostream&, const masslessBody&);
/*---------------------------------------------------------------------------*\
Class masslessBody Declaration
\*---------------------------------------------------------------------------*/
class masslessBody
:
public rigidBody
{
public:
// Constructors
//- Construct a massless body
inline masslessBody();
//- Construct a named massless body
inline masslessBody(const word& name);
//- Destructor
virtual ~masslessBody();
// Member Functions
//- Return true if this body is a massless component of a composite body
virtual bool massless() const;
// IOstream Operators
//friend Ostream& operator<<(Ostream&, const masslessBody&);
};
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
} // End namespace RBD
} // End namespace Foam
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
#include "masslessBodyI.H"
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
#endif
// ************************************************************************* //

40
src/rigidBodyDynamics/bodies/masslessBody/masslessBodyI.H Normal file
View File

@ -0,0 +1,40 @@
/*---------------------------------------------------------------------------*\
========= |
\\ / 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/>.
\*---------------------------------------------------------------------------*/
// * * * * * * * * * * * * * * * * Constructors * * * * * * * * * * * * * * //
inline Foam::RBD::masslessBody::masslessBody()
:
rigidBody("massless", rigidBodyInertia())
{}
inline Foam::RBD::masslessBody::masslessBody(const word& name)
:
rigidBody(name, rigidBodyInertia())
{}
// ************************************************************************* //

67
src/rigidBodyDynamics/bodies/rigidBody/rigidBody.C Normal file
View File

@ -0,0 +1,67 @@
/*---------------------------------------------------------------------------*\
========= |
\\ / 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 "rigidBody.H"
#include "subBody.H"
// * * * * * * * * * * * * * * * * Selectors * * * * * * * * * * * * * * * * //
Foam::autoPtr<Foam::RBD::rigidBody> Foam::RBD::rigidBody::New
(
const word& name,
const scalar& m,
const vector& c,
const symmTensor& Ic
)
{
return autoPtr<rigidBody>(new rigidBody(name, m, c, Ic));
}
// * * * * * * * * * * * * * * * * Destructor * * * * * * * * * * * * * * * //
Foam::RBD::rigidBody::~rigidBody()
{}
// * * * * * * * * * * * * * * Member Functions * * * * * * * * * * * * * * //
bool Foam::RBD::rigidBody::massless() const
{
return false;
}
void Foam::RBD::rigidBody::merge(const subBody& subBody)
{
*this = rigidBody
(
name(),
*this + transform(subBody.parentXT(), subBody)
);
}
// ************************************************************************* //

145
src/rigidBodyDynamics/bodies/rigidBody/rigidBody.H Normal file
View File

@ -0,0 +1,145 @@
/*---------------------------------------------------------------------------*\
========= |
\\ / 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::RBD::rigidBody
Description
SourceFiles
rigidBodyI.H
\*---------------------------------------------------------------------------*/
#ifndef RBD_rigidBody_H
#define RBD_rigidBody_H
#include "rigidBodyInertia.H"
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
namespace Foam
{
// Forward declaration of Foam classes
class Istream;
class Ostream;
namespace RBD
{
// Forward declaration of classes
class subBody;
// Forward declaration of friend functions and operators
class rigidBody;
Ostream& operator<<(Ostream&, const rigidBody&);
/*---------------------------------------------------------------------------*\
Class rigidBody Declaration
\*---------------------------------------------------------------------------*/
class rigidBody
:
public rigidBodyInertia
{
// Private data
//- Name of body
word name_;
public:
// Constructors
//- Construct from mass, centre of mass and moment of inertia tensor
// about the centre of mass
inline rigidBody
(
const word& name,
const scalar& m,
const vector& c,
const symmTensor& Ic
);
//- Construct from with the given rigidBodyInertia
inline rigidBody
(
const word& name,
const rigidBodyInertia& rbi
);
//- Return clone of this rigidBody
inline autoPtr<rigidBody> clone() const;
// Selectors
//- Select constructed from components
static autoPtr<rigidBody> New
(
const word& name,
const scalar& m,
const vector& c,
const symmTensor& Ic
);
//- Destructor
virtual ~rigidBody();
// Member Functions
//- Return name
inline const word& name() const;
//- Return false as this body is not massless
virtual bool massless() const;
//- Merge a body into this parent body
void merge(const subBody&);
// IOstream Operators
//friend Ostream& operator<<(Ostream&, const rigidBody&);
};
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
} // End namespace RBD
} // End namespace Foam
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
#include "rigidBodyI.H"
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
#endif
// ************************************************************************* //

66
src/rigidBodyDynamics/bodies/rigidBody/rigidBodyI.H Normal file
View File

@ -0,0 +1,66 @@
/*---------------------------------------------------------------------------*\
========= |
\\ / 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/>.
\*---------------------------------------------------------------------------*/
// * * * * * * * * * * * * * * * * Constructors * * * * * * * * * * * * * * //
inline Foam::RBD::rigidBody::rigidBody
(
const word& name,
const scalar& m,
const vector& c,
const symmTensor& Ic
)
:
rigidBodyInertia(m, c, Ic),
name_(name)
{}
inline Foam::RBD::rigidBody::rigidBody
(
const word& name,
const rigidBodyInertia& rbi
)
:
rigidBodyInertia(rbi),
name_(name)
{}
inline Foam::autoPtr<Foam::RBD::rigidBody> Foam::RBD::rigidBody::clone() const
{
return autoPtr<rigidBody>(new rigidBody(*this));
}
// * * * * * * * * * * * * * * * Member Functions * * * * * * * * * * * * * //
inline const Foam::word& Foam::RBD::rigidBody::name() const
{
return name_;
}
// ************************************************************************* //

125
src/rigidBodyDynamics/bodies/subBody/subBody.H Normal file
View File

@ -0,0 +1,125 @@
/*---------------------------------------------------------------------------*\
========= |
\\ / 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::subBody
Description
This specialized rigidBody holds the original body after it has been merged
into a parent.
SourceFiles
subBodyI.H
subBody.C
\*---------------------------------------------------------------------------*/
#ifndef RBD_subBody_H
#define RBD_subBody_H
#include "rigidBody.H"
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
namespace Foam
{
// Forward declaration of Foam classes
class Istream;
class Ostream;
namespace RBD
{
// Forward declaration of friend functions and operators
class subBody;
Ostream& operator<<(Ostream&, const subBody&);
/*---------------------------------------------------------------------------*\
Class subBody Declaration
\*---------------------------------------------------------------------------*/
class subBody
:
public rigidBody
{
// Private data
//- Original body from which this sub-body was constructed
autoPtr<rigidBody> body_;
//- Parent body ID
label parentID_;
//- Transform with respect to parent body
spatialTransform parentXT_;
public:
// Constructors
//- Construct a merged version of the given rigidBody
// providing the ID of the parent body to which this will be merged
// and the transform relative to the parent
inline subBody
(
autoPtr<rigidBody> bodyPtr,
const label parentID,
const spatialTransform& parentXT
);
// Member Functions
//- Return the original body from which this sub-body was constructed
inline const rigidBody& body() const;
//- Return the parent body Id
inline label parentID() const;
//- Return the transform with respect to the parent body
inline const spatialTransform& parentXT() const;
// IOstream Operators
//friend Ostream& operator<<(Ostream&, const subBody&);
};
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
} // End namespace RBD
} // End namespace Foam
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
#include "subBodyI.H"
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
#endif
// ************************************************************************* //

62
src/rigidBodyDynamics/bodies/subBody/subBodyI.H Normal file
View File

@ -0,0 +1,62 @@
/*---------------------------------------------------------------------------*\
========= |
\\ / 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/>.
\*---------------------------------------------------------------------------*/
// * * * * * * * * * * * * * * * * Constructors * * * * * * * * * * * * * * //
inline Foam::RBD::subBody::subBody
(
autoPtr<rigidBody> bodyPtr,
const label parentID,
const spatialTransform& parentXT
)
:
rigidBody(bodyPtr()),
body_(bodyPtr),
parentID_(parentID),
parentXT_(parentXT)
{}
// * * * * * * * * * * * * * * * Member Functions * * * * * * * * * * * * * //
inline const Foam::RBD::rigidBody& Foam::RBD::subBody::body() const
{
return body_();
}
inline Foam::label Foam::RBD::subBody::parentID() const
{
return parentID_;
}
inline const Foam::spatialTransform& Foam::RBD::subBody::parentXT() const
{
return parentXT_;
}
// ************************************************************************* //

98
src/rigidBodyDynamics/joints/Pa/Pa.C Normal file
View File

@ -0,0 +1,98 @@
/*---------------------------------------------------------------------------*\
========= |
\\ / 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 "Pa.H"
#include "rigidBodyModel.H"
#include "addToRunTimeSelectionTable.H"
// * * * * * * * * * * * * * * Static Data Members * * * * * * * * * * * * * //
namespace Foam
{
namespace RBD
{
namespace joints
{
defineTypeNameAndDebug(Pa, 0);
addToRunTimeSelectionTable
(
joint,
Pa,
dictionary
);
}
}
}
// * * * * * * * * * * * * * * * * Constructors * * * * * * * * * * * * * * //
Foam::RBD::joints::Pa::Pa(const rigidBodyModel& model, const vector& axis)
:
joint(model, 1)
{
S_[0] = spatialVector(Zero, axis);
}
Foam::RBD::joints::Pa::Pa(const rigidBodyModel& model, const dictionary& dict)
:
joint(model, 1)
{
S_[0] = spatialVector(Zero, dict.lookup("axis"));
}
Foam::autoPtr<Foam::RBD::joint> Foam::RBD::joints::Pa::clone() const
{
return autoPtr<joint>(new Pa(*this));
}
// * * * * * * * * * * * * * * * * Destructor * * * * * * * * * * * * * * * //
Foam::RBD::joints::Pa::~Pa()
{}
// * * * * * * * * * * * * * * Member Functions * * * * * * * * * * * * * * //
void Foam::RBD::joints::Pa::jcalc
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const
{
J.X = Xt(S_[0].l()*q[qIndex_]);
J.S1 = S_[0];
J.v = S_[0]*qDot[qIndex_];
J.c = Zero;
}
// ************************************************************************* //

111
src/rigidBodyDynamics/joints/Pa/Pa.H Normal file
View File

@ -0,0 +1,111 @@
/*---------------------------------------------------------------------------*\
========= |
\\ / 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::RBD::joints::Pa
Description
Prismatic joint for translation along the specified arbitrary axis.
Reference:
\verbatim
Featherstone, R. (2008).
Rigid body dynamics algorithms.
Springer.
Chapter 4.
\endverbatim
SourceFiles
Pa.C
\*---------------------------------------------------------------------------*/
#ifndef RBD_joints_Pa_H
#define RBD_joints_Pa_H
#include "joint.H"
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
namespace Foam
{
namespace RBD
{
namespace joints
{
/*---------------------------------------------------------------------------*\
Class Pa Declaration
\*---------------------------------------------------------------------------*/
class Pa
:
public joint
{
public:
//- Runtime type information
TypeName("Pa");
// Constructors
//- Construct for given model and axis
Pa(const rigidBodyModel& model, const vector& axis);
//- Construct for given model from dictionary
Pa(const rigidBodyModel&, const dictionary& dict);
//- Clone this joint
virtual autoPtr<joint> clone() const;
//- Destructor
virtual ~Pa();
// Member Functions
//- Update the model state for this joint
virtual void jcalc
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const;
};
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
} // End namespace joints
} // End namespace RBD
} // End namespace Foam
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
#endif
// ************************************************************************* //

98
src/rigidBodyDynamics/joints/Px/Px.C Normal file
View File

@ -0,0 +1,98 @@
/*---------------------------------------------------------------------------*\
========= |
\\ / 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 "Px.H"
#include "rigidBodyModel.H"
#include "addToRunTimeSelectionTable.H"
// * * * * * * * * * * * * * * Static Data Members * * * * * * * * * * * * * //
namespace Foam
{
namespace RBD
{
namespace joints
{
defineTypeNameAndDebug(Px, 0);
addToRunTimeSelectionTable
(
joint,
Px,
dictionary
);
}
}
}
// * * * * * * * * * * * * * * * * Constructors * * * * * * * * * * * * * * //
Foam::RBD::joints::Px::Px(const rigidBodyModel& model)
:
joint(model, 1)
{
S_[0] = spatialVector(0, 0, 0, 1, 0, 0);
}
Foam::RBD::joints::Px::Px(const rigidBodyModel& model, const dictionary& dict)
:
joint(model, 1)
{
S_[0] = spatialVector(0, 0, 0, 1, 0, 0);
}
Foam::autoPtr<Foam::RBD::joint> Foam::RBD::joints::Px::clone() const
{
return autoPtr<joint>(new Px(*this));
}
// * * * * * * * * * * * * * * * * Destructor * * * * * * * * * * * * * * * //
Foam::RBD::joints::Px::~Px()
{}
// * * * * * * * * * * * * * * Member Functions * * * * * * * * * * * * * * //
void Foam::RBD::joints::Px::jcalc
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const
{
J.X = Xt(S_[0].l()*q[qIndex_]);
J.S1 = S_[0];
J.v = S_[0]*qDot[qIndex_];
J.c = Zero;
}
// ************************************************************************* //

111
src/rigidBodyDynamics/joints/Px/Px.H Normal file
View File

@ -0,0 +1,111 @@
/*---------------------------------------------------------------------------*\
========= |
\\ / 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::RBD::joints::Px
Description
Prismatic joint for translation along the x-axis.
Reference:
\verbatim
Featherstone, R. (2008).
Rigid body dynamics algorithms.
Springer.
Chapter 4.
\endverbatim
SourceFiles
Px.C
\*---------------------------------------------------------------------------*/
#ifndef RBD_joints_Px_H
#define RBD_joints_Px_H
#include "joint.H"
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
namespace Foam
{
namespace RBD
{
namespace joints
{
/*---------------------------------------------------------------------------*\
Class Px Declaration
\*---------------------------------------------------------------------------*/
class Px
:
public joint
{
public:
//- Runtime type information
TypeName("Px");
// Constructors
//- Construct for given model
Px(const rigidBodyModel& model);
//- Construct for given model from dictionary
Px(const rigidBodyModel&, const dictionary& dict);
//- Clone this joint
virtual autoPtr<joint> clone() const;
//- Destructor
virtual ~Px();
// Member Functions
//- Update the model state for this joint
virtual void jcalc
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const;
};
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
} // End namespace joints
} // End namespace RBD
} // End namespace Foam
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
#endif
// ************************************************************************* //

112
src/rigidBodyDynamics/joints/Pxyz/Pxyz.C Normal file
View File

@ -0,0 +1,112 @@
/*---------------------------------------------------------------------------*\
========= |
\\ / 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 "Pxyz.H"
#include "rigidBodyModel.H"
#include "addToRunTimeSelectionTable.H"
// * * * * * * * * * * * * * * Static Data Members * * * * * * * * * * * * * //
namespace Foam
{
namespace RBD
{
namespace joints
{
defineTypeNameAndDebug(Pxyz, 0);
addToRunTimeSelectionTable
(
joint,
Pxyz,
dictionary
);
}
}
}
// * * * * * * * * * * * * * * * * Constructors * * * * * * * * * * * * * * //
Foam::RBD::joints::Pxyz::Pxyz(const rigidBodyModel& model)
:
joint(model, 3)
{
S_[0] = spatialVector(0, 0, 0, 1, 0, 0);
S_[1] = spatialVector(0, 0, 0, 0, 1, 0);
S_[2] = spatialVector(0, 0, 0, 0, 0, 1);
}
Foam::RBD::joints::Pxyz::Pxyz
(
const rigidBodyModel& model,
const dictionary& dict
)
:
joint(model, 3)
{
S_[0] = spatialVector(0, 0, 0, 1, 0, 0);
S_[1] = spatialVector(0, 0, 0, 0, 1, 0);
S_[2] = spatialVector(0, 0, 0, 0, 0, 1);
}
Foam::autoPtr<Foam::RBD::joint> Foam::RBD::joints::Pxyz::clone() const
{
return autoPtr<joint>(new Pxyz(*this));
}
// * * * * * * * * * * * * * * * * Destructor * * * * * * * * * * * * * * * //
Foam::RBD::joints::Pxyz::~Pxyz()
{}
// * * * * * * * * * * * * * * Member Functions * * * * * * * * * * * * * * //
void Foam::RBD::joints::Pxyz::jcalc
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const
{
J.X.E() = tensor::I;
J.X.r() = q.block<vector>(qIndex_);
J.S = Zero;
J.S(3,0) = 1;
J.S(4,1) = 1;
J.S(5,2) = 1;
J.v = spatialVector(Zero, qDot.block<vector>(qIndex_));
J.c = Zero;
}
// ************************************************************************* //

111
src/rigidBodyDynamics/joints/Pxyz/Pxyz.H Normal file
View File

@ -0,0 +1,111 @@
/*---------------------------------------------------------------------------*\
========= |
\\ / 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::RBD::joints::Pxyz
Description
Prismatic joint for translation in the x/y/z directions.
Reference:
\verbatim
Featherstone, R. (2008).
Rigid body dynamics algorithms.
Springer.
Chapter 4.
\endverbatim
SourceFiles
Pxyz.C
\*---------------------------------------------------------------------------*/
#ifndef RBD_joints_Pxyz_H
#define RBD_joints_Pxyz_H
#include "joint.H"
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
namespace Foam
{
namespace RBD
{
namespace joints
{
/*---------------------------------------------------------------------------*\
Class Pxyz Declaration
\*---------------------------------------------------------------------------*/
class Pxyz
:
public joint
{
public:
//- Runtime type information
TypeName("Pxyz");
// Constructors
//- Construct for given model
Pxyz(const rigidBodyModel& model);
//- Construct for given model from dictionary
Pxyz(const rigidBodyModel&, const dictionary& dict);
//- Clone this joint
virtual autoPtr<joint> clone() const;
//- Destructor
virtual ~Pxyz();
// Member Functions
//- Update the model state for this joint
virtual void jcalc
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const;
};
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
} // End namespace joints
} // End namespace RBD
} // End namespace Foam
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
#endif
// ************************************************************************* //

98
src/rigidBodyDynamics/joints/Py/Py.C Normal file
View File

@ -0,0 +1,98 @@
/*---------------------------------------------------------------------------*\
========= |
\\ / 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 "Py.H"
#include "rigidBodyModel.H"
#include "addToRunTimeSelectionTable.H"
// * * * * * * * * * * * * * * Static Data Members * * * * * * * * * * * * * //
namespace Foam
{
namespace RBD
{
namespace joints
{
defineTypeNameAndDebug(Py, 0);
addToRunTimeSelectionTable
(
joint,
Py,
dictionary
);
}
}
}
// * * * * * * * * * * * * * * * * Constructors * * * * * * * * * * * * * * //
Foam::RBD::joints::Py::Py(const rigidBodyModel& model)
:
joint(model, 1)
{
S_[0] = spatialVector(0, 0, 0, 0, 1, 0);
}
Foam::RBD::joints::Py::Py(const rigidBodyModel& model, const dictionary& dict)
:
joint(model, 1)
{
S_[0] = spatialVector(0, 0, 0, 0, 1, 0);
}
Foam::autoPtr<Foam::RBD::joint> Foam::RBD::joints::Py::clone() const
{
return autoPtr<joint>(new Py(*this));
}
// * * * * * * * * * * * * * * * * Destructor * * * * * * * * * * * * * * * //
Foam::RBD::joints::Py::~Py()
{}
// * * * * * * * * * * * * * * Member Functions * * * * * * * * * * * * * * //
void Foam::RBD::joints::Py::jcalc
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const
{
J.X = Xt(S_[0].l()*q[qIndex_]);
J.S1 = S_[0];
J.v = S_[0]*qDot[qIndex_];
J.c = Zero;
}
// ************************************************************************* //

111
src/rigidBodyDynamics/joints/Py/Py.H Normal file
View File

@ -0,0 +1,111 @@
/*---------------------------------------------------------------------------*\
========= |
\\ / 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::RBD::joints::Py
Description
Prismatic joint for translation along the y-axis.
Reference:
\verbatim
Featherstone, R. (2008).
Rigid body dynamics algorithms.
Springer.
Chapter 4.
\endverbatim
SourceFiles
Py.C
\*---------------------------------------------------------------------------*/
#ifndef RBD_joints_Py_H
#define RBD_joints_Py_H
#include "joint.H"
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
namespace Foam
{
namespace RBD
{
namespace joints
{
/*---------------------------------------------------------------------------*\
Class Py Declaration
\*---------------------------------------------------------------------------*/
class Py
:
public joint
{
public:
//- Runtime type information
TypeName("Py");
// Constructors
//- Construct for given model
Py(const rigidBodyModel& model);
//- Construct for given model from dictionary
Py(const rigidBodyModel&, const dictionary& dict);
//- Clone this joint
virtual autoPtr<joint> clone() const;
//- Destructor
virtual ~Py();
// Member Functions
//- Update the model state for this joint
virtual void jcalc
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const;
};
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
} // End namespace joints
} // End namespace RBD
} // End namespace Foam
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
#endif
// ************************************************************************* //

98
src/rigidBodyDynamics/joints/Pz/Pz.C Normal file
View File

@ -0,0 +1,98 @@
/*---------------------------------------------------------------------------*\
========= |
\\ / 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 "Pz.H"
#include "rigidBodyModel.H"
#include "addToRunTimeSelectionTable.H"
// * * * * * * * * * * * * * * Static Data Members * * * * * * * * * * * * * //
namespace Foam
{
namespace RBD
{
namespace joints
{
defineTypeNameAndDebug(Pz, 0);
addToRunTimeSelectionTable
(
joint,
Pz,
dictionary
);
}
}
}
// * * * * * * * * * * * * * * * * Constructors * * * * * * * * * * * * * * //
Foam::RBD::joints::Pz::Pz(const rigidBodyModel& model)
:
joint(model, 1)
{
S_[0] = spatialVector(0, 0, 0, 0, 0, 1);
}
Foam::RBD::joints::Pz::Pz(const rigidBodyModel& model, const dictionary& dict)
:
joint(model, 1)
{
S_[0] = spatialVector(0, 0, 0, 0, 0, 1);
}
Foam::autoPtr<Foam::RBD::joint> Foam::RBD::joints::Pz::clone() const
{
return autoPtr<joint>(new Pz(*this));
}
// * * * * * * * * * * * * * * * * Destructor * * * * * * * * * * * * * * * //
Foam::RBD::joints::Pz::~Pz()
{}
// * * * * * * * * * * * * * * Member Functions * * * * * * * * * * * * * * //
void Foam::RBD::joints::Pz::jcalc
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const
{
J.X = Xt(S_[0].l()*q[qIndex_]);
J.S1 = S_[0];
J.v = S_[0]*qDot[qIndex_];
J.c = Zero;
}
// ************************************************************************* //

111
src/rigidBodyDynamics/joints/Pz/Pz.H Normal file
View File

@ -0,0 +1,111 @@
/*---------------------------------------------------------------------------*\
========= |
\\ / 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::RBD::joints::Pz
Description
Prismatic joint for translation along the x-axis.
Reference:
\verbatim
Featherstone, R. (2008).
Rigid body dynamics algorithms.
Springer.
Chapter 4.
\endverbatim
SourceFiles
Pz.C
\*---------------------------------------------------------------------------*/
#ifndef RBD_joints_Pz_H
#define RBD_joints_Pz_H
#include "joint.H"
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
namespace Foam
{
namespace RBD
{
namespace joints
{
/*---------------------------------------------------------------------------*\
Class Pz Declaration
\*---------------------------------------------------------------------------*/
class Pz
:
public joint
{
public:
//- Runtime type information
TypeName("Pz");
// Constructors
//- Construct for given model
Pz(const rigidBodyModel& model);
//- Construct for given model from dictionary
Pz(const rigidBodyModel&, const dictionary& dict);
//- Clone this joint
virtual autoPtr<joint> clone() const;
//- Destructor
virtual ~Pz();
// Member Functions
//- Update the model state for this joint
virtual void jcalc
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const;
};
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
} // End namespace joints
} // End namespace RBD
} // End namespace Foam
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
#endif
// ************************************************************************* //

98
src/rigidBodyDynamics/joints/Ra/Ra.C Normal file
View File

@ -0,0 +1,98 @@
/*---------------------------------------------------------------------------*\
========= |
\\ / 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 "Ra.H"
#include "rigidBodyModel.H"
#include "addToRunTimeSelectionTable.H"
// * * * * * * * * * * * * * * Static Data Members * * * * * * * * * * * * * //
namespace Foam
{
namespace RBD
{
namespace joints
{
defineTypeNameAndDebug(Ra, 0);
addToRunTimeSelectionTable
(
joint,
Ra,
dictionary
);
}
}
}
// * * * * * * * * * * * * * * * * Constructors * * * * * * * * * * * * * * //
Foam::RBD::joints::Ra::Ra(const rigidBodyModel& model, const vector& axis)
:
joint(model, 1)
{
S_[0] = spatialVector(axis, Zero);
}
Foam::RBD::joints::Ra::Ra(const rigidBodyModel& model, const dictionary& dict)
:
joint(model, 1)
{
S_[0] = spatialVector(dict.lookup("axis"), Zero);
}
Foam::autoPtr<Foam::RBD::joint> Foam::RBD::joints::Ra::clone() const
{
return autoPtr<joint>(new Ra(*this));
}
// * * * * * * * * * * * * * * * * Destructor * * * * * * * * * * * * * * * //
Foam::RBD::joints::Ra::~Ra()
{}
// * * * * * * * * * * * * * * Member Functions * * * * * * * * * * * * * * //
void Foam::RBD::joints::Ra::jcalc
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const
{
J.X = Xr(S_[0].w(), q[qIndex_]);
J.S1 = S_[0];
J.v = S_[0]*qDot[qIndex_];
J.c = Zero;
}
// ************************************************************************* //

111
src/rigidBodyDynamics/joints/Ra/Ra.H Normal file
View File

@ -0,0 +1,111 @@
/*---------------------------------------------------------------------------*\
========= |
\\ / 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::RBD::joints::Ra
Description
Revolute joint for rotation about the specified arbitrary axis.
Reference:
\verbatim
Featherstone, R. (2008).
Rigid body dynamics algorithms.
Springer.
Chapter 4.
\endverbatim
SourceFiles
Ra.C
\*---------------------------------------------------------------------------*/
#ifndef RBD_joints_Ra_H
#define RBD_joints_Ra_H
#include "joint.H"
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
namespace Foam
{
namespace RBD
{
namespace joints
{
/*---------------------------------------------------------------------------*\
Class Ra Declaration
\*---------------------------------------------------------------------------*/
class Ra
:
public joint
{
public:
//- Runtime type information
TypeName("Ra");
// Constructors
//- Construct for given model and axis
Ra(const rigidBodyModel& model, const vector& axis);
//- Construct for given model from dictionary
Ra(const rigidBodyModel&, const dictionary& dict);
//- Clone this joint
virtual autoPtr<joint> clone() const;
//- Destructor
virtual ~Ra();
// Member Functions
//- Update the model state for this joint
virtual void jcalc
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const;
};
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
} // End namespace joints
} // End namespace RBD
} // End namespace Foam
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
#endif
// ************************************************************************* //

114
src/rigidBodyDynamics/joints/Rs/Rs.C Normal file
View File

@ -0,0 +1,114 @@
/*---------------------------------------------------------------------------*\
========= |
\\ / 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 "Rs.H"
#include "rigidBodyModel.H"
#include "addToRunTimeSelectionTable.H"
// * * * * * * * * * * * * * * Static Data Members * * * * * * * * * * * * * //
namespace Foam
{
namespace RBD
{
namespace joints
{
defineTypeNameAndDebug(Rs, 0);
addToRunTimeSelectionTable
(
joint,
Rs,
dictionary
);
}
}
}
// * * * * * * * * * * * * * * * * Constructors * * * * * * * * * * * * * * //
Foam::RBD::joints::Rs::Rs(const rigidBodyModel& model)
:
joint(model, 3)
{
S_[0] = spatialVector(1, 0, 0, 0, 0, 0);
S_[1] = spatialVector (0, 1, 0, 0, 0, 0);
S_[2] = spatialVector(0, 0, 1, 0, 0, 0);
}
Foam::RBD::joints::Rs::Rs(const rigidBodyModel& model, const dictionary& dict)
:
joint(model, 3)
{
S_[0] = spatialVector(1, 0, 0, 0, 0, 0);
S_[1] = spatialVector (0, 1, 0, 0, 0, 0);
S_[2] = spatialVector(0, 0, 1, 0, 0, 0);
}
Foam::autoPtr<Foam::RBD::joint> Foam::RBD::joints::Rs::clone() const
{
return autoPtr<joint>(new Rs(*this));
}
// * * * * * * * * * * * * * * * * Destructor * * * * * * * * * * * * * * * //
Foam::RBD::joints::Rs::~Rs()
{}
// * * * * * * * * * * * * * * Member Functions * * * * * * * * * * * * * * //
Foam::label Foam::RBD::joints::Rs::nw() const
{
return 1;
}
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.r() = Zero;
J.S = Zero;
J.S.xx() = 1;
J.S.yy() = 1;
J.S.zz() = 1;
J.v = spatialVector(qDot.block<vector>(qIndex_), Zero);
J.c = Zero;
}
// ************************************************************************* //

116
src/rigidBodyDynamics/joints/Rs/Rs.H Normal file
View File

@ -0,0 +1,116 @@
/*---------------------------------------------------------------------------*\
========= |
\\ / 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::RBD::joints::Rs
Description
Spherical joint for rotation about the x/y/z-axes using a quaternion
(Euler parameters) to avoid gimble-lock.
Reference:
\verbatim
Featherstone, R. (2008).
Rigid body dynamics algorithms.
Springer.
Chapter 4.
\endverbatim
SourceFiles
Rs.C
\*---------------------------------------------------------------------------*/
#ifndef RBD_joints_Rs_H
#define RBD_joints_Rs_H
#include "joint.H"
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
namespace Foam
{
namespace RBD
{
namespace joints
{
/*---------------------------------------------------------------------------*\
Class Rs Declaration
\*---------------------------------------------------------------------------*/
class Rs
:
public joint
{
public:
//- Runtime type information
TypeName("Rs");
// Constructors
//- Construct for given model
Rs(const rigidBodyModel& model);
//- Construct for given model from dictionary
Rs(const rigidBodyModel&, const dictionary& dict);
//- Clone this joint
virtual autoPtr<joint> clone() const;
//- Destructor
virtual ~Rs();
// Member Functions
//- Return the number of additional state variable for this joint
// For the quaternion this is 1 in addition to the 3 stored in q
virtual label nw() const;
//- Update the model state for this joint
virtual void jcalc
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const;
};
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
} // End namespace joints
} // End namespace RBD
} // End namespace Foam
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
#endif
// ************************************************************************* //

99
src/rigidBodyDynamics/joints/Rx/Rx.C Normal file
View File

@ -0,0 +1,99 @@
/*---------------------------------------------------------------------------*\
========= |
\\ / 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 "Rx.H"
#include "rigidBodyModel.H"
#include "addToRunTimeSelectionTable.H"
// * * * * * * * * * * * * * * Static Data Members * * * * * * * * * * * * * //
namespace Foam
{
namespace RBD
{
namespace joints
{
defineTypeNameAndDebug(Rx, 0);
addToRunTimeSelectionTable
(
joint,
Rx,
dictionary
);
}
}
}
// * * * * * * * * * * * * * * * * Constructors * * * * * * * * * * * * * * //
Foam::RBD::joints::Rx::Rx(const rigidBodyModel& model)
:
joint(model, 1)
{
S_[0] = spatialVector(1, 0, 0, 0, 0, 0);
}
Foam::RBD::joints::Rx::Rx(const rigidBodyModel& model, const dictionary& dict)
:
joint(model, 1)
{
S_[0] = spatialVector(1, 0, 0, 0, 0, 0);
}
Foam::autoPtr<Foam::RBD::joint> Foam::RBD::joints::Rx::clone() const
{
return autoPtr<joint>(new Rx(*this));
}
// * * * * * * * * * * * * * * * * Destructor * * * * * * * * * * * * * * * //
Foam::RBD::joints::Rx::~Rx()
{}
// * * * * * * * * * * * * * * Member Functions * * * * * * * * * * * * * * //
void Foam::RBD::joints::Rx::jcalc
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const
{
J.X = Xrx(q[qIndex_]);
J.S1 = S_[0];
J.v = Zero;
J.v.wx() = qDot[qIndex_];
J.c = Zero;
}
// ************************************************************************* //

111
src/rigidBodyDynamics/joints/Rx/Rx.H Normal file
View File

@ -0,0 +1,111 @@
/*---------------------------------------------------------------------------*\
========= |
\\ / 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::RBD::joints::Rx
Description
Revolute joint for rotation about the x-axis
Reference:
\verbatim
Featherstone, R. (2008).
Rigid body dynamics algorithms.
Springer.
Chapter 4.
\endverbatim
SourceFiles
Rx.C
\*---------------------------------------------------------------------------*/
#ifndef RBD_joints_Rx_H
#define RBD_joints_Rx_H
#include "joint.H"
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
namespace Foam
{
namespace RBD
{
namespace joints
{
/*---------------------------------------------------------------------------*\
Class Rx Declaration
\*---------------------------------------------------------------------------*/
class Rx
:
public joint
{
public:
//- Runtime type information
TypeName("Rx");
// Constructors
//- Construct for given model
Rx(const rigidBodyModel& model);
//- Construct for given model from dictionary
Rx(const rigidBodyModel&, const dictionary& dict);
//- Clone this joint
virtual autoPtr<joint> clone() const;
//- Destructor
virtual ~Rx();
// Member Functions
//- Update the model state for this joint
virtual void jcalc
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const;
};
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
} // End namespace joints
} // End namespace RBD
} // End namespace Foam
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
#endif
// ************************************************************************* //

146
src/rigidBodyDynamics/joints/Rxyz/Rxyz.C Normal file
View File

@ -0,0 +1,146 @@
/*---------------------------------------------------------------------------*\
========= |
\\ / 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 "Rxyz.H"
#include "rigidBodyModel.H"
#include "addToRunTimeSelectionTable.H"
// * * * * * * * * * * * * * * Static Data Members * * * * * * * * * * * * * //
namespace Foam
{
namespace RBD
{
namespace joints
{
defineTypeNameAndDebug(Rxyz, 0);
addToRunTimeSelectionTable
(
joint,
Rxyz,
dictionary
);
}
}
}
// * * * * * * * * * * * * * * * * Constructors * * * * * * * * * * * * * * //
Foam::RBD::joints::Rxyz::Rxyz(const rigidBodyModel& model)
:
joint(model, 3)
{
S_[0] = spatialVector(1, 0, 0, 0, 0, 0);
S_[1] = spatialVector(0, 1, 0, 0, 0, 0);
S_[2] = spatialVector(0, 0, 1, 0, 0, 0);
}
Foam::RBD::joints::Rxyz::Rxyz
(
const rigidBodyModel& model,
const dictionary& dict
)
:
joint(model, 3)
{
S_[0] = spatialVector(1, 0, 0, 0, 0, 0);
S_[1] = spatialVector(0, 1, 0, 0, 0, 0);
S_[2] = spatialVector(0, 0, 1, 0, 0, 0);
}
Foam::autoPtr<Foam::RBD::joint> Foam::RBD::joints::Rxyz::clone() const
{
return autoPtr<joint>(new Rxyz(*this));
}
// * * * * * * * * * * * * * * * * Destructor * * * * * * * * * * * * * * * //
Foam::RBD::joints::Rxyz::~Rxyz()
{}
// * * * * * * * * * * * * * * Member Functions * * * * * * * * * * * * * * //
void Foam::RBD::joints::Rxyz::jcalc
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const
{
vector qj(q.block<vector>(qIndex_));
scalar s0 = sin(qj.x());
scalar c0 = cos(qj.x());
scalar s1 = sin(qj.y());
scalar c1 = cos(qj.y());
scalar s2 = sin(qj.z());
scalar c2 = cos(qj.z());
J.X.E() = tensor
(
c2*c1, s2*c0 + c2*s1*s0, s2*s0 - c2*s1*c0,
-s2*c1, c2*c0 - s2*s1*s0, c2*s0 + s2*s1*c0,
s1, -c1*s0, c1*c0
);
J.X.r() = Zero;
J.S = Zero;
J.S.xx() = c2*c1;
J.S.xy() = s2;
J.S.yx() = -s2*c1;
J.S.yy() = c2;
J.S.zx() = s1;
J.S.zz() = 1;
vector qDotj(qDot.block<vector>(qIndex_));
J.v = J.S & qDotj;
J.c = spatialVector
(
-s2*c1*qDotj.z()*qDotj.x()
- c2*s1*qDotj.y()*qDotj.x()
+ c2*qDotj.z()*qDotj.y(),
-c2*c1*qDotj.z()*qDotj.x()
+ s2*s1*qDotj.y()*qDotj.x()
- s2*qDotj.z()*qDotj.y(),
c1*qDotj.y()*qDotj.x(),
0,
0,
0
);
}
// ************************************************************************* //

112
src/rigidBodyDynamics/joints/Rxyz/Rxyz.H Normal file
View File

@ -0,0 +1,112 @@
/*---------------------------------------------------------------------------*\
========= |
\\ / 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::RBD::joints::Rxyz
Description
Spherical joint for rotation about the x/y/z-axes using Euler-angles
in the order x, y, z.
Reference:
\verbatim
Featherstone, R. (2008).
Rigid body dynamics algorithms.
Springer.
Chapter 4.
\endverbatim
SourceFiles
Rxyz.C
\*---------------------------------------------------------------------------*/
#ifndef RBD_joints_Rxyz_H
#define RBD_joints_Rxyz_H
#include "joint.H"
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
namespace Foam
{
namespace RBD
{
namespace joints
{
/*---------------------------------------------------------------------------*\
Class Rxyz Declaration
\*---------------------------------------------------------------------------*/
class Rxyz
:
public joint
{
public:
//- Runtime type information
TypeName("Rxyz");
// Constructors
//- Construct for given model
Rxyz(const rigidBodyModel& model);
//- Construct for given model from dictionary
Rxyz(const rigidBodyModel&, const dictionary& dict);
//- Clone this joint
virtual autoPtr<joint> clone() const;
//- Destructor
virtual ~Rxyz();
// Member Functions
//- Update the model state for this joint
virtual void jcalc
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const;
};
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
} // End namespace joints
} // End namespace RBD
} // End namespace Foam
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
#endif
// ************************************************************************* //

99
src/rigidBodyDynamics/joints/Ry/Ry.C Normal file
View File

@ -0,0 +1,99 @@
/*---------------------------------------------------------------------------*\
========= |
\\ / 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 "Ry.H"
#include "rigidBodyModel.H"
#include "addToRunTimeSelectionTable.H"
// * * * * * * * * * * * * * * Static Data Members * * * * * * * * * * * * * //
namespace Foam
{
namespace RBD
{
namespace joints
{
defineTypeNameAndDebug(Ry, 0);
addToRunTimeSelectionTable
(
joint,
Ry,
dictionary
);
}
}
}
// * * * * * * * * * * * * * * * * Constructors * * * * * * * * * * * * * * //
Foam::RBD::joints::Ry::Ry(const rigidBodyModel& model)
:
joint(model, 1)
{
S_[0] = spatialVector(0, 1, 0, 0, 0, 0);
}
Foam::RBD::joints::Ry::Ry(const rigidBodyModel& model, const dictionary& dict)
:
joint(model, 1)
{
S_[0] = spatialVector(0, 1, 0, 0, 0, 0);
}
Foam::autoPtr<Foam::RBD::joint> Foam::RBD::joints::Ry::clone() const
{
return autoPtr<joint>(new Ry(*this));
}
// * * * * * * * * * * * * * * * * Destructor * * * * * * * * * * * * * * * //
Foam::RBD::joints::Ry::~Ry()
{}
// * * * * * * * * * * * * * * Member Functions * * * * * * * * * * * * * * //
void Foam::RBD::joints::Ry::jcalc
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const
{
J.X = Xry(q[qIndex_]);
J.S1 = S_[0];
J.v = Zero;
J.v.wy() = qDot[qIndex_];
J.c = Zero;
}
// ************************************************************************* //

111
src/rigidBodyDynamics/joints/Ry/Ry.H Normal file
View File

@ -0,0 +1,111 @@
/*---------------------------------------------------------------------------*\
========= |
\\ / 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::RBD::joints::Ry
Description
Revolute joint for rotation about the y-axis
Reference:
\verbatim
Featherstone, R. (2008).
Rigid body dynamics algorithms.
Springer.
Chapter 4.
\endverbatim
SourceFiles
Ry.C
\*---------------------------------------------------------------------------*/
#ifndef RBD_joints_Ry_H
#define RBD_joints_Ry_H
#include "joint.H"
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
namespace Foam
{
namespace RBD
{
namespace joints
{
/*---------------------------------------------------------------------------*\
Class Ry Declaration
\*---------------------------------------------------------------------------*/
class Ry
:
public joint
{
public:
//- Runtime type information
TypeName("Ry");
// Constructors
//- Construct for given model
Ry(const rigidBodyModel& model);
//- Construct for given model from dictionary
Ry(const rigidBodyModel&, const dictionary& dict);
//- Clone this joint
virtual autoPtr<joint> clone() const;
//- Destructor
virtual ~Ry();
// Member Functions
//- Update the model state for this joint
virtual void jcalc
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const;
};
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
} // End namespace joints
} // End namespace RBD
} // End namespace Foam
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
#endif
// ************************************************************************* //

146
src/rigidBodyDynamics/joints/Ryxz/Ryxz.C Normal file
View File

@ -0,0 +1,146 @@
/*---------------------------------------------------------------------------*\
========= |
\\ / 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 "Ryxz.H"
#include "rigidBodyModel.H"
#include "addToRunTimeSelectionTable.H"
// * * * * * * * * * * * * * * Static Data Members * * * * * * * * * * * * * //
namespace Foam
{
namespace RBD
{
namespace joints
{
defineTypeNameAndDebug(Ryxz, 0);
addToRunTimeSelectionTable
(
joint,
Ryxz,
dictionary
);
}
}
}
// * * * * * * * * * * * * * * * * Constructors * * * * * * * * * * * * * * //
Foam::RBD::joints::Ryxz::Ryxz(const rigidBodyModel& model)
:
joint(model, 3)
{
S_[0] = spatialVector(0, 1, 0, 0, 0, 0);
S_[1] = spatialVector(1, 0, 0, 0, 0, 0);
S_[2] = spatialVector(0, 0, 1, 0, 0, 0);
}
Foam::RBD::joints::Ryxz::Ryxz
(
const rigidBodyModel& model,
const dictionary& dict
)
:
joint(model, 3)
{
S_[0] = spatialVector(0, 1, 0, 0, 0, 0);
S_[1] = spatialVector(1, 0, 0, 0, 0, 0);
S_[2] = spatialVector(0, 0, 1, 0, 0, 0);
}
Foam::autoPtr<Foam::RBD::joint> Foam::RBD::joints::Ryxz::clone() const
{
return autoPtr<joint>(new Ryxz(*this));
}
// * * * * * * * * * * * * * * * * Destructor * * * * * * * * * * * * * * * //
Foam::RBD::joints::Ryxz::~Ryxz()
{}
// * * * * * * * * * * * * * * Member Functions * * * * * * * * * * * * * * //
void Foam::RBD::joints::Ryxz::jcalc
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const
{
vector qj(q.block<vector>(qIndex_));
scalar s0 = sin(qj.x());
scalar c0 = cos(qj.x());
scalar s1 = sin(qj.y());
scalar c1 = cos(qj.y());
scalar s2 = sin(qj.z());
scalar c2 = cos(qj.z());
J.X.E() = tensor
(
c2*c0 + s2*s1*s0, s2*c1, -c2*s0 + s2*s1*c0,
-s2*c0 + c2*s1*s0, c2*c1, s2*s0 + c2*s1*c0,
c1*s0, -s1, c1*c0
);
J.X.r() = Zero;
J.S = Zero;
J.S.xx() = s2*c1;
J.S.xy() = c2;
J.S.yx() = c2*c1;
J.S.yy() = -s2;
J.S.zx() = -s1;
J.S.zz() = 1;
vector qDotj(qDot.block<vector>(qIndex_));
J.v = J.S & qDotj;
J.c = spatialVector
(
c2*c1*qDotj.z()*qDotj.x()
- s2*s1*qDotj.y()*qDotj.x()
- s2*qDotj.z()*qDotj.y(),
-s2*c1*qDotj.z()*qDotj.x()
- c2*s1*qDotj.y()*qDotj.x()
- c2*qDotj.z()*qDotj.y(),
-c1*qDotj.y()*qDotj.x(),
0,
0,
0
);
}
// ************************************************************************* //

112
src/rigidBodyDynamics/joints/Ryxz/Ryxz.H Normal file
View File

@ -0,0 +1,112 @@
/*---------------------------------------------------------------------------*\
========= |
\\ / 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::RBD::joints::Ryxz
Description
Spherical joint for rotation about the x/y/z-axes using Euler-angles
in the order y, x, z.
Reference:
\verbatim
Featherstone, R. (2008).
Rigid body dynamics algorithms.
Springer.
Chapter 4.
\endverbatim
SourceFiles
Ryxz.C
\*---------------------------------------------------------------------------*/
#ifndef RBD_joints_Ryxz_H
#define RBD_joints_Ryxz_H
#include "joint.H"
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
namespace Foam
{
namespace RBD
{
namespace joints
{
/*---------------------------------------------------------------------------*\
Class Ryxz Declaration
\*---------------------------------------------------------------------------*/
class Ryxz
:
public joint
{
public:
//- Runtime type information
TypeName("Ryxz");
// Constructors
//- Construct for given model
Ryxz(const rigidBodyModel& model);
//- Construct for given model from dictionary
Ryxz(const rigidBodyModel&, const dictionary& dict);
//- Clone this joint
virtual autoPtr<joint> clone() const;
//- Destructor
virtual ~Ryxz();
// Member Functions
//- Update the model state for this joint
virtual void jcalc
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const;
};
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
} // End namespace joints
} // End namespace RBD
} // End namespace Foam
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
#endif
// ************************************************************************* //

99
src/rigidBodyDynamics/joints/Rz/Rz.C Normal file
View File

@ -0,0 +1,99 @@
/*---------------------------------------------------------------------------*\
========= |
\\ / 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 "Rz.H"
#include "rigidBodyModel.H"
#include "addToRunTimeSelectionTable.H"
// * * * * * * * * * * * * * * Static Data Members * * * * * * * * * * * * * //
namespace Foam
{
namespace RBD
{
namespace joints
{
defineTypeNameAndDebug(Rz, 0);
addToRunTimeSelectionTable
(
joint,
Rz,
dictionary
);
}
}
}
// * * * * * * * * * * * * * * * * Constructors * * * * * * * * * * * * * * //
Foam::RBD::joints::Rz::Rz(const rigidBodyModel& model)
:
joint(model, 1)
{
S_[0] = spatialVector(0, 0, 1, 0, 0, 0);
}
Foam::RBD::joints::Rz::Rz(const rigidBodyModel& model, const dictionary& dict)
:
joint(model, 1)
{
S_[0] = spatialVector(0, 0, 1, 0, 0, 0);
}
Foam::autoPtr<Foam::RBD::joint> Foam::RBD::joints::Rz::clone() const
{
return autoPtr<joint>(new Rz(*this));
}
// * * * * * * * * * * * * * * * * Destructor * * * * * * * * * * * * * * * //
Foam::RBD::joints::Rz::~Rz()
{}
// * * * * * * * * * * * * * * Member Functions * * * * * * * * * * * * * * //
void Foam::RBD::joints::Rz::jcalc
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const
{
J.X = Xrz(q[qIndex_]);
J.S1 = S_[0];
J.v = Zero;
J.v.wz() = qDot[qIndex_];
J.c = Zero;
}
// ************************************************************************* //

111
src/rigidBodyDynamics/joints/Rz/Rz.H Normal file
View File

@ -0,0 +1,111 @@
/*---------------------------------------------------------------------------*\
========= |
\\ / 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::RBD::joints::Rz
Description
Revolute joint for rotation about the z-axis
Reference:
\verbatim
Featherstone, R. (2008).
Rigid body dynamics algorithms.
Springer.
Chapter 4.
\endverbatim
SourceFiles
Rz.C
\*---------------------------------------------------------------------------*/
#ifndef RBD_joints_Rz_H
#define RBD_joints_Rz_H
#include "joint.H"
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
namespace Foam
{
namespace RBD
{
namespace joints
{
/*---------------------------------------------------------------------------*\
Class Rz Declaration
\*---------------------------------------------------------------------------*/
class Rz
:
public joint
{
public:
//- Runtime type information
TypeName("Rz");
// Constructors
//- Construct for given model
Rz(const rigidBodyModel& model);
//- Construct for given model from dictionary
Rz(const rigidBodyModel&, const dictionary& dict);
//- Clone this joint
virtual autoPtr<joint> clone() const;
//- Destructor
virtual ~Rz();
// Member Functions
//- Update the model state for this joint
virtual void jcalc
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const;
};
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
} // End namespace joints
} // End namespace RBD
} // End namespace Foam
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
#endif
// ************************************************************************* //

146
src/rigidBodyDynamics/joints/Rzyx/Rzyx.C Normal file
View File

@ -0,0 +1,146 @@
/*---------------------------------------------------------------------------*\
========= |
\\ / 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 "Rzyx.H"
#include "rigidBodyModel.H"
#include "addToRunTimeSelectionTable.H"
// * * * * * * * * * * * * * * Static Data Members * * * * * * * * * * * * * //
namespace Foam
{
namespace RBD
{
namespace joints
{
defineTypeNameAndDebug(Rzyx, 0);
addToRunTimeSelectionTable
(
joint,
Rzyx,
dictionary
);
}
}
}
// * * * * * * * * * * * * * * * * Constructors * * * * * * * * * * * * * * //
Foam::RBD::joints::Rzyx::Rzyx(const rigidBodyModel& model)
:
joint(model, 3)
{
S_[0] = spatialVector(0, 0, 1, 0, 0, 0);
S_[1] = spatialVector(0, 1, 0, 0, 0, 0);
S_[2] = spatialVector(1, 0, 0, 0, 0, 0);
}
Foam::RBD::joints::Rzyx::Rzyx
(
const rigidBodyModel& model,
const dictionary& dict
)
:
joint(model, 3)
{
S_[0] = spatialVector(0, 0, 1, 0, 0, 0);
S_[1] = spatialVector(0, 1, 0, 0, 0, 0);
S_[2] = spatialVector(1, 0, 0, 0, 0, 0);
}
Foam::autoPtr<Foam::RBD::joint> Foam::RBD::joints::Rzyx::clone() const
{
return autoPtr<joint>(new Rzyx(*this));
}
// * * * * * * * * * * * * * * * * Destructor * * * * * * * * * * * * * * * //
Foam::RBD::joints::Rzyx::~Rzyx()
{}
// * * * * * * * * * * * * * * Member Functions * * * * * * * * * * * * * * //
void Foam::RBD::joints::Rzyx::jcalc
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const
{
vector qj(q.block<vector>(qIndex_));
scalar s0 = sin(qj.x());
scalar c0 = cos(qj.x());
scalar s1 = sin(qj.y());
scalar c1 = cos(qj.y());
scalar s2 = sin(qj.z());
scalar c2 = cos(qj.z());
J.X.E() = tensor
(
c0*c1, s0*c1, -s1,
c0*s1*s2 - s0*c2, s0*s1*s2 + c0*c2, c1*s2,
c0*s1*c2 + s0*s2, s0*s1*c2 - c0*s2, c1*c2
);
J.X.r() = Zero;
J.S = Zero;
J.S.xx() = -s1;
J.S.xz() = 1;
J.S.yx() = c1*s2;
J.S.yy() = c2;
J.S.zx() = c1*c2;
J.S.zy() = -s2;
vector qDotj(qDot.block<vector>(qIndex_));
J.v = J.S & qDotj;
J.c = spatialVector
(
-c1*qDotj.x()*qDotj.y(),
-s1*s2*qDotj.x()*qDotj.y()
+ c1*c2*qDotj.x()*qDotj.z()
- s2*qDotj.y()*qDotj.z(),
-s1*c2*qDotj.x()*qDotj.y()
- c1*s2*qDotj.x()*qDotj.z()
- c2*qDotj.y()*qDotj.z(),
0,
0,
0
);
}
// ************************************************************************* //

112
src/rigidBodyDynamics/joints/Rzyx/Rzyx.H Normal file
View File

@ -0,0 +1,112 @@
/*---------------------------------------------------------------------------*\
========= |
\\ / 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::RBD::joints::Rzyx
Description
Spherical joint for rotation about the x/y/z-axes using Euler-angles
in the order z, y, x.
Reference:
\verbatim
Featherstone, R. (2008).
Rigid body dynamics algorithms.
Springer.
Chapter 4.
\endverbatim
SourceFiles
Rzyx.C
\*---------------------------------------------------------------------------*/
#ifndef RBD_joints_Rzyx_H
#define RBD_joints_Rzyx_H
#include "joint.H"
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
namespace Foam
{
namespace RBD
{
namespace joints
{
/*---------------------------------------------------------------------------*\
Class Rzyx Declaration
\*---------------------------------------------------------------------------*/
class Rzyx
:
public joint
{
public:
//- Runtime type information
TypeName("Rzyx");
// Constructors
//- Construct for given model
Rzyx(const rigidBodyModel& model);
//- Construct for given model from dictionary
Rzyx(const rigidBodyModel&, const dictionary& dict);
//- Clone this joint
virtual autoPtr<joint> clone() const;
//- Destructor
virtual ~Rzyx();
// Member Functions
//- Update the model state for this joint
virtual void jcalc
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const;
};
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
} // End namespace joints
} // End namespace RBD
} // End namespace Foam
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
#endif
// ************************************************************************* //

77
src/rigidBodyDynamics/joints/joint/joint.C Normal file
View File

@ -0,0 +1,77 @@
/*---------------------------------------------------------------------------*\
========= |
\\ / 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 "joint.H"
#include "rigidBodyModel.H"
#include "Rs.H"
#include "Rzyx.H"
#include "Pxyz.H"
// * * * * * * * * * * * * * * Static Data Members * * * * * * * * * * * * * //
namespace Foam
{
namespace RBD
{
defineTypeNameAndDebug(joint, 0);
defineRunTimeSelectionTable(joint, dictionary);
}
}
// * * * * * * * * * * * * * * * * Selectors * * * * * * * * * * * * * * * * //
Foam::autoPtr<Foam::RBD::joint> Foam::RBD::joint::New(joint* jointPtr)
{
return autoPtr<joint>(jointPtr);
}
// * * * * * * * * * * * * * * * * Destructor * * * * * * * * * * * * * * * //
Foam::RBD::joint::~joint()
{}
// * * * * * * * * * * * * * * Member Functions * * * * * * * * * * * * * * //
Foam::PtrList<Foam::RBD::joint> Foam::RBD::joint::floating
(
const rigidBodyModel& model
)
{
PtrList<joint> cj(2);
cj.set(0, new joints::Pxyz(model));
//cj.set(1, new joints::Rs(model));
cj.set(1, new joints::Rzyx(model));
return cj;
}
void Foam::RBD::joint::write(Ostream& os) const
{}
// ************************************************************************* //

269
src/rigidBodyDynamics/joints/joint/joint.H Normal file
View File

@ -0,0 +1,269 @@
/*---------------------------------------------------------------------------*\
========= |
\\ / 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::RBD::joint
Description
Abstract base-class for all rigid-body joints.
Reference:
\verbatim
Featherstone, R. (2008).
Rigid body dynamics algorithms.
Springer.
Chapter 4.
\endverbatim
SourceFiles
jointI.H
joint.C
\*---------------------------------------------------------------------------*/
#ifndef RBD_joint_H
#define RBD_joint_H
#include "List.H"
#include "spatialVector.H"
#include "compactSpatialTensor.H"
#include "CompactSpatialTensorT.H"
#include "spatialTransform.H"
#include "quaternion.H"
#include "scalarField.H"
#include "runTimeSelectionTables.H"
namespace Foam
{
namespace RBD
{
// Forward declaration of classes
class rigidBodyModel;
/*---------------------------------------------------------------------------*\
Class joint Declaration
\*---------------------------------------------------------------------------*/
class joint
{
protected:
// Protected data
//- Joint motion sub-space
List<spatialVector> S_;
//- Index of this joint in the rigidBodyModel
label index_;
//- Index of this joints data in the rigidBodyModel state
label qIndex_;
//- Index of this joints quaternion data in the rigidBodyModel state
label wIndex_;
private:
// Private member functions to be used by rigidBodyModel
//- Allow the rigidBodyModel to set the index for this joint
label& index()
{
return index_;
}
//- Allow the rigidBodyModel to set the qIndex for this joint
label& qIndex()
{
return qIndex_;
}
//- Allow the rigidBodyModel to set the wIndex for this joint
label& wIndex()
{
return wIndex_;
}
public:
//- Allow the rigidBodyModel class to set the joint indices
friend class rigidBodyModel;
//- Joint state returned by jcalc
class XSvc
{
public:
//- The joint transformation
spatialTransform X;
//- The joint motion sub-space (3-DoF)
compactSpatialTensor S;
//- The joint motion sub-space (1-DoF)
spatialVector S1;
//- The constrained joint velocity
spatialVector v;
//- The constrained joint acceleration correction
// due to changes in the motion sub-space S
spatialVector c;
//- Null constructor
XSvc()
:
X(),
v(Zero),
c(Zero)
{}
};
public:
//- Runtime type information
TypeName("joint");
// Declare run-time constructor selection table
declareRunTimeSelectionTable
(
autoPtr,
joint,
dictionary,
(
const rigidBodyModel& model,
const dictionary& dict
),
(model, dict)
);
// Constructors
//- Construct joint for given rigidBodyModel
// setting the size of the motion sub-space
// to the given degrees of freedom of the joint
inline joint(const rigidBodyModel&, const label nDoF);
//- Clone this joint (needed by PtrList)
virtual autoPtr<joint> clone() const = 0;
//- Destructor
virtual ~joint();
// Selectors
//- Simple selector to return an autoPtr<joint> of the given joint*
static autoPtr<joint> New(joint* jointPtr);
//- Select constructed from dictionary
static autoPtr<joint> New
(
const rigidBodyModel& model,
const dictionary& dict
);
// Member Functions
//- Return the number of degrees of freedom in this joint
inline label nDoF() const;
//- Return the number of additional state variables need by this joint
inline virtual label nw() const;
//- Return the index of this joint in the model
inline label index() const;
//- Return start index for the state variables for this joint
// in the rigidBodyModel state fields
inline label qIndex() const;
//- Return start index for the additional state variables for this joint
// in the rigidBodyModel w state field
inline label wIndex() const;
//- Return the joint motion sub-space
inline const List<spatialVector>& S() const;
//- Return a list of joints needed to emulate a floating body
static PtrList<joint> floating(const rigidBodyModel& model);
//- Update the rigidBodyModel state for the joint given
// the joint state q, w and velocity qDot
virtual void jcalc
(
XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const = 0;
//- Write
virtual void write(Ostream&) const;
// Member Operators
//- Return the state quaternion for this joint
// if it uses a quaternion representation for rotation
inline quaternion operator()
(
const scalarField& q,
const scalarField& w
) const;
//- Set the state quaternion for this joint
// if it uses a quaternion representation for rotation
inline void operator()
(
const quaternion& quat,
scalarField& q,
scalarField& w
) const;
};
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
} // End namespace RBD
} // End namespace Foam
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
#include "jointI.H"
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
#endif
// ************************************************************************* //

110
src/rigidBodyDynamics/joints/joint/jointI.H Normal file
View File

@ -0,0 +1,110 @@
/*---------------------------------------------------------------------------*\
========= |
\\ / 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/>.
\*---------------------------------------------------------------------------*/
// * * * * * * * * * * * * * * * * Constructors * * * * * * * * * * * * * * //
inline Foam::RBD::joint::joint(const rigidBodyModel&, const label nDoF)
:
S_(nDoF),
index_(0),
qIndex_(0),
wIndex_(0)
{}
// * * * * * * * * * * * * * * * Member Functions * * * * * * * * * * * * * //
inline Foam::label Foam::RBD::joint::nDoF() const
{
return S_.size();
}
inline Foam::label Foam::RBD::joint::nw() const
{
return 0;
}
inline Foam::label Foam::RBD::joint::index() const
{
return index_;
}
inline Foam::label Foam::RBD::joint::qIndex() const
{
return qIndex_;
}
inline Foam::label Foam::RBD::joint::wIndex() const
{
return wIndex_;
}
inline const Foam::List<Foam::spatialVector>& Foam::RBD::joint::S() const
{
return S_;
}
// * * * * * * * * * * * * * * * Member Operators * * * * * * * * * * * * * //
inline Foam::quaternion Foam::RBD::joint::operator()
(
const scalarField& q,
const scalarField& w
) const
{
if (nw() != 1)
{
FatalErrorInFunction
<< "Attempt to get the quaternion for a non-spherical joint"
<< abort(FatalError);
}
return quaternion(w[wIndex_], q.block<vector>(qIndex_));
}
inline void Foam::RBD::joint::operator()
(
const quaternion& quat,
scalarField& q,
scalarField& w
) const
{
if (nw() != 1)
{
FatalErrorInFunction
<< "Attempt to set quaternion for a non-spherical joint"
<< abort(FatalError);
}
w[wIndex_] = quat.w();
q[qIndex_] = quat.v().x();
q[qIndex_+1] = quat.v().y();
q[qIndex_+2] = quat.v().z();
}
// ************************************************************************* //

19
src/rigidBodyDynamics/joints/joints.H Normal file
View File

@ -0,0 +1,19 @@
// Null joint for the root-body
#include "nullJoint.H"
// Revolute joints
#include "Rx.H"
#include "Ry.H"
#include "Rz.H"
#include "Ra.H"
#include "Rs.H"
#include "Rzyx.H"
#include "Rxyz.H"
#include "Ryxz.H"
// Prismatic joints
#include "Px.H"
#include "Py.H"
#include "Pz.H"
#include "Pa.H"
#include "Pxyz.H"

97
src/rigidBodyDynamics/joints/nullJoint/nullJoint.C Normal file
View File

@ -0,0 +1,97 @@
/*---------------------------------------------------------------------------*\
========= |
\\ / 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 "nullJoint.H"
#include "rigidBodyModel.H"
#include "addToRunTimeSelectionTable.H"
// * * * * * * * * * * * * * * Static Data Members * * * * * * * * * * * * * //
namespace Foam
{
namespace RBD
{
namespace joints
{
defineTypeNameAndDebug(null, 0);
addToRunTimeSelectionTable
(
joint,
null,
dictionary
);
}
}
}
// * * * * * * * * * * * * * * * * Constructors * * * * * * * * * * * * * * //
Foam::RBD::joints::null::null(const rigidBodyModel& model)
:
joint(model, 0)
{}
Foam::RBD::joints::null::null
(
const rigidBodyModel& model,
const dictionary& dict
)
:
joint(model, 0)
{}
Foam::autoPtr<Foam::RBD::joint> Foam::RBD::joints::null::clone() const
{
return autoPtr<joint>(new null(*this));
}
// * * * * * * * * * * * * * * * * Destructor * * * * * * * * * * * * * * * //
Foam::RBD::joints::null::~null()
{}
// * * * * * * * * * * * * * * Member Functions * * * * * * * * * * * * * * //
void Foam::RBD::joints::null::jcalc
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const
{
FatalErrorInFunction
<< "Cannot calculate the state of a null-joint"
<< abort(FatalError);
}
// ************************************************************************* //

111
src/rigidBodyDynamics/joints/nullJoint/nullJoint.H Normal file
View File

@ -0,0 +1,111 @@
/*---------------------------------------------------------------------------*\
========= |
\\ / 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::RBD::joints::null
Description
Null joint for the root-body.
Reference:
\verbatim
Featherstone, R. (2008).
Rigid body dynamics algorithms.
Springer.
Chapter 4.
\endverbatim
SourceFiles
nullJoint.C
\*---------------------------------------------------------------------------*/
#ifndef RBD_joints_null_H
#define RBD_joints_null_H
#include "joint.H"
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
namespace Foam
{
namespace RBD
{
namespace joints
{
/*---------------------------------------------------------------------------*\
Class null Declaration
\*---------------------------------------------------------------------------*/
class null
:
public joint
{
public:
//- Runtime type information
TypeName("null");
// Constructors
//- Construct for given model
null(const rigidBodyModel& model);
//- Construct for given model from dictionary
null(const rigidBodyModel&, const dictionary& dict);
//- Clone this joint
virtual autoPtr<joint> clone() const;
//- Destructor
virtual ~null();
// Member Functions
//- Update the model state for this joint
virtual void jcalc
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const;
};
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
} // End namespace joints
} // End namespace RBD
} // End namespace Foam
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
#endif
// ************************************************************************* //

175
src/rigidBodyDynamics/rigidBodyInertia/rigidBodyInertia.H Normal file
View File

@ -0,0 +1,175 @@
/*---------------------------------------------------------------------------*\
========= |
\\ / 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::rigidBodyInertia
Description
This class represents the linear and angular inertia of a rigid body
by the mass, centre of mass and moment of inertia tensor about the
centre of mass.
Reference:
\verbatim
Featherstone, R. (2008).
Rigid body dynamics algorithms.
Springer.
\endverbatim
SourceFiles
rigidBodyInertiaI.H
rigidBodyInertia.C
rigidBodyInertiaIO.C
\*---------------------------------------------------------------------------*/
#ifndef RBD_rigidBodyInertia_H
#define RBD_rigidBodyInertia_H
#include "vector.H"
#include "symmTensor.H"
#include "spatialVector.H"
#include "spatialTensor.H"
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
namespace Foam
{
// Forward declaration of classes
class Istream;
class Ostream;
namespace RBD
{
// Forward declaration of friend functions and operators
class rigidBodyInertia;
Istream& operator>>(Istream&, rigidBodyInertia&);
Ostream& operator<<(Ostream&, const rigidBodyInertia&);
/*---------------------------------------------------------------------------*\
Class rigidBodyInertia Declaration
\*---------------------------------------------------------------------------*/
class rigidBodyInertia
{
// Private data
//- Mass of the rigid-body
scalar m_;
//- Centre of mass of the rigid-body
vector c_;
//- Inertia tensor about the centre of mass
symmTensor Ic_;
public:
// Static member functions
//- Return the difference between the inertia tensor of the rigid-body
// about the origin - about the centre of mass
// for the given mass and centre of mass
inline static symmTensor Ioc(const scalar m, const vector& c);
// Constructors
//- Null constructor, initializes to zero
inline rigidBodyInertia();
//- Construct from mass, centre of mass and moment of inertia tensor
// about the centre of mass
inline rigidBodyInertia
(
const scalar m,
const vector& c,
const symmTensor& Ic
);
//- Construct from the components of a spatial tensor
inline explicit rigidBodyInertia(const spatialTensor& st);
//- Construct from Istream
inline explicit rigidBodyInertia(Istream& is);
// Member Functions
//- Return the mass of the rigid-body
inline scalar m() const;
//- Return the centre of mass of the rigid-body
inline const vector& c() const;
//- Return the inertia tensor of the rigid-body about the centre of mass
inline const symmTensor& Ic() const;
//- Return the difference between the inertia tensor of the rigid-body
// about the origin - about the centre of mass
inline symmTensor Ioc() const;
//- Return the difference between the inertia tensor of the rigid-body
// about the a new centre of mass - about the current centre of mass
inline symmTensor Icc(const vector& c) const;
//- Return the inertia tensor of the rigid-body about the origin
inline symmTensor Io() const;
//- Return the kinetic energy of the body with the given velocity
inline scalar kineticEnergy(const spatialVector& v);
// Member Operators
//- Conversion to spatial tensor
inline operator spatialTensor() const;
inline void operator+=(const rigidBodyInertia&);
// IOstream Operators
friend Istream& operator>>(Istream&, rigidBodyInertia&);
friend Ostream& operator<<(Ostream&, const rigidBodyInertia&);
};
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
} // End namespace RBD
} // End namespace Foam
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
#include "rigidBodyInertiaI.H"
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
#endif
// ************************************************************************* //

243
src/rigidBodyDynamics/rigidBodyInertia/rigidBodyInertiaI.H Normal file
View File

@ -0,0 +1,243 @@
/*---------------------------------------------------------------------------*\
========= |
\\ / 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 "spatialTransform.H"
#include "transform.H"
// * * * * * * * * * * * * * Static Member Functions * * * * * * * * * * * * //
inline Foam::symmTensor Foam::RBD::rigidBodyInertia::Ioc
(
const scalar m,
const vector& c
)
{
return m*(Foam::I*magSqr(c) - sqr(c));
}
// * * * * * * * * * * * * * * * * Constructors * * * * * * * * * * * * * * //
inline Foam::RBD::rigidBodyInertia::rigidBodyInertia()
:
m_(0),
c_(Zero),
Ic_(Zero)
{}
inline Foam::RBD::rigidBodyInertia::rigidBodyInertia
(
const scalar m,
const vector& c,
const symmTensor& Ic
)
:
m_(m),
c_(c),
Ic_(Ic)
{}
inline Foam::RBD::rigidBodyInertia::rigidBodyInertia(const spatialTensor& st)
:
m_(st(3, 3)),
c_(vector(-st(1, 5), st(0, 5), -st(0, 4))/m_),
Ic_(symm(st.block<tensor, 0, 0>()()) - Ioc())
{}
inline Foam::RBD::rigidBodyInertia::rigidBodyInertia(Istream& is)
:
m_(readScalar(is)),
c_(is),
Ic_(is)
{}
// * * * * * * * * * * * * * * * Member Functions * * * * * * * * * * * * * //
inline Foam::scalar Foam::RBD::rigidBodyInertia::m() const
{
return m_;
}
inline const Foam::vector& Foam::RBD::rigidBodyInertia::c() const
{
return c_;
}
inline const Foam::symmTensor& Foam::RBD::rigidBodyInertia::Ic() const
{
return Ic_;
}
inline Foam::symmTensor Foam::RBD::rigidBodyInertia::Ioc() const
{
return Ioc(m_, c_);
}
inline Foam::symmTensor Foam::RBD::rigidBodyInertia::Icc(const vector& c) const
{
return Ioc(m_, c - c_);
}
inline Foam::symmTensor Foam::RBD::rigidBodyInertia::Io() const
{
return Ic_ + Ioc();
}
// * * * * * * * * * * * * * * * Member Operators * * * * * * * * * * * * * //
inline Foam::RBD::rigidBodyInertia::operator spatialTensor() const
{
tensor mcStar(m_*(*c_));
return spatialTensor
(
Io(), mcStar,
-mcStar, m_*I
);
}
// * * * * * * * * * * * * * * IOstream Operators * * * * * * * * * * * * * //
inline Foam::Istream& Foam::RBD::operator>>
(
Istream& is,
rigidBodyInertia& rbi
)
{
is >> rbi.m_ >> rbi.c_ >> rbi.Ic_;
return is;
}
inline Foam::Ostream& Foam::RBD::operator<<
(
Ostream& os,
const rigidBodyInertia& rbi
)
{
os << rbi.m_ << nl << rbi.c_ << nl << rbi.Ic_ << endl;
return os;
}
// * * * * * * * * * * * * * * * Global Operators * * * * * * * * * * * * * //
namespace Foam
{
namespace RBD
{
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
//- Return the rigid-body inertia of the combined body
inline rigidBodyInertia operator+
(
const rigidBodyInertia& rbi1,
const rigidBodyInertia& rbi2
)
{
const scalar m12 = rbi1.m() + rbi2.m();
const vector c12 = (rbi1.m()*rbi1.c() + rbi2.m()*rbi2.c())/m12;
return rigidBodyInertia
(
m12,
c12,
rbi1.Ic() + rbi1.Icc(c12) + rbi2.Ic() + rbi2.Icc(c12)
);
}
//- Inner-product with a spatialVector (e.g. velocity returning the momentum)
inline spatialVector operator&
(
const rigidBodyInertia& rbi,
const spatialVector& sv
)
{
const vector av(sv.w());
const vector lv(sv.l());
return spatialVector
(
(rbi.Io() & av) + rbi.m()*(rbi.c() ^ lv),
rbi.m()*lv - rbi.m()*(rbi.c() ^ av)
);
}
//- Return (^BX_A)^* I ^AX_B
inline rigidBodyInertia transform
(
const spatialTransform& X,
const rigidBodyInertia& I
)
{
const vector Xc((X.E().T() & I.c()) + X.r());
return rigidBodyInertia
(
I.m(),
Xc,
transform(X.E().T(), I.Ic())
);
}
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
} // End namespace RBD
} // End namespace Foam
// * * * * * * * * * * * * * * * Member Functions * * * * * * * * * * * * * //
inline Foam::scalar Foam::RBD::rigidBodyInertia::kineticEnergy
(
const spatialVector& v
)
{
return 0.5*(v && (*this & v));
}
// * * * * * * * * * * * * * * * Member Operators * * * * * * * * * * * * * //
inline void Foam::RBD::rigidBodyInertia::operator+=
(
const rigidBodyInertia& rbi
)
{
*this = *this + rbi;
}
// ************************************************************************* //

239
src/rigidBodyDynamics/rigidBodyModel/forwardDynamics.C Normal file
View File

@ -0,0 +1,239 @@
/*---------------------------------------------------------------------------*\
========= |
\\ / 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 "rigidBodyModel.H"
// * * * * * * * * * * * * * * Member Functions * * * * * * * * * * * * * * //
void Foam::RBD::rigidBodyModel::forwardDynamics
(
const scalarField& q,
const scalarField& w,
const scalarField& qDot,
const scalarField& tau,
const Field<spatialVector>& fx,
scalarField& qDdot
) const
{
DebugInFunction
<< "q = " << q << nl
<< "qDot = " << qDot << nl
<< "tau = " << tau << endl;
// Joint state returned by jcalc
joint::XSvc J;
v_[0] = Zero;
for (label i=1; i<nBodies(); i++)
{
const joint& jnt = joints()[i];
jnt.jcalc(J, q, w, qDot);
S_[i] = J.S;
S1_[i] = J.S1;
Xlambda_[i] = J.X & XT_[i];
const label lambdai = lambda_[i];
if (lambdai != 0)
{
X0_[i] = Xlambda_[i] & X0_[lambdai];
}
else
{
X0_[i] = Xlambda_[i];
}
v_[i] = (Xlambda_[i] & v_[lambdai]) + J.v;
c_[i] = J.c + (v_[i] ^ J.v);
IA_[i] = I(i);
pA_[i] = v_[i] ^* (I(i) & v_[i]);
if (fx.size())
{
pA_[i] -= *X0_[i] & fx[i];
}
}
for (label i=nBodies()-1; i>0; i--)
{
const joint& jnt = joints()[i];
const label qi = jnt.qIndex();
if (jnt.nDoF() == 1)
{
U1_[i] = IA_[i] & S1_[i];
Dinv_[i].xx() = 1/(S1_[i] && U1_[i]);
u_[i].x() = tau[qi] - (S1_[i] && pA_[i]);
const label lambdai = lambda_[i];
if (lambdai != 0)
{
const spatialTensor Ia
(
IA_[i] - (U1_[i]*(Dinv_[i].xx()*U1_[i]))
);
const spatialVector pa
(
pA_[i] + (Ia & c_[i]) + U1_[i]*(Dinv_[i].xx()*u_[i].x())
);
IA_[lambdai] +=
spatialTensor(Xlambda_[i].T())
& Ia
& spatialTensor(Xlambda_[i]);
pA_[lambdai] += Xlambda_[i].T() & pa;
}
}
else
{
U_[i] = IA_[i] & S_[i];
Dinv_[i] = (S_[i].T() & U_[i]).inv();
u_[i] = tau.block<vector>(qi) - (S_[i].T() & pA_[i]);
const label lambdai = lambda_[i];
if (lambdai != 0)
{
spatialTensor Ia
(
IA_[i]
- (U_[i] & Dinv_[i] & U_[i].T())
);
spatialVector pa
(
pA_[i]
+ (Ia & c_[i])
+ (U_[i] & Dinv_[i] & u_[i])
);
IA_[lambdai] +=
spatialTensor(Xlambda_[i].T())
& Ia
& spatialTensor(Xlambda_[i]);
pA_[lambdai] += Xlambda_[i].T() & pa;
}
}
}
a_[0] = spatialVector(Zero, -g_);
for (label i=1; i<nBodies(); i++)
{
const joint& jnt = joints()[i];
const label qi = jnt.qIndex();
a_[i] = (Xlambda_[i] & a_[lambda_[i]]) + c_[i];
if (jnt.nDoF() == 1)
{
qDdot[qi] = Dinv_[i].xx()*(u_[i].x() - (U1_[i] && a_[i]));
a_[i] += S1_[i]*qDdot[qi];
}
else
{
vector qDdoti(Dinv_[i] & (u_[i] - (U_[i].T() & a_[i])));
// Need to add mutable "block<vector>" to Field
qDdot[qi] = qDdoti.x();
qDdot[qi+1] = qDdoti.y();
qDdot[qi+2] = qDdoti.z();
a_[i] += (S_[i] & qDdoti);
}
}
DebugInfo
<< "qDdot = " << qDdot << nl
<< "a = " << a_ << endl;
}
void Foam::RBD::rigidBodyModel::forwardDynamicsCorrection
(
const scalarField& q,
const scalarField& w,
const scalarField& qDot,
const scalarField& qDdot
) const
{
DebugInFunction << endl;
// Joint state returned by jcalc
joint::XSvc J;
v_[0] = Zero;
a_[0] = spatialVector(Zero, -g_);
for (label i=1; i<nBodies(); i++)
{
const joint& jnt = joints()[i];
const label qi = jnt.qIndex();
jnt.jcalc(J, q, w, qDot);
S_[i] = J.S;
S1_[i] = J.S1;
Xlambda_[i] = J.X & XT_[i];
const label lambdai = lambda_[i];
if (lambdai != 0)
{
X0_[i] = Xlambda_[i] & X0_[lambdai];
}
else
{
X0_[i] = Xlambda_[i];
}
v_[i] = (Xlambda_[i] & v_[lambdai]) + J.v;
c_[i] = J.c + (v_[i] ^ J.v);
a_[i] = (Xlambda_[i] & a_[lambdai]) + c_[i];
if (jnt.nDoF() == 1)
{
a_[i] += S1_[i]*qDdot[qi];
}
else
{
a_[i] += S_[i] & qDdot.block<vector>(qi);
}
}
DebugInfo<< "a = " << a_ << endl;
}
// ************************************************************************* //

224
src/rigidBodyDynamics/rigidBodyModel/rigidBodyModel.C Normal file
View File

@ -0,0 +1,224 @@
/*---------------------------------------------------------------------------*\
========= |
\\ / 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 "rigidBodyModel.H"
#include "masslessBody.H"
#include "nullJoint.H"
// * * * * * * * * * * * * * * Static Data Members * * * * * * * * * * * * * //
namespace Foam
{
namespace RBD
{
defineTypeNameAndDebug(rigidBodyModel, 0);
}
}
// * * * * * * * * * * * * * Private Member Functions * * * * * * * * * * * //
void Foam::RBD::rigidBodyModel::initializeRootBody()
{
bodies_.append(new masslessBody);
lambda_.append(0);
joints_.append(new joints::null(*this));
XT_.append(spatialTransform());
nDoF_ = 0;
nw_ = 0;
resizeState();
}
void Foam::RBD::rigidBodyModel::resizeState()
{
Xlambda_.append(spatialTransform());
X0_.append(spatialTransform());
v_.append(Zero);
a_.append(Zero);
c_.append(Zero);
IA_.append(spatialTensor::I);
pA_.append(Zero);
S_.append(Zero);
S1_.append(Zero);
U_.append(Zero);
U1_.append(Zero);
Dinv_.append(Zero);
u_.append(Zero);
}
// * * * * * * * * * * * * * * * * Constructors * * * * * * * * * * * * * * * //
Foam::RBD::rigidBodyModel::rigidBodyModel()
:
g_(Zero)
{
initializeRootBody();
}
// * * * * * * * * * * * * * * Member Functions * * * * * * * * * * * * * * //
Foam::label Foam::RBD::rigidBodyModel::join
(
const label parentID,
const spatialTransform& XT,
const autoPtr<joint>& jointPtr,
const autoPtr<rigidBody>& bodyPtr
)
{
// Append the body
bodies_.append(bodyPtr);
// If the parentID refers to a merged body find the parent into which it has
// been merged and set lambda and XT accordingly
if (merged(parentID))
{
const subBody& sBody = mergedBody(parentID);
lambda_.append(sBody.parentID());
XT_.append(XT & sBody.parentXT());
}
else
{
lambda_.append(parentID);
XT_.append(XT);
}
// Append the joint
const joint& prevJoint = joints_[joints_.size() - 1];
joints_.append(jointPtr);
joint& curJoint = joints_[joints_.size() - 1];
curJoint.index() = joints_.size() - 1;
curJoint.qIndex() = prevJoint.qIndex() + prevJoint.nDoF();
curJoint.wIndex() = prevJoint.wIndex() + prevJoint.nw();
// Increment the degrees of freedom
nDoF_ += curJoint.nDoF();
nw_ += curJoint.nw();
resizeState();
return nBodies()-1;
}
Foam::label Foam::RBD::rigidBodyModel::join
(
const label parentID,
const spatialTransform& XT,
const PtrList<joint>& compositeJoint,
const autoPtr<rigidBody>& bodyPtr
)
{
label parent = parentID;
// For all but the final joint in the set add a masslessBody with the
// joint and transform
for (label j=0; j<compositeJoint.size()-1; j++)
{
parent = join
(
parent,
j == 0 ? XT : spatialTransform(),
compositeJoint[j].clone(),
autoPtr<rigidBody>(new masslessBody)
);
}
// For the final joint in the set add the read body
return join
(
parent,
compositeJoint.size() == 1 ? XT : spatialTransform(),
compositeJoint[compositeJoint.size()-1].clone(),
bodyPtr
);
}
Foam::label Foam::RBD::rigidBodyModel::merge
(
const label parentID,
const spatialTransform& XT,
const autoPtr<rigidBody>& bodyPtr
)
{
autoPtr<subBody> sBodyPtr;
// If the parentID refers to a merged body find the parent into which it has
// been merged and merge this on into the same parent with the appropriate
// transform
if (merged(parentID))
{
const subBody& sBody = mergedBody(parentID);
sBodyPtr.set
(
new subBody
(
bodyPtr,
sBody.parentID(),
XT & sBody.parentXT()
)
);
}
else
{
sBodyPtr.set(new subBody(bodyPtr, parentID, XT));
}
const subBody& sBody = sBodyPtr();
mergedBodies_.append(sBodyPtr);
// Merge the sub-body with the parent
bodies_[sBody.parentID()].merge(sBody);
return mergedBodyID(mergedBodies_.size() - 1);
}
Foam::spatialTransform Foam::RBD::rigidBodyModel::X0
(
const label bodyId
) const
{
if (merged(bodyId))
{
const subBody& mBody = mergedBody(bodyId);
return mBody.parentXT() & X0_[mBody.parentID()];
}
else
{
return X0_[bodyId];
}
}
// ************************************************************************* //

313
src/rigidBodyDynamics/rigidBodyModel/rigidBodyModel.H Normal file
View File

@ -0,0 +1,313 @@
/*---------------------------------------------------------------------------*\
========= |
\\ / 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::RBD::rigidBodyModel
Description
Basic rigid-body model representing a system of rigid-bodies connected by
1-6 DoF joints.
This class holds various body and joint state fields needed by the
kinematics and forward-dynamics algorithms presented in
reference:
\verbatim
Featherstone, R. (2008).
Rigid body dynamics algorithms.
Springer.
Chapter 4.
\endverbatim
SourceFiles
rigidBodyModel.C
kinematics.C
forwardDynamics.C
\*---------------------------------------------------------------------------*/
#ifndef RBD_rigidBodyModel_H
#define RBD_rigidBodyModel_H
#include "rigidBody.H"
#include "subBody.H"
#include "joint.H"
#include "PtrList.H"
namespace Foam
{
namespace RBD
{
/*---------------------------------------------------------------------------*\
Class rigidBodyModel Declaration
\*---------------------------------------------------------------------------*/
class rigidBodyModel
{
//- Initialize the model with the root-body which is a fixed massless body
// at the origin.
void initializeRootBody();
//- Resize the state fields following the joining of a body
void resizeState();
protected:
// Protected data representing the model structure
//- List of the bodies.
// The 0'th body represents the fixed origin and is constructed
// automatically. The subsequent (moving) bodies are appended by the
// join member function.
PtrList<rigidBody> bodies_;
//- Bodies may be merged into existing bodies, the inertia of which is
// updated to represent the combined body which is more efficient than
// attaching them with fixed joints. These 'merged' bodies are held on
// this list.
PtrList<subBody> mergedBodies_;
//- List of indices of the parent of each body
DynamicList<label> lambda_;
//- Each body it attached with a joint which are held on this list.
PtrList<joint> joints_;
//- Transform from the parent body frame to the joint frame.
DynamicList<spatialTransform> XT_;
//- The number of degrees of freedom of the model
// used to set the size of the of joint state fields q, qDot and qDdot.
label nDoF_;
//- The number of additional state variable needed to describe the joint
// states. Typically this is the number of quaternion joints for which
// the 'w' element is additional to the 3-degrees of freedom of the
// joint.
label nw_;
// Other protected member data
//- Acceleration due to gravity
vector g_;
// Mutable transforms maintained by kinematics and forward-dynamics
//- Transform from the parent body to the current body
mutable DynamicList<spatialTransform> Xlambda_;
//- Transform for external forces to the bodies reference frame
mutable DynamicList<spatialTransform> X0_;
// Mutable kinematic body state fields
//- The spatial velocity of the bodies
mutable DynamicList<spatialVector> v_;
//- The spatial acceleration of the bodies
mutable DynamicList<spatialVector> a_;
//- The velocity dependent spatial acceleration of the joints
mutable DynamicList<spatialVector> c_;
// Mutable state fields needed by the forward-dynamics algorithm
//- Velocity-product acceleration
//- Articulated body inertia
mutable DynamicList<spatialTensor> IA_;
//- Articulated body bias force
mutable DynamicList<spatialVector> pA_;
// Mutable joint state fields
//- Motion subspace for joints with 3 degrees of freedom
mutable DynamicList<compactSpatialTensor> S_;
//- Motion subspace for joints with 1 degrees of freedom
mutable DynamicList<spatialVector> S1_;
//- Sub-expression IA.S in the forward-dynamics algorithm
mutable DynamicList<compactSpatialTensor> U_;
//- Sub-expression IA.S1 in the forward-dynamics algorithm
mutable DynamicList<spatialVector> U1_;
//- Sub-expression (S^T.U)^-1 in the forward-dynamics algorithm
mutable DynamicList<tensor> Dinv_;
//- Sub-expression tau - S^T.pA in the forward-dynamics algorithm
mutable DynamicList<vector> u_;
public:
//- Runtime type information
TypeName("rigidBodyModel");
// Constructors
//- Null-constructor which adds the single root-body at the origin
rigidBodyModel();
//- Destructor
virtual ~rigidBodyModel()
{}
// Member Functions
//- Return the number of bodies in the model (bodies().size())
inline label nBodies() const;
//- Return the list of the bodies in the model
inline PtrList<rigidBody> bodies() const;
//- List of indices of the parent of each body
inline const DynamicList<label>& lambda() const;
//- Return the list of joints in the model
inline const PtrList<joint>& joints() const;
//- Return the number of degrees of freedom of the model
// used to set the size of the of joint state fields q, qDot and qDdot.
inline label nDoF() const;
//- Return the number of additional state variable needed to describe
// the joint states. Typically this is the number of quaternion
// joints for which the 'w' element is additional to the 3-degrees of
// freedom of the joint.
inline label nw() const;
//- Return the acceleration due to gravity
inline const vector& g() const;
//- Allow the acceleration due to gravity to be set
// after model construction
inline vector& g();
//- Return the inertia of body i
inline const rigidBodyInertia& I(const label i) const;
//- Join the given body to the parent with ID parentID via the given
// joint with transform from the parent frame to the joint frame XT.
virtual label join
(
const label parentID,
const spatialTransform& XT,
const autoPtr<joint>& jointPtr,
const autoPtr<rigidBody>& bodyPtr
);
//- Join the given body to the parent with ID parentID via the given
// composite joint (specified as a list of co-located joints) with
// transform from the parent frame to the joint frame XT.
// Composite joins are useful to represent complex joints with degrees
// of freedom other than 1 or 3 which are directly supported.
label join
(
const label parentID,
const spatialTransform& XT,
const PtrList<joint>& compositeJoint,
const autoPtr<rigidBody>& bodyPtr
);
//- Merge the given body with transform X into the parent with ID
// parentID. The parent body assumes the properties of the combined
// body (inertia etc.) and the merged body is held on a
// separate list for reference.
label merge
(
const label parentID,
const spatialTransform& X,
const autoPtr<rigidBody>& bodyPtr
);
//- Return true if the body with given ID has been merged with a parent
inline bool merged(label bodyID) const;
//- Return the index of the merged body in the mergedBody list
// from the given body ID
inline label mergedBodyIndex(const label mergedBodyID) const;
//- Return the merged body ID for the given merged body index
// in the mergedBody list
inline label mergedBodyID(const label mergedBodyIndex) const;
//- Return the merged body for the given body ID
inline const subBody& mergedBody(label mergedBodyID) const;
//- Return the current transform to the global frame for the given body
spatialTransform X0(const label bodyId) const;
//- Calculate the joint acceleration qDdot from the joint state q,
// velocity qDot, internal force tau (in the joint frame) and
// external force fx (in the global frame) using the articulated body
// algorithm (Section 7.3 and Table 7.1)
void forwardDynamics
(
const scalarField& q,
const scalarField& w,
const scalarField& qDot,
const scalarField& tau,
const Field<spatialVector>& fx,
scalarField& qDdot
) 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;
};
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
} // End namespace RBD
} // End namespace Foam
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
#include "rigidBodyModelI.H"
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
#endif
// ************************************************************************* //

120
src/rigidBodyDynamics/rigidBodyModel/rigidBodyModelI.H Normal file
View File

@ -0,0 +1,120 @@
/*---------------------------------------------------------------------------*\
========= |
\\ / 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 Foam::label Foam::RBD::rigidBodyModel::nBodies() const
{
return bodies_.size();
}
inline Foam::PtrList<Foam::RBD::rigidBody>
Foam::RBD::rigidBodyModel::bodies() const
{
return bodies_;
}
inline const Foam::PtrList<Foam::RBD::joint>&
Foam::RBD::rigidBodyModel::joints() const
{
return joints_;
}
inline Foam::label Foam::RBD::rigidBodyModel::nDoF() const
{
return nDoF_;
}
inline Foam::label Foam::RBD::rigidBodyModel::nw() const
{
return nw_;
}
inline const Foam::vector& Foam::RBD::rigidBodyModel::g() const
{
return g_;
}
inline Foam::vector& Foam::RBD::rigidBodyModel::g()
{
return g_;
}
inline const Foam::DynamicList<Foam::label>&
Foam::RBD::rigidBodyModel::lambda() const
{
return lambda_;
}
inline const Foam::RBD::rigidBodyInertia&
Foam::RBD::rigidBodyModel::I(const label i) const
{
return bodies_[i];
}
inline bool Foam::RBD::rigidBodyModel::merged(label bodyID) const
{
return bodyID < 0;
}
inline Foam::label
Foam::RBD::rigidBodyModel::mergedBodyID(const label mergedBodyIndex) const
{
return -1 - mergedBodyIndex;
}
inline Foam::label
Foam::RBD::rigidBodyModel::mergedBodyIndex(const label mergedBodyID) const
{
return -1 - mergedBodyID;
}
inline const Foam::RBD::subBody&
Foam::RBD::rigidBodyModel::mergedBody(label mergedBodyID) const
{
if (!merged(mergedBodyID))
{
FatalErrorInFunction
<< "Body " << mergedBodyID << " has not been merged"
<< abort(FatalError);
}
return mergedBodies_[mergedBodyIndex(mergedBodyID)];
}
// ************************************************************************* //