Program Listing for File PhysicalBody.h#
↰ Return to documentation for file (include/Karana/SOADyn/PhysicalBody.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 PhysicalBody class.
*/
#pragma once
#include "Karana/KCore/DataCache.h"
#include "Karana/Math/SpatialInertia.h"
#include "Karana/Math/SpatialVector.h"
#include "Karana/ProxyScene/ProxyScenePart.h"
#include "Karana/SOADyn/BodyBase.h"
#include "Karana/SOADyn/PhysicalHinge.h"
#include "Karana/Scene/ScenePartSpec.h"
namespace Karana::Dynamics {
namespace kc = Karana::Core;
namespace kf = Karana::Frame;
namespace ks = Karana::Scene;
class Node;
class ConstraintNode;
class HingePnode;
class Hinge;
class Multibody;
class CompoundBody;
class LoopConstraintCutJoint;
/** @brief Struct with parameters for a PhysicalBody rigid physical body */
struct PhysicalBodyParams {
km::SpatialInertia sp_i =
km::SpatialInertia(2.0,
km::Vec3(0.3, 0.2, 0.1),
Eigen::DiagonalMatrix<double, 3>(
3, 2, 1)); //!< the spatial inertia to assign to each body
std::vector<km::Vec3> axes = {{0, 1, 0}}; //!< the subhinge axes for the body hinges
km::HomTran body_to_joint_transform =
km::HomTran(km::UnitQuaternion(.8, .6, 0, 0),
km::Vec3(.2, .3, .4)); //!< the body to joint transform for the bodies
km::HomTran inb_to_joint_transform = km::HomTran(
km::UnitQuaternion(.5, .5, .5, .5),
km::Vec3(.4, .2, .3)); //!< the inboard body to joint transform for the bodies
std::vector<ks::ScenePartSpec> scene_part_specs =
{}; //!< list of scene part specs for the body
/** @brief copy operator
@param p the other params struct
@return the updated params struct
*/
PhysicalBodyParams &operator=(const PhysicalBodyParams &p) {
if (this != &p) {
// Assign Derived-specific members here
sp_i = p.sp_i;
// htype = p.htype;
axes = p.axes;
body_to_joint_transform = p.body_to_joint_transform;
inb_to_joint_transform = p.inb_to_joint_transform;
scene_part_specs = p.scene_part_specs;
}
return *this;
}
};
/**
* @class PhysicalBody
* @brief Represents a rigid physical body
*
* This class is for rigid physical bodies. See the
* \sref{physical_bodies_sec} section for more discussion on physical
* bodies.
*/
class PhysicalBody : public kf::Frame, public BodyBase {
/* so can access _newtonian2body_f2f */
friend class Multibody;
// for access to _invdyn_cache
friend class PhysicalHinge;
// for access to _spI etc for inverse dynamics
friend class HingePnode;
// for access to _child_onodes_list
friend class SubTree;
friend class CompoundBody;
// for access to _constraint_nodes_list
friend class SubGraph;
// for access to _get/_setBodyToNodeTransforms
friend class Node;
// for access to _getOnodeATBISmootherAlphaPlus
friend class HingeOnode;
// for access to _setupNode
friend class ConstraintNode;
// for access to _isInboardOf
friend class CompoundSubhinge;
// for access to _atbi_filter_include_gravity etc
friend class Algorithms;
public:
/**
* @brief PhysicalBody constructor. The constructor is not meant to be called directly.
* Please use the create(...) method instead to create an instance.
*
* @param name Name of the PhysicalBody
* @param mb The Multibody to attach the PhysicalBody to.
*/
PhysicalBody(std::string_view name, kc::ks_ptr<Multibody> mb);
/**
* @brief PhysicalBody destructor.
*/
virtual ~PhysicalBody();
/**
* @brief Creates a new PhysicalBody with the given name.
* @param name The name of the frame to create.
* @param mb The Multibody instance.
* @return The created body.
*/
static kc::ks_ptr<PhysicalBody> create(std::string_view name, kc::ks_ptr<Multibody> mb);
std::string_view typeString(bool brief = true) const noexcept override {
return kc::Base::typeString(brief);
}
const kc::id_t &id() const override { return kf::Frame::id(); }
/** @brief Needed to have a definition of BodyBase::name() method
@return the name
*/
std::string_view name() const override { return kf::Frame::name(); };
/**
* @brief Return true is this PhysicalBody is the Multibody's virtual root body
* @return true if this is the virtual root body
*/
bool isRootBody() const override;
/**
* @brief Set the PhysicalBody parameters
* @param params the parameters to set
*/
virtual void setParams(const PhysicalBodyParams ¶ms);
/**
* @brief Replace an existing cut-joint loop constraint with one at this body
*
* This method can be used to change the choice of cut-joint in
* a graph topology system by replacing an existing cut-joint
* with a new one at this body. It is required that (a) the new
* cut-joint body be the ancestor of one, and only one of the
* bodies that are the attachments of the input cut-joint. This
* method will discard the input cut-joint, and replace the
* parent hinge of the new cut-joint body and replace it with an
* equivalent cut-joint constraint.
*
* @param prev_cutjoint the cut-joint constraint to be replace
* @return the new cut-joint constraint
*/
kc::ks_ptr<LoopConstraintCutJoint>
relocateCutJoint(kc::ks_ptr<LoopConstraintCutJoint> prev_cutjoint);
/**
* @brief Add a serial chain of rigid PhysicalBody instances to the multibody system
*
* Add a serial chain of rigid bodies to the multibody system to
* the specified root body with the specified mass properties,
* hinge type and hinge connection properties. This method is
* mainly used for procedurally generating large test
* systems. See \sref{multibody_procedural_sec} section for more
* discussion on this topic.
*
* @param name prefix string to use for the new body names
* @param nbodies number of bodies
* @param root the parent body to attach the chain to
* @param htype the hinge type
* @param params the body params for each body
* @return the list of new rigid bodies
*/
static std::vector<kc::ks_ptr<PhysicalBody>>
addSerialChain(std::string_view name,
size_t nbodies,
PhysicalBody &root,
HingeType htype = HingeType::REVOLUTE,
PhysicalBodyParams *params = nullptr);
/**
* @brief Add a sub-tree of rigid PhysicalBody instances to the multibody system
*
* Add a sub-tree of rigid bodies to the multibody system to the
* specified root body with the specified mass properties, hinge
* type and hinge connection properties. This method is mainly
* used for procedurally generating large test systems. See the
* \sref{multibody_procedural_sec} section for more discussion on this
* topic.
*
* @param name prefix string to use for the new body names
* @param branch_length the number of bodies in a branch
* @param nbranches the number of children branches
* @param depth the number of branching levels to create
* @param root the parent body to attach the chain to
* @param htype the hinge type
* @param params the body params for each body
* @return the list of new rigid bodies
*/
static std::vector<kc::ks_ptr<PhysicalBody>> addTree(std::string_view name,
size_t branch_length,
size_t nbranches,
size_t depth,
PhysicalBody &root,
HingeType htype = HingeType::REVOLUTE,
PhysicalBodyParams *params = nullptr);
public:
/**
* @brief Set the Karana::Math::SpatialInertia spatial inertia for the body in the specified
* reference Karana::Frame::Frame frame
*
* If no frame is specified, then the spatial inertia is assumed
* to be represented in and about the body frame
*
* @param sp_i the input spatial inertia
* @param ref_frame the reference frame for the input spatial inertia
*/
void setSpatialInertia(const km::SpatialInertia &sp_i,
const kc::ks_ptr<kf::Frame> &ref_frame = nullptr);
/**
* @brief Return the body's Karana::Math::SpatialInertia spatial inertia about the body
* frame
* @return the spatial inertia
*/
const km::SpatialInertia &getSpatialInertia() const;
/**
* @brief Set the the body frame to body HingePnode pnode transform.
*
* For a deformable body, this is the transform value in the
* undeformed configuration. This method should only be used for
* body pnodes. The Node::setBodyToNodeTransform() method should
* be used for all other nodes.
*
* @param T The input transform
*/
virtual void setBodyToJointTransform(const km::HomTran &T);
/**
* @brief Return the body frame to body HingePnode pnode transform.
*
* For a deformable body, this is the transform value in the
* undeformed configuration.
*
* @return the transform
*/
virtual km::HomTran getBodyToJointTransform() const;
/**
* @brief Return the body frame observed spatial acceleration
* for the body with respect to the Newtonian frame
*
* Return the body frame observed spatial acceleration for the
* body with respect to inertial frame about and in the body frame. The
* returned value is in the body frame. Note that this is *NOT*
* the newtonian_frame.frameToFrame(body).relSpaAccel() which is
* the Newtonian frame observed spatial acceleration!
*
* @return the spatial acceleration
*/
const km::SpatialVector bodyObservedSpatialAccel() const;
/**
* @brief Return the overall spatial force on the body at its reference frame and in the
* body frame
*
* This method will accumulate the external spatial forces from
* all the force nodes (including active contact nodes) on the
* body. If with_constraints is true, then constraint
* spatial forces from the constraint nodes will also be
* accumulated in the returned value.
*
* @param with_constraints set to true to include constraint node contributions
* @return the overall spatial force on the body, at the body reference frame in the body
* frame
*/
km::SpatialVector externalSpatialForce(bool with_constraints) const;
/**
* @brief Split up the parent hinge's subhinges into sequence of hinges with single
* subhinges
*
* The resulting system will have equivalent dynamics to the current system.
*
* This method is a no-op if the parent hinge has a single subhinge.
*/
void splitSubhinges();
/**
* @brief Add a mass less and locked parent body between the current parent
*
* The resulting system will have equivalent dynamics to the current system.
*
* This method is a no-op if the parent hinge has a single subhinge.
*/
void insertDummyParentBody();
/**
* @brief Reattach the body to the specified parent PhysicalBody
*
* Reattach this PhysicalBody to the new parent PhysicalBody
* body via a hinge of the specified type. If the new hinge type
* is a 6-dof hinge, then the inertial pose, spatial velocity
* and spatial acceleration of the body are preserved after
* reattachment. For a non 6-dof hinge, no such pose
* preservation is done, and follow up steps to set the hinge
* parameters (e.g., axes) and initial state will be required.
*
* See \sref{mbody_config_changes_sec} section for
* information.
*
* @param new_parent The new parent body
* @param hinge_type The new hinge type
*/
void reattach(PhysicalBody &new_parent, HingeType hinge_type = HingeType::FULL6DOF);
/**
* @brief Reattach the body to the virtual root via a 6 dof hinge.
*
* Reattach the body to the virtual root via a 6 dof hinge
* (while preserving the inertial pose, spatial velocity and
* spatial acceleration. See \sref{mbody_config_changes_sec} section for
* information.
*/
void detach();
/**
* @brief Return the ScenePart with the specified name
*
* See \sref{proxyscene_sec} section for proxy scene related discussion.
*
* @param nm the name of the scene part
* @return The scene part with machine name, or nullptr if none found
*/
kc::ks_ptr<ks::ProxyScenePart> getScenePart(std::string_view nm) const;
/**
* @brief Return the list of SceneParts created from Karana::Scene::ScenePartSpec values
*
* See \sref{proxyscene_sec} section for proxy scene related discussion.
*
* @return The list of scene parts
*/
const std::vector<kc::ks_ptr<ks::ProxyScenePart>> &getSceneParts() const;
/**
* @brief Return the list of ScenePartSpecs
*
* See \sref{proxyscene_sec} section for proxy scene related discussion.
*
* @return The list of scene part specs
*/
const std::vector<ks::ScenePartSpec> &getScenePartSpecs() const;
/**
* @brief Replace the parent hinge with an equivalent loop constraint
*
* The method removes the hinge between a parent/child body
* pair, and replaces it with an equivalent cut-joint
* LoopConstraintCutJoint between the bodies. The child body is
* attached to the virtual root via a 6 dof hinge. The
* parameters are set so that there is no change in the pose,
* velocities etc, and the new system is kinematically and
* dynamically equivalent - albeit one with redundant
* coordinates and constraints. See \sref{FA_TA_models_sec} sec
* for discussion about the duality between PhysicalHinge and
* LoopConstraintCutJoint classes in Multibody representations.
*
* @return the new LoopConstraint instance
*/
kc::ks_ptr<LoopConstraintCutJoint> toCutJointConstraint();
/**
* @brief Convert this body into a floating base body.
*
* Given a body with parent PhysicalBody body that is a 6-dof
* floating base body, reverse the connecting parent hinge to
* make this body into a 6-dof floating base body. This
* operation maintains the required tree structure for the
* multibody system. The new hinge has the same subhinges as the
* original hinge - but in reversed order. The subhinge
* parameters preserve the original params while taking into
* account the orientation reversals. The end result is that the
* poses of the bodies remains unaffected. See the
* \sref{mbody_config_changes_sec} for more on Multibody configuration
* changes.
*/
void shiftBaseBody();
/**
* @brief Return the pnode for the parent hinge
*
* See \sref{body_hinge_sec} section for more on body pnodes and onodes.
* @return the HingePnode instance
*/
const kc::ks_ptr<HingePnode> &pnode() const;
/**
* @brief Return the onode for the parent hinge
*
* See \sref{body_hinge_sec} section for more on body pnodes and onodes.
* @return the HingeOnode instance
*/
const kc::ks_ptr<HingeOnode> &onode() const;
const kc::ks_ptr<PhysicalBody> &physicalParentBody() const override;
kc::ks_ptr<HingeBase> parentHinge() const override;
/**
* @brief Return the Karana::Frame::FrameToFrame from the Newtonian frame to the body frame
* @return the Karana::Frame::FrameToFrame instance
*/
kc::ks_ptr<kf::OrientedChainedFrameToFrame> newtonianToBodyFrameToFrame() const {
return _newtonian2body_f2f;
}
/**
* @brief Get a list of the nodes on this body. This does not include pnodes or onodes.
* @returns A list of nodes on the body, not including pnodes or onodes.
*/
const std::vector<kc::ks_ptr<Node>> &nodeList() const;
/**
* @brief Get a list of the constraint nodes on this body.
* @returns A list of constraint nodes on the body
*/
const std::vector<kc::ks_ptr<ConstraintNode>> &constraintNodeList() const;
/**
* @brief Retrieve the node with the specified name.
*
* Return nullptr if there is no node with the specified name.
*
* @param node_name the node's name
* @return The node on the body with the specified name.
*/
kc::ks_ptr<Node> getNode(std::string_view node_name);
/**
* @brief Retrieve the constraint node with the specified name.
*
* Return nullptr if there is no constraint node with the specified name.
*
* @param node_name the node's name
* @return The constraint node on the body with the specified name.
*/
kc::ks_ptr<ConstraintNode> getConstraintNode(std::string_view node_name);
/**
* @brief Set the gravitational linear acceleration for the body represented in the
* specified frame.
*
* If no frame is specified, then the linear acceleration is
* assumed to be represented in the body frame, else the gravity
* vector is immediately rotated from the specified frame into
* the local body frame.
*
* @param g the gravitational linear acceleration vector
* @param ref_frame the representation frame for the input gravity accel vector
*/
void setGravAccel(const km::Vec3 &g, const kc::ks_ptr<kf::Frame> &ref_frame = nullptr);
/**
* @brief Accumulate the gravitational linear acceleration for the body represented in the
* specified frame.
*
* If no frame is specified, then the linear acceleration is
* assumed to be represented in the body frame
*
* @param g the gravitational linear acceleration vector
* @param ref_frame the representation frame for the input gravity accel vector
*/
void accumGravAccel(const km::Vec3 &g, const kc::ks_ptr<kf::Frame> &ref_frame = nullptr);
/**
* @brief Return the gravity linear acceleration at the body in the body frame
* @return the gravity acceleration vector
*/
const km::Vec3 &getGravAccel() { return _grav_accel_cache->get(); }
/**
* @brief Set the gravity gradient moment vector for the body
* represented in the specified frame.
*
* If no frame is specified, then the moment is assumed to be
* represented in the body frame
*
* @param grav_gradient the gravity gradient vector
* @param ref_frame the representation frame for the input gravity accel vector
*/
void setGravityGradient(const km::Vec3 &grav_gradient,
const kc::ks_ptr<kf::Frame> &ref_frame = nullptr);
/**
* @brief Accumulate the gravity gradient moment vector for the body
* represented in the specified frame.
*
* If no frame is specified, then the moment is assumed to be
* represented in the body frame
*
* @param grav_gradient the gravity gradient vector
* @param ref_frame the representation frame for the input gravity accel vector
*/
void accumGravityGradient(const km::Vec3 &grav_gradient,
const kc::ks_ptr<kf::Frame> &ref_frame = nullptr);
/**
* @brief Return the 6x6 OSCM Upsilon matrix for a body node using the pnode Upsilon.
*
* This method is used for the scatter step for onode Upsilon computation.
*
* @param nd the body node
* @return the OSCM Upsilon matrix
*/
virtual km::Mat66 getNodeUpsilonFromPnode(const Node &nd);
/**
* @brief Return the 6x6 OSCM Upsilon matrix for a body node using the body's Upsilon.
*
* Their method is used to compute the child onode UpsilonPlus
* value after crossing the modal dofs.
*
* @param node the body node
* @return the OSCM Upsilon matrix
*/
virtual km::Mat66 getNodeUpsilonFromBody(const Node &node);
/** @brief Options struct for dumpString */
struct DumpOptions : Karana::Frame::Frame::DumpOptions {
/**
* @brief Assignment operator.
*
* @param p The DumpOptions to copy values from.
* @return A reference to the newly assigned DumpOptions.
*/
// Adding suppression, as CodeChecker complains this method has the same name as a
// method in the base version. This is just a CodeChecker false positive.
// codechecker_suppress [cppcheck-duplInheritedMember]
DumpOptions &operator=(const DumpOptions &p) {
if (this != &p) {
Frame::DumpOptions::operator=(p); // Call base class assignment operator
// Assign Derived-specific members here
cache_deps = p.cache_deps;
}
return *this;
}
};
std::string dumpString(std::string_view prefix,
const Karana::Core::Base::DumpOptions *options) const override;
bool isReady() const override;
/**
* @brief Return the kinetic energy contribution for the body
* @return the kinetic energy value
*/
virtual double kineticEnergy() const;
/**
* @brief Return the spatial momentum contribution for the body
* @return the spatial momentum vector
*/
virtual km::SpatialVector spatialMomentum() const;
/**
* @brief Compute the body frame referenced OSCM Upsilon matrix for the body.
*
* This method is specialized by flex bodies as well. For a rigid
* body the value is a 6x6 matrix, but is larger by the number of
* modes for a deformable body.
*
* @return the OSCM Upsilon matrix
*/
km::Mat getUpsilonMatrix() override;
/**
* @brief Add a ScenePart description to the body to be realized
* whenever a ProxyScene is available.
*
* The new scene part spec's name must not conflict with the
* name of an existing scene part spec of scene part for the
* body.
*
* @param scene_part The ScenePart description
*/
void addScenePartSpec(const ks::ScenePartSpec &scene_part);
/** @brief Discard all nodes in the _nodes_usage_map
*/
void discardAllNodes();
protected:
kc::ks_ptr<kf::FrameToFrame>
f2f() const override; // NOLINT(readability-identifier-naming) TODO
/** @brief Struct for inverse dynamics computations */
struct InvDynVectors {
/** the inter-body spatial interaction force for the hinge at its
* onode and represented in the onode frame.
*
*/
km::SpatialVector f;
};
/** @brief Struct of flags that can be used to turn on and off the
inclusion of various contributions in the ATBI filter gather
recursions. Handy for TA correction phase sweeps, and
computing mass matrix etc. */
struct GatherSweepFlags {
/** flag that can be used to turn on and off the inclusion of
gravity contributions in the ATBI filter gather
recursions. Handy for TA correction phase sweeps */
bool gravity = true;
/** flag that can be used to turn on and off the inclusion of
external force contributions in the gather
recursions. Handy for TA correction phase sweeps */
bool external_forces = true;
/** flag that can be used to turn on and off the inclusion of
constraint force contributions in the gather
recursions. Constraint nodes are however always on since we use them for the
cut-joint hinge torque contributions as well */
bool constraint_forces = true;
/** flag that can be used to turn on and off the inclusion of
deformation elastic force contributions in the flex dynamics gather
recursions. Handy to turn off for mass matrix and TA
algorithm computations. */
bool elastic_forces = true;
/** flag that can be used to turn on and off the inclusion of
deformation generalized modal force contributions in the flex dynamics gather
recursions. Handy to turn on for fwd dynamics based mass matrix inverse
algorithm computations. */
bool modal_gen_forces = false;
};
protected:
/** @brief Get the struct with gather sweep flags
@return the gather sweep flags
*/
GatherSweepFlags &_gatherSweepFlags() { return _gather_sweep_flags; }
/** @brief For a rigid body, returns zero vector, while for a
flex body return the Coriolis term for the specified child
onode represented in the onode frame.
This is used to in the computation of alpha_plus during the
smoothing sweep, as well as to include an embedded body's
aprime Coriolis term computation
@param child_onode the child onode
@return the Coriolis acceleration
*/
virtual km::SpatialVector _getChildOnodeCoriolis(const HingeOnode &child_onode) const;
/** @brief For a rigid body, returns zero vector, while for a
flex body return the Coriolis term for the specified child
pnode represented in the pnode frame.
This is used to in the computation of alpha_plus during the
smoothing sweep, as well as to include an embedded body's
aprime Coriolis term computation
@return the Coriolis acceleration
*/
virtual km::SpatialVector _getPnodeCoriolis() const;
/** @brief For a rigid body, return the pnode P, while for a flex body
return the body's P at its pnode_trans.
This is used to
include an embedded body's ATBI P in it parent compound body's P
@return the ATBI P matrix
*/
virtual km::Mat _getBodyP() const;
/** @brief For a rigid body, return the pnode z, while for a flex body
return the body's z at its pnode_trans.
This is used to include an embedded body's ATBI z in it
parent compound body's z
@return the ATBI z vector
*/
virtual km::Vec _getBodyZ();
/**
* @brief Track the usage of the node.
* @param nd the node
*/
void _trackUsageNode(const kc::ks_ptr<Node> &nd);
/**
* @brief Move the node to the location given by the transform T relative to the body frame.
*
* This method should not be used to set a normal nodes value. It is intended to be used for
* contact force nodes. Normal user-defined force nodes, constraint nodes, etc. should have
* their values set on the node directly.
*
* This moves the node so its transform is T relative to the body frame. Derived classes can
* specialize this method to set other values, such as those given in the deformation
* provider.
*
* @param node The node to move.
* @param T The transform of the node relative to the body frame.
*/
virtual void _moveNode(const kc::ks_ptr<Node> &node, const km::HomTran &T);
/**
* @brief Return a shared pointer for this body
*
* @return the shared pointer
*/
kc::ks_ptr<PhysicalBody> _asSharedPtr();
/**
* @brief Create a ProxyScenePart from the ScenePartSpec and add it to the body.
* @param scene_part The ScenePartSpec to add.
*/
void _realizeScenePart(const ks::ScenePartSpec &scene_part);
/** @brief detach the node from the parent body
@param nd the node
*/
virtual void _detachNode(kc::ks_ptr<Node> &nd);
/**
* @brief Discard the provided PhysicalBody.
* @param base - Base pointer to the PhysicalBody to discard.
*/
void _discard(kc::ks_ptr<Base> &base) override;
/** @brief Helper method to create a node. This method is specialized by
bodies that are deformable
@param node the node
@param force_node if true, this is a force node
*/
virtual void _setupNode(Node &node, bool force_node);
/** @brief Create the scene parts for the body from its list of
ScenePartSpecs */
void _processScenePartSpecs();
/** @brief For a rigid body, return the body spatial inertia mass
properties transformed rigidly to the pnode as a 6x6
matrix.
For a flex body, return the (nU+6) square body level
mass matrix transformed to the pnode.
@return the body's mass properties matrix
*/
virtual km::Mat66 _getPnodeMassMatrix() const;
/** @brief Propagate the child onode CRB inertia contributions to
compute the CRB inertia for this body transformed to the
pnode.
For a rigid body, all of this is done with spatial
inertias. For a flex body, we have to work with the (nU+6)
size local body mass properties while taking into account
the onode deformations.
@param R the buffer for the computed value
*/
virtual void _getPnodeCRBInertiaMatrix(km::Mat &R);
/** @brief Set the the transform to a body node.
For a deformable body, this is the transform value in the
undeformed configuration. This method should be used for all
nodes, except for the pnode for whom the
get/setBodyToJointTransform() method should be used. These
methods are protected to allow them to specialized for rigid
and deformable bodies.
@param node the body node
@param t the transform value
*/
virtual void _setBodyToNodeTransform(Node &node, const km::HomTran &t);
/** @brief Get the the transform for a body node.
For a deformable body, this is the transform value in the
undeformed configuration. This method should be used for all
nodes, except for the pnode for whom the
get/setBodyToJointTransform() method should be used. These
methods are protected to allow them to specialized for rigid
and deformable bodies.
@param node the body node
@return the transform value
*/
virtual km::HomTran _getBodyToNodeTransform(const Node &node) const;
/** @brief Data cache callback for updating the body's gyroscopic
* spatial force.
*
* This gyroscopic term corresponds to rigid body
* equations of motion where the body frame derivatives are used
* for generalized accel, i.e. the accel corresponds to
* pframeObservedRelSpAccel(). The returned spatial force is about
* the and at the pnode frame.
*
* @param val the data buffer for the computed value
*/
virtual void _computeGyroscopicForce(km::SpatialVector &val) const;
/** @brief Return 6*6 matrix that is the pnode to child node psi
matrix.
While for a rigid body this is simply phi(pnode,
onode), for a flex body it includes the modal psi part in
crossing modal dofs when going between the pnode and onode
of the flex body. This method *DOES* depend on
ATBI quantities.
@param node the child node
@return the psi transform matrix
*/
virtual km::Mat66 _pnodeToChildNodeTransform(const Node &node);
/** @brief Return the phi matrix that is the from frame to a child body node
phi matrix.
While for a rigid body this matrix is simply phi(body,
node), for a flex body it includes the deformation
contribution for the body's node (this is the 'A'
matrix). This method does *NOT* depend on ATBI
quantities.
@param node the child node
@param from_frame the from frame
@return the phi matrix
*/
virtual km::Mat _bodyToChildNodeTransform(const Node &node,
const kf::Frame &from_frame) const;
/** @brief Return matrix that is the pnode to body frame contribution
to the psi matrix.
While for a rigid body this is simply
phi(pnode, body), for a flex body it includes the modal psi
part in crossing modal dofs when going between the pnode and
body frame for the flex body, This method is used when
computing the cross ATBI matrix between parent/child bodies
for computing cross-Upsilon data.This method *DOES* depend
on ATBI quantities.
@return the psi matrix
*/
virtual km::Mat _pnodeToBodyPsi() const;
/**
* @brief Zero out the external spatial forces, gravity accel and
gravity gradient values for the body.
*/
void _clearExternals();
/** @brief Return the frame for the pnode after translational deformation
For a rigid body, this returns the pnode. However, flex bodies
can override this method to return the appropriate deformed
translational frame for the pnode (used in ATBI
computations
@return the frame
*/
virtual kc::ks_ptr<kf::Frame> _getPnodeDeformedTransFrame() const;
protected:
/** @brief Helper method to create the pnode
The floating_frame argument is only used for deformable bodies.
@param floating_frame if true, this is a floating frame body
@return the pnode
*/
virtual kc::ks_ptr<HingePnode> _createPnode(bool floating_frame);
/** @brief Helper method to create an onode
@param name the onode's name
@return the onode
*/
virtual kc::ks_ptr<HingeOnode> _createOnode(std::string_view name);
/** @brief Helper method to discard the pnode
*/
virtual void _discardHingePnode();
/** @brief Helper method to discard an onode
@param onode the onode
*/
virtual void _discardHingeOnode(kc::ks_ptr<HingeOnode> &onode);
/** @brief Helper method to track a body's child onode
@param onode the onode
*/
virtual void _registerChildOnode(kc::ks_ptr<HingeOnode> onode);
/** @brief Helper method to stop tracking a body's child onode
@param onode the onode
*/
virtual void _unregisterChildOnode(kc::ks_ptr<HingeOnode> onode);
protected:
/**
* @brief Return the ATBI z filter spatial vector
* @return the z spatial vector
*/
virtual km::SpatialVector _getPnodeATBIFilterZ();
/**
* @brief Return the ATBI P matrix
* @return the P matrix
*/
virtual km::Mat66 _getPnodeATBIMatricesP();
/**
* @brief Return the ATBI alpha^+ spatial acceleration for a child onode
* @param onode the child onode
* @return the spatial acceleration
*/
virtual km::SpatialVector _getOnodeATBISmootherAlphaPlus(const HingeOnode &onode);
/**
* @brief Helper method for the ATBI smoother stage
*/
virtual void _processPnodeATBISmootherVectors();
/** @brief Helper method for the ATBI smoother stage
TODO - get rid of this flex specific method */
virtual void _processModalATBISmootherVectors() {};
/**
* @brief Helper method for updating the ATBI OSI Upsilon matrix
* @return the Upsilon matrix
*/
virtual km::Mat66 _processPnodeUpsilonMatrices();
/** @brief Helper method for computing the inter-body inverse dynamics inter-body spatial
force
Used by the onode to compute its interbody inverse dynamics
spatial force
@return the computed spatial force
*/
virtual km::SpatialVector _getPnodeInvDynForcesF();
/**
* @brief Create stick parts for the body
*
* @param scene the proxy scene
* @param c the stick parts configuration data
*/
void _createStickParts(kc::ks_ptr<ks::ProxyScene> &scene, const StickPartsConfig &c);
protected:
/* set up the cache dependencies between a physical body and its
child physical body */
void _setupChildBodyCacheDependencies(kc::ks_ptr<BodyBase> child_body) override;
void _setupChildPhysicalBodyCacheDependencies(const PhysicalBody &child_body,
bool skip_scatter) override;
/* set up the cache dependencies between a physical body and its
child compound body */
void _setupChildCompoundBodyCacheDependencies(kc::ks_ptr<CompoundBody> child_body) override;
void _setupBaseBodyCacheDependencies() override;
/* set up the cache dependencies between a physical body and its
child physical body */
void _teardownChildBodyCacheDependencies(kc::ks_ptr<BodyBase> child_body) override;
void _teardownChildPhysicalBodyCacheDependencies(const PhysicalBody &child_body,
bool skip_scatter) override;
/* set up the cache dependencies between a physical body and its
child compound body */
void
_teardownChildCompoundBodyCacheDependencies(kc::ks_ptr<CompoundBody> child_body) override;
void _teardownBaseBodyCacheDependencies() override;
protected:
/// the body's spatial inertia at - and in - the body frame
km::SpatialInertia _spI;
/// the cached gravitational linear accel value for the body in
/// the body frame
km::Vec3 _grav_accel_body_frame;
/// the cached gravity gradient moment value for the body in
/// the body frame
km::Vec3 _grav_gradient_body_frame;
kc::ks_ptr<HingePnode> _parent_pnode = nullptr; ///< attachment pnode
/// f2f from the newtonian frame to this body's frame
kc::ks_ptr<kf::OrientedChainedFrameToFrame> _newtonian2body_f2f = nullptr;
/** the gather sweep flags */
GatherSweepFlags _gather_sweep_flags;
/** list of scene_part_specs for the body */
std::vector<ks::ScenePartSpec> _scene_part_specs;
/** list of scene parts created for the scene_part_specs for the body */
std::vector<kc::ks_ptr<ks::ProxyScenePart>> _scene_part_spec_parts;
protected:
/** the data cache for the body's spatial inertia */
kc::ks_ptr<kc::DataCache<km::SpatialInertia>> _spI_cache = nullptr;
/** the data cache for the body's gravitational acceleration */
kc::ks_ptr<kc::DataCache<km::Vec3>> _grav_accel_cache = nullptr;
/** the data cache for the body's gyroscopic force */
kc::ks_ptr<kc::DataCache<km::SpatialVector>> _gyroscopic_force_cache = nullptr;
protected:
/** List of child body attachment onodes. For a physical body,
this is the list of onodes for all the physical children
bodies.
*/
kc::RegistryList<HingeOnode> _child_onodes_list;
protected:
kc::RegistryList<Node> _nodes_list; ///< track nodes on the body
kc::RegistryList<Node> _external_force_nodes_list; ///< List of nodes that can apply forces
kc::RegistryList<Node> _contact_force_nodes_list; ///< List of nodes that can apply forces
///< for the current dynamics calculation.
/**
* In the _contact_force_nodes_list, values to the left of this (from begin() to begin() +
* index) are inactive and values to the right are active. Another way to say this is this
* index points to a node that is active, and it is the first active node in the list.
* Everything at this index and later is active.
*/
size_t _active_contact_force_node_index = 0;
kc::RegistryList<ConstraintNode> _constraint_nodes_list; ///< List of constraint nodes
};
} // namespace Karana::Dynamics