Program Listing for File JacobianGenerator.h#
↰ Return to documentation for file (include/Karana/SOADyn/JacobianGenerator.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 FrameToFrameJacobianGenerator and MultiJacobianGenerator
* classes.
*/
#pragma once
#include "CoordData.h"
#include "Karana/KCore/LockingBase.h"
namespace Karana::Dynamics {
namespace kc = Karana::Core;
namespace km = Karana::Math;
namespace kf = Karana::Frame;
class SubTree;
/**
* @class FrameToFrameJacobianGenerator
* @brief Class for computing relative Jacobians and pose gradients for a frame pair defined by
* a Karana::Frame::FrameToFrame
*
* The source and target frames for the Jacobian are the
* oframe/pframe Karana::Frame::Frame pair for the
* Karana::Frame::FrameToFrame provided to the constructor. See the
* \sref{jacobian_sec} section for more discussion on Jacobians.
*
* CAUTION: When using the "analytical" computation approach
* (recommended), an important assumption is that the f2f path
* contains only regular edges, i.e. one whose transform values that
* depend on coordinate values do so via regular subhinges and body
* coordinates, and not in come non-standard way (e.g., subtree CM
* frames, ephemeris frames). Since this Jacobian generator will not
* know about these non-standard dependencies, it will not be able
* to take them into account when computing the Jacobians, so use
* the "numerical" approach for these situations.
*
*/
class FrameToFrameJacobianGenerator : public kc::LockingBase {
// for access to _coord_data
friend class MultiJacobianGenerator;
public:
/**
* @brief Factory method for creating a FrameToFrameJacobianGenerator
*
* @param name name for the new object
* @param f_to_f the Karana::Frame::FrameToFrame instance to compute the relative Jacobian
* for
* @param coord_data the list of CoordData with the coordinates
* @return the new FrameToFrameJacobianGenerator instance
*/
static kc::ks_ptr<FrameToFrameJacobianGenerator> create(std::string_view name,
kc::ks_ptr<kf::FrameToFrame> f_to_f,
kc::ks_ptr<CoordData> coord_data);
/**
* @brief Constructor
*
* Constructor for the FrameToFrameJacobianGenerator. Once created, this
* object can be used repeatedly to compute the configuration
* dependent Jacobian and pose gradient matrices for different
* poses. The returned matrices are for the f2f's pframe with respect to its
* oframe. While the common use case is with the source frame
* being the inertial frame, this object supports the more
* general case where the source frame may be attached to
* another moving body in the system.
*
* The computed jacobian matrices do not include columns for the
* frozen coords in the CoordData. An alternative option would
* be to set the columns to zero values, but this is non-optimal
* since the column values would need to be computed (only to be
* discarded), and the returned matrix would be larger.
*
* @param name name for the new object
* @param f2f the Karana::Frame::FrameToFrame instance to compute the relative Jacobian for
* @param coord_data the list of CoordData with the coordinates
*/
FrameToFrameJacobianGenerator(std::string_view name,
kc::ks_ptr<kf::FrameToFrame> f2f,
kc::ks_ptr<CoordData> coord_data);
/**
* @brief Method to compute the Jacobian matrix
*
* Return the overall assembled 6xnU Jacobian to the target frame
* spatial velocity (represented in the target frame) with respect to the
* path map's source frame from the U generalized velocities of
* all the component coordinate providers. Use the component
* analytical Jacobians if analytical is true, or the numerical
* differencing approach otherwise. If not provided, the source
* frame is assumed to be the newtonian frame. This Jacobian is
* such that the following will be true:
*
* f2f.relSpVel() == f2f.relTransform().getUnitQuaternion() (jac * getU())
*
* The input polarity map must be compatible with the same
* source/target frame values used when calling this method. It will
* be created if it is not provided. Coordinate providers not in the
* polarity map are assumed to not have an effect on the target frame,
* and zero sub-matrix entries are assigned for them.
*
* For normal inverse kinematics (e.g., with respect to inertial frame), we normally
* exclude coordinate providers that are outboard of (or on a separate
* branch from) the target frame of interest.
*
* Otherwise, the polarity map value for each coordinate provider is
* used to negate the sub-matrix entries for the ones with opposed
* polarity. We include such opposed polarity coordinate provider
* elements for cases where the source frame is not the inertial
* frame, and the coordinate provider is on the path from the source
* to the target frame, but may be on a different branch and o the
* path from the source frame to the common ancestor frame. In this
* case, the coordinate provider's effect is the negative of the
* normal effect on the target frame.
*
* @param analytical If true, use the analytical process, else numerical differencing
* @return the Jacobian matrix
*/
km::Mat jacobian(bool analytical = true);
/**
* @brief Method to compute the time derivative of the Jacobian
* matrix
*
* The multibody argument is required when analytical is
* false, since a total time derivative of the Jacobian needs to
* be computed, and this requires that all the multibody
* coordinates be perturbed.
*
* @param analytical If true, use the analytical process, else numerical differencing
* @param mb the multibody system
* @return the time derivative of the Jacobian matrix
*/
km::Mat jacobianDot(bool analytical = true, kc::ks_ptr<Multibody> mb = nullptr);
/**
* @brief Method to compute the pose gradient matrix
*
* Return the overall assembled 6xnU gradient for the target frame
* pose with respect to the Q generalized coordinates of all the component
* coordinate providers. Use the component analytical gradient
* contributions if analytical is true, or the numerical differencing
* approach otherwise.
*
* The input polarity map must be compatible with the same
* source/target frame values used when calling this method. It will
* be created if it is not provided. Coordinate providers not in the
* polarity map are assumed to not have an effect on the target frame,
* and zero sub-matrix entries are assigned for them. For normal
* inverse kinematics (e.g., with respect to inertial frame), we exclude coordinate
* providers that are outboard of (or on a separate branch from) the
* target frame of interest.
*
* Otherwise, the polarity map value for each coordinate provider is
* used to negate the sub-matrix entries for the ones with opposed
* polarity. We include such opposed polarity coordinate provider
* elements for cases where the source frame is not the inertial
* frame, and the coordinate provider is on the path from the source
* to the target frame, but may be on a different branch and o the
* path from the source frame to the common ancestor frame. In this
* case, the coordinate provider's effect is the negative of the
* normal effect on the target frame.
*
* We can choose to eliminate some columns from the computed matrix
* by specifying the corresponding coordinate providers in the
* skip_coords list. An alternative option is to set the columns to
* zero values, but this is non-optimal since the column values would
* need to be computed (only to be discarded), and the returned matrix
* would be larger.
*
* @param analytical If true, use the analytical process, else numerical differencing
* @return the pose gradient matrix
*/
km::Mat poseGradient(bool analytical = true);
std::string dumpString(std::string_view prefix,
const kc::Base::DumpOptions *options) const override;
protected:
/**
* @brief Compute the polarities for all the coordinates
*
* @return the polarity map
*/
const std::unordered_map<kc::id_t, bool> &_getPolarityMap() const { return _polarity_map; }
/** For the subhinges in this CoordData, a polarity map with
bool entries for the component subhinges. Set the entry to
true if the subhinge's edge direction is aligned with the
f2f path. Add a false entry if
include_opposed is true. Skip adding an entry for the
subhinge if the subhinge edge does not lie in the path. The
polarity map is used to limit the subhinge entries of
interest to just those on the f2f path when
assembling the Jacobian for the target frame (see the
jacobian method).
This polarity map is used for assembling the Jacobian matrix
for the f2f path. A true entry for a subhinge
means that subhinge's Jacobian to the target should be used
as is since the pframe is outboard of the
coord. When the target frame is not outboard, i.e. we
have opposite polarity and the negative of the contributing
subhinge Jacobian matrix should be used.
*/
void _setupPolarityMap();
void _makeHealthy() override;
/**
* @brief Discard the provided FrameToFrameJacobianGenerator.
* @param base - Base pointer to the FrameToFrameJacobianGenerator to discard.
*/
void _discard(kc::ks_ptr<Base> &base) override;
protected:
/** the frame to frame instance for this jacobian generator */
kc::ks_ptr<kf::FrameToFrame> _f2f = nullptr;
/** the polarity map with polarity info for each of the CoordBase
instances */
std::unordered_map<kc::id_t, bool> _polarity_map;
/** the CoordData with the CoordBases the Jacobian is for */
kc::ks_ptr<CoordData> _coord_data;
};
/**
* @class MultiJacobianGenerator
* @brief Class for the Jacobian generator for a list of Karana::Frame::FrameToFrame related
* frame pairs for a list of CoordData
*
* This class builds on the FrameToFrameJacobianGenerator class to support
* multiple Karana::Frame::FrameToFrame instances. This class is used
* by the ConstraintKinematicsSolver to compute Jacobians needed for
* the iterative constraint kinematics algorithms. See the
* \sref{jacobian_sec} section for more discussion on Jacobians.
*
* CAUTION: When using the "analytical" computation approach
* (recommended), an important assumption is that the f2f paths
* contains only regular edges, i.e. one whose transform values that
* depend on coordinate values do so via regular subhinges and body
* coordinates, and not in come non-standard way (e.g., subtree CM
* frames, ephemeris frames). Since this Jacobian generator will not
* know about these non-standard dependencies, it will not be able
* to take them into account when computing the Jacobians, so use
* the "numerical" approach for these situations.
*
*/
class MultiJacobianGenerator : public kc::LockingBase {
public:
/**
* @brief Factory method for creating a new MultiJacobianGenerator instance
*
* @param name the name for the new object.
* @param f2fs the list of FrameToFrame's for the Jacobian generator
* @param coord_data the CoordDatas to use for the Jacobian columns
* @return a new MultiJacobianGenerator instance
*/
static kc::ks_ptr<MultiJacobianGenerator>
create(std::string_view name,
const std::vector<kc::ks_ptr<kf::FrameToFrame>> &f2fs,
kc::ks_ptr<CoordData> coord_data);
/**
* @brief Constructor
*
* @param name the name for the new object.
* @param f2fs the list of FrameToFrame's for the Jacobian generator
* @param coord_data the CoordDatas to use for the Jacobian columns
*/
MultiJacobianGenerator(std::string_view name,
const std::vector<kc::ks_ptr<kf::FrameToFrame>> &f2fs,
kc::ks_ptr<CoordData> coord_data);
/**
* @brief Destructor
*/
~MultiJacobianGenerator();
//----------------------------
/**
* @brief Return the Jacobian matrix
*
* Return the Jacobian matrix map from the free U velocity
* coordinate to the f2fs relative velocity.
*
* @param analytical If true, use the analytical method, else numerical differencing
* @return the Jacobian matrix
*/
km::Mat jacobian(bool analytical = true);
/**
* @brief Method to compute the time derivative of the Jacobian matrix
*
* The multibody argument is required when analytical is
* false, since a total time derivative of the Jacobian needs to
* be computed, and this requires that all the multibody
* coordinates be perturbed.
*
* @param analytical If true, use the analytical method, else numerical differencing
* @param mb the multibody system
* @return the time derivative of the Jacobian matrix
*/
km::Mat jacobianDot(bool analytical = true, kc::ks_ptr<Multibody> mb = nullptr);
/**
* @brief Return the pose gradient matrix
*
* Return the gradient matrix map from the free Q configuration
* coordinates to the f2fs relative pose
*
* @param analytical If true, use the analytical method, else numerical differencing
* @return the pose gradient matrix
*/
km::Mat poseGradient(bool analytical = true);
std::string dumpString(std::string_view prefix,
const kc::Base::DumpOptions *options) const override;
//----------------------------
protected:
/**
* @brief - Discard the provided MultiJacobianGenerator.
* @param base - Base pointer to the MultiJacobianGenerator to discard.
*/
void _discard(kc::ks_ptr<Base> &base) override;
/** the frame to frame instances for this multi-jacobian generator */
std::vector<kc::ks_ptr<kf::FrameToFrame>> _f2fs;
/** map of Jacobian generators for each frame to frame instance */
std::unordered_map<kc::ks_ptr<kf::FrameToFrame>, kc::ks_ptr<FrameToFrameJacobianGenerator>>
_jac_gen_map;
/** the CoordData with the CoordBases the multi-Jacobian is for */
kc::ks_ptr<CoordData> _coord_data;
};
} // namespace Karana::Dynamics