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-2019 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  //- Initialize the model with the root-body
82  // which is a fixed massless bodyat the origin.
83  void initializeRootBody();
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
217  rigidBodyModel(const dictionary& dict);
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 bodyID(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  vector masterPoint(const label bodyID, const vector& p) const;
324 
325  //- Return the velocity of the given point on the given body
326  spatialVector v(const label bodyID, const vector& p) const;
327 
328  //- Apply the restraints and accumulate the internal joint forces
329  // into the tau field and external forces into the fx field
330  void applyRestraints(scalarField& tau, Field<spatialVector>& fx) const;
331 
332  //- Calculate the joint acceleration qDdot from the joint state q,
333  // velocity qDot, internal force tau (in the joint frame) and
334  // external force fx (in the global frame) using the articulated body
335  // algorithm (Section 7.3 and Table 7.1)
336  void forwardDynamics
337  (
338  rigidBodyModelState& state,
339  const scalarField& tau,
340  const Field<spatialVector>& fx
341  ) const;
342 
343  //- Correct the velocity and acceleration of the bodies in the model
344  // from the given joint state fields following an integration step
345  // of the forwardDynamics
346  void forwardDynamicsCorrection(const rigidBodyModelState& state) const;
347 
348  //- Write
349  virtual void write(Ostream&) const;
350 
351  //- Read coefficients dictionary and update system parameters,
352  // restraints but not the current state
353  bool read(const dictionary& dict);
354 
355 
356  // Ostream Operator
357 
358  friend Ostream& operator<<(Ostream&, const rigidBodyModel&);
359 };
360 
361 
362 // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
363 
364 } // End namespace RBD
365 } // End namespace Foam
366 
367 // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
368 
369 #include "rigidBodyModelI.H"
370 
371 // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
372 
373 #endif
374 
375 // ************************************************************************* //
const subBody & mergedBody(label mergedBodyID) const
Return the merged body for the given body ID.
Ostream & operator<<(Ostream &, const rigidBody &)
Definition: rigidBodyI.H:73
dictionary dict
const rigidBodyInertia & I(const label i) const
Return the inertia of body i.
DynamicList< vector > u_
Sub-expression tau - S^T.pA in the forward-dynamics algorithm.
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
DynamicList< spatialTransform > X0_
Transform for external forces to the bodies reference frame.
friend Ostream & operator<<(Ostream &, const rigidBodyModel &)
A list of keyword definitions, which are a keyword followed by any number of values (e...
Definition: dictionary.H:158
DynamicList< spatialVector > pA_
Articulated body bias force.
void forwardDynamicsCorrection(const rigidBodyModelState &state) const
Correct the velocity and acceleration of the bodies in the model.
const spatialVector & v(const label i) const
Return the spatial velocity of the bodies.
label merge(const label parentID, const spatialTransform &X, autoPtr< rigidBody > bodyPtr)
Merge the given body with transform X into the parent with ID.
void applyRestraints(scalarField &tau, Field< spatialVector > &fx) const
Apply the restraints and accumulate the internal joint forces.
DynamicList< spatialTensor > IA_
Velocity-product acceleration.
label nBodies() const
Return the number of bodies in the model (bodies().size())
label mergedBodyID(const label mergedBodyIndex) const
Return the merged body ID for the given merged body index.
label master(label bodyID) const
Return the ID of the master body for a sub-body otherwise.
DynamicList< spatialVector > S1_
Motion subspace for joints with 1 degrees of freedom.
Holds the motion state of rigid-body model.
HashTable< label, word > bodyIDs_
Lookup-table of the IDs of the bodies.
PtrList< restraint > restraints_
Motion restraints.
DynamicList< spatialVector > v_
The spatial velocity of the bodies.
DynamicList< tensor > Dinv_
Sub-expression (S^T.U)^-1 in the forward-dynamics algorithm.
PtrList< joint > joints_
Each body it attached with a joint which are held on this list.
PtrList< rigidBody > bodies_
List of the bodies.
This specialized rigidBody holds the original body after it has been merged into a master...
Definition: subBody.H:52
A class for handling words, derived from string.
Definition: word.H:59
label mergedBodyIndex(const label mergedBodyID) const
Return the index of the merged body in the mergedBody list.
label nDoF_
The number of degrees of freedom of the model.
DynamicList< compactSpatialTensor > S_
Motion subspace for joints with 3 degrees of freedom.
const word & name(const label bodyID) const
Return the name of body with the given ID.
bool read(const dictionary &dict)
Read coefficients dictionary and update system parameters,.
const PtrList< joint > & joints() const
Return the list of joints in the model.
DynamicList< spatialTransform > XT_
Transform from the parent body frame to the joint frame.
TypeName("rigidBodyModel")
Runtime type information.
bool unitQuaternions() const
Return true if any of the joints using quaternions.
DynamicList< spatialVector > c_
The velocity dependent spatial acceleration of the joints.
An STL-conforming hash table.
Definition: HashTable.H:61
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.
virtual void write(Ostream &) const
Write.
An Ostream is an abstract base class for all output systems (streams, files, token lists...
Definition: Ostream.H:53
DynamicList< spatialTransform > Xlambda_
Transform from the parent body to the current body.
DynamicList< label > lambda_
List of indices of the parent of each body.
DynamicList< compactSpatialTensor > U_
Sub-expression IA.S in the forward-dynamics algorithm.
label nDoF() const
Return the number of degrees of freedom of the model.
const vector & g() const
Return the acceleration due to gravity.
PtrList< rigidBody > bodies() const
Return the list of the bodies in the model.
bool unitQuaternions_
True if any of the joints using quaternions.
PtrList< subBody > mergedBodies_
Bodies may be merged into existing bodies, the inertia of which is.
DynamicList< spatialVector > U1_
Sub-expression IA.S1 in the forward-dynamics algorithm.
wordList movingBodyNames() const
Return the names of the moving bodies.
A templated 1D list of pointers to objects of type <T>, where the size of the array is known and used...
Definition: List.H:70
virtual ~rigidBodyModel()
Destructor.
const DynamicList< label > & lambda() const
List of indices of the parent of each body.
Compact representation of the Plücker spatial transformation tensor in terms of the rotation tensor E...
An auto-pointer similar to the STL auto_ptr but with automatic casting to a reference to the type and...
Definition: PtrList.H:52
label bodyID(const word &name) const
Return the ID of the body with the given name.
Basic rigid-body model representing a system of rigid-bodies connected by 1-6 DoF joints...
volScalarField & p
spatialTransform X0(const label bodyId) const
Return the current transform to the global frame for the given body.
rigidBodyModel()
Null-constructor which adds the single root-body at the origin.
vector masterPoint(const label bodyID, const vector &p) const
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.
void forwardDynamics(rigidBodyModelState &state, const scalarField &tau, const Field< spatialVector > &fx) const
Calculate the joint acceleration qDdot from the joint state q,.
bool merged(label bodyID) const
Return true if the body with given ID has been merged with a parent.
vector g_
Acceleration due to gravity.
DynamicList< spatialVector > a_
The spatial acceleration of the bodies.
Namespace for OpenFOAM.
This class represents the linear and angular inertia of a rigid body by the mass, centre of mass and ...