Mujoco System Mass Matrix Module

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``.

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 <vector>
+
+/*! @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<int> scStartIdx;    //!< [-] list of the first joint index for each spacecraft
+    std::vector<int> jointTypes;    //!< [-] list of the joint types (0:free,1:ball,2:slide,3:hinge) in the system
+    std::vector<double> massMatrix; //!< [-] mass matrix of the system in row-major order
+
+} MJSysMassMatrixMsgPayload;
+
+#endif

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 <iostream>
+#include <cstring>
+#include <mujoco/mujoco.h>
+#include <iomanip>
+
+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<std::size_t>(model->nv);
+
+    // determine the joint types in the system
+    this->jointTypes.clear();
+    this->jointTypes.reserve(static_cast<std::size_t>(model->njnt));
+    for (int j = 0; j < model->njnt; ++j) {
+        const int jt = model->jnt_type[j];
+        this->jointTypes.push_back(static_cast<std::size_t>(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<std::size_t>(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<mjtNum> Mdense(NN, mjtNum(0));
+    mj_fullM(model, Mdense.data(), data->qM);
+
+
+    // write to the output message
+    payload.nSC = static_cast<int>(this->nSC);
+    payload.scStartIdx.reserve(this->scStartIdx.size());
+    for (auto idx : this->scStartIdx) {
+        payload.scStartIdx.push_back(static_cast<int>(idx));
+    }
+    payload.jointTypes.reserve(this->jointTypes.size());
+    for (auto jt : this->jointTypes) {
+        payload.jointTypes.push_back(static_cast<int>(jt));
+    }
+    payload.massMatrix.assign(Mdense.begin(), Mdense.end());
+    this->massMatrixOutMsg.write(&payload, this->moduleID, CurrentSimNanos);
+}

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<MJSysMassMatrixMsgPayload> 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<std::size_t> scStartIdx;              //!< list of the first joint index for each spacecraft
+    std::vector<std::size_t> jointTypes;              //!< list of joint types in the system
+};
+
+
+#endif

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__])
+%}

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.
+    This module only allows free joints to be used as the base joint of each 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)