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,6 @@
Fixed
^^^^^

* Fixed :class:`~isaaclab_newton.sensors.contact_sensor.ContactSensor` to use
current Newton contact sensor API names, removing deprecation warnings from
Newton contact sensor test runs.
Original file line number Diff line number Diff line change
Expand Up @@ -1351,7 +1351,7 @@ def _normalize_for_labels(expr: str | list[str] | None, labels: list[str]) -> st
sensing_obj_shapes=_normalize_for_labels(_to_fnmatch(shape_names_expr), shape_labels),
counterpart_bodies=_normalize_for_labels(_to_fnmatch(contact_partners_body_expr), body_labels),
counterpart_shapes=_normalize_for_labels(_to_fnmatch(contact_partners_shape_expr), shape_labels),
include_total=True,
measure_total=True,
verbose=verbose,
)

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -322,17 +322,17 @@ def _initialize_impl(self):
raise RuntimeError(self._init_error) from err

def _create_buffers(self):
# Get Newton sensor shape: (n_sensors * n_envs, n_counterparts)
newton_shape = self.contact_view.shape
# Get Newton sensor count from total force: (n_sensors * n_envs)
total_sensor_count = self.contact_view.total_force.shape[0]

# resolve the true count of sensors
self._num_sensors = newton_shape[0] // self._num_envs
self._num_sensors = total_sensor_count // self._num_envs

# Check that number of sensors is an integer
if newton_shape[0] % self._num_envs != 0:
if total_sensor_count % self._num_envs != 0:
raise RuntimeError(
"Number of sensors is not an integer multiple of the number of environments. Received:"
f" {newton_shape[0]} sensors across {self._num_envs} environments."
f" {total_sensor_count} sensors across {self._num_envs} environments."
)
if self._num_sensors == 0:
raise RuntimeError(
Expand All @@ -350,25 +350,49 @@ 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[idx].split("/")[-1]
return body_labels[int(idx)].split("/")[-1]
if kind_name == "SHAPE" or kind_value == 1:
return shape_labels[idx].split("/")[-1]
return shape_labels[int(idx)].split("/")[-1]
return "MATCH_ANY"

flat_sensing = [obj for world_objs in self.contact_view.sensing_objs for obj in world_objs]
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]
# Assumes the environments are processed in order.
self._sensor_names = self._sensor_names[: self._num_sensors]
flat_counterparts = [obj for world_objs in self.contact_view.counterparts for obj in world_objs]
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]

# Number of filter objects (counterparts minus the total column)
self._num_filter_objects = max(newton_shape[1] - 1, 0)
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

# Store reshaped Newton net_force view for copying data
# Newton net_force shape: (n_sensors * n_envs, n_counterparts)
# Reshaped to: (n_envs, n_sensors, n_counterparts)
self._newton_forces_view = self.contact_view.net_force.reshape((self._num_envs, self._num_sensors, -1))
# Store flat Newton force views for copying data. These may be non-contiguous
# views, so the copy kernel indexes them without reshaping.
self._newton_total_force_view = self.contact_view.total_force
self._newton_force_matrix_view = force_matrix if self._num_filter_objects > 0 else None

# prepare data buffers
logger.info(
Expand Down Expand Up @@ -413,7 +437,9 @@ def _update_buffers_impl(self, env_mask: wp.array):
dim=(self._num_envs, self._num_sensors, max(self._num_filter_objects, 1)),
inputs=[
env_mask,
self._newton_forces_view,
self._num_sensors,
self._newton_total_force_view,
self._newton_force_matrix_view,
],
outputs=[
self._data._net_forces_w,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,9 @@
def copy_from_newton_kernel(
# in
env_mask: wp.array(dtype=wp.bool),
newton_forces: wp.array3d(dtype=wp.vec3f), # (n_envs, n_sensors, n_counterparts)
num_sensors: int,
newton_total_force: wp.array(dtype=wp.vec3f), # (n_envs * n_sensors)
newton_force_matrix: wp.array2d(dtype=wp.vec3f), # (n_envs * n_sensors, n_filter_objects) or None
# outputs
net_force_total: wp.array2d(dtype=wp.vec3f), # (n_envs, n_sensors)
force_matrix: wp.array3d(dtype=wp.vec3f), # (n_envs, n_sensors, n_filter_objects) or None
Expand All @@ -30,13 +32,14 @@ def copy_from_newton_kernel(
return

# Copy total force (column 0) - only thread with f_idx == 0 does this
src_idx = env * num_sensors + sensor
if f_idx == 0:
net_force_total[env, sensor] = newton_forces[env, sensor, 0]
net_force_total[env, sensor] = newton_total_force[src_idx]

# Copy per-filter-object forces (columns 1+)
# Copy per-filter-object forces.
# Guard with `if force_matrix:` to handle None case (no filter objects)
if force_matrix:
force_matrix[env, sensor, f_idx] = newton_forces[env, sensor, f_idx + 1]
force_matrix[env, sensor, f_idx] = newton_force_matrix[src_idx, f_idx]


@wp.kernel
Expand Down
20 changes: 10 additions & 10 deletions source/isaaclab_newton/test/assets/test_articulation.py
Original file line number Diff line number Diff line change
Expand Up @@ -983,8 +983,8 @@ def test_external_force_buffer(sim, num_articulations, device, articulation_type

# check if the articulation's force and torque buffers are correctly updated
for i in range(num_articulations):
assert articulation.permanent_wrench_composer.composed_force.torch[i, 0, 0].item() == force
assert articulation.permanent_wrench_composer.composed_torque.torch[i, 0, 0].item() == force
assert articulation.permanent_wrench_composer.out_force_b.torch[i, 0, 0].item() == force
assert articulation.permanent_wrench_composer.out_torque_b.torch[i, 0, 0].item() == force

# Check if the instantaneous wrench is correctly added to the permanent wrench
articulation.instantaneous_wrench_composer.add_forces_and_torques_index(
Expand Down Expand Up @@ -1724,10 +1724,10 @@ def test_reset(sim, num_articulations, device, articulation_type):
# Reset should zero external forces and torques
assert not articulation._instantaneous_wrench_composer.active
assert not articulation._permanent_wrench_composer.active
assert torch.count_nonzero(articulation._instantaneous_wrench_composer.composed_force.torch) == 0
assert torch.count_nonzero(articulation._instantaneous_wrench_composer.composed_torque.torch) == 0
assert torch.count_nonzero(articulation._permanent_wrench_composer.composed_force.torch) == 0
assert torch.count_nonzero(articulation._permanent_wrench_composer.composed_torque.torch) == 0
assert torch.count_nonzero(articulation._instantaneous_wrench_composer.out_force_b.torch) == 0
assert torch.count_nonzero(articulation._instantaneous_wrench_composer.out_torque_b.torch) == 0
assert torch.count_nonzero(articulation._permanent_wrench_composer.out_force_b.torch) == 0
assert torch.count_nonzero(articulation._permanent_wrench_composer.out_torque_b.torch) == 0

if num_articulations > 1:
num_bodies = articulation.num_bodies
Expand All @@ -1742,10 +1742,10 @@ def test_reset(sim, num_articulations, device, articulation_type):
articulation.reset(env_ids=torch.tensor([0], device=device))
assert articulation._instantaneous_wrench_composer.active
assert articulation._permanent_wrench_composer.active
assert torch.count_nonzero(articulation._instantaneous_wrench_composer.composed_force.torch) == num_bodies * 3
assert torch.count_nonzero(articulation._instantaneous_wrench_composer.composed_torque.torch) == num_bodies * 3
assert torch.count_nonzero(articulation._permanent_wrench_composer.composed_force.torch) == num_bodies * 3
assert torch.count_nonzero(articulation._permanent_wrench_composer.composed_torque.torch) == num_bodies * 3
assert torch.count_nonzero(articulation._instantaneous_wrench_composer.out_force_b.torch) == num_bodies * 3
assert torch.count_nonzero(articulation._instantaneous_wrench_composer.out_torque_b.torch) == num_bodies * 3
assert torch.count_nonzero(articulation._permanent_wrench_composer.out_force_b.torch) == num_bodies * 3
assert torch.count_nonzero(articulation._permanent_wrench_composer.out_torque_b.torch) == num_bodies * 3


@pytest.mark.isaacsim_ci
Expand Down
12 changes: 6 additions & 6 deletions source/isaaclab_newton/test/assets/test_rigid_object.py
Original file line number Diff line number Diff line change
Expand Up @@ -276,8 +276,8 @@ def test_external_force_buffer(device):

# check if the cube's force and torque buffers are correctly updated
for i in range(cube_object.num_instances):
assert cube_object._permanent_wrench_composer.composed_force.torch[i, 0, 0].item() == force
assert cube_object._permanent_wrench_composer.composed_torque.torch[i, 0, 0].item() == force
assert cube_object._permanent_wrench_composer.out_force_b.torch[i, 0, 0].item() == force
assert cube_object._permanent_wrench_composer.out_torque_b.torch[i, 0, 0].item() == force

# Check if the instantaneous wrench is correctly added to the permanent wrench
cube_object.permanent_wrench_composer.add_forces_and_torques_index(
Expand Down Expand Up @@ -565,10 +565,10 @@ def test_reset_rigid_object(num_cubes, device):
# Reset should zero external forces and torques
assert not cube_object._instantaneous_wrench_composer.active
assert not cube_object._permanent_wrench_composer.active
assert torch.count_nonzero(cube_object._instantaneous_wrench_composer.composed_force.torch) == 0
assert torch.count_nonzero(cube_object._instantaneous_wrench_composer.composed_torque.torch) == 0
assert torch.count_nonzero(cube_object._permanent_wrench_composer.composed_force.torch) == 0
assert torch.count_nonzero(cube_object._permanent_wrench_composer.composed_torque.torch) == 0
assert torch.count_nonzero(cube_object._instantaneous_wrench_composer.out_force_b.torch) == 0
assert torch.count_nonzero(cube_object._instantaneous_wrench_composer.out_torque_b.torch) == 0
assert torch.count_nonzero(cube_object._permanent_wrench_composer.out_force_b.torch) == 0
assert torch.count_nonzero(cube_object._permanent_wrench_composer.out_torque_b.torch) == 0


@pytest.mark.isaacsim_ci
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -241,8 +241,8 @@ def test_external_force_buffer(device):

# check if the object collection's force and torque buffers are correctly updated
for i in range(num_envs):
assert object_collection._permanent_wrench_composer.composed_force.torch[i, 0, 0].item() == force
assert object_collection._permanent_wrench_composer.composed_torque.torch[i, 0, 0].item() == force
assert object_collection._permanent_wrench_composer.out_force_b.torch[i, 0, 0].item() == force
assert object_collection._permanent_wrench_composer.out_torque_b.torch[i, 0, 0].item() == force

object_collection.instantaneous_wrench_composer.add_forces_and_torques_index(
body_ids=object_ids,
Expand Down Expand Up @@ -502,10 +502,10 @@ def test_reset_object_collection(num_envs, num_cubes, device):
# Reset should zero external forces and torques
assert not object_collection._instantaneous_wrench_composer.active
assert not object_collection._permanent_wrench_composer.active
assert torch.count_nonzero(object_collection._instantaneous_wrench_composer.composed_force.torch) == 0
assert torch.count_nonzero(object_collection._instantaneous_wrench_composer.composed_torque.torch) == 0
assert torch.count_nonzero(object_collection._permanent_wrench_composer.composed_force.torch) == 0
assert torch.count_nonzero(object_collection._permanent_wrench_composer.composed_torque.torch) == 0
assert torch.count_nonzero(object_collection._instantaneous_wrench_composer.out_force_b.torch) == 0
assert torch.count_nonzero(object_collection._instantaneous_wrench_composer.out_torque_b.torch) == 0
assert torch.count_nonzero(object_collection._permanent_wrench_composer.out_force_b.torch) == 0
assert torch.count_nonzero(object_collection._permanent_wrench_composer.out_torque_b.torch) == 0


@pytest.mark.parametrize("num_envs", [1, 3])
Expand Down
Loading
Loading