Program Listing for File SubTree.h#
↰ Return to documentation for file (include/Karana/SOADyn/SubTree.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 SubTree class.
*/
#pragma once
#include "Karana/KCore/LockingBase.h"
#include "Karana/KCore/Tree.h"
#include "Karana/KCore/UsageTrackingMap.h"
#include "Karana/SOADyn/CoordData.h"
#include "Karana/SOADyn/PhysicalBody.h"
#include "Karana/SOADyn/SubhingeBase.h"
namespace Karana::Dynamics {
namespace kc = Karana::Core;
namespace km = Karana::Math;
class BodyBase;
class CompoundBody;
class Multibody;
class PhysicalBody;
class CoordData;
/**
* @class SubTree
* @brief Represents a SubTree with connected bodies
*
* This class is for a sub-tree of connected bodies
*
* See the \sref{physical_bodies_sec}, \sref{treembody_sec}, and
* \sref{subtrees_sec} sections for more discussion related to the SubTree
* class.
*/
class SubTree : public kc::LockingBase {
/* for access to _unsorted_bodies_list */
friend class CompoundBody;
friend class CompoundSubhinge;
/* for access to _body2parent_map */
friend class PhysicalBody;
/* for access to getPsiMatrix ... */
friend class HingePnode;
/* for access to _clearExternals */
friend class ModelManager;
/* for access to _needs_state_reset */
friend class StatePropagator;
/* for access to _getMinvBlock */
friend class Algorithms;
public:
/**
* @brief Constructs a SubTree.
*
* @param name The name for the new SubTree
* @param root The virtual root body for the new SubTree
* @param parent_subtree The parent SubTree for the new SubTree
*/
SubTree(std::string_view name,
const kc::ks_ptr<BodyBase> &root,
const kc::ks_ptr<SubTree> &parent_subtree = nullptr);
/**
* @brief SubTree destructor.
*/
virtual ~SubTree();
/**
* @brief Method to check whether this is a SubTree or a SubGraph
* @return False if this is a SubTree and not a SubGraph
*/
virtual bool isSubGraph() const { return false; }
/**
* @brief Returns the virtual root BodyBase body for the tree.
* @return The virtual root body.
*/
virtual const kc::ks_ptr<BodyBase> &virtualRoot() const { return _virtual_root_body; }
public:
/**
* @brief Return the specified body's BodyBase parent body in this SubTree
*
* Since a SubTree can have physical and compound bodies, the
* returned parent body can be a physical or a compound body.
*
* @param body the specified body
* @return the parent body
*/
const kc::ks_ptr<BodyBase> &parentBody(const BodyBase &body) const;
/**
* @brief Return the list of BodyBase base bodies for the SubTree
*
* Base bodies are the immediate children of the SubTree's virtual
* root body.
*
* @return the list of BodyBase base bodies
*/
const std::vector<kc::ks_ptr<BodyBase>> &baseBodies() const;
/**
* @brief Return the list of BodyBase leaf bodies for the SubTree
*
* Leaf bodies are the bodies with children in the SubTree
*
* @return the list of leaf BodyBase bodies
*/
const std::vector<kc::ks_ptr<BodyBase>> &leafBodies() const;
/**
* @brief Return the common ancestor body for the pair of BodyBase bodies in this SubTree
*
* @param bd the first body
* @param bd1 the other body
* @return the common ancestor BodyBase body
*/
kc::ks_ptr<BodyBase> ancestorBody(const BodyBase &bd, const BodyBase &bd1) const;
/**
* @brief Return true if the specified BodyBase body belongs to the SubTree or to any of
* its nested hierarchy of SubTrees
*
* @param bd the input body
* @return true if the body belongs to the SubTree
*/
bool containsBody(const BodyBase &bd) const;
/** @brief Return true if the specified BodyBase body directly belongs to the SubTree
*
* This method will return false if the input body is embedded
* within one its bodies.
*
* @param bd the input body
* @return true if the body belongs to the SubTree
*/
bool hasBody(const BodyBase &bd) const;
/**
* @brief Look up a BodyBase body in the SubTree by name.
*
* This method will throw an error if there are multiple bodies
* with the same specified name. This method will return a
* nullptr if there is no body with the matching name.
*
* @param name the body's name
* @return the BodyBase body instance
*/
kc::ks_ptr<BodyBase> getBody(std::string_view name) const;
/**
* @brief Look up BodyBase bodies in the SubTree with the specified name.
*
* @param name the body name
* @return the list of BodyBase body instances
*/
std::vector<kc::ks_ptr<BodyBase>> getBodies(std::string_view name) const;
/**
* @brief Look up the containing compound body for a body
* contained by the subtree
*
* A body contained in a SubTree, may be a direct child, or be
* embedded at some level in a compound body in the
* subtree. This method will return the direct child compound
* body that contains the input body at some level in its nested
* hierarchy of embedded bodies. This method will return nullptr
* if the input body is not contained in the subtree.
*
* @param bd the input body
* @return the CompoundBody instance containing the body
*/
kc::ks_ptr<CompoundBody> getContainingCompoundBody(BodyBase &bd) const;
/**
* @brief Return the list of BodyBase child bodies for a body for this subtree.
*
* The list may include physical and compound bodies.
*
* @param bd the input body
* @return list of BodyBase children bodies
*/
std::vector<kc::ks_ptr<BodyBase>> childrenBodies(const BodyBase &bd) const;
/**
* @brief Return true if the BodyBase body is a base body for this sub-tree
*
* @param bd the input body
* @return true, if the body is a base body in the SubTree
*/
bool isBaseBody(const BodyBase &bd) const;
/**
* @brief Return the number of bodies in the tree
*
* @return the number of bodies
*/
size_t numBodies() const;
public:
/**
* @brief Returns coord obj and its coord offset corresponding to overall U offset
*
* A SubTree has multiple CoordData which are packed and
* unpacked the coordinate values for its set of CoordBase
* instances. This method returns the CoordBase and its local
* coordinate offset corresponding to the specified overall U
* offset value. This method returns the CoordBase and its
* local offset for the specified overall column offset value in
* the matrix returned by the
* Algorithms::evalVelocityConstraintMatrix() method, or the
* vector returned by the SubTree::getU() method.
*
* @param u_offset the input overall U offset value
* @return The CoordBase instance and its local U coord offset
*/
virtual const std::pair<kc::ks_ptr<CoordBase>, size_t> coordAt(size_t u_offset) const;
/**
* @brief Returns the packing offset for the specified CoordBase
*
* The SubTree has get/set methods to pack and unpack the
* coordinate values for its set of CoordBase and CoordData
* instances. This method returns the offsets for the Q and U
* values for the specified CoordBase in these combined arrays.
*
* @param c The CoordBase to check for
* @return A CoordOffset struct with the Q and U offsets
*/
CoordData::CoordOffset coordOffsets(const CoordBase &c) const;
/**
* @brief The number of Q generalized coords for the CoordBase in the SubTree.
*
* See \sref{coords_sec} section for more on Q, U etc generalized coordinates.
*
* @return the number of coordinates
*/
size_t nQ() const;
/**
* @brief The number of U velocity coords for the CoordBase.
*
* See \sref{coords_sec} section for more on Q, U etc generalized coordinates.
*
* @return the number of velocity coordinates
*/
size_t nU() const;
/**
* @brief Return the Q coordinates as an array
*
* See \sref{coords_sec} section for more on Q, U etc generalized coordinates.
*
* @return Array of values
*/
km::Vec getQ() const;
/**
* @brief Set the Q coordinates.
*
* See \sref{coords_sec} section for more on Q, U etc generalized coordinates.
*
* @param Q Array of values.
*/
void setQ(const Eigen::Ref<const km::Vec> &Q);
/**
* @brief Set the Q coordinates to a constant value
*
* See \sref{coords_sec} section for more on Q, U etc generalized coordinates.
*
* @param fill_value Fill value.
*/
void setQ(double fill_value);
/**
* @brief Return the Qdot rate coordinates as an array
*
* @return Array of values
*/
km::Vec getQdot() const;
/**
* @brief Return the U velocity coordinates as an array
*
* See \sref{coords_sec} section for more on Q, U etc generalized coordinates.
*
* @return Array of values
*/
km::Vec getU() const;
/**
* @brief Set the U velocity coordinates.
*
* See \sref{coords_sec} section for more on Q, U etc generalized coordinates.
*
* @param U Array of values.
*/
void setU(const Eigen::Ref<const km::Vec> &U);
/**
* @brief Set the U velocity coordinates to a constant value
*
* See \sref{coords_sec} section for more on Q, U etc generalized coordinates.
*
* @param fill_value Fill value.
*/
void setU(double fill_value);
/**
* @brief Return the Udot acceleration coordinates as an array
*
* See \sref{coords_sec} section for more on Q, U etc generalized coordinates.
*
* @return Array of values
*/
km::Vec getUdot() const;
/**
* @brief Set the Udot acceleration coordinates.
*
* See \sref{coords_sec} section for more on Q, U etc generalized coordinates.
*
* @param Udot Array of values.
*/
void
setUdot(const Eigen::Ref<const km::Vec> &Udot); // NOLINT(readability-identifier-naming)
/**
* @brief Set the Udot acceleration coordinates to a constant value
*
*
* See \sref{coords_sec} section for more on Q, U etc generalized coordinates.
*
* @param fill_value Fill value.
*/
void setUdot(double fill_value);
/**
* @brief Return the T generalized forces as an array
*
* @return Array of values
*/
km::Vec getT() const;
/**
* @brief Set the T generalized forces.
*
* See \sref{coords_sec} section for more on Q, U etc generalized coordinates.
*
* @param T Array of values.
*/
void setT(const Eigen::Ref<const km::Vec> &T);
/**
* @brief Set the U velocity coordinates to a constant value
*
* See \sref{coords_sec} section for more on Q, U etc generalized coordinates.
*
* @param fill_value Fill value.
*/
void setT(double fill_value);
public:
/**
* @brief Factory method to create a new SubTree instance
*
* Create a new SubTree from the bodies in this SubTree starting
* at the new_root body. If new_leaves is empty, then the sub-tree
* will contain all the descending bodies. Otherwise, the sub-tree
* will consist of the spanning tree made of the new root and the new
* leaves.
*
* @param parent_subtree The parent SubTree for the new SubTree
* @param name The name for the new SubTree
* @param new_root The virtual root body for the new SubTree
* @param use_branches The branches to get bodies from
* @param stop_at list of last bodies on a branch
* @return A new SubTree instance
*/
static kc::ks_ptr<SubTree>
create(std::string_view name,
const kc::ks_ptr<SubTree> &parent_subtree,
kc::ks_ptr<BodyBase> new_root,
const std::vector<kc::ks_ptr<BodyBase>> &use_branches = {},
const std::vector<kc::ks_ptr<BodyBase>> &stop_at = {});
/**
* @brief Return the parent Multibody instance
*
* @return the Multibody instance
*/
const kc::ks_ptr<Multibody> &multibody() const { return _multibody; }
public:
/**
* @brief Return the OSCM matrix for the pnodes for the specified bodies
*
* The returned matrix is square and symmetric and of 6xnbodies row/column size
*
* @param bodies the input list of bodies
* @return the pnodes OSCM matrix
*/
km::Mat bodyPnodesOSCM(const std::vector<kc::ks_ptr<BodyBase>> &bodies);
/**
* @brief Return the OSCM matrix for the specified bodies
*
* The returned matrix is square and symmetric. The block entries
* for deformable bodies will have more than 6 rows/columns.
*
* @param bodies the input list of bodies
* @return the bodies OSCM matrix
*/
km::Mat bodyOSCM(const std::vector<kc::ks_ptr<BodyBase>> &bodies);
/**
* @brief Return the OSCM matrix for the list of frames
*
* The returned matrix is square and symmetric and of 6xnframes
* row/column size. Each frame in the input list is required to
* be downstream of a body. If subhinges are provided, then the
* returned matrix also contains additional rows and columns for
* the subhinges from the mass-matrix, along with cross-terms.
*
* @param body_frames the input list of body frames
* @param subhinges the input list of body frames
* @return the frames OSCM matrix
*/
km::Mat framesOSCM(const std::vector<kc::ks_ptr<kf::Frame>> &body_frames,
const std::vector<kc::ks_ptr<PhysicalSubhinge>> &subhinges = {});
public:
/**
* @brief Display a breakdown of the elements of the state vector into its component
* elements.
*
* The state vector is typically associated with the
* StatePropagator state. See \sref{system_state_sec} section for
* more on the state vector, and the \sref{state_propagator_sec}
* for the StatePropagator class.
*
* @param x The state vector
*/
virtual void showState(const km::Vec &x);
/** @brief Enable/disable the use of local charts for the
* subhinges that support charts.
*
* Typically charts should be enabled when doing time domain
* simulations, but disabled when doing configuration kinematics,
* and linearization operations.
*
* @param flag flag to enable/disable charts
*/
virtual void enableSubhingeCharts(bool flag);
/** @brief Return whether any Subhinges have local charts enabled
*
* Typically charts should be enabled when doing time domain
* simulations, but disabled when doing configuration kinematics,
* and linearization operations.
*
* @returns whether charts have been enabled or not
*/
bool subhingeChartsEnabled() const { return _enabled_subhinge_charts; };
public:
std::string
dumpString(std::string_view prefix = "",
const Karana::Core::Base::DumpOptions *options = nullptr) const override;
/**
* @brief Display information about the SubTree and its bodies
*
* @param prefix the prefix for each line in the output
*/
virtual void displayModel(std::string_view prefix = "") const;
/** @brief A struct with options to tailor the content from dumpTree()
*/
struct DumpTreeOptions {
// bool typeString = false; ///< include the body's typeString info
bool hinge_type = true; ///< include the body's hinge's typeString info
bool healthy_status = false; ///< include the frame's healthy status
bool ref_count = false; ///< include the frame's reference count
bool hinge_ref_count = false; ///< include the frame's edge's reference count
bool id = false; ///< include the frame's tree id
};
/** @brief Display the body tree. The contents of the options struct can be
used to control the content and verbosity of the displayed
output.
* @param prefix the prefix for each line in the output
@param options Options to tailor the output of dumpTree
*/
virtual void dumpTree(std::string_view prefix = "",
const DumpTreeOptions options =
DumpTreeOptions(true, false, false, false, false)) const;
/**
* @brief Return the CoordData for the subhinges
* See \sref{coord_data_sec} for more on the CoordData class.
* @return the CoordData for the subhinges
*/
const kc::ks_ptr<CoordData> &subhingeCoordData() const;
/**
* @brief Return the CoordData for the body deformation coords
* See \sref{coord_data_sec} for more on the CoordData class.
*
* @return the CoordData for the body deformation coords
*/
const kc::ks_ptr<CoordData> bodyCoordData() const { return _body_coord_data; }
/**
* @brief Return the subhinge+body tree CoordData for the SubTree
*
* This CoordData is a combination of the subhinge and body
* CoordDatas
*
* See \sref{coord_data_sec} for more on the CoordData class.
* @return the CoordData for the tree
*/
virtual const kc::ks_ptr<CoordData> treeCoordData() const;
/**
* @brief Return the subhinge tree CoordData for the SubTree
*
* This CoordData is a combination of the subhinge and cut-joint
* constraint hinge CoordDatas. This method is specialized by
* the SubGraph class to include the latter.
*
* See \sref{coord_data_sec} for more on the CoordData class.
* @return the articulation CoordData for the tree
*/
virtual const kc::ks_ptr<CoordData> articulationCoordData() const;
/**
* @brief Return true if the SubTree contains any CompoundBody instances
*
* @return true if the SubTree has compound bodies
*/
bool hasCompoundBodies() const { return _has_compound_bodies; }
/**
* @brief Compute and return the topologically sorted list of PhysicalBody instances in the
* tree
*
* The list includes all physical bodies, even those embedded
* within CompoundBody bodies in the SubTree.
*
* @return the list of PhysicalBody instances
*/
const std::vector<kc::ks_ptr<PhysicalBody>> &sortedPhysicalBodiesList() const;
/** @brief Compute and return the sorted list of bodies in the tree.
* This will be a mix of PhysicalBody and CompoundBody instances.
* @return A vector containing the bodies of the SubTree.
*/
const std::vector<kc::ks_ptr<BodyBase>> &sortedBodiesList() const;
/**
* @brief Enable the running of dynamics algorithms for the SubTree
*
* This methods sets up this SubTree for algorithmic use if it
* has not been setup as such already. Note, that algorithmic
* use is permissible only if this SubTree is disjoint from all
* other algorithmic SubTrees (i.e. the physical bodies are not
* shared). See the \sref{algorithms_sec} section for more on the
* available SubTree level computational algorithms.
*/
void enableAlgorithmicUse();
/**
* @brief Disable the running of dynamics algorithms for the SubTree
*
* This methods unsets this SubTree from algorithmic use if it has
* not been unset already. See the \sref{algorithms_sec} section for more on the
* available SubTree level computational algorithms.
*/
void disableAlgorithmicUse();
/**
* @brief Set the uniform gravity accel value for all the physical bodies in the SubTree
*
* The input value is in the specified reference frame. If no
* reference frame is provided, then the newtonian root frame is
* assumed to be the reference frame. This method can only be
* called on a SubTree that is healthy.
*
* @param g the gravity accel vector
* @param ref_frame the Frame reference frame
*/
void setUniformGravAccel(const km::Vec3 &g, kc::ks_ptr<kf::Frame> ref_frame = nullptr);
/**
* @brief Accumulate the uniform gravity accel value for all the physical bodies in the
* SubTree
*
* The input value is in the specified reference frame. If no
* reference frame is provided, then the newtonian root frame
* is assumed to be the reference frame.
*
* @param g the gravity accel vector
* @param ref_frame the reference frame
*/
void accumUniformGravAccel(const km::Vec3 &g, kc::ks_ptr<kf::Frame> ref_frame = nullptr);
/**
* @brief Return the SubTree's center of mass (CM) Karana::Frame::Frame instance
*
* @return the CM frame Frame instance
*/
const kc::ks_ptr<kf::Frame> &cmFrame() const { return _cm_frame; }
public:
/**
* @brief Sequentially articulate a subhinge for a physical body.
*
* With graphics on, this step can be used to examine and debug
* issues with the model.
*
* @param shg subhinge to articulate
* @param subhinge_index the index of the coordinate element to articulate
* @param range_q is the excursion angle range (in radians)
* @param d_q is the angle step size (in radians)
* @param pause time in seconds to sleep between articulation steps
* @param cb callback to invoke every time the Q coordinate is changed
*/
void articulateSubhinge(SubhingeBase &shg,
size_t subhinge_index,
double range_q = .5,
double d_q = .01,
double pause = .01,
std::function<void()> cb = nullptr) const;
/**
* @brief Sequentially articulate all the 1 dof subhinges for the physical bodies.
*
* With graphics on, this step can be used to examine and debug
* any issues with the model.
*
* @param range_q is the excursion angle range (in radians)
* @param d_q is the angle step size (in radians)
* @param pause time in seconds to sleep between articulation steps
*/
void articulateBodies(double range_q = .5, double d_q = .01, double pause = .01) const;
/**
* @brief Call sanitizeCoords() on all SubTree subhinges with coordinates sanitization
* needs
*
* Return true if any of them actually did a sanitization action.
*
* @return true if any of the subhinges had to sanitize their coordinates
*/
bool sanitizedCoords();
/**
* @brief Clear out the external spatial forces, gravity accel
* and gravity gradient values for all bodies, and all
* generalized coord, velocities, accels and forces. This is a
* helper method to initialize the SubTree to a zero state.
*
* This method should be used with caution. By initializing
* setting all coordinates etc to 0 values, it renders the
* isReady() method ineffective for detecting
* not ready state and other values.
*/
virtual void resetData();
/**
* @brief Return the cross Upsilon(left, right) matrix for a pair of pnodes
*
* This method requires that the left pnode be an ancestor of the
* right pnode.
*
* @param left the pnode on the left
* @param right the pnode on the right
* @return the cross Upsilon(left, right) matrix
*/
km::Mat66 crossUpsilonMatrix(const HingePnode &left, const HingePnode &right);
/**
* @brief Compute the body frame referenced cross-OSCM Upsilon matrix.
*
* For a rigid body the value is a 6x6 matrix, but is larger by
* the number of modes for a deformable body.
*
* @param lbd the left-hand body for the cross-OSCM Upsilon value
* @param rbd the right-hand body for the cross-OSCM Upsilon value
* @return the cross-OSCM Upsilon matrix
*/
km::Mat getCrossUpsilonMatrix(const PhysicalBody &lbd, const PhysicalBody &rbd);
public:
/**
* @brief Return the ATBI psi(left.pnode(), right.pnode()) matrix for the pnodes of the body
* pair
*
* This method requires that the left body be an ancestor of the
* right body.
*
* @param left the body on the left
* @param right the body on the right
* @return the psi(left.pnode(), right.pnode()) matrix
*/
km::Mat getBodyPairPnodePsiMatrix(const BodyBase &left, const BodyBase &right);
/**
* @brief Return the ATBI psi(left, right) matrix for body frame to body frame
*
* The regular psi = phi * tauper
* This method requires that the left body be an ancestor of the
* right body.
*
* @param left the body on the left
* @param right the body on the right
* @return the psi(left, right) matrix for the body frame to body frame
*/
km::Mat getBodyPairPsiMatrix(const BodyBase &left, const BodyBase &right);
/**
* @brief Return the regular phi(left, right) matrix for body frame to body frame
* right pframe)
*
* This method requires that the left body be an ancestor of the
* right body.
*
* @param left the body on the left
* @param right the body on the right
* @return the phi(left, right) matrix
*/
km::Mat getBodyPairPhiMatrix(const BodyBase &left, const BodyBase &right);
/**
* @brief Return the psi(left, right) matrix for the CoordBase pair (from left pframe to
* right pframe)
*
* This method requires that the left CoordBase be an ancestor
* of the right CoordBase. The returned matrix is of size
* (left.nU()+6, right.nU()+6), with the lhs in the left pframe,
* and the rhs in the right pframe. For a subhinge, its pframe is
* its regular pframe. For a flex body, the pframe is the body
* frame.
*
* @param left the CoordBase on the left
* @param right the CoordBase on the right
* @return the psi(left, right) matrix
*/
const km::Mat &getCoordBasePairPsiMatrix(const CoordBase &left, const CoordBase &right);
/**
* @brief Return the oframe variant psi(left, right) matrix for the CoordBase pair (from
* left oframe to right oframe)
*
* The oframe variant psi = tauper * phi This method requires
* that the left CoordBase be an ancestor of the right
* CoordBase. For a subhinge, its oframe is its regular
* oframe. For a flex body, the oframe is the body pnode.
*
* @param left the CoordBase on the left
* @param right the CoordBase on the right
* @return the psi(left, right) matrix
*/
const km::Mat &getCoordBasePairOframePsiMatrix(const CoordBase &left,
const CoordBase &right);
/**
* @brief Return the 6x6 phi(left, right) lhs pframe to rhs pframe phi matrix for the
* CoordBase pair
*
* This method requires that the left CoordBase be an ancestor of the
* right CoordBase. For a subhinge,
* its pframe is its regular pframe. For a flex body, the pframe
* is the body frame.
*
* @param left the CoordBase on the left
* @param right the CoordBase on the right
* @return the 6x6 phi(left, right) matrix
*/
const km::Mat &getCoordBasePairPhiMatrix(const CoordBase &left, const CoordBase &right);
/**
* @brief Dump the details of the tree forward dynamics computations
*
* This is a debugging function that prints out the full state,
* the generalized forces, the external and constraint forces,
* and the gravity acceleration involved in the computation of
* the state derivative. This method will be called with each
* call to Algorithms::evalForwardDynamics if the
* enable_dump_dynamics public member flag has been set to `true`. The
* flag can be set to `false` to disable the dump calls.
*
* @param prefix the prefix for each line in the output
*/
void dumpDynamics(std::string_view prefix = "") const;
/** Flag to turn dumpDynamics calls on or off when the tree
forward dynamics are computed. */
bool enable_dump_dynamics = false;
protected:
/**
* @brief Copy a state vector x into the SubTree generalized and
* velocity coordinates. The x state vector consists of 4
* sub-vectors in a sequence:
* - the subhinge Q coordinates
* - the body deformation Q coordinates
* - the subhinge U generalized velocity coordinates
* - the body deformation U velocity coordinates.
*
* The state vector is typically associated with the
* StatePropagator state. See \sref{system_state_sec} section for
* more on the state vector, and the \sref{state_propagator_sec}
* for the StatePropagator class.
*
* @param x The state to copy into the SubTree.
* @param global if true, x contains global chart coordinate values
*/
virtual void _stateToDynamics(const km::Vec &x, bool global = false);
/**
* @brief Copy a SubTree's generalized and velocity coordinates into a vector x.
* The x state vector consists of 4 sub-vectors in a sequence:
* - the subhinge Q coordinates
* - the body deformation Q coordinates
* - the subhinge U generalized velocity coordinates
* - the body deformation U velocity coordinates.
*
* The state vector is typically associated with the
* StatePropagator state. See \sref{system_state_sec} section for
* more on the state vector, and the \sref{state_propagator_sec}
* for the StatePropagator class.
*
* @param x The vector x to copy the state into.
* @param global Use global coordinates if true, local coordinates otherwise.
*/
virtual void _dynamicsToState(Eigen::Ref<km::Vec> x, bool global = false) const;
/**
* @brief Return a vector of SubTree's generalized and velocity coordinates.
*
* The state vector is typically associated with the
* StatePropagator state. See \sref{system_state_sec} section for
* more on the state vector, and the \sref{state_propagator_sec}
* for the StatePropagator class.
*
* @param global Use global coordinates if true, local coordinates otherwise.
* @returns The state vector, which consists of 4 sub-vectors in a sequence:
* - the subhinge Q coordinates
* - the body deformation Q coordinates
* - the subhinge U generalized velocity coordinates
* - the body deformation U velocity coordinates.
*/
virtual km::Vec _dynamicsToState(bool global = false) const;
/**
* @brief Copy a SubTree's velocity and acceleration coordinates into a vector dx.
* The dx state vector consists of 4 sub-vectors in a sequence:
* - the subhinge Qdot coordinates
* - the body deformation Qdot coordinates
* - the subhinge Udot generalized velocity coordinates
* - the body deformation Udot velocity coordinates.
*
* The state vector is typically associated with the
* StatePropagator state. See \sref{system_state_sec} section for
* more on the state vector, and the \sref{state_propagator_sec}
* for the StatePropagator class.
*
* @param dx The vector dx to copy the state derivative into.
*/
virtual void _dynamicsToStateDeriv(Eigen::Ref<km::Vec> dx) const;
/**
* @brief Return a vector of SubTree's velocity and acceleration coordinates.
*
* The state vector is typically associated with the
* StatePropagator state. See \sref{system_state_sec} section for
* more on the state vector, and the \sref{state_propagator_sec}
* for the StatePropagator class.
*
* @returns The state vector, which consists of 4 sub-vectors in a sequence:
* - the subhinge Qdot coordinates
* - the body deformation Qdot coordinates
* - the subhinge Udot generalized velocity coordinates
* - the body deformation Udot velocity coordinates.
*/
virtual km::Vec _dynamicsToStateDeriv() const;
/**
* @brief Return the list of CoordData for this sub-tree
*
* The CoordData list contains one for the body subhinges in the
* the SubTree, and another for the body deformation
* coordinates. See \sref{coord_data_sec} for more on the
* CoordData class.
*
* @return the list of CoordData
*/
virtual std::vector<kc::ks_ptr<CoordData>> _coordDataList() const;
/**
* @brief Set the local chart Q coordinates
*
* See \sref{coords_sec} section for discussion on Q, U etc coordinates.
*
* @param val Array of values.
*/
void _localChartSetQ(const Eigen::Ref<const km::Vec> &val);
/**
* @brief Return the local chart Q coordinates - without sanitizing coordinates
*
* See \sref{coords_sec} section for discussion on Q, U etc coordinates.
*
* @return Array of values.
*/
km::Vec _localChartGetQ() const;
/** Reset all physical subhinge chart offsets */
virtual void _resetSubhingeCharts();
/**
* @brief Return the CoordOffset struct with offsets for the CoordBase with specified id in
* the subgraph's treeCoordData
*
* @param id the CoordBase id
* @return the CoordOffset struct
*/
virtual CoordData::CoordOffset _coordOffsets(kc::id_t id) const;
/**
* @brief Return the cross Upsilon(left, right) matrix for a pair of CoordBase instances
*
* This method requires that the left one to be an ancestor of
* the right one.
*
* @param lsh the CoordBase on the left
* @param rsh the CoordBase on the right
* @return the cross Upsilon(left, right) matrix
*/
km::Mat _getCrossUpsilonMatrix(CoordBase &lsh, CoordBase &rsh);
/** @brief Return the mass matrix inverse block for this list of
CoordBase instances using the spatial operator decomposition
of the mass matrix inverse.
This method is used by the
Algorithms::evalTreeMassMatrixInverse() method to compute
the mass matrix inverse. It is also used to compute part of
the Gamma matrix for the TA constrained dynamics
algorithm.
@param cbs the list of CoordBase instances
@return the mass matrix inverse block
*/
km::Mat _getMinvBlock(const std::vector<kc::ks_ptr<CoordBase>> &cbs);
/** @brief Return the mass matrix inverse block corresponding to this
pair of CoordBase instances using the spatial operator
decomposition of the mass matrix inverse.
This method is used by the
Algorithms::evalTreeMassMatrixInverse() method to compute
the mass matrix inverse. It is also used to compute part of
the Gamma matrix for the TA constrained dynamics
algorithm.
@param rcb the row CoordBase instance
@param ccb the column CoordBase instance
@return the mass matrix inverse block
*/
km::Mat _getMinvBlock(CoordBase &rcb, CoordBase &ccb);
/** @brief Return the mass matrix/OSCM cross-coupling block from the
subhinge to the pnode.
This is used to create the extended
OSCM matrix for computing Gamma for TA constrained
dynamics.
@param rcb the row CoordBase instance
@param pnode the column pnode
@return the mass matrix inverse block
*/
km::Mat _getMinvBlock(CoordBase &rcb, HingePnode &pnode);
/** @brief Return the cross-matrix block in the extended OSCM for the
subhinges and frames involved in coordinate/loop constraints
for the Gamma computation
@param body_frames the body frames for the list of body nodes
@param bdnds the list of body nodes
@param shgs the list of physical subhinges
@return the mass matrix inverse block
*/
km::Mat _getMinvOSCMBlock(const std::vector<kc::ks_ptr<kf::Frame>> &body_frames,
const std::vector<kc::ks_ptr<Node>> &bdnds,
const std::vector<kc::ks_ptr<PhysicalSubhinge>> &shgs);
/**
* @brief Clear out the external spatial forces, gravity accel and
gravity gradient values for all bodies, and all generalized
accels and forces.
This is used by the StatePropagator within
its deriv method before calling the model methods.
*/
virtual void _clearExternals();
protected:
/**
* @brief - Discard the provided SubTree.
* @param base - Base pointer to the SubTree to discard.
*/
void _discard(kc::ks_ptr<Base> &base) override;
/** @brief Return the Phi() data cache for the CoordBase pair.
This method requires that the left CoordBase to be an
ancestor of the right CoordBase
@param left the row CoordBase instance
@param right the column CoordBase instance
@param cd the container CoordData for the left/right CoordBase pair
@return the Phi data cache
*/
const kc::ks_ptr<kc::DataCache<km::Mat>> &_lookupOrCreateCoordBasePairPhiCache(
const CoordBase &left, const CoordBase &right, const CoordData &cd);
/**
* @brief Track usage for the CoordBase pair Phi data cache
*
* @param key the id pair key
* @param phi_cache the Phi() data cache
*/
void _trackUsageCoordBasePhiCache(const std::pair<kc::id_t, kc::id_t> &key,
const kc::ks_ptr<kc::DataCache<km::Mat>> &phi_cache);
/** @brief Return the data cache for the regular psi = tauper * phi.
This method
requires that the left CoordBase to be an ancestor of the
right CoordBase
@param left the row CoordBase instance
@param right the column CoordBase instance
@param cd the container CoordData for the left/right CoordBase pair
@return the Psi data cache
*/
const kc::ks_ptr<kc::DataCache<km::Mat>> &_lookupOrCreateCoordBasePairPsiCache(
const CoordBase &left, const CoordBase &right, const CoordData &cd);
/**
* @brief Track usage for the CoordBase pair Psi data cache
*
* @param key the id pair key
* @param psi_cache the Psi() data cache
*/
void _trackUsageCoordBasePsiCache(const std::pair<kc::id_t, kc::id_t> &key,
const kc::ks_ptr<kc::DataCache<km::Mat>> &psi_cache);
/** @brief Return the data cache for the oframe variant psi = tauper * phi
This method requires that the left CoordBase to be an ancestor of the right CoordBase
@param left the row CoordBase instance
@param right the column CoordBase instance
@param cd the container CoordData for the left/right CoordBase pair
@return the Psi data cache
*/
const kc::ks_ptr<kc::DataCache<km::Mat>> &_lookupOrCreateCoordBasePairOframePsiCache(
const CoordBase &left, const CoordBase &right, const CoordData &cd);
void
/**
* @brief Track usage for the CoordBase pair oframe relative Psi data cache
*
* @param key the id pair key
* @param psi_cache the Psi() data cache
*/
_trackUsageCoordBaseOframePsiCache(const std::pair<kc::id_t, kc::id_t> &key,
const kc::ks_ptr<kc::DataCache<km::Mat>> &psi_cache);
/** @brief Data cache callback for the pnode referenced psi = tauper * phi
This method requires that the left body be an ancestor of the right body
@param val the data buffer for the computed value
@param left the row body instance
@param right the column body instance
*/
void _computeBodyPairPnodePsi(km::Mat &val, const BodyBase &left, const BodyBase &right);
/** @brief Data cache callback for the body referenced psi = tauper * phi
This method requires that the left body be an ancestor of the right body
@param val the data buffer for the computed value
@param left the row body instance
@param right the column body instance
*/
void _computeBodyPairPsi(km::Mat &val, const BodyBase &left, const BodyBase &right);
/** @brief Data cache callback for the body referenced phi
This method requires that the left body be an ancestor of the right body
@param val the data buffer for the computed value
@param left the row body instance
@param right the column body instance
*/
void _computeBodyPairPhi(km::Mat &val, const BodyBase &left, const BodyBase &right);
/** @brief Data cache callback for the CoordBase pair phi
This method requires that the left CoordBase be an ancestor of the right CoordBase
@param val the data buffer for the computed value
@param left the row CoordBase instance
@param right the column CoordBase instance
@param cd the container CoordData for the left/right CoordBase pair
*/
void _computeCoordBasePairPhi(km::Mat &val,
const CoordBase &left,
const CoordBase &right,
const CoordData &cd);
/** @brief Data cache callback for the for the regular psi = tauper * phi
This method requires that the left CoordBase be an ancestor
of the right CoordBase
@param val the data buffer for the computed value
@param left the row CoordBase instance
@param right the column CoordBase instance
@param cd the container CoordData for the left/right CoordBase pair
*/
void _computeCoordBasePairPsi(km::Mat &val,
const CoordBase &left,
const CoordBase &right,
const CoordData &cd);
/** Data cache callback for the oframe variant psi = tauper * phi.
This method requires that the left CoordBase be an ancestor of
the right CoordBase
@param val the data buffer for the computed value
@param left the row CoordBase instance
@param right the column CoordBase instance
@param cd the container CoordData for the left/right CoordBase pair
*/
void _computeCoordBasePairOframePsi(km::Mat &val,
const CoordBase &left,
const CoordBase &right,
const CoordData &cd);
/**
* @brief Helper method to display the tree structure of the bodies
*
* @param prefix the string prefix for each output line @param
* options the dump options @param extras map with extra
* information for specific bodies
*/
void _dumpTree(
std::string_view prefix = "",
const DumpTreeOptions options = DumpTreeOptions(false, false, false, false, false),
std::map<kc::id_t, std::string> extras = {}) const;
protected:
/**
* @brief Track usage for the compound body
*
* @param bd the compound body
*/
void _trackUsageCompoundBody(kc::ks_ptr<CompoundBody> bd);
/** set up the _tree member */
void _setupTree();
/** Update cached list of subhinges that have coordinate offset
requirements (e.g., spherical subhinges) */
void _setupCoordSanitizationSubhinges();
/** Tear down the internal topology _tree for the SubTree. While
this method can only be called multiple times for the
multibody SubTree to handle changes in configuration, it can
only be called once for a lower level, nested SubTree.
*/
virtual void _makeNotHealthy() override;
/**
* @brief Discard all of the SubTrees. This is recursive, so will discard
* all SubTrees of all children as well.
*/
void _discardAllSubTrees();
/**
* @brief Internally used method to discard the cm frame.
*/
virtual void _discardCmFrame();
/** @brief Return a filtered list of child onodes whose parent bodies
belong to this sub-tree.
This list is used in the gather sweeps for this sub-tree
@param onodes input list of onodes
@return the filtered list of onodes
*/
std::vector<kc::ks_ptr<HingeOnode>>
_filteredChildOnodes(const kc::RegistryList<Karana::Dynamics::HingeOnode> &onodes) const;
/**
* @brief Registers an existing body with the container.
* @param body The body to register.
*/
virtual void _registerBody(const kc::ks_ptr<BodyBase> &body);
/**
* @brief Unregister a body from the bodies SubTree.
*
* Note that this method can only be called for the top-level
* multibody SubTree to support configuration changes. All other
* SubTrees will need to discarded and recreated to handle
* topology changes.
*
* @param body The body to unregister.
*/
virtual void _unregisterBody(const kc::ks_ptr<BodyBase> &body);
/** @brief Return true if this sub tree and other other tree do not
share any physical bodies
@param other the other SubTree instance
@return true if the sub-trees do not share any physical bodies
*/
bool _areDisjoint(const SubTree &other) const;
/** @brief Helper method used to set up a new child SubTree
@param child_subtree the new child sub-tree
@param branch_ids the branches with the specified body ids to include
@param stop_at list of body ids to stop including bodies beyond
*/
void _populateNewChildTreeWithBodies(kc::ks_ptr<SubTree> child_subtree,
const std::vector<kc::id_t> &branch_ids,
const std::vector<kc::ks_ptr<BodyBase>> &stop_at);
/** @brief Helper method to created sorted physical bodies list in
_sorted_physical_bodies */
void _setupSortedPhysicalBodiesList();
/** @brief Helper method to topologically sort the SubTree's bodies to
populate _sorted_bodies list */
void _setupSortedBodiesList();
/**
* @brief Data cache callback to compute the sub-tree's CM frame's pose
*
* @param val the data buffer for the computed value
*/
void _computeCMPose(km::HomTran &val);
/**
* @brief Data cache callback to compute the sub-tree's CM frame's spatial velocity
*
* @param val the data buffer for the computed value
*/
void _computeCMSpVel(km::SpatialVector &val);
/**
* @brief Data cache callback to compute the sub-tree's CM frame's spatial acceleration
*
* @param val the data buffer for the computed value
*/
void _computeCMSpAccel(km::SpatialVector &val);
/**
* @brief Return the OSCM matrix for the list of body attached frames
*
* The returned matrix is square and symmetric and of 6xnframes
* row/column size. Each frame in the input list is required to
* be downstream of a body. The bdnds list contains the ancestor
* node for each frame. The bbds contains the list of ancestor
* bodies - with duplicates removed.
*
* @param body_frames the input list of body frames
* @param bdnds the list of ancestor body nodes for the frames (same size as body_frames)
* @param bbds uniquified list of ancestor parents bodies for the frames
* @return the frames OSCM matrix
*/
km::Mat _framesOSCM(const std::vector<kc::ks_ptr<kf::Frame>> &body_frames,
const std::vector<kc::ks_ptr<Node>> &bdnds,
const std::vector<kc::ks_ptr<BodyBase>> &bbds);
/**
* @brief Return the extended OSCM matrix for the list of body attached frames and subhinges
*
* The returned matrix is square and symmetric. Each frame in
* the input list is required to be downstream of a body. The
* bdnds list contains the ancestor node for each frame. The
* bbds contains the list of ancestor bodies - with duplicates
* removed. Additional rows and columns from the mass matrix
* inverse and cross-terms are included for the specified
* subhinges .
*
* @param body_frames the input list of body frames
* @param bdnds the list of ancestor body nodes for the frames (same size as body_frames)
* @param bbds uniquified list of ancestor parents bodies for the frames
* @param shgs list of subhinges
* @param shg_dofs the number of degrees of freedom in the list of subhinges
* @return the extended OSCM matrix
*/
km::Mat _framesSubhingesOSCM(const std::vector<kc::ks_ptr<kf::Frame>> &body_frames,
const std::vector<kc::ks_ptr<Node>> &bdnds,
const std::vector<kc::ks_ptr<BodyBase>> &bbds,
const std::vector<kc::ks_ptr<PhysicalSubhinge>> &shgs = {},
size_t shg_dofs = 0);
/** @brief Return any contained subhinge that has prescribed motion enabled.
*
* This method is handy to check for the presence of prescribed subhinges within compound
* bodies - which are currently not allowed.
*
* @return a subhinge with prescribed motion enabled, or nullptr if no subhinge has
* prescribed motion enabled
*/
kc::ks_ptr<PhysicalSubhinge> _hasPrescribedSubhinge() const;
protected:
/// the parent multibody instance
kc::ks_ptr<Multibody> _multibody;
/// Virtual root for the SubTree
kc::ks_ptr<BodyBase> _virtual_root_body = nullptr;
/// unsorted list of bodies in the SubTree
kc::RegistryList<BodyBase> _unsorted_bodies_list;
/// topologically sorted list of bodies in the SubTree populated
/// by _setupSortedBodiesList
std::vector<kc::ks_ptr<BodyBase>> _sorted_bodies;
/// parent SubTree for this SubTree (null for multibody level)
kc::ks_ptr<SubTree> _parent_subtree = nullptr;
/** the center of mass frame for the SubTree. It is attached to
the physical root body for the sub-tree. Its pose,
spatial-velocity are updated with respect to this root body
as oframe. The edge spatial acceleration is left
undefined */
kc::ks_ptr<kf::Frame> _cm_frame = nullptr;
/// CoordData instance to track the coordinates across the
/// component subhinges in this SubTree
kc::ks_ptr<CoordData> _subhinge_coord_data = nullptr;
/// CoordData instance to track the coordinates across the
/// component bodies in this SubTree
kc::ks_ptr<CoordData> _body_coord_data = nullptr;
/// merged CoordData instance with the subhinge and body
/// coordinates
mutable kc::ks_ptr<CoordData> _tree_coord_data = nullptr;
/// merged CoordData instance with the subhinge and cut-joint 6constraint
/// coordinates (used by compound subhinges)
mutable kc::ks_ptr<CoordData> _articulation_coord_data = nullptr;
protected:
/// Usage map for children SubTrees
kc::UsageTrackingMap<kc::id_t, SubTree> _child_subtrees_usage_map;
/// Usage map for compound bodies within the SubGraph
kc::UsageTrackingMap<kc::id_t, CompoundBody> _compound_bodies_usage_map;
/// Usage map for the CoordBase pair Phi data caches
kc::UsageTrackingMap<std::pair<kc::id_t, kc::id_t>, kc::DataCache<km::Mat>>
_coordbase_phi_cache_usage_map;
/// Usage map for the CoordBase pair regular psi variant = phi * tauper
kc::UsageTrackingMap<std::pair<kc::id_t, kc::id_t>, kc::DataCache<km::Mat>>
_coordbase_psi_cache_usage_map;
/// for the oframe psi variant = tauper * phi
kc::UsageTrackingMap<std::pair<kc::id_t, kc::id_t>, kc::DataCache<km::Mat>>
_coordbase_oframe_psi_cache_usage_map;
/** map to keep track of the parent body for the SubTree
bodies. This information is used to populate the Tree
instance. For the physical multibody, this map is strictly
defined by the physical subhinge connecting a body to its
parent. However when there are compound bodies, the
relationship may no longer be based on physical hinges. The
key is the id of a body, while the value is the
parent body in this SubTree. */
std::map<kc::id_t, kc::ks_ptr<BodyBase>> _body2parent_map;
/** the list of base bodies for the sub-tree */
std::vector<kc::ks_ptr<BodyBase>> _base_bodies;
/** the list of leaf bodies for the sub-tree */
std::vector<kc::ks_ptr<BodyBase>> _leaf_bodies;
/** sorted list of physical bodies created by _setupSortedPhysicalBodiesList */
std::vector<kc::ks_ptr<PhysicalBody>> _sorted_physical_bodies;
/** List of subhinges coordinates sanitization needs (e.g., SphericalSubhinge type) */
std::vector<kc::ks_ptr<PhysicalSubhinge>> _coord_sanitization_subhinges;
/** the tree instance for the body ids in the sub-tree */
mutable std::unique_ptr<const kc::Tree<kc::id_t>> _tree;
/** flag specifying whether the sub-tree contains any compound bodies */
bool _has_compound_bodies = false;
/** whether the SubTree has been set up for calling
inverse/forward dynamics algorithms that use gather/scatter
recursions. */
bool _enabled_algorthmic = false;
/** whether coordinates sanitization has been enabled or not for the SubTree. Usually we
* want this on when doing time domain simulations, and off when solving for coordinate
* solutions using the CK solver. */
bool _enabled_subhinge_charts = true;
/** prefix used by dumpDynamics() if none is specified */
std::string _dump_dynamics_prefix = "";
/// Boolean that indicates whether a state reset is needed due to configuration changes
NeedsStateReset _needs_state_reset = NeedsStateReset::NUM_STATES_CHANGED;
/// Keep track of why the state has been reset
std::vector<std::string> _why_state_needs_reset{"SubTree created."};
};
} // namespace Karana::Dynamics