google-deepmind
mujoco.mj_geomDistance() returns negative values for non-colliding (sphere, hfield) geom pairs
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
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.
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
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
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).
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
Sorry to bother, I had ran into a similar problem, but on opposite, my code is test for collision computation.
here is a minimal example to reproduce the bug:
import mujoco
import numpy as np
mjcf = """
<mujoco>
<option gravity = "0 0 -4"/>
<worldbody>
<geom name="floor" type="plane" pos="0 0 0" size="10 10 0.05"/>
<body name="cube1" pos="0 0 0.5">
<geom name="box1" type="box" mass="10" size="0.5 0.5 0.5" rgba="1 0 0 0.5"
solref="0.02 1" solimp="0.99 0.99 0.001 0.5 1" contype="1" conaffinity="1"/>
</body>
<body name="cube2" pos="0 0 10.25">
<freejoint/>
<geom name="box2" type="box" mass="1" size="0.25 0.25 0.25" rgba="0 0 1 0.5"
solref="0.02 1" solimp="0.99 0.99 0.001 0.5 1" contype="1" conaffinity="1"/>
</body>
</worldbody>
</mujoco>
"""
model = mujoco.MjModel.from_xml_string(mjcf)
data = mujoco.MjData(model)
n = 0
mujoco.mj_resetData(model, data)
dist = []
qvel = []
qacc = []
force = []
qfrc_constraint = []
while n<10000:
mujoco.mj_step(model, data)
qvel.append(data.qvel[2])
qacc.append(data.qacc[2])
qfrc_constraint.append(data.qfrc_constraint[2])
# 获取几何体ID
geom1_id = 1
geom2_id = 2
fromto = np.zeros(6, dtype=np.float64)
distmax = 10.0
dist0 = mujoco.mj_geomDistance(model, data, 1, 2, distmax,fromto)
dist.append(dist0)
if data.ncon > 0:
force0 = np.zeros(6)
mujoco.mj_contactForce(model, data, 0, force0)
force.append(force0[0])
else:
force.append(0)
n += 1
force = np.array(force)
qacc = np.array(qacc)
qvel = np.array(qvel)
dist = np.array(dist)
start = 1088
end = start + 10
for i in range(10):
print(dist[start:end][i],qvel[start:end][i],qacc[start:end][i],force[start:end][i],qfrc_constraint[start:end][i])
After run the code, you can see that although the distance is below 0, but the force remains 0. I suppose the force will always be greater than 0 if the distance is below 0.
Do you have any idea
I suppose the force will always be greater than 0 if the distance is below 0.
This is not necessarily true. For example if the relative velocity is positive.
@yuvaltassa Thanks for answering, could you see the issue in https://github.com/google-deepmind/mujoco/issues/2696？ I want to verify the equation below, but failed.