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/changelog.d/maximiliank-conveyor-support.added.rst b/source/isaaclab_contrib/changelog.d/maximiliank-conveyor-support.added.rst new file mode 100644 index 000000000000..6d71071aca0e --- /dev/null +++ b/source/isaaclab_contrib/changelog.d/maximiliank-conveyor-support.added.rst @@ -0,0 +1 @@ +* Added experimental straight and curved conveyor surfaces using Newton's upstream force model. 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..4d7624b34844 --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/conveyors/README.md @@ -0,0 +1,19 @@ +# Experimental conveyor support + +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. +- `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. + Pass the simulation context's `backend_manager`; solved contact forces use + Newton's solver observables rather than the legacy contact-force attribute. + +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/__init__.py b/source/isaaclab_contrib/isaaclab_contrib/conveyors/__init__.py new file mode 100644 index 000000000000..9ceb327848ad --- /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. + +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 new file mode 100644 index 000000000000..8dc356f0d5ca --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/conveyors/newton.py @@ -0,0 +1,259 @@ +# 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 + +"""Bind authored conveyor surfaces to Newton's upstream force model and solver lifecycle.""" + +from __future__ import annotations + +import re +from collections.abc import Sequence +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 +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 + + +@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 _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() + + +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) + values /= float(np.linalg.norm(values)) + return wp.vec3(*(float(value) for value in values)) + + +class SurfaceVelocity: + """Own replicated conveyor bindings from model initialization through graph teardown. + + 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__( + self, + num_envs: int, + surface_specs: Sequence[SurfaceVelocitySpec], + *, + manager: NewtonManager, + body_pattern: str, + body_count_per_env: int, + startup_duration_s: float = 1.0, + env_path_format: str = DEFAULT_ENV_TEMPLATE, + ) -> None: + """Register model and solver callbacks for the authored surfaces. + + Args: + num_envs: Number of replicated simulation environments. + surface_specs: Surface descriptions in within-environment order. + manager: Newton backend manager owned by the simulation context. + 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. + """ + 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}.") + self._num_envs = num_envs + self._manager = manager + 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 + manager.request_solver_observables(SolverBase.ObservableKind.CONTACT_F) + self._physics_ready_handle = manager.register_callback( + self._queue_solver_binding, PhysicsEvent.PHYSICS_READY, name="surface_velocity_solver_init" + ) + + def _queue_solver_binding(self, _event: Any) -> None: + """Bind after solver allocation on initial construction and every hard reset.""" + self._manager.register_solver_init_callback(self._bind_solver) + + def _bind_solver(self) -> None: + """Resolve the newly finalized model and register its graph callbacks.""" + backend = self._manager.backend + model = backend.model + contacts = backend.contacts + observables = self._manager.get_solver_observables() + if contacts is None or observables is None or observables.contact_f is None: + raise RuntimeError("The conveyor requires solved per-contact force observables.") + 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) + surface_shapes = [-1] * len(self._prim_paths) + shape_world = model.shape_world.numpy() + 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 < self._num_envs: + continue + 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(self._num_envs, dtype=np.int32) + body_world = model.body_world.numpy() + for body_id, label in enumerate(model.body_label): + if self._body_pattern.search(label) is None: + continue + world_id = int(body_world[body_id]) + 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 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 {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": _world_vector(transform, spec.surface_normal), + "friction": spec.friction_coefficient, + "threshold": spec.contact_threshold, + } + if spec.curved: + 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: + conveyor.add_constant_belt(shape_id, wp.vec3(*(direction * speed)), **parameters) + conveyor.finalize(contacts, observables) + conveyor.set_speed_scale(0.0) + + 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) + self._manager.register_state_force_callback(self._apply) + self._manager.register_post_solver_substep_callback(self._update) + + def _apply(self, state) -> None: + """Apply the wrench from the preceding solve.""" + self._conveyor.apply(state) + + def _update(self, contacts, observables, 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(contacts, observables, 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, + ) + + 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 = 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.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._model.device, + ) + + def close(self) -> None: + """Detach lifecycle and graph callbacks before the model is released.""" + if self._closed: + return + self._manager.unregister_solver_init_callback(self._bind_solver) + self._physics_ready_handle.deregister() + self._manager.unregister_state_force_callback(self._apply) + self._manager.unregister_post_solver_substep_callback(self._update) + self._conveyor = None + self._model = None + self._closed = True 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..639f08796363 --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/conveyors/surface_velocity.py @@ -0,0 +1,113 @@ +# 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 descriptions of prescribed conveyor surface velocities.""" + +from __future__ import annotations + +import math +from collections.abc import Sequence +from dataclasses import dataclass + +from isaaclab.cloner.cloner_cfg import DEFAULT_ENV_TEMPLATE + +_ENV_REGEX_NS = "{ENV_REGEX_NS}" + + +@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. Applied forces and reset state belong to the adapter. + + ``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: 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]. + 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. + """ + + prim_path: str + velocity: float = 0.0 + 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: + """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 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( + num_envs: int, + surface_specs: Sequence[SurfaceVelocitySpec], + env_path_format: str = DEFAULT_ENV_TEMPLATE, +) -> 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 surface ownership is ambiguous or the clone template is invalid. + """ + if num_envs < 1: + raise ValueError("Conveyor num_envs must be positive.") + if not surface_specs: + raise ValueError("At least one SurfaceVelocitySpec is required.") + 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_newton.py b/source/isaaclab_contrib/test/conveyors/test_surface_velocity_newton.py new file mode 100644 index 000000000000..73dd02dddcdf --- /dev/null +++ b/source/isaaclab_contrib/test/conveyors/test_surface_velocity_newton.py @@ -0,0 +1,107 @@ +# 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 transport, reset, and callback-lifetime contracts for the Newton adapter.""" + +import numpy as np +import pytest +import warp as wp +from isaaclab_newton.physics import MJWarpSolverCfg, NewtonBuilderCfg, NewtonCfg + +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 + + +@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=device, + dt=1 / 120, + 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: + manager = sim.backend_manager + 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)): + builder.add_shape_box( + -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}", + ) + 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() + callbacks_before = set(manager._callbacks) + driver = SurfaceVelocity( + 2, + ( + 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, + ), + ), + manager=manager, + body_pattern="/Cube$", + body_count_per_env=1, + startup_duration_s=2.0, + ) + try: + previous_model = None + for _ in range(2): + sim.reset() + model = manager.backend.model + assert model is not previous_model + previous_model = model + before = manager.backend.state_0.body_q.numpy().copy() + for _ in range(120): + sim.step(render=False) + after = manager.backend.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() + 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 manager._graph is not None + finally: + driver.close() + driver.close() + assert not manager._solver_init_callbacks + assert not manager._state_force_callbacks + assert not manager._post_solver_substep_callbacks + assert set(manager._callbacks) == callbacks_before + forces.fill_(1.0) + sim.step(render=False) + np.testing.assert_array_equal(forces.numpy(), np.ones((4, 6))) 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..9e01c56f442f --- /dev/null +++ b/source/isaaclab_contrib/test/conveyors/test_surface_velocity_spec.py @@ -0,0 +1,56 @@ +# 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 import-light surface-velocity descriptions.""" + +from __future__ import annotations + +import pytest + +from isaaclab_contrib.conveyors.surface_velocity import SurfaceVelocitySpec, resolve_surface_velocity_paths + + +@pytest.mark.parametrize( + ("kwargs", "message"), + [ + ({"prim_path": "relative/Belt"}, "prim_path"), + ({"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}/Nested/BeltA"), SurfaceVelocitySpec("{ENV_REGEX_NS}/BeltB")) + + paths = resolve_surface_velocity_paths(2, specs, "/Factory/cell_{}") + + assert paths == ( + "/Factory/cell_0/Nested/BeltA", + "/Factory/cell_0/BeltB", + "/Factory/cell_1/Nested/BeltA", + "/Factory/cell_1/BeltB", + ) + + +@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) 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..fbe26da77e7e --- /dev/null +++ b/source/isaaclab_newton/changelog.d/maximiliank-conveyor-lifecycle.added.rst @@ -0,0 +1,2 @@ +* Added requested solver observables 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..305671fa3fa8 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 static-kinematic and kinematic-kinematic contacts. + + 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 """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 d1ae69db52e7..2409a7d03612 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py @@ -376,6 +376,7 @@ def _close_backend(self, *, dispatch_stop: bool = True) -> None: # The CUDA graph points at the old model's freed buffers (CUDA 700 on the next step). self._graph = None self._graph_capture_pending = False + self._solver_observables = None self._eval_fk = _eval_fk_unbound self._reset_solver_internals_delegate = _reset_solver_internals_unbound self._use_newton_actuators_active = False @@ -577,6 +578,11 @@ def clear(self): self._pre_actuator_callbacks: list[Callable[[], None]] = [] self._post_actuator_callbacks: list[Callable[[], None]] = [] self._state_force_callbacks: list[Callable[[State], None]] = [] + self._post_solver_substep_callbacks: list[ + Callable[[Contacts | None, SolverBase.Observables | None, State, float], None] + ] = [] + self._requested_solver_observables: set[str] = set() + self._solver_observables: SolverBase.Observables | None = None self._post_step_callbacks: list[Callable[[], None]] = [] # Substeps clear the input state's forces, so each one re-applies this copy of the staged body forces. self._staged_body_f: wp.array | None = None @@ -1224,7 +1230,7 @@ def _step_solver( Default invokes the backend's solver once. Subclasses can override to batch multiple solvers within a single substep. """ - self.backend.solver.step(state_0, state_1, control, contacts, substep_dt) + self.backend.solver.step(state_0, state_1, control, contacts, substep_dt, observables=self._solver_observables) def _solver_specific_clear(self) -> None: """Solver-specific cleanup hook called from :meth:`clear`. @@ -1299,6 +1305,11 @@ def initialize_solver(self) -> None: "self._supports_rigid_body_force_input." ) self._initialize_contacts() + self._solver_observables = ( + backend.solver.observables(self._requested_solver_observables, contacts=backend.contacts) + if self._requested_solver_observables + else None + ) state_0 = backend.state_0 self._staged_body_f = wp.zeros_like(state_0.body_f) if state_0.body_count else None @@ -1385,6 +1396,8 @@ def _run_solver_substeps(self, contacts) -> None: for callback in self._state_force_callbacks: callback(backend.state_0) self._step_solver(backend.state_0, backend.state_0, backend.control, contacts, backend.solver_dt) + for callback in self._post_solver_substep_callbacks: + callback(contacts, self._solver_observables, backend.state_0, backend.solver_dt) backend.state_0.clear_forces() if collide_mid_loop and (i + 1) % collide_every == 0 and i + 1 < num_substeps: backend.collision_pipeline.collide(backend.state_0, contacts) @@ -1395,6 +1408,8 @@ def _run_solver_substeps(self, contacts) -> None: for callback in self._state_force_callbacks: callback(backend.state_0) self._step_solver(backend.state_0, backend.state_1, backend.control, contacts, backend.solver_dt) + for callback in self._post_solver_substep_callbacks: + callback(contacts, self._solver_observables, backend.state_1, backend.solver_dt) if need_copy_on_last and i == num_substeps - 1: backend.state_0.assign(backend.state_1) else: @@ -1488,6 +1503,14 @@ def create_visual_shape_color_writer( view = ArticulationView(model, prim_paths, verbose=False) return VisualShapeColorWriter(model, view, body_names) + def request_solver_observables(self, *kinds: str) -> None: + """Request solver outputs before initialization; requests survive hard resets.""" + self._requested_solver_observables.update(kinds) + + def get_solver_observables(self) -> SolverBase.Observables | None: + """Return the outputs populated by each solver substep, or ``None`` when unrequested.""" + return self._solver_observables + def get_scene_data_provider(self) -> SceneDataProvider: """Return the active scene data provider.""" return SimulationContext.instance().get_scene_data_provider() @@ -1579,6 +1602,34 @@ def register_state_force_callback(self, callback: Callable[[State], None]) -> No return self._state_force_callbacks.append(callback) + def unregister_state_force_callback(self, callback: Callable[[State], None]) -> None: + """Detach a force callback and recapture an active graph without it.""" + if callback in self._state_force_callbacks: + self._state_force_callbacks.remove(callback) + if self._graph is not None: + self._invalidate_graph() + + def register_post_solver_substep_callback( + self, callback: Callable[[Contacts | None, SolverBase.Observables | None, State, float], None] + ) -> None: + """Register a graph-safe hook after each solve, before state swapping and force clearing.""" + if callback not in self._post_solver_substep_callbacks: + self._post_solver_substep_callbacks.append(callback) + + def unregister_post_solver_substep_callback( + self, callback: Callable[[Contacts | None, SolverBase.Observables | None, State, float], None] + ) -> None: + """Detach a post-solve callback and recapture an active graph without it.""" + if callback in self._post_solver_substep_callbacks: + self._post_solver_substep_callbacks.remove(callback) + if self._graph is not None: + self._invalidate_graph() + + def unregister_solver_init_callback(self, callback: Callable[[], None]) -> None: + """Remove a pending one-shot solver initialization hook, if still registered.""" + with contextlib.suppress(ValueError): + self._solver_init_callbacks.remove(callback) + def register_post_step_callback(self, callback: Callable[[], None]) -> None: """Append a hook to the list invoked after the last solver substep on every step.