Mujoco System Mass Matrix Module

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

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

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 <iostream>
+#include <cstring>
+#include <mujoco/mujoco.h>
+#include <iomanip>
+
+/*! 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<mjtNum> 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);
+
+}

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 <Eigen/Dense>
+
+/*! @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<MJSysMassMatrixMsgPayload> 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

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

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)