diff --git a/python/mujoco/bindings_test.py b/python/mujoco/bindings_test.py index d293448e..9c377ecf 100644 --- a/python/mujoco/bindings_test.py +++ b/python/mujoco/bindings_test.py @@ -1205,10 +1205,16 @@ Euler integrator, semi-implicit in velocity. def test_mjd_sub_quat(self): quat1 = np.array((0.2, 0.3, 0.3, 0.4)) - quat2 = np.array((0.2, 0.3, 0.3, 0.4)) + quat2 = np.array((0.1, 0.2, 0.4, 0.5)) d1 = np.empty(9, np.float64) d2 = np.empty(9, np.float64) mujoco.mjd_subQuat(quat1, quat2, d1, d2) + d3 = np.empty((3, 3), np.float64) + d4 = np.empty((3, 3), np.float64) + mujoco.mjd_subQuat(quat1, quat2, None, d3) + mujoco.mjd_subQuat(quat1, quat2, d4, None) + np.testing.assert_array_equal(d2, d3.flatten()) + np.testing.assert_array_equal(d1, d4.flatten()) def test_mjd_quat_intergrate(self): scale = 0.1 diff --git a/python/mujoco/functions.cc b/python/mujoco/functions.cc index 3d59f082..4c7bed7c 100644 --- a/python/mujoco/functions.cc +++ b/python/mujoco/functions.cc @@ -1353,7 +1353,28 @@ PYBIND11_MODULE(_functions, pymodule) { DsDa.has_value() ? DsDa->data() : nullptr, DmDq.has_value() ? DmDq->data() : nullptr); }); - Def(pymodule); + Def( + pymodule, + [](Eigen::Ref qa, Eigen::Ref qb, + std::optional> Da, + std::optional> Db) { + if (qa.size() != 4) { + throw py::type_error("qa must have size 4"); + } + if (qb.size() != 4) { + throw py::type_error("qb must have size 4"); + } + if (Da.has_value() && Da->size() != 9) { + throw py::type_error("Da must have size 9"); + } + if (Db.has_value() && Db->size() != 9) { + throw py::type_error("Db must have size 9"); + } + return InterceptMjErrors(::mjd_subQuat)( + qa.data(), qb.data(), + Da.has_value() ? Da->data() : nullptr, + Db.has_value() ? Db->data() : nullptr); + }); Def(pymodule); pymodule.def(