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.

virtual ~SubTree()

SubTree destructor.

inline virtual const std::string &typeString() const noexcept override

Returns the type string of LockingBase.

Returns:

The type string.

inline virtual bool isSubGraph() const

Method to check wether 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

inline 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

inline 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

const 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.

Parameters:

bd – the input body

Returns:

true if the body belongs to the subtree

kc::ks_ptr<BodyBase> getBody(const std::string &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.

Parameters:

name – the body’s name

Returns:

the BodyBase body instance

std::vector<kc::ks_ptr<BodyBase>> getBodies(const std::string &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

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

inline size_t numBodies() const

Return the number of bodies in the tree.

Returns:

the number of bodies

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

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 specifiied 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 teh 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 stateToDynamics(const km::Vec &x)

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 velolicty coordiantes

• 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.

virtual void dynamicsToState(Eigen::Ref<km::Vec> x) 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 velolicty coordiantes

• 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.

virtual km::Vec dynamicsToState() 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.

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 velolicty coordiantes

• 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 velolicty coordiantes

• 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 velolicty coordiantes

• the body deformation Udot velocity coordinates.

virtual std::string dumpString(const std::string &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(const std::string &prefix = "") const

Display information about the subtree and its bodies.

Parameters:

prefix – the prefix for each line in the output

virtual void dumpTree(const std::string &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:

options – Options to tailor the output of dumpTree

inline 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

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

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

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>> &physicalBodiesList() const

Compute and return the 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>> &bodiesList() 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 current.

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 rangeQ = .5, double dQ = .01, double pause = .01, std::function<void()> cb = nullptr) const

Sequenatially 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

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

• dQ – 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 rangeQ = .5, double dQ = .01, double pause = .01) const

Sequenatially 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:

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

• dQ – 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 santize 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 isFinalized() method ineffective for detecting uninitialized 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 Upislon value

• rbd – the right-hand body for the cross-OSCM Upislon 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 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, right) matrix

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

Return the regular psi(left, right) matrix for the body pair (from left pframe to right pframe)

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 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 frame. 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 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 phi(left, right) matrix

Public Static Functions

static kc::ks_ptr<SubTree> create(const kc::ks_ptr<SubTree> &parent_subtree, std::string_view name, 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

• new_leaves – The list of leaf bodies that define the extent of the subtree bodies

Returns:

A new SubTree instance

Protected Functions

virtual void _resetSubhingeCharts()

Reset all physical subhinge chart offsets

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

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:

• left – the CoordBase on the left

• right – 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.

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.

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 extensed OSCM matrix for computing Gamma for TA constrained dynamics.

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

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)

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

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

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

callback for the regular psi = tauper * phi. this method requires that the left CoordBase to be an ancestor of the right CoordBase

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

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

for the oframe variant psi = tauper * phi. this method requires that the left CoordBase to be an ancestor of the right CoordBase

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

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

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

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

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

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

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

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

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

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

callback for the regular psi = tauper * phi. this method requires that the left CoordBase be an ancestor of the right CoordBase

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

callback for the oframe variant psi = tauper * phi. this method requires that the left CoordBase be an ancestor of the right CoordBase

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

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

void _setupTree()

set up the _tree member

void _setupCoordSanitizationSubhinges()

Update cached list of subhinges that have coordinate offset requirments (eg. spherical subhinges)

virtual void _makeStale() 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

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)

Unregisters 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:

frame – 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

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

void _setupPhysicalBodiesList()

void _setupBodiesList()

km::HomTran _computeCMPose()

km::SpatialVector _computeCMSpVel()

km::SpatialVector _computeCMSpAccel()

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

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

• bbds_frames – 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

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

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

• subhinges – 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

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

sorted list of bodies in the subtree

kc::ks_ptr<SubTree> _parent_subtree = nullptr

parent subtree for this subtree (null for multiboy level)

kc::ks_ptr<kf::Frame> _cm_frame = nullptr

the center of mass frame for the subtree. It is attached to the pysical 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

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

kc::UsageTrackingMap<std::pair<kc::id_t, kc::id_t>, kc::DataCache<km::Mat>> _coordbase_psi_cache_usage_map

for the 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

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

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

sorted list of physical bodies

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

List of subhinges coordinates sanitization needs (eg. SphericalSubhinge type)

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

bool _has_compound_bodies = false

bool _enabled_algorthmic = false

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

struct dumpTreeOptions

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

Public Members

bool hingeType = true

include the body’s hinge’s typeString info

bool lockStatus = false

include the frame’s lock status

bool refCount = false

include the frame’s reference count

bool hingeRefCount = false

include the frame’s edge’s reference count

bool id = false

include the frame’s tree id