Program Listing for File FrameToFrame.h#
↰ Return to documentation for file (include/Karana/Frame/FrameToFrame.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 FrameToFrame class.
*/
#pragma once
#include "Karana/Frame/Frame.h"
#include "Karana/KCore/Base.h"
#include "Karana/KCore/DataCache.h"
#include "Karana/Math/HomTran.h"
#include "Karana/Math/SpatialVector.h"
namespace Karana::Frame {
namespace kc = Karana::Core;
namespace km = Karana::Math;
/**
* @class FrameToFrame
* @brief Represents a connection between two frames.
*
* This class handles the transformation, velocity, and acceleration between two frames.
*
* See \sref{frames_layer_sec} for more discussion on
* the frames layer.
*/
class FrameToFrame : public Karana::Core::LockingBase {
// For access to _empty
friend class Frame;
public:
/**
* @brief FrameToFrame destructor.
*/
virtual ~FrameToFrame();
/**
* @brief Get the oframe of the FrameToFrame.
* @return oframe.
*/
const kc::ks_ptr<Frame> &oframe() const;
/**
* @brief Get the pframe of the FrameToFrame.
* @return pframe.
*/
const kc::ks_ptr<Frame> &pframe() const;
/**
* @brief Check the relationship of a sub-chain FrameToFrame's path with respect to the
* overall FrameToFrame path.
*
* Return true if the sub-chain FrameToFrame's path is contained in the
* FrameToFrame's oframe/pframe path and oriented with the path, false if
* it has opposed orientation, and nullopt if it is not fully
* contained in the path.
*
* @param sub_f_to_f the sub-path FrameToFrame's orientation to check
* @return null if not on path, and true if on the path and oriented
*/
std::optional<bool> subchainOrientation(const FrameToFrame &sub_f_to_f) const;
/**
* @brief Get the relative homogeneous transformation between the oframe and pframe.
*
* The actual work is done by the transform cache callback, and
* this is a simple public wrapper for it.
*
* @return The homogeneous transformation.
*/
const km::HomTran &relTransform() const;
/**
* @brief Get the relative velocity between the oframe and pframe.
*
* This is the spatial velocity of the pframe with respect to the oframe, as
* observed from the oframe, and represented in the oframe.
*
* The resulting linear velocity, o_v(o,p) is the derivative of
* o_l(o,p) vector, i.e. the time derivative of the oframe
* coordinate representation of the oframe/pframe translational
* vector. The actual work is done by the velocity cache
* callback, and this is a simple public wrapper for it.
*
* @return The spatial velocity vector.
*/
const km::SpatialVector &relSpVel() const;
/**
* @brief Return the transform rates for the oframe to pframe relative velocity
*
* This method converts the spatial velocity of the pframe with respect to
* the oframe, into the minimal coordinate rates 6-vector for
* the relative transform. The minimal coordinates are the
* Karana::Math::RotationVector representation of the attitude part, and the
* relative position of the linear part.
*
* @return The coordinate rates as a 6-vector
*/
km::Vec6 oframeDerivRelRates() const;
/**
* @brief Get the relative acceleration between the oframe and pframe.
*
* This is the spatial acceleration of the pframe with respect to the oframe, as
* observed from the oframe, and represented in the oframe.
*
* The resulting linear accel, o_a(o,p) is the derivative of
* o_v(o,p) velocity, i.e. the time derivative of the oframe
* coordinate representation of the o_v(o,p) oframe/pframe
* translational velocity. The actual work is done by the accel
* cache callback, and this is a simple public wrapper for it.
*
* @return The spatial acceleration vector.
*/
const km::SpatialVector &relSpAccel() const;
/**
* @brief Return the pframe observed relative spatial velocity between the oframe and
* pframe.
*
* This is the spatial velocity of the pframe with respect to the oframe, as
* observed from the pframe, and represented in the pframe.
*
* The resulting linear velocity, p_v(o,p) is the derivative of
* p_l(o,p) = p_R_o * o_l(o,p) vector, i.e. the time derivative of
* the pframe coordinate representation of the oframe/pframe
* translational vector. Thus
*
* p_v(o,p) = p_R_o * [ o_v(o,p) + o_l(o,p) x w(o,p) ]
*
* Note that the returned value is *NOT* p_v(p,o) that
* corresponds to f_to_f(pframe, oframe).relSpVel()
* where the roles of oframe and pframe are switched, and the
* value would be the velocity of oframe with respect to pframe.
*
* @return The pframe observed spatial velocity vector.
*/
km::SpatialVector pframeObservedRelSpVel() const;
/**
* @brief Return the pframe observed relative spatial acceleration between the oframe and
* pframe.
*
* This is the spatial acceleration of the pframe with respect to the oframe, as
* observed from the pframe, and represented in pframe.
*
* The resulting linear accel, p_a(o,p) is the derivative of
* p_R_o * o_v(o,p) vector, i.e. the time derivative of the pframe
* coordinate representation of the o_v(o,p) oframe/pframe
* translational velocity.
*
* p_a(o,p) = p_R_o * [ o_a(o,p) + o_v(o,p) x w(o,p) ]
*
* Note that the returned value is *NOT* p_a(p,o), which
* corresponds to f_to_f(pframe, oframe).relSpAccel(). To
* get p_a(p,o) you need to switch the roles of oframe and
* pframe and simply call pframe.relSpAccel(oframe).
*
* @return The pframe observed spatial acceleration vector.
*/
km::SpatialVector pframeObservedRelSpAccel() const;
/**
* @brief Transform the hypothetical oframe deriv relative
* spatial velocity into pframe relative value
*
* For this f_to_f, convert the hypothetical oframe derivative
* relative spatial velocity for the f_to_f into the
* corresponding pframe relative spatial velocity, i.e the
* derivative of the reversed pframe to the oframe vector
* quantities as observed from the pframe, and representing in
* the pframe. Thus this is computing B_alpha(B,A) from A_alpha(A,
* B). This method assumes that the relative transform for the
* f_to_f is valid and uses it.
*
* Note that when the hypothetical input oframe relative spatial
* velocity is the true value we will have the following
* equivalence:
*
* toPframeRelativeSpVel(relSpVel()) == pframe()->frameToFrame(oframe())->relSpVel()
*
* Note that this method changes the velocity vector from being
* oframe/pframe to being pframe/oframe, and thus is doing more
* than just changing the observing frame./
*
* This method is handy for state initialization, where we are
* trying to initialize the multibody velocity coordinates based
* on some physical requirements on body/node spatial
* velocities. This method can be use to convert these
* requirements into requirements on the relative spatial
* velocities for the hinge, and at which point the hinge fitU()
* method can be used to compute the U velocity coordinates for
* the hinge that meet the requirements.
*
* @param oframe_v the hypothetical oframe relative spatial velocity
* @return the corresponding pframe relative spatial velocity
*/
const km::SpatialVector toPframeRelativeSpVel(const km::SpatialVector &oframe_v) const;
/**
* @brief Transform the provided oframe relative spatial accel into pframe relative value
*
* For this f_to_f, convert the hypothetical oframe relative
* spatial accel for the f_to_f into the corresponding pframe
* relative spatial accel as if we are taking the derivative of
* the pframe to the oframe vector quantities as observed from
* the pframe, and represented in the pframe. This method
* assumes that the relative transform and spatial velocity for
* the f_to_f are valid and uses them.
*
* Note that when the hypothetical input spatial accel is the
* true value we will have the following equivalence:
*
* toPframeRelativeSpAccel(relSpAccel()) ==
* pframe()->frameToFrame(oframe())->relSpAccel()
*
*
* Note that this method changes the acceleration vector from
* being oframe/pframe to being pframe/oframe, and thus is doing
* more than just changing the observing frame./
*
* This method is handy for multibody state initialization,
* where we are trying to initialize the multibody acceleration
* coordinates based on some physical requirements on body/node
* spatial acceleration. This method can be use to convert these
* requirements into requirements on the relative spatial
* acceleration on hinges, and at which point the hinge fitUdot()
* method can be used to compute the Udot acceleration
* coordinates for the hinge that meet the requirements.
*
* @param oframe_a the hypothetical oframe relative spatial accel
* @return the corresponding pframe relative spatial accel
*/
const km::SpatialVector toPframeRelativeSpAccel(const km::SpatialVector &oframe_a) const;
/**
* @brief Transform the 3-vector pframe observed derivative to oframe observed derivative
*
* This method converts the pframe observed time derivative of a
* 3-vector into the oframe observed time derivative.
*
* Usage: This method is used to change the observing frame for
* a 3-vector derivative to a new one. To do this, create a
* f_to_f FrameToFrame from the new observing frame to the
* original observing frame for the vector derivative, and pass
* in the vector, x, and its time derivative, xdot, as arguments
* to f_to_f.toOframeObserved(x, xdot). The returned value will
* be the time derivative vector in the new observing frame.
*
* @param pframe_x the pframe representation of the 3-vector whose time derivative is being
* taken
* @param pframe_x_dot the (original) pframe observed time derivative of pframe_x
* @return the (new) oframe observed time derivative of pframe_x
*/
const km::Vec3 toOframeObserved(const km::Vec3 &pframe_x,
const km::Vec3 &pframe_x_dot) const;
/**
* @brief Get the time derivative of phi(oframe, pframe)
*
* See \sref{phiDef} "rigid body transformation matrix" for a
* definition of the transformation matrix.
*
* \f[ \dot\phi(o, p) = -\phi(o, p) [\tilde V_o(o, p)]^* \f]
*
* @see Karana::Math::HomTran::phi(), Karana::Math::HomTran::phiStar()
*
* @returns phi time derivative matrix.
*/
km::Mat66 phiDotMatrix() const;
/**
* @brief Return the transform data cache.
* @return the transform DataCache
*/
const kc::ks_ptr<kc::DataCache<km::HomTran>> &transformCache() const {
return _transform_cache;
}
/**
* @brief Return the velocity data cache.
* @return the velocity DataCache
*/
const kc::ks_ptr<kc::DataCache<km::SpatialVector>> &velocityCache() const {
return _velocity_cache;
}
/**
* @brief Return the acceleration data cache.
* @return the acceleration DataCache
*/
const kc::ks_ptr<kc::DataCache<km::SpatialVector>> &accelCache() const {
return _accel_cache;
}
/* The Coriolis accel methods below are for Exercise 1.14
(Spatial acceleration transformations) which focuses on
combining oframe/pframe accel alpha(o,p) with pframe/target
accel alpha(p, t) to get oframe/target accel alpha(o,t). We
have variants for what frames are each observed in
since the Coriolis accel terms will all be different. In each
of these cases, we are working with V(o,p) and V(p,t), and
the accel definition depends on which frame's representation
of V are we differentiating to get alpha, i.e. what is the
observation frame when obtaining the acceleration. So as in
Eq 1.65 in Section 1.5, we use the notation
alpha_H(A,B) = d_H V(F, G)
-----------
dt
to specify the acceleration of the G frame with respect to the F frame as
observed in the H frame. It is important to note that the H
frame in above accleration is *NOT* involved in V(F,G), and
that this is the spatial velocity of G with respect to F as observed from
the F frame.
In general, different choices of the A,B,C observation frame
can be used in the following general expression relating the
o/p/t relative accelerations
alpha_A(o,t) = \phi^*(p,t) alpha_B(o,p) + alpha_C(p,t) + a_ABC
The "a_ABC" denotes the Coriolis acceleration term for the
specific choice of the A/B/C observation frames.
In our multibody dynamics implementation, we work with body
accelerations that are the acceleration of the body with respect to the
inertial but observed in the body frame and represented in the
body frame. Thus, when propagating the body accelerations from
body to body in a scatter recursion, we are combining the
inertial acceleration of the parent body (observed in the
parent body frame), with the hinge induced acceleration
(observed in the parent body frame) to get the the child body's
inertial acceleration as observed from the child body's
frame. In this case
A = child body, B = parent body, C = parent body
and the appropriate Coriolis accel term would be from the
coriolisAccel_tpp().
On the other hand, when combining the oframe/pframe
oframeDerivSpAccel with the pframe/target relSpAccel, we have
A = oframe, B = oframe, C = pframe
and the appropriate Coriolis accel term would be from
coriolisAccel_oop().
*/
/**
* @brief Compute the 'oop' version of the Coriolis acceleration as defined above.
*
* The result is expressed in oframe. The expression is from
* Exercise 1.14 (Spatial acceleration transformations), Eq 1.69. The
* returned value is in the 'o' from frame.
*
* a_oop = | w(o,p) x w(p,t) |
* | |
* | w(o,p) x [v(o, t) - v(o,p) + v(p,t] |
*
* While not strictly necessary, we are passing the additional
* f_to_f arguments to avoid the costs of frame to frame lookups.
*
* @param target the target Frame
* @param p_to_t_f_to_f the pframe to target frame2frame
* @return the 'oop' Coriolis spatial acceleration vector
*/
km::SpatialVector coriolisAccelOop(const Frame &target,
const FrameToFrame &p_to_t_f_to_f) const;
/**
* @brief Compute the 'tpp' version of the Coriolis acceleration as defined above.
*
* The result is expressed in the target frame. The expression is
* from Exercise 1.14 (Spatial acceleration transformations), Eq
* 1.73. The returned value is in the 't' target frame.
*
* a_tpp = | w(o,p) x w(p,t) |
* | |
* | w(o,p) x v(p, t) + v(o,t) x w(p,t] |
*
* While not strictly necessary, we are passing the additional
* f_to_f arguments to avoid the costs of frame to frame lookups.
*
* @param target the target Frame
* @param p_to_t_f_to_f the pframe to target frame2frame
* @param o_to_t_f_to_f the oframe to target frame2frame
* @return the 'tpp' Coriolis spatial acceleration vector
*/
km::SpatialVector coriolisAccelTpp(const Frame &target,
const FrameToFrame &p_to_t_f_to_f,
const FrameToFrame &o_to_t_f_to_f) const;
/**
* @brief Solve for sub f_to_f's transform needed to achieve desired relative transform
*
* Denoting this as the A/C f_to_f, and the desired relative
* transform as desired_T, this method solves for the relative
* transform required of the sub_f_to_f (which is assumed to be in
* the A/C path). It is required that sub_f_to_f be at one end or the
* other of the A/C f_to_f, i.e. it's oframe is A, or that its pframe
* is C.
*
* @param sub_f_to_f the sub f_to_f whose required transform is to be computed
* @param T the desired relative transform for this f_to_f
* @return the relative transform required for the sub f_to_f
*/
km::HomTran solveTransform(const FrameToFrame &sub_f_to_f, const km::HomTran &T) const;
/**
* @brief Solve for sub f_to_f's relative spatial velocity needed to achieve desired
* relative spatial velocity
*
* Denoting this as the A/C f_to_f, and the desired relative
* spatial velocity as V, this method solves for the
* relative spatial velocity required of the sub_f_to_f (which is
* assumed to be in the A/C path). It is required that sub_f_to_f be
* at one end or the other of the A/C f_to_f, i.e. it's oframe is
* A, or that its pframe is C. The sub_f_to_f thus splits the A/C
* f_to_f path in two parts. The other_f_to_f is the f_to_f for the
* remaining half.
*
* We pass in extra f_to_f's to avoid the cost of lookups within
* this method.
*
* @param sub_f_to_f the sub f_to_f whose required spatial velocity is to be computed
* @param other_f_to_f the f_to_f for the segment of oframe/pframe not covered by sub_f_to_f
* @param V the desired relative spatial velocity for this f_to_f
* @return the relative spatial velocity required for the sub f_to_f
*/
km::SpatialVector solveSpVel(const FrameToFrame &sub_f_to_f,
const FrameToFrame &other_f_to_f,
const km::SpatialVector &V) const;
/**
* @brief Solve for sub f_to_f's relative spatial acceleration needed to achieve desired
* relative spatial acceleration
*
* Denoting this as the A/C f_to_f, and the desired relative
* spatial acceleration as A, this method solves for the
* relative spatial acceleration required of the sub_f_to_f (which
* is assumed to be in the A/C path). It is required that sub_f_to_f
* be at one end or the other of the A/C f_to_f, i.e. it's oframe
* is A, or that its pframe is C. The sub_f_to_f thus splits the
* A/C f_to_f path in two parts. The other_f_to_f is the f_to_f for the
* remaining half.
*
* We pass in extra f_to_f's to avoid the cost of lookups within
* this method.
*
* @param sub_f_to_f the sub f_to_f whose required spatial acceleration is to be computed
* @param other_f_to_f the f_to_f for the segment of oframe/pframe not covered by sub_f_to_f
* @param A the desired relative spatial acceleration for this f_to_f
* @return the relative spatial acceleration required for the sub f_to_f
*/
km::SpatialVector solveSpAccel(const FrameToFrame &sub_f_to_f,
const FrameToFrame &other_f_to_f,
const km::SpatialVector &A) const;
public:
/** Options struct for the dumpString() method */
struct DumpOptions : LockingBase::DumpOptions {
DumpOptions() = default;
unsigned int depth =
1; ///< The depth of nested objects to include (0 is all, 1 is one deep etc.)
bool data = true; ///< Report data values and cache status
/**
* @brief Copy assignment operator.
*
* @param p DumpOptions to copy from.
* @return A reference to the assigned DumpOptions.
*/
DumpOptions &operator=(const DumpOptions &p) {
if (this != &p) {
LockingBase::DumpOptions::operator=(p); // Call base class assignment operator
// Assign Derived-specific members here
depth = p.depth;
data = p.data;
}
return *this;
}
};
/**
* @brief Return a formatted string containing information about this object.
*
* @param prefix String prefix to use for formatting.
* @param options Dump options (if null, defaults will be used).
* @return A string representation of the object.
*/
std::string dumpString(std::string_view prefix = "",
const Base::DumpOptions *options = nullptr) const override;
/**
* @brief Freeze all data caches.
*/
void freezeDataCaches();
/**
* @brief Unfreeze all data caches.
*/
void unfreezeDataCaches();
protected:
/**
* @brief Constructor for FrameToFrame.
*
* @param oframe oframe of the FrameToFrame.
* @param pframe pframe of the FrameToFrame.
* @param name The name to use for the frame to frame. If none is supplied (the default),
* then an automatically derived name will be used.
*/
FrameToFrame(const kc::ks_ptr<Frame> &oframe,
const kc::ks_ptr<Frame> &pframe,
std::string_view name = "");
/**
* @brief Empty out the FrameToFrame completely. This happens when the oframe or pframe
* associated with the FrameToFrame is deleted.
*/
virtual void _empty();
/**
* @brief Make this FrameToFrame healthy.
*/
void _makeHealthy() override;
/**
* @brief Create a name for a FrameToFrame with the given oframe and pframe.
*
* @param oframe The oframe of the FrameToFrame.
* @param pframe The pframe of the FrameToFrame
* @param suffix The suffix to use in the name.
* @returns The name for the FrameToFrame.
*/
static std::string
_mkName(const Frame &oframe, const Frame &pframe, std::string_view suffix);
/**
* @brief Compute the transformation between the oframe and pframe.
*
* This the relTransform data cache callback.
*
* @param T The buffer to place the computed homogeneous transformation in.
*/
virtual void _computeTransform(Karana::Math::HomTran &T) = 0;
/**
* @brief Compute the spatial velocity between the oframe and pframe.
*
* This the relSpVel data cache callback.
*
* @param V The buffer to put the computed spatial velocity in.
*/
virtual void _computeVelocity(Karana::Math::SpatialVector &V) = 0;
/**
* @brief Compute the spatial acceleration between the oframe and pframe.
*
* This the relSpAccel data cache callback.
*
* @param a The buffer to put the computed spatial acceleration in.
*/
virtual void _computeAccel(Karana::Math::SpatialVector &a) = 0;
/**
* @brief Propagate spatial velocity from this oframe/pframe pair to a target frame.
*
* This method combines the relSpVel values of 2
* connected segments A/B and B/C to derive the overall A/C
* relSpVel value. The resulting derivatives are in the
* oframe, and the result is expressed in the oframe.
*
* The expression is covered in Eq. 1.48 in Exercise 1.8
* (Evaluating spatial velocities.)
*
* V(A, C) = phistar(B, C) * V(A, B) + V(B, C)
*
* While not strictly necessary, we are passing the additional
* f_to_f arguments to avoid the costs of frame to frame lookups.
*
* @param target The target frame.
* @param p_to_t_f_to_f the pframe to target frameToFrame.
* @param o_to_t_f_to_f the oframe to target frameToFrame.
* @return The combination of the oframe/pframe velocity with the pframe/target velocity.
*/
km::SpatialVector _propagateVelocity_oop(const Frame &target,
const FrameToFrame &p_to_t_f_to_f,
const FrameToFrame &o_to_t_f_to_f) const;
/**
* @brief Propagate spatial acceleration from this oframe/pframe pair to a target frame
*
* This method combines the relSpAccel values of 2
* connected segments A/B and B/C to derive the overall A/C
* relSpAccel value. The resulting derivatives are in
* the oframe, and the result is expressed in the oframe.
*
* The expression is covered in Eq 1.74 in Exercise 1.15
* (Evaluating spatial accelerations.). This includes the
* expression for the Coriolis term. The coriolisAccel_oop()
* method is used to compute the Coriolis acceleration for this
* case.
*
* \f[ \alpha(A, C) = phistar(B, C) * \alpha(A, B) + \alpha(B, C) + \text{Coriolis
* accel} \f]
*
* While not strictly necessary, we are passing the additional
* f_to_f arguments to avoid the costs of frame to frame lookups.
*
* @param target The target frame.
* @param p_to_t_f_to_f the pframe to target FrameToFrame.
* @param o_to_t_f_to_f the oframe to target FrameToFrame.
* @return The overall oframe/target spatial acceleration.
*/
km::SpatialVector _propagateAccel_oop(const Frame &target,
const FrameToFrame &p_to_t_f_to_f,
const FrameToFrame &o_to_t_f_to_f) const;
protected:
/// Cache for the homogeneous transform
// kc::ks_ptr<kc::DataCache<int>> _junk_cache;
kc::ks_ptr<kc::DataCache<km::HomTran>> _transform_cache;
/// Cache for the spatial velocity
kc::ks_ptr<kc::DataCache<km::SpatialVector>> _velocity_cache;
/// Cache for the spatial acceleration
kc::ks_ptr<kc::DataCache<km::SpatialVector>> _accel_cache;
protected:
/// The oframe of the FrameToFrame.
kc::ks_ptr<Frame> _oframe;
/// The pframe of the FrameToFrame.
kc::ks_ptr<Frame> _pframe;
};
} // namespace Karana::Frame