Joint Dependent Mesh/BBox for Embodiment by zhx06 · Pull Request #968 · isaac-sim/IsaacLab-Arena

zhx06 · 2026-07-28T22:54:48Z

Summary

Joint-dependent mesh/bbox for embodiments

Detailed description

Compute embodiment bounding boxes at configured initial joint positions
Use one combined posed link-box collision mesh for Droid and Franka
Preserve existing list-based joint-pose APIs
Enable Droid mesh collision in the Lightwheel kitchen; other embodiment configurations retain bbox collision by default

Validation comparison

Droid Lightwheel RoboCasa kitchen — 50 candidates

Before: Droid bbox

[placement] Validated 50 candidate layout(s); passed per check:
on_relation=49/50, next_to=50/50, not_next_to=50/50,
face_to=50/50, no_overlap=48/50

After: Droid mesh

[placement] Validated 50 candidate layout(s); passed per check:
on_relation=49/50, next_to=49/50, not_next_to=50/50,
face_to=50/50, no_overlap=48/50

greptile-apps · 2026-07-28T23:00:38Z

+        return PlacementGeometrySource(
+            usd_path=spawn.usd_path,
+            scale=(scale_x, scale_y, scale_z),
+            joint_pos=dict(robot.init_state.joint_pos or {}),


Initial joint pose is ignored

When a Franka or Droid embodiment receives a non-default initial_joint_pose, the reset event applies that pose while placement geometry still reads robot.init_state.joint_pos, causing relation placement to use a bounding box and collision mesh for the wrong arm configuration.

Knowledge Base Used:

Scene Composition, Affordances, and Relation Solving

Tasks and MDP Terms

This is better handle in droid/franka. set_initial_joint_pose currently only updates the reset event's default_pose.
can expand set_initial_joint_pose to also all self.set_joint_initial_pos(...) with a name→value map matching set_default_joint_pose order.

That's the right fix. Looking at both constructors, set_initial_joint_pose only writes to the event's positional default_pose list but leaves init_state.joint_pos (what get_placement_geometry_source reads) unchanged.

One ordering issue to watch in FrankaEmbodimentBase.__init__: set_initial_joint_pose is called before self.scene_config is assigned:

self.event_config = FrankaEventCfg() if initial_joint_pose is not None: self.set_initial_joint_pose(initial_joint_pose) # scene_config is still None here ... self.scene_config = FrankaSceneCfg() # assigned after

So if you expand set_initial_joint_pose to also call self.set_joint_initial_pos(...), it'll hit the assert self.scene_config is not None in the base class. Droid doesn't have this problem since self.scene_config = DroidSceneCfg() is assigned before the call.

The simplest fix for Franka is to move scene_config assignment before the joint pose call:

self.scene_config = FrankaSceneCfg() self.event_config = FrankaEventCfg() if initial_joint_pose is not None: self.set_initial_joint_pose(initial_joint_pose)

Then the expanded set_initial_joint_pose in both Franka and Droid can safely call self.set_joint_initial_pos(joint_name_map) alongside the existing default_pose update, and placement geometry will reflect the correct configuration.

greptile-apps · 2026-07-28T23:00:41Z

Greptile Summary

This PR introduces joint-dependent placement geometry and reusable collision-mesh artifacts.

Poses embodiment bounding boxes and collision meshes from configured articulation joints.
Adds persistent local and published mesh loading, validation, eviction, and export tooling.
Adds per-embodiment robot-library folders, USD articulation kinematics, visualization helpers, and extensive simulation tests.

Confidence Score: 3/5

The PR should not merge until Franka and Droid placement geometry honors constructor-supplied initial joint poses; source-aware cache invalidation should also be strengthened.

Franka and Droid can reset into a constructor-selected arm pose while their newly introduced bounding box and collision mesh are computed from a different joint mapping, causing incorrect placement geometry on a supported path.

Files Needing Attention: isaaclab_arena/embodiments/embodiment_base.py, isaaclab_arena/embodiments/franka/franka.py, isaaclab_arena/embodiments/droid/droid.py, isaaclab_arena/utils/collision_mesh_store.py

Important Files Changed

Filename	Overview
isaaclab_arena/embodiments/embodiment_base.py	Routes embodiment geometry through joint-aware helpers, but reads a different joint-pose source than Franka and Droid constructor overrides.
isaaclab_arena/utils/usd_articulation.py	Adds offline USD articulation forward kinematics for revolute, prismatic, fixed, instanced, and closed-loop geometry.
isaaclab_arena/utils/usd_helpers.py	Adds cached posed-mesh extraction and posed Gprim bounding-box computation.
isaaclab_arena/utils/collision_mesh_store.py	Adds persistent and published mesh artifacts, though source identity does not invalidate artifacts after in-place USD updates.
isaaclab_arena/scripts/export_ready_pose_collision_meshes.py	Adds a simulation-backed exporter that deduplicates embodiment variants and reports partial failures.
isaaclab_arena/tests/test_usd_articulation.py	Adds broad articulation-kinematics and real-robot geometry coverage.
isaaclab_arena/tests/test_collision_mesh_store.py	Covers pose keys, artifact validation, scaling, atomic storage, publication, and cache trimming.

Flowchart

%%{init: {'theme': 'neutral'}}%%
flowchart LR
  Config[Embodiment scene config] --> Source[PlacementGeometrySource]
  Source --> Pose[USD articulation posing]
  Pose --> BBox[Posed bounding box]
  Pose --> Extract[Mesh extraction]
  Source --> Store{Stored artifact valid?}
  Store -->|yes| Mesh[Scaled collision mesh]
  Store -->|no| Extract
  Extract --> Cache[Local/published mesh store]
  Cache --> Mesh
  BBox --> Placement[Relation placement]
  Mesh --> Placement

_{Reviews (1): Last reviewed commit: "add joint support for robots" | Re-trigger Greptile}

arena-review-bot · 2026-07-28T23:00:41Z

+what makes a mesh reusable: embodiments spawn at their configured pose rather than at zero, so keying
+on the asset alone would store a mesh nobody asks for.
+
+Lookup order is the local cache, then the robot's own folder under ``ARENA_ROBOT_LIBRARY_DIR`` on


🟡 Does the published robot library need to ship now?

The in-process lru_cache on the posed-geometry helpers already covers the relation-solver hot path (repeated bbox queries within a run), so this disk + Nucleus layer's marginal benefit is the one-time 0.1–2.5 s extraction per fresh process. Weighed against that, it adds a publish pipeline that must be re-run on every USD/joint change, a Nucleus dependency in the placement path, and a 1 GiB ~/.cache cache that is now on by default for every user (a default-path change, not opt-in). Could we ship the in-process cache first — plus a plain local disk cache if the cross-process cost actually bites — and defer the exported-library machinery until it's shown to be a bottleneck?

arena-review-bot · 2026-07-28T23:01:13Z

🤖 Isaac Lab-Arena Review Bot

Summary

This PR makes an embodiment's placement bounding box and collision mesh reflect the robot as actually spawned — posed at its configured init_state.joint_pos via offline USD forward kinematics — instead of the arbitrary joint configuration the asset was authored in. That is a real correctness improvement for relation-based placement, and it is backed by an unusually strong test suite (PhysX ground-truth link-pose comparison, closed-loop articulations, instanced geometry, prismatic/revolute cases, LRU eviction, and stale/foreign-artifact rejection). The FK and geometry code is careful and well-documented.

Design, Boundaries & Scope

My one real question is scope, raised inline on collision_mesh_store.py: the change ships a two-tier persistence layer — a 1 GiB on-disk LRU cache plus a Nucleus-published robot library with an export/upload pipeline. The in-process lru_cache on the posed-geometry helpers already covers the relation-solver hot path within a run, so the disk + Nucleus layer only saves the one-time 0.1–2.5 s extraction per fresh process. Against that it adds ongoing maintenance (re-export on every USD/joint change), a Nucleus dependency in the placement path, and a cache that grows in every user's ~/.cache on the default path. Worth confirming that cross-process cost actually bites before taking on the exported-library machinery; the in-process cache (plus perhaps a plain local disk cache) may deliver most of the value for far less surface.

Boundaries otherwise hold: the new FK/store code is generic USD/IO utility, no robot-specific logic leaks into core, and the embodiment geometry methods stay pure (no live env).

Findings

🟡 collision_mesh_store.py — question whether the published-library + disk-LRU persistence needs to ship now, or could be deferred behind the in-process cache (inline).

Test Coverage

Excellent. New tests follow the inner/outer run_simulation_app_function pattern with deferred sim imports and land in Phase 1 (in-process persistent app, no cameras/subprocess), matching the existing sibling test. Coverage spans unit FK, real-Droid geometry, PhysX agreement, and the full store lifecycle including negative cases. No gaps worth calling out.

Verdict

Minor fixes needed — essentially ship-ready; please just weigh in on the persistence-layer scope question before merge.

qianl-nv

I have some general questions on why we need the "local/Omniverse cache for pre-computed mesh" part.

collecting the mesh for droid is only taking about 1s, it's hardly the bottlenet in the overall pipeline atm. we don't think we need to go done for the perf there using cache. Finding ways the speed up the mesh mode for Background (where we absolutely need it) is more important imo.
for embodiment, what's blocking is actually the joint-angle-based bounding box collection. unless we are confident of shipping v0.3 with both background and embodiment using mesh mode (so far it has always take forever for solver), we need a working version of background in mesh mode + embodiment in bbox mode.

qianl-nv · 2026-07-29T05:24:31Z

+        return PlacementGeometrySource(
+            usd_path=spawn.usd_path,
+            scale=(scale_x, scale_y, scale_z),
+            joint_pos=dict(robot.init_state.joint_pos or {}),


This is better handle in droid/franka. set_initial_joint_pose currently only updates the reset event's default_pose.
can expand set_initial_joint_pose to also all self.set_joint_initial_pos(...) with a name→value map matching set_default_joint_pose order.

Retire the obsolete serial no-overlap reference and its profiling controls now that the batched implementation is the only supported path. Move serial oracle to unit test and add regression testing Cleanup unused MeshPairCache data

qianl-nv

For droid/franka let's just overwrite get_collision_mesh instead of creating a new get_collsion_meshes. The return should be one mesh, this should remove a lot of downstream changes.

Please rebase the MR onto qianl/feature/mesh-optimization
Update the commit message with solver/validation logging before/after this change for the droid kitchen example

Will do another pass after above modification.

qianl-nv · 2026-08-03T13:21:09Z


-    def set_initial_joint_pose(self, initial_joint_pose: list[float]) -> None:
-        self.event_config.init_franka_arm_pose.params["default_pose"] = initial_joint_pose
+    def set_initial_joint_pose(self, initial_joint_pose: Mapping[str, float]) -> None:


can we keep this initial_joint_pose: list[float] type unchanged, and pre-define the list of joint name constants in matching order? I'd avoid unnecessary modification to existing API interface if possible

Sure, it still takes initial_joint_pose: list[float]

qianl-nv · 2026-08-03T13:21:46Z


-    def set_initial_joint_pose(self, initial_joint_pose: list[float]) -> None:
-        self.event_config.init_franka_arm_pose.params["default_pose"] = initial_joint_pose
+    def set_initial_joint_pose(self, initial_joint_pose: Mapping[str, float]) -> None:


Same as above

qianl-nv · 2026-08-03T13:23:15Z

        self,
        enable_cameras: bool = False,
        initial_pose: Pose | None = None,
-        initial_joint_pose: list[float] | None = None,


why remove this from init?

It is restored.

qianl-nv · 2026-08-03T13:27:10Z

-        """Return root-relative bounds computed from the articulation's USD geometry.
+        """Return root-relative bounds of the articulation posed at its configured joint positions.
+
+        Shared and cached across callers, so treat the result as read-only.


since _compute_local_bounding_box_from_usd_at_joint_pos results can stored in lru_caches and modification to it persist, it's best to make a copy of it in the compute_local_bounding_box_from_usd_at_joint_pos wrapper before returning, so we don't rely on the caller to not accidentally modify it.

Done. The public wrapper now constructs a new bounding box, avoiding the chance of modifying it directly.

qianl-nv · 2026-08-03T13:33:37Z

-        return self._collision_mesh
+        """Return one mesh containing all posed link boxes."""
+        # Import locally because USD/pxr is available only after simulation initialization.
+        from isaaclab_arena.utils.usd_helpers import extract_trimesh_from_usd_at_joint_pos


ditto about cache copy

Addressed. It now returns mesh.copy()

qianl-nv · 2026-08-03T13:35:58Z

+        source = self.get_placement_geometry_source()
+        return extract_trimesh_from_usd_at_joint_pos(source.usd_path, source.joint_pos, source.scale)
+
+    def get_collision_meshes(self) -> tuple[trimesh.Trimesh, ...]:


I suggest we don't return a new API get_collision_meshes. we just overwrite the get_collision_mesh in droid/franka. also can we union the extract_link_bbox_meshes_from_usd_at_joint_pos before returning so it acts as one mesh?

Done. get_collision_meshes is removed. Droid and Franka now override the existing get_collision_mesh

Signed-off-by: zhx06 <zihaox@nvidia.com>

qianl-nv

Some more nits.
Plz do one more pass in cleaning / simplying the util functions

qianl-nv · 2026-08-03T16:23:27Z



+@dataclass(frozen=True)
+class PlacementGeometrySource:


ArticulationGeometrySpec or EmbodimentGeometrySpec would be more accurate.

Addressed. Renamed to ArticulationGeometrySpec

qianl-nv · 2026-08-03T16:29:39Z

+
+"""Forward kinematics over a USD articulation's physics joints.
+
+Authored USD transforms only describe the one joint configuration an asset happens to be saved in,


Instead of explaining "how" the transform are computed, you need to explain why we need to implement this forward kinematics here (instead of re-using lab/curobo ect)

qianl-nv · 2026-08-03T16:49:14Z

    assert stage is not None, f"could not open USD: {usd_path}"
    default_prim = stage.GetDefaultPrim() or stage.GetPseudoRoot()
    return extract_trimesh_from_prim(stage, default_prim.GetPath().pathString, scale)
+


let's add a code block separation here for better grouping as this file gets long

# ----------------------------------------------------------------------------- # Joint-posed articulation geometry helpers # -----------------------------------------------------------------------------

Addressed. Section separator added.

qianl-nv · 2026-08-03T16:52:27Z

    return extract_trimesh_from_prim(stage, default_prim.GetPath().pathString, scale)
+
+
+def extract_link_bbox_meshes_from_usd_at_joint_pos(


this wrapper is not used anymore, remove

Addressed. This wrapper has been removed.

qianl-nv · 2026-08-03T16:53:02Z

+# NOTE(zihaox, 2026-07-28): Cache here rather than on the asset. Isaac Lab reaches assets through
+# EventTermCfg params, and configclass's validation walk tracks no visited set, so a trimesh held by
+# an asset sends it recursing through trimesh's internal back-references until the stack overflows.
+@functools.lru_cache(maxsize=_POSED_GEOMETRY_CACHE_SIZE)


since this is now only called from _extract_trimesh_from_usd_at_joint_pos which is also lru cached, we can remove the cache here.

Addressed. The redundant inner cache was removed.

qianl-nv · 2026-08-03T16:53:32Z

+    return trimesh.util.concatenate(meshes)
+
+
+def extract_trimesh_from_usd_at_joint_pos(


let's move the wrapper before _extract_trimesh_from_usd_at_joint_pos for better readibility

Addressed. Moved the public extract_trimesh_from_usd_at_joint_pos wrapper before the private cached implementation.

qianl-nv · 2026-08-03T17:13:52Z

+
+
+def _joint_edges(
+    stage: Usd.Stage,


stage not used

This is removed.

qianl-nv · 2026-08-03T17:17:58Z

+    if not edges:
+        return {}
+
+    rest_transforms = {


can reuse one UsdGeom.XformCache for body transforms to avoid repeatingly construct it in the for loop
can also inline and remove the one-use _local_to_world helper

xform_cache = UsdGeom.XformCache(Usd.TimeCode.Default()) rest_transforms: dict[str, np.ndarray] = {} for path in {path for edge in edges for path in (edge.parent, edge.child) if path}: prim = stage.GetPrimAtPath(path) assert prim, f"Joint references a missing prim: {path}" rest_transforms[path] = np.array(xform_cache.GetLocalToWorldTransform(prim), dtype=np.float64)

Addressed. The changes have been applied as suggested.

qianl-nv · 2026-08-03T17:19:02Z

+    values_by_path = {joint_prims[name].GetPath().pathString: value for name, value in joint_pos.items()}
+
+    edges: list[_JointEdge] = []
+    for prim in Usd.PrimRange(root_prim):


store root_sdf_path = Sdf.Path(root_path)
outside the for loop

Addressed. he root path is now stored once as root_sdf_path and stays out of the for loop.

Signed-off-by: zhx06 <zihaox@nvidia.com>

zhx06 requested review from alexmillane, cvolkcvolk, peterd-NV, qianl-nv, viiik-inside and xyao-nv as code owners July 28, 2026 22:54

greptile-apps Bot reviewed Jul 28, 2026

View reviewed changes

arena-review-bot Bot reviewed Jul 28, 2026

View reviewed changes

qianl-nv reviewed Jul 29, 2026

View reviewed changes

zhx06 force-pushed the zxiao/feature/joint_dependent_placement_geometry branch 2 times, most recently from 4e98441 to 204ab44 Compare July 30, 2026 18:25

qianl-nv added 2 commits August 3, 2026 15:15

WIP: Mesh solver benchmark and optimization

f815a50

Remove serial mesh solver profiling

7c8ede9

Retire the obsolete serial no-overlap reference and its profiling controls now that the batched implementation is the only supported path. Move serial oracle to unit test and add regression testing Cleanup unused MeshPairCache data

qianl-nv reviewed Aug 3, 2026

View reviewed changes

Comment thread isaaclab_arena/embodiments/droid/droid.py

zhx06 added 5 commits August 3, 2026 07:59

add joint support for robots

5b09b56

Signed-off-by: zhx06 <zihaox@nvidia.com>

simplify codebase

1b01a0b

Signed-off-by: zhx06 <zihaox@nvidia.com>

fix franka

b211779

Signed-off-by: zhx06 <zihaox@nvidia.com>

remove caching, keep joint_position bbox/meshes

54c2fb9

Signed-off-by: zhx06 <zihaox@nvidia.com>

address review comments

fb0fa80

Signed-off-by: zhx06 <zihaox@nvidia.com>

zhx06 force-pushed the zxiao/feature/joint_dependent_placement_geometry branch from 204ab44 to fb0fa80 Compare August 3, 2026 15:54

zhx06 changed the base branch from main to qianl/feature/mesh-optimization August 3, 2026 15:55

zhx06 changed the base branch from qianl/feature/mesh-optimization to main August 3, 2026 15:56

zhx06 changed the base branch from main to qianl/feature/mesh-optimization August 3, 2026 15:57

zhx06 changed the base branch from qianl/feature/mesh-optimization to main August 3, 2026 17:00

qianl-nv reviewed Aug 3, 2026

View reviewed changes

utils cleanup

6359d9e

Signed-off-by: zhx06 <zihaox@nvidia.com>


		"""Forward kinematics over a USD articulation's physics joints.

		Authored USD transforms only describe the one joint configuration an asset happens to be saved in,

		return extract_trimesh_from_prim(stage, default_prim.GetPath().pathString, scale)


		def extract_link_bbox_meshes_from_usd_at_joint_pos(

		return trimesh.util.concatenate(meshes)


		def extract_trimesh_from_usd_at_joint_pos(



		@dataclass(frozen=True)
		class PlacementGeometrySource:

Uh oh!

Conversation

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

Uh oh!

Summary

Detailed description

Validation comparison

Droid Lightwheel RoboCasa kitchen — 50 candidates

Before: Droid bbox

After: Droid mesh

Uh oh!

greptile-apps Bot Jul 28, 2026

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

greptile-apps Bot Jul 29, 2026

Choose a reason for hiding this comment

Uh oh!

Uh oh!

greptile-apps Bot commented Jul 28, 2026

Greptile Summary

Confidence Score: 3/5

Important Files Changed

Flowchart

Uh oh!

arena-review-bot Bot Jul 28, 2026

Choose a reason for hiding this comment

Uh oh!

arena-review-bot Bot commented Jul 28, 2026

🤖 Isaac Lab-Arena Review Bot

Summary

Design, Boundaries & Scope

Findings

Test Coverage

Verdict

Uh oh!

qianl-nv left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

qianl-nv 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

Uh oh!

zhx06 Aug 3, 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!

Uh oh!

qianl-nv 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

zhx06 commented Jul 28, 2026 •

edited

Loading

zhx06 Aug 3, 2026 •

edited

Loading