diff --git a/isaaclab_arena/assets/background_library.py b/isaaclab_arena/assets/background_library.py index 379567fb0..10be7d43b 100644 --- a/isaaclab_arena/assets/background_library.py +++ b/isaaclab_arena/assets/background_library.py @@ -138,6 +138,22 @@ def __init__(self): super().__init__() +@register_asset +class OfficeTableBackground(LibraryBackground): + """ + A basic office table. + """ + + name = "office_table_background" + tags = ["background"] + usd_path = f"{ISAACLAB_NUCLEUS_DIR}/Mimic/nut_pour_task/nut_pour_assets/table.usd" + object_min_z = -0.05 + scale = (1.0, 1.0, 0.7) + + def __init__(self): + super().__init__(scale=self.scale) + + @register_asset class LightwheelKitchenBackground(LibraryBackground): """ diff --git a/isaaclab_arena/environments/arena_env_builder.py b/isaaclab_arena/environments/arena_env_builder.py index 3b1e6b5d3..d78066e67 100644 --- a/isaaclab_arena/environments/arena_env_builder.py +++ b/isaaclab_arena/environments/arena_env_builder.py @@ -88,12 +88,20 @@ def _solve_relations(self) -> None: placer_params = ObjectPlacerParams( placement_seed=self.args.placement_seed, apply_positions_to_objects=False, - solver_params=RelationSolverParams(save_position_history=False, verbose=False), + solver_params=RelationSolverParams( + save_position_history=False, + verbose=False, + no_collision_xy_only=getattr(self.args, "no_collision_xy_only", True), + no_collision_include_anchors=getattr(self.args, "no_collision_include_anchors", False), + ), ) + placement_max_attempts = getattr(self.args, "placement_max_attempts", None) + if placement_max_attempts is not None: + placer_params.max_placement_attempts = placement_max_attempts if cli_resolve is not None: placer_params.resolve_on_reset = cli_resolve - pool_size = num_envs * placer_params.min_unique_layouts_per_env + pool_size = getattr(self.args, "placement_pool_size", num_envs * placer_params.min_unique_layouts_per_env) placement_pool = PooledObjectPlacer( objects=objects_with_relations, @@ -412,7 +420,30 @@ def make_registered_and_return_cfg( ) -> tuple[ManagerBasedEnv, IsaacLabArenaManagerBasedRLEnvCfg]: name, cfg = self.build_registered(env_cfg) env = gym.make(name, cfg=cfg, render_mode=render_mode) + if self.arena_env.force_convex_hull: + _force_convex_hull(env) # ViewportCameraController sets the camera before KitVisualizer.initialize() is called, # so the call is silently ignored. Re-apply here once the visualizers are fully initialized. reapply_viewer_cfg(env) return env, cfg + + +def _force_convex_hull(env: ManagerBasedEnv) -> None: + """Replace ``convexDecomposition`` with ``convexHull`` on all MeshCollision prims. + + ``convexDecomposition`` on raw scanned meshes (e.g. robolab assets) creates + irregular contact surfaces that are unstable in multi-object scenarios. + ``convexHull`` produces a single convex shape that behaves predictably. + """ + from pxr import UsdPhysics + + stage = env.unwrapped.sim.stage + for prim in stage.Traverse(): + if not prim.HasAPI(UsdPhysics.MeshCollisionAPI): + continue + mesh_col = UsdPhysics.MeshCollisionAPI(prim) + approx_attr = mesh_col.GetApproximationAttr() + if not approx_attr or not approx_attr.HasValue(): + continue + if approx_attr.Get() == "convexDecomposition": + approx_attr.Set("convexHull") diff --git a/isaaclab_arena/environments/isaaclab_arena_environment.py b/isaaclab_arena/environments/isaaclab_arena_environment.py index 7f2be18c2..de9ffadfd 100644 --- a/isaaclab_arena/environments/isaaclab_arena_environment.py +++ b/isaaclab_arena/environments/isaaclab_arena_environment.py @@ -31,6 +31,7 @@ def __init__( env_cfg_callback: Callable[IsaacLabArenaManagerBasedRLEnvCfg] | None = None, rl_framework_entry_point: str | None = None, rl_policy_cfg: str | None = None, + force_convex_hull: bool = False, ): """ Args: @@ -49,6 +50,10 @@ def __init__( ``rl_policy_cfg`` is set. rl_policy_cfg: Import path to the RL policy config class, e.g. ``"my_module:RLPolicyCfg"``. + force_convex_hull: If True, replace ``convexDecomposition`` with ``convexHull`` + on all MeshCollision prims after scene creation. Needed for assets with + raw scanned meshes (e.g. robolab objects) that are unstable with + ``convexDecomposition``. """ self.name = name self.scene = scene @@ -57,6 +62,7 @@ def __init__( self.teleop_device = teleop_device self.orchestrator = orchestrator self.env_cfg_callback = env_cfg_callback + self.force_convex_hull = force_convex_hull if (rl_framework_entry_point is None) != (rl_policy_cfg is None): raise ValueError("rl_framework_entry_point and rl_policy_cfg must both be set or both be None.") self.rl_framework_entry_point = rl_framework_entry_point diff --git a/isaaclab_arena/evaluation/object_stability_repro.py b/isaaclab_arena/evaluation/object_stability_repro.py new file mode 100644 index 000000000..ffbffbf93 --- /dev/null +++ b/isaaclab_arena/evaluation/object_stability_repro.py @@ -0,0 +1,338 @@ +# Copyright (c) 2025-2026, The Isaac Lab Arena Project Developers (https://github.com/isaac-sim/IsaacLab-Arena/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Reproduce the object stability issue fixed by the placement MR. + +This script is intentionally small and CLI-oriented so reviewers can compare the +old behavior with the proposed fixes in the same scene: + +* ``--origin_main_solver`` emulates the origin/main solver behavior: 3D + no-collision loss (``xy_only=False``) and anchor/table comparisons. +* The fixed run omits ``--origin_main_solver`` so no-collision is XY-only and + anchors are excluded (``xy_only=True``), then adds ``--force_convex_hull`` to + use simpler collision geometry for fragile scanned objects. + +The baseline command should report ``overall=fell_off``. The fixed command +should report ``overall=stable``. + +Visual baseline repro:: + + /isaac-sim/python.sh isaaclab_arena/evaluation/object_stability_repro.py \ + --viz kit --num_envs 4 --env_spacing 4.0 --seed 123 --placement_seed 123 \ + --all_envs --settle_steps 60 --dwell_steps 3000 --origin_main_solver --xy_only false \ + gr1_table_multi_object_no_collision --embodiment gr1_joint \ + --objects parmesan_cheese_canister_hope_robolab mustard_bottle_hope_robolab \ + bbq_sauce_bottle_hope_robolab milk_carton_hope_robolab + +Visual fixed run:: + + /isaac-sim/python.sh isaaclab_arena/evaluation/object_stability_repro.py \ + --viz kit --num_envs 4 --env_spacing 4.0 --seed 123 --placement_seed 123 \ + --all_envs --settle_steps 60 --dwell_steps 3000 --xy_only true --force_convex_hull \ + gr1_table_multi_object_no_collision --embodiment gr1_joint \ + --objects parmesan_cheese_canister_hope_robolab mustard_bottle_hope_robolab \ + bbq_sauce_bottle_hope_robolab milk_carton_hope_robolab +""" + +from __future__ import annotations + +import argparse +import json +import math +import sys +import torch +from typing import Any + +from isaaclab_arena.cli.isaaclab_arena_cli import get_isaaclab_arena_cli_parser +from isaaclab_arena.utils.isaaclab_utils.simulation_app import SimulationAppContext +from isaaclab_arena.utils.random import set_seed +from isaaclab_arena_environments.cli import get_arena_builder_from_cli, get_isaaclab_arena_environments_cli_parser + + +def _parse_bool(value: str) -> bool: + value_lower = value.lower() + if value_lower in ("1", "true", "yes", "on"): + return True + if value_lower in ("0", "false", "no", "off"): + return False + raise argparse.ArgumentTypeError("Expected one of: true, false, yes, no, 1, 0") + + +def _add_repro_args(parser: argparse.ArgumentParser) -> None: + group = parser.add_argument_group("Object stability repro") + group.add_argument("--env_id", type=int, default=0, help="Environment index to check.") + group.add_argument("--all_envs", action="store_true", default=False, help="Check every environment.") + group.add_argument("--settle_steps", type=int, default=60, help="Zero-action steps before final metrics.") + group.add_argument( + "--dwell_steps", type=int, default=0, help="Extra zero-action steps after metrics for viewer inspection." + ) + group.add_argument( + "--placement_pool_size", + type=int, + default=None, + help="Number of placement layouts to pre-solve. Default is --num_envs for this repro.", + ) + group.add_argument( + "--placement_max_attempts", + type=int, + default=1, + help="Placement attempts per requested layout. Default 1 keeps the visual repro fast.", + ) + group.add_argument("--json", action="store_true", default=False, help="Emit final machine-readable JSON.") + group.add_argument( + "--origin_main_solver", + action="store_true", + default=False, + help=( + "Emulate origin/main no-collision behavior: use 3D overlap loss and include anchor/table objects in" + " no-collision comparisons. If omitted, the proposed solver behavior is used: xy_only=True and" + " anchors excluded." + ), + ) + group.add_argument( + "--xy_only", + type=_parse_bool, + default=None, + metavar="{true,false}", + help=( + "Set no-collision XY-only mode explicitly. Use '--xy_only false' for the origin/main baseline and" + " '--xy_only true' for the proposed tabletop solver behavior. If omitted, this defaults from" + " --origin_main_solver." + ), + ) + group.add_argument( + "--force_convex_hull", + action="store_true", + default=False, + help=( + "Replace convexDecomposition mesh collisions with convexHull after scene creation. This isolates the" + " scanned-object collision mesh fix." + ), + ) + group.add_argument("--first_step_jump_thresh", type=float, default=0.02) + group.add_argument("--z_drop_thresh", type=float, default=0.30) + group.add_argument("--xy_drift_thresh", type=float, default=0.05) + group.add_argument("--tilt_thresh_deg", type=float, default=20.0) + group.add_argument("--vel_thresh_lin", type=float, default=0.05) + group.add_argument("--vel_thresh_ang", type=float, default=0.20) + + +def main() -> int: + parser = get_isaaclab_arena_cli_parser() + _add_repro_args(parser) + args_cli, _ = parser.parse_known_args() + + with SimulationAppContext(args_cli): + parser = get_isaaclab_arena_environments_cli_parser(parser) + args_cli = parser.parse_args() + + # The baseline keeps origin/main semantics. The fixed run treats tabletop + # no-collision as 2D packing and lets On(table) control height. + args_cli.no_collision_xy_only = ( + not args_cli.origin_main_solver if args_cli.xy_only is None else bool(args_cli.xy_only) + ) + args_cli.no_collision_include_anchors = args_cli.origin_main_solver + args_cli.resolve_on_reset = False + if args_cli.placement_pool_size is None: + args_cli.placement_pool_size = int(args_cli.num_envs) + + if getattr(args_cli, "seed", None) is not None: + set_seed(args_cli.seed) + + print( + "[stability-repro] building env with placement_pool_size={} placement_max_attempts={}".format( + args_cli.placement_pool_size, + args_cli.placement_max_attempts, + ), + flush=True, + ) + arena_builder = get_arena_builder_from_cli(args_cli) + arena_builder.arena_env.force_convex_hull = bool(args_cli.force_convex_hull) + env, _ = arena_builder.make_registered_and_return_cfg() + if getattr(args_cli, "seed", None) is not None: + set_seed(args_cli.seed, env) + + try: + return _run_check(env, arena_builder.arena_env, args_cli) + finally: + env.close() + + +def _run_check(env, arena_env, args_cli: argparse.Namespace) -> int: + names = _collect_checkable_objects(arena_env) + assert names, "No non-anchor rigid objects found to check." + env_ids = list(range(int(args_cli.num_envs))) if args_cli.all_envs else [int(args_cli.env_id)] + solver_mode = "origin_main_baseline" if args_cli.origin_main_solver else "proposed_xy_only" + print( + "[stability-repro] solver_mode={} no_collision_xy_only={} no_collision_include_anchors={} " + "force_convex_hull={}".format( + solver_mode, + args_cli.no_collision_xy_only, + args_cli.no_collision_include_anchors, + args_cli.force_convex_hull, + ), + flush=True, + ) + print( + "[stability-repro] objects={} envs={}".format( + names, + env_ids, + ), + flush=True, + ) + + # Measure both immediate spawn response and post-settle behavior so the + # output catches contact explosions, tipping, sliding, and falling. + env.reset() + zero_action = torch.zeros(env.action_space.shape, device=env.unwrapped.device) + spawn_poses = _snapshot_poses(env, names, env_ids) + env.step(zero_action) + first_step_poses = _snapshot_poses(env, names, env_ids) + for _ in range(int(args_cli.settle_steps)): + env.step(zero_action) + + thresholds = _thresholds(args_cli) + per_env = { + env_id: _compute_env_metrics(env, names, env_id, spawn_poses[env_id], first_step_poses[env_id], thresholds) + for env_id in env_ids + } + overall = _overall_status(per_env) + + for env_id, metrics_by_name in per_env.items(): + for name, metrics in metrics_by_name.items(): + print( + "[stability-repro] env={} {}: {} | jump1={:.4f}m xy_drift={:.4f}m z_drop={:.4f}m " + "tilt={:.1f}deg |v|={:.4f}m/s |w|={:.4f}rad/s".format( + env_id, + name, + metrics["status"], + metrics["first_step_jump_m"], + metrics["xy_drift_m"], + metrics["z_drop_m"], + math.degrees(metrics["tilt_rad"]), + metrics["lin_vel_norm"], + metrics["ang_vel_norm"], + ), + flush=True, + ) + print(f"[stability-repro] overall={overall}", flush=True) + + if args_cli.json: + print( + json.dumps({ + "overall_status": overall, + "origin_main_solver": args_cli.origin_main_solver, + "force_convex_hull": args_cli.force_convex_hull, + "objects": per_env, + }), + flush=True, + ) + sys.stdout.flush() + + for _ in range(int(args_cli.dwell_steps)): + env.step(zero_action) + return 0 if overall == "stable" else 4 + + +def _collect_checkable_objects(arena_env) -> list[str]: + # Keep pxr-backed imports after SimulationApp starts. Importing them at + # module load time can break Kit extension initialization. + from isaaclab_arena.assets.object_base import ObjectType + from isaaclab_arena.relations.relations import IsAnchor + + names = [] + embodiment_name = getattr(arena_env.embodiment, "name", None) + for asset in arena_env.scene.assets.values(): + if getattr(asset, "object_type", None) != ObjectType.RIGID: + continue + if asset.name == embodiment_name: + continue + if any(isinstance(relation, IsAnchor) for relation in getattr(asset, "get_relations", lambda: [])()): + continue + names.append(asset.name) + return names + + +def _snapshot_poses( + env, names: list[str], env_ids: list[int] +) -> dict[int, dict[str, tuple[torch.Tensor, torch.Tensor]]]: + return {env_id: {name: _get_rigid_pose(env, name, env_id) for name in names} for env_id in env_ids} + + +def _get_rigid_pose(env, name: str, env_id: int) -> tuple[torch.Tensor, torch.Tensor]: + import warp as wp + + obj = env.unwrapped.scene.rigid_objects[name] + return wp.to_torch(obj.data.root_pos_w)[env_id].clone(), wp.to_torch(obj.data.root_quat_w)[env_id].clone() + + +def _get_rigid_velocity(env, name: str, env_id: int) -> tuple[torch.Tensor, torch.Tensor]: + import warp as wp + + obj = env.unwrapped.scene.rigid_objects[name] + return wp.to_torch(obj.data.root_lin_vel_w)[env_id].clone(), wp.to_torch(obj.data.root_ang_vel_w)[env_id].clone() + + +def _compute_env_metrics(env, names: list[str], env_id: int, spawn_poses, first_step_poses, thresholds: dict) -> dict: + metrics_by_name = {} + for name in names: + spawn_pos, spawn_quat = spawn_poses[name] + first_pos, _ = first_step_poses[name] + settled_pos, settled_quat = _get_rigid_pose(env, name, env_id) + lin_vel, ang_vel = _get_rigid_velocity(env, name, env_id) + metrics: dict[str, Any] = { + "first_step_jump_m": float(torch.linalg.norm(first_pos - spawn_pos).item()), + "xy_drift_m": float(torch.linalg.norm(settled_pos[:2] - spawn_pos[:2]).item()), + "z_drop_m": float(max(0.0, (spawn_pos[2] - settled_pos[2]).item())), + "tilt_rad": _tilt_angle_rad(spawn_quat, settled_quat), + "lin_vel_norm": float(torch.linalg.norm(lin_vel).item()), + "ang_vel_norm": float(torch.linalg.norm(ang_vel).item()), + } + metrics["status"] = _classify(metrics, thresholds) + metrics_by_name[name] = metrics + return metrics_by_name + + +def _tilt_angle_rad(quat_init_wxyz: torch.Tensor, quat_now_wxyz: torch.Tensor) -> float: + import isaaclab.utils.math as pose_utils + + z_init = pose_utils.matrix_from_quat(quat_init_wxyz)[:, 2] + z_now = pose_utils.matrix_from_quat(quat_now_wxyz)[:, 2] + return float(torch.acos(torch.clamp(torch.dot(z_init, z_now), -1.0, 1.0)).item()) + + +def _thresholds(args_cli: argparse.Namespace) -> dict: + return { + "first_step_jump_thresh": float(args_cli.first_step_jump_thresh), + "z_drop_thresh": float(args_cli.z_drop_thresh), + "xy_drift_thresh": float(args_cli.xy_drift_thresh), + "tilt_thresh_rad": math.radians(float(args_cli.tilt_thresh_deg)), + "vel_thresh_lin": float(args_cli.vel_thresh_lin), + "vel_thresh_ang": float(args_cli.vel_thresh_ang), + } + + +def _classify(metrics: dict, thresholds: dict) -> str: + if metrics["first_step_jump_m"] > thresholds["first_step_jump_thresh"]: + return "spawn_collision" + if metrics["z_drop_m"] > thresholds["z_drop_thresh"]: + return "fell_off" + if metrics["tilt_rad"] > thresholds["tilt_thresh_rad"]: + return "tipped" + if metrics["xy_drift_m"] > thresholds["xy_drift_thresh"]: + return "slid" + if metrics["lin_vel_norm"] > thresholds["vel_thresh_lin"] or metrics["ang_vel_norm"] > thresholds["vel_thresh_ang"]: + return "unsettled" + return "stable" + + +def _overall_status(per_env: dict[int, dict[str, dict]]) -> str: + priority = ["spawn_collision", "fell_off", "tipped", "slid", "unsettled", "stable"] + statuses = [metrics["status"] for metrics_by_name in per_env.values() for metrics in metrics_by_name.values()] + return min(statuses, key=priority.index) + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/isaaclab_arena/llm_env_gen/STABILITY_FINDINGS.md b/isaaclab_arena/llm_env_gen/STABILITY_FINDINGS.md new file mode 100644 index 000000000..d7bca194e --- /dev/null +++ b/isaaclab_arena/llm_env_gen/STABILITY_FINDINGS.md @@ -0,0 +1,484 @@ +# Robolab Object Stability: `convexDecomposition` vs `convexHull` + +## Problem + +Many robolab assets use `convexDecomposition` as their collision mesh approximation. +On raw scanned meshes this creates irregular contact surfaces that cause objects to +bounce, tip over, or slide — especially in multi-object scenes where objects settle +near each other. + +## Fix + +Replace `convexDecomposition` with `convexHull` on all MeshCollision prims after +scene creation. This is exposed via `IsaacLabArenaEnvironment(force_convex_hull=True)` +and wired through `ArenaEnvBuilder.make_registered_and_return_cfg()`. + +## Reproduce + +All commands run inside the Arena Docker container. + +### 1. Single-object stability check + +```bash +# Check a specific object (without fix — uses convexDecomposition): +/isaac-sim/python.sh isaaclab_arena/llm_env_gen/run_stability_check.py \ + --headless --num_envs 1 --settle_steps 60 \ + gr1_table_multi_object_no_collision --embodiment gr1_joint \ + --objects mustard_bottle_hope_robolab +``` + +### 2. Scan many objects: baseline (no convexHull fix) + +```bash +# Scan all robolab objects (baseline): +/isaac-sim/python.sh isaaclab_arena/llm_env_gen/run_stability_scan.py + +# Scan with convexHull fix enabled: +/isaac-sim/python.sh isaaclab_arena/llm_env_gen/run_stability_scan.py --force_convex_hull + +# Compare both modes side by side: +/isaac-sim/python.sh isaaclab_arena/llm_env_gen/run_stability_scan.py --compare + +# Include multi-object interaction test: +/isaac-sim/python.sh isaaclab_arena/llm_env_gen/run_stability_scan.py --multi +``` + +### 3. Visual inspection (with Kit viewer) + +```bash +/isaac-sim/python.sh isaaclab_arena/llm_env_gen/run_stability_check.py \ + --viz kit --num_envs 1 --settle_steps 60 --dwell_steps 200 \ + gr1_table_multi_object_no_collision --embodiment gr1_joint \ + --objects mustard_bottle_hope_robolab milk_carton_hope_robolab +``` + +--- + +## Results: Single-Object Baseline Scan (origin/main, no convexHull) + +Branch: `origin/main`, 67 robolab objects, each placed alone on table, 1 env. +**57 stable, 8 unstable, 3 errors.** + +### Unstable Objects + +| # | Object | Status | Tilt | Z-drop | XY-drift | Notes | +|---|--------|--------|------|--------|----------|-------| +| 4 | `blue_block_basic_robolab` | **tipped** | 90.3° | 0.020m | 0.040m | A basic block! | +| 11 | `cheez_it_ycb_robolab` | **tipped** | 90.6° | 0.091m | 0.136m | Falls and slides | +| 38 | `orange_juice_carton_hope_robolab` | **tipped** | 91.2° | 0.080m | 0.129m | Falls and slides | +| 49 | `red_onion_fruits_veggies_robolab` | **tipped** | 118.0° | 0.000m | 0.038m | Flips upside down | +| 40 | `peas_and_carrots_hope_robolab` | **tipped** | 20.1° | 0.005m | 0.009m | Borderline | +| 25 | `jello_ycb_robolab` | unsettled | 7.0° | 0.017m | 0.001m | Still vibrating | +| 41 | `pineapple_slices_can_hope_robolab` | unsettled | 0.2° | 0.020m | 0.000m | Still vibrating | +| 58 | `spoon_handal_robolab` | unsettled | 0.9° | 0.020m | 0.000m | Still vibrating | + +### Errors (asset loading issues) + +- `lunchbag_objaverse_robolab` +- `measuring_spoon_handal_robolab` +- `pitted_cherries_hope_robolab` + +### Borderline Objects (stable but high tilt) + +| Object | Tilt | Notes | +|--------|------|-------| +| `chocolate_pudding_ycb_robolab` | 13.2° | | +| `alphabet_soup_can_hope_robolab` | 10.7° | | +| `red_bell_pepper_objaverse_robolab` | 9.9° | Round shape | +| `jello_ycb_robolab` | 7.0° | Flagged unsettled | +| `ladle_handal_robolab` | 6.8° | | +| `coffee_can_ycb_robolab` | 6.5° | | +| `brick_ycb_robolab` | 5.8° | | + +### Full Results Table + +``` + # Object Status Tilt Z-drop XY-drift Jump1 + 1 alphabet_soup_can_hope_robolab stable 10.7° 0.0151m 0.0068m 0.0025m + 2 banana_ycb_robolab stable 0.1° 0.0202m 0.0001m 0.0025m + 3 bbq_sauce_bottle_hope_robolab stable 0.6° 0.0203m 0.0011m 0.0025m + 4 blue_block_basic_robolab tipped 90.3° 0.0200m 0.0400m 0.0032m + 5 bowl_ycb_robolab stable 0.1° 0.0202m 0.0001m 0.0025m + 6 brick_ycb_robolab stable 5.8° 0.0177m 0.0019m 0.0025m + 7 butter_hope_robolab stable 0.3° 0.0197m 0.0003m 0.0025m + 8 canned_mushrooms_hope_robolab stable 0.6° 0.0206m 0.0001m 0.0025m + 9 canned_peaches_hope_robolab stable 0.6° 0.0204m 0.0002m 0.0025m + 10 canned_tuna_hope_robolab stable 0.2° 0.0200m 0.0000m 0.0025m + 11 cheez_it_ycb_robolab tipped 90.6° 0.0914m 0.1363m 0.0025m + 12 chocolate_pudding_mix_hope_robolab stable 1.0° 0.0204m 0.0004m 0.0025m + 13 chocolate_pudding_ycb_robolab stable 13.2° 0.0097m 0.0024m 0.0016m + 14 clamp_ycb_robolab stable 0.5° 0.0206m 0.0003m 0.0025m + 15 coffee_can_ycb_robolab stable 6.5° 0.0152m 0.0077m 0.0025m + 16 cordless_drill_ycb_robolab stable 0.2° 0.0209m 0.0001m 0.0025m + 17 corn_can_hope_robolab stable 0.2° 0.0203m 0.0001m 0.0025m + 18 cream_cheese_hope_robolab stable 1.8° 0.0205m 0.0008m 0.0025m + 19 dry_erase_marker_ycb_robolab stable 4.7° 0.0204m 0.0011m 0.0025m + 20 granola_bars_hope_robolab stable 0.3° 0.0213m 0.0004m 0.0025m + 21 green_beans_can_hope_robolab stable 0.2° 0.0200m 0.0001m 0.0025m + 22 green_block_basic_robolab stable 0.1° 0.0202m 0.0000m 0.0025m + 23 gregorys_coffee_cup_objaverse_robolab stable 0.0° 0.0200m 0.0001m 0.0025m + 24 hammer_handal_robolab stable 0.8° 0.0236m 0.0003m 0.0025m + 25 jello_ycb_robolab unsettled 7.0° 0.0168m 0.0012m 0.0025m + 26 ketchup_bottle_hope_robolab stable 1.3° 0.0207m 0.0023m 0.0025m + 27 ladle_handal_robolab stable 6.8° 0.0385m 0.0024m 0.0025m + 28 lunchbag_objaverse_robolab ERROR + 29 macaroni_and_cheese_hope_robolab stable 0.7° 0.0205m 0.0012m 0.0025m + 30 mayonnaise_bottle_hope_robolab stable 0.7° 0.0202m 0.0012m 0.0025m + 31 measuring_cups_handal_robolab stable 2.1° 0.0196m 0.0007m 0.0025m + 32 measuring_spoon_handal_robolab ERROR + 33 milk_carton_hope_robolab stable 1.2° 0.0211m 0.0020m 0.0025m + 34 mug_ycb_robolab stable 0.2° 0.0102m 0.0001m 0.0025m + 35 mustard_bottle_hope_robolab stable 1.9° 0.0207m 0.0027m 0.0025m + 36 mustard_ycb_robolab stable 0.1° 0.0202m 0.0001m 0.0025m + 37 oatmeal_raisin_cookies_hope_robolab stable 0.9° 0.0205m 0.0014m 0.0025m + 38 orange_juice_carton_hope_robolab tipped 91.2° 0.0800m 0.1290m 0.0009m + 39 parmesan_cheese_canister_hope_robolab stable 0.6° 0.0204m 0.0006m 0.0025m + 40 peas_and_carrots_hope_robolab tipped 20.1° 0.0049m 0.0091m 0.0027m + 41 pineapple_slices_can_hope_robolab unsettled 0.2° 0.0202m 0.0002m 0.0025m + 42 pitcher_ycb_robolab stable 0.3° 0.0203m 0.0005m 0.0025m + 43 pitted_cherries_hope_robolab ERROR + 44 popcorn_box_hope_robolab stable 1.7° 0.0210m 0.0007m 0.0025m + 45 raisin_box_hope_robolab stable 0.2° 0.0203m 0.0006m 0.0025m + 46 ranch_dressing_hope_robolab stable 0.5° 0.0203m 0.0007m 0.0025m + 47 red_bell_pepper_objaverse_robolab stable 9.9° 0.0180m 0.0065m 0.0025m + 48 red_block_basic_robolab stable 0.0° 0.0202m 0.0000m 0.0025m + 49 red_onion_fruits_veggies_robolab tipped 118.0° 0.0000m 0.0383m 0.0025m + 50 salad_tongs_handal_robolab stable 2.8° 0.0274m 0.0004m 0.0025m + 51 scissors_ycb_robolab stable 0.4° 0.0204m 0.0002m 0.0025m + 52 serving_spoon_handal_robolab stable 1.7° 0.0241m 0.0005m 0.0025m + 53 serving_spoons_handal_robolab stable 2.3° 0.0248m 0.0006m 0.0025m + 54 snickers_bar_objaverse_robolab stable 0.5° 0.0200m 0.0001m 0.0025m + 55 soft_scrub_ycb_robolab stable 0.6° 0.0204m 0.0010m 0.0025m + 56 spaghetti_hope_robolab stable 1.7° 0.0204m 0.0006m 0.0025m + 57 spam_can_ycb_robolab stable 0.2° 0.0201m 0.0001m 0.0025m + 58 spoon_handal_robolab unsettled 0.9° 0.0202m 0.0002m 0.0025m + 59 spoon_1_handal_robolab stable 0.6° 0.0197m 0.0002m 0.0025m + 60 spoon_2_handal_robolab stable 0.2° 0.0200m 0.0000m 0.0025m + 61 spring_clamp_ycb_robolab stable 3.7° 0.0204m 0.0007m 0.0025m + 62 sugar_box_ycb_robolab stable 0.3° 0.0201m 0.0005m 0.0025m + 63 tomato_sauce_can_hope_robolab stable 0.3° 0.0201m 0.0003m 0.0025m + 64 tomato_soup_can_ycb_robolab stable 0.2° 0.0203m 0.0002m 0.0025m + 65 tuna_can_ycb_robolab stable 0.2° 0.0201m 0.0000m 0.0025m + 66 wood_block_ycb_robolab stable 0.9° 0.0107m 0.0017m 0.0025m + 67 yellow_block_basic_robolab stable 0.0° 0.0202m 0.0000m 0.0025m + 68 yogurt_cup_hope_robolab stable 0.1° 0.0201m 0.0001m 0.0025m +``` + +--- + +## Important: Single-Object vs Multi-Object Instability + +**Key finding**: most objects that are stable alone become unstable in multi-object scenes. + +In the single-object scan above, `mustard_bottle_hope_robolab` (1.9° tilt) and +`milk_carton_hope_robolab` (1.2° tilt) are both perfectly stable. However, in +multi-object heterogeneous scenes (7 objects, 16 envs, `zxiao/het-placement-clean`): + +| Object | Single (alone) | Multi (7 objects, no fix) | Multi (7 objects, with fix) | +|--------|---------------|--------------------------|---------------------------| +| bottles (mustard/milk/OJ/parmesan) | stable 1-2° | **tipped 90°, fell off** | **stable** 1.2° | +| tools (spoons) | stable 1° | tilt 7.8° | **stable** 1.0° | +| lime | stable | tipped 37° | tipped 22° (round shape) | + +**Root cause**: `convexDecomposition` creates irregular contact patches. When a single +object settles on a flat table, the irregularity doesn't matter much. But when +multiple objects settle near each other, the irregular surfaces cause chain-reaction +bumps — one object's wobble nudges the next, which nudges the next, amplifying +instability. `convexHull` smooths these surfaces and breaks the chain. + +Additional factors in multi-object heterogeneous mode: +- **Non-uniform scaling**: different object variants have different scales, which + distorts the decomposition meshes further +- **Tighter placement**: the solver places objects closer together, increasing + the chance of contact-surface interactions +- **More settling energy**: more objects means more total kinetic energy during + the settling phase + +--- + +## Stability Checker Metrics + +| Metric | Threshold | What it catches | +|--------|-----------|----------------| +| `first_step_jump_m` | > 0.02m | PhysX resolving interpenetration | +| `z_drop_m` | > 0.30m | Object falling off surface | +| `tilt_rad` | > 20° | Object tipping over | +| `xy_drift_m` | > 0.05m | Object sliding on surface | +| `lin_vel_norm` | > 0.05 m/s | Object still bouncing | +| `ang_vel_norm` | > 0.20 rad/s | Object still spinning | + +## Files + +- `run_stability_check.py` — Single-env stability checker (from `xyao/exp/llm_env_gen`) +- `run_stability_scan.py` — Batch scan across many objects (subprocess-based) +- `stability_utils.py` — Shared primitives (classification, pose readout, AABB overlap) +- `STABILITY_FINDINGS.md` — This file + +## Milestone: Solver-Level Root Cause Analysis + +### Single-Object Placement + +With a single object on the table, placement is **safe on `origin/main`** — no solver +fixes needed. Evidence: + +- **57 out of 67** robolab objects are stable (z_drop ~0.02m, tilt <5°). +- The solver computes `NoCollision` between the object and the table anchor, but + with only one object the `On` relation dominates and the small Z push from the + anchor pair settles harmlessly. +- The 8 unstable objects (`blue_block`, `cheez_it`, `orange_juice_carton`, etc.) + are broken due to their **collision mesh geometry** (`convexDecomposition` on + raw scans), not solver placement. These need `convexHull` override. +- Non-uniform scaling (e.g. `bbq_sauce_bottle` at `(0.9, 0.9, 1.4)`) does **not** + cause instability in single-object placement. + +### Multi-Object Placement + +With multiple objects, two solver-level bugs compound to cause instability: + +**Bug 1 — Anchor included in `NoCollision` pairs.** +The solver computes `NoCollision` between every placeable object and the table +anchor (the very surface they must sit on via `On`). The `On` relation pulls +objects down to the table; `NoCollision` pushes them up and outward. These +conflicting forces push objects to table edges and raise them to different +heights. + +**Bug 2 — `NoCollision` operates in 3D (no `xy_only`).** +The `NoCollisionLossStrategy` computes `overlap_x * overlap_y * overlap_z` +(volume). The solver can resolve a collision by pushing objects apart along Z +instead of XY — "stacking" them. This creates objects floating at different +heights above the table, with larger drops and more settling energy. + +Evidence from multi-object test (7 bottles, `origin/main`, no fixes): + +| Object | spawn_z | z_drop | tilt | Status | +|--------|---------|--------|------|--------| +| parmesan_cheese_canister | 0.5925 | 0.011m | 0.6° | stable | +| mustard_bottle | 0.6196 | **0.065m** | **90.1°** | **tipped** | +| bbq_sauce_bottle | **0.6468** | 0.013m | 1.0° | stable | +| ranch_dressing | 0.6294 | **0.412m** | **85.8°** | **fell off** | + +Spawn Z spread = 5.4 cm (0.5925 → 0.6468). The same objects are all stable +when placed alone (spawn_z ~0.6391, z_drop ~0.02m, tilt <2°). + +### Fixed-Layout Prefix Replay + +To separate "the solved layout is bad" from "a later object-object contact starts +a chain reaction", we replayed a solved 6-object layout from `env_id=1` and added +objects back incrementally without re-solving. + +Object order: + +```text +banana_ycb_robolab +lime01_fruits_veggies_robolab +mustard_bottle_hope_robolab +alphabet_soup_can_hope_robolab +spoon_handal_robolab +popcorn_box_hope_robolab +``` + +Replay command: + +```bash +docker exec isaaclab_arena-latest bash -c "cd /workspaces/isaaclab_arena && \ + /isaac-sim/python.sh isaaclab_arena/llm_env_gen/run_fixed_layout_prefix_viz.py \ + --viz kit --seed 123 --num_envs 4 --env_spacing 4.0 \ + --source_env_id 1 --target_env_id 1 --start_count 2 \ + --settle_steps 60 --dwell_steps 500 \ + gr1_table_multi_object_no_collision --embodiment gr1_joint \ + --objects banana_ycb_robolab lime01_fruits_veggies_robolab \ + mustard_bottle_hope_robolab alphabet_soup_can_hope_robolab \ + spoon_handal_robolab popcorn_box_hope_robolab" +``` + +Observed behavior: + +| Active prefix size | Added object | Result | +|--------------------|--------------|--------| +| 2 | `banana_ycb_robolab`, `lime01_fruits_veggies_robolab` | stable | +| 3 | `mustard_bottle_hope_robolab` | stable | +| 4 | `alphabet_soup_can_hope_robolab` | stable | +| 5 | `spoon_handal_robolab` | `mustard_bottle_hope_robolab` falls/tips | +| 6 | `popcorn_box_hope_robolab` | failure remains | + +Measured replay output: + +| Active prefix size | Overall | Key object statuses | +|--------------------|---------|---------------------| +| 2 | stable | banana stable, lime stable | +| 3 | stable | mustard stable (tilt 2.0°, drop 0.011m, xy 0.003m) | +| 4 | stable | alphabet soup stable; mustard still stable | +| 5 | fell_off | mustard fell off (settle: tilt 80.2°, drop 0.580m, xy 4.238m; dwell: tilt 90.0°, drop 0.597m, xy 4.327m); spoon unsettled | +| 6 | fell_off | mustard fell off (dwell: tilt 90.1°, drop 0.597m, xy 0.989m); spoon tipped (tilt 171.6°); popcorn stable | + +Important: this replay helper does **not** enable `force_convex_hull`, and in this +diagnosis branch the solver defaults are `no_collision_xy_only=True` and +`no_collision_include_anchors=False`. Therefore the fixed-layout failure above +is reproduced with the solver-side fixes already enabled, but without the +convex-hull collision-mesh fix. + +This points to an object-object contact/chain-reaction issue in a fixed solved +layout, not just a table USD setup issue. The table may contribute in rare cases, +but the same class of instability survived table swaps; the stronger signal is +the fragile scanned object collision geometry plus the solver's original 3D +no-collision behavior. + +For targeted subset checks, use: + +```bash +docker exec isaaclab_arena-latest bash -c "cd /workspaces/isaaclab_arena && \ + /isaac-sim/python.sh isaaclab_arena/llm_env_gen/run_fixed_layout_subset_viz.py \ + --viz kit --seed 123 --num_envs 4 --env_spacing 4.0 \ + --source_env_id 1 --target_env_id 1 \ + --active_objects mustard_bottle_hope_robolab,spoon_handal_robolab \ + --settle_steps 60 --dwell_steps 500 \ + gr1_table_multi_object_no_collision --embodiment gr1_joint \ + --objects banana_ycb_robolab lime01_fruits_veggies_robolab \ + mustard_bottle_hope_robolab alphabet_soup_can_hope_robolab \ + spoon_handal_robolab popcorn_box_hope_robolab" +``` + +### Required Fixes for Multi-Object + +| Fix | What it does | Impact | +|-----|-------------|--------| +| **Remove anchor from `NoCollision`** | Stop computing overlap between objects and the table surface | Eliminates the conflicting `On` vs `NoCollision` forces that push objects to edges and raise them unevenly | +| **`xy_only=True` in `NoCollisionLossStrategy`** | Only penalize XY overlap, ignore Z | Prevents solver from separating objects vertically; all objects land at consistent height | + +Both fixes are independent and complementary: +- Removing anchor alone still allows Z-stacking between object pairs. +- `xy_only` alone still has the anchor fighting the `On` relation. +- **Both together** give consistent heights and stable placement. + +--- + +## Current Diagnosis Notes for Heterogeneous Placement + +These notes summarize the later diagnosis runs that should be useful when +porting the heterogeneous-placement demo/fixes to a clean branch. + +### Asset Scope + +The fixed-layout replay uses only Robolab assets, but they come from mixed +Robolab subgroups: + +| Object | Robolab subgroup | +|--------|------------------| +| `banana_ycb_robolab` | YCB | +| `lime01_fruits_veggies_robolab` | fruits/veggies | +| `mustard_bottle_hope_robolab` | HOPE | +| `alphabet_soup_can_hope_robolab` | HOPE | +| `spoon_handal_robolab` | handal/tool | +| `popcorn_box_hope_robolab` | HOPE | + +### Table Ablation + +Most table/counter examples in this repo use a background/base asset as the +support surface, then create an `ObjectReference` to the tabletop/countertop +prim. The GR1 no-collision example originally used `office_table` from +`object_library.py`, so we tested the table hypothesis in two ways: + +1. Runtime override: force `office_table` to `ObjectType.BASE`. +2. Code-path alignment: add `office_table_background` in `background_library.py` + and make the GR1 example use that background asset. + +Both tests reproduced the same fixed-layout failure: + +| Prefix | Result | +|--------|--------| +| 2 objects | stable | +| 3 objects, mustard added | mustard stable | +| 4 objects, alphabet soup added | mustard still stable | +| 5 objects, spoon added | mustard tips/falls | +| 6 objects, popcorn added | failure remains | + +Conclusion: table setup may be worth keeping aligned with the repo pattern, but +it does not appear to be the main cause of this failure. + +### Spawn Height / Clearance + +The `On(table)` relation default is `clearance_m=0.01`, so the intended +clearance is 1 cm between the object bottom bbox and the table top. The logged +object root `z` value is not the clearance; it includes the object's local bbox +offset: + +```text +root_z = table_top_z + clearance_m - object_bbox_min_z +``` + +For `mustard_bottle_hope_robolab`, the baseline placement root height is around +`z=0.6207`. In a mustard-only drop-height sweep, the baseline row settled by +about `0.0107m`, matching the 1 cm configured clearance. + +Drop-height sweep command: + +```bash +/isaac-sim/python.sh isaaclab_arena/llm_env_gen/run_drop_height_sweep.py \ + --headless --seed 123 --num_envs 1 --settle_steps 120 \ + --heights_m 0,0.002,0.005,0.01,0.02,0.03,0.04,0.05,0.075,0.10 \ + gr1_table_multi_object_no_collision \ + --embodiment gr1_joint \ + --objects mustard_bottle_hope_robolab +``` + +Observed single-run result: + +| Extra Z above baseline | Status | +|------------------------|--------| +| 0.000m | stable | +| 0.002m | stable | +| 0.005m | stable | +| 0.010m | stable | +| 0.020m | stable | +| 0.030m | fell/tipped | +| 0.040m | fell/tipped | +| 0.050m | stable | +| 0.075m | fell/tipped | +| 0.100m | fell/tipped | + +The result is non-monotonic, so do not claim `0.05m` is generally safe. A +conservative interpretation is that mustard is consistently safe up to about +`0.02m` extra height in this single-object run, but contact behavior becomes +sensitive above that. The fixed-layout replay uses only the baseline 1 cm +clearance, so the multi-object failure is not caused by a large initial drop. + +### Peter's PhysX Parameter Suggestions + +We tested several object-level PhysX overrides with convex hull disabled: + +| Parameter ablation | Result | +|--------------------|--------| +| `max_depenetration_velocity=0.01` | did not stabilize; mustard still tipped/fell | +| `contact_offset=0.001`, `rest_offset=0.0` | worse; larger drift/falls | +| `contact_offset=0.005`, `rest_offset=0.0` | worse; multiple objects fell/slid | +| `solver_position_iteration_count=64` | worse in this replay; more sliding/falling | +| `restitution=0.0` | inconclusive as a direct USD override; sim default already shows restitution 0.0, and runtime material binding did not attach to colliders | + +Conclusion: the easy tuning knobs did not resolve the failure. Contact/rest +offset and high solver iterations can make this fixed layout less stable. + +### Working Interpretation + +The strongest current signal is still object collision geometry / contact +behavior: + +- The failing fixed layout reports `initial_overlaps=[]`. +- Mustard is stable before the spoon is introduced. +- Adding `spoon_handal_robolab` triggers mustard tipping/falling even though the + objects are visually separated at spawn. +- The same failure survives table changes and common PhysX parameter ablations. +- `force_convex_hull` remains the most effective mitigation observed so far for + fragile Robolab scanned objects. + +Open follow-up: rerun the same fixed-layout replay with CPU physics. If the +failure disappears on CPU, this points toward GPU PhysX/contact behavior. If it +persists, the object collision geometry remains the stronger suspect. + +--- + +## Open Questions + +- [ ] Run convexHull scan to see which of the 8 unstable objects get fixed +- [ ] Apply both solver fixes and re-run multi-object test to verify +- [ ] Test with non-uniform scaling in multi-object to isolate that factor +- [ ] Investigate the 3 error objects (lunchbag, measuring_spoon, pitted_cherries) diff --git a/isaaclab_arena/llm_env_gen/__init__.py b/isaaclab_arena/llm_env_gen/__init__.py new file mode 100644 index 000000000..fee3a6a9f --- /dev/null +++ b/isaaclab_arena/llm_env_gen/__init__.py @@ -0,0 +1,4 @@ +# Copyright (c) 2025-2026, The Isaac Lab Arena Project Developers (https://github.com/isaac-sim/IsaacLab-Arena/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 diff --git a/isaaclab_arena/llm_env_gen/run_drop_height_sweep.py b/isaaclab_arena/llm_env_gen/run_drop_height_sweep.py new file mode 100644 index 000000000..b6f770f18 --- /dev/null +++ b/isaaclab_arena/llm_env_gen/run_drop_height_sweep.py @@ -0,0 +1,171 @@ +# Copyright (c) 2026, The Isaac Lab Arena Project Developers (https://github.com/isaac-sim/IsaacLab-Arena/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Sweep initial drop heights for one object on the GR1 table. + +This diagnosis tool fixes an object's XY pose and orientation from the normal +placement result, then repeatedly raises only its Z position by each requested +height and lets physics settle. The expected vertical fall from the raised spawn +height is not counted as falling off; the fall-off check compares the settled +pose against the original table-placement baseline. +""" + +from __future__ import annotations + +import math +import torch + +from isaaclab_arena.cli.isaaclab_arena_cli import get_isaaclab_arena_cli_parser +from isaaclab_arena.llm_env_gen.stability_utils import ( + classify_object, + get_rigid_pose, + get_rigid_velocity, + tilt_angle_rad, +) +from isaaclab_arena.utils.isaaclab_utils.simulation_app import SimulationAppContext, teardown_simulation_app +from isaaclab_arena.utils.random import set_seed +from isaaclab_arena_environments.cli import get_arena_builder_from_cli, get_isaaclab_arena_environments_cli_parser + + +def _add_cli_args(parser) -> None: + group = parser.add_argument_group("Drop Height Sweep") + group.add_argument("--object", type=str, default="mustard_bottle_hope_robolab", help="Object to drop.") + group.add_argument( + "--heights_m", + type=str, + default="0.0,0.005,0.01,0.02,0.03,0.04,0.05,0.075,0.10", + help="Comma-separated initial Z offsets above the baseline placement pose, in meters.", + ) + group.add_argument("--env_id", type=int, default=0, help="Environment index to inspect.") + group.add_argument("--settle_steps", type=int, default=120, help="Steps to run before metric readout.") + group.add_argument("--dwell_steps", type=int, default=0, help="Extra steps after each readout for visual inspection.") + group.add_argument("--tilt_thresh_deg", type=float, default=20.0, help="Stable tilt threshold.") + group.add_argument("--xy_drift_thresh", type=float, default=0.05, help="Stable XY drift threshold in meters.") + group.add_argument( + "--z_below_baseline_thresh", + type=float, + default=0.05, + help="Allowed settled Z below the baseline table-placement pose.", + ) + group.add_argument("--vel_thresh_lin", type=float, default=0.05, help="Stable linear velocity threshold.") + group.add_argument("--vel_thresh_ang", type=float, default=0.20, help="Stable angular velocity threshold.") + + +def main() -> int: + parser = get_isaaclab_arena_cli_parser() + _add_cli_args(parser) + args_cli, _ = parser.parse_known_args() + + with SimulationAppContext(args_cli): + parser = get_isaaclab_arena_environments_cli_parser(parser) + args_cli = parser.parse_args() + + if args_cli.seed is not None: + set_seed(args_cli.seed) + + if not getattr(args_cli, "objects", None): + args_cli.objects = [args_cli.object] + elif args_cli.object not in args_cli.objects: + raise ValueError(f"--object {args_cli.object!r} must be included in --objects {args_cli.objects!r}") + + arena_builder = get_arena_builder_from_cli(args_cli) + env, _ = arena_builder.make_registered_and_return_cfg() + if args_cli.seed is not None: + set_seed(args_cli.seed, env) + + try: + _run_sweep(env, args_cli) + finally: + teardown_simulation_app(suppress_exceptions=False, make_new_stage=True) + env.close() + + return 0 + + +def _run_sweep(env, args_cli) -> None: + object_name = args_cli.object + env_id = int(args_cli.env_id) + assert 0 <= env_id < int(args_cli.num_envs) + + zero_action = torch.zeros(env.action_space.shape, device=env.unwrapped.device) + env_ids = torch.tensor([env_id], device=env.unwrapped.device) + obj = env.unwrapped.scene.rigid_objects[object_name] + + env.reset() + baseline_pos, baseline_quat = get_rigid_pose(env, object_name, env_id) + baseline_pose = torch.cat([baseline_pos, baseline_quat]).clone() + + thresholds = { + "first_step_jump_thresh": 0.02, + "z_drop_thresh": float(args_cli.z_below_baseline_thresh), + "tilt_thresh_rad": math.radians(float(args_cli.tilt_thresh_deg)), + "xy_drift_thresh": float(args_cli.xy_drift_thresh), + "vel_thresh_lin": float(args_cli.vel_thresh_lin), + "vel_thresh_ang": float(args_cli.vel_thresh_ang), + } + + print( + f"[drop-sweep] object={object_name} baseline_xyz=" + f"({baseline_pos[0].item():.4f}, {baseline_pos[1].item():.4f}, {baseline_pos[2].item():.4f})", + flush=True, + ) + print( + f"{'height_m':>8s} {'status':>12s} {'tilt_deg':>9s} {'below_base_m':>12s} " + f"{'xy_m':>8s} {'settle_drop_m':>13s} {'lin_vel':>8s} {'ang_vel':>8s}", + flush=True, + ) + + stable_heights = [] + heights_m = [float(value) for value in args_cli.heights_m.split(",") if value] + for height_m in heights_m: + env.reset() + + spawn_pose = baseline_pose.clone() + spawn_pose[2] += float(height_m) + obj.write_root_pose_to_sim(spawn_pose.unsqueeze(0), env_ids=env_ids) + obj.write_root_velocity_to_sim(torch.zeros(1, 6, device=env.unwrapped.device), env_ids=env_ids) + + spawn_pos, spawn_quat = get_rigid_pose(env, object_name, env_id) + env.step(zero_action) + first_step_pos, _ = get_rigid_pose(env, object_name, env_id) + + for _ in range(int(args_cli.settle_steps)): + env.step(zero_action) + + now_pos, now_quat = get_rigid_pose(env, object_name, env_id) + lin_vel, ang_vel = get_rigid_velocity(env, object_name, env_id) + + metrics = { + "first_step_jump_m": float(torch.linalg.norm(first_step_pos - spawn_pos).item()), + "xy_drift_m": float(torch.linalg.norm((now_pos - baseline_pos)[:2]).item()), + "z_drop_m": float(max(0.0, (baseline_pos[2] - now_pos[2]).item())), + "tilt_rad": tilt_angle_rad(baseline_quat, now_quat), + "lin_vel_norm": float(torch.linalg.norm(lin_vel).item()), + "ang_vel_norm": float(torch.linalg.norm(ang_vel).item()), + "aabb_overlap_with": [], + } + status = classify_object(metrics, thresholds) + if status == "stable": + stable_heights.append(float(height_m)) + + print( + f"{height_m:8.4f} {status:>12s} {math.degrees(metrics['tilt_rad']):9.2f} " + f"{metrics['z_drop_m']:12.4f} {metrics['xy_drift_m']:8.4f} " + f"{max(0.0, (spawn_pos[2] - now_pos[2]).item()):13.4f} " + f"{metrics['lin_vel_norm']:8.4f} {metrics['ang_vel_norm']:8.4f}", + flush=True, + ) + + for _ in range(int(args_cli.dwell_steps)): + env.step(zero_action) + + if stable_heights: + print(f"[drop-sweep] max_stable_height_m={max(stable_heights):.4f}", flush=True) + else: + print("[drop-sweep] max_stable_height_m=None", flush=True) + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/isaaclab_arena/llm_env_gen/run_fixed_layout_prefix_viz.py b/isaaclab_arena/llm_env_gen/run_fixed_layout_prefix_viz.py new file mode 100644 index 000000000..1a583a27f --- /dev/null +++ b/isaaclab_arena/llm_env_gen/run_fixed_layout_prefix_viz.py @@ -0,0 +1,382 @@ +# Copyright (c) 2025-2026, The Isaac Lab Arena Project Developers (https://github.com/isaac-sim/IsaacLab-Arena/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Replay one solved multi-object layout while adding objects incrementally. + +This is a diagnosis tool for separating solver layout quality from object-level +stability. It solves/builds the requested environment once, captures the full +layout from ``--source_env_id``, remaps those relative poses into +``--target_env_id``, and then replays prefixes of the object list without +re-solving. +""" + +from __future__ import annotations + +import math +import sys + +import torch +import warp as wp + +from isaaclab_arena.cli.isaaclab_arena_cli import get_isaaclab_arena_cli_parser +from isaaclab_arena.llm_env_gen.stability_utils import ( + classify_object, + compute_aabb_overlap_pairs, + get_rigid_pose, + get_rigid_velocity, + tilt_angle_rad, +) +from isaaclab_arena.utils.isaaclab_utils.simulation_app import SimulationAppContext, teardown_simulation_app +from isaaclab_arena.utils.random import set_seed +from isaaclab_arena_environments.cli import get_arena_builder_from_cli, get_isaaclab_arena_environments_cli_parser + + +def _add_cli_args(parser, target_env_default: int = 0, include_start_count: bool = True) -> None: + group = parser.add_argument_group("Fixed Layout Prefix Replay") + group.add_argument("--source_env_id", type=int, default=1, help="Env index to capture the solved full layout from.") + group.add_argument( + "--target_env_id", + type=int, + default=target_env_default, + help="Env index to replay the captured layout into.", + ) + if include_start_count: + group.add_argument("--start_count", type=int, default=2, help="First prefix size to replay.") + group.add_argument("--settle_steps", type=int, default=60, help="Steps to run before the first metric readout.") + group.add_argument("--dwell_steps", type=int, default=500, help="Additional steps to run for visual inspection.") + group.add_argument( + "--table_as_base", + action="store_true", + default=False, + help="Spawn office_table as a BASE asset instead of a RIGID object for table-setup ablation.", + ) + group.add_argument( + "--object_max_depenetration_velocity", + type=float, + default=None, + help="Override max_depenetration_velocity for replayed object assets.", + ) + group.add_argument( + "--object_solver_position_iterations", + type=int, + default=None, + help="Override solver_position_iteration_count for replayed object assets.", + ) + group.add_argument( + "--object_contact_offset", + type=float, + default=None, + help="Override contact_offset for replayed object assets.", + ) + group.add_argument( + "--object_rest_offset", + type=float, + default=None, + help="Override rest_offset for replayed object assets.", + ) + group.add_argument( + "--object_restitution", + type=float, + default=None, + help="Override rigid-body material restitution for replayed object assets.", + ) + + +def main() -> int: + parser = get_isaaclab_arena_cli_parser() + _add_cli_args(parser) + args_cli, _ = parser.parse_known_args() + + with SimulationAppContext(args_cli): + parser = get_isaaclab_arena_environments_cli_parser(parser) + args_cli = parser.parse_args() + + if args_cli.seed is not None: + set_seed(args_cli.seed) + + _apply_diagnostic_overrides(args_cli) + arena_builder = get_arena_builder_from_cli(args_cli) + env, _ = arena_builder.make_registered_and_return_cfg() + if args_cli.seed is not None: + set_seed(args_cli.seed, env) + _apply_runtime_restitution_override(env, args_cli) + + try: + _run_replay(env, arena_builder.arena_env, args_cli) + finally: + teardown_simulation_app(suppress_exceptions=False, make_new_stage=True) + env.close() + + return 0 + + +def _apply_diagnostic_overrides(args_cli) -> None: + """Apply temporary asset config overrides for physics ablations. + + These overrides intentionally avoid convex hull. They test whether the fixed + layout failure is sensitive to table spawning or common PhysX contact knobs. + """ + if not any( + [ + args_cli.table_as_base, + args_cli.object_max_depenetration_velocity is not None, + args_cli.object_solver_position_iterations is not None, + args_cli.object_contact_offset is not None, + args_cli.object_rest_offset is not None, + args_cli.object_restitution is not None, + ] + ): + return + + import isaaclab.sim as sim_utils + + from isaaclab_arena.assets.object_base import ObjectType + from isaaclab_arena.assets.registries import AssetRegistry + + registry = AssetRegistry() + + if args_cli.table_as_base: + office_table_cls = registry.get_asset_by_name("office_table") + office_table_cls.object_type = ObjectType.BASE + print("[prefix-viz] override: office_table object_type=BASE", flush=True) + + rigid_kwargs = {} + if args_cli.object_max_depenetration_velocity is not None: + rigid_kwargs["max_depenetration_velocity"] = float(args_cli.object_max_depenetration_velocity) + if args_cli.object_solver_position_iterations is not None: + rigid_kwargs["solver_position_iteration_count"] = int(args_cli.object_solver_position_iterations) + + collision_kwargs = {} + if args_cli.object_contact_offset is not None: + collision_kwargs["contact_offset"] = float(args_cli.object_contact_offset) + if args_cli.object_rest_offset is not None: + collision_kwargs["rest_offset"] = float(args_cli.object_rest_offset) + + for object_name in args_cli.objects: + object_cls = registry.get_asset_by_name(object_name) + spawn_cfg_addon = dict(getattr(object_cls, "spawn_cfg_addon", {}) or {}) + if rigid_kwargs: + spawn_cfg_addon["rigid_props"] = sim_utils.RigidBodyPropertiesCfg(**rigid_kwargs) + if collision_kwargs: + spawn_cfg_addon["collision_props"] = sim_utils.CollisionPropertiesCfg(**collision_kwargs) + object_cls.spawn_cfg_addon = spawn_cfg_addon + + print( + "[prefix-viz] object overrides: rigid={} collision={} restitution={}".format( + rigid_kwargs, + collision_kwargs, + args_cli.object_restitution, + ), + flush=True, + ) + + +def _apply_runtime_restitution_override(env, args_cli) -> None: + if args_cli.object_restitution is None: + return + + import isaaclab.sim as sim_utils + import omni.usd + from pxr import Usd, UsdPhysics + + stage = omni.usd.get_context().get_stage() + material_path = "/World/diagnostic_object_physics_material" + material_cfg = sim_utils.RigidBodyMaterialCfg(restitution=float(args_cli.object_restitution)) + material_cfg.func(material_path, material_cfg) + + bind_count = 0 + for object_name in args_cli.objects: + for env_id in range(int(args_cli.num_envs)): + root_prim = stage.GetPrimAtPath(f"/World/envs/env_{env_id}/{object_name}") + if not root_prim.IsValid(): + continue + for prim in Usd.PrimRange(root_prim): + if prim.HasAPI(UsdPhysics.CollisionAPI): + if sim_utils.bind_physics_material(prim.GetPath(), material_path, stage=stage): + bind_count += 1 + + print( + f"[prefix-viz] runtime restitution override: restitution={args_cli.object_restitution} " + f"material={material_path} bound_colliders={bind_count}", + flush=True, + ) + + +def _run_replay(env, arena_env, args_cli) -> None: + names = list(args_cli.objects) + source_env_id = int(args_cli.source_env_id) + target_env_id = int(args_cli.target_env_id) + num_envs = int(args_cli.num_envs) + assert 0 <= source_env_id < num_envs + assert 0 <= target_env_id < num_envs + assert 1 <= int(args_cli.start_count) <= len(names) + + zero_action = torch.zeros(env.action_space.shape, device=env.unwrapped.device) + env_ids = torch.arange(num_envs, device=env.unwrapped.device) + target_env_ids = torch.tensor([target_env_id], device=env.unwrapped.device) + thresholds = { + "first_step_jump_thresh": 0.02, + "z_drop_thresh": 0.30, + "tilt_thresh_rad": math.radians(20.0), + "xy_drift_thresh": 0.05, + "vel_thresh_lin": 0.05, + "vel_thresh_ang": 0.20, + } + + env.reset() + source_poses = _capture_root_poses(env, names, source_env_id) + replay_poses = _remap_poses_to_target_env(env, names, source_poses, source_env_id, target_env_id) + + print( + f"[prefix-viz] captured full layout from env={source_env_id:02d}; " + f"replaying in env={target_env_id:02d}", + flush=True, + ) + for name in names: + rel = source_poses[name][:3] - env.unwrapped.scene.env_origins[source_env_id] + pos = replay_poses[name][:3] + print( + f"[prefix-viz] {name}: rel=({rel[0].item():.3f}, {rel[1].item():.3f}, {rel[2].item():.3f}) " + f"target=({pos[0].item():.3f}, {pos[1].item():.3f}, {pos[2].item():.3f})", + flush=True, + ) + + for count in range(int(args_cli.start_count), len(names) + 1): + active_names = names[:count] + env.reset() + _write_prefix_layout(env, names, active_names, replay_poses, target_env_id, env_ids, target_env_ids) + + overlap_pairs = compute_aabb_overlap_pairs(env, arena_env, active_names, target_env_id) + overlap_partners = _overlap_partners(active_names, overlap_pairs) + print(f"\n[prefix-viz] active_count={count} active={active_names}", flush=True) + print(f"[prefix-viz] initial_overlaps={overlap_pairs}", flush=True) + + spawn_pose = {name: get_rigid_pose(env, name, target_env_id) for name in active_names} + env.step(zero_action) + first_step_pose = {name: get_rigid_pose(env, name, target_env_id) for name in active_names} + + for _ in range(int(args_cli.settle_steps)): + env.step(zero_action) + _print_metrics("settle", env, active_names, target_env_id, spawn_pose, first_step_pose, overlap_partners, thresholds) + + for _ in range(int(args_cli.dwell_steps)): + env.step(zero_action) + _print_metrics("dwell", env, active_names, target_env_id, spawn_pose, first_step_pose, overlap_partners, thresholds) + + +def _capture_root_poses(env, names: list[str], env_id: int) -> dict[str, torch.Tensor]: + return { + name: wp.to_torch(env.unwrapped.scene.rigid_objects[name].data.root_pose_w)[env_id].clone() + for name in names + } + + +def _remap_poses_to_target_env( + env, + names: list[str], + source_poses: dict[str, torch.Tensor], + source_env_id: int, + target_env_id: int, +) -> dict[str, torch.Tensor]: + origins = env.unwrapped.scene.env_origins + source_origin = origins[source_env_id] + target_origin = origins[target_env_id] + replay_poses = {} + for name in names: + pose = source_poses[name].clone() + relative_xyz = pose[:3] - source_origin + pose[:3] = target_origin + relative_xyz + replay_poses[name] = pose + return replay_poses + + +def _write_prefix_layout( + env, + names: list[str], + active_names: list[str], + replay_poses: dict[str, torch.Tensor], + target_env_id: int, + env_ids: torch.Tensor, + target_env_ids: torch.Tensor, +) -> None: + active = set(active_names) + origins = env.unwrapped.scene.env_origins + for idx, name in enumerate(names): + asset = env.unwrapped.scene.rigid_objects[name] + if name in active: + asset.write_root_pose_to_sim(replay_poses[name].unsqueeze(0), env_ids=target_env_ids) + asset.write_root_velocity_to_sim(torch.zeros(1, 6, device=env.unwrapped.device), env_ids=target_env_ids) + else: + inactive_pose = replay_poses[name].repeat(len(target_env_ids), 1) + inactive_pose[:, :3] = origins[target_env_id] + torch.tensor( + [20.0 + 2.0 * idx, 20.0, 2.0], device=env.unwrapped.device + ) + asset.write_root_pose_to_sim(inactive_pose, env_ids=target_env_ids) + asset.write_root_velocity_to_sim(torch.zeros(1, 6, device=env.unwrapped.device), env_ids=target_env_ids) + + # Move all non-target env copies away to keep the viewer focused on one replay. + non_target_env_ids = env_ids[env_ids != target_env_id] + if len(non_target_env_ids) == 0: + continue + far_pose = replay_poses[name].repeat(len(non_target_env_ids), 1) + far_pose[:, :3] = origins[non_target_env_ids] + torch.tensor( + [20.0 + 2.0 * idx, 20.0, 2.0], device=env.unwrapped.device + ) + asset.write_root_pose_to_sim(far_pose, env_ids=non_target_env_ids) + asset.write_root_velocity_to_sim( + torch.zeros(len(non_target_env_ids), 6, device=env.unwrapped.device), + env_ids=non_target_env_ids, + ) + + +def _overlap_partners(active_names: list[str], overlap_pairs: list[dict]) -> dict[str, list[str]]: + partners = {name: [] for name in active_names} + for pair in overlap_pairs: + partners[pair["a"]].append(pair["b"]) + partners[pair["b"]].append(pair["a"]) + return partners + + +def _print_metrics( + stage: str, + env, + active_names: list[str], + env_id: int, + spawn_pose: dict[str, tuple[torch.Tensor, torch.Tensor]], + first_step_pose: dict[str, tuple[torch.Tensor, torch.Tensor]], + overlap_partners: dict[str, list[str]], + thresholds: dict, +) -> None: + statuses = [] + chunks = [] + for name in active_names: + spawn_pos, spawn_quat = spawn_pose[name] + t1_pos, _ = first_step_pose[name] + now_pos, now_quat = get_rigid_pose(env, name, env_id) + lin_vel, ang_vel = get_rigid_velocity(env, name, env_id) + metrics = { + "first_step_jump_m": float(torch.linalg.norm(t1_pos - spawn_pos).item()), + "xy_drift_m": float(torch.linalg.norm((now_pos - spawn_pos)[:2]).item()), + "z_drop_m": float(max(0.0, (spawn_pos[2] - now_pos[2]).item())), + "tilt_rad": tilt_angle_rad(spawn_quat, now_quat), + "lin_vel_norm": float(torch.linalg.norm(lin_vel).item()), + "ang_vel_norm": float(torch.linalg.norm(ang_vel).item()), + "aabb_overlap_with": overlap_partners.get(name, []), + } + status = classify_object(metrics, thresholds) + statuses.append(status) + chunks.append( + f"{name}={status}(tilt={math.degrees(metrics['tilt_rad']):.1f}," + f"drop={metrics['z_drop_m']:.3f},xy={metrics['xy_drift_m']:.3f}," + f"jump={metrics['first_step_jump_m']:.3f})" + ) + overall = "stable" if all(status == "stable" for status in statuses) else next( + status for status in statuses if status != "stable" + ) + print(f"[prefix-viz] {stage} overall={overall} | " + ", ".join(chunks), flush=True) + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/isaaclab_arena/llm_env_gen/run_fixed_layout_subset_viz.py b/isaaclab_arena/llm_env_gen/run_fixed_layout_subset_viz.py new file mode 100644 index 000000000..e3516531a --- /dev/null +++ b/isaaclab_arena/llm_env_gen/run_fixed_layout_subset_viz.py @@ -0,0 +1,184 @@ +# Copyright (c) 2025-2026, The Isaac Lab Arena Project Developers (https://github.com/isaac-sim/IsaacLab-Arena/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Replay an exact subset from one solved multi-object layout. + +This complements ``run_fixed_layout_prefix_viz.py``. The prefix tool is useful +for adding objects in order; this tool is useful for targeted checks such as +``spoon`` alone at the pose it had in the failing full env1 layout. +""" + +from __future__ import annotations + +import math +import sys + +import torch + +from isaaclab_arena.cli.isaaclab_arena_cli import get_isaaclab_arena_cli_parser +from isaaclab_arena.llm_env_gen.run_fixed_layout_prefix_viz import ( + _add_cli_args as _add_prefix_diagnostic_args, + _apply_diagnostic_overrides, + _capture_root_poses, + _overlap_partners, + _remap_poses_to_target_env, + _write_prefix_layout, +) +from isaaclab_arena.llm_env_gen.stability_utils import ( + classify_object, + compute_aabb_overlap_pairs, + get_rigid_pose, + get_rigid_velocity, + tilt_angle_rad, +) +from isaaclab_arena.utils.isaaclab_utils.simulation_app import SimulationAppContext, teardown_simulation_app +from isaaclab_arena.utils.random import set_seed +from isaaclab_arena_environments.cli import get_arena_builder_from_cli, get_isaaclab_arena_environments_cli_parser + + +def _add_cli_args(parser) -> None: + _add_prefix_diagnostic_args(parser, target_env_default=1, include_start_count=False) + group = parser.add_argument_group("Fixed Layout Subset Replay") + group.add_argument( + "--active_objects", + type=str, + default=None, + help="Comma-separated object subset to replay at the positions captured from the full layout.", + ) + group.add_argument("--settle_steps", type=int, default=60, help="Steps to run before the first metric readout.") + group.add_argument("--dwell_steps", type=int, default=500, help="Additional steps to run for visual inspection.") + + +def main() -> int: + parser = get_isaaclab_arena_cli_parser() + _add_cli_args(parser) + args_cli, _ = parser.parse_known_args() + + with SimulationAppContext(args_cli): + parser = get_isaaclab_arena_environments_cli_parser(parser) + args_cli = parser.parse_args() + + if args_cli.seed is not None: + set_seed(args_cli.seed) + + _apply_diagnostic_overrides(args_cli) + arena_builder = get_arena_builder_from_cli(args_cli) + env, _ = arena_builder.make_registered_and_return_cfg() + if args_cli.seed is not None: + set_seed(args_cli.seed, env) + + try: + _run_replay(env, arena_builder.arena_env, args_cli) + finally: + teardown_simulation_app(suppress_exceptions=False, make_new_stage=True) + env.close() + + return 0 + + +def _run_replay(env, arena_env, args_cli) -> None: + names = list(args_cli.objects) + assert args_cli.active_objects, "--active_objects must be provided before the environment name." + active_names = [name.strip() for name in args_cli.active_objects.split(",") if name.strip()] + unknown = sorted(set(active_names) - set(names)) + assert not unknown, f"--active_objects contains objects not present in --objects: {unknown}" + + source_env_id = int(args_cli.source_env_id) + target_env_id = int(args_cli.target_env_id) + num_envs = int(args_cli.num_envs) + assert 0 <= source_env_id < num_envs + assert 0 <= target_env_id < num_envs + + zero_action = torch.zeros(env.action_space.shape, device=env.unwrapped.device) + env_ids = torch.arange(num_envs, device=env.unwrapped.device) + target_env_ids = torch.tensor([target_env_id], device=env.unwrapped.device) + thresholds = { + "first_step_jump_thresh": 0.02, + "z_drop_thresh": 0.30, + "tilt_thresh_rad": math.radians(20.0), + "xy_drift_thresh": 0.05, + "vel_thresh_lin": 0.05, + "vel_thresh_ang": 0.20, + } + + env.reset() + source_poses = _capture_root_poses(env, names, source_env_id) + replay_poses = _remap_poses_to_target_env(env, names, source_poses, source_env_id, target_env_id) + + print( + f"[subset-viz] captured full layout from env={source_env_id:02d}; " + f"replaying active={active_names} in env={target_env_id:02d}", + flush=True, + ) + for name in active_names: + rel = source_poses[name][:3] - env.unwrapped.scene.env_origins[source_env_id] + pos = replay_poses[name][:3] + print( + f"[subset-viz] {name}: rel=({rel[0].item():.3f}, {rel[1].item():.3f}, {rel[2].item():.3f}) " + f"target=({pos[0].item():.3f}, {pos[1].item():.3f}, {pos[2].item():.3f})", + flush=True, + ) + + env.reset() + _write_prefix_layout(env, names, active_names, replay_poses, target_env_id, env_ids, target_env_ids) + + overlap_pairs = compute_aabb_overlap_pairs(env, arena_env, active_names, target_env_id) + overlap_partners = _overlap_partners(active_names, overlap_pairs) + print(f"[subset-viz] initial_overlaps={overlap_pairs}", flush=True) + + spawn_pose = {name: get_rigid_pose(env, name, target_env_id) for name in active_names} + env.step(zero_action) + first_step_pose = {name: get_rigid_pose(env, name, target_env_id) for name in active_names} + + for _ in range(int(args_cli.settle_steps)): + env.step(zero_action) + _print_metrics("settle", env, active_names, target_env_id, spawn_pose, first_step_pose, overlap_partners, thresholds) + + for _ in range(int(args_cli.dwell_steps)): + env.step(zero_action) + _print_metrics("dwell", env, active_names, target_env_id, spawn_pose, first_step_pose, overlap_partners, thresholds) + + +def _print_metrics( + stage: str, + env, + active_names: list[str], + env_id: int, + spawn_pose: dict[str, tuple[torch.Tensor, torch.Tensor]], + first_step_pose: dict[str, tuple[torch.Tensor, torch.Tensor]], + overlap_partners: dict[str, list[str]], + thresholds: dict, +) -> None: + statuses = [] + chunks = [] + for name in active_names: + spawn_pos, spawn_quat = spawn_pose[name] + t1_pos, _ = first_step_pose[name] + now_pos, now_quat = get_rigid_pose(env, name, env_id) + lin_vel, ang_vel = get_rigid_velocity(env, name, env_id) + metrics = { + "first_step_jump_m": float(torch.linalg.norm(t1_pos - spawn_pos).item()), + "xy_drift_m": float(torch.linalg.norm((now_pos - spawn_pos)[:2]).item()), + "z_drop_m": float(max(0.0, (spawn_pos[2] - now_pos[2]).item())), + "tilt_rad": tilt_angle_rad(spawn_quat, now_quat), + "lin_vel_norm": float(torch.linalg.norm(lin_vel).item()), + "ang_vel_norm": float(torch.linalg.norm(ang_vel).item()), + "aabb_overlap_with": overlap_partners.get(name, []), + } + status = classify_object(metrics, thresholds) + statuses.append(status) + chunks.append( + f"{name}={status}(tilt={math.degrees(metrics['tilt_rad']):.1f}," + f"drop={metrics['z_drop_m']:.3f},xy={metrics['xy_drift_m']:.3f}," + f"jump={metrics['first_step_jump_m']:.3f})" + ) + overall = "stable" if all(status == "stable" for status in statuses) else next( + status for status in statuses if status != "stable" + ) + print(f"[subset-viz] {stage} overall={overall} | " + ", ".join(chunks), flush=True) + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/isaaclab_arena/llm_env_gen/run_stability_check.py b/isaaclab_arena/llm_env_gen/run_stability_check.py new file mode 100644 index 000000000..fb091f22d --- /dev/null +++ b/isaaclab_arena/llm_env_gen/run_stability_check.py @@ -0,0 +1,269 @@ +# Copyright (c) 2025-2026, The Isaac Lab Arena Project Developers (https://github.com/isaac-sim/IsaacLab-Arena/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Driver: bring up an Arena env, settle physics, classify each rigid +object as stable / fell_off / tipped / slid / unsettled / spawn_collision. + +All primitives live in +:mod:`isaaclab_arena.llm_env_gen.stability_utils`; this module owns only +the SimulationApp lifecycle, env bring-up, settle loop, optional Kit +viewport capture, and the JSON / exit-code surface. + +Examples (inside the Arena container):: + + /isaac-sim/python.sh isaaclab_arena/llm_env_gen/run_stability_check.py \\ + --viz kit --num_envs 1 avocadoPnPbowltable + + /isaac-sim/python.sh isaaclab_arena/llm_env_gen/run_stability_check.py \\ + --viz kit --num_envs 1 --object avocado avocadoPnPbowltable + +Exit codes: + +* 0 — every checked object is ``stable`` +* 4 — at least one object is unstable (fell_off / tipped / slid / unsettled) +* 5 — at least one object had a spawn-time collision (AABB overlap or + first-step pose jump above threshold) + +When both 4 and 5 apply, 5 wins — collisions are the upstream cause. +""" + +from __future__ import annotations + +import contextlib +import json +import os +import sys +import torch + +from isaaclab_arena.cli.isaaclab_arena_cli import get_isaaclab_arena_cli_parser +from isaaclab_arena.llm_env_gen.stability_utils import ( + STABILITY_STATUS_SPAWN_COLLISION, + STABILITY_STATUS_STABLE, + add_stability_cli_args, + classify_object, + collect_checkable_objects, + compute_aabb_overlap_pairs, + format_metrics_line, + get_rigid_pose, + get_rigid_velocity, + thresholds_from_args, + tilt_angle_rad, +) +from isaaclab_arena.utils.isaaclab_utils.simulation_app import SimulationAppContext, teardown_simulation_app +from isaaclab_arena.utils.random import set_seed +from isaaclab_arena_environments.cli import get_arena_builder_from_cli, get_isaaclab_arena_environments_cli_parser + + +def main() -> int: + parser = get_isaaclab_arena_cli_parser() + add_stability_cli_args(parser) + args_cli, _ = parser.parse_known_args() + + with SimulationAppContext(args_cli): + # Env subparsers register after SimApp boots so they can introspect + # the registry for env-specific flags (e.g. --object on the env). + parser = get_isaaclab_arena_environments_cli_parser(parser) + args_cli = parser.parse_args() + + arena_builder = get_arena_builder_from_cli(args_cli) + if os.environ.get("ARENA_FORCE_CONVEX_HULL") == "1": + arena_builder.arena_env.force_convex_hull = True + return check_stability_for_arena_builder(arena_builder, args_cli) + + +def check_stability_for_arena_builder(arena_builder, args_cli) -> int: + """Run the stability check against an already-built ``ArenaEnvBuilder``. + + Caller owns the SimulationApp lifecycle. The env is built, exercised, + and closed inside this call so the caller can rebuild a fresh + ``arena_builder`` (with a different placement seed) and call again + without re-booting Isaac Sim. + + Returns: + * 0 — every object is stable + * 4 — at least one object unstable (no spawn collision) + * 5 — at least one object had a spawn collision (precedence over 4) + """ + env_name = getattr(arena_builder.arena_env, "name", type(arena_builder.arena_env).__name__) + save_render_dir = getattr(args_cli, "save_render_dir", None) + + only_name = getattr(args_cli, "stability_object", None) + names = collect_checkable_objects(arena_builder.arena_env, only_name=only_name) + print( + f"[stability] Env: '{env_name}' | checking {len(names)} object(s): {names}", + flush=True, + ) + assert names, f"[stability] No checkable rigid objects found in env '{env_name}'." + + env, _ = arena_builder.make_registered_and_return_cfg() + if args_cli.seed is not None: + set_seed(args_cli.seed, env) + + env_id = int(args_cli.env_id) + num_envs = int(args_cli.num_envs) + assert 0 <= env_id < num_envs, f"--env_id {env_id} out of range for --num_envs {num_envs}" + + try: + env.reset() + + # ---- 1) Initial-state pairwise AABB overlap ----------------------- + # Compute *before* stepping. After PhysX kicks in it shoves + # interpenetrating bodies apart and the overlap signal is gone. + overlap_pairs = compute_aabb_overlap_pairs(env, arena_builder.arena_env, names, env_id) + overlap_partners: dict[str, list[str]] = {n: [] for n in names} + for pair in overlap_pairs: + overlap_partners[pair["a"]].append(pair["b"]) + overlap_partners[pair["b"]].append(pair["a"]) + if overlap_pairs: + print("[stability] Initial AABB overlaps detected:", flush=True) + for pair in overlap_pairs: + ox, oy, oz = pair["overlap_xyz"] + print( + f"[stability] {pair['a']} <-> {pair['b']}: overlap=({ox:.4f}, {oy:.4f}, {oz:.4f}) m", + flush=True, + ) + else: + print("[stability] No initial AABB overlaps.", flush=True) + + # ---- 2) Snapshot spawn pose --------------------------------------- + zero = torch.zeros(env.action_space.shape, device=env.unwrapped.device) + spawn_pose: dict[str, tuple[torch.Tensor, torch.Tensor]] = {n: get_rigid_pose(env, n, env_id) for n in names} + + # ---- 3) One physics step → first-step jump ------------------------ + env.step(zero) + first_step_pose: dict[str, tuple[torch.Tensor, torch.Tensor]] = { + n: get_rigid_pose(env, n, env_id) for n in names + } + + # ---- 4) Settle loop ----------------------------------------------- + for _ in range(int(args_cli.settle_steps)): + env.step(zero) + + # ---- 5) Final readout & per-object classification ----------------- + thresholds = thresholds_from_args(args_cli) + per_object: dict[str, dict] = {} + for name in names: + spawn_pos, spawn_quat = spawn_pose[name] + t1_pos, _ = first_step_pose[name] + now_pos, now_quat = get_rigid_pose(env, name, env_id) + lin_vel, ang_vel = get_rigid_velocity(env, name, env_id) + + xy_drift = float(torch.linalg.norm((now_pos - spawn_pos)[:2]).item()) + z_drop = float(max(0.0, (spawn_pos[2] - now_pos[2]).item())) + first_step_jump = float(torch.linalg.norm(t1_pos - spawn_pos).item()) + tilt = tilt_angle_rad(spawn_quat, now_quat) + lin_norm = float(torch.linalg.norm(lin_vel).item()) + ang_norm = float(torch.linalg.norm(ang_vel).item()) + + metrics = { + "first_step_jump_m": first_step_jump, + "xy_drift_m": xy_drift, + "z_drop_m": z_drop, + "tilt_rad": tilt, + "lin_vel_norm": lin_norm, + "ang_vel_norm": ang_norm, + "aabb_overlap_with": overlap_partners[name], + "spawn_xyz": [float(v.item()) for v in spawn_pos], + "settled_xyz": [float(v.item()) for v in now_pos], + } + status = classify_object(metrics, thresholds) + metrics["status"] = status + per_object[name] = metrics + print(format_metrics_line(name, metrics, status), flush=True) + + # ---- 6) Overall verdict + exit code ------------------------------- + statuses = [m["status"] for m in per_object.values()] + if STABILITY_STATUS_SPAWN_COLLISION in statuses: + overall = STABILITY_STATUS_SPAWN_COLLISION + exit_code = 5 + elif all(s == STABILITY_STATUS_STABLE for s in statuses): + overall = STABILITY_STATUS_STABLE + exit_code = 0 + else: + # Pick the worst non-collision status as the headline label. + overall = next(s for s in statuses if s != STABILITY_STATUS_STABLE) + exit_code = 4 + print(f"[stability] Overall: {overall}", flush=True) + + # ---- 7) Optional viewport dwell + capture ------------------------- + if not getattr(args_cli, "headless", False): + for _ in range(int(args_cli.dwell_steps)): + env.step(zero) + + if save_render_dir: + _save_scene_render(env, save_render_dir, env_name, overall) + + if args_cli.json: + payload = { + "env_name": env_name, + "overall_status": overall, + "objects": per_object, + "aabb_overlap_pairs": overlap_pairs, + } + print(json.dumps(payload), flush=True) + sys.stdout.flush() + + return exit_code + + finally: + # Mirror run_reachability_check.py: tear down the SimulationContext + # and open a fresh USD stage *before* env.close() so a subsequent + # gym.make() in the same process gets a clean stage. + teardown_simulation_app(suppress_exceptions=False, make_new_stage=True) + env.close() + + +def _save_scene_render(env, save_render_dir: str, env_name: str, overall_status: str) -> None: + """Capture the active Kit viewport and save it under ``save_render_dir``. + + Naming: ``_stability_.png``. Uses + ``omni.kit.viewport.utility.capture_viewport_to_file`` so it works + without ``--enable_cameras``. Headless runs (no viewport) skip with + a log line. + """ + from pathlib import Path + + out_dir = Path(save_render_dir) + out_dir.mkdir(parents=True, exist_ok=True) + out_path = out_dir / f"{env_name}_stability_{overall_status}.png" + + try: + from omni.kit.viewport.utility import capture_viewport_to_file, get_active_viewport + except ImportError as exc: + print(f"[stability] viewport capture unavailable ({exc!r}); skipping render save.", flush=True) + return + + viewport = get_active_viewport() + if viewport is None: + print( + "[stability] no active Kit viewport (running headless?); skipping render save.", + flush=True, + ) + return + + capture_viewport_to_file(viewport, file_path=str(out_path)) + # Capture is async; pump the app's frame loop until the PNG lands so + # teardown right after this call doesn't destroy the viewport mid-write. + with contextlib.suppress(ImportError): + import omni.kit.app + + app = omni.kit.app.get_app() + for _ in range(60): + app.update() + if out_path.exists(): + break + + if out_path.exists(): + print(f"[stability] Saved viewport render: {out_path}", flush=True) + else: + print( + f"[stability] viewport capture queued for {out_path} but did not land within 60 frames; " + "subsequent app.update() ticks should still flush it.", + flush=True, + ) + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/isaaclab_arena/llm_env_gen/run_stability_scan.py b/isaaclab_arena/llm_env_gen/run_stability_scan.py new file mode 100644 index 000000000..c64b0c02f --- /dev/null +++ b/isaaclab_arena/llm_env_gen/run_stability_scan.py @@ -0,0 +1,312 @@ +# Copyright (c) 2026, The Isaac Lab Arena Project Developers (https://github.com/isaac-sim/IsaacLab-Arena/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Scan robolab objects for stability under natural placement (On table, solver-placed). + +Each object is tested in its own subprocess to avoid SimApp reuse crashes. +Runs ``run_stability_check.py --json`` per object, parses the JSON output, +and prints a comparison table. + +Usage (inside container):: + + # Scan all robolab objects (no convexHull fix — baseline): + /isaac-sim/python.sh isaaclab_arena/llm_env_gen/run_stability_scan.py + + # Scan with convexHull fix enabled: + /isaac-sim/python.sh isaaclab_arena/llm_env_gen/run_stability_scan.py --force_convex_hull + + # Compare both modes side by side: + /isaac-sim/python.sh isaaclab_arena/llm_env_gen/run_stability_scan.py --compare + + # Test specific objects only: + /isaac-sim/python.sh isaaclab_arena/llm_env_gen/run_stability_scan.py \ + --objects mustard_bottle_hope_robolab milk_carton_hope_robolab + + # Multi-object scene (4 objects together): + /isaac-sim/python.sh isaaclab_arena/llm_env_gen/run_stability_scan.py --multi +""" + +from __future__ import annotations + +import json +import math +import os +import subprocess + +PYTHON = os.environ.get("ISAAC_SIM_PYTHON", "/isaac-sim/python.sh") +CHECKER = "isaaclab_arena/llm_env_gen/run_stability_check.py" + +ROBOLAB_OBJECTS = [ + "alphabet_soup_can_hope_robolab", + "banana_ycb_robolab", + "bbq_sauce_bottle_hope_robolab", + "blue_block_basic_robolab", + "bowl_ycb_robolab", + "brick_ycb_robolab", + "butter_hope_robolab", + "canned_mushrooms_hope_robolab", + "canned_peaches_hope_robolab", + "canned_tuna_hope_robolab", + "cheez_it_ycb_robolab", + "chocolate_pudding_mix_hope_robolab", + "chocolate_pudding_ycb_robolab", + "clamp_ycb_robolab", + "coffee_can_ycb_robolab", + "cordless_drill_ycb_robolab", + "corn_can_hope_robolab", + "cream_cheese_hope_robolab", + "dry_erase_marker_ycb_robolab", + "granola_bars_hope_robolab", + "green_beans_can_hope_robolab", + "green_block_basic_robolab", + "gregorys_coffee_cup_objaverse_robolab", + "hammer_handal_robolab", + "jello_ycb_robolab", + "ketchup_bottle_hope_robolab", + "ladle_handal_robolab", + "lunchbag_objaverse_robolab", + "macaroni_and_cheese_hope_robolab", + "mayonnaise_bottle_hope_robolab", + "measuring_cups_handal_robolab", + "measuring_spoon_handal_robolab", + "milk_carton_hope_robolab", + "mug_ycb_robolab", + "mustard_bottle_hope_robolab", + "mustard_ycb_robolab", + "oatmeal_raisin_cookies_hope_robolab", + "orange_juice_carton_hope_robolab", + "parmesan_cheese_canister_hope_robolab", + "peas_and_carrots_hope_robolab", + "pineapple_slices_can_hope_robolab", + "pitcher_ycb_robolab", + "pitted_cherries_hope_robolab", + "popcorn_box_hope_robolab", + "raisin_box_hope_robolab", + "ranch_dressing_hope_robolab", + "red_bell_pepper_objaverse_robolab", + "red_block_basic_robolab", + "red_onion_fruits_veggies_robolab", + "salad_tongs_handal_robolab", + "scissors_ycb_robolab", + "serving_spoon_handal_robolab", + "serving_spoons_handal_robolab", + "snickers_bar_objaverse_robolab", + "soft_scrub_ycb_robolab", + "spaghetti_hope_robolab", + "spam_can_ycb_robolab", + "spoon_handal_robolab", + "spoon_1_handal_robolab", + "spoon_2_handal_robolab", + "spring_clamp_ycb_robolab", + "sugar_box_ycb_robolab", + "tomato_sauce_can_hope_robolab", + "tomato_soup_can_ycb_robolab", + "tuna_can_ycb_robolab", + "wood_block_ycb_robolab", + "yellow_block_basic_robolab", + "yogurt_cup_hope_robolab", +] + +MULTI_OBJECT_SET = [ + "mustard_bottle_hope_robolab", + "milk_carton_hope_robolab", + "alphabet_soup_can_hope_robolab", + "sugar_box_ycb_robolab", +] + + +def run_check(object_names: list[str], force_convex_hull: bool = False, timeout: int = 120) -> dict | None: + """Run stability check in a subprocess and return parsed JSON result.""" + cmd = [ + PYTHON, + CHECKER, + "--headless", + "--json", + "--num_envs", + "1", + "--settle_steps", + "60", + "gr1_table_multi_object_no_collision", + "--embodiment", + "gr1_joint", + "--objects", + ] + object_names + + env = os.environ.copy() + if force_convex_hull: + env["ARENA_FORCE_CONVEX_HULL"] = "1" + + try: + result = subprocess.run( + cmd, + capture_output=True, + text=True, + timeout=timeout, + env=env, + start_new_session=True, + ) + except subprocess.TimeoutExpired: + return None + + if result.returncode not in (0, 4, 5): + return None + + for line in result.stdout.strip().split("\n"): + line = line.strip() + if line.startswith("{"): + try: + return json.loads(line) + except json.JSONDecodeError: + continue + return None + + +def extract_metrics(data: dict, name: str) -> dict | None: + if data is None: + return None + objects = data.get("objects", {}) + return objects.get(name) + + +def fmt_status(metrics: dict | None) -> tuple[str, float]: + if metrics is None: + return "ERROR", 0.0 + return metrics["status"], math.degrees(metrics["tilt_rad"]) + + +def main(): + import argparse + + parser = argparse.ArgumentParser(description="Scan robolab objects for stability") + parser.add_argument( + "--objects", nargs="*", type=str, default=None, help="Specific objects to test. Default: all robolab objects." + ) + parser.add_argument( + "--force_convex_hull", action="store_true", default=False, help="Enable convexHull override for all tests." + ) + parser.add_argument( + "--compare", + action="store_true", + default=False, + help="Run both with and without convexHull and show side-by-side comparison.", + ) + parser.add_argument("--multi", action="store_true", default=False, help="Also test a multi-object scene.") + parser.add_argument("--timeout", type=int, default=120, help="Per-object subprocess timeout in seconds.") + args = parser.parse_args() + + object_list = args.objects if args.objects else ROBOLAB_OBJECTS + total = len(object_list) + + if args.compare: + print(f"\n{'=' * 100}") + print(f"{'ROBOLAB STABILITY SCAN — convexDecomposition vs convexHull':^100}") + print(f"{'=' * 100}") + print( + f"{'#':>3} {'Object':<45s} {'Decomp Status':>14s} {'Tilt':>6s} {'Hull Status':>14s} {'Tilt':>6s} {'Effect':>10s}" + ) + print(f"{'-' * 100}") + + changed = [] + for i, name in enumerate(object_list): + decomp_data = run_check([name], force_convex_hull=False, timeout=args.timeout) + hull_data = run_check([name], force_convex_hull=True, timeout=args.timeout) + + d_status, d_tilt = fmt_status(extract_metrics(decomp_data, name)) + h_status, h_tilt = fmt_status(extract_metrics(hull_data, name)) + + effect = "" + if d_status != h_status and h_status == "stable": + effect = "FIXED" + changed.append(name) + elif d_status != h_status: + effect = "CHANGED" + changed.append(name) + elif d_tilt > 5 and h_tilt < d_tilt * 0.7: + effect = "improved" + + print( + f"{i + 1:>3} {name:<45s} {d_status:>14s} {d_tilt:5.1f}° {h_status:>14s} {h_tilt:5.1f}° {effect:>10s}", + flush=True, + ) + + print(f"\n{'=' * 100}") + print(f"SUMMARY: {len(changed)}/{total} objects changed with convexHull") + if changed: + for n in changed: + print(f" - {n}") + print(f"{'=' * 100}") + + else: + hull = args.force_convex_hull + mode_label = "convexHull" if hull else "convexDecomposition (baseline)" + print(f"\n{'=' * 90}") + print(f"{'ROBOLAB STABILITY SCAN — ' + mode_label:^90}") + print(f"{'=' * 90}") + print(f"{'#':>3} {'Object':<45s} {'Status':>14s} {'Tilt':>7s} {'Z-drop':>8s} {'XY-drift':>9s} {'Jump1':>7s}") + print(f"{'-' * 90}") + + unstable = [] + errors = [] + for i, name in enumerate(object_list): + data = run_check([name], force_convex_hull=hull, timeout=args.timeout) + m = extract_metrics(data, name) + + if m is None: + print(f"{i + 1:>3} {name:<45s} {'ERROR':>14s}", flush=True) + errors.append(name) + continue + + status = m["status"] + tilt = math.degrees(m["tilt_rad"]) + z_drop = m["z_drop_m"] + xy_drift = m["xy_drift_m"] + jump1 = m["first_step_jump_m"] + + marker = " ***" if status != "stable" else "" + print( + f"{i + 1:>3} {name:<45s} {status:>14s} {tilt:5.1f}° {z_drop:7.4f}m {xy_drift:8.4f}m" + f" {jump1:6.4f}m{marker}", + flush=True, + ) + + if status != "stable": + unstable.append((name, status, tilt)) + + print(f"\n{'=' * 90}") + print(f"RESULTS: {total - len(unstable) - len(errors)} stable, {len(unstable)} unstable, {len(errors)} errors") + if unstable: + print("\nUnstable objects:") + for name, status, tilt in unstable: + print(f" {name:<45s} {status:>14s} (tilt={tilt:.1f}°)") + if errors: + print(f"\nErrors: {', '.join(errors)}") + print(f"{'=' * 90}") + + if args.multi: + print(f"\n{'=' * 90}") + print(f"{'MULTI-OBJECT SCENE':^90}") + print(f"Objects: {', '.join(MULTI_OBJECT_SET)}") + print(f"{'=' * 90}") + + for hull, label in [(False, "convexDecomposition"), (True, "convexHull")]: + data = run_check(MULTI_OBJECT_SET, force_convex_hull=hull, timeout=args.timeout) + print(f"\n {label}:") + if data is None: + print(" ERROR: subprocess failed") + continue + for name in MULTI_OBJECT_SET: + m = extract_metrics(data, name) + if m is None: + print(f" {name:<40s} ERROR") + continue + tilt = math.degrees(m["tilt_rad"]) + print( + f" {name:<40s} {m['status']:>12s} " + f"tilt={tilt:5.1f}° z_drop={m['z_drop_m']:.3f}m xy_drift={m['xy_drift_m']:.3f}m" + ) + + +if __name__ == "__main__": + main() diff --git a/isaaclab_arena/llm_env_gen/scan_robolab_stability.py b/isaaclab_arena/llm_env_gen/scan_robolab_stability.py new file mode 100644 index 000000000..b90f7db83 --- /dev/null +++ b/isaaclab_arena/llm_env_gen/scan_robolab_stability.py @@ -0,0 +1,212 @@ +# Copyright (c) 2026, The Isaac Lab Arena Project Developers (https://github.com/isaac-sim/IsaacLab-Arena/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Scan all robolab objects for stability with and without convexHull override. + +Boots the GR1 table environment once per object (homogeneous mode, 1 env), +runs the stability check, and prints a summary table. + +Usage (inside container): + /isaac-sim/python.sh isaaclab_arena/llm_env_gen/scan_robolab_stability.py --headless +""" + +from __future__ import annotations + +import torch + +from isaaclab_arena.cli.isaaclab_arena_cli import get_isaaclab_arena_cli_parser +from isaaclab_arena.llm_env_gen.stability_utils import ( + add_stability_cli_args, + classify_object, + get_rigid_pose, + get_rigid_velocity, + thresholds_from_args, + tilt_angle_rad, +) +from isaaclab_arena.utils.isaaclab_utils.simulation_app import SimulationAppContext, teardown_simulation_app + +ROBOLAB_OBJECTS = [ + "alphabet_soup_can_hope_robolab", + "banana_ycb_robolab", + "bbq_sauce_bottle_hope_robolab", + "blue_block_basic_robolab", + "bowl_ycb_robolab", + "brick_ycb_robolab", + "butter_hope_robolab", + "canned_mushrooms_hope_robolab", + "canned_peaches_hope_robolab", + "canned_tuna_hope_robolab", + "cheez_it_ycb_robolab", + "chocolate_pudding_mix_hope_robolab", + "chocolate_pudding_ycb_robolab", + "clamp_ycb_robolab", + "coffee_can_ycb_robolab", + "cordless_drill_ycb_robolab", + "corn_can_hope_robolab", + "cream_cheese_hope_robolab", + "dry_erase_marker_ycb_robolab", + "granola_bars_hope_robolab", + "green_beans_can_hope_robolab", + "green_block_basic_robolab", + "gregorys_coffee_cup_objaverse_robolab", + "hammer_handal_robolab", + "jello_ycb_robolab", + "ketchup_bottle_hope_robolab", + "ladle_handal_robolab", + "lunchbag_objaverse_robolab", + "macaroni_and_cheese_hope_robolab", + "mayonnaise_bottle_hope_robolab", + "measuring_cups_handal_robolab", + "measuring_spoon_handal_robolab", + "milk_carton_hope_robolab", + "mug_ycb_robolab", + "mustard_bottle_hope_robolab", + "mustard_ycb_robolab", + "oatmeal_raisin_cookies_hope_robolab", + "orange_juice_carton_hope_robolab", + "parmesan_cheese_canister_hope_robolab", + "peas_and_carrots_hope_robolab", + "pineapple_slices_can_hope_robolab", + "pitcher_ycb_robolab", + "pitted_cherries_hope_robolab", + "popcorn_box_hope_robolab", + "raisin_box_hope_robolab", + "ranch_dressing_hope_robolab", + "red_bell_pepper_objaverse_robolab", + "red_block_basic_robolab", + "red_onion_fruits_veggies_robolab", + "salad_tongs_handal_robolab", + "scissors_ycb_robolab", + "serving_spoon_handal_robolab", + "serving_spoons_handal_robolab", + "snickers_bar_objaverse_robolab", + "soft_scrub_ycb_robolab", + "spaghetti_hope_robolab", + "spam_can_ycb_robolab", + "spoon_handal_robolab", + "spoon_1_handal_robolab", + "spoon_2_handal_robolab", + "spring_clamp_ycb_robolab", + "sugar_box_ycb_robolab", + "tomato_sauce_can_hope_robolab", + "tomato_soup_can_ycb_robolab", + "tuna_can_ycb_robolab", + "wood_block_ycb_robolab", + "yellow_block_basic_robolab", + "yogurt_cup_hope_robolab", +] + + +def run_one_object(args_cli, object_name: str, force_convex_hull: bool) -> dict: + """Boot the GR1 env with a single object and return stability metrics.""" + from isaaclab_arena_environments.cli import get_arena_builder_from_cli, get_isaaclab_arena_environments_cli_parser + + parser = get_isaaclab_arena_cli_parser() + add_stability_cli_args(parser) + parser = get_isaaclab_arena_environments_cli_parser(parser) + + test_args = parser.parse_args([ + "--headless", + "--num_envs", + "1", + "--settle_steps", + "60", + "gr1_table_multi_object_no_collision", + "--embodiment", + "gr1_joint", + "--mode", + "homogeneous", + "--objects", + object_name, + ]) + + # Override force_convex_hull + arena_builder = get_arena_builder_from_cli(test_args) + arena_builder.arena_env.force_convex_hull = force_convex_hull + + env, _ = arena_builder.make_registered_and_return_cfg() + + try: + env.reset() + + zero = torch.zeros(env.action_space.shape, device=env.unwrapped.device) + spawn_pos, spawn_quat = get_rigid_pose(env, object_name, 0) + + env.step(zero) + t1_pos, _ = get_rigid_pose(env, object_name, 0) + + for _ in range(60): + env.step(zero) + + now_pos, now_quat = get_rigid_pose(env, object_name, 0) + lin_vel, ang_vel = get_rigid_velocity(env, object_name, 0) + + thresholds = thresholds_from_args(test_args) + metrics = { + "first_step_jump_m": float(torch.linalg.norm(t1_pos - spawn_pos).item()), + "xy_drift_m": float(torch.linalg.norm((now_pos - spawn_pos)[:2]).item()), + "z_drop_m": float(max(0.0, (spawn_pos[2] - now_pos[2]).item())), + "tilt_rad": tilt_angle_rad(spawn_quat, now_quat), + "lin_vel_norm": float(torch.linalg.norm(lin_vel).item()), + "ang_vel_norm": float(torch.linalg.norm(ang_vel).item()), + "aabb_overlap_with": [], + } + metrics["status"] = classify_object(metrics, thresholds) + return metrics + finally: + teardown_simulation_app(suppress_exceptions=False, make_new_stage=True) + env.close() + + +def main(): + parser = get_isaaclab_arena_cli_parser() + add_stability_cli_args(parser) + args_cli, _ = parser.parse_known_args() + + results = [] + + with SimulationAppContext(args_cli): + for obj_name in ROBOLAB_OBJECTS: + row = {"name": obj_name} + for hull_mode, label in [(False, "decomp"), (True, "hull")]: + try: + metrics = run_one_object(args_cli, obj_name, force_convex_hull=hull_mode) + row[f"status_{label}"] = metrics["status"] + row[f"tilt_{label}"] = f"{metrics['tilt_rad'] * 57.2958:.1f}" + row[f"z_drop_{label}"] = f"{metrics['z_drop_m']:.4f}" + row[f"xy_drift_{label}"] = f"{metrics['xy_drift_m']:.4f}" + except Exception as e: + row[f"status_{label}"] = f"ERROR: {e}" + row[f"tilt_{label}"] = "N/A" + row[f"z_drop_{label}"] = "N/A" + row[f"xy_drift_{label}"] = "N/A" + + changed = row.get("status_decomp") != row.get("status_hull") + marker = " *** CHANGED ***" if changed else "" + print( + f"{obj_name:<50s} decomp={row.get('status_decomp', '?'):>16s} " + f"(tilt={row.get('tilt_decomp', '?'):>6s}°) | " + f"hull={row.get('status_hull', '?'):>16s} " + f"(tilt={row.get('tilt_hull', '?'):>6s}°){marker}", + flush=True, + ) + results.append(row) + + print("\n\n=== SUMMARY: Objects where convexHull changes stability ===") + for r in results: + if r.get("status_decomp") != r.get("status_hull"): + print(f" {r['name']:<50s} {r.get('status_decomp', '?'):>16s} -> {r.get('status_hull', '?'):>16s}") + + print("\n=== SUMMARY: Objects unstable even WITH convexHull ===") + for r in results: + if r.get("status_hull") not in ("stable", None) and not str(r.get("status_hull", "")).startswith("ERROR"): + print( + f" {r['name']:<50s} hull={r.get('status_hull', '?'):>16s} " + f"(tilt={r.get('tilt_hull', '?')}°, z_drop={r.get('z_drop_hull', '?')}m)" + ) + + +if __name__ == "__main__": + main() diff --git a/isaaclab_arena/llm_env_gen/stability_utils.py b/isaaclab_arena/llm_env_gen/stability_utils.py new file mode 100644 index 000000000..a7fe7d47b --- /dev/null +++ b/isaaclab_arena/llm_env_gen/stability_utils.py @@ -0,0 +1,410 @@ +# Copyright (c) 2025-2026, The Isaac Lab Arena Project Developers (https://github.com/isaac-sim/IsaacLab-Arena/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Physics-stability primitives shared by the stability-check driver. + +Pure functions and constants only — no CLI entrypoint and no +:class:`SimulationAppContext` lifecycle. The companion driver +:mod:`isaaclab_arena.llm_env_gen.run_stability_check` composes these +primitives into the full env-bring-up → settle → classify flow. + +Scope: any Arena env. Iterates :pyattr:`scene.rigid_objects` and skips +anchor-tagged references and the robot. Measures, per object: + +* spawn-time pairwise AABB overlap (geometric, no physics needed) +* first-step pose jump (1 physics step — catches PhysX-resolved + interpenetration) +* settle-time XY drift, Z drop, linear / angular velocity, and tilt + angle from the initial up-axis + +These are the same kinds of failure modes an LLM-generated env can +introduce: an item placed inside another, a tippy object, or a pose that +slides off the table once gravity engages. +""" + +from __future__ import annotations + +import argparse +import math +import torch + +# --------------------------------------------------------------------------- +# Status constants (stable strings — safe to emit in JSON payloads) +# --------------------------------------------------------------------------- + +STABILITY_STATUS_STABLE = "stable" +STABILITY_STATUS_SPAWN_COLLISION = "spawn_collision" +STABILITY_STATUS_FELL_OFF = "fell_off" +STABILITY_STATUS_TIPPED = "tipped" +STABILITY_STATUS_SLID = "slid" +STABILITY_STATUS_UNSETTLED = "unsettled" + + +# --------------------------------------------------------------------------- +# CLI argument registration +# --------------------------------------------------------------------------- + + +def add_stability_cli_args(parser: argparse.ArgumentParser) -> None: + """Register stability-check CLI arguments on the given parser. + + Call from the driver before :func:`SimulationAppContext` is entered + so ``--help`` works without Isaac Sim booted. + """ + group = parser.add_argument_group( + "Physics Stability Check", + "Arguments for the physics-stability checker.", + ) + group.add_argument( + "--env_id", + type=int, + default=0, + help="Environment index to check (0 to --num_envs-1).", + ) + group.add_argument( + "--object", + dest="stability_object", + type=str, + default=None, + metavar="NAME", + help=( + "Optional: only check the named rigid object instead of every " + "non-anchor rigid in the scene. Use the asset name as it appears " + "in env.unwrapped.scene.rigid_objects." + ), + ) + group.add_argument( + "--settle_steps", + type=int, + default=60, + help=( + "Number of zero-action env.step() calls used to let physics settle " + "after the initial spawn jump. Default 60 (~2s at 30 Hz)." + ), + ) + group.add_argument( + "--vel_thresh_lin", + type=float, + default=0.05, + help="Linear-velocity norm threshold (m/s) at end-of-settle. Default 0.05.", + ) + group.add_argument( + "--vel_thresh_ang", + type=float, + default=0.20, + help="Angular-velocity norm threshold (rad/s) at end-of-settle. Default 0.20.", + ) + group.add_argument( + "--xy_drift_thresh", + type=float, + default=0.05, + help=( + "XY position drift threshold (m) between spawn and settled state. " + "Above this an object is flagged as 'slid'. Default 0.05." + ), + ) + group.add_argument( + "--z_drop_thresh", + type=float, + default=0.30, + help=( + "Z drop threshold (m) — settled Z below spawn Z by more than this is " + "flagged as 'fell_off'. Default 0.30 — comfortably larger than any " + "settling drop (an asset-stability probe spawns 0.10 m above the " + "tabletop and settles within ~0.10–0.15 m), but well below a real " + "table-edge fall (~0.7 m). Tighten with a smaller value if your env " + "expects assets to land already in contact with the surface." + ), + ) + group.add_argument( + "--tilt_thresh_deg", + type=float, + default=20.0, + help=( + "Tilt threshold (degrees) — angle between initial and settled body " + "Z-axes. Above this the object is flagged as 'tipped'. Default 20." + ), + ) + group.add_argument( + "--first_step_jump_thresh", + type=float, + default=0.02, + help=( + "First-step pose jump threshold (m). After one physics step, an " + "object whose center moved more than this is flagged as a " + "spawn collision. Default 0.02 (a settling drop on contact is " + "smaller; PhysX shoving an interpenetration apart is larger)." + ), + ) + group.add_argument( + "--dwell_steps", + type=int, + default=90, + help=( + "Zero-action sim steps to take after classification so the Kit " + "viewer stays up for inspection. Only runs when a viewer is open. " + "Default 90." + ), + ) + group.add_argument( + "--save_render_dir", + type=str, + default=None, + metavar="DIR", + help=( + "If set, capture the active Kit viewport after the stability check " + "and save it as '_stability_.png'. Requires " + "'--viz kit'; headless runs are skipped." + ), + ) + group.add_argument( + "--json", + action="store_true", + default=False, + help="Emit a final single-line JSON payload to stdout with per-object metrics.", + ) + + +# --------------------------------------------------------------------------- +# Object discovery — pure walk over arena_env.scene +# --------------------------------------------------------------------------- + + +def collect_checkable_objects(arena_env, only_name: str | None = None) -> list[str]: + """Return the names of rigid objects worth checking for stability. + + Iterates the arena scene's assets and returns those that: + + * are tagged ``ObjectType.RIGID`` + * are not the robot embodiment + * carry no ``IsAnchor`` relation (anchors reference background sub-prims + and are static — checking them is meaningless) + + If ``only_name`` is given, the result is filtered to that single name + (asserts the name is in the candidate set). + """ + from isaaclab_arena.assets.object_base import ObjectType + from isaaclab_arena.relations.relations import IsAnchor + + embodiment_name = getattr(arena_env.embodiment, "name", None) + + names: list[str] = [] + for asset in arena_env.scene.assets.values(): + # Filter to assets that expose object_type — backgrounds, lights, + # ground planes, and the embodiment do not. + object_type = getattr(asset, "object_type", None) + if object_type != ObjectType.RIGID: + continue + # Skip the embodiment if it ever gets RIGID-tagged. + if asset.name == embodiment_name: + continue + # Skip anchor references (tabletop_anchor and friends — static). + relations = getattr(asset, "get_relations", lambda: [])() + if any(isinstance(r, IsAnchor) for r in relations): + continue + names.append(asset.name) + + if only_name is not None: + assert only_name in names, f"--object {only_name!r} not in checkable rigid objects: {names}" + return [only_name] + return names + + +# --------------------------------------------------------------------------- +# Per-object world-frame readouts +# --------------------------------------------------------------------------- + + +def get_rigid_pose(env, name: str, env_id: int) -> tuple[torch.Tensor, torch.Tensor]: + """Return ``(pos_xyz, quat_wxyz)`` of a rigid object in world coordinates. + + Both tensors are 1-D (``(3,)`` and ``(4,)``) on the env's compute device. + """ + import warp as wp + + rigid_objects = env.unwrapped.scene.rigid_objects + assert ( + name in rigid_objects + ), f"Object '{name}' not found in scene.rigid_objects. Available: {list(rigid_objects.keys())}" + obj = rigid_objects[name] + pos = wp.to_torch(obj.data.root_pos_w)[env_id].clone() + quat = wp.to_torch(obj.data.root_quat_w)[env_id].clone() + return pos, quat + + +def get_rigid_velocity(env, name: str, env_id: int) -> tuple[torch.Tensor, torch.Tensor]: + """Return ``(lin_vel_w, ang_vel_w)`` of a rigid object in world coordinates.""" + import warp as wp + + rigid_objects = env.unwrapped.scene.rigid_objects + assert ( + name in rigid_objects + ), f"Object '{name}' not found in scene.rigid_objects. Available: {list(rigid_objects.keys())}" + obj = rigid_objects[name] + lin = wp.to_torch(obj.data.root_lin_vel_w)[env_id].clone() + ang = wp.to_torch(obj.data.root_ang_vel_w)[env_id].clone() + return lin, ang + + +# --------------------------------------------------------------------------- +# Initial-state pairwise AABB overlap (geometric, no physics step needed) +# --------------------------------------------------------------------------- + + +def _world_aabb_from_local( + local_min_xyz: torch.Tensor, + local_max_xyz: torch.Tensor, + pos_w: torch.Tensor, + quat_wxyz: torch.Tensor, +) -> tuple[torch.Tensor, torch.Tensor]: + """World axis-aligned envelope of an oriented body-frame box. + + Builds the 8 local corners, rotates them by ``quat_wxyz``, translates by + ``pos_w``, and takes the envelope min/max. For yaw-only orientations + (the common case after On(table) resolution) this is exact; for + arbitrary tilts it's a conservative over-approximation, which can + produce false-positive overlap reports — but never false negatives. + """ + import isaaclab.utils.math as PoseUtils + + device = pos_w.device + dtype = pos_w.dtype + lo = local_min_xyz.to(device=device, dtype=dtype) + hi = local_max_xyz.to(device=device, dtype=dtype) + corners = torch.tensor( + [ + [lo[0], lo[1], lo[2]], + [lo[0], lo[1], hi[2]], + [lo[0], hi[1], lo[2]], + [lo[0], hi[1], hi[2]], + [hi[0], lo[1], lo[2]], + [hi[0], lo[1], hi[2]], + [hi[0], hi[1], lo[2]], + [hi[0], hi[1], hi[2]], + ], + device=device, + dtype=dtype, + ) + R = PoseUtils.matrix_from_quat(quat_wxyz.to(device=device, dtype=dtype)) + world_corners = corners @ R.T + pos_w + return world_corners.amin(dim=0), world_corners.amax(dim=0) + + +def compute_aabb_overlap_pairs(env, arena_env, names: list[str], env_id: int) -> list[dict]: + """Return a list of pairwise world-AABB overlaps among ``names`` at current state. + + Each entry: ``{"a": name_a, "b": name_b, "overlap_xyz": (dx, dy, dz)}`` + where each component is the linear overlap on that axis (always > 0 + for entries returned). Pairs with non-positive overlap on any axis + are omitted. + + Uses each object's *local* bbox (``asset.get_bounding_box()``) + combined with the live ``root_pos_w`` / ``root_quat_w`` from the + spawned rigid body — :pymeth:`Asset.get_world_bounding_box()` returns + the local box for objects whose ``initial_pose`` is unset (e.g. + placed by an ``On(table)`` resolver), so it can't be used directly. + """ + asset_by_name = {a.name: a for a in arena_env.scene.assets.values() if a.name in names} + bboxes: dict[str, tuple[torch.Tensor, torch.Tensor]] = {} + for name in names: + asset = asset_by_name[name] + local_bbox = asset.get_bounding_box() + local_min = local_bbox.min_point[0].clone() + local_max = local_bbox.max_point[0].clone() + pos_w, quat_w = get_rigid_pose(env, name, env_id) + bboxes[name] = _world_aabb_from_local(local_min, local_max, pos_w, quat_w) + + out: list[dict] = [] + sorted_names = sorted(bboxes.keys()) + for i, a in enumerate(sorted_names): + a_min, a_max = bboxes[a] + for b in sorted_names[i + 1 :]: + b_min, b_max = bboxes[b] + ox = float((torch.min(a_max[0], b_max[0]) - torch.max(a_min[0], b_min[0])).item()) + oy = float((torch.min(a_max[1], b_max[1]) - torch.max(a_min[1], b_min[1])).item()) + oz = float((torch.min(a_max[2], b_max[2]) - torch.max(a_min[2], b_min[2])).item()) + if ox > 0.0 and oy > 0.0 and oz > 0.0: + out.append({"a": a, "b": b, "overlap_xyz": (ox, oy, oz)}) + return out + + +# --------------------------------------------------------------------------- +# Tilt computation +# --------------------------------------------------------------------------- + + +def tilt_angle_rad(quat_init_wxyz: torch.Tensor, quat_now_wxyz: torch.Tensor) -> float: + """Angle (rad) between the initial and current body Z-axes in world frame. + + A perfectly upright object that hasn't rotated about world Z returns 0. + Pure yaw rotation also returns 0 (the body Z-axis is unchanged), which + is what we want — yaw doesn't affect physical stability. + """ + import isaaclab.utils.math as PoseUtils + + R_init = PoseUtils.matrix_from_quat(quat_init_wxyz) + R_now = PoseUtils.matrix_from_quat(quat_now_wxyz) + z_init = R_init[:, 2] + z_now = R_now[:, 2] + cos_theta = torch.clamp(torch.dot(z_init, z_now), -1.0, 1.0) + return float(torch.acos(cos_theta).item()) + + +# --------------------------------------------------------------------------- +# Classification +# --------------------------------------------------------------------------- + + +def classify_object(metrics: dict, thresholds: dict) -> str: + """Return the stability status string for one object. + + Precedence (worst-cause-first): + 1. ``spawn_collision`` if ``first_step_jump_m`` > thresh OR the object + appears in any AABB overlap pair (caller folds that into metrics). + 2. ``fell_off`` if ``z_drop_m`` > thresh + 3. ``tipped`` if ``tilt_rad`` > tilt_thresh_rad + 4. ``slid`` if ``xy_drift_m`` > thresh + 5. ``unsettled`` if either velocity norm exceeds thresh + 6. ``stable`` otherwise + """ + if metrics.get("aabb_overlap_with"): + return STABILITY_STATUS_SPAWN_COLLISION + if metrics["first_step_jump_m"] > thresholds["first_step_jump_thresh"]: + return STABILITY_STATUS_SPAWN_COLLISION + if metrics["z_drop_m"] > thresholds["z_drop_thresh"]: + return STABILITY_STATUS_FELL_OFF + if metrics["tilt_rad"] > thresholds["tilt_thresh_rad"]: + return STABILITY_STATUS_TIPPED + if metrics["xy_drift_m"] > thresholds["xy_drift_thresh"]: + return STABILITY_STATUS_SLID + if metrics["lin_vel_norm"] > thresholds["vel_thresh_lin"] or metrics["ang_vel_norm"] > thresholds["vel_thresh_ang"]: + return STABILITY_STATUS_UNSETTLED + return STABILITY_STATUS_STABLE + + +def thresholds_from_args(args_cli: argparse.Namespace) -> dict: + """Pack the CLI threshold values into the dict ``classify_object`` expects.""" + return { + "first_step_jump_thresh": float(args_cli.first_step_jump_thresh), + "z_drop_thresh": float(args_cli.z_drop_thresh), + "tilt_thresh_rad": math.radians(float(args_cli.tilt_thresh_deg)), + "xy_drift_thresh": float(args_cli.xy_drift_thresh), + "vel_thresh_lin": float(args_cli.vel_thresh_lin), + "vel_thresh_ang": float(args_cli.vel_thresh_ang), + } + + +def format_metrics_line(name: str, metrics: dict, status: str) -> str: + """One-line human-readable summary suitable for ``print(..., flush=True)``.""" + return ( + f"[stability] {name}: {status} | " + f"jump1={metrics['first_step_jump_m']:.4f}m " + f"xy_drift={metrics['xy_drift_m']:.4f}m " + f"z_drop={metrics['z_drop_m']:.4f}m " + f"tilt={math.degrees(metrics['tilt_rad']):.2f}deg " + f"|v|={metrics['lin_vel_norm']:.4f}m/s " + f"|w|={metrics['ang_vel_norm']:.4f}rad/s" + + (f" | overlaps={metrics['aabb_overlap_with']}" if metrics.get("aabb_overlap_with") else "") + ) diff --git a/isaaclab_arena/relations/relation_loss_strategies.py b/isaaclab_arena/relations/relation_loss_strategies.py index af42a2e98..a66dad8ca 100644 --- a/isaaclab_arena/relations/relation_loss_strategies.py +++ b/isaaclab_arena/relations/relation_loss_strategies.py @@ -332,15 +332,17 @@ class NoCollisionLossStrategy: is a built-in solver behavior, not a user-specified relation. """ - def __init__(self, slope: float = 10.0, debug: bool = False): + def __init__(self, slope: float = 10.0, debug: bool = False, xy_only: bool = False): """ Args: slope: Gradient magnitude for overlap volume loss (default: 10.0). Loss scales with slope times overlap volume. debug: If True, print detailed loss component breakdown. + xy_only: If True, compute overlap only in the XY plane. """ self.slope = slope self.debug = debug + self.xy_only = xy_only def compute_loss( self, @@ -382,8 +384,9 @@ def compute_loss( overlap_y = interval_overlap_axis_loss(child_world_min[:, 1], child_world_max[:, 1], parent_y_min, parent_y_max) overlap_z = interval_overlap_axis_loss(child_world_min[:, 2], child_world_max[:, 2], parent_z_min, parent_z_max) - # 2. Volume loss: slope * product of per-axis overlap lengths (overlap volume when slope 1.0) - overlap_volume = overlap_x * overlap_y * overlap_z + # 2. Overlap loss. For objects already constrained On(table), Z overlap is expected + # and should not push objects upward away from the support surface. + overlap_volume = overlap_x * overlap_y if self.xy_only else overlap_x * overlap_y * overlap_z total_loss = self.slope * overlap_volume if self.debug and child_pos.shape[0] == 1: @@ -402,7 +405,8 @@ def compute_loss( f" {child_world_max[0, 2].item():.4f}], parent_z=[{parent_z_min[0].item():.4f}," f" {parent_z_max[0].item():.4f}])" ) - print(f" [NoCollision] volume={overlap_volume[0].item():.6f}, loss={total_loss[0].item():.6f}") + loss_kind = "xy_area" if self.xy_only else "volume" + print(f" [NoCollision] {loss_kind}={overlap_volume[0].item():.6f}, loss={total_loss[0].item():.6f}") return total_loss.squeeze(0) if single_input else total_loss diff --git a/isaaclab_arena/relations/relation_solver.py b/isaaclab_arena/relations/relation_solver.py index 066dfa069..f100c10d8 100644 --- a/isaaclab_arena/relations/relation_solver.py +++ b/isaaclab_arena/relations/relation_solver.py @@ -41,7 +41,10 @@ def __init__( """ self.params = params or RelationSolverParams() # High slope (vs 10-100 for relation strategies) so overlap avoidance dominates. - self._no_collision_strategy = NoCollisionLossStrategy(slope=10000.0) + self._no_collision_strategy = NoCollisionLossStrategy( + slope=10000.0, + xy_only=self.params.no_collision_xy_only, + ) self._last_loss_history: list[float] = [] self._last_position_history: list = [] self._last_loss_per_env: torch.Tensor | None = None @@ -124,10 +127,9 @@ def _compute_total_loss(self, state: RelationSolverState, debug: bool = False) - return total_loss.mean() def _compute_no_overlap_loss(self, state: RelationSolverState, debug: bool = False) -> torch.Tensor: - """Compute pairwise no-overlap loss for all non-anchor objects against all other objects. + """Compute pairwise no-overlap loss for non-anchor object pairs. Each unique pair is evaluated twice (once per direction): - - Non-anchor vs anchor: gradient flows to the non-anchor only. - Non-anchor vs non-anchor: both objects receive gradient by computing the loss in both directions with the other's position detached. @@ -143,23 +145,22 @@ def _compute_no_overlap_loss(self, state: RelationSolverState, debug: bool = Fal non_anchor_objects = state.optimizable_objects anchor_objects = list(state.anchor_objects) - for i, child in enumerate(non_anchor_objects): child_pos = state.get_position(child) child_bbox = child.get_bounding_box().to(device) - # Against all anchors - for anchor in anchor_objects: - anchor_world_bbox = anchor.get_world_bounding_box().to(device) - loss = self._no_collision_strategy.compute_loss( - clearance_m=self.params.clearance_m, - child_pos=child_pos, - child_bbox=child_bbox, - parent_world_bbox=anchor_world_bbox, - ) - if debug: - print(f" [NoOverlap] {child.name} vs {anchor.name}: loss={loss.mean().item():.6f}") - total_loss = total_loss + loss + if self.params.no_collision_include_anchors: + for anchor in anchor_objects: + anchor_world_bbox = anchor.get_world_bounding_box().to(device) + loss = self._no_collision_strategy.compute_loss( + clearance_m=self.params.clearance_m, + child_pos=child_pos, + child_bbox=child_bbox, + parent_world_bbox=anchor_world_bbox, + ) + if debug: + print(f" [NoOverlap] {child.name} vs {anchor.name}: loss={loss.mean().item():.6f}") + total_loss = total_loss + loss # Against other non-anchors (unique pairs, both directions) for j in range(i + 1, len(non_anchor_objects)): diff --git a/isaaclab_arena/relations/relation_solver_params.py b/isaaclab_arena/relations/relation_solver_params.py index 24a53d079..be91b5f63 100644 --- a/isaaclab_arena/relations/relation_solver_params.py +++ b/isaaclab_arena/relations/relation_solver_params.py @@ -50,6 +50,22 @@ class RelationSolverParams: The solver adds a no-overlap loss for all pairs automatically. Set to 0.0 to only reject actual overlaps (no safety margin).""" + no_collision_xy_only: bool = True + """If True, built-in no-collision only penalizes XY overlap. + + Objects constrained by ``On(table)`` are expected to overlap the support in Z; + allowing the no-collision loss to resolve collisions by moving objects upward + creates vertical stacking and larger physics drops. + """ + + no_collision_include_anchors: bool = False + """If True, built-in no-collision also compares non-anchor objects with anchors. + + This is disabled by default because anchors are often support surfaces used by + ``On`` relations. Including them makes ``On`` pull objects down while + no-collision pushes them up/outward. + """ + # default_factory ensures each instance gets its own dict (mutable defaults are shared across instances) strategies: dict[type[RelationBase], RelationLossStrategy | UnaryRelationLossStrategy] = field( default_factory=_default_strategies diff --git a/isaaclab_arena_environments/gr1_table_multi_object_no_collision_environment.py b/isaaclab_arena_environments/gr1_table_multi_object_no_collision_environment.py index 61d0e1a1b..c65a96182 100644 --- a/isaaclab_arena_environments/gr1_table_multi_object_no_collision_environment.py +++ b/isaaclab_arena_environments/gr1_table_multi_object_no_collision_environment.py @@ -73,13 +73,13 @@ def get_env(self, args_cli: argparse.Namespace) -> IsaacLabArenaEnvironment: ) ground_plane = self.asset_registry.get_asset_by_name("ground_plane")() - table_background = self.asset_registry.get_asset_by_name("office_table")() + table_background = self.asset_registry.get_asset_by_name("office_table_background")() light = self.asset_registry.get_asset_by_name("light")() # Table surface as anchor for On relations tabletop_reference = ObjectReference( name="table", - prim_path="{ENV_REGEX_NS}/office_table/Geometry/sm_tabletop_a01_01/sm_tabletop_a01_top_01", + prim_path="{ENV_REGEX_NS}/office_table_background/Geometry/sm_tabletop_a01_01/sm_tabletop_a01_top_01", parent_asset=table_background, ) tabletop_reference.add_relation(IsAnchor())