diff --git a/docs/source/api/index.rst b/docs/source/api/index.rst index 5b0cb7c8f4ce..600f114b19a5 100644 --- a/docs/source/api/index.rst +++ b/docs/source/api/index.rst @@ -87,7 +87,6 @@ The following modules are available in the ``isaaclab_contrib`` extension: controllers coupling custom_coupling - deformable mdp rl sensors diff --git a/docs/source/api/lab_contrib/isaaclab_contrib.deformable.rst b/docs/source/api/lab_contrib/isaaclab_contrib.deformable.rst deleted file mode 100644 index fc8bf8e979e4..000000000000 --- a/docs/source/api/lab_contrib/isaaclab_contrib.deformable.rst +++ /dev/null @@ -1,25 +0,0 @@ -isaaclab_contrib.deformable -=========================== - -.. automodule:: isaaclab_contrib.deformable - - .. rubric:: Classes - - .. autosummary:: - - deformable_object.DeformableObject - deformable_object_data.DeformableObjectData - -Deformable Object ------------------ - -.. autoclass:: isaaclab_contrib.deformable.deformable_object.DeformableObject - :members: - :inherited-members: - :show-inheritance: - -.. autoclass:: isaaclab_contrib.deformable.deformable_object_data.DeformableObjectData - :members: - :inherited-members: - :show-inheritance: - :exclude-members: __init__ diff --git a/docs/source/concepts/deformables.rst b/docs/source/concepts/deformables.rst index 73381f5fa667..169934fa3d35 100644 --- a/docs/source/concepts/deformables.rst +++ b/docs/source/concepts/deformables.rst @@ -48,7 +48,7 @@ use a separate asset and solver; see :doc:`/source/concepts/using_mpm`. .. note:: All three kinds are under active development. On Newton, deformables are implemented in - :mod:`isaaclab_contrib.deformable` and re-exported through :mod:`isaaclab_newton.assets`. + :mod:`isaaclab_newton.assets`, alongside rigid objects and articulations. Cable support in particular is experimental: its spawner cfg, asset class, and material defaults may still change. diff --git a/docs/source/developer-tools/extending_newton_solvers.rst b/docs/source/developer-tools/extending_newton_solvers.rst index 54165675f4d1..91dda16a4479 100644 --- a/docs/source/developer-tools/extending_newton_solvers.rst +++ b/docs/source/developer-tools/extending_newton_solvers.rst @@ -91,14 +91,12 @@ class invokes them. - Stores the simulation context, reads gravity from the simulation configuration, and creates the scene data backend. - none - * - :meth:`~isaaclab_newton.physics.NewtonManager.create_builder`, - :meth:`~isaaclab_newton.physics.NewtonManager.set_builder`, or - :meth:`~isaaclab_newton.physics.NewtonManager.instantiate_builder_from_stage` - - Creates or imports the ``ModelBuilder``. + * - :meth:`~isaaclab_newton.physics.NewtonManager.create_builder` and + :meth:`~isaaclab_newton.physics.NewtonManager.set_builder` + - The cloner imports declared prototypes, composes worlds, and supplies the completed builder. - ``_register_builder_attributes()`` (not called by ``set_builder()``) * - :meth:`~isaaclab_newton.physics.NewtonManager.start_simulation` - - Finalizes the model, then allocates states, reset masks, and Fabric - prims. + - Finalizes the model, then allocates states and reset masks. - ``_register_builder_attributes()``, ``_prepare_builder_for_finalize()`` * - :meth:`~isaaclab_newton.physics.NewtonManager.initialize_solver` diff --git a/docs/source/experimental-features/bleeding-edge.rst b/docs/source/experimental-features/bleeding-edge.rst index 48e0937f74a1..26c148fbae84 100644 --- a/docs/source/experimental-features/bleeding-edge.rst +++ b/docs/source/experimental-features/bleeding-edge.rst @@ -62,10 +62,10 @@ The following features are currently available in ``isaaclab_contrib``: - Geometric controllers for multirotor attitude, velocity, acceleration, and position tracking on SO(3) (Lee et al.). Suitable for both trajectory following and RL baselines. - API reference: :mod:`~isaaclab_contrib.controllers` - * - **Newton VBD Deformable Objects** - - Extended deformable object support using the Newton physics backend with Vertex Block Descent (VBD), - including proxy coupling between MJWarp and VBD. - - API reference: :mod:`~isaaclab_contrib.deformable`, :mod:`~isaaclab_contrib.coupling` + * - **Newton Solver Coupling** + - Proxy coupling between MJWarp and VBD. Deformable assets are implemented in + :mod:`isaaclab_newton.assets`. + - API reference: :mod:`~isaaclab_contrib.coupling` Contributing ------------ diff --git a/docs/source/how-to/run_deformable_object.rst b/docs/source/how-to/run_deformable_object.rst index 749efc827cee..bea41680c56d 100644 --- a/docs/source/how-to/run_deformable_object.rst +++ b/docs/source/how-to/run_deformable_object.rst @@ -17,7 +17,7 @@ The deformable object API and schema define/modify functions are shared across b property and material configuration classes are backend-specific. PhysX simulates soft bodies using the Finite Element Method (FEM); the Newton experimental backend uses the core VBD solver from :mod:`isaaclab_newton.physics` with the deformable object integration from -:mod:`isaaclab_contrib.deformable`. +:mod:`isaaclab_newton.assets`. The volume deformable comprises of two tetrahedral meshes -- a simulation mesh and a collision mesh. The simulation mesh is used to simulate the deformations of the soft body, while the collision mesh is used to detect collisions with other objects in the scene. For PhysX-specific details, please check the `PhysX documentation`_. diff --git a/source/isaaclab/changelog.d/deformable-prototype-bindings.rst b/source/isaaclab/changelog.d/deformable-prototype-bindings.rst new file mode 100644 index 000000000000..27b4160fb99a --- /dev/null +++ b/source/isaaclab/changelog.d/deformable-prototype-bindings.rst @@ -0,0 +1,5 @@ +Changed +^^^^^^^ + +* Bound deformable visual geometry during native import, leaving runtime publication to + attach native point buffers without re-reading the stage. diff --git a/source/isaaclab/isaaclab/assets/articulation/ordering_resolvers.py b/source/isaaclab/isaaclab/assets/articulation/ordering_resolvers.py index 8c0f5b352865..7633fdedadab 100644 --- a/source/isaaclab/isaaclab/assets/articulation/ordering_resolvers.py +++ b/source/isaaclab/isaaclab/assets/articulation/ordering_resolvers.py @@ -514,14 +514,14 @@ def _get_mjwarp_names_from_newton_usd_builder( ) -> dict[Literal["joint", "body"], tuple[str, ...]] | None: """Build a lightweight Newton prototype view with MJWarp-style articulation names.""" # NOTE: "dfs" and bodies_follow_joint_ordering=True mirror the defaults of - # Newton's ModelBuilder.add_usd. isaaclab_newton's NewtonManager calls - # add_usd (see instantiate_builder_from_stage) without passing + # Newton's ModelBuilder.add_usd. isaaclab_newton's build_source_builders calls + # add_usd without passing # joint_ordering/bodies_follow_joint_ordering, so a live Newton backend's # native order matches this emulation only because both sides currently # rely on the same Newton library defaults. The "mjwarp" convention's # same-backend identity path (active backend "newton") returns that live # order directly, assuming it equals what these hardcoded constants would - # produce. If NewtonManager ever passes explicit ordering arguments to + # produce. If the cloner ever passes explicit ordering arguments to # add_usd, these constants must be updated in lockstep or MJWarp # resolution will silently diverge from the live backend. return _get_names_from_newton_usd_builder( diff --git a/source/isaaclab/isaaclab/scene_data/deformable_discovery.py b/source/isaaclab/isaaclab/scene_data/deformable_discovery.py index 2da77fed4df9..f2c618b3159d 100644 --- a/source/isaaclab/isaaclab/scene_data/deformable_discovery.py +++ b/source/isaaclab/isaaclab/scene_data/deformable_discovery.py @@ -15,7 +15,7 @@ import numpy as np import warp as wp -from pxr import Gf, Sdf, Usd, UsdGeom +from pxr import Sdf, Usd, UsdGeom from .. import sim as sim_utils from ..cloner import ClonePlan @@ -48,14 +48,14 @@ class DeformableStageEntry: def deformable_geometry_batches( - entries: Sequence[DeformableStageEntry], points: wp.array, offsets: Sequence[int] + entries: Sequence[DeformableStageEntry], offsets: Sequence[int], *, device: str ) -> list[tuple[SceneDataFormat.Points | SceneDataFormat.WeightedPoints, dict[str, tuple[int, int]]]]: """Bind visual mesh paths to native nodal ranges or static barycentric interpolation tables. Args: entries: Declared deformable instances in native body order. - points: Flat native nodal positions [m], including any body padding. offsets: Native point offset for each entry. + device: Device for static interpolation tables; native points are bound by the producer. Returns: Native-format publications and exact visual mesh paths with their output ranges. @@ -68,7 +68,7 @@ def deformable_geometry_batches( for entry, offset in zip(entries, offsets, strict=True): key = (id(entry.vertices), id(entry.indices), id(entry.vis_vertices)) if key not in prototype_remaps: - if entry.vertex_count == entry.vis_vertex_count and np.array_equal(entry.vertices, entry.vis_vertices): + if entry.vertices is entry.vis_vertices or np.array_equal(entry.vertices, entry.vis_vertices): prototype_remaps[key] = None else: if entry.deformable_type != "volume": @@ -85,69 +85,15 @@ def deformable_geometry_batches( weights.append(prototype_weights) weighted[entry.vis_mesh_path] = (output_offset, entry.vis_vertex_count) output_offset += entry.vis_vertex_count - batches = [] - if direct: - publication = SceneDataFormat.Points() - publication.points = points - batches.append((publication, direct)) + batches = [(SceneDataFormat.Points(), direct)] if direct else [] if weighted: publication = SceneDataFormat.WeightedPoints() - publication.points = points - publication.indices = wp.array(np.concatenate(indices), dtype=wp.int32, device=points.device) - publication.weights = wp.array(np.concatenate(weights), dtype=wp.float32, device=points.device) + publication.indices = wp.array(np.concatenate(indices), dtype=wp.int32, device=device) + publication.weights = wp.array(np.concatenate(weights), dtype=wp.float32, device=device) batches.append((publication, weighted)) return batches -def _matrix4d_to_numpy(matrix: Gf.Matrix4d) -> np.ndarray: - """Convert a USD matrix to a host ``(4, 4)`` float64 array.""" - return np.array([[matrix[i][j] for j in range(4)] for i in range(4)], dtype=np.float64) - - -def _transform_points(matrix: np.ndarray, points: np.ndarray) -> np.ndarray: - """Apply a USD ``(4, 4)`` transform to ``(N, 3)`` points [m], returning float32. - - USD ``Gf.Matrix4d.Transform`` uses row-vector convention (``p @ M``). The numpy - matrix from :func:`_matrix4d_to_numpy` stores ``matrix[i, j] = usd[i][j]``, so the - matching host multiply is ``hom @ matrix``, not ``matrix @ hom``. - """ - if points.size == 0: - return np.empty((0, 3), dtype=np.float32) - ones = np.ones((points.shape[0], 1), dtype=np.float64) - hom = np.concatenate([points.astype(np.float64, copy=False), ones], axis=1) - baked = (hom @ matrix)[:, :3] - return baked.astype(np.float32, copy=False) - - -def _usd_points_to_numpy(points) -> np.ndarray: - """Convert USD point arrays to ``(N, 3)`` float32.""" - if not points: - return np.empty((0, 3), dtype=np.float32) - return np.asarray(points, dtype=np.float32).reshape(-1, 3) - - -def _get_applied_schema_names(prim) -> set[str]: - """Return applied API schema names from composed schemas and explicit ``apiSchemas`` metadata.""" - names = set(prim.GetAppliedSchemas()) - api_schemas = prim.GetMetadata("apiSchemas") - if isinstance(api_schemas, Sdf.TokenListOp): - names.update(str(token) for token in api_schemas.explicitItems) - return names - - -def _prim_has_schema(prim, schema_substring: str) -> bool: - """Return ``True`` if any applied API schema name contains ``schema_substring``.""" - return any(schema_substring in name for name in _get_applied_schema_names(prim)) - - -def _mesh_point_count(prim) -> int: - """Return the number of points authored on a Mesh or TetMesh prim.""" - if prim.GetTypeName() not in ("Mesh", "TetMesh"): - return 0 - pts = UsdGeom.PointBased(prim).GetPointsAttr().Get() - return len(pts or []) - - def _select_visual_mesh(vis_candidates: list, sim_mesh_prim, sim_vertex_count: int): """Choose the visual mesh among candidates when several non-sim meshes exist. @@ -163,18 +109,13 @@ def _select_visual_mesh(vis_candidates: list, sim_mesh_prim, sim_vertex_count: i # Rare case that the sim mesh has multiple visual candidates. sim_parent = sim_mesh_prim.GetParent() - sim_parent_path = sim_parent.GetPath() if sim_parent is not None and sim_parent.IsValid() else None def _score(prim) -> tuple: """Rank visual-mesh candidates; higher tuples are preferred by ``max``.""" name = prim.GetName().lower() name_bonus = int(any(token in name for token in ("visual", "render", "display", "proxy"))) - sibling_bonus = int( - sim_parent_path is not None - and prim.GetParent() is not None - and prim.GetParent().GetPath() == sim_parent_path - ) - count_bonus = int(_mesh_point_count(prim) == sim_vertex_count) + sibling_bonus = int(prim.GetParent() == sim_parent) + count_bonus = int(len(UsdGeom.PointBased(prim).GetPointsAttr().Get() or []) == sim_vertex_count) path = prim.GetPath().pathString return (sibling_bonus, name_bonus, count_bonus, -path.count("/"), path) @@ -198,6 +139,11 @@ def deformable_entry(root_prim: Usd.Prim) -> DeformableStageEntry | None: ) tet_prims = [prim for prim in mesh_prims if prim.GetTypeName() == "TetMesh"] mesh_prims = [prim for prim in mesh_prims if prim.GetTypeName() == "Mesh"] + sim_candidates, vis_candidates = [], [] + for prim in mesh_prims: + schemas = prim.GetPrimTypeInfo().GetAppliedAPISchemas() + is_sim = any("DeformableSimAPI" in schema for schema in schemas) + (sim_candidates if is_sim else vis_candidates).append(prim) if tet_prims: if len(tet_prims) > 1: @@ -210,20 +156,12 @@ def deformable_entry(root_prim: Usd.Prim) -> DeformableStageEntry | None: deformable_type = "volume" sim_mesh_prim = tet_prims[0] - vis_candidates = [p for p in mesh_prims if not _prim_has_schema(p, "DeformableSimAPI")] tet_mesh = UsdGeom.TetMesh(sim_mesh_prim) pts = tet_mesh.GetPointsAttr().Get() or [] raw_tet_indices = tet_mesh.GetTetVertexIndicesAttr().Get() or [] - indices = np.array([int(v) for vec4i in raw_tet_indices for v in vec4i], dtype=np.int32) + indices = np.asarray(raw_tet_indices, dtype=np.int32).reshape(-1) elif mesh_prims: deformable_type = "surface" - sim_candidates: list = [] - vis_candidates: list = [] - for prim in mesh_prims: - if _prim_has_schema(prim, "DeformableSimAPI"): - sim_candidates.append(prim) - else: - vis_candidates.append(prim) sim_mesh_prim = sim_candidates[0] if sim_candidates else mesh_prims[0] if not sim_candidates: vis_candidates = [] @@ -235,31 +173,24 @@ def deformable_entry(root_prim: Usd.Prim) -> DeformableStageEntry | None: return None vis_mesh_prim = _select_visual_mesh(vis_candidates, sim_mesh_prim, len(pts)) - vis_pts = ( - UsdGeom.Mesh(vis_mesh_prim).GetPointsAttr().Get() - if vis_mesh_prim.GetTypeName() == "Mesh" - else UsdGeom.TetMesh(vis_mesh_prim).GetPointsAttr().Get() - ) - vis_count = len(vis_pts or []) - xform_cache = UsdGeom.XformCache() - mesh_to_parent_frame = _matrix4d_to_numpy( - xform_cache.GetLocalToWorldTransform(sim_mesh_prim) - * xform_cache.GetLocalToWorldTransform(root_prim.GetParent()).GetInverse() - ) - vertices = _transform_points(mesh_to_parent_frame, _usd_points_to_numpy(pts)) - - vis_mesh_to_parent_frame = _matrix4d_to_numpy( - xform_cache.GetLocalToWorldTransform(vis_mesh_prim) - * xform_cache.GetLocalToWorldTransform(root_prim.GetParent()).GetInverse() - ) - vis_vertices = _transform_points(vis_mesh_to_parent_frame, _usd_points_to_numpy(vis_pts or [])) + parent_transform = xform_cache.GetLocalToWorldTransform(root_prim.GetParent()) + world_to_parent = parent_transform.GetInverse() + mesh_to_parent = np.asarray(xform_cache.GetLocalToWorldTransform(sim_mesh_prim) * world_to_parent) + # USD affine transforms multiply row vectors. + vertices = np.asarray(pts, dtype=np.float32).reshape(-1, 3) + vertices = (vertices @ mesh_to_parent[:3, :3] + mesh_to_parent[3, :3]).astype(np.float32) + vis_vertices = vertices + if vis_mesh_prim != sim_mesh_prim: + vis_pts = UsdGeom.PointBased(vis_mesh_prim).GetPointsAttr().Get() + vis_to_parent = np.asarray(xform_cache.GetLocalToWorldTransform(vis_mesh_prim) * world_to_parent) + vis_vertices = np.asarray(vis_pts or [], dtype=np.float32).reshape(-1, 3) + vis_vertices = (vis_vertices @ vis_to_parent[:3, :3] + vis_to_parent[3, :3]).astype(np.float32) vis_indices = np.empty(0, dtype=np.int32) if vis_mesh_prim.GetTypeName() == "Mesh": vis_indices = np.asarray(UsdGeom.Mesh(vis_mesh_prim).GetFaceVertexIndicesAttr().Get() or [], dtype=np.int32) - parent_transform = xform_cache.GetLocalToWorldTransform(root_prim.GetParent()) rotation = parent_transform.ExtractRotationQuat() return DeformableStageEntry( root_path=str(root_path), @@ -267,7 +198,7 @@ def deformable_entry(root_prim: Usd.Prim) -> DeformableStageEntry | None: vis_mesh_path=str(vis_mesh_prim.GetPath()), deformable_type=deformable_type, vertex_count=len(pts), - vis_vertex_count=vis_count, + vis_vertex_count=len(vis_vertices), vertices=vertices, indices=indices, vis_vertices=vis_vertices, diff --git a/source/isaaclab/isaaclab/scene_data/deformable_vis_remap.py b/source/isaaclab/isaaclab/scene_data/deformable_vis_remap.py index ac7a9189cd83..ed17a5444112 100644 --- a/source/isaaclab/isaaclab/scene_data/deformable_vis_remap.py +++ b/source/isaaclab/isaaclab/scene_data/deformable_vis_remap.py @@ -15,7 +15,6 @@ logger = logging.getLogger(__name__) -_BARY_EPS = 1e-5 _SPATIAL_ACCEL_MIN_OPS = 10_000 @@ -50,31 +49,6 @@ def _det3( return m00 * (m11 * m22 - m12 * m21) - m01 * (m10 * m22 - m12 * m20) + m02 * (m10 * m21 - m11 * m20) -@wp.func -def _tet_barycentric_weights(a: wp.vec3f, b: wp.vec3f, c: wp.vec3f, d: wp.vec3f, point: wp.vec3f) -> wp.vec4f: - """Return barycentric weights for ``point`` in tet ``(a,b,c,d)``, or ``(-1,...)`` when degenerate.""" - ba = b - a - ca = c - a - da = d - a - rhs = point - a - - det = _det3(ba[0], ca[0], da[0], ba[1], ca[1], da[1], ba[2], ca[2], da[2]) - if wp.abs(det) < 1.0e-12: - return wp.vec4f(-1.0, -1.0, -1.0, -1.0) - - w1 = _det3(rhs[0], ca[0], da[0], rhs[1], ca[1], da[1], rhs[2], ca[2], da[2]) / det - w2 = _det3(ba[0], rhs[0], da[0], ba[1], rhs[1], da[1], ba[2], rhs[2], da[2]) / det - w3 = _det3(ba[0], ca[0], rhs[0], ba[1], ca[1], rhs[1], ba[2], ca[2], rhs[2]) / det - w0 = 1.0 - w1 - w2 - w3 - return wp.vec4f(w0, w1, w2, w3) - - -@wp.func -def _min_bary_weight(weights: wp.vec4f) -> float: - """Return the minimum barycentric weight (inside-hull indicator).""" - return wp.min(wp.min(weights[0], weights[1]), wp.min(weights[2], weights[3])) - - @wp.func def _tet_aabb_contains_point( a: wp.vec3f, b: wp.vec3f, c: wp.vec3f, d: wp.vec3f, point: wp.vec3f, margin: float @@ -179,162 +153,6 @@ def _build_volume_vis_barycentric_remap_kernel( bary_weights[vis_idx, 3] = best_w3 -@wp.kernel -def _remap_volume_vis_positions_kernel( - sim_particle_q: wp.array(dtype=wp.vec3f), - render_particle_q: wp.array(dtype=wp.vec3f), - sim_offset: int, - render_offset: int, - tet_vertex_indices: wp.array2d(dtype=wp.int32), - bary_weights: wp.array2d(dtype=wp.float32), -): - """Interpolate sim tet nodal positions into visual mesh vertex positions.""" - vis_idx = wp.tid() - w0 = bary_weights[vis_idx, 0] - w1 = bary_weights[vis_idx, 1] - w2 = bary_weights[vis_idx, 2] - w3 = bary_weights[vis_idx, 3] - i0 = sim_offset + tet_vertex_indices[vis_idx, 0] - i1 = sim_offset + tet_vertex_indices[vis_idx, 1] - i2 = sim_offset + tet_vertex_indices[vis_idx, 2] - i3 = sim_offset + tet_vertex_indices[vis_idx, 3] - p = w0 * sim_particle_q[i0] + w1 * sim_particle_q[i1] + w2 * sim_particle_q[i2] + w3 * sim_particle_q[i3] - render_particle_q[render_offset + vis_idx] = p - - -@wp.kernel -def _batch_remap_volume_vis_positions_kernel( - sim_particle_q: wp.array(dtype=wp.vec3f), - render_particle_q: wp.array(dtype=wp.vec3f), - entity_ids: wp.array(dtype=wp.int32), - sim_offsets: wp.array(dtype=wp.int32), - render_offsets: wp.array(dtype=wp.int32), - vis_counts: wp.array(dtype=wp.int32), - vis_prefix: wp.array(dtype=wp.int32), - tet_vertex_indices: wp.array2d(dtype=wp.int32), - bary_weights: wp.array2d(dtype=wp.float32), -): - """Batched barycentric remap for multiple volume shadow entities.""" - vis_idx = wp.tid() - entity_id = entity_ids[vis_idx] - local_vis = vis_idx - vis_prefix[entity_id] - w0 = bary_weights[local_vis, 0] - w1 = bary_weights[local_vis, 1] - w2 = bary_weights[local_vis, 2] - w3 = bary_weights[local_vis, 3] - sim_offset = sim_offsets[entity_id] - render_offset = render_offsets[entity_id] - i0 = sim_offset + tet_vertex_indices[local_vis, 0] - i1 = sim_offset + tet_vertex_indices[local_vis, 1] - i2 = sim_offset + tet_vertex_indices[local_vis, 2] - i3 = sim_offset + tet_vertex_indices[local_vis, 3] - p = w0 * sim_particle_q[i0] + w1 * sim_particle_q[i1] + w2 * sim_particle_q[i2] + w3 * sim_particle_q[i3] - render_particle_q[render_offset + local_vis] = p - - -@wp.kernel -def _batch_copy_particle_slices_kernel( - src: wp.array(dtype=wp.vec3f), - dst: wp.array(dtype=wp.vec3f), - entity_ids: wp.array(dtype=wp.int32), - src_offsets: wp.array(dtype=wp.int32), - dst_offsets: wp.array(dtype=wp.int32), - counts: wp.array(dtype=wp.int32), - count_prefix: wp.array(dtype=wp.int32), -): - """Copy contiguous sim particle slices into render slots for multiple entities.""" - tid = wp.tid() - entity_id = entity_ids[tid] - local_idx = tid - count_prefix[entity_id] - dst[dst_offsets[entity_id] + local_idx] = src[src_offsets[entity_id] + local_idx] - - -def launch_volume_vis_remap( - sim_particle_q: wp.array(dtype=wp.vec3f), - render_particle_q: wp.array(dtype=wp.vec3f), - sim_offset: int, - render_offset: int, - remap: VolumeVisRemap, -) -> None: - """Barycentrically interpolate sim tet nodes into visual render slots. - - Args: - sim_particle_q: Live sim particle positions [m], shape ``[sim_count]``, ``wp.vec3f``. - render_particle_q: Shadow render buffer [m], shape ``[render_count]``, ``wp.vec3f``. - sim_offset: Starting index of this body's sim particles in ``sim_particle_q``. - render_offset: Starting index of this body's visual particles in ``render_particle_q``. - remap: Pre-built device-resident barycentric tables for this body. - """ - vis_count = remap.tet_vertex_indices.shape[0] - wp.launch( - _remap_volume_vis_positions_kernel, - dim=vis_count, - inputs=[ - sim_particle_q, - render_particle_q, - sim_offset, - render_offset, - remap.tet_vertex_indices, - remap.bary_weights, - ], - device=sim_particle_q.device, - ) - - -def launch_batch_volume_vis_remap( - sim_particle_q: wp.array(dtype=wp.vec3f), - render_particle_q: wp.array(dtype=wp.vec3f), - entity_ids: wp.array(dtype=wp.int32), - sim_offsets: wp.array(dtype=wp.int32), - render_offsets: wp.array(dtype=wp.int32), - vis_counts: wp.array(dtype=wp.int32), - vis_prefix: wp.array(dtype=wp.int32), - tet_vertex_indices: wp.array2d(dtype=wp.int32), - bary_weights: wp.array2d(dtype=wp.float32), -) -> None: - """Barycentrically remap multiple volume entities in one launch.""" - total_vis = int(entity_ids.shape[0]) - if total_vis == 0: - return - wp.launch( - _batch_remap_volume_vis_positions_kernel, - dim=total_vis, - inputs=[ - sim_particle_q, - render_particle_q, - entity_ids, - sim_offsets, - render_offsets, - vis_counts, - vis_prefix, - tet_vertex_indices, - bary_weights, - ], - device=sim_particle_q.device, - ) - - -def launch_batch_particle_slice_copy( - src: wp.array(dtype=wp.vec3f), - dst: wp.array(dtype=wp.vec3f), - entity_ids: wp.array(dtype=wp.int32), - src_offsets: wp.array(dtype=wp.int32), - dst_offsets: wp.array(dtype=wp.int32), - counts: wp.array(dtype=wp.int32), - count_prefix: wp.array(dtype=wp.int32), -) -> None: - """Copy multiple contiguous particle slices from ``src`` into ``dst`` in one launch.""" - total = int(entity_ids.shape[0]) - if total == 0: - return - wp.launch( - _batch_copy_particle_slices_kernel, - dim=total, - inputs=[src, dst, entity_ids, src_offsets, dst_offsets, counts, count_prefix], - device=src.device, - ) - - def build_volume_vis_barycentric_remap( sim_vertices: np.ndarray, tet_indices: np.ndarray, diff --git a/source/isaaclab/test/scene_data/test_deformable_vis_remap.py b/source/isaaclab/test/scene_data/test_deformable_vis_remap.py index 2276adb57cd8..42c950d00951 100644 --- a/source/isaaclab/test/scene_data/test_deformable_vis_remap.py +++ b/source/isaaclab/test/scene_data/test_deformable_vis_remap.py @@ -11,10 +11,7 @@ import pytest import warp as wp -from isaaclab.scene_data.deformable_vis_remap import ( - build_volume_vis_barycentric_remap, - launch_volume_vis_remap, -) +from isaaclab.scene_data.deformable_vis_remap import build_volume_vis_barycentric_remap pytestmark = pytest.mark.usefixtures("wp_init") @@ -62,69 +59,3 @@ def test_build_volume_vis_barycentric_remap_clamps_outside_hull_vertex(caplog): assert remap is not None assert any("clamped" in record.message for record in caplog.records) - - -def test_launch_volume_vis_remap_interpolates_sim_into_render_buffer(): - remap = build_volume_vis_barycentric_remap( - np.array([[0.0, 0.0, 0.0], [1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]], dtype=np.float32), - np.array([0, 1, 2, 3], dtype=np.int32), - np.array([[0.25, 0.25, 0.25]], dtype=np.float32), - ) - assert remap is not None - - sim_q = wp.array( - [ - wp.vec3(0.0, 0.0, 0.0), - wp.vec3(1.0, 0.0, 0.0), - wp.vec3(0.0, 1.0, 0.0), - wp.vec3(0.0, 0.0, 1.0), - ], - dtype=wp.vec3f, - device="cpu", - ) - render_q = wp.zeros(1, dtype=wp.vec3f, device="cpu") - launch_volume_vis_remap(sim_q, render_q, 0, 0, remap) - result = render_q.numpy()[0] - np.testing.assert_allclose(result, [0.25, 0.25, 0.25], atol=1e-4) - - -def test_launch_volume_vis_remap_reuses_device_arrays(): - remap = build_volume_vis_barycentric_remap( - np.array([[0.0, 0.0, 0.0], [1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]], dtype=np.float32), - np.array([0, 1, 2, 3], dtype=np.int32), - np.array([[0.25, 0.25, 0.25]], dtype=np.float32), - ) - assert remap is not None - - tet_ptr = remap.tet_vertex_indices.ptr - bary_ptr = remap.bary_weights.ptr - - sim_q = wp.array( - [ - wp.vec3(0.0, 0.0, 0.0), - wp.vec3(1.0, 0.0, 0.0), - wp.vec3(0.0, 1.0, 0.0), - wp.vec3(0.0, 0.0, 1.0), - ], - dtype=wp.vec3f, - device="cpu", - ) - render_q = wp.zeros(1, dtype=wp.vec3f, device="cpu") - - launch_volume_vis_remap(sim_q, render_q, 0, 0, remap) - - sim_q = wp.array( - [ - wp.vec3(1.0, 0.0, 0.0), - wp.vec3(2.0, 0.0, 0.0), - wp.vec3(1.0, 1.0, 0.0), - wp.vec3(1.0, 0.0, 1.0), - ], - dtype=wp.vec3f, - device="cpu", - ) - launch_volume_vis_remap(sim_q, render_q, 0, 0, remap) - - assert remap.tet_vertex_indices.ptr == tet_ptr - assert remap.bary_weights.ptr == bary_ptr - np.testing.assert_allclose(render_q.numpy()[0], [1.25, 0.25, 0.25], atol=1e-4) diff --git a/source/isaaclab/test/sim/test_simulation_context.py b/source/isaaclab/test/sim/test_simulation_context.py index d066ded512ba..48b4d7223007 100644 --- a/source/isaaclab/test/sim/test_simulation_context.py +++ b/source/isaaclab/test/sim/test_simulation_context.py @@ -25,6 +25,8 @@ import omni.timeline import isaaclab.sim as sim_utils +from isaaclab.assets import AssetBaseCfg +from isaaclab.cloner import CloneCfg, clone_plan_from_env_0, replicate from isaaclab.physics import PhysicsEvent from isaaclab.sim import SimulationCfg, SimulationContext @@ -196,12 +198,17 @@ def test_timeline_play_stop(monkeypatch): sim = SimulationContext() scene_data = sim.physics_manager.get_scene_data_backend() publication = scene_data.transforms - cube_cfg = sim_utils.CuboidCfg( - size=(0.1, 0.1, 0.1), - rigid_props=sim_utils.UsdPhysicsRigidBodyCfg(), - collision_props=sim_utils.UsdPhysicsCollisionCfg(), + cube_cfg = AssetBaseCfg( + prim_path="/World/Cube", + spawn=sim_utils.CuboidCfg( + size=(0.1, 0.1, 0.1), + rigid_props=sim_utils.UsdPhysicsRigidBodyCfg(), + collision_props=sim_utils.UsdPhysicsCollisionCfg(), + ), ) - cube_cfg.func("/World/Cube", cube_cfg) + plan = clone_plan_from_env_0(CloneCfg(), (cube_cfg,), 1, 0.0) + cube_cfg.class_type(cube_cfg) + replicate(plan) # initially simulation should be stopped assert sim.is_stopped() diff --git a/source/isaaclab_contrib/changelog.d/newton-deformable-ownership.major.rst b/source/isaaclab_contrib/changelog.d/newton-deformable-ownership.major.rst new file mode 100644 index 000000000000..aebeebc92d31 --- /dev/null +++ b/source/isaaclab_contrib/changelog.d/newton-deformable-ownership.major.rst @@ -0,0 +1,6 @@ +Removed +^^^^^^^ + +* **Breaking:** Removed ``isaaclab_contrib.deformable`` after moving the implementation to Newton. + Import ``DeformableObject`` and ``DeformableObjectData`` from :mod:`isaaclab_newton.assets` instead, + or use the backend-independent :class:`isaaclab.assets.DeformableObject`. diff --git a/source/isaaclab_contrib/isaaclab_contrib/deformable/__init__.py b/source/isaaclab_contrib/isaaclab_contrib/deformable/__init__.py deleted file mode 100644 index 02c5059784e6..000000000000 --- a/source/isaaclab_contrib/isaaclab_contrib/deformable/__init__.py +++ /dev/null @@ -1,13 +0,0 @@ -# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). -# All rights reserved. -# -# SPDX-License-Identifier: BSD-3-Clause - -"""Sub-package for externally contributed assets. - -This package contains contributed code that depends on Isaac Lab's public API but is not required for core functionality. This includes implementations of Newton solvers for deformables. -""" - -from isaaclab.utils.module import lazy_export - -lazy_export() diff --git a/source/isaaclab_contrib/isaaclab_contrib/deformable/__init__.pyi b/source/isaaclab_contrib/isaaclab_contrib/deformable/__init__.pyi deleted file mode 100644 index 7b61168e4d9a..000000000000 --- a/source/isaaclab_contrib/isaaclab_contrib/deformable/__init__.pyi +++ /dev/null @@ -1,12 +0,0 @@ -# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). -# All rights reserved. -# -# SPDX-License-Identifier: BSD-3-Clause - -__all__ = [ - "DeformableObject", - "DeformableObjectData", -] - -from .deformable_object import DeformableObject -from .deformable_object_data import DeformableObjectData diff --git a/source/isaaclab_contrib/isaaclab_contrib/deformable/deformable_object.py b/source/isaaclab_contrib/isaaclab_contrib/deformable/deformable_object.py deleted file mode 100644 index 8a4d272eddec..000000000000 --- a/source/isaaclab_contrib/isaaclab_contrib/deformable/deformable_object.py +++ /dev/null @@ -1,970 +0,0 @@ -# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). -# All rights reserved. -# -# SPDX-License-Identifier: BSD-3-Clause - -from __future__ import annotations - -import logging -from collections.abc import Sequence -from dataclasses import dataclass, field -from typing import TYPE_CHECKING - -import numpy as np -import torch -import warp as wp -from isaaclab_newton.physics import NewtonManager as SimulationManager - -import isaaclab.sim as sim_utils -from isaaclab.assets.deformable_object.base_deformable_object import BaseDeformableObject -from isaaclab.markers import VisualizationMarkers -from isaaclab.physics import PhysicsEvent -from isaaclab.scene_data.deformable_vis_remap import VolumeVisRemap, build_volume_vis_barycentric_remap -from isaaclab.utils.warp import ProxyArray - -from .deformable_object_data import DeformableObjectData -from .kernels import ( - compute_nodal_state_w, - enforce_kinematic_targets, - scatter_particles_state_vec6f_mask, - scatter_particles_vec3f_index, - scatter_particles_vec3f_mask, - set_kinematic_flags_to_one, - vec6f, - write_nodal_kinematic_target_index, - write_nodal_kinematic_target_mask, -) - - -@dataclass -class DeformableRegistryEntry: - """Entry in the deformable body registry. - - Registered by :class:`DeformableObject` during ``__init__``, consumed by - the Newton clone context inside the per-world ``begin_world``/``end_world`` loop. - After replication, ``particle_offsets`` and ``particles_per_body`` are filled in - so the asset can bind to the correct particle ranges. - """ - - prim_path: str - sim_mesh_prim_path: str - vis_mesh_prim_path: str - vertices: list - indices: list - init_pos: tuple[float, float, float] - init_rot: tuple[float, float, float, float] # (x, y, z, w) - deformable_type: str | None = None # "volume" or "surface" - # Cloth params - density: float = 1.0 - tri_ke: float = 1e4 - tri_ka: float = 1e4 - tri_kd: float = 1.5e-6 - edge_ke: float = 5.0 - edge_kd: float = 1e-2 - particle_radius: float = 0.008 - # Tet params - k_mu: float = 1e5 - k_lambda: float = 1e5 - k_damp: float = 0.0 - # Filled by the Newton clone context: - particle_offsets: list[int] = field(default_factory=list) - particles_per_body: int = 0 - volume_vis_remap: VolumeVisRemap | None = None - """Prototype interpolation tables when visual vertices differ from native simulation nodes.""" - - -if TYPE_CHECKING: - from isaaclab.assets.deformable_object.deformable_object_cfg import DeformableObjectCfg - -logger = logging.getLogger(__name__) - - -def add_deformable_entry_to_builder( - builder, - entry: DeformableRegistryEntry, - env_idx: int, - env_position: np.ndarray, - env_rotation: np.ndarray, -) -> None: - """Add a deformable registry entry to a Newton ``ModelBuilder`` for one environment. - - Depending on the deformable type (``"volume"`` or ``"surface"``), calls - ``builder.add_soft_mesh()`` or ``builder.add_cloth_mesh()`` with the mesh - data and material properties stored in the registry entry. - - Also records the particle offset for the instance and, on the first - environment, records the per-body particle count. - - Args: - builder: The Newton ``ModelBuilder``. - entry: A :class:`DeformableRegistryEntry` with mesh data and config. - env_idx: The environment index. - env_position: World position [x, y, z] [m] for this environment. - env_rotation: World orientation as quaternion ``(x, y, z, w)`` for this environment. - """ - if env_idx == 0: - entry.particle_offsets.clear() - entry.particles_per_body = 0 - - before_count = getattr(builder, "particle_count", 0) - - env_pos = wp.vec3(float(env_position[0]), float(env_position[1]), float(env_position[2])) - env_rot = wp.quat( - float(env_rotation[0]), - float(env_rotation[1]), - float(env_rotation[2]), - float(env_rotation[3]), - ) - init_pos = wp.vec3(float(entry.init_pos[0]), float(entry.init_pos[1]), float(entry.init_pos[2])) - init_rot = wp.quat( - float(entry.init_rot[0]), - float(entry.init_rot[1]), - float(entry.init_rot[2]), - float(entry.init_rot[3]), - ) - body_pos = env_pos + wp.quat_rotate(env_rot, init_pos) - body_rot = env_rot * init_rot - - if entry.deformable_type == "volume": - builder.add_soft_mesh( - pos=body_pos, - rot=body_rot, - scale=1.0, - vel=wp.vec3(0.0, 0.0, 0.0), - vertices=entry.vertices, - indices=entry.indices, - density=entry.density, - k_mu=entry.k_mu, - k_lambda=entry.k_lambda, - k_damp=entry.k_damp, - particle_radius=entry.particle_radius, - ) - elif entry.deformable_type == "surface": - builder.add_cloth_mesh( - pos=body_pos, - rot=body_rot, - scale=1.0, - vel=wp.vec3(0.0, 0.0, 0.0), - vertices=entry.vertices, - indices=entry.indices, - density=entry.density, - tri_ke=entry.tri_ke, - tri_ka=entry.tri_ka, - tri_kd=entry.tri_kd, - edge_ke=entry.edge_ke, - edge_kd=entry.edge_kd, - particle_radius=entry.particle_radius, - ) - else: - raise ValueError( - f"Invalid deformable type '{entry.deformable_type}' for registry entry with prim path '{entry.prim_path}'" - ) - - after_count = getattr(builder, "particle_count", 0) - delta = after_count - before_count - - entry.particle_offsets.append(before_count) - if env_idx == 0: - entry.particles_per_body = delta - elif entry.particles_per_body != delta: - raise RuntimeError( - f"Deformable body '{entry.prim_path}' produced {delta} particles in env {env_idx}, " - f"but env 0 produced {entry.particles_per_body}." - ) - - -def add_registered_deformables_to_builder( - builder, - world_idx: int, - env_position: np.ndarray, - env_rotation: np.ndarray, -) -> None: - """Add all registered deformable entries to one Newton builder world.""" - for entry in SimulationManager._deformable_registry: - add_deformable_entry_to_builder(builder, entry, world_idx, env_position, env_rotation) - - -def install_deformable_builder_hooks() -> None: - """Install deformable builder hooks without removing hooks owned by other extensions.""" - SimulationManager._deformable_registry = [] - if not hasattr(SimulationManager, "_per_world_builder_hooks"): - SimulationManager._per_world_builder_hooks = [] - if add_registered_deformables_to_builder not in SimulationManager._per_world_builder_hooks: - SimulationManager._per_world_builder_hooks.append(add_registered_deformables_to_builder) - - -def clear_deformable_builder_hooks() -> None: - """Clear deformable registry state and remove only deformable-owned builder hooks.""" - SimulationManager._deformable_registry = [] - if hasattr(SimulationManager, "_per_world_builder_hooks"): - SimulationManager._per_world_builder_hooks = [ - hook - for hook in SimulationManager._per_world_builder_hooks - if hook is not add_registered_deformables_to_builder - ] - - -class DeformableObject(BaseDeformableObject): - """A deformable object asset class (Newton backend). - - This class manages cloth/deformable bodies in the Newton physics engine. Newton stores all - particles in flat arrays (``state.particle_q``, ``state.particle_qd``). This class builds - a per-instance indexing layer on top of those flat arrays, enabling the standard - :class:`BaseDeformableObject` interface for reading/writing nodal state. - - The cloth mesh is added to the Newton :class:`ModelBuilder` during the ``MODEL_INIT`` phase. - The mesh data is read from the USD prim at :attr:`cfg.prim_path`, and cloth simulation - parameters (density, stiffness, etc.) come from :attr:`DeformableObjectCfg`. - """ - - cfg: DeformableObjectCfg - """Configuration instance for the deformable object.""" - - __backend_name__: str = "newton" - """The name of the backend for the deformable object.""" - - def __init__(self, cfg: DeformableObjectCfg): - """Initialize the deformable object. - - Args: - cfg: A configuration instance. - """ - super().__init__(cfg) - - # initialize deformable type to None, should be set to either surface or volume on initialization - self._deformable_type: str | None = None - - # Read mesh from the spawned USD prim and register in the deformable registry. - self._registry_entry = self._register_deformable() - - # Register custom vec6f type for nodal state validation. - self._DTYPE_TO_TORCH_TRAILING_DIMS = {**self._DTYPE_TO_TORCH_TRAILING_DIMS, vec6f: (6,)} - - """ - Properties - """ - - @property - def data(self) -> DeformableObjectData: - return self._data - - @property - def num_instances(self) -> int: - return self._num_instances - - @property - def num_bodies(self) -> int: - """Number of bodies in the asset. - - This is always 1 since each object is a single deformable body. - """ - return 1 - - @property - def max_sim_vertices_per_body(self) -> int: - """The maximum number of simulation mesh vertices per deformable body.""" - return self._particles_per_body - - """ - Operations. - """ - - def reset(self, env_ids: Sequence[int] | None = None, env_mask: wp.array | None = None) -> None: - """Reset the deformable object. - - No-op to match the PhysX deformable object convention. - - Args: - env_ids: Environment indices. If None, then all indices are used. - env_mask: Environment mask. If None, then all the instances are updated. - Shape is (num_instances,). - """ - pass - - def write_data_to_sim(self): - """Apply kinematic targets to the Newton simulation. - - Reads the stored kinematic target buffer and enforces it on particles: - kinematic particles (flag=0) get inv_mass=0, particle_flags=0, target position, - and zero velocity; free particles (flag=1) get their original inv_mass and - particle_flags=1 (ACTIVE) restored. - - Writes to both ``state_0`` and ``state_1`` so kinematic positions survive - the state swaps that happen between substeps. - """ - if ( - self._data.nodal_kinematic_target is None - or self._default_particle_inv_mass is None - or self._default_particle_flags is None - ): - return - - model = SimulationManager.get_model() - if model is None: - return - - for state in self._iter_particle_states(): - wp.launch( - enforce_kinematic_targets, - dim=(self._num_instances, self._particles_per_body), - inputs=[ - self._data.nodal_kinematic_target.warp, - self._particle_offsets, - self._default_particle_inv_mass, - self._default_particle_flags, - ], - outputs=[ - state.particle_q, - state.particle_qd, - model.particle_inv_mass, - model.particle_flags, - ], - device=self.device, - ) - - def update(self, dt: float): - self._data.update(dt) - - """ - Operations - Write to simulation. - """ - - def write_nodal_pos_to_sim_index( - self, - nodal_pos: torch.Tensor | wp.array | ProxyArray, - env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, - full_data: bool = False, - ) -> None: - """Set the nodal positions over selected environment indices into the simulation. - - Args: - nodal_pos: Nodal positions in simulation frame [m]. - Shape is (len(env_ids), max_sim_vertices_per_body, 3) - or (num_instances, max_sim_vertices_per_body, 3). - env_ids: Environment indices. If None, then all indices are used. - full_data: Whether to expect full data. Defaults to False. - """ - env_ids = self._resolve_env_ids(env_ids) - if isinstance(nodal_pos, ProxyArray): - nodal_pos = nodal_pos.warp - if full_data: - self.assert_shape_and_dtype( - nodal_pos, (self.num_instances, self._particles_per_body), wp.vec3f, "nodal_pos" - ) - else: - self.assert_shape_and_dtype(nodal_pos, (env_ids.shape[0], self._particles_per_body), wp.vec3f, "nodal_pos") - if isinstance(nodal_pos, torch.Tensor): - nodal_pos = wp.from_torch(nodal_pos.contiguous(), dtype=wp.vec3f) - - for state in self._iter_particle_states(): - wp.launch( - scatter_particles_vec3f_index, - dim=(env_ids.shape[0], self._particles_per_body), - inputs=[nodal_pos, env_ids, self._particle_offsets, full_data], - outputs=[state.particle_q], - device=self.device, - ) - - SimulationManager._mark_particles_dirty() - self._invalidate_nodal_pos_cache() - - def write_nodal_velocity_to_sim_index( - self, - nodal_vel: torch.Tensor | wp.array | ProxyArray, - env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, - full_data: bool = False, - ) -> None: - """Set the nodal velocity over selected environment indices into the simulation. - - Args: - nodal_vel: Nodal velocities in simulation frame [m/s]. - Shape is (len(env_ids), max_sim_vertices_per_body, 3) - or (num_instances, max_sim_vertices_per_body, 3). - env_ids: Environment indices. If None, then all indices are used. - full_data: Whether to expect full data. Defaults to False. - """ - env_ids = self._resolve_env_ids(env_ids) - if isinstance(nodal_vel, ProxyArray): - nodal_vel = nodal_vel.warp - if full_data: - self.assert_shape_and_dtype( - nodal_vel, (self.num_instances, self._particles_per_body), wp.vec3f, "nodal_vel" - ) - else: - self.assert_shape_and_dtype(nodal_vel, (env_ids.shape[0], self._particles_per_body), wp.vec3f, "nodal_vel") - if isinstance(nodal_vel, torch.Tensor): - nodal_vel = wp.from_torch(nodal_vel.contiguous(), dtype=wp.vec3f) - - for state in self._iter_particle_states(): - wp.launch( - scatter_particles_vec3f_index, - dim=(env_ids.shape[0], self._particles_per_body), - inputs=[nodal_vel, env_ids, self._particle_offsets, full_data], - outputs=[state.particle_qd], - device=self.device, - ) - - self._invalidate_nodal_vel_cache() - - def write_nodal_kinematic_target_to_sim_index( - self, - targets: torch.Tensor | wp.array | ProxyArray, - env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, - full_data: bool = False, - ) -> None: - """Set the kinematic targets of the simulation mesh for the deformable bodies. - - Newton has no native kinematic target API. Instead: - - Kinematic (flag=0.0): set ``particle_inv_mass`` to 0, write target pos, zero vel - - Free (flag=1.0): restore original ``particle_inv_mass`` - - Args: - targets: The kinematic targets comprising of nodal positions and flags [m]. - Shape is (len(env_ids), max_sim_vertices_per_body, 4) - or (num_instances, max_sim_vertices_per_body, 4). - env_ids: Environment indices. If None, then all indices are used. - full_data: Whether to expect full data. Defaults to False. - """ - env_ids = self._resolve_env_ids(env_ids) - if isinstance(targets, ProxyArray): - targets = targets.warp - if full_data: - self.assert_shape_and_dtype(targets, (self.num_instances, self._particles_per_body), wp.vec4f, "targets") - else: - self.assert_shape_and_dtype(targets, (env_ids.shape[0], self._particles_per_body), wp.vec4f, "targets") - if isinstance(targets, torch.Tensor): - if targets.dim() == 2: - targets = targets.unsqueeze(0) - targets = wp.from_torch(targets.contiguous(), dtype=wp.vec4f) - - # Store kinematic targets in our data buffer - if self._data.nodal_kinematic_target is not None: - wp.launch( - write_nodal_kinematic_target_index, - dim=(env_ids.shape[0], self._particles_per_body), - inputs=[targets, env_ids, full_data], - outputs=[self._data.nodal_kinematic_target.warp], - device=self.device, - ) - - """ - Operations - Write to simulation (mask variants). - """ - - def write_nodal_state_to_sim_mask( - self, - nodal_state: torch.Tensor | wp.array | ProxyArray, - env_mask: wp.array | torch.Tensor | None = None, - ) -> None: - """Set the nodal state over selected environment mask into the simulation. - - Args: - nodal_state: Nodal state in simulation frame [m, m/s]. - Shape is (num_instances, max_sim_vertices_per_body, 6). - env_mask: Environment mask. If None, then all indices are used. - Shape is (num_instances,). - """ - env_mask = self._resolve_mask(env_mask, self._ALL_ENV_MASK) - if isinstance(nodal_state, ProxyArray): - nodal_state = nodal_state.warp - self.assert_shape_and_dtype(nodal_state, (env_mask.shape[0], self._particles_per_body), vec6f, "nodal_state") - if isinstance(nodal_state, torch.Tensor): - nodal_state = wp.from_torch(nodal_state.contiguous(), dtype=vec6f) - - for state in self._iter_particle_states(): - wp.launch( - scatter_particles_state_vec6f_mask, - dim=(env_mask.shape[0], self._particles_per_body), - inputs=[nodal_state, env_mask, self._particle_offsets], - outputs=[state.particle_q, state.particle_qd], - device=self.device, - ) - - SimulationManager._mark_particles_dirty() - self._invalidate_nodal_state_cache() - - def write_nodal_pos_to_sim_mask( - self, - nodal_pos: torch.Tensor | wp.array | ProxyArray, - env_mask: wp.array | torch.Tensor | None = None, - ) -> None: - """Set the nodal positions over selected environment mask into the simulation. - - Args: - nodal_pos: Nodal positions in simulation frame [m]. - Shape is (num_instances, max_sim_vertices_per_body, 3). - env_mask: Environment mask. If None, then all indices are used. - Shape is (num_instances,). - """ - env_mask = self._resolve_mask(env_mask, self._ALL_ENV_MASK) - if isinstance(nodal_pos, ProxyArray): - nodal_pos = nodal_pos.warp - self.assert_shape_and_dtype(nodal_pos, (env_mask.shape[0], self._particles_per_body), wp.vec3f, "nodal_pos") - if isinstance(nodal_pos, torch.Tensor): - nodal_pos = wp.from_torch(nodal_pos.contiguous(), dtype=wp.vec3f) - - for state in self._iter_particle_states(): - wp.launch( - scatter_particles_vec3f_mask, - dim=(env_mask.shape[0], self._particles_per_body), - inputs=[nodal_pos, env_mask, self._particle_offsets], - outputs=[state.particle_q], - device=self.device, - ) - - SimulationManager._mark_particles_dirty() - self._invalidate_nodal_pos_cache() - - def write_nodal_velocity_to_sim_mask( - self, - nodal_vel: torch.Tensor | wp.array | ProxyArray, - env_mask: wp.array | torch.Tensor | None = None, - ) -> None: - """Set the nodal velocity over selected environment mask into the simulation. - - Args: - nodal_vel: Nodal velocities in simulation frame [m/s]. - Shape is (num_instances, max_sim_vertices_per_body, 3). - env_mask: Environment mask. If None, then all indices are used. - Shape is (num_instances,). - """ - env_mask = self._resolve_mask(env_mask, self._ALL_ENV_MASK) - if isinstance(nodal_vel, ProxyArray): - nodal_vel = nodal_vel.warp - self.assert_shape_and_dtype(nodal_vel, (env_mask.shape[0], self._particles_per_body), wp.vec3f, "nodal_vel") - if isinstance(nodal_vel, torch.Tensor): - nodal_vel = wp.from_torch(nodal_vel.contiguous(), dtype=wp.vec3f) - - for state in self._iter_particle_states(): - wp.launch( - scatter_particles_vec3f_mask, - dim=(env_mask.shape[0], self._particles_per_body), - inputs=[nodal_vel, env_mask, self._particle_offsets], - outputs=[state.particle_qd], - device=self.device, - ) - - self._invalidate_nodal_vel_cache() - - def write_nodal_kinematic_target_to_sim_mask( - self, - targets: torch.Tensor | wp.array | ProxyArray, - env_mask: wp.array | torch.Tensor | None = None, - ) -> None: - """Set the kinematic targets over selected environment mask into the target buffer. - - Args: - targets: The kinematic targets comprising of nodal positions and flags [m]. - Shape is (num_instances, max_sim_vertices_per_body, 4). - env_mask: Environment mask. If None, then all indices are used. - Shape is (num_instances,). - """ - env_mask = self._resolve_mask(env_mask, self._ALL_ENV_MASK) - if isinstance(targets, ProxyArray): - targets = targets.warp - self.assert_shape_and_dtype(targets, (env_mask.shape[0], self._particles_per_body), wp.vec4f, "targets") - if isinstance(targets, torch.Tensor): - targets = wp.from_torch(targets.contiguous(), dtype=wp.vec4f) - - if self._data.nodal_kinematic_target is not None: - wp.launch( - write_nodal_kinematic_target_mask, - dim=(env_mask.shape[0], self._particles_per_body), - inputs=[targets, env_mask], - outputs=[self._data.nodal_kinematic_target.warp], - device=self.device, - ) - - """ - Internal helper. - """ - - def _resolve_env_ids(self, env_ids): - """Resolve environment indices to a warp int32 array.""" - if env_ids is None or (isinstance(env_ids, slice) and env_ids == slice(None)): - return self._ALL_INDICES - elif isinstance(env_ids, list): - return wp.array(env_ids, dtype=wp.int32, device=self.device) - elif isinstance(env_ids, torch.Tensor): - return wp.from_torch(env_ids.to(torch.int32), dtype=wp.int32) - return env_ids - - def _resolve_mask(self, mask: wp.array | torch.Tensor | None, full_mask: wp.array) -> wp.array: - """Resolve an environment mask to a warp bool array.""" - if mask is None: - return full_mask - if isinstance(mask, torch.Tensor): - if mask.dtype != torch.bool: - mask = mask.to(torch.bool) - return wp.from_torch(mask, dtype=wp.bool) - return mask - - def _iter_particle_states(self): - """Yield active Newton states.""" - for state in (SimulationManager.get_state_0(), SimulationManager.get_state_1()): - if state is None: - continue - yield state - - def _invalidate_nodal_pos_cache(self) -> None: - """Invalidate cached position-derived deformable data.""" - self._data._nodal_pos_w.timestamp = -1.0 - self._data._nodal_state_w.timestamp = -1.0 - self._data._root_pos_w.timestamp = -1.0 - - def _invalidate_nodal_vel_cache(self) -> None: - """Invalidate cached velocity-derived deformable data.""" - self._data._nodal_vel_w.timestamp = -1.0 - self._data._nodal_state_w.timestamp = -1.0 - self._data._root_vel_w.timestamp = -1.0 - - def _invalidate_nodal_state_cache(self) -> None: - """Invalidate all cached nodal state data.""" - self._invalidate_nodal_pos_cache() - self._invalidate_nodal_vel_cache() - - def _register_deformable(self) -> DeformableRegistryEntry: - """Read mesh from the spawned USD prim and register in NewtonManager's deformable registry. - - Returns: - The registry entry (also stored on NewtonManager._deformable_registry). - - Note: - pxr imports are deferred to this method (not module level) so that - ``resolve_task_config`` can import the env-cfg module before Kit - starts without polluting the ``pxr`` module cache. - """ - from pxr import Gf, UsdGeom, UsdShade - - # Resolve the path of the actually-spawned template prim. This must mirror - # :meth:`AssetBase.__init__`: ``spawn_path`` is set by ``InteractiveScene`` - # when the asset is part of the template-based cloning flow (the spawn - # lives at ``/World/template//proto_asset_*`` and per-env clones at - # ``/World/envs/env_*/`` are not yet authored). For Direct envs - # that spawn straight at the cloned regex, ``spawn_path`` is unset, so - # we fall back to ``prim_path`` — which already matches the spawned prim. - # The cloned-regex ``cfg.prim_path`` is still used below to build the - # registry entry's :attr:`sim_mesh_prim_path` / :attr:`vis_mesh_prim_path` - # so post-replicate consumers resolve all per-env clones. - lookup_path = ( - self.cfg.spawn.spawn_path - if self.cfg.spawn is not None and self.cfg.spawn.spawn_path is not None - else self.cfg.prim_path - ) - template_prim = sim_utils.find_first_matching_prim(lookup_path) - if template_prim is None: - raise RuntimeError(f"Failed to find prim for expression: '{lookup_path}'.") - template_prim_path = template_prim.GetPrimPath() - - # Discover sim / visual mesh prims under the template. - # The spawner authors a visual UsdGeom.Mesh and a separate simulation mesh - # (UsdGeom.TetMesh for volume, UsdGeom.Mesh for surface) with a - # ``*DeformableSimAPI`` applied, so we split candidates by that schema. - def _is_sim_mesh(prim) -> bool: - # composed view: also sees token-authored (unregistered) schemas, unlike GetAppliedSchemas - return any("DeformableSimAPI" in api for api in prim.GetPrimTypeInfo().GetAppliedAPISchemas()) - - tet_prims = sim_utils.get_all_matching_child_prims(template_prim_path, lambda p: p.GetTypeName() == "TetMesh") - mesh_prims = sim_utils.get_all_matching_child_prims(template_prim_path, lambda p: p.GetTypeName() == "Mesh") - - if len(tet_prims) > 1: - raise ValueError( - f"Found multiple TetMesh prims under '{template_prim_path}': " - f"{[p.GetPrimPath() for p in tet_prims]}." - " Deformable body schema supports only one simulation mesh per asset." - ) - - # Pick simulation and visual mesh prims. - if len(tet_prims) == 1: - deformable_type = "volume" - mesh_prim = tet_prims[0] - vis_candidates = [p for p in mesh_prims if not _is_sim_mesh(p)] - elif len(mesh_prims) > 0: - deformable_type = "surface" - sim_candidates = [p for p in mesh_prims if _is_sim_mesh(p)] - vis_candidates = [p for p in mesh_prims if not _is_sim_mesh(p)] - if len(sim_candidates) > 1: - raise ValueError( - f"Found multiple simulation Mesh prims under '{template_prim_path}': " - f"{[p.GetPrimPath() for p in sim_candidates]}." - " Deformable body schema supports only one simulation mesh per asset." - ) - # Fall back to the single authored Mesh when no explicit sim mesh was tagged - # (legacy / self-simulated surfaces where the visual mesh *is* the sim mesh). - mesh_prim = sim_candidates[0] if sim_candidates else vis_candidates[0] - if not sim_candidates: - vis_candidates = [] # visual == sim, no separate embedding target - else: - raise ValueError( - f"Could not find any surface or volume mesh in '{template_prim_path}'. Please check asset." - ) - - # Revert visual and simulation mesh prim paths back to template-relative form for registry storage, - # since the actual prim paths will differ per world instance after replication. - # When vis_candidates is empty the visual mesh IS the simulation mesh - # (e.g. a plain surface cloth with no separate visual embedding). - vis_mesh_prim = vis_candidates[0] if vis_candidates else mesh_prim - vis_mesh_prim_path = str(vis_mesh_prim.GetPrimPath()) - vis_mesh_prim_path = self.cfg.prim_path + vis_mesh_prim_path[len(template_prim_path.pathString) :] - sim_mesh_prim_path = str(mesh_prim.GetPrimPath()) - sim_mesh_prim_path = self.cfg.prim_path + sim_mesh_prim_path[len(template_prim_path.pathString) :] - logger.info("Registered visual UsdGeom.Mesh at %s.", vis_mesh_prim_path) - - # Bake the template prim's xform directly into the vertex positions. - xform_cache = UsdGeom.XformCache() - - def _bake_points(raw_pts, prim) -> list[wp.vec3]: - mesh_to_parent_frame = ( - xform_cache.GetLocalToWorldTransform(prim) - * xform_cache.GetLocalToWorldTransform(template_prim.GetParent()).GetInverse() - ) - out = [] - for p in raw_pts: - q = mesh_to_parent_frame.Transform(Gf.Vec3d(float(p[0]), float(p[1]), float(p[2]))) - out.append(wp.vec3(float(q[0]), float(q[1]), float(q[2]))) - return out - - if deformable_type == "volume": - tet_mesh = UsdGeom.TetMesh(mesh_prim) - pts = tet_mesh.GetPointsAttr().Get() - vertices = _bake_points(pts, mesh_prim) - raw_tet_indices = tet_mesh.GetTetVertexIndicesAttr().Get() - indices = [] - for vec4i in raw_tet_indices: - indices.extend([int(vec4i[0]), int(vec4i[1]), int(vec4i[2]), int(vec4i[3])]) - logger.info("Registered UsdGeom.TetMesh: %d vertices, %d tetrahedra.", len(pts), len(indices) // 4) - else: # surface - usd_mesh = UsdGeom.Mesh(mesh_prim) - pts = usd_mesh.GetPointsAttr().Get() - vertices = _bake_points(pts, mesh_prim) - indices = list(usd_mesh.GetFaceVertexIndicesAttr().Get()) - logger.info("Registered UsdGeom.Mesh: %d vertices.", len(pts)) - - volume_vis_remap = None - if vis_mesh_prim != mesh_prim: - vis_points = UsdGeom.PointBased(vis_mesh_prim).GetPointsAttr().Get() - vis_vertices = np.asarray(_bake_points(vis_points, vis_mesh_prim), dtype=np.float32) - sim_vertices = np.asarray(vertices, dtype=np.float32) - if not np.array_equal(sim_vertices, vis_vertices): - if deformable_type != "volume": - raise ValueError(f"Surface visual topology differs from its native nodes: {template_prim_path}") - volume_vis_remap = build_volume_vis_barycentric_remap( - sim_vertices, np.asarray(indices, dtype=np.int32), vis_vertices - ) - if volume_vis_remap is None: - raise ValueError(f"Cannot bind visual vertices to deformable tetrahedra: {template_prim_path}") - - # init_pos/init_rot are already baked into the vertices by the Xform - # transform above. Setting them to identity prevents add_cloth_mesh/add_soft_mesh - # from applying them a second time. - # Note: add_deformable_entry_to_builder passes init_rot directly to - # wp.quat(x, y, z, w), so identity must be (0, 0, 0, 1) not (1, 0, 0, 0). - init_pos = (0.0, 0.0, 0.0) - init_rot = (0.0, 0.0, 0.0, 1.0) - - # Look up the bound deformable physics material - if not template_prim.HasAPI(UsdShade.MaterialBindingAPI): - raise ValueError( - f"Template prim '{template_prim_path}' must have a UsdShade.MaterialBindingAPI applied" - " with a Newton deformable physics material target." - ) - material_targets = UsdShade.MaterialBindingAPI(template_prim).GetDirectBindingRel("physics").GetTargets() - stage = template_prim.GetStage() - material_prim = None - for mat_path in material_targets: - mat_prim = stage.GetPrimAtPath(mat_path) - if mat_prim.GetAttribute("newton:density").IsValid(): - material_prim = mat_prim - break - if material_prim is None: - raise ValueError( - f"Could not find a Newton deformable physics material" - f" among the physics material targets of '{template_prim_path}'." - ) - - def _get_material_attr(name: str, default): - attr = material_prim.GetAttribute(name) - return attr.Get() if attr.IsValid() else default - - density = _get_material_attr("newton:density", DeformableRegistryEntry.density) - particle_radius = _get_material_attr("newton:particleRadius", DeformableRegistryEntry.particle_radius) - k_mu = _get_material_attr("newton:kMu", DeformableRegistryEntry.k_mu) - k_lambda = _get_material_attr("newton:kLambda", DeformableRegistryEntry.k_lambda) - k_damp = _get_material_attr("newton:kDamp", DeformableRegistryEntry.k_damp) - - tri_ke = _get_material_attr("newton:triKe", DeformableRegistryEntry.tri_ke) - tri_ka = _get_material_attr("newton:triKa", DeformableRegistryEntry.tri_ka) - tri_kd = _get_material_attr("newton:triKd", DeformableRegistryEntry.tri_kd) - edge_ke = _get_material_attr("newton:edgeKe", DeformableRegistryEntry.edge_ke) - edge_kd = _get_material_attr("newton:edgeKd", DeformableRegistryEntry.edge_kd) - - entry = DeformableRegistryEntry( - prim_path=self.cfg.prim_path, - sim_mesh_prim_path=sim_mesh_prim_path, - vis_mesh_prim_path=vis_mesh_prim_path, - vertices=vertices, - indices=indices, - deformable_type=deformable_type, - volume_vis_remap=volume_vis_remap, - init_pos=init_pos, - init_rot=init_rot, - density=density, - tri_ke=tri_ke, - tri_ka=tri_ka, - tri_kd=tri_kd, - edge_ke=edge_ke, - edge_kd=edge_kd, - particle_radius=particle_radius, - k_mu=k_mu, - k_lambda=k_lambda, - k_damp=k_damp, - ) - SimulationManager._deformable_registry.append(entry) - self._deformable_type = deformable_type - return entry - - def _initialize_impl(self): - """Initialize physics handles and buffers after the Newton model is ready.""" - entry = self._registry_entry - self._num_instances = len(entry.particle_offsets) - self._particles_per_body = entry.particles_per_body - self._recorded_particle_offsets = entry.particle_offsets - - if self._num_instances == 0: - raise RuntimeError( - f"No deformable body instances found for '{self.cfg.prim_path}'. " - "Ensure clone-plan replication or MODEL_INIT processed the registry." - ) - - logger.info("Newton deformable object initialized at: %s", self.cfg.prim_path) - logger.info("Number of instances: %d", self._num_instances) - logger.info("Particles per body: %d", self._particles_per_body) - - # Build particle offset array on device - self._particle_offsets = wp.array(self._recorded_particle_offsets, dtype=wp.int32, device=self.device) - - # Create data container - self._data = DeformableObjectData( - particle_offsets=self._particle_offsets, - particles_per_body=self._particles_per_body, - num_instances=self._num_instances, - device=self.device, - ) - - # Create buffers - self._create_buffers() - - # Update data once - self.update(0.0) - - # Register rebind callback for full resets - self._physics_ready_handle = SimulationManager.register_callback( - lambda _: self._data._create_simulation_bindings(), - PhysicsEvent.PHYSICS_READY, - name=f"deformable_object_rebind_{self.cfg.prim_path}", - ) - - def _create_buffers(self): - """Create buffers for storing data.""" - # Constants - self._ALL_INDICES = wp.array(np.arange(self._num_instances, dtype=np.int32), device=self.device) - self._ALL_ENV_MASK = wp.ones((self._num_instances,), dtype=wp.bool, device=self.device) - - # Snapshot default positions from current state (after finalize + FK) - state = SimulationManager.get_state_0() - if state is not None and state.particle_q is not None: - from .kernels import gather_particles_vec3f - - self._default_nodal_pos_w = wp.zeros( - (self._num_instances, self._particles_per_body), dtype=wp.vec3f, device=self.device - ) - wp.launch( - gather_particles_vec3f, - dim=(self._num_instances, self._particles_per_body), - inputs=[state.particle_q, self._particle_offsets, self._particles_per_body], - outputs=[self._default_nodal_pos_w], - device=self.device, - ) - - # Compute default nodal state as vec6f (positions + zero velocities) - nodal_velocities = wp.zeros( - (self._num_instances, self._particles_per_body), dtype=wp.vec3f, device=self.device - ) - default_nodal_state_w = wp.zeros( - (self._num_instances, self._particles_per_body), dtype=vec6f, device=self.device - ) - wp.launch( - compute_nodal_state_w, - dim=(self._num_instances, self._particles_per_body), - inputs=[self._default_nodal_pos_w, nodal_velocities], - outputs=[default_nodal_state_w], - device=self.device, - ) - self._data.default_nodal_state_w = ProxyArray(default_nodal_state_w) - else: - self._default_nodal_pos_w = None - - # Snapshot default particle_inv_mass for kinematic target restoration - model = SimulationManager.get_model() - if model is not None and hasattr(model, "particle_inv_mass") and model.particle_inv_mass is not None: - self._default_particle_inv_mass = wp.clone(model.particle_inv_mass) - else: - self._default_particle_inv_mass = None - if model is not None and hasattr(model, "particle_flags") and model.particle_flags is not None: - self._default_particle_flags = wp.clone(model.particle_flags) - else: - self._default_particle_flags = None - - # Kinematic targets -- allocate and initialize with free flags - nodal_kinematic_target = wp.zeros( - (self._num_instances, self._particles_per_body), dtype=wp.vec4f, device=self.device - ) - wp.launch( - set_kinematic_flags_to_one, - dim=(self._num_instances * self._particles_per_body,), - inputs=[nodal_kinematic_target.reshape((self._num_instances * self._particles_per_body,))], - device=self.device, - ) - self._data.nodal_kinematic_target = ProxyArray(nodal_kinematic_target) - - # Set up the model parameters - model = SimulationManager.get_model() - if model is not None: - if hasattr(model, "edge_rest_angle"): - model.edge_rest_angle.zero_() - - """ - Internal simulation callbacks. - """ - - def _set_debug_vis_impl(self, debug_vis: bool): - if debug_vis: - if not hasattr(self, "target_visualizer"): - self.target_visualizer = VisualizationMarkers(self.cfg.visualizer_cfg) - self.target_visualizer.set_visibility(True) - else: - if hasattr(self, "target_visualizer"): - self.target_visualizer.set_visibility(False) - - def _debug_vis_callback(self, event): - num_enabled = 0 - if self._deformable_type == "volume": - kinematic_target_torch = self.data.nodal_kinematic_target.torch - targets_enabled = kinematic_target_torch[:, :, 3] == 0.0 - num_enabled = int(torch.sum(targets_enabled).item()) - if num_enabled == 0: - positions = torch.tensor([[0.0, 0.0, -10.0]], device=self.device) - else: - positions = kinematic_target_torch[targets_enabled][..., :3] - self.target_visualizer.visualize(positions) - - def _clear_callbacks(self) -> None: - """Clears all registered callbacks.""" - super()._clear_callbacks() - if hasattr(self, "_physics_ready_handle") and self._physics_ready_handle is not None: - self._physics_ready_handle.deregister() - self._physics_ready_handle = None - - def _invalidate_initialize_callback(self, event): - """Invalidates the scene elements.""" - super()._invalidate_initialize_callback(event) diff --git a/source/isaaclab_contrib/test/coupling/test_coupler.py b/source/isaaclab_contrib/test/coupling/test_coupler.py index 8df757caff45..4348ca0716f7 100644 --- a/source/isaaclab_contrib/test/coupling/test_coupler.py +++ b/source/isaaclab_contrib/test/coupling/test_coupler.py @@ -17,7 +17,6 @@ from dataclasses import dataclass, field from types import SimpleNamespace -import isaaclab_newton.physics.newton_manager as newton_manager_module import numpy as np import pytest from isaaclab_newton.physics import ( @@ -572,16 +571,10 @@ def test_nested_solver_scopes_mujoco_joint_properties( joint.GetPrim().CreateAttribute("mjc:frictionloss", Sdf.ValueTypeNames.Double, True).Set(0.11) joint.GetPrim().CreateAttribute("mjc:damping", Sdf.ValueTypeNames.Double, True).Set(0.23) - monkeypatch.setattr(newton_manager_module, "get_current_stage", lambda: stage) - monkeypatch.setattr(newton_manager_module, "_restore_visible_colliders_without_visual_shapes", lambda *args: None) - monkeypatch.setattr(newton_manager_module, "replace_newton_builder_shape_colors", lambda *args: None) - monkeypatch.setattr(newton_manager_module, "import_builder_visual_material_paths", lambda *args: None) - monkeypatch.setattr(NewtonManager, "_builder", None) - monkeypatch.setattr(NewtonManager, "_deformable_registry", []) - monkeypatch.setattr(NewtonManager, "_per_world_builder_hooks", []) - - NewtonCouplerManager.instantiate_builder_from_stage() - builder = NewtonManager._builder + builder = NewtonCouplerManager.create_builder(up_axis="Z") + builder.add_usd( + stage, root_path=root_path, schema_resolvers=NewtonCouplerManager._get_usd_import_schema_resolvers() + ) model = builder.finalize(device="cpu") assert model.joint_friction.numpy()[-1] == pytest.approx(expected_friction) diff --git a/source/isaaclab_contrib/test/custom_coupling/test_manual_coupling.py b/source/isaaclab_contrib/test/custom_coupling/test_manual_coupling.py index 7fa740667a39..19bd06271475 100644 --- a/source/isaaclab_contrib/test/custom_coupling/test_manual_coupling.py +++ b/source/isaaclab_contrib/test/custom_coupling/test_manual_coupling.py @@ -17,7 +17,7 @@ """Rest everything follows.""" import torch -from isaaclab_newton.assets import RigidObject +from isaaclab_newton.assets import DeformableObject, RigidObject from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg, VBDSolverCfg from isaaclab_newton.sim.schemas import NewtonDeformableBodyPropertiesCfg from isaaclab_newton.sim.spawners.materials import NewtonDeformableBodyMaterialCfg @@ -29,7 +29,6 @@ from isaaclab.sim import SimulationCfg, build_simulation_context from isaaclab_contrib.custom_coupling import CoupledMJWarpVBDSolverCfg -from isaaclab_contrib.deformable import DeformableObject def _make_coupled_cfg(coupling_mode: str) -> SimulationCfg: diff --git a/source/isaaclab_contrib/test/deformable/test_deformable_builder_hooks.py b/source/isaaclab_contrib/test/deformable/test_deformable_builder_hooks.py deleted file mode 100644 index 77bc00910ac1..000000000000 --- a/source/isaaclab_contrib/test/deformable/test_deformable_builder_hooks.py +++ /dev/null @@ -1,155 +0,0 @@ -# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). -# All rights reserved. -# -# SPDX-License-Identifier: BSD-3-Clause - -import math -from types import SimpleNamespace -from unittest.mock import Mock - -import numpy as np -import pytest -import warp as wp -from isaaclab_newton.physics import NewtonManager -from isaaclab_newton.sim.spawners.materials import NewtonDeformableMaterialCfg - -from isaaclab.cloner import ClonePlan, PrototypeWorldTopology - -from isaaclab_contrib.deformable import DeformableObject -from isaaclab_contrib.deformable.deformable_object import ( - DeformableRegistryEntry, - add_deformable_entry_to_builder, -) - - -class _FakeBuilder: - def __init__(self): - self.particle_count = 0 - self.cloth_meshes = [] - - def add_cloth_mesh(self, **kwargs) -> None: - self.cloth_meshes.append(kwargs) - self.particle_count += len(kwargs["vertices"]) - - -def _make_surface_entry() -> DeformableRegistryEntry: - half_sqrt = math.sqrt(0.5) - return DeformableRegistryEntry( - prim_path="{ENV_REGEX_NS}/cloth", - sim_mesh_prim_path="{ENV_REGEX_NS}/cloth/mesh", - vis_mesh_prim_path="{ENV_REGEX_NS}/cloth/mesh", - vertices=[ - wp.vec3(0.0, 0.0, 0.0), - wp.vec3(1.0, 0.0, 0.0), - wp.vec3(0.0, 1.0, 0.0), - ], - indices=[0, 1, 2], - init_pos=(1.0, 0.0, 0.0), - init_rot=(0.0, 0.0, half_sqrt, half_sqrt), - deformable_type="surface", - ) - - -def test_builder_hook_preserves_placement_materials_and_rebuild_offsets(): - """Clone placement preserves material defaults and rebuilding replaces particle offsets.""" - entry = _make_surface_entry() - builder = _FakeBuilder() - half_sqrt = math.sqrt(0.5) - - add_deformable_entry_to_builder( - builder, - entry, - env_idx=0, - env_position=[10.0, 20.0, 30.0], - env_rotation=[0.0, 0.0, half_sqrt, half_sqrt], - ) - - mesh = builder.cloth_meshes[0] - rotated_x_axis = wp.quat_rotate(mesh["rot"], wp.vec3(1.0, 0.0, 0.0)) - - assert tuple(mesh["pos"]) == pytest.approx((10.0, 21.0, 30.0)) - assert tuple(rotated_x_axis) == pytest.approx((-1.0, 0.0, 0.0), abs=1e-6) - material = NewtonDeformableMaterialCfg() - assert mesh["density"] == material.density - assert mesh["particle_radius"] == material.particle_radius - identity = [0.0, 0.0, 0.0, 1.0] - add_deformable_entry_to_builder(builder, entry, 1, [1.0, 0.0, 0.0], identity) - assert entry.particle_offsets == [0, 3] - - add_deformable_entry_to_builder(_FakeBuilder(), entry, 0, [0.0, 0.0, 0.0], identity) - assert entry.particle_offsets == [0] - assert entry.particles_per_body == 3 - - -def test_planned_geometry_aliases_surface_nodes_and_interpolates_volume_once(monkeypatch): - """Sparse planned instances publish exact visual paths without requiring cloned USD meshes.""" - from isaaclab_newton.physics.newton_manager import NewtonSceneDataBackend - - from pxr import Sdf, Usd, UsdGeom, UsdShade - - import isaaclab.sim.utils.queries as queries - from isaaclab.scene_data import SceneDataProvider - - import isaaclab_contrib.deformable.deformable_object as deformable - - stage = Usd.Stage.CreateInMemory() - root = UsdGeom.Xform.Define(stage, "/Scene/copy_0/Volume") - root.AddTranslateOp().Set((10, 20, 30)) - tet = UsdGeom.TetMesh.Define(stage, "/Scene/copy_0/Volume/sim") - tet.CreatePointsAttr([(0, 0, 0), (1, 0, 0), (0, 1, 0), (0, 0, 1)]) - tet.CreateTetVertexIndicesAttr([(0, 1, 2, 3)]) - UsdGeom.Xformable(tet).AddTranslateOp().Set((1, 0, 0)) - visual = UsdGeom.Mesh.Define(stage, "/Scene/copy_0/Volume/vis") - visual.CreatePointsAttr([(0.5, 0, 0), (0, 0.5, 0), (0, 0, 0.5)]) - visual.CreateFaceVertexCountsAttr([3]) - visual.CreateFaceVertexIndicesAttr([0, 1, 2]) - UsdGeom.Xformable(visual).AddTranslateOp().Set((1, 0, 0)) - material = UsdShade.Material.Define(stage, "/Material") - material.GetPrim().CreateAttribute("newton:density", Sdf.ValueTypeNames.Float).Set(1000.0) - UsdShade.MaterialBindingAPI.Apply(root.GetPrim()).Bind(material, materialPurpose="physics") - monkeypatch.setattr(queries, "get_current_stage", lambda: stage) - monkeypatch.setattr(NewtonManager, "_deformable_registry", []) - remap = Mock(wraps=deformable.build_volume_vis_barycentric_remap) - monkeypatch.setattr(deformable, "build_volume_vis_barycentric_remap", remap) - asset = SimpleNamespace( - cfg=SimpleNamespace( - prim_path="/Scene/copy_[^/]+/Volume", spawn=SimpleNamespace(spawn_path="/Scene/copy_0/Volume") - ) - ) - entry = DeformableObject._register_deformable(asset) - entry.particle_offsets, entry.particles_per_body = [7, 19], 4 - surface = _make_surface_entry() - surface.prim_path = "/Scene/copy_[^/]+/cloth" - surface.vis_mesh_prim_path = surface.prim_path + "/mesh" - surface.particle_offsets, surface.particles_per_body = [31, 37], 3 - NewtonManager._deformable_registry.append(surface) - world_layout = np.zeros(43, dtype=np.int32) - world_layout[[7, 42]] = 1 - plan = ClonePlan( - PrototypeWorldTopology( - num_asset_prototypes=2, - world_prototypes=np.array([0, 1]), - world_prototype_starts=np.array([0, 0, 0, 2]), - world_prototype_layout=world_layout, - ), - asset_cfgs=(asset.cfg, SimpleNamespace(prim_path=surface.prim_path)), - env_template="/Scene/copy_{}", - ) - nodes = np.zeros((40, 3), dtype=np.float32) - nodes[7:11], nodes[19:23] = np.asarray(entry.vertices), np.asarray(entry.vertices) + [100, 0, 0] - state = SimpleNamespace(particle_q=wp.array(nodes, dtype=wp.vec3f, device="cpu"), body_q=None) - monkeypatch.setattr(NewtonManager, "_cable_bindings", {}) - monkeypatch.setattr(NewtonManager, "backend", SimpleNamespace(state_0=state)) - backend = NewtonSceneDataBackend() - backend.initialize_geometry(plan) - stage.RemovePrim("/Scene") - points = SceneDataProvider(backend).get_geometry_points() - assert set(points) == {f"/Scene/copy_{index}/{mesh}" for index in (7, 42) for mesh in ("Volume/vis", "cloth/mesh")} - for index, offset in zip((7, 42), surface.particle_offsets, strict=True): - view = points[f"/Scene/copy_{index}/cloth/mesh"] - assert view.ptr == state.particle_q.ptr + offset * wp.types.type_size_in_bytes(wp.vec3f) - assert len(view) == surface.particles_per_body - expected = np.array([[11.5, 20, 30], [11, 20.5, 30], [11, 20, 30.5]]) - np.testing.assert_allclose(points["/Scene/copy_7/Volume/vis"].numpy(), expected) - np.testing.assert_allclose(points["/Scene/copy_42/Volume/vis"].numpy(), expected + [100, 0, 0]) - remap.assert_called_once() diff --git a/source/isaaclab_newton/changelog.d/newton-deformable-ownership.rst b/source/isaaclab_newton/changelog.d/newton-deformable-ownership.rst new file mode 100644 index 000000000000..14148f877b05 --- /dev/null +++ b/source/isaaclab_newton/changelog.d/newton-deformable-ownership.rst @@ -0,0 +1,34 @@ +Changed +^^^^^^^ + +* Moved the Newton deformable asset, data buffers, and kernels into + :mod:`isaaclab_newton.assets`, removing the optional dependency on ``isaaclab_contrib``. + Imported declared deformable prototypes once during cloning and selected their native particle + ranges during asset initialization, removing the geometry registry and per-world construction hook. + The shared :class:`isaaclab.assets.DeformableObjectCfg` and backend-independent asset API remained unchanged. + +* **Breaking:** Made ``NewtonManager.instantiate_builder_from_stage()`` consume the active clone plan + instead of discovering environment roots. Construct an ``InteractiveScene``, explicitly replicate a + ``ClonePlan``, or supply a native builder with ``NewtonManager.set_builder()``. + +Fixed +^^^^^ + +* Applied clone-plan world compositions to Newton deformables and imported shared deformables once. + Preserved rotated particle positions, velocities, and tetrahedral rest frames during builder composition. + Built render-only deformable meshes in source builders before native replication, instead of + rebuilding their geometry for every destination world. + +* Moved MPM particle-range and visual-geometry binding into clone/import, removing asset-side registration. + +* Moved Fabric body-prim preparation out of Newton physics startup and into the shared Fabric rendering resource. + Preserved rendering-compatible CUDA graph capture independently of Fabric bindings. + +* Scoped deformable kinematic defaults to each asset's selected particles instead of copying the entire model. + Preserved imported cloth rest angles during asset initialization. + +* Preserved imported robot bodies selected by sensors that opted out of cloning. + +* Kept source labels and material references unchanged during world composition, naming only + appended instances. Used imported static geometry for collider visibility instead of traversing + USD subtrees, and preserved Newton's visual meshes and material bindings for approximated colliders. diff --git a/source/isaaclab_newton/isaaclab_newton/assets/deformable_object/__init__.pyi b/source/isaaclab_newton/isaaclab_newton/assets/deformable_object/__init__.pyi index 5878e22680c6..7b61168e4d9a 100644 --- a/source/isaaclab_newton/isaaclab_newton/assets/deformable_object/__init__.pyi +++ b/source/isaaclab_newton/isaaclab_newton/assets/deformable_object/__init__.pyi @@ -8,5 +8,5 @@ __all__ = [ "DeformableObjectData", ] -from isaaclab_contrib.deformable.deformable_object import DeformableObject -from isaaclab_contrib.deformable.deformable_object_data import DeformableObjectData +from .deformable_object import DeformableObject +from .deformable_object_data import DeformableObjectData diff --git a/source/isaaclab_newton/isaaclab_newton/assets/deformable_object/deformable_object.py b/source/isaaclab_newton/isaaclab_newton/assets/deformable_object/deformable_object.py new file mode 100644 index 000000000000..0141132e21e7 --- /dev/null +++ b/source/isaaclab_newton/isaaclab_newton/assets/deformable_object/deformable_object.py @@ -0,0 +1,553 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import logging +import re +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import numpy as np +import torch +import warp as wp + +from isaaclab.assets.deformable_object.base_deformable_object import BaseDeformableObject +from isaaclab.markers import VisualizationMarkers +from isaaclab.physics import PhysicsEvent +from isaaclab.utils.warp import ProxyArray + +from ...physics.newton_manager import NewtonManager as SimulationManager +from .deformable_object_data import DeformableObjectData +from .kernels import ( + compute_nodal_state_w, + enforce_kinematic_targets, + gather_particles, + scatter_particles_state_vec6f_mask, + scatter_particles_vec3f_index, + scatter_particles_vec3f_mask, + vec6f, + write_nodal_kinematic_target_index, + write_nodal_kinematic_target_mask, +) + +if TYPE_CHECKING: + from isaaclab.assets.deformable_object.deformable_object_cfg import DeformableObjectCfg + +logger = logging.getLogger(__name__) + + +class DeformableObject(BaseDeformableObject): + """A deformable object asset class (Newton backend). + + This class manages cloth/deformable bodies in the Newton physics engine. Newton stores all + particles in flat arrays (``state.particle_q``, ``state.particle_qd``). This class builds + a per-instance indexing layer on top of those flat arrays, enabling the standard + :class:`BaseDeformableObject` interface for reading/writing nodal state. + + Newton cloning imports the authored prototypes and replicates their native element ranges. + Initialization selects this asset's ranges from the completed backend, without reading USD. + """ + + cfg: DeformableObjectCfg + """Configuration instance for the deformable object.""" + + __backend_name__: str = "newton" + """The name of the backend for the deformable object.""" + + def __init__(self, cfg: DeformableObjectCfg): + """Initialize the deformable object. + + Args: + cfg: A configuration instance. + """ + super().__init__(cfg) + + # Register custom vec6f type for nodal state validation. + self._DTYPE_TO_TORCH_TRAILING_DIMS = {**self._DTYPE_TO_TORCH_TRAILING_DIMS, vec6f: (6,)} + + """ + Properties + """ + + @property + def data(self) -> DeformableObjectData: + return self._data + + @property + def num_instances(self) -> int: + return self._num_instances + + @property + def num_bodies(self) -> int: + """Number of bodies in the asset. + + This is always 1 since each object is a single deformable body. + """ + return 1 + + @property + def max_sim_vertices_per_body(self) -> int: + """The maximum number of simulation mesh vertices per deformable body.""" + return self._particles_per_body + + """ + Operations. + """ + + def reset(self, env_ids: Sequence[int] | None = None, env_mask: wp.array | None = None) -> None: + """Reset the deformable object. + + No-op to match the PhysX deformable object convention. + + Args: + env_ids: Environment indices. If None, then all indices are used. + env_mask: Environment mask. If None, then all the instances are updated. + Shape is (num_instances,). + """ + pass + + def write_data_to_sim(self): + """Apply kinematic targets to the Newton simulation. + + Reads the stored kinematic target buffer and enforces it on particles: + kinematic particles (flag=0) get inv_mass=0, particle_flags=0, target position, + and zero velocity; free particles (flag=1) get their original inv_mass and + particle_flags=1 (ACTIVE) restored. + + Writes to both ``state_0`` and ``state_1`` so kinematic positions survive + the state swaps that happen between substeps. + """ + if self._data.nodal_kinematic_target is None: + return + + model = SimulationManager.get_model() + + for state in self._iter_particle_states(): + wp.launch( + enforce_kinematic_targets, + dim=(self._num_instances, self._particles_per_body), + inputs=[ + self._data.nodal_kinematic_target.warp, + self._particle_offsets, + self._default_particle_inv_mass, + self._default_particle_flags, + ], + outputs=[ + state.particle_q, + state.particle_qd, + model.particle_inv_mass, + model.particle_flags, + ], + device=self.device, + ) + + def update(self, dt: float): + self._data.update(dt) + + """ + Operations - Write to simulation. + """ + + def write_nodal_pos_to_sim_index( + self, + nodal_pos: torch.Tensor | wp.array | ProxyArray, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + full_data: bool = False, + ) -> None: + """Set the nodal positions over selected environment indices into the simulation. + + Args: + nodal_pos: Nodal positions in simulation frame [m]. + Shape is (len(env_ids), max_sim_vertices_per_body, 3) + or (num_instances, max_sim_vertices_per_body, 3). + env_ids: Environment indices. If None, then all indices are used. + full_data: Whether to expect full data. Defaults to False. + """ + env_ids = self._resolve_env_ids(env_ids) + if isinstance(nodal_pos, ProxyArray): + nodal_pos = nodal_pos.warp + if full_data: + self.assert_shape_and_dtype( + nodal_pos, (self.num_instances, self._particles_per_body), wp.vec3f, "nodal_pos" + ) + else: + self.assert_shape_and_dtype(nodal_pos, (env_ids.shape[0], self._particles_per_body), wp.vec3f, "nodal_pos") + if isinstance(nodal_pos, torch.Tensor): + nodal_pos = wp.from_torch(nodal_pos.contiguous(), dtype=wp.vec3f) + + for state in self._iter_particle_states(): + wp.launch( + scatter_particles_vec3f_index, + dim=(env_ids.shape[0], self._particles_per_body), + inputs=[nodal_pos, env_ids, self._particle_offsets, full_data], + outputs=[state.particle_q], + device=self.device, + ) + + SimulationManager._mark_particles_dirty() + self._invalidate_nodal_pos_cache() + + def write_nodal_velocity_to_sim_index( + self, + nodal_vel: torch.Tensor | wp.array | ProxyArray, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + full_data: bool = False, + ) -> None: + """Set the nodal velocity over selected environment indices into the simulation. + + Args: + nodal_vel: Nodal velocities in simulation frame [m/s]. + Shape is (len(env_ids), max_sim_vertices_per_body, 3) + or (num_instances, max_sim_vertices_per_body, 3). + env_ids: Environment indices. If None, then all indices are used. + full_data: Whether to expect full data. Defaults to False. + """ + env_ids = self._resolve_env_ids(env_ids) + if isinstance(nodal_vel, ProxyArray): + nodal_vel = nodal_vel.warp + if full_data: + self.assert_shape_and_dtype( + nodal_vel, (self.num_instances, self._particles_per_body), wp.vec3f, "nodal_vel" + ) + else: + self.assert_shape_and_dtype(nodal_vel, (env_ids.shape[0], self._particles_per_body), wp.vec3f, "nodal_vel") + if isinstance(nodal_vel, torch.Tensor): + nodal_vel = wp.from_torch(nodal_vel.contiguous(), dtype=wp.vec3f) + + for state in self._iter_particle_states(): + wp.launch( + scatter_particles_vec3f_index, + dim=(env_ids.shape[0], self._particles_per_body), + inputs=[nodal_vel, env_ids, self._particle_offsets, full_data], + outputs=[state.particle_qd], + device=self.device, + ) + + self._invalidate_nodal_vel_cache() + + def write_nodal_kinematic_target_to_sim_index( + self, + targets: torch.Tensor | wp.array | ProxyArray, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + full_data: bool = False, + ) -> None: + """Set the kinematic targets of the simulation mesh for the deformable bodies. + + Newton has no native kinematic target API. Instead: + - Kinematic (flag=0.0): set ``particle_inv_mass`` to 0, write target pos, zero vel + - Free (flag=1.0): restore original ``particle_inv_mass`` + + Args: + targets: The kinematic targets comprising of nodal positions and flags [m]. + Shape is (len(env_ids), max_sim_vertices_per_body, 4) + or (num_instances, max_sim_vertices_per_body, 4). + env_ids: Environment indices. If None, then all indices are used. + full_data: Whether to expect full data. Defaults to False. + """ + env_ids = self._resolve_env_ids(env_ids) + if isinstance(targets, ProxyArray): + targets = targets.warp + if full_data: + self.assert_shape_and_dtype(targets, (self.num_instances, self._particles_per_body), wp.vec4f, "targets") + else: + self.assert_shape_and_dtype(targets, (env_ids.shape[0], self._particles_per_body), wp.vec4f, "targets") + if isinstance(targets, torch.Tensor): + if targets.dim() == 2: + targets = targets.unsqueeze(0) + targets = wp.from_torch(targets.contiguous(), dtype=wp.vec4f) + + # Store kinematic targets in our data buffer + if self._data.nodal_kinematic_target is not None: + wp.launch( + write_nodal_kinematic_target_index, + dim=(env_ids.shape[0], self._particles_per_body), + inputs=[targets, env_ids, full_data], + outputs=[self._data.nodal_kinematic_target.warp], + device=self.device, + ) + + """ + Operations - Write to simulation (mask variants). + """ + + def write_nodal_state_to_sim_mask( + self, + nodal_state: torch.Tensor | wp.array | ProxyArray, + env_mask: wp.array | torch.Tensor | None = None, + ) -> None: + """Set the nodal state over selected environment mask into the simulation. + + Args: + nodal_state: Nodal state in simulation frame [m, m/s]. + Shape is (num_instances, max_sim_vertices_per_body, 6). + env_mask: Environment mask. If None, then all indices are used. + Shape is (num_instances,). + """ + env_mask = self._resolve_mask(env_mask, self._ALL_ENV_MASK) + if isinstance(nodal_state, ProxyArray): + nodal_state = nodal_state.warp + self.assert_shape_and_dtype(nodal_state, (env_mask.shape[0], self._particles_per_body), vec6f, "nodal_state") + if isinstance(nodal_state, torch.Tensor): + nodal_state = wp.from_torch(nodal_state.contiguous(), dtype=vec6f) + + for state in self._iter_particle_states(): + wp.launch( + scatter_particles_state_vec6f_mask, + dim=(env_mask.shape[0], self._particles_per_body), + inputs=[nodal_state, env_mask, self._particle_offsets], + outputs=[state.particle_q, state.particle_qd], + device=self.device, + ) + + SimulationManager._mark_particles_dirty() + self._invalidate_nodal_pos_cache() + self._invalidate_nodal_vel_cache() + + def write_nodal_pos_to_sim_mask( + self, + nodal_pos: torch.Tensor | wp.array | ProxyArray, + env_mask: wp.array | torch.Tensor | None = None, + ) -> None: + """Set the nodal positions over selected environment mask into the simulation. + + Args: + nodal_pos: Nodal positions in simulation frame [m]. + Shape is (num_instances, max_sim_vertices_per_body, 3). + env_mask: Environment mask. If None, then all indices are used. + Shape is (num_instances,). + """ + env_mask = self._resolve_mask(env_mask, self._ALL_ENV_MASK) + if isinstance(nodal_pos, ProxyArray): + nodal_pos = nodal_pos.warp + self.assert_shape_and_dtype(nodal_pos, (env_mask.shape[0], self._particles_per_body), wp.vec3f, "nodal_pos") + if isinstance(nodal_pos, torch.Tensor): + nodal_pos = wp.from_torch(nodal_pos.contiguous(), dtype=wp.vec3f) + + for state in self._iter_particle_states(): + wp.launch( + scatter_particles_vec3f_mask, + dim=(env_mask.shape[0], self._particles_per_body), + inputs=[nodal_pos, env_mask, self._particle_offsets], + outputs=[state.particle_q], + device=self.device, + ) + + SimulationManager._mark_particles_dirty() + self._invalidate_nodal_pos_cache() + + def write_nodal_velocity_to_sim_mask( + self, + nodal_vel: torch.Tensor | wp.array | ProxyArray, + env_mask: wp.array | torch.Tensor | None = None, + ) -> None: + """Set the nodal velocity over selected environment mask into the simulation. + + Args: + nodal_vel: Nodal velocities in simulation frame [m/s]. + Shape is (num_instances, max_sim_vertices_per_body, 3). + env_mask: Environment mask. If None, then all indices are used. + Shape is (num_instances,). + """ + env_mask = self._resolve_mask(env_mask, self._ALL_ENV_MASK) + if isinstance(nodal_vel, ProxyArray): + nodal_vel = nodal_vel.warp + self.assert_shape_and_dtype(nodal_vel, (env_mask.shape[0], self._particles_per_body), wp.vec3f, "nodal_vel") + if isinstance(nodal_vel, torch.Tensor): + nodal_vel = wp.from_torch(nodal_vel.contiguous(), dtype=wp.vec3f) + + for state in self._iter_particle_states(): + wp.launch( + scatter_particles_vec3f_mask, + dim=(env_mask.shape[0], self._particles_per_body), + inputs=[nodal_vel, env_mask, self._particle_offsets], + outputs=[state.particle_qd], + device=self.device, + ) + + self._invalidate_nodal_vel_cache() + + def write_nodal_kinematic_target_to_sim_mask( + self, + targets: torch.Tensor | wp.array | ProxyArray, + env_mask: wp.array | torch.Tensor | None = None, + ) -> None: + """Set the kinematic targets over selected environment mask into the target buffer. + + Args: + targets: The kinematic targets comprising of nodal positions and flags [m]. + Shape is (num_instances, max_sim_vertices_per_body, 4). + env_mask: Environment mask. If None, then all indices are used. + Shape is (num_instances,). + """ + env_mask = self._resolve_mask(env_mask, self._ALL_ENV_MASK) + if isinstance(targets, ProxyArray): + targets = targets.warp + self.assert_shape_and_dtype(targets, (env_mask.shape[0], self._particles_per_body), wp.vec4f, "targets") + if isinstance(targets, torch.Tensor): + targets = wp.from_torch(targets.contiguous(), dtype=wp.vec4f) + + if self._data.nodal_kinematic_target is not None: + wp.launch( + write_nodal_kinematic_target_mask, + dim=(env_mask.shape[0], self._particles_per_body), + inputs=[targets, env_mask], + outputs=[self._data.nodal_kinematic_target.warp], + device=self.device, + ) + + """ + Internal helper. + """ + + def _resolve_env_ids(self, env_ids): + """Resolve environment indices to a warp int32 array.""" + if env_ids is None or (isinstance(env_ids, slice) and env_ids == slice(None)): + return self._ALL_INDICES + elif isinstance(env_ids, list): + return wp.array(env_ids, dtype=wp.int32, device=self.device) + elif isinstance(env_ids, torch.Tensor): + return wp.from_torch(env_ids.to(torch.int32), dtype=wp.int32) + return env_ids + + def _resolve_mask(self, mask: wp.array | torch.Tensor | None, full_mask: wp.array) -> wp.array: + """Resolve an environment mask to a warp bool array.""" + if mask is None: + return full_mask + if isinstance(mask, torch.Tensor): + if mask.dtype != torch.bool: + mask = mask.to(torch.bool) + return wp.from_torch(mask, dtype=wp.bool) + return mask + + def _iter_particle_states(self): + """Yield active Newton states.""" + for state in (SimulationManager.get_state_0(), SimulationManager.get_state_1()): + if state is None: + continue + yield state + + def _invalidate_nodal_pos_cache(self) -> None: + """Invalidate cached position-derived deformable data.""" + self._data._nodal_pos_w.timestamp = -1.0 + self._data._nodal_state_w.timestamp = -1.0 + self._data._root_pos_w.timestamp = -1.0 + + def _invalidate_nodal_vel_cache(self) -> None: + """Invalidate cached velocity-derived deformable data.""" + self._data._nodal_vel_w.timestamp = -1.0 + self._data._nodal_state_w.timestamp = -1.0 + self._data._root_vel_w.timestamp = -1.0 + + def _initialize_impl(self): + """Initialize physics handles and buffers after the Newton model is ready.""" + # Replace this selection with Newton's family views when the pinned version includes + # https://github.com/newton-physics/newton/pull/3326. + pattern = re.compile(self.cfg.prim_path) + selected = [ + value for path, value in SimulationManager.backend.deformable_ranges.items() if pattern.fullmatch(path) + ] + if not selected: + raise RuntimeError(f"No imported deformable matches '{self.cfg.prim_path}'.") + self._particles_per_body, self._deformable_type = selected[0][1:] + if any((count, kind) != selected[0][1:] for _, count, kind in selected): + raise ValueError(f"Deformable selection '{self.cfg.prim_path}' requires equal particle counts and types.") + self._num_instances = len(selected) + + logger.info("Newton deformable object initialized at: %s", self.cfg.prim_path) + logger.info("Number of instances: %d", self._num_instances) + logger.info("Particles per body: %d", self._particles_per_body) + + # Build particle offset array on device + self._particle_offsets = wp.array([offset for offset, _, _ in selected], dtype=wp.int32, device=self.device) + + # Create data container + self._data = DeformableObjectData( + particle_offsets=self._particle_offsets, + particles_per_body=self._particles_per_body, + num_instances=self._num_instances, + device=self.device, + ) + + # Create buffers + self._create_buffers() + + # Update data once + self.update(0.0) + + # Register rebind callback for full resets + self._physics_ready_handle = SimulationManager.register_callback( + lambda _: self._data._create_simulation_bindings(), + PhysicsEvent.PHYSICS_READY, + name=f"deformable_object_rebind_{self.cfg.prim_path}", + ) + + def _create_buffers(self): + """Create buffers for storing data.""" + # Constants + self._ALL_INDICES = wp.array(np.arange(self._num_instances, dtype=np.int32), device=self.device) + self._ALL_ENV_MASK = wp.ones((self._num_instances,), dtype=wp.bool, device=self.device) + + # Defaults use the same asset-local selection as data reads, not the whole model. + shape = (self._num_instances, self._particles_per_body) + default_nodal_state_w = wp.empty(shape, dtype=vec6f, device=self.device) + wp.launch( + compute_nodal_state_w, + dim=shape, + inputs=[self._data.nodal_pos_w.warp, wp.zeros(shape, dtype=wp.vec3f, device=self.device)], + outputs=[default_nodal_state_w], + device=self.device, + ) + self._data.default_nodal_state_w = ProxyArray(default_nodal_state_w) + model = SimulationManager.get_model() + self._default_particle_inv_mass = wp.empty(shape, dtype=wp.float32, device=self.device) + self._default_particle_flags = wp.empty(shape, dtype=wp.int32, device=self.device) + for source, default in ( + (model.particle_inv_mass, self._default_particle_inv_mass), + (model.particle_flags, self._default_particle_flags), + ): + wp.launch( + gather_particles, + dim=shape, + inputs=[source, self._particle_offsets], + outputs=[default], + device=self.device, + ) + + self._data.nodal_kinematic_target = ProxyArray( + wp.full(shape, value=wp.vec4f(0.0, 0.0, 0.0, 1.0), device=self.device) + ) + + """ + Internal simulation callbacks. + """ + + def _set_debug_vis_impl(self, debug_vis: bool): + if debug_vis: + if not hasattr(self, "target_visualizer"): + self.target_visualizer = VisualizationMarkers(self.cfg.visualizer_cfg) + self.target_visualizer.set_visibility(True) + else: + if hasattr(self, "target_visualizer"): + self.target_visualizer.set_visibility(False) + + def _debug_vis_callback(self, event): + num_enabled = 0 + if self._deformable_type == "volume": + kinematic_target_torch = self.data.nodal_kinematic_target.torch + targets_enabled = kinematic_target_torch[:, :, 3] == 0.0 + num_enabled = int(torch.sum(targets_enabled).item()) + if num_enabled == 0: + positions = torch.tensor([[0.0, 0.0, -10.0]], device=self.device) + else: + positions = kinematic_target_torch[targets_enabled][..., :3] + self.target_visualizer.visualize(positions) + + def _clear_callbacks(self) -> None: + """Clears all registered callbacks.""" + super()._clear_callbacks() + if hasattr(self, "_physics_ready_handle") and self._physics_ready_handle is not None: + self._physics_ready_handle.deregister() + self._physics_ready_handle = None diff --git a/source/isaaclab_contrib/isaaclab_contrib/deformable/deformable_object_data.py b/source/isaaclab_newton/isaaclab_newton/assets/deformable_object/deformable_object_data.py similarity index 74% rename from source/isaaclab_contrib/isaaclab_contrib/deformable/deformable_object_data.py rename to source/isaaclab_newton/isaaclab_newton/assets/deformable_object/deformable_object_data.py index 48be4c3a4bb7..4d0fc22c42a0 100644 --- a/source/isaaclab_contrib/isaaclab_contrib/deformable/deformable_object_data.py +++ b/source/isaaclab_newton/isaaclab_newton/assets/deformable_object/deformable_object_data.py @@ -6,13 +6,14 @@ from __future__ import annotations import warp as wp -from isaaclab_newton.physics import NewtonManager as SimulationManager from isaaclab.assets.deformable_object.base_deformable_object_data import BaseDeformableObjectData -from isaaclab.utils.buffers import TimestampedBuffer +from isaaclab.utils.buffers import TimestampedBuffer, reset_timestamps from isaaclab.utils.warp import ProxyArray -from .kernels import compute_mean_vec3f_over_vertices, compute_nodal_state_w, gather_particles_vec3f, vec6f +from isaaclab_newton.physics import NewtonManager as SimulationManager + +from .kernels import compute_mean_vec3f_over_vertices, compute_nodal_state_w, gather_particles, vec6f class DeformableObjectData(BaseDeformableObjectData): @@ -49,13 +50,11 @@ def __init__(self, particle_offsets: wp.array, particles_per_body: int, num_inst self._nodal_state_w = TimestampedBuffer(wp.empty(nodal_shape, dtype=vec6f, device=device)) self._root_pos_w = TimestampedBuffer(wp.empty(num_instances, dtype=wp.vec3f, device=device)) self._root_vel_w = TimestampedBuffer(wp.empty(num_instances, dtype=wp.vec3f, device=device)) - self._nodal_pos_w_ta: ProxyArray | None = None - self._nodal_vel_w_ta: ProxyArray | None = None - self._nodal_state_w_ta: ProxyArray | None = None - self._root_pos_w_ta: ProxyArray | None = None - self._root_vel_w_ta: ProxyArray | None = None - - self._create_simulation_bindings() + self._nodal_pos_w_ta = ProxyArray(self._nodal_pos_w.data) + self._nodal_vel_w_ta = ProxyArray(self._nodal_vel_w.data) + self._nodal_state_w_ta = ProxyArray(self._nodal_state_w.data) + self._root_pos_w_ta = ProxyArray(self._root_pos_w.data) + self._root_vel_w_ta = ProxyArray(self._root_vel_w.data) ## # Defaults. @@ -76,30 +75,15 @@ def __init__(self, particle_offsets: wp.array, particles_per_body: int, num_inst """ def _create_simulation_bindings(self) -> None: - """Validate the current Newton particle state and invalidate gathered buffers. + """Invalidate gathered buffers after model reinitialization. Newton may swap :attr:`state_0` and :attr:`state_1` across substeps, so deformable data does not keep long-lived particle array bindings. Read properties query :meth:`SimulationManager.get_state_0` at gather time and materialize object-local views from the current flat particle arrays. """ - self._get_current_particle_state() - - # Invalidate lazy buffers gathered from the previous simulation state. - self._nodal_pos_w.timestamp = -1.0 - self._nodal_vel_w.timestamp = -1.0 - self._nodal_state_w.timestamp = -1.0 - self._root_pos_w.timestamp = -1.0 - self._root_vel_w.timestamp = -1.0 - - def _get_current_particle_state(self): - """Return the current Newton state containing deformable particle arrays.""" - state = SimulationManager.get_state_0() - if state is None or state.particle_q is None or state.particle_qd is None: - raise RuntimeError( - "Failed to access Newton deformable particle state. Ensure the Newton model has been finalized and " - "contains particle position and velocity arrays." - ) - return state + reset_timestamps( + (self._nodal_pos_w, self._nodal_vel_w, self._nodal_state_w, self._root_pos_w, self._root_vel_w) + ) ## # Properties. @@ -109,34 +93,30 @@ def _get_current_particle_state(self): def nodal_pos_w(self) -> ProxyArray: """Nodal positions in simulation world frame [m]. Shape is (num_instances, particles_per_body) vec3f.""" if self._nodal_pos_w.timestamp < self._sim_timestamp: - state = self._get_current_particle_state() + state = SimulationManager.get_state_0() wp.launch( - gather_particles_vec3f, + gather_particles, dim=(self._num_instances, self._particles_per_body), - inputs=[state.particle_q, self._particle_offsets, self._particles_per_body], + inputs=[state.particle_q, self._particle_offsets], outputs=[self._nodal_pos_w.data], device=self.device, ) self._nodal_pos_w.timestamp = self._sim_timestamp - if self._nodal_pos_w_ta is None: - self._nodal_pos_w_ta = ProxyArray(self._nodal_pos_w.data) return self._nodal_pos_w_ta @property def nodal_vel_w(self) -> ProxyArray: """Nodal velocities in simulation world frame [m/s]. Shape is (num_instances, particles_per_body) vec3f.""" if self._nodal_vel_w.timestamp < self._sim_timestamp: - state = self._get_current_particle_state() + state = SimulationManager.get_state_0() wp.launch( - gather_particles_vec3f, + gather_particles, dim=(self._num_instances, self._particles_per_body), - inputs=[state.particle_qd, self._particle_offsets, self._particles_per_body], + inputs=[state.particle_qd, self._particle_offsets], outputs=[self._nodal_vel_w.data], device=self.device, ) self._nodal_vel_w.timestamp = self._sim_timestamp - if self._nodal_vel_w_ta is None: - self._nodal_vel_w_ta = ProxyArray(self._nodal_vel_w.data) return self._nodal_vel_w_ta @property @@ -154,8 +134,6 @@ def nodal_state_w(self) -> ProxyArray: device=self.device, ) self._nodal_state_w.timestamp = self._sim_timestamp - if self._nodal_state_w_ta is None: - self._nodal_state_w_ta = ProxyArray(self._nodal_state_w.data) return self._nodal_state_w_ta ## @@ -177,8 +155,6 @@ def root_pos_w(self) -> ProxyArray: device=self.device, ) self._root_pos_w.timestamp = self._sim_timestamp - if self._root_pos_w_ta is None: - self._root_pos_w_ta = ProxyArray(self._root_pos_w.data) return self._root_pos_w_ta @property @@ -196,6 +172,4 @@ def root_vel_w(self) -> ProxyArray: device=self.device, ) self._root_vel_w.timestamp = self._sim_timestamp - if self._root_vel_w_ta is None: - self._root_vel_w_ta = ProxyArray(self._root_vel_w.data) return self._root_vel_w_ta diff --git a/source/isaaclab_contrib/isaaclab_contrib/deformable/kernels.py b/source/isaaclab_newton/isaaclab_newton/assets/deformable_object/kernels.py similarity index 76% rename from source/isaaclab_contrib/isaaclab_contrib/deformable/kernels.py rename to source/isaaclab_newton/isaaclab_newton/assets/deformable_object/kernels.py index 23b41df2e1f4..8691e6fecbfa 100644 --- a/source/isaaclab_contrib/isaaclab_contrib/deformable/kernels.py +++ b/source/isaaclab_newton/isaaclab_newton/assets/deformable_object/kernels.py @@ -5,24 +5,24 @@ """Warp kernels for Newton deformable object gather/scatter operations.""" +from typing import Any + import warp as wp vec6f = wp.types.vector(length=6, dtype=wp.float32) @wp.kernel -def gather_particles_vec3f( - src: wp.array(dtype=wp.vec3f), +def gather_particles( + src: wp.array(dtype=Any), offsets: wp.array(dtype=wp.int32), - num_particles: int, - dst: wp.array2d(dtype=wp.vec3f), + dst: wp.array2d(dtype=Any), ): """Gather particle data from a flat array into a per-instance 2D array. Args: src: Flat source particle array (all instances concatenated). Shape is (total_particles,). offsets: Per-instance start offset into the flat array. Shape is (num_instances,). - num_particles: Number of particles per instance. dst: Output 2D array. Shape is (num_instances, num_particles). """ i, j = wp.tid() @@ -181,68 +181,12 @@ def compute_mean_vec3f_over_vertices( result[i] = acc / float(num_vertices) -@wp.kernel -def scatter_zero_vel_index( - env_ids: wp.array(dtype=wp.int32), - offsets: wp.array(dtype=wp.int32), - num_particles: int, - dst: wp.array(dtype=wp.vec3f), -): - """Zero the velocity of particles for selected environments. - - Args: - env_ids: Environment indices to zero velocities for. Shape is (num_selected,). - offsets: Per-instance start offset into the flat array. Shape is (num_instances,). - num_particles: Number of particles per instance. - dst: Flat destination velocity array. Shape is (total_particles,). - """ - i, j = wp.tid() - env_id = env_ids[i] - dst[offsets[env_id] + j] = wp.vec3f(0.0, 0.0, 0.0) - - -@wp.kernel -def scatter_default_pos_index( - default_pos: wp.array2d(dtype=wp.vec3f), - env_ids: wp.array(dtype=wp.int32), - offsets: wp.array(dtype=wp.int32), - dst: wp.array(dtype=wp.vec3f), -): - """Scatter default positions for selected environments into the flat simulation array. - - Args: - default_pos: Default positions per instance. Shape is (num_instances, num_particles). - env_ids: Environment indices to reset. Shape is (num_selected,). - offsets: Per-instance start offset into the flat array. Shape is (num_instances,). - dst: Flat destination particle array. Shape is (total_particles,). - """ - i, j = wp.tid() - env_id = env_ids[i] - dst[offsets[env_id] + j] = default_pos[env_id, j] - - -@wp.kernel -def set_kinematic_flags_to_one( - data: wp.array(dtype=wp.vec4f), -): - """Set the w-component (kinematic flag) of all vec4f entries to 1.0. - - This is used to initialize all vertices as non-kinematic (free) nodes. - - Args: - data: Input/output array of vec4f kinematic targets. Shape is (N*V,). - """ - i = wp.tid() - v = data[i] - data[i] = wp.vec4f(v[0], v[1], v[2], 1.0) - - @wp.kernel def enforce_kinematic_targets( targets: wp.array2d(dtype=wp.vec4f), offsets: wp.array(dtype=wp.int32), - default_inv_mass: wp.array(dtype=wp.float32), - default_flags: wp.array(dtype=wp.int32), + default_inv_mass: wp.array2d(dtype=wp.float32), + default_flags: wp.array2d(dtype=wp.int32), particle_q: wp.array(dtype=wp.vec3f), particle_qd: wp.array(dtype=wp.vec3f), particle_inv_mass: wp.array(dtype=wp.float32), @@ -258,8 +202,8 @@ def enforce_kinematic_targets( targets: Per-instance kinematic targets. Shape is (num_instances, particles_per_body). Each vec4f contains (target_x, target_y, target_z, flag). offsets: Per-instance start offset into the flat particle array. - default_inv_mass: Saved default inverse masses. Shape is (total_particles,). - default_flags: Saved default particle flags. Shape is (total_particles,). + default_inv_mass: Saved inverse masses [1/kg]. Shape is (num_instances, particles_per_body). + default_flags: Saved particle flags. Shape is (num_instances, particles_per_body). particle_q: Flat particle positions to write. Shape is (total_particles,). particle_qd: Flat particle velocities to write. Shape is (total_particles,). particle_inv_mass: Flat particle inverse masses to write. Shape is (total_particles,). @@ -276,5 +220,5 @@ def enforce_kinematic_targets( particle_q[flat_idx] = wp.vec3f(t[0], t[1], t[2]) particle_qd[flat_idx] = wp.vec3f(0.0, 0.0, 0.0) else: - particle_inv_mass[flat_idx] = default_inv_mass[flat_idx] - particle_flags[flat_idx] = default_flags[flat_idx] + particle_inv_mass[flat_idx] = default_inv_mass[i, j] + particle_flags[flat_idx] = default_flags[i, j] diff --git a/source/isaaclab_newton/isaaclab_newton/assets/mpm_object/mpm_object.py b/source/isaaclab_newton/isaaclab_newton/assets/mpm_object/mpm_object.py index 46516ae2cb78..486864307013 100644 --- a/source/isaaclab_newton/isaaclab_newton/assets/mpm_object/mpm_object.py +++ b/source/isaaclab_newton/isaaclab_newton/assets/mpm_object/mpm_object.py @@ -2,12 +2,11 @@ # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause - from __future__ import annotations import logging +import re from collections.abc import Sequence -from dataclasses import dataclass, field from typing import TYPE_CHECKING import numpy as np @@ -15,9 +14,7 @@ import warp as wp from isaaclab.assets.deformable_object.base_deformable_object import BaseDeformableObject -from isaaclab.cloner import path as cloner_path from isaaclab.physics import PhysicsEvent -from isaaclab.sim import SimulationContext from isaaclab.utils.warp import ProxyArray from isaaclab_newton.physics import NewtonManager as SimulationManager @@ -35,123 +32,11 @@ from .mpm_object_data import MPMObjectData if TYPE_CHECKING: - from newton import ModelBuilder - from .mpm_object_cfg import MPMObjectCfg logger = logging.getLogger(__name__) -# TODO: Remove this range registry once https://github.com/newton-physics/newton/issues/4225 -# exposes composed MPM ranges. -@dataclass -class MPMObjectRegistryEntry: - """Particle ranges imported for an MPM object.""" - - cfg: MPMObjectCfg - particle_offsets: list[int] = field(default_factory=list) - particles_per_object: int = 0 - - -def reset_registered_mpm_particle_ranges() -> None: - """Clear particle ranges before rebuilding the Newton model.""" - for entry in SimulationManager._mpm_object_registry: - entry.particle_offsets.clear() - entry.particles_per_object = 0 - - -def record_registered_mpm_particle_ranges( - path_particle_map: dict[str, tuple[int, int]], - destination_particle_offset: int = 0, - *, - builder: ModelBuilder | None = None, - source_builder: ModelBuilder | None = None, - source_xform: Sequence[float] | None = None, -) -> None: - """Record MPM particle ranges from a USD import. - - Args: - path_particle_map: Source-local half-open particle ranges keyed by USD prim path. - destination_particle_offset: Particle count before merging a source builder. - builder: Optional destination builder containing the merged particles. - source_builder: Optional source builder containing the imported particles. - source_xform: Optional source-to-destination transform as ``(x, y, z, qx, qy, qz, qw)``. - """ - transform_args = (builder, source_builder, source_xform) - if any(value is not None for value in transform_args) and not all(value is not None for value in transform_args): - raise ValueError("builder, source_builder, and source_xform must be provided together.") - - for entry in SimulationManager._mpm_object_registry: - points_path = f"{entry.cfg.spawn.spawn_path}{_SIMULATION_POINTS_SUFFIX}" - if particle_range := path_particle_map.get(points_path): - start, end = particle_range - particle_count = int(end - start) - if particle_count <= 0: - raise ValueError(f"MPM particle range for '{points_path}' must be non-empty; got {particle_range}.") - if entry.particles_per_object not in (0, particle_count): - raise RuntimeError( - f"MPM object '{entry.cfg.prim_path}' imported {particle_count} particles, " - f"but earlier instances imported {entry.particles_per_object}." - ) - - destination_start = destination_particle_offset + int(start) - if builder is not None and source_builder is not None and source_xform is not None: - _restore_imported_mpm_transform( - builder, - source_builder, - int(start), - int(end), - destination_start, - source_xform, - points_path, - ) - entry.particles_per_object = particle_count - entry.particle_offsets.append(destination_start) - - -def _restore_imported_mpm_transform( - builder: ModelBuilder, - source_builder: ModelBuilder, - source_start: int, - source_end: int, - destination_start: int, - source_xform: Sequence[float], - points_path: str, -) -> None: - """Apply the rotation that Newton builder merging omits for particles.""" - # TODO: Remove this workaround when https://github.com/newton-physics/newton/issues/4115 is fixed. - particle_count = source_end - source_start - destination_end = destination_start + particle_count - if source_start < 0 or source_end > source_builder.particle_count: - raise ValueError( - f"MPM source range {(source_start, source_end)} for '{points_path}' exceeds " - f"the source builder's {source_builder.particle_count} particles." - ) - if destination_start < 0 or destination_end > builder.particle_count: - raise ValueError( - f"MPM destination range {(destination_start, destination_end)} for '{points_path}' exceeds " - f"the destination builder's {builder.particle_count} particles." - ) - - xform = np.asarray(source_xform, dtype=np.float32) - if xform.shape != (7,) or not np.all(np.isfinite(xform)): - raise ValueError(f"MPM source transform for '{points_path}' must contain seven finite values; got {xform}.") - rotation = xform[3:] - if np.array_equal(rotation, np.array((0.0, 0.0, 0.0, 1.0), dtype=np.float32)): - return - - positions = np.asarray(source_builder.particle_q[source_start:source_end], dtype=np.float32) - velocities = np.asarray(source_builder.particle_qd[source_start:source_end], dtype=np.float32) - builder.particle_q[destination_start:destination_end] = (_rotate_vectors(positions, rotation) + xform[:3]).tolist() - builder.particle_qd[destination_start:destination_end] = _rotate_vectors(velocities, rotation).tolist() - - -def _rotate_vectors(vectors: np.ndarray, quaternion: np.ndarray) -> np.ndarray: - """Rotate vectors by one ``xyzw`` quaternion.""" - twice_cross = 2.0 * np.cross(quaternion[:3], vectors) - return vectors + quaternion[3] * twice_cross + np.cross(quaternion[:3], twice_cross) - - class MPMObject(BaseDeformableObject): """Newton MPM particle object asset. @@ -170,8 +55,6 @@ class MPMObject(BaseDeformableObject): def __init__(self, cfg: MPMObjectCfg): super().__init__(cfg) - self._registry_entry = MPMObjectRegistryEntry(self.cfg) - SimulationManager._mpm_object_registry.append(self._registry_entry) self._physics_ready_handle = None @property @@ -341,16 +224,16 @@ def _as_warp(self, data, dtype, shape: tuple[int, int], name: str) -> wp.array: return data def _initialize_impl(self): - entry = self._registry_entry - self._num_instances = len(entry.particle_offsets) - self._particles_per_object = entry.particles_per_object - self._recorded_particle_offsets = entry.particle_offsets - - if self._num_instances == 0 or self._particles_per_object == 0: - raise RuntimeError( - f"No MPM particle instances found for '{self.cfg.prim_path}'. " - "Ensure Newton replication processed the MPM object registry." - ) + expression = re.compile(self.cfg.prim_path + _SIMULATION_POINTS_SUFFIX) + ranges = [ + value for path, value in SimulationManager.backend.particle_ranges.items() if expression.fullmatch(path) + ] + if not ranges: + raise RuntimeError(f"No imported MPM particles match '{self.cfg.prim_path}'.") + offsets, counts = zip(*ranges, strict=True) + if len(set(counts)) != 1: + raise ValueError(f"MPM instances at '{self.cfg.prim_path}' must have equal particle counts.") + self._num_instances, self._particles_per_object = len(ranges), counts[0] logger.info( "Newton MPM object initialized at '%s': %d instances x %d particles.", @@ -359,7 +242,7 @@ def _initialize_impl(self): self._particles_per_object, ) - self._particle_offsets = wp.array(self._recorded_particle_offsets, dtype=wp.int32, device=self.device) + self._particle_offsets = wp.array(offsets, dtype=wp.int32, device=self.device) self._data = MPMObjectData( particle_offsets=self._particle_offsets, particles_per_object=self._particles_per_object, @@ -409,27 +292,6 @@ def _create_buffers(self): ) self._data.default_nodal_state_w = ProxyArray(default_state) self._data.default_particle_state_w = self._data.default_nodal_state_w - self._bind_particle_visualization() - - def _bind_particle_visualization(self) -> None: - """Bind the plan's pre-authored render clouds to native particle ranges.""" - if not self.cfg.spawn.visible: - return - - plan = SimulationContext.instance().get_clone_plan() - sources = cloner_path.get_asset_prototype_paths(plan) - asset_ids = {index for index, source in enumerate(sources) if source == self.cfg.spawn.spawn_path} - templates, starts = cloner_path.get_world_prototype_asset_templates(plan) - asset_prim_paths = [] - for world, prototype in enumerate((-1, *plan.topology.world_prototype_layout), -1): - for index in range(*starts[prototype + 1 : prototype + 3]): - if plan.topology.world_prototypes[index] in asset_ids: - asset_prim_paths.append(templates[index].format(world)) - for prim_path, offset in zip(asset_prim_paths, self._recorded_particle_offsets, strict=True): - SimulationManager.register_particle_visual_prim( - f"{prim_path}/Particles", particle_offset=offset, particle_count=self._particles_per_object - ) - logger.info("MPM particle visualization initialized for: %s", self.cfg.prim_path) def _resolve_env_ids(self, env_ids): if env_ids is None or (isinstance(env_ids, slice) and env_ids == slice(None)): @@ -481,6 +343,3 @@ def _clear_callbacks(self) -> None: if self._physics_ready_handle is not None: self._physics_ready_handle.deregister() self._physics_ready_handle = None - registry = SimulationManager._mpm_object_registry - if self._registry_entry in registry: - registry.remove(self._registry_entry) diff --git a/source/isaaclab_newton/isaaclab_newton/assets/mpm_object/mpm_object_data.py b/source/isaaclab_newton/isaaclab_newton/assets/mpm_object/mpm_object_data.py index 4cd775cdf7ca..3f6cb4b9dc6f 100644 --- a/source/isaaclab_newton/isaaclab_newton/assets/mpm_object/mpm_object_data.py +++ b/source/isaaclab_newton/isaaclab_newton/assets/mpm_object/mpm_object_data.py @@ -8,7 +8,7 @@ import warp as wp from isaaclab.assets.deformable_object.base_deformable_object_data import BaseDeformableObjectData -from isaaclab.utils.buffers import TimestampedBuffer +from isaaclab.utils.buffers import TimestampedBuffer, reset_timestamps from isaaclab.utils.warp import ProxyArray from isaaclab_newton.physics import NewtonManager as SimulationManager @@ -43,31 +43,17 @@ def __init__(self, particle_offsets: wp.array, particles_per_object: int, num_in self.default_particle_state_w: ProxyArray | None = None self.nodal_kinematic_target: ProxyArray | None = None - self._create_simulation_bindings() - def _create_simulation_bindings(self) -> None: - """Validate current Newton particle arrays and invalidate gathered buffers.""" - self._get_current_particle_state() - self._particle_pos_w.timestamp = -1.0 - self._particle_vel_w.timestamp = -1.0 - self._particle_state_w.timestamp = -1.0 - self._root_pos_w.timestamp = -1.0 - self._root_vel_w.timestamp = -1.0 - - def _get_current_particle_state(self): - state = SimulationManager.get_state_0() - if state is None or state.particle_q is None or state.particle_qd is None: - raise RuntimeError( - "Failed to access Newton MPM particle state. Ensure the Newton model has been finalized and contains " - "particle position and velocity arrays." - ) - return state + """Invalidate gathered buffers after model reinitialization.""" + reset_timestamps( + (self._particle_pos_w, self._particle_vel_w, self._particle_state_w, self._root_pos_w, self._root_vel_w) + ) @property def particle_pos_w(self) -> ProxyArray: """Particle positions in simulation world frame [m].""" if self._particle_pos_w.timestamp < self._sim_timestamp: - state = self._get_current_particle_state() + state = SimulationManager.get_state_0() wp.launch( gather_particles_vec3f, dim=(self._num_instances, self._particles_per_object), @@ -82,7 +68,7 @@ def particle_pos_w(self) -> ProxyArray: def particle_vel_w(self) -> ProxyArray: """Particle velocities in simulation world frame [m/s].""" if self._particle_vel_w.timestamp < self._sim_timestamp: - state = self._get_current_particle_state() + state = SimulationManager.get_state_0() wp.launch( gather_particles_vec3f, dim=(self._num_instances, self._particles_per_object), diff --git a/source/isaaclab_newton/isaaclab_newton/cloner/newton_clone_utils.py b/source/isaaclab_newton/isaaclab_newton/cloner/newton_clone_utils.py index aed510039f73..92f4c5dea162 100644 --- a/source/isaaclab_newton/isaaclab_newton/cloner/newton_clone_utils.py +++ b/source/isaaclab_newton/isaaclab_newton/cloner/newton_clone_utils.py @@ -5,103 +5,85 @@ from __future__ import annotations -from collections.abc import Callable, Iterator, Sequence -from contextlib import contextmanager +from collections.abc import Callable, Sequence from typing import Any import numpy as np import warp as wp -from newton import GeoType, JointType, ModelBuilder, ShapeFlags +from newton import JointType, ModelBuilder, ShapeFlags -from pxr import Usd, UsdGeom, UsdPhysics +from pxr import Sdf, Usd, UsdGeom, UsdPhysics, UsdShade from isaaclab.cloner import ClonePlan from isaaclab.cloner import path as clone_path +from isaaclab.scene_data.deformable_discovery import DeformableStageEntry, deformable_entry from isaaclab.sim.utils.newton_model_utils import replace_newton_builder_shape_colors +from isaaclab.utils.string import to_camel_case from isaaclab_newton.renderers.visual_material import import_builder_visual_material_paths +from isaaclab_newton.sim.spawners.materials import ( + NewtonDeformableBodyMaterialCfg, + NewtonSurfaceDeformableBodyMaterialCfg, +) -def _has_visible_non_collision_geometry(stage: Usd.Stage, prim_path: str) -> bool: - """Return whether a prim hierarchy contains visible geometry without collision.""" - root_prim = stage.GetPrimAtPath(prim_path) - if not root_prim: - return False - for prim in Usd.PrimRange(root_prim): - if not prim.IsA(UsdGeom.Gprim) or prim.HasAPI(UsdPhysics.CollisionAPI): - continue - imageable = UsdGeom.Imageable(prim) - is_visible = imageable.ComputeVisibility() != UsdGeom.Tokens.invisible - if is_visible and imageable.ComputePurpose() in (UsdGeom.Tokens.default_, UsdGeom.Tokens.proxy): - return True - return False - - -def _static_collider_owner_path(stage: Usd.Stage, collider_path: str) -> str: - """Return the nearest rigid-body ancestor or the collider's immediate parent.""" - collider_prim = stage.GetPrimAtPath(collider_path) - prim = collider_prim.GetParent() if collider_prim else None - while prim and not prim.IsPseudoRoot(): - if prim.HasAPI(UsdPhysics.RigidBodyAPI): - return str(prim.GetPath()) - prim = prim.GetParent() - return collider_path.rpartition("/")[0] - - -def _restore_visible_colliders_without_visual_shapes( - builder: ModelBuilder, - stage: Usd.Stage, - path_shape_map: dict[str, int] | None, - load_visual_shapes: bool = True, -) -> None: - """Show viewport-visible colliders on bodies without separate visual shapes. - - Newton groups static colliders under the world body, where unrelated visual - geometry can hide them. Isaac Lab procedural shapes use one default-purpose USD - geometry for both collision and visualization. Imported collision meshes, - guide-purpose geometry, and colliders with separate visuals remain hidden. +def add_deformable_from_usd(builder: ModelBuilder, stage: Usd.Stage, *, root_path: str) -> DeformableStageEntry: + """Import one declared deformable's geometry, Newton material, and native element ranges. - With ``load_visual_shapes=False`` the pass is skipped: Newton never hides a collider - when the model holds no visual-only shapes, so every flag it would set is already set, - and nothing draws them in a run that opted out of visual geometry. The skipped USD - visibility/purpose resolution is per collider shape, so it is worth avoiding. + Args: + builder: Source builder that receives the complete deformable prototype. + stage: Stage containing the authored geometry and bound physics material. + root_path: Deformable-body prim path. + + Returns: + Prototype geometry for SDP's visual-mesh binding. + + Note: + Replace this importer with native ``add_usd`` when Isaac Lab's authored schemas and + Newton material attributes have parity (newton-physics/newton#3036 and #3038). + The native USD importer landed in #3192; that alone does not establish material parity. + Remove private group recording when the pinned Newton includes #3326's native recording. """ - if not path_shape_map or not load_visual_shapes: - return - # Newton may synthesize a visible ``*_visual`` mesh for a proxy-purpose collider. - # It is not an authored USD prim and must remain hidden alongside its source collider. - for index, path in enumerate(builder.shape_label): - if not path.endswith("_visual") or stage.GetPrimAtPath(path): - continue - collider_prim = stage.GetPrimAtPath(path.removesuffix("_visual")) - if collider_prim and collider_prim.HasAPI(UsdPhysics.CollisionAPI): - builder.shape_flags[index] &= ~ShapeFlags.VISIBLE - bodies_with_visual_shapes = set() - for body, flags in zip(builder.shape_body, builder.shape_flags, strict=True): - is_visible = flags & ShapeFlags.VISIBLE - is_collider = flags & ShapeFlags.COLLIDE_SHAPES - if body >= 0 and is_visible and not is_collider: - bodies_with_visual_shapes.add(body) - # Resolved on first use: a static parent whose colliders are all filtered out below is - # never traversed at all. - static_owners_with_visual_shapes: dict[str, bool] = {} - for path, index in path_shape_map.items(): - flags, body_index = builder.shape_flags[index], builder.shape_body[index] - is_collider = bool(flags & ShapeFlags.COLLIDE_SHAPES) - is_mesh = builder.shape_type[index] == GeoType.MESH - has_visuals = body_index in bodies_with_visual_shapes - if not is_collider or is_mesh or has_visuals: - continue - if body_index < 0: - owner_path = _static_collider_owner_path(stage, path) - if owner_path not in static_owners_with_visual_shapes: - static_owners_with_visual_shapes[owner_path] = _has_visible_non_collision_geometry(stage, owner_path) - if static_owners_with_visual_shapes[owner_path]: - continue - imageable = UsdGeom.Imageable(stage.GetPrimAtPath(path)) - is_visible = imageable and imageable.ComputeVisibility() != UsdGeom.Tokens.invisible - if is_visible and imageable.ComputePurpose() in (UsdGeom.Tokens.default_, UsdGeom.Tokens.proxy): - builder.shape_flags[index] = flags | ShapeFlags.VISIBLE + prim = stage.GetPrimAtPath(root_path) + geometry = deformable_entry(prim) + if geometry is None: + raise ValueError(f"No simulation mesh found under deformable {root_path!r}.") + material = UsdShade.MaterialBindingAPI(prim).ComputeBoundMaterial(materialPurpose="physics")[0].GetPrim() + if not material: + raise ValueError(f"Deformable {root_path!r} requires a bound Newton physics material.") + if geometry.deformable_type == "volume": + add_mesh = builder.add_soft_mesh + defaults = NewtonDeformableBodyMaterialCfg() + names = ("density", "particle_radius", "k_mu", "k_lambda", "k_damp") + else: + add_mesh = builder.add_cloth_mesh + defaults = NewtonSurfaceDeformableBodyMaterialCfg() + names = ("density", "particle_radius", "tri_ke", "tri_ka", "tri_kd", "edge_ke", "edge_kd") + material_kwargs = {} + for name in names: + attr = material.GetAttribute(f"newton:{to_camel_case(name, to='cC')}") + material_kwargs[name] = attr.Get() if attr.IsValid() else getattr(defaults, name) + + particle_start, tri_start = builder.particle_count, len(builder.tri_indices) + edge_start, tet_start = len(builder.edge_indices), len(builder.tet_indices) + add_mesh( + vertices=geometry.vertices, + indices=geometry.indices, + pos=wp.vec3(*geometry.init_pos), + rot=wp.quat(*geometry.init_rot), + scale=1.0, + vel=wp.vec3(), + label=root_path, + **material_kwargs, + ) + particle_range = (particle_start, builder.particle_count) + if geometry.deformable_type == "volume": + builder._record_soft_group(root_path, particle_range, (tet_start, len(builder.tet_indices))) + else: + builder._record_cloth_group( + root_path, particle_range, (tri_start, len(builder.tri_indices)), (edge_start, len(builder.edge_indices)) + ) + return geometry def build_source_builders( @@ -151,11 +133,36 @@ def build_source_builders( ], return_deformable_results=True, ) - _restore_visible_colliders_without_visual_shapes( - builder, stage, import_result["path_shape_map"], load_visual_shapes - ) replace_newton_builder_shape_colors(builder, stage) if load_visual_shapes: + # Newton groups all static shapes under body -1. Use imported visual paths to + # keep an unrelated static visual from hiding a procedural primitive collider. + shapes = import_result["path_shape_map"] + static_shapes = {path: index for path, index in shapes.items() if builder.shape_body[index] == -1} + visual_ancestors = set() + for path, index in static_shapes.items(): + flags = builder.shape_flags[index] + if flags & ShapeFlags.VISIBLE and not flags & ShapeFlags.COLLIDE_SHAPES: + visual_ancestors.update(Sdf.Path(path).GetPrefixes()) + for path, index in static_shapes.items(): + flags = builder.shape_flags[index] + if flags & ShapeFlags.VISIBLE or not flags & ShapeFlags.COLLIDE_SHAPES: + continue + prim = stage.GetPrimAtPath(path) + if prim.IsA(UsdGeom.Mesh): + continue + owner, ancestor = prim.GetParent(), prim.GetParent() + while ancestor and not ancestor.IsPseudoRoot(): + if ancestor.HasAPI(UsdPhysics.RigidBodyAPI): + owner = ancestor + break + ancestor = ancestor.GetParent() + if owner.GetPath() in visual_ancestors: + continue + imageable = UsdGeom.Imageable(prim) + visible = imageable.ComputeVisibility() != UsdGeom.Tokens.invisible + if visible and imageable.ComputePurpose() in (UsdGeom.Tokens.default_, UsdGeom.Tokens.proxy): + builder.shape_flags[index] |= ShapeFlags.VISIBLE import_builder_visual_material_paths(builder, stage) _name_root_joints_after_their_body(builder) builders[source] = builder @@ -176,18 +183,6 @@ def _name_root_joints_after_their_body(builder: ModelBuilder) -> None: builder.joint_label[index] = f"{body_label}_free_joint" -def _quat_multiply(a: np.ndarray, b: np.ndarray) -> np.ndarray: - """Hamilton product of xyzw quaternion arrays, broadcast over the leading axes.""" - ax, ay, az, aw = a[..., 0], a[..., 1], a[..., 2], a[..., 3] - bx, by, bz, bw = b[..., 0], b[..., 1], b[..., 2], b[..., 3] - out = np.empty(np.broadcast_shapes(a.shape, b.shape), dtype=np.float32) - out[..., 0] = aw * bx + ax * bw + ay * bz - az * by - out[..., 1] = aw * by - ax * bz + ay * bw + az * bx - out[..., 2] = aw * bz + ax * by - ay * bx + az * bw - out[..., 3] = aw * bw - ax * bx - ay * by - az * bz - return out - - def _quat_rotate(q: np.ndarray, v: np.ndarray) -> np.ndarray: """Rotate vectors ``v`` by xyzw quaternions ``q``, broadcast over the leading axes.""" axis, angle_w = q[..., :3], q[..., 3:4] @@ -195,20 +190,24 @@ def _quat_rotate(q: np.ndarray, v: np.ndarray) -> np.ndarray: return (v + angle_w * t + np.cross(axis, t)).astype(np.float32) -def _compose_world_xforms(world_p: np.ndarray, world_q: np.ndarray, local: Sequence[float]) -> np.ndarray: - """``world_xform_w * local`` for every world, as one ``[num_worlds, 7]`` xyzw array.""" - local = np.asarray(local, dtype=np.float32) - out = np.empty((world_p.shape[0], 7), dtype=np.float32) - out[:, :3] = world_p + _quat_rotate(world_q, np.broadcast_to(local[:3], world_p.shape)) - out[:, 3:] = _quat_multiply(world_q, local[3:]) - return out - - -def _invert_xform(xform: Sequence[float] | np.ndarray) -> np.ndarray: - """Inverse of a single xyzw transform, assuming a unit quaternion.""" - xform = np.asarray(xform, dtype=np.float32) - quat_inv = np.array([-xform[3], -xform[4], -xform[5], xform[6]], dtype=np.float32) - return np.concatenate([-_quat_rotate(quat_inv, xform[:3]), quat_inv]) +def _rotate_builder_particles( + builder: ModelBuilder, source: ModelBuilder, particle_start: int, tet_start: int, xform: np.ndarray +) -> None: + """Apply particle and rest-frame rotations omitted by Newton's builder composition.""" + # Remove when the pinned Newton fixes newton-physics/newton#4115, including tet rest frames. + if source.particle_count == 0 or np.array_equal(xform[3:], (0.0, 0.0, 0.0, 1.0)): + return + particle_slice = slice(particle_start, particle_start + source.particle_count) + builder.particle_q[particle_slice] = ( + _quat_rotate(xform[3:], np.asarray(source.particle_q, dtype=np.float32)) + xform[:3] + ).tolist() + builder.particle_qd[particle_slice] = _quat_rotate( + xform[3:], np.asarray(source.particle_qd, dtype=np.float32) + ).tolist() + if source.tet_count: + # Dm^-1 rotates on the right: (R Dm)^-1 = Dm^-1 R^T. + poses = np.asarray(source.tet_poses, dtype=np.float32).reshape(-1, 3, 3) + builder.tet_poses[tet_start : tet_start + source.tet_count] = _quat_rotate(xform[3:], poses).tolist() def _label_groups(builder: ModelBuilder) -> dict[str, list]: @@ -223,55 +222,55 @@ def _label_groups(builder: ModelBuilder) -> dict[str, list]: return groups -@contextmanager -def _rebase_builder_paths( - builder: ModelBuilder, source: str, destination: str, prefix: str, reference_paths: Sequence[tuple[str, str]] -) -> Iterator[None]: - """Borrow one prototype with instance-relative labels, restoring its names after the copy.""" - builder._resolve_custom_frequency_articulation_owners() - labels = _label_groups(builder) - world_frequencies = {attr.frequency for attr in builder.custom_attributes.values() if attr.references == "world"} - paths = [] - for attr in builder.custom_attributes.values(): - if attr.dtype is not str: - continue - is_world_path = attr.frequency in world_frequencies - is_material_path = attr.namespace == "isaaclab" and attr.name == "visual_material_path" - if (is_world_path or is_material_path) and not any(attr.values is values for values in labels.values()): - paths.append(attr) - original_labels = {name: list(values) for name, values in labels.items()} - original_paths = [attr.values.copy() for attr in paths] - source, destination = source.rstrip("/") or "/", destination.rstrip("/") or "/" +def _add_asset_builder( + builder: ModelBuilder, + asset: ModelBuilder, + source: str, + destination: str, + prefix: str, + reference_paths: Sequence[tuple[str, str]], + xform: np.ndarray, +) -> None: + """Copy one asset into a world prototype without mutating the imported source paths.""" + offsets, tet_start = ModelBuilder._builder_merge_counts(builder), builder.tet_count + builder.add_builder(asset, xform=xform) + # Name only the appended copy. The imported asset keeps its labels and references; + # native replication later adds world prefixes to this composed builder. + labels, copied_labels = _label_groups(asset), _label_groups(builder) + target = destination[len(prefix) :] + for name, values in labels.items(): + copied = copied_labels[name] + for index, label in enumerate(values, len(copied) - len(values)): + if not isinstance(label, str) or not label.startswith("/"): + continue + suffix = clone_path.relative_to(label, source) + if suffix is None and label.startswith(source + "_"): + suffix = label[len(source) :] + if suffix is None or not target + suffix: + raise ValueError(f"Newton label {label!r} cannot be cloned from {source!r} to {destination!r}.") + copied[index] = (target + suffix).lstrip("/") if prefix else target + suffix destinations = dict(reference_paths) destinations[source] = destination - sources = sorted(destinations, key=len, reverse=True) - try: - for values in labels.values(): - for index, label in enumerate(values): - if not isinstance(label, str) or not label.startswith("/"): - continue - suffix = clone_path.relative_to(label, source) - if suffix is None: - suffix = label[len(source) :] if label.startswith(source + "_") else None - if suffix is None: - raise ValueError(f"Newton label {label!r} is outside clone source {source!r}.") - values[index] = (destination[len(prefix) :] + suffix).lstrip("/") if prefix else destination + suffix - if not values[index]: - raise ValueError(f"Newton label {label!r} cannot be prefixed by destination {destination!r}.") - for attr in paths: - for index in attr.values if isinstance(attr.values, dict) else range(len(attr.values)): - value = attr.values[index] - if isinstance(value, str): - for root in sources: - if (suffix := clone_path.relative_to(value, root)) is not None: - attr.values[index] = destinations[root].rstrip("/") + suffix or "/" - break - yield - finally: - for name, values in original_labels.items(): - labels[name][:] = values - for attr, values in zip(paths, original_paths, strict=True): - attr.values = values + roots = sorted(destinations, key=len, reverse=True) + attrs = asset.custom_attributes + world_frequencies = {attr.frequency for attr in attrs.values() if attr.references == "world"} + for name, attr in attrs.items(): + if attr.dtype is not str or any(attr.values is values for values in labels.values()): + continue + if attr.frequency not in world_frequencies and name != "isaaclab:visual_material_path": + continue + copied = builder.custom_attributes[name].values + if isinstance(attr.values, dict): + offset, items = offsets[attr.frequency.name.lower()], attr.values.items() + else: + offset, items = len(copied) - len(attr.values), enumerate(attr.values) + for index, value in items: + if isinstance(value, str): + for root in roots: + if (suffix := clone_path.relative_to(value, root)) is not None: + copied[offset + index] = destinations[root].rstrip("/") + suffix or "/" + break + _rotate_builder_particles(builder, asset, offsets["particle"], tet_start, xform) def replicate_builder_mapping( @@ -285,8 +284,8 @@ def replicate_builder_mapping( source_site_indices: dict[int, dict[str, list[int]]] | None = None, env_root_sites: dict[str, wp.transform] | None = None, per_world_builder_hooks: Sequence[Callable[[ModelBuilder, int, np.ndarray, np.ndarray], None]] = (), - source_builder_added: Callable[[str, int, ModelBuilder, Sequence[float]], None] | None = None, -) -> tuple[dict[str, list[list[int]]], list[wp.transform], list[tuple[str, int]]]: + source_builder_added: Callable[[str, str, int, int], None] | None = None, +) -> tuple[dict[str, list[list[int]]], list[wp.transform]]: """Compose routed source builders once per world prototype, then batch their selected worlds.""" topology, env_template = plan.topology, plan.env_template layout = topology.world_prototype_layout @@ -307,48 +306,50 @@ def replicate_builder_mapping( reference_paths = [(sources[topology.world_prototypes[index]], templates[index]) for index in range(start, end)] components = [(source, template) for source, template in reference_paths if source in source_builders] prototype = builder if prototype_id == -1 else ModelBuilder(up_axis=builder.up_axis) - sites, particle_offsets = {}, [] + sites, asset_offsets = {}, [] for source, destination in components: # Remove the source world's placement before composing assets into other world prototypes. if source not in source_inverse: source_inverse[source] = ( np.asarray(wp.transform(), dtype=np.float32) if first_world == -1 or clone_path.match(source, env_template) is None - else _invert_xform(xforms_np[first_world]) + else np.asarray(wp.transform_inverse(world_xforms[first_world]), dtype=np.float32) ) asset = source_builders[source] - particle_offsets.append(prototype.particle_count) + asset_offsets.append((prototype.shape_count, prototype.particle_count)) for label, indices in source_site_indices.get(id(asset), {}).items(): sites.setdefault(label, []).extend(prototype.shape_count + index for index in indices) prefix = "" if prototype_id == -1 else env_template - with _rebase_builder_paths(asset, source, destination, prefix, reference_paths): - prototype.add_builder(asset, xform=source_inverse[source]) + _add_asset_builder(prototype, asset, source, destination, prefix, reference_paths, source_inverse[source]) if prototype_id == -1: for label, indices in sites.items(): local_site_map[label] = np.tile(indices, (num_worlds, 1)).tolist() if source_builder_added is not None: - for (source, _), offset in zip(components, particle_offsets, strict=True): - source_builder_added(source, offset, source_builders[source], source_inverse[source]) + for (source, destination), offsets in zip(components, asset_offsets, strict=True): + source_builder_added(source, destination, *offsets) continue for label, xform in env_root_sites.items(): sites.setdefault(label, []).append(prototype.add_site(body=-1, xform=xform, label=label)) - world_builders[prototype_id] = prototype, sites, components, particle_offsets + world_builders[prototype_id] = prototype, sites, components, asset_offsets # Preserve destination order, batching each contiguous run of an identical world prototype. boundaries = np.r_[0, np.flatnonzero(np.diff(layout)) + 1, num_worlds] for start, stop in zip(boundaries[:-1], boundaries[1:], strict=True): if start == stop: continue - prototype, sites, components, particle_offsets = world_builders[layout[start]] - base_shape, base_particle = builder.shape_count, builder.particle_count + prototype, sites, components, asset_offsets = world_builders[layout[start]] + base_shape, base_particle, base_tet = builder.shape_count, builder.particle_count, builder.tet_count shape_offsets = base_shape + np.arange(stop - start) * prototype.shape_count particle_bases = base_particle + np.arange(stop - start) * prototype.particle_count if per_world_builder_hooks: for world in range(start, stop): shape_offsets[world - start] = builder.shape_count particle_bases[world - start] = builder.particle_count + tet_start = builder.tet_count builder.begin_world() builder.add_builder(prototype, xform=xforms_np[world], label_prefix=env_template.format(env_ids[world])) + offset = particle_bases[world - start] + _rotate_builder_particles(builder, prototype, offset, tet_start, xforms_np[world]) labels = _label_groups(builder) label_starts = {name: len(values) for name, values in labels.items()} for hook in per_world_builder_hooks: @@ -361,16 +362,21 @@ def replicate_builder_mapping( else: prefixes = [env_template.format(env_ids[world]) for world in range(start, stop)] builder.replicate(prototype, int(stop - start), xforms=xforms_np[start:stop], label_prefixes=prefixes) + if prototype.particle_count: + for world in np.flatnonzero(np.any(xforms_np[start:stop, 3:] != (0.0, 0.0, 0.0, 1.0), axis=1)): + tet_start = base_tet + world * prototype.tet_count + _rotate_builder_particles( + builder, prototype, particle_bases[world], tet_start, xforms_np[start + world] + ) for label, indices in sites.items(): per_world = local_site_map.setdefault(label, [[] for _ in range(num_worlds)]) per_world[start:stop] = (shape_offsets[:, None] + np.asarray(indices)).tolist() if source_builder_added is not None: - for (source, _), offset in zip(components, particle_offsets, strict=True): - transforms = _compose_world_xforms( - positions[start:stop], quaternions[start:stop], source_inverse[source] - ) - for index, xform in enumerate(transforms): - source_builder_added(source, int(particle_bases[index]) + offset, source_builders[source], xform) + for (source, destination), (shape, particle) in zip(components, asset_offsets, strict=True): + for index, world in enumerate(range(start, stop)): + target = destination.format(env_ids[world]) + offsets = int(shape_offsets[index]) + shape, int(particle_bases[index]) + particle + source_builder_added(source, target, *offsets) # Resolve remaining world-slot placeholders using each native element's owning world. worlds_by_frequency = { attr.frequency: attr.values for attr in builder.custom_attributes.values() if attr.references == "world" @@ -386,4 +392,4 @@ def replicate_builder_mapping( value = attr.values[index] if isinstance(value, str) and "{}" in value and worlds[index] is not None: attr.values[index] = value.format(env_ids[worlds[index]]) - return local_site_map, world_xforms, [(label, index) for index, label in enumerate(builder.body_label)] + return local_site_map, world_xforms diff --git a/source/isaaclab_newton/isaaclab_newton/cloner/replicate.py b/source/isaaclab_newton/isaaclab_newton/cloner/replicate.py index bf8339d3a95a..03b004a23df7 100644 --- a/source/isaaclab_newton/isaaclab_newton/cloner/replicate.py +++ b/source/isaaclab_newton/isaaclab_newton/cloner/replicate.py @@ -15,18 +15,29 @@ import warp as wp from newton import ModelBuilder -from pxr import Usd, UsdGeom +from pxr import Sdf, Usd, UsdGeom from isaaclab.assets import AssetBaseCfg from isaaclab.cloner import ClonePlan, PrototypeWorldTopology from isaaclab.cloner import path as cloner_path from isaaclab.physics import PhysicsEvent, PhysicsManager -from isaaclab.scene_data.deformable_discovery import deformable_prototypes, expand_deformable_entries +from isaaclab.scene_data import SceneDataFormat +from isaaclab.scene_data.deformable_discovery import ( + deformable_geometry_batches, + deformable_prototypes, + expand_deformable_entries, +) +from isaaclab.sensors import SensorBaseCfg from isaaclab.sim import SpawnerCfg +from isaaclab.sim.utils.queries import has_deformable_body_api -from isaaclab_newton.cloner.newton_clone_utils import build_source_builders, replicate_builder_mapping +from isaaclab_newton.cloner.newton_clone_utils import ( + add_deformable_from_usd, + build_source_builders, + replicate_builder_mapping, +) from isaaclab_newton.physics import NewtonBackendCfg, NewtonCfg, NewtonManager -from isaaclab_newton.physics.visualization_deformables import add_shadow_deformables_to_builder +from isaaclab_newton.sim.spawners.mpm.mpm import _SIMULATION_POINTS_SUFFIX if TYPE_CHECKING: from isaaclab.sim import SimulationContext @@ -98,25 +109,21 @@ def _replicate_newton( quaternions: np.ndarray | None = None, ) -> tuple[ModelBuilder, object, dict]: """Import and replicate the plan's Newton representation, with or without Newton physics.""" - # MPMObject imports NewtonManager, so defer this reciprocal import until model construction. - from isaaclab_newton.assets.mpm_object.mpm_object import ( # noqa: PLC0415 - record_registered_mpm_particle_ranges, - reset_registered_mpm_particle_ranges, - ) - - cfg, topology = sim.cfg.physics, plan.topology + cfg = sim.cfg.physics sources = cloner_path.get_asset_prototype_paths(plan) templates, starts = cloner_path.get_world_prototype_asset_templates(plan) shared = templates[: starts[1]] global_paths = tuple( root for root, parent in zip(shared, cloner_path.get_parent_indices(shared), strict=True) if parent == -1 ) + # A sensor selects an existing body; opting out of cloning must not remove that body from its owner. exclude_paths = tuple( - source for index, source in enumerate(sources) if source is not None and index not in asset_prototype_ids + source + for index, source in enumerate(sources) + if source is not None and index not in asset_prototype_ids + if not isinstance(plan.asset_cfgs[index], SensorBaseCfg) ) simulation = isinstance(cfg, NewtonCfg) - if simulation: - reset_registered_mpm_particle_ranges() if positions is None: positions = np.zeros((len(env_ids), 3), dtype=np.float32) if quaternions is None: @@ -131,46 +138,62 @@ def _replicate_newton( load_visual_shapes = sim.is_rendering or sim.can_render_rgb_array() or sim.visual_shapes_required builder = create_builder() import_paths = (sim.cfg.physics_prim_path, *global_paths) if simulation else global_paths + source_paths = list(dict.fromkeys(sources[index] for index in asset_prototype_ids if sources[index] is not None)) if simulation: - global_ignore_paths = manager_cls._inject_terrain_heightfields(stage, builder, root_paths=import_paths) - # Native deformables are appended by per-world hooks, not the USD importer. - ignore_paths = [] - for entry in NewtonManager._deformable_registry: - for index in range(starts[1], len(templates)): - source = sources[topology.world_prototypes[index]] - if source is not None and (matched := cloner_path.match(entry.prim_path, templates[index])) is not None: - ignore_paths.append(source + matched.suffix) - global_ignore_paths.extend(ignore_paths) - global_ignore_paths.extend(entry.prim_path for entry in NewtonManager._deformable_registry) + deformable_paths = [] + for source in source_paths: + prim = stage.GetPrimAtPath(source) + is_mesh_body = prim and not prim.IsA(UsdGeom.Points) and not prim.IsA(UsdGeom.BasisCurves) + if is_mesh_body and has_deformable_body_api(prim): + deformable_paths.append(source) + ignore_paths = manager_cls._inject_terrain_heightfields(stage, builder, root_paths=import_paths) + ignore_paths.extend((*exclude_paths, *deformable_paths)) else: entries = deformable_prototypes(stage, plan, exclude_paths=exclude_paths) - paths = (path for entry in entries for path in (entry.root_path, entry.sim_mesh_path, entry.vis_mesh_path)) - ignore_paths = list(dict.fromkeys(paths)) - global_ignore_paths = [*exclude_paths, *ignore_paths] + ignore_paths = [*exclude_paths, *(entry.root_path for entry in entries)] stage_info = None if simulation: stage_info = builder.add_usd(stage, root_path=sim.cfg.physics_prim_path, schema_resolvers=schema_resolvers) import_results: dict[str, dict[str, Any]] = {} - source_paths = [sources[index] for index in asset_prototype_ids if sources[index] is not None] - options = dict(ignore_paths=global_ignore_paths, load_visual_shapes=load_visual_shapes) + options = dict(ignore_paths=ignore_paths, load_visual_shapes=load_visual_shapes) options.update(skip_mesh_approximation=not simulation, import_results_out=import_results) source_builders = build_source_builders(stage, source_paths, create_builder, schema_resolvers, **options) - - # Keep only renderable cables from this representation's actual imports. - cable_counts = {} + source_geometry = {source: {} for source in source_builders} + if simulation: + entries = [add_deformable_from_usd(source_builders[path], stage, root_path=path) for path in deformable_paths] + else: + # Import visual meshes once into their owning prototypes, before the common native replication. + for entry in entries: + ancestors = reversed(Sdf.Path(entry.root_path).GetPrefixes()) + source = next(str(path) for path in ancestors if str(path) in source_builders) + native = source_builders[source] + source_geometry[source][entry.vis_mesh_path] = native.particle_count + pose = dict(pos=wp.vec3(*entry.init_pos), rot=wp.quat(*entry.init_rot), scale=1.0, vel=wp.vec3()) + if entry.deformable_type == "surface" or entry.vis_mesh_path != entry.sim_mesh_path: + add_mesh = native.add_cloth_mesh + mesh = dict(vertices=entry.vis_vertices, indices=entry.vis_indices, density=1.0) + mesh.update(tri_ke=1e4, tri_ka=1e4, tri_kd=1.5e-6, edge_ke=5.0, edge_kd=1e-2, particle_radius=0.008) + else: + add_mesh = native.add_soft_mesh + mesh = dict(vertices=entry.vertices, indices=entry.indices, density=1000.0) + mesh.update(k_mu=1e5, k_lambda=1e5, k_damp=0.0) + add_mesh(**mesh, **pose) + + # Resolve native capsule indices once per source, not by rediscovering labels after cloning. + source_cables = {} for source, imported in import_results.items(): + cables = source_cables[source] = {} + if not imported["path_cable_map"]: + continue + shapes = {label: index for index, label in enumerate(source_builders[source].shape_label)} for path, (bodies, _) in imported["path_cable_map"].items(): if imported["path_cable_attrs"][path]["closed"]: continue if len(UsdGeom.BasisCurves(stage.GetPrimAtPath(path)).GetCurveVertexCountsAttr().Get()) != 1: continue - for world, prototype in enumerate((-1, *topology.world_prototype_layout), -1): - for index in range(starts[prototype + 1], starts[prototype + 2]): - if sources[topology.world_prototypes[index]] == source: - target = templates[index].format(-1 if world == -1 else int(env_ids[world])) - cable_counts[cloner_path.rebase(path, source, target)] = len(bodies) + cables[path] = [shapes[f"{path}_edge_capsule_{segment}"] for segment in range(len(bodies))] if simulation: global_sites, source_sites, root_sites = NewtonManager._cl_inject_sites(builder, source_builders) @@ -181,38 +204,57 @@ def _replicate_newton( imported.shape_collision_group[:] = [0] * imported.shape_count global_sites, source_sites, root_sites = {}, {}, {} - def record_particles(source: str, offset: int, source_builder: ModelBuilder, xform: Sequence[float]) -> None: - particles = import_results[source].get("path_particle_map", {}) - record_registered_mpm_particle_ranges( - particles, offset, builder=builder, source_builder=source_builder, source_xform=xform - ) + particle_ranges, visual_ranges, cable_bindings, geometry_offsets = {}, {}, {}, {} + # The USD importer owns simulation ranges; MPM's spawner authors a separate visible point prim. + particle_visual_paths = { + path: path.removesuffix(_SIMULATION_POINTS_SUFFIX) + "/Particles" + for imported in import_results.values() + for path in imported["path_particle_map"] + if path.endswith(_SIMULATION_POINTS_SUFFIX) + and stage.GetPrimAtPath(path.removesuffix(_SIMULATION_POINTS_SUFFIX) + "/Particles") + } + + def record_geometry(source: str, destination: str, shape_offset: int, particle_offset: int) -> None: + for path, shapes in source_cables[source].items(): + cable_bindings[cloner_path.rebase(path, source, destination)] = [shape_offset + shape for shape in shapes] + for path, (start, end) in import_results[source]["path_particle_map"].items(): + native_range = (particle_offset + start, end - start) + particle_ranges[cloner_path.rebase(path, source, destination)] = native_range + if path in particle_visual_paths: + visual_ranges[cloner_path.rebase(particle_visual_paths[path], source, destination)] = native_range + for path, offset in source_geometry[source].items(): + geometry_offsets[cloner_path.rebase(path, source, destination)] = particle_offset + offset options = dict(env_ids=env_ids, source_site_indices=source_sites, env_root_sites=root_sites) options["per_world_builder_hooks"] = NewtonManager._per_world_builder_hooks if simulation else () - options["source_builder_added"] = record_particles if simulation and NewtonManager._mpm_object_registry else None - local_site_map, world_xforms, fabric_body_bindings = replicate_builder_mapping( + has_geometry = any(source_cables.values()) or any(result["path_particle_map"] for result in import_results.values()) + options["source_builder_added"] = record_geometry if has_geometry or any(source_geometry.values()) else None + local_site_map, world_xforms = replicate_builder_mapping( builder, plan, positions, quaternions, source_builders, **options ) site_index_map = {label: (idx, None) for label, idx in global_sites.items()} site_index_map.update((label, (None, per_world)) for label, per_world in local_site_map.items()) - NewtonManager._cable_bindings = {} - if cable_counts: - shape_ids = {label: index for index, label in enumerate(builder.shape_label)} - NewtonManager._cable_bindings = { - path: [shape_ids[f"{path}_edge_capsule_{segment}"] for segment in range(count)] - for path, count in cable_counts.items() - } + NewtonManager._cable_bindings = cable_bindings if simulation: + geometry = expand_deformable_entries(entries, plan, env_ids, positions) + ranges = { + label: start + for family in ("cloth", "soft") + for label, start in zip( + getattr(builder, f"_{family}_label"), getattr(builder, f"_{family}_particle_start"), strict=True + ) + } + offsets = [ranges[entry.root_path] for entry in geometry] + batches = deformable_geometry_batches(geometry, offsets, device=sim.device) + if visual_ranges: + batches.append((SceneDataFormat.Points(), visual_ranges)) + NewtonManager._scene_data_backend._geometry_batches = batches NewtonManager._cl_site_index_map = site_index_map - NewtonManager._cl_fabric_body_bindings = fabric_body_bindings NewtonManager._world_xforms = world_xforms NewtonManager._cl_protos = source_builders - NewtonManager.set_builder(builder) + NewtonManager.set_builder(builder, particle_ranges=particle_ranges) NewtonManager._num_envs = len(env_ids) else: - geometry_offsets = add_shadow_deformables_to_builder( - builder, expand_deformable_entries(entries, plan, env_ids, positions) - ) backend_cfg = NewtonBackendCfg(builder=builder, device=sim.device, num_envs=len(env_ids), simulation=False) sim.physics_manager.register_callback( partial(NewtonManager._initialize_visualization_model, backend_cfg, geometry_offsets), diff --git a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py index 2b358df09c36..b7e6db61ccbd 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py @@ -41,14 +41,10 @@ from pxr import Usd, UsdGeom -from isaaclab.assets import AssetBaseCfg -from isaaclab.cloner import ClonePlan, make_clone_plan -from isaaclab.cloner import path as cloner_path +import isaaclab.cloner as cloner from isaaclab.physics import CallbackHandle, PhysicsEvent, PhysicsManager from isaaclab.scene_data import SceneDataBackend, SceneDataFormat, SceneDataProvider -from isaaclab.sim import SimulationContext, SpawnerCfg -from isaaclab.sim.utils.newton_model_utils import replace_newton_builder_shape_colors -from isaaclab.sim.utils.stage import get_current_stage +from isaaclab.sim import SimulationContext from isaaclab.utils import checked_apply from isaaclab.utils.buffers import TimestampedBuffer from isaaclab.utils.string import resolve_matching_names @@ -56,10 +52,6 @@ from isaaclab.utils.version import has_kit from isaaclab.utils.warp.index_kernel import IndexKernelDispatcher -from isaaclab_newton.cloner.newton_clone_utils import ( - _restore_visible_colliders_without_visual_shapes, - replicate_builder_mapping, -) from isaaclab_newton.physics.featherstone_manager_cfg import FeatherstoneSolverCfg from isaaclab_newton.physics.mjwarp_manager_cfg import MJWarpSolverCfg from isaaclab_newton.physics.newton_manager_cfg import NewtonBackendCfg, NewtonCfg, NewtonShapeCfg, NewtonSolverCfg @@ -67,7 +59,6 @@ from isaaclab_newton.renderers.visual_material import ( VisualMaterialWriter, VisualShapeColorWriter, - import_builder_visual_material_paths, ) if TYPE_CHECKING: @@ -181,6 +172,19 @@ class NewtonBackend: def __init__(self, cfg: NewtonBackendCfg): self.model = cfg.builder.finalize(device=cfg.device) + self.particle_ranges = cfg.particle_ranges + # Newton 1.6 preserves groups through builder replication but not finalization. + # Remove this snapshot when the pinned Newton includes newton-physics/newton#3326. + self.deformable_ranges = { + label: (start, end - start, kind) + for family, kind in (("cloth", "surface"), ("soft", "volume")) + for label, start, end in zip( + getattr(cfg.builder, f"_{family}_label"), + getattr(cfg.builder, f"_{family}_particle_start"), + getattr(cfg.builder, f"_{family}_particle_end"), + strict=True, + ) + } self.model.num_envs = self.model.world_count if cfg.num_envs is None else cfg.num_envs if cfg.gravity is not None: self.model.set_gravity(cfg.gravity) @@ -197,6 +201,8 @@ def __init__(self, cfg: NewtonBackendCfg): def close(self) -> None: """Drop native handles after consumers release their bindings.""" self.control = self.state_1 = self.state_0 = self.model = None + self.deformable_ranges.clear() + self.particle_ranges = {} class NewtonSceneDataBackend(SceneDataBackend): @@ -213,43 +219,14 @@ def __init__(self): self.geometry_timestamp = 0 self._geometry_batches = [] - def initialize_geometry(self, plan: ClonePlan) -> None: + def initialize_geometry(self) -> None: """Bind imported geometry paths to native particle ranges and capsule endpoints.""" state = NewtonManager.get_state_0() - ranges, visual_ranges = {}, {} - indices, weights = [], [] - visual_offset = 0 - if NewtonManager._deformable_registry: - templates, starts = cloner_path.get_world_prototype_asset_templates(plan) - for entry in NewtonManager._deformable_registry: - asset_ids = cloner_path.get_asset_prototypes(plan, entry.prim_path) - suffix = entry.vis_mesh_prim_path[len(entry.prim_path) :] - paths = [] - for world, prototype in enumerate((-1, *plan.topology.world_prototype_layout), -1): - for index in range(*starts[prototype + 1 : prototype + 3]): - if plan.topology.world_prototypes[index] in asset_ids: - paths.append(templates[index].format(world) + suffix) - if entry.volume_vis_remap is None: - for path, offset in zip(paths, entry.particle_offsets, strict=True): - ranges[path] = offset, entry.particles_per_body - else: - prototype_indices = entry.volume_vis_remap.tet_vertex_indices.numpy() - prototype_weights = entry.volume_vis_remap.bary_weights.numpy() - count = len(prototype_indices) - for path, offset in zip(paths, entry.particle_offsets, strict=True): - indices.append(prototype_indices + offset) - weights.append(prototype_weights) - visual_ranges[path] = (visual_offset, count) - visual_offset += count - source = SceneDataFormat.Points() - source.points = state.particle_q - self._geometry_batches = [(source, ranges)] - if visual_ranges: - source = SceneDataFormat.WeightedPoints() - source.points = state.particle_q - source.indices = wp.array(np.concatenate(indices), dtype=wp.int32, device=source.points.device) - source.weights = wp.array(np.concatenate(weights), dtype=wp.float32, device=source.points.device) - self._geometry_batches.append((source, visual_ranges)) + self._geometry_batches = [ + (source, ranges) + for source, ranges in self._geometry_batches + if source._cls is not SceneDataFormat.CapsuleEndpoints + ] endpoints, ranges = [], {} offset = 0 for path, shapes in NewtonManager._cable_bindings.items(): @@ -285,7 +262,7 @@ def get_geometry_batches(self, output_format=SceneDataFormat.Points): if getattr(source, attribute) is not data: setattr(source, attribute, data) self.geometry_timestamp += 1 - return [(source, ranges) for source, ranges in self._geometry_batches if ranges] + return self._geometry_batches @property def transforms(self) -> SceneDataFormat.Transform: @@ -442,8 +419,6 @@ class NewtonManager(PhysicsManager): _sensor_bvh_shape_flags: ShapeFlags = ShapeFlags.VISIBLE # USD/Fabric sync - _usdrt_stage = None - _clone_physics_only = False transforms_may_change_on_graph_replay: bool = False _cable_bindings: dict[str, list[int]] = {} @@ -466,7 +441,7 @@ class NewtonManager(PhysicsManager): _visualization_stop_callback: CallbackHandle | None = None _builder_attribute_solvers: tuple[type[SolverBase], ...] = () - _mpm_object_registry: list = [] + _particle_ranges: dict[str, tuple[int, int]] = {} # CL: Cloning / Replication logic # TODO: These attributes support cloning-specific logic and should be moved into a cloner class @@ -480,13 +455,11 @@ class NewtonManager(PhysicsManager): _LocalSite = tuple[None, list[list[int]]] _SiteEntry = _GlobalSite | _LocalSite _cl_site_index_map: dict[str, _SiteEntry] = {} - _cl_fabric_body_bindings: list[tuple[str, int]] | None = None _world_xforms: list[wp.transform] | None = None # Per-source builders retained from replication, keyed by clone-plan source # path. Single-model consumers (e.g. batched Newton IK) finalize a single-env # model from these using the asset prototype's native source path. _cl_protos: dict[str, ModelBuilder] = {} - _deformable_registry: list = [] _per_world_builder_hooks: list[Callable[[ModelBuilder, int, np.ndarray, np.ndarray], None]] = [] @classmethod @@ -508,19 +481,6 @@ def initialize(cls, sim_context: SimulationContext) -> None: if sim is not None: NewtonManager._gravity_vector = sim.cfg.gravity # type: ignore[union-attr] - # USD/Fabric sync for Omniverse rendering (visualizer) or Newton+RTX (Kit cameras) - try: - requested = sim.resolve_visualizer_types() - except Exception: - requested = [] - viz_raw = sim.get_setting("/isaaclab/visualizer/types") - if isinstance(viz_raw, str): - requested = [v for part in viz_raw.split(",") for v in part.split() if v] - from isaaclab.app.settings_manager import get_settings_manager - - cameras_enabled = bool(get_settings_manager().get("/isaaclab/cameras_enabled", False)) - cls._clone_physics_only = not has_kit() or ("kit" not in requested and not cameras_enabled) - NewtonManager._scene_data_backend = NewtonSceneDataBackend() @classmethod @@ -610,9 +570,9 @@ def video_capture_backend(cls) -> str: @classmethod def sync_transforms_to_fabric(cls) -> None: """Publish rigid-body poses through SDP to Fabric, leaving authored USD untouched.""" - if cls._usdrt_stage is None or cls.backend is None: - return sim = PhysicsManager._sim + if sim.fabric_cfg is None or cls.backend is None: + return sim.get_or_create_backend(sim.fabric_cfg).update_transforms(sim.get_scene_data_provider()) @classmethod @@ -649,17 +609,6 @@ def _mark_particles_dirty(cls) -> None: if device.is_cuda and device.stream.is_capturing: NewtonManager.transforms_may_change_on_graph_replay = True - @classmethod - def register_particle_visual_prim(cls, prim_path: str, particle_offset: int, particle_count: int) -> None: - """Publish a declared point prim's native particle range. - - Args: - prim_path: Planned point-geometry destination. - particle_offset: First native particle index. - particle_count: Number of particles. - """ - NewtonManager._scene_data_backend._geometry_batches[0][1][prim_path] = (particle_offset, particle_count) - @classmethod def step(cls) -> None: """Step the physics simulation. @@ -826,11 +775,9 @@ def clear(cls): NewtonManager._sensor_state = None NewtonManager._sensor_state_dirty = True NewtonManager._sensor_bvh_shape_flags = ShapeFlags.VISIBLE - NewtonManager._usdrt_stage = None NewtonManager.transforms_may_change_on_graph_replay = False NewtonManager._cable_bindings = {} - NewtonManager._mpm_object_registry = [] - NewtonManager._deformable_registry = [] + NewtonManager._particle_ranges = {} NewtonManager._per_world_builder_hooks = [] NewtonManager._up_axis = "Z" NewtonManager._transform_mapping = None @@ -841,7 +788,6 @@ def clear(cls): NewtonManager._scene_data_backend = None NewtonManager._cl_pending_sites = {} NewtonManager._cl_site_index_map = {} - NewtonManager._cl_fabric_body_bindings = None NewtonManager._world_xforms = None NewtonManager._cl_protos = {} NewtonManager._pending_extended_state_attributes = set() @@ -855,9 +801,10 @@ def clear(cls): NewtonManager.backend = None @classmethod - def set_builder(cls, builder: ModelBuilder) -> None: - """Set the Newton model builder.""" + def set_builder(cls, builder: ModelBuilder, *, particle_ranges: dict[str, tuple[int, int]] | None = None) -> None: + """Set the Newton model builder and its imported point-to-particle ranges.""" NewtonManager._builder = builder + NewtonManager._particle_ranges = {} if particle_ranges is None else particle_ranges @classmethod def create_builder(cls, up_axis: str | None = None, **kwargs) -> ModelBuilder: @@ -983,7 +930,7 @@ def _cl_inject_sites( main_builder: ModelBuilder, source_builders: dict[str, ModelBuilder], ) -> tuple[dict[str, int], dict[int, dict[str, list[int]]], dict[str, wp.transform]]: - """Inject registered sites into plan-owned builders before replication. + """Inject registered sites into source builders or an explicitly supplied main builder. Body sites are matched against source builders, then the main builder containing shared assets. Bodyless global sites are added to *main_builder* with ``body=-1``. @@ -1021,9 +968,8 @@ def _cl_inject_sites( for source_builder in (*source_builders.values(), main_builder): if source_builder is main_builder and any_matched: break - body_labels = list(source_builder.body_label) matched_indices, matched_names = resolve_matching_names( - body_pattern, body_labels, raise_when_no_match=False + body_pattern, source_builder.body_label, raise_when_no_match=False ) if not matched_indices: # Pattern has no matches in this source builder continue @@ -1034,57 +980,15 @@ def _cl_inject_sites( site_label = f"{body_name}/{label}" source_site_idx = source_builder.add_site(body=body_idx, xform=xform, label=site_label) site_indices.append(source_site_idx) - logger.debug(f"Injected site '{site_label}' into clone-plan builder") + logger.debug(f"Injected site '{site_label}' into builder") source_site_indices.setdefault(id(source_builder), {})[label] = site_indices if not any_matched: - raise ValueError( - f"Site '{label}' with body_pattern '{body_pattern}' matched no clone-plan builder bodies." - ) + raise ValueError(f"Site '{label}' with body_pattern '{body_pattern}' matched no builder bodies.") cls._cl_pending_sites.clear() return global_site_indices, source_site_indices, env_root_sites - @classmethod - def _cl_inject_sites_fallback(cls) -> None: - """Inject pending sites into the flat builder (no-replication path). - - Populates :attr:`_cl_site_index_map` with the unified per-world structure: - - - Global sites (``body_pattern is None``): ``(shape_idx, None)`` - - Local and world sites: ``(None, [[idx, ...]])`` — one sublist for the single world. - """ - builder = cls._builder - body_labels = list(builder.body_label) - - for (body_pattern, per_world, _xform_key), (label, xform) in cls._cl_pending_sites.items(): - if per_world: - site_idx = builder.add_site(body=-1, xform=xform, label=label) - cls._cl_site_index_map[label] = (None, [[site_idx]]) - continue - if body_pattern is None: - site_idx = builder.add_site(body=-1, xform=xform, label=label) - cls._cl_site_index_map[label] = (site_idx, None) - else: - try: - matched_indices, matched_names = resolve_matching_names(body_pattern, body_labels) - except ValueError as e: - raise ValueError( - f"Site '{label}' with body_pattern '{body_pattern}' matched no bodies " - f"in the flat builder. Available body labels: {body_labels}." - ) from e - - site_indices: list[int] = [] - for body_idx in matched_indices: - site_label = f"{builder.body_label[body_idx]}/{label}" - site_idx = builder.add_site(body=body_idx, xform=xform, label=site_label) - site_indices.append(site_idx) - - # Single world (no replication): one-element outer list - cls._cl_site_index_map[label] = (None, [site_indices]) - - cls._cl_pending_sites.clear() - @classmethod def add_model_change(cls, change: ModelFlags) -> None: """Register a model change to notify the solver.""" @@ -1246,9 +1150,16 @@ def start_simulation(cls) -> None: logger.info("Dispatching MODEL_INIT callbacks") cls.dispatch_event(PhysicsEvent.MODEL_INIT) - # Inject any pending site requests (no-replication fallback path). - # In the replication path, _cl_inject_sites() already ran from newton_replicate. - cls._cl_inject_sites_fallback() + # Explicit builders and MODEL_INIT callbacks use the same site import as clone prototypes. + if cls._cl_pending_sites: + global_sites, body_sites, world_sites = cls._cl_inject_sites(cls._builder, {}) + cls._cl_site_index_map.update((label, (index, None)) for label, index in global_sites.items()) + cls._cl_site_index_map.update( + (label, (None, [indices])) for label, indices in body_sites.get(id(cls._builder), {}).items() + ) + for label, xform in world_sites.items(): + index = cls._builder.add_site(body=-1, xform=xform, label=label) + cls._cl_site_index_map[label] = (None, [[index]]) device = PhysicsManager._device logger.info(f"Finalizing model on device: {device}") @@ -1264,6 +1175,7 @@ def start_simulation(cls) -> None: with Timer(name="newton_finalize_builder", msg="Finalize builder took:", activity="Finalizing physics model"): cfg = NewtonBackendCfg( builder=cls._builder, + particle_ranges=cls._particle_ranges, device=device, num_envs=cls._num_envs, gravity=cls._gravity_vector, @@ -1289,65 +1201,10 @@ def start_simulation(cls) -> None: NewtonManager._world_reset_mask = wp.zeros(cls.backend.model.world_count + 1, dtype=wp.bool, device=device) NewtonManager._fk_reset_mask = wp.zeros(cls.backend.model.articulation_count, dtype=wp.bool, device=device) - # Setup USD/Fabric sync for Kit viewport rendering - if not cls._clone_physics_only: - import usdrt - - body_paths = list(cls.backend.model.body_label) - NewtonManager._usdrt_stage = get_current_stage(fabric=True) - body_bindings = NewtonManager._cl_fabric_body_bindings - if body_bindings is None: - # Non-replicated Newton stages do not pass through NewtonReplicateContext. - body_bindings = [(body_path, i) for i, body_path in enumerate(body_paths)] - - fabric_hierarchy = usdrt.hierarchy.IFabricHierarchy().get_fabric_hierarchy( - cls._usdrt_stage.GetFabricId(), cls._usdrt_stage.GetStageIdAsStageId() - ) - - NewtonManager._initialize_fabric_body_prims(cls._usdrt_stage, fabric_hierarchy, usdrt, body_bindings) - - cls._scene_data_backend.initialize_geometry(PhysicsManager._sim.get_clone_plan()) + cls._scene_data_backend.initialize_geometry() logger.info("Dispatching PHYSICS_READY callbacks") cls.dispatch_event(PhysicsEvent.PHYSICS_READY) - @staticmethod - def _initialize_fabric_body_prims(stage, fabric_hierarchy, usdrt, body_bindings: Sequence[tuple[str, int]]) -> None: - """Initialize Fabric body prims used by Newton transform sync.""" - for prim_path, _ in body_bindings: - prim = stage.GetPrimAtPath(prim_path) - if prim.IsValid(): - xformable_prim = usdrt.Rt.Xformable(prim) - xformable_prim.SetWorldXformFromUsd() - else: - prim = stage.DefinePrim(prim_path, "Xform") - xformable_prim = usdrt.Rt.Xformable(prim) - xformable_prim.CreateFabricHierarchyWorldMatrixAttr() - - # Include native bodies absent from USD in the SDP rigid-transform binding. - prim.AddAppliedSchema("PhysicsRigidBodyAPI") - - fabric_hierarchy.update_world_xforms() - - @classmethod - def collect_cable_segment_shape_ids(cls) -> dict[str, list[int]]: - """Map each renderable cable prim path to its ordered Newton segment shape ids. - - Concrete destination paths and segment order come from Newton ``shape_label`` values - ``{curve}_edge_capsule_{N}``. Each returned id is the index of that capsule in Newton's - shape arrays after finalization (``shape_body``, ``shape_transform``, ``shape_scale``, …), - not the ``_edge_capsule_N`` suffix. Example: - ``{"/World/envs/env_0/Cable/geometry/mesh": [42, 43, 44]}`` means segments ``0..2`` came - from labels ``.../mesh_edge_capsule_0``, ``_1``, ``_2``, and Newton assigned those shapes - indices ``42..44`` after earlier scene shapes. - - Bindings are recorded during native import and cloning, including destinations that - exist only in a native backend. No completed-stage lookup is needed. - - Returns: - Concrete cable prim paths mapped to ordered Newton segment shape ids. - """ - return cls._cable_bindings - @classmethod def _inject_terrain_heightfields( cls, stage: Usd.Stage, builder: ModelBuilder, *, root_paths: Sequence[str] @@ -1411,11 +1268,6 @@ def _inject_terrain_heightfields( ignore_paths.append(prim.GetPath().pathString) return ignore_paths - @classmethod - def _get_usd_import_ignore_paths(cls) -> list[str]: - """Return solver-specific prim paths excluded from USD import.""" - return [] - @classmethod def _get_usd_import_schema_resolvers(cls) -> list[SchemaResolver]: """Return ordered schema resolvers for physics-model USD imports. @@ -1431,144 +1283,8 @@ def _get_usd_import_schema_resolvers(cls) -> list[SchemaResolver]: @classmethod def instantiate_builder_from_stage(cls): - """Create builder from USD stage. - - Detects env Xforms (e.g. ``/World/Env_0``, ``/World/Env_1``) and builds - each as a separate Newton world via ``begin_world``/``end_world``. - Falls back to a flat ``add_usd`` when no env Xforms are found. - - """ - import re - - from pxr import UsdGeom - - # MPMObject imports NewtonManager, so defer this reciprocal import until model construction. - from isaaclab_newton.assets.mpm_object.mpm_object import ( # noqa: PLC0415 - record_registered_mpm_particle_ranges, - reset_registered_mpm_particle_ranges, - ) - - stage = get_current_stage() - reset_registered_mpm_particle_ranges() - up_axis = UsdGeom.GetStageUpAxis(stage) - - # Scan /World children for env-like Xforms (Env_0, env_1, ...) - env_pattern = re.compile(r"^[Ee]nv_(\d+)$") - world_prim = stage.GetPrimAtPath("/World") - env_paths: list[tuple[int, str]] = [] - if world_prim and world_prim.IsValid(): - for child in world_prim.GetChildren(): - m = env_pattern.match(child.GetName()) - if m: - env_paths.append((int(m.group(1)), child.GetPath().pathString)) - env_paths.sort(key=lambda x: x[0]) - - builder = cls.create_builder(up_axis=up_axis) - - schema_resolvers = cls._get_usd_import_schema_resolvers() - - # NOTE: None of the add_usd calls below pass joint_ordering or - # bodies_follow_joint_ordering, so the live articulation's native - # joint/body order comes from Newton's ModelBuilder.add_usd defaults - # (joint_ordering="dfs", bodies_follow_joint_ordering=True). - # isaaclab.assets.articulation.ordering_resolvers hardcodes matching - # constants to emulate that same order for cross-backend name - # resolution (see _get_mjwarp_names_from_newton_usd_builder). If - # ordering arguments are ever passed here, update the resolver - # constants in lockstep or MJWarp resolution will silently diverge - # from the live backend. - hf_ignore_paths = cls._inject_terrain_heightfields(stage, builder, root_paths=("/",)) - solver_ignore_paths = cls._get_usd_import_ignore_paths() - - if not env_paths: - # No env Xforms — flat loading - import_result = builder.add_usd( - stage, ignore_paths=[*hf_ignore_paths, *solver_ignore_paths], schema_resolvers=schema_resolvers - ) - record_registered_mpm_particle_ranges(import_result.get("path_particle_map", {})) - _restore_visible_colliders_without_visual_shapes(builder, stage, import_result["path_shape_map"]) - replace_newton_builder_shape_colors(builder, stage) - import_builder_visual_material_paths(builder, stage) - NewtonManager._world_xforms = [wp.transform()] - for hook in cls._per_world_builder_hooks: - hook( - builder, - 0, - np.zeros(3, dtype=np.float32), - np.asarray((0.0, 0.0, 0.0, 1.0), dtype=np.float32), - ) - else: - # Load everything except the env subtrees (ground plane, lights, etc.) - # and any terrain colliders already added as heightfields above. - ignore_paths = [path for _, path in env_paths] + hf_ignore_paths + solver_ignore_paths - import_result = builder.add_usd(stage, ignore_paths=ignore_paths, schema_resolvers=schema_resolvers) - record_registered_mpm_particle_ranges(import_result.get("path_particle_map", {})) - _restore_visible_colliders_without_visual_shapes(builder, stage, import_result["path_shape_map"]) - replace_newton_builder_shape_colors(builder, stage) - import_builder_visual_material_paths(builder, stage) - - _, proto_path = env_paths[0] - source_builders = {proto_path: cls.create_builder(up_axis=up_axis)} - import_result = source_builders[proto_path].add_usd( - stage, - root_path=proto_path, - ignore_paths=solver_ignore_paths, - schema_resolvers=schema_resolvers, - ) - _restore_visible_colliders_without_visual_shapes( - source_builders[proto_path], stage, import_result["path_shape_map"] - ) - replace_newton_builder_shape_colors(source_builders[proto_path], stage) - import_builder_visual_material_paths(source_builders[proto_path], stage) - cls._cl_protos = source_builders - - global_site_indices, source_site_indices, env_root_sites = cls._cl_inject_sites(builder, source_builders) - xform_cache = UsdGeom.XformCache() - poses = [] - for _, env_path in env_paths: - world_xform = xform_cache.GetLocalToWorldTransform(stage.GetPrimAtPath(env_path)) - translation = world_xform.ExtractTranslation() - rotation = world_xform.ExtractRotationQuat() - imag = rotation.GetImaginary() - poses.append((tuple(translation), (*imag, rotation.GetReal()))) - - positions = np.asarray([pos for pos, _ in poses], dtype=np.float32) - quaternions = np.asarray([quat for _, quat in poses], dtype=np.float32) - env_template = proto_path.rsplit("_", 1)[0] + "_{}" - asset = AssetBaseCfg(prim_path=proto_path, spawn=SpawnerCfg(spawn_path=proto_path)) - plan = make_clone_plan((asset,), ((0,),), len(env_paths), positions=positions, env_template=env_template) - - def record_source_particle_ranges(source, particle_offset, source_builder, source_xform) -> None: - if source == proto_path: - record_registered_mpm_particle_ranges( - import_result.get("path_particle_map", {}), - particle_offset, - builder=builder, - source_builder=source_builder, - source_xform=source_xform, - ) - - local_site_map, world_xforms, _ = replicate_builder_mapping( - builder=builder, - plan=plan, - positions=positions, - quaternions=quaternions, - source_builders=source_builders, - env_ids=np.asarray([index for index, _ in env_paths]), - source_site_indices=source_site_indices, - env_root_sites=env_root_sites, - per_world_builder_hooks=cls._per_world_builder_hooks, - source_builder_added=record_source_particle_ranges if cls._mpm_object_registry else None, - ) - - NewtonManager._cl_site_index_map = {label: (idx, None) for label, idx in global_site_indices.items()} - NewtonManager._cl_site_index_map.update( - (label, (None, per_world)) for label, per_world in local_site_map.items() - ) - NewtonManager._world_xforms = world_xforms - NewtonManager._num_envs = len(env_paths) - - cls.set_builder(builder) + """Import the explicitly declared clone plan into the Newton builder.""" + cloner.replicate(PhysicsManager._sim.get_clone_plan()) @classmethod def _initialize_contacts(cls) -> None: @@ -1887,7 +1603,8 @@ def _capture_graph(cls, capture_target: Callable[[], None]) -> wp.Graph: device = PhysicsManager._device stream = wp.get_stream(device) mode = wp.CaptureMode.THREAD_LOCAL - if cls._usdrt_stage is not None: + sim = PhysicsManager._sim + if has_kit() and (sim.has_gui or sim.has_offscreen_render): # RTX uses the legacy CUDA stream. A nonblocking stream avoids implicit synchronization. stream = wp.stream_from_torch(torch.cuda.Stream(device=device)) mode = wp.CaptureMode.RELAXED diff --git a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager_cfg.py b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager_cfg.py index 60edff3d86e4..081cbb636ffd 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager_cfg.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager_cfg.py @@ -34,6 +34,8 @@ class NewtonBackendCfg(BackendCfg): class_type: type[NewtonBackend] | str = "{DIR}.newton_manager:NewtonBackend" builder: ModelBuilder = field(kw_only=True, metadata={"copy": False}) """Populated clone builder. Reusing this builder and matching settings shares one resource.""" + particle_ranges: dict[str, tuple[int, int]] = field(default_factory=dict) + """Imported point paths to native particle offsets and counts, retained until Newton exposes composed ranges.""" device: str = MISSING """Allocation device, such as ``cpu`` or ``cuda:0``.""" num_envs: int | None = None diff --git a/source/isaaclab_newton/isaaclab_newton/physics/vbd_manager.py b/source/isaaclab_newton/isaaclab_newton/physics/vbd_manager.py index d9292e118ed7..f0923ebe9d35 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/vbd_manager.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/vbd_manager.py @@ -7,33 +7,16 @@ from __future__ import annotations -from typing import TYPE_CHECKING - from newton import Model from newton.solvers import SolverVBD from .newton_manager import NewtonManager from .vbd_manager_cfg import VBDSolverCfg -if TYPE_CHECKING: - from isaaclab.sim.simulation_context import SimulationContext - class NewtonVBDManager(NewtonManager): """Newton manager specialization for the VBD solver.""" - @classmethod - def initialize(cls, sim_context: SimulationContext) -> None: - """Initialize VBD deformable integration when contrib is available.""" - try: - from isaaclab_contrib.deformable.deformable_object import install_deformable_builder_hooks - except ModuleNotFoundError as exc: - if exc.name not in {"isaaclab_contrib", "isaaclab_contrib.deformable"}: - raise - else: - install_deformable_builder_hooks() - super().initialize(sim_context) - @classmethod def start_simulation(cls) -> None: """Color the prebuilt model before starting simulation.""" @@ -41,23 +24,6 @@ def start_simulation(cls) -> None: cls._builder.color(balance_colors=False) super().start_simulation() - @classmethod - def instantiate_builder_from_stage(cls) -> None: - """Create and color the VBD builder from the USD stage.""" - super().instantiate_builder_from_stage() - if cls._builder is None: - raise RuntimeError("Newton stage import did not create a builder.") - # Warp's optional balancing pass can cycle indefinitely for valid graph colorings. - # The initial assignment is sufficient for VBD correctness. - cls._builder.color(balance_colors=False) - - @classmethod - def _get_usd_import_ignore_paths(cls) -> list[str]: - """Return registered deformable mesh paths excluded from USD import.""" - return [ - path for entry in cls._deformable_registry for path in (entry.sim_mesh_prim_path, entry.vis_mesh_prim_path) - ] - @classmethod def _create_solver(cls, model: Model, solver_cfg: VBDSolverCfg) -> SolverVBD: """Construct the configured VBD solver.""" @@ -71,17 +37,6 @@ def _build_solver(cls, model: Model, solver_cfg: VBDSolverCfg) -> None: NewtonManager._needs_collision_pipeline = True NewtonManager._supports_rigid_body_force_input = not solver_cfg.integrate_with_external_rigid_solver - @classmethod - def _solver_specific_clear(cls) -> None: - """Clear contrib deformable integration when available.""" - try: - from isaaclab_contrib.deformable.deformable_object import clear_deformable_builder_hooks - except ModuleNotFoundError as exc: - if exc.name not in {"isaaclab_contrib", "isaaclab_contrib.deformable"}: - raise - else: - clear_deformable_builder_hooks() - @classmethod def _simulate_physics_only(cls) -> None: """Rebuild the VBD particle BVH before stepping physics.""" diff --git a/source/isaaclab_newton/isaaclab_newton/physics/visualization_deformables.py b/source/isaaclab_newton/isaaclab_newton/physics/visualization_deformables.py deleted file mode 100644 index d7d878eecac3..000000000000 --- a/source/isaaclab_newton/isaaclab_newton/physics/visualization_deformables.py +++ /dev/null @@ -1,59 +0,0 @@ -# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). -# All rights reserved. -# -# SPDX-License-Identifier: BSD-3-Clause - -"""Build Newton rendering geometry from imported deformable prototypes.""" - -from __future__ import annotations - -import warp as wp -from newton import ModelBuilder - -from isaaclab.scene_data.deformable_discovery import DeformableStageEntry - - -def add_shadow_deformables_to_builder(builder: ModelBuilder, entries: list[DeformableStageEntry]) -> dict[str, int]: - """Build declared visual geometry and return its native particle destinations. - - Args: - builder: Render-only Newton model under construction. - entries: Imported geometry expanded to this representation's destination instances. - - Returns: - Exact visual mesh paths mapped to their first render-particle indices. - SDP supplies the corresponding world-space points after physics initialization. - """ - offsets = {} - for entry in entries: - offsets[entry.vis_mesh_path] = builder.particle_count - if entry.deformable_type == "surface" or entry.vis_mesh_path != entry.sim_mesh_path: - builder.add_cloth_mesh( - pos=wp.vec3(*entry.init_pos), - rot=wp.quat(*entry.init_rot), - scale=1.0, - vel=wp.vec3(0.0, 0.0, 0.0), - vertices=entry.vis_vertices, - indices=entry.vis_indices, - density=1.0, - tri_ke=1e4, - tri_ka=1e4, - tri_kd=1.5e-6, - edge_ke=5.0, - edge_kd=1e-2, - particle_radius=0.008, - ) - else: - builder.add_soft_mesh( - pos=wp.vec3(*entry.init_pos), - rot=wp.quat(*entry.init_rot), - scale=1.0, - vel=wp.vec3(0.0, 0.0, 0.0), - vertices=entry.vertices, - indices=entry.indices, - density=1000.0, - k_mu=1e5, - k_lambda=1e5, - k_damp=0.0, - ) - return offsets diff --git a/source/isaaclab_newton/isaaclab_newton/renderers/visual_material.py b/source/isaaclab_newton/isaaclab_newton/renderers/visual_material.py index d80ebd88de2c..7f9ca622e033 100644 --- a/source/isaaclab_newton/isaaclab_newton/renderers/visual_material.py +++ b/source/isaaclab_newton/isaaclab_newton/renderers/visual_material.py @@ -34,6 +34,9 @@ def import_builder_visual_material_paths(builder: ModelBuilder, stage: Usd.Stage paths = builder.custom_attributes["isaaclab:visual_material_path"].values for shape_index, shape_path in enumerate(builder.shape_label): shape_prim = stage.GetPrimAtPath(shape_path) + # Newton preserves an approximated collider's authored mesh as _visual. + if not shape_prim and shape_path.endswith("_visual"): + shape_prim = stage.GetPrimAtPath(shape_path.removesuffix("_visual")) imageable = UsdGeom.Imageable(shape_prim) if not shape_prim.IsValid() or (imageable and imageable.ComputePurpose() == UsdGeom.Tokens.guide): continue diff --git a/source/isaaclab_newton/isaaclab_newton/sim/spawners/mpm/visualization.py b/source/isaaclab_newton/isaaclab_newton/sim/spawners/mpm/visualization.py index 8aaaf74a1fe4..6ebaf74d4dff 100644 --- a/source/isaaclab_newton/isaaclab_newton/sim/spawners/mpm/visualization.py +++ b/source/isaaclab_newton/isaaclab_newton/sim/spawners/mpm/visualization.py @@ -22,8 +22,7 @@ def create_mpm_particle_visualization( """Create one ``UsdGeom.Points`` prim per environment for USD-stage MPM particle rendering. The created prims are static USD containers: per-frame position updates are - handled by rendering consumers through SDP for prims registered via - :meth:`isaaclab_newton.physics.NewtonManager.register_particle_visual_prim`. + handled by rendering consumers through the clone-imported SDP particle bindings. Args: prim_paths: ``Points`` prim paths, one per environment. diff --git a/source/isaaclab_newton/isaaclab_newton/sim/views/newton_site_frame_view.py b/source/isaaclab_newton/isaaclab_newton/sim/views/newton_site_frame_view.py index 3f1d761c9412..b6d824eba7a1 100644 --- a/source/isaaclab_newton/isaaclab_newton/sim/views/newton_site_frame_view.py +++ b/source/isaaclab_newton/isaaclab_newton/sim/views/newton_site_frame_view.py @@ -561,7 +561,8 @@ def _mirror_to_fabric(self) -> None: # Bodies sync at render cadence, so after a ``render=False`` step the local derivation below # would read a stale parent. No-op when clean. - NewtonManager.sync_transforms_to_fabric() + sim = sim_utils.SimulationContext.instance() + sim.get_or_create_backend(sim.fabric_cfg).update_transforms(sim.get_scene_data_provider()) count = self._fabric_sel.count pos_ta, quat_ta = self._get_world_poses_impl(None) diff --git a/source/isaaclab_contrib/test/deformable/test_deformable_object.py b/source/isaaclab_newton/test/assets/test_deformable_object.py similarity index 91% rename from source/isaaclab_contrib/test/deformable/test_deformable_object.py rename to source/isaaclab_newton/test/assets/test_deformable_object.py index b5a9b0bd9e45..f8495a7b63fc 100644 --- a/source/isaaclab_contrib/test/deformable/test_deformable_object.py +++ b/source/isaaclab_newton/test/assets/test_deformable_object.py @@ -28,6 +28,8 @@ NewtonSurfaceDeformableBodyMaterialCfg, ) +from pxr import Vt + import isaaclab.sim as sim_utils import isaaclab.utils.math as math_utils from isaaclab.assets import DeformableObject, DeformableObjectCfg @@ -88,7 +90,7 @@ def generate_cubes_scene(num_cubes: int = 1, height: float = 1.0) -> DeformableO def generate_cloth_scene(num_cloths: int = 1, height: float = 1.0) -> DeformableObject: - """Generate a scene with surface deformable cloth squares. + """Generate a scene with folded surface deformable cloth squares. Args: num_cloths: Number of cloths to generate. @@ -118,6 +120,11 @@ def generate_cloth_scene(num_cloths: int = 1, height: float = 1.0) -> Deformable CloneCfg(clone_template="/World/env_{}"), (cloth_object_cfg,), num_cloths, 1.0, positions=origins ) cloth_object = DeformableObject(cfg=cloth_object_cfg) + for path in ("/World/env_0/Cloth/sim_mesh", "/World/env_0/Cloth/geometry/mesh"): + points_attr = sim_utils.get_current_stage().GetPrimAtPath(path).GetAttribute("points") + points = np.asarray(points_attr.Get()).copy() + points[:, 2] += np.abs(points[:, 0]) + points_attr.Set(Vt.Vec3fArray.FromNumpy(points)) replicate(plan) return cloth_object @@ -155,7 +162,7 @@ def generate_cuboid_and_cylinder_scene(height: float = 1.0) -> tuple[DeformableO ), init_state=DeformableObjectCfg.InitialStateCfg(pos=(0.4, 0.0, height + 0.2)), ) - plan = clone_plan_from_env_0(CloneCfg(clone_template="/World/env_{}"), (cuboid_cfg, cylinder_cfg), 1, 0.0) + plan = clone_plan_from_env_0(CloneCfg(clone_template="/World/env_{}"), (cuboid_cfg, cylinder_cfg), 2, 1.0) cuboid, cylinder = DeformableObject(cfg=cuboid_cfg), DeformableObject(cfg=cylinder_cfg) replicate(plan) return cuboid, cylinder @@ -228,8 +235,11 @@ def test_surface_initialization_and_freefall(sim): """Test initialization and stepping for surface deformable objects.""" num_cloths = 2 cloth_object = generate_cloth_scene(num_cloths=num_cloths, height=5.0) + rest_angles = np.asarray(NewtonManager._builder.edge_rest_angle, dtype=np.float32) + assert np.any(np.abs(rest_angles) > 0.1) sim.reset() + np.testing.assert_array_equal(NewtonManager.get_model().edge_rest_angle.numpy(), rest_angles) assert cloth_object.is_initialized assert cloth_object.num_instances == num_cloths @@ -547,7 +557,7 @@ def test_set_kinematic_targets(sim): def test_multiple_deformable_assets_do_not_alias(sim): - """Test independent writes for two different deformable assets in one scene.""" + """Defaults and kinematic writes cover only each asset's selected particles.""" cuboid, cylinder = generate_cuboid_and_cylinder_scene(height=2.0) sim.reset() @@ -562,4 +572,27 @@ def test_multiple_deformable_assets_do_not_alias(sim): torch.testing.assert_close(cuboid.data.nodal_pos_w.torch, cuboid_pos, rtol=1e-5, atol=1e-5) torch.testing.assert_close(cylinder.data.nodal_pos_w.torch, cylinder_default, rtol=1e-5, atol=1e-5) - assert cuboid._recorded_particle_offsets != cylinder._recorded_particle_offsets + model = NewtonManager.get_model() + default_inv_mass, default_flags = model.particle_inv_mass.numpy(), model.particle_flags.numpy() + for asset in (cuboid, cylinder): + shape = (asset.num_instances, asset.max_sim_vertices_per_body) + assert asset._default_particle_inv_mass.shape == shape + assert asset._default_particle_flags.shape == shape + targets = asset.data.nodal_kinematic_target.torch.clone() + targets[0, :, :3] = asset.data.nodal_pos_w.torch[0] + targets[0, :, 3] = 0.0 + asset.write_nodal_kinematic_target_to_sim_index(targets) + asset.write_data_to_sim() + + start = int(asset._particle_offsets.numpy()[0]) + expected_inv_mass, expected_flags = default_inv_mass.copy(), default_flags.copy() + expected_inv_mass[start : start + shape[1]] = 0.0 + expected_flags[start : start + shape[1]] = 0 + np.testing.assert_array_equal(model.particle_inv_mass.numpy(), expected_inv_mass) + np.testing.assert_array_equal(model.particle_flags.numpy(), expected_flags) + + targets[0, :, 3] = 1.0 + asset.write_nodal_kinematic_target_to_sim_index(targets) + asset.write_data_to_sim() + np.testing.assert_array_equal(model.particle_inv_mass.numpy(), default_inv_mass) + np.testing.assert_array_equal(model.particle_flags.numpy(), default_flags) diff --git a/source/isaaclab_newton/test/cloner/test_clone_xform_math.py b/source/isaaclab_newton/test/cloner/test_clone_xform_math.py index 26aac888c3ab..78507b19e4a7 100644 --- a/source/isaaclab_newton/test/cloner/test_clone_xform_math.py +++ b/source/isaaclab_newton/test/cloner/test_clone_xform_math.py @@ -8,12 +8,7 @@ import numpy as np import pytest import warp as wp -from isaaclab_newton.cloner.newton_clone_utils import ( - _compose_world_xforms, - _invert_xform, - _quat_multiply, - _quat_rotate, -) +from isaaclab_newton.cloner.newton_clone_utils import _quat_rotate _TOL = 1e-5 @@ -44,13 +39,6 @@ def _sample_positions(num_samples: int) -> np.ndarray: ) -def test_quat_multiply_matches_warp(): - quats = _sample_quats(16) - for quat_a in quats: - expected = np.array([wp.mul(wp.quat(*quat_a), wp.quat(*quat_b)) for quat_b in quats]) - np.testing.assert_allclose(_quat_multiply(np.broadcast_to(quat_a, quats.shape), quats), expected, atol=_TOL) - - def test_quat_rotate_matches_warp(): quats = _sample_quats(16) vectors = _sample_positions(16) @@ -58,31 +46,5 @@ def test_quat_rotate_matches_warp(): np.testing.assert_allclose(_quat_rotate(quats, vectors), expected, atol=_TOL) -def test_compose_world_xforms_matches_transform_multiply(): - world_p = _sample_positions(16) - world_q = _sample_quats(16) - local_p = _sample_positions(16) - local_q = _sample_quats(16) - - for local in np.concatenate([local_p, local_q], axis=1): - expected = np.array( - [ - wp.transform_multiply( - wp.transform(wp.vec3(*p), wp.quat(*q)), wp.transform(wp.vec3(*local[:3]), wp.quat(*local[3:])) - ) - for p, q in zip(world_p, world_q, strict=True) - ] - ) - np.testing.assert_allclose(_compose_world_xforms(world_p, world_q, local), expected, atol=_TOL) - - -def test_invert_xform_matches_transform_inverse(): - positions = _sample_positions(16) - quats = _sample_quats(16) - for position, quat in zip(positions, quats, strict=True): - expected = np.array(wp.transform_inverse(wp.transform(wp.vec3(*position), wp.quat(*quat)))) - np.testing.assert_allclose(_invert_xform(np.concatenate([position, quat])), expected, atol=_TOL) - - if __name__ == "__main__": pytest.main([__file__, "-v"]) diff --git a/source/isaaclab_newton/test/cloner/test_collision_approximation.py b/source/isaaclab_newton/test/cloner/test_collision_approximation.py index b80b73fc68b9..fee301834066 100644 --- a/source/isaaclab_newton/test/cloner/test_collision_approximation.py +++ b/source/isaaclab_newton/test/cloner/test_collision_approximation.py @@ -10,7 +10,7 @@ from isaaclab_newton.cloner.newton_clone_utils import build_source_builders from newton import GeoType, ShapeFlags -from pxr import Sdf, Usd, UsdGeom, UsdPhysics +from pxr import Sdf, Usd, UsdGeom, UsdPhysics, UsdShade _SOURCE = "/World/Asset" @@ -131,11 +131,22 @@ def _build(stage: Usd.Stage, **kwargs) -> newton.ModelBuilder: class TestClonerCollisionApproximation: """build_source_builders must leave every collider at its USD-authored approximation.""" - def test_authored_convex_decomposition_produces_multiple_hulls(self): - """A concave mesh authored with convexDecomposition decomposes into 2+ hulls.""" - shapes = _collision_shapes(_build(_make_stage("convexDecomposition"))) + @pytest.mark.parametrize("dynamic", [True, False]) + def test_authored_convex_decomposition_produces_multiple_hulls(self, dynamic): + """Decompose collisions while preserving the authored visual mesh and material binding.""" + stage = _make_stage("convexDecomposition") + if not dynamic: + stage.GetPrimAtPath(_SOURCE).RemoveAPI(UsdPhysics.RigidBodyAPI) + material = UsdShade.Material.Define(stage, f"{_SOURCE}/material") + UsdShade.MaterialBindingAPI.Apply(stage.GetPrimAtPath(_SOURCE)).Bind(material) + builder = _build(stage) + shapes = _collision_shapes(builder) assert len(shapes) >= 2, f"expected a multi-hull decomposition, got {shapes}" assert all(geo_type == GeoType.CONVEX_MESH for geo_type in shapes.values()) + named_flags = zip(builder.shape_label, builder.shape_flags, strict=True) + assert [path for path, flags in named_flags if flags & ShapeFlags.VISIBLE] == [f"{_SOURCE}/geom_visual"] + visual = builder.shape_label.index(f"{_SOURCE}/geom_visual") + assert builder.finalize(device="cpu").isaaclab.visual_material_path[visual] == str(material.GetPath()) def test_skip_mesh_approximation_bypasses_authored_convex_decomposition(self): """A render-only import can bypass decomposition and retain the original mesh.""" diff --git a/source/isaaclab_newton/test/cloner/test_newton_builder_world_hook.py b/source/isaaclab_newton/test/cloner/test_newton_builder_world_hook.py index 12f717aa209a..8a2c36745448 100644 --- a/source/isaaclab_newton/test/cloner/test_newton_builder_world_hook.py +++ b/source/isaaclab_newton/test/cloner/test_newton_builder_world_hook.py @@ -12,10 +12,16 @@ import newton import numpy as np import pytest +import warp as wp from isaaclab_newton.cloner import copy_newton_clone_source, newton_builder_world_hook -from isaaclab_newton.physics import NewtonCfg, NewtonManager +from isaaclab_newton.physics import NewtonCfg, NewtonManager, VBDSolverCfg +from isaaclab_newton.physics.newton_manager import NewtonSceneDataBackend -from pxr import Usd, UsdGeom, UsdLux, UsdPhysics +from pxr import Sdf, Usd, UsdGeom, UsdLux, UsdPhysics, UsdShade + +from isaaclab.assets import AssetBaseCfg +from isaaclab.cloner import make_clone_plan +from isaaclab.sim import SimulationCfg, SpawnerCfg, build_simulation_context replicate_module = importlib.import_module("isaaclab_newton.cloner.replicate") @@ -80,7 +86,7 @@ def test_copy_newton_clone_source_owns_mutable_geometry(monkeypatch): def test_explicit_global_import_uses_global_world( monkeypatch, load_visual_shapes, is_rendering, rgb_array, visual_shapes_required, expected ): - """Global imports honor visual requirements and leave native deformables to world hooks.""" + """Global imports honor visual requirements and import shared deformables exactly once.""" stage = Usd.Stage.CreateInMemory() UsdPhysics.Scene.Define(stage, "/physicsScene") UsdGeom.Xform.Define(stage, "/World") @@ -91,13 +97,13 @@ def test_explicit_global_import_uses_global_world( native_mesh = UsdGeom.TetMesh.Define(stage, "/World/Native/sim") native_mesh.CreatePointsAttr(points) native_mesh.CreateTetVertexIndicesAttr([(0, 1, 2, 3)]) + root = stage.GetPrimAtPath("/World/Native") + root.SetMetadata("apiSchemas", Sdf.TokenListOp.CreateExplicit(["OmniPhysicsDeformableBodyAPI"])) + material = UsdShade.Material.Define(stage, "/World/Native/Material") + material.GetPrim().CreateAttribute("newton:density", Sdf.ValueTypeNames.Float).Set(1000.0) + UsdShade.MaterialBindingAPI.Apply(root).Bind(material, materialPurpose="physics") global_paths = ("/World/Ground", "/World/Light", "/World/Native") - def add_native_particles(builder, *_args): - builder.add_particles( - pos=points, vel=[(0.0, 0.0, 0.0)] * len(points), mass=[0.01] * len(points), radius=[0.005] * len(points) - ) - imports = [] add_usd = newton.ModelBuilder.add_usd @@ -116,6 +122,7 @@ def import_usd(builder, *args, **kwargs): "_sim", SimpleNamespace( physics_manager=manager, + device="cpu", cfg=SimpleNamespace( physics=NewtonCfg(load_visual_shapes=load_visual_shapes), physics_prim_path="/physicsScene" ), @@ -124,12 +131,11 @@ def import_usd(builder, *args, **kwargs): visual_shapes_required=visual_shapes_required, ), ) - monkeypatch.setattr(NewtonManager, "_deformable_registry", (SimpleNamespace(prim_path="/World/Native"),)) + monkeypatch.setattr(NewtonManager, "_scene_data_backend", NewtonSceneDataBackend()) monkeypatch.setattr(replicate_module.NewtonManager, "_cl_inject_sites", mock.Mock(return_value=({}, {}, {}))) - monkeypatch.setattr(NewtonManager, "_per_world_builder_hooks", (add_native_particles,)) + monkeypatch.setattr(NewtonManager, "_per_world_builder_hooks", ()) monkeypatch.setattr(NewtonManager, "_builder", None) monkeypatch.setattr(NewtonManager, "_cl_site_index_map", {}) - monkeypatch.setattr(NewtonManager, "_cl_fabric_body_bindings", []) monkeypatch.setattr(NewtonManager, "_world_xforms", None) monkeypatch.setattr(NewtonManager, "_cl_protos", {}) monkeypatch.setattr(NewtonManager, "_num_envs", 0) @@ -146,5 +152,104 @@ def import_usd(builder, *args, **kwargs): ground_index = model.shape_label.index("/World/Ground") assert model.shape_world.numpy()[ground_index] == -1 assert model.world_count == 2 - assert model.particle_count == len(points) * model.world_count + assert model.particle_count == len(points) + np.testing.assert_array_equal(model.particle_world.numpy(), -1) assert "/World/Light" not in model.shape_label # USD lights are not Newton physics entities. + + +def _author_deformable(stage, path, kind): + root = UsdGeom.Xform.Define(stage, path).GetPrim() + root.SetMetadata("apiSchemas", Sdf.TokenListOp.CreateExplicit(["OmniPhysicsDeformableBodyAPI"])) + points = [(0, 0, 0), (1, 0, 0), (0, 1, 0), (0, 0, 1)] + if kind == "volume": + mesh = UsdGeom.TetMesh.Define(stage, path + "/sim") + mesh.CreatePointsAttr(points) + mesh.CreateTetVertexIndicesAttr([(0, 1, 2, 3)]) + visual = UsdGeom.Mesh.Define(stage, path + "/vis") + visual.CreatePointsAttr([(0.5, 0, 0), (0, 0.5, 0), (0, 0, 0.5)]) + visual.CreateFaceVertexCountsAttr([3]) + visual.CreateFaceVertexIndicesAttr([0, 1, 2]) + schema = "OmniPhysicsVolumeDeformableSimAPI" + else: + mesh = UsdGeom.Mesh.Define(stage, path + "/sim") + mesh.CreatePointsAttr(points[:3]) + mesh.CreateFaceVertexCountsAttr([3]) + mesh.CreateFaceVertexIndicesAttr([0, 1, 2]) + schema = "OmniPhysicsSurfaceDeformableSimAPI" + mesh.GetPrim().SetMetadata("apiSchemas", Sdf.TokenListOp.CreateExplicit([schema])) + material = UsdShade.Material.Define(stage, path + "/material") + for name, value in (("density", 6.0), ("triKe", 123.0), ("kMu", 456.0)): + material.GetPrim().CreateAttribute("newton:" + name, Sdf.ValueTypeNames.Float).Set(value) + UsdShade.MaterialBindingAPI.Apply(root).Bind(material, materialPurpose="physics") + + +@pytest.mark.parametrize("heterogeneous", [False, True], ids=["batched", "heterogeneous"]) +def test_imported_deformables_follow_plan_and_publish_geometry(heterogeneous): + """Import each prototype once and bind native/embedded visuals without inspecting generated clones.""" + sim_cfg = SimulationCfg(device="cpu", physics=NewtonCfg(solver_cfg=VBDSolverCfg(), load_visual_shapes=False)) + with build_simulation_context(sim_cfg=sim_cfg) as sim: + stage = sim.stage + UsdGeom.SetStageUpAxis(stage, "Z") + UsdGeom.SetStageMetersPerUnit(stage, 1.0) + UsdPhysics.Scene.Define(stage, "/physicsScene") + volume_world = int(heterogeneous) + volume_path = f"/World/env_{volume_world}/Volume" + _author_deformable(stage, "/World/env_0/Cloth", "surface") + _author_deformable(stage, volume_path, "volume") + _author_deformable(stage, "/Shared/Cloth", "surface") + positions = np.asarray([[0, 0, 0], [2, 0, 0], [4, 0, 0]], dtype=np.float32) + if heterogeneous: + UsdGeom.Xform.Define(stage, "/World/env_1").AddTranslateOp().Set(tuple(positions[1].astype(float))) + rotations = np.asarray([[0, 0, 0, 1], [0, 0, 0, 1], [0, 0, 2**-0.5, 2**-0.5]], dtype=np.float32) + assets = ( + AssetBaseCfg(prim_path="/World/env_[^/]+/Cloth", spawn=SpawnerCfg(spawn_path="/World/env_0/Cloth")), + AssetBaseCfg(prim_path="/World/env_[^/]+/Volume", spawn=SpawnerCfg(spawn_path=volume_path)), + AssetBaseCfg(prim_path="/Shared/Cloth"), + ) + prototypes = ((0,), (1,)) if heterogeneous else ((0, 1),) + layout = np.array([0, int(heterogeneous), 0]) + placement = dict(env_template="/World/env_{}", positions=positions) + plan = make_clone_plan( + assets, prototypes, 3, shared_assets=(2,), clone_strategy=lambda _weights, _count: layout, **placement + ) + sim.set_clone_plan(plan) + options = dict(plan=plan, asset_prototype_ids=(0, 1, 2), positions=positions, quaternions=rotations) + builder, _, _ = replicate_module._replicate_newton(stage, np.arange(3), sim, **options) + sim.reset() + native = NewtonManager.backend + stage.RemovePrim("/World") + stage.RemovePrim("/Shared") + + expected_counts = [3, 4, 3] if heterogeneous else [7, 7, 7] + assert builder.particle_count == sum(expected_counts) + 3 + np.testing.assert_array_equal(np.bincount(np.asarray(builder.particle_world) + 1), [3, *expected_counts]) + assert native.deformable_ranges["/Shared/Cloth"] == (0, 3, "surface") + points = sim.get_scene_data_provider().get_geometry_points() + expected_paths = {"/Shared/Cloth/sim"} + local_vertices = np.asarray([(0, 0, 0), (1, 0, 0), (0, 1, 0), (0, 0, 1)], dtype=np.float32) + for world in range(3): + kinds = ( + ("Volume",) if heterogeneous and world == 1 else ("Cloth",) if heterogeneous else ("Cloth", "Volume") + ) + for name in kinds: + path = f"/World/env_{world}/{name}" + start, count, _ = native.deformable_ranges[path] + xform = wp.transform(positions[world], rotations[world]) + expected = np.asarray([wp.transform_point(xform, wp.vec3(p)) for p in local_vertices[:count]]) + np.testing.assert_allclose( + native.state_0.particle_q.numpy()[start : start + count], expected, atol=1e-6 + ) + visual_path = path + ("/sim" if name == "Cloth" else "/vis") + expected_paths.add(visual_path) + if name == "Cloth": + assert points[visual_path].ptr == native.state_0.particle_q.ptr + start * 12 + np.testing.assert_allclose(points[visual_path].numpy(), expected, atol=1e-6) + else: + np.testing.assert_allclose(points[visual_path].numpy(), (expected[1:] + expected[0]) / 2, atol=1e-6) + assert set(points) == expected_paths + np.testing.assert_allclose(np.asarray(builder.tri_materials)[builder._cloth_tri_start, 0], 123.0) + np.testing.assert_allclose(np.asarray(builder.tet_materials)[:, 0], 456.0) + for tet, pose in zip(builder.tet_indices, builder.tet_poses, strict=True): + vertices = np.asarray(builder.particle_q)[tet] + np.testing.assert_allclose((vertices[1:] - vertices[0]).T @ pose, np.eye(3), atol=1e-6) + np.testing.assert_allclose(builder.particle_radius, 0.008) diff --git a/source/isaaclab_newton/test/cloner/test_rename_builder_labels.py b/source/isaaclab_newton/test/cloner/test_rename_builder_labels.py index c3d5ccb41b4d..00f9ff9fbc3f 100644 --- a/source/isaaclab_newton/test/cloner/test_rename_builder_labels.py +++ b/source/isaaclab_newton/test/cloner/test_rename_builder_labels.py @@ -16,18 +16,13 @@ from isaaclab_newton.cloner import newton_clone_utils as newton_clone_utils_module from isaaclab_newton.cloner import replicate as replicate_module from isaaclab_newton.cloner.newton_clone_utils import replicate_builder_mapping -from isaaclab_newton.physics import visualization_deformables as visualization_deformables_module from pxr import Sdf, Usd, UsdGeom, UsdPhysics, UsdShade from isaaclab.assets import AssetBaseCfg from isaaclab.cloner import ClonePlan, PrototypeWorldTopology, make_clone_plan from isaaclab.cloner import path as cloner_path -from isaaclab.scene_data.deformable_discovery import ( - DeformableStageEntry, - deformable_prototypes, - expand_deformable_entries, -) +from isaaclab.sensors import SensorBaseCfg from isaaclab.sim import SpawnerCfg from isaaclab.sim.schemas import define_deformable_curve_properties @@ -87,7 +82,7 @@ def test_local_and_env_root_sites_keep_indices_labels_and_world_positions(self): plan = make_clone_plan(tuple(AssetBaseCfg(prim_path=path) for path in (source_path, other_path)), ((0, 1),), 3) with mock.patch.object(builder, "replicate", wraps=builder.replicate) as replicate: - local_site_map, _, _ = replicate_builder_mapping( + local_site_map, _ = replicate_builder_mapping( builder, plan, positions, quaternions, assets, env_ids=np.arange(3, dtype=np.int64), **sites ) @@ -137,6 +132,12 @@ def test_visualization_builder_imports_only_declared_global_roots(self): self.assertIsNone(stage_info) self.assertEqual(site_index_map, {}) + # A non-cloning sensor must not exclude the body selected from its owner's subtree. + cfgs = AssetBaseCfg(prim_path="/World"), SensorBaseCfg(prim_path="/World/Declared") + plan = make_clone_plan(cfgs, ((),), 1, shared_assets=(0, 1)) + builder, _, _ = NewtonReplicateContext(self.sim).replicate(plan, (0,)) + self.assertCountEqual(builder.body_label, ["/World/Declared", "/World/Undeclared", "/World/Excluded"]) + assets = ( AssetBaseCfg(prim_path="/Copies/env_[^/]+/Body", spawn=SpawnerCfg(spawn_path="/World/Declared")), AssetBaseCfg(prim_path="/World"), @@ -196,11 +197,7 @@ def test_cable_import_binds_only_supported_native_instances_without_destination_ plan = make_clone_plan(assets, ((0, 1),), 2, shared_assets=range(2, len(assets)), env_template="/Scene/copy_{}") builder, _, _ = NewtonReplicateContext(self.sim).replicate(plan, (0, *range(2, len(shared) + 2))) model = builder.finalize(device="cpu") - with mock.patch( - "isaaclab_newton.physics.newton_manager.get_current_stage", - side_effect=AssertionError("Stage discovery is not a binding input."), - ): - bindings = replicate_module.NewtonManager.collect_cable_segment_shape_ids() + bindings = replicate_module.NewtonManager._cable_bindings self.assertEqual(set(bindings), {"/Scene/SharedRope", "/Scene/copy_0/Rope", "/Scene/copy_1/Rope"}) for path, shape_ids in bindings.items(): self.assertEqual( @@ -282,68 +279,70 @@ def test_visualization_builder_uses_clone_plan_sources_and_rewrites_labels(self) {world: f"/World/envs/env_{world}/Material" for world in range(3)}, ) - def test_shadow_deformables_use_plan_placement_without_destination_prims(self): - stage = Usd.Stage.CreateInMemory() + def test_render_deformables_import_once_then_follow_native_replication(self): + stage = self.sim.stage = Usd.Stage.CreateInMemory() + UsdGeom.SetStageUpAxis(stage, UsdGeom.Tokens.z) self._define_xform(stage, "/Scene/copy_7", (10.0, 0.0, 0.0)) self._define_xform(stage, "/Scene/copy_7/Parent", (2.0, 0.0, 0.0)) - path = "/Scene/copy_7/Parent/Cloth" - vertices = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0]], dtype=np.float32) - cloth = UsdGeom.Mesh.Define(stage, path) - cloth.AddTranslateOp().Set((3.0, 0.0, 0.0)) - cloth.GetPrim().SetMetadata("apiSchemas", Sdf.TokenListOp.CreateExplicit(["OmniPhysicsDeformableBodyAPI"])) - cloth.CreatePointsAttr(vertices) - cloth.CreateFaceVertexCountsAttr([3]) - cloth.CreateFaceVertexIndicesAttr([0, 1, 2]) - asset = AssetBaseCfg(prim_path="/Scene/copy_[^/]+/Parent/Cloth", spawn=SpawnerCfg(spawn_path=path)) - options = dict(env_template="/Scene/copy_{}", clone_strategy=lambda _weights, _count: np.array([0, 1, 0])) - plan = make_clone_plan((asset,), ((0,), ()), 3, **options) + sources = "/Scene/copy_7/Parent", "/Sources/Volume", "/Sources/Tet", "/Shared" + vertices = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0], [0, 0, 1]], dtype=np.float32) + visual = np.concatenate((vertices[:3], vertices[:3] + [0, 0, 1])).astype(np.float32) + roots = sources[0] + "/Cloth", *sources[1:] + for root, volume in zip(roots, (False, True, True, False), strict=True): + prim = UsdGeom.Xform.Define(stage, root).GetPrim() + prim.SetMetadata("apiSchemas", Sdf.TokenListOp.CreateExplicit(["OmniPhysicsDeformableBodyAPI"])) + if root.endswith("Cloth"): + UsdGeom.Xform(prim).AddTranslateOp().Set((3.0, 0.0, 0.0)) + if volume: + mesh = UsdGeom.TetMesh.Define(stage, root + "/sim") + mesh.CreatePointsAttr(vertices) + mesh.CreateTetVertexIndicesAttr([(0, 1, 2, 3)]) + if root == sources[1]: + mesh = UsdGeom.Mesh.Define(stage, root + "/vis") + mesh.CreatePointsAttr(visual) + mesh.CreateFaceVertexCountsAttr([3, 3]) + mesh.CreateFaceVertexIndicesAttr(np.arange(6)) + else: + mesh = UsdGeom.Mesh.Define(stage, root + "/sim") + mesh.CreatePointsAttr(vertices[:3]) + mesh.CreateFaceVertexCountsAttr([3]) + mesh.CreateFaceVertexIndicesAttr([0, 1, 2]) + assets = tuple( + AssetBaseCfg(prim_path="/Scene/copy_[^/]+/" + name, spawn=SpawnerCfg(spawn_path=source)) + for name, source in zip(("Parent", "Volume", "Tet"), sources[:3], strict=True) + ) + (AssetBaseCfg(prim_path="/Shared"),) env_ids = np.array([7, 9, 12]) - positions = np.array([[10, 0, 0], [20, 0, 0], [30, 0, 0]], dtype=np.float32) - prototypes = deformable_prototypes(stage, plan) - for positions in (positions, None): + quaternions = np.asarray([[0, 0, 0, 1], [0, 0, 0, 1], [0, 0, 2**-0.5, 2**-0.5]], dtype=np.float32) + for positions in (np.array([[10, 0, 0], [20, 0, 0], [30, 0, 0]], dtype=np.float32), None): with self.subTest(positions=positions): - builder = newton.ModelBuilder() - instances = expand_deformable_entries(prototypes, plan, env_ids, positions) - offsets = visualization_deformables_module.add_shadow_deformables_to_builder(builder, instances) - self.assertEqual(offsets, {path: 0, "/Scene/copy_12/Parent/Cloth": 3}) + options = dict(shared_assets=(3,), env_template="/Scene/copy_{}") + options.update(clone_strategy=lambda _weights, _count: np.array([0, 1, 0]), positions=positions) + plan = make_clone_plan(assets, ((0,), (1, 1, 2)), 3, **options) + with mock.patch.object( + newton.ModelBuilder, "add_cloth_mesh", autospec=True, side_effect=newton.ModelBuilder.add_cloth_mesh + ) as add_cloth: + options = dict(plan=plan, asset_prototype_ids=range(4)) + options.update(positions=positions, quaternions=quaternions) + builder, _, _ = replicate_module._replicate_newton(stage, env_ids, self.sim, **options) + self.assertEqual(add_cloth.call_count, 3) # Three prototypes, not five destination meshes. + np.testing.assert_array_equal(np.bincount(np.asarray(builder.particle_world) + 1), [3, 3, 16, 3]) + offsets = self.sim.physics_manager.register_callback.call_args.args[0].args[1] + expected = {"/Shared/sim": vertices[:3]} + origins = np.zeros((3, 3)) if positions is None else positions + for world, env_id in enumerate(env_ids): + xform = wp.transform(origins[world], quaternions[world]) + meshes = {"Volume/vis": visual, "Volume_1/vis": visual, "Tet/sim": vertices} + if world != 1: + meshes = {"Parent/Cloth/sim": vertices[:3] + [15, 0, 0] - origins[0]} + for suffix, points in meshes.items(): + expected[f"/Scene/copy_{env_id}/{suffix}"] = np.asarray( + [wp.transform_point(xform, wp.vec3(point)) for point in points] + ) + self.assertEqual(set(offsets), set(expected)) + for path, points in expected.items(): + start = offsets[path] + np.testing.assert_allclose(builder.particle_q[start : start + len(points)], points, atol=1e-5) self.assertFalse(stage.GetPrimAtPath("/Scene/copy_12")) - offset = np.zeros(3) if positions is None else positions[2] - positions[0] - # Root translation is baked into vertices, not applied a second time at placement. - source_points = vertices + [15.0, 0.0, 0.0] - np.testing.assert_allclose(builder.particle_q, np.concatenate((source_points, source_points + offset))) - - def test_shadow_visual_topologies_keep_heterogeneous_offsets(self): - vertices = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0], [0, 0, 1], [1, 0, 1], [0, 1, 1]]) - entries = tuple( - DeformableStageEntry( - root_path=f"/Source/{name}", - sim_mesh_path=f"/Source/{name}/Simulation", - vis_mesh_path=f"/Source/{name}/Visual", - deformable_type="volume", - vertex_count=4, - vis_vertex_count=count, - vis_vertices=vertices[:count], - vis_indices=np.arange(count), - ) - for name, count in (("A", 3), ("B", 6)) - ) - assets = tuple( - AssetBaseCfg(prim_path="/Copies/[^/]+/Body", spawn=SpawnerCfg(spawn_path=entry.root_path)) - for entry in entries - ) - options = dict(env_template="/Copies/{}", clone_strategy=lambda _weights, _count: np.array([0, 1, 0])) - plan = make_clone_plan(assets, ((0,), (1,)), 3, **options) - env_ids = np.array([2, 10, 30]) - builder = newton.ModelBuilder() - builder.add_particle(pos=wp.vec3(), vel=wp.vec3(), mass=1.0) - offsets = visualization_deformables_module.add_shadow_deformables_to_builder( - builder, expand_deformable_entries(entries, plan, env_ids) - ) - self.assertEqual( - offsets, {"/Copies/2/Body/Visual": 1, "/Copies/30/Body/Visual": 4, "/Copies/10/Body/Visual": 7} - ) - self.assertEqual(builder.particle_count, 13) - self.assertEqual(len(builder.tri_indices), 4) class TestReplicationNamesItsCopies(unittest.TestCase): @@ -405,7 +404,19 @@ def resolve_motor_owners(builder): positions = np.zeros((len(env_ids), 3), dtype=np.float32) quaternions = np.tile([0, 0, 0, 1], (len(env_ids), 1)).astype(np.float32) assets = {self._SRC: source, sibling_material: newton.ModelBuilder()} - replicate_builder_mapping(builder, plan, positions, quaternions, assets, env_ids=env_ids) + add_builder = newton.ModelBuilder.add_builder + source_paths = {name: attr.values.copy() for name, attr in attributes.items() if attr.dtype is str} + + def append_copy(destination, asset, **kwargs): + if asset is source: + for name, labels in original.items(): + self.assertEqual(getattr(asset, name), labels) + for name, values in source_paths.items(): + self.assertEqual(asset.custom_attributes[name].values, values) + return add_builder(destination, asset, **kwargs) + + with mock.patch.object(newton.ModelBuilder, "add_builder", append_copy): + replicate_builder_mapping(builder, plan, positions, quaternions, assets, env_ids=env_ids) for name, source_labels in original.items(): expected = [ label.replace(self._SRC, f"{self._ENV.format(i)}/Robot", 1) for i in env_ids for label in source_labels @@ -426,6 +437,16 @@ def resolve_motor_owners(builder): {index * 3: self._SRC + "/Looks/material" for index in range(len(env_ids))}, ) + # Declaring the environment root must produce the same names as declaring its assets. + root = self._ENV.format(0) + plan = make_clone_plan((AssetBaseCfg(prim_path=root),), ((0,),), len(env_ids)) + rooted = newton.ModelBuilder() + replicate_builder_mapping(rooted, plan, positions, quaternions, {root: source}, env_ids=env_ids) + for name in label_names: + self.assertEqual(getattr(rooted, name), getattr(builder, name)) + for name in source_paths: + self.assertEqual(rooted.custom_attributes[name].values, builder.custom_attributes[name].values) + def test_hook_labels_are_rewritten_after_the_slow_path(self): source = newton.ModelBuilder() source.add_body(label=f"{self._SRC}/base") diff --git a/source/isaaclab_newton/test/cloner/test_visual_shape_import.py b/source/isaaclab_newton/test/cloner/test_visual_shape_import.py index c42705d57710..6cd49c001fba 100644 --- a/source/isaaclab_newton/test/cloner/test_visual_shape_import.py +++ b/source/isaaclab_newton/test/cloner/test_visual_shape_import.py @@ -3,135 +3,34 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Unit tests: the Newton cloner imports visual-only geometry only when it is drawn.""" - -from types import SimpleNamespace +"""The Newton cloner imports visual-only geometry only when it is drawn.""" import newton import pytest -from isaaclab_newton.cloner import newton_clone_utils from isaaclab_newton.cloner.newton_clone_utils import build_source_builders from newton import ShapeFlags from pxr import Usd, UsdGeom, UsdPhysics -_SOURCE = "/World/Asset" - -def _make_stage() -> Usd.Stage: - """A rigid body with one collider cube and one visual-only cube.""" +@pytest.mark.parametrize("load_visual_shapes", [True, False]) +@pytest.mark.parametrize("dynamic", [True, False]) +def test_visual_shapes_hide_only_their_own_colliders(load_visual_shapes, dynamic): + """Nested colliders stay hidden beside their visuals, including disabled rigid bodies.""" stage = Usd.Stage.CreateInMemory() UsdGeom.SetStageUpAxis(stage, UsdGeom.Tokens.z) UsdGeom.SetStageMetersPerUnit(stage, 1.0) - xform = UsdGeom.Xform.Define(stage, _SOURCE) - UsdPhysics.RigidBodyAPI.Apply(xform.GetPrim()) - - collider = UsdGeom.Cube.Define(stage, f"{_SOURCE}/collision") + path = "/World/Asset" + root = UsdGeom.Xform.Define(stage, path) + UsdPhysics.RigidBodyAPI.Apply(root.GetPrim()).CreateRigidBodyEnabledAttr(dynamic) + collider = UsdGeom.Cube.Define(stage, path + "/Collisions/collision") UsdPhysics.CollisionAPI.Apply(collider.GetPrim()) - UsdGeom.Cube.Define(stage, f"{_SOURCE}/visual") - return stage - - -def _shape_counts(builder: newton.ModelBuilder) -> tuple[int, int]: - """Number of (colliding, visual-only) shapes in the builder.""" - colliding = sum(1 for flags in builder.shape_flags if flags & ShapeFlags.COLLIDE_SHAPES) - return colliding, len(builder.shape_flags) - colliding - - -class _CountingStage: - """Stage proxy that counts the prim lookups a pass performs.""" - - def __init__(self, stage: Usd.Stage): - self._stage = stage - self.prim_lookups = 0 - - def GetPrimAtPath(self, path: str) -> Usd.Prim: # noqa: N802 (USD API spelling) - self.prim_lookups += 1 - return self._stage.GetPrimAtPath(path) - - -def _build(stage: Usd.Stage, **kwargs) -> newton.ModelBuilder: - return build_source_builders( - stage, - [_SOURCE], - create_builder=lambda: newton.ModelBuilder(up_axis=newton.Axis.Z), - schema_resolvers=[], - **kwargs, - )[_SOURCE] - - -class TestClonerVisualShapeImport: - """``build_source_builders`` must keep colliders regardless of the visual-shape flag.""" - - @pytest.mark.parametrize("load_visual_shapes", [True, False]) - def test_visual_shape_flag_gates_only_visual_only_geometry(self, load_visual_shapes): - """The visual-only cube is imported only with visual shapes; the collider is always kept.""" - colliding, visual_only = _shape_counts(_build(_make_stage(), load_visual_shapes=load_visual_shapes)) - assert colliding == 1 - assert visual_only == int(load_visual_shapes) - - def test_skipping_visual_shapes_skips_collider_visibility_resolution(self): - """Without visual shapes no collider is hidden, so the restore pass must not run. - - The pass resolves USD visibility and purpose once per collider shape, which is pure - overhead in a run that imports no visual geometry: the flags it would set are set - already, and nothing draws them. - """ - stage = _make_stage() - builder = _build(stage, load_visual_shapes=False) - path_shape_map = {f"{_SOURCE}/collision": 0} - flags_before = list(builder.shape_flags) - - headless_stage = _CountingStage(stage) - newton_clone_utils._restore_visible_colliders_without_visual_shapes( - builder, headless_stage, path_shape_map, load_visual_shapes=False - ) - assert headless_stage.prim_lookups == 0 - assert list(builder.shape_flags) == flags_before - - # The same call with the flag on does reach the stage, so the guard is what saved it. - rendering_stage = _CountingStage(stage) - newton_clone_utils._restore_visible_colliders_without_visual_shapes( - builder, rendering_stage, path_shape_map, load_visual_shapes=True - ) - assert rendering_stage.prim_lookups == 1 - # ...and the collider was visible either way, so skipping it changed nothing. - assert list(builder.shape_flags) == flags_before - - def test_nested_static_collider_uses_rigid_body_root_for_visual_lookup(self): - """A collision subtree stays hidden when its rigid-body root has visuals.""" - stage = Usd.Stage.CreateInMemory() - root = UsdGeom.Xform.Define(stage, _SOURCE) - UsdPhysics.RigidBodyAPI.Apply(root.GetPrim()) - collider = UsdGeom.Cube.Define(stage, f"{_SOURCE}/Collisions/collision") - UsdPhysics.CollisionAPI.Apply(collider.GetPrim()) - UsdGeom.Cube.Define(stage, f"{_SOURCE}/Visuals/visual") - builder = SimpleNamespace( - shape_body=[-1], - shape_flags=[ShapeFlags.COLLIDE_SHAPES], - shape_label=[str(collider.GetPrim().GetPath())], - shape_type=[newton.GeoType.BOX], - ) - - newton_clone_utils._restore_visible_colliders_without_visual_shapes( - builder, stage, {str(collider.GetPrim().GetPath()): 0} - ) - - assert not builder.shape_flags[0] & ShapeFlags.VISIBLE - - def test_generated_proxy_collider_visual_remains_hidden(self): - """Newton's unauthored visual companion must not expose a proxy collider.""" - stage = Usd.Stage.CreateInMemory() - collider = UsdGeom.Cube.Define(stage, f"{_SOURCE}/collision") - UsdPhysics.CollisionAPI.Apply(collider.GetPrim()) - collider_path = str(collider.GetPrim().GetPath()) - builder = SimpleNamespace( - shape_body=[-1, -1], - shape_flags=[ShapeFlags.COLLIDE_SHAPES, ShapeFlags.VISIBLE], - shape_label=[collider_path, f"{collider_path}_visual"], - shape_type=[newton.GeoType.BOX, newton.GeoType.MESH], - ) - - newton_clone_utils._restore_visible_colliders_without_visual_shapes(builder, stage, {collider_path: 0}) - - assert not builder.shape_flags[1] & ShapeFlags.VISIBLE + UsdGeom.Cube.Define(stage, path + "/Visuals/visual") + + builder = build_source_builders(stage, [path], newton.ModelBuilder, [], load_visual_shapes=load_visual_shapes)[path] + flags = dict(zip(builder.shape_label, builder.shape_flags, strict=True)) + assert len(flags) == 1 + load_visual_shapes + assert flags[path + "/Collisions/collision"] & ShapeFlags.COLLIDE_SHAPES + assert bool(flags[path + "/Collisions/collision"] & ShapeFlags.VISIBLE) is not load_visual_shapes + if load_visual_shapes: + assert flags[path + "/Visuals/visual"] & ShapeFlags.VISIBLE diff --git a/source/isaaclab_newton/test/physics/test_newton_fabric_body_sync.py b/source/isaaclab_newton/test/physics/test_newton_fabric_body_sync.py index fa2f177b8329..9b8e5e12a1fe 100644 --- a/source/isaaclab_newton/test/physics/test_newton_fabric_body_sync.py +++ b/source/isaaclab_newton/test/physics/test_newton_fabric_body_sync.py @@ -301,6 +301,7 @@ def test_root_pose_sync_preserves_authored_scale(device, renderer_cfg): sim.reset() scene.reset() + sim.get_or_create_backend(sim.fabric_cfg).bind_transforms(sim.get_scene_data_provider()) _render(sim, scene) torch.testing.assert_close(_fabric_scale(body_path), authored_scale, rtol=0.0, atol=1.0e-5) @@ -313,7 +314,7 @@ def test_root_pose_sync_preserves_authored_scale(device, renderer_cfg): scene["cube"].write_root_link_pose_to_sim_index(root_pose=target_pose) if isinstance(renderer_cfg, NewtonWarpRendererCfg): assert not sim.visualizers - NewtonManager.sync_transforms_to_fabric() + sim.get_or_create_backend(sim.fabric_cfg).update_transforms(sim.get_scene_data_provider()) _render(sim, scene) torch.testing.assert_close(_fabric_position(body_path), target_pose[0, :3].cpu(), rtol=0.0, atol=1.0e-4) diff --git a/source/isaaclab_newton/test/physics/test_newton_manager_abstraction.py b/source/isaaclab_newton/test/physics/test_newton_manager_abstraction.py index c203f4bd75b8..69254e4843c3 100644 --- a/source/isaaclab_newton/test/physics/test_newton_manager_abstraction.py +++ b/source/isaaclab_newton/test/physics/test_newton_manager_abstraction.py @@ -23,10 +23,8 @@ import contextlib import logging -import sys from inspect import signature from types import SimpleNamespace -from unittest.mock import Mock import isaaclab_newton.physics.newton_manager as newton_manager_module import numpy as np @@ -725,7 +723,6 @@ def test_mpm_prepare_builder_converts_convex_mesh_before_solver_construction(): assert isinstance(solver, SolverImplicitMPM) -@pytest.mark.parametrize("import_path", ["clone", "standalone"]) @pytest.mark.parametrize( ("manager_cls", "solver_cfg", "expected_friction", "expected_damping"), [ @@ -734,9 +731,9 @@ def test_mpm_prepare_builder_converts_convex_mesh_before_solver_construction(): ], ) def test_production_imports_scope_mujoco_joint_properties( - monkeypatch, import_path, manager_cls, solver_cfg, expected_friction, expected_damping + monkeypatch, manager_cls, solver_cfg, expected_friction, expected_damping ): - """Only MJWarp imports MuJoCo joint properties through either production path.""" + """Only MJWarp imports MuJoCo joint properties through clone-plan construction.""" from pxr import Sdf, Usd, UsdGeom, UsdPhysics stage = Usd.Stage.CreateInMemory() @@ -745,7 +742,7 @@ def test_production_imports_scope_mujoco_joint_properties( physics_prim_path = "/physicsScene" UsdPhysics.Scene.Define(stage, physics_prim_path) - root_path = "/Sources/robot" if import_path == "clone" else "/World/robot" + root_path = "/Sources/robot" root = UsdGeom.Cube.Define(stage, root_path).GetPrim() UsdPhysics.RigidBodyAPI.Apply(root) UsdPhysics.ArticulationRootAPI.Apply(root) @@ -769,38 +766,29 @@ def test_production_imports_scope_mujoco_joint_properties( PhysicsManager, "_sim", SimpleNamespace( - physics_manager=manager_cls, cfg=SimpleNamespace(physics=physics_cfg, physics_prim_path=physics_prim_path) + physics_manager=manager_cls, + device="cpu", + cfg=SimpleNamespace(physics=physics_cfg, physics_prim_path=physics_prim_path), ), ) monkeypatch.setattr(PhysicsManager, "_cfg", physics_cfg) monkeypatch.setattr(PhysicsManager, "_device", "cpu") monkeypatch.setattr(NewtonManager, "_builder", None) - monkeypatch.setattr(NewtonManager, "_deformable_registry", []) + monkeypatch.setattr(NewtonManager, "_scene_data_backend", newton_manager_module.NewtonSceneDataBackend()) monkeypatch.setattr(NewtonManager, "_cl_pending_sites", {}) monkeypatch.setattr(NewtonManager, "_per_world_builder_hooks", []) monkeypatch.setattr(NewtonManager, "_world_xforms", None) monkeypatch.setattr(NewtonManager, "_cl_site_index_map", {}) - monkeypatch.setattr(NewtonManager, "_cl_fabric_body_bindings", []) monkeypatch.setattr(NewtonManager, "_cl_protos", {}) monkeypatch.setattr(NewtonManager, "_num_envs", 0) - if import_path == "clone": - builder, _ = newton_physics_replicate( - stage=stage, - sources=(root_path,), - destinations=("/World/envs/env_{}/robot",), - env_ids=np.array([0], dtype=np.int64), - mapping=np.ones((1, 1), dtype=np.bool_), - ) - else: - monkeypatch.setattr(newton_manager_module, "get_current_stage", lambda: stage) - monkeypatch.setattr( - newton_manager_module, "_restore_visible_colliders_without_visual_shapes", lambda *args: None - ) - monkeypatch.setattr(newton_manager_module, "replace_newton_builder_shape_colors", lambda *args: None) - monkeypatch.setattr(newton_manager_module, "import_builder_visual_material_paths", lambda *args: None) - manager_cls.instantiate_builder_from_stage() - builder = NewtonManager._builder + builder, _ = newton_physics_replicate( + stage=stage, + sources=(root_path,), + destinations=("/World/envs/env_{}/robot",), + env_ids=np.array([0], dtype=np.int64), + mapping=np.ones((1, 1), dtype=np.bool_), + ) model = builder.finalize(device="cpu") @@ -993,7 +981,6 @@ def test_mpm_supported_cuda_graph_capture_defers_until_initial_reset(monkeypatch monkeypatch.setattr(PhysicsManager, "_cfg", SimpleNamespace(use_cuda_graph=True), raising=False) monkeypatch.setattr(PhysicsManager, "_device", "cuda:0", raising=False) monkeypatch.setattr(NewtonManager, "_solver", solver, raising=False) - monkeypatch.setattr(NewtonManager, "_usdrt_stage", None, raising=False) monkeypatch.setattr(NewtonManager, "_graph", object(), raising=False) monkeypatch.setattr(NewtonManager, "_graph_capture_pending", False, raising=False) @@ -1054,7 +1041,7 @@ def __exit__(self, exc_type, exc_value, traceback): return False monkeypatch.setattr(PhysicsManager, "_device", "cuda:1", raising=False) - monkeypatch.setattr(NewtonManager, "_usdrt_stage", None, raising=False) + monkeypatch.setattr(newton_manager_module, "has_kit", lambda: False) stream = SimpleNamespace(device="cuda:1") monkeypatch.setattr(wp, "get_stream", lambda device: stream if device == "cuda:1" else None) monkeypatch.setattr(wp, "ScopedStream", lambda stream: contextlib.nullcontext()) @@ -1248,12 +1235,6 @@ def test_initialize_solver_prepares_picking_after_scene_data(monkeypatch): ) with build_simulation_context(sim_cfg=sim_cfg) as sim: - monkeypatch.setitem(sys.modules, "usdrt", Mock()) - monkeypatch.setattr(NewtonMJWarpManager, "_clone_physics_only", False) - monkeypatch.setattr(newton_manager_module, "get_current_stage", lambda **kwargs: Mock()) - monkeypatch.setattr( - NewtonManager, "_initialize_fabric_body_prims", staticmethod(lambda *args: events.append("body")) - ) def on_physics_ready(_): events.append("ready") @@ -1273,7 +1254,7 @@ def on_physics_ready(_): sim.reset() sim.reset() - assert events == ["body", "ready", "prepare"] * 2 + assert events == ["ready", "prepare"] * 2 # --------------------------------------------------------------------------- @@ -1637,7 +1618,8 @@ def test_graph_capture_preserves_first_step_and_recapture(monkeypatch, solver_cf NewtonManager.set_builder(builder) sim.reset() if rtx_capture: - monkeypatch.setattr(NewtonManager, "_usdrt_stage", object()) + monkeypatch.setattr(newton_manager_module, "has_kit", lambda: True) + monkeypatch.setattr(sim, "_has_offscreen_render", True) NewtonManager.activate_newton_actuator_path() counter = wp.zeros(1, dtype=wp.int32, device="cuda:0") NewtonManager.register_post_step_callback(lambda: wp.launch(_count_physics_steps, 1, inputs=[counter])) @@ -1674,7 +1656,8 @@ def test_stateful_actuator_graph_matches_eager_across_decimation_changes(monkeyp builder.end_world() NewtonManager.set_builder(builder) sim.reset() - monkeypatch.setattr(NewtonManager, "_usdrt_stage", object()) + monkeypatch.setattr(newton_manager_module, "has_kit", lambda: True) + monkeypatch.setattr(sim, "_has_offscreen_render", True) NewtonManager.activate_newton_actuator_path() samples = [] for decimation in (1, 2, 3): diff --git a/source/isaaclab_newton/test/physics/test_vbd_core.py b/source/isaaclab_newton/test/physics/test_vbd_core.py index 78d56c2342ff..407e6eaae8e0 100644 --- a/source/isaaclab_newton/test/physics/test_vbd_core.py +++ b/source/isaaclab_newton/test/physics/test_vbd_core.py @@ -12,6 +12,7 @@ import pytest from isaaclab_newton.physics import NewtonBackendCfg, NewtonManager, NewtonSoftContactCfg +from newton import ModelBuilder from isaaclab.sim import SimulationContext @@ -50,7 +51,7 @@ def state(self): def control(self): return object() - class Builder: + class Builder(ModelBuilder): def finalize(self, *, device): return model @@ -74,101 +75,6 @@ def __deepcopy__(self, memo): assert backend.model is backend.state_0 is backend.state_1 is backend.control is None -@pytest.mark.parametrize("env_paths", [(), ("/World/Env_0", "/World/Env_1")], ids=["flat", "replicated"]) -def test_vbd_excludes_registered_deformable_meshes(monkeypatch, env_paths): - """VBD excludes registered simulation and visual meshes from USD import.""" - physics = importlib.import_module("isaaclab_newton.physics") - pxr = importlib.import_module("pxr") - newton_module = importlib.import_module("isaaclab_newton.physics.newton_manager") - builders = [] - hook_calls = [] - replicate_calls = [] - - class Builder: - def __init__(self): - self.imports = [] - self.color_calls = [] - - def add_usd(self, stage, *, root_path=None, ignore_paths=(), schema_resolvers=()): - self.imports.append((root_path, list(ignore_paths))) - return {"path_shape_map": {}} - - def color(self, *, balance_colors): - self.color_calls.append(balance_colors) - - children = [ - SimpleNamespace( - GetName=lambda path=path: path.rsplit("/", 1)[-1], - GetPath=lambda path=path: SimpleNamespace(pathString=path), - ) - for path in env_paths - ] - world_prim = SimpleNamespace(IsValid=lambda: True, GetChildren=lambda: children) - stage = SimpleNamespace(GetPrimAtPath=lambda path: world_prim if path == "/World" else path) - rotation = SimpleNamespace(GetImaginary=lambda: (0.0, 0.0, 0.0), GetReal=lambda: 1.0) - matrix = SimpleNamespace(ExtractTranslation=lambda: (0.0, 0.0, 0.0), ExtractRotationQuat=lambda: rotation) - usd_geom = SimpleNamespace( - GetStageUpAxis=lambda stage: "Z", - XformCache=lambda: SimpleNamespace(GetLocalToWorldTransform=lambda prim: matrix), - ) - - def create_builder(cls, *, up_axis): - builder = Builder() - builders.append(builder) - return builder - - def replicate(*args, **kwargs): - replicate_calls.append(kwargs) - return {}, [object() for _ in env_paths], [] - - monkeypatch.setattr(newton_module, "get_current_stage", lambda: stage) - monkeypatch.setattr(pxr, "UsdGeom", usd_geom) - monkeypatch.setattr(newton_module, "_restore_visible_colliders_without_visual_shapes", lambda *args: None) - monkeypatch.setattr(newton_module, "import_builder_visual_material_paths", lambda *args: None) - monkeypatch.setattr(newton_module, "replace_newton_builder_shape_colors", lambda *args: None) - monkeypatch.setattr(newton_module, "replicate_builder_mapping", replicate) - monkeypatch.setattr(physics.NewtonVBDManager, "create_builder", classmethod(create_builder)) - monkeypatch.setattr( - physics.NewtonVBDManager, - "_inject_terrain_heightfields", - classmethod(lambda cls, stage, builder, root_paths: ["/World/terrain"]), - ) - monkeypatch.setattr( - physics.NewtonVBDManager, - "_cl_inject_sites", - classmethod(lambda cls, builder, source_builders: ({}, {}, {})), - ) - monkeypatch.setattr( - physics.NewtonVBDManager, "set_builder", classmethod(lambda cls, builder: setattr(cls, "_builder", builder)) - ) - - def hook(builder, world_idx, position, rotation): - hook_calls.append(world_idx) - - monkeypatch.setattr(physics.NewtonVBDManager, "_per_world_builder_hooks", [hook]) - monkeypatch.setattr( - physics.NewtonVBDManager, - "_deformable_registry", - [SimpleNamespace(sim_mesh_prim_path="/World/soft/sim", vis_mesh_prim_path="/World/soft/visual")], - ) - monkeypatch.setattr(physics.NewtonVBDManager, "_builder", None) - monkeypatch.setattr(NewtonManager, "_cl_site_index_map", {}) - monkeypatch.setattr(NewtonManager, "_world_xforms", []) - monkeypatch.setattr(NewtonManager, "_num_envs", 0) - - physics.NewtonVBDManager.instantiate_builder_from_stage() - - deformable_paths = ["/World/soft/sim", "/World/soft/visual"] - if env_paths: - assert builders[0].imports == [(None, [*env_paths, "/World/terrain", *deformable_paths])] - assert builders[1].imports == [("/World/Env_0", deformable_paths)] - assert replicate_calls[0]["per_world_builder_hooks"] == [hook] - else: - assert builders[0].imports == [(None, ["/World/terrain", *deformable_paths])] - assert hook_calls == [0] - assert builders[0].color_calls == [False] - - def test_vbd_colors_prebuilt_builder_before_start(monkeypatch): """VBD colors a prebuilt builder before starting simulation.""" physics = importlib.import_module("isaaclab_newton.physics") diff --git a/source/isaaclab_newton/test/sensors/test_site_injection.py b/source/isaaclab_newton/test/sensors/test_site_injection.py index 7ef68b1ed8ed..119925292059 100644 --- a/source/isaaclab_newton/test/sensors/test_site_injection.py +++ b/source/isaaclab_newton/test/sensors/test_site_injection.py @@ -9,6 +9,7 @@ import warp as wp from isaaclab_newton.physics.newton_manager import NewtonManager from isaaclab_newton.sensors.frame_transformer.frame_transformer import FrameTransformer +from newton import ModelBuilder from isaaclab.utils.warp.math_ops import transform_to_vec_quat @@ -31,102 +32,31 @@ def test_wrong_dtype_raises(self): transform_to_vec_quat(wp.zeros(3, dtype=wp.vec3f, device="cpu")) -# --------------------------------------------------------------------------- -# NewtonManager._cl_inject_sites_fallback -# --------------------------------------------------------------------------- - - -class MockBuilder: - """Minimal stand-in for ModelBuilder.""" - - def __init__(self, body_labels: list[str]): - self.body_label = body_labels - self._next_idx = 0 - - def add_site(self, body: int, xform: wp.transform, label: str) -> int: - idx = self._next_idx - self._next_idx += 1 - return idx - - -class TestFallbackGlobalSite: - """Global site (body_pattern=None) must produce a (int, None) entry.""" - - def setup_method(self): - NewtonManager.clear() - NewtonManager._builder = MockBuilder(["body0", "body1"]) - - def test_global_site_entry_is_int_none_tuple(self): - xform = wp.transform() - NewtonManager._cl_pending_sites = {(None, False, tuple(xform)): ("ft_0", xform)} - NewtonManager._cl_inject_sites_fallback() - - entry = NewtonManager._cl_site_index_map["ft_0"] - global_idx, per_world = entry - assert isinstance(global_idx, int) - assert per_world is None - assert len(NewtonManager._cl_pending_sites) == 0 - - -class TestFallbackLocalWildcard: - """Wildcard local site matching N bodies must produce (None, [[idx0..idxN-1]]) — one world.""" - - def setup_method(self): - NewtonManager.clear() - NewtonManager._builder = MockBuilder(["Robot/FL_foot", "Robot/FR_foot", "Robot/RL_foot", "Robot/RR_foot"]) - - def test_wildcard_entry_shape(self): - xform = wp.transform() - NewtonManager._cl_pending_sites = {("Robot/.*_foot", False, tuple(xform)): ("ft_0", xform)} - NewtonManager._cl_inject_sites_fallback() - - entry = NewtonManager._cl_site_index_map["ft_0"] - global_idx, per_world = entry - assert global_idx is None - assert len(per_world) == 1 # one world - assert len(per_world[0]) == 4 # four bodies matched - - def test_no_match_raises(self): - xform = wp.transform() - NewtonManager._cl_pending_sites = {("Robot/nonexistent", False, tuple(xform)): ("ft_0", xform)} - with pytest.raises(ValueError): - NewtonManager._cl_inject_sites_fallback() - - -class TestWorldSite: - """World-local sites are per-world, not global.""" - - def setup_method(self): - NewtonManager.clear() - NewtonManager._builder = MockBuilder([]) - - def test_world_site_reuses_label(self): - xform = wp.transform((1.0, 2.0, 3.0), wp.quat_identity()) - label_0 = NewtonManager.cl_register_site(None, xform, per_world=True) - label_1 = NewtonManager.cl_register_site(None, xform, per_world=True) - - assert label_0 == label_1 - - def test_world_site_fallback_entry_is_local(self): - xform = wp.transform((1.0, 2.0, 3.0), wp.quat_identity()) - label = NewtonManager.cl_register_site(None, xform, per_world=True) - NewtonManager._cl_inject_sites_fallback() - - global_idx, per_world = NewtonManager._cl_site_index_map[label] - assert global_idx is None - assert isinstance(per_world, list) - assert len(per_world) == 1 - assert len(per_world[0]) == 1 - - def test_inject_sites_returns_world_sites(self): - xform = wp.transform((1.0, 2.0, 3.0), wp.quat_identity()) - label = NewtonManager.cl_register_site(None, xform, per_world=True) - global_sites, proto_sites, world_sites = NewtonManager._cl_inject_sites(MockBuilder([]), {}) - - assert global_sites == {} - assert proto_sites == {} - assert world_sites[label] == xform - assert NewtonManager._cl_pending_sites == {} +@pytest.mark.parametrize("replicated", [False, True]) +def test_sites_bind_once_to_clone_sources_or_explicit_builder(monkeypatch, replicated): + main = ModelBuilder() + source = ModelBuilder() if replicated else main + for leg in ("FL", "FR", "RL", "RR"): + source.add_body(label=f"Robot/{leg}_foot") + monkeypatch.setattr(NewtonManager, "_cl_pending_sites", {}) + xform = wp.transform((1.0, 2.0, 3.0), wp.quat_identity()) + global_label = NewtonManager.cl_register_site(None, xform) + local_label = NewtonManager.cl_register_site("Robot/.*_foot", xform) + world_label = NewtonManager.cl_register_site(None, xform, per_world=True) + assert NewtonManager.cl_register_site(None, xform, per_world=True) == world_label + + sources = {"Robot": source} if replicated else {} + global_sites, body_sites, world_sites = NewtonManager._cl_inject_sites(main, sources) + assert main.shape_body[global_sites[global_label]] == -1 + indices = body_sites[id(source)][local_label] + assert [source.shape_body[index] for index in indices] == list(range(4)) + assert [source.shape_label[index] for index in indices] == [f"{name}/{local_label}" for name in source.body_label] + assert world_sites == {world_label: xform} + assert not NewtonManager._cl_pending_sites + + NewtonManager.cl_register_site("Robot/nonexistent", xform) + with pytest.raises(ValueError, match="matched no builder bodies"): + NewtonManager._cl_inject_sites(main, sources) # --------------------------------------------------------------------------- diff --git a/source/isaaclab_newton/test/sim/test_mpm_spawners.py b/source/isaaclab_newton/test/sim/test_mpm_spawners.py index 88943aa69255..017b1cbdfe13 100644 --- a/source/isaaclab_newton/test/sim/test_mpm_spawners.py +++ b/source/isaaclab_newton/test/sim/test_mpm_spawners.py @@ -8,24 +8,17 @@ import subprocess import sys import textwrap -from types import SimpleNamespace import numpy as np import pytest -import warp as wp newton = pytest.importorskip("newton") -from isaaclab_newton.assets import MPMObject -from isaaclab_newton.physics import NewtonManager -from isaaclab_newton.physics.newton_manager import NewtonSceneDataBackend from isaaclab_newton.sim.spawners.mpm import MPMGridCfg, MPMParticleMaterialCfg, MPMPointsCfg from newton.solvers import SolverImplicitMPM from pxr import UsdGeom, UsdPhysics, UsdShade import isaaclab.sim as sim_utils -from isaaclab.assets import AssetBaseCfg -from isaaclab.cloner import make_clone_plan pytestmark = pytest.mark.unit @@ -95,47 +88,6 @@ def test_mpm_points_author_and_import_through_usd(stage, monkeypatch): np.testing.assert_allclose( [transform.Transform(point) for point in visual_points.GetPointsAttr().Get()], builder.particle_q, atol=2.0e-7 ) - shared_cfg = cfg.copy() - shared_cfg.func("/World/Shared", shared_cfg, translation=(-2.0, 0.0, 0.0)) - layout = np.zeros(100, dtype=np.int32) - layout[[7, 12]], layout[99] = 1, 2 - cfg.spawn_path, shared_cfg.spawn_path = "/World/Media", "/World/Shared" - plan = make_clone_plan( - ( - AssetBaseCfg(prim_path="/Scene/copy_[^/]+/Media", spawn=cfg), - AssetBaseCfg(prim_path="/Scene/copy_[^/]+/Media", spawn=sim_utils.SpawnerCfg(spawn_path="/World/Other")), - AssetBaseCfg(prim_path="/World/Shared", spawn=shared_cfg), - ), - ((), (0,), (1,)), - 100, - shared_assets=(2,), - clone_strategy=lambda _weights, _num_worlds: layout, - env_template="/Scene/copy_{}", - ) - monkeypatch.setattr(sim_utils.SimulationContext, "instance", lambda: SimpleNamespace(get_clone_plan=lambda: plan)) - state = SimpleNamespace(particle_q=wp.zeros(12, dtype=wp.vec3f, device="cpu")) - monkeypatch.setattr(NewtonManager, "backend", SimpleNamespace(state_0=state)) - backend = NewtonSceneDataBackend() - backend.initialize_geometry(plan) - monkeypatch.setattr(NewtonManager, "_scene_data_backend", backend) - asset = SimpleNamespace( - cfg=SimpleNamespace(prim_path="/Scene/copy_[^/]+/Media", spawn=cfg), - _recorded_particle_offsets=[4, 10], - _particles_per_object=2, - ) - MPMObject._bind_particle_visualization(asset) - asset.cfg.prim_path = "/World/Shared" - asset.cfg.spawn = shared_cfg - asset._recorded_particle_offsets = [0] - MPMObject._bind_particle_visualization(asset) - assert not stage.GetPrimAtPath("/Scene/copy_12/Media/Particles") - [(publication, ranges)] = backend.get_geometry_batches() - assert publication.points is state.particle_q - assert ranges == { - "/Scene/copy_7/Media/Particles": (4, 2), - "/Scene/copy_12/Media/Particles": (10, 2), - "/World/Shared/Particles": (0, 2), - } assert visual_points.GetPrim().GetAttribute("isaaclab:pointsUpdateFrequency").Get() == cfg.visual_update_frequency diff --git a/source/isaaclab_ov/changelog.d/deformable-prototype-bindings.rst b/source/isaaclab_ov/changelog.d/deformable-prototype-bindings.rst new file mode 100644 index 000000000000..b772a1f855d0 --- /dev/null +++ b/source/isaaclab_ov/changelog.d/deformable-prototype-bindings.rst @@ -0,0 +1,6 @@ +Fixed +^^^^^ + +* Bound SDP rigid-body poses through one native OVPhysX view, removing USD discovery, + clone-path reconstruction, and an extra view adapter. Published canonical articulation-link + paths instead of root aliases, with native metadata initialized on both CPU and GPU. diff --git a/source/isaaclab_ov/isaaclab_ov/physics/ovphysx_manager.py b/source/isaaclab_ov/isaaclab_ov/physics/ovphysx_manager.py index 85c99ef825d9..3dd1d9f26269 100644 --- a/source/isaaclab_ov/isaaclab_ov/physics/ovphysx_manager.py +++ b/source/isaaclab_ov/isaaclab_ov/physics/ovphysx_manager.py @@ -91,12 +91,12 @@ def _prepare_default_cache_dir(cache_dir: str) -> str: class OvPhysxSceneDataBackend(SceneDataBackend): """Scene-data backend for the OVPhysX physics manager. - Each rigid-body binding reads directly into its portion of one native pose - buffer. Pointer aliases preserve the binding shape without staging or merging. + The runtime's rigid-body view fills one pose buffer directly. Deformable views + fill their native slices without an intermediate packing pass. """ def __init__(self): - self._rigid_bindings: list[tuple[OvPhysxView, wp.array]] = [] + self.backend: OvPhysxBackend | None = None self._transforms = TimestampedBuffer(SceneDataFormat.Transform()) self.transforms_timestamp = 0 self.geometry_timestamp = 0 @@ -111,64 +111,35 @@ def transform_count(self) -> int: @property def transform_paths(self) -> list[str]: - """Concatenated ``prim_paths`` across all bindings, in registration order.""" - return [path for view, _ in self._rigid_bindings for path in view.prim_paths] + """Native body paths in the same order as the published poses.""" + view = None if self.backend is None else self.backend.rigid_body_view + return [] if view is None else view.prim_paths - def setup(self, physx, stage, device: str, entries: Sequence[DeformableStageEntry] | None = None) -> None: - """Discover RigidBodyAPI prims, dedup by env-wildcard form, create one binding per pattern. + def setup( + self, backend: OvPhysxBackend, device: str, entries: Sequence[DeformableStageEntry] | None = None + ) -> None: + """Bind the native body table and declared deformable geometry. Args: - physx: Live ``ovphysx.PhysX`` instance (the wheel handle). - stage: USD stage to traverse for RigidBodyAPI prims. + backend: Initialized native physics resource. device: Warp device string used to allocate the published buffers. entries: Declared deformables captured before native stage import. ``None`` leaves scene geometry uninitialized; an empty sequence declares no deformables. """ - from isaaclab_ov import tensor_types as TT # local: keep heavy ovphysx out of module load - - self._rigid_bindings = [] + self.backend = backend self._transforms.data.transforms = None self.transforms_timestamp += 1 self._deformable_bindings = [] self.geometry_timestamp += 1 self._geometry = TimestampedBuffer() - if stage is None: - return - - # Discover RigidBodyAPI prims, dedup by env-wildcard form. - patterns: set[str] = set() - for prim in stage.Traverse(): - if prim.HasAPI(UsdPhysics.RigidBodyAPI): - patterns.add(re.sub(r"/World/envs/env_\d+", "/World/envs/env_*", prim.GetPath().pathString)) - - views = [] - for pattern in sorted(patterns): - view = OvPhysxView(physx, pattern=pattern, device=device) - view.binding_for(TT.RIGID_BODY_POSE) - if view.count == 0: - logger.debug("Pattern %s matched 0 rigid bodies; skipping.", pattern) - view.close() - continue - views.append(view) - - if views: - poses = wp.empty(sum(view.count for view in views), dtype=wp.transformf, device=device) - self._transforms.data.transforms = poses - offset = 0 - for view in views: - buffer = wp.array( - ptr=poses.ptr + offset * wp.types.type_size_in_bytes(wp.transformf), - shape=(view.count,), - dtype=wp.transformf, - device=device, - copy=False, - ) - self._rigid_bindings.append((view, buffer)) - offset += view.count + if backend.rigid_body_view is not None and backend.rigid_body_view.count: + self._transforms.data.transforms = wp.empty( + backend.rigid_body_view.count, dtype=wp.transformf, device=device + ) if entries is not None: - self._setup_deformable_bindings(physx, entries, device) + self._setup_deformable_bindings(backend.physx, entries, device) def _setup_deformable_bindings(self, physx, entries: Sequence[DeformableStageEntry], device: str) -> None: """Bind exact planned deformables directly into one flat publication buffer.""" @@ -216,7 +187,9 @@ def _setup_deformable_bindings(self, physx, entries: Sequence[DeformableStageEnt self._deformable_bindings.append((view, tensor_type, buffer)) offset += view.count * count offsets = np.cumsum(np.r_[0, counts[:-1]]) - self._geometry.data = deformable_geometry_batches(native_entries, points, offsets) + self._geometry.data = deformable_geometry_batches(native_entries, offsets, device=device) + for publication, _ in self._geometry.data: + publication.points = points def get_geometry_batches(self, output_format: Any = SceneDataFormat.Points) -> list: """Publish native positions with exact visual paths and interpolation metadata. @@ -248,8 +221,8 @@ def transforms(self) -> SceneDataFormat.Transform: """Publish native rigid-body poses [m, xyzw].""" if self._transforms.timestamp != self.transforms_timestamp: OvPhysxManager.pre_render() - for view, buffer in self._rigid_bindings: - view.read_into("rigid_body_pose", buffer) + if self._transforms.data.transforms is not None: + self.backend.rigid_body_view.read(self._transforms.data.transforms) self._transforms.timestamp = self.transforms_timestamp return self._transforms.data @@ -280,6 +253,7 @@ def __init__(self, cfg: OvPhysxBackendCfg): active_cuda_gpus=cfg.device.removeprefix("cuda:") if is_gpu else None, ) self.stage: Any = None + self.rigid_body_view: Any = None def close(self) -> None: """Release bindings, the runtime, and its stage in native teardown order.""" @@ -295,6 +269,9 @@ def close(self) -> None: release_owners = destroy_entry_point == "release" try: try: + if self.rigid_body_view is not None: + self.rigid_body_view.destroy() + self.rigid_body_view = None OvPhysxView._close_all_for(physx) finally: try: @@ -613,6 +590,9 @@ def _prepare_physx_for_stage_reuse(cls) -> None: physx = cls.backend.physx if physx is None: return + if cls.backend.rigid_body_view is not None: + cls.backend.rigid_body_view.destroy() + cls.backend.rigid_body_view = None OvPhysxView._close_all_for(physx) physx.wait_op(physx.reset_stage()) if cls.backend.stage is not None: @@ -856,7 +836,7 @@ def _warmup_and_load(cls) -> None: choice and registers process-exit cleanup. On a forced re-warm before :meth:`close`, it reuses the active instance, attaches the new USD through OVStage, rebuilds active clone recipes through full-stage materialization - or runtime replay, and (on GPU) re-runs the supported warmup entry point + or runtime replay, and re-runs the supported warmup entry point so the new stage's bodies are resident. Raises: @@ -936,18 +916,27 @@ def _warmup_and_load(cls) -> None: cls._replay_pending_clones(cls.backend.physx, requires_full_stage=cls._requires_full_stage) - # GPU bodies must be re-warmed after every OVStage attachment: the cached PhysX - # instance carries its old buffer layout from the previous stage. - if ovphysx_device == "gpu": - cls._warmup_physx(cls.backend.physx) + # Native metadata and bindings must see the newly attached bodies, including on CPU. + cls._warmup_physx(cls.backend.physx) - # Initialize the SceneDataBackend now that the wheel's PhysX is live and - # the OVStage is attached. The central - # ``isaaclab.scene.scene_data_provider.SceneDataProvider`` consumes this - # via :meth:`get_scene_data_backend`. + # Bind SDP after the native runtime has realized the stage and clones. if cls._scene_data_backend is None: cls._scene_data_backend = OvPhysxSceneDataBackend() - cls._scene_data_backend.setup(cls.backend.physx, sim.stage, PhysicsManager._device, entries) + # Native output metadata names the actual bodies, not articulation-root aliases. + # SDK imports follow runtime initialization so inactive backends remain optional. + from ovphysx.types import SimObjectType + from ovstage import PathDictionary + + body_paths = [] + with PathDictionary() as paths: + for kind in (SimObjectType.RIGID_BODY, SimObjectType.ARTICULATION_LINK): + # Mass is host-resident: obtaining paths needs no GPU pose read or another step. + with cls.backend.physx.read(kind, ["mass"]) as bodies: + for group in bodies.groups: + body_paths.extend(paths.get_path_strings(group.prim_list)) + if body_paths: + cls.backend.rigid_body_view = cls.backend.physx.create_tensor_binding(prim_paths=body_paths) + cls._scene_data_backend.setup(cls.backend, PhysicsManager._device, entries) cls.dispatch_event(PhysicsEvent.MODEL_INIT, payload={}) cls._warmup_done = True diff --git a/source/isaaclab_ov/test/assets/test_rigid_object.py b/source/isaaclab_ov/test/assets/test_rigid_object.py index 3052256c6d56..6e715db69101 100644 --- a/source/isaaclab_ov/test/assets/test_rigid_object.py +++ b/source/isaaclab_ov/test/assets/test_rigid_object.py @@ -25,7 +25,6 @@ import logging import sys from typing import Literal -from unittest.mock import MagicMock import pytest import torch @@ -42,7 +41,7 @@ from isaaclab_ov import tensor_types as TT # noqa: E402 from isaaclab_ov.assets import RigidObject # noqa: E402 -from isaaclab_ov.physics import OvPhysxCfg, OvPhysxManager # noqa: E402 +from isaaclab_ov.physics import OvPhysxCfg # noqa: E402 import isaaclab.sim as sim_utils # noqa: E402 from isaaclab.assets import RigidObjectCfg # noqa: E402 @@ -79,7 +78,6 @@ def _ovphysx_skip_other_device(request): callspec = getattr(request.node, "callspec", None) device = callspec.params.get("device") if callspec is not None else None if device is None: - # Test does not parametrize on device (e.g. test_warmup_attach_stage_not_called_for_cpu). return locked = _LOCKED_DEVICE[0] if locked is None: @@ -199,6 +197,11 @@ def test_initialization_with_kinematic_enabled(num_cubes, device): # Play sim sim.reset() + # SDP bindings must be ready on CPU and GPU before any asset pose read. + provider = sim.get_scene_data_provider() + assert provider.transform_count == num_cubes + assert set(provider.backend.transform_paths) == {f"/World/Table_{i}/Object" for i in range(num_cubes)} + # Check if object is initialized assert cube_object.is_initialized assert len(cube_object.body_names) == 1 @@ -1014,50 +1017,3 @@ def test_write_root_state(num_cubes, device, state_location): # skip lin_vel because it differs from link frame, this should be fine because we are only checking # if velocity update is triggered, which can be determined by comparing angular velocity torch.testing.assert_close(root_com_vel_w[:, 3:], root_link_vel_w[:, 3:]) - - -def test_warmup_attach_stage_not_called_for_cpu(monkeypatch): - """Keep IsaacLab's explicit warmup on its GPU-only compatibility path. - - OVPhysX 0.5 exposes only ``warmup_gpu()``, while OVPhysX 0.6 also supports - CPU through ``warmup()``. IsaacLab preserves its existing explicit GPU-only - scheduling path for compatibility with both generations. OVPhysX 0.5 needs - no explicit CPU warmup; on 0.6, the first tensor read performs lazy warmup. - The manager therefore does not invoke the compatibility helper on CPU. - - After the first reset constructs the runtime, we replace - :meth:`OvPhysxManager._warmup_physx` with a :class:`MagicMock`, force the - manager to load the stage again, and assert the helper was not called. - Watching the compatibility helper keeps this regression test independent - of which warmup entry point the installed OVPhysX generation exposes. - - The test always runs CPU regardless of session parametrization, so it is - skipped when the session-locked device is anything other than CPU. The - skip is enforced inline (rather than in the autouse fixture) so the rest - of the suite can still pin to GPU when invoked together. - """ - if _LOCKED_DEVICE[0] not in (None, "cpu"): - pytest.skip( - f"ovphysx process-global device lock is held by '{_LOCKED_DEVICE[0]}'; cannot run " - "CPU-only regression test in the same session." - ) - _LOCKED_DEVICE[0] = "cpu" - - with _ovphysx_sim_context(device="cpu", add_ground_plane=True, dt=0.01, auto_add_lighting=True) as sim: - # Allocate a single rigid body so the manager has something to load. - generate_cubes_scene(num_cubes=1, height=1.0, device="cpu") - - # First reset constructs (or reuses) the real ovphysx.PhysX instance. - sim.reset() - assert OvPhysxManager.get_physx_instance() is not None, "PhysX should be constructed after sim.reset()" - - warmup_spy = MagicMock() - monkeypatch.setattr(OvPhysxManager, "_warmup_physx", warmup_spy) - # Force _warmup_and_load to run again on the next reset so the spy - # observes the explicit-warmup (or non-call) decision; close() resets - # _warmup_done back to False but we just called sim.reset() above. - OvPhysxManager._warmup_done = False - sim.reset() - - assert OvPhysxManager._warmup_done is True - warmup_spy.assert_not_called() diff --git a/source/isaaclab_ov/test/physics/test_ovphysx_scene_data_backend.py b/source/isaaclab_ov/test/physics/test_ovphysx_scene_data_backend.py index 56383bed1a53..7a1ddbe8c723 100644 --- a/source/isaaclab_ov/test/physics/test_ovphysx_scene_data_backend.py +++ b/source/isaaclab_ov/test/physics/test_ovphysx_scene_data_backend.py @@ -28,6 +28,7 @@ def _native_backend(monkeypatch): backend = OvPhysxBackend.__new__(OvPhysxBackend) backend.physx = None backend.stage = None + backend.rigid_body_view = None monkeypatch.setattr(OvPhysxManager, "backend", backend) monkeypatch.setattr(OvPhysxManager, "_scene_data_backend", OvPhysxSceneDataBackend()) monkeypatch.setattr(OvPhysxManager, "kinematics_dirty", False) @@ -73,16 +74,6 @@ def _serialize_full_stage_with_pending_clones(stage) -> str: OvPhysxManager._requires_full_stage = previous -def _fake_rigid_body_prim(path: str): - """Build a traversal stub with RigidBodyAPI and no deformable schemas.""" - return SimpleNamespace( - HasAPI=lambda api: True, - GetPath=lambda p=path: SimpleNamespace(pathString=p), - GetAppliedSchemas=lambda: [], - GetMetadata=lambda key: None, - ) - - def test_manager_full_stage_materializes_only_missing_heterogeneous_targets(): """A full-stage export copies missing heterogeneous targets without replacing authored ones.""" from isaaclab_ov.physics import OvPhysxManager @@ -395,6 +386,7 @@ def pinned_config(*, num_threads=None, cooked_collider_cache_dir=None, carbonite backend = module.OvPhysxBackend(OvPhysxBackendCfg(device=device, cooked_collider_cache_dir=cache_dir)) physx = backend.physx OvPhysxManager.backend.physx = physx + OvPhysxManager.backend.rigid_body_view = SimpleNamespace(destroy=lambda: physx.calls.append(("destroy_view",))) monkeypatch.setattr(OvPhysxManager, "get_physics_dt", lambda: 0.02) monkeypatch.setattr(PhysicsManager, "_sim_time", 0.0) version = OvPhysxManager._scene_data_backend.transforms_timestamp @@ -405,7 +397,9 @@ def pinned_config(*, num_threads=None, cooked_collider_cache_dir=None, carbonite assert physx.constructor["active_cuda_gpus"] == expected_active_cuda_gpus assert physx.constructor["config"].num_threads == 8 assert physx.constructor["config"].cooked_collider_cache_dir == cache_dir - assert physx.calls == [("step_sync", 0.02), ("update_articulations_kinematic",), ("reset_stage",), ("wait_op", 23)] + updates = [("step_sync", 0.02), ("update_articulations_kinematic",)] + assert physx.calls == updates + [("destroy_view",), ("reset_stage",), ("wait_op", 23)] + assert OvPhysxManager.backend.rigid_body_view is None assert PhysicsManager._sim_time == 0.02 assert OvPhysxManager._scene_data_backend.transforms_timestamp > version assert not OvPhysxManager.kinematics_dirty @@ -423,7 +417,8 @@ def test_transforms_finish_dirty_kinematics_before_native_reads(monkeypatch): backend = OvPhysxManager._scene_data_backend poses = wp.zeros(1, dtype=wp.transformf, device="cpu") backend._transforms.data.transforms = poses - backend._rigid_bindings = [(SimpleNamespace(read_into=lambda *args: calls.append("read")), poses)] + backend.backend = OvPhysxManager.backend + backend.backend.rigid_body_view = SimpleNamespace(read=lambda _: calls.append("read")) sdp = SceneDataProvider(backend) monkeypatch.setattr(OvPhysxManager, "kinematics_dirty", True) sdp.get_transforms(SceneDataFormat.Transform()) @@ -542,6 +537,7 @@ def release(self): physx = FakePhysX() OvPhysxManager.backend.physx = physx + OvPhysxManager.backend.rigid_body_view = SimpleNamespace(destroy=lambda: events.append(("destroy_view",))) monkeypatch.setattr( om_mod.OvPhysxView, "_close_all_for", @@ -559,12 +555,14 @@ def release(self): ("populate", stage, "#usda 1.0", 1, "all"), ("seal", 1), ("attach", stage, 1), + ("destroy_view",), ("close_views", physx), ("reset",), ("wait", 17), ("release",), ("destroy",), ] + assert OvPhysxManager.backend.rigid_body_view is None @pytest.mark.parametrize( @@ -787,11 +785,10 @@ def _stop_at_stage_creation(): assert SimulationContext.instance() is None -def test_transforms_read_native_slices_only_when_dirty(monkeypatch): - """Native bindings fill one shared pose buffer directly and skip clean publications.""" +def test_transforms_read_native_buffer_only_when_dirty(): + """One fused native binding fills the pose buffer directly and skips clean publications.""" import isaaclab_ov.physics.ovphysx_manager as module import numpy as np - import warp as wp from isaaclab.scene_data import SceneDataFormat, SceneDataProvider @@ -799,28 +796,15 @@ def test_transforms_read_native_slices_only_when_dirty(monkeypatch): paths = ["/World/envs/env_0/Cart", "/World/envs/env_1/Cart", "/World/envs/env_0/Pole"] reads = [] - class FakePhysX: - def create_tensor_binding(self, pattern, tensor_type): - start, end = (0, 2) if pattern.endswith("/Cart") else (2, 3) - - def read(dst): - reads.append((start, dst.ptr)) - wp.copy(dst, wp.array(expected[start:end], dtype=wp.float32, device="cpu")) + def read(dst): + reads.append(dst.ptr) + dst.assign(expected) - return SimpleNamespace( - shape=(end - start, 7), - count=end - start, - dtype=SimpleNamespace(code=2, bits=32, lanes=1), - prim_paths=paths[start:end], - read=read, - destroy=lambda: None, - ) + native = SimpleNamespace(physx=None, rigid_body_view=SimpleNamespace(count=len(paths), prim_paths=paths, read=read)) - monkeypatch.setattr(module, "UsdPhysics", SimpleNamespace(RigidBodyAPI=object())) - stage = SimpleNamespace(Traverse=lambda: (_fake_rigid_body_prim(path) for path in paths)) backend = module.OvPhysxSceneDataBackend() publication = backend.transforms - backend.setup(FakePhysX(), stage, "cpu") + backend.setup(native, "cpu") assert backend.transforms is publication sdp = SceneDataProvider(backend) @@ -828,38 +812,24 @@ def read(dst): assert sdp.get_transforms(native) assert backend.transform_count == len(paths) assert backend.transform_paths == paths - assert reads == [(0, native.transforms.ptr), (2, native.transforms.ptr + 2 * 7 * 4)] + assert reads == [native.transforms.ptr] np.testing.assert_array_equal(native.transforms.numpy(), expected) second_output = SceneDataFormat.Transform() assert sdp.get_transforms(second_output) assert second_output.transforms is native.transforms - assert len(reads) == 2 + assert len(reads) == 1 expected[:, 0] += 10 backend.transforms_timestamp += 1 assert sdp.get_transforms(second_output) assert second_output.transforms is native.transforms - assert len(reads) == 4 + assert len(reads) == 2 np.testing.assert_array_equal(native.transforms.numpy(), expected) - backend.setup(None, None, "cpu") + backend.setup(SimpleNamespace(rigid_body_view=None), "cpu") assert backend.transforms is publication assert publication.transforms is None - - -def test_setup_propagates_failed_rigid_binding(monkeypatch): - """A failed binding cannot silently remove a body from the publication.""" - import isaaclab_ov.physics.ovphysx_manager as module - - class FailingPhysX: - def create_tensor_binding(self, pattern, tensor_type): - raise RuntimeError("simulated binding failure") - - monkeypatch.setattr(module, "UsdPhysics", SimpleNamespace(RigidBodyAPI=object())) - stage = SimpleNamespace(Traverse=lambda: iter([_fake_rigid_body_prim("/World/Object")])) - backend = module.OvPhysxSceneDataBackend() - with pytest.raises(RuntimeError, match="simulated binding failure"): - backend.setup(FailingPhysX(), stage, "cpu") + assert backend.transform_count == 0 and backend.transform_paths == [] def test_failed_rigid_read_is_retried(): @@ -869,12 +839,12 @@ def test_failed_rigid_read_is_retried(): from isaaclab.scene_data import SceneDataFormat, SceneDataProvider - def fail_read(name, dst): + def fail_read(dst): raise RuntimeError("simulated read failure") backend = OvPhysxSceneDataBackend() backend._transforms.data.transforms = wp.empty(1, dtype=wp.transformf, device="cpu") - backend._rigid_bindings = [(SimpleNamespace(read_into=fail_read), backend._transforms.data.transforms)] + backend.backend = SimpleNamespace(rigid_body_view=SimpleNamespace(read=fail_read)) sdp = SceneDataProvider(backend) for _ in range(2): @@ -888,12 +858,12 @@ def test_geometry_publication_distinguishes_undeclared_and_empty_scenes(declared from isaaclab_ov.physics.ovphysx_manager import OvPhysxSceneDataBackend backend = OvPhysxSceneDataBackend() - stage = SimpleNamespace(Traverse=lambda: iter(())) - backend.setup(None, stage, "cpu", () if declared else None) + native = SimpleNamespace(physx=None, rigid_body_view=SimpleNamespace(count=0)) + backend.setup(native, "cpu", () if declared else None) if declared: assert backend.get_geometry_batches() == [] assert backend.native_geometry_formats == () - backend.setup(None, stage, "cpu") + backend.setup(native, "cpu") with pytest.raises(RuntimeError, match="ClonePlan"): backend.get_geometry_batches() with pytest.raises(RuntimeError, match="ClonePlan"): @@ -946,12 +916,12 @@ def read(dst): ) backend = module.OvPhysxSceneDataBackend() - stage = SimpleNamespace(Traverse=lambda: iter(())) + native = SimpleNamespace(physx=NativePhysX(), rigid_body_view=SimpleNamespace(count=0)) if node_padding: with pytest.raises(RuntimeError, match="node counts"): - backend.setup(NativePhysX(), stage, "cpu", entries) + backend.setup(native, "cpu", entries) return - backend.setup(NativePhysX(), stage, "cpu", entries) + backend.setup(native, "cpu", entries) assert {kind for _, kind in bindings} == {TT.SURFACE_DEFORMABLE_SIM_POSITION, TT.DEFORMABLE_SIM_NODAL_POSITION} provider = SceneDataProvider(backend) diff --git a/source/isaaclab_physx/changelog.d/deformable-prototype-bindings.rst b/source/isaaclab_physx/changelog.d/deformable-prototype-bindings.rst new file mode 100644 index 000000000000..4b4db4b4efb8 --- /dev/null +++ b/source/isaaclab_physx/changelog.d/deformable-prototype-bindings.rst @@ -0,0 +1,7 @@ +Fixed +^^^^^ + +* Published canonical articulation-link paths for SDP and moved foreign-physics Fabric prim + preparation into the shared rendering resource. +* Avoided reloading GPU PhysX scenes after explicit attachment while preserving CPU initialization + through the Kit bridge. diff --git a/source/isaaclab_physx/isaaclab_physx/physics/physx_manager.py b/source/isaaclab_physx/isaaclab_physx/physics/physx_manager.py index 670382060935..3f2584d18a33 100644 --- a/source/isaaclab_physx/isaaclab_physx/physics/physx_manager.py +++ b/source/isaaclab_physx/isaaclab_physx/physics/physx_manager.py @@ -32,7 +32,7 @@ import omni.timeline import omni.usd import usdrt -from pxr import Sdf, Usd, UsdPhysics, UsdUtils +from pxr import Sdf, UsdPhysics, UsdUtils import isaaclab.sim as sim_utils from isaaclab.physics import CallbackHandle, PhysicsEvent, PhysicsManager @@ -209,46 +209,7 @@ def clear(self) -> None: self._fabric_points_selection = None def get_rigid_body_view(self) -> omni.physics.tensors.RigidBodyView | None: - """Lazily create a rigid body view covering all rigid bodies in the scene. - - Discovers exact rigid body prims by traversing USD, then compacts cloned - environment paths into wildcard patterns. If a rigid body name is also - used by a non-rigid prim, the exact path is kept to avoid PhysX resolving - the wildcard to the non-rigid prim. - """ - if self._rigid_body_view is not None: - return self._rigid_body_view - - if self.backend is None: - return None - - stage: Usd.Stage = omni.usd.get_context().get_stage() - if stage is None: - return None - - rigid_body_paths: list[str] = [] - non_rigid_body_names: set[str] = set() - for prim in stage.Traverse(): - prim_path = prim.GetPath().pathString - if prim.HasAPI(UsdPhysics.RigidBodyAPI) and not prim.IsA(UsdPhysics.Joint): - rigid_body_paths.append(prim_path) - elif re.search(r"/World/envs/env_\d+/", prim_path): - non_rigid_body_names.add(prim_path.rsplit("/", 1)[-1]) - - patterns: set[str] = set() - exact_paths: list[str] = [] - for prim_path in rigid_body_paths: - body_name = prim_path.rsplit("/", 1)[-1] - if body_name in non_rigid_body_names: - exact_paths.append(prim_path) - else: - patterns.add(re.sub(r"/World/envs/env_\d+", "/World/envs/env_*", prim_path)) - - body_paths = [*sorted(patterns), *exact_paths] - if not body_paths: - return None - - self._rigid_body_view = self.backend.simulation_view.create_rigid_body_view(body_paths) + """Return the native body selection bound during initialization.""" return self._rigid_body_view def _setup_deformable_geometry(self, entries: Sequence[DeformableStageEntry]) -> None: @@ -270,9 +231,8 @@ def _setup_deformable_geometry(self, entries: Sequence[DeformableStageEntry]) -> counts = np.asarray([entry.vertex_count for entry in ordered], dtype=np.int32) if np.any(counts > view.max_simulation_nodes_per_body): raise RuntimeError("PhysX deformable node capacity is smaller than the clone plan requires.") - points = view.get_simulation_nodal_positions().view(wp.vec3f).flatten() native_offsets = np.arange(view.count) * view.max_simulation_nodes_per_body - batches = deformable_geometry_batches(ordered, points, native_offsets) + batches = deformable_geometry_batches(ordered, native_offsets, device=str(PhysicsManager._device)) bindings.append((view, batches)) self._deformable_bindings = bindings self._geometry = TimestampedBuffer([batch for _, batches in bindings for batch in batches]) @@ -983,16 +943,14 @@ def _warmup_and_create_views(cls) -> None: physx = omni.physx.get_physx_interface() physx_sim = omni.physx.get_physx_simulation_interface() - # Attach stage to PhysX BEFORE loading/starting - only needed for GPU pipeline. - # For CPU, the old SimulationManager never called attach_stage() explicitly. - # Calling attach_stage() + force_load_physics_from_usd() together causes a - # double-initialization that corrupts the CPU broadphase (MBP) collision setup, - # causing objects to fall through surfaces non-deterministically. + # Both APIs load physics. Force-loading after attachment destroys and rebuilds it. + # CPU uses the Kit bridge's implicit attachment to preserve MBP collision setup. if is_gpu: physx_sim.attach_stage(stage_id) + else: + physx.force_load_physics_from_usd() # warmup physx - physx.force_load_physics_from_usd() physx.start_simulation() physx.update_simulation(cls.get_physics_dt(), 0.0) physx_sim.fetch_results() @@ -1005,6 +963,22 @@ def _warmup_and_create_views(cls) -> None: # Register the complete tensor view only after PhysX has loaded the stage. cls.backend = sim.get_or_create_backend(PhysxBackendCfg(stage_id=stage_id)) cls._scene_data_backend.backend = cls.backend + view = cls.backend.simulation_view.create_rigid_body_view("/**") + # PhysX returns a wrapper with no native handle for an empty selection. + cls._scene_data_backend._rigid_body_view = view if view._backend is not None else None + if view._backend is not None: + # Wildcard bindings expose articulation aliases; publish the actual root-link paths. + native = cls.backend.simulation_view + paths = view.prim_paths + articulation_type = omni.physics.tensors.ObjectType.Articulation + roots = [path for path in paths if native.get_object_type(path) == articulation_type] + if roots: + articulations = native.create_articulation_view(roots) + links = (body_paths[0] for body_paths in articulations.link_paths) + aliases = dict(zip(articulations.prim_paths, links, strict=True)) + cls._scene_data_backend._rigid_body_view = native.create_rigid_body_view( + [aliases.get(path, path) for path in paths] + ) # Final update after view creation physx.update_simulation(cls.get_physics_dt(), 0.0) diff --git a/source/isaaclab_physx/isaaclab_physx/renderers/fabric.py b/source/isaaclab_physx/isaaclab_physx/renderers/fabric.py index c3b04372400b..bbe33ec153a6 100644 --- a/source/isaaclab_physx/isaaclab_physx/renderers/fabric.py +++ b/source/isaaclab_physx/isaaclab_physx/renderers/fabric.py @@ -65,11 +65,17 @@ def bind_transforms(self, provider: SceneDataProvider) -> None: stage = self.stage stage.SynchronizeToFabric() + for path in provider.backend.transform_paths: + prim = stage.GetPrimAtPath(path) + if prim: + usdrt.Rt.Xformable(prim).SetWorldXformFromUsd() + else: + prim = stage.DefinePrim(path, "Xform") + usdrt.Rt.Xformable(prim).CreateFabricHierarchyWorldMatrixAttr() + prim.AddAppliedSchema("PhysicsRigidBodyAPI") self.hierarchy.update_world_xforms() for index, path in enumerate(provider.backend.transform_paths): prim = stage.GetPrimAtPath(path) - if not prim or not prim.HasAPI("PhysicsRigidBodyAPI"): - continue prim.CreateAttribute("isaaclab:transformIndex", usdrt.Sdf.ValueTypeNames.Int, custom=True).Set(index) # Physics publishes absolute body poses; only visual descendants inherit them. self.hierarchy.set_reset_xform_stack(prim.GetPath().fabricPath, True) diff --git a/source/isaaclab_physx/test/assets/test_rigid_object.py b/source/isaaclab_physx/test/assets/test_rigid_object.py index 2322f6f59c27..8d3388edaaee 100644 --- a/source/isaaclab_physx/test/assets/test_rigid_object.py +++ b/source/isaaclab_physx/test/assets/test_rigid_object.py @@ -799,35 +799,19 @@ def test_write_root_state(num_cubes, device, with_offset, state_location): @pytest.mark.isaacsim_ci -def test_warmup_attach_stage_not_called_for_cpu(): - """Regression test: CPU warmup must force-load without explicitly attaching the stage. - - Bug (commit 0ba9c5cb3b): ``PhysxManager._warmup_and_create_views()`` called - ``_physx_sim.attach_stage()`` unconditionally before ``force_load_physics_from_usd()``. - These are two alternative initialization patterns; combining them causes - double-initialization that corrupts the CPU MBP broadphase, producing - non-deterministic collision failures (objects passing through surfaces). - - The CPU pipeline attaches implicitly via ``force_load_physics_from_usd()`` when - the ``omni.physics.physx`` bridge registers the backend. - - This test verifies that the PhysX backend is registered with the unified physics - API, ``attach_stage`` is not called, and ``force_load_physics_from_usd`` is called - exactly once during CPU warmup. - """ +@pytest.mark.parametrize("device", test_devices()) +def test_warmup_loads_physics_once(device): + """Attach on GPU or force-load on CPU, without destroying and rebuilding native objects.""" from unittest.mock import MagicMock, patch import omni.kit.app import omni.physx - with build_simulation_context(device="cpu", add_ground_plane=True, dt=0.01, auto_add_lighting=True) as sim: + with build_simulation_context(device=device, add_ground_plane=True, dt=0.01, auto_add_lighting=True) as sim: sim._app_control_on_stop_handle = None - generate_cubes_scene(num_cubes=1, height=1.0, device="cpu") + cube, _ = generate_cubes_scene(num_cubes=1, height=1.0, device=device) - # PhysxManager no longer caches the simulation interface; it resolves it on each use - # via ``omni.physx.get_physx_simulation_interface()`` (the accessor memoizes it). - # The PhysX interfaces are C++ bindings whose attributes are read-only. Patch - # their accessors with wrapping mocks so the real calls still execute. + # Wrap read-only native interfaces through their accessors; real calls still execute. physx_spy = MagicMock(wraps=omni.physx.get_physx_interface()) physx_sim_spy = MagicMock(wraps=omni.physx.get_physx_simulation_interface()) with ( @@ -840,8 +824,11 @@ def test_warmup_attach_stage_not_called_for_cpu(): assert extension_manager.is_extension_enabled("omni.physics.physx"), ( "The omni.physics.physx bridge must register PhysX with the unified physics API." ) - assert physx_sim_spy.attach_stage.call_count == 0, ( - f"attach_stage() was called {physx_sim_spy.attach_stage.call_count} time(s) during CPU warmup. " - "This indicates the CPU MBP broadphase double-initialization regression is present." - ) - physx_spy.force_load_physics_from_usd.assert_called_once_with() + assert physx_sim_spy.attach_stage.call_count == int(device.startswith("cuda")) + assert physx_spy.force_load_physics_from_usd.call_count == int(device == "cpu") + assert cube.is_initialized and cube.num_instances == 1 + initial = cube.data.root_pos_w.torch.clone() + for _ in range(10): + sim.step(render=False) + cube.update(sim.get_physics_dt()) + assert torch.all(cube.data.root_pos_w.torch[:, 2] < initial[:, 2]) diff --git a/source/isaaclab_physx/test/sim/test_physx_scene_data_backend.py b/source/isaaclab_physx/test/sim/test_physx_scene_data_backend.py index 6816a67bf845..aa7d555bdd79 100644 --- a/source/isaaclab_physx/test/sim/test_physx_scene_data_backend.py +++ b/source/isaaclab_physx/test/sim/test_physx_scene_data_backend.py @@ -133,46 +133,6 @@ def test_pose_publication_refreshes_after_physics_but_reuses_clean_reads(monkeyp assert backend.transforms_timestamp > version -@pytest.mark.parametrize("joint_has_rigid_body_api", [False, True]) -def test_rigid_body_view_uses_exact_path_for_joint_name_collision(monkeypatch, joint_has_rigid_body_api): - """Joint names must keep same-named rigid bodies out of wildcard views.""" - from isaaclab_physx.physics import physx_manager - from isaaclab_physx.physics.physx_manager import PhysxSceneDataBackend - - from pxr import Usd, UsdGeom, UsdPhysics - - stage = Usd.Stage.CreateInMemory() - body_prim = UsdGeom.Xform.Define(stage, "/World/envs/env_0/Robot/robot0_forearm").GetPrim() - UsdPhysics.RigidBodyAPI.Apply(body_prim) - unique_body_prim = UsdGeom.Xform.Define(stage, "/World/envs/env_0/Robot/torso").GetPrim() - UsdPhysics.RigidBodyAPI.Apply(unique_body_prim) - joint_prim = UsdPhysics.FixedJoint.Define(stage, "/World/envs/env_0/Robot/joints/robot0_forearm").GetPrim() - if joint_has_rigid_body_api: - UsdPhysics.RigidBodyAPI.Apply(joint_prim) - - captured_paths = [] - - class _SimulationView: - def create_rigid_body_view(self, body_paths): - captured_paths.extend(body_paths) - return SimpleNamespace(prim_paths=body_paths) - - monkeypatch.setattr( - physx_manager.omni.usd, - "get_context", - lambda: SimpleNamespace(get_stage=lambda: stage), - ) - - backend = PhysxSceneDataBackend() - backend.backend = SimpleNamespace(simulation_view=_SimulationView()) - backend.get_rigid_body_view() - - assert captured_paths == [ - "/World/envs/env_*/Robot/torso", - "/World/envs/env_0/Robot/robot0_forearm", - ] - - @pytest.mark.parametrize("declared", [False, True]) def test_geometry_publication_distinguishes_undeclared_and_empty_scenes(monkeypatch, declared): """Only an explicitly initialized, empty geometry declaration publishes no batches."""