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/SOADyn/SubhingeBase.h"
namespace Karana::Dynamics {
namespace kc = Karana::Core;
namespace km = Karana::Math;
class PhysicalHinge;
class CompoundHinge;
class CompoundBody;
class PhysicalBody;
/**
* @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;
/* for access to the _computeVrootPnode* methods */
friend class PhysicalBody;
friend class PhysicalModalBody;
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);
}
const km::Mat atbiCoordMapMatrix() const override;
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 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 Constructs a CompoundSubhinge
* @param hge the parent hinge instance
*/
CompoundSubhinge(kc::ks_ptr<CompoundHinge> hge);
std::optional<bool> isOriented(const kf::FrameToFrame &f2f) const override;
protected:
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's pnode
* frame. Only the rigid body P contributions are being kept
* since that is all this is needed.
*
*/
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
for each of the aggregated 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 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 lefts 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 body_nU size z ATBI spatial vector for the subhinge on the pframe
* side of the subhinge and represented in pframe */
km::Vec z;
/** the body_nU size zplus vector after crossing the subhinge about and
* expressed in the subhinge's oframe. */
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
@param bd the embedded body
@return the a' value for the embedded body
*/
km::SpatialVector _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 to the pnode of each aggregated 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 \f$\underbar H_{G}^* = \phi^*
H_G\f$ coord map matrix for the compound subhinge as defined
in Eq 18.4 that includes just the subhinge dofs for the
embedded bodies.
The size of the matrix is (6nbodies) x (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 to
the pnodes and modal coords of the aggregated bodies.
There is one block row per embedded body of size 6 for the
rigid body spatial velocity elements. The columns
are organized in the subhingeCoordData coords order.
@param val the buffer for the data
*/
void _computeATBICoordMapMatrix(km::Mat &val) const;
/* this includes flex coords on lhs and rhs */
/** @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 pnodes 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 spatial force vector at the
physical parent.
@return the computed spatial force vector for the vroot body
*/
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::SpatialVector _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::SpatialVector>>> _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 &_atbiCoordMapMatrix() const;
};
} // namespace Karana::Dynamics