Basic rigid-body model representing a system of rigid-bodies connected by 1-6 DoF joints. More...
Public Member Functions | |
TypeName ("rigidBodyModel") | |
Runtime type information. More... | |
rigidBodyModel () | |
Null-constructor which adds the single root-body at the origin. More... | |
rigidBodyModel (const dictionary &dict) | |
Construct from dictionary. More... | |
virtual | ~rigidBodyModel () |
Destructor. More... | |
label | nBodies () const |
Return the number of bodies in the model (bodies().size()) More... | |
PtrList< rigidBody > | bodies () const |
Return the list of the bodies in the model. More... | |
const DynamicList< label > & | lambda () const |
List of indices of the parent of each body. More... | |
const PtrList< joint > & | joints () const |
Return the list of joints in the model. More... | |
label | nDoF () const |
Return the number of degrees of freedom of the model. More... | |
bool | unitQuaternions () const |
Return true if any of the joints using quaternions. More... | |
const vector & | g () const |
Return the acceleration due to gravity. More... | |
vector & | g () |
Allow the acceleration due to gravity to be set. More... | |
const word & | name (const label bodyID) const |
Return the name of body with the given ID. More... | |
wordList | movingBodyNames () const |
Return the names of the moving bodies. More... | |
const rigidBodyInertia & | I (const label i) const |
Return the inertia of body i. More... | |
const spatialVector & | v (const label i) const |
Return the spatial velocity of the bodies. More... | |
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. More... | |
label | join (const label parentID, const spatialTransform &XT, autoPtr< joints::composite > cJoint, autoPtr< rigidBody > bodyPtr) |
Join the given body to the parent with ID parentID via the given. More... | |
label | merge (const label parentID, const spatialTransform &X, autoPtr< rigidBody > bodyPtr) |
Merge the given body with transform X into the parent with ID. More... | |
bool | merged (label bodyID) const |
Return true if the body with given ID has been merged with a parent. More... | |
label | master (label bodyID) const |
Return the ID of the master body for a sub-body otherwise. More... | |
label | mergedBodyIndex (const label mergedBodyID) const |
Return the index of the merged body in the mergedBody list. More... | |
label | mergedBodyID (const label mergedBodyIndex) const |
Return the merged body ID for the given merged body index. More... | |
const subBody & | mergedBody (label mergedBodyID) const |
Return the merged body for the given body ID. More... | |
label | bodyID (const word &name) const |
Return the ID of the body with the given name. More... | |
spatialTransform | X0 (const label bodyId) const |
Return the current transform to the global frame for the given body. More... | |
vector | masterPoint (const label bodyID, const vector &p) const |
spatialVector | v (const label bodyID, const vector &p) const |
Return the velocity of the given point on the given body. More... | |
void | applyRestraints (scalarField &tau, Field< spatialVector > &fx, const rigidBodyModelState &state) const |
Apply the restraints and accumulate the internal joint forces. More... | |
void | forwardDynamics (rigidBodyModelState &state, const scalarField &tau, const Field< spatialVector > &fx) const |
Calculate the joint acceleration qDdot from the joint state q,. More... | |
void | forwardDynamicsCorrection (const rigidBodyModelState &state) const |
Correct the velocity and acceleration of the bodies in the model. More... | |
virtual void | write (Ostream &) const |
Write. More... | |
bool | read (const dictionary &dict) |
Read coefficients dictionary and update system parameters,. More... | |
Protected Member Functions | |
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. More... | |
Protected Attributes | |
PtrList< rigidBody > | bodies_ |
List of the bodies. More... | |
PtrList< subBody > | mergedBodies_ |
Bodies may be merged into existing bodies, the inertia of which is. More... | |
HashTable< label, word > | bodyIDs_ |
Lookup-table of the IDs of the bodies. More... | |
DynamicList< label > | lambda_ |
List of indices of the parent of each body. More... | |
PtrList< joint > | joints_ |
Each body it attached with a joint which are held on this list. More... | |
DynamicList< spatialTransform > | XT_ |
Transform from the parent body frame to the joint frame. More... | |
label | nDoF_ |
The number of degrees of freedom of the model. More... | |
bool | unitQuaternions_ |
True if any of the joints using quaternions. More... | |
PtrList< restraint > | restraints_ |
Motion restraints. More... | |
vector | g_ |
Acceleration due to gravity. More... | |
DynamicList< spatialTransform > | Xlambda_ |
Transform from the parent body to the current body. More... | |
DynamicList< spatialTransform > | X0_ |
Transform for external forces to the bodies reference frame. More... | |
DynamicList< spatialVector > | v_ |
The spatial velocity of the bodies. More... | |
DynamicList< spatialVector > | a_ |
The spatial acceleration of the bodies. More... | |
DynamicList< spatialVector > | c_ |
The velocity dependent spatial acceleration of the joints. More... | |
DynamicList< spatialTensor > | IA_ |
Velocity-product acceleration. More... | |
DynamicList< spatialVector > | pA_ |
Articulated body bias force. More... | |
DynamicList< compactSpatialTensor > | S_ |
Motion subspace for joints with 3 degrees of freedom. More... | |
DynamicList< spatialVector > | S1_ |
Motion subspace for joints with 1 degrees of freedom. More... | |
DynamicList< compactSpatialTensor > | U_ |
Sub-expression IA.S in the forward-dynamics algorithm. More... | |
DynamicList< spatialVector > | U1_ |
Sub-expression IA.S1 in the forward-dynamics algorithm. More... | |
DynamicList< tensor > | Dinv_ |
Sub-expression (S^T.U)^-1 in the forward-dynamics algorithm. More... | |
DynamicList< vector > | u_ |
Sub-expression tau - S^T.pA in the forward-dynamics algorithm. More... | |
Friends | |
Ostream & | operator<< (Ostream &, const rigidBodyModel &) |
Basic rigid-body model representing a system of rigid-bodies connected by 1-6 DoF joints.
This class holds various body and joint state fields needed by the kinematics and forward-dynamics algorithms presented in
reference:
Featherstone, R. (2008). Rigid body dynamics algorithms. Springer. Chapter 4.
Definition at line 76 of file rigidBodyModel.H.
rigidBodyModel | ( | ) |
Null-constructor which adds the single root-body at the origin.
Definition at line 166 of file rigidBodyModel.C.
rigidBodyModel | ( | const dictionary & | dict | ) |
Construct from dictionary.
Definition at line 174 of file rigidBodyModel.C.
References rigidBodyModel::bodyID(), forAllConstIter(), dictionary::found(), rigidBodyModel::join(), dictionary::lookup(), rigidBodyModel::merge(), rigidBody::New(), joint::New(), rigidBodyModel::read(), and dictionary::subDict().
|
virtual |
Destructor.
Definition at line 214 of file rigidBodyModel.C.
References rigidBodyModel::join().
|
protectedvirtual |
Join the given body to the parent with ID parentID via the given.
joint with transform from the parent frame to the joint frame XT.
Definition at line 120 of file rigidBodyModel.C.
References subBody::masterID(), subBody::masterXT(), rigidBody::name(), joint::nDoF(), and joint::unitQuaternion().
Referenced by rigidBodyModel::join().
TypeName | ( | "rigidBodyModel" | ) |
Runtime type information.
|
inline |
Return the number of bodies in the model (bodies().size())
Definition at line 28 of file rigidBodyModelI.H.
References rigidBodyModel::bodies_.
Referenced by rigidBodyModel::movingBodyNames(), rigidBodyMeshMotionSolver::solve(), rigidBodyMeshMotion::solve(), and rigidBodyModel::write().
|
inline |
Return the list of the bodies in the model.
Definition at line 35 of file rigidBodyModelI.H.
References rigidBodyModel::bodies_.
|
inline |
List of indices of the parent of each body.
Definition at line 42 of file rigidBodyModelI.H.
References rigidBodyModel::lambda_.
Referenced by rigidBodyMotion::transformPoints().
|
inline |
Return the list of joints in the model.
Definition at line 49 of file rigidBodyModelI.H.
References rigidBodyModel::joints_.
Referenced by rigidBodySolver::correctQuaternionJoints().
|
inline |
Return the number of degrees of freedom of the model.
used to set the size of the of joint state fields q, qDot and qDdot.
Definition at line 55 of file rigidBodyModelI.H.
References rigidBodyModel::nDoF_.
Referenced by rigidBodyMeshMotionSolver::solve(), and rigidBodyMeshMotion::solve().
|
inline |
Return true if any of the joints using quaternions.
Definition at line 61 of file rigidBodyModelI.H.
References rigidBodyModel::unitQuaternions_.
Referenced by rigidBodySolver::correctQuaternionJoints().
|
inline |
Return the acceleration due to gravity.
Definition at line 67 of file rigidBodyModelI.H.
References rigidBodyModel::g_.
Referenced by rigidBodyMotion::rigidBodyMotion(), rigidBodyMeshMotionSolver::solve(), and rigidBodyMeshMotion::solve().
|
inline |
Allow the acceleration due to gravity to be set.
after model construction
Definition at line 73 of file rigidBodyModelI.H.
References rigidBodyModel::g_, and rigidBodyModel::name().
|
inline |
Return the name of body with the given ID.
Definition at line 80 of file rigidBodyModelI.H.
References rigidBodyModel::bodies_, rigidBodyModel::bodyID(), rigidBodyModel::merged(), rigidBodyModel::mergedBody(), and subBody::name().
Referenced by rigidBodyModel::bodyID(), rigidBodyModel::g(), rigidBodyModel::merge(), rigidBodyMotion::status(), and rigidBodyModel::write().
Foam::wordList movingBodyNames | ( | ) | const |
Return the names of the moving bodies.
Definition at line 387 of file rigidBodyModel.C.
References rigidBodyModel::bodies_, rigidBodyModel::nBodies(), and List< T >::setSize().
|
inline |
Return the inertia of body i.
Definition at line 96 of file rigidBodyModelI.H.
References rigidBodyModel::bodies_.
|
inline |
Return the spatial velocity of the bodies.
Definition at line 103 of file rigidBodyModelI.H.
References rigidBodyModel::v_.
Referenced by rigidBodyModel::masterPoint(), rigidBodyMotion::status(), and rigidBodyState::write().
|
virtual |
Join the given body to the parent with ID parentID via the given.
joint with transform from the parent frame to the joint frame XT.
Definition at line 221 of file rigidBodyModel.C.
References rigidBodyModel::join_(), and autoPtr< T >::ptr().
Referenced by rigidBodyModel::rigidBodyModel(), and rigidBodyModel::~rigidBodyModel().
Foam::label join | ( | const label | parentID, |
const spatialTransform & | XT, | ||
autoPtr< joints::composite > | cJoint, | ||
autoPtr< rigidBody > | bodyPtr | ||
) |
Join the given body to the parent with ID parentID via the given.
composite joint (specified as a list of co-located joints) with transform from the parent frame to the joint frame XT. Composite joins are useful to represent complex joints with degrees of freedom other than 1 or 3 which are directly supported.
Definition at line 255 of file rigidBodyModel.C.
References rigidBodyModel::bodies_, rigidBodyModel::bodyID(), Foam::clone(), rigidBodyModel::join_(), rigidBodyModel::merge(), autoPtr< T >::ptr(), and UPtrList< T >::size().
Foam::label merge | ( | const label | parentID, |
const spatialTransform & | X, | ||
autoPtr< rigidBody > | bodyPtr | ||
) |
Merge the given body with transform X into the parent with ID.
parentID. The parent body assumes the properties of the combined body (inertia etc.) and the merged body is held on a separate list for reference.
Definition at line 313 of file rigidBodyModel.C.
References rigidBodyModel::bodies_, rigidBodyModel::bodyIDs_, subBody::masterID(), subBody::masterXT(), rigidBodyModel::merged(), rigidBodyModel::mergedBodies_, rigidBodyModel::mergedBody(), rigidBodyModel::mergedBodyID(), subBody::name(), rigidBodyModel::name(), autoPtr< T >::set(), and rigidBodyModel::X0().
Referenced by rigidBodyModel::join(), and rigidBodyModel::rigidBodyModel().
|
inline |
Return true if the body with given ID has been merged with a parent.
Definition at line 109 of file rigidBodyModelI.H.
Referenced by rigidBodyModel::masterPoint(), rigidBodyModel::merge(), rigidBodyModel::mergedBody(), rigidBodyModel::name(), rigidBodyModel::X0(), and rigidBodyMotion::X00().
|
inline |
Return the ID of the master body for a sub-body otherwise.
return the given body ID
Definition at line 115 of file rigidBodyModelI.H.
References rigidBodyModel::bodyID(), subBody::masterID(), and rigidBodyModel::mergedBody().
Referenced by rigidBodyModel::v().
|
inline |
Return the index of the merged body in the mergedBody list.
from the given body ID
Definition at line 136 of file rigidBodyModelI.H.
References rigidBodyModel::mergedBodyID().
Referenced by rigidBodyModel::mergedBody(), and rigidBodyModel::mergedBodyID().
|
inline |
Return the merged body ID for the given merged body index.
in the mergedBody list
Definition at line 129 of file rigidBodyModelI.H.
References rigidBodyModel::mergedBodyIndex().
Referenced by rigidBodyModel::merge(), and rigidBodyModel::mergedBodyIndex().
|
inline |
Return the merged body for the given body ID.
Definition at line 143 of file rigidBodyModelI.H.
References Foam::abort(), Foam::FatalError, FatalErrorInFunction, rigidBodyModel::merged(), rigidBodyModel::mergedBodies_, and rigidBodyModel::mergedBodyIndex().
Referenced by rigidBodyModel::master(), rigidBodyModel::masterPoint(), rigidBodyModel::merge(), rigidBodyModel::name(), rigidBodyModel::X0(), and rigidBodyMotion::X00().
|
inline |
Return the ID of the body with the given name.
Definition at line 156 of file rigidBodyModelI.H.
References rigidBodyModel::bodyIDs_, rigidBodyModel::masterPoint(), and rigidBodyModel::name().
Referenced by rigidBodyModel::join(), rigidBodyModel::master(), rigidBodyModel::name(), rigidBodyMeshMotion::rigidBodyMeshMotion(), rigidBodyMeshMotionSolver::rigidBodyMeshMotionSolver(), rigidBodyModel::rigidBodyModel(), rigidBodyMeshMotionSolver::solve(), rigidBodyMeshMotion::solve(), and rigidBodyState::write().
Foam::spatialTransform X0 | ( | const label | bodyId | ) | const |
Return the current transform to the global frame for the given body.
Definition at line 371 of file rigidBodyModel.C.
References subBody::masterID(), subBody::masterXT(), rigidBodyModel::merged(), rigidBodyModel::mergedBody(), and rigidBodyModel::X0_.
Referenced by rigidBodyModel::merge(), rigidBodyMotion::status(), rigidBodyMotion::transformPoints(), and rigidBodyState::write().
|
inline |
Definition at line 163 of file rigidBodyModelI.H.
References Foam::inv(), rigidBodyModel::merged(), rigidBodyModel::mergedBody(), p, rigidBodyModel::v(), and Foam::Zero.
Referenced by rigidBodyModel::bodyID(), and rigidBodyModel::v().
|
inline |
Return the velocity of the given point on the given body.
Definition at line 184 of file rigidBodyModelI.H.
References rigidBodyModel::master(), rigidBodyModel::masterPoint(), Foam::T(), rigidBodyModel::v_, and rigidBodyModel::X0_.
void applyRestraints | ( | scalarField & | tau, |
Field< spatialVector > & | fx, | ||
const rigidBodyModelState & | state | ||
) | const |
Apply the restraints and accumulate the internal joint forces.
into the tau field and external forces into the fx field
Definition at line 33 of file forwardDynamics.C.
References DebugInfo, forAll, and rigidBodyModel::forwardDynamics().
void forwardDynamics | ( | rigidBodyModelState & | state, |
const scalarField & | tau, | ||
const Field< spatialVector > & | fx | ||
) | const |
Calculate the joint acceleration qDdot from the joint state q,.
velocity qDot, internal force tau (in the joint frame) and external force fx (in the global frame) using the articulated body algorithm (Section 7.3 and Table 7.1)
Definition at line 55 of file forwardDynamics.C.
References Field< Type >::block(), joint::XSvc::c, DebugInfo, DebugInFunction, Foam::endl(), rigidBodyModel::forwardDynamicsCorrection(), Foam::I, Foam::inv(), joint::jcalc(), joint::nDoF(), Foam::nl, rigidBodyModelState::q(), rigidBodyModelState::qDdot(), rigidBodyModelState::qDot(), joint::XSvc::S, joint::XSvc::S1, List< Type >::size(), T, joint::XSvc::v, x, Vector< Cmpt >::x(), joint::XSvc::X, Vector< Cmpt >::y(), Vector< Cmpt >::z(), and Foam::Zero.
Referenced by rigidBodyModel::applyRestraints(), and rigidBodyMotion::forwardDynamics().
void forwardDynamicsCorrection | ( | const rigidBodyModelState & | state | ) | const |
Correct the velocity and acceleration of the bodies in the model.
from the given joint state fields following an integration step of the forwardDynamics
Definition at line 227 of file forwardDynamics.C.
References Field< Type >::block(), joint::XSvc::c, DebugInfo, DebugInFunction, Foam::endl(), joint::jcalc(), joint::nDoF(), rigidBodyModelState::qDdot(), joint::XSvc::S, joint::XSvc::S1, joint::XSvc::v, joint::XSvc::X, and Foam::Zero.
Referenced by rigidBodyModel::forwardDynamics(), and rigidBodyMotion::solve().
|
virtual |
Write.
Reimplemented in rigidBodyMotion.
Definition at line 406 of file rigidBodyModel.C.
References token::BEGIN_BLOCK, rigidBodyModel::bodies_, Foam::decrIndent(), token::END_BLOCK, Foam::endl(), forAll, Foam::incrIndent(), Foam::indent(), rigidBodyModel::joints_, rigidBodyModel::lambda_, rigidBodyModel::mergedBodies_, rigidBodyModel::name(), rigidBodyModel::nBodies(), Foam::nl, rigidBodyModel::restraints_, Foam::writeEntry(), and rigidBodyModel::XT_.
Referenced by Foam::RBD::operator<<(), and rigidBodyMotion::write().
bool read | ( | const dictionary & | dict | ) |
Read coefficients dictionary and update system parameters,.
restraints but not the current state
Definition at line 471 of file rigidBodyModel.C.
References rigidBodyModel::restraints_.
Referenced by rigidBodyMotion::read(), and rigidBodyModel::rigidBodyModel().
|
friend |
List of the bodies.
The 0'th body represents the fixed origin and is constructed automatically. The subsequent (moving) bodies are appended by the join member function.
Definition at line 101 of file rigidBodyModel.H.
Referenced by rigidBodyModel::bodies(), rigidBodyModel::I(), rigidBodyModel::join(), rigidBodyModel::merge(), rigidBodyModel::movingBodyNames(), rigidBodyModel::name(), rigidBodyModel::nBodies(), and rigidBodyModel::write().
Bodies may be merged into existing bodies, the inertia of which is.
updated to represent the combined body which is more efficient than attaching them with fixed joints. These 'merged' bodies are held on this list.
Definition at line 107 of file rigidBodyModel.H.
Referenced by rigidBodyModel::merge(), rigidBodyModel::mergedBody(), and rigidBodyModel::write().
Lookup-table of the IDs of the bodies.
Definition at line 110 of file rigidBodyModel.H.
Referenced by rigidBodyModel::bodyID(), and rigidBodyModel::merge().
|
protected |
List of indices of the parent of each body.
Definition at line 113 of file rigidBodyModel.H.
Referenced by rigidBodyModel::lambda(), and rigidBodyModel::write().
Each body it attached with a joint which are held on this list.
Definition at line 116 of file rigidBodyModel.H.
Referenced by rigidBodyModel::joints(), and rigidBodyModel::write().
|
protected |
Transform from the parent body frame to the joint frame.
Definition at line 119 of file rigidBodyModel.H.
Referenced by rigidBodyModel::write().
|
protected |
The number of degrees of freedom of the model.
used to set the size of the of joint state fields q, qDot and qDdot.
Definition at line 123 of file rigidBodyModel.H.
Referenced by rigidBodyModel::nDoF().
|
protected |
True if any of the joints using quaternions.
Definition at line 126 of file rigidBodyModel.H.
Referenced by rigidBodyModel::unitQuaternions().
Motion restraints.
Definition at line 129 of file rigidBodyModel.H.
Referenced by rigidBodyModel::read(), and rigidBodyModel::write().
|
protected |
Acceleration due to gravity.
Definition at line 135 of file rigidBodyModel.H.
Referenced by rigidBodyModel::g().
|
mutableprotected |
Transform from the parent body to the current body.
Definition at line 141 of file rigidBodyModel.H.
|
mutableprotected |
Transform for external forces to the bodies reference frame.
Definition at line 144 of file rigidBodyModel.H.
Referenced by rigidBodyModel::v(), and rigidBodyModel::X0().
|
mutableprotected |
The spatial velocity of the bodies.
Definition at line 150 of file rigidBodyModel.H.
Referenced by rigidBodyModel::v().
|
mutableprotected |
The spatial acceleration of the bodies.
Definition at line 153 of file rigidBodyModel.H.
|
mutableprotected |
The velocity dependent spatial acceleration of the joints.
Definition at line 156 of file rigidBodyModel.H.
|
mutableprotected |
Velocity-product acceleration.
Articulated body inertia
Definition at line 164 of file rigidBodyModel.H.
|
mutableprotected |
Articulated body bias force.
Definition at line 167 of file rigidBodyModel.H.
|
mutableprotected |
Motion subspace for joints with 3 degrees of freedom.
Definition at line 173 of file rigidBodyModel.H.
|
mutableprotected |
Motion subspace for joints with 1 degrees of freedom.
Definition at line 176 of file rigidBodyModel.H.
|
mutableprotected |
Sub-expression IA.S in the forward-dynamics algorithm.
Definition at line 179 of file rigidBodyModel.H.
|
mutableprotected |
Sub-expression IA.S1 in the forward-dynamics algorithm.
Definition at line 182 of file rigidBodyModel.H.
|
mutableprotected |
Sub-expression (S^T.U)^-1 in the forward-dynamics algorithm.
Definition at line 185 of file rigidBodyModel.H.
|
mutableprotected |
Sub-expression tau - S^T.pA in the forward-dynamics algorithm.
Definition at line 188 of file rigidBodyModel.H.