Program Listing for File CELoopKinematics.h#
↰ Return to documentation for file (include/Karana/SOADyn/CELoopKinematics.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 declarations for the constraint embedding
* inverse kinematics solvers.
*/
#pragma once
#include "ConstraintKinematicsSolver.h"
#include "Karana/Frame/PrescribedFrameToFrame.h"
#include "Karana/KCore/LockingBase.h"
#include "Karana/Math/Defs.h"
// #include "Karana/SOADyn/SubGraph.h"
namespace Karana::Frame {
class PrescribedFrameToFrame;
}
namespace Karana::Dynamics {
namespace kc = Karana::Core;
namespace kf = Karana::Frame;
namespace ks = Karana::Scene;
class SubGraph;
class CELoopKinematicsBase;
class CELoopKinematicsNumeric;
class ConstraintKinematicsSolver;
class PhysicalSubhinge;
/**
* @class CELoopKinematicsBaseVars
* @brief The Vars for the CELoopKinematicsBase class.
*/
class CELoopKinematicsBaseVars : public kc::BaseVars {
public:
/**
* @brief CELoopKinematicsBaseVars constructor. The constructor is not meant to be
* called directly. Please use the create(...) method instead to create
* an instance.
*
* @param solver The solver associated with this CELoopKinematicsBaseVars.
*/
CELoopKinematicsBaseVars(const kc::ks_ptr<const CELoopKinematicsBase> &solver);
/**
* @brief Destructor.
*/
~CELoopKinematicsBaseVars();
/**
* @brief Create an instance of the CELoopKinematicsBaseVars.
*
* @param solver The solver associated with this CELoopKinematicsBaseVars.
* @returns A pointer to the newly created instance of CELoopKinematicsBaseVars.
*/
static kc::ks_ptr<CELoopKinematicsBaseVars>
create(const kc::ks_ptr<const CELoopKinematicsBase> &solver);
/**
* @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;
/// independent Q coord indices
kc::ks_ptr<Karana::Core::Var<km::VecInt>>
indep_indices_Q; // NOLINT(readability-identifier-naming)
/// independent U coord indices
kc::ks_ptr<Karana::Core::Var<km::VecInt>>
indep_indices_U; // NOLINT(readability-identifier-naming)
/// dependent U coord indices
kc::ks_ptr<Karana::Core::Var<km::VecInt>>
dep_indices_U; // NOLINT(readability-identifier-naming)
/// inert U coord indices
kc::ks_ptr<Karana::Core::Var<km::VecInt>>
inert_indices_U; // NOLINT(readability-identifier-naming)
};
/**
* @class CELoopKinematicsBase
* @brief Base class for CE loop kinematics solvers
* */
class CELoopKinematicsBase : public Karana::Core::BaseWithVars {
friend class CECompoundSubhinge;
friend class CECompoundBody;
// for vars
friend class CELoopKinematicsBaseVars;
public:
/**
* @brief Constructor
*
* @param name The name of the CELoopKinematicsBase.
* @param bodies_sg The SubGraph that the CELoopKinematicsBase uses.
*/
CELoopKinematicsBase(std::string_view name, kc::ks_ptr<SubGraph> bodies_sg);
/// deleted copy constructor
CELoopKinematicsBase(const CELoopKinematicsBase &) = delete;
/// deleted copy assignment operator
CELoopKinematicsBase &operator=(const CELoopKinematicsBase &) = delete;
/**
* Destructor
*/
virtual ~CELoopKinematicsBase(){};
/**
* @brief 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 to indicate whether incoming coords are local or global.
* @return true if the dependent Q coordinates were successfully updated
*/
virtual bool updateDependentQ(const km::Vec &min_q, bool global) const = 0;
// virtual km::Mat X() const = 0;
/**
* @brief Fill the X matrix that maps the min independent U to the
* full U (independent, dependent, constraint dofs) for the
* compound body
*
* @param X The matrix to fill with the X matrix.
*/
virtual void computeConstraintX(km::Mat &X) const = 0;
/**
* @brief Return Xdot*U product of the time derivative
of X constraint matrix with the U generalized velocities
* @param XdotU the vector to fill with the compute value
*/
virtual void
computeXdotU(km::Vec &XdotU) const = 0; // NOLINT(readability-identifier-naming) TODO
/**
* @brief Get the independent U coordinates
*
* @return the independent U coordinate
*/
const std::vector<Coordinate> &getIndependentCoordsU() const;
/**
* @brief Get the independent Q coordinates
*
* @return the independent Q coordinate
*/
const std::vector<Coordinate> &getIndependentCoordsQ() const;
std::string
dumpString(std::string_view prefix = "",
const Karana::Core::Base::DumpOptions *options = nullptr) const override;
protected:
Karana::Core::ks_ptr<Karana::Core::BaseVars> _getVars() const override;
/**
* @brief Return the rank threshold used to determine constraint matrix rank
* @return the rank threshold
*/
virtual double _getRankThreshold() const;
/**
* @brief Return the indices for th list of coordinates.
* @param cbs the list of coordinates
* @param for_U if true, the lists for U coordinates, else for Q coordinates
* @return the Q coordinate indices
*/
km::VecInt _toCoordIndices(const std::vector<Coordinate> &cbs, bool for_U) const;
/**
* indices of all independent U coords among the embedded coordinates
*/
km::VecInt _indep_indices_U;
/**
* indices of all independent Q coords among the embedded coordinates
*/
km::VecInt _indep_indices_Q;
/**
* indices of all inert coords among the _bodies_tree coordinates
(i.e. indices that have no effect on the constraint errors
such as for planar systems) */
km::VecInt _inert_indices_U;
/**
* indices of all dependent coords among the _bodies_sg coordinates
*/
km::VecInt _dep_indices_U;
/**
* the aggregation subgraph for the compound body
*/
kc::ks_ptr<SubGraph> _bodies_sg;
/**
* List contains the physical subhinge+offset value for each of the
independent U indices */
std::vector<Coordinate> _indep_cbs_U;
/**
* List contains the physical subhinge+offset value for each of the
independent Q indices */
std::vector<Coordinate> _indep_cbs_Q;
/* List contains the
independent U indices */
// km::VecInt _indep_indices_U;
/**
* List contains the
independent Q indices */
// km::VecInt _indep_indices_Q;
};
/**
* @class CELoopKinematicsNumericVars
* @brief The Vars for the CELoopKinematicsNumeric class.
*/
class CELoopKinematicsNumericVars : public CELoopKinematicsBaseVars {
public:
/**
* @brief CELoopKinematicsNumericVars constructor. The constructor is not meant to be
* called directly. Please use the create(...) method instead to create
* an instance.
*
* @param solver The solver associated with this CELoopKinematicsNumericVars.
*/
CELoopKinematicsNumericVars(const kc::ks_ptr<const CELoopKinematicsNumeric> &solver);
/**
* @brief Destructor.
*/
~CELoopKinematicsNumericVars();
/**
* @brief Create an instance of the CELoopKinematicsNumericVars.
*
* @param solver The solver associated with this CELoopKinematicsNumericVars.
* @returns A pointer to the newly created instance of CELoopKinematicsNumericVars.
*/
static kc::ks_ptr<CELoopKinematicsNumericVars>
create(const kc::ks_ptr<const CELoopKinematicsNumeric> &solver);
/**
* @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;
/// independent rows of the Y matrix
kc::ks_ptr<Karana::Core::Var<km::VecInt>> rows_Y; // NOLINT(readability-identifier-naming)
/**
* threshold tolerance to use when computing Jacobian matrix rank
*/
kc::ks_ptr<Karana::Core::Var<double>> rank_threshold;
};
/**
* @class CELoopKinematicsNumeric
* @brief The numerical iterations based CE kinematics loop solver
* */
class CELoopKinematicsNumeric : public CELoopKinematicsBase {
// for vars
friend class CELoopKinematicsNumericVars;
public:
/**
* @brief CELoopKinematicsNumeric constructor. The constructor is not meant
* to be called directly. Please use the create(...) method instead
* to create an instance.
*
* @param name Instance name.
* @param bodies_sg SubGraph with the embedded bodies.
* @param indep_cbs_U The list of independent U coordinates
* @param indep_cbs_Q The list of independent Q coordinates
* @param rank_threshold threshold to use when computing error Jacobian rank
*/
CELoopKinematicsNumeric(
std::string_view name,
kc::ks_ptr<SubGraph> bodies_sg,
const std::vector<Coordinate> &indep_cbs_U, // NOLINT(readability-identifier-naming)
const std::vector<Coordinate> &indep_cbs_Q, // NOLINT(readability-identifier-naming)
double rank_threshold = 1e-8);
/**
* @brief Constructs a CELoopKinematicsNumeric instance
*
* @param name Instance name.
* @param bodies_sg SubGraph with bodies being embedded.
* @param indep_cbs_U The list of independent U coordinates.
* @param indep_cbs_Q The list of independent Q coordinates.
* @param rank_threshold threshold to use when computing error Jacobian rank
* @returns A CELoopKinematicsNumeric instance.
*/
static Karana::Core::ks_ptr<CELoopKinematicsNumeric>
create(std::string_view name,
kc::ks_ptr<SubGraph> bodies_sg,
const std::vector<Coordinate>
&indep_cbs_U, // NOLINT(readability-identifier-naming) TODO
const std::vector<Coordinate>
&indep_cbs_Q, // NOLINT(readability-identifier-naming) TODO
double rank_threshold = 1e-8);
// km::Mat Xd() const override;
bool updateDependentQ(const km::Vec &min_q, bool global) const override;
void computeConstraintX(km::Mat &X) const override;
// km::Vec computeXdotU(const km::Mat &constraintX) const override;
void
computeXdotU(km::Vec &XdotU) const override; // NOLINT(readability-identifier-naming) TODO
/**
* @brief Set the independent Q and U coordinate indices.
*
* At least one of input lists is required to be
* non-empty. Empty lists are ignored. The indices consist of a
* subhinge and an offset in its coords list.
*
* @param indep_cbs_U the independent U coordinates
* @param indep_cbs_Q the independent Q coordinates
*/
void setIndependentCoords(
const std::vector<Coordinate> &indep_cbs_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 std::vector<Coordinate>
&indep_cbs_Q = {} // NOLINT(readability-identifier-naming)
// clang-format on
); // override;
std::string
dumpString(std::string_view prefix = "",
const Karana::Core::Base::DumpOptions *options = nullptr) const override;
/**
* @brief Return the constraint kinematics solver instance
* @return the constraint kinematics solver
*/
const kc::ks_ptr<ConstraintKinematicsSolver> &cks() const;
protected:
Karana::Core::ks_ptr<Karana::Core::BaseVars> _getVars() const override;
/**
* @brief Helper method to update the independent Q and U coordinate indices.
*
*/
void _setIndependentIndices();
double _getRankThreshold() const override;
/**
* @brief Compute the constraint error row indices to use in X
* computation
*
* The inert columns is used to exclude columns that do not
* contribute to the constraint error (eg. out of plane coords
* for a planar system)
*
* @param inert_columns the columns to treat as inert
*/
void _updateConstraintRowIndices(const km::VecInt &inert_columns);
/**
* @brief Compute the coordinate indices that are inert,
i.e. ones that do hot contribute to the constraint errors
(eg. out of plane coords in a planar system) */
void _computeInertIndices();
/**
* @brief Discard the the CELoopKinematics.
*
* @param base - A pointer to the CELoopKinematics to discard.
*/
void _discard(Karana::Core::ks_ptr<Karana::Core::Base> &base) override;
/**
* @brief Update the CoordBase values for the coord indices.
*
*/
// void _updateCBs();
protected:
/**
* 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 */
km::VecInt _rows_Y;
/**
* threshold tolerance to use when computing Jacobian matrix rank
*/
double _rank_threshold;
};
/**
* @class CELoopKinematicsPlanar4Bar
* @brief The analytical kinematics solver for a planar 4-bar system
*
* This analytical solver for a planar 4-bar linkage requires that the
* subgraph's bodies be a serial chain of length 3. The cut-join loop
* constraint is between the third body and the first body's parent
* body. All the hinges involved should be REVOLUTE with parallel
* rotational axes. The independent coordinate is the first body's
* coordinate.
*/
class CELoopKinematicsPlanar4Bar : public CELoopKinematicsBase {
public:
/**
* Constructor
*/
/**
* @brief Constructor
*
* @param name instance name
* @param bodies_sg SubGraph with the embedded bodies
*/
CELoopKinematicsPlanar4Bar(std::string_view name, kc::ks_ptr<SubGraph> bodies_sg);
/**
* @brief Constructs a CELoopKinematicsPlanar4Bar instance
*
* @param name instance name
* @param bodies_sg SubGraph with bodies being embedded
* @return a CELoopKinematicsPlanar4Bar instance
*/
static Karana::Core::ks_ptr<CELoopKinematicsPlanar4Bar>
create(std::string_view name, kc::ks_ptr<SubGraph> bodies_sg);
bool updateDependentQ(const km::Vec &min_q, bool global) const override;
void computeConstraintX(km::Mat &X) const override;
void
computeXdotU(km::Vec &XdotU) const override; // NOLINT(readability-identifier-naming) TODO
std::string
dumpString(std::string_view prefix = "",
const Karana::Core::Base::DumpOptions *options = nullptr) const override;
/**
* @brief Return the planar frame for the 4-bar
*
* @return the planar frame
*/
kc::ks_ptr<kf::Frame> planarFrame() const { return _planar_frame; }
/**
* @brief Select the open of crossed solution mode
*
* @param mode the solution mode
*/
void setSolutionMode(bool mode);
protected:
/**
* enum for the open and crossed 4-bar configurations
*/
// enum class AssemblyMode { OPEN, CROSSED };
/**
* @brief Helper method to solver 4-bar configuration kinematics
*
* This method solves the 4-bar kinematics in the planar frame's XY plane
*
* @param theta the independent coordinate
* @param mode the open or crossed configuration solution being requested
* @param eps_geom tolerance used for checking solution Q solution existence
* @param status return true if the solution succeeded
*/
void _fourbarSolveQ(double theta,
bool &status,
bool mode = true, // AssemblyMode::OPEN,
double eps_geom = 1e-12) const;
/**
* @brief Helper method to solve for the X vector
*
* @param Q the full Q coordinates
* @param X the vector to populate with the computed values
* @param eps_det tolerance to check for singular configuration
*/
void _fourbarX(const km::Vec &Q, km::Vec &X, double eps_det = 1e-12) const;
/**
* @brief Helper method to compute Xdot*U
*
* @param Q the full Q coordinates
* @param U the full U velocity coordinates
* @param XdotU the vector to populate with the computed values
* @param eps_det tolerance to check for singular configuration
*/
void _fourbarXdotU(const km::Vec &Q,
const km::Vec &U,
km::Vec &XdotU, // NOLINT(readability-identifier-naming) TODO
double eps_det = 1e-12) const;
void _discard(Karana::Core::ks_ptr<Karana::Core::Base> &base) override;
protected:
// 4-bar link lengths
/// first link length
double _a;
/// second link length
double _b;
/// third link length
double _c;
/// fourth link length
double _d;
/// the common rotation axis for the fourbar
// km::Vec3 _axis;
/// the constant offset angle for the planar frame
double _planar_offset_angle;
/// planar frame. This frame is fixes to the anchor body. The
/// 4-bar moves in its XY plane. The Z axis is the hinge axes,
/// while the X axis is defined by the line joining the 2 body
/// fixed nodes for the ends of the 4-bar on the anchor body
kc::ks_ptr<kf::Frame> _planar_frame = nullptr;
/// cache the f2f from the planar frame to the B 4-bar vertex
kc::ks_ptr<kf::FrameToFrame> _planar2B_f2f = nullptr;
// FourBarKinematicsVA sol;
/// solution mode - true means open, else crossed
bool _solution_mode = false;
};
} // namespace Karana::Dynamics