Program Listing for File LoopConstraintBase.h#
↰ Return to documentation for file (include/Karana/SOADyn/LoopConstraintBase.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 LoopConstraintBase and LoopConstraintConVel classes.
*/
#pragma once
#include "Karana/SOADyn/CoordinateConstraint.h"
namespace Karana::Dynamics {
class Multibody;
class HingePnode;
class ConstraintNode;
class PhysicalHinge;
/**
* @class LoopConstraintBase
* @brief Base class for loop constraints
* See the \sref{constraints_sec} section for more discussion on loop constraints.
*/
class LoopConstraintBase : public BilateralConstraintBase {
// 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;
public:
/**
* @brief Constructor
* @param mb the multibody instance
* @param type The bilateral constraint type
* @param constraint_f2f the FrameToFrame being constrained
* @param name instance name
*/
LoopConstraintBase(std::string_view name,
kc::ks_ptr<Multibody> &mb,
BilateralConstraintType type,
kc::ks_ptr<kf::FrameToFrame> constraint_f2f);
/**
* @brief Return the Karana::Frame::FrameToFrame whose oframe/pframe define the constraint
*
* When the constraint is satisfied, the relative configuration,
* velocity and accel values across the
* Karana::Frame::FrameToFrame f2f are constrained to those
* satisfied by the associated constraint hinge
*
* @return the constraint FrameToFrame instance
*/
kc::ks_ptr<kf::FrameToFrame> constraintFrameToFrame() const { return _constraint_f2f; }
/**
* @brief Return true if the loop constraint constrains Q configuration coordinates
* @return true if the loop constraint constrains the Q coordinates
*/
virtual bool constrainsQ() const { return true; }
/**
* @brief Return the source (oframe) node for the constraint
*
* Return the source frame as a constraint node, if the source
* frame is attached to a non-virtual root body. Return a null
* pointer if the source frame is not attached to a multibody
* body.
*
* @return the source constraint node.
*/
kc::ks_ptr<ConstraintNode> sourceNode() const { return _source_node; }
std::string
dumpString(std::string_view prefix = "",
const Karana::Core::Base::DumpOptions *options = nullptr) const override;
/**
* @brief Return the target (pframe) node for the constraint
*
* Return the target frame as a constraint node, if the target
* frame is attached to a non-virtual root body. Return a null
* pointer if the target frame is not attached to a multibody
* body.
*
* @return the source constraint node.
*/
kc::ks_ptr<ConstraintNode> targetNode() const { return _target_node; }
protected:
/** @brief Struct for the constraint QMat matrices needed for
* kinematics analysis, Gamma, TA dynamics algorithm. The
* constraint requirement is that
*
* [source_Q, target_Q] * [V_source \\ V_target] = 0
*
* where V_source and V_target are the spatial velocities (with respect to the
* inertial frame) of the source and target frames respectively
* for the constraint Karana::Frame::FrameToFrame f2f.
*
* rel_Q on the other hand acts directly on the the relative
* spatial velocity (source frame derivative) across the
* constraint oframe/pframe as follows:
*
* rel_Q * V(source, target) = 0
*
* We also in some cases allow the RHS in the above equations
* to non-zero for user inputs.
*/
struct QMats {
public:
/// acts on V(source), inertial frame derivative
km::Mat source_Q; // NOLINT(readability-identifier-naming)
/// acts on V(target), inertial frame derivative
km::Mat target_Q; // NOLINT(readability-identifier-naming)
/// acts on relative V(source, target), source frame derivative
km::Mat rel_Q; // NOLINT(readability-identifier-naming)
public:
/** @brief Resize the constraint matrices.
@param n_c The number of constrained dofs.
*/
void resize(size_t n_c) {
source_Q.resize(n_c, 6);
target_Q.resize(n_c, 6);
rel_Q.resize(n_c, 6);
}
};
/** Return the constraint QMat matrices for this constraint
* @returns the QMats struct for this constraint
*/
const QMats &getQMats() const { // NOLINT(readability-identifier-naming) TODO
return _matsQ_cache->get();
}
protected:
/** @brief Convert the lambda Lagrange multiplier term for the constraint
into constraint forces at its constraint nodes
@param lambda the Lagrange multiplier value
*/
void _lambda2ConstraintForces(const km::Vec &lambda) const;
protected:
/**
* @brief Discard the provided LoopConstraintBase.
* @param base - Base pointer to the LoopConstraintBase to discard.
*/
void _discard(kc::ks_ptr<Base> &base) override;
/**
* @brief Callback to compute the source and target Q matrices for the constraint
*
* @param val the data buffer for the computed value
*/
virtual void _computeQMats(QMats &val);
/** the defining Karana::Frame::FrameToFrame for this constraint. The only
motion allowed across this f2f is that permitted by the
QMat constraint matrices across this f2f. */
kc::ks_ptr<kf::FrameToFrame> _constraint_f2f;
/** the constraint node of the physical body the source frame is attached
to. if null, the source frame is not attached to a body */
kc::ks_ptr<ConstraintNode> _source_node = nullptr;
/** the constraint node of the physical body the target frame is attached
to. if null, the target frame is not attached to a body */
kc::ks_ptr<ConstraintNode> _target_node = nullptr;
/** cache for computing the nResiduals()x6 the Q related orthogonal
complement matrices for the constraint hinge coordinate
matrix. */
kc::ks_ptr<kc::DataCache<QMats>> _matsQ_cache = nullptr;
private:
};
/**
* @class LoopConstraintConVel
* @brief Class for constant velocity loop constraints
*
* This constraint requires that a component of the relative
* angular/linear velocity of a pair of body constraint nodes
* projected onto a unit axis be equal. This constraint only applies
* at the velocity and acceleration levels, and not the positional
* level.
*
* See the \sref{convel_loop_constraints_sec} section for more discussion on convel loop
* constraints.
*/
class LoopConstraintConVel : public LoopConstraintBase {
// for access to _accelResidual
friend class Algorithms;
public:
/**
* @brief Factory method for creating a LoopConstraintConVel 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
* @return a LoopConstraintCutJoint instance
*/
static const kc::ks_ptr<LoopConstraintConVel>
create(std::string_view name,
kc::ks_ptr<Multibody> &mb,
kc::ks_ptr<kf::FrameToFrame> constraint_f_to_f);
/**
* @brief Constructor
*
* @param mb The Multibody instance
* @param constraint_f2f the Karana::Frame::FrameToFrame defining the constrained Frame
* pair
* @param name the name for the loop constraint instance
*/
LoopConstraintConVel(std::string_view name,
kc::ks_ptr<Multibody> &mb,
kc::ks_ptr<kf::FrameToFrame> constraint_f2f);
/** @brief Return true if the constraint applied to Q configuration coordinates.
@return false since convel constraints only impact velocities
*/
bool constrainsQ() const override { return false; }
size_t nResiduals() const override;
km::Vec poseError() const override;
/**
* @brief Set the velocity and acceleration inputs for the convel constraint.
*
* The input is used to set the difference between the source
* and target component velocity and acceleration component
* values.
*
* @param vel The input velocity
* @param accel The input accel
*/
void setInput(double vel, double accel);
/**
* @brief Set the unit axis long which the convel constraint applies.
*
* @param axis The unit axis vector in the source node frame
* @param is_rotational If true, the constraint is assumed to on relative angular velocity,
* else translational
*/
void setUnitAxis(const km::Vec &axis, bool is_rotational);
/**
* @brief Get the unit axis to which the convel constraint applies.
*
* @returns The unit axis vector in the source node frame
*/
const km::Vec3 &getUnitAxis() const;
/**
* @brief Get whether the constraint is on relative angular velocity
*
* @returns If the constraint is angular, else linear
*/
bool isRotational() const;
bool isReady() const override;
km::Vec velError() const override;
km::Vec accelError() const override;
std::string
dumpString(std::string_view prefix = "",
const Karana::Core::Base::DumpOptions *options = nullptr) const override;
protected:
void _computeQMats(QMats &val) override;
km::Vec _poseResidual() const override;
km::Vec _velResidual() const override;
km::Vec _accelResidual() const override;
private:
/** the constraint unit axis */
km::Vec3 _unit_axis;
/** the constraint type - rotational or translational */
bool _is_rotational = true;
double _input_vel = 0;
double _input_accel = 0;
};
} // namespace Karana::Dynamics