Program Listing for File SubGraph.h#
↰ Return to documentation for file (include/Karana/SOADyn/SubGraph.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 SubGraph class.
*/
#pragma once
#include "Karana/SOADyn/Defs.h"
#include "Karana/SOADyn/SubTree.h"
namespace Karana::Dynamics {
class BilateralConstraintBase;
class LoopConstraintBase;
class LoopConstraintCutJoint;
class LoopConstraintConVel;
class CoordinateConstraint;
class ConstraintKinematicsSolver;
class MultiJacobianGenerator;
/**
* @class SubGraph
* @brief Represents a subtree of bodies with motion constraints
*
* This class is a container for a sub-tree of bodies with possible
* motion constraints on them.
*
* See the \sref{physical_bodies_sec}, \sref{treembody_sec}, and
* \sref{subgraphs_sec} sections for more discussion related to the
* SubGraph class.
*/
class SubGraph : public SubTree {
// for access to _loop_constraint_list
friend class LoopConstraintCutJoint;
friend class LoopConstraintConVel;
friend class CECompoundBody;
// for access to _enabled_cutjoint_loop_constraints_list
friend ConstraintKinematicsSolver;
// for access to _Qrows
friend class Algorithms;
// for access to _resetSubhingeCharts
friend class ModelManager;
// for access to Jacobian generator
friend class PartitionedCompoundBody;
public:
/**
* @brief Constructs a SubGraph.
*
* @param name The name for the new SubGraph
* @param root The virtual root body for the new SubGraph
* @param parent_sg The parent SubGraph for the new SubGraph
* subgraph
*/
SubGraph(std::string_view name,
kc::ks_ptr<BodyBase> root,
kc::ks_ptr<SubGraph> parent_sg = nullptr);
/**
* @brief SubGraph destructor.
*/
virtual ~SubGraph(){};
bool isSubGraph() const override { return true; }
/**
* @brief Factory method to create a new SubGraph instance. See
* the SubTree documentation for more information on the bodies
* that are included in the sub-graph.
*
* If the inherit_constraints option is true then all relevant
* constraints from the parent SubGraph are added to this SubGraph in one
* swoop.
*
* @param parent_sg The parent SubGraph for the new SubGraph
* @param name The name for the new SubGraph
* @param new_root The virtual root body for the new SubGraph
* @param use_branches The list of bodies that define branches whose bodies to include
* @param stop_at The list of bodies whose descendants should not be included
* @param inherit_constraints if true, add relevant constraints from parent to this subgraph
* @return A new SubGraph instance
*/
static kc::ks_ptr<SubGraph>
create(std::string_view name,
kc::ks_ptr<SubGraph> parent_sg,
kc::ks_ptr<BodyBase> new_root,
const std::vector<kc::ks_ptr<BodyBase>> &use_branches = {},
const std::vector<kc::ks_ptr<BodyBase>> &stop_at = {},
bool inherit_constraints = true);
/**
* @brief Return the CoordData for the cut-joint loop constraint subhinges
* See \sref{coord_data_sec} section for more on the CoordData class.
* @return the cut-joint constraints CoordData instance
*/
const kc::ks_ptr<CoordData> &cutjointCoordData() const;
/**
* @brief Create and return an aggregation SubGraph for the list of constraints
*
* For a set of bilateral constraints, the aggregation sub-graph
* is the smallest sub-graph that contains their constrained
* bodies, and is "path-induced" for the underlying sub-graph. A
* path-induced sub-graph includes all the bodies from the
* parent sub-graph that are connected to each other via hinges
* and bilateral constraints (and form a sub-tree if the
* constraints are ignored). If with_velocity_constraints is
* true (usually recommended), the velocity level non-hinge
* convel constraints are also considered when identifying the
* aggregation SubGraph. Aggregation SubGraphs are used for
* multibody system constraint-embedding.
*
* @param name The name for the new SubGraph
* @param constraints The input list of constraints
* @param with_velocity_constraints If true, also use the non-hinge convel constraints when
* identifying the aggregation SubGraph
* @return the aggregation SubGraph
*/
kc::ks_ptr<SubGraph>
aggregationSubGraph(std::string_view name,
std::vector<kc::ks_ptr<BilateralConstraintBase>> constraints,
bool with_velocity_constraints);
/**
* @brief Create CECompoundBody instances for all enabled constraints
*
* Cycle through all of the enabled constraints, and create
* aggregation subgraphs for each and use it to create a
* CECompoundBody that embeds the constraint. On completion, the
* subgraph will be a pure tree, with all constraints having
* been embedded in a CECompound body. Depending on the topology,
* some of the compound bodies will be nested. *
*/
void setupConstraintEmbedding();
/**
* @brief Look up an enabled constraint by name.
* See \sref{constraints_sec} section for more info on loop constraints.
* @param name the constraint's name
* @return the constraint instance, or null if there is no enabled constraint with the
* specified name
*/
kc::ks_ptr<BilateralConstraintBase> getEnabledConstraint(std::string_view name) const;
/**
* @brief Return coordinate constraints connected to a body.
* See \sref{constraints_sec} section for more info on constraints.
*
* @param body the body instance
* @return the list of coordinate constraints involving the body
*/
std::vector<kc::ks_ptr<CoordinateConstraint>>
getBodyCoordinateConstraints(const PhysicalBody &body) const;
/**
* @brief Return loop constraints connected to a body.
* See \sref{constraints_sec} section for more info on constraints.
*
* @param body the body instance
* @return the list of loop constraints involving the body
*/
std::vector<kc::ks_ptr<LoopConstraintBase>>
getBodyLoopConstraints(const PhysicalBody &body) const;
/**
* @brief Return PhysicalBody instances involved in a loop for a
* loop constraint
*
* The Karana::Frame::Frame pair associated with
* a loop constraint define a pair of bodies whose relative
* motion is constrained. All PhysicalBody instances belonging
* to the minimal spanning tree containing this pair of bodies
* is effected by the loop constraint, and are included in the
* list of bodies returned by this method. See the
* \sref{constraints_sec} section for more info on loop constraints.
*
* @param c the constraint instance
* @return the list of bodies in the loop
*/
std::vector<kc::ks_ptr<PhysicalBody>>
getLoopConstraintBodies(const LoopConstraintBase &c) const;
void displayModel(std::string_view prefix = "") const override;
std::string dumpString(std::string_view prefix,
const Karana::Core::Base::DumpOptions *options) const override;
/**
* @brief Return the list of enabled constraints for the SubGraph
*
* This returns the list of constraints that are currently
* "active" for this SubGraph. See \sref{constraints_sec} section for
* more info on enabling/disabling constraints.
*
* @param type the type of constraint to include in the returned list
* @return the list of enabled constraints
*/
std::vector<kc::ks_ptr<BilateralConstraintBase>>
enabledConstraints(BilateralConstraintType type = BilateralConstraintType::ALL) const;
/**
* @brief Enable a constraint for the SubGraph
*
* If not already, add this loop constraint to the enabled list
* for use in constraint kinematics, and TA dynamics etc
* algorithms. See \sref{constraints_sec} section for more info on
* enabling/disabling loop constraints.
*
* @param constraint the new constraint
*/
void enableConstraint(const kc::ks_ptr<BilateralConstraintBase> constraint);
/**
* @brief Disable a constraint for the SubGraph
*
* Remove this constraint from the enabled constraints
* list for this SubGraph. See \sref{constraints_sec} section for
* more info on enabling/disabling constraints.
*
* @param constraint the constraint
*/
void disableConstraint(kc::ks_ptr<BilateralConstraintBase> constraint);
/**
* @brief Return the constraint kinematics solver for the registered constraints
*
* See the \sref{constraint_kinematics_sec} section for more on the
* ConstraintKinematicsSolver class.
*
* @return the ConstraintKinematicsSolver instance
*/
const kc::ks_ptr<ConstraintKinematicsSolver> &cks();
/**
* @brief Returns coord obj and its coord offset corresponding to overall U offset
*
* A SubGraph has multiple CoordData has get/set methods to pack
* and unpack the coordinate values for its set of CoordBase
* instances. This method returns the CoordBase and its local
* coordinate offset for the specified overall U offset
* value. This method returns the CoordBase and its local offset
* for the specified overall column offset value in the matrix
* returned by the Algorithms::evalVelocityConstraintMatrix()
* method, or the vector returned by the SubGraph::getU()
* method.
*
*
* @param u_offset the input overall U offset value
* @return The CoordBase instance and its local U coord offset
*/
const std::pair<kc::ks_ptr<CoordBase>, size_t> coordAt(size_t u_offset) const override;
/**
* @brief Return the constraint obj and its local offset in the overall constraint error
* vector
*
* The constraint errors are organized as 6 elements for each
* cutjoint-loop constraint, followed by scalar entries for the
* non-cutjoint loop constraints, and then scalars for each of the
* coordinate constraints. Given an index value, this method
* returns the corresponding constraint and its local offset in
* this vector.
*
* Not that this is also the organization of the rows in the
* matrices returned by the Algorithms::evalVelocityConstraintMatrix(True)
* method and the vector returned by the SubGraph::velError() method.
*
* The related constraintResidualAt() method returns the constraint
* based on the packing order for constraint errors.
*
* @param row_offset the overall row offset value @return The
* @return corresponding constraint instance and its local offset
*/
const std::pair<kc::ks_ptr<kc::LockingBase>, size_t>
constraintErrorAt(size_t row_offset) const;
/**
* @brief Return the constraint obj and its local offset in the overall constraint residuals
* vector
*
* The constraint residuals are organized as the residual
* elements for each cutjoint-loop constraint
* (6-constraint.hinge().nU() size), followed by scalar entries
* for the convel loop constraints, and then scalars for each
* of the coordinate constraints. Given an index value, this
* method returns the corresponding constraint and its local
* offset in this vector.
*
* This method returns the constraint and its local offset for
* the specified overall row offset value in the matrix returned
* by the Algorithms::evalVelocityConstraintMatrix(False)
* method.
*
* The related constraintErrorAt() method returns the constraint
* based on the packing order for constraint errors.
*
* @param row_offset the input overall row offset value
* @return corresponding constraint instance and its local offset
*/
const std::pair<kc::ks_ptr<kc::LockingBase>, size_t>
constraintResidualAt(size_t row_offset) const;
/**
* @brief Compute the Gc overall constraint matrix for the loop constraints (WIP)
*
* The Gc constraint matrix defines the overall velocity level
* constraints for the generalized velocity coordinates based on
* the loop closure constraints. Thus Gc*U=0 when the velocity
* constraints are satisfied. Gc itself is Q * Jacobian.
*
* The column space of Gc^* defines the squeeze forces, i.e
* (Gc^* x Y) generalized forces cause no motion for arbitrary
* Y.
*
* @return The velocity constraint Gc matrix
*/
km::Mat constraintsGc();
/**
* @brief Compute the OSCM related Gamma matrix for the constraint nodes
*
* The Gamma matrix is used by the TA constraint dynamics
* algorithm to compute the Lagrange multipliers for the TA
* forward dynamics correction step.
*
* @return The Gamma matrix
*/
km::Mat Gamma(); // NOLINT(readability-identifier-naming)
/**
* @brief Return the subhinge+body+constraints CoordData for the SubGraph
*
* This CoordData is a combination of the subhinge, body and
* constraint CoordDatas
*
* See \sref{coord_data_sec} for more on the CoordData class.
* @return the CoordData for the SubGraph
*/
const kc::ks_ptr<CoordData> graphCoordData() const;
const kc::ks_ptr<CoordData> treeCoordData() const override;
const kc::ks_ptr<CoordData> articulationCoordData() const override;
/**
* @brief Sequentially articulate a regular or loop constraint
* subhinge for a physical body.
*
* With graphics on, this step can be used to examine and debug
* issues with the model. If there are enabled loop constraints,
* this method will invoke the constraint kinematics solver each
* step to enforce the loop constraints.
*
* @param shg subhinge to articulate
* @param subhinge_index the index of the coordinate element to articulate
* @param disable_ik if true, constraints solver is not invoked during articulation
* @param range_q is the excursion angle range (in radians)
* @param d_q is the angle step size (in radians)
* @param pause time in seconds to sleep between articulation steps
*/
void articulateSubhinge(kc::ks_ptr<SubhingeBase> shg,
size_t subhinge_index,
bool disable_ik = false,
double range_q = .5,
double d_q = .01,
double pause = .01);
void dumpTree(std::string_view prefix = "",
const DumpTreeOptions options =
DumpTreeOptions(true, false, false, false, false)) const override;
/**
* @brief Return the overall constraints pose error vector for the enabled
* constraints
* @return the pose error vector
*/
km::Vec poseError() const;
/**
* @brief Return the overall constraint spatial velocity error for the enabled constraints
* @return the spatial velocity error
*/
km::Vec velError() const;
/**
* @brief Return the overall constraint spatial acceleration error for the enabled loop
constraints
*
* @return the spatial acceleration error
*/
km::Vec accelError() const;
/** @brief Return the pose error gradient matrix for all the constraints
*
* Assemble the overall pose gradient matrix for all the
* constraint errors with respect to all SubGraph coords. The gradient
* matrix is thus (6*n_hlc+n_nhlc+n_cc, nU) in size, where n_hlc
* denotes the number of cutjoint loop constraints, n_nhlc the number
* of convel loop constraints, and n_cc the number of
* coordinate constraints. This is used by the solveQ() method of
* the CK solver.
*
* @return the constraint pose error gradient matrix
*/
km::Mat constraintsErrorPoseGradient() const;
/** @brief Return the velocity error Jacobian matrix for all the constraints
*
* Assemble the overall velocity Jacobian matrix for all the
* constraints with respect to all SubGraph coords. The Jacobian matrix is
* thus (6*n_hlc+n_nhlc+n_cc, nU) in size, where n_hlc denotes the
* number of cutjoint loop constraints, n_nhlc the number of
* convel loop constraints, and n_cc the number of coordinate
* constraints. This is used by the solveU() and solveUdot()
* methods of the CK solver.
*
* @return the constraint velocity error Jacobian matrix
*/
km::Mat constraintsErrorVelJacobian() const;
/** @brief Return the time derivative of the velocity error Jacobian matrix for all the
* constraints
*
* The Jacobian matrix time derivative is
* thus (6*n_hlc+n_nhlc+n_cc, nU) in size, where n_hlc denotes the
* number of cutjoint loop constraints, n_nhlc the number of
* convel loop constraints, and n_cc the number of coordinate
* constraints. This is used for constraint embedding.
*
* @return the constraint velocity error Jacobian matrix
*/
km::Mat constraintsErrorVelJacobianDot() const;
/**
* @brief Return the size of the residuals vector for loop constraints
*
* This returns the combined length of the residuals for the
* hinge and convel loop constraints. See
* \sref{coordinate_constraints_sec} section for more info on
* enabling/disabling coordinate constraints.
*
* @return the size of the residuals vector
*/
size_t nLoopConstraintResiduals() const;
void enableSubhingeCharts(bool flag) override;
/**
* @brief Helper method to return the complimentary list of indices in a
specified range for the specified list of indices. This method is
used to get the list of dependent indices for a list of
independent indices.
* @param n_u The number of velocity coordinates.
* @param indep_indices The independent indices.
* @returns list of complementary indices
*/
static std::vector<unsigned int>
complementaryIndices(size_t n_u, const std::vector<unsigned int> &indep_indices);
protected:
std::vector<kc::ks_ptr<CoordData>> _coordDataList() const override;
#if 0
/**
* @brief Return the list of enabled cutjoint-loop constraints for the SubGraph
*
* This returns the list of cutjoint-loop constraints that are currently
* "active" for this SubGraph. See \sref{constraints_sec} section for
* more info on enabling/disabling constraints.
*
* @return the list of enabled cutjoint loop-constraints
*/
const std::vector<kc::ks_ptr<LoopConstraintCutJoint>> &
enabledCutJointLoopConstraints() const;
/**
* @brief Return the list of enabled convel loop constraints for the SubGraph
*
* This returns the list of convel-loop constraints that are currently
* "active" for this SubGraph. See \sref{constraints_sec} section for
* more info on enabling/disabling constraints.
*
* @return the list of enabled convel loop-constraints
*/
const std::vector<kc::ks_ptr<LoopConstraintConVel>> &enabledConVelLoopConstraints() const;
/**
* @brief Return the list of enabled coordinate constraints for the SubGraph
*
* This returns the list of coordinate constraints that are currently
* "active" for this SubGraph. See \sref{coordinate_constraints_sec} section for
* more info on enabling/disabling coordinate constraints.
*
* @return the list of enabled coordinate constraints
*/
const std::vector<kc::ks_ptr<CoordinateConstraint>> &enabledCoordinateConstraints() const;
#endif
/**
* @brief Compute the Q constraints matrix for the loop constraints
*
* The Q matrix defines the overall velocity level constraints
* for the loop closure constraints. Its product with the
* stacked relative spatial velocities across the loop
* constraints (followed by U values for coordinate constraints)
* is zero when the velocity constraints are satisfied.
*
* The size of the matrix is (n_residuals, 6 *
* n_nodes + 2 * n_cc) where n_residuals is the overall size of
* the residuals across all the bilateral constraints, n_nodes
* is the number of loop constraint nodes, and n_cc is the
* number of coordinate constraints.
*
* It is used by the TA constraint dynamics algorithm to compute
* Gamma needed to compute the Lagrange multipliers for the TA
* forward dynamics correction step. It is also used to compute
* the velocity residuals based Gc velocity constraint matrix.
*
* It is also used to define the "squeeze" matrix for the
* SubGraph.
*
* @return The constraints Q matrix
*/
km::Mat _constraintsQmat() const;
/**
* @brief Return the multi-Jacobian generator for the inertial relative constraint nodes
*
* The computed Jacobian are to all the constraint nodes with respect to
* inertial for all the tree coordinates. The matrices are thus
* (6*n_nodes + 2*n_cc, tree_nU) in size, where n_nodes denotes
* the number of constraint nodes, n_cc the number of coordinate
* constraints, and tree_nU the number of subhinge+body
* coordinates. This is used to compute the cutjoint loop
* constraint row contributions in the velocity residuals based
* Gc matrix.
*
* @return the multi-jacobian generator
*/
kc::ks_ptr<MultiJacobianGenerator> _constraintsNodesTreeJacobianGen() const;
/**
* @brief Retrieve a vector of ConstraintNodes used by the SubGraph.
*
* @returns A vector of ConstraintNodes used by the SubGraph.
*/
const std::vector<kc::ks_ptr<ConstraintNode>> &_constraintNodes() const;
protected:
/**
* @brief Look up an enabled coordinate constraint by name.
* See \sref{coordinate_constraints_sec} section for more info on loop constraints.
* @param name the coordinate constraint's name
* @return the CoordinateConstraint instance
*/
kc::ks_ptr<CoordinateConstraint>
_getEnabledCoordinateConstraint(std::string_view name) const;
/**
* @brief Look up an enabled cu-joint constraint by name.
* See \sref{constraints_sec} section for more info on loop constraints.
* @param name the cut-joint constraint's name
* @return the LoopConstraintCutJoint instance
*/
kc::ks_ptr<LoopConstraintCutJoint>
_getEnabledCutJointConstraint(std::string_view name) const;
/**
* @brief Look up an enabled convel loop constraint by name.
* See \sref{constraints_sec} section for more info on loop constraints.
* @param name the loop constraint's name
* @return the LoopConstraintConVel instance
*/
kc::ks_ptr<LoopConstraintConVel> _getEnabledConVelConstraint(std::string_view name) const;
/**
* @brief Return the multi-Jacobian generator for the cutjoint loop constraint error f2fs
*
* The computed Jacobian and pose gradient matrices are to all
* the cutjoint loop constraint error f2fs from all the coordinates
* in the SubGraph. The matrices are thus (6*n_hlc, nU) in size,
* where n_hlc denotes the number of cutjoint-loop
* constraints. This is used to compute the cutjoint loop
* constraint row contributions in the overall Jacobian and pose
* gradient for the constraint errors with respect to the SubGraph
* coordinates.
*
* @return the multi-jacobian generator
*/
kc::ks_ptr<MultiJacobianGenerator> _cutjointConstraintsErrorJacobianGen() const;
/**
* @brief Return the multi-Jacobian generator for the convel loop constraint f2fs
*
* The computed Jacobian and pose gradient matrices are to all
* the convel loop constraint f2fs from all the coordinates
* in the SubGraph. The matrices are thus (6*n_nhlc, nU) in
* size, where n_nhlc denotes the number of convel-loop
* constraints. This is used to compute the convel loop
* constraint row contributions in the overall Jacobian for the
* constraint velocity errors with respect to the SubGraph coordinates.
*
* @return the multi-jacobian generator
*/
kc::ks_ptr<MultiJacobianGenerator> _convelConstraintsJacobianGen() const;
/** @brief Return the Jacobian for the scalar error for each of the
coordinate constraints with respect to all the SubGraph coordinates.
The Jacobian matrix is (n_cc, nU) in size, where n_cc
denotes the number of coordinate constraints. This is used
to compute the cutjoint loop constraint row contributions
for the overall Jacobian and pose gradient for the
constraint errors with respect to the SubGraph
coordinates.
@return the Jacobian matrix
*/
km::Mat _getCoordinateConstraintsErrorJacobian() const;
/** @brief Return the Jacobian to the scalar error for each of the
convel constraints with respect to all the SubGraph
coords.
The Jacobian matrix is (n_nhlc, nU) in size, where
n_nhlc denotes the number of convel loop
constraints. This is used to compute the convel loop
constraint row contributions to the overall Jacobian for the
constraint velocity errors with respect to the SubGraph
coordinates.
@return the Jacobian matrix
*/
km::Mat _getConVelLoopConstraintsErrorJacobian() const;
/** @brief Return the time derivative of the Jacobian for the
scalar error for each of the convel constraints with respect
to all the SubGraph coords.
The Jacobian matrix is (n_nhlc, nU) in size, where
n_nhlc denotes the number of convel loop
constraints.
@return the Jacobian time derivative matrix
*/
km::Mat _getConVelLoopConstraintsErrorJacobianDot() const;
protected:
/**
* @brief Add all loop and coordinate constraints from the
* parent SubGraph that are legal for this SubGraph.
*
* This method is a helper method to populate a new SubGraph
* with all the constraints from the parent SubGraph.
*/
void _inheritConstraints();
/**
* @brief Return the CoordOffset struct with offsets for the CoordBase with specified id in
* the subgraph's graphCoordData
*
* @param id the CoordBase id
* @return the CoordOffset struct
*/
CoordData::CoordOffset _coordOffsets(kc::id_t id) const override;
void _resetSubhingeCharts() override;
void _clearExternals() override;
/**
* @brief Discard the provided SubGraph.
*
* @param base - Base pointer to the SubGraph to discard.
*/
void _discard(kc::ks_ptr<Base> &base) override;
/**
* @brief Helper method to create the constraint CoordData member for the subgraph
*/
void _makeConstrainCoordData() const;
/**
* @brief Enable a loop constraint for the SubGraph
*
* If not already, add this loop constraint to the enabled list
* for use in constraint kinematics, and TA dynamics etc
* algorithms. See \sref{constraints_sec} section for more info on
* enabling/disabling loop constraints.
*
* @param constraint the new loop constraint
*/
void _enableLoopConstraint(const kc::ks_ptr<LoopConstraintBase> constraint);
/**
* @brief Disable a loop constraint for the SubGraph
*
* Remove this loop constraint from the enabled loop constraints
* list for this SubGraph. See \sref{constraints_sec} section for
* more info on enabling/disabling loop constraints.
*
* @param constraint the loop constraint
*/
void _disableLoopConstraint(kc::ks_ptr<LoopConstraintBase> constraint);
/**
* @brief Enable a coordinate constraint for the SubGraph
*
* If not already, add this coordinate constraint to the enabled list
* for use in constraint kinematics, and TA dynamics etc
* algorithms. See \sref{coordinate_constraints_sec} section for more info on
* enabling/disabling coordinate constraints.
*
* @param constraint the new coordinate constraint
*/
void _enableCoordinateConstraint(const kc::ks_ptr<CoordinateConstraint> constraint);
/**
* @brief Disable a coordinate constraint for the SubGraph
*
* Remove this coordinate constraint from the enabled coordinate constraints
* list for this SubGraph. See \sref{coordinate_constraints_sec} section for
* more info on enabling/disabling coordinate constraints.
*
* @param constraint the coordinate constraint
*/
void _disableCoordinateConstraint(kc::ks_ptr<CoordinateConstraint> constraint);
protected:
/** Hinge based loop constraints enabled for the SubGraph */
kc::RegistryList<LoopConstraintCutJoint> _enabled_cutjoint_loop_constraints_list;
/** convel loop constraints enabled for the SubGraph */
kc::RegistryList<LoopConstraintConVel> _enabled_convel_loop_constraints_list;
/** Compound constraints enabled for the SubGraph */
kc::RegistryList<CoordinateConstraint> _enabled_coordinate_constraints_list;
/// Boolean that indicates whether there are any enabled constraints or not.
bool _has_enabled_constraints = false;
/** the number of rows in the Qmat constraint matrix for loop constraints */
size_t _rowsQ_loop = 0;
/** the number of rows in the Qmat constraint matrix for coordinate constraints */
size_t _rowsQ_coord = 0;
/** The subset of the loop constraint source/target frames that
are loop constraint nodes. Using the registry list will also
prevent the same constraint node from being used multiple
times by different loop constraints. */
kc::RegistryList<ConstraintNode> _loop_constraint_nodes_list;
/** The list of subhinges associated with coordinate
constraints. Using the registry list will also prevent the
same subhinge from being used multiple times by different
coordinate constraints. */
kc::RegistryList<PhysicalSubhinge> _coordinate_constraint_subhinges_list;
/// CoordData instance to track the coordinates across the
/// loop constraint subhinges in this SubGraph
mutable kc::ks_ptr<CoordData> _constraint_coord_data = nullptr;
/** the ConstraintKinematicsSolver instance for the subgraph */
kc::ks_ptr<ConstraintKinematicsSolver> _cks = nullptr;
/// merged CoordData instance with the hinge subhinge, body
/// coordinates and cutjoint loop constraint subhinges. Compared with
/// the _tree_coord_data, this contains the additional loop
/// constraint subhinge coordinates
mutable kc::ks_ptr<CoordData> _graph_coord_data = nullptr;
/** the multi-Jacobian generator for the cut-joint constraint frame to frames */
mutable kc::ks_ptr<MultiJacobianGenerator> _cutjoint_loops_multi_jacgen = nullptr;
/** MultiJacobianGenerator for non-hinge-based loop constraints
relative transform */
mutable kc::ks_ptr<MultiJacobianGenerator> _non_cutjoint_loops_multi_jacgen = nullptr;
/** MultiJacobianGenerator for constraint nodes with respect to inertial for the tree coords
* only */
mutable kc::ks_ptr<MultiJacobianGenerator> _constraint_nodes_tree_multi_jacgen;
/** if true, the Baumgarte error control term will be used in TA
forward dynamics */
bool _baumgarte_stabilization_enabled = false;
/** stiffness coefficient (beta^2) for the Baumgarte error control term in
TA forward dynamics */
double _baumgarte_stiffness = km::notReadyNaN;
/** damping coefficient (2*alpha) for the Baumgarte error control
term in TA forward dynamics */
double _baumgarte_damping = km::notReadyNaN;
};
} // namespace Karana::Dynamics