Program Listing for File PinSubhinge.h#
↰ Return to documentation for file (include/Karana/SOADyn/PinSubhinge.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 concrete subhinge classes, e.g., PinSubhinge,
* LockedSubhinge, etc.
*/
#pragma once
#include "Karana/SOADyn/PhysicalSubhinge.h"
namespace Karana::Dynamics {
namespace kf = Karana::Frame;
namespace km = Karana::Math;
/**
* @class LockedSubhinge
* @brief Represents a locked subhinge
*
* See \sref{subhinges_sec} section for more information on physical
* subhinges.
*/
class LockedSubhinge : public PhysicalSubhinge_T<0, 0> {
public:
/**
* @brief Constructs a LockedSubhinge.
@param oframe the oframe frame
@param pframe the pframe frame
@param name the name for the subhinge
@param hge the parent hinge
*/
LockedSubhinge(kc::ks_ptr<kf::Frame> oframe,
kc::ks_ptr<kf::Frame> pframe,
std::string_view name,
kc::ks_ptr<FramePairHinge> hge);
/**
* @brief LockedSubhinge destructor.
*/
virtual ~LockedSubhinge();
SubhingeType subhingeType() const override;
protected:
kc::ks_ptr<PhysicalSubhinge::PhysicalSubhingeParams> _getParams() const override;
void _setParams(const PhysicalSubhinge::PhysicalSubhingeParams ¶ms) override;
};
/**
* @class Physical1DofSubhinge
* @brief Base class for 1 degree of freedom physical subhinges
*
* See \sref{subhinges_sec} section for more information on physical
* subhinges.
*/
class Physical1DofSubhinge : public PhysicalSubhinge_T<1, 1> {
public:
/**
* @brief Constructs a Physical1DofSubhinge.
@param oframe the oframe frame
@param pframe the pframe frame
@param name the name for the subhinge
@param hge the parent hinge
*/
Physical1DofSubhinge(kc::ks_ptr<kf::Frame> oframe,
kc::ks_ptr<kf::Frame> pframe,
std::string_view name,
kc::ks_ptr<FramePairHinge> hge);
~Physical1DofSubhinge(){};
/**
* @brief Set the unit axis for the pin hinge.
* @param axis The unit axis for the pin hinge.
*/
virtual void setUnitAxis(const km::Vec3 &axis);
/**
* @brief Get the unit axis for the pin hinge.
* @returns The unit axis for the pin hinge.
*/
const km::Vec3 &getUnitAxis() const { return _unit_axis; }
/**
* @brief Set the lower/upper pair of joint limit values for the 1 dof subhinge
* @param limits the lower and upper limit values
*/
void setJointLimits(const Karana::Math::Vec &limits);
/**
* @brief Return the lower/upper pair of joint limit values for the 1 dof subhinge
* @return array with the lower and upper joint limit values
*/
km::Vec getJointLimits() const { return _joint_limits; }
/**
* @brief Set the gear ratio for the 1 dof subhinge
* @param gear_ratio the gear ratio
*/
void setGearRatio(double gear_ratio);
/**
* @brief Return the gear ratio for the 1 dof subhinge
* @return the gear ratio value
*/
double getGearRatio() const { return _gear_ratio; }
/** @brief Specialization of param class for this subhinge type */
struct PhysicalSubhingeParams : public PhysicalSubhinge::PhysicalSubhingeParams {
km::Vec3 unit_axis; //!< the subhinge unit axis
PhysicalSubhingeParams() = default;
/** @brief Assignment operator
@param p the other params
@return the updated params
*/
PhysicalSubhingeParams &operator=(const PhysicalSubhinge::PhysicalSubhingeParams *p) {
if (this != p) {
PhysicalSubhinge::PhysicalSubhingeParams::operator=(
*p); // Call base class assignment operator
// Assign Derived-specific members here
unit_axis = static_cast<const PhysicalSubhingeParams *>(p)->unit_axis;
}
return *this;
}
};
bool isReady() const override;
std::string
dumpString(std::string_view prefix = "",
const Karana::Core::Base::DumpOptions *options = nullptr) const override;
protected:
kc::ks_ptr<PhysicalSubhinge::PhysicalSubhingeParams> _getParams() const override;
void _setParams(const PhysicalSubhinge::PhysicalSubhingeParams ¶ms) override;
protected:
/** joint limits for the subhinge, initialized by default to
NaNs */
km::Vec _joint_limits = km::Vec(2);
/** gear ratio for the pin subhinge, initialized by default to a
NaN */
double _gear_ratio = km::notReadyNaN;
km::Vec3 _unit_axis = {0, 0, 0}; ///< axis of rotation in pframe
};
/**
* @class PinSubhinge
* @brief Represents a 1 dof rotational subhinge
*
* See \sref{subhinges_sec} section for more information on physical
* subhinges.
*
*/
class PinSubhinge : public Physical1DofSubhinge {
// for access to constructor
// friend class PinHinge;
friend class PhysicalBody;
public:
/**
* @brief PinSubhinge destructor.
*/
virtual ~PinSubhinge(){};
SubhingeType subhingeType() const override;
/**
* @brief Constructs a PinSubhinge.
@param oframe the oframe frame
@param pframe the pframe frame
@param name the name for the subhinge
@param hge the parent hinge
*/
PinSubhinge(kc::ks_ptr<kf::Frame> oframe,
kc::ks_ptr<kf::Frame> pframe,
std::string_view name,
kc::ks_ptr<FramePairHinge> hge);
/**
* @brief Set the unit axis for the pin hinge.
* @param axis The unit axis for the pin hinge.
*/
void setUnitAxis(const km::Vec3 &axis) override;
protected:
void _computeTransform(Karana::Math::HomTran &T) override;
void _computeVelocity(Karana::Math::SpatialVector &V) override;
void _computeAccel(Karana::Math::SpatialVector &a) override;
};
/**
* @class LinearSubhinge
* @brief Represents a 1 dof translational subhinge
*
*
* See \sref{subhinges_sec} section for more information on physical
* subhinges.
*/
class LinearSubhinge : public Physical1DofSubhinge {
public:
/**
* @brief LinearSubhinge destructor.
*/
virtual ~LinearSubhinge(){};
SubhingeType subhingeType() const override;
/**
* @brief Set the unit axis for the pin hinge.
* @param axis The unit axis for the pin hinge.
*/
void setUnitAxis(const km::Vec3 &axis) override;
/**
* @brief Constructs a LinearSubhinge.
@param oframe the oframe frame
@param pframe the pframe frame
@param name the name for the subhinge
@param hge the parent hinge
*/
LinearSubhinge(kc::ks_ptr<kf::Frame> oframe,
kc::ks_ptr<kf::Frame> pframe,
std::string_view name,
kc::ks_ptr<FramePairHinge> hge);
protected:
void _computeTransform(Karana::Math::HomTran &T) override;
void _computeVelocity(Karana::Math::SpatialVector &V) override;
void _computeAccel(Karana::Math::SpatialVector &a) override;
};
/**
* @class ScrewSubhinge
* @brief Represents a 1 dof helical subhinge with coupled rotation/translation about a common
* axis
*
* See \sref{subhinges_sec} section for more information on physical
* subhinges.
*/
class ScrewSubhinge : public Physical1DofSubhinge {
public:
/**
* @brief ScrewSubhinge destructor.
*/
virtual ~ScrewSubhinge(){};
SubhingeType subhingeType() const override;
/**
* @brief Set the unit axis for the pin hinge.
* @param axis The unit axis for the pin hinge.
*/
void setUnitAxis(const km::Vec3 &axis) override;
bool isReady() const override;
/**
* @brief Set the pitch value for the subhinge
*
* The pitch is defined as meters of translation/radian.
*
* @param pitch the pitch value
*/
void setPitch(double pitch);
/**
* @brief Return the pitch value for the subhinge
*
* The pitch is defined as meters of translation/radian.
*
* @return the pitch value
*/
double getPitch() const { return _pitch; }
/**
* @brief Constructs a ScrewSubhinge.
@param oframe the oframe frame
@param pframe the pframe frame
@param name the name for the subhinge
@param hge the parent hinge
*/
ScrewSubhinge(kc::ks_ptr<kf::Frame> oframe,
kc::ks_ptr<kf::Frame> pframe,
std::string_view name,
kc::ks_ptr<FramePairHinge> hge);
protected:
void _computeTransform(Karana::Math::HomTran &T) override;
void _computeVelocity(Karana::Math::SpatialVector &V) override;
void _computeAccel(Karana::Math::SpatialVector &a) override;
/** @brief Specialization of param class for this subhinge type */
struct PhysicalSubhingeParams : public Physical1DofSubhinge::PhysicalSubhingeParams {
double pitch; //!< the screw's pitch (meters of translation/radian)
/**
* @brief Assignment operator.
*
* @param p The PhysicalSubhingeParams to copy values from.
* @return A reference to the newly assigned PhysicalSubhingeParams.
*/
// 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]
PhysicalSubhingeParams &operator=(const PhysicalSubhinge::PhysicalSubhingeParams *p) {
if (this != p) {
Physical1DofSubhinge::PhysicalSubhingeParams::operator=(
p); // Call base class assignment operator
pitch = static_cast<const PhysicalSubhingeParams *>(p)->pitch;
}
return *this;
}
};
kc::ks_ptr<PhysicalSubhinge::PhysicalSubhingeParams> _getParams() const override;
void _setParams(const PhysicalSubhinge::PhysicalSubhingeParams ¶ms) override;
private:
double _pitch;
};
/**
* @class SphericalSubhinge
* @brief Represents a 1 dof rotational subhinge using minimal RotationVector coords
*
* See \sref{subhinges_sec} section for more information on physical
* subhinges.
*/
class SphericalSubhinge : public PhysicalSubhinge_T<3, 3> {
public:
/**
* @brief SphericalSubhinge destructor.
*/
virtual ~SphericalSubhinge();
SubhingeType subhingeType() const override;
/**
* @brief Constructs a SphericalSubhinge.
@param oframe the oframe frame
@param pframe the pframe frame
@param name the name for the subhinge
@param hge the parent hinge
*/
SphericalSubhinge(kc::ks_ptr<kf::Frame> oframe,
kc::ks_ptr<kf::Frame> pframe,
std::string_view name,
kc::ks_ptr<FramePairHinge> hge);
const km::Vec &getQdot() const override;
void setU(const Eigen::Ref<const km::Vec> &val) override;
std::string dumpString(std::string_view prefix,
const Karana::Core::Base::DumpOptions *options) const override;
/* Report that this is a type of subhinge that has coordinate
sanitization needs (for recentering the chart) */
bool requiresCoordSanitization() const override { return true; }
/**
* @brief Set the threshold for the RotationVector coordinates chart recentering
*
* Set the max allowed angle before the chart is recentered when
* sanitizeCoords() is called.
*
* @param val the threshold value
*/
void setMaxChartAngle(double val) { _max_chart_angle = val; }
/**
* @brief Return the threshold for the RotationVector coordinates chart recentering
*
* Return the max allowed angle before the chart is recentered when
* sanitizeCoords() is called.
*
* @return the threshold value
*/
double getMaxChartAngle() const { return _max_chart_angle; }
/* Recenter the local chart if the coordinates angle exceeds
the allowed max for the chart */
bool sanitizeCoords() override;
/**
* @brief Return the current offset for the RotationVector coordinates chart
* @return the chart offset UnitQuaternion
*/
const km::UnitQuaternion &getChartOffset() const { return _chart_offset; }
void resetChart() override { _chart_offset.setIdentity(); }
/** @brief Return the global Q coordinates - not the chart relative ones
@return the Q coordinates
*/
const km::Vec &getQ() const override;
/** @brief Set the global Q coordinates - not the chart relative ones.
@param val the input Q coordinates
*/
void setQ(const Eigen::Ref<const km::Vec> &val) override;
protected:
void _computeTransform(Karana::Math::HomTran &T) override;
void _computeVelocity(Karana::Math::SpatialVector &V) override;
void _computeAccel(Karana::Math::SpatialVector &a) override;
/**
* @brief Data cache callback to compute the Qdot values
*
* @param val the data buffer for the computed value
*/
void _computeQdot(km::Vec &val) const;
km::Mat _poseGradient(const kf::FrameToFrame &f2f) const override;
/** @brief Return the (nU x nU) matrix that converts U generalized
velocities into Qdot gen coord rates.
This matrix is non-identity only when quasi-velocities are
used - such as for spherical subhinges. Currently unused.
@return the U to Qdot conversion matrix
*/
km::Mat33 _genvelToQdotMap() const;
/** @brief Return the (nU x nU) matrix that converts Qdot gen coord
rates into U generalized velocities for the coordinates
provider.
This matrix is non-identity only when
quasi-velocities are used - such as for spherical
subhinges.
This matrix is used in transforming the velocity space
Jacobian into a pose gradient that works with Qdot values
instead of U values. During this process, the Jacobian is
from the oframe (not pframe) for the CoordBase_T, and the
_qdotToGenvelMap should also be the oframe (and not the
pframe) version. We point this out to avoid confusion since
we work with the pframe variants of this map when computing
Qdot values for spherical subhinges (since the angular
velocity is in the pframe frame).
@return the Qdot to U conversion matrix
*/
km::Mat33 _qdotToGenvelMap() const;
/** Specialization of param class for this subhinge type */
struct PhysicalSubhingeParams : PhysicalSubhinge::PhysicalSubhingeParams {
double max_chart_angle; //!< the maximum angle threshold to trigger a chart reset
/** @brief Assignment operator
@param p the other params
@return the updated params
*/
PhysicalSubhingeParams &operator=(const PhysicalSubhinge::PhysicalSubhingeParams *p) {
if (this != p) {
PhysicalSubhinge::PhysicalSubhingeParams::operator=(
*p); // Call base class assignment operator
// Assign Derived-specific members here
max_chart_angle =
static_cast<const PhysicalSubhingeParams *>(p)->max_chart_angle;
}
return *this;
}
};
kc::ks_ptr<PhysicalSubhinge::PhysicalSubhingeParams> _getParams() const override;
void _setParams(const PhysicalSubhinge::PhysicalSubhingeParams ¶ms) override;
void _enableCoordChart(bool flag) override; // {_chart_enabled = flag; };
private:
/** the max angle value that triggers chart recentering. The
exponential map is injective only for angles up to PI, and
we should recenter when getting close to this edge, so use
half PI as the conservative max value to trigger
recentering. */
double _max_chart_angle;
/** the chart offset quaternion from the global origin */
km::UnitQuaternion _chart_offset = km::UnitQuaternion(0, 0, 0, 1, false, true);
/** the global Q coordinate value set by setQ() and used by getQ()
to return a reference as well as the pristine set value. */
mutable km::Vec _global_Q;
bool _chart_enabled = true;
};
/**
* @class SphericalQuatSubhinge
* @brief Represents a 3 dof rotational subhinge using unit quaternion for generalized coords
*
*
* See \sref{subhinges_sec} section for more information on physical
* subhinges.
*/
class SphericalQuatSubhinge : public PhysicalSubhinge_T<4, 3> {
public:
/**
* @brief SphericalQuatSubhinge destructor.
*/
virtual ~SphericalQuatSubhinge();
SubhingeType subhingeType() const override;
/**
* @brief Constructs a SphericalQuatSubhinge.
@param oframe the oframe frame
@param pframe the pframe frame
@param name the name for the subhinge
@param hge the parent hinge
*/
SphericalQuatSubhinge(kc::ks_ptr<kf::Frame> oframe,
kc::ks_ptr<kf::Frame> pframe,
std::string_view name,
kc::ks_ptr<FramePairHinge> hge);
void setU(const Eigen::Ref<const km::Vec> &val) override;
// void setQ(const Eigen::Ref<const km::Vec> &val) override;
const km::Vec &getQdot() const override;
std::string dumpString(std::string_view prefix,
const Karana::Core::Base::DumpOptions *options) const override;
protected:
void _localChartSetQ(const Eigen::Ref<const km::Vec> &val) override;
void _computeTransform(Karana::Math::HomTran &T) override;
void _computeVelocity(Karana::Math::SpatialVector &V) override;
void _computeAccel(Karana::Math::SpatialVector &a) override;
/**
* @brief Data cache callback to compute the Qdot values
*
* @param val the data buffer for the computed value
*/
void _computeQdot(km::Vec &val) const;
km::Mat _poseGradient(const kf::FrameToFrame &f2f) const override;
/** @brief Return the (nU x nU) matrix that converts U generalized
velocities into Qdot gen coord rates.
This matrix is non-identity only when quasi-velocities are
used - such as for spherical subhinges. Currently unused.
@return the U to Qdot conversion matrix
*/
km::Mat _genvelToQdotMap() const;
/** @brief Return the (nU x nU) matrix that converts Qdot gen coord
rates into U generalized velocities for the coordinates
provider.
This matrix is non-identity only when
quasi-velocities are used - such as for spherical
subhinges.
This matrix is used in transforming the velocity space
Jacobian into a pose gradient that works with Qdot values
instead of U values. During this process, the Jacobian is
from the oframe (not pframe) for the CoordBase_T, and the
_qdotToGenvelMap should also be the oframe (and not the
pframe) version. We point this out to avoid confusion since
we work with the pframe variants of this map when computing
Qdot values for spherical subhinges (since the angular
velocity is in the pframe frame).
@return the Qdot to U conversion matrix
*/
km::Mat _qdotToGenvelMap() const;
kc::ks_ptr<PhysicalSubhingeParams> _getParams() const override;
};
/**
* @class Linear3Subhinge
* @brief Represents a 3 dof translational subhinge
*
*
* See \sref{subhinges_sec} section for more information on physical
* subhinges.
*/
class Linear3Subhinge : public PhysicalSubhinge_T<3, 3> {
public:
/**
* @brief Linear3Subhinge destructor.
*/
virtual ~Linear3Subhinge();
SubhingeType subhingeType() const override;
/**
* @brief Constructs a Linear3Subhinge
@param oframe the oframe frame
@param pframe the pframe frame
@param name the name for the subhinge
@param hge the parent hinge
*/
Linear3Subhinge(kc::ks_ptr<kf::Frame> oframe,
kc::ks_ptr<kf::Frame> pframe,
std::string_view name,
kc::ks_ptr<FramePairHinge> hge);
protected:
void _computeTransform(Karana::Math::HomTran &T) override;
void _computeVelocity(Karana::Math::SpatialVector &V) override;
void _computeAccel(Karana::Math::SpatialVector &a) override;
kc::ks_ptr<PhysicalSubhingeParams> _getParams() const override;
};
} // namespace Karana::Dynamics