Program Listing for File PhysicalSubhinge.h#
↰ Return to documentation for file (include/Karana/SOADyn/PhysicalSubhinge.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 PhysicalSubhinge and PhysicalSubhinge_T classes.
*
* The _T version is the templatized version for PhysicalSubhinges of specific sizes. Templatizing
* provides optimization benefits, as Eigen can work with known, fixed sizes.
*/
#pragma once
#include "Karana/Frame/EdgeFrameToFrame.h"
#include "Karana/SOADyn/SubhingeBase.h"
namespace Karana::Frame {
class Frame;
}
namespace Karana::Dynamics {
namespace kc = Karana::Core;
namespace kf = Karana::Frame;
class FramePairHinge;
/**
* @class PhysicalSubhinge
* @brief Represents the abstract base class for physical articulation subhinges
*
* This class is the base class for subhinges from which physical
* subhinge classes are derived. See \sref{subhinges_sec} section for
* more information on physical subhinges.
*/
class PhysicalSubhinge : public Karana::Frame::EdgeFrameToFrame, public SubhingeBase {
// for access to id()
friend class FramePairHinge;
// for access to SubhingeParams
friend class LoopConstraintCutJoint;
// for access to _computeATBIMatrices()
friend class HingeOnode;
friend class PhysicalBody;
// for access to _computeATBISmootherVectors()
friend class HingePnode;
// for access to _coriolisAccel
friend class PhysicalHinge;
// for access to PhysicalSubhingeParams
friend class OneDofSubhingeData;
// for access to _newtonian2oframe
friend class Multibody;
// for access to ATBIMatrices
friend class SubTree;
// for access to enableChart()
friend class SubGraph;
// for access to getATBIDinv()
friend class Algorithms;
friend class HingePnode;
public:
/**
* @brief PhysicalSubhinge destructor.
*/
virtual ~PhysicalSubhinge();
std::string_view typeString(bool brief = true) const noexcept override {
return kc::Base::typeString(brief);
}
std::string_view name() const override { return kf::EdgeFrameToFrame::name(); }
const kc::id_t &id() const override { return kf::EdgeFrameToFrame::id(); }
/**
* @brief Return matrix to map coord velocities to physical velocities
*
* The product of this matrix with the generalized velocities
* vector (U) evaluates to the full relative spatial velocities
* contribution from this object.
*
* For a PhysicalSubhinge, this is a 6xNU size matrix where NU is
* number of generalized velocity coordinates for the subhinge. For a
* physical subhinge, this matrix is uses oframe
* representation. The product with U returns the relative spatial
* velocity across the subhinge referenced in the oframe.
*
* For a CompoundSubhinge, this matrix is (6*nbodies)xNU in size
* where NU is the overall number of generalized velocity coordinates
* for the aggregated body subhinges. This matrix is the Jacobian from
* the embedded physical body subhinges to the embedded body relative
* spatial velocity contribution from them.
*
* For a PhysicalModalBody body, the size is 6xNU, where NU is
* the number of modal coordinates. It returns the modal matrix
* for the pnode.
*
* @return coordinate map matrix
*/
virtual const km::Mat oframeCoordMapMatrix() const = 0;
/**
* @brief Method to recenter or otherwise sanitize the subhinge's coordinates
*
* Some subhinge may have coordinates that need to be "sanitized"
* from time to time. An example if that of spherical subhinges, where
* the orientation coordinate representation may require recentering
* of the local charts when the values are nearing singularity. This
* method can specialized for subhinges that have this need. The
* return value should be true if any such sanitization was done to
* modify the coordinate values.
*
* @return true, is any sanitization was needed and carried out
*/
virtual bool sanitizeCoords() { return false; };
/**
* @brief Return true if the subhinge has coordinate sanitization needs.
*
* This method can be used to identify the subset of subhinges
* that have coordinate sanitization needs, so that
* sanitizeCoords() can be called for them when needed.
*
* @return true if the subhinge has coordinate sanitization needs.
*/
virtual bool requiresCoordSanitization() const { return false; }
bool isReady() const override;
/**
* @brief Reset any sanitization related chart offsets etc that may have been applied.
*/
virtual void resetChart() {};
/**
* @brief Set/unset the prescribed flag for the subhinge
*
* @param flag the prescribed state to set
*/
void setPrescribed(bool flag) override;
/**
* @brief Return the slot index for the subhinge in the parent hinge's list of subhinges
*
* @return the subhinge index
*/
size_t getIndex() const { return _index; }
protected:
kc::ks_ptr<kf::FrameToFrame>
f2f() const override; // NOLINT(readability-identifier-naming) TODO
/** Enable/disable the use of local charts for the
subhinge. Typically charts should be enabled when doing time
domain simulations, but disabled when doing configuration
kinematics, and linearization operations. */
virtual void _enableCoordChart(bool /*flag*/) {};
km::Mat _oframe2pframePsi() const override;
km::Mat _oframe2pframePhi() const override;
km::Mat _pframe2otherPhi(const kf::Frame &other) const override;
/** @brief Return the previous inboard subhinge to this one
@return the upstream subhinge
*/
kc::ks_ptr<PhysicalSubhinge> _getUpstreamSubhinge() const;
km::Mat getUpsilonMatrix() override;
/** @brief Base class for physical subhinges that is specialized for each
subhinge type. This is handy for creating new subhinges with
matching params from existing subhinges (e.g., when introducing
cut-joints). */
struct PhysicalSubhingeParams {
km::Vec Q; //!< The Q generalized coordinates
km::Vec U; //!< The U generalized velocity coordinates
/// The Udot generalized acceleration coordinates
km::Vec Udot; // NOLINT(readability-identifier-naming)
km::Vec T; //!< The T generalized force values
bool prescribed = false; //!< The prescribed motion setting
PhysicalSubhingeParams() = default;
/** @brief Copy constructor
@param p the other params
*/
PhysicalSubhingeParams(const PhysicalSubhinge::PhysicalSubhingeParams &p) {
Q = p.Q;
U = p.U;
Udot = p.Udot;
T = p.T;
prescribed = p.prescribed;
}
/** @brief Assignment operator
@param p the other params
@return the updated params
*/
PhysicalSubhingeParams &operator=(const PhysicalSubhinge::PhysicalSubhingeParams &p) {
if (this != &p) {
Q = p.Q;
U = p.U;
Udot = p.Udot;
T = p.T;
prescribed = p.prescribed;
}
return *this;
}
};
/** @brief Get params for the subhinge.
@return the params
*/
virtual kc::ks_ptr<PhysicalSubhingeParams> _getParams() const = 0;
/** @brief Set the coordinate params for the subhinge.
@param params the input params
*/
virtual void _fillCoordParams(PhysicalSubhingeParams ¶ms) const = 0;
/** @brief Set params for the subhinge.
@param params the params
*/
virtual void _setParams(const PhysicalSubhingeParams ¶ms) = 0;
/** @brief Set params from a reversed version of this subhinge.
@param params the params
*/
virtual void _setReversedParams(const PhysicalSubhingeParams ¶ms) = 0;
/**
* @brief Data cache callback to compute ATBI matrices
* @param P the input ATBI P matrix
* @return the updated ATBI P matrix
*/
virtual km::Mat66 _computeATBIMatrices(const km::Mat66 &P) = 0;
/**
* @brief Data cache callback to compute ATBI filter vectors
* @param z the input ATBI filter z vector
* @return the updated ATBI filter z vector
*/
virtual km::SpatialVector _computeATBIFilterVectors(const km::SpatialVector &z) = 0;
/**
* @brief Data cache callback to compute ATBI smoother vectors
* @param alpha the input spatial acceleration
* @return the output spatial acceleration
*/
virtual km::SpatialVector _computeATBISmootherVectors(const km::SpatialVector &alpha) = 0;
/** @brief Propagate the UpsilonPlus from the oframe to the pframe
@param upsilon_plus_oframe the input Upsilon matrix
@return the updated Upsilon matrix
*/
virtual km::Mat66 _computeUpsilonMatrices(const km::Mat66 &upsilon_plus_oframe) = 0;
/** @brief Data cache callback to compute the subhinge's generalized force value for inverse
dynamics
@param pnode_invdyn_f the input spatial force
*/
virtual void _computeInvDynGenForce(const km::SpatialVector &pnode_invdyn_f) = 0;
/** @brief Return the Coriolis acceleration "a" that factors into the
recursive acceleration propagation across the subhinge.
This assumes that we are combining the oframe observed
acceleration at the oframe with oframe observed subhinge
relative spatial accel to compute the pframe observed pframe
acceleration.
@return the Coriolis spatial acceleration vector
*/
km::SpatialVector _coriolisAccel() const;
/**
* @brief Constructs a PhysicalSubhinge.
* @param oframe the oframe frame
* @param pframe the pframe frame
* @param name the subhinge name
* @param hge the parent hinge
*/
PhysicalSubhinge(kc::ks_ptr<kf::Frame> oframe,
kc::ks_ptr<kf::Frame> pframe,
std::string_view name,
kc::ks_ptr<FramePairHinge> hge);
protected:
/** the f2f from this subhinge's oframe to the child body's
* pnode */
kc::ks_ptr<kf::OrientedChainedFrameToFrame> _oframe2pnode_f2f = nullptr;
/** the frame to frame from the newtonian frame to the oframe */
kc::ks_ptr<kf::OrientedChainedFrameToFrame> _newtonian2oframe_f2f = nullptr;
/** the frame to frame from the newtonian frame to the pframe */
kc::ks_ptr<kf::OrientedChainedFrameToFrame> _newtonian2pframe_f2f = nullptr;
/** the Upsilon value at the subhinge in the pframe */
km::Mat66 _pframe_Upsilon;
/** the slot index of the subhinge in the parent hinge's subhinge list */
size_t _index = std::numeric_limits<std::size_t>::max();
};
/**
* @class PhysicalSubhinge_T
* @brief Represents the template class for fixed-size physical subhinges
*
* See \sref{subhinges_sec} section for more information on physical
* subhinges.
*/
template <int NQ, int NU>
class PhysicalSubhinge_T : public PhysicalSubhinge { // NOLINT(readability-identifier-naming)
// for access to
friend class Algorithms;
public:
/**
* @brief Destructor.
*/
virtual ~PhysicalSubhinge_T();
/**
* @brief Get the ATBI matrix psi value.
*
* @return The ATBI matrix psi value.
*/
km::Mat getATBIMatPsi() const override;
km::Mat getATBID() const override;
km::Mat getATBIDinv() const override;
km::Mat getATBIG() const override;
km::Mat getATBITauper() const override;
km::Mat pframeCoordMapMatrix() const override;
void setUdot(const Eigen::Ref<const km::Vec> &val) override;
void setT(const Eigen::Ref<const km::Vec> &val) override;
const km::Mat atbiCoordMapMatrix() const override {
return _atbi_coord_map_matrix_cache->get();
}
const km::Mat oframeCoordMapMatrix() const override {
return _oframe_coord_map_matrix_cache->get();
}
std::string dumpString(std::string_view prefix,
const kc::Base::DumpOptions *options) const override;
using kc::Base::dump;
protected:
/**
* @brief Constructor
* @param oframe the oframe frame
* @param pframe the pframe frame
* @param name the subhinge name
* @param hge the parent hinge
* @param n_q the number of Q coordinates
* @param n_u the number of U coordinates
*/
PhysicalSubhinge_T(kc::ks_ptr<kf::Frame> oframe,
kc::ks_ptr<kf::Frame> pframe,
std::string_view name,
kc::ks_ptr<FramePairHinge> hge,
size_t n_q,
size_t n_u);
km::Mat _jacobian(const kf::FrameToFrame &f2f) const override;
km::Mat _jacobianDot(const kf::FrameToFrame &f2f) const override;
km::Mat66 _computeATBIMatrices(const km::Mat66 &P) override;
km::SpatialVector _computeATBIFilterVectors(const km::SpatialVector &z) override;
km::SpatialVector _computeATBISmootherVectors(const km::SpatialVector &alpha) override;
km::Mat66 _computeUpsilonMatrices(const km::Mat66 &upsilon_plus_oframe) override;
/**
* @brief Type for a matrix that is 6 x NU. Becomes a vector if NU is 1.
*/
using body_6_NU_Mat = std::conditional_t<NU == 1,
Eigen::Vector<double, 6>,
Eigen::Matrix<double, 6, NU, Eigen::RowMajor>>;
/** @brief Overall struct for ATBI dynamics matrix quantities for an
articulation subhinge which includes the fixed and subhinge
dof size matrices */
struct ATBIMatrices : public CoordBase::ATBIMatrices {
km::Mat66 P; //!< The 6x6 PR articulated body inertia for the subhinge on the
//!< pframe/outboard side of the subhinge. The value is about
//!< and expressed in the pframe frame for the subhinge.
/**
* The Pplus matrix after crossing the deformation dofs, about and
* expressed in the oframe frame for the subhinge.
*/
km::Mat66 Pplus; // NOLINT(readability-identifier-naming)
public:
/** HP = H*P (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.
*
*/
Eigen::Matrix<double, NU, 6, Eigen::RowMajor>
HP; // NOLINT(readability-identifier-naming)
/** D = H*P*HT (nU x nU)
*
*/
Eigen::Matrix<double, NU, NU, Eigen::RowMajor> D;
/** Dinv = inverse(D) (nU x nU)
*
*/
Eigen::Matrix<double, NU, NU, Eigen::RowMajor>
Dinv; // NOLINT(readability-identifier-naming)
/** G = P*H*Dinv (body_nU x nU)
*
* For a subhinge the lhs is in the pframe, while it in the
* pnode deformed translational frame for a flex body.
*
*/
body_6_NU_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::Mat66 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::Mat66 psi;
};
/** @brief Overall struct for ATBI dynamics filter vector quantities for an
articulation subhinge which includes the fixed and subhinge
dof size matrices */
struct ATBIFilterVectors : public CoordBase::ATBIFilterVectors {
public:
km::SpatialVector z; //!< the z ATBI spatial vector for the subhinge on the pframe side
//!< of the subhinge and represented in pframe
km::SpatialVector zplus; //!< the zplus vector after crossing the subhinge about and
//!< expressed in the subhinge's oframe.
public:
/** Residual hinge force (nU)
*
* eps = T - H * (zR + PR*a)
*
*/
km::Vec epsilon;
/** Residual hinge acceleration (nU)
*
* nu = Dinv * eps
*
*/
km::Vec nu;
public:
/**
* @brief Resize the ATBI vectors
* @param n_u the number of U generalized velocity coordinates
*/
void resize(size_t n_u) {
if constexpr (NU == Eigen::Dynamic) {
epsilon.resize(n_u);
nu.resize(n_u);
km::makeNotReady(epsilon);
km::makeNotReady(nu);
}
}
};
/** @brief Data cache callback to compute the pframe representation of the joint map
* matrix
*
* @param val the data buffer for the computed value
*/
void _computeATBICoordMapMatrix(body_6_NU_Mat &val) const;
/** @brief Helper method to compute the joint map
* matrix is used for ATBI computations.
*
* @return the coord map matrix
*/
const body_6_NU_Mat &_atbiCoordMapMatrix() const {
return _atbi_coord_map_matrix_cache->get();
}
/** @brief Helper method to compute the joint map
* matrix represented in the oframe
*
* @return the coord map matrix
*/
const body_6_NU_Mat &_oframeCoordMapMatrix() const {
return _oframe_coord_map_matrix_cache->get();
}
void _computeInvDynGenForce(const km::SpatialVector &pnode_invdyn_f) override;
void _computeTransform(Karana::Math::HomTran &T) override;
void _computeVelocity(Karana::Math::SpatialVector &V) override;
void _computeAccel(Karana::Math::SpatialVector &a) override;
virtual kc::ks_ptr<PhysicalSubhingeParams> _getParams() const override = 0;
void _fillCoordParams(PhysicalSubhingeParams ¶ms) const override;
/** @brief Apply the params from the twin subhinge to this subhinge
@param params the input parameters
*/
virtual void _setParams(const PhysicalSubhingeParams ¶ms) override;
/** @brief Apply the params from the original subhinge to the reversed
version of this subhinge
@param params the input parameters
*/
void _setReversedParams(const PhysicalSubhingeParams ¶ms) override;
protected:
/** the 6xNU pframe joint map matrix. */
kc::ks_ptr<kc::DataCache<body_6_NU_Mat>> _atbi_coord_map_matrix_cache = nullptr;
/// the 6xNU oframe joint map matrix data cache
kc::ks_ptr<kc::DataCache<body_6_NU_Mat>> _oframe_coord_map_matrix_cache = nullptr;
};
// extern template class PhysicalSubhinge_T<0, 0>;
extern template class PhysicalSubhinge_T<1, 1>;
extern template class PhysicalSubhinge_T<3, 3>;
extern template class PhysicalSubhinge_T<4, 3>;
} // namespace Karana::Dynamics