Expose mju_sparse2dense to public API.

PiperOrigin-RevId: 688985153
Change-Id: I9559f1d47363d0afada80c86abdc729cd1910b63

doc/APIreference/APIfunctions.rst

@@ -38,6 +38,7 @@ API function can be classified as:
 - **Math**
    - Aliases for C :ref:`standard math<Standardmath>` functions.
    - :ref:`Vector math<Vectormath>`.
+   - :ref:`Sparse math<Sparsemath>`.
    - :ref:`Quaternions<Quaternions>`.
    - :ref:`Pose transformations<Poses>`.
    - :ref:`Matrix decompositions and solvers<Decompositions>`.

doc/APIreference/functions.rst

@@ -3440,6 +3440,19 @@ mju_transformSpatial
 Coordinate transform of 6D motion or force vector in rotation:translation format.
 rotnew2old is 3-by-3, NULL means no rotation; flg_force specifies force or motion type.
 
+.. _Sparsemath:
+
+Sparse math
+^^^^^^^^^^^
+.. _mju_sparse2dense:
+
+mju_sparse2dense
+~~~~~~~~~~~~~~~~
+
+.. mujoco-include:: mju_sparse2dense
+
+Convert matrix from sparse to dense.
+
 .. _Quaternions:
 
 Quaternions

doc/includes/references.h

@@ -3467,6 +3467,8 @@ void mju_sqrMatTD(mjtNum* res, const mjtNum* mat, const mjtNum* diag, int nr, in
 void mju_transformSpatial(mjtNum res[6], const mjtNum vec[6], int flg_force,
                           const mjtNum newpos[3], const mjtNum oldpos[3],
                           const mjtNum rotnew2old[9]);
+void mju_sparse2dense(mjtNum* res, const mjtNum* mat, int nr, int nc,
+                      const int* rownnz, const int* rowadr, const int* colind);
 void mju_rotVecQuat(mjtNum res[3], const mjtNum vec[3], const mjtNum quat[4]);
 void mju_negQuat(mjtNum res[4], const mjtNum quat[4]);
 void mju_mulQuat(mjtNum res[4], const mjtNum quat1[4], const mjtNum quat2[4]);

include/mujoco/mujoco.h

@@ -1081,6 +1081,13 @@ MJAPI void mju_transformSpatial(mjtNum res[6], const mjtNum vec[6], int flg_forc
                                 const mjtNum rotnew2old[9]);
 
 
+//---------------------------------- Sparse math ---------------------------------------------------
+
+// Convert matrix from sparse to dense.
+MJAPI void mju_sparse2dense(mjtNum* res, const mjtNum* mat, int nr, int nc,
+                            const int* rownnz, const int* rowadr, const int* colind);
+
+
 //---------------------------------- Quaternions ---------------------------------------------------
 
 // Rotate vector by quaternion.

introspect/functions.py

@@ -7062,6 +7062,52 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
          ),
          doc='Coordinate transform of 6D motion or force vector in rotation:translation format. rotnew2old is 3-by-3, NULL means no rotation; flg_force specifies force or motion type.',  # pylint: disable=line-too-long
      )),
+    ('mju_sparse2dense',
+     FunctionDecl(
+         name='mju_sparse2dense',
+         return_type=ValueType(name='void'),
+         parameters=(
+             FunctionParameterDecl(
+                 name='res',
+                 type=PointerType(
+                     inner_type=ValueType(name='mjtNum'),
+                 ),
+             ),
+             FunctionParameterDecl(
+                 name='mat',
+                 type=PointerType(
+                     inner_type=ValueType(name='mjtNum', is_const=True),
+                 ),
+             ),
+             FunctionParameterDecl(
+                 name='nr',
+                 type=ValueType(name='int'),
+             ),
+             FunctionParameterDecl(
+                 name='nc',
+                 type=ValueType(name='int'),
+             ),
+             FunctionParameterDecl(
+                 name='rownnz',
+                 type=PointerType(
+                     inner_type=ValueType(name='int', is_const=True),
+                 ),
+             ),
+             FunctionParameterDecl(
+                 name='rowadr',
+                 type=PointerType(
+                     inner_type=ValueType(name='int', is_const=True),
+                 ),
+             ),
+             FunctionParameterDecl(
+                 name='colind',
+                 type=PointerType(
+                     inner_type=ValueType(name='int', is_const=True),
+                 ),
+             ),
+         ),
+         doc='Convert matrix from sparse to dense.',
+     )),
     ('mju_rotVecQuat',
      FunctionDecl(
          name='mju_rotVecQuat',

python/mujoco/bindings_test.py

@@ -1337,6 +1337,16 @@ Euler integrator, semi-implicit in velocity.
     mat = np.array([[1., 2., 3.], [4., 5., 6.], [7., 8., 9.]])
     self.assertEqual(mujoco.mju_mulVecMatVec(vec1, mat, vec2), 204.)
 
+  def test_mju_sparse_to_dense(self):
+    expected = np.array([[0., 1., 0.], [2., 0., 3.]])
+    mat = np.array((1., 2., 3.))
+    rownnz = np.array([1, 2])
+    rowadr = np.array([0, 1])
+    colind = np.array([1, 0, 2])
+    res = np.zeros((2, 3))
+    mujoco.mju_sparse2dense(res, mat, rownnz, rowadr, colind)
+    np.testing.assert_array_equal(res, expected)
+
   def test_mju_euler_to_quat(self):
     quat = np.zeros(4)
     euler = np.array([0, np.pi/2, 0])

python/mujoco/functions.cc

@@ -1021,6 +1021,25 @@ PYBIND11_MODULE(_functions, pymodule) {
       });
   Def<traits::mju_transformSpatial>(pymodule);
 
+  // Sparse math
+  DEF_WITH_OMITTED_PY_ARGS(traits::mju_sparse2dense, "nr", "nc")(
+      pymodule,
+      [](Eigen::Ref<EigenArrayXX> res,
+         Eigen::Ref<const EigenVectorX> mat,
+         Eigen::Ref<const EigenVectorI> rownnz,
+         Eigen::Ref<const EigenVectorI> rowadr,
+         Eigen::Ref<const EigenVectorI> colind) {
+        if (res.rows() != rownnz.size()) {
+          throw py::type_error("#rows in res should equal size of rownnz");
+        }
+        if (res.rows() != rowadr.size()) {
+          throw py::type_error("#rows in res should equal size of rowadr");
+        }
+        return ::mju_sparse2dense(res.data(), mat.data(), res.rows(),
+                                  res.cols(), rownnz.data(), rowadr.data(),
+                                  colind.data());
+      });
+
   // Quaternions
   Def<traits::mju_rotVecQuat>(pymodule);
   Def<traits::mju_negQuat>(pymodule);

src/engine/engine_util_sparse.h

@@ -38,8 +38,8 @@ MJAPI void mju_dense2sparse(mjtNum* res, const mjtNum* mat, int nr, int nc,
                             int* rownnz, int* rowadr, int* colind);
 
 // convert matrix from sparse to dense
-void mju_sparse2dense(mjtNum* res, const mjtNum* mat, int nr, int nc,
+MJAPI void mju_sparse2dense(mjtNum* res, const mjtNum* mat, int nr, int nc,
-                      const int* rownnz, const int* rowadr, const int* colind);
+                            const int* rownnz, const int* rowadr, const int* colind);
 
 // multiply sparse matrix and dense vector:  res = mat * vec
 MJAPI void mju_mulMatVecSparse(mjtNum* res, const mjtNum* mat, const mjtNum* vec,

unity/Runtime/Bindings/MjBindings.cs

@@ -7220,6 +7220,9 @@ public static unsafe extern void mju_sqrMatTD(double* res, double* mat, double*
 [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
 public static unsafe extern void mju_transformSpatial(double* res, double* vec, int flg_force, double* newpos, double* oldpos, double* rotnew2old);
 
+[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
+public static unsafe extern void mju_sparse2dense(double* res, double* mat, int nr, int nc, int* rownnz, int* rowadr, int* colind);
+
 [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
 public static unsafe extern void mju_rotVecQuat(double* res, double* vec, double* quat);