Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
@@ -0,0 +1,7 @@
Fixed
^^^^^

* Fixed :class:`~isaaclab_newton.sensors.ContactSensor` metadata extraction
after the migration to Newton 1.1, where ``sensing_obj_type`` and
``counterpart_type`` became scalar strings and ``counterpart_indices``
became per-row.
Original file line number Diff line number Diff line change
Expand Up @@ -346,48 +346,42 @@ def _create_buffers(self):
body_labels = self._get_model_labels("body")
shape_labels = self._get_model_labels("shape")

def get_name(idx, kind):
kind_name = getattr(kind, "name", None)
kind_value = getattr(kind, "value", kind)
if kind_name == "BODY" or kind_value == 2:
return body_labels[int(idx)].split("/")[-1]
if kind_name == "SHAPE" or kind_value == 1:
return shape_labels[int(idx)].split("/")[-1]
return "MATCH_ANY"

def flatten_metadata(values):
if isinstance(values, wp.array):
values = values.numpy()
flat_values = np.asarray(values, dtype=object).reshape(-1).tolist()
if flat_values and isinstance(flat_values[0], list | tuple | np.ndarray):
return [
value
for nested_values in flat_values
for value in np.asarray(nested_values, dtype=object).reshape(-1).tolist()
]
return flat_values

flat_sensing = list(
zip(
flatten_metadata(self.contact_view.sensing_obj_idx),
flatten_metadata(self.contact_view.sensing_obj_type),
)
)
self._sensor_names = [get_name(idx, kind) for idx, kind in flat_sensing]
s_kind = self.contact_view.sensing_obj_type
if s_kind == "body":
s_labels = body_labels
elif s_kind == "shape":
s_labels = shape_labels
else:
raise RuntimeError(f"Unexpected Newton sensing_obj_type {s_kind!r}; expected 'body' or 'shape'.")
self._sensor_names = [s_labels[i].split("/")[-1] for i in self.contact_view.sensing_obj_idx]
# Assumes the environments are processed in order.
self._sensor_names = self._sensor_names[: self._num_sensors]
flat_counterparts = list(
zip(
flatten_metadata(self.contact_view.counterpart_indices),
flatten_metadata(self.contact_view.counterpart_type),
)
)
self._filter_object_names = [get_name(idx, kind) for idx, kind in flat_counterparts]

c_kind = self.contact_view.counterpart_type
c_idx_per_sensor = self.contact_view.counterpart_indices
if c_kind is None:
if self._generate_force_matrix:
raise RuntimeError("Filter expressions were configured but Newton reports no counterpart type.")
self._filter_object_names = []
else:
if c_kind == "body":
c_labels = body_labels
elif c_kind == "shape":
c_labels = shape_labels
else:
raise RuntimeError(f"Unexpected Newton counterpart_type {c_kind!r}; expected 'body' or 'shape'.")
# Envs are homogeneous: every sensor row sees the same counterpart list. Take row 0.
row0 = c_idx_per_sensor[0] if c_idx_per_sensor else []
Comment thread
camevor marked this conversation as resolved.
self._filter_object_names = [c_labels[i].split("/")[-1] for i in row0]
if self._generate_force_matrix and not self._filter_object_names:
logger.warning("Filter expressions matched zero counterpart objects; force matrix will be empty.")

force_matrix = self.contact_view.force_matrix
force_matrix_shape = force_matrix.shape if force_matrix is not None else (total_sensor_count, 0)
# Number of filter objects.
self._num_filter_objects = force_matrix_shape[1] if len(force_matrix_shape) > 1 else 0
if self._num_filter_objects > 0 and force_matrix is None:
raise RuntimeError("Filter counterparts present but Newton force_matrix is None.")

# Store flat Newton force views for copying data. These may be non-contiguous
# views, so the copy kernel indexes them without reshaping.
Expand Down
107 changes: 107 additions & 0 deletions source/isaaclab_newton/test/sensors/test_contact_sensor.py
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,7 @@

import pytest
import torch
from isaaclab_newton.sensors.contact_sensor import ContactSensorCfg as NewtonContactSensorCfg
from physics.physics_test_utils import (
COLLISION_PIPELINES,
STABLE_SHAPES,
Expand Down Expand Up @@ -780,6 +781,112 @@ def test_finger_contact_sensor_isolation(device: str, use_mujoco_contacts: bool,
)


# ===================================================================
# Sensor metadata
# ===================================================================


def _make_two_box_scene_cfg(num_envs: int) -> ContactSensorTestSceneCfg:
"""Scene with two distinct Cuboid bodies (BoxA, BoxB) per env."""
rigid_props = sim_utils.RigidBodyPropertiesCfg(disable_gravity=True, linear_damping=0.0, angular_damping=0.0)
scene_cfg = ContactSensorTestSceneCfg(num_envs=num_envs, env_spacing=5.0, lazy_sensor_update=False)
scene_cfg.object_a = RigidObjectCfg(
prim_path="{ENV_REGEX_NS}/BoxA",
spawn=sim_utils.CuboidCfg(
size=(0.3, 0.3, 0.3),
rigid_props=rigid_props,
collision_props=sim_utils.CollisionPropertiesCfg(collision_enabled=True),
mass_props=sim_utils.MassPropertiesCfg(mass=1.0),
activate_contact_sensors=True,
),
init_state=RigidObjectCfg.InitialStateCfg(pos=(-0.5, 0.0, 1.0)),
)
scene_cfg.object_b = RigidObjectCfg(
prim_path="{ENV_REGEX_NS}/BoxB",
spawn=sim_utils.CuboidCfg(
size=(0.3, 0.3, 0.3),
rigid_props=rigid_props,
collision_props=sim_utils.CollisionPropertiesCfg(collision_enabled=True),
mass_props=sim_utils.MassPropertiesCfg(mass=1.0),
activate_contact_sensors=True,
),
init_state=RigidObjectCfg.InitialStateCfg(pos=(0.5, 0.0, 1.0)),
)
return scene_cfg


@pytest.mark.parametrize("device", ["cuda:0", "cpu"])
def test_sensor_metadata(device: str):
"""Verify sensor_names and filter_object_names match the underlying sensing and
counterpart configuration across body-mode, body-mode-with-filter, and shape-mode.
"""
num_envs = 4
sim_cfg = make_sim_cfg(use_mujoco_contacts=False, device=device, gravity=(0.0, 0.0, -9.81))

# (1) Body-mode, no filter: pattern matches two distinct body names per env.
with build_simulation_context(sim_cfg=sim_cfg, auto_add_lighting=True, add_ground_plane=True) as sim:
sim._app_control_on_stop_handle = None
scene_cfg = _make_two_box_scene_cfg(num_envs)
scene_cfg.contact_sensor_a = ContactSensorCfg(
prim_path="{ENV_REGEX_NS}/Box.*",
update_period=0.0,
history_length=1,
)
scene = InteractiveScene(scene_cfg)
sim.reset()
scene.reset()

sensor: ContactSensor = scene["contact_sensor_a"]
assert sensor.num_sensors == 2, f"expected 2 sensors per env, got {sensor.num_sensors}"
assert sensor.sensor_names == ["BoxA", "BoxB"], f"unexpected sensor_names: {sensor.sensor_names}"
assert sensor.filter_object_names == [], (
f"expected empty filter_object_names with no filter, got {sensor.filter_object_names}"
)

# (2) Body-mode, with filter: one body matches the sensor pattern, one matches the filter pattern.
with build_simulation_context(sim_cfg=sim_cfg, auto_add_lighting=True, add_ground_plane=True) as sim:
sim._app_control_on_stop_handle = None
scene_cfg = _make_two_box_scene_cfg(num_envs)
scene_cfg.contact_sensor_a = ContactSensorCfg(
prim_path="{ENV_REGEX_NS}/BoxA",
filter_prim_paths_expr=["{ENV_REGEX_NS}/BoxB"],
update_period=0.0,
history_length=1,
)
scene = InteractiveScene(scene_cfg)
sim.reset()
scene.reset()

sensor: ContactSensor = scene["contact_sensor_a"]
assert sensor.num_sensors == 1, f"expected 1 sensor per env, got {sensor.num_sensors}"
assert sensor.sensor_names == ["BoxA"], f"unexpected sensor_names: {sensor.sensor_names}"
assert sensor.num_filter_objects == 1, f"expected 1 filter object per sensor, got {sensor.num_filter_objects}"
assert sensor.filter_object_names == ["BoxB"], f"unexpected filter_object_names: {sensor.filter_object_names}"

# (3) Shape-mode, no filter: pattern matches shapes (not bodies).
# `sensor_shape_prim_expr` is a Newton-only extension, so this block uses the
# backend-specific NewtonContactSensorCfg subclass.
with build_simulation_context(sim_cfg=sim_cfg, auto_add_lighting=True, add_ground_plane=True) as sim:
sim._app_control_on_stop_handle = None
scene_cfg = _make_two_box_scene_cfg(num_envs)
scene_cfg.contact_sensor_a = NewtonContactSensorCfg(
prim_path="{ENV_REGEX_NS}/Box.*",
sensor_shape_prim_expr=["{ENV_REGEX_NS}/Box.*"],
update_period=0.0,
history_length=1,
)
scene = InteractiveScene(scene_cfg)
sim.reset()
scene.reset()

sensor: ContactSensor = scene["contact_sensor_a"]
assert sensor.num_sensors == 2, f"expected 2 shape sensors per env, got {sensor.num_sensors}"
assert sensor.sensor_names == ["mesh", "mesh"], f"unexpected shape sensor_names: {sensor.sensor_names}"
assert sensor.filter_object_names == [], (
f"expected empty filter_object_names with no filter, got {sensor.filter_object_names}"
)


# ===================================================================
# Utility
# ===================================================================
Expand Down
Loading