Rename stand_scale to stand_height and lift robot base to match

Signed-off-by: Xinjie Yao <xyao@nvidia.com>

Author: xyao-nv

isaaclab_arena/embodiments/droid/droid.py

@@ -43,8 +43,16 @@
 from isaaclab_arena.utils.pose import Pose
 from isaaclab_arena.variations.camera_extrinsics_variation import CameraExtrinsicsVariation
 
-# Default per-axis scale for the base stand.
-_DEFAULT_STAND_SCALE: tuple[float, float, float] = (1.2, 1.2, 1.7)
+# The base stand's x/y footprint is fixed; only its height (the z-axis scale factor) is
+# configurable, so the stand can be made taller or shorter without changing its footprint.
+_STAND_FOOTPRINT_SCALE_XY: tuple[float, float] = (1.2, 1.2)
+_DEFAULT_STAND_HEIGHT_SCALE: float = 1.7
+
+# The stand's top is pinned at the robot base and it extends downward to the floor, so scaling its
+# height moves the floor contact, not the mount. Measured native (scale=1.0) stand height in meters;
+# used to lift the robot base by the same amount the stand grows, keeping the stand's bottom on the
+# floor while the robot rides higher.
+_STAND_UNIT_HEIGHT_M: float = 0.795
 
 
 class DroidEmbodimentBase(EmbodimentBase, ABC):
@@ -64,11 +72,15 @@ def __init__(
         initial_joint_pose: list[float] | None = None,
         concatenate_observation_terms: bool = False,
         arm_mode: ArmMode | None = None,
-        stand_scale: tuple[float, float, float] = _DEFAULT_STAND_SCALE,
+        stand_height: float = _DEFAULT_STAND_HEIGHT_SCALE,
     ):
         super().__init__(enable_cameras, initial_pose, concatenate_observation_terms, arm_mode)
         self.scene_config = DroidSceneCfg()
-        self.scene_config.stand.spawn.scale = stand_scale
+        self.scene_config.stand.spawn.scale = (*_STAND_FOOTPRINT_SCALE_XY, stand_height)
+        # Lift the robot base (and stand) so a taller/shorter stand keeps its bottom on the floor.
+        self._robot_base_z_offset = _STAND_UNIT_HEIGHT_M * (stand_height - _DEFAULT_STAND_HEIGHT_SCALE)
+        self.scene_config.robot.init_state.pos = self._lift_z(self.scene_config.robot.init_state.pos)
+        self.scene_config.stand.init_state.pos = self._lift_z(self.scene_config.stand.init_state.pos)
         self.action_config = None
         self.camera_config = DroidCameraCfg()
         self.observation_config = DroidObservationsCfg()
@@ -79,13 +91,19 @@ def __init__(
         self.mimic_env = None
         self.add_variation(CameraExtrinsicsVariation(camera_name="wrist_camera"))
 
+    def _lift_z(self, pos: tuple[float, float, float]) -> tuple[float, float, float]:
+        """Return ``pos`` shifted up by the stand-height-driven robot base offset."""
+        return (pos[0], pos[1], pos[2] + self._robot_base_z_offset)
+
     def _update_scene_cfg_with_robot_initial_pose(self, scene_config: Any, pose: Pose) -> Any:
-        # We override the default initial pose setting function in order to also set
-        # the initial pose of the stand.
+        # We override the default initial pose setting function in order to also set the initial pose
+        # of the stand, and to re-apply the stand-height lift on top of the requested pose (the base
+        # implementation overwrites init_state.pos with the raw pose).
         scene_config = super()._update_scene_cfg_with_robot_initial_pose(scene_config, pose)
         if scene_config is None or not hasattr(scene_config, "robot"):
             raise RuntimeError("scene_config must be populated with a `robot` before calling `set_robot_initial_pose`.")
-        scene_config.stand.init_state.pos = pose.position_xyz
+        scene_config.robot.init_state.pos = self._lift_z(pose.position_xyz)
+        scene_config.stand.init_state.pos = self._lift_z(pose.position_xyz)
         scene_config.stand.init_state.rot = pose.rotation_xyzw
 
         return scene_config
@@ -114,15 +132,15 @@ def __init__(
         initial_joint_pose: list[float] | None = None,
         concatenate_observation_terms: bool = False,
         arm_mode: ArmMode | None = None,
-        stand_scale: tuple[float, float, float] = _DEFAULT_STAND_SCALE,
+        stand_height: float = _DEFAULT_STAND_HEIGHT_SCALE,
     ):
         super().__init__(
             enable_cameras,
             initial_pose,
             initial_joint_pose,
             concatenate_observation_terms,
             arm_mode,
-            stand_scale,
+            stand_height,
         )
         self.action_config = DroidDifferentialIKActionsCfg()
 
@@ -141,15 +159,15 @@ def __init__(
         initial_joint_pose: list[float] | None = None,
         concatenate_observation_terms: bool = False,
         arm_mode: ArmMode | None = None,
-        stand_scale: tuple[float, float, float] = _DEFAULT_STAND_SCALE,
+        stand_height: float = _DEFAULT_STAND_HEIGHT_SCALE,
     ):
         super().__init__(
             enable_cameras,
             initial_pose,
             initial_joint_pose,
             concatenate_observation_terms,
             arm_mode,
-            stand_scale,
+            stand_height,
         )
         self.action_config = DroidRelativeJointPositionActionsCfg()
 
@@ -169,15 +187,15 @@ def __init__(
         initial_joint_pose: list[float] | None = None,
         concatenate_observation_terms: bool = False,
         arm_mode: ArmMode | None = None,
-        stand_scale: tuple[float, float, float] = _DEFAULT_STAND_SCALE,
+        stand_height: float = _DEFAULT_STAND_HEIGHT_SCALE,
     ):
         super().__init__(
             enable_cameras,
             initial_pose,
             initial_joint_pose,
             concatenate_observation_terms,
             arm_mode,
-            stand_scale,
+            stand_height,
         )
         self.action_config = DroidAbsoluteJointPositionActionsCfg()
 
@@ -252,7 +270,7 @@ class DroidSceneCfg:
             usd_path=(
                 f"{ISAACLAB_NUCLEUS_DIR}/Arena/assets/object_library/srl_robolab_assets/robots/franka_stand_grey.usda"
             ),
-            scale=_DEFAULT_STAND_SCALE,
+            scale=(*_STAND_FOOTPRINT_SCALE_XY, _DEFAULT_STAND_HEIGHT_SCALE),
             activate_contact_sensors=False,
         ),
     )

isaaclab_arena/tests/test_droid_stand_scale.py

@@ -8,29 +8,50 @@
 from isaaclab_arena.tests.utils.subprocess import run_simulation_app_function
 
 _DEFAULT_STAND_SCALE = (1.2, 1.2, 1.7)
-_CUSTOM_STAND_SCALE = (1.5, 1.5, 2.0)
+_CUSTOM_STAND_HEIGHT = 2.0
+_EXPECTED_CUSTOM_SCALE = (1.2, 1.2, 2.0)
 
 
 def _test_droid_stand_scale(simulation_app) -> bool:
-    """Check that ``stand_scale`` threads through to the stand spawn without touching the robot."""
+    """Check ``stand_height`` sets only the stand z-scale and lifts the robot base to match."""
 
     from isaaclab_arena.assets.registries import AssetRegistry
-    from isaaclab_arena.embodiments.droid.droid import DroidAbsoluteJointPositionEmbodiment
+    from isaaclab_arena.embodiments.droid.droid import (
+        _DEFAULT_STAND_HEIGHT_SCALE,
+        _STAND_UNIT_HEIGHT_M,
+        DroidAbsoluteJointPositionEmbodiment,
+    )
+    from isaaclab_arena.utils.pose import Pose
+
+    expected_offset = _STAND_UNIT_HEIGHT_M * (_CUSTOM_STAND_HEIGHT - _DEFAULT_STAND_HEIGHT_SCALE)
 
     try:
-        # The default is applied when the caller does not set stand_scale.
+        # The default leaves the stand at its nominal scale and the robot base at z=0.
         default_emb = DroidAbsoluteJointPositionEmbodiment()
         assert tuple(default_emb.scene_config.stand.spawn.scale) == _DEFAULT_STAND_SCALE
+        assert default_emb.scene_config.robot.init_state.pos[2] == 0.0
 
-        # An override reaches the stand spawn config, leaving the robot articulation at native scale.
-        custom_emb = DroidAbsoluteJointPositionEmbodiment(stand_scale=_CUSTOM_STAND_SCALE)
-        assert tuple(custom_emb.scene_config.stand.spawn.scale) == _CUSTOM_STAND_SCALE
+        # An override changes only the z-scale (x/y footprint and robot mesh untouched)...
+        custom_emb = DroidAbsoluteJointPositionEmbodiment(stand_height=_CUSTOM_STAND_HEIGHT)
+        assert tuple(custom_emb.scene_config.stand.spawn.scale) == _EXPECTED_CUSTOM_SCALE
         assert custom_emb.scene_config.robot.spawn.scale in (None, (1.0, 1.0, 1.0))
 
-        # The YAML-spec path instantiates the embodiment via asset_class(**params); stand_scale
-        # arriving as a list (as it would from YAML) applies just the same.
-        registry_emb = AssetRegistry().get_asset_by_name("droid_abs_joint_pos")(stand_scale=list(_CUSTOM_STAND_SCALE))
-        assert tuple(registry_emb.scene_config.stand.spawn.scale) == _CUSTOM_STAND_SCALE
+        # ...and lifts the robot base and stand together so the stand's floor contact is preserved.
+        assert abs(custom_emb.scene_config.robot.init_state.pos[2] - expected_offset) < 1e-6
+        assert abs(custom_emb.scene_config.stand.init_state.pos[2] - expected_offset) < 1e-6
+
+        # The lift is re-applied on top of an explicit initial_pose (which the base class would
+        # otherwise overwrite): the requested z plus the stand-height offset.
+        posed_emb = DroidAbsoluteJointPositionEmbodiment(stand_height=_CUSTOM_STAND_HEIGHT)
+        posed_emb.set_initial_pose(Pose(position_xyz=(0.3, 0.0, 0.5), rotation_xyzw=(0.0, 0.0, 0.0, 1.0)))
+        scene_cfg = posed_emb.get_scene_cfg()
+        assert abs(scene_cfg.robot.init_state.pos[2] - (0.5 + expected_offset)) < 1e-6
+        assert abs(scene_cfg.stand.init_state.pos[2] - (0.5 + expected_offset)) < 1e-6
+
+        # The YAML-spec path instantiates the embodiment via asset_class(**params); a scalar
+        # stand_height from YAML applies just the same.
+        registry_emb = AssetRegistry().get_asset_by_name("droid_abs_joint_pos")(stand_height=_CUSTOM_STAND_HEIGHT)
+        assert tuple(registry_emb.scene_config.stand.spawn.scale) == _EXPECTED_CUSTOM_SCALE
 
     except Exception as e:
         print(f"Error: {e}")

isaaclab_arena_environments/robolab/bagel_plate_banana_bowl_linked.yaml

@@ -8,7 +8,9 @@ embodiment:
   id: droid
   registry_name: droid_abs_joint_pos
   params:
-    stand_scale: [1.2, 1.2, 1.7]
+    # Height (z-axis scale factor) of the robot base stand. The robot base is lifted to match so the
+    # stand stays on the floor; footprint and robot size are unaffected.
+    stand_height: 1.7
 background:
   id: maple_table_robolab
   registry_name: maple_table_robolab