diff --git a/doc/APIreference.rst b/doc/APIreference.rst index 7fb87909..adad1afd 100644 --- a/doc/APIreference.rst +++ b/doc/APIreference.rst @@ -5917,7 +5917,7 @@ mju_fill .. code-block:: C - void mju_fill(mjtNum* res, int n, mjtNum val); + void mju_fill(mjtNum* res, mjtNum val, int n); Set res = val. @@ -6101,9 +6101,9 @@ mju_symmetrize .. code-block:: C - void mju_symmetrize(mjtNum* mat, int n); + void mju_symmetrize(mjtNum* res, const mjtNum* mat, int n); -Symmetrize square matrix :math:`M = \frac{1}{2}(M + M^T)`. +Symmetrize square matrix :math:`R = \frac{1}{2}(M + M^T)`. .. _mju_eye: diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 95aa549c..d1dd98e4 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -872,7 +872,7 @@ MJAPI mjtNum mju_normalize4(mjtNum res[4]); MJAPI void mju_zero(mjtNum* res, int n); // Set res = val. -MJAPI void mju_fill(mjtNum* res, int n, mjtNum val); +MJAPI void mju_fill(mjtNum* res, mjtNum val, int n); // Set res = vec. MJAPI void mju_copy(mjtNum* res, const mjtNum* data, int n); @@ -925,8 +925,8 @@ MJAPI mjtNum mju_mulVecMatVec(const mjtNum* vec1, const mjtNum* mat, const mjtNu // Transpose matrix: res = mat'. MJAPI void mju_transpose(mjtNum* res, const mjtNum* mat, int nr, int nc); -// Symmetrize square matrix M = (M + M')/2. -MJAPI void mju_symmetrize(mjtNum* mat, int n); +// Symmetrize square matrix res = (mat + mat')/2. +MJAPI void mju_symmetrize(mjtNum* res, const mjtNum* mat, int n); // Set mat to the identity matrix. MJAPI void mju_eye(mjtNum* mat, int n); diff --git a/introspect/functions.py b/introspect/functions.py index ccf78002..65e14f56 100644 --- a/introspect/functions.py +++ b/introspect/functions.py @@ -5295,14 +5295,14 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ inner_type=ValueType(name='mjtNum'), ), ), - FunctionParameterDecl( - name='n', - type=ValueType(name='int'), - ), FunctionParameterDecl( name='val', type=ValueType(name='mjtNum'), ), + FunctionParameterDecl( + name='n', + type=ValueType(name='int'), + ), ), doc='Set res = val.', )), @@ -5756,17 +5756,23 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ return_type=ValueType(name='void'), parameters=( FunctionParameterDecl( - name='mat', + name='res', type=PointerType( inner_type=ValueType(name='mjtNum'), ), ), + FunctionParameterDecl( + name='mat', + type=PointerType( + inner_type=ValueType(name='mjtNum', is_const=True), + ), + ), FunctionParameterDecl( name='n', type=ValueType(name='int'), ), ), - doc="Symmetrize square matrix M = (M + M')/2.", + doc="Symmetrize square matrix res = (mat + mat')/2.", )), ('mju_eye', FunctionDecl( diff --git a/python/mujoco/bindings_test.py b/python/mujoco/bindings_test.py index 79ab3ad8..3673fc1f 100644 --- a/python/mujoco/bindings_test.py +++ b/python/mujoco/bindings_test.py @@ -1049,6 +1049,27 @@ Euler integrator, semi-implicit in velocity. rank = mujoco.mju_boxQP(res, r, index, h, g, lower, upper) self.assertGreater(rank, -1) + def test_mju_fill(self): + res = np.empty(3, np.float64) + mujoco.mju_fill(res, 1.5) + np.testing.assert_array_equal(res, np.full(3, 1.5)) + + def test_mju_eye(self): + eye4 = np.empty((4, 4), np.float64) + mujoco.mju_eye(eye4) + np.testing.assert_array_equal(eye4, np.eye(4)) + + def test_mju_symmetrize(self): + mat = np.linspace(0, 1, 16).reshape(4, 4) + res = np.empty((4, 4), np.float64) + mujoco.mju_symmetrize(res, mat) + np.testing.assert_array_equal(res, 0.5*(mat + mat.T)) + + def test_mju_clip(self): + self.assertEqual(mujoco.mju_clip(1.5, 1.0, 2.0), 1.5) + self.assertEqual(mujoco.mju_clip(1.5, 2.0, 3.0), 2.0) + self.assertEqual(mujoco.mju_clip(1.5, 0.0, 1.0), 1.0) + def test_mju_mul_vec_mat_vec(self): vec1 = np.array([1., 2., 3.]) vec2 = np.array([3., 2., 1.]) diff --git a/python/mujoco/functions.cc b/python/mujoco/functions.cc index 2e718316..02580c7f 100644 --- a/python/mujoco/functions.cc +++ b/python/mujoco/functions.cc @@ -632,6 +632,11 @@ PYBIND11_MODULE(_functions, pymodule) { [](Eigen::Ref res) { return InterceptMjErrors(::mju_zero)(res.data(), res.size()); }); + DEF_WITH_OMITTED_PY_ARGS(traits::mju_fill, "n")( + pymodule, + [](Eigen::Ref res, mjtNum val) { + return InterceptMjErrors(::mju_fill)(res.data(), val, res.size()); + }); DEF_WITH_OMITTED_PY_ARGS(traits::mju_copy, "n")( pymodule, [](Eigen::Ref res, @@ -817,6 +822,27 @@ PYBIND11_MODULE(_functions, pymodule) { return InterceptMjErrors(::mju_transpose)( res.data(), mat.data(), mat.rows(), mat.cols()); }); + DEF_WITH_OMITTED_PY_ARGS(traits::mju_symmetrize, "n")( + pymodule, + [](Eigen::Ref res, + Eigen::Ref mat) { + if (mat.cols() != mat.rows()) { + throw py::type_error("mat should be square"); + } + if (res.cols() != mat.cols() || res.rows() != mat.rows()) { + throw py::type_error("res and mat should have the same shape"); + } + return InterceptMjErrors(::mju_symmetrize)( + res.data(), mat.data(), mat.rows()); + }); + DEF_WITH_OMITTED_PY_ARGS(traits::mju_eye, "n")( + pymodule, + [](Eigen::Ref mat) { + if (mat.cols() != mat.rows()) { + throw py::type_error("mat should be square"); + } + return InterceptMjErrors(::mju_eye)(mat.data(), mat.rows()); + }); DEF_WITH_OMITTED_PY_ARGS(traits::mju_mulMatMat, "r1", "c1", "c2")( pymodule, [](Eigen::Ref res, @@ -1030,6 +1056,7 @@ PYBIND11_MODULE(_functions, pymodule) { Def(pymodule); Def(pymodule); Def(pymodule); + Def(pymodule); Def(pymodule); Def(pymodule); Def(pymodule); diff --git a/src/engine/engine_util_blas.c b/src/engine/engine_util_blas.c index 309a1ccf..54ad5625 100644 --- a/src/engine/engine_util_blas.c +++ b/src/engine/engine_util_blas.c @@ -245,7 +245,7 @@ void mju_zero(mjtNum* res, int n) { // res = val -void mju_fill(mjtNum* res, int n, mjtNum val) { +void mju_fill(mjtNum* res, mjtNum val, int n) { for (int i=0; i