diff --git a/applications/test/rigidBodyDynamics/spring/Make/files b/applications/test/rigidBodyDynamics/spring/Make/files new file mode 100644 index 0000000000..397706b3ac --- /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 0000000000..4a9d828b67 --- /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 0000000000..3024b0622b --- /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 0000000000..65c6e88a5a --- /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 . + +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 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 0000000000..28813508f4 --- /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 . +# +# 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 5849d63b1e..ad592dc7ef 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 0000000000..ed1efb7f42 --- /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 . + +\*---------------------------------------------------------------------------*/ + +#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 0000000000..83d26b512e --- /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 . + +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 clone() const + { + return autoPtr + ( + 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 0000000000..3a75c4efbb --- /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 . + +\*---------------------------------------------------------------------------*/ + +#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 0000000000..917e2e6e19 --- /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 . + +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 clone() const = 0; + + + // Selectors + + //- Select constructed from the dict dictionary and Time + static autoPtr 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 0000000000..ee8e0f2f1c --- /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 . + +\*---------------------------------------------------------------------------*/ + +#include "rigidBodyRestraint.H" + +// * * * * * * * * * * * * * * * * Selectors * * * * * * * * * * * * * * * * // + +Foam::autoPtr +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(cstrIter()(name, dict, model)); +} + + +// ************************************************************************* // diff --git a/src/rigidBodyDynamics/rigidBodyModel/forwardDynamics.C b/src/rigidBodyDynamics/rigidBodyModel/forwardDynamics.C index fc7bfd5e0c..411404dd85 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& 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 e292fcb525..fe6471ac30 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, 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 ff7a0a5391..5f4d18fb0d 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 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& 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 34fa0670ef..478547892e 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)] + ); +} + + // ************************************************************************* //