Program Listing for File FramePairHinge.h#
↰ Return to documentation for file (include/Karana/SOADyn/FramePairHinge.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/OrientedChainedFrameToFrame.h"
#include "Karana/Math/NonlinearSolver.h"
#include "Karana/ProxyScene/ProxyScene.h"
#include "Karana/SOADyn/Defs.h"
#include "Karana/SOADyn/HingeBase.h"
#include "Karana/SOADyn/PhysicalSubhinge.h"
// #include "Karana/SOADyn/Multibody.h"
namespace Karana::Dynamics {
namespace kc = Karana::Core;
namespace kf = Karana::Frame;
namespace km = Karana::Math;
namespace ks = Karana::Scene;
class PinSubhinge;
class FrameToFrameJacobianGenerator;
/**
* @class FramePairHinge
* @brief Represents the articulation class used by PhysicalHinge and LoopConstraintCutJoint
* physical articulation classes
*
* See \sref{body_hinge_sec} section for more discussion on hinges.
*
*/
class FramePairHinge : public HingeBase, public kf::OrientedChainedFrameToFrame {
/* for access to _changeActionT */
friend class PhysicalSubhinge;
/* for access to _oneTimeSetup */
friend class LoopConstraintCutJoint;
template <int, int> friend class PhysicalSubhinge_T;
/* for access to _newtonian_frame */
friend class Multibody;
/* for access to _createStickParts */
friend class PhysicalBody;
/* for access to subhingeTypes */
friend class SubGraph;
/* for access to pframeCoordMapMatrix */
friend class PhysicalModalBody;
public:
/**
* @brief FramePairHinge destructor.
*/
virtual ~FramePairHinge();
/**
* @brief FramePairHinge creation static factory method.
*
* @param oframe The oframe
* @param pframe The pframe
* @param name instance name
* @param htype The hinge type
* @param subhinge_types The list of subhinge types for a custom hinge type
* @return A new FramePairHinge instance
*/
static kc::ks_ptr<FramePairHinge>
create(kc::ks_ptr<kf::Frame> oframe,
kc::ks_ptr<kf::Frame> pframe,
std::string_view name,
HingeType htype,
const std::vector<SubhingeType> &subhinge_types = std::vector<SubhingeType>());
/**
* @brief FramePairHinge constructor.
* @param oframe The oframe
* @param pframe The pframe
* @param name instance name
* @param htype The hinge type
* @param subhinge_types The list of subhinge types for a custom hinge type
*/
FramePairHinge(
kc::ks_ptr<kf::Frame> oframe,
kc::ks_ptr<kf::Frame> pframe,
std::string_view name,
HingeType htype,
const std::vector<SubhingeType> &subhinge_types = std::vector<SubhingeType>());
std::string_view typeString(bool brief = true) const noexcept override {
return kc::Base::typeString(brief);
}
std::string_view name() const override { return kf::OrientedChainedFrameToFrame::name(); }
public:
/**
* @brief Method to find best fit Q coordinates for the hinge for the input transform
*
* This method can be used to initialize the hinge's Q coordinates
* to values that best fit the input transform T. If f_to_f is
* non-null, then T is assumed to be the desired relative transform for
* the f_to_f, and if null then this hinge is used as the f_to_f.
*
* @param T Input transform
* @param f_to_f The frame to frame instance the T transform is for
* @return The residual error transform
*/
km::HomTran fitQ(const km::HomTran &T, kc::ks_ptr<FrameToFrame> f_to_f = nullptr);
/**
* @brief Method to find best fit U velocity coordinates for the input spatial velocity
* vector
*
* This method can be used to initialize the hinge U velocity
* coordinates to values that best fit the input spatial
* velocity for the hinge. If f_to_f is non-null, then V is assumed
* to be the desired spatial velocity for this f_to_f , and if null
* then this hinge is used as the f_to_f .
*
* @param V Input spatial velocity vector
* @param f_to_f The frame to frame instance the V spatial velocity is for
* @return The residual spatial velocity error
*/
km::SpatialVector fitU(const km::SpatialVector &V,
kc::ks_ptr<FrameToFrame> f_to_f = nullptr);
/**
* @brief Method to find best fit Udot accel coordinates for the input spatial acceleration
* vector
*
* This method can be used to initialize the hinge Udot
* acceleration coordinates to values that best fit the input
* spatial acceleration for the hinge. If f_to_f is non-null, then
* A is assumed to be the desired spatial accel for this f_to_f ,
* and if null then this hinge is used as the f_to_f .
*
* @param alpha Input spatial acceleration vector
* @param f_to_f The frame to frame instance the A spatial acceleration is for
* @return The residual spatial acceleration error
*/
km::SpatialVector fitUdot(const km::SpatialVector &alpha,
kc::ks_ptr<FrameToFrame> f_to_f = nullptr);
public:
std::string
dumpString(std::string_view prefix = "",
const Karana::Core::Base::DumpOptions *options = nullptr) const override;
protected:
/**
* @brief Return the list of SubhingeType subhinge types for this hinge
*
* @return the list of subhinge types
*/
const std::vector<SubhingeType> &
subhingeTypes() const { // NOLINT(readability-identifier-naming) TODO
return _subhinge_types;
}
/** @brief Return the overall 6xnU() pframe coord map matrix for the hinge
*
* Return the overall 6xnU() coord map matrix for the hinge
* referenced to and in the pframe frame with contributions from
* all of its subhinges. Note that the matrix will be
* configuration dependent when there are multiple subhinges. The
* product of this matrix with the generalized velocity U
* coordinates for the hinge returns the delta velocity across the
* hinge rigidly transformed to the pframe.
*
* @return the coord map matrix
*/
km::Mat pframeCoordMapMatrix() const override;
/** @brief Return the overall 6xnU() oframe coord map matrix for the hinge
*
* Return the overall 6xnU() coord map matrix for the hinge,
* referenced to and in the oframe frame with contributions from
* all of its subhinges. Note that the matrix will be
* configuration dependent when there are multiple subhinges. The
* product of this matrix with the generalized velocity U
* coordinates for the hinge returns the delta velocity across the
* hinge in the oframe.
*
* @return the coord map matrix
*/
km::Mat oframeCoordMapMatrix() const override;
/**
* @brief Return the Jacobian generator for the hinge
*
* This is used internally by the fit*() methods
*
* @return The Jacobian generator
*/
kc::ks_ptr<FrameToFrameJacobianGenerator> _jacgen();
/** @brief Carry out side effect of the the generalized force changing for
the subhinge.
An example side effect is to mark the ATBI filter data cache
for the hinge onode as not healthy for a physical hinge. */
virtual void _changeActionT() {};
/** @brief Helper method to set up this FramePairHinge */
void _oneTimeSetup();
/** @brief Helper method to tear down this FramePairHinge */
void _oneTimeTeardown();
/**
* @brief Create stick parts for the hinge
*
* @param scene the proxy scene
* @param c the stick parts configuration data
* @param constraint if true, this is a hinge constraint, else a regular one
*/
void _createStickParts(kc::ks_ptr<ks::ProxyScene> &scene,
const StickPartsConfig &c,
bool constraint) const;
/** @brief Helper method to create the subhinges for this hinge.
@param subhinge_types list of subhinge types for the hinge
*/
void _createPhysicalSubhinges(const std::vector<SubhingeType> &subhinge_types);
/** @brief Helper method to discard the subhinges for this hinge.
*/
void _discardPhysicalSubhinges();
/** @brief Helper method to create the nonlinear solver used by the fit*() methods */
void _makeNlSolver();
/** @brief Helper method to create the Jacobian generator used by the fit*() methods */
void _makeJacobianGenerator();
protected:
/** the Newtonian frame for dynamics */
kc::ks_ptr<kf::Frame> _newtonian_frame = nullptr;
/** the Jacobian generator for the hinge */
kc::ks_ptr<FrameToFrameJacobianGenerator> _hinge_jacgen = nullptr;
/** the nonlinear solver for the hinge */
kc::ks_ptr<km::NonlinearSolver> _nl_solver = nullptr;
/** frame used by the fitQ method. this frame is positioned at the
desired hinge transform pose, and the solver to drive this
frame to pframe relTransform to zero to get the best fit Q
coordinates */
kc::ks_ptr<kf::Frame> _fit_frame = nullptr;
};
} // namespace Karana::Dynamics