diff --git a/source/isaaclab/config/extension.toml b/source/isaaclab/config/extension.toml index 56e7a261123..80ec2d5a370 100644 --- a/source/isaaclab/config/extension.toml +++ b/source/isaaclab/config/extension.toml @@ -1,7 +1,7 @@ [package] # Note: Semantic Versioning is used: https://semver.org/ -version = "4.3.0" +version = "4.3.2" # Description title = "Isaac Lab framework for Robot Learning" diff --git a/source/isaaclab/docs/CHANGELOG.rst b/source/isaaclab/docs/CHANGELOG.rst index bb67104d6eb..507f9ba07a2 100644 --- a/source/isaaclab/docs/CHANGELOG.rst +++ b/source/isaaclab/docs/CHANGELOG.rst @@ -1,6 +1,47 @@ Changelog --------- + +4.3.2 (2026-02-25) +~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixed inconsistent ``body_mass`` shape in :class:`~isaaclab.assets.BaseRigidObjectData` + from ``(num_instances, 1, 1)`` to ``(num_instances, 1)`` to align with the articulation + convention. + +* Unified inertia scaling in :func:`~isaaclab.envs.mdp.events.randomize_rigid_body_mass` + to use a single code path for both articulations and rigid objects. + +Changed +^^^^^^^ + +* Reworked mock interfaces for assets and sensors to align with updated data shapes and + remove stale convenience aliases. + + +4.3.1 (2026-02-27) +~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added :meth:`~isaaclab.assets.AssetBase.assert_shape_and_dtype` and + :meth:`~isaaclab.assets.AssetBase.assert_shape_and_dtype_mask` validation methods to + :class:`~isaaclab.assets.AssetBase` for runtime shape and dtype checking of write method + inputs. Checks are only active in debug mode (``__debug__``), adding zero overhead in + optimized builds. + +Changed +^^^^^^^ + +* Fixed tendon setter signatures in :class:`~isaaclab.assets.BaseArticulation` + (``set_fixed_tendon_*`` and ``set_spatial_tendon_*``) now accept ``float`` values in + addition to tensors and warp arrays. + + 4.3.0 (2026-02-26) ~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab/isaaclab/test/mock_interfaces/assets/mock_articulation.py b/source/isaaclab/isaaclab/test/mock_interfaces/assets/mock_articulation.py index 0c6cf19c3b1..faacd78b963 100644 --- a/source/isaaclab/isaaclab/test/mock_interfaces/assets/mock_articulation.py +++ b/source/isaaclab/isaaclab/test/mock_interfaces/assets/mock_articulation.py @@ -604,12 +604,12 @@ def body_mass(self) -> wp.array: @property def body_inertia(self) -> wp.array: - """Body inertias. Shape: (N, num_bodies, 3, 3).""" + """Body inertias (flattened 3x3). Shape: (N, num_bodies, 9).""" if self._body_inertia is None: - np_inertia = np.zeros((self._num_instances, self._num_bodies, 3, 3), dtype=np.float32) - np_inertia[..., 0, 0] = 1.0 - np_inertia[..., 1, 1] = 1.0 - np_inertia[..., 2, 2] = 1.0 + np_inertia = np.zeros((self._num_instances, self._num_bodies, 9), dtype=np.float32) + np_inertia[..., 0] = 1.0 # Ixx + np_inertia[..., 4] = 1.0 # Iyy + np_inertia[..., 8] = 1.0 # Izz return wp.array(np_inertia, dtype=wp.float32, device=self.device) return self._body_inertia @@ -655,15 +655,8 @@ def root_com_ang_vel_b(self) -> wp.array: """Root CoM angular velocity in body frame. Shape: (N, 3).""" return wp.clone(self.root_com_ang_vel_w, self.device) - @property - def com_pos_b(self) -> wp.array: - """Convenience alias for root_com_pos_w. Shape: (N, 3).""" - return wp.clone(self.root_com_pos_w, self.device) - - @property - def com_quat_b(self) -> wp.array: - """Convenience alias for root_com_quat_w. Shape: (N, 4).""" - return wp.clone(self.root_com_quat_w, self.device) + # com_pos_b and com_quat_b are inherited from BaseArticulationData + # (they delegate to body_com_pos_b and body_com_quat_b respectively) # -- Fixed tendon properties -- @@ -1070,7 +1063,11 @@ def permanent_wrench_composer(self) -> None: # -- Core methods -- - def reset(self, env_ids: Sequence[int] | None = None) -> None: + def reset( + self, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + env_mask: wp.array | None = None, + ) -> None: """Reset articulation state for specified environments.""" pass @@ -1091,47 +1088,50 @@ def find_bodies(self, name_keys: str | Sequence[str], preserve_order: bool = Fal def find_joints( self, name_keys: str | Sequence[str], - joint_subset: list[int] | None = None, + joint_subset: list[str] | None = None, preserve_order: bool = False, ) -> tuple[list[int], list[str]]: """Find joints by name regex patterns.""" names = self._joint_names if joint_subset is not None: - names = [names[i] for i in joint_subset] - indices, matched_names = self._find_by_regex(names, name_keys, preserve_order) - if joint_subset is not None: - indices = [joint_subset[i] for i in indices] - return indices, matched_names + subset_indices = [self._joint_names.index(n) for n in joint_subset] + names = joint_subset + indices, matched_names = self._find_by_regex(names, name_keys, preserve_order) + indices = [subset_indices[i] for i in indices] + return indices, matched_names + return self._find_by_regex(names, name_keys, preserve_order) def find_fixed_tendons( self, name_keys: str | Sequence[str], - tendon_subsets: list[int] | None = None, + tendon_subsets: list[str] | None = None, preserve_order: bool = False, ) -> tuple[list[int], list[str]]: """Find fixed tendons by name regex patterns.""" names = self._fixed_tendon_names if tendon_subsets is not None: - names = [names[i] for i in tendon_subsets] - indices, matched_names = self._find_by_regex(names, name_keys, preserve_order) - if tendon_subsets is not None: - indices = [tendon_subsets[i] for i in indices] - return indices, matched_names + subset_indices = [self._fixed_tendon_names.index(n) for n in tendon_subsets] + names = tendon_subsets + indices, matched_names = self._find_by_regex(names, name_keys, preserve_order) + indices = [subset_indices[i] for i in indices] + return indices, matched_names + return self._find_by_regex(names, name_keys, preserve_order) def find_spatial_tendons( self, name_keys: str | Sequence[str], - tendon_subsets: list[int] | None = None, + tendon_subsets: list[str] | None = None, preserve_order: bool = False, ) -> tuple[list[int], list[str]]: """Find spatial tendons by name regex patterns.""" names = self._spatial_tendon_names if tendon_subsets is not None: - names = [names[i] for i in tendon_subsets] - indices, matched_names = self._find_by_regex(names, name_keys, preserve_order) - if tendon_subsets is not None: - indices = [tendon_subsets[i] for i in indices] - return indices, matched_names + subset_indices = [self._spatial_tendon_names.index(n) for n in tendon_subsets] + names = tendon_subsets + indices, matched_names = self._find_by_regex(names, name_keys, preserve_order) + indices = [subset_indices[i] for i in indices] + return indices, matched_names + return self._find_by_regex(names, name_keys, preserve_order) def _find_by_regex( self, names: list[str], name_keys: str | Sequence[str], preserve_order: bool @@ -1165,127 +1165,127 @@ def _find_by_regex( def write_root_state_to_sim( self, - root_state: torch.Tensor, - env_ids: Sequence[int] | None = None, + root_state: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: """Write root state to simulation.""" pass def write_root_com_state_to_sim( self, - root_state: torch.Tensor, - env_ids: Sequence[int] | None = None, + root_state: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: """Write root CoM state to simulation.""" pass def write_root_link_state_to_sim( self, - root_state: torch.Tensor, - env_ids: Sequence[int] | None = None, + root_state: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: """Write root link state to simulation.""" pass def write_root_pose_to_sim( self, - root_pose: torch.Tensor, - env_ids: Sequence[int] | None = None, + root_pose: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: """Write root pose to simulation.""" pass def write_root_link_pose_to_sim( self, - root_pose: torch.Tensor, - env_ids: Sequence[int] | None = None, + root_pose: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: """Write root link pose to simulation.""" pass def write_root_com_pose_to_sim( self, - root_pose: torch.Tensor, - env_ids: Sequence[int] | None = None, + root_pose: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: """Write root CoM pose to simulation.""" pass def write_root_velocity_to_sim( self, - root_velocity: torch.Tensor, - env_ids: Sequence[int] | None = None, + root_velocity: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: """Write root velocity to simulation.""" pass def write_root_com_velocity_to_sim( self, - root_velocity: torch.Tensor, - env_ids: Sequence[int] | None = None, + root_velocity: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: """Write root CoM velocity to simulation.""" pass def write_root_link_velocity_to_sim( self, - root_velocity: torch.Tensor, - env_ids: Sequence[int] | None = None, + root_velocity: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: """Write root link velocity to simulation.""" pass def write_joint_state_to_sim( self, - position: torch.Tensor, - velocity: torch.Tensor, + position: torch.Tensor | wp.array, + velocity: torch.Tensor | wp.array, joint_ids: Sequence[int] | slice | None = None, - env_ids: Sequence[int] | None = None, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: """Write joint state to simulation.""" pass def write_joint_position_to_sim( self, - position: torch.Tensor, + position: torch.Tensor | wp.array, joint_ids: Sequence[int] | slice | None = None, - env_ids: Sequence[int] | None = None, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: """Write joint positions to simulation.""" pass def write_joint_velocity_to_sim( self, - velocity: torch.Tensor, + velocity: torch.Tensor | wp.array, joint_ids: Sequence[int] | slice | None = None, - env_ids: Sequence[int] | None = None, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: """Write joint velocities to simulation.""" pass def write_joint_stiffness_to_sim( self, - stiffness: torch.Tensor | float, + stiffness: torch.Tensor | float | wp.array, joint_ids: Sequence[int] | slice | None = None, - env_ids: Sequence[int] | None = None, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: """Write joint stiffness to simulation.""" pass def write_joint_damping_to_sim( self, - damping: torch.Tensor | float, + damping: torch.Tensor | float | wp.array, joint_ids: Sequence[int] | slice | None = None, - env_ids: Sequence[int] | None = None, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: """Write joint damping to simulation.""" pass def write_joint_position_limit_to_sim( self, - limits: torch.Tensor | float, + limits: torch.Tensor | float | wp.array, joint_ids: Sequence[int] | slice | None = None, - env_ids: Sequence[int] | None = None, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, warn_limit_violation: bool = True, ) -> None: """Write joint position limits to simulation.""" @@ -1293,36 +1293,36 @@ def write_joint_position_limit_to_sim( def write_joint_velocity_limit_to_sim( self, - limits: torch.Tensor | float, + limits: torch.Tensor | float | wp.array, joint_ids: Sequence[int] | slice | None = None, - env_ids: Sequence[int] | None = None, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: """Write joint velocity limits to simulation.""" pass def write_joint_effort_limit_to_sim( self, - limits: torch.Tensor | float, + limits: torch.Tensor | float | wp.array, joint_ids: Sequence[int] | slice | None = None, - env_ids: Sequence[int] | None = None, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: """Write joint effort limits to simulation.""" pass def write_joint_armature_to_sim( self, - armature: torch.Tensor | float, + armature: torch.Tensor | float | wp.array, joint_ids: Sequence[int] | slice | None = None, - env_ids: Sequence[int] | None = None, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: """Write joint armature to simulation.""" pass def write_joint_friction_coefficient_to_sim( self, - coeff: torch.Tensor | float, + coeff: torch.Tensor | float | wp.array, joint_ids: Sequence[int] | slice | None = None, - env_ids: Sequence[int] | None = None, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: """Write joint friction coefficient to simulation.""" pass @@ -1330,7 +1330,7 @@ def write_joint_friction_coefficient_to_sim( def write_joint_friction_to_sim( self, joint_ids: Sequence[int] | slice | None = None, - env_ids: Sequence[int] | None = None, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: """Write joint friction to simulation.""" pass @@ -1338,7 +1338,7 @@ def write_joint_friction_to_sim( def write_joint_limits_to_sim( self, joint_ids: Sequence[int] | slice | None = None, - env_ids: Sequence[int] | None = None, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: """Write joint limits to simulation.""" pass @@ -1347,39 +1347,39 @@ def write_joint_limits_to_sim( def set_masses( self, - masses: torch.Tensor, + masses: torch.Tensor | wp.array, body_ids: Sequence[int] | slice | None = None, - env_ids: Sequence[int] | None = None, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: """Set body masses.""" pass def set_coms( self, - coms: torch.Tensor, - body_ids: Sequence[int] | slice | None = None, - env_ids: Sequence[int] | None = None, + coms: torch.Tensor | wp.array, + body_ids: Sequence[int] | None = None, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: """Set body centers of mass.""" pass def set_inertias( self, - inertias: torch.Tensor, - body_ids: Sequence[int] | slice | None = None, - env_ids: Sequence[int] | None = None, + inertias: torch.Tensor | wp.array, + body_ids: Sequence[int] | None = None, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: """Set body inertias.""" pass def set_external_force_and_torque( self, - forces: torch.Tensor, - torques: torch.Tensor, - positions: torch.Tensor | None = None, + forces: torch.Tensor | wp.array, + torques: torch.Tensor | wp.array, + positions: torch.Tensor | wp.array | None = None, body_ids: Sequence[int] | slice | None = None, - env_ids: Sequence[int] | None = None, - is_global: bool = True, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + is_global: bool = False, ) -> None: """Set external forces and torques.""" pass @@ -1566,6 +1566,7 @@ def write_spatial_tendon_properties_to_sim( def write_root_pose_to_sim_index( self, + *, root_pose: torch.Tensor | wp.array, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: @@ -1573,6 +1574,7 @@ def write_root_pose_to_sim_index( def write_root_pose_to_sim_mask( self, + *, root_pose: torch.Tensor | wp.array, env_mask: wp.array | None = None, ) -> None: @@ -1580,6 +1582,7 @@ def write_root_pose_to_sim_mask( def write_root_link_pose_to_sim_index( self, + *, root_pose: torch.Tensor | wp.array, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: @@ -1587,6 +1590,7 @@ def write_root_link_pose_to_sim_index( def write_root_link_pose_to_sim_mask( self, + *, root_pose: torch.Tensor | wp.array, env_mask: wp.array | None = None, ) -> None: @@ -1594,6 +1598,7 @@ def write_root_link_pose_to_sim_mask( def write_root_com_pose_to_sim_index( self, + *, root_pose: torch.Tensor | wp.array, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: @@ -1601,6 +1606,7 @@ def write_root_com_pose_to_sim_index( def write_root_com_pose_to_sim_mask( self, + *, root_pose: torch.Tensor | wp.array, env_mask: wp.array | None = None, ) -> None: @@ -1608,6 +1614,7 @@ def write_root_com_pose_to_sim_mask( def write_root_velocity_to_sim_index( self, + *, root_velocity: torch.Tensor | wp.array, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: @@ -1615,6 +1622,7 @@ def write_root_velocity_to_sim_index( def write_root_velocity_to_sim_mask( self, + *, root_velocity: torch.Tensor | wp.array, env_mask: wp.array | None = None, ) -> None: @@ -1622,6 +1630,7 @@ def write_root_velocity_to_sim_mask( def write_root_com_velocity_to_sim_index( self, + *, root_velocity: torch.Tensor | wp.array, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: @@ -1629,6 +1638,7 @@ def write_root_com_velocity_to_sim_index( def write_root_com_velocity_to_sim_mask( self, + *, root_velocity: torch.Tensor | wp.array, env_mask: wp.array | None = None, ) -> None: @@ -1636,6 +1646,7 @@ def write_root_com_velocity_to_sim_mask( def write_root_link_velocity_to_sim_index( self, + *, root_velocity: torch.Tensor | wp.array, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: @@ -1643,6 +1654,7 @@ def write_root_link_velocity_to_sim_index( def write_root_link_velocity_to_sim_mask( self, + *, root_velocity: torch.Tensor | wp.array, env_mask: wp.array | None = None, ) -> None: @@ -1652,72 +1664,81 @@ def write_root_link_velocity_to_sim_mask( def write_joint_position_to_sim_index( self, + *, position: torch.Tensor | wp.array, - joint_ids: Sequence[int] | slice | None = None, - env_ids: Sequence[int] | slice | None = None, + joint_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: pass def write_joint_position_to_sim_mask( self, + *, position: torch.Tensor | wp.array, - env_mask: wp.array | None = None, joint_mask: wp.array | None = None, + env_mask: wp.array | None = None, ) -> None: pass def write_joint_velocity_to_sim_index( self, + *, velocity: torch.Tensor | wp.array, - joint_ids: Sequence[int] | slice | None = None, - env_ids: Sequence[int] | slice | None = None, + joint_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: pass def write_joint_velocity_to_sim_mask( self, + *, velocity: torch.Tensor | wp.array, - env_mask: wp.array | None = None, joint_mask: wp.array | None = None, + env_mask: wp.array | None = None, ) -> None: pass def write_joint_stiffness_to_sim_index( self, + *, stiffness: torch.Tensor | float | wp.array, - joint_ids: Sequence[int] | slice | None = None, + joint_ids: Sequence[int] | torch.Tensor | wp.array | None = None, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: pass def write_joint_stiffness_to_sim_mask( self, + *, stiffness: torch.Tensor | float | wp.array, - env_mask: wp.array | None = None, joint_mask: wp.array | None = None, + env_mask: wp.array | None = None, ) -> None: pass def write_joint_damping_to_sim_index( self, + *, damping: torch.Tensor | float | wp.array, - joint_ids: Sequence[int] | slice | None = None, + joint_ids: Sequence[int] | torch.Tensor | wp.array | None = None, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: pass def write_joint_damping_to_sim_mask( self, + *, damping: torch.Tensor | float | wp.array, - env_mask: wp.array | None = None, joint_mask: wp.array | None = None, + env_mask: wp.array | None = None, ) -> None: pass def write_joint_position_limit_to_sim_index( self, + *, limits: torch.Tensor | float | wp.array, - joint_ids: Sequence[int] | slice | None = None, + joint_ids: Sequence[int] | torch.Tensor | wp.array | None = None, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, warn_limit_violation: bool = True, ) -> None: @@ -1725,74 +1746,83 @@ def write_joint_position_limit_to_sim_index( def write_joint_position_limit_to_sim_mask( self, + *, limits: torch.Tensor | float | wp.array, warn_limit_violation: bool = True, - env_mask: wp.array | None = None, joint_mask: wp.array | None = None, + env_mask: wp.array | None = None, ) -> None: pass def write_joint_velocity_limit_to_sim_index( self, + *, limits: torch.Tensor | float | wp.array, - joint_ids: Sequence[int] | slice | None = None, + joint_ids: Sequence[int] | torch.Tensor | wp.array | None = None, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: pass def write_joint_velocity_limit_to_sim_mask( self, + *, limits: torch.Tensor | float | wp.array, - env_mask: wp.array | None = None, joint_mask: wp.array | None = None, + env_mask: wp.array | None = None, ) -> None: pass def write_joint_effort_limit_to_sim_index( self, + *, limits: torch.Tensor | float | wp.array, - joint_ids: Sequence[int] | slice | None = None, + joint_ids: Sequence[int] | torch.Tensor | wp.array | None = None, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: pass def write_joint_effort_limit_to_sim_mask( self, + *, limits: torch.Tensor | float | wp.array, - env_mask: wp.array | None = None, joint_mask: wp.array | None = None, + env_mask: wp.array | None = None, ) -> None: pass def write_joint_armature_to_sim_index( self, + *, armature: torch.Tensor | float | wp.array, - joint_ids: Sequence[int] | slice | None = None, + joint_ids: Sequence[int] | torch.Tensor | wp.array | None = None, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: pass def write_joint_armature_to_sim_mask( self, + *, armature: torch.Tensor | float | wp.array, - env_mask: wp.array | None = None, joint_mask: wp.array | None = None, + env_mask: wp.array | None = None, ) -> None: pass def write_joint_friction_coefficient_to_sim_index( self, + *, joint_friction_coeff: torch.Tensor | float | wp.array, - joint_ids: Sequence[int] | slice | None = None, + joint_ids: Sequence[int] | torch.Tensor | wp.array | None = None, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: pass def write_joint_friction_coefficient_to_sim_mask( self, + *, joint_friction_coeff: torch.Tensor | float | wp.array, - env_mask: wp.array | None = None, joint_mask: wp.array | None = None, + env_mask: wp.array | None = None, ) -> None: pass @@ -1800,49 +1830,55 @@ def write_joint_friction_coefficient_to_sim_mask( def set_masses_index( self, + *, masses: torch.Tensor | wp.array, - body_ids: Sequence[int] | slice | None = None, + body_ids: Sequence[int] | torch.Tensor | wp.array | None = None, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: pass def set_masses_mask( self, + *, masses: torch.Tensor | wp.array, - env_mask: wp.array | None = None, body_mask: wp.array | None = None, + env_mask: wp.array | None = None, ) -> None: pass def set_coms_index( self, + *, coms: torch.Tensor | wp.array, - body_ids: Sequence[int] | None = None, + body_ids: Sequence[int] | torch.Tensor | wp.array | None = None, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: pass def set_coms_mask( self, + *, coms: torch.Tensor | wp.array, - env_mask: wp.array | None = None, body_mask: wp.array | None = None, + env_mask: wp.array | None = None, ) -> None: pass def set_inertias_index( self, + *, inertias: torch.Tensor | wp.array, - body_ids: Sequence[int] | None = None, + body_ids: Sequence[int] | torch.Tensor | wp.array | None = None, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: pass def set_inertias_mask( self, + *, inertias: torch.Tensor | wp.array, - env_mask: wp.array | None = None, body_mask: wp.array | None = None, + env_mask: wp.array | None = None, ) -> None: pass @@ -1850,49 +1886,55 @@ def set_inertias_mask( def set_joint_position_target_index( self, + *, target: torch.Tensor | wp.array, - joint_ids: Sequence[int] | slice | None = None, + joint_ids: Sequence[int] | torch.Tensor | wp.array | None = None, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: pass def set_joint_position_target_mask( self, + *, target: torch.Tensor | wp.array, - env_mask: wp.array | None = None, joint_mask: wp.array | None = None, + env_mask: wp.array | None = None, ) -> None: pass def set_joint_velocity_target_index( self, + *, target: torch.Tensor | wp.array, - joint_ids: Sequence[int] | slice | None = None, + joint_ids: Sequence[int] | torch.Tensor | wp.array | None = None, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: pass def set_joint_velocity_target_mask( self, + *, target: torch.Tensor | wp.array, - env_mask: wp.array | None = None, joint_mask: wp.array | None = None, + env_mask: wp.array | None = None, ) -> None: pass def set_joint_effort_target_index( self, + *, target: torch.Tensor | wp.array, - joint_ids: Sequence[int] | slice | None = None, + joint_ids: Sequence[int] | torch.Tensor | wp.array | None = None, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: pass def set_joint_effort_target_mask( self, + *, target: torch.Tensor | wp.array, - env_mask: wp.array | None = None, joint_mask: wp.array | None = None, + env_mask: wp.array | None = None, ) -> None: pass @@ -1900,111 +1942,125 @@ def set_joint_effort_target_mask( def set_fixed_tendon_stiffness_index( self, - stiffness: torch.Tensor | wp.array, - fixed_tendon_ids: Sequence[int] | slice | None = None, + *, + stiffness: float | torch.Tensor | wp.array, + fixed_tendon_ids: Sequence[int] | torch.Tensor | wp.array | None = None, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: pass def set_fixed_tendon_stiffness_mask( self, - stiffness: torch.Tensor | wp.array, - env_mask: wp.array | None = None, + *, + stiffness: float | torch.Tensor | wp.array, fixed_tendon_mask: wp.array | None = None, + env_mask: wp.array | None = None, ) -> None: pass def set_fixed_tendon_damping_index( self, - damping: torch.Tensor | wp.array, - fixed_tendon_ids: Sequence[int] | slice | None = None, + *, + damping: float | torch.Tensor | wp.array, + fixed_tendon_ids: Sequence[int] | torch.Tensor | wp.array | None = None, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: pass def set_fixed_tendon_damping_mask( self, - damping: torch.Tensor | wp.array, - env_mask: wp.array | None = None, + *, + damping: float | torch.Tensor | wp.array, fixed_tendon_mask: wp.array | None = None, + env_mask: wp.array | None = None, ) -> None: pass def set_fixed_tendon_limit_stiffness_index( self, - limit_stiffness: torch.Tensor | wp.array, - fixed_tendon_ids: Sequence[int] | slice | None = None, + *, + limit_stiffness: float | torch.Tensor | wp.array, + fixed_tendon_ids: Sequence[int] | torch.Tensor | wp.array | None = None, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: pass def set_fixed_tendon_limit_stiffness_mask( self, - limit_stiffness: torch.Tensor | wp.array, - env_mask: wp.array | None = None, + *, + limit_stiffness: float | torch.Tensor | wp.array, fixed_tendon_mask: wp.array | None = None, + env_mask: wp.array | None = None, ) -> None: pass def set_fixed_tendon_position_limit_index( self, - limit: torch.Tensor | wp.array, - fixed_tendon_ids: Sequence[int] | slice | None = None, + *, + limit: float | torch.Tensor | wp.array, + fixed_tendon_ids: Sequence[int] | torch.Tensor | wp.array | None = None, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: pass def set_fixed_tendon_position_limit_mask( self, - limit: torch.Tensor | wp.array, - env_mask: wp.array | None = None, + *, + limit: float | torch.Tensor | wp.array, fixed_tendon_mask: wp.array | None = None, + env_mask: wp.array | None = None, ) -> None: pass def set_fixed_tendon_rest_length_index( self, - rest_length: torch.Tensor | wp.array, - fixed_tendon_ids: Sequence[int] | slice | None = None, + *, + rest_length: float | torch.Tensor | wp.array, + fixed_tendon_ids: Sequence[int] | torch.Tensor | wp.array | None = None, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: pass def set_fixed_tendon_rest_length_mask( self, - rest_length: torch.Tensor | wp.array, - env_mask: wp.array | None = None, + *, + rest_length: float | torch.Tensor | wp.array, fixed_tendon_mask: wp.array | None = None, + env_mask: wp.array | None = None, ) -> None: pass def set_fixed_tendon_offset_index( self, - offset: torch.Tensor | wp.array, - fixed_tendon_ids: Sequence[int] | slice | None = None, + *, + offset: float | torch.Tensor | wp.array, + fixed_tendon_ids: Sequence[int] | torch.Tensor | wp.array | None = None, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: pass def set_fixed_tendon_offset_mask( self, - offset: torch.Tensor | wp.array, - env_mask: wp.array | None = None, + *, + offset: float | torch.Tensor | wp.array, fixed_tendon_mask: wp.array | None = None, + env_mask: wp.array | None = None, ) -> None: pass def write_fixed_tendon_properties_to_sim_index( self, - fixed_tendon_ids: Sequence[int] | slice | None = None, + *, + fixed_tendon_ids: Sequence[int] | torch.Tensor | wp.array | None = None, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: pass def write_fixed_tendon_properties_to_sim_mask( self, - env_mask: wp.array | None = None, + *, fixed_tendon_mask: wp.array | None = None, + env_mask: wp.array | None = None, ) -> None: pass @@ -2012,78 +2068,88 @@ def write_fixed_tendon_properties_to_sim_mask( def set_spatial_tendon_stiffness_index( self, - stiffness: torch.Tensor | wp.array, - spatial_tendon_ids: Sequence[int] | slice | None = None, + *, + stiffness: float | torch.Tensor | wp.array, + spatial_tendon_ids: Sequence[int] | torch.Tensor | wp.array | None = None, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: pass def set_spatial_tendon_stiffness_mask( self, - stiffness: torch.Tensor | wp.array, - env_mask: wp.array | None = None, + *, + stiffness: float | torch.Tensor | wp.array, spatial_tendon_mask: wp.array | None = None, + env_mask: wp.array | None = None, ) -> None: pass def set_spatial_tendon_damping_index( self, - damping: torch.Tensor | wp.array, - spatial_tendon_ids: Sequence[int] | slice | None = None, + *, + damping: float | torch.Tensor | wp.array, + spatial_tendon_ids: Sequence[int] | torch.Tensor | wp.array | None = None, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: pass def set_spatial_tendon_damping_mask( self, - damping: torch.Tensor | wp.array, - env_mask: wp.array | None = None, + *, + damping: float | torch.Tensor | wp.array, spatial_tendon_mask: wp.array | None = None, + env_mask: wp.array | None = None, ) -> None: pass def set_spatial_tendon_limit_stiffness_index( self, - limit_stiffness: torch.Tensor | wp.array, - spatial_tendon_ids: Sequence[int] | slice | None = None, + *, + limit_stiffness: float | torch.Tensor | wp.array, + spatial_tendon_ids: Sequence[int] | torch.Tensor | wp.array | None = None, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: pass def set_spatial_tendon_limit_stiffness_mask( self, - limit_stiffness: torch.Tensor | wp.array, - env_mask: wp.array | None = None, + *, + limit_stiffness: float | torch.Tensor | wp.array, spatial_tendon_mask: wp.array | None = None, + env_mask: wp.array | None = None, ) -> None: pass def set_spatial_tendon_offset_index( self, - offset: torch.Tensor, - spatial_tendon_ids: Sequence[int] | slice | None = None, + *, + offset: float | torch.Tensor | wp.array, + spatial_tendon_ids: Sequence[int] | torch.Tensor | wp.array | None = None, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: pass def set_spatial_tendon_offset_mask( self, - offset: torch.Tensor, - env_mask: wp.array | None = None, + *, + offset: float | torch.Tensor | wp.array, spatial_tendon_mask: wp.array | None = None, + env_mask: wp.array | None = None, ) -> None: pass def write_spatial_tendon_properties_to_sim_index( self, - spatial_tendon_ids: Sequence[int] | slice | None = None, + *, + spatial_tendon_ids: Sequence[int] | torch.Tensor | wp.array | None = None, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: pass def write_spatial_tendon_properties_to_sim_mask( self, - env_mask: wp.array | None = None, + *, spatial_tendon_mask: wp.array | None = None, + env_mask: wp.array | None = None, ) -> None: pass diff --git a/source/isaaclab/isaaclab/test/mock_interfaces/assets/mock_rigid_object.py b/source/isaaclab/isaaclab/test/mock_interfaces/assets/mock_rigid_object.py index 7ae1b403055..a230ff37ecf 100644 --- a/source/isaaclab/isaaclab/test/mock_interfaces/assets/mock_rigid_object.py +++ b/source/isaaclab/isaaclab/test/mock_interfaces/assets/mock_rigid_object.py @@ -367,9 +367,9 @@ def body_com_quat_b(self) -> wp.array: @property def body_mass(self) -> wp.array: - """Body mass. Shape: (N, 1, 1).""" + """Body mass. Shape: (N, 1).""" if self._body_mass is None: - return wp.ones((self._num_instances, 1, 1), dtype=wp.float32, device=self.device) + return wp.ones((self._num_instances, 1), dtype=wp.float32, device=self.device) return self._body_mass @property @@ -419,15 +419,8 @@ def root_com_ang_vel_b(self) -> wp.array: # -- Shorthand properties (root_pose_w, body_pos_w, com_pos_b, etc.) -- - @property - def com_pos_b(self) -> wp.array: - """Convenience alias for root_com_pos_w. Shape: (N, 3).""" - return wp.clone(self.root_com_pos_w, self.device) - - @property - def com_quat_b(self) -> wp.array: - """Convenience alias for root_com_quat_w. Shape: (N, 4).""" - return wp.clone(self.root_com_quat_w, self.device) + # com_pos_b and com_quat_b are inherited from BaseRigidObjectData + # (they delegate to body_com_pos_b and body_com_quat_b respectively) # Inherited from BaseRigidObjectData @@ -569,7 +562,11 @@ def permanent_wrench_composer(self) -> None: # -- Core methods -- - def reset(self, env_ids: Sequence[int] | None = None) -> None: + def reset( + self, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + env_mask: wp.array | None = None, + ) -> None: """Reset rigid object state for specified environments.""" pass @@ -613,72 +610,72 @@ def find_bodies(self, name_keys: str | Sequence[str], preserve_order: bool = Fal def write_root_state_to_sim( self, - root_state: torch.Tensor, - env_ids: Sequence[int] | None = None, + root_state: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: """Write root state to simulation.""" pass def write_root_com_state_to_sim( self, - root_state: torch.Tensor, - env_ids: Sequence[int] | None = None, + root_state: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: """Write root CoM state to simulation.""" pass def write_root_link_state_to_sim( self, - root_state: torch.Tensor, - env_ids: Sequence[int] | None = None, + root_state: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: """Write root link state to simulation.""" pass def write_root_pose_to_sim( self, - root_pose: torch.Tensor, - env_ids: Sequence[int] | None = None, + root_pose: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: """Write root pose to simulation.""" pass def write_root_link_pose_to_sim( self, - root_pose: torch.Tensor, - env_ids: Sequence[int] | None = None, + root_pose: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: """Write root link pose to simulation.""" pass def write_root_com_pose_to_sim( self, - root_pose: torch.Tensor, - env_ids: Sequence[int] | None = None, + root_pose: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: """Write root CoM pose to simulation.""" pass def write_root_velocity_to_sim( self, - root_velocity: torch.Tensor, - env_ids: Sequence[int] | None = None, + root_velocity: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: """Write root velocity to simulation.""" pass def write_root_com_velocity_to_sim( self, - root_velocity: torch.Tensor, - env_ids: Sequence[int] | None = None, + root_velocity: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: """Write root CoM velocity to simulation.""" pass def write_root_link_velocity_to_sim( self, - root_velocity: torch.Tensor, - env_ids: Sequence[int] | None = None, + root_velocity: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: """Write root link velocity to simulation.""" pass @@ -687,39 +684,39 @@ def write_root_link_velocity_to_sim( def set_masses( self, - masses: torch.Tensor, + masses: torch.Tensor | wp.array, body_ids: Sequence[int] | slice | None = None, - env_ids: Sequence[int] | None = None, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: """Set body masses.""" pass def set_coms( self, - coms: torch.Tensor, - body_ids: Sequence[int] | slice | None = None, - env_ids: Sequence[int] | None = None, + coms: torch.Tensor | wp.array, + body_ids: Sequence[int] | None = None, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: """Set body centers of mass.""" pass def set_inertias( self, - inertias: torch.Tensor, - body_ids: Sequence[int] | slice | None = None, - env_ids: Sequence[int] | None = None, + inertias: torch.Tensor | wp.array, + body_ids: Sequence[int] | None = None, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: """Set body inertias.""" pass def set_external_force_and_torque( self, - forces: torch.Tensor, - torques: torch.Tensor, - positions: torch.Tensor | None = None, + forces: torch.Tensor | wp.array, + torques: torch.Tensor | wp.array, + positions: torch.Tensor | wp.array | None = None, body_ids: Sequence[int] | slice | None = None, - env_ids: Sequence[int] | None = None, - is_global: bool = True, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + is_global: bool = False, ) -> None: """Set external forces and torques.""" pass @@ -728,6 +725,7 @@ def set_external_force_and_torque( def write_root_pose_to_sim_index( self, + *, root_pose: torch.Tensor | wp.array, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: @@ -735,6 +733,7 @@ def write_root_pose_to_sim_index( def write_root_pose_to_sim_mask( self, + *, root_pose: torch.Tensor | wp.array, env_mask: wp.array | None = None, ) -> None: @@ -742,6 +741,7 @@ def write_root_pose_to_sim_mask( def write_root_link_pose_to_sim_index( self, + *, root_pose: torch.Tensor | wp.array, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: @@ -749,6 +749,7 @@ def write_root_link_pose_to_sim_index( def write_root_link_pose_to_sim_mask( self, + *, root_pose: torch.Tensor | wp.array, env_mask: wp.array | None = None, ) -> None: @@ -756,6 +757,7 @@ def write_root_link_pose_to_sim_mask( def write_root_com_pose_to_sim_index( self, + *, root_pose: torch.Tensor | wp.array, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: @@ -763,6 +765,7 @@ def write_root_com_pose_to_sim_index( def write_root_com_pose_to_sim_mask( self, + *, root_pose: torch.Tensor | wp.array, env_mask: wp.array | None = None, ) -> None: @@ -770,6 +773,7 @@ def write_root_com_pose_to_sim_mask( def write_root_velocity_to_sim_index( self, + *, root_velocity: torch.Tensor | wp.array, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: @@ -777,6 +781,7 @@ def write_root_velocity_to_sim_index( def write_root_velocity_to_sim_mask( self, + *, root_velocity: torch.Tensor | wp.array, env_mask: wp.array | None = None, ) -> None: @@ -784,6 +789,7 @@ def write_root_velocity_to_sim_mask( def write_root_com_velocity_to_sim_index( self, + *, root_velocity: torch.Tensor | wp.array, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: @@ -791,6 +797,7 @@ def write_root_com_velocity_to_sim_index( def write_root_com_velocity_to_sim_mask( self, + *, root_velocity: torch.Tensor | wp.array, env_mask: wp.array | None = None, ) -> None: @@ -798,6 +805,7 @@ def write_root_com_velocity_to_sim_mask( def write_root_link_velocity_to_sim_index( self, + *, root_velocity: torch.Tensor | wp.array, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: @@ -805,6 +813,7 @@ def write_root_link_velocity_to_sim_index( def write_root_link_velocity_to_sim_mask( self, + *, root_velocity: torch.Tensor | wp.array, env_mask: wp.array | None = None, ) -> None: @@ -812,48 +821,54 @@ def write_root_link_velocity_to_sim_mask( def set_masses_index( self, + *, masses: torch.Tensor | wp.array, - body_ids: Sequence[int] | slice | None = None, + body_ids: Sequence[int] | torch.Tensor | wp.array | None = None, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: pass def set_masses_mask( self, + *, masses: torch.Tensor | wp.array, - env_mask: wp.array | None = None, body_mask: wp.array | None = None, + env_mask: wp.array | None = None, ) -> None: pass def set_coms_index( self, + *, coms: torch.Tensor | wp.array, - body_ids: Sequence[int] | None = None, + body_ids: Sequence[int] | torch.Tensor | wp.array | None = None, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: pass def set_coms_mask( self, + *, coms: torch.Tensor | wp.array, - env_mask: wp.array | None = None, body_mask: wp.array | None = None, + env_mask: wp.array | None = None, ) -> None: pass def set_inertias_index( self, + *, inertias: torch.Tensor | wp.array, - body_ids: Sequence[int] | None = None, + body_ids: Sequence[int] | torch.Tensor | wp.array | None = None, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: pass def set_inertias_mask( self, + *, inertias: torch.Tensor | wp.array, - env_mask: wp.array | None = None, body_mask: wp.array | None = None, + env_mask: wp.array | None = None, ) -> None: pass diff --git a/source/isaaclab/isaaclab/test/mock_interfaces/assets/mock_rigid_object_collection.py b/source/isaaclab/isaaclab/test/mock_interfaces/assets/mock_rigid_object_collection.py index 7142228d40d..82ec8b10e11 100644 --- a/source/isaaclab/isaaclab/test/mock_interfaces/assets/mock_rigid_object_collection.py +++ b/source/isaaclab/isaaclab/test/mock_interfaces/assets/mock_rigid_object_collection.py @@ -457,8 +457,9 @@ def object_names(self) -> list[str]: def reset( self, - env_ids: Sequence[int] | None = None, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, object_ids: slice | torch.Tensor | None = None, + env_mask: wp.array | None = None, ) -> None: """Reset rigid object collection state for specified environments.""" pass @@ -475,11 +476,11 @@ def update(self, dt: float) -> None: def find_bodies( self, name_keys: str | Sequence[str], preserve_order: bool = False - ) -> tuple[torch.Tensor, list[str], list[int]]: + ) -> tuple[torch.Tensor, list[str]]: """Find bodies by name regex patterns. Returns: - Tuple of (body_mask, body_names, body_indices). + Tuple of (body_mask, body_names). """ if isinstance(name_keys, str): name_keys = [name_keys] @@ -507,7 +508,7 @@ def find_bodies( body_mask = torch.zeros(self._num_bodies, dtype=torch.bool, device=self._device) body_mask[matched_indices] = True - return body_mask, matched_names, matched_indices + return body_mask, matched_names def find_objects(self, name_keys: str | Sequence[str], preserve_order: bool = False) -> tuple[list[int], list[str]]: return self.find_bodies(name_keys, preserve_order) @@ -516,8 +517,8 @@ def find_objects(self, name_keys: str | Sequence[str], preserve_order: bool = Fa def write_body_state_to_sim( self, - body_states: torch.Tensor, - env_ids: Sequence[int] | None = None, + body_states: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, body_ids: Sequence[int] | slice | None = None, ) -> None: """Write body states to simulation.""" @@ -525,8 +526,8 @@ def write_body_state_to_sim( def write_body_com_state_to_sim( self, - body_states: torch.Tensor, - env_ids: Sequence[int] | None = None, + body_states: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, body_ids: Sequence[int] | slice | None = None, ) -> None: """Write body CoM states to simulation.""" @@ -534,8 +535,8 @@ def write_body_com_state_to_sim( def write_body_link_state_to_sim( self, - body_states: torch.Tensor, - env_ids: Sequence[int] | None = None, + body_states: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, body_ids: Sequence[int] | slice | None = None, ) -> None: """Write body link states to simulation.""" @@ -543,8 +544,8 @@ def write_body_link_state_to_sim( def write_body_pose_to_sim( self, - body_poses: torch.Tensor, - env_ids: Sequence[int] | None = None, + body_poses: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, body_ids: Sequence[int] | slice | None = None, ) -> None: """Write body poses to simulation.""" @@ -552,8 +553,8 @@ def write_body_pose_to_sim( def write_body_link_pose_to_sim( self, - body_poses: torch.Tensor, - env_ids: Sequence[int] | None = None, + body_poses: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, body_ids: Sequence[int] | slice | None = None, ) -> None: """Write body link poses to simulation.""" @@ -561,8 +562,8 @@ def write_body_link_pose_to_sim( def write_body_com_pose_to_sim( self, - body_poses: torch.Tensor, - env_ids: Sequence[int] | None = None, + body_poses: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, body_ids: Sequence[int] | slice | None = None, ) -> None: """Write body CoM poses to simulation.""" @@ -570,8 +571,8 @@ def write_body_com_pose_to_sim( def write_body_velocity_to_sim( self, - body_velocities: torch.Tensor, - env_ids: Sequence[int] | None = None, + body_velocities: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, body_ids: Sequence[int] | slice | None = None, ) -> None: """Write body velocities to simulation.""" @@ -579,8 +580,8 @@ def write_body_velocity_to_sim( def write_body_com_velocity_to_sim( self, - body_velocities: torch.Tensor, - env_ids: Sequence[int] | None = None, + body_velocities: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, body_ids: Sequence[int] | slice | None = None, ) -> None: """Write body CoM velocities to simulation.""" @@ -588,8 +589,8 @@ def write_body_com_velocity_to_sim( def write_body_link_velocity_to_sim( self, - body_velocities: torch.Tensor, - env_ids: Sequence[int] | None = None, + body_velocities: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, body_ids: Sequence[int] | slice | None = None, ) -> None: """Write body link velocities to simulation.""" @@ -599,39 +600,39 @@ def write_body_link_velocity_to_sim( def set_masses( self, - masses: torch.Tensor, + masses: torch.Tensor | wp.array, body_ids: Sequence[int] | slice | None = None, - env_ids: Sequence[int] | None = None, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: """Set body masses.""" pass def set_coms( self, - coms: torch.Tensor, - body_ids: Sequence[int] | slice | None = None, - env_ids: Sequence[int] | None = None, + coms: torch.Tensor | wp.array, + body_ids: Sequence[int] | None = None, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: """Set body centers of mass.""" pass def set_inertias( self, - inertias: torch.Tensor, - body_ids: Sequence[int] | slice | None = None, - env_ids: Sequence[int] | None = None, + inertias: torch.Tensor | wp.array, + body_ids: Sequence[int] | None = None, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: """Set body inertias.""" pass def set_external_force_and_torque( self, - forces: torch.Tensor, - torques: torch.Tensor, - positions: torch.Tensor | None = None, + forces: torch.Tensor | wp.array, + torques: torch.Tensor | wp.array, + positions: torch.Tensor | wp.array | None = None, body_ids: Sequence[int] | slice | None = None, - env_ids: Sequence[int] | None = None, - is_global: bool = True, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + is_global: bool = False, ) -> None: """Set external forces and torques.""" pass @@ -640,6 +641,7 @@ def set_external_force_and_torque( def write_body_pose_to_sim_index( self, + *, body_poses: torch.Tensor | wp.array, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, body_ids: slice | torch.Tensor | None = None, @@ -648,6 +650,7 @@ def write_body_pose_to_sim_index( def write_body_pose_to_sim_mask( self, + *, body_poses: torch.Tensor | wp.array, env_mask: wp.array | None = None, body_mask: wp.array | None = None, @@ -656,6 +659,7 @@ def write_body_pose_to_sim_mask( def write_body_link_pose_to_sim_index( self, + *, body_poses: torch.Tensor | wp.array, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, body_ids: slice | torch.Tensor | None = None, @@ -664,6 +668,7 @@ def write_body_link_pose_to_sim_index( def write_body_link_pose_to_sim_mask( self, + *, body_poses: torch.Tensor | wp.array, env_mask: wp.array | None = None, body_mask: wp.array | None = None, @@ -672,6 +677,7 @@ def write_body_link_pose_to_sim_mask( def write_body_com_pose_to_sim_index( self, + *, body_poses: torch.Tensor | wp.array, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, body_ids: slice | torch.Tensor | None = None, @@ -680,6 +686,7 @@ def write_body_com_pose_to_sim_index( def write_body_com_pose_to_sim_mask( self, + *, body_poses: torch.Tensor | wp.array, env_mask: wp.array | None = None, body_mask: wp.array | None = None, @@ -688,6 +695,7 @@ def write_body_com_pose_to_sim_mask( def write_body_velocity_to_sim_index( self, + *, body_velocities: torch.Tensor | wp.array, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, body_ids: slice | torch.Tensor | None = None, @@ -696,6 +704,7 @@ def write_body_velocity_to_sim_index( def write_body_velocity_to_sim_mask( self, + *, body_velocities: torch.Tensor | wp.array, env_mask: wp.array | None = None, body_mask: wp.array | None = None, @@ -704,6 +713,7 @@ def write_body_velocity_to_sim_mask( def write_body_com_velocity_to_sim_index( self, + *, body_velocities: torch.Tensor | wp.array, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, body_ids: slice | torch.Tensor | None = None, @@ -712,6 +722,7 @@ def write_body_com_velocity_to_sim_index( def write_body_com_velocity_to_sim_mask( self, + *, body_velocities: torch.Tensor | wp.array, env_mask: wp.array | None = None, body_mask: wp.array | None = None, @@ -720,6 +731,7 @@ def write_body_com_velocity_to_sim_mask( def write_body_link_velocity_to_sim_index( self, + *, body_velocities: torch.Tensor | wp.array, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, body_ids: slice | torch.Tensor | None = None, @@ -728,6 +740,7 @@ def write_body_link_velocity_to_sim_index( def write_body_link_velocity_to_sim_mask( self, + *, body_velocities: torch.Tensor | wp.array, env_mask: wp.array | None = None, body_mask: wp.array | None = None, @@ -736,49 +749,55 @@ def write_body_link_velocity_to_sim_mask( def set_masses_index( self, + *, masses: torch.Tensor | wp.array, - body_ids: Sequence[int] | None = None, + body_ids: Sequence[int] | torch.Tensor | wp.array | None = None, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: pass def set_masses_mask( self, + *, masses: torch.Tensor | wp.array, - env_mask: wp.array | None = None, body_mask: wp.array | None = None, + env_mask: wp.array | None = None, ) -> None: pass def set_coms_index( self, + *, coms: torch.Tensor | wp.array, - body_ids: Sequence[int] | None = None, + body_ids: Sequence[int] | torch.Tensor | wp.array | None = None, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: pass def set_coms_mask( self, + *, coms: torch.Tensor | wp.array, - env_mask: wp.array | None = None, body_mask: wp.array | None = None, + env_mask: wp.array | None = None, ) -> None: pass def set_inertias_index( self, + *, inertias: torch.Tensor | wp.array, - body_ids: Sequence[int] | None = None, + body_ids: Sequence[int] | torch.Tensor | wp.array | None = None, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ) -> None: pass def set_inertias_mask( self, + *, inertias: torch.Tensor | wp.array, - env_mask: wp.array | None = None, body_mask: wp.array | None = None, + env_mask: wp.array | None = None, ) -> None: pass diff --git a/source/isaaclab/isaaclab/test/mock_interfaces/sensors/mock_contact_sensor.py b/source/isaaclab/isaaclab/test/mock_interfaces/sensors/mock_contact_sensor.py index caf67a7f9f4..82e134ccd6e 100644 --- a/source/isaaclab/isaaclab/test/mock_interfaces/sensors/mock_contact_sensor.py +++ b/source/isaaclab/isaaclab/test/mock_interfaces/sensors/mock_contact_sensor.py @@ -356,9 +356,14 @@ def num_instances(self) -> int: """Number of sensor instances.""" return self._num_instances + @property + def num_sensors(self) -> int: + """Number of sensors (primary property).""" + return self._num_bodies + @property def num_bodies(self) -> int: - """Number of bodies with contact sensors.""" + """Number of bodies with contact sensors (deprecated, use num_sensors).""" return self._num_bodies @property @@ -378,8 +383,20 @@ def device(self) -> str: # -- Methods -- + def find_sensors(self, name_keys: str | Sequence[str], preserve_order: bool = False) -> tuple[list[int], list[str]]: + """Find sensors by name regex patterns (primary method). + + Args: + name_keys: Regex pattern(s) to match sensor/body names. + preserve_order: If True, preserve order of name_keys in output. + + Returns: + Tuple of (sensor_indices, sensor_names) matching the patterns. + """ + return self.find_bodies(name_keys, preserve_order) + def find_bodies(self, name_keys: str | Sequence[str], preserve_order: bool = False) -> tuple[list[int], list[str]]: - """Find bodies by name regex patterns. + """Find bodies by name regex patterns (deprecated, use find_sensors). Args: name_keys: Regex pattern(s) to match body names. @@ -412,7 +429,7 @@ def find_bodies(self, name_keys: str | Sequence[str], preserve_order: bool = Fal return matched_indices, matched_names - def compute_first_contact(self, dt: float, abs_tol: float = 1.0e-8) -> torch.Tensor: + def compute_first_contact(self, dt: float, abs_tol: float = 1.0e-8) -> wp.array: """Check which bodies established contact within dt seconds. Args: @@ -420,11 +437,12 @@ def compute_first_contact(self, dt: float, abs_tol: float = 1.0e-8) -> torch.Ten abs_tol: Absolute tolerance for contact time comparison. Returns: - Boolean tensor of shape (N, B) indicating first contact. + Boolean warp array of shape (N, B) indicating first contact. """ - return wp.to_torch(self._data.current_contact_time) < (dt + abs_tol) + result = wp.to_torch(self._data.current_contact_time) < (dt + abs_tol) + return wp.from_torch(result) - def compute_first_air(self, dt: float, abs_tol: float = 1.0e-8) -> torch.Tensor: + def compute_first_air(self, dt: float, abs_tol: float = 1.0e-8) -> wp.array: """Check which bodies broke contact within dt seconds. Args: @@ -432,9 +450,10 @@ def compute_first_air(self, dt: float, abs_tol: float = 1.0e-8) -> torch.Tensor: abs_tol: Absolute tolerance for air time comparison. Returns: - Boolean tensor of shape (N, B) indicating first air. + Boolean warp array of shape (N, B) indicating first air. """ - return wp.to_torch(self._data.current_air_time) < (dt + abs_tol) + result = wp.to_torch(self._data.current_air_time) < (dt + abs_tol) + return wp.from_torch(result) def reset(self, env_ids: Sequence[int] | None = None) -> None: """Reset sensor state for specified environments. diff --git a/source/isaaclab/isaaclab/test/mock_interfaces/utils/mock_wrench_composer.py b/source/isaaclab/isaaclab/test/mock_interfaces/utils/mock_wrench_composer.py index 4532f694420..f35228ea6dc 100644 --- a/source/isaaclab/isaaclab/test/mock_interfaces/utils/mock_wrench_composer.py +++ b/source/isaaclab/isaaclab/test/mock_interfaces/utils/mock_wrench_composer.py @@ -114,7 +114,7 @@ def add_forces_and_torques( env_ids: wp.array | torch.Tensor | None = None, is_global: bool = False, ) -> None: - """Add forces and torques (mock - just sets active flag). + """Add forces and torques (deprecated, use add_forces_and_torques_index). Args: forces: Forces. (num_envs, num_bodies, 3). Defaults to None. @@ -124,8 +124,14 @@ def add_forces_and_torques( env_ids: Environment ids. Defaults to None (all environments). is_global: Whether the forces and torques are applied in the global frame. Defaults to False. """ - if forces is not None or torques is not None: - self._active = True + self.add_forces_and_torques_index( + forces=forces, + torques=torques, + positions=positions, + body_ids=body_ids, + env_ids=env_ids, + is_global=is_global, + ) def set_forces_and_torques( self, @@ -136,7 +142,7 @@ def set_forces_and_torques( env_ids: wp.array | torch.Tensor | None = None, is_global: bool = False, ) -> None: - """Set forces and torques (mock - just sets active flag). + """Set forces and torques (deprecated, use set_forces_and_torques_index). Args: forces: Forces. (num_envs, num_bodies, 3). Defaults to None. @@ -146,14 +152,75 @@ def set_forces_and_torques( env_ids: Environment ids. Defaults to None (all environments). is_global: Whether the forces and torques are applied in the global frame. Defaults to False. """ + self.set_forces_and_torques_index( + forces=forces, + torques=torques, + positions=positions, + body_ids=body_ids, + env_ids=env_ids, + is_global=is_global, + ) + + # -- Index/Mask method variants -- + + def add_forces_and_torques_index( + self, + forces: wp.array | torch.Tensor | None = None, + torques: wp.array | torch.Tensor | None = None, + positions: wp.array | torch.Tensor | None = None, + body_ids: torch.Tensor | None = None, + env_ids: torch.Tensor | None = None, + is_global: bool = False, + ) -> None: + """Add forces and torques by index (mock - just sets active flag).""" + if forces is not None or torques is not None: + self._active = True + + def add_forces_and_torques_mask( + self, + forces: wp.array | torch.Tensor | None = None, + torques: wp.array | torch.Tensor | None = None, + positions: wp.array | torch.Tensor | None = None, + body_mask: wp.array | torch.Tensor | None = None, + env_mask: wp.array | torch.Tensor | None = None, + is_global: bool = False, + ) -> None: + """Add forces and torques by mask (mock - just sets active flag).""" + if forces is not None or torques is not None: + self._active = True + + def set_forces_and_torques_index( + self, + forces: wp.array | torch.Tensor | None = None, + torques: wp.array | torch.Tensor | None = None, + positions: wp.array | torch.Tensor | None = None, + body_ids: wp.array | torch.Tensor | None = None, + env_ids: wp.array | torch.Tensor | None = None, + is_global: bool = False, + ) -> None: + """Set forces and torques by index (mock - just sets active flag).""" + if forces is not None or torques is not None: + self._active = True + + def set_forces_and_torques_mask( + self, + forces: wp.array | torch.Tensor | None = None, + torques: wp.array | torch.Tensor | None = None, + positions: wp.array | torch.Tensor | None = None, + body_mask: wp.array | torch.Tensor | None = None, + env_mask: wp.array | torch.Tensor | None = None, + is_global: bool = False, + ) -> None: + """Set forces and torques by mask (mock - just sets active flag).""" if forces is not None or torques is not None: self._active = True - def reset(self, env_ids: wp.array | torch.Tensor | None = None) -> None: + def reset(self, env_ids: wp.array | torch.Tensor | None = None, env_mask: wp.array | None = None) -> None: """Reset the composed force and torque. Args: env_ids: Environment ids to reset. Defaults to None (all environments). + env_mask: Environment mask to reset. Defaults to None (all environments). """ if env_ids is None: self._composed_force_b.zero_() diff --git a/source/isaaclab/isaaclab/utils/configclass.py b/source/isaaclab/isaaclab/utils/configclass.py index fc119247ee9..588e8234b57 100644 --- a/source/isaaclab/isaaclab/utils/configclass.py +++ b/source/isaaclab/isaaclab/utils/configclass.py @@ -89,6 +89,11 @@ class EnvCfg: .. _dataclass: https://docs.python.org/3/library/dataclasses.html """ + # snapshot field names declared in *this* class body before configclass + # merges parent fields — used by _field_module_dir to resolve {DIR} correctly. + _own_ann = set(cls.__dict__.get("__annotations__", {}).keys()) + _own_body = {k for k in cls.__dict__ if not k.startswith("__")} + cls.__configclass_own_fields__ = frozenset(_own_ann | _own_body) # add type annotations _add_annotation_types(cls) # add field factory @@ -184,10 +189,19 @@ def _field_module_dir(obj: Any, key: str | None = None) -> str | None: cls = type(obj) if key is not None: # Use nearest declaration in MRO (subclass override wins). + # We prefer __configclass_own_fields__ (the snapshot taken before + # _process_mutable_types copies parent fields into every subclass's + # __dict__) so that {DIR} resolves relative to the class that + # *originally* declared the field, not the subclass that inherited it. for mro_cls in cls.__mro__: if mro_cls is object: continue - if key in mro_cls.__dict__: + own_fields = getattr(mro_cls, "__configclass_own_fields__", None) + if own_fields is not None: + if key in own_fields: + cls = mro_cls + break + elif key in mro_cls.__dict__: cls = mro_cls break module_name = getattr(cls, "__module__", "") diff --git a/source/isaaclab/test/test_mock_interfaces/test_mock_assets.py b/source/isaaclab/test/test_mock_interfaces/test_mock_assets.py index f283b93a301..acb5e8130f4 100644 --- a/source/isaaclab/test/test_mock_interfaces/test_mock_assets.py +++ b/source/isaaclab/test/test_mock_interfaces/test_mock_assets.py @@ -215,7 +215,7 @@ def test_body_state_shapes(self, obj): def test_body_properties(self, obj): """Test body property shapes.""" - assert obj.data.body_mass.shape == (4, 1, 1) + assert obj.data.body_mass.shape == (4, 1) assert obj.data.body_inertia.shape == (4, 1, 9) @@ -252,7 +252,7 @@ def test_body_state_shapes(self, collection): def test_find_bodies_returns_mask(self, collection): """Test that find_bodies returns a mask tensor.""" - body_mask, names, indices = collection.find_bodies("body_0") + body_mask, names = collection.find_bodies("body_0") assert isinstance(body_mask, torch.Tensor) assert body_mask.shape == (5,) assert body_mask[0] diff --git a/source/isaaclab/test/test_mock_interfaces/test_mock_data_properties.py b/source/isaaclab/test/test_mock_interfaces/test_mock_data_properties.py index 3faa6d6b813..4411328990a 100644 --- a/source/isaaclab/test/test_mock_interfaces/test_mock_data_properties.py +++ b/source/isaaclab/test/test_mock_interfaces/test_mock_data_properties.py @@ -387,7 +387,7 @@ def test_body_state_shapes(self, data, property_name, expected_shape): "property_name,expected_shape", [ ("body_mass", (4, 13)), - ("body_inertia", (4, 13, 3, 3)), + ("body_inertia", (4, 13, 9)), ("body_incoming_joint_wrench_b", (4, 13, 6)), ], ) @@ -530,7 +530,7 @@ def data(self): ("body_com_vel_w", (4, 1, 6)), ("body_com_state_w", (4, 1, 13)), ("body_com_acc_w", (4, 1, 6)), - ("body_mass", (4, 1, 1)), + ("body_mass", (4, 1)), ("body_inertia", (4, 1, 9)), ("projected_gravity_b", (4, 3)), ("heading_w", (4,)), @@ -753,8 +753,8 @@ def test_body_aliases(self, data, alias, source, expected_shape): @pytest.mark.parametrize( "alias,expected_shape", [ - ("com_pos_b", (4, 3)), - ("com_quat_b", (4, 4)), + ("com_pos_b", (4, 13, 3)), + ("com_quat_b", (4, 13, 4)), ], ) def test_com_body_frame_aliases(self, data, alias, expected_shape): @@ -826,8 +826,8 @@ def test_aliases(self, data, alias, source, expected_shape): @pytest.mark.parametrize( "alias,expected_shape", [ - ("com_pos_b", (4, 3)), - ("com_quat_b", (4, 4)), + ("com_pos_b", (4, 1, 3)), + ("com_quat_b", (4, 1, 4)), ], ) def test_com_body_frame_aliases(self, data, alias, expected_shape): diff --git a/source/isaaclab/test/test_mock_interfaces/test_mock_sensors.py b/source/isaaclab/test/test_mock_interfaces/test_mock_sensors.py index d44d0d256a3..04b33cc7f97 100644 --- a/source/isaaclab/test/test_mock_interfaces/test_mock_sensors.py +++ b/source/isaaclab/test/test_mock_interfaces/test_mock_sensors.py @@ -165,23 +165,27 @@ def test_find_bodies(self, sensor): def test_compute_first_contact(self, sensor): """Test first contact computation.""" + import warp as wp + # Set contact time to 0.5 for all bodies sensor.data.set_current_contact_time(torch.full((4, 4), 0.5)) # Check with dt=1.0 - should be True (0.5 < 1.0) first_contact = sensor.compute_first_contact(dt=1.0) - assert torch.all(first_contact) + assert torch.all(wp.to_torch(first_contact)) # Check with dt=0.1 - should be False (0.5 > 0.1) first_contact = sensor.compute_first_contact(dt=0.1) - assert torch.all(~first_contact) + assert torch.all(~wp.to_torch(first_contact)) def test_compute_first_air(self, sensor): """Test first air computation.""" + import warp as wp + sensor.data.set_current_air_time(torch.full((4, 4), 0.2)) first_air = sensor.compute_first_air(dt=0.5) - assert torch.all(first_air) + assert torch.all(wp.to_torch(first_air)) def test_history_buffer(self): """Test history buffer when enabled.""" diff --git a/source/isaaclab_newton/config/extension.toml b/source/isaaclab_newton/config/extension.toml index 7bca448a441..a3b133fa114 100644 --- a/source/isaaclab_newton/config/extension.toml +++ b/source/isaaclab_newton/config/extension.toml @@ -1,7 +1,7 @@ [package] # Note: Semantic Versioning is used: https://semver.org/ -version = "0.2.2" +version = "0.2.3" # Description title = "Newton simulation interfaces for IsaacLab core package" diff --git a/source/isaaclab_newton/docs/CHANGELOG.rst b/source/isaaclab_newton/docs/CHANGELOG.rst index 57e125c9478..c7367f9efad 100644 --- a/source/isaaclab_newton/docs/CHANGELOG.rst +++ b/source/isaaclab_newton/docs/CHANGELOG.rst @@ -1,6 +1,19 @@ Changelog --------- +0.2.3 (2026-02-27) +~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added runtime shape and dtype validation to all write methods in + :class:`~isaaclab_newton.assets.Articulation` and + :class:`~isaaclab_newton.assets.RigidObject` using + :meth:`~isaaclab.assets.AssetBase.assert_shape_and_dtype` and + :meth:`~isaaclab.assets.AssetBase.assert_shape_and_dtype_mask`. + + 0.2.2 (2026-02-26) ~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab_physx/config/extension.toml b/source/isaaclab_physx/config/extension.toml index 2eee37b55ab..3df0b84078f 100644 --- a/source/isaaclab_physx/config/extension.toml +++ b/source/isaaclab_physx/config/extension.toml @@ -1,7 +1,7 @@ [package] # Note: Semantic Versioning is used: https://semver.org/ -version = "0.5.2" +version = "0.5.3" # Description title = "PhysX simulation interfaces for IsaacLab core package" diff --git a/source/isaaclab_physx/docs/CHANGELOG.rst b/source/isaaclab_physx/docs/CHANGELOG.rst index acfc16f31c2..89d58637917 100644 --- a/source/isaaclab_physx/docs/CHANGELOG.rst +++ b/source/isaaclab_physx/docs/CHANGELOG.rst @@ -1,6 +1,22 @@ Changelog --------- +0.5.3 (2026-02-27) +~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added runtime shape and dtype validation to all write methods in + :class:`~isaaclab_physx.assets.Articulation`, + :class:`~isaaclab_physx.assets.RigidObject`, + :class:`~isaaclab_physx.assets.RigidObjectCollection`, + :class:`~isaaclab_physx.assets.DeformableObject`, and + :class:`~isaaclab_physx.assets.SurfaceGripper` using + :meth:`~isaaclab.assets.AssetBase.assert_shape_and_dtype`. Validates input dimensions + and types before kernel launch to catch mismatches early. + + 0.5.2 (2026-02-25) ~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab_physx/isaaclab_physx/test/mock_interfaces/views/mock_articulation_view.py b/source/isaaclab_physx/isaaclab_physx/test/mock_interfaces/views/mock_articulation_view.py index f9af2a0fff9..3c0fc04af41 100644 --- a/source/isaaclab_physx/isaaclab_physx/test/mock_interfaces/views/mock_articulation_view.py +++ b/source/isaaclab_physx/isaaclab_physx/test/mock_interfaces/views/mock_articulation_view.py @@ -142,6 +142,13 @@ def prim_paths(self) -> list[str]: """USD prim paths for each instance.""" return self._prim_paths + @property + def dof_paths(self) -> list[list[str]]: + """DOF paths for each instance.""" + dof_names = self._shared_metatype.dof_names or [f"joint_{i}" for i in range(self._num_dofs)] + single_instance_paths = [f"{self._prim_paths[0]}/{name}" for name in dof_names] + return [single_instance_paths] * self._count + # -- Root Getters -- def get_root_transforms(self) -> torch.Tensor: @@ -926,6 +933,95 @@ def set_mock_link_incoming_joint_force(self, forces: torch.Tensor) -> None: """ self._link_incoming_joint_force = forces.to(self._device) + # -- Actions (no-op for testing) -- + + def apply_forces_and_torques_at_position( + self, + force_data: torch.Tensor | None = None, + torque_data: torch.Tensor | None = None, + position_data: torch.Tensor | None = None, + indices: torch.Tensor | None = None, + is_global: bool = True, + ) -> None: + """Apply forces and torques at positions (no-op in mock). + + Args: + force_data: Forces to apply, shape (N, 3) or (len(indices), 3). + torque_data: Torques to apply, shape (N, 3) or (len(indices), 3). + position_data: Positions to apply forces at, shape (N, 3) or (len(indices), 3). + indices: Optional indices of articulations to apply to. + is_global: Whether forces/torques are in global frame. + """ + pass # No-op for mock + + # -- Tendon Getters (stubs) -- + + def get_fixed_tendon_stiffnesses(self) -> torch.Tensor: + """Get fixed tendon stiffnesses. Returns zeros of shape (N, max_fixed_tendons).""" + return torch.zeros(self._count, self._max_fixed_tendons, device="cpu") + + def get_fixed_tendon_dampings(self) -> torch.Tensor: + """Get fixed tendon dampings. Returns zeros of shape (N, max_fixed_tendons).""" + return torch.zeros(self._count, self._max_fixed_tendons, device="cpu") + + def get_fixed_tendon_limit_stiffnesses(self) -> torch.Tensor: + """Get fixed tendon limit stiffnesses. Returns zeros of shape (N, max_fixed_tendons).""" + return torch.zeros(self._count, self._max_fixed_tendons, device="cpu") + + def get_fixed_tendon_rest_lengths(self) -> torch.Tensor: + """Get fixed tendon rest lengths. Returns zeros of shape (N, max_fixed_tendons).""" + return torch.zeros(self._count, self._max_fixed_tendons, device="cpu") + + def get_fixed_tendon_offsets(self) -> torch.Tensor: + """Get fixed tendon offsets. Returns zeros of shape (N, max_fixed_tendons).""" + return torch.zeros(self._count, self._max_fixed_tendons, device="cpu") + + def get_fixed_tendon_limits(self) -> torch.Tensor: + """Get fixed tendon limits. Returns zeros of shape (N, max_fixed_tendons, 2).""" + return torch.zeros(self._count, self._max_fixed_tendons, 2, device="cpu") + + def get_spatial_tendon_stiffnesses(self) -> torch.Tensor: + """Get spatial tendon stiffnesses. Returns zeros of shape (N, max_spatial_tendons).""" + return torch.zeros(self._count, self._max_spatial_tendons, device="cpu") + + def get_spatial_tendon_dampings(self) -> torch.Tensor: + """Get spatial tendon dampings. Returns zeros of shape (N, max_spatial_tendons).""" + return torch.zeros(self._count, self._max_spatial_tendons, device="cpu") + + def get_spatial_tendon_limit_stiffnesses(self) -> torch.Tensor: + """Get spatial tendon limit stiffnesses. Returns zeros of shape (N, max_spatial_tendons).""" + return torch.zeros(self._count, self._max_spatial_tendons, device="cpu") + + def get_spatial_tendon_offsets(self) -> torch.Tensor: + """Get spatial tendon offsets. Returns zeros of shape (N, max_spatial_tendons).""" + return torch.zeros(self._count, self._max_spatial_tendons, device="cpu") + + # -- Tendon Setters (no-op stubs) -- + + def set_fixed_tendon_properties( + self, + stiffness: torch.Tensor | None = None, + damping: torch.Tensor | None = None, + limit_stiffness: torch.Tensor | None = None, + pos_limits: torch.Tensor | None = None, + rest_length: torch.Tensor | None = None, + offset: torch.Tensor | None = None, + indices: torch.Tensor | None = None, + ) -> None: + """Set fixed tendon properties (no-op in mock).""" + pass # No-op for mock + + def set_spatial_tendon_properties( + self, + stiffness: torch.Tensor | None = None, + damping: torch.Tensor | None = None, + limit_stiffness: torch.Tensor | None = None, + offset: torch.Tensor | None = None, + indices: torch.Tensor | None = None, + ) -> None: + """Set spatial tendon properties (no-op in mock).""" + pass # No-op for mock + def set_random_mock_data(self) -> None: """Set all internal state to random values for benchmarking. diff --git a/source/isaaclab_physx/isaaclab_physx/test/mock_interfaces/views/mock_articulation_view_warp.py b/source/isaaclab_physx/isaaclab_physx/test/mock_interfaces/views/mock_articulation_view_warp.py index 4d02ee4be6a..559c86c89f0 100644 --- a/source/isaaclab_physx/isaaclab_physx/test/mock_interfaces/views/mock_articulation_view_warp.py +++ b/source/isaaclab_physx/isaaclab_physx/test/mock_interfaces/views/mock_articulation_view_warp.py @@ -179,6 +179,13 @@ def prim_paths(self) -> list[str]: """USD prim paths for each instance.""" return self._prim_paths + @property + def dof_paths(self) -> list[list[str]]: + """DOF paths for each instance.""" + dof_names = self._shared_metatype.dof_names or [f"joint_{i}" for i in range(self._num_dofs)] + single_instance_paths = [f"{self._prim_paths[0]}/{name}" for name in dof_names] + return [single_instance_paths] * self._count + # -- Root Getters -- def get_root_transforms(self) -> wp.array: @@ -1021,6 +1028,95 @@ def set_mock_inertias(self, inertias: wp.array) -> None: """ self._inertias = wp.clone(inertias) + # -- Actions (no-op for testing) -- + + def apply_forces_and_torques_at_position( + self, + force_data: wp.array | None = None, + torque_data: wp.array | None = None, + position_data: wp.array | None = None, + indices: wp.array | None = None, + is_global: bool = True, + ) -> None: + """Apply forces and torques at positions (no-op in mock). + + Args: + force_data: Forces to apply, shape (N, 3) or (len(indices), 3) with dtype=wp.float32. + torque_data: Torques to apply, shape (N, 3) or (len(indices), 3) with dtype=wp.float32. + position_data: Positions to apply forces at, shape (N, 3) or (len(indices), 3) with dtype=wp.float32. + indices: Optional indices of articulations to apply to. + is_global: Whether forces/torques are in global frame. + """ + pass # No-op for mock + + # -- Tendon Getters (stubs) -- + + def get_fixed_tendon_stiffnesses(self) -> wp.array: + """Get fixed tendon stiffnesses. Returns zeros of shape (N, max_fixed_tendons).""" + return wp.zeros((self._count, self._max_fixed_tendons), dtype=wp.float32, device="cpu") + + def get_fixed_tendon_dampings(self) -> wp.array: + """Get fixed tendon dampings. Returns zeros of shape (N, max_fixed_tendons).""" + return wp.zeros((self._count, self._max_fixed_tendons), dtype=wp.float32, device="cpu") + + def get_fixed_tendon_limit_stiffnesses(self) -> wp.array: + """Get fixed tendon limit stiffnesses. Returns zeros of shape (N, max_fixed_tendons).""" + return wp.zeros((self._count, self._max_fixed_tendons), dtype=wp.float32, device="cpu") + + def get_fixed_tendon_rest_lengths(self) -> wp.array: + """Get fixed tendon rest lengths. Returns zeros of shape (N, max_fixed_tendons).""" + return wp.zeros((self._count, self._max_fixed_tendons), dtype=wp.float32, device="cpu") + + def get_fixed_tendon_offsets(self) -> wp.array: + """Get fixed tendon offsets. Returns zeros of shape (N, max_fixed_tendons).""" + return wp.zeros((self._count, self._max_fixed_tendons), dtype=wp.float32, device="cpu") + + def get_fixed_tendon_limits(self) -> wp.array: + """Get fixed tendon limits. Returns zeros of shape (N, max_fixed_tendons, 2).""" + return wp.zeros((self._count, self._max_fixed_tendons, 2), dtype=wp.float32, device="cpu") + + def get_spatial_tendon_stiffnesses(self) -> wp.array: + """Get spatial tendon stiffnesses. Returns zeros of shape (N, max_spatial_tendons).""" + return wp.zeros((self._count, self._max_spatial_tendons), dtype=wp.float32, device="cpu") + + def get_spatial_tendon_dampings(self) -> wp.array: + """Get spatial tendon dampings. Returns zeros of shape (N, max_spatial_tendons).""" + return wp.zeros((self._count, self._max_spatial_tendons), dtype=wp.float32, device="cpu") + + def get_spatial_tendon_limit_stiffnesses(self) -> wp.array: + """Get spatial tendon limit stiffnesses. Returns zeros of shape (N, max_spatial_tendons).""" + return wp.zeros((self._count, self._max_spatial_tendons), dtype=wp.float32, device="cpu") + + def get_spatial_tendon_offsets(self) -> wp.array: + """Get spatial tendon offsets. Returns zeros of shape (N, max_spatial_tendons).""" + return wp.zeros((self._count, self._max_spatial_tendons), dtype=wp.float32, device="cpu") + + # -- Tendon Setters (no-op stubs) -- + + def set_fixed_tendon_properties( + self, + stiffness: wp.array | None = None, + damping: wp.array | None = None, + limit_stiffness: wp.array | None = None, + pos_limits: wp.array | None = None, + rest_length: wp.array | None = None, + offset: wp.array | None = None, + indices: wp.array | None = None, + ) -> None: + """Set fixed tendon properties (no-op in mock).""" + pass # No-op for mock + + def set_spatial_tendon_properties( + self, + stiffness: wp.array | None = None, + damping: wp.array | None = None, + limit_stiffness: wp.array | None = None, + offset: wp.array | None = None, + indices: wp.array | None = None, + ) -> None: + """Set spatial tendon properties (no-op in mock).""" + pass # No-op for mock + # -- Benchmark Utilities -- def set_random_mock_data(self) -> None: diff --git a/source/isaaclab_physx/isaaclab_physx/test/mock_interfaces/views/mock_rigid_body_view.py b/source/isaaclab_physx/isaaclab_physx/test/mock_interfaces/views/mock_rigid_body_view.py index e2d91e638cb..8c1ca5c92ea 100644 --- a/source/isaaclab_physx/isaaclab_physx/test/mock_interfaces/views/mock_rigid_body_view.py +++ b/source/isaaclab_physx/isaaclab_physx/test/mock_interfaces/views/mock_rigid_body_view.py @@ -322,18 +322,18 @@ def set_mock_inertias(self, inertias: torch.Tensor) -> None: def apply_forces_and_torques_at_position( self, - forces: torch.Tensor | None = None, - torques: torch.Tensor | None = None, - positions: torch.Tensor | None = None, + force_data: torch.Tensor | None = None, + torque_data: torch.Tensor | None = None, + position_data: torch.Tensor | None = None, indices: torch.Tensor | None = None, is_global: bool = True, ) -> None: """Apply forces and torques at positions (no-op in mock). Args: - forces: Forces to apply, shape (N, 3) or (len(indices), 3). - torques: Torques to apply, shape (N, 3) or (len(indices), 3). - positions: Positions to apply forces at, shape (N, 3) or (len(indices), 3). + force_data: Forces to apply, shape (N, 3) or (len(indices), 3). + torque_data: Torques to apply, shape (N, 3) or (len(indices), 3). + position_data: Positions to apply forces at, shape (N, 3) or (len(indices), 3). indices: Optional indices of bodies to apply to. is_global: Whether forces/torques are in global frame. """ diff --git a/source/isaaclab_physx/isaaclab_physx/test/mock_interfaces/views/mock_rigid_body_view_warp.py b/source/isaaclab_physx/isaaclab_physx/test/mock_interfaces/views/mock_rigid_body_view_warp.py index f56e42154d0..a3ef67fefdd 100644 --- a/source/isaaclab_physx/isaaclab_physx/test/mock_interfaces/views/mock_rigid_body_view_warp.py +++ b/source/isaaclab_physx/isaaclab_physx/test/mock_interfaces/views/mock_rigid_body_view_warp.py @@ -381,18 +381,18 @@ def set_random_mock_data(self) -> None: def apply_forces_and_torques_at_position( self, - forces: wp.array | None = None, - torques: wp.array | None = None, - positions: wp.array | None = None, + force_data: wp.array | None = None, + torque_data: wp.array | None = None, + position_data: wp.array | None = None, indices: wp.array | None = None, is_global: bool = True, ) -> None: """Apply forces and torques at positions (no-op in mock). Args: - forces: Forces to apply, shape (N, 3) or (len(indices), 3) with dtype=wp.float32. - torques: Torques to apply, shape (N, 3) or (len(indices), 3) with dtype=wp.float32. - positions: Positions to apply forces at, shape (N, 3) or (len(indices), 3) with dtype=wp.float32. + force_data: Forces to apply, shape (N, 3) or (len(indices), 3) with dtype=wp.float32. + torque_data: Torques to apply, shape (N, 3) or (len(indices), 3) with dtype=wp.float32. + position_data: Positions to apply forces at, shape (N, 3) or (len(indices), 3) with dtype=wp.float32. indices: Optional indices of bodies to apply to. is_global: Whether forces/torques are in global frame. """ diff --git a/source/isaaclab_physx/test/test_mock_interfaces/test_mock_rigid_body_view.py b/source/isaaclab_physx/test/test_mock_interfaces/test_mock_rigid_body_view.py index 014423ee481..04423ce6008 100644 --- a/source/isaaclab_physx/test/test_mock_interfaces/test_mock_rigid_body_view.py +++ b/source/isaaclab_physx/test/test_mock_interfaces/test_mock_rigid_body_view.py @@ -196,9 +196,9 @@ def test_apply_forces_and_torques_at_position_noop(self): view = MockRigidBodyView(count=4) # Should not raise view.apply_forces_and_torques_at_position( - forces=torch.randn(4, 3), - torques=torch.randn(4, 3), - positions=torch.randn(4, 3), + force_data=torch.randn(4, 3), + torque_data=torch.randn(4, 3), + position_data=torch.randn(4, 3), ) diff --git a/source/isaaclab_physx/test/test_mock_interfaces/test_mock_rigid_body_view_warp.py b/source/isaaclab_physx/test/test_mock_interfaces/test_mock_rigid_body_view_warp.py index 7fb30236a87..2e752052e9a 100644 --- a/source/isaaclab_physx/test/test_mock_interfaces/test_mock_rigid_body_view_warp.py +++ b/source/isaaclab_physx/test/test_mock_interfaces/test_mock_rigid_body_view_warp.py @@ -236,9 +236,9 @@ def test_apply_forces_and_torques_at_position_noop(self): torques = wp.zeros((4, 3), dtype=wp.float32, device="cpu") positions = wp.zeros((4, 3), dtype=wp.float32, device="cpu") view.apply_forces_and_torques_at_position( - forces=forces, - torques=torques, - positions=positions, + force_data=forces, + torque_data=torques, + position_data=positions, )