Program Listing for File Algorithms.h#
↰ Return to documentation for file (include/Karana/SOADyn/Algorithms.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 Algorithms class.
*
* This class contains dynamics algorithms, which are all accessible as static methods.
*/
#pragma once
#include "Karana/Math/StateSpace.h"
#include "Karana/SOADyn/StatePropagator.h"
#include "Karana/SOADyn/SubGraph.h"
namespace Karana::Dynamics::Algorithms {
namespace kc = Karana::Core;
namespace kd = Karana::Dynamics;
/**
* @brief Calculate the overall kinetic energy of the component bodies in the SubTree
*
* See the \sref{mass_prop_algorithms_sec} section for more
* information.
*
* @param st SubTree instance
* @returns Kinetic energy.
*/
double evalKineticEnergy(const SubTree &st);
/**
* @brief Calculate the overall gravitational potential energy of the component bodies in
* the SubTree
*
* See the \sref{mass_prop_algorithms_sec} section for more
* information.
*
* @param st SubTree instance
* @returns Gravitational potential energy.
*/
double evalGravitationalPotentialEnergy(const SubTree &st);
/**
* @brief Return the location of the sub-tree CM from the virtual root.
*
* Return the current location of the center-of-mass frame for the
* whole sub-tree with respect to the virtual root.
*
*
* See the \sref{embedded_control_algorithms_sec} section for more
* information.
*
* @param st SubTree instance
* @return The position of the CM with respect to the virtual root.
*
*/
km::Vec3 evalCmLocation(const SubTree &st);
/**
* @brief Calculate the overall spatial momentum for the SubTree
*
* The returned value is by default is referenced about, and
* represented in the virtual root frame. Note that this
* quantity is conserved in the absence of external forces on
* the system.
*
*
* See the \sref{embedded_control_algorithms_sec} section for more
* information.
*
* @param st SubTree instance
* @returns Spatial momentum.
*/
km::SpatialVector evalSpatialMomentum(const SubTree &st);
/**
* @brief Calculate the overall external spatial force for for the SubTree about its CM
*
* The computed value includes external force contributions from
* all of the force nodes on all the bodies (including active
* contact nodes) as well as any constraint forces from
* constraint nodes. The returned value is referenced about, and
* represented in the sub-tree's center of mass frame.
*
* See the \sref{embedded_control_algorithms_sec} section for more
* information.
*
* @param st SubTree instance
* @returns The overall external spatial force.
*/
km::SpatialVector evalExternalSpatialForce(const SubTree &st);
/**
* @brief Calculate the centroidal spatial momentum for the SubTree
*
* This is the same as the sub-tree's spatial momentum (computed
* by evalSpatialMomentum()), except that this value is
* referenced about the sub-tree's center of mass location,
* though still represented in the virtual root frame. Note that
* this quantity is conserved in the absence of external forces
* on the system.
*
* This method does not support deformable bodies.
*
*
* See the \sref{embedded_control_algorithms_sec} section for more
* information.
*
* @param st SubTree instance
* @returns Spatial momentum.
*/
km::SpatialVector evalCentroidalMomentum(SubTree &st);
/**
* @brief Calculate the centroidal momentum matrix for the SubTree
*
* This returns the centroidal momentum matrix (CMM) that defines
* the mapping between the system generalized velocities and the
* system's centroidal momentum. Note that this matrix is
* configuration dependent. This matrix is used in the context
* the control of legged robots and managing the motion of their
* CM for stable walking.
*
*
* See the \sref{embedded_control_algorithms_sec} section for more
* information.
*
* @param st SubTree instance
* @returns The 6xnU centroidal momentum matrix (CMM).
*/
km::Mat evalCentroidalMomentumMatrix(SubTree &st);
/**
* @brief Calculate the tree system mass matrix using CRB approach
*
* This method uses the composite rigid body inertias (CRB)
* recursive method for computing the mass matrix
*
* See the \sref{mass_prop_algorithms_sec} section for more
* information.
*
* This method does not allow internal 6 dof hinges to avoid
* ambiguities about whether mass properties should be coupled
* across such hinges. Also the prescribed motion flag for any
* subhinge that has been enabled is reset. This is to ensure
* that the mass matrix computed from this method are consistent
* with the results from the alternative dynamics based
* computation methods.
*
* @param st SubTree instance
* @param coord_data CoordData to specify the order of the mass matrix rows and columns
* @return The tree mass matrix
*/
km::Mat evalTreeMassMatrixCRB(SubTree &st, kc::ks_ptr<CoordData> coord_data = nullptr);
/**
* @brief Calculate the tree system mass matrix using Newton-Euler inverse dynamics
*
* Calculate the tree system mass matrix using NE inverse
* dynamics. A CoordData argument can be specified to set the
* specific order for the rows and columns in the computed mass
* matrix.
*
* This method does not currently support deformable bodies.
*
* This method does not allow internal 6 dof hinges to avoid
* ambiguities about whether mass properties should be coupled
* across such hinges. Also the prescribed motion flag for any
* subhinge that has been enabled is reset. This is to ensure
* that the mass matrix computed from this method are consistent
* with the results from the alternative dynamics based
* computation methods.
*
*
* See the \sref{mass_prop_algorithms_sec} section for more
* information.
*
* @param st SubTree instance
* @param coord_data CoordData to specify the order of the mass matrix rows and columns
* @return The tree mass matrix
*/
km::Mat evalTreeMassMatrixInvDyn(SubTree &st, kc::ks_ptr<CoordData> coord_data = nullptr);
/**
* @brief Calculate the tree system mass matrix inverse using forward dynamics
*
* Calculate the tree system mass matrix inverse using ATBI
* forward dynamics. A CoordData argument can be specified to
* set the specific order for the rows and columns in the
* computed mass matrix inverse. This method currently supports
* physical rigid and flexible bodies, but not compound bodies.
*
* This method does not allow internal 6 dof hinges to avoid
* ambiguities about whether mass properties should be coupled
* across such hinges. Also the prescribed motion flag for any
* subhinge that has been enabled is reset. This is to ensure
* that the mass matrix computed from this method are consistent
* with the results from the alternative dynamics based
* computation methods.
*
*
* See the \sref{mass_prop_algorithms_sec} section for more
* information.
*
* @param st SubTree instance
* @param coord_data CoordData to specify the order of the mass matrix inverse's rows and
* columns
* @return The tree mass matrix inverse
*/
km::Mat evalTreeMassMatrixInvFwdDyn(SubTree &st, kc::ks_ptr<CoordData> coord_data);
/**
* @brief Calculate serial-chain system mass matrix inverse using {{ SOA }} operator-based
* decomposition expressions
*
* This method does not require the mass matrix or matrix
* inversions to compute the mass matrix inverse! The algorithm
* instead takes advantage of an analytical operator based
* decomposition of the mass matrix inverse to carry out the
* computation much lower cost.
*
* A CoordData argument can be specified to set the specific
* order for the rows and columns in the computed mass matrix
* inverse. This method cannot be used for systems containing
* deformable bodies. This mass matrix inverse computation can
* also be done via the more general evalTreeMassMatrixInverse()
* which also works for deformable bodies. We however also have
* this method since it is considerably simpler for the simpler
* serial-chain topology.
*
* This method does not allow internal 6 dof hinges to avoid
* ambiguities about whether mass properties should be coupled
* across such hinges. Also the prescribed motion flag for any
* subhinge that has been enabled is reset. This is to ensure
* that the mass matrix computed from this method are consistent
* with the results from the alternative dynamics based
* computation methods.
*
*
* See the \sref{mass_prop_algorithms_sec} section for more
* information.
*
* @param st SubTree instance
* @param coord_data CoordData to specify the order of the mass matrix inverse's rows and
* columns
* @return The serial-chain mass matrix inverse
*/
km::Mat evalSerialChainMassMatrixInverse(SubTree &st,
kc::ks_ptr<CoordData> coord_data = nullptr);
/**
* @brief Calculate the tree system mass matrix inverse using {{ SOA }} decomposition
* expressions
*
* This method does not require the mass matrix or matrix
* inversions to compute the mass matrix inverse! The algorithm
* instead takes advantage of an analytical operator based
* decomposition of the mass matrix inverse to carry out the
* computation much lower cost. For serial-chain system, the
* mass matrix inverse can also be computed using the
* evalSerialChainMassMatrixInverse() specifically for such
* systems.
*
* A CoordData argument can be specified to set the specific
* order for the rows and columns in the computed mass matrix
* inverse. This method is currently limited to physical and
* rigid bodies.
*
* This method supports both rigid and deformable bodies.
*
* This method does not allow internal 6 dof hinges to avoid
* ambiguities about whether mass properties should be coupled
* across such hinges. Also the prescribed motion flag for any
* subhinge that has been enabled is reset. This is to ensure
* that the mass matrix computed from this method are consistent
* with the results from the alternative dynamics based
* computation methods.
*
*
* See the \sref{mass_prop_algorithms_sec} section for more
* information.
*
* @param st SubTree instance
* @param coord_data CoordData to specify the order of the mass matrix inverse's rows and
* columns
* @return The tree mass matrix inverse
*/
km::Mat evalTreeMassMatrixInverse(SubTree &st, kc::ks_ptr<CoordData> coord_data = nullptr);
/**
* @brief Evaluate Newton-Euler inverse dynamics
*
*
* See the \sref{embedded_control_algorithms_sec} section for
* more information.
*
* @param st SubTree instance
*/
void evalInverseDynamics(SubTree &st);
/**
* @brief Compute the forward dynamics for a system.
*
* This uses tree-augmented forward dynamics if there are with loop
* constraints, and tree forward dynamics if there are no loop constraints.
*
* The tree-augmented algorithm solves the graph system forward dynamics
* using recursive {{ SOA }} recursive algorithms with exact
* enforcement of the loop constraints. This algorithm does
* not require the computation of inversion of the mass matrix.
*
* The tree forward dynamics uses Articulated Body Inertia (ATBI)
* based forward dynamics.
*
* See the \sref{simulation_algorithms_sec} section for more
* information.
*
* @param st SubTree instance
*/
void evalForwardDynamics(SubTree &st);
/**
* @brief Calculate the computed torque using NE algorithm
*
* Method to compute the "computed torque", i.e. the generalized
* forces needed - usually for a specific generalized
* acceleration inputs. This assumes that the state (generalized
* coordinates (Q) and velocities (U) have been set to the
* desired values in the multibody system, as have the
* generalized accelerations (Udot). If the generalized forces
* are T in the system, the returned value from this method
* subtracts out the input T from the values computed by inverse
* dynamics.
*
* This method should not to be used for systems with deformable
* bodies. Systems with deformable bodies are under-actuated,
* while computed torque only applies to fully actuated
* systems. Prescribed subhinges and constraints are ignored in
* this computation.
*
*
* See the \sref{embedded_control_algorithms_sec} section for more
* information.
*
* @param st SubTree instance
* @return The computed T generalized forces
*/
km::Vec evalComputedTorque(SubTree &st);
/**
* @brief Compute the gravity compensation torque for the system
*
* Compute the generalized forces needed to support the
* gravitational load on the system. This method automatically
* takes into account additional bodies that are attached or
* detached from the system.
*
*
* See the \sref{embedded_control_algorithms_sec} section for more
* information.
*
* @param st SubTree instance
* @return The vector of generalized forces.
*/
km::Vec evalGravityCompensation(SubTree &st);
/**
* @brief Compute the gradient matrix for the constraint pose errors.
*
* Compute the gradient matrix for the constraint pose errors
* for the bilateral constraints on the SubGraph.
*
* Note that the returned matrix may not have full row rank when
* the constraints are not all independent.
*
* The SubGraph::constraintErrorAt() and the
* SubGraph::constraintResidualAt() methods and can be used to
* look up the constraint instance associated with any specific
* row index in the returned matrix (based on whether
* with_constraint is 'true' or 'false'
* respectively). Similarly, the SubGraph::coordAt() method can
* be used to look up the coordinate instance for a specific
* column index in the returned matrix.
*
* See the \sref{constraints_kin_algorithms_sec} section for more
* information.
*
* @param sg SubGraph instance
* @return The gradient matrix for the constraint pose errors.
*/
km::Mat evalConstraintPoseGradientMatrix(SubGraph &sg);
/**
* @brief Compute the velocity constraint matrix.
*
* Compute the velocity constraint matrix, Gc, such that Gc * U
* = 0, i.e. the matrix whose null space contains the
* generalized velocity U values that satisfy the loop and
* coordinate constraints on the SubGraph.
*
* If "with_constraints" is true, then Gc's columns include ones
* for the loop hinge constraint coordinates, and the block row
* for each constraint contain the mapping to their constraint
* errors.
*
* On the other hand, if "with_constraints" is false, Gc's
* columns exclude ones for the constraint coordinates, and the
* block row for each loop hinge constraint maps to the residual
* vector for each constraint. In the latter case, the number of
* columns and rows shrink equally by the number of constraint
* coordinates.
*
* Note that the returned matrix may not have full row rank when
* the constraints are not all independent. The
* evalIndepVelConstraintCoordIndices() method can be used to check
* for the rows that are independent.
*
* The SubGraph::constraintErrorAt() and the
* SubGraph::constraintResidualAt() methods and can be used to
* look up the constraint instance associated with any specific
* row index in the returned matrix (based on whether
* with_constraint is 'true' or 'false'
* respectively). Similarly, the SubGraph::coordAt() method can
* be used to look up the coordinate instance for a specific
* column index in the returned matrix.
*
* See the \sref{constraints_kin_algorithms_sec} section for more
* information.
*
* @param sg SubGraph instance
* @param with_constraints If true, columns for the cutjoint loop constraint coords are
* included
* @return The velocity constraint matrix.
*/
km::Mat evalVelocityConstraintMatrix(SubGraph &sg, bool with_constraints);
/**
* @brief Compute the best indices for independent Q coordinates for the bilateral
* constraints on the system
*
* For a SubGraph with constraints, only a subset of the Q pose-level
* coordinates are independent. This method computes the indices of the
* coordinates in the Q vector that are best choice for
* independent coordinates (at the current configuration). A
* non-empty available_indices list can be used to restrict the
* pool of indices to pick the independent coordinates from.
*
* Since the pose and velocity level constraints may not be the
* same, the results from this method may not match the
* constraint and coordinate indices returned by the
* evalIndepVelConstraintCoordIndices() method.
*
* See the \sref{constraints_kin_algorithms_sec} section for more
* information.
*
* @param sg SubGraph instance
* @param available_indices If non-empty, the indices to pick the independent coordinates
* from
* @return a list of independent coordinate indices based on the pose-level constraints
*/
km::VecInt evalIndepPoseCoordIndices(SubGraph &sg, const km::VecInt &available_indices = {});
// const std::vector<unsigned int> &available_indices = {});
/**
* @brief Compute the indices for best independent bilateral
* constraints on the system based on the pose gradients matrix
* map.
*
* For a SubGraph with constraints, only a subset of the Q
* pose-level constraints may be independent. This method
* computes the indices of the independent constraints.
*
* Since the pose and velocity level constraints may not be the
* same, the results from this method may not match the
* constraint and coordinate indices returned by the
* evalIndepVelConstraintCoordIndices() method.
*
* See the \sref{constraints_kin_algorithms_sec} section for more
* information.
*
* @param sg SubGraph instance
* @return a pair consisting of a list of independent constraint indices, and a list of
* independent coordinate indices based on the pose-level constraints
*/
km::VecInt evalIndepPoseConstraintIndices(SubGraph &sg);
/**
* @brief Compute the best indices for independent constraints and velocity coordinates
* using the velocity constraint matrix
*
* For a SubGraph with constraints, only a subset of the
* U velocity coordinates are independent (and not all constraints
* may be independent). This method computes the indices of the
* independent constraints, and the indices of the coordinates
* in the U vector that are best choice for independent velocity
* coordinates (at the current configuration).
*
* Since the pose and velocity level constraints may not be the
* same, the results from this method may not match the
* constraint and coordinate indices returned by the
* evalIndepPoseCoordIndices() and evalIndepPoseConstraintIndices() methods.
*
* See the \sref{constraints_kin_algorithms_sec} section for more
* information.
*
* @param sg SubGraph instance
* @param available_indices the list of indices to pick the independent ones from
* @return a pair consisting of a list of independent constraint indices, and a list of
* independent coordinate indices based on the velocity-level constraints
*/
std::pair<km::VecInt, km::VecInt>
evalIndepVelConstraintCoordIndices(SubGraph &sg, const km::VecInt &available_indices = {});
/**
* @brief Compute the best indices for independent and dependent velocity coordinates
* matrix map
*
* For a SubGraph with constraints, only a subset of the
* velocity coordinates are independent. This method computes
* the indices of the coordinates in the U vector that are best
* choice for independent velocity coordinates (at the current
* configuration). It also returns a matrix X that can be used
* to compute the dependent velocity coordinates, U_d, for a set
* of independent velocity coordinates, U_i, via U_d = X * U_i,
*
*
* See the \sref{constraints_kin_algorithms_sec} section for more
* information.
*
* @param sg SubGraph instance
* @param available_indices the list of indices to pick the independent ones from
* @return The list of independent coordinate indices and the dependent coordinate mapping
* matrix
*/
std::pair<km::VecInt, km::Mat>
evalVelCoordinatePartitioning(SubGraph &sg, const km::VecInt &available_indices = {});
/**
* @brief Compute the dependent velocity coordinates matrix map for the specified set of
* independent coordinates
*
* For a SubGraph with constraints, only a subset of the
* velocity coordinates are independent. This method computes
* the matrix X that can be used to compute the dependent
* velocity coordinates, U_d, for the specified set of
* independent velocity coordinates, U_i, via U_d = X * U_i,
*
* See the \sref{constraints_kin_algorithms_sec} section for more
* information.
*
* @param sg SubGraph instance
* @param indep_indices the list of independent U indices
* @param full if true, the matrix also includes columns for the independent coordinates,
* else only for the dependent coordinates
* @return the dependent coordinates mapping matrix
*/
km::Mat evalDependentVelCoordinatesMatrix(SubGraph &sg,
const km::VecInt &indep_indices,
bool full = false);
/**
* @brief Return a matrix for computing squeeze generalized forces
*
* Compute the squeeze matrix, S, whose column space consists of
* "squeeze" generalized forces, i.e. generalized forces that
* only effect internal forces within the constrained multibody
* and do not induce any motion. Thus, the generalized force,
* S*x, for any vector x will not induce any motion. The row
* dimension is nU(), while the column dimension is the size of
* the constraints on the system.
*
* Note that the returned matrix may not have full column rank
* when the constraints are not all independent. The
* evalIndepVelConstraintCoordIndices() method can be used to check
* for the columns that are independent.
*
*
* See the \sref{constraints_kin_algorithms_sec} section for more
* information.
*
* @param sg SubGraph instance
* @return The squeeze matrix.
*/
km::Mat evalSqueezeMatrix(SubGraph &sg);
/**
* @brief Create generator for linearized state space representation of the system
*
* Method to create a StateSpace object for creating (A,B,C,D)
* state space linear system matrices for sub-tree multibody
* system. The returned state space generation object's
* generate(x,u) can be called to compute the state space matrices
* at the state and input vector set point values about which the
* linearization should be carried out.
*
* The first two inputs are the size of the inputs and outputs for
* the state space system. The u_fn argument is a user-defined
* function that takes (x,u) arguments and sets the appropriate
* forces, gravity etc values for the in the multibody system for
* dynamics computations. The y_fn argument takes (x, u, y)
* arguments and computes the outputs y for the (x, u) and state
* and input values.
*
*
* See the \sref{gnc_algorithms_sec} section for more
* information.
*
* @param mm The ModelManager whose SubTree and models will be used to generate the
* StateSpace.
* @param n_inputs number of inputs
* @param n_outputs number of outputs
* @param u_fn function to evaluate the state derivative value for an input value
* @param y_fn function to evaluate the output value for a given state
* @return State space generator instance
*/
km::StateSpace stateSpaceGenerator(ModelManager &mm,
size_t n_inputs,
size_t n_outputs,
std::function<void(const km::Vec &x, const km::Vec &u)> u_fn,
km::lin_fn y_fn);
/**
* @brief Calculate the overall spatial inertia for a sub-tree
*
* Return the overall spatial inertia of all the bodies in the sub-tree about the
* reference frame. The multibody virtual root serves as the
* default reference frame.
*
*
* See the \sref{mass_prop_algorithms_sec} section for more
* information.
*
* @param st SubTree instance
* @param ref_frame reference frame for the spatial inertia (default: the virtual root)
* @return The overall tree spatial inertia
*/
km::SpatialInertia evalSpatialInertia(const SubTree &st,
kc::ks_ptr<kf::Frame> ref_frame = nullptr);
/**
* @brief Return a Jacobian generator for the subtree
*
* Create a Jacobian generator object for the specified SubGraph
* and its coordinates and the list source/target frame pairs in
* the list of frame2frames. Note that the resulting jacobian and
* pose gradient matrices will exclude columns for any coordinates
* frozen in the coord data of the subtree.
*
*
* See the \sref{kinematics_algorithms_sec} section for more
* information.
*
* @param f2fs list of FrameToFrames to compute the Jacobian for
* @param cd the CoordData that specify the columns in the computed
* Jacobian
* @return The Jacobian generator instance
*/
kc::ks_ptr<kd::MultiJacobianGenerator>
jacobianGenerator(const std::vector<kc::ks_ptr<kf::FrameToFrame>> &f2fs,
kc::ks_ptr<CoordData> cd);
/**
* @brief Return a constraint/inverse kinematics solver instance
*
* Creates a constraint kinematics (CK) solver for the specified
* SubGraph and its coordinates and loop constraints. The CK
* solver uses the CoordData for the SubGraph for the
* coordinates to solver for. Coordinates can be frozen during
* the solution process. Only the subhinge coordinates are
* solved for, and deformation coordinates - if any - are not
* used.
*
*
* See the \sref{kinematics_algorithms_sec} section for more
* information.
*
* @param sg SubGraph instance
* @return The constraint kinematics solver instance
*/
kc::ks_ptr<kd::ConstraintKinematicsSolver> constraintKinematicsSolver(SubGraph &sg);
/**
* @brief Calculate the tree system operational space compliance matrix (OSCM)
*
* The OSCM is defined at J*M^{-1}*J^*, where J is the Jacobian
* matrix, and M is the mass matrix of the system. This method
* does not use this brute force expression. Instead it uses the
* {{ SOA }} recursive methods to do this computation without ever
* computing the mass matrix or its inverse, or even the
* Jacobian matrix itself.
*
* In addition, one or more subhinges can also be provided. The
* returned matrix will include additional mass matrix inverse
* rows and columns for these subhinges. This is equivalent to
* computing the OSCM for the "extended Jacobian" defined as
* [J \\ E], where E is a selector matrix that picks out a subset
* of the subhinges in the system. While the rows and columns
* associated with the frames define a mapping between spatial
* forces and spatial accelerations, the subhinge rows and
* columns define the mapping between generalized forces and
* generalized accelerations. The off-diagonal sub-blocks
* define cross-term mappings.
*
* If no subhinges are specified, the returned matrix is the
* regular OSCM, and if no frames are specified, the returned
* matrix is the sub-block of the mass matrix inverse with rows
* and columns for the specified subhinges. This more general
* form of the OSCM allows users to properly handle a mix of
* constraints from the task and joint spaces.
*
*
* See the \sref{mass_prop_algorithms_sec} section for more
* information.
*
* @param st SubTree instance
* @param body_frames list of Frame instances to compute the OSCM for
* @param subhinges list of physical subhinge mass matrix inverse contributions to include
*
* @return The tree OSCM matrix
*/
km::Mat evalFramesOSCM(SubTree &st,
const std::vector<kc::ks_ptr<kf::Frame>> &body_frames,
const std::vector<kc::ks_ptr<PhysicalSubhinge>> &subhinges = {});
/**
* @class AlgorithmsPrivate
* @brief Private functions used by the functions in algorithms.
*
* This is a container class for private static methods to compute various
* multibody algorithms. See \sref{algorithms_sec} section for more information.
*/
class AlgorithmsPrivate {
// For access to _evalTreeForwardDynamics
friend class ModelManager;
friend km::Mat evalTreeMassMatrixInvFwdDyn(SubTree &, kc::ks_ptr<CoordData>);
friend void evalForwardDynamics(SubTree &);
private:
/**
* @brief Evaluate Articulated Body Inertia (ATBI) based forward dynamics for a tree system
*
*
* See the \sref{simulation_algorithms_sec} section for more
* information.
*
* @param st SubTree instance
*/
static void _evalTreeForwardDynamics(SubTree &st);
};
} // namespace Karana::Dynamics::Algorithms