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-2018 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 inertia of body i
256  inline const rigidBodyInertia& I(const label i) const;
257 
258  //- Return the spatial velocity of the bodies
259  inline const spatialVector& v(const label i) const;
260 
261  //- Join the given body to the parent with ID parentID via the given
262  // joint with transform from the parent frame to the joint frame XT.
263  virtual label join
264  (
265  const label parentID,
266  const spatialTransform& XT,
267  autoPtr<joint> jointPtr,
268  autoPtr<rigidBody> bodyPtr
269  );
270 
271  //- Join the given body to the parent with ID parentID via the given
272  // composite joint (specified as a list of co-located joints) with
273  // transform from the parent frame to the joint frame XT.
274  // Composite joins are useful to represent complex joints with degrees
275  // of freedom other than 1 or 3 which are directly supported.
276  label join
277  (
278  const label parentID,
279  const spatialTransform& XT,
281  autoPtr<rigidBody> bodyPtr
282  );
283 
284  //- Merge the given body with transform X into the parent with ID
285  // parentID. The parent body assumes the properties of the combined
286  // body (inertia etc.) and the merged body is held on a
287  // separate list for reference.
288  label merge
289  (
290  const label parentID,
291  const spatialTransform& X,
292  autoPtr<rigidBody> bodyPtr
293  );
294 
295  //- Return true if the body with given ID has been merged with a parent
296  inline bool merged(label bodyID) const;
297 
298  //- Return the ID of the master body for a sub-body otherwise
299  // return the given body ID
300  inline label master(label bodyID) const;
301 
302  //- Return the index of the merged body in the mergedBody list
303  // from the given body ID
304  inline label mergedBodyIndex(const label mergedBodyID) const;
305 
306  //- Return the merged body ID for the given merged body index
307  // in the mergedBody list
308  inline label mergedBodyID(const label mergedBodyIndex) const;
309 
310  //- Return the merged body for the given body ID
311  inline const subBody& mergedBody(label mergedBodyID) const;
312 
313  //- Return the ID of the body with the given name
314  inline label bodyID(const word& name) const;
315 
316  //- Return the current transform to the global frame for the given body
317  spatialTransform X0(const label bodyId) const;
318 
319  // Find the corresponding point in the master body frame
320  vector masterPoint(const label bodyID, const vector& p) const;
321 
322  //- Return the velocity of the given point on the given body
323  spatialVector v(const label bodyID, const vector& p) const;
324 
325  //- Apply the restraints and accumulate the internal joint forces
326  // into the tau field and external forces into the fx field
327  void applyRestraints(scalarField& tau, Field<spatialVector>& fx) const;
328 
329  //- Calculate the joint acceleration qDdot from the joint state q,
330  // velocity qDot, internal force tau (in the joint frame) and
331  // external force fx (in the global frame) using the articulated body
332  // algorithm (Section 7.3 and Table 7.1)
333  void forwardDynamics
334  (
335  rigidBodyModelState& state,
336  const scalarField& tau,
337  const Field<spatialVector>& fx
338  ) const;
339 
340  //- Correct the velocity and acceleration of the bodies in the model
341  // from the given joint state fields following an integration step
342  // of the forwardDynamics
343  void forwardDynamicsCorrection(const rigidBodyModelState& state) const;
344 
345  //- Write
346  virtual void write(Ostream&) const;
347 
348  //- Read coefficients dictionary and update system parameters,
349  // restraints but not the current state
350  bool read(const dictionary& dict);
351 
352 
353  // Ostream Operator
354 
355  friend Ostream& operator<<(Ostream&, const rigidBodyModel&);
356 };
357 
358 
359 // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
360 
361 } // End namespace RBD
362 } // End namespace Foam
363 
364 // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
365 
366 #include "rigidBodyModelI.H"
367 
368 // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
369 
370 #endif
371 
372 // ************************************************************************* //
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:137
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:62
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.
A templated 1D list of pointers to objects of type <T>, where the size of the array is known and used...
Definition: List.H:63
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 ...