deep box-box collisions

schlexl commented 4 months ago

Description: I am currently optimizing body/geom positions, but my solver converges to positions that are in clear and deep contact. According to the documentation, "For example in models where bodies are initialized in penetration, large repulsive forces could push them away and cause divergence.", but as far as I understood this should be not the problem in this case?

I have provided an XML configuration with two different geom positions, as well as a script to replay the Issue. When using the deeply penetrating geom (pos="1.10164554 -0.11389316 0.74"), no collisions are detected, even though the geomDistance is negative. This seems like an unintended behavior.

<mujoco model="tu_tisch_verstellbar">
    <compiler convexhull="true" angle="radian"/>
  <worldbody>
      <light pos="0 0 5.5" dir="0 0 -1" directional="true"/>
      <geom name="ground" type="plane" size="0 0 1" pos="0 0 0" quat="1 0 0 0"  contype="0" group="0" />

      <body name="tu_tisch_center_ground" pos="0 0 0.0">
          <geom name="tabletop_collision" pos="1.10164554 -0.11389316  0.74" quat="-3.48312918e-01 -1.42624718e-10 -6.59013393e-11  9.37378318e-01" type="box" contype="1" conaffinity="1" condim="3" size="0.65 0.48 0.04" group="0"/>
            <!--geom name="tabletop_collision" pos="0.68 -0.6  0.74" quat="-3.48312918e-01 -1.42624718e-10 -6.59013393e-11  9.37378318e-01" type="box" contype="1" conaffinity="1" condim="3" size="0.65 0.48 0.04" group="0"/-->
      </body>

      <body name="cage" pos="0 0 0.0">
        <geom name="cage_col_side_right" group="0" contype="1" conaffinity="1" condim="3" type="box" size="0.1 1.2 1.4" pos="1.4 0 1.425" rgba="0.8 0.6 0.4 1"/>
      </body>
  </worldbody>

  <contact>
    <pair name="test_collision1" geom1="tabletop_collision" geom2="cage_col_side_right" condim="3"/>
</contact>
</mujoco>

Replay script:

import mujoco
import mujoco.viewer

def display_mujoco_env(xml_path):
    with open(xml_path, 'r') as f:
            xml = f.read()
    model = mujoco.MjModel.from_xml_string(xml)
    data = mujoco.MjData(model)
    with mujoco.viewer.launch_passive(model, data) as viewer:
        while viewer.is_running():
            mujoco.mj_step(model, data)
            viewer.sync()
            geom1_id = int(model.geom("tabletop_collision").id)
            geom2_id = int(model.geom("cage_col_side_right").id)
            distance = mujoco.mj_geomDistance(model, data, geom1_id, geom2_id, 10, None)
            print(f"Distance: {distance}")
            print((data.contact.dist))
display_mujoco_env("test.xml")

deep penetration: deep_penetration

shallow penetration: shallow_penetration

Include the following context:
- Ubuntu 22.04.3 LTS
- mujoco 3.1.6
- Bindings: mujoco

Thank you in advance!

Edit: in mujoco version 3.0.0 both deep and shallow collisions get detected.

vanessastad commented 3 months ago

Hi

I am experiencing the same problem with Mujoco 3.1.5 The collision between my robot and a box is not detected when the box is thin. My workaround is to use a thicker box since I am using it as a transparent geometry to limit my workspace. Anyways, this is not the best solution.

yuvaltassa commented 3 months ago

We are working on a general re-write of the collision pipeline (ETA 1-2 months) that should fix this. These "bad" examples are extremely valuable for this purpose, so thanks (please provide more if you encounter them!). We will update here when the fix is in.

google-deepmind / mujoco

deep box-box collisions #1800