diff --git a/doc/APIreference/APIfunctions.rst b/doc/APIreference/APIfunctions.rst index c34be65b86..824221b62e 100644 --- a/doc/APIreference/APIfunctions.rst +++ b/doc/APIreference/APIfunctions.rst @@ -38,6 +38,7 @@ API function can be classified as: - **Math** - Aliases for C :ref:`standard math` functions. - :ref:`Vector math`. + - :ref:`Sparse math`. - :ref:`Quaternions`. - :ref:`Pose transformations`. - :ref:`Matrix decompositions and solvers`. diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index 1358aae5f6..441a72d1b1 100644 --- a/doc/APIreference/functions.rst +++ b/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 diff --git a/doc/includes/references.h b/doc/includes/references.h index 75e1419ba3..4da16480e9 100644 --- a/doc/includes/references.h +++ b/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]); diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 026abf0c91..5f60763e9c 100644 --- a/include/mujoco/mujoco.h +++ b/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. diff --git a/introspect/functions.py b/introspect/functions.py index 8142a70a2f..377ba4a609 100644 --- a/introspect/functions.py +++ b/introspect/functions.py @@ -7062,6 +7062,52 @@ ), 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', diff --git a/python/mujoco/bindings_test.py b/python/mujoco/bindings_test.py index edf9218a74..5448a609e1 100644 --- a/python/mujoco/bindings_test.py +++ b/python/mujoco/bindings_test.py @@ -1337,6 +1337,16 @@ def test_mju_mul_vec_mat_vec(self): 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]) diff --git a/python/mujoco/functions.cc b/python/mujoco/functions.cc index 1dd47eed4d..0770611dd6 100644 --- a/python/mujoco/functions.cc +++ b/python/mujoco/functions.cc @@ -1021,6 +1021,25 @@ PYBIND11_MODULE(_functions, pymodule) { }); Def(pymodule); + // Sparse math + DEF_WITH_OMITTED_PY_ARGS(traits::mju_sparse2dense, "nr", "nc")( + pymodule, + [](Eigen::Ref res, + Eigen::Ref mat, + Eigen::Ref rownnz, + Eigen::Ref rowadr, + Eigen::Ref 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(pymodule); Def(pymodule); diff --git a/src/engine/engine_util_sparse.h b/src/engine/engine_util_sparse.h index 8b252dc077..ae14cf7013 100644 --- a/src/engine/engine_util_sparse.h +++ b/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, - const int* rownnz, const int* rowadr, const int* colind); +MJAPI void mju_sparse2dense(mjtNum* res, const mjtNum* mat, int nr, int nc, + 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, diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 6562d6e042..9bd3c6bb8f 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -7220,6 +7220,9 @@ public unsafe struct mjvSceneState_ { [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);