[Bug]: Contacts detection of feet and ground, sometimes wrong

[ucacaxm]

Bug Description

Hi,

I detect contact between feet and ground (flat).
I did it with 'get_contact' and inspect all the detected contact, and also with a sensor.

Most of the time, is is ok. But sometimes it is wrong like on the image. The left foot has the debug sphere of the sensor showing a contact but there is no contact.

Is it because of a delay between what is displayed and the simulation?

Thanks,

Steps to Reproduce

If possible, provide a script triggering the bug, e.g.

import genesis as gs
import numpy as np

gs.init(backend=gs.gpu)

scene = gs.Scene(
    viewer_options=gs.options.ViewerOptions(
        camera_pos=(3, -1, 1.5),
        camera_lookat=(0, 0, 0.5),
        camera_fov=40,
    ),
    show_viewer=True,
)

scene.profiling_options.show_FPS = False

plane = scene.add_entity(
    gs.morphs.Plane(),
)

robot = scene.add_entity(
        morph=gs.morphs.URDF(
            file="data/urdf/tux.urdf",
            pos=(0, 0, 2.0),
            scale=1.0,
            links_to_keep = {"left_foot", "right_foot"},
        ),
    )

leftfoot_link = robot.get_link('left_foot')
rightfoot_link = robot.get_link('right_foot')




sensor_options = gs.sensors.Contact(
    entity_idx=robot.idx,
    link_idx_local=leftfoot_link.idx_local,
    draw_debug=True,
)
sensor = scene.add_sensor(sensor_options)

scene.build( )


for i in range(1000):
    scene.step()

    print(sensor.read())

    #ground_contacts = plane.get_contacts( with_entity=leftfoot_link.entity, exclude_self_contact=True )
    ground_contacts = leftfoot_link.entity.get_contacts( with_entity=plane, exclude_self_contact=False )

    if leftfoot_link.idx in ground_contacts["link_a"] or leftfoot_link.idx in ground_contacts["link_b"]:
        leftfoot_ground_contacts = True
    else:
        leftfoot_ground_contacts = False

    if rightfoot_link.idx in ground_contacts["link_a"] or rightfoot_link.idx in ground_contacts["link_b"]:
        rightfoot_ground_contacts = True
    else:
        rightfoot_ground_contacts = False

    print("contact: l", leftfoot_ground_contacts, "  r=", rightfoot_ground_contacts)

    

Make sure to attached any needed assets (here attachement.xml)!

Expected Behavior

Get correct contact information

Screenshots/Videos

No response

Relevant log output

Environment

• OS: Ubuntu 24.04
• GPU/CPU RTX 4090

Release version or Commit ID

[Genesis] [13:25:49] [INFO] Running on [NVIDIA GeForce RTX 4090] with backend gs.cuda. Device memory: 23.64 GB.
[Genesis] [13:25:49] [INFO] 🚀 Genesis initialized. 🔖 version: 0.3.4, 🌱 seed: None, 📏 precision: '32', 🐛 debug: False, 🎨 theme: 'dark'.

Additional Context

No response

[duburcqa]

I will need an entire runnable script to debug this.

[ucacaxm]

Sorry, I simplified my code but it was too much.
Here the good version: random actions are applied.

import genesis as gs
import numpy as np
import torch
import math 

gs.init(backend=gs.gpu)

scene = gs.Scene(
    viewer_options=gs.options.ViewerOptions(
        camera_pos=(3, -1, 1.5),
        camera_lookat=(0, 0, 0.5),
        camera_fov=40,
    ),
    show_viewer=True,
)

scene.profiling_options.show_FPS = False

plane = scene.add_entity(
    gs.morphs.Plane(),
)

robot = scene.add_entity(
        morph=gs.morphs.URDF(
            file="data/urdf/locox.urdf",
            pos=(0, 0, 1.0),
            euler=(0, 0, 0),
            scale=1.0,
            links_to_keep = {"left_foot", "right_foot"},
        ),
    )

leftfoot_link = robot.get_link('left_foot')
rightfoot_link = robot.get_link('right_foot')




sensor_optionsL = gs.sensors.Contact(
    entity_idx=robot.idx,
    link_idx_local=leftfoot_link.idx_local,
    draw_debug=True,
)
sensor = scene.add_sensor(sensor_optionsL)

sensor_optionsR = gs.sensors.Contact(
    entity_idx=robot.idx,
    link_idx_local=rightfoot_link.idx_local,
    draw_debug=True,
)
sensor = scene.add_sensor(sensor_optionsR)

scene.build( )


joint_names =  {"right_hip_x",
                "right_hip_y",
                "right_hip_z",
                "right_knee",
                "right_ankle_x",
                "right_ankle_y",
                "right_ankle_z",
                "left_hip_x",
                "left_hip_y",
                "left_hip_z",
                "left_knee",
                "left_ankle_x",
                "left_ankle_y",
                "left_ankle_z",
    }
motors_dof_idx = [robot.get_joint(name).dof_start for name in joint_names]


for i in range(1000):
    num_actions = 14
    actions = (2.0*torch.rand( (num_actions), device=gs.device)-1.0) * math.pi

    #actions = torch.clip(actions, -self.env_cfg["clip_actions"], self.env_cfg["clip_actions"])
    target_dof_pos = actions * 2.0  + 0.0
    robot.control_dofs_position(target_dof_pos, motors_dof_idx)


    scene.step()

    print(sensor.read())

    ground_contacts = plane.get_contacts() # with_entity=leftfoot_link.entity, exclude_self_contact=True )
    #ground_contacts = leftfoot_link.entity.get_contacts( with_entity=plane, exclude_self_contact=False )

    if leftfoot_link.idx in ground_contacts["link_a"] or leftfoot_link.idx in ground_contacts["link_b"]:
        leftfoot_ground_contacts = True
    else:
        leftfoot_ground_contacts = False

    if rightfoot_link.idx in ground_contacts["link_a"] or rightfoot_link.idx in ground_contacts["link_b"]:
        rightfoot_ground_contacts = True
    else:
        rightfoot_ground_contacts = False

    print("contact: l", leftfoot_ground_contacts, "  r=", rightfoot_ground_contacts)
    

And locox.urdf

<?xml version="1.0"?>
<robot name="humanoid">

<material name="red">
  <color rgba="0.6 0.1 0.1 1"/>
</material>

<material name="blue">
  <color rgba="0.1 0.1 0.8 1"/>
</material>

<material name="green">
  <color rgba="0 1 0 1"/>
</material>

<material name="grey">
  <color rgba="0.5 0.5 0.5 1"/>
</material>

<material name="darkgrey">
  <color rgba="0.3 0.3 0.3 1"/>
</material>

<material name="yellow">
  <color rgba="1.0 1.0 0.0 1"/>
</material>


  <!-- ======== TRUNK ======== -->
  <link name="trunk">
    <visual>
      <geometry>
        <box size="0.25 0.08 0.5"/>
      </geometry>
      <origin xyz="0 0 0.25" rpy="3.14159 0 0"/>
      <material name="grey"/>
    </visual>
    <visual>
      <geometry>
      <sphere radius="0.06"/>
      </geometry>
      <origin xyz="0.10 0 0.0"/>
      <material name="blue"/>
    </visual>
    <visual>
      <geometry>
      <sphere radius="0.06"/>
      </geometry>
      <origin xyz="-0.10 0 0.0"/>
      <material name="blue"/>
    </visual>
    <collision>
      <geometry>
        <box size="0.25 0.08 0.5"/>
      </geometry>
      <origin xyz="0 0 0.25" rpy="3.14159 0 0"/>
    </collision>
    <visual>
      <geometry>
      <sphere radius="0.10"/>
      </geometry>
      <origin xyz="0.0 0 0.6"/>
      <material name="grey"/>
    </visual>
    <visual>
      <geometry>
      <sphere radius="0.02"/>
      </geometry>
      <origin xyz="0.045 0.075 0.62"/>
      <material name="red"/>
    </visual>
    <visual>
      <geometry>
      <sphere radius="0.02"/>
      </geometry>
      <origin xyz="-0.045 0.075 0.62"/>
      <material name="red"/>
    </visual>

    <inertial>
      <mass value="10"/>
      <inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
      <origin xyz="0 0 0.25" rpy="3.14159 0 0"/>
    </inertial>
  </link>

  <!-- ======== right LEG ======== -->
  <link name="right_thigh">
    <visual>
      <geometry><cylinder radius="0.05" length="0.4"/></geometry>
      <origin xyz="0 0 -0.21" rpy="0 0 0"/>
      <material name="grey"/>
    </visual>
    <collision>
      <geometry><cylinder radius="0.05" length="0.4"/></geometry>
      <origin xyz="0 0 -0.21" rpy="0 0 0"/>
    </collision>
    <inertial>
      <mass value="2"/>
      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
      <origin xyz="0.1 0 -0.2" rpy="0 0 0"/>
    </inertial>
  </link>

  <joint name="right_hip_x" type="revolute">
    <parent link="trunk"/>
    <child link="right_hip_x_link"/>
    <origin xyz="0.1 0 0" rpy="0 0 0"/>
    <axis xyz="1 0 0"/>
    <limit lower="-1.57" upper="1.57" effort="50" velocity="8"/>
  </joint>

  <link name="right_hip_x_link">
    <inertial>
      <origin xyz="0 0 0"/>
      <mass value="0.05"/>  <!-- masse minimale raisonnable -->
      <inertia 
          ixx="5e-5" ixy="0" ixz="0"
          iyy="5e-5" iyz="0"
          izz="5e-5"/>       <!-- inertie cohérente avec 0.05 kg -->
    </inertial>
  </link>
  <joint name="right_hip_y" type="revolute">
    <parent link="right_hip_x_link"/>
    <child link="right_hip_y_link"/>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <axis xyz="0 -1 0"/>
    <limit lower="0" upper="1.57" effort="50" velocity="8"/>
  </joint>
  <link name="right_hip_y_link">
    <inertial>
      <origin xyz="0 0 0"/>
      <mass value="0.05"/>  <!-- masse minimale raisonnable -->
      <inertia 
          ixx="5e-5" ixy="0" ixz="0"
          iyy="5e-5" iyz="0"
          izz="5e-5"/>       <!-- inertie cohérente avec 0.05 kg -->
    </inertial>
  </link>

  <joint name="right_hip_z" type="revolute">
    <parent link="right_hip_y_link"/>
    <child link="right_thigh"/>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit lower="-0.5" upper="0.5" effort="50" velocity="8"/>
  </joint>


  <link name="right_shin">
    <visual>
      <geometry><cylinder radius="0.04" length="0.4"/></geometry>
      <origin xyz="0 0 -0.2"/>
      <material name="grey"/>
    </visual>
    <visual>
      <geometry>
      <sphere radius="0.055"/>
      </geometry>
      <origin xyz="0 0 0.0"/> <!-- position du genou dans le repère du lien -->
      <material name="blue"/>
    </visual>
    <collision>
      <geometry><cylinder radius="0.04" length="0.4"/></geometry>
      <origin xyz="0 0 -0.2"/>
    </collision>
    <inertial>
      <mass value="1.5"/>
      <inertia ixx="0.005" ixy="0" ixz="0" iyy="0.005" iyz="0" izz="0.005"/>
      <origin xyz="0 0 -0.2"/>
    </inertial>
  </link>

  <joint name="right_knee" type="revolute">
    <parent link="right_thigh"/>
    <child link="right_shin"/>
    <origin xyz="0 0 -0.4" rpy="0 0 0"/>
    <axis xyz="-1 0 0"/>
    <limit lower="0" upper="2.0" effort="50" velocity="8"/>
  </joint>

  <link name="right_foot">
    <visual>
      <geometry><box size="0.08 0.20 0.02"/></geometry>
      <origin xyz="0 0.05 -0.02"/>
      <material name="green"/>
    </visual>
    <collision>
      <geometry><box size="0.08 0.20 0.02"/></geometry>
      <origin xyz="0 0.05 -0.02"/>
    </collision>
    <inertial>
      <mass value="0.5"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <origin xyz="0 0 -0.02"/>
    </inertial>
  </link>

  <joint name="right_ankle_x" type="revolute">
    <parent link="right_shin"/>
    <child link="right_ankle_x_link"/>
    <origin xyz="0 0 -0.4" rpy="0 0 0"/>
    <axis xyz="1 0 0"/>
    <limit lower="-0.5" upper="0.5" effort="20" velocity="8"/>
  </joint>
  <link name="right_ankle_x_link">
      <inertial>
      <origin xyz="0 0 0"/>
      <mass value="0.05"/>  <!-- masse minimale raisonnable -->
      <inertia 
          ixx="5e-5" ixy="0" ixz="0"
          iyy="5e-5" iyz="0"
          izz="5e-5"/>       <!-- inertie cohérente avec 0.05 kg -->
    </inertial>
  </link>
  <joint name="right_ankle_y" type="revolute">
    <parent link="right_ankle_x_link"/>
    <child link="right_ankle_y_link"/>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <limit lower="-0.2" upper="0.2" effort="20" velocity="8"/>
  </joint>
  <link name="right_ankle_y_link">
    <inertial>
      <origin xyz="0 0 0"/>
      <mass value="0.05"/>  <!-- masse minimale raisonnable -->
      <inertia 
          ixx="5e-5" ixy="0" ixz="0"
          iyy="5e-5" iyz="0"
          izz="5e-5"/>       <!-- inertie cohérente avec 0.05 kg -->
    </inertial>
  </link>
  <joint name="right_ankle_z" type="revolute">
    <parent link="right_ankle_y_link"/>
    <child link="right_foot"/>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit lower="-0.2" upper="0.2" effort="20" velocity="8"/>
  </joint>

  <!-- ======== left LEG ======== -->
  <link name="left_thigh">
    <visual>
      <geometry><cylinder radius="0.05" length="0.4"/></geometry>
      <origin xyz="0 0 -0.2"/>
      <material name="grey"/>
    </visual>
    <collision>
      <geometry><cylinder radius="0.05" length="0.4"/></geometry>
      <origin xyz="-0 0 -0.2"/>
    </collision>
    <inertial>
      <mass value="2"/>
      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
      <origin xyz="0 0 -0.2"/>
    </inertial>
  </link>

  <joint name="left_hip_x" type="revolute">
    <parent link="trunk"/>
    <child link="left_thigh_x_link"/>
    <origin xyz="-0.1 0 0" rpy="0 0 0"/>
    <axis xyz="1 0 0"/>
    <limit lower="-1.57" upper="1.57" effort="50" velocity="8"/>
  </joint>
  <link name="left_thigh_x_link">
    <inertial>
      <origin xyz="0 0 0"/>
      <mass value="0.05"/>  <!-- masse minimale raisonnable -->
      <inertia 
          ixx="5e-5" ixy="0" ixz="0"
          iyy="5e-5" iyz="0"
          izz="5e-5"/>       <!-- inertie cohérente avec 0.05 kg -->
    </inertial>
  </link>
  <joint name="left_hip_y" type="revolute">
    <parent link="left_thigh_x_link"/>
    <child link="left_thigh_y_link"/>
    <origin xyz="-0 0 0" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <limit lower="0" upper="1.57" effort="50" velocity="8"/>
  </joint>
  <link name="left_thigh_y_link">
    <inertial>
      <origin xyz="0 0 0"/>
      <mass value="0.05"/>  <!-- masse minimale raisonnable -->
      <inertia 
          ixx="5e-5" ixy="0" ixz="0"
          iyy="5e-5" iyz="0"
          izz="5e-5"/>       <!-- inertie cohérente avec 0.05 kg -->
    </inertial>
  </link>
  <joint name="left_hip_z" type="revolute">
    <parent link="left_thigh_y_link"/>
    <child link="left_thigh"/>
    <origin xyz="-0 0 0" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit lower="-0.5" upper="0.5" effort="50" velocity="8"/>
  </joint>

  <link name="left_shin">
    <visual>
      <geometry><cylinder radius="0.04" length="0.4"/></geometry>
      <origin xyz="0 0 -0.2"/>
      <material name="grey"/>
    </visual>
    <visual>
      <geometry>
      <sphere radius="0.055"/>
      </geometry>
      <origin xyz="0 0 0.0"/> <!-- position du genou dans le repère du lien -->
      <material name="blue"/>
    </visual>
    <collision>
      <geometry><cylinder radius="0.04" length="0.4"/></geometry>
      <origin xyz="0 0 -0.2"/>
    </collision>
    <inertial>
      <mass value="1.5"/>
      <inertia ixx="0.005" ixy="0" ixz="0" iyy="0.005" iyz="0" izz="0.005"/>
      <origin xyz="0 0 -0.2"/>
    </inertial>
  </link>

  <joint name="left_knee" type="revolute">
    <parent link="left_thigh"/>
    <child link="left_shin"/>
    <origin xyz="0 0 -0.4" rpy="0 0 0"/>
    <axis xyz="-1 0 0"/>
    <limit lower="0" upper="2.0" effort="50" velocity="8"/>
  </joint>

  <link name="left_foot">
    <visual>
      <geometry><box size="0.08 0.20 0.02"/></geometry>
      <origin xyz="0 0.05 -0.02"/>
      <material name="red"/>
    </visual>
    <collision>
      <geometry><box size="0.08 0.20 0.02"/></geometry>
      <origin xyz="0 0.05 -0.02"/>
    </collision>
    <inertial>
      <mass value="0.5"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <origin xyz="0 0 -0.02"/>
    </inertial>
  </link>

  <joint name="left_ankle_x" type="revolute">
    <parent link="left_shin"/>
    <child link="left_ankle_x_link"/>
    <origin xyz="0 0 -0.4" rpy="0 0 0"/>
    <axis xyz="1 0 0"/>
    <limit lower="-0.5" upper="0.5" effort="20" velocity="8"/>
  </joint>
  <link name="left_ankle_x_link">
    <inertial>
      <origin xyz="0 0 0"/>
      <mass value="0.05"/>  <!-- masse minimale raisonnable -->
      <inertia 
          ixx="5e-5" ixy="0" ixz="0"
          iyy="5e-5" iyz="0"
          izz="5e-5"/>       <!-- inertie cohérente avec 0.05 kg -->
    </inertial>
  </link>
  <joint name="left_ankle_y" type="revolute">
    <parent link="left_ankle_x_link"/>
    <child link="left_ankle_y_link"/>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <limit lower="-0.2" upper="0.2" effort="20" velocity="8"/>
  </joint>
  <link name="left_ankle_y_link">
    <inertial>
      <origin xyz="0 0 0"/>
      <mass value="0.05"/>  <!-- masse minimale raisonnable -->
      <inertia 
          ixx="5e-5" ixy="0" ixz="0"
          iyy="5e-5" iyz="0"
          izz="5e-5"/>       <!-- inertie cohérente avec 0.05 kg -->
    </inertial>
  </link>
  <joint name="left_ankle_z" type="revolute">
    <parent link="left_ankle_y_link"/>
    <child link="left_foot"/>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit lower="-0.2" upper="0.2" effort="20" velocity="8"/>
  </joint>

</robot>

contact.py

[stergiosba]

With the fact that this is a custom model I suspected something is off with the way collisions are defined probably in the URDF.
The culprit for the debug spheres showing is that your foot link is coming in contact with the link above it.
I modified your test script so that its more controllable and you can replicate what I have in the attached video. Hope these help but it looks like there is no issue on Genesis' side.

Download this file:

contact_keyboard.py

contact.mp4

contact_2.mp4

[duburcqa]

@stergiosba

I cannot reproduce your results

---

@ucacaxm

Currently, contact information are updated at the beginning of the step, not the end. As a result, the contact information being measured are already outdated by one step when the sensor data is updated.

I was thinking about moving contact detection at the end of the step. But then, this will be the contact forces that will be invalidated... The only clean way to address this is doing contact detection and solving all constraints at the end of the step, and solving all the constraints once again at the beginning of the next step (but without updating contact detection). This may significantly increase the simulation time...

[duburcqa]

@ucacaxm

It turns out I could not reproduce your issue actually. All I can get is an offset by one step, which is "expected" for now.

[YilingQiao]

okay, I will try to reproduce it.

[ucacaxm]

Thanks

The culprit for the debug spheres showing is that your foot link is coming in contact with the link above it.

Yes your right with the sensor. I had trouble ton configure the sensor to get
only the feet and ground (I was confused between idx and idx_local).
Then I moved to use the ground get_contacts() and search in it.

I would suggest to change a bit the examples in order to detect ground contact.
I will try to propose a pull request on the contact_force_go2.py example.

All I can get is an offset by one step, which is "expected" for now.

I will investigate also in this direction and around the URDF config.

Thanks for your help