mujoco.mj_geomDistance() returns negative values for non-colliding (sphere, hfield) geom pairs

[7oponaut]

mujoco.mj_geomDistance() returns negative values for non-colliding (sphere, hfield) geom pairs.

In contrast, when testing with (sphere, plane) geom pairs, the function returns positive values as expected.

I am using the mujoco_menagerie/google_barkour_vb scenes for testing, since these include floor geoms of both the planar and hfield types.

To make the measurements, I added the attribute name="neck_geom" to the geom with the "neck" mesh in mujoco_menagerie/google_barkour_vb/barkour_vb_mjx.xml and then I ran the code I provided at the end of this report.

I raise the height of the freejoint of the robot before simulation because it gets spawned too low into the hfield by default.

For non-colliding (sphere, plane) geom pairs, mujoco.mj_geomDistance() returns positive values as expected. Example scene: mujoco_menagerie/google_barkour_vb/scene_mjx.xml

For non-colliding (sphere, hfield) geom pairs, mujoco.mj_geomDistance() returns negative values. Example scene: mujoco_menagerie/google_barkour_vb/scene_hfield_mjx.xml

According to the code linked from the documentation, the mjc_ConvexHField() function handles these collisions.

mujoco.mj_geomDistance() produces negative falues for (sphere, hfield) geom pairs both with and without jax acceleration.

Video of the hfield scene:

https://github.com/google-deepmind/mujoco/assets/174741646/27e613a0-a72a-4712-aa40-21730fd7c70b

The code I used for generating the figures and the video:

import argparse

import cv2
import matplotlib.pyplot as plt
import mujoco

parser = argparse.ArgumentParser()
parser.add_argument(
    "--hfield", action="store_true", help="Use hfield scene as opposed to planar scene"
)
parser.add_argument("--jax", action="store_true", help="Use jax acceleration")
parser.add_argument(
    "--elevate", type=float, help="Set freejoint z coordinate to supplied value"
)
args = parser.parse_args()

width, height = 640, 480
distmax = 10.0

hfield_str = "_hfield" if args.hfield else ""
mj_model = mujoco.MjModel.from_xml_path(f"google_barkour_vb/scene{hfield_str}_mjx.xml")
mj_data = mujoco.MjData(mj_model)
renderer = mujoco.Renderer(mj_model, height=height, width=width)

duration = 3.8  # (seconds)
framerate = 60  # (Hz)

heights = []
distances = []
frames = []
mujoco.mj_resetData(mj_model, mj_data)

if args.elevate is not None:
    mj_data.qpos[2] = args.elevate

if not args.jax:
    while mj_data.time < duration:
        mujoco.mj_step(mj_model, mj_data)
        heights.append(mj_data.geom("neck_geom").xpos[2])
        distance = mujoco.mj_geomDistance(
            mj_model,
            mj_data,
            mj_model.geom("neck_geom").id,
            mj_model.geom("floor").id,
            distmax,
            None,
        )
        distances.append(distance)
        if len(frames) < mj_data.time * framerate:
            renderer.update_scene(mj_data)
            pixels = renderer.render()
            frames.append(pixels)
else:
    import jax
    from mujoco import mjx

    jit_step = jax.jit(mjx.step)
    mjx_model = mjx.put_model(mj_model)
    mjx_data = mjx.put_data(mj_model, mj_data)

    while mjx_data.time < duration:
        mjx_data = jit_step(mjx_model, mjx_data)
        mjx.get_data_into(mj_data, mj_model, mjx_data)
        heights.append(mj_data.geom("neck_geom").xpos[2])
        distance = mujoco.mj_geomDistance(
            mj_model,
            mj_data,
            mj_model.geom("neck_geom").id,
            mj_model.geom("floor").id,
            distmax,
            None,
        )
        distances.append(distance)
        if len(frames) < mjx_data.time * framerate:
            renderer.update_scene(mj_data)
            pixels = renderer.render()
            frames.append(pixels)

renderer.close()

device_suffix = "mjx" if args.jax else "mj"
hfield_suffix = f"hfield{'O' if args.hfield else 'X'}"
elevate_suffix = f"elevate{'X' if args.elevate is None else args.elevate}"
save_suffix = "_".join([hfield_suffix, device_suffix, elevate_suffix])

plt.title(save_suffix)
plt.plot(heights, label="height")
plt.plot(distances, label="distance")
plt.xlabel("frame")
plt.ylabel("value")
plt.legend()
plt.savefig(f"figure_{save_suffix}.png")

codec = cv2.VideoWriter_fourcc(*"mp4v")
video_writer = cv2.VideoWriter(f"video_{save_suffix}.mp4", codec, 60, (width, height))
for frame in frames:
    video_writer.write(frame[:, :, [2, 1, 0]])
video_writer.release()

Planar and hfield configurations respecitvely:

python test.py --elevate 1
python test.py --elevate 1 --hfield

OS: ArchLinux 2024.07.01 MuJoCo version: 3.1.6 mujoco_menagerie commit id: 143ae53 Using mujoco python bindings

[yuvaltassa]

Thanks for this. We are currently in the process of significantly improving the collision functions which should hopefully solve this. We will make sure to include height fields in our tests.

[nikisalli]

I have a similar problem too, the fromto and distance sensors and the mj_geomDistance function all return wrong values for non plane geometries as can be seen visually from the following picture: The segments going from the body ovals to the flat floor are correct but the ones connecting the ovals with the box geometries are always wrong

[nikisalli]

here is a minimal example to reproduce the bug:

import mujoco.viewer
import time

m = mujoco.MjModel.from_xml_string(
    """
    <mujoco>
        <asset>
            <texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="3072"/>
            <texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3" markrgb="0.8 0.8 0.8" width="300" height="300"/>
            <material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.2"/>
        </asset>

        <worldbody>
            <geom name="floor" group="1" size="0 0 0.05" type="plane" material="groundplane"/>
            <body name="test" pos="-0.5 0 0">
                <geom type='box' size='0.1 0.1 0.1' pos='0 0 0.2'/>
                <joint type="free"/>
            </body>
            <body name="test2" pos="0.5 0 0">
                <geom type='box' size='0.1 0.1 0.1' pos='0 0 0.2'/>
                <joint type="free"/>
            </body>
        </worldbody>

        <sensor>
            <fromto name="test" cutoff="10" body1="test" body2="test2"/>
        </sensor>
    </mujoco>
    """
)
d = mujoco.MjData(m)

with mujoco.viewer.launch_passive(m, d) as viewer:
    viewer.sync()
    start = time.time()
    while viewer.is_running():
        step_start = time.time()
        # Step the physics.
        mujoco.mj_step(m, d)
        viewer.sync()

        time_until_next_step = m.opt.timestep - (time.time() - step_start)
        if time_until_next_step > 0:
            time.sleep(time_until_next_step)

https://github.com/user-attachments/assets/fc023362-5846-4d56-a592-59c25a9e3e1b

[kbayes]

Hi @nikisalli,

For boxes, MuJoCo is using a bespoke box-box collider. I recommend trying meshes for boxes, so it forces the collider to use the more general MPR collider from libccd. I think that should give you better results for geom distances. In the case of contact, you might need to enable the multiccd flag to generate multiple contacts to give more stable contacts (the box-box collider by default gives multiple contacts).

[nikisalli]

thank you but libccd is too slow for my use case, I think I'll stick to multiple capsules to approximate my geometry as I see capsule-box colliders work much better than anything else