Add mj_geomDistance API function and sensors for geometric distance, normal direction and segment between between two geoms. Fixes #51

PiperOrigin-RevId: 632458301
Change-Id: I65b7e5bb0df59008028114cf4a07e0d6365ff3d3

python/mujoco/bindings_test.py

@@ -1194,6 +1194,14 @@ Euler integrator, semi-implicit in velocity.
     mujoco.mjd_inverseFD(self.model, self.data, eps, flg_centered,
                          None, None, None, None, None, None, None)
 
+  def test_geom_distance(self):
+    mujoco.mj_forward(self.model, self.data)
+    fromto = np.empty(6, np.float64)
+    dist = mujoco.mj_geomDistance(self.model, self.data, 0, 2, 200, fromto)
+    self.assertEqual(dist, 41.9)
+    np.testing.assert_array_equal(fromto,
+                                  np.array((42., 0., 0., 42., 0., 41.9)))
+
   def test_inverse_fd(self):
     eps = 1e-6
     flg_centered = 0

python/mujoco/functions.cc

@@ -531,6 +531,18 @@ PYBIND11_MODULE(_functions, pymodule) {
   Def<traits::mj_objectVelocity>(pymodule);
   Def<traits::mj_objectAcceleration>(pymodule);
   Def<traits::mj_contactForce>(pymodule);
+  Def<traits::mj_geomDistance>(
+      pymodule,
+      [](const raw::MjModel* m, const raw::MjData* d,
+         int geom1, int geom2, mjtNum distmax,
+         std::optional<Eigen::Ref<EigenArrayXX>> fromto) {
+        if (fromto.has_value() && fromto->size() != 6) {
+          throw py::type_error("fromto should be of size 6");
+        }
+        return InterceptMjErrors(::mj_geomDistance)(
+            m, d, geom1, geom2, distmax,
+            fromto.has_value() ? fromto->data() : nullptr);
+      });
   Def<traits::mj_differentiatePos>(
       pymodule,
       [](const raw::MjModel* m, Eigen::Ref<EigenVectorX> qvel,