Program Listing for File Node.h#
↰ Return to documentation for file (include/Karana/SOADyn/Node.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 FramePairHinge class.
*/
#pragma once
#include "Karana/Frame/Frame.h"
#include "Karana/KCore/DataCache.h"
#include "Karana/Math/HomTran.h"
#include "Karana/ProxyScene/ProxyScene.h"
namespace Karana::Dynamics {
namespace kc = Karana::Core;
namespace kf = Karana::Frame;
namespace km = Karana::Math;
namespace ks = Karana::Scene;
class PhysicalBody;
class PinSubhinge;
class SubhingeBase;
class Node;
class Multibody;
class LoopConstraintBase;
struct StickPartsConfig;
/**
* @class NodeDeformationProvider
* @brief Base abstract class node deformation
*
* See \sref{modal_flex_body_sec} section for more information of
* deformation provider classes.
*
* Base abstract class that provides data and methods for modeling
* node deformation
*/
class NodeDeformationProvider : public kc::Base {
// for access to _getParams, _setParams
friend class PhysicalBody;
public:
/**
* @brief Constructor
* @param name instance name
* @param node the parent Node
*/
NodeDeformationProvider(std::string_view name, kc::ks_ptr<Node> node)
: Base(name)
, _nd(node) {}
/**
* @brief Destructor
*/
~NodeDeformationProvider() { _nd.reset(); }
protected:
/** @brief Struct for deformation specific parameters for this
class. This class can be used to transfer deformation
properties to other instances. */
struct DeformationParams {
km::HomTran
body_to_node_transform; //!< the body to node transform value in undeformed state
DeformationParams() = default;
/** @brief Copy constructor
@param p instance to copy from
@return the copied instance
*/
DeformationParams &operator=(const DeformationParams *p) {
if (this != p) {
body_to_node_transform = p->body_to_node_transform;
}
return *this;
}
};
protected:
/**
* @brief Return the node deformation parameters
* @return the deformation parameters
*/
virtual kc::ks_ptr<DeformationParams> _getParams() const = 0;
/**
* @brief Set the node deformation parameters
* @param params the deformation parameters
*/
virtual void _setParams(const DeformationParams ¶ms) = 0;
protected:
/** the parent node */
kc::ks_ptr<Node> _nd = nullptr;
};
/**
* @class Node
* @brief Represents the body node base class
*
* This class is for nodes attached to PhysicalBody instances. See
* \sref{body_nodes_sec} section for more information on the Node
* class.
*/
class Node : public kf::Frame {
// for access to _parent_body
friend class PhysicalBody;
// for access to _deformation_provider
friend class PhysicalModalBody;
// for access to _newtonian2node_f2f
friend class CompoundSubhinge;
friend class Multibody;
friend class SubGraph;
friend class PhysicalSubhinge;
// for access to external_force_cache
friend class HingePnode;
public:
/**
* @brief Factory method to create a Node instance.
*
* This method first checks if a detached node is available for use. If
* not, the method creates a new Node for the body.
*
* @param bd the physical parent body
* @param name the name for the node
* @param force_node if true, the node is marked as one that can apply forces
* @return a Node instance
*/
static kc::ks_ptr<Node>
lookupOrCreate(std::string_view name, kc::ks_ptr<PhysicalBody> bd, bool force_node = false);
/**
* @brief Factory method to create a Node instance that is used for contact forces.
*
* A contact force node is one whose forces will only apply for for one dynamics
* calculation. Whenever the externals are cleared, these nodes will become inactive. These
* nodes are always force nodes. If no name is provided, one will be given to them
* automatically.
*
* This method first checks if a detached node is available for use. If
* not, the method creates a new Node for the body.
*
* @param bd The physical parent body
* @param T The transform of the node relative to the body.
* @param name instance name
* @return a Node instance
*/
static kc::ks_ptr<Node> lookupOrCreateContact(const kc::ks_ptr<PhysicalBody> &bd,
const km::HomTran &T,
std::string_view name = "");
/**
* @brief Method to detach the node from the current parent body
*
* Once detached, the node is put in the pool of detached nodes
* for possible reuse when new nodes are needed.
*/
virtual void detach();
/**
* @brief Node 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 node.
* @param bd PhysicalBody to attach the node to.
*/
Node(std::string_view name, const kc::ks_ptr<PhysicalBody> &bd);
/**
* @brief Node destructor.
*/
virtual ~Node();
/**
* @brief Returns the type string of the SubTree.
* @return The type string.
*/
/**
* @brief Return the PhysicalBody parent body for the node.
* @return the parent PhysicalBody instance
*/
const kc::ks_ptr<PhysicalBody> &parentBody() const { return _parent_body; }
bool isReady() const override;
/**
* @brief Set the node's transform with respect to the parent PhysicalBody's body frame
*
* This transform defines the Node's location for an undeformed
* PhysicalBody parent body. Use Karana::Frame::FrameToFrame
* queries to find the actual location of a Node after body
* deformation.
*
* @param t the offset transform
*/
void setBodyToNodeTransform(const km::HomTran &t);
/**
* @brief Return the node's transform with respect to the parent PhysicalBody's body frame
*
* This transform defines the Node's location for an undeformed
* PhysicalBody parent body. Use Karana::Frame::FrameToFrame
* queries to find the actual location of a Node after body
* deformation.
*
* @return the node's offset transform
*/
km::HomTran getBodyToNodeTransform() const;
/**
* @brief Check whether the Node is a force node
*
* A force node is one that has been designated as one that can
* apply forces on the body. See \sref{body_nodes_sec} section
* for more on force nodes.
*
* @return true if the node is a force node
*/
bool isExternalForceNode() const;
/**
* @brief Return the node frame observed spatial acceleration of
* the node with respect to the Newtonian frame.
*
* This is the spatial acceleration of the node with respect to the
* Newtonian Karana::Frame::Frame, as observed from and
* represented in the node frame. Note that this is *not* and
* should not be confused with
* newtoninan_frame.frameToFrame(node).relSpAccel() which returns
* the Newtonian frame observed acceleration of the node with
* respect to the Newtonian frame!
*
* @return the spatial acceleration vector
*/
km::SpatialVector nodeObservedSpatialAccel() const;
/**
* @brief Get the external spatial force at the node represented in the node frame.
@return the spatial force vector
*/
const km::SpatialVector &getSpForce() const { return _external_force_cache->get(); }
/**
* @brief Set the external spatial force at the node represented
* in the specified Karana::Frame::Frame frame.
*
* This method resets the specified spatial force to the
* currently set spatial force in the node. If no frame is
* specified, then the spatial force is assumed to be
* represented in the local node frame, else the force is
* immediately rotated from the specified frame into the node's
* local frame. Note that most algorithms will check this node
* for a spatial force only it has been registered as a force
* node - so make sure that the node has bee registered as a force
* node.
*
* @param spforce The spatial force value
* @param ref_frame The reference Karana::Frame::Frame frame for the spatial force values
*/
virtual void setExternalSpForce(const km::SpatialVector &spforce,
const kc::ks_ptr<kf::Frame> &ref_frame = nullptr);
/**
* @brief Accumulate the specified external spatial force at the node
* represented in the specified Karana::Frame::Frame frame.
*
* Accumulate the external spatial force at the node represented
* in the specified frame. This method adds the specified spatial
* force to the currently set spatial force in the node. If no
* frame is specified, then the spatial force is assumed to be
* represented in the local node frame. Note that most
* algorithms will check this node for a spatial force only it
* has been registered as a force node - so make sure that this
* has been done.
*
* @param spforce The spatial force value
* @param ref_frame The reference frame for the spatial force values
*/
virtual void accumExternalSpForce(const km::SpatialVector &spforce,
const kc::ks_ptr<kf::Frame> &ref_frame = nullptr);
/**
* @brief Return the <NodeDeformationProvider deformation provider for the node
* @return the NodeDeformationProvider instance
*/
const kc::ks_ptr<NodeDeformationProvider> &deformationProvider() const {
return _deformation_provider;
}
std::string dumpString(std::string_view prefix,
const Karana::Core::Base::DumpOptions *options) 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> newtonianToNodeFrameToFrame() const;
protected:
/**
* @brief Zero out the external spatial force at the node.
*/
void _clearExternals();
/**
* @brief Discard a node.
* @param base - A base pointer to the node to discard.
*/
void _discard(kc::ks_ptr<kc::Base> &base) override;
/**
* @brief Create stick parts for the node
*
* @param scene the proxy scene
* @param c the stick parts configuration data
* @param add_sphere if true, add a sphere scene part for the node
*/
void _createStickParts(kc::ks_ptr<ks::ProxyScene> &scene,
const StickPartsConfig &c,
bool add_sphere);
protected:
/** the parent body the node is attached to */
kc::ks_ptr<PhysicalBody> _parent_body = nullptr;
/** external force being applied in the node frame */
km::SpatialVector _node_frame_spforce;
/** data cache for handling external forces */
kc::ks_ptr<kc::DataCache<km::SpatialVector>> _external_force_cache = nullptr;
/** f2f from the Newtonian frame to this node */
kc::ks_ptr<kf::OrientedChainedFrameToFrame> _newtonian2node_f2f = nullptr;
/** f2f from the parent body's pnode to this node */
kc::ks_ptr<kf::OrientedChainedFrameToFrame> _pnode2node_f2f = nullptr;
/** helper class that carries data and methods for the specific type
of node deformation */
kc::ks_ptr<NodeDeformationProvider> _deformation_provider = nullptr;
/** the multibody instance the node belongs to */
kc::ks_ptr<Multibody> _multibody = nullptr;
};
/**
* @class: ConstraintNode
* @brief This class is used by LoopConstraintBase loop constraints as attachment body nodes.
*
* ConstraintNodes are always force nodes, but do not allow the
* accumulation of forces. The node constraint forces are computed
* automatically by dynamics algorithms such as
* Karana::Dynamics::Algorithms::evalForwardDynamics(). See the
* \sref{cutjoint_constraints_sec} section for more on constraint nodes.
*/
class ConstraintNode : public Node {
// for access to setExternalSpForce
friend class LoopConstraintBase;
friend class CECompoundBody;
// for access to setExternalSpForceT
friend class LoopConstraintCutJoint;
friend class LoopConstraintConVel;
friend class Algorithms;
public:
/**
* @brief Factory method to create a ConstraintNode instance.
*
* This method first checks if a detached constraint node is
* available for use. If not, the method creates a new
* ConstraintNode for the body.
*
* @param bd the physical parent body
* @param name the name for the node
* @return a ConstraintNode instance
*/
static kc::ks_ptr<ConstraintNode> lookupOrCreate(std::string_view name,
kc::ks_ptr<PhysicalBody> bd);
/**
* @brief Constructor
*
* This method first checks if a detached constraint node is
* available for use. If not, the method creates a new
* ConstraintNode for the body.
*
* @param name the name for the node
* @param bd the physical parent body
*/
ConstraintNode(std::string_view name, kc::ks_ptr<PhysicalBody> bd)
: Node(name, bd){};
/**
* @brief Return the loop constraint using this constraint node
*
* @return the loop constraint using this node.
*/
kc::ks_ptr<LoopConstraintBase> loopConstraint() const;
std::string
dumpString(std::string_view prefix = "",
const Karana::Core::Base::DumpOptions *options = nullptr) const override;
protected:
/** detach the node from the current parent body, and put in the
pool of detached nodes for possible reuse */
void detach() override;
/** @brief Method to directly set the external force at the node
Making this method protected since the constraint forces at the
nodes are computed algorithmically by methods such as
Karana::Dynamics::Algorithms::evalForwardDynamics()
@param spforce the external force value
@param ref_frame the frame in which the spforce is represented
*/
void setExternalSpForce(const km::SpatialVector &spforce,
const kc::ks_ptr<kf::Frame> &ref_frame = nullptr) override;
/**
* @brief Discard a constraint node.
* @param base - A base pointer to the constraint node to discard.
*/
void _discard(kc::ks_ptr<kc::Base> &base) override;
protected:
/** loop constraint that this constraint node is being used by */
kc::ks_ptr<LoopConstraintBase> _loop_constraint = nullptr;
};
} // namespace Karana::Dynamics