Program Listing for File CECompoundBody.h#
↰ Return to documentation for file (include/Karana/SOADyn/CECompoundBody.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 CECompoundBody class.
*/
#pragma once
#include "Karana/SOADyn/CompoundBody.h"
#include <vector>
namespace Karana::Dynamics {
namespace kc = Karana::Core;
class CoordData;
class SubGraph;
class ConstraintKinematicsSolver;
class MultiJacobianGenerator;
class PhysicalSubhinge;
class CoordinateConstraint;
class CELoopKinematicsBase;
class CECompoundBody;
/**
* @class CECompoundBodyVars
* @brief The Vars for the CECompoundBody class.
*/
// class CECompoundBodyVars : public Karana::Core::BaseVars {
class CECompoundBodyVars : public CompoundBodyVars {
public:
/**
* @brief CECompoundBodyVars constructor. The constructor is not meant to be
* called directly. Please use the create(...) method instead to create
* an instance.
*
* @param body The body associated with this CECompoundBodyVars.
*/
CECompoundBodyVars(const kc::ks_ptr<const CECompoundBody> &body);
/**
* @brief Destructor.
*/
~CECompoundBodyVars();
/**
* @brief Create an instance of the CECompoundBodyVars.
*
* @param body The body associated with this CECompoundBodyVars.
* @returns A pointer to the newly created instance of PhysicalBodyVars.
*/
static kc::ks_ptr<CECompoundBodyVars> create(const kc::ks_ptr<const CECompoundBody> &body);
/**
* @brief Get all the Vars that this VarHolder has.
*
* @returns A map of Vars, where the Var name is the key and the Var is the value.
*/
kc::NestedVars getAllVars() const override;
/// tolerance used to compute rank
kc::ks_ptr<Karana::Core::Var_T<double>> rank_threshold;
};
/**
* @class CECompoundBody
* @brief Represents a compound body with embedded constraints (for constraint embedding)
*
* This class is for compound bodies with embedded constraints
*/
class CECompoundBody : public CompoundBody {
/* to get access to _toDependentQ() etc */
friend class CECompoundSubhinge;
// for vars
friend class CECompoundBodyVars;
// To allow discarding the LoopSolver before we make the bodies_tree
// unhealthy
friend class SubTree;
public:
/**
* @brief Return the indices of the independent Q coordinates.
* @return the independent Q coordinate indices
*/
const km::VecInt &getIndependentIndicesU() const;
/**
* @brief Return the indices of the independent U coordinates.
* @return the independent U coordinate indices
*/
const km::VecInt &getIndependentIndicesQ() const;
/**
* @brief Set the independent Q and U coordinate indices.
*
* If the Q indices are unspecified, they are assumed to be the
* same as the U indices.
*
* @param indep_indices_U the independent U coordinate indices
* @param indep_indices_Q the independent Q coordinate indices
*/
void setIndependentIndices(
const km::VecInt &indep_indices_U, // NOLINT(readability-identifier-naming)
// Note that clang-format pushes down the linter comment,
// so we disable it just for this line
// clang-format off
const km::VecInt
&indep_indices_Q = {} // NOLINT(readability-identifier-naming)
// clang-format on
);
public:
/**
* @brief Constructs a CECompoundBody with list of independent bodies
*
* The CECompoundBody will own the SubGraph passed in. That means the CECompoundBody will
* attempt to discard the SubGraph when it is discarded.
*
* @param name instance name
* @param parent_subtree the parent SubGraph this the body will belong to
* @param component_bodies SubGraph with bodies being embedded
* @param indep_bodies the bodies whose coordinates should be independent
* @param rank_threshold threshold to use when computing error Jacobian rank
* @returns a CECompoundBody instance
*/
static kc::ks_ptr<CECompoundBody>
create(std::string_view name,
kc::ks_ptr<SubGraph> parent_subtree,
kc::ks_ptr<SubGraph> component_bodies,
const std::vector<kc::ks_ptr<PhysicalBody>> &indep_bodies,
double rank_threshold = 1e-8);
/**
* @brief Constructs a CECompoundBody with list of independent coord indices
*
* The option of specifying independent coordinates at the
* indices level gives finer-grain control than the other
* create() method. Also, this method allows specifying
* different values for the independent Q and U coordinates. The
* U coordinates are used if the Q list is empty. If empty
* indices are specified, then the required number of
* independent coordinate indices will be auto picked.
*
* The CECompoundBody will own the SubGraph passed in. That means the CECompoundBody will
* attempt to discard the SubGraph when it is discarded.
*
* @param name instance name
* @param parent_subtree the parent SubGraph this the body will belong to
* @param component_bodies SubGraph with bodies being embedded
* @param indep_indices_U the list of independent U coordinate indices
* @param indep_indices_Q the list of independent Q coordinate indices
* @param rank_threshold threshold to use when computing error Jacobian rank
* @returns a CECompoundBody instance
*/
static kc::ks_ptr<CECompoundBody>
create(std::string_view name,
kc::ks_ptr<SubGraph> parent_subtree,
kc::ks_ptr<SubGraph> component_bodies,
const km::VecInt &indep_indices_U, // NOLINT(readability-identifier-naming)
const km::VecInt &indep_indices_Q // NOLINT(readability-identifier-naming)
= {},
double rank_threshold = 1e-8);
/**
* @brief Destructor.
*/
virtual ~CECompoundBody();
/**
* @brief Constructs a CECompoundBody with list of independent
* bodies. The constructor is not meant to be called
* directly. Please use the create(...) method instead
* to create an instance one of the classes derived from
* this.
*
* The CECompoundBody will own the SubGraph passed in. That means the CECompoundBody will
* attempt to discard the SubGraph when it is discarded.
*
* @param name instance name
* @param parent_sg the parent SubGraph this the body will belong to
* @param bodies_sg SubGraph with bodies being embedded
* @param indep_indices_U the list of independent U coordinate indices
* @param indep_indices_Q the list of independent Q coordinate indices
* @param rank_threshold threshold to use when computing error Jacobian rank
*/
CECompoundBody(std::string_view name,
kc::ks_ptr<SubGraph> parent_sg,
kc::ks_ptr<SubGraph> bodies_sg,
const km::VecInt &indep_indices_U, // NOLINT(readability-identifier-naming)
const km::VecInt &indep_indices_Q // NOLINT(readability-identifier-naming)
= {},
double rank_threshold = 1e-8);
/**
* @brief Constructs a CECompoundBody with list of independent
* coord indices. The constructor is not meant to be
* called directly. Please use the create(...) method
* instead to create an instance one of the classes
* derived from this.
*
* The CECompoundBody will own the SubGraph passed in. That means the CECompoundBody will
* attempt to discard the SubGraph when it is discarded.
*
* @param name instance name
* @param parent_sg the parent SubGraph this the body will belong to
* @param bodies_sg SubGraph with bodies being embedded
* @param indep_bodies the bodies whose coordinates should be independent
* @param rank_threshold threshold to use when computing error Jacobian rank
*/
CECompoundBody(std::string_view name,
kc::ks_ptr<SubGraph> parent_sg,
kc::ks_ptr<SubGraph> bodies_sg,
const std::vector<kc::ks_ptr<PhysicalBody>> &indep_bodies,
double rank_threshold = 1e-8);
std::string
dumpString(std::string_view prefix = "",
const Karana::Core::Base::DumpOptions *options = nullptr) const override;
/** @brief Return the CELoopKinematicsSolver for the compound body
* @return the CE loop solver
*/
kc::ks_ptr<CELoopKinematicsBase> loopSolver() const { return _ce_loop_solver; }
/** @brief Replace the embedded child compound body with the embedded
physical bodies it contains with this compound body.
The child compound body is discarded by this method after
the transfer has been completed.
@param child_cpbd the embedded compound body to be flattened
*/
void flatten(kc::ks_ptr<CompoundBody> &child_cpbd) override;
protected:
/** @brief Compute the independent coordinates for the compound body's constraints
*
* @param rank_threshold threshold to use when computing error Jacobian rank
@return the list of independent coordinate indices
*/
const km::VecInt _computeIndependentIndicesU(double rank_threshold) const;
Karana::Core::ks_ptr<Karana::Core::BaseVars> _getVars() const override;
void _discard(Karana::Core::ks_ptr<Karana::Core::Base> &base) override;
kc::ks_ptr<CompoundSubhinge> _createSubhinge(kc::ks_ptr<CompoundHinge> hinge) override;
/** @brief Return the X matrix that maps the min independent U to the
dependent U (without constraint dofs) for the compound body
* @return the non-constraint X matrix
*/
km::Mat _matX() const;
/** Return the X matrix that maps the min independent U to the
full U (independent, dependent, constraint dofs) for the
compound body
* @returns the full constraint X matrix
*/
const km::Mat &_constraintX() const;
/** Return the Xdot*U product vector where Xdot is the time derivative
of the X constraint matrix
* @returns the Xdot*U product
*/
const km::Vec &_constraintXdotU() const;
/** @brief Get the f' values from tree forward dynamics for all of the
embedded bodies stacked into a single vector
* @return the stacked vector with f' values
*/
km::Vec _getFprimeTreeFwdDyn() const override;
/**
* @brief Update the provided bodies_tree to use the provided new_root as its new virtual
* root.
*
* This does so recursively, so if the bodies_tree provided has embedded CompoundBodys that
* use the same virtual root, then they will also be updated.
*
* @param new_root The new body for _bodies_tree to use as its virtual root.
*/
void _updateBodiesTreeVirtualRoot(const kc::ks_ptr<BodyBase> &new_root) override;
/**
* @brief Create the CE loop kinematics solver and assign it
* to _ce_loop_solver.
*
* @param lindep_indices_U The independent velocity indices.
* @param lindep_indices_Q The independent coordinate indices.
* @param rank_threshold The threshold to use when computing the rank of the constraint
* matrix
*/
void _createCeLoopSolver(
const km::VecInt &lindep_indices_U, // NOLINT(readability-identifier-naming)
const km::VecInt &lindep_indices_Q, // NOLINT(readability-identifier-naming)
double rank_threshold);
protected:
/** the data cache for the constraint X matrix */
kc::ks_ptr<kc::DataCache<km::Mat>> _cache_constraintX = nullptr;
/** the data cache for the constraint Xdot*U product */
kc::ks_ptr<kc::DataCache<km::Vec>> _cache_XdotU = nullptr;
/** The CE loop constraints solver for the compound body */
kc::ks_ptr<CELoopKinematicsBase> _ce_loop_solver = nullptr;
/// rank_threshold threshold to use when computing error Jacobian rank
double _rank_threshold;
};
} // namespace Karana::Dynamics