Class SubTree

• Defined in File SubTree.h

Nested Relationships

Nested Types

• Struct SubTree::DumpTreeOptions

Inheritance Relationships

Base Type

• public Karana::Core::LockingBase (Class LockingBase)

Derived Type

• public Karana::Dynamics::SubGraph (Class SubGraph)

Class Documentation

class SubTree : public Karana::Core::LockingBase

Represents a SubTree with connected bodies.

This class is for a sub-tree of connected bodies

See the Physical bodies and hinges , Creating a tree multibody system , and

Sub-Trees sections for more discussion related to the SubTree class.

Subclassed by Karana::Dynamics::SubGraph

Public Functions

SubTree(std::string_view name, const kc::ks_ptr<BodyBase> &root, const kc::ks_ptr<SubTree> &parent_subtree = nullptr)

Constructs a SubTree.

Parameters:

• name – The name for the new SubTree

• root – The virtual root body for the new SubTree

• parent_subtree – The parent SubTree for the new SubTree

virtual ~SubTree()

SubTree destructor.

inline virtual bool isSubGraph() const

Method to check whether this is a SubTree or a SubGraph.

Returns:

False if this is a SubTree and not a SubGraph

inline virtual const kc::ks_ptr<BodyBase> &virtualRoot() const

Returns the virtual root BodyBase body for the tree.

Returns:

The virtual root body.

const kc::ks_ptr<BodyBase> &parentBody(const BodyBase &body) const

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.

Parameters:

body – the specified body

Returns:

the parent body

const std::vector<kc::ks_ptr<BodyBase>> &baseBodies() const

Return the list of BodyBase base bodies for the SubTree.

Base bodies are the immediate children of the SubTree’s virtual root body.

Returns:

the list of BodyBase base bodies

const std::vector<kc::ks_ptr<BodyBase>> &leafBodies() const

Return the list of BodyBase leaf bodies for the SubTree.

Leaf bodies are the bodies with children in the SubTree

Returns:

the list of leaf BodyBase bodies

kc::ks_ptr<BodyBase> ancestorBody(const BodyBase &bd, const BodyBase &bd1) const

Return the common ancestor body for the pair of BodyBase bodies in this SubTree.

Parameters:

• bd – the first body

• bd1 – the other body

Returns:

the common ancestor BodyBase body

bool containsBody(const BodyBase &bd) const

Return true if the specified BodyBase body belongs to the SubTree or to any of its nested hierarchy of SubTrees.

Parameters:

bd – the input body

Returns:

true if the body belongs to the SubTree

bool hasBody(const BodyBase &bd) const

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.

Parameters:

bd – the input body

Returns:

true if the body belongs to the SubTree

kc::ks_ptr<BodyBase> getBody(std::string_view name) const

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.

Parameters:

name – the body’s name

Returns:

the BodyBase body instance

std::vector<kc::ks_ptr<BodyBase>> getBodies(std::string_view name) const

Look up BodyBase bodies in the SubTree with the specified name.

Parameters:

name – the body name

Returns:

the list of BodyBase body instances

kc::ks_ptr<CompoundBody> getContainingCompoundBody(BodyBase &bd) const

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.

Parameters:

bd – the input body

Returns:

the CompoundBody instance containing the body

std::vector<kc::ks_ptr<BodyBase>> childrenBodies(const BodyBase &bd) const

Return the list of BodyBase child bodies for a body for this subtree.

The list may include physical and compound bodies.

Parameters:

bd – the input body

Returns:

list of BodyBase children bodies

bool isBaseBody(const BodyBase &bd) const

Return true if the BodyBase body is a base body for this sub-tree.

Parameters:

bd – the input body

Returns:

true, if the body is a base body in the SubTree

size_t numBodies() const

Return the number of bodies in the tree.

Returns:

the number of bodies

virtual const std::pair<kc::ks_ptr<CoordBase>, size_t> coordAt(size_t u_offset) const

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.

Parameters:

u_offset – the input overall U offset value

Returns:

The CoordBase instance and its local U coord offset

CoordData::CoordOffset coordOffsets(const CoordBase &c) const

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.

Parameters:

c – The CoordBase to check for

Returns:

A CoordOffset struct with the Q and U offsets

size_t nQ() const

The number of Q generalized coords for the CoordBase in the SubTree.

See Generalized Q, U etc coordinates section for more on Q, U etc generalized coordinates.

Returns:

the number of coordinates

size_t nU() const

The number of U velocity coords for the CoordBase.

See Generalized Q, U etc coordinates section for more on Q, U etc generalized coordinates.

Returns:

the number of velocity coordinates

km::Vec getQ() const

Return the Q coordinates as an array.

See Generalized Q, U etc coordinates section for more on Q, U etc generalized coordinates.

Returns:

Array of values

void setQ(const Eigen::Ref<const km::Vec> &Q)

Set the Q coordinates.

See Generalized Q, U etc coordinates section for more on Q, U etc generalized coordinates.

Parameters:

Q – Array of values.

void setQ(double fill_value)

Set the Q coordinates to a constant value.

See Generalized Q, U etc coordinates section for more on Q, U etc generalized coordinates.

Parameters:

fill_value – Fill value.

km::Vec getQdot() const

Return the Qdot rate coordinates as an array.

Returns:

Array of values

km::Vec getU() const

Return the U velocity coordinates as an array.

See Generalized Q, U etc coordinates section for more on Q, U etc generalized coordinates.

Returns:

Array of values

void setU(const Eigen::Ref<const km::Vec> &U)

Set the U velocity coordinates.

See Generalized Q, U etc coordinates section for more on Q, U etc generalized coordinates.

Parameters:

U – Array of values.

void setU(double fill_value)

Set the U velocity coordinates to a constant value.

See Generalized Q, U etc coordinates section for more on Q, U etc generalized coordinates.

Parameters:

fill_value – Fill value.

km::Vec getUdot() const

Return the Udot acceleration coordinates as an array.

See Generalized Q, U etc coordinates section for more on Q, U etc generalized coordinates.

Returns:

Array of values

void setUdot(const Eigen::Ref<const km::Vec> &Udot)

Set the Udot acceleration coordinates.

See Generalized Q, U etc coordinates section for more on Q, U etc generalized coordinates.

Parameters:

Udot – Array of values.

void setUdot(double fill_value)

Set the Udot acceleration coordinates to a constant value.

See Generalized Q, U etc coordinates section for more on Q, U etc generalized coordinates.

Parameters:

fill_value – Fill value.

km::Vec getT() const

Return the T generalized forces as an array.

Returns:

Array of values

void setT(const Eigen::Ref<const km::Vec> &T)

Set the T generalized forces.

See Generalized Q, U etc coordinates section for more on Q, U etc generalized coordinates.

Parameters:

T – Array of values.

void setT(double fill_value)

Set the U velocity coordinates to a constant value.

See Generalized Q, U etc coordinates section for more on Q, U etc generalized coordinates.

Parameters:

fill_value – Fill value.

inline const kc::ks_ptr<Multibody> &multibody() const

Return the parent Multibody instance.

Returns:

the Multibody instance

km::Mat bodyPnodesOSCM(const std::vector<kc::ks_ptr<BodyBase>> &bodies)

Return the OSCM matrix for the pnodes for the specified bodies.

The returned matrix is square and symmetric and of 6xnbodies row/column size

Parameters:

bodies – the input list of bodies

Returns:

the pnodes OSCM matrix

km::Mat bodyOSCM(const std::vector<kc::ks_ptr<BodyBase>> &bodies)

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.

Parameters:

bodies – the input list of bodies

Returns:

the bodies OSCM matrix

km::Mat framesOSCM(const std::vector<kc::ks_ptr<kf::Frame>> &body_frames, const std::vector<kc::ks_ptr<PhysicalSubhinge>> &subhinges = {})

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.

Parameters:

• body_frames – the input list of body frames

• subhinges – the input list of body frames

Returns:

the frames OSCM matrix

virtual void showState(const km::Vec &x)

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 System state section for more on the state vector, and the The state propagator for the StatePropagator class.

Parameters:

x – The state vector

virtual void enableSubhingeCharts(bool flag)

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.

Parameters:

flag – flag to enable/disable charts

inline bool subhingeChartsEnabled() const

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

virtual std::string dumpString(std::string_view prefix = "", const Karana::Core::Base::DumpOptions *options = nullptr) const override

Return a formatted string containing information about this object.

Parameters:

• prefix – String prefix to use for formatting.

• options – Dump options (if null, defaults will be used).

Returns:

A string representation of the object.

virtual void displayModel(std::string_view prefix = "") const

Display information about the SubTree and its bodies.

Parameters:

prefix – the prefix for each line in the output

virtual void dumpTree(std::string_view prefix = "", const DumpTreeOptions options = DumpTreeOptions(true, false, false, false, false)) const

Display the body tree. The contents of the options struct can be used to control the content and verbosity of the displayed output.

Parameters:

• prefix – the prefix for each line in the output

• options – Options to tailor the output of dumpTree

const kc::ks_ptr<CoordData> &subhingeCoordData() const

Return the CoordData for the subhinges See.

The CoordData container for more on the CoordData class.

Returns:

the CoordData for the subhinges

inline const kc::ks_ptr<CoordData> bodyCoordData() const

Return the CoordData for the body deformation coords See.

The CoordData container for more on the CoordData class.

Returns:

the CoordData for the body deformation coords

virtual const kc::ks_ptr<CoordData> treeCoordData() const

Return the subhinge+body tree CoordData for the SubTree.

This CoordData is a combination of the subhinge and body CoordDatas

See The CoordData container for more on the CoordData class.

Returns:

the CoordData for the tree

virtual const kc::ks_ptr<CoordData> articulationCoordData() const

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 The CoordData container for more on the CoordData class.

Returns:

the articulation CoordData for the tree

inline bool hasCompoundBodies() const

Return true if the SubTree contains any CompoundBody instances.

Returns:

true if the SubTree has compound bodies

const std::vector<kc::ks_ptr<PhysicalBody>> &sortedPhysicalBodiesList() const

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.

Returns:

the list of PhysicalBody instances

const std::vector<kc::ks_ptr<BodyBase>> &sortedBodiesList() const

Compute and return the sorted list of bodies in the tree. This will be a mix of PhysicalBody and CompoundBody instances.

Returns:

A vector containing the bodies of the SubTree.

void enableAlgorithmicUse()

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 Dynamics Computations section for more on the available SubTree level computational algorithms.

void disableAlgorithmicUse()

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 Dynamics Computations section for more on the available SubTree level computational algorithms.

void setUniformGravAccel(const km::Vec3 &g, kc::ks_ptr<kf::Frame> ref_frame = nullptr)

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.

Parameters:

• g – the gravity accel vector

• ref_frame – the Frame reference frame

void accumUniformGravAccel(const km::Vec3 &g, kc::ks_ptr<kf::Frame> ref_frame = nullptr)

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.

Parameters:

• g – the gravity accel vector

• ref_frame – the reference frame

inline const kc::ks_ptr<kf::Frame> &cmFrame() const

Return the SubTree’s center of mass (CM) Karana::Frame::Frame instance.

Returns:

the CM frame Frame instance

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

Sequentially articulate a subhinge for a physical body.

With graphics on, this step can be used to examine and debug issues with the model.

Parameters:

• shg – subhinge to articulate

• subhinge_index – the index of the coordinate element to articulate

• range_q – is the excursion angle range (in radians)

• d_q – is the angle step size (in radians)

• pause – time in seconds to sleep between articulation steps

• cb – callback to invoke every time the Q coordinate is changed

void articulateBodies(double range_q = .5, double d_q = .01, double pause = .01) const

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.

Parameters:

• range_q – is the excursion angle range (in radians)

• d_q – is the angle step size (in radians)

• pause – time in seconds to sleep between articulation steps

bool sanitizedCoords()

Call sanitizeCoords() on all SubTree subhinges with coordinates sanitization needs.

Return true if any of them actually did a sanitization action.

Returns:

true if any of the subhinges had to sanitize their coordinates

virtual void resetData()

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.

km::Mat66 crossUpsilonMatrix(const HingePnode &left, const HingePnode &right)

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.

Parameters:

• left – the pnode on the left

• right – the pnode on the right

Returns:

the cross Upsilon(left, right) matrix

km::Mat getCrossUpsilonMatrix(const PhysicalBody &lbd, const PhysicalBody &rbd)

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.

Parameters:

• lbd – the left-hand body for the cross-OSCM Upsilon value

• rbd – the right-hand body for the cross-OSCM Upsilon value

Returns:

the cross-OSCM Upsilon matrix

km::Mat getBodyPairPnodePsiMatrix(const BodyBase &left, const BodyBase &right)

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.

Parameters:

• left – the body on the left

• right – the body on the right

Returns:

the psi(left.pnode(), right.pnode()) matrix

km::Mat getBodyPairPsiMatrix(const BodyBase &left, const BodyBase &right)

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.

Parameters:

• left – the body on the left

• right – the body on the right

Returns:

the psi(left, right) matrix for the body frame to body frame

km::Mat getBodyPairPhiMatrix(const BodyBase &left, const BodyBase &right)

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.

Parameters:

• left – the body on the left

• right – the body on the right

Returns:

the phi(left, right) matrix

const km::Mat &getCoordBasePairPsiMatrix(const CoordBase &left, const CoordBase &right)

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.

Parameters:

• left – the CoordBase on the left

• right – the CoordBase on the right

Returns:

the psi(left, right) matrix

const km::Mat &getCoordBasePairOframePsiMatrix(const CoordBase &left, const CoordBase &right)

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.

Parameters:

• left – the CoordBase on the left

• right – the CoordBase on the right

Returns:

the psi(left, right) matrix

const km::Mat &getCoordBasePairPhiMatrix(const CoordBase &left, const CoordBase &right)

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.

Parameters:

• left – the CoordBase on the left

• right – the CoordBase on the right

Returns:

the 6x6 phi(left, right) matrix

void dumpDynamics(std::string_view prefix = "") const

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.

Parameters:

prefix – the prefix for each line in the output

Public Members

bool enable_dump_dynamics = false

Flag to turn dumpDynamics calls on or off when the tree forward dynamics are computed.

Public Static Functions

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 = {})

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.

Parameters:

• parent_subtree – The parent SubTree for the new SubTree

• name – The name for the new SubTree

• new_root – The virtual root body for the new SubTree

• use_branches – The branches to get bodies from

• stop_at – list of last bodies on a branch

Returns:

A new SubTree instance

Protected Types

enum class NeedsStateReset

A class used to tell whether the SubTree needs a state reset or not.

Values:

enumerator NO_RESET_NEEDED

No reset needed. States are okay.

enumerator NUMERICS_CHANGED

The numerics have changed, but the number of states remains the same.

enumerator NUM_STATES_CHANGED

The number of states has changed.

Protected Functions

virtual void _stateToDynamics(const km::Vec &x, bool global = false)

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 System state section for more on the state vector, and the The state propagator for the StatePropagator class.

Parameters:

• x – The state to copy into the SubTree.

• global – if true, x contains global chart coordinate values

virtual void _dynamicsToState(Eigen::Ref<km::Vec> x, bool global = false) const

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 System state section for more on the state vector, and the The state propagator for the StatePropagator class.

Parameters:

• x – The vector x to copy the state into.

• global – Use global coordinates if true, local coordinates otherwise.

virtual km::Vec _dynamicsToState(bool global = false) const

Return a vector of SubTree’s generalized and velocity coordinates.

The state vector is typically associated with the StatePropagator state. See System state section for more on the state vector, and the The state propagator for the StatePropagator class.

Parameters:

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 void _dynamicsToStateDeriv(Eigen::Ref<km::Vec> dx) const

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 System state section for more on the state vector, and the The state propagator for the StatePropagator class.

Parameters:

dx – The vector dx to copy the state derivative into.

virtual km::Vec _dynamicsToStateDeriv() const

Return a vector of SubTree’s velocity and acceleration coordinates.

The state vector is typically associated with the StatePropagator state. See System state section for more on the state vector, and the The state propagator 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 std::vector<kc::ks_ptr<CoordData>> _coordDataList() const

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 The CoordData container for more on the CoordData class.

Returns:

the list of CoordData

void _localChartSetQ(const Eigen::Ref<const km::Vec> &val)

Set the local chart Q coordinates.

See Generalized Q, U etc coordinates section for discussion on Q, U etc coordinates.

Parameters:

val – Array of values.

km::Vec _localChartGetQ() const

Return the local chart Q coordinates - without sanitizing coordinates.

See Generalized Q, U etc coordinates section for discussion on Q, U etc coordinates.

Returns:

Array of values.

virtual void _resetSubhingeCharts()

Reset all physical subhinge chart offsets

virtual CoordData::CoordOffset _coordOffsets(kc::id_t id) const

Return the CoordOffset struct with offsets for the CoordBase with specified id in the subgraph’s treeCoordData.

Parameters:

id – the CoordBase id

Returns:

the CoordOffset struct

km::Mat _getCrossUpsilonMatrix(CoordBase &lsh, CoordBase &rsh)

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.

Parameters:

• lsh – the CoordBase on the left

• rsh – the CoordBase on the right

Returns:

the cross Upsilon(left, right) matrix

km::Mat _getMinvBlock(const std::vector<kc::ks_ptr<CoordBase>> &cbs)

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.

Parameters:

cbs – the list of CoordBase instances

Returns:

the mass matrix inverse block

km::Mat _getMinvBlock(CoordBase &rcb, CoordBase &ccb)

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.

Parameters:

• rcb – the row CoordBase instance

• ccb – the column CoordBase instance

Returns:

the mass matrix inverse block

km::Mat _getMinvBlock(CoordBase &rcb, HingePnode &pnode)

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.

Parameters:

• rcb – the row CoordBase instance

• pnode – the column pnode

Returns:

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)

Return the cross-matrix block in the extended OSCM for the subhinges and frames involved in coordinate/loop constraints for the Gamma computation.

Parameters:

• body_frames – the body frames for the list of body nodes

• bdnds – the list of body nodes

• shgs – the list of physical subhinges

Returns:

the mass matrix inverse block

virtual void _clearExternals()

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.

void _discard(kc::ks_ptr<Base> &base) override

• Discard the provided SubTree.

Parameters:

base – - Base pointer to the SubTree to discard.

const kc::ks_ptr<kc::DataCache<km::Mat>> &_lookupOrCreateCoordBasePairPhiCache(const CoordBase &left, const CoordBase &right, const CoordData &cd)

Return the Phi() data cache for the CoordBase pair.

This method requires that the left CoordBase to be an ancestor of the right CoordBase

Parameters:

• left – the row CoordBase instance

• right – the column CoordBase instance

• cd – the container CoordData for the left/right CoordBase pair

Returns:

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)

Track usage for the CoordBase pair Phi data cache.

Parameters:

• key – the id pair key

• phi_cache – the Phi() data cache

const kc::ks_ptr<kc::DataCache<km::Mat>> &_lookupOrCreateCoordBasePairPsiCache(const CoordBase &left, const CoordBase &right, const CoordData &cd)

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

Parameters:

• left – the row CoordBase instance

• right – the column CoordBase instance

• cd – the container CoordData for the left/right CoordBase pair

Returns:

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)

Track usage for the CoordBase pair Psi data cache.

Parameters:

• key – the id pair key

• psi_cache – the Psi() data cache

const kc::ks_ptr<kc::DataCache<km::Mat>> &_lookupOrCreateCoordBasePairOframePsiCache(const CoordBase &left, const CoordBase &right, const CoordData &cd)

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

Parameters:

• left – the row CoordBase instance

• right – the column CoordBase instance

• cd – the container CoordData for the left/right CoordBase pair

Returns:

the Psi data cache

void _trackUsageCoordBaseOframePsiCache(const std::pair<kc::id_t, kc::id_t> &key, const kc::ks_ptr<kc::DataCache<km::Mat>> &psi_cache)

Track usage for the CoordBase pair oframe relative Psi data cache.

Parameters:

• key – the id pair key

• psi_cache – the Psi() data cache

void _computeBodyPairPnodePsi(km::Mat &val, const BodyBase &left, const BodyBase &right)

Data cache callback for the pnode referenced psi = tauper * phi.

This method requires that the left body be an ancestor of the right body

Parameters:

• val – the data buffer for the computed value

• left – the row body instance

• right – the column body instance

void _computeBodyPairPsi(km::Mat &val, const BodyBase &left, const BodyBase &right)

Data cache callback for the body referenced psi = tauper * phi.

This method requires that the left body be an ancestor of the right body

Parameters:

• val – the data buffer for the computed value

• left – the row body instance

• right – the column body instance

void _computeBodyPairPhi(km::Mat &val, const BodyBase &left, const BodyBase &right)

Data cache callback for the body referenced phi.

This method requires that the left body be an ancestor of the right body

Parameters:

• val – the data buffer for the computed value

• left – the row body instance

• right – the column body instance

void _computeCoordBasePairPhi(km::Mat &val, const CoordBase &left, const CoordBase &right, const CoordData &cd)

Data cache callback for the CoordBase pair phi.

This method requires that the left CoordBase be an ancestor of the right CoordBase

Parameters:

• val – the data buffer for the computed value

• left – the row CoordBase instance

• right – the column CoordBase instance

• 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 for the regular psi = tauper * phi.

This method requires that the left CoordBase be an ancestor of the right CoordBase

Parameters:

• val – the data buffer for the computed value

• left – the row CoordBase instance

• right – the column CoordBase instance

• cd – the container CoordData for the left/right CoordBase pair

void _computeCoordBasePairOframePsi(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

Parameters:

• val – the data buffer for the computed value

• left – the row CoordBase instance

• right – the column CoordBase instance

• cd – the container CoordData for the left/right CoordBase pair

void _dumpTree(std::string_view prefix = "", const DumpTreeOptions options = DumpTreeOptions(false, false, false, false, false), std::map<kc::id_t, std::string> extras = {}) const

Helper method to display the tree structure of the bodies.

Parameters:

• prefix – the string prefix for each output line

• options – the dump options

• extras – map with extra information for specific bodies

void _trackUsageCompoundBody(kc::ks_ptr<CompoundBody> bd)

Track usage for the compound body.

Parameters:

bd – the compound body

void _setupTree()

set up the _tree member

void _setupCoordSanitizationSubhinges()

Update cached list of subhinges that have coordinate offset requirements (e.g., spherical subhinges)

virtual void _makeNotHealthy() override

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.

void _discardAllSubTrees()

Discard all of the SubTrees. This is recursive, so will discard all SubTrees of all children as well.

virtual void _discardCmFrame()

Internally used method to discard the cm frame.

std::vector<kc::ks_ptr<HingeOnode>> _filteredChildOnodes(const kc::RegistryList<Karana::Dynamics::HingeOnode> &onodes) const

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

Parameters:

onodes – input list of onodes

Returns:

the filtered list of onodes

virtual void _registerBody(const kc::ks_ptr<BodyBase> &body)

Registers an existing body with the container.

Parameters:

body – The body to register.

virtual void _unregisterBody(const kc::ks_ptr<BodyBase> &body)

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.

Parameters:

body – The body to unregister.

bool _areDisjoint(const SubTree &other) const

Return true if this sub tree and other other tree do not share any physical bodies.

Parameters:

other – the other SubTree instance

Returns:

true if the sub-trees do not share any physical bodies

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)

Helper method used to set up a new child SubTree.

Parameters:

• child_subtree – the new child sub-tree

• branch_ids – the branches with the specified body ids to include

• stop_at – list of body ids to stop including bodies beyond

void _setupSortedPhysicalBodiesList()

Helper method to created sorted physical bodies list in _sorted_physical_bodies.

void _setupSortedBodiesList()

Helper method to topologically sort the SubTree’s bodies to populate _sorted_bodies list.

void _computeCMPose(km::HomTran &val)

Data cache callback to compute the sub-tree’s CM frame’s pose.

Parameters:

val – the data buffer for the computed value

void _computeCMSpVel(km::SpatialVector &val)

Data cache callback to compute the sub-tree’s CM frame’s spatial velocity.

Parameters:

val – the data buffer for the computed value

void _computeCMSpAccel(km::SpatialVector &val)

Data cache callback to compute the sub-tree’s CM frame’s spatial acceleration.

Parameters:

val – the data buffer for the computed value

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)

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.

Parameters:

• body_frames – the input list of body frames

• bdnds – the list of ancestor body nodes for the frames (same size as body_frames)

• bbds – uniquified list of ancestor parents bodies for the frames

Returns:

the frames 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)

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 .

Parameters:

• body_frames – the input list of body frames

• bdnds – the list of ancestor body nodes for the frames (same size as body_frames)

• bbds – uniquified list of ancestor parents bodies for the frames

• shgs – list of subhinges

• shg_dofs – the number of degrees of freedom in the list of subhinges

Returns:

the extended OSCM matrix

Protected Attributes

kc::ks_ptr<Multibody> _multibody

the parent multibody instance

kc::ks_ptr<BodyBase> _virtual_root_body = nullptr

Virtual root for the SubTree.

kc::RegistryList<BodyBase> _unsorted_bodies_list

unsorted list of bodies in the SubTree

std::vector<kc::ks_ptr<BodyBase>> _sorted_bodies

topologically sorted list of bodies in the SubTree populated by _setupSortedBodiesList

kc::ks_ptr<SubTree> _parent_subtree = nullptr

parent SubTree for this SubTree (null for multibody level)

kc::ks_ptr<kf::Frame> _cm_frame = 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<CoordData> _subhinge_coord_data = nullptr

CoordData instance to track the coordinates across the component subhinges in this SubTree

kc::ks_ptr<CoordData> _body_coord_data = nullptr

CoordData instance to track the coordinates across the component bodies in this SubTree

mutable kc::ks_ptr<CoordData> _tree_coord_data = nullptr

merged CoordData instance with the subhinge and body coordinates

mutable kc::ks_ptr<CoordData> _articulation_coord_data = nullptr

merged CoordData instance with the subhinge and cut-joint 6constraint coordinates (used by compound subhinges)

kc::UsageTrackingMap<kc::id_t, SubTree> _child_subtrees_usage_map

Usage map for children SubTrees.

kc::UsageTrackingMap<kc::id_t, CompoundBody> _compound_bodies_usage_map

Usage map for compound bodies within the SubGraph.

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 Phi data caches.

kc::UsageTrackingMap<std::pair<kc::id_t, kc::id_t>, kc::DataCache<km::Mat>> _coordbase_psi_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_oframe_psi_cache_usage_map

for the oframe psi variant = tauper * phi

std::map<kc::id_t, kc::ks_ptr<BodyBase>> _body2parent_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::vector<kc::ks_ptr<BodyBase>> _base_bodies

the list of base bodies for the sub-tree

std::vector<kc::ks_ptr<BodyBase>> _leaf_bodies

the list of leaf bodies for the sub-tree

std::vector<kc::ks_ptr<PhysicalBody>> _sorted_physical_bodies

sorted list of physical bodies created by _setupSortedPhysicalBodiesList

std::vector<kc::ks_ptr<PhysicalSubhinge>> _coord_sanitization_subhinges

List of subhinges coordinates sanitization needs (e.g., SphericalSubhinge type)

mutable std::unique_ptr<const kc::Tree<kc::id_t>> _tree

the tree instance for the body ids in the sub-tree

bool _has_compound_bodies = false

flag specifying whether the sub-tree contains any compound bodies

bool _enabled_algorthmic = false

whether the SubTree has been set up for calling inverse/forward dynamics algorithms that use gather/scatter recursions.

bool _enabled_subhinge_charts = true

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.

std::string _dump_dynamics_prefix = ""

prefix used by dumpDynamics() if none is specified

NeedsStateReset _needs_state_reset = NeedsStateReset::NUM_STATES_CHANGED

Boolean that indicates whether a state reset is needed due to configuration changes.

std::vector<std::string> _why_state_needs_reset = {"SubTree created."}

Keep track of why the state has been reset.

struct DumpTreeOptions

A struct with options to tailor the content from dumpTree()

Public Members

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