Program Listing for File CoordBase.h#
↰ Return to documentation for file (include/Karana/SOADyn/CoordBase.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 CoordBase class.
*/
#pragma once
#include "Karana/Frame/Frame.h"
#include "Karana/Frame/FrameToFrame.h"
#include "Karana/KCore/Base.h"
#include "Karana/KCore/DataCache.h"
#include "Karana/Math/Defs.h"
namespace Karana::Dynamics {
class HingeOnode;
class Multibody;
namespace kc = Karana::Core;
namespace km = Karana::Math;
namespace kf = Karana::Frame;
/**
* @class CoordBase
* @brief Represents the base class for Q, U etc coordinate data providers.
*
* This class is the base class for subhinges, compound subhinges,
* and bodies that can contribute motion degrees of freedom to the
* system. These degrees of freedom are associated with coordinates
* designated Q, U etc.
*
* See \sref{coords_sec} section for discussion on Q, U etc coordinates.
*
*/
class CoordBase {
// for access to _newtonian_frame
friend class FrameToFrameJacobianGenerator;
friend class FramePairHinge;
// for access to _pframe2otherPhi and related methods
friend class SubTree;
// for access to _jacobian, _jacobianDot
friend class CompoundSubhinge;
// for access to _localChartGetQ, _localChartSetQ
friend class CoordData;
friend class CECompoundSubhinge;
friend class CECompoundBody;
public:
/**
* @brief Constructs a CoordBase.
* @param nm instance name
* @param id instance id
*/
CoordBase(std::string_view nm, kc::id_t id);
/**
* @brief Destructor.
*/
virtual ~CoordBase(); // = default;
/**
* @brief Return string with information about the object.
*
* @param prefix the prefix to apply to each output line
* @param options options to control dump output
* @return String with informational content.
*/
std::string dumpString(std::string_view prefix, const kc::Base::DumpOptions *options) const;
/**
* @brief Return the object's name.
*
* @return The object's string name.
*/
virtual std::string_view name() const { return _name; };
/**
* @brief Return the object's id.
*
* @return The object's id.
*/
virtual const kc::id_t &id() const { return _id; }
/**
* @brief Returns the type string of the BodyBase.
*
* @param brief if true, return the short form of the class type name, else the full type
* name
* @return The type string.
*/
virtual std::string_view typeString(bool brief = true) const noexcept = 0;
/**
* @brief Check whether parameters have been set.
*
* @return True if the parameters have been set.
*/
virtual bool isReady() const; // override;
/**
* @brief Get the ATBI psi matrix value.
*
* @return The ATBI psi matrix value for this subhinge.
*/
virtual km::Mat getATBIMatPsi() const = 0;
/**
* @brief Get the ATBI D matrix value.
*
* @return The ATBI D matrix value for this subhinge.
*/
virtual km::Mat getATBID() const = 0;
/**
* @brief Get the ATBI D inverse matrix value.
*
* @return The ATBI D inverse matrix value for this subhinge.
*/
virtual km::Mat getATBIDinv() const = 0;
/** The ATBI G matrix at the pframe. For a subhinge, pframe is the
regular subhinge pframe, and for a flex body it is the body
frame. This is different from _atbi_mats.G.
*
* @return The ATBI G matrix value for this subhinge.
*/
virtual km::Mat getATBIG() const = 0;
/** The ATBI tauper matrix at the pframe. For a subhinge, pframe is the
regular subhinge pframe, and for a flex body it is the body
frame. This is different from _atbi_mats.tauper.
*
* @return The ATBI tauper matrix value for this subhinge.
*/
virtual km::Mat getATBITauper() const = 0;
/** The coord map matrix with the lhs in the pframe. For a
subhinge, pframe is the regular subhinge pframe, and for a
flex body it is the body frame. For a subhinge, this is a
(6,nU) size matrix, while for a flex body its size is
(nU+6, nU).
*
* @return The pframe referenced subhinge coordinate map matrix value for this subhinge.
*/
virtual km::Mat pframeCoordMapMatrix() const {
// default defintion
return atbiCoordMapMatrix();
}
/**
* @brief Return the coordinate map matrix to be used for ATBI computations
* @return coordinate map matrix
*/
virtual const km::Mat atbiCoordMapMatrix() const { return km::Mat(); }
/** The Upsilon at the pframe (after crossing the ATBI dofs). For
* a subhinge, pframe is the regular subhinge pframe, and for a
* flex body it is the body frame. For a subhinge, this is a nU
* size square matrix, while for a flex body it is a (nU+6) size
* square matrix.
*
* @return The ATBI Upsilon matrix value for this subhinge.
*/
virtual km::Mat getUpsilonMatrix() = 0;
public:
/** Get/set methods for buffers for the generalized coordinates
* for this coordinate provider.
*/
/**
* @brief Set the global (chart) Q coordinates.
*
* See \sref{coords_sec} section for discussion on Q, U etc coordinates.
*
* @param val Array of values.
*/
virtual void setQ(const Eigen::Ref<const km::Vec> &val);
/**
* @brief Set the global (chart) Q coordinates to a constant value
*
* See \sref{coords_sec} section for discussion on Q, U etc coordinates.
*
* @param fill_value Fill value.
*/
virtual void setQ(double fill_value);
/**
* @brief Return the global (chart) Q coordinates
*
* See \sref{coords_sec} section for discussion on Q, U etc coordinates.
*
* @return Array of values.
*/
virtual const km::Vec &getQ() const;
/**
* @brief Set the U velocity coordinates.
*
* See \sref{coords_sec} section for discussion on Q, U etc coordinates.
*
* @param val Array of values.
*/
virtual void setU(const Eigen::Ref<const km::Vec> &val);
/**
* @brief Set the U velocity coordinates to a constant value
*
* See \sref{coords_sec} section for discussion on Q, U etc coordinates.
*
* @param fill_value Fill value.
*/
virtual void setU(double fill_value);
/**
* @brief Return the U velocity coordinates
*
* See \sref{coords_sec} section for discussion on Q, U etc coordinates.
*
* @return Array of values.
*/
virtual const km::Vec &getU() const;
/**
* @brief Set the T generalized forces.
*
* See \sref{coords_sec} section for discussion on Q, U etc coordinates.
*
* @param val Array of values.
*/
virtual void setT(const Eigen::Ref<const km::Vec> &val);
/**
* @brief Set the T generalized forces to a constant value
*
* See \sref{coords_sec} section for discussion on Q, U etc coordinates.
*
* @param fill_value Fill value.
*/
virtual void setT(double fill_value);
/**
* @brief Return the T generalized forces
*
* See \sref{coords_sec} section for discussion on Q, U etc coordinates.
*
* @return Array of values.
*/
virtual const km::Vec &getT() const;
/**
* @brief Set the Udot acceleration coordinates.
*
* See \sref{coords_sec} section for discussion on Q, U etc coordinates.
*
* @param val Array of values.
*/
virtual void setUdot(const Eigen::Ref<const km::Vec> &val);
/**
* @brief Set the Udot acceleration coordinates to a constant value
*
* See \sref{coords_sec} section for discussion on Q, U etc coordinates.
*
* @param fill_value Fill value.
*/
virtual void setUdot(double fill_value);
/**
* @brief Return the Udot acceleration coordinates
*
* See \sref{coords_sec} section for discussion on Q, U etc coordinates.
*
* @return Array of values.
*/
virtual const km::Vec &getUdot() const;
/**
* @brief Return the Qdot rate coordinates
*
* See \sref{coords_sec} section for discussion on Q, U etc coordinates.
*
* @return Array of values.
*/
virtual const km::Vec &getQdot() const { return getU(); }
/**
* @brief The number of generalized coords for the CoordBase.
*
* See \sref{coords_sec} section for discussion on Q, U etc coordinates.
*
* @return the number of generalized coordinates
*/
size_t nQ() const { return _nQ; };
/**
* @brief The number of velocity coords for the CoordBase_T.
*
* See \sref{coords_sec} section for discussion on Q, U etc coordinates.
*
* @return the number of velocity coordinates
*/
size_t nU() const { return _nU; };
protected:
/**
* @brief Return the frame to frame for the CoordBase.
*
* The returned f2f helps locate the CoordBase in the frames
* tree. Its transform etc data *does not* however represent the
* relative transformation changes from the changes to this
* object's coordinates.
*
* @return Return the oframe Frame instance
*/
virtual kc::ks_ptr<kf::FrameToFrame>
f2f() const = 0; // NOLINT(readability-identifier-naming) TODO
/**
* @brief Checks whether a CoordBase edge is oriented along the Karana::Frame::FrameToFrame
* path.
*
* @param f2f The f2f.
* @return std::nullopt if the coord edge is not on the f2f path
* std::optional(true) if the coord edge is oriented along the f2f path.
* std::optional(false) if the coord edge is oriented opposed to the f2f path.
*/
virtual std::optional<bool>
isOriented(const kf::FrameToFrame &f2f) const; // NOLINT(readability-identifier-naming) TODO
/**
* @brief Return the Jacobian matrix for the target Karana::Frame::Frame
*
* For this coordinates provider, return the 6xnU Jacobian matrix for
* the target frame that maps generalized velocities U to the target
* Karana::Frame::Frame's spatial velocity (and represented in the target
* Karana::Frame::Frame). The Jacobian is useful for velocity space kinematics, computing
* force reflection and for mass matrix and operational space inertia related computations.
*
* See \sref{frames_layer_sec} section for more information on
* frames, and \sref{jacobian_sec} section for more on Jacobians.
*
* Note that this, and the methods following it *do not* check
* whether the dependency of the target frame's pose on the
* coordinates, and assume instead that the target frame is
* rigid attached to the oframe.
*
* @param f2f the f2f whose pframe is the target frame for the Jacobian
* @return the Jacobian matrix
*/
virtual km::Mat _jacobian(const kf::FrameToFrame &f2f) const = 0;
/**
* @brief Return the time derivative of the Jacobian matrix for the target
* Karana::Frame::Frame
*
* For this coordinates provider, return the 6xnU Jacobian
* matrix time derivative for the target Karana::Frame::Frame.
*
* See \sref{frames_layer_sec} section for more information on
* frames, and \sref{jacobian_sec} section for more on Jacobians.
*
* @param f2f the f2f whose pframe is the target frame for the Jacobian
* @return the Jacobian matrix time derivative
* @see jacobian()
*/
virtual km::Mat _jacobianDot(const kf::FrameToFrame &f2f) const = 0;
/**
* @brief Return the relative Jacobian matrix for a Karana::Frame::FrameToFrame using
* numerical differencing
*
* This method uses numerical differencing to compute the 6xnU
* Jacobian for a Karana::Frame::FrameToFrame's pframe with respect to its
* oframe. The primary purpose of this method is to
* cross-validate the analytical jacobian computation, where the
* f2f pframe is the target frame.
*
* See \sref{frames_layer_sec} section for more information on
* frames, and \sref{jacobian_sec} section for more on Jacobians.
*
* @param f2f the Karana::Frame::FrameToFrame defining the relative Jacobian from/to frames
* @return the Jacobian matrix
*/
km::Mat _jacobianNumDiff(const kf::FrameToFrame &f2f);
/**
* @brief Return the relative JacobianDot matrix for a Karana::Frame::FrameToFrame using
* numerical differencing
*
* This method uses numerical differencing to compute the 6xnU
* JacobianDot for a Karana::Frame::FrameToFrame's pframe with respect to its
* oframe. The primary purpose of this method is to
* cross-validate the analytical jacobianDot computation, where the
* f2f pframe is the target frame.
*
* The multibody argument is required when analytical is
* false, since a total time derivative of the Jacobian needs to
* be computed, and this requires that all the multibody
* coordinates be perturbed (and not just the ones for the CoordBase).
*
* See \sref{frames_layer_sec} section for more information on
* frames, and \sref{jacobian_sec} section for more on Jacobians.
*
* @param f2f the Karana::Frame::FrameToFrame defining the relative Jacobian from/to frames
* @param mb the multibody system
* @return the JacobianDot matrix
*/
km::Mat _jacobianDotNumDiff(const kf::FrameToFrame &f2f, kc::ks_ptr<Multibody> mb);
/**
* @brief Return the pose gradient matrix for a Karana::Frame::FrameToFrame
*
* Return the 6xnU gradient matrix for the pose of a
* Karana::Frame::FrameToFrame's pframe with respect to its oframe with
* respect to the generalized coordinates Q using analytical
* methods. The relative orientation is expressed using
* RotationVectors for a minimal coordinates
* representation. Though closely related to the Jacobian, the
* gradient matrix is a coordinate space mapping, while the
* Jacobian is a velocity space mapping. The gradient matrix is
* handy for inverse kinematics computations.
*
* See \sref{frames_layer_sec} section for more information on
* frames, and \sref{jacobian_sec} section for more on Jacobians.
*
* @param f_to_f the FrameToFrame defining the relative from/to frames
* @return the gradient matrix
*/
virtual km::Mat _poseGradient(const kf::FrameToFrame &f_to_f) const;
/**
* @brief Return the pose gradient matrix for a Karana::Frame::FrameToFrame using numerical
* differencing
*
* This method uses numerical differencing to compute the 6xnU
* pose gradient of a Karana::Frame::FrameToFrame's pframe with respect to
* its oframe using numerical differencing. The primary purpose
* of this method is to cross-validate the analytical pose
* gradient computation.
*
* See \sref{frames_layer_sec} section for more information on
* frames, and \sref{jacobian_sec} section for more on Jacobians.
*
* @param f2f the FrameToFrame defining the relative Jacobian from/to frames
* @return the pose gradient matrix
*/
km::Mat _poseGradientNumDiff(const kf::FrameToFrame &f2f);
protected:
/** @brief Overall struct for ATBI dynamics matrix quantities for
a coordinates provider (subhinge or deformable bodies). */
struct ATBIMatrices {
/**
* @brief Constructor.
*/
ATBIMatrices() = default;
/**
* @brief Copy constructor.
*/
ATBIMatrices(const ATBIMatrices &) = default;
virtual ~ATBIMatrices(){};
};
/**
* @brief Return the ATBI matrices
*
* @return the ATBIMatrices struct
*/
const ATBIMatrices &_atbiMatrices() const { return *_atbi_matrices; }
/** @brief Overall struct for ATBI dynamics vector quantities for
a coordinates provider (subhinge or deformable bodies). */
struct ATBIFilterVectors {
/**
* @brief Constructor.
*/
ATBIFilterVectors() = default;
/**
* @brief Copy constructor.
*/
ATBIFilterVectors(const ATBIFilterVectors &) = default;
virtual ~ATBIFilterVectors(){};
};
/**
* @brief Return the ATBI filter vectors
*
* @return the ATBIFilterVectors struct
*/
const ATBIFilterVectors &_atbiFilterVectors() const { return *_atbi_filter_vectors; }
protected:
/** @brief Return the oframe to pframe Psi() matrix for this CoordBase
For
- a flex body this is a 6x(nU+6) psi matrix, the lhs is at the pnode, and the rhs at
the body frame.
- For a subhinge this is a 6x6 psi matrix, the lhs/rhs are
the subhinge's oframe and pframe pair.
- a compound subhinge this is the 6x(nbodies*(6+nU)) psi
matrix, the lhs is at the physical parent,
and the rhs at the embedded bodies.
@return the Psi() matrix
*/
virtual km::Mat _oframe2pframePsi() const = 0;
/** @brief Return the oframe to pframe Phi() matrix for this CoordBase
For
- a flex body this is a 6x(nU+6) phi matrix, the lhs is at
the pnode, and the rhs at the body frame.
- a subhinge this is a 6x6 phi matrix, the lhs/rhs are the
subhinge's oframe and pframe pair.
- a compound subhinge this is the 6x(nbodies*(6+nU)) E_Phi_G
matrix, the lhs is at the physical parent,
and the rhs at the embedded bodies.
@return the Phi() matrix
*/
virtual km::Mat _oframe2pframePhi() const = 0;
/** @brief Return the pframe to other frame Phi() matrix for this CoordBase
For
- a flex body this is a (nU+6)x6 phi matrix - the lhs is at
the body frame, and the rhs at the other frame.
- a subhinge this is a 6x6 matrix - the lhs is the
pframe, and the rhs at the other frame.
- a compound subhinge this is the (nbodies*(6+nU))x6 matrix,
the lhs is at the embedded body frames, and the
rhs at the other frame.
@param other the other to frame
@return the Phi() matrix
*/
virtual km::Mat _pframe2otherPhi(const kf::Frame &other) const = 0;
protected:
/**
* @brief Set the local chart Q coordinates
*
* See \sref{coords_sec} section for discussion on Q, U etc coordinates.
*
* @param val Array of values.
*/
virtual void _localChartSetQ(const Eigen::Ref<const km::Vec> &val);
/**
* @brief Return the local chart Q coordinates - without sanitizing coordinates
*
* See \sref{coords_sec} section for discussion on Q, U etc coordinates.
*
* @return Array of values.
*/
virtual const km::Vec &_localChartGetQ() const;
/**
* @brief Resize the internal Q, U etc coordinate vector buffers for this CoordBase
*
*
* @param n_q size of the Q configuration coordinates
* @param n_u size of the U velocity coordinates
*/
void _resizeCoordBuffers(size_t n_q, size_t n_u);
protected:
/** The frame to use as the inertial, i.e. newtonian frame. By
default this is the virtual root body, but can be set to a
different frame. */
kc::ks_ptr<kf::Frame> _newtonian_frame = nullptr;
/** the CoordBase name */
std::string _name;
/** the unique id for the CoordBase */
kc::id_t _id;
/** the data cache for the Q coordinates */
kc::ks_ptr<kc::DataCache<km::Vec>> _cache_Q = nullptr;
/** the data cache for the U coordinates */
kc::ks_ptr<kc::DataCache<km::Vec>> _cache_U = nullptr;
/** the data cache for the Udot coordinates */
kc::ks_ptr<kc::DataCache<km::Vec>> _cache_Udot = nullptr;
/** the data cache for the T coordinates */
kc::ks_ptr<kc::DataCache<km::Vec>> _cache_T = nullptr;
/** the data cache for the Qdot coordinates */
kc::ks_ptr<kc::DataCache<km::Vec>> _cache_Qdot = nullptr;
/** member for the ATBI matrices for this CoordBase */
kc::ks_ptr<ATBIMatrices> _atbi_matrices = nullptr;
/** member for the ATBI filter vectors for this CoordBase */
kc::ks_ptr<ATBIFilterVectors> _atbi_filter_vectors = nullptr;
/** The number of Q coordinates for this CoordBase */
size_t _nQ = 0;
/** The number of U coordinates for this CoordBase */
size_t _nU = 0;
protected:
/** Buffers for the generalized coordinates for this physical
* subhinge. We do these here because the coordinate buffers for
* compound subhinges come from the CoordData associated with
* the embedded bodies. Additional such buffers will be
* available within the body class for for deformable bodies.
*
* We use these buffers to allow the user to directly set these
* values using the get/set methods. The actual data is managed
* by the associated caches */
km::Vec _buf_Q; ///< gen coords buffer
km::Vec _buf_U; ///< gen velocities buffer
km::Vec _buf_Udot; ///< gen accels buffer
km::Vec _buf_T; ///< gen forces buffer
};
} // namespace Karana::Dynamics