diff --git a/python/mujoco/bindings_test.py b/python/mujoco/bindings_test.py index e81bd3c2..1222569c 100644 --- a/python/mujoco/bindings_test.py +++ b/python/mujoco/bindings_test.py @@ -1135,6 +1135,41 @@ Euler integrator, semi-implicit in velocity. bodyexclude=0, geomid=geomid) + def test_mj_multi_ray(self): + nray = 3 + geom1 = np.zeros(1, np.int32) + pnt = np.array([-0.3, 0, 0.1]) + vec = np.array([[1, 0, 0], [0, 0, 1], [0, 0, -1]], np.float64) + dist_ex = np.array([0.2, -1, 0.1]) + geom_ex = np.array([1, -1, 0]) + geomid = np.zeros(nray, np.int32) + dist = np.zeros(nray, np.float64) + + mujoco.mj_forward(self.model, self.data) + mujoco.mj_multiRay( + m=self.model, + d=self.data, + pnt=pnt, + vec=vec.flatten(), + geomgroup=None, + flg_static=1, + bodyexclude=-1, + geomid=geomid, + dist=dist, + nray=nray, + cutoff=mujoco.mjMAXVAL) + + for i in range(0, 3): + self.assertEqual( + dist[i], + mujoco.mj_ray( + self.model, self.data, pnt, vec[i], None, 1, -1, geom1 + ), + ) + self.assertEqual(geomid[i], geom1) + self.assertEqual(geomid[i], geom_ex[i]) + self.assertAlmostEqual(dist[i], dist_ex[i]) + def test_inverse_fd_none(self): eps = 1e-6 flg_centered = 0 diff --git a/python/mujoco/functions.cc b/python/mujoco/functions.cc index cc8c774a..8e154e1d 100644 --- a/python/mujoco/functions.cc +++ b/python/mujoco/functions.cc @@ -24,6 +24,7 @@ #include #include #include +#include "errors.h" #include "function_traits.h" #include "functions.h" #include "private.h" @@ -38,6 +39,7 @@ PYBIND11_MODULE(_functions, pymodule) { namespace py = ::pybind11; namespace traits = python_traits; + using EigenVectorI = Eigen::Vector; using EigenVectorX = Eigen::Vector; using EigenArrayXX = Eigen::Array< mjtNum, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>; @@ -568,7 +570,25 @@ PYBIND11_MODULE(_functions, pymodule) { Def(pymodule); // Ray collision - Def(pymodule); + Def( + pymodule, + [](const raw::MjModel* m, raw::MjData* d, const mjtNum(*pnt)[3], + Eigen::Ref vec, + std::optional>> + geomgroup, + mjtByte flg_static, int bodyexclude, Eigen::Ref geomid, + Eigen::Ref dist, int nray, mjtNum cutoff) { + if (dist.size() != nray || geomid.size() != nray) { + throw py::type_error("dist and geomid should be of size nray"); + } + if (vec.size() != 3 * nray) { + throw py::type_error("vec should be of size 3*nray"); + } + InterceptMjErrors(::mj_multiRay)( + m, d, &(*pnt)[0], vec.data(), + geomgroup.has_value() ? geomgroup->data() : nullptr, flg_static, + bodyexclude, geomid.data(), dist.data(), nray, cutoff); + }); Def( pymodule, [](const raw::MjModel* m, const raw::MjData* d, const mjtNum(*pnt)[3], diff --git a/test/engine/engine_ray_test.cc b/test/engine/engine_ray_test.cc index 362d600f..65d85545 100644 --- a/test/engine/engine_ray_test.cc +++ b/test/engine/engine_ray_test.cc @@ -191,7 +191,7 @@ TEST_F(RayTest, MultiRayEqualsSingleRay) { } // compute intersections with multiray functions - mjtNum dist_multiray[3*N*M]; + mjtNum dist_multiray[N*M]; int rgeomid_multiray[N*M]; mj_multiRay(m, d, pnt, vec, NULL, 1, -1, rgeomid_multiray, dist_multiray, N * M, mjMAXVAL);