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