From c9bdc98020335e1d57a6f2dadd0da60348369ea9 Mon Sep 17 00:00:00 2001 From: William Schwend Date: Fri, 12 Sep 2025 13:25:49 -0600 Subject: [PATCH 1/5] Create new payload definition --- .../MJSysMassMatrixMsgPayload.h | 36 +++++++++++++++++++ 1 file changed, 36 insertions(+) create mode 100755 src/architecture/msgPayloadDefC/MJSysMassMatrixMsgPayload.h diff --git a/src/architecture/msgPayloadDefC/MJSysMassMatrixMsgPayload.h b/src/architecture/msgPayloadDefC/MJSysMassMatrixMsgPayload.h new file mode 100755 index 0000000000..aabdf750aa --- /dev/null +++ b/src/architecture/msgPayloadDefC/MJSysMassMatrixMsgPayload.h @@ -0,0 +1,36 @@ +/* + ISC License + + Copyright (c) 2025, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#ifndef MJ_SYS_MASS_MATRIX_MESSAGE_H +#define MJ_SYS_MASS_MATRIX_MESSAGE_H + +#include "architecture/utilities/macroDefinitions.h" + +#define MJ_MASSMATRIX_DIM (6 + MAX_EFF_CNT) // row/col size + +/*! @brief This structure is used in the messaging system to communicate what the mass matrix for +the full spacecraft system is currently from Mujoco.*/ +typedef struct { + int nbase; //!< [-] number of base DOFs (6 if free base, else 0) + int nj; //!< [-] number of joint DOFs populated in mass matrix + double MassMatrix[6 + MAX_EFF_CNT][6 + MAX_EFF_CNT]; //!< [varies by component] system mass matrix in generalized coordinates +}MJSysMassMatrixMsgPayload; + + +#endif From ae3f9cc44224cb454728230feb478d6d048b869f Mon Sep 17 00:00:00 2001 From: William Schwend Date: Fri, 12 Sep 2025 13:26:37 -0600 Subject: [PATCH 2/5] Create new MJSystemMassMatrix module --- .../MJSystemMassMatrix/MJSystemMassMatrix.cpp | 88 +++++++++++++++++++ .../MJSystemMassMatrix/MJSystemMassMatrix.h | 60 +++++++++++++ .../MJSystemMassMatrix/MJSystemMassMatrix.i | 44 ++++++++++ 3 files changed, 192 insertions(+) create mode 100644 src/simulation/mujocoDynamics/MJSystemMassMatrix/MJSystemMassMatrix.cpp create mode 100644 src/simulation/mujocoDynamics/MJSystemMassMatrix/MJSystemMassMatrix.h create mode 100644 src/simulation/mujocoDynamics/MJSystemMassMatrix/MJSystemMassMatrix.i diff --git a/src/simulation/mujocoDynamics/MJSystemMassMatrix/MJSystemMassMatrix.cpp b/src/simulation/mujocoDynamics/MJSystemMassMatrix/MJSystemMassMatrix.cpp new file mode 100644 index 0000000000..13fc7e14ee --- /dev/null +++ b/src/simulation/mujocoDynamics/MJSystemMassMatrix/MJSystemMassMatrix.cpp @@ -0,0 +1,88 @@ +/* + ISC License + + Copyright (c) 2025, Autonomous Vehicle Systems Lab, University of Colorado Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + +*/ + + +#include "simulation/mujocoDynamics/MJSystemMassMatrix/MJSystemMassMatrix.h" +#include "architecture/utilities/macroDefinitions.h" +#include +#include +#include +#include + +/*! Initialize C-wrapped output messages */ +void +MJSystemMassMatrix::SelfInit(){ + MJSysMassMatrixMsg_C_init(&this->massMatrixOutMsgC); +} + +/*! This is the constructor for the module class. It sets default variable + values and initializes the various parts of the model */ +MJSystemMassMatrix::MJSystemMassMatrix() +{ +} + +/*! This method is used to reset the module and checks that required input messages are connect. +*/ +void MJSystemMassMatrix::Reset(uint64_t CurrentSimNanos) +{ + if (!scene) { + bskLogger.bskLog(BSK_ERROR, "MJSystemMassMatrix: scene pointer not set!"); + } + + const auto model = scene->getMujocoModel(); + // extract the DOF dimensions + this->nDOF = model -> nv; + this->nbase = (model->nq >= 7 ? 6 : 0); + this->nj = std::max(0, this->nDOF - this->nbase); + + if (this->nbase + this->nj > 6 + MAX_EFF_CNT) { + bskLogger.bskLog(BSK_ERROR, "MJSystemMassMatrix: number of DOF (%d) exceeds message capacity (%d)!", this->nbase+this->nj, 6+MAX_EFF_CNT); + } +} + + +/*! This extracts the total spacecraft mass matrix from the MuJoCo scene. +*/ +void MJSystemMassMatrix::UpdateState(uint64_t CurrentSimNanos) +{ + const auto model = scene->getMujocoModel(); + const auto data = scene->getMujocoData(); + MJSysMassMatrixMsgPayload payload; + + // always zero the output message buffers before assigning values + payload = this->massMatrixOutMsg.zeroMsgPayload; + + // Build dense M in mjtNum + std::vector Mdense(this->nDOF * this->nDOF, mjtNum(0)); + mj_fullM(model, Mdense.data(), data->qM); + + for (int i = 0; i < this->nbase + this->nj; ++i) { + const mjtNum* src_row = Mdense.data() + i*this->nDOF; // stride by nDOF + double* dst_row = &payload.MassMatrix[i][0]; + std::copy_n(src_row, this->nDOF, dst_row); + } + + // write to the output messages + payload.nbase = this->nbase; + payload.nj = this->nj; + this->massMatrixOutMsg.write(&payload, this->moduleID, CurrentSimNanos); + // Write the C-wrapped output message + MJSysMassMatrixMsg_C_write(&payload, &this->massMatrixOutMsgC, this->moduleID, CurrentSimNanos); + +} diff --git a/src/simulation/mujocoDynamics/MJSystemMassMatrix/MJSystemMassMatrix.h b/src/simulation/mujocoDynamics/MJSystemMassMatrix/MJSystemMassMatrix.h new file mode 100644 index 0000000000..2972fdd476 --- /dev/null +++ b/src/simulation/mujocoDynamics/MJSystemMassMatrix/MJSystemMassMatrix.h @@ -0,0 +1,60 @@ +/* + ISC License + + Copyright (c) 2025, Autonomous Vehicle Systems Lab, University of Colorado Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + +*/ + + +#ifndef MJSYSTEMMASSMATRIX_H +#define MJSYSTEMMASSMATRIX_H + +#include "architecture/_GeneralModuleFiles/sys_model.h" +#include "cMsgCInterface/MJSysMassMatrixMsg_C.h" +#include "architecture/utilities/bskLogging.h" +#include "architecture/messaging/messaging.h" +#include "simulation/mujocoDynamics/_GeneralModuleFiles/MJScene.h" +#include "architecture/utilities/avsEigenSupport.h" +#include + +/*! @brief This is a C++ module to extract the system mass matrix from Mujoco + */ +class MJSystemMassMatrix: public SysModel { +public: + MJSystemMassMatrix(); + ~MJSystemMassMatrix() = default; + + void SelfInit(); + void Reset(uint64_t CurrentSimNanos); + void UpdateState(uint64_t CurrentSimNanos); + +public: + + MJScene* scene{nullptr}; //!< pointer to the MuJoCo scene + + Message massMatrixOutMsg; //!< system mass matrix C++ output msg in generalized coordinates + MJSysMassMatrixMsg_C massMatrixOutMsgC = {}; //!< system mass matrix C output msg in generalized coordinates + + BSKLogger bskLogger; //!< BSK Logging +private: + + int nDOF{0}; // number of total DOF + int nbase{0}; // number of base DOF + int nj{0}; // number of joint DOF + +}; + + +#endif diff --git a/src/simulation/mujocoDynamics/MJSystemMassMatrix/MJSystemMassMatrix.i b/src/simulation/mujocoDynamics/MJSystemMassMatrix/MJSystemMassMatrix.i new file mode 100644 index 0000000000..9cfccc80ef --- /dev/null +++ b/src/simulation/mujocoDynamics/MJSystemMassMatrix/MJSystemMassMatrix.i @@ -0,0 +1,44 @@ +/* + ISC License + + Copyright (c) 2025, Autonomous Vehicle Systems Lab, University of Colorado Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + +*/ + +%module MJSystemMassMatrix + +%include "architecture/utilities/bskException.swg" +%default_bsk_exception(); + +%{ + #include "MJSystemMassMatrix.h" +%} + +%pythoncode %{ + from Basilisk.architecture.swig_common_model import * +%} +%include "std_string.i" +%include "swig_conly_data.i" + +%include "sys_model.i" +%include "MJSystemMassMatrix.h" + +%include "architecture/msgPayloadDefC/MJSysMassMatrixMsgPayload.h" +struct MJSysMassMatrixMsg_C; + +%pythoncode %{ +import sys +protectAllClasses(sys.modules[__name__]) +%} From 8858cf239f082698643917307f9cd1b28c2b31bf Mon Sep 17 00:00:00 2001 From: William Schwend Date: Fri, 12 Sep 2025 13:27:13 -0600 Subject: [PATCH 3/5] Create new MJSystemMassMatrix module documentation --- .../MJSystemMassMatrix/MJSystemMassMatrix.rst | 54 +++++++++++++++++++ 1 file changed, 54 insertions(+) create mode 100644 src/simulation/mujocoDynamics/MJSystemMassMatrix/MJSystemMassMatrix.rst diff --git a/src/simulation/mujocoDynamics/MJSystemMassMatrix/MJSystemMassMatrix.rst b/src/simulation/mujocoDynamics/MJSystemMassMatrix/MJSystemMassMatrix.rst new file mode 100644 index 0000000000..8955f497c0 --- /dev/null +++ b/src/simulation/mujocoDynamics/MJSystemMassMatrix/MJSystemMassMatrix.rst @@ -0,0 +1,54 @@ +Executive Summary +----------------- +The ``MJSystemMassMatrix`` module extracts the full system mass matrix as well as stores the degrees of freedom from a Mujoco scene with a single spacecraft. + +.. warning:: + This module is designed to be used with a Mujoco object that is a single spacecraft that has a rigid hub and a set of multi-jointed arms. The values extracted from + the Mujoco scene are in generalized coordinates so other spacecraft layouts may result in unexpected behavior. + +Message Connection Descriptions +------------------------------- +The following table lists all the module input and output messages. +The module msg connection is set by the user from python. +The msg type contains a link to the message structure definition, while the description +provides information on what this message is used for. + +.. list-table:: Module I/O Messages + :widths: 25 25 50 + :header-rows: 1 + + * - Msg Variable Name + - Msg Type + - Description + * - massMatrixOutMsg + - :ref:`MJSysMassMatrixMsgPayload` + - system mass matrix C ++ output msg in generalized coordinates + * - massMatrixOutMsgC + - :ref:`MJSysMassMatrixMsgPayload` + - system mass matrix C output msg in generalized coordinates + +User Guide +---------- +This section is to outline the steps needed to setup the ``MJSystemMassMatrix`` module in Python using Basilisk. + +#. Import the MJSystemMassMatrix class:: + + from Basilisk.simulation import MJSystemMassMatrix + +#. Enable extra EOM call when building the Mujoco scene:: + + scene.extraEoMCall = True + +#. Create an instance of MJSystemMassMatrix:: + + module = MJSystemMassMatrix.MJSystemMassMatrix() + +#. Set the scene the module is attached to:: + + module.scene = scene + +#. The MJSystemMassMatrix output message is ``massMatrixOutMsg``. + +#. Add the module to the task list:: + + unitTestSim.AddModelToTask(unitTaskName, module) From a1b4fdb865ffed62dfb70faf8c159801e4e4b24f Mon Sep 17 00:00:00 2001 From: William Schwend Date: Fri, 12 Sep 2025 13:27:53 -0600 Subject: [PATCH 4/5] Create new MJSystemMassMatrix module unit test --- .../_UnitTest/test_MJSystemMassMatrix.py | 324 ++++++++++++++++++ 1 file changed, 324 insertions(+) create mode 100644 src/simulation/mujocoDynamics/MJSystemMassMatrix/_UnitTest/test_MJSystemMassMatrix.py diff --git a/src/simulation/mujocoDynamics/MJSystemMassMatrix/_UnitTest/test_MJSystemMassMatrix.py b/src/simulation/mujocoDynamics/MJSystemMassMatrix/_UnitTest/test_MJSystemMassMatrix.py new file mode 100644 index 0000000000..b601afc72c --- /dev/null +++ b/src/simulation/mujocoDynamics/MJSystemMassMatrix/_UnitTest/test_MJSystemMassMatrix.py @@ -0,0 +1,324 @@ +# +# ISC License +# +# Copyright (c) 2025, Autonomous Vehicle Systems Lab, University of Colorado Boulder +# +# Permission to use, copy, modify, and/or distribute this software for any +# purpose with or without fee is hereby granted, provided that the above +# copyright notice and this permission notice appear in all copies. +# +# THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES +# WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF +# MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR +# ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES +# WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN +# ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF +# OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. +# +# + +import os +import numpy as np +from numpy.testing import assert_allclose, assert_equal, assert_array_less +import pytest +import tempfile + +from Basilisk.utilities import SimulationBaseClass, macros +try: + from Basilisk.simulation import MJSystemMassMatrix, mujoco + + couldImportMujoco = True +except: + couldImportMujoco = False + +# Constants used throughout tests +HUB_MASS = 50.0 +HUB_I_COM = np.diag([10.0, 11.0, 12.0]) # hub inertia @ COM +ARM_MASS = 10.0 +ROD_I_COM = np.diag([ 5.0, 6.0, 7.0]) # child inertia @ COM +SX, SY, SZ = 0.5, 0.3, 0.2 + +def _write_tmp(xml_text: str, base: str) -> str: + td = tempfile.mkdtemp(prefix="mjmass_") + p = os.path.join(td, f"{base}.xml") + with open(p, "w") as f: + f.write(xml_text) + return p + +def xml_single(): + return f""" + """ + +def xml_one_hinge(): + # Hinge about z at child origin; child COM at joint → M22 ≈ [Izz] + return f""" + """ + +def xml_one_slider(): + # Slider along x at child origin; child COM at joint → M22 ≈ [m] + return f""" + """ + +def xml_two_joints_separate(): + # Two independent branches: hinge child and slider child → M22 ≈ diag([Izz, m]) + return f""" + """ + +def _skew(v): + x, y, z = v + return np.array([[0, -z, y], + [z, 0, -x], + [-y, x, 0]], float) + +def _composite_spatial_inertia(bodies): + """ + bodies: [{'m':float,'r':(3,), 'Icom':(3,3)}, ...] + Return (M6, rC). + """ + m_tot = sum(b['m'] for b in bodies) + rC = sum(b['m']*np.asarray(b['r'], float) for b in bodies) / m_tot + + I_O = np.zeros((3,3)) + for b in bodies: + r = np.asarray(b['r'], float) + Icom = np.asarray(b['Icom'], float) + I_O += Icom + b['m'] * ((r@r)*np.eye(3) - np.outer(r, r)) + + M6 = np.zeros((6,6)) + M6[:3,:3] = m_tot*np.eye(3) + M6[:3,3:] = -m_tot*_skew(rC) + M6[3:,:3] = m_tot*_skew(rC) + M6[3:,3:] = I_O + return M6, rC + +def expected_base_block(bodies): + M6, _ = _composite_spatial_inertia(bodies) + return M6 + +def expected_joint_block(model_name): + if model_name == "one_hinge": + return np.array([[ROD_I_COM[2,2]]], float) # hinge about z → Izz + if model_name == "one_slider": + return np.array([[ARM_MASS]], float) # slider along x → mass + if model_name == "two_joints": + return np.diag([ROD_I_COM[2,2], ARM_MASS]).astype(float) + +def add_allclose(testFailCount, testMessages, name, A, B, rtol=0.0, atol=1e-10): + try: + assert_allclose(A, B, rtol=rtol, atol=atol) + except AssertionError as e: + testFailCount += 1 + testMessages.append(f"{name}: {str(e)}\n") + return testFailCount, testMessages + +def add_equal(testFailCount, testMessages, name, a, b): + try: + assert_equal(a, b) + except AssertionError as e: + testFailCount += 1 + testMessages.append(f"{name}: {str(e)}\n") + return testFailCount, testMessages + +def add_symmetry(testFailCount, testMessages, name, M, tol=1e-12): + return add_allclose(testFailCount, testMessages, name, M, M.T, rtol=0.0, atol=tol) + +def add_psd(testFailCount, testMessages, name, M, tol=-1e-12): + try: + w = np.linalg.eigvalsh(0.5*(M+M.T)) + # require every eigenvalue >= tol (allow tiny negative for roundoff) + assert_array_less(np.full_like(w, tol), w) + except AssertionError as e: + testFailCount += 1 + testMessages.append(f"{name}: not PSD; {str(e)}; eigvals={w}\n") + return testFailCount, testMessages + +def add_padding_zero(testFailCount, testMessages, name, M, used, atol=1e-14): + try: + if used < M.shape[0]: + assert_allclose(M[used:, :], 0.0, rtol=0.0, atol=atol) + assert_allclose(M[:, used:], 0.0, rtol=0.0, atol=atol) + except AssertionError as e: + testFailCount += 1 + testMessages.append(f"{name}: {str(e)}\n") + return testFailCount, testMessages + +def read_matrix_from_recorder(rec): + """Infer DIM at runtime; return (Mmsg, used, nbase, nj).""" + nbase = int(rec.nbase[-1]) + nj = int(rec.nj[-1]) + used = nbase + nj + Mraw = np.array(rec.MassMatrix[-1,:,:]) + if Mraw.ndim == 1: + dim = int(round(np.sqrt(Mraw.size))) + Mmsg = Mraw.reshape(dim, dim) + else: + dim = Mraw.shape[0] + Mmsg = Mraw + return Mmsg, used, nbase, nj + +@pytest.mark.skipif(not couldImportMujoco, reason="Compiled Basilisk without --mujoco") +@pytest.mark.parametrize("model_name,xml_fn, nj_expected", [ + ("single", xml_single, 0), + ("one_hinge", xml_one_hinge, 1), + ("one_slider", xml_one_slider, 1), + ("two_joints", xml_two_joints_separate, 2), +]) + +def test_MJSystemMassMatrix(show_plots, model_name, xml_fn, nj_expected): + """Module Unit Test""" + [testResults, testMessage] = MJSystemMassMatrixTestFunction(show_plots, model_name, xml_fn, nj_expected) + assert testResults < 1, testMessage + + +def MJSystemMassMatrixTestFunction(show_plots, model_name, xml_fn, nj_expected): + r""" + **Validation Test Description** + + This unit test sets up a spacecraft to determine its system mass matrix. + + **Test Parameters** + + The spacecraft geometry is varied between tests + + Args: + model_name (str): the model of the spacecraft for this parameterized unit test + xml_fn (str): the XML file name for the spacecraft model + nj_expected (int): the expected number of joints in the spacecraft model + + **Description of Variables Being Tested** + + In this test we are checking the system mass matrix values and properties to ensure it is accurate + """ + testFailCount = 0 + testMessages = [] + unitTaskName = "unitTask" + unitProcessName = "TestProcess" + + xml_path = _write_tmp(xml_fn(), model_name) + + unitTestSim = SimulationBaseClass.SimBaseClass() + testProcessRate = macros.sec2nano(0.1) + testProc = unitTestSim.CreateNewProcess(unitProcessName) + testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) + + # create the Mujoco scene + scene = mujoco.MJScene.fromFile(xml_path) + scene.extraEoMCall = True + unitTestSim.AddModelToTask(unitTaskName, scene) + + # setup module to be tested + module = MJSystemMassMatrix.MJSystemMassMatrix() + module.ModelTag = "MJSystemMassMatrixTag" + module.scene = scene + unitTestSim.AddModelToTask(unitTaskName, module) + + # setup output message recorder objects + massMatrixOutMsgRec = module.massMatrixOutMsg.recorder() + massMatrixOutMsgCRec = module.massMatrixOutMsgC.recorder() + unitTestSim.AddModelToTask(unitTaskName, massMatrixOutMsgRec) + unitTestSim.AddModelToTask(unitTaskName, massMatrixOutMsgCRec) + + unitTestSim.InitializeSimulation() + unitTestSim.ConfigureStopTime(macros.sec2nano(0.1)) + unitTestSim.ExecuteSimulation() + + # Read from recorder + Mmsg, used, nbase, nj = read_matrix_from_recorder(massMatrixOutMsgRec) + MmsgC, usedC, nbaseC, njC = read_matrix_from_recorder(massMatrixOutMsgCRec) + + # check sizing + testFailCount, testMessages = add_equal(testFailCount, testMessages, "nbase", nbase, 6) + testFailCount, testMessages = add_equal(testFailCount, testMessages, "nj", nj, nj_expected) + + # build truth dictionary + bodies = [dict(m=HUB_MASS, r=np.zeros(3), Icom=HUB_I_COM)] + if model_name == "one_hinge": + bodies.append(dict(m=ARM_MASS, r=np.array([ 0.6, 0.0, 0.0]), Icom=ROD_I_COM)) + elif model_name == "one_slider": + bodies.append(dict(m=ARM_MASS, r=np.array([-0.6, 0.0, 0.0]), Icom=ROD_I_COM)) + elif model_name == "two_joints": + bodies.append(dict(m=ARM_MASS, r=np.array([ 0.6, 0.0, 0.0]), Icom=ROD_I_COM)) + bodies.append(dict(m=ARM_MASS, r=np.array([-0.6, 0.0, 0.0]), Icom=ROD_I_COM)) + + M6_truth = expected_base_block(bodies) + M6 = Mmsg[:6, :6] + testFailCount, testMessages = add_allclose(testFailCount, testMessages, "Base 6x6", M6, M6_truth, atol=1e-10, rtol=0.0) + + # Joint block truth + if nj_expected > 0: + M22 = Mmsg[6:6+nj_expected, 6:6+nj_expected] + M22_truth = expected_joint_block(model_name) + testFailCount, testMessages = add_allclose(testFailCount, testMessages, "M22", M22, M22_truth, atol=1e-10, rtol=0.0) + + # Global properties on full populated block + M_used = Mmsg[:used, :used] + testFailCount, testMessages = add_symmetry(testFailCount, testMessages, "Symmetry(full used block)", M_used, tol=1e-12) + testFailCount, testMessages = add_psd(testFailCount, testMessages, "PSD(full used block)", M_used, tol=-1e-12) + + # Zero padding outside populated block + testFailCount, testMessages = add_padding_zero(testFailCount, testMessages, "Padding", Mmsg, used, atol=1e-14) + + # Check the two output messages agree + testFailCount, testMessages = add_allclose(testFailCount, testMessages, "C vs C++ full matrix", Mmsg, MmsgC, rtol=0.0, atol=1e-14) + testFailCount, testMessages = add_equal(testFailCount, testMessages, "C vs C++ nbase", nbase, nbaseC) + testFailCount, testMessages = add_equal(testFailCount, testMessages, "C vs C++ nj", nj, njC) + + if testFailCount == 0: + print("PASSED: " + module.ModelTag) + else: + print(testMessages) + + return [testFailCount, "".join(testMessages)] + + +if __name__ == "__main__": + # Run one case by hand + MJSystemMassMatrixTestFunction(False, "single", xml_single, 0) From debf5014c686fc5950df9ac2f9a951b6c4173f2f Mon Sep 17 00:00:00 2001 From: William Schwend Date: Mon, 22 Sep 2025 10:22:04 -0600 Subject: [PATCH 5/5] Update release notes --- docs/source/Support/bskReleaseNotes.rst | 1 + 1 file changed, 1 insertion(+) diff --git a/docs/source/Support/bskReleaseNotes.rst b/docs/source/Support/bskReleaseNotes.rst index 411cda164d..ebd6139cdf 100644 --- a/docs/source/Support/bskReleaseNotes.rst +++ b/docs/source/Support/bskReleaseNotes.rst @@ -31,6 +31,7 @@ Version |release| - Made statistical unit tests more robust - Added fault modeling capability to :ref:`magnetometer` module. - Added new module :ref:`MJSystemCoM` to extract the system center of mass position and velocity from a MuJoCo simulation. +- Added new module :ref:`MJSystemMassMatrix` to extract the system mass matrix from a MuJoCo simulation. - Refactored the CI build system scripts - Removed deprecated use of ``Basilisk.simulation.planetEphemeris.ClassicElementsMsgPayload``. Users need to use ``ClassicalElements()`` defined in ``orbitalMotion``.