rigidBodyModel.H
Go to the documentation of this file.
1 /*---------------------------------------------------------------------------*\
2  ========= |
3  \\ / F ield | OpenFOAM: The Open Source CFD Toolbox
4  \\ / O peration |
5  \\ / A nd | www.openfoam.com
6  \\/ M anipulation |
7 -------------------------------------------------------------------------------
8  Copyright (C) 2016 OpenFOAM Foundation
9  Copyright (C) 2019 OpenCFD Ltd.
10 -------------------------------------------------------------------------------
11 License
12  This file is part of OpenFOAM.
13 
14  OpenFOAM is free software: you can redistribute it and/or modify it
15  under the terms of the GNU General Public License as published by
16  the Free Software Foundation, either version 3 of the License, or
17  (at your option) any later version.
18 
19  OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
20  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
21  FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
22  for more details.
23 
24  You should have received a copy of the GNU General Public License
25  along with OpenFOAM. If not, see <http://www.gnu.org/licenses/>.
26 
27 Class
28  Foam::RBD::rigidBodyModel
29 
30 Description
31  Basic rigid-body model representing a system of rigid-bodies connected by
32  1-6 DoF joints.
33 
34  This class holds various body and joint state fields needed by the
35  kinematics and forward-dynamics algorithms presented in
36 
37  reference:
38  \verbatim
39  Featherstone, R. (2008).
40  Rigid body dynamics algorithms.
41  Springer.
42  Chapter 4.
43  \endverbatim
44 
45 SourceFiles
46  rigidBodyModel.C
47  kinematics.C
48  forwardDynamics.C
49 
50 \*---------------------------------------------------------------------------*/
51 
52 #ifndef RBD_rigidBodyModel_H
53 #define RBD_rigidBodyModel_H
54 
55 #include "rigidBody.H"
56 #include "subBody.H"
57 #include "joint.H"
58 #include "compositeJoint.H"
59 #include "PtrList.H"
60 #include "HashTable.H"
61 
62 namespace Foam
63 {
64 
65 // Forward declaration of friend functions and operators
66 class Time;
67 
68 namespace RBD
69 {
70 
71 // Forward declaration of friend functions and operators
72 class rigidBodyModel;
73 
74 Ostream& operator<<(Ostream&, const rigidBodyModel&);
75 
76 class rigidBodyModelState;
77 class restraint;
78 
79 
80 /*---------------------------------------------------------------------------*\
81  Class rigidBodyModel Declaration
82 \*---------------------------------------------------------------------------*/
83 
84 class rigidBodyModel
85 {
86  // Private member functions
87 
88  //- Initialize the model with the root-body
89  // which is a fixed massless bodyat the origin.
90  void initializeRootBody();
91 
92  //- Resize the state fields following the joining of a body
93  void resizeState();
94 
95  //- Convert the body with given ID into a composite-body
96  void makeComposite(const label bodyID);
97 
98  //- Add restraints to the motion
99  void addRestraints(const dictionary& dict);
100 
101 protected:
102 
103  //- Reference to time database
104  const Time& time_;
105 
106  // Protected data representing the model structure
107 
108  //- List of the bodies.
109  // The 0'th body represents the fixed origin and is constructed
110  // automatically. The subsequent (moving) bodies are appended by the
111  // join member function.
113 
114  //- Bodies may be merged into existing bodies, the inertia of which is
115  // updated to represent the combined body which is more efficient than
116  // attaching them with fixed joints. These 'merged' bodies are held on
117  // this list.
119 
120  //- Lookup-table of the IDs of the bodies
122 
123  //- List of indices of the parent of each body
125 
126  //- Each body it attached with a joint which are held on this list.
128 
129  //- Transform from the parent body frame to the joint frame.
131 
132  //- The number of degrees of freedom of the model
133  // used to set the size of the of joint state fields q, qDot and qDdot.
134  label nDoF_;
135 
136  //- True if any of the joints using quaternions
137  bool unitQuaternions_;
138 
139  //- Motion restraints
141 
142 
143  // Other protected member data
144 
145  //- Acceleration due to gravity
146  vector g_;
147 
148 
149  // Mutable transforms maintained by kinematics and forward-dynamics
150 
151  //- Transform from the parent body to the current body
153 
154  //- Transform for external forces to the bodies reference frame
156 
157 
158  // Mutable kinematic body state fields
159 
160  //- The spatial velocity of the bodies
162 
163  //- The spatial acceleration of the bodies
165 
166  //- The velocity dependent spatial acceleration of the joints
168 
169 
170  // Mutable state fields needed by the forward-dynamics algorithm
171 
172  //- Velocity-product acceleration
173 
174  //- Articulated body inertia
176 
177  //- Articulated body bias force
179 
180 
181  // Mutable joint state fields
182 
183  //- Motion subspace for joints with 3 degrees of freedom
185 
186  //- Motion subspace for joints with 1 degrees of freedom
188 
189  //- Sub-expression IA.S in the forward-dynamics algorithm
191 
192  //- Sub-expression IA.S1 in the forward-dynamics algorithm
194 
195  //- Sub-expression (S^T.U)^-1 in the forward-dynamics algorithm
196  mutable DynamicList<tensor> Dinv_;
197 
198  //- Sub-expression tau - S^T.pA in the forward-dynamics algorithm
199  mutable DynamicList<vector> u_;
200 
201 
202  // Protected member functions
203 
204  //- Join the given body to the parent with ID parentID via the given
205  // joint with transform from the parent frame to the joint frame XT.
206  virtual label join_
207  (
208  const label parentID,
209  const spatialTransform& XT,
210  autoPtr<joint> jointPtr,
211  autoPtr<rigidBody> bodyPtr
212  );
213 
214 
215 public:
216 
217  //- Runtime type information
218  TypeName("rigidBodyModel");
219 
220 
221  // Constructors
222 
223  //- Null-constructor which adds the single root-body at the origin
224  rigidBodyModel(const Time& time);
225 
226  //- Construct from dictionary
227  rigidBodyModel(const Time& time, const dictionary& dict);
228 
229 
230  //- Destructor
231  virtual ~rigidBodyModel();
232 
233 
234  // Member Functions
235 
236  //- Return the time
237  inline const Time& time() const;
238 
239  //- Return the number of bodies in the model (bodies().size())
240  inline label nBodies() const;
241 
242  //- Return the list of the bodies in the model
243  inline PtrList<rigidBody> bodies() const;
244 
245  //- List of indices of the parent of each body
246  inline const DynamicList<label>& lambda() const;
247 
248  //- Return the list of joints in the model
249  inline const PtrList<joint>& joints() const;
250 
251  //- Return the number of degrees of freedom of the model
252  // used to set the size of the of joint state fields q, qDot and qDdot.
253  inline label nDoF() const;
254 
255  //- Return true if any of the joints using quaternions
256  inline bool unitQuaternions() const;
257 
258  //- Return the acceleration due to gravity
259  inline const vector& g() const;
260 
261  //- Allow the acceleration due to gravity to be set
262  // after model construction
263  inline vector& g();
264 
265  //- Return the name of body with the given ID
266  inline const word& name(const label bodyID) const;
267 
268  //- Return the inertia of body i
269  inline const rigidBodyInertia& I(const label i) const;
270 
271  //- Return the spatial velocity of the bodies
272  inline const spatialVector& v(const label i) const;
273 
274  //- Return the spatial acceleration of the bodies
275  inline const spatialVector& a(const label i) const;
276 
277  //- Join the given body to the parent with ID parentID via the given
278  // joint with transform from the parent frame to the joint frame XT.
279  virtual label join
280  (
281  const label parentID,
282  const spatialTransform& XT,
283  autoPtr<joint> jointPtr,
284  autoPtr<rigidBody> bodyPtr
285  );
286 
287  //- Join the given body to the parent with ID parentID via the given
288  // composite joint (specified as a list of co-located joints) with
289  // transform from the parent frame to the joint frame XT.
290  // Composite joins are useful to represent complex joints with degrees
291  // of freedom other than 1 or 3 which are directly supported.
292  label join
293  (
294  const label parentID,
295  const spatialTransform& XT,
297  autoPtr<rigidBody> bodyPtr
298  );
299 
300  //- Merge the given body with transform X into the parent with ID
301  // parentID. The parent body assumes the properties of the combined
302  // body (inertia etc.) and the merged body is held on a
303  // separate list for reference.
304  label merge
305  (
306  const label parentID,
307  const spatialTransform& X,
308  autoPtr<rigidBody> bodyPtr
309  );
310 
311  //- Return true if the body with given ID has been merged with a parent
312  inline bool merged(label bodyID) const;
313 
314  //- Return the ID of the master body for a sub-body otherwise
315  // return the given body ID
316  inline label master(label bodyID) const;
317 
318  //- Return the index of the merged body in the mergedBody list
319  // from the given body ID
320  inline label mergedBodyIndex(const label mergedBodyID) const;
321 
322  //- Return the merged body ID for the given merged body index
323  // in the mergedBody list
324  inline label mergedBodyID(const label mergedBodyIndex) const;
325 
326  //- Return the merged body for the given body ID
327  inline const subBody& mergedBody(label mergedBodyID) const;
328 
329  //- Return the ID of the body with the given name
330  inline label bodyID(const word& name) const;
331 
332  //- Return the current transform to the global frame for the given body
333  spatialTransform X0(const label bodyId) const;
334 
335  // Find the corresponding point in the master body frame
336  vector masterPoint(const label bodyID, const vector& p) const;
337 
338  //- Return the velocity of the given point on the given body
339  spatialVector v(const label bodyID, const vector& p) const;
340 
341  //- Apply the restraints and accumulate the internal joint forces
342  // into the tau field and external forces into the fx field
343  void applyRestraints
344  (
345  scalarField& tau,
347  const rigidBodyModelState& state
348  ) const;
349 
350  //- Calculate the joint acceleration qDdot from the joint state q,
351  // velocity qDot, internal force tau (in the joint frame) and
352  // external force fx (in the global frame) using the articulated body
353  // algorithm (Section 7.3 and Table 7.1)
354  void forwardDynamics
355  (
356  rigidBodyModelState& state,
357  const scalarField& tau,
358  const Field<spatialVector>& fx
359  ) const;
360 
361  //- Correct the velocity and acceleration of the bodies in the model
362  // from the given joint state fields following an integration step
363  // of the forwardDynamics
364  void forwardDynamicsCorrection(const rigidBodyModelState& state) const;
365 
366  //- Write
367  virtual void write(Ostream&) const;
368 
369  //- Read coefficients dictionary and update system parameters,
370  // restraints but not the current state
371  bool read(const dictionary& dict);
372 
373 
374  // Ostream Operator
375 
376  friend Ostream& operator<<(Ostream&, const rigidBodyModel&);
377 };
378 
379 
380 // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
381 
382 } // End namespace RBD
383 } // End namespace Foam
384 
385 // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
386 
387 #include "rigidBodyModelI.H"
388 
389 // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
390 
391 #endif
392 
393 // ************************************************************************* //
Foam::RBD::rigidBodyModel::~rigidBodyModel
virtual ~rigidBodyModel()
Destructor.
Definition: rigidBodyModel.C:224
subBody.H
p
volScalarField & p
Definition: createFieldRefs.H:8
Foam::Time
Class to control time during OpenFOAM simulations that is also the top-level objectRegistry.
Definition: Time.H:73
Foam::RBD::rigidBodyModelState
Holds the motion state of rigid-body model.
Definition: rigidBodyModelState.H:67
Foam::word
A class for handling words, derived from Foam::string.
Definition: word.H:65
Foam::RBD::rigidBodyModel::X0
spatialTransform X0(const label bodyId) const
Return the current transform to the global frame for the given body.
Definition: rigidBodyModel.C:381
HashTable.H
Foam::RBD::rigidBodyModel::nBodies
label nBodies() const
Return the number of bodies in the model (bodies().size())
Definition: rigidBodyModelI.H:37
Foam::RBD::rigidBodyModel::merged
bool merged(label bodyID) const
Return true if the body with given ID has been merged with a parent.
Definition: rigidBodyModelI.H:123
Foam::spatialTransform
Compact representation of the Pl├╝cker spatial transformation tensor in terms of the rotation tensor E...
Definition: spatialTransform.H:70
Foam::DynamicList< label >
Foam::RBD::rigidBodyModel::v_
DynamicList< spatialVector > v_
The spatial velocity of the bodies.
Definition: rigidBodyModel.H:160
Foam::RBD::rigidBodyModel::Dinv_
DynamicList< tensor > Dinv_
Sub-expression (S^T.U)^-1 in the forward-dynamics algorithm.
Definition: rigidBodyModel.H:195
Foam::RBD::rigidBodyModel::bodies_
PtrList< rigidBody > bodies_
List of the bodies.
Definition: rigidBodyModel.H:111
joint.H
Foam::RBD::rigidBodyModel::I
const rigidBodyInertia & I(const label i) const
Return the inertia of body i.
Definition: rigidBodyModelI.H:105
Foam::RBD::rigidBodyModel::operator<<
friend Ostream & operator<<(Ostream &, const rigidBodyModel &)
Foam::RBD::rigidBodyModel::rigidBodyModel
rigidBodyModel(const Time &time)
Null-constructor which adds the single root-body at the origin.
Definition: rigidBodyModel.C:169
Foam::RBD::rigidBodyModel::mergedBodyIndex
label mergedBodyIndex(const label mergedBodyID) const
Return the index of the merged body in the mergedBody list.
Definition: rigidBodyModelI.H:150
Foam::RBD::operator<<
Ostream & operator<<(Ostream &, const rigidBody &)
Definition: rigidBodyI.H:75
Foam::RBD::rigidBodyModel::pA_
DynamicList< spatialVector > pA_
Articulated body bias force.
Definition: rigidBodyModel.H:177
Foam::RBD::rigidBodyModel::v
const spatialVector & v(const label i) const
Return the spatial velocity of the bodies.
Definition: rigidBodyModelI.H:112
Foam::RBD::rigidBodyModel::joints
const PtrList< joint > & joints() const
Return the list of joints in the model.
Definition: rigidBodyModelI.H:58
Foam::RBD::rigidBodyModel::X0_
DynamicList< spatialTransform > X0_
Transform for external forces to the bodies reference frame.
Definition: rigidBodyModel.H:154
Foam::RBD::rigidBodyModel::S_
DynamicList< compactSpatialTensor > S_
Motion subspace for joints with 3 degrees of freedom.
Definition: rigidBodyModel.H:183
Foam::RBD::rigidBodyModel::a
const spatialVector & a(const label i) const
Return the spatial acceleration of the bodies.
Definition: rigidBodyModelI.H:118
Foam::RBD::rigidBodyModel::forwardDynamicsCorrection
void forwardDynamicsCorrection(const rigidBodyModelState &state) const
Correct the velocity and acceleration of the bodies in the model.
Definition: forwardDynamics.C:210
Foam::RBD::rigidBodyModel::lambda_
DynamicList< label > lambda_
List of indices of the parent of each body.
Definition: rigidBodyModel.H:123
Foam::Field< scalar >
Foam::RBD::rigidBodyModel::time
const Time & time() const
Return the time.
Definition: rigidBodyModelI.H:31
Foam::RBD::rigidBodyModel::mergedBodyID
label mergedBodyID(const label mergedBodyIndex) const
Return the merged body ID for the given merged body index.
Definition: rigidBodyModelI.H:143
Foam::RBD::rigidBodyModel::IA_
DynamicList< spatialTensor > IA_
Velocity-product acceleration.
Definition: rigidBodyModel.H:174
Foam::SpatialVector< scalar >
Foam::RBD::rigidBodyModel::unitQuaternions_
bool unitQuaternions_
True if any of the joints using quaternions.
Definition: rigidBodyModel.H:136
Foam::PtrList
A list of pointers to objects of type <T>, with allocation/deallocation management of the pointers....
Definition: List.H:59
Foam::RBD::subBody
Definition: subBody.H:57
Foam::RBD::rigidBodyModel::master
label master(label bodyID) const
Return the ID of the master body for a sub-body otherwise.
Definition: rigidBodyModelI.H:129
dict
dictionary dict
Definition: searchingEngine.H:14
Foam::RBD::rigidBodyModel
Basic rigid-body model representing a system of rigid-bodies connected by 1-6 DoF joints.
Definition: rigidBodyModel.H:83
Foam::dictionary
A list of keyword definitions, which are a keyword followed by a number of values (eg,...
Definition: dictionary.H:123
Foam::RBD::rigidBodyModel::S1_
DynamicList< spatialVector > S1_
Motion subspace for joints with 1 degrees of freedom.
Definition: rigidBodyModel.H:186
rigidBody.H
Foam::RBD::rigidBodyModel::restraints_
PtrList< restraint > restraints_
Motion restraints.
Definition: rigidBodyModel.H:139
Foam
Namespace for OpenFOAM.
Definition: atmBoundaryLayer.C:33
Foam::RBD::rigidBodyModel::bodyID
label bodyID(const word &name) const
Return the ID of the body with the given name.
Definition: rigidBodyModelI.H:170
Foam::RBD::rigidBodyModel::joints_
PtrList< joint > joints_
Each body it attached with a joint which are held on this list.
Definition: rigidBodyModel.H:126
Foam::RBD::rigidBodyModel::lambda
const DynamicList< label > & lambda() const
List of indices of the parent of each body.
Definition: rigidBodyModelI.H:51
Foam::HashTable< label >
Foam::RBD::rigidBodyModel::time_
const Time & time_
Reference to time database.
Definition: rigidBodyModel.H:103
Foam::RBD::rigidBodyModel::nDoF_
label nDoF_
The number of degrees of freedom of the model.
Definition: rigidBodyModel.H:133
Foam::RBD::rigidBodyModel::name
const word & name(const label bodyID) const
Return the name of body with the given ID.
Definition: rigidBodyModelI.H:89
Foam::RBD::rigidBodyModel::forwardDynamics
void forwardDynamics(rigidBodyModelState &state, const scalarField &tau, const Field< spatialVector > &fx) const
Calculate the joint acceleration qDdot from the joint state q,.
Definition: forwardDynamics.C:57
Foam::autoPtr
Pointer management similar to std::unique_ptr, with some additional methods and type checking.
Definition: HashPtrTable.H:53
Foam::RBD::rigidBodyModel::masterPoint
vector masterPoint(const label bodyID, const vector &p) const
Definition: rigidBodyModelI.H:177
Foam::RBD::rigidBodyModel::g_
vector g_
Acceleration due to gravity.
Definition: rigidBodyModel.H:145
Foam::RBD::rigidBodyModel::join_
virtual label join_(const label parentID, const spatialTransform &XT, autoPtr< joint > jointPtr, autoPtr< rigidBody > bodyPtr)
Join the given body to the parent with ID parentID via the given.
Definition: rigidBodyModel.C:123
Foam::RBD::rigidBodyModel::join
virtual label join(const label parentID, const spatialTransform &XT, autoPtr< joint > jointPtr, autoPtr< rigidBody > bodyPtr)
Join the given body to the parent with ID parentID via the given.
Definition: rigidBodyModel.C:231
Foam::RBD::rigidBodyModel::read
bool read(const dictionary &dict)
Read coefficients dictionary and update system parameters,.
Definition: rigidBodyModel.C:456
Foam::RBD::rigidBodyModel::a_
DynamicList< spatialVector > a_
The spatial acceleration of the bodies.
Definition: rigidBodyModel.H:163
Foam::Vector< scalar >
Foam::RBD::rigidBodyModel::unitQuaternions
bool unitQuaternions() const
Return true if any of the joints using quaternions.
Definition: rigidBodyModelI.H:70
Foam::RBD::rigidBodyModel::c_
DynamicList< spatialVector > c_
The velocity dependent spatial acceleration of the joints.
Definition: rigidBodyModel.H:166
Foam::RBD::rigidBodyModel::applyRestraints
void applyRestraints(scalarField &tau, Field< spatialVector > &fx, const rigidBodyModelState &state) const
Apply the restraints and accumulate the internal joint forces.
Definition: forwardDynamics.C:35
Foam::RBD::rigidBodyModel::TypeName
TypeName("rigidBodyModel")
Runtime type information.
Foam::RBD::rigidBodyModel::write
virtual void write(Ostream &) const
Write.
Definition: rigidBodyModel.C:395
Foam::RBD::rigidBodyModel::XT_
DynamicList< spatialTransform > XT_
Transform from the parent body frame to the joint frame.
Definition: rigidBodyModel.H:129
Foam::RBD::rigidBodyModel::mergedBody
const subBody & mergedBody(label mergedBodyID) const
Return the merged body for the given body ID.
Definition: rigidBodyModelI.H:157
compositeJoint.H
rigidBodyModelI.H
PtrList.H
Foam::RBD::rigidBodyModel::g
const vector & g() const
Return the acceleration due to gravity.
Definition: rigidBodyModelI.H:76
Foam::RBD::rigidBodyModel::Xlambda_
DynamicList< spatialTransform > Xlambda_
Transform from the parent body to the current body.
Definition: rigidBodyModel.H:151
Foam::RBD::rigidBodyModel::u_
DynamicList< vector > u_
Sub-expression tau - S^T.pA in the forward-dynamics algorithm.
Definition: rigidBodyModel.H:198
Foam::Ostream
An Ostream is an abstract base class for all output systems (streams, files, token lists,...
Definition: Ostream.H:56
Foam::RBD::rigidBodyModel::U_
DynamicList< compactSpatialTensor > U_
Sub-expression IA.S in the forward-dynamics algorithm.
Definition: rigidBodyModel.H:189
Foam::RBD::rigidBodyModel::mergedBodies_
PtrList< subBody > mergedBodies_
Bodies may be merged into existing bodies, the inertia of which is.
Definition: rigidBodyModel.H:117
Foam::RBD::rigidBodyModel::nDoF
label nDoF() const
Return the number of degrees of freedom of the model.
Definition: rigidBodyModelI.H:64
Foam::RBD::rigidBodyModel::bodyIDs_
HashTable< label > bodyIDs_
Lookup-table of the IDs of the bodies.
Definition: rigidBodyModel.H:120
Foam::RBD::rigidBodyModel::bodies
PtrList< rigidBody > bodies() const
Return the list of the bodies in the model.
Definition: rigidBodyModelI.H:44
Foam::RBD::rigidBodyModel::U1_
DynamicList< spatialVector > U1_
Sub-expression IA.S1 in the forward-dynamics algorithm.
Definition: rigidBodyModel.H:192
Foam::RBD::rigidBodyInertia
Definition: rigidBodyInertia.H:77
Foam::RBD::rigidBodyModel::merge
label merge(const label parentID, const spatialTransform &X, autoPtr< rigidBody > bodyPtr)
Merge the given body with transform X into the parent with ID.
Definition: rigidBodyModel.C:323