Program Listing for File HingeNode.h#
↰ Return to documentation for file (include/Karana/SOADyn/HingeNode.h)
/*
* Copyright (c) 2024-2026 Karana Dynamics Pty Ltd. All rights reserved.
*
* NOTICE TO USER:
*
* This source code and/or documentation (the "Licensed Materials") is
* the confidential and proprietary information of Karana Dynamics Inc.
* Use of these Licensed Materials is governed by the terms and conditions
* of a separate software license agreement between Karana Dynamics and the
* Licensee ("License Agreement"). Unless expressly permitted under that
* agreement, any reproduction, modification, distribution, or disclosure
* of the Licensed Materials, in whole or in part, to any third party
* without the prior written consent of Karana Dynamics is strictly prohibited.
*
* THE LICENSED MATERIALS ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND.
* KARANA DYNAMICS DISCLAIMS ALL WARRANTIES, EXPRESS OR IMPLIED, INCLUDING
* BUT NOT LIMITED TO WARRANTIES OF MERCHANTABILITY, NON-INFRINGEMENT, AND
* FITNESS FOR A PARTICULAR PURPOSE.
*
* IN NO EVENT SHALL KARANA DYNAMICS BE LIABLE FOR ANY DAMAGES WHATSOEVER,
* INCLUDING BUT NOT LIMITED TO LOSS OF PROFITS, DATA, OR USE, EVEN IF
* ADVISED OF THE POSSIBILITY OF SUCH DAMAGES, WHETHER IN CONTRACT, TORT,
* OR OTHERWISE ARISING OUT OF OR IN CONNECTION WITH THE LICENSED MATERIALS.
*
* U.S. Government End Users: The Licensed Materials are a "commercial item"
* as defined at 48 C.F.R. 2.101, and are provided to the U.S. Government
* only as a commercial end item under the terms of this license.
*
* Any use of the Licensed Materials in individual or commercial software must
* include, in the user documentation and internal source code comments,
* this Notice, Disclaimer, and U.S. Government Use Provision.
*/
/**
* @file
* @brief Contains the declarations for the HingeNode, HingeOnode, and HingePnode classes.
*/
#pragma once
#include "Karana/KCore/DataCache.h"
#include "Karana/Math/SpatialVector.h"
#include "Karana/SOADyn/Node.h"
namespace Karana::Frame {
class FrameToFrame;
}
namespace Karana::Dynamics {
namespace kf = Karana::Frame;
namespace km = Karana::Math;
class PhysicalHinge;
class PhysicalSubhinge;
class HingeOnode;
/** @brief Struct for inverse dynamics vector quantities for a onode */
struct HingeOnodeInvDynVectors {
/** the inter-body spatial interaction force for the hinge at
* its onode and represented in the onode frame as computed by
* the inverse dynamics process.
*
*/
km::SpatialVector f;
};
/** @brief Struct for ATBI dynamics matrix quantities for a rigid body */
struct HingeOnodeATBIMatrices {
/** Rigid body 6x6 ATBI Pplus matrix for the child body about
* the onode location and represented in the onode frame. */
km::Mat66 Pplus; // NOLINT(readability-identifier-naming)
};
/** @brief Struct for onode Upsilon matrix quantities at the onode */
struct HingeOnodeUpsilonMatrices {
/** Rigid body OSI UpsilonPlus matrix for the child body about and
in the onode frame */
km::Mat66 UpsilonPlus; // NOLINT(readability-identifier-naming)
};
/** @brief Struct for ATBI dynamics filter vector quantities at the onode */
struct HingeOnodeATBIFilterVectors {
/** Rigid 6 vector of zplus for the body about the onode location
* and represented in the onode frame. */
km::SpatialVector zplus;
/** Extra zplus that might come from the TA closed loop dynamics
free dynamics pass that is needed for inter-body constraint
computations with this algorithm */
km::SpatialVector zplus_extra;
};
/** @brief Struct for ATBI dynamics smoother vector quantities at the onode */
struct HingeOnodeATBISmootherVectors {
/** alpha+ accel vector at the onode and represented in the
onode frame. Need this for inter-body force computations as
well. */
km::SpatialVector alpha_plus;
/** Extra alpha_plus that might come from the TA closed loop dynamics
free dynamics pass that is needed for inter-body constraint
computations with this algorithm */
km::SpatialVector alpha_plus_extra;
};
/** @brief the ATBI data caches for a onode */
struct HingeOnodeATBIDataCaches {
/** data cache for ATBI matrices */
kc::ks_ptr<kc::DataCache<HingeOnodeATBIMatrices>> matrix_cache = nullptr;
/** data cache for ATBI filter vectors */
kc::ks_ptr<kc::DataCache<HingeOnodeATBIFilterVectors>> filter_cache = nullptr;
/** data cache for ATBI smoother vectors */
kc::ks_ptr<kc::DataCache<HingeOnodeATBISmootherVectors>> smoother_cache = nullptr;
/** data cache for ATBI based inter-body force computations */
kc::ks_ptr<kc::DataCache<km::SpatialVector>> tree_interbody_force_cache = nullptr;
/** data cache for Upsilon matrices */
kc::ks_ptr<kc::DataCache<HingeOnodeUpsilonMatrices>> upsilon_matrix_cache = nullptr;
};
/**
* @class HingeNode
* @brief Represents the base class for physical HingePnode and HingeOnode classes
*/
class HingeNode : public Node {
/* for access to _hinge */
friend class PhysicalBody;
public:
/**
* @brief HingeNode destructor.
*/
virtual ~HingeNode();
/**
* @brief Return the PhysicalHinge hinge the node is attached to
*
* @return The attached PhysicalHinge instance
*/
const kc::ks_ptr<PhysicalHinge> &hinge() const { return _hinge; }
/** @brief Struct to manage the options for tailoring the output from the
dumpString() method. */
struct DumpOptions : kf::Frame::DumpOptions {
/**
* @brief Assignment operator.
*
* @param p The DumpOptions to copy values from.
* @return A reference to the newly assigned DumpOptions.
*/
// Adding suppression, as CodeChecker complains this method has the same name as a
// method in the base version. This is just a CodeChecker false positive.
// codechecker_suppress [cppcheck-duplInheritedMember]
DumpOptions &operator=(const DumpOptions &p) {
if (this != &p) {
Frame::DumpOptions::operator=(p); // Call base class assignment operator
// Assign Derived-specific members here
cache_deps = p.cache_deps;
}
return *this;
}
};
std::string
dumpString(std::string_view prefix = "",
const Karana::Core::Base::DumpOptions *options = nullptr) const override;
protected:
/**
* @brief HingeNode constructor
*
* @param name - Name of the hinge node
* @param bd - The PhysicalBody to attach this HingeNode to
*/
HingeNode(std::string_view name, kc::ks_ptr<PhysicalBody> bd);
/** the hinge attached to this onode */
kc::ks_ptr<PhysicalHinge> _hinge = nullptr;
};
/**
* @class HingePnode
* @brief Represents the hinge pnode class
*
* See \sref{body_hinge_sec} section for more discussion of hinge,
* onodes and pnodes.
*
* This class is for the pnodes for physical hinges
*/
class HingePnode : public HingeNode {
// for access to _pnode2body_f2f
friend class PhysicalBody;
// for access to _invdyn_cache
friend class PhysicalHinge;
friend class HingeOnode;
friend class CompoundBody;
// for access to _computeInvDyn etc
friend class Multibody;
friend class SubTree;
friend class PhysicalModalBody;
// for access to gyro cache
friend class CompoundSubhinge;
// for access to invdyn and other data caches
friend class Node;
// for access to _hinge
friend class LoopConstraint;
// for access to Upsilon method
friend class PhysicalSubhinge;
// for access to _externalForces
friend class Algorithms;
public:
/**
* @brief Constructor
* @param bd the parent body
*/
HingePnode(kc::ks_ptr<PhysicalBody> bd);
/**
* @brief HingePnode destructor.
*/
virtual ~HingePnode();
bool isReady() const override;
public:
/** @brief Struct for inverse dynamics vector quantities for a HingePnode */
struct InvDynVectors {
/** the inter-body spatial interaction force for the body about
* the pnode location and represented in the body frame. */
km::SpatialVector f;
};
public:
/** @brief Struct for ATBI dynamics matrix quantities for a HingePnode */
struct ATBIMatrices {
/** Rigid body 6x6 ATBI P matrix for the body about the pnode
* location and represented in the pnode frame. */
km::Mat66 P; // NOLINT(readability-identifier-naming)
};
/** @brief Struct for onode OSI matrix quantities at the HingePnode */
struct UpsilonMatrices {
/** OSI Upsilon matrix for the child body about and
in the pnode frame */
km::Mat66 Upsilon; // NOLINT(readability-identifier-naming)
};
/** @brief Struct for ATBI dynamics filter vector quantities for a HingePnode */
struct ATBIFilterVectors {
/** Rigid 6 vector of z for the body about the pnode location
* and represented in the pnode frame. */
km::SpatialVector z;
};
/** @brief Struct for ATBI dynamics smoother vector quantities for a HingePnode */
struct ATBISmootherVectors {};
/** @brief Overall set of ATBI data caches for the HingePnode */
struct ATBIDataCaches {
/** data cache for ATBI matrices */
kc::ks_ptr<kc::DataCache<ATBIMatrices>> matrix_cache = nullptr;
/** data cache for ATBI filter vectors */
kc::ks_ptr<kc::DataCache<ATBIFilterVectors>> filter_cache = nullptr;
/** data cache for ATBI smoother vectors */
kc::ks_ptr<kc::DataCache<ATBISmootherVectors>> smoother_cache = nullptr;
/** data cache for Upsilon matrices */
kc::ks_ptr<kc::DataCache<UpsilonMatrices>> upsilon_matrix_cache = nullptr;
};
/**
* @brief Return the ATBI matrix cache
*
* @return the ATBI matrix cache
*/
const kc::ks_ptr<kc::DataCache<ATBIMatrices>> atbiMatrixCache() const {
return _atbi_data_caches.matrix_cache;
}
/**
* @brief Update and return the ATBI matrices
*
* @return the ATBI matrices
*/
const ATBIMatrices &atbiMatrices() const { return atbiMatrixCache()->get(); }
/**
* @brief Return the OSCM Upsilon matrix cache
*
* @return the OSCM Upsilon matrix cache
*/
const kc::ks_ptr<kc::DataCache<UpsilonMatrices>> upsilonMatrixCache() const {
return _atbi_data_caches.upsilon_matrix_cache;
}
/**
* @brief Update and return the OSCM Upsilon matrices
*
* @return the OSCM Upsilon matrices
*/
const UpsilonMatrices &upsilonMatrices() const { return upsilonMatrixCache()->get(); }
/**
* @brief Return the ATBI filter cache
*
* @return the ATBI filter cache
*/
const kc::ks_ptr<kc::DataCache<ATBIFilterVectors>> atbiFilterCache() const {
return _atbi_data_caches.filter_cache;
}
/**
* @brief Update and return the ATBI filter vectors
*
* @return the ATBI filter vectors
*/
const ATBIFilterVectors &atbiFilterVectors() const { return atbiFilterCache()->get(); }
/**
* @brief Return the ATBI smoother cache
*
* @return the ATBI smoother cache
*/
const kc::ks_ptr<kc::DataCache<ATBISmootherVectors>> &atbiSmootherCache() const {
return _atbi_data_caches.smoother_cache;
}
/**
* @brief Update and return the ATBI smoother vectors
*
* @return the ATBI smoother vectors
*/
const ATBISmootherVectors &atbiSmootherVectors() const {
return atbiSmootherCache()->get();
}
/**
* @brief Return the inverse dynamics cache
*
* @return the inverse dynamics cache
*/
const kc::ks_ptr<kc::DataCache<InvDynVectors>> inverseDynamicsCache() const {
return _invdyn_data_cache;
}
/**
* @brief Update and return the inverse dynamics vectors
*
* @return the inverse dynamics vectors
*/
const InvDynVectors &inverseDynamicsVectors() const {
return inverseDynamicsCache()->get();
}
/**
* @brief Update and return the CRB spatial inertia for the body referenced to the pnode.
*
* This is a (nmodes+6) size square, symmetric, positive
semi-definite matrix
*
* @return the CRB inertia matrix.
*/
const km::Mat &crbInertiaMatrix() const { return _crb_inertia_matrix_cache->get(); }
public:
std::string dumpString(std::string_view prefix,
const Karana::Core::Base::DumpOptions *options) const override;
protected:
/**
* @brief Helper method to set up the pnode's data caches.
*/
void _oneTimeSetupDataCaches();
/**
* @brief Helper method to tear down the pnode's data caches.
*/
void _oneTimeTeardownDataCaches();
/**
* @brief Helper method to tear down the pnode.
*/
void _oneTimeTeardown();
/**
* @brief Helper method to compute the overall external spatial force on body transformed to
the pnode.
* @return the overall external spatial force
*/
km::SpatialVector _externalForces() const;
/**
* @brief Helper method to compute the overall constraint spatial force on the body
transformed to the pnode.
* @return the overall external spatial force
*/
km::SpatialVector _constraintForces() const;
/** @brief Return true if the pnode is connected to a full 6-dof hinge,
false otherwise.
This is used in flex body dynamics to define
whether or not the pnode is a material point on the body and
can thus deform with the body.
@return true if the body hinge is a 6 dof hinge
*/
bool _isFloatingFrame() const;
protected:
/** @brief Data cache callback to update the pnode referenced spatial
inertia for the body
@param val the data buffer for the computed value
*/
void _computeMassMatrix(km::Mat66 &val);
/** Data cache callback for updating the body's gyroscopic
* spatial force. This gyroscopic term corresponds to rigid body
* equations of motion where the body frame derivatives are used
* for generalized accel, i.e. the accel corresponds to
* pframeObservedRelSpAccel(). The returned spatial force is about
* and at the pnode frame.
*/
virtual void _computeGyroscopicForce(km::SpatialVector &) const;
/** @brief Data cache callback to update the overall inverse dynamics
* forces values for this body.
*
* The returned spatial force is
* at the pnode frame.
*
* @param val the data buffer for the computed value
*/
virtual void _computeInvDynForces(InvDynVectors &val);
/** @brief Data cache callback to update the ATBI matrix values.
*
* @param val the data buffer for the computed value
*/
virtual void _computeATBIMatrices(ATBIMatrices &val);
/** @brief Data cache callback to update the ATBI filter values.
*
* @param val the data buffer for the computed value
*/
void _computeATBIFilterVectors(ATBIFilterVectors &val);
/** @brief Data cache callback to update the ATBI smoother values.
*
* @param val the data buffer for the computed value
*/
void _computeATBISmootherVectors(ATBISmootherVectors &val);
/** @brief Data cache callback to update the OSI Upsilon values.
*
* @param val the data buffer for the computed value
*/
virtual void _computeUpsilonMatrices(UpsilonMatrices &val);
/** @brief Data cache callback to update the CRB inertia
* values.
*
* @param val the data buffer for the computed value
*/
void _computeCRBInertiaMatrix(km::Mat &val);
protected:
/** The cached oriented f2f from the pnode to the body frame */
kc::ks_ptr<kf::FrameToFrame> _pnode2body_f2f;
/** Data cache for the gyroscopic forces */
kc::ks_ptr<kc::DataCache<km::SpatialVector>> _gyroscopic_force_cache;
/// data cache for body spatial inertia transformed to the pnode
kc::ks_ptr<kc::DataCache<km::Mat66>> _massmat_cache;
/** ATBI data caches */
ATBIDataCaches _atbi_data_caches;
/** Inverse dynamics data cache */
kc::ks_ptr<kc::DataCache<InvDynVectors>> _invdyn_data_cache = nullptr;
/** the parent body's apparent mass data transformed to the pnode */
km::Mat66 _apparent_mass_data;
/** CRB inertia data cache */
kc::ks_ptr<kc::DataCache<km::Mat>> _crb_inertia_matrix_cache = nullptr;
};
/**
* @class HingeOnode
* @brief Represents the hinge onode class
*
* See \sref{body_hinge_sec} section for more discussion of hinge,
* onodes and pnodes.
*
* This class is for onodes for physical hinges
*/
class HingeOnode : public HingeNode {
// for access to _pnode2onode_f2f
friend class HingePnode;
friend class PhysicalBody;
// for access to _body2onode_f2f
friend class PhysicalSubhinge;
// for access to _P_atbi
friend class PhysicalHinge;
// for access to _computeInvDyn etc
friend class Multibody;
friend class SubTree;
friend class PhysicalModalBody;
// for access to data cache setup method
friend class CompoundBody;
// for access to _saveExtraATBIVariables()
friend class Algorithms;
public:
/**
* @brief HingeOnode constructor.
* @param name the onode name
* @param bd the parent body
*/
HingeOnode(std::string_view name, kc::ks_ptr<PhysicalBody> bd);
/**
* @brief HingeOnode destructor.
*/
virtual ~HingeOnode();
std::string dumpString(std::string_view prefix,
const Karana::Core::Base::DumpOptions *options) const override;
bool isReady() const override;
/**
* @brief Return the ATBI matrix cache
*
* @return the ATBI matrix cache
*/
const kc::ks_ptr<kc::DataCache<HingeOnodeATBIMatrices>> &atbiMatrixCache() const {
return _atbi_data_caches.matrix_cache;
}
/**
* @brief Update and return the ATBI matrices
*
* @return the ATBI matrices
*/
const HingeOnodeATBIMatrices &atbiMatrices() const { return atbiMatrixCache()->get(); }
/**
* @brief Return the ATBI filter cache
*
* @return the ATBI filter cache
*/
const kc::ks_ptr<kc::DataCache<HingeOnodeATBIFilterVectors>> &atbiFilterCache() const {
return _atbi_data_caches.filter_cache;
}
/**
* @brief Update and return the ATBI filter vectors
*
* @return the ATBI filter vectors
*/
const HingeOnodeATBIFilterVectors &atbiFilterVectors() const {
return atbiFilterCache()->get();
}
/**
* @brief Return the ATBI smoother cache
*
* @return the ATBI smoother cache
*/
const kc::ks_ptr<kc::DataCache<HingeOnodeATBISmootherVectors>> atbiSmootherCache() const {
return _atbi_data_caches.smoother_cache;
}
/**
* @brief Update and return the ATBI smoother vectors
*
* @return the ATBI smoother vectors
*/
const HingeOnodeATBISmootherVectors &atbiSmootherVectors() const {
return atbiSmootherCache()->get();
}
/**
* @brief Return the inter-body spatial force at the hinge
*
* While the spatial force is at the onode, and in the onode
* frame, its sign is that for the version applying on the
* outboard (the pnode) body. This values should be negated to
* get the equal and opposite spatial force on the onode's body.
*
* @return the inter-body spatial force
*/
const km::SpatialVector &getInterBodyForceTreeFwdDyn() const {
return _atbi_data_caches.tree_interbody_force_cache->get();
}
/**
* @brief Return the OSCM Upsilon matrix cache
*
* @return the OSCM Upsilon matrix cache
*/
const kc::ks_ptr<kc::DataCache<HingeOnodeUpsilonMatrices>> &upsilonMatrixCache() const {
return _atbi_data_caches.upsilon_matrix_cache;
}
/**
* @brief Update and return the OSCM Upsilon matrices
*
* @return the OSCM Upsilon matrices
*/
const HingeOnodeUpsilonMatrices &upsilonMatrices() const {
return upsilonMatrixCache()->get();
}
/**
* @brief Return the inverse dynamics cache
*
* @return the inverse dynamics cache
*/
const kc::ks_ptr<kc::DataCache<HingeOnodeInvDynVectors>> inverseDynamicsCache() const {
return _invdyn_data_cache;
}
/**
* @brief Update and return the inverse dynamics vectors
*
* @return the inverse dynamics vectors
*/
const HingeOnodeInvDynVectors &inverseDynamicsVectors() const {
return inverseDynamicsCache()->get();
}
/**
* @brief Return the interbody force at the onode for Tree Augmented (TA) dynamics with
* constraints
*
* This method returns the interbody force at the onode when
* doing TA dynamics for the system in the presence of loop
* constraints. This is only meant to be used for regular hinges
* (not cut-joint hinges)
*
* @return the inter-body spatial force
*/
km::SpatialVector getInterBodyForceTAFwdDyn();
protected:
/**
* Helper method in TA dynamics with loop constraints to save the
* zrplus and alpha_plus values from the "free" dynamics solution
* for possible use in interbody force computations where we need
* to combine these values with those from the correction
* step.
*/
void _saveExtraATBIVariables();
/** @brief Data cache callback to update the overall inverse dynamics
* forces.
*
* @param val the data buffer for the computed value
*/
void _computeInvDynForces(HingeOnodeInvDynVectors &val);
/** @brief Data cache callback to update the ATBI matrix values.
*
* @param val the data buffer for the computed value
*/
void _computeATBIMatrices(HingeOnodeATBIMatrices &val);
/** @brief Data cache callback to update the ATBI filter values.
*
* @param val the data buffer for the computed value
*/
void _computeATBIFilterVectors(HingeOnodeATBIFilterVectors &val);
/** @brief Data cache callback to update the ATBI smoother values.
*
* @param val the data buffer for the computed value
* @param is_root_onode if true, this onode is on the virtual root body
*/
void _computeATBISmootherVectors(HingeOnodeATBISmootherVectors &val, bool is_root_onode);
/** @brief Data cache callback to update the OSI Upsilon values.
*
* @param val the data buffer for the computed value
* @param is_root_onode if true, this onode is on the virtual root body
*/
virtual void _computeUpsilonMatrices(HingeOnodeUpsilonMatrices &val, bool is_root_onode);
protected:
/** @brief Compute the ATBI based inter-body force for the hinge at the
onode represented in the onode frame.
@param val the data buffer for the computed value
*/
void _computeInterBodyForceTreeFwdDyn(km::SpatialVector &val) const;
protected:
/**
* @brief Helper method to set up the onode's data caches.
*/
void _oneTimeSetupDataCaches();
/**
* @brief Helper method to tear down the onode's data caches.
*/
void _oneTimeTeardownDataCaches();
/** @brief Sets up the onode's gather cache callbacks to depend on the child
pnode's gather data caches */
void _oneTimeSetupDataCacheGatherCallbacks();
/** @brief Tears down the onode's gather cache callbacks dependency on the child
pnode's gather data caches */
void _oneTimeTeardownDataCacheGatherCallbacks();
/**
* @brief Helper method to tear down the onode.
*/
void _oneTimeTeardown();
/** @brief Called to set up a pnode's gather data cache dependency
on a onode whenever a new pnode or a new onode is created,
i.e. whenever there is a new pnode/onode pairing */
void _setupDataCachesWithParentPnode();
/** @brief Called to unset a pnode's gather data cache dependency
on a onode whenever a new pnode or a new onode is created,
i.e. whenever there is a new pnode/onode pairing */
void _teardownDataCachesWithParentPnode();
protected:
/** the oriented f2f from this onode's parent body's pnode to
the onode */
kc::ks_ptr<kf::OrientedChainedFrameToFrame> _pnode2onode_f2f = nullptr;
/** the oriented f2f from this onode's parent body's frame to
the onode */
kc::ks_ptr<kf::FrameToFrame> _body2onode_f2f = nullptr;
/** the ATBI data caches */
HingeOnodeATBIDataCaches _atbi_data_caches;
/** the inverse dynamics data cache */
kc::ks_ptr<kc::DataCache<HingeOnodeInvDynVectors>> _invdyn_data_cache = nullptr;
};
} // namespace Karana::Dynamics