Program Listing for File CompoundBody.h#
↰ Return to documentation for file (include/Karana/SOADyn/CompoundBody.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 CompoundBody class.
*/
#pragma once
#include "Karana/SOADyn/BodyBase.h"
#include <map>
#include <vector>
namespace Karana::Dynamics {
namespace kc = Karana::Core;
class PhysicalBody;
class HingePnode;
class CompoundHinge;
class CompoundSubhinge;
/**
* @class CompoundBody
* @brief Represents a compound body
*
* This class is for compound bodies representing a subtree of bodies.
*/
class CompoundBody : public kc::LockingBase, public BodyBase {
/* for access to _physical_bodies_tree */
friend SubTree;
/* for access to _pnodes_list */
friend CompoundSubhinge;
/* for access to _base_onodes_list */
friend PhysicalBody;
/* for access to inverseDynamicsVectors() */
friend class Algorithms;
public:
/**
* @brief Factory create method for a CompoundBody
*
* @param parent_subtree the parent SubTree for the compound body
* @param name body name
* @param component_bodies SubTree with the bodies behind aggregated
* @return the CompoundBody instance
*/
static kc::ks_ptr<CompoundBody> create(std::string_view name,
kc::ks_ptr<SubTree> parent_subtree,
kc::ks_ptr<SubTree> component_bodies);
/**
* @brief CompoundBody constructor. The constructor is not meant to be called directly.
* Please use the create(...) method instead to create an instance.
*
* @param name body name
* @param parent_subtree the parent SubTree for the compound body
* @param component_bodies SubTree with the bodies behind aggregated
*/
CompoundBody(std::string_view name,
kc::ks_ptr<SubTree> parent_subtree,
kc::ks_ptr<SubTree> component_bodies);
/**
* @brief CompoundBody destructor.
*/
virtual ~CompoundBody();
std::string_view typeString(bool brief = true) const noexcept override {
return kc::Base::typeString(brief);
}
/**
* @brief Helper method for reconciling multiple inheritance
*
* @return the object id
*/
const kc::id_t &id() const override { return Karana::Core::LockingBase::id(); }
/**
* @brief Helper method for reconciling multiple inheritance
*
* @return the object name
*/
std::string_view name() const override { return Karana::Core::LockingBase::name(); }
/**
* @brief Return the SubTree for the aggregated bodies
*
* @return the aggregated bodies SubTree
*/
const kc::ks_ptr<SubTree> &bodiesTree() const { return _bodies_tree; }
/**
* @brief Return the SubTree with aggregated physical bodies
*
* The returned tree only has physical bodies, and is obtained by
* recursively expanding out any compound bodies within the
* aggregated bodies in this compound body
*
* @return the SubTree with all aggregated physical bodies
*/
kc::ks_ptr<SubTree> physicalBodiesTree() const {
return _physical_bodies_tree ? _physical_bodies_tree : _bodies_tree;
}
/** @brief Replace the embedded child compound body with the embedded
physical bodies it contains with this compound body.
The child compound body is discarded by this method after
the transfer has been completed.
@param child_cpbd the embedded compound body to be flattened
*/
void flatten(kc::ks_ptr<CompoundBody> &child_cpbd);
/** @brief Flatten all the embedded compound bodies within this body.
*/
void flattenAll();
public:
void setQ(const Eigen::Ref<const km::Vec> &val) override;
const km::Vec &getQ() const override;
void setU(const Eigen::Ref<const km::Vec> &val) override;
const km::Vec &getU() const override;
void setUdot(const Eigen::Ref<const km::Vec> &val) override;
const km::Vec &getUdot() const override;
/* Return the compound hinge for this compound body */
kc::ks_ptr<HingeBase> parentHinge() const override;
virtual const kc::ks_ptr<PhysicalBody> &physicalParentBody() const override {
return _physical_parent_body;
}
/** @brief Struct with options for dumpString */
struct DumpOptions : Karana::Core::LockingBase::DumpOptions {
/**
* @brief Assignment operator.
* @param p The instance to copy from
* @return The instance with data copied in.
*/
// 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) {
kc::LockingBase::DumpOptions::operator=(
p); // Call base class assignment operator
// Assign Derived-specific members here
cache_deps = p.cache_deps;
}
return *this;
}
};
/**
* @brief Return true if the specified BodyBase body is embedded
* at some level within this compound body
*
* @param bd the input body
* @return true if the body belongs to the subtree
*/
bool containsBody(const BodyBase &bd) const;
std::string dumpString(std::string_view prefix,
const Karana::Core::Base::DumpOptions *options) const override;
public:
/**
* @brief Return the ATBI matrix cache
*
* @return the ATBI matrix cache for the body
*/
const kc::ks_ptr<kc::DataCache<bool>> atbiMatrixCache() const {
return _atbi_data_caches.matrix_cache;
}
/**
* @brief Return the ATBI filter cache
*
* @return the ATBI filter cache for the body
*/
const kc::ks_ptr<kc::DataCache<bool>> atbiFilterCache() const {
return _atbi_data_caches.filter_cache;
}
/**
* @brief Return the ATBI smoother cache
*
* @return the ATBI smoother cache for the body
*/
const kc::ks_ptr<kc::DataCache<bool>> atbiSmootherCache() const {
return _atbi_data_caches.smoother_cache;
}
/**
* @brief Return the OSCM related Upsilon cache
*
* @return the OSCM related Upsilon cache for the body
*/
const kc::ks_ptr<kc::DataCache<bool>> upsilonMatrixCache() const {
return _atbi_data_caches.upsilon_matrix_cache;
}
/** @brief Struct for inverse dynamics interbody spatial force
quantities for the embedded bodies. */
struct InvDynVectors {
/** the (6 nbodies +nU) raw spatial interaction force from the
outboard children bodies together with the local
contribution in their pnode frames. We need to process
this with Phi_G to get the actual inter-body spatial
force for the embedded bodies.
*/
km::Vec f_prime;
/** the (6 nbodies + nU) interbody spatial force for the embedded
bodies. This is Phi_G*f_prime
*/
km::Vec f;
};
/**
* @brief Return the updated inverse dynamics vectors
*
* @return the inverse dynamics vectors
*/
const InvDynVectors &inverseDynamicsVectors() const {
return _inverseDynamicsCache()->get();
}
/**
* @brief Return the stacked vector of interbody forces for the embedded physical bodies
* (at their onodes)
*
* @return the onode referenced interbody forces vector
*/
km::Vec getInterBodyForceTreeFwdDyn() const;
protected:
kc::ks_ptr<kf::FrameToFrame>
f2f() const override; // NOLINT(readability-identifier-naming) TODO
/** @brief ATBI data caches for the body */
struct ATBIDataCaches {
/** ATBI matrix data cache for the body */
kc::ks_ptr<kc::DataCache<bool>> matrix_cache = nullptr;
/** ATBI filter data cache for the body */
kc::ks_ptr<kc::DataCache<bool>> filter_cache = nullptr;
/** ATBI smoother data cache for the body */
kc::ks_ptr<kc::DataCache<bool>> smoother_cache = nullptr;
/** ATBI OSI data cache for the body */
kc::ks_ptr<kc::DataCache<bool>> upsilon_matrix_cache = nullptr;
};
/**
* @brief Return the inverse dynamics cache
*
* @return the inverse dynamics cache for the body
*/
const kc::ks_ptr<kc::DataCache<InvDynVectors>> _inverseDynamicsCache() const {
return _invdyn_data_cache;
}
protected:
/** @brief Create the compound subhinge
* @param hinge the parent compound hinge
* @return the new compound subhinge
*/
virtual kc::ks_ptr<CompoundSubhinge> _createSubhinge(kc::ks_ptr<CompoundHinge> hinge);
/**
* @brief - Discard the provided CompoundBody.
* @param base - Base pointer to the CompoundBody to discard.
*/
void _discard(kc::ks_ptr<kc::Base> &base) override;
/**
* @brief Return the list of physical bodies in the compound body
*
* Compute and return the list of physical bodies in the compound
* body. This does not include the physical root body
*
* @return list of physical bodies
*/
std::vector<kc::ks_ptr<PhysicalBody>> _physicalBodiesList() const;
/**
* @brief Return the stacked vector of interbody forces for the embedded physical bodies
* (at their pnodes)
*
* @return the pnode referenced interbody forces vector
*/
km::Vec _getInterBodyForcePnodesTreeFwdDyn() const;
protected:
void _setupChildBodyCacheDependencies(kc::ks_ptr<BodyBase> child_body) override;
void _setupChildPhysicalBodyCacheDependencies(const PhysicalBody &child_body,
bool skip_scatter) override;
void _setupChildCompoundBodyCacheDependencies(kc::ks_ptr<CompoundBody> child_body) override;
void _setupBaseBodyCacheDependencies() override;
void _teardownChildBodyCacheDependencies(kc::ks_ptr<BodyBase> child_body) override;
void _teardownChildPhysicalBodyCacheDependencies(const PhysicalBody &child_body,
bool skip_scatter) override;
void
_teardownChildCompoundBodyCacheDependencies(kc::ks_ptr<CompoundBody> child_body) override;
void _teardownBaseBodyCacheDependencies() override;
protected:
/**
* @brief Do initial setup of the compound body
*/
void _initialSetup();
/**
* @brief Do post setup of the compound body
*/
void _postSetup();
/**
* @brief One time setup of the body
*/
void _oneTimeSetupDataCaches();
/**
* @brief One time tear down of the body
*/
void _oneTimeTeardownDataCaches();
/** @brief compute Phi_G * f_prime (for the actual inter-body force)
@param f_prime the input f' forces
@return the vector product
*/
km::Vec _compute_Phi_G_fprime(const km::Vec &f_prime) const;
protected:
km::Mat _oframe2pframePsi() const override;
km::Mat _oframe2pframePhi() const override;
km::Mat _pframe2otherPhi(const kf::Frame &other) const override;
/**
* @brief Data cache callback for inter body force values for inverse dynamics
*
* @param val InvDynVectors with returned values.
*/
void _computeInvDynForces(InvDynVectors &val);
/**
* @brief Compute fprime body forces using tree ATBI fwd dynamics data
*
* @returns the computed f' stacked vector value
*/
virtual km::Vec _getFprimeTreeFwdDyn() const;
protected:
/** the single physical virtual root parent body for the G subtree to
use. Note that all the base bodies in the G subtree are attached
(at possible multiple points) to the virtual root physical body
even if the topological parent for this compound body may be a
compound body itself. The aggregation principle, requires that all
the base physical bodies in G physical have to be attached to a
single physical body (at different onodes likely) and this parent
physical body will belong to the parent compound body. */
kc::ks_ptr<PhysicalBody> _physical_parent_body = nullptr;
/** List of onodes for the bodies directly attached to the
physical virtual root of the aggregated bodies. This list is
used in the gather recursions step from the compound body into
the parent body via the E_G operator. */
kc::RegistryList<HingeOnode> _base_onodes_list;
/** SubTree with the bodies in the compound body. This list may
contain compound bodies as well. */
kc::ks_ptr<SubTree> _bodies_tree = nullptr;
/** SubTree with the physical bodies in the compound body. If
_bodies_tree contains only physical bodies, then this variable
simply points to it. */
kc::ks_ptr<SubTree> _physical_bodies_tree = nullptr;
/** the parent compound hinge for this body */
kc::ks_ptr<CompoundHinge> _hinge = nullptr;
/** Map of of child body attachment onodes for each aggregated
body in the compound body. The list is the onodes for each
of the aggregated bodies consists of just the subset of its
child onodes that are connected to physical bodies external
to the compound body.
*/
std::map<kc::ks_ptr<PhysicalBody>, kc::RegistryList<HingeOnode>> _child_onodes_map;
/** Map of of base onode for the branch each aggregated body in
the compound body. All of the base onode's are attached to
the physical parent body.
*/
std::map<kc::ks_ptr<PhysicalBody>, kc::ks_ptr<HingeOnode>> _base_onodes_map;
/** Data cache for inverse dynamics data */
kc::ks_ptr<kc::DataCache<InvDynVectors>> _invdyn_data_cache = nullptr;
/** struct with data caches for ATBI data */
ATBIDataCaches _atbi_data_caches;
private:
/*
* We want to return const km::Vec& for the
* getter methods. However, we need to use CoordData
* for CompoundSubhine. 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 getU
mutable km::Vec _getU_cache;
/// Vector for storing getUdot
mutable km::Vec _getUdot_cache;
/// Vector for storing getT
mutable km::Vec _getT_cache;
/** the nbodies x 6 interbody spatial force for the embedded
bodies at their pnodes
*/
// km::Vec _f;
};
} // namespace Karana::Dynamics