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/KCore/LockingBase.h"
// #include "Karana/SOADyn/SubGraph.h"
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<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<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_T<km::VecInt>>
indep_indices_Q; // NOLINT(readability-identifier-naming)
/// independent U coord indices
kc::ks_ptr<Karana::Core::Var_T<km::VecInt>>
indep_indices_U; // NOLINT(readability-identifier-naming)
/// dependent U coord indices
kc::ks_ptr<Karana::Core::Var_T<km::VecInt>>
dep_indices_U; // NOLINT(readability-identifier-naming)
/// inert U coord indices
kc::ks_ptr<Karana::Core::Var_T<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);
/** 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.
*/
virtual void 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 Return the indices of the independent U coordinates.
* @return the independent U coordinate indices
*/
virtual const km::VecInt &getIndependentIndicesU() const;
/**
* @brief Return the indices of the independent Q coordinates.
* @return the independent Q coordinate indices
*/
virtual const km::VecInt &getIndependentIndicesQ() const;
std::string
dumpString(std::string_view prefix = "",
const Karana::Core::Base::DumpOptions *options = nullptr) const override;
const CELoopKinematicsBaseVars &getVars() const override;
protected:
/** 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<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;
};
/**
* @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<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<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_T<km::VecInt>> rows_Y; // NOLINT(readability-identifier-naming)
};
/**
* @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_indices_U The list of independent U coordinate indices.
* @param indep_indices_Q The list of independent Q coordinate indices.
*/
CELoopKinematicsNumeric(
std::string_view name,
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)
/**
* @brief Constructs a CELoopKinematicsNumeric instance
*
* @param name Instance name.
* @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.
* @returns A CELoopKinematicsNumeric instance.
*/
static Karana::Core::ks_ptr<CELoopKinematicsNumeric>
create(std::string_view name,
kc::ks_ptr<SubGraph> bodies_sg,
const km::VecInt &indep_indices_U, // NOLINT(readability-identifier-naming) TODO
const km::VecInt &indep_indices_Q // NOLINT(readability-identifier-naming) TODO
);
// km::Mat Xd() const override;
void 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.
*
* 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
); // override;
std::string
dumpString(std::string_view prefix = "",
const Karana::Core::Base::DumpOptions *options = nullptr) const override;
const CELoopKinematicsNumericVars &getVars() const override;
protected:
/**
* @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;
};
/**
* @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
* @returns a CELoopKinematicsPlanar4Bar instance
*/
static Karana::Core::ks_ptr<CELoopKinematicsPlanar4Bar>
create(std::string_view name, kc::ks_ptr<SubGraph> bodies_sg);
void 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
protected:
/** enum for the open and crossed 4-bar configurations */
enum class AssemblyMode { OPEN, CROSSED };
/** @brief Helper method to solver 4-bar configuration kinematics
*
* @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
* @return the psi and phi (M_PI minus the second and third Q values )
*/
Eigen::Vector2d _fourbarSolveQ(double theta,
AssemblyMode mode = 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;
// FourBarKinematicsVA sol;
};
} // namespace Karana::Dynamics