Program Listing for File LoopConstraintCutJoint.h#
↰ Return to documentation for file (include/Karana/SOADyn/LoopConstraintCutJoint.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 LoopConstraintCutJoint class.
*/
#pragma once
#include "Karana/SOADyn/FramePairHinge.h"
#include "Karana/SOADyn/LoopConstraintBase.h"
namespace Karana::Dynamics {
class Multibody;
class HingePnode;
class ConstraintNode;
class FramePairHinge;
class PhysicalBody;
/**
* @class LoopConstraintCutJoint
* @brief Class for loop constraints that are based on a hinge to define the constraint
* See the \sref{cutjoint_constraints_sec} section for more discussion on cutjoint loop
* constraints.
*/
class LoopConstraintCutJoint : public LoopConstraintBase {
// for access to _sourceParentPnode
friend class SubGraph;
// for access to _error_f2f
friend class Multibody;
// for access to residual methods for the TA algorithm
friend class Algorithms;
// For access to _cleanup_source.
friend class PhysicalBody;
public:
/**
* @brief Factory method for creating a LoopConstraintCutJoint instance
*
* @param mb The Multibody instance
* @param constraint_f_to_f the Karana::Frame::FrameToFrame defining the constrained Frame
* pair
* @param name the name for the loop constraint instance
* @param htype the HingeBase::HingeType defining the constraint type
* @param subhinge_types the list of SubhingeType subhinge types for a custom
* hinge type
* @return a LoopConstraintCutJoint instance
*/
static const kc::ks_ptr<LoopConstraintCutJoint>
create(std::string_view name,
kc::ks_ptr<Multibody> &mb,
kc::ks_ptr<kf::FrameToFrame> constraint_f_to_f,
HingeType htype,
const std::vector<SubhingeType> &subhinge_types = std::vector<SubhingeType>());
/**
* @brief Constructor
*
* @param mb The Multibody instance
* @param constraint_f_to_f the Karana::Frame::FrameToFrame defining the constrained Frame
* pair
* @param name the name for the loop constraint instance
* @param htype the HingeType defining the constraint type
* @param subhinge_types the list of SubhingeType subhinge types for a custom
* hinge type
*/
LoopConstraintCutJoint(
std::string_view name,
kc::ks_ptr<Multibody> &mb,
kc::ks_ptr<kf::FrameToFrame> constraint_f_to_f,
HingeType htype,
const std::vector<SubhingeType> &subhinge_types = std::vector<SubhingeType>());
/**
* @brief Return the FramePairHinge hinge associated with this loop constraint
*
* When the constraint is satisfied, the relative configuration,
* velocity and accel values across the f2f are constrained to those
* satisfied by the associated constraint hinge
*
* @return the FramePairHinge instance
*/
kc::ks_ptr<FramePairHinge> hinge() const { return _hinge; }
km::Vec poseError() const override;
/**
* @brief Return the interbody spatial force at the source node for TA dynamics
*
* This method returns the interbody force at the source when
* doing TA dynamics for the loop constraint. The force has the
* sign as if acting on the target node (this is consistent with
* our general convention to book keep the pnode's inter-body
* force at the onode). The term also includes contributions
* from any generalized forces at the cut-joint.
*
* The method should only be called after TA forward dynamics has
* been called, since it expects the constraint forces to be set
* in the constraint nodes. Also this method assumes that the T
* value in the CoordData member is the cutjoint generalized force
* value.
*
* @return the inter-body spatial force
*/
km::SpatialVector getInterBodyForceTAFwdDyn();
/**
* @brief Convert this loop constraint into an inter-body PhysicalHinge
*
* Replaces the loop constraint with a hinge between the
* constraint nodes to make one body the child of the
* other. This method can only be called when the new child body
* (either of source or target) is a base body with a 6 dof
* hinge. This requirement allows us to preserve the system's
* tree topology. If reverse false, the target node's body is
* made the child of the source node's body, and the converse if
* reverse is true. The original loop constraint is discarded by
* this method.
*
* @param lc The loop constraint to replace with a hinge
* @param reverse If true, reverse the polarity of the hinge
* @return the new hinge
*/
static kc::ks_ptr<PhysicalHinge> toPhysicalHinge(kc::ks_ptr<LoopConstraintCutJoint> lc,
bool reverse);
size_t nResiduals() const override { return 6 - _hinge->nU(); }
/**
* @brief Return the current constraint spatial velocity error
* @return the spatial velocity error as a 6-vector
*/
km::Vec velError() const override;
/**
* @brief Return the current constraint spatial acceleration error
* @return the spatial acceleration error as a 6-vector
*/
km::Vec accelError() const override;
/**
* @brief Apply the input T generalized force at the constraint hinge as an external force.
*
* This method is used to handle any generalized forces at
* cut-joints. Once the joint is cut, its generalized forces are
* no longer included in the normal dynamics. The workaround is
* to convert the cut-joint generalized forces into equivalent
* equal and opposite external forces at the constraint nodes
* for the cut-joint.
*
* @param T the input generalized forces
* @param no_accumulate if true, then accumulate the input forces at the constraint nodes
*/
void spExternalForceFromT(const km::Vec &T, bool no_accumulate = true);
/**
* @brief the Karana::Frame::FrameToFrame that measures the constraint residual error
*
* The constraint is fully satisfied only when this f2f relative
* transform, spatial velocity and acceleration values are zero,
* i.e. its oframe and pframe are coincident
*
* @return the error FrameToFrame instance
*/
kc::ks_ptr<kf::FrameToFrame> errorFrameToFrame() const { return _error_f2f; }
std::string
dumpString(std::string_view prefix = "",
const Karana::Core::Base::DumpOptions *options = nullptr) const override;
protected:
km::Vec _accelResidual() const override;
/** Update the generalized force values for the constraint hinge
from the constraint forces. This is used when running inverse
dynamics with loops, to recover the constraint hinge
generalized forces from the constraint forces. */
void _updateInvDynT();
protected:
/**
* @brief Discard the provided LoopConstraintCutJoint.
* @param base - Base pointer to the LoopConstraintCutJoint to discard.
*/
void _discard(kc::ks_ptr<Base> &base) override;
void _computeQMats(QMats &val) override;
/** @brief Get the full spatial force needed at the target
constraint node for the generalized force being applied at
the constraint hinge
@param T the applied generalized force
@return the spatial force at the target constraint node
*/
km::SpatialVector _constraintSpForceTToTarget(const km::Vec &T) const;
/** @brief Get the full spatial force needed at the source
constraint node for the spatial force being applied at
the target constraint node
@param tgt_f the spatial force at the target constraint node
@return the spatial force at the source constraint node
*/
km::SpatialVector _constraintSpForceTToSource(const km::SpatialVector &tgt_f) const;
km::Vec _poseResidual() const override;
km::Vec _velResidual() const override;
protected:
/** the f2f measure the error for the constraint. The constraint is
satisfied when the transform etc values for this f2f are
zero */
kc::ks_ptr<kf::FrameToFrame> _error_f2f;
/** the cut-joint constraint's hinge type */
std::optional<HingeType> _hinge_type = std::nullopt;
private:
kc::ks_ptr<FramePairHinge> _hinge = nullptr;
/**
* If true, the also cleanup the source frame during discard.
* Used so that frames created by toCutJointConstraint are cleaned up
* correctly.
*/
bool _cleanup_source = false;
};
} // namespace Karana::Dynamics