Program Listing for File SpatialInertia.h#
↰ Return to documentation for file (include/Karana/Math/SpatialInertia.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 SpatialInertia class.
*/
#pragma once
#include "Karana/Math/Defs.h"
#include "Karana/Math/HomTran.h"
#include "Karana/Math/SpatialVector.h"
#include <cfloat>
namespace Karana::Math {
using namespace Eigen;
/**
* @class SpatialInertia
* @brief Represents the spatial inertia. This is a 6x6 matrix with 10 independent quantities.
* These are given by the mass (scalar), center of mass offset (3-vector), and inertia
* 3x3 symmetric matrix (6 independent values).
*
* They
* are used to define the relative pose of pairs for frames.
* See \sref{structure of a spatial inertia,spinertiaDef} for
* the definition of a spatial inertia.
*/
class SpatialInertia {
// Constructors
public:
/**
* @brief Default constructor.
* Initializes all members to zero.
*/
SpatialInertia();
/**
* @brief Mark this SpatialInertia as not ready.
*/
void makeNotReady();
/**
* @brief Check whether this SpatialInertia is initialized.
* @return True if initialized, false otherwise.
*/
bool isReady() const;
/**
* @brief Constructor that sets mass, center of mass offset, and inertia matrix.
*
* @param mass Mass of the object.
* @param body_to_cm Vector from the body frame to the center of mass.
* @param inertia Inertia matrix in the body frame.
* @param validate Verify that the values lead to a symmetric, positive semi-definite CM
* inertia
*/
SpatialInertia(double mass,
const Vec3 &body_to_cm,
const Mat33 &inertia,
bool validate = true);
// Copy constructor, move constructor, and assignment operators
public:
/**
* @brief Copy constructor.
* @param other Other SpatialInertia to copy from.
*/
SpatialInertia(const SpatialInertia &other);
/** @brief Zero out the spatial inertia. */
void setZero();
/**
* @brief Copy assignment operator.
* @param other Other SpatialInertia to copy from.
* @return Reference to this object.
*/
SpatialInertia &operator=(const SpatialInertia &other);
/**
* @brief Move constructor.
* @param other SpatialInertia to move from.
*/
SpatialInertia(SpatialInertia &&other) noexcept;
/**
* @brief Move assignment operator.
* @param other SpatialInertia to move from.
* @return Reference to this object.
*/
SpatialInertia &operator=(SpatialInertia &&other) noexcept;
/**
* @brief Add SpatialInertias. The two SpatialInertias are assumed to be about a common
* frame.
*
* @param other The SpatialInertia to add to this one.
* @return Resultant SpatialInertia.
*/
SpatialInertia operator+(const SpatialInertia &other) const;
/**
* @brief Subtract SpatialInertias. The two SpatialInertias are assumed to be about a common
* frame.
*
* @param other The SpatialInertia to subtract from this one.
* @return Resultant SpatialInertia.
*/
SpatialInertia operator-(const SpatialInertia &other) const;
/**
* @brief Add another SpatialInertia to this one in-place. The two SpatialInertias are
* assumed to be about a common frame.
*
* @param other The SpatialInertia to add to this one.
* @return Reference to this SpatialInertia.
*/
SpatialInertia &operator+=(const SpatialInertia &other);
/**
* @brief Subtract another SpatialInertia from this one in-place. The two SpatialInertias
* are assumed to be about a common frame.
*
* @param other The SpatialInertia to subtract from this one.
* @return Reference to this SpatialInertia.
*/
SpatialInertia &operator-=(const SpatialInertia &other);
public:
/**
* @brief Equality operator.
* @param other SpatialInertia to compare with.
* @return True if equal, false otherwise.
*/
bool operator==(const SpatialInertia &other) const;
/**
* @brief Approximate equality check.
* @param other SpatialInertia to compare with.
* @param prec Precision threshold.
* @return True if approximately equal, false otherwise.
*/
bool isApprox(const SpatialInertia &other, double prec = MATH_EPSILON);
/**
* @brief Returns the type name of the object.
*
* @return A string representing the type name of the object.
*/
std::string_view typeString() const;
/**
* @brief Returns the mass of the object.
*
* @return The mass of the object.
*/
double mass() const;
/**
* @brief Returns the vector from the body frame to the center of mass.
*
* @return The vector from the body frame to the center of mass.
*/
const Vec3 &bodyToCm() const;
/**
* @brief Returns the body_to_cm * mass.
*
* @return The mass * body_to_cm.
*/
const Vec3 &mp() const;
/**
* @brief Returns the inertia matrix in the body frame.
*
* @return The inertia matrix in the body frame.
*/
const Mat33 &inertia() const;
/**
* @brief Returns the inertia matrix about the center of mass.
*
* @return The inertia matrix about the center of mass.
*/
const Mat33 &cmInertia() const;
/**
* @brief Get a string representation of this SpatialInertia.
*
* @param prefix String prefix for each line.
* @param precision Number of digits to use for floating point values.
* @param format_type The format type to use.
* @return String with information about this SpatialInertia.
*/
std::string dumpString(std::string_view prefix = "",
unsigned int precision = 10,
DumpFormatType format_type = DumpFormatType::DEFAULT_FLOAT) const;
/**
* @brief Print dumpString to std::cout.
*
* @param prefix String prefix for each line.
* @param precision Number of digits to use for floating point values.
* @param format_type The format type to use.
*/
void dump(std::string_view prefix = "",
unsigned int precision = 10,
DumpFormatType format_type = DumpFormatType::DEFAULT_FLOAT) const;
/**
* @brief Multiplies the spatial inertia with a spatial vector.
*
* @param vec The spatial vector to multiply.
* @return The resulting spatial vector after multiplication.
*/
SpatialVector operator*(const SpatialVector &vec) const;
/**
* @brief Returns the 6x6 matrix representation of the spatial inertia.
*
* @return The 6x6 matrix representation of the spatial inertia.
*/
const Mat66 &matrix() const;
/**
* @brief Calculates the a new spatial inertia using the parallel axis theorem.
*
* This method computes spI(x) = \\phi(x,y) * spI(y) * \\phi^*(x,y)
* where \\phi(x,y) is associated with the T(x,y) homogeneous
* transform
*
* @param T Homogeneous transformation T(x,y) to apply, where x is new, and y orig location
* @return The `SpatialInertia` at x transformed by T.
*/
const SpatialInertia parallelAxis(const HomTran &T) const;
protected:
/**
* @brief Set the values of this SpatialInertia.
*
* @param mass The mass of the SpatialInertia.
* @param body_to_cm The body-to-center-of-mass of the SpatialInertia.
* @param inertia The inertia of the SpatialInertia.
* @param at_cm `true` if the given inertia is about the center of mass, `false` otherwise.
* @param validate Verify that the values lead to a symmetric, positive semi-definite CM
* inertia
*/
void _setInertia(
double mass, const Vec3 &body_to_cm, const Mat33 &inertia, bool at_cm, bool validate);
/**
* @brief Check that the 3x3 inertia matrix is positive semi-definite.
*
* @param inertia - The 3x3 inertia matrix to check.
* */
void _validateInertia(const Mat33 &inertia) const; // NOLINT(readability-identifier-naming)
/**
* Return delta inertia for the 3x3 CM inertia for parallel axis transformation.
*
* Note that the returned delta value is the same for offset and -offset.
*
* @param offset The 3-vector offset
* @return Returned value is (- mass * tilde(offset) * tilde(offset
*
*/
Mat33 _parallelAxisCmInertiaOffset(const Vec3 &offset) const;
private:
double _mass = notReadyNaN;
Vec3 _body_to_cm, _mp;
Mat33 _inertia, _cm_inertia;
mutable bool _cached_spi_matrix_flag = false;
mutable Mat66 _cached_spi_matrix;
};
} // namespace Karana::Math