diff --git a/applications/test/rigidBodyDynamics/spring/Make/files b/applications/test/rigidBodyDynamics/spring/Make/files
new file mode 100644
index 0000000000000000000000000000000000000000..397706b3ac9f99d312b44de7802400c655226388
--- /dev/null
+++ b/applications/test/rigidBodyDynamics/spring/Make/files
@@ -0,0 +1,3 @@
+spring.C
+
+EXE = $(FOAM_USER_APPBIN)/Test-spring
diff --git a/applications/test/rigidBodyDynamics/spring/Make/options b/applications/test/rigidBodyDynamics/spring/Make/options
new file mode 100644
index 0000000000000000000000000000000000000000..4a9d828b67c8d2e6090ad4601496f7cd38dbe731
--- /dev/null
+++ b/applications/test/rigidBodyDynamics/spring/Make/options
@@ -0,0 +1,5 @@
+EXE_INC = \
+ -I$(LIB_SRC)/rigidBodyDynamics/lnInclude
+
+EXE_LIBS = \
+ -lrigidBodyDynamics
diff --git a/applications/test/rigidBodyDynamics/spring/spring b/applications/test/rigidBodyDynamics/spring/spring
new file mode 100644
index 0000000000000000000000000000000000000000..3024b0622b795eeeecca0f66da91cf3b5dd6a393
--- /dev/null
+++ b/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;
+ }
+}
diff --git a/applications/test/rigidBodyDynamics/spring/spring.C b/applications/test/rigidBodyDynamics/spring/spring.C
new file mode 100644
index 0000000000000000000000000000000000000000..65c6e88a5a74297240643620a7d672cbfca29b25
--- /dev/null
+++ b/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;
+}
+
+
+// ************************************************************************* //
diff --git a/applications/test/rigidBodyDynamics/spring/spring.gnuplot b/applications/test/rigidBodyDynamics/spring/spring.gnuplot
new file mode 100644
index 0000000000000000000000000000000000000000..28813508f43c6d8ac405e9a7c3007f7468226546
--- /dev/null
+++ b/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"
+
+#------------------------------------------------------------------------------
diff --git a/src/rigidBodyDynamics/Make/files b/src/rigidBodyDynamics/Make/files
index 5849d63b1ea1872d008c983599b990fd11a19877..ad592dc7ef55ea41c5c897759e87d8834fd721a0 100644
--- a/src/rigidBodyDynamics/Make/files
+++ b/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
diff --git a/src/rigidBodyDynamics/restraints/linearSpring/linearSpring.C b/src/rigidBodyDynamics/restraints/linearSpring/linearSpring.C
new file mode 100644
index 0000000000000000000000000000000000000000..ed1efb7f4244c410869f8d555ea80de379814e0c
--- /dev/null
+++ b/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;
+}
+
+
+// ************************************************************************* //
diff --git a/src/rigidBodyDynamics/restraints/linearSpring/linearSpring.H b/src/rigidBodyDynamics/restraints/linearSpring/linearSpring.H
new file mode 100644
index 0000000000000000000000000000000000000000..83d26b512eba232a78b08b394b8e2567bf54dc6d
--- /dev/null
+++ b/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
+
+// ************************************************************************* //
diff --git a/src/rigidBodyDynamics/restraints/restraint/rigidBodyRestraint.C b/src/rigidBodyDynamics/restraints/restraint/rigidBodyRestraint.C
new file mode 100644
index 0000000000000000000000000000000000000000..3a75c4efbba5aa0015732cbad2f6c6a4d16530d6
--- /dev/null
+++ b/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;
+}
+
+
+// ************************************************************************* //
diff --git a/src/rigidBodyDynamics/restraints/restraint/rigidBodyRestraint.H b/src/rigidBodyDynamics/restraints/restraint/rigidBodyRestraint.H
new file mode 100644
index 0000000000000000000000000000000000000000..917e2e6e1993886f87fe05a898c6bc4ebd2667b9
--- /dev/null
+++ b/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
+
+// ************************************************************************* //
diff --git a/src/rigidBodyDynamics/restraints/restraint/rigidBodyRestraintNew.C b/src/rigidBodyDynamics/restraints/restraint/rigidBodyRestraintNew.C
new file mode 100644
index 0000000000000000000000000000000000000000..ee8e0f2f1c844e8a7bc65e8d018d81cedf37bb05
--- /dev/null
+++ b/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));
+}
+
+
+// ************************************************************************* //
diff --git a/src/rigidBodyDynamics/rigidBodyModel/forwardDynamics.C b/src/rigidBodyDynamics/rigidBodyModel/forwardDynamics.C
index fc7bfd5e0c93e4bdf2688e5fe37a449d2e573f8a..411404dd85532e025d425424b532a267c2e5f971 100644
--- a/src/rigidBodyDynamics/rigidBodyModel/forwardDynamics.C
+++ b/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,
diff --git a/src/rigidBodyDynamics/rigidBodyModel/rigidBodyModel.C b/src/rigidBodyDynamics/rigidBodyModel/rigidBodyModel.C
index e292fcb525d9fa1d596862ee96dcc66c43e5fc9b..fe6471ac30a61b345c88465d30a781f6a9f5fc8e 100644
--- a/src/rigidBodyDynamics/rigidBodyModel/rigidBodyModel.C
+++ b/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;
}
diff --git a/src/rigidBodyDynamics/rigidBodyModel/rigidBodyModel.H b/src/rigidBodyDynamics/rigidBodyModel/rigidBodyModel.H
index ff7a0a5391a47bca971a45546deab113fcfbc0d8..5f4d18fb0dfde690fce43bcc1b8035846c046d24 100644
--- a/src/rigidBodyDynamics/rigidBodyModel/rigidBodyModel.H
+++ b/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
diff --git a/src/rigidBodyDynamics/rigidBodyModel/rigidBodyModelI.H b/src/rigidBodyDynamics/rigidBodyModel/rigidBodyModelI.H
index 34fa0670ef822543cab3a7d4796606ef9cf94b71..478547892e9c96d63f2b4d6e0d52785d782e2ab3 100644
--- a/src/rigidBodyDynamics/rigidBodyModel/rigidBodyModelI.H
+++ b/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)]
+ );
+}
+
+
// ************************************************************************* //