diff --git a/isaaclab_arena/environment_spec/arena_env_graph_conversion_utils.py b/isaaclab_arena/environment_spec/arena_env_graph_conversion_utils.py index 56edc58522..24b568f1df 100644 --- a/isaaclab_arena/environment_spec/arena_env_graph_conversion_utils.py +++ b/isaaclab_arena/environment_spec/arena_env_graph_conversion_utils.py @@ -62,6 +62,10 @@ def build_checks_for_placer_params(graph_spec: ArenaEnvGraphSpec) -> ObjectPlace enabled_checks=set(enabled_checks) if enabled_checks is not None else None, required_checks=set(required_checks) if required_checks is not None else None, solver_params=RelationSolverParams(verbose=False, save_position_history=False), + debug_visualize=placement_validators is not None and placement_validators.debug_visualize, + debug_visualize_output_path=( + placement_validators.debug_visualize_output_path if placement_validators is not None else None + ), ) diff --git a/isaaclab_arena/environment_spec/arena_env_graph_types.py b/isaaclab_arena/environment_spec/arena_env_graph_types.py index 595e331217..9cf5882c95 100644 --- a/isaaclab_arena/environment_spec/arena_env_graph_types.py +++ b/isaaclab_arena/environment_spec/arena_env_graph_types.py @@ -307,6 +307,22 @@ class PlacementValidatorSpec(BaseModel): ), ) + debug_visualize: bool = Field( + default=False, + description=( + "Stream every candidate layout the checks evaluate to a spawned Rerun viewer window. Debug " + "aid, off by default; needs a reachable display. The viewer is its own process, so this " + "never starts Isaac Sim, and it closes with the run." + ), + ) + debug_visualize_output_path: str | None = Field( + default=None, + description=( + "Path to record the debug visualization to as a Rerun .rrd file, for headless runs. Enables " + "the visualization on its own; combine with debug_visualize to both record and watch live." + ), + ) + @model_validator(mode="after") def _validate_required_subset(self) -> PlacementValidatorSpec: if self.enabled_checks is not None and self.required_checks is not None: diff --git a/isaaclab_arena/relations/object_placer.py b/isaaclab_arena/relations/object_placer.py index 0035172d78..87dbff216c 100644 --- a/isaaclab_arena/relations/object_placer.py +++ b/isaaclab_arena/relations/object_placer.py @@ -14,6 +14,7 @@ from isaaclab_arena.relations.placement_result import PlacementResult from isaaclab_arena.relations.placement_validation import PlacementValidationResults from isaaclab_arena.relations.placement_validators import build_validators +from isaaclab_arena.relations.placement_visualizer import get_or_create_placement_visualizer from isaaclab_arena.relations.relation_solver import RelationSolver from isaaclab_arena.relations.relations import ( FaceTo, @@ -78,7 +79,8 @@ class ObjectPlacer: def __init__(self, params: ObjectPlacerParams | None = None): self.params = params or ObjectPlacerParams() self._solver = RelationSolver(params=self.params.solver_params) - self._validators: list[PlacementValidator] = build_validators(self.params) + self._visualizer = get_or_create_placement_visualizer(self.params) + self._validators: list[PlacementValidator] = build_validators(self.params, self._visualizer) def place( self, @@ -595,17 +597,23 @@ def _validate_candidates( # required_checks=None means "every enabled check is required"; an empty set means no checks. required = self.params.required_checks num_candidates = len(positions) - # Per-check count of layouts evaluated by that check - num_layouts_evaluated_by_check: dict[str, int] = {} + # Per check, which layouts of this batch (each refill) it actually ran on + evaluated_layout_indices_by_check: dict[str, list[int]] = {} layout_pass_verdicts_by_check: dict[str, list[bool]] = {} + # for debugging visualization tracking which layouts were checked + layout_indices_across_batch: list[int] | None = None + if self._visualizer is not None: + layout_indices_across_batch = self._visualizer.log_layout_batch(positions, orientations, bboxes) + self._run_inexpensive_checks( positions, orientations, bboxes, collision_objects, layout_pass_verdicts_by_check, - num_layouts_evaluated_by_check, + evaluated_layout_indices_by_check, + layout_indices_across_batch, ) self._run_expensive_checks( positions, @@ -614,11 +622,19 @@ def _validate_candidates( collision_objects, required, layout_pass_verdicts_by_check, - num_layouts_evaluated_by_check, + evaluated_layout_indices_by_check, + layout_indices_across_batch, ) + if self._visualizer is not None and layout_indices_across_batch is not None: + self._visualizer.log_layout_batch_verdicts( + layout_indices_across_batch, + layout_pass_verdicts_by_check, + evaluated_layout_indices_by_check, + self.params.required_checks, + ) if layout_pass_verdicts_by_check: summary = ", ".join( - f"{check}={sum(verdicts)}/{num_layouts_evaluated_by_check[check]}" + f"{check}={sum(verdicts)}/{len(evaluated_layout_indices_by_check[check])}" for check, verdicts in layout_pass_verdicts_by_check.items() ) print(f"[placement] Validated {num_candidates} candidate layout(s); passed per check: {summary}") @@ -639,16 +655,19 @@ def _run_inexpensive_checks( bboxes: list[dict[PlaceableAsset, AxisAlignedBoundingBox]], collision_objects: list[CollisionObject], layout_pass_verdicts_by_check: dict[str, list[bool]], - num_layouts_evaluated_by_check: dict[str, int], + evaluated_layout_indices_by_check: dict[str, list[int]], + layout_indices_across_batch: list[int] | None, ) -> None: - """Run every inexpensive validator on all candidates, recording verdicts and evaluated counts.""" + """Run every inexpensive validator on all candidates, recording verdicts and evaluated layouts.""" num_candidates = len(positions) for validator in self._validators: if not validator.run_after_inexpensive_checks: + if self._visualizer is not None: + self._visualizer.set_active_layout_indices_across_batch(layout_indices_across_batch or []) layout_pass_verdicts_by_check[validator.check] = validator.validate_batch( positions, orientations, bboxes, collision_objects ) - num_layouts_evaluated_by_check[validator.check] = num_candidates + evaluated_layout_indices_by_check[validator.check] = list(range(num_candidates)) def _run_expensive_checks( self, @@ -658,7 +677,8 @@ def _run_expensive_checks( collision_objects: list[CollisionObject], required: set[str] | None, layout_pass_verdicts_by_check: dict[str, list[bool]], - num_layouts_evaluated_by_check: dict[str, int], + evaluated_layout_indices_by_check: dict[str, list[int]], + layout_indices_across_batch: list[int] | None, ) -> None: """Run each expensive validator only on candidates that passed the required inexpensive checks.""" num_candidates = len(positions) @@ -669,6 +689,10 @@ def _run_expensive_checks( for i in range(num_candidates) if self._passes_required_checks(layout_pass_verdicts_by_check, required, i) ] + if self._visualizer is not None and layout_indices_across_batch is not None: + self._visualizer.set_active_layout_indices_across_batch( + [layout_indices_across_batch[i] for i in passed_layout_indices] + ) # only passed layouts are validated verdicts_over_passed_layout = validator.validate_batch( [positions[i] for i in passed_layout_indices], @@ -677,10 +701,10 @@ def _run_expensive_checks( collision_objects, ) verdicts = [False] * num_candidates - for sub_idx, cand_idx in enumerate(passed_layout_indices): - verdicts[cand_idx] = verdicts_over_passed_layout[sub_idx] + for layout_index_within_batch, verdict in zip(passed_layout_indices, verdicts_over_passed_layout): + verdicts[layout_index_within_batch] = verdict layout_pass_verdicts_by_check[validator.check] = verdicts - num_layouts_evaluated_by_check[validator.check] = len(passed_layout_indices) + evaluated_layout_indices_by_check[validator.check] = passed_layout_indices @staticmethod def _passes_required_checks( diff --git a/isaaclab_arena/relations/object_placer_params.py b/isaaclab_arena/relations/object_placer_params.py index b6192c7769..0d0945d486 100644 --- a/isaaclab_arena/relations/object_placer_params.py +++ b/isaaclab_arena/relations/object_placer_params.py @@ -84,3 +84,9 @@ class ObjectPlacerParams: reachability_config: ReachabilityConfig = field(default_factory=ReachabilityConfig) """Tuning for the optional ``ik_reachable`` build-time check. See ReachabilityConfig for more details.""" + + debug_visualize: bool = False + """If True, stream every validated candidate layout to a spawned Rerun viewer window. Off by default.""" + + debug_visualize_output_path: str | None = None + """Path to record the debug visualization to as a Rerun ``.rrd`` file, for headless runs.""" diff --git a/isaaclab_arena/relations/placement_validators.py b/isaaclab_arena/relations/placement_validators.py index 65ee5612eb..0abe7591b3 100644 --- a/isaaclab_arena/relations/placement_validators.py +++ b/isaaclab_arena/relations/placement_validators.py @@ -29,6 +29,7 @@ from isaaclab_arena.relations.collision_object import CollisionObject from isaaclab_arena.relations.object_placer_params import ObjectPlacerParams from isaaclab_arena.relations.placement_asset import PlaceableAsset + from isaaclab_arena.relations.placement_visualizer import PlacementRerunVisualizer from isaaclab_arena.relations.warp_mesh_manager import WarpMeshAndSphereCache from isaaclab_arena.utils.bounding_box import AxisAlignedBoundingBox @@ -48,8 +49,9 @@ class PlacementValidator(ABC): set this flag, so an expensive check (e.g. IK reachability) never runs on a layout rejected on cheaper geometry.""" - def __init__(self, params: ObjectPlacerParams) -> None: + def __init__(self, params: ObjectPlacerParams, visualizer: PlacementRerunVisualizer | None = None) -> None: self._params = params + self._visualizer = visualizer @classmethod def is_available(cls, params: ObjectPlacerParams) -> bool: @@ -83,7 +85,9 @@ def get_build_time_checks() -> tuple[str, ...]: return tuple(PlacementValidatorRegistry().get_all_keys()) -def build_validators(params: ObjectPlacerParams) -> list[PlacementValidator]: +def build_validators( + params: ObjectPlacerParams, visualizer: PlacementRerunVisualizer | None = None +) -> list[PlacementValidator]: """Construct the enabled build-time validators in registration order. A registered check whose is_available() returns False is delisted; a check named in @@ -92,6 +96,8 @@ def build_validators(params: ObjectPlacerParams) -> list[PlacementValidator]: Args: params: Placement params injected into each registered validator. + visualizer: The caller's debug view, injected into each validator so a check can draw its + own visualization layer on it; None when the run has no view. """ registry = PlacementValidatorRegistry() registered_checks = get_build_time_checks() @@ -104,7 +110,7 @@ def build_validators(params: ObjectPlacerParams) -> list[PlacementValidator]: for check in registered_checks: validator_cls = registry.get_validator_by_name(check) if validator_cls.is_available(params): - validators.append(validator_cls(params)) + validators.append(validator_cls(params, visualizer)) return validators @@ -355,8 +361,8 @@ class NoOverlapValidator(PlacementValidator): check = PlacementCheck.NO_OVERLAP - def __init__(self, params: ObjectPlacerParams) -> None: - super().__init__(params) + def __init__(self, params: ObjectPlacerParams, visualizer: PlacementRerunVisualizer | None = None) -> None: + super().__init__(params, visualizer) self._cpu_mesh_manager: WarpMeshAndSphereCache | None = None def validate_batch( diff --git a/isaaclab_arena/relations/placement_visualizer.py b/isaaclab_arena/relations/placement_visualizer.py new file mode 100644 index 0000000000..54813bce46 --- /dev/null +++ b/isaaclab_arena/relations/placement_visualizer.py @@ -0,0 +1,391 @@ +# 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 + +"""Rerun debug view of build-time placement validation, sim-free (no SimApp). + +Turn it on with ``ObjectPlacerParams.debug_visualize`` for a viewer window, or +``debug_visualize_output_path`` for an ``.rrd`` recording; either one alone is enough. From an env +graph YAML (worked example: ``isaaclab_arena/tests/test_data/placement_debug_view_env_graph.yaml``):: + + placement_validators: + debug_visualize: true + debug_visualize_output_path: /tmp/placement.rrd +""" + +from __future__ import annotations + +import math +import socket +import subprocess +import time +from pathlib import Path +from typing import TYPE_CHECKING, Any + +from isaaclab_arena.relations.relations import get_anchor_objects + +if TYPE_CHECKING: + from isaaclab_arena.relations.object_placer_params import ObjectPlacerParams + from isaaclab_arena.relations.placement_asset import PlaceableAsset + from isaaclab_arena.utils.bounding_box import AxisAlignedBoundingBox + +LAYOUT_TIMELINE = "layout" +"""Rerun timeline whose sequence index is the layout number.""" + +LAYOUT_ENTITY = "world/layout" +"""Entity path of the layout's object boxes.""" + +ROBOT_ENTITY = "world/robot" +"""Entity path for check-specific robot layers; cleared on every layout, so nothing carries over.""" + +ANCHOR_COLOR = (140, 140, 150) +"""Color of the layout's anchors, which are fixed and only act as obstacles.""" + +MOVABLE_COLOR = (70, 130, 220) +"""Color of the objects placement actually solves for.""" + +VIEWER_HOST = "127.0.0.1" +"""Interface the spawned viewer is reached on; it always runs alongside the process that logs to it.""" + +VIEWER_PORT = 9876 +"""Port the spawned viewer serves on; Rerun's default, so ``rerun --connect`` finds it unprompted.""" + +VIEWER_SHUTDOWN_TIMEOUT_S = 10.0 +"""How long an explicit close() waits for the viewer window to go away before giving up on it.""" + +VIEWER_STARTUP_TIMEOUT_S = 20.0 +"""How long spawning waits for the viewer window to start serving before letting placement run on.""" + +VIEWER_PROBE_TIMEOUT_S = 0.2 +"""How long one connection attempt to the viewer port may take before it counts as unanswered.""" + +VIEWER_PROBE_INTERVAL_S = 0.1 +"""How long to wait between connection attempts while the viewer window starts.""" + +_ACTIVE_VISUALIZER: PlacementRerunVisualizer | None = None +"""The process's live view tied to a placer: the recording, the viewer port and the output path.""" + + +def get_or_create_placement_visualizer(params: ObjectPlacerParams) -> PlacementRerunVisualizer | None: + """Return the process's Rerun view of placement validation, or None when no one asks for it. + + Args: + params: Placement parameters carrying the ``debug_visualize`` / ``debug_visualize_output_path`` fields. + """ + # Global to avoid creating multiple visualizers for the same process. + global _ACTIVE_VISUALIZER + if not params.debug_visualize and params.debug_visualize_output_path is None: + return None + if _ACTIVE_VISUALIZER is None: + _ACTIVE_VISUALIZER = PlacementRerunVisualizer( + spawn=params.debug_visualize, output_path=params.debug_visualize_output_path + ) + return _ACTIVE_VISUALIZER + + +def find_rerun_viewer_executable() -> str | None: + """Return the path of the Rerun viewer binary shipped with ``rerun-sdk``, or None if absent. + + Isaac Sim's Python does not put the packaged ``rerun_cli`` directory on PATH, so ``rr.spawn()`` + fails to find the viewer unless it is passed explicitly. + """ + import rerun as rr + + executable = Path(rr.__file__).parents[1] / "rerun_cli" / "rerun" + return str(executable) if executable.is_file() else None + + +def spawn_viewer_process() -> tuple[subprocess.Popen, Any]: + """Spawn a viewer window that dies with this process; return it and the sink that streams to it. + + ``setpriv --pdeathsig`` rather than ``rerun.spawn()``, which detaches the viewer and drops its + pid: the kernel then closes the window even on the hard ``os._exit`` Isaac Sim shuts down with. + """ + import rerun as rr + + executable = find_rerun_viewer_executable() + assert executable is not None, "rerun-sdk ships no viewer binary here; record to an .rrd instead." + if _viewer_port_answers(): + print( + f"WARNING: something already serves on port {VIEWER_PORT}; this run's layouts will " + "stream into that window rather than a new one." + ) + viewer_process = subprocess.Popen([ + "setpriv", + "--pdeathsig", + "TERM", + "--", + executable, + f"--port={VIEWER_PORT}", + "--memory-limit=75%", + "--server-memory-limit=1GiB", + # Wait for this run's recording instead of opening on the welcome screen. + "--expect-data-soon", + ]) + _wait_until_viewer_serves(viewer_process) + return viewer_process, rr.GrpcSink(url=f"rerun+http://{VIEWER_HOST}:{VIEWER_PORT}/proxy") + + +def _viewer_port_answers() -> bool: + """Whether anything at all is serving on the viewer port -- not necessarily our own viewer.""" + with socket.socket() as probe: + probe.settimeout(VIEWER_PROBE_TIMEOUT_S) + return probe.connect_ex((VIEWER_HOST, VIEWER_PORT)) == 0 + + +def _wait_until_viewer_serves(viewer_process: subprocess.Popen) -> None: + """Block until the spawned viewer answers on its port, so the first layouts are not lost. + + Never fatal -- a view that fails to come up does not stop the run it was only meant to explain. + """ + deadline = time.monotonic() + VIEWER_STARTUP_TIMEOUT_S + while time.monotonic() < deadline: + if _viewer_port_answers(): + # An answering port is not proof it is ours: a viewer that lost the race for it exits + # right after, leaving this run logging into somebody else's window. + if viewer_process.poll() is None: + return + print(f"WARNING: the Rerun viewer exited; layouts will stream to whatever else holds port {VIEWER_PORT}.") + return + if viewer_process.poll() is not None: + print("WARNING: the Rerun viewer exited while starting; placement will not be visualized.") + return + time.sleep(VIEWER_PROBE_INTERVAL_S) + print(f"WARNING: the Rerun viewer did not serve within {VIEWER_STARTUP_TIMEOUT_S:.0f}s; layouts may be missing.") + + +def summarize_layout_verdict( + layout_index_across_batch: int, verdicts_by_check: dict[str, bool], required_checks: set[str] | None +) -> tuple[str, bool]: + """Describe how placement judged one layout, as ``(message, accepted)``. + + Args: + layout_index_across_batch: Timeline index of the layout, used in the message. + verdicts_by_check: Verdict per check that ran on this layout. + required_checks: Checks that gate acceptance; None means every check that ran gates it. + """ + failed = [check for check, passed in verdicts_by_check.items() if not passed] + blocking = [check for check in failed if required_checks is None or check in required_checks] + advisory = [check for check in failed if check not in blocking] + layout = f"layout {layout_index_across_batch}" + if blocking: + return f"{layout}: rejected (failed: {', '.join(blocking)})", False + if advisory: + return f"{layout}: accepted (failed but not required: {', '.join(advisory)})", True + return f"{layout}: accepted", True + + +class PlacementRerunVisualizer: + """Streams every validated layout to Rerun, one frame per layout.""" + + def __init__(self, app_id: str = "arena_placement", spawn: bool = True, output_path: str | None = None) -> None: + """Start the recording and, unless recording headlessly, spawn a viewer window. + + Args: + app_id: Rerun application id, shown in the viewer title. + spawn: Whether to spawn a local viewer window and stream to it. + output_path: Optional path to also record the stream to, for replay on another machine. + """ + import rerun as rr + + rr.init(app_id, spawn=False) + sinks: list = [] + self._viewer_process: subprocess.Popen | None = None + if spawn: + self._viewer_process, viewer_sink = spawn_viewer_process() + sinks.append(viewer_sink) + if output_path is not None: + Path(output_path).parent.mkdir(parents=True, exist_ok=True) + sinks.append(rr.FileSink(output_path)) + assert sinks, "PlacementRerunVisualizer needs a viewer to spawn or an output path to record to." + rr.set_sinks(*sinks) + rr.log("world", rr.ViewCoordinates.RIGHT_HAND_Z_UP, static=True) + self._next_layout_index_across_batch = 0 + self._active_layout_indices_across_batch: list[int] = [] + + def __deepcopy__(self, memory_map: dict[int, object]) -> PlacementRerunVisualizer: + """Return the live view for ``copy.deepcopy`` instead of duplicating it. + + Isaac Lab's configclass deep-copies the placement event params that carry the pool, and a + duplicated view would keep its own layout counter and overwrite frames this one already drew. + + Args: + memory_map: ``copy.deepcopy``'s ``id(original) -> copy`` cache. + """ + memory_map[id(self)] = self + return self + + @property + def num_logged_layouts(self) -> int: + """How many layouts have been given a frame so far.""" + return self._next_layout_index_across_batch + + def reserve_layout_indices_across_batch(self, num_layouts: int) -> list[int]: + """Reserve and return one timeline index per layout of the batch about to be validated. + + Indices keep counting across batches so a pool that refills several times does not overwrite + its earlier frames. + """ + start = self._next_layout_index_across_batch + self._next_layout_index_across_batch += num_layouts + return list(range(start, self._next_layout_index_across_batch)) + + def set_active_layout_indices_across_batch(self, layout_indices_across_batch: list[int]) -> None: + """Set the batch ``get_layout_index_across_batch`` resolves against, in the order the check sees it.""" + self._active_layout_indices_across_batch = list(layout_indices_across_batch) + + def get_layout_index_across_batch(self, layout_index_within_batch: int) -> int: + """Timeline index of the layout at ``layout_index_within_batch`` of the active batch.""" + return self._active_layout_indices_across_batch[layout_index_within_batch] + + def set_time(self, layout_index_across_batch: int) -> None: + """Point the recording at one layout's frame, so subsequent logs land on it.""" + import rerun as rr + + rr.set_time(LAYOUT_TIMELINE, sequence=layout_index_across_batch) + + def log_layout( + self, + layout_index_across_batch: int, + positions: dict[PlaceableAsset, tuple[float, float, float]], + orientations: dict[PlaceableAsset, float], + bboxes: dict[PlaceableAsset, AxisAlignedBoundingBox], + anchors: set[PlaceableAsset], + ) -> None: + """Log one layout's solved layout as boxes in the world frame. + + Args: + layout_index_across_batch: Timeline index to log against. + positions: Solved (x, y, z) per object. + orientations: Absolute world Z-yaw per object; objects without one are drawn unrotated. + bboxes: Per-object local bounding box. + anchors: The layout's anchor objects, drawn in the anchor color. + """ + import rerun as rr + + self.set_time(layout_index_across_batch) + # A check that skips this layout must not leave its previous layout's robot on screen. + rr.log(ROBOT_ENTITY, rr.Clear(recursive=True)) + + objects = list(positions) + centers, half_sizes, quaternions = [], [], [] + for obj in objects: + yaw = orientations.get(obj, 0.0) + bbox = bboxes[obj] + local_center = [float(v) for v in bbox.center[0].tolist()] + cos_yaw, sin_yaw = math.cos(yaw), math.sin(yaw) + rotated_center = ( + cos_yaw * local_center[0] - sin_yaw * local_center[1], + sin_yaw * local_center[0] + cos_yaw * local_center[1], + local_center[2], + ) + position = positions[obj] + centers.append([position[i] + rotated_center[i] for i in range(3)]) + half_sizes.append([0.5 * float(v) for v in bbox.size[0].tolist()]) + quaternions.append(rr.Quaternion(xyzw=[0.0, 0.0, math.sin(0.5 * yaw), math.cos(0.5 * yaw)])) + + rr.log( + LAYOUT_ENTITY, + rr.Boxes3D( + centers=centers, + half_sizes=half_sizes, + quaternions=quaternions, + colors=[ANCHOR_COLOR if obj in anchors else MOVABLE_COLOR for obj in objects], + labels=[obj.name for obj in objects], + fill_mode=rr.components.FillMode.MajorWireframe, + ), + ) + + def log_layout_batch( + self, + positions: list[dict[PlaceableAsset, tuple[float, float, float]]], + orientations: list[dict[PlaceableAsset, float]], + bboxes: list[dict[PlaceableAsset, AxisAlignedBoundingBox]], + ) -> list[int]: + """Log every solved layout of one batch, returning the timeline index each was drawn on. + + Args: + positions: Solved (x, y, z) per object, one dict per layout. + orientations: Absolute world Z-yaw per object, one dict per layout. + bboxes: Per-object local bounding box, one dict per layout. + """ + layout_indices_across_batch = self.reserve_layout_indices_across_batch(len(positions)) + for layout_index_within_batch, layout_index_across_batch in enumerate(layout_indices_across_batch): + self.log_layout( + layout_index_across_batch, + positions[layout_index_within_batch], + orientations[layout_index_within_batch], + bboxes[layout_index_within_batch], + anchors=set(get_anchor_objects(list(positions[layout_index_within_batch]))), + ) + return layout_indices_across_batch + + def log_layout_verdicts( + self, layout_index_across_batch: int, verdicts_by_check: dict[str, bool], required_checks: set[str] | None + ) -> None: + """Log which checks accepted one layout, as a text line and an accepted/rejected marker. + + Args: + layout_index_across_batch: Timeline index to log against. + verdicts_by_check: Verdict per check that ran on this layout; one that skipped it is absent. + required_checks: Checks that gate acceptance; None means every check that ran gates it. + """ + import rerun as rr + + self.set_time(layout_index_across_batch) + message, accepted = summarize_layout_verdict(layout_index_across_batch, verdicts_by_check, required_checks) + rr.log( + f"{LAYOUT_ENTITY}/verdict", + rr.TextLog(message, level=rr.TextLogLevel.INFO if accepted else rr.TextLogLevel.WARN), + ) + for check, passed in verdicts_by_check.items(): + rr.log(f"checks/{check}", rr.Scalars(float(passed))) + + def log_layout_batch_verdicts( + self, + layout_indices_across_batch: list[int], + verdicts_by_check: dict[str, list[bool]], + evaluated_layout_indices_by_check: dict[str, list[int]], + required_checks: set[str] | None, + ) -> None: + """Log the check verdicts of one validated batch of layouts, one layout at a time. + + A check is left off the layouts it did not run on, so a skipped one is not drawn as failed. + + Args: + layout_indices_across_batch: Timeline index of each layout of the batch, in batch order. + verdicts_by_check: Per check, its verdict for every layout of the batch. + evaluated_layout_indices_by_check: Per check, which layouts of the batch it ran on. + required_checks: Checks that gate acceptance; None means every check that ran gates it. + """ + evaluated = {check: set(indices) for check, indices in evaluated_layout_indices_by_check.items()} + for layout_index_within_batch, layout_index_across_batch in enumerate(layout_indices_across_batch): + self.log_layout_verdicts( + layout_index_across_batch, + { + check: verdicts[layout_index_within_batch] + for check, verdicts in verdicts_by_check.items() + if layout_index_within_batch in evaluated[check] + }, + required_checks, + ) + + def close(self) -> None: + """Flush pending data and shut down the viewer window this run spawned.""" + import rerun as rr + + # None once Rerun's own shutdown hook has torn the recording down ahead of this call. + recording = rr.get_global_data_recording() + if recording is not None: + recording.flush() + viewer_process, self._viewer_process = self._viewer_process, None + if viewer_process is None: + return + viewer_process.terminate() + try: + viewer_process.wait(timeout=VIEWER_SHUTDOWN_TIMEOUT_S) + except subprocess.TimeoutExpired: + # A window wedged past SIGTERM would otherwise keep the port and outlive the run. + viewer_process.kill() + viewer_process.wait() diff --git a/isaaclab_arena/tests/test_arena_env_graph_spec.py b/isaaclab_arena/tests/test_arena_env_graph_spec.py index c1248fc8be..1a027814b5 100644 --- a/isaaclab_arena/tests/test_arena_env_graph_spec.py +++ b/isaaclab_arena/tests/test_arena_env_graph_spec.py @@ -12,6 +12,7 @@ import pytest from pydantic import ValidationError +import isaaclab_arena_environments from isaaclab_arena.assets.object_type import ObjectType from isaaclab_arena.assets.registries import ObjectRelationLibraryRegistry, TaskRegistry from isaaclab_arena.environment_spec.arena_env_graph_spec import ArenaEnvGraphSpec @@ -22,6 +23,7 @@ TEST_DATA_DIR = Path(__file__).parent / "test_data" _GRAPH = TEST_DATA_DIR / "pick_and_place_maple_table_env_graph.yaml" _OBJECT_SET_GRAPH = TEST_DATA_DIR / "object_set_maple_table_env_graph.yaml" +_DEBUG_VIEW_GRAPH = TEST_DATA_DIR / "placement_debug_view_env_graph.yaml" def test_graph_spec_loads_pick_and_place_yaml(): @@ -461,3 +463,34 @@ def test_a_spec_naming_a_searched_simready_asset_by_its_search_name_is_rejected( assert result.returncode != 0 assert "Unknown asset registry_name 'simready_replay_teapot'" in result.stderr + + +def test_graph_spec_leaves_placement_debug_view_off_by_default(): + """A graph YAML that says nothing about debug visualization builds placement params with it off.""" + from isaaclab_arena.environment_spec.arena_env_graph_conversion_utils import build_checks_for_placer_params + + params = build_checks_for_placer_params(ArenaEnvGraphSpec.from_yaml(_GRAPH)) + + assert not params.debug_visualize + assert params.debug_visualize_output_path is None + + +def test_graph_spec_forwards_placement_debug_view_to_placer_params(): + """A YAML asking for the debug view reaches the params ObjectPlacer reads, both fields intact.""" + from isaaclab_arena.environment_spec.arena_env_graph_conversion_utils import build_checks_for_placer_params + + params = build_checks_for_placer_params(ArenaEnvGraphSpec.from_yaml(_DEBUG_VIEW_GRAPH)) + + assert params.debug_visualize + assert params.debug_visualize_output_path == "/tmp/placement_debug_view.rrd" + + +def test_graph_spec_leaves_shipped_envs_out_of_the_debug_view(): + """The debug view spawns a window on every build, so no env under version control may ask for it.""" + asking_for_the_view = [ + yaml_path + for yaml_path in Path(isaaclab_arena_environments.__file__).parent.rglob("*.yaml") + if "debug_visualize: true" in yaml_path.read_text() + ] + + assert not asking_for_the_view, f"debug_visualize must stay off in shipped envs: {asking_for_the_view}" diff --git a/isaaclab_arena/tests/test_data/placement_debug_view_env_graph.yaml b/isaaclab_arena/tests/test_data/placement_debug_view_env_graph.yaml new file mode 100644 index 0000000000..786dd92b29 --- /dev/null +++ b/isaaclab_arena/tests/test_data/placement_debug_view_env_graph.yaml @@ -0,0 +1,39 @@ +# 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 +env_name: llm_gen_maple_table_robolab_PickAndPlaceTask +embodiment: + id: droid + registry_name: droid_abs_joint_pos +background: + id: maple_table_robolab + registry_name: maple_table_robolab +objects: +- id: rubiks_cube_hot3d_robolab + registry_name: rubiks_cube_hot3d_robolab +- id: bowl_ycb_robolab + registry_name: bowl_ycb_robolab +placement_validators: + enabled_checks: [no_overlap, on_relation, ik_reachable] + required_checks: [no_overlap, on_relation] + debug_visualize: true + debug_visualize_output_path: /tmp/placement_debug_view.rrd +relations: +- kind: is_anchor + subject: maple_table_robolab +- kind: 'on' + subject: rubiks_cube_hot3d_robolab + reference: maple_table_robolab +- kind: 'on' + subject: bowl_ycb_robolab + reference: maple_table_robolab +task: + composition: atomic + description: pick up the rubiks cube and place it in the bowl + subtasks: + - kind: PickAndPlaceTask + params: + pick_up_object: rubiks_cube_hot3d_robolab + destination_location: bowl_ycb_robolab + background_scene: maple_table_robolab diff --git a/isaaclab_arena/tests/test_placement_visualizer.py b/isaaclab_arena/tests/test_placement_visualizer.py new file mode 100644 index 0000000000..891edbb95c --- /dev/null +++ b/isaaclab_arena/tests/test_placement_visualizer.py @@ -0,0 +1,234 @@ +# 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 + +"""Tests for the Rerun debug view of build-time placement validation. + +Placement is sim-free, so these run a real solve and a real recording, headless: the ``.rrd`` sink +replaces the viewer window, and no Isaac Sim or GPU is involved. +""" + +from __future__ import annotations + +import subprocess + +import pytest + +from isaaclab_arena.relations import placement_visualizer +from isaaclab_arena.relations.object_placer import ObjectPlacer +from isaaclab_arena.relations.object_placer_params import ObjectPlacerParams +from isaaclab_arena.relations.placement_visualizer import PlacementRerunVisualizer, summarize_layout_verdict +from isaaclab_arena.relations.relation_solver_params import RelationSolverParams +from isaaclab_arena.relations.relations import IsAnchor, On +from isaaclab_arena.tests.dummy_object import DummyObject +from isaaclab_arena.utils.bounding_box import AxisAlignedBoundingBox +from isaaclab_arena.utils.pose import Pose + +MAX_PLACEMENT_ATTEMPTS = 3 +"""Candidate layouts solved per placement, i.e. the number of frames one place() should draw.""" + + +@pytest.fixture(autouse=True) +def _fresh_process_visualizer(monkeypatch): + """Drop the process-wide view between tests so each one gets its own recording and ``.rrd``.""" + monkeypatch.setattr(placement_visualizer, "_ACTIVE_VISUALIZER", None) + + +def _desk_and_box() -> list[DummyObject]: + """A desk anchor with a box placed on it -- the smallest layout with something to solve.""" + desk = DummyObject( + name="desk", + bounding_box=AxisAlignedBoundingBox(min_point=(0.0, 0.0, 0.0), max_point=(1.0, 1.0, 0.1)), + ) + desk.set_initial_pose(Pose(position_xyz=(0.0, 0.0, 0.0), rotation_xyzw=(0.0, 0.0, 0.0, 1.0))) + desk.add_relation(IsAnchor()) + box = DummyObject( + name="box", + bounding_box=AxisAlignedBoundingBox(min_point=(0.0, 0.0, 0.0), max_point=(0.2, 0.2, 0.2)), + ) + box.add_relation(On(desk, clearance_m=0.01)) + return [desk, box] + + +def _placer_params(**overrides) -> ObjectPlacerParams: + """Placement params for a small, deterministic solve.""" + return ObjectPlacerParams( + solver_params=RelationSolverParams(max_iters=200, convergence_threshold=1e-3), + apply_positions_to_objects=False, + max_placement_attempts=MAX_PLACEMENT_ATTEMPTS, + placement_seed=5, + **overrides, + ) + + +def test_placement_has_no_debug_view_by_default(): + """The debug view is opt-in, so a default placement never touches Rerun.""" + placer = ObjectPlacer(_placer_params()) + + assert placer._visualizer is None + + +def test_placement_records_every_candidate_layout(tmp_path): + """Recording to an .rrd draws every candidate the solve produced, one frame each, without a viewer.""" + rrd_path = tmp_path / "placement.rrd" + placer = ObjectPlacer(_placer_params(debug_visualize_output_path=str(rrd_path))) + + placer.place(_desk_and_box(), num_envs=1) + visualizer = placer._visualizer + visualizer.close() + + assert visualizer.num_logged_layouts == MAX_PLACEMENT_ATTEMPTS + assert rrd_path.is_file() and rrd_path.stat().st_size > 0 + + +def test_placement_shares_one_debug_view_across_placers(tmp_path): + """Every placer in the process draws into the same view, keeping one viewer and one timeline.""" + params = _placer_params(debug_visualize_output_path=str(tmp_path / "placement.rrd")) + + first = ObjectPlacer(params) + second = ObjectPlacer(_placer_params(debug_visualize_output_path=str(tmp_path / "ignored.rrd"))) + + assert first._visualizer is not None and first._visualizer is second._visualizer + + +def test_a_placer_with_the_view_off_gives_its_checks_no_view(tmp_path): + """A placer that asked for no view leaves its checks viewless, even while another's view is live.""" + ObjectPlacer(_placer_params(debug_visualize_output_path=str(tmp_path / "placement.rrd"))) + + placer = ObjectPlacer(_placer_params()) + + assert all(validator._visualizer is None for validator in placer._validators) + + +class _FakeViewerProcess: + """Stands in for the spawned viewer window, so the test needs no display. + + Args: + wedged: Whether the window ignores SIGTERM, i.e. whether the first wait() times out. + """ + + def __init__(self, wedged: bool = False) -> None: + self.terminate_calls = 0 + self.kill_calls = 0 + self._wedged = wedged + + def terminate(self) -> None: + self.terminate_calls += 1 + + def kill(self) -> None: + self.kill_calls += 1 + + def wait(self, timeout: float | None = None) -> int: + if self._wedged and self.kill_calls == 0: + raise subprocess.TimeoutExpired(cmd="rerun", timeout=timeout) + return 0 + + +def test_closing_the_view_shuts_down_the_viewer_it_spawned(tmp_path, monkeypatch): + """The window belongs to the run, so closing the view takes it down instead of leaving it on the port.""" + import rerun as rr + + viewer = _FakeViewerProcess() + # Record where the viewer's live stream would go, so the test needs neither a display nor a port. + monkeypatch.setattr( + placement_visualizer, + "spawn_viewer_process", + lambda: (viewer, rr.FileSink(str(tmp_path / "viewer_stand_in.rrd"))), + ) + visualizer = PlacementRerunVisualizer(app_id="arena_test", spawn=True, output_path=str(tmp_path / "p.rrd")) + + visualizer.close() + visualizer.close() + + assert viewer.terminate_calls == 1 + + +def test_closing_a_wedged_viewer_falls_back_to_killing_it(tmp_path, monkeypatch): + """A window that ignores SIGTERM still has to go, or it outlives the run holding the viewer port.""" + import rerun as rr + + viewer = _FakeViewerProcess(wedged=True) + monkeypatch.setattr( + placement_visualizer, + "spawn_viewer_process", + lambda: (viewer, rr.FileSink(str(tmp_path / "viewer_stand_in.rrd"))), + ) + visualizer = PlacementRerunVisualizer(app_id="arena_test", spawn=True, output_path=str(tmp_path / "p.rrd")) + + visualizer.close() + + assert viewer.kill_calls == 1 + + +def test_only_required_checks_reject_a_candidate(): + """The view has to agree with the placer, which gates layouts on required_checks alone.""" + verdicts = {"no_overlap": True, "ik_reachable": False} + + message, accepted = summarize_layout_verdict(3, verdicts, required_checks={"no_overlap"}) + + assert accepted + assert message == "layout 3: accepted (failed but not required: ik_reachable)" + + +def test_a_failed_required_check_rejects_a_candidate(): + """A failure the placer does gate on reads as a rejection, naming what blocked it.""" + verdicts = {"no_overlap": False, "ik_reachable": False} + + message, accepted = summarize_layout_verdict(3, verdicts, required_checks={"no_overlap"}) + + assert not accepted + assert message == "layout 3: rejected (failed: no_overlap)" + + +def test_every_check_gates_a_candidate_when_none_are_named_required(): + """required_checks=None means every check that ran is required, matching ObjectPlacerParams.""" + verdicts = {"no_overlap": True, "ik_reachable": False} + + message, accepted = summarize_layout_verdict(3, verdicts, required_checks=None) + + assert not accepted + assert message == "layout 3: rejected (failed: ik_reachable)" + + +def test_a_check_that_skipped_a_candidate_is_not_drawn_as_rejecting_it(tmp_path, monkeypatch): + """Expensive checks only see candidates the cheap ones passed; the rest are unevaluated, not failed.""" + visualizer = PlacementRerunVisualizer(app_id="arena_test", spawn=False, output_path=str(tmp_path / "p.rrd")) + drawn_verdicts: dict[int, dict[str, bool]] = {} + monkeypatch.setattr( + visualizer, + "log_layout_verdicts", + lambda layout_index_across_batch, verdicts_by_check, required_checks: drawn_verdicts.update( + {layout_index_across_batch: verdicts_by_check} + ), + ) + + visualizer.log_layout_batch_verdicts( + layout_indices_across_batch=[0, 1], + verdicts_by_check={"no_overlap": [True, False], "ik_reachable": [True, False]}, + evaluated_layout_indices_by_check={"no_overlap": [0, 1], "ik_reachable": [0]}, + required_checks=None, + ) + + assert drawn_verdicts == { + 0: {"no_overlap": True, "ik_reachable": True}, + 1: {"no_overlap": False}, + } + + +def test_candidate_frames_keep_counting_across_batches(tmp_path): + """A pool that refills gets fresh frames, so a later batch does not overwrite an earlier one.""" + visualizer = PlacementRerunVisualizer(app_id="arena_test", spawn=False, output_path=str(tmp_path / "p.rrd")) + + assert visualizer.reserve_layout_indices_across_batch(3) == [0, 1, 2] + assert visualizer.reserve_layout_indices_across_batch(2) == [3, 4] + + +def test_active_candidates_map_index_within_batch_to_frame(tmp_path): + """A check that only ran on some candidates still resolves each one's own frame.""" + visualizer = PlacementRerunVisualizer(app_id="arena_test", spawn=False, output_path=str(tmp_path / "p.rrd")) + + visualizer.set_active_layout_indices_across_batch([1, 4]) + + assert visualizer.get_layout_index_across_batch(0) == 1 + assert visualizer.get_layout_index_across_batch(1) == 4 diff --git a/isaaclab_arena_curobo/ik_reachability_validator.py b/isaaclab_arena_curobo/ik_reachability_validator.py index 6bb779b02c..226c1ab662 100644 --- a/isaaclab_arena_curobo/ik_reachability_validator.py +++ b/isaaclab_arena_curobo/ik_reachability_validator.py @@ -30,7 +30,9 @@ from isaaclab_arena.assets.object_base import ObjectBase from isaaclab_arena.relations.collision_object import CollisionObject from isaaclab_arena.relations.object_placer_params import ObjectPlacerParams + from isaaclab_arena.relations.placement_visualizer import PlacementRerunVisualizer from isaaclab_arena.utils.bounding_box import AxisAlignedBoundingBox + from isaaclab_arena_curobo.reachability_visualizer import ReachabilityRerunLayer def get_object_world_pose_from_layout( @@ -61,8 +63,8 @@ class ReachabilityValidator(PlacementValidator): check = PlacementCheck.IK_REACHABLE run_after_inexpensive_checks = True - def __init__(self, params: ObjectPlacerParams) -> None: - super().__init__(params) + def __init__(self, params: ObjectPlacerParams, visualizer: PlacementRerunVisualizer | None = None) -> None: + super().__init__(params, visualizer) config = params.reachability_config self._grasp_z_offset = config.grasp_z_offset_m self._ik_pos_threshold = config.ik_position_threshold_m @@ -78,6 +80,15 @@ def __init__(self, params: ObjectPlacerParams) -> None: self._base_quat_xyzw = base_pose.rotation_xyzw # Guards the zero-target warning so it fires once per validator, not once per candidate layout. self._warned_no_targets = False + self._rerun_layer = self._make_rerun_layer() + + def _make_rerun_layer(self) -> ReachabilityRerunLayer | None: + """Return this check's layer of the placement visualizer, or None when no one asked for it.""" + if self._visualizer is None: + return None + from isaaclab_arena_curobo.reachability_visualizer import ReachabilityRerunLayer + + return ReachabilityRerunLayer(self._visualizer) @classmethod def is_available(cls, params: ObjectPlacerParams) -> bool: @@ -99,18 +110,26 @@ def validate_batch( bboxes: list[dict[ObjectBase, AxisAlignedBoundingBox]], collision_objects: list[CollisionObject], ) -> list[bool]: - return [self._validate(positions[i], orientations[i]) for i in range(len(positions))] + return [ + self._validate(positions[i], orientations[i], layout_index_within_batch=i) for i in range(len(positions)) + ] def _validate( self, positions: dict[ObjectBase, tuple[float, float, float]], orientations: dict[ObjectBase, float], + layout_index_within_batch: int, ) -> bool: """Whether the robot can reach a top-down grasp at the target objects in one candidate layout. Rebuilds each object's world pose and a per-object collision cuboid, syncs them into the solver's world, then batches a single IK solve over the target objects' top-down grasps. A layout with nothing to grasp (anchor-only, or no target present) is trivially reachable. + + Args: + positions: Solved (x, y, z) per object. + orientations: Absolute world Z-yaw per object. + layout_index_within_batch: Position of this layout in the batch given to ``validate_batch``. """ objects = list(positions.keys()) anchors = set(get_anchor_objects(objects)) @@ -149,12 +168,24 @@ def _validate( ) for obj in targets ]) - feasible, _, _ = solve_ik_feasibility( + feasible, position_error, rotation_error = solve_ik_feasibility( self._solver, grasp_poses, position_threshold=self._ik_pos_threshold, rotation_threshold=self._ik_rot_threshold, ) + if self._rerun_layer is not None: + layout_index_across_batch = self._visualizer.get_layout_index_across_batch(layout_index_within_batch) + self._rerun_layer.log_layout( + layout_index_across_batch=layout_index_across_batch, + base_pos=self._base_pos, + base_quat_xyzw=self._base_quat_xyzw, + target_names=[obj.name for obj in targets], + grasp_poses_base_frame=grasp_poses, + feasible=feasible, + position_error=position_error, + rotation_error=rotation_error, + ) return bool(feasible.all().item()) def _select_reachability_targets(self, objects: list[ObjectBase], anchors: set[ObjectBase]) -> list[ObjectBase]: diff --git a/isaaclab_arena_curobo/reachability_visualizer.py b/isaaclab_arena_curobo/reachability_visualizer.py new file mode 100644 index 0000000000..bb4c4a600d --- /dev/null +++ b/isaaclab_arena_curobo/reachability_visualizer.py @@ -0,0 +1,92 @@ +# 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 + +"""The reachability check's layer of the placement Rerun debug view, sim-free (no SimApp). +Adds to existing visualizer what only the IK check knows -- where the +robot stands, the top-down grasps it solved, and whether each one was reachable. +""" + +from __future__ import annotations + +import torch + +from isaaclab_arena.relations.placement_visualizer import ROBOT_ENTITY, PlacementRerunVisualizer + +REACHABLE_COLOR = (40, 200, 80) +"""Color of a grasp the robot can reach.""" + +UNREACHABLE_COLOR = (220, 50, 50) +"""Color of a grasp the robot cannot reach, i.e. the one that rejected the layout.""" + +BASE_AXIS_LENGTH = 0.2 +"""Length (m) of the drawn robot base frame axes.""" + +GRASP_AXIS_LENGTH = 0.1 +"""Length (m) of the drawn grasp frame axes.""" + +BASE_ENTITY = f"{ROBOT_ENTITY}/base" +"""Entity path of the robot base frame; the grasps below it are logged in that frame.""" + + +class ReachabilityRerunLayer: + """Draws the reachability check's verdict for a layout into the shared placement view.""" + + def __init__(self, visualizer: PlacementRerunVisualizer) -> None: + """Bind the layer to the placement visualizer. + + Args: + visualizer: The process's placement visualizer, which owns the recording and the timeline. + """ + self._visualizer = visualizer + + def log_layout( + self, + layout_index_across_batch: int, + base_pos: tuple[float, float, float], + base_quat_xyzw: tuple[float, float, float, float], + target_names: list[str], + grasp_poses_base_frame: torch.Tensor, + feasible: torch.Tensor, + position_error: torch.Tensor, + rotation_error: torch.Tensor, + ) -> None: + """Log the robot's side of one evaluated layout. + + Args: + layout_index_across_batch: Timeline index of the layout, as assigned by the placement view. + base_pos: Robot base position in the world frame. + base_quat_xyzw: Robot base orientation in the world frame. + target_names: Names of the objects a grasp was solved for, aligned with the tensors below. + grasp_poses_base_frame: ``(b, 4, 4)`` grasp transforms in the robot base frame. + feasible: ``(b,)`` per-grasp IK verdict. + position_error: ``(b,)`` per-grasp IK position error (m). + rotation_error: ``(b,)`` per-grasp IK rotation error (rad). + """ + import rerun as rr + + self._visualizer.set_time(layout_index_across_batch) + # Grasps are solved in the robot base frame, so they are logged as children of the base + # transform and Rerun composes them back into the world frame. + rr.log(BASE_ENTITY, rr.Transform3D(translation=base_pos, quaternion=rr.Quaternion(xyzw=base_quat_xyzw))) + rr.log(BASE_ENTITY, rr.TransformAxes3D(BASE_AXIS_LENGTH)) + + grasps = grasp_poses_base_frame.detach().cpu() + for i, name in enumerate(target_names): + reachable = bool(feasible[i].item()) + color = REACHABLE_COLOR if reachable else UNREACHABLE_COLOR + entity = f"{BASE_ENTITY}/grasps/{name}" + rr.log(entity, rr.Transform3D(translation=grasps[i, :3, 3], mat3x3=grasps[i, :3, :3])) + rr.log(entity, rr.TransformAxes3D(GRASP_AXIS_LENGTH)) + rr.log( + f"{entity}/verdict", + rr.Points3D( + [[0.0, 0.0, 0.0]], + colors=[color], + radii=0.015, + labels=[f"{name}: {'reachable' if reachable else 'unreachable'}"], + ), + ) + rr.log(f"errors/{name}/position_m", rr.Scalars(float(position_error[i].item()))) + rr.log(f"errors/{name}/rotation_rad", rr.Scalars(float(rotation_error[i].item()))) diff --git a/isaaclab_arena_curobo/tests/test_ik_reachability_validator.py b/isaaclab_arena_curobo/tests/test_ik_reachability_validator.py index 76eb0fceed..3699df9b11 100644 --- a/isaaclab_arena_curobo/tests/test_ik_reachability_validator.py +++ b/isaaclab_arena_curobo/tests/test_ik_reachability_validator.py @@ -178,14 +178,70 @@ def _make_unstamped_desk_box_pool(num_envs: int = 1, min_layouts_per_env: int = ) -def _make_reachability_validator(embodiment): +def _make_reachability_validator(embodiment, visualizer=None): """Construct the registered ReachabilityValidator with ``embodiment`` set on its params.""" from isaaclab_arena.relations.object_placer_params import ObjectPlacerParams from isaaclab_arena_curobo.ik_reachability_validator import ReachabilityValidator params = ObjectPlacerParams() params.reachability_config.embodiment = embodiment - return ReachabilityValidator(params) + return ReachabilityValidator(params, visualizer) + + +@pytest.mark.curobo_deps +def test_validator_skips_visualization_by_default(monkeypatch): + """The debug view is opt-in: placement without one means the check adds no layer.""" + _patch_curobo(monkeypatch, feasible_fn=lambda n: [True] * n) + validator = _make_reachability_validator(_fake_embodiment()) + + assert validator._rerun_layer is None + + +@pytest.mark.curobo_deps +def test_validator_draws_each_candidate_on_its_own_frame(monkeypatch): + """The check draws on the frame of the candidate it was handed, not on its position in the batch. + + Expensive checks only see the candidates that passed the cheap ones, so the two differ. + """ + _patch_curobo(monkeypatch, feasible_fn=lambda n: [False] * n) + visualizer = MagicMock() + # The batch the check is given is candidates 7 and 9 of the run. + visualizer.get_layout_index_across_batch.side_effect = [7, 9] + validator = _make_reachability_validator(_fake_embodiment(), visualizer=visualizer) + drawn: list[dict] = [] + monkeypatch.setattr(validator._rerun_layer, "log_layout", lambda **kwargs: drawn.append(kwargs)) + + layout = _make_desk_box_pool().layouts_per_env()[0][0] + assert validator.validate_batch( + [layout.positions, layout.positions], [layout.orientations, layout.orientations], [{}, {}], [] + ) == [False, False] + + assert [entry["layout_index_across_batch"] for entry in drawn] == [7, 9] + assert [entry["target_names"] for entry in drawn] == [["box"], ["box"]] + + +def test_reachability_layer_records_to_rrd(tmp_path): + """The layer's grasps and verdicts reach a recording without a viewer or cuRobo.""" + from isaaclab_arena.relations.placement_visualizer import PlacementRerunVisualizer + from isaaclab_arena_curobo.reachability_visualizer import ReachabilityRerunLayer + + rrd_path = tmp_path / "placement.rrd" + visualizer = PlacementRerunVisualizer(app_id="arena_test", spawn=False, output_path=str(rrd_path)) + layer = ReachabilityRerunLayer(visualizer) + + layer.log_layout( + layout_index_across_batch=0, + base_pos=(0.0, 0.0, 0.0), + base_quat_xyzw=(0.0, 0.0, 0.0, 1.0), + target_names=["box"], + grasp_poses_base_frame=torch.eye(4).unsqueeze(0), + feasible=torch.tensor([False]), + position_error=torch.tensor([0.3]), + rotation_error=torch.tensor([0.1]), + ) + visualizer.close() + + assert rrd_path.is_file() and rrd_path.stat().st_size > 0 @pytest.mark.curobo_deps