Program Listing for File ConstraintKinematicsSolver.h#
↰ Return to documentation for file (include/Karana/SOADyn/ConstraintKinematicsSolver.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 ConstraintKinematicsSolver class.
*/
#pragma once
#include <string>
#include <vector>
#include "JacobianGenerator.h"
#include "Karana/KCore/LockingBase.h"
#include "Karana/Math/NonlinearSolver.h"
#include "Karana/SOADyn/Defs.h"
namespace Karana::Dynamics {
namespace kc = Karana::Core;
namespace km = Karana::Math;
template <int, int, int> class CoordBase_T;
class CoordData;
class LoopConstraint;
class SubGraph;
/**
* @class ConstraintKinematicsSolver
* @brief Class for solving for coordinates that satisfy closure constraints
*
* This class is the base class for bodies from which physical and
* compound body classes are derived. See \sref{constraint_kinematics_sec} section for more
* information.
*/
class ConstraintKinematicsSolver : public kc::LockingBase {
/* for access to _velJacobian() */
friend class Algorithms;
/* for access to cutjoint jacobian generator */
friend class CECompoundBody;
public:
/**
* @brief Factory method to create a ConstraintKinematicsSolver instance
*
* Creates constraint kinematics solver for the specified
* SubGraph's coordinates and constraints. Frozen_coordinates can
* be set in the SubGraph's CoordData to specify ones that should
* be frozen and not changed by the kinematics solver.
*
* @param name instance name
* @param sg the SubGraph to use for coordinates and constraints
* @return new ConstraintKinematicsSolver instance.
*/
static kc::ks_ptr<ConstraintKinematicsSolver> create(std::string_view name,
kc::ks_ptr<SubGraph> sg);
/**
* @brief Constructor.
*
* @param name instance name
* @param sg the SubGraph to use for coordinates and constraints
*/
ConstraintKinematicsSolver(std::string_view name, kc::ks_ptr<SubGraph> sg);
/**
* @brief Destructor.
*/
~ConstraintKinematicsSolver();
//----------------------------
/**
* @brief Solve for the Q generalized coordinates to satisfy constraints
*
* See \sref{coords_sec} section for discussion on Q, U etc coordinates.
*
* @return The final error.
*/
double solveQ();
/**
* @brief Solve for the U velocity coordinates to satisfy velocity level constraints
*
* See \sref{coords_sec} section for discussion on Q, U etc coordinates.
*
* @return The final error.
*/
double solveU();
/**
* @brief Solve for the Udot accel coordinates to satisfy accel level constraints
*
* Solve for the Udot gen accel coordinates to zero out
* constraints acceleration error. Unlike, solveQ() and
* solveU(), this method does change the frozen gen accel
* values.
*
* See \sref{coords_sec} section for discussion on Q, U etc coordinates.
*
* @return The final error.
*/
double solveUdot();
/**
* @brief Method to freeze a coordinate component
*
* The frozen coordinate element values are not changed by the solveQ(),
* solveU() and solveUdot() methods used for solving the
* constraint kinematics.
*
* @param coord The coordinate whose element is being frozen
* @param index The index for the coordinate element being frozen
* @param type The coordinate type to freeze
*/
void freezeCoord(kc::ks_ptr<CoordBase> coord,
size_t index,
CKFrozenCoordType type = CKFrozenCoordType::Q_U);
/**
* @brief Method to unfreeze a coordinate component
*
* @param coord The coordinate whose element is being unfrozen
* @param index The index for the coordinate element being unfrozen
* @param type The coordinate type to unfreeze
*/
void unfreezeCoord(kc::ks_ptr<CoordBase> coord,
size_t index,
CKFrozenCoordType type = CKFrozenCoordType::Q_U);
/**
* @brief Method to empty out list of Q and U frozen coordinates
*
*/
void clearFrozenCoords();
/// alias for list of coordinate indices
using CoordIndices = std::vector<unsigned int>;
/**
* @brief Return the list of frozen coordinate indices.
*
* @param Q_type If true, the Q frozen coord indices are returned, else the U ones
* @return the list of frozen coordinate indices.
*/
CoordIndices frozenCoords(bool Q_type) const; // NOLINT(readability-identifier-naming)
/**
* @brief Return the non-frozen Q coordinates as an array
*
* See \sref{coords_sec} section for more on Q, U etc generalized coordinates.
*
* @return Array of values
*/
km::Vec getNonfrozenQ() const;
/**
* @brief Set the non-frozen Q coordinates.
*
* See \sref{coords_sec} section for more on Q, U etc generalized coordinates.
*
* @param Q Array of values.
*/
void setNonfrozenQ(const Eigen::Ref<const km::Vec> &Q);
/**
* @brief Return the non-frozen U velocity coordinates as an array
*
*
* See \sref{coords_sec} section for more on Q, U etc generalized coordinates.
*
* @return Array of values
*/
km::Vec getNonfrozenU() const;
/**
* @brief Set the non-frozen U velocity coordinates.
*
* See \sref{coords_sec} section for more on Q, U etc generalized coordinates.
*
* @param U Array of values.
*/
void setNonfrozenU(const Eigen::Ref<const km::Vec> &U);
/**
* @brief Return the non-frozen Udot acceleration coordinates as an array
*
* See \sref{coords_sec} section for more on Q, U etc generalized coordinates.
*
* @return Array of values
*/
km::Vec getNonfrozenUdot() const;
/**
* @brief Set the non-frozen Udot acceleration coordinates.
*
* See \sref{coords_sec} section for more on Q, U etc generalized coordinates.
*
* @param u_dot Array of values.
*/
void setNonfrozenUdot(const Eigen::Ref<const km::Vec> &u_dot);
std::string
dumpString(std::string_view prefix = "",
const Karana::Core::Base::DumpOptions *options = nullptr) const override;
/**
* @brief Verify that the velocity Jacobian values from the analytical and numerical
* differencing approaches agree.
*
* @param tolerance the comparison tolerance to use
* @return return true if the Jacobians agree
*/
bool checkVelJacobian(double tolerance = 1e-6) const;
protected:
/** @brief Helper method to handle internal frozen coords data for new frozen coordinate
@param frozen_coords list of indices of frozen coordinates
@param new_frozen index of newly frozen coordinate
@param total_n total_n the overall number of coordinate indices
@return the list of nonfrozen indices
*/
CoordIndices
_updateFrozenCoords(CoordIndices &frozen_coords, size_t new_frozen, size_t total_n);
/** @brief Helper method to handle internal frozen coords data for new unfrozen coordinate
@param frozen_coords list of indices of frozen coordinates
@param new_unfrozen index of newly unfrozen coordinate
@param total_n total_n the overall number of coordinate indices
@return the list of nonfrozen indices
*/
CoordIndices
_updateNonFrozenCoords(CoordIndices &frozen_coords, size_t new_unfrozen, size_t total_n);
/** @brief Return the nonlinear solver instance configured to solver the
constraint kinematics problem
@return the nonlinear solver instance
*/
kc::ks_ptr<km::NonlinearSolver> _nlSolver() { return _nl_solver; }
/**
* @brief Discard the provided ConstraintKinematicsSolver.
* @param base - Base pointer to the ConstraintKinematicsSolver to discard.
*/
void _discard(kc::ks_ptr<Base> &base) override;
/**
* @brief Helper method to create the nonlinear solver instance
*/
void _makeHealthy() override;
/** @brief Method to tear down and recreate cached data including the
Jacobian generator and the nonlinear solver.
This method is called by _makeHealthy() when the constraints
or coord base elements change.
*/
void _setup();
/**
* @brief Helper method to create the nonlinear solver member.
*/
void _makeNlSolver();
/**
* @brief Update the list of dependent Q coordinates to use by the non-linear solver.
*
* Update the list of indices of Q coords that should be used by
* the non-linear solver to satisfy the coord constraints. This
* method picks the complement of the best independent Q coord
* indices. It also skips the frozen coords since they are not
* supposed to change. This method can be called to update this
* list of dependent coordinates since the choice is
* configuration state dependent.
*/
void _updateDependentCoordsQ();
/** @brief Assemble the overall velocity Jacobian matrix for all the
constraints using numerical approach.
All subhinge columns are included.
@return the Jacobian matrix
*/
km::Mat _velJacobianNumDiff() const;
/** @brief Recompute _nonfrozen_indices from _frozen_indices
@param frozen_coords list of frozen coordinate indices
@param n the overall number for coordinate indices
@return the list of non-frozen coordinate indices
*/
CoordIndices _recomputeNonFrozenCoord(const CoordIndices &frozen_coords, size_t n) const;
protected:
/** the SubGraph for this CK solver */
kc::ks_ptr<SubGraph> _sg;
/** MultiJacobianGenerator for hinge-based loop constraints */
kc::ks_ptr<MultiJacobianGenerator> _cutjoint_loops_multi_jacgen = nullptr;
/** MultiJacobianGenerator for non-hinge-based loop constraints */
kc::ks_ptr<MultiJacobianGenerator> _non_cutjoint_loops_multi_jacgen = nullptr;
/** the nonlinear solver used for inverse kinematics */
kc::ks_ptr<km::NonlinearSolver> _nl_solver = nullptr;
/** list of indices of dependent coordinate that are allowed to
change when solving for Q values when enforcing the configuration
level constraints. */
CoordIndices _dependent_Q_coord_indices;
/** list of indices of Q coordinates that are frozen by the
application by calls to freezeCoords(). These are used to
compute _nonfrozen_indices that used by solveQ(). */
CoordIndices _frozen_indices_Q;
/** list of indices of U coordinates that are frozen by the
application by calls to freezeCoords(). These are used to
compute _nonfrozen_indices that used by solveU() and
solveUdot() methods. */
CoordIndices _frozen_indices_U;
/** list of indices of Q coordinates that are not frozen by the
application. These are used by the solveQ() method. */
CoordIndices _nonfrozen_indices_Q;
/** list of indices of U coordinates that are not frozen by the
application. These are used by the solveU() and solveUdot()
methods. */
CoordIndices _nonfrozen_indices_U;
};
} // namespace Karana::Dynamics