Add an optional build-time, sim-free gate to keep only the object layouts a robot can actually reach, using cuRobo IK (Part 2/3) by xyao-nv · Pull Request #914 · isaac-sim/IsaacLab-Arena

xyao-nv · 2026-07-15T16:02:10Z

Summary

Today the solver only checks that objects fit geometrically, before placement event. This MR adds a second, optional check: a layout is stored only when it passes geometry and the robot can reach a top-down grasp at every movable object. The placement loop keeps re-sampling until every env has enough reachable layouts.

Detailed description

New CLI flag --validate_reachability wired through ArenaEnvBuilderCfg into the placement pool. Off by default.
The core placer only knows about a generic reachability_validator: A small registry pulls up the validator and provides the curobo ik entry point. So the core stays curobo-agnostic.
A new standalone (SimApp-free) IK layout validator that reconstructs each object's world pose from a layout, builds one collision box per object, and batch-checks top-down grasp IK feasibility. The validator only runs on geometry-valid candidates so the expensive IK check never wastes time on layouts that already failed.
Consolidated the cuRobo IK helpers. Added supportive utils funcs.

How to

Run in curobo image (-c). Pass --validate_reachability on the command line.

python isaaclab_arena/evaluation/policy_runner.py       --viz kit       --policy_type zero_action       --enable_cameras       --num_steps 100   --validate_reachability    --env_graph_spec_yaml isaaclab_arena_environments/robolab/tasks/rubiks_cube.yaml

Logs will show how many layouts are rejected after validation upon each refill

Placement pool solved 5 candidates, 4 valid, 1 failed validation

Note

Different than Add Curobo-IK reachability post-validation check (Part 1/3) #837 where it's an isolated, post-validation, sim-live, env-coupled process. Here it refactors previous Curobo machinery and uses Curobo in a sim-free manner.
Instead of using live USD mesh for all its compute, it approximates the world by using cuboid with object's AABB. It's a simpler machinery compared with a throwaway env to avoid conflict from calling env.reset() terms.

TODO

Apply reachability only among task-centric objects.
Disable hand joint to check collision between world <> arms (Curobo.success)

xyao-nv · 2026-07-15T21:19:31Z

+            # Load the extension providing the gate, then pull its validator into the pool's solve loop.
+            extensions = ["isaaclab_arena_curobo"]
+            import_extensions(extensions)
+            placer_params.reachability_validator = resolve_reachability_validator(


I cannot think of other ways to pass this callable as optional to solver call. Open for suggestions.

Is it possible to make it an input to solve_and_apply_relation_placement, so that we can also remove it from ObjectPlacerParams? The reachability check is quite detached from the solver, so I don't have a very good suggestion at this moment

xyao-nv · 2026-07-15T21:20:04Z

    device: str = "cuda:0"
    language_instruction: str | None = None

+    validate_reachability: bool = False


Because curobo does not support pip, so leaving reachability validation as optional

- Add optional ObjectPlacerParams.layout_validator; PooledObjectPlacer gates each geometry-valid candidate on it, so the solve loop reject-&-refills until every env has enough accepted layouts. Core stays cuRobo-free (generic Callable). - Add sim-free cuRobo oracle (simfree_ik): standalone MotionGen/IKSolver over a bounding-box collision world -- no SimulationApp, Isaac Lab env, or env.reset. - Add make_simfree_ik_layout_validator: build the solver from an embodiment and return a layout_validator closure (bbox world + top-down grasp IK per movable object). - Add run_ik_gated_placement.py: attach the validator before compose so the eval env is built once with an IK-reachable pool (no throwaway env; recorder/pool/RNG stay pristine). - Tests: core gating + reject-&-refill, and the factory closure with cuRobo mocked. Signed-off-by: Xinjie Yao <xyao@nvidia.com>

Let an environment declare a build-time layout validator on its placement config, resolved through a string-keyed registry an extension populates on import, so core never imports cuRobo. - Add ObjectPlacerParams.layout_validator_key/_kwargs so an env declares the gate its placement wants (e.g. "simfree_ik" for cuRobo IK reachability). - Move extension loading onto ArenaEnvBuilderCfg.enable_extensions and import it in the builder, so both the argparse and typed run-config paths resolve declared keys identically; demote --layout_validator_key to an override. - Register "simfree_ik" from isaaclab_arena_curobo on import via a lazy factory; factor shared frame math into curobo_frame_utils. - Replace run_ik_gated_placement.py with test_ik_gated_placement.py driving the env-declared gate end-to-end (10/10 layouts IK-reachable). Signed-off-by: Xinjie Yao <xyao@nvidia.com>

Collapse the string-keyed layout-validator indirection into a single reachability-validator slot selected by a boolean, so a run asks for the gate with --validate_reachability instead of naming a key. Core still never imports cuRobo; the provider registers into the slot via --enable_extensions. - Replace ObjectPlacerParams.layout_validator_key/_kwargs and the keyed registry with a single slot plus ArenaEnvBuilderCfg.validate_reachability (+ reachability_validator_kwargs); the builder fills placer_params.layout_validator from the slot when the flag is set. - Swap --layout_validator_key for the --validate_reachability store_true flag. - Register the sim-free cuRobo IK validator into the single slot on import. - Update test_ik_gated_placement.py to drive the flag (10/10 IK-reachable). Signed-off-by: Xinjie Yao <xyao@nvidia.com>

Name the two IK paths for what actually differs instead of the vague "simfree": the standalone path (no live env) vs the env-coupled path, and the grasp-pose pair by its input source. - Rename the sim-free family to "standalone": simfree_ik.py -> standalone_ik.py, SimFreeIKReachability -> StandaloneIKReachability, SimFreeCuboid -> StandaloneCuboid, check_ik_feasibility_simfree -> check_ik_feasibility_standalone, make_simfree_ik_layout_validator -> make_standalone_ik_layout_validator, and the layout-validator module + its test. - Rename the grasp-pose pair so the diff is the input source: top_down_grasp_pose_in_robot_frame -> top_down_grasp_pose_from_env (reads a live env), and top_down_grasp_pose_simfree -> top_down_grasp_pose_from_world_poses (takes explicit world poses). - Finish the WIP top_down_grasp_matrix: import Pose and convert the grasp rotation matrix to a quaternion before building the Pose (rotation_xyzw is a 4-quat), fixing an undefined name and a matrix-as-quaternion mismatch; plus an "assymetric" typo. Validated in the cuRobo image: build-time IK gate still 10/10 layouts reachable; mocked validator unit test passes. Signed-off-by: Xinjie Yao <xyao@nvidia.com>

zhx06

Focused pass on the reachability-solver interface — i.e. how the IK gate validates the geometry-valid layouts. The plumbing (registry indirection, geometry-first short-circuit, reject-&-refill) is clean and keeps core vendor-agnostic. My main concern is that the current IK check is pose-residual only and does not yet enforce the collision world it goes to some trouble to build, so "IK reachable" doesn't actually validate the geometric/collision feasibility a grasp needs. Inline notes below.

zhx06 · 2026-07-16T01:12:21Z

+    num_poses = positions.shape[0]
+    pos_err = ik_result.position_error.view(num_poses, -1)
+    rot_err = ik_result.rotation_error.view(num_poses, -1)
+    best_idx = pos_err.argmin(dim=1, keepdim=True)


Subtle false-negative in the best-seed selection: you pick the seed with minimum position error (pos_err.argmin), then test that one seed's rotation error against rotation_threshold. A different seed could satisfy both thresholds while the min-position seed fails rotation, and you'd wrongly report infeasible. Feasibility should be "does any seed meet both bars":

ok = (pos_err < position_threshold) & (rot_err < rotation_threshold) feasible = ok.any(dim=1)

Once collision success is added (comment below), the per-pose reduction should likewise be over successful seeds only, otherwise best_pos_err/best_rot_err can report an in-collision solution's residuals.

Current (argmin): it first ranks seeds by position error alone, picks that one seed, then checks both position and rotation on it. So the rotation test is applied to whatever seed happened to win on position.

Yours (.any): each seed must clear both thresholds itself; a pose is feasible if any seed does.

Actually internally for Curobo, right now.
It optimizes num_seeds (12) in parallel, but it returns only the top return_seeds = 1 solution per pose — has already reduced to its own single best seed.

I can adopt yours.

zhx06

Follow-up focused on the relation-solver side of the interface — where the gate actually plugs into PooledObjectPlacer. The predicate hook itself is a clean seam, but the placement of the call inside the refill loop has a real efficiency cost, and the Callable[..., bool] contract hides a mutation. Notes inline.

zhx06 · 2026-07-16T01:19:59Z

            env_results = ranked_results_per_env[cur_env][:layouts_per_env]
-            valid_results = [r for r in env_results if r.success]
+            # if reachability validator is not set, results are only checked through geometry validation
+            valid_results = [r for r in env_results if r.success and self._passes_reachability_validator(r)]


The expensive validator runs more than it needs to, in two ways:

Already-full envs are re-validated every batch. _solve_and_store loops up to max_placement_attempts batches, sizing each batch by the least-full env (max_missing = target - min(available)). But this line computes valid_results — and thus runs the IK validator — for every env, including ones where missing <= 0 (the else branch on line 231 just bumps a counter and stores nothing). So an env that filled in batch 0 keeps paying a full round of cuRobo IK solves on every later batch until the slowest env catches up.

More candidates validated than stored. Even for a not-yet-full env you validate all layouts_per_env candidates, then keep only valid_results[:missing].

Suggest gating on missing before validating and stopping once missing are accepted, e.g.:

missing = target_num_layouts_per_env - self._env_pools[cur_env].available if missing <= 0: continue # env already satisfied — don't run the validator valid_results = [] for r in env_results: if r.success and self._passes_reachability_validator(r): valid_results.append(r) if len(valid_results) >= missing: break

This keeps the total_solved/total_valid log honest for the envs that matter and can cut IK calls substantially when envs fill at different rates.

Well it shall be another improvement MR and you can add it according to your agent suggestion :)

zhx06 · 2026-07-16T01:19:59Z

@@ -203,7 +219,8 @@ def _store_env_matched_results(
        fallback_envs = []
        for cur_env in range(self._num_envs):
            env_results = ranked_results_per_env[cur_env][:layouts_per_env]


When a reachability_validator is set, produce more geometry candidates than needed so the IK gate has fallbacks in one batch — otherwise unreachable top-missing candidates force a full geometry+cuRobo re-solve every refill. Concretely: pass results_per_env = layouts_per_env * overfetch (e.g. 4×, gated on self._reachability_validator) into place_ranked_per_env, drop this [:layouts_per_env] slice so the tail is reachable, and stop validating at missing (the :223 early-stop). Geometry sampling is cheap next to a cuRobo solve, so this trades a bit of sampling for far fewer expensive batches.

zhx06 · 2026-07-16T01:19:59Z

    allow_best_loss_fallbacks: bool = True
    """Whether pooled placement may use best-loss layouts when no valid layout is found."""
+
+    reachability_validator: Callable[[PlacementResult], bool] | None = None


The type advertises a pure predicate — Callable[[PlacementResult], bool] — but the cuRobo validator supplied by the extension mutates the argument: make_standalone_ik_layout_validator stamps a required PlacementCheck.IK_REACHABLE onto result.validation_results (flipping result.success when it rejects). A caller reading only this signature won't expect the passed result to change, and the stamped .success=False then conflates "geometry failed" with "IK rejected" for any downstream consumer that inspects the layout. Worth (a) documenting the side-effect here, and (b) confirming the .success conflation is intended — an audit reader can no longer tell a geometry failure from a reachability rejection.

zhx06 · 2026-07-16T01:25:57Z

            env_results = ranked_results_per_env[cur_env][:layouts_per_env]
-            valid_results = [r for r in env_results if r.success]
+            # if reachability validator is not set, results are only checked through geometry validation
+            valid_results = [r for r in env_results if r.success and self._passes_reachability_validator(r)]


It seems _passes_reachability_validator is time-consuming. Can we check missing first to avoid extra computing?

zhx06

A few more fixable items on the core relation-solver side (the geometry path that feeds the gate), independent of cuRobo.

zhx06 · 2026-07-16T01:35:31Z

+            and getattr(init_state, "pos", None) is not None
+            and getattr(init_state, "rot", None) is not None
+        ):
+            return tuple(float(v) for v in init_state.pos), tuple(float(v) for v in init_state.rot)


Quaternion-convention bug. The return contract is rotation_xyzw, and the initial_pose branch honors it. But Isaac Lab's init_state.rot is wxyz (identity (1,0,0,0)), so returning it verbatim mislabels wxyz as xyzw — identity becomes (1,0,0,0) read as xyzw, i.e. a 180° rotation about X. The default fallback correctly uses xyzw identity (0,0,0,1), which makes the mismatch concrete. get_base_pose() feeds base_quat_xyzw straight into the IK validator, so a non-identity robot base orientation gets corrupted. Convert:

from isaaclab.utils.math import convert_quat rot_xyzw = convert_quat(torch.tensor(init_state.rot), to="xyzw") return tuple(float(v) for v in init_state.pos), tuple(float(v) for v in rot_xyzw)

are you/your agent sure Lab's init_state.rot is wxyz?

zhx06 · 2026-07-16T01:35:31Z

+        kwargs: Extra tuning forwarded verbatim to the entry point (e.g. IK thresholds).
+    """
+    for module_name in module_names or []:
+        factory = getattr(importlib.import_module(module_name), _REACHABILITY_VALIDATOR_FACTORY_ATTR, None)


import_module raises a raw ImportError when the extension's deps aren't installed (e.g. cuRobo missing), which is exactly the case the CLI help promises to handle ("make sure dev environment has curobo deps installed"). Wrap it so a missing extension is skipped or re-raised with that guidance instead of a bare ImportError:

try: module = importlib.import_module(module_name) except ImportError as e: raise RuntimeError(f"{module_name} enabled for --validate_reachability but import failed: {e}. Install its deps (e.g. cuRobo).") from e

zhx06 · 2026-07-16T01:35:31Z

+            # Pull the gate exported by an enabled extension into the pool's solve loop. resolve_*
+            # imports the extension by name, so no cuRobo import appears in core.
+            placer_params.reachability_validator = resolve_reachability_validator(
+                embodiment, ["isaaclab_arena_curobo"], **self.cfg.reachability_validator_kwargs


The extension list is hardcoded ["isaaclab_arena_curobo"] here, which quietly contradicts the registry's stated design ("the module names come from config ... no cuRobo import appears in core"). The string is fine, but consider sourcing it from a config field (e.g. self.cfg.reachability_validator_modules) so core carries no vendor name and a different backend can be wired without editing the builder.

Gate the standalone (build-time) IK reachability check on a collision-free joint solution, not just pose convergence: solve_ik_feasibility grows a require_collision_free flag that also requires cuRobo's success at the best-error seed. StandaloneIKReachability opts in and, mirroring the live planner's contact planning, disables its hand-link collision spheres so a top-down grasp is not rejected for touching its own target object. The env-coupled planner leaves the flag off, so its behavior is unchanged. Signed-off-by: Xinjie Yao <xyao@nvidia.com>

cvolkcvolk · 2026-07-16T10:21:23Z

+    ik_rot_threshold: float = 0.1,
+    device: str | torch.device | None = None,
+    stamp_results: bool = True,
+) -> Callable[[PlacementResult], bool]:


I think the cuRobo validator should remain read-only with respect to Arena-owned state such as PlacementResult. Could it simply return whether the layout is reachable and let for example PooledObjectPlacer record the result?

alexmillane

Thanks for putting this together. Great that it's working!

I've left a few comments. I think we need to investigate the class heirachy and see how we can build this into the solver such that's not passed in from the top level.

alexmillane · 2026-07-16T09:31:00Z

+The pool's solve loop calls it on each geometry-valid candidate; a candidate is stored only when the robot can reach a
+top-down grasp at every movable object, so the loop keeps solving (reject-&-refill) until every env has enough reachable layouts.
+
+Requires a CUDA GPU.


I think our whole framework requires a CUDA GPU right? Suggestion to remove this?

alexmillane · 2026-07-16T09:43:43Z

+    arena_group.add_argument(
+        "--validate_reachability",
+        action="store_true",
+        default=False,
+        help=(
+            "Gate build-time pooled placement on reachability, storing only layouts whose objects the"
+            " robot can reach (cuRobo top-down-grasp IK). isaaclab_arena_curobo is automatically imported"
+            " when this flag is set. Please make sure dev environment has curobo deps installed."
+        ),
+    )


Is there some way that we can enable this on a per-environment level so a user needn't worry about whether or not curobo validation is required? This shifts the responsibility from the person running the environment to the person creating the environment, which makes more sense to me.

alexmillane · 2026-07-16T09:44:54Z

            raise RuntimeError("scene_config must be populated with a `robot` before calling `set_joint_initial_pos`.")
        self.scene_config.robot.init_state.joint_pos.update(joint_pos)

+    def get_base_pose(self) -> tuple[tuple[float, float, float], tuple[float, float, float, float]]:


elsewhere we use our Pose object to communicate poses. Any reason to do something different here?

If you need the pose as tuples, suggestion to add a conversion to our pose object.

alexmillane · 2026-07-16T09:47:44Z

+        # Gate pooled placement on reachability on demand.
+        if self.cfg.validate_reachability and placer_params.reachability_validator is None:
+            embodiment = self.arena_env.embodiment
+            assert embodiment is not None, "--validate_reachability requires an environment with a robot embodiment."
+            # Pull the gate exported by an enabled extension into the pool's solve loop. resolve_*
+            # imports the extension by name, so no cuRobo import appears in core.
+            placer_params.reachability_validator = resolve_reachability_validator(
+                embodiment, ["isaaclab_arena_curobo"], **self.cfg.reachability_validator_kwargs
+            )


I feel like this validator (actually, all validators) should only be visible inside the solver. They should be activated in the config, and then the solver should sort out how they're exectude. Having one particular validator appearing on the top-level in env-builder seems a bit off.

Is there some reason that we need this here? One reason is that currently this is controlled by a flag. See above for my comment on that.

alexmillane · 2026-07-16T09:49:20Z

+    reachability_validator_kwargs: dict[str, Any] = field(default_factory=dict)
+    """Keyword tuning forwarded to the reachability-validator factory (e.g. IK thresholds). Ignored when
+    ``validate_reachability`` is ``False`` or a callable was set directly on the placer params."""


Are we in control of the validator, or is this a curobo thing? If this is us, could we pass the kwargs as a dataclass. A dataclass communicates valid args to the outside word. With a dict, a user has no idea what options are available. They just put some stuff in, maybe it works, maybe it crashes.

alexmillane · 2026-07-16T11:33:32Z

+        pose_ctx: Object providing ``_to_curobo_device``, ``_make_pose`` and ``logger``. A ``CuroboPlanner`` or a ``StandaloneIKReachability``.
+        ik_solver: cuRobo ``IKSolver`` exposing ``solve_batch``.


I'm a little surprised that we seem to pass in two curobo solver-related objects.

pose_ctx: CuroboPlanner

and

ik_solver: IKSolver

What's the difference here? Why do we need so planner/solvers to solve the feasibility problem?

alexmillane · 2026-07-16T11:34:58Z

+    name: str
+    center_xyz: tuple[float, float, float]
+    dims_xyz: tuple[float, float, float]
+    quat_wxyz: tuple[float, float, float, float] = (1.0, 0.0, 0.0, 0.0)


elsewhere in the framework we use xyzw. I suppose that this is because curobo uses wxyz?

I would tend to stick to the isaaclab convention within our code, and then convert at the last moment.

alexmillane · 2026-07-16T11:35:35Z

+    center_xyz: tuple[float, float, float]
+    dims_xyz: tuple[float, float, float]
+    quat_wxyz: tuple[float, float, float, float] = (1.0, 0.0, 0.0, 0.0)


Could we combine the center and orientation into one of our pose objects?

alexmillane · 2026-07-16T11:39:19Z

+def get_object_world_pose_from_layout(
+    result: PlacementResult,
+    obj: ObjectBase,
+    base_rotations: dict,
+) -> tuple[tuple[float, float, float], tuple[float, ...]]:


Maybe this is a limitation of the sovler, but I'm wondering why we're passing around base_rotations separately.

Could we not combine the rotations into the PlacementResult and treat the solution as one complete thing. Not something that we need to add some random rotations to?

Ah maybe this is because were trying to stay away from kit?

alexmillane · 2026-07-16T11:48:24Z

+    return top_down_grasp_matrix(t_R_O, q_R_O_xyzw, grasp_z_offset, align_yaw_to_object)
+
+
+class StandaloneIKReachability:


How does this class relate to the non-standalone version? Presumably, they do very similar things? Do they currently follow completely different code paths?

Is there some way we could unify them. For example the kit-requiring version can hold one of these?

xyao-nv commented Jul 15, 2026

View reviewed changes

Comment thread isaaclab_arena/environments/arena_env_builder.py Outdated

xyao-nv commented Jul 15, 2026

View reviewed changes

Comment thread isaaclab_arena_curobo/curobo_ik_utils.py Outdated

xyao-nv added 11 commits July 15, 2026 15:06

curobo frame

73b51ab

unify

d6c8bd5

remove motion gen

a1d63d0

wip

2211c24

cleanup

6bd9279

cleanup

7c9916f

wip

314de34

xyao-nv force-pushed the xyao/feature/curobo_buildtime_simfree_validation branch from 3134be9 to 314de34 Compare July 15, 2026 22:14

lint

9a45de6

xyao-nv changed the title ~~Xyao/feature/curobo buildtime simfree validation~~ Add an optional build-time, sim-free gate to keep only the object layouts a robot can actually reach, using cuRobo IK (Part 2/3) Jul 15, 2026

zhx06 reviewed Jul 16, 2026

View reviewed changes

xyao-nv added 2 commits July 15, 2026 22:48

review

a4d6455

cvolkcvolk reviewed Jul 16, 2026

View reviewed changes

alexmillane reviewed Jul 16, 2026

View reviewed changes

		pose_ctx: Object providing ``_to_curobo_device``, ``_make_pose`` and ``logger``. A ``CuroboPlanner`` or a ``StandaloneIKReachability``.
		ik_solver: cuRobo ``IKSolver`` exposing ``solve_batch``.

		return top_down_grasp_matrix(t_R_O, q_R_O_xyzw, grasp_z_offset, align_yaw_to_object)


		class StandaloneIKReachability:

Uh oh!

Conversation

xyao-nv commented Jul 15, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Summary

Detailed description

How to

Note

TODO

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

xyao-nv Jul 15, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

zhx06 left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

zhx06 left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

zhx06 Jul 16, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

zhx06 left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

alexmillane left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

xyao-nv commented Jul 15, 2026 •

edited

Loading

xyao-nv Jul 15, 2026 •

edited

Loading

zhx06 Jul 16, 2026 •

edited

Loading