rigidBodyModel.C
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) 2020 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
27\*---------------------------------------------------------------------------*/
28
29#include "rigidBodyModel.H"
30#include "masslessBody.H"
31#include "compositeBody.H"
32#include "jointBody.H"
33#include "nullJoint.H"
34#include "rigidBodyRestraint.H"
35
36// * * * * * * * * * * * * * * Static Data Members * * * * * * * * * * * * * //
37
38namespace Foam
39{
40namespace RBD
41{
43}
44}
45
46
47// * * * * * * * * * * * * * Private Member Functions * * * * * * * * * * * //
48
49void Foam::RBD::rigidBodyModel::initializeRootBody()
50{
51 bodies_.append(new masslessBody("root"));
52 lambda_.append(0);
53 bodyIDs_.insert("root", 0);
54 joints_.append(new joints::null());
55 XT_.append(spatialTransform());
56
57 nDoF_ = 0;
58 unitQuaternions_ = false;
59
60 resizeState();
61}
62
63
64void Foam::RBD::rigidBodyModel::resizeState()
65{
66 Xlambda_.append(spatialTransform());
67 X0_.append(spatialTransform());
68
69 v_.append(Zero);
70 a_.append(Zero);
71 c_.append(Zero);
72
73 IA_.append(spatialTensor::I);
74 pA_.append(Zero);
75
76 S_.append(Zero);
77 S1_.append(Zero);
78 U_.append(Zero);
79 U1_.append(Zero);
80 Dinv_.append(Zero);
81 u_.append(Zero);
82}
83
84
85void Foam::RBD::rigidBodyModel::addRestraints
86(
87 const dictionary& dict
88)
89{
90 if (dict.found("restraints"))
91 {
92 const dictionary& restraintDict = dict.subDict("restraints");
93
94 label i = 0;
95
96 restraints_.setSize(restraintDict.size());
97
98 for (const entry& dEntry : restraintDict)
99 {
100 if (dEntry.isDict())
101 {
102 restraints_.set
103 (
104 i++,
106 (
107 dEntry.keyword(),
108 dEntry.dict(),
109 *this
110 )
111 );
112 }
113 }
114
115 restraints_.setSize(i);
116 }
117}
118
119
120// * * * * * * * * * * * * Protected Member Functions * * * * * * * * * * * //
121
123(
124 const label parentID,
125 const spatialTransform& XT,
126 autoPtr<joint> jointPtr,
127 autoPtr<rigidBody> bodyPtr
128)
129{
130 // Append the body
131 const rigidBody& body = bodyPtr();
132 bodies_.append(bodyPtr);
133 const label bodyID = nBodies()-1;
134 bodyIDs_.insert(body.name(), bodyID);
135
136 // If the parentID refers to a merged body find the parent into which it has
137 // been merged and set lambda and XT accordingly
138 if (merged(parentID))
139 {
140 const subBody& sBody = mergedBody(parentID);
141 lambda_.append(sBody.masterID());
142 XT_.append(XT & sBody.masterXT());
143 }
144 else
145 {
146 lambda_.append(parentID);
147 XT_.append(XT);
148 }
149
150 // Append the joint
151 const joint& prevJoint = joints_[joints_.size() - 1];
152 joints_.append(jointPtr);
153 joint& curJoint = joints_[joints_.size() - 1];
154 curJoint.index() = joints_.size() - 1;
155 curJoint.qIndex() = prevJoint.qIndex() + prevJoint.nDoF();
156
157 // Increment the degrees of freedom
158 nDoF_ += curJoint.nDoF();
159 unitQuaternions_ = unitQuaternions_ || curJoint.unitQuaternion();
160
161 resizeState();
162
163 return bodyID;
164}
165
166
167// * * * * * * * * * * * * * * * * Constructors * * * * * * * * * * * * * * * //
168
170:
171 time_(time),
172 g_(Zero)
173{
174 initializeRootBody();
175}
176
177
179(
180 const Time& time,
181 const dictionary& dict
182)
183:
184 time_(time),
185 g_(Zero)
186{
187 initializeRootBody();
188
189 const dictionary& bodiesDict = dict.subDict("bodies");
190
191 for (const entry& dEntry : bodiesDict)
192 {
193 const keyType& key = dEntry.keyword();
194 const dictionary& bodyDict = dEntry.dict();
195
196 if (bodyDict.found("mergeWith"))
197 {
198 merge
199 (
200 bodyID(bodyDict.get<word>("mergeWith")),
201 bodyDict.get<spatialTransform>("transform"),
202 rigidBody::New(key, bodyDict)
203 );
204 }
205 else
206 {
207 join
208 (
209 bodyID(bodyDict.get<word>("parent")),
210 bodyDict.get<spatialTransform>("transform"),
211 joint::New(bodyDict.subDict("joint")),
212 rigidBody::New(key, bodyDict)
213 );
214 }
215 }
216
217 // Read the restraints and any other re-readable settings.
218 read(dict);
219}
220
221
222// * * * * * * * * * * * * * * * * Destructor * * * * * * * * * * * * * * * //
223
225{}
226
227
228// * * * * * * * * * * * * * * * Member Functions * * * * * * * * * * * * * //
229
231(
232 const label parentID,
233 const spatialTransform& XT,
234 autoPtr<joint> jointPtr,
235 autoPtr<rigidBody> bodyPtr
236)
237{
238 if (isA<joints::composite>(jointPtr()))
239 {
240 return join
241 (
242 parentID,
243 XT,
245 (
246 dynamic_cast<joints::composite*>(jointPtr.ptr())
247 ),
248 bodyPtr
249 );
250 }
251 else
252 {
253 return join_
254 (
255 parentID,
256 XT,
257 jointPtr,
258 bodyPtr
259 );
260 }
261}
262
263
265(
266 const label parentID,
267 const spatialTransform& XT,
269 autoPtr<rigidBody> bodyPtr
270)
271{
272 label parent = parentID;
273 joints::composite& cJoint = cJointPtr();
274
275 // For all but the final joint in the set add a jointBody with the
276 // joint and transform
277 for (label j=0; j<cJoint.size()-1; j++)
278 {
279 parent = join_
280 (
281 parent,
282 j == 0 ? XT : spatialTransform(),
283 cJoint[j].clone(),
285 );
286 }
287
288 // For the final joint in the set add the real body
289 parent = join_
290 (
291 parent,
292 cJoint.size() == 1 ? XT : spatialTransform(),
293 autoPtr<joint>(cJointPtr.ptr()),
294 bodyPtr
295 );
296
297 // Set the properties of the last joint in the list to those set
298 // by rigidBodyModel
299 cJoint.setLastJoint();
300
301 return parent;
302}
303
304
305void Foam::RBD::rigidBodyModel::makeComposite(const label bodyID)
306{
307 if (!isA<compositeBody>(bodies_[bodyID]))
308 {
309 // Retrieve the un-merged body
310 autoPtr<rigidBody> bodyPtr = bodies_.release(bodyID);
311
312 // Insert the compositeBody containing the original body
313 bodies_.set
314 (
315 bodyID,
316 new compositeBody(bodyPtr)
317 );
318 }
319}
320
321
323(
324 const label parentID,
325 const spatialTransform& XT,
326 autoPtr<rigidBody> bodyPtr
327)
328{
329 autoPtr<subBody> sBodyPtr;
330
331 // If the parentID refers to a merged body find the parent into which it has
332 // been merged and merge this on into the same parent with the appropriate
333 // transform
334 if (merged(parentID))
335 {
336 const subBody& sBody = mergedBody(parentID);
337
338 makeComposite(sBody.masterID());
339
340 sBodyPtr.reset
341 (
342 new subBody
343 (
344 bodyPtr,
345 bodies_[sBody.masterID()].name(),
346 sBody.masterID(),
347 XT & sBody.masterXT()
348 )
349 );
350 }
351 else
352 {
353 makeComposite(parentID);
354
355 sBodyPtr.reset
356 (
357 new subBody
358 (
359 bodyPtr,
360 bodies_[parentID].name(),
361 parentID,
362 XT
363 )
364 );
365 }
366
367 const subBody& sBody = sBodyPtr();
368 mergedBodies_.append(sBodyPtr);
369
370 // Merge the sub-body with the parent
371 bodies_[sBody.masterID()].merge(sBody);
372
373 const label sBodyID = mergedBodyID(mergedBodies_.size() - 1);
374 bodyIDs_.insert(sBody.name(), sBodyID);
375
376 return sBodyID;
377}
378
379
381(
382 const label bodyId
383) const
384{
385 if (merged(bodyId))
386 {
387 const subBody& mBody = mergedBody(bodyId);
388 return mBody.masterXT() & X0_[mBody.masterID()];
389 }
390
391 return X0_[bodyId];
392}
393
394
396{
397 os.beginBlock("bodies");
398
399 // Write the moving bodies
400 for (label i=1; i<nBodies(); i++)
401 {
402 // Do not write joint-bodies created automatically to support elements
403 // of composite joints
404 if (!isType<jointBody>(bodies_[i]))
405 {
406 os.beginBlock(bodies_[i].name());
407
408 bodies_[i].write(os);
409
410 os.writeEntry("parent", bodies_[lambda_[i]].name());
411 os.writeEntry("transform", XT_[i]);
412
413 os << indent << "joint" << nl
414 << joints_[i] << endl;
415
416 os.endBlock();
417 }
418 }
419
420 // Write the bodies merged into the parent bodies for efficiency
421 forAll(mergedBodies_, i)
422 {
423 os.beginBlock(mergedBodies_[i].name());
424
425 mergedBodies_[i].body().write(os);
426
427 os.writeEntry("transform", mergedBodies_[i].masterXT());
428 os.writeEntry("mergeWith", mergedBodies_[i].masterName());
429
430 os.endBlock();
431 }
432
433 os.endBlock();
434
435
436 if (!restraints_.empty())
437 {
438 os.beginBlock("restraints");
439
440 forAll(restraints_, ri)
441 {
442 // const word& restraintType(restraints_[ri].type());
443
444 os.beginBlock(restraints_[ri].name());
445
446 restraints_[ri].write(os);
447
448 os.endBlock();
449 }
450
451 os.endBlock();
452 }
453}
454
455
457{
458 restraints_.clear();
459 addRestraints(dict);
460
461 return true;
462}
463
464
465// * * * * * * * * * * * * * * * Ostream Operator * * * * * * * * * * * * * //
466
468{
469 rbm.write(os);
470 return os;
471}
472
473
474// ************************************************************************* //
void append(const T &val)
Copy append an element to the end of this list.
Definition: DynamicListI.H:503
bool insert(const Key &key, const T &obj)
Copy insert a new entry, not overwriting existing entries.
Definition: HashTableI.H:180
An Ostream is an abstract base class for all output systems (streams, files, token lists,...
Definition: Ostream.H:62
void append(T *ptr)
Append an element to the end of the list.
Definition: PtrListI.H:113
virtual bool read()
Re-read model coefficients if they have changed.
Abstract base-class for all rigid-body joints.
Definition: joint.H:85
label nDoF() const
Return the number of degrees of freedom in this joint.
Definition: jointI.H:40
virtual bool unitQuaternion() const
Return true if this joint describes rotation using a quaternion.
Definition: jointI.H:45
Prismatic joint for translation along the specified arbitrary axis.
Basic rigid-body model representing a system of rigid-bodies connected by 1-6 DoF joints.
DynamicList< spatialTransform > XT_
Transform from the parent body frame to the joint frame.
virtual void write(Ostream &) const
Write.
label nDoF_
The number of degrees of freedom of the model.
PtrList< rigidBody > bodies_
List 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.
bool read(const dictionary &dict)
Read coefficients dictionary and update system parameters,.
bool unitQuaternions_
True if any of the joints using quaternions.
label bodyID(const word &name) const
Return the ID of the body with the given name.
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.
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.
HashTable< label > bodyIDs_
Lookup-table of the IDs of the bodies.
virtual ~rigidBodyModel()
Destructor.
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.
const word & name() const
Return name.
Definition: rigidBodyI.H:67
const spatialTransform & masterXT() const
Return the transform with respect to the master body.
Definition: subBodyI.H:77
label masterID() const
Return the master body Id.
Definition: subBodyI.H:71
const word & name() const
Return the body name.
Definition: subBodyI.H:59
static const SpatialTensor I
Identity matrix for square matrices.
Definition: SpatialTensor.H:83
Class to control time during OpenFOAM simulations that is also the top-level objectRegistry.
Definition: Time.H:80
static autoPtr< Time > New()
Construct (dummy) Time - no functionObjects or libraries.
Definition: Time.C:717
label size() const noexcept
The number of elements in the list.
Definition: UPtrListI.H:106
Pointer management similar to std::unique_ptr, with some additional methods and type checking.
Definition: autoPtr.H:66
T * release() noexcept
Release ownership and return the pointer.
Definition: autoPtrI.H:100
void reset(autoPtr< T > &&other) noexcept
Delete managed object and set to new given pointer.
Definition: autoPtrI.H:117
T * ptr() noexcept
Same as release().
Definition: autoPtr.H:172
virtual bool merge() const
A list of keyword definitions, which are a keyword followed by a number of values (eg,...
Definition: dictionary.H:126
T get(const word &keyword, enum keyType::option matchOpt=keyType::REGEX) const
const dictionary & subDict(const word &keyword, enum keyType::option matchOpt=keyType::REGEX) const
Find and return a sub-dictionary.
Definition: dictionary.C:460
bool found(const word &keyword, enum keyType::option matchOpt=keyType::REGEX) const
Search for an entry (const access) with the given keyword.
Definition: dictionaryI.H:87
A keyword and a list of tokens is an 'entry'.
Definition: entry.H:70
virtual bool write()
Write the output fields.
A class for handling keywords in dictionaries.
Definition: keyType.H:71
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
#define defineTypeNameAndDebug(Type, DebugSwitch)
Define the typeName and debug information.
Definition: className.H:121
OBJstream os(runTime.globalPath()/outputName)
Ostream & operator<<(Ostream &, const rigidBody &)
Definition: rigidBodyI.H:75
Namespace for OpenFOAM.
Ostream & endl(Ostream &os)
Add newline and flush stream.
Definition: Ostream.H:372
Ostream & indent(Ostream &os)
Indent stream.
Definition: Ostream.H:342
static constexpr const zero Zero
Global zero (0)
Definition: zero.H:131
word name(const expressions::valueTypeCode typeCode)
A word representation of a valueTypeCode. Empty for INVALID.
Definition: exprTraits.C:59
constexpr char nl
The newline '\n' character (0x0a)
Definition: Ostream.H:53
dictionary dict
#define forAll(list, i)
Loop across all elements in list.
Definition: stdFoam.H:333