Program Listing for File Multibody.h#
↰ Return to documentation for file (include/Karana/SOADyn/Multibody.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 Multibody class.
*/
#pragma once
#include "Karana/KCore/UsageTrackingMap.h"
#include "Karana/ProxyScene/ProxyScene.h"
#include "Karana/SOADyn/Defs.h"
#include "Karana/SOADyn/HingeNode.h"
#include "Karana/SOADyn/SubGraph.h"
#include "Karana/Scene/Color.h"
#include "Karana/Scene/Layer.h"
namespace Karana::Frame {
class FrameContainer;
}
namespace Karana::Dynamics {
namespace kc = Karana::Core;
namespace kf = Karana::Frame;
namespace ks = Karana::Scene;
class PhysicalBody;
class LoopConstraintCutJoint;
class LoopConstraintConVel;
class CoordinateConstraint;
/**
* @class Multibody
* @brief Represents the multibody system class with physical bodies
*
* See the \sref{physical_bodies_sec} and \sref{treembody_sec} sections
* for more discussion related to the Multibody class.
*
* This is the class for containing the multibody system definition
*/
class Multibody : public SubGraph {
// for access to trackUsagePhysicalBody()
friend class PhysicalBody;
friend class PhysicalModalBody;
friend class PhysicalApparentMassBody;
// for access to body2parent_map
friend class PhysicalHinge;
// for access to _tree
friend class PhysicalSubhinge;
// for access to _algorithmic_subtree
friend class SubTree;
// for access to _existing_loop_constraints_list
friend class LoopConstraintConVel;
friend class LoopConstraintCutJoint;
// for access to usage tracker
friend class CoordinateConstraint;
// for access to _detached_nodes_list
friend class Node;
// for access to _detached_constraint_nodes_list
friend class ConstraintNode;
public:
/**
* @brief Constructs a Multibody.
*
* @param name The name for the new Multibody instance
* @param fc The Karana::Frame::FrameContainer to use
*/
Multibody(std::string_view name, kc::ks_ptr<kf::FrameContainer> fc);
/**
* @brief Multibody destructor.
*/
virtual ~Multibody(){};
/**
* @brief Factory method to create a new Multibody instance.
*
* The virtual root body is attached to the multibody root
* frame. If the multibody root frame is not specified, then the
* method uses the newtonian frame, and if that is unspecified
* as well, then the method uses the frame container's root
* frame as the root frame.
*
* @param name The name for the new Multibody instance
* @param fc The Karana::Frame::FrameContainer to use
* @param mbroot_frame The parent Karana::Frame::Frame for the virtual root body
* @param newtonian_frame The inertial Karana::Frame::FrameToFrame frame
* @return a new Multibody instance
*/
static kc::ks_ptr<Multibody> create(std::string_view name, //
kc::ks_ptr<kf::FrameContainer> fc,
kc::ks_ptr<kf::Frame> mbroot_frame = nullptr,
kc::ks_ptr<kf::Frame> newtonian_frame = nullptr);
/**
* @brief This C++-only function will create and populate a
* Multibody from a SubGraphDS file. This creates a Python interpreter
* under the hood and utilizes Python functions to do so. If
* creating from Python, just use `SubGraphDS` directly. See
* \sref{importing_model_data_files_sec} section for more information.
*
* @param filename - The filename of the file to load from.
* @param fc - The FrameContainer to use for this Multibody.
* @param newtonian_frame - Pointer to a frame to use as the newtonian frame. If nullptr
* (the default), then the newtonian_frame will be the mbody virtual root.
* @param scene - Pointer to a ProxyScene. If nullptr, then no scene is present and geometry
* will not be loaded.
* @return a new Multibody instance
*/
static kc::ks_ptr<Multibody>
fromFile(const std::filesystem::path &filename,
const kc::ks_ptr<kf::FrameContainer> &fc,
const kc::ks_ptr<kf::Frame> &newtonian_frame = nullptr,
const kc::ks_ptr<ks::ProxyScene> &scene = nullptr);
/**
* @brief This C++-only function will create a file from a
* Multibody. This creates a Python interpreter under the hood
* and utilizes Python functions to do so. If creating from
* Python, just use `SubGraphDS` directly. See the
* \sref{importing_model_data_files_sec} section for more information.
*
* @param filename - The name of the file to create.
*/
void toFile(const std::filesystem::path &filename);
/**
* @brief Convert all PhysicalHinges into LoopConstraintCutJoints
* to create a fully augmented (FA) model
*
* The FA model consists of independent physical bodies, where
* the couple hinges are replaced with equivalent loop
* constraints. The FA model is also referred to as the absolute
* coordinates model. The main purpose of this method is to
* create FA models to compare and benchmark performance of
* absolute coordinate dynamics with the minimal coordinate
* methods. This method introduces constraints, and hence the TA
* algorithm must be used for dynamics computations for the FA
* model. See \sref{FA_TA_models_sec} section for discussion of FA
* models.
*/
void toFullyAugmentedModel();
std::string
dumpString(std::string_view prefix = "",
const Karana::Core::Base::DumpOptions *options = nullptr) const override;
/**
* @brief Look up and return the closest Node ancestor for a Karana::Frame::Frame.
*
* Return null if the Karana::Frame::Frame is not the descendant
of a body Node. See \sref{body_nodes_sec} section for
information on body nodes.
*
* @param frame the input Karana::Frame::Frame
* @return the ancestor Node for the Frame.
*/
kc::ks_ptr<Node> getNodeAncestor(const kc::ks_ptr<kf::Frame> &frame) const;
/**
* @brief Return the Newtonian Karana::Frame::Frame
*
* See the \sref{manual_creation_sec} section for more information.
* @return the Newtonian Frame instance
*/
const kc::ks_ptr<kf::Frame> &getNewtonianFrame() const;
/**
* @brief Return the Karana::Frame::FrameContainer frame container for the multibody system
*
* See the \sref{manual_creation_sec} section for more information.
* @return the Karana::Frame::FrameContainer instance
*/
const kc::ks_ptr<kf::FrameContainer> &frameContainer() const;
/**
* @brief Return the list of existing constraint instances (enabled and disabled)
*
* @return constraints list
*/
std::vector<kc::ks_ptr<BilateralConstraintBase>> constraints() const;
/**
* @brief Look up a existing constraint by name (enabled or disabled)
* See \sref{constraints_sec} section for more info on constraints.
* @param name the constraint's name
* @return the CoordinateConstraint instance
*/
kc::ks_ptr<BilateralConstraintBase> getConstraint(std::string_view name) const;
/**
* @brief Create stick parts for the multibody.
*
* A Karana::Scene::ProxyScene scene must be registered with the
* Multibody first.
*
* @param c Config that controls the look of the stick parts visualization.
*/
void createStickParts(const StickPartsConfig &c = StickPartsConfig());
/**
* @brief Set the Karana::Scene::ProxyScene instance for the multibody system
*
* See \sref{proxyscene_sec} section for more information on
* Karana::Scene::ProxyScene.
*
* @param scene The scene instance
*/
void setScene(const kc::ks_ptr<ks::ProxyScene> &scene);
/**
* @brief Get the Karana::Scene::ProxyScene scene member for the multibody system
* See \sref{proxyscene_sec} section for more information on
* Karana::Scene::ProxyScene.
*
* @return The scene member
*/
kc::ks_ptr<ks::ProxyScene> &getScene();
/**
* @brief Add a serial chain of PhysicalBody instances to the multibody system
*
* Add a serial chain of bodies to the multibody system to the
* specified root body with the specified mass properties, hinge
* type and hinge connection properties. This method is mainly
* used for procedurally generating large test systems. This
* helper method takes a body creation method as argument and
* can thus be used to create rigid PhysicalBody or deformable
* PhysicalModalBody bodies. See \sref{multibody_procedural_sec}
* section for more discussion on procedural creation of
* multibody systems.
*
* @param create_body Callback method to use to create a body
* @param name prefix string to use for the new body names
* @param nbodies number of bodies
* @param root the parent body to attach the chain to
* @param htype the hinge type for the body hinges
* @param params body parameters
* @return the list of new bodies
*/
std::vector<kc::ks_ptr<PhysicalBody>> addPhysicalBodiesChain(
const std::function<kc::ks_ptr<PhysicalBody>(std::string_view nm)> &create_body,
std::string_view name,
size_t nbodies,
PhysicalBody &root,
HingeType htype,
const PhysicalBodyParams ¶ms);
/**
* @brief Add a sub-tree of PhysicalBody instances to the multibody system
*
* Add a sub-tree of bodies to the multibody system to the
* specified root body with the specified mass properties, hinge
* type and hinge connection properties. This method is mainly
* used for procedurally generating large test systems. This
* helper method takes a body creation method as argument and
* can thus be used to create rigid or deformable bodies. See
* \sref{multibody_procedural_sec} section for more discussion on
* procedural creation of multibody systems.
*
* @param create_body Callback method to use to create a body
* @param name prefix string to use for the new body names
* @param branch_length the number of bodies in a branch
* @param nbranches the number of children branches
* @param depth the number of branching levels to create
* @param root the parent body to attach the chain to
* @param htype the hinge type for the body hinges
* @param params body parameters
* @return the list of new bodies
*/
std::vector<kc::ks_ptr<PhysicalBody>> addPhysicalBodiesTree(
const std::function<kc::ks_ptr<PhysicalBody>(std::string_view nm)> &create_body,
std::string_view name,
size_t branch_length,
size_t nbranches,
size_t depth,
PhysicalBody &root,
HingeType htype,
const PhysicalBodyParams ¶ms);
protected:
/**
* @brief Track the usage of the coordinate constraint
* @param c the coordinate constraint
*/
void _trackUsageCoordinateConstraint(kc::ks_ptr<CoordinateConstraint> c);
/**
* @brief Track the usage of a physical body
* @param bd the body
*/
void _trackUsagePhysicalBody(kc::ks_ptr<PhysicalBody> bd);
void _makeHealthy() override;
void _makeNotHealthy() override;
/**
* @brief Discard the center of mass (CM) Karana::Frame::Frame frame.
*/
void _discardCmFrame() override final;
/**
* @brief Discard the provided Multibody.
* @param base - Base pointer to the Multibody to discard.
*/
void _discard(kc::ks_ptr<Base> &base) override;
private:
#if 0
void _createStickPartsBody(const kc::ks_ptr<PhysicalBody> &bd,
const StickPartsConfig &c);
/**
* @brief Create stick parts for a hinge.
* @param hge The hinge to create stick parts for.
* @param c The config for drawing the stick parts.
* @param constraint Bool. Set true if this hinge comes from a constraint so it can be
* colored appropriately.
*/
void _createStickPartsHinge(const FramePairHinge &hge,
const StickPartsConfig &c,
bool constraint);
#endif
void _trackUsageLoopConstraintBase(kc::ks_ptr<LoopConstraintBase> lc);
/**
* @brief Return the list of existing LoopConstraintBase instances (enabled and disabled)
*
* @return loop constraints list
*/
const std::vector<kc::ks_ptr<LoopConstraintBase>> &_loopConstraints() const;
/**
* @brief Return the list of existing CoordinateConstraint instances (enabled and disabled)
*
* @return coordinate constraints list
*/
std::vector<kc::ks_ptr<CoordinateConstraint>> _coordinateConstraints() const;
/**
* @brief Look up a existing coordinate constraint by name (enabled or disabled)
* See \sref{coordinate_constraints_sec} section for more info on loop constraints.
* @param name the coordinate constraint's name
* @return the CoordinateConstraint instance
*/
kc::ks_ptr<CoordinateConstraint> _getCoordinateConstraint(std::string_view name) const;
/**
* @brief Look up a existing loop constraint by name (enabled or disabled)
* See \sref{constraints_sec} for more info on loop constraints.
* @param name the loop constraint's name
* @return the LoopConstraintBase instance
*/
kc::ks_ptr<LoopConstraintBase> _getLoopConstraint(std::string_view name) const;
protected:
//-------------------------------
//-------------------------------
kc::ks_ptr<kf::FrameContainer> _fc = nullptr; ///< Frame container
/** The frame to use as the inertial, i.e. newtonian frame. By
default this is the virtual root body, but can be set to a
different frame. */
kc::ks_ptr<kf::Frame> _newtonian_frame = nullptr;
/** the raw unsorted list of physical bodies in the multibody
system */
kc::RegistryList<PhysicalBody> _physical_unsorted_bodies_list;
/** the forward/inverse etc methods that rely on gather/scatter
recursions across the system. These recursions rely on data
caches and their dependencies to be set up bespoke for the
gather/scatter algorithms for the calling subtree. The
following registry tracks the subtrees for whom data cache
dependencies have been set up, Note that all of the
algorithmic subtrees at any given time must have disjoint
physical bodies.
*/
kc::RegistryList<SubTree> _algorithmic_subtrees;
/** list of regular nodes that are not currently attached to a body */
kc::RegistryList<Node> _detached_nodes_list; ///< track nodes on the body
/** list of constraint nodes that are not currently attached to a body */
kc::RegistryList<ConstraintNode>
_detached_constraint_nodes_list; ///< track nodes on the body
/** Master list of loop constraints that currently exist. The
actual usage tracking is done at the f2f level as for
hinges. This list is kept at the multibody level to keep
track of the loop constraints and help with clean up at the
end. */
kc::RegistryList<LoopConstraintBase> _existing_loop_constraints_list;
/** The scene for the multibody instance */
kc::ks_ptr<ks::ProxyScene> _scene = nullptr;
/** usage map for coordinate constraints */
kc::UsageTrackingMap<kc::id_t, CoordinateConstraint> _coordinate_constraint_usage_map;
/** usage map for loop constraints */
kc::UsageTrackingMap<kc::id_t, LoopConstraintBase> _loop_constraint_base_usage_map;
};
} // namespace Karana::Dynamics