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-------------------------------------------------------------------------------
11License
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
27Class
28 Foam::RBD::rigidBodyModel
29
30Description
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
45SourceFiles
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
62namespace Foam
63{
64
65// Forward declaration of friend functions and operators
66class Time;
67
68namespace RBD
69{
70
71// Forward declaration of friend functions and operators
72class rigidBodyModel;
73
74Ostream& operator<<(Ostream&, const rigidBodyModel&);
75
76class rigidBodyModelState;
77class restraint;
78
79
80/*---------------------------------------------------------------------------*\
81 Class rigidBodyModel Declaration
82\*---------------------------------------------------------------------------*/
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
101protected:
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
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
215public:
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
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// ************************************************************************* //
A 1D vector of objects of type <T> that resizes itself as necessary to accept the new objects.
Definition: DynamicList.H:72
A HashTable similar to std::unordered_map.
Definition: HashTable.H:123
An Ostream is an abstract base class for all output systems (streams, files, token lists,...
Definition: Ostream.H:62
A list of pointers to objects of type <T>, with allocation/deallocation management of the pointers....
Definition: PtrList.H:73
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.
const Time & time_
Reference to time database.
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.
const Time & time() const
Return the time.
label mergedBodyID(const label mergedBodyIndex) const
Return the merged body ID for the given merged body index.
DynamicList< spatialVector > pA_
Articulated body bias force.
bool unitQuaternions() const
Return true if any of the joints using quaternions.
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.
const spatialVector & a(const label i) const
Return the spatial acceleration of the bodies.
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.
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.
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.
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 bodyID(const word &name) const
Return the ID of the body with the given name.
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.
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.
friend Ostream & operator<<(Ostream &, const rigidBodyModel &)
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.
HashTable< label > bodyIDs_
Lookup-table of the IDs of the bodies.
virtual ~rigidBodyModel()
Destructor.
DynamicList< spatialVector > v_
The spatial velocity of the bodies.
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.
Class to control time during OpenFOAM simulations that is also the top-level objectRegistry.
Definition: Time.H:80
Pointer management similar to std::unique_ptr, with some additional methods and type checking.
Definition: autoPtr.H:66
A list of keyword definitions, which are a keyword followed by a number of values (eg,...
Definition: dictionary.H:126
Compact representation of the Pl├╝cker spatial transformation tensor in terms of the rotation tensor E...
A class for handling words, derived from Foam::string.
Definition: word.H:68
volScalarField & p
Ostream & operator<<(Ostream &, const rigidBody &)
Definition: rigidBodyI.H:75
Namespace for OpenFOAM.
static const Identity< scalar > I
Definition: Identity.H:94
runTime write()
dictionary dict
#define TypeName(TypeNameString)
Declare a ClassName() with extra virtual type info.
Definition: typeInfo.H:73