From e7d3aa3bf2b3670a83b764083c13f02f7ea16508 Mon Sep 17 00:00:00 2001 From: Maximilian Krause Date: Tue, 6 Oct 2026 14:53:40 +0200 Subject: [PATCH 1/9] Stage conveyor adapters in Contrib --- .../maximiliank-conveyor-support.added.rst | 1 + .../maximiliank-conveyor-support.minor | 0 .../isaaclab_contrib/conveyors/README.md | 21 + .../isaaclab_contrib/conveyors/__init__.py | 10 + .../isaaclab_contrib/conveyors/newton.py | 804 ++++++++++++++++++ .../isaaclab_contrib/conveyors/physx.py | 532 ++++++++++++ .../conveyors/surface_velocity.py | 179 ++++ .../conveyors/test_surface_velocity_newton.py | 166 ++++ .../conveyors/test_surface_velocity_physx.py | 147 ++++ .../conveyors/test_surface_velocity_spec.py | 45 + .../maximiliank-conveyor-lifecycle.added.rst | 2 + .../maximiliank-conveyor-lifecycle.minor | 0 .../physics/newton_collision_cfg.py | 7 + .../isaaclab_newton/physics/newton_manager.py | 111 +++ 14 files changed, 2025 insertions(+) create mode 100644 source/isaaclab_contrib/changelog.d/maximiliank-conveyor-support.added.rst create mode 100644 source/isaaclab_contrib/changelog.d/maximiliank-conveyor-support.minor create mode 100644 source/isaaclab_contrib/isaaclab_contrib/conveyors/README.md create mode 100644 source/isaaclab_contrib/isaaclab_contrib/conveyors/__init__.py create mode 100644 source/isaaclab_contrib/isaaclab_contrib/conveyors/newton.py create mode 100644 source/isaaclab_contrib/isaaclab_contrib/conveyors/physx.py create mode 100644 source/isaaclab_contrib/isaaclab_contrib/conveyors/surface_velocity.py create mode 100644 source/isaaclab_contrib/test/conveyors/test_surface_velocity_newton.py create mode 100644 source/isaaclab_contrib/test/conveyors/test_surface_velocity_physx.py create mode 100644 source/isaaclab_contrib/test/conveyors/test_surface_velocity_spec.py create mode 100644 source/isaaclab_newton/changelog.d/maximiliank-conveyor-lifecycle.added.rst create mode 100644 source/isaaclab_newton/changelog.d/maximiliank-conveyor-lifecycle.minor diff --git a/source/isaaclab_contrib/changelog.d/maximiliank-conveyor-support.added.rst b/source/isaaclab_contrib/changelog.d/maximiliank-conveyor-support.added.rst new file mode 100644 index 000000000000..3dc15f179a03 --- /dev/null +++ b/source/isaaclab_contrib/changelog.d/maximiliank-conveyor-support.added.rst @@ -0,0 +1 @@ +* Added experimental straight and curved conveyor controls with Newton force-model and native CPU PhysX adapters. diff --git a/source/isaaclab_contrib/changelog.d/maximiliank-conveyor-support.minor b/source/isaaclab_contrib/changelog.d/maximiliank-conveyor-support.minor new file mode 100644 index 000000000000..e69de29bb2d1 diff --git a/source/isaaclab_contrib/isaaclab_contrib/conveyors/README.md b/source/isaaclab_contrib/isaaclab_contrib/conveyors/README.md new file mode 100644 index 000000000000..751755a2487e --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/conveyors/README.md @@ -0,0 +1,21 @@ +# Experimental conveyor support + +These adapters stage conveyor integration in `isaaclab_contrib`; their API is +experimental and is not exported from Isaac Lab core or the backend packages. + +- `surface_velocity.SurfaceVelocitySpec` describes a straight or curved surface + in the collision prim's local frame. `SurfaceVelocityView` describes the shared + control operations. +- `newton.SurfaceVelocity` uses Newton's upstream `ConveyorForceModel`. The + adapter owns replicated surface/body bindings and solver lifecycle integration, + including hard resets and registration before CUDA graph capture. It requires + the Newton backend. +- `physx.SurfaceVelocity` uses native `PhysxSurfaceVelocityAPI` on CPU. + Author the API before simulation parsing with `apply_surface_velocity_api`. + This path requires the PhysX backend and Kit; GPU dynamics are unsupported. + +Both adapters retain commanded speeds while disabled and clear selected worlds' +runtime state on reset. The Newton adapter supports live traction controls; +PhysX uses authored contact materials and rejects unsupported traction setters. + +No meshes, assets, task registrations, or policy checkpoints are bundled here. diff --git a/source/isaaclab_contrib/isaaclab_contrib/conveyors/__init__.py b/source/isaaclab_contrib/isaaclab_contrib/conveyors/__init__.py new file mode 100644 index 000000000000..6dddd7297fff --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/conveyors/__init__.py @@ -0,0 +1,10 @@ +# 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 + +"""Experimental conveyor support for contributed tasks. + +Backend adapters are imported explicitly from :mod:`.newton` or :mod:`.physx` so +importing the surface descriptions does not load either optional backend. +""" diff --git a/source/isaaclab_contrib/isaaclab_contrib/conveyors/newton.py b/source/isaaclab_contrib/isaaclab_contrib/conveyors/newton.py new file mode 100644 index 000000000000..52e4c5b8fcfa --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/conveyors/newton.py @@ -0,0 +1,804 @@ +# 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 + +"""Batched surface controls and lifecycle integration for Newton's conveyor force model. + +Newton owns contact classification and traction computation. This adapter resolves replicated +USD surfaces, selects transported bodies, and preserves controls across solver rebuilds. +""" + +from __future__ import annotations + +import re +from collections.abc import Sequence +from typing import Any + +import numpy as np +import warp as wp +from isaaclab_newton.physics import NewtonManager +from newton.examples.basic.example_basic_conveyor_forces import ConveyorForceModel +from newton.examples.basic.example_basic_conveyor_forces import ( + Vec3Pair as Vec3Pair, +) +from newton.examples.basic.example_basic_conveyor_forces import ( + compute_basis_vectors as compute_basis_vectors, +) +from newton.examples.basic.example_basic_conveyor_forces import ( + compute_point_force as compute_point_force, +) +from newton.examples.basic.example_basic_conveyor_forces import ( + compute_point_impulse as compute_point_impulse, +) + +from isaaclab.physics import PhysicsEvent + +from .surface_velocity import SurfaceVelocitySpec + + +@wp.kernel +def _update_belt_velocities( + direction: wp.array[wp.vec3], + radius: wp.array[wp.float32], + effective_velocity: wp.array[wp.float32], + linear_velocity: wp.array[wp.vec3], + angular_velocity: wp.array[wp.vec3], +): + conveyor_id = wp.tid() + linear_velocity[conveyor_id] = direction[conveyor_id] * effective_velocity[conveyor_id] + angular_velocity[conveyor_id] = direction[conveyor_id] * (effective_velocity[conveyor_id] / radius[conveyor_id]) + + +@wp.kernel +def _filter_body_forces( + body_is_tracked: wp.array[wp.int32], + body_force: wp.array[wp.spatial_vector], +): + body_id = wp.tid() + if body_is_tracked[body_id] == 0: + body_force[body_id] = wp.spatial_vector() + + +@wp.kernel +def _advance_startup_scale( + dt: wp.float32, duration: wp.float32, elapsed: wp.array[wp.float32], scale: wp.array[wp.float32] +): + elapsed[0] += dt + scale[0] = wp.min(1.0, elapsed[0] / duration) + + +@wp.kernel +def _integrate_encoders( + dt: wp.float32, + effective_velocity: wp.array[wp.float32], + position: wp.array[wp.float32], +): + conveyor_id = wp.tid() + position[conveyor_id] += dt * effective_velocity[conveyor_id] + + +@wp.kernel +def _update_effective_velocities( + commanded_velocity: wp.array[wp.float32], + enabled: wp.array[wp.int32], + effective_velocity: wp.array[wp.float32], +): + conveyor_id = wp.tid() + effective_velocity[conveyor_id] = commanded_velocity[conveyor_id] * wp.float32(enabled[conveyor_id]) + + +@wp.kernel +def _gather_float_values( + source: wp.array[wp.float32], + indices: wp.array[wp.int32], + values: wp.array[wp.float32], +): + output_id = wp.tid() + values[output_id] = source[indices[output_id]] + + +@wp.kernel +def _gather_int_values( + source: wp.array[wp.int32], + indices: wp.array[wp.int32], + values: wp.array[wp.int32], +): + output_id = wp.tid() + values[output_id] = source[indices[output_id]] + + +@wp.kernel +def _clear_selected_body_forces( + body_world: wp.array[wp.int32], + world_mask: wp.array[wp.bool], + body_force: wp.array[wp.spatial_vector], +): + body_id = wp.tid() + world_id = body_world[body_id] + if world_id >= 0 and world_mask[world_id]: + body_force[body_id] = wp.spatial_vector() + + +@wp.kernel +def _clear_selected_encoders( + conveyor_world: wp.array[wp.int32], + world_mask: wp.array[wp.bool], + encoder_position: wp.array[wp.float32], +): + conveyor_id = wp.tid() + world_id = conveyor_world[conveyor_id] + if world_mask[world_id]: + encoder_position[conveyor_id] = 0.0 + + +def _require_buffer_length(name: str, buffer: Any, expected: int) -> None: + """Reject missing or mis-sized buffers before a Warp launch can access them.""" + actual = len(buffer) if buffer is not None else 0 + if actual != expected: + raise RuntimeError(f"Conveyor force buffer {name!r} has length {actual}, expected {expected}.") + + +def _as_numpy(values: Any) -> np.ndarray: + """Convert supported tensor-like values to a host NumPy array.""" + if hasattr(values, "detach"): + return values.detach().cpu().numpy() + if hasattr(values, "numpy") and not isinstance(values, np.ndarray): + return values.numpy() + return np.asarray(values) + + +def _world_vector(transform_values: np.ndarray, local_vector: tuple[float, float, float]) -> wp.vec3: + """Rotate a local vector into world space and normalize it.""" + transform = wp.transform( + wp.vec3(*(float(value) for value in transform_values[:3])), + wp.quat(*(float(value) for value in transform_values[3:])), + ) + rotated = wp.transform_vector(transform, wp.vec3(*local_vector)) + values = np.asarray([float(rotated[index]) for index in range(3)], dtype=np.float32) + norm = float(np.linalg.norm(values)) + if norm <= 1.0e-8: + raise ValueError(f"Conveyor direction or surface normal must be non-zero, got {local_vector}.") + values /= norm + return wp.vec3(*(float(value) for value in values)) + + +def _world_point(transform_values: np.ndarray, local_point: tuple[float, float, float]) -> wp.vec3: + """Transform a local point into world space.""" + transform = wp.transform( + wp.vec3(*(float(value) for value in transform_values[:3])), + wp.quat(*(float(value) for value in transform_values[3:])), + ) + point = wp.transform_point(transform, wp.vec3(*local_point)) + return wp.vec3(*(float(point[index]) for index in range(3))) + + +def _resolve_belt_prim_path(prim_path: str, env_path_format: str, world_id: int) -> str: + """Resolve one replicated conveyor template to an exact Newton shape label.""" + if prim_path.startswith("{ENV_REGEX_NS}/"): + return prim_path.format(ENV_REGEX_NS=env_path_format.format(world_id)) + return prim_path + + +def _shape_belongs_to_prim(shape_label: str, prim_path: str) -> bool: + """Return whether a Newton collision-shape label is the prim or one of its descendants.""" + return shape_label == prim_path or shape_label.startswith(f"{prim_path.rstrip('/')}/") + + +def _validate_env_path_format(env_path_format: str) -> None: + """Validate the concrete per-world path format derived from the scene cloner.""" + if not isinstance(env_path_format, str) or env_path_format.count("{}") != 1: + raise ValueError(f"Conveyor env_path_format must contain exactly one '{{}}', got {env_path_format!r}.") + if not env_path_format.startswith("/"): + raise ValueError(f"Conveyor env_path_format must be absolute, got {env_path_format!r}.") + + +def _validate_newton_surface_specs(surface_specs: Sequence[SurfaceVelocitySpec]) -> None: + """Validate Newton-specific requirements before registering lifecycle callbacks.""" + if not surface_specs: + raise ValueError("At least one surface-velocity specification is required.") + prim_paths = [spec.prim_path for spec in surface_specs] + if len(set(prim_paths)) != len(prim_paths): + raise ValueError(f"Conveyor prim paths must be unique, got {prim_paths}.") + for index, path in enumerate(prim_paths): + for other in prim_paths[index + 1 :]: + if _shape_belongs_to_prim(path, other) or _shape_belongs_to_prim(other, path): + raise ValueError(f"Conveyor prim paths must not be ancestors of one another: {path!r}, {other!r}.") + for spec in surface_specs: + if spec.curved and spec.radius is None: + raise ValueError(f"Newton requires an explicit positive radius for curved belt {spec.prim_path!r}.") + + +class SurfaceVelocity: + """Adapt Newton's MuJoCo conveyor force model to the Isaac Lab lifecycle. + + The driver is created after the simulation context but before its first + reset. It requests solved contact forces before model finalization, then + binds model-specific buffers after solver initialization and before CUDA + graph capture. A hard simulation reset transparently replaces that binding + with buffers for the re-finalized model. Resolved belt indices use deterministic + environment-major ordering across those rebuilds. + """ + + def __init__( + self, + num_envs: int, + surface_specs: Sequence[SurfaceVelocitySpec], + *, + body_pattern: str, + body_count_per_env: int | None = None, + startup_duration_s: float = 1.0, + env_path_format: str = "/World/envs/env_{}", + ) -> None: + """Register the force pipeline for the next Newton model initialization. + + Args: + num_envs: Number of replicated simulation environments. + surface_specs: Authored surface descriptions in stable within-environment order. + body_pattern: Regular expression selecting bodies that receive surface traction. + body_count_per_env: Expected selected body count per environment, or ``None``. + startup_duration_s: Duration of the initial traction ramp [s]. + env_path_format: Format string resolving one exact environment root from its integer world index. + """ + surface_specs = tuple(surface_specs) + if not all(isinstance(spec, SurfaceVelocitySpec) for spec in surface_specs): + raise TypeError("Every surface specification must be a SurfaceVelocitySpec.") + _validate_newton_surface_specs(surface_specs) + if not isinstance(num_envs, int) or isinstance(num_envs, bool) or num_envs <= 0: + raise ValueError(f"num_envs must be a positive integer, got {num_envs!r}.") + if num_envs > 1 and any(not spec.prim_path.startswith("{ENV_REGEX_NS}/") for spec in surface_specs): + raise ValueError( + "Replicated conveyor environments require every belt prim_path to start with '{ENV_REGEX_NS}/'." + ) + if not np.isfinite(startup_duration_s) or startup_duration_s <= 0.0: + raise ValueError(f"Conveyor startup duration must be positive, got {startup_duration_s}.") + _validate_env_path_format(env_path_format) + if not isinstance(body_pattern, str): + raise ValueError(f"body_pattern must be a regular-expression string, got {body_pattern!r}.") + try: + re.compile(body_pattern) + except re.error as exc: + raise ValueError(f"Invalid body pattern: {body_pattern!r}.") from exc + if body_count_per_env is not None and ( + not isinstance(body_count_per_env, int) or isinstance(body_count_per_env, bool) or body_count_per_env < 0 + ): + raise ValueError("body_count_per_env must be a non-negative integer or None.") + self._binding: _SurfaceVelocityBinding | None = None + self._closed = False + self._num_envs = num_envs + self._surface_specs = surface_specs + self._binding_kwargs = { + "num_envs": num_envs, + "surface_specs": self._surface_specs, + "startup_duration_s": startup_duration_s, + "env_path_format": env_path_format, + "body_pattern": body_pattern, + "body_count_per_env": body_count_per_env, + } + self._model_init_handle = NewtonManager.register_callback( + self._request_contact_forces, + PhysicsEvent.MODEL_INIT, + name="surface_velocity_contact_attribute", + ) + try: + NewtonManager.register_solver_init_callback(self._bind_solver) + except Exception: + self._model_init_handle.deregister() + raise + + def _require_binding(self) -> _SurfaceVelocityBinding: + """Return the current binding or fail before solver initialization.""" + binding = self._binding + if binding is None: + raise RuntimeError("Surface velocity is not bound to an initialized Newton solver.") + return binding + + @property + def specs(self) -> tuple[SurfaceVelocitySpec, ...]: + """Authored surface descriptions in stable within-environment order.""" + return self._surface_specs + + @property + def surfaces_per_env(self) -> int: + """Number of authored surfaces in each replicated environment.""" + return len(self._surface_specs) + + @property + def num_surfaces(self) -> int: + """Total number of resolved surfaces across all environments.""" + return self._num_envs * self.surfaces_per_env + + @property + def count(self) -> int: + """Alias for :attr:`num_surfaces`, matching tensor-view naming.""" + return self.num_surfaces + + @property + def initialized(self) -> bool: + """Whether the driver is bound to the active Newton solver.""" + return self._binding is not None + + @property + def prim_paths(self) -> tuple[str, ...]: + """Resolved Newton shape labels in environment-major belt order.""" + return self._require_binding().surface_paths + + @property + def surface_paths(self) -> tuple[str, ...]: + """Alias for :attr:`prim_paths`.""" + return self.prim_paths + + def set_velocities(self, velocities: Any, indices: Any = None) -> None: + """Set signed surface speeds, preserving commands while surfaces are disabled.""" + self._require_binding().set_velocities(velocities, indices) + + def get_velocities(self, indices: Any = None, clone: bool = True) -> wp.array: + """Return effective surface speeds, with disabled surfaces reported as zero.""" + return self._require_binding().get_velocities(indices, clone) + + def get_commanded_velocities(self, indices: Any = None, clone: bool = True) -> wp.array: + """Return staged surface speeds without applying the enabled mask.""" + return self._require_binding().get_commanded_velocities(indices, clone) + + def set_enabled(self, flags: Any, indices: Any = None) -> None: + """Enable or disable selected surfaces without discarding their speed commands.""" + self._require_binding().set_enabled(flags, indices) + + def get_enabled(self, indices: Any = None, clone: bool = True) -> wp.array: + """Return integer enabled flags for selected surfaces.""" + return self._require_binding().get_enabled(indices, clone) + + def get_encoder_positions(self, indices: Any = None, clone: bool = True) -> wp.array: + """Return physics-rate integrated surface travel distances [m].""" + return self._require_binding().get_encoder_positions(indices, clone) + + def reset(self, env_ids: Any = None) -> None: + """Clear stale force and encoder state for selected environments.""" + self._require_binding().reset(env_ids) + + def _request_contact_forces(self, _event: Any) -> None: + """Request solved per-contact forces before the Newton model is finalized.""" + NewtonManager.request_extended_contact_attribute("force") + + def _bind_solver(self, model: Any, contacts: Any) -> None: + """Bind graph callbacks and buffers to the newly initialized solver.""" + previous = self._binding + settings = None + if previous is not None: + settings = ( + previous._command_velocity_host.copy(), + previous._enabled_host.copy(), + ) + previous.close() + self._binding = None + + binding = _SurfaceVelocityBinding(model=model, contacts=contacts, **self._binding_kwargs) + if settings is not None: + velocities, enabled = settings + binding.set_velocities(velocities) + binding.set_enabled(enabled) + self._binding = binding + + def close(self) -> None: + """Deregister lifecycle and graph callbacks; repeated calls are safe.""" + if self._closed: + return + NewtonManager.unregister_solver_init_callback(self._bind_solver) + self._model_init_handle.deregister() + if self._binding is not None: + self._binding.close() + self._binding = None + self._closed = True + + +class _SurfaceVelocityBinding: + """Bind replicated surfaces and controls to Newton's conveyor force model.""" + + def __init__( + self, + model: Any, + contacts: Any, + num_envs: int, + surface_specs: Sequence[SurfaceVelocitySpec], + *, + body_pattern: str, + body_count_per_env: int | None = None, + startup_duration_s: float = 1.0, + env_path_format: str = "/World/envs/env_{}", + ) -> None: + """Initialize the binding before Newton CUDA graph capture. + + Args: + model: Finalized Newton model owned by the active solver. + contacts: Contact buffer owned by the active solver. + num_envs: Number of replicated simulation environments. + surface_specs: Authored surface descriptions in stable within-environment order. + body_pattern: Regular expression selecting bodies that receive surface traction. + body_count_per_env: Expected selected body count per environment, or ``None``. + startup_duration_s: Duration of the initial traction ramp [s]. + env_path_format: Format string resolving one exact environment root from its integer world index. + """ + if not isinstance(num_envs, int) or isinstance(num_envs, bool) or num_envs <= 0: + raise ValueError(f"num_envs must be a positive integer, got {num_envs!r}.") + if not np.isfinite(startup_duration_s) or startup_duration_s <= 0.0: + raise ValueError(f"Conveyor startup duration must be positive, got {startup_duration_s}.") + _validate_env_path_format(env_path_format) + + self._surface_specs = tuple(surface_specs) + _validate_newton_surface_specs(self._surface_specs) + try: + compiled_body_pattern = re.compile(body_pattern) + except re.error as exc: + raise ValueError(f"Invalid body pattern: {body_pattern!r}.") from exc + + if model is None or contacts is None: + raise RuntimeError("The conveyor driver requires an initialized Newton model and contact buffer.") + if contacts.force is None: + raise RuntimeError( + "Newton did not allocate per-contact force reporting. The conveyor driver must request the " + "'force' contact attribute before model finalization." + ) + if model.world_count != num_envs: + raise RuntimeError(f"Newton model has {model.world_count} worlds, expected {num_envs}.") + + self._model = model + self._contacts = contacts + self._device = model.device + self._num_envs = num_envs + self._startup_duration_s = startup_duration_s + self._closed = False + self._validate_backend_buffers() + + surfaces_per_env = len(self._surface_specs) + conveyor_count = num_envs * surfaces_per_env + conveyor_shapes = [-1] * conveyor_count + direction = [wp.vec3() for _ in range(conveyor_count)] + pivot_point = [wp.vec3() for _ in range(conveyor_count)] + radius = [1.0] * conveyor_count + surface_normal = [wp.vec3() for _ in range(conveyor_count)] + conveyor_world = [conveyor_id // surfaces_per_env for conveyor_id in range(conveyor_count)] + surface_paths = [""] * conveyor_count + + shape_body = model.shape_body.numpy() + shape_world = model.shape_world.numpy() + shape_transform = model.shape_transform.numpy() + seen_sections: set[tuple[int, int]] = set() + for shape_id, label in enumerate(model.shape_label): + world_id = int(shape_world[shape_id]) + if not 0 <= world_id < num_envs: + continue + matching_specs = [ + index + for index, spec in enumerate(self._surface_specs) + if _shape_belongs_to_prim(label, _resolve_belt_prim_path(spec.prim_path, env_path_format, world_id)) + ] + if not matching_specs: + continue + if len(matching_specs) > 1: + raise RuntimeError(f"Conveyor shape {label!r} matches more than one section specification.") + if int(shape_body[shape_id]) >= 0: + raise ValueError(f"Conveyor shape must be static: {label}") + + spec_id = matching_specs[0] + section_key = (world_id, spec_id) + if section_key in seen_sections: + raise RuntimeError( + f"World {world_id} contains multiple shapes matching conveyor section " + f"{self._surface_specs[spec_id].prim_path!r}." + ) + seen_sections.add(section_key) + + spec = self._surface_specs[spec_id] + conveyor_id = world_id * surfaces_per_env + spec_id + conveyor_shapes[conveyor_id] = shape_id + direction[conveyor_id] = _world_vector(shape_transform[shape_id], spec.direction) + pivot_point[conveyor_id] = _world_point(shape_transform[shape_id], spec.pivot_point) + radius[conveyor_id] = 1.0 if spec.radius is None else spec.radius + surface_normal[conveyor_id] = _world_vector(shape_transform[shape_id], spec.surface_normal) + surface_paths[conveyor_id] = label + + expected_sections = { + (world_id, spec_id) for world_id in range(num_envs) for spec_id in range(len(self._surface_specs)) + } + missing_sections = sorted(expected_sections - seen_sections) + if missing_sections: + details = ", ".join( + f"world {world_id}: {self._surface_specs[spec_id].prim_path}" + for world_id, spec_id in missing_sections[:8] + ) + raise RuntimeError(f"Missing {len(missing_sections)} conveyor collision sections ({details}).") + + body_is_tracked = np.zeros(model.body_count, dtype=np.int32) + tracked_counts = np.zeros(num_envs, dtype=np.int32) + body_world = model.body_world.numpy() + for body_id, label in enumerate(model.body_label): + if compiled_body_pattern.search(label) is None: + continue + world_id = int(body_world[body_id]) + if not 0 <= world_id < num_envs: + raise RuntimeError(f"Transported body {label!r} belongs to invalid world {world_id}.") + body_is_tracked[body_id] = 1 + tracked_counts[world_id] += 1 + + if body_count_per_env is not None: + bad_worlds = np.flatnonzero(tracked_counts != body_count_per_env) + if bad_worlds.size: + details = ", ".join(f"world {world_id}: {tracked_counts[world_id]}" for world_id in bad_worlds[:8]) + raise RuntimeError( + f"Body pattern {body_pattern!r} expected {body_count_per_env} bodies per world ({details})." + ) + if not np.any(body_is_tracked): + raise RuntimeError(f"Body pattern {body_pattern!r} matched no Newton bodies.") + + self._conveyor = ConveyorForceModel(model, solver_type="mujoco") + for conveyor_id, shape_id in enumerate(conveyor_shapes): + spec = self._surface_specs[conveyor_id % surfaces_per_env] + parameters = { + "surface_normal": surface_normal[conveyor_id], + "friction": spec.friction_coefficient, + "threshold": spec.contact_threshold, + } + if spec.curved: + self._conveyor.add_pivot_belt(shape_id, pivot_point[conveyor_id], wp.vec3(), **parameters) + else: + self._conveyor.add_constant_belt(shape_id, wp.vec3(), **parameters) + self._conveyor.finalize(contacts) + + self._surface_paths = tuple(surface_paths) + self._body_is_tracked = wp.array(body_is_tracked, dtype=wp.int32, device=self._device) + self._direction = wp.array(direction, dtype=wp.vec3, device=self._device) + self._radius = wp.array(radius, dtype=wp.float32, device=self._device) + self._conveyor_world = wp.array(conveyor_world, dtype=wp.int32, device=self._device) + + authored_velocity = np.asarray([spec.velocity for spec in self._surface_specs], dtype=np.float32) + authored_enabled = np.asarray([spec.enabled for spec in self._surface_specs], dtype=np.int32) + self._command_velocity_host = np.tile(authored_velocity, num_envs) + self._enabled_host = np.tile(authored_enabled, num_envs) + self._command_velocity = wp.array(self._command_velocity_host, dtype=wp.float32, device=self._device) + self._enabled = wp.array(self._enabled_host, dtype=wp.int32, device=self._device) + self._effective_velocity = wp.zeros(conveyor_count, dtype=wp.float32, device=self._device) + self._encoder_position = wp.zeros(conveyor_count, dtype=wp.float32, device=self._device) + self._elapsed_time = wp.zeros(1, dtype=wp.float32, device=self._device) + self._conveyor.set_speed_scale(0.0) + + self._world_mask_host = np.zeros(num_envs, dtype=np.bool_) + self._world_mask = wp.zeros(num_envs, dtype=wp.bool, device=self._device) + self._refresh_effective_velocities() + + NewtonManager.register_state_force_callback(self.apply) + NewtonManager.register_post_solver_substep_callback(self.update) + + @property + def surface_paths(self) -> tuple[str, ...]: + """Resolved Newton shape labels in conveyor-index order.""" + return self._surface_paths + + def set_velocities(self, velocities: Any, indices: Any = None) -> None: + """Set signed surface speeds, preserving commands while surfaces are disabled.""" + selected = self._resolve_indices(indices) + self._command_velocity_host[selected] = self._broadcast_1d(velocities, len(selected), "velocities") + self._command_velocity.assign(self._command_velocity_host) + self._refresh_effective_velocities() + + def get_velocities(self, indices: Any = None, clone: bool = True) -> wp.array: + """Return effective surface speeds, with disabled surfaces reported as zero.""" + return self._get_device_values(self._effective_velocity, indices, clone) + + def get_commanded_velocities(self, indices: Any = None, clone: bool = True) -> wp.array: + """Return staged surface speeds without applying the enabled mask.""" + return self._get_device_values(self._command_velocity, indices, clone) + + def set_enabled(self, flags: Any, indices: Any = None) -> None: + """Enable or disable selected surfaces without discarding their speed commands.""" + selected = self._resolve_indices(indices) + values = self._broadcast_1d(flags, len(selected), "enabled flags") + if not np.all(np.isin(values, (0.0, 1.0))): + raise ValueError(f"Surface enabled flags must contain {len(selected)} boolean values.") + self._enabled_host[selected] = values.astype(np.int32) + self._enabled.assign(self._enabled_host) + self._refresh_effective_velocities() + + def get_enabled(self, indices: Any = None, clone: bool = True) -> wp.array: + """Return integer enabled flags for selected surfaces.""" + return self._get_device_int_values(self._enabled, indices, clone) + + def get_encoder_positions(self, indices: Any = None, clone: bool = True) -> wp.array: + """Return physics-rate integrated surface travel distances [m].""" + return self._get_device_values(self._encoder_position, indices, clone) + + def reset(self, env_ids: Any = None) -> None: + """Clear stale force and encoder state for selected environments. + + A full reset also restarts the global startup ramp. Partial vectorized + resets leave other environments' conveyor forces and startup state intact. + + Args: + env_ids: Environment indices to reset, or ``None`` for every environment. + """ + if env_ids is None: + self._conveyor.conveyor_body_f.zero_() + self._encoder_position.zero_() + self._elapsed_time.zero_() + self._conveyor.global_velocity_scale.zero_() + return + + ids = _as_numpy(env_ids) + if not np.issubdtype(ids.dtype, np.integer): + raise IndexError(f"Surface reset environment indices must be integers, got {env_ids!r}.") + ids = ids.astype(np.int64, copy=False).reshape(-1) + if np.any((ids < 0) | (ids >= self._num_envs)): + raise IndexError(f"Conveyor reset environment indices are out of range: {ids.tolist()}.") + self._world_mask_host.fill(False) + self._world_mask_host[ids] = True + if np.all(self._world_mask_host): + self.reset() + return + self._world_mask.assign(self._world_mask_host) + wp.launch( + _clear_selected_body_forces, + dim=self._model.body_count, + inputs=[self._model.body_world, self._world_mask], + outputs=[self._conveyor.conveyor_body_f], + device=self._device, + ) + wp.launch( + _clear_selected_encoders, + dim=len(self._surface_paths), + inputs=[self._conveyor_world, self._world_mask], + outputs=[self._encoder_position], + device=self._device, + ) + + def close(self) -> None: + """Deregister Newton callbacks and release references held by the driver.""" + if self._closed: + return + NewtonManager.unregister_state_force_callback(self.apply) + NewtonManager.unregister_post_solver_substep_callback(self.update) + self._closed = True + + def apply(self, state) -> None: + """Apply the wrench computed from the preceding physics solve.""" + self._conveyor.apply(state) + + def update(self, solver, contacts, state, dt: float) -> None: + """Update surface travel and let Newton compute the next conveyor wrench.""" + wp.launch( + _advance_startup_scale, + dim=1, + inputs=[dt, self._startup_duration_s], + outputs=[self._elapsed_time, self._conveyor.global_velocity_scale], + device=self._device, + ) + wp.launch( + _integrate_encoders, + dim=len(self._surface_paths), + inputs=[dt, self._effective_velocity], + outputs=[self._encoder_position], + device=self._device, + ) + self._conveyor.update(solver, contacts, state, dt) + # Exclude robot links and other bodies outside the transported-body selection. + wp.launch( + _filter_body_forces, + dim=self._model.body_count, + inputs=[self._body_is_tracked], + outputs=[self._conveyor.conveyor_body_f], + device=self._device, + ) + + def _validate_backend_buffers(self) -> None: + """Validate every fixed-size Newton buffer consumed by conveyor kernels.""" + model = self._model + contacts = self._contacts + _require_buffer_length("model.shape_body", model.shape_body, model.shape_count) + _require_buffer_length("model.shape_world", model.shape_world, model.shape_count) + _require_buffer_length("model.shape_transform", model.shape_transform, model.shape_count) + _require_buffer_length("model.body_world", model.body_world, model.body_count) + _require_buffer_length("model.body_com", model.body_com, model.body_count) + _require_buffer_length("model.body_inv_mass", model.body_inv_mass, model.body_count) + _require_buffer_length("model.body_inv_inertia", model.body_inv_inertia, model.body_count) + _require_buffer_length("contacts.force", contacts.force, contacts.rigid_contact_max) + _require_buffer_length( + "contacts.rigid_contact_shape0", contacts.rigid_contact_shape0, contacts.rigid_contact_max + ) + _require_buffer_length( + "contacts.rigid_contact_shape1", contacts.rigid_contact_shape1, contacts.rigid_contact_max + ) + _require_buffer_length( + "contacts.rigid_contact_normal", contacts.rigid_contact_normal, contacts.rigid_contact_max + ) + _require_buffer_length( + "contacts.rigid_contact_point0", contacts.rigid_contact_point0, contacts.rigid_contact_max + ) + _require_buffer_length( + "contacts.rigid_contact_point1", contacts.rigid_contact_point1, contacts.rigid_contact_max + ) + _require_buffer_length( + "contacts.rigid_contact_offset0", contacts.rigid_contact_offset0, contacts.rigid_contact_max + ) + _require_buffer_length( + "contacts.rigid_contact_offset1", contacts.rigid_contact_offset1, contacts.rigid_contact_max + ) + _require_buffer_length("contacts.rigid_contact_count", contacts.rigid_contact_count, 1) + + def _resolve_indices(self, indices: Any) -> np.ndarray: + """Normalize and validate a surface index selection.""" + if indices is None: + return np.arange(len(self._surface_paths), dtype=np.int64) + if isinstance(indices, slice): + return np.arange(len(self._surface_paths), dtype=np.int64)[indices] + selected = _as_numpy(indices) + if selected.dtype == np.bool_: + if selected.ndim != 1 or selected.size != len(self._surface_paths): + raise IndexError(f"Boolean surface indices must have length {len(self._surface_paths)}.") + return np.flatnonzero(selected).astype(np.int64) + if not np.issubdtype(selected.dtype, np.integer): + raise IndexError(f"Surface indices must be integers, got {indices!r}.") + selected = selected.astype(np.int64, copy=False).reshape(-1) + if np.any((selected < 0) | (selected >= len(self._surface_paths))): + raise IndexError(f"Conveyor surface indices are out of range: {selected.tolist()}.") + return selected + + def _refresh_effective_velocities(self) -> None: + """Apply the enabled mask at the one device-side command seam.""" + wp.launch( + _update_effective_velocities, + dim=len(self._surface_paths), + inputs=[self._command_velocity, self._enabled], + outputs=[self._effective_velocity], + device=self._device, + ) + wp.launch( + _update_belt_velocities, + dim=len(self._surface_paths), + inputs=[self._direction, self._radius, self._effective_velocity], + outputs=[self._conveyor.conv_const_vel, self._conveyor.conv_pivot_angvel], + device=self._device, + ) + + def _get_device_values(self, source: wp.array, indices: Any, clone: bool) -> wp.array: + """Clone a complete device buffer or gather a selected subset.""" + if indices is None: + return wp.clone(source) if clone else source + selected = self._resolve_indices(indices) + selected_device = wp.array(selected, dtype=wp.int32, device=self._device) + values = wp.empty(len(selected), dtype=wp.float32, device=self._device) + if len(selected) > 0: + wp.launch( + _gather_float_values, + dim=len(selected), + inputs=[source, selected_device], + outputs=[values], + device=self._device, + ) + return values + + def _get_device_int_values(self, source: wp.array, indices: Any, clone: bool) -> wp.array: + """Clone an integer device buffer or gather a selected subset.""" + if indices is None: + return wp.clone(source) if clone else source + selected = self._resolve_indices(indices) + selected_device = wp.array(selected, dtype=wp.int32, device=self._device) + values = wp.empty(len(selected), dtype=wp.int32, device=self._device) + if len(selected) > 0: + wp.launch( + _gather_int_values, + dim=len(selected), + inputs=[source, selected_device], + outputs=[values], + device=self._device, + ) + return values + + @staticmethod + def _broadcast_1d(values: Any, count: int, name: str) -> np.ndarray: + """Broadcast one scalar or validate one value per selected surface.""" + array = np.asarray(_as_numpy(values), dtype=np.float32) + if not np.all(np.isfinite(array)): + raise ValueError(f"Conveyor {name} must contain only finite values.") + if array.ndim == 0 or array.size == 1: + return np.full(count, float(array.reshape(-1)[0]), dtype=np.float32) + if array.ndim != 1 or array.size != count: + raise ValueError(f"Conveyor {name} need one value or {count} values, got shape {array.shape}.") + return array diff --git a/source/isaaclab_contrib/isaaclab_contrib/conveyors/physx.py b/source/isaaclab_contrib/isaaclab_contrib/conveyors/physx.py new file mode 100644 index 000000000000..93622a53f039 --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/conveyors/physx.py @@ -0,0 +1,532 @@ +# 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 + +"""CPU-only native PhysX surface-velocity control. + +The module deliberately keeps PhysX schema imports behind authoring and binding calls. Its +geometry conversion and host-side control state therefore remain usable in import-light tests +and tooling that do not launch Kit. + +The task configuration rejects GPU dynamics: the supported native ``PhysxSurfaceVelocityAPI`` +path can lose conveyor contacts there. The force-driven Newton adapter is the scalable GPU path. +""" + +from __future__ import annotations + +import math +from collections.abc import Sequence +from dataclasses import dataclass +from typing import Any, Protocol + +import numpy as np + +from isaaclab_contrib.conveyors.surface_velocity import SurfaceVelocitySpec + +_ENV_REGEX_NS = "{ENV_REGEX_NS}" + + +@dataclass(frozen=True, slots=True) +class PhysxSurfaceVelocityTwist: + """Local PhysX surface twist. + + Attributes: + linear_velocity: Local linear surface velocity [m/s]. + angular_velocity_deg: Local angular surface velocity [deg/s]. + """ + + linear_velocity: tuple[float, float, float] + angular_velocity_deg: tuple[float, float, float] + + +def compute_surface_velocity_twist( + spec: SurfaceVelocitySpec, velocity: float | None = None +) -> PhysxSurfaceVelocityTwist: + """Convert one backend-neutral belt command to a local PhysX surface twist. + + Straight belts map their signed speed to a normalized linear direction. Curved belts map + speed over radius to PhysX's degree-per-second angular convention. PhysX rotates the surface + field about the rigid-body origin, so ``-omega x pivot`` is included in the linear component + to move the instantaneous center to :attr:`SurfaceVelocitySpec.pivot_point`. + + Args: + spec: Authored conveyor intent in the collision prim's local frame. + velocity: Optional signed speed override [m/s]. Defaults to ``spec.velocity``. + + Returns: + Local linear and angular PhysX surface velocities. + + Raises: + ValueError: If the speed is not finite, the direction is zero, or a curved belt has no + positive radius. + """ + speed = spec.velocity if velocity is None else _finite_float("velocity", velocity) + direction = np.asarray(spec.direction, dtype=np.float64) + magnitude = float(np.linalg.norm(direction)) + if not math.isfinite(magnitude) or magnitude <= 1.0e-8: + raise ValueError(f"Conveyor direction must be finite and non-zero, got {spec.direction!r}.") + unit_direction = direction / magnitude + + if not spec.curved: + linear = unit_direction * speed + return PhysxSurfaceVelocityTwist(tuple(float(value) for value in linear), (0.0, 0.0, 0.0)) + + if spec.radius is None or not math.isfinite(spec.radius) or spec.radius <= 0.0: + raise ValueError("A curved PhysX conveyor requires a finite positive radius.") + angular_rad = unit_direction * (speed / spec.radius) + pivot = np.asarray(spec.pivot_point, dtype=np.float64) + linear = -np.cross(angular_rad, pivot) + angular_deg = np.degrees(angular_rad) + return PhysxSurfaceVelocityTwist( + tuple(float(value) for value in linear), tuple(float(value) for value in angular_deg) + ) + + +def apply_surface_velocity_api( + prim_or_path: Any, + spec: SurfaceVelocitySpec, + *, + velocity_scale: float = 0.0, + stage: Any | None = None, +) -> None: + """Apply and author a kinematic PhysX surface-velocity API on one belt prim. + + This function is intended for a task spawner, before PhysX parses the stage. It lazily imports + ``pxr.PhysxSchema`` and applies both the rigid-body and surface-velocity schemas. The rigid body + is made kinematic, surface velocities are authored in local space, and the initial command is + scaled explicitly. A zero default prevents pre-runtime motion during simulation warmup. + + Args: + prim_or_path: USD prim object or exact prim path. + spec: Conveyor intent used to author the local twist. + velocity_scale: Finite multiplier for the initial signed surface speed. + stage: Optional USD stage used when ``prim_or_path`` is a string. Defaults to the current + Isaac Lab stage. + + Raises: + RuntimeError: If the PhysX schema is unavailable or the target prim does not exist. + ValueError: If ``velocity_scale`` is not finite. + """ + scale = _finite_float("velocity_scale", velocity_scale) + twist = compute_surface_velocity_twist(spec, velocity=spec.velocity * scale) + binding = _PhysxSchemaSurfaceWriter((prim_or_path,), stage=stage, apply_api=True) + try: + binding.write(0, enabled=spec.enabled, twist=twist) + finally: + binding.close() + + +def resolve_surface_velocity_paths( + num_envs: int, + surface_specs: Sequence[SurfaceVelocitySpec], + env_path_format: str = "/World/envs/env_{}", +) -> tuple[str, ...]: + """Resolve conveyor templates to exact environment-major prim paths. + + Args: + num_envs: Number of replicated environments. + surface_specs: Within-environment surface descriptions. + env_path_format: Exact environment path format containing one ``{}`` field. + + Returns: + Exact paths ordered by environment, then by ``surface_specs`` order. + + Raises: + ValueError: If inputs cannot produce one unique path per environment and belt. + """ + if not isinstance(num_envs, int) or isinstance(num_envs, bool) or num_envs <= 0: + raise ValueError(f"Conveyor num_envs must be a positive integer, got {num_envs!r}.") + specs = tuple(surface_specs) + if not specs or not all(isinstance(spec, SurfaceVelocitySpec) for spec in specs): + raise ValueError("surface_specs must contain at least one SurfaceVelocitySpec.") + if not isinstance(env_path_format, str) or env_path_format.count("{}") != 1: + raise ValueError(f"Conveyor env_path_format must contain exactly one '{{}}', got {env_path_format!r}.") + try: + env_paths = tuple(env_path_format.format(env_id) for env_id in range(num_envs)) + except (IndexError, KeyError, ValueError) as exc: + raise ValueError(f"Invalid conveyor env_path_format: {env_path_format!r}.") from exc + if any(not path.startswith("/") or "{" in path or "}" in path for path in env_paths): + raise ValueError(f"Conveyor env_path_format must produce exact absolute paths, got {env_path_format!r}.") + + templates = tuple(spec.prim_path for spec in specs) + if len(set(templates)) != len(templates): + raise ValueError("Conveyor belt spec paths must be unique within an environment.") + if num_envs > 1 and any(_ENV_REGEX_NS not in path for path in templates): + raise ValueError("Replicated PhysX conveyors require every belt prim_path to use {ENV_REGEX_NS}.") + + paths = tuple(template.replace(_ENV_REGEX_NS, env_path) for env_path in env_paths for template in templates) + if len(set(paths)) != len(paths): + raise ValueError("Resolved PhysX conveyor prim paths must be unique.") + return paths + + +class SurfaceVelocity: + """Host-side CPU reference facade for native PhysX surface velocity. + + The facade binds exact environment-major paths whose schemas were already authored by + :func:`apply_surface_velocity_api`. Call :meth:`start` to register physics-rate updates, + or call :meth:`update` manually. Commands and enabled state survive full resets; a full reset + clears encoders and restarts the one-second startup ramp. This facade authors USD attributes + on the host and is not a GPU conveyor implementation. + """ + + def __init__( + self, + num_envs: int, + surface_specs: Sequence[SurfaceVelocitySpec], + *, + env_path_format: str = "/World/envs/env_{}", + startup_duration_s: float = 1.0, + stage: Any | None = None, + writer: Any | None = None, + ) -> None: + """Bind native surface attributes and initialize host-side control state. + + Args: + num_envs: Number of replicated environments. + surface_specs: Within-environment surface descriptions. + env_path_format: Exact replicated environment path format. + startup_duration_s: Duration of the global surface-speed ramp [s]. + stage: Optional USD stage used by the default schema writer. + writer: Optional writer implementing ``write(index, enabled=..., twist=...)`` and + ``close()``. This import-light seam is primarily for focused tests. + """ + duration = _finite_float("startup_duration_s", startup_duration_s) + if duration <= 0.0: + raise ValueError(f"Conveyor startup_duration_s must be positive, got {startup_duration_s!r}.") + self._num_envs = num_envs + self._surface_specs = tuple(surface_specs) + self._surface_paths = resolve_surface_velocity_paths(num_envs, self._surface_specs, env_path_format) + self._surfaces_per_env = len(self._surface_specs) + self._startup_duration_s = duration + self._elapsed_time = 0.0 + self._velocity_scale = 0.0 + self._closed = False + self._callback_handle: Any | None = None + self._writer: _SurfaceVelocityWriter = ( + writer + if writer is not None + else _PhysxSchemaSurfaceWriter(self._surface_paths, stage=stage, apply_api=False) + ) + + self._command_velocity = np.tile( + np.asarray([spec.velocity for spec in self._surface_specs], dtype=np.float32), self._num_envs + ) + self._enabled = np.tile( + np.asarray([spec.enabled for spec in self._surface_specs], dtype=np.bool_), self._num_envs + ) + self._encoder_position = np.zeros(self.num_surfaces, dtype=np.float32) + self._last_authored: list[tuple[bool, PhysxSurfaceVelocityTwist] | None] = [None] * self.num_surfaces + self._flush(force=True) + + @property + def specs(self) -> tuple[SurfaceVelocitySpec, ...]: + """Return authored descriptions in stable within-environment order.""" + return self._surface_specs + + @property + def surfaces_per_env(self) -> int: + """Return the number of authored surfaces per environment.""" + return self._surfaces_per_env + + @property + def prim_paths(self) -> tuple[str, ...]: + """Return exact PhysX prim paths in environment-major order.""" + return self._surface_paths + + @property + def surface_paths(self) -> tuple[str, ...]: + """Return an alias for :attr:`prim_paths`.""" + return self.prim_paths + + @property + def num_surfaces(self) -> int: + """Return the total number of bound conveyor surfaces.""" + return len(self._surface_paths) + + @property + def count(self) -> int: + """Return an alias for :attr:`num_surfaces`.""" + return self.num_surfaces + + @property + def initialized(self) -> bool: + """Return whether the facade still owns live schema bindings.""" + return not self._closed + + def start(self) -> None: + """Register one lazy PhysX post-step callback; repeated calls are safe.""" + self._require_open() + if self._callback_handle is not None: + return + from isaaclab_physx.physics.physx_manager import IsaacEvents, PhysxManager + + self._callback_handle = PhysxManager.register_callback( + self.update, + IsaacEvents.POST_PHYSICS_STEP, + name="physx_surface_velocity", + ) + + def update(self, dt: float) -> None: + """Advance encoder state and the startup ramp by one physics step. + + Args: + dt: Positive physics step duration [s]. + """ + self._require_open() + step = _finite_float("physics dt", dt) + if step <= 0.0: + raise ValueError(f"Conveyor physics dt must be positive, got {dt!r}.") + effective_velocity = self._command_velocity * self._enabled + self._encoder_position += np.asarray(step * effective_velocity, dtype=np.float32) + self._elapsed_time = min(self._startup_duration_s, self._elapsed_time + step) + self._velocity_scale = self._elapsed_time / self._startup_duration_s + self._flush() + + def set_velocities(self, velocities: Any, indices: Any = None) -> None: + """Set signed speeds [m/s], preserving commands while belts are disabled.""" + selected = self._resolve_indices(indices) + self._command_velocity[selected] = self._broadcast_finite(velocities, len(selected), "velocities") + self._flush(selected) + + def get_velocities(self, indices: Any = None, clone: bool = True) -> np.ndarray: + """Return effective speeds with disabled belts reported as zero.""" + return self._get_values(self._command_velocity * self._enabled, indices, clone) + + def get_commanded_velocities(self, indices: Any = None, clone: bool = True) -> np.ndarray: + """Return commanded speeds before applying the enabled mask.""" + return self._get_values(self._command_velocity, indices, clone) + + def set_enabled(self, flags: Any, indices: Any = None) -> None: + """Enable or disable belts without discarding their commands.""" + selected = self._resolve_indices(indices) + values = _as_numpy(flags) + if values.ndim == 0: + values = np.full(len(selected), values.item()) + else: + values = values.reshape(-1) + if values.size == 1: + values = np.full(len(selected), values.item()) + if values.size != len(selected) or not np.all(np.isin(values, (False, True, 0, 1))): + raise ValueError(f"Conveyor enabled flags must contain {len(selected)} boolean values.") + self._enabled[selected] = values.astype(np.bool_) + self._flush(selected) + + def get_enabled(self, indices: Any = None, clone: bool = True) -> np.ndarray: + """Return enabled flags as integer values.""" + return self._get_values(self._enabled.astype(np.int32), indices, clone) + + def get_encoder_positions(self, indices: Any = None, clone: bool = True) -> np.ndarray: + """Return physics-rate integrated commanded belt travel [m].""" + return self._get_values(self._encoder_position, indices, clone) + + def reset(self, env_ids: Any = None) -> None: + """Clear selected encoders and restart the global ramp on a full reset. + + Velocity commands and enabled state are deliberately preserved, including across a full + hard-reset path. Partial resets do not disturb the global ramp used by other environments. + + Args: + env_ids: Environment indices to reset, or ``None`` for all environments. + """ + self._require_open() + ids = self._resolve_env_ids(env_ids) + rows = (ids[:, None] * self._surfaces_per_env + np.arange(self._surfaces_per_env)[None, :]).reshape(-1) + self._encoder_position[rows] = 0.0 + if len(np.unique(ids)) == self._num_envs: + self._elapsed_time = 0.0 + self._velocity_scale = 0.0 + self._flush(force=True) + + def close(self) -> None: + """Deregister callbacks, disable authored motion, and release bindings safely.""" + if self._closed: + return + if self._callback_handle is not None: + self._callback_handle.deregister() + self._callback_handle = None + zero = PhysxSurfaceVelocityTwist((0.0, 0.0, 0.0), (0.0, 0.0, 0.0)) + for index in range(self.num_surfaces): + self._writer.write(index, enabled=False, twist=zero) + self._writer.close() + self._closed = True + + def _flush(self, indices: Any = None, *, force: bool = False) -> None: + """Author scaled effective commands for selected rows.""" + selected = self._resolve_indices(indices) + for index in selected: + enabled = bool(self._enabled[index]) + spec = self._surface_specs[index % self._surfaces_per_env] + speed = float(self._command_velocity[index]) * self._velocity_scale if enabled else 0.0 + twist = compute_surface_velocity_twist(spec, velocity=speed) + state = (enabled, twist) + if force or state != self._last_authored[index]: + self._writer.write(int(index), enabled=enabled, twist=twist) + self._last_authored[index] = state + + def _resolve_indices(self, indices: Any) -> np.ndarray: + """Normalize and validate a belt row selection.""" + self._require_open() + if indices is None: + return np.arange(self.num_surfaces, dtype=np.int64) + if isinstance(indices, slice): + return np.arange(self.num_surfaces, dtype=np.int64)[indices] + selected = _as_numpy(indices) + if selected.dtype == np.bool_: + if selected.ndim != 1 or selected.size != self.num_surfaces: + raise IndexError(f"Boolean surface indices must have length {self.num_surfaces}.") + return np.flatnonzero(selected).astype(np.int64) + if not np.issubdtype(selected.dtype, np.integer): + raise IndexError(f"Conveyor indices must be integers, got {indices!r}.") + selected = selected.astype(np.int64, copy=False).reshape(-1) + if np.any((selected < 0) | (selected >= self.num_surfaces)): + raise IndexError(f"Conveyor surface indices are out of range: {selected.tolist()}.") + return selected + + def _resolve_env_ids(self, env_ids: Any) -> np.ndarray: + """Normalize and validate an environment selection.""" + if env_ids is None: + return np.arange(self._num_envs, dtype=np.int64) + ids = _as_numpy(env_ids) + if not np.issubdtype(ids.dtype, np.integer): + raise IndexError(f"Conveyor reset environment indices must be integers, got {env_ids!r}.") + ids = ids.astype(np.int64, copy=False).reshape(-1) + if np.any((ids < 0) | (ids >= self._num_envs)): + raise IndexError(f"Conveyor reset environment indices are out of range: {ids.tolist()}.") + return ids + + @staticmethod + def _broadcast_finite(values: Any, count: int, name: str) -> np.ndarray: + """Return a finite float32 vector with scalar broadcasting.""" + try: + result = np.asarray(_as_numpy(values), dtype=np.float32) + except (TypeError, ValueError, OverflowError) as exc: + raise ValueError(f"Conveyor {name} must contain finite numeric values.") from exc + if result.ndim == 0: + result = np.full(count, result.item(), dtype=np.float32) + else: + result = result.reshape(-1) + if result.size == 1: + result = np.full(count, result.item(), dtype=np.float32) + if result.size != count or not np.all(np.isfinite(result)): + raise ValueError(f"Conveyor {name} must contain one or {count} finite values.") + return result + + def _get_values(self, source: np.ndarray, indices: Any, clone: bool) -> np.ndarray: + """Return a complete buffer or a selected copy.""" + self._require_open() + if indices is None: + return source.copy() if clone else source + return source[self._resolve_indices(indices)].copy() + + def _require_open(self) -> None: + """Fail predictably after backend resources have been released.""" + if self._closed: + raise RuntimeError("The PhysX conveyor surface facade is closed.") + + +class _SurfaceVelocityWriter(Protocol): + """Minimal schema-writer seam used by the runtime facade.""" + + def write(self, index: int, *, enabled: bool, twist: PhysxSurfaceVelocityTwist) -> None: + """Author one bound surface state.""" + ... + + def close(self) -> None: + """Release retained schema attributes.""" + ... + + +class _PhysxSchemaSurfaceWriter: + """Cached USD attribute writer with all schema imports kept lazy.""" + + def __init__(self, prims_or_paths: Sequence[Any], *, stage: Any | None, apply_api: bool) -> None: + """Resolve prims, optionally apply schemas, and cache surface attributes.""" + try: + from pxr import Gf, PhysxSchema, UsdPhysics + except ImportError as exc: + raise RuntimeError( + "PhysxSurfaceVelocityAPI is unavailable. Use this backend inside an Isaac Sim process with PhysX " + "schemas." + ) from exc + + if stage is None and any(isinstance(value, str) for value in prims_or_paths): + from isaaclab.sim.utils.stage import get_current_stage + + stage = get_current_stage() + self._gf = Gf + self._attributes: list[tuple[Any, Any, Any]] = [] + for value in prims_or_paths: + prim = stage.GetPrimAtPath(value) if isinstance(value, str) else value + if prim is None or not prim.IsValid(): + raise RuntimeError(f"Cannot bind PhysX conveyor surface: prim {value!r} does not exist.") + + rigid_body = UsdPhysics.RigidBodyAPI(prim) + if apply_api and not prim.HasAPI(UsdPhysics.RigidBodyAPI): + rigid_body = UsdPhysics.RigidBodyAPI.Apply(prim) + elif not prim.HasAPI(UsdPhysics.RigidBodyAPI): + raise RuntimeError(f"PhysX conveyor prim {prim.GetPath()} has no authored RigidBodyAPI.") + if apply_api: + rigid_body.CreateRigidBodyEnabledAttr().Set(True) + rigid_body.CreateKinematicEnabledAttr().Set(True) + elif rigid_body.GetKinematicEnabledAttr().Get() is not True: + raise RuntimeError(f"PhysX conveyor prim {prim.GetPath()} must be an authored kinematic rigid body.") + + if prim.HasAPI(PhysxSchema.PhysxSurfaceVelocityAPI): + surface_api = PhysxSchema.PhysxSurfaceVelocityAPI(prim) + elif apply_api: + surface_api = PhysxSchema.PhysxSurfaceVelocityAPI.Apply(prim) + else: + raise RuntimeError( + f"PhysX conveyor prim {prim.GetPath()} has no authored PhysxSurfaceVelocityAPI; " + "call apply_surface_velocity_api from its spawner before simulation starts." + ) + surface_api.CreateSurfaceVelocityLocalSpaceAttr().Set(True) + self._attributes.append( + ( + surface_api.CreateSurfaceVelocityEnabledAttr(), + surface_api.CreateSurfaceVelocityAttr(), + surface_api.CreateSurfaceAngularVelocityAttr(), + ) + ) + + def write(self, index: int, *, enabled: bool, twist: PhysxSurfaceVelocityTwist) -> None: + """Author one cached local-space surface twist.""" + enabled_attr, linear_attr, angular_attr = self._attributes[index] + # PhysX caches the contact-modification flag when a surface-velocity + # shape is parsed. Cycle it around command changes, matching the + # official Isaac Conveyor node, so live updates are picked up without + # leaving a stale contact modifier attached to the shape. + enabled_attr.Set(False) + linear_attr.Set(self._gf.Vec3f(*twist.linear_velocity)) + angular_attr.Set(self._gf.Vec3f(*twist.angular_velocity_deg)) + enabled_attr.Set(enabled) + + def close(self) -> None: + """Release cached schema attribute handles.""" + self._attributes.clear() + + +def _finite_float(name: str, value: Any) -> float: + """Coerce one finite scalar with a stable validation error.""" + try: + result = float(value) + except (TypeError, ValueError, OverflowError) as exc: + raise ValueError(f"Conveyor {name} must be finite, got {value!r}.") from exc + if not math.isfinite(result): + raise ValueError(f"Conveyor {name} must be finite, got {value!r}.") + return result + + +def _as_numpy(values: Any) -> np.ndarray: + """Move supported tensor-like values to a host NumPy array without importing their libraries.""" + if isinstance(values, np.ndarray): + return values + if hasattr(values, "detach"): + values = values.detach() + if hasattr(values, "cpu"): + values = values.cpu() + if hasattr(values, "numpy"): + return np.asarray(values.numpy()) + return np.asarray(values) diff --git a/source/isaaclab_contrib/isaaclab_contrib/conveyors/surface_velocity.py b/source/isaaclab_contrib/isaaclab_contrib/conveyors/surface_velocity.py new file mode 100644 index 000000000000..f00badb6b941 --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/conveyors/surface_velocity.py @@ -0,0 +1,179 @@ +# 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 + +"""Backend-neutral surface-velocity descriptions and control interface.""" + +from __future__ import annotations + +import math +from dataclasses import dataclass +from typing import Any, Protocol, runtime_checkable + +_ENV_REGEX_NS = "{ENV_REGEX_NS}" + + +def _validate_prim_path(value: str) -> None: + """Validate one exact USD prim path or supported replicated-path template.""" + if not isinstance(value, str) or not value: + raise ValueError("Surface prim_path must be a non-empty string.") + if value.startswith(f"{_ENV_REGEX_NS}/"): + path = value[len(_ENV_REGEX_NS) :] + elif value.startswith("/"): + path = value + else: + raise ValueError(f"Surface prim_path must be absolute or start with '{_ENV_REGEX_NS}/', got {value!r}.") + if "{" in path or "}" in path: + raise ValueError(f"Surface prim_path supports only a leading '{_ENV_REGEX_NS}' placeholder, got {value!r}.") + components = path[1:].split("/") + if not components or any(not component or component in {".", ".."} for component in components): + raise ValueError(f"Surface prim_path must identify a concrete prim without empty components, got {value!r}.") + if any(any(character.isspace() for character in component) for component in components): + raise ValueError(f"Surface prim_path components must not contain whitespace, got {value!r}.") + + +def _validate_scalar(name: str, value: Any) -> float: + """Return one finite float with consistent validation errors.""" + try: + result = float(value) + except (TypeError, ValueError, OverflowError) as exc: + raise ValueError(f"Surface {name} must be finite, got {value!r}.") from exc + if not math.isfinite(result): + raise ValueError(f"Surface {name} must be finite, got {value!r}.") + return result + + +def _validate_vector(name: str, value: tuple[float, ...], length: int, *, nonzero: bool = False) -> tuple[float, ...]: + """Return one finite vector with a stable tuple representation.""" + try: + result = tuple(float(component) for component in value) + except (TypeError, ValueError) as exc: + raise ValueError(f"Surface {name} must contain {length} finite values, got {value!r}.") from exc + if len(result) != length or not all(math.isfinite(component) for component in result): + raise ValueError(f"Surface {name} must contain {length} finite values, got {value!r}.") + if nonzero and math.sqrt(sum(component * component for component in result)) <= 1.0e-8: + raise ValueError(f"Surface {name} must be non-zero, got {value!r}.") + return result + + +@dataclass(frozen=True, slots=True) +class SurfaceVelocitySpec: + """Persistent intent for one collision surface with prescribed tangential velocity. + + The fields follow the authored conveyor model proposed for Isaac Sim while remaining independent of + Kit, OpenUSD, and any physics backend. Directions, surface normals, and the optional pivot point are + expressed in the collision prim's local frame. Runtime state such as encoder positions and applied + forces intentionally does not belong in this description. + + ``prim_path`` may contain Isaac Lab's ``{ENV_REGEX_NS}`` placeholder. A backend resolves that template + to every replicated collision prim while preserving one deterministic belt index per environment. An + absolute path addresses one unreplicated surface; replicated environments should use the placeholder. + + Args: + prim_path: Collision prim path or replicated Isaac Lab prim-path template. + velocity: Initial signed centerline surface velocity [m/s]. + enabled: Whether the surface initially transports contacting bodies. + direction: Local travel direction, or local rotation axis for a curved belt. + curved: Whether the velocity field rotates about ``pivot_point``. + pivot_point: Local rotation center for a curved belt [m]. An OpenUSD adapter may map this point to the + world transform of a prim targeted by the proposed schema's pivot relationship; it is not copied + into a schema attribute. + radius: Optional centerline radius used by backends that cannot derive it from geometry [m]. + surface_normal: Local outward normal of the carrying surface. + contact_threshold: Minimum contact-normal alignment accepted for traction. + friction_coefficient: Coulomb limit for synthetic surface traction. + """ + + prim_path: str + velocity: float = 0.0 + enabled: bool = True + direction: tuple[float, float, float] = (1.0, 0.0, 0.0) + curved: bool = False + pivot_point: tuple[float, float, float] = (0.0, 0.0, 0.0) + radius: float | None = None + surface_normal: tuple[float, float, float] = (0.0, 0.0, 1.0) + contact_threshold: float = 0.997 + friction_coefficient: float = 0.7 + + def __post_init__(self) -> None: + """Normalize immutable vectors and validate authored values.""" + _validate_prim_path(self.prim_path) + for name in ("enabled", "curved"): + if not isinstance(getattr(self, name), bool): + raise ValueError(f"Surface {name} must be a bool, got {getattr(self, name)!r}.") + + velocity = _validate_scalar("velocity", self.velocity) + contact_threshold = _validate_scalar("contact_threshold", self.contact_threshold) + friction_coefficient = _validate_scalar("friction_coefficient", self.friction_coefficient) + if not 0.0 <= contact_threshold <= 1.0: + raise ValueError(f"Surface contact_threshold must be in [0, 1], got {self.contact_threshold!r}.") + if friction_coefficient < 0.0: + raise ValueError( + f"Surface friction_coefficient must be finite and non-negative, got {self.friction_coefficient!r}." + ) + radius = None if self.radius is None else _validate_scalar("radius", self.radius) + if radius is not None and radius <= 0.0: + raise ValueError(f"Surface radius must be finite and positive when provided, got {self.radius!r}.") + + object.__setattr__(self, "velocity", velocity) + object.__setattr__(self, "direction", _validate_vector("direction", self.direction, 3, nonzero=True)) + object.__setattr__(self, "pivot_point", _validate_vector("pivot_point", self.pivot_point, 3)) + object.__setattr__( + self, "surface_normal", _validate_vector("surface_normal", self.surface_normal, 3, nonzero=True) + ) + object.__setattr__(self, "radius", radius) + object.__setattr__(self, "contact_threshold", contact_threshold) + object.__setattr__(self, "friction_coefficient", friction_coefficient) + + +@runtime_checkable +class SurfaceVelocityView(Protocol): + """Common tensorized control contract implemented by physics backends.""" + + @property + def prim_paths(self) -> tuple[str, ...]: + """Resolved collision prim paths in stable surface-index order.""" + ... + + @property + def num_surfaces(self) -> int: + """Number of resolved moving surfaces.""" + ... + + @property + def count(self) -> int: + """Alias for :attr:`num_surfaces`, matching tensor-view naming.""" + ... + + def set_velocities(self, velocities: Any, indices: Any = None) -> None: + """Set signed surface velocities [m/s] for selected surfaces.""" + ... + + def get_velocities(self, indices: Any = None, clone: bool = True) -> Any: + """Return effective surface velocities [m/s] for selected surfaces.""" + ... + + def get_commanded_velocities(self, indices: Any = None, clone: bool = True) -> Any: + """Return commanded surface velocities [m/s] before the enabled mask.""" + ... + + def set_enabled(self, flags: Any, indices: Any = None) -> None: + """Enable or disable selected surfaces without discarding their commands.""" + ... + + def get_enabled(self, indices: Any = None, clone: bool = True) -> Any: + """Return integer enabled flags for selected surfaces.""" + ... + + def get_encoder_positions(self, indices: Any = None, clone: bool = True) -> Any: + """Return integrated surface travel [m] for selected surfaces.""" + ... + + def reset(self, env_ids: Any = None) -> None: + """Clear runtime state for selected replicated environments.""" + ... + + def close(self) -> None: + """Release backend resources and callbacks.""" + ... diff --git a/source/isaaclab_contrib/test/conveyors/test_surface_velocity_newton.py b/source/isaaclab_contrib/test/conveyors/test_surface_velocity_newton.py new file mode 100644 index 000000000000..9bea66335600 --- /dev/null +++ b/source/isaaclab_contrib/test/conveyors/test_surface_velocity_newton.py @@ -0,0 +1,166 @@ +# 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 + +"""Native CPU contracts for the Newton conveyor adapter.""" + +import numpy as np +import pytest +import warp as wp +from isaaclab_newton.physics import MJWarpSolverCfg, NewtonBuilderCfg, NewtonCfg, NewtonManager + +from isaaclab.sim import SimulationCfg, build_simulation_context + +from isaaclab_contrib.conveyors.newton import SurfaceVelocity +from isaaclab_contrib.conveyors.surface_velocity import SurfaceVelocitySpec + + +def test_native_conveyor_controls_rebind_and_reset_independently(monkeypatch): + """Real contacts, models and callbacks preserve commands and discard only selected-world traction.""" + cfg = SimulationCfg( + device="cpu", + dt=1 / 120, + physics=NewtonCfg(solver_cfg=MJWarpSolverCfg(use_mujoco_contacts=False), use_cuda_graph=False), + ) + with build_simulation_context(sim_cfg=cfg) as sim: + builder = sim.get_or_create_backend(NewtonBuilderCfg(physics_cfg=cfg.physics)) + for world in range(2): + builder.begin_world() + root = f"/World/envs/env_{world}" + for name, y in (("B", 0.5), ("A", -0.5), ("Nested/BeltA", 2.0)): + label = f"{root}/{name}" if "/" in name else f"{root}/Belt{name}" + builder.add_shape_box( + -1, xform=wp.transform((0, y, 0), wp.quat_identity()), hx=1, hy=0.2, hz=0.1, label=label + ) + cube = builder.add_link(xform=wp.transform((0, -0.5, 0.21), wp.quat_identity()), label=f"{root}/Cube0") + builder.add_shape_box(cube, hx=0.1, hy=0.1, hz=0.1) + builder.add_articulation([builder.add_joint_free(cube)]) + builder.end_world() + builder.add_shape_box(-1, hx=0.1, hy=0.1, hz=0.1, label="/World/props/BeltA") + callbacks_before = set(NewtonManager._callbacks) + specs = ( + SurfaceVelocitySpec("{ENV_REGEX_NS}/BeltA", velocity=0.1), + SurfaceVelocitySpec("{ENV_REGEX_NS}/BeltB", velocity=-0.2, enabled=False), + ) + calls, bindings = [], [] + + def actuator(): + calls.append("actuator") + + def force(state): + calls.append("force") + + def substep(solver, contacts, state, dt): + calls.append("substep") + + def initialized(model, contacts): + bindings.append((model, contacts)) + + observers = ( + (actuator, NewtonManager.register_post_actuator_callback, NewtonManager.unregister_post_actuator_callback), + (force, NewtonManager.register_state_force_callback, NewtonManager.unregister_state_force_callback), + ( + substep, + NewtonManager.register_post_solver_substep_callback, + NewtonManager.unregister_post_solver_substep_callback, + ), + (initialized, NewtonManager.register_solver_init_callback, NewtonManager.unregister_solver_init_callback), + ) + for callback, register, _ in observers: + register(callback) + NewtonManager.register_solver_init_callback(initialized) + driver = SurfaceVelocity(2, specs, body_pattern="Cube0$", body_count_per_env=1) + try: + with pytest.raises(RuntimeError, match="not bound"): + driver.set_velocities(0.2) + sim.reset() + assert bindings == [(NewtonManager.get_model(), NewtonManager.get_contacts())] + first = driver._binding + assert NewtonManager.get_contacts().force is not None + assert driver.prim_paths == tuple( + f"/World/envs/env_{world}/Belt{name}" for world in range(2) for name in ("A", "B") + ) + model = NewtonManager.get_model() + mapping = first._conveyor.shape_conveyor.numpy() + assert [ + mapping[i] for i, label in enumerate(model.shape_label) if "Nested" in label or "/props/" in label + ] == [-1] * 3 + driver.set_velocities([0.3, -0.25, -0.4], indices=[0, 1, 3]) + driver.set_enabled([False, True], indices=[2, 3]) + np.testing.assert_allclose(driver.get_velocities().numpy(), [0.3, 0, 0, -0.4]) + sim.step(render=False) + assert calls == ["actuator"] + ["force", "substep"] * cfg.physics.num_substeps + np.testing.assert_allclose(driver.get_encoder_positions().numpy(), np.array([0.3, 0, 0, -0.4]) / 120) + first._conveyor.conveyor_body_f.assign(np.ones((2, 6), dtype=np.float32)) + driver.reset([0]) + state = model.state() + first.apply(state) + np.testing.assert_array_equal(state.body_f.numpy(), [[0] * 6, [1] * 6]) + np.testing.assert_allclose(driver.get_encoder_positions().numpy(), [0, 0, 0, -0.4 / 120]) + sim.reset() + assert first._closed and driver._binding is not first + assert driver._binding._model is NewtonManager.get_model() + assert driver._binding._contacts is NewtonManager.get_contacts() + np.testing.assert_allclose(driver.get_velocities().numpy(), [0.3, 0, 0, -0.4]) + assert len(bindings) == 2 and bindings[1] == (NewtonManager.get_model(), NewtonManager.get_contacts()) + assert bindings[0][0] is not bindings[1][0] and bindings[0][1] is not bindings[1][1] + for callback, _, unregister in observers: + unregister(callback) + unregister(callback) + previous_calls = list(calls) + sim.step(render=False) + sim.reset() + assert calls == previous_calls and len(bindings) == 2 + np.testing.assert_array_equal(driver.get_enabled().numpy(), [1, 0, 0, 1]) + np.testing.assert_allclose(driver.get_commanded_velocities().numpy(), [0.3, -0.25, 0.1, -0.4]) + callbacks_live = set(NewtonManager._callbacks) + with monkeypatch.context() as patch: + + def fail_registration(cls, callback): + raise RuntimeError("solver callback unavailable") + + patch.setattr(NewtonManager, "register_solver_init_callback", classmethod(fail_registration)) + with pytest.raises(RuntimeError, match="solver callback unavailable"): + SurfaceVelocity(2, specs, body_pattern="Cube0$") + assert set(NewtonManager._callbacks) == callbacks_live + finally: + driver.close() + driver.close() + assert set(NewtonManager._callbacks) == callbacks_before + assert not NewtonManager._solver_init_callbacks + assert not NewtonManager._state_force_callbacks + assert not NewtonManager._post_solver_substep_callbacks + + +@pytest.mark.parametrize( + ("specs", "num_envs", "env_path", "error", "message"), + [ + ((), 1, "/World/envs/env_{}", ValueError, "At least one"), + ((object(),), 1, "/World/envs/env_{}", TypeError, "SurfaceVelocitySpec"), + ((SurfaceVelocitySpec("/Belt"),) * 2, 1, "/World/envs/env_{}", ValueError, "unique"), + ( + (SurfaceVelocitySpec("/Belt"), SurfaceVelocitySpec("/Belt/Child")), + 1, + "/World/envs/env_{}", + ValueError, + "ancestors", + ), + ((SurfaceVelocitySpec("/Curve", curved=True),), 1, "/World/envs/env_{}", ValueError, "positive radius"), + ((SurfaceVelocitySpec("/Shared/Belt"),), 2, "/World/envs/env_{}", ValueError, "Replicated"), + ((SurfaceVelocitySpec("{ENV_REGEX_NS}/Belt"),), 1, "/World/envs/env_.*", ValueError, "env_path_format"), + ], +) +def test_invalid_conveyors_leave_no_callbacks(specs, num_envs, env_path, error, message): + """Rejected authoring must not leave a partially registered adapter.""" + before = set(NewtonManager._callbacks) + with pytest.raises(error, match=message): + SurfaceVelocity(num_envs, specs, env_path_format=env_path, body_pattern="Cube0$") + assert set(NewtonManager._callbacks) == before + + +def test_unreplicated_absolute_surface_path_is_preserved(): + """A single-environment surface can target a global authored path.""" + from isaaclab_contrib.conveyors.newton import _resolve_belt_prim_path + + assert _resolve_belt_prim_path("/World/Shared/Belt", "/World/envs/env_{}", 0) == "/World/Shared/Belt" diff --git a/source/isaaclab_contrib/test/conveyors/test_surface_velocity_physx.py b/source/isaaclab_contrib/test/conveyors/test_surface_velocity_physx.py new file mode 100644 index 000000000000..91348d447553 --- /dev/null +++ b/source/isaaclab_contrib/test/conveyors/test_surface_velocity_physx.py @@ -0,0 +1,147 @@ +# 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-light tests for native PhysX surface velocity.""" + +from __future__ import annotations + +import math +from types import SimpleNamespace +from unittest.mock import Mock, call + +import numpy as np +import pytest +import torch + +import isaaclab_contrib.conveyors.physx as surface_module +from isaaclab_contrib.conveyors.surface_velocity import SurfaceVelocitySpec + + +class _FakeWriter: + """Record authored states without importing USD or PhysX schemas.""" + + def __init__(self) -> None: + self.writes: list[tuple[int, bool, surface_module.PhysxSurfaceVelocityTwist]] = [] + self.close_count = 0 + + def write(self, index: int, *, enabled: bool, twist: surface_module.PhysxSurfaceVelocityTwist) -> None: + self.writes.append((index, enabled, twist)) + + def close(self) -> None: + self.close_count += 1 + + +def _surface_spec(name: str = "Belt", **kwargs) -> SurfaceVelocitySpec: + """Return one replicated belt description for facade tests.""" + return SurfaceVelocitySpec(prim_path=f"{{ENV_REGEX_NS}}/{name}", velocity=0.4, **kwargs) + + +def test_twist_conversion_normalizes_straight_direction() -> None: + """Straight surface speed is independent of the authored direction magnitude.""" + spec = _surface_spec(direction=(3.0, 4.0, 0.0)) + + twist = surface_module.compute_surface_velocity_twist(spec, velocity=2.0) + + np.testing.assert_allclose(twist.linear_velocity, (1.2, 1.6, 0.0)) + assert twist.angular_velocity_deg == (0.0, 0.0, 0.0) + + +def test_twist_conversion_uses_degrees_and_compensates_curved_pivot() -> None: + """Curved belts rotate about their local pivot rather than the rigid-body origin.""" + spec = _surface_spec( + direction=(0.0, 0.0, 2.0), + curved=True, + radius=2.0, + pivot_point=(2.0, 0.0, 0.0), + ) + + twist = surface_module.compute_surface_velocity_twist(spec, velocity=math.pi) + + np.testing.assert_allclose(twist.angular_velocity_deg, (0.0, 0.0, 90.0), atol=1.0e-12) + np.testing.assert_allclose(twist.linear_velocity, (0.0, -math.pi, 0.0), atol=1.0e-12) + + +def test_curved_twist_requires_an_explicit_radius() -> None: + """A native angular rate cannot be inferred from unspecified task geometry.""" + spec = _surface_spec(curved=True) + + with pytest.raises(ValueError, match="positive radius"): + surface_module.compute_surface_velocity_twist(spec) + + +def test_paths_are_resolved_in_environment_major_order() -> None: + """Runtime rows stay deterministic across stage discovery ordering.""" + specs = (_surface_spec("BeltA"), _surface_spec("Nested/BeltB")) + + paths = surface_module.resolve_surface_velocity_paths(2, specs) + + assert paths == ( + "/World/envs/env_0/BeltA", + "/World/envs/env_0/Nested/BeltB", + "/World/envs/env_1/BeltA", + "/World/envs/env_1/Nested/BeltB", + ) + with pytest.raises(ValueError, match="require every belt"): + surface_module.resolve_surface_velocity_paths(2, (SurfaceVelocitySpec(prim_path="/World/Shared/Belt"),)) + + +def test_facade_ramps_playback_integrates_encoders_and_preserves_commands_on_reset() -> None: + """Full resets restart playback without erasing policy-visible command state.""" + writer = _FakeWriter() + facade = surface_module.SurfaceVelocity(2, (_surface_spec(),), writer=writer) + + assert facade.prim_paths == ("/World/envs/env_0/Belt", "/World/envs/env_1/Belt") + assert facade.num_surfaces == facade.count == 2 + assert [record[2].linear_velocity for record in writer.writes] == [(0.0, 0.0, 0.0)] * 2 + + facade.update(0.25) + np.testing.assert_allclose([record[2].linear_velocity[0] for record in writer.writes[-2:]], (0.1, 0.1)) + np.testing.assert_allclose(facade.get_encoder_positions(), (0.1, 0.1)) + + facade.set_velocities(torch.tensor([0.8], requires_grad=True), indices=torch.tensor([0])) + facade.set_enabled(False, indices=[1]) + facade.update(0.25) + np.testing.assert_allclose(facade.get_commanded_velocities(), (0.8, 0.4)) + np.testing.assert_allclose(facade.get_velocities(), (0.8, 0.0)) + np.testing.assert_allclose(facade.get_encoder_positions(), (0.3, 0.1)) + + facade.reset(env_ids=torch.tensor([0])) + np.testing.assert_allclose(facade.get_encoder_positions(), (0.0, 0.1)) + facade.reset(env_ids=[1, 0]) + np.testing.assert_allclose(facade.get_encoder_positions(), (0.0, 0.0)) + np.testing.assert_allclose(facade.get_commanded_velocities(), (0.8, 0.4)) + assert facade.get_enabled().tolist() == [1, 0] + np.testing.assert_allclose(writer.writes[-2][2].linear_velocity, (0.0, 0.0, 0.0)) + + facade.close() + facade.close() + assert writer.close_count == 1 + assert [(index, enabled) for index, enabled, _ in writer.writes[-2:]] == [(0, False), (1, False)] + + +def test_authoring_helper_applies_kinematic_local_surface_schema(monkeypatch: pytest.MonkeyPatch) -> None: + """Author a real kinematic USD body; mock only the Kit-dependent PhysX API.""" + import pxr + from pxr import Gf, Usd, UsdPhysics + + stage = Usd.Stage.CreateInMemory() + prim = stage.DefinePrim("/World/Belt", "Xform") + surface_api = Mock() + # HasAPI needs a registered USD type. Substitute only the unavailable PhysX schema. + schema = SimpleNamespace(PhysxSurfaceVelocityAPI=UsdPhysics.CollisionAPI) + monkeypatch.setattr(UsdPhysics.CollisionAPI, "Apply", Mock(return_value=surface_api)) + monkeypatch.setattr(pxr, "PhysxSchema", schema, raising=False) + + surface_module.apply_surface_velocity_api(prim, _surface_spec(), velocity_scale=0.0) + + assert prim.HasAPI(UsdPhysics.RigidBodyAPI) + rigid_body = UsdPhysics.RigidBodyAPI(prim) + assert rigid_body.GetRigidBodyEnabledAttr().Get() is True + assert rigid_body.GetKinematicEnabledAttr().Get() is True + schema.PhysxSurfaceVelocityAPI.Apply.assert_called_once_with(prim) + surface_api.CreateSurfaceVelocityLocalSpaceAttr().Set.assert_called_once_with(True) + assert surface_api.CreateSurfaceVelocityEnabledAttr().Set.call_args_list == [call(False), call(True)] + surface_api.CreateSurfaceVelocityAttr().Set.assert_called_once_with(Gf.Vec3f(0.0)) + surface_api.CreateSurfaceAngularVelocityAttr().Set.assert_called_once_with(Gf.Vec3f(0.0)) diff --git a/source/isaaclab_contrib/test/conveyors/test_surface_velocity_spec.py b/source/isaaclab_contrib/test/conveyors/test_surface_velocity_spec.py new file mode 100644 index 000000000000..db0a7adedb90 --- /dev/null +++ b/source/isaaclab_contrib/test/conveyors/test_surface_velocity_spec.py @@ -0,0 +1,45 @@ +# 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 + +"""Focused tests for backend-neutral surface-velocity descriptions.""" + +from __future__ import annotations + +import pytest + +from isaaclab_contrib.conveyors.surface_velocity import SurfaceVelocitySpec + + +def test_surface_velocity_spec_normalizes_vectors() -> None: + """List inputs become immutable vectors before either backend consumes them.""" + spec = SurfaceVelocitySpec( + prim_path="{ENV_REGEX_NS}/Belt", direction=[3, 4, 0], pivot_point=[1, 2, 3], surface_normal=[0, 0, 1] + ) + assert spec.direction == (3.0, 4.0, 0.0) + assert spec.pivot_point == (1.0, 2.0, 3.0) + assert spec.surface_normal == (0.0, 0.0, 1.0) + + +@pytest.mark.parametrize( + ("kwargs", "message"), + [ + ({"prim_path": "relative/Belt"}, "prim_path"), + ({"prim_path": "/"}, "prim_path"), + ({"prim_path": "/World/{ENV_REGEX_NS}/Belt"}, "prim_path"), + ({"prim_path": "/World/Belt", "velocity": None}, "velocity"), + ({"prim_path": "/World/Belt", "velocity": float("nan")}, "velocity"), + ({"prim_path": "/World/Belt", "enabled": 1}, "enabled"), + ({"prim_path": "/World/Belt", "curved": "yes"}, "curved"), + ({"prim_path": "/World/Belt", "direction": (0.0, 0.0, 0.0)}, "direction"), + ({"prim_path": "/World/Belt", "surface_normal": (0.0, 0.0)}, "surface_normal"), + ({"prim_path": "/World/Belt", "radius": 0.0}, "radius"), + ({"prim_path": "/World/Belt", "contact_threshold": 1.1}, "contact_threshold"), + ({"prim_path": "/World/Belt", "friction_coefficient": -0.1}, "friction_coefficient"), + ], +) +def test_surface_velocity_spec_rejects_invalid_authored_values(kwargs: dict, message: str) -> None: + """Invalid persistent intent fails before any physics lifecycle is registered.""" + with pytest.raises(ValueError, match=message): + SurfaceVelocitySpec(**kwargs) diff --git a/source/isaaclab_newton/changelog.d/maximiliank-conveyor-lifecycle.added.rst b/source/isaaclab_newton/changelog.d/maximiliank-conveyor-lifecycle.added.rst new file mode 100644 index 000000000000..53086e356237 --- /dev/null +++ b/source/isaaclab_newton/changelog.d/maximiliank-conveyor-lifecycle.added.rst @@ -0,0 +1,2 @@ +* Added solver-initialization and post-substep hooks with callback cleanup for scene-owned force systems. +* Exposed collision filtering for static and kinematic shape pairs. diff --git a/source/isaaclab_newton/changelog.d/maximiliank-conveyor-lifecycle.minor b/source/isaaclab_newton/changelog.d/maximiliank-conveyor-lifecycle.minor new file mode 100644 index 000000000000..e69de29bb2d1 diff --git a/source/isaaclab_newton/isaaclab_newton/physics/newton_collision_cfg.py b/source/isaaclab_newton/isaaclab_newton/physics/newton_collision_cfg.py index 8bb8b75cae0f..a13c4a84c21e 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/newton_collision_cfg.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/newton_collision_cfg.py @@ -106,6 +106,13 @@ class NewtonCollisionPipelineCfg: Defaults to ``"explicit"`` (same as Newton's default when ``broad_phase=None``). """ + include_static_kinematic_pairs: bool = True + """Whether to generate contacts between two immovable shapes. + + Set to ``False`` to exclude static-static, static-kinematic, and kinematic-kinematic + pairs. Defaults to ``True``, matching Newton's default. + """ + reduce_contacts: bool = True """Whether to reduce contacts for mesh-mesh collisions. diff --git a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py index 93ff618a4bb8..bfbe8a2d506f 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py @@ -515,6 +515,13 @@ def setup_deformable_body(cls, prim: Any, deformable_type: str, sim_mesh_prim: A _post_actuator_callbacks: list[Callable[[], None]] = [] # In-graph hooks invoked immediately before every solver substep. _state_force_callbacks: list[Callable[[State], None]] = [] + # In-graph hooks invoked after every solver substep, before the post-step + # state is swapped into the active buffer and its external forces cleared. + _post_solver_substep_callbacks: list[Callable[[SolverBase, Contacts | None, State, float], None]] = [] + # Lifecycle hooks invoked after the solver and contact buffers are created, + # but before any CUDA graph is captured. Scene-owned force systems use this + # seam to bind model-specific buffers and register their in-graph callbacks. + _solver_init_callbacks: list[Callable[[Model, Contacts | None], None]] = [] # In-graph hooks invoked after the last solver substep and before sensors, # in registration order. Articulations with non-identity ordering register # their backend-to-user state republish kernels here so the reorders are @@ -825,6 +832,8 @@ def clear(cls): NewtonManager._adapter = None NewtonManager._post_actuator_callbacks = [] NewtonManager._state_force_callbacks = [] + NewtonManager._post_solver_substep_callbacks = [] + NewtonManager._solver_init_callbacks = [] NewtonManager._post_step_callbacks = [] # Set by an articulation that took the ``use_newton_actuators=True`` # branch in ``_process_actuators_cfg``. Together with the adapter @@ -1611,6 +1620,12 @@ def initialize_solver(cls) -> None: ) cls._initialize_contacts() + # Scene-owned systems that consume solver/contact buffers must bind + # here: both resources now exist, while CUDA graph capture has not yet + # started. The callbacks intentionally persist across hard resets and + # rebind to each re-finalized model. + cls._run_solver_init_callbacks() + # Picking callbacks must be registered after the concrete solver has # published its force-input capability, but before CUDA graph capture. sim = PhysicsManager._sim @@ -1678,6 +1693,8 @@ def _run_solver_substeps(cls, contacts) -> None: for callback in cls._state_force_callbacks: callback(backend.state_0) cls._step_solver(backend.state_0, backend.state_0, backend.control, contacts, cls._solver_dt) + for cb in cls._post_solver_substep_callbacks: + cb(cls._solver, contacts, backend.state_0, cls._solver_dt) backend.state_0.clear_forces() if collide_mid_loop and (i + 1) % collide_every == 0 and i + 1 < cls._num_substeps: cls._collision_pipeline.collide(backend.state_0, contacts) @@ -1688,6 +1705,8 @@ def _run_solver_substeps(cls, contacts) -> None: for callback in cls._state_force_callbacks: callback(backend.state_0) cls._step_solver(backend.state_0, backend.state_1, backend.control, contacts, cls._solver_dt) + for cb in cls._post_solver_substep_callbacks: + cb(cls._solver, contacts, backend.state_1, cls._solver_dt) if need_copy_on_last and i == cls._num_substeps - 1: backend.state_0.assign(backend.state_1) else: @@ -1902,6 +1921,98 @@ def register_state_force_callback(cls, callback: Callable[[State], None]) -> Non return NewtonManager._state_force_callbacks.append(callback) + @classmethod + def unregister_state_force_callback(cls, callback: Callable[[State], None]) -> None: + """Remove a previously registered state-force callback. + + Removing a callback that was never registered or was already removed is + a safe no-op. This lets scene-owned systems release bound-method + references before the global Newton manager is cleared. + + Args: + callback: Previously registered callback. + """ + with contextlib.suppress(ValueError): + NewtonManager._state_force_callbacks.remove(callback) + + @classmethod + def unregister_post_actuator_callback(cls, callback: Callable[[], None]) -> None: + """Remove a previously registered post-actuator callback. + + Removing a callback that was never registered or was already removed is + a safe no-op. This lets scene-owned systems release bound-method + references before the global Newton manager is cleared. + + Args: + callback: Previously registered callback. + """ + with contextlib.suppress(ValueError): + cls._post_actuator_callbacks.remove(callback) + + @classmethod + def register_post_solver_substep_callback( + cls, callback: Callable[[SolverBase, Contacts | None, State, float], None] + ) -> None: + """Append a hook invoked immediately after every solver substep. + + The callback receives the active solver, contacts, post-substep state, + and solver timestep [s]. It runs before double-buffered state swapping + and before external forces are cleared, matching Newton's native + per-substep force-feedback loop. Callbacks must be graph-safe. + + Args: + callback: Function called after each solver substep. + """ + if callback in NewtonManager._post_solver_substep_callbacks: + return + cls._post_solver_substep_callbacks.append(callback) + + @classmethod + def register_solver_init_callback(cls, callback: Callable[[Model, Contacts | None], None]) -> None: + """Register a callback that binds resources before CUDA graph capture. + + The callback runs after every solver/contact initialization, including + hard resets, and before any simulation graph is captured. It may + allocate model-specific buffers and register graph-safe step callbacks. + + Args: + callback: Function receiving the active model and contact buffer. + """ + if callback in NewtonManager._solver_init_callbacks: + return + NewtonManager._solver_init_callbacks.append(callback) + + @classmethod + def _run_solver_init_callbacks(cls) -> None: + """Bind scene-owned solver resources before graph capture.""" + for callback in tuple(NewtonManager._solver_init_callbacks): + callback(cls.backend.model, cls._contacts) + + @classmethod + def unregister_solver_init_callback(cls, callback: Callable[[Model, Contacts | None], None]) -> None: + """Remove a previously registered solver-initialization callback. + + Removing a callback that was never registered or was already removed is + a safe no-op. + + Args: + callback: Previously registered callback. + """ + with contextlib.suppress(ValueError): + NewtonManager._solver_init_callbacks.remove(callback) + + @classmethod + def unregister_post_solver_substep_callback( + cls, callback: Callable[[SolverBase, Contacts | None, State, float], None] + ) -> None: + """Remove a previously registered post-solver-substep callback. + + Args: + callback: Previously registered callback. + """ + with contextlib.suppress(ValueError): + cls._post_solver_substep_callbacks.remove(callback) + @classmethod def register_post_step_callback(cls, callback: Callable[[], None]) -> None: """Append a hook to the list invoked after the last solver substep on every step. From f54d05c25433e386913e56a6d99b6364a944aa30 Mon Sep 17 00:00:00 2001 From: Maximilian Krause Date: Thu, 8 Oct 2026 13:43:48 +0200 Subject: [PATCH 2/9] Trim unused conveyor adapter state and compatibility APIs --- .../isaaclab_contrib/conveyors/README.md | 6 +- .../isaaclab_contrib/conveyors/newton.py | 172 ++---------------- .../isaaclab_contrib/conveyors/physx.py | 48 +---- .../conveyors/surface_velocity.py | 11 +- .../conveyors/test_surface_velocity_newton.py | 4 +- .../conveyors/test_surface_velocity_physx.py | 13 +- 6 files changed, 28 insertions(+), 226 deletions(-) diff --git a/source/isaaclab_contrib/isaaclab_contrib/conveyors/README.md b/source/isaaclab_contrib/isaaclab_contrib/conveyors/README.md index 751755a2487e..1c5a84cba40b 100644 --- a/source/isaaclab_contrib/isaaclab_contrib/conveyors/README.md +++ b/source/isaaclab_contrib/isaaclab_contrib/conveyors/README.md @@ -14,8 +14,8 @@ experimental and is not exported from Isaac Lab core or the backend packages. Author the API before simulation parsing with `apply_surface_velocity_api`. This path requires the PhysX backend and Kit; GPU dynamics are unsupported. -Both adapters retain commanded speeds while disabled and clear selected worlds' -runtime state on reset. The Newton adapter supports live traction controls; -PhysX uses authored contact materials and rejects unsupported traction setters. +Both adapters retain commanded speeds while disabled and restart their startup +ramp on a full reset. Newton also clears stale forces for selected worlds. +Newton uses the spec's traction parameters; PhysX uses authored contact materials. No meshes, assets, task registrations, or policy checkpoints are bundled here. diff --git a/source/isaaclab_contrib/isaaclab_contrib/conveyors/newton.py b/source/isaaclab_contrib/isaaclab_contrib/conveyors/newton.py index 52e4c5b8fcfa..61845a1bcdb0 100644 --- a/source/isaaclab_contrib/isaaclab_contrib/conveyors/newton.py +++ b/source/isaaclab_contrib/isaaclab_contrib/conveyors/newton.py @@ -19,18 +19,6 @@ import warp as wp from isaaclab_newton.physics import NewtonManager from newton.examples.basic.example_basic_conveyor_forces import ConveyorForceModel -from newton.examples.basic.example_basic_conveyor_forces import ( - Vec3Pair as Vec3Pair, -) -from newton.examples.basic.example_basic_conveyor_forces import ( - compute_basis_vectors as compute_basis_vectors, -) -from newton.examples.basic.example_basic_conveyor_forces import ( - compute_point_force as compute_point_force, -) -from newton.examples.basic.example_basic_conveyor_forces import ( - compute_point_impulse as compute_point_impulse, -) from isaaclab.physics import PhysicsEvent @@ -68,16 +56,6 @@ def _advance_startup_scale( scale[0] = wp.min(1.0, elapsed[0] / duration) -@wp.kernel -def _integrate_encoders( - dt: wp.float32, - effective_velocity: wp.array[wp.float32], - position: wp.array[wp.float32], -): - conveyor_id = wp.tid() - position[conveyor_id] += dt * effective_velocity[conveyor_id] - - @wp.kernel def _update_effective_velocities( commanded_velocity: wp.array[wp.float32], @@ -89,20 +67,10 @@ def _update_effective_velocities( @wp.kernel -def _gather_float_values( - source: wp.array[wp.float32], +def _gather_values( + source: wp.array(dtype=Any), indices: wp.array[wp.int32], - values: wp.array[wp.float32], -): - output_id = wp.tid() - values[output_id] = source[indices[output_id]] - - -@wp.kernel -def _gather_int_values( - source: wp.array[wp.int32], - indices: wp.array[wp.int32], - values: wp.array[wp.int32], + values: wp.array(dtype=Any), ): output_id = wp.tid() values[output_id] = source[indices[output_id]] @@ -120,25 +88,6 @@ def _clear_selected_body_forces( body_force[body_id] = wp.spatial_vector() -@wp.kernel -def _clear_selected_encoders( - conveyor_world: wp.array[wp.int32], - world_mask: wp.array[wp.bool], - encoder_position: wp.array[wp.float32], -): - conveyor_id = wp.tid() - world_id = conveyor_world[conveyor_id] - if world_mask[world_id]: - encoder_position[conveyor_id] = 0.0 - - -def _require_buffer_length(name: str, buffer: Any, expected: int) -> None: - """Reject missing or mis-sized buffers before a Warp launch can access them.""" - actual = len(buffer) if buffer is not None else 0 - if actual != expected: - raise RuntimeError(f"Conveyor force buffer {name!r} has length {actual}, expected {expected}.") - - def _as_numpy(values: Any) -> np.ndarray: """Convert supported tensor-like values to a host NumPy array.""" if hasattr(values, "detach"): @@ -210,7 +159,7 @@ def _validate_newton_surface_specs(surface_specs: Sequence[SurfaceVelocitySpec]) class SurfaceVelocity: - """Adapt Newton's MuJoCo conveyor force model to the Isaac Lab lifecycle. + """Adapt Newton's upstream conveyor force model to the Isaac Lab lifecycle. The driver is created after the simulation context but before its first reset. It requests solved contact forces before model finalization, then @@ -308,11 +257,6 @@ def num_surfaces(self) -> int: """Total number of resolved surfaces across all environments.""" return self._num_envs * self.surfaces_per_env - @property - def count(self) -> int: - """Alias for :attr:`num_surfaces`, matching tensor-view naming.""" - return self.num_surfaces - @property def initialized(self) -> bool: """Whether the driver is bound to the active Newton solver.""" @@ -323,11 +267,6 @@ def prim_paths(self) -> tuple[str, ...]: """Resolved Newton shape labels in environment-major belt order.""" return self._require_binding().surface_paths - @property - def surface_paths(self) -> tuple[str, ...]: - """Alias for :attr:`prim_paths`.""" - return self.prim_paths - def set_velocities(self, velocities: Any, indices: Any = None) -> None: """Set signed surface speeds, preserving commands while surfaces are disabled.""" self._require_binding().set_velocities(velocities, indices) @@ -348,12 +287,8 @@ def get_enabled(self, indices: Any = None, clone: bool = True) -> wp.array: """Return integer enabled flags for selected surfaces.""" return self._require_binding().get_enabled(indices, clone) - def get_encoder_positions(self, indices: Any = None, clone: bool = True) -> wp.array: - """Return physics-rate integrated surface travel distances [m].""" - return self._require_binding().get_encoder_positions(indices, clone) - def reset(self, env_ids: Any = None) -> None: - """Clear stale force and encoder state for selected environments.""" + """Clear stale force state for selected environments.""" self._require_binding().reset(env_ids) def _request_contact_forces(self, _event: Any) -> None: @@ -418,18 +353,8 @@ def __init__( startup_duration_s: Duration of the initial traction ramp [s]. env_path_format: Format string resolving one exact environment root from its integer world index. """ - if not isinstance(num_envs, int) or isinstance(num_envs, bool) or num_envs <= 0: - raise ValueError(f"num_envs must be a positive integer, got {num_envs!r}.") - if not np.isfinite(startup_duration_s) or startup_duration_s <= 0.0: - raise ValueError(f"Conveyor startup duration must be positive, got {startup_duration_s}.") - _validate_env_path_format(env_path_format) - self._surface_specs = tuple(surface_specs) - _validate_newton_surface_specs(self._surface_specs) - try: - compiled_body_pattern = re.compile(body_pattern) - except re.error as exc: - raise ValueError(f"Invalid body pattern: {body_pattern!r}.") from exc + compiled_body_pattern = re.compile(body_pattern) if model is None or contacts is None: raise RuntimeError("The conveyor driver requires an initialized Newton model and contact buffer.") @@ -447,7 +372,6 @@ def __init__( self._num_envs = num_envs self._startup_duration_s = startup_duration_s self._closed = False - self._validate_backend_buffers() surfaces_per_env = len(self._surface_specs) conveyor_count = num_envs * surfaces_per_env @@ -456,7 +380,6 @@ def __init__( pivot_point = [wp.vec3() for _ in range(conveyor_count)] radius = [1.0] * conveyor_count surface_normal = [wp.vec3() for _ in range(conveyor_count)] - conveyor_world = [conveyor_id // surfaces_per_env for conveyor_id in range(conveyor_count)] surface_paths = [""] * conveyor_count shape_body = model.shape_body.numpy() @@ -548,7 +471,6 @@ def __init__( self._body_is_tracked = wp.array(body_is_tracked, dtype=wp.int32, device=self._device) self._direction = wp.array(direction, dtype=wp.vec3, device=self._device) self._radius = wp.array(radius, dtype=wp.float32, device=self._device) - self._conveyor_world = wp.array(conveyor_world, dtype=wp.int32, device=self._device) authored_velocity = np.asarray([spec.velocity for spec in self._surface_specs], dtype=np.float32) authored_enabled = np.asarray([spec.enabled for spec in self._surface_specs], dtype=np.int32) @@ -557,7 +479,6 @@ def __init__( self._command_velocity = wp.array(self._command_velocity_host, dtype=wp.float32, device=self._device) self._enabled = wp.array(self._enabled_host, dtype=wp.int32, device=self._device) self._effective_velocity = wp.zeros(conveyor_count, dtype=wp.float32, device=self._device) - self._encoder_position = wp.zeros(conveyor_count, dtype=wp.float32, device=self._device) self._elapsed_time = wp.zeros(1, dtype=wp.float32, device=self._device) self._conveyor.set_speed_scale(0.0) @@ -600,14 +521,10 @@ def set_enabled(self, flags: Any, indices: Any = None) -> None: def get_enabled(self, indices: Any = None, clone: bool = True) -> wp.array: """Return integer enabled flags for selected surfaces.""" - return self._get_device_int_values(self._enabled, indices, clone) - - def get_encoder_positions(self, indices: Any = None, clone: bool = True) -> wp.array: - """Return physics-rate integrated surface travel distances [m].""" - return self._get_device_values(self._encoder_position, indices, clone) + return self._get_device_values(self._enabled, indices, clone) def reset(self, env_ids: Any = None) -> None: - """Clear stale force and encoder state for selected environments. + """Clear stale force state for selected environments. A full reset also restarts the global startup ramp. Partial vectorized resets leave other environments' conveyor forces and startup state intact. @@ -617,7 +534,6 @@ def reset(self, env_ids: Any = None) -> None: """ if env_ids is None: self._conveyor.conveyor_body_f.zero_() - self._encoder_position.zero_() self._elapsed_time.zero_() self._conveyor.global_velocity_scale.zero_() return @@ -641,13 +557,6 @@ def reset(self, env_ids: Any = None) -> None: outputs=[self._conveyor.conveyor_body_f], device=self._device, ) - wp.launch( - _clear_selected_encoders, - dim=len(self._surface_paths), - inputs=[self._conveyor_world, self._world_mask], - outputs=[self._encoder_position], - device=self._device, - ) def close(self) -> None: """Deregister Newton callbacks and release references held by the driver.""" @@ -662,7 +571,7 @@ def apply(self, state) -> None: self._conveyor.apply(state) def update(self, solver, contacts, state, dt: float) -> None: - """Update surface travel and let Newton compute the next conveyor wrench.""" + """Advance the startup ramp and let Newton compute the next conveyor wrench.""" wp.launch( _advance_startup_scale, dim=1, @@ -670,13 +579,6 @@ def update(self, solver, contacts, state, dt: float) -> None: outputs=[self._elapsed_time, self._conveyor.global_velocity_scale], device=self._device, ) - wp.launch( - _integrate_encoders, - dim=len(self._surface_paths), - inputs=[dt, self._effective_velocity], - outputs=[self._encoder_position], - device=self._device, - ) self._conveyor.update(solver, contacts, state, dt) # Exclude robot links and other bodies outside the transported-body selection. wp.launch( @@ -687,41 +589,6 @@ def update(self, solver, contacts, state, dt: float) -> None: device=self._device, ) - def _validate_backend_buffers(self) -> None: - """Validate every fixed-size Newton buffer consumed by conveyor kernels.""" - model = self._model - contacts = self._contacts - _require_buffer_length("model.shape_body", model.shape_body, model.shape_count) - _require_buffer_length("model.shape_world", model.shape_world, model.shape_count) - _require_buffer_length("model.shape_transform", model.shape_transform, model.shape_count) - _require_buffer_length("model.body_world", model.body_world, model.body_count) - _require_buffer_length("model.body_com", model.body_com, model.body_count) - _require_buffer_length("model.body_inv_mass", model.body_inv_mass, model.body_count) - _require_buffer_length("model.body_inv_inertia", model.body_inv_inertia, model.body_count) - _require_buffer_length("contacts.force", contacts.force, contacts.rigid_contact_max) - _require_buffer_length( - "contacts.rigid_contact_shape0", contacts.rigid_contact_shape0, contacts.rigid_contact_max - ) - _require_buffer_length( - "contacts.rigid_contact_shape1", contacts.rigid_contact_shape1, contacts.rigid_contact_max - ) - _require_buffer_length( - "contacts.rigid_contact_normal", contacts.rigid_contact_normal, contacts.rigid_contact_max - ) - _require_buffer_length( - "contacts.rigid_contact_point0", contacts.rigid_contact_point0, contacts.rigid_contact_max - ) - _require_buffer_length( - "contacts.rigid_contact_point1", contacts.rigid_contact_point1, contacts.rigid_contact_max - ) - _require_buffer_length( - "contacts.rigid_contact_offset0", contacts.rigid_contact_offset0, contacts.rigid_contact_max - ) - _require_buffer_length( - "contacts.rigid_contact_offset1", contacts.rigid_contact_offset1, contacts.rigid_contact_max - ) - _require_buffer_length("contacts.rigid_contact_count", contacts.rigid_contact_count, 1) - def _resolve_indices(self, indices: Any) -> np.ndarray: """Normalize and validate a surface index selection.""" if indices is None: @@ -763,27 +630,10 @@ def _get_device_values(self, source: wp.array, indices: Any, clone: bool) -> wp. return wp.clone(source) if clone else source selected = self._resolve_indices(indices) selected_device = wp.array(selected, dtype=wp.int32, device=self._device) - values = wp.empty(len(selected), dtype=wp.float32, device=self._device) - if len(selected) > 0: - wp.launch( - _gather_float_values, - dim=len(selected), - inputs=[source, selected_device], - outputs=[values], - device=self._device, - ) - return values - - def _get_device_int_values(self, source: wp.array, indices: Any, clone: bool) -> wp.array: - """Clone an integer device buffer or gather a selected subset.""" - if indices is None: - return wp.clone(source) if clone else source - selected = self._resolve_indices(indices) - selected_device = wp.array(selected, dtype=wp.int32, device=self._device) - values = wp.empty(len(selected), dtype=wp.int32, device=self._device) + values = wp.empty(len(selected), dtype=source.dtype, device=self._device) if len(selected) > 0: wp.launch( - _gather_int_values, + _gather_values, dim=len(selected), inputs=[source, selected_device], outputs=[values], diff --git a/source/isaaclab_contrib/isaaclab_contrib/conveyors/physx.py b/source/isaaclab_contrib/isaaclab_contrib/conveyors/physx.py index 93622a53f039..c492fdc9389b 100644 --- a/source/isaaclab_contrib/isaaclab_contrib/conveyors/physx.py +++ b/source/isaaclab_contrib/isaaclab_contrib/conveyors/physx.py @@ -18,7 +18,7 @@ import math from collections.abc import Sequence from dataclasses import dataclass -from typing import Any, Protocol +from typing import Any import numpy as np @@ -167,7 +167,7 @@ class SurfaceVelocity: The facade binds exact environment-major paths whose schemas were already authored by :func:`apply_surface_velocity_api`. Call :meth:`start` to register physics-rate updates, or call :meth:`update` manually. Commands and enabled state survive full resets; a full reset - clears encoders and restarts the one-second startup ramp. This facade authors USD attributes + restarts the startup ramp. This facade authors USD attributes on the host and is not a GPU conveyor implementation. """ @@ -179,7 +179,6 @@ def __init__( env_path_format: str = "/World/envs/env_{}", startup_duration_s: float = 1.0, stage: Any | None = None, - writer: Any | None = None, ) -> None: """Bind native surface attributes and initialize host-side control state. @@ -189,8 +188,6 @@ def __init__( env_path_format: Exact replicated environment path format. startup_duration_s: Duration of the global surface-speed ramp [s]. stage: Optional USD stage used by the default schema writer. - writer: Optional writer implementing ``write(index, enabled=..., twist=...)`` and - ``close()``. This import-light seam is primarily for focused tests. """ duration = _finite_float("startup_duration_s", startup_duration_s) if duration <= 0.0: @@ -204,11 +201,7 @@ def __init__( self._velocity_scale = 0.0 self._closed = False self._callback_handle: Any | None = None - self._writer: _SurfaceVelocityWriter = ( - writer - if writer is not None - else _PhysxSchemaSurfaceWriter(self._surface_paths, stage=stage, apply_api=False) - ) + self._writer = _PhysxSchemaSurfaceWriter(self._surface_paths, stage=stage, apply_api=False) self._command_velocity = np.tile( np.asarray([spec.velocity for spec in self._surface_specs], dtype=np.float32), self._num_envs @@ -216,7 +209,6 @@ def __init__( self._enabled = np.tile( np.asarray([spec.enabled for spec in self._surface_specs], dtype=np.bool_), self._num_envs ) - self._encoder_position = np.zeros(self.num_surfaces, dtype=np.float32) self._last_authored: list[tuple[bool, PhysxSurfaceVelocityTwist] | None] = [None] * self.num_surfaces self._flush(force=True) @@ -235,21 +227,11 @@ def prim_paths(self) -> tuple[str, ...]: """Return exact PhysX prim paths in environment-major order.""" return self._surface_paths - @property - def surface_paths(self) -> tuple[str, ...]: - """Return an alias for :attr:`prim_paths`.""" - return self.prim_paths - @property def num_surfaces(self) -> int: """Return the total number of bound conveyor surfaces.""" return len(self._surface_paths) - @property - def count(self) -> int: - """Return an alias for :attr:`num_surfaces`.""" - return self.num_surfaces - @property def initialized(self) -> bool: """Return whether the facade still owns live schema bindings.""" @@ -269,7 +251,7 @@ def start(self) -> None: ) def update(self, dt: float) -> None: - """Advance encoder state and the startup ramp by one physics step. + """Advance the startup ramp by one physics step. Args: dt: Positive physics step duration [s]. @@ -278,8 +260,6 @@ def update(self, dt: float) -> None: step = _finite_float("physics dt", dt) if step <= 0.0: raise ValueError(f"Conveyor physics dt must be positive, got {dt!r}.") - effective_velocity = self._command_velocity * self._enabled - self._encoder_position += np.asarray(step * effective_velocity, dtype=np.float32) self._elapsed_time = min(self._startup_duration_s, self._elapsed_time + step) self._velocity_scale = self._elapsed_time / self._startup_duration_s self._flush() @@ -317,12 +297,8 @@ def get_enabled(self, indices: Any = None, clone: bool = True) -> np.ndarray: """Return enabled flags as integer values.""" return self._get_values(self._enabled.astype(np.int32), indices, clone) - def get_encoder_positions(self, indices: Any = None, clone: bool = True) -> np.ndarray: - """Return physics-rate integrated commanded belt travel [m].""" - return self._get_values(self._encoder_position, indices, clone) - def reset(self, env_ids: Any = None) -> None: - """Clear selected encoders and restart the global ramp on a full reset. + """Restart the global startup ramp on a full reset. Velocity commands and enabled state are deliberately preserved, including across a full hard-reset path. Partial resets do not disturb the global ramp used by other environments. @@ -332,8 +308,6 @@ def reset(self, env_ids: Any = None) -> None: """ self._require_open() ids = self._resolve_env_ids(env_ids) - rows = (ids[:, None] * self._surfaces_per_env + np.arange(self._surfaces_per_env)[None, :]).reshape(-1) - self._encoder_position[rows] = 0.0 if len(np.unique(ids)) == self._num_envs: self._elapsed_time = 0.0 self._velocity_scale = 0.0 @@ -426,18 +400,6 @@ def _require_open(self) -> None: raise RuntimeError("The PhysX conveyor surface facade is closed.") -class _SurfaceVelocityWriter(Protocol): - """Minimal schema-writer seam used by the runtime facade.""" - - def write(self, index: int, *, enabled: bool, twist: PhysxSurfaceVelocityTwist) -> None: - """Author one bound surface state.""" - ... - - def close(self) -> None: - """Release retained schema attributes.""" - ... - - class _PhysxSchemaSurfaceWriter: """Cached USD attribute writer with all schema imports kept lazy.""" diff --git a/source/isaaclab_contrib/isaaclab_contrib/conveyors/surface_velocity.py b/source/isaaclab_contrib/isaaclab_contrib/conveyors/surface_velocity.py index f00badb6b941..b5251657958b 100644 --- a/source/isaaclab_contrib/isaaclab_contrib/conveyors/surface_velocity.py +++ b/source/isaaclab_contrib/isaaclab_contrib/conveyors/surface_velocity.py @@ -63,7 +63,7 @@ class SurfaceVelocitySpec: The fields follow the authored conveyor model proposed for Isaac Sim while remaining independent of Kit, OpenUSD, and any physics backend. Directions, surface normals, and the optional pivot point are - expressed in the collision prim's local frame. Runtime state such as encoder positions and applied + expressed in the collision prim's local frame. Runtime state such as commanded speeds and applied forces intentionally does not belong in this description. ``prim_path`` may contain Isaac Lab's ``{ENV_REGEX_NS}`` placeholder. A backend resolves that template @@ -141,11 +141,6 @@ def num_surfaces(self) -> int: """Number of resolved moving surfaces.""" ... - @property - def count(self) -> int: - """Alias for :attr:`num_surfaces`, matching tensor-view naming.""" - ... - def set_velocities(self, velocities: Any, indices: Any = None) -> None: """Set signed surface velocities [m/s] for selected surfaces.""" ... @@ -166,10 +161,6 @@ def get_enabled(self, indices: Any = None, clone: bool = True) -> Any: """Return integer enabled flags for selected surfaces.""" ... - def get_encoder_positions(self, indices: Any = None, clone: bool = True) -> Any: - """Return integrated surface travel [m] for selected surfaces.""" - ... - def reset(self, env_ids: Any = None) -> None: """Clear runtime state for selected replicated environments.""" ... diff --git a/source/isaaclab_contrib/test/conveyors/test_surface_velocity_newton.py b/source/isaaclab_contrib/test/conveyors/test_surface_velocity_newton.py index 9bea66335600..365358ad6929 100644 --- a/source/isaaclab_contrib/test/conveyors/test_surface_velocity_newton.py +++ b/source/isaaclab_contrib/test/conveyors/test_surface_velocity_newton.py @@ -89,15 +89,15 @@ def initialized(model, contacts): driver.set_velocities([0.3, -0.25, -0.4], indices=[0, 1, 3]) driver.set_enabled([False, True], indices=[2, 3]) np.testing.assert_allclose(driver.get_velocities().numpy(), [0.3, 0, 0, -0.4]) + np.testing.assert_allclose(driver.get_velocities(indices=[3, 0]).numpy(), [-0.4, 0.3]) + np.testing.assert_array_equal(driver.get_enabled(indices=[2, 0]).numpy(), [0, 1]) sim.step(render=False) assert calls == ["actuator"] + ["force", "substep"] * cfg.physics.num_substeps - np.testing.assert_allclose(driver.get_encoder_positions().numpy(), np.array([0.3, 0, 0, -0.4]) / 120) first._conveyor.conveyor_body_f.assign(np.ones((2, 6), dtype=np.float32)) driver.reset([0]) state = model.state() first.apply(state) np.testing.assert_array_equal(state.body_f.numpy(), [[0] * 6, [1] * 6]) - np.testing.assert_allclose(driver.get_encoder_positions().numpy(), [0, 0, 0, -0.4 / 120]) sim.reset() assert first._closed and driver._binding is not first assert driver._binding._model is NewtonManager.get_model() diff --git a/source/isaaclab_contrib/test/conveyors/test_surface_velocity_physx.py b/source/isaaclab_contrib/test/conveyors/test_surface_velocity_physx.py index 91348d447553..81d992469aae 100644 --- a/source/isaaclab_contrib/test/conveyors/test_surface_velocity_physx.py +++ b/source/isaaclab_contrib/test/conveyors/test_surface_velocity_physx.py @@ -87,30 +87,29 @@ def test_paths_are_resolved_in_environment_major_order() -> None: surface_module.resolve_surface_velocity_paths(2, (SurfaceVelocitySpec(prim_path="/World/Shared/Belt"),)) -def test_facade_ramps_playback_integrates_encoders_and_preserves_commands_on_reset() -> None: +def test_facade_ramps_playback_and_preserves_commands_on_reset(monkeypatch: pytest.MonkeyPatch) -> None: """Full resets restart playback without erasing policy-visible command state.""" writer = _FakeWriter() - facade = surface_module.SurfaceVelocity(2, (_surface_spec(),), writer=writer) + monkeypatch.setattr(surface_module, "_PhysxSchemaSurfaceWriter", lambda *args, **kwargs: writer) + facade = surface_module.SurfaceVelocity(2, (_surface_spec(),)) assert facade.prim_paths == ("/World/envs/env_0/Belt", "/World/envs/env_1/Belt") - assert facade.num_surfaces == facade.count == 2 + assert facade.num_surfaces == 2 assert [record[2].linear_velocity for record in writer.writes] == [(0.0, 0.0, 0.0)] * 2 facade.update(0.25) np.testing.assert_allclose([record[2].linear_velocity[0] for record in writer.writes[-2:]], (0.1, 0.1)) - np.testing.assert_allclose(facade.get_encoder_positions(), (0.1, 0.1)) facade.set_velocities(torch.tensor([0.8], requires_grad=True), indices=torch.tensor([0])) facade.set_enabled(False, indices=[1]) facade.update(0.25) np.testing.assert_allclose(facade.get_commanded_velocities(), (0.8, 0.4)) np.testing.assert_allclose(facade.get_velocities(), (0.8, 0.0)) - np.testing.assert_allclose(facade.get_encoder_positions(), (0.3, 0.1)) + writes_before_reset = list(writer.writes) facade.reset(env_ids=torch.tensor([0])) - np.testing.assert_allclose(facade.get_encoder_positions(), (0.0, 0.1)) + assert writer.writes == writes_before_reset facade.reset(env_ids=[1, 0]) - np.testing.assert_allclose(facade.get_encoder_positions(), (0.0, 0.0)) np.testing.assert_allclose(facade.get_commanded_velocities(), (0.8, 0.4)) assert facade.get_enabled().tolist() == [1, 0] np.testing.assert_allclose(writer.writes[-2][2].linear_velocity, (0.0, 0.0, 0.0)) From 1c91d6ea1b8afe8e5899c986aadeec9a4a9c7d81 Mon Sep 17 00:00:00 2001 From: Maximilian Krause Date: Thu, 8 Oct 2026 13:50:41 +0200 Subject: [PATCH 3/9] Remove conveyor test-only lifecycle seams --- .../isaaclab_contrib/conveyors/newton.py | 1 - .../test/conveyors/test_surface_velocity_newton.py | 4 ++-- .../isaaclab_newton/physics/newton_manager.py | 14 -------------- 3 files changed, 2 insertions(+), 17 deletions(-) diff --git a/source/isaaclab_contrib/isaaclab_contrib/conveyors/newton.py b/source/isaaclab_contrib/isaaclab_contrib/conveyors/newton.py index 61845a1bcdb0..897199ba92ba 100644 --- a/source/isaaclab_contrib/isaaclab_contrib/conveyors/newton.py +++ b/source/isaaclab_contrib/isaaclab_contrib/conveyors/newton.py @@ -367,7 +367,6 @@ def __init__( raise RuntimeError(f"Newton model has {model.world_count} worlds, expected {num_envs}.") self._model = model - self._contacts = contacts self._device = model.device self._num_envs = num_envs self._startup_duration_s = startup_duration_s diff --git a/source/isaaclab_contrib/test/conveyors/test_surface_velocity_newton.py b/source/isaaclab_contrib/test/conveyors/test_surface_velocity_newton.py index 365358ad6929..a8796a83d297 100644 --- a/source/isaaclab_contrib/test/conveyors/test_surface_velocity_newton.py +++ b/source/isaaclab_contrib/test/conveyors/test_surface_velocity_newton.py @@ -57,8 +57,8 @@ def substep(solver, contacts, state, dt): def initialized(model, contacts): bindings.append((model, contacts)) + NewtonManager.register_post_actuator_callback(actuator) observers = ( - (actuator, NewtonManager.register_post_actuator_callback, NewtonManager.unregister_post_actuator_callback), (force, NewtonManager.register_state_force_callback, NewtonManager.unregister_state_force_callback), ( substep, @@ -101,13 +101,13 @@ def initialized(model, contacts): sim.reset() assert first._closed and driver._binding is not first assert driver._binding._model is NewtonManager.get_model() - assert driver._binding._contacts is NewtonManager.get_contacts() np.testing.assert_allclose(driver.get_velocities().numpy(), [0.3, 0, 0, -0.4]) assert len(bindings) == 2 and bindings[1] == (NewtonManager.get_model(), NewtonManager.get_contacts()) assert bindings[0][0] is not bindings[1][0] and bindings[0][1] is not bindings[1][1] for callback, _, unregister in observers: unregister(callback) unregister(callback) + NewtonManager._post_actuator_callbacks.remove(actuator) previous_calls = list(calls) sim.step(render=False) sim.reset() diff --git a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py index bfbe8a2d506f..9ee451d09947 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py @@ -1935,20 +1935,6 @@ def unregister_state_force_callback(cls, callback: Callable[[State], None]) -> N with contextlib.suppress(ValueError): NewtonManager._state_force_callbacks.remove(callback) - @classmethod - def unregister_post_actuator_callback(cls, callback: Callable[[], None]) -> None: - """Remove a previously registered post-actuator callback. - - Removing a callback that was never registered or was already removed is - a safe no-op. This lets scene-owned systems release bound-method - references before the global Newton manager is cleared. - - Args: - callback: Previously registered callback. - """ - with contextlib.suppress(ValueError): - cls._post_actuator_callbacks.remove(callback) - @classmethod def register_post_solver_substep_callback( cls, callback: Callable[[SolverBase, Contacts | None, State, float], None] From ffbfb8d31be120939b6e1283930b471eaf0078ce Mon Sep 17 00:00:00 2001 From: Maximilian Krause Date: Thu, 8 Oct 2026 14:13:12 +0200 Subject: [PATCH 4/9] Share conveyor path resolution across backends --- .../isaaclab_contrib/conveyors/newton.py | 41 +++------------- .../isaaclab_contrib/conveyors/physx.py | 48 +------------------ .../conveyors/surface_velocity.py | 48 +++++++++++++++++++ .../conveyors/test_surface_velocity_newton.py | 14 +++--- .../conveyors/test_surface_velocity_physx.py | 16 ------- .../conveyors/test_surface_velocity_spec.py | 19 +++++++- 6 files changed, 81 insertions(+), 105 deletions(-) diff --git a/source/isaaclab_contrib/isaaclab_contrib/conveyors/newton.py b/source/isaaclab_contrib/isaaclab_contrib/conveyors/newton.py index 897199ba92ba..5e82df8a9dc4 100644 --- a/source/isaaclab_contrib/isaaclab_contrib/conveyors/newton.py +++ b/source/isaaclab_contrib/isaaclab_contrib/conveyors/newton.py @@ -22,7 +22,7 @@ from isaaclab.physics import PhysicsEvent -from .surface_velocity import SurfaceVelocitySpec +from .surface_velocity import SurfaceVelocitySpec, resolve_surface_velocity_paths @wp.kernel @@ -122,33 +122,14 @@ def _world_point(transform_values: np.ndarray, local_point: tuple[float, float, return wp.vec3(*(float(point[index]) for index in range(3))) -def _resolve_belt_prim_path(prim_path: str, env_path_format: str, world_id: int) -> str: - """Resolve one replicated conveyor template to an exact Newton shape label.""" - if prim_path.startswith("{ENV_REGEX_NS}/"): - return prim_path.format(ENV_REGEX_NS=env_path_format.format(world_id)) - return prim_path - - def _shape_belongs_to_prim(shape_label: str, prim_path: str) -> bool: """Return whether a Newton collision-shape label is the prim or one of its descendants.""" return shape_label == prim_path or shape_label.startswith(f"{prim_path.rstrip('/')}/") -def _validate_env_path_format(env_path_format: str) -> None: - """Validate the concrete per-world path format derived from the scene cloner.""" - if not isinstance(env_path_format, str) or env_path_format.count("{}") != 1: - raise ValueError(f"Conveyor env_path_format must contain exactly one '{{}}', got {env_path_format!r}.") - if not env_path_format.startswith("/"): - raise ValueError(f"Conveyor env_path_format must be absolute, got {env_path_format!r}.") - - def _validate_newton_surface_specs(surface_specs: Sequence[SurfaceVelocitySpec]) -> None: """Validate Newton-specific requirements before registering lifecycle callbacks.""" - if not surface_specs: - raise ValueError("At least one surface-velocity specification is required.") prim_paths = [spec.prim_path for spec in surface_specs] - if len(set(prim_paths)) != len(prim_paths): - raise ValueError(f"Conveyor prim paths must be unique, got {prim_paths}.") for index, path in enumerate(prim_paths): for other in prim_paths[index + 1 :]: if _shape_belongs_to_prim(path, other) or _shape_belongs_to_prim(other, path): @@ -190,18 +171,10 @@ def __init__( env_path_format: Format string resolving one exact environment root from its integer world index. """ surface_specs = tuple(surface_specs) - if not all(isinstance(spec, SurfaceVelocitySpec) for spec in surface_specs): - raise TypeError("Every surface specification must be a SurfaceVelocitySpec.") + prim_paths = resolve_surface_velocity_paths(num_envs, surface_specs, env_path_format) _validate_newton_surface_specs(surface_specs) - if not isinstance(num_envs, int) or isinstance(num_envs, bool) or num_envs <= 0: - raise ValueError(f"num_envs must be a positive integer, got {num_envs!r}.") - if num_envs > 1 and any(not spec.prim_path.startswith("{ENV_REGEX_NS}/") for spec in surface_specs): - raise ValueError( - "Replicated conveyor environments require every belt prim_path to start with '{ENV_REGEX_NS}/'." - ) if not np.isfinite(startup_duration_s) or startup_duration_s <= 0.0: raise ValueError(f"Conveyor startup duration must be positive, got {startup_duration_s}.") - _validate_env_path_format(env_path_format) if not isinstance(body_pattern, str): raise ValueError(f"body_pattern must be a regular-expression string, got {body_pattern!r}.") try: @@ -220,7 +193,7 @@ def __init__( "num_envs": num_envs, "surface_specs": self._surface_specs, "startup_duration_s": startup_duration_s, - "env_path_format": env_path_format, + "prim_paths": prim_paths, "body_pattern": body_pattern, "body_count_per_env": body_count_per_env, } @@ -339,7 +312,7 @@ def __init__( body_pattern: str, body_count_per_env: int | None = None, startup_duration_s: float = 1.0, - env_path_format: str = "/World/envs/env_{}", + prim_paths: tuple[str, ...], ) -> None: """Initialize the binding before Newton CUDA graph capture. @@ -351,7 +324,7 @@ def __init__( body_pattern: Regular expression selecting bodies that receive surface traction. body_count_per_env: Expected selected body count per environment, or ``None``. startup_duration_s: Duration of the initial traction ramp [s]. - env_path_format: Format string resolving one exact environment root from its integer world index. + prim_paths: Exact collision prim paths in environment-major order. """ self._surface_specs = tuple(surface_specs) compiled_body_pattern = re.compile(body_pattern) @@ -391,8 +364,8 @@ def __init__( continue matching_specs = [ index - for index, spec in enumerate(self._surface_specs) - if _shape_belongs_to_prim(label, _resolve_belt_prim_path(spec.prim_path, env_path_format, world_id)) + for index in range(surfaces_per_env) + if _shape_belongs_to_prim(label, prim_paths[world_id * surfaces_per_env + index]) ] if not matching_specs: continue diff --git a/source/isaaclab_contrib/isaaclab_contrib/conveyors/physx.py b/source/isaaclab_contrib/isaaclab_contrib/conveyors/physx.py index c492fdc9389b..1d85f1cd08a8 100644 --- a/source/isaaclab_contrib/isaaclab_contrib/conveyors/physx.py +++ b/source/isaaclab_contrib/isaaclab_contrib/conveyors/physx.py @@ -22,9 +22,7 @@ import numpy as np -from isaaclab_contrib.conveyors.surface_velocity import SurfaceVelocitySpec - -_ENV_REGEX_NS = "{ENV_REGEX_NS}" +from .surface_velocity import SurfaceVelocitySpec, resolve_surface_velocity_paths @dataclass(frozen=True, slots=True) @@ -117,50 +115,6 @@ def apply_surface_velocity_api( binding.close() -def resolve_surface_velocity_paths( - num_envs: int, - surface_specs: Sequence[SurfaceVelocitySpec], - env_path_format: str = "/World/envs/env_{}", -) -> tuple[str, ...]: - """Resolve conveyor templates to exact environment-major prim paths. - - Args: - num_envs: Number of replicated environments. - surface_specs: Within-environment surface descriptions. - env_path_format: Exact environment path format containing one ``{}`` field. - - Returns: - Exact paths ordered by environment, then by ``surface_specs`` order. - - Raises: - ValueError: If inputs cannot produce one unique path per environment and belt. - """ - if not isinstance(num_envs, int) or isinstance(num_envs, bool) or num_envs <= 0: - raise ValueError(f"Conveyor num_envs must be a positive integer, got {num_envs!r}.") - specs = tuple(surface_specs) - if not specs or not all(isinstance(spec, SurfaceVelocitySpec) for spec in specs): - raise ValueError("surface_specs must contain at least one SurfaceVelocitySpec.") - if not isinstance(env_path_format, str) or env_path_format.count("{}") != 1: - raise ValueError(f"Conveyor env_path_format must contain exactly one '{{}}', got {env_path_format!r}.") - try: - env_paths = tuple(env_path_format.format(env_id) for env_id in range(num_envs)) - except (IndexError, KeyError, ValueError) as exc: - raise ValueError(f"Invalid conveyor env_path_format: {env_path_format!r}.") from exc - if any(not path.startswith("/") or "{" in path or "}" in path for path in env_paths): - raise ValueError(f"Conveyor env_path_format must produce exact absolute paths, got {env_path_format!r}.") - - templates = tuple(spec.prim_path for spec in specs) - if len(set(templates)) != len(templates): - raise ValueError("Conveyor belt spec paths must be unique within an environment.") - if num_envs > 1 and any(_ENV_REGEX_NS not in path for path in templates): - raise ValueError("Replicated PhysX conveyors require every belt prim_path to use {ENV_REGEX_NS}.") - - paths = tuple(template.replace(_ENV_REGEX_NS, env_path) for env_path in env_paths for template in templates) - if len(set(paths)) != len(paths): - raise ValueError("Resolved PhysX conveyor prim paths must be unique.") - return paths - - class SurfaceVelocity: """Host-side CPU reference facade for native PhysX surface velocity. diff --git a/source/isaaclab_contrib/isaaclab_contrib/conveyors/surface_velocity.py b/source/isaaclab_contrib/isaaclab_contrib/conveyors/surface_velocity.py index b5251657958b..b528cc9bf668 100644 --- a/source/isaaclab_contrib/isaaclab_contrib/conveyors/surface_velocity.py +++ b/source/isaaclab_contrib/isaaclab_contrib/conveyors/surface_velocity.py @@ -8,6 +8,7 @@ from __future__ import annotations import math +from collections.abc import Sequence from dataclasses import dataclass from typing import Any, Protocol, runtime_checkable @@ -127,6 +128,53 @@ def __post_init__(self) -> None: object.__setattr__(self, "friction_coefficient", friction_coefficient) +def resolve_surface_velocity_paths( + num_envs: int, + surface_specs: Sequence[SurfaceVelocitySpec], + env_path_format: str = "/World/envs/env_{}", +) -> tuple[str, ...]: + """Resolve conveyor templates to exact environment-major prim paths. + + Args: + num_envs: Number of replicated environments. + surface_specs: Within-environment surface descriptions. + env_path_format: Exact environment path format containing one ``{}`` field. + + Returns: + Exact paths ordered by environment, then by ``surface_specs`` order. + + Raises: + ValueError: If inputs cannot produce one unique path per environment and belt. + TypeError: If a surface description is not a :class:`SurfaceVelocitySpec`. + """ + if not isinstance(num_envs, int) or isinstance(num_envs, bool) or num_envs <= 0: + raise ValueError(f"Conveyor num_envs must be a positive integer, got {num_envs!r}.") + specs = tuple(surface_specs) + if not specs: + raise ValueError("At least one SurfaceVelocitySpec is required.") + if not all(isinstance(spec, SurfaceVelocitySpec) for spec in specs): + raise TypeError("Every surface specification must be a SurfaceVelocitySpec.") + if not isinstance(env_path_format, str) or env_path_format.count("{}") != 1: + raise ValueError(f"Conveyor env_path_format must contain exactly one '{{}}', got {env_path_format!r}.") + try: + env_paths = tuple(env_path_format.format(env_id) for env_id in range(num_envs)) + except (IndexError, KeyError, ValueError) as exc: + raise ValueError(f"Invalid conveyor env_path_format: {env_path_format!r}.") from exc + if any(not path.startswith("/") or "{" in path or "}" in path for path in env_paths): + raise ValueError(f"Conveyor env_path_format must produce exact absolute paths, got {env_path_format!r}.") + + templates = tuple(spec.prim_path for spec in specs) + if len(set(templates)) != len(templates): + raise ValueError("Conveyor belt spec paths must be unique within an environment.") + if num_envs > 1 and any(_ENV_REGEX_NS not in path for path in templates): + raise ValueError("Replicated conveyors require every belt prim_path to use {ENV_REGEX_NS}.") + + paths = tuple(template.replace(_ENV_REGEX_NS, env_path) for env_path in env_paths for template in templates) + if len(set(paths)) != len(paths): + raise ValueError("Resolved conveyor prim paths must be unique.") + return paths + + @runtime_checkable class SurfaceVelocityView(Protocol): """Common tensorized control contract implemented by physics backends.""" diff --git a/source/isaaclab_contrib/test/conveyors/test_surface_velocity_newton.py b/source/isaaclab_contrib/test/conveyors/test_surface_velocity_newton.py index a8796a83d297..46476138dab3 100644 --- a/source/isaaclab_contrib/test/conveyors/test_surface_velocity_newton.py +++ b/source/isaaclab_contrib/test/conveyors/test_surface_velocity_newton.py @@ -149,6 +149,13 @@ def fail_registration(cls, callback): ((SurfaceVelocitySpec("/Curve", curved=True),), 1, "/World/envs/env_{}", ValueError, "positive radius"), ((SurfaceVelocitySpec("/Shared/Belt"),), 2, "/World/envs/env_{}", ValueError, "Replicated"), ((SurfaceVelocitySpec("{ENV_REGEX_NS}/Belt"),), 1, "/World/envs/env_.*", ValueError, "env_path_format"), + ( + (SurfaceVelocitySpec("{ENV_REGEX_NS}/Belt"),), + 1, + "/World/envs/env_{}/{missing}", + ValueError, + "env_path_format", + ), ], ) def test_invalid_conveyors_leave_no_callbacks(specs, num_envs, env_path, error, message): @@ -157,10 +164,3 @@ def test_invalid_conveyors_leave_no_callbacks(specs, num_envs, env_path, error, with pytest.raises(error, match=message): SurfaceVelocity(num_envs, specs, env_path_format=env_path, body_pattern="Cube0$") assert set(NewtonManager._callbacks) == before - - -def test_unreplicated_absolute_surface_path_is_preserved(): - """A single-environment surface can target a global authored path.""" - from isaaclab_contrib.conveyors.newton import _resolve_belt_prim_path - - assert _resolve_belt_prim_path("/World/Shared/Belt", "/World/envs/env_{}", 0) == "/World/Shared/Belt" diff --git a/source/isaaclab_contrib/test/conveyors/test_surface_velocity_physx.py b/source/isaaclab_contrib/test/conveyors/test_surface_velocity_physx.py index 81d992469aae..a6f229031f58 100644 --- a/source/isaaclab_contrib/test/conveyors/test_surface_velocity_physx.py +++ b/source/isaaclab_contrib/test/conveyors/test_surface_velocity_physx.py @@ -71,22 +71,6 @@ def test_curved_twist_requires_an_explicit_radius() -> None: surface_module.compute_surface_velocity_twist(spec) -def test_paths_are_resolved_in_environment_major_order() -> None: - """Runtime rows stay deterministic across stage discovery ordering.""" - specs = (_surface_spec("BeltA"), _surface_spec("Nested/BeltB")) - - paths = surface_module.resolve_surface_velocity_paths(2, specs) - - assert paths == ( - "/World/envs/env_0/BeltA", - "/World/envs/env_0/Nested/BeltB", - "/World/envs/env_1/BeltA", - "/World/envs/env_1/Nested/BeltB", - ) - with pytest.raises(ValueError, match="require every belt"): - surface_module.resolve_surface_velocity_paths(2, (SurfaceVelocitySpec(prim_path="/World/Shared/Belt"),)) - - def test_facade_ramps_playback_and_preserves_commands_on_reset(monkeypatch: pytest.MonkeyPatch) -> None: """Full resets restart playback without erasing policy-visible command state.""" writer = _FakeWriter() diff --git a/source/isaaclab_contrib/test/conveyors/test_surface_velocity_spec.py b/source/isaaclab_contrib/test/conveyors/test_surface_velocity_spec.py index db0a7adedb90..e34fc0e1d78e 100644 --- a/source/isaaclab_contrib/test/conveyors/test_surface_velocity_spec.py +++ b/source/isaaclab_contrib/test/conveyors/test_surface_velocity_spec.py @@ -9,7 +9,7 @@ import pytest -from isaaclab_contrib.conveyors.surface_velocity import SurfaceVelocitySpec +from isaaclab_contrib.conveyors.surface_velocity import SurfaceVelocitySpec, resolve_surface_velocity_paths def test_surface_velocity_spec_normalizes_vectors() -> None: @@ -43,3 +43,20 @@ def test_surface_velocity_spec_rejects_invalid_authored_values(kwargs: dict, mes """Invalid persistent intent fails before any physics lifecycle is registered.""" with pytest.raises(ValueError, match=message): SurfaceVelocitySpec(**kwargs) + + +def test_paths_are_resolved_in_environment_major_order() -> None: + """Runtime rows stay deterministic across stage discovery ordering.""" + specs = (SurfaceVelocitySpec("{ENV_REGEX_NS}/BeltA"), SurfaceVelocitySpec("{ENV_REGEX_NS}/Nested/BeltB")) + + paths = resolve_surface_velocity_paths(2, specs) + + assert paths == ( + "/World/envs/env_0/BeltA", + "/World/envs/env_0/Nested/BeltB", + "/World/envs/env_1/BeltA", + "/World/envs/env_1/Nested/BeltB", + ) + with pytest.raises(ValueError, match="require every belt"): + resolve_surface_velocity_paths(2, (SurfaceVelocitySpec(prim_path="/World/Shared/Belt"),)) + assert resolve_surface_velocity_paths(1, (SurfaceVelocitySpec("/World/Shared/Belt"),)) == ("/World/Shared/Belt",) From de3d752c5030c7992db78a76ad9ab82d6c80164d Mon Sep 17 00:00:00 2001 From: Maximilian Krause Date: Mon, 12 Oct 2026 12:38:58 +0200 Subject: [PATCH 5/9] Detach conveyor callbacks from captured CUDA graphs --- .../isaaclab_contrib/conveyors/README.md | 4 +- .../isaaclab_contrib/conveyors/newton.py | 2 +- .../isaaclab_contrib/conveyors/physx.py | 8 +--- .../conveyors/test_surface_velocity_newton.py | 38 +++++++++++++++++++ .../physics/newton_collision_cfg.py | 6 +-- .../isaaclab_newton/physics/newton_manager.py | 22 ++++++++--- 6 files changed, 62 insertions(+), 18 deletions(-) diff --git a/source/isaaclab_contrib/isaaclab_contrib/conveyors/README.md b/source/isaaclab_contrib/isaaclab_contrib/conveyors/README.md index 1c5a84cba40b..89f3abf701a6 100644 --- a/source/isaaclab_contrib/isaaclab_contrib/conveyors/README.md +++ b/source/isaaclab_contrib/isaaclab_contrib/conveyors/README.md @@ -8,8 +8,8 @@ experimental and is not exported from Isaac Lab core or the backend packages. control operations. - `newton.SurfaceVelocity` uses Newton's upstream `ConveyorForceModel`. The adapter owns replicated surface/body bindings and solver lifecycle integration, - including hard resets and registration before CUDA graph capture. It requires - the Newton backend. + including hard resets and registration before CUDA graph capture. It supports + the Newton MJWarp solver; other Newton solvers are outside this adapter's scope. - `physx.SurfaceVelocity` uses native `PhysxSurfaceVelocityAPI` on CPU. Author the API before simulation parsing with `apply_surface_velocity_api`. This path requires the PhysX backend and Kit; GPU dynamics are unsupported. diff --git a/source/isaaclab_contrib/isaaclab_contrib/conveyors/newton.py b/source/isaaclab_contrib/isaaclab_contrib/conveyors/newton.py index 5e82df8a9dc4..a8ba32930cb2 100644 --- a/source/isaaclab_contrib/isaaclab_contrib/conveyors/newton.py +++ b/source/isaaclab_contrib/isaaclab_contrib/conveyors/newton.py @@ -140,7 +140,7 @@ def _validate_newton_surface_specs(surface_specs: Sequence[SurfaceVelocitySpec]) class SurfaceVelocity: - """Adapt Newton's upstream conveyor force model to the Isaac Lab lifecycle. + """Adapt Newton's upstream MuJoCo conveyor force model to the MJWarp lifecycle. The driver is created after the simulation context but before its first reset. It requests solved contact forces before model finalization, then diff --git a/source/isaaclab_contrib/isaaclab_contrib/conveyors/physx.py b/source/isaaclab_contrib/isaaclab_contrib/conveyors/physx.py index 1d85f1cd08a8..3cae39e948db 100644 --- a/source/isaaclab_contrib/isaaclab_contrib/conveyors/physx.py +++ b/source/isaaclab_contrib/isaaclab_contrib/conveyors/physx.py @@ -359,13 +359,7 @@ class _PhysxSchemaSurfaceWriter: def __init__(self, prims_or_paths: Sequence[Any], *, stage: Any | None, apply_api: bool) -> None: """Resolve prims, optionally apply schemas, and cache surface attributes.""" - try: - from pxr import Gf, PhysxSchema, UsdPhysics - except ImportError as exc: - raise RuntimeError( - "PhysxSurfaceVelocityAPI is unavailable. Use this backend inside an Isaac Sim process with PhysX " - "schemas." - ) from exc + from pxr import Gf, PhysxSchema, UsdPhysics if stage is None and any(isinstance(value, str) for value in prims_or_paths): from isaaclab.sim.utils.stage import get_current_stage diff --git a/source/isaaclab_contrib/test/conveyors/test_surface_velocity_newton.py b/source/isaaclab_contrib/test/conveyors/test_surface_velocity_newton.py index 92c4cf278ea0..a4030da85fa4 100644 --- a/source/isaaclab_contrib/test/conveyors/test_surface_velocity_newton.py +++ b/source/isaaclab_contrib/test/conveyors/test_surface_velocity_newton.py @@ -11,6 +11,7 @@ from isaaclab_newton.physics import MJWarpSolverCfg, NewtonBuilderCfg, NewtonCfg, NewtonManager from isaaclab.sim import SimulationCfg, build_simulation_context +from isaaclab.test.utils import DeviceScope, test_devices from isaaclab_contrib.conveyors.newton import SurfaceVelocity from isaaclab_contrib.conveyors.surface_velocity import SurfaceVelocitySpec @@ -163,3 +164,40 @@ def test_invalid_conveyors_leave_no_callbacks(specs, num_envs, env_path, error, with pytest.raises(error, match=message): SurfaceVelocity(num_envs, specs, env_path_format=env_path, body_pattern="Cube0$") assert set(NewtonManager._callbacks) == before + + +@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) +def test_removed_callbacks_leave_captured_graph(device): + """Removed callbacks stop writing GPU buffers in eager and recaptured simulation steps.""" + cfg = SimulationCfg(device=device, physics=NewtonCfg(use_cuda_graph=True)) + with build_simulation_context(sim_cfg=cfg) as sim: + builder = sim.get_or_create_backend(NewtonBuilderCfg(physics_cfg=cfg.physics)) + body = builder.add_link(xform=wp.transform_identity()) + builder.add_shape_box(body, hx=0.1, hy=0.1, hz=0.1) + builder.add_articulation([builder.add_joint_free(body)]) + observed = wp.zeros(2, device=device) + ones = wp.ones(2, device=device) + + def force(state): + wp.copy(observed, ones, dest_offset=0, count=1) + + def substep(solver, contacts, state, dt): + wp.copy(observed, ones, dest_offset=1, count=1) + + NewtonManager.register_state_force_callback(force) + NewtonManager.register_post_solver_substep_callback(substep) + sim.reset() + sim.step(render=False) + assert NewtonManager._graph is not None + np.testing.assert_array_equal(observed.numpy(), [1, 1]) + NewtonManager.unregister_state_force_callback(force) + NewtonManager.unregister_post_solver_substep_callback(substep) + observed.zero_() + sim.step(render=False) + np.testing.assert_array_equal(observed.numpy(), [0, 0]) + graph = NewtonManager._graph + NewtonManager.unregister_state_force_callback(force) + NewtonManager.unregister_post_solver_substep_callback(substep) + assert NewtonManager._graph is graph + sim.step(render=False) + np.testing.assert_array_equal(observed.numpy(), [0, 0]) diff --git a/source/isaaclab_newton/isaaclab_newton/physics/newton_collision_cfg.py b/source/isaaclab_newton/isaaclab_newton/physics/newton_collision_cfg.py index a13c4a84c21e..305671fa3fa8 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/newton_collision_cfg.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/newton_collision_cfg.py @@ -107,10 +107,10 @@ class NewtonCollisionPipelineCfg: """ include_static_kinematic_pairs: bool = True - """Whether to generate contacts between two immovable shapes. + """Whether to generate static-kinematic and kinematic-kinematic contacts. - Set to ``False`` to exclude static-static, static-kinematic, and kinematic-kinematic - pairs. Defaults to ``True``, matching Newton's default. + Static-static pairs are always excluded. Set to ``False`` to also exclude + static-kinematic and kinematic-kinematic pairs. Defaults to ``True``, matching Newton's default. """ reduce_contacts: bool = True diff --git a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py index 3a0b3ef9dd48..6f4b854188cd 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py @@ -1960,13 +1960,18 @@ def unregister_state_force_callback(cls, callback: Callable[[State], None]) -> N Removing a callback that was never registered or was already removed is a safe no-op. This lets scene-owned systems release bound-method - references before the global Newton manager is cleared. + references before the global Newton manager is cleared. An active CUDA + graph is rebuilt on the next step to discard the removed callback. Args: callback: Previously registered callback. """ - with contextlib.suppress(ValueError): - NewtonManager._state_force_callbacks.remove(callback) + if callback not in cls._state_force_callbacks: + return + cls._state_force_callbacks.remove(callback) + if cls._graph is not None: + cls._graph = None + cls._graph_capture_pending = True @classmethod def register_post_solver_substep_callback( @@ -2026,11 +2031,18 @@ def unregister_post_solver_substep_callback( ) -> None: """Remove a previously registered post-solver-substep callback. + An active CUDA graph is rebuilt on the next step. Removing an absent + callback is a safe no-op. + Args: callback: Previously registered callback. """ - with contextlib.suppress(ValueError): - cls._post_solver_substep_callbacks.remove(callback) + if callback not in cls._post_solver_substep_callbacks: + return + cls._post_solver_substep_callbacks.remove(callback) + if cls._graph is not None: + cls._graph = None + cls._graph_capture_pending = True @classmethod def register_post_step_callback(cls, callback: Callable[[], None]) -> None: From 886a2c97b0af846508604d7b59f66483d2b68a8e Mon Sep 17 00:00:00 2001 From: Maximilian Krause Date: Fri, 23 Oct 2026 15:06:10 +0200 Subject: [PATCH 6/9] Keep experimental conveyor support Newton-only --- .../maximiliank-conveyor-support.added.rst | 2 +- .../isaaclab_contrib/conveyors/README.md | 14 +- .../isaaclab_contrib/conveyors/__init__.py | 4 +- .../isaaclab_contrib/conveyors/newton.py | 17 +- .../isaaclab_contrib/conveyors/physx.py | 442 ------------------ .../conveyors/surface_velocity.py | 59 +-- .../conveyors/test_surface_velocity_physx.py | 130 ------ .../conveyors/test_surface_velocity_spec.py | 4 +- 8 files changed, 17 insertions(+), 655 deletions(-) delete mode 100644 source/isaaclab_contrib/isaaclab_contrib/conveyors/physx.py delete mode 100644 source/isaaclab_contrib/test/conveyors/test_surface_velocity_physx.py diff --git a/source/isaaclab_contrib/changelog.d/maximiliank-conveyor-support.added.rst b/source/isaaclab_contrib/changelog.d/maximiliank-conveyor-support.added.rst index 3dc15f179a03..c44fddcb77d1 100644 --- a/source/isaaclab_contrib/changelog.d/maximiliank-conveyor-support.added.rst +++ b/source/isaaclab_contrib/changelog.d/maximiliank-conveyor-support.added.rst @@ -1 +1 @@ -* Added experimental straight and curved conveyor controls with Newton force-model and native CPU PhysX adapters. +* Added experimental straight and curved conveyor controls using Newton's upstream force model. diff --git a/source/isaaclab_contrib/isaaclab_contrib/conveyors/README.md b/source/isaaclab_contrib/isaaclab_contrib/conveyors/README.md index 89f3abf701a6..d6cb67eaa1b2 100644 --- a/source/isaaclab_contrib/isaaclab_contrib/conveyors/README.md +++ b/source/isaaclab_contrib/isaaclab_contrib/conveyors/README.md @@ -1,21 +1,17 @@ # Experimental conveyor support -These adapters stage conveyor integration in `isaaclab_contrib`; their API is +This adapter stages Newton conveyor integration in `isaaclab_contrib`; its API is experimental and is not exported from Isaac Lab core or the backend packages. - `surface_velocity.SurfaceVelocitySpec` describes a straight or curved surface - in the collision prim's local frame. `SurfaceVelocityView` describes the shared - control operations. + in the collision prim's local frame. - `newton.SurfaceVelocity` uses Newton's upstream `ConveyorForceModel`. The adapter owns replicated surface/body bindings and solver lifecycle integration, including hard resets and registration before CUDA graph capture. It supports the Newton MJWarp solver; other Newton solvers are outside this adapter's scope. -- `physx.SurfaceVelocity` uses native `PhysxSurfaceVelocityAPI` on CPU. - Author the API before simulation parsing with `apply_surface_velocity_api`. - This path requires the PhysX backend and Kit; GPU dynamics are unsupported. -Both adapters retain commanded speeds while disabled and restart their startup -ramp on a full reset. Newton also clears stale forces for selected worlds. -Newton uses the spec's traction parameters; PhysX uses authored contact materials. +The adapter retains commanded speeds while disabled, restarts its startup ramp +on a full reset, and clears stale forces for selected worlds. The surface spec +controls traction parameters. No meshes, assets, task registrations, or policy checkpoints are bundled here. diff --git a/source/isaaclab_contrib/isaaclab_contrib/conveyors/__init__.py b/source/isaaclab_contrib/isaaclab_contrib/conveyors/__init__.py index 6dddd7297fff..9ceb327848ad 100644 --- a/source/isaaclab_contrib/isaaclab_contrib/conveyors/__init__.py +++ b/source/isaaclab_contrib/isaaclab_contrib/conveyors/__init__.py @@ -5,6 +5,6 @@ """Experimental conveyor support for contributed tasks. -Backend adapters are imported explicitly from :mod:`.newton` or :mod:`.physx` so -importing the surface descriptions does not load either optional backend. +The Newton adapter is imported explicitly from :mod:`.newton` so importing the +surface descriptions does not load the optional backend. """ diff --git a/source/isaaclab_contrib/isaaclab_contrib/conveyors/newton.py b/source/isaaclab_contrib/isaaclab_contrib/conveyors/newton.py index a8ba32930cb2..0207335654f8 100644 --- a/source/isaaclab_contrib/isaaclab_contrib/conveyors/newton.py +++ b/source/isaaclab_contrib/isaaclab_contrib/conveyors/newton.py @@ -215,25 +215,10 @@ def _require_binding(self) -> _SurfaceVelocityBinding: raise RuntimeError("Surface velocity is not bound to an initialized Newton solver.") return binding - @property - def specs(self) -> tuple[SurfaceVelocitySpec, ...]: - """Authored surface descriptions in stable within-environment order.""" - return self._surface_specs - - @property - def surfaces_per_env(self) -> int: - """Number of authored surfaces in each replicated environment.""" - return len(self._surface_specs) - @property def num_surfaces(self) -> int: """Total number of resolved surfaces across all environments.""" - return self._num_envs * self.surfaces_per_env - - @property - def initialized(self) -> bool: - """Whether the driver is bound to the active Newton solver.""" - return self._binding is not None + return self._num_envs * len(self._surface_specs) @property def prim_paths(self) -> tuple[str, ...]: diff --git a/source/isaaclab_contrib/isaaclab_contrib/conveyors/physx.py b/source/isaaclab_contrib/isaaclab_contrib/conveyors/physx.py deleted file mode 100644 index 3cae39e948db..000000000000 --- a/source/isaaclab_contrib/isaaclab_contrib/conveyors/physx.py +++ /dev/null @@ -1,442 +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 - -"""CPU-only native PhysX surface-velocity control. - -The module deliberately keeps PhysX schema imports behind authoring and binding calls. Its -geometry conversion and host-side control state therefore remain usable in import-light tests -and tooling that do not launch Kit. - -The task configuration rejects GPU dynamics: the supported native ``PhysxSurfaceVelocityAPI`` -path can lose conveyor contacts there. The force-driven Newton adapter is the scalable GPU path. -""" - -from __future__ import annotations - -import math -from collections.abc import Sequence -from dataclasses import dataclass -from typing import Any - -import numpy as np - -from .surface_velocity import SurfaceVelocitySpec, resolve_surface_velocity_paths - - -@dataclass(frozen=True, slots=True) -class PhysxSurfaceVelocityTwist: - """Local PhysX surface twist. - - Attributes: - linear_velocity: Local linear surface velocity [m/s]. - angular_velocity_deg: Local angular surface velocity [deg/s]. - """ - - linear_velocity: tuple[float, float, float] - angular_velocity_deg: tuple[float, float, float] - - -def compute_surface_velocity_twist( - spec: SurfaceVelocitySpec, velocity: float | None = None -) -> PhysxSurfaceVelocityTwist: - """Convert one backend-neutral belt command to a local PhysX surface twist. - - Straight belts map their signed speed to a normalized linear direction. Curved belts map - speed over radius to PhysX's degree-per-second angular convention. PhysX rotates the surface - field about the rigid-body origin, so ``-omega x pivot`` is included in the linear component - to move the instantaneous center to :attr:`SurfaceVelocitySpec.pivot_point`. - - Args: - spec: Authored conveyor intent in the collision prim's local frame. - velocity: Optional signed speed override [m/s]. Defaults to ``spec.velocity``. - - Returns: - Local linear and angular PhysX surface velocities. - - Raises: - ValueError: If the speed is not finite, the direction is zero, or a curved belt has no - positive radius. - """ - speed = spec.velocity if velocity is None else _finite_float("velocity", velocity) - direction = np.asarray(spec.direction, dtype=np.float64) - magnitude = float(np.linalg.norm(direction)) - if not math.isfinite(magnitude) or magnitude <= 1.0e-8: - raise ValueError(f"Conveyor direction must be finite and non-zero, got {spec.direction!r}.") - unit_direction = direction / magnitude - - if not spec.curved: - linear = unit_direction * speed - return PhysxSurfaceVelocityTwist(tuple(float(value) for value in linear), (0.0, 0.0, 0.0)) - - if spec.radius is None or not math.isfinite(spec.radius) or spec.radius <= 0.0: - raise ValueError("A curved PhysX conveyor requires a finite positive radius.") - angular_rad = unit_direction * (speed / spec.radius) - pivot = np.asarray(spec.pivot_point, dtype=np.float64) - linear = -np.cross(angular_rad, pivot) - angular_deg = np.degrees(angular_rad) - return PhysxSurfaceVelocityTwist( - tuple(float(value) for value in linear), tuple(float(value) for value in angular_deg) - ) - - -def apply_surface_velocity_api( - prim_or_path: Any, - spec: SurfaceVelocitySpec, - *, - velocity_scale: float = 0.0, - stage: Any | None = None, -) -> None: - """Apply and author a kinematic PhysX surface-velocity API on one belt prim. - - This function is intended for a task spawner, before PhysX parses the stage. It lazily imports - ``pxr.PhysxSchema`` and applies both the rigid-body and surface-velocity schemas. The rigid body - is made kinematic, surface velocities are authored in local space, and the initial command is - scaled explicitly. A zero default prevents pre-runtime motion during simulation warmup. - - Args: - prim_or_path: USD prim object or exact prim path. - spec: Conveyor intent used to author the local twist. - velocity_scale: Finite multiplier for the initial signed surface speed. - stage: Optional USD stage used when ``prim_or_path`` is a string. Defaults to the current - Isaac Lab stage. - - Raises: - RuntimeError: If the PhysX schema is unavailable or the target prim does not exist. - ValueError: If ``velocity_scale`` is not finite. - """ - scale = _finite_float("velocity_scale", velocity_scale) - twist = compute_surface_velocity_twist(spec, velocity=spec.velocity * scale) - binding = _PhysxSchemaSurfaceWriter((prim_or_path,), stage=stage, apply_api=True) - try: - binding.write(0, enabled=spec.enabled, twist=twist) - finally: - binding.close() - - -class SurfaceVelocity: - """Host-side CPU reference facade for native PhysX surface velocity. - - The facade binds exact environment-major paths whose schemas were already authored by - :func:`apply_surface_velocity_api`. Call :meth:`start` to register physics-rate updates, - or call :meth:`update` manually. Commands and enabled state survive full resets; a full reset - restarts the startup ramp. This facade authors USD attributes - on the host and is not a GPU conveyor implementation. - """ - - def __init__( - self, - num_envs: int, - surface_specs: Sequence[SurfaceVelocitySpec], - *, - env_path_format: str = "/World/envs/env_{}", - startup_duration_s: float = 1.0, - stage: Any | None = None, - ) -> None: - """Bind native surface attributes and initialize host-side control state. - - Args: - num_envs: Number of replicated environments. - surface_specs: Within-environment surface descriptions. - env_path_format: Exact replicated environment path format. - startup_duration_s: Duration of the global surface-speed ramp [s]. - stage: Optional USD stage used by the default schema writer. - """ - duration = _finite_float("startup_duration_s", startup_duration_s) - if duration <= 0.0: - raise ValueError(f"Conveyor startup_duration_s must be positive, got {startup_duration_s!r}.") - self._num_envs = num_envs - self._surface_specs = tuple(surface_specs) - self._surface_paths = resolve_surface_velocity_paths(num_envs, self._surface_specs, env_path_format) - self._surfaces_per_env = len(self._surface_specs) - self._startup_duration_s = duration - self._elapsed_time = 0.0 - self._velocity_scale = 0.0 - self._closed = False - self._callback_handle: Any | None = None - self._writer = _PhysxSchemaSurfaceWriter(self._surface_paths, stage=stage, apply_api=False) - - self._command_velocity = np.tile( - np.asarray([spec.velocity for spec in self._surface_specs], dtype=np.float32), self._num_envs - ) - self._enabled = np.tile( - np.asarray([spec.enabled for spec in self._surface_specs], dtype=np.bool_), self._num_envs - ) - self._last_authored: list[tuple[bool, PhysxSurfaceVelocityTwist] | None] = [None] * self.num_surfaces - self._flush(force=True) - - @property - def specs(self) -> tuple[SurfaceVelocitySpec, ...]: - """Return authored descriptions in stable within-environment order.""" - return self._surface_specs - - @property - def surfaces_per_env(self) -> int: - """Return the number of authored surfaces per environment.""" - return self._surfaces_per_env - - @property - def prim_paths(self) -> tuple[str, ...]: - """Return exact PhysX prim paths in environment-major order.""" - return self._surface_paths - - @property - def num_surfaces(self) -> int: - """Return the total number of bound conveyor surfaces.""" - return len(self._surface_paths) - - @property - def initialized(self) -> bool: - """Return whether the facade still owns live schema bindings.""" - return not self._closed - - def start(self) -> None: - """Register one lazy PhysX post-step callback; repeated calls are safe.""" - self._require_open() - if self._callback_handle is not None: - return - from isaaclab_physx.physics.physx_manager import IsaacEvents, PhysxManager - - self._callback_handle = PhysxManager.register_callback( - self.update, - IsaacEvents.POST_PHYSICS_STEP, - name="physx_surface_velocity", - ) - - def update(self, dt: float) -> None: - """Advance the startup ramp by one physics step. - - Args: - dt: Positive physics step duration [s]. - """ - self._require_open() - step = _finite_float("physics dt", dt) - if step <= 0.0: - raise ValueError(f"Conveyor physics dt must be positive, got {dt!r}.") - self._elapsed_time = min(self._startup_duration_s, self._elapsed_time + step) - self._velocity_scale = self._elapsed_time / self._startup_duration_s - self._flush() - - def set_velocities(self, velocities: Any, indices: Any = None) -> None: - """Set signed speeds [m/s], preserving commands while belts are disabled.""" - selected = self._resolve_indices(indices) - self._command_velocity[selected] = self._broadcast_finite(velocities, len(selected), "velocities") - self._flush(selected) - - def get_velocities(self, indices: Any = None, clone: bool = True) -> np.ndarray: - """Return effective speeds with disabled belts reported as zero.""" - return self._get_values(self._command_velocity * self._enabled, indices, clone) - - def get_commanded_velocities(self, indices: Any = None, clone: bool = True) -> np.ndarray: - """Return commanded speeds before applying the enabled mask.""" - return self._get_values(self._command_velocity, indices, clone) - - def set_enabled(self, flags: Any, indices: Any = None) -> None: - """Enable or disable belts without discarding their commands.""" - selected = self._resolve_indices(indices) - values = _as_numpy(flags) - if values.ndim == 0: - values = np.full(len(selected), values.item()) - else: - values = values.reshape(-1) - if values.size == 1: - values = np.full(len(selected), values.item()) - if values.size != len(selected) or not np.all(np.isin(values, (False, True, 0, 1))): - raise ValueError(f"Conveyor enabled flags must contain {len(selected)} boolean values.") - self._enabled[selected] = values.astype(np.bool_) - self._flush(selected) - - def get_enabled(self, indices: Any = None, clone: bool = True) -> np.ndarray: - """Return enabled flags as integer values.""" - return self._get_values(self._enabled.astype(np.int32), indices, clone) - - def reset(self, env_ids: Any = None) -> None: - """Restart the global startup ramp on a full reset. - - Velocity commands and enabled state are deliberately preserved, including across a full - hard-reset path. Partial resets do not disturb the global ramp used by other environments. - - Args: - env_ids: Environment indices to reset, or ``None`` for all environments. - """ - self._require_open() - ids = self._resolve_env_ids(env_ids) - if len(np.unique(ids)) == self._num_envs: - self._elapsed_time = 0.0 - self._velocity_scale = 0.0 - self._flush(force=True) - - def close(self) -> None: - """Deregister callbacks, disable authored motion, and release bindings safely.""" - if self._closed: - return - if self._callback_handle is not None: - self._callback_handle.deregister() - self._callback_handle = None - zero = PhysxSurfaceVelocityTwist((0.0, 0.0, 0.0), (0.0, 0.0, 0.0)) - for index in range(self.num_surfaces): - self._writer.write(index, enabled=False, twist=zero) - self._writer.close() - self._closed = True - - def _flush(self, indices: Any = None, *, force: bool = False) -> None: - """Author scaled effective commands for selected rows.""" - selected = self._resolve_indices(indices) - for index in selected: - enabled = bool(self._enabled[index]) - spec = self._surface_specs[index % self._surfaces_per_env] - speed = float(self._command_velocity[index]) * self._velocity_scale if enabled else 0.0 - twist = compute_surface_velocity_twist(spec, velocity=speed) - state = (enabled, twist) - if force or state != self._last_authored[index]: - self._writer.write(int(index), enabled=enabled, twist=twist) - self._last_authored[index] = state - - def _resolve_indices(self, indices: Any) -> np.ndarray: - """Normalize and validate a belt row selection.""" - self._require_open() - if indices is None: - return np.arange(self.num_surfaces, dtype=np.int64) - if isinstance(indices, slice): - return np.arange(self.num_surfaces, dtype=np.int64)[indices] - selected = _as_numpy(indices) - if selected.dtype == np.bool_: - if selected.ndim != 1 or selected.size != self.num_surfaces: - raise IndexError(f"Boolean surface indices must have length {self.num_surfaces}.") - return np.flatnonzero(selected).astype(np.int64) - if not np.issubdtype(selected.dtype, np.integer): - raise IndexError(f"Conveyor indices must be integers, got {indices!r}.") - selected = selected.astype(np.int64, copy=False).reshape(-1) - if np.any((selected < 0) | (selected >= self.num_surfaces)): - raise IndexError(f"Conveyor surface indices are out of range: {selected.tolist()}.") - return selected - - def _resolve_env_ids(self, env_ids: Any) -> np.ndarray: - """Normalize and validate an environment selection.""" - if env_ids is None: - return np.arange(self._num_envs, dtype=np.int64) - ids = _as_numpy(env_ids) - if not np.issubdtype(ids.dtype, np.integer): - raise IndexError(f"Conveyor reset environment indices must be integers, got {env_ids!r}.") - ids = ids.astype(np.int64, copy=False).reshape(-1) - if np.any((ids < 0) | (ids >= self._num_envs)): - raise IndexError(f"Conveyor reset environment indices are out of range: {ids.tolist()}.") - return ids - - @staticmethod - def _broadcast_finite(values: Any, count: int, name: str) -> np.ndarray: - """Return a finite float32 vector with scalar broadcasting.""" - try: - result = np.asarray(_as_numpy(values), dtype=np.float32) - except (TypeError, ValueError, OverflowError) as exc: - raise ValueError(f"Conveyor {name} must contain finite numeric values.") from exc - if result.ndim == 0: - result = np.full(count, result.item(), dtype=np.float32) - else: - result = result.reshape(-1) - if result.size == 1: - result = np.full(count, result.item(), dtype=np.float32) - if result.size != count or not np.all(np.isfinite(result)): - raise ValueError(f"Conveyor {name} must contain one or {count} finite values.") - return result - - def _get_values(self, source: np.ndarray, indices: Any, clone: bool) -> np.ndarray: - """Return a complete buffer or a selected copy.""" - self._require_open() - if indices is None: - return source.copy() if clone else source - return source[self._resolve_indices(indices)].copy() - - def _require_open(self) -> None: - """Fail predictably after backend resources have been released.""" - if self._closed: - raise RuntimeError("The PhysX conveyor surface facade is closed.") - - -class _PhysxSchemaSurfaceWriter: - """Cached USD attribute writer with all schema imports kept lazy.""" - - def __init__(self, prims_or_paths: Sequence[Any], *, stage: Any | None, apply_api: bool) -> None: - """Resolve prims, optionally apply schemas, and cache surface attributes.""" - from pxr import Gf, PhysxSchema, UsdPhysics - - if stage is None and any(isinstance(value, str) for value in prims_or_paths): - from isaaclab.sim.utils.stage import get_current_stage - - stage = get_current_stage() - self._gf = Gf - self._attributes: list[tuple[Any, Any, Any]] = [] - for value in prims_or_paths: - prim = stage.GetPrimAtPath(value) if isinstance(value, str) else value - if prim is None or not prim.IsValid(): - raise RuntimeError(f"Cannot bind PhysX conveyor surface: prim {value!r} does not exist.") - - rigid_body = UsdPhysics.RigidBodyAPI(prim) - if apply_api and not prim.HasAPI(UsdPhysics.RigidBodyAPI): - rigid_body = UsdPhysics.RigidBodyAPI.Apply(prim) - elif not prim.HasAPI(UsdPhysics.RigidBodyAPI): - raise RuntimeError(f"PhysX conveyor prim {prim.GetPath()} has no authored RigidBodyAPI.") - if apply_api: - rigid_body.CreateRigidBodyEnabledAttr().Set(True) - rigid_body.CreateKinematicEnabledAttr().Set(True) - elif rigid_body.GetKinematicEnabledAttr().Get() is not True: - raise RuntimeError(f"PhysX conveyor prim {prim.GetPath()} must be an authored kinematic rigid body.") - - if prim.HasAPI(PhysxSchema.PhysxSurfaceVelocityAPI): - surface_api = PhysxSchema.PhysxSurfaceVelocityAPI(prim) - elif apply_api: - surface_api = PhysxSchema.PhysxSurfaceVelocityAPI.Apply(prim) - else: - raise RuntimeError( - f"PhysX conveyor prim {prim.GetPath()} has no authored PhysxSurfaceVelocityAPI; " - "call apply_surface_velocity_api from its spawner before simulation starts." - ) - surface_api.CreateSurfaceVelocityLocalSpaceAttr().Set(True) - self._attributes.append( - ( - surface_api.CreateSurfaceVelocityEnabledAttr(), - surface_api.CreateSurfaceVelocityAttr(), - surface_api.CreateSurfaceAngularVelocityAttr(), - ) - ) - - def write(self, index: int, *, enabled: bool, twist: PhysxSurfaceVelocityTwist) -> None: - """Author one cached local-space surface twist.""" - enabled_attr, linear_attr, angular_attr = self._attributes[index] - # PhysX caches the contact-modification flag when a surface-velocity - # shape is parsed. Cycle it around command changes, matching the - # official Isaac Conveyor node, so live updates are picked up without - # leaving a stale contact modifier attached to the shape. - enabled_attr.Set(False) - linear_attr.Set(self._gf.Vec3f(*twist.linear_velocity)) - angular_attr.Set(self._gf.Vec3f(*twist.angular_velocity_deg)) - enabled_attr.Set(enabled) - - def close(self) -> None: - """Release cached schema attribute handles.""" - self._attributes.clear() - - -def _finite_float(name: str, value: Any) -> float: - """Coerce one finite scalar with a stable validation error.""" - try: - result = float(value) - except (TypeError, ValueError, OverflowError) as exc: - raise ValueError(f"Conveyor {name} must be finite, got {value!r}.") from exc - if not math.isfinite(result): - raise ValueError(f"Conveyor {name} must be finite, got {value!r}.") - return result - - -def _as_numpy(values: Any) -> np.ndarray: - """Move supported tensor-like values to a host NumPy array without importing their libraries.""" - if isinstance(values, np.ndarray): - return values - if hasattr(values, "detach"): - values = values.detach() - if hasattr(values, "cpu"): - values = values.cpu() - if hasattr(values, "numpy"): - return np.asarray(values.numpy()) - return np.asarray(values) diff --git a/source/isaaclab_contrib/isaaclab_contrib/conveyors/surface_velocity.py b/source/isaaclab_contrib/isaaclab_contrib/conveyors/surface_velocity.py index b528cc9bf668..a88036e59294 100644 --- a/source/isaaclab_contrib/isaaclab_contrib/conveyors/surface_velocity.py +++ b/source/isaaclab_contrib/isaaclab_contrib/conveyors/surface_velocity.py @@ -3,14 +3,14 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Backend-neutral surface-velocity descriptions and control interface.""" +"""Import-light descriptions of prescribed conveyor surface velocities.""" from __future__ import annotations import math from collections.abc import Sequence from dataclasses import dataclass -from typing import Any, Protocol, runtime_checkable +from typing import Any _ENV_REGEX_NS = "{ENV_REGEX_NS}" @@ -62,10 +62,8 @@ def _validate_vector(name: str, value: tuple[float, ...], length: int, *, nonzer class SurfaceVelocitySpec: """Persistent intent for one collision surface with prescribed tangential velocity. - The fields follow the authored conveyor model proposed for Isaac Sim while remaining independent of - Kit, OpenUSD, and any physics backend. Directions, surface normals, and the optional pivot point are - expressed in the collision prim's local frame. Runtime state such as commanded speeds and applied - forces intentionally does not belong in this description. + Directions, surface normals, and the optional pivot point are expressed in the collision prim's + local frame. Runtime state such as commanded speeds and applied forces belongs to the adapter. ``prim_path`` may contain Isaac Lab's ``{ENV_REGEX_NS}`` placeholder. A backend resolves that template to every replicated collision prim while preserving one deterministic belt index per environment. An @@ -77,10 +75,8 @@ class SurfaceVelocitySpec: enabled: Whether the surface initially transports contacting bodies. direction: Local travel direction, or local rotation axis for a curved belt. curved: Whether the velocity field rotates about ``pivot_point``. - pivot_point: Local rotation center for a curved belt [m]. An OpenUSD adapter may map this point to the - world transform of a prim targeted by the proposed schema's pivot relationship; it is not copied - into a schema attribute. - radius: Optional centerline radius used by backends that cannot derive it from geometry [m]. + pivot_point: Local rotation center for a curved belt [m]. + radius: Centerline radius required for a curved belt [m]. surface_normal: Local outward normal of the carrying surface. contact_threshold: Minimum contact-normal alignment accepted for traction. friction_coefficient: Coulomb limit for synthetic surface traction. @@ -173,46 +169,3 @@ def resolve_surface_velocity_paths( if len(set(paths)) != len(paths): raise ValueError("Resolved conveyor prim paths must be unique.") return paths - - -@runtime_checkable -class SurfaceVelocityView(Protocol): - """Common tensorized control contract implemented by physics backends.""" - - @property - def prim_paths(self) -> tuple[str, ...]: - """Resolved collision prim paths in stable surface-index order.""" - ... - - @property - def num_surfaces(self) -> int: - """Number of resolved moving surfaces.""" - ... - - def set_velocities(self, velocities: Any, indices: Any = None) -> None: - """Set signed surface velocities [m/s] for selected surfaces.""" - ... - - def get_velocities(self, indices: Any = None, clone: bool = True) -> Any: - """Return effective surface velocities [m/s] for selected surfaces.""" - ... - - def get_commanded_velocities(self, indices: Any = None, clone: bool = True) -> Any: - """Return commanded surface velocities [m/s] before the enabled mask.""" - ... - - def set_enabled(self, flags: Any, indices: Any = None) -> None: - """Enable or disable selected surfaces without discarding their commands.""" - ... - - def get_enabled(self, indices: Any = None, clone: bool = True) -> Any: - """Return integer enabled flags for selected surfaces.""" - ... - - def reset(self, env_ids: Any = None) -> None: - """Clear runtime state for selected replicated environments.""" - ... - - def close(self) -> None: - """Release backend resources and callbacks.""" - ... diff --git a/source/isaaclab_contrib/test/conveyors/test_surface_velocity_physx.py b/source/isaaclab_contrib/test/conveyors/test_surface_velocity_physx.py deleted file mode 100644 index a6f229031f58..000000000000 --- a/source/isaaclab_contrib/test/conveyors/test_surface_velocity_physx.py +++ /dev/null @@ -1,130 +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-light tests for native PhysX surface velocity.""" - -from __future__ import annotations - -import math -from types import SimpleNamespace -from unittest.mock import Mock, call - -import numpy as np -import pytest -import torch - -import isaaclab_contrib.conveyors.physx as surface_module -from isaaclab_contrib.conveyors.surface_velocity import SurfaceVelocitySpec - - -class _FakeWriter: - """Record authored states without importing USD or PhysX schemas.""" - - def __init__(self) -> None: - self.writes: list[tuple[int, bool, surface_module.PhysxSurfaceVelocityTwist]] = [] - self.close_count = 0 - - def write(self, index: int, *, enabled: bool, twist: surface_module.PhysxSurfaceVelocityTwist) -> None: - self.writes.append((index, enabled, twist)) - - def close(self) -> None: - self.close_count += 1 - - -def _surface_spec(name: str = "Belt", **kwargs) -> SurfaceVelocitySpec: - """Return one replicated belt description for facade tests.""" - return SurfaceVelocitySpec(prim_path=f"{{ENV_REGEX_NS}}/{name}", velocity=0.4, **kwargs) - - -def test_twist_conversion_normalizes_straight_direction() -> None: - """Straight surface speed is independent of the authored direction magnitude.""" - spec = _surface_spec(direction=(3.0, 4.0, 0.0)) - - twist = surface_module.compute_surface_velocity_twist(spec, velocity=2.0) - - np.testing.assert_allclose(twist.linear_velocity, (1.2, 1.6, 0.0)) - assert twist.angular_velocity_deg == (0.0, 0.0, 0.0) - - -def test_twist_conversion_uses_degrees_and_compensates_curved_pivot() -> None: - """Curved belts rotate about their local pivot rather than the rigid-body origin.""" - spec = _surface_spec( - direction=(0.0, 0.0, 2.0), - curved=True, - radius=2.0, - pivot_point=(2.0, 0.0, 0.0), - ) - - twist = surface_module.compute_surface_velocity_twist(spec, velocity=math.pi) - - np.testing.assert_allclose(twist.angular_velocity_deg, (0.0, 0.0, 90.0), atol=1.0e-12) - np.testing.assert_allclose(twist.linear_velocity, (0.0, -math.pi, 0.0), atol=1.0e-12) - - -def test_curved_twist_requires_an_explicit_radius() -> None: - """A native angular rate cannot be inferred from unspecified task geometry.""" - spec = _surface_spec(curved=True) - - with pytest.raises(ValueError, match="positive radius"): - surface_module.compute_surface_velocity_twist(spec) - - -def test_facade_ramps_playback_and_preserves_commands_on_reset(monkeypatch: pytest.MonkeyPatch) -> None: - """Full resets restart playback without erasing policy-visible command state.""" - writer = _FakeWriter() - monkeypatch.setattr(surface_module, "_PhysxSchemaSurfaceWriter", lambda *args, **kwargs: writer) - facade = surface_module.SurfaceVelocity(2, (_surface_spec(),)) - - assert facade.prim_paths == ("/World/envs/env_0/Belt", "/World/envs/env_1/Belt") - assert facade.num_surfaces == 2 - assert [record[2].linear_velocity for record in writer.writes] == [(0.0, 0.0, 0.0)] * 2 - - facade.update(0.25) - np.testing.assert_allclose([record[2].linear_velocity[0] for record in writer.writes[-2:]], (0.1, 0.1)) - - facade.set_velocities(torch.tensor([0.8], requires_grad=True), indices=torch.tensor([0])) - facade.set_enabled(False, indices=[1]) - facade.update(0.25) - np.testing.assert_allclose(facade.get_commanded_velocities(), (0.8, 0.4)) - np.testing.assert_allclose(facade.get_velocities(), (0.8, 0.0)) - - writes_before_reset = list(writer.writes) - facade.reset(env_ids=torch.tensor([0])) - assert writer.writes == writes_before_reset - facade.reset(env_ids=[1, 0]) - np.testing.assert_allclose(facade.get_commanded_velocities(), (0.8, 0.4)) - assert facade.get_enabled().tolist() == [1, 0] - np.testing.assert_allclose(writer.writes[-2][2].linear_velocity, (0.0, 0.0, 0.0)) - - facade.close() - facade.close() - assert writer.close_count == 1 - assert [(index, enabled) for index, enabled, _ in writer.writes[-2:]] == [(0, False), (1, False)] - - -def test_authoring_helper_applies_kinematic_local_surface_schema(monkeypatch: pytest.MonkeyPatch) -> None: - """Author a real kinematic USD body; mock only the Kit-dependent PhysX API.""" - import pxr - from pxr import Gf, Usd, UsdPhysics - - stage = Usd.Stage.CreateInMemory() - prim = stage.DefinePrim("/World/Belt", "Xform") - surface_api = Mock() - # HasAPI needs a registered USD type. Substitute only the unavailable PhysX schema. - schema = SimpleNamespace(PhysxSurfaceVelocityAPI=UsdPhysics.CollisionAPI) - monkeypatch.setattr(UsdPhysics.CollisionAPI, "Apply", Mock(return_value=surface_api)) - monkeypatch.setattr(pxr, "PhysxSchema", schema, raising=False) - - surface_module.apply_surface_velocity_api(prim, _surface_spec(), velocity_scale=0.0) - - assert prim.HasAPI(UsdPhysics.RigidBodyAPI) - rigid_body = UsdPhysics.RigidBodyAPI(prim) - assert rigid_body.GetRigidBodyEnabledAttr().Get() is True - assert rigid_body.GetKinematicEnabledAttr().Get() is True - schema.PhysxSurfaceVelocityAPI.Apply.assert_called_once_with(prim) - surface_api.CreateSurfaceVelocityLocalSpaceAttr().Set.assert_called_once_with(True) - assert surface_api.CreateSurfaceVelocityEnabledAttr().Set.call_args_list == [call(False), call(True)] - surface_api.CreateSurfaceVelocityAttr().Set.assert_called_once_with(Gf.Vec3f(0.0)) - surface_api.CreateSurfaceAngularVelocityAttr().Set.assert_called_once_with(Gf.Vec3f(0.0)) diff --git a/source/isaaclab_contrib/test/conveyors/test_surface_velocity_spec.py b/source/isaaclab_contrib/test/conveyors/test_surface_velocity_spec.py index e34fc0e1d78e..93b07e414069 100644 --- a/source/isaaclab_contrib/test/conveyors/test_surface_velocity_spec.py +++ b/source/isaaclab_contrib/test/conveyors/test_surface_velocity_spec.py @@ -3,7 +3,7 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Focused tests for backend-neutral surface-velocity descriptions.""" +"""Focused tests for import-light surface-velocity descriptions.""" from __future__ import annotations @@ -13,7 +13,7 @@ def test_surface_velocity_spec_normalizes_vectors() -> None: - """List inputs become immutable vectors before either backend consumes them.""" + """List inputs become immutable vectors before the adapter consumes them.""" spec = SurfaceVelocitySpec( prim_path="{ENV_REGEX_NS}/Belt", direction=[3, 4, 0], pivot_point=[1, 2, 3], surface_normal=[0, 0, 1] ) From 4305eba923e425052552baae5af3cabebb2486a6 Mon Sep 17 00:00:00 2001 From: Maximilian Krause Date: Fri, 23 Oct 2026 16:24:06 +0200 Subject: [PATCH 7/9] Narrow conveyor surface descriptions to authored task inputs --- .../conveyors/surface_velocity.py | 152 ++++++------------ .../conveyors/test_surface_velocity_spec.py | 54 +++---- 2 files changed, 70 insertions(+), 136 deletions(-) diff --git a/source/isaaclab_contrib/isaaclab_contrib/conveyors/surface_velocity.py b/source/isaaclab_contrib/isaaclab_contrib/conveyors/surface_velocity.py index a88036e59294..ab4113d0a38a 100644 --- a/source/isaaclab_contrib/isaaclab_contrib/conveyors/surface_velocity.py +++ b/source/isaaclab_contrib/isaaclab_contrib/conveyors/surface_velocity.py @@ -10,69 +10,23 @@ import math from collections.abc import Sequence from dataclasses import dataclass -from typing import Any _ENV_REGEX_NS = "{ENV_REGEX_NS}" -def _validate_prim_path(value: str) -> None: - """Validate one exact USD prim path or supported replicated-path template.""" - if not isinstance(value, str) or not value: - raise ValueError("Surface prim_path must be a non-empty string.") - if value.startswith(f"{_ENV_REGEX_NS}/"): - path = value[len(_ENV_REGEX_NS) :] - elif value.startswith("/"): - path = value - else: - raise ValueError(f"Surface prim_path must be absolute or start with '{_ENV_REGEX_NS}/', got {value!r}.") - if "{" in path or "}" in path: - raise ValueError(f"Surface prim_path supports only a leading '{_ENV_REGEX_NS}' placeholder, got {value!r}.") - components = path[1:].split("/") - if not components or any(not component or component in {".", ".."} for component in components): - raise ValueError(f"Surface prim_path must identify a concrete prim without empty components, got {value!r}.") - if any(any(character.isspace() for character in component) for component in components): - raise ValueError(f"Surface prim_path components must not contain whitespace, got {value!r}.") - - -def _validate_scalar(name: str, value: Any) -> float: - """Return one finite float with consistent validation errors.""" - try: - result = float(value) - except (TypeError, ValueError, OverflowError) as exc: - raise ValueError(f"Surface {name} must be finite, got {value!r}.") from exc - if not math.isfinite(result): - raise ValueError(f"Surface {name} must be finite, got {value!r}.") - return result - - -def _validate_vector(name: str, value: tuple[float, ...], length: int, *, nonzero: bool = False) -> tuple[float, ...]: - """Return one finite vector with a stable tuple representation.""" - try: - result = tuple(float(component) for component in value) - except (TypeError, ValueError) as exc: - raise ValueError(f"Surface {name} must contain {length} finite values, got {value!r}.") from exc - if len(result) != length or not all(math.isfinite(component) for component in result): - raise ValueError(f"Surface {name} must contain {length} finite values, got {value!r}.") - if nonzero and math.sqrt(sum(component * component for component in result)) <= 1.0e-8: - raise ValueError(f"Surface {name} must be non-zero, got {value!r}.") - return result - - @dataclass(frozen=True, slots=True) class SurfaceVelocitySpec: """Persistent intent for one collision surface with prescribed tangential velocity. Directions, surface normals, and the optional pivot point are expressed in the collision prim's - local frame. Runtime state such as commanded speeds and applied forces belongs to the adapter. + local frame. Applied forces and reset state belong to the adapter. - ``prim_path`` may contain Isaac Lab's ``{ENV_REGEX_NS}`` placeholder. A backend resolves that template - to every replicated collision prim while preserving one deterministic belt index per environment. An - absolute path addresses one unreplicated surface; replicated environments should use the placeholder. + ``prim_path`` starts with Isaac Lab's ``{ENV_REGEX_NS}/`` placeholder. The adapter + resolves it using the scene's clone template, preserving environment-major surface order. Args: - prim_path: Collision prim path or replicated Isaac Lab prim-path template. - velocity: Initial signed centerline surface velocity [m/s]. - enabled: Whether the surface initially transports contacting bodies. + prim_path: Replicated collision-prim template with concrete descendant names. + velocity: Fixed signed centerline surface velocity [m/s]. direction: Local travel direction, or local rotation axis for a curved belt. curved: Whether the velocity field rotates about ``pivot_point``. pivot_point: Local rotation center for a curved belt [m]. @@ -84,7 +38,6 @@ class SurfaceVelocitySpec: prim_path: str velocity: float = 0.0 - enabled: bool = True direction: tuple[float, float, float] = (1.0, 0.0, 0.0) curved: bool = False pivot_point: tuple[float, float, float] = (0.0, 0.0, 0.0) @@ -94,34 +47,30 @@ class SurfaceVelocitySpec: friction_coefficient: float = 0.7 def __post_init__(self) -> None: - """Normalize immutable vectors and validate authored values.""" - _validate_prim_path(self.prim_path) - for name in ("enabled", "curved"): - if not isinstance(getattr(self, name), bool): - raise ValueError(f"Surface {name} must be a bool, got {getattr(self, name)!r}.") - - velocity = _validate_scalar("velocity", self.velocity) - contact_threshold = _validate_scalar("contact_threshold", self.contact_threshold) - friction_coefficient = _validate_scalar("friction_coefficient", self.friction_coefficient) - if not 0.0 <= contact_threshold <= 1.0: + """Reject invalid authored geometry and contact parameters.""" + if not self.prim_path.startswith(f"{_ENV_REGEX_NS}/") or not all( + part.isidentifier() for part in self.prim_path[len(_ENV_REGEX_NS) + 1 :].split("/") + ): + raise ValueError(f"Surface prim_path must start with '{_ENV_REGEX_NS}/' and name concrete prims.") + for name, vector in ( + ("direction", self.direction), + ("pivot_point", self.pivot_point), + ("surface_normal", self.surface_normal), + ): + if len(vector) != 3 or not all(math.isfinite(value) for value in vector): + raise ValueError(f"Surface {name} must contain three finite values.") + if name != "pivot_point" and math.hypot(*vector) <= 1.0e-8: + raise ValueError(f"Surface {name} must be non-zero.") + if not math.isfinite(self.velocity): + raise ValueError("Surface velocity must be finite.") + if not 0.0 <= self.contact_threshold <= 1.0: raise ValueError(f"Surface contact_threshold must be in [0, 1], got {self.contact_threshold!r}.") - if friction_coefficient < 0.0: - raise ValueError( - f"Surface friction_coefficient must be finite and non-negative, got {self.friction_coefficient!r}." - ) - radius = None if self.radius is None else _validate_scalar("radius", self.radius) - if radius is not None and radius <= 0.0: - raise ValueError(f"Surface radius must be finite and positive when provided, got {self.radius!r}.") - - object.__setattr__(self, "velocity", velocity) - object.__setattr__(self, "direction", _validate_vector("direction", self.direction, 3, nonzero=True)) - object.__setattr__(self, "pivot_point", _validate_vector("pivot_point", self.pivot_point, 3)) - object.__setattr__( - self, "surface_normal", _validate_vector("surface_normal", self.surface_normal, 3, nonzero=True) - ) - object.__setattr__(self, "radius", radius) - object.__setattr__(self, "contact_threshold", contact_threshold) - object.__setattr__(self, "friction_coefficient", friction_coefficient) + if not math.isfinite(self.friction_coefficient) or self.friction_coefficient < 0.0: + raise ValueError("Surface friction_coefficient must be finite and non-negative.") + if self.curved and self.radius is None: + raise ValueError("Curved surfaces require a positive radius.") + if self.radius is not None and (not math.isfinite(self.radius) or self.radius <= 0.0): + raise ValueError("Surface radius must be finite and positive.") def resolve_surface_velocity_paths( @@ -140,32 +89,23 @@ def resolve_surface_velocity_paths( Exact paths ordered by environment, then by ``surface_specs`` order. Raises: - ValueError: If inputs cannot produce one unique path per environment and belt. - TypeError: If a surface description is not a :class:`SurfaceVelocitySpec`. + ValueError: If surface ownership is ambiguous or the clone template is invalid. """ - if not isinstance(num_envs, int) or isinstance(num_envs, bool) or num_envs <= 0: - raise ValueError(f"Conveyor num_envs must be a positive integer, got {num_envs!r}.") - specs = tuple(surface_specs) - if not specs: + if num_envs < 1: + raise ValueError("Conveyor num_envs must be positive.") + if not surface_specs: raise ValueError("At least one SurfaceVelocitySpec is required.") - if not all(isinstance(spec, SurfaceVelocitySpec) for spec in specs): - raise TypeError("Every surface specification must be a SurfaceVelocitySpec.") - if not isinstance(env_path_format, str) or env_path_format.count("{}") != 1: - raise ValueError(f"Conveyor env_path_format must contain exactly one '{{}}', got {env_path_format!r}.") - try: - env_paths = tuple(env_path_format.format(env_id) for env_id in range(num_envs)) - except (IndexError, KeyError, ValueError) as exc: - raise ValueError(f"Invalid conveyor env_path_format: {env_path_format!r}.") from exc - if any(not path.startswith("/") or "{" in path or "}" in path for path in env_paths): - raise ValueError(f"Conveyor env_path_format must produce exact absolute paths, got {env_path_format!r}.") - - templates = tuple(spec.prim_path for spec in specs) - if len(set(templates)) != len(templates): - raise ValueError("Conveyor belt spec paths must be unique within an environment.") - if num_envs > 1 and any(_ENV_REGEX_NS not in path for path in templates): - raise ValueError("Replicated conveyors require every belt prim_path to use {ENV_REGEX_NS}.") - - paths = tuple(template.replace(_ENV_REGEX_NS, env_path) for env_path in env_paths for template in templates) - if len(set(paths)) != len(paths): - raise ValueError("Resolved conveyor prim paths must be unique.") - return paths + templates = tuple(spec.prim_path for spec in surface_specs) + ordered_templates = sorted(templates) + if any( + right == left or right.startswith(f"{left}/") for left, right in zip(ordered_templates, ordered_templates[1:]) + ): + raise ValueError("Conveyor prim paths must be unique and must not be ancestors of one another.") + if env_path_format.count("{}") != 1: + raise ValueError("Conveyor env_path_format must contain exactly one '{}'.") + env_paths = tuple(env_path_format.format(env_id) for env_id in range(num_envs)) + if any( + not path.startswith("/") or not all(part.isidentifier() for part in path[1:].split("/")) for path in env_paths + ): + raise ValueError("Conveyor env_path_format must produce concrete absolute prim paths.") + return tuple(template.replace(_ENV_REGEX_NS, env_path) for env_path in env_paths for template in templates) diff --git a/source/isaaclab_contrib/test/conveyors/test_surface_velocity_spec.py b/source/isaaclab_contrib/test/conveyors/test_surface_velocity_spec.py index 93b07e414069..9e01c56f442f 100644 --- a/source/isaaclab_contrib/test/conveyors/test_surface_velocity_spec.py +++ b/source/isaaclab_contrib/test/conveyors/test_surface_velocity_spec.py @@ -12,51 +12,45 @@ from isaaclab_contrib.conveyors.surface_velocity import SurfaceVelocitySpec, resolve_surface_velocity_paths -def test_surface_velocity_spec_normalizes_vectors() -> None: - """List inputs become immutable vectors before the adapter consumes them.""" - spec = SurfaceVelocitySpec( - prim_path="{ENV_REGEX_NS}/Belt", direction=[3, 4, 0], pivot_point=[1, 2, 3], surface_normal=[0, 0, 1] - ) - assert spec.direction == (3.0, 4.0, 0.0) - assert spec.pivot_point == (1.0, 2.0, 3.0) - assert spec.surface_normal == (0.0, 0.0, 1.0) - - @pytest.mark.parametrize( ("kwargs", "message"), [ ({"prim_path": "relative/Belt"}, "prim_path"), - ({"prim_path": "/"}, "prim_path"), - ({"prim_path": "/World/{ENV_REGEX_NS}/Belt"}, "prim_path"), - ({"prim_path": "/World/Belt", "velocity": None}, "velocity"), - ({"prim_path": "/World/Belt", "velocity": float("nan")}, "velocity"), - ({"prim_path": "/World/Belt", "enabled": 1}, "enabled"), - ({"prim_path": "/World/Belt", "curved": "yes"}, "curved"), - ({"prim_path": "/World/Belt", "direction": (0.0, 0.0, 0.0)}, "direction"), - ({"prim_path": "/World/Belt", "surface_normal": (0.0, 0.0)}, "surface_normal"), - ({"prim_path": "/World/Belt", "radius": 0.0}, "radius"), - ({"prim_path": "/World/Belt", "contact_threshold": 1.1}, "contact_threshold"), - ({"prim_path": "/World/Belt", "friction_coefficient": -0.1}, "friction_coefficient"), + ({"prim_path": "{ENV_REGEX_NS}/Belt.*"}, "prim_path"), + ({"velocity": float("nan")}, "velocity"), + ({"direction": (0.0, 0.0, 0.0)}, "direction"), + ({"surface_normal": (0.0, 0.0)}, "surface_normal"), + ({"pivot_point": (0.0, float("inf"), 0.0)}, "pivot_point"), + ({"curved": True}, "radius"), + ({"radius": 0.0}, "radius"), + ({"contact_threshold": 1.1}, "contact_threshold"), + ({"friction_coefficient": -0.1}, "friction_coefficient"), ], ) def test_surface_velocity_spec_rejects_invalid_authored_values(kwargs: dict, message: str) -> None: """Invalid persistent intent fails before any physics lifecycle is registered.""" + kwargs = {"prim_path": "{ENV_REGEX_NS}/Belt", **kwargs} with pytest.raises(ValueError, match=message): SurfaceVelocitySpec(**kwargs) def test_paths_are_resolved_in_environment_major_order() -> None: """Runtime rows stay deterministic across stage discovery ordering.""" - specs = (SurfaceVelocitySpec("{ENV_REGEX_NS}/BeltA"), SurfaceVelocitySpec("{ENV_REGEX_NS}/Nested/BeltB")) + specs = (SurfaceVelocitySpec("{ENV_REGEX_NS}/Nested/BeltA"), SurfaceVelocitySpec("{ENV_REGEX_NS}/BeltB")) - paths = resolve_surface_velocity_paths(2, specs) + paths = resolve_surface_velocity_paths(2, specs, "/Factory/cell_{}") assert paths == ( - "/World/envs/env_0/BeltA", - "/World/envs/env_0/Nested/BeltB", - "/World/envs/env_1/BeltA", - "/World/envs/env_1/Nested/BeltB", + "/Factory/cell_0/Nested/BeltA", + "/Factory/cell_0/BeltB", + "/Factory/cell_1/Nested/BeltA", + "/Factory/cell_1/BeltB", ) - with pytest.raises(ValueError, match="require every belt"): - resolve_surface_velocity_paths(2, (SurfaceVelocitySpec(prim_path="/World/Shared/Belt"),)) - assert resolve_surface_velocity_paths(1, (SurfaceVelocitySpec("/World/Shared/Belt"),)) == ("/World/Shared/Belt",) + + +@pytest.mark.parametrize("paths", [("Belt", "Belt"), ("Belt/Child", "Belt")]) +def test_paths_reject_ambiguous_surface_ownership(paths: tuple[str, str]) -> None: + """One collider cannot be assigned to duplicate or overlapping conveyor roots.""" + specs = tuple(SurfaceVelocitySpec(f"{{ENV_REGEX_NS}}/{path}") for path in paths) + with pytest.raises(ValueError, match="unique.*ancestors"): + resolve_surface_velocity_paths(1, specs) From a0d4a299ebb4a5a31f8a0a804c80552b9164d503 Mon Sep 17 00:00:00 2001 From: Maximilian Krause Date: Fri, 23 Oct 2026 16:28:15 +0200 Subject: [PATCH 8/9] Limit Newton conveyors to authored surface velocities --- .../maximiliank-conveyor-support.added.rst | 2 +- .../isaaclab_contrib/conveyors/README.md | 6 +- .../isaaclab_contrib/conveyors/newton.py | 607 ++++-------------- .../conveyors/test_surface_velocity_newton.py | 222 ++----- 4 files changed, 189 insertions(+), 648 deletions(-) diff --git a/source/isaaclab_contrib/changelog.d/maximiliank-conveyor-support.added.rst b/source/isaaclab_contrib/changelog.d/maximiliank-conveyor-support.added.rst index c44fddcb77d1..6d71071aca0e 100644 --- a/source/isaaclab_contrib/changelog.d/maximiliank-conveyor-support.added.rst +++ b/source/isaaclab_contrib/changelog.d/maximiliank-conveyor-support.added.rst @@ -1 +1 @@ -* Added experimental straight and curved conveyor controls using Newton's upstream force model. +* Added experimental straight and curved conveyor surfaces using Newton's upstream force model. diff --git a/source/isaaclab_contrib/isaaclab_contrib/conveyors/README.md b/source/isaaclab_contrib/isaaclab_contrib/conveyors/README.md index d6cb67eaa1b2..10d23f25cff1 100644 --- a/source/isaaclab_contrib/isaaclab_contrib/conveyors/README.md +++ b/source/isaaclab_contrib/isaaclab_contrib/conveyors/README.md @@ -10,8 +10,8 @@ experimental and is not exported from Isaac Lab core or the backend packages. including hard resets and registration before CUDA graph capture. It supports the Newton MJWarp solver; other Newton solvers are outside this adapter's scope. -The adapter retains commanded speeds while disabled, restarts its startup ramp -on a full reset, and clears stale forces for selected worlds. The surface spec -controls traction parameters. +Surface speeds and traction parameters come from the authored descriptions. +Full resets restart the startup ramp; episode resets clear stale forces for +only the selected worlds. No meshes, assets, task registrations, or policy checkpoints are bundled here. diff --git a/source/isaaclab_contrib/isaaclab_contrib/conveyors/newton.py b/source/isaaclab_contrib/isaaclab_contrib/conveyors/newton.py index 0207335654f8..70748f6b216f 100644 --- a/source/isaaclab_contrib/isaaclab_contrib/conveyors/newton.py +++ b/source/isaaclab_contrib/isaaclab_contrib/conveyors/newton.py @@ -3,11 +3,7 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Batched surface controls and lifecycle integration for Newton's conveyor force model. - -Newton owns contact classification and traction computation. This adapter resolves replicated -USD surfaces, selects transported bodies, and preserves controls across solver rebuilds. -""" +"""Bind authored conveyor surfaces to Newton's upstream force model and solver lifecycle.""" from __future__ import annotations @@ -16,6 +12,7 @@ from typing import Any import numpy as np +import torch import warp as wp from isaaclab_newton.physics import NewtonManager from newton.examples.basic.example_basic_conveyor_forces import ConveyorForceModel @@ -26,23 +23,7 @@ @wp.kernel -def _update_belt_velocities( - direction: wp.array[wp.vec3], - radius: wp.array[wp.float32], - effective_velocity: wp.array[wp.float32], - linear_velocity: wp.array[wp.vec3], - angular_velocity: wp.array[wp.vec3], -): - conveyor_id = wp.tid() - linear_velocity[conveyor_id] = direction[conveyor_id] * effective_velocity[conveyor_id] - angular_velocity[conveyor_id] = direction[conveyor_id] * (effective_velocity[conveyor_id] / radius[conveyor_id]) - - -@wp.kernel -def _filter_body_forces( - body_is_tracked: wp.array[wp.int32], - body_force: wp.array[wp.spatial_vector], -): +def _filter_body_forces(body_is_tracked: wp.array[wp.int32], body_force: wp.array[wp.spatial_vector]): body_id = wp.tid() if body_is_tracked[body_id] == 0: body_force[body_id] = wp.spatial_vector() @@ -56,31 +37,9 @@ def _advance_startup_scale( scale[0] = wp.min(1.0, elapsed[0] / duration) -@wp.kernel -def _update_effective_velocities( - commanded_velocity: wp.array[wp.float32], - enabled: wp.array[wp.int32], - effective_velocity: wp.array[wp.float32], -): - conveyor_id = wp.tid() - effective_velocity[conveyor_id] = commanded_velocity[conveyor_id] * wp.float32(enabled[conveyor_id]) - - -@wp.kernel -def _gather_values( - source: wp.array(dtype=Any), - indices: wp.array[wp.int32], - values: wp.array(dtype=Any), -): - output_id = wp.tid() - values[output_id] = source[indices[output_id]] - - @wp.kernel def _clear_selected_body_forces( - body_world: wp.array[wp.int32], - world_mask: wp.array[wp.bool], - body_force: wp.array[wp.spatial_vector], + body_world: wp.array[wp.int32], world_mask: wp.array[wp.bool], body_force: wp.array[wp.spatial_vector] ): body_id = wp.tid() world_id = body_world[body_id] @@ -88,66 +47,21 @@ def _clear_selected_body_forces( body_force[body_id] = wp.spatial_vector() -def _as_numpy(values: Any) -> np.ndarray: - """Convert supported tensor-like values to a host NumPy array.""" - if hasattr(values, "detach"): - return values.detach().cpu().numpy() - if hasattr(values, "numpy") and not isinstance(values, np.ndarray): - return values.numpy() - return np.asarray(values) - - -def _world_vector(transform_values: np.ndarray, local_vector: tuple[float, float, float]) -> wp.vec3: - """Rotate a local vector into world space and normalize it.""" - transform = wp.transform( - wp.vec3(*(float(value) for value in transform_values[:3])), - wp.quat(*(float(value) for value in transform_values[3:])), - ) - rotated = wp.transform_vector(transform, wp.vec3(*local_vector)) +def _world_vector(transform: wp.transform, vector: tuple[float, float, float]) -> wp.vec3: + """Rotate and normalize an authored direction or surface normal.""" + rotated = wp.transform_vector(transform, wp.vec3(*vector)) values = np.asarray([float(rotated[index]) for index in range(3)], dtype=np.float32) - norm = float(np.linalg.norm(values)) - if norm <= 1.0e-8: - raise ValueError(f"Conveyor direction or surface normal must be non-zero, got {local_vector}.") - values /= norm + values /= float(np.linalg.norm(values)) return wp.vec3(*(float(value) for value in values)) -def _world_point(transform_values: np.ndarray, local_point: tuple[float, float, float]) -> wp.vec3: - """Transform a local point into world space.""" - transform = wp.transform( - wp.vec3(*(float(value) for value in transform_values[:3])), - wp.quat(*(float(value) for value in transform_values[3:])), - ) - point = wp.transform_point(transform, wp.vec3(*local_point)) - return wp.vec3(*(float(point[index]) for index in range(3))) - - -def _shape_belongs_to_prim(shape_label: str, prim_path: str) -> bool: - """Return whether a Newton collision-shape label is the prim or one of its descendants.""" - return shape_label == prim_path or shape_label.startswith(f"{prim_path.rstrip('/')}/") - - -def _validate_newton_surface_specs(surface_specs: Sequence[SurfaceVelocitySpec]) -> None: - """Validate Newton-specific requirements before registering lifecycle callbacks.""" - prim_paths = [spec.prim_path for spec in surface_specs] - for index, path in enumerate(prim_paths): - for other in prim_paths[index + 1 :]: - if _shape_belongs_to_prim(path, other) or _shape_belongs_to_prim(other, path): - raise ValueError(f"Conveyor prim paths must not be ancestors of one another: {path!r}, {other!r}.") - for spec in surface_specs: - if spec.curved and spec.radius is None: - raise ValueError(f"Newton requires an explicit positive radius for curved belt {spec.prim_path!r}.") - - class SurfaceVelocity: - """Adapt Newton's upstream MuJoCo conveyor force model to the MJWarp lifecycle. + """Own replicated conveyor bindings from model initialization through graph teardown. - The driver is created after the simulation context but before its first - reset. It requests solved contact forces before model finalization, then - binds model-specific buffers after solver initialization and before CUDA - graph capture. A hard simulation reset transparently replaces that binding - with buffers for the re-finalized model. Resolved belt indices use deterministic - environment-major ordering across those rebuilds. + Register after the simulation context is created and before its first reset. + Speeds come from the authored surface descriptions. Newton computes traction + after each solver substep and applies it on the next one. Hard resets rebuild + the binding before CUDA graph capture; episode resets clear selected-world forces. """ def __init__( @@ -156,349 +70,157 @@ def __init__( surface_specs: Sequence[SurfaceVelocitySpec], *, body_pattern: str, - body_count_per_env: int | None = None, + body_count_per_env: int, startup_duration_s: float = 1.0, env_path_format: str = "/World/envs/env_{}", ) -> None: - """Register the force pipeline for the next Newton model initialization. + """Register model and solver callbacks for the authored surfaces. Args: num_envs: Number of replicated simulation environments. - surface_specs: Authored surface descriptions in stable within-environment order. - body_pattern: Regular expression selecting bodies that receive surface traction. - body_count_per_env: Expected selected body count per environment, or ``None``. - startup_duration_s: Duration of the initial traction ramp [s]. - env_path_format: Format string resolving one exact environment root from its integer world index. + surface_specs: Surface descriptions in within-environment order. + body_pattern: Regular expression selecting transported bodies. + body_count_per_env: Expected transported-body count. + startup_duration_s: Initial traction ramp duration [s]. + env_path_format: Clone-template format resolving environment roots. """ - surface_specs = tuple(surface_specs) - prim_paths = resolve_surface_velocity_paths(num_envs, surface_specs, env_path_format) - _validate_newton_surface_specs(surface_specs) + self._surface_specs = tuple(surface_specs) + self._prim_paths = resolve_surface_velocity_paths(num_envs, self._surface_specs, env_path_format) + self._body_pattern = re.compile(body_pattern) if not np.isfinite(startup_duration_s) or startup_duration_s <= 0.0: raise ValueError(f"Conveyor startup duration must be positive, got {startup_duration_s}.") - if not isinstance(body_pattern, str): - raise ValueError(f"body_pattern must be a regular-expression string, got {body_pattern!r}.") - try: - re.compile(body_pattern) - except re.error as exc: - raise ValueError(f"Invalid body pattern: {body_pattern!r}.") from exc - if body_count_per_env is not None and ( - not isinstance(body_count_per_env, int) or isinstance(body_count_per_env, bool) or body_count_per_env < 0 - ): - raise ValueError("body_count_per_env must be a non-negative integer or None.") - self._binding: _SurfaceVelocityBinding | None = None - self._closed = False self._num_envs = num_envs - self._surface_specs = surface_specs - self._binding_kwargs = { - "num_envs": num_envs, - "surface_specs": self._surface_specs, - "startup_duration_s": startup_duration_s, - "prim_paths": prim_paths, - "body_pattern": body_pattern, - "body_count_per_env": body_count_per_env, - } + self._body_count_per_env = body_count_per_env + self._startup_duration_s = startup_duration_s + self._model = None + self._conveyor: ConveyorForceModel | None = None + self._closed = False self._model_init_handle = NewtonManager.register_callback( - self._request_contact_forces, - PhysicsEvent.MODEL_INIT, - name="surface_velocity_contact_attribute", + self._request_contact_forces, PhysicsEvent.MODEL_INIT, name="surface_velocity_contact_attribute" ) - try: - NewtonManager.register_solver_init_callback(self._bind_solver) - except Exception: - self._model_init_handle.deregister() - raise - - def _require_binding(self) -> _SurfaceVelocityBinding: - """Return the current binding or fail before solver initialization.""" - binding = self._binding - if binding is None: - raise RuntimeError("Surface velocity is not bound to an initialized Newton solver.") - return binding - - @property - def num_surfaces(self) -> int: - """Total number of resolved surfaces across all environments.""" - return self._num_envs * len(self._surface_specs) - - @property - def prim_paths(self) -> tuple[str, ...]: - """Resolved Newton shape labels in environment-major belt order.""" - return self._require_binding().surface_paths - - def set_velocities(self, velocities: Any, indices: Any = None) -> None: - """Set signed surface speeds, preserving commands while surfaces are disabled.""" - self._require_binding().set_velocities(velocities, indices) - - def get_velocities(self, indices: Any = None, clone: bool = True) -> wp.array: - """Return effective surface speeds, with disabled surfaces reported as zero.""" - return self._require_binding().get_velocities(indices, clone) - - def get_commanded_velocities(self, indices: Any = None, clone: bool = True) -> wp.array: - """Return staged surface speeds without applying the enabled mask.""" - return self._require_binding().get_commanded_velocities(indices, clone) - - def set_enabled(self, flags: Any, indices: Any = None) -> None: - """Enable or disable selected surfaces without discarding their speed commands.""" - self._require_binding().set_enabled(flags, indices) - - def get_enabled(self, indices: Any = None, clone: bool = True) -> wp.array: - """Return integer enabled flags for selected surfaces.""" - return self._require_binding().get_enabled(indices, clone) - - def reset(self, env_ids: Any = None) -> None: - """Clear stale force state for selected environments.""" - self._require_binding().reset(env_ids) + NewtonManager.register_solver_init_callback(self._bind_solver) def _request_contact_forces(self, _event: Any) -> None: - """Request solved per-contact forces before the Newton model is finalized.""" + """Request solved contact forces before model finalization.""" NewtonManager.request_extended_contact_attribute("force") def _bind_solver(self, model: Any, contacts: Any) -> None: - """Bind graph callbacks and buffers to the newly initialized solver.""" - previous = self._binding - settings = None - if previous is not None: - settings = ( - previous._command_velocity_host.copy(), - previous._enabled_host.copy(), - ) - previous.close() - self._binding = None - - binding = _SurfaceVelocityBinding(model=model, contacts=contacts, **self._binding_kwargs) - if settings is not None: - velocities, enabled = settings - binding.set_velocities(velocities) - binding.set_enabled(enabled) - self._binding = binding - - def close(self) -> None: - """Deregister lifecycle and graph callbacks; repeated calls are safe.""" - if self._closed: - return - NewtonManager.unregister_solver_init_callback(self._bind_solver) - self._model_init_handle.deregister() - if self._binding is not None: - self._binding.close() - self._binding = None - self._closed = True - - -class _SurfaceVelocityBinding: - """Bind replicated surfaces and controls to Newton's conveyor force model.""" - - def __init__( - self, - model: Any, - contacts: Any, - num_envs: int, - surface_specs: Sequence[SurfaceVelocitySpec], - *, - body_pattern: str, - body_count_per_env: int | None = None, - startup_duration_s: float = 1.0, - prim_paths: tuple[str, ...], - ) -> None: - """Initialize the binding before Newton CUDA graph capture. - - Args: - model: Finalized Newton model owned by the active solver. - contacts: Contact buffer owned by the active solver. - num_envs: Number of replicated simulation environments. - surface_specs: Authored surface descriptions in stable within-environment order. - body_pattern: Regular expression selecting bodies that receive surface traction. - body_count_per_env: Expected selected body count per environment, or ``None``. - startup_duration_s: Duration of the initial traction ramp [s]. - prim_paths: Exact collision prim paths in environment-major order. - """ - self._surface_specs = tuple(surface_specs) - compiled_body_pattern = re.compile(body_pattern) - - if model is None or contacts is None: - raise RuntimeError("The conveyor driver requires an initialized Newton model and contact buffer.") - if contacts.force is None: - raise RuntimeError( - "Newton did not allocate per-contact force reporting. The conveyor driver must request the " - "'force' contact attribute before model finalization." - ) - if model.world_count != num_envs: - raise RuntimeError(f"Newton model has {model.world_count} worlds, expected {num_envs}.") - - self._model = model - self._device = model.device - self._num_envs = num_envs - self._startup_duration_s = startup_duration_s - self._closed = False + """Resolve the newly finalized model and register its graph callbacks.""" + if contacts is None or contacts.force is None: + raise RuntimeError("The conveyor requires solved per-contact forces allocated before model finalization.") + if model.world_count != self._num_envs: + raise RuntimeError(f"Newton model has {model.world_count} worlds, expected {self._num_envs}.") surfaces_per_env = len(self._surface_specs) - conveyor_count = num_envs * surfaces_per_env - conveyor_shapes = [-1] * conveyor_count - direction = [wp.vec3() for _ in range(conveyor_count)] - pivot_point = [wp.vec3() for _ in range(conveyor_count)] - radius = [1.0] * conveyor_count - surface_normal = [wp.vec3() for _ in range(conveyor_count)] - surface_paths = [""] * conveyor_count - - shape_body = model.shape_body.numpy() + surface_shapes = [-1] * len(self._prim_paths) shape_world = model.shape_world.numpy() - shape_transform = model.shape_transform.numpy() - seen_sections: set[tuple[int, int]] = set() + shape_body = model.shape_body.numpy() for shape_id, label in enumerate(model.shape_label): world_id = int(shape_world[shape_id]) - if not 0 <= world_id < num_envs: - continue - matching_specs = [ - index - for index in range(surfaces_per_env) - if _shape_belongs_to_prim(label, prim_paths[world_id * surfaces_per_env + index]) - ] - if not matching_specs: + if not 0 <= world_id < self._num_envs: continue - if len(matching_specs) > 1: - raise RuntimeError(f"Conveyor shape {label!r} matches more than one section specification.") - if int(shape_body[shape_id]) >= 0: - raise ValueError(f"Conveyor shape must be static: {label}") - - spec_id = matching_specs[0] - section_key = (world_id, spec_id) - if section_key in seen_sections: - raise RuntimeError( - f"World {world_id} contains multiple shapes matching conveyor section " - f"{self._surface_specs[spec_id].prim_path!r}." - ) - seen_sections.add(section_key) - - spec = self._surface_specs[spec_id] - conveyor_id = world_id * surfaces_per_env + spec_id - conveyor_shapes[conveyor_id] = shape_id - direction[conveyor_id] = _world_vector(shape_transform[shape_id], spec.direction) - pivot_point[conveyor_id] = _world_point(shape_transform[shape_id], spec.pivot_point) - radius[conveyor_id] = 1.0 if spec.radius is None else spec.radius - surface_normal[conveyor_id] = _world_vector(shape_transform[shape_id], spec.surface_normal) - surface_paths[conveyor_id] = label - - expected_sections = { - (world_id, spec_id) for world_id in range(num_envs) for spec_id in range(len(self._surface_specs)) - } - missing_sections = sorted(expected_sections - seen_sections) - if missing_sections: - details = ", ".join( - f"world {world_id}: {self._surface_specs[spec_id].prim_path}" - for world_id, spec_id in missing_sections[:8] - ) - raise RuntimeError(f"Missing {len(missing_sections)} conveyor collision sections ({details}).") + for spec_id in range(surfaces_per_env): + surface_id = world_id * surfaces_per_env + spec_id + path = self._prim_paths[surface_id] + if label != path and not label.startswith(f"{path}/"): + continue + if shape_body[shape_id] >= 0: + raise ValueError(f"Conveyor shape must be static: {label}") + if surface_shapes[surface_id] >= 0: + raise RuntimeError(f"Multiple collision shapes match conveyor surface {path!r}.") + surface_shapes[surface_id] = shape_id + missing = [path for path, shape_id in zip(self._prim_paths, surface_shapes, strict=True) if shape_id < 0] + if missing: + raise RuntimeError(f"Missing {len(missing)} conveyor collision sections: {missing[:8]}.") body_is_tracked = np.zeros(model.body_count, dtype=np.int32) - tracked_counts = np.zeros(num_envs, dtype=np.int32) + tracked_counts = np.zeros(self._num_envs, dtype=np.int32) body_world = model.body_world.numpy() for body_id, label in enumerate(model.body_label): - if compiled_body_pattern.search(label) is None: + if self._body_pattern.search(label) is None: continue world_id = int(body_world[body_id]) - if not 0 <= world_id < num_envs: + if not 0 <= world_id < self._num_envs: raise RuntimeError(f"Transported body {label!r} belongs to invalid world {world_id}.") body_is_tracked[body_id] = 1 tracked_counts[world_id] += 1 - - if body_count_per_env is not None: - bad_worlds = np.flatnonzero(tracked_counts != body_count_per_env) - if bad_worlds.size: - details = ", ".join(f"world {world_id}: {tracked_counts[world_id]}" for world_id in bad_worlds[:8]) - raise RuntimeError( - f"Body pattern {body_pattern!r} expected {body_count_per_env} bodies per world ({details})." - ) + if np.any(tracked_counts != self._body_count_per_env): + raise RuntimeError( + f"Body pattern {self._body_pattern.pattern!r} expected {self._body_count_per_env} bodies per world, " + f"got {tracked_counts.tolist()}." + ) if not np.any(body_is_tracked): - raise RuntimeError(f"Body pattern {body_pattern!r} matched no Newton bodies.") - - self._conveyor = ConveyorForceModel(model, solver_type="mujoco") - for conveyor_id, shape_id in enumerate(conveyor_shapes): - spec = self._surface_specs[conveyor_id % surfaces_per_env] + raise RuntimeError(f"Body pattern {self._body_pattern.pattern!r} matched no Newton bodies.") + + conveyor = ConveyorForceModel(model, solver_type="mujoco") + transforms = model.shape_transform.numpy() + for surface_id, shape_id in enumerate(surface_shapes): + spec = self._surface_specs[surface_id % surfaces_per_env] + values = transforms[shape_id] + transform = wp.transform(wp.vec3(*values[:3]), wp.quat(*values[3:])) + direction = np.asarray(_world_vector(transform, spec.direction), dtype=np.float32) + speed = np.float32(spec.velocity) parameters = { - "surface_normal": surface_normal[conveyor_id], + "surface_normal": _world_vector(transform, spec.surface_normal), "friction": spec.friction_coefficient, "threshold": spec.contact_threshold, } if spec.curved: - self._conveyor.add_pivot_belt(shape_id, pivot_point[conveyor_id], wp.vec3(), **parameters) + conveyor.add_pivot_belt( + shape_id, + wp.transform_point(transform, wp.vec3(*spec.pivot_point)), + wp.vec3(*(direction * (speed / np.float32(spec.radius)))), + **parameters, + ) else: - self._conveyor.add_constant_belt(shape_id, wp.vec3(), **parameters) - self._conveyor.finalize(contacts) - - self._surface_paths = tuple(surface_paths) - self._body_is_tracked = wp.array(body_is_tracked, dtype=wp.int32, device=self._device) - self._direction = wp.array(direction, dtype=wp.vec3, device=self._device) - self._radius = wp.array(radius, dtype=wp.float32, device=self._device) - - authored_velocity = np.asarray([spec.velocity for spec in self._surface_specs], dtype=np.float32) - authored_enabled = np.asarray([spec.enabled for spec in self._surface_specs], dtype=np.int32) - self._command_velocity_host = np.tile(authored_velocity, num_envs) - self._enabled_host = np.tile(authored_enabled, num_envs) - self._command_velocity = wp.array(self._command_velocity_host, dtype=wp.float32, device=self._device) - self._enabled = wp.array(self._enabled_host, dtype=wp.int32, device=self._device) - self._effective_velocity = wp.zeros(conveyor_count, dtype=wp.float32, device=self._device) - self._elapsed_time = wp.zeros(1, dtype=wp.float32, device=self._device) - self._conveyor.set_speed_scale(0.0) - - self._world_mask_host = np.zeros(num_envs, dtype=np.bool_) - self._world_mask = wp.zeros(num_envs, dtype=wp.bool, device=self._device) - self._refresh_effective_velocities() - - NewtonManager.register_state_force_callback(self.apply) - NewtonManager.register_post_solver_substep_callback(self.update) + conveyor.add_constant_belt(shape_id, wp.vec3(*(direction * speed)), **parameters) + conveyor.finalize(contacts) + conveyor.set_speed_scale(0.0) - @property - def surface_paths(self) -> tuple[str, ...]: - """Resolved Newton shape labels in conveyor-index order.""" - return self._surface_paths - - def set_velocities(self, velocities: Any, indices: Any = None) -> None: - """Set signed surface speeds, preserving commands while surfaces are disabled.""" - selected = self._resolve_indices(indices) - self._command_velocity_host[selected] = self._broadcast_1d(velocities, len(selected), "velocities") - self._command_velocity.assign(self._command_velocity_host) - self._refresh_effective_velocities() - - def get_velocities(self, indices: Any = None, clone: bool = True) -> wp.array: - """Return effective surface speeds, with disabled surfaces reported as zero.""" - return self._get_device_values(self._effective_velocity, indices, clone) - - def get_commanded_velocities(self, indices: Any = None, clone: bool = True) -> wp.array: - """Return staged surface speeds without applying the enabled mask.""" - return self._get_device_values(self._command_velocity, indices, clone) - - def set_enabled(self, flags: Any, indices: Any = None) -> None: - """Enable or disable selected surfaces without discarding their speed commands.""" - selected = self._resolve_indices(indices) - values = self._broadcast_1d(flags, len(selected), "enabled flags") - if not np.all(np.isin(values, (0.0, 1.0))): - raise ValueError(f"Surface enabled flags must contain {len(selected)} boolean values.") - self._enabled_host[selected] = values.astype(np.int32) - self._enabled.assign(self._enabled_host) - self._refresh_effective_velocities() - - def get_enabled(self, indices: Any = None, clone: bool = True) -> wp.array: - """Return integer enabled flags for selected surfaces.""" - return self._get_device_values(self._enabled, indices, clone) - - def reset(self, env_ids: Any = None) -> None: - """Clear stale force state for selected environments. + self._model = model + self._conveyor = conveyor + self._body_is_tracked = wp.array(body_is_tracked, dtype=wp.int32, device=model.device) + self._elapsed_time = wp.zeros(1, dtype=wp.float32, device=model.device) + self._world_mask_host = np.zeros(self._num_envs, dtype=np.bool_) + self._world_mask = wp.zeros(self._num_envs, dtype=wp.bool, device=model.device) + NewtonManager.register_state_force_callback(self._apply) + NewtonManager.register_post_solver_substep_callback(self._update) + + def _apply(self, state) -> None: + """Apply the wrench from the preceding solve.""" + self._conveyor.apply(state) - A full reset also restarts the global startup ramp. Partial vectorized - resets leave other environments' conveyor forces and startup state intact. + def _update(self, solver, contacts, state, dt: float) -> None: + """Compute the next wrench using Newton's upstream traction model.""" + wp.launch( + _advance_startup_scale, + dim=1, + inputs=[dt, self._startup_duration_s], + outputs=[self._elapsed_time, self._conveyor.global_velocity_scale], + device=self._model.device, + ) + self._conveyor.update(solver, contacts, state, dt) + wp.launch( + _filter_body_forces, + dim=self._model.body_count, + inputs=[self._body_is_tracked], + outputs=[self._conveyor.conveyor_body_f], + device=self._model.device, + ) - Args: - env_ids: Environment indices to reset, or ``None`` for every environment. - """ + def reset(self, env_ids: Sequence[int] | torch.Tensor | None = None) -> None: + """Clear selected-world forces; a full reset also restarts the startup ramp.""" + if self._conveyor is None: + raise RuntimeError("Surface velocity is not bound to an initialized Newton solver.") if env_ids is None: self._conveyor.conveyor_body_f.zero_() self._elapsed_time.zero_() self._conveyor.global_velocity_scale.zero_() return - - ids = _as_numpy(env_ids) + ids = env_ids.detach().cpu().numpy() if isinstance(env_ids, torch.Tensor) else np.asarray(env_ids) if not np.issubdtype(ids.dtype, np.integer): raise IndexError(f"Surface reset environment indices must be integers, got {env_ids!r}.") - ids = ids.astype(np.int64, copy=False).reshape(-1) + ids = ids.reshape(-1) if np.any((ids < 0) | (ids >= self._num_envs)): raise IndexError(f"Conveyor reset environment indices are out of range: {ids.tolist()}.") self._world_mask_host.fill(False) @@ -512,100 +234,17 @@ def reset(self, env_ids: Any = None) -> None: dim=self._model.body_count, inputs=[self._model.body_world, self._world_mask], outputs=[self._conveyor.conveyor_body_f], - device=self._device, + device=self._model.device, ) def close(self) -> None: - """Deregister Newton callbacks and release references held by the driver.""" + """Detach lifecycle and graph callbacks before the model is released.""" if self._closed: return - NewtonManager.unregister_state_force_callback(self.apply) - NewtonManager.unregister_post_solver_substep_callback(self.update) + NewtonManager.unregister_solver_init_callback(self._bind_solver) + self._model_init_handle.deregister() + NewtonManager.unregister_state_force_callback(self._apply) + NewtonManager.unregister_post_solver_substep_callback(self._update) + self._conveyor = None + self._model = None self._closed = True - - def apply(self, state) -> None: - """Apply the wrench computed from the preceding physics solve.""" - self._conveyor.apply(state) - - def update(self, solver, contacts, state, dt: float) -> None: - """Advance the startup ramp and let Newton compute the next conveyor wrench.""" - wp.launch( - _advance_startup_scale, - dim=1, - inputs=[dt, self._startup_duration_s], - outputs=[self._elapsed_time, self._conveyor.global_velocity_scale], - device=self._device, - ) - self._conveyor.update(solver, contacts, state, dt) - # Exclude robot links and other bodies outside the transported-body selection. - wp.launch( - _filter_body_forces, - dim=self._model.body_count, - inputs=[self._body_is_tracked], - outputs=[self._conveyor.conveyor_body_f], - device=self._device, - ) - - def _resolve_indices(self, indices: Any) -> np.ndarray: - """Normalize and validate a surface index selection.""" - if indices is None: - return np.arange(len(self._surface_paths), dtype=np.int64) - if isinstance(indices, slice): - return np.arange(len(self._surface_paths), dtype=np.int64)[indices] - selected = _as_numpy(indices) - if selected.dtype == np.bool_: - if selected.ndim != 1 or selected.size != len(self._surface_paths): - raise IndexError(f"Boolean surface indices must have length {len(self._surface_paths)}.") - return np.flatnonzero(selected).astype(np.int64) - if not np.issubdtype(selected.dtype, np.integer): - raise IndexError(f"Surface indices must be integers, got {indices!r}.") - selected = selected.astype(np.int64, copy=False).reshape(-1) - if np.any((selected < 0) | (selected >= len(self._surface_paths))): - raise IndexError(f"Conveyor surface indices are out of range: {selected.tolist()}.") - return selected - - def _refresh_effective_velocities(self) -> None: - """Apply the enabled mask at the one device-side command seam.""" - wp.launch( - _update_effective_velocities, - dim=len(self._surface_paths), - inputs=[self._command_velocity, self._enabled], - outputs=[self._effective_velocity], - device=self._device, - ) - wp.launch( - _update_belt_velocities, - dim=len(self._surface_paths), - inputs=[self._direction, self._radius, self._effective_velocity], - outputs=[self._conveyor.conv_const_vel, self._conveyor.conv_pivot_angvel], - device=self._device, - ) - - def _get_device_values(self, source: wp.array, indices: Any, clone: bool) -> wp.array: - """Clone a complete device buffer or gather a selected subset.""" - if indices is None: - return wp.clone(source) if clone else source - selected = self._resolve_indices(indices) - selected_device = wp.array(selected, dtype=wp.int32, device=self._device) - values = wp.empty(len(selected), dtype=source.dtype, device=self._device) - if len(selected) > 0: - wp.launch( - _gather_values, - dim=len(selected), - inputs=[source, selected_device], - outputs=[values], - device=self._device, - ) - return values - - @staticmethod - def _broadcast_1d(values: Any, count: int, name: str) -> np.ndarray: - """Broadcast one scalar or validate one value per selected surface.""" - array = np.asarray(_as_numpy(values), dtype=np.float32) - if not np.all(np.isfinite(array)): - raise ValueError(f"Conveyor {name} must contain only finite values.") - if array.ndim == 0 or array.size == 1: - return np.full(count, float(array.reshape(-1)[0]), dtype=np.float32) - if array.ndim != 1 or array.size != count: - raise ValueError(f"Conveyor {name} need one value or {count} values, got shape {array.shape}.") - return array diff --git a/source/isaaclab_contrib/test/conveyors/test_surface_velocity_newton.py b/source/isaaclab_contrib/test/conveyors/test_surface_velocity_newton.py index a4030da85fa4..96d2c98f5ae0 100644 --- a/source/isaaclab_contrib/test/conveyors/test_surface_velocity_newton.py +++ b/source/isaaclab_contrib/test/conveyors/test_surface_velocity_newton.py @@ -3,7 +3,7 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Native CPU contracts for the Newton conveyor adapter.""" +"""Native transport, reset, and callback-lifetime contracts for the Newton adapter.""" import numpy as np import pytest @@ -17,187 +17,89 @@ from isaaclab_contrib.conveyors.surface_velocity import SurfaceVelocitySpec -def test_native_conveyor_controls_rebind_and_reset_independently(monkeypatch): - """Real contacts, models and callbacks preserve commands and discard only selected-world traction.""" +@pytest.mark.parametrize("device", test_devices(DeviceScope.CPU_AND_DEFAULT_CUDA)) +def test_transport_reset_rebind_and_close(device): + """Real straight/pivot traction survives hard resets and leaves no eager or captured callbacks.""" cfg = SimulationCfg( - device="cpu", + device=device, dt=1 / 120, - physics=NewtonCfg(solver_cfg=MJWarpSolverCfg(use_mujoco_contacts=False), use_cuda_graph=False), + physics=NewtonCfg( + solver_cfg=MJWarpSolverCfg(use_mujoco_contacts=False, integrator="implicitfast", cone="elliptic"), + use_cuda_graph=device != "cpu", + ), ) with build_simulation_context(sim_cfg=cfg) as sim: builder = sim.get_or_create_backend(NewtonBuilderCfg(physics_cfg=cfg.physics)) for world in range(2): builder.begin_world() root = f"/World/envs/env_{world}" - for name, y in (("B", 0.5), ("A", -0.5), ("Nested/BeltA", 2.0)): - label = f"{root}/{name}" if "/" in name else f"{root}/Belt{name}" + for name, y in (("B", 0.5), ("A", -0.5)): builder.add_shape_box( - -1, xform=wp.transform((0, y, 0), wp.quat_identity()), hx=1, hy=0.2, hz=0.1, label=label + -1, + xform=wp.transform((0, y, 0), wp.quat_identity()), + hx=1, + hy=0.35, + hz=0.1, + cfg=builder.ShapeConfig(mu=1.1e-5), + label=f"{root}/Belt{name}", ) - cube = builder.add_link(xform=wp.transform((0, -0.5, 0.21), wp.quat_identity()), label=f"{root}/Cube0") - builder.add_shape_box(cube, hx=0.1, hy=0.1, hz=0.1) - builder.add_articulation([builder.add_joint_free(cube)]) + for name, x, y in (("Cube", 0, -0.5 if world == 0 else 0.5), ("Untracked", -0.5, -0.5)): + body = builder.add_link(xform=wp.transform((x, y, 0.21), wp.quat_identity()), label=f"{root}/{name}") + builder.add_shape_box(body, hx=0.1, hy=0.1, hz=0.1, cfg=builder.ShapeConfig(mu=1.1e-5)) + builder.add_articulation([builder.add_joint_free(body)]) builder.end_world() - builder.add_shape_box(-1, hx=0.1, hy=0.1, hz=0.1, label="/World/props/BeltA") callbacks_before = set(NewtonManager._callbacks) - specs = ( - SurfaceVelocitySpec("{ENV_REGEX_NS}/BeltA", velocity=0.1), - SurfaceVelocitySpec("{ENV_REGEX_NS}/BeltB", velocity=-0.2, enabled=False), - ) - calls, bindings = [], [] - - def actuator(): - calls.append("actuator") - - def force(state): - calls.append("force") - - def substep(solver, contacts, state, dt): - calls.append("substep") - - def initialized(model, contacts): - bindings.append((model, contacts)) - - NewtonManager.register_post_actuator_callback(actuator) - observers = ( - (force, NewtonManager.register_state_force_callback, NewtonManager.unregister_state_force_callback), + driver = SurfaceVelocity( + 2, ( - substep, - NewtonManager.register_post_solver_substep_callback, - NewtonManager.unregister_post_solver_substep_callback, + SurfaceVelocitySpec("{ENV_REGEX_NS}/BeltA", velocity=0.2), + SurfaceVelocitySpec( + "{ENV_REGEX_NS}/BeltB", + velocity=0.2, + curved=True, + direction=(0, 0, 1), + pivot_point=(-0.5, 0, 0), + radius=0.5, + ), ), - (initialized, NewtonManager.register_solver_init_callback, NewtonManager.unregister_solver_init_callback), + body_pattern="/Cube$", + body_count_per_env=1, + startup_duration_s=2.0, ) - for callback, register, _ in observers: - register(callback) - NewtonManager.register_solver_init_callback(initialized) - driver = SurfaceVelocity(2, specs, body_pattern="Cube0$", body_count_per_env=1) try: - with pytest.raises(RuntimeError, match="not bound"): - driver.set_velocities(0.2) - sim.reset() - assert bindings == [(NewtonManager.get_model(), NewtonManager.get_contacts())] - first = driver._binding - assert NewtonManager.get_contacts().force is not None - assert driver.prim_paths == tuple( - f"/World/envs/env_{world}/Belt{name}" for world in range(2) for name in ("A", "B") - ) - model = NewtonManager.get_model() - mapping = first._conveyor.shape_conveyor.numpy() - assert [ - mapping[i] for i, label in enumerate(model.shape_label) if "Nested" in label or "/props/" in label - ] == [-1] * 3 - driver.set_velocities([0.3, -0.25, -0.4], indices=[0, 1, 3]) - driver.set_enabled([False, True], indices=[2, 3]) - np.testing.assert_allclose(driver.get_velocities().numpy(), [0.3, 0, 0, -0.4]) - np.testing.assert_allclose(driver.get_velocities(indices=[3, 0]).numpy(), [-0.4, 0.3]) - np.testing.assert_array_equal(driver.get_enabled(indices=[2, 0]).numpy(), [0, 1]) - sim.step(render=False) - assert calls == ["actuator"] + ["force", "substep"] * cfg.physics.num_substeps - first._conveyor.conveyor_body_f.assign(np.ones((2, 6), dtype=np.float32)) + previous_model = None + for _ in range(2): + sim.reset() + model = NewtonManager.get_model() + assert model is not previous_model + previous_model = model + before = NewtonManager.get_state_0().body_q.numpy().copy() + for _ in range(120): + sim.step(render=False) + after = NewtonManager.get_state_0().body_q.numpy() + assert after[0, 0] - before[0, 0] > 0.025 # Straight belt in world 0. + assert after[2, 1] - before[2, 1] > 0.025 # Pivot belt in world 1. + np.testing.assert_allclose(after[[1, 3], :2], before[[1, 3], :2], atol=0.002) + assert np.isfinite(after).all() and np.all(after[:, 2] > 0.15) + + forces = driver._conveyor.conveyor_body_f + forces.assign(np.ones((4, 6), dtype=np.float32)) driver.reset([0]) state = model.state() - first.apply(state) - np.testing.assert_array_equal(state.body_f.numpy(), [[0] * 6, [1] * 6]) - sim.reset() - assert first._closed and driver._binding is not first - assert driver._binding._model is NewtonManager.get_model() - np.testing.assert_allclose(driver.get_velocities().numpy(), [0.3, 0, 0, -0.4]) - assert len(bindings) == 2 and bindings[1] == (NewtonManager.get_model(), NewtonManager.get_contacts()) - assert bindings[0][0] is not bindings[1][0] and bindings[0][1] is not bindings[1][1] - for callback, _, unregister in observers: - unregister(callback) - unregister(callback) - previous_calls = list(calls) - sim.step(render=False) - sim.reset() - assert calls == previous_calls and len(bindings) == 2 - np.testing.assert_array_equal(driver.get_enabled().numpy(), [1, 0, 0, 1]) - np.testing.assert_allclose(driver.get_commanded_velocities().numpy(), [0.3, -0.25, 0.1, -0.4]) - callbacks_live = set(NewtonManager._callbacks) - with monkeypatch.context() as patch: - - def fail_registration(cls, callback): - raise RuntimeError("solver callback unavailable") - - patch.setattr(NewtonManager, "register_solver_init_callback", classmethod(fail_registration)) - with pytest.raises(RuntimeError, match="solver callback unavailable"): - SurfaceVelocity(2, specs, body_pattern="Cube0$") - assert set(NewtonManager._callbacks) == callbacks_live + driver._conveyor.apply(state) + np.testing.assert_array_equal(state.body_f.numpy(), [[0] * 6, [0] * 6, [1] * 6, [1] * 6]) + driver.reset([1, 0]) + np.testing.assert_array_equal(forces.numpy(), np.zeros((4, 6))) + assert driver._conveyor.global_velocity_scale.numpy()[0] == 0 + if device != "cpu": + assert NewtonManager._graph is not None finally: driver.close() driver.close() - assert set(NewtonManager._callbacks) == callbacks_before assert not NewtonManager._solver_init_callbacks assert not NewtonManager._state_force_callbacks assert not NewtonManager._post_solver_substep_callbacks - - -@pytest.mark.parametrize( - ("specs", "num_envs", "env_path", "error", "message"), - [ - ((), 1, "/World/envs/env_{}", ValueError, "At least one"), - ((object(),), 1, "/World/envs/env_{}", TypeError, "SurfaceVelocitySpec"), - ((SurfaceVelocitySpec("/Belt"),) * 2, 1, "/World/envs/env_{}", ValueError, "unique"), - ( - (SurfaceVelocitySpec("/Belt"), SurfaceVelocitySpec("/Belt/Child")), - 1, - "/World/envs/env_{}", - ValueError, - "ancestors", - ), - ((SurfaceVelocitySpec("/Curve", curved=True),), 1, "/World/envs/env_{}", ValueError, "positive radius"), - ((SurfaceVelocitySpec("/Shared/Belt"),), 2, "/World/envs/env_{}", ValueError, "Replicated"), - ((SurfaceVelocitySpec("{ENV_REGEX_NS}/Belt"),), 1, "/World/envs/env_.*", ValueError, "env_path_format"), - ( - (SurfaceVelocitySpec("{ENV_REGEX_NS}/Belt"),), - 1, - "/World/envs/env_{}/{missing}", - ValueError, - "env_path_format", - ), - ], -) -def test_invalid_conveyors_leave_no_callbacks(specs, num_envs, env_path, error, message): - """Rejected authoring must not leave a partially registered adapter.""" - before = set(NewtonManager._callbacks) - with pytest.raises(error, match=message): - SurfaceVelocity(num_envs, specs, env_path_format=env_path, body_pattern="Cube0$") - assert set(NewtonManager._callbacks) == before - - -@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) -def test_removed_callbacks_leave_captured_graph(device): - """Removed callbacks stop writing GPU buffers in eager and recaptured simulation steps.""" - cfg = SimulationCfg(device=device, physics=NewtonCfg(use_cuda_graph=True)) - with build_simulation_context(sim_cfg=cfg) as sim: - builder = sim.get_or_create_backend(NewtonBuilderCfg(physics_cfg=cfg.physics)) - body = builder.add_link(xform=wp.transform_identity()) - builder.add_shape_box(body, hx=0.1, hy=0.1, hz=0.1) - builder.add_articulation([builder.add_joint_free(body)]) - observed = wp.zeros(2, device=device) - ones = wp.ones(2, device=device) - - def force(state): - wp.copy(observed, ones, dest_offset=0, count=1) - - def substep(solver, contacts, state, dt): - wp.copy(observed, ones, dest_offset=1, count=1) - - NewtonManager.register_state_force_callback(force) - NewtonManager.register_post_solver_substep_callback(substep) - sim.reset() - sim.step(render=False) - assert NewtonManager._graph is not None - np.testing.assert_array_equal(observed.numpy(), [1, 1]) - NewtonManager.unregister_state_force_callback(force) - NewtonManager.unregister_post_solver_substep_callback(substep) - observed.zero_() - sim.step(render=False) - np.testing.assert_array_equal(observed.numpy(), [0, 0]) - graph = NewtonManager._graph - NewtonManager.unregister_state_force_callback(force) - NewtonManager.unregister_post_solver_substep_callback(substep) - assert NewtonManager._graph is graph + assert set(NewtonManager._callbacks) == callbacks_before + forces.fill_(1.0) sim.step(render=False) - np.testing.assert_array_equal(observed.numpy(), [0, 0]) + np.testing.assert_array_equal(forces.numpy(), np.ones((4, 6))) From df0531972eb75ba1c50af1d03e85f670d0910015 Mon Sep 17 00:00:00 2001 From: Maximilian Krause Date: Mon, 26 Oct 2026 06:26:18 +0100 Subject: [PATCH 9/9] fix: Use shared conveyor clone defaults Use the clone configuration owner for existing conveyor namespace defaults. This preserves the public values while satisfying the current clone-ownership check across the integrated task stack. Recorded upstream main: e907f82edf18223be68df8f0201ed2660d9798fa --- .../changelog.d/conveyor-clone-default.fixed.rst | 1 + source/isaaclab_contrib/isaaclab_contrib/conveyors/newton.py | 3 ++- .../isaaclab_contrib/conveyors/surface_velocity.py | 4 +++- 3 files changed, 6 insertions(+), 2 deletions(-) create mode 100644 source/isaaclab_contrib/changelog.d/conveyor-clone-default.fixed.rst diff --git a/source/isaaclab_contrib/changelog.d/conveyor-clone-default.fixed.rst b/source/isaaclab_contrib/changelog.d/conveyor-clone-default.fixed.rst new file mode 100644 index 000000000000..b8217db029c1 --- /dev/null +++ b/source/isaaclab_contrib/changelog.d/conveyor-clone-default.fixed.rst @@ -0,0 +1 @@ +* Fixed conveyor namespace defaults to follow the shared clone configuration. diff --git a/source/isaaclab_contrib/isaaclab_contrib/conveyors/newton.py b/source/isaaclab_contrib/isaaclab_contrib/conveyors/newton.py index c91e2d079853..29e426209bec 100644 --- a/source/isaaclab_contrib/isaaclab_contrib/conveyors/newton.py +++ b/source/isaaclab_contrib/isaaclab_contrib/conveyors/newton.py @@ -18,6 +18,7 @@ from newton.examples.basic.example_basic_conveyor_forces import ConveyorForceModel from newton.solvers import SolverBase +from isaaclab.cloner.cloner_cfg import DEFAULT_ENV_TEMPLATE from isaaclab.physics import PhysicsEvent from .surface_velocity import SurfaceVelocitySpec, resolve_surface_velocity_paths @@ -74,7 +75,7 @@ def __init__( body_pattern: str, body_count_per_env: int, startup_duration_s: float = 1.0, - env_path_format: str = "/World/envs/env_{}", + env_path_format: str = DEFAULT_ENV_TEMPLATE, ) -> None: """Register model and solver callbacks for the authored surfaces. diff --git a/source/isaaclab_contrib/isaaclab_contrib/conveyors/surface_velocity.py b/source/isaaclab_contrib/isaaclab_contrib/conveyors/surface_velocity.py index ab4113d0a38a..639f08796363 100644 --- a/source/isaaclab_contrib/isaaclab_contrib/conveyors/surface_velocity.py +++ b/source/isaaclab_contrib/isaaclab_contrib/conveyors/surface_velocity.py @@ -11,6 +11,8 @@ from collections.abc import Sequence from dataclasses import dataclass +from isaaclab.cloner.cloner_cfg import DEFAULT_ENV_TEMPLATE + _ENV_REGEX_NS = "{ENV_REGEX_NS}" @@ -76,7 +78,7 @@ def __post_init__(self) -> None: def resolve_surface_velocity_paths( num_envs: int, surface_specs: Sequence[SurfaceVelocitySpec], - env_path_format: str = "/World/envs/env_{}", + env_path_format: str = DEFAULT_ENV_TEMPLATE, ) -> tuple[str, ...]: """Resolve conveyor templates to exact environment-major prim paths.