rigidBodyDynamics: Added support for restraints and a linear spring with damper

applications/test/rigidBodyDynamics/spring: Test of the linear spring with damper restraint
Damped simple harmonic motion of a weight on a spring is simulated and
the results compared with analytical solution

    Test-spring
    gnuplot spring.gnuplot
    evince spring.eps

This development is sponsored by Carnegie Wave Energy Ltd.

applications/test/rigidBodyDynamics/spring/Make/files

@@ -0,0 +1,3 @@
+spring.C
+
+EXE = $(FOAM_USER_APPBIN)/Test-spring

applications/test/rigidBodyDynamics/spring/Make/options

@@ -0,0 +1,5 @@
+EXE_INC = \
+    -I$(LIB_SRC)/rigidBodyDynamics/lnInclude
+
+EXE_LIBS = \
+    -lrigidBodyDynamics

applications/test/rigidBodyDynamics/spring/spring

@@ -0,0 +1,30 @@
+bodies
+{
+    weight
+    {
+        type            rigidBody;
+        mass            9.6;
+        centreOfMass    (0 0 0);
+        inertia         (0.1052 0 0 0.1052 0 0.1778);
+        parent          root;
+        transform       (1 0 0 0 1 0 0 0 1) (0 0 0);
+        joint
+        {
+            type            Pz;
+        }
+    }
+}
+
+restraints
+{
+    spring
+    {
+        type            linearSpring;
+        body            weight;
+        anchor          (0 0 0.5);
+        refAttachmentPt (0 0 0);
+        stiffness       5000;
+        damping         50;
+        restLength      0.4;
+    }
+}

applications/test/rigidBodyDynamics/spring/spring.C

@@ -0,0 +1,109 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+Application
+    spring
+
+Description
+    Simple weight and damped-spirng simulation with 1-DoF.
+
+\*---------------------------------------------------------------------------*/
+
+#include "rigidBodyModel.H"
+#include "masslessBody.H"
+#include "sphere.H"
+#include "joints.H"
+#include "IFstream.H"
+#include "OFstream.H"
+
+using namespace Foam;
+using namespace RBD;
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+int main(int argc, char *argv[])
+{
+    // Create the spring model from dictionary
+    rigidBodyModel spring(dictionary(IFstream("spring")()));
+
+    Info<< spring << endl;
+
+    // Create the joint-space state fields
+    scalarField q(spring.nDoF(), Zero);
+    scalarField w(spring.nw(), Zero);
+    scalarField qDot(spring.nDoF(), Zero);
+    scalarField qDdot(spring.nDoF(), Zero);
+    scalarField tau(spring.nDoF(), Zero);
+    Field<spatialVector> fx(spring.nBodies(), Zero);
+
+    OFstream qFile("qVsTime");
+    OFstream qDotFile("qDotVsTime");
+
+    // Integrate the motion of the spring for 4s using a symplectic method
+    scalar deltaT = 0.002;
+    for (scalar t=0; t<4; t+=deltaT)
+    {
+        qDot += 0.5*deltaT*qDdot;
+        q += deltaT*qDot;
+
+        // Update the body-state prior to the evaluation of the restraints
+        spring.forwardDynamicsCorrection
+        (
+            q,
+            w,
+            qDot,
+            qDdot
+        );
+
+        // Accumulate the restraint forces
+        fx = Zero;
+        spring.applyRestraints(fx);
+
+        // Calculate the body acceleration for the given state
+        // and restraint forces
+        spring.forwardDynamics
+        (
+            q,
+            w,
+            qDot,
+            tau,
+            fx,
+            qDdot
+        );
+
+        // Update the velocity
+        qDot += 0.5*deltaT*qDdot;
+
+        // Write the results for graph generation
+        // using 'gnuplot spring.gnuplot'
+        qFile << t << " " << q[0] << endl;
+        qDotFile << t << " " << qDot[0] << endl;
+    }
+
+    Info<< "\nEnd\n" << endl;
+
+    return 0;
+}
+
+
+// ************************************************************************* //

applications/test/rigidBodyDynamics/spring/spring.gnuplot

@@ -0,0 +1,76 @@
+#------------------------------------------------------------------------------
+# =========                 |
+# \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+#  \\    /   O peration     |
+#   \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+#    \\/     M anipulation  |
+#-------------------------------------------------------------------------------
+# License
+#     This file is part of OpenFOAM.
+#
+#     OpenFOAM is free software: you can redistribute it and/or modify it
+#     under the terms of the GNU General Public License as published by
+#     the Free Software Foundation, either version 3 of the License, or
+#     (at your option) any later version.
+#
+#     OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+#     ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+#     FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+#     for more details.
+#
+#     You should have received a copy of the GNU General Public License
+#     along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+#
+# Script
+#     spring.gnuplot
+#
+# Description
+#     Creates an PostScript graph file of Test-spring results vs
+#     the analytical solution.
+#
+#------------------------------------------------------------------------------
+
+reset
+
+set samples 2000
+
+k = 5000.0
+m = 9.6
+c = 50.0
+a = -0.1
+
+omega = sqrt(k/m)
+zeta = c/(2.0*m*omega)
+
+phi = atan((sqrt(1.0 - zeta**2))/zeta)
+A = a/sin(phi)
+
+pos(A, t, omega, phi, zeta) = A*exp(-zeta*omega*t)*sin(sqrt(1-zeta**2)*omega*t + phi)
+vel(A, t, omega, phi, zeta) = \
+A*exp(-zeta*omega*t)*\
+( \
+  sqrt(1-zeta**2)*omega*cos(sqrt(1-zeta**2)*omega*t + phi) \
+- zeta*omega*sin(sqrt(1-zeta**2)*omega*t + phi) \
+)
+
+set xlabel "Time/[s]"
+set ylabel "Position"
+
+set ytics nomirror
+set y2tics
+
+set yrange [-0.1:0.1]
+set y2range [-2:2]
+
+set xzeroaxis
+
+set terminal postscript eps color enhanced solid
+set output "spring.eps"
+
+plot \
+    "qVsTime" u 1:($2 - 0.1) w l t "Simulation, centre of mass relative to start", \
+    pos(A, x, omega, phi, zeta) w l t "Analytical solution, centre of mass", \
+    "qDotVsTime" u 1:2 w l axes x1y2 t "Simulation, vertical velocity", \
+    vel(A, x, omega, phi, zeta) w l axes x1y2 t "Analytical solution, vertical velocity"
+
+#------------------------------------------------------------------------------

src/rigidBodyDynamics/Make/files

@@ -25,6 +25,10 @@ joints/Pz/Pz.C
 joints/Pa/Pa.C
 joints/Pxyz/Pxyz.C
 
+restraints/restraint/rigidBodyRestraint.C
+restraints/restraint/rigidBodyRestraintNew.C
+restraints/linearSpring/linearSpring.C
+
 rigidBodyModel/rigidBodyModel.C
 rigidBodyModel/forwardDynamics.C
 

src/rigidBodyDynamics/restraints/linearSpring/linearSpring.C

@@ -0,0 +1,152 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+\*---------------------------------------------------------------------------*/
+
+#include "linearSpring.H"
+#include "rigidBodyModel.H"
+#include "addToRunTimeSelectionTable.H"
+
+// * * * * * * * * * * * * * * Static Data Members * * * * * * * * * * * * * //
+
+namespace Foam
+{
+namespace RBD
+{
+namespace restraints
+{
+    defineTypeNameAndDebug(linearSpring, 0);
+
+    addToRunTimeSelectionTable
+    (
+        restraint,
+        linearSpring,
+        dictionary
+    );
+}
+}
+}
+
+
+// * * * * * * * * * * * * * * * * Constructors  * * * * * * * * * * * * * * //
+
+Foam::RBD::restraints::linearSpring::linearSpring
+(
+    const word& name,
+    const dictionary& dict,
+    const rigidBodyModel& model
+)
+:
+    restraint(name, dict, model),
+    anchor_(),
+    refAttachmentPt_(),
+    stiffness_(),
+    damping_(),
+    restLength_()
+{
+    read(dict);
+}
+
+
+// * * * * * * * * * * * * * * * * Destructor  * * * * * * * * * * * * * * * //
+
+Foam::RBD::restraints::linearSpring::~linearSpring()
+{}
+
+
+// * * * * * * * * * * * * * * Member Functions  * * * * * * * * * * * * * * //
+
+Foam::spatialVector Foam::RBD::restraints::linearSpring::restrain() const
+{
+    spatialVector attachmentPt
+    (
+        model_.X0(bodyIndex_).inv() && spatialVector(Zero, refAttachmentPt_)
+    );
+
+    // Current axis of the spring
+    vector r = attachmentPt.l() - anchor_;
+    scalar magR = mag(r);
+    r /= (magR + VSMALL);
+
+    // Velocity of the end of the spring
+    vector v = model_.v(bodyIndex_, refAttachmentPt_).l();
+
+    // Force and moment including optional damping
+    vector force = (-stiffness_*(magR - restLength_) - damping_*(r & v))*r;
+    vector moment = (attachmentPt.l() - model_.X0(bodyIndex_).r()) ^ force;
+
+    if (model_.debug)
+    {
+        Info<< " attachmentPt - anchor " << r*magR
+            << " spring length " << magR
+            << " force " << force
+            << " moment " << moment
+            << endl;
+    }
+
+    return spatialVector(moment, force);
+}
+
+
+bool Foam::RBD::restraints::linearSpring::read
+(
+    const dictionary& dict
+)
+{
+    restraint::read(dict);
+
+    coeffs_.lookup("anchor") >> anchor_;
+    coeffs_.lookup("refAttachmentPt") >> refAttachmentPt_;
+    coeffs_.lookup("stiffness") >> stiffness_;
+    coeffs_.lookup("damping") >> damping_;
+    coeffs_.lookup("restLength") >> restLength_;
+
+    return true;
+}
+
+
+void Foam::RBD::restraints::linearSpring::write
+(
+    Ostream& os
+) const
+{
+    restraint::write(os);
+
+    os.writeKeyword("anchor")
+        << anchor_ << token::END_STATEMENT << nl;
+
+    os.writeKeyword("refAttachmentPt")
+        << refAttachmentPt_ << token::END_STATEMENT << nl;
+
+    os.writeKeyword("stiffness")
+        << stiffness_ << token::END_STATEMENT << nl;
+
+    os.writeKeyword("damping")
+        << damping_ << token::END_STATEMENT << nl;
+
+    os.writeKeyword("restLength")
+        << restLength_ << token::END_STATEMENT << nl;
+}
+
+
+// ************************************************************************* //

src/rigidBodyDynamics/restraints/linearSpring/linearSpring.H

@@ -0,0 +1,130 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+Class
+    Foam::RBD::restraints::linearSpring
+
+Description
+    Linear spring restraint.
+
+SourceFiles
+    linearSpring.C
+
+\*---------------------------------------------------------------------------*/
+
+#ifndef linearSpring_H
+#define linearSpring_H
+
+#include "rigidBodyRestraint.H"
+#include "point.H"
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+namespace Foam
+{
+namespace RBD
+{
+namespace restraints
+{
+
+/*---------------------------------------------------------------------------*\
+                          Class linearSpring Declaration
+\*---------------------------------------------------------------------------*/
+
+class linearSpring
+:
+    public restraint
+{
+    // Private data
+
+        //- Anchor point, where the spring is attached to an immovable
+        //  object
+        point anchor_;
+
+        //- Reference point of attachment to the solid body
+        point refAttachmentPt_;
+
+        //- Spring stiffness coefficient (N/m)
+        scalar stiffness_;
+
+        //- Damping coefficient (Ns/m)
+        scalar damping_;
+
+        //- Rest length - length of spring when no forces are applied to it
+        scalar restLength_;
+
+
+public:
+
+    //- Runtime type information
+    TypeName("linearSpring");
+
+
+    // Constructors
+
+        //- Construct from components
+        linearSpring
+        (
+            const word& name,
+            const dictionary& dict,
+            const rigidBodyModel& model
+        );
+
+        //- Construct and return a clone
+        virtual autoPtr<restraint> clone() const
+        {
+            return autoPtr<restraint>
+            (
+                new linearSpring(*this)
+            );
+        }
+
+
+    //- Destructor
+    virtual ~linearSpring();
+
+
+    // Member Functions
+
+        //- Return the external force applied to the body by this restraint
+        virtual spatialVector restrain() const;
+
+        //- Update properties from given dictionary
+        virtual bool read(const dictionary& dict);
+
+        //- Write
+        virtual void write(Ostream&) const;
+};
+
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+} // End namespace restraints
+} // End namespace RBD
+} // End namespace Foam
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+#endif
+
+// ************************************************************************* //

src/rigidBodyDynamics/restraints/restraint/rigidBodyRestraint.C

@@ -0,0 +1,84 @@
+/*---------------------------------------------------------------------------* \
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+\*---------------------------------------------------------------------------*/
+
+#include "rigidBodyRestraint.H"
+#include "rigidBodyModel.H"
+
+// * * * * * * * * * * * * * * Static Data Members * * * * * * * * * * * * * //
+
+namespace Foam
+{
+namespace RBD
+{
+    defineTypeNameAndDebug(restraint, 0);
+    defineRunTimeSelectionTable(restraint, dictionary);
+}
+}
+
+
+// * * * * * * * * * * * * * * * * Constructors  * * * * * * * * * * * * * * //
+
+Foam::RBD::restraint::restraint
+(
+    const word& name,
+    const dictionary& dict,
+    const rigidBodyModel& model
+)
+:
+    name_(name),
+    bodyIndex_(model.bodyID(dict.lookup("body"))),
+    coeffs_(dict),
+    model_(model)
+{}
+
+
+// * * * * * * * * * * * * * * * * Destructor  * * * * * * * * * * * * * * * //
+
+Foam::RBD::restraint::~restraint()
+{}
+
+
+// * * * * * * * * * * * * * * Member Functions  * * * * * * * * * * * * * * //
+
+const Foam::dictionary& Foam::RBD::restraint::coeffDict() const
+{
+    return coeffs_;
+}
+
+
+bool Foam::RBD::restraint::read(const dictionary& dict)
+{
+    coeffs_ = dict;
+    return true;
+}
+
+
+void Foam::RBD::restraint::write(Ostream& os) const
+{
+    os.writeKeyword("type") << type() << token::END_STATEMENT << nl;
+}
+
+
+// ************************************************************************* //

src/rigidBodyDynamics/restraints/restraint/rigidBodyRestraint.H

@@ -0,0 +1,171 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+Namespace
+    Foam::RBD::restraints
+
+Description
+    Namespace for rigid-body dynamics restraints
+
+Class
+    Foam::RBD::restraint
+
+Description
+    Base class for defining restraints for rigid-body dynamics
+
+SourceFiles
+    rigidBodyRestraint.C
+    rigidBodyRestraintNew.C
+
+\*---------------------------------------------------------------------------*/
+
+#ifndef RBD_restraint_H
+#define RBD_restraint_H
+
+#include "dictionary.H"
+#include "autoPtr.H"
+#include "spatialVector.H"
+#include "runTimeSelectionTables.H"
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+namespace Foam
+{
+namespace RBD
+{
+
+// Forward declaration of classes
+class rigidBodyModel;
+
+/*---------------------------------------------------------------------------*\
+                Class restraint Declaration
+\*---------------------------------------------------------------------------*/
+
+class restraint
+{
+
+protected:
+
+    // Protected data
+
+        //- Name of the restraint
+        word name_;
+
+        //- Index of the body the restraint is applied to
+        label bodyIndex_;
+
+        //- Restraint model specific coefficient dictionary
+        dictionary coeffs_;
+
+        //- Reference to the model
+        const rigidBodyModel& model_;
+
+
+public:
+
+    //- Runtime type information
+    TypeName("restraint");
+
+
+    // Declare run-time constructor selection table
+
+        declareRunTimeSelectionTable
+        (
+            autoPtr,
+            restraint,
+            dictionary,
+            (
+                const word& name,
+                const dictionary& dict,
+                const rigidBodyModel& model
+            ),
+            (name, dict, model)
+        );
+
+
+    // Constructors
+
+        //- Construct from the dict dictionary and Time
+        restraint
+        (
+            const word& name,
+            const dictionary& dict,
+            const rigidBodyModel& model
+        );
+
+        //- Construct and return a clone
+        virtual autoPtr<restraint> clone() const = 0;
+
+
+    // Selectors
+
+        //- Select constructed from the dict dictionary and Time
+        static autoPtr<restraint> New
+        (
+            const word& name,
+            const dictionary& dict,
+            const rigidBodyModel& model
+        );
+
+
+    //- Destructor
+    virtual ~restraint();
+
+
+    // Member Functions
+
+        //- Return the name
+        const word& name() const
+        {
+            return name_;
+        }
+
+        label bodyIndex() const
+        {
+            return bodyIndex_;
+        }
+
+        //- Return the external force applied to the body by this restraint
+        virtual spatialVector restrain() const = 0;
+
+        //- Update properties from given dictionary
+        virtual bool read(const dictionary& dict);
+
+        //- Return access to coeffs
+        const dictionary& coeffDict() const;
+
+        //- Write
+        virtual void write(Ostream&) const = 0;
+};
+
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+} // End namespace RBD
+} // End namespace Foam
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+
+#endif
+
+// ************************************************************************* //

src/rigidBodyDynamics/restraints/restraint/rigidBodyRestraintNew.C

@@ -0,0 +1,57 @@
+/*---------------------------------------------------------------------------*\
+  =========                 |
+  \\      /  F ield         | OpenFOAM: The Open Source CFD Toolbox
+   \\    /   O peration     |
+    \\  /    A nd           | Copyright (C) 2016 OpenFOAM Foundation
+     \\/     M anipulation  |
+-------------------------------------------------------------------------------
+License
+    This file is part of OpenFOAM.
+
+    OpenFOAM is free software: you can redistribute it and/or modify it
+    under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
+    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+    for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.
+
+\*---------------------------------------------------------------------------*/
+
+#include "rigidBodyRestraint.H"
+
+// * * * * * * * * * * * * * * * * Selectors * * * * * * * * * * * * * * * * //
+
+Foam::autoPtr<Foam::RBD::restraint>
+Foam::RBD::restraint::New
+(
+    const word& name,
+    const dictionary& dict,
+    const rigidBodyModel& model
+)
+{
+    const word restraintType(dict.lookup("type"));
+
+    dictionaryConstructorTable::iterator cstrIter =
+        dictionaryConstructorTablePtr_->find(restraintType);
+
+    if (cstrIter == dictionaryConstructorTablePtr_->end())
+    {
+        FatalErrorInFunction
+            << "Unknown restraint type "
+            << restraintType << nl << nl
+            << "Valid restraint types are : " << endl
+            << dictionaryConstructorTablePtr_->sortedToc()
+            << exit(FatalError);
+    }
+
+    return autoPtr<restraint>(cstrIter()(name, dict, model));
+}
+
+
+// ************************************************************************* //

src/rigidBodyDynamics/rigidBodyModel/forwardDynamics.C

@@ -27,6 +27,23 @@ License
 
 // * * * * * * * * * * * * * * Member Functions  * * * * * * * * * * * * * * //
 
+void Foam::RBD::rigidBodyModel::applyRestraints(Field<spatialVector>& fx) const
+{
+    if (restraints_.empty())
+    {
+        return;
+    }
+
+    forAll(restraints_, ri)
+    {
+        DebugInfo << "Restraint " << restraints_[ri].name();
+
+        // Accumulate the restraint forces
+        fx[restraints_[ri].bodyIndex()] += restraints_[ri].restrain();
+    }
+}
+
+
 void Foam::RBD::rigidBodyModel::forwardDynamics
 (
     const scalarField& q,

src/rigidBodyDynamics/rigidBodyModel/rigidBodyModel.C

@@ -78,6 +78,41 @@ void Foam::RBD::rigidBodyModel::resizeState()
 }
 
 
+void Foam::RBD::rigidBodyModel::addRestraints
+(
+    const dictionary& dict
+)
+{
+    if (dict.found("restraints"))
+    {
+        const dictionary& restraintDict = dict.subDict("restraints");
+
+        label i = 0;
+
+        restraints_.setSize(restraintDict.size());
+
+        forAllConstIter(IDLList<entry>, restraintDict, iter)
+        {
+            if (iter().isDict())
+            {
+                restraints_.set
+                (
+                    i++,
+                    restraint::New
+                    (
+                        iter().keyword(),
+                        iter().dict(),
+                        *this
+                    )
+                );
+            }
+        }
+
+        restraints_.setSize(i);
+    }
+}
+
+
 // * * * * * * * * * * * * * * * * Constructors * * * * * * * * * * * * * * * //
 
 Foam::RBD::rigidBodyModel::rigidBodyModel()
@@ -120,6 +155,9 @@ Foam::RBD::rigidBodyModel::rigidBodyModel(const dictionary& dict)
             );
         }
     }
+
+    // Read the restraints and any other re-readable settings.
+    read(dict);
 }
 
 
@@ -144,8 +182,8 @@ Foam::label Foam::RBD::rigidBodyModel::join_
     if (merged(parentID))
     {
         const subBody& sBody = mergedBody(parentID);
-        lambda_.append(sBody.parentID());
-        XT_.append(XT & sBody.parentXT());
+        lambda_.append(sBody.masterID());
+        XT_.append(XT & sBody.masterXT());
     }
     else
     {
@@ -279,16 +317,16 @@ Foam::label Foam::RBD::rigidBodyModel::merge
     {
         const subBody& sBody = mergedBody(parentID);
 
-        makeComposite(sBody.parentID());
+        makeComposite(sBody.masterID());
 
         sBodyPtr.set
         (
             new subBody
             (
                 bodyPtr,
-                bodies_[sBody.parentID()].name(),
-                sBody.parentID(),
-                XT & sBody.parentXT()
+                bodies_[sBody.masterID()].name(),
+                sBody.masterID(),
+                XT & sBody.masterXT()
             )
         );
     }
@@ -312,7 +350,7 @@ Foam::label Foam::RBD::rigidBodyModel::merge
     mergedBodies_.append(sBodyPtr);
 
     // Merge the sub-body with the parent
-    bodies_[sBody.parentID()].merge(sBody);
+    bodies_[sBody.masterID()].merge(sBody);
 
     const label sBodyID = mergedBodyID(mergedBodies_.size() - 1);
     bodyIDs_.insert(sBody.name(), sBodyID);
@@ -329,7 +367,7 @@ Foam::spatialTransform Foam::RBD::rigidBodyModel::X0
     if (merged(bodyId))
     {
         const subBody& mBody = mergedBody(bodyId);
-        return mBody.parentXT() & X0_[mBody.parentID()];
+        return mBody.masterXT() & X0_[mBody.masterID()];
     }
     else
     {
@@ -376,15 +414,45 @@ void Foam::RBD::rigidBodyModel::write(Ostream& os) const
         mergedBodies_[i].body().write(os);
 
         os.writeKeyword("transform")
-            << mergedBodies_[i].parentXT() << token::END_STATEMENT << nl;
+            << mergedBodies_[i].masterXT() << token::END_STATEMENT << nl;
 
         os.writeKeyword("mergeWith")
-            << mergedBodies_[i].parentName() << token::END_STATEMENT << nl;
+            << mergedBodies_[i].masterName() << token::END_STATEMENT << nl;
 
         os  << decrIndent << indent << token::END_BLOCK << endl;
     }
 
     os  << decrIndent << indent << token::END_BLOCK << nl;
+
+
+    if (!restraints_.empty())
+    {
+        os  << indent << "restraints" << nl
+            << indent << token::BEGIN_BLOCK << incrIndent << nl;
+
+        forAll(restraints_, ri)
+        {
+            word restraintType = restraints_[ri].type();
+
+            os  << indent << restraints_[ri].name() << nl
+                << indent << token::BEGIN_BLOCK << incrIndent << endl;
+
+            restraints_[ri].write(os);
+
+            os  << decrIndent << indent << token::END_BLOCK << endl;
+        }
+
+        os  << decrIndent << indent << token::END_BLOCK << nl;
+    }
+}
+
+
+bool Foam::RBD::rigidBodyModel::read(const dictionary& dict)
+{
+    restraints_.clear();
+    addRestraints(dict);
+
+    return true;
 }
 
 

src/rigidBodyDynamics/rigidBodyModel/rigidBodyModel.H

@@ -53,6 +53,7 @@ SourceFiles
 #include "subBody.H"
 #include "joint.H"
 #include "compositeJoint.H"
+#include "rigidBodyRestraint.H"
 #include "PtrList.H"
 #include "HashTable.H"
 
@@ -85,6 +86,9 @@ class rigidBodyModel
         //- Convert the body with given ID into a composite-body
         void makeComposite(const label bodyID);
 
+        //- Add restraints to the motion
+        void addRestraints(const dictionary& dict);
+
 
 protected:
 
@@ -124,6 +128,9 @@ protected:
         //  joint.
         label nw_;
 
+        //- Motion restraints
+        PtrList<restraint> restraints_;
+
 
     // Other protected member data
 
@@ -288,6 +295,10 @@ public:
         //- Return true if the body with given ID has been merged with a parent
         inline bool merged(label bodyID) const;
 
+        //- Return the ID of the master body for a sub-body otherwise
+        //  return the given body ID
+        inline label master(label bodyID) const;
+
         //- Return the index of the merged body in the mergedBody list
         //  from the given body ID
         inline label mergedBodyIndex(const label mergedBodyID) const;
@@ -305,6 +316,16 @@ public:
         //- Return the current transform to the global frame for the given body
         spatialTransform X0(const label bodyId) const;
 
+        // Find the corresponding point in the master body frame
+        vector masterPoint(const label bodyID, const vector& p) const;
+
+        //- Return the velocity of the given point on the given body
+        spatialVector v(const label bodyID, const vector& p) const;
+
+        //- Apply the restraints and accumulate the external forces
+        //  into the fx field
+        void applyRestraints(Field<spatialVector>& fx) const;
+
         //- Calculate the joint acceleration qDdot from the joint state q,
         //  velocity qDot, internal force tau (in the joint frame) and
         //  external force fx (in the global frame) using the articulated body
@@ -333,6 +354,10 @@ public:
         //- Write
         virtual void write(Ostream&) const;
 
+        //- Read coefficients dictionary and update system parameters,
+        //  restraints but not the current state
+        bool read(const dictionary& dict);
+
 
     // Ostream Operator
 

src/rigidBodyDynamics/rigidBodyModel/rigidBodyModelI.H

@@ -89,6 +89,19 @@ inline bool Foam::RBD::rigidBodyModel::merged(label bodyID) const
 }
 
 
+inline Foam::label Foam::RBD::rigidBodyModel::master(label bodyID) const
+{
+    if (bodyID < 0)
+    {
+        return mergedBody(bodyID).masterID();
+    }
+    else
+    {
+        return bodyID;
+    }
+}
+
+
 inline Foam::label
 Foam::RBD::rigidBodyModel::mergedBodyID(const label mergedBodyIndex) const
 {
@@ -123,4 +136,43 @@ inline Foam::label Foam::RBD::rigidBodyModel::bodyID(const word& name) const
 }
 
 
+inline Foam::vector Foam::RBD::rigidBodyModel::masterPoint
+(
+    const label bodyID,
+    const vector& p
+) const
+{
+    if (merged(bodyID))
+    {
+        return
+        (
+            mergedBody(bodyID).masterXT().inv()
+         && spatialVector(Zero, p)
+        ).l();
+    }
+    else
+    {
+        return p;
+    }
+}
+
+
+inline Foam::spatialVector Foam::RBD::rigidBodyModel::v
+(
+    const label bodyID,
+    const vector& p
+) const
+{
+    return
+    (
+        spatialTransform
+        (
+            X0_[master(bodyID)].E().T(),
+            masterPoint(bodyID, p)
+        )
+      & v_[master(bodyID)]
+    );
+}
+
+
 // ************************************************************************* //