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

introspect/enums.py

@@ -338,9 +338,12 @@ ENUMS: Mapping[str, EnumDecl] = dict([
              ('mjSENS_SUBTREECOM', 34),
              ('mjSENS_SUBTREELINVEL', 35),
              ('mjSENS_SUBTREEANGMOM', 36),
-             ('mjSENS_CLOCK', 37),
-             ('mjSENS_PLUGIN', 38),
-             ('mjSENS_USER', 39),
+             ('mjSENS_GEOMDIST', 37),
+             ('mjSENS_GEOMNORMAL', 38),
+             ('mjSENS_GEOMFROMTO', 39),
+             ('mjSENS_CLOCK', 40),
+             ('mjSENS_PLUGIN', 41),
+             ('mjSENS_USER', 42),
          ]),
      )),
     ('mjtStage',

introspect/functions.py

@@ -2743,6 +2743,45 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
          ),
          doc='Compute object 6D acceleration (rot:lin) in object-centered frame, world/local orientation.',  # pylint: disable=line-too-long
      )),
+    ('mj_geomDistance',
+     FunctionDecl(
+         name='mj_geomDistance',
+         return_type=ValueType(name='mjtNum'),
+         parameters=(
+             FunctionParameterDecl(
+                 name='m',
+                 type=PointerType(
+                     inner_type=ValueType(name='mjModel', is_const=True),
+                 ),
+             ),
+             FunctionParameterDecl(
+                 name='d',
+                 type=PointerType(
+                     inner_type=ValueType(name='mjData', is_const=True),
+                 ),
+             ),
+             FunctionParameterDecl(
+                 name='geom1',
+                 type=ValueType(name='int'),
+             ),
+             FunctionParameterDecl(
+                 name='geom2',
+                 type=ValueType(name='int'),
+             ),
+             FunctionParameterDecl(
+                 name='distmax',
+                 type=ValueType(name='mjtNum'),
+             ),
+             FunctionParameterDecl(
+                 name='fromto',
+                 type=ArrayType(
+                     inner_type=ValueType(name='mjtNum'),
+                     extents=(6,),
+                 ),
+             ),
+         ),
+         doc='Returns smallest signed distance between two geoms and optionally segment from geom1 to geom2.',  # pylint: disable=line-too-long
+     )),
     ('mj_contactForce',
      FunctionDecl(
          name='mj_contactForce',