diff --git a/source/isaaclab_newton/changelog.d/antoiner-backend-deprecation-warning-cleanup.rst b/source/isaaclab_newton/changelog.d/antoiner-backend-deprecation-warning-cleanup.rst new file mode 100644 index 000000000000..2fd8df763f82 --- /dev/null +++ b/source/isaaclab_newton/changelog.d/antoiner-backend-deprecation-warning-cleanup.rst @@ -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. diff --git a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py index 2bb2f1f37397..f73d2cb3bbcb 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py @@ -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, ) diff --git a/source/isaaclab_newton/isaaclab_newton/sensors/contact_sensor/contact_sensor.py b/source/isaaclab_newton/isaaclab_newton/sensors/contact_sensor/contact_sensor.py index 02850e9cc903..43e878fecbfb 100644 --- a/source/isaaclab_newton/isaaclab_newton/sensors/contact_sensor/contact_sensor.py +++ b/source/isaaclab_newton/isaaclab_newton/sensors/contact_sensor/contact_sensor.py @@ -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( @@ -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( @@ -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, diff --git a/source/isaaclab_newton/isaaclab_newton/sensors/contact_sensor/contact_sensor_kernels.py b/source/isaaclab_newton/isaaclab_newton/sensors/contact_sensor/contact_sensor_kernels.py index 324f0106682f..81fef0bcab63 100644 --- a/source/isaaclab_newton/isaaclab_newton/sensors/contact_sensor/contact_sensor_kernels.py +++ b/source/isaaclab_newton/isaaclab_newton/sensors/contact_sensor/contact_sensor_kernels.py @@ -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 @@ -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 diff --git a/source/isaaclab_newton/test/assets/test_articulation.py b/source/isaaclab_newton/test/assets/test_articulation.py index a5eacf95045e..1d6cc79af8f1 100644 --- a/source/isaaclab_newton/test/assets/test_articulation.py +++ b/source/isaaclab_newton/test/assets/test_articulation.py @@ -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( @@ -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 @@ -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 diff --git a/source/isaaclab_newton/test/assets/test_rigid_object.py b/source/isaaclab_newton/test/assets/test_rigid_object.py index 3b8e3aa9bf85..67796416dce6 100644 --- a/source/isaaclab_newton/test/assets/test_rigid_object.py +++ b/source/isaaclab_newton/test/assets/test_rigid_object.py @@ -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( @@ -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 diff --git a/source/isaaclab_newton/test/assets/test_rigid_object_collection.py b/source/isaaclab_newton/test/assets/test_rigid_object_collection.py index 0873da8ecf22..cec62a98bcd3 100644 --- a/source/isaaclab_newton/test/assets/test_rigid_object_collection.py +++ b/source/isaaclab_newton/test/assets/test_rigid_object_collection.py @@ -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, @@ -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]) diff --git a/source/isaaclab_newton/test/sensors/test_frame_transformer.py b/source/isaaclab_newton/test/sensors/test_frame_transformer.py index 513ba9dda918..236fb53f25d9 100644 --- a/source/isaaclab_newton/test/sensors/test_frame_transformer.py +++ b/source/isaaclab_newton/test/sensors/test_frame_transformer.py @@ -168,21 +168,28 @@ def test_frame_transformer_feet_wrt_base(sim): # # reset if count % 25 == 0: # reset root state - root_state = scene.articulations["robot"].data.default_root_state.torch.clone() + root_state = torch.cat( + ( + scene.articulations["robot"].data.default_root_pose.torch, + scene.articulations["robot"].data.default_root_vel.torch, + ), + dim=-1, + ).clone() root_state[:, :3] += scene.env_origins joint_pos = scene.articulations["robot"].data.default_joint_pos.torch joint_vel = scene.articulations["robot"].data.default_joint_vel.torch # -- set root state # -- robot - scene.articulations["robot"].write_root_pose_to_sim(root_state[:, :7]) - scene.articulations["robot"].write_root_velocity_to_sim(root_state[:, 7:]) - scene.articulations["robot"].write_joint_state_to_sim(joint_pos, joint_vel) + scene.articulations["robot"].write_root_pose_to_sim_index(root_pose=root_state[:, :7]) + scene.articulations["robot"].write_root_velocity_to_sim_index(root_velocity=root_state[:, 7:]) + scene.articulations["robot"].write_joint_position_to_sim_index(position=joint_pos) + scene.articulations["robot"].write_joint_velocity_to_sim_index(velocity=joint_vel) # reset buffers scene.reset() # set joint targets robot_actions = default_actions + 0.5 * torch.randn_like(default_actions) - scene.articulations["robot"].set_joint_position_target(robot_actions) + scene.articulations["robot"].set_joint_position_target_index(target=robot_actions) # write data to sim scene.write_data_to_sim() # perform step @@ -266,21 +273,28 @@ def test_frame_transformer_feet_wrt_thigh(sim): # # reset if count % 25 == 0: # reset root state - root_state = scene.articulations["robot"].data.default_root_state.torch.clone() + root_state = torch.cat( + ( + scene.articulations["robot"].data.default_root_pose.torch, + scene.articulations["robot"].data.default_root_vel.torch, + ), + dim=-1, + ).clone() root_state[:, :3] += scene.env_origins joint_pos = scene.articulations["robot"].data.default_joint_pos.torch joint_vel = scene.articulations["robot"].data.default_joint_vel.torch # -- set root state # -- robot - scene.articulations["robot"].write_root_pose_to_sim(root_state[:, :7]) - scene.articulations["robot"].write_root_velocity_to_sim(root_state[:, 7:]) - scene.articulations["robot"].write_joint_state_to_sim(joint_pos, joint_vel) + scene.articulations["robot"].write_root_pose_to_sim_index(root_pose=root_state[:, :7]) + scene.articulations["robot"].write_root_velocity_to_sim_index(root_velocity=root_state[:, 7:]) + scene.articulations["robot"].write_joint_position_to_sim_index(position=joint_pos) + scene.articulations["robot"].write_joint_velocity_to_sim_index(velocity=joint_vel) # reset buffers scene.reset() # set joint targets robot_actions = default_actions + 0.5 * torch.randn_like(default_actions) - scene.articulations["robot"].set_joint_position_target(robot_actions) + scene.articulations["robot"].set_joint_position_target_index(target=robot_actions) # write data to sim scene.write_data_to_sim() # perform step @@ -344,21 +358,28 @@ def test_frame_transformer_robot_body_to_external_cube(sim): # # reset if count % 25 == 0: # reset root state - root_state = scene.articulations["robot"].data.default_root_state.torch.clone() + root_state = torch.cat( + ( + scene.articulations["robot"].data.default_root_pose.torch, + scene.articulations["robot"].data.default_root_vel.torch, + ), + dim=-1, + ).clone() root_state[:, :3] += scene.env_origins joint_pos = scene.articulations["robot"].data.default_joint_pos.torch joint_vel = scene.articulations["robot"].data.default_joint_vel.torch # -- set root state # -- robot - scene.articulations["robot"].write_root_pose_to_sim(root_state[:, :7]) - scene.articulations["robot"].write_root_velocity_to_sim(root_state[:, 7:]) - scene.articulations["robot"].write_joint_state_to_sim(joint_pos, joint_vel) + scene.articulations["robot"].write_root_pose_to_sim_index(root_pose=root_state[:, :7]) + scene.articulations["robot"].write_root_velocity_to_sim_index(root_velocity=root_state[:, 7:]) + scene.articulations["robot"].write_joint_position_to_sim_index(position=joint_pos) + scene.articulations["robot"].write_joint_velocity_to_sim_index(velocity=joint_vel) # reset buffers scene.reset() # set joint targets robot_actions = default_actions + 0.5 * torch.randn_like(default_actions) - scene.articulations["robot"].set_joint_position_target(robot_actions) + scene.articulations["robot"].set_joint_position_target_index(target=robot_actions) # write data to sim scene.write_data_to_sim() # perform step @@ -440,12 +461,18 @@ def test_frame_transformer_offset_frames(sim): # # reset if count % 25 == 0: # reset root state - root_state = scene["cube"].data.default_root_state.torch.clone() + root_state = torch.cat( + ( + scene["cube"].data.default_root_pose.torch, + scene["cube"].data.default_root_vel.torch, + ), + dim=-1, + ).clone() root_state[:, :3] += scene.env_origins # -- set root state # -- cube - scene["cube"].write_root_pose_to_sim(root_state[:, :7]) - scene["cube"].write_root_velocity_to_sim(root_state[:, 7:]) + scene["cube"].write_root_pose_to_sim_index(root_pose=root_state[:, :7]) + scene["cube"].write_root_velocity_to_sim_index(root_velocity=root_state[:, 7:]) # reset buffers scene.reset() @@ -532,21 +559,28 @@ def test_frame_transformer_all_bodies(sim): # # reset if count % 25 == 0: # reset root state - root_state = scene.articulations["robot"].data.default_root_state.torch.clone() + root_state = torch.cat( + ( + scene.articulations["robot"].data.default_root_pose.torch, + scene.articulations["robot"].data.default_root_vel.torch, + ), + dim=-1, + ).clone() root_state[:, :3] += scene.env_origins joint_pos = scene.articulations["robot"].data.default_joint_pos.torch joint_vel = scene.articulations["robot"].data.default_joint_vel.torch # -- set root state # -- robot - scene.articulations["robot"].write_root_pose_to_sim(root_state[:, :7]) - scene.articulations["robot"].write_root_velocity_to_sim(root_state[:, 7:]) - scene.articulations["robot"].write_joint_state_to_sim(joint_pos, joint_vel) + scene.articulations["robot"].write_root_pose_to_sim_index(root_pose=root_state[:, :7]) + scene.articulations["robot"].write_root_velocity_to_sim_index(root_velocity=root_state[:, 7:]) + scene.articulations["robot"].write_joint_position_to_sim_index(position=joint_pos) + scene.articulations["robot"].write_joint_velocity_to_sim_index(velocity=joint_vel) # reset buffers scene.reset() # set joint targets robot_actions = default_actions + 0.5 * torch.randn_like(default_actions) - scene.articulations["robot"].set_joint_position_target(robot_actions) + scene.articulations["robot"].set_joint_position_target_index(target=robot_actions) # write data to sim scene.write_data_to_sim() # perform step @@ -727,22 +761,38 @@ class MultiRobotSceneCfg(InteractiveSceneCfg): # Reset periodically if count % 10 == 0: # Reset robot - root_state = scene.articulations["robot"].data.default_root_state.torch.clone() + root_state = torch.cat( + ( + scene.articulations["robot"].data.default_root_pose.torch, + scene.articulations["robot"].data.default_root_vel.torch, + ), + dim=-1, + ).clone() root_state[:, :3] += scene.env_origins - scene.articulations["robot"].write_root_pose_to_sim(root_state[:, :7]) - scene.articulations["robot"].write_root_velocity_to_sim(root_state[:, 7:]) - scene.articulations["robot"].write_joint_state_to_sim( - scene.articulations["robot"].data.default_joint_pos.torch, - scene.articulations["robot"].data.default_joint_vel.torch, + scene.articulations["robot"].write_root_pose_to_sim_index(root_pose=root_state[:, :7]) + scene.articulations["robot"].write_root_velocity_to_sim_index(root_velocity=root_state[:, 7:]) + scene.articulations["robot"].write_joint_position_to_sim_index( + position=scene.articulations["robot"].data.default_joint_pos.torch + ) + scene.articulations["robot"].write_joint_velocity_to_sim_index( + velocity=scene.articulations["robot"].data.default_joint_vel.torch ) # Reset robot_1 - root_state_1 = scene.articulations["robot_1"].data.default_root_state.torch.clone() + root_state_1 = torch.cat( + ( + scene.articulations["robot_1"].data.default_root_pose.torch, + scene.articulations["robot_1"].data.default_root_vel.torch, + ), + dim=-1, + ).clone() root_state_1[:, :3] += scene.env_origins - scene.articulations["robot_1"].write_root_pose_to_sim(root_state_1[:, :7]) - scene.articulations["robot_1"].write_root_velocity_to_sim(root_state_1[:, 7:]) - scene.articulations["robot_1"].write_joint_state_to_sim( - scene.articulations["robot_1"].data.default_joint_pos.torch, - scene.articulations["robot_1"].data.default_joint_vel.torch, + scene.articulations["robot_1"].write_root_pose_to_sim_index(root_pose=root_state_1[:, :7]) + scene.articulations["robot_1"].write_root_velocity_to_sim_index(root_velocity=root_state_1[:, 7:]) + scene.articulations["robot_1"].write_joint_position_to_sim_index( + position=scene.articulations["robot_1"].data.default_joint_pos.torch + ) + scene.articulations["robot_1"].write_joint_velocity_to_sim_index( + velocity=scene.articulations["robot_1"].data.default_joint_vel.torch ) scene.reset() diff --git a/source/isaaclab_physx/changelog.d/antoiner-backend-deprecation-warning-cleanup.rst b/source/isaaclab_physx/changelog.d/antoiner-backend-deprecation-warning-cleanup.rst new file mode 100644 index 000000000000..bdec42289b0d --- /dev/null +++ b/source/isaaclab_physx/changelog.d/antoiner-backend-deprecation-warning-cleanup.rst @@ -0,0 +1,5 @@ +Fixed +^^^^^ + +* Fixed PhysX backend tests to use current contact sensor and asset API names, + removing deprecation warnings from scoped test runs. diff --git a/source/isaaclab_physx/test/assets/test_articulation.py b/source/isaaclab_physx/test/assets/test_articulation.py index 3687dae5961d..f9052fcd6a0b 100644 --- a/source/isaaclab_physx/test/assets/test_articulation.py +++ b/source/isaaclab_physx/test/assets/test_articulation.py @@ -844,8 +844,8 @@ def test_external_force_buffer(sim, num_articulations, device): # 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( @@ -1559,10 +1559,10 @@ def test_reset(sim, num_articulations, device): # 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 @@ -1577,10 +1577,10 @@ def test_reset(sim, num_articulations, device): 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.parametrize("num_articulations", [1, 2]) diff --git a/source/isaaclab_physx/test/assets/test_rigid_object.py b/source/isaaclab_physx/test/assets/test_rigid_object.py index 6a2211787e19..b7f422c4f2f0 100644 --- a/source/isaaclab_physx/test/assets/test_rigid_object.py +++ b/source/isaaclab_physx/test/assets/test_rigid_object.py @@ -253,8 +253,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( @@ -426,13 +426,13 @@ def test_external_force_on_single_body_at_position(num_cubes, device): is_global=is_global, ) torch.testing.assert_close( - cube_object._permanent_wrench_composer.composed_force.torch[:, 0, :], + cube_object._permanent_wrench_composer.out_force_b.torch[:, 0, :], desired_force[:, 0, :], rtol=1e-6, atol=1e-7, ) torch.testing.assert_close( - cube_object._permanent_wrench_composer.composed_torque.torch[:, 0, :], + cube_object._permanent_wrench_composer.out_torque_b.torch[:, 0, :], desired_torque[:, 0, :], rtol=1e-6, atol=1e-7, @@ -557,10 +557,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.parametrize("num_cubes", [1, 2]) diff --git a/source/isaaclab_physx/test/assets/test_rigid_object_collection.py b/source/isaaclab_physx/test/assets/test_rigid_object_collection.py index 8401cc395a0c..f42f6dfcb085 100644 --- a/source/isaaclab_physx/test/assets/test_rigid_object_collection.py +++ b/source/isaaclab_physx/test/assets/test_rigid_object_collection.py @@ -259,8 +259,8 @@ def test_external_force_buffer(sim, 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, @@ -681,10 +681,10 @@ def test_reset_object_collection(sim, 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]) diff --git a/source/isaaclab_physx/test/sensors/test_contact_sensor.py b/source/isaaclab_physx/test/sensors/test_contact_sensor.py index 00772e6cb0d3..685d9204b3ef 100644 --- a/source/isaaclab_physx/test/sensors/test_contact_sensor.py +++ b/source/isaaclab_physx/test/sensors/test_contact_sensor.py @@ -480,7 +480,7 @@ def test_friction_reporting(setup_simulation, grav_dir): sim.reset() scene["contact_sensor"].reset() - scene["shape"].write_root_pose_to_sim( + scene["shape"].write_root_pose_to_sim_index( root_pose=torch.tensor([0, 0.0, CUBE_CFG.spawn.size[2] / 2.0, 1, 0, 0, 0], device=device).unsqueeze(0) ) @@ -703,7 +703,7 @@ def _test_sensor_contact( duration = durations[idx] while current_test_time < duration: # set object states to contact the ground plane - shape.write_root_pose_to_sim(root_pose=torch.tensor(test_pose, device=shape.device).unsqueeze(0)) + shape.write_root_pose_to_sim_index(root_pose=torch.tensor(test_pose, device=shape.device).unsqueeze(0)) # perform simulation step _perform_sim_step(sim, scene, sim_dt) # increment contact time @@ -735,7 +735,7 @@ def _test_sensor_contact( _test_friction_forces(shape, sensor, mode) # switch the contact mode for 1 dt step before the next contact test begins. - shape.write_root_pose_to_sim(root_pose=torch.tensor(reset_pose, device=shape.device).unsqueeze(0)) + shape.write_root_pose_to_sim_index(root_pose=torch.tensor(reset_pose, device=shape.device).unsqueeze(0)) # perform simulation step _perform_sim_step(sim, scene, sim_dt) # set the last air time to 2 sim_dt steps, because last_air_time and last_contact_time @@ -750,9 +750,9 @@ def _test_friction_forces(shape: RigidObject, sensor: ContactSensor, mode: Conta return # check shape of the friction_forces_w tensor (wp.to_torch expands vec3f -> float32 trailing dim) - num_bodies = sensor.num_bodies + num_sensors = sensor.num_sensors friction_torch = sensor._data.friction_forces_w.torch - assert friction_torch.shape == (sensor.num_instances // num_bodies, num_bodies, 1, 3) + assert friction_torch.shape == (sensor.num_instances // num_sensors, num_sensors, 1, 3) # compare friction forces if mode == ContactTestMode.IN_CONTACT: assert torch.any(torch.abs(friction_torch) > 1e-5).item() @@ -762,14 +762,14 @@ def _test_friction_forces(shape: RigidObject, sensor: ContactSensor, mode: Conta friction_forces_t = wp.to_torch(friction_forces) buffer_count_t = wp.to_torch(buffer_count).to(torch.int32) buffer_start_t = wp.to_torch(buffer_start_indices).to(torch.int32) - for i in range(sensor.num_instances * num_bodies): + for i in range(sensor.num_instances * num_sensors): for j in range(sensor.contact_view.filter_count): start_index_ij = buffer_start_t[i, j] count_ij = buffer_count_t[i, j] force = torch.sum(friction_forces_t[start_index_ij : (start_index_ij + count_ij), :], dim=0) - env_idx = i // num_bodies - body_idx = i % num_bodies - assert torch.allclose(force, friction_torch[env_idx, body_idx, j, :], atol=1e-5) + env_idx = i // num_sensors + sensor_idx = i % num_sensors + assert torch.allclose(force, friction_torch[env_idx, sensor_idx, j, :], atol=1e-5) elif mode == ContactTestMode.NON_CONTACT: assert torch.all(friction_torch == 0.0).item() @@ -788,9 +788,9 @@ def _test_contact_position(shape: RigidObject, sensor: ContactSensor, mode: Cont return # check shape of the contact_pos_w tensor (wp.to_torch expands vec3f -> float32 trailing dim) - num_bodies = sensor.num_bodies + num_sensors = sensor.num_sensors contact_pos_torch = sensor._data.contact_pos_w.torch - assert contact_pos_torch.shape == (sensor.num_instances // num_bodies, num_bodies, 1, 3) + assert contact_pos_torch.shape == (sensor.num_instances // num_sensors, num_sensors, 1, 3) # check contact positions if mode == ContactTestMode.IN_CONTACT: pos_w_torch = sensor._data.pos_w.torch diff --git a/source/isaaclab_physx/test/sensors/test_frame_transformer.py b/source/isaaclab_physx/test/sensors/test_frame_transformer.py index 631e9ba118dd..28559f9b6da5 100644 --- a/source/isaaclab_physx/test/sensors/test_frame_transformer.py +++ b/source/isaaclab_physx/test/sensors/test_frame_transformer.py @@ -153,21 +153,28 @@ def test_frame_transformer_feet_wrt_base(sim): # # reset if count % 25 == 0: # reset root state - root_state = scene.articulations["robot"].data.default_root_state.torch.clone() + root_state = torch.cat( + ( + scene.articulations["robot"].data.default_root_pose.torch, + scene.articulations["robot"].data.default_root_vel.torch, + ), + dim=-1, + ).clone() root_state[:, :3] += scene.env_origins joint_pos = scene.articulations["robot"].data.default_joint_pos.torch joint_vel = scene.articulations["robot"].data.default_joint_vel.torch # -- set root state # -- robot - scene.articulations["robot"].write_root_pose_to_sim(root_state[:, :7]) - scene.articulations["robot"].write_root_velocity_to_sim(root_state[:, 7:]) - scene.articulations["robot"].write_joint_state_to_sim(joint_pos, joint_vel) + scene.articulations["robot"].write_root_pose_to_sim_index(root_pose=root_state[:, :7]) + scene.articulations["robot"].write_root_velocity_to_sim_index(root_velocity=root_state[:, 7:]) + scene.articulations["robot"].write_joint_position_to_sim_index(position=joint_pos) + scene.articulations["robot"].write_joint_velocity_to_sim_index(velocity=joint_vel) # reset buffers scene.reset() # set joint targets robot_actions = default_actions + 0.5 * torch.randn_like(default_actions) - scene.articulations["robot"].set_joint_position_target(robot_actions) + scene.articulations["robot"].set_joint_position_target_index(target=robot_actions) # write data to sim scene.write_data_to_sim() # perform step @@ -251,21 +258,28 @@ def test_frame_transformer_feet_wrt_thigh(sim): # # reset if count % 25 == 0: # reset root state - root_state = scene.articulations["robot"].data.default_root_state.torch.clone() + root_state = torch.cat( + ( + scene.articulations["robot"].data.default_root_pose.torch, + scene.articulations["robot"].data.default_root_vel.torch, + ), + dim=-1, + ).clone() root_state[:, :3] += scene.env_origins joint_pos = scene.articulations["robot"].data.default_joint_pos.torch joint_vel = scene.articulations["robot"].data.default_joint_vel.torch # -- set root state # -- robot - scene.articulations["robot"].write_root_pose_to_sim(root_state[:, :7]) - scene.articulations["robot"].write_root_velocity_to_sim(root_state[:, 7:]) - scene.articulations["robot"].write_joint_state_to_sim(joint_pos, joint_vel) + scene.articulations["robot"].write_root_pose_to_sim_index(root_pose=root_state[:, :7]) + scene.articulations["robot"].write_root_velocity_to_sim_index(root_velocity=root_state[:, 7:]) + scene.articulations["robot"].write_joint_position_to_sim_index(position=joint_pos) + scene.articulations["robot"].write_joint_velocity_to_sim_index(velocity=joint_vel) # reset buffers scene.reset() # set joint targets robot_actions = default_actions + 0.5 * torch.randn_like(default_actions) - scene.articulations["robot"].set_joint_position_target(robot_actions) + scene.articulations["robot"].set_joint_position_target_index(target=robot_actions) # write data to sim scene.write_data_to_sim() # perform step @@ -329,21 +343,28 @@ def test_frame_transformer_robot_body_to_external_cube(sim): # # reset if count % 25 == 0: # reset root state - root_state = scene.articulations["robot"].data.default_root_state.torch.clone() + root_state = torch.cat( + ( + scene.articulations["robot"].data.default_root_pose.torch, + scene.articulations["robot"].data.default_root_vel.torch, + ), + dim=-1, + ).clone() root_state[:, :3] += scene.env_origins joint_pos = scene.articulations["robot"].data.default_joint_pos.torch joint_vel = scene.articulations["robot"].data.default_joint_vel.torch # -- set root state # -- robot - scene.articulations["robot"].write_root_pose_to_sim(root_state[:, :7]) - scene.articulations["robot"].write_root_velocity_to_sim(root_state[:, 7:]) - scene.articulations["robot"].write_joint_state_to_sim(joint_pos, joint_vel) + scene.articulations["robot"].write_root_pose_to_sim_index(root_pose=root_state[:, :7]) + scene.articulations["robot"].write_root_velocity_to_sim_index(root_velocity=root_state[:, 7:]) + scene.articulations["robot"].write_joint_position_to_sim_index(position=joint_pos) + scene.articulations["robot"].write_joint_velocity_to_sim_index(velocity=joint_vel) # reset buffers scene.reset() # set joint targets robot_actions = default_actions + 0.5 * torch.randn_like(default_actions) - scene.articulations["robot"].set_joint_position_target(robot_actions) + scene.articulations["robot"].set_joint_position_target_index(target=robot_actions) # write data to sim scene.write_data_to_sim() # perform step @@ -425,12 +446,18 @@ def test_frame_transformer_offset_frames(sim): # # reset if count % 25 == 0: # reset root state - root_state = scene["cube"].data.default_root_state.torch.clone() + root_state = torch.cat( + ( + scene["cube"].data.default_root_pose.torch, + scene["cube"].data.default_root_vel.torch, + ), + dim=-1, + ).clone() root_state[:, :3] += scene.env_origins # -- set root state # -- cube - scene["cube"].write_root_pose_to_sim(root_state[:, :7]) - scene["cube"].write_root_velocity_to_sim(root_state[:, 7:]) + scene["cube"].write_root_pose_to_sim_index(root_pose=root_state[:, :7]) + scene["cube"].write_root_velocity_to_sim_index(root_velocity=root_state[:, 7:]) # reset buffers scene.reset() @@ -513,21 +540,28 @@ def test_frame_transformer_all_bodies(sim): # # reset if count % 25 == 0: # reset root state - root_state = scene.articulations["robot"].data.default_root_state.torch.clone() + root_state = torch.cat( + ( + scene.articulations["robot"].data.default_root_pose.torch, + scene.articulations["robot"].data.default_root_vel.torch, + ), + dim=-1, + ).clone() root_state[:, :3] += scene.env_origins joint_pos = scene.articulations["robot"].data.default_joint_pos.torch joint_vel = scene.articulations["robot"].data.default_joint_vel.torch # -- set root state # -- robot - scene.articulations["robot"].write_root_pose_to_sim(root_state[:, :7]) - scene.articulations["robot"].write_root_velocity_to_sim(root_state[:, 7:]) - scene.articulations["robot"].write_joint_state_to_sim(joint_pos, joint_vel) + scene.articulations["robot"].write_root_pose_to_sim_index(root_pose=root_state[:, :7]) + scene.articulations["robot"].write_root_velocity_to_sim_index(root_velocity=root_state[:, 7:]) + scene.articulations["robot"].write_joint_position_to_sim_index(position=joint_pos) + scene.articulations["robot"].write_joint_velocity_to_sim_index(velocity=joint_vel) # reset buffers scene.reset() # set joint targets robot_actions = default_actions + 0.5 * torch.randn_like(default_actions) - scene.articulations["robot"].set_joint_position_target(robot_actions) + scene.articulations["robot"].set_joint_position_target_index(target=robot_actions) # write data to sim scene.write_data_to_sim() # perform step @@ -708,22 +742,38 @@ class MultiRobotSceneCfg(InteractiveSceneCfg): # Reset periodically if count % 10 == 0: # Reset robot - root_state = scene.articulations["robot"].data.default_root_state.torch.clone() + root_state = torch.cat( + ( + scene.articulations["robot"].data.default_root_pose.torch, + scene.articulations["robot"].data.default_root_vel.torch, + ), + dim=-1, + ).clone() root_state[:, :3] += scene.env_origins - scene.articulations["robot"].write_root_pose_to_sim(root_state[:, :7]) - scene.articulations["robot"].write_root_velocity_to_sim(root_state[:, 7:]) - scene.articulations["robot"].write_joint_state_to_sim( - scene.articulations["robot"].data.default_joint_pos.torch, - scene.articulations["robot"].data.default_joint_vel.torch, + scene.articulations["robot"].write_root_pose_to_sim_index(root_pose=root_state[:, :7]) + scene.articulations["robot"].write_root_velocity_to_sim_index(root_velocity=root_state[:, 7:]) + scene.articulations["robot"].write_joint_position_to_sim_index( + position=scene.articulations["robot"].data.default_joint_pos.torch + ) + scene.articulations["robot"].write_joint_velocity_to_sim_index( + velocity=scene.articulations["robot"].data.default_joint_vel.torch ) # Reset robot_1 - root_state_1 = scene.articulations["robot_1"].data.default_root_state.torch.clone() + root_state_1 = torch.cat( + ( + scene.articulations["robot_1"].data.default_root_pose.torch, + scene.articulations["robot_1"].data.default_root_vel.torch, + ), + dim=-1, + ).clone() root_state_1[:, :3] += scene.env_origins - scene.articulations["robot_1"].write_root_pose_to_sim(root_state_1[:, :7]) - scene.articulations["robot_1"].write_root_velocity_to_sim(root_state_1[:, 7:]) - scene.articulations["robot_1"].write_joint_state_to_sim( - scene.articulations["robot_1"].data.default_joint_pos.torch, - scene.articulations["robot_1"].data.default_joint_vel.torch, + scene.articulations["robot_1"].write_root_pose_to_sim_index(root_pose=root_state_1[:, :7]) + scene.articulations["robot_1"].write_root_velocity_to_sim_index(root_velocity=root_state_1[:, 7:]) + scene.articulations["robot_1"].write_joint_position_to_sim_index( + position=scene.articulations["robot_1"].data.default_joint_pos.torch + ) + scene.articulations["robot_1"].write_joint_velocity_to_sim_index( + velocity=scene.articulations["robot_1"].data.default_joint_vel.torch ) scene.reset()