diff --git a/docs/source/Support/bskReleaseNotes.rst b/docs/source/Support/bskReleaseNotes.rst index b4b153a0b6..a77c144902 100644 --- a/docs/source/Support/bskReleaseNotes.rst +++ b/docs/source/Support/bskReleaseNotes.rst @@ -37,6 +37,7 @@ Version |release| - Fix typo in how :ref:`gravityEffector` compute the planets gravity potential contributions - 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``. diff --git a/src/architecture/msgPayloadDefCpp/MJSysMassMatrixMsgPayload.h b/src/architecture/msgPayloadDefCpp/MJSysMassMatrixMsgPayload.h new file mode 100755 index 0000000000..bf1ca6cd74 --- /dev/null +++ b/src/architecture/msgPayloadDefCpp/MJSysMassMatrixMsgPayload.h @@ -0,0 +1,38 @@ +/* + 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 + +/*! @brief Structure used by the messaging system to communicate details about the spacecraft system including its mass matrix.*/ +typedef struct +//@cond DOXYGEN_IGNORE +MJSysMassMatrixMsgPayload +//@endcond +{ + int nSC; //!< [-] number of spacecraft in the system + std::vector scStartIdx; //!< [-] list of the first joint index for each spacecraft + std::vector jointTypes; //!< [-] list of the joint types (0:free,1:ball,2:slide,3:hinge) in the system + std::vector massMatrix; //!< [-] mass matrix of the system in row-major order + +} MJSysMassMatrixMsgPayload; + +#endif diff --git a/src/simulation/mujocoDynamics/MJSystemMassMatrix/MJSystemMassMatrix.cpp b/src/simulation/mujocoDynamics/MJSystemMassMatrix/MJSystemMassMatrix.cpp new file mode 100644 index 0000000000..2456665064 --- /dev/null +++ b/src/simulation/mujocoDynamics/MJSystemMassMatrix/MJSystemMassMatrix.cpp @@ -0,0 +1,108 @@ +/* + 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 + +void MJSystemMassMatrix::Reset(uint64_t CurrentSimNanos) +{ + if (!scene) { + bskLogger.bskLog(BSK_ERROR, "MJSystemMassMatrix: scene pointer not set!"); + } + + const mjModel* model = scene->getMujocoModel(); + if (!model) { + bskLogger.bskLog(BSK_ERROR, "MJSystemMassMatrix: MuJoCo model not available in Reset()"); + } + + // extract the DOF dimensions + this->nDOF = static_cast(model->nv); + + // determine the joint types in the system + this->jointTypes.clear(); + this->jointTypes.reserve(static_cast(model->njnt)); + for (int j = 0; j < model->njnt; ++j) { + const int jt = model->jnt_type[j]; + this->jointTypes.push_back(static_cast(jt)); + } + + // determine the number of spacecraft in the system and store the starting joint index for each spacecraft + // a new spacecraft is assumed to be added for each kinematic tree + this->nSC = 0; + this->scStartIdx.clear(); + for (int b = 1; b < model->nbody; ++b) { + if (model->body_parentid[b] == 0) { + const int jntCount = model->body_jntnum[b]; + if (jntCount > 0) { + const int rootJ = model->body_jntadr[b]; + this->scStartIdx.push_back(static_cast(rootJ)); + ++this->nSC; + } + } + } + + + // verify that all free joints are at the root of a kinematic tree + for (int j = 0; j < model->njnt; ++j) { + if (model->jnt_type[j] == mjJNT_FREE) { + const int b = model->jnt_bodyid[j]; + + const bool bodyIsTreeRoot = (model->body_parentid[b] == 0); + const bool jointIsFirstOnBody = (j == model->body_jntadr[b]); + + if (!bodyIsTreeRoot || !jointIsFirstOnBody) { + bskLogger.bskLog(BSK_ERROR, + "MJSystemMassMatrix: Free joint j=%d on body b=%d must be the first joint on a tree root body.", j, b); + } + } + } +} + +void MJSystemMassMatrix::UpdateState(uint64_t CurrentSimNanos) +{ + const mjModel* model = scene->getMujocoModel(); + const mjData* data = scene->getMujocoData(); + + // always zero the output message buffers before assigning values + MJSysMassMatrixMsgPayload payload = this->massMatrixOutMsg.zeroMsgPayload; + + // Build dense M matrix from MuJoCo + const std::size_t NN = (this->nDOF) * this->nDOF; + std::vector Mdense(NN, mjtNum(0)); + mj_fullM(model, Mdense.data(), data->qM); + + + // write to the output message + payload.nSC = static_cast(this->nSC); + payload.scStartIdx.reserve(this->scStartIdx.size()); + for (auto idx : this->scStartIdx) { + payload.scStartIdx.push_back(static_cast(idx)); + } + payload.jointTypes.reserve(this->jointTypes.size()); + for (auto jt : this->jointTypes) { + payload.jointTypes.push_back(static_cast(jt)); + } + payload.massMatrix.assign(Mdense.begin(), Mdense.end()); + this->massMatrixOutMsg.write(&payload, 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..73eefdf280 --- /dev/null +++ b/src/simulation/mujocoDynamics/MJSystemMassMatrix/MJSystemMassMatrix.h @@ -0,0 +1,56 @@ +/* + 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 "architecture/msgPayloadDefCpp/MJSysMassMatrixMsgPayload.h" +#include "architecture/utilities/bskLogging.h" +#include "architecture/messaging/messaging.h" +#include "simulation/mujocoDynamics/_GeneralModuleFiles/MJScene.h" + +/*! @brief This is a C++ module to extract the system mass matrix from Mujoco + */ +class MJSystemMassMatrix: public SysModel { +public: + MJSystemMassMatrix() = default; /*! This is the constructor for the module class. */ + ~MJSystemMassMatrix() = default; /*! This is the destructor for the module class. */ + + void Reset(uint64_t CurrentSimNanos); /*! This method is used to reset the module and checks that the scene is setup. */ + void UpdateState(uint64_t CurrentSimNanos); /*! This method is used to extract the total spacecraft mass matrix from the MuJoCo scene. */ + +public: + + MJScene* scene{nullptr}; //!< pointer to the MuJoCo scene + + Message massMatrixOutMsg; //!< system mass matrix C++ output msg in generalized coordinates + + BSKLogger bskLogger; //!< BSK Logging +private: + + std::size_t nDOF{0}; //!< number of total DOF + std::size_t nSC{0}; //!< number of spacecraft in the system + std::vector scStartIdx; //!< list of the first joint index for each spacecraft + std::vector jointTypes; //!< list of joint types in the system +}; + + +#endif diff --git a/src/simulation/mujocoDynamics/MJSystemMassMatrix/MJSystemMassMatrix.i b/src/simulation/mujocoDynamics/MJSystemMassMatrix/MJSystemMassMatrix.i new file mode 100644 index 0000000000..4499f08504 --- /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/msgPayloadDefCpp/MJSysMassMatrixMsgPayload.h" +struct MJSysMassMatrixMsg_C; + +%pythoncode %{ +import sys +protectAllClasses(sys.modules[__name__]) +%} diff --git a/src/simulation/mujocoDynamics/MJSystemMassMatrix/MJSystemMassMatrix.rst b/src/simulation/mujocoDynamics/MJSystemMassMatrix/MJSystemMassMatrix.rst new file mode 100644 index 0000000000..c01d7b9001 --- /dev/null +++ b/src/simulation/mujocoDynamics/MJSystemMassMatrix/MJSystemMassMatrix.rst @@ -0,0 +1,51 @@ +Executive Summary +----------------- +The ``MJSystemMassMatrix`` module extracts the full system mass matrix. It also stores the number of spacecraft in the scene, indexes their starting joint, and the types of joints used. + +.. note:: + This module assumes that each body in the MuJoCo scene that is a direct child of the world body is a separate spacecraft so long as it has at least one joint. + If a free joint is used as part of a spacecraft, it must be used as the first joint of that spacecraft. + +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 + +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) 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..dc9b1e58cb --- /dev/null +++ b/src/simulation/mujocoDynamics/MJSystemMassMatrix/_UnitTest/test_MJSystemMassMatrix.py @@ -0,0 +1,262 @@ +# +# 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 numpy as np +import pytest + +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 + +xmlSingle = f""" + """ + +xmlOneHinge = f""" + """ + +xmlOneSlider = f""" + """ + +xmlDouble = f""" + """ + +xmlTranslation = f""" + """ + +def hingedMassMatrix(hubMass, hubInertia, armMass, armInertia, armLength=0.6): + """ + Helper function to compute the expected mass matrix for a hub with a hinged arm when the hinge angle is zero. + """ + M = np.zeros((7,7), float) + + # Hub translational mass + M[0:3,0:3] = hubMass * np.eye(3) + + # Hub rotational inertia + M[3:6,3:6] = hubInertia + + # Arm contribution + M[0:3,0:3] += armMass * np.eye(3) + M[3:6,3:6] += armInertia + M[4,4] += armMass * armLength**2 + M[5,5] += armMass * armLength**2 + M[2,4] = M[4,2] = -armMass * armLength + M[1,5] = M[5,1] = armMass * armLength + M[6,6] =M[6,5] = M[5,6] = armInertia[2,2] + + + return M + +def sliderMassMatrix(hubMass, hubInertia, armMass, armInertia, armLength=0.6): + """ + Helper function to compute the expected mass matrix for a hub with a sliding arm when the slide position is zero. + """ + M = np.zeros((7,7), float) + + # Hub translational mass + M[0:3,0:3] = hubMass * np.eye(3) + + # Hub rotational inertia + M[3:6,3:6] = hubInertia + + # Arm contribution + M[0:3,0:3] += armMass * np.eye(3) + M[3:6,3:6] += armInertia + M[4,4] += armMass * armLength**2 + M[5,5] += armMass * armLength**2 + M[2,4] = M[4,2] = armMass * armLength + M[1,5] = M[5,1] = -armMass * armLength + M[6,6] = M[0,6] = M[6,0] = armMass + + + return M + + +@pytest.mark.skipif(not couldImportMujoco, reason="Compiled Basilisk without --mujoco") +@pytest.mark.parametrize("modelName, xml, nSC, scStartIdx, jointTypes", [ + ("One SC", xmlSingle, 1, [0], [0]), + ("One Hinge", xmlOneHinge, 1, [0], [0,3]), + ("One Slider", xmlOneSlider, 1, [0], [0,2]), + ("Two SC", xmlDouble, 2, [0,1], [0,0]), + ("Translation SC Only", xmlTranslation, 1, [0], [2,2,2]) +]) + +def test_MJSystemMassMatrix(modelName, xml, nSC, scStartIdx, jointTypes): + 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: + modelName (str): the model of the spacecraft for this parameterized unit test + xml (str): the XML name for the spacecraft model + nSC (int): the expected number of spacecraft in the model + scStartIdx (list): the expected starting joint index for each spacecraft + jointTypes (list): the expected joint types in the model + + **Description of Variables Being Tested** + + In this test we are checking the system mass matrix values and properties to ensure it is accurate + """ + unitTaskName = "unitTask" + unitProcessName = "TestProcess" + + 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(xml) + 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() + unitTestSim.AddModelToTask(unitTaskName, massMatrixOutMsgRec) + + unitTestSim.InitializeSimulation() + unitTestSim.ConfigureStopTime(macros.sec2nano(0.1)) + unitTestSim.ExecuteSimulation() + + # Read from recorder + nSCOutMsgData = massMatrixOutMsgRec.nSC[0] + scStartIdxOutMsgData = massMatrixOutMsgRec.scStartIdx[0,:] + jointTypesOutMsgData = massMatrixOutMsgRec.jointTypes[0,:] + massMatrixOutMsgData = massMatrixOutMsgRec.massMatrix[0,:] + nDOF = int(np.sqrt(len(massMatrixOutMsgData))) + massMatrixOutMsgData = massMatrixOutMsgData.reshape((nDOF, nDOF), order='C') + + print("massMatrixOutMsgData = ", massMatrixOutMsgData) + + # Build the expected mass matrix + if modelName == "One SC": + massMatrixTruth = np.zeros((6,6), float) + massMatrixTruth[:3,:3] = HUB_MASS * np.eye(3) + massMatrixTruth[3:,3:] = HUB_I_COM + elif modelName == "One Hinge": + massMatrixTruth = hingedMassMatrix(HUB_MASS, HUB_I_COM, ARM_MASS, ROD_I_COM) + elif modelName == "One Slider": + massMatrixTruth = sliderMassMatrix(HUB_MASS, HUB_I_COM, ARM_MASS, ROD_I_COM) + elif modelName == "Two SC": + massMatrixTruth = np.zeros((12,12), float) + massMatrixTruth[:3,:3] = HUB_MASS * np.eye(3) + massMatrixTruth[3:6,3:6] = HUB_I_COM + massMatrixTruth[6:9,6:9] = HUB_MASS * np.eye(3) + massMatrixTruth[9:,9:] = HUB_I_COM + elif modelName == "Translation SC Only": + massMatrixTruth = HUB_MASS * np.eye(3) + + # Assert the number of spacecraft is correct + assert nSCOutMsgData == nSC, f"{modelName} number of spacecraft {nSCOutMsgData} does not match expected {nSC}" + + # Assert the starting joint indices are correct + np.testing.assert_array_equal(scStartIdxOutMsgData, np.array(scStartIdx), + err_msg=f"{modelName} starting joint indices mismatch") + + # Assert the joint types are correct + np.testing.assert_array_equal(jointTypesOutMsgData, np.array(jointTypes), + err_msg=f"{modelName} joint types mismatch") + + # Assert the mass matrix is correct + assert massMatrixOutMsgData == pytest.approx(massMatrixTruth, rel=1e-8), \ + f"{modelName} mass matrix {massMatrixOutMsgData} not close to expected {massMatrixTruth}" + + +if __name__ == "__main__": + # Run one case by hand + test_MJSystemMassMatrix( "One Hinge", xmlOneHinge, 1, [0], [0,3])