Program Listing for File CompoundSubhinge.h#
↰ Return to documentation for file (include/Karana/SOADyn/CompoundSubhinge.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 CompoundSubhinge class.
*/
#pragma once
#include <map>
#include <unordered_map>
#include <utility>
#include "Karana/KCore/LockingBase.h"
#include "Karana/Math/Defs.h"
#include "Karana/Math/SpatialVector.h"
#include "Karana/SOADyn/SubhingeBase.h"
namespace Karana::Dynamics {
namespace kc = Karana::Core;
namespace km = Karana::Math;
class PhysicalHinge;
class CompoundHinge;
class CompoundBody;
class PhysicalBody;
class CompoundSubhinge;
/**
* @class CompoundSubhingeVars
* @brief The Vars for the CompoundSubhinge class.
*/
class CompoundSubhingeVars : public Karana::Core::LockingBaseVars {
public:
/**
* @brief CompoundSubhingeVars constructor. The constructor is not meant to be
* called directly. Please use the create(...) method instead to create
* an instance.
*
* @param sh The CompoundSubhinge associated with this CompoundSubhingeVars.
*/
CompoundSubhingeVars(const kc::ks_ptr<const CompoundSubhinge> &sh);
/**
* @brief Destructor.
*/
~CompoundSubhingeVars();
/**
* @brief Create an instance of the CompoundSubhingeVars.
*
* @param sh The subhinge associated with this CompoundSubhingeVars.
* @returns A pointer to the newly created instance of CompoundSubhingeVars.
*/
static kc::ks_ptr<CompoundSubhingeVars>
create(const kc::ks_ptr<const CompoundSubhinge> &sh);
/**
* @brief Get all the Vars that this VarHolder has.
*
* @returns A map of Vars, where the Var name is the key and the Var is the value.
*/
kc::NestedVars getAllVars() const override;
/// The subhinge type
kc::ks_ptr<Karana::Core::Var_T<std::string>> subhinge_type;
/// The Q coordinates
kc::ks_ptr<Karana::Core::Var_T<km::Vec>> Q;
/// The U coordinates
kc::ks_ptr<Karana::Core::Var_T<km::Vec>> U;
/// The Qdot coordinates
kc::ks_ptr<Karana::Core::Var_T<km::Vec>> Qdot; // NOLINT(readability-identifier-naming)
/// The Udot coordinates
kc::ks_ptr<Karana::Core::Var_T<km::Vec>> Udot; // NOLINT(readability-identifier-naming)
/// The T coordinates
kc::ks_ptr<Karana::Core::Var_T<km::Vec>> T;
};
/**
* @class CompoundSubhinge
* @brief Represents the articulation subhinge class for CompoundBody subhinges
*/
class CompoundSubhinge : public Karana::Core::LockingBase, public SubhingeBase {
/* for access to the _computeATBI* methods */
friend class CompoundBody;
friend class CECompoundBody;
/* for access to the _computeVrootPnode* methods */
friend class PhysicalBody;
friend class PhysicalModalBody;
// For access to protected members for vars
friend class CompoundSubhingeVars;
public:
/**
* @brief CompoundSubhinge destructor.
*/
virtual ~CompoundSubhinge();
std::string_view name() const override { return kc::LockingBase::name(); }
const kc::id_t &id() const override { return kc::LockingBase::id(); }
std::string_view typeString(bool brief = true) const noexcept override {
return kc::Base::typeString(brief);
}
SubhingeType subhingeType() const override;
kc::ks_ptr<kf::FrameToFrame> f2f() const override;
std::string dumpString(std::string_view prefix,
const Karana::Core::Base::DumpOptions *options) const override;
bool isReady() const override;
void setQ(const Eigen::Ref<const km::Vec> &val) override;
void setQ(double fill_value) override;
const km::Vec &getQ() const override;
void setU(const Eigen::Ref<const km::Vec> &val) override;
void setU(double fill_value) override;
const km::Vec &getU() const override;
void setT(const Eigen::Ref<const km::Vec> &val) override;
void setT(double fill_value) override;
const km::Vec &getT() const override;
void accumT(const Eigen::Ref<const km::Vec> &val) override;
void accumT(double fill_value) override;
void setUdot(const Eigen::Ref<const km::Vec> &val) override;
void setUdot(double fill_value) override;
const km::Vec &getUdot() const override;
const km::Vec &getQdot() const override;
/**
* @brief CompoundSubhinge constructor. The constructor is not meant to be
* called directly. Please use the create(...) method instead to create
* an instance.
*
* @param hge the parent hinge instance
*/
CompoundSubhinge(const kc::ks_ptr<CompoundHinge> &hge);
/**
* @brief Create a new instance of CompoundSubhinge.
*
* @param hge the parent hinge instance
* @returns A new instance of CompoundSubhinge.
*/
static Karana::Core::ks_ptr<CompoundSubhinge> create(const kc::ks_ptr<CompoundHinge> &hge);
protected:
Karana::Core::ks_ptr<Karana::Core::BaseVars> _getVars() const override;
std::optional<bool> isOriented(const kf::FrameToFrame &f2f) const override;
/* Get the full Hst matrix for the compound subhinge. There is
one row and one column for each embedded body. Each row is of
size (bd->nU()+6) with modal followed by 6 rows for each
embedded body. Columns however are grouped by all subhinge
articulation coords first, followed by all flex coords
(matching treeCoordData ordering). Thus in column space, the
coords for a body are split up with subhinge coord columns
going in the first block, and the flex ones boing into the
second block.
The first sh_coord_data->nU() columns contain the subhinge
coord data jacobian contributions, while the remaining
bd_coord_data->nU() columns contain the body deformation
jacobian contributions.
*/
const km::Mat _atbiCoordMapMatrix() const override;
void _localChartSetQ(const Eigen::Ref<const km::Vec> &val) override;
const km::Vec &_localChartGetQ() const override;
/** @brief Overall struct for ATBI dynamics matrix quantities
for compound body subhinge which includes the embedded body
subhinge and flex dof matrices */
struct ATBIMatrices : public CoordBase::ATBIMatrices {
/** The body_nU square block diagonal matrix with 6x6 ATBI P
* matrix element contributions from each of the aggregated
* physical bodies about their pnodes. Each body's
* contribution is expressed in the body pnode's deformed
* translational frame.
*
*/
km::Mat P; // (note: body_nU = 6 * n_bodies + flex_nU)
/** The body_nU square Pplus block diagonal matrix with 6x6
matrix elements after crossing the compound
subhinge. The block matrix entries are about the pnode
deformed translational frame for each of the embedded
physical bodies. Note that this matrix is *NOT* block
diagonal */
km::Mat Pplus; // NOLINT(readability-identifier-naming)
/** HP = H*P (full_nU x body_nU)
*
* For a subhinge the lhs is in the pframe, while it in the
* pnode deformed translational frame for a flex body.
*
*/
km::Mat HP; // NOLINT(readability-identifier-naming)
/** D = H*P*HT (full_nU x full_nU)
*
*/
km::Mat D;
/** Dinv = inverse(D) (full_nU x full_nU)
*
*/
km::Mat Dinv; // NOLINT(readability-identifier-naming)
/** G = P*H*Dinv (body_nU x full_nU)
*
* For a subhinge the lhs is in the pframe, while it is in
* the pnode deformed translational frame for a flex body.
*
*/
km::Mat G;
/** tauper = I - GH (body_nU x body_nU)
*
* For a subhinge the lhs is in the pframe, while it in the
* pnode deformed translational frame for a flex body.
*
*/
km::Mat tauper;
/** psi = phi * tauper (body_nU x body_nU)
*
* For a subhinge, the left is in the oframe, right in the
* pframe. For a flex body, the left is in the pnode frame
* and the right in the body frame.
*
*/
km::Mat psi;
/** @brief Resize the ATBI matrix elements
@param P_nu is (6*nbodies+flex_nu) where nbodies is the number
of aggregated physical bodies.
@param D_nu is the overall number of generalized velocities from all the
aggregated bodies (subhinges+flex coords)
*/
void resize(size_t P_nu, size_t D_nu) { // NOLINT(readability-identifier-naming)
// size_t body_nU = 6 * n_bodies;
P.resize(P_nu, P_nu);
P.setZero();
Pplus.resize(P_nu, P_nu);
HP.resize(P_nu, D_nu);
D.resize(D_nu, D_nu);
Dinv.resize(D_nu, D_nu);
G.resize(P_nu, D_nu);
tauper.resize(P_nu, P_nu);
psi.resize(P_nu, P_nu);
km::makeNotReady(HP);
km::makeNotReady(D);
km::makeNotReady(Dinv);
km::makeNotReady(G);
km::makeNotReady(tauper);
km::makeNotReady(psi);
}
};
/** @brief Overall struct for ATBI dynamics filter quantities
for compound body subhinge which includes the embedded body
subhinge and flex dof vectors */
struct ATBIFilterVectors : public CoordBase::ATBIFilterVectors {
public:
/** the (6 bd + nU) size z ATBI for the subhinge with
entries for each embedded body. Each (nd->nU() + 6)
embedded body entry contains the modal part, and the
rigid body part (include aprime contribution) is in
the pnode deformed translational frame. */
km::Vec z;
/** the (6 bd + nU) size size zplus vector after crossing the
compound subhinge. Each (nd->nU() + 6) embedded body entry
contains the modal part, and the rigid body part is in the
pnode deformed translational frame. */
km::Vec zplus;
/** Residual hinge force (full_nU)
*
* eps = T - H * (zR + PR*a)
*
*/
km::Vec epsilon;
/** Residual hinge acceleration (full_nU)
*
* nu = Dinv * eps
*
*/
km::Vec nu;
/**
* @brief Resize the ATBI matrix elements
*
* @param P_nu is (6*nbodies+flex_nu) where nbodies is the number
* of aggregated physical bodies.
* @param D_nu is the overall number of generalized velocities from all the
* aggregated bodies (subhinges+flex coords)
*/
void resize(size_t P_nu, size_t D_nu) { // NOLINT(readability-identifier-naming)
// size_t body_nU = 6 * n_bodies;
z.resize(P_nu);
zplus.resize(P_nu);
epsilon.resize(D_nu);
nu.resize(D_nu);
km::makeNotReady(epsilon);
km::makeNotReady(nu);
}
};
protected:
virtual km::Mat _getUpsilonMatrix() override { return km::Mat(); };
/** @brief Update and return the aprime Coriolis accel for an embedded
physical body in the pnode frame
@param bd the embedded body
@return the a' value for the embedded body
*/
km::SpatialAcceleration _aprimeAccel(kc::ks_ptr<PhysicalBody> bd) const;
km::Mat _jacobian(const kf::FrameToFrame &f2f) const override;
km::Mat _jacobianDot(const kf::FrameToFrame &f2f) const override;
/** @brief Compute A(p, G) = E_G A_G in Section 18.3.1.
This is a (6+vroot_body_nU)x(6nbodies) size row matrix with
\f$\phi(vroot_pnode, pnode)\f$ (6+vroot_body_nU)x6 matrix
elements for the \f$\phi\f$ matrix from the physical
virtual root body's pnode deformed translational frame to
the deformed translational frame for the pnode of each
embedded physical body.
@return the computed E_Phi_G matrix
*/
const km::Mat &_getE_Phi_G() const { return _matE_Phi_G_cache->get(); }
/** @brief Callback to compute the "full" \f$\underbar H_{G}^* =
\phi^* H_G\f$ coord map matrix for the compound subhinge as
defined in Eq 18.4 that includes both the subhinge and flex
dofs for the embedded bodies.
The size of the matrix is (6nbodies + bd_nU) x (bd_nU +
sh_nU), where bd_nU denotes the overall deformation dofs for
the embedded bodies, and sh_nU the overall subhinge
generalized velocities for the embedded subhinges. This is
in fact the Jacobian from the aggregated subhinges and modal
coords to the pnode deformed translational frames (and *not*
the pnode itself) and modal coords of the aggregated bodies.
There is one block row per embedded body, with the size of
the row being the number of body deformation coords plus 6,
with the modal etadot coords coming first followed by the 6
rigid body spatial velocity elements. However, the columns
are organized in the treeCoordData coords order - so all
sh_nU subhinge coordinates come first, followed by all the
bd_nU modal coordinates coming after.
@param val the buffer for the data
*/
void _computeFullCoordMapMatrix(km::Mat &val) const;
/**
* @brief Discard the provided CompoundSubhinge.
* @param base - Base pointer to the CompoundSubhinge to discard.
*/
void _discard(kc::ks_ptr<kc::Base> &base) override;
/** @brief Cache callback to compute the ATBI P matrix at the physical
parent body's pnode.
This computes a (6+vroot_body_nU) size square matrix
@return the computed P matrix for the vroot body
*/
km::Mat _getVrootPnodeATBIMatrixP();
/** @brief Method to compute the ATBI z at the physical physical parent
body (pnode for rigid parent, body frame for flex parent)
This computes a (6+vroot_body_nU) size vector
@return the computed z vector for the vroot body
*/
km::Vec _getVrootPnodeATBIFilterZ();
/** @brief Method to compute the inverse dynamics
(6+vroot_body_nU) modal+rigid force vector contribution at
the physical parent's pnode (for rigid part) from this
compound body.
@return the modal+rigid force force vector for the vroot body
*/
km::Vec _getVrootPnodeInvDynF();
// km::SpatialVector _getVrootPnodeInvDynF();
/** Callback to update the ATBI matrices data cache */
void _computeATBIMatrices();
/** Callback to update the ATBI filter data cache */
void _computeATBIFilterVectors();
/** Callback to update the ATBI smoother data cache */
void _computeATBISmootherVectors();
/** Callback to update the ATBI OSI data cache */
void _computeUpsilonMatrices();
/** @brief Method to update the a' Coriolis spatial acceleration for an
embedded body
@param bd the embedded body
@param prev_bd the parent of the embedded body
@return the Coriolis spatial acceleration vector
*/
km::SpatialAcceleration _computeAprimeAccel(kc::ks_ptr<PhysicalBody> bd,
kc::ks_ptr<PhysicalBody> prev_bd) const;
km::Mat _getATBIMatPsi() const override { return km::Mat(); };
km::Mat _getATBID() const override;
km::Mat _getATBIDinv() const override;
km::Mat _getATBIG() const override { return km::Mat(); };
km::Mat _getATBITauper() const override { return km::Mat(); };
protected:
/** @brief Return the XdotU vector for the specific embedded
physical hinge.
This method returns a null vector for
regular compound bodies. Its return value is non-null
however for constraint-embedded compound bodies whose X
matrix is configuration dependent.
@param hge the hinge
@return the a' spatial acceleration value
*/
virtual km::Vec _getHingeXdotU(PhysicalHinge &hge) const;
/** Compute the subhinge's generalized force value for inverse
dynamics */
void _computeInvDynGenForce();
/** the compound body associated with this compound subhinge */
kc::ks_ptr<CompoundBody> _parent_body = nullptr;
/** @brief Callback method to compute the
(6+vroot_body_nU)x(6*nbodies) size E_Ph_G matrix for the
compound body
@param val the data buffer for the computed data
*/
void _computeE_Phi_G(km::Mat &val);
km::Mat _oframe2pframePsi() const override;
km::Mat _oframe2pframePhi() const override;
km::Mat _pframe2otherPhi(const kf::Frame &other) const override;
protected:
/** map with orientation values for every <body, coord element>
pair for the bodies and physical coord data physical elements
in the compound body. Thus, for every <body, coord element>
pair in the map, the true/false value indicates that the
coord element is oriented/opposed in the virtual root/pnode
path. A missing entry indicates that the coord element is not
in the path from the virtual root to the pnode, and hence does
not contribute to its motion.
This map is used in assembling Jacobian from coord elements to
frames in the multibody system.
*/
// std::map<std::pair<kc::id_t, kc::id_t>, bool> _body_pnodes_orientation_map;
/** Data caches for evaluating the "aprime" Coriolis accel values
in the ATBI forward dynamics algorithm for all the aggregated
physical bodies. Each cache returns the a' Coriolis accel
value for the pnode of a physical body. */
std::unordered_map<kc::id_t, kc::ks_ptr<kc::DataCache<km::SpatialAcceleration>>>
_aprime_caches;
/** Data cache for the nbodies size row matrix with
\f$\phi(vroot_pnode, pnode)\f$ 6x6 matrix elements for the
\f$\phi\f$ matrix from the physical virtual root body's pnode
to the pnode of each aggregated physical body. This is
denoted \f$A(p, G) = E_G A_G\f$ in Section 18.3.1. */
kc::ks_ptr<kc::DataCache<km::Mat>> _matE_Phi_G_cache = nullptr;
/** the (6*nbodies)x(sh_nU) joint map matrix data cache for ATBI
computations for the subhinge. */
// kc::ks_ptr<kc::DataCache<km::Mat>> _atbi_coord_map_matrix_cache = nullptr;
/** the (6*nbodies+flex_nU)x(sh_nU+flex_nU) joint map matrix data cache
for computations for the subhinge. */
kc::ks_ptr<kc::DataCache<km::Mat>> _full_coord_map_matrix_cache = nullptr;
protected:
/*
* We want to return const km::Vec& for the
* getter methods. However, we need to use CoordData
* for CompoundSubhinge. Therefore, we add these as mutable
* variables (since they need to change in the const functions)
* and return references to them.
*/
/// Vector for storing getQ
mutable km::Vec _getQ_cache;
/// Vector for storing getQdot
mutable km::Vec _getQdot_cache;
/// Vector for storing getU
mutable km::Vec _getU_cache;
/// Vector for storing getUdot
mutable km::Vec _getUdot_cache;
/// Vector for storing getT
mutable km::Vec _getT_cache;
private:
// const km::Mat &_atbiCoordMapMatrixLocal() const;
};
} // namespace Karana::Dynamics