Program Listing for File FrameUtils.h#
↰ Return to documentation for file (include/Karana/Frame/FrameUtils.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/Math/HomTran.h"
#include "Karana/Math/SpatialVector.h"
namespace Karana::Frame {
namespace km = Karana::Math;
// NOLINTBEGIN(readability-identifier-naming)
/**
* @brief Utility function to propgagate the spatial velocity from a
* 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 o_to_p_T the oframe to pframe rel transform.
* @param o_to_p_V the oframe to pframe rel spatial velocity.
* @param p_to_t_T the pframe to target rel transform.
* @param p_to_t_V the pframe to target rel spatial velocity.
* @param o_to_t_T the oframe to target rel transform.
* @return The oframe observed and represented spatial velocity of the target wrt oframe
*/
km::SpatialVelocity propagateVelocityOopUtil(const km::HomTran &o_to_p_T,
const km::SpatialVelocity &o_to_p_V,
const km::HomTran &p_to_t_T,
const km::SpatialVelocity &p_to_t_V,
const km::HomTran &o_to_t_T);
/**
* @brief Rigidly transform a oframe represented pframe spatial
* velocity to a oframe observed target spatial velocity
*
* This method is a special case of propagateVelocityOopUtil() when
* the pframe and target frame are rigidly attached to each other.
*
* @param o_to_p_T the oframe to pframe rel transform.
* @param p_to_t_T the pframe to target rel transform.
* @param o_to_p_V the oframe to pframe rel spatial velocity.
* @return the oframe observed and represented spatial velocity of the target wrt the oframe
*/
km::SpatialVelocity rigidlyPropagateVelocityOoUtil(const km::HomTran &o_to_p_T,
const km::HomTran &p_to_t_T,
const km::SpatialVelocity &o_to_p_V);
/**
* @brief Rigidly transform a oframe observed but pframe represented
* pframe spatial velocity to a oframe observed but target
* represented target spatial velocity (i.e. the pframe and target
* are assumed to be rigidly attached to each other)
*
* @param p_to_t_T the pframe to target rel transform.
* @param p_o_to_p_V the oframe to pframe rel spatial velocity.
* @return the target frame observed and represented spatial velocity of the pframe wrt the
* oframe
*/
km::SpatialVelocity rigidlyPropagateVelocityTpUtil(const km::HomTran &p_to_t_T,
const km::SpatialVelocity &p_o_to_p_V);
/**
* @brief Utility function to propgagate the spatial acceleration
* from a 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 o_to_p_T the oframe to pframe rel transform.
* @param o_to_p_V the oframe to pframe rel spatial velocity.
* @param o_to_p_A the oframe to pframe rel spatial acceleration.
* @param p_to_t_T the pframe to target rel transform.
* @param p_to_t_V the pframe to target rel spatial velocity.
* @param p_to_t_A the pframe to target rel spatial acceleration.
* @param o_to_t_T the oframe to target rel transform.
* @return the oframe observed and represented spatial acceleration of the target wrt the
* oframe
*/
km::SpatialAcceleration propagateAccelOopUtil(const km::HomTran &o_to_p_T,
const km::SpatialVelocity &o_to_p_V,
const km::SpatialAcceleration &o_to_p_A,
const km::HomTran &p_to_t_T,
const km::SpatialVelocity &p_to_t_V,
const km::SpatialAcceleration &p_to_t_A,
const km::HomTran &o_to_t_T);
/**
* @brief Rigidly transform a oframe observed pframe spatial
* acceleration to a oframe observed target spatial acceleration (i.e. the
* pframe and target are assumed to be rigidly attached to each
* other)
*
* @param o_to_p_T the oframe to pframe rel transform.
* @param p_to_t_T the pframe to target rel transform.
* @param o_o_to_p_V the oframe to pframe rel spatial velocity observed and represented in the
* oframe
* @param o_o_to_p_A the oframe to pframe rel spatial acceleration observed and represented in
* the oframe
* @return the oframe to target spatial velocity observed and represented in the oframe
*/
km::SpatialAcceleration
rigidlyPropagateAccelerationOoUtil(const km::HomTran &o_to_p_T,
const km::HomTran &p_to_t_T,
const km::SpatialVelocity &o_o_to_p_V,
const km::SpatialAcceleration &o_o_to_p_A);
/**
* @brief Rigidly transform a oframe observed spatial acceleration
* into a target observed spatial acceleration (i.e. the pframe and
* target are assumed to be rigidly attached to each other)
*
* @param p_to_t_T the pframe to target rel transform.
* @param p_o_to_p_A the oframe to pframe rel spatial acceleration (observed and represented in
* the pframe)
* @return the oframe to target spatial acceleration observed and represented in the oframe
*/
km::SpatialAcceleration
rigidlyPropagateAccelerationTpUtil(const km::HomTran &p_to_t_T,
const km::SpatialAcceleration &p_o_to_p_A);
/* The Coriolis accel utility 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.70 in Section 1.5, we use the notation
alpha_H(F,G) = 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 the above accleration is *NOT* involved in V(F,G), and
that it 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 Utility function to 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 o_to_p_T the oframe to pframe rel transform.
* @param o_to_p_V the oframe to pframe rel spatial velocity.
* @param p_to_t_T the pframe to target rel transform.
* @param p_to_t_V the pframe to target rel spatial velocity.
* @return the 'oop' Coriolis spatial acceleration vector
*/
km::SpatialAcceleration coriolisAccelOopUtil(const km::HomTran &o_to_p_T,
const km::SpatialVelocity &o_to_p_V,
const km::HomTran &p_to_t_T,
const km::SpatialVelocity &p_to_t_V);
/**
* @brief Utility function to 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 o_to_p_T the oframe to pframe rel transform.
* @param o_to_p_V the oframe to pframe rel spatial velocity.
* @param p_to_t_V the pframe to target rel spatial velocity.
* @param o_to_t_T the oframe to target rel transform.
* @param o_to_t_V the oframe to target rel spatial velocity.
* @return the 'tpp' Coriolis spatial acceleration vector
*/
km::SpatialAcceleration coriolisAccelTppUtil(const km::HomTran &o_to_p_T,
const km::SpatialVelocity &o_to_p_V,
const km::SpatialVelocity &p_to_t_V,
const km::HomTran &o_to_t_T,
const km::SpatialVelocity &o_to_t_V);
/**
* @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.
*
* @param o_to_p_T the oframe to pframe rel transform.
* @param o_to_p_V the oframe to pframe rel spatial velocity.
* @return the pframe observed spatial velocity, i.e. p_to_o_V
*/
km::SpatialVelocity pframeObservedRelSpVelUtil(const km::HomTran &o_to_p_T,
const km::SpatialVelocity &o_to_p_V);
/**
* @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).
*
* @param T the oframe to pframe rel transform.
* @param V the oframe to pframe rel spatial velocity.
* @param A the oframe to pframe rel spatial acceleration.
* @return the pframe observed spatial acceleration, i.e. p_to_o_A
*/
km::SpatialAcceleration pframeObservedRelSpAccelUtil(const km::HomTran &T,
const km::SpatialVelocity &V,
const km::SpatialAcceleration &A);
// NOLINTEND(readability-identifier-naming)
} // namespace Karana::Frame