diff --git a/CONTRIBUTORS.md b/CONTRIBUTORS.md index 0feaabb2c8f..5c78a4cbe86 100644 --- a/CONTRIBUTORS.md +++ b/CONTRIBUTORS.md @@ -51,6 +51,7 @@ Guidelines for modifications: * Bingjie Tang * Brayden Zhang * Brian Bingham +* Brian McCann * Cameron Upright * Calvin Yu * Cathy Y. Li diff --git a/source/isaaclab/config/extension.toml b/source/isaaclab/config/extension.toml index cbc2de67560..6a3eb95cff3 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 = "0.47.2" +version = "0.48.2" # Description title = "Isaac Lab framework for Robot Learning" diff --git a/source/isaaclab/docs/CHANGELOG.rst b/source/isaaclab/docs/CHANGELOG.rst index 38834c3624d..39e9e33548e 100644 --- a/source/isaaclab/docs/CHANGELOG.rst +++ b/source/isaaclab/docs/CHANGELOG.rst @@ -1,6 +1,14 @@ Changelog --------- +0.48.2 (2025-10-22) +~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Implement ability to attach an imu sensor to xform primitives in a usd file. This PR is based on work by @GiulioRomualdi here: #3094 Addressing issue #3088. + 0.47.2 (2025-10-22) ~~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab/isaaclab/sensors/imu/imu.py b/source/isaaclab/isaaclab/sensors/imu/imu.py index 1c700eeedb2..5424853f9f1 100644 --- a/source/isaaclab/isaaclab/sensors/imu/imu.py +++ b/source/isaaclab/isaaclab/sensors/imu/imu.py @@ -11,11 +11,11 @@ import isaacsim.core.utils.stage as stage_utils from isaacsim.core.simulation_manager import SimulationManager -from pxr import UsdPhysics import isaaclab.sim as sim_utils import isaaclab.utils.math as math_utils from isaaclab.markers import VisualizationMarkers +from isaaclab.sim.utils import resolve_pose_relative_to_physx_parent from ..sensor_base import SensorBase from .imu_data import ImuData @@ -27,10 +27,12 @@ class Imu(SensorBase): """The Inertia Measurement Unit (IMU) sensor. - The sensor can be attached to any :class:`RigidObject` or :class:`Articulation` in the scene. The sensor provides complete state information. - The sensor is primarily used to provide the linear acceleration and angular velocity of the object in the body frame. The sensor also provides - the position and orientation of the object in the world frame and the angular acceleration and linear velocity in the body frame. The extra - data outputs are useful for simulating with or comparing against "perfect" state estimation. + The sensor can be attached to any prim path and produces body-frame linear acceleration and angular velocity, + along with world-frame pose and body-frame linear and angular accelerations/velocities. + + If the provided path is not a rigid body, the closest rigid-body ancestor is used for simulation queries. + The fixed transform from that ancestor to the target prim is computed once during initialization and + composed with the configured sensor offset. .. note:: @@ -40,10 +42,13 @@ class Imu(SensorBase): .. note:: - It is suggested to use the OffsetCfg to define an IMU frame relative to a rigid body prim defined at the root of - a :class:`RigidObject` or a prim that is defined by a non-fixed joint in an :class:`Articulation` (except for the - root of a fixed based articulation). The use frames with fixed joints and small mass/inertia to emulate a transform - relative to a body frame can result in lower performance and accuracy. + The user can configure the sensor offset in the configuration file. The offset is applied relative to the rigid source prim. + If the target prim is not a rigid body, the offset is composed with the fixed transform + from the rigid ancestor to the target prim. The offset is applied in the body frame of the rigid source prim. + The offset is defined as a position vector and a quaternion rotation, which + are applied in the order: position, then rotation. The position is applied as a translation + in the body frame of the rigid source prim, and the rotation is applied as a rotation + in the body frame of the rigid source prim. """ @@ -61,6 +66,9 @@ def __init__(self, cfg: ImuCfg): # Create empty variables for storing output data self._data = ImuData() + # Internal: expression used to build the rigid body view (may be different from cfg.prim_path) + self._rigid_parent_expr: str | None = None + def __str__(self) -> str: """Returns: A string containing information about the instance.""" return ( @@ -119,11 +127,9 @@ def update(self, dt: float, force_recompute: bool = False): def _initialize_impl(self): """Initializes the sensor handles and internal buffers. - This function creates handles and registers the provided data types with the replicator registry to - be able to access the data from the sensor. It also initializes the internal buffers to store the data. - - Raises: - RuntimeError: If the imu prim is not a RigidBodyPrim + - If the target prim path is a rigid body, build the view directly on it. + - Otherwise find the closest rigid-body ancestor, cache the fixed transform from that ancestor + to the target prim, and build the view on the ancestor expression. """ # Initialize parent class super()._initialize_impl() @@ -133,11 +139,12 @@ def _initialize_impl(self): prim = sim_utils.find_first_matching_prim(self.cfg.prim_path) if prim is None: raise RuntimeError(f"Failed to find a prim at path expression: {self.cfg.prim_path}") - # check if it is a RigidBody Prim - if prim.HasAPI(UsdPhysics.RigidBodyAPI): - self._view = self._physics_sim_view.create_rigid_body_view(self.cfg.prim_path.replace(".*", "*")) - else: - raise RuntimeError(f"Failed to find a RigidBodyAPI for the prim paths: {self.cfg.prim_path}") + + # Determine rigid source prim and (if needed) the fixed transform from that rigid prim to target prim + self._rigid_parent_expr, fixed_pos_b, fixed_quat_b = resolve_pose_relative_to_physx_parent(self.cfg.prim_path) + + # Create the rigid body view on the ancestor + self._view = self._physics_sim_view.create_rigid_body_view(self._rigid_parent_expr) # Get world gravity gravity = self._physics_sim_view.get_gravity() @@ -148,35 +155,53 @@ def _initialize_impl(self): # Create internal buffers self._initialize_buffers_impl() + # Compose the configured offset with the fixed ancestor->target transform (done once) + # new_offset = fixed * cfg.offset + # where composition is: p = p_fixed + R_fixed * p_cfg, q = q_fixed * q_cfg + if fixed_pos_b is not None and fixed_quat_b is not None: + # Broadcast fixed transform across instances + fixed_p = torch.tensor(fixed_pos_b, device=self._device).repeat(self._view.count, 1) + fixed_q = torch.tensor(fixed_quat_b, device=self._device).repeat(self._view.count, 1) + + cfg_p = self._offset_pos_b.clone() + cfg_q = self._offset_quat_b.clone() + + composed_p = fixed_p + math_utils.quat_apply(fixed_q, cfg_p) + composed_q = math_utils.quat_mul(fixed_q, cfg_q) + + self._offset_pos_b = composed_p + self._offset_quat_b = composed_q + def _update_buffers_impl(self, env_ids: Sequence[int]): """Fills the buffers of the sensor data.""" # default to all sensors if len(env_ids) == self._num_envs: env_ids = slice(None) - # obtain the poses of the sensors + # world pose of the rigid source (ancestor) from the PhysX view pos_w, quat_w = self._view.get_transforms()[env_ids].split([3, 4], dim=-1) quat_w = quat_w.roll(1, dims=-1) - # store the poses + # sensor pose in world: apply composed offset self._data.pos_w[env_ids] = pos_w + math_utils.quat_apply(quat_w, self._offset_pos_b[env_ids]) self._data.quat_w[env_ids] = math_utils.quat_mul(quat_w, self._offset_quat_b[env_ids]) - # get the offset from COM to link origin + # COM of rigid source (body frame) com_pos_b = self._view.get_coms().to(self.device).split([3, 4], dim=-1)[0] - # obtain the velocities of the link COM + # Velocities at rigid source COM lin_vel_w, ang_vel_w = self._view.get_velocities()[env_ids].split([3, 3], dim=-1) - # if an offset is present or the COM does not agree with the link origin, the linear velocity has to be - # transformed taking the angular velocity into account + + # If sensor offset or COM != link origin, account for angular velocity contribution lin_vel_w += torch.linalg.cross( ang_vel_w, math_utils.quat_apply(quat_w, self._offset_pos_b[env_ids] - com_pos_b[env_ids]), dim=-1 ) - # numerical derivative + # numerical derivative (world frame) lin_acc_w = (lin_vel_w - self._prev_lin_vel_w[env_ids]) / self._dt + self._gravity_bias_w[env_ids] ang_acc_w = (ang_vel_w - self._prev_ang_vel_w[env_ids]) / self._dt - # stack data in world frame and batch rotate + + # batch rotate world->body using current sensor orientation dynamics_data = torch.stack((lin_vel_w, ang_vel_w, lin_acc_w, ang_acc_w, self.GRAVITY_VEC_W[env_ids]), dim=0) dynamics_data_rot = math_utils.quat_apply_inverse(self._data.quat_w[env_ids].repeat(5, 1), dynamics_data).chunk( 5, dim=0 @@ -207,7 +232,7 @@ def _initialize_buffers_impl(self): self._prev_lin_vel_w = torch.zeros_like(self._data.pos_w) self._prev_ang_vel_w = torch.zeros_like(self._data.pos_w) - # store sensor offset transformation + # store sensor offset (applied relative to rigid source). This may be composed later with a fixed ancestor->target transform. self._offset_pos_b = torch.tensor(list(self.cfg.offset.pos), device=self._device).repeat(self._view.count, 1) self._offset_quat_b = torch.tensor(list(self.cfg.offset.rot), device=self._device).repeat(self._view.count, 1) # set gravity bias diff --git a/source/isaaclab/isaaclab/sim/utils.py b/source/isaaclab/isaaclab/sim/utils.py index 338ec5d843a..b57412cd2b6 100644 --- a/source/isaaclab/isaaclab/sim/utils.py +++ b/source/isaaclab/isaaclab/sim/utils.py @@ -895,6 +895,90 @@ def find_matching_prim_paths(prim_path_regex: str, stage: Usd.Stage | None = Non return output_prim_paths +def check_prim_implements_apis( + prim: Usd.Prim, apis: list[Usd.APISchemaBase] | Usd.APISchemaBase = UsdPhysics.RigidBodyAPI +) -> bool: + """Return true if prim implements all apis, False otherwise.""" + + if type(apis) is not list: + return prim.HasAPI(apis) + else: + return all(prim.HasAPI(api) for api in apis) + + +def resolve_pose_relative_to_physx_parent( + prim_path_regex: str, + implements_apis: list[Usd.APISchemaBase] | Usd.APISchemaBase = UsdPhysics.RigidBodyAPI, + *, + stage: Usd.Stage | None = None, +) -> tuple[str, tuple[float, float, float], tuple[float, float, float, float]]: + """For some applications, it can be important to identify the closest parent primitive which implements certain APIs + in order to retrieve data from PhysX (for example, force information requires more than an XFormPrim). When an object is + nested beneath a reference frame which is not represented by a PhysX tensor, it can be useful to extract the relative pose + between the primitive and the closest parent implementing the necessary API in the PhysX representation. This function + identifies the closest appropriate parent. The fixed transform is computed as ancestor->target (in ancestor + /body frame). If the first primitive in the prim_path already implements the necessary APIs, return identity. + + Args: + prim_path_regex (str): A str refelcting a primitive path pattern (e.g. from a cfg) + implements_apis (list[ Usd.APISchemaBase] | Usd.APISchemaBase): APIs ancestor must implement. + Returns: + ancestor_path (str): Prim Path Expression including wildcards for the closest PhysX Parent + fixed_pos_b (tuple[float, float, float]): positional offset + fixed_quat_b (tuple[float, float, float, float]): rotational offset + ). + """ + target_prim = find_first_matching_prim(prim_path_regex, stage) + + if target_prim is None: + raise RuntimeError(f"Path: {prim_path_regex} does not match any existing primitives.") + # If target prim itself implements all required APIs, we can use it directly. + if check_prim_implements_apis(target_prim, implements_apis): + return prim_path_regex.replace(".*", "*"), None, None + # Walk up to find closest ancestor which implements all required APIs + ancestor = target_prim.GetParent() + while ancestor and ancestor.IsValid(): + if check_prim_implements_apis(ancestor, implements_apis): + break + ancestor = ancestor.GetParent() + if not ancestor or not ancestor.IsValid(): + raise RuntimeError(f"Path '{target_prim.GetPath()}' has no ancestor which implements all required APIs.") + # Compute fixed transform ancestor->target at default time + xcache = UsdGeom.XformCache(Usd.TimeCode.Default()) + + # Compute relative transform + X_ancestor_to_target, __ = xcache.ComputeRelativeTransform(target_prim, ancestor) + + # Extract pos, quat from matrix (right-handed, column major) + # Gf decomposes as translation and rotation quaternion + t = X_ancestor_to_target.ExtractTranslation() + r = X_ancestor_to_target.ExtractRotationQuat() + + fixed_pos_b = (t[0], t[1], t[2]) + # Convert Gf.Quatf (w, x, y, z) to tensor order [w, x, y, z] + fixed_quat_b = (float(r.GetReal()), r.GetImaginary()[0], r.GetImaginary()[1], r.GetImaginary()[2]) + + # This restores regex patterns from the original PathPattern in the path to return. + # ( Omnikit 18+ may provide a cleaner approach without relying on strings ) + child_path = target_prim.GetPrimPath() + ancestor_path = ancestor.GetPrimPath() + rel = child_path.MakeRelativePath(ancestor_path).pathString + + if rel and prim_path_regex.endswith(rel): + # Remove "/" or "" at end + cut_len = len(rel) + trimmed = prim_path_regex + if trimmed.endswith("/" + rel): + trimmed = trimmed[: -(cut_len + 1)] + else: + trimmed = trimmed[:-cut_len] + ancestor_path = trimmed + + ancestor_path = ancestor_path.replace(".*", "*") + + return ancestor_path, fixed_pos_b, fixed_quat_b + + def find_global_fixed_joint_prim( prim_path: str | Sdf.Path, check_enabled_only: bool = False, stage: Usd.Stage | None = None ) -> UsdPhysics.Joint | None: diff --git a/source/isaaclab/test/sensors/test_imu.py b/source/isaaclab/test/sensors/test_imu.py index 7f621a36574..576df483f2d 100644 --- a/source/isaaclab/test/sensors/test_imu.py +++ b/source/isaaclab/test/sensors/test_imu.py @@ -11,7 +11,6 @@ app_launcher = AppLauncher(headless=True, enable_cameras=True) simulation_app = app_launcher.app - """Rest everything follows.""" import pathlib @@ -26,7 +25,7 @@ from isaaclab.assets import ArticulationCfg, RigidObjectCfg from isaaclab.markers.config import GREEN_ARROW_X_MARKER_CFG, RED_ARROW_X_MARKER_CFG from isaaclab.scene import InteractiveScene, InteractiveSceneCfg -from isaaclab.sensors.imu import ImuCfg +from isaaclab.sensors.imu import Imu, ImuCfg from isaaclab.terrains import TerrainImporterCfg from isaaclab.utils import configclass @@ -83,6 +82,7 @@ class MySceneCfg(InteractiveSceneCfg): # articulations - robot robot = ANYMAL_C_CFG.replace(prim_path="{ENV_REGEX_NS}/robot") + # pendulum1 pendulum = ArticulationCfg( prim_path="{ENV_REGEX_NS}/pendulum", spawn=sim_utils.UrdfFileCfg( @@ -102,6 +102,27 @@ class MySceneCfg(InteractiveSceneCfg): "joint_1_act": ImplicitActuatorCfg(joint_names_expr=["joint_.*"], stiffness=0.0, damping=0.3), }, ) + # pendulum2 + pendulum2 = ArticulationCfg( + prim_path="{ENV_REGEX_NS}/pendulum2", + spawn=sim_utils.UrdfFileCfg( + fix_base=True, + merge_fixed_joints=True, + make_instanceable=False, + asset_path=f"{pathlib.Path(__file__).parent.resolve()}/urdfs/simple_2_link.urdf", + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=True, solver_position_iteration_count=4, solver_velocity_iteration_count=0 + ), + joint_drive=sim_utils.UrdfConverterCfg.JointDriveCfg( + gains=sim_utils.UrdfConverterCfg.JointDriveCfg.PDGainsCfg(stiffness=None, damping=None) + ), + ), + init_state=ArticulationCfg.InitialStateCfg(), + actuators={ + "joint_1_act": ImplicitActuatorCfg(joint_names_expr=["joint_.*"], stiffness=0.0, damping=0.3), + }, + ) + # sensors - imu (filled inside unit test) imu_ball: ImuCfg = ImuCfg( prim_path="{ENV_REGEX_NS}/ball", @@ -123,7 +144,30 @@ class MySceneCfg(InteractiveSceneCfg): ), gravity_bias=(0.0, 0.0, 0.0), ) - + imu_robot_norb: ImuCfg = ImuCfg( + prim_path="{ENV_REGEX_NS}/robot/LF_HIP/LF_hip_fixed", + offset=ImuCfg.OffsetCfg( + pos=POS_OFFSET, + rot=ROT_OFFSET, + ), + gravity_bias=(0.0, 0.0, 0.0), + ) + imu_indirect_pendulum_link: ImuCfg = ImuCfg( + prim_path="{ENV_REGEX_NS}/pendulum2/link_1/imu_link", + debug_vis=not app_launcher._headless, + visualizer_cfg=RED_ARROW_X_MARKER_CFG.replace(prim_path="/Visuals/Acceleration/imu_link"), + gravity_bias=(0.0, 0.0, 9.81), + ) + imu_indirect_pendulum_base: ImuCfg = ImuCfg( + prim_path="{ENV_REGEX_NS}/pendulum2/link_1", + offset=ImuCfg.OffsetCfg( + pos=PEND_POS_OFFSET, + rot=PEND_ROT_OFFSET, + ), + debug_vis=not app_launcher._headless, + visualizer_cfg=GREEN_ARROW_X_MARKER_CFG.replace(prim_path="/Visuals/Acceleration/base"), + gravity_bias=(0.0, 0.0, 9.81), + ) imu_pendulum_imu_link: ImuCfg = ImuCfg( prim_path="{ENV_REGEX_NS}/pendulum/imu_link", debug_vis=not app_launcher._headless, @@ -145,7 +189,8 @@ def __post_init__(self): """Post initialization.""" # change position of the robot self.robot.init_state.pos = (0.0, 2.0, 1.0) - self.pendulum.init_state.pos = (-1.0, 1.0, 0.5) + self.pendulum.init_state.pos = (-2.0, 1.0, 0.5) + self.pendulum2.init_state.pos = (2.0, 1.0, 0.5) # change asset self.robot.spawn.usd_path = f"{ISAAC_NUCLEUS_DIR}/Robots/ANYbotics/anymal_c/anymal_c.usd" @@ -441,10 +486,153 @@ def test_single_dof_pendulum(setup_sim): ) +@pytest.mark.isaacsim_ci +def test_indirect_attachment(setup_sim): + """Test attaching the imu through an xForm primitive configuration argument.""" + sim, scene = setup_sim + # pendulum length + pend_length = PEND_POS_OFFSET[0] + + # should achieve same results between the two imu sensors on the robot + for idx in range(500): + + # write data to sim + scene.write_data_to_sim() + # perform step + sim.step() + # read data from sim + scene.update(sim.get_physics_dt()) + + # get pendulum joint state + joint_pos = scene.articulations["pendulum2"].data.joint_pos + joint_vel = scene.articulations["pendulum2"].data.joint_vel + joint_acc = scene.articulations["pendulum2"].data.joint_acc + + imu = scene.sensors["imu_indirect_pendulum_link"] + imu_base = scene.sensors["imu_indirect_pendulum_base"] + + torch.testing.assert_close( + imu._offset_pos_b, + imu_base._offset_pos_b, + ) + torch.testing.assert_close(imu._offset_quat_b, imu_base._offset_quat_b, rtol=1e-4, atol=1e-4) + + # IMU and base data + imu_data = scene.sensors["imu_indirect_pendulum_link"].data + base_data = scene.sensors["imu_indirect_pendulum_base"].data + # extract imu_link imu_sensor dynamics + lin_vel_w_imu_link = math_utils.quat_apply(imu_data.quat_w, imu_data.lin_vel_b) + lin_acc_w_imu_link = math_utils.quat_apply(imu_data.quat_w, imu_data.lin_acc_b) + + # calculate the joint dynamics from the imu_sensor (y axis of imu_link is parallel to joint axis of pendulum) + joint_vel_imu = math_utils.quat_apply(imu_data.quat_w, imu_data.ang_vel_b)[..., 1].unsqueeze(-1) + joint_acc_imu = math_utils.quat_apply(imu_data.quat_w, imu_data.ang_acc_b)[..., 1].unsqueeze(-1) + + # calculate analytical solution + vx = -joint_vel * pend_length * torch.sin(joint_pos) + vy = torch.zeros(2, 1, device=scene.device) + vz = -joint_vel * pend_length * torch.cos(joint_pos) + gt_linear_vel_w = torch.cat([vx, vy, vz], dim=-1) + + ax = -joint_acc * pend_length * torch.sin(joint_pos) - joint_vel**2 * pend_length * torch.cos(joint_pos) + ay = torch.zeros(2, 1, device=scene.device) + az = -joint_acc * pend_length * torch.cos(joint_pos) + joint_vel**2 * pend_length * torch.sin(joint_pos) + 9.81 + gt_linear_acc_w = torch.cat([ax, ay, az], dim=-1) + + # skip first step where initial velocity is zero + if idx < 2: + continue + + # compare imu projected gravity + gravity_dir_w = torch.tensor((0.0, 0.0, -1.0), device=scene.device).repeat(2, 1) + gravity_dir_b = math_utils.quat_apply_inverse(imu_data.quat_w, gravity_dir_w) + torch.testing.assert_close( + imu_data.projected_gravity_b, + gravity_dir_b, + ) + + # compare imu angular velocity with joint velocity + torch.testing.assert_close( + joint_vel, + joint_vel_imu, + rtol=1e-1, + atol=1e-3, + ) + # compare imu angular acceleration with joint acceleration + torch.testing.assert_close( + joint_acc, + joint_acc_imu, + rtol=1e-1, + atol=1e-3, + ) + # compare imu linear velocity with simple pendulum calculation + torch.testing.assert_close( + gt_linear_vel_w, + lin_vel_w_imu_link, + rtol=1e-1, + atol=1e-3, + ) + # compare imu linear acceleration with simple pendulum calculation + torch.testing.assert_close( + gt_linear_acc_w, + lin_acc_w_imu_link, + rtol=1e-1, + atol=1e0, + ) + + # check the position between offset and imu definition + torch.testing.assert_close( + base_data.pos_w, + imu_data.pos_w, + rtol=1e-5, + atol=1e-5, + ) + + # check the orientation between offset and imu definition + torch.testing.assert_close( + base_data.quat_w, + imu_data.quat_w, + rtol=1e-4, + atol=1e-4, + ) + + # check the angular velocities of the imus between offset and imu definition + torch.testing.assert_close( + base_data.ang_vel_b, + imu_data.ang_vel_b, + rtol=1e-4, + atol=1e-4, + ) + # check the angular acceleration of the imus between offset and imu definition + torch.testing.assert_close( + base_data.ang_acc_b, + imu_data.ang_acc_b, + rtol=1e-4, + atol=1e-4, + ) + + # check the linear velocity of the imus between offset and imu definition + torch.testing.assert_close( + base_data.lin_vel_b, + imu_data.lin_vel_b, + rtol=1e-2, + atol=5e-3, + ) + + # check the linear acceleration of the imus between offset and imu definition + torch.testing.assert_close( + base_data.lin_acc_b, + imu_data.lin_acc_b, + rtol=1e-1, + atol=1e-1, + ) + + @pytest.mark.isaacsim_ci def test_offset_calculation(setup_sim): """Test offset configuration argument.""" sim, scene = setup_sim + # should achieve same results between the two imu sensors on the robot for idx in range(500): # set acceleration @@ -516,6 +704,21 @@ def test_offset_calculation(setup_sim): ) +@pytest.mark.isaacsim_ci +def test_attachment_validity(setup_sim): + """Test invalid imu attachment. An imu cannot be attached directly to the world. It must be somehow attached to + something implementing physics.""" + sim, scene = setup_sim + imu_world_cfg = ImuCfg( + prim_path="/World/envs/env_0", + gravity_bias=(0.0, 0.0, 0.0), + ) + with pytest.raises(RuntimeError) as exc_info: + imu_world = Imu(imu_world_cfg) + imu_world._initialize_impl() + assert exc_info.type is RuntimeError + + @pytest.mark.isaacsim_ci def test_env_ids_propagation(setup_sim): """Test that env_ids argument propagates through update and reset methods"""