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 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;
public:
/**
* @brief Return the indices of the independent Q coordinates.
* @return the independent Q coordinate indices
*/
std::vector<unsigned int> independentIndicesU() const { return _indep_indices_U; }
/**
* @brief Return the indices of the independent U coordinates.
* @return the independent U coordinate indices
*/
std::vector<unsigned int> independentIndicesQ() const { return _indep_indices_Q; }
public:
/**
* @brief Constructs a CECompoundBody with list of independent bodies
*
* @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
* @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);
/**
* @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.
*
* @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
* @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<unsigned int>
&indep_indices_U, // NOLINT(readability-identifier-naming)
const std::vector<unsigned int>
&indep_indices_Q // NOLINT(readability-identifier-naming)
= {});
/**
* @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.
*
* @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
*/
CECompoundBody(std::string_view name,
kc::ks_ptr<SubGraph> parent_sg,
kc::ks_ptr<SubGraph> bodies_sg,
const std::vector<unsigned int>
&indep_indices_U, // NOLINT(readability-identifier-naming)
const std::vector<unsigned int>
&indep_indices_Q // NOLINT(readability-identifier-naming)
= {});
/**
* @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.
*
* @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
*/
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);
std::string
dumpString(std::string_view prefix = "",
const Karana::Core::Base::DumpOptions *options = nullptr) const override;
protected:
void _discard(Karana::Core::ks_ptr<Karana::Core::Base> &base) override;
kc::ks_ptr<CompoundSubhinge> _createSubhinge(kc::ks_ptr<CompoundHinge> hinge) override;
/** Convert the input minimal independent coords Q into the full
dependent Q for all the aggregated subhinges for the
compound body as appropriate and set the values. Note that
the constraints are at the velocity level and not the Q
level, and in some cases Q is subject to a different (and
perhaps no) constraints, The global flag controls whether
the incoming Q values are global are local chart
coordinates.
* @param min_Q the minimal coordinates
* @param global flag whether incoming coords are local or global
*/
void
/** Convert the input minimal coords Q into the full dependent Q for all the
aggregated subhinges for the compound body */
_updateDependentQ(const km::Vec &min_Q, // NOLINT(readability-identifier-naming)
bool global) 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 X matrix that maps the min independent U to the
dependent U (without constraint dofs) for the compound body
* @returns the non-constraint X matrix
*/
km::Mat _matX() 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 Cache callback to compute the X matrix that maps the min independent U to the
full dependent U for the compound body
* @param val the matrix to populate
*/
void _computeConstraintX(km::Mat &val) const;
/** @brief Cache callback to compute the Xdot*U product
* @param val the vector to populate
*/
void _computeXdotU(km::Vec &val) 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 Compute the constraint error row indices to use in X
computation */
void _updateConstraintRowIndices();
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;
/** indices of all independent U coords among the embedded coordinates */
std::vector<unsigned int> _indep_indices_U;
/** indices of all independent Q coords among the embedded coordinates */
std::vector<unsigned int> _indep_indices_Q;
/** List contains the physical subhinge+offset value for each of the
independent U indices */
std::vector<std::pair<kc::ks_ptr<PhysicalSubhinge>, size_t>> _indep_cbs_U;
/** List contains the physical subhinge+offset value for each of the
independent Q indices */
std::vector<std::pair<kc::ks_ptr<PhysicalSubhinge>, size_t>> _indep_cbs_Q;
/** the CK solver used to solve for the dependent coordinates */
kc::ks_ptr<ConstraintKinematicsSolver> _cks = nullptr;
/** indices for non-redundant rows for the aggregation graphs
constraint velocity error Jacobian */
std::vector<unsigned int> _rows_Y;
/** same as _rows_Y */
std::vector<unsigned int> _rows_Ydot;
/** indices of all dependent coords among the _bodies_tree coordinates */
std::vector<unsigned int> _dep_indices_U;
/** indices of all frozen coords among the _bodies_tree coordinates */
std::vector<unsigned int> _frozen_indices_U;
};
} // namespace Karana::Dynamics