Penetration instead of collision

[NadeenElh]

Hi!

I'm a graduate student and I'm using MuJoCo for my research which requires peg-in-hole disassembly using Reinforcement Learning.

My setup

MuJoCo: 3.3.2 | Python | Ubuntu 20.04

My question

I'm testing using this model and the collision between the peg and hole worked fine when they were in their own xml file.

But, when I added a robot arm (Kuka IIWA 14), and ran the RL algorithm, the peg started penetrating the hole.
The peg is redefined as a child of the last link of the robot arm, like an extra link.
Is the penetration now due to the kinematic characteristics of the robot arm itself and the peg being a child part or is it an error in definition somehow? Is contact (contact forces) detectable between the peg and hole in this setting or does the peg have to be a (dynamically) free object for collision/contact to be valid?

If there's no way for it to be valid with this setting, would it be better to make it a separate body from the robot arm and use a weld equality on two sites instead?

I've attached the XML file.

Model:

minimal XML

<mujoco model="iiwa14_in_arena">
  <statistic center="0 0 0"/>
  
  <visual>
    <quality shadowsize="8192"/>
  </visual>
  <compiler angle="radian" meshdir="assets" autolimits="true"/>
  <option timestep="0.0005" integrator="implicitfast">
    <flag warmstart="enable" frictionloss="enable"/>
  </option>

  <default>
    <default class="visual">
      <geom group="2" type="mesh" contype="0" conaffinity="0" material="gray"/>
    </default>
    
    <default class="collision">
      <geom group="3" type="sphere" solimp="0.9 0.95 0.001 0.5 2" solref="0.02 1" condim="6" friction="1 0.5 0.1"/>
    </default>
    
    <!-- IIWA specific defaults -->
    <default class="iiwa">
      <material specular="0.5" shininess="0.25"/>
      <joint axis="0 0 1"/>
      <general gaintype="fixed" biastype="affine" gainprm="2000" biasprm="0 -2000 -200"/>
      <default class="joint1">
        <joint range="-2.96706 2.96706"/>
        <general ctrlrange="-2.96706 2.96706"/>
      </default>
      
      <default class="joint2">
        <joint range="-2.0944 2.0944"/>
        <general ctrlrange="-2.0944 2.0944"/>
      </default>
      
      <default class="joint3">
        <joint range="-3.05433 3.05433"/>
        <general ctrlrange="-3.05433 3.05433"/>
      </default>
      
        <site size="0.05" rgba="1 0 0 1" group="4"/>
    </default>
  </default>

  <!-- Combined assets -->
  <asset>
    <material name="None" specular="0.8000000000000002" shininess="0.5" rgba="0.8 0.8 0.8 1"/>
    <mesh file="peg_visual.obj"/>
    <mesh file="peg_collision_0.obj"/>
    <mesh file="peg_collision_1.obj"/>
    <mesh file="peg_collision_2.obj"/>
    <mesh file="peg_collision_3.obj"/>
    <mesh file="peg_collision_4.obj"/>
    <mesh file="peg_collision_5.obj"/>
    <mesh file="peg_collision_6.obj"/>
    <mesh file="peg_collision_7.obj"/>
    <mesh file="peg_collision_8.obj"/>
    <mesh file="hole_visual.obj"/>
    <mesh file="hole_collision_0.obj"/>
    <mesh file="hole_collision_1.obj"/>
    <mesh file="hole_collision_2.obj"/>
    <mesh file="hole_collision_3.obj"/>
    <mesh file="hole_collision_4.obj"/>
    <mesh file="hole_collision_5.obj"/>
    <mesh file="hole_collision_6.obj"/>
    <mesh file="hole_collision_7.obj"/>
    <mesh file="hole_collision_8.obj"/>
    <mesh file="hole_collision_9.obj"/>
    <mesh file="hole_collision_10.obj"/>
    <texture type="skybox" builtin="gradient" rgb1=".4 .6 .8" rgb2="0 0 0" width="100" height="100"/>
    <texture name="groundplane" type="2d" builtin="checker" rgb1=".2 .3 .4" rgb2=".1 .2 .3" width="300" height="300" mark="edge" markrgb=".8 .8 .8"/>
    <material name="groundplane" texture="groundplane" texrepeat="5 5" texuniform="true" reflectance=".2"/>
    
    <!-- IIWA assets -->
    <material class="iiwa" name="gray" rgba="0.4 0.4 0.4 1"/>
    <material class="iiwa" name="light_gray" rgba="0.6 0.6 0.6 1"/>
    <material class="iiwa" name="black" rgba="0 0 0 1"/>
    <material class="iiwa" name="orange" rgba="1 0.423529 0.0392157 1"/>

    <mesh file="link_0.obj"/>
    <mesh file="link_1.obj"/>
    <mesh file="link_2_orange.obj"/>
    <mesh file="link_2_grey.obj"/>
    <mesh file="link_3.obj"/>
    <mesh file="band.obj"/>
    <mesh file="kuka.obj"/>
    <mesh file="link_4_orange.obj"/>
    <mesh file="link_4_grey.obj"/>
    <mesh file="link_5.obj"/>
    <mesh file="link_6_orange.obj"/>
    <mesh file="link_6_grey.obj"/>
    <mesh file="link_7.obj"/>
  </asset>

  <!-- Combined worldbody -->
  <worldbody>
    <body name="hole" pos="0.7 0.0 0.017">
      <geom material="None" mesh="hole_visual" class="visual"/>
      <geom name="hole_geom_0" mesh="hole_collision_0" class="collision" contype = "1" conaffinity = "1"/>
      <geom name="hole_geom_1" mesh="hole_collision_1" class="collision" contype = "1" conaffinity = "1"/>
      <geom name="hole_geom_2" mesh="hole_collision_2" class="collision" contype = "1" conaffinity = "1"/>
      <geom name="hole_geom_3" mesh="hole_collision_3" class="collision" contype = "1" conaffinity = "1"/>
      <geom name="hole_geom_4" mesh="hole_collision_4" class="collision" contype = "1" conaffinity = "1"/>
      <geom name="hole_geom_5" mesh="hole_collision_5" class="collision" contype = "1" conaffinity = "1"/>
      <geom name="hole_geom_6" mesh="hole_collision_6" class="collision" contype = "1" conaffinity = "1"/>
      <geom name="hole_geom_7" mesh="hole_collision_7" class="collision" contype = "1" conaffinity = "1"/>
      <geom name="hole_geom_8" mesh="hole_collision_8" class="collision" contype = "1" conaffinity = "1"/>
      <geom name="hole_geom_9" mesh="hole_collision_9" class="collision" contype = "1" conaffinity = "1"/>
      <geom name="hole_geom_10" mesh="hole_collision_10" class="collision" contype = "1" conaffinity = "1"/>
    </body>
    
    <camera name="fixed_viewer" pos="0 -1.5 0.5" quat="0.7 0.7 0 0"/>
    <geom name="ground" type="plane" size="2 2 2" material="groundplane" friction="0.4"/>
    <light directional="false" diffuse=".7 .7 .7" pos="0 0 1.5" dir="0 0 -1" specular=".3 .3 .3" castshadow="true"/>
    
    <!-- IIWA robot -->
    <body name="base" childclass="iiwa" pos="0 0 0">
      <inertial mass="5" pos="-0.1 0 0.07" diaginertia="0.05 0.06 0.03"/>
      <geom class="visual" mesh="link_0"/>
      <geom name="base_0" class="collision" size="0.12 0.12 0.03" pos="0 0 0.03"/>
      <geom name="base_1" class="collision" size="0.08" pos="-0.08 0 0.103"/>
      <geom name="base_2" class="collision" size="0.08" pos="-0.08 0 0.04"/>
      <geom name="base_3" class="collision" size="0.1 0.05" pos="0 0 0.14" euler="0 0 0"/>
      <body name="link1" pos="0 0 0.1575">
        <inertial mass="5.76" pos="0 -0.03 0.12" diaginertia="0.0333 0.033 0.0123"/>
        <joint name="joint1" class="joint1"/>
        <geom class="visual" mesh="link_1"/>
        <geom name="link1_0" class="collision" size="0.08" pos="0 0 -0.0005"/>
        <geom name="link1_1" class="collision" size="0.075" pos="0.01 -0.025 0.0425"/>
        <geom name="link1_2" class="collision" size="0.075" pos="-0.01 -0.025 0.0425"/>
        <geom name="link1_3" class="collision" size="0.07" pos="0.01 -0.045 0.1025"/>
        <geom name="link1_4" class="collision" size="0.07" pos="-0.01 -0.045 0.1025"/>
        <body name="link2" pos="0 0 0.2025" quat="0 0 1 1">
          <inertial mass="6.35" pos="0.0003 0.059 0.042" diaginertia="0.0305 0.0304 0.011" quat="0 0 1 1"/>
          <joint name="joint2" class="joint2"/>
          <geom class="visual" material="orange" mesh="link_2_orange"/>
          <geom class="visual" mesh="link_2_grey"/>
          <geom name="link2_0" class="collision" size="0.095" pos="0 0 -0.01"/>
          <geom name="link2_1" class="collision" size="0.09" pos="0 0 0.045"/>
          <geom name="link2_2" class="collision" size="0.07" pos="-0.01 0.04 0.054"/>
          <geom name="link2_3" class="collision" size="0.065" pos="-0.01 0.09 0.04"/>
          <geom name="link2_4" class="collision" size="0.065" pos="-0.01 0.13 0.02"/>
          <geom name="link2_5" class="collision" size="0.07" pos="0.01 0.04 0.054"/>
          <geom name="link2_6" class="collision" size="0.065" pos="0.01 0.09 0.04"/>
          <geom name="link2_7" class="collision" size="0.065" pos="0.01 0.13 0.02"/>
          <geom name="link2_8" class="collision" size="0.075" pos="0 0.18 0"/>
          <body name="link3" pos="0 0.2045 0" quat="0 0 1 1">
            <inertial mass="3.5" pos="0 0.03 0.13" diaginertia="0.025 0.0238 0.0076"/>
            <joint name="joint3" class="joint1"/>
            <geom class="visual" mesh="link_3"/>
            <geom class="visual" material="light_gray" mesh="band"/>
            <geom class="visual" material="black" mesh="kuka"/>
            <geom name="link3_0" class="collision" size="0.075" pos="0 0 0.0355"/>
            <geom name="link3_1" class="collision" size="0.06" pos="0.01 0.023 0.0855"/>
            <geom name="link3_2" class="collision" size="0.055" pos="0.01 0.048 0.1255"/>
            <geom name="link3_3" class="collision" size="0.06" pos="0.01 0.056 0.1755"/>
            <geom name="link3_4" class="collision" size="0.06" pos="-0.01 0.023 0.0855"/>
            <geom name="link3_5" class="collision" size="0.055" pos="-0.01 0.048 0.1255"/>
            <geom name="link3_6" class="collision" size="0.06" pos="-0.01 0.056 0.1755"/>
            <geom name="link3_7" class="collision" size="0.075" pos="0 0.045 0.2155"/>
            <geom name="link3_8" class="collision" size="0.075" pos="0 0 0.2155"/>
            <body name="link4" pos="0 0 0.2155" quat="1 1 0 0">
              <inertial mass="3.5" pos="0 0.067 0.034" diaginertia="0.017 0.0164 0.006" quat="1 1 0 0"/>
              <joint name="joint4" class="joint2"/>
              <geom class="visual" material="orange" mesh="link_4_orange"/>
              <geom class="visual" mesh="link_4_grey"/>
              <geom name="link4_0" class="collision" size="0.078" pos="0 0.01 0.046"/>
              <geom name="link4_1" class="collision" size="0.06" pos="0.01 0.06 0.052"/>
              <geom name="link4_2" class="collision" size="0.065" pos="0.01 0.12 0.034"/>
              <geom name="link4_3" class="collision" size="0.06" pos="-0.01 0.06 0.052"/>
              <geom name="link4_4" class="collision" size="0.065" pos="-0.01 0.12 0.034"/>
              <geom name="link4_5" class="collision" size="0.075" pos="0 0.184 0"/>
              <body name="link5" pos="0 0.1845 0" quat="0 0 1 1">
                <inertial mass="3.5" pos="0.0001 0.021 0.076" diaginertia="0.01 0.0087 0.00449"/>
                <joint name="joint5" class="joint1"/>
                <geom class="visual" mesh="link_5"/>
                <geom class="visual" material="light_gray" mesh="band"/>
                <geom class="visual" material="black" mesh="kuka"/>
                <geom name="link5_0" class="collision" size="0.075" pos="0 0 0.0335"/>
                <geom name="link5_1" class="collision" size="0.05" pos="-0.012 0.031 0.0755"/>
                <geom name="link5_2" class="collision" size="0.05" pos="0.012 0.031 0.0755"/>
                <geom name="link5_3" class="collision" size="0.04" pos="-0.012 0.06 0.1155"/>
                <geom name="link5_4" class="collision" size="0.04" pos="0.012 0.06 0.1155"/>
                <geom name="link5_5" class="collision" size="0.04" pos="-0.01 0.065 0.1655"/>
                <geom name="link5_6" class="collision" size="0.04" pos="0.01 0.065 0.1655"/>
                <geom name="link5_7" class="collision" size="0.035" pos="-0.012 0.065 0.1855"/>
                <geom name="link5_8" class="collision" size="0.035" pos="0.012 0.065 0.1855"/>
                <body name="link6" pos="0 0 0.2155" quat="1 1 0 0">
                  <inertial mass="1.8" pos="0 0.0006 0.0004" diaginertia="0.0049 0.0047 0.0036" quat="1 1 0 0"/>
                  <joint name="joint6" class="joint2"/>
                  <geom class="visual" material="orange" mesh="link_6_orange"/>
                  <geom class="visual" mesh="link_6_grey"/>
                  <geom name="link6_0" class="collision" size="0.055" pos="0 0 -0.059"/>
                  <geom name="link6_1" class="collision" size="0.065" pos="0 -0.03 0.011"/>
                  <geom name="link6_2" class="collision" size="0.08"/>
                  <body name="link7" pos="0 0.081 0" quat="0 0 1 1">
                    <inertial mass="1.2" pos="0 0 0.02" diaginertia="0.001 0.001 0.001"/>
                    <joint name="joint7" class="joint3"/>
                    <geom class="visual" mesh="link_7"/>
                    <geom name="link7_0" class="collision" size="0.06" pos="0 0 0.001"/>
                      <body name="peg" pos="0 0 0.112" quat="0 0 1 0">
                        <geom material="None" mesh="peg_visual" class="visual"/>
                        <geom name="peg_geom_0" mesh="peg_collision_0" class="collision" contype = "1" conaffinity = "1"/>
                        <geom name="peg_geom_1" mesh="peg_collision_1" class="collision" contype = "1" conaffinity = "1"/>
                        <geom name="peg_geom_2" mesh="peg_collision_2" class="collision" contype = "1" conaffinity = "1"/>
                        <geom name="peg_geom_3" mesh="peg_collision_3" class="collision" contype = "1" conaffinity = "1"/>
                        <geom name="peg_geom_4" mesh="peg_collision_4" class="collision" contype = "1" conaffinity = "1"/>
                        <geom name="peg_geom_5" mesh="peg_collision_5" class="collision" contype = "1" conaffinity = "1"/>
                        <geom name="peg_geom_6" mesh="peg_collision_6" class="collision" contype = "1" conaffinity = "1"/>
                        <geom name="peg_geom_7" mesh="peg_collision_7" class="collision" contype = "1" conaffinity = "1"/>
                        <geom name="peg_geom_8" mesh="peg_collision_8" class="collision" contype = "1" conaffinity = "1"/>
                          <site name="peg_tip" type="sphere" pos="0 0 -0.00005" size="0.0025" rgba="1 0 0 1"/>
                      </body>
                  </body>
                </body>
              </body>
            </body>
          </body>
        </body>
      </body>
    </body>                                               

  </worldbody>

  <!-- IIWA contact exclusions -->
  <contact>
    <exclude body1="base" body2="link1"/>
    <exclude body1="base" body2="link2"/>
    <exclude body1="base" body2="link3"/>
    <exclude body1="link1" body2="link3"/>
    <exclude body1="link3" body2="link5"/>
    <exclude body1="link4" body2="link7"/>
    <exclude body1="link5" body2="link7"/>
    <exclude body1="link6" body2="hole"/>
    <exclude body1="link7" body2="hole"/>
  </contact>

  <!-- IIWA actuators -->
  <actuator>
    <general name="actuator1" joint="joint1" class="joint1"/>
    <general name="actuator2" joint="joint2" class="joint2"/>
    <general name="actuator3" joint="joint3" class="joint1"/>
    <general name="actuator4" joint="joint4" class="joint2"/>
    <general name="actuator5" joint="joint5" class="joint1"/>
    <general name="actuator6" joint="joint6" class="joint2"/>
    <general name="actuator7" joint="joint7" class="joint3"/>
  </actuator>

  <keyframe>
    <key name="home" 
         qpos="-1.4e-05              
            1.28          
            9.57e-05         
            -1.1746807           
            0.000107      
            0.879      
            3.42e-08"
         ctrl="0 1.08 0 -1.15 0 0.8 0"/>
  </keyframe>

</mujoco>

### Confirmations

-  I searched the [latest documentation](https://mujoco.readthedocs.io/en/latest/overview.html) thoroughly before posting.
-  I searched previous [Issues](https://github.com/google-deepmind/mujoco/issues) and [Discussions](https://github.com/google-deepmind/mujoco/discussions), I am certain this has not been raised before.

[v-r-a]

Hi,

I gave it a try and found that there is something wrong with the Kuka IIWA 14 model. I attached the peg to UR5e and it worked well.

I couldn't figure out exactly, but I think this is due to the default definitions overriding the meshes in the collision class.

See below:

No contact:

Contact of the spheres defined by default in the collision class:

You could make a new collision class or just remove the collision class from the peg and hole and redefine it as follows to make it work:
<geom name="peg_geom_0" type="mesh" mesh="peg_collision_0" />

[NadeenElh]

Hi! Thanks for taking the time to recreate and answer!

I'm honestly not sure what's the reason behind KUKA's ability to override this either. Thanks for the UR5e tip! Maybe I can switch.

[v-r-a]

In the second look, I just realised that I unnecesarily got confused! There is nothing wrong with the KUKA xml.

One should define the geom type="mesh" before defining the mesh itself, which has not been done for the peg and the hole geoms. You just have to insert type="mesh" in the peg and hole collision geoms. Rest of the solref, solimp and friction properties will be inherited from the collision class or you could redefine them in a new class and use it.

Moreover, default class="collision" is used which has type="sphere" defined. So it seems obvious for the model compiler to ignore the mesh files. I am now wondering why did it work for UR5e! I think one should not leave any room for undefined behaviour.

[NadeenElh]

Did adding the type of the geom as "mesh" solve it on your end?
I'm not sure it worked for me. Perhaps the collision class is still overriding the meshes?

[v-r-a]

Yes.

Tried with various MuJoCo versions 3.2.3 / 3.3.3 / older ones.

You could load the model, toggle the positions and check for yourself using simulate or https://mujoco.readthedocs.io/en/stable/python.html#standalone-app.

[NadeenElh]

It's interesting that when I tried this away from the reinforcement learning code, the collision worked fine. But when I run the training itself, it still penetrates.

Video: https://drive.google.com/drive/folders/1JZ3G2-S1aN2rJ_y_Jwcdlx85nX23MkPx?usp=sharing