diff --git a/src/rigidBodyDynamics/Make/files b/src/rigidBodyDynamics/Make/files new file mode 100644 index 0000000000..2ae2c197ad --- /dev/null +++ b/src/rigidBodyDynamics/Make/files @@ -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 diff --git a/src/rigidBodyDynamics/Make/options b/src/rigidBodyDynamics/Make/options new file mode 100644 index 0000000000..79be6f3a7d --- /dev/null +++ b/src/rigidBodyDynamics/Make/options @@ -0,0 +1,3 @@ +EXE_INC = + +LIB_LIBS = diff --git a/src/rigidBodyDynamics/bodies/masslessBody/masslessBody.C b/src/rigidBodyDynamics/bodies/masslessBody/masslessBody.C new file mode 100644 index 0000000000..b5e3cfecd8 --- /dev/null +++ b/src/rigidBodyDynamics/bodies/masslessBody/masslessBody.C @@ -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 . + +\*---------------------------------------------------------------------------*/ + +#include "masslessBody.H" + +// * * * * * * * * * * * * * * * * Destructor * * * * * * * * * * * * * * * // + +Foam::RBD::masslessBody::~masslessBody() +{} + + +// * * * * * * * * * * * * * * Member Functions * * * * * * * * * * * * * * // + +bool Foam::RBD::masslessBody::massless() const +{ + return true; +} + + +// ************************************************************************* // diff --git a/src/rigidBodyDynamics/bodies/masslessBody/masslessBody.H b/src/rigidBodyDynamics/bodies/masslessBody/masslessBody.H new file mode 100644 index 0000000000..46e0104093 --- /dev/null +++ b/src/rigidBodyDynamics/bodies/masslessBody/masslessBody.H @@ -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 . + +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 + +// ************************************************************************* // diff --git a/src/rigidBodyDynamics/bodies/masslessBody/masslessBodyI.H b/src/rigidBodyDynamics/bodies/masslessBody/masslessBodyI.H new file mode 100644 index 0000000000..06acc415b6 --- /dev/null +++ b/src/rigidBodyDynamics/bodies/masslessBody/masslessBodyI.H @@ -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 . + +\*---------------------------------------------------------------------------*/ + +// * * * * * * * * * * * * * * * * Constructors * * * * * * * * * * * * * * // + +inline Foam::RBD::masslessBody::masslessBody() +: + rigidBody("massless", rigidBodyInertia()) +{} + + +inline Foam::RBD::masslessBody::masslessBody(const word& name) +: + rigidBody(name, rigidBodyInertia()) +{} + + +// ************************************************************************* // diff --git a/src/rigidBodyDynamics/bodies/rigidBody/rigidBody.C b/src/rigidBodyDynamics/bodies/rigidBody/rigidBody.C new file mode 100644 index 0000000000..57848850d6 --- /dev/null +++ b/src/rigidBodyDynamics/bodies/rigidBody/rigidBody.C @@ -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 . + +\*---------------------------------------------------------------------------*/ + +#include "rigidBody.H" +#include "subBody.H" + +// * * * * * * * * * * * * * * * * Selectors * * * * * * * * * * * * * * * * // + +Foam::autoPtr Foam::RBD::rigidBody::New +( + const word& name, + const scalar& m, + const vector& c, + const symmTensor& Ic +) +{ + return autoPtr(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) + ); +} + + +// ************************************************************************* // diff --git a/src/rigidBodyDynamics/bodies/rigidBody/rigidBody.H b/src/rigidBodyDynamics/bodies/rigidBody/rigidBody.H new file mode 100644 index 0000000000..8658659e3a --- /dev/null +++ b/src/rigidBodyDynamics/bodies/rigidBody/rigidBody.H @@ -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 . + +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 clone() const; + + + // Selectors + + //- Select constructed from components + static autoPtr 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 + +// ************************************************************************* // diff --git a/src/rigidBodyDynamics/bodies/rigidBody/rigidBodyI.H b/src/rigidBodyDynamics/bodies/rigidBody/rigidBodyI.H new file mode 100644 index 0000000000..2e68928a45 --- /dev/null +++ b/src/rigidBodyDynamics/bodies/rigidBody/rigidBodyI.H @@ -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 . + +\*---------------------------------------------------------------------------*/ + +// * * * * * * * * * * * * * * * * 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::clone() const +{ + return autoPtr(new rigidBody(*this)); +} + + +// * * * * * * * * * * * * * * * Member Functions * * * * * * * * * * * * * // + +inline const Foam::word& Foam::RBD::rigidBody::name() const +{ + return name_; +} + + +// ************************************************************************* // diff --git a/src/rigidBodyDynamics/bodies/subBody/subBody.H b/src/rigidBodyDynamics/bodies/subBody/subBody.H new file mode 100644 index 0000000000..684cb4b7e3 --- /dev/null +++ b/src/rigidBodyDynamics/bodies/subBody/subBody.H @@ -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 . + +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 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 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 + +// ************************************************************************* // diff --git a/src/rigidBodyDynamics/bodies/subBody/subBodyI.H b/src/rigidBodyDynamics/bodies/subBody/subBodyI.H new file mode 100644 index 0000000000..2cfd606cc4 --- /dev/null +++ b/src/rigidBodyDynamics/bodies/subBody/subBodyI.H @@ -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 . + +\*---------------------------------------------------------------------------*/ + +// * * * * * * * * * * * * * * * * Constructors * * * * * * * * * * * * * * // + +inline Foam::RBD::subBody::subBody +( + autoPtr 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_; +} + + +// ************************************************************************* // diff --git a/src/rigidBodyDynamics/joints/Pa/Pa.C b/src/rigidBodyDynamics/joints/Pa/Pa.C new file mode 100644 index 0000000000..8472269504 --- /dev/null +++ b/src/rigidBodyDynamics/joints/Pa/Pa.C @@ -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 . + +\*---------------------------------------------------------------------------*/ + +#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::joints::Pa::clone() const +{ + return autoPtr(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; +} + + +// ************************************************************************* // diff --git a/src/rigidBodyDynamics/joints/Pa/Pa.H b/src/rigidBodyDynamics/joints/Pa/Pa.H new file mode 100644 index 0000000000..99208d1e72 --- /dev/null +++ b/src/rigidBodyDynamics/joints/Pa/Pa.H @@ -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 . + +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 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 + +// ************************************************************************* // diff --git a/src/rigidBodyDynamics/joints/Px/Px.C b/src/rigidBodyDynamics/joints/Px/Px.C new file mode 100644 index 0000000000..3edbb7a2a3 --- /dev/null +++ b/src/rigidBodyDynamics/joints/Px/Px.C @@ -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 . + +\*---------------------------------------------------------------------------*/ + +#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::joints::Px::clone() const +{ + return autoPtr(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; +} + + +// ************************************************************************* // diff --git a/src/rigidBodyDynamics/joints/Px/Px.H b/src/rigidBodyDynamics/joints/Px/Px.H new file mode 100644 index 0000000000..0bbd6028b8 --- /dev/null +++ b/src/rigidBodyDynamics/joints/Px/Px.H @@ -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 . + +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 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 + +// ************************************************************************* // diff --git a/src/rigidBodyDynamics/joints/Pxyz/Pxyz.C b/src/rigidBodyDynamics/joints/Pxyz/Pxyz.C new file mode 100644 index 0000000000..131bcad8c9 --- /dev/null +++ b/src/rigidBodyDynamics/joints/Pxyz/Pxyz.C @@ -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 . + +\*---------------------------------------------------------------------------*/ + +#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::joints::Pxyz::clone() const +{ + return autoPtr(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(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(qIndex_)); + J.c = Zero; +} + + +// ************************************************************************* // diff --git a/src/rigidBodyDynamics/joints/Pxyz/Pxyz.H b/src/rigidBodyDynamics/joints/Pxyz/Pxyz.H new file mode 100644 index 0000000000..b5e87f6f1f --- /dev/null +++ b/src/rigidBodyDynamics/joints/Pxyz/Pxyz.H @@ -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 . + +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 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 + +// ************************************************************************* // diff --git a/src/rigidBodyDynamics/joints/Py/Py.C b/src/rigidBodyDynamics/joints/Py/Py.C new file mode 100644 index 0000000000..e7e5b147ff --- /dev/null +++ b/src/rigidBodyDynamics/joints/Py/Py.C @@ -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 . + +\*---------------------------------------------------------------------------*/ + +#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::joints::Py::clone() const +{ + return autoPtr(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; +} + + +// ************************************************************************* // diff --git a/src/rigidBodyDynamics/joints/Py/Py.H b/src/rigidBodyDynamics/joints/Py/Py.H new file mode 100644 index 0000000000..fb2a87601a --- /dev/null +++ b/src/rigidBodyDynamics/joints/Py/Py.H @@ -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 . + +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 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 + +// ************************************************************************* // diff --git a/src/rigidBodyDynamics/joints/Pz/Pz.C b/src/rigidBodyDynamics/joints/Pz/Pz.C new file mode 100644 index 0000000000..cbccb6dddd --- /dev/null +++ b/src/rigidBodyDynamics/joints/Pz/Pz.C @@ -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 . + +\*---------------------------------------------------------------------------*/ + +#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::joints::Pz::clone() const +{ + return autoPtr(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; +} + + +// ************************************************************************* // diff --git a/src/rigidBodyDynamics/joints/Pz/Pz.H b/src/rigidBodyDynamics/joints/Pz/Pz.H new file mode 100644 index 0000000000..d1a7a4e61f --- /dev/null +++ b/src/rigidBodyDynamics/joints/Pz/Pz.H @@ -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 . + +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 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 + +// ************************************************************************* // diff --git a/src/rigidBodyDynamics/joints/Ra/Ra.C b/src/rigidBodyDynamics/joints/Ra/Ra.C new file mode 100644 index 0000000000..2230863898 --- /dev/null +++ b/src/rigidBodyDynamics/joints/Ra/Ra.C @@ -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 . + +\*---------------------------------------------------------------------------*/ + +#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::joints::Ra::clone() const +{ + return autoPtr(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; +} + + +// ************************************************************************* // diff --git a/src/rigidBodyDynamics/joints/Ra/Ra.H b/src/rigidBodyDynamics/joints/Ra/Ra.H new file mode 100644 index 0000000000..b429678318 --- /dev/null +++ b/src/rigidBodyDynamics/joints/Ra/Ra.H @@ -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 . + +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 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 + +// ************************************************************************* // diff --git a/src/rigidBodyDynamics/joints/Rs/Rs.C b/src/rigidBodyDynamics/joints/Rs/Rs.C new file mode 100644 index 0000000000..c5da501f0e --- /dev/null +++ b/src/rigidBodyDynamics/joints/Rs/Rs.C @@ -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 . + +\*---------------------------------------------------------------------------*/ + +#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::joints::Rs::clone() const +{ + return autoPtr(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(qIndex_), Zero); + J.c = Zero; +} + + +// ************************************************************************* // diff --git a/src/rigidBodyDynamics/joints/Rs/Rs.H b/src/rigidBodyDynamics/joints/Rs/Rs.H new file mode 100644 index 0000000000..3e1b0655f3 --- /dev/null +++ b/src/rigidBodyDynamics/joints/Rs/Rs.H @@ -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 . + +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 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 + +// ************************************************************************* // diff --git a/src/rigidBodyDynamics/joints/Rx/Rx.C b/src/rigidBodyDynamics/joints/Rx/Rx.C new file mode 100644 index 0000000000..c90f6fd976 --- /dev/null +++ b/src/rigidBodyDynamics/joints/Rx/Rx.C @@ -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 . + +\*---------------------------------------------------------------------------*/ + +#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::joints::Rx::clone() const +{ + return autoPtr(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; +} + + +// ************************************************************************* // diff --git a/src/rigidBodyDynamics/joints/Rx/Rx.H b/src/rigidBodyDynamics/joints/Rx/Rx.H new file mode 100644 index 0000000000..79ca52d9f2 --- /dev/null +++ b/src/rigidBodyDynamics/joints/Rx/Rx.H @@ -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 . + +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 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 + +// ************************************************************************* // diff --git a/src/rigidBodyDynamics/joints/Rxyz/Rxyz.C b/src/rigidBodyDynamics/joints/Rxyz/Rxyz.C new file mode 100644 index 0000000000..8dda6d3e6a --- /dev/null +++ b/src/rigidBodyDynamics/joints/Rxyz/Rxyz.C @@ -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 . + +\*---------------------------------------------------------------------------*/ + +#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::joints::Rxyz::clone() const +{ + return autoPtr(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(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(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 + ); +} + + +// ************************************************************************* // diff --git a/src/rigidBodyDynamics/joints/Rxyz/Rxyz.H b/src/rigidBodyDynamics/joints/Rxyz/Rxyz.H new file mode 100644 index 0000000000..7ada2cc4a5 --- /dev/null +++ b/src/rigidBodyDynamics/joints/Rxyz/Rxyz.H @@ -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 . + +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 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 + +// ************************************************************************* // diff --git a/src/rigidBodyDynamics/joints/Ry/Ry.C b/src/rigidBodyDynamics/joints/Ry/Ry.C new file mode 100644 index 0000000000..4814264f1b --- /dev/null +++ b/src/rigidBodyDynamics/joints/Ry/Ry.C @@ -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 . + +\*---------------------------------------------------------------------------*/ + +#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::joints::Ry::clone() const +{ + return autoPtr(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; +} + + +// ************************************************************************* // diff --git a/src/rigidBodyDynamics/joints/Ry/Ry.H b/src/rigidBodyDynamics/joints/Ry/Ry.H new file mode 100644 index 0000000000..4a88a659a4 --- /dev/null +++ b/src/rigidBodyDynamics/joints/Ry/Ry.H @@ -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 . + +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 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 + +// ************************************************************************* // diff --git a/src/rigidBodyDynamics/joints/Ryxz/Ryxz.C b/src/rigidBodyDynamics/joints/Ryxz/Ryxz.C new file mode 100644 index 0000000000..a52896299a --- /dev/null +++ b/src/rigidBodyDynamics/joints/Ryxz/Ryxz.C @@ -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 . + +\*---------------------------------------------------------------------------*/ + +#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::joints::Ryxz::clone() const +{ + return autoPtr(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(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(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 + ); +} + + +// ************************************************************************* // diff --git a/src/rigidBodyDynamics/joints/Ryxz/Ryxz.H b/src/rigidBodyDynamics/joints/Ryxz/Ryxz.H new file mode 100644 index 0000000000..604e849b3a --- /dev/null +++ b/src/rigidBodyDynamics/joints/Ryxz/Ryxz.H @@ -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 . + +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 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 + +// ************************************************************************* // diff --git a/src/rigidBodyDynamics/joints/Rz/Rz.C b/src/rigidBodyDynamics/joints/Rz/Rz.C new file mode 100644 index 0000000000..1465dfc460 --- /dev/null +++ b/src/rigidBodyDynamics/joints/Rz/Rz.C @@ -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 . + +\*---------------------------------------------------------------------------*/ + +#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::joints::Rz::clone() const +{ + return autoPtr(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; +} + + +// ************************************************************************* // diff --git a/src/rigidBodyDynamics/joints/Rz/Rz.H b/src/rigidBodyDynamics/joints/Rz/Rz.H new file mode 100644 index 0000000000..b14b6408fb --- /dev/null +++ b/src/rigidBodyDynamics/joints/Rz/Rz.H @@ -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 . + +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 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 + +// ************************************************************************* // diff --git a/src/rigidBodyDynamics/joints/Rzyx/Rzyx.C b/src/rigidBodyDynamics/joints/Rzyx/Rzyx.C new file mode 100644 index 0000000000..f55775330f --- /dev/null +++ b/src/rigidBodyDynamics/joints/Rzyx/Rzyx.C @@ -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 . + +\*---------------------------------------------------------------------------*/ + +#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::joints::Rzyx::clone() const +{ + return autoPtr(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(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(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 + ); +} + + +// ************************************************************************* // diff --git a/src/rigidBodyDynamics/joints/Rzyx/Rzyx.H b/src/rigidBodyDynamics/joints/Rzyx/Rzyx.H new file mode 100644 index 0000000000..52c53939f1 --- /dev/null +++ b/src/rigidBodyDynamics/joints/Rzyx/Rzyx.H @@ -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 . + +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 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 + +// ************************************************************************* // diff --git a/src/rigidBodyDynamics/joints/joint/joint.C b/src/rigidBodyDynamics/joints/joint/joint.C new file mode 100644 index 0000000000..04f41d65a4 --- /dev/null +++ b/src/rigidBodyDynamics/joints/joint/joint.C @@ -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 . + +\*---------------------------------------------------------------------------*/ + +#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::New(joint* jointPtr) +{ + return autoPtr(jointPtr); +} + + +// * * * * * * * * * * * * * * * * Destructor * * * * * * * * * * * * * * * // + +Foam::RBD::joint::~joint() +{} + + +// * * * * * * * * * * * * * * Member Functions * * * * * * * * * * * * * * // + +Foam::PtrList Foam::RBD::joint::floating +( + const rigidBodyModel& model +) +{ + PtrList 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 +{} + + +// ************************************************************************* // diff --git a/src/rigidBodyDynamics/joints/joint/joint.H b/src/rigidBodyDynamics/joints/joint/joint.H new file mode 100644 index 0000000000..4acfb7da95 --- /dev/null +++ b/src/rigidBodyDynamics/joints/joint/joint.H @@ -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 . + +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 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 clone() const = 0; + + + //- Destructor + virtual ~joint(); + + + // Selectors + + //- Simple selector to return an autoPtr of the given joint* + static autoPtr New(joint* jointPtr); + + //- Select constructed from dictionary + static autoPtr 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& S() const; + + //- Return a list of joints needed to emulate a floating body + static PtrList 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 + +// ************************************************************************* // diff --git a/src/rigidBodyDynamics/joints/joint/jointI.H b/src/rigidBodyDynamics/joints/joint/jointI.H new file mode 100644 index 0000000000..f2020c047c --- /dev/null +++ b/src/rigidBodyDynamics/joints/joint/jointI.H @@ -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 . + +\*---------------------------------------------------------------------------*/ + +// * * * * * * * * * * * * * * * * 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::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(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(); +} + + +// ************************************************************************* // diff --git a/src/rigidBodyDynamics/joints/joints.H b/src/rigidBodyDynamics/joints/joints.H new file mode 100644 index 0000000000..2ddd6c905c --- /dev/null +++ b/src/rigidBodyDynamics/joints/joints.H @@ -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" diff --git a/src/rigidBodyDynamics/joints/nullJoint/nullJoint.C b/src/rigidBodyDynamics/joints/nullJoint/nullJoint.C new file mode 100644 index 0000000000..04cccfb6c2 --- /dev/null +++ b/src/rigidBodyDynamics/joints/nullJoint/nullJoint.C @@ -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 . + +\*---------------------------------------------------------------------------*/ + +#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::joints::null::clone() const +{ + return autoPtr(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); +} + + +// ************************************************************************* // diff --git a/src/rigidBodyDynamics/joints/nullJoint/nullJoint.H b/src/rigidBodyDynamics/joints/nullJoint/nullJoint.H new file mode 100644 index 0000000000..d7fc158d1a --- /dev/null +++ b/src/rigidBodyDynamics/joints/nullJoint/nullJoint.H @@ -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 . + +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 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 + +// ************************************************************************* // diff --git a/src/rigidBodyDynamics/rigidBodyInertia/rigidBodyInertia.H b/src/rigidBodyDynamics/rigidBodyInertia/rigidBodyInertia.H new file mode 100644 index 0000000000..b5f1233837 --- /dev/null +++ b/src/rigidBodyDynamics/rigidBodyInertia/rigidBodyInertia.H @@ -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 . + +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 + +// ************************************************************************* // diff --git a/src/rigidBodyDynamics/rigidBodyInertia/rigidBodyInertiaI.H b/src/rigidBodyDynamics/rigidBodyInertia/rigidBodyInertiaI.H new file mode 100644 index 0000000000..96d0264126 --- /dev/null +++ b/src/rigidBodyDynamics/rigidBodyInertia/rigidBodyInertiaI.H @@ -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 . + +\*---------------------------------------------------------------------------*/ + +#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()()) - 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; +} + + +// ************************************************************************* // diff --git a/src/rigidBodyDynamics/rigidBodyModel/forwardDynamics.C b/src/rigidBodyDynamics/rigidBodyModel/forwardDynamics.C new file mode 100644 index 0000000000..fc7bfd5e0c --- /dev/null +++ b/src/rigidBodyDynamics/rigidBodyModel/forwardDynamics.C @@ -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 . + +\*---------------------------------------------------------------------------*/ + +#include "rigidBodyModel.H" + +// * * * * * * * * * * * * * * Member Functions * * * * * * * * * * * * * * // + +void Foam::RBD::rigidBodyModel::forwardDynamics +( + const scalarField& q, + const scalarField& w, + const scalarField& qDot, + const scalarField& tau, + const Field& 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; i0; 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(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" 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(qi); + } + } + + DebugInfo<< "a = " << a_ << endl; +} + + +// ************************************************************************* // diff --git a/src/rigidBodyDynamics/rigidBodyModel/rigidBodyModel.C b/src/rigidBodyDynamics/rigidBodyModel/rigidBodyModel.C new file mode 100644 index 0000000000..74a6565efd --- /dev/null +++ b/src/rigidBodyDynamics/rigidBodyModel/rigidBodyModel.C @@ -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 . + +\*---------------------------------------------------------------------------*/ + +#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& jointPtr, + const autoPtr& 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& compositeJoint, + const autoPtr& 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(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& bodyPtr +) +{ + autoPtr 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]; + } +} + + +// ************************************************************************* // diff --git a/src/rigidBodyDynamics/rigidBodyModel/rigidBodyModel.H b/src/rigidBodyDynamics/rigidBodyModel/rigidBodyModel.H new file mode 100644 index 0000000000..aa3db57876 --- /dev/null +++ b/src/rigidBodyDynamics/rigidBodyModel/rigidBodyModel.H @@ -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 . + +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 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 mergedBodies_; + + //- List of indices of the parent of each body + DynamicList