[FEATURE] Contact Sensor with Kinematic Probe by YilingQiao · Pull Request #2292 · Genesis-Embodied-AI/Genesis

YilingQiao · 2026-01-23T22:56:48Z

Description

Add KinematicContactProbe sensor for contact detection without physics side effects.

New sensor type that uses support function queries to detect contact
Supports multiple probes per sensor with probe_local_pos and probe_local_normal lists
Returns penetration depth, contact position, normal, and estimated force per probe
Lazy evaluation: queries execute on read(), not every step (unless delay configured)

To solve #2254

How It Works

Algorithm for each probe:

Transform probe_pos and probe_normal from link-local to world frame
For each nearby geom, compute support_pos = support(-probe_normal)
- This finds the point on the geom furthest in the opposite direction of the probe normal
If distance(support_pos, probe_pos) <= radius, compute:
- penetration = dot(probe_pos - support_pos, probe_normal)

Force Output (Not Physical)

The returned force = stiffness * penetration * probe_normal is a user-defined estimate, not derived from the physics solver (because this probe is only at kinematic level). Genesis is not a mass-spring model. The stiffness parameter is for convenience to convert penetration into a force-like quantity.

Return Values

sensor.read() returns a KinematicContactProbeData NamedTuple:

Field	Shape	Description
`penetration`	`(n_envs, n_probes)` or `(n_probes,)`	Penetration depth in meters. Positive when contact detected, zero otherwise.
`position`	`(n_envs, n_probes, 3)` or `(n_probes, 3)`	Contact point in link-local frame. The point on the contacting geom closest to the probe (in the -normal direction).
`normal`	`(n_envs, n_probes, 3)` or `(n_probes, 3)`	Contact normal in link-local frame. Same as `probe_local_normal` when contact detected, zero otherwise.
`force`	`(n_envs, n_probes, 3)` or `(n_probes, 3)`	Estimated force in link-local frame: `stiffness * penetration * probe_normal` (NOT physical).

Try

python examples/sensors/kinematic_contact_probe.py -v -t 6

kinematic_probe.mp4

Test plan

Basic contact detection with plane
No-contact case returns zeros
Multi-environment support
Multi-probe sensor support
Collision filtering (contype/conaffinity)
Box geometry support function
Noise and delay functionality

Checklist:

I read the CONTRIBUTING document.
I followed the Submitting Code Changes section of CONTRIBUTING document.
I tagged the title correctly (including BUG FIX/FEATURE/MISC/BREAKING)
I updated the documentation accordingly or no change is needed.
I tested my changes and added instructions on how to test it for reviewers.

I have added tests to cover my changes.
All new and existing tests passed.

github-actions · 2026-01-24T22:49:34Z

🔴 Benchmark Regression Detected ➡️ Report

genesis/engine/sensors/kinematic_contact_probe.py

github-actions · 2026-02-02T02:40:29Z

⚠️ Abnormal Benchmark Result Detected ➡️ Report

duburcqa · 2026-02-02T13:24:10Z

genesis/engine/sensors/kinematic_contact_probe.py

+            probe_positions_local=shared_metadata.probe_positions.contiguous(),
+            probe_normals_local=shared_metadata.probe_normals.contiguous(),
+            probe_sensor_idx=shared_metadata.probe_sensor_idx.contiguous(),
+            links_pos=links_pos.contiguous(),
+            links_quat=links_quat.contiguous(),
+            radii=shared_metadata.radii.contiguous(),
+            stiffness=shared_metadata.stiffness.contiguous(),
+            links_idx=shared_metadata.links_idx.contiguous(),
+            contypes=shared_metadata.contypes.contiguous(),
+            conaffinities=shared_metadata.conaffinities.contiguous(),
+            n_probes_per_sensor=shared_metadata.n_probes_per_sensor.contiguous(),
+            sensor_cache_start=shared_metadata.sensor_cache_start.contiguous(),
+            sensor_probe_start=shared_metadata.sensor_probe_start.contiguous(),


Remove continuous(). If this is currently necessary, the implementation should be refactored so that it is no longer necessary but applying transpose and changing the expected memory layout in the kernel.

duburcqa · 2026-02-02T13:24:34Z

genesis/engine/sensors/kinematic_contact_probe.py

+            constraint_state=solver.constraint_solver.constraint_state,
+            equalities_info=solver.equalities_info,
+            support_field_info=solver.collider._support_field._support_field_info,
+            output=shared_ground_truth_cache.contiguous(),


Same. Find a way to remove contiguous.

duburcqa · 2026-02-02T13:26:53Z

genesis/engine/sensors/kinematic_contact_probe.py

+        links_pos = solver.get_links_pos(links_idx=shared_metadata.links_idx)
+        links_quat = solver.get_links_quat(links_idx=shared_metadata.links_idx)
+
+        if solver.n_envs == 0:
+            links_pos = links_pos[None]
+            links_quat = links_quat[None]
+
+        links_pos = links_pos.reshape(B, n_sensors, 3)
+        links_quat = links_quat.reshape(B, n_sensors, 4)


Using these public getters is not very efficient. I would recommend passing shared_metadata.links_idx to _kernel_kinematic_contact_probe_support_query, along with solver.links_state.

duburcqa · 2026-02-02T13:28:47Z

genesis/engine/sensors/kinematic_contact_probe.py

+        )
+        self._shared_metadata.contypes = concat_with_tensor(
+            self._shared_metadata.contypes,
+            torch.tensor([self._options.contype], dtype=torch.int32, device=gs.device),


self._options.contype is good enough, no need to cast as tensor. This will allow to keep everything on one line and focus on what matters.

duburcqa · 2026-02-02T13:28:59Z

genesis/engine/sensors/kinematic_contact_probe.py

+        )
+        self._shared_metadata.conaffinities = concat_with_tensor(
+            self._shared_metadata.conaffinities,
+            torch.tensor([self._options.conaffinity], dtype=torch.int32, device=gs.device),


Same. No need to cast as tensor.

duburcqa · 2026-02-02T13:54:55Z

genesis/engine/sensors/kinematic_contact_probe.py

+    contypes: torch.Tensor = make_tensor_field((0,), dtype_factory=lambda: torch.int32)
+    conaffinities: torch.Tensor = make_tensor_field((0,), dtype_factory=lambda: torch.int32)
+
+    probe_sensor_idx: torch.Tensor = make_tensor_field((0,), dtype_factory=lambda: torch.int32)
+    probe_positions: torch.Tensor = make_tensor_field((0, 3))
+    probe_normals: torch.Tensor = make_tensor_field((0, 3))
+
+    n_probes_per_sensor: torch.Tensor = make_tensor_field((0,), dtype_factory=lambda: torch.int32)
+    sensor_cache_start: torch.Tensor = make_tensor_field((0,), dtype_factory=lambda: torch.int32)
+    sensor_probe_start: torch.Tensor = make_tensor_field((0,), dtype_factory=lambda: torch.int32)


I'm replacing dtype_factory=lambda: [...] in favour of simply dtype=[...], which is now possible because sensors cannot be imported before initializing Genesis anymore.

duburcqa · 2026-02-02T13:56:57Z

genesis/engine/sensors/kinematic_contact_probe.py

+        self._n_probes = sensor_options.n_probes
+        super().__init__(sensor_options, sensor_idx, data_cls, sensor_manager)


Skip one line between defining derived class attributes and calling base init.

duburcqa · 2026-02-02T14:00:16Z

genesis/engine/sensors/kinematic_contact_probe.py

+
+        self._shared_metadata.n_probes_per_sensor = concat_with_tensor(
+            self._shared_metadata.n_probes_per_sensor,
+            torch.tensor([n_probes], dtype=torch.int32, device=gs.device),


Casting to tensor manually is not necessary, and enforcing 1D / sequence is not necessary either.

Just do:

self._shared_metadata.n_probes_per_sensor = concat_with_tensor( self._shared_metadata.n_probes_per_sensor, n_probes, expand=(1,), dim=0 )

duburcqa · 2026-02-02T14:00:25Z

genesis/engine/sensors/kinematic_contact_probe.py

+        current_cache_start = sum(self._shared_metadata.cache_sizes[:-1]) if self._shared_metadata.cache_sizes else 0
+        self._shared_metadata.sensor_cache_start = concat_with_tensor(
+            self._shared_metadata.sensor_cache_start,
+            torch.tensor([current_cache_start], dtype=torch.int32, device=gs.device),


duburcqa · 2026-02-02T14:00:31Z

genesis/engine/sensors/kinematic_contact_probe.py

+        current_probe_start = self._shared_metadata.total_n_probes
+        self._shared_metadata.sensor_probe_start = concat_with_tensor(
+            self._shared_metadata.sensor_probe_start,
+            torch.tensor([current_probe_start], dtype=torch.int32, device=gs.device),


duburcqa · 2026-02-02T14:01:33Z

genesis/engine/sensors/kinematic_contact_probe.py

+        positions_tensor = torch.tensor(probe_positions, dtype=gs.tc_float, device=gs.device)
+        self._shared_metadata.probe_positions = torch.cat(
+            [self._shared_metadata.probe_positions, positions_tensor], dim=0
+        )


Why are you concatenating manually instead of using our dedicated utility for this?

duburcqa · 2026-02-02T14:01:42Z

genesis/engine/sensors/kinematic_contact_probe.py

+        normals_tensor = torch.tensor(probe_normals, dtype=gs.tc_float, device=gs.device)
+        norms = normals_tensor.norm(dim=1, keepdim=True).clamp(min=1e-8)
+        normals_tensor = normals_tensor / norms
+        self._shared_metadata.probe_normals = torch.cat([self._shared_metadata.probe_normals, normals_tensor], dim=0)


Why are you concatenating manually instead of using our dedicated utility for this?

duburcqa · 2026-02-02T14:12:07Z

genesis/engine/sensors/kinematic_contact_probe.py

+        if not self._manager._has_delay_configured(type(self)):
+            self._manager.update_sensor_type_cache(type(self))


Don't do this. You are abusing the software design. It is the responsibility of the manager to determine whether to update a given sensor type or not.

duburcqa · 2026-02-02T14:14:26Z

genesis/engine/sensors/sensor_manager.py

Move all these changes outside of this PR. It is completely unrelated and requires further discussion. First, we need to make sure this lazy-update thing is really necessary, next we need to implement this feature in a way that does not delegate more responsibility that expected to sensors.

duburcqa · 2026-02-02T14:17:35Z

genesis/engine/sensors/kinematic_contact_probe.py

+            if geom_type == gs.GEOM_TYPE.PLANE:
+                g_pos = geoms_state.pos[i_g, i_b]
+                g_quat = geoms_state.quat[i_g, i_b]
+                geom_data = geoms_info.data[i_g]
+                plane_normal_local = gs.ti_vec3([geom_data[0], geom_data[1], geom_data[2]])
+                plane_normal_world = gu.ti_transform_by_quat(plane_normal_local, g_quat)
+                dist_to_plane = (probe_pos - g_pos).dot(plane_normal_world)
+                support_pos = probe_pos - dist_to_plane * plane_normal_world


Why are you handling if geom_type == gs.GEOM_TYPE.PLANE: separately from other cases defined in _func_support_point_for_probe ? I don't say why they could not be unified. Just rename the function if you think it is no longer appropriate.

duburcqa · 2026-02-02T14:18:14Z

genesis/engine/sensors/kinematic_contact_probe.py

+@ti.func
+def _func_point_in_expanded_aabb(
+    i_g: ti.i32,
+    i_b: ti.i32,
+    geoms_state: array_class.GeomsState,
+    point: ti.types.vector(3, gs.ti_float),
+    expansion: gs.ti_float,
+):
+    aabb_min = geoms_state.aabb_min[i_g, i_b] - expansion
+    aabb_max = geoms_state.aabb_max[i_g, i_b] + expansion
+    return (point >= aabb_min).all() and (point <= aabb_max).all()


We already have a function for this, just import it from rigid solver, you are allowed to do this.

this is a specific variants of aabb only used in this sensor, we don't need to accommodate this function in the main code?

duburcqa · 2026-02-02T14:18:52Z

genesis/engine/sensors/kinematic_contact_probe.py

+@ti.func
+def _func_check_collision_filter(
+    i_g: ti.i32,
+    i_b: ti.i32,
+    sensor_link_idx: ti.i32,
+    sensor_contype: ti.i32,
+    sensor_conaffinity: ti.i32,
+    geoms_info: array_class.GeomsInfo,
+    rigid_global_info: array_class.RigidGlobalInfo,
+    constraint_state: array_class.ConstraintState,
+    equalities_info: array_class.EqualitiesInfo,
+):


We don't already have a function that is doing exactly this? If we do, just import it directly instead of copy-pasting it here.

this part introduced affinity for sensor, and only used in this sensor. We don't need this plugin to affected the main physics solver code

duburcqa · 2026-02-02T14:21:13Z

genesis/options/sensors/options.py

+    @property
+    def n_probes(self) -> int:
+        """Return the number of probes defined for this sensor."""
+        return len(self.probe_local_pos)
+
+    def get_probe_positions(self) -> list[Tuple3FType]:
+        """Return probe positions as a list of tuples."""
+        return [tuple(pos) for pos in self.probe_local_pos]
+
+    def get_probe_normals(self) -> list[Tuple3FType]:
+        """Return probe normals as a list of tuples."""
+        return [tuple(normal) for normal in self.probe_local_normal]


I don't think this should be part of the options.

duburcqa · 2026-02-02T14:22:17Z

tests/test_sensors.py

+    scene.build(n_envs=0)
+    scene.step()
+
+    assert probe.read().penetration[0] > 0.0
+
+    mount.set_pos(torch.tensor([0.0, 0.0, 2.0], dtype=gs.tc_float))
+    scene.step()
+    assert probe.read().penetration[0] == 0.0


This is clearly not sufficient. You should check that the result is analytically valid on a synthetic scenario that is simple enough to do so.

I just noticed your example script. I think it is perfectly suitable for analytical validation already. You need to convert this in a unit test and do more comprehensive validation of the sensor measurement.

duburcqa · 2026-02-02T14:22:59Z

tests/test_sensors.py

+
+    assert probe.read().penetration[0] > 0.0
+
+    mount.set_pos(torch.tensor([0.0, 0.0, 2.0], dtype=gs.tc_float))


You can just do mount.set_pos([0.0, 0.0, 2.0]). No need to force casting to tensor. It just makes what you are doing harder to read.

duburcqa · 2026-02-02T14:24:37Z

tests/test_sensors.py

+
+    scene.add_entity(gs.morphs.Box(size=(0.5, 0.5, 0.5), pos=(0.0, 0.0, 0.25), fixed=True))
+
+    mount = scene.add_entity(gs.morphs.Sphere(radius=0.02, pos=(0.25, 0.25, 0.06), fixed=True, collision=False))


Use multiple lines systematically for scene declaration with very few exceptions. And no need to skip lines between the declaration of each entity.

duburcqa · 2026-02-02T14:25:10Z

tests/test_sensors.py

+def test_kinematic_contact_probe_box_support(show_viewer, tol):
+    """Test BOX geometry detection via support function."""
+    scene = gs.Scene(
+        sim_options=gs.options.SimOptions(dt=1e-2, substeps=1, gravity=(0.0, 0.0, 0.0)),


One line per argument. Moreover, remove them if they are not absolutely necessary for your test.

duburcqa · 2026-02-02T14:26:23Z

tests/test_sensors.py

+    assert probe.read().penetration[0] > 0.0
+
+    mount.set_pos(torch.tensor([0.0, 0.0, 2.0], dtype=gs.tc_float))
+    scene.step()
+    assert probe.read().penetration[0] == 0.0


Just (vaguely) checking penetration is not good enough. What about all the other outputs?! See what we did for the IMU sensor, it is much more comprehensive.

YilingQiao · 2026-02-02T19:03:52Z

We do not add features pro-actively without at least well identified use cases. Could you clarify in the description of this PR what are the targeted use cases of this feature? Note that following our template for PR would have avoided having this question in the first place since there is already a dedicated section for motivations.

It's for resolving this request as in the PR description.
#2254

YilingQiao and others added 8 commits January 23, 2026 14:31

kinematic probe

fed513c

simplify code

93da809

uddate documentation

70597c2

doc

4807659

tests

1ff761c

change

5a95c87

Merge branch 'main' into yiling/260122_ghost_force

a832694

Merge branch 'main' into yiling/260122_ghost_force

23e9ef1

duburcqa reviewed Jan 25, 2026

View reviewed changes

genesis/engine/sensors/kinematic_contact_probe.py Show resolved Hide resolved

YilingQiao marked this pull request as ready for review January 28, 2026 19:03

YilingQiao changed the title ~~[WIP][FEATURE] Contact Sensor with Kinematic Probe~~ [FEATURE] Contact Sensor with Kinematic Probe Jan 28, 2026

Merge branch 'main' into yiling/260122_ghost_force

3e37ef7

duburcqa reviewed Feb 2, 2026

View reviewed changes

Milotrince mentioned this pull request Feb 5, 2026

[FEATURE] Add example viewer plugins. #2357

Open

5 tasks

		self._n_probes = sensor_options.n_probes
		super().__init__(sensor_options, sensor_idx, data_cls, sensor_manager)

		if not self._manager._has_delay_configured(type(self)):
		self._manager.update_sensor_type_cache(type(self))


		assert probe.read().penetration[0] > 0.0

		mount.set_pos(torch.tensor([0.0, 0.0, 2.0], dtype=gs.tc_float))


		scene.add_entity(gs.morphs.Box(size=(0.5, 0.5, 0.5), pos=(0.0, 0.0, 0.25), fixed=True))

		mount = scene.add_entity(gs.morphs.Sphere(radius=0.02, pos=(0.25, 0.25, 0.06), fixed=True, collision=False))

Conversation

YilingQiao commented Jan 23, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Description

How It Works

Force Output (Not Physical)

Return Values

Test plan

Checklist:

Uh oh!

github-actions bot commented Jan 24, 2026

Uh oh!

Uh oh!

github-actions bot commented Feb 2, 2026

Uh oh!

duburcqa Feb 2, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

duburcqa Feb 2, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

YilingQiao commented Feb 2, 2026

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

YilingQiao commented Jan 23, 2026 •

edited

Loading

duburcqa Feb 2, 2026 •

edited

Loading

duburcqa Feb 2, 2026 •

edited

Loading