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 -------------------------------------------------------------------------------
11 License
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 
38 namespace Foam
39 {
40 namespace RBD
41 {
42  defineTypeNameAndDebug(rigidBodyModel, 0);
43 }
44 }
45 
46 
47 // * * * * * * * * * * * * * Private Member Functions * * * * * * * * * * * //
48 
49 void 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 
64 void 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 
85 void 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,
268  autoPtr<joints::composite> cJointPtr,
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 
305 void 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 // ************************************************************************* //
Foam::RBD::rigidBody
Definition: rigidBody.H:65
nullJoint.H
Foam::entry
A keyword and a list of tokens is an 'entry'.
Definition: entry.H:67
compositeBody.H
Foam::RBD::rigidBodyModel::~rigidBodyModel
virtual ~rigidBodyModel()
Destructor.
Definition: rigidBodyModel.C:224
Foam::RBD::joint::New
static autoPtr< joint > New(joint *jointPtr)
Simple selector to return an autoPtr<joint> of the given joint*.
Definition: joint.C:46
Foam::Time
Class to control time during OpenFOAM simulations that is also the top-level objectRegistry.
Definition: Time.H:73
Foam::RBD::joint::nDoF
label nDoF() const
Return the number of degrees of freedom in this joint.
Definition: jointI.H:40
Foam::word
A class for handling words, derived from Foam::string.
Definition: word.H:65
Foam::RBD::rigidBodyModel::X0
spatialTransform X0(const label bodyId) const
Return the current transform to the global frame for the given body.
Definition: rigidBodyModel.C:381
Foam::spatialTransform
Compact representation of the Pl├╝cker spatial transformation tensor in terms of the rotation tensor E...
Definition: spatialTransform.H:70
Foam::Zero
static constexpr const zero Zero
Global zero (0)
Definition: zero.H:131
Foam::dictionary::found
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
Foam::RBD::joints::composite
Prismatic joint for translation along the specified arbitrary axis.
Definition: compositeJoint.H:69
Foam::read
bool read(const char *buf, int32_t &val)
Same as readInt32.
Definition: int32.H:108
Foam::glTF::key
auto key(const Type &t) -> typename std::enable_if< std::is_enum< Type >::value, typename std::underlying_type< Type >::type >::type
Definition: foamGltfBase.H:108
Foam::HashTable::insert
bool insert(const Key &key, const T &obj)
Copy insert a new entry, not overwriting existing entries.
Definition: HashTableI.H:180
Foam::RBD::rigidBodyModel::bodies_
PtrList< rigidBody > bodies_
List of the bodies.
Definition: rigidBodyModel.H:111
Foam::RBD::compositeBody
Definition: compositeBody.H:57
Foam::endl
Ostream & endl(Ostream &os)
Add newline and flush stream.
Definition: Ostream.H:369
Foam::dictionary::get
T get(const word &keyword, enum keyType::option matchOpt=keyType::REGEX) const
Definition: dictionaryTemplates.C:107
Foam::RBD::subBody::masterID
label masterID() const
Return the master body Id.
Definition: subBodyI.H:71
Foam::RBD::rigidBodyModel::rigidBodyModel
rigidBodyModel(const Time &time)
Null-constructor which adds the single root-body at the origin.
Definition: rigidBodyModel.C:169
masslessBody
Foam::RBD::operator<<
Ostream & operator<<(Ostream &, const rigidBody &)
Definition: rigidBodyI.H:75
rigidBodyModel.H
forAll
#define forAll(list, i)
Loop across all elements in list.
Definition: stdFoam.H:296
Foam::keyType
A class for handling keywords in dictionaries.
Definition: keyType.H:68
Foam::RBD::rigidBodyModel::lambda_
DynamicList< label > lambda_
List of indices of the parent of each body.
Definition: rigidBodyModel.H:123
Foam::DynamicList::append
DynamicList< T, SizeMin > & append(const T &val)
Append an element to the end of this list.
Definition: DynamicListI.H:511
Foam::autoPtr::release
T * release() noexcept
Release ownership and return the pointer.
Definition: autoPtrI.H:100
Foam::dictionary::subDict
const dictionary & subDict(const word &keyword, enum keyType::option matchOpt=keyType::REGEX) const
Find and return a sub-dictionary.
Definition: dictionary.C:460
Foam::RBD::rigidBodyModel::unitQuaternions_
bool unitQuaternions_
True if any of the joints using quaternions.
Definition: rigidBodyModel.H:136
Foam::autoPtr::ptr
T * ptr() noexcept
Same as release().
Definition: autoPtr.H:172
Foam::PtrList::append
void append(T *ptr)
Append an element to the end of the list.
Definition: PtrListI.H:113
Foam::RBD::subBody
Definition: subBody.H:57
Foam::SpatialTensor::I
static const SpatialTensor I
Identity matrix for square matrices.
Definition: SpatialTensor.H:83
Foam::RBD::defineTypeNameAndDebug
defineTypeNameAndDebug(cuboid, 0)
dict
dictionary dict
Definition: searchingEngine.H:14
Foam::RBD::rigidBodyModel
Basic rigid-body model representing a system of rigid-bodies connected by 1-6 DoF joints.
Definition: rigidBodyModel.H:83
Foam::dictionary
A list of keyword definitions, which are a keyword followed by a number of values (eg,...
Definition: dictionary.H:123
os
OBJstream os(runTime.globalPath()/outputName)
Foam
Namespace for OpenFOAM.
Definition: atmBoundaryLayer.C:33
Foam::RBD::rigidBodyModel::joints_
PtrList< joint > joints_
Each body it attached with a joint which are held on this list.
Definition: rigidBodyModel.H:126
Foam::indent
Ostream & indent(Ostream &os)
Indent stream.
Definition: Ostream.H:339
Foam::autoPtr::reset
void reset(autoPtr< T > &&other) noexcept
Delete managed object and set to new given pointer.
Definition: autoPtrI.H:117
Foam::RBD::rigidBodyModel::nDoF_
label nDoF_
The number of degrees of freedom of the model.
Definition: rigidBodyModel.H:133
Foam::autoPtr
Pointer management similar to std::unique_ptr, with some additional methods and type checking.
Definition: HashPtrTable.H:53
Foam::RBD::rigidBody::name
const word & name() const
Return name.
Definition: rigidBodyI.H:67
Foam::RBD::rigidBodyModel::join_
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.
Definition: rigidBodyModel.C:123
Foam::nl
constexpr char nl
Definition: Ostream.H:404
Foam::RBD::rigidBodyModel::join
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.
Definition: rigidBodyModel.C:231
Foam::RBD::rigidBodyModel::read
bool read(const dictionary &dict)
Read coefficients dictionary and update system parameters,.
Definition: rigidBodyModel.C:456
Foam::RBD::rigidBodyModel::write
virtual void write(Ostream &) const
Write.
Definition: rigidBodyModel.C:395
Foam::RBD::rigidBodyModel::XT_
DynamicList< spatialTransform > XT_
Transform from the parent body frame to the joint frame.
Definition: rigidBodyModel.H:129
Foam::RBD::jointBody
Definition: jointBody.H:55
jointBody.H
Foam::name
word name(const expressions::valueTypeCode typeCode)
A word representation of a valueTypeCode. Empty for INVALID.
Definition: exprTraits.C:59
Foam::RBD::joint
Abstract base-class for all rigid-body joints.
Definition: joint.H:84
Foam::RBD::subBody::masterXT
const spatialTransform & masterXT() const
Return the transform with respect to the master body.
Definition: subBodyI.H:77
Foam::Ostream
An Ostream is an abstract base class for all output systems (streams, files, token lists,...
Definition: Ostream.H:56
Foam::RBD::restraint::New
static autoPtr< restraint > New(const word &name, const dictionary &dict, const rigidBodyModel &model)
Select constructed from the dict dictionary and Time.
Definition: rigidBodyRestraintNew.C:35
Foam::RBD::joint::unitQuaternion
virtual bool unitQuaternion() const
Return true if this joint describes rotation using a quaternion.
Definition: jointI.H:45
Foam::RBD::rigidBodyModel::bodyIDs_
HashTable< label > bodyIDs_
Lookup-table of the IDs of the bodies.
Definition: rigidBodyModel.H:120
Foam::RBD::subBody::name
const word & name() const
Return the body name.
Definition: subBodyI.H:59
masslessBody.H
rigidBodyRestraint.H
Foam::RBD::rigidBody::New
static autoPtr< rigidBody > New(const word &name, const scalar &m, const vector &c, const symmTensor &Ic)
Select constructed from components.
Definition: rigidBody.C:63
Foam::RBD::rigidBodyModel::merge
label merge(const label parentID, const spatialTransform &X, autoPtr< rigidBody > bodyPtr)
Merge the given body with transform X into the parent with ID.
Definition: rigidBodyModel.C:323