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 | Copyright (C) 2016 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 // ************************************************************************* //
Ostream & operator<<(Ostream &, const rigidBody &)
Definition: rigidBodyI.H:73
dictionary dict
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
void forwardDynamics(rigidBodyModelState &state, const scalarField &tau, const Field< spatialVector > &fx) const
Calculate the joint acceleration qDdot from the joint state q,.
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
void applyRestraints(scalarField &tau, Field< spatialVector > &fx) const
Apply the restraints and accumulate the internal joint forces.
DynamicList< spatialVector > pA_
Articulated body bias force.
const subBody & mergedBody(label mergedBodyID) const
Return the merged body for the given body ID.
label bodyID(const word &name) const
Return the ID of the body with the given name.
label merge(const label parentID, const spatialTransform &X, autoPtr< rigidBody > bodyPtr)
Merge the given body with transform X into the parent with ID.
const DynamicList< label > & lambda() const
List of indices of the parent of each body.
bool merged(label bodyID) const
Return true if the body with given ID has been merged with a parent.
DynamicList< spatialTensor > IA_
Velocity-product acceleration.
bool unitQuaternions() const
Return true if any of the joints using quaternions.
vector masterPoint(const label bodyID, const vector &p) const
const PtrList< joint > & joints() const
Return the list of joints in the model.
label nBodies() const
Return the number of bodies in the model (bodies().size())
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.
label nDoF() const
Return the number of degrees of freedom of the model.
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.
const spatialVector & v(const label i) const
Return the spatial velocity of the bodies.
label mergedBodyIndex(const label mergedBodyID) const
Return the index of the merged body in the mergedBody list.
A class for handling words, derived from string.
Definition: word.H:59
spatialTransform X0(const label bodyId) const
Return the current transform to the global frame for the given body.
label nDoF_
The number of degrees of freedom of the model.
DynamicList< compactSpatialTensor > S_
Motion subspace for joints with 3 degrees of freedom.
bool read(const dictionary &dict)
Read coefficients dictionary and update system parameters,.
const vector & g() const
Return the acceleration due to gravity.
DynamicList< spatialTransform > XT_
Transform from the parent body frame to the joint frame.
PtrList< rigidBody > bodies() const
Return the list of the bodies in the model.
TypeName("rigidBodyModel")
Runtime type information.
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.
label master(label bodyID) const
Return the ID of the master body for a sub-body otherwise.
An Ostream is an abstract base class for all output systems (streams, files, token lists...
Definition: Ostream.H:53
virtual void write(Ostream &) const
Write.
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.
bool unitQuaternions_
True if any of the joints using quaternions.
label mergedBodyID(const label mergedBodyIndex) const
Return the merged body ID for the given merged body index.
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:62
const rigidBodyInertia & I(const label i) const
Return the inertia of body i.
virtual ~rigidBodyModel()
Destructor.
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:53
Basic rigid-body model representing a system of rigid-bodies connected by 1-6 DoF joints...
const word & name(const label bodyID) const
Return the name of body with the given ID.
volScalarField & p
rigidBodyModel()
Null-constructor which adds the single root-body at the origin.
void forwardDynamicsCorrection(const rigidBodyModelState &state) const
Correct the velocity and acceleration of the bodies in the model.
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.
vector g_
Acceleration due to gravity.
DynamicList< spatialVector > a_
The spatial acceleration of the bodies.
Namespace for OpenFOAM.