google / brax

Massively parallel rigidbody physics simulation on accelerator hardware.
Apache License 2.0
2.14k stars 234 forks source link

Mesh-Mesh Collision Problems #441

Closed jariasn closed 4 months ago

jariasn commented 5 months ago

Hi!

I was trying to simulate the collision between a teapot and a can. I run the simulation for about 30 time-steps but the objects don't collide with each other. However, the can and the teapot do collide with the plane. I checked the conaffinity and contype parameters, which should be correctly set. https://github.com/google/brax/assets/64325659/2d27afea-670c-4d2e-b865-1403590943c7

The relevant XML file is:

<mujoco model="ur5">
  <compiler angle="radian" autolimits="true"/>
  <statistic meansize="0.173315" extent="1.0834" center="0.408334 0.0869587 0.0891592"/>

  <default>
    <joint armature='0.04' damping="1" limited="true"/>
    <geom friction=".8 .1 .1" density="300" margin="0.002" condim="1" contype="0" conaffinity="0"/>
  </default>

  <option gravity="0 0 -9.81" timestep="0.01" iterations="4" />

  <asset>
      <include file="incl/table_assets.xml"/>
      <include file="incl/robot_assets.xml"/>
      <include file="incl/object_assets.xml"/>
  </asset>

  <worldbody>
      <light diffuse=".5 .5 .5" pos="0 0 3" dir="0 0 -1"/>
      <geom conaffinity="1" condim="3" friction="1 .1 .1" material="MatGnd2" name="floor" pos="0 0 0" size="20 20 0.125" type="plane"/>
      <include file="incl/robot.xml"/>
      <include file="incl/object.xml"/>

      <body name="teapot" pos="4 0 0.15">
        <geom name="teapot" pos="0 0 0" type="mesh" contype="1" mesh="teapot"/>
        <joint armature="0" damping="0" limited="false" margin="0.01" name="teapot" pos="0 0 0" type="free"/>
      </body>

      <body name="can" pos="4 0 0.08">
        <geom name="can" pos="0 0 0" type="mesh" contype="1" conaffinity="1" mesh="can"/>
        <joint armature="0" damping="0" limited="false" margin="0.01" name="can" pos="0 0 0" type="free"/>
      </body>
  </worldbody>

  <include file="incl/actuators.xml"/>

</mujoco>

To load this model, I use this script.

Another problem that I encountered when trying to load two bodies with <geom type="mesh"> is that the simulation freezes and it starts consuming a lot of resources. It freezes at:

I0117 12:04:46.256506 8010682368 mesh.py:242] Converting mesh (-5009138114913360041, 1111321369098598208) into convex hull.
I0117 12:04:46.807301 8010682368 mesh.py:242] Converting mesh (-3756864523981354566, 795501625399689805) into convex hull.
I0117 12:04:46.840853 8010682368 xla_bridge.py:660] Unable to initialize backend 'cuda': 
I0117 12:04:46.840950 8010682368 xla_bridge.py:660] Unable to initialize backend 'rocm': module 'jaxlib.xla_extension' has no attribute 'GpuAllocatorConfig'
I0117 12:04:46.841929 8010682368 xla_bridge.py:660] Unable to initialize backend 'tpu': INTERNAL: Failed to open libtpu.so: dlopen(libtpu.so, 0x0001): tried: 'libtpu.so' (no such file), '/System/Volumes/Preboot/Cryptexes/OSlibtpu.so' (no such file), '/Users/usr/miniconda3/bin/../lib/libtpu.so' (no such file), '/usr/lib/libtpu.so' (no such file, not in dyld cache), 'libtpu.so' (no such file), '/usr/local/lib/libtpu.so' (no such file), '/usr/lib/libtpu.so' (no such file, not in dyld cache)

This happens only when I try to simulate these bodies using the visualizer.py script. In order for me to visualize more than one body with <geom type="mesh"> I need to use the HTML tool provided. Why can't I load them using the visualizer.py script? The XML I used is:

<mujoco model="teapot_can">
  <compiler angle="degree" coordinate="local" inertiafromgeom="true"/>
  <option gravity="0 0 -9.81" timestep="0.01" iterations="4" integrator="Euler" />
  <custom>
  </custom>
  <default>
    <joint armature='0.04' damping="1" limited="true"/>
    <geom friction="1 .1 .1" density="300" margin="0.002" condim="6" contype="0" conaffinity="0"/>

  </default>
  <asset>
    <texture builtin="gradient" height="100" rgb1="1 1 1" rgb2="0 0 0" type="skybox" width="100"/>
    <texture builtin="flat" height="1278" mark="cross" markrgb="1 1 1" name="texgeom" random="0.01" rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" type="cube" width="127"/>
    <texture builtin="checker" height="100" name="texplane" rgb1="0 0 0" rgb2="0.8 0.8 0.8" type="2d" width="100"/>
    <material name="MatPlane" reflectance="0.5" shininess="1" specular="1" texrepeat="60 60" texture="texplane"/>
    <material name="geom" texture="texgeom" texuniform="true"/>
    <mesh name="teapot" file="stl/utahteapot.stl" scale="0.1 0.1 0.1"/>
    <mesh name="can" file="obj/tomato_soup_can.obj" scale="5 5 5"/>
  </asset>
  <worldbody>
    <light cutoff="100" diffuse="1 1 1" dir="-0 0 -1.3" directional="true" exponent="1" pos="0 0 1.3" specular=".1 .1 .1"/>
    <geom conaffinity="1" condim="3" material="MatPlane" name="floor" pos="0 0 0" size="40 40 40" type="plane"/>
    <body name="teapot" pos="2 0 0.75">
      <geom name="teapot" pos="0 0 0" type="mesh" contype="1" mesh="teapot"/>
      <joint armature="0" damping="0" limited="false" margin="0.01" name="teapot" pos="0 0 0" type="free"/>
    </body>

    <body name="can" pos="0 0 0.2">
      <geom name="can" pos="0 0 0" type="mesh" contype="1" conaffinity="1" mesh="can"/>
      <joint armature="0" damping="0" limited="false" margin="0.01" name="can" pos="0 0 0" type="free"/>
    </body>
  </worldbody>
  <actuator>
  </actuator>
</mujoco>

If I comment out any of the two bodies, the simulation works without any problems.

Thank you in advance for your help!

btaba commented 4 months ago

Hi @jariasn

[1] I loaded a stripped down version of your example in brax with http://localhost:8080/simulate/example.xml and it looks good to me

<mujoco model="ur5">
  <compiler angle="radian" autolimits="true"/>

  <default>
    <joint armature='0.04' damping="1" limited="true"/>
    <geom friction=".8 .1 .1" density="300" margin="0.002" contype="0" conaffinity="0"/>
  </default>

  <option gravity="0 0 -9.81" timestep="0.001" iterations="4" />

  <asset>
      <mesh file="/abspathto/mujoco/mjx/mujoco/mjx/test_data/meshes/pyramid.stl"/>
      <mesh file="/abspathto/mujoco/mjx/mujoco/mjx/test_data/meshes/tetrahedron.stl"/>
  </asset>

  <worldbody>
      <light diffuse=".5 .5 .5" pos="0 0 0" dir="0 0 -1"/>
      <geom conaffinity="1" friction="1 .1 .1"  name="floor" pos="0 0 0" size="20 20 0.125" type="plane"/>

      <body name="teapot" pos="4 0 2">
        <geom name="teapot" pos="0 0 0" type="mesh" contype="1" mesh="pyramid" mass="0.1"/>
        <joint armature="0" damping="0" limited="false" margin="0.01" name="teapot" pos="0 0 0" type="free"/>
      </body>

      <body name="can" pos="4 0 1">
        <geom name="can" pos="0 0 0" type="mesh" contype="1" conaffinity="1" mesh="tetrahedron" mass="0.1"/>
        <joint armature="0" damping="0" limited="false" margin="0.01" name="can" pos="0 0 0" type="free"/>
      </body>
  </worldbody>
</mujoco>

Not that I lowered the timestep since the simulation was unstable at 0.01.

[2] Which endpoint are you using with the visualizer? The play endpoint should effectively do a very similar thing as IPython.display.HTML in colab (I'm assuming that's what you mean by "HTML tool").

Note that mesh collisions are not performant in Brax as per this README. We are actively working to speed these collisions up.