From d9486bdc839a6e43a511b89bc1255eea3e9cbdb1 Mon Sep 17 00:00:00 2001 From: Maximilian Krause Date: Thu, 6 Aug 2026 16:48:49 -0700 Subject: [PATCH 01/23] Add custom mesh spawner --- .../maximiliank-custom-mesh.minor.rst | 5 ++ source/isaaclab/isaaclab/sim/__init__.pyi | 4 + .../isaaclab/sim/spawners/__init__.pyi | 4 + .../isaaclab/sim/spawners/meshes/__init__.pyi | 4 + .../isaaclab/sim/spawners/meshes/meshes.py | 86 ++++++++++++++++--- .../sim/spawners/meshes/meshes_cfg.py | 22 +++++ source/isaaclab/test/sim/test_spawn_meshes.py | 19 ++++ 7 files changed, 134 insertions(+), 10 deletions(-) create mode 100644 source/isaaclab/changelog.d/maximiliank-custom-mesh.minor.rst diff --git a/source/isaaclab/changelog.d/maximiliank-custom-mesh.minor.rst b/source/isaaclab/changelog.d/maximiliank-custom-mesh.minor.rst new file mode 100644 index 000000000000..e81d993575ef --- /dev/null +++ b/source/isaaclab/changelog.d/maximiliank-custom-mesh.minor.rst @@ -0,0 +1,5 @@ +Added +^^^^^ + +* Added :class:`~isaaclab.sim.MeshCustomCfg` for spawning meshes from authored + vertices and triangular faces. diff --git a/source/isaaclab/isaaclab/sim/__init__.pyi b/source/isaaclab/isaaclab/sim/__init__.pyi index 1ac7aed7907f..4f0587f396b6 100644 --- a/source/isaaclab/isaaclab/sim/__init__.pyi +++ b/source/isaaclab/isaaclab/sim/__init__.pyi @@ -125,6 +125,7 @@ __all__ = [ "VisualMaterialCfg", "spawn_mesh_capsule", "spawn_mesh_cone", + "spawn_mesh_custom", "spawn_mesh_cuboid", "spawn_mesh_cylinder", "spawn_mesh_rectangle", @@ -132,6 +133,7 @@ __all__ = [ "MeshCapsuleCfg", "MeshCfg", "MeshConeCfg", + "MeshCustomCfg", "MeshCuboidCfg", "MeshCylinderCfg", "MeshRectangleCfg", @@ -340,6 +342,7 @@ from .spawners import ( MeshCapsuleCfg, MeshCfg, MeshConeCfg, + MeshCustomCfg, MeshCuboidCfg, MeshCylinderCfg, MeshRectangleCfg, @@ -382,6 +385,7 @@ from .spawners import ( spawn_light, spawn_mesh_capsule, spawn_mesh_cone, + spawn_mesh_custom, spawn_mesh_cuboid, spawn_mesh_cylinder, spawn_mesh_rectangle, diff --git a/source/isaaclab/isaaclab/sim/spawners/__init__.pyi b/source/isaaclab/isaaclab/sim/spawners/__init__.pyi index 5e0e353e8198..28d35b253570 100644 --- a/source/isaaclab/isaaclab/sim/spawners/__init__.pyi +++ b/source/isaaclab/isaaclab/sim/spawners/__init__.pyi @@ -41,6 +41,7 @@ __all__ = [ "VisualMaterialCfg", "spawn_mesh_capsule", "spawn_mesh_cone", + "spawn_mesh_custom", "spawn_mesh_cuboid", "spawn_mesh_cylinder", "spawn_mesh_rectangle", @@ -48,6 +49,7 @@ __all__ = [ "MeshCapsuleCfg", "MeshCfg", "MeshConeCfg", + "MeshCustomCfg", "MeshCuboidCfg", "MeshCylinderCfg", "MeshRectangleCfg", @@ -121,6 +123,7 @@ from .materials import ( from .meshes import ( spawn_mesh_capsule, spawn_mesh_cone, + spawn_mesh_custom, spawn_mesh_cuboid, spawn_mesh_cylinder, spawn_mesh_rectangle, @@ -128,6 +131,7 @@ from .meshes import ( MeshCapsuleCfg, MeshCfg, MeshConeCfg, + MeshCustomCfg, MeshCuboidCfg, MeshCylinderCfg, MeshRectangleCfg, diff --git a/source/isaaclab/isaaclab/sim/spawners/meshes/__init__.pyi b/source/isaaclab/isaaclab/sim/spawners/meshes/__init__.pyi index c853dca4d1d0..fd1e4ac5fdf5 100644 --- a/source/isaaclab/isaaclab/sim/spawners/meshes/__init__.pyi +++ b/source/isaaclab/isaaclab/sim/spawners/meshes/__init__.pyi @@ -6,6 +6,7 @@ __all__ = [ "spawn_mesh_capsule", "spawn_mesh_cone", + "spawn_mesh_custom", "spawn_mesh_cuboid", "spawn_mesh_cylinder", "spawn_mesh_rectangle", @@ -13,6 +14,7 @@ __all__ = [ "MeshCapsuleCfg", "MeshCfg", "MeshConeCfg", + "MeshCustomCfg", "MeshCuboidCfg", "MeshCylinderCfg", "MeshRectangleCfg", @@ -22,6 +24,7 @@ __all__ = [ from .meshes import ( spawn_mesh_capsule, spawn_mesh_cone, + spawn_mesh_custom, spawn_mesh_cuboid, spawn_mesh_cylinder, spawn_mesh_rectangle, @@ -31,6 +34,7 @@ from .meshes_cfg import ( MeshCapsuleCfg, MeshCfg, MeshConeCfg, + MeshCustomCfg, MeshCuboidCfg, MeshCylinderCfg, MeshRectangleCfg, diff --git a/source/isaaclab/isaaclab/sim/spawners/meshes/meshes.py b/source/isaaclab/isaaclab/sim/spawners/meshes/meshes.py index 38876a1378eb..4bd805463b9f 100644 --- a/source/isaaclab/isaaclab/sim/spawners/meshes/meshes.py +++ b/source/isaaclab/isaaclab/sim/spawners/meshes/meshes.py @@ -12,7 +12,7 @@ import trimesh import trimesh.transformations -from pxr import Usd, UsdPhysics +from pxr import Usd, UsdGeom, UsdPhysics from isaaclab.sim import schemas from isaaclab.sim.utils import bind_physics_material, bind_visual_material, clone, create_prim, get_current_stage @@ -33,6 +33,63 @@ logger = logging.getLogger(__name__) +def _srgb_to_linear_channel(value: float) -> float: + """Convert an sRGB channel to the linear value expected by USD display color.""" + if value <= 0.04045: + return value / 12.92 + return ((value + 0.055) / 1.055) ** 2.4 + + +@clone +def spawn_mesh_custom( + prim_path: str, + cfg: meshes_cfg.MeshCustomCfg, + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, + **kwargs, +) -> Usd.Prim: + """Create a USD mesh from explicitly authored vertices and triangular faces. + + This spawner is intended for custom static geometry such as terrain patches, + tracks, and fixtures that cannot be represented by the generated mesh + primitives. Face winding is preserved, so counter-clockwise faces define the + colliding side when exact triangle-mesh collision is selected. + + Args: + prim_path: Prim path or pattern at which to spawn the asset. + cfg: Custom mesh configuration. + translation: Translation relative to the parent prim [m]. + orientation: Quaternion orientation in ``(x, y, z, w)`` order. + **kwargs: Additional cloning keyword arguments. + + Returns: + The created root prim. + + Raises: + ValueError: If the vertex or face arrays are malformed, a face index is + out of range, or the collision approximation is unknown. + """ + del kwargs + vertices = np.asarray(cfg.vertices, dtype=np.float32) + faces = np.asarray(cfg.faces, dtype=np.int64) + if vertices.ndim != 2 or vertices.shape[1:] != (3,) or len(vertices) < 3: + raise ValueError(f"Custom mesh vertices must have shape (N, 3) with N >= 3, got {vertices.shape}.") + if faces.ndim != 2 or faces.shape[1:] != (3,) or len(faces) < 1: + raise ValueError(f"Custom mesh faces must have shape (M, 3) with M >= 1, got {faces.shape}.") + if np.any(faces < 0) or np.any(faces >= len(vertices)): + raise ValueError(f"Custom mesh face indices must be in [0, {len(vertices) - 1}].") + if cfg.collision_approximation not in schemas.MESH_APPROXIMATION_TOKENS: + raise ValueError( + f"Unknown mesh collision approximation {cfg.collision_approximation!r}. " + f"Valid options are: {list(schemas.MESH_APPROXIMATION_TOKENS)}" + ) + + mesh = trimesh.Trimesh(vertices=vertices, faces=faces, process=False) + stage = get_current_stage() + _spawn_mesh_geom_from_mesh(prim_path, cfg, mesh, translation, orientation, stage=stage) + return stage.GetPrimAtPath(prim_path) + + @clone def spawn_mesh_sphere( prim_path: str, @@ -441,7 +498,7 @@ def _spawn_mesh_geom_from_mesh( "points": mesh.vertices, "faceVertexIndices": mesh.faces.flatten(), "faceVertexCounts": np.asarray([3] * len(mesh.faces)), - "subdivisionScheme": "bilinear", + "subdivisionScheme": getattr(cfg, "subdivision_scheme", "bilinear"), }, stage=stage, ) @@ -465,13 +522,15 @@ def _spawn_mesh_geom_from_mesh( ) elif cfg.collision_props is not None: # decide on type of collision approximation based on the mesh - if cfg.__class__.__name__ == "MeshSphereCfg": - collision_approximation = "boundingSphere" - elif cfg.__class__.__name__ == "MeshCuboidCfg": - collision_approximation = "boundingCube" - else: - # for: MeshCylinderCfg, MeshCapsuleCfg, MeshConeCfg - collision_approximation = "convexHull" + collision_approximation = getattr(cfg, "collision_approximation", None) + if collision_approximation is None: + if cfg.__class__.__name__ == "MeshSphereCfg": + collision_approximation = "boundingSphere" + elif cfg.__class__.__name__ == "MeshCuboidCfg": + collision_approximation = "boundingCube" + else: + # for: MeshCylinderCfg, MeshCapsuleCfg, MeshConeCfg + collision_approximation = "convexHull" # apply collision approximation to mesh # note: for primitives, we use the convex hull approximation -- this should be sufficient for most cases. mesh_collision_api = UsdPhysics.MeshCollisionAPI.Apply(mesh_prim) @@ -486,8 +545,15 @@ def _spawn_mesh_geom_from_mesh( # apply visual material if cfg.visual_material is not None: + # Keep PreviewSurface colors available to lightweight renderers that + # consume USD displayColor but cannot construct Kit materials. + diffuse_color = getattr(cfg.visual_material, "diffuse_color", None) + if diffuse_color is not None: + display_color = tuple(_srgb_to_linear_channel(value) for value in diffuse_color) + UsdGeom.Mesh(mesh_prim).CreateDisplayColorAttr([display_color]) if not has_kit(): - logger.warning("Skipping visual material application for '%s' in kitless mode.", mesh_prim_path) + if diffuse_color is None: + logger.warning("Skipping visual material application for '%s' in kitless mode.", mesh_prim_path) else: if not cfg.visual_material_path.startswith("/"): material_path = f"{geom_prim_path}/{cfg.visual_material_path}" diff --git a/source/isaaclab/isaaclab/sim/spawners/meshes/meshes_cfg.py b/source/isaaclab/isaaclab/sim/spawners/meshes/meshes_cfg.py index ba3152dc31f8..1d2584439911 100644 --- a/source/isaaclab/isaaclab/sim/spawners/meshes/meshes_cfg.py +++ b/source/isaaclab/isaaclab/sim/spawners/meshes/meshes_cfg.py @@ -76,6 +76,28 @@ class MeshCfg(RigidObjectSpawnerCfg, DeformableObjectSpawnerCfg): """ +@configclass +class MeshCustomCfg(MeshCfg): + """Configuration parameters for a mesh authored from vertices and triangular faces. + + See :meth:`spawn_mesh_custom` for more information. + """ + + func: Callable | str = "{DIR}.meshes:spawn_mesh_custom" + + vertices: tuple[tuple[float, float, float], ...] = MISSING + """Vertex positions [m].""" + + faces: tuple[tuple[int, int, int], ...] = MISSING + """Triangle vertex indices with counter-clockwise front-face winding.""" + + collision_approximation: str = "none" + """Mesh collision approximation name. Defaults to ``"none"`` for exact triangle-mesh collision.""" + + subdivision_scheme: Literal["none", "catmullClark", "loop", "bilinear"] = "none" + """USD subdivision scheme. Defaults to ``"none"`` to preserve the authored surface.""" + + @configclass class MeshSphereCfg(MeshCfg): """Configuration parameters for a sphere mesh prim with deformable properties. diff --git a/source/isaaclab/test/sim/test_spawn_meshes.py b/source/isaaclab/test/sim/test_spawn_meshes.py index 8dd2f5f35f6c..49ba0232cb68 100644 --- a/source/isaaclab/test/sim/test_spawn_meshes.py +++ b/source/isaaclab/test/sim/test_spawn_meshes.py @@ -156,6 +156,25 @@ def test_spawn_rectangle(sim, resolution, size): assert prim.GetPrimTypeInfo().GetTypeName() == "Mesh" +def test_spawn_custom_mesh(sim): + """Test spawning an exact collision mesh from authored triangle data.""" + cfg = sim_utils.MeshCustomCfg( + vertices=((-0.5, -0.5, 0.0), (0.5, -0.5, 0.0), (0.5, 0.5, 0.0), (-0.5, 0.5, 0.0)), + faces=((0, 1, 2), (0, 2, 3)), + collision_props=sim_utils.CollisionBaseCfg(), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.2, 0.4, 0.8)), + ) + prim = cfg.func("/World/CustomMesh", cfg) + + assert prim.IsValid() + mesh_prim = sim.stage.GetPrimAtPath("/World/CustomMesh/geometry/mesh") + assert mesh_prim.GetPrimTypeInfo().GetTypeName() == "Mesh" + assert mesh_prim.GetAttribute("faceVertexIndices").Get() == [0, 1, 2, 0, 2, 3] + assert mesh_prim.GetAttribute("subdivisionScheme").Get() == "none" + assert mesh_prim.GetAttribute("physics:approximation").Get() == "none" + assert mesh_prim.GetAttribute("primvars:displayColor").HasAuthoredValue() + + """ Physics properties. """ From d152a2f460e156ecdbdb944c1578c5549af6014f Mon Sep 17 00:00:00 2001 From: Maximilian Krause Date: Thu, 6 Aug 2026 16:48:53 -0700 Subject: [PATCH 02/23] Add Newton conveyor Franka task --- .../maximiliank-conveyor-franka.minor.rst | 5 + .../contrib/conveyor_franka/__init__.py | 17 + .../conveyor_franka/conveyor_force_driver.py | 321 +++++++++++++++ .../conveyor_franka/conveyor_franka_env.py | 58 +++ .../conveyor_franka_env_cfg.py | 367 ++++++++++++++++++ .../conveyor_franka/conveyor_geometry.py | 203 ++++++++++ .../contrib/test_conveyor_franka_geometry.py | 77 ++++ 7 files changed, 1048 insertions(+) create mode 100644 source/isaaclab_tasks/changelog.d/maximiliank-conveyor-franka.minor.rst create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/__init__.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_force_driver.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env_cfg.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_geometry.py create mode 100644 source/isaaclab_tasks/test/contrib/test_conveyor_franka_geometry.py diff --git a/source/isaaclab_tasks/changelog.d/maximiliank-conveyor-franka.minor.rst b/source/isaaclab_tasks/changelog.d/maximiliank-conveyor-franka.minor.rst new file mode 100644 index 000000000000..1fae7f1d38d0 --- /dev/null +++ b/source/isaaclab_tasks/changelog.d/maximiliank-conveyor-franka.minor.rst @@ -0,0 +1,5 @@ +Added +^^^^^ + +* Added a contributed manager-based environment with guarded, counter-rotating force-driven racetrack + conveyors and a MuJoCo Menagerie Franka. diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/__init__.py new file mode 100644 index 000000000000..bb4577de56f2 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/__init__.py @@ -0,0 +1,17 @@ +# Copyright (c) 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 + +"""Force-driven conveyor scene with a Franka robot.""" + +import gymnasium as gym + +gym.register( + id="IsaacContrib-Conveyor-Franka-Newton-v0", + entry_point=f"{__name__}.conveyor_franka_env:ConveyorFrankaEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.conveyor_franka_env_cfg:ConveyorFrankaEnvCfg", + }, +) diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_force_driver.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_force_driver.py new file mode 100644 index 000000000000..69928731add5 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_force_driver.py @@ -0,0 +1,321 @@ +# Copyright (c) 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 + +"""Task-local force model for static conveyor surfaces under Newton physics. + +The driver reads solver-reported normal contact forces, computes a Coulomb-limited force that +drives each parcel's contact point toward the belt velocity, and applies that wrench on the +following physics step. +""" + +from __future__ import annotations + +import re + +import warp as wp + +from isaaclab_newton.physics import NewtonManager + +from .conveyor_geometry import BELT_CENTER_X, BELT_CENTER_Y, BELT_HALF_STRAIGHT, belt_direction + +_BELT_LABEL = re.compile(r"Conveyor(Left|Right)Belt") + + +@wp.struct +class BeltContact: + """Reduced contact data consumed by the conveyor force kernel.""" + + valid: wp.int32 + body: wp.int32 + point: wp.vec3 + normal: wp.vec3 + normal_force: wp.float32 + target_velocity: wp.vec3 + + +@wp.kernel +def _extract_linear_force(spatial_force: wp.array[wp.spatial_vector], force: wp.array[wp.vec3]): + contact_id = wp.tid() + force[contact_id] = wp.spatial_top(spatial_force[contact_id]) + + +@wp.func +def _racetrack_velocity( + point: wp.vec3, + center: wp.vec3, + half_straight: wp.float32, + direction: wp.float32, + speed: wp.float32, +) -> wp.vec3: + relative = point - center + tangent = wp.vec3() + if relative[0] > half_straight: + radial = wp.vec3(relative[0] - half_straight, relative[1], 0.0) + radial_length = wp.length(radial) + if radial_length > 0.0: + tangent = wp.vec3(radial[1], -radial[0], 0.0) / radial_length + elif relative[0] < -half_straight: + radial = wp.vec3(relative[0] + half_straight, relative[1], 0.0) + radial_length = wp.length(radial) + if radial_length > 0.0: + tangent = wp.vec3(radial[1], -radial[0], 0.0) / radial_length + elif relative[1] >= 0.0: + tangent = wp.vec3(1.0, 0.0, 0.0) + else: + tangent = wp.vec3(-1.0, 0.0, 0.0) + return tangent * direction * speed + + +@wp.kernel +def _classify_contacts( + contact_count: wp.array[wp.int32], + shape0: wp.array[wp.int32], + shape1: wp.array[wp.int32], + normal: wp.array[wp.vec3], + point0: wp.array[wp.vec3], + point1: wp.array[wp.vec3], + contact_force: wp.array[wp.vec3], + shape_body: wp.array[wp.int32], + shape_is_belt: wp.array[wp.int32], + shape_belt_center: wp.array[wp.vec3], + shape_belt_direction: wp.array[wp.float32], + shape_transform: wp.array[wp.transform], + body_q: wp.array[wp.transform], + half_straight: wp.float32, + speed: wp.float32, + normal_threshold: wp.float32, + contacts_out: wp.array[BeltContact], + body_contact_count: wp.array[wp.int32], +): + contact_id = wp.tid() + result = BeltContact() + result.valid = 0 + + if contact_id < contact_count[0]: + contact_shape0 = shape0[contact_id] + contact_shape1 = shape1[contact_id] + if contact_shape0 >= 0 and contact_shape1 >= 0: + belt0 = shape_is_belt[contact_shape0] + belt1 = shape_is_belt[contact_shape1] + contact_normal = normal[contact_id] + + body = wp.int32(-1) + belt_shape = wp.int32(-1) + local_point = wp.vec3() + normal_toward_body = wp.vec3() + if belt0 == 1 and belt1 == 0: + belt_shape = contact_shape0 + body = shape_body[contact_shape1] + local_point = point1[contact_id] + normal_toward_body = contact_normal + elif belt1 == 1 and belt0 == 0: + belt_shape = contact_shape1 + body = shape_body[contact_shape0] + local_point = point0[contact_id] + normal_toward_body = -contact_normal + + alignment = wp.dot(normal_toward_body, wp.vec3(0.0, 0.0, 1.0)) + normal_force = wp.abs(wp.dot(contact_force[contact_id], contact_normal)) + if body >= 0 and belt_shape >= 0 and alignment >= normal_threshold and normal_force > 0.0: + result.valid = 1 + result.body = body + result.point = wp.transform_point(body_q[body], local_point) + result.normal = normal_toward_body + result.normal_force = normal_force + belt_center = wp.transform_point(shape_transform[belt_shape], shape_belt_center[belt_shape]) + result.target_velocity = _racetrack_velocity( + result.point, + belt_center, + half_straight, + shape_belt_direction[belt_shape], + speed, + ) + wp.atomic_add(body_contact_count, body, 1) + + contacts_out[contact_id] = result + + +@wp.kernel +def _accumulate_forces( + dt: wp.float32, + friction: wp.float32, + contacts: wp.array[BeltContact], + body_q: wp.array[wp.transform], + body_qd: wp.array[wp.spatial_vector], + body_com: wp.array[wp.vec3], + body_inv_mass: wp.array[wp.float32], + body_contact_count: wp.array[wp.int32], + body_force: wp.array[wp.spatial_vector], +): + contact_id = wp.tid() + contact = contacts[contact_id] + if contact.valid == 0: + return + + count = body_contact_count[contact.body] + inverse_mass = body_inv_mass[contact.body] + if count <= 0 or inverse_mass <= 0.0: + return + + pose = body_q[contact.body] + center_of_mass = wp.transform_point(pose, body_com[contact.body]) + center_to_contact = contact.point - center_of_mass + velocity = body_qd[contact.body] + point_velocity = wp.spatial_top(velocity) + wp.cross(wp.spatial_bottom(velocity), center_to_contact) + + velocity_error = contact.target_velocity - point_velocity + velocity_error = velocity_error - contact.normal * wp.dot(velocity_error, contact.normal) + desired_force = velocity_error / (inverse_mass * dt * float(count)) + + desired_magnitude = wp.length(desired_force) + max_magnitude = friction * contact.normal_force + if desired_magnitude > max_magnitude and desired_magnitude > 0.0: + desired_force = desired_force * (max_magnitude / desired_magnitude) + + torque = wp.cross(center_to_contact, desired_force) + wp.atomic_add(body_force, contact.body, wp.spatial_vector(desired_force, torque)) + + +@wp.kernel +def _add_body_force(dst: wp.array[wp.spatial_vector], src: wp.array[wp.spatial_vector]): + body_id = wp.tid() + dst[body_id] = dst[body_id] + src[body_id] + + +class ConveyorForceDriver: + """Convert Newton contact forces into moving-surface forces for the racetrack belts.""" + + def __init__( + self, + num_envs: int, + speed: float = 0.35, + friction: float = 0.5, + normal_threshold: float = 0.95, + ) -> None: + """Initialize the driver after Newton simulation startup. + + Args: + num_envs: Number of replicated simulation environments. + speed: Conveyor surface speed [m/s]. + friction: Coulomb friction coefficient used to limit traction. + normal_threshold: Minimum upward contact-normal alignment. + """ + model = NewtonManager.get_model() + contacts = NewtonManager.get_contacts() + if model is None or contacts is None: + raise RuntimeError("The conveyor driver must be created after Newton simulation initialization.") + if contacts.force is None: + raise RuntimeError( + "Newton did not allocate per-contact force reporting. The scene contact sensor must initialize " + "before the conveyor driver." + ) + + self._model = model + self._contacts = contacts + self._device = model.device + self._dt = NewtonManager.get_solver_dt() + self._speed = speed + self._friction = friction + self._normal_threshold = normal_threshold + + shape_is_belt = [0] * model.shape_count + shape_belt_center = [wp.vec3()] * model.shape_count + shape_belt_direction = [0.0] * model.shape_count + shape_body = model.shape_body.numpy() + matched_shapes = 0 + for shape_id, label in enumerate(model.shape_label): + match = _BELT_LABEL.search(label) + if match is None: + continue + if int(shape_body[shape_id]) >= 0: + raise ValueError(f"Conveyor shape must be static: {label}") + + side = match.group(1) + shape_is_belt[shape_id] = 1 + center_y = BELT_CENTER_Y if side == "Left" else -BELT_CENTER_Y + shape_belt_center[shape_id] = wp.vec3(BELT_CENTER_X, center_y, 0.0) + shape_belt_direction[shape_id] = belt_direction(side) + matched_shapes += 1 + + expected_shapes = 2 * num_envs + if matched_shapes != expected_shapes: + raise RuntimeError(f"Expected {expected_shapes} conveyor shapes, but matched {matched_shapes}.") + + self._shape_is_belt = wp.array(shape_is_belt, dtype=wp.int32, device=self._device) + self._shape_belt_center = wp.array(shape_belt_center, dtype=wp.vec3, device=self._device) + self._shape_belt_direction = wp.array(shape_belt_direction, dtype=wp.float32, device=self._device) + self._contact_force = wp.zeros(contacts.rigid_contact_max, dtype=wp.vec3, device=self._device) + self._belt_contacts = wp.empty(contacts.rigid_contact_max, dtype=BeltContact, device=self._device) + self._body_contact_count = wp.zeros(model.body_count, dtype=wp.int32, device=self._device) + self._body_force = wp.zeros(model.body_count, dtype=wp.spatial_vector, device=self._device) + + NewtonManager.register_post_actuator_callback(self.apply) + + def clear(self) -> None: + """Discard forces computed before an environment reset.""" + self._body_force.zero_() + self._body_contact_count.zero_() + + def apply(self) -> None: + """Apply the wrench computed from the preceding physics step.""" + state = NewtonManager.get_state_0() + wp.launch( + _add_body_force, + dim=self._model.body_count, + inputs=[state.body_f, self._body_force], + device=self._device, + ) + + def update(self) -> None: + """Read current contact forces and compute the next conveyor wrench.""" + state = NewtonManager.get_state_0() + self._body_force.zero_() + self._body_contact_count.zero_() + wp.launch( + _extract_linear_force, + dim=self._contacts.rigid_contact_max, + inputs=[self._contacts.force, self._contact_force], + device=self._device, + ) + wp.launch( + _classify_contacts, + dim=self._contacts.rigid_contact_max, + inputs=[ + self._contacts.rigid_contact_count, + self._contacts.rigid_contact_shape0, + self._contacts.rigid_contact_shape1, + self._contacts.rigid_contact_normal, + self._contacts.rigid_contact_point0, + self._contacts.rigid_contact_point1, + self._contact_force, + self._model.shape_body, + self._shape_is_belt, + self._shape_belt_center, + self._shape_belt_direction, + self._model.shape_transform, + state.body_q, + BELT_HALF_STRAIGHT, + self._speed, + self._normal_threshold, + ], + outputs=[self._belt_contacts, self._body_contact_count], + device=self._device, + ) + wp.launch( + _accumulate_forces, + dim=self._contacts.rigid_contact_max, + inputs=[ + self._dt, + self._friction, + self._belt_contacts, + state.body_q, + state.body_qd, + self._model.body_com, + self._model.body_inv_mass, + self._body_contact_count, + ], + outputs=[self._body_force], + device=self._device, + ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env.py new file mode 100644 index 000000000000..82ffda560922 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env.py @@ -0,0 +1,58 @@ +# Copyright (c) 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 + +"""Manager-based environment that installs the task-local conveyor force driver.""" + +from __future__ import annotations + +from collections.abc import Sequence + +import torch + +from isaaclab.envs import ManagerBasedRLEnv +from isaaclab.envs.common import VecEnvStepReturn + +from .conveyor_force_driver import ConveyorForceDriver +from .conveyor_franka_env_cfg import ConveyorFrankaEnvCfg + + +class ConveyorFrankaEnv(ManagerBasedRLEnv): + """Manager-based environment with force-driven Newton conveyor surfaces.""" + + cfg: ConveyorFrankaEnvCfg + + def __init__(self, cfg: ConveyorFrankaEnvCfg, **kwargs): + # Gym forwards registry metadata alongside the resolved configuration. + del kwargs + super().__init__(cfg) + self._conveyor_driver = ConveyorForceDriver( + num_envs=self.num_envs, + speed=cfg.conveyor_force.speed, + friction=cfg.conveyor_force.friction, + normal_threshold=cfg.conveyor_force.normal_threshold, + ) + + def step(self, action: torch.Tensor) -> VecEnvStepReturn: + """Step the manager-based environment and prepare traction for the next step.""" + result = super().step(action) + # The contact sensor has now asked Newton to publish per-contact forces. + self._conveyor_driver.update() + return result + + def _reset_idx(self, env_ids: Sequence[int]): + """Reset selected environments and restore the zero-action arm target.""" + super()._reset_idx(env_ids) + + conveyor_driver = getattr(self, "_conveyor_driver", None) + if conveyor_driver is not None: + conveyor_driver.clear() + + # There is deliberately no arm action term yet. Keep the position-controlled + # Franka at its configured pose during zero-action scene playback. + robot = self.scene["robot"] + joint_targets = robot.data.default_joint_pos + if env_ids is not None: + joint_targets = joint_targets[env_ids] + robot.set_joint_position_target(joint_targets, env_ids=env_ids) diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env_cfg.py new file mode 100644 index 000000000000..bd4261ae2af0 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env_cfg.py @@ -0,0 +1,367 @@ +# Copyright (c) 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 + +"""Configuration for the force-driven conveyor and Franka demonstration scene.""" + +from __future__ import annotations + +from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg, NewtonCollisionPipelineCfg, NewtonShapeCfg + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg +from isaaclab.envs import ManagerBasedRLEnvCfg +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sensors import ContactSensorCfg +from isaaclab.sim import SimulationCfg +from isaaclab.sim.spawners.materials import RigidBodyMaterialBaseCfg +from isaaclab.utils.configclass import configclass + +from isaaclab_assets.robots.franka import FRANKA_PANDA_MENAGERIE_CFG + +from .conveyor_geometry import ( + BELT_CENTER_X, + BELT_CENTER_Y, + BELT_COLOR, + BELT_TURN_RADIUS, + GUARD_COLOR, + PARCEL_COLOR, + MeshSpec, + belt_mesh_spec, + guard_mesh_specs, +) + +_DYNAMIC_PROPERTIES = sim_utils.RigidBodyBaseCfg() + + +def _srgb_to_linear_channel(value: float) -> float: + """Convert an sRGB channel to the linear value expected by USD displayColor.""" + if value <= 0.04045: + return value / 12.92 + return ((value + 0.055) / 1.055) ** 2.4 + + +@configclass +class ActionsCfg: + """Empty action configuration for zero-action scene playback.""" + + pass + + +@configclass +class ObservationsCfg: + """Empty observation configuration while the task objective is being designed.""" + + pass + + +@configclass +class ConveyorForceCfg: + """Configuration for force-based conveyor traction.""" + + speed: float = 0.35 + """Tangential conveyor surface speed [m/s].""" + + friction: float = 0.5 + """Coulomb friction coefficient used to limit traction.""" + + normal_threshold: float = 0.95 + """Minimum upward contact-normal alignment in the range [0, 1].""" + + def __post_init__(self) -> None: + """Validate conveyor force parameters.""" + if self.speed < 0.0: + raise ValueError(f"Conveyor speed must be non-negative, got {self.speed}.") + if self.friction < 0.0: + raise ValueError(f"Conveyor friction must be non-negative, got {self.friction}.") + if not 0.0 <= self.normal_threshold <= 1.0: + raise ValueError(f"Conveyor normal threshold must be in [0, 1], got {self.normal_threshold}.") + + +@sim_utils.clone +def _spawn_shape_with_display_color( + prim_path: str, + cfg: sim_utils.ShapeCfg, + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, + **kwargs, +): + """Spawn a primitive and author a renderer-independent USD display color.""" + if isinstance(cfg, sim_utils.CuboidCfg): + prim = sim_utils.spawn_cuboid(prim_path, cfg, translation, orientation, **kwargs) + elif isinstance(cfg, sim_utils.CylinderCfg): + prim = sim_utils.spawn_cylinder(prim_path, cfg, translation, orientation, **kwargs) + elif isinstance(cfg, sim_utils.CapsuleCfg): + prim = sim_utils.spawn_capsule(prim_path, cfg, translation, orientation, **kwargs) + elif isinstance(cfg, sim_utils.SphereCfg): + prim = sim_utils.spawn_sphere(prim_path, cfg, translation, orientation, **kwargs) + else: + raise TypeError(f"Unsupported colored primitive configuration: {type(cfg).__name__}") + + if cfg.visual_material is not None: + from pxr import Usd, UsdGeom + + display_color = tuple(_srgb_to_linear_channel(value) for value in cfg.visual_material.diffuse_color) + for child in Usd.PrimRange(prim): + if child.IsA(UsdGeom.Gprim): + UsdGeom.Gprim(child).CreateDisplayColorAttr([display_color]) + return prim + + +def _static_cuboid( + prim_path: str, + size: tuple[float, float, float], + pos: tuple[float, float, float], + color: tuple[float, float, float], + rot: tuple[float, float, float, float] = (0.0, 0.0, 0.0, 1.0), + friction: float = 0.7, + roughness: float = 0.75, + metallic: float = 0.0, +) -> AssetBaseCfg: + """Build a static colliding cuboid configuration.""" + spawn = sim_utils.CuboidCfg( + size=size, + collision_props=sim_utils.CollisionBaseCfg(), + physics_material=RigidBodyMaterialBaseCfg( + static_friction=friction, + dynamic_friction=friction, + restitution=0.0, + ), + visual_material=sim_utils.PreviewSurfaceCfg( + diffuse_color=color, + roughness=roughness, + metallic=metallic, + ), + ) + spawn.func = _spawn_shape_with_display_color + return AssetBaseCfg( + prim_path=prim_path, + init_state=AssetBaseCfg.InitialStateCfg(pos=pos, rot=rot), + spawn=spawn, + ) + + +def _static_mesh( + prim_path: str, + spec: MeshSpec, + color: tuple[float, float, float], + friction: float, + roughness: float, + metallic: float, +) -> AssetBaseCfg: + """Build a static colliding triangle-mesh configuration.""" + return AssetBaseCfg( + prim_path=prim_path, + spawn=sim_utils.MeshCustomCfg( + vertices=spec.vertices, + faces=spec.faces, + collision_props=sim_utils.CollisionBaseCfg(), + physics_material=RigidBodyMaterialBaseCfg( + static_friction=friction, + dynamic_friction=friction, + restitution=0.0, + ), + visual_material=sim_utils.PreviewSurfaceCfg( + diffuse_color=color, + roughness=roughness, + metallic=metallic, + ), + ), + ) + + +def _parcel( + name: str, + spawn: sim_utils.ShapeCfg, + pos: tuple[float, float, float], +) -> RigidObjectCfg: + """Build a dynamic parcel configuration.""" + spawn.rigid_props = _DYNAMIC_PROPERTIES + spawn.mass_props = sim_utils.MassPropertiesCfg(mass=0.25) + spawn.collision_props = sim_utils.CollisionBaseCfg() + spawn.physics_material = RigidBodyMaterialBaseCfg( + # The force driver supplies traction explicitly. This is just above MuJoCo's + # minimum valid coefficient and mirrors Newton's force-conveyor example. + static_friction=1.1e-5, + dynamic_friction=1.1e-5, + restitution=0.05, + ) + spawn.func = _spawn_shape_with_display_color + return RigidObjectCfg( + prim_path=f"{{ENV_REGEX_NS}}/{name}", + init_state=RigidObjectCfg.InitialStateCfg(pos=pos), + spawn=spawn, + ) + + +@configclass +class ConveyorFrankaSceneCfg(InteractiveSceneCfg): + """Scene with two counter-rotating racetrack conveyors around a table-mounted Franka.""" + + # Use the MuJoCo Menagerie-derived model with Newton's MuJoCo MJWarp solver. + robot = FRANKA_PANDA_MENAGERIE_CFG.replace( + prim_path="{ENV_REGEX_NS}/Robot", + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + "panda_joint1": 0.0, + "panda_joint2": -0.35, + "panda_joint3": 0.0, + "panda_joint4": -2.35, + "panda_joint5": 0.0, + "panda_joint6": 2.0, + "panda_joint7": 0.78, + "panda_finger_joint.*": 0.04, + } + ), + ) + + tabletop = _static_cuboid( + prim_path="{ENV_REGEX_NS}/Tabletop", + size=(2.0, 1.9, 0.08), + pos=(0.50, 0.0, -0.04), + color=(0.32, 0.34, 0.37), + ) + table_pedestal = _static_cuboid( + prim_path="{ENV_REGEX_NS}/TablePedestal", + size=(0.75, 0.55, 0.76), + pos=(0.25, 0.0, -0.46), + color=(0.18, 0.20, 0.23), + ) + + parcel_left_box = _parcel( + "ParcelLeftBox", + sim_utils.CuboidCfg( + size=(0.075, 0.055, 0.06), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=PARCEL_COLOR, roughness=0.8), + ), + (BELT_CENTER_X - 0.12, BELT_CENTER_Y + BELT_TURN_RADIUS, 0.085), + ) + parcel_left_cylinder = _parcel( + "ParcelLeftCylinder", + sim_utils.CylinderCfg( + radius=0.032, + height=0.065, + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.18, 0.48, 0.82), roughness=0.8), + ), + (BELT_CENTER_X + 0.18, BELT_CENTER_Y - BELT_TURN_RADIUS, 0.085), + ) + parcel_right_box = _parcel( + "ParcelRightBox", + sim_utils.CuboidCfg( + size=(0.06, 0.06, 0.075), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.86, 0.34, 0.12), roughness=0.8), + ), + (BELT_CENTER_X + 0.14, -BELT_CENTER_Y + BELT_TURN_RADIUS, 0.0925), + ) + parcel_right_capsule = _parcel( + "ParcelRightCapsule", + sim_utils.CapsuleCfg( + radius=0.026, + height=0.075, + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.34, 0.68, 0.28), roughness=0.8), + ), + (BELT_CENTER_X - 0.18, -BELT_CENTER_Y - BELT_TURN_RADIUS, 0.095), + ) + + parcel_contacts = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Parcel.*", + update_period=0.0, + history_length=1, + debug_vis=False, + ) + + ground = AssetBaseCfg( + prim_path="/World/GroundPlane", + init_state=AssetBaseCfg.InitialStateCfg(pos=(0.0, 0.0, -0.85)), + spawn=sim_utils.GroundPlaneCfg(), + ) + dome_light = AssetBaseCfg( + prim_path="/World/DomeLight", + spawn=sim_utils.DomeLightCfg(color=(0.8, 0.8, 0.8), intensity=2500.0), + ) + + def __post_init__(self) -> None: + """Generate both racetrack belts and their inner/outer guardrails.""" + for side in ("Left", "Right"): + belt_spec = belt_mesh_spec(side) + setattr( + self, + f"conveyor_{side.lower()}_belt", + _static_mesh( + prim_path=f"{{ENV_REGEX_NS}}/{belt_spec.name}", + spec=belt_spec, + color=BELT_COLOR, + # MuJoCo requires a tiny positive value even though the force driver, + # rather than solver friction, supplies the belt motion. + friction=1.1e-5, + roughness=0.9, + metallic=0.0, + ), + ) + + for spec in guard_mesh_specs(side): + boundary = "inner" if spec.name.endswith("Inner") else "outer" + setattr( + self, + f"guard_{side.lower()}_{boundary}", + _static_mesh( + prim_path=f"{{ENV_REGEX_NS}}/{spec.name}", + spec=spec, + color=GUARD_COLOR, + friction=0.2, + roughness=0.3, + metallic=0.8, + ), + ) + + +@configclass +class ConveyorFrankaEnvCfg(ManagerBasedRLEnvCfg): + """Manager-based environment configuration for the conveyor Franka scene.""" + + scene: ConveyorFrankaSceneCfg = ConveyorFrankaSceneCfg(num_envs=1, env_spacing=3.0, replicate_physics=True) + conveyor_force: ConveyorForceCfg = ConveyorForceCfg() + # MDP managers will be populated once the manipulation objective is defined. + actions: ActionsCfg = ActionsCfg() + observations: ObservationsCfg = ObservationsCfg() + rewards = None + terminations = None + decimation: int = 1 + episode_length_s: float = 1.0e6 + + sim: SimulationCfg = SimulationCfg( + dt=1.0 / 120.0, + render_interval=2, + physics=NewtonCfg( + solver_cfg=MJWarpSolverCfg( + solver="newton", + integrator="implicitfast", + njmax=300, + nconmax=256, + impratio=10.0, + cone="elliptic", + update_data_interval=2, + iterations=100, + ls_iterations=15, + ls_parallel=False, + use_mujoco_contacts=False, + ccd_iterations=35, + ), + collision_cfg=NewtonCollisionPipelineCfg(), + default_shape_cfg=NewtonShapeCfg(), + num_substeps=2, + use_cuda_graph=False, + load_visual_shapes=True, + ), + ) + + def __post_init__(self) -> None: + self.seed = 42 + # Frame the complete robot and both conveyor lanes in the Newton viewer. + from isaaclab_visualizers.newton import NewtonVisualizerCfg + + self.sim.default_visualizer_cfg = NewtonVisualizerCfg( + eye=(2.3, -2.7, 1.8), + lookat=(0.45, 0.0, 0.35), + ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_geometry.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_geometry.py new file mode 100644 index 000000000000..33667f197402 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_geometry.py @@ -0,0 +1,203 @@ +# Copyright (c) 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 + +"""Shared racetrack geometry and velocity-field descriptions.""" + +from __future__ import annotations + +import math +from dataclasses import dataclass + +BELT_COLOR = (0.09, 0.09, 0.09) +"""Dark-rubber color used by Newton's conveyor example.""" + +GUARD_COLOR = (0.66, 0.69, 0.74) +"""Brushed-metal color used by Newton's conveyor example.""" + +PARCEL_COLOR = (0.72, 0.55, 0.35) +"""Cardboard color used by Newton's conveyor example.""" + +BELT_CENTER_X = 0.58 +BELT_CENTER_Y = 0.51 +BELT_TURN_RADIUS = 0.24 +BELT_HALF_STRAIGHT = 0.44 +BELT_WIDTH = 0.15 +BELT_THICKNESS = 0.04 +BELT_TOP_Z = 0.04 +TURN_SEGMENT_COUNT = 48 + +GUARD_THICKNESS = 0.018 +# Keep the rails below the parcel tops so both lanes remain easy to read from +# the default oblique camera. +GUARD_HEIGHT = 0.035 +GUARD_BASE_OVERLAP = 0.005 + + +@dataclass(frozen=True) +class MeshSpec: + """Triangle mesh and semantic name for one static racetrack component.""" + + name: str + vertices: tuple[tuple[float, float, float], ...] + faces: tuple[tuple[int, int, int], ...] + + +def belt_direction(side: str) -> float: + """Return ``1`` for clockwise motion and ``-1`` for counter-clockwise motion.""" + if side == "Left": + return 1.0 + if side == "Right": + return -1.0 + raise ValueError(f"Unknown conveyor side: {side!r}.") + + +def _racetrack_centerline(center_y: float) -> tuple[tuple[float, float, float, float], ...]: + """Sample a clockwise racetrack centerline with an outward normal at every point.""" + left_x = BELT_CENTER_X - BELT_HALF_STRAIGHT + right_x = BELT_CENTER_X + BELT_HALF_STRAIGHT + radius = BELT_TURN_RADIUS + points: list[tuple[float, float, float, float]] = [ + (left_x, center_y + radius, 0.0, 1.0), + (right_x, center_y + radius, 0.0, 1.0), + ] + + for index in range(1, TURN_SEGMENT_COUNT + 1): + angle = 0.5 * math.pi - index * math.pi / TURN_SEGMENT_COUNT + normal_x = math.cos(angle) + normal_y = math.sin(angle) + points.append((right_x + radius * normal_x, center_y + radius * normal_y, normal_x, normal_y)) + + points.append((left_x, center_y - radius, 0.0, -1.0)) + for index in range(1, TURN_SEGMENT_COUNT): + angle = -0.5 * math.pi - index * math.pi / TURN_SEGMENT_COUNT + normal_x = math.cos(angle) + normal_y = math.sin(angle) + points.append((left_x + radius * normal_x, center_y + radius * normal_y, normal_x, normal_y)) + return tuple(points) + + +def _racetrack_prism_mesh( + name: str, + center_y: float, + lateral_offset: float, + width: float, + z_min: float, + z_max: float, +) -> MeshSpec: + """Build one closed prism following a racetrack centerline.""" + centerline = _racetrack_centerline(center_y) + half_width = 0.5 * width + outer_offset = lateral_offset + half_width + inner_offset = lateral_offset - half_width + + outer_top = [(x + nx * outer_offset, y + ny * outer_offset, z_max) for x, y, nx, ny in centerline] + inner_top = [(x + nx * inner_offset, y + ny * inner_offset, z_max) for x, y, nx, ny in centerline] + outer_bottom = [(x + nx * outer_offset, y + ny * outer_offset, z_min) for x, y, nx, ny in centerline] + inner_bottom = [(x + nx * inner_offset, y + ny * inner_offset, z_min) for x, y, nx, ny in centerline] + points = tuple(inner_top + outer_top + inner_bottom + outer_bottom) + + count = len(centerline) + outer_top_offset = count + inner_bottom_offset = 2 * count + outer_bottom_offset = 3 * count + indices: list[int] = [] + for index in range(count): + next_index = (index + 1) % count + inner_top_i = index + inner_top_j = next_index + outer_top_i = outer_top_offset + index + outer_top_j = outer_top_offset + next_index + inner_bottom_i = inner_bottom_offset + index + inner_bottom_j = inner_bottom_offset + next_index + outer_bottom_i = outer_bottom_offset + index + outer_bottom_j = outer_bottom_offset + next_index + + # Top, bottom, outer wall, and inner wall; two triangles per surface. + indices.extend((inner_top_i, outer_top_i, outer_top_j, inner_top_i, outer_top_j, inner_top_j)) + indices.extend( + ( + inner_bottom_i, + inner_bottom_j, + outer_bottom_j, + inner_bottom_i, + outer_bottom_j, + outer_bottom_i, + ) + ) + indices.extend( + ( + outer_bottom_i, + outer_bottom_j, + outer_top_j, + outer_bottom_i, + outer_top_j, + outer_top_i, + ) + ) + indices.extend( + ( + inner_bottom_i, + inner_top_i, + inner_top_j, + inner_bottom_i, + inner_top_j, + inner_bottom_j, + ) + ) + + # The centerline is sampled clockwise so its tangent matches the positive + # conveyor direction. The face pattern above is written for a + # counter-clockwise ring, so reverse every triangle to keep its collision + # normal outward (most importantly, the belt top must point +Z). + for triangle_start in range(0, len(indices), 3): + indices[triangle_start + 1], indices[triangle_start + 2] = ( + indices[triangle_start + 2], + indices[triangle_start + 1], + ) + + return MeshSpec( + name=name, + vertices=points, + faces=tuple(tuple(indices[offset : offset + 3]) for offset in range(0, len(indices), 3)), + ) + + +def belt_mesh_spec(side: str) -> MeshSpec: + """Build one seamless, watertight conveyor belt mesh.""" + center_y = BELT_CENTER_Y if side == "Left" else -BELT_CENTER_Y + return _racetrack_prism_mesh( + name=f"Conveyor{side}Belt", + center_y=center_y, + lateral_offset=0.0, + width=BELT_WIDTH, + z_min=BELT_TOP_Z - BELT_THICKNESS, + z_max=BELT_TOP_Z, + ) + + +def guard_mesh_specs(side: str) -> tuple[MeshSpec, MeshSpec]: + """Build seamless inner and outer guardrail meshes for one racetrack.""" + center_y = BELT_CENTER_Y if side == "Left" else -BELT_CENTER_Y + rail_offset = 0.5 * (BELT_WIDTH + GUARD_THICKNESS) + z_min = BELT_TOP_Z - GUARD_BASE_OVERLAP + z_max = BELT_TOP_Z + GUARD_HEIGHT + return ( + _racetrack_prism_mesh( + name=f"Guard{side}Inner", + center_y=center_y, + lateral_offset=-rail_offset, + width=GUARD_THICKNESS, + z_min=z_min, + z_max=z_max, + ), + _racetrack_prism_mesh( + name=f"Guard{side}Outer", + center_y=center_y, + lateral_offset=rail_offset, + width=GUARD_THICKNESS, + z_min=z_min, + z_max=z_max, + ), + ) diff --git a/source/isaaclab_tasks/test/contrib/test_conveyor_franka_geometry.py b/source/isaaclab_tasks/test/contrib/test_conveyor_franka_geometry.py new file mode 100644 index 000000000000..892502214a1f --- /dev/null +++ b/source/isaaclab_tasks/test/contrib/test_conveyor_franka_geometry.py @@ -0,0 +1,77 @@ +# Copyright (c) 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 + +"""Tests for the contributed conveyor Franka racetrack geometry.""" + +from collections import Counter + +import pytest + +from isaaclab_tasks.contrib.conveyor_franka.conveyor_franka_env_cfg import ConveyorForceCfg +from isaaclab_tasks.contrib.conveyor_franka.conveyor_geometry import ( + BELT_TOP_Z, + TURN_SEGMENT_COUNT, + MeshSpec, + belt_direction, + belt_mesh_spec, + guard_mesh_specs, +) + + +def _edge_use_counts(spec: MeshSpec) -> Counter[tuple[int, int]]: + """Count triangle uses of every undirected mesh edge.""" + edges: Counter[tuple[int, int]] = Counter() + for triangle in spec.faces: + for start, end in zip(triangle, triangle[1:] + triangle[:1], strict=True): + edges[tuple(sorted((start, end)))] += 1 + return edges + + +def test_racetrack_mesh_counts_and_names_are_consistent(): + """Verify each lane has one belt and two uniquely named rail meshes.""" + specs = [] + for side in ("Left", "Right"): + specs.append(belt_mesh_spec(side)) + specs.extend(guard_mesh_specs(side)) + + assert len(specs) == 6 + assert len({spec.name for spec in specs}) == len(specs) + expected_loop_vertices = 2 * TURN_SEGMENT_COUNT + 2 + for spec in specs: + assert len(spec.vertices) == 4 * expected_loop_vertices + assert len(spec.faces) == 8 * expected_loop_vertices + + +def test_racetrack_meshes_are_watertight(): + """Verify belts and rails have no open or multiply connected triangle edges.""" + for side in ("Left", "Right"): + for spec in (belt_mesh_spec(side), *guard_mesh_specs(side)): + assert set(_edge_use_counts(spec).values()) == {2} + + +def test_belt_top_faces_point_upward(): + """The one-sided triangle-mesh collision surface must support parcels from above.""" + for side in ("Left", "Right"): + spec = belt_mesh_spec(side) + for face in spec.faces: + a, b, c = (spec.vertices[index] for index in face) + if a[2] == b[2] == c[2] == BELT_TOP_Z: + cross_z = (b[0] - a[0]) * (c[1] - a[1]) - (b[1] - a[1]) * (c[0] - a[0]) + assert cross_z > 0.0 + + +def test_racetrack_lanes_counter_rotate(): + """Verify the two analytic conveyor velocity fields use opposite directions.""" + assert belt_direction("Left") == -belt_direction("Right") + + +@pytest.mark.parametrize( + ("parameter", "value"), + (("speed", -0.1), ("friction", -0.1), ("normal_threshold", 1.1)), +) +def test_conveyor_force_config_rejects_invalid_values(parameter: str, value: float): + """Verify force configuration rejects values outside its physical domain.""" + with pytest.raises(ValueError): + ConveyorForceCfg(**{parameter: value}) From 470a16d3a84be6d4fc91bd8f873796be4bc0baf9 Mon Sep 17 00:00:00 2001 From: Maximilian Krause Date: Thu, 6 Aug 2026 16:49:24 -0700 Subject: [PATCH 03/23] Format conveyor force driver --- .../contrib/conveyor_franka/conveyor_force_driver.py | 1 - 1 file changed, 1 deletion(-) diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_force_driver.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_force_driver.py index 69928731add5..69edcfb4e61b 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_force_driver.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_force_driver.py @@ -15,7 +15,6 @@ import re import warp as wp - from isaaclab_newton.physics import NewtonManager from .conveyor_geometry import BELT_CENTER_X, BELT_CENTER_Y, BELT_HALF_STRAIGHT, belt_direction From 77bf43e2bf26ba02cfd1ad07d9c528d1cf483070 Mon Sep 17 00:00:00 2001 From: Maximilian Krause Date: Thu, 6 Aug 2026 17:01:41 -0700 Subject: [PATCH 04/23] Declare conveyor runtime dependencies --- .../conveyor_franka/conveyor_franka_env_cfg.py | 6 ++++++ source/isaaclab_tasks/pyproject.toml | 2 ++ .../test/contrib/test_conveyor_franka_geometry.py | 12 +++++++++++- 3 files changed, 19 insertions(+), 1 deletion(-) diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env_cfg.py index bd4261ae2af0..9fbd56e57ed7 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env_cfg.py @@ -359,6 +359,12 @@ class ConveyorFrankaEnvCfg(ManagerBasedRLEnvCfg): def __post_init__(self) -> None: self.seed = 42 # Frame the complete robot and both conveyor lanes in the Newton viewer. + try: + import isaaclab_visualizers # noqa: F401 + except ModuleNotFoundError as exc: + if exc.name != "isaaclab_visualizers": + raise + return from isaaclab_visualizers.newton import NewtonVisualizerCfg self.sim.default_visualizer_cfg = NewtonVisualizerCfg( diff --git a/source/isaaclab_tasks/pyproject.toml b/source/isaaclab_tasks/pyproject.toml index 25599c2c2d72..976449802e3b 100644 --- a/source/isaaclab_tasks/pyproject.toml +++ b/source/isaaclab_tasks/pyproject.toml @@ -20,11 +20,13 @@ requires-python = ">=3.12" dependencies = [ "isaaclab", "isaaclab_assets", + "isaaclab_newton", ] [tool.uv.sources] isaaclab = { path = "../isaaclab", editable = true } isaaclab_assets = { path = "../isaaclab_assets", editable = true } +isaaclab_newton = { path = "../isaaclab_newton", editable = true } [project.urls] Homepage = "https://github.com/isaac-sim/IsaacLab" diff --git a/source/isaaclab_tasks/test/contrib/test_conveyor_franka_geometry.py b/source/isaaclab_tasks/test/contrib/test_conveyor_franka_geometry.py index 892502214a1f..7777d5dc3c31 100644 --- a/source/isaaclab_tasks/test/contrib/test_conveyor_franka_geometry.py +++ b/source/isaaclab_tasks/test/contrib/test_conveyor_franka_geometry.py @@ -5,11 +5,12 @@ """Tests for the contributed conveyor Franka racetrack geometry.""" +import sys from collections import Counter import pytest -from isaaclab_tasks.contrib.conveyor_franka.conveyor_franka_env_cfg import ConveyorForceCfg +from isaaclab_tasks.contrib.conveyor_franka.conveyor_franka_env_cfg import ConveyorForceCfg, ConveyorFrankaEnvCfg from isaaclab_tasks.contrib.conveyor_franka.conveyor_geometry import ( BELT_TOP_Z, TURN_SEGMENT_COUNT, @@ -67,6 +68,15 @@ def test_racetrack_lanes_counter_rotate(): assert belt_direction("Left") == -belt_direction("Right") +def test_environment_config_without_optional_visualizers(monkeypatch): + """The task configuration remains usable without the visualizer package.""" + monkeypatch.setitem(sys.modules, "isaaclab_visualizers", None) + + cfg = ConveyorFrankaEnvCfg() + + assert cfg.sim.default_visualizer_cfg is None + + @pytest.mark.parametrize( ("parameter", "value"), (("speed", -0.1), ("friction", -0.1), ("normal_threshold", 1.1)), From 01d9955299a6a46fb4c8c3efbb3080b237e4fd24 Mon Sep 17 00:00:00 2001 From: Maximilian Krause Date: Thu, 6 Aug 2026 17:07:49 -0700 Subject: [PATCH 05/23] Address conveyor review feedback --- .../isaaclab/sim/spawners/meshes/meshes.py | 18 ++++++++++-------- source/isaaclab/test/sim/test_spawn_meshes.py | 2 +- .../contrib/conveyor_franka/__init__.py | 2 +- .../conveyor_franka/conveyor_force_driver.py | 2 +- .../conveyor_franka/conveyor_franka_env.py | 8 +++----- .../conveyor_franka/conveyor_franka_env_cfg.py | 2 +- .../conveyor_franka/conveyor_geometry.py | 2 +- .../contrib/test_conveyor_franka_geometry.py | 2 +- 8 files changed, 19 insertions(+), 19 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/spawners/meshes/meshes.py b/source/isaaclab/isaaclab/sim/spawners/meshes/meshes.py index 4bd805463b9f..83c4323ff34c 100644 --- a/source/isaaclab/isaaclab/sim/spawners/meshes/meshes.py +++ b/source/isaaclab/isaaclab/sim/spawners/meshes/meshes.py @@ -87,6 +87,15 @@ def spawn_mesh_custom( mesh = trimesh.Trimesh(vertices=vertices, faces=faces, process=False) stage = get_current_stage() _spawn_mesh_geom_from_mesh(prim_path, cfg, mesh, translation, orientation, stage=stage) + + # Author the diffuse color as a fallback for kitless visualizers, where materials cannot be bound. + if not has_kit(): + diffuse_color = getattr(cfg.visual_material, "diffuse_color", None) + if diffuse_color is not None: + display_color = tuple(_srgb_to_linear_channel(value) for value in diffuse_color) + mesh_prim = UsdGeom.Mesh(stage.GetPrimAtPath(f"{prim_path}/geometry/mesh")) + mesh_prim.CreateDisplayColorAttr([display_color]) + return stage.GetPrimAtPath(prim_path) @@ -545,15 +554,8 @@ def _spawn_mesh_geom_from_mesh( # apply visual material if cfg.visual_material is not None: - # Keep PreviewSurface colors available to lightweight renderers that - # consume USD displayColor but cannot construct Kit materials. - diffuse_color = getattr(cfg.visual_material, "diffuse_color", None) - if diffuse_color is not None: - display_color = tuple(_srgb_to_linear_channel(value) for value in diffuse_color) - UsdGeom.Mesh(mesh_prim).CreateDisplayColorAttr([display_color]) if not has_kit(): - if diffuse_color is None: - logger.warning("Skipping visual material application for '%s' in kitless mode.", mesh_prim_path) + logger.warning("Skipping visual material application for '%s' in kitless mode.", mesh_prim_path) else: if not cfg.visual_material_path.startswith("/"): material_path = f"{geom_prim_path}/{cfg.visual_material_path}" diff --git a/source/isaaclab/test/sim/test_spawn_meshes.py b/source/isaaclab/test/sim/test_spawn_meshes.py index 49ba0232cb68..e3acdaea1c9a 100644 --- a/source/isaaclab/test/sim/test_spawn_meshes.py +++ b/source/isaaclab/test/sim/test_spawn_meshes.py @@ -172,7 +172,7 @@ def test_spawn_custom_mesh(sim): assert mesh_prim.GetAttribute("faceVertexIndices").Get() == [0, 1, 2, 0, 2, 3] assert mesh_prim.GetAttribute("subdivisionScheme").Get() == "none" assert mesh_prim.GetAttribute("physics:approximation").Get() == "none" - assert mesh_prim.GetAttribute("primvars:displayColor").HasAuthoredValue() + assert not mesh_prim.GetAttribute("primvars:displayColor").HasAuthoredValue() """ diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/__init__.py index bb4577de56f2..a15c87d8c659 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/__init__.py @@ -1,4 +1,4 @@ -# Copyright (c) 2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# 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 diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_force_driver.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_force_driver.py index 69edcfb4e61b..6fdbe9ef63ad 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_force_driver.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_force_driver.py @@ -1,4 +1,4 @@ -# Copyright (c) 2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# 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 diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env.py index 82ffda560922..ebf34ce17413 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env.py @@ -1,4 +1,4 @@ -# Copyright (c) 2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# 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 @@ -23,10 +23,8 @@ class ConveyorFrankaEnv(ManagerBasedRLEnv): cfg: ConveyorFrankaEnvCfg - def __init__(self, cfg: ConveyorFrankaEnvCfg, **kwargs): - # Gym forwards registry metadata alongside the resolved configuration. - del kwargs - super().__init__(cfg) + def __init__(self, cfg: ConveyorFrankaEnvCfg, render_mode: str | None = None, **kwargs): + super().__init__(cfg, render_mode=render_mode, **kwargs) self._conveyor_driver = ConveyorForceDriver( num_envs=self.num_envs, speed=cfg.conveyor_force.speed, diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env_cfg.py index 9fbd56e57ed7..d432a84bf20b 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env_cfg.py @@ -1,4 +1,4 @@ -# Copyright (c) 2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# 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 diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_geometry.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_geometry.py index 33667f197402..c8ad11c20756 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_geometry.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_geometry.py @@ -1,4 +1,4 @@ -# Copyright (c) 2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# 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 diff --git a/source/isaaclab_tasks/test/contrib/test_conveyor_franka_geometry.py b/source/isaaclab_tasks/test/contrib/test_conveyor_franka_geometry.py index 7777d5dc3c31..a9889e919de6 100644 --- a/source/isaaclab_tasks/test/contrib/test_conveyor_franka_geometry.py +++ b/source/isaaclab_tasks/test/contrib/test_conveyor_franka_geometry.py @@ -1,4 +1,4 @@ -# Copyright (c) 2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause From e58fbc2e7a8207add184022f5a70f25a77418103 Mon Sep 17 00:00:00 2001 From: Maximilian Krause Date: Thu, 6 Aug 2026 23:09:57 -0700 Subject: [PATCH 06/23] Add conveyor transfer MDP Add reset-state curriculum, observations, rewards, and relative joint actions for four-cube cross-conveyor transfer. Calibrate Franka contact dynamics and belt friction for stable moving-belt manipulation. --- .../contrib/conveyor_franka/__init__.py | 3 + .../conveyor_franka/agents/__init__.py | 6 + .../conveyor_franka/agents/rsl_rl_ppo_cfg.py | 179 +++++++ .../conveyor_franka/conveyor_franka_env.py | 10 +- .../conveyor_franka_env_cfg.py | 268 ++++++++--- .../conveyor_franka/franka_robot_cfg.py | 48 ++ .../contrib/conveyor_franka/mdp/__init__.py | 42 ++ .../contrib/conveyor_franka/mdp/actions.py | 96 ++++ .../conveyor_franka/mdp/actions_cfg.py | 48 ++ .../conveyor_franka/mdp/curriculums.py | 304 ++++++++++++ .../contrib/conveyor_franka/mdp/kinematics.py | 71 +++ .../conveyor_franka/mdp/observations.py | 152 ++++++ .../conveyor_franka/mdp/reset_events.py | 444 ++++++++++++++++++ .../contrib/conveyor_franka/mdp/rewards.py | 123 +++++ .../contrib/conveyor_franka/mdp/state.py | 42 ++ .../conveyor_franka/mdp/terminations.py | 180 +++++++ .../test/contrib/test_conveyor_franka_mdp.py | 289 ++++++++++++ 17 files changed, 2231 insertions(+), 74 deletions(-) create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/agents/__init__.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/agents/rsl_rl_ppo_cfg.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/franka_robot_cfg.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/__init__.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/actions.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/actions_cfg.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/curriculums.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/kinematics.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/observations.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/reset_events.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/rewards.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/state.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/terminations.py create mode 100644 source/isaaclab_tasks/test/contrib/test_conveyor_franka_mdp.py diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/__init__.py index a15c87d8c659..160e417072d8 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/__init__.py @@ -7,11 +7,14 @@ import gymnasium as gym +from . import agents + gym.register( id="IsaacContrib-Conveyor-Franka-Newton-v0", entry_point=f"{__name__}.conveyor_franka_env:ConveyorFrankaEnv", disable_env_checker=True, kwargs={ "env_cfg_entry_point": f"{__name__}.conveyor_franka_env_cfg:ConveyorFrankaEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:ConveyorFrankaPPORunnerCfg", }, ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/agents/__init__.py new file mode 100644 index 000000000000..0e6e80a26022 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/agents/__init__.py @@ -0,0 +1,6 @@ +# 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 + +"""Agent configurations for conveyor transfer.""" diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 000000000000..8e6bdf4d71af --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/agents/rsl_rl_ppo_cfg.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 + +"""RSL-RL PPO configuration for conveyor transfer.""" + +import torch +import torch.nn as nn +from rsl_rl.modules.distribution import GaussianDistribution +from torch.distributions import Bernoulli, Normal + +from isaaclab.utils.configclass import configclass + +from isaaclab_rl.rsl_rl import RslRlMLPModelCfg, RslRlOnPolicyRunnerCfg, RslRlPpoAlgorithmCfg + + +class _ConveyorDeterministicOutput(nn.Module): + """Threshold the gripper logit into the action used by the environment.""" + + def forward(self, output: torch.Tensor) -> torch.Tensor: + gripper = torch.where( + output[..., -1:] >= 0.0, + torch.ones_like(output[..., -1:]), + -torch.ones_like(output[..., -1:]), + ) + return torch.cat((output[..., :-1], gripper), dim=-1) + + +class ConveyorGaussianBernoulliDistribution(GaussianDistribution): + """Use Gaussian exploration for the arm and a Bernoulli gripper. + + The final policy output controls a physically binary action. Sampling it + from a Gaussian would assign different log probabilities to values that + become the same open or close command after thresholding. A Bernoulli + instead optimizes exactly the physical decision seen by the environment. + """ + + def __init__( + self, + output_dim: int, + init_std: float = 0.45, + std_range: tuple[float, float] = (0.15, 0.65), + std_type: str = "scalar", + **kwargs, + ) -> None: + if output_dim < 2: + raise ValueError("The conveyor distribution requires arm outputs followed by one gripper output.") + if len(std_range) != 2 or std_range[0] <= 0.0 or std_range[0] >= std_range[1]: + raise ValueError("std_range must contain positive, increasing bounds.") + if not std_range[0] < init_std < std_range[1]: + raise ValueError("init_std must lie strictly inside std_range.") + if std_type != "scalar": + raise ValueError("The conveyor distribution supports only scalar standard-deviation parameters.") + super().__init__(output_dim, init_std=init_std, std_type=std_type, **kwargs) + self.std_range = (float(std_range[0]), float(std_range[1])) + self._arm_distribution: Normal | None = None + self._gripper_distribution: Bernoulli | None = None + initial_fraction = (init_std - self.std_range[0]) / (self.std_range[1] - self.std_range[0]) + initial_logit = torch.logit(torch.tensor(initial_fraction, dtype=self.std_param.dtype)) + with torch.no_grad(): + self.std_param.fill_(initial_logit) + + def update(self, mlp_output: torch.Tensor) -> None: + """Update continuous-arm and binary-gripper distributions.""" + minimum_std, maximum_std = self.std_range + arm_std = minimum_std + (maximum_std - minimum_std) * torch.sigmoid(self.std_param[:-1]) + self._arm_distribution = Normal(mlp_output[..., :-1], arm_std) + self._gripper_distribution = Bernoulli(logits=mlp_output[..., -1:]) + + def sample(self) -> torch.Tensor: + """Sample seven residuals followed by an exact signed binary action.""" + gripper_open = self._gripper_distribution.sample() + return torch.cat((self._arm_distribution.sample(), 2.0 * gripper_open - 1.0), dim=-1) + + def deterministic_output(self, mlp_output: torch.Tensor) -> torch.Tensor: + """Return arm means and the most likely binary gripper command.""" + return _ConveyorDeterministicOutput()(mlp_output) + + def as_deterministic_output_module(self) -> nn.Module: + """Return an exportable deterministic-output transform.""" + return _ConveyorDeterministicOutput() + + @property + def mean(self) -> torch.Tensor: + """Return arm means and the expected signed gripper command.""" + gripper_mean = 2.0 * self._gripper_distribution.probs - 1.0 + return torch.cat((self._arm_distribution.mean, gripper_mean), dim=-1) + + @property + def std(self) -> torch.Tensor: + """Return arm standard deviations and signed-Bernoulli spread.""" + gripper_std = 2.0 * torch.sqrt(self._gripper_distribution.probs * (1.0 - self._gripper_distribution.probs)) + return torch.cat((self._arm_distribution.stddev, gripper_std), dim=-1) + + @property + def entropy(self) -> torch.Tensor: + """Return joint Gaussian-plus-Bernoulli entropy.""" + return self._arm_distribution.entropy().sum(dim=-1) + self._gripper_distribution.entropy().sum(dim=-1) + + @property + def params(self) -> tuple[torch.Tensor, ...]: + """Return parameters needed to evaluate the mixed KL divergence.""" + return self._arm_distribution.mean, self._arm_distribution.stddev, self._gripper_distribution.logits + + def log_prob(self, outputs: torch.Tensor) -> torch.Tensor: + """Evaluate the exact continuous/binary physical action.""" + arm_log_prob = self._arm_distribution.log_prob(outputs[..., :-1]).sum(dim=-1) + gripper_open = (outputs[..., -1:] >= 0.0).to(outputs.dtype) + return arm_log_prob + self._gripper_distribution.log_prob(gripper_open).sum(dim=-1) + + def kl_divergence( + self, + old_params: tuple[torch.Tensor, ...], + new_params: tuple[torch.Tensor, ...], + ) -> torch.Tensor: + """Return ``KL(old || new)`` for both action families.""" + old_arm_mean, old_arm_std, old_gripper_logits = old_params + new_arm_mean, new_arm_std, new_gripper_logits = new_params + arm_kl = torch.distributions.kl_divergence( + Normal(old_arm_mean, old_arm_std), + Normal(new_arm_mean, new_arm_std), + ).sum(dim=-1) + old_gripper_probability = torch.sigmoid(old_gripper_logits) + gripper_kl = ( + old_gripper_probability * (old_gripper_logits - new_gripper_logits) + - torch.nn.functional.softplus(old_gripper_logits) + + torch.nn.functional.softplus(new_gripper_logits) + ).sum(dim=-1) + return arm_kl + gripper_kl.clamp_min(0.0) + + +@configclass +class ConveyorGaussianBernoulliDistributionCfg(RslRlMLPModelCfg.GaussianDistributionCfg): + """Bounded Gaussian arm exploration with a Bernoulli gripper.""" + + class_name: str = ( + "isaaclab_tasks.contrib.conveyor_franka.agents.rsl_rl_ppo_cfg:ConveyorGaussianBernoulliDistribution" + ) + std_range: tuple[float, float] = (0.15, 0.65) + + +@configclass +class ConveyorFrankaPPORunnerCfg(RslRlOnPolicyRunnerCfg): + """PPO configuration for four-cube commanded transfer.""" + + num_steps_per_env = 32 + max_iterations = 4000 + save_interval = 50 + experiment_name = "conveyor_franka_transfer" + clip_actions = 1.0 + obs_groups = {"actor": ["policy"], "critic": ["policy"]} + actor = RslRlMLPModelCfg( + hidden_dims=[512, 256, 128], + activation="elu", + obs_normalization=True, + distribution_cfg=ConveyorGaussianBernoulliDistributionCfg(init_std=0.45), + ) + critic = RslRlMLPModelCfg( + hidden_dims=[512, 256, 128], + activation="elu", + obs_normalization=True, + ) + algorithm = RslRlPpoAlgorithmCfg( + value_loss_coef=1.0, + use_clipped_value_loss=True, + clip_param=0.2, + entropy_coef=0.001, + num_learning_epochs=5, + num_mini_batches=16, + learning_rate=1.0e-4, + schedule="fixed", + # At 60 Hz, the ten-second pickup-to-placement horizon needs the same + # long-horizon discount used by the reset-driven Franka stack task. + gamma=0.999, + lam=0.95, + desired_kl=0.01, + max_grad_norm=1.0, + ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env.py index ebf34ce17413..cc092a2a55ef 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env.py @@ -40,17 +40,9 @@ def step(self, action: torch.Tensor) -> VecEnvStepReturn: return result def _reset_idx(self, env_ids: Sequence[int]): - """Reset selected environments and restore the zero-action arm target.""" + """Reset selected environments and discard stale conveyor forces.""" super()._reset_idx(env_ids) conveyor_driver = getattr(self, "_conveyor_driver", None) if conveyor_driver is not None: conveyor_driver.clear() - - # There is deliberately no arm action term yet. Keep the position-controlled - # Franka at its configured pose during zero-action scene playback. - robot = self.scene["robot"] - joint_targets = robot.data.default_joint_pos - if env_ids is not None: - joint_targets = joint_targets[env_ids] - robot.set_joint_position_target(joint_targets, env_ids=env_ids) diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env_cfg.py index d432a84bf20b..217704c9345e 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env_cfg.py @@ -8,20 +8,27 @@ from __future__ import annotations from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg, NewtonCollisionPipelineCfg, NewtonShapeCfg +from isaaclab_newton.sim.schemas import MujocoCollisionCfg, NewtonMaterialPropertiesCfg import isaaclab.sim as sim_utils from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg from isaaclab.envs import ManagerBasedRLEnvCfg +from isaaclab.managers import CurriculumTermCfg as CurrTerm +from isaaclab.managers import EventTermCfg as EventTerm +from isaaclab.managers import ObservationGroupCfg as ObsGroup +from isaaclab.managers import ObservationTermCfg as ObsTerm +from isaaclab.managers import RewardTermCfg as RewTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.managers import TerminationTermCfg as DoneTerm from isaaclab.scene import InteractiveSceneCfg from isaaclab.sensors import ContactSensorCfg from isaaclab.sim import SimulationCfg +from isaaclab.sim.schemas import UsdPhysicsCollisionCfg from isaaclab.sim.spawners.materials import RigidBodyMaterialBaseCfg from isaaclab.utils.configclass import configclass -from isaaclab_assets.robots.franka import FRANKA_PANDA_MENAGERIE_CFG - +from . import mdp from .conveyor_geometry import ( - BELT_CENTER_X, BELT_CENTER_Y, BELT_COLOR, BELT_TURN_RADIUS, @@ -31,6 +38,7 @@ belt_mesh_spec, guard_mesh_specs, ) +from .franka_robot_cfg import FRANKA_PANDA_CONVEYOR_CFG _DYNAMIC_PROPERTIES = sim_utils.RigidBodyBaseCfg() @@ -44,16 +52,146 @@ def _srgb_to_linear_channel(value: float) -> float: @configclass class ActionsCfg: - """Empty action configuration for zero-action scene playback.""" - - pass + """Relative arm and binary gripper actions.""" + + arm_action = mdp.ConveyorRelativeJointPositionActionCfg( + asset_name="robot", + joint_names=["panda_joint[1-7]"], + scale=0.12, + max_delta=0.12, + gravity_compensation=True, + ) + gripper_action = mdp.ResetBufferedGripperActionCfg( + asset_name="robot", + joint_names=["panda_finger_joint[1-2]"], + open_command_expr={"panda_finger_joint.*": 0.04}, + close_command_expr={"panda_finger_joint.*": 0.0}, + force_close_steps=5, + ) @configclass class ObservationsCfg: - """Empty observation configuration while the task objective is being designed.""" + """Policy observations with stable cube identity and transfer commands.""" + + @configclass + class PolicyCfg(ObsGroup): + """Fully observed transfer policy input.""" + + joint_pos = ObsTerm( + func=mdp.joint_pos_rel, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=["panda_joint[1-7]"])}, + ) + joint_vel = ObsTerm( + func=mdp.joint_vel_rel, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=["panda_joint[1-7]"])}, + ) + gripper_pos = ObsTerm( + func=mdp.gripper_joint_positions, + params={"robot_cfg": SceneEntityCfg("robot", joint_names=["panda_finger_joint[1-2]"])}, + ) + objects = ObsTerm(func=mdp.transfer_object_observation) + active_transfer = ObsTerm(func=mdp.active_transfer_features) + target_cube = ObsTerm(func=mdp.target_cube_one_hot) + cube_conveyors = ObsTerm(func=mdp.cube_conveyor_state) + target_side = ObsTerm(func=mdp.target_side_one_hot) + eef_velocity = ObsTerm(func=mdp.end_effector_velocity) + eef_axes = ObsTerm(func=mdp.end_effector_axes) + last_action = ObsTerm(func=mdp.last_action) + + def __post_init__(self) -> None: + self.enable_corruption = False + self.concatenate_terms = True + + policy: PolicyCfg = PolicyCfg() + + +@configclass +class EventCfg: + """Reset the scene, then restore one validated transfer state.""" + + reset_all = EventTerm(func=mdp.reset_scene_to_default, mode="reset") + reset_from_state_table = EventTerm( + func=mdp.ConveyorResetStateTable, + mode="reset", + params={ + "fixed_recipe": None, + "fixed_variant_id": None, + "fixed_target_cube_id": None, + "fixed_source_side_id": None, + "belt_start_x_range": (0.30, 0.82), + "cube_position_noise": 0.015, + "arm_joint_noise": 0.015, + }, + ) + + +@configclass +class RewardsCfg: + """Transfer progress, completion, and regularization rewards.""" + + progress = RewTerm(func=mdp.ConveyorTransferProgressReward, weight=60.0) + success = RewTerm(func=mdp.transfer_success_reward, weight=600.0) + failure = RewTerm(func=mdp.terminal_failure, weight=-60.0) + arm_action_l2 = RewTerm( + func=mdp.action_term_l2, + params={"action_name": "arm_action"}, + weight=-1.0e-3, + ) + action_rate_l2 = RewTerm(func=mdp.action_rate_l2, weight=-1.0e-3) + joint_velocity_l2 = RewTerm( + func=mdp.finite_joint_velocity_l2, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=["panda_joint[1-7]"])}, + weight=-1.0e-4, + ) - pass + +@configclass +class TerminationsCfg: + """Successful placement, physical failure, and horizon terms.""" + + learning_progress_context = DoneTerm( + func=mdp.ConveyorResetLearningProgress, + params={ + "minimum_episode_steps": 3, + "minimum_progress": 0.35, + "maximum_target_potential": 5.0, + }, + ) + success = DoneTerm( + func=mdp.StableConveyorTransfer, + params={ + "minimum_episode_steps": 2, + "hold_steps": 3, + "lateral_tolerance": 0.055, + "maximum_cube_speed": 0.65, + "minimum_finger_position": 0.027, + "minimum_tool_clearance": 0.055, + }, + ) + cube_out_of_workspace = DoneTerm(func=mdp.cube_out_of_workspace) + nonfinite_scene_state = DoneTerm(func=mdp.nonfinite_scene_state) + time_out = DoneTerm(func=mdp.time_out, time_out=True) + + +@configclass +class CurriculumCfg: + """Adaptive phase-balanced reset-state sampling.""" + + reset_sampling = CurrTerm( + func=mdp.ConveyorResetCurriculum, + params={ + "progress_context_name": "learning_progress_context", + "final_success_termination_name": "success", + # Match Franka Stack: sampling follows each row's recent policy + # competence instead of retaining stale early failures forever. + "monitored_history_len": 50, + # Keep a deployment-facing stream while the remaining starts + # adapt around the rolling pickup-to-placement frontier. Adaptive + # rows remain balanced across recipe, cube identity, and side. + "deployment_probability": 0.35, + }, + ) @configclass @@ -149,14 +287,21 @@ def _static_mesh( friction: float, roughness: float, metallic: float, + mujoco_priority: int | None = None, ) -> AssetBaseCfg: """Build a static colliding triangle-mesh configuration.""" + collision_props = sim_utils.CollisionBaseCfg() + if mujoco_priority is not None: + collision_props = [ + UsdPhysicsCollisionCfg(collision_enabled=True), + MujocoCollisionCfg(priority=mujoco_priority), + ] return AssetBaseCfg( prim_path=prim_path, spawn=sim_utils.MeshCustomCfg( vertices=spec.vertices, faces=spec.faces, - collision_props=sim_utils.CollisionBaseCfg(), + collision_props=collision_props, physics_material=RigidBodyMaterialBaseCfg( static_friction=friction, dynamic_friction=friction, @@ -171,21 +316,29 @@ def _static_mesh( ) -def _parcel( +def _cube( name: str, - spawn: sim_utils.ShapeCfg, + color: tuple[float, float, float], pos: tuple[float, float, float], ) -> RigidObjectCfg: - """Build a dynamic parcel configuration.""" + """Build one numbered dynamic transfer cube.""" + spawn = sim_utils.CuboidCfg( + size=(0.04, 0.04, 0.04), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=color, roughness=0.75), + ) spawn.rigid_props = _DYNAMIC_PROPERTIES - spawn.mass_props = sim_utils.MassPropertiesCfg(mass=0.25) - spawn.collision_props = sim_utils.CollisionBaseCfg() - spawn.physics_material = RigidBodyMaterialBaseCfg( - # The force driver supplies traction explicitly. This is just above MuJoCo's - # minimum valid coefficient and mirrors Newton's force-conveyor example. - static_friction=1.1e-5, - dynamic_friction=1.1e-5, - restitution=0.05, + spawn.mass_props = sim_utils.MassPropertiesCfg(mass=0.05) + spawn.collision_props = sim_utils.CollisionBaseCfg(contact_offset=0.0, rest_offset=0.0) + spawn.physics_material = NewtonMaterialPropertiesCfg( + # The belt's higher MuJoCo contact priority overrides this friction + # only for belt/cube pairs, leaving physical finger/cube friction. + static_friction=0.8, + dynamic_friction=0.6, + restitution=0.0, + torsional_friction=0.002, + rolling_friction=0.0001, + contact_stiffness=1.0e4, + contact_damping=200.0, ) spawn.func = _spawn_shape_with_display_color return RigidObjectCfg( @@ -199,8 +352,8 @@ def _parcel( class ConveyorFrankaSceneCfg(InteractiveSceneCfg): """Scene with two counter-rotating racetrack conveyors around a table-mounted Franka.""" - # Use the MuJoCo Menagerie-derived model with Newton's MuJoCo MJWarp solver. - robot = FRANKA_PANDA_MENAGERIE_CFG.replace( + # Use the MuJoCo Menagerie-derived model with explicit manipulation gains. + robot = FRANKA_PANDA_CONVEYOR_CFG.replace( prim_path="{ENV_REGEX_NS}/Robot", init_state=ArticulationCfg.InitialStateCfg( joint_pos={ @@ -229,43 +382,13 @@ class ConveyorFrankaSceneCfg(InteractiveSceneCfg): color=(0.18, 0.20, 0.23), ) - parcel_left_box = _parcel( - "ParcelLeftBox", - sim_utils.CuboidCfg( - size=(0.075, 0.055, 0.06), - visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=PARCEL_COLOR, roughness=0.8), - ), - (BELT_CENTER_X - 0.12, BELT_CENTER_Y + BELT_TURN_RADIUS, 0.085), - ) - parcel_left_cylinder = _parcel( - "ParcelLeftCylinder", - sim_utils.CylinderCfg( - radius=0.032, - height=0.065, - visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.18, 0.48, 0.82), roughness=0.8), - ), - (BELT_CENTER_X + 0.18, BELT_CENTER_Y - BELT_TURN_RADIUS, 0.085), - ) - parcel_right_box = _parcel( - "ParcelRightBox", - sim_utils.CuboidCfg( - size=(0.06, 0.06, 0.075), - visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.86, 0.34, 0.12), roughness=0.8), - ), - (BELT_CENTER_X + 0.14, -BELT_CENTER_Y + BELT_TURN_RADIUS, 0.0925), - ) - parcel_right_capsule = _parcel( - "ParcelRightCapsule", - sim_utils.CapsuleCfg( - radius=0.026, - height=0.075, - visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.34, 0.68, 0.28), roughness=0.8), - ), - (BELT_CENTER_X - 0.18, -BELT_CENTER_Y - BELT_TURN_RADIUS, 0.095), - ) + cube_0 = _cube("Cube0", (0.15, 0.35, 0.90), (0.30, BELT_CENTER_Y - BELT_TURN_RADIUS, 0.06)) + cube_1 = _cube("Cube1", (0.90, 0.20, 0.15), (0.78, BELT_CENTER_Y - BELT_TURN_RADIUS, 0.06)) + cube_2 = _cube("Cube2", (0.15, 0.75, 0.25), (0.30, -BELT_CENTER_Y + BELT_TURN_RADIUS, 0.06)) + cube_3 = _cube("Cube3", PARCEL_COLOR, (0.78, -BELT_CENTER_Y + BELT_TURN_RADIUS, 0.06)) - parcel_contacts = ContactSensorCfg( - prim_path="{ENV_REGEX_NS}/Parcel.*", + cube_contacts = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Cube.*", update_period=0.0, history_length=1, debug_vis=False, @@ -297,6 +420,9 @@ def __post_init__(self) -> None: friction=1.1e-5, roughness=0.9, metallic=0.0, + # MuJoCo otherwise resolves equal-priority pair friction + # with max(belt, cube), pinning parcels to the static mesh. + mujoco_priority=1, ), ) @@ -318,17 +444,18 @@ def __post_init__(self) -> None: @configclass class ConveyorFrankaEnvCfg(ManagerBasedRLEnvCfg): - """Manager-based environment configuration for the conveyor Franka scene.""" + """Manager-based RL task for commanded conveyor-to-conveyor cube transfer.""" scene: ConveyorFrankaSceneCfg = ConveyorFrankaSceneCfg(num_envs=1, env_spacing=3.0, replicate_physics=True) conveyor_force: ConveyorForceCfg = ConveyorForceCfg() - # MDP managers will be populated once the manipulation objective is defined. actions: ActionsCfg = ActionsCfg() observations: ObservationsCfg = ObservationsCfg() - rewards = None - terminations = None - decimation: int = 1 - episode_length_s: float = 1.0e6 + events: EventCfg = EventCfg() + rewards: RewardsCfg = RewardsCfg() + terminations: TerminationsCfg = TerminationsCfg() + curriculum: CurriculumCfg = CurriculumCfg() + decimation: int = 2 + episode_length_s: float = 10.0 sim: SimulationCfg = SimulationCfg( dt=1.0 / 120.0, @@ -349,7 +476,10 @@ class ConveyorFrankaEnvCfg(ManagerBasedRLEnvCfg): ccd_iterations=35, ), collision_cfg=NewtonCollisionPipelineCfg(), - default_shape_cfg=NewtonShapeCfg(), + # Refresh contacts between the two 240 Hz solver substeps and + # preserve the authored surfaces without speculative separation. + collision_decimation=1, + default_shape_cfg=NewtonShapeCfg(margin=0.0, gap=0.0), num_substeps=2, use_cuda_graph=False, load_visual_shapes=True, @@ -371,3 +501,11 @@ def __post_init__(self) -> None: eye=(2.3, -2.7, 1.8), lookat=(0.45, 0.0, 0.35), ) + + def play_mode(self) -> None: + """Evaluate complete transfers from randomized moving-belt starts.""" + super().play_mode() + self.scene.num_envs = min(self.scene.num_envs, 8) + self.events.reset_from_state_table.params["fixed_recipe"] = int(mdp.ConveyorResetRecipe.BELT) + self.events.reset_from_state_table.params["fixed_variant_id"] = mdp.BELT_DEPLOYMENT_VARIANT + self.curriculum = None diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/franka_robot_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/franka_robot_cfg.py new file mode 100644 index 000000000000..5af59966c399 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/franka_robot_cfg.py @@ -0,0 +1,48 @@ +# 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 + +"""Task-calibrated Franka configuration for conveyor manipulation.""" + +from isaaclab.actuators import ImplicitActuatorCfg + +from isaaclab_assets.robots.franka import FRANKA_PANDA_MENAGERIE_CFG + +FRANKA_PANDA_CONVEYOR_CFG = FRANKA_PANDA_MENAGERIE_CFG.copy() +FRANKA_PANDA_CONVEYOR_CFG.spawn.rigid_props.disable_gravity = False +FRANKA_PANDA_CONVEYOR_CFG.actuators = { + "panda_arm": ImplicitActuatorCfg( + joint_names_expr=["panda_joint[1-7]"], + effort_limit_sim={"panda_joint[1-4]": 87.0, "panda_joint[5-7]": 12.0}, + velocity_limit_sim={"panda_joint[1-4]": 20.0, "panda_joint[5-7]": 25.0}, + stiffness={ + "panda_joint[1-4]": 600.0, + "panda_joint5": 250.0, + "panda_joint6": 150.0, + "panda_joint7": 50.0, + }, + damping={ + "panda_joint[1-4]": 50.0, + "panda_joint5": 30.0, + "panda_joint6": 25.0, + "panda_joint7": 15.0, + }, + armature={ + "panda_joint[1-2]": 0.6057, + "panda_joint[3-4]": 0.4625, + "panda_joint[5-7]": 0.2055, + }, + ), + "panda_hand": ImplicitActuatorCfg( + joint_names_expr=["panda_finger_joint[1-2]"], + effort_limit_sim=70.0, + velocity_limit_sim=2.0, + stiffness=350.0, + # Keep the 0.1 kg m^2 armature close to critical damping so the + # fingers establish contact within a few 50 Hz policy steps. + damping=10.0, + armature=0.1, + ), +} +"""Menagerie Franka with explicit manipulation gains and gravity compensation-ready dynamics.""" diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/__init__.py new file mode 100644 index 000000000000..a1e89483ef1f --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/__init__.py @@ -0,0 +1,42 @@ +# 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 + +"""MDP terms for the conveyor-to-conveyor Franka transfer task.""" + +from isaaclab.envs.mdp import * # noqa: F401, F403 + +from .actions import ConveyorRelativeJointPositionAction, ResetBufferedGripperAction +from .actions_cfg import ConveyorRelativeJointPositionActionCfg, ResetBufferedGripperActionCfg +from .curriculums import ConveyorResetCurriculum +from .observations import ( + active_transfer_features, + cube_conveyor_state, + end_effector_axes, + end_effector_velocity, + gripper_joint_positions, + target_cube_one_hot, + target_side_one_hot, + transfer_object_observation, +) +from .reset_events import ( + BELT_DEPLOYMENT_VARIANT, + ConveyorResetRecipe, + ConveyorResetStateTable, + build_reset_rows, +) +from .rewards import ( + ConveyorTransferProgressReward, + action_term_l2, + finite_joint_velocity_l2, + terminal_failure, + transfer_success_reward, +) +from .state import ConveyorTransferState +from .terminations import ( + ConveyorResetLearningProgress, + StableConveyorTransfer, + cube_out_of_workspace, + nonfinite_scene_state, +) diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/actions.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/actions.py new file mode 100644 index 000000000000..7fc5de5e8ec5 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/actions.py @@ -0,0 +1,96 @@ +# 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 + +"""Reset-safe relative joint actions for conveyor transfer.""" + +from __future__ import annotations + +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import torch + +from isaaclab.envs.mdp.actions.binary_joint_actions import BinaryJointPositionAction +from isaaclab.envs.mdp.actions.joint_actions import JointAction + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedEnv + + from .actions_cfg import ConveyorRelativeJointPositionActionCfg, ResetBufferedGripperActionCfg + + +class ConveyorRelativeJointPositionAction(JointAction): + """Apply one measured-state-relative target per policy step. + + Isaac Lab's generic relative action adds the residual during every physics + substep. This term computes the target once in :meth:`process_actions`, so + simulation decimation does not multiply the requested displacement. + """ + + cfg: ConveyorRelativeJointPositionActionCfg + + def __init__(self, cfg: ConveyorRelativeJointPositionActionCfg, env: ManagerBasedEnv) -> None: + super().__init__(cfg, env) + if cfg.max_delta <= 0.0: + raise ValueError("max_delta must be positive.") + if cfg.joint_limit_margin < 0.0: + raise ValueError("joint_limit_margin must be non-negative.") + self._workspace_lower = torch.tensor(cfg.workspace_lower, dtype=torch.float32, device=self.device) + self._workspace_upper = torch.tensor(cfg.workspace_upper, dtype=torch.float32, device=self.device) + if self._workspace_lower.shape != (self.action_dim,) or self._workspace_upper.shape != (self.action_dim,): + raise ValueError("workspace bounds must contain one value per controlled joint.") + if torch.any(self._workspace_lower >= self._workspace_upper): + raise ValueError("Every lower workspace bound must be less than its upper bound.") + resolved_joint_ids = ( + list(range(self._asset.num_joints)) if isinstance(self._joint_ids, slice) else self._joint_ids + ) + self._gravity_joint_ids = [joint_id + self._asset.num_base_dofs for joint_id in resolved_joint_ids] + self._position_targets = self._asset.data.joint_pos.torch[:, self._joint_ids].clone() + + def process_actions(self, actions: torch.Tensor) -> None: + """Convert normalized residuals into bounded position targets [rad].""" + super().process_actions(actions) + delta = torch.clamp(self._processed_actions, min=-self.cfg.max_delta, max=self.cfg.max_delta) + positions = self._asset.data.joint_pos.torch[:, self._joint_ids] + limits = self._asset.data.soft_joint_pos_limits.torch[:, self._joint_ids] + lower = torch.maximum(limits[..., 0] + self.cfg.joint_limit_margin, self._workspace_lower) + upper = torch.minimum(limits[..., 1] - self.cfg.joint_limit_margin, self._workspace_upper) + self._position_targets = torch.clamp(positions + delta, min=lower, max=upper) + self._processed_actions = self._position_targets + self._raw_actions[:] = actions + + def apply_actions(self) -> None: + """Hold the policy-step target and gravity feedforward through all physics substeps.""" + self._asset.set_joint_position_target_index(target=self._position_targets, joint_ids=self._joint_ids) + if self.cfg.gravity_compensation: + gravity = self._asset.data.gravity_compensation_forces.torch[:, self._gravity_joint_ids] + gravity = torch.where(torch.isfinite(gravity), gravity, torch.zeros_like(gravity)) + self._asset.set_joint_effort_target_index(target=gravity, joint_ids=self._joint_ids) + + def reset(self, env_ids: Sequence[int] | None = None) -> None: + """Initialize targets from the sampled reset pose.""" + super().reset(env_ids) + positions = self._asset.data.joint_pos.torch[:, self._joint_ids] + if env_ids is None: + self._position_targets[:] = positions + self._processed_actions[:] = positions + else: + self._position_targets[env_ids] = positions[env_ids] + self._processed_actions[env_ids] = positions[env_ids] + + +class ResetBufferedGripperAction(BinaryJointPositionAction): + """Keep reset-authored grasps closed during a short settling window.""" + + cfg: ResetBufferedGripperActionCfg + + def process_actions(self, actions: torch.Tensor) -> None: + """Map binary commands and preserve initially held cubes.""" + super().process_actions(actions) + state = getattr(self._env, "conveyor_transfer_state", None) + if state is None: + return + force_close = (state.held_cube_ids >= 0) & (self._env.episode_length_buf < self.cfg.force_close_steps) + self._processed_actions[force_close] = self._close_command diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/actions_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/actions_cfg.py new file mode 100644 index 000000000000..c951391bf502 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/actions_cfg.py @@ -0,0 +1,48 @@ +# 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 + +"""Action configurations for conveyor transfer.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +from isaaclab.envs.mdp.actions.actions_cfg import BinaryJointPositionActionCfg, JointActionCfg +from isaaclab.utils.configclass import configclass + +if TYPE_CHECKING: + from .actions import ConveyorRelativeJointPositionAction, ResetBufferedGripperAction + + +@configclass +class ConveyorRelativeJointPositionActionCfg(JointActionCfg): + """Configuration for measured-state relative Franka joint control.""" + + class_type: type[ConveyorRelativeJointPositionAction] | str = "{DIR}.actions:ConveyorRelativeJointPositionAction" + + joint_limit_margin: float = 0.02 + """Distance kept from each soft joint limit [rad].""" + + max_delta: float = 0.12 + """Maximum target change per policy step [rad].""" + + gravity_compensation: bool = False + """Whether to add model-based gravity feedforward to the arm joints.""" + + workspace_lower: tuple[float, ...] = (-0.75, -0.45, -0.55, -2.75, -0.45, 1.85, -0.10) + """Lower boundary of the validated transfer workspace [rad].""" + + workspace_upper: tuple[float, ...] = (0.85, 0.85, 0.35, -1.75, 0.45, 3.05, 1.65) + """Upper boundary of the validated transfer workspace [rad].""" + + +@configclass +class ResetBufferedGripperActionCfg(BinaryJointPositionActionCfg): + """Configuration for reset-grasp protection.""" + + class_type: type[ResetBufferedGripperAction] | str = "{DIR}.actions:ResetBufferedGripperAction" + + force_close_steps: int = 5 + """Initial policy steps that preserve a reset-authored grasp.""" diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/curriculums.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/curriculums.py new file mode 100644 index 000000000000..e7dc9180ff58 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/curriculums.py @@ -0,0 +1,304 @@ +# 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 + +"""Adaptive reset-state curriculum for conveyor transfer.""" + +from __future__ import annotations + +import math +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import torch + +from isaaclab.managers import CurriculumTermCfg, ManagerTermBase + +from .reset_events import BELT_DEPLOYMENT_VARIANT, CUBE_COUNT, ConveyorResetRecipe, reset_variant_counts + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +def _ring_append_bool_count_rate( + data: torch.Tensor, + stream_ids: torch.Tensor, + values: torch.Tensor, + pointer: torch.Tensor, + size: torch.Tensor, + true_count: torch.Tensor, + rate: torch.Tensor, +) -> None: + """Append a batch to exact per-row Boolean rolling windows.""" + if stream_ids.numel() == 0: + return + + capacity = data.shape[1] + unique_ids, inverse, counts = torch.unique(stream_ids, return_inverse=True, return_counts=True) + if unique_ids.numel() == stream_ids.numel(): + columns = pointer[stream_ids].long() + overwritten = torch.where( + size[stream_ids] == capacity, + data[stream_ids, columns].to(dtype=true_count.dtype), + torch.zeros_like(true_count[stream_ids]), + ) + new_true_counts = true_count[stream_ids] - overwritten + values.to(dtype=true_count.dtype) + data[stream_ids, columns] = values + pointer[stream_ids] = ((columns + 1) % capacity).to(dtype=pointer.dtype) + size[stream_ids] = (size[stream_ids] + 1).clamp(max=capacity) + true_count[stream_ids] = new_true_counts + rate[stream_ids] = new_true_counts.to(rate.dtype) / size[stream_ids].clamp(min=1) + return + + order = torch.argsort(inverse, stable=True) + sorted_ids = stream_ids[order] + sorted_values = values[order] + group_starts = counts.cumsum(0) - counts + local_rank = torch.arange(stream_ids.numel(), device=data.device) - torch.repeat_interleave(group_starts, counts) + inverse_sorted = inverse[order] + counts_sorted = counts[inverse_sorted] + true_added = torch.zeros(unique_ids.shape, device=data.device, dtype=true_count.dtype) + true_added.scatter_add_(0, inverse, values.to(dtype=true_count.dtype)) + + keep_start = (counts - capacity).clamp(min=0) + keep = local_rank >= torch.repeat_interleave(keep_start, counts) + true_kept = torch.zeros_like(true_added) + true_kept.scatter_add_(0, inverse_sorted[keep], sorted_values[keep].to(dtype=true_count.dtype)) + + overwrite_start = capacity - size[sorted_ids].long() + overwrite_mask = (counts_sorted < capacity) & (local_rank >= overwrite_start) + overwritten = torch.zeros_like(true_added) + overwrite_ids = sorted_ids[overwrite_mask] + overwrite_columns = (pointer[overwrite_ids].long() + local_rank[overwrite_mask]) % capacity + overwritten.scatter_add_( + 0, + inverse_sorted[overwrite_mask], + data[overwrite_ids, overwrite_columns].to(dtype=true_count.dtype), + ) + + kept_ids = sorted_ids[keep] + kept_columns = (pointer[kept_ids].long() + local_rank[keep]) % capacity + data[kept_ids, kept_columns] = sorted_values[keep] + replace = counts >= capacity + new_true_counts = torch.where(replace, true_kept, true_count[unique_ids] - overwritten + true_added) + new_size = (size[unique_ids].long() + counts).clamp(max=capacity) + pointer[unique_ids] = ((pointer[unique_ids].long() + counts) % capacity).to(dtype=pointer.dtype) + size[unique_ids] = new_size.to(dtype=size.dtype) + true_count[unique_ids] = new_true_counts + rate[unique_ids] = new_true_counts.to(rate.dtype) / new_size.clamp(min=1).to(rate.dtype) + + +def reset_sampling_probabilities( + recipe_ids: torch.Tensor, + variant_ids: torch.Tensor, + target_cube_ids: torch.Tensor, + source_side_ids: torch.Tensor, + attempts: torch.Tensor, + successes: torch.Tensor, + deployment_probability: float, + epsilon: float, +) -> torch.Tensor: + """Mix guaranteed deployment starts with adaptive intermediate rows.""" + if not ( + recipe_ids.shape + == variant_ids.shape + == target_cube_ids.shape + == source_side_ids.shape + == attempts.shape + == successes.shape + ): + raise ValueError("Reset row metadata and outcomes must have matching shapes.") + if not 0.0 < deployment_probability < 1.0: + raise ValueError("deployment_probability must lie strictly between zero and one.") + if epsilon <= 0.0: + raise ValueError("epsilon must be positive.") + + deployment_rows = (recipe_ids == int(ConveyorResetRecipe.BELT)) & (variant_ids == BELT_DEPLOYMENT_VARIANT) + if not bool(torch.any(deployment_rows)) or bool(torch.all(deployment_rows)): + raise ValueError("Reset table must contain deployment and intermediate rows.") + + rates = successes.float() / attempts.clamp_min(1).float() + frontier = 4.0 * rates * (1.0 - rates) + adaptive = frontier + epsilon + adaptive[deployment_rows] = 0.0 + + # Mirror Franka Stack's recipe/layout balancing: success in one physical + # phase must not starve another cube identity or transfer direction. + command_ids = 2 * target_cube_ids + source_side_ids + command_count = 2 * CUBE_COUNT + if bool(torch.any((command_ids < 0) | (command_ids >= command_count))): + raise ValueError("Reset table contains an invalid cube or source-side id.") + stratum_ids = recipe_ids * command_count + command_ids + stratum_count = len(ConveyorResetRecipe) * command_count + stratum_mass = torch.zeros(stratum_count, dtype=adaptive.dtype, device=adaptive.device) + stratum_mass.scatter_add_(0, stratum_ids, adaptive) + if bool(torch.any(stratum_mass <= 0.0)): + raise ValueError("Every recipe, cube, and source-side stratum must have intermediate reset rows.") + adaptive /= stratum_mass[stratum_ids] + adaptive[deployment_rows] = 0.0 + adaptive *= (1.0 - deployment_probability) / adaptive.sum() + deployment = deployment_rows.to(dtype=adaptive.dtype) + deployment *= deployment_probability / deployment.sum() + return adaptive + deployment + + +class ConveyorResetCurriculum(ManagerTermBase): + """Record row outcomes and sample the next physical reset states.""" + + def __init__(self, cfg: CurriculumTermCfg, env: ManagerBasedRLEnv): + super().__init__(cfg, env) + reset_term = env.event_manager.get_term_cfg("reset_from_state_table").func + if not hasattr(reset_term, "row_count"): + raise RuntimeError("ConveyorResetCurriculum requires ConveyorResetStateTable.") + self._reset_term = reset_term + self._attempts = torch.zeros(reset_term.row_count, dtype=torch.long, device=env.device) + self._progress_successes = torch.zeros_like(self._attempts) + self._final_successes = torch.zeros_like(self._attempts) + history_len = int(cfg.params.get("monitored_history_len", 50)) + if history_len < 1: + raise ValueError("monitored_history_len must be positive.") + self._progress_history = torch.zeros((reset_term.row_count, history_len), dtype=torch.bool, device=env.device) + self._history_pointer = torch.zeros(reset_term.row_count, dtype=torch.int32, device=env.device) + self._history_size = torch.zeros_like(self._history_pointer) + self._history_success_count = torch.zeros_like(self._history_pointer) + self._rolling_progress_rates = torch.zeros(reset_term.row_count, dtype=torch.float32, device=env.device) + variant_counts = reset_variant_counts() + self._diagnostic_variant_rows = tuple( + ( + recipe.name.lower(), + variant_id, + (reset_term.recipe_ids == int(recipe)) & (reset_term.variant_ids == variant_id), + ) + for recipe in (ConveyorResetRecipe.PREGRASP, ConveyorResetRecipe.BELT) + for variant_id in range(variant_counts[int(recipe)]) + ) + + def __call__( + self, + env: ManagerBasedRLEnv, + env_ids: Sequence[int], + progress_context_name: str = "learning_progress_context", + final_success_termination_name: str = "success", + deployment_probability: float = 0.35, + epsilon: float = 0.05, + monitored_history_len: int = 50, + ) -> dict[str, torch.Tensor]: + """Update adaptive evidence, sample rows, and expose diagnostics.""" + del monitored_history_len + ids = torch.as_tensor(env_ids, dtype=torch.long, device=env.device).flatten() + state = env.conveyor_transfer_state + batch_progress = torch.zeros((), dtype=torch.float32, device=env.device) + batch_success = torch.zeros((), dtype=torch.float32, device=env.device) + completed = state.initialized[ids] & (env.episode_length_buf[ids] > 0) + completed_ids = ids[completed] + if completed_ids.numel(): + progress_context = env.termination_manager.get_term_cfg(progress_context_name).func + final_success = env.termination_manager.get_term_cfg(final_success_termination_name).func + progressed = progress_context.ever_success[completed_ids] + succeeded = final_success.ever_success[completed_ids] + rows = state.row_ids[completed_ids] + _ring_append_bool_count_rate( + self._progress_history, + rows, + progressed, + self._history_pointer, + self._history_size, + self._history_success_count, + self._rolling_progress_rates, + ) + self._attempts.scatter_add_(0, rows, torch.ones_like(rows)) + self._progress_successes.scatter_add_(0, rows, progressed.long()) + self._final_successes.scatter_add_(0, rows, succeeded.long()) + batch_progress = progressed.float().mean() + batch_success = succeeded.float().mean() + + probabilities = reset_sampling_probabilities( + self._reset_term.recipe_ids, + self._reset_term.variant_ids, + self._reset_term.target_cube_ids, + self._reset_term.source_side_ids, + self._history_size, + self._history_success_count, + deployment_probability, + epsilon, + ) + if ids.numel(): + state.row_ids[ids] = torch.multinomial(probabilities, ids.numel(), replacement=True) + + attempted_rows = self._attempts > 0 + total_progress = self._history_success_count.sum().float() / self._history_size.sum().clamp_min(1) + cumulative_progress = self._progress_successes.sum().float() / self._attempts.sum().clamp_min(1) + total_success = self._final_successes.sum().float() / self._attempts.sum().clamp_min(1) + entropy = -(probabilities * probabilities.clamp_min(torch.finfo(probabilities.dtype).tiny).log()).sum() + entropy /= math.log(probabilities.numel()) + metrics: dict[str, torch.Tensor] = { + "batch_progress_rate": batch_progress, + "batch_success_rate": batch_success, + "row_coverage": attempted_rows.float().mean(), + "overall_progress_rate": total_progress, + "cumulative_progress_rate": cumulative_progress, + "overall_success_rate": total_success, + "sampling_entropy": entropy, + } + for recipe in ConveyorResetRecipe: + mask = self._reset_term.recipe_ids == int(recipe) + recipe_attempts = self._attempts[mask].sum() + metrics[f"recipe_{recipe.name.lower()}_probability"] = probabilities[mask].sum() + recipe_history_size = self._history_size[mask].sum() + metrics[f"recipe_{recipe.name.lower()}_progress_rate"] = self._history_success_count[ + mask + ].sum().float() / recipe_history_size.clamp_min(1) + metrics[f"recipe_{recipe.name.lower()}_success_rate"] = self._final_successes[ + mask + ].sum().float() / recipe_attempts.clamp_min(1) + for recipe_name, variant_id, mask in self._diagnostic_variant_rows: + attempts = self._attempts[mask].sum() + history_size = self._history_size[mask].sum() + prefix = f"recipe_{recipe_name}_variant_{variant_id}" + metrics[f"{prefix}_probability"] = probabilities[mask].sum() + metrics[f"{prefix}_progress_rate"] = self._history_success_count[ + mask + ].sum().float() / history_size.clamp_min(1) + metrics[f"{prefix}_success_rate"] = self._final_successes[mask].sum().float() / attempts.clamp_min(1) + return metrics + + def get_state(self) -> dict[str, torch.Tensor]: + """Return curriculum evidence for checkpointing.""" + return { + "attempts": self._attempts.clone(), + "progress_successes": self._progress_successes.clone(), + "final_successes": self._final_successes.clone(), + "progress_history": self._progress_history.clone(), + "history_pointer": self._history_pointer.clone(), + "history_size": self._history_size.clone(), + "history_success_count": self._history_success_count.clone(), + "rolling_progress_rates": self._rolling_progress_rates.clone(), + } + + def set_state(self, state: dict[str, torch.Tensor]) -> None: + """Restore curriculum evidence from a checkpoint.""" + targets = { + "attempts": self._attempts, + "progress_successes": self._progress_successes, + "final_successes": self._final_successes, + "progress_history": self._progress_history, + "history_pointer": self._history_pointer, + "history_size": self._history_size, + "history_success_count": self._history_success_count, + "rolling_progress_rates": self._rolling_progress_rates, + } + for name, target in targets.items(): + if name not in state or state[name].shape != target.shape: + raise ValueError(f"Conveyor curriculum checkpoint has invalid '{name}'.") + history_len = self._progress_history.shape[1] + if bool(torch.any((state["history_pointer"] < 0) | (state["history_pointer"] >= history_len))): + raise ValueError("Conveyor curriculum checkpoint has invalid history pointers.") + if bool(torch.any((state["history_size"] < 0) | (state["history_size"] > history_len))): + raise ValueError("Conveyor curriculum checkpoint has invalid history sizes.") + if bool( + torch.any((state["history_success_count"] < 0) | (state["history_success_count"] > state["history_size"])) + ): + raise ValueError("Conveyor curriculum checkpoint has invalid rolling success counts.") + for name, target in targets.items(): + target.copy_(state[name].to(device=target.device, dtype=target.dtype)) diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/kinematics.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/kinematics.py new file mode 100644 index 000000000000..29ab17c9ce6f --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/kinematics.py @@ -0,0 +1,71 @@ +# 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-independent Franka tool-state helpers.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import torch + +from isaaclab.managers import SceneEntityCfg +from isaaclab.utils import math as math_utils + +if TYPE_CHECKING: + from isaaclab.assets import Articulation + from isaaclab.envs import ManagerBasedRLEnv + + +def _end_effector_cache_entry( + env: ManagerBasedRLEnv, + robot_cfg: SceneEntityCfg, + body_name: str, + body_offset: tuple[float, float, float], +) -> tuple[Articulation, int, torch.Tensor]: + """Resolve and cache the Franka hand body and tool-frame offset.""" + robot: Articulation = env.scene[robot_cfg.name] + cache = getattr(env, "_conveyor_end_effector_cache", None) + if cache is None: + cache = {} + env._conveyor_end_effector_cache = cache + key = (robot_cfg.name, body_name, body_offset) + entry = cache.get(key) + if entry is None: + body_ids, _ = robot.find_bodies(body_name) + if len(body_ids) != 1: + raise ValueError(f"Expected one end-effector body matching '{body_name}', found {len(body_ids)}.") + entry = (body_ids[0], torch.tensor(body_offset, dtype=torch.float32, device=env.device)) + cache[key] = entry + return robot, entry[0], entry[1] + + +def end_effector_pose( + env: ManagerBasedRLEnv, + robot_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + body_name: str = "panda_hand", + body_offset: tuple[float, float, float] = (0.0, 0.0, 0.1034), +) -> tuple[torch.Tensor, torch.Tensor]: + """Return the Franka tool-center position [m] and orientation.""" + robot, body_id, offset = _end_effector_cache_entry(env, robot_cfg, body_name, body_offset) + orientation = robot.data.body_quat_w.torch[:, body_id] + position = robot.data.body_pos_w.torch[:, body_id] + position = position + math_utils.quat_apply(orientation, offset.expand(env.num_envs, -1)) + return position, orientation + + +def tool_velocity( + env: ManagerBasedRLEnv, + robot_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + body_name: str = "panda_hand", + body_offset: tuple[float, float, float] = (0.0, 0.0, 0.1034), +) -> torch.Tensor: + """Return tool-center linear and angular velocity [m/s, rad/s].""" + robot, body_id, offset = _end_effector_cache_entry(env, robot_cfg, body_name, body_offset) + orientation = robot.data.body_quat_w.torch[:, body_id] + body_velocity = robot.data.body_vel_w.torch[:, body_id] + offset_world = math_utils.quat_apply(orientation, offset.expand(env.num_envs, -1)) + linear_velocity = body_velocity[:, :3] + torch.linalg.cross(body_velocity[:, 3:], offset_world) + return torch.cat((linear_velocity, body_velocity[:, 3:]), dim=1) diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/observations.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/observations.py new file mode 100644 index 000000000000..47edd45430b4 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/observations.py @@ -0,0 +1,152 @@ +# 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 + +"""Task-conditioned observations for conveyor transfer.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import torch + +from isaaclab.managers import SceneEntityCfg +from isaaclab.utils import math as math_utils + +from .kinematics import end_effector_pose, tool_velocity +from .reset_events import CUBE_COUNT, TRANSFER_X, side_inner_y + +if TYPE_CHECKING: + from isaaclab.assets import Articulation, RigidObject + from isaaclab.envs import ManagerBasedRLEnv + + +def _transfer_state(env: ManagerBasedRLEnv): + """Return initialized transfer state or raise a focused configuration error.""" + state = getattr(env, "conveyor_transfer_state", None) + if state is None: + raise AttributeError("Conveyor observations require ConveyorResetStateTable runtime state.") + return state + + +def _cube_assets(env: ManagerBasedRLEnv) -> tuple[RigidObject, ...]: + """Return the four cubes in stable identity order.""" + return tuple(env.scene[f"cube_{cube_id}"] for cube_id in range(CUBE_COUNT)) + + +def _cube_state(env: ManagerBasedRLEnv) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor]: + """Stack cube world positions, orientations, and spatial velocities.""" + cubes = _cube_assets(env) + return ( + torch.stack(tuple(cube.data.root_pos_w.torch for cube in cubes), dim=1), + torch.stack(tuple(cube.data.root_quat_w.torch for cube in cubes), dim=1), + torch.stack(tuple(cube.data.root_vel_w.torch for cube in cubes), dim=1), + ) + + +def _active_cube_values(values: torch.Tensor, target_cube_ids: torch.Tensor) -> torch.Tensor: + """Gather one cube row for every vectorized environment.""" + shape = (values.shape[0], 1, *values.shape[2:]) + index = target_cube_ids.view(values.shape[0], 1, *([1] * (values.ndim - 2))).expand(shape) + return torch.gather(values, 1, index).squeeze(1) + + +def target_cube_one_hot(env: ManagerBasedRLEnv) -> torch.Tensor: + """Encode which numbered cube the policy must transfer.""" + state = _transfer_state(env) + return torch.nn.functional.one_hot(state.target_cube_ids.long(), num_classes=CUBE_COUNT).float() + + +def target_side_one_hot(env: ManagerBasedRLEnv) -> torch.Tensor: + """Encode the destination conveyor, opposite the reset source side.""" + state = _transfer_state(env) + return torch.nn.functional.one_hot(1 - state.source_side_ids.long(), num_classes=2).float() + + +def classify_cube_conveyors(local_positions: torch.Tensor, transit_half_width: float = 0.14) -> torch.Tensor: + """Classify positions as left conveyor, in transit, or right conveyor.""" + if local_positions.shape[-1] != 3: + raise ValueError("Cube positions must end in xyz coordinates.") + side_ids = torch.full(local_positions.shape[:-1], 1, dtype=torch.long, device=local_positions.device) + side_ids[local_positions[..., 1] > transit_half_width] = 0 + side_ids[local_positions[..., 1] < -transit_half_width] = 2 + return torch.nn.functional.one_hot(side_ids, num_classes=3).float() + + +def cube_conveyor_state(env: ManagerBasedRLEnv) -> torch.Tensor: + """Return left/transit/right one-hot state for every numbered cube.""" + positions, _, _ = _cube_state(env) + local_positions = positions - env.scene.env_origins.unsqueeze(1) + return classify_cube_conveyors(local_positions).flatten(start_dim=1) + + +def transfer_object_observation(env: ManagerBasedRLEnv) -> torch.Tensor: + """Describe all four cubes in stable identity slots. + + The observation contains local positions, tool-relative positions, local + up axes, and linear/angular velocities. Cube identity does not change + during an episode; :func:`target_cube_one_hot` selects the active slot. + """ + positions, quaternions, velocities = _cube_state(env) + local_positions = positions - env.scene.env_origins.unsqueeze(1) + tool_position, _ = end_effector_pose(env) + tool_relative = positions - tool_position.unsqueeze(1) + rotations = math_utils.matrix_from_quat(quaternions.flatten(end_dim=1)).view(env.num_envs, CUBE_COUNT, 3, 3) + up_axes = rotations[..., 2] + return torch.cat( + ( + local_positions.flatten(start_dim=1), + tool_relative.flatten(start_dim=1), + up_axes.flatten(start_dim=1), + velocities.flatten(start_dim=1), + ), + dim=1, + ) + + +def active_transfer_features(env: ManagerBasedRLEnv) -> torch.Tensor: + """Return active-cube and destination-relative position features [m].""" + state = _transfer_state(env) + positions, _, _ = _cube_state(env) + active_position = _active_cube_values(positions, state.target_cube_ids.long()) + local_active_position = active_position - env.scene.env_origins + tool_position, _ = end_effector_pose(env) + target_side_ids = 1 - state.source_side_ids.long() + target_position = torch.stack( + ( + torch.full_like(target_side_ids, TRANSFER_X, dtype=active_position.dtype), + side_inner_y(target_side_ids), + torch.full_like(target_side_ids, 0.06, dtype=active_position.dtype), + ), + dim=1, + ) + return torch.cat( + ( + local_active_position, + active_position - tool_position, + target_position - local_active_position, + ), + dim=1, + ) + + +def gripper_joint_positions( + env: ManagerBasedRLEnv, + robot_cfg: SceneEntityCfg = SceneEntityCfg("robot", joint_names=["panda_finger_joint[1-2]"]), +) -> torch.Tensor: + """Return the two Franka finger positions [m].""" + robot: Articulation = env.scene[robot_cfg.name] + return robot.data.joint_pos.torch[:, robot_cfg.joint_ids] + + +def end_effector_axes(env: ManagerBasedRLEnv) -> torch.Tensor: + """Return continuous tool-frame x and z axes.""" + _, orientation = end_effector_pose(env) + rotation = math_utils.matrix_from_quat(orientation) + return torch.cat((rotation[:, :, 0], rotation[:, :, 2]), dim=1) + + +def end_effector_velocity(env: ManagerBasedRLEnv) -> torch.Tensor: + """Return tool-center linear and angular velocity [m/s, rad/s].""" + return tool_velocity(env) diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/reset_events.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/reset_events.py new file mode 100644 index 000000000000..84880be5e712 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/reset_events.py @@ -0,0 +1,444 @@ +# 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 + +"""Validated reset-state table for conveyor-to-conveyor transfer.""" + +from __future__ import annotations + +import math +from collections.abc import Sequence +from dataclasses import dataclass +from enum import IntEnum +from typing import TYPE_CHECKING + +import torch + +from isaaclab.managers import EventTermCfg, ManagerTermBase + +from ..conveyor_geometry import BELT_CENTER_Y, BELT_TOP_Z, BELT_TURN_RADIUS +from .state import create_transfer_state + +if TYPE_CHECKING: + from isaaclab.assets import Articulation, RigidObject + from isaaclab.envs import ManagerBasedRLEnv + + +CUBE_COUNT = 4 +CUBE_SIZE = 0.04 +CUBE_REST_Z = BELT_TOP_Z + 0.5 * CUBE_SIZE +TRANSFER_X = 0.52 +LEFT_SIDE = 0 +RIGHT_SIDE = 1 +_BELT_CURRICULUM_FRACTIONS = (0.0, 0.15, 0.30, 0.45, 0.60, 0.80, 1.0) +BELT_DEPLOYMENT_VARIANT = len(_BELT_CURRICULUM_FRACTIONS) - 1 + + +class ConveyorResetRecipe(IntEnum): + """Reset phases ordered from easiest to complete task start.""" + + GOAL = 0 + PLACE = 1 + CARRY = 2 + LIFT = 3 + GRASP = 4 + PREGRASP = 5 + BELT = 6 + + +@dataclass(frozen=True) +class ConveyorResetRow: + """One physical reset state and transfer command.""" + + recipe: ConveyorResetRecipe + variant_id: int + target_cube_id: int + source_side_id: int + arm_positions: tuple[float, ...] + held: bool + belt_range_fraction: float + + +_HOME_ARM = (0.0, -0.35, 0.0, -2.35, 0.0, 2.0, 0.78) + +# Constrained IK anchors target x=0.52 m with a downward-facing tool. Side 0 +# is the positive-y conveyor and side 1 is the negative-y conveyor. +_SOURCE_GRASP_ARM = ( + (0.5144946, 0.5427007, -0.0340481, -2.0731085, 0.0350549, 2.6153000, 1.2350098), + (-0.3400387, 0.5475255, -0.1330232, -2.0714802, 0.1368175, 2.6109921, 0.2080209), +) +_SOURCE_PREGRASP_ARM = ( + (0.5190744, 0.3742786, -0.0406565, -2.0872751, 0.0236238, 2.4611441, 1.2428656), + (-0.3220402, 0.3787346, -0.1588824, -2.0865274, 0.0928583, 2.4588233, 0.2380420), +) +_SOURCE_LIFT_ARM = ( + (0.5212608, 0.2372272, -0.0439391, -2.0514939, 0.0137043, 2.2884652, 1.2495379), + (-0.3132103, 0.2404478, -0.1720021, -2.0512362, 0.0540920, 2.2875542, 0.2640870), +) +_TARGET_PLACE_ARM = ( + # Source left, target right. + (-0.2921792, 0.4521663, -0.1853937, -2.0854943, 0.1398595, 2.5262992, 0.2062777), + # Source right, target left. + (0.4780173, 0.4442472, 0.0008774, -2.0873070, -0.0005178, 2.5315716, 1.2591928), +) +_CARRY_ARM = (0.0, 0.0137827, 0.0, -2.2661237, 0.0, 2.2798989, 0.78) + + +def _interpolate_arm( + start: tuple[float, ...], + end: tuple[float, ...], + fractions: tuple[float, ...], +) -> tuple[tuple[float, ...], ...]: + """Linearly interpolate validated joint-space anchors.""" + return tuple( + tuple( + start_value + fraction * (end_value - start_value) + for start_value, end_value in zip(start, end, strict=True) + ) + for fraction in fractions + ) + + +def _arm_position_variants( + recipe: ConveyorResetRecipe, + source_side_id: int, +) -> tuple[tuple[float, ...], ...]: + """Return dense reset states along the nominal transfer trajectory.""" + source_grasp = _SOURCE_GRASP_ARM[source_side_id] + source_pregrasp = _SOURCE_PREGRASP_ARM[source_side_id] + source_lift = _SOURCE_LIFT_ARM[source_side_id] + target_lift = _SOURCE_LIFT_ARM[1 - source_side_id] + if recipe == ConveyorResetRecipe.GOAL: + return (_SOURCE_PREGRASP_ARM[1 - source_side_id],) + if recipe == ConveyorResetRecipe.PLACE: + return _interpolate_arm(target_lift, _TARGET_PLACE_ARM[source_side_id], (0.25, 0.50, 0.75, 1.0)) + if recipe == ConveyorResetRecipe.CARRY: + return ( + *_interpolate_arm(source_lift, _CARRY_ARM, (0.33, 0.66, 1.0)), + *_interpolate_arm(_CARRY_ARM, target_lift, (0.33, 0.66, 1.0)), + ) + if recipe == ConveyorResetRecipe.LIFT: + return _interpolate_arm(source_grasp, source_lift, (0.25, 0.50, 0.75, 1.0)) + if recipe == ConveyorResetRecipe.GRASP: + return (source_grasp,) + if recipe == ConveyorResetRecipe.PREGRASP: + return _interpolate_arm(source_pregrasp, source_grasp, (0.0, 0.30, 0.55, 0.75, 0.88)) + if recipe == ConveyorResetRecipe.BELT: + return _interpolate_arm(source_pregrasp, _HOME_ARM, _BELT_CURRICULUM_FRACTIONS) + raise ValueError(f"Unsupported reset recipe: {recipe}.") + + +def reset_variant_counts() -> tuple[int, ...]: + """Return the number of trajectory variants in each reset recipe.""" + return tuple(len(_arm_position_variants(recipe, LEFT_SIDE)) for recipe in ConveyorResetRecipe) + + +def build_reset_rows() -> tuple[ConveyorResetRow, ...]: + """Build the complete identity, direction, and dense-trajectory cross product.""" + return tuple( + ConveyorResetRow( + recipe=recipe, + variant_id=variant_id, + target_cube_id=cube_id, + source_side_id=source_side, + arm_positions=arm_positions, + held=recipe + in ( + ConveyorResetRecipe.GRASP, + ConveyorResetRecipe.LIFT, + ConveyorResetRecipe.CARRY, + ConveyorResetRecipe.PLACE, + ), + belt_range_fraction=_BELT_CURRICULUM_FRACTIONS[variant_id] if recipe == ConveyorResetRecipe.BELT else 0.0, + ) + for recipe in ConveyorResetRecipe + for cube_id in range(CUBE_COUNT) + for source_side in (LEFT_SIDE, RIGHT_SIDE) + for variant_id, arm_positions in enumerate(_arm_position_variants(recipe, source_side)) + ) + + +_FRANKA_JOINT_ORIGINS = ( + ((0.0, 0.0, 0.333), (0.0, 0.0, 0.0)), + ((0.0, 0.0, 0.0), (-math.pi / 2.0, 0.0, 0.0)), + ((0.0, -0.316, 0.0), (math.pi / 2.0, 0.0, 0.0)), + ((0.0825, 0.0, 0.0), (math.pi / 2.0, 0.0, 0.0)), + ((-0.0825, 0.384, 0.0), (-math.pi / 2.0, 0.0, 0.0)), + ((0.0, 0.0, 0.0), (math.pi / 2.0, 0.0, 0.0)), + ((0.088, 0.0, 0.0), (math.pi / 2.0, 0.0, 0.0)), +) + + +def _rotation_matrix_from_rpy(roll: float, pitch: float, yaw: float, reference: torch.Tensor) -> torch.Tensor: + """Return a fixed XYZ roll-pitch-yaw rotation matrix.""" + cr, sr = math.cos(roll), math.sin(roll) + cp, sp = math.cos(pitch), math.sin(pitch) + cy, sy = math.cos(yaw), math.sin(yaw) + return reference.new_tensor( + ( + (cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr), + (sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr), + (-sp, cp * sr, cp * cr), + ) + ) + + +def franka_tool_position(joint_positions: torch.Tensor) -> torch.Tensor: + """Compute Panda tool-center positions [m] from seven joints [rad].""" + if joint_positions.shape[-1] != 7: + raise ValueError("Franka reset forward kinematics expects seven joint positions.") + shape = joint_positions.shape[:-1] + joints = joint_positions.reshape(-1, 7) + count = joints.shape[0] + rotation = torch.eye(3, dtype=joints.dtype, device=joints.device).expand(count, -1, -1).clone() + position = torch.zeros((count, 3), dtype=joints.dtype, device=joints.device) + reference = joints[0] if count else joint_positions.new_zeros(7) + for joint_id, (origin_position, origin_rpy) in enumerate(_FRANKA_JOINT_ORIGINS): + origin = joint_positions.new_tensor(origin_position).expand(count, -1) + position += torch.bmm(rotation, origin.unsqueeze(-1)).squeeze(-1) + rotation = torch.matmul(rotation, _rotation_matrix_from_rpy(*origin_rpy, reference=reference)) + angle = joints[:, joint_id] + cosine, sine = torch.cos(angle), torch.sin(angle) + zeros, ones = torch.zeros_like(angle), torch.ones_like(angle) + joint_rotation = torch.stack( + ( + torch.stack((cosine, -sine, zeros), dim=1), + torch.stack((sine, cosine, zeros), dim=1), + torch.stack((zeros, zeros, ones), dim=1), + ), + dim=1, + ) + rotation = torch.bmm(rotation, joint_rotation) + tool_offset = joint_positions.new_tensor((0.0, 0.0, 0.2104)).expand(count, -1) + position += torch.bmm(rotation, tool_offset.unsqueeze(-1)).squeeze(-1) + return position.reshape(*shape, 3) + + +def side_inner_y(side_ids: torch.Tensor) -> torch.Tensor: + """Return the reachable inner-straight y coordinate [m] for each side.""" + magnitude = BELT_CENTER_Y - BELT_TURN_RADIUS + return torch.where(side_ids == LEFT_SIDE, magnitude, -magnitude) + + +def _sample_collision_free_active_x( + base_x: torch.Tensor, + cube_sides: torch.Tensor, + target_cube_ids: torch.Tensor, + source_side_ids: torch.Tensor, + lower: torch.Tensor, + upper: torch.Tensor, + minimum_separation: float = 0.055, + attempts: int = 16, +) -> torch.Tensor: + """Sample active-cube positions without overlapping an inactive cube.""" + if base_x.ndim != 2 or base_x.shape[1] != CUBE_COUNT or cube_sides.shape != base_x.shape: + raise ValueError("base_x and cube_sides must have shape (N, CUBE_COUNT).") + count = base_x.shape[0] + expected_vector_shape = (count,) + if any(value.shape != expected_vector_shape for value in (target_cube_ids, source_side_ids, lower, upper)): + raise ValueError("Active-cube sampling controls must have shape (N,).") + if minimum_separation <= 0.0 or attempts < 1: + raise ValueError("Invalid collision-free active-cube sampling parameters.") + + cube_ids = torch.arange(CUBE_COUNT, device=base_x.device).expand(count, -1) + inactive_on_source = (cube_sides == source_side_ids.unsqueeze(1)) & (cube_ids != target_cube_ids.unsqueeze(1)) + sampled = lower + torch.rand_like(lower) * (upper - lower) + for _ in range(attempts): + conflicts = torch.any( + (torch.abs(sampled.unsqueeze(1) - base_x) < minimum_separation) & inactive_on_source, dim=1 + ) + replacement = lower + torch.rand_like(lower) * (upper - lower) + sampled = torch.where(conflicts, replacement, sampled) + + conflicts = torch.any((torch.abs(sampled.unsqueeze(1) - base_x) < minimum_separation) & inactive_on_source, dim=1) + fallback = torch.full_like(sampled, TRANSFER_X) + return torch.where(conflicts, fallback, sampled) + + +class ConveyorResetStateTable(ManagerTermBase): + """Restore validated transfer states spanning released goal to moving start.""" + + def __init__(self, cfg: EventTermCfg, env: ManagerBasedRLEnv): + super().__init__(cfg, env) + self._rows = build_reset_rows() + self.recipe_ids = torch.tensor([row.recipe for row in self._rows], dtype=torch.long, device=env.device) + self.target_cube_ids = torch.tensor( + [row.target_cube_id for row in self._rows], dtype=torch.long, device=env.device + ) + self.variant_ids = torch.tensor([row.variant_id for row in self._rows], dtype=torch.long, device=env.device) + self.source_side_ids = torch.tensor( + [row.source_side_id for row in self._rows], dtype=torch.long, device=env.device + ) + self._arm_positions = torch.tensor( + [row.arm_positions for row in self._rows], dtype=torch.float32, device=env.device + ) + self._held_rows = torch.tensor([row.held for row in self._rows], dtype=torch.bool, device=env.device) + self._belt_range_fractions = torch.tensor( + [row.belt_range_fraction for row in self._rows], dtype=torch.float32, device=env.device + ) + self._robot: Articulation = env.scene["robot"] + self._cubes: tuple[RigidObject, ...] = tuple(env.scene[f"cube_{cube_id}"] for cube_id in range(CUBE_COUNT)) + self._arm_joint_ids = self._robot.find_joints("panda_joint[1-7]", preserve_order=True)[0] + self._finger_joint_ids = self._robot.find_joints("panda_finger_joint[1-2]", preserve_order=True)[0] + if len(self._arm_joint_ids) != 7 or len(self._finger_joint_ids) != 2: + raise ValueError("Conveyor transfer requires seven Panda arm joints and two finger joints.") + self._state = create_transfer_state(env, self.row_count) + + @property + def row_count(self) -> int: + """Number of immutable physical reset rows.""" + return len(self._rows) + + @property + def recipe_names(self) -> tuple[str, ...]: + """Stable reset recipe labels.""" + return tuple(recipe.name.lower() for recipe in ConveyorResetRecipe) + + def _filtered_rows( + self, + fixed_recipe: int | None, + fixed_variant_id: int | None, + fixed_target_cube_id: int | None, + fixed_source_side_id: int | None, + ) -> torch.Tensor: + """Return rows matching optional deterministic evaluation controls.""" + mask = torch.ones(self.row_count, dtype=torch.bool, device=self.device) + if fixed_recipe is not None: + if not 0 <= fixed_recipe < len(ConveyorResetRecipe): + raise ValueError(f"fixed_recipe must lie in [0, {len(ConveyorResetRecipe) - 1}].") + mask &= self.recipe_ids == fixed_recipe + if fixed_variant_id is not None: + if fixed_variant_id < 0: + raise ValueError("fixed_variant_id must be non-negative.") + mask &= self.variant_ids == fixed_variant_id + if fixed_target_cube_id is not None: + if not 0 <= fixed_target_cube_id < CUBE_COUNT: + raise ValueError(f"fixed_target_cube_id must lie in [0, {CUBE_COUNT - 1}].") + mask &= self.target_cube_ids == fixed_target_cube_id + if fixed_source_side_id is not None: + if fixed_source_side_id not in (LEFT_SIDE, RIGHT_SIDE): + raise ValueError("fixed_source_side_id must be 0 (left) or 1 (right).") + mask &= self.source_side_ids == fixed_source_side_id + return torch.nonzero(mask, as_tuple=False).flatten() + + def __call__( + self, + env: ManagerBasedRLEnv, + env_ids: torch.Tensor, + fixed_recipe: int | None = None, + fixed_variant_id: int | None = None, + fixed_target_cube_id: int | None = None, + fixed_source_side_id: int | None = None, + belt_start_x_range: tuple[float, float] = (0.30, 0.82), + cube_position_noise: float = 0.015, + arm_joint_noise: float = 0.015, + ) -> None: + """Write sampled robot and four-cube states directly into simulation.""" + if env_ids is None or env_ids.numel() == 0: + return + if belt_start_x_range[0] >= belt_start_x_range[1]: + raise ValueError("belt_start_x_range must be strictly increasing.") + if cube_position_noise < 0.0 or arm_joint_noise < 0.0: + raise ValueError("Reset randomization ranges must be non-negative.") + + if ( + fixed_recipe is None + and fixed_variant_id is None + and fixed_target_cube_id is None + and fixed_source_side_id is None + ): + row_ids = self._state.row_ids[env_ids] + else: + candidates = self._filtered_rows( + fixed_recipe, + fixed_variant_id, + fixed_target_cube_id, + fixed_source_side_id, + ) + if candidates.numel() == 0: + raise RuntimeError("No conveyor reset rows match the fixed reset controls.") + row_ids = candidates[torch.randint(candidates.numel(), (env_ids.numel(),), device=self.device)] + self._state.row_ids[env_ids] = row_ids + + recipes = self.recipe_ids[row_ids] + target_cube_ids = self.target_cube_ids[row_ids] + source_side_ids = self.source_side_ids[row_ids] + held_rows = self._held_rows[row_ids] + self._state.recipe_ids[env_ids] = recipes + self._state.target_cube_ids[env_ids] = target_cube_ids + self._state.source_side_ids[env_ids] = source_side_ids + self._state.held_cube_ids[env_ids] = torch.where(held_rows, target_cube_ids, -1) + self._state.initialized[env_ids] = True + + arm_positions = self._arm_positions[row_ids].clone() + if arm_joint_noise > 0.0: + noise = (2.0 * torch.rand_like(arm_positions) - 1.0) * arm_joint_noise + noise[held_rows] = 0.0 + arm_positions += noise + joint_positions = self._robot.data.default_joint_pos.torch[env_ids].clone() + joint_velocities = torch.zeros_like(joint_positions) + joint_positions[:, self._arm_joint_ids] = arm_positions + finger_positions = torch.full((env_ids.numel(), 2), 0.04, dtype=joint_positions.dtype, device=self.device) + finger_positions[held_rows] = 0.019 + joint_positions[:, self._finger_joint_ids] = finger_positions + self._robot.set_joint_position_target_index(target=joint_positions, env_ids=env_ids) + self._robot.set_joint_velocity_target_index(target=joint_velocities, env_ids=env_ids) + self._robot.write_joint_position_to_sim_index(position=joint_positions, env_ids=env_ids) + self._robot.write_joint_velocity_to_sim_index(velocity=joint_velocities, env_ids=env_ids) + + count = env_ids.numel() + base_x = arm_positions.new_tensor((0.26, 0.42, 0.72, 0.88)).expand(count, -1).clone() + base_sides = torch.tensor((LEFT_SIDE, LEFT_SIDE, RIGHT_SIDE, RIGHT_SIDE), device=self.device) + cube_sides = base_sides.expand(count, -1).clone() + cube_sides.scatter_(1, target_cube_ids.unsqueeze(1), source_side_ids.unsqueeze(1)) + if cube_position_noise > 0.0: + base_x += (2.0 * torch.rand_like(base_x) - 1.0) * cube_position_noise + cube_y = side_inner_y(cube_sides) + cube_positions = torch.stack( + (base_x, cube_y, torch.full_like(base_x, CUBE_REST_Z)), + dim=2, + ) + + active_lower = torch.full((count,), TRANSFER_X, dtype=arm_positions.dtype, device=self.device) + active_upper = active_lower.clone() + belt_rows = recipes == int(ConveyorResetRecipe.BELT) + if bool(torch.any(belt_rows)): + range_fraction = self._belt_range_fractions[row_ids] + range_lower = TRANSFER_X + range_fraction * (belt_start_x_range[0] - TRANSFER_X) + range_upper = TRANSFER_X + range_fraction * (belt_start_x_range[1] - TRANSFER_X) + active_lower[belt_rows] = range_lower[belt_rows] + active_upper[belt_rows] = range_upper[belt_rows] + active_x = _sample_collision_free_active_x( + base_x, + cube_sides, + target_cube_ids, + source_side_ids, + active_lower, + active_upper, + ) + active_positions = torch.stack( + (active_x, side_inner_y(source_side_ids), torch.full_like(active_x, CUBE_REST_Z)), + dim=1, + ) + goal_rows = recipes == int(ConveyorResetRecipe.GOAL) + active_positions[goal_rows, 1] = side_inner_y(1 - source_side_ids[goal_rows]) + if bool(torch.any(held_rows)): + active_positions[held_rows] = franka_tool_position(arm_positions[held_rows]) + cube_positions.scatter_( + 1, + target_cube_ids.view(-1, 1, 1).expand(-1, 1, 3), + active_positions.unsqueeze(1), + ) + + identity_quaternion = arm_positions.new_tensor((0.0, 0.0, 0.0, 1.0)).expand(count, -1) + for cube_id, cube in enumerate(self._cubes): + root_pose = cube.data.default_root_pose.torch[env_ids].clone() + root_pose[:, :3] = cube_positions[:, cube_id] + env.scene.env_origins[env_ids] + root_pose[:, 3:7] = identity_quaternion + root_velocity = torch.zeros((count, 6), dtype=root_pose.dtype, device=self.device) + cube.write_root_pose_to_sim_index(root_pose=root_pose, env_ids=env_ids) + cube.write_root_velocity_to_sim_index(root_velocity=root_velocity, env_ids=env_ids) + + def reset(self, env_ids: Sequence[int] | None = None) -> None: + """Keep the immutable reset table across environment resets.""" diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/rewards.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/rewards.py new file mode 100644 index 000000000000..77aa7bc68dad --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/rewards.py @@ -0,0 +1,123 @@ +# 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 + +"""Dense progress and sparse completion rewards for conveyor transfer.""" + +from __future__ import annotations + +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import torch + +from isaaclab.managers import ManagerTermBase, RewardTermCfg, SceneEntityCfg + +from .kinematics import end_effector_pose +from .reset_events import CUBE_COUNT, CUBE_REST_Z, TRANSFER_X, side_inner_y + +if TYPE_CHECKING: + from isaaclab.assets import Articulation, RigidObject + from isaaclab.envs import ManagerBasedRLEnv + + +def transfer_potential( + cube_positions: torch.Tensor, + tool_positions: torch.Tensor, + finger_positions: torch.Tensor, + source_side_ids: torch.Tensor, +) -> torch.Tensor: + """Return a monotonic pickup-to-release shaping potential.""" + source_y = side_inner_y(source_side_ids) + target_y = side_inner_y(1 - source_side_ids) + target_position = torch.stack( + ( + torch.full_like(source_y, TRANSFER_X), + target_y, + torch.full_like(source_y, CUBE_REST_Z), + ), + dim=1, + ) + tool_distance = torch.linalg.vector_norm(tool_positions - cube_positions, dim=1) + reach = torch.exp(-12.0 * tool_distance) + gripper_closure = torch.clamp((0.04 - torch.amin(finger_positions, dim=1)) / 0.021, min=0.0, max=1.0) + grasp = gripper_closure * torch.exp(-25.0 * tool_distance) + lift = torch.clamp((cube_positions[:, 2] - CUBE_REST_Z) / 0.14, min=0.0, max=1.0) + direction_denominator = (target_y - source_y).clamp(min=-1.0, max=1.0) + crossing = (cube_positions[:, 1] - source_y) / direction_denominator + crossing = torch.clamp(crossing, min=0.0, max=1.0) + target_distance = torch.linalg.vector_norm(cube_positions - target_position, dim=1) + target = torch.exp(-14.0 * target_distance) + transport = crossing * torch.maximum(torch.clamp(2.0 * lift, max=1.0), target) + released = (torch.amin(finger_positions, dim=1) > 0.027).float() * target + return 0.5 * reach + 0.75 * grasp + 1.25 * lift + 2.0 * transport + 2.0 * target + released + + +def current_transfer_potential(env: ManagerBasedRLEnv) -> torch.Tensor: + """Gather current task state and evaluate the shaping potential.""" + state = env.conveyor_transfer_state + cubes: tuple[RigidObject, ...] = tuple(env.scene[f"cube_{cube_id}"] for cube_id in range(CUBE_COUNT)) + positions = torch.stack(tuple(cube.data.root_pos_w.torch for cube in cubes), dim=1) + index = state.target_cube_ids.view(env.num_envs, 1, 1).expand(-1, 1, 3) + active_position = torch.gather(positions, 1, index).squeeze(1) - env.scene.env_origins + tool_position, _ = end_effector_pose(env) + tool_position = tool_position - env.scene.env_origins + robot: Articulation = env.scene["robot"] + finger_ids, _ = robot.find_joints("panda_finger_joint[1-2]", preserve_order=True) + finger_positions = robot.data.joint_pos.torch[:, finger_ids] + return transfer_potential(active_position, tool_position, finger_positions, state.source_side_ids) + + +class ConveyorTransferProgressReward(ManagerTermBase): + """Reward positive changes in a phase-aware transfer potential.""" + + def __init__(self, cfg: RewardTermCfg, env: ManagerBasedRLEnv): + super().__init__(cfg, env) + self._previous = torch.zeros(env.num_envs, dtype=torch.float32, device=env.device) + + def __call__(self, env: ManagerBasedRLEnv) -> torch.Tensor: + """Return per-step potential improvement.""" + current = current_transfer_potential(env) + improvement = current - self._previous + self._previous.copy_(current) + return improvement + + def reset(self, env_ids: Sequence[int] | None = None) -> None: + """Anchor shaping to the newly sampled reset state.""" + current = current_transfer_potential(self._env) + if env_ids is None: + self._previous.copy_(current) + else: + self._previous[env_ids] = current[env_ids] + + +def transfer_success_reward(env: ManagerBasedRLEnv, termination_name: str = "success") -> torch.Tensor: + """Return one on the transfer-completion transition.""" + return env.termination_manager.get_term(termination_name).float() + + +def terminal_failure(env: ManagerBasedRLEnv, success_termination_name: str = "success") -> torch.Tensor: + """Return one for non-timeout terminal failures.""" + succeeded = env.termination_manager.get_term(success_termination_name) + return (env.reset_terminated & ~succeeded).float() + + +def action_term_l2(env: ManagerBasedRLEnv, action_name: str) -> torch.Tensor: + """Penalize one named raw action term.""" + action = env.action_manager.get_term(action_name).raw_actions + return torch.sum(torch.square(action), dim=1) + + +def finite_joint_velocity_l2( + env: ManagerBasedRLEnv, + asset_cfg: SceneEntityCfg = SceneEntityCfg("robot", joint_names=["panda_joint[1-7]"]), + maximum_velocity: float = 3.0, +) -> torch.Tensor: + """Penalize bounded arm velocity while sanitizing divergent states.""" + if maximum_velocity <= 0.0: + raise ValueError("maximum_velocity must be positive.") + robot: Articulation = env.scene[asset_cfg.name] + velocity = robot.data.joint_vel.torch[:, asset_cfg.joint_ids] + velocity = torch.nan_to_num(velocity, nan=0.0, posinf=maximum_velocity, neginf=-maximum_velocity) + return torch.sum(torch.square(torch.clamp(velocity, -maximum_velocity, maximum_velocity)), dim=1) diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/state.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/state.py new file mode 100644 index 000000000000..4d153aa81e78 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/state.py @@ -0,0 +1,42 @@ +# 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 + +"""Typed runtime state shared by conveyor-transfer MDP terms.""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import TYPE_CHECKING + +import torch + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +@dataclass +class ConveyorTransferState: + """Episode-local transfer command and reset metadata.""" + + row_ids: torch.Tensor + recipe_ids: torch.Tensor + target_cube_ids: torch.Tensor + source_side_ids: torch.Tensor + held_cube_ids: torch.Tensor + initialized: torch.Tensor + + +def create_transfer_state(env: ManagerBasedRLEnv, row_count: int) -> ConveyorTransferState: + """Create and attach the environment's transfer-state owner.""" + state = ConveyorTransferState( + row_ids=torch.randint(row_count, (env.num_envs,), dtype=torch.long, device=env.device), + recipe_ids=torch.zeros(env.num_envs, dtype=torch.long, device=env.device), + target_cube_ids=torch.zeros(env.num_envs, dtype=torch.long, device=env.device), + source_side_ids=torch.zeros(env.num_envs, dtype=torch.long, device=env.device), + held_cube_ids=torch.full((env.num_envs,), -1, dtype=torch.long, device=env.device), + initialized=torch.zeros(env.num_envs, dtype=torch.bool, device=env.device), + ) + env.conveyor_transfer_state = state + return state diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/terminations.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/terminations.py new file mode 100644 index 000000000000..280a1d823dc3 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/terminations.py @@ -0,0 +1,180 @@ +# 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 + +"""Termination terms for conveyor transfer.""" + +from __future__ import annotations + +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import torch + +from isaaclab.managers import ManagerTermBase, SceneEntityCfg, TerminationTermCfg + +from ..conveyor_geometry import BELT_CENTER_X, BELT_HALF_STRAIGHT +from .kinematics import end_effector_pose +from .reset_events import CUBE_COUNT, side_inner_y +from .rewards import current_transfer_potential + +if TYPE_CHECKING: + from isaaclab.assets import Articulation, RigidObject + from isaaclab.envs import ManagerBasedRLEnv + + +def transfer_success_mask( + cube_positions: torch.Tensor, + cube_linear_velocities: torch.Tensor, + tool_positions: torch.Tensor, + finger_positions: torch.Tensor, + target_side_ids: torch.Tensor, + lateral_tolerance: float = 0.055, + maximum_cube_speed: float = 0.65, + minimum_finger_position: float = 0.027, + minimum_tool_clearance: float = 0.055, +) -> torch.Tensor: + """Return whether the active cube is released on its destination belt.""" + target_y = side_inner_y(target_side_ids) + on_straight = torch.abs(cube_positions[:, 0] - BELT_CENTER_X) < BELT_HALF_STRAIGHT + on_lane = torch.abs(cube_positions[:, 1] - target_y) < lateral_tolerance + supported_height = (cube_positions[:, 2] > 0.045) & (cube_positions[:, 2] < 0.095) + moving_safely = torch.linalg.vector_norm(cube_linear_velocities, dim=1) < maximum_cube_speed + released = torch.amin(finger_positions, dim=1) > minimum_finger_position + hand_clear = torch.linalg.vector_norm(tool_positions - cube_positions, dim=1) > minimum_tool_clearance + return on_straight & on_lane & supported_height & moving_safely & released & hand_clear + + +class StableConveyorTransfer(ManagerTermBase): + """Require a released destination-belt placement for consecutive steps.""" + + def __init__(self, cfg: TerminationTermCfg, env: ManagerBasedRLEnv): + super().__init__(cfg, env) + self._stable_steps = torch.zeros(env.num_envs, dtype=torch.long, device=env.device) + self.ever_success = torch.zeros(env.num_envs, dtype=torch.bool, device=env.device) + + def __call__( + self, + env: ManagerBasedRLEnv, + minimum_episode_steps: int = 2, + hold_steps: int = 3, + lateral_tolerance: float = 0.055, + maximum_cube_speed: float = 0.65, + minimum_finger_position: float = 0.027, + minimum_tool_clearance: float = 0.055, + ) -> torch.Tensor: + """Return stable transfer success for each environment.""" + if minimum_episode_steps < 0 or hold_steps < 1: + raise ValueError("minimum_episode_steps must be non-negative and hold_steps must be positive.") + state = env.conveyor_transfer_state + cubes: tuple[RigidObject, ...] = tuple(env.scene[f"cube_{cube_id}"] for cube_id in range(CUBE_COUNT)) + positions = torch.stack(tuple(cube.data.root_pos_w.torch for cube in cubes), dim=1) + velocities = torch.stack(tuple(cube.data.root_lin_vel_w.torch for cube in cubes), dim=1) + index = state.target_cube_ids.view(env.num_envs, 1, 1).expand(-1, 1, 3) + active_position = torch.gather(positions, 1, index).squeeze(1) - env.scene.env_origins + active_velocity = torch.gather(velocities, 1, index).squeeze(1) + tool_position, _ = end_effector_pose(env) + tool_position = tool_position - env.scene.env_origins + robot: Articulation = env.scene["robot"] + finger_ids, _ = robot.find_joints("panda_finger_joint[1-2]", preserve_order=True) + finger_positions = robot.data.joint_pos.torch[:, finger_ids] + successful = transfer_success_mask( + active_position, + active_velocity, + tool_position, + finger_positions, + 1 - state.source_side_ids, + lateral_tolerance=lateral_tolerance, + maximum_cube_speed=maximum_cube_speed, + minimum_finger_position=minimum_finger_position, + minimum_tool_clearance=minimum_tool_clearance, + ) + successful &= env.episode_length_buf >= minimum_episode_steps + self._stable_steps = torch.where(successful, self._stable_steps + 1, torch.zeros_like(self._stable_steps)) + stable = self._stable_steps >= hold_steps + self.ever_success |= stable + return stable + + def reset(self, env_ids: Sequence[int] | None = None) -> None: + """Clear success history for selected environments.""" + if env_ids is None: + env_ids = slice(None) + self._stable_steps[env_ids] = 0 + self.ever_success[env_ids] = False + + +class ConveyorResetLearningProgress(ManagerTermBase): + """Track row-relative progress without terminating the episode. + + The adaptive reset sampler needs useful evidence before complete transfers + are common. Each reset row therefore asks the policy to increase the same + transfer potential used for dense shaping by a fixed amount. Episodes keep + running toward strict released placement; this context only records which + rows have advanced meaningfully. + """ + + def __init__(self, cfg: TerminationTermCfg, env: ManagerBasedRLEnv): + super().__init__(cfg, env) + self._target_potential = torch.zeros(env.num_envs, dtype=torch.float32, device=env.device) + self.is_success = torch.zeros(env.num_envs, dtype=torch.bool, device=env.device) + self.new_success = torch.zeros_like(self.is_success) + self.ever_success = torch.zeros_like(self.is_success) + self._no_termination = torch.zeros_like(self.is_success) + + def __call__( + self, + env: ManagerBasedRLEnv, + minimum_episode_steps: int = 3, + minimum_progress: float = 0.35, + maximum_target_potential: float = 5.0, + ) -> torch.Tensor: + """Update sticky row-progress evidence and return an all-false mask.""" + if minimum_episode_steps < 0 or minimum_progress <= 0.0 or maximum_target_potential <= 0.0: + raise ValueError("Invalid conveyor reset-learning progress thresholds.") + current = current_transfer_potential(env) + reached = (current >= self._target_potential) & (env.episode_length_buf >= minimum_episode_steps) + self.is_success.copy_(reached) + self.new_success.copy_(reached & ~self.ever_success) + self.ever_success |= reached + return self._no_termination + + def reset(self, env_ids: Sequence[int] | None = None) -> None: + """Set a meaningful potential target from each newly sampled row.""" + if env_ids is None: + env_ids = slice(None) + initial = current_transfer_potential(self._env) + minimum_progress = float(self.cfg.params.get("minimum_progress", 0.35)) + maximum_target = float(self.cfg.params.get("maximum_target_potential", 5.0)) + self._target_potential[env_ids] = torch.clamp_max(initial[env_ids] + minimum_progress, maximum_target) + self.is_success[env_ids] = False + self.new_success[env_ids] = False + self.ever_success[env_ids] = False + + +def cube_out_of_workspace( + env: ManagerBasedRLEnv, + minimum: tuple[float, float, float] = (-0.10, -1.05, -0.05), + maximum: tuple[float, float, float] = (1.30, 1.05, 0.80), +) -> torch.Tensor: + """Terminate when any cube leaves the recoverable workspace.""" + cubes: tuple[RigidObject, ...] = tuple(env.scene[f"cube_{cube_id}"] for cube_id in range(CUBE_COUNT)) + positions = torch.stack(tuple(cube.data.root_pos_w.torch for cube in cubes), dim=1) + positions -= env.scene.env_origins.unsqueeze(1) + lower = positions.new_tensor(minimum) + upper = positions.new_tensor(maximum) + return torch.any((positions < lower) | (positions > upper), dim=(1, 2)) + + +def nonfinite_scene_state( + env: ManagerBasedRLEnv, + robot_cfg: SceneEntityCfg = SceneEntityCfg("robot"), +) -> torch.Tensor: + """Terminate environments containing nonfinite robot or cube state.""" + robot: Articulation = env.scene[robot_cfg.name] + invalid = ~torch.all(torch.isfinite(robot.data.joint_pos.torch), dim=1) + invalid |= ~torch.all(torch.isfinite(robot.data.joint_vel.torch), dim=1) + for cube_id in range(CUBE_COUNT): + cube: RigidObject = env.scene[f"cube_{cube_id}"] + invalid |= ~torch.all(torch.isfinite(cube.data.root_state_w.torch), dim=1) + return invalid diff --git a/source/isaaclab_tasks/test/contrib/test_conveyor_franka_mdp.py b/source/isaaclab_tasks/test/contrib/test_conveyor_franka_mdp.py new file mode 100644 index 000000000000..9ce735a0e53e --- /dev/null +++ b/source/isaaclab_tasks/test/contrib/test_conveyor_franka_mdp.py @@ -0,0 +1,289 @@ +# 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 + +"""Unit tests for conveyor-transfer state, curriculum, and success geometry.""" + +from collections import Counter + +import torch +from isaaclab_newton.sim.schemas import MujocoCollisionCfg, NewtonMaterialPropertiesCfg + +from isaaclab_tasks.contrib.conveyor_franka.agents.rsl_rl_ppo_cfg import ( + ConveyorGaussianBernoulliDistribution, +) +from isaaclab_tasks.contrib.conveyor_franka.conveyor_franka_env_cfg import ConveyorFrankaEnvCfg +from isaaclab_tasks.contrib.conveyor_franka.mdp.curriculums import ( + _ring_append_bool_count_rate, + reset_sampling_probabilities, +) +from isaaclab_tasks.contrib.conveyor_franka.mdp.observations import classify_cube_conveyors +from isaaclab_tasks.contrib.conveyor_franka.mdp.reset_events import ( + CUBE_COUNT, + ConveyorResetRecipe, + _sample_collision_free_active_x, + build_reset_rows, + franka_tool_position, + reset_variant_counts, +) +from isaaclab_tasks.contrib.conveyor_franka.mdp.rewards import transfer_potential +from isaaclab_tasks.contrib.conveyor_franka.mdp.terminations import transfer_success_mask + + +def test_contact_and_drive_cfg_preserve_transport_and_grasp_friction(): + """Belt contact precedence must not weaken the cube's grasp material.""" + cfg = ConveyorFrankaEnvCfg() + belt_collision = cfg.scene.conveyor_left_belt.spawn.collision_props + belt_mujoco = next(fragment for fragment in belt_collision if isinstance(fragment, MujocoCollisionCfg)) + cube_material = cfg.scene.cube_0.spawn.physics_material + hand_actuator = cfg.scene.robot.actuators["panda_hand"] + + assert belt_mujoco.priority == 1 + assert isinstance(cube_material, NewtonMaterialPropertiesCfg) + assert cube_material.dynamic_friction == 0.6 + assert cube_material.contact_stiffness == 1.0e4 + assert cube_material.contact_damping == 200.0 + assert hand_actuator.stiffness == 350.0 + assert hand_actuator.damping == 10.0 + assert cfg.actions.arm_action.gravity_compensation + assert cfg.sim.physics.collision_decimation == 1 + + +def test_reset_rows_cover_every_cube_direction_and_phase_once(): + """The reset bank is the complete command and physical-phase cross product.""" + rows = build_reset_rows() + + assert len(rows) == sum(reset_variant_counts()) * CUBE_COUNT * 2 + assert Counter((row.recipe, row.variant_id, row.target_cube_id, row.source_side_id) for row in rows) == Counter( + (recipe, variant_id, cube_id, side_id) + for recipe in ConveyorResetRecipe + for variant_id in range(reset_variant_counts()[int(recipe)]) + for cube_id in range(CUBE_COUNT) + for side_id in range(2) + ) + held_recipes = { + ConveyorResetRecipe.GRASP, + ConveyorResetRecipe.LIFT, + ConveyorResetRecipe.CARRY, + ConveyorResetRecipe.PLACE, + } + assert all(row.held == (row.recipe in held_recipes) for row in rows) + + +def test_reset_arm_anchors_reach_expected_transfer_waypoints(): + """IK anchors place the tool over source, transit, or destination waypoints.""" + anchor_variants = { + ConveyorResetRecipe.GOAL: 0, + ConveyorResetRecipe.PLACE: 3, + ConveyorResetRecipe.CARRY: 2, + ConveyorResetRecipe.LIFT: 3, + ConveyorResetRecipe.GRASP: 0, + ConveyorResetRecipe.PREGRASP: 0, + } + rows = [ + row + for row in build_reset_rows() + if row.target_cube_id == 0 and anchor_variants.get(row.recipe) == row.variant_id + ] + joints = torch.tensor([row.arm_positions for row in rows], dtype=torch.float64) + positions = franka_tool_position(joints) + + for row, position in zip(rows, positions, strict=True): + source_y = 0.27 if row.source_side_id == 0 else -0.27 + target_y = -source_y + if row.recipe == ConveyorResetRecipe.BELT: + continue + expected = { + ConveyorResetRecipe.PREGRASP: (0.52, source_y, 0.14), + ConveyorResetRecipe.GRASP: (0.52, source_y, 0.06), + ConveyorResetRecipe.LIFT: (0.52, source_y, 0.22), + ConveyorResetRecipe.CARRY: (0.52, 0.0, 0.25), + ConveyorResetRecipe.PLACE: (0.52, target_y, 0.105), + ConveyorResetRecipe.GOAL: (0.52, target_y, 0.14), + }[row.recipe] + torch.testing.assert_close(position, torch.tensor(expected, dtype=position.dtype), atol=3.0e-4, rtol=0.0) + + +def test_cube_conveyor_state_has_stable_three_way_encoding(): + """Cube side observations distinguish both belts from the transfer corridor.""" + positions = torch.tensor([[[0.5, 0.27, 0.06], [0.5, 0.0, 0.20], [0.5, -0.27, 0.06]]]) + + encoded = classify_cube_conveyors(positions) + + torch.testing.assert_close( + encoded, + torch.tensor([[[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]]), + ) + + +def test_released_goal_state_succeeds_only_on_commanded_conveyor(): + """The same physical cube placement is successful for exactly one direction.""" + cube_positions = torch.tensor([[0.58, -0.27, 0.06], [0.58, -0.27, 0.06]]) + cube_velocities = torch.zeros((2, 3)) + tool_positions = torch.tensor([[0.58, -0.27, 0.14], [0.58, -0.27, 0.14]]) + finger_positions = torch.full((2, 2), 0.04) + target_side_ids = torch.tensor([1, 0]) + + successful = transfer_success_mask( + cube_positions, + cube_velocities, + tool_positions, + finger_positions, + target_side_ids, + ) + + assert successful.tolist() == [True, False] + + +def test_transfer_potential_increases_through_release(): + """Dense shaping must not punish lowering and releasing at the destination.""" + source_side = torch.zeros(5, dtype=torch.long) + cube_positions = torch.tensor( + [ + [0.52, 0.27, 0.06], + [0.52, 0.27, 0.20], + [0.52, 0.00, 0.20], + [0.52, -0.27, 0.105], + [0.52, -0.27, 0.06], + ] + ) + tool_positions = cube_positions.clone() + tool_positions[-1, 2] += 0.10 + finger_positions = torch.full((5, 2), 0.019) + finger_positions[-1] = 0.04 + + potentials = transfer_potential(cube_positions, tool_positions, finger_positions, source_side) + + assert torch.all(potentials[1:] > potentials[:-1]) + + +def test_transfer_potential_rewards_closing_only_near_cube(): + """The acquisition bridge credits a close command only around the object.""" + cube_positions = torch.tensor([[0.52, 0.27, 0.06], [0.52, 0.27, 0.06]]) + tool_positions = torch.tensor([[0.52, 0.27, 0.07], [0.52, 0.27, 0.20]]) + source_side = torch.zeros(2, dtype=torch.long) + open_fingers = torch.full((2, 2), 0.04) + closed_fingers = torch.full((2, 2), 0.019) + + open_potential = transfer_potential(cube_positions, tool_positions, open_fingers, source_side) + closed_potential = transfer_potential(cube_positions, tool_positions, closed_fingers, source_side) + + assert closed_potential[0] - open_potential[0] > 0.5 + assert closed_potential[1] - open_potential[1] < 0.03 + + +def test_policy_distribution_samples_exact_binary_gripper_and_finite_kl(): + """PPO likelihoods match the gripper command that reaches physics.""" + distribution = ConveyorGaussianBernoulliDistribution(output_dim=8) + distribution.update(torch.zeros((4096, 8))) + + samples = distribution.sample() + old_params = tuple(parameter.clone() for parameter in distribution.params) + distribution.update(torch.full((4096, 8), 0.2)) + divergence = distribution.kl_divergence(old_params, distribution.params) + + assert set(torch.unique(samples[:, -1]).tolist()) == {-1.0, 1.0} + assert torch.isfinite(distribution.log_prob(samples)).all() + assert torch.isfinite(divergence).all() + assert torch.all(divergence >= 0.0) + + +def test_reset_sampling_guarantees_deployment_mass_and_tracks_frontier(): + """Sampling reserves deployment starts and favors intermediate frontier rows.""" + rows = build_reset_rows() + recipe_ids = torch.tensor([row.recipe for row in rows], dtype=torch.long) + variant_ids = torch.tensor([row.variant_id for row in rows], dtype=torch.long) + target_cube_ids = torch.tensor([row.target_cube_id for row in rows], dtype=torch.long) + source_side_ids = torch.tensor([row.source_side_id for row in rows], dtype=torch.long) + attempts = torch.zeros(len(rows), dtype=torch.long) + successes = torch.zeros_like(attempts) + place_stratum = (recipe_ids == int(ConveyorResetRecipe.PLACE)) & (target_cube_ids == 0) & (source_side_ids == 0) + place_ids = torch.nonzero(place_stratum, as_tuple=False).flatten() + attempts[place_ids[0]] = 100 + successes[place_ids[0]] = 100 + attempts[place_ids[1]] = 100 + successes[place_ids[1]] = 50 + + probabilities = reset_sampling_probabilities( + recipe_ids, + variant_ids, + target_cube_ids, + source_side_ids, + attempts, + successes, + deployment_probability=0.35, + epsilon=0.05, + ) + deployment_rows = (recipe_ids == int(ConveyorResetRecipe.BELT)) & ( + variant_ids == reset_variant_counts()[int(ConveyorResetRecipe.BELT)] - 1 + ) + + torch.testing.assert_close(probabilities.sum(), torch.tensor(1.0)) + torch.testing.assert_close(probabilities[deployment_rows].sum(), torch.tensor(0.35)) + torch.testing.assert_close(probabilities[~deployment_rows].sum(), torch.tensor(0.65)) + assert probabilities[place_ids[0]] < probabilities[place_ids[1]] + for recipe in ConveyorResetRecipe: + for cube_id in range(CUBE_COUNT): + for source_side_id in range(2): + stratum_rows = ( + (recipe_ids == int(recipe)) & (target_cube_ids == cube_id) & (source_side_ids == source_side_id) + ) + torch.testing.assert_close( + probabilities[stratum_rows & ~deployment_rows].sum(), + torch.tensor(0.65 / (len(ConveyorResetRecipe) * 2 * CUBE_COUNT)), + ) + for cube_id in range(CUBE_COUNT): + for source_side_id in range(2): + command_rows = (target_cube_ids == cube_id) & (source_side_ids == source_side_id) + torch.testing.assert_close( + probabilities[command_rows & deployment_rows].sum(), + torch.tensor(0.35 / (2 * CUBE_COUNT)), + ) + + +def test_rolling_progress_monitor_forgets_stale_outcomes_in_order(): + """Per-row curriculum evidence retains only each row's latest outcomes.""" + history = torch.zeros((2, 3), dtype=torch.bool) + pointer = torch.zeros(2, dtype=torch.int32) + size = torch.zeros_like(pointer) + true_count = torch.zeros_like(pointer) + rate = torch.zeros(2) + + _ring_append_bool_count_rate( + history, + torch.tensor([0, 0, 1, 0, 0]), + torch.tensor([False, False, True, True, True]), + pointer, + size, + true_count, + rate, + ) + + assert size.tolist() == [3, 1] + assert true_count.tolist() == [2, 1] + torch.testing.assert_close(rate, torch.tensor([2.0 / 3.0, 1.0])) + + +def test_active_cube_sampling_avoids_inactive_source_lane_cubes(): + """Random deployment starts cannot begin with interpenetrating parcels.""" + count = 2048 + base_x = torch.tensor((0.26, 0.42, 0.72, 0.88)).expand(count, -1).clone() + cube_sides = torch.tensor((0, 0, 1, 1)).expand(count, -1).clone() + target_cube_ids = torch.arange(count) % CUBE_COUNT + source_side_ids = torch.arange(count) % 2 + cube_sides.scatter_(1, target_cube_ids.unsqueeze(1), source_side_ids.unsqueeze(1)) + + sampled = _sample_collision_free_active_x( + base_x, + cube_sides, + target_cube_ids, + source_side_ids, + torch.full((count,), 0.30), + torch.full((count,), 0.82), + ) + + cube_ids = torch.arange(CUBE_COUNT).expand(count, -1) + inactive_on_source = (cube_sides == source_side_ids.unsqueeze(1)) & (cube_ids != target_cube_ids.unsqueeze(1)) + separation = torch.abs(sampled.unsqueeze(1) - base_x) + assert torch.all(separation[inactive_on_source] >= 0.055) From d1a679357b6b24e4a19d9e467919b7779d8d2a82 Mon Sep 17 00:00:00 2001 From: Maximilian Krause Date: Mon, 10 Aug 2026 15:19:11 -0700 Subject: [PATCH 07/23] Refine conveyor transfer task and force model --- .../maximiliank-conveyor-substep-callback.rst | 4 + .../isaaclab_newton/physics/newton_manager.py | 66 ++ .../test_newton_manager_abstraction.py | 37 + .../conveyor_franka/agents/rsl_rl_ppo_cfg.py | 3 + .../conveyor_franka/conveyor_force_driver.py | 936 +++++++++++++++--- .../conveyor_franka/conveyor_franka_env.py | 27 +- .../conveyor_franka_env_cfg.py | 256 +++-- .../conveyor_franka/conveyor_geometry.py | 120 ++- .../contrib/conveyor_franka/mdp/__init__.py | 5 + .../conveyor_franka/mdp/curriculums.py | 99 +- .../conveyor_franka/mdp/reset_events.py | 135 ++- .../contrib/conveyor_franka/mdp/rewards.py | 45 +- .../contrib/conveyor_franka/mdp/state.py | 10 +- .../conveyor_franka/mdp/terminations.py | 113 ++- .../contrib/test_conveyor_franka_geometry.py | 112 ++- .../test/contrib/test_conveyor_franka_mdp.py | 123 ++- uv.lock | 24 +- 17 files changed, 1808 insertions(+), 307 deletions(-) create mode 100644 source/isaaclab_newton/changelog.d/maximiliank-conveyor-substep-callback.rst diff --git a/source/isaaclab_newton/changelog.d/maximiliank-conveyor-substep-callback.rst b/source/isaaclab_newton/changelog.d/maximiliank-conveyor-substep-callback.rst new file mode 100644 index 000000000000..fdc1236d8422 --- /dev/null +++ b/source/isaaclab_newton/changelog.d/maximiliank-conveyor-substep-callback.rst @@ -0,0 +1,4 @@ +Added +^^^^^ + +* Added lifecycle-safe Newton manager callbacks for contact-force feedback after each solver substep. diff --git a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py index 9145b4f919c8..b2d3505d5dae 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py @@ -406,6 +406,9 @@ class NewtonManager(PhysicsManager): _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]] = [] # 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 @@ -1058,6 +1061,7 @@ def clear(cls): NewtonManager._adapter = None NewtonManager._post_actuator_callbacks = [] NewtonManager._state_force_callbacks = [] + NewtonManager._post_solver_substep_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 @@ -2307,6 +2311,8 @@ def _run_solver_substeps(cls, contacts) -> None: for callback in cls._state_force_callbacks: callback(cls._state_0) cls._step_solver(cls._state_0, cls._state_0, cls._control, contacts, cls._solver_dt) + for cb in cls._post_solver_substep_callbacks: + cb(cls._solver, contacts, cls._state_0, cls._solver_dt) cls._state_0.clear_forces() if collide_mid_loop and (i + 1) % collide_every == 0 and i + 1 < cls._num_substeps: cls._collision_pipeline.collide(cls._state_0, contacts) @@ -2317,6 +2323,8 @@ def _run_solver_substeps(cls, contacts) -> None: for callback in cls._state_force_callbacks: callback(cls._state_0) cls._step_solver(cls._state_0, cls._state_1, cls._control, contacts, cls._solver_dt) + for cb in cls._post_solver_substep_callbacks: + cb(cls._solver, contacts, cls._state_1, cls._solver_dt) if need_copy_on_last and i == cls._num_substeps - 1: cls._state_0.assign(cls._state_1) else: @@ -3131,6 +3139,64 @@ 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 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. diff --git a/source/isaaclab_newton/test/physics/test_newton_manager_abstraction.py b/source/isaaclab_newton/test/physics/test_newton_manager_abstraction.py index d4b69cca87ad..7e33ed7ca69c 100644 --- a/source/isaaclab_newton/test/physics/test_newton_manager_abstraction.py +++ b/source/isaaclab_newton/test/physics/test_newton_manager_abstraction.py @@ -258,6 +258,43 @@ def contacts(self): assert NewtonManager._contacts.rigid_contact_max == 2 +def test_in_graph_callback_registration_has_symmetric_cleanup(monkeypatch: pytest.MonkeyPatch) -> None: + """Scene-owned callbacks can deregister safely before Newton manager teardown.""" + + def actuator_callback(): + pass + + def state_force_callback(_state): + pass + + def substep_callback(_solver, _contacts, _state, _dt): + pass + + monkeypatch.setattr(NewtonManager, "_post_actuator_callbacks", []) + monkeypatch.setattr(NewtonManager, "_state_force_callbacks", []) + monkeypatch.setattr(NewtonManager, "_post_solver_substep_callbacks", []) + + NewtonManager.register_post_actuator_callback(actuator_callback) + NewtonManager.register_state_force_callback(state_force_callback) + NewtonManager.register_post_solver_substep_callback(substep_callback) + + assert NewtonManager._post_actuator_callbacks == [actuator_callback] + assert NewtonManager._state_force_callbacks == [state_force_callback] + assert NewtonManager._post_solver_substep_callbacks == [substep_callback] + + NewtonManager.unregister_post_actuator_callback(actuator_callback) + NewtonManager.unregister_state_force_callback(state_force_callback) + NewtonManager.unregister_post_solver_substep_callback(substep_callback) + # Repeated cleanup is intentionally a safe no-op. + NewtonManager.unregister_post_actuator_callback(actuator_callback) + NewtonManager.unregister_state_force_callback(state_force_callback) + NewtonManager.unregister_post_solver_substep_callback(substep_callback) + + assert NewtonManager._post_actuator_callbacks == [] + assert NewtonManager._state_force_callbacks == [] + assert NewtonManager._post_solver_substep_callbacks == [] + + def test_refit_sensor_bvh_rejects_missing_sensor_state(monkeypatch): """BVH refitting raises when a particle BVH exists without an initialized sensor state.""" model = SimpleNamespace(shape_count=0, particle_count=1, bvh_particles=object()) diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/agents/rsl_rl_ppo_cfg.py index 8e6bdf4d71af..4b11fd71d318 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/agents/rsl_rl_ppo_cfg.py @@ -145,6 +145,9 @@ class ConveyorFrankaPPORunnerCfg(RslRlOnPolicyRunnerCfg): """PPO configuration for four-cube commanded transfer.""" num_steps_per_env = 32 + # RSL-RL's usual randomized episode counters would desynchronize the + # per-subgoal timeout clock before the first policy step. + init_at_random_ep_len = False max_iterations = 4000 save_interval = 50 experiment_name = "conveyor_franka_transfer" diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_force_driver.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_force_driver.py index 6fdbe9ef63ad..0ec122432d27 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_force_driver.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_force_driver.py @@ -3,35 +3,145 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Task-local force model for static conveyor surfaces under Newton physics. +"""Batched contact-force conveyor surfaces for Newton physics. The driver reads solver-reported normal contact forces, computes a Coulomb-limited force that -drives each parcel's contact point toward the belt velocity, and applies that wrench on the -following physics step. +drives each transported body's contact points toward their conveyor velocity fields, and applies +the resulting body wrenches on the following physics solve. """ 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 .conveyor_geometry import BELT_CENTER_X, BELT_CENTER_Y, BELT_HALF_STRAIGHT, belt_direction +from .conveyor_geometry import ConveyorSectionSpec -_BELT_LABEL = re.compile(r"Conveyor(Left|Right)Belt") +_VELOCITY_FIELD_TYPE_CONSTANT = 0 +_VELOCITY_FIELD_TYPE_PIVOT = 1 +_SAME_NORMAL_THRESHOLD = 0.999 + + +@wp.struct +class Vec3Pair: + """Two orthonormal vectors spanning a contact tangent plane.""" + + v0: wp.vec3 + v1: wp.vec3 + + +@wp.func +def compute_basis_vectors(direction: wp.vec3) -> Vec3Pair: + """Return the reference conveyor's tangent basis for a unit direction.""" + basis = Vec3Pair() + if wp.abs(direction[1]) <= 0.9999: + basis.v0 = wp.normalize(wp.vec3(direction[2], 0.0, -direction[0])) + basis.v1 = wp.vec3( + direction[1] * basis.v0[2], + (direction[2] * basis.v0[0]) - (direction[0] * basis.v0[2]), + -direction[1] * basis.v0[0], + ) + else: + basis.v0 = wp.vec3(1.0, 0.0, 0.0) + basis.v1 = wp.normalize(wp.vec3(0.0, direction[2], -direction[1])) + return basis + + +@wp.func +def compute_point_impulse( + normal: wp.vec3, + normal_impulse: wp.float32, + current_vel: wp.vec3, + target_vel: wp.vec3, + response_linear: wp.float32, + inv_inertia_world: wp.mat33, + center_of_mass_to_point: wp.vec3, + friction_coefficient: wp.float32, + mass_splitting_scale: wp.float32, +) -> wp.vec3: + """Compute a Coulomb-clamped tangential impulse using point effective mass.""" + rel_vel = target_vel - current_vel + basis = compute_basis_vectors(normal) + + r_cross_t0 = wp.cross(center_of_mass_to_point, basis.v0) + r_cross_t1 = wp.cross(center_of_mass_to_point, basis.v1) + k00 = response_linear + wp.dot(r_cross_t0, wp.mul(inv_inertia_world, r_cross_t0)) + k11 = response_linear + wp.dot(r_cross_t1, wp.mul(inv_inertia_world, r_cross_t1)) + k01 = wp.dot(r_cross_t0, wp.mul(inv_inertia_world, r_cross_t1)) + det = (k00 * k11) - (k01 * k01) + + i0 = wp.float32(0.0) + i1 = wp.float32(0.0) + if det > 0.0: + v0 = wp.dot(basis.v0, rel_vel) + v1 = wp.dot(basis.v1, rel_vel) + i0 = ((k11 * v0) - (k01 * v1)) * mass_splitting_scale / det + i1 = ((k00 * v1) - (k01 * v0)) * mass_splitting_scale / det + + friction_impulse_max = normal_impulse * friction_coefficient + zero_err_magn = wp.sqrt((i0 * i0) + (i1 * i1)) + impulse_magn = wp.min(friction_impulse_max, zero_err_magn) + if zero_err_magn > 0.0: + ratio = impulse_magn / zero_err_magn + else: + ratio = 0.0 + return (basis.v0 * (i0 * ratio)) + (basis.v1 * (i1 * ratio)) + + +@wp.func +def compute_point_force( + dt: wp.float32, + inverse_dt: wp.float32, + com_world: wp.vec3, + body_inverse_mass: wp.float32, + body_inverse_inertia_world: wp.mat33, + body_linear_velocity: wp.vec3, + body_angular_velocity: wp.vec3, + contact_position: wp.vec3, + contact_normal: wp.vec3, + contact_force: wp.float32, + mass_splitting_scale: wp.float32, + target_vel: wp.vec3, + friction_coefficient: wp.float32, +) -> wp.spatial_vector: + """Compute force and torque at one conveyor contact.""" + contact_impulse = contact_force * dt + center_of_mass_to_point = contact_position - com_world + current_point_vel = body_linear_velocity + wp.cross(body_angular_velocity, center_of_mass_to_point) + + tangential_impulse = compute_point_impulse( + contact_normal, + contact_impulse, + current_point_vel, + target_vel, + body_inverse_mass, + body_inverse_inertia_world, + center_of_mass_to_point, + friction_coefficient, + mass_splitting_scale, + ) + + force = tangential_impulse * inverse_dt + torque = wp.cross(center_of_mass_to_point, force) + return wp.spatial_vector(force, torque) @wp.struct class BeltContact: - """Reduced contact data consumed by the conveyor force kernel.""" + """Reduced contact data consumed by the conveyor force kernels.""" valid: wp.int32 body: wp.int32 + conveyor: wp.int32 point: wp.vec3 normal: wp.vec3 normal_force: wp.float32 - target_velocity: wp.vec3 + next_body_contact: wp.int32 @wp.kernel @@ -40,33 +150,6 @@ def _extract_linear_force(spatial_force: wp.array[wp.spatial_vector], force: wp. force[contact_id] = wp.spatial_top(spatial_force[contact_id]) -@wp.func -def _racetrack_velocity( - point: wp.vec3, - center: wp.vec3, - half_straight: wp.float32, - direction: wp.float32, - speed: wp.float32, -) -> wp.vec3: - relative = point - center - tangent = wp.vec3() - if relative[0] > half_straight: - radial = wp.vec3(relative[0] - half_straight, relative[1], 0.0) - radial_length = wp.length(radial) - if radial_length > 0.0: - tangent = wp.vec3(radial[1], -radial[0], 0.0) / radial_length - elif relative[0] < -half_straight: - radial = wp.vec3(relative[0] + half_straight, relative[1], 0.0) - radial_length = wp.length(radial) - if radial_length > 0.0: - tangent = wp.vec3(radial[1], -radial[0], 0.0) / radial_length - elif relative[1] >= 0.0: - tangent = wp.vec3(1.0, 0.0, 0.0) - else: - tangent = wp.vec3(-1.0, 0.0, 0.0) - return tangent * direction * speed - - @wp.kernel def _classify_contacts( contact_count: wp.array[wp.int32], @@ -77,75 +160,184 @@ def _classify_contacts( point1: wp.array[wp.vec3], contact_force: wp.array[wp.vec3], shape_body: wp.array[wp.int32], - shape_is_belt: wp.array[wp.int32], - shape_belt_center: wp.array[wp.vec3], - shape_belt_direction: wp.array[wp.float32], - shape_transform: wp.array[wp.transform], + shape_conveyor: wp.array[wp.int32], + body_is_tracked: wp.array[wp.int32], body_q: wp.array[wp.transform], - half_straight: wp.float32, - speed: wp.float32, - normal_threshold: wp.float32, + conveyor_surface_normal: wp.array[wp.vec3], + conveyor_threshold: wp.array[wp.float32], contacts_out: wp.array[BeltContact], - body_contact_count: wp.array[wp.int32], + body_contact_head: wp.array[wp.int32], ): contact_id = wp.tid() result = BeltContact() result.valid = 0 + result.next_body_contact = -1 if contact_id < contact_count[0]: contact_shape0 = shape0[contact_id] contact_shape1 = shape1[contact_id] if contact_shape0 >= 0 and contact_shape1 >= 0: - belt0 = shape_is_belt[contact_shape0] - belt1 = shape_is_belt[contact_shape1] + conveyor0 = shape_conveyor[contact_shape0] + conveyor1 = shape_conveyor[contact_shape1] contact_normal = normal[contact_id] body = wp.int32(-1) - belt_shape = wp.int32(-1) + conveyor = wp.int32(-1) local_point = wp.vec3() normal_toward_body = wp.vec3() - if belt0 == 1 and belt1 == 0: - belt_shape = contact_shape0 + if conveyor0 >= 0 and conveyor1 < 0: + conveyor = conveyor0 body = shape_body[contact_shape1] local_point = point1[contact_id] normal_toward_body = contact_normal - elif belt1 == 1 and belt0 == 0: - belt_shape = contact_shape1 + elif conveyor1 >= 0 and conveyor0 < 0: + conveyor = conveyor1 body = shape_body[contact_shape0] local_point = point0[contact_id] normal_toward_body = -contact_normal - alignment = wp.dot(normal_toward_body, wp.vec3(0.0, 0.0, 1.0)) - normal_force = wp.abs(wp.dot(contact_force[contact_id], contact_normal)) - if body >= 0 and belt_shape >= 0 and alignment >= normal_threshold and normal_force > 0.0: - result.valid = 1 - result.body = body - result.point = wp.transform_point(body_q[body], local_point) - result.normal = normal_toward_body - result.normal_force = normal_force - belt_center = wp.transform_point(shape_transform[belt_shape], shape_belt_center[belt_shape]) - result.target_velocity = _racetrack_velocity( - result.point, - belt_center, - half_straight, - shape_belt_direction[belt_shape], - speed, - ) - wp.atomic_add(body_contact_count, body, 1) + if body >= 0 and conveyor >= 0 and body_is_tracked[body] != 0: + alignment = wp.dot(normal_toward_body, conveyor_surface_normal[conveyor]) + normal_force = wp.abs(wp.dot(contact_force[contact_id], contact_normal)) + if alignment >= conveyor_threshold[conveyor] and normal_force > 0.0: + result.valid = 1 + result.body = body + result.conveyor = conveyor + result.point = wp.transform_point(body_q[body], local_point) + result.normal = normal_toward_body + result.normal_force = normal_force + result.next_body_contact = wp.atomic_exch(body_contact_head, body, contact_id) contacts_out[contact_id] = result +@wp.kernel +def _prepare_contact_patches( + contacts: wp.array[BeltContact], + body_contact_head: wp.array[wp.int32], + body_q: wp.array[wp.transform], + body_com: wp.array[wp.vec3], + contact_patch_head: wp.array[wp.int32], + adjusted_normal_force: wp.array[wp.float32], + mass_splitting_scale: wp.array[wp.float32], +): + """Correlate contacts by normal and normalize loads across overlapping sections.""" + body_id = wp.tid() + reference_point = wp.transform_point(body_q[body_id], body_com[body_id]) + patch_contact_id = body_contact_head[body_id] + + while patch_contact_id >= 0: + if contact_patch_head[patch_contact_id] < 0: + patch_contact = contacts[patch_contact_id] + basis = compute_basis_vectors(patch_contact.normal) + point_count = wp.int32(0) + first_conveyor = patch_contact.conveyor + spans_multiple_conveyors = wp.int32(0) + patch_force_sum = wp.float32(0.0) + min0 = wp.float32(1.0e30) + max0 = wp.float32(-1.0e30) + min1 = wp.float32(1.0e30) + max1 = wp.float32(-1.0e30) + + contact_id = body_contact_head[body_id] + while contact_id >= 0: + contact = contacts[contact_id] + if ( + contact_patch_head[contact_id] < 0 + and wp.dot(patch_contact.normal, contact.normal) > _SAME_NORMAL_THRESHOLD + ): + contact_patch_head[contact_id] = patch_contact_id + point_count += 1 + if contact.conveyor != first_conveyor: + spans_multiple_conveyors = 1 + patch_force_sum += contact.normal_force + + delta = contact.point - reference_point + projection0 = wp.dot(basis.v0, delta) + projection1 = wp.dot(basis.v1, delta) + min0 = wp.min(min0, projection0) + max0 = wp.max(max0, projection0) + min1 = wp.min(min1, projection1) + max1 = wp.max(max1, projection1) + contact_id = contact.next_body_contact + + splitting_scale = 1.0 / wp.float32(point_count) + if point_count == 1 or spans_multiple_conveyors == 0: + contact_id = body_contact_head[body_id] + while contact_id >= 0: + contact = contacts[contact_id] + if contact_patch_head[contact_id] == patch_contact_id: + adjusted_normal_force[contact_id] = contact.normal_force + mass_splitting_scale[contact_id] = splitting_scale + contact_id = contact.next_body_contact + else: + kernel_radius = 0.25 * ((max0 - min0) + (max1 - min1)) + kernel_radius_sqr = kernel_radius * kernel_radius + if kernel_radius > 0.0: + point_force_weight_sum = wp.float32(0.0) + contact_id = body_contact_head[body_id] + while contact_id >= 0: + contact = contacts[contact_id] + if contact_patch_head[contact_id] == patch_contact_id: + density = wp.float32(1.0) + other_contact_id = body_contact_head[body_id] + while other_contact_id >= 0: + other_contact = contacts[other_contact_id] + if ( + other_contact_id != contact_id + and contact_patch_head[other_contact_id] == patch_contact_id + ): + delta = contact.point - other_contact.point + projected_delta = delta - ( + wp.dot(delta, patch_contact.normal) * patch_contact.normal + ) + density += wp.exp(-0.5 * wp.length_sq(projected_delta) / kernel_radius_sqr) + other_contact_id = other_contact.next_body_contact + + weight = 1.0 / density + adjusted_normal_force[contact_id] = weight + mass_splitting_scale[contact_id] = splitting_scale + point_force_weight_sum += weight + contact_id = contact.next_body_contact + + force_per_weight = patch_force_sum / point_force_weight_sum + contact_id = body_contact_head[body_id] + while contact_id >= 0: + contact = contacts[contact_id] + if contact_patch_head[contact_id] == patch_contact_id: + adjusted_normal_force[contact_id] *= force_per_weight + contact_id = contact.next_body_contact + else: + adjusted_force = patch_force_sum / wp.float32(point_count) + contact_id = body_contact_head[body_id] + while contact_id >= 0: + contact = contacts[contact_id] + if contact_patch_head[contact_id] == patch_contact_id: + adjusted_normal_force[contact_id] = adjusted_force + mass_splitting_scale[contact_id] = splitting_scale + contact_id = contact.next_body_contact + + patch_contact_id = contacts[patch_contact_id].next_body_contact + + @wp.kernel def _accumulate_forces( dt: wp.float32, - friction: wp.float32, contacts: wp.array[BeltContact], body_q: wp.array[wp.transform], body_qd: wp.array[wp.spatial_vector], body_com: wp.array[wp.vec3], body_inv_mass: wp.array[wp.float32], - body_contact_count: wp.array[wp.int32], + body_inv_inertia: wp.array[wp.mat33], + adjusted_normal_force: wp.array[wp.float32], + mass_splitting_scale: wp.array[wp.float32], + conveyor_field_type: wp.array[wp.int32], + conveyor_direction: wp.array[wp.vec3], + conveyor_pivot_point: wp.array[wp.vec3], + conveyor_radius: wp.array[wp.float32], + conveyor_effective_velocity: wp.array[wp.float32], + conveyor_friction: wp.array[wp.float32], + velocity_scale: wp.array[wp.float32], body_force: wp.array[wp.spatial_vector], ): contact_id = wp.tid() @@ -153,28 +345,78 @@ def _accumulate_forces( if contact.valid == 0: return - count = body_contact_count[contact.body] - inverse_mass = body_inv_mass[contact.body] - if count <= 0 or inverse_mass <= 0.0: + splitting_scale = mass_splitting_scale[contact_id] + if splitting_scale <= 0.0: return + conveyor = contact.conveyor + effective_velocity = conveyor_effective_velocity[conveyor] * velocity_scale[0] + if conveyor_field_type[conveyor] == _VELOCITY_FIELD_TYPE_CONSTANT: + target_velocity = conveyor_direction[conveyor] * effective_velocity + else: + angular_velocity = conveyor_direction[conveyor] * (effective_velocity / conveyor_radius[conveyor]) + target_velocity = wp.cross(angular_velocity, contact.point - conveyor_pivot_point[conveyor]) + pose = body_q[contact.body] center_of_mass = wp.transform_point(pose, body_com[contact.body]) - center_to_contact = contact.point - center_of_mass + rotation = wp.quat_to_matrix(wp.transform_get_rotation(pose)) + inverse_inertia_world = rotation * body_inv_inertia[contact.body] * wp.transpose(rotation) velocity = body_qd[contact.body] - point_velocity = wp.spatial_top(velocity) + wp.cross(wp.spatial_bottom(velocity), center_to_contact) - velocity_error = contact.target_velocity - point_velocity - velocity_error = velocity_error - contact.normal * wp.dot(velocity_error, contact.normal) - desired_force = velocity_error / (inverse_mass * dt * float(count)) + force = compute_point_force( + dt, + 1.0 / dt, + center_of_mass, + body_inv_mass[contact.body], + inverse_inertia_world, + wp.spatial_top(velocity), + wp.spatial_bottom(velocity), + contact.point, + contact.normal, + adjusted_normal_force[contact_id], + splitting_scale, + target_velocity, + conveyor_friction[conveyor], + ) + wp.atomic_add(body_force, contact.body, force) + + +@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) + - desired_magnitude = wp.length(desired_force) - max_magnitude = friction * contact.normal_force - if desired_magnitude > max_magnitude and desired_magnitude > 0.0: - desired_force = desired_force * (max_magnitude / desired_magnitude) +@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] - torque = wp.cross(center_to_contact, desired_force) - wp.atomic_add(body_force, contact.body, wp.spatial_vector(desired_force, torque)) + +@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 @@ -183,24 +425,115 @@ def _add_body_force(dst: wp.array[wp.spatial_vector], src: wp.array[wp.spatial_v dst[body_id] = dst[body_id] + src[body_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))) + + class ConveyorForceDriver: - """Convert Newton contact forces into moving-surface forces for the racetrack belts.""" + """Run one batched moving-surface force pipeline for a Newton scene.""" def __init__( self, num_envs: int, + surface_specs: Sequence[ConveyorSectionSpec], speed: float = 0.35, friction: float = 0.5, - normal_threshold: float = 0.95, + normal_threshold: float = 0.997, + startup_duration_s: float = 1.0, + transported_body_pattern: str = r"(?:^|/)Cube_?[0-3](?:/|$)", + transported_body_count_per_env: int | None = None, ) -> None: """Initialize the driver after Newton simulation startup. Args: num_envs: Number of replicated simulation environments. - speed: Conveyor surface speed [m/s]. + surface_specs: Collision sections and matching velocity fields. + speed: Initial signed conveyor surface speed [m/s]. friction: Coulomb friction coefficient used to limit traction. - normal_threshold: Minimum upward contact-normal alignment. + normal_threshold: Minimum contact-normal alignment in the range [0, 1]. + startup_duration_s: Duration of the initial traction ramp [s]. + transported_body_pattern: Regular expression selecting bodies that receive traction. + transported_body_count_per_env: Expected selected body count per environment, or ``None``. """ + if num_envs <= 0: + raise ValueError(f"Number of conveyor environments must be positive, got {num_envs}.") + if not np.isfinite(speed): + raise ValueError(f"Conveyor speed must be finite, got {speed}.") + if not np.isfinite(friction) or friction < 0.0: + raise ValueError(f"Conveyor friction must be non-negative, got {friction}.") + if not np.isfinite(normal_threshold) or not 0.0 <= normal_threshold <= 1.0: + raise ValueError(f"Conveyor normal threshold must be in [0, 1], got {normal_threshold}.") + 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._surface_specs = tuple(surface_specs) + self._validate_surface_specs() + try: + body_pattern = re.compile(transported_body_pattern) + except re.error as exc: + raise ValueError(f"Invalid transported-body pattern: {transported_body_pattern!r}.") from exc + model = NewtonManager.get_model() contacts = NewtonManager.get_contacts() if model is None or contacts is None: @@ -210,56 +543,236 @@ def __init__( "Newton did not allocate per-contact force reporting. The scene contact sensor must initialize " "before the conveyor driver." ) + 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._dt = NewtonManager.get_solver_dt() - self._speed = speed - self._friction = friction - self._normal_threshold = normal_threshold - - shape_is_belt = [0] * model.shape_count - shape_belt_center = [wp.vec3()] * model.shape_count - shape_belt_direction = [0.0] * model.shape_count + self._num_envs = num_envs + self._startup_duration_s = startup_duration_s + self._closed = False + self._validate_backend_buffers() + + shape_conveyor = [-1] * model.shape_count + field_type: list[int] = [] + direction: list[wp.vec3] = [] + pivot_point: list[wp.vec3] = [] + radius: list[float] = [] + surface_normal: list[wp.vec3] = [] + conveyor_world: list[int] = [] + surface_paths: list[str] = [] + shape_body = model.shape_body.numpy() - matched_shapes = 0 + shape_world = model.shape_world.numpy() + shape_transform = model.shape_transform.numpy() + patterns = tuple(re.compile(rf"(?:^|/){re.escape(spec.mesh.name)}(?:/|$)") for spec in self._surface_specs) + seen_sections: set[tuple[int, int]] = set() for shape_id, label in enumerate(model.shape_label): - match = _BELT_LABEL.search(label) - if match is None: + matching_specs = [index for index, pattern in enumerate(patterns) if pattern.search(label)] + 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}") - side = match.group(1) - shape_is_belt[shape_id] = 1 - center_y = BELT_CENTER_Y if side == "Left" else -BELT_CENTER_Y - shape_belt_center[shape_id] = wp.vec3(BELT_CENTER_X, center_y, 0.0) - shape_belt_direction[shape_id] = belt_direction(side) - matched_shapes += 1 - - expected_shapes = 2 * num_envs - if matched_shapes != expected_shapes: - raise RuntimeError(f"Expected {expected_shapes} conveyor shapes, but matched {matched_shapes}.") - - self._shape_is_belt = wp.array(shape_is_belt, dtype=wp.int32, device=self._device) - self._shape_belt_center = wp.array(shape_belt_center, dtype=wp.vec3, device=self._device) - self._shape_belt_direction = wp.array(shape_belt_direction, dtype=wp.float32, device=self._device) - self._contact_force = wp.zeros(contacts.rigid_contact_max, dtype=wp.vec3, device=self._device) - self._belt_contacts = wp.empty(contacts.rigid_contact_max, dtype=BeltContact, device=self._device) - self._body_contact_count = wp.zeros(model.body_count, dtype=wp.int32, device=self._device) + spec_id = matching_specs[0] + world_id = int(shape_world[shape_id]) + if not 0 <= world_id < num_envs: + raise RuntimeError(f"Conveyor shape {label!r} belongs to invalid world {world_id}.") + 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].mesh.name!r}." + ) + seen_sections.add(section_key) + + spec = self._surface_specs[spec_id] + conveyor_id = len(field_type) + shape_conveyor[shape_id] = conveyor_id + field_type.append( + _VELOCITY_FIELD_TYPE_CONSTANT if spec.velocity_field_type == "constant" else _VELOCITY_FIELD_TYPE_PIVOT + ) + direction.append(_world_vector(shape_transform[shape_id], spec.direction)) + pivot_point.append(_world_point(shape_transform[shape_id], spec.pivot_point)) + radius.append(1.0 if spec.radius is None else spec.radius) + surface_normal.append(_world_vector(shape_transform[shape_id], spec.surface_normal)) + conveyor_world.append(world_id) + surface_paths.append(label) + + expected_sections = {(world_id, spec_id) for world_id in range(num_envs) for spec_id in range(len(patterns))} + missing_sections = sorted(expected_sections - seen_sections) + if missing_sections: + details = ", ".join( + f"world {world_id}: {self._surface_specs[spec_id].mesh.name}" + 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 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 transported_body_count_per_env is not None: + bad_worlds = np.flatnonzero(tracked_counts != transported_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"Transported-body pattern {transported_body_pattern!r} expected " + f"{transported_body_count_per_env} bodies per world ({details})." + ) + if not np.any(body_is_tracked): + raise RuntimeError(f"Transported-body pattern {transported_body_pattern!r} matched no Newton bodies.") + + conveyor_count = len(field_type) + self._surface_paths = tuple(surface_paths) + self._shape_conveyor = wp.array(shape_conveyor, dtype=wp.int32, device=self._device) + self._body_is_tracked = wp.array(body_is_tracked, dtype=wp.int32, device=self._device) + self._field_type = wp.array(field_type, dtype=wp.int32, device=self._device) + self._direction = wp.array(direction, dtype=wp.vec3, device=self._device) + self._pivot_point = wp.array(pivot_point, dtype=wp.vec3, device=self._device) + self._radius = wp.array(radius, dtype=wp.float32, device=self._device) + self._surface_normal = wp.array(surface_normal, dtype=wp.vec3, device=self._device) + self._conveyor_world = wp.array(conveyor_world, dtype=wp.int32, device=self._device) + + self._command_velocity_host = np.full(conveyor_count, speed, dtype=np.float32) + self._enabled_host = np.ones(conveyor_count, dtype=np.int32) + self._friction_host = np.full(conveyor_count, friction, dtype=np.float32) + self._threshold_host = np.full(conveyor_count, normal_threshold, dtype=np.float32) + 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._friction = wp.array(self._friction_host, dtype=wp.float32, device=self._device) + self._threshold = wp.array(self._threshold_host, 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._velocity_scale = wp.zeros(1, dtype=wp.float32, device=self._device) + + contact_capacity = contacts.rigid_contact_max + self._contact_force = wp.zeros(contact_capacity, dtype=wp.vec3, device=self._device) + self._belt_contacts = wp.empty(contact_capacity, dtype=BeltContact, device=self._device) + self._body_contact_head = wp.full(model.body_count, -1, dtype=wp.int32, device=self._device) + self._contact_patch_head = wp.full(contact_capacity, -1, dtype=wp.int32, device=self._device) + self._adjusted_normal_force = wp.zeros(contact_capacity, dtype=wp.float32, device=self._device) + self._mass_splitting_scale = wp.zeros(contact_capacity, dtype=wp.float32, device=self._device) self._body_force = wp.zeros(model.body_count, dtype=wp.spatial_vector, device=self._device) + 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").astype(np.bool_) + self._enabled_host[selected] = values.astype(np.int32) + self._enabled.assign(self._enabled_host) + self._refresh_effective_velocities() + + def set_friction_coefficients(self, coefficients: Any, indices: Any = None) -> None: + """Set Coulomb traction limits for selected surfaces.""" + selected = self._resolve_indices(indices) + values = self._broadcast_1d(coefficients, len(selected), "friction coefficients") + if np.any(values < 0.0): + raise ValueError("Conveyor friction coefficients must be non-negative.") + self._friction_host[selected] = values + self._friction.assign(self._friction_host) + + def set_contact_processing_thresholds(self, thresholds: Any, indices: Any = None) -> None: + """Set minimum contact-normal alignment for selected surfaces.""" + selected = self._resolve_indices(indices) + values = self._broadcast_1d(thresholds, len(selected), "contact thresholds") + if np.any((values < 0.0) | (values > 1.0)): + raise ValueError("Conveyor contact thresholds must lie in [0, 1].") + self._threshold_host[selected] = values + self._threshold.assign(self._threshold_host) + + 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. - NewtonManager.register_post_actuator_callback(self.apply) + Args: + env_ids: Environment indices to reset, or ``None`` for every environment. + """ + if env_ids is None: + self._body_force.zero_() + self._encoder_position.zero_() + self._elapsed_time.zero_() + self._velocity_scale.zero_() + return + + ids = np.asarray(_as_numpy(env_ids), dtype=np.int64).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._body_force], + 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 clear(self) -> None: - """Discard forces computed before an environment reset.""" - self._body_force.zero_() - self._body_contact_count.zero_() + 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) -> None: - """Apply the wrench computed from the preceding physics step.""" - state = NewtonManager.get_state_0() + def apply(self, state) -> None: + """Apply the wrench computed from the preceding physics solve.""" wp.launch( _add_body_force, dim=self._model.body_count, @@ -267,54 +780,189 @@ def apply(self) -> None: device=self._device, ) - def update(self) -> None: - """Read current contact forces and compute the next conveyor wrench.""" - state = NewtonManager.get_state_0() + def update(self, solver, contacts, state, dt: float) -> None: + """Read solved contacts and compute the next per-solve conveyor wrench.""" + solver.update_contacts(contacts) self._body_force.zero_() - self._body_contact_count.zero_() + self._body_contact_head.fill_(-1) + self._contact_patch_head.fill_(-1) + self._mass_splitting_scale.zero_() + wp.launch( + _advance_startup_scale, + dim=1, + inputs=[dt, self._startup_duration_s], + outputs=[self._elapsed_time, self._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, + ) wp.launch( _extract_linear_force, dim=self._contacts.rigid_contact_max, - inputs=[self._contacts.force, self._contact_force], + inputs=[contacts.force, self._contact_force], device=self._device, ) wp.launch( _classify_contacts, dim=self._contacts.rigid_contact_max, inputs=[ - self._contacts.rigid_contact_count, - self._contacts.rigid_contact_shape0, - self._contacts.rigid_contact_shape1, - self._contacts.rigid_contact_normal, - self._contacts.rigid_contact_point0, - self._contacts.rigid_contact_point1, + contacts.rigid_contact_count, + contacts.rigid_contact_shape0, + contacts.rigid_contact_shape1, + contacts.rigid_contact_normal, + contacts.rigid_contact_point0, + contacts.rigid_contact_point1, self._contact_force, self._model.shape_body, - self._shape_is_belt, - self._shape_belt_center, - self._shape_belt_direction, - self._model.shape_transform, + self._shape_conveyor, + self._body_is_tracked, state.body_q, - BELT_HALF_STRAIGHT, - self._speed, - self._normal_threshold, + self._surface_normal, + self._threshold, ], - outputs=[self._belt_contacts, self._body_contact_count], + outputs=[self._belt_contacts, self._body_contact_head], + device=self._device, + ) + wp.launch( + _prepare_contact_patches, + dim=self._model.body_count, + inputs=[self._belt_contacts, self._body_contact_head, state.body_q, self._model.body_com], + outputs=[self._contact_patch_head, self._adjusted_normal_force, self._mass_splitting_scale], device=self._device, ) wp.launch( _accumulate_forces, dim=self._contacts.rigid_contact_max, inputs=[ - self._dt, - self._friction, + dt, self._belt_contacts, state.body_q, state.body_qd, self._model.body_com, self._model.body_inv_mass, - self._body_contact_count, + self._model.body_inv_inertia, + self._adjusted_normal_force, + self._mass_splitting_scale, + self._field_type, + self._direction, + self._pivot_point, + self._radius, + self._effective_velocity, + self._friction, + self._velocity_scale, ], outputs=[self._body_force], device=self._device, ) + + def _validate_surface_specs(self) -> None: + """Validate structural surface descriptions before resolving Newton shapes.""" + if not self._surface_specs: + raise ValueError("At least one conveyor surface specification is required.") + names = [spec.mesh.name for spec in self._surface_specs] + if len(set(names)) != len(names): + raise ValueError(f"Conveyor surface names must be unique, got {names}.") + for spec in self._surface_specs: + if spec.velocity_field_type not in {"constant", "pivot"}: + raise ValueError( + f"Unknown velocity field {spec.velocity_field_type!r} for conveyor surface {spec.mesh.name!r}." + ) + direction = np.asarray(spec.direction, dtype=np.float64) + pivot_point = np.asarray(spec.pivot_point, dtype=np.float64) + surface_normal = np.asarray(spec.surface_normal, dtype=np.float64) + if direction.shape != (3,) or not np.all(np.isfinite(direction)) or np.linalg.norm(direction) <= 1.0e-8: + raise ValueError(f"Conveyor surface {spec.mesh.name!r} needs a non-zero 3-D direction.") + if pivot_point.shape != (3,) or not np.all(np.isfinite(pivot_point)): + raise ValueError(f"Conveyor surface {spec.mesh.name!r} needs a 3-D pivot point.") + if ( + surface_normal.shape != (3,) + or not np.all(np.isfinite(surface_normal)) + or np.linalg.norm(surface_normal) <= 1.0e-8 + ): + raise ValueError(f"Conveyor surface {spec.mesh.name!r} needs a non-zero 3-D surface normal.") + if spec.velocity_field_type == "pivot" and ( + spec.radius is None or not np.isfinite(spec.radius) or spec.radius <= 0.0 + ): + raise ValueError(f"Pivot conveyor surface {spec.mesh.name!r} needs a positive arc radius.") + + 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_count", contacts.rigid_contact_count, 1) + + def _resolve_indices(self, indices: Any) -> np.ndarray: + """Normalize and validate a conveyor index selection.""" + if indices is None: + return np.arange(len(self._surface_paths), dtype=np.int64) + selected = np.asarray(_as_numpy(indices), dtype=np.int64).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, + ) + + 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 + + @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_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env.py index cc092a2a55ef..4b41c36ca744 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env.py @@ -9,13 +9,11 @@ from collections.abc import Sequence -import torch - from isaaclab.envs import ManagerBasedRLEnv -from isaaclab.envs.common import VecEnvStepReturn from .conveyor_force_driver import ConveyorForceDriver from .conveyor_franka_env_cfg import ConveyorFrankaEnvCfg +from .conveyor_geometry import belt_collision_section_specs class ConveyorFrankaEnv(ManagerBasedRLEnv): @@ -27,22 +25,29 @@ def __init__(self, cfg: ConveyorFrankaEnvCfg, render_mode: str | None = None, ** super().__init__(cfg, render_mode=render_mode, **kwargs) self._conveyor_driver = ConveyorForceDriver( num_envs=self.num_envs, + surface_specs=tuple( + section for side in ("Left", "Right") for section in belt_collision_section_specs(side) + ), speed=cfg.conveyor_force.speed, friction=cfg.conveyor_force.friction, normal_threshold=cfg.conveyor_force.normal_threshold, + startup_duration_s=cfg.conveyor_force.startup_duration_s, + transported_body_pattern=cfg.conveyor_force.transported_body_pattern, + transported_body_count_per_env=cfg.conveyor_force.transported_body_count_per_env, ) - def step(self, action: torch.Tensor) -> VecEnvStepReturn: - """Step the manager-based environment and prepare traction for the next step.""" - result = super().step(action) - # The contact sensor has now asked Newton to publish per-contact forces. - self._conveyor_driver.update() - return result - def _reset_idx(self, env_ids: Sequence[int]): """Reset selected environments and discard stale conveyor forces.""" super()._reset_idx(env_ids) conveyor_driver = getattr(self, "_conveyor_driver", None) if conveyor_driver is not None: - conveyor_driver.clear() + conveyor_driver.reset(env_ids) + + def close(self): + """Release the conveyor callbacks before the Newton scene is destroyed.""" + conveyor_driver = getattr(self, "_conveyor_driver", None) + if conveyor_driver is not None: + conveyor_driver.close() + self._conveyor_driver = None + super().close() diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env_cfg.py index 217704c9345e..92b941a96670 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env_cfg.py @@ -7,8 +7,11 @@ from __future__ import annotations +import math +import re + from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg, NewtonCollisionPipelineCfg, NewtonShapeCfg -from isaaclab_newton.sim.schemas import MujocoCollisionCfg, NewtonMaterialPropertiesCfg +from isaaclab_newton.sim.schemas import MujocoCollisionCfg, NewtonCollisionCfg, NewtonMaterialPropertiesCfg import isaaclab.sim as sim_utils from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg @@ -23,7 +26,7 @@ from isaaclab.scene import InteractiveSceneCfg from isaaclab.sensors import ContactSensorCfg from isaaclab.sim import SimulationCfg -from isaaclab.sim.schemas import UsdPhysicsCollisionCfg +from isaaclab.sim.schemas import CollisionFragment, UsdPhysicsCollisionCfg from isaaclab.sim.spawners.materials import RigidBodyMaterialBaseCfg from isaaclab.utils.configclass import configclass @@ -35,12 +38,35 @@ GUARD_COLOR, PARCEL_COLOR, MeshSpec, + belt_collision_section_specs, belt_mesh_spec, guard_mesh_specs, ) from .franka_robot_cfg import FRANKA_PANDA_CONVEYOR_CFG _DYNAMIC_PROPERTIES = sim_utils.RigidBodyBaseCfg() +_CONTACT_GAP = 0.01 +_CUBE_CONTACT_MARGIN = 0.003 +_MUJOCO_SOLIMP = (0.9, 0.95, 0.001, 0.5, 2.0) +_MUJOCO_SOLREF = (0.02, 1.0) +_POLICY_DT = 1.0 / 60.0 +_SUBGOAL_TIMEOUT_S = 20.0 +_TRANSFER_SEQUENCE_LENGTH = 8 + + +def _collision_properties(contact_margin: float = 0.0, mujoco_priority: int = 0) -> list[CollisionFragment]: + """Build explicit Newton and MuJoCo contact properties for one collider.""" + return [ + UsdPhysicsCollisionCfg(collision_enabled=True), + NewtonCollisionCfg(contact_margin=contact_margin, contact_gap=_CONTACT_GAP), + MujocoCollisionCfg( + condim=3, + priority=mujoco_priority, + solimp=_MUJOCO_SOLIMP, + solmix=1.0, + solref=_MUJOCO_SOLREF, + ), + ] def _srgb_to_linear_channel(value: float) -> float: @@ -108,7 +134,7 @@ def __post_init__(self) -> None: @configclass class EventCfg: - """Reset the scene, then restore one validated transfer state.""" + """Restore validated states and advance completed transfer goals.""" reset_all = EventTerm(func=mdp.reset_scene_to_default, mode="reset") reset_from_state_table = EventTerm( @@ -124,15 +150,28 @@ class EventCfg: "arm_joint_noise": 0.015, }, ) + advance_transfer_goal = EventTerm( + func=mdp.advance_conveyor_transfer_goal, + mode="interval", + interval_range_s=(_POLICY_DT, _POLICY_DT), + params={"success_context_name": "transfer_success_context"}, + ) @configclass class RewardsCfg: - """Transfer progress, completion, and regularization rewards.""" + """Sparse transfer completion plus safety regularization rewards.""" - progress = RewTerm(func=mdp.ConveyorTransferProgressReward, weight=60.0) - success = RewTerm(func=mdp.transfer_success_reward, weight=600.0) - failure = RewTerm(func=mdp.terminal_failure, weight=-60.0) + success = RewTerm( + func=mdp.transfer_success_reward, + params={"context_term_name": "transfer_success_context"}, + weight=600.0, + ) + failure = RewTerm( + func=mdp.terminal_failure, + params={"success_context_name": "transfer_success_context"}, + weight=-60.0, + ) arm_action_l2 = RewTerm( func=mdp.action_term_l2, params={"action_name": "arm_action"}, @@ -148,7 +187,7 @@ class RewardsCfg: @configclass class TerminationsCfg: - """Successful placement, physical failure, and horizon terms.""" + """Continuing transfer context, safety failures, and training truncations.""" learning_progress_context = DoneTerm( func=mdp.ConveyorResetLearningProgress, @@ -156,9 +195,12 @@ class TerminationsCfg: "minimum_episode_steps": 3, "minimum_progress": 0.35, "maximum_target_potential": 5.0, + "minimum_acquisition_lift": 0.025, + "maximum_acquisition_tool_distance": 0.075, + "maximum_acquisition_finger_position": 0.030, }, ) - success = DoneTerm( + transfer_success_context = DoneTerm( func=mdp.StableConveyorTransfer, params={ "minimum_episode_steps": 2, @@ -171,7 +213,16 @@ class TerminationsCfg: ) cube_out_of_workspace = DoneTerm(func=mdp.cube_out_of_workspace) nonfinite_scene_state = DoneTerm(func=mdp.nonfinite_scene_state) - time_out = DoneTerm(func=mdp.time_out, time_out=True) + subgoal_time_out = DoneTerm( + func=mdp.subgoal_time_out, + params={"timeout_s": _SUBGOAL_TIMEOUT_S}, + time_out=True, + ) + transfer_sequence_time_out = DoneTerm( + func=mdp.transfer_sequence_time_out, + params={"maximum_transfers": _TRANSFER_SEQUENCE_LENGTH}, + time_out=True, + ) @configclass @@ -182,14 +233,22 @@ class CurriculumCfg: func=mdp.ConveyorResetCurriculum, params={ "progress_context_name": "learning_progress_context", - "final_success_termination_name": "success", + "final_success_context_name": "transfer_success_context", # Match Franka Stack: sampling follows each row's recent policy # competence instead of retaining stale early failures forever. "monitored_history_len": 50, # Keep a deployment-facing stream while the remaining starts - # adapt around the rolling pickup-to-placement frontier. Adaptive - # rows remain balanced across recipe, cube identity, and side. - "deployment_probability": 0.35, + # adapt around the rolling pickup-to-placement frontier. Every + # recipe, cube identity, and direction retains equal total mass. + "deployment_probability_initial": 0.35, + "deployment_probability_final": 0.90, + "deployment_progress_start": 0.45, + "deployment_progress_end": 0.80, + "deployment_coverage_target": 0.50, + # Optional staged-training control. None keeps the deployable + # bidirectional task; a side id can focus the same adaptive reset + # distribution on one weak direction without changing reset rows. + "fixed_source_side_id": None, }, ) @@ -204,17 +263,37 @@ class ConveyorForceCfg: friction: float = 0.5 """Coulomb friction coefficient used to limit traction.""" - normal_threshold: float = 0.95 + normal_threshold: float = 0.997 """Minimum upward contact-normal alignment in the range [0, 1].""" + startup_duration_s: float = 1.0 + """Duration over which conveyor traction ramps to full speed [s].""" + + transported_body_pattern: str = r"(?:^|/)Cube_?[0-3](?:/|$)" + """Regular expression selecting rigid bodies that receive conveyor forces.""" + + transported_body_count_per_env: int = 4 + """Expected number of transported rigid bodies in each environment.""" + def __post_init__(self) -> None: """Validate conveyor force parameters.""" - if self.speed < 0.0: + if not math.isfinite(self.speed) or self.speed < 0.0: raise ValueError(f"Conveyor speed must be non-negative, got {self.speed}.") - if self.friction < 0.0: + if not math.isfinite(self.friction) or self.friction < 0.0: raise ValueError(f"Conveyor friction must be non-negative, got {self.friction}.") - if not 0.0 <= self.normal_threshold <= 1.0: + if not math.isfinite(self.normal_threshold) or not 0.0 <= self.normal_threshold <= 1.0: raise ValueError(f"Conveyor normal threshold must be in [0, 1], got {self.normal_threshold}.") + if not math.isfinite(self.startup_duration_s) or self.startup_duration_s <= 0.0: + raise ValueError(f"Conveyor startup duration must be positive, got {self.startup_duration_s}.") + if self.transported_body_count_per_env <= 0: + raise ValueError( + "Conveyor transported body count per environment must be positive, got " + f"{self.transported_body_count_per_env}." + ) + try: + re.compile(self.transported_body_pattern) + except re.error as exc: + raise ValueError(f"Invalid conveyor transported-body pattern: {self.transported_body_pattern!r}.") from exc @sim_utils.clone @@ -247,6 +326,20 @@ def _spawn_shape_with_display_color( return prim +@sim_utils.clone +def _spawn_hidden_collision_mesh( + prim_path: str, + cfg: sim_utils.MeshCustomCfg, + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, + **kwargs, +): + """Spawn a collision-only custom mesh and hide it from all visualizers.""" + prim = sim_utils.spawn_mesh_custom(prim_path, cfg, translation, orientation, **kwargs) + sim_utils.set_prim_visibility(prim, False) + return prim + + def _static_cuboid( prim_path: str, size: tuple[float, float, float], @@ -260,7 +353,7 @@ def _static_cuboid( """Build a static colliding cuboid configuration.""" spawn = sim_utils.CuboidCfg( size=size, - collision_props=sim_utils.CollisionBaseCfg(), + collision_props=_collision_properties(), physics_material=RigidBodyMaterialBaseCfg( static_friction=friction, dynamic_friction=friction, @@ -280,33 +373,19 @@ def _static_cuboid( ) -def _static_mesh( +def _visual_mesh( prim_path: str, spec: MeshSpec, color: tuple[float, float, float], - friction: float, roughness: float, metallic: float, - mujoco_priority: int | None = None, ) -> AssetBaseCfg: - """Build a static colliding triangle-mesh configuration.""" - collision_props = sim_utils.CollisionBaseCfg() - if mujoco_priority is not None: - collision_props = [ - UsdPhysicsCollisionCfg(collision_enabled=True), - MujocoCollisionCfg(priority=mujoco_priority), - ] + """Build a non-colliding custom mesh used only for rendering.""" return AssetBaseCfg( prim_path=prim_path, spawn=sim_utils.MeshCustomCfg( vertices=spec.vertices, faces=spec.faces, - collision_props=collision_props, - physics_material=RigidBodyMaterialBaseCfg( - static_friction=friction, - dynamic_friction=friction, - restitution=0.0, - ), visual_material=sim_utils.PreviewSurfaceCfg( diffuse_color=color, roughness=roughness, @@ -316,6 +395,28 @@ def _static_mesh( ) +def _hidden_collision_mesh( + prim_path: str, + spec: MeshSpec, + friction: float, + mujoco_priority: int, +) -> AssetBaseCfg: + """Build a hidden static triangle-mesh collider.""" + spawn = sim_utils.MeshCustomCfg( + vertices=spec.vertices, + faces=spec.faces, + visible=False, + collision_props=_collision_properties(mujoco_priority=mujoco_priority), + physics_material=RigidBodyMaterialBaseCfg( + static_friction=friction, + dynamic_friction=friction, + restitution=0.0, + ), + ) + spawn.func = _spawn_hidden_collision_mesh + return AssetBaseCfg(prim_path=prim_path, spawn=spawn) + + def _cube( name: str, color: tuple[float, float, float], @@ -328,7 +429,7 @@ def _cube( ) spawn.rigid_props = _DYNAMIC_PROPERTIES spawn.mass_props = sim_utils.MassPropertiesCfg(mass=0.05) - spawn.collision_props = sim_utils.CollisionBaseCfg(contact_offset=0.0, rest_offset=0.0) + spawn.collision_props = _collision_properties(contact_margin=_CUBE_CONTACT_MARGIN) spawn.physics_material = NewtonMaterialPropertiesCfg( # The belt's higher MuJoCo contact priority overrides this friction # only for belt/cube pairs, leaving physical finger/cube friction. @@ -337,8 +438,8 @@ def _cube( restitution=0.0, torsional_friction=0.002, rolling_friction=0.0001, - contact_stiffness=1.0e4, - contact_damping=200.0, + contact_stiffness=2.5e3, + contact_damping=100.0, ) spawn.func = _spawn_shape_with_display_color return RigidObjectCfg( @@ -405,41 +506,65 @@ class ConveyorFrankaSceneCfg(InteractiveSceneCfg): ) def __post_init__(self) -> None: - """Generate both racetrack belts and their inner/outer guardrails.""" + """Generate visual belts, velocity-field collision sections, and guardrails.""" for side in ("Left", "Right"): belt_spec = belt_mesh_spec(side) setattr( self, - f"conveyor_{side.lower()}_belt", - _static_mesh( + f"conveyor_{side.lower()}_belt_visual", + _visual_mesh( prim_path=f"{{ENV_REGEX_NS}}/{belt_spec.name}", spec=belt_spec, color=BELT_COLOR, - # MuJoCo requires a tiny positive value even though the force driver, - # rather than solver friction, supplies the belt motion. - friction=1.1e-5, roughness=0.9, metallic=0.0, - # MuJoCo otherwise resolves equal-priority pair friction - # with max(belt, cube), pinning parcels to the static mesh. - mujoco_priority=1, ), ) + section_keys = ("top_straight", "bottom_straight", "right_turn", "left_turn") + for section_key, section in zip(section_keys, belt_collision_section_specs(side), strict=True): + spec = section.mesh + setattr( + self, + f"conveyor_{side.lower()}_{section_key}_collision", + _hidden_collision_mesh( + prim_path=f"{{ENV_REGEX_NS}}/{spec.name}", + spec=spec, + # MuJoCo requires a tiny positive value even though the force driver, + # rather than solver friction, supplies the belt motion. + friction=1.1e-5, + # Override cube friction only for collision-section/cube pairs. + mujoco_priority=1, + ), + ) + for spec in guard_mesh_specs(side): boundary = "inner" if spec.name.endswith("Inner") else "outer" setattr( self, - f"guard_{side.lower()}_{boundary}", - _static_mesh( - prim_path=f"{{ENV_REGEX_NS}}/{spec.name}", + f"guard_{side.lower()}_{boundary}_visual", + _visual_mesh( + prim_path=f"{{ENV_REGEX_NS}}/{spec.name}Visual", spec=spec, color=GUARD_COLOR, - friction=0.2, roughness=0.3, metallic=0.8, ), ) + setattr( + self, + f"guard_{side.lower()}_{boundary}_collision", + _hidden_collision_mesh( + prim_path=f"{{ENV_REGEX_NS}}/{spec.name}Collision", + spec=spec, + # The compact turns need freely sliding guide contacts; + # tangential rail friction can wedge a cube against the + # wall even though its belt drive remains valid. + friction=1.1e-5, + # Override the cube's grasp friction only for rail contacts. + mujoco_priority=1, + ), + ) @configclass @@ -455,7 +580,7 @@ class ConveyorFrankaEnvCfg(ManagerBasedRLEnvCfg): terminations: TerminationsCfg = TerminationsCfg() curriculum: CurriculumCfg = CurriculumCfg() decimation: int = 2 - episode_length_s: float = 10.0 + episode_length_s: float = _SUBGOAL_TIMEOUT_S * _TRANSFER_SEQUENCE_LENGTH sim: SimulationCfg = SimulationCfg( dt=1.0 / 120.0, @@ -464,23 +589,23 @@ class ConveyorFrankaEnvCfg(ManagerBasedRLEnvCfg): solver_cfg=MJWarpSolverCfg( solver="newton", integrator="implicitfast", - njmax=300, - nconmax=256, - impratio=10.0, + njmax=2000, + nconmax=1000, + impratio=0.1, cone="elliptic", - update_data_interval=2, + update_data_interval=1, iterations=100, - ls_iterations=15, + ls_iterations=50, ls_parallel=False, use_mujoco_contacts=False, ccd_iterations=35, ), collision_cfg=NewtonCollisionPipelineCfg(), - # Refresh contacts between the two 240 Hz solver substeps and - # preserve the authored surfaces without speculative separation. - collision_decimation=1, - default_shape_cfg=NewtonShapeCfg(margin=0.0, gap=0.0), - num_substeps=2, + # Manager decimation supplies the reference's two 120 Hz solves + # per 60 Hz policy step, with contacts refreshed before each solve. + collision_decimation=0, + default_shape_cfg=NewtonShapeCfg(margin=0.0, gap=_CONTACT_GAP, ke=2.5e3, kd=100.0), + num_substeps=1, use_cuda_graph=False, load_visual_shapes=True, ), @@ -503,9 +628,14 @@ def __post_init__(self) -> None: ) def play_mode(self) -> None: - """Evaluate complete transfers from randomized moving-belt starts.""" + """Run continuing transfers from randomized moving-belt starts.""" super().play_mode() self.scene.num_envs = min(self.scene.num_envs, 8) self.events.reset_from_state_table.params["fixed_recipe"] = int(mdp.ConveyorResetRecipe.BELT) self.events.reset_from_state_table.params["fixed_variant_id"] = mdp.BELT_DEPLOYMENT_VARIANT + # Successful placements already transition to a new commanded cube. + # Playback removes training-only refreshes and runs until physics leaves + # the recoverable workspace. + self.terminations.subgoal_time_out = None + self.terminations.transfer_sequence_time_out = None self.curriculum = None diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_geometry.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_geometry.py index c8ad11c20756..52b1ad524c52 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_geometry.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_geometry.py @@ -9,6 +9,7 @@ import math from dataclasses import dataclass +from typing import Literal BELT_COLOR = (0.09, 0.09, 0.09) """Dark-rubber color used by Newton's conveyor example.""" @@ -26,12 +27,18 @@ BELT_WIDTH = 0.15 BELT_THICKNESS = 0.04 BELT_TOP_Z = 0.04 -TURN_SEGMENT_COUNT = 48 +TURN_SEGMENT_COUNT = 96 + +# Collision surfaces extend underneath the rails and overlap at section seams. +# This keeps the dynamic parcels on a continuous +Z-facing surface without +# exposing the belt prism's vertical side faces to the contact solver. +BELT_COLLISION_OVERHANG = 0.02 +BELT_COLLISION_SEAM_OVERLAP = 0.004 GUARD_THICKNESS = 0.018 # Keep the rails below the parcel tops so both lanes remain easy to read from # the default oblique camera. -GUARD_HEIGHT = 0.035 +GUARD_HEIGHT = 0.02 GUARD_BASE_OVERLAP = 0.005 @@ -44,6 +51,25 @@ class MeshSpec: faces: tuple[tuple[int, int, int], ...] +@dataclass(frozen=True) +class ConveyorSectionSpec: + """Collision mesh and velocity field for one conveyor section. + + The direction and pivot are expressed in the collision prim's local frame. + Constant sections interpret ``direction`` as the unit travel direction; + pivot sections interpret it as the unit rotation axis and use ``radius`` + to convert commanded linear speed to angular speed. ``surface_normal`` is + also local, so rotated or inclined sections need no world-space special case. + """ + + mesh: MeshSpec + velocity_field_type: Literal["constant", "pivot"] + direction: tuple[float, float, float] + 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) + + def belt_direction(side: str) -> float: """Return ``1`` for clockwise motion and ``-1`` for counter-clockwise motion.""" if side == "Left": @@ -165,10 +191,10 @@ def _racetrack_prism_mesh( def belt_mesh_spec(side: str) -> MeshSpec: - """Build one seamless, watertight conveyor belt mesh.""" + """Build the seamless, watertight visual mesh for one conveyor belt.""" center_y = BELT_CENTER_Y if side == "Left" else -BELT_CENTER_Y return _racetrack_prism_mesh( - name=f"Conveyor{side}Belt", + name=f"Conveyor{side}BeltVisual", center_y=center_y, lateral_offset=0.0, width=BELT_WIDTH, @@ -177,6 +203,92 @@ def belt_mesh_spec(side: str) -> MeshSpec: ) +def _straight_collision_mesh(name: str, center_y: float) -> MeshSpec: + """Build one horizontal straight collision surface with +Z face winding.""" + half_width = 0.5 * BELT_WIDTH + BELT_COLLISION_OVERHANG + x_min = BELT_CENTER_X - BELT_HALF_STRAIGHT - BELT_COLLISION_SEAM_OVERLAP + x_max = BELT_CENTER_X + BELT_HALF_STRAIGHT + BELT_COLLISION_SEAM_OVERLAP + vertices = ( + (x_min, center_y - half_width, BELT_TOP_Z), + (x_max, center_y - half_width, BELT_TOP_Z), + (x_max, center_y + half_width, BELT_TOP_Z), + (x_min, center_y + half_width, BELT_TOP_Z), + ) + return MeshSpec(name=name, vertices=vertices, faces=((0, 1, 2), (0, 2, 3))) + + +def _turn_collision_mesh(name: str, pivot_x: float, center_y: float, start_angle: float) -> MeshSpec: + """Build one horizontal annular half-turn collision surface with +Z normals.""" + half_width = 0.5 * BELT_WIDTH + BELT_COLLISION_OVERHANG + inner_radius = BELT_TURN_RADIUS - half_width + outer_radius = BELT_TURN_RADIUS + half_width + angle_overlap = BELT_COLLISION_SEAM_OVERLAP / BELT_TURN_RADIUS + angle_start = start_angle - angle_overlap + angle_step = (math.pi + 2.0 * angle_overlap) / TURN_SEGMENT_COUNT + angles = tuple(angle_start + index * angle_step for index in range(TURN_SEGMENT_COUNT + 1)) + + inner = tuple( + (pivot_x + inner_radius * math.cos(angle), center_y + inner_radius * math.sin(angle), BELT_TOP_Z) + for angle in angles + ) + outer = tuple( + (pivot_x + outer_radius * math.cos(angle), center_y + outer_radius * math.sin(angle), BELT_TOP_Z) + for angle in angles + ) + outer_offset = len(inner) + faces: list[tuple[int, int, int]] = [] + for index in range(TURN_SEGMENT_COUNT): + next_index = index + 1 + faces.extend( + ( + (index, outer_offset + index, outer_offset + next_index), + (index, outer_offset + next_index, next_index), + ) + ) + return MeshSpec(name=name, vertices=inner + outer, faces=tuple(faces)) + + +def belt_collision_mesh_specs(side: str) -> tuple[MeshSpec, MeshSpec, MeshSpec, MeshSpec]: + """Build straight and pivot-field collision sections for one racetrack.""" + return tuple(section.mesh for section in belt_collision_section_specs(side)) + + +def belt_collision_section_specs( + side: str, +) -> tuple[ConveyorSectionSpec, ConveyorSectionSpec, ConveyorSectionSpec, ConveyorSectionSpec]: + """Build collision meshes and their matching conveyor velocity fields.""" + center_y = BELT_CENTER_Y if side == "Left" else -BELT_CENTER_Y + left_x = BELT_CENTER_X - BELT_HALF_STRAIGHT + right_x = BELT_CENTER_X + BELT_HALF_STRAIGHT + direction = belt_direction(side) + return ( + ConveyorSectionSpec( + mesh=_straight_collision_mesh(f"Conveyor{side}TopStraightCollision", center_y + BELT_TURN_RADIUS), + velocity_field_type="constant", + direction=(direction, 0.0, 0.0), + ), + ConveyorSectionSpec( + mesh=_straight_collision_mesh(f"Conveyor{side}BottomStraightCollision", center_y - BELT_TURN_RADIUS), + velocity_field_type="constant", + direction=(-direction, 0.0, 0.0), + ), + ConveyorSectionSpec( + mesh=_turn_collision_mesh(f"Conveyor{side}RightTurnCollision", right_x, center_y, -0.5 * math.pi), + velocity_field_type="pivot", + direction=(0.0, 0.0, -direction), + pivot_point=(right_x, center_y, 0.0), + radius=BELT_TURN_RADIUS, + ), + ConveyorSectionSpec( + mesh=_turn_collision_mesh(f"Conveyor{side}LeftTurnCollision", left_x, center_y, 0.5 * math.pi), + velocity_field_type="pivot", + direction=(0.0, 0.0, -direction), + pivot_point=(left_x, center_y, 0.0), + radius=BELT_TURN_RADIUS, + ), + ) + + def guard_mesh_specs(side: str) -> tuple[MeshSpec, MeshSpec]: """Build seamless inner and outer guardrail meshes for one racetrack.""" center_y = BELT_CENTER_Y if side == "Left" else -BELT_CENTER_Y diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/__init__.py index a1e89483ef1f..e96e26dc4871 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/__init__.py @@ -24,12 +24,15 @@ BELT_DEPLOYMENT_VARIANT, ConveyorResetRecipe, ConveyorResetStateTable, + advance_conveyor_transfer_goal, build_reset_rows, + select_next_transfer_cube, ) from .rewards import ( ConveyorTransferProgressReward, action_term_l2, finite_joint_velocity_l2, + physical_cube_acquisition_mask, terminal_failure, transfer_success_reward, ) @@ -39,4 +42,6 @@ StableConveyorTransfer, cube_out_of_workspace, nonfinite_scene_state, + subgoal_time_out, + transfer_sequence_time_out, ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/curriculums.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/curriculums.py index e7dc9180ff58..4efca2a70266 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/curriculums.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/curriculums.py @@ -96,7 +96,7 @@ def reset_sampling_probabilities( source_side_ids: torch.Tensor, attempts: torch.Tensor, successes: torch.Tensor, - deployment_probability: float, + deployment_probability: float | torch.Tensor, epsilon: float, ) -> torch.Tensor: """Mix guaranteed deployment starts with adaptive intermediate rows.""" @@ -109,7 +109,15 @@ def reset_sampling_probabilities( == successes.shape ): raise ValueError("Reset row metadata and outcomes must have matching shapes.") - if not 0.0 < deployment_probability < 1.0: + deployment_probability = torch.as_tensor( + deployment_probability, + dtype=torch.float32, + device=attempts.device, + ) + if deployment_probability.numel() != 1: + raise ValueError("deployment_probability must be a scalar.") + deployment_probability = deployment_probability.reshape(()) + if bool((deployment_probability <= 0.0) | (deployment_probability >= 1.0)): raise ValueError("deployment_probability must lie strictly between zero and one.") if epsilon <= 0.0: raise ValueError("epsilon must be positive.") @@ -143,6 +151,34 @@ def reset_sampling_probabilities( return adaptive + deployment +def deployment_probability_from_progress( + progress_rate: torch.Tensor, + row_coverage: torch.Tensor, + initial_probability: float = 0.35, + final_probability: float = 0.90, + progress_start: float = 0.45, + progress_end: float = 0.80, + coverage_target: float = 0.50, +) -> torch.Tensor: + """Interpolate deployment sampling from rolling competence and row coverage.""" + if progress_rate.numel() != 1 or row_coverage.numel() != 1: + raise ValueError("progress_rate and row_coverage must be scalar tensors.") + if not 0.0 < initial_probability <= final_probability < 1.0: + raise ValueError("Deployment probabilities must be ordered strictly inside (0, 1).") + if not 0.0 <= progress_start < progress_end <= 1.0: + raise ValueError("Deployment progress thresholds must be ordered inside [0, 1].") + if not 0.0 < coverage_target <= 1.0: + raise ValueError("coverage_target must lie inside (0, 1].") + if bool((progress_rate < 0.0) | (progress_rate > 1.0) | (row_coverage < 0.0) | (row_coverage > 1.0)): + raise ValueError("Rolling progress and row coverage must lie inside [0, 1].") + + progress_fraction = ((progress_rate - progress_start) / (progress_end - progress_start)).clamp(0.0, 1.0) + coverage_fraction = (row_coverage / coverage_target).clamp(0.0, 1.0) + readiness = progress_fraction * coverage_fraction + readiness = readiness.square() * (3.0 - 2.0 * readiness) + return initial_probability + (final_probability - initial_probability) * readiness + + class ConveyorResetCurriculum(ManagerTermBase): """Record row outcomes and sample the next physical reset states.""" @@ -179,10 +215,15 @@ def __call__( env: ManagerBasedRLEnv, env_ids: Sequence[int], progress_context_name: str = "learning_progress_context", - final_success_termination_name: str = "success", - deployment_probability: float = 0.35, + final_success_context_name: str = "transfer_success_context", + deployment_probability_initial: float = 0.35, + deployment_probability_final: float = 0.90, + deployment_progress_start: float = 0.45, + deployment_progress_end: float = 0.80, + deployment_coverage_target: float = 0.50, epsilon: float = 0.05, monitored_history_len: int = 50, + fixed_source_side_id: int | None = None, ) -> dict[str, torch.Tensor]: """Update adaptive evidence, sample rows, and expose diagnostics.""" del monitored_history_len @@ -194,7 +235,7 @@ def __call__( completed_ids = ids[completed] if completed_ids.numel(): progress_context = env.termination_manager.get_term_cfg(progress_context_name).func - final_success = env.termination_manager.get_term_cfg(final_success_termination_name).func + final_success = env.termination_manager.get_term_cfg(final_success_context_name).func progressed = progress_context.ever_success[completed_ids] succeeded = final_success.ever_success[completed_ids] rows = state.row_ids[completed_ids] @@ -213,6 +254,18 @@ def __call__( batch_progress = progressed.float().mean() batch_success = succeeded.float().mean() + attempted_rows = self._history_size > 0 + row_coverage = attempted_rows.float().mean() + total_progress = self._history_success_count.sum().float() / self._history_size.sum().clamp_min(1) + deployment_probability = deployment_probability_from_progress( + total_progress, + row_coverage, + initial_probability=deployment_probability_initial, + final_probability=deployment_probability_final, + progress_start=deployment_progress_start, + progress_end=deployment_progress_end, + coverage_target=deployment_coverage_target, + ) probabilities = reset_sampling_probabilities( self._reset_term.recipe_ids, self._reset_term.variant_ids, @@ -223,11 +276,14 @@ def __call__( deployment_probability, epsilon, ) + if fixed_source_side_id is not None: + if fixed_source_side_id not in (0, 1): + raise ValueError("fixed_source_side_id must be 0 (left) or 1 (right).") + probabilities *= self._reset_term.source_side_ids == fixed_source_side_id + probabilities /= probabilities.sum() if ids.numel(): state.row_ids[ids] = torch.multinomial(probabilities, ids.numel(), replacement=True) - attempted_rows = self._attempts > 0 - total_progress = self._history_success_count.sum().float() / self._history_size.sum().clamp_min(1) cumulative_progress = self._progress_successes.sum().float() / self._attempts.sum().clamp_min(1) total_success = self._final_successes.sum().float() / self._attempts.sum().clamp_min(1) entropy = -(probabilities * probabilities.clamp_min(torch.finfo(probabilities.dtype).tiny).log()).sum() @@ -235,7 +291,23 @@ def __call__( metrics: dict[str, torch.Tensor] = { "batch_progress_rate": batch_progress, "batch_success_rate": batch_success, - "row_coverage": attempted_rows.float().mean(), + "batch_transfer_count": ( + state.transfer_counts[completed_ids].float().mean() + if completed_ids.numel() + else torch.zeros((), dtype=torch.float32, device=env.device) + ), + "batch_left_to_right_transfers": ( + state.direction_transfer_counts[completed_ids, 0].float().mean() + if completed_ids.numel() + else torch.zeros((), dtype=torch.float32, device=env.device) + ), + "batch_right_to_left_transfers": ( + state.direction_transfer_counts[completed_ids, 1].float().mean() + if completed_ids.numel() + else torch.zeros((), dtype=torch.float32, device=env.device) + ), + "deployment_probability": deployment_probability, + "row_coverage": row_coverage, "overall_progress_rate": total_progress, "cumulative_progress_rate": cumulative_progress, "overall_success_rate": total_success, @@ -252,6 +324,17 @@ def __call__( metrics[f"recipe_{recipe.name.lower()}_success_rate"] = self._final_successes[ mask ].sum().float() / recipe_attempts.clamp_min(1) + for side_id, side_name in ((0, "left_to_right"), (1, "right_to_left")): + mask = self._reset_term.source_side_ids == side_id + attempts = self._attempts[mask].sum() + history_size = self._history_size[mask].sum() + metrics[f"direction_{side_name}_probability"] = probabilities[mask].sum() + metrics[f"direction_{side_name}_progress_rate"] = self._history_success_count[ + mask + ].sum().float() / history_size.clamp_min(1) + metrics[f"direction_{side_name}_success_rate"] = self._final_successes[ + mask + ].sum().float() / attempts.clamp_min(1) for recipe_name, variant_id, mask in self._diagnostic_variant_rows: attempts = self._attempts[mask].sum() history_size = self._history_size[mask].sum() diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/reset_events.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/reset_events.py index 84880be5e712..2da5f6f015f6 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/reset_events.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/reset_events.py @@ -3,7 +3,7 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Validated reset-state table for conveyor-to-conveyor transfer.""" +"""Reset-state and continuing-goal events for conveyor transfer.""" from __future__ import annotations @@ -32,6 +32,9 @@ LEFT_SIDE = 0 RIGHT_SIDE = 1 _BELT_CURRICULUM_FRACTIONS = (0.0, 0.15, 0.30, 0.45, 0.60, 0.80, 1.0) +_GRASP_CLOSURE_FRACTIONS = (0.25, 0.50, 0.75, 0.90, 1.0) +_OPEN_FINGER_POSITION = 0.040 +_CLOSED_FINGER_POSITION = 0.019 BELT_DEPLOYMENT_VARIANT = len(_BELT_CURRICULUM_FRACTIONS) - 1 @@ -56,6 +59,7 @@ class ConveyorResetRow: target_cube_id: int source_side_id: int arm_positions: tuple[float, ...] + finger_position: float held: bool belt_range_fraction: float @@ -121,14 +125,32 @@ def _arm_position_variants( if recipe == ConveyorResetRecipe.LIFT: return _interpolate_arm(source_grasp, source_lift, (0.25, 0.50, 0.75, 1.0)) if recipe == ConveyorResetRecipe.GRASP: - return (source_grasp,) + return (source_grasp,) * len(_GRASP_CLOSURE_FRACTIONS) if recipe == ConveyorResetRecipe.PREGRASP: - return _interpolate_arm(source_pregrasp, source_grasp, (0.0, 0.30, 0.55, 0.75, 0.88)) + # Include the exact open-gripper acquisition pose. The previous last + # row stopped at 88% of the approach, leaving an approximately 1 cm + # gap between reset-driven approach learning and the already-held + # GRASP row. Dense near-contact rows make the physical close-and-lift + # transition learnable from the same sparse delivery objective. + return _interpolate_arm(source_pregrasp, source_grasp, (0.0, 0.50, 0.75, 0.92, 1.0)) if recipe == ConveyorResetRecipe.BELT: return _interpolate_arm(source_pregrasp, _HOME_ARM, _BELT_CURRICULUM_FRACTIONS) raise ValueError(f"Unsupported reset recipe: {recipe}.") +def _finger_position_variants(recipe: ConveyorResetRecipe) -> tuple[float, ...]: + """Return finger positions paired with a recipe's arm variants [m].""" + variant_count = len(_arm_position_variants(recipe, LEFT_SIDE)) + if recipe == ConveyorResetRecipe.GRASP: + return tuple( + _OPEN_FINGER_POSITION + fraction * (_CLOSED_FINGER_POSITION - _OPEN_FINGER_POSITION) + for fraction in _GRASP_CLOSURE_FRACTIONS + ) + if recipe in (ConveyorResetRecipe.LIFT, ConveyorResetRecipe.CARRY, ConveyorResetRecipe.PLACE): + return (_CLOSED_FINGER_POSITION,) * variant_count + return (_OPEN_FINGER_POSITION,) * variant_count + + def reset_variant_counts() -> tuple[int, ...]: """Return the number of trajectory variants in each reset recipe.""" return tuple(len(_arm_position_variants(recipe, LEFT_SIDE)) for recipe in ConveyorResetRecipe) @@ -143,19 +165,19 @@ def build_reset_rows() -> tuple[ConveyorResetRow, ...]: target_cube_id=cube_id, source_side_id=source_side, arm_positions=arm_positions, - held=recipe - in ( - ConveyorResetRecipe.GRASP, - ConveyorResetRecipe.LIFT, - ConveyorResetRecipe.CARRY, - ConveyorResetRecipe.PLACE, + finger_position=finger_position, + held=( + recipe in (ConveyorResetRecipe.LIFT, ConveyorResetRecipe.CARRY, ConveyorResetRecipe.PLACE) + or (recipe == ConveyorResetRecipe.GRASP and variant_id == len(_GRASP_CLOSURE_FRACTIONS) - 1) ), belt_range_fraction=_BELT_CURRICULUM_FRACTIONS[variant_id] if recipe == ConveyorResetRecipe.BELT else 0.0, ) for recipe in ConveyorResetRecipe for cube_id in range(CUBE_COUNT) for source_side in (LEFT_SIDE, RIGHT_SIDE) - for variant_id, arm_positions in enumerate(_arm_position_variants(recipe, source_side)) + for variant_id, (arm_positions, finger_position) in enumerate( + zip(_arm_position_variants(recipe, source_side), _finger_position_variants(recipe), strict=True) + ) ) @@ -273,6 +295,9 @@ def __init__(self, cfg: EventTermCfg, env: ManagerBasedRLEnv): self._arm_positions = torch.tensor( [row.arm_positions for row in self._rows], dtype=torch.float32, device=env.device ) + self._finger_positions = torch.tensor( + [row.finger_position for row in self._rows], dtype=torch.float32, device=env.device + ) self._held_rows = torch.tensor([row.held for row in self._rows], dtype=torch.bool, device=env.device) self._belt_range_fractions = torch.tensor( [row.belt_range_fraction for row in self._rows], dtype=torch.float32, device=env.device @@ -369,18 +394,23 @@ def __call__( self._state.target_cube_ids[env_ids] = target_cube_ids self._state.source_side_ids[env_ids] = source_side_ids self._state.held_cube_ids[env_ids] = torch.where(held_rows, target_cube_ids, -1) + self._state.goal_ids[env_ids] = 0 + self._state.subgoal_start_steps[env_ids] = 0 + self._state.transfer_counts[env_ids] = 0 + self._state.direction_transfer_counts[env_ids] = 0 self._state.initialized[env_ids] = True arm_positions = self._arm_positions[row_ids].clone() if arm_joint_noise > 0.0: noise = (2.0 * torch.rand_like(arm_positions) - 1.0) * arm_joint_noise - noise[held_rows] = 0.0 + # Preserve the exact approach, closure, and held-object manifold. + # Only deployment-like BELT starts receive robot randomization. + noise[recipes != int(ConveyorResetRecipe.BELT)] = 0.0 arm_positions += noise joint_positions = self._robot.data.default_joint_pos.torch[env_ids].clone() joint_velocities = torch.zeros_like(joint_positions) joint_positions[:, self._arm_joint_ids] = arm_positions - finger_positions = torch.full((env_ids.numel(), 2), 0.04, dtype=joint_positions.dtype, device=self.device) - finger_positions[held_rows] = 0.019 + finger_positions = self._finger_positions[row_ids].to(dtype=joint_positions.dtype).unsqueeze(1).expand(-1, 2) joint_positions[:, self._finger_joint_ids] = finger_positions self._robot.set_joint_position_target_index(target=joint_positions, env_ids=env_ids) self._robot.set_joint_velocity_target_index(target=joint_velocities, env_ids=env_ids) @@ -442,3 +472,82 @@ def __call__( def reset(self, env_ids: Sequence[int] | None = None) -> None: """Keep the immutable reset table across environment resets.""" + + +def select_next_transfer_cube( + cube_positions: torch.Tensor, + current_cube_ids: torch.Tensor, + source_side_ids: torch.Tensor, + transit_half_width: float = 0.14, +) -> torch.Tensor: + """Sample the next numbered cube already located on each source belt. + + A different eligible cube is sampled uniformly. The just-placed cube is + the fallback, so a valid goal remains available even when it is temporarily + the only parcel on that conveyor. + """ + if cube_positions.ndim != 3 or cube_positions.shape[1:] != (CUBE_COUNT, 3): + raise ValueError(f"cube_positions must have shape (N, {CUBE_COUNT}, 3).") + count = cube_positions.shape[0] + if current_cube_ids.shape != (count,) or source_side_ids.shape != (count,): + raise ValueError("Current cube and source-side ids must match the position batch.") + if transit_half_width <= 0.0: + raise ValueError("transit_half_width must be positive.") + + cube_ids = torch.arange(CUBE_COUNT, device=cube_positions.device).expand(count, -1) + on_left = cube_positions[:, :, 1] > transit_half_width + on_right = cube_positions[:, :, 1] < -transit_half_width + candidates = torch.where(source_side_ids.unsqueeze(1) == LEFT_SIDE, on_left, on_right) + alternatives = candidates & (cube_ids != current_cube_ids.unsqueeze(1)) + candidates = torch.where(torch.any(alternatives, dim=1, keepdim=True), alternatives, candidates) + + has_candidates = torch.any(candidates, dim=1) + fallback = torch.zeros_like(candidates) + fallback.scatter_(1, current_cube_ids.unsqueeze(1), True) + candidates = torch.where(has_candidates.unsqueeze(1), candidates, fallback) + return torch.multinomial(candidates.float(), 1).squeeze(1) + + +def advance_conveyor_transfer_goal( + env: ManagerBasedRLEnv, + env_ids: torch.Tensor, + success_context_name: str = "transfer_success_context", + transit_half_width: float = 0.14, +) -> None: + """Consume completed transfers and command another cube in the reverse direction.""" + if env_ids is None: + env_ids = torch.arange(env.num_envs, device=env.device) + else: + env_ids = torch.as_tensor(env_ids, dtype=torch.long, device=env.device).flatten() + if env_ids.numel() == 0: + return + + context = env.termination_manager.get_term_cfg(success_context_name).func + if not hasattr(context, "pending_success") or not hasattr(context, "consume_success"): + raise RuntimeError("Continuing conveyor goals require a stable transfer-success context.") + completed_ids = env_ids[context.pending_success[env_ids]] + if completed_ids.numel() == 0: + return + + state = env.conveyor_transfer_state + cubes: tuple[RigidObject, ...] = tuple(env.scene[f"cube_{cube_id}"] for cube_id in range(CUBE_COUNT)) + positions = torch.stack(tuple(cube.data.root_pos_w.torch[completed_ids] for cube in cubes), dim=1) + positions -= env.scene.env_origins[completed_ids].unsqueeze(1) + previous_cube_ids = state.target_cube_ids[completed_ids].clone() + previous_source_side_ids = state.source_side_ids[completed_ids].clone() + next_source_side_ids = 1 - previous_source_side_ids + next_cube_ids = select_next_transfer_cube( + positions, + previous_cube_ids, + next_source_side_ids, + transit_half_width=transit_half_width, + ) + + state.direction_transfer_counts[completed_ids, previous_source_side_ids] += 1 + state.transfer_counts[completed_ids] += 1 + state.goal_ids[completed_ids] += 1 + state.subgoal_start_steps[completed_ids] = env.episode_length_buf[completed_ids] + state.target_cube_ids[completed_ids] = next_cube_ids + state.source_side_ids[completed_ids] = next_source_side_ids + state.held_cube_ids[completed_ids] = -1 + context.consume_success(completed_ids) diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/rewards.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/rewards.py index 77aa7bc68dad..349a730ddbcf 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/rewards.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/rewards.py @@ -92,15 +92,22 @@ def reset(self, env_ids: Sequence[int] | None = None) -> None: self._previous[env_ids] = current[env_ids] -def transfer_success_reward(env: ManagerBasedRLEnv, termination_name: str = "success") -> torch.Tensor: - """Return one on the transfer-completion transition.""" - return env.termination_manager.get_term(termination_name).float() +def transfer_success_reward( + env: ManagerBasedRLEnv, + context_term_name: str = "transfer_success_context", +) -> torch.Tensor: + """Return one on each stable transfer-completion transition.""" + context = env.termination_manager.get_term_cfg(context_term_name).func + return context.new_success.float() -def terminal_failure(env: ManagerBasedRLEnv, success_termination_name: str = "success") -> torch.Tensor: +def terminal_failure( + env: ManagerBasedRLEnv, + success_context_name: str = "transfer_success_context", +) -> torch.Tensor: """Return one for non-timeout terminal failures.""" - succeeded = env.termination_manager.get_term(success_termination_name) - return (env.reset_terminated & ~succeeded).float() + success_context = env.termination_manager.get_term_cfg(success_context_name).func + return (env.reset_terminated & ~success_context.pending_success).float() def action_term_l2(env: ManagerBasedRLEnv, action_name: str) -> torch.Tensor: @@ -109,6 +116,32 @@ def action_term_l2(env: ManagerBasedRLEnv, action_name: str) -> torch.Tensor: return torch.sum(torch.square(action), dim=1) +def physical_cube_acquisition_mask( + env: ManagerBasedRLEnv, + minimum_lift: float = 0.025, + maximum_tool_distance: float = 0.075, + maximum_finger_position: float = 0.030, +) -> torch.Tensor: + """Return physically closed, lifted, tool-local commanded-cube grasps.""" + if minimum_lift <= 0.0 or maximum_tool_distance <= 0.0 or maximum_finger_position <= 0.0: + raise ValueError("Physical acquisition thresholds must be positive.") + state = env.conveyor_transfer_state + cubes: tuple[RigidObject, ...] = tuple(env.scene[f"cube_{cube_id}"] for cube_id in range(CUBE_COUNT)) + positions = torch.stack(tuple(cube.data.root_pos_w.torch for cube in cubes), dim=1) + index = state.target_cube_ids.view(env.num_envs, 1, 1).expand(-1, 1, 3) + active_position = torch.gather(positions, 1, index).squeeze(1) + tool_position, _ = end_effector_pose(env) + robot: Articulation = env.scene["robot"] + finger_ids, _ = robot.find_joints("panda_finger_joint[1-2]", preserve_order=True) + finger_positions = robot.data.joint_pos.torch[:, finger_ids] + local_cube_z = active_position[:, 2] - env.scene.env_origins[:, 2] + return ( + (local_cube_z >= CUBE_REST_Z + minimum_lift) + & (torch.linalg.vector_norm(active_position - tool_position, dim=1) <= maximum_tool_distance) + & (torch.amax(finger_positions, dim=1) <= maximum_finger_position) + ) + + def finite_joint_velocity_l2( env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot", joint_names=["panda_joint[1-7]"]), diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/state.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/state.py index 4d153aa81e78..434d08105146 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/state.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/state.py @@ -18,13 +18,17 @@ @dataclass class ConveyorTransferState: - """Episode-local transfer command and reset metadata.""" + """Episode-local transfer command, reset metadata, and subgoal progress.""" row_ids: torch.Tensor recipe_ids: torch.Tensor target_cube_ids: torch.Tensor source_side_ids: torch.Tensor held_cube_ids: torch.Tensor + goal_ids: torch.Tensor + subgoal_start_steps: torch.Tensor + transfer_counts: torch.Tensor + direction_transfer_counts: torch.Tensor initialized: torch.Tensor @@ -36,6 +40,10 @@ def create_transfer_state(env: ManagerBasedRLEnv, row_count: int) -> ConveyorTra target_cube_ids=torch.zeros(env.num_envs, dtype=torch.long, device=env.device), source_side_ids=torch.zeros(env.num_envs, dtype=torch.long, device=env.device), held_cube_ids=torch.full((env.num_envs,), -1, dtype=torch.long, device=env.device), + goal_ids=torch.zeros(env.num_envs, dtype=torch.long, device=env.device), + subgoal_start_steps=torch.zeros(env.num_envs, dtype=torch.long, device=env.device), + transfer_counts=torch.zeros(env.num_envs, dtype=torch.long, device=env.device), + direction_transfer_counts=torch.zeros((env.num_envs, 2), dtype=torch.long, device=env.device), initialized=torch.zeros(env.num_envs, dtype=torch.bool, device=env.device), ) env.conveyor_transfer_state = state diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/terminations.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/terminations.py index 280a1d823dc3..290ea1c66eee 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/terminations.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/terminations.py @@ -7,6 +7,7 @@ from __future__ import annotations +import math from collections.abc import Sequence from typing import TYPE_CHECKING @@ -14,16 +15,25 @@ from isaaclab.managers import ManagerTermBase, SceneEntityCfg, TerminationTermCfg -from ..conveyor_geometry import BELT_CENTER_X, BELT_HALF_STRAIGHT +from ..conveyor_geometry import ( + BELT_CENTER_X, + BELT_HALF_STRAIGHT, + BELT_TURN_RADIUS, + BELT_WIDTH, + GUARD_THICKNESS, +) from .kinematics import end_effector_pose -from .reset_events import CUBE_COUNT, side_inner_y -from .rewards import current_transfer_potential +from .reset_events import CUBE_COUNT, CUBE_SIZE, ConveyorResetRecipe, side_inner_y +from .rewards import current_transfer_potential, physical_cube_acquisition_mask if TYPE_CHECKING: from isaaclab.assets import Articulation, RigidObject from isaaclab.envs import ManagerBasedRLEnv +_TRACK_X_CLEARANCE = BELT_TURN_RADIUS + 0.5 * BELT_WIDTH + GUARD_THICKNESS + CUBE_SIZE + + def transfer_success_mask( cube_positions: torch.Tensor, cube_linear_velocities: torch.Tensor, @@ -47,12 +57,16 @@ def transfer_success_mask( class StableConveyorTransfer(ManagerTermBase): - """Require a released destination-belt placement for consecutive steps.""" + """Track stable released placements without terminating the episode.""" def __init__(self, cfg: TerminationTermCfg, env: ManagerBasedRLEnv): super().__init__(cfg, env) self._stable_steps = torch.zeros(env.num_envs, dtype=torch.long, device=env.device) + self.is_success = torch.zeros(env.num_envs, dtype=torch.bool, device=env.device) + self.new_success = torch.zeros_like(self.is_success) + self.pending_success = torch.zeros_like(self.is_success) self.ever_success = torch.zeros(env.num_envs, dtype=torch.bool, device=env.device) + self._no_termination = torch.zeros_like(self.is_success) def __call__( self, @@ -64,7 +78,7 @@ def __call__( minimum_finger_position: float = 0.027, minimum_tool_clearance: float = 0.055, ) -> torch.Tensor: - """Return stable transfer success for each environment.""" + """Update current, edge-triggered, and sticky transfer success state.""" if minimum_episode_steps < 0 or hold_steps < 1: raise ValueError("minimum_episode_steps must be non-negative and hold_steps must be positive.") state = env.conveyor_transfer_state @@ -90,18 +104,52 @@ def __call__( minimum_finger_position=minimum_finger_position, minimum_tool_clearance=minimum_tool_clearance, ) - successful &= env.episode_length_buf >= minimum_episode_steps + subgoal_steps = env.episode_length_buf - state.subgoal_start_steps + successful &= subgoal_steps >= minimum_episode_steps self._stable_steps = torch.where(successful, self._stable_steps + 1, torch.zeros_like(self._stable_steps)) stable = self._stable_steps >= hold_steps - self.ever_success |= stable - return stable + self.new_success.copy_(stable & ~self.is_success & ~self.pending_success) + self.is_success.copy_(stable) + self.pending_success |= self.new_success + self.ever_success |= self.new_success + env.extras["successes"] = self.ever_success + return self._no_termination - def reset(self, env_ids: Sequence[int] | None = None) -> None: - """Clear success history for selected environments.""" - if env_ids is None: - env_ids = slice(None) + def consume_success(self, env_ids: Sequence[int]) -> None: + """Clear per-subgoal success state after the goal-transition event.""" self._stable_steps[env_ids] = 0 - self.ever_success[env_ids] = False + self.is_success[env_ids] = False + self.new_success[env_ids] = False + self.pending_success[env_ids] = False + + def reset(self, env_ids: Sequence[int] | None = None) -> None: + """Log episode success and clear history for selected environments.""" + successes = self.ever_success if env_ids is None else self.ever_success[env_ids] + if successes.numel() > 0: + self._env.extras.setdefault("log", {})["Metrics/success_rate"] = successes.float().mean().item() + + reset_ids = slice(None) if env_ids is None else env_ids + self._stable_steps[reset_ids] = 0 + self.is_success[reset_ids] = False + self.new_success[reset_ids] = False + self.pending_success[reset_ids] = False + self.ever_success[reset_ids] = False + + +def subgoal_time_out(env: ManagerBasedRLEnv, timeout_s: float = 20.0) -> torch.Tensor: + """Truncate environments that make no transfer within one subgoal timeout [s].""" + if timeout_s <= 0.0: + raise ValueError("timeout_s must be positive.") + timeout_steps = math.ceil(timeout_s / env.step_dt) + state = env.conveyor_transfer_state + return env.episode_length_buf - state.subgoal_start_steps >= timeout_steps + + +def transfer_sequence_time_out(env: ManagerBasedRLEnv, maximum_transfers: int = 8) -> torch.Tensor: + """Truncate long successful sequences so reset coverage remains fresh.""" + if maximum_transfers < 1: + raise ValueError("maximum_transfers must be positive.") + return env.conveyor_transfer_state.transfer_counts >= maximum_transfers class ConveyorResetLearningProgress(ManagerTermBase): @@ -128,12 +176,35 @@ def __call__( minimum_episode_steps: int = 3, minimum_progress: float = 0.35, maximum_target_potential: float = 5.0, + minimum_acquisition_lift: float = 0.025, + maximum_acquisition_tool_distance: float = 0.075, + maximum_acquisition_finger_position: float = 0.030, ) -> torch.Tensor: """Update sticky row-progress evidence and return an all-false mask.""" - if minimum_episode_steps < 0 or minimum_progress <= 0.0 or maximum_target_potential <= 0.0: + if ( + minimum_episode_steps < 0 + or minimum_progress <= 0.0 + or maximum_target_potential <= 0.0 + or minimum_acquisition_lift <= 0.0 + or maximum_acquisition_tool_distance <= 0.0 + or maximum_acquisition_finger_position <= 0.0 + ): raise ValueError("Invalid conveyor reset-learning progress thresholds.") current = current_transfer_potential(env) reached = (current >= self._target_potential) & (env.episode_length_buf >= minimum_episode_steps) + state = env.conveyor_transfer_state + acquisition_recipe = ( + (state.recipe_ids == int(ConveyorResetRecipe.GRASP)) + | (state.recipe_ids == int(ConveyorResetRecipe.PREGRASP)) + | (state.recipe_ids == int(ConveyorResetRecipe.BELT)) + ) + physically_acquired = physical_cube_acquisition_mask( + env, + minimum_lift=minimum_acquisition_lift, + maximum_tool_distance=maximum_acquisition_tool_distance, + maximum_finger_position=maximum_acquisition_finger_position, + ) + reached &= ~acquisition_recipe | physically_acquired self.is_success.copy_(reached) self.new_success.copy_(reached & ~self.ever_success) self.ever_success |= reached @@ -154,10 +225,18 @@ def reset(self, env_ids: Sequence[int] | None = None) -> None: def cube_out_of_workspace( env: ManagerBasedRLEnv, - minimum: tuple[float, float, float] = (-0.10, -1.05, -0.05), - maximum: tuple[float, float, float] = (1.30, 1.05, 0.80), + minimum: tuple[float, float, float] = ( + BELT_CENTER_X - BELT_HALF_STRAIGHT - _TRACK_X_CLEARANCE, + -1.05, + -0.05, + ), + maximum: tuple[float, float, float] = ( + BELT_CENTER_X + BELT_HALF_STRAIGHT + _TRACK_X_CLEARANCE, + 1.05, + 0.80, + ), ) -> torch.Tensor: - """Terminate when any cube leaves the recoverable workspace.""" + """Terminate when any cube leaves the complete guarded racetrack workspace.""" cubes: tuple[RigidObject, ...] = tuple(env.scene[f"cube_{cube_id}"] for cube_id in range(CUBE_COUNT)) positions = torch.stack(tuple(cube.data.root_pos_w.torch for cube in cubes), dim=1) positions -= env.scene.env_origins.unsqueeze(1) diff --git a/source/isaaclab_tasks/test/contrib/test_conveyor_franka_geometry.py b/source/isaaclab_tasks/test/contrib/test_conveyor_franka_geometry.py index a9889e919de6..352c6d344b64 100644 --- a/source/isaaclab_tasks/test/contrib/test_conveyor_franka_geometry.py +++ b/source/isaaclab_tasks/test/contrib/test_conveyor_franka_geometry.py @@ -8,13 +8,23 @@ import sys from collections import Counter +import numpy as np import pytest +import warp as wp +from isaaclab_tasks.contrib.conveyor_franka.conveyor_force_driver import ( + BeltContact, + _integrate_encoders, + _prepare_contact_patches, + _update_effective_velocities, +) from isaaclab_tasks.contrib.conveyor_franka.conveyor_franka_env_cfg import ConveyorForceCfg, ConveyorFrankaEnvCfg from isaaclab_tasks.contrib.conveyor_franka.conveyor_geometry import ( BELT_TOP_Z, + BELT_TURN_RADIUS, TURN_SEGMENT_COUNT, MeshSpec, + belt_collision_section_specs, belt_direction, belt_mesh_spec, guard_mesh_specs, @@ -68,6 +78,97 @@ def test_racetrack_lanes_counter_rotate(): assert belt_direction("Left") == -belt_direction("Right") +def test_collision_sections_and_velocity_fields_share_one_description(): + """Straight and curved force fields stay aligned with their collision meshes.""" + for side in ("Left", "Right"): + sections = belt_collision_section_specs(side) + + assert len(sections) == 4 + assert [section.velocity_field_type for section in sections] == ["constant", "constant", "pivot", "pivot"] + assert all(section.radius == BELT_TURN_RADIUS for section in sections[2:]) + assert sections[0].direction == tuple(-value for value in sections[1].direction) + assert sections[2].direction == sections[3].direction + + +def _make_belt_contact(conveyor: int, force: float, next_contact: int) -> BeltContact: + """Build one horizontal contact for the patch-normalization kernel.""" + contact = BeltContact() + contact.valid = 1 + contact.body = 0 + contact.conveyor = conveyor + contact.point = wp.vec3() + contact.normal = wp.vec3(0.0, 0.0, 1.0) + contact.normal_force = force + contact.next_body_contact = next_contact + return contact + + +@pytest.mark.parametrize( + ("conveyors", "expected_forces"), + (((0, 0), (4.0, 6.0)), ((0, 1), (5.0, 5.0))), +) +def test_contact_patch_normalizes_only_across_overlapping_sections(conveyors, expected_forces): + """A seam preserves total load without perturbing contacts on one section.""" + contacts = wp.array( + [ + _make_belt_contact(conveyors[0], 4.0, 1), + _make_belt_contact(conveyors[1], 6.0, -1), + ], + dtype=BeltContact, + device="cpu", + ) + body_contact_head = wp.array([0], dtype=wp.int32, device="cpu") + body_q = wp.array([wp.transform()], dtype=wp.transform, device="cpu") + body_com = wp.array([wp.vec3()], dtype=wp.vec3, device="cpu") + patch_head = wp.full(2, -1, dtype=wp.int32, device="cpu") + adjusted_force = wp.zeros(2, dtype=wp.float32, device="cpu") + splitting_scale = wp.zeros(2, dtype=wp.float32, device="cpu") + + wp.launch( + _prepare_contact_patches, + dim=1, + inputs=[contacts, body_contact_head, body_q, body_com], + outputs=[patch_head, adjusted_force, splitting_scale], + device="cpu", + ) + + np.testing.assert_allclose(adjusted_force.numpy(), expected_forces) + np.testing.assert_allclose(splitting_scale.numpy(), (0.5, 0.5)) + np.testing.assert_allclose(adjusted_force.numpy().sum(), 10.0) + + +def test_disabled_surface_remembers_command_and_stops_encoder(): + """One effective-speed seam drives both traction and encoder state.""" + commanded = wp.array([2.0], dtype=wp.float32, device="cpu") + enabled = wp.array([0], dtype=wp.int32, device="cpu") + effective = wp.zeros(1, dtype=wp.float32, device="cpu") + encoder = wp.zeros(1, dtype=wp.float32, device="cpu") + + wp.launch( + _update_effective_velocities, + dim=1, + inputs=[commanded, enabled], + outputs=[effective], + device="cpu", + ) + wp.launch(_integrate_encoders, dim=1, inputs=[0.5, effective], outputs=[encoder], device="cpu") + np.testing.assert_allclose(effective.numpy(), (0.0,)) + np.testing.assert_allclose(encoder.numpy(), (0.0,)) + + commanded.assign(np.array([3.0], dtype=np.float32)) + enabled.fill_(1) + wp.launch( + _update_effective_velocities, + dim=1, + inputs=[commanded, enabled], + outputs=[effective], + device="cpu", + ) + wp.launch(_integrate_encoders, dim=1, inputs=[0.5, effective], outputs=[encoder], device="cpu") + np.testing.assert_allclose(effective.numpy(), (3.0,)) + np.testing.assert_allclose(encoder.numpy(), (1.5,)) + + def test_environment_config_without_optional_visualizers(monkeypatch): """The task configuration remains usable without the visualizer package.""" monkeypatch.setitem(sys.modules, "isaaclab_visualizers", None) @@ -79,9 +180,16 @@ def test_environment_config_without_optional_visualizers(monkeypatch): @pytest.mark.parametrize( ("parameter", "value"), - (("speed", -0.1), ("friction", -0.1), ("normal_threshold", 1.1)), + ( + ("speed", -0.1), + ("friction", -0.1), + ("normal_threshold", 1.1), + ("startup_duration_s", 0.0), + ("transported_body_count_per_env", 0), + ("transported_body_pattern", "["), + ), ) -def test_conveyor_force_config_rejects_invalid_values(parameter: str, value: float): +def test_conveyor_force_config_rejects_invalid_values(parameter: str, value: object): """Verify force configuration rejects values outside its physical domain.""" with pytest.raises(ValueError): ConveyorForceCfg(**{parameter: value}) diff --git a/source/isaaclab_tasks/test/contrib/test_conveyor_franka_mdp.py b/source/isaaclab_tasks/test/contrib/test_conveyor_franka_mdp.py index 9ce735a0e53e..15cf26a3b3b1 100644 --- a/source/isaaclab_tasks/test/contrib/test_conveyor_franka_mdp.py +++ b/source/isaaclab_tasks/test/contrib/test_conveyor_franka_mdp.py @@ -6,16 +6,19 @@ """Unit tests for conveyor-transfer state, curriculum, and success geometry.""" from collections import Counter +from types import SimpleNamespace import torch from isaaclab_newton.sim.schemas import MujocoCollisionCfg, NewtonMaterialPropertiesCfg from isaaclab_tasks.contrib.conveyor_franka.agents.rsl_rl_ppo_cfg import ( + ConveyorFrankaPPORunnerCfg, ConveyorGaussianBernoulliDistribution, ) from isaaclab_tasks.contrib.conveyor_franka.conveyor_franka_env_cfg import ConveyorFrankaEnvCfg from isaaclab_tasks.contrib.conveyor_franka.mdp.curriculums import ( _ring_append_bool_count_rate, + deployment_probability_from_progress, reset_sampling_probabilities, ) from isaaclab_tasks.contrib.conveyor_franka.mdp.observations import classify_cube_conveyors @@ -26,15 +29,20 @@ build_reset_rows, franka_tool_position, reset_variant_counts, + select_next_transfer_cube, ) from isaaclab_tasks.contrib.conveyor_franka.mdp.rewards import transfer_potential -from isaaclab_tasks.contrib.conveyor_franka.mdp.terminations import transfer_success_mask +from isaaclab_tasks.contrib.conveyor_franka.mdp.terminations import ( + subgoal_time_out, + transfer_sequence_time_out, + transfer_success_mask, +) def test_contact_and_drive_cfg_preserve_transport_and_grasp_friction(): """Belt contact precedence must not weaken the cube's grasp material.""" cfg = ConveyorFrankaEnvCfg() - belt_collision = cfg.scene.conveyor_left_belt.spawn.collision_props + belt_collision = cfg.scene.conveyor_left_top_straight_collision.spawn.collision_props belt_mujoco = next(fragment for fragment in belt_collision if isinstance(fragment, MujocoCollisionCfg)) cube_material = cfg.scene.cube_0.spawn.physics_material hand_actuator = cfg.scene.robot.actuators["panda_hand"] @@ -42,12 +50,12 @@ def test_contact_and_drive_cfg_preserve_transport_and_grasp_friction(): assert belt_mujoco.priority == 1 assert isinstance(cube_material, NewtonMaterialPropertiesCfg) assert cube_material.dynamic_friction == 0.6 - assert cube_material.contact_stiffness == 1.0e4 - assert cube_material.contact_damping == 200.0 + assert cube_material.contact_stiffness == 2.5e3 + assert cube_material.contact_damping == 100.0 assert hand_actuator.stiffness == 350.0 assert hand_actuator.damping == 10.0 assert cfg.actions.arm_action.gravity_compensation - assert cfg.sim.physics.collision_decimation == 1 + assert cfg.sim.physics.collision_decimation == 0 def test_reset_rows_cover_every_cube_direction_and_phase_once(): @@ -62,13 +70,24 @@ def test_reset_rows_cover_every_cube_direction_and_phase_once(): for cube_id in range(CUBE_COUNT) for side_id in range(2) ) - held_recipes = { - ConveyorResetRecipe.GRASP, - ConveyorResetRecipe.LIFT, - ConveyorResetRecipe.CARRY, - ConveyorResetRecipe.PLACE, - } - assert all(row.held == (row.recipe in held_recipes) for row in rows) + for row in rows: + expected_held = row.recipe in { + ConveyorResetRecipe.LIFT, + ConveyorResetRecipe.CARRY, + ConveyorResetRecipe.PLACE, + } or ( + row.recipe == ConveyorResetRecipe.GRASP + and row.variant_id == reset_variant_counts()[int(ConveyorResetRecipe.GRASP)] - 1 + ) + assert row.held == expected_held + + grasp_rows = [ + row + for row in rows + if row.recipe == ConveyorResetRecipe.GRASP and row.target_cube_id == 0 and row.source_side_id == 0 + ] + assert all(first.finger_position > second.finger_position for first, second in zip(grasp_rows, grasp_rows[1:])) + assert grasp_rows[-1].held def test_reset_arm_anchors_reach_expected_transfer_waypoints(): @@ -205,6 +224,9 @@ def test_reset_sampling_guarantees_deployment_mass_and_tracks_frontier(): attempts[place_ids[1]] = 100 successes[place_ids[1]] = 50 + deployment_rows = (recipe_ids == int(ConveyorResetRecipe.BELT)) & ( + variant_ids == reset_variant_counts()[int(ConveyorResetRecipe.BELT)] - 1 + ) probabilities = reset_sampling_probabilities( recipe_ids, variant_ids, @@ -215,9 +237,6 @@ def test_reset_sampling_guarantees_deployment_mass_and_tracks_frontier(): deployment_probability=0.35, epsilon=0.05, ) - deployment_rows = (recipe_ids == int(ConveyorResetRecipe.BELT)) & ( - variant_ids == reset_variant_counts()[int(ConveyorResetRecipe.BELT)] - 1 - ) torch.testing.assert_close(probabilities.sum(), torch.tensor(1.0)) torch.testing.assert_close(probabilities[deployment_rows].sum(), torch.tensor(0.35)) @@ -225,21 +244,71 @@ def test_reset_sampling_guarantees_deployment_mass_and_tracks_frontier(): assert probabilities[place_ids[0]] < probabilities[place_ids[1]] for recipe in ConveyorResetRecipe: for cube_id in range(CUBE_COUNT): - for source_side_id in range(2): - stratum_rows = ( - (recipe_ids == int(recipe)) & (target_cube_ids == cube_id) & (source_side_ids == source_side_id) - ) + for side_id in range(2): + stratum_rows = (recipe_ids == int(recipe)) & (target_cube_ids == cube_id) & (source_side_ids == side_id) torch.testing.assert_close( probabilities[stratum_rows & ~deployment_rows].sum(), - torch.tensor(0.65 / (len(ConveyorResetRecipe) * 2 * CUBE_COUNT)), + torch.tensor(0.65 / (len(ConveyorResetRecipe) * CUBE_COUNT * 2)), ) - for cube_id in range(CUBE_COUNT): - for source_side_id in range(2): - command_rows = (target_cube_ids == cube_id) & (source_side_ids == source_side_id) - torch.testing.assert_close( - probabilities[command_rows & deployment_rows].sum(), - torch.tensor(0.35 / (2 * CUBE_COUNT)), - ) + torch.testing.assert_close( + probabilities[deployment_rows & (source_side_ids == 0)].sum(), + torch.tensor(0.35 / 2), + ) + torch.testing.assert_close( + probabilities[deployment_rows & (source_side_ids == 1)].sum(), + torch.tensor(0.35 / 2), + ) + + +def test_deployment_probability_increases_with_rolling_readiness(): + """Mastered, well-covered reset rows shift sampling toward deployment starts.""" + kwargs = { + "initial_probability": 0.35, + "final_probability": 0.90, + "progress_start": 0.45, + "progress_end": 0.80, + "coverage_target": 0.50, + } + + initial = deployment_probability_from_progress(torch.tensor(0.30), torch.tensor(1.0), **kwargs) + middle = deployment_probability_from_progress(torch.tensor(0.625), torch.tensor(0.50), **kwargs) + final = deployment_probability_from_progress(torch.tensor(0.90), torch.tensor(1.0), **kwargs) + + torch.testing.assert_close(initial, torch.tensor(0.35)) + torch.testing.assert_close(middle, torch.tensor(0.625)) + torch.testing.assert_close(final, torch.tensor(0.90)) + + +def test_next_transfer_cube_is_random_among_eligible_alternatives(): + """Continuing commands use the one-hot target instead of a cyclic identity shortcut.""" + count = 4096 + positions = torch.zeros((count, CUBE_COUNT, 3)) + positions[:, :, 1] = torch.tensor((-0.27, -0.27, -0.27, 0.27)) + current_cube_ids = torch.zeros(count, dtype=torch.long) + source_side_ids = torch.ones(count, dtype=torch.long) + torch.manual_seed(7) + + selected = select_next_transfer_cube(positions, current_cube_ids, source_side_ids) + + assert set(selected.tolist()) == {1, 2} + frequencies = torch.bincount(selected, minlength=CUBE_COUNT).float() / count + assert torch.all(torch.abs(frequencies[1:3] - 0.5) < 0.05) + + +def test_continuing_training_truncates_only_stalled_or_long_sequences(): + """Subgoal and sequence limits bound training without ending successful transfers.""" + assert not ConveyorFrankaPPORunnerCfg().init_at_random_ep_len + env = SimpleNamespace( + step_dt=0.1, + episode_length_buf=torch.tensor((199, 200, 450)), + conveyor_transfer_state=SimpleNamespace( + subgoal_start_steps=torch.tensor((0, 0, 300)), + transfer_counts=torch.tensor((0, 7, 8)), + ), + ) + + assert subgoal_time_out(env, timeout_s=20.0).tolist() == [False, True, False] + assert transfer_sequence_time_out(env, maximum_transfers=8).tolist() == [False, False, True] def test_rolling_progress_monitor_forgets_stale_outcomes_in_order(): diff --git a/uv.lock b/uv.lock index 427a0541dbb4..54d4f65bc773 100644 --- a/uv.lock +++ b/uv.lock @@ -1765,7 +1765,7 @@ wheels = [ [[package]] name = "isaaclab" -version = "15.4.0" +version = "15.6.0" source = { editable = "source/isaaclab" } [[package]] @@ -1785,7 +1785,7 @@ requires-dist = [ [[package]] name = "isaaclab-contrib" -version = "1.1.0" +version = "1.3.0" source = { editable = "source/isaaclab_contrib" } dependencies = [ { name = "isaaclab", marker = "(platform_machine == 'aarch64' and sys_platform == 'linux') or (platform_machine == 'x86_64' and sys_platform == 'linux') or (platform_machine == 'AMD64' and sys_platform == 'win32')" }, @@ -2115,7 +2115,7 @@ requires-dist = [{ name = "isaaclab", editable = "source/isaaclab" }] [[package]] name = "isaaclab-mimic" -version = "2.0.3" +version = "2.0.4" source = { editable = "source/isaaclab_mimic" } dependencies = [ { name = "isaaclab", marker = "(platform_machine == 'aarch64' and sys_platform == 'linux') or (platform_machine == 'x86_64' and sys_platform == 'linux') or (platform_machine == 'AMD64' and sys_platform == 'win32')" }, @@ -2132,7 +2132,7 @@ requires-dist = [ [[package]] name = "isaaclab-newton" -version = "3.0.0" +version = "3.2.1" source = { editable = "source/isaaclab_newton" } dependencies = [ { name = "isaaclab", marker = "(platform_machine == 'aarch64' and sys_platform == 'linux') or (platform_machine == 'x86_64' and sys_platform == 'linux') or (platform_machine == 'AMD64' and sys_platform == 'win32')" }, @@ -2143,7 +2143,7 @@ requires-dist = [{ name = "isaaclab", editable = "source/isaaclab" }] [[package]] name = "isaaclab-ov" -version = "0.10.4" +version = "0.10.5" source = { editable = "source/isaaclab_ov" } dependencies = [ { name = "isaaclab", marker = "(platform_machine == 'aarch64' and sys_platform == 'linux') or (platform_machine == 'x86_64' and sys_platform == 'linux') or (platform_machine == 'AMD64' and sys_platform == 'win32')" }, @@ -2158,7 +2158,7 @@ requires-dist = [ [[package]] name = "isaaclab-ovphysx" -version = "8.2.4" +version = "8.3.0" source = { editable = "source/isaaclab_ovphysx" } dependencies = [ { name = "isaaclab", marker = "(platform_machine == 'aarch64' and sys_platform == 'linux') or (platform_machine == 'x86_64' and sys_platform == 'linux') or (platform_machine == 'AMD64' and sys_platform == 'win32')" }, @@ -2173,7 +2173,7 @@ requires-dist = [ [[package]] name = "isaaclab-physx" -version = "4.1.0" +version = "4.2.1" source = { editable = "source/isaaclab_physx" } dependencies = [ { name = "isaaclab", marker = "(platform_machine == 'aarch64' and sys_platform == 'linux') or (platform_machine == 'x86_64' and sys_platform == 'linux') or (platform_machine == 'AMD64' and sys_platform == 'win32')" }, @@ -2195,7 +2195,7 @@ requires-dist = [{ name = "isaaclab", editable = "source/isaaclab" }] [[package]] name = "isaaclab-rl" -version = "0.13.0" +version = "0.14.0" source = { editable = "source/isaaclab_rl" } dependencies = [ { name = "isaaclab", marker = "(platform_machine == 'aarch64' and sys_platform == 'linux') or (platform_machine == 'x86_64' and sys_platform == 'linux') or (platform_machine == 'AMD64' and sys_platform == 'win32')" }, @@ -2212,17 +2212,19 @@ requires-dist = [ [[package]] name = "isaaclab-tasks" -version = "13.0.0" +version = "15.0.0" source = { editable = "source/isaaclab_tasks" } dependencies = [ { name = "isaaclab", marker = "(platform_machine == 'aarch64' and sys_platform == 'linux') or (platform_machine == 'x86_64' and sys_platform == 'linux') or (platform_machine == 'AMD64' and sys_platform == 'win32')" }, { name = "isaaclab-assets", marker = "(platform_machine == 'aarch64' and sys_platform == 'linux') or (platform_machine == 'x86_64' and sys_platform == 'linux') or (platform_machine == 'AMD64' and sys_platform == 'win32')" }, + { name = "isaaclab-newton", marker = "(platform_machine == 'aarch64' and sys_platform == 'linux') or (platform_machine == 'x86_64' and sys_platform == 'linux') or (platform_machine == 'AMD64' and sys_platform == 'win32')" }, ] [package.metadata] requires-dist = [ { name = "isaaclab", editable = "source/isaaclab" }, { name = "isaaclab-assets", editable = "source/isaaclab_assets" }, + { name = "isaaclab-newton", editable = "source/isaaclab_newton" }, ] [[package]] @@ -2242,7 +2244,7 @@ requires-dist = [ [[package]] name = "isaaclab-teleop" -version = "0.7.1" +version = "0.8.0" source = { editable = "source/isaaclab_teleop" } dependencies = [ { name = "isaaclab", marker = "(platform_machine == 'aarch64' and sys_platform == 'linux') or (platform_machine == 'x86_64' and sys_platform == 'linux') or (platform_machine == 'AMD64' and sys_platform == 'win32')" }, @@ -2253,7 +2255,7 @@ requires-dist = [{ name = "isaaclab", editable = "source/isaaclab" }] [[package]] name = "isaaclab-visualizers" -version = "1.4.0" +version = "1.5.1" source = { editable = "source/isaaclab_visualizers" } dependencies = [ { name = "isaaclab", marker = "(platform_machine == 'aarch64' and sys_platform == 'linux') or (platform_machine == 'x86_64' and sys_platform == 'linux') or (platform_machine == 'AMD64' and sys_platform == 'win32')" }, From 66de12d7c7ac66b869251d111c96244ceeceb5f7 Mon Sep 17 00:00:00 2001 From: Maximilian Krause Date: Mon, 10 Aug 2026 16:04:03 -0700 Subject: [PATCH 08/23] Improve conveyor viewer controls and contacts --- .../maximiliank-conveyor-franka.minor.rst | 3 +- .../conveyor_franka/conveyor_force_driver.py | 18 +-- .../conveyor_franka/conveyor_franka_env.py | 17 +++ .../conveyor_franka_env_cfg.py | 88 +++++++++++--- .../conveyor_franka/conveyor_geometry.py | 99 ++++++++++++---- .../conveyor_franka/conveyor_goal_selector.py | 107 ++++++++++++++++++ .../contrib/conveyor_franka/mdp/__init__.py | 1 + .../conveyor_franka/mdp/reset_events.py | 82 +++++++++++++- .../contrib/test_conveyor_franka_geometry.py | 7 +- .../test/contrib/test_conveyor_franka_mdp.py | 54 ++++++++- .../maximiliank-newton-ui-callback.rst | 4 + .../newton/newton_visualizer.py | 21 +++- .../test/test_newton_adapter.py | 18 +++ 13 files changed, 461 insertions(+), 58 deletions(-) create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_goal_selector.py create mode 100644 source/isaaclab_visualizers/changelog.d/maximiliank-newton-ui-callback.rst diff --git a/source/isaaclab_tasks/changelog.d/maximiliank-conveyor-franka.minor.rst b/source/isaaclab_tasks/changelog.d/maximiliank-conveyor-franka.minor.rst index 1fae7f1d38d0..62fc01309178 100644 --- a/source/isaaclab_tasks/changelog.d/maximiliank-conveyor-franka.minor.rst +++ b/source/isaaclab_tasks/changelog.d/maximiliank-conveyor-franka.minor.rst @@ -2,4 +2,5 @@ Added ^^^^^ * Added a contributed manager-based environment with guarded, counter-rotating force-driven racetrack - conveyors and a MuJoCo Menagerie Franka. + conveyors, robust primitive and closed-mesh belt colliders, a MuJoCo Menagerie Franka, and an interactive + Newton-viewer cube-goal selector. diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_force_driver.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_force_driver.py index 0ec122432d27..911dd153105b 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_force_driver.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_force_driver.py @@ -566,7 +566,7 @@ def __init__( shape_body = model.shape_body.numpy() shape_world = model.shape_world.numpy() shape_transform = model.shape_transform.numpy() - patterns = tuple(re.compile(rf"(?:^|/){re.escape(spec.mesh.name)}(?:/|$)") for spec in self._surface_specs) + patterns = tuple(re.compile(rf"(?:^|/){re.escape(spec.geometry.name)}(?:/|$)") for spec in self._surface_specs) seen_sections: set[tuple[int, int]] = set() for shape_id, label in enumerate(model.shape_label): matching_specs = [index for index, pattern in enumerate(patterns) if pattern.search(label)] @@ -585,7 +585,7 @@ def __init__( if section_key in seen_sections: raise RuntimeError( f"World {world_id} contains multiple shapes matching conveyor section " - f"{self._surface_specs[spec_id].mesh.name!r}." + f"{self._surface_specs[spec_id].geometry.name!r}." ) seen_sections.add(section_key) @@ -606,7 +606,7 @@ def __init__( missing_sections = sorted(expected_sections - seen_sections) if missing_sections: details = ", ".join( - f"world {world_id}: {self._surface_specs[spec_id].mesh.name}" + f"world {world_id}: {self._surface_specs[spec_id].geometry.name}" for world_id, spec_id in missing_sections[:8] ) raise RuntimeError(f"Missing {len(missing_sections)} conveyor collision sections ({details}).") @@ -864,31 +864,31 @@ def _validate_surface_specs(self) -> None: """Validate structural surface descriptions before resolving Newton shapes.""" if not self._surface_specs: raise ValueError("At least one conveyor surface specification is required.") - names = [spec.mesh.name for spec in self._surface_specs] + names = [spec.geometry.name for spec in self._surface_specs] if len(set(names)) != len(names): raise ValueError(f"Conveyor surface names must be unique, got {names}.") for spec in self._surface_specs: if spec.velocity_field_type not in {"constant", "pivot"}: raise ValueError( - f"Unknown velocity field {spec.velocity_field_type!r} for conveyor surface {spec.mesh.name!r}." + f"Unknown velocity field {spec.velocity_field_type!r} for conveyor surface {spec.geometry.name!r}." ) direction = np.asarray(spec.direction, dtype=np.float64) pivot_point = np.asarray(spec.pivot_point, dtype=np.float64) surface_normal = np.asarray(spec.surface_normal, dtype=np.float64) if direction.shape != (3,) or not np.all(np.isfinite(direction)) or np.linalg.norm(direction) <= 1.0e-8: - raise ValueError(f"Conveyor surface {spec.mesh.name!r} needs a non-zero 3-D direction.") + raise ValueError(f"Conveyor surface {spec.geometry.name!r} needs a non-zero 3-D direction.") if pivot_point.shape != (3,) or not np.all(np.isfinite(pivot_point)): - raise ValueError(f"Conveyor surface {spec.mesh.name!r} needs a 3-D pivot point.") + raise ValueError(f"Conveyor surface {spec.geometry.name!r} needs a 3-D pivot point.") if ( surface_normal.shape != (3,) or not np.all(np.isfinite(surface_normal)) or np.linalg.norm(surface_normal) <= 1.0e-8 ): - raise ValueError(f"Conveyor surface {spec.mesh.name!r} needs a non-zero 3-D surface normal.") + raise ValueError(f"Conveyor surface {spec.geometry.name!r} needs a non-zero 3-D surface normal.") if spec.velocity_field_type == "pivot" and ( spec.radius is None or not np.isfinite(spec.radius) or spec.radius <= 0.0 ): - raise ValueError(f"Pivot conveyor surface {spec.mesh.name!r} needs a positive arc radius.") + raise ValueError(f"Pivot conveyor surface {spec.geometry.name!r} needs a positive arc radius.") def _validate_backend_buffers(self) -> None: """Validate every fixed-size Newton buffer consumed by conveyor kernels.""" diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env.py index 4b41c36ca744..34a1b66d4374 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env.py @@ -14,6 +14,7 @@ from .conveyor_force_driver import ConveyorForceDriver from .conveyor_franka_env_cfg import ConveyorFrankaEnvCfg from .conveyor_geometry import belt_collision_section_specs +from .conveyor_goal_selector import ConveyorGoalSelector class ConveyorFrankaEnv(ManagerBasedRLEnv): @@ -35,6 +36,22 @@ def __init__(self, cfg: ConveyorFrankaEnvCfg, render_mode: str | None = None, ** transported_body_pattern=cfg.conveyor_force.transported_body_pattern, transported_body_count_per_env=cfg.conveyor_force.transported_body_count_per_env, ) + self._goal_selector: ConveyorGoalSelector | None = None + self._setup_goal_selector() + + def _setup_goal_selector(self) -> None: + """Attach one task panel to the first interactive Newton visualizer.""" + for visualizer in self.sim.visualizers: + if getattr(visualizer.cfg, "visualizer_type", None) not in {"newton_gl", "newton_rtx"}: + continue + register_callback = getattr(visualizer, "register_ui_callback", None) + if register_callback is None: + continue + visible_env_ids = visualizer.get_visualized_env_ids() + env_id = visible_env_ids[0] if visible_env_ids else 0 + self._goal_selector = ConveyorGoalSelector(self, env_id) + register_callback(self._goal_selector.render, position="panel") + return def _reset_idx(self, env_ids: Sequence[int]): """Reset selected environments and discard stale conveyor forces.""" diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env_cfg.py index 92b941a96670..eadcf00b6872 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env_cfg.py @@ -35,8 +35,9 @@ BELT_CENTER_Y, BELT_COLOR, BELT_TURN_RADIUS, + CUBE_COLORS, GUARD_COLOR, - PARCEL_COLOR, + CuboidSpec, MeshSpec, belt_collision_section_specs, belt_mesh_spec, @@ -47,6 +48,8 @@ _DYNAMIC_PROPERTIES = sim_utils.RigidBodyBaseCfg() _CONTACT_GAP = 0.01 _CUBE_CONTACT_MARGIN = 0.003 +_MANIPULATION_CONTACT_STIFFNESS = 1.0e4 +_MANIPULATION_CONTACT_DAMPING = 200.0 _MUJOCO_SOLIMP = (0.9, 0.95, 0.001, 0.5, 2.0) _MUJOCO_SOLREF = (0.02, 1.0) _POLICY_DT = 1.0 / 60.0 @@ -395,11 +398,25 @@ def _visual_mesh( ) +def _collision_material(friction: float, stiff_contact: bool) -> NewtonMaterialPropertiesCfg: + """Build frictionless-drive contact material, optionally with manipulation-grade gains.""" + return NewtonMaterialPropertiesCfg( + static_friction=friction, + dynamic_friction=friction, + restitution=0.0, + torsional_friction=0.0, + rolling_friction=0.0, + contact_stiffness=_MANIPULATION_CONTACT_STIFFNESS if stiff_contact else None, + contact_damping=_MANIPULATION_CONTACT_DAMPING if stiff_contact else None, + ) + + def _hidden_collision_mesh( prim_path: str, spec: MeshSpec, friction: float, mujoco_priority: int, + stiff_contact: bool = False, ) -> AssetBaseCfg: """Build a hidden static triangle-mesh collider.""" spawn = sim_utils.MeshCustomCfg( @@ -407,16 +424,57 @@ def _hidden_collision_mesh( faces=spec.faces, visible=False, collision_props=_collision_properties(mujoco_priority=mujoco_priority), - physics_material=RigidBodyMaterialBaseCfg( - static_friction=friction, - dynamic_friction=friction, - restitution=0.0, - ), + physics_material=_collision_material(friction, stiff_contact), ) spawn.func = _spawn_hidden_collision_mesh return AssetBaseCfg(prim_path=prim_path, spawn=spawn) +def _hidden_collision_cuboid( + prim_path: str, + spec: CuboidSpec, + friction: float, + mujoco_priority: int, + stiff_contact: bool = False, +) -> AssetBaseCfg: + """Build a hidden native cuboid collider.""" + return AssetBaseCfg( + prim_path=prim_path, + init_state=AssetBaseCfg.InitialStateCfg(pos=spec.position), + spawn=sim_utils.CuboidCfg( + size=spec.size, + visible=False, + collision_props=_collision_properties(mujoco_priority=mujoco_priority), + physics_material=_collision_material(friction, stiff_contact), + ), + ) + + +def _hidden_collision_geometry( + prim_path: str, + spec: MeshSpec | CuboidSpec, + friction: float, + mujoco_priority: int, + stiff_contact: bool = False, +) -> AssetBaseCfg: + """Build hidden collision geometry while preferring native primitives where possible.""" + if isinstance(spec, CuboidSpec): + return _hidden_collision_cuboid( + prim_path=prim_path, + spec=spec, + friction=friction, + mujoco_priority=mujoco_priority, + stiff_contact=stiff_contact, + ) + return _hidden_collision_mesh( + prim_path=prim_path, + spec=spec, + friction=friction, + mujoco_priority=mujoco_priority, + stiff_contact=stiff_contact, + ) + + def _cube( name: str, color: tuple[float, float, float], @@ -438,8 +496,8 @@ def _cube( restitution=0.0, torsional_friction=0.002, rolling_friction=0.0001, - contact_stiffness=2.5e3, - contact_damping=100.0, + contact_stiffness=_MANIPULATION_CONTACT_STIFFNESS, + contact_damping=_MANIPULATION_CONTACT_DAMPING, ) spawn.func = _spawn_shape_with_display_color return RigidObjectCfg( @@ -483,10 +541,10 @@ class ConveyorFrankaSceneCfg(InteractiveSceneCfg): color=(0.18, 0.20, 0.23), ) - cube_0 = _cube("Cube0", (0.15, 0.35, 0.90), (0.30, BELT_CENTER_Y - BELT_TURN_RADIUS, 0.06)) - cube_1 = _cube("Cube1", (0.90, 0.20, 0.15), (0.78, BELT_CENTER_Y - BELT_TURN_RADIUS, 0.06)) - cube_2 = _cube("Cube2", (0.15, 0.75, 0.25), (0.30, -BELT_CENTER_Y + BELT_TURN_RADIUS, 0.06)) - cube_3 = _cube("Cube3", PARCEL_COLOR, (0.78, -BELT_CENTER_Y + BELT_TURN_RADIUS, 0.06)) + cube_0 = _cube("Cube0", CUBE_COLORS[0], (0.30, BELT_CENTER_Y - BELT_TURN_RADIUS, 0.06)) + cube_1 = _cube("Cube1", CUBE_COLORS[1], (0.78, BELT_CENTER_Y - BELT_TURN_RADIUS, 0.06)) + cube_2 = _cube("Cube2", CUBE_COLORS[2], (0.30, -BELT_CENTER_Y + BELT_TURN_RADIUS, 0.06)) + cube_3 = _cube("Cube3", CUBE_COLORS[3], (0.78, -BELT_CENTER_Y + BELT_TURN_RADIUS, 0.06)) cube_contacts = ContactSensorCfg( prim_path="{ENV_REGEX_NS}/Cube.*", @@ -523,11 +581,11 @@ def __post_init__(self) -> None: section_keys = ("top_straight", "bottom_straight", "right_turn", "left_turn") for section_key, section in zip(section_keys, belt_collision_section_specs(side), strict=True): - spec = section.mesh + spec = section.geometry setattr( self, f"conveyor_{side.lower()}_{section_key}_collision", - _hidden_collision_mesh( + _hidden_collision_geometry( prim_path=f"{{ENV_REGEX_NS}}/{spec.name}", spec=spec, # MuJoCo requires a tiny positive value even though the force driver, @@ -535,6 +593,8 @@ def __post_init__(self) -> None: friction=1.1e-5, # Override cube friction only for collision-section/cube pairs. mujoco_priority=1, + # Match the locally stable Franka Stack support surface. + stiff_contact=True, ), ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_geometry.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_geometry.py index 52b1ad524c52..eaa8d9f79925 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_geometry.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_geometry.py @@ -18,6 +18,13 @@ """Brushed-metal color used by Newton's conveyor example.""" PARCEL_COLOR = (0.72, 0.55, 0.35) +CUBE_COLORS = ( + (0.15, 0.35, 0.90), + (0.90, 0.20, 0.15), + (0.15, 0.75, 0.25), + PARCEL_COLOR, +) +"""Stable sRGB colors for the four numbered transfer cubes.""" """Cardboard color used by Newton's conveyor example.""" BELT_CENTER_X = 0.58 @@ -51,9 +58,18 @@ class MeshSpec: faces: tuple[tuple[int, int, int], ...] +@dataclass(frozen=True) +class CuboidSpec: + """Native cuboid and semantic name for one static racetrack component.""" + + name: str + size: tuple[float, float, float] + position: tuple[float, float, float] + + @dataclass(frozen=True) class ConveyorSectionSpec: - """Collision mesh and velocity field for one conveyor section. + """Collision geometry and velocity field for one conveyor section. The direction and pivot are expressed in the collision prim's local frame. Constant sections interpret ``direction`` as the unit travel direction; @@ -62,7 +78,7 @@ class ConveyorSectionSpec: also local, so rotated or inclined sections need no world-space special case. """ - mesh: MeshSpec + geometry: MeshSpec | CuboidSpec velocity_field_type: Literal["constant", "pivot"] direction: tuple[float, float, float] pivot_point: tuple[float, float, float] = (0.0, 0.0, 0.0) @@ -203,22 +219,20 @@ def belt_mesh_spec(side: str) -> MeshSpec: ) -def _straight_collision_mesh(name: str, center_y: float) -> MeshSpec: - """Build one horizontal straight collision surface with +Z face winding.""" +def _straight_collision_cuboid(name: str, center_y: float) -> CuboidSpec: + """Build one solid native cuboid for a straight conveyor section.""" half_width = 0.5 * BELT_WIDTH + BELT_COLLISION_OVERHANG x_min = BELT_CENTER_X - BELT_HALF_STRAIGHT - BELT_COLLISION_SEAM_OVERLAP x_max = BELT_CENTER_X + BELT_HALF_STRAIGHT + BELT_COLLISION_SEAM_OVERLAP - vertices = ( - (x_min, center_y - half_width, BELT_TOP_Z), - (x_max, center_y - half_width, BELT_TOP_Z), - (x_max, center_y + half_width, BELT_TOP_Z), - (x_min, center_y + half_width, BELT_TOP_Z), + return CuboidSpec( + name=name, + size=(x_max - x_min, 2.0 * half_width, BELT_THICKNESS), + position=(0.5 * (x_min + x_max), center_y, BELT_TOP_Z - 0.5 * BELT_THICKNESS), ) - return MeshSpec(name=name, vertices=vertices, faces=((0, 1, 2), (0, 2, 3))) def _turn_collision_mesh(name: str, pivot_x: float, center_y: float, start_angle: float) -> MeshSpec: - """Build one horizontal annular half-turn collision surface with +Z normals.""" + """Build one closed annular half-turn prism with a +Z top surface.""" half_width = 0.5 * BELT_WIDTH + BELT_COLLISION_OVERHANG inner_radius = BELT_TURN_RADIUS - half_width outer_radius = BELT_TURN_RADIUS + half_width @@ -227,30 +241,67 @@ def _turn_collision_mesh(name: str, pivot_x: float, center_y: float, start_angle angle_step = (math.pi + 2.0 * angle_overlap) / TURN_SEGMENT_COUNT angles = tuple(angle_start + index * angle_step for index in range(TURN_SEGMENT_COUNT + 1)) - inner = tuple( + inner_top = tuple( (pivot_x + inner_radius * math.cos(angle), center_y + inner_radius * math.sin(angle), BELT_TOP_Z) for angle in angles ) - outer = tuple( + outer_top = tuple( (pivot_x + outer_radius * math.cos(angle), center_y + outer_radius * math.sin(angle), BELT_TOP_Z) for angle in angles ) - outer_offset = len(inner) + bottom_z = BELT_TOP_Z - BELT_THICKNESS + inner_bottom = tuple((x, y, bottom_z) for x, y, _ in inner_top) + outer_bottom = tuple((x, y, bottom_z) for x, y, _ in outer_top) + + count = len(inner_top) + outer_top_offset = count + inner_bottom_offset = 2 * count + outer_bottom_offset = 3 * count faces: list[tuple[int, int, int]] = [] for index in range(TURN_SEGMENT_COUNT): next_index = index + 1 + inner_top_i = index + inner_top_j = next_index + outer_top_i = outer_top_offset + index + outer_top_j = outer_top_offset + next_index + inner_bottom_i = inner_bottom_offset + index + inner_bottom_j = inner_bottom_offset + next_index + outer_bottom_i = outer_bottom_offset + index + outer_bottom_j = outer_bottom_offset + next_index faces.extend( ( - (index, outer_offset + index, outer_offset + next_index), - (index, outer_offset + next_index, next_index), + # Top, bottom, outer wall, and inner wall. + (inner_top_i, outer_top_i, outer_top_j), + (inner_top_i, outer_top_j, inner_top_j), + (inner_bottom_i, inner_bottom_j, outer_bottom_j), + (inner_bottom_i, outer_bottom_j, outer_bottom_i), + (outer_bottom_i, outer_bottom_j, outer_top_j), + (outer_bottom_i, outer_top_j, outer_top_i), + (inner_bottom_i, inner_top_i, inner_top_j), + (inner_bottom_i, inner_top_j, inner_bottom_j), ) ) - return MeshSpec(name=name, vertices=inner + outer, faces=tuple(faces)) + + # Close both radial ends of the annular prism. + end = TURN_SEGMENT_COUNT + faces.extend( + ( + (0, inner_bottom_offset, outer_bottom_offset), + (0, outer_bottom_offset, outer_top_offset), + (end, outer_top_offset + end, outer_bottom_offset + end), + (end, outer_bottom_offset + end, inner_bottom_offset + end), + ) + ) + return MeshSpec( + name=name, + vertices=inner_top + outer_top + inner_bottom + outer_bottom, + faces=tuple(faces), + ) -def belt_collision_mesh_specs(side: str) -> tuple[MeshSpec, MeshSpec, MeshSpec, MeshSpec]: - """Build straight and pivot-field collision sections for one racetrack.""" - return tuple(section.mesh for section in belt_collision_section_specs(side)) +def belt_collision_geometry_specs(side: str) -> tuple[CuboidSpec | MeshSpec, ...]: + """Build native straight and closed-mesh turn collision geometry for one racetrack.""" + return tuple(section.geometry for section in belt_collision_section_specs(side)) def belt_collision_section_specs( @@ -263,24 +314,24 @@ def belt_collision_section_specs( direction = belt_direction(side) return ( ConveyorSectionSpec( - mesh=_straight_collision_mesh(f"Conveyor{side}TopStraightCollision", center_y + BELT_TURN_RADIUS), + geometry=_straight_collision_cuboid(f"Conveyor{side}TopStraightCollision", center_y + BELT_TURN_RADIUS), velocity_field_type="constant", direction=(direction, 0.0, 0.0), ), ConveyorSectionSpec( - mesh=_straight_collision_mesh(f"Conveyor{side}BottomStraightCollision", center_y - BELT_TURN_RADIUS), + geometry=_straight_collision_cuboid(f"Conveyor{side}BottomStraightCollision", center_y - BELT_TURN_RADIUS), velocity_field_type="constant", direction=(-direction, 0.0, 0.0), ), ConveyorSectionSpec( - mesh=_turn_collision_mesh(f"Conveyor{side}RightTurnCollision", right_x, center_y, -0.5 * math.pi), + geometry=_turn_collision_mesh(f"Conveyor{side}RightTurnCollision", right_x, center_y, -0.5 * math.pi), velocity_field_type="pivot", direction=(0.0, 0.0, -direction), pivot_point=(right_x, center_y, 0.0), radius=BELT_TURN_RADIUS, ), ConveyorSectionSpec( - mesh=_turn_collision_mesh(f"Conveyor{side}LeftTurnCollision", left_x, center_y, 0.5 * math.pi), + geometry=_turn_collision_mesh(f"Conveyor{side}LeftTurnCollision", left_x, center_y, 0.5 * math.pi), velocity_field_type="pivot", direction=(0.0, 0.0, -direction), pivot_point=(left_x, center_y, 0.0), diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_goal_selector.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_goal_selector.py new file mode 100644 index 000000000000..1d9c39737720 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_goal_selector.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 + +"""Minimal Newton-viewer selector for conveyor transfer goals.""" + +from __future__ import annotations + +import time +from typing import TYPE_CHECKING, Any + +from .conveyor_geometry import CUBE_COLORS +from .mdp.reset_events import LEFT_SIDE, set_conveyor_transfer_goal + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +def _mix_color( + color: tuple[float, float, float], + target: tuple[float, float, float], + amount: float, +) -> tuple[float, float, float]: + """Linearly mix two RGB colors.""" + return tuple(value + amount * (target_value - value) for value, target_value in zip(color, target, strict=True)) + + +class ConveyorGoalSelector: + """Render four color-matched cube buttons for one displayed environment.""" + + def __init__(self, env: ManagerBasedRLEnv, env_id: int) -> None: + """Create a selector bound to one vectorized environment index.""" + if not 0 <= env_id < env.num_envs: + raise IndexError(f"Conveyor selector environment {env_id} is out of range.") + self._env = env + self._env_id = env_id + self._target_cube_id = 0 + self._source_side_id = LEFT_SIDE + self._last_refresh_time = float("-inf") + + def render(self, imgui: Any) -> None: + """Draw the selector and publish a new transfer command when clicked.""" + imgui.set_next_item_open(True, imgui.Cond_.appearing) + if not imgui.collapsing_header(f"Transfer Goal · Env {self._env_id}"): + return + imgui.separator() + + if not self._refresh_command(): + imgui.text_disabled("Waiting for transfer state...") + return + + style = imgui.get_style() + available_width = float(imgui.get_content_region_avail().x) + spacing = float(style.item_spacing.x) + button_width = max(36.0, (available_width - spacing * (len(CUBE_COLORS) - 1)) / len(CUBE_COLORS)) + button_height = max(34.0, min(46.0, button_width * 0.72)) + + selected_cube_id: int | None = None + for cube_id, color in enumerate(CUBE_COLORS): + selected = cube_id == self._target_cube_id + text_color = (0.05, 0.05, 0.05) if sum(color) > 1.45 else (1.0, 1.0, 1.0) + border_color = (1.0, 0.88, 0.20) if selected else (0.12, 0.12, 0.12) + label = f"{cube_id + 1}##conveyor_goal_{cube_id}" + + imgui.push_style_color(imgui.Col_.button, imgui.ImVec4(*color, 1.0)) + imgui.push_style_color( + imgui.Col_.button_hovered, + imgui.ImVec4(*_mix_color(color, (1.0, 1.0, 1.0), 0.18), 1.0), + ) + imgui.push_style_color( + imgui.Col_.button_active, + imgui.ImVec4(*_mix_color(color, (0.0, 0.0, 0.0), 0.12), 1.0), + ) + imgui.push_style_color(imgui.Col_.border, imgui.ImVec4(*border_color, 1.0)) + imgui.push_style_color(imgui.Col_.text, imgui.ImVec4(*text_color, 1.0)) + imgui.push_style_var(imgui.StyleVar_.frame_border_size, 3.0 if selected else 1.0) + imgui.push_style_var(imgui.StyleVar_.frame_rounding, 5.0) + + if imgui.button(label, imgui.ImVec2(button_width, button_height)): + selected_cube_id = cube_id + + imgui.pop_style_var(2) + imgui.pop_style_color(5) + if cube_id + 1 < len(CUBE_COLORS): + imgui.same_line() + + if selected_cube_id is not None and selected_cube_id != self._target_cube_id: + set_conveyor_transfer_goal(self._env, selected_cube_id, env_ids=(self._env_id,)) + self._refresh_command(force=True) + + source_name = "Left" if self._source_side_id == LEFT_SIDE else "Right" + target_name = "Right" if self._source_side_id == LEFT_SIDE else "Left" + imgui.text(f"Cube {self._target_cube_id + 1}: {source_name} -> {target_name}") + imgui.text_disabled("Click a color to change the next transfer.") + + def _refresh_command(self, force: bool = False) -> bool: + """Refresh the small host-side UI cache at most ten times per second.""" + state = getattr(self._env, "conveyor_transfer_state", None) + if state is None: + return False + current_time = time.monotonic() + if force or current_time - self._last_refresh_time >= 0.1: + self._target_cube_id = int(state.target_cube_ids[self._env_id].item()) + self._source_side_id = int(state.source_side_ids[self._env_id].item()) + self._last_refresh_time = current_time + return True diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/__init__.py index e96e26dc4871..b7c202fc3d5c 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/__init__.py @@ -27,6 +27,7 @@ advance_conveyor_transfer_goal, build_reset_rows, select_next_transfer_cube, + set_conveyor_transfer_goal, ) from .rewards import ( ConveyorTransferProgressReward, diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/reset_events.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/reset_events.py index 2da5f6f015f6..3b75daee4920 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/reset_events.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/reset_events.py @@ -508,6 +508,75 @@ def select_next_transfer_cube( return torch.multinomial(candidates.float(), 1).squeeze(1) +def _assign_conveyor_transfer_goal( + env: ManagerBasedRLEnv, + env_ids: torch.Tensor, + target_cube_ids: torch.Tensor, + source_side_ids: torch.Tensor, + success_context_name: str, +) -> None: + """Publish a new transfer command and clear progress from the previous command.""" + context = env.termination_manager.get_term_cfg(success_context_name).func + if not hasattr(context, "consume_success"): + raise RuntimeError("Conveyor goal changes require a stable transfer-success context.") + + state = env.conveyor_transfer_state + state.goal_ids[env_ids] += 1 + state.subgoal_start_steps[env_ids] = env.episode_length_buf[env_ids] + state.target_cube_ids[env_ids] = target_cube_ids + state.source_side_ids[env_ids] = source_side_ids + state.held_cube_ids[env_ids] = -1 + context.consume_success(env_ids) + + +def set_conveyor_transfer_goal( + env: ManagerBasedRLEnv, + target_cube_id: int, + env_ids: Sequence[int] | torch.Tensor | None = None, + success_context_name: str = "transfer_success_context", +) -> None: + """Command a numbered cube to move from its current conveyor to the opposite one. + + The cube's current local y-position determines its source conveyor. Positions + in the central transfer region use the nearest side of the workspace center. + + Args: + env: Conveyor Franka environment whose command state is updated. + target_cube_id: Stable numbered cube index. + env_ids: Environments receiving the command, or ``None`` for all environments. + success_context_name: Stable-success term reset for the new command. + """ + if not isinstance(target_cube_id, int) or isinstance(target_cube_id, bool): + raise TypeError("target_cube_id must be an integer.") + if not 0 <= target_cube_id < CUBE_COUNT: + raise ValueError(f"target_cube_id must lie in [0, {CUBE_COUNT - 1}].") + + if env_ids is None: + resolved_env_ids = torch.arange(env.num_envs, dtype=torch.long, device=env.device) + else: + resolved_env_ids = torch.as_tensor(env_ids, dtype=torch.long, device=env.device).flatten() + if resolved_env_ids.numel() == 0: + return + if torch.any((resolved_env_ids < 0) | (resolved_env_ids >= env.num_envs)): + raise IndexError("Conveyor goal environment indices are out of range.") + + cube: RigidObject = env.scene[f"cube_{target_cube_id}"] + local_y = cube.data.root_pos_w.torch[resolved_env_ids, 1] - env.scene.env_origins[resolved_env_ids, 1] + source_side_ids = torch.where( + local_y >= 0.0, + torch.full_like(resolved_env_ids, LEFT_SIDE), + torch.full_like(resolved_env_ids, RIGHT_SIDE), + ) + target_cube_ids = torch.full_like(resolved_env_ids, target_cube_id) + _assign_conveyor_transfer_goal( + env, + resolved_env_ids, + target_cube_ids, + source_side_ids, + success_context_name, + ) + + def advance_conveyor_transfer_goal( env: ManagerBasedRLEnv, env_ids: torch.Tensor, @@ -545,9 +614,10 @@ def advance_conveyor_transfer_goal( state.direction_transfer_counts[completed_ids, previous_source_side_ids] += 1 state.transfer_counts[completed_ids] += 1 - state.goal_ids[completed_ids] += 1 - state.subgoal_start_steps[completed_ids] = env.episode_length_buf[completed_ids] - state.target_cube_ids[completed_ids] = next_cube_ids - state.source_side_ids[completed_ids] = next_source_side_ids - state.held_cube_ids[completed_ids] = -1 - context.consume_success(completed_ids) + _assign_conveyor_transfer_goal( + env, + completed_ids, + next_cube_ids, + next_source_side_ids, + success_context_name, + ) diff --git a/source/isaaclab_tasks/test/contrib/test_conveyor_franka_geometry.py b/source/isaaclab_tasks/test/contrib/test_conveyor_franka_geometry.py index 352c6d344b64..13342ec15a40 100644 --- a/source/isaaclab_tasks/test/contrib/test_conveyor_franka_geometry.py +++ b/source/isaaclab_tasks/test/contrib/test_conveyor_franka_geometry.py @@ -23,6 +23,7 @@ BELT_TOP_Z, BELT_TURN_RADIUS, TURN_SEGMENT_COUNT, + CuboidSpec, MeshSpec, belt_collision_section_specs, belt_direction, @@ -79,12 +80,16 @@ def test_racetrack_lanes_counter_rotate(): def test_collision_sections_and_velocity_fields_share_one_description(): - """Straight and curved force fields stay aligned with their collision meshes.""" + """Straight and curved force fields stay aligned with robust collision geometry.""" for side in ("Left", "Right"): sections = belt_collision_section_specs(side) assert len(sections) == 4 assert [section.velocity_field_type for section in sections] == ["constant", "constant", "pivot", "pivot"] + assert all(isinstance(section.geometry, CuboidSpec) for section in sections[:2]) + for section in sections[2:]: + assert isinstance(section.geometry, MeshSpec) + assert set(_edge_use_counts(section.geometry).values()) == {2} assert all(section.radius == BELT_TURN_RADIUS for section in sections[2:]) assert sections[0].direction == tuple(-value for value in sections[1].direction) assert sections[2].direction == sections[3].direction diff --git a/source/isaaclab_tasks/test/contrib/test_conveyor_franka_mdp.py b/source/isaaclab_tasks/test/contrib/test_conveyor_franka_mdp.py index 15cf26a3b3b1..fbaa8c0b4184 100644 --- a/source/isaaclab_tasks/test/contrib/test_conveyor_franka_mdp.py +++ b/source/isaaclab_tasks/test/contrib/test_conveyor_franka_mdp.py @@ -30,6 +30,7 @@ franka_tool_position, reset_variant_counts, select_next_transfer_cube, + set_conveyor_transfer_goal, ) from isaaclab_tasks.contrib.conveyor_franka.mdp.rewards import transfer_potential from isaaclab_tasks.contrib.conveyor_franka.mdp.terminations import ( @@ -44,14 +45,18 @@ def test_contact_and_drive_cfg_preserve_transport_and_grasp_friction(): cfg = ConveyorFrankaEnvCfg() belt_collision = cfg.scene.conveyor_left_top_straight_collision.spawn.collision_props belt_mujoco = next(fragment for fragment in belt_collision if isinstance(fragment, MujocoCollisionCfg)) + belt_material = cfg.scene.conveyor_left_top_straight_collision.spawn.physics_material cube_material = cfg.scene.cube_0.spawn.physics_material hand_actuator = cfg.scene.robot.actuators["panda_hand"] assert belt_mujoco.priority == 1 + assert isinstance(belt_material, NewtonMaterialPropertiesCfg) + assert belt_material.contact_stiffness == 1.0e4 + assert belt_material.contact_damping == 200.0 assert isinstance(cube_material, NewtonMaterialPropertiesCfg) assert cube_material.dynamic_friction == 0.6 - assert cube_material.contact_stiffness == 2.5e3 - assert cube_material.contact_damping == 100.0 + assert cube_material.contact_stiffness == 1.0e4 + assert cube_material.contact_damping == 200.0 assert hand_actuator.stiffness == 350.0 assert hand_actuator.damping == 10.0 assert cfg.actions.arm_action.gravity_compensation @@ -295,6 +300,51 @@ def test_next_transfer_cube_is_random_among_eligible_alternatives(): assert torch.all(torch.abs(frequencies[1:3] - 0.5) < 0.05) +def test_manual_transfer_goal_uses_selected_cube_current_side(): + """Viewer goal changes preserve cube identity and infer the opposite destination.""" + + class _Scene(dict): + pass + + class _SuccessContext: + consumed_env_ids = None + + def consume_success(self, env_ids): + self.consumed_env_ids = env_ids.clone() + + origins = torch.tensor(((0.0, 1.0, 0.0), (0.0, -2.0, 0.0))) + selected_cube_positions = torch.tensor(((0.2, 1.3, 0.06), (0.7, -2.3, 0.06))) + scene = _Scene( + cube_2=SimpleNamespace(data=SimpleNamespace(root_pos_w=SimpleNamespace(torch=selected_cube_positions))) + ) + scene.env_origins = origins + context = _SuccessContext() + state = SimpleNamespace( + target_cube_ids=torch.tensor((0, 1)), + source_side_ids=torch.tensor((1, 0)), + held_cube_ids=torch.tensor((0, 1)), + goal_ids=torch.tensor((4, 7)), + subgoal_start_steps=torch.tensor((2, 3)), + ) + env = SimpleNamespace( + num_envs=2, + device="cpu", + scene=scene, + conveyor_transfer_state=state, + episode_length_buf=torch.tensor((11, 19)), + termination_manager=SimpleNamespace(get_term_cfg=lambda _name: SimpleNamespace(func=context)), + ) + + set_conveyor_transfer_goal(env, 2) + + assert state.target_cube_ids.tolist() == [2, 2] + assert state.source_side_ids.tolist() == [0, 1] + assert state.held_cube_ids.tolist() == [-1, -1] + assert state.goal_ids.tolist() == [5, 8] + assert state.subgoal_start_steps.tolist() == [11, 19] + assert context.consumed_env_ids.tolist() == [0, 1] + + def test_continuing_training_truncates_only_stalled_or_long_sequences(): """Subgoal and sequence limits bound training without ending successful transfers.""" assert not ConveyorFrankaPPORunnerCfg().init_at_random_ep_len diff --git a/source/isaaclab_visualizers/changelog.d/maximiliank-newton-ui-callback.rst b/source/isaaclab_visualizers/changelog.d/maximiliank-newton-ui-callback.rst new file mode 100644 index 000000000000..4f2dd1181e2c --- /dev/null +++ b/source/isaaclab_visualizers/changelog.d/maximiliank-newton-ui-callback.rst @@ -0,0 +1,4 @@ +Added +^^^^^ + +* Exposed Newton viewer ImGui callback registration through the Isaac Lab visualizer. diff --git a/source/isaaclab_visualizers/isaaclab_visualizers/newton/newton_visualizer.py b/source/isaaclab_visualizers/isaaclab_visualizers/newton/newton_visualizer.py index 4a25e9bbb912..9fabb9e33c5a 100644 --- a/source/isaaclab_visualizers/isaaclab_visualizers/newton/newton_visualizer.py +++ b/source/isaaclab_visualizers/isaaclab_visualizers/newton/newton_visualizer.py @@ -12,7 +12,8 @@ import math import os import sys -from typing import TYPE_CHECKING +from collections.abc import Callable +from typing import TYPE_CHECKING, Any, Literal import numpy as np # noqa: F401 — used in type hints and colorization helpers import torch @@ -1153,6 +1154,24 @@ def supports_live_plots(self) -> bool: """Newton RTX viewers do not provide live-plot panels; GL viewers do.""" return False + def register_ui_callback( + self, + callback: Callable[[Any], None], + position: Literal["side", "stats", "free", "panel", "rendering"] = "side", + ) -> None: + """Register a callback with the active Newton viewer's ImGui interface. + + Args: + callback: Function called during UI rendering with the active ImGui module. + position: Newton viewer UI region in which the callback is rendered. + + Raises: + RuntimeError: If the visualizer has not created an interactive viewer. + """ + if self._viewer is None: + raise RuntimeError("Newton UI callbacks require an initialized interactive viewer.") + self._viewer.register_ui_callback(callback, position=position) + def is_training_paused(self) -> bool: """Return whether training is paused from viewer controls.""" if not self._is_initialized or self._viewer is None: diff --git a/source/isaaclab_visualizers/test/test_newton_adapter.py b/source/isaaclab_visualizers/test/test_newton_adapter.py index 675c7ff4469c..53a71943e972 100644 --- a/source/isaaclab_visualizers/test/test_newton_adapter.py +++ b/source/isaaclab_visualizers/test/test_newton_adapter.py @@ -211,6 +211,24 @@ def __init__(self): assert visualizer.cfg.lookat == (0.0, 0.0, 1.0) +def test_newton_visualizer_register_ui_callback_forwards_to_active_viewer(): + callback = Mock() + viewer = SimpleNamespace(register_ui_callback=Mock()) + visualizer = NewtonGLVisualizer(NewtonGLVisualizerCfg()) + visualizer._viewer = viewer + + visualizer.register_ui_callback(callback, position="panel") + + viewer.register_ui_callback.assert_called_once_with(callback, position="panel") + + +def test_newton_visualizer_register_ui_callback_requires_active_viewer(): + visualizer = NewtonGLVisualizer(NewtonGLVisualizerCfg()) + + with pytest.raises(RuntimeError, match="initialized interactive viewer"): + visualizer.register_ui_callback(Mock(), position="panel") + + def test_newton_visualizer_auto_creates_streaming_camera_when_scene_camera_exists(monkeypatch): """Auto-create mode should not silently replace its configured view with a scene camera.""" existing_camera = SimpleNamespace( From a5510baba31aeb708605e88689f3768248194e85 Mon Sep 17 00:00:00 2001 From: Maximilian Krause Date: Tue, 11 Aug 2026 13:14:18 -0700 Subject: [PATCH 09/23] Use shared success monitor for conveyor resets --- .../conveyor_franka_env_cfg.py | 11 +- .../contrib/conveyor_franka/mdp/__init__.py | 1 + .../conveyor_franka/mdp/curriculums.py | 179 +++++------------- .../test/contrib/test_conveyor_franka_mdp.py | 41 ++-- 4 files changed, 73 insertions(+), 159 deletions(-) diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env_cfg.py index eadcf00b6872..b1bd4cd3df32 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env_cfg.py @@ -237,9 +237,14 @@ class CurriculumCfg: params={ "progress_context_name": "learning_progress_context", "final_success_context_name": "transfer_success_context", - # Match Franka Stack: sampling follows each row's recent policy - # competence instead of retaining stale early failures forever. - "monitored_history_len": 50, + # Shared target-rate monitor keeps each physical reset row near the + # policy's 50% competence frontier without stale early outcomes. + "success_monitor": mdp.SuccessMonitorCfg( + monitored_history_len=50, + target_success_rate=0.5, + kappa=1.0, + temperature=1.0, + ), # Keep a deployment-facing stream while the remaining starts # adapt around the rolling pickup-to-placement frontier. Every # recipe, cube identity, and direction retains equal total mass. diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/__init__.py index b7c202fc3d5c..a5ec84b8f123 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/__init__.py @@ -6,6 +6,7 @@ """MDP terms for the conveyor-to-conveyor Franka transfer task.""" from isaaclab.envs.mdp import * # noqa: F401, F403 +from isaaclab_tasks.core.lift.mdp.events_cfg import SuccessMonitorCfg from .actions import ConveyorRelativeJointPositionAction, ResetBufferedGripperAction from .actions_cfg import ConveyorRelativeJointPositionActionCfg, ResetBufferedGripperActionCfg diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/curriculums.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/curriculums.py index 4efca2a70266..67b937aec5dd 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/curriculums.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/curriculums.py @@ -15,120 +15,45 @@ from isaaclab.managers import CurriculumTermCfg, ManagerTermBase +from isaaclab_tasks.core.lift.mdp.events_cfg import SuccessMonitorCfg + from .reset_events import BELT_DEPLOYMENT_VARIANT, CUBE_COUNT, ConveyorResetRecipe, reset_variant_counts if TYPE_CHECKING: from isaaclab.envs import ManagerBasedRLEnv -def _ring_append_bool_count_rate( - data: torch.Tensor, - stream_ids: torch.Tensor, - values: torch.Tensor, - pointer: torch.Tensor, - size: torch.Tensor, - true_count: torch.Tensor, - rate: torch.Tensor, -) -> None: - """Append a batch to exact per-row Boolean rolling windows.""" - if stream_ids.numel() == 0: - return - - capacity = data.shape[1] - unique_ids, inverse, counts = torch.unique(stream_ids, return_inverse=True, return_counts=True) - if unique_ids.numel() == stream_ids.numel(): - columns = pointer[stream_ids].long() - overwritten = torch.where( - size[stream_ids] == capacity, - data[stream_ids, columns].to(dtype=true_count.dtype), - torch.zeros_like(true_count[stream_ids]), - ) - new_true_counts = true_count[stream_ids] - overwritten + values.to(dtype=true_count.dtype) - data[stream_ids, columns] = values - pointer[stream_ids] = ((columns + 1) % capacity).to(dtype=pointer.dtype) - size[stream_ids] = (size[stream_ids] + 1).clamp(max=capacity) - true_count[stream_ids] = new_true_counts - rate[stream_ids] = new_true_counts.to(rate.dtype) / size[stream_ids].clamp(min=1) - return - - order = torch.argsort(inverse, stable=True) - sorted_ids = stream_ids[order] - sorted_values = values[order] - group_starts = counts.cumsum(0) - counts - local_rank = torch.arange(stream_ids.numel(), device=data.device) - torch.repeat_interleave(group_starts, counts) - inverse_sorted = inverse[order] - counts_sorted = counts[inverse_sorted] - true_added = torch.zeros(unique_ids.shape, device=data.device, dtype=true_count.dtype) - true_added.scatter_add_(0, inverse, values.to(dtype=true_count.dtype)) - - keep_start = (counts - capacity).clamp(min=0) - keep = local_rank >= torch.repeat_interleave(keep_start, counts) - true_kept = torch.zeros_like(true_added) - true_kept.scatter_add_(0, inverse_sorted[keep], sorted_values[keep].to(dtype=true_count.dtype)) - - overwrite_start = capacity - size[sorted_ids].long() - overwrite_mask = (counts_sorted < capacity) & (local_rank >= overwrite_start) - overwritten = torch.zeros_like(true_added) - overwrite_ids = sorted_ids[overwrite_mask] - overwrite_columns = (pointer[overwrite_ids].long() + local_rank[overwrite_mask]) % capacity - overwritten.scatter_add_( - 0, - inverse_sorted[overwrite_mask], - data[overwrite_ids, overwrite_columns].to(dtype=true_count.dtype), - ) - - kept_ids = sorted_ids[keep] - kept_columns = (pointer[kept_ids].long() + local_rank[keep]) % capacity - data[kept_ids, kept_columns] = sorted_values[keep] - replace = counts >= capacity - new_true_counts = torch.where(replace, true_kept, true_count[unique_ids] - overwritten + true_added) - new_size = (size[unique_ids].long() + counts).clamp(max=capacity) - pointer[unique_ids] = ((pointer[unique_ids].long() + counts) % capacity).to(dtype=pointer.dtype) - size[unique_ids] = new_size.to(dtype=size.dtype) - true_count[unique_ids] = new_true_counts - rate[unique_ids] = new_true_counts.to(rate.dtype) / new_size.clamp(min=1).to(rate.dtype) - - def reset_sampling_probabilities( recipe_ids: torch.Tensor, variant_ids: torch.Tensor, target_cube_ids: torch.Tensor, source_side_ids: torch.Tensor, - attempts: torch.Tensor, - successes: torch.Tensor, + target_weights: torch.Tensor, deployment_probability: float | torch.Tensor, - epsilon: float, ) -> torch.Tensor: - """Mix guaranteed deployment starts with adaptive intermediate rows.""" + """Balance target-rate weights and mix in guaranteed deployment starts.""" if not ( - recipe_ids.shape - == variant_ids.shape - == target_cube_ids.shape - == source_side_ids.shape - == attempts.shape - == successes.shape + recipe_ids.shape == variant_ids.shape == target_cube_ids.shape == source_side_ids.shape == target_weights.shape ): - raise ValueError("Reset row metadata and outcomes must have matching shapes.") + raise ValueError("Reset row metadata and target weights must have matching shapes.") deployment_probability = torch.as_tensor( deployment_probability, dtype=torch.float32, - device=attempts.device, + device=target_weights.device, ) if deployment_probability.numel() != 1: raise ValueError("deployment_probability must be a scalar.") deployment_probability = deployment_probability.reshape(()) if bool((deployment_probability <= 0.0) | (deployment_probability >= 1.0)): raise ValueError("deployment_probability must lie strictly between zero and one.") - if epsilon <= 0.0: - raise ValueError("epsilon must be positive.") + if not bool(torch.all(torch.isfinite(target_weights))) or bool(torch.any(target_weights < 0.0)): + raise ValueError("target_weights must be finite and non-negative.") deployment_rows = (recipe_ids == int(ConveyorResetRecipe.BELT)) & (variant_ids == BELT_DEPLOYMENT_VARIANT) if not bool(torch.any(deployment_rows)) or bool(torch.all(deployment_rows)): raise ValueError("Reset table must contain deployment and intermediate rows.") - rates = successes.float() / attempts.clamp_min(1).float() - frontier = 4.0 * rates * (1.0 - rates) - adaptive = frontier + epsilon + adaptive = target_weights.clone() adaptive[deployment_rows] = 0.0 # Mirror Franka Stack's recipe/layout balancing: success in one physical @@ -191,14 +116,15 @@ def __init__(self, cfg: CurriculumTermCfg, env: ManagerBasedRLEnv): self._attempts = torch.zeros(reset_term.row_count, dtype=torch.long, device=env.device) self._progress_successes = torch.zeros_like(self._attempts) self._final_successes = torch.zeros_like(self._attempts) - history_len = int(cfg.params.get("monitored_history_len", 50)) - if history_len < 1: - raise ValueError("monitored_history_len must be positive.") - self._progress_history = torch.zeros((reset_term.row_count, history_len), dtype=torch.bool, device=env.device) - self._history_pointer = torch.zeros(reset_term.row_count, dtype=torch.int32, device=env.device) - self._history_size = torch.zeros_like(self._history_pointer) - self._history_success_count = torch.zeros_like(self._history_pointer) - self._rolling_progress_rates = torch.zeros(reset_term.row_count, dtype=torch.float32, device=env.device) + monitor_cfg = cfg.params.get("success_monitor") + if not isinstance(monitor_cfg, SuccessMonitorCfg): + raise TypeError("ConveyorResetCurriculum requires a SuccessMonitorCfg.") + self._progress_monitor = monitor_cfg.class_type( + monitor_cfg, + num_partitions=1, + partition_size=reset_term.row_count, + device=env.device, + ) variant_counts = reset_variant_counts() self._diagnostic_variant_rows = tuple( ( @@ -214,6 +140,7 @@ def __call__( self, env: ManagerBasedRLEnv, env_ids: Sequence[int], + success_monitor: SuccessMonitorCfg, progress_context_name: str = "learning_progress_context", final_success_context_name: str = "transfer_success_context", deployment_probability_initial: float = 0.35, @@ -221,12 +148,10 @@ def __call__( deployment_progress_start: float = 0.45, deployment_progress_end: float = 0.80, deployment_coverage_target: float = 0.50, - epsilon: float = 0.05, - monitored_history_len: int = 50, fixed_source_side_id: int | None = None, ) -> dict[str, torch.Tensor]: """Update adaptive evidence, sample rows, and expose diagnostics.""" - del monitored_history_len + del success_monitor ids = torch.as_tensor(env_ids, dtype=torch.long, device=env.device).flatten() state = env.conveyor_transfer_state batch_progress = torch.zeros((), dtype=torch.float32, device=env.device) @@ -239,24 +164,18 @@ def __call__( progressed = progress_context.ever_success[completed_ids] succeeded = final_success.ever_success[completed_ids] rows = state.row_ids[completed_ids] - _ring_append_bool_count_rate( - self._progress_history, - rows, - progressed, - self._history_pointer, - self._history_size, - self._history_success_count, - self._rolling_progress_rates, - ) + self._progress_monitor.success_update(rows, progressed) self._attempts.scatter_add_(0, rows, torch.ones_like(rows)) self._progress_successes.scatter_add_(0, rows, progressed.long()) self._final_successes.scatter_add_(0, rows, succeeded.long()) batch_progress = progressed.float().mean() batch_success = succeeded.float().mean() - attempted_rows = self._history_size > 0 + history_size = self._progress_monitor.success_size + history_success_count = self._progress_monitor.success_buf.sum(dim=1) + attempted_rows = history_size > 0 row_coverage = attempted_rows.float().mean() - total_progress = self._history_success_count.sum().float() / self._history_size.sum().clamp_min(1) + total_progress = history_success_count.sum() / history_size.sum().clamp_min(1) deployment_probability = deployment_probability_from_progress( total_progress, row_coverage, @@ -271,10 +190,8 @@ def __call__( self._reset_term.variant_ids, self._reset_term.target_cube_ids, self._reset_term.source_side_ids, - self._history_size, - self._history_success_count, + self._progress_monitor.target_weights(), deployment_probability, - epsilon, ) if fixed_source_side_id is not None: if fixed_source_side_id not in (0, 1): @@ -317,46 +234,45 @@ def __call__( mask = self._reset_term.recipe_ids == int(recipe) recipe_attempts = self._attempts[mask].sum() metrics[f"recipe_{recipe.name.lower()}_probability"] = probabilities[mask].sum() - recipe_history_size = self._history_size[mask].sum() - metrics[f"recipe_{recipe.name.lower()}_progress_rate"] = self._history_success_count[ + recipe_history_size = history_size[mask].sum() + metrics[f"recipe_{recipe.name.lower()}_progress_rate"] = history_success_count[ mask - ].sum().float() / recipe_history_size.clamp_min(1) + ].sum() / recipe_history_size.clamp_min(1) metrics[f"recipe_{recipe.name.lower()}_success_rate"] = self._final_successes[ mask ].sum().float() / recipe_attempts.clamp_min(1) for side_id, side_name in ((0, "left_to_right"), (1, "right_to_left")): mask = self._reset_term.source_side_ids == side_id attempts = self._attempts[mask].sum() - history_size = self._history_size[mask].sum() + direction_history_size = history_size[mask].sum() metrics[f"direction_{side_name}_probability"] = probabilities[mask].sum() - metrics[f"direction_{side_name}_progress_rate"] = self._history_success_count[ + metrics[f"direction_{side_name}_progress_rate"] = history_success_count[ mask - ].sum().float() / history_size.clamp_min(1) + ].sum() / direction_history_size.clamp_min(1) metrics[f"direction_{side_name}_success_rate"] = self._final_successes[ mask ].sum().float() / attempts.clamp_min(1) for recipe_name, variant_id, mask in self._diagnostic_variant_rows: attempts = self._attempts[mask].sum() - history_size = self._history_size[mask].sum() + variant_history_size = history_size[mask].sum() prefix = f"recipe_{recipe_name}_variant_{variant_id}" metrics[f"{prefix}_probability"] = probabilities[mask].sum() - metrics[f"{prefix}_progress_rate"] = self._history_success_count[ - mask - ].sum().float() / history_size.clamp_min(1) + metrics[f"{prefix}_progress_rate"] = history_success_count[mask].sum() / variant_history_size.clamp_min(1) metrics[f"{prefix}_success_rate"] = self._final_successes[mask].sum().float() / attempts.clamp_min(1) return metrics def get_state(self) -> dict[str, torch.Tensor]: """Return curriculum evidence for checkpointing.""" + history_success_count = self._progress_monitor.success_buf.sum(dim=1).long() return { "attempts": self._attempts.clone(), "progress_successes": self._progress_successes.clone(), "final_successes": self._final_successes.clone(), - "progress_history": self._progress_history.clone(), - "history_pointer": self._history_pointer.clone(), - "history_size": self._history_size.clone(), - "history_success_count": self._history_success_count.clone(), - "rolling_progress_rates": self._rolling_progress_rates.clone(), + "progress_history": self._progress_monitor.success_buf.bool().clone(), + "history_pointer": self._progress_monitor.success_pointer.clone(), + "history_size": self._progress_monitor.success_size.clone(), + "history_success_count": history_success_count, + "rolling_progress_rates": self._progress_monitor.success_rate.clone(), } def set_state(self, state: dict[str, torch.Tensor]) -> None: @@ -365,16 +281,17 @@ def set_state(self, state: dict[str, torch.Tensor]) -> None: "attempts": self._attempts, "progress_successes": self._progress_successes, "final_successes": self._final_successes, - "progress_history": self._progress_history, - "history_pointer": self._history_pointer, - "history_size": self._history_size, - "history_success_count": self._history_success_count, - "rolling_progress_rates": self._rolling_progress_rates, + "progress_history": self._progress_monitor.success_buf, + "history_pointer": self._progress_monitor.success_pointer, + "history_size": self._progress_monitor.success_size, + "rolling_progress_rates": self._progress_monitor.success_rate, } for name, target in targets.items(): if name not in state or state[name].shape != target.shape: raise ValueError(f"Conveyor curriculum checkpoint has invalid '{name}'.") - history_len = self._progress_history.shape[1] + if "history_success_count" not in state or state["history_success_count"].shape != self._attempts.shape: + raise ValueError("Conveyor curriculum checkpoint has invalid 'history_success_count'.") + history_len = self._progress_monitor.success_buf.shape[1] if bool(torch.any((state["history_pointer"] < 0) | (state["history_pointer"] >= history_len))): raise ValueError("Conveyor curriculum checkpoint has invalid history pointers.") if bool(torch.any((state["history_size"] < 0) | (state["history_size"] > history_len))): diff --git a/source/isaaclab_tasks/test/contrib/test_conveyor_franka_mdp.py b/source/isaaclab_tasks/test/contrib/test_conveyor_franka_mdp.py index fbaa8c0b4184..31aa16b42e80 100644 --- a/source/isaaclab_tasks/test/contrib/test_conveyor_franka_mdp.py +++ b/source/isaaclab_tasks/test/contrib/test_conveyor_franka_mdp.py @@ -17,7 +17,6 @@ ) from isaaclab_tasks.contrib.conveyor_franka.conveyor_franka_env_cfg import ConveyorFrankaEnvCfg from isaaclab_tasks.contrib.conveyor_franka.mdp.curriculums import ( - _ring_append_bool_count_rate, deployment_probability_from_progress, reset_sampling_probabilities, ) @@ -220,14 +219,14 @@ def test_reset_sampling_guarantees_deployment_mass_and_tracks_frontier(): variant_ids = torch.tensor([row.variant_id for row in rows], dtype=torch.long) target_cube_ids = torch.tensor([row.target_cube_id for row in rows], dtype=torch.long) source_side_ids = torch.tensor([row.source_side_id for row in rows], dtype=torch.long) - attempts = torch.zeros(len(rows), dtype=torch.long) - successes = torch.zeros_like(attempts) place_stratum = (recipe_ids == int(ConveyorResetRecipe.PLACE)) & (target_cube_ids == 0) & (source_side_ids == 0) place_ids = torch.nonzero(place_stratum, as_tuple=False).flatten() - attempts[place_ids[0]] = 100 - successes[place_ids[0]] = 100 - attempts[place_ids[1]] = 100 - successes[place_ids[1]] = 50 + monitor_cfg = ConveyorFrankaEnvCfg().curriculum.reset_sampling.params["success_monitor"] + monitor = monitor_cfg.class_type(monitor_cfg, num_partitions=1, partition_size=len(rows), device="cpu") + monitor.success_update( + torch.cat((place_ids[0].repeat(50), place_ids[1].repeat(50))), + torch.cat((torch.ones(50, dtype=torch.bool), torch.arange(50) % 2 == 0)), + ) deployment_rows = (recipe_ids == int(ConveyorResetRecipe.BELT)) & ( variant_ids == reset_variant_counts()[int(ConveyorResetRecipe.BELT)] - 1 @@ -237,10 +236,8 @@ def test_reset_sampling_guarantees_deployment_mass_and_tracks_frontier(): variant_ids, target_cube_ids, source_side_ids, - attempts, - successes, + monitor.target_weights(), deployment_probability=0.35, - epsilon=0.05, ) torch.testing.assert_close(probabilities.sum(), torch.tensor(1.0)) @@ -363,25 +360,19 @@ def test_continuing_training_truncates_only_stalled_or_long_sequences(): def test_rolling_progress_monitor_forgets_stale_outcomes_in_order(): """Per-row curriculum evidence retains only each row's latest outcomes.""" - history = torch.zeros((2, 3), dtype=torch.bool) - pointer = torch.zeros(2, dtype=torch.int32) - size = torch.zeros_like(pointer) - true_count = torch.zeros_like(pointer) - rate = torch.zeros(2) - - _ring_append_bool_count_rate( - history, + monitor_cfg = ( + ConveyorFrankaEnvCfg().curriculum.reset_sampling.params["success_monitor"].replace(monitored_history_len=3) + ) + monitor = monitor_cfg.class_type(monitor_cfg, num_partitions=1, partition_size=2, device="cpu") + + monitor.success_update( torch.tensor([0, 0, 1, 0, 0]), torch.tensor([False, False, True, True, True]), - pointer, - size, - true_count, - rate, ) - assert size.tolist() == [3, 1] - assert true_count.tolist() == [2, 1] - torch.testing.assert_close(rate, torch.tensor([2.0 / 3.0, 1.0])) + assert monitor.success_size.tolist() == [3, 1] + assert monitor.success_buf.sum(dim=1).tolist() == [2, 1] + torch.testing.assert_close(monitor.get_success_rate(), torch.tensor([2.0 / 3.0, 1.0])) def test_active_cube_sampling_avoids_inactive_source_lane_cubes(): From d33cd7bb7eca186e17ce15db6e61f2e26dfb3fed Mon Sep 17 00:00:00 2001 From: Maximilian Krause Date: Wed, 12 Aug 2026 15:49:41 -0700 Subject: [PATCH 10/23] Refactor conveyor goals into command term --- .../conveyor_franka_env_cfg.py | 74 ++-- .../conveyor_franka/conveyor_goal_selector.py | 12 +- .../contrib/conveyor_franka/mdp/__init__.py | 7 +- .../contrib/conveyor_franka/mdp/actions.py | 6 +- .../conveyor_franka/mdp/actions_cfg.py | 3 + .../contrib/conveyor_franka/mdp/commands.py | 346 ++++++++++++++++++ .../conveyor_franka/mdp/curriculums.py | 23 +- .../conveyor_franka/mdp/observations.py | 29 +- .../conveyor_franka/mdp/reset_events.py | 137 +------ .../contrib/conveyor_franka/mdp/rewards.py | 64 ++-- .../contrib/conveyor_franka/mdp/state.py | 50 --- .../conveyor_franka/mdp/terminations.py | 204 +---------- .../contrib/test_conveyor_franka_geometry.py | 170 +-------- .../test/contrib/test_conveyor_franka_mdp.py | 121 +++--- 14 files changed, 518 insertions(+), 728 deletions(-) create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/commands.py delete mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/state.py diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env_cfg.py index b1bd4cd3df32..94e157526406 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env_cfg.py @@ -52,7 +52,6 @@ _MANIPULATION_CONTACT_DAMPING = 200.0 _MUJOCO_SOLIMP = (0.9, 0.95, 0.001, 0.5, 2.0) _MUJOCO_SOLREF = (0.02, 1.0) -_POLICY_DT = 1.0 / 60.0 _SUBGOAL_TIMEOUT_S = 20.0 _TRANSFER_SEQUENCE_LENGTH = 8 @@ -99,6 +98,27 @@ class ActionsCfg: ) +@configclass +class CommandsCfg: + """Success-driven cube-transfer command.""" + + transfer = mdp.ConveyorTransferCommandCfg( + reset_event_name="reset_from_state_table", + minimum_subgoal_steps=2, + hold_steps=3, + lateral_tolerance=0.055, + maximum_cube_speed=0.65, + minimum_finger_position=0.027, + minimum_tool_clearance=0.055, + minimum_progress_steps=3, + minimum_progress=0.35, + maximum_target_potential=5.0, + minimum_acquisition_lift=0.025, + maximum_acquisition_tool_distance=0.075, + maximum_acquisition_finger_position=0.030, + ) + + @configclass class ObservationsCfg: """Policy observations with stable cube identity and transfer commands.""" @@ -137,7 +157,7 @@ def __post_init__(self) -> None: @configclass class EventCfg: - """Restore validated states and advance completed transfer goals.""" + """Restore validated physical reset states.""" reset_all = EventTerm(func=mdp.reset_scene_to_default, mode="reset") reset_from_state_table = EventTerm( @@ -153,12 +173,6 @@ class EventCfg: "arm_joint_noise": 0.015, }, ) - advance_transfer_goal = EventTerm( - func=mdp.advance_conveyor_transfer_goal, - mode="interval", - interval_range_s=(_POLICY_DT, _POLICY_DT), - params={"success_context_name": "transfer_success_context"}, - ) @configclass @@ -167,12 +181,12 @@ class RewardsCfg: success = RewTerm( func=mdp.transfer_success_reward, - params={"context_term_name": "transfer_success_context"}, + params={"command_name": "transfer"}, weight=600.0, ) failure = RewTerm( func=mdp.terminal_failure, - params={"success_context_name": "transfer_success_context"}, + params={"command_name": "transfer"}, weight=-60.0, ) arm_action_l2 = RewTerm( @@ -190,40 +204,18 @@ class RewardsCfg: @configclass class TerminationsCfg: - """Continuing transfer context, safety failures, and training truncations.""" + """Safety failures and bounded training sequences.""" - learning_progress_context = DoneTerm( - func=mdp.ConveyorResetLearningProgress, - params={ - "minimum_episode_steps": 3, - "minimum_progress": 0.35, - "maximum_target_potential": 5.0, - "minimum_acquisition_lift": 0.025, - "maximum_acquisition_tool_distance": 0.075, - "maximum_acquisition_finger_position": 0.030, - }, - ) - transfer_success_context = DoneTerm( - func=mdp.StableConveyorTransfer, - params={ - "minimum_episode_steps": 2, - "hold_steps": 3, - "lateral_tolerance": 0.055, - "maximum_cube_speed": 0.65, - "minimum_finger_position": 0.027, - "minimum_tool_clearance": 0.055, - }, - ) cube_out_of_workspace = DoneTerm(func=mdp.cube_out_of_workspace) nonfinite_scene_state = DoneTerm(func=mdp.nonfinite_scene_state) subgoal_time_out = DoneTerm( func=mdp.subgoal_time_out, - params={"timeout_s": _SUBGOAL_TIMEOUT_S}, + params={"timeout_s": _SUBGOAL_TIMEOUT_S, "command_name": "transfer"}, time_out=True, ) transfer_sequence_time_out = DoneTerm( func=mdp.transfer_sequence_time_out, - params={"maximum_transfers": _TRANSFER_SEQUENCE_LENGTH}, + params={"maximum_transfers": _TRANSFER_SEQUENCE_LENGTH, "command_name": "transfer"}, time_out=True, ) @@ -235,8 +227,7 @@ class CurriculumCfg: reset_sampling = CurrTerm( func=mdp.ConveyorResetCurriculum, params={ - "progress_context_name": "learning_progress_context", - "final_success_context_name": "transfer_success_context", + "command_name": "transfer", # Shared target-rate monitor keeps each physical reset row near the # policy's 50% competence frontier without stale early outcomes. "success_monitor": mdp.SuccessMonitorCfg( @@ -636,9 +627,10 @@ def __post_init__(self) -> None: class ConveyorFrankaEnvCfg(ManagerBasedRLEnvCfg): """Manager-based RL task for commanded conveyor-to-conveyor cube transfer.""" - scene: ConveyorFrankaSceneCfg = ConveyorFrankaSceneCfg(num_envs=1, env_spacing=3.0, replicate_physics=True) + scene: ConveyorFrankaSceneCfg = ConveyorFrankaSceneCfg(num_envs=256, env_spacing=3.0, replicate_physics=True) conveyor_force: ConveyorForceCfg = ConveyorForceCfg() actions: ActionsCfg = ActionsCfg() + commands: CommandsCfg = CommandsCfg() observations: ObservationsCfg = ObservationsCfg() events: EventCfg = EventCfg() rewards: RewardsCfg = RewardsCfg() @@ -685,11 +677,13 @@ def __post_init__(self) -> None: if exc.name != "isaaclab_visualizers": raise return - from isaaclab_visualizers.newton import NewtonVisualizerCfg + from isaaclab_visualizers.newton import NewtonGLVisualizerCfg - self.sim.default_visualizer_cfg = NewtonVisualizerCfg( + # Explicit --viz newton_rtx replaces this backend while retaining the shared camera hints. + self.sim.default_visualizer_cfg = NewtonGLVisualizerCfg( eye=(2.3, -2.7, 1.8), lookat=(0.45, 0.0, 0.35), + streaming_view=False, ) def play_mode(self) -> None: diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_goal_selector.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_goal_selector.py index 1d9c39737720..853580a8a95e 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_goal_selector.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_goal_selector.py @@ -11,7 +11,7 @@ from typing import TYPE_CHECKING, Any from .conveyor_geometry import CUBE_COLORS -from .mdp.reset_events import LEFT_SIDE, set_conveyor_transfer_goal +from .mdp.reset_events import LEFT_SIDE if TYPE_CHECKING: from isaaclab.envs import ManagerBasedRLEnv @@ -35,6 +35,7 @@ def __init__(self, env: ManagerBasedRLEnv, env_id: int) -> None: raise IndexError(f"Conveyor selector environment {env_id} is out of range.") self._env = env self._env_id = env_id + self._command = env.command_manager.get_term("transfer") self._target_cube_id = 0 self._source_side_id = LEFT_SIDE self._last_refresh_time = float("-inf") @@ -86,7 +87,7 @@ def render(self, imgui: Any) -> None: imgui.same_line() if selected_cube_id is not None and selected_cube_id != self._target_cube_id: - set_conveyor_transfer_goal(self._env, selected_cube_id, env_ids=(self._env_id,)) + self._command.set_goal(selected_cube_id, env_ids=(self._env_id,)) self._refresh_command(force=True) source_name = "Left" if self._source_side_id == LEFT_SIDE else "Right" @@ -96,12 +97,9 @@ def render(self, imgui: Any) -> None: def _refresh_command(self, force: bool = False) -> bool: """Refresh the small host-side UI cache at most ten times per second.""" - state = getattr(self._env, "conveyor_transfer_state", None) - if state is None: - return False current_time = time.monotonic() if force or current_time - self._last_refresh_time >= 0.1: - self._target_cube_id = int(state.target_cube_ids[self._env_id].item()) - self._source_side_id = int(state.source_side_ids[self._env_id].item()) + self._target_cube_id = int(self._command.target_cube_ids[self._env_id].item()) + self._source_side_id = int(self._command.source_side_ids[self._env_id].item()) self._last_refresh_time = current_time return True diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/__init__.py index a5ec84b8f123..ce1212bf20c1 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/__init__.py @@ -10,6 +10,7 @@ from .actions import ConveyorRelativeJointPositionAction, ResetBufferedGripperAction from .actions_cfg import ConveyorRelativeJointPositionActionCfg, ResetBufferedGripperActionCfg +from .commands import ConveyorTransferCommand, ConveyorTransferCommandCfg, transfer_success_mask from .curriculums import ConveyorResetCurriculum from .observations import ( active_transfer_features, @@ -25,23 +26,17 @@ BELT_DEPLOYMENT_VARIANT, ConveyorResetRecipe, ConveyorResetStateTable, - advance_conveyor_transfer_goal, build_reset_rows, select_next_transfer_cube, - set_conveyor_transfer_goal, ) from .rewards import ( - ConveyorTransferProgressReward, action_term_l2, finite_joint_velocity_l2, physical_cube_acquisition_mask, terminal_failure, transfer_success_reward, ) -from .state import ConveyorTransferState from .terminations import ( - ConveyorResetLearningProgress, - StableConveyorTransfer, cube_out_of_workspace, nonfinite_scene_state, subgoal_time_out, diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/actions.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/actions.py index 7fc5de5e8ec5..4b64617a6aee 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/actions.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/actions.py @@ -89,8 +89,6 @@ class ResetBufferedGripperAction(BinaryJointPositionAction): def process_actions(self, actions: torch.Tensor) -> None: """Map binary commands and preserve initially held cubes.""" super().process_actions(actions) - state = getattr(self._env, "conveyor_transfer_state", None) - if state is None: - return - force_close = (state.held_cube_ids >= 0) & (self._env.episode_length_buf < self.cfg.force_close_steps) + command = self._env.command_manager.get_term(self.cfg.command_name) + force_close = (command.held_cube_ids >= 0) & (self._env.episode_length_buf < self.cfg.force_close_steps) self._processed_actions[force_close] = self._close_command diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/actions_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/actions_cfg.py index c951391bf502..588e85404adb 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/actions_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/actions_cfg.py @@ -46,3 +46,6 @@ class ResetBufferedGripperActionCfg(BinaryJointPositionActionCfg): force_close_steps: int = 5 """Initial policy steps that preserve a reset-authored grasp.""" + + command_name: str = "transfer" + """Command term that owns reset-authored held-cube state.""" diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/commands.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/commands.py new file mode 100644 index 000000000000..6d1d783e3870 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/commands.py @@ -0,0 +1,346 @@ +# 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 + +"""Success-driven transfer commands for the conveyor Franka task.""" + +from __future__ import annotations + +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import torch + +from isaaclab.managers import CommandTerm, CommandTermCfg +from isaaclab.utils.configclass import configclass + +from ..conveyor_geometry import BELT_CENTER_X, BELT_HALF_STRAIGHT +from .kinematics import end_effector_pose +from .reset_events import CUBE_COUNT, ConveyorResetRecipe, select_next_transfer_cube, side_inner_y +from .rewards import current_transfer_potential, physical_cube_acquisition_mask + +if TYPE_CHECKING: + from isaaclab.assets import Articulation, RigidObject + from isaaclab.envs import ManagerBasedRLEnv + + +def transfer_success_mask( + cube_positions: torch.Tensor, + cube_linear_velocities: torch.Tensor, + tool_positions: torch.Tensor, + finger_positions: torch.Tensor, + target_side_ids: torch.Tensor, + lateral_tolerance: float = 0.055, + maximum_cube_speed: float = 0.65, + minimum_finger_position: float = 0.027, + minimum_tool_clearance: float = 0.055, +) -> torch.Tensor: + """Return whether the active cube is released on its destination belt.""" + target_y = side_inner_y(target_side_ids) + on_straight = torch.abs(cube_positions[:, 0] - BELT_CENTER_X) < BELT_HALF_STRAIGHT + on_lane = torch.abs(cube_positions[:, 1] - target_y) < lateral_tolerance + supported_height = (cube_positions[:, 2] > 0.045) & (cube_positions[:, 2] < 0.095) + moving_safely = torch.linalg.vector_norm(cube_linear_velocities, dim=1) < maximum_cube_speed + released = torch.amin(finger_positions, dim=1) > minimum_finger_position + hand_clear = torch.linalg.vector_norm(tool_positions - cube_positions, dim=1) > minimum_tool_clearance + return on_straight & on_lane & supported_height & moving_safely & released & hand_clear + + +class ConveyorTransferCommand(CommandTerm): + """Command one numbered cube to the opposite belt and redraw on success.""" + + cfg: ConveyorTransferCommandCfg + + def __init__(self, cfg: ConveyorTransferCommandCfg, env: ManagerBasedRLEnv) -> None: + self._validate_cfg(cfg) + super().__init__(cfg, env) + + reset_term = env.event_manager.get_term_cfg(cfg.reset_event_name).func + required_reset_fields = ("row_ids", "recipe_ids", "target_cube_ids", "source_side_ids", "held_rows") + if not all(hasattr(reset_term, name) for name in required_reset_fields): + raise RuntimeError("ConveyorTransferCommand requires ConveyorResetStateTable reset metadata.") + self._reset_term = reset_term + self._robot: Articulation = env.scene["robot"] + self._cubes: tuple[RigidObject, ...] = tuple(env.scene[f"cube_{cube_id}"] for cube_id in range(CUBE_COUNT)) + self._finger_joint_ids = self._robot.find_joints("panda_finger_joint[1-2]", preserve_order=True)[0] + + self.target_cube_ids = torch.zeros(self.num_envs, dtype=torch.long, device=self.device) + self.source_side_ids = torch.zeros_like(self.target_cube_ids) + self.recipe_ids = torch.zeros_like(self.target_cube_ids) + self.held_cube_ids = torch.full_like(self.target_cube_ids, -1) + self.subgoal_start_steps = torch.zeros_like(self.target_cube_ids) + self.transfer_counts = torch.zeros_like(self.target_cube_ids) + self.direction_transfer_counts = torch.zeros((self.num_envs, 2), dtype=torch.long, device=self.device) + + self._stable_steps = torch.zeros_like(self.target_cube_ids) + self.is_success = torch.zeros(self.num_envs, dtype=torch.bool, device=self.device) + self.new_success = torch.zeros_like(self.is_success) + self.pending_success = torch.zeros_like(self.is_success) + self.ever_success = torch.zeros_like(self.is_success) + self.progress_ever_success = torch.zeros_like(self.is_success) + self._target_potential = torch.zeros(self.num_envs, dtype=torch.float32, device=self.device) + self._last_evaluation_steps = torch.full_like(self.target_cube_ids, -1) + self._resampling_from_reset = False + + self.metrics["success_rate"] = torch.zeros(self.num_envs, dtype=torch.float32, device=self.device) + self.metrics["transfer_count"] = torch.zeros_like(self.metrics["success_rate"]) + self.metrics["left_to_right_transfers"] = torch.zeros_like(self.metrics["success_rate"]) + self.metrics["right_to_left_transfers"] = torch.zeros_like(self.metrics["success_rate"]) + + @staticmethod + def _validate_cfg(cfg: ConveyorTransferCommandCfg) -> None: + """Validate success and reset-progress thresholds.""" + if cfg.minimum_subgoal_steps < 0 or cfg.hold_steps < 1: + raise ValueError("minimum_subgoal_steps must be non-negative and hold_steps must be positive.") + if cfg.lateral_tolerance <= 0.0 or cfg.maximum_cube_speed <= 0.0: + raise ValueError("Conveyor placement tolerances and speed limits must be positive.") + if cfg.minimum_finger_position <= 0.0 or cfg.minimum_tool_clearance <= 0.0: + raise ValueError("Conveyor release thresholds must be positive.") + if cfg.minimum_progress_steps < 0 or cfg.minimum_progress <= 0.0 or cfg.maximum_target_potential <= 0.0: + raise ValueError("Conveyor reset-progress thresholds are invalid.") + if ( + cfg.minimum_acquisition_lift <= 0.0 + or cfg.maximum_acquisition_tool_distance <= 0.0 + or cfg.maximum_acquisition_finger_position <= 0.0 + ): + raise ValueError("Conveyor acquisition thresholds must be positive.") + + @property + def command(self) -> torch.Tensor: + """Return target-cube and destination-belt one-hot commands.""" + cube = torch.nn.functional.one_hot(self.target_cube_ids, num_classes=CUBE_COUNT) + destination = torch.nn.functional.one_hot(1 - self.source_side_ids, num_classes=2) + return torch.cat((cube, destination), dim=1).float() + + def reset(self, env_ids: Sequence[int] | slice | None = None) -> dict[str, float]: + """Log per-command completion and initialize commands from sampled reset rows.""" + ids = self._resolve_env_ids(env_ids) + valid = self.command_counter[ids] > 0 + attempts = self.command_counter[ids].clamp_min(1) + self.metrics["success_rate"][ids] = torch.where( + valid, + self.transfer_counts[ids].float() / attempts.float(), + 0.0, + ) + self.metrics["transfer_count"][ids] = self.transfer_counts[ids].float() + self.metrics["left_to_right_transfers"][ids] = self.direction_transfer_counts[ids, 0].float() + self.metrics["right_to_left_transfers"][ids] = self.direction_transfer_counts[ids, 1].float() + + self._resampling_from_reset = True + try: + extras = super().reset(ids) + finally: + self._resampling_from_reset = False + + initial_potential = current_transfer_potential(self._env, command=self) + self._target_potential[ids] = torch.clamp_max( + initial_potential[ids] + self.cfg.minimum_progress, + self.cfg.maximum_target_potential, + ) + self._last_evaluation_steps[ids] = -1 + self._env.extras.setdefault("log", {})["Metrics/success_rate"] = extras.pop("success_rate") + return extras + + def evaluate(self) -> None: + """Evaluate stable completion and reset-learning progress once per policy step.""" + evaluation_steps = self._env.episode_length_buf + evaluate_mask = (self.command_counter > 0) & (self._last_evaluation_steps != evaluation_steps) + if not bool(torch.any(evaluate_mask)): + return + + positions = torch.stack(tuple(cube.data.root_pos_w.torch for cube in self._cubes), dim=1) + velocities = torch.stack(tuple(cube.data.root_lin_vel_w.torch for cube in self._cubes), dim=1) + index = self.target_cube_ids.view(self.num_envs, 1, 1).expand(-1, 1, 3) + active_position = torch.gather(positions, 1, index).squeeze(1) - self._env.scene.env_origins + active_velocity = torch.gather(velocities, 1, index).squeeze(1) + tool_position, _ = end_effector_pose(self._env) + tool_position = tool_position - self._env.scene.env_origins + finger_positions = self._robot.data.joint_pos.torch[:, self._finger_joint_ids] + successful = transfer_success_mask( + active_position, + active_velocity, + tool_position, + finger_positions, + 1 - self.source_side_ids, + lateral_tolerance=self.cfg.lateral_tolerance, + maximum_cube_speed=self.cfg.maximum_cube_speed, + minimum_finger_position=self.cfg.minimum_finger_position, + minimum_tool_clearance=self.cfg.minimum_tool_clearance, + ) + subgoal_steps = evaluation_steps - self.subgoal_start_steps + successful &= subgoal_steps >= self.cfg.minimum_subgoal_steps + + next_stable_steps = torch.where(successful, self._stable_steps + 1, torch.zeros_like(self._stable_steps)) + self._stable_steps[evaluate_mask] = next_stable_steps[evaluate_mask] + stable = self._stable_steps >= self.cfg.hold_steps + new_success = evaluate_mask & stable & ~self.is_success & ~self.pending_success + self.is_success[evaluate_mask] = stable[evaluate_mask] + self.new_success.copy_(new_success) + self.pending_success |= new_success + self.ever_success |= new_success + success_ids = new_success.nonzero(as_tuple=False).squeeze(-1) + if success_ids.numel(): + source_sides = self.source_side_ids[success_ids] + self.transfer_counts[success_ids] += 1 + self.direction_transfer_counts[success_ids, source_sides] += 1 + + potential = current_transfer_potential(self._env, command=self) + progressed = (potential >= self._target_potential) & (evaluation_steps >= self.cfg.minimum_progress_steps) + acquisition_recipe = ( + (self.recipe_ids == int(ConveyorResetRecipe.GRASP)) + | (self.recipe_ids == int(ConveyorResetRecipe.PREGRASP)) + | (self.recipe_ids == int(ConveyorResetRecipe.BELT)) + ) + physically_acquired = physical_cube_acquisition_mask( + self._env, + command=self, + minimum_lift=self.cfg.minimum_acquisition_lift, + maximum_tool_distance=self.cfg.maximum_acquisition_tool_distance, + maximum_finger_position=self.cfg.maximum_acquisition_finger_position, + ) + progressed &= ~acquisition_recipe | physically_acquired + self.progress_ever_success |= evaluate_mask & progressed + self._last_evaluation_steps[evaluate_mask] = evaluation_steps[evaluate_mask] + self._env.extras["successes"] = self.ever_success.clone() + + def set_goal(self, target_cube_id: int, env_ids: Sequence[int] | torch.Tensor | None = None) -> None: + """Replace the active command using the selected cube's current conveyor.""" + if not isinstance(target_cube_id, int) or isinstance(target_cube_id, bool): + raise TypeError("target_cube_id must be an integer.") + if not 0 <= target_cube_id < CUBE_COUNT: + raise ValueError(f"target_cube_id must lie in [0, {CUBE_COUNT - 1}].") + ids = self._resolve_env_ids(env_ids) + if ids.numel() == 0: + return + cube = self._cubes[target_cube_id] + local_y = cube.data.root_pos_w.torch[ids, 1] - self._env.scene.env_origins[ids, 1] + source_side_ids = (local_y < 0.0).long() + target_cube_ids = torch.full_like(ids, target_cube_id) + self._assign_goal(ids, target_cube_ids, source_side_ids) + + def _update_metrics(self) -> None: + self.evaluate() + + def _resample_command(self, env_ids: Sequence[int]) -> None: + ids = self._resolve_env_ids(env_ids) + if ids.numel() == 0: + return + if self._resampling_from_reset: + rows = self._reset_term.row_ids[ids] + target_cube_ids = self._reset_term.target_cube_ids[rows] + source_side_ids = self._reset_term.source_side_ids[rows] + self.recipe_ids[ids] = self._reset_term.recipe_ids[rows] + held_rows = self._reset_term.held_rows[rows] + self.transfer_counts[ids] = 0 + self.direction_transfer_counts[ids] = 0 + self.ever_success[ids] = False + self.progress_ever_success[ids] = False + self._assign_goal(ids, target_cube_ids, source_side_ids) + self.held_cube_ids[ids] = torch.where(held_rows, target_cube_ids, -1) + self.subgoal_start_steps[ids] = 0 + return + + positions = torch.stack(tuple(cube.data.root_pos_w.torch[ids] for cube in self._cubes), dim=1) + positions -= self._env.scene.env_origins[ids].unsqueeze(1) + next_source_side_ids = 1 - self.source_side_ids[ids] + next_cube_ids = select_next_transfer_cube( + positions, + self.target_cube_ids[ids], + next_source_side_ids, + transit_half_width=self.cfg.transit_half_width, + ) + self._assign_goal(ids, next_cube_ids, next_source_side_ids) + + def _update_command(self) -> None: + completed_ids = self.pending_success.nonzero(as_tuple=False).squeeze(-1) + if completed_ids.numel(): + self._resample(completed_ids) + + def _assign_goal( + self, + env_ids: torch.Tensor, + target_cube_ids: torch.Tensor, + source_side_ids: torch.Tensor, + ) -> None: + """Publish one command and clear completion state from the previous command.""" + self.target_cube_ids[env_ids] = target_cube_ids + self.source_side_ids[env_ids] = source_side_ids + self.held_cube_ids[env_ids] = -1 + self.subgoal_start_steps[env_ids] = self._env.episode_length_buf[env_ids] + self._stable_steps[env_ids] = 0 + self.is_success[env_ids] = False + self.new_success[env_ids] = False + self.pending_success[env_ids] = False + self._last_evaluation_steps[env_ids] = -1 + + def _resolve_env_ids(self, env_ids: Sequence[int] | torch.Tensor | slice | None) -> torch.Tensor: + """Return validated environment indices on the command device.""" + if env_ids is None: + ids = torch.arange(self.num_envs, dtype=torch.long, device=self.device) + elif isinstance(env_ids, slice): + ids = torch.arange(self.num_envs, dtype=torch.long, device=self.device)[env_ids] + else: + ids = torch.as_tensor(env_ids, dtype=torch.long, device=self.device).flatten() + if bool(torch.any((ids < 0) | (ids >= self.num_envs))): + raise IndexError("Conveyor command environment indices are out of range.") + return ids + + def _set_debug_vis_impl(self, debug_vis: bool) -> None: + raise NotImplementedError("Conveyor commands are visualized by the Newton goal selector.") + + def _debug_vis_callback(self, event) -> None: + raise NotImplementedError("Conveyor commands are visualized by the Newton goal selector.") + + +@configclass +class ConveyorTransferCommandCfg(CommandTermCfg): + """Configuration for success-driven, bidirectional cube-transfer commands.""" + + class_type: type[ConveyorTransferCommand] | str = "{DIR}.commands:ConveyorTransferCommand" + """Command-term implementation resolved lazily by the manager.""" + + resampling_time_range: tuple[float, float] = (1.0e6, 1.0e6) + """Time-based resampling interval [s]; success normally resamples first.""" + + reset_event_name: str = "reset_from_state_table" + """Reset event containing the immutable physical-state table.""" + + transit_half_width: float = 0.14 + """Half-width of the corridor excluded when selecting a source-belt cube [m].""" + + minimum_subgoal_steps: int = 2 + """Minimum policy steps before a placement can succeed.""" + + hold_steps: int = 3 + """Consecutive policy steps for which a placement must remain valid.""" + + lateral_tolerance: float = 0.055 + """Maximum lateral error from the destination belt center [m].""" + + maximum_cube_speed: float = 0.65 + """Maximum cube speed for a completed placement [m/s].""" + + minimum_finger_position: float = 0.027 + """Minimum position of each finger for the cube to count as released [m].""" + + minimum_tool_clearance: float = 0.055 + """Minimum tool-to-cube distance for the cube to count as released [m].""" + + minimum_progress_steps: int = 3 + """Minimum policy steps before reset-learning progress can be credited.""" + + minimum_progress: float = 0.35 + """Required increase in the dimensionless transfer potential after reset.""" + + maximum_target_potential: float = 5.0 + """Upper bound for the dimensionless reset-progress target.""" + + minimum_acquisition_lift: float = 0.025 + """Minimum cube lift used to validate physical acquisition [m].""" + + maximum_acquisition_tool_distance: float = 0.075 + """Maximum tool-to-cube distance used to validate physical acquisition [m].""" + + maximum_acquisition_finger_position: float = 0.030 + """Maximum finger position used to validate physical acquisition [m].""" diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/curriculums.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/curriculums.py index 67b937aec5dd..3d70ad663e5c 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/curriculums.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/curriculums.py @@ -141,8 +141,7 @@ def __call__( env: ManagerBasedRLEnv, env_ids: Sequence[int], success_monitor: SuccessMonitorCfg, - progress_context_name: str = "learning_progress_context", - final_success_context_name: str = "transfer_success_context", + command_name: str = "transfer", deployment_probability_initial: float = 0.35, deployment_probability_final: float = 0.90, deployment_progress_start: float = 0.45, @@ -153,17 +152,15 @@ def __call__( """Update adaptive evidence, sample rows, and expose diagnostics.""" del success_monitor ids = torch.as_tensor(env_ids, dtype=torch.long, device=env.device).flatten() - state = env.conveyor_transfer_state + command = env.command_manager.get_term(command_name) batch_progress = torch.zeros((), dtype=torch.float32, device=env.device) batch_success = torch.zeros((), dtype=torch.float32, device=env.device) - completed = state.initialized[ids] & (env.episode_length_buf[ids] > 0) + completed = (command.command_counter[ids] > 0) & (env.episode_length_buf[ids] > 0) completed_ids = ids[completed] if completed_ids.numel(): - progress_context = env.termination_manager.get_term_cfg(progress_context_name).func - final_success = env.termination_manager.get_term_cfg(final_success_context_name).func - progressed = progress_context.ever_success[completed_ids] - succeeded = final_success.ever_success[completed_ids] - rows = state.row_ids[completed_ids] + progressed = command.progress_ever_success[completed_ids] + succeeded = command.ever_success[completed_ids] + rows = self._reset_term.row_ids[completed_ids] self._progress_monitor.success_update(rows, progressed) self._attempts.scatter_add_(0, rows, torch.ones_like(rows)) self._progress_successes.scatter_add_(0, rows, progressed.long()) @@ -199,7 +196,7 @@ def __call__( probabilities *= self._reset_term.source_side_ids == fixed_source_side_id probabilities /= probabilities.sum() if ids.numel(): - state.row_ids[ids] = torch.multinomial(probabilities, ids.numel(), replacement=True) + self._reset_term.row_ids[ids] = torch.multinomial(probabilities, ids.numel(), replacement=True) cumulative_progress = self._progress_successes.sum().float() / self._attempts.sum().clamp_min(1) total_success = self._final_successes.sum().float() / self._attempts.sum().clamp_min(1) @@ -209,17 +206,17 @@ def __call__( "batch_progress_rate": batch_progress, "batch_success_rate": batch_success, "batch_transfer_count": ( - state.transfer_counts[completed_ids].float().mean() + command.transfer_counts[completed_ids].float().mean() if completed_ids.numel() else torch.zeros((), dtype=torch.float32, device=env.device) ), "batch_left_to_right_transfers": ( - state.direction_transfer_counts[completed_ids, 0].float().mean() + command.direction_transfer_counts[completed_ids, 0].float().mean() if completed_ids.numel() else torch.zeros((), dtype=torch.float32, device=env.device) ), "batch_right_to_left_transfers": ( - state.direction_transfer_counts[completed_ids, 1].float().mean() + command.direction_transfer_counts[completed_ids, 1].float().mean() if completed_ids.numel() else torch.zeros((), dtype=torch.float32, device=env.device) ), diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/observations.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/observations.py index 47edd45430b4..e7f699c36bc4 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/observations.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/observations.py @@ -22,12 +22,9 @@ from isaaclab.envs import ManagerBasedRLEnv -def _transfer_state(env: ManagerBasedRLEnv): - """Return initialized transfer state or raise a focused configuration error.""" - state = getattr(env, "conveyor_transfer_state", None) - if state is None: - raise AttributeError("Conveyor observations require ConveyorResetStateTable runtime state.") - return state +def _transfer_command(env: ManagerBasedRLEnv, command_name: str = "transfer"): + """Return the configured transfer command.""" + return env.command_manager.get_term(command_name) def _cube_assets(env: ManagerBasedRLEnv) -> tuple[RigidObject, ...]: @@ -52,16 +49,16 @@ def _active_cube_values(values: torch.Tensor, target_cube_ids: torch.Tensor) -> return torch.gather(values, 1, index).squeeze(1) -def target_cube_one_hot(env: ManagerBasedRLEnv) -> torch.Tensor: +def target_cube_one_hot(env: ManagerBasedRLEnv, command_name: str = "transfer") -> torch.Tensor: """Encode which numbered cube the policy must transfer.""" - state = _transfer_state(env) - return torch.nn.functional.one_hot(state.target_cube_ids.long(), num_classes=CUBE_COUNT).float() + command = _transfer_command(env, command_name) + return torch.nn.functional.one_hot(command.target_cube_ids.long(), num_classes=CUBE_COUNT).float() -def target_side_one_hot(env: ManagerBasedRLEnv) -> torch.Tensor: +def target_side_one_hot(env: ManagerBasedRLEnv, command_name: str = "transfer") -> torch.Tensor: """Encode the destination conveyor, opposite the reset source side.""" - state = _transfer_state(env) - return torch.nn.functional.one_hot(1 - state.source_side_ids.long(), num_classes=2).float() + command = _transfer_command(env, command_name) + return torch.nn.functional.one_hot(1 - command.source_side_ids.long(), num_classes=2).float() def classify_cube_conveyors(local_positions: torch.Tensor, transit_half_width: float = 0.14) -> torch.Tensor: @@ -105,14 +102,14 @@ def transfer_object_observation(env: ManagerBasedRLEnv) -> torch.Tensor: ) -def active_transfer_features(env: ManagerBasedRLEnv) -> torch.Tensor: +def active_transfer_features(env: ManagerBasedRLEnv, command_name: str = "transfer") -> torch.Tensor: """Return active-cube and destination-relative position features [m].""" - state = _transfer_state(env) + command = _transfer_command(env, command_name) positions, _, _ = _cube_state(env) - active_position = _active_cube_values(positions, state.target_cube_ids.long()) + active_position = _active_cube_values(positions, command.target_cube_ids.long()) local_active_position = active_position - env.scene.env_origins tool_position, _ = end_effector_pose(env) - target_side_ids = 1 - state.source_side_ids.long() + target_side_ids = 1 - command.source_side_ids.long() target_position = torch.stack( ( torch.full_like(target_side_ids, TRANSFER_X, dtype=active_position.dtype), diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/reset_events.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/reset_events.py index 3b75daee4920..a47985929168 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/reset_events.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/reset_events.py @@ -3,7 +3,7 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Reset-state and continuing-goal events for conveyor transfer.""" +"""Reset-state sampling for conveyor transfer.""" from __future__ import annotations @@ -18,7 +18,6 @@ from isaaclab.managers import EventTermCfg, ManagerTermBase from ..conveyor_geometry import BELT_CENTER_Y, BELT_TOP_Z, BELT_TURN_RADIUS -from .state import create_transfer_state if TYPE_CHECKING: from isaaclab.assets import Articulation, RigidObject @@ -298,7 +297,7 @@ def __init__(self, cfg: EventTermCfg, env: ManagerBasedRLEnv): self._finger_positions = torch.tensor( [row.finger_position for row in self._rows], dtype=torch.float32, device=env.device ) - self._held_rows = torch.tensor([row.held for row in self._rows], dtype=torch.bool, device=env.device) + self.held_rows = torch.tensor([row.held for row in self._rows], dtype=torch.bool, device=env.device) self._belt_range_fractions = torch.tensor( [row.belt_range_fraction for row in self._rows], dtype=torch.float32, device=env.device ) @@ -308,7 +307,7 @@ def __init__(self, cfg: EventTermCfg, env: ManagerBasedRLEnv): self._finger_joint_ids = self._robot.find_joints("panda_finger_joint[1-2]", preserve_order=True)[0] if len(self._arm_joint_ids) != 7 or len(self._finger_joint_ids) != 2: raise ValueError("Conveyor transfer requires seven Panda arm joints and two finger joints.") - self._state = create_transfer_state(env, self.row_count) + self.row_ids = torch.randint(self.row_count, (env.num_envs,), dtype=torch.long, device=env.device) @property def row_count(self) -> int: @@ -373,7 +372,7 @@ def __call__( and fixed_target_cube_id is None and fixed_source_side_id is None ): - row_ids = self._state.row_ids[env_ids] + row_ids = self.row_ids[env_ids] else: candidates = self._filtered_rows( fixed_recipe, @@ -384,21 +383,12 @@ def __call__( if candidates.numel() == 0: raise RuntimeError("No conveyor reset rows match the fixed reset controls.") row_ids = candidates[torch.randint(candidates.numel(), (env_ids.numel(),), device=self.device)] - self._state.row_ids[env_ids] = row_ids + self.row_ids[env_ids] = row_ids recipes = self.recipe_ids[row_ids] target_cube_ids = self.target_cube_ids[row_ids] source_side_ids = self.source_side_ids[row_ids] - held_rows = self._held_rows[row_ids] - self._state.recipe_ids[env_ids] = recipes - self._state.target_cube_ids[env_ids] = target_cube_ids - self._state.source_side_ids[env_ids] = source_side_ids - self._state.held_cube_ids[env_ids] = torch.where(held_rows, target_cube_ids, -1) - self._state.goal_ids[env_ids] = 0 - self._state.subgoal_start_steps[env_ids] = 0 - self._state.transfer_counts[env_ids] = 0 - self._state.direction_transfer_counts[env_ids] = 0 - self._state.initialized[env_ids] = True + held_rows = self.held_rows[row_ids] arm_positions = self._arm_positions[row_ids].clone() if arm_joint_noise > 0.0: @@ -506,118 +496,3 @@ def select_next_transfer_cube( fallback.scatter_(1, current_cube_ids.unsqueeze(1), True) candidates = torch.where(has_candidates.unsqueeze(1), candidates, fallback) return torch.multinomial(candidates.float(), 1).squeeze(1) - - -def _assign_conveyor_transfer_goal( - env: ManagerBasedRLEnv, - env_ids: torch.Tensor, - target_cube_ids: torch.Tensor, - source_side_ids: torch.Tensor, - success_context_name: str, -) -> None: - """Publish a new transfer command and clear progress from the previous command.""" - context = env.termination_manager.get_term_cfg(success_context_name).func - if not hasattr(context, "consume_success"): - raise RuntimeError("Conveyor goal changes require a stable transfer-success context.") - - state = env.conveyor_transfer_state - state.goal_ids[env_ids] += 1 - state.subgoal_start_steps[env_ids] = env.episode_length_buf[env_ids] - state.target_cube_ids[env_ids] = target_cube_ids - state.source_side_ids[env_ids] = source_side_ids - state.held_cube_ids[env_ids] = -1 - context.consume_success(env_ids) - - -def set_conveyor_transfer_goal( - env: ManagerBasedRLEnv, - target_cube_id: int, - env_ids: Sequence[int] | torch.Tensor | None = None, - success_context_name: str = "transfer_success_context", -) -> None: - """Command a numbered cube to move from its current conveyor to the opposite one. - - The cube's current local y-position determines its source conveyor. Positions - in the central transfer region use the nearest side of the workspace center. - - Args: - env: Conveyor Franka environment whose command state is updated. - target_cube_id: Stable numbered cube index. - env_ids: Environments receiving the command, or ``None`` for all environments. - success_context_name: Stable-success term reset for the new command. - """ - if not isinstance(target_cube_id, int) or isinstance(target_cube_id, bool): - raise TypeError("target_cube_id must be an integer.") - if not 0 <= target_cube_id < CUBE_COUNT: - raise ValueError(f"target_cube_id must lie in [0, {CUBE_COUNT - 1}].") - - if env_ids is None: - resolved_env_ids = torch.arange(env.num_envs, dtype=torch.long, device=env.device) - else: - resolved_env_ids = torch.as_tensor(env_ids, dtype=torch.long, device=env.device).flatten() - if resolved_env_ids.numel() == 0: - return - if torch.any((resolved_env_ids < 0) | (resolved_env_ids >= env.num_envs)): - raise IndexError("Conveyor goal environment indices are out of range.") - - cube: RigidObject = env.scene[f"cube_{target_cube_id}"] - local_y = cube.data.root_pos_w.torch[resolved_env_ids, 1] - env.scene.env_origins[resolved_env_ids, 1] - source_side_ids = torch.where( - local_y >= 0.0, - torch.full_like(resolved_env_ids, LEFT_SIDE), - torch.full_like(resolved_env_ids, RIGHT_SIDE), - ) - target_cube_ids = torch.full_like(resolved_env_ids, target_cube_id) - _assign_conveyor_transfer_goal( - env, - resolved_env_ids, - target_cube_ids, - source_side_ids, - success_context_name, - ) - - -def advance_conveyor_transfer_goal( - env: ManagerBasedRLEnv, - env_ids: torch.Tensor, - success_context_name: str = "transfer_success_context", - transit_half_width: float = 0.14, -) -> None: - """Consume completed transfers and command another cube in the reverse direction.""" - if env_ids is None: - env_ids = torch.arange(env.num_envs, device=env.device) - else: - env_ids = torch.as_tensor(env_ids, dtype=torch.long, device=env.device).flatten() - if env_ids.numel() == 0: - return - - context = env.termination_manager.get_term_cfg(success_context_name).func - if not hasattr(context, "pending_success") or not hasattr(context, "consume_success"): - raise RuntimeError("Continuing conveyor goals require a stable transfer-success context.") - completed_ids = env_ids[context.pending_success[env_ids]] - if completed_ids.numel() == 0: - return - - state = env.conveyor_transfer_state - cubes: tuple[RigidObject, ...] = tuple(env.scene[f"cube_{cube_id}"] for cube_id in range(CUBE_COUNT)) - positions = torch.stack(tuple(cube.data.root_pos_w.torch[completed_ids] for cube in cubes), dim=1) - positions -= env.scene.env_origins[completed_ids].unsqueeze(1) - previous_cube_ids = state.target_cube_ids[completed_ids].clone() - previous_source_side_ids = state.source_side_ids[completed_ids].clone() - next_source_side_ids = 1 - previous_source_side_ids - next_cube_ids = select_next_transfer_cube( - positions, - previous_cube_ids, - next_source_side_ids, - transit_half_width=transit_half_width, - ) - - state.direction_transfer_counts[completed_ids, previous_source_side_ids] += 1 - state.transfer_counts[completed_ids] += 1 - _assign_conveyor_transfer_goal( - env, - completed_ids, - next_cube_ids, - next_source_side_ids, - success_context_name, - ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/rewards.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/rewards.py index 349a730ddbcf..51a649cc9aae 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/rewards.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/rewards.py @@ -3,16 +3,15 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Dense progress and sparse completion rewards for conveyor transfer.""" +"""Progress utilities and sparse completion rewards for conveyor transfer.""" from __future__ import annotations -from collections.abc import Sequence from typing import TYPE_CHECKING import torch -from isaaclab.managers import ManagerTermBase, RewardTermCfg, SceneEntityCfg +from isaaclab.managers import SceneEntityCfg from .kinematics import end_effector_pose from .reset_events import CUBE_COUNT, CUBE_REST_Z, TRANSFER_X, side_inner_y @@ -21,6 +20,8 @@ from isaaclab.assets import Articulation, RigidObject from isaaclab.envs import ManagerBasedRLEnv + from .commands import ConveyorTransferCommand + def transfer_potential( cube_positions: torch.Tensor, @@ -54,60 +55,44 @@ def transfer_potential( return 0.5 * reach + 0.75 * grasp + 1.25 * lift + 2.0 * transport + 2.0 * target + released -def current_transfer_potential(env: ManagerBasedRLEnv) -> torch.Tensor: +def current_transfer_potential( + env: ManagerBasedRLEnv, + command_name: str = "transfer", + command: ConveyorTransferCommand | None = None, +) -> torch.Tensor: """Gather current task state and evaluate the shaping potential.""" - state = env.conveyor_transfer_state + if command is None: + command = env.command_manager.get_term(command_name) cubes: tuple[RigidObject, ...] = tuple(env.scene[f"cube_{cube_id}"] for cube_id in range(CUBE_COUNT)) positions = torch.stack(tuple(cube.data.root_pos_w.torch for cube in cubes), dim=1) - index = state.target_cube_ids.view(env.num_envs, 1, 1).expand(-1, 1, 3) + index = command.target_cube_ids.view(env.num_envs, 1, 1).expand(-1, 1, 3) active_position = torch.gather(positions, 1, index).squeeze(1) - env.scene.env_origins tool_position, _ = end_effector_pose(env) tool_position = tool_position - env.scene.env_origins robot: Articulation = env.scene["robot"] finger_ids, _ = robot.find_joints("panda_finger_joint[1-2]", preserve_order=True) finger_positions = robot.data.joint_pos.torch[:, finger_ids] - return transfer_potential(active_position, tool_position, finger_positions, state.source_side_ids) - - -class ConveyorTransferProgressReward(ManagerTermBase): - """Reward positive changes in a phase-aware transfer potential.""" - - def __init__(self, cfg: RewardTermCfg, env: ManagerBasedRLEnv): - super().__init__(cfg, env) - self._previous = torch.zeros(env.num_envs, dtype=torch.float32, device=env.device) - - def __call__(self, env: ManagerBasedRLEnv) -> torch.Tensor: - """Return per-step potential improvement.""" - current = current_transfer_potential(env) - improvement = current - self._previous - self._previous.copy_(current) - return improvement - - def reset(self, env_ids: Sequence[int] | None = None) -> None: - """Anchor shaping to the newly sampled reset state.""" - current = current_transfer_potential(self._env) - if env_ids is None: - self._previous.copy_(current) - else: - self._previous[env_ids] = current[env_ids] + return transfer_potential(active_position, tool_position, finger_positions, command.source_side_ids) def transfer_success_reward( env: ManagerBasedRLEnv, - context_term_name: str = "transfer_success_context", + command_name: str = "transfer", ) -> torch.Tensor: """Return one on each stable transfer-completion transition.""" - context = env.termination_manager.get_term_cfg(context_term_name).func - return context.new_success.float() + command = env.command_manager.get_term(command_name) + command.evaluate() + return command.new_success.float() def terminal_failure( env: ManagerBasedRLEnv, - success_context_name: str = "transfer_success_context", + command_name: str = "transfer", ) -> torch.Tensor: """Return one for non-timeout terminal failures.""" - success_context = env.termination_manager.get_term_cfg(success_context_name).func - return (env.reset_terminated & ~success_context.pending_success).float() + command = env.command_manager.get_term(command_name) + command.evaluate() + return (env.reset_terminated & ~command.pending_success).float() def action_term_l2(env: ManagerBasedRLEnv, action_name: str) -> torch.Tensor: @@ -118,6 +103,8 @@ def action_term_l2(env: ManagerBasedRLEnv, action_name: str) -> torch.Tensor: def physical_cube_acquisition_mask( env: ManagerBasedRLEnv, + command_name: str = "transfer", + command: ConveyorTransferCommand | None = None, minimum_lift: float = 0.025, maximum_tool_distance: float = 0.075, maximum_finger_position: float = 0.030, @@ -125,10 +112,11 @@ def physical_cube_acquisition_mask( """Return physically closed, lifted, tool-local commanded-cube grasps.""" if minimum_lift <= 0.0 or maximum_tool_distance <= 0.0 or maximum_finger_position <= 0.0: raise ValueError("Physical acquisition thresholds must be positive.") - state = env.conveyor_transfer_state + if command is None: + command = env.command_manager.get_term(command_name) cubes: tuple[RigidObject, ...] = tuple(env.scene[f"cube_{cube_id}"] for cube_id in range(CUBE_COUNT)) positions = torch.stack(tuple(cube.data.root_pos_w.torch for cube in cubes), dim=1) - index = state.target_cube_ids.view(env.num_envs, 1, 1).expand(-1, 1, 3) + index = command.target_cube_ids.view(env.num_envs, 1, 1).expand(-1, 1, 3) active_position = torch.gather(positions, 1, index).squeeze(1) tool_position, _ = end_effector_pose(env) robot: Articulation = env.scene["robot"] diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/state.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/state.py deleted file mode 100644 index 434d08105146..000000000000 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/state.py +++ /dev/null @@ -1,50 +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 - -"""Typed runtime state shared by conveyor-transfer MDP terms.""" - -from __future__ import annotations - -from dataclasses import dataclass -from typing import TYPE_CHECKING - -import torch - -if TYPE_CHECKING: - from isaaclab.envs import ManagerBasedRLEnv - - -@dataclass -class ConveyorTransferState: - """Episode-local transfer command, reset metadata, and subgoal progress.""" - - row_ids: torch.Tensor - recipe_ids: torch.Tensor - target_cube_ids: torch.Tensor - source_side_ids: torch.Tensor - held_cube_ids: torch.Tensor - goal_ids: torch.Tensor - subgoal_start_steps: torch.Tensor - transfer_counts: torch.Tensor - direction_transfer_counts: torch.Tensor - initialized: torch.Tensor - - -def create_transfer_state(env: ManagerBasedRLEnv, row_count: int) -> ConveyorTransferState: - """Create and attach the environment's transfer-state owner.""" - state = ConveyorTransferState( - row_ids=torch.randint(row_count, (env.num_envs,), dtype=torch.long, device=env.device), - recipe_ids=torch.zeros(env.num_envs, dtype=torch.long, device=env.device), - target_cube_ids=torch.zeros(env.num_envs, dtype=torch.long, device=env.device), - source_side_ids=torch.zeros(env.num_envs, dtype=torch.long, device=env.device), - held_cube_ids=torch.full((env.num_envs,), -1, dtype=torch.long, device=env.device), - goal_ids=torch.zeros(env.num_envs, dtype=torch.long, device=env.device), - subgoal_start_steps=torch.zeros(env.num_envs, dtype=torch.long, device=env.device), - transfer_counts=torch.zeros(env.num_envs, dtype=torch.long, device=env.device), - direction_transfer_counts=torch.zeros((env.num_envs, 2), dtype=torch.long, device=env.device), - initialized=torch.zeros(env.num_envs, dtype=torch.bool, device=env.device), - ) - env.conveyor_transfer_state = state - return state diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/terminations.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/terminations.py index 290ea1c66eee..ab17a7747644 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/terminations.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/terminations.py @@ -3,17 +3,16 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Termination terms for conveyor transfer.""" +"""Actual episode termination and truncation terms for conveyor transfer.""" from __future__ import annotations import math -from collections.abc import Sequence from typing import TYPE_CHECKING import torch -from isaaclab.managers import ManagerTermBase, SceneEntityCfg, TerminationTermCfg +from isaaclab.managers import SceneEntityCfg from ..conveyor_geometry import ( BELT_CENTER_X, @@ -22,9 +21,7 @@ BELT_WIDTH, GUARD_THICKNESS, ) -from .kinematics import end_effector_pose -from .reset_events import CUBE_COUNT, CUBE_SIZE, ConveyorResetRecipe, side_inner_y -from .rewards import current_transfer_potential, physical_cube_acquisition_mask +from .reset_events import CUBE_COUNT, CUBE_SIZE if TYPE_CHECKING: from isaaclab.assets import Articulation, RigidObject @@ -34,193 +31,32 @@ _TRACK_X_CLEARANCE = BELT_TURN_RADIUS + 0.5 * BELT_WIDTH + GUARD_THICKNESS + CUBE_SIZE -def transfer_success_mask( - cube_positions: torch.Tensor, - cube_linear_velocities: torch.Tensor, - tool_positions: torch.Tensor, - finger_positions: torch.Tensor, - target_side_ids: torch.Tensor, - lateral_tolerance: float = 0.055, - maximum_cube_speed: float = 0.65, - minimum_finger_position: float = 0.027, - minimum_tool_clearance: float = 0.055, +def subgoal_time_out( + env: ManagerBasedRLEnv, + timeout_s: float = 20.0, + command_name: str = "transfer", ) -> torch.Tensor: - """Return whether the active cube is released on its destination belt.""" - target_y = side_inner_y(target_side_ids) - on_straight = torch.abs(cube_positions[:, 0] - BELT_CENTER_X) < BELT_HALF_STRAIGHT - on_lane = torch.abs(cube_positions[:, 1] - target_y) < lateral_tolerance - supported_height = (cube_positions[:, 2] > 0.045) & (cube_positions[:, 2] < 0.095) - moving_safely = torch.linalg.vector_norm(cube_linear_velocities, dim=1) < maximum_cube_speed - released = torch.amin(finger_positions, dim=1) > minimum_finger_position - hand_clear = torch.linalg.vector_norm(tool_positions - cube_positions, dim=1) > minimum_tool_clearance - return on_straight & on_lane & supported_height & moving_safely & released & hand_clear - - -class StableConveyorTransfer(ManagerTermBase): - """Track stable released placements without terminating the episode.""" - - def __init__(self, cfg: TerminationTermCfg, env: ManagerBasedRLEnv): - super().__init__(cfg, env) - self._stable_steps = torch.zeros(env.num_envs, dtype=torch.long, device=env.device) - self.is_success = torch.zeros(env.num_envs, dtype=torch.bool, device=env.device) - self.new_success = torch.zeros_like(self.is_success) - self.pending_success = torch.zeros_like(self.is_success) - self.ever_success = torch.zeros(env.num_envs, dtype=torch.bool, device=env.device) - self._no_termination = torch.zeros_like(self.is_success) - - def __call__( - self, - env: ManagerBasedRLEnv, - minimum_episode_steps: int = 2, - hold_steps: int = 3, - lateral_tolerance: float = 0.055, - maximum_cube_speed: float = 0.65, - minimum_finger_position: float = 0.027, - minimum_tool_clearance: float = 0.055, - ) -> torch.Tensor: - """Update current, edge-triggered, and sticky transfer success state.""" - if minimum_episode_steps < 0 or hold_steps < 1: - raise ValueError("minimum_episode_steps must be non-negative and hold_steps must be positive.") - state = env.conveyor_transfer_state - cubes: tuple[RigidObject, ...] = tuple(env.scene[f"cube_{cube_id}"] for cube_id in range(CUBE_COUNT)) - positions = torch.stack(tuple(cube.data.root_pos_w.torch for cube in cubes), dim=1) - velocities = torch.stack(tuple(cube.data.root_lin_vel_w.torch for cube in cubes), dim=1) - index = state.target_cube_ids.view(env.num_envs, 1, 1).expand(-1, 1, 3) - active_position = torch.gather(positions, 1, index).squeeze(1) - env.scene.env_origins - active_velocity = torch.gather(velocities, 1, index).squeeze(1) - tool_position, _ = end_effector_pose(env) - tool_position = tool_position - env.scene.env_origins - robot: Articulation = env.scene["robot"] - finger_ids, _ = robot.find_joints("panda_finger_joint[1-2]", preserve_order=True) - finger_positions = robot.data.joint_pos.torch[:, finger_ids] - successful = transfer_success_mask( - active_position, - active_velocity, - tool_position, - finger_positions, - 1 - state.source_side_ids, - lateral_tolerance=lateral_tolerance, - maximum_cube_speed=maximum_cube_speed, - minimum_finger_position=minimum_finger_position, - minimum_tool_clearance=minimum_tool_clearance, - ) - subgoal_steps = env.episode_length_buf - state.subgoal_start_steps - successful &= subgoal_steps >= minimum_episode_steps - self._stable_steps = torch.where(successful, self._stable_steps + 1, torch.zeros_like(self._stable_steps)) - stable = self._stable_steps >= hold_steps - self.new_success.copy_(stable & ~self.is_success & ~self.pending_success) - self.is_success.copy_(stable) - self.pending_success |= self.new_success - self.ever_success |= self.new_success - env.extras["successes"] = self.ever_success - return self._no_termination - - def consume_success(self, env_ids: Sequence[int]) -> None: - """Clear per-subgoal success state after the goal-transition event.""" - self._stable_steps[env_ids] = 0 - self.is_success[env_ids] = False - self.new_success[env_ids] = False - self.pending_success[env_ids] = False - - def reset(self, env_ids: Sequence[int] | None = None) -> None: - """Log episode success and clear history for selected environments.""" - successes = self.ever_success if env_ids is None else self.ever_success[env_ids] - if successes.numel() > 0: - self._env.extras.setdefault("log", {})["Metrics/success_rate"] = successes.float().mean().item() - - reset_ids = slice(None) if env_ids is None else env_ids - self._stable_steps[reset_ids] = 0 - self.is_success[reset_ids] = False - self.new_success[reset_ids] = False - self.pending_success[reset_ids] = False - self.ever_success[reset_ids] = False - - -def subgoal_time_out(env: ManagerBasedRLEnv, timeout_s: float = 20.0) -> torch.Tensor: """Truncate environments that make no transfer within one subgoal timeout [s].""" if timeout_s <= 0.0: raise ValueError("timeout_s must be positive.") + command = env.command_manager.get_term(command_name) + command.evaluate() timeout_steps = math.ceil(timeout_s / env.step_dt) - state = env.conveyor_transfer_state - return env.episode_length_buf - state.subgoal_start_steps >= timeout_steps + elapsed = env.episode_length_buf - command.subgoal_start_steps + return (elapsed >= timeout_steps) & ~command.pending_success -def transfer_sequence_time_out(env: ManagerBasedRLEnv, maximum_transfers: int = 8) -> torch.Tensor: - """Truncate long successful sequences so reset coverage remains fresh.""" +def transfer_sequence_time_out( + env: ManagerBasedRLEnv, + maximum_transfers: int = 8, + command_name: str = "transfer", +) -> torch.Tensor: + """Truncate completed sequences so reset-state coverage remains fresh.""" if maximum_transfers < 1: raise ValueError("maximum_transfers must be positive.") - return env.conveyor_transfer_state.transfer_counts >= maximum_transfers - - -class ConveyorResetLearningProgress(ManagerTermBase): - """Track row-relative progress without terminating the episode. - - The adaptive reset sampler needs useful evidence before complete transfers - are common. Each reset row therefore asks the policy to increase the same - transfer potential used for dense shaping by a fixed amount. Episodes keep - running toward strict released placement; this context only records which - rows have advanced meaningfully. - """ - - def __init__(self, cfg: TerminationTermCfg, env: ManagerBasedRLEnv): - super().__init__(cfg, env) - self._target_potential = torch.zeros(env.num_envs, dtype=torch.float32, device=env.device) - self.is_success = torch.zeros(env.num_envs, dtype=torch.bool, device=env.device) - self.new_success = torch.zeros_like(self.is_success) - self.ever_success = torch.zeros_like(self.is_success) - self._no_termination = torch.zeros_like(self.is_success) - - def __call__( - self, - env: ManagerBasedRLEnv, - minimum_episode_steps: int = 3, - minimum_progress: float = 0.35, - maximum_target_potential: float = 5.0, - minimum_acquisition_lift: float = 0.025, - maximum_acquisition_tool_distance: float = 0.075, - maximum_acquisition_finger_position: float = 0.030, - ) -> torch.Tensor: - """Update sticky row-progress evidence and return an all-false mask.""" - if ( - minimum_episode_steps < 0 - or minimum_progress <= 0.0 - or maximum_target_potential <= 0.0 - or minimum_acquisition_lift <= 0.0 - or maximum_acquisition_tool_distance <= 0.0 - or maximum_acquisition_finger_position <= 0.0 - ): - raise ValueError("Invalid conveyor reset-learning progress thresholds.") - current = current_transfer_potential(env) - reached = (current >= self._target_potential) & (env.episode_length_buf >= minimum_episode_steps) - state = env.conveyor_transfer_state - acquisition_recipe = ( - (state.recipe_ids == int(ConveyorResetRecipe.GRASP)) - | (state.recipe_ids == int(ConveyorResetRecipe.PREGRASP)) - | (state.recipe_ids == int(ConveyorResetRecipe.BELT)) - ) - physically_acquired = physical_cube_acquisition_mask( - env, - minimum_lift=minimum_acquisition_lift, - maximum_tool_distance=maximum_acquisition_tool_distance, - maximum_finger_position=maximum_acquisition_finger_position, - ) - reached &= ~acquisition_recipe | physically_acquired - self.is_success.copy_(reached) - self.new_success.copy_(reached & ~self.ever_success) - self.ever_success |= reached - return self._no_termination - - def reset(self, env_ids: Sequence[int] | None = None) -> None: - """Set a meaningful potential target from each newly sampled row.""" - if env_ids is None: - env_ids = slice(None) - initial = current_transfer_potential(self._env) - minimum_progress = float(self.cfg.params.get("minimum_progress", 0.35)) - maximum_target = float(self.cfg.params.get("maximum_target_potential", 5.0)) - self._target_potential[env_ids] = torch.clamp_max(initial[env_ids] + minimum_progress, maximum_target) - self.is_success[env_ids] = False - self.new_success[env_ids] = False - self.ever_success[env_ids] = False + command = env.command_manager.get_term(command_name) + command.evaluate() + return command.transfer_counts >= maximum_transfers def cube_out_of_workspace( diff --git a/source/isaaclab_tasks/test/contrib/test_conveyor_franka_geometry.py b/source/isaaclab_tasks/test/contrib/test_conveyor_franka_geometry.py index 13342ec15a40..1d2aab67d7fc 100644 --- a/source/isaaclab_tasks/test/contrib/test_conveyor_franka_geometry.py +++ b/source/isaaclab_tasks/test/contrib/test_conveyor_franka_geometry.py @@ -3,30 +3,14 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Tests for the contributed conveyor Franka racetrack geometry.""" +"""Durable geometry checks for the contributed conveyor Franka task.""" -import sys from collections import Counter -import numpy as np -import pytest -import warp as wp - -from isaaclab_tasks.contrib.conveyor_franka.conveyor_force_driver import ( - BeltContact, - _integrate_encoders, - _prepare_contact_patches, - _update_effective_velocities, -) -from isaaclab_tasks.contrib.conveyor_franka.conveyor_franka_env_cfg import ConveyorForceCfg, ConveyorFrankaEnvCfg from isaaclab_tasks.contrib.conveyor_franka.conveyor_geometry import ( BELT_TOP_Z, - BELT_TURN_RADIUS, TURN_SEGMENT_COUNT, - CuboidSpec, MeshSpec, - belt_collision_section_specs, - belt_direction, belt_mesh_spec, guard_mesh_specs, ) @@ -41,160 +25,26 @@ def _edge_use_counts(spec: MeshSpec) -> Counter[tuple[int, int]]: return edges -def test_racetrack_mesh_counts_and_names_are_consistent(): - """Verify each lane has one belt and two uniquely named rail meshes.""" - specs = [] - for side in ("Left", "Right"): - specs.append(belt_mesh_spec(side)) - specs.extend(guard_mesh_specs(side)) +def test_racetrack_visual_meshes_are_named_watertight_loops(): + """Belts and rails remain uniquely named, closed racetrack meshes.""" + specs = tuple(spec for side in ("Left", "Right") for spec in (belt_mesh_spec(side), *guard_mesh_specs(side))) + expected_loop_vertices = 2 * TURN_SEGMENT_COUNT + 2 assert len(specs) == 6 assert len({spec.name for spec in specs}) == len(specs) - expected_loop_vertices = 2 * TURN_SEGMENT_COUNT + 2 for spec in specs: assert len(spec.vertices) == 4 * expected_loop_vertices assert len(spec.faces) == 8 * expected_loop_vertices - - -def test_racetrack_meshes_are_watertight(): - """Verify belts and rails have no open or multiply connected triangle edges.""" - for side in ("Left", "Right"): - for spec in (belt_mesh_spec(side), *guard_mesh_specs(side)): - assert set(_edge_use_counts(spec).values()) == {2} + assert set(_edge_use_counts(spec).values()) == {2} def test_belt_top_faces_point_upward(): - """The one-sided triangle-mesh collision surface must support parcels from above.""" + """One-sided triangle-mesh surfaces support parcels from above.""" for side in ("Left", "Right"): spec = belt_mesh_spec(side) for face in spec.faces: - a, b, c = (spec.vertices[index] for index in face) - if a[2] == b[2] == c[2] == BELT_TOP_Z: + vertices = tuple(spec.vertices[index] for index in face) + if all(vertex[2] == BELT_TOP_Z for vertex in vertices): + a, b, c = vertices cross_z = (b[0] - a[0]) * (c[1] - a[1]) - (b[1] - a[1]) * (c[0] - a[0]) assert cross_z > 0.0 - - -def test_racetrack_lanes_counter_rotate(): - """Verify the two analytic conveyor velocity fields use opposite directions.""" - assert belt_direction("Left") == -belt_direction("Right") - - -def test_collision_sections_and_velocity_fields_share_one_description(): - """Straight and curved force fields stay aligned with robust collision geometry.""" - for side in ("Left", "Right"): - sections = belt_collision_section_specs(side) - - assert len(sections) == 4 - assert [section.velocity_field_type for section in sections] == ["constant", "constant", "pivot", "pivot"] - assert all(isinstance(section.geometry, CuboidSpec) for section in sections[:2]) - for section in sections[2:]: - assert isinstance(section.geometry, MeshSpec) - assert set(_edge_use_counts(section.geometry).values()) == {2} - assert all(section.radius == BELT_TURN_RADIUS for section in sections[2:]) - assert sections[0].direction == tuple(-value for value in sections[1].direction) - assert sections[2].direction == sections[3].direction - - -def _make_belt_contact(conveyor: int, force: float, next_contact: int) -> BeltContact: - """Build one horizontal contact for the patch-normalization kernel.""" - contact = BeltContact() - contact.valid = 1 - contact.body = 0 - contact.conveyor = conveyor - contact.point = wp.vec3() - contact.normal = wp.vec3(0.0, 0.0, 1.0) - contact.normal_force = force - contact.next_body_contact = next_contact - return contact - - -@pytest.mark.parametrize( - ("conveyors", "expected_forces"), - (((0, 0), (4.0, 6.0)), ((0, 1), (5.0, 5.0))), -) -def test_contact_patch_normalizes_only_across_overlapping_sections(conveyors, expected_forces): - """A seam preserves total load without perturbing contacts on one section.""" - contacts = wp.array( - [ - _make_belt_contact(conveyors[0], 4.0, 1), - _make_belt_contact(conveyors[1], 6.0, -1), - ], - dtype=BeltContact, - device="cpu", - ) - body_contact_head = wp.array([0], dtype=wp.int32, device="cpu") - body_q = wp.array([wp.transform()], dtype=wp.transform, device="cpu") - body_com = wp.array([wp.vec3()], dtype=wp.vec3, device="cpu") - patch_head = wp.full(2, -1, dtype=wp.int32, device="cpu") - adjusted_force = wp.zeros(2, dtype=wp.float32, device="cpu") - splitting_scale = wp.zeros(2, dtype=wp.float32, device="cpu") - - wp.launch( - _prepare_contact_patches, - dim=1, - inputs=[contacts, body_contact_head, body_q, body_com], - outputs=[patch_head, adjusted_force, splitting_scale], - device="cpu", - ) - - np.testing.assert_allclose(adjusted_force.numpy(), expected_forces) - np.testing.assert_allclose(splitting_scale.numpy(), (0.5, 0.5)) - np.testing.assert_allclose(adjusted_force.numpy().sum(), 10.0) - - -def test_disabled_surface_remembers_command_and_stops_encoder(): - """One effective-speed seam drives both traction and encoder state.""" - commanded = wp.array([2.0], dtype=wp.float32, device="cpu") - enabled = wp.array([0], dtype=wp.int32, device="cpu") - effective = wp.zeros(1, dtype=wp.float32, device="cpu") - encoder = wp.zeros(1, dtype=wp.float32, device="cpu") - - wp.launch( - _update_effective_velocities, - dim=1, - inputs=[commanded, enabled], - outputs=[effective], - device="cpu", - ) - wp.launch(_integrate_encoders, dim=1, inputs=[0.5, effective], outputs=[encoder], device="cpu") - np.testing.assert_allclose(effective.numpy(), (0.0,)) - np.testing.assert_allclose(encoder.numpy(), (0.0,)) - - commanded.assign(np.array([3.0], dtype=np.float32)) - enabled.fill_(1) - wp.launch( - _update_effective_velocities, - dim=1, - inputs=[commanded, enabled], - outputs=[effective], - device="cpu", - ) - wp.launch(_integrate_encoders, dim=1, inputs=[0.5, effective], outputs=[encoder], device="cpu") - np.testing.assert_allclose(effective.numpy(), (3.0,)) - np.testing.assert_allclose(encoder.numpy(), (1.5,)) - - -def test_environment_config_without_optional_visualizers(monkeypatch): - """The task configuration remains usable without the visualizer package.""" - monkeypatch.setitem(sys.modules, "isaaclab_visualizers", None) - - cfg = ConveyorFrankaEnvCfg() - - assert cfg.sim.default_visualizer_cfg is None - - -@pytest.mark.parametrize( - ("parameter", "value"), - ( - ("speed", -0.1), - ("friction", -0.1), - ("normal_threshold", 1.1), - ("startup_duration_s", 0.0), - ("transported_body_count_per_env", 0), - ("transported_body_pattern", "["), - ), -) -def test_conveyor_force_config_rejects_invalid_values(parameter: str, value: object): - """Verify force configuration rejects values outside its physical domain.""" - with pytest.raises(ValueError): - ConveyorForceCfg(**{parameter: value}) diff --git a/source/isaaclab_tasks/test/contrib/test_conveyor_franka_mdp.py b/source/isaaclab_tasks/test/contrib/test_conveyor_franka_mdp.py index 31aa16b42e80..2222f70a93ff 100644 --- a/source/isaaclab_tasks/test/contrib/test_conveyor_franka_mdp.py +++ b/source/isaaclab_tasks/test/contrib/test_conveyor_franka_mdp.py @@ -9,13 +9,13 @@ from types import SimpleNamespace import torch -from isaaclab_newton.sim.schemas import MujocoCollisionCfg, NewtonMaterialPropertiesCfg from isaaclab_tasks.contrib.conveyor_franka.agents.rsl_rl_ppo_cfg import ( ConveyorFrankaPPORunnerCfg, ConveyorGaussianBernoulliDistribution, ) from isaaclab_tasks.contrib.conveyor_franka.conveyor_franka_env_cfg import ConveyorFrankaEnvCfg +from isaaclab_tasks.contrib.conveyor_franka.mdp.commands import ConveyorTransferCommand, transfer_success_mask from isaaclab_tasks.contrib.conveyor_franka.mdp.curriculums import ( deployment_probability_from_progress, reset_sampling_probabilities, @@ -29,37 +29,9 @@ franka_tool_position, reset_variant_counts, select_next_transfer_cube, - set_conveyor_transfer_goal, ) from isaaclab_tasks.contrib.conveyor_franka.mdp.rewards import transfer_potential -from isaaclab_tasks.contrib.conveyor_franka.mdp.terminations import ( - subgoal_time_out, - transfer_sequence_time_out, - transfer_success_mask, -) - - -def test_contact_and_drive_cfg_preserve_transport_and_grasp_friction(): - """Belt contact precedence must not weaken the cube's grasp material.""" - cfg = ConveyorFrankaEnvCfg() - belt_collision = cfg.scene.conveyor_left_top_straight_collision.spawn.collision_props - belt_mujoco = next(fragment for fragment in belt_collision if isinstance(fragment, MujocoCollisionCfg)) - belt_material = cfg.scene.conveyor_left_top_straight_collision.spawn.physics_material - cube_material = cfg.scene.cube_0.spawn.physics_material - hand_actuator = cfg.scene.robot.actuators["panda_hand"] - - assert belt_mujoco.priority == 1 - assert isinstance(belt_material, NewtonMaterialPropertiesCfg) - assert belt_material.contact_stiffness == 1.0e4 - assert belt_material.contact_damping == 200.0 - assert isinstance(cube_material, NewtonMaterialPropertiesCfg) - assert cube_material.dynamic_friction == 0.6 - assert cube_material.contact_stiffness == 1.0e4 - assert cube_material.contact_damping == 200.0 - assert hand_actuator.stiffness == 350.0 - assert hand_actuator.damping == 10.0 - assert cfg.actions.arm_action.gravity_compensation - assert cfg.sim.physics.collision_decimation == 0 +from isaaclab_tasks.contrib.conveyor_franka.mdp.terminations import subgoal_time_out, transfer_sequence_time_out def test_reset_rows_cover_every_cube_direction_and_phase_once(): @@ -303,78 +275,69 @@ def test_manual_transfer_goal_uses_selected_cube_current_side(): class _Scene(dict): pass - class _SuccessContext: - consumed_env_ids = None - - def consume_success(self, env_ids): - self.consumed_env_ids = env_ids.clone() - origins = torch.tensor(((0.0, 1.0, 0.0), (0.0, -2.0, 0.0))) selected_cube_positions = torch.tensor(((0.2, 1.3, 0.06), (0.7, -2.3, 0.06))) - scene = _Scene( - cube_2=SimpleNamespace(data=SimpleNamespace(root_pos_w=SimpleNamespace(torch=selected_cube_positions))) + cubes = tuple( + SimpleNamespace( + data=SimpleNamespace( + root_pos_w=SimpleNamespace( + torch=selected_cube_positions if cube_id == 2 else torch.zeros_like(selected_cube_positions) + ) + ) + ) + for cube_id in range(CUBE_COUNT) ) + scene = _Scene() scene.env_origins = origins - context = _SuccessContext() - state = SimpleNamespace( - target_cube_ids=torch.tensor((0, 1)), - source_side_ids=torch.tensor((1, 0)), - held_cube_ids=torch.tensor((0, 1)), - goal_ids=torch.tensor((4, 7)), - subgoal_start_steps=torch.tensor((2, 3)), - ) - env = SimpleNamespace( + command = object.__new__(ConveyorTransferCommand) + command._env = SimpleNamespace( num_envs=2, device="cpu", scene=scene, - conveyor_transfer_state=state, episode_length_buf=torch.tensor((11, 19)), - termination_manager=SimpleNamespace(get_term_cfg=lambda _name: SimpleNamespace(func=context)), ) - - set_conveyor_transfer_goal(env, 2) - - assert state.target_cube_ids.tolist() == [2, 2] - assert state.source_side_ids.tolist() == [0, 1] - assert state.held_cube_ids.tolist() == [-1, -1] - assert state.goal_ids.tolist() == [5, 8] - assert state.subgoal_start_steps.tolist() == [11, 19] - assert context.consumed_env_ids.tolist() == [0, 1] + command._cubes = cubes + command.target_cube_ids = torch.tensor((0, 1)) + command.source_side_ids = torch.tensor((1, 0)) + command.held_cube_ids = torch.tensor((0, 1)) + command.subgoal_start_steps = torch.tensor((2, 3)) + command._stable_steps = torch.ones(2, dtype=torch.long) + command.is_success = torch.ones(2, dtype=torch.bool) + command.new_success = torch.ones(2, dtype=torch.bool) + command.pending_success = torch.ones(2, dtype=torch.bool) + command._last_evaluation_steps = torch.tensor((11, 19)) + + command.set_goal(2) + + assert command.target_cube_ids.tolist() == [2, 2] + assert command.source_side_ids.tolist() == [0, 1] + assert command.held_cube_ids.tolist() == [-1, -1] + assert command.subgoal_start_steps.tolist() == [11, 19] + torch.testing.assert_close( + command.command, + torch.tensor(((0, 0, 1, 0, 0, 1), (0, 0, 1, 0, 1, 0)), dtype=torch.float32), + ) def test_continuing_training_truncates_only_stalled_or_long_sequences(): """Subgoal and sequence limits bound training without ending successful transfers.""" assert not ConveyorFrankaPPORunnerCfg().init_at_random_ep_len + command = SimpleNamespace( + evaluate=lambda: None, + subgoal_start_steps=torch.tensor((0, 0, 300)), + pending_success=torch.zeros(3, dtype=torch.bool), + transfer_counts=torch.tensor((0, 7, 8)), + ) env = SimpleNamespace( step_dt=0.1, episode_length_buf=torch.tensor((199, 200, 450)), - conveyor_transfer_state=SimpleNamespace( - subgoal_start_steps=torch.tensor((0, 0, 300)), - transfer_counts=torch.tensor((0, 7, 8)), - ), + command_manager=SimpleNamespace(get_term=lambda _name: command), ) assert subgoal_time_out(env, timeout_s=20.0).tolist() == [False, True, False] assert transfer_sequence_time_out(env, maximum_transfers=8).tolist() == [False, False, True] -def test_rolling_progress_monitor_forgets_stale_outcomes_in_order(): - """Per-row curriculum evidence retains only each row's latest outcomes.""" - monitor_cfg = ( - ConveyorFrankaEnvCfg().curriculum.reset_sampling.params["success_monitor"].replace(monitored_history_len=3) - ) - monitor = monitor_cfg.class_type(monitor_cfg, num_partitions=1, partition_size=2, device="cpu") - - monitor.success_update( - torch.tensor([0, 0, 1, 0, 0]), - torch.tensor([False, False, True, True, True]), - ) - - assert monitor.success_size.tolist() == [3, 1] - assert monitor.success_buf.sum(dim=1).tolist() == [2, 1] - torch.testing.assert_close(monitor.get_success_rate(), torch.tensor([2.0 / 3.0, 1.0])) - - def test_active_cube_sampling_avoids_inactive_source_lane_cubes(): """Random deployment starts cannot begin with interpenetrating parcels.""" count = 2048 From a200805d271802cdd510b291959dddf3a395ddd6 Mon Sep 17 00:00:00 2001 From: Maximilian Krause Date: Wed, 12 Aug 2026 18:52:28 -0700 Subject: [PATCH 11/23] Set conveyor viewer camera pose --- .../contrib/conveyor_franka/conveyor_franka_env_cfg.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env_cfg.py index 94e157526406..3c93777b90e2 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env_cfg.py @@ -680,9 +680,11 @@ def __post_init__(self) -> None: from isaaclab_visualizers.newton import NewtonGLVisualizerCfg # Explicit --viz newton_rtx replaces this backend while retaining the shared camera hints. + # Newton camera pose: position (2.13, 0.0, 1.0), pitch -23.9 degrees, + # yaw 180 degrees. The look-at point is one unit along that view ray. self.sim.default_visualizer_cfg = NewtonGLVisualizerCfg( - eye=(2.3, -2.7, 1.8), - lookat=(0.45, 0.0, 0.35), + eye=(2.13, 0.0, 1.0), + lookat=(1.2157460448, 0.0, 0.5948584132), streaming_view=False, ) From a27ac6a11570fd06e34161932e6df7009b3931ac Mon Sep 17 00:00:00 2001 From: Maximilian Krause Date: Thu, 13 Aug 2026 18:19:23 -0700 Subject: [PATCH 12/23] Align conveyor task backends and lifecycle --- .../maximiliank-conveyor-belt-api.rst | 5 + source/isaaclab/isaaclab/physics/__init__.pyi | 3 + .../isaaclab/physics/conveyor_belt.py | 211 +++++++ .../isaaclab/test/sim/test_conveyor_belt.py | 77 +++ .../maximiliank-conveyor-substep-callback.rst | 3 +- .../isaaclab_newton/physics/newton_manager.py | 45 ++ .../test_newton_manager_abstraction.py | 31 + .../maximiliank-conveyor-franka.minor.rst | 4 + .../contrib/conveyor_franka/README.md | 65 ++ .../contrib/conveyor_franka/__init__.py | 15 +- .../conveyor_franka/conveyor_force_driver.py | 446 +++++++++++--- .../conveyor_franka/conveyor_franka_env.py | 96 ++- .../conveyor_franka_env_cfg.py | 149 +++-- .../conveyor_franka_physx_env_cfg.py | 425 +++++++++++++ .../conveyor_franka/conveyor_geometry.py | 96 +-- .../conveyor_franka/conveyor_physx_surface.py | 558 ++++++++++++++++++ .../conveyor_franka/franka_robot_cfg.py | 20 +- .../contrib/conveyor_franka/mdp/__init__.py | 38 +- .../contrib/conveyor_franka/mdp/__init__.pyi | 77 +++ .../contrib/conveyor_franka/mdp/actions.py | 59 +- .../conveyor_franka/mdp/actions_cfg.py | 3 - .../conveyor_franka/mdp/reset_events.py | 71 ++- .../conveyor_franka/mdp/terminations.py | 20 + source/isaaclab_tasks/pyproject.toml | 2 + .../contrib/test_conveyor_force_driver.py | 335 +++++++++++ .../contrib/test_conveyor_franka_geometry.py | 30 + .../test/contrib/test_conveyor_franka_mdp.py | 157 ++++- .../contrib/test_conveyor_franka_physx_cfg.py | 99 ++++ .../contrib/test_conveyor_physx_surface.py | 247 ++++++++ uv.lock | 16 +- 30 files changed, 3125 insertions(+), 278 deletions(-) create mode 100644 source/isaaclab/changelog.d/maximiliank-conveyor-belt-api.rst create mode 100644 source/isaaclab/isaaclab/physics/conveyor_belt.py create mode 100644 source/isaaclab/test/sim/test_conveyor_belt.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/README.md create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_physx_env_cfg.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_physx_surface.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/__init__.pyi create mode 100644 source/isaaclab_tasks/test/contrib/test_conveyor_force_driver.py create mode 100644 source/isaaclab_tasks/test/contrib/test_conveyor_franka_physx_cfg.py create mode 100644 source/isaaclab_tasks/test/contrib/test_conveyor_physx_surface.py diff --git a/source/isaaclab/changelog.d/maximiliank-conveyor-belt-api.rst b/source/isaaclab/changelog.d/maximiliank-conveyor-belt-api.rst new file mode 100644 index 000000000000..0bf7faa88ae6 --- /dev/null +++ b/source/isaaclab/changelog.d/maximiliank-conveyor-belt-api.rst @@ -0,0 +1,5 @@ +Added +^^^^^ + +* Added a backend-neutral conveyor belt specification and tensorized control contract for reusable, + vectorized conveyor implementations. diff --git a/source/isaaclab/isaaclab/physics/__init__.pyi b/source/isaaclab/isaaclab/physics/__init__.pyi index 92d9bfac2ff4..55aed0e59a48 100644 --- a/source/isaaclab/isaaclab/physics/__init__.pyi +++ b/source/isaaclab/isaaclab/physics/__init__.pyi @@ -5,11 +5,14 @@ __all__ = [ "CallbackHandle", + "ConveyorBeltSpec", + "ConveyorBeltView", "PhysicsEvent", "PhysicsManager", "PhysicsCfg", "PhysxAutoCfg", ] +from .conveyor_belt import ConveyorBeltSpec, ConveyorBeltView from .physics_manager import CallbackHandle, PhysicsEvent, PhysicsManager from .physics_manager_cfg import PhysicsCfg, PhysxAutoCfg diff --git a/source/isaaclab/isaaclab/physics/conveyor_belt.py b/source/isaaclab/isaaclab/physics/conveyor_belt.py new file mode 100644 index 000000000000..4d4ff37b5f63 --- /dev/null +++ b/source/isaaclab/isaaclab/physics/conveyor_belt.py @@ -0,0 +1,211 @@ +# 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 conveyor belt 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("Conveyor 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"Conveyor prim_path must be absolute or start with '{_ENV_REGEX_NS}/', got {value!r}.") + if "{" in path or "}" in path: + raise ValueError(f"Conveyor 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"Conveyor 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"Conveyor 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"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 _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"Conveyor {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"Conveyor {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"Conveyor {name} must be non-zero, got {value!r}.") + return result + + +@dataclass(frozen=True, slots=True) +class ConveyorBeltSpec: + """Persistent intent for one static collision surface acting as a conveyor. + + 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 belt traction. + animate_texture: Whether a renderer may scroll a compatible belt texture. + animate_direction: Texture-space animation direction. + animate_scale: Texture-coordinate travel per meter of encoder travel [1/m]. + """ + + 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 + animate_texture: bool = False + animate_direction: tuple[float, float] = (1.0, 0.0) + animate_scale: float = 1.0 + + def __post_init__(self) -> None: + """Normalize immutable vectors and validate authored values.""" + _validate_prim_path(self.prim_path) + for name in ("enabled", "curved", "animate_texture"): + if not isinstance(getattr(self, name), bool): + raise ValueError(f"Conveyor {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) + animate_scale = _validate_scalar("animate_scale", self.animate_scale) + if not 0.0 <= contact_threshold <= 1.0: + raise ValueError(f"Conveyor contact_threshold must be in [0, 1], got {self.contact_threshold!r}.") + if friction_coefficient < 0.0: + raise ValueError( + f"Conveyor friction_coefficient must be finite and non-negative, got {self.friction_coefficient!r}." + ) + if animate_scale < 0.0: + raise ValueError(f"Conveyor animate_scale must be finite and non-negative, got {self.animate_scale!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"Conveyor 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, + "animate_direction", + _validate_vector("animate_direction", self.animate_direction, 2, nonzero=self.animate_texture), + ) + object.__setattr__(self, "radius", radius) + object.__setattr__(self, "contact_threshold", contact_threshold) + object.__setattr__(self, "friction_coefficient", friction_coefficient) + object.__setattr__(self, "animate_scale", animate_scale) + + +@runtime_checkable +class ConveyorBeltView(Protocol): + """Tensorized control contract implemented by conveyor physics backends.""" + + @property + def prim_paths(self) -> tuple[str, ...]: + """Resolved collision prim paths in stable belt-index order.""" + ... + + @property + def num_belts(self) -> int: + """Number of resolved conveyor surfaces.""" + ... + + @property + def count(self) -> int: + """Alias for :attr:`num_belts`, matching tensor-view naming.""" + ... + + def set_velocities(self, velocities: Any, indices: Any = None) -> None: + """Set signed surface velocities [m/s] for selected belts.""" + ... + + def get_velocities(self, indices: Any = None, clone: bool = True) -> Any: + """Return effective surface velocities [m/s] for selected belts.""" + ... + + 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 belts without discarding their commands.""" + ... + + def get_enabled(self, indices: Any = None, clone: bool = True) -> Any: + """Return integer enabled flags for selected belts.""" + ... + + def set_friction_coefficients(self, coefficients: Any, indices: Any = None) -> None: + """Set Coulomb traction coefficients for selected belts.""" + ... + + def get_friction_coefficients(self, indices: Any = None, clone: bool = True) -> Any: + """Return Coulomb traction coefficients for selected belts.""" + ... + + def set_contact_processing_thresholds(self, thresholds: Any, indices: Any = None) -> None: + """Set contact-normal alignment thresholds for selected belts.""" + ... + + def get_contact_processing_thresholds(self, indices: Any = None, clone: bool = True) -> Any: + """Return contact-normal alignment thresholds for selected belts.""" + ... + + def get_encoder_positions(self, indices: Any = None, clone: bool = True) -> Any: + """Return integrated belt travel [m] for selected belts.""" + ... + + 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/test/sim/test_conveyor_belt.py b/source/isaaclab/test/sim/test_conveyor_belt.py new file mode 100644 index 000000000000..13d8d3f14dd0 --- /dev/null +++ b/source/isaaclab/test/sim/test_conveyor_belt.py @@ -0,0 +1,77 @@ +# 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 + +"""Tests for the backend-neutral conveyor belt contract.""" + +from __future__ import annotations + +import pytest + +from isaaclab.physics import ConveyorBeltSpec + + +def test_conveyor_belt_spec_preserves_authored_semantics() -> None: + """The shared description carries schema-aligned fields without backend imports.""" + spec = ConveyorBeltSpec( + prim_path="{ENV_REGEX_NS}/Belt/Curve", + velocity=-0.35, + enabled=False, + direction=(0, 0, -1), + curved=True, + pivot_point=(0.58, 0.51, 0.0), + radius=0.24, + surface_normal=(0, 0, 1), + contact_threshold=0.997, + friction_coefficient=0.5, + animate_texture=True, + animate_direction=(1, 0), + animate_scale=0.5, + ) + + assert spec.prim_path == "{ENV_REGEX_NS}/Belt/Curve" + assert spec.velocity == -0.35 + assert spec.enabled is False + assert spec.direction == (0.0, 0.0, -1.0) + assert spec.curved is True + assert spec.pivot_point == (0.58, 0.51, 0.0) + assert spec.radius == 0.24 + assert spec.surface_normal == (0.0, 0.0, 1.0) + assert spec.contact_threshold == 0.997 + assert spec.friction_coefficient == 0.5 + assert spec.animate_texture is True + assert spec.animate_direction == (1.0, 0.0) + assert spec.animate_scale == 0.5 + + +@pytest.mark.parametrize( + ("kwargs", "message"), + [ + ({"prim_path": "relative/Belt"}, "prim_path"), + ({"prim_path": "/"}, "prim_path"), + ({"prim_path": "/World//Belt"}, "prim_path"), + ({"prim_path": "/World/Belt/"}, "prim_path"), + ({"prim_path": "{ENV_REGEX_NS}/"}, "prim_path"), + ({"prim_path": "{UNKNOWN_NS}/Belt"}, "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": "/World/Belt", "animate_scale": -1.0}, "animate_scale"), + ( + {"prim_path": "/World/Belt", "animate_texture": True, "animate_direction": (0.0, 0.0)}, + "animate_direction", + ), + ], +) +def test_conveyor_belt_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): + ConveyorBeltSpec(**kwargs) diff --git a/source/isaaclab_newton/changelog.d/maximiliank-conveyor-substep-callback.rst b/source/isaaclab_newton/changelog.d/maximiliank-conveyor-substep-callback.rst index fdc1236d8422..4d6a292e70b8 100644 --- a/source/isaaclab_newton/changelog.d/maximiliank-conveyor-substep-callback.rst +++ b/source/isaaclab_newton/changelog.d/maximiliank-conveyor-substep-callback.rst @@ -1,4 +1,5 @@ Added ^^^^^ -* Added lifecycle-safe Newton manager callbacks for contact-force feedback after each solver substep. +* Added lifecycle-safe Newton manager callbacks for binding model-specific resources before CUDA graph capture + and applying contact-force feedback after each solver substep. diff --git a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py index b99ee68bfe3c..ab16b5f74b85 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py @@ -411,6 +411,10 @@ class NewtonManager(PhysicsManager): # 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 @@ -1082,6 +1086,7 @@ def clear(cls): 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 @@ -2101,6 +2106,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 @@ -3217,6 +3228,40 @@ def register_post_solver_substep_callback( 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._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] diff --git a/source/isaaclab_newton/test/physics/test_newton_manager_abstraction.py b/source/isaaclab_newton/test/physics/test_newton_manager_abstraction.py index cd921729e20b..3abb221db4ea 100644 --- a/source/isaaclab_newton/test/physics/test_newton_manager_abstraction.py +++ b/source/isaaclab_newton/test/physics/test_newton_manager_abstraction.py @@ -275,29 +275,60 @@ def state_force_callback(_state): def substep_callback(_solver, _contacts, _state, _dt): pass + def solver_init_callback(_model, _contacts): + pass + monkeypatch.setattr(NewtonManager, "_post_actuator_callbacks", []) monkeypatch.setattr(NewtonManager, "_state_force_callbacks", []) monkeypatch.setattr(NewtonManager, "_post_solver_substep_callbacks", []) + monkeypatch.setattr(NewtonManager, "_solver_init_callbacks", []) NewtonManager.register_post_actuator_callback(actuator_callback) NewtonManager.register_state_force_callback(state_force_callback) NewtonManager.register_post_solver_substep_callback(substep_callback) + NewtonManager.register_solver_init_callback(solver_init_callback) assert NewtonManager._post_actuator_callbacks == [actuator_callback] assert NewtonManager._state_force_callbacks == [state_force_callback] assert NewtonManager._post_solver_substep_callbacks == [substep_callback] + assert NewtonManager._solver_init_callbacks == [solver_init_callback] NewtonManager.unregister_post_actuator_callback(actuator_callback) NewtonManager.unregister_state_force_callback(state_force_callback) NewtonManager.unregister_post_solver_substep_callback(substep_callback) + NewtonManager.unregister_solver_init_callback(solver_init_callback) # Repeated cleanup is intentionally a safe no-op. NewtonManager.unregister_post_actuator_callback(actuator_callback) NewtonManager.unregister_state_force_callback(state_force_callback) NewtonManager.unregister_post_solver_substep_callback(substep_callback) + NewtonManager.unregister_solver_init_callback(solver_init_callback) assert NewtonManager._post_actuator_callbacks == [] assert NewtonManager._state_force_callbacks == [] assert NewtonManager._post_solver_substep_callbacks == [] + assert NewtonManager._solver_init_callbacks == [] + + +def test_solver_init_callbacks_rebind_to_each_model(monkeypatch: pytest.MonkeyPatch) -> None: + """Solver-init callbacks run once per model, including hard-reset replacements.""" + calls = [] + + def callback(model, contacts): + calls.append((model, contacts)) + + monkeypatch.setattr(NewtonManager, "_solver_init_callbacks", []) + NewtonManager.register_solver_init_callback(callback) + # Duplicate registration must not cause two graph bindings. + NewtonManager.register_solver_init_callback(callback) + + first = (object(), object()) + second = (object(), object()) + for model, contacts in (first, second): + monkeypatch.setattr(NewtonManager, "_model", model) + monkeypatch.setattr(NewtonManager, "_contacts", contacts) + NewtonManager._run_solver_init_callbacks() + + assert calls == [first, second] def test_refit_sensor_bvh_rejects_missing_sensor_state(monkeypatch): diff --git a/source/isaaclab_tasks/changelog.d/maximiliank-conveyor-franka.minor.rst b/source/isaaclab_tasks/changelog.d/maximiliank-conveyor-franka.minor.rst index 62fc01309178..b68be30e6fa4 100644 --- a/source/isaaclab_tasks/changelog.d/maximiliank-conveyor-franka.minor.rst +++ b/source/isaaclab_tasks/changelog.d/maximiliank-conveyor-franka.minor.rst @@ -4,3 +4,7 @@ Added * Added a contributed manager-based environment with guarded, counter-rotating force-driven racetrack conveyors, robust primitive and closed-mesh belt colliders, a MuJoCo Menagerie Franka, and an interactive Newton-viewer cube-goal selector. +* Added schema-aligned conveyor descriptions and a tensorized control view while retaining a single, + kitless Newton force owner with CUDA-graph and hard-reset-safe lifecycle binding. +* Added the opt-in ``IsaacContrib-Conveyor-Franka-PhysX-CPU-v0`` reference task, which explicitly + rejects GPU dynamics because the supported native surface-velocity path can drop conveyor contacts. diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/README.md b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/README.md new file mode 100644 index 000000000000..e85e666f836b --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/README.md @@ -0,0 +1,65 @@ + + +# Conveyor Franka + +This package provides checkpoint-compatible Newton and PhysX variants of a manager-based task in +which a Franka transfers four numbered cubes between two counter-rotating racetrack conveyors. +Both variants preserve the same ordered eight-dimensional action space, policy observations, +commands, rewards, reset recipes, 120 Hz physics step, and 60 Hz policy rate. + +## Backend support + +| Task | Physics device | Intended use | Conveyor actuation | +| --- | --- | --- | --- | +| `IsaacContrib-Conveyor-Franka-Newton-v0` | CUDA | Training and scalable playback | Batched Warp contact-force feedback captured with the Newton solver graph | +| `IsaacContrib-Conveyor-Franka-PhysX-CPU-v0` | CPU only | Native-PhysX reference and checkpoint playback | Authored `PhysxSurfaceVelocityAPI` on kinematic belt sections | + +The PhysX task rejects CUDA during configuration validation. In the supported Isaac Sim runtime, +enabling the native surface-velocity contact-modification path under GPU dynamics can drop the belt +contacts and let packages pass through the conveyor. CPU PhysX preserves those contacts. Use the +Newton task whenever GPU simulation or vectorized throughput is required. + +The two backends deliberately share the policy tensor contract, so an RSL-RL checkpoint can be +loaded by either task without reshaping or reordering tensors. Their contact and actuator dynamics +are not numerically identical; validate task behavior when transferring a policy between them. + +## Newton GPU playback + +Newton is kitless and supports the lightweight GL viewer: + +```bash +DISPLAY=:1 uv run isaaclab play --rl_library rsl_rl \ + --task IsaacContrib-Conveyor-Franka-Newton-v0 \ + --checkpoint /path/to/model.pt \ + --num_envs 8 --device cuda:0 --viz newton_gl --real-time +``` + +Training uses the same task ID and defaults to 256 environments: + +```bash +uv run isaaclab train --rl_library rsl_rl \ + --task IsaacContrib-Conveyor-Franka-Newton-v0 \ + --num_envs 256 --device cuda:0 +``` + +## PhysX CPU playback + +The native PhysX variant requires an Isaac Sim-enabled launch and an explicit CPU device. One +environment is the default and recommended interactive configuration: + +```bash +DISPLAY=:1 uv run isaaclab play --rl_library rsl_rl \ + --task IsaacContrib-Conveyor-Franka-PhysX-CPU-v0 \ + --checkpoint /path/to/model.pt \ + --num_envs 1 --device cpu --viz kit --real-time \ + agent.device=cpu +``` + +Overriding the task to CUDA is an error by design; keep the explicit `--device cpu` in launch +commands for clarity. The native surface-velocity backend stages commands through USD, while the +Newton backend keeps its batched state, contact processing, and force application on the GPU. diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/__init__.py index 160e417072d8..e3af7e9d3f26 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/__init__.py @@ -3,7 +3,7 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Force-driven conveyor scene with a Franka robot.""" +"""Backend-selectable conveyor scene with a Franka robot.""" import gymnasium as gym @@ -18,3 +18,16 @@ "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:ConveyorFrankaPPORunnerCfg", }, ) + +gym.register( + # The native PhysxSurfaceVelocityAPI path is intentionally CPU-only. Keep + # that execution contract visible in the public task ID so a CUDA launch is + # never mistaken for a supported configuration. + id="IsaacContrib-Conveyor-Franka-PhysX-CPU-v0", + entry_point=f"{__name__}.conveyor_franka_env:ConveyorFrankaEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.conveyor_franka_physx_env_cfg:ConveyorFrankaPhysxEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:ConveyorFrankaPPORunnerCfg", + }, +) diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_force_driver.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_force_driver.py index 911dd153105b..798e0b47ea91 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_force_driver.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_force_driver.py @@ -20,7 +20,7 @@ import warp as wp from isaaclab_newton.physics import NewtonManager -from .conveyor_geometry import ConveyorSectionSpec +from isaaclab.physics import ConveyorBeltSpec, PhysicsEvent _VELOCITY_FIELD_TYPE_CONSTANT = 0 _VELOCITY_FIELD_TYPE_PIVOT = 1 @@ -419,6 +419,16 @@ def _gather_float_values( 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 _add_body_force(dst: wp.array[wp.spatial_vector], src: wp.array[wp.spatial_vector]): body_id = wp.tid() @@ -490,58 +500,315 @@ 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_belt_specs(belt_specs: Sequence[ConveyorBeltSpec]) -> None: + """Validate Newton-specific requirements before registering lifecycle callbacks.""" + if not belt_specs: + raise ValueError("At least one conveyor belt specification is required.") + prim_paths = [spec.prim_path for spec in belt_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 belt_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 ConveyorForceDriver: - """Run one batched moving-surface force pipeline for a Newton scene.""" + """Own a conveyor force pipeline across the Newton simulation 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[ConveyorSectionSpec], - speed: float = 0.35, - friction: float = 0.5, - normal_threshold: float = 0.997, + belt_specs: Sequence[ConveyorBeltSpec] | None = None, + speed: float | None = None, + friction: float | None = None, + normal_threshold: float | None = None, startup_duration_s: float = 1.0, + env_path_format: str = "/World/envs/env_{}", transported_body_pattern: str = r"(?:^|/)Cube_?[0-3](?:/|$)", transported_body_count_per_env: int | None = None, ) -> None: - """Initialize the driver after Newton simulation startup. + """Register the force pipeline for the next Newton model initialization. Args: num_envs: Number of replicated simulation environments. - surface_specs: Collision sections and matching velocity fields. - speed: Initial signed conveyor surface speed [m/s]. - friction: Coulomb friction coefficient used to limit traction. - normal_threshold: Minimum contact-normal alignment in the range [0, 1]. + belt_specs: Authored conveyor descriptions in stable within-environment order. + speed: Optional initial surface-velocity override [m/s] applied to every belt. + friction: Optional Coulomb-traction override applied to every belt. + normal_threshold: Optional contact-normal alignment override applied to every belt. 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. transported_body_pattern: Regular expression selecting bodies that receive traction. transported_body_count_per_env: Expected selected body count per environment, or ``None``. """ + if belt_specs is None: + raise ValueError("At least one conveyor belt specification is required.") + belt_specs = tuple(belt_specs) + if not belt_specs: + raise ValueError("At least one conveyor belt specification is required.") + if not all(isinstance(spec, ConveyorBeltSpec) for spec in belt_specs): + raise TypeError("Every conveyor belt specification must be a ConveyorBeltSpec.") + _validate_newton_belt_specs(belt_specs) if num_envs <= 0: raise ValueError(f"Number of conveyor environments must be positive, got {num_envs}.") - if not np.isfinite(speed): + if num_envs > 1 and any(not spec.prim_path.startswith("{ENV_REGEX_NS}/") for spec in belt_specs): + raise ValueError( + "Replicated conveyor environments require every belt prim_path to start with '{ENV_REGEX_NS}/'." + ) + if speed is not None and not np.isfinite(speed): raise ValueError(f"Conveyor speed must be finite, got {speed}.") - if not np.isfinite(friction) or friction < 0.0: + if friction is not None and (not np.isfinite(friction) or friction < 0.0): raise ValueError(f"Conveyor friction must be non-negative, got {friction}.") - if not np.isfinite(normal_threshold) or not 0.0 <= normal_threshold <= 1.0: + if normal_threshold is not None and (not np.isfinite(normal_threshold) or not 0.0 <= normal_threshold <= 1.0): raise ValueError(f"Conveyor normal threshold must be in [0, 1], got {normal_threshold}.") 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) + try: + re.compile(transported_body_pattern) + except re.error as exc: + raise ValueError(f"Invalid transported-body pattern: {transported_body_pattern!r}.") from exc + if transported_body_count_per_env is not None and transported_body_count_per_env < 0: + raise ValueError("Expected transported-body count must be non-negative or None.") + self._binding: _ConveyorForceBinding | None = None + self._closed = False + self._num_envs = num_envs + self._belt_specs = belt_specs + self._binding_kwargs = { + "num_envs": num_envs, + "belt_specs": self._belt_specs, + "speed": speed, + "friction": friction, + "normal_threshold": normal_threshold, + "startup_duration_s": startup_duration_s, + "env_path_format": env_path_format, + "transported_body_pattern": transported_body_pattern, + "transported_body_count_per_env": transported_body_count_per_env, + } + self._model_init_handle = NewtonManager.register_callback( + self._request_contact_forces, + PhysicsEvent.MODEL_INIT, + name="conveyor_force_contact_attribute", + ) + try: + NewtonManager.register_solver_init_callback(self._bind_solver) + except Exception: + self._model_init_handle.deregister() + raise + + def _require_binding(self) -> _ConveyorForceBinding: + """Return the current binding or fail before solver initialization.""" + binding = self._binding + if binding is None: + raise RuntimeError("The conveyor force driver is not bound to an initialized Newton solver.") + return binding + + @property + def specs(self) -> tuple[ConveyorBeltSpec, ...]: + """Authored belt descriptions in stable within-environment order.""" + return self._belt_specs + + @property + def belts_per_env(self) -> int: + """Number of authored belts in each replicated environment.""" + return len(self._belt_specs) + + @property + def num_belts(self) -> int: + """Total number of resolved belts across all environments.""" + return self._num_envs * self.belts_per_env + + @property + def count(self) -> int: + """Alias for :attr:`num_belts`, matching tensor-view naming.""" + return self.num_belts + + @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 set_friction_coefficients(self, coefficients: Any, indices: Any = None) -> None: + """Set Coulomb traction limits for selected surfaces.""" + self._require_binding().set_friction_coefficients(coefficients, indices) + + def get_friction_coefficients(self, indices: Any = None, clone: bool = True) -> wp.array: + """Return Coulomb traction limits for selected surfaces.""" + return self._require_binding().get_friction_coefficients(indices, clone) + + def set_contact_processing_thresholds(self, thresholds: Any, indices: Any = None) -> None: + """Set minimum contact-normal alignment for selected surfaces.""" + self._require_binding().set_contact_processing_thresholds(thresholds, indices) + + def get_contact_processing_thresholds(self, indices: Any = None, clone: bool = True) -> wp.array: + """Return contact-normal alignment thresholds for selected surfaces.""" + return self._require_binding().get_contact_processing_thresholds(indices, clone) - self._surface_specs = tuple(surface_specs) - self._validate_surface_specs() + 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._friction_host.copy(), + previous._threshold_host.copy(), + ) + previous.close() + self._binding = None + + binding = _ConveyorForceBinding(model=model, contacts=contacts, **self._binding_kwargs) + if settings is not None: + velocities, enabled, friction, thresholds = settings + binding.set_velocities(velocities) + binding.set_enabled(enabled) + binding.set_friction_coefficients(friction) + binding.set_contact_processing_thresholds(thresholds) + 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 _ConveyorForceBinding: + """Run one batched moving-surface force pipeline for one Newton model.""" + + def __init__( + self, + model: Any, + contacts: Any, + num_envs: int, + belt_specs: Sequence[ConveyorBeltSpec], + speed: float | None = None, + friction: float | None = None, + normal_threshold: float | None = None, + startup_duration_s: float = 1.0, + env_path_format: str = "/World/envs/env_{}", + transported_body_pattern: str = r"(?:^|/)Cube_?[0-3](?:/|$)", + transported_body_count_per_env: int | None = None, + ) -> 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. + belt_specs: Authored conveyor descriptions in stable within-environment order. + speed: Optional initial surface-velocity override [m/s] applied to every belt. + friction: Optional Coulomb-traction override applied to every belt. + normal_threshold: Optional contact-normal alignment override applied to every belt. + 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. + transported_body_pattern: Regular expression selecting bodies that receive traction. + transported_body_count_per_env: Expected selected body count per environment, or ``None``. + """ + if num_envs <= 0: + raise ValueError(f"Number of conveyor environments must be positive, got {num_envs}.") + if speed is not None and not np.isfinite(speed): + raise ValueError(f"Conveyor speed must be finite, got {speed}.") + if friction is not None and (not np.isfinite(friction) or friction < 0.0): + raise ValueError(f"Conveyor friction must be non-negative, got {friction}.") + if normal_threshold is not None and (not np.isfinite(normal_threshold) or not 0.0 <= normal_threshold <= 1.0): + raise ValueError(f"Conveyor normal threshold must be in [0, 1], got {normal_threshold}.") + 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._belt_specs = tuple(belt_specs) + _validate_newton_belt_specs(self._belt_specs) try: body_pattern = re.compile(transported_body_pattern) except re.error as exc: raise ValueError(f"Invalid transported-body pattern: {transported_body_pattern!r}.") from exc - model = NewtonManager.get_model() - contacts = NewtonManager.get_contacts() if model is None or contacts is None: - raise RuntimeError("The conveyor driver must be created after Newton simulation initialization.") + 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 scene contact sensor must initialize " - "before the conveyor driver." + "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}.") @@ -554,22 +821,30 @@ def __init__( self._closed = False self._validate_backend_buffers() + belts_per_env = len(self._belt_specs) + conveyor_count = num_envs * belts_per_env shape_conveyor = [-1] * model.shape_count - field_type: list[int] = [] - direction: list[wp.vec3] = [] - pivot_point: list[wp.vec3] = [] - radius: list[float] = [] - surface_normal: list[wp.vec3] = [] - conveyor_world: list[int] = [] - surface_paths: list[str] = [] + field_type = [0] * 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 // belts_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() - patterns = tuple(re.compile(rf"(?:^|/){re.escape(spec.geometry.name)}(?:/|$)") for spec in self._surface_specs) seen_sections: set[tuple[int, int]] = set() for shape_id, label in enumerate(model.shape_label): - matching_specs = [index for index, pattern in enumerate(patterns) if pattern.search(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._belt_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: @@ -578,36 +853,31 @@ def __init__( raise ValueError(f"Conveyor shape must be static: {label}") spec_id = matching_specs[0] - world_id = int(shape_world[shape_id]) - if not 0 <= world_id < num_envs: - raise RuntimeError(f"Conveyor shape {label!r} belongs to invalid world {world_id}.") 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].geometry.name!r}." + f"{self._belt_specs[spec_id].prim_path!r}." ) seen_sections.add(section_key) - spec = self._surface_specs[spec_id] - conveyor_id = len(field_type) + spec = self._belt_specs[spec_id] + conveyor_id = world_id * belts_per_env + spec_id shape_conveyor[shape_id] = conveyor_id - field_type.append( - _VELOCITY_FIELD_TYPE_CONSTANT if spec.velocity_field_type == "constant" else _VELOCITY_FIELD_TYPE_PIVOT - ) - direction.append(_world_vector(shape_transform[shape_id], spec.direction)) - pivot_point.append(_world_point(shape_transform[shape_id], spec.pivot_point)) - radius.append(1.0 if spec.radius is None else spec.radius) - surface_normal.append(_world_vector(shape_transform[shape_id], spec.surface_normal)) - conveyor_world.append(world_id) - surface_paths.append(label) - - expected_sections = {(world_id, spec_id) for world_id in range(num_envs) for spec_id in range(len(patterns))} + field_type[conveyor_id] = _VELOCITY_FIELD_TYPE_PIVOT if spec.curved else _VELOCITY_FIELD_TYPE_CONSTANT + 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._belt_specs)) + } missing_sections = sorted(expected_sections - seen_sections) if missing_sections: details = ", ".join( - f"world {world_id}: {self._surface_specs[spec_id].geometry.name}" - for world_id, spec_id in missing_sections[:8] + f"world {world_id}: {self._belt_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}).") @@ -634,7 +904,6 @@ def __init__( if not np.any(body_is_tracked): raise RuntimeError(f"Transported-body pattern {transported_body_pattern!r} matched no Newton bodies.") - conveyor_count = len(field_type) self._surface_paths = tuple(surface_paths) self._shape_conveyor = wp.array(shape_conveyor, dtype=wp.int32, device=self._device) self._body_is_tracked = wp.array(body_is_tracked, dtype=wp.int32, device=self._device) @@ -645,10 +914,20 @@ def __init__( self._surface_normal = wp.array(surface_normal, dtype=wp.vec3, device=self._device) self._conveyor_world = wp.array(conveyor_world, dtype=wp.int32, device=self._device) - self._command_velocity_host = np.full(conveyor_count, speed, dtype=np.float32) - self._enabled_host = np.ones(conveyor_count, dtype=np.int32) - self._friction_host = np.full(conveyor_count, friction, dtype=np.float32) - self._threshold_host = np.full(conveyor_count, normal_threshold, dtype=np.float32) + authored_velocity = np.asarray([spec.velocity for spec in self._belt_specs], dtype=np.float32) + authored_enabled = np.asarray([spec.enabled for spec in self._belt_specs], dtype=np.int32) + authored_friction = np.asarray([spec.friction_coefficient for spec in self._belt_specs], dtype=np.float32) + authored_threshold = np.asarray([spec.contact_threshold for spec in self._belt_specs], dtype=np.float32) + self._command_velocity_host = np.tile(authored_velocity, num_envs) + self._enabled_host = np.tile(authored_enabled, num_envs) + self._friction_host = np.tile(authored_friction, num_envs) + self._threshold_host = np.tile(authored_threshold, num_envs) + if speed is not None: + self._command_velocity_host.fill(speed) + if friction is not None: + self._friction_host.fill(friction) + if normal_threshold is not None: + self._threshold_host.fill(normal_threshold) 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) @@ -701,6 +980,10 @@ def set_enabled(self, flags: Any, indices: Any = None) -> None: 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 set_friction_coefficients(self, coefficients: Any, indices: Any = None) -> None: """Set Coulomb traction limits for selected surfaces.""" selected = self._resolve_indices(indices) @@ -710,6 +993,10 @@ def set_friction_coefficients(self, coefficients: Any, indices: Any = None) -> N self._friction_host[selected] = values self._friction.assign(self._friction_host) + def get_friction_coefficients(self, indices: Any = None, clone: bool = True) -> wp.array: + """Return Coulomb traction limits for selected surfaces.""" + return self._get_device_values(self._friction, indices, clone) + def set_contact_processing_thresholds(self, thresholds: Any, indices: Any = None) -> None: """Set minimum contact-normal alignment for selected surfaces.""" selected = self._resolve_indices(indices) @@ -719,6 +1006,10 @@ def set_contact_processing_thresholds(self, thresholds: Any, indices: Any = None self._threshold_host[selected] = values self._threshold.assign(self._threshold_host) + def get_contact_processing_thresholds(self, indices: Any = None, clone: bool = True) -> wp.array: + """Return contact-normal alignment thresholds for selected surfaces.""" + return self._get_device_values(self._threshold, 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) @@ -860,36 +1151,6 @@ def update(self, solver, contacts, state, dt: float) -> None: device=self._device, ) - def _validate_surface_specs(self) -> None: - """Validate structural surface descriptions before resolving Newton shapes.""" - if not self._surface_specs: - raise ValueError("At least one conveyor surface specification is required.") - names = [spec.geometry.name for spec in self._surface_specs] - if len(set(names)) != len(names): - raise ValueError(f"Conveyor surface names must be unique, got {names}.") - for spec in self._surface_specs: - if spec.velocity_field_type not in {"constant", "pivot"}: - raise ValueError( - f"Unknown velocity field {spec.velocity_field_type!r} for conveyor surface {spec.geometry.name!r}." - ) - direction = np.asarray(spec.direction, dtype=np.float64) - pivot_point = np.asarray(spec.pivot_point, dtype=np.float64) - surface_normal = np.asarray(spec.surface_normal, dtype=np.float64) - if direction.shape != (3,) or not np.all(np.isfinite(direction)) or np.linalg.norm(direction) <= 1.0e-8: - raise ValueError(f"Conveyor surface {spec.geometry.name!r} needs a non-zero 3-D direction.") - if pivot_point.shape != (3,) or not np.all(np.isfinite(pivot_point)): - raise ValueError(f"Conveyor surface {spec.geometry.name!r} needs a 3-D pivot point.") - if ( - surface_normal.shape != (3,) - or not np.all(np.isfinite(surface_normal)) - or np.linalg.norm(surface_normal) <= 1.0e-8 - ): - raise ValueError(f"Conveyor surface {spec.geometry.name!r} needs a non-zero 3-D surface normal.") - if spec.velocity_field_type == "pivot" and ( - spec.radius is None or not np.isfinite(spec.radius) or spec.radius <= 0.0 - ): - raise ValueError(f"Pivot conveyor surface {spec.geometry.name!r} needs a positive arc radius.") - def _validate_backend_buffers(self) -> None: """Validate every fixed-size Newton buffer consumed by conveyor kernels.""" model = self._model @@ -955,6 +1216,23 @@ def _get_device_values(self, source: wp.array, indices: Any, clone: bool) -> wp. ) 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.""" diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env.py index 34a1b66d4374..0e67480c4cc2 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env.py @@ -3,44 +3,108 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Manager-based environment that installs the task-local conveyor force driver.""" +"""Manager-based environment that installs the selected conveyor physics adapter.""" from __future__ import annotations from collections.abc import Sequence from isaaclab.envs import ManagerBasedRLEnv +from isaaclab.physics import ConveyorBeltView from .conveyor_force_driver import ConveyorForceDriver from .conveyor_franka_env_cfg import ConveyorFrankaEnvCfg from .conveyor_geometry import belt_collision_section_specs from .conveyor_goal_selector import ConveyorGoalSelector +from .conveyor_physx_surface import PhysxSurfaceVelocityConveyor class ConveyorFrankaEnv(ManagerBasedRLEnv): - """Manager-based environment with force-driven Newton conveyor surfaces.""" + """Manager-based environment with backend-native conveyor surfaces.""" cfg: ConveyorFrankaEnvCfg def __init__(self, cfg: ConveyorFrankaEnvCfg, render_mode: str | None = None, **kwargs): + self._conveyor_driver: ConveyorBeltView | None = None super().__init__(cfg, render_mode=render_mode, **kwargs) - self._conveyor_driver = ConveyorForceDriver( - num_envs=self.num_envs, - surface_specs=tuple( - section for side in ("Left", "Right") for section in belt_collision_section_specs(side) - ), - speed=cfg.conveyor_force.speed, - friction=cfg.conveyor_force.friction, - normal_threshold=cfg.conveyor_force.normal_threshold, - startup_duration_s=cfg.conveyor_force.startup_duration_s, - transported_body_pattern=cfg.conveyor_force.transported_body_pattern, - transported_body_count_per_env=cfg.conveyor_force.transported_body_count_per_env, - ) self._goal_selector: ConveyorGoalSelector | None = None self._setup_goal_selector() + def _init_sim(self) -> None: + """Install the conveyor adapter at the lifecycle point required by its backend.""" + belt_spec_kwargs = { + "velocity": self.cfg.conveyor_force.speed, + "friction_coefficient": self.cfg.conveyor_force.friction, + "contact_threshold": self.cfg.conveyor_force.normal_threshold, + } + spec_builder = getattr(self.cfg.scene, "build_conveyor_belt_specs", None) + if spec_builder is None: + belt_specs = tuple( + section.belt + for side in ("Left", "Right") + for section in belt_collision_section_specs(side, **belt_spec_kwargs) + ) + else: + belt_specs = tuple(spec_builder(**belt_spec_kwargs)) + env_path_format = self.cfg.scene.clone_cfg.clone_regex.replace(".*", "{}") + + # Newton needs solved-contact attributes and graph callbacks registered + # before the first reset finalizes and captures the solver. PhysX belt + # schemas, by contrast, are authored by the scene spawners and its live + # command adapter is attached only after PhysX has parsed that scene. + from isaaclab_newton.physics import NewtonCfg + + if isinstance(self.cfg.sim.physics, NewtonCfg): + driver = ConveyorForceDriver( + num_envs=self.cfg.scene.num_envs, + belt_specs=belt_specs, + startup_duration_s=self.cfg.conveyor_force.startup_duration_s, + env_path_format=env_path_format, + transported_body_pattern=self.cfg.conveyor_force.transported_body_pattern, + transported_body_count_per_env=self.cfg.conveyor_force.transported_body_count_per_env, + ) + self._conveyor_driver = driver + try: + super()._init_sim() + except Exception: + driver.close() + self._conveyor_driver = None + raise + return + + from isaaclab_physx.physics import PhysxCfg + + if not isinstance(self.cfg.sim.physics, PhysxCfg): + raise ValueError(f"Unsupported conveyor physics backend: {type(self.cfg.sim.physics).__name__}.") + + configure_conveyor = getattr(self.cfg.scene, "configure_conveyor", None) + if configure_conveyor is not None: + configure_conveyor(friction_coefficient=self.cfg.conveyor_force.friction) + super()._init_sim() + driver = PhysxSurfaceVelocityConveyor( + num_envs=self.cfg.scene.num_envs, + belt_specs=belt_specs, + env_path_format=env_path_format, + startup_duration_s=self.cfg.conveyor_force.startup_duration_s, + stage=self.sim.stage, + ) + try: + driver.start() + except Exception: + driver.close() + raise + self._conveyor_driver = driver + + @property + def conveyor_belt(self) -> ConveyorBeltView: + """Tensorized conveyor control view for this environment.""" + driver = self._conveyor_driver + if driver is None: + raise RuntimeError("The conveyor belt is unavailable before simulation initialization or after close().") + return driver + def _setup_goal_selector(self) -> None: - """Attach one task panel to the first interactive Newton visualizer.""" + """Attach one task panel to the first supported interactive visualizer.""" for visualizer in self.sim.visualizers: if getattr(visualizer.cfg, "visualizer_type", None) not in {"newton_gl", "newton_rtx"}: continue @@ -62,7 +126,7 @@ def _reset_idx(self, env_ids: Sequence[int]): conveyor_driver.reset(env_ids) def close(self): - """Release the conveyor callbacks before the Newton scene is destroyed.""" + """Release conveyor callbacks before the physics scene is destroyed.""" conveyor_driver = getattr(self, "_conveyor_driver", None) if conveyor_driver is not None: conveyor_driver.close() diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env_cfg.py index 3c93777b90e2..3e54e29ff264 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env_cfg.py @@ -24,36 +24,63 @@ from isaaclab.managers import SceneEntityCfg from isaaclab.managers import TerminationTermCfg as DoneTerm from isaaclab.scene import InteractiveSceneCfg -from isaaclab.sensors import ContactSensorCfg from isaaclab.sim import SimulationCfg from isaaclab.sim.schemas import CollisionFragment, UsdPhysicsCollisionCfg from isaaclab.sim.spawners.materials import RigidBodyMaterialBaseCfg from isaaclab.utils.configclass import configclass +from isaaclab.visualizers import VisualizerCfg from . import mdp from .conveyor_geometry import ( - BELT_CENTER_Y, BELT_COLOR, - BELT_TURN_RADIUS, + BELT_INNER_STRAIGHT_Y, + BELT_OUTER_STRAIGHT_Y, CUBE_COLORS, + CUBE_INNER_SLOT_X, + CUBE_OUTER_SLOT_X, GUARD_COLOR, CuboidSpec, MeshSpec, - belt_collision_section_specs, + belt_collision_geometry_specs, belt_mesh_spec, guard_mesh_specs, ) from .franka_robot_cfg import FRANKA_PANDA_CONVEYOR_CFG +from .mdp.terminations import invalid_action as invalid_policy_action _DYNAMIC_PROPERTIES = sim_utils.RigidBodyBaseCfg() _CONTACT_GAP = 0.01 _CUBE_CONTACT_MARGIN = 0.003 -_MANIPULATION_CONTACT_STIFFNESS = 1.0e4 -_MANIPULATION_CONTACT_DAMPING = 200.0 _MUJOCO_SOLIMP = (0.9, 0.95, 0.001, 0.5, 2.0) _MUJOCO_SOLREF = (0.02, 1.0) _SUBGOAL_TIMEOUT_S = 20.0 _TRANSFER_SEQUENCE_LENGTH = 8 +_ARM_JOINT_NAMES = tuple(f"panda_joint{joint_id}" for joint_id in range(1, 8)) +_FINGER_JOINT_NAMES = ("panda_finger_joint1", "panda_finger_joint2") + + +def _validate_common_config(cfg: ConveyorFrankaEnvCfg) -> None: + """Validate backend-independent timing and policy tensor contracts.""" + cfg.conveyor_force.validate_config() + if not cfg.sim.use_newton_actuators: + raise ValueError("The conveyor Franka requires the shared Newton-actuator execution path.") + if not math.isfinite(cfg.sim.dt) or cfg.sim.dt <= 0.0 or cfg.decimation <= 0: + raise ValueError("Simulation dt and environment decimation must be positive.") + arm_action = cfg.actions.arm_action + if arm_action.joint_names != list(_ARM_JOINT_NAMES) or not arm_action.preserve_order: + raise ValueError("Arm actions must preserve the explicit panda_joint1-to-panda_joint7 ordering.") + if not math.isfinite(arm_action.max_delta) or arm_action.max_delta <= 0.0: + raise ValueError("Arm max_delta must be finite and positive.") + if not math.isfinite(arm_action.joint_limit_margin) or arm_action.joint_limit_margin < 0.0: + raise ValueError("Arm joint_limit_margin must be finite and non-negative.") + lower = arm_action.workspace_lower + upper = arm_action.workspace_upper + if len(lower) != len(_ARM_JOINT_NAMES) or len(upper) != len(_ARM_JOINT_NAMES): + raise ValueError("Arm workspace bounds must contain one value per controlled joint.") + if any(not math.isfinite(value) for value in (*lower, *upper)): + raise ValueError("Arm workspace bounds must be finite.") + if any(low >= high for low, high in zip(lower, upper, strict=True)): + raise ValueError("Every arm workspace lower bound must be less than its upper bound.") def _collision_properties(contact_margin: float = 0.0, mujoco_priority: int = 0) -> list[CollisionFragment]: @@ -84,14 +111,14 @@ class ActionsCfg: arm_action = mdp.ConveyorRelativeJointPositionActionCfg( asset_name="robot", - joint_names=["panda_joint[1-7]"], + joint_names=list(_ARM_JOINT_NAMES), + preserve_order=True, scale=0.12, max_delta=0.12, - gravity_compensation=True, ) gripper_action = mdp.ResetBufferedGripperActionCfg( asset_name="robot", - joint_names=["panda_finger_joint[1-2]"], + joint_names=list(_FINGER_JOINT_NAMES), open_command_expr={"panda_finger_joint.*": 0.04}, close_command_expr={"panda_finger_joint.*": 0.0}, force_close_steps=5, @@ -129,15 +156,15 @@ class PolicyCfg(ObsGroup): joint_pos = ObsTerm( func=mdp.joint_pos_rel, - params={"asset_cfg": SceneEntityCfg("robot", joint_names=["panda_joint[1-7]"])}, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=list(_ARM_JOINT_NAMES), preserve_order=True)}, ) joint_vel = ObsTerm( func=mdp.joint_vel_rel, - params={"asset_cfg": SceneEntityCfg("robot", joint_names=["panda_joint[1-7]"])}, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=list(_ARM_JOINT_NAMES), preserve_order=True)}, ) gripper_pos = ObsTerm( func=mdp.gripper_joint_positions, - params={"robot_cfg": SceneEntityCfg("robot", joint_names=["panda_finger_joint[1-2]"])}, + params={"robot_cfg": SceneEntityCfg("robot", joint_names=list(_FINGER_JOINT_NAMES), preserve_order=True)}, ) objects = ObsTerm(func=mdp.transfer_object_observation) active_transfer = ObsTerm(func=mdp.active_transfer_features) @@ -197,7 +224,7 @@ class RewardsCfg: action_rate_l2 = RewTerm(func=mdp.action_rate_l2, weight=-1.0e-3) joint_velocity_l2 = RewTerm( func=mdp.finite_joint_velocity_l2, - params={"asset_cfg": SceneEntityCfg("robot", joint_names=["panda_joint[1-7]"])}, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=list(_ARM_JOINT_NAMES), preserve_order=True)}, weight=-1.0e-4, ) @@ -207,6 +234,10 @@ class TerminationsCfg: """Safety failures and bounded training sequences.""" cube_out_of_workspace = DoneTerm(func=mdp.cube_out_of_workspace) + invalid_action = DoneTerm( + func=invalid_policy_action, + params={"action_names": ("arm_action", "gripper_action")}, + ) nonfinite_scene_state = DoneTerm(func=mdp.nonfinite_scene_state) subgoal_time_out = DoneTerm( func=mdp.subgoal_time_out, @@ -276,6 +307,10 @@ class ConveyorForceCfg: def __post_init__(self) -> None: """Validate conveyor force parameters.""" + self.validate_config() + + def validate_config(self) -> None: + """Validate final conveyor-force values after overrides are applied.""" if not math.isfinite(self.speed) or self.speed < 0.0: raise ValueError(f"Conveyor speed must be non-negative, got {self.speed}.") if not math.isfinite(self.friction) or self.friction < 0.0: @@ -394,16 +429,12 @@ def _visual_mesh( ) -def _collision_material(friction: float, stiff_contact: bool) -> NewtonMaterialPropertiesCfg: - """Build frictionless-drive contact material, optionally with manipulation-grade gains.""" +def _collision_material(friction: float) -> NewtonMaterialPropertiesCfg: + """Build a contact material for surfaces using the task-wide raw MuJoCo response.""" return NewtonMaterialPropertiesCfg( static_friction=friction, dynamic_friction=friction, restitution=0.0, - torsional_friction=0.0, - rolling_friction=0.0, - contact_stiffness=_MANIPULATION_CONTACT_STIFFNESS if stiff_contact else None, - contact_damping=_MANIPULATION_CONTACT_DAMPING if stiff_contact else None, ) @@ -412,7 +443,6 @@ def _hidden_collision_mesh( spec: MeshSpec, friction: float, mujoco_priority: int, - stiff_contact: bool = False, ) -> AssetBaseCfg: """Build a hidden static triangle-mesh collider.""" spawn = sim_utils.MeshCustomCfg( @@ -420,7 +450,7 @@ def _hidden_collision_mesh( faces=spec.faces, visible=False, collision_props=_collision_properties(mujoco_priority=mujoco_priority), - physics_material=_collision_material(friction, stiff_contact), + physics_material=_collision_material(friction), ) spawn.func = _spawn_hidden_collision_mesh return AssetBaseCfg(prim_path=prim_path, spawn=spawn) @@ -431,7 +461,6 @@ def _hidden_collision_cuboid( spec: CuboidSpec, friction: float, mujoco_priority: int, - stiff_contact: bool = False, ) -> AssetBaseCfg: """Build a hidden native cuboid collider.""" return AssetBaseCfg( @@ -441,7 +470,7 @@ def _hidden_collision_cuboid( size=spec.size, visible=False, collision_props=_collision_properties(mujoco_priority=mujoco_priority), - physics_material=_collision_material(friction, stiff_contact), + physics_material=_collision_material(friction), ), ) @@ -451,7 +480,6 @@ def _hidden_collision_geometry( spec: MeshSpec | CuboidSpec, friction: float, mujoco_priority: int, - stiff_contact: bool = False, ) -> AssetBaseCfg: """Build hidden collision geometry while preferring native primitives where possible.""" if isinstance(spec, CuboidSpec): @@ -460,14 +488,12 @@ def _hidden_collision_geometry( spec=spec, friction=friction, mujoco_priority=mujoco_priority, - stiff_contact=stiff_contact, ) return _hidden_collision_mesh( prim_path=prim_path, spec=spec, friction=friction, mujoco_priority=mujoco_priority, - stiff_contact=stiff_contact, ) @@ -490,10 +516,6 @@ def _cube( static_friction=0.8, dynamic_friction=0.6, restitution=0.0, - torsional_friction=0.002, - rolling_friction=0.0001, - contact_stiffness=_MANIPULATION_CONTACT_STIFFNESS, - contact_damping=_MANIPULATION_CONTACT_DAMPING, ) spawn.func = _spawn_shape_with_display_color return RigidObjectCfg( @@ -537,17 +559,10 @@ class ConveyorFrankaSceneCfg(InteractiveSceneCfg): color=(0.18, 0.20, 0.23), ) - cube_0 = _cube("Cube0", CUBE_COLORS[0], (0.30, BELT_CENTER_Y - BELT_TURN_RADIUS, 0.06)) - cube_1 = _cube("Cube1", CUBE_COLORS[1], (0.78, BELT_CENTER_Y - BELT_TURN_RADIUS, 0.06)) - cube_2 = _cube("Cube2", CUBE_COLORS[2], (0.30, -BELT_CENTER_Y + BELT_TURN_RADIUS, 0.06)) - cube_3 = _cube("Cube3", CUBE_COLORS[3], (0.78, -BELT_CENTER_Y + BELT_TURN_RADIUS, 0.06)) - - cube_contacts = ContactSensorCfg( - prim_path="{ENV_REGEX_NS}/Cube.*", - update_period=0.0, - history_length=1, - debug_vis=False, - ) + cube_0 = _cube("Cube0", CUBE_COLORS[0], (CUBE_INNER_SLOT_X, BELT_INNER_STRAIGHT_Y, 0.06)) + cube_1 = _cube("Cube1", CUBE_COLORS[1], (CUBE_OUTER_SLOT_X, BELT_OUTER_STRAIGHT_Y, 0.06)) + cube_2 = _cube("Cube2", CUBE_COLORS[2], (CUBE_INNER_SLOT_X, -BELT_INNER_STRAIGHT_Y, 0.06)) + cube_3 = _cube("Cube3", CUBE_COLORS[3], (CUBE_OUTER_SLOT_X, -BELT_OUTER_STRAIGHT_Y, 0.06)) ground = AssetBaseCfg( prim_path="/World/GroundPlane", @@ -576,8 +591,7 @@ def __post_init__(self) -> None: ) section_keys = ("top_straight", "bottom_straight", "right_turn", "left_turn") - for section_key, section in zip(section_keys, belt_collision_section_specs(side), strict=True): - spec = section.geometry + for section_key, spec in zip(section_keys, belt_collision_geometry_specs(side), strict=True): setattr( self, f"conveyor_{side.lower()}_{section_key}_collision", @@ -589,8 +603,6 @@ def __post_init__(self) -> None: friction=1.1e-5, # Override cube friction only for collision-section/cube pairs. mujoco_priority=1, - # Match the locally stable Franka Stack support surface. - stiff_contact=True, ), ) @@ -646,14 +658,16 @@ class ConveyorFrankaEnvCfg(ManagerBasedRLEnvCfg): solver_cfg=MJWarpSolverCfg( solver="newton", integrator="implicitfast", - njmax=2000, - nconmax=1000, - impratio=0.1, + # Per-environment capacities include headroom for all four cubes, + # the gripper, table, belts, and guard contacts without allocating + # the previous 256k-contact scene-wide driver buffers. + njmax=300, + nconmax=200, + impratio=1.0, cone="elliptic", update_data_interval=1, iterations=100, ls_iterations=50, - ls_parallel=False, use_mujoco_contacts=False, ccd_iterations=35, ), @@ -663,37 +677,42 @@ class ConveyorFrankaEnvCfg(ManagerBasedRLEnvCfg): collision_decimation=0, default_shape_cfg=NewtonShapeCfg(margin=0.0, gap=_CONTACT_GAP, ke=2.5e3, kd=100.0), num_substeps=1, - use_cuda_graph=False, - load_visual_shapes=True, + use_cuda_graph=True, + # Import render-only geometry only when a visualizer or camera needs it. + load_visual_shapes=None, ), + use_newton_actuators=True, ) def __post_init__(self) -> None: - self.seed = 42 - # Frame the complete robot and both conveyor lanes in the Newton viewer. - try: - import isaaclab_visualizers # noqa: F401 - except ModuleNotFoundError as exc: - if exc.name != "isaaclab_visualizers": - raise - return - from isaaclab_visualizers.newton import NewtonGLVisualizerCfg - - # Explicit --viz newton_rtx replaces this backend while retaining the shared camera hints. + # Any visualizer selected at runtime receives these shared camera hints. # Newton camera pose: position (2.13, 0.0, 1.0), pitch -23.9 degrees, # yaw 180 degrees. The look-at point is one unit along that view ray. - self.sim.default_visualizer_cfg = NewtonGLVisualizerCfg( + self.sim.default_visualizer_cfg = VisualizerCfg( eye=(2.13, 0.0, 1.0), lookat=(1.2157460448, 0.0, 0.5948584132), - streaming_view=False, + max_visible_envs=1, + randomly_sample_visible_envs=False, ) + def validate_config(self) -> None: + """Validate the final task configuration after command-line overrides are applied.""" + _validate_common_config(self) + physics = self.sim.physics + if not isinstance(physics, NewtonCfg) or not isinstance(physics.solver_cfg, MJWarpSolverCfg): + raise ValueError("The conveyor force driver requires the Newton MJWarp backend.") + if physics.solver_cfg.use_mujoco_contacts: + raise ValueError("The conveyor force driver requires the Newton collision-pipeline contact path.") + if physics.collision_cfg is None: + raise ValueError("The conveyor force driver requires an explicit Newton collision pipeline.") + def play_mode(self) -> None: - """Run continuing transfers from randomized moving-belt starts.""" + """Run continuing transfers from evenly distributed moving-belt starts.""" super().play_mode() self.scene.num_envs = min(self.scene.num_envs, 8) self.events.reset_from_state_table.params["fixed_recipe"] = int(mdp.ConveyorResetRecipe.BELT) self.events.reset_from_state_table.params["fixed_variant_id"] = mdp.BELT_DEPLOYMENT_VARIANT + self.events.reset_from_state_table.params["cube_position_noise"] = 0.0 # Successful placements already transition to a new commanded cube. # Playback removes training-only refreshes and runs until physics leaves # the recoverable workspace. diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_physx_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_physx_env_cfg.py new file mode 100644 index 000000000000..b19e317b0a1d --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_physx_env_cfg.py @@ -0,0 +1,425 @@ +# 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 configuration for conveyor-Franka policy playback.""" + +from __future__ import annotations + +import functools +from dataclasses import replace + +from isaaclab_physx.physics import PhysxCfg +from isaaclab_physx.sim.schemas import PhysxCollisionCfg, PhysxSDFMeshCfg +from isaaclab_physx.sim.spawners.materials import PhysxRigidBodyMaterialCfg + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg +from isaaclab.physics import ConveyorBeltSpec +from isaaclab.sim import SimulationCfg +from isaaclab.sim.schemas import CollisionFragment, UsdPhysicsCollisionCfg +from isaaclab.utils.configclass import configclass + +from .conveyor_franka_env_cfg import ( + ConveyorFrankaEnvCfg, + ConveyorFrankaSceneCfg, + _ARM_JOINT_NAMES, + _CUBE_CONTACT_MARGIN, + _CONTACT_GAP, + _spawn_hidden_collision_mesh, + _spawn_shape_with_display_color, + _validate_common_config, + _visual_mesh, +) +from .conveyor_geometry import ( + BELT_COLOR, + BELT_INNER_STRAIGHT_Y, + BELT_OUTER_STRAIGHT_Y, + CUBE_COLORS, + CUBE_INNER_SLOT_X, + CUBE_OUTER_SLOT_X, + GUARD_COLOR, + ConveyorSectionSpec, + CuboidSpec, + MeshSpec, + belt_collision_section_specs, + belt_mesh_spec, + guard_mesh_specs, +) +from .conveyor_physx_surface import apply_physx_surface_velocity_api +from .franka_robot_cfg import FRANKA_PANDA_CONVEYOR_PHYSX_CFG + +_PHYSX_DYNAMIC_PROPERTIES = sim_utils.RigidBodyBaseCfg() +_PHYSX_KINEMATIC_PROPERTIES = sim_utils.RigidBodyBaseCfg( + rigid_body_enabled=True, + kinematic_enabled=True, + disable_gravity=True, +) + + +def _physx_collision_properties(contact_offset: float = 0.005) -> list[CollisionFragment]: + """Build the standard collision and PhysX offset fragments for one collider.""" + return [ + UsdPhysicsCollisionCfg(collision_enabled=True), + PhysxCollisionCfg(contact_offset=contact_offset, rest_offset=0.0), + ] + + +def _physx_material(friction: float) -> PhysxRigidBodyMaterialCfg: + """Build a deterministic PhysX material for a conveyor-task collision surface.""" + return PhysxRigidBodyMaterialCfg( + static_friction=friction, + dynamic_friction=friction, + restitution=0.0, + friction_combine_mode="min", + restitution_combine_mode="min", + ) + + +def _physx_static_cuboid( + prim_path: str, + size: tuple[float, float, float], + pos: tuple[float, float, float], + color: tuple[float, float, float], +) -> AssetBaseCfg: + """Build one static PhysX support cuboid.""" + spawn = sim_utils.CuboidCfg( + size=size, + collision_props=_physx_collision_properties(), + physics_material=_physx_material(0.7), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=color, roughness=0.75), + ) + spawn.func = _spawn_shape_with_display_color + return AssetBaseCfg( + prim_path=prim_path, + init_state=AssetBaseCfg.InitialStateCfg(pos=pos), + spawn=spawn, + ) + + +def _physx_cube( + name: str, + color: tuple[float, float, float], + pos: tuple[float, float, float], +) -> RigidObjectCfg: + """Build one numbered dynamic cube with native PhysX contact properties.""" + spawn = sim_utils.CuboidCfg( + size=(0.04, 0.04, 0.04), + rigid_props=_PHYSX_DYNAMIC_PROPERTIES, + mass_props=sim_utils.MassPropertiesCfg(mass=0.05), + collision_props=_physx_collision_properties(contact_offset=_CUBE_CONTACT_MARGIN), + physics_material=PhysxRigidBodyMaterialCfg( + static_friction=0.8, + dynamic_friction=0.6, + restitution=0.0, + restitution_combine_mode="min", + ), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=color, roughness=0.75), + ) + spawn.func = _spawn_shape_with_display_color + return RigidObjectCfg( + prim_path=f"{{ENV_REGEX_NS}}/{name}", + init_state=RigidObjectCfg.InitialStateCfg(pos=pos), + spawn=spawn, + ) + + +def _pivot_centered_section(section: ConveyorSectionSpec) -> tuple[ConveyorSectionSpec, tuple[float, float, float]]: + """Express a curved section around its rigid-body origin for native angular surface velocity.""" + if not section.belt.curved: + geometry = section.geometry + if not isinstance(geometry, CuboidSpec): + raise TypeError("Straight PhysX conveyor sections must use native cuboids.") + return section, geometry.position + + geometry = section.geometry + if not isinstance(geometry, MeshSpec): + raise TypeError("Curved PhysX conveyor sections must use closed meshes.") + pivot = section.belt.pivot_point + local_geometry = MeshSpec( + name=geometry.name, + vertices=tuple( + (vertex[0] - pivot[0], vertex[1] - pivot[1], vertex[2] - pivot[2]) for vertex in geometry.vertices + ), + faces=geometry.faces, + ) + local_belt = replace(section.belt, pivot_point=(0.0, 0.0, 0.0)) + return ConveyorSectionSpec(geometry=local_geometry, belt=local_belt), pivot + + +def physx_belt_section_specs( + side: str, + *, + velocity: float = 0.0, + friction_coefficient: float = 0.5, + contact_threshold: float = 0.997, +) -> tuple[tuple[ConveyorSectionSpec, tuple[float, float, float]], ...]: + """Return pivot-centered sections sharing the exact runtime PhysX semantics.""" + return tuple( + _pivot_centered_section(section) + for section in belt_collision_section_specs( + side, + velocity=velocity, + friction_coefficient=friction_coefficient, + contact_threshold=contact_threshold, + ) + ) + + +@sim_utils.clone +def _spawn_physx_conveyor_mesh( + prim_path: str, + cfg: sim_utils.MeshCustomCfg, + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, + *, + belt_spec: ConveyorBeltSpec, + **kwargs, +): + """Spawn one hidden SDF turn and author native surface velocity before PhysX parsing.""" + prim = sim_utils.spawn_mesh_custom(prim_path, cfg, translation, orientation, **kwargs) + sim_utils.set_prim_visibility(prim, False) + apply_physx_surface_velocity_api(prim, belt_spec, velocity_scale=0.0) + return prim + + +@sim_utils.clone +def _spawn_physx_conveyor_cuboid( + prim_path: str, + cfg: sim_utils.CuboidCfg, + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, + *, + belt_spec: ConveyorBeltSpec, + **kwargs, +): + """Spawn one hidden analytic straight and author native surface velocity before PhysX parsing.""" + prim = sim_utils.spawn_cuboid(prim_path, cfg, translation, orientation, **kwargs) + sim_utils.set_prim_visibility(prim, False) + apply_physx_surface_velocity_api(prim, belt_spec, velocity_scale=0.0) + return prim + + +def _physx_conveyor_collision( + prim_path: str, + section: ConveyorSectionSpec, + root_position: tuple[float, float, float], + friction: float, +) -> AssetBaseCfg: + """Build one native-velocity belt section with analytic or watertight-SDF collision.""" + geometry = section.geometry + if isinstance(geometry, CuboidSpec): + spawn = sim_utils.CuboidCfg( + size=geometry.size, + visible=False, + rigid_props=_PHYSX_KINEMATIC_PROPERTIES, + collision_props=_physx_collision_properties(contact_offset=_CONTACT_GAP), + physics_material=_physx_material(friction), + ) + spawn.func = functools.partial(_spawn_physx_conveyor_cuboid, belt_spec=section.belt) + else: + spawn = sim_utils.MeshCustomCfg( + vertices=geometry.vertices, + faces=geometry.faces, + visible=False, + rigid_props=_PHYSX_KINEMATIC_PROPERTIES, + collision_props=[ + *_physx_collision_properties(contact_offset=_CONTACT_GAP), + PhysxSDFMeshCfg(sdf_resolution=128, sdf_subgrid_resolution=6), + ], + collision_approximation="sdf", + physics_material=_physx_material(friction), + ) + spawn.func = functools.partial(_spawn_physx_conveyor_mesh, belt_spec=section.belt) + return AssetBaseCfg( + prim_path=prim_path, + init_state=AssetBaseCfg.InitialStateCfg(pos=root_position), + spawn=spawn, + ) + + +def _physx_guard_collision(prim_path: str, spec: MeshSpec) -> AssetBaseCfg: + """Build one freely sliding static guide collider.""" + spawn = sim_utils.MeshCustomCfg( + vertices=spec.vertices, + faces=spec.faces, + visible=False, + collision_props=_physx_collision_properties(), + collision_approximation="none", + physics_material=_physx_material(1.1e-5), + ) + spawn.func = _spawn_hidden_collision_mesh + return AssetBaseCfg(prim_path=prim_path, spawn=spawn) + + +@configclass +class ConveyorFrankaPhysxSceneCfg(ConveyorFrankaSceneCfg): + """PhysX scene preserving the Newton task's names, layout, and tensor contracts.""" + + robot = FRANKA_PANDA_CONVEYOR_PHYSX_CFG.replace( + prim_path="{ENV_REGEX_NS}/Robot", + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + "panda_joint1": 0.0, + "panda_joint2": -0.35, + "panda_joint3": 0.0, + "panda_joint4": -2.35, + "panda_joint5": 0.0, + "panda_joint6": 2.0, + "panda_joint7": 0.78, + "panda_finger_joint.*": 0.04, + } + ), + ) + + tabletop = _physx_static_cuboid( + prim_path="{ENV_REGEX_NS}/Tabletop", + size=(2.0, 1.9, 0.08), + pos=(0.50, 0.0, -0.04), + color=(0.32, 0.34, 0.37), + ) + table_pedestal = _physx_static_cuboid( + prim_path="{ENV_REGEX_NS}/TablePedestal", + size=(0.75, 0.55, 0.76), + pos=(0.25, 0.0, -0.46), + color=(0.18, 0.20, 0.23), + ) + + cube_0 = _physx_cube("Cube0", CUBE_COLORS[0], (CUBE_INNER_SLOT_X, BELT_INNER_STRAIGHT_Y, 0.06)) + cube_1 = _physx_cube("Cube1", CUBE_COLORS[1], (CUBE_OUTER_SLOT_X, BELT_OUTER_STRAIGHT_Y, 0.06)) + cube_2 = _physx_cube("Cube2", CUBE_COLORS[2], (CUBE_INNER_SLOT_X, -BELT_INNER_STRAIGHT_Y, 0.06)) + cube_3 = _physx_cube("Cube3", CUBE_COLORS[3], (CUBE_OUTER_SLOT_X, -BELT_OUTER_STRAIGHT_Y, 0.06)) + + def __post_init__(self) -> None: + """Generate visuals plus native PhysX belt and guide collision bodies.""" + for side in ("Left", "Right"): + visual = belt_mesh_spec(side) + setattr( + self, + f"conveyor_{side.lower()}_belt_visual", + _visual_mesh( + prim_path=f"{{ENV_REGEX_NS}}/{visual.name}", + spec=visual, + color=BELT_COLOR, + roughness=0.9, + metallic=0.0, + ), + ) + + section_keys = ("top_straight", "bottom_straight", "right_turn", "left_turn") + for section_key, (section, root_position) in zip( + section_keys, physx_belt_section_specs(side), strict=True + ): + setattr( + self, + f"conveyor_{side.lower()}_{section_key}_collision", + _physx_conveyor_collision( + prim_path=f"{{ENV_REGEX_NS}}/{section.geometry.name}", + section=section, + root_position=root_position, + friction=0.5, + ), + ) + + for guard in guard_mesh_specs(side): + boundary = "inner" if guard.name.endswith("Inner") else "outer" + setattr( + self, + f"guard_{side.lower()}_{boundary}_visual", + _visual_mesh( + prim_path=f"{{ENV_REGEX_NS}}/{guard.name}Visual", + spec=guard, + color=GUARD_COLOR, + roughness=0.3, + metallic=0.8, + ), + ) + setattr( + self, + f"guard_{side.lower()}_{boundary}_collision", + _physx_guard_collision(f"{{ENV_REGEX_NS}}/{guard.name}Collision", guard), + ) + + def build_conveyor_belt_specs( + self, + *, + velocity: float, + friction_coefficient: float, + contact_threshold: float, + ) -> tuple[ConveyorBeltSpec, ...]: + """Return the same pivot-local belt descriptions used by the PhysX spawners.""" + return tuple( + section.belt + for side in ("Left", "Right") + for section, _ in physx_belt_section_specs( + side, + velocity=velocity, + friction_coefficient=friction_coefficient, + contact_threshold=contact_threshold, + ) + ) + + def configure_conveyor(self, *, friction_coefficient: float) -> None: + """Propagate a final command-line friction override into every belt material before spawning.""" + for side in ("left", "right"): + for section_key in ("top_straight", "bottom_straight", "right_turn", "left_turn"): + asset = getattr(self, f"conveyor_{side}_{section_key}_collision") + material = asset.spawn.physics_material + material.static_friction = friction_coefficient + material.dynamic_friction = friction_coefficient + + +@configclass +class ConveyorFrankaPhysxEnvCfg(ConveyorFrankaEnvCfg): + """Checkpoint-compatible CPU reference using native PhysX surface velocity. + + The supported and tested native ``PhysxSurfaceVelocityAPI`` path is CPU-only. Use + :class:`ConveyorFrankaEnvCfg` for scalable GPU simulation with Newton. + """ + + scene: ConveyorFrankaPhysxSceneCfg = ConveyorFrankaPhysxSceneCfg( + # USD surface-velocity commands are authored on the host. One environment + # is the useful default for this CPU reference; callers may opt into a + # small replicated batch explicitly. + num_envs=1, + env_spacing=3.0, + replicate_physics=True, + ) + sim: SimulationCfg = SimulationCfg( + # The supported GPU contact-modification path drops contacts for shapes + # with PhysxSurfaceVelocityAPI enabled. CPU PhysX preserves the authored + # API and its belt contacts; fail validation on an accidental CUDA + # override instead of letting cubes tunnel silently. + device="cpu", + dt=1.0 / 120.0, + render_interval=2, + physics=PhysxCfg( + solver_type=1, + solve_articulation_contact_last=True, + max_position_iteration_count=64, + max_velocity_iteration_count=16, + bounce_threshold_velocity=0.2, + friction_offset_threshold=0.01, + friction_correlation_distance=0.00625, + enable_ccd=True, + ), + use_fabric=True, + use_newton_actuators=True, + ) + + def validate_config(self) -> None: + """Validate PhysX selection and the shared checkpoint-facing policy contract.""" + _validate_common_config(self) + if not isinstance(self.sim.physics, PhysxCfg): + raise ValueError("The PhysX conveyor task requires the native Isaac Sim PhysX backend.") + if self.sim.device != "cpu": + raise ValueError( + "The native PhysX conveyor is CPU-only because enabled PhysxSurfaceVelocityAPI shapes can lose " + "contacts under GPU dynamics. Run this task with '--device cpu'; use the Newton task for GPU " + "simulation." + ) + if self.scene.robot.spawn.joint_drive_props is not None: + raise ValueError("The PhysX Franka must not author MuJoCo-only joint-drive properties.") + if self.scene.robot.spawn.rigid_props.disable_gravity is not True: + raise ValueError("The PhysX Franka must preserve the trained gravity-compensated policy contract.") diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_geometry.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_geometry.py index eaa8d9f79925..c2ea7c2ff80d 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_geometry.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_geometry.py @@ -9,7 +9,8 @@ import math from dataclasses import dataclass -from typing import Literal + +from isaaclab.physics import ConveyorBeltSpec BELT_COLOR = (0.09, 0.09, 0.09) """Dark-rubber color used by Newton's conveyor example.""" @@ -36,6 +37,14 @@ BELT_TOP_Z = 0.04 TURN_SEGMENT_COUNT = 96 +# Four deployment slots place one cube on each straight run of the two +# racetracks. The x coordinates are mirrored about the racetrack center, so +# the inner/outer pair on a belt is separated by exactly half a lap. +BELT_INNER_STRAIGHT_Y = BELT_CENTER_Y - BELT_TURN_RADIUS +BELT_OUTER_STRAIGHT_Y = BELT_CENTER_Y + BELT_TURN_RADIUS +CUBE_INNER_SLOT_X = BELT_CENTER_X - BELT_HALF_STRAIGHT / 3.0 +CUBE_OUTER_SLOT_X = BELT_CENTER_X + BELT_HALF_STRAIGHT / 3.0 + # Collision surfaces extend underneath the rails and overlap at section seams. # This keeps the dynamic parcels on a continuous +Z-facing surface without # exposing the belt prism's vertical side faces to the contact solver. @@ -69,21 +78,10 @@ class CuboidSpec: @dataclass(frozen=True) class ConveyorSectionSpec: - """Collision geometry and velocity field for one conveyor section. - - The direction and pivot are expressed in the collision prim's local frame. - Constant sections interpret ``direction`` as the unit travel direction; - pivot sections interpret it as the unit rotation axis and use ``radius`` - to convert commanded linear speed to angular speed. ``surface_normal`` is - also local, so rotated or inclined sections need no world-space special case. - """ + """Task geometry paired with its backend-neutral conveyor description.""" geometry: MeshSpec | CuboidSpec - velocity_field_type: Literal["constant", "pivot"] - direction: tuple[float, float, float] - 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) + belt: ConveyorBeltSpec def belt_direction(side: str) -> float: @@ -301,39 +299,69 @@ def _turn_collision_mesh(name: str, pivot_x: float, center_y: float, start_angle def belt_collision_geometry_specs(side: str) -> tuple[CuboidSpec | MeshSpec, ...]: """Build native straight and closed-mesh turn collision geometry for one racetrack.""" - return tuple(section.geometry for section in belt_collision_section_specs(side)) + center_y = BELT_CENTER_Y if side == "Left" else -BELT_CENTER_Y + left_x = BELT_CENTER_X - BELT_HALF_STRAIGHT + right_x = BELT_CENTER_X + BELT_HALF_STRAIGHT + return ( + _straight_collision_cuboid(f"Conveyor{side}TopStraightCollision", center_y + BELT_TURN_RADIUS), + _straight_collision_cuboid(f"Conveyor{side}BottomStraightCollision", center_y - BELT_TURN_RADIUS), + _turn_collision_mesh(f"Conveyor{side}RightTurnCollision", right_x, center_y, -0.5 * math.pi), + _turn_collision_mesh(f"Conveyor{side}LeftTurnCollision", left_x, center_y, 0.5 * math.pi), + ) def belt_collision_section_specs( side: str, + *, + velocity: float = 0.0, + friction_coefficient: float = 0.7, + contact_threshold: float = 0.997, + enabled: bool = True, ) -> tuple[ConveyorSectionSpec, ConveyorSectionSpec, ConveyorSectionSpec, ConveyorSectionSpec]: - """Build collision meshes and their matching conveyor velocity fields.""" + """Build collision geometry and authored conveyor intent for one racetrack.""" center_y = BELT_CENTER_Y if side == "Left" else -BELT_CENTER_Y left_x = BELT_CENTER_X - BELT_HALF_STRAIGHT right_x = BELT_CENTER_X + BELT_HALF_STRAIGHT direction = belt_direction(side) + top_straight, bottom_straight, right_turn, left_turn = belt_collision_geometry_specs(side) + + def belt( + geometry: MeshSpec | CuboidSpec, + travel_direction: tuple[float, float, float], + *, + curved: bool = False, + pivot_point: tuple[float, float, float] = (0.0, 0.0, 0.0), + radius: float | None = None, + ) -> ConveyorSectionSpec: + return ConveyorSectionSpec( + geometry=geometry, + belt=ConveyorBeltSpec( + prim_path=f"{{ENV_REGEX_NS}}/{geometry.name}", + velocity=velocity, + enabled=enabled, + direction=travel_direction, + curved=curved, + pivot_point=pivot_point, + radius=radius, + contact_threshold=contact_threshold, + friction_coefficient=friction_coefficient, + ), + ) + return ( - ConveyorSectionSpec( - geometry=_straight_collision_cuboid(f"Conveyor{side}TopStraightCollision", center_y + BELT_TURN_RADIUS), - velocity_field_type="constant", - direction=(direction, 0.0, 0.0), - ), - ConveyorSectionSpec( - geometry=_straight_collision_cuboid(f"Conveyor{side}BottomStraightCollision", center_y - BELT_TURN_RADIUS), - velocity_field_type="constant", - direction=(-direction, 0.0, 0.0), - ), - ConveyorSectionSpec( - geometry=_turn_collision_mesh(f"Conveyor{side}RightTurnCollision", right_x, center_y, -0.5 * math.pi), - velocity_field_type="pivot", - direction=(0.0, 0.0, -direction), + belt(top_straight, (direction, 0.0, 0.0)), + belt(bottom_straight, (-direction, 0.0, 0.0)), + belt( + right_turn, + (0.0, 0.0, -direction), + curved=True, pivot_point=(right_x, center_y, 0.0), radius=BELT_TURN_RADIUS, ), - ConveyorSectionSpec( - geometry=_turn_collision_mesh(f"Conveyor{side}LeftTurnCollision", left_x, center_y, 0.5 * math.pi), - velocity_field_type="pivot", - direction=(0.0, 0.0, -direction), + belt( + left_turn, + (0.0, 0.0, -direction), + curved=True, pivot_point=(left_x, center_y, 0.0), radius=BELT_TURN_RADIUS, ), diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_physx_surface.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_physx_surface.py new file mode 100644 index 000000000000..40fcf39bb1cd --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_physx_surface.py @@ -0,0 +1,558 @@ +# 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 conveyors for the conveyor-Franka task. + +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.physics import ConveyorBeltSpec + +_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_physx_surface_velocity_twist( + spec: ConveyorBeltSpec, 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:`ConveyorBeltSpec.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_physx_surface_velocity_api( + prim_or_path: Any, + spec: ConveyorBeltSpec, + *, + 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_physx_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_physx_conveyor_paths( + num_envs: int, + belt_specs: Sequence[ConveyorBeltSpec], + 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. + belt_specs: Within-environment belt descriptions. + env_path_format: Exact environment path format containing one ``{}`` field. + + Returns: + Exact paths ordered by environment, then by ``belt_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(belt_specs) + if not specs or not all(isinstance(spec, ConveyorBeltSpec) for spec in specs): + raise ValueError("Conveyor belt_specs must contain at least one ConveyorBeltSpec.") + 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 PhysxSurfaceVelocityConveyor: + """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_physx_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, + belt_specs: Sequence[ConveyorBeltSpec], + *, + 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. + belt_specs: Within-environment belt 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._belt_specs = tuple(belt_specs) + self._surface_paths = resolve_physx_conveyor_paths(num_envs, self._belt_specs, env_path_format) + self._belts_per_env = len(self._belt_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._belt_specs], dtype=np.float32), self._num_envs + ) + self._enabled = np.tile(np.asarray([spec.enabled for spec in self._belt_specs], dtype=np.bool_), self._num_envs) + self._friction = np.tile( + np.asarray([spec.friction_coefficient for spec in self._belt_specs], dtype=np.float32), self._num_envs + ) + self._threshold = np.tile( + np.asarray([spec.contact_threshold for spec in self._belt_specs], dtype=np.float32), self._num_envs + ) + self._encoder_position = np.zeros(self.num_belts, dtype=np.float32) + self._last_authored: list[tuple[bool, PhysxSurfaceVelocityTwist] | None] = [None] * self.num_belts + self._flush(force=True) + + @property + def specs(self) -> tuple[ConveyorBeltSpec, ...]: + """Return authored descriptions in stable within-environment order.""" + return self._belt_specs + + @property + def belts_per_env(self) -> int: + """Return the number of authored belts per environment.""" + return self._belts_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_belts(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_belts`.""" + return self.num_belts + + @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 import IsaacEvents, PhysxManager + + self._callback_handle = PhysxManager.register_callback( + self.update, + IsaacEvents.POST_PHYSICS_STEP, + name="physx_conveyor_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 set_friction_coefficients(self, coefficients: Any, indices: Any = None) -> None: + """Reject unsupported runtime friction mutation explicitly.""" + del coefficients, indices + raise NotImplementedError( + "Native PhysX surface velocity uses authored collision materials; mutate material friction explicitly." + ) + + def get_friction_coefficients(self, indices: Any = None, clone: bool = True) -> np.ndarray: + """Return authored friction metadata retained for control introspection.""" + return self._get_values(self._friction, indices, clone) + + def set_contact_processing_thresholds(self, thresholds: Any, indices: Any = None) -> None: + """Reject unsupported normal-threshold mutation explicitly.""" + del thresholds, indices + raise NotImplementedError( + "PhysxSurfaceVelocityAPI has no contact-normal threshold; the native body-level field affects all contacts." + ) + + def get_contact_processing_thresholds(self, indices: Any = None, clone: bool = True) -> np.ndarray: + """Return authored threshold metadata retained for control introspection.""" + return self._get_values(self._threshold, 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._belts_per_env + np.arange(self._belts_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_belts): + 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._belt_specs[index % self._belts_per_env] + speed = float(self._command_velocity[index]) * self._velocity_scale if enabled else 0.0 + twist = compute_physx_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_belts, dtype=np.int64) + if isinstance(indices, slice): + return np.arange(self.num_belts, dtype=np.int64)[indices] + selected = _as_numpy(indices) + if selected.dtype == np.bool_: + if selected.ndim != 1 or selected.size != self.num_belts: + raise IndexError(f"Boolean conveyor indices must have length {self.num_belts}.") + 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_belts)): + 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_physx_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_tasks/isaaclab_tasks/contrib/conveyor_franka/franka_robot_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/franka_robot_cfg.py index 5af59966c399..89250854b6c2 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/franka_robot_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/franka_robot_cfg.py @@ -5,12 +5,17 @@ """Task-calibrated Franka configuration for conveyor manipulation.""" +from isaaclab_newton.sim.schemas import MujocoJointCfg + from isaaclab.actuators import ImplicitActuatorCfg from isaaclab_assets.robots.franka import FRANKA_PANDA_MENAGERIE_CFG FRANKA_PANDA_CONVEYOR_CFG = FRANKA_PANDA_MENAGERIE_CFG.copy() FRANKA_PANDA_CONVEYOR_CFG.spawn.rigid_props.disable_gravity = False +# Route gravity compensation through MuJoCo's actuator channel so effort limits +# and the solver apply it consistently with the configured implicit drives. +FRANKA_PANDA_CONVEYOR_CFG.spawn.joint_drive_props = [MujocoJointCfg(actuatorgravcomp=True)] FRANKA_PANDA_CONVEYOR_CFG.actuators = { "panda_arm": ImplicitActuatorCfg( joint_names_expr=["panda_joint[1-7]"], @@ -45,4 +50,17 @@ armature=0.1, ), } -"""Menagerie Franka with explicit manipulation gains and gravity compensation-ready dynamics.""" +"""Menagerie Franka with explicit manipulation gains and solver-native gravity compensation.""" + + +FRANKA_PANDA_CONVEYOR_PHYSX_CFG = FRANKA_PANDA_CONVEYOR_CFG.copy() +# PhysX does not consume MuJoCo's actuator-gravity-compensation attribute. Disabling +# gravity on the robot is the closest solver-native equivalent and keeps the trained +# position-policy contract unchanged without adding a task-side effort loop. +FRANKA_PANDA_CONVEYOR_PHYSX_CFG.spawn.rigid_props.disable_gravity = True +FRANKA_PANDA_CONVEYOR_PHYSX_CFG.spawn.joint_drive_props = None +# Contact-rich manipulation benefits from resolving the articulation for more than +# the generic asset defaults, especially with the deliberately stiff trained gains. +FRANKA_PANDA_CONVEYOR_PHYSX_CFG.spawn.articulation_props.solver_position_iteration_count = 32 +FRANKA_PANDA_CONVEYOR_PHYSX_CFG.spawn.articulation_props.solver_velocity_iteration_count = 4 +"""PhysX variant with the same joints, gains, action ordering, and gravity-compensated policy contract.""" diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/__init__.py index ce1212bf20c1..56b9c4494587 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/__init__.py @@ -5,40 +5,6 @@ """MDP terms for the conveyor-to-conveyor Franka transfer task.""" -from isaaclab.envs.mdp import * # noqa: F401, F403 -from isaaclab_tasks.core.lift.mdp.events_cfg import SuccessMonitorCfg +from isaaclab.utils.module import lazy_export -from .actions import ConveyorRelativeJointPositionAction, ResetBufferedGripperAction -from .actions_cfg import ConveyorRelativeJointPositionActionCfg, ResetBufferedGripperActionCfg -from .commands import ConveyorTransferCommand, ConveyorTransferCommandCfg, transfer_success_mask -from .curriculums import ConveyorResetCurriculum -from .observations import ( - active_transfer_features, - cube_conveyor_state, - end_effector_axes, - end_effector_velocity, - gripper_joint_positions, - target_cube_one_hot, - target_side_one_hot, - transfer_object_observation, -) -from .reset_events import ( - BELT_DEPLOYMENT_VARIANT, - ConveyorResetRecipe, - ConveyorResetStateTable, - build_reset_rows, - select_next_transfer_cube, -) -from .rewards import ( - action_term_l2, - finite_joint_velocity_l2, - physical_cube_acquisition_mask, - terminal_failure, - transfer_success_reward, -) -from .terminations import ( - cube_out_of_workspace, - nonfinite_scene_state, - subgoal_time_out, - transfer_sequence_time_out, -) +lazy_export() diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/__init__.pyi b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/__init__.pyi new file mode 100644 index 000000000000..d4c939e36985 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/__init__.pyi @@ -0,0 +1,77 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +__all__ = [ + "BELT_DEPLOYMENT_VARIANT", + "ConveyorRelativeJointPositionAction", + "ConveyorRelativeJointPositionActionCfg", + "ConveyorResetCurriculum", + "ConveyorResetRecipe", + "ConveyorResetStateTable", + "ConveyorTransferCommand", + "ConveyorTransferCommandCfg", + "ResetBufferedGripperAction", + "ResetBufferedGripperActionCfg", + "SuccessMonitorCfg", + "action_term_l2", + "active_transfer_features", + "build_reset_rows", + "cube_conveyor_state", + "cube_out_of_workspace", + "end_effector_axes", + "end_effector_velocity", + "finite_joint_velocity_l2", + "gripper_joint_positions", + "invalid_action", + "nonfinite_scene_state", + "physical_cube_acquisition_mask", + "select_next_transfer_cube", + "subgoal_time_out", + "target_cube_one_hot", + "target_side_one_hot", + "terminal_failure", + "transfer_object_observation", + "transfer_sequence_time_out", + "transfer_success_mask", + "transfer_success_reward", +] + +from .actions import ConveyorRelativeJointPositionAction, ResetBufferedGripperAction +from .actions_cfg import ConveyorRelativeJointPositionActionCfg, ResetBufferedGripperActionCfg +from .commands import ConveyorTransferCommand, ConveyorTransferCommandCfg, transfer_success_mask +from .curriculums import ConveyorResetCurriculum +from .observations import ( + active_transfer_features, + cube_conveyor_state, + end_effector_axes, + end_effector_velocity, + gripper_joint_positions, + target_cube_one_hot, + target_side_one_hot, + transfer_object_observation, +) +from .reset_events import ( + BELT_DEPLOYMENT_VARIANT, + ConveyorResetRecipe, + ConveyorResetStateTable, + build_reset_rows, + select_next_transfer_cube, +) +from .rewards import ( + action_term_l2, + finite_joint_velocity_l2, + physical_cube_acquisition_mask, + terminal_failure, + transfer_success_reward, +) +from .terminations import ( + cube_out_of_workspace, + invalid_action, + nonfinite_scene_state, + subgoal_time_out, + transfer_sequence_time_out, +) +from isaaclab.envs.mdp import * +from isaaclab_tasks.core.lift.mdp.events_cfg import SuccessMonitorCfg diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/actions.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/actions.py index 4b64617a6aee..7a2e42b25561 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/actions.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/actions.py @@ -7,6 +7,7 @@ from __future__ import annotations +import math from collections.abc import Sequence from typing import TYPE_CHECKING @@ -33,25 +34,31 @@ class ConveyorRelativeJointPositionAction(JointAction): def __init__(self, cfg: ConveyorRelativeJointPositionActionCfg, env: ManagerBasedEnv) -> None: super().__init__(cfg, env) - if cfg.max_delta <= 0.0: - raise ValueError("max_delta must be positive.") - if cfg.joint_limit_margin < 0.0: - raise ValueError("joint_limit_margin must be non-negative.") + if not math.isfinite(cfg.max_delta) or cfg.max_delta <= 0.0: + raise ValueError("max_delta must be finite and positive.") + if not math.isfinite(cfg.joint_limit_margin) or cfg.joint_limit_margin < 0.0: + raise ValueError("joint_limit_margin must be finite and non-negative.") self._workspace_lower = torch.tensor(cfg.workspace_lower, dtype=torch.float32, device=self.device) self._workspace_upper = torch.tensor(cfg.workspace_upper, dtype=torch.float32, device=self.device) if self._workspace_lower.shape != (self.action_dim,) or self._workspace_upper.shape != (self.action_dim,): raise ValueError("workspace bounds must contain one value per controlled joint.") if torch.any(self._workspace_lower >= self._workspace_upper): raise ValueError("Every lower workspace bound must be less than its upper bound.") - resolved_joint_ids = ( - list(range(self._asset.num_joints)) if isinstance(self._joint_ids, slice) else self._joint_ids - ) - self._gravity_joint_ids = [joint_id + self._asset.num_base_dofs for joint_id in resolved_joint_ids] + if not torch.all(torch.isfinite(self._workspace_lower)) or not torch.all(torch.isfinite(self._workspace_upper)): + raise ValueError("workspace bounds must be finite.") + self._invalid_actions = torch.zeros(self.num_envs, dtype=torch.bool, device=self.device) self._position_targets = self._asset.data.joint_pos.torch[:, self._joint_ids].clone() + @property + def invalid_actions(self) -> torch.Tensor: + """Whether the latest policy action contained a non-finite component.""" + return self._invalid_actions + def process_actions(self, actions: torch.Tensor) -> None: """Convert normalized residuals into bounded position targets [rad].""" - super().process_actions(actions) + self._invalid_actions.copy_(~torch.isfinite(actions).all(dim=1)) + finite_actions = torch.nan_to_num(actions, nan=0.0, posinf=1.0, neginf=-1.0).clamp(-1.0, 1.0) + super().process_actions(finite_actions) delta = torch.clamp(self._processed_actions, min=-self.cfg.max_delta, max=self.cfg.max_delta) positions = self._asset.data.joint_pos.torch[:, self._joint_ids] limits = self._asset.data.soft_joint_pos_limits.torch[:, self._joint_ids] @@ -59,15 +66,10 @@ def process_actions(self, actions: torch.Tensor) -> None: upper = torch.minimum(limits[..., 1] - self.cfg.joint_limit_margin, self._workspace_upper) self._position_targets = torch.clamp(positions + delta, min=lower, max=upper) self._processed_actions = self._position_targets - self._raw_actions[:] = actions def apply_actions(self) -> None: - """Hold the policy-step target and gravity feedforward through all physics substeps.""" + """Hold the policy-step target through all physics substeps.""" self._asset.set_joint_position_target_index(target=self._position_targets, joint_ids=self._joint_ids) - if self.cfg.gravity_compensation: - gravity = self._asset.data.gravity_compensation_forces.torch[:, self._gravity_joint_ids] - gravity = torch.where(torch.isfinite(gravity), gravity, torch.zeros_like(gravity)) - self._asset.set_joint_effort_target_index(target=gravity, joint_ids=self._joint_ids) def reset(self, env_ids: Sequence[int] | None = None) -> None: """Initialize targets from the sampled reset pose.""" @@ -79,6 +81,7 @@ def reset(self, env_ids: Sequence[int] | None = None) -> None: else: self._position_targets[env_ids] = positions[env_ids] self._processed_actions[env_ids] = positions[env_ids] + self._invalid_actions[env_ids] = False class ResetBufferedGripperAction(BinaryJointPositionAction): @@ -86,9 +89,31 @@ class ResetBufferedGripperAction(BinaryJointPositionAction): cfg: ResetBufferedGripperActionCfg + def __init__(self, cfg: ResetBufferedGripperActionCfg, env: ManagerBasedEnv) -> None: + super().__init__(cfg, env) + self._invalid_actions = torch.zeros(self.num_envs, dtype=torch.bool, device=self.device) + + @property + def invalid_actions(self) -> torch.Tensor: + """Whether the latest gripper command contained a non-finite value.""" + return self._invalid_actions + def process_actions(self, actions: torch.Tensor) -> None: - """Map binary commands and preserve initially held cubes.""" - super().process_actions(actions) + """Map finite binary commands and preserve initially held cubes.""" + if actions.dtype == torch.bool: + self._invalid_actions.zero_() + finite_actions = actions + else: + self._invalid_actions.copy_(~torch.isfinite(actions).all(dim=1)) + # A non-finite gripper command closes the fingers for the final safe + # step before the invalid-action termination is evaluated. + finite_actions = torch.nan_to_num(actions, nan=-1.0, posinf=1.0, neginf=-1.0).clamp(-1.0, 1.0) + super().process_actions(finite_actions) command = self._env.command_manager.get_term(self.cfg.command_name) force_close = (command.held_cube_ids >= 0) & (self._env.episode_length_buf < self.cfg.force_close_steps) self._processed_actions[force_close] = self._close_command + + def reset(self, env_ids: Sequence[int] | None = None) -> None: + """Clear buffered invalid-command state for selected environments.""" + super().reset(env_ids) + self._invalid_actions[env_ids] = False diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/actions_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/actions_cfg.py index 588e85404adb..40bc923f779e 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/actions_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/actions_cfg.py @@ -28,9 +28,6 @@ class ConveyorRelativeJointPositionActionCfg(JointActionCfg): max_delta: float = 0.12 """Maximum target change per policy step [rad].""" - gravity_compensation: bool = False - """Whether to add model-based gravity feedforward to the arm joints.""" - workspace_lower: tuple[float, ...] = (-0.75, -0.45, -0.55, -2.75, -0.45, 1.85, -0.10) """Lower boundary of the validated transfer workspace [rad].""" diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/reset_events.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/reset_events.py index a47985929168..5f6ff0dd9017 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/reset_events.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/reset_events.py @@ -17,7 +17,14 @@ from isaaclab.managers import EventTermCfg, ManagerTermBase -from ..conveyor_geometry import BELT_CENTER_Y, BELT_TOP_Z, BELT_TURN_RADIUS +from ..conveyor_geometry import ( + BELT_CENTER_X, + BELT_INNER_STRAIGHT_Y, + BELT_OUTER_STRAIGHT_Y, + BELT_TOP_Z, + CUBE_INNER_SLOT_X, + CUBE_OUTER_SLOT_X, +) if TYPE_CHECKING: from isaaclab.assets import Articulation, RigidObject @@ -238,8 +245,41 @@ def franka_tool_position(joint_positions: torch.Tensor) -> torch.Tensor: def side_inner_y(side_ids: torch.Tensor) -> torch.Tensor: """Return the reachable inner-straight y coordinate [m] for each side.""" - magnitude = BELT_CENTER_Y - BELT_TURN_RADIUS - return torch.where(side_ids == LEFT_SIDE, magnitude, -magnitude) + return torch.where(side_ids == LEFT_SIDE, BELT_INNER_STRAIGHT_Y, -BELT_INNER_STRAIGHT_Y) + + +def _balanced_cube_slots( + target_cube_ids: torch.Tensor, + source_side_ids: torch.Tensor, +) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor, torch.Tensor]: + """Assign one cube to every inner/outer racetrack run. + + Slots are ordered ``left inner``, ``left outer``, ``right inner``, and + ``right outer``. The commanded cube swaps with the canonical occupant of + its source-side inner slot, keeping it reachable without duplicating or + emptying any deployment slot. + """ + if target_cube_ids.ndim != 1 or source_side_ids.shape != target_cube_ids.shape: + raise ValueError("Target cube and source-side ids must be matching vectors.") + if bool(torch.any((target_cube_ids < 0) | (target_cube_ids >= CUBE_COUNT))): + raise ValueError("Target cube ids are out of range.") + if bool(torch.any((source_side_ids != LEFT_SIDE) & (source_side_ids != RIGHT_SIDE))): + raise ValueError("Source-side ids must be 0 (left) or 1 (right).") + + count = target_cube_ids.numel() + cube_slots = torch.arange(CUBE_COUNT, device=target_cube_ids.device).expand(count, -1).clone() + source_inner_slots = 2 * source_side_ids + displaced_cube_ids = source_inner_slots + target_original_slots = target_cube_ids + cube_slots.scatter_(1, target_cube_ids.unsqueeze(1), source_inner_slots.unsqueeze(1)) + cube_slots.scatter_(1, displaced_cube_ids.unsqueeze(1), target_original_slots.unsqueeze(1)) + + cube_sides = torch.div(cube_slots, 2, rounding_mode="floor") + on_outer_run = torch.remainder(cube_slots, 2).bool() + cube_x = torch.where(on_outer_run, CUBE_OUTER_SLOT_X, CUBE_INNER_SLOT_X) + y_magnitude = torch.where(on_outer_run, BELT_OUTER_STRAIGHT_Y, BELT_INNER_STRAIGHT_Y) + cube_y = torch.where(cube_sides == LEFT_SIDE, y_magnitude, -y_magnitude) + return cube_slots, cube_sides, cube_x, cube_y def _sample_collision_free_active_x( @@ -408,17 +448,11 @@ def __call__( self._robot.write_joint_velocity_to_sim_index(velocity=joint_velocities, env_ids=env_ids) count = env_ids.numel() - base_x = arm_positions.new_tensor((0.26, 0.42, 0.72, 0.88)).expand(count, -1).clone() - base_sides = torch.tensor((LEFT_SIDE, LEFT_SIDE, RIGHT_SIDE, RIGHT_SIDE), device=self.device) - cube_sides = base_sides.expand(count, -1).clone() - cube_sides.scatter_(1, target_cube_ids.unsqueeze(1), source_side_ids.unsqueeze(1)) + cube_slots, cube_sides, base_x, cube_y = _balanced_cube_slots(target_cube_ids, source_side_ids) + base_x = base_x.to(dtype=arm_positions.dtype) + cube_y = cube_y.to(dtype=arm_positions.dtype) if cube_position_noise > 0.0: base_x += (2.0 * torch.rand_like(base_x) - 1.0) * cube_position_noise - cube_y = side_inner_y(cube_sides) - cube_positions = torch.stack( - (base_x, cube_y, torch.full_like(base_x, CUBE_REST_Z)), - dim=2, - ) active_lower = torch.full((count,), TRANSFER_X, dtype=arm_positions.dtype, device=self.device) active_upper = active_lower.clone() @@ -437,6 +471,19 @@ def __call__( active_lower, active_upper, ) + + # On a full deployment reset, mirror the other cube on the source + # conveyor across the racetrack center. Opposite straight runs then + # differ by exactly half a lap even when the active start is sampled. + source_outer_slots = 2 * source_side_ids + 1 + source_outer_cube = cube_slots == source_outer_slots.unsqueeze(1) + mirrored_outer_x = (2.0 * BELT_CENTER_X - active_x).unsqueeze(1) + base_x = torch.where(belt_rows.unsqueeze(1) & source_outer_cube, mirrored_outer_x, base_x) + cube_positions = torch.stack( + (base_x, cube_y, torch.full_like(base_x, CUBE_REST_Z)), + dim=2, + ) + active_positions = torch.stack( (active_x, side_inner_y(source_side_ids), torch.full_like(active_x, CUBE_REST_Z)), dim=1, diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/terminations.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/terminations.py index ab17a7747644..60f34adb9416 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/terminations.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/terminations.py @@ -8,6 +8,7 @@ from __future__ import annotations import math +from collections.abc import Sequence from typing import TYPE_CHECKING import torch @@ -31,6 +32,25 @@ _TRACK_X_CLEARANCE = BELT_TURN_RADIUS + 0.5 * BELT_WIDTH + GUARD_THICKNESS + CUBE_SIZE +def invalid_action( + env: ManagerBasedRLEnv, + action_names: Sequence[str] = ("arm_action", "gripper_action"), +) -> torch.Tensor: + """Terminate environments whose latest policy action contained a non-finite value. + + Args: + env: Manager-based conveyor environment. + action_names: Action terms exposing an ``invalid_actions`` Boolean tensor. + + Returns: + Per-environment invalid-action mask. + """ + if not action_names: + raise ValueError("At least one action term is required for invalid-action termination.") + masks = tuple(env.action_manager.get_term(name).invalid_actions for name in action_names) + return torch.stack(masks, dim=0).any(dim=0) + + def subgoal_time_out( env: ManagerBasedRLEnv, timeout_s: float = 20.0, diff --git a/source/isaaclab_tasks/pyproject.toml b/source/isaaclab_tasks/pyproject.toml index 196ef3981d9d..ab8c5e840e0e 100644 --- a/source/isaaclab_tasks/pyproject.toml +++ b/source/isaaclab_tasks/pyproject.toml @@ -21,12 +21,14 @@ dependencies = [ "isaaclab", "isaaclab_assets", "isaaclab_newton", + "isaaclab_physx", ] [tool.uv.sources] isaaclab = { path = "../isaaclab", editable = true } isaaclab_assets = { path = "../isaaclab_assets", editable = true } isaaclab_newton = { path = "../isaaclab_newton", editable = true } +isaaclab_physx = { path = "../isaaclab_physx", editable = true } [project.urls] Homepage = "https://github.com/isaac-sim/IsaacLab" diff --git a/source/isaaclab_tasks/test/contrib/test_conveyor_force_driver.py b/source/isaaclab_tasks/test/contrib/test_conveyor_force_driver.py new file mode 100644 index 000000000000..33c1d5df4957 --- /dev/null +++ b/source/isaaclab_tasks/test/contrib/test_conveyor_force_driver.py @@ -0,0 +1,335 @@ +# 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 + +"""Lifecycle tests for the Newton conveyor force driver.""" + +from __future__ import annotations + +from types import SimpleNamespace + +import numpy as np +import pytest +import warp as wp + +from isaaclab.physics import ConveyorBeltSpec, PhysicsEvent + +import isaaclab_tasks.contrib.conveyor_franka.conveyor_force_driver as driver_module + + +def _belt_spec(name: str = "Belt") -> ConveyorBeltSpec: + """Build one valid replicated test belt.""" + return ConveyorBeltSpec(prim_path=f"{{ENV_REGEX_NS}}/{name}", velocity=0.35, friction_coefficient=0.5) + + +class _FakeCallbackHandle: + def __init__(self) -> None: + self.deregister_count = 0 + + def deregister(self) -> None: + self.deregister_count += 1 + + +class _FakeBinding: + instances = [] + + def __init__(self, model, contacts, **kwargs) -> None: + self.model = model + self.contacts = contacts + self.kwargs = kwargs + self.closed = False + self._command_velocity_host = np.array([0.35, 0.35], dtype=np.float32) + self._enabled_host = np.ones(2, dtype=np.int32) + self._friction_host = np.full(2, 0.5, dtype=np.float32) + self._threshold_host = np.full(2, 0.997, dtype=np.float32) + type(self).instances.append(self) + + def set_velocities(self, values) -> None: + self._command_velocity_host = np.asarray(values, dtype=np.float32).copy() + + def set_enabled(self, values) -> None: + self._enabled_host = np.asarray(values, dtype=np.int32).copy() + + def set_friction_coefficients(self, values) -> None: + self._friction_host = np.asarray(values, dtype=np.float32).copy() + + def set_contact_processing_thresholds(self, values) -> None: + self._threshold_host = np.asarray(values, dtype=np.float32).copy() + + def close(self) -> None: + self.closed = True + + +def test_driver_requests_force_and_rebinds_on_solver_reinitialization(monkeypatch: pytest.MonkeyPatch) -> None: + """The driver binds pre-capture and replaces model-owned buffers after a hard reset.""" + event_callbacks = [] + solver_callbacks = [] + unregistered_solver_callbacks = [] + requested_attributes = [] + callback_handle = _FakeCallbackHandle() + _FakeBinding.instances = [] + + def register_callback(cls, callback, event, order=0, name=None, wrap_weak_ref=True): + event_callbacks.append((callback, event, name)) + return callback_handle + + monkeypatch.setattr(driver_module.NewtonManager, "register_callback", classmethod(register_callback)) + monkeypatch.setattr( + driver_module.NewtonManager, + "register_solver_init_callback", + classmethod(lambda cls, callback: solver_callbacks.append(callback)), + ) + monkeypatch.setattr( + driver_module.NewtonManager, + "unregister_solver_init_callback", + classmethod(lambda cls, callback: unregistered_solver_callbacks.append(callback)), + ) + monkeypatch.setattr( + driver_module.NewtonManager, + "request_extended_contact_attribute", + classmethod(lambda cls, attribute: requested_attributes.append(attribute)), + ) + monkeypatch.setattr(driver_module, "_ConveyorForceBinding", _FakeBinding) + + driver = driver_module.ConveyorForceDriver(num_envs=2, belt_specs=(_belt_spec(),)) + + assert driver.specs == (_belt_spec(),) + assert driver.belts_per_env == 1 + assert driver.num_belts == 2 + assert driver.count == 2 + assert not driver.initialized + + assert [(event, name) for _, event, name in event_callbacks] == [ + (PhysicsEvent.MODEL_INIT, "conveyor_force_contact_attribute") + ] + event_callbacks[0][0](None) + assert requested_attributes == ["force"] + + first_model, first_contacts = object(), object() + solver_callbacks[0](first_model, first_contacts) + first_binding = _FakeBinding.instances[-1] + assert driver.initialized + first_binding.set_velocities([0.2, -0.1]) + first_binding.set_enabled([1, 0]) + first_binding.set_friction_coefficients([0.4, 0.6]) + first_binding.set_contact_processing_thresholds([0.98, 0.99]) + + second_model, second_contacts = object(), object() + solver_callbacks[0](second_model, second_contacts) + second_binding = _FakeBinding.instances[-1] + + assert first_binding.closed + assert second_binding.model is second_model + assert second_binding.contacts is second_contacts + np.testing.assert_allclose(second_binding._command_velocity_host, [0.2, -0.1]) + np.testing.assert_array_equal(second_binding._enabled_host, [1, 0]) + np.testing.assert_allclose(second_binding._friction_host, [0.4, 0.6]) + np.testing.assert_allclose(second_binding._threshold_host, [0.98, 0.99]) + + driver.close() + driver.close() + assert second_binding.closed + assert unregistered_solver_callbacks == [solver_callbacks[0]] + assert callback_handle.deregister_count == 1 + + +def test_unbound_driver_rejects_control_calls(monkeypatch: pytest.MonkeyPatch) -> None: + """Control methods are unavailable until the solver-init callback creates a binding.""" + callback_handle = _FakeCallbackHandle() + monkeypatch.setattr( + driver_module.NewtonManager, + "register_callback", + classmethod(lambda cls, *args, **kwargs: callback_handle), + ) + monkeypatch.setattr( + driver_module.NewtonManager, + "register_solver_init_callback", + classmethod(lambda cls, callback: None), + ) + monkeypatch.setattr( + driver_module.NewtonManager, + "unregister_solver_init_callback", + classmethod(lambda cls, callback: None), + ) + + driver = driver_module.ConveyorForceDriver(num_envs=1, belt_specs=(_belt_spec(),)) + with pytest.raises(RuntimeError, match="not bound"): + driver.set_velocities(0.2) + driver.close() + + +def test_driver_rejects_invalid_specs_before_registering_callbacks(monkeypatch: pytest.MonkeyPatch) -> None: + """Invalid descriptions cannot leave lifecycle callbacks behind.""" + registered = [] + monkeypatch.setattr( + driver_module.NewtonManager, + "register_callback", + classmethod(lambda cls, *args, **kwargs: registered.append(args)), + ) + + with pytest.raises(ValueError, match="At least one"): + driver_module.ConveyorForceDriver(num_envs=1, belt_specs=()) + with pytest.raises(TypeError, match="ConveyorBeltSpec"): + driver_module.ConveyorForceDriver(num_envs=1, belt_specs=(object(),)) + with pytest.raises(ValueError, match="unique"): + driver_module.ConveyorForceDriver(num_envs=1, belt_specs=(_belt_spec(), _belt_spec())) + with pytest.raises(ValueError, match="ancestors"): + driver_module.ConveyorForceDriver( + num_envs=1, + belt_specs=( + ConveyorBeltSpec(prim_path="{ENV_REGEX_NS}/Belt"), + ConveyorBeltSpec(prim_path="{ENV_REGEX_NS}/Belt/Child"), + ), + ) + with pytest.raises(ValueError, match="explicit positive radius"): + driver_module.ConveyorForceDriver( + num_envs=1, + belt_specs=(ConveyorBeltSpec(prim_path="{ENV_REGEX_NS}/Curve", curved=True),), + ) + with pytest.raises(ValueError, match="Replicated conveyor environments"): + driver_module.ConveyorForceDriver( + num_envs=2, + belt_specs=(ConveyorBeltSpec(prim_path="/World/Shared/Belt"),), + ) + with pytest.raises(ValueError, match="env_path_format"): + driver_module.ConveyorForceDriver(num_envs=1, belt_specs=(_belt_spec(),), env_path_format="/World/envs/env_.*") + + assert registered == [] + + +def test_belt_paths_are_exact_and_environment_scoped() -> None: + """A descriptor cannot bind a same-named shape outside the replicated environment root.""" + resolve = driver_module._resolve_belt_prim_path + + assert resolve("{ENV_REGEX_NS}/Belt", "/World/envs/env_{}", 0) == "/World/envs/env_0/Belt" + assert resolve("{ENV_REGEX_NS}/Nested/Belt", "/World/envs/env_{}", 123) == ("/World/envs/env_123/Nested/Belt") + assert resolve("/World/Shared/Belt", "/World/envs/env_{}", 7) == "/World/Shared/Belt" + belongs = driver_module._shape_belongs_to_prim + assert belongs("/World/envs/env_0/Belt/geometry/mesh", "/World/envs/env_0/Belt") + assert not belongs("/World/props/Belt/geometry/mesh", "/World/envs/env_0/Belt") + assert not belongs("/World/envs/env_0/Nested/Belt", "/World/envs/env_0/Belt") + + +def test_driver_cleans_up_model_callback_when_solver_registration_fails(monkeypatch: pytest.MonkeyPatch) -> None: + """A lifecycle registration failure cannot leave a partially active driver.""" + callback_handle = _FakeCallbackHandle() + monkeypatch.setattr( + driver_module.NewtonManager, + "register_callback", + classmethod(lambda cls, *args, **kwargs: callback_handle), + ) + + def fail_registration(cls, callback): + raise RuntimeError("solver callback unavailable") + + monkeypatch.setattr( + driver_module.NewtonManager, + "register_solver_init_callback", + classmethod(fail_registration), + ) + + with pytest.raises(RuntimeError, match="solver callback unavailable"): + driver_module.ConveyorForceDriver(num_envs=1, belt_specs=(_belt_spec(),)) + + assert callback_handle.deregister_count == 1 + + +def test_binding_uses_deterministic_environment_major_belt_indices(monkeypatch: pytest.MonkeyPatch) -> None: + """Newton discovery order cannot reorder commands or encoder rows after a rebuild.""" + monkeypatch.setattr( + driver_module.NewtonManager, + "register_state_force_callback", + classmethod(lambda cls, callback: None), + ) + monkeypatch.setattr( + driver_module.NewtonManager, + "register_post_solver_substep_callback", + classmethod(lambda cls, callback: None), + ) + monkeypatch.setattr( + driver_module.NewtonManager, + "unregister_state_force_callback", + classmethod(lambda cls, callback: None), + ) + monkeypatch.setattr( + driver_module.NewtonManager, + "unregister_post_solver_substep_callback", + classmethod(lambda cls, callback: None), + ) + + shape_labels = ( + "/World/envs/env_1/BeltB", + "/World/envs/env_0/BeltA", + "/World/envs/env_1/BeltA", + "/World/envs/env_0/BeltB", + ) + shape_count = len(shape_labels) + identity = wp.transform(wp.vec3(), wp.quat_identity()) + model = SimpleNamespace( + world_count=2, + device="cpu", + shape_count=shape_count, + shape_label=shape_labels, + shape_body=wp.full(shape_count, -1, dtype=wp.int32, device="cpu"), + shape_world=wp.array([1, 0, 1, 0], dtype=wp.int32, device="cpu"), + shape_transform=wp.array([identity] * shape_count, dtype=wp.transform, device="cpu"), + body_count=2, + body_label=("/World/envs/env_0/Cube0", "/World/envs/env_1/Cube0"), + body_world=wp.array([0, 1], dtype=wp.int32, device="cpu"), + body_com=wp.zeros(2, dtype=wp.vec3, device="cpu"), + body_inv_mass=wp.ones(2, dtype=wp.float32, device="cpu"), + body_inv_inertia=wp.array([wp.mat33(1.0)] * 2, dtype=wp.mat33, device="cpu"), + ) + contact_capacity = 4 + contacts = SimpleNamespace( + rigid_contact_max=contact_capacity, + force=wp.zeros(contact_capacity, dtype=wp.spatial_vector, device="cpu"), + rigid_contact_shape0=wp.full(contact_capacity, -1, dtype=wp.int32, device="cpu"), + rigid_contact_shape1=wp.full(contact_capacity, -1, dtype=wp.int32, device="cpu"), + rigid_contact_normal=wp.zeros(contact_capacity, dtype=wp.vec3, device="cpu"), + rigid_contact_point0=wp.zeros(contact_capacity, dtype=wp.vec3, device="cpu"), + rigid_contact_point1=wp.zeros(contact_capacity, dtype=wp.vec3, device="cpu"), + rigid_contact_count=wp.zeros(1, dtype=wp.int32, device="cpu"), + ) + specs = ( + ConveyorBeltSpec( + prim_path="{ENV_REGEX_NS}/BeltA", + velocity=0.1, + friction_coefficient=0.4, + contact_threshold=0.98, + ), + ConveyorBeltSpec( + prim_path="{ENV_REGEX_NS}/BeltB", + velocity=-0.2, + enabled=False, + friction_coefficient=0.6, + contact_threshold=0.99, + ), + ) + + binding = driver_module._ConveyorForceBinding( + model=model, + contacts=contacts, + num_envs=2, + belt_specs=specs, + transported_body_count_per_env=1, + ) + try: + assert binding.surface_paths == ( + "/World/envs/env_0/BeltA", + "/World/envs/env_0/BeltB", + "/World/envs/env_1/BeltA", + "/World/envs/env_1/BeltB", + ) + np.testing.assert_array_equal(binding._shape_conveyor.numpy(), [3, 0, 2, 1]) + np.testing.assert_array_equal(binding._conveyor_world.numpy(), [0, 0, 1, 1]) + np.testing.assert_allclose(binding._command_velocity_host, [0.1, -0.2, 0.1, -0.2]) + np.testing.assert_array_equal(binding._enabled_host, [1, 0, 1, 0]) + np.testing.assert_allclose(binding._friction_host, [0.4, 0.6, 0.4, 0.6]) + np.testing.assert_allclose(binding._threshold_host, [0.98, 0.99, 0.98, 0.99]) + np.testing.assert_array_equal(binding.get_enabled(indices=[3, 0]).numpy(), [0, 1]) + np.testing.assert_allclose(binding.get_friction_coefficients(indices=[1, 2]).numpy(), [0.6, 0.4]) + np.testing.assert_allclose(binding.get_contact_processing_thresholds(indices=[2, 1]).numpy(), [0.98, 0.99]) + finally: + binding.close() diff --git a/source/isaaclab_tasks/test/contrib/test_conveyor_franka_geometry.py b/source/isaaclab_tasks/test/contrib/test_conveyor_franka_geometry.py index 1d2aab67d7fc..6b319bfe7ccb 100644 --- a/source/isaaclab_tasks/test/contrib/test_conveyor_franka_geometry.py +++ b/source/isaaclab_tasks/test/contrib/test_conveyor_franka_geometry.py @@ -7,10 +7,13 @@ from collections import Counter +from isaaclab_tasks.contrib.conveyor_franka.conveyor_franka_env_cfg import _collision_properties, _cube from isaaclab_tasks.contrib.conveyor_franka.conveyor_geometry import ( BELT_TOP_Z, + BELT_TURN_RADIUS, TURN_SEGMENT_COUNT, MeshSpec, + belt_collision_section_specs, belt_mesh_spec, guard_mesh_specs, ) @@ -48,3 +51,30 @@ def test_belt_top_faces_point_upward(): a, b, c = vertices cross_z = (b[0] - a[0]) * (c[1] - a[1]) - (b[1] - a[1]) * (c[0] - a[0]) assert cross_z > 0.0 + + +def test_contact_configuration_uses_one_mujoco_parameterization(): + """Raw MuJoCo solref must not be combined with shadowed Newton force-space gains.""" + mujoco_cfg = _collision_properties()[-1] + cube_material = _cube("TestCube", (1.0, 0.0, 0.0), (0.0, 0.0, 0.0)).spawn.physics_material + + assert mujoco_cfg.solref is not None + assert cube_material.contact_stiffness is None + assert cube_material.contact_damping is None + assert cube_material.torsional_friction is None + assert cube_material.rolling_friction is None + + +def test_collision_sections_carry_schema_aligned_belt_intent(): + """Task geometry and runtime descriptions share paths, units, and curve semantics.""" + sections = belt_collision_section_specs("Left", velocity=0.35, friction_coefficient=0.5, contact_threshold=0.997) + + assert len(sections) == 4 + assert tuple(section.belt.prim_path for section in sections) == tuple( + f"{{ENV_REGEX_NS}}/{section.geometry.name}" for section in sections + ) + assert tuple(section.belt.velocity for section in sections) == (0.35,) * 4 + assert tuple(section.belt.friction_coefficient for section in sections) == (0.5,) * 4 + assert tuple(section.belt.contact_threshold for section in sections) == (0.997,) * 4 + assert tuple(section.belt.curved for section in sections) == (False, False, True, True) + assert tuple(section.belt.radius for section in sections) == (None, None, BELT_TURN_RADIUS, BELT_TURN_RADIUS) diff --git a/source/isaaclab_tasks/test/contrib/test_conveyor_franka_mdp.py b/source/isaaclab_tasks/test/contrib/test_conveyor_franka_mdp.py index 2222f70a93ff..91fd7cd80d27 100644 --- a/source/isaaclab_tasks/test/contrib/test_conveyor_franka_mdp.py +++ b/source/isaaclab_tasks/test/contrib/test_conveyor_franka_mdp.py @@ -15,6 +15,15 @@ ConveyorGaussianBernoulliDistribution, ) from isaaclab_tasks.contrib.conveyor_franka.conveyor_franka_env_cfg import ConveyorFrankaEnvCfg +from isaaclab_tasks.contrib.conveyor_franka.conveyor_geometry import ( + BELT_CENTER_X, + BELT_INNER_STRAIGHT_Y, + BELT_OUTER_STRAIGHT_Y, +) +from isaaclab_tasks.contrib.conveyor_franka.mdp.actions import ( + ConveyorRelativeJointPositionAction, + ResetBufferedGripperAction, +) from isaaclab_tasks.contrib.conveyor_franka.mdp.commands import ConveyorTransferCommand, transfer_success_mask from isaaclab_tasks.contrib.conveyor_franka.mdp.curriculums import ( deployment_probability_from_progress, @@ -24,6 +33,7 @@ from isaaclab_tasks.contrib.conveyor_franka.mdp.reset_events import ( CUBE_COUNT, ConveyorResetRecipe, + _balanced_cube_slots, _sample_collision_free_active_x, build_reset_rows, franka_tool_position, @@ -31,7 +41,128 @@ select_next_transfer_cube, ) from isaaclab_tasks.contrib.conveyor_franka.mdp.rewards import transfer_potential -from isaaclab_tasks.contrib.conveyor_franka.mdp.terminations import subgoal_time_out, transfer_sequence_time_out +from isaaclab_tasks.contrib.conveyor_franka.mdp.terminations import ( + invalid_action, + subgoal_time_out, + transfer_sequence_time_out, +) + + +def _make_arm_action_term() -> ConveyorRelativeJointPositionAction: + """Build the tensor-only portion of the arm action term without a simulator.""" + workspace_lower = torch.tensor((-0.75, -0.45, -0.55, -2.75, -0.45, 1.85, -0.10)) + workspace_upper = torch.tensor((0.85, 0.85, 0.35, -1.75, 0.45, 3.05, 1.65)) + positions = ((workspace_lower + workspace_upper) * 0.5).repeat(2, 1) + limits = torch.tensor((-4.0, 4.0)).repeat(2, 7, 1) + action = object.__new__(ConveyorRelativeJointPositionAction) + action.cfg = SimpleNamespace(max_delta=0.12, joint_limit_margin=0.02, clip=None) + action._asset = SimpleNamespace( + data=SimpleNamespace( + joint_pos=SimpleNamespace(torch=positions), + soft_joint_pos_limits=SimpleNamespace(torch=limits), + ) + ) + action._joint_ids = slice(None) + action._scale = 0.12 + action._offset = 0.0 + action._workspace_lower = workspace_lower + action._workspace_upper = workspace_upper + action._raw_actions = torch.zeros((2, 7)) + action._processed_actions = torch.zeros((2, 7)) + action._position_targets = positions.clone() + action._invalid_actions = torch.zeros(2, dtype=torch.bool) + return action + + +def _make_gripper_action_term() -> ResetBufferedGripperAction: + """Build the tensor-only portion of the binary gripper action term.""" + action = object.__new__(ResetBufferedGripperAction) + action.cfg = SimpleNamespace(clip=None, command_name="transfer", force_close_steps=2) + action._raw_actions = torch.zeros((2, 1)) + action._processed_actions = torch.zeros((2, 2)) + action._open_command = torch.full((2,), 0.04) + action._close_command = torch.zeros(2) + action._invalid_actions = torch.zeros(2, dtype=torch.bool) + command = SimpleNamespace(held_cube_ids=torch.full((2,), -1, dtype=torch.long)) + action._env = SimpleNamespace( + command_manager=SimpleNamespace(get_term=lambda _name: command), + episode_length_buf=torch.zeros(2, dtype=torch.long), + ) + return action + + +def test_arm_action_sanitizes_nonfinite_values_and_clamps_normalized_input(): + """Invalid policy outputs cannot reach joint targets or exceed one normalized unit.""" + action = _make_arm_action_term() + policy_actions = torch.tensor(((float("nan"), float("inf"), -float("inf"), 5.0, -5.0, 0.5, -0.5), (-2.0,) * 7)) + + action.process_actions(policy_actions) + + expected = torch.tensor(((0.0, 1.0, -1.0, 1.0, -1.0, 0.5, -0.5), (-1.0,) * 7)) + torch.testing.assert_close(action.raw_actions, expected) + assert action.invalid_actions.tolist() == [True, False] + assert torch.isfinite(action.processed_actions).all() + expected_targets = action._asset.data.joint_pos.torch + expected * 0.12 + torch.testing.assert_close(action.processed_actions, expected_targets) + + +def test_gripper_action_sanitizes_nonfinite_values_before_binary_mapping(): + """The eighth policy dimension cannot silently turn a NaN into an open command.""" + action = _make_gripper_action_term() + + action.process_actions(torch.tensor(((float("nan"),), (float("inf"),)))) + + torch.testing.assert_close(action.raw_actions, torch.tensor(((-1.0,), (1.0,)))) + assert action.invalid_actions.tolist() == [True, True] + torch.testing.assert_close(action.processed_actions[0], action._close_command) + torch.testing.assert_close(action.processed_actions[1], action._open_command) + + +def test_invalid_action_termination_and_reset_are_per_environment(): + """Arm and gripper failures aggregate per environment, and reset clears the arm flag.""" + arm_action = _make_arm_action_term() + gripper_action = _make_gripper_action_term() + arm_action._invalid_actions[:] = torch.tensor((True, False)) + gripper_action._invalid_actions[:] = torch.tensor((False, True)) + actions = {"arm_action": arm_action, "gripper_action": gripper_action} + env = SimpleNamespace(action_manager=SimpleNamespace(get_term=actions.__getitem__)) + + assert invalid_action(env).tolist() == [True, True] + arm_action.reset([0]) + gripper_action._invalid_actions[1] = False + + assert invalid_action(env).tolist() == [False, False] + + +def test_final_config_validation_catches_overridden_arm_contracts(): + """Top-level validation runs after overrides and protects workspace-to-joint alignment.""" + cfg = ConveyorFrankaEnvCfg() + assert cfg.seed is None + cfg.validate() + + cfg.actions.arm_action.preserve_order = False + try: + cfg.validate() + except ValueError as exc: + assert "preserve" in str(exc) + else: + raise AssertionError("Expected invalid arm ordering to fail configuration validation.") + + +def test_production_solver_and_viewer_defaults_are_bounded(): + """The task keeps CUDA graphs enabled and avoids scene-wide over-allocation or rendering.""" + cfg = ConveyorFrankaEnvCfg() + physics = cfg.sim.physics + solver = physics.solver_cfg + + assert cfg.conveyor_force.speed == 0.35 + assert physics.use_cuda_graph is True + assert physics.load_visual_shapes is None + assert solver.njmax == 300 + assert solver.nconmax == 200 + assert solver.impratio == 1.0 + assert cfg.sim.default_visualizer_cfg.max_visible_envs == 1 + assert cfg.sim.default_visualizer_cfg.randomly_sample_visible_envs is False def test_reset_rows_cover_every_cube_direction_and_phase_once(): @@ -360,3 +491,27 @@ def test_active_cube_sampling_avoids_inactive_source_lane_cubes(): inactive_on_source = (cube_sides == source_side_ids.unsqueeze(1)) & (cube_ids != target_cube_ids.unsqueeze(1)) separation = torch.abs(sampled.unsqueeze(1) - base_x) assert torch.all(separation[inactive_on_source] >= 0.055) + + +def test_deployment_layout_uses_each_racetrack_straight_run_once(): + """Every command keeps its target reachable and distributes all four cubes evenly.""" + target_cube_ids = torch.arange(CUBE_COUNT).repeat_interleave(2) + source_side_ids = torch.arange(2).repeat(CUBE_COUNT) + + slots, cube_sides, cube_x, cube_y = _balanced_cube_slots(target_cube_ids, source_side_ids) + + expected_slots = torch.arange(CUBE_COUNT).expand(target_cube_ids.numel(), -1) + torch.testing.assert_close(torch.sort(slots, dim=1).values, expected_slots) + active_slots = slots.gather(1, target_cube_ids.unsqueeze(1)).squeeze(1) + torch.testing.assert_close(active_slots, 2 * source_side_ids) + assert torch.all(torch.sum(cube_sides == 0, dim=1) == 2) + assert torch.all(torch.sum(cube_sides == 1, dim=1) == 2) + + expected_y = torch.tensor( + (-BELT_OUTER_STRAIGHT_Y, -BELT_INNER_STRAIGHT_Y, BELT_INNER_STRAIGHT_Y, BELT_OUTER_STRAIGHT_Y) + ) + torch.testing.assert_close(torch.sort(cube_y, dim=1).values, expected_y.expand_as(cube_y)) + for side_id in (0, 1): + side_x = torch.where(cube_sides == side_id, cube_x, torch.nan) + expected_sum = torch.full((source_side_ids.numel(),), 2 * BELT_CENTER_X, dtype=cube_x.dtype) + torch.testing.assert_close(torch.nansum(side_x, dim=1), expected_sum) diff --git a/source/isaaclab_tasks/test/contrib/test_conveyor_franka_physx_cfg.py b/source/isaaclab_tasks/test/contrib/test_conveyor_franka_physx_cfg.py new file mode 100644 index 000000000000..aa3faaaea286 --- /dev/null +++ b/source/isaaclab_tasks/test/contrib/test_conveyor_franka_physx_cfg.py @@ -0,0 +1,99 @@ +# 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 checks for the checkpoint-compatible PhysX conveyor configuration.""" + +import gymnasium as gym +import pytest + +from isaaclab_physx.physics import PhysxCfg +from isaaclab_physx.sim.spawners.materials import PhysxRigidBodyMaterialCfg + +import isaaclab_tasks # noqa: F401 +from isaaclab_tasks.contrib.conveyor_franka.conveyor_franka_env_cfg import ConveyorFrankaEnvCfg +from isaaclab_tasks.contrib.conveyor_franka.conveyor_franka_physx_env_cfg import ( + ConveyorFrankaPhysxEnvCfg, + physx_belt_section_specs, +) +from isaaclab_tasks.contrib.conveyor_franka.conveyor_geometry import BELT_TURN_RADIUS, MeshSpec + + +def test_physx_task_is_registered_with_a_dedicated_config() -> None: + """The native backend is opt-in and cannot alter the Newton task registration.""" + physx_spec = gym.spec("IsaacContrib-Conveyor-Franka-PhysX-CPU-v0") + newton_spec = gym.spec("IsaacContrib-Conveyor-Franka-Newton-v0") + + assert physx_spec.entry_point == newton_spec.entry_point + assert physx_spec.kwargs["env_cfg_entry_point"].endswith(":ConveyorFrankaPhysxEnvCfg") + assert newton_spec.kwargs["env_cfg_entry_point"].endswith(":ConveyorFrankaEnvCfg") + + +def test_physx_config_preserves_policy_and_timing_contracts() -> None: + """A Newton checkpoint sees the same ordered 8-D action and 60 Hz policy interface.""" + newton_cfg = ConveyorFrankaEnvCfg() + physx_cfg = ConveyorFrankaPhysxEnvCfg() + + newton_cfg.validate() + physx_cfg.validate() + assert isinstance(physx_cfg.sim.physics, PhysxCfg) + assert physx_cfg.sim.device == "cpu" + assert physx_cfg.scene.num_envs == 1 + assert physx_cfg.sim.dt == newton_cfg.sim.dt == 1.0 / 120.0 + assert physx_cfg.decimation == newton_cfg.decimation == 2 + assert physx_cfg.actions.arm_action.joint_names == newton_cfg.actions.arm_action.joint_names + assert physx_cfg.actions.gripper_action.joint_names == newton_cfg.actions.gripper_action.joint_names + assert physx_cfg.conveyor_force.speed == newton_cfg.conveyor_force.speed == 0.35 + assert physx_cfg.scene.robot.spawn.joint_drive_props is None + assert physx_cfg.scene.robot.spawn.rigid_props.disable_gravity is True + + +@pytest.mark.parametrize("device", ["cuda", "cuda:0", "cuda:1"]) +def test_physx_config_rejects_broken_gpu_surface_velocity_contacts(device: str) -> None: + """The pinned Isaac Sim GPU path must not silently let cubes tunnel through belts.""" + cfg = ConveyorFrankaPhysxEnvCfg() + cfg.sim.device = device + + with pytest.raises(ValueError, match="CPU-only.*--device cpu"): + cfg.validate() + + +def test_curved_physx_sections_are_pivot_local_watertight_sdfs() -> None: + """Turn roots sit at their pivots so native angular velocity has the intended center.""" + cfg = ConveyorFrankaPhysxEnvCfg() + sections = physx_belt_section_specs("Left", velocity=0.35) + + assert len(sections) == 4 + for section, root_position in sections[2:]: + assert isinstance(section.geometry, MeshSpec) + assert section.belt.curved + assert section.belt.radius == BELT_TURN_RADIUS + assert section.belt.pivot_point == (0.0, 0.0, 0.0) + assert root_position[2] == 0.0 + + turn_asset = cfg.scene.conveyor_left_right_turn_collision + assert turn_asset.spawn.collision_approximation == "sdf" + assert isinstance(turn_asset.spawn.physics_material, PhysxRigidBodyMaterialCfg) + assert turn_asset.spawn.physics_material.dynamic_friction == 0.5 + assert turn_asset.spawn.rigid_props.kinematic_enabled is True + + +def test_runtime_specs_and_material_override_have_one_source_of_truth() -> None: + """Final overrides reach both the runtime view and native PhysX material authoring.""" + cfg = ConveyorFrankaPhysxEnvCfg() + cfg.scene.configure_conveyor(friction_coefficient=0.42) + specs = cfg.scene.build_conveyor_belt_specs( + velocity=0.21, + friction_coefficient=0.42, + contact_threshold=0.95, + ) + + assert len(specs) == 8 + assert {spec.velocity for spec in specs} == {0.21} + assert {spec.friction_coefficient for spec in specs} == {0.42} + assert {spec.contact_threshold for spec in specs} == {0.95} + for side in ("left", "right"): + for section_key in ("top_straight", "bottom_straight", "right_turn", "left_turn"): + asset = getattr(cfg.scene, f"conveyor_{side}_{section_key}_collision") + assert asset.spawn.physics_material.dynamic_friction == 0.42 diff --git a/source/isaaclab_tasks/test/contrib/test_conveyor_physx_surface.py b/source/isaaclab_tasks/test/contrib/test_conveyor_physx_surface.py new file mode 100644 index 000000000000..52742801ba61 --- /dev/null +++ b/source/isaaclab_tasks/test/contrib/test_conveyor_physx_surface.py @@ -0,0 +1,247 @@ +# 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 the task-local PhysX conveyor surface backend.""" + +from __future__ import annotations + +import math +import sys +from types import ModuleType + +import numpy as np +import pytest +import torch + +from isaaclab.physics import ConveyorBeltSpec + +import isaaclab_tasks.contrib.conveyor_franka.conveyor_physx_surface as surface_module + + +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 _belt_spec(name: str = "Belt", **kwargs) -> ConveyorBeltSpec: + """Return one replicated belt description for facade tests.""" + return ConveyorBeltSpec(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 = _belt_spec(direction=(3.0, 4.0, 0.0)) + + twist = surface_module.compute_physx_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 = _belt_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_physx_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) + omega_rad = np.radians(twist.angular_velocity_deg) + point = np.asarray((3.0, 0.0, 0.0)) + actual_at_point = np.asarray(twist.linear_velocity) + np.cross(omega_rad, point) + expected_at_point = np.cross(omega_rad, point - np.asarray(spec.pivot_point)) + np.testing.assert_allclose(actual_at_point, expected_at_point) + + +def test_curved_twist_requires_an_explicit_radius() -> None: + """A native angular rate cannot be inferred from unspecified task geometry.""" + spec = _belt_spec(curved=True) + + with pytest.raises(ValueError, match="positive radius"): + surface_module.compute_physx_surface_velocity_twist(spec) + + +def test_paths_are_resolved_in_environment_major_order() -> None: + """Runtime rows stay deterministic across stage discovery ordering.""" + specs = (_belt_spec("BeltA"), _belt_spec("Nested/BeltB")) + + paths = surface_module.resolve_physx_conveyor_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_physx_conveyor_paths(2, (ConveyorBeltSpec(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.PhysxSurfaceVelocityConveyor(2, (_belt_spec(),), writer=writer) + + assert facade.prim_paths == ("/World/envs/env_0/Belt", "/World/envs/env_1/Belt") + assert facade.num_belts == 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(0.8, indices=[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=[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_facade_rejects_unrepresentable_runtime_mutations() -> None: + """PhysX metadata getters cannot imply that unsupported setters took effect.""" + facade = surface_module.PhysxSurfaceVelocityConveyor( + 1, + (_belt_spec(friction_coefficient=0.55, contact_threshold=0.98),), + writer=_FakeWriter(), + ) + + np.testing.assert_allclose(facade.get_friction_coefficients(), (0.55,)) + np.testing.assert_allclose(facade.get_contact_processing_thresholds(), (0.98,)) + with pytest.raises(NotImplementedError, match="material friction"): + facade.set_friction_coefficients(0.8) + with pytest.raises(NotImplementedError, match="no contact-normal threshold"): + facade.set_contact_processing_thresholds(0.9) + facade.close() + + +def test_facade_accepts_torch_control_and_reset_indices() -> None: + """Normal Isaac Lab tensor selectors are copied to host before NumPy validation.""" + facade = surface_module.PhysxSurfaceVelocityConveyor(2, (_belt_spec(),), writer=_FakeWriter()) + device = "cuda" if torch.cuda.is_available() else "cpu" + + facade.set_velocities(torch.tensor([0.6], device=device), indices=torch.tensor([1], device=device)) + facade.update(0.25) + facade.reset(env_ids=torch.tensor([1], device=device)) + + np.testing.assert_allclose(facade.get_commanded_velocities(), (0.4, 0.6)) + np.testing.assert_allclose(facade.get_encoder_positions(), (0.1, 0.0)) + facade.close() + + +def test_authoring_helper_applies_kinematic_local_surface_schema(monkeypatch: pytest.MonkeyPatch) -> None: + """The lazy authoring seam applies both schemas and authors an initially stopped belt.""" + + class FakeAttribute: + def __init__(self) -> None: + self.value = None + + def Set(self, value) -> bool: + self.value = value + return True + + def Get(self): + return self.value + + class FakePrim: + def __init__(self) -> None: + self.apis = set() + self.attributes = {} + + def IsValid(self) -> bool: + return True + + def HasAPI(self, api_type) -> bool: + return api_type in self.apis + + def GetPath(self) -> str: + return "/World/Belt" + + class FakeRigidBodyAPI: + def __init__(self, prim: FakePrim) -> None: + self.prim = prim + + @classmethod + def Apply(cls, prim: FakePrim): + prim.apis.add(cls) + return cls(prim) + + def CreateRigidBodyEnabledAttr(self) -> FakeAttribute: + return self.prim.attributes.setdefault("rigid_enabled", FakeAttribute()) + + def CreateKinematicEnabledAttr(self) -> FakeAttribute: + return self.prim.attributes.setdefault("kinematic", FakeAttribute()) + + def GetKinematicEnabledAttr(self) -> FakeAttribute: + return self.prim.attributes.setdefault("kinematic", FakeAttribute()) + + class FakeSurfaceAPI: + def __init__(self, prim: FakePrim) -> None: + self.prim = prim + + @classmethod + def Apply(cls, prim: FakePrim): + prim.apis.add(cls) + return cls(prim) + + def _attribute(self, name: str) -> FakeAttribute: + return self.prim.attributes.setdefault(name, FakeAttribute()) + + def CreateSurfaceVelocityLocalSpaceAttr(self) -> FakeAttribute: + return self._attribute("local_space") + + def CreateSurfaceVelocityEnabledAttr(self) -> FakeAttribute: + return self._attribute("surface_enabled") + + def CreateSurfaceVelocityAttr(self) -> FakeAttribute: + return self._attribute("linear") + + def CreateSurfaceAngularVelocityAttr(self) -> FakeAttribute: + return self._attribute("angular") + + fake_pxr = ModuleType("pxr") + fake_pxr.Gf = type("FakeGf", (), {"Vec3f": staticmethod(lambda *values: tuple(values))}) + fake_pxr.PhysxSchema = type("FakePhysxSchema", (), {"PhysxSurfaceVelocityAPI": FakeSurfaceAPI}) + fake_pxr.UsdPhysics = type("FakeUsdPhysics", (), {"RigidBodyAPI": FakeRigidBodyAPI}) + monkeypatch.setitem(sys.modules, "pxr", fake_pxr) + prim = FakePrim() + + surface_module.apply_physx_surface_velocity_api(prim, _belt_spec(), velocity_scale=0.0) + + assert FakeRigidBodyAPI in prim.apis + assert FakeSurfaceAPI in prim.apis + assert prim.attributes["rigid_enabled"].value is True + assert prim.attributes["kinematic"].value is True + assert prim.attributes["local_space"].value is True + assert prim.attributes["surface_enabled"].value is True + assert prim.attributes["linear"].value == (0.0, 0.0, 0.0) + assert prim.attributes["angular"].value == (0.0, 0.0, 0.0) diff --git a/uv.lock b/uv.lock index 7607bb795ac3..f056c0df5f59 100644 --- a/uv.lock +++ b/uv.lock @@ -1764,7 +1764,7 @@ wheels = [ [[package]] name = "isaaclab" -version = "16.0.1" +version = "16.1.0" source = { editable = "source/isaaclab" } [[package]] @@ -1784,7 +1784,7 @@ requires-dist = [ [[package]] name = "isaaclab-contrib" -version = "1.3.0" +version = "1.3.1" source = { editable = "source/isaaclab_contrib" } dependencies = [ { name = "isaaclab", marker = "(platform_machine == 'aarch64' and sys_platform == 'linux') or (platform_machine == 'x86_64' and sys_platform == 'linux') or (platform_machine == 'AMD64' and sys_platform == 'win32')" }, @@ -2119,7 +2119,7 @@ provides-extras = ["tetrahedralization", "video", "test", "sb3", "skrl", "rl-gam [[package]] name = "isaaclab-experimental" -version = "0.1.5" +version = "0.2.0" source = { editable = "source/isaaclab_experimental" } dependencies = [ { name = "isaaclab", marker = "(platform_machine == 'aarch64' and sys_platform == 'linux') or (platform_machine == 'x86_64' and sys_platform == 'linux') or (platform_machine == 'AMD64' and sys_platform == 'win32')" }, @@ -2147,7 +2147,7 @@ requires-dist = [ [[package]] name = "isaaclab-newton" -version = "4.0.0" +version = "5.0.0" source = { editable = "source/isaaclab_newton" } dependencies = [ { name = "isaaclab", marker = "(platform_machine == 'aarch64' and sys_platform == 'linux') or (platform_machine == 'x86_64' and sys_platform == 'linux') or (platform_machine == 'AMD64' and sys_platform == 'win32')" }, @@ -2158,7 +2158,7 @@ requires-dist = [{ name = "isaaclab", editable = "source/isaaclab" }] [[package]] name = "isaaclab-ov" -version = "2.0.0" +version = "2.0.1" source = { editable = "source/isaaclab_ov" } dependencies = [ { name = "isaaclab", marker = "(platform_machine == 'aarch64' and sys_platform == 'linux') or (platform_machine == 'x86_64' and sys_platform == 'linux') or (platform_machine == 'AMD64' and sys_platform == 'win32')" }, @@ -2212,12 +2212,13 @@ requires-dist = [ [[package]] name = "isaaclab-tasks" -version = "16.0.0" +version = "16.1.0" source = { editable = "source/isaaclab_tasks" } dependencies = [ { name = "isaaclab", marker = "(platform_machine == 'aarch64' and sys_platform == 'linux') or (platform_machine == 'x86_64' and sys_platform == 'linux') or (platform_machine == 'AMD64' and sys_platform == 'win32')" }, { name = "isaaclab-assets", marker = "(platform_machine == 'aarch64' and sys_platform == 'linux') or (platform_machine == 'x86_64' and sys_platform == 'linux') or (platform_machine == 'AMD64' and sys_platform == 'win32')" }, { name = "isaaclab-newton", marker = "(platform_machine == 'aarch64' and sys_platform == 'linux') or (platform_machine == 'x86_64' and sys_platform == 'linux') or (platform_machine == 'AMD64' and sys_platform == 'win32')" }, + { name = "isaaclab-physx", marker = "(platform_machine == 'aarch64' and sys_platform == 'linux') or (platform_machine == 'x86_64' and sys_platform == 'linux') or (platform_machine == 'AMD64' and sys_platform == 'win32')" }, ] [package.metadata] @@ -2225,6 +2226,7 @@ requires-dist = [ { name = "isaaclab", editable = "source/isaaclab" }, { name = "isaaclab-assets", editable = "source/isaaclab_assets" }, { name = "isaaclab-newton", editable = "source/isaaclab_newton" }, + { name = "isaaclab-physx", editable = "source/isaaclab_physx" }, ] [[package]] @@ -2255,7 +2257,7 @@ requires-dist = [{ name = "isaaclab", editable = "source/isaaclab" }] [[package]] name = "isaaclab-visualizers" -version = "1.5.1" +version = "1.5.2" source = { editable = "source/isaaclab_visualizers" } dependencies = [ { name = "isaaclab", marker = "(platform_machine == 'aarch64' and sys_platform == 'linux') or (platform_machine == 'x86_64' and sys_platform == 'linux') or (platform_machine == 'AMD64' and sys_platform == 'win32')" }, From 5714e65e239d7d17e0745ce9dfb2a033fcc7a684 Mon Sep 17 00:00:00 2001 From: Maximilian Krause Date: Thu, 13 Aug 2026 18:24:56 -0700 Subject: [PATCH 13/23] Use clone templates for conveyor bindings --- .../contrib/conveyor_franka/conveyor_franka_env.py | 2 +- .../conveyor_franka/conveyor_franka_physx_env_cfg.py | 9 +++------ .../test/contrib/test_conveyor_franka_physx_cfg.py | 1 - 3 files changed, 4 insertions(+), 8 deletions(-) diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env.py index 0e67480c4cc2..ef714f68bcab 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env.py @@ -46,7 +46,7 @@ def _init_sim(self) -> None: ) else: belt_specs = tuple(spec_builder(**belt_spec_kwargs)) - env_path_format = self.cfg.scene.clone_cfg.clone_regex.replace(".*", "{}") + env_path_format = self.cfg.scene.clone_cfg.clone_template # Newton needs solved-contact attributes and graph callbacks registered # before the first reset finalizes and captures the solver. PhysX belt diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_physx_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_physx_env_cfg.py index b19e317b0a1d..6f7a14d6d6fd 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_physx_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_physx_env_cfg.py @@ -22,11 +22,10 @@ from isaaclab.utils.configclass import configclass from .conveyor_franka_env_cfg import ( + _CONTACT_GAP, + _CUBE_CONTACT_MARGIN, ConveyorFrankaEnvCfg, ConveyorFrankaSceneCfg, - _ARM_JOINT_NAMES, - _CUBE_CONTACT_MARGIN, - _CONTACT_GAP, _spawn_hidden_collision_mesh, _spawn_shape_with_display_color, _validate_common_config, @@ -308,9 +307,7 @@ def __post_init__(self) -> None: ) section_keys = ("top_straight", "bottom_straight", "right_turn", "left_turn") - for section_key, (section, root_position) in zip( - section_keys, physx_belt_section_specs(side), strict=True - ): + for section_key, (section, root_position) in zip(section_keys, physx_belt_section_specs(side), strict=True): setattr( self, f"conveyor_{side.lower()}_{section_key}_collision", diff --git a/source/isaaclab_tasks/test/contrib/test_conveyor_franka_physx_cfg.py b/source/isaaclab_tasks/test/contrib/test_conveyor_franka_physx_cfg.py index aa3faaaea286..a3ab36ed2824 100644 --- a/source/isaaclab_tasks/test/contrib/test_conveyor_franka_physx_cfg.py +++ b/source/isaaclab_tasks/test/contrib/test_conveyor_franka_physx_cfg.py @@ -7,7 +7,6 @@ import gymnasium as gym import pytest - from isaaclab_physx.physics import PhysxCfg from isaaclab_physx.sim.spawners.materials import PhysxRigidBodyMaterialCfg From c57f7bcd5b316e7bf6cf4d321acc7882b5f8720a Mon Sep 17 00:00:00 2001 From: Maximilian Krause Date: Fri, 14 Aug 2026 11:41:20 -0700 Subject: [PATCH 14/23] Preserve task devices and finite action rewards --- .../maximiliank-conveyor-franka.minor.rst | 8 ++++ .../conveyor_franka_env_cfg.py | 6 ++- .../contrib/conveyor_franka/mdp/__init__.pyi | 2 + .../contrib/conveyor_franka/mdp/actions.py | 16 +++++++ .../contrib/conveyor_franka/mdp/rewards.py | 13 +++++ .../isaaclab_tasks/utils/parse_cfg.py | 8 ++-- .../test/contrib/test_contrib_environments.py | 2 +- .../test/contrib/test_conveyor_franka_mdp.py | 48 ++++++++++++++++++- .../contrib/test_conveyor_franka_physx_cfg.py | 9 ++++ source/isaaclab_tasks/test/env_test_utils.py | 8 ++-- uv.lock | 22 ++++----- 11 files changed, 121 insertions(+), 21 deletions(-) diff --git a/source/isaaclab_tasks/changelog.d/maximiliank-conveyor-franka.minor.rst b/source/isaaclab_tasks/changelog.d/maximiliank-conveyor-franka.minor.rst index b68be30e6fa4..431ba99956c6 100644 --- a/source/isaaclab_tasks/changelog.d/maximiliank-conveyor-franka.minor.rst +++ b/source/isaaclab_tasks/changelog.d/maximiliank-conveyor-franka.minor.rst @@ -8,3 +8,11 @@ Added kitless Newton force owner with CUDA-graph and hard-reset-safe lifecycle binding. * Added the opt-in ``IsaacContrib-Conveyor-Franka-PhysX-CPU-v0`` reference task, which explicitly rejects GPU dynamics because the supported native surface-velocity path can drop conveyor contacts. + +Changed +^^^^^^^ + +* Allowed :func:`isaaclab_tasks.utils.parse_env_cfg` callers to preserve a task's configured simulation device by + passing ``device=None``. +* Kept action-rate penalties finite for rejected NaN or infinite policy commands by tracking the sanitized + commands accepted by the task's action terms. diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env_cfg.py index 3e54e29ff264..e6e1acf19d76 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env_cfg.py @@ -221,7 +221,11 @@ class RewardsCfg: params={"action_name": "arm_action"}, weight=-1.0e-3, ) - action_rate_l2 = RewTerm(func=mdp.action_rate_l2, weight=-1.0e-3) + action_rate_l2 = RewTerm( + func=mdp.finite_action_rate_l2, + params={"action_names": ("arm_action", "gripper_action")}, + weight=-1.0e-3, + ) joint_velocity_l2 = RewTerm( func=mdp.finite_joint_velocity_l2, params={"asset_cfg": SceneEntityCfg("robot", joint_names=list(_ARM_JOINT_NAMES), preserve_order=True)}, diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/__init__.pyi b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/__init__.pyi index d4c939e36985..8007e7febd58 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/__init__.pyi +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/__init__.pyi @@ -22,6 +22,7 @@ __all__ = [ "cube_out_of_workspace", "end_effector_axes", "end_effector_velocity", + "finite_action_rate_l2", "finite_joint_velocity_l2", "gripper_joint_positions", "invalid_action", @@ -61,6 +62,7 @@ from .reset_events import ( ) from .rewards import ( action_term_l2, + finite_action_rate_l2, finite_joint_velocity_l2, physical_cube_acquisition_mask, terminal_failure, diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/actions.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/actions.py index 7a2e42b25561..bdadbac2b7b7 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/actions.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/actions.py @@ -46,9 +46,15 @@ def __init__(self, cfg: ConveyorRelativeJointPositionActionCfg, env: ManagerBase raise ValueError("Every lower workspace bound must be less than its upper bound.") if not torch.all(torch.isfinite(self._workspace_lower)) or not torch.all(torch.isfinite(self._workspace_upper)): raise ValueError("workspace bounds must be finite.") + self._previous_actions = torch.zeros_like(self._raw_actions) self._invalid_actions = torch.zeros(self.num_envs, dtype=torch.bool, device=self.device) self._position_targets = self._asset.data.joint_pos.torch[:, self._joint_ids].clone() + @property + def previous_actions(self) -> torch.Tensor: + """Previous finite policy actions, shape ``(num_envs, action_dim)``.""" + return self._previous_actions + @property def invalid_actions(self) -> torch.Tensor: """Whether the latest policy action contained a non-finite component.""" @@ -56,6 +62,7 @@ def invalid_actions(self) -> torch.Tensor: def process_actions(self, actions: torch.Tensor) -> None: """Convert normalized residuals into bounded position targets [rad].""" + self._previous_actions.copy_(self._raw_actions) self._invalid_actions.copy_(~torch.isfinite(actions).all(dim=1)) finite_actions = torch.nan_to_num(actions, nan=0.0, posinf=1.0, neginf=-1.0).clamp(-1.0, 1.0) super().process_actions(finite_actions) @@ -81,6 +88,7 @@ def reset(self, env_ids: Sequence[int] | None = None) -> None: else: self._position_targets[env_ids] = positions[env_ids] self._processed_actions[env_ids] = positions[env_ids] + self._previous_actions[env_ids] = 0.0 self._invalid_actions[env_ids] = False @@ -91,8 +99,14 @@ class ResetBufferedGripperAction(BinaryJointPositionAction): def __init__(self, cfg: ResetBufferedGripperActionCfg, env: ManagerBasedEnv) -> None: super().__init__(cfg, env) + self._previous_actions = torch.zeros_like(self._raw_actions) self._invalid_actions = torch.zeros(self.num_envs, dtype=torch.bool, device=self.device) + @property + def previous_actions(self) -> torch.Tensor: + """Previous finite policy actions, shape ``(num_envs, action_dim)``.""" + return self._previous_actions + @property def invalid_actions(self) -> torch.Tensor: """Whether the latest gripper command contained a non-finite value.""" @@ -100,6 +114,7 @@ def invalid_actions(self) -> torch.Tensor: def process_actions(self, actions: torch.Tensor) -> None: """Map finite binary commands and preserve initially held cubes.""" + self._previous_actions.copy_(self._raw_actions) if actions.dtype == torch.bool: self._invalid_actions.zero_() finite_actions = actions @@ -116,4 +131,5 @@ def process_actions(self, actions: torch.Tensor) -> None: def reset(self, env_ids: Sequence[int] | None = None) -> None: """Clear buffered invalid-command state for selected environments.""" super().reset(env_ids) + self._previous_actions[env_ids] = 0.0 self._invalid_actions[env_ids] = False diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/rewards.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/rewards.py index 51a649cc9aae..ebcb05a69753 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/rewards.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/rewards.py @@ -101,6 +101,19 @@ def action_term_l2(env: ManagerBasedRLEnv, action_name: str) -> torch.Tensor: return torch.sum(torch.square(action), dim=1) +def finite_action_rate_l2( + env: ManagerBasedRLEnv, + action_names: tuple[str, ...] = ("arm_action", "gripper_action"), +) -> torch.Tensor: + """Penalize changes between the finite policy commands accepted by action terms.""" + if not action_names: + raise ValueError("At least one action term is required for the action-rate reward.") + terms = tuple(env.action_manager.get_term(name) for name in action_names) + action = torch.cat(tuple(term.raw_actions for term in terms), dim=1) + previous_action = torch.cat(tuple(term.previous_actions for term in terms), dim=1) + return torch.sum(torch.square(action - previous_action), dim=1) + + def physical_cube_acquisition_mask( env: ManagerBasedRLEnv, command_name: str = "transfer", diff --git a/source/isaaclab_tasks/isaaclab_tasks/utils/parse_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/utils/parse_cfg.py index 3d364c9f65e4..7e4f12a21fa1 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/utils/parse_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/utils/parse_cfg.py @@ -144,13 +144,14 @@ def load_cfg_from_registry(task_name: str, entry_point_key: str) -> dict | objec def parse_env_cfg( - task_name: str, device: str = "cuda:0", num_envs: int | None = None, use_fabric: bool | None = None + task_name: str, device: str | None = "cuda:0", num_envs: int | None = None, use_fabric: bool | None = None ) -> ManagerBasedRLEnvCfg | DirectRLEnvCfg: """Parse configuration for an environment and override based on inputs. Args: task_name: The name of the environment. - device: The device to run the simulation on. Defaults to "cuda:0". + device: The device to run the simulation on. Defaults to "cuda:0". If None, the task's configured + simulation device is preserved. num_envs: Number of environments to create. Defaults to None, in which case it is left unchanged. use_fabric: Whether to enable/disable fabric interface. If false, all read/write operations go through USD. This slows down the simulation but allows seeing the changes in the USD through the USD stage. @@ -179,7 +180,8 @@ def parse_env_cfg( cfg = resolve_presets(cfg) # simulation device - cfg.sim.device = device + if device is not None: + cfg.sim.device = device # disable fabric to read/write through USD if use_fabric is not None: cfg.sim.use_fabric = use_fabric diff --git a/source/isaaclab_tasks/test/contrib/test_contrib_environments.py b/source/isaaclab_tasks/test/contrib/test_contrib_environments.py index d2fbcd88d7a0..9bd8aaa54ef4 100644 --- a/source/isaaclab_tasks/test/contrib/test_contrib_environments.py +++ b/source/isaaclab_tasks/test/contrib/test_contrib_environments.py @@ -61,4 +61,4 @@ def _contrib_environment_params() -> list: @pytest.mark.parametrize("task_name", _contrib_environment_params()) def test_contrib_environments(task_name): - _run_environments(task_name, device="cuda", num_envs=2) + _run_environments(task_name, device=None, num_envs=2) diff --git a/source/isaaclab_tasks/test/contrib/test_conveyor_franka_mdp.py b/source/isaaclab_tasks/test/contrib/test_conveyor_franka_mdp.py index 91fd7cd80d27..3aa4cb81bc59 100644 --- a/source/isaaclab_tasks/test/contrib/test_conveyor_franka_mdp.py +++ b/source/isaaclab_tasks/test/contrib/test_conveyor_franka_mdp.py @@ -40,7 +40,7 @@ reset_variant_counts, select_next_transfer_cube, ) -from isaaclab_tasks.contrib.conveyor_franka.mdp.rewards import transfer_potential +from isaaclab_tasks.contrib.conveyor_franka.mdp.rewards import finite_action_rate_l2, transfer_potential from isaaclab_tasks.contrib.conveyor_franka.mdp.terminations import ( invalid_action, subgoal_time_out, @@ -68,6 +68,7 @@ def _make_arm_action_term() -> ConveyorRelativeJointPositionAction: action._workspace_lower = workspace_lower action._workspace_upper = workspace_upper action._raw_actions = torch.zeros((2, 7)) + action._previous_actions = torch.zeros((2, 7)) action._processed_actions = torch.zeros((2, 7)) action._position_targets = positions.clone() action._invalid_actions = torch.zeros(2, dtype=torch.bool) @@ -79,6 +80,7 @@ def _make_gripper_action_term() -> ResetBufferedGripperAction: action = object.__new__(ResetBufferedGripperAction) action.cfg = SimpleNamespace(clip=None, command_name="transfer", force_close_steps=2) action._raw_actions = torch.zeros((2, 1)) + action._previous_actions = torch.zeros((2, 1)) action._processed_actions = torch.zeros((2, 2)) action._open_command = torch.full((2,), 0.04) action._close_command = torch.zeros(2) @@ -134,6 +136,50 @@ def test_invalid_action_termination_and_reset_are_per_environment(): assert invalid_action(env).tolist() == [False, False] +def test_action_rate_uses_finite_commands_and_preserves_invalid_termination(): + """A rejected policy output has a finite final reward without hiding its termination.""" + arm_action = _make_arm_action_term() + gripper_action = _make_gripper_action_term() + arm_action.process_actions(torch.full((2, 7), 0.25)) + gripper_action.process_actions(torch.tensor(((-1.0,), (1.0,)))) + arm_action.process_actions( + torch.tensor(((float("nan"), float("inf"), -float("inf"), 5.0, -5.0, 0.5, -0.5), (0.5,) * 7)) + ) + gripper_action.process_actions(torch.tensor(((float("nan"),), (-1.0,)))) + actions = {"arm_action": arm_action, "gripper_action": gripper_action} + env = SimpleNamespace(num_envs=2, action_manager=SimpleNamespace(get_term=actions.__getitem__)) + + reward = finite_action_rate_l2(env) + + expected_arm = torch.square(arm_action.raw_actions - arm_action.previous_actions).sum(dim=1) + expected_gripper = torch.square(gripper_action.raw_actions - gripper_action.previous_actions).sum(dim=1) + torch.testing.assert_close(reward, expected_arm + expected_gripper) + assert torch.isfinite(reward).all() + assert invalid_action(env).tolist() == [True, False] + + +def test_action_rate_matches_standard_l2_for_ordinary_policy_actions(): + """Finite in-range policy commands retain the standard action-rate semantics.""" + arm_action = _make_arm_action_term() + gripper_action = _make_gripper_action_term() + previous_arm = torch.tensor(((0.1,) * 7, (-0.2,) * 7)) + current_arm = torch.tensor(((-0.3,) * 7, (0.4,) * 7)) + previous_gripper = torch.tensor(((-1.0,), (1.0,))) + current_gripper = -previous_gripper + arm_action.process_actions(previous_arm) + gripper_action.process_actions(previous_gripper) + arm_action.process_actions(current_arm) + gripper_action.process_actions(current_gripper) + actions = {"arm_action": arm_action, "gripper_action": gripper_action} + env = SimpleNamespace(num_envs=2, action_manager=SimpleNamespace(get_term=actions.__getitem__)) + + reward = finite_action_rate_l2(env) + + previous = torch.cat((previous_arm, previous_gripper), dim=1) + current = torch.cat((current_arm, current_gripper), dim=1) + torch.testing.assert_close(reward, torch.square(current - previous).sum(dim=1)) + + def test_final_config_validation_catches_overridden_arm_contracts(): """Top-level validation runs after overrides and protects workspace-to-joint alignment.""" cfg = ConveyorFrankaEnvCfg() diff --git a/source/isaaclab_tasks/test/contrib/test_conveyor_franka_physx_cfg.py b/source/isaaclab_tasks/test/contrib/test_conveyor_franka_physx_cfg.py index a3ab36ed2824..10bdb1e105b6 100644 --- a/source/isaaclab_tasks/test/contrib/test_conveyor_franka_physx_cfg.py +++ b/source/isaaclab_tasks/test/contrib/test_conveyor_franka_physx_cfg.py @@ -17,6 +17,7 @@ physx_belt_section_specs, ) from isaaclab_tasks.contrib.conveyor_franka.conveyor_geometry import BELT_TURN_RADIUS, MeshSpec +from isaaclab_tasks.utils.parse_cfg import parse_env_cfg def test_physx_task_is_registered_with_a_dedicated_config() -> None: @@ -48,6 +49,14 @@ def test_physx_config_preserves_policy_and_timing_contracts() -> None: assert physx_cfg.scene.robot.spawn.rigid_props.disable_gravity is True +def test_physx_task_default_device_survives_an_unset_parser_override() -> None: + """Default-backend callers can preserve the task's declared CPU device.""" + cfg = parse_env_cfg("IsaacContrib-Conveyor-Franka-PhysX-CPU-v0", device=None, num_envs=2) + + assert cfg.sim.device == "cpu" + assert cfg.scene.num_envs == 2 + + @pytest.mark.parametrize("device", ["cuda", "cuda:0", "cuda:1"]) def test_physx_config_rejects_broken_gpu_surface_velocity_contacts(device: str) -> None: """The pinned Isaac Sim GPU path must not silently let cubes tunnel through belts.""" diff --git a/source/isaaclab_tasks/test/env_test_utils.py b/source/isaaclab_tasks/test/env_test_utils.py index db71493fd9a1..606db10e7686 100644 --- a/source/isaaclab_tasks/test/env_test_utils.py +++ b/source/isaaclab_tasks/test/env_test_utils.py @@ -218,7 +218,7 @@ def _configure_osc_smoke_actions(env, actions: torch.Tensor) -> None: def _run_environments( task_name, - device, + device: str | None, num_envs, num_steps=20, multi_agent=False, @@ -230,7 +230,7 @@ def _run_environments( Args: task_name: Name of the environment. - device: Device to use (e.g., 'cuda'). + device: Device override to use (e.g., 'cuda'), or None to preserve the task default. num_envs: Number of environments. num_steps: Number of simulation steps. multi_agent: Whether the environment is multi-agent. @@ -274,7 +274,7 @@ def _run_environments( def _check_random_actions( task_name: str, - device: str, + device: str | None, num_envs: int, num_steps: int = 20, multi_agent: bool = False, @@ -286,7 +286,7 @@ def _check_random_actions( Args: task_name: Name of the environment. - device: Device to use (e.g., 'cuda'). + device: Device override to use (e.g., 'cuda'), or None to preserve the task default. num_envs: Number of environments. num_steps: Number of simulation steps. multi_agent: Whether the environment is multi-agent. diff --git a/uv.lock b/uv.lock index f056c0df5f59..ede1fdfbf5d6 100644 --- a/uv.lock +++ b/uv.lock @@ -1764,12 +1764,12 @@ wheels = [ [[package]] name = "isaaclab" -version = "16.1.0" +version = "16.2.0" source = { editable = "source/isaaclab" } [[package]] name = "isaaclab-assets" -version = "0.6.3" +version = "0.6.4" source = { editable = "source/isaaclab_assets" } dependencies = [ { name = "isaaclab", marker = "(platform_machine == 'aarch64' and sys_platform == 'linux') or (platform_machine == 'x86_64' and sys_platform == 'linux') or (platform_machine == 'AMD64' and sys_platform == 'win32')" }, @@ -1784,7 +1784,7 @@ requires-dist = [ [[package]] name = "isaaclab-contrib" -version = "1.3.1" +version = "1.4.0" source = { editable = "source/isaaclab_contrib" } dependencies = [ { name = "isaaclab", marker = "(platform_machine == 'aarch64' and sys_platform == 'linux') or (platform_machine == 'x86_64' and sys_platform == 'linux') or (platform_machine == 'AMD64' and sys_platform == 'win32')" }, @@ -2119,7 +2119,7 @@ provides-extras = ["tetrahedralization", "video", "test", "sb3", "skrl", "rl-gam [[package]] name = "isaaclab-experimental" -version = "0.2.0" +version = "0.2.1" source = { editable = "source/isaaclab_experimental" } dependencies = [ { name = "isaaclab", marker = "(platform_machine == 'aarch64' and sys_platform == 'linux') or (platform_machine == 'x86_64' and sys_platform == 'linux') or (platform_machine == 'AMD64' and sys_platform == 'win32')" }, @@ -2130,7 +2130,7 @@ requires-dist = [{ name = "isaaclab", editable = "source/isaaclab" }] [[package]] name = "isaaclab-mimic" -version = "2.0.4" +version = "2.0.5" source = { editable = "source/isaaclab_mimic" } dependencies = [ { name = "isaaclab", marker = "(platform_machine == 'aarch64' and sys_platform == 'linux') or (platform_machine == 'x86_64' and sys_platform == 'linux') or (platform_machine == 'AMD64' and sys_platform == 'win32')" }, @@ -2147,7 +2147,7 @@ requires-dist = [ [[package]] name = "isaaclab-newton" -version = "5.0.0" +version = "5.1.0" source = { editable = "source/isaaclab_newton" } dependencies = [ { name = "isaaclab", marker = "(platform_machine == 'aarch64' and sys_platform == 'linux') or (platform_machine == 'x86_64' and sys_platform == 'linux') or (platform_machine == 'AMD64' and sys_platform == 'win32')" }, @@ -2158,7 +2158,7 @@ requires-dist = [{ name = "isaaclab", editable = "source/isaaclab" }] [[package]] name = "isaaclab-ov" -version = "2.0.1" +version = "2.0.2" source = { editable = "source/isaaclab_ov" } dependencies = [ { name = "isaaclab", marker = "(platform_machine == 'aarch64' and sys_platform == 'linux') or (platform_machine == 'x86_64' and sys_platform == 'linux') or (platform_machine == 'AMD64' and sys_platform == 'win32')" }, @@ -2173,7 +2173,7 @@ requires-dist = [ [[package]] name = "isaaclab-physx" -version = "5.0.0" +version = "5.0.1" source = { editable = "source/isaaclab_physx" } dependencies = [ { name = "isaaclab", marker = "(platform_machine == 'aarch64' and sys_platform == 'linux') or (platform_machine == 'x86_64' and sys_platform == 'linux') or (platform_machine == 'AMD64' and sys_platform == 'win32')" }, @@ -2195,7 +2195,7 @@ requires-dist = [{ name = "isaaclab", editable = "source/isaaclab" }] [[package]] name = "isaaclab-rl" -version = "0.14.1" +version = "0.15.0" source = { editable = "source/isaaclab_rl" } dependencies = [ { name = "isaaclab", marker = "(platform_machine == 'aarch64' and sys_platform == 'linux') or (platform_machine == 'x86_64' and sys_platform == 'linux') or (platform_machine == 'AMD64' and sys_platform == 'win32')" }, @@ -2212,7 +2212,7 @@ requires-dist = [ [[package]] name = "isaaclab-tasks" -version = "16.1.0" +version = "16.2.0" source = { editable = "source/isaaclab_tasks" } dependencies = [ { name = "isaaclab", marker = "(platform_machine == 'aarch64' and sys_platform == 'linux') or (platform_machine == 'x86_64' and sys_platform == 'linux') or (platform_machine == 'AMD64' and sys_platform == 'win32')" }, @@ -2257,7 +2257,7 @@ requires-dist = [{ name = "isaaclab", editable = "source/isaaclab" }] [[package]] name = "isaaclab-visualizers" -version = "1.5.2" +version = "1.6.0" source = { editable = "source/isaaclab_visualizers" } dependencies = [ { name = "isaaclab", marker = "(platform_machine == 'aarch64' and sys_platform == 'linux') or (platform_machine == 'x86_64' and sys_platform == 'linux') or (platform_machine == 'AMD64' and sys_platform == 'win32')" }, From 281dcb8fef986d7a9fdbb782005410216d4a7db5 Mon Sep 17 00:00:00 2001 From: Maximilian Krause Date: Fri, 14 Aug 2026 12:25:32 -0700 Subject: [PATCH 15/23] Localize conveyor task interfaces --- .../maximiliank-conveyor-belt-api.rst | 5 ----- source/isaaclab/isaaclab/physics/__init__.pyi | 3 --- .../maximiliank-conveyor-franka.minor.rst | 2 +- .../contrib/conveyor_franka}/conveyor_belt.py | 20 ++----------------- .../conveyor_franka/conveyor_force_driver.py | 4 +++- .../conveyor_franka/conveyor_franka_env.py | 2 +- .../conveyor_franka_physx_env_cfg.py | 2 +- .../conveyor_franka/conveyor_geometry.py | 2 +- .../conveyor_franka/conveyor_physx_surface.py | 2 +- .../test/contrib}/test_conveyor_belt.py | 4 ++-- .../contrib/test_conveyor_force_driver.py | 3 ++- .../contrib/test_conveyor_physx_surface.py | 3 +-- 12 files changed, 15 insertions(+), 37 deletions(-) delete mode 100644 source/isaaclab/changelog.d/maximiliank-conveyor-belt-api.rst rename source/{isaaclab/isaaclab/physics => isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka}/conveyor_belt.py (91%) rename source/{isaaclab/test/sim => isaaclab_tasks/test/contrib}/test_conveyor_belt.py (95%) diff --git a/source/isaaclab/changelog.d/maximiliank-conveyor-belt-api.rst b/source/isaaclab/changelog.d/maximiliank-conveyor-belt-api.rst deleted file mode 100644 index 0bf7faa88ae6..000000000000 --- a/source/isaaclab/changelog.d/maximiliank-conveyor-belt-api.rst +++ /dev/null @@ -1,5 +0,0 @@ -Added -^^^^^ - -* Added a backend-neutral conveyor belt specification and tensorized control contract for reusable, - vectorized conveyor implementations. diff --git a/source/isaaclab/isaaclab/physics/__init__.pyi b/source/isaaclab/isaaclab/physics/__init__.pyi index 55aed0e59a48..92d9bfac2ff4 100644 --- a/source/isaaclab/isaaclab/physics/__init__.pyi +++ b/source/isaaclab/isaaclab/physics/__init__.pyi @@ -5,14 +5,11 @@ __all__ = [ "CallbackHandle", - "ConveyorBeltSpec", - "ConveyorBeltView", "PhysicsEvent", "PhysicsManager", "PhysicsCfg", "PhysxAutoCfg", ] -from .conveyor_belt import ConveyorBeltSpec, ConveyorBeltView from .physics_manager import CallbackHandle, PhysicsEvent, PhysicsManager from .physics_manager_cfg import PhysicsCfg, PhysxAutoCfg diff --git a/source/isaaclab_tasks/changelog.d/maximiliank-conveyor-franka.minor.rst b/source/isaaclab_tasks/changelog.d/maximiliank-conveyor-franka.minor.rst index 431ba99956c6..12d2a839748f 100644 --- a/source/isaaclab_tasks/changelog.d/maximiliank-conveyor-franka.minor.rst +++ b/source/isaaclab_tasks/changelog.d/maximiliank-conveyor-franka.minor.rst @@ -4,7 +4,7 @@ Added * Added a contributed manager-based environment with guarded, counter-rotating force-driven racetrack conveyors, robust primitive and closed-mesh belt colliders, a MuJoCo Menagerie Franka, and an interactive Newton-viewer cube-goal selector. -* Added schema-aligned conveyor descriptions and a tensorized control view while retaining a single, +* Added task-local, schema-aligned conveyor descriptions and a tensorized control view while retaining a single, kitless Newton force owner with CUDA-graph and hard-reset-safe lifecycle binding. * Added the opt-in ``IsaacContrib-Conveyor-Franka-PhysX-CPU-v0`` reference task, which explicitly rejects GPU dynamics because the supported native surface-velocity path can drop conveyor contacts. diff --git a/source/isaaclab/isaaclab/physics/conveyor_belt.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_belt.py similarity index 91% rename from source/isaaclab/isaaclab/physics/conveyor_belt.py rename to source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_belt.py index 4d4ff37b5f63..23d6a6344179 100644 --- a/source/isaaclab/isaaclab/physics/conveyor_belt.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_belt.py @@ -3,7 +3,7 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Backend-neutral conveyor belt descriptions and control interface.""" +"""Task-local conveyor belt descriptions and shared control interface.""" from __future__ import annotations @@ -145,7 +145,7 @@ def __post_init__(self) -> None: @runtime_checkable class ConveyorBeltView(Protocol): - """Tensorized control contract implemented by conveyor physics backends.""" + """Common tensorized control contract implemented by both task backends.""" @property def prim_paths(self) -> tuple[str, ...]: @@ -182,22 +182,6 @@ def get_enabled(self, indices: Any = None, clone: bool = True) -> Any: """Return integer enabled flags for selected belts.""" ... - def set_friction_coefficients(self, coefficients: Any, indices: Any = None) -> None: - """Set Coulomb traction coefficients for selected belts.""" - ... - - def get_friction_coefficients(self, indices: Any = None, clone: bool = True) -> Any: - """Return Coulomb traction coefficients for selected belts.""" - ... - - def set_contact_processing_thresholds(self, thresholds: Any, indices: Any = None) -> None: - """Set contact-normal alignment thresholds for selected belts.""" - ... - - def get_contact_processing_thresholds(self, indices: Any = None, clone: bool = True) -> Any: - """Return contact-normal alignment thresholds for selected belts.""" - ... - def get_encoder_positions(self, indices: Any = None, clone: bool = True) -> Any: """Return integrated belt travel [m] for selected belts.""" ... diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_force_driver.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_force_driver.py index 798e0b47ea91..d8a00d265196 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_force_driver.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_force_driver.py @@ -20,7 +20,9 @@ import warp as wp from isaaclab_newton.physics import NewtonManager -from isaaclab.physics import ConveyorBeltSpec, PhysicsEvent +from isaaclab.physics import PhysicsEvent + +from .conveyor_belt import ConveyorBeltSpec _VELOCITY_FIELD_TYPE_CONSTANT = 0 _VELOCITY_FIELD_TYPE_PIVOT = 1 diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env.py index ef714f68bcab..dc23cccb91b5 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env.py @@ -10,8 +10,8 @@ from collections.abc import Sequence from isaaclab.envs import ManagerBasedRLEnv -from isaaclab.physics import ConveyorBeltView +from .conveyor_belt import ConveyorBeltView from .conveyor_force_driver import ConveyorForceDriver from .conveyor_franka_env_cfg import ConveyorFrankaEnvCfg from .conveyor_geometry import belt_collision_section_specs diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_physx_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_physx_env_cfg.py index 6f7a14d6d6fd..4f806567710f 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_physx_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_physx_env_cfg.py @@ -16,11 +16,11 @@ import isaaclab.sim as sim_utils from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg -from isaaclab.physics import ConveyorBeltSpec from isaaclab.sim import SimulationCfg from isaaclab.sim.schemas import CollisionFragment, UsdPhysicsCollisionCfg from isaaclab.utils.configclass import configclass +from .conveyor_belt import ConveyorBeltSpec from .conveyor_franka_env_cfg import ( _CONTACT_GAP, _CUBE_CONTACT_MARGIN, diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_geometry.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_geometry.py index c2ea7c2ff80d..93b2fa71fd96 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_geometry.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_geometry.py @@ -10,7 +10,7 @@ import math from dataclasses import dataclass -from isaaclab.physics import ConveyorBeltSpec +from .conveyor_belt import ConveyorBeltSpec BELT_COLOR = (0.09, 0.09, 0.09) """Dark-rubber color used by Newton's conveyor example.""" diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_physx_surface.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_physx_surface.py index 40fcf39bb1cd..7424de7ff7f4 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_physx_surface.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_physx_surface.py @@ -22,7 +22,7 @@ import numpy as np -from isaaclab.physics import ConveyorBeltSpec +from .conveyor_belt import ConveyorBeltSpec _ENV_REGEX_NS = "{ENV_REGEX_NS}" diff --git a/source/isaaclab/test/sim/test_conveyor_belt.py b/source/isaaclab_tasks/test/contrib/test_conveyor_belt.py similarity index 95% rename from source/isaaclab/test/sim/test_conveyor_belt.py rename to source/isaaclab_tasks/test/contrib/test_conveyor_belt.py index 13d8d3f14dd0..c8e33366cbe0 100644 --- a/source/isaaclab/test/sim/test_conveyor_belt.py +++ b/source/isaaclab_tasks/test/contrib/test_conveyor_belt.py @@ -3,13 +3,13 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Tests for the backend-neutral conveyor belt contract.""" +"""Tests for the conveyor-Franka task's shared belt contract.""" from __future__ import annotations import pytest -from isaaclab.physics import ConveyorBeltSpec +from isaaclab_tasks.contrib.conveyor_franka.conveyor_belt import ConveyorBeltSpec def test_conveyor_belt_spec_preserves_authored_semantics() -> None: diff --git a/source/isaaclab_tasks/test/contrib/test_conveyor_force_driver.py b/source/isaaclab_tasks/test/contrib/test_conveyor_force_driver.py index 33c1d5df4957..022a1b123d62 100644 --- a/source/isaaclab_tasks/test/contrib/test_conveyor_force_driver.py +++ b/source/isaaclab_tasks/test/contrib/test_conveyor_force_driver.py @@ -13,9 +13,10 @@ import pytest import warp as wp -from isaaclab.physics import ConveyorBeltSpec, PhysicsEvent +from isaaclab.physics import PhysicsEvent import isaaclab_tasks.contrib.conveyor_franka.conveyor_force_driver as driver_module +from isaaclab_tasks.contrib.conveyor_franka.conveyor_belt import ConveyorBeltSpec def _belt_spec(name: str = "Belt") -> ConveyorBeltSpec: diff --git a/source/isaaclab_tasks/test/contrib/test_conveyor_physx_surface.py b/source/isaaclab_tasks/test/contrib/test_conveyor_physx_surface.py index 52742801ba61..c59f5183f04c 100644 --- a/source/isaaclab_tasks/test/contrib/test_conveyor_physx_surface.py +++ b/source/isaaclab_tasks/test/contrib/test_conveyor_physx_surface.py @@ -15,9 +15,8 @@ import pytest import torch -from isaaclab.physics import ConveyorBeltSpec - import isaaclab_tasks.contrib.conveyor_franka.conveyor_physx_surface as surface_module +from isaaclab_tasks.contrib.conveyor_franka.conveyor_belt import ConveyorBeltSpec class _FakeWriter: From dc934ed8e371d53e4e7314c86f2c14c190f2d44f Mon Sep 17 00:00:00 2001 From: Maximilian Krause Date: Wed, 19 Aug 2026 15:00:44 -0700 Subject: [PATCH 16/23] Document pretrained conveyor checkpoint --- .../isaaclab_tasks/contrib/conveyor_franka/README.md | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/README.md b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/README.md index e85e666f836b..b19bdcf00678 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/README.md +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/README.md @@ -35,10 +35,15 @@ Newton is kitless and supports the lightweight GL viewer: ```bash DISPLAY=:1 uv run isaaclab play --rl_library rsl_rl \ --task IsaacContrib-Conveyor-Franka-Newton-v0 \ - --checkpoint /path/to/model.pt \ + --checkpoint pretrained \ --num_envs 8 --device cuda:0 --viz newton_gl --real-time ``` +The `pretrained` selector downloads the RSL-RL policy published specifically for the Newton MJWarp +backend. To evaluate another policy, replace `pretrained` with an explicit checkpoint path. The +PhysX task resolves a different backend-specific artifact name, so transferring this Newton policy +to PhysX currently requires the explicit local checkpoint path shown below. + Training uses the same task ID and defaults to 256 environments: ```bash From 92eed73fc42480597fd1867a58456c3ba84063f0 Mon Sep 17 00:00:00 2001 From: Maximilian Krause Date: Mon, 24 Aug 2026 19:19:10 -0700 Subject: [PATCH 17/23] Add asset-rich conveyor playback scene --- .../maximiliank-conveyor-franka.minor.rst | 3 + .../contrib/conveyor_franka/README.md | 18 + .../contrib/conveyor_franka/__init__.py | 12 + .../conveyor_franka_asset_env_cfg.py | 411 ++++++++++++++++++ .../conveyor_franka_asset_terrain.py | 24 + .../contrib/test_conveyor_franka_asset_cfg.py | 191 ++++++++ 6 files changed, 659 insertions(+) create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_asset_env_cfg.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_asset_terrain.py create mode 100644 source/isaaclab_tasks/test/contrib/test_conveyor_franka_asset_cfg.py diff --git a/source/isaaclab_tasks/changelog.d/maximiliank-conveyor-franka.minor.rst b/source/isaaclab_tasks/changelog.d/maximiliank-conveyor-franka.minor.rst index 12d2a839748f..33edec2474ee 100644 --- a/source/isaaclab_tasks/changelog.d/maximiliank-conveyor-franka.minor.rst +++ b/source/isaaclab_tasks/changelog.d/maximiliank-conveyor-franka.minor.rst @@ -6,6 +6,9 @@ Added Newton-viewer cube-goal selector. * Added task-local, schema-aligned conveyor descriptions and a tensorized control view while retaining a single, kitless Newton force owner with CUDA-graph and hard-reset-safe lifecycle binding. +* Added a checkpoint-compatible Newton Play variant rendered with A09/A12 functional-loop visuals, a render-only + Thor robot table, packing station, pallet bays, and warehouse dressing while retaining the task's lightweight + collision and traction surfaces. * Added the opt-in ``IsaacContrib-Conveyor-Franka-PhysX-CPU-v0`` reference task, which explicitly rejects GPU dynamics because the supported native surface-velocity path can drop conveyor contacts. diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/README.md b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/README.md index b19bdcf00678..6543b468843b 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/README.md +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/README.md @@ -17,6 +17,7 @@ commands, rewards, reset recipes, 120 Hz physics step, and 60 Hz policy rate. | Task | Physics device | Intended use | Conveyor actuation | | --- | --- | --- | --- | | `IsaacContrib-Conveyor-Franka-Newton-v0` | CUDA | Training and scalable playback | Batched Warp contact-force feedback captured with the Newton solver graph | +| `IsaacContrib-Conveyor-Franka-Newton-Play-v0` | CUDA | Warehouse-dressed Digital Twin playback | Same Newton force feedback on lightweight hidden collision surfaces | | `IsaacContrib-Conveyor-Franka-PhysX-CPU-v0` | CPU only | Native-PhysX reference and checkpoint playback | Authored `PhysxSurfaceVelocityAPI` on kinematic belt sections | The PhysX task rejects CUDA during configuration validation. In the supported Isaac Sim runtime, @@ -44,6 +45,23 @@ backend. To evaluate another policy, replace `pretrained` with an explicit check PhysX task resolves a different backend-specific artifact name, so transferring this Newton policy to PhysX currently requires the explicit local checkpoint path shown below. +For presentation, use the checkpoint-compatible Play variant. It replaces the procedural render +geometry with `ConveyorBelt_A09` straight sections and `ConveyorBelt_A12` 180-degree turns from +`Isaac/Props/Conveyors`. A Thor robot table, packing station, separate loaded and empty pallet bays, +safety markings, and a warehouse backdrop complete the scene. Every added USD is render-only with its +authored physics APIs and action graphs stripped locally. The same lightweight hidden surfaces remain +the sole owners of contact and conveyor forces, so scene dressing cannot introduce double contacts or +change the trained policy dynamics. Measured asset feet sit on the global `z=0` ground plane; the complete +policy workspace is elevated without changing its local coordinates. This asset-rich variant defaults to +one environment to keep interactive startup and rendering practical. + +```bash +DISPLAY=:1 uv run isaaclab play --rl_library rsl_rl \ + --task IsaacContrib-Conveyor-Franka-Newton-Play-v0 \ + --checkpoint pretrained \ + --num_envs 1 --device cuda:0 --viz newton_gl --real-time +``` + Training uses the same task ID and defaults to 256 environments: ```bash diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/__init__.py index e3af7e9d3f26..1222979e24bd 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/__init__.py @@ -19,6 +19,18 @@ }, ) +gym.register( + # The conventional Play suffix lets the pretrained-checkpoint resolver + # reuse the base Newton task's published policy automatically. + id="IsaacContrib-Conveyor-Franka-Newton-Play-v0", + entry_point=f"{__name__}.conveyor_franka_env:ConveyorFrankaEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.conveyor_franka_asset_env_cfg:ConveyorFrankaA09A12EnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:ConveyorFrankaPPORunnerCfg", + }, +) + gym.register( # The native PhysxSurfaceVelocityAPI path is intentionally CPU-only. Keep # that execution contract visible in the public task ID so a CUDA launch is diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_asset_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_asset_env_cfg.py new file mode 100644 index 000000000000..f734b796ab88 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_asset_env_cfg.py @@ -0,0 +1,411 @@ +# 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 + +"""Digital Twin warehouse visuals for conveyor-Franka playback.""" + +from __future__ import annotations + +from collections.abc import Callable +from typing import TYPE_CHECKING + +import isaaclab.sim as sim_utils +from isaaclab.assets import AssetBaseCfg +from isaaclab.terrains import TerrainImporterCfg +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR +from isaaclab.utils.configclass import configclass + +if TYPE_CHECKING: + from isaaclab.terrains import TerrainImporter + +from .conveyor_franka_env_cfg import ( + ConveyorFrankaEnvCfg, + ConveyorFrankaSceneCfg, + _spawn_shape_with_display_color, +) +from .conveyor_geometry import ( + BELT_CENTER_X, + BELT_CENTER_Y, + BELT_HALF_STRAIGHT, + BELT_TOP_Z, + BELT_TURN_RADIUS, +) + +_CONVEYOR_ASSET_DIR = f"{ISAAC_NUCLEUS_DIR}/Props/Conveyors" +_A09_ASSET_PATH = f"{_CONVEYOR_ASSET_DIR}/ConveyorBelt_A09.usd" +_A12_ASSET_PATH = f"{_CONVEYOR_ASSET_DIR}/ConveyorBelt_A12.usd" +_THOR_TABLE_ASSET_PATH = f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/thor_table.usd" +_PACKING_TABLE_ASSET_PATH = f"{ISAAC_NUCLEUS_DIR}/Props/PackingTable/packing_table.usd" +_PALLET_ASSET_PATH = f"{ISAAC_NUCLEUS_DIR}/Props/Pallet/pallet.usd" +_LOADED_PALLET_ASSET_PATH = f"{ISAAC_NUCLEUS_DIR}/Props/Pallet/o3dyn_pallet.usd" + +# The A12 endpoints are 2.9922 m apart, its belt crown is 1.78053 m above the +# asset origin, and the lowest rendered point of both A09 and A12 is authored at +# z=0. Scale from those measured bounds so the asset feet sit on the global +# ground while the visual surface follows the existing task colliders. +_A12_ENDPOINT_SEPARATION = 2.9922 +_ASSET_BELT_TOP_Z = 1.78053 +_ASSET_LOWEST_Z = 0.0 +_ASSET_XY_SCALE = 2.0 * BELT_TURN_RADIUS / _A12_ENDPOINT_SEPARATION +_GROUND_PLANE_Z = 0.0 +# Preserve the assets' lateral/vertical proportions instead of stretching the +# supports to the original table height. The scaled belt crown then determines +# how far to elevate the policy workspace. +_ASSET_Z_SCALE = _ASSET_XY_SCALE +_ASSET_ROOT_Z = _GROUND_PLANE_Z - _ASSET_LOWEST_Z * _ASSET_Z_SCALE +_ASSET_BELT_WORLD_Z = _ASSET_ROOT_Z + _ASSET_BELT_TOP_Z * _ASSET_Z_SCALE +_WORKSPACE_ELEVATION = _ASSET_BELT_WORLD_Z - BELT_TOP_Z + +# A09 is a 4 m straight. Its travel-axis scale is independent of the common +# lateral scale so one asset spans the task's complete 0.88 m straight run. +_A09_LENGTH = 4.0 +_A09_X_SCALE = 2.0 * BELT_HALF_STRAIGHT / _A09_LENGTH + +# The Thor table authors its mounting surface at local z=0 and its lowest foot +# at z=-0.795 m. Uniformly scaling that distance to the elevated robot base +# puts every foot on the global ground without distorting the table. +_THOR_TABLE_LOWEST_Z = -0.795 +_THOR_TABLE_SCALE = _WORKSPACE_ELEVATION / -_THOR_TABLE_LOWEST_Z + +_BACKDROP_COLOR = (0.075, 0.09, 0.12) +_BACKDROP_ACCENT_COLOR = (0.16, 0.20, 0.25) +_SAFETY_YELLOW = (0.95, 0.58, 0.055) + +_PHYSICS_SCHEMA_PREFIXES = ("Physics", "Physx", "Newton", "Mujoco") +_PHYSICS_SCHEMA_NAMES = frozenset(("IsaacConveyorAPI",)) + + +def _is_physics_schema(schema_name: str) -> bool: + """Return whether an applied schema can add physics ownership to a visual asset.""" + return schema_name in _PHYSICS_SCHEMA_NAMES or schema_name.startswith(_PHYSICS_SCHEMA_PREFIXES) + + +def _make_usd_subtree_visual_only(root_prim) -> None: + """Author a render-only override for a referenced USD subtree. + + The source asset remains untouched. All edits are stronger opinions in the + task stage and fail closed if a physics schema cannot be removed. + """ + # Import USD only when a stage exists. Besides respecting Kit startup + # ordering, this keeps import-light task discovery kitless. + from pxr import Sdf, Usd, UsdPhysics + + children = tuple(Usd.PrimRange(root_prim, Usd.TraverseInstanceProxies())) + instance_proxies = tuple(str(child.GetPath()) for child in children if child.IsInstanceProxy()) + if instance_proxies: + raise RuntimeError( + "Visual-only USD overrides require editable descendants; set make_uninstanceable=True. " + f"Found instance proxies below {root_prim.GetPath()}: {instance_proxies[:3]}" + ) + + with Sdf.ChangeBlock(): + for child in children: + if child.GetTypeName().startswith("OmniGraph") or child.IsA(UsdPhysics.Scene): + child.SetActive(False) + + for child in children: + if not child.IsValid() or not child.IsActive(): + continue + if child.IsA(UsdPhysics.Joint): + child.SetActive(False) + continue + for schema_name in tuple(child.GetAppliedSchemas()): + if _is_physics_schema(schema_name) and not child.RemoveAppliedSchema(schema_name): + raise RuntimeError(f"Failed to remove physics schema {schema_name!r} from {child.GetPath()}.") + + remaining = { + str(child.GetPath()): tuple(schema for schema in child.GetAppliedSchemas() if _is_physics_schema(schema)) + for child in children + if child.IsValid() and child.IsActive() + } + remaining = {path: schemas for path, schemas in remaining.items() if schemas} + if remaining: + raise RuntimeError(f"Visual-only USD subtree still contains physics schemas: {remaining}") + + +@sim_utils.clone +def _spawn_visual_only_usd( + prim_path: str, + cfg: sim_utils.UsdFileCfg, + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, + **kwargs, +): + """Spawn a USD asset and strip its authored physics metadata.""" + prim = sim_utils.spawn_from_usd(prim_path, cfg, translation, orientation, **kwargs) + + # Presentation assets may contain nested dynamic props (the packing table, + # for example, carries an authored container rigid body). Merely disabling + # collision still exposes those bodies and joints to a backend parser. + # Author local API deletions so the entire referenced hierarchy is a pure + # render layer and cannot alter either Newton's model or PhysX's scene. + _make_usd_subtree_visual_only(prim) + return prim + + +@configclass +class _VisualOnlyUsdFileCfg(sim_utils.UsdFileCfg): + """USD reference whose composed subtree is guaranteed to remain render-only.""" + + func: Callable = _spawn_visual_only_usd + # Recursive schema overrides cannot be authored on USD instance proxies. + make_uninstanceable: bool = True + + +@configclass +class _ElevatedGroundPlaneCfg(TerrainImporterCfg): + """Ground plane that reports the elevated workspace as its environment origin.""" + + class_type: type[TerrainImporter] | str = ( + "{DIR}.conveyor_franka_asset_terrain:ConveyorFrankaGroundPlaneTerrainImporter" + ) + workspace_origin_offset: tuple[float, float, float] = (0.0, 0.0, _WORKSPACE_ELEVATION) + """Translation from clone-grid origins to policy workspaces [m].""" + + +def _visual_usd_asset( + prim_path: str, + usd_path: str, + position: tuple[float, float, float], + scale: tuple[float, float, float], + rotation: tuple[float, float, float, float] = (0.0, 0.0, 0.0, 1.0), +) -> AssetBaseCfg: + """Build one visual asset whose authored colliders are explicitly disabled.""" + spawn = _VisualOnlyUsdFileCfg( + usd_path=usd_path, + scale=scale, + ) + return AssetBaseCfg( + prim_path=prim_path, + init_state=AssetBaseCfg.InitialStateCfg(pos=position, rot=rotation), + # Physics remains owned by the task's lightweight, hidden belt and rail + # proxies. The spawn callback strips authored physics metadata to keep + # the visual geometry entirely non-authoritative. + spawn=spawn, + ) + + +def _visual_cuboid( + prim_path: str, + size: tuple[float, float, float], + position: tuple[float, float, float], + color: tuple[float, float, float], + roughness: float = 0.72, + metallic: float = 0.0, +) -> AssetBaseCfg: + """Build one non-colliding scene-dressing cuboid.""" + spawn = sim_utils.CuboidCfg( + func=_spawn_shape_with_display_color, + size=size, + visual_material=sim_utils.PreviewSurfaceCfg( + diffuse_color=color, + roughness=roughness, + metallic=metallic, + ), + ) + return AssetBaseCfg( + prim_path=prim_path, + init_state=AssetBaseCfg.InitialStateCfg(pos=position), + spawn=spawn, + ) + + +@configclass +class ConveyorFrankaA09A12SceneCfg(ConveyorFrankaSceneCfg): + """Checkpoint-compatible Digital Twin scene with visual warehouse dressing.""" + + def __post_init__(self) -> None: + """Replace procedural visuals while retaining the validated physics proxies.""" + super().__post_init__() + + # Raise every inherited env-scoped task component as one rigid + # workspace. The environment reports the same offset as its origin, so + # observations, resets, rewards, and the pretrained policy retain their + # original local coordinates. + for asset in vars(self).values(): + if isinstance(asset, AssetBaseCfg) and asset.prim_path.startswith("{ENV_REGEX_NS}/"): + x, y, z = asset.init_state.pos + asset.init_state.pos = (x, y, z + _WORKSPACE_ELEVATION) + + # The ground is global rather than env-scoped and remains at the USD + # convention's default elevation. + self.ground = _ElevatedGroundPlaneCfg( + prim_path="/World/GroundPlane", + terrain_type="plane", + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.055, 0.065, 0.082), roughness=0.82), + ) + self.dome_light.spawn.color = (0.70, 0.78, 0.92) + self.dome_light.spawn.intensity = 1850.0 + + left_x = BELT_CENTER_X - BELT_HALF_STRAIGHT + right_x = BELT_CENTER_X + BELT_HALF_STRAIGHT + straight_scale = (_A09_X_SCALE, _ASSET_XY_SCALE, _ASSET_Z_SCALE) + turn_scale = (_ASSET_XY_SCALE, _ASSET_XY_SCALE, _ASSET_Z_SCALE) + + # The Franka is fixed at the elevated workspace origin and does not need + # support collision. Replace the temporary plinth with the purpose-built + # Thor table: its mount stays at the robot base and its feet land on z=0. + self.tabletop = _visual_usd_asset( + prim_path="{ENV_REGEX_NS}/RobotThorTableVisual", + usd_path=_THOR_TABLE_ASSET_PATH, + position=(0.0, 0.0, _WORKSPACE_ELEVATION), + scale=(_THOR_TABLE_SCALE,) * 3, + ) + self.table_pedestal = None + + for side in ("Left", "Right"): + side_key = side.lower() + center_y = BELT_CENTER_Y if side == "Left" else -BELT_CENTER_Y + + # The Digital Twin pieces already render their belt, frame, and + # guides, so remove only the procedural render geometry. Hidden + # belt and guide collision assets created above stay authoritative. + setattr(self, f"conveyor_{side_key}_belt_visual", None) + setattr(self, f"guard_{side_key}_inner_visual", None) + setattr(self, f"guard_{side_key}_outer_visual", None) + + for run, y_position in ( + ("top", center_y + BELT_TURN_RADIUS), + ("bottom", center_y - BELT_TURN_RADIUS), + ): + setattr( + self, + f"conveyor_{side_key}_{run}_a09_visual", + _visual_usd_asset( + prim_path=f"{{ENV_REGEX_NS}}/Conveyor{side}{run.title()}A09Visual", + usd_path=_A09_ASSET_PATH, + position=(right_x, y_position, _ASSET_ROOT_Z), + scale=straight_scale, + ), + ) + + # A12 starts at one end of its diameter and bends toward local +X. + # The right piece uses its authored orientation. Rotating the left + # piece by 180 degrees produces the opposite semicircle without a + # negative scale or mirrored geometry. + setattr( + self, + f"conveyor_{side_key}_right_a12_visual", + _visual_usd_asset( + prim_path=f"{{ENV_REGEX_NS}}/Conveyor{side}RightA12Visual", + usd_path=_A12_ASSET_PATH, + position=(right_x, center_y + BELT_TURN_RADIUS, _ASSET_ROOT_Z), + scale=turn_scale, + ), + ) + setattr( + self, + f"conveyor_{side_key}_left_a12_visual", + _visual_usd_asset( + prim_path=f"{{ENV_REGEX_NS}}/Conveyor{side}LeftA12Visual", + usd_path=_A12_ASSET_PATH, + position=(left_x, center_y - BELT_TURN_RADIUS, _ASSET_ROOT_Z), + scale=turn_scale, + rotation=(0.0, 0.0, 1.0, 0.0), + ), + ) + + # Warehouse props provide scale and context but are intentionally + # presentation-only. Their placement stays behind the robot and outside + # the manipulation workspace. + self.packing_station_visual = _visual_usd_asset( + prim_path="{ENV_REGEX_NS}/PackingStationVisual", + usd_path=_PACKING_TABLE_ASSET_PATH, + position=(-1.05, -1.42, _GROUND_PLANE_Z), + scale=(0.42, 0.42, 0.42), + rotation=(0.0, 0.0, 0.70710678, 0.70710678), + ) + self.loaded_pallet_visual = _visual_usd_asset( + prim_path="{ENV_REGEX_NS}/LoadedPalletVisual", + usd_path=_LOADED_PALLET_ASSET_PATH, + position=(-1.08, 1.22, _GROUND_PLANE_Z), + scale=(0.56, 0.56, 0.56), + rotation=(0.0, 0.0, -0.25881905, 0.96592583), + ) + self.empty_pallet_visual = _visual_usd_asset( + prim_path="{ENV_REGEX_NS}/EmptyPalletVisual", + usd_path=_PALLET_ASSET_PATH, + position=(0.00, 1.72, _GROUND_PLANE_Z), + scale=(0.58, 0.58, 0.58), + rotation=(0.0, 0.0, 0.13052619, 0.99144486), + ) + + # A low-detail wall and safety-zone markings frame the high-detail USD + # assets without importing a full warehouse stage or adding collision. + self.warehouse_back_wall_visual = _visual_cuboid( + prim_path="{ENV_REGEX_NS}/WarehouseBackWallVisual", + size=(0.06, 4.4, 2.0), + position=(-1.72, 0.0, 1.0), + color=_BACKDROP_COLOR, + roughness=0.82, + ) + self.warehouse_side_wall_visual = _visual_cuboid( + prim_path="{ENV_REGEX_NS}/WarehouseSideWallVisual", + size=(5.3, 0.06, 2.0), + position=(0.93, 2.18, 1.0), + color=_BACKDROP_COLOR, + roughness=0.82, + ) + for index, y_position in enumerate((-1.75, -0.58, 0.58, 1.75)): + setattr( + self, + f"warehouse_wall_column_{index}_visual", + _visual_cuboid( + prim_path=f"{{ENV_REGEX_NS}}/WarehouseWallColumn{index}Visual", + size=(0.09, 0.08, 2.08), + position=(-1.66, y_position, 1.04), + color=_BACKDROP_ACCENT_COLOR, + roughness=0.55, + metallic=0.35, + ), + ) + for index, x_position in enumerate((-1.62, -0.48, 0.66, 1.80, 2.94)): + setattr( + self, + f"warehouse_side_column_{index}_visual", + _visual_cuboid( + prim_path=f"{{ENV_REGEX_NS}}/WarehouseSideColumn{index}Visual", + size=(0.08, 0.09, 2.08), + position=(x_position, 2.12, 1.04), + color=_BACKDROP_ACCENT_COLOR, + roughness=0.55, + metallic=0.35, + ), + ) + for index, (size, position) in enumerate( + ( + ((1.85, 0.025, 0.004), (0.48, 1.02, 0.002)), + ((1.85, 0.025, 0.004), (0.48, -1.02, 0.002)), + ((0.025, 2.065, 0.004), (-0.445, 0.0, 0.002)), + ((0.025, 2.065, 0.004), (1.405, 0.0, 0.002)), + ) + ): + setattr( + self, + f"safety_zone_{index}_visual", + _visual_cuboid( + prim_path=f"{{ENV_REGEX_NS}}/SafetyZone{index}Visual", + size=size, + position=position, + color=_SAFETY_YELLOW, + roughness=0.68, + ), + ) + + +@configclass +class ConveyorFrankaA09A12EnvCfg(ConveyorFrankaEnvCfg): + """Newton presentation variant with Digital Twin visuals and unchanged task physics.""" + + scene: ConveyorFrankaA09A12SceneCfg = ConveyorFrankaA09A12SceneCfg( + num_envs=1, + env_spacing=6.0, + replicate_physics=True, + ) + + def __post_init__(self) -> None: + """Frame the complete presentation scene while retaining all task settings.""" + super().__post_init__() + self.sim.default_visualizer_cfg.eye = (4.10, -3.65, 2.35) + self.sim.default_visualizer_cfg.lookat = (0.80, 0.0, 0.38) diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_asset_terrain.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_asset_terrain.py new file mode 100644 index 000000000000..da6a01614893 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_asset_terrain.py @@ -0,0 +1,24 @@ +# 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 + +"""Terrain-origin support for the asset-rich conveyor playback variant.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +from isaaclab.terrains import TerrainImporter + +if TYPE_CHECKING: + from .conveyor_franka_asset_env_cfg import _ElevatedGroundPlaneCfg + + +class ConveyorFrankaGroundPlaneTerrainImporter(TerrainImporter): + """Plane terrain whose environment origins follow an elevated workspace.""" + + def __init__(self, cfg: _ElevatedGroundPlaneCfg): + """Create the ground plane and translate its policy-facing origins.""" + super().__init__(cfg) + self.env_origins.add_(self.env_origins.new_tensor(cfg.workspace_origin_offset)) diff --git a/source/isaaclab_tasks/test/contrib/test_conveyor_franka_asset_cfg.py b/source/isaaclab_tasks/test/contrib/test_conveyor_franka_asset_cfg.py new file mode 100644 index 000000000000..57f39bd7da3f --- /dev/null +++ b/source/isaaclab_tasks/test/contrib/test_conveyor_franka_asset_cfg.py @@ -0,0 +1,191 @@ +# 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 checks for the Digital Twin conveyor playback scene.""" + +import math +import subprocess +import sys + +import gymnasium as gym + +import isaaclab_tasks # noqa: F401 +from isaaclab_tasks.contrib.conveyor_franka.conveyor_franka_asset_env_cfg import ( + ConveyorFrankaA09A12EnvCfg, + _make_usd_subtree_visual_only, +) +from isaaclab_tasks.contrib.conveyor_franka.conveyor_franka_env_cfg import ConveyorFrankaEnvCfg +from isaaclab_tasks.contrib.conveyor_franka.conveyor_geometry import ( + BELT_CENTER_X, + BELT_CENTER_Y, + BELT_HALF_STRAIGHT, + BELT_TOP_Z, + BELT_TURN_RADIUS, +) + + +def test_a09_a12_play_task_reuses_the_newton_policy_contract() -> None: + """The visual task is registered as a Play variant with unchanged policy-facing config.""" + task = gym.spec("IsaacContrib-Conveyor-Franka-Newton-Play-v0") + cfg = ConveyorFrankaA09A12EnvCfg() + base_cfg = ConveyorFrankaEnvCfg() + + assert task.kwargs["env_cfg_entry_point"].endswith(":ConveyorFrankaA09A12EnvCfg") + assert task.id.replace("-Play", "") == "IsaacContrib-Conveyor-Franka-Newton-v0" + assert cfg.scene.num_envs == 1 + assert cfg.actions == base_cfg.actions + assert cfg.observations == base_cfg.observations + assert cfg.commands == base_cfg.commands + assert cfg.events == base_cfg.events + assert cfg.rewards == base_cfg.rewards + assert cfg.terminations == base_cfg.terminations + assert cfg.decimation == base_cfg.decimation + assert cfg.sim.dt == base_cfg.sim.dt + + +def test_a09_a12_config_import_does_not_preload_usd() -> None: + """Task discovery must not import USD before a requested Kit application starts.""" + code = ( + "import sys; " + "import isaaclab_tasks.contrib.conveyor_franka.conveyor_franka_asset_env_cfg; " + "raise SystemExit('pxr' in sys.modules)" + ) + result = subprocess.run([sys.executable, "-c", code], check=False) + assert result.returncode == 0 + + +def test_visual_only_usd_strips_physics_and_execution_metadata() -> None: + """Decorative references cannot introduce bodies, contacts, joints, or action graphs.""" + from pxr import Usd, UsdPhysics + + stage = Usd.Stage.CreateInMemory() + root = stage.DefinePrim("/VisualAsset", "Xform") + body = stage.DefinePrim("/VisualAsset/Body", "Xform") + shape = stage.DefinePrim("/VisualAsset/Body/Shape", "Cube") + joint = UsdPhysics.FixedJoint.Define(stage, "/VisualAsset/Joint").GetPrim() + graph = stage.DefinePrim("/VisualAsset/ActionGraph", "OmniGraph") + physics_scene = UsdPhysics.Scene.Define(stage, "/VisualAsset/PhysicsScene").GetPrim() + + UsdPhysics.ArticulationRootAPI.Apply(root) + UsdPhysics.RigidBodyAPI.Apply(body) + UsdPhysics.MassAPI.Apply(body) + UsdPhysics.CollisionAPI.Apply(shape) + UsdPhysics.MeshCollisionAPI.Apply(shape) + UsdPhysics.FilteredPairsAPI.Apply(shape) + shape.AddAppliedSchema("PhysxCollisionAPI") + + _make_usd_subtree_visual_only(root) + + assert not root.HasAPI(UsdPhysics.ArticulationRootAPI) + assert not body.HasAPI(UsdPhysics.RigidBodyAPI) + assert not body.HasAPI(UsdPhysics.MassAPI) + assert not shape.HasAPI(UsdPhysics.CollisionAPI) + assert not shape.HasAPI(UsdPhysics.MeshCollisionAPI) + assert not shape.HasAPI(UsdPhysics.FilteredPairsAPI) + assert "PhysxCollisionAPI" not in shape.GetAppliedSchemas() + assert not joint.IsActive() + assert not graph.IsActive() + assert not physics_scene.IsActive() + + +def test_digital_twin_assets_replace_only_procedural_render_geometry() -> None: + """A09/A12 visuals coexist with the unchanged lightweight collision proxies.""" + scene = ConveyorFrankaA09A12EnvCfg().scene + + assert scene.conveyor_left_belt_visual is None + assert scene.guard_left_inner_visual is None + assert hasattr(scene, "conveyor_left_top_straight_collision") + assert hasattr(scene, "conveyor_left_right_turn_collision") + assert hasattr(scene, "guard_left_inner_collision") + + asset_names = tuple(name for name in vars(scene) if name.endswith(("_a09_visual", "_a12_visual"))) + assert len(asset_names) == 8 + assert sum(name.endswith("_a09_visual") for name in asset_names) == 4 + assert sum(name.endswith("_a12_visual") for name in asset_names) == 4 + + for name in asset_names: + asset = getattr(scene, name) + assert asset.spawn.usd_path.endswith("ConveyorBelt_A09.usd" if "a09" in name else "ConveyorBelt_A12.usd") + assert asset.spawn.collision_props is None + assert asset.spawn.make_uninstanceable + + +def test_thor_table_is_visual_only_and_all_support_feet_reach_the_ground() -> None: + """The Thor mount and measured conveyor lows sit on the common z=0 floor.""" + cfg = ConveyorFrankaA09A12EnvCfg() + scene = cfg.scene + + assert scene.tabletop.prim_path.endswith("/RobotThorTableVisual") + assert scene.tabletop.spawn.usd_path.endswith("/Props/Mounts/thor_table.usd") + assert scene.tabletop.spawn.collision_props is None + ground_z = 0.0 + assert scene.table_pedestal is None + assert math.isclose(scene.tabletop.init_state.pos[2] - 0.795 * scene.tabletop.spawn.scale[2], ground_z) + + for name in vars(scene): + if name.endswith(("_a09_visual", "_a12_visual")): + assert math.isclose(getattr(scene, name).init_state.pos[2], ground_z) + + assert ground_z == 0.0 + workspace_z = scene.ground.workspace_origin_offset[2] + assert 0.2 < workspace_z < 0.3 + assert math.isclose(scene.robot.init_state.pos[2], workspace_z) + assert math.isclose(scene.cube_0.init_state.pos[2], 0.06 + workspace_z) + base_collision_z = ConveyorFrankaEnvCfg().scene.conveyor_left_top_straight_collision.init_state.pos[2] + assert math.isclose(scene.conveyor_left_top_straight_collision.init_state.pos[2], base_collision_z + workspace_z) + + +def test_warehouse_props_are_render_only_and_pallet_bays_do_not_overlap() -> None: + """Scene dressing stays outside the policy contract and owns no collision.""" + scene = ConveyorFrankaA09A12EnvCfg().scene + assert not any(name.startswith("sorter_") for name in vars(scene)) + + prop_names = ("packing_station_visual", "loaded_pallet_visual", "empty_pallet_visual") + for name in prop_names: + asset = getattr(scene, name) + assert asset.spawn.collision_props is None + assert asset.spawn.make_uninstanceable + + # Measured, conservatively axis-aligned extents after scaling leave a clear + # aisle between the loaded and empty pallet bays even with their rotations. + loaded = scene.loaded_pallet_visual + empty = scene.empty_pallet_visual + loaded_half_extent_x = ( + 0.5 * loaded.spawn.scale[0] * (1.203 * math.cos(math.radians(30.0)) + 0.80281 * math.sin(math.radians(30.0))) + ) + empty_half_extent_x = ( + 0.5 * empty.spawn.scale[0] * (1.213235 * math.cos(math.radians(15.0)) + 0.802298 * math.sin(math.radians(15.0))) + ) + assert loaded.init_state.pos[0] + loaded_half_extent_x < empty.init_state.pos[0] - empty_half_extent_x + + assert scene.ground.terrain_type == "plane" + assert scene.ground.visual_material.diffuse_color == (0.055, 0.065, 0.082) + assert scene.warehouse_back_wall_visual.spawn.collision_props is None + assert scene.warehouse_side_wall_visual.spawn.collision_props is None + assert scene.safety_zone_0_visual.spawn.collision_props is None + + +def test_asset_transforms_match_the_existing_racetrack_surface() -> None: + """Asset endpoints, radius, and belt crown align with the policy's original geometry.""" + scene = ConveyorFrankaA09A12EnvCfg().scene + top = scene.conveyor_left_top_a09_visual + right_turn = scene.conveyor_left_right_a12_visual + left_turn = scene.conveyor_left_left_a12_visual + + assert top.init_state.pos[:2] == (BELT_CENTER_X + BELT_HALF_STRAIGHT, BELT_CENTER_Y + BELT_TURN_RADIUS) + assert right_turn.init_state.pos[:2] == top.init_state.pos[:2] + assert left_turn.init_state.pos[:2] == ( + BELT_CENTER_X - BELT_HALF_STRAIGHT, + BELT_CENTER_Y - BELT_TURN_RADIUS, + ) + assert left_turn.init_state.rot == (0.0, 0.0, 1.0, 0.0) + + a09_length = 4.0 * top.spawn.scale[0] + a12_diameter = 2.9922 * right_turn.spawn.scale[0] + asset_belt_top = right_turn.init_state.pos[2] + 1.78053 * right_turn.spawn.scale[2] + assert math.isclose(a09_length, 2.0 * BELT_HALF_STRAIGHT) + assert math.isclose(a12_diameter, 2.0 * BELT_TURN_RADIUS) + assert math.isclose(right_turn.spawn.scale[2], right_turn.spawn.scale[1]) + assert math.isclose(asset_belt_top, BELT_TOP_Z + scene.ground.workspace_origin_offset[2]) From 6f41cf38c8f0ec23803ccc0238554839c1937d09 Mon Sep 17 00:00:00 2001 From: Maximilian Krause Date: Thu, 3 Sep 2026 07:10:04 +0000 Subject: [PATCH 18/23] Move conveyor surface velocity into physics backends --- docs/source/api/lab/isaaclab.physics.rst | 8 + .../lab_newton/isaaclab_newton.physics.rst | 8 + .../api/lab_physx/isaaclab_physx.physics.rst | 18 ++ .../maximiliank-surface-velocity.minor.rst | 5 + source/isaaclab/isaaclab/physics/__init__.pyi | 3 + .../isaaclab/physics/surface_velocity.py} | 76 ++--- .../test/sim/test_surface_velocity.py} | 23 +- .../maximiliank-conveyor-substep-callback.rst | 1 + .../isaaclab_newton/physics/__init__.pyi | 2 + .../physics/surface_velocity.py} | 278 +++++++----------- .../test/physics/test_surface_velocity.py} | 165 +++++++---- .../maximiliank-surface-velocity.minor.rst | 5 + .../isaaclab_physx/physics/__init__.pyi | 12 + .../physics/surface_velocity.py} | 120 +++----- .../test/sim/test_surface_velocity.py} | 53 ++-- .../maximiliank-conveyor-franka.minor.rst | 4 +- .../contrib/conveyor_franka/README.md | 6 + .../conveyor_franka/conveyor_franka_env.py | 24 +- .../conveyor_franka_physx_env_cfg.py | 15 +- .../conveyor_franka/conveyor_geometry.py | 6 +- 20 files changed, 398 insertions(+), 434 deletions(-) create mode 100644 source/isaaclab/changelog.d/maximiliank-surface-velocity.minor.rst rename source/{isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_belt.py => isaaclab/isaaclab/physics/surface_velocity.py} (66%) rename source/{isaaclab_tasks/test/contrib/test_conveyor_belt.py => isaaclab/test/sim/test_surface_velocity.py} (74%) rename source/{isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_force_driver.py => isaaclab_newton/isaaclab_newton/physics/surface_velocity.py} (80%) rename source/{isaaclab_tasks/test/contrib/test_conveyor_force_driver.py => isaaclab_newton/test/physics/test_surface_velocity.py} (68%) create mode 100644 source/isaaclab_physx/changelog.d/maximiliank-surface-velocity.minor.rst rename source/{isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_physx_surface.py => isaaclab_physx/isaaclab_physx/physics/surface_velocity.py} (82%) rename source/{isaaclab_tasks/test/contrib/test_conveyor_physx_surface.py => isaaclab_physx/test/sim/test_surface_velocity.py} (78%) diff --git a/docs/source/api/lab/isaaclab.physics.rst b/docs/source/api/lab/isaaclab.physics.rst index d747af4083b3..12bfbb7af4b2 100644 --- a/docs/source/api/lab/isaaclab.physics.rst +++ b/docs/source/api/lab/isaaclab.physics.rst @@ -18,6 +18,8 @@ The following classes are part of the public :mod:`isaaclab.physics` API. PhysicsEvent PhysicsManager PhysxAutoCfg + SurfaceVelocitySpec + SurfaceVelocityView .. autoclass:: CallbackHandle :show-inheritance: @@ -34,3 +36,9 @@ The following classes are part of the public :mod:`isaaclab.physics` API. .. autoclass:: PhysxAutoCfg :show-inheritance: + +.. autoclass:: SurfaceVelocitySpec + :show-inheritance: + +.. autoclass:: SurfaceVelocityView + :show-inheritance: diff --git a/docs/source/api/lab_newton/isaaclab_newton.physics.rst b/docs/source/api/lab_newton/isaaclab_newton.physics.rst index cb21aceb62e2..6139fccab209 100644 --- a/docs/source/api/lab_newton/isaaclab_newton.physics.rst +++ b/docs/source/api/lab_newton/isaaclab_newton.physics.rst @@ -34,6 +34,7 @@ KaminoPADMMSolverCfg MPMSolverCfg HydroelasticSDFCfg + SurfaceVelocity .. currentmodule:: isaaclab_newton.physics @@ -148,6 +149,13 @@ Physics Configuration :show-inheritance: :exclude-members: __init__ +Surface Velocity +---------------- + +.. autoclass:: SurfaceVelocity + :members: + :show-inheritance: + Solver Managers --------------- diff --git a/docs/source/api/lab_physx/isaaclab_physx.physics.rst b/docs/source/api/lab_physx/isaaclab_physx.physics.rst index 5a1378d42eb8..416d5ebe73ef 100644 --- a/docs/source/api/lab_physx/isaaclab_physx.physics.rst +++ b/docs/source/api/lab_physx/isaaclab_physx.physics.rst @@ -9,6 +9,8 @@ PhysxManager PhysxCfg + SurfaceVelocity + PhysxSurfaceVelocityTwist .. currentmodule:: isaaclab_physx.physics @@ -27,6 +29,22 @@ Physics Configuration :show-inheritance: :exclude-members: __init__ +Surface Velocity +---------------- + +.. autoclass:: SurfaceVelocity + :members: + :show-inheritance: + +.. autoclass:: PhysxSurfaceVelocityTwist + :members: + +.. autofunction:: apply_surface_velocity_api + +.. autofunction:: compute_surface_velocity_twist + +.. autofunction:: resolve_surface_velocity_paths + Additional Public Classes ------------------------- diff --git a/source/isaaclab/changelog.d/maximiliank-surface-velocity.minor.rst b/source/isaaclab/changelog.d/maximiliank-surface-velocity.minor.rst new file mode 100644 index 000000000000..4043fd658003 --- /dev/null +++ b/source/isaaclab/changelog.d/maximiliank-surface-velocity.minor.rst @@ -0,0 +1,5 @@ +Added +^^^^^ + +* Added backend-neutral surface-velocity descriptions and a tensorized control contract under + ``isaaclab.physics.surface_velocity``. diff --git a/source/isaaclab/isaaclab/physics/__init__.pyi b/source/isaaclab/isaaclab/physics/__init__.pyi index 92d9bfac2ff4..b31b566729f1 100644 --- a/source/isaaclab/isaaclab/physics/__init__.pyi +++ b/source/isaaclab/isaaclab/physics/__init__.pyi @@ -9,7 +9,10 @@ __all__ = [ "PhysicsManager", "PhysicsCfg", "PhysxAutoCfg", + "SurfaceVelocitySpec", + "SurfaceVelocityView", ] from .physics_manager import CallbackHandle, PhysicsEvent, PhysicsManager from .physics_manager_cfg import PhysicsCfg, PhysxAutoCfg +from .surface_velocity import SurfaceVelocitySpec, SurfaceVelocityView diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_belt.py b/source/isaaclab/isaaclab/physics/surface_velocity.py similarity index 66% rename from source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_belt.py rename to source/isaaclab/isaaclab/physics/surface_velocity.py index 23d6a6344179..f00badb6b941 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_belt.py +++ b/source/isaaclab/isaaclab/physics/surface_velocity.py @@ -3,7 +3,7 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Task-local conveyor belt descriptions and shared control interface.""" +"""Backend-neutral surface-velocity descriptions and control interface.""" from __future__ import annotations @@ -17,20 +17,20 @@ 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("Conveyor prim_path must be a non-empty string.") + 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"Conveyor prim_path must be absolute or start with '{_ENV_REGEX_NS}/', got {value!r}.") + 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"Conveyor prim_path supports only a leading '{_ENV_REGEX_NS}' placeholder, got {value!r}.") + 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"Conveyor prim_path must identify a concrete prim without empty components, got {value!r}.") + 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"Conveyor prim_path components must not contain whitespace, got {value!r}.") + raise ValueError(f"Surface prim_path components must not contain whitespace, got {value!r}.") def _validate_scalar(name: str, value: Any) -> float: @@ -38,9 +38,9 @@ def _validate_scalar(name: str, value: Any) -> float: try: result = float(value) except (TypeError, ValueError, OverflowError) as exc: - raise ValueError(f"Conveyor {name} must be finite, got {value!r}.") from exc + raise ValueError(f"Surface {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}.") + raise ValueError(f"Surface {name} must be finite, got {value!r}.") return result @@ -49,17 +49,17 @@ def _validate_vector(name: str, value: tuple[float, ...], length: int, *, nonzer try: result = tuple(float(component) for component in value) except (TypeError, ValueError) as exc: - raise ValueError(f"Conveyor {name} must contain {length} finite values, got {value!r}.") from 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"Conveyor {name} must contain {length} finite values, got {value!r}.") + 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"Conveyor {name} must be non-zero, got {value!r}.") + raise ValueError(f"Surface {name} must be non-zero, got {value!r}.") return result @dataclass(frozen=True, slots=True) -class ConveyorBeltSpec: - """Persistent intent for one static collision surface acting as a conveyor. +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 @@ -82,10 +82,7 @@ class ConveyorBeltSpec: 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 belt traction. - animate_texture: Whether a renderer may scroll a compatible belt texture. - animate_direction: Texture-space animation direction. - animate_scale: Texture-coordinate travel per meter of encoder travel [1/m]. + friction_coefficient: Coulomb limit for synthetic surface traction. """ prim_path: str @@ -98,33 +95,26 @@ class ConveyorBeltSpec: surface_normal: tuple[float, float, float] = (0.0, 0.0, 1.0) contact_threshold: float = 0.997 friction_coefficient: float = 0.7 - animate_texture: bool = False - animate_direction: tuple[float, float] = (1.0, 0.0) - animate_scale: float = 1.0 def __post_init__(self) -> None: """Normalize immutable vectors and validate authored values.""" _validate_prim_path(self.prim_path) - for name in ("enabled", "curved", "animate_texture"): + for name in ("enabled", "curved"): if not isinstance(getattr(self, name), bool): - raise ValueError(f"Conveyor {name} must be a bool, got {getattr(self, name)!r}.") + 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) - animate_scale = _validate_scalar("animate_scale", self.animate_scale) if not 0.0 <= contact_threshold <= 1.0: - raise ValueError(f"Conveyor contact_threshold must be in [0, 1], got {self.contact_threshold!r}.") + raise ValueError(f"Surface contact_threshold must be in [0, 1], got {self.contact_threshold!r}.") if friction_coefficient < 0.0: raise ValueError( - f"Conveyor friction_coefficient must be finite and non-negative, got {self.friction_coefficient!r}." + f"Surface friction_coefficient must be finite and non-negative, got {self.friction_coefficient!r}." ) - if animate_scale < 0.0: - raise ValueError(f"Conveyor animate_scale must be finite and non-negative, got {self.animate_scale!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"Conveyor radius must be finite and positive when provided, got {self.radius!r}.") + 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)) @@ -132,42 +122,36 @@ def __post_init__(self) -> None: object.__setattr__( self, "surface_normal", _validate_vector("surface_normal", self.surface_normal, 3, nonzero=True) ) - object.__setattr__( - self, - "animate_direction", - _validate_vector("animate_direction", self.animate_direction, 2, nonzero=self.animate_texture), - ) object.__setattr__(self, "radius", radius) object.__setattr__(self, "contact_threshold", contact_threshold) object.__setattr__(self, "friction_coefficient", friction_coefficient) - object.__setattr__(self, "animate_scale", animate_scale) @runtime_checkable -class ConveyorBeltView(Protocol): - """Common tensorized control contract implemented by both task backends.""" +class SurfaceVelocityView(Protocol): + """Common tensorized control contract implemented by physics backends.""" @property def prim_paths(self) -> tuple[str, ...]: - """Resolved collision prim paths in stable belt-index order.""" + """Resolved collision prim paths in stable surface-index order.""" ... @property - def num_belts(self) -> int: - """Number of resolved conveyor surfaces.""" + def num_surfaces(self) -> int: + """Number of resolved moving surfaces.""" ... @property def count(self) -> int: - """Alias for :attr:`num_belts`, matching tensor-view naming.""" + """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 belts.""" + """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 belts.""" + """Return effective surface velocities [m/s] for selected surfaces.""" ... def get_commanded_velocities(self, indices: Any = None, clone: bool = True) -> Any: @@ -175,15 +159,15 @@ def get_commanded_velocities(self, indices: Any = None, clone: bool = True) -> A ... def set_enabled(self, flags: Any, indices: Any = None) -> None: - """Enable or disable selected belts without discarding their commands.""" + """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 belts.""" + """Return integer enabled flags for selected surfaces.""" ... def get_encoder_positions(self, indices: Any = None, clone: bool = True) -> Any: - """Return integrated belt travel [m] for selected belts.""" + """Return integrated surface travel [m] for selected surfaces.""" ... def reset(self, env_ids: Any = None) -> None: diff --git a/source/isaaclab_tasks/test/contrib/test_conveyor_belt.py b/source/isaaclab/test/sim/test_surface_velocity.py similarity index 74% rename from source/isaaclab_tasks/test/contrib/test_conveyor_belt.py rename to source/isaaclab/test/sim/test_surface_velocity.py index c8e33366cbe0..a043d3a6cfab 100644 --- a/source/isaaclab_tasks/test/contrib/test_conveyor_belt.py +++ b/source/isaaclab/test/sim/test_surface_velocity.py @@ -3,18 +3,18 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Tests for the conveyor-Franka task's shared belt contract.""" +"""Focused tests for backend-neutral surface-velocity descriptions.""" from __future__ import annotations import pytest -from isaaclab_tasks.contrib.conveyor_franka.conveyor_belt import ConveyorBeltSpec +from isaaclab.physics import SurfaceVelocitySpec -def test_conveyor_belt_spec_preserves_authored_semantics() -> None: +def test_surface_velocity_spec_preserves_authored_semantics() -> None: """The shared description carries schema-aligned fields without backend imports.""" - spec = ConveyorBeltSpec( + spec = SurfaceVelocitySpec( prim_path="{ENV_REGEX_NS}/Belt/Curve", velocity=-0.35, enabled=False, @@ -25,9 +25,6 @@ def test_conveyor_belt_spec_preserves_authored_semantics() -> None: surface_normal=(0, 0, 1), contact_threshold=0.997, friction_coefficient=0.5, - animate_texture=True, - animate_direction=(1, 0), - animate_scale=0.5, ) assert spec.prim_path == "{ENV_REGEX_NS}/Belt/Curve" @@ -40,9 +37,6 @@ def test_conveyor_belt_spec_preserves_authored_semantics() -> None: assert spec.surface_normal == (0.0, 0.0, 1.0) assert spec.contact_threshold == 0.997 assert spec.friction_coefficient == 0.5 - assert spec.animate_texture is True - assert spec.animate_direction == (1.0, 0.0) - assert spec.animate_scale == 0.5 @pytest.mark.parametrize( @@ -64,14 +58,9 @@ def test_conveyor_belt_spec_preserves_authored_semantics() -> None: ({"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": "/World/Belt", "animate_scale": -1.0}, "animate_scale"), - ( - {"prim_path": "/World/Belt", "animate_texture": True, "animate_direction": (0.0, 0.0)}, - "animate_direction", - ), ], ) -def test_conveyor_belt_spec_rejects_invalid_authored_values(kwargs: dict, message: str) -> None: +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): - ConveyorBeltSpec(**kwargs) + SurfaceVelocitySpec(**kwargs) diff --git a/source/isaaclab_newton/changelog.d/maximiliank-conveyor-substep-callback.rst b/source/isaaclab_newton/changelog.d/maximiliank-conveyor-substep-callback.rst index 4d6a292e70b8..8ec1ac4466f3 100644 --- a/source/isaaclab_newton/changelog.d/maximiliank-conveyor-substep-callback.rst +++ b/source/isaaclab_newton/changelog.d/maximiliank-conveyor-substep-callback.rst @@ -3,3 +3,4 @@ Added * Added lifecycle-safe Newton manager callbacks for binding model-specific resources before CUDA graph capture and applying contact-force feedback after each solver substep. +* Added reusable, batched surface-velocity contact forces under ``isaaclab_newton.physics.surface_velocity``. diff --git a/source/isaaclab_newton/isaaclab_newton/physics/__init__.pyi b/source/isaaclab_newton/isaaclab_newton/physics/__init__.pyi index 5edd6ce555aa..ef64fbcff5a7 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/__init__.pyi +++ b/source/isaaclab_newton/isaaclab_newton/physics/__init__.pyi @@ -27,6 +27,7 @@ __all__ = [ "NewtonShapeCfg", "NewtonSoftContactCfg", "NewtonSolverCfg", + "SurfaceVelocity", "NewtonVBDManager", "VBDSolverCfg", "NewtonXPBDManager", @@ -59,6 +60,7 @@ from .newton_manager_cfg import ( NewtonSoftContactCfg, NewtonSolverCfg, ) +from .surface_velocity import SurfaceVelocity from .vbd_manager import NewtonVBDManager from .vbd_manager_cfg import VBDSolverCfg from .xpbd_manager import NewtonXPBDManager diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_force_driver.py b/source/isaaclab_newton/isaaclab_newton/physics/surface_velocity.py similarity index 80% rename from source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_force_driver.py rename to source/isaaclab_newton/isaaclab_newton/physics/surface_velocity.py index d8a00d265196..3c8109bf9ca1 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_force_driver.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/surface_velocity.py @@ -3,7 +3,7 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Batched contact-force conveyor surfaces for Newton physics. +"""Batched contact-force surface velocities for Newton physics. The driver reads solver-reported normal contact forces, computes a Coulomb-limited force that drives each transported body's contact points toward their conveyor velocity fields, and applies @@ -18,11 +18,10 @@ import numpy as np import warp as wp -from isaaclab_newton.physics import NewtonManager -from isaaclab.physics import PhysicsEvent +from isaaclab.physics import PhysicsEvent, SurfaceVelocitySpec -from .conveyor_belt import ConveyorBeltSpec +from .newton_manager import NewtonManager _VELOCITY_FIELD_TYPE_CONSTANT = 0 _VELOCITY_FIELD_TYPE_PIVOT = 1 @@ -522,24 +521,24 @@ def _validate_env_path_format(env_path_format: str) -> None: raise ValueError(f"Conveyor env_path_format must be absolute, got {env_path_format!r}.") -def _validate_newton_belt_specs(belt_specs: Sequence[ConveyorBeltSpec]) -> None: +def _validate_newton_surface_specs(surface_specs: Sequence[SurfaceVelocitySpec]) -> None: """Validate Newton-specific requirements before registering lifecycle callbacks.""" - if not belt_specs: - raise ValueError("At least one conveyor belt specification is required.") - prim_paths = [spec.prim_path for spec in belt_specs] + 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 belt_specs: + 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 ConveyorForceDriver: - """Own a conveyor force pipeline across the Newton simulation lifecycle. +class SurfaceVelocity: + """Own a contact-force surface-velocity pipeline across the Newton lifecycle. The driver is created after the simulation context but before its first reset. It requests solved contact forces before model finalization, then @@ -552,76 +551,62 @@ class ConveyorForceDriver: def __init__( self, num_envs: int, - belt_specs: Sequence[ConveyorBeltSpec] | None = None, - speed: float | None = None, - friction: float | None = None, - normal_threshold: float | None = None, + 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_{}", - transported_body_pattern: str = r"(?:^|/)Cube_?[0-3](?:/|$)", - transported_body_count_per_env: int | None = None, ) -> None: """Register the force pipeline for the next Newton model initialization. Args: num_envs: Number of replicated simulation environments. - belt_specs: Authored conveyor descriptions in stable within-environment order. - speed: Optional initial surface-velocity override [m/s] applied to every belt. - friction: Optional Coulomb-traction override applied to every belt. - normal_threshold: Optional contact-normal alignment override applied to every belt. + 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. - transported_body_pattern: Regular expression selecting bodies that receive traction. - transported_body_count_per_env: Expected selected body count per environment, or ``None``. """ - if belt_specs is None: - raise ValueError("At least one conveyor belt specification is required.") - belt_specs = tuple(belt_specs) - if not belt_specs: - raise ValueError("At least one conveyor belt specification is required.") - if not all(isinstance(spec, ConveyorBeltSpec) for spec in belt_specs): - raise TypeError("Every conveyor belt specification must be a ConveyorBeltSpec.") - _validate_newton_belt_specs(belt_specs) - if num_envs <= 0: - raise ValueError(f"Number of conveyor environments must be positive, got {num_envs}.") - if num_envs > 1 and any(not spec.prim_path.startswith("{ENV_REGEX_NS}/") for spec in belt_specs): + 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 speed is not None and not np.isfinite(speed): - raise ValueError(f"Conveyor speed must be finite, got {speed}.") - if friction is not None and (not np.isfinite(friction) or friction < 0.0): - raise ValueError(f"Conveyor friction must be non-negative, got {friction}.") - if normal_threshold is not None and (not np.isfinite(normal_threshold) or not 0.0 <= normal_threshold <= 1.0): - raise ValueError(f"Conveyor normal threshold must be in [0, 1], got {normal_threshold}.") 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(transported_body_pattern) + re.compile(body_pattern) except re.error as exc: - raise ValueError(f"Invalid transported-body pattern: {transported_body_pattern!r}.") from exc - if transported_body_count_per_env is not None and transported_body_count_per_env < 0: - raise ValueError("Expected transported-body count must be non-negative or None.") - self._binding: _ConveyorForceBinding | None = None + 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._belt_specs = belt_specs + self._surface_specs = surface_specs self._binding_kwargs = { "num_envs": num_envs, - "belt_specs": self._belt_specs, - "speed": speed, - "friction": friction, - "normal_threshold": normal_threshold, + "surface_specs": self._surface_specs, "startup_duration_s": startup_duration_s, "env_path_format": env_path_format, - "transported_body_pattern": transported_body_pattern, - "transported_body_count_per_env": transported_body_count_per_env, + "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="conveyor_force_contact_attribute", + name="surface_velocity_contact_attribute", ) try: NewtonManager.register_solver_init_callback(self._bind_solver) @@ -629,32 +614,32 @@ def __init__( self._model_init_handle.deregister() raise - def _require_binding(self) -> _ConveyorForceBinding: + def _require_binding(self) -> _SurfaceVelocityBinding: """Return the current binding or fail before solver initialization.""" binding = self._binding if binding is None: - raise RuntimeError("The conveyor force driver is not bound to an initialized Newton solver.") + raise RuntimeError("Surface velocity is not bound to an initialized Newton solver.") return binding @property - def specs(self) -> tuple[ConveyorBeltSpec, ...]: - """Authored belt descriptions in stable within-environment order.""" - return self._belt_specs + def specs(self) -> tuple[SurfaceVelocitySpec, ...]: + """Authored surface descriptions in stable within-environment order.""" + return self._surface_specs @property - def belts_per_env(self) -> int: - """Number of authored belts in each replicated environment.""" - return len(self._belt_specs) + def surfaces_per_env(self) -> int: + """Number of authored surfaces in each replicated environment.""" + return len(self._surface_specs) @property - def num_belts(self) -> int: - """Total number of resolved belts across all environments.""" - return self._num_envs * self.belts_per_env + 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_belts`, matching tensor-view naming.""" - return self.num_belts + """Alias for :attr:`num_surfaces`, matching tensor-view naming.""" + return self.num_surfaces @property def initialized(self) -> bool: @@ -691,22 +676,6 @@ 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 set_friction_coefficients(self, coefficients: Any, indices: Any = None) -> None: - """Set Coulomb traction limits for selected surfaces.""" - self._require_binding().set_friction_coefficients(coefficients, indices) - - def get_friction_coefficients(self, indices: Any = None, clone: bool = True) -> wp.array: - """Return Coulomb traction limits for selected surfaces.""" - return self._require_binding().get_friction_coefficients(indices, clone) - - def set_contact_processing_thresholds(self, thresholds: Any, indices: Any = None) -> None: - """Set minimum contact-normal alignment for selected surfaces.""" - self._require_binding().set_contact_processing_thresholds(thresholds, indices) - - def get_contact_processing_thresholds(self, indices: Any = None, clone: bool = True) -> wp.array: - """Return contact-normal alignment thresholds for selected surfaces.""" - return self._require_binding().get_contact_processing_thresholds(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) @@ -727,19 +696,15 @@ def _bind_solver(self, model: Any, contacts: Any) -> None: settings = ( previous._command_velocity_host.copy(), previous._enabled_host.copy(), - previous._friction_host.copy(), - previous._threshold_host.copy(), ) previous.close() self._binding = None - binding = _ConveyorForceBinding(model=model, contacts=contacts, **self._binding_kwargs) + binding = _SurfaceVelocityBinding(model=model, contacts=contacts, **self._binding_kwargs) if settings is not None: - velocities, enabled, friction, thresholds = settings + velocities, enabled = settings binding.set_velocities(velocities) binding.set_enabled(enabled) - binding.set_friction_coefficients(friction) - binding.set_contact_processing_thresholds(thresholds) self._binding = binding def close(self) -> None: @@ -754,7 +719,7 @@ def close(self) -> None: self._closed = True -class _ConveyorForceBinding: +class _SurfaceVelocityBinding: """Run one batched moving-surface force pipeline for one Newton model.""" def __init__( @@ -762,14 +727,12 @@ def __init__( model: Any, contacts: Any, num_envs: int, - belt_specs: Sequence[ConveyorBeltSpec], - speed: float | None = None, - friction: float | None = None, - normal_threshold: float | None = None, + 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_{}", - transported_body_pattern: str = r"(?:^|/)Cube_?[0-3](?:/|$)", - transported_body_count_per_env: int | None = None, ) -> None: """Initialize the binding before Newton CUDA graph capture. @@ -777,33 +740,24 @@ def __init__( model: Finalized Newton model owned by the active solver. contacts: Contact buffer owned by the active solver. num_envs: Number of replicated simulation environments. - belt_specs: Authored conveyor descriptions in stable within-environment order. - speed: Optional initial surface-velocity override [m/s] applied to every belt. - friction: Optional Coulomb-traction override applied to every belt. - normal_threshold: Optional contact-normal alignment override applied to every belt. + 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. - transported_body_pattern: Regular expression selecting bodies that receive traction. - transported_body_count_per_env: Expected selected body count per environment, or ``None``. """ - if num_envs <= 0: - raise ValueError(f"Number of conveyor environments must be positive, got {num_envs}.") - if speed is not None and not np.isfinite(speed): - raise ValueError(f"Conveyor speed must be finite, got {speed}.") - if friction is not None and (not np.isfinite(friction) or friction < 0.0): - raise ValueError(f"Conveyor friction must be non-negative, got {friction}.") - if normal_threshold is not None and (not np.isfinite(normal_threshold) or not 0.0 <= normal_threshold <= 1.0): - raise ValueError(f"Conveyor normal threshold must be in [0, 1], got {normal_threshold}.") + 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._belt_specs = tuple(belt_specs) - _validate_newton_belt_specs(self._belt_specs) + self._surface_specs = tuple(surface_specs) + _validate_newton_surface_specs(self._surface_specs) try: - body_pattern = re.compile(transported_body_pattern) + compiled_body_pattern = re.compile(body_pattern) except re.error as exc: - raise ValueError(f"Invalid transported-body pattern: {transported_body_pattern!r}.") from 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.") @@ -823,15 +777,15 @@ def __init__( self._closed = False self._validate_backend_buffers() - belts_per_env = len(self._belt_specs) - conveyor_count = num_envs * belts_per_env + surfaces_per_env = len(self._surface_specs) + conveyor_count = num_envs * surfaces_per_env shape_conveyor = [-1] * model.shape_count field_type = [0] * 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 // belts_per_env for conveyor_id 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() @@ -844,7 +798,7 @@ def __init__( continue matching_specs = [ index - for index, spec in enumerate(self._belt_specs) + 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: @@ -859,12 +813,12 @@ def __init__( if section_key in seen_sections: raise RuntimeError( f"World {world_id} contains multiple shapes matching conveyor section " - f"{self._belt_specs[spec_id].prim_path!r}." + f"{self._surface_specs[spec_id].prim_path!r}." ) seen_sections.add(section_key) - spec = self._belt_specs[spec_id] - conveyor_id = world_id * belts_per_env + spec_id + spec = self._surface_specs[spec_id] + conveyor_id = world_id * surfaces_per_env + spec_id shape_conveyor[shape_id] = conveyor_id field_type[conveyor_id] = _VELOCITY_FIELD_TYPE_PIVOT if spec.curved else _VELOCITY_FIELD_TYPE_CONSTANT direction[conveyor_id] = _world_vector(shape_transform[shape_id], spec.direction) @@ -874,12 +828,13 @@ def __init__( 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._belt_specs)) + (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._belt_specs[spec_id].prim_path}" for world_id, spec_id in missing_sections[:8] + 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}).") @@ -887,7 +842,7 @@ def __init__( 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 body_pattern.search(label) is None: + if compiled_body_pattern.search(label) is None: continue world_id = int(body_world[body_id]) if not 0 <= world_id < num_envs: @@ -895,16 +850,15 @@ def __init__( body_is_tracked[body_id] = 1 tracked_counts[world_id] += 1 - if transported_body_count_per_env is not None: - bad_worlds = np.flatnonzero(tracked_counts != transported_body_count_per_env) + 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"Transported-body pattern {transported_body_pattern!r} expected " - f"{transported_body_count_per_env} bodies per world ({details})." + 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"Transported-body pattern {transported_body_pattern!r} matched no Newton bodies.") + raise RuntimeError(f"Body pattern {body_pattern!r} matched no Newton bodies.") self._surface_paths = tuple(surface_paths) self._shape_conveyor = wp.array(shape_conveyor, dtype=wp.int32, device=self._device) @@ -916,25 +870,17 @@ def __init__( self._surface_normal = wp.array(surface_normal, dtype=wp.vec3, 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._belt_specs], dtype=np.float32) - authored_enabled = np.asarray([spec.enabled for spec in self._belt_specs], dtype=np.int32) - authored_friction = np.asarray([spec.friction_coefficient for spec in self._belt_specs], dtype=np.float32) - authored_threshold = np.asarray([spec.contact_threshold for spec in self._belt_specs], dtype=np.float32) + 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) + authored_friction = np.asarray([spec.friction_coefficient for spec in self._surface_specs], dtype=np.float32) + authored_threshold = np.asarray([spec.contact_threshold for spec in self._surface_specs], dtype=np.float32) self._command_velocity_host = np.tile(authored_velocity, num_envs) self._enabled_host = np.tile(authored_enabled, num_envs) - self._friction_host = np.tile(authored_friction, num_envs) - self._threshold_host = np.tile(authored_threshold, num_envs) - if speed is not None: - self._command_velocity_host.fill(speed) - if friction is not None: - self._friction_host.fill(friction) - if normal_threshold is not None: - self._threshold_host.fill(normal_threshold) 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._friction = wp.array(self._friction_host, dtype=wp.float32, device=self._device) - self._threshold = wp.array(self._threshold_host, dtype=wp.float32, device=self._device) + self._friction = wp.array(np.tile(authored_friction, num_envs), dtype=wp.float32, device=self._device) + self._threshold = wp.array(np.tile(authored_threshold, num_envs), 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._velocity_scale = wp.zeros(1, dtype=wp.float32, device=self._device) @@ -977,7 +923,9 @@ def get_commanded_velocities(self, indices: Any = None, clone: bool = True) -> w 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").astype(np.bool_) + 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() @@ -986,32 +934,6 @@ 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 set_friction_coefficients(self, coefficients: Any, indices: Any = None) -> None: - """Set Coulomb traction limits for selected surfaces.""" - selected = self._resolve_indices(indices) - values = self._broadcast_1d(coefficients, len(selected), "friction coefficients") - if np.any(values < 0.0): - raise ValueError("Conveyor friction coefficients must be non-negative.") - self._friction_host[selected] = values - self._friction.assign(self._friction_host) - - def get_friction_coefficients(self, indices: Any = None, clone: bool = True) -> wp.array: - """Return Coulomb traction limits for selected surfaces.""" - return self._get_device_values(self._friction, indices, clone) - - def set_contact_processing_thresholds(self, thresholds: Any, indices: Any = None) -> None: - """Set minimum contact-normal alignment for selected surfaces.""" - selected = self._resolve_indices(indices) - values = self._broadcast_1d(thresholds, len(selected), "contact thresholds") - if np.any((values < 0.0) | (values > 1.0)): - raise ValueError("Conveyor contact thresholds must lie in [0, 1].") - self._threshold_host[selected] = values - self._threshold.assign(self._threshold_host) - - def get_contact_processing_thresholds(self, indices: Any = None, clone: bool = True) -> wp.array: - """Return contact-normal alignment thresholds for selected surfaces.""" - return self._get_device_values(self._threshold, 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) @@ -1032,7 +954,10 @@ def reset(self, env_ids: Any = None) -> None: self._velocity_scale.zero_() return - ids = np.asarray(_as_numpy(env_ids), dtype=np.int64).reshape(-1) + 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) @@ -1183,10 +1108,19 @@ def _validate_backend_buffers(self) -> None: _require_buffer_length("contacts.rigid_contact_count", contacts.rigid_contact_count, 1) def _resolve_indices(self, indices: Any) -> np.ndarray: - """Normalize and validate a conveyor index selection.""" + """Normalize and validate a surface index selection.""" if indices is None: return np.arange(len(self._surface_paths), dtype=np.int64) - selected = np.asarray(_as_numpy(indices), dtype=np.int64).reshape(-1) + 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 diff --git a/source/isaaclab_tasks/test/contrib/test_conveyor_force_driver.py b/source/isaaclab_newton/test/physics/test_surface_velocity.py similarity index 68% rename from source/isaaclab_tasks/test/contrib/test_conveyor_force_driver.py rename to source/isaaclab_newton/test/physics/test_surface_velocity.py index 022a1b123d62..728ddac3df98 100644 --- a/source/isaaclab_tasks/test/contrib/test_conveyor_force_driver.py +++ b/source/isaaclab_newton/test/physics/test_surface_velocity.py @@ -3,25 +3,63 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Lifecycle tests for the Newton conveyor force driver.""" +"""Lifecycle tests for Newton surface velocity.""" from __future__ import annotations from types import SimpleNamespace +import isaaclab_newton.physics.surface_velocity as surface_module import numpy as np import pytest import warp as wp +from isaaclab_newton.physics.surface_velocity import compute_point_impulse + +from isaaclab.physics import PhysicsEvent, SurfaceVelocitySpec + +_BODY_PATTERN = r"(?:^|/)Cube_?[0-3](?:/|$)" + + +@wp.kernel +def _compute_test_impulses( + target_velocity: wp.array(dtype=wp.vec3), + normal_impulse: wp.array(dtype=wp.float32), + output: wp.array(dtype=wp.vec3), +): + index = wp.tid() + output[index] = compute_point_impulse( + wp.vec3(0.0, 0.0, 1.0), + normal_impulse[index], + wp.vec3(), + target_velocity[index], + 1.0, + wp.mat33(), + wp.vec3(), + 0.5, + 1.0, + ) -from isaaclab.physics import PhysicsEvent -import isaaclab_tasks.contrib.conveyor_franka.conveyor_force_driver as driver_module -from isaaclab_tasks.contrib.conveyor_franka.conveyor_belt import ConveyorBeltSpec +def _surface_spec(name: str = "Belt") -> SurfaceVelocitySpec: + """Build one valid replicated test belt.""" + return SurfaceVelocitySpec(prim_path=f"{{ENV_REGEX_NS}}/{name}", velocity=0.35, friction_coefficient=0.5) -def _belt_spec(name: str = "Belt") -> ConveyorBeltSpec: - """Build one valid replicated test belt.""" - return ConveyorBeltSpec(prim_path=f"{{ENV_REGEX_NS}}/{name}", velocity=0.35, friction_coefficient=0.5) +def test_point_impulse_tracks_velocity_and_respects_coulomb_limit() -> None: + """Point traction reaches a small target but clamps large requests to ``mu * normal_impulse``.""" + target_velocity = wp.array([(0.25, 0.0, 0.0), (10.0, 0.0, 0.0)], dtype=wp.vec3, device="cpu") + normal_impulse = wp.array([2.0, 2.0], dtype=wp.float32, device="cpu") + output = wp.zeros(2, dtype=wp.vec3, device="cpu") + + wp.launch( + _compute_test_impulses, + dim=2, + inputs=[target_velocity, normal_impulse], + outputs=[output], + device="cpu", + ) + + np.testing.assert_allclose(output.numpy(), ((0.25, 0.0, 0.0), (1.0, 0.0, 0.0)), atol=1.0e-6) class _FakeCallbackHandle: @@ -42,8 +80,6 @@ def __init__(self, model, contacts, **kwargs) -> None: self.closed = False self._command_velocity_host = np.array([0.35, 0.35], dtype=np.float32) self._enabled_host = np.ones(2, dtype=np.int32) - self._friction_host = np.full(2, 0.5, dtype=np.float32) - self._threshold_host = np.full(2, 0.997, dtype=np.float32) type(self).instances.append(self) def set_velocities(self, values) -> None: @@ -52,12 +88,6 @@ def set_velocities(self, values) -> None: def set_enabled(self, values) -> None: self._enabled_host = np.asarray(values, dtype=np.int32).copy() - def set_friction_coefficients(self, values) -> None: - self._friction_host = np.asarray(values, dtype=np.float32).copy() - - def set_contact_processing_thresholds(self, values) -> None: - self._threshold_host = np.asarray(values, dtype=np.float32).copy() - def close(self) -> None: self.closed = True @@ -75,34 +105,34 @@ def register_callback(cls, callback, event, order=0, name=None, wrap_weak_ref=Tr event_callbacks.append((callback, event, name)) return callback_handle - monkeypatch.setattr(driver_module.NewtonManager, "register_callback", classmethod(register_callback)) + monkeypatch.setattr(surface_module.NewtonManager, "register_callback", classmethod(register_callback)) monkeypatch.setattr( - driver_module.NewtonManager, + surface_module.NewtonManager, "register_solver_init_callback", classmethod(lambda cls, callback: solver_callbacks.append(callback)), ) monkeypatch.setattr( - driver_module.NewtonManager, + surface_module.NewtonManager, "unregister_solver_init_callback", classmethod(lambda cls, callback: unregistered_solver_callbacks.append(callback)), ) monkeypatch.setattr( - driver_module.NewtonManager, + surface_module.NewtonManager, "request_extended_contact_attribute", classmethod(lambda cls, attribute: requested_attributes.append(attribute)), ) - monkeypatch.setattr(driver_module, "_ConveyorForceBinding", _FakeBinding) + monkeypatch.setattr(surface_module, "_SurfaceVelocityBinding", _FakeBinding) - driver = driver_module.ConveyorForceDriver(num_envs=2, belt_specs=(_belt_spec(),)) + driver = surface_module.SurfaceVelocity(num_envs=2, surface_specs=(_surface_spec(),), body_pattern=_BODY_PATTERN) - assert driver.specs == (_belt_spec(),) - assert driver.belts_per_env == 1 - assert driver.num_belts == 2 + assert driver.specs == (_surface_spec(),) + assert driver.surfaces_per_env == 1 + assert driver.num_surfaces == 2 assert driver.count == 2 assert not driver.initialized assert [(event, name) for _, event, name in event_callbacks] == [ - (PhysicsEvent.MODEL_INIT, "conveyor_force_contact_attribute") + (PhysicsEvent.MODEL_INIT, "surface_velocity_contact_attribute") ] event_callbacks[0][0](None) assert requested_attributes == ["force"] @@ -113,8 +143,6 @@ def register_callback(cls, callback, event, order=0, name=None, wrap_weak_ref=Tr assert driver.initialized first_binding.set_velocities([0.2, -0.1]) first_binding.set_enabled([1, 0]) - first_binding.set_friction_coefficients([0.4, 0.6]) - first_binding.set_contact_processing_thresholds([0.98, 0.99]) second_model, second_contacts = object(), object() solver_callbacks[0](second_model, second_contacts) @@ -125,8 +153,6 @@ def register_callback(cls, callback, event, order=0, name=None, wrap_weak_ref=Tr assert second_binding.contacts is second_contacts np.testing.assert_allclose(second_binding._command_velocity_host, [0.2, -0.1]) np.testing.assert_array_equal(second_binding._enabled_host, [1, 0]) - np.testing.assert_allclose(second_binding._friction_host, [0.4, 0.6]) - np.testing.assert_allclose(second_binding._threshold_host, [0.98, 0.99]) driver.close() driver.close() @@ -139,22 +165,22 @@ def test_unbound_driver_rejects_control_calls(monkeypatch: pytest.MonkeyPatch) - """Control methods are unavailable until the solver-init callback creates a binding.""" callback_handle = _FakeCallbackHandle() monkeypatch.setattr( - driver_module.NewtonManager, + surface_module.NewtonManager, "register_callback", classmethod(lambda cls, *args, **kwargs: callback_handle), ) monkeypatch.setattr( - driver_module.NewtonManager, + surface_module.NewtonManager, "register_solver_init_callback", classmethod(lambda cls, callback: None), ) monkeypatch.setattr( - driver_module.NewtonManager, + surface_module.NewtonManager, "unregister_solver_init_callback", classmethod(lambda cls, callback: None), ) - driver = driver_module.ConveyorForceDriver(num_envs=1, belt_specs=(_belt_spec(),)) + driver = surface_module.SurfaceVelocity(num_envs=1, surface_specs=(_surface_spec(),), body_pattern=_BODY_PATTERN) with pytest.raises(RuntimeError, match="not bound"): driver.set_velocities(0.2) driver.close() @@ -164,49 +190,59 @@ def test_driver_rejects_invalid_specs_before_registering_callbacks(monkeypatch: """Invalid descriptions cannot leave lifecycle callbacks behind.""" registered = [] monkeypatch.setattr( - driver_module.NewtonManager, + surface_module.NewtonManager, "register_callback", classmethod(lambda cls, *args, **kwargs: registered.append(args)), ) with pytest.raises(ValueError, match="At least one"): - driver_module.ConveyorForceDriver(num_envs=1, belt_specs=()) - with pytest.raises(TypeError, match="ConveyorBeltSpec"): - driver_module.ConveyorForceDriver(num_envs=1, belt_specs=(object(),)) + surface_module.SurfaceVelocity(num_envs=1, surface_specs=(), body_pattern=_BODY_PATTERN) + with pytest.raises(TypeError, match="SurfaceVelocitySpec"): + surface_module.SurfaceVelocity(num_envs=1, surface_specs=(object(),), body_pattern=_BODY_PATTERN) with pytest.raises(ValueError, match="unique"): - driver_module.ConveyorForceDriver(num_envs=1, belt_specs=(_belt_spec(), _belt_spec())) + surface_module.SurfaceVelocity( + num_envs=1, surface_specs=(_surface_spec(), _surface_spec()), body_pattern=_BODY_PATTERN + ) with pytest.raises(ValueError, match="ancestors"): - driver_module.ConveyorForceDriver( + surface_module.SurfaceVelocity( num_envs=1, - belt_specs=( - ConveyorBeltSpec(prim_path="{ENV_REGEX_NS}/Belt"), - ConveyorBeltSpec(prim_path="{ENV_REGEX_NS}/Belt/Child"), + surface_specs=( + SurfaceVelocitySpec(prim_path="{ENV_REGEX_NS}/Belt"), + SurfaceVelocitySpec(prim_path="{ENV_REGEX_NS}/Belt/Child"), ), + body_pattern=_BODY_PATTERN, ) with pytest.raises(ValueError, match="explicit positive radius"): - driver_module.ConveyorForceDriver( + surface_module.SurfaceVelocity( num_envs=1, - belt_specs=(ConveyorBeltSpec(prim_path="{ENV_REGEX_NS}/Curve", curved=True),), + surface_specs=(SurfaceVelocitySpec(prim_path="{ENV_REGEX_NS}/Curve", curved=True),), + body_pattern=_BODY_PATTERN, ) with pytest.raises(ValueError, match="Replicated conveyor environments"): - driver_module.ConveyorForceDriver( + surface_module.SurfaceVelocity( num_envs=2, - belt_specs=(ConveyorBeltSpec(prim_path="/World/Shared/Belt"),), + surface_specs=(SurfaceVelocitySpec(prim_path="/World/Shared/Belt"),), + body_pattern=_BODY_PATTERN, ) with pytest.raises(ValueError, match="env_path_format"): - driver_module.ConveyorForceDriver(num_envs=1, belt_specs=(_belt_spec(),), env_path_format="/World/envs/env_.*") + surface_module.SurfaceVelocity( + num_envs=1, + surface_specs=(_surface_spec(),), + body_pattern=_BODY_PATTERN, + env_path_format="/World/envs/env_.*", + ) assert registered == [] def test_belt_paths_are_exact_and_environment_scoped() -> None: """A descriptor cannot bind a same-named shape outside the replicated environment root.""" - resolve = driver_module._resolve_belt_prim_path + resolve = surface_module._resolve_belt_prim_path assert resolve("{ENV_REGEX_NS}/Belt", "/World/envs/env_{}", 0) == "/World/envs/env_0/Belt" assert resolve("{ENV_REGEX_NS}/Nested/Belt", "/World/envs/env_{}", 123) == ("/World/envs/env_123/Nested/Belt") assert resolve("/World/Shared/Belt", "/World/envs/env_{}", 7) == "/World/Shared/Belt" - belongs = driver_module._shape_belongs_to_prim + belongs = surface_module._shape_belongs_to_prim assert belongs("/World/envs/env_0/Belt/geometry/mesh", "/World/envs/env_0/Belt") assert not belongs("/World/props/Belt/geometry/mesh", "/World/envs/env_0/Belt") assert not belongs("/World/envs/env_0/Nested/Belt", "/World/envs/env_0/Belt") @@ -216,7 +252,7 @@ def test_driver_cleans_up_model_callback_when_solver_registration_fails(monkeypa """A lifecycle registration failure cannot leave a partially active driver.""" callback_handle = _FakeCallbackHandle() monkeypatch.setattr( - driver_module.NewtonManager, + surface_module.NewtonManager, "register_callback", classmethod(lambda cls, *args, **kwargs: callback_handle), ) @@ -225,13 +261,13 @@ def fail_registration(cls, callback): raise RuntimeError("solver callback unavailable") monkeypatch.setattr( - driver_module.NewtonManager, + surface_module.NewtonManager, "register_solver_init_callback", classmethod(fail_registration), ) with pytest.raises(RuntimeError, match="solver callback unavailable"): - driver_module.ConveyorForceDriver(num_envs=1, belt_specs=(_belt_spec(),)) + surface_module.SurfaceVelocity(num_envs=1, surface_specs=(_surface_spec(),), body_pattern=_BODY_PATTERN) assert callback_handle.deregister_count == 1 @@ -239,22 +275,22 @@ def fail_registration(cls, callback): def test_binding_uses_deterministic_environment_major_belt_indices(monkeypatch: pytest.MonkeyPatch) -> None: """Newton discovery order cannot reorder commands or encoder rows after a rebuild.""" monkeypatch.setattr( - driver_module.NewtonManager, + surface_module.NewtonManager, "register_state_force_callback", classmethod(lambda cls, callback: None), ) monkeypatch.setattr( - driver_module.NewtonManager, + surface_module.NewtonManager, "register_post_solver_substep_callback", classmethod(lambda cls, callback: None), ) monkeypatch.setattr( - driver_module.NewtonManager, + surface_module.NewtonManager, "unregister_state_force_callback", classmethod(lambda cls, callback: None), ) monkeypatch.setattr( - driver_module.NewtonManager, + surface_module.NewtonManager, "unregister_post_solver_substep_callback", classmethod(lambda cls, callback: None), ) @@ -294,13 +330,13 @@ def test_binding_uses_deterministic_environment_major_belt_indices(monkeypatch: rigid_contact_count=wp.zeros(1, dtype=wp.int32, device="cpu"), ) specs = ( - ConveyorBeltSpec( + SurfaceVelocitySpec( prim_path="{ENV_REGEX_NS}/BeltA", velocity=0.1, friction_coefficient=0.4, contact_threshold=0.98, ), - ConveyorBeltSpec( + SurfaceVelocitySpec( prim_path="{ENV_REGEX_NS}/BeltB", velocity=-0.2, enabled=False, @@ -309,12 +345,13 @@ def test_binding_uses_deterministic_environment_major_belt_indices(monkeypatch: ), ) - binding = driver_module._ConveyorForceBinding( + binding = surface_module._SurfaceVelocityBinding( model=model, contacts=contacts, num_envs=2, - belt_specs=specs, - transported_body_count_per_env=1, + surface_specs=specs, + body_pattern=_BODY_PATTERN, + body_count_per_env=1, ) try: assert binding.surface_paths == ( @@ -327,10 +364,8 @@ def test_binding_uses_deterministic_environment_major_belt_indices(monkeypatch: np.testing.assert_array_equal(binding._conveyor_world.numpy(), [0, 0, 1, 1]) np.testing.assert_allclose(binding._command_velocity_host, [0.1, -0.2, 0.1, -0.2]) np.testing.assert_array_equal(binding._enabled_host, [1, 0, 1, 0]) - np.testing.assert_allclose(binding._friction_host, [0.4, 0.6, 0.4, 0.6]) - np.testing.assert_allclose(binding._threshold_host, [0.98, 0.99, 0.98, 0.99]) + np.testing.assert_allclose(binding._friction.numpy(), [0.4, 0.6, 0.4, 0.6]) + np.testing.assert_allclose(binding._threshold.numpy(), [0.98, 0.99, 0.98, 0.99]) np.testing.assert_array_equal(binding.get_enabled(indices=[3, 0]).numpy(), [0, 1]) - np.testing.assert_allclose(binding.get_friction_coefficients(indices=[1, 2]).numpy(), [0.6, 0.4]) - np.testing.assert_allclose(binding.get_contact_processing_thresholds(indices=[2, 1]).numpy(), [0.98, 0.99]) finally: binding.close() diff --git a/source/isaaclab_physx/changelog.d/maximiliank-surface-velocity.minor.rst b/source/isaaclab_physx/changelog.d/maximiliank-surface-velocity.minor.rst new file mode 100644 index 000000000000..33debf44ed07 --- /dev/null +++ b/source/isaaclab_physx/changelog.d/maximiliank-surface-velocity.minor.rst @@ -0,0 +1,5 @@ +Added +^^^^^ + +* Added native PhysX surface-velocity schema authoring and runtime control under + ``isaaclab_physx.physics.surface_velocity``. diff --git a/source/isaaclab_physx/isaaclab_physx/physics/__init__.pyi b/source/isaaclab_physx/isaaclab_physx/physics/__init__.pyi index 9eeb559c2927..6515e487eb1d 100644 --- a/source/isaaclab_physx/isaaclab_physx/physics/__init__.pyi +++ b/source/isaaclab_physx/isaaclab_physx/physics/__init__.pyi @@ -7,7 +7,19 @@ __all__ = [ "PhysxManager", "IsaacEvents", "PhysxCfg", + "PhysxSurfaceVelocityTwist", + "SurfaceVelocity", + "apply_surface_velocity_api", + "compute_surface_velocity_twist", + "resolve_surface_velocity_paths", ] from .physx_manager import PhysxManager, IsaacEvents from .physx_manager_cfg import PhysxCfg +from .surface_velocity import ( + PhysxSurfaceVelocityTwist, + SurfaceVelocity, + apply_surface_velocity_api, + compute_surface_velocity_twist, + resolve_surface_velocity_paths, +) diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_physx_surface.py b/source/isaaclab_physx/isaaclab_physx/physics/surface_velocity.py similarity index 82% rename from source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_physx_surface.py rename to source/isaaclab_physx/isaaclab_physx/physics/surface_velocity.py index 7424de7ff7f4..68b382594ae1 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_physx_surface.py +++ b/source/isaaclab_physx/isaaclab_physx/physics/surface_velocity.py @@ -3,7 +3,7 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""CPU-only native PhysX surface-velocity conveyors for the conveyor-Franka task. +"""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 @@ -22,7 +22,7 @@ import numpy as np -from .conveyor_belt import ConveyorBeltSpec +from isaaclab.physics import SurfaceVelocitySpec _ENV_REGEX_NS = "{ENV_REGEX_NS}" @@ -40,15 +40,15 @@ class PhysxSurfaceVelocityTwist: angular_velocity_deg: tuple[float, float, float] -def compute_physx_surface_velocity_twist( - spec: ConveyorBeltSpec, velocity: float | None = None +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:`ConveyorBeltSpec.pivot_point`. + to move the instantaneous center to :attr:`SurfaceVelocitySpec.pivot_point`. Args: spec: Authored conveyor intent in the collision prim's local frame. @@ -83,9 +83,9 @@ def compute_physx_surface_velocity_twist( ) -def apply_physx_surface_velocity_api( +def apply_surface_velocity_api( prim_or_path: Any, - spec: ConveyorBeltSpec, + spec: SurfaceVelocitySpec, *, velocity_scale: float = 0.0, stage: Any | None = None, @@ -109,7 +109,7 @@ def apply_physx_surface_velocity_api( ValueError: If ``velocity_scale`` is not finite. """ scale = _finite_float("velocity_scale", velocity_scale) - twist = compute_physx_surface_velocity_twist(spec, velocity=spec.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) @@ -117,29 +117,29 @@ def apply_physx_surface_velocity_api( binding.close() -def resolve_physx_conveyor_paths( +def resolve_surface_velocity_paths( num_envs: int, - belt_specs: Sequence[ConveyorBeltSpec], + 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. - belt_specs: Within-environment belt descriptions. + surface_specs: Within-environment surface descriptions. env_path_format: Exact environment path format containing one ``{}`` field. Returns: - Exact paths ordered by environment, then by ``belt_specs`` order. + 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(belt_specs) - if not specs or not all(isinstance(spec, ConveyorBeltSpec) for spec in specs): - raise ValueError("Conveyor belt_specs must contain at least one ConveyorBeltSpec.") + 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: @@ -161,11 +161,11 @@ def resolve_physx_conveyor_paths( return paths -class PhysxSurfaceVelocityConveyor: +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_physx_surface_velocity_api`. Call :meth:`start` to register physics-rate updates, + :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. @@ -174,7 +174,7 @@ class PhysxSurfaceVelocityConveyor: def __init__( self, num_envs: int, - belt_specs: Sequence[ConveyorBeltSpec], + surface_specs: Sequence[SurfaceVelocitySpec], *, env_path_format: str = "/World/envs/env_{}", startup_duration_s: float = 1.0, @@ -185,7 +185,7 @@ def __init__( Args: num_envs: Number of replicated environments. - belt_specs: Within-environment belt descriptions. + 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. @@ -196,9 +196,9 @@ def __init__( 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._belt_specs = tuple(belt_specs) - self._surface_paths = resolve_physx_conveyor_paths(num_envs, self._belt_specs, env_path_format) - self._belts_per_env = len(self._belt_specs) + 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 @@ -211,28 +211,24 @@ def __init__( ) self._command_velocity = np.tile( - np.asarray([spec.velocity for spec in self._belt_specs], dtype=np.float32), self._num_envs + 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._belt_specs], dtype=np.bool_), self._num_envs) - self._friction = np.tile( - np.asarray([spec.friction_coefficient for spec in self._belt_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._threshold = np.tile( - np.asarray([spec.contact_threshold for spec in self._belt_specs], dtype=np.float32), self._num_envs - ) - self._encoder_position = np.zeros(self.num_belts, dtype=np.float32) - self._last_authored: list[tuple[bool, PhysxSurfaceVelocityTwist] | None] = [None] * self.num_belts + 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[ConveyorBeltSpec, ...]: + def specs(self) -> tuple[SurfaceVelocitySpec, ...]: """Return authored descriptions in stable within-environment order.""" - return self._belt_specs + return self._surface_specs @property - def belts_per_env(self) -> int: - """Return the number of authored belts per environment.""" - return self._belts_per_env + 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, ...]: @@ -245,14 +241,14 @@ def surface_paths(self) -> tuple[str, ...]: return self.prim_paths @property - def num_belts(self) -> int: + 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_belts`.""" - return self.num_belts + """Return an alias for :attr:`num_surfaces`.""" + return self.num_surfaces @property def initialized(self) -> bool: @@ -264,12 +260,12 @@ def start(self) -> None: self._require_open() if self._callback_handle is not None: return - from isaaclab_physx.physics import IsaacEvents, PhysxManager + from .physx_manager import IsaacEvents, PhysxManager self._callback_handle = PhysxManager.register_callback( self.update, IsaacEvents.POST_PHYSICS_STEP, - name="physx_conveyor_surface_velocity", + name="physx_surface_velocity", ) def update(self, dt: float) -> None: @@ -321,28 +317,6 @@ 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 set_friction_coefficients(self, coefficients: Any, indices: Any = None) -> None: - """Reject unsupported runtime friction mutation explicitly.""" - del coefficients, indices - raise NotImplementedError( - "Native PhysX surface velocity uses authored collision materials; mutate material friction explicitly." - ) - - def get_friction_coefficients(self, indices: Any = None, clone: bool = True) -> np.ndarray: - """Return authored friction metadata retained for control introspection.""" - return self._get_values(self._friction, indices, clone) - - def set_contact_processing_thresholds(self, thresholds: Any, indices: Any = None) -> None: - """Reject unsupported normal-threshold mutation explicitly.""" - del thresholds, indices - raise NotImplementedError( - "PhysxSurfaceVelocityAPI has no contact-normal threshold; the native body-level field affects all contacts." - ) - - def get_contact_processing_thresholds(self, indices: Any = None, clone: bool = True) -> np.ndarray: - """Return authored threshold metadata retained for control introspection.""" - return self._get_values(self._threshold, 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) @@ -358,7 +332,7 @@ def reset(self, env_ids: Any = None) -> None: """ self._require_open() ids = self._resolve_env_ids(env_ids) - rows = (ids[:, None] * self._belts_per_env + np.arange(self._belts_per_env)[None, :]).reshape(-1) + 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 @@ -373,7 +347,7 @@ def close(self) -> 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_belts): + for index in range(self.num_surfaces): self._writer.write(index, enabled=False, twist=zero) self._writer.close() self._closed = True @@ -383,9 +357,9 @@ def _flush(self, indices: Any = None, *, force: bool = False) -> None: selected = self._resolve_indices(indices) for index in selected: enabled = bool(self._enabled[index]) - spec = self._belt_specs[index % self._belts_per_env] + 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_physx_surface_velocity_twist(spec, velocity=speed) + 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) @@ -395,18 +369,18 @@ 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_belts, dtype=np.int64) + return np.arange(self.num_surfaces, dtype=np.int64) if isinstance(indices, slice): - return np.arange(self.num_belts, dtype=np.int64)[indices] + 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_belts: - raise IndexError(f"Boolean conveyor indices must have length {self.num_belts}.") + 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_belts)): + if np.any((selected < 0) | (selected >= self.num_surfaces)): raise IndexError(f"Conveyor surface indices are out of range: {selected.tolist()}.") return selected @@ -506,7 +480,7 @@ def __init__(self, prims_or_paths: Sequence[Any], *, stage: Any | None, apply_ap else: raise RuntimeError( f"PhysX conveyor prim {prim.GetPath()} has no authored PhysxSurfaceVelocityAPI; " - "call apply_physx_surface_velocity_api from its spawner before simulation starts." + "call apply_surface_velocity_api from its spawner before simulation starts." ) surface_api.CreateSurfaceVelocityLocalSpaceAttr().Set(True) self._attributes.append( diff --git a/source/isaaclab_tasks/test/contrib/test_conveyor_physx_surface.py b/source/isaaclab_physx/test/sim/test_surface_velocity.py similarity index 78% rename from source/isaaclab_tasks/test/contrib/test_conveyor_physx_surface.py rename to source/isaaclab_physx/test/sim/test_surface_velocity.py index c59f5183f04c..1bde98d6f559 100644 --- a/source/isaaclab_tasks/test/contrib/test_conveyor_physx_surface.py +++ b/source/isaaclab_physx/test/sim/test_surface_velocity.py @@ -3,7 +3,7 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Import-light tests for the task-local PhysX conveyor surface backend.""" +"""Import-light tests for native PhysX surface velocity.""" from __future__ import annotations @@ -11,12 +11,12 @@ import sys from types import ModuleType +import isaaclab_physx.physics.surface_velocity as surface_module import numpy as np import pytest import torch -import isaaclab_tasks.contrib.conveyor_franka.conveyor_physx_surface as surface_module -from isaaclab_tasks.contrib.conveyor_franka.conveyor_belt import ConveyorBeltSpec +from isaaclab.physics import SurfaceVelocitySpec class _FakeWriter: @@ -33,16 +33,16 @@ def close(self) -> None: self.close_count += 1 -def _belt_spec(name: str = "Belt", **kwargs) -> ConveyorBeltSpec: +def _surface_spec(name: str = "Belt", **kwargs) -> SurfaceVelocitySpec: """Return one replicated belt description for facade tests.""" - return ConveyorBeltSpec(prim_path=f"{{ENV_REGEX_NS}}/{name}", velocity=0.4, **kwargs) + 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 = _belt_spec(direction=(3.0, 4.0, 0.0)) + spec = _surface_spec(direction=(3.0, 4.0, 0.0)) - twist = surface_module.compute_physx_surface_velocity_twist(spec, velocity=2.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) @@ -50,14 +50,14 @@ def test_twist_conversion_normalizes_straight_direction() -> None: 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 = _belt_spec( + 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_physx_surface_velocity_twist(spec, velocity=math.pi) + 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) @@ -70,17 +70,17 @@ def test_twist_conversion_uses_degrees_and_compensates_curved_pivot() -> None: def test_curved_twist_requires_an_explicit_radius() -> None: """A native angular rate cannot be inferred from unspecified task geometry.""" - spec = _belt_spec(curved=True) + spec = _surface_spec(curved=True) with pytest.raises(ValueError, match="positive radius"): - surface_module.compute_physx_surface_velocity_twist(spec) + 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 = (_belt_spec("BeltA"), _belt_spec("Nested/BeltB")) + specs = (_surface_spec("BeltA"), _surface_spec("Nested/BeltB")) - paths = surface_module.resolve_physx_conveyor_paths(2, specs) + paths = surface_module.resolve_surface_velocity_paths(2, specs) assert paths == ( "/World/envs/env_0/BeltA", @@ -89,16 +89,16 @@ def test_paths_are_resolved_in_environment_major_order() -> None: "/World/envs/env_1/Nested/BeltB", ) with pytest.raises(ValueError, match="require every belt"): - surface_module.resolve_physx_conveyor_paths(2, (ConveyorBeltSpec(prim_path="/World/Shared/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.PhysxSurfaceVelocityConveyor(2, (_belt_spec(),), writer=writer) + 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_belts == facade.count == 2 + 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) @@ -126,26 +126,9 @@ def test_facade_ramps_playback_integrates_encoders_and_preserves_commands_on_res assert [(index, enabled) for index, enabled, _ in writer.writes[-2:]] == [(0, False), (1, False)] -def test_facade_rejects_unrepresentable_runtime_mutations() -> None: - """PhysX metadata getters cannot imply that unsupported setters took effect.""" - facade = surface_module.PhysxSurfaceVelocityConveyor( - 1, - (_belt_spec(friction_coefficient=0.55, contact_threshold=0.98),), - writer=_FakeWriter(), - ) - - np.testing.assert_allclose(facade.get_friction_coefficients(), (0.55,)) - np.testing.assert_allclose(facade.get_contact_processing_thresholds(), (0.98,)) - with pytest.raises(NotImplementedError, match="material friction"): - facade.set_friction_coefficients(0.8) - with pytest.raises(NotImplementedError, match="no contact-normal threshold"): - facade.set_contact_processing_thresholds(0.9) - facade.close() - - def test_facade_accepts_torch_control_and_reset_indices() -> None: """Normal Isaac Lab tensor selectors are copied to host before NumPy validation.""" - facade = surface_module.PhysxSurfaceVelocityConveyor(2, (_belt_spec(),), writer=_FakeWriter()) + facade = surface_module.SurfaceVelocity(2, (_surface_spec(),), writer=_FakeWriter()) device = "cuda" if torch.cuda.is_available() else "cpu" facade.set_velocities(torch.tensor([0.6], device=device), indices=torch.tensor([1], device=device)) @@ -234,7 +217,7 @@ def CreateSurfaceAngularVelocityAttr(self) -> FakeAttribute: monkeypatch.setitem(sys.modules, "pxr", fake_pxr) prim = FakePrim() - surface_module.apply_physx_surface_velocity_api(prim, _belt_spec(), velocity_scale=0.0) + surface_module.apply_surface_velocity_api(prim, _surface_spec(), velocity_scale=0.0) assert FakeRigidBodyAPI in prim.apis assert FakeSurfaceAPI in prim.apis diff --git a/source/isaaclab_tasks/changelog.d/maximiliank-conveyor-franka.minor.rst b/source/isaaclab_tasks/changelog.d/maximiliank-conveyor-franka.minor.rst index 33edec2474ee..583ae22b78e9 100644 --- a/source/isaaclab_tasks/changelog.d/maximiliank-conveyor-franka.minor.rst +++ b/source/isaaclab_tasks/changelog.d/maximiliank-conveyor-franka.minor.rst @@ -4,8 +4,8 @@ Added * Added a contributed manager-based environment with guarded, counter-rotating force-driven racetrack conveyors, robust primitive and closed-mesh belt colliders, a MuJoCo Menagerie Franka, and an interactive Newton-viewer cube-goal selector. -* Added task-local, schema-aligned conveyor descriptions and a tensorized control view while retaining a single, - kitless Newton force owner with CUDA-graph and hard-reset-safe lifecycle binding. +* Used the reusable surface-velocity physics interfaces while retaining a single, kitless Newton force owner with + CUDA-graph and hard-reset-safe lifecycle binding. * Added a checkpoint-compatible Newton Play variant rendered with A09/A12 functional-loop visuals, a render-only Thor robot table, packing station, pallet bays, and warehouse dressing while retaining the task's lightweight collision and traction surfaces. diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/README.md b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/README.md index 6543b468843b..05abc3b44238 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/README.md +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/README.md @@ -29,6 +29,12 @@ The two backends deliberately share the policy tensor contract, so an RSL-RL che loaded by either task without reshaping or reordering tensors. Their contact and actuator dynamics are not numerically identical; validate task behavior when transferring a policy between them. +Surface-velocity intent and the tensorized control contract live in +`isaaclab.physics.surface_velocity`. Backend mechanics are separate: Newton's solved-contact force +pipeline lives in `isaaclab_newton.physics.surface_velocity`, while PhysX schema authoring and live +attribute control live in `isaaclab_physx.physics.surface_velocity`. The task package owns only the +racetrack geometry, backend lifecycle selection, and task-level commands. + ## Newton GPU playback Newton is kitless and supports the lightweight GL viewer: diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env.py index dc23cccb91b5..2511a8cc152d 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_env.py @@ -10,13 +10,11 @@ from collections.abc import Sequence from isaaclab.envs import ManagerBasedRLEnv +from isaaclab.physics import SurfaceVelocityView -from .conveyor_belt import ConveyorBeltView -from .conveyor_force_driver import ConveyorForceDriver from .conveyor_franka_env_cfg import ConveyorFrankaEnvCfg from .conveyor_geometry import belt_collision_section_specs from .conveyor_goal_selector import ConveyorGoalSelector -from .conveyor_physx_surface import PhysxSurfaceVelocityConveyor class ConveyorFrankaEnv(ManagerBasedRLEnv): @@ -25,7 +23,7 @@ class ConveyorFrankaEnv(ManagerBasedRLEnv): cfg: ConveyorFrankaEnvCfg def __init__(self, cfg: ConveyorFrankaEnvCfg, render_mode: str | None = None, **kwargs): - self._conveyor_driver: ConveyorBeltView | None = None + self._conveyor_driver: SurfaceVelocityView | None = None super().__init__(cfg, render_mode=render_mode, **kwargs) self._goal_selector: ConveyorGoalSelector | None = None self._setup_goal_selector() @@ -52,16 +50,16 @@ def _init_sim(self) -> None: # before the first reset finalizes and captures the solver. PhysX belt # schemas, by contrast, are authored by the scene spawners and its live # command adapter is attached only after PhysX has parsed that scene. - from isaaclab_newton.physics import NewtonCfg + from isaaclab_newton.physics import NewtonCfg, SurfaceVelocity if isinstance(self.cfg.sim.physics, NewtonCfg): - driver = ConveyorForceDriver( + driver = SurfaceVelocity( num_envs=self.cfg.scene.num_envs, - belt_specs=belt_specs, + surface_specs=belt_specs, startup_duration_s=self.cfg.conveyor_force.startup_duration_s, env_path_format=env_path_format, - transported_body_pattern=self.cfg.conveyor_force.transported_body_pattern, - transported_body_count_per_env=self.cfg.conveyor_force.transported_body_count_per_env, + body_pattern=self.cfg.conveyor_force.transported_body_pattern, + body_count_per_env=self.cfg.conveyor_force.transported_body_count_per_env, ) self._conveyor_driver = driver try: @@ -72,7 +70,7 @@ def _init_sim(self) -> None: raise return - from isaaclab_physx.physics import PhysxCfg + from isaaclab_physx.physics import PhysxCfg, SurfaceVelocity if not isinstance(self.cfg.sim.physics, PhysxCfg): raise ValueError(f"Unsupported conveyor physics backend: {type(self.cfg.sim.physics).__name__}.") @@ -81,9 +79,9 @@ def _init_sim(self) -> None: if configure_conveyor is not None: configure_conveyor(friction_coefficient=self.cfg.conveyor_force.friction) super()._init_sim() - driver = PhysxSurfaceVelocityConveyor( + driver = SurfaceVelocity( num_envs=self.cfg.scene.num_envs, - belt_specs=belt_specs, + surface_specs=belt_specs, env_path_format=env_path_format, startup_duration_s=self.cfg.conveyor_force.startup_duration_s, stage=self.sim.stage, @@ -96,7 +94,7 @@ def _init_sim(self) -> None: self._conveyor_driver = driver @property - def conveyor_belt(self) -> ConveyorBeltView: + def conveyor_belt(self) -> SurfaceVelocityView: """Tensorized conveyor control view for this environment.""" driver = self._conveyor_driver if driver is None: diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_physx_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_physx_env_cfg.py index 4f806567710f..122f9b374bc2 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_physx_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_physx_env_cfg.py @@ -10,17 +10,17 @@ import functools from dataclasses import replace -from isaaclab_physx.physics import PhysxCfg +from isaaclab_physx.physics import PhysxCfg, apply_surface_velocity_api from isaaclab_physx.sim.schemas import PhysxCollisionCfg, PhysxSDFMeshCfg from isaaclab_physx.sim.spawners.materials import PhysxRigidBodyMaterialCfg import isaaclab.sim as sim_utils from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg +from isaaclab.physics import SurfaceVelocitySpec from isaaclab.sim import SimulationCfg from isaaclab.sim.schemas import CollisionFragment, UsdPhysicsCollisionCfg from isaaclab.utils.configclass import configclass -from .conveyor_belt import ConveyorBeltSpec from .conveyor_franka_env_cfg import ( _CONTACT_GAP, _CUBE_CONTACT_MARGIN, @@ -46,7 +46,6 @@ belt_mesh_spec, guard_mesh_specs, ) -from .conveyor_physx_surface import apply_physx_surface_velocity_api from .franka_robot_cfg import FRANKA_PANDA_CONVEYOR_PHYSX_CFG _PHYSX_DYNAMIC_PROPERTIES = sim_utils.RigidBodyBaseCfg() @@ -173,13 +172,13 @@ def _spawn_physx_conveyor_mesh( translation: tuple[float, float, float] | None = None, orientation: tuple[float, float, float, float] | None = None, *, - belt_spec: ConveyorBeltSpec, + belt_spec: SurfaceVelocitySpec, **kwargs, ): """Spawn one hidden SDF turn and author native surface velocity before PhysX parsing.""" prim = sim_utils.spawn_mesh_custom(prim_path, cfg, translation, orientation, **kwargs) sim_utils.set_prim_visibility(prim, False) - apply_physx_surface_velocity_api(prim, belt_spec, velocity_scale=0.0) + apply_surface_velocity_api(prim, belt_spec, velocity_scale=0.0) return prim @@ -190,13 +189,13 @@ def _spawn_physx_conveyor_cuboid( translation: tuple[float, float, float] | None = None, orientation: tuple[float, float, float, float] | None = None, *, - belt_spec: ConveyorBeltSpec, + belt_spec: SurfaceVelocitySpec, **kwargs, ): """Spawn one hidden analytic straight and author native surface velocity before PhysX parsing.""" prim = sim_utils.spawn_cuboid(prim_path, cfg, translation, orientation, **kwargs) sim_utils.set_prim_visibility(prim, False) - apply_physx_surface_velocity_api(prim, belt_spec, velocity_scale=0.0) + apply_surface_velocity_api(prim, belt_spec, velocity_scale=0.0) return prim @@ -344,7 +343,7 @@ def build_conveyor_belt_specs( velocity: float, friction_coefficient: float, contact_threshold: float, - ) -> tuple[ConveyorBeltSpec, ...]: + ) -> tuple[SurfaceVelocitySpec, ...]: """Return the same pivot-local belt descriptions used by the PhysX spawners.""" return tuple( section.belt diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_geometry.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_geometry.py index 93b2fa71fd96..3a3159c4c3e2 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_geometry.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_geometry.py @@ -10,7 +10,7 @@ import math from dataclasses import dataclass -from .conveyor_belt import ConveyorBeltSpec +from isaaclab.physics import SurfaceVelocitySpec BELT_COLOR = (0.09, 0.09, 0.09) """Dark-rubber color used by Newton's conveyor example.""" @@ -81,7 +81,7 @@ class ConveyorSectionSpec: """Task geometry paired with its backend-neutral conveyor description.""" geometry: MeshSpec | CuboidSpec - belt: ConveyorBeltSpec + belt: SurfaceVelocitySpec def belt_direction(side: str) -> float: @@ -335,7 +335,7 @@ def belt( ) -> ConveyorSectionSpec: return ConveyorSectionSpec( geometry=geometry, - belt=ConveyorBeltSpec( + belt=SurfaceVelocitySpec( prim_path=f"{{ENV_REGEX_NS}}/{geometry.name}", velocity=velocity, enabled=enabled, From baf58aeee4b207526789fad51f92fffbf58ff6b6 Mon Sep 17 00:00:00 2001 From: Maximilian Krause Date: Thu, 24 Sep 2026 12:00:53 +0200 Subject: [PATCH 19/23] Add USD conveyor warehouse sorting and user guide --- .gitignore | 2 + docs/index.rst | 1 + docs/source/_static/conveyor_franka.jpg | Bin 0 -> 146429 bytes .../source/_static/css/environment-browser.js | 3 + docs/source/setup/conveyor_franka.rst | 98 ++++ .../maximiliank-conveyor-franka.minor.rst | 25 +- .../contrib/conveyor_franka/README.md | 130 ++++- .../contrib/conveyor_franka/__init__.py | 5 +- .../assets/conveyor_quarter_supported.usd | 3 + .../assets/conveyor_routes.usda | 3 + .../assets/conveyor_straight_supported.usd | 3 + .../conveyor_franka/assets/parcel.usda | 3 + .../conveyor_franka/assets/parcel_blue.usda | 3 + .../conveyor_franka/assets/parcel_green.usda | 3 + .../conveyor_franka/assets/parcel_orange.usda | 3 + .../conveyor_franka/assets/parcel_purple.usda | 3 + .../conveyor_franka/assets/warehouse.usda | 3 + .../conveyor_franka/conveyor_cube_pool.py | 99 ++++ .../conveyor_franka_asset_env_cfg.py | 328 ++++++------ .../conveyor_franka_asset_terrain.py | 6 + .../conveyor_franka_warehouse_env.py | 151 ++++++ .../conveyor_franka/conveyor_geometry.py | 15 +- .../conveyor_warehouse_geometry.py | 226 ++++++++ .../conveyor_franka/franka_robot_cfg.py | 7 +- .../contrib/conveyor_franka/mdp/__init__.pyi | 2 +- .../contrib/conveyor_franka/mdp/commands.py | 16 +- .../conveyor_franka/mdp/curriculums.py | 2 +- .../conveyor_franka/mdp/observations.py | 24 +- .../contrib/conveyor_franka/mdp/rewards.py | 11 +- .../contrib/conveyor_franka/mdp/sorting.py | 128 +++++ .../conveyor_franka/mdp/terminations.py | 12 +- source/isaaclab_tasks/pyproject.toml | 1 + .../contrib/test_conveyor_franka_asset_cfg.py | 505 +++++++++++++++--- .../contrib/test_conveyor_franka_geometry.py | 58 +- .../test/contrib/test_conveyor_franka_mdp.py | 89 ++- uv.lock | 14 +- 36 files changed, 1657 insertions(+), 328 deletions(-) create mode 100644 docs/source/_static/conveyor_franka.jpg create mode 100644 docs/source/setup/conveyor_franka.rst create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/assets/conveyor_quarter_supported.usd create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/assets/conveyor_routes.usda create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/assets/conveyor_straight_supported.usd create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/assets/parcel.usda create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/assets/parcel_blue.usda create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/assets/parcel_green.usda create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/assets/parcel_orange.usda create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/assets/parcel_purple.usda create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/assets/warehouse.usda create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_cube_pool.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_warehouse_env.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_warehouse_geometry.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/sorting.py diff --git a/.gitignore b/.gitignore index 4bd3675e9df8..bbe4d2cdf505 100644 --- a/.gitignore +++ b/.gitignore @@ -14,6 +14,8 @@ # No USD files allowed in the repo **/*.usd **/*.usda +!source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/assets/*.usda +!source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/assets/*.usd **/*.usdc **/*.usdz diff --git a/docs/index.rst b/docs/index.rst index 8f399b0e8b55..2edd30e1cfe3 100644 --- a/docs/index.rst +++ b/docs/index.rst @@ -91,6 +91,7 @@ Table of Contents source/setup/ecosystem source/setup/installation/index source/setup/environments + source/setup/conveyor_franka source/setup/quickstart source/setup/tutorial source/setup/demos diff --git a/docs/source/_static/conveyor_franka.jpg b/docs/source/_static/conveyor_franka.jpg new file mode 100644 index 0000000000000000000000000000000000000000..7ee6660229669376303f787c9d6110a1b2ad1a78 GIT binary patch literal 146429 zcmb4qQ;=p&uZXZQHhO+qTdA_vOZUJh>w(c170C%&4f? zxiVJP_saJU;HQ+Bq!<7M1ONc}_W-`v0m1+XFmP~iFo=Hx1Ox;m6dW|vKS6|r{Q-xB zh>VPch=hcKj*EeUij9VZgh_;njfY1-K!AclOiGMTii=Nx|DO?%f1g4@LLopyBjBSV zq2m9a<+}%f1PwX^0RRIb27n@gfFXf=4*+lh007i~(f)q{0R;p97bWyRt;SCP2pA|h zC3asD#|^1mE|fq?$!_n(p<2>|-fFBmxV zznudG1qJ)Jng6*ZVuU~zL}5}iKqYqg&*?0)Q19*yBpL}Ri?EX8tziI}h_X@L_Zk2W z?4NlgFeCs!V1qD}umKt#Tp|`EK@=Pj93Vj$1_1vlq5u$4_&2nX{uBV*NQ#B#fJ#!% zNdY8f6LuviL7K@klw)K$0Yrmprc!paJyxeSK22F-G01Zj8L>OiwHv55C9LtI56CBF z=ns)*$qwqB1T+27rlAaF!xAH;R-?`$P}XxE8`S0`jGdSEwN{~ZT_;hynM4N9Y~32$ zRxtg_2=(#h+G}VJGI7f32C6+Z ztVplG0Ud`M`JtH~yt$aYe5w54Djuxjk8)CXQhC1?r0v?OwAK@?M0yM?1IDg&6{DP$ zq8U3p;~*t{1-AxE)A)J&J4wp-%CzSR{F8}52Due+{!W7oXbut&IIzncQdf7s1Ug2K zNsF$q9i&>cZg1r$bQcZx$c70GyLcxM2kQ(AjOIpbY%*&d@%lCPJO#9k-#z_yCtG2K zc+W9^0UvYLSyIN}BZ1~s=HOfpM<=b?q?aFe!fA&I>UM7J25n82PI1BiU7Z^N7g1_F z+tOi4-wM^qs3qHBg7!x#e6jfBIrUr}1ERkHw_gvb z#ZcpP!$M~XNy^NJm^+n4y}}%-fnSJQdGaACS~_M1kD98Mzvrl4Vz(7}NliW7AUgwg z%7$vEQlk-@k}Y}PH8s@+z5!%jS}rIxH3~Zh&Pf)K6N_iC5jz*iq{Z zrOmt^%KX!6^H2uex|u5=i}B)jNV1H0c)1wD8GcSK+Dwxze+2xXbyr=|#&AS~FSo-j zpzJTq-_VQQ9qo+KNDQz;y-E+s?{ML70NwTnFn`LuU9>HYS%Ey)3cML$F++rRJ& z%dlW`;i0&v<nu4FJpaQ$zYlU*^-$pQ^aT$j^(^9CQH}>%j-AIK#7tQg@h0$I=h+wpL!6pqwlou; z=P;1twa?Xk_=O_arf0G{lDG4S-3K;x6JCmwSUj(PFq8j9)$b=%V)keA7c3+V|8g0R z-1MD9#G|AwhE)gxPhZ7CaQ#{(YJRPzmf)kAsHT+r!NcsoPtd_<1PH@)h-n+f*;x;SF<4$lBq-R9(b$bvDB7+ zCjeIx_MBqxenA6YxmifQ^CoN6A(RN7CE=Q?9iEh1uRMzT%V=u3cDJqaCtHe(nZo%G z*8`jzwS=ypI%S)6<@V2Udc{2SlS0?6DfqVUOBEw8mXv37W3x<7LM6eRJ zR(KfKPeapFx{@gych^q+v94cN7@cKAk-{Ssc&bxmhF6S+(b zB^o0#EvIrlmV=vYrDC>mZC2>J<~pHezfE{H_-4N}qC2yq-kG}7P(79CL6OP*z>qhd>-G$z+Wg$|BGlB1mt8F<9x zmwtwEG#tC}U;lwsr~cI1h)WfykESnZxbdiHwES;?ML9d+vYo;y9BD+XxPuhv-m}hv zskOCzdBif}wH!0cK1t*a!7If#0Lab64UlpyLHO4E4k%jr&2JL>6XV(hZ|zsdO~Wy5y?_x5Xxt{ z4|r3;0UK|DOfUH@CSktl&B$(hm&47xb0>CGSKG|(G24O|pfG4(O0mEQjm^rikzH4k zx3oL{3$(frGWDDRTZm_s2Fus7rTnkzqF@z!gD{no4Iyqz+^Tny8IPg8%F!SA8P0h@ zd79s(aREooF?=vbxay$ReT9p3#@&_&@p~!rilV4`dO`PMeb0)w$w@PO8@7;#qL^1d z_g&TdZSRd~9I&a=)Wzk0OW}H?LWGpki55w(6dKFNiep(v!MC)JKnz4|cbh>_pv@vk zR{W9kO~)CW#tF{itBIZs%^sO_ zfy&N$WmVL1ZU9eNwffL~|Q zXNL=OnD@tayrH7B2m>Jvdra0@fQZ|hC?VJC8N49mRwuM&FRZY_VHd#Up ze$oV$1*?;X$FO8>#HjB`OOoBE#CL8W1}q_UPpTBe6_L>os{xANfKa}szfFy6TDUtB zdp1`2AhDI+)mDU%ABr4EqvTI@86ba8L4wPD5PCho$GomOyEQzMtIRR#iITJ%nIk6`tR0ZXB9bOC0&Ii*6mSrrX9ExloskID z^Eq-i!6jxuP+=gD0no8RCji1QZ~zQ(6p{dV7`OyrOj6ug0uU}8SC9Y@Dd40m+E4(+ zRhHu}#!YqD{0GI!-{0oKgLF&Qk{6UJ`PdFpfGOZHYmP=2g?_!gCZp zlwq=6c_2goa#o+=AE^Mkg7To^Yh5!(X;((_Q5zMk+R9&l<9u0e|HbpS-biCxS;t2z zopJ904V1i}EoU8g{*(8dbz+~|hMX(XxF}szT^);UX53Yios5?gok7I62ZKKZZd-xJl#KXNpX{Z4Avxnf%$5gG3qJPS^CY)0#+R8 zWud9)UdjKn zoN?{-c6jJR{bha+oS}WPhQ}L+syZ*VE0M_N)q<*7Ny_a`$+u56QIjqeAw%z^I2=Yz zbx?(}VG=g&WV>=`u!AE@4OWFYiA((cnY|DC8vvnwV7G!T`(FK$hiP49@1}Cp-~hfV zUCI6$s*=}+js53Pc@c!yL@_?qnsI$6$H@JhQ3IS+E-8;@Tcl0cZfB-kSG3@D!v5MS zc;u8YKS^BVs9OW$VbSAoPdkYZG`BJ!3!x}80JFEn3L@y;W-X+(e43gTEP8NW1GjF*ltsuC`>M#emNTjfL+-y-@SE;caB z9QlCa(z_-o%pI?Uf~*1d0M}m-G*nttD^GX=9LXq7Cc?HwIc&Tg`VtCXVt?lE589wLLnAfNM1husy(4Z z7VJa?1eQxCuQtI-c8>r}8S#4ucY^SSf&oqOzsj9Q2kfTX(T7A7JfwE*ipAx0P6$MJ zbX4>%Ti)nT6-!vciczX*;USz2J6Ar-_HqUbXzq{F<(6SK%(7v-er@swCcF! z#W`_02$VBoFA$@^wOK2-MC9@x6W)T>?%Mrd%Q};tXG$cQam2a)cNUDSoNO3U#Pmq8 z{yt=Q@c2eWq>HOf{-ES>@a>`;@#I@gh|bf5Vv_Hqi0ddMB5uVzNk!YKj=pe2b;uGpPHJ>cHmCI;y>usxdedd1EVYtw zrji;OL$?$GrqOC@Vy70>G}zWUvyF4NWpL8yCC0qQED!$cS#?(=sWs{<4~p{&O-ae+ zMwsg?I^^Uj@M($S(Hy zize#fGXp+Jg^}H`iQ-v-MxJl^G>#krK;6BDTEhlaWj1WfmV(3$>}6ZUX*~5udcr5$ ze7%A<%bVBKoZDVXG_&9G+N7RF#j)4Eq5>uegujg>Y$10#bfpAO%fIsI{pDWq^dCxf z{yn1Fei({X9gZmT>fKm$xG>}5WsrFes3$9&Q^ z9UiQdAJVd~kRf{aZOfo*gU_O`hzRt}3FECb_aXRGFie`d%FF_}F2!F6Gv9zCvuB?l zzs4Jq?kTc9pFU6zQBGARHfIAWt!jdmYWSNwFzrSG z+u$gtsNEN=x{SreO#V)$5x*OkR9O|8a{c|x&$VuBwrv(WRY@NmxS+9dKSAEpt>G8{ zL;^W;gC_CJ03h83qRDdO$*}3ehk+3T1&36Q-+;wG9&yUN?D0&LU=bDTpnp`a^m@`m ztUTf?{v>TJ)u^@v>Rd=cXRxu-cN*Iu6==1M=bjIwd`m?p821UgLP<#JA`~ z|DpE^V|QbAZZieJsiKFj6=SKfFcKEm*9o3V6LM1SUTh6v!sOSWB}bvTDS-|Ac;;x} z%zIkg-)ZuwQrP%$$%7Dg-92hh&`}$R`Kuu?__Ms)47%O>kIyjQo~-(O?&#U*DX5&iS;z^X5x=_b-u1dY(3fswd_FL7b113Hao9-DGVu=5a%C~ zE|XPMlsP!(dJ~QNxe!zwBmHhPjmrT+<6qnKd8u0Km_uj{y`7{ zR6u#Ij|IEXA>fyGD-fvXrDUs!?1#JP{G)5wKD|wWS^(YV6Y~A16XtH=Y;Au{!_T1E zGi6|l&`#V8OC7AxoUMpH+tWF-3y82)`NvfZn+ zQTP{WrT2ke%zVgW8aCwlcPH1xVi;I!*!IrnnV+@481x0z3pQgq4OAhj*;-(|i%C-e zf$v_u+d*hPG&hV7Wav?cK=!2I1%J3Qv#u{9c`^S!$w7Ux6a=QIZYMveE0Q`X*3J>k z(!fbuvdEIi-f`2%Gk~sSYQ?0&qO5ytd(dfp6?PL@lSo@xT)@?}-$Q9J?pgaAF#2;U zMe`fbuNwIOX8Isv`EgMB0w*?8n9MQRhjib7Xc6^UgCoyT za3uQOBY`MnXuvG0xVo^~7!bwd?69C9k+|i(PRgC{s5B^5D3yJU^q<)>3z1eeJJx6VQ*8-D2HneTJrcE7wu*+Z_&V#__aL& zYscvx`f#Q}EG!mfz5{Qu^6&_~hdgERaETc=^ck_oBL`ZUhqL#PyMZdV3$H_3?oAup zW-|Hvy;=eSo<%(-+dGkf4K32dY05b7@`BxdYNi+qJ zS=rQw%AdsTbRvuy&5e5B2>4`sro5^!c$@tOWJ(4oocM#I;aK9tZ3^70(|&qMcvtij zct5jmx7Tu*KQ@T%!E#kqu3vf0uQh|v;!qwN-{s_l_Or~n)bUpku1=A%^{u;7!MU^) zxe(5`1>Z@DakF&GdW&YbIZzMhw?>?tt;pxIe&$d|9_-N^baR*LVs%|qUwP48P)&LU z%s)oOSIMKgu{Dm#v$#oMQnvW(%O;$q7)9sK)?tlf3oPKft>8~Rg1(cvJOVD6pFWAx zxqtB`4Z~3d)DF6)sLX2lEmFu5msR z7sXKgdwSX9(&G&lo38i<6=S#b){dY6*DQgTT2xeTL;lQHPc2EU$u?EA3R`WlaX5Df z@I(&}j9>AFfhe^8~ zti0Q15Pgk2uqJ3zQJ9V5`^lslVzpRI6g7LhQ<$?I?viQ6XTY-z6=?3Mrap?rn9Q<`YhFv zhAEr&TZenHgK%Y7>LCVF=v_BW&YL8c(cg_`%^T{uJyt?;^Tb3I4ro?^YiODl(>vdF z5U{*)m)|^MNl9ag+L=(6G~CP#{+U~iqDQYiDArFgPN@{eqc2ioSig3I(AASf0@B*_ z+SPPbh@ud~!Ih_qqJuxSpxIIXZ_Fw224h!*Z4j+){En|{R9@$o>{F9A0FuC} z43-vIEU*-p1+%63_=XDpJ%xA*p?FU5vTz(g8DRu?d5L^xq^l~N{RF4f+ znRN{8YB6BLN0r;jWx7~KrXfCaz7TrGzae$};;k|Rx{|5)a0??n=b21oWbD)UalQ_* z;IL@5g3I#M+BgIN+HTbM!RlbP1zu{qtJ3I4C)>Nk2CLGjQ1>=Zz^WqEfgulk5*sdT zz$a>8@N(4Tupg`ez1E<{uN>`x5@mx$Mvle

v|6jfrSwempe$eUgxHx6w20Qr$&eh;jY40a2GGc^*Jfk|;4N@J@^ zJH8%GiQuFz_Bd8t(Wb-Ut>P8&h63xm_>xq-=N5{OG9r{zdK_s59kyTQ?cQT!YW}r_ z`rm-AgIOc~yGi`I@(FDSG&)j+J)3GP%;AQgm_1-_=&$7(=ZE?rRfMpYds$9c|MHUg zO`Etf28XXk?l^aB5=1k4W@U~2H-OseE;3JiWI6VDT@i0&6M`R)9>2Y0k>!d$Qn>@9 ze7a?8a|~JJFkUla0wQC_6*vAHkdz@B0acQSDkyjU3e;{zu{f0uh17?_0XKjYH)kY~ zT8qf4uZq`LI5$Uc3~>D;xCn3Cn*{Dr-8JBQfe3zX|MOa`=!Ktn`J-2ORA|hj-Zs9a<6bAK2G>1$v zQ&PUn+@8%S^C4=dX>=Q}Zt^BQ@oHe6Gc#kAO?1)ea|slrTSA!UuGQ864QX5K{Yh&lwPP4LFu|2cdJA-eN_HLXP{uun;tqMnVq-Md`Gacl+t5p;m3B>?V2*%QRgwH1 zZA@G=&dI55g0t4L#_7`cCQ&qiV|U?hXsJo*+^)Y-!!&#(VqFH8Rs6WS?E|zjk7$A( z94CvWrdDcJx->YF+iMshyCza_+J(GSc1yRr)~YU&u<(fPMi*4QfR2%eaqtD zti5z(Alpr;iW3^Eo_N&O`Nas`v#0O=$aKD+@9Uep^q;B zHJH~H(rt_*=x8zcg?e`u@g8EcPx{iCGstJG#VxA_ZZRz&{LW!zGav_OrRYwgbPJLd zU?d`X=0qo($Rt0~p-#U6siJ)j)Sja$o8;^m*30<3W$)6Gd%R^H4*9g@)7TOPWqIKY zn_SPIWSH~KN!6A$L*&*fg!c0| z+e6Zy*0#ulf$4_qY|p63Rn%1lBw}_B6lb{ya$oh2s!0i8LIGRx>H}4%^4w4{8glh8 zrCzM87tf`ac82x&W;0DEqKS+0C|vewy`v!?l7XW9t9GwRt&{aXu2h3Ujo);IB4?*Z z1J^=;Q~deQ<((M9LXjbb3wM;+aFGzx761pTOzU?+ZNo|UJdYz+SY906awo6y!UX(u zvYE{NI%N{CPjt?ueFGPKA={bC8w@-B%n*(4a`Xc6U1 zoQX_F`IgyD%Cd{oJh0)44?u!tWp^ysZ5p#xD3L8uyr%#RKNt9@CF_bVA?WBs`8Dnm z_&1jOL;OY=sOo7~DuWxNI^PmSa$-!IdV7ZJ&mZ9THj?U{VY5f|wAx z0C;FV5jX%Cz)dUwC@6Fg@q;c%C?~E1NQx-CEUGl99OX3?VVesAMrr!7f9$r(&ZsLI zvV18Dsza!#H?DdxDN8nLB8EOi(UZJK|(Y99Bxj0 zxNiX}sVN*=e*7sc|JeS&GRILz0{xWdA!zLzFo!@$+(*$k@+b1~C*r7sj^yCvW`RZv zqS8FBtEp?Yx!V|3M^ON_f1k#`#pZ2^BEFH$U$4|1Eq2Ka`|HvK7eQtc=W^3%M_L2X z4|lnLu#bW!EQmJP@xqDI zMwkV-X5Kq@BI1obGu+w!F~%U`-Zq5h=T@iKUT}VY<{g*CsM6jnb~ApkwAA+Q===s& zVR4e31HS=jYadzLgS4Fcb5Z*%$|_{_^Gs$#J;$&Yi#C$rN&<*#IacX!&B z>?s-x>2f0ex+pZpb5=SH#!w_EirBplXKR&9WTa$VI_O-t@%@;J$dE~i=U&;pR(8Oa zuyTrYZnR+%dcuv*iA9Y`rreTdj%kn4L&~=H6_^pLgb|QX$=e{PQ7Oh za0SRe`gZcup~emAksDo(G;Om~DZOGkY@M=iBApLI@7aIdZIf=?wP9}3#%9uCaOwO| z$dePhRf`ijg=kuL0hX1zyQFDO{Ry)+s=~ zQ}$qWy!XXs&*}8BS!I_&4zx>_<~Y&t@M;@2VUsz`+D$UuOhp8+3AJJhV4XBLevHUM zs`o{%bK9Rf0nzJ6ji#Ojp!B#+$xq0kr46+BlcL`T*g3ZHiR}*0T8%@BPxya8!=vsQ zg8TQTRvNno6T%jH7O_f+VwvhB1_vtuHeEE*xcVS zd0e*AxOY-NVY|ue*0i()_8r<{-{5%IEhVbYKU4xq1vDgjZ6DUR)-QJcz?2&Y{8gS+ zloNIqtvUe6RzX#=lFq3pHYhZ7cG@HRIcM??thY+`Zyt>{23G@PkL`?|{`CY@2k7W4 z)@w_o*8E)BK&m>P+m256oUh8tU7Q^lEu7J3qbJZB8n_&kZeuva{Sq7#3tP#s$qyZe z%R$^n^pY!f*S52)vo2)7Ym5tQMuR^{2*ZA%w@Wi6K`H54^3Vn6de?QL-Fi24;r?{2 zmR3{qLVKa@G&RYwx%LeHqppwKw!~hy=V2@GK z)bMl>dj(ButiM0-3*VcE{#IY{FhAe@%M9^V$SEA7R&5j$g&sm;74BDFF*2ATD?qSu zo@s}LvCqUCPW12T0xrT=UX^KDfL8Ou$Z25K%{!$1+R|QXNm62pTl{>NxHoxyY12W2 z6w88RO39ebS+uX#1Q&L*z)7!9If!8#t;z>i63D9Y6dh{S@NYt+hLC_`l-fGZZ37eW znL3=zi*R-eBW(lkpd_cCgJjl&QIY7t8R-3PG&@zoxgAwe?KNy`z>Rt^S7IDDY(i+u zEtz=@7w7pvrXrYVShweh!JHnW?OgLFK1XY#+YXrf+ag7-c|0QKQOdd0L`rtdsKco@0zK<24pP4wMjLt%NAu*~w*_^2EAWYhi`{7Vc_u)N|A>!zkvK`4HK zR^OLP^#zP$zcAtlOa<}}9{;qVorb1e9>*b4$Xi}pZ9WZ`IPR!ahjEd( z<5<=Wg+OF7CKRu`x~jqwAa=7V17t^pcBnElTwy~+?B<%S4-7sY*txN>Lu&=SZJurq z#8zFQpwe;Lvd)K`?&ajb@hU}8$_rA-!Ff=sLPh4Ah^bCZIfv zZQU(vTKNi7?;gaa&QcN1g;ee!jkfHM*p-fp9xq=ArYhH_xGINwZAYmw4(TYv$zqe0 z`BQu@?0RiQ^EfwOf;%8-+`oJRT&_f3+z3YnIR%%(&xcQ!yj4K?yLER(w z(o;$cLX-S(LV@m5#esefJC(UR(ZQf)1La~DQBEG;i{rk%!c5n_`VT=>4%KNi7H~fC zbjZp<92u545%*gE;sta9NQ{Yv{h;hS?ZTmsoDx|-jB=8Yeg?g4axK=Wr zYPfT2C>#IQ7b@k>il`~q9@zL5bXa!7n9A%DlcAoU>@!=_ztPYeGL9#ngs!L^#S?AE zk_l+a5Wr3}oQJr*mJOUl?3>s=)ga$Nk2Gv(`Gn=!AA(D3L44qAcrYZ(9DdnjbY9S= zd(c~Z4X%s-NCbx8Nf4)8dq?;S)d8`a8sz4L-2v7aK&~Np`Cac(1JE(aIf4eY)AhguqeI1uWBjk|9}K=juE2jAN!>jD%vCB4mEXHw<^`ck7}FAO9^Fw7~=>0UT$R zI|1adsX9MGqr3&z>+a8YHShc>j9r(CG$2fimRZ~-t~Go>bx9w?`X%FI+i4t7REzRB zjcv63#nm>uU|`SxZ67Gc5k=bIc%dAPA|QY42OW(lZpY^R-lMc;Ogc<50p(Ygfk?2C zZOSM4<;3^lMi3Bg9~yj-8ZtnFsqt>KfNG8ngk)FI!yfGgztWl24@S~l6joL_W|$;X z?m^smGzyBB?O%gv$wuua255-wd1b%H{gu%0h5q|gmT$BnDgkd}GXw>-?k<9Y`3*4{&Op^Eh`QJZF(kSrTR^d=Er3zO*x|6(i>1}yM$nggRS_~isg_R z{d!=4?I~E2($A8&egGF z3{fQ5h`A+G+?o?jEEINB*sDPtW)zQj)gJZ(VJ4>tlUV0!a^(9=#QWfZeZ%dL3oCdc zm3k8UMY7VnW!;)E4OI3Zi#vYEkTM_QR{h6b{Pi(O%#Q357f(j*&%^h&2J77^(v3C{ z_&rX-he4AatnDvnajW8XeBMGa6Js`=`!($%jpbn`H)I124(%(j7hunPu_o zUUb8ndlBzaO;bymrOtE-#k?f*i{cA}%^kn1g7%MSy2-Q{?4)aO1k}~$Y|lJX}Fe@K6%hpyYLg;p1XfH zN~_BC#0#nyKa$xi-V3D*)-rhXr%s*p765EZrBtVRAq4w}GuO|Fc^glOkt+|m;Z#HZ z%!0UXmz^q=t62LXAOuU*wR-@iUo zHWM7i*>e)p9n-xISjO$5PTn(YGiG)g*XAFCecqu3LS zn@2xoMeO)C`B{5wk=$9`DjU+BZ;aEV^33!{QNrqcu!z{-0PCM~;-k7E=Th@N2F9Lz zY4SUI1UP_QN{q{MJd?a)pb-_KX)K)~{8h)K#-81pn?=VUMxzL}8znrEY_fZH_hMTW z&J|2SGkti+f&77Z`vnhyXIpW+E{hUa{mQbJQ0XMyIn$>Eu4qgLl{<4pjU}->wyV#G z@Kug>e99gCRnn=>t{K;9M4_ehO4I%kw2VUMq*r8&JzLXK)7L}z8n%J_k68z@4K2|M zK@y;h3v(ZIuzC%CU;MHx@7pTw-Dc1<_?7$JK6b)klG}`kj1@NK4462elHvO3Um~Dx z@s#RE;1>};r5<*aXxyIKK6@b@u(eJOMck+7?Iot?qpo(8Sz_2n@XU#|_pKXw+D=>4qQMPX%jMd!r5}0|({Tx*Iku9@|{lq$H&%U~` z6r!D|uWptcj|ahOthi>Wqto74u;PXIxhXZ)$Pv%XYzUNEns2S1TMgSCd)5k`^*@jT$ktUIdfIqs*$~sT zEK;`jf*w**_PVQotD-o#4J#WXF}-1JH{aJ>(UGj1LXBm8GIjeQJFUISq)f+}X9uyB z&BE~>E#k38L4DV{n)V}?wm$d*K-AP7luS`-m9DKw0NEsj<0F8!c7Qt#F;rWJ6}3Dd zpRu~_b2)Z$Q^8KUeXk6KBV<>DJ{80(0h9A3Z}~j zF^fA^>)ifAo7%K7(u%~~O6$KarES%V+VSqtbSdv5r)Gcp%!**PK}>jabbvi9RZL4W zE|A-L2mVvgV&$6KC3CpSfs?!Eg*JnkzUz~3DB^=!+l8uj<*kzra;Is3iM|~}j1_6o z1lvsP{L!e~%>kh6(4lw42DEJ(7FHC;m&PV2>B_x!bEsM0Kxrv(K+DwV!oS;MV`IZu zmCE^eE)~VWTPXWA?HRsXIhD&)?P>Nxoo#b&1 z*ns>v5F zn^>NXbGm=?BCIJcA}C^|fujsuHj^o(@dCEa=9=3>MyU0j^d|ODnptg|@k6hDyBD%9 z+8P1My3s_VWON#3uC=Vq+N?pjV;rFsj%ftQ2z65!O7JiVQS8VO@c7cerlKHjE6v!J zks*+1K|W`PZr|B1sAsz(Yulr&(l{|E*Q*fWU|h2EZ#>F27C&j;nEzu)6;rg5BNq&y zC~Yk&%$e)gosMl|$EY<4D?pE4xcw8Em3+xY6;?IpIgX%AWW!v}|4@ zmEwtt35MmA#y#P>Ad$%OXI7Ny?7I|#TXh>H|6G4T3IDLj!HBOpyWkH3> zMoLKn8HU>hHkZKfbW!rduusli+3E#bZgt8LhfY_vDh8?MBwA|QPB+mPhtUrNf+lY}zjS@U2_dH6VttlEXGGj?L#1X?O8n|NXF?Yt1!cM`z$aai*{tT~DT zcxO`KbO(D^5{!#xTA@b}S+3CC3e>g1L-C!OcMg3fmwp$N!rGrnT%XFw*Z4eIdgYPT zPBE5qRzyYwzznKJ&3TJ((R7&M$2eC{ak--dQMnC!ir4y!|dDN>Xgkd#&ItG zSh!!NwrzL#GFNAPbAWg4jDg50$Vla?V+WwLO$!$;P9fmf)v7ryuwhosnVsl952&ee-V5{q+Sv^P9?CYtK@x9u@vT|s;W zb#4|~QeG}IfY>ZqrNxC`7}RaAnKuWMg{d}^*yb+NYtt= zEWkJpqZI9Hi#92V1-6EBKb?Y@sH6NB4N_RvV$M4D09)J zR!nc#MpQFn(68k(THmu&ZTrFUwbYY8#~Hab z+IgO@#{s+Bv7=;AoGLL&JcH>fuZkRphcnxwfYNCX)jzspVt1{9L1ho2sMsD<90P^Q zslukKf1^N{SFHIlYG%8<|5kbyZsWj_%j1mHJz~y-#OL9`d6D9;-s2Jh$##7xNr$dU z*(TiUhe4g#Jec{!m60ogeGSG_`H%)8^dW`>3YxZa7% zuohzn9c207(__KEl1On4dA$Q#hsCx$3J1$VN@8kxt}didUg2TOLwq&bAqE;oX|RS) zUeF#24q6ga_OC}qhi{Y{M*+LV20OWu>?JJ9JE)Zd@nlD)v=Lb!p_v+m}rW^p<(fvL@!Y$vzO2|t7ooLMQ z#vBa{ixyn3zQ{Ufre9+%#rVbHHm|>bT41;3`YdZ&ZH6rg2K1{ci@aK9&HRQV3Hr43 z<`o2zOPVpX<|Ji?UMWw^V#@S#)2g%RxY%A>AqX+@T8SY)8k~%sk9JCe#yqELpR!Ld0EM%6RQ2NpghQU zhk^DfiZ!WDkzCX<(N<>2UWFSQ8C_d8OGvIM_tb^8GBYbJcoCp?GHKk)4^JVw0-(wE%ta0 z>}sn1GBsgLOrNimMKhPg%nE-rEd!vN*-2 zozvbdzR?b2`h&Apf#XKmh@~KmQh4qHc?bSyvGq=z94@D@ki-M}1_Cp$0FW$q6g^jI zNl6kl1#&D;G1yX?U3vMQI3t4o-o_kqQvxR53hui+FbKUt=pS{Ig8mhmzwari8+3al@hkUBFz zRyfY+I6h6XOyNH8`$&isX54jDz$#%(v#o6+gvB7ZNk=P9&f}pwx)`MPJHW%yDRY<# zOBEMpH6bS-ZX-{&0REg259OvVr=&<2_tZ)2jT>*xlAtKXoKzl{?uy>muQ;AsA%RMz zDIlrqKdPxyV!uliapVPV#4tUBE@M2F+VAG6LyF2S@!VKpIbFcqH55}MiP%!01&UHg zpz3E;h26RHd;~$IhIdL5}BX#9r&p83{G=- z>z?!4?fxZpCiGQLYe|esPitWSd&-kb(QXRM8k*|q&oCU;1Ma%IgjUF_n|)3n9(1$2??_jmo6-JJ77oJP?(L+u`(1Lag1J20 zFv?(VV`=eHDX`jixy^z$J6oz0HKb|7=|^RuZ{WBOMKqpI(zLX- z0LUU_k+|vGiDvzS&QY;BB4M}{ej^x?gAA&vj`r6S1h)P*vc09qd6n(Izb9hR+V`b-9w1-jyw6rj(PY$iT-n-fOoU`_<1|D|US)NZ*u2-wUZZc1>6Y9`biI zv7glnaedC^dCHT_+h=kM5lIVU+~{{?jk`Xx2<6_O^^ocf6Gwj$b$DL)MBq8KRJb+X zlUnZ)by+gVfsF>c391(c@$yT{?&~lrA~{0Q!>}nb!3?pwH^mug79&b-mY$lN215~z zgM(W406kYxm5c^8$smFeTW<6o(EDkUSkLH?ejuXI{l1;#4%UKw)>LU0$d_>6k~j^C zLC%fLC9)fh$t~OBnubX{pa(i9#^w?n%Y$)lN>U-RRqX4j_-5^R>>*Za1>8cPg}U8f zzhao~Kv;`yP!H&)V^88|=@@%jRA!qfmf&Y|fCC!BBtO@)_JY#AW;65pw&C?Z)yaQh z1!>8@6=`=+E!a);hlo~k+p2$oRh%3*R&L#n@F;*Xu^AU*hiI}cXvy;rf~h7#nYPXJ z)5xoij=mQE{w3t&H|Aomrl95+)?h>$T1QxC)BYbvT5$GdRQE7VI7hd_&J}`(0J6GFHV7NPMtIStL%- z;ZjD!^jfa!B$8H_#%6Bpzzxd7dUWGU5)?a_OCY z_^!(*0=VXHE@o`5X>XaT_wCtqxIfx-7wG|y}fV+*(_u#fnXlYrj##%8z-fm#GsUpU=X;+n`myp}aT$J*gWfmnEjs}4t zlH`&|;eJc1^eXsHP^n^NH={?&)d%a-Cm8fLqQB(AB@^J9-EG{(RS=5wBVPY-|! z{7(v+NUerkwKkXD%0&_Sap0l{i|(Y+yXo7$>xCW8sbMwEJI8XX9F}V_=3G{n7OA9y zRyG!!09XmNhc!&>X07K_-s|@~h1G`(sZl9qB+e~vHAaP&O{O z_oz|hb!@ezlh*tEQgJLnxRV&#!0AMOjm@p>R*wm*7nIRY@3dOhFXi_yaBQhrg3)<3 z4l@gy=?sqRsV;6n;b(U%HpPiV%RXvJ&u3OxJ|$;{Xsd08pyA%oo@Q!!K6Fvvz=Wyr z+m9wX9A?+edy9Qo)Mw1YaQB1uYRi5sRQXc~#W4J96s)YKV_Q~3nj)AujmhrqPK?$4 zj0gNgCnL=;AI?Ar-GAb!>@E`SjE@zylka?)WjvBH{{Y2nAKcPE6$+xisuz6*DQ@Ts z{ufWv=4>acd6t@aB>I+cu<`Sf;>;LK(lxV3Q3FlxTfM#Ns>;4H;y3$^xBbQ|DxD@7 zO*}nxA;kHQ1~(Dj4+SpPB%@?Yg_@dUF12b*ftVksBJbP6w6@3%x8K$ZdFT+?@V71mQb4WYvRTNs? z4pWS~uO&?6^{*Eva*){CN1WEX#3v?i(?9PNmZjvlnP7{_a_3nkk;=q3H2n+=&sZjO*>=t&L~RP1=* z=EfRFfgBRs4S}@tpBLRjZ1TOg4`AiQkBCwLl#e!d4YviObhcY0%2*pKoYMD;4mVCq zhnBI=WmAUN-GHRwVZ*I1efV-yXLYIH;H;K}l!7{<@8=PJ?SAy>WWaiHIQN1P2Na-g zrqA*-5{O&E5PB9qP?_90PF=bS#f$|8!6wT_4Y@><=A*mG9d%m-jnG>&VS zVDxj(4mQeooh!_c*{l~lNlvE-$!acgY3OL`S&d?IK1??OX%2BZ=b4Q@!E&R?m9uQd z^1d2j_G62zKHL11I3Z;bYN=9N6qyLwvMs%IB^GCkI4w{HCJ?Fs#x z>GT)Qx{@ZzSe&BM%ZWR;LrX`vCJsoajQu?=OWoTjEkwb=@dv{-dhARS>d za65ffsgdtITEpVdR2te?sv&$&ji5V@k*DCZI*wW~$|-CMP>Lzg1;rCy3Nd95F^eqXd$DXoeHZB3m5C64L2ga0t`$O-A=J?P;)ZvF{1o z&gkgzchbcln-r!O5=qr>td#@|mYzuoAjU2&&3dbPkc|b+*&9 z2WhY;hkBDqNj-e73_nL0ypB9P6nJBjdakAx-boIT_^p*K6jV5GQ6A?>E_d2C;<8>< zR9kDj%{J8CRfn<_h10pvwkK2;v7=zdfwGw$3@ng5zGsocT!j0R7(eFRPheAnhN0rP z(r+hLDPYX!igrH}iHre(k+s_nJC#^0AE@tOK<-vqzM?UV5chy=8V7Gbnyvtz8{2c- zy(P41x(Q3m_Bb~Znym5W45c$7$LeO0)@xvKB>m>D(T4}tYV^ia6w3k}K{j1MH zsnqonnC@Qo=>T&#UWdsv(^F*}G8&K0ww=27=f~!`97QEAZmpd=CfeobD-EKWo}*Na z8Usikb;$2Ms?7(1WRQ6-TWy6+K#fmKrO`cU0b6F z+Xh0y+UGPE#^>0c*AlqyZ*g^WhmFZypDj~V(`CFek?x{0LrTEl>0Ht_9f51B+(<<} z^JVFCZ^V02c1*<$Lq(kxKdMrGArzs&Z$|h1$2aCe%Ynp!Yph)v6>l4jOB*d5;#y69=rj@LeKm3OC4P zFo5?Pmo$)1u&#a3JPs6?wMiCil0?D&5aa&rKi6B^+gbcb;lDK|GqsY+~y zg*lBZ4Llk_8}@KY??}2DiZTN3me+2XzhYBK;-Hf+(oDK3Y1!LyxlScH)OTGMq{;6z zQJ<>Mn&o(uw|QGAtqk=ZqbruqHy!0fsH;Ppf5LIF}9~LVbDKz z$yF0vJBz~ftBsmFY@+9)3Oz)Wf{c_!Wgn2NloCARtIVhc>Y3ns*Y zm92biW1QgOaltm)iFC$YOBJOqE&2h&moD(4(^St);^-tSaTX+YC%-CJgK;o72&8CyZexYo;6Xqh zeaKZ&Q}m}tQsP`{)50o@Cvo(PZv?vhGMB1!-Azb8MX?8+?rblt z+T83973g5R{{WrZ+&`uvE86D=R9|Sz&z}{Cnk{}RvMi46i(&M*o*z98bV67rZkMq9 zLF7jwx%OJDt;v}A%XqYf_;lx09Yc=RC*9abZtJB72Q)Yxh`OZC7zH+A!i^->WXw9? zn%lTrR5DpI)-`DH3PYiqo#}zgziq+n@LLxf-2?>g373@XxF(zs z1$JS%z%dZhXBt^;a(uo;(j+jo z<4c2>Z6=^@-rjrr6b1uUaVol{+A6hd1Eg>RvtzfpNb{dc;>L&R zkZr2mNA6JZYsO=7ZwZCPuf!4aS|!|z7Q4+6-&Hp5Yz}*L+!nZQZ)R6eG5_BHo6lRlc`&0FrT*g_z*rq_)R$G#c z-$>0a!6QFq=isz7a&?rj$$w5ZyV?~lc>&l2gz;;>D$5N&$uxw2P|w*4Uv(8*lZ~#e zfE{0mRLmzu)vQu#V@0>M$9f`5D_z8dmpbfl?o8K8VU{LH(2pEX={CnX5;GaV!M-eNtVt!P;cM40iK9j$7Z{p(r}nx>~hB2{BpKbFoaX{uzU zs?(sohi5mir!br*3_lMvt$UxT+Me@kxc&;p``r#I9OcV`R913F@9?h)8&>yc*=KZLcty8c0RiWmNFUa7Wt9o+j? z`{_Z(J)+;_WwcXG_HS>QqmAuzL1_dY1QnXhV17~#w&8-uu|$Ha5y2%8)G)pRb{E+A z9$pAy^^L5nf?9i)IktY=g__QbrCCdL;y&uHX-k)8S7Iz{U@l;_-E+S?E|<|Qd~eD| zD8Xx~OZ4_qyadEiz*>o<7X`M-x#J){YYN-p$x~CSp+ikaUzY}qA zSSw(fk*|_!y;B&YW58=nV)nV->c)Z2GDcoBxQ)f{VyoL%1Y~uxGC#lO{8Xwsx>wXR zHl`E5CxW=ARL17d_lB?UkAPKb&o-a+f~fYmd;+Upwuj+I*pCHu`1AI^sH>nZi3HO= z1tLU(kQ9ibx(YxDhzQWSfk;GTj%ANk_WyTa3Y9r%9e`5n?r{cy&xPID@O8 ziOrquEpv&z!8bc7h$=gJPfk_v7_Mt4l{fJ;apswtELk6|hw)D{HOM%}2_JRC=Ca5o zI~#ZTQ02sKm*-~Mi#bCPA=JR?<7qAN3S7C$nplfGmD`W&0_VsoJ{@o2^J7;Qx?h{* zjnr9^i*B+ho8e-lvmV1-Z`R1)y&3r~d4B|^ytM3Wl`a!O?UXh=YUOW-?lzf&5umKY z9}E*~lZYA!`7Gu`fz-8lw_Pujbm6?O4~0`p3QW4= zm9v>ee9h~6-!L~7hQo=t-8*`xXl!*bI05Hha(7q{DU+E8#3L0l4rA03Uk6(FzL*W(H z%@161-R__B-fi^Xf;%i*GgWI2<|hurAK;}$o1JeynkRS?6(0+@T{G_)9y4nCweB@m zI-vRWbaDf{omZ9UdJILBDd_udgZ_(2N?O#Ss$BNBmj8_bx!QjK@o_NeqGA&GpR^r^R z+^lBD@W=IS6Ugli)2Y!G-+sk;+g0q^<$IGG+kSQHqEnbHebNg>h#YLA{#L`DHmZz$ z?>FLz)=`@l)xdlt3m0QiZx`@2tBO)uNqkO4n4EuvIE|0~CI0{w4mo5E&>TZg5p?Ef zWmo08hsO+skEwYs3^ul{w^4&;)c24CY;Q5SabBu~)M-X7cpiSNd8$fD_>(e|8fvN~ zAZh>rdoEAlv~_ONO#cA7vpH&bY2v7oX&TQtyiWYos{DG0!sal&&O9s^9_p0pS9nKO z+N2>xWtTX4(F4zIldO{RGJ@A z0mcRrI}L*3xgCvkHE*{4V@3T-xx~=e+QGmy4|4O@^HQEMvD>tZP|ifVbUAQA-t=9Y zu6eARh5rCZd=%9W4If7H`buC;j>TD1IlRw5q?_iH{{T(iB_wWalvq=HlNh7JFv^!6 zQIek`giyIW;w?n5{xINq}q5xCz|!1k5hs_^;ck)@hi!q&8#?_d`Rsv=y$ z0ONXw57rCN@=Ix+-43uj8*y4|Hj==aYdH}32Ddy*Aaj}yo)VV<;^H9`n30h&m-95DP6-$HC8ahBr3*95j&GkLrk?!A;lKAck z@@O5($8EieXNf$7bU^b6^GH4g0N!>fu?t8o@;KTL9V8{TnU@V~T-%?j`zm0Zini#8 zM(P)^TsEWp_xYxzh0OpHRNhx0l{}a4N>x&WYi zk!c{H10@UYl2J78Wnu-JT~g+EyGsE8Gz*T^cEQ>%vsvC|nHdv9foSAy?J8omA(>o4 zP+IE{R(@SlR2Gdl0{e}Xm2LDJkkNldyDa@pd4i<%aEVQ~^X+A(5%3i$9?Bb(q?V9H z!Pun^3$Sv4isPD6Ly$@5PLU&;mpNA6;CW<@X|=X5C+58eK`&oJ1jme#wcgQn!+LCn zCT59V3mR_MzQF3>-Oq0|*)p8srz~NUgR|*l{g9xSv1(Bwx+0{3S44>mpa4=zQAsEO zT@ovx5CWG2{{Tn`x{T{hGY97&flL7Zm>hNa|oQJTk2PdVL?{lK%kp zD3-;ex!16Hg!nxa;NPsKjrZ|rDoItjR#@iCIBby+WAbJ+5Yo}+i);?yZNToVZltM` z&-roqoGryL;2DZi4N;ctWoX|?J-waj?2nkE$8g`IEp1#rY|J|oZg%*sG2B^m{ON^M zV3?rIcsq2E>()fkvxhg^B+YR4+F(%v79zK+E;=cJm!PD9n2Li_dpFx=oQ4M z%g0Fcu6F}c8g2mRZeAab>$W`XR~5`H7-oBL#|!LtW8S!c3hMLyR>N zF#C0X)U|xp2chYS!?7A(OsNn505fg>07~C9?`6Qck1bgvuR-RTC()b2-r_=sRNGWP z#Y>?hs{a5HQLA>ScY;*Aqn6_>`oM#f&>NKmOk}#+t8~^aWYa< zyLUwyJ*BrzyAGsqxq+Uol*e#IWMnWw{Fw#(*G%5-xz_lkzYdtX@mJJE+$=}DD+6?Q z-A^+k1vVvvRoBrvl3YdF9!1@tYWx(a;HspjhMq32R>r{1Pb3pzZc2Ua$sO)|QOp=B zYH@!fo*HTC9_PwVo~Yb=?bxQnm?@j2Nl8^5hZ4|!1wzIE{Ha7uwvYkq4hmU?yw$US z+(ekA41||C%@?@T&Izb1aLr=U#M8hNwUf?)g(T8M?kxpZD4Y~H-AoR~X&T8x{{W1{ zcHe`A)`$s1iEG6M4%nCQtBRS|ely}2qV^EDBujtQZvr+lu`-E0Smw`#u-ZDT8ag;BxBa>_~U zP+ZlqV^8ZJ#b{l@R8!#fux1LZPATc$%7$q%xR6Ho<7F~eV-4AubW)Di)Uh_Gv!;A*CpIpQppjeAcb7GA!FJU(*B+;lt60P^ly z(Lnbb%Ue5*_6@T@_bLqCLfC4c8sBkpNjKbbS@?A1s^TN>qZdyspv7P3k@4fS!aXiAGE!n(dK8sx<|-Z zT-c#)EpdYKM%QW3_MHXHCu}(&fY8 zw7nMecHbnEE-zFj;Cd1QNwn zhTXc$E*}>GcMDqAu(&+db0TC6YdKI<)!{QxwY1U*v8X7s9ZU=}}_-0Mb7@i*%A&20#byTw7M3J`O z*pPkUdIO?dQ#9feV~}Q8=P*;w8%-qe=+rLYKsFkMfLyaK^r?(xrhY-fDX{vCIvS}P z1d~Sou(8q-Hx2`zVti^%dff%OLwIa{(&yb6)Ipy5YOc*r!`~bbQ;Ar| zxHv~Hmd4xdzXuWEqBW%B%+`d|wS;Oaah6_u4k#Rs>(IaH4TVu+xi%brf;xAaDxEUV z7#A!gu#5Av?3>Cz{{RQE^M_$@;q;HiCWWjqnpql4+RMAnwYeSctgHIG*edNBsmaW} z{ZBrg2_&ye<}9DlB?WAB7_2l{eMF4b#+s+acHYBwcfh;{wS@~H^CHZGmLN{caBM-#dlm7Z+N*q&I% zWy0}TdbydqOHJD&c{!(;Kf8+1;(Zrpx-7Oa{-&CGKPPQNx~Z=AFpJ!@+koS1sdI|P zrBx|Bq_#17k?K~tTPtJPo=M`s=nZ4(+8rnoF}gwF=@E5Zh|$$y)$`ES!su#Xd1SG> zi(EK`Ia$G17mSUEBh;HNM`vgbEpTZz1dWQ4FfURdzQ2swQ#spMO#!@bPEH!anM5=v@Sg z>M0$2(?h=>FecqWhPv0Hhi(K5C2B(I8cdZrt(drArirDnura5QL6B3nt+h=Dt!UB@ zaz>($QA&37U*$uU-NM>$;zX>$D5`6GwerZxi6^^%{?kvv=wpsT>Y~E_F4J;$Rs^Vc zlf2dr@~q($v_!_{vwh-~wAJ)%bv_MJ=^rb>AjSsVFLJy2cENGH1)A|V$1*MN3$D5D z99Nv;SnY6;>bP8`DmjxD<8)U|hk_EWxyMMd!#^_P`d=j}r)#Km4%1J5*C|ffjy=US zY*eWy&L_Pmf-+HD1fs&4eM+M|N+ZiP4)6IDY&Ti{k)_d;=O1W~h*pPr5cV+r6JU5| zIg4U7IF&zMDRdZtbA_BqH`tEF)TF}UB=t4eggz5${h(G0plJU9h~m@zx`wmZ+y4O6 zx;xJMCRUT@!AePTmCwVm2DZ>+^`G-0iT?ofgmBT~pxOAmZ~N4sdv@Dt9hCa{{Y0!@4GX8WG3H;JPSVq#4J9~R_}w=xpmEgcDzzj*1nm|oyCk{ zBs_6jVQyIOogM+DfK~aF24HnEH_UTmBsgqEfj=c>eU&bJltY!OpTL&s9>r+t$a@@jEX5Wf zp77`@ndLFQq-$)RaBv&%ej{byjC6sao<&QgqQG@P06Z5A=$124(j7bbLmaXn^4sE9 zX?;l&I1Bb!v$mby3(c#!@I@y5fwRO`EmJjx}62y0ZW7V>iMy5aLgXOQ|-lZ$=R z^HKwXJxaMiM`#+!_Jxx6g z8)4HF3=R&68=G<6TG8To+!U>qkzv|8uC#+4tlw&CqlB6%+Gj&HXe+C)acRF2IQV!jgg0FO0HoZtEkhfaFrZmqNCTgvV)l*h zJIA~$&X4mfd5Z|v;?~wc18NvMc{q0)F0sxu$u0<80)nR(>C%TzBI?r_Ep!YA&b95V z5pPi4Zn`Fz&C&K2@=3E^-wQp~9>t&O8?Tc*kqQ%Zd3 zDqJnt2nXYJ9ZPgN5lAUXDo6q_i5<`YvL^(0L{I_>0U1OP0fou`0MafnaLg`CZX}Vn z@n}CK);TV1)BQ1%DpOQ}aSc!hyHiN~6g`Psmpu{H%TdfZMTdNJ0iBB1VU_vA_^Th$7&nl#--_o7P7* zMa)g8fH~MLYxI`eXr0GT#Dn4%_6g#tr)5MEHb~wzHN$}j@k$z8N`hJ{$i$G!-7as; zcdQlzh*V`vZP|AhmCmD(>ZGV}*pAFCXbfek-AYa=D*GauJxZ zC|q}SFn_VYeAi0{b;Z6|uOq2y^GOr%@6EV&I3{r&r?}?S<{W(4E!q-sF?X=@v00yV z(zhau*+QSMZBvX*Bo=~fK)1C_y||_-q>Y-LB%N5(dzDy&6$6}O^RKI&`%~=-1|KwY z#$N=2Sjo6>7QZDqrB+J}l=6ciH(9+bfPI3TB&5mbMB(}+N(Mb^gkdGF4q!Zpx-#I_ z7ZLyh=I9^~)JlzMkTHdq*WNnukY4)lBM% z2o~1ry`Z*}%;c4UdIDRV3ScO}f#qjvvex$4?{K04$ zLc-g%?ze)RHjp9-8z9z&HPl;d`}l-`Ng(Fa>s94tvWVfSKy02g@{SD$z@+su+w*Qw zh8YRF4k@~z*D*n8yUVlSn z?mHKs=B8&Ch0ViTF(S@!0C<1)y?v7M?n}(sPYbTf_=Hp~lAVrpF|-iV!Q-pAEjoej zz4)&m;zh=Z%_NXF1F?2Kie@o%eTT`Yv^BC^gTQr;TlkaZOPyrNV!6Az9oMA#+G5Mo;{A@Sd%F$ zaT=MaLmJ@`b(wwlu-fXkC1*@`CqALrG>`_=hV`tLF!1d+w|L@tuJsyR)O72E%-me4 zT`a94-^}j$J2njRa}HF*hmq94WQnh*kg@NJQEkUzZ<~Ghy7RU#h1Fo0j(jSyp|upX z9b4_d*z8Af7CyCMl{1bdmvd|L6*D51c}$&rZrPFtn~~Z#vGpeNu4VodW;2aRhr>lH z+QBPgc_HPyYTuo#JxfF&;U|rZRjVqr*Ee~64z>Q3EY*c&`FHbWnZ4V!pMtdi07m=~ z_?EqMV ze{#B$rHULcqlz3CD~g&tQl2=XGD`zAiwns1?%-~A+LEN$}c=*Hkv3J4+d? zIF_66vxy$XF;b5*mr}EpId*b`+2dWwU+G3IoiOJOfy2lD0I};`{*C+ebBZ*7=Oq6C z^)Y0B(r$WcjLn71Pdrl(BC#>NJL{g$6|4UMM&|?3<}N?uAO8UAVu^kNFHimCz2l$g z$4dlxKNGGD0$D3u$Ge$p7}5vpA8O-2!whyjxe9KJ(>+;o1td2^F1w|-4WOc0e_HY3ERbDaB=3;(!d=!wy`67Cf@l@B1FvMMZ z7L$8{wYjTB#x*6(E^(abQ|$U@;%dLrV-1FYCR81Z?WAZhErq zOyBO1xerRl{UhVVU-|3)E7Un*v^d0+nZ}Kg#{5GMV`bC1cHn?dGSjFpF7&2H{{X~H zdf)l!J}IzH0*41xpgV?Yc zGYkB+n((|V#$;_xQyBJ-v41<=b*ujXNK8akbcZbrE^m_I&__)Z8hqCIx}9O-ZR!cq z)=7KJu+p5R?)`s5o-~r+&@N8P(6~NA$yp~Q#f@i3+f!2T(=n76Ha9=|HZUE??caer zFF#O*`E&*M)7Ccj?6_Uy{VJr8VE7b3yQ_6Xu6Jd_TW^U}s@rjjdyThIP@P7zjC{%- zmijrtsPe2C!zp!!h3#aL#d7xbZ}eW$cK}=h4fyi1tLUSnY(F>P^5WQzI_e0asp=gI zBW>O0gMCEU9s3Uj(K3UdH~KWr*-ifdlj_C|m|j!37? zB;%c#HIyMJ&zDBCgQ0ZE(bgZ5DQIfwF*=HtRZ8gkshBl3!uQlT`xiC7%e?;prCXdZ z+`h~Crdi1y;4|@D7m~FZIYBlq>YQO!#w$AP@>bG&qzmvIrmu?2?7{Kn77DQ6P~>!b%7rQR1OuU(x#1wGR>4u~vH* zr`{#yM(W0)Z+QKnxMPB055{@$J%0=T0B*gb0FL=kqkKe-+Tmgm z2DFmb4V}bYJ9ZB>yp{)pq>EZTsawAt)6R9FaO`0!H7V0f&f6y>b>MmJYG*MZ71IQoFi4w!N#o$}#WIrh(OLIco`PF!uU!8C zwOsGmV%@l%ODE9nJ7UPR@&RKs(#U*u7W^6(vfZev{dPq?B7$-xZbX-XfJ(YT7u{3^ z>LJ0yh}}CqYvq;F%S}%s&8|y0+k%c!IP94jxa?L@PjYNnt0Is~{ zPE#IOBUZ)6{^hCR;;~&Z)COgyL{9E~!0z2LlFX44?z0=vlS{LOY}WHxLi?V}y`Q~y zHn8hHm9Boja^>9uQ+1z~r#oCl7(JemKPA&01fyW~kUL3DuKjjWmvmT$#{G1*?9@}N z+T!He+;>sqic!A1dehPC8jHx^u_25cj9ylO%!O=5hfM>k27HKhfvJ>R6-4d zZg+2RJV#+33#kPk5TvKCqMDAbF)TAMvF$v;U9*&i@JvpZ7{ejNDBrFrTXEXc%pUMm z(z!klQB%?OX?_=5CcGl8+L#c_3uovrdW^0MRZPvS_+IQ6DSy;g0M6vANBo_vs+FbjfdCYm66r*4v zuBPiBf3zIie$h}*L$mWlfJ1)8Bm{UMg^jo(Cfq^*G&v?Gt)zqV^)N;E z4V_yP&t!gg!Bx&N-Ih>qWLcC;JvMQ|*gVMUBt3zEd`hpIJdMOSZ-I2bAQGL;CHaOb z6(2)l7CWYR#GM>sPPNM&3y1#eN6lByoN-sW@wT2Z*cpF`RhYgrU5e64M_nM6nm{$h zqz1ai;{LD_WqVH@MypBWYyBK5q>~24sT}LF3EEgJcD}3c!1jl7X}=Jatj*CWC1m5y(U%Uh3?OqYn*RVL zGCbRbym`#1jP@1-@F}Y1ll;P+3X7qBRTgi+sqySF)XzF5Z=D3AjBoAmJPMHpVZtS_ z^2SC_DH>b*l$O^}NNp8;B#t}-W@$eZ)lw4KlyH*W7`%&*PTAj=aggY73S*&~zPKE( z;lT3>i>AWPt{*Y3khb6u>yR~izJB$k=?gEv5{5ie4ZAT)PdYa+*viqqoA!=NpXVHH znC1^nES+O2ppDI)fRICjfVginx$L&;cOe}kB*sa~__ly1Zi!tK}>X zXZc-!`jr zXFN4gJNB9QE~XqVxgJ)ig9^u?#3Yg8FxN)=Hs^4+(dN7j!+O0wF+1>Fi@EldIg!?( zZ#r=0Yv5`y*9}rPwW(O1+7F0S_+~wWI#BP*bb+K1=SUmzQtPo?DoESa;&g8y@DsHB z)@(csjMC|z0^em7fTdSw8so$ zHV>)>+-{|}bz6lQ8BH8hNZB6N?%;26eZ9)Tr)#KdQZt~mV#FTwfNqQ)2mzcu#nf<< z$EzO%s@Qy@j!3N1;x5fX<51jRy6C85G19T8mB+lNZ@5#lBL#FAP;{oESs!@=b+CbL zhT_~Ua1Pz-&5_3&10!f_ZcTzw;DNd&CieueABqH`2_){SNdq&GaH$7-PqD#XMNtEo z9juMJ6%Jzm0A@&kM5(mS52cC2n;Lz}>ERL7v~1A3(VY6zp~Blp(yE(lB?j(}U@aQw z@(oH4csDE-RUo;(i6g-*ODG#k{{U*9aKUTVeu4I+&7suq-u`XuLay?Of%L9C5U8w~ zw6c<0V8VEJBbfkPRVHGusg9_Bc@f~Enq1n1vw<0V7oF2Oin-=lI-x9Y(_%pv((b~oBE=zlSm#G1W%2cZ8y`0I zzTP2cvazYDuImI3@_3`t`DSmQBdkE(T{fN#0k5T%!%mwy+PvG|+w%MZwM7*oAlr#6 z@~$3S7%DvCKm$qM{5dHQR6#Aesaorciyk3WRM66zfl$KeUPyMHtP#v`^`pZnI-0gU zwXS##AhWBmH{dwz2Oz76<3e*Rmt4W*xlc*M{bzT4O+OXX+qTzS!=*uN#~rP`J4yDr zY~ha;(USdUiIJ>xoeKlFH(oq9o!ZCi6@aM>@t2Q)zW};olZCQEhFuy_^jS_b6X8NoNNP#_Sti6aofi} ze?xuVdl_T-shq?NM+lH@*6Y9cY(IMIJruZq{5_%vkeBw{xfNp^XeN*kHNI&*>#6im z`{X)teh!p=v0M0*QcDl1ip9Ni;92Dm1w~%N(PzzRA^iI-_JYghXIiePeuI7`X(``% znZMQvTzCEa{oh1ChW`MzkH6|$To$+x3eQyG^b-@-(n8{S=oeRxds^3$b{8kuuvNL+ z4}_QbEkmLP+P1&3V2wJG)|sZGrnISowbZoZJWWB>H_7!yW^ZA3?zA;IZwoR;*0M;o zk`9OWcim?xD&noq(bhr;WWonDIB&RHdbtA(Pk*8wcl8z0;uUJsjUHChqNkG_!(WvD8B8B9V>{W7}{+2H=9W)LkuSx>{pS+*_^el2};hk69lZs~ic9 zb_GNn)}Nto5S)^RiSN6pshoREP5cvoy2baPEBh@-Yfh)>`keO~#Bm&IzDoQ~s%om4 zEviW!a&5qO1+dJWCE_^dMvp7iVYJb*CdAgy8=Z0^wXO~L*eR0W*j%2K6eI49KLru_ zXA5~GHdgpZnz*G2V=3q%@O!A|mP=CCqP~^}Qp?W4u?#v|$YYScoW$VL(Beyhza94; zwSdkTq02AfCAZ{AT1hgN9PdJUhI+yID6Yw@B#-u; z2CAY-wBGu4IGcK|wAAP* z6QOrOw=Q%dc6TZ&9FOW!FqXZpZxC)5rSUp!JEh)^==_ql7%>Q5*&Il}Qz&ho*A6xy zn|ha>u^cA14uH>D7gZ1-7nchJj2g2NhHojsYO11@gY&enaBvzM5P%m1=Ce2 zHyOd|8O-!6tYs_xRa@%k^N)i zF}1+Dz+Z5A1!%#>KCa~|RgFomb%?DrJCFK7WlVcA;kZxaIulsxStT;GNY;=X8)7vP zeg}f{eO)xvl~Ynw#^|SpM@J(a#ja_w2f(g?&YE-7V1FE=eZJKzRtpoYn-RpSu?mvj zzN%-w?49Fn$=^Qi=35bD+C)$Q&QG>>~y-$S9lMO8b`_JOD10+Qztl12}zy7Igvs91Z-@yE zo#1^XEX?pNcfhB??@9!oB`YN0it31m7Ro>6yYO85rb_6grKP8cxzF9&bepc22>_x% zUBB%rrkW_#|Ic!6&31-?=N>@F7*nN#vBUusZe-IiwbliL~r! zvu3CRv#uxObm40YbcrTm?bHv%DA19jEQPL;>S^s}*LviDAAt#dOssG7sH8vL7W~SL zAjp)5n;ou^dfSldYG)4C&TsE%PQ=+92{AQ?u*lUZ21ZF2-A2RQ$7V^I0J~{M{{Z3% zoO2VW!J+b;LOO`)!2rtG^|XJKm0glCT0CbS#Oml~>!6Mp*dL^g$tRzJx{JeQT$XTa zU%mLI-MTk8+loQrmPo7FPQ%5baqTL+-B?gx$%Xq$v^NGzzRI?=-lg5!DeXIMt7?z& zhxn^cVi3{dlr3bQD8|WTiyf<)^x+|GEi8BAe}Mc~Noib{rwWZ&gfQP`Gu~6EEX@?D z-7LBRVQvkIN=DD9U>>HQ#dL#iD-F<;1{{^vpXuskc=cJo^p&{d$rYYv!k&>p^&m%j zEzl`N$vAUwQLF5NtOo@P3?6)kxlPZ|EmX~yCv%Hn&yADZj9)V)p#sCXT~s@ zFdF#$t_fTqc?=|w;swClye>=8t}zA=ou;UkHy9;}@%{1+{!c`EjF0&R3adKePi>%v zo-X?rA9~CN8zG^=V0SfNQ)0KKx)#%Be0o4yaR&f?%NpN!3cgC}*`gFC{ae zlzIwNoTa-Mp%tj8PcYX-g)#eNy0e93FcQw3y zss!1)3zx2RA;SHQ-e+PIzK~BbMjkQ>0H4b zM2*>?@&})au*`Th4k+HM5slSMVI_g&iyqN#3a6o&$K)E?UUAizTeyMvEmeJk-QK=) z{XuhaON!Qlo5Gfwy*lW2>?y)l@OWCIgJ+C3n+eBeq@#{#FQ*spvybAM$@y}d8^GqRlB!r|j%P46pcdFQr+#m2gw_19K^`H}CZ<{w;!CM_s#c%8 zR0wJ!mHHU)TzMzV6SQ2#ARln9bBkM}qNfS18fC3MV#T626%9TyxA0Tq&Nz!jq>2an zh*^(g89kZVsHLcD264vXk4cg# zo%W=U{{W4V*}cWO6V4I7#u#Oxm8XV>A8}4$Z)w9RNFv=!KV&Qnxz`h#o$*UeNOd%~ zQ$vRy>y+gVX?yVKu*v9};ON8!&3@J+PX%aFa%w&-TdYSYP%-fmTDMf7M) zF4YSi+R%N-ij7YrqlP%kZ)P-KQ0z`NQ0wybu4hwkLFVnj9nu@$!pT<_P|-O9h`$S< zEH?mMCEdt3V2l9hJ*%2@s4b5`?YDpr)+?jjmCZA}CVWo5qFJSLpmSRrE;N8`x5ae$ zRVmc3m~>5;c!Z-GSeH{NPziv#+g{gO%#8m4^==O5yneA*S=@Y=M%E4|+i$^a@+0qo zG!9Zb;<`LbE|SlW+{0moQ%?fUV`n(`m6*;>>nSNio3SL5-n4bjEfNql5q*_bAq*46 zPTHMXT5cm|&VS$+KZekqTZr|} zaxHH;8_H~xg<>>#eMIl9oshN702dx6ecApM}6W?C9`w-AJz@GNjC7?A!Ze(+i4Z8|H~hb~J>vZ&T2 zoD;Ab>N;#nlBS}v7~2~o4r{M}H`KOuc)>{f$cIs@*5?Uvs=R z_DThC_;Us|2^uj8w);lvY+2VZLvN>JzVm;2Q{nLZwLUBC*?&uySp5G0WOgfyq{>N+ z~+hskX;{y$<>+Iz+PzUKEYpX53} z?t+$Wb%Vom_7_b_6)~r&Yh!MZU1RW8aA&LnQZ-K`UwMb{RhJL1oiP3D?pa+@#<}=2 zQ+<%Vf~JEq;8TyI#A5ZEDUo70e7~EkiQW$hZp!6<#{LDB%_sSn#F$(Xm>fm)I^Coy z`0C8~kfx_$ds^GM;EN5o4%If*BaXB0NSq}Hi7XC_Qq?i>a5e~wg_Tw@ElhATMy8VM z5(SE9F1kYcuCdNG0Pa;%$_Jq4YtZ~2Mz}S?;^2*yK2B>{iPbfXYZ@SJfEFH+RvDGV zVea3Hf`ybd8tS3}KSJIfC3ezZ+F$;Eq0ZIcfAutPWO90JYrBZLZPZjw9(61y`~f~% z>t4s$kWR!dDs60ZB?!IImCtlkRG3_I)6+*Bj;LdGG+SbKQ!tzZ3aY~)no4pW_T0Eg z@5MvuB1>U3;G2U`-Mkiu03XQ|yMIO(UZq*Xhr&s`^F4kiIa|_|)RT(FJUxY6xk8EG zny8tcBsnzvoWeZH=L6p#?^?Etn*nl+M<>`MEGquxeqpbMT1$9+OjNl(tRJGRfy4BH zA0nBTDdLP5W2xlsYwhs?Z!{a*NxC}`#91~xQsH%90TSTWuVmM}=JH@TtY*fFL-$BO z1sY5@5R7Vd9N=Ep&{t3gefv~MSlOwI9eZnk18=NT;HlzjK=19oF^v zZk6%}b#Pml(QBngmT>1W!g_XxU(IxHBV^UZ==DrX;C1>n{P!3HOL%gY7I;ZiKgQp| zM5xK}afYqU>*2~W$mfr!u~uHqh6GrBP9Le zWp7#?=r!(R2qNnkcsah%jBmj>NDxP4NB|&$3IH&xtB>)LtS+l-P3lUd_Ag7kZ~8A5 z!m6FyI3KFDhik0QMvc>#-s^f(yW9fP9ha57d9NedSgc#a*dU#dxk|Dk7E@Viduao~ zHC|7X+WbgQaC=iYg}RUNCcTJ61Of#TQY{}Um z;pGvudZTaRthQtpt#;%i)Hx>i9FPZPLfejN<#eD^K&66=h6e!(JyS#73l8lvZZB>{ zI*hp)3>GxFRU2tzc&uy>k-CdrbKTmZ; z94>o`Ow0_Sr_%W!cSvsgUzc!N=T0i0GTcOD+^}MLR*tk8TKz<=sa^wBPy2AUzY<>BB8kI?HRizZdca?vGRg{S-s9}uB_2tUL+8{KQ7 z58YP1(!UR!MJW;uO=HQut-z$u`9dmMXyB45B-lv#FLxg6B=Y2LOH%F4m5mv-D?3i= zrGSPVTzeI*J-nJb8~LG37NE&ZAb?dv-+3HQGOU<)vE(jFspg50hXDl(q=CKB^GhU9 z;w-qYO!8kaIhVbH?M*h^t_{+yM_-vHb^AU6X}0eQto>7`^i>m~$T(wOEL_*W)ae{Y zxP_@{?^StP5l@wHs9W`D;`ZnY$}_rD-elOwtRydFUFk<~h}z18ld!+dNO|hFxl1ZO z9^EwH_L5YBogIp@zY#x7(SAxWPS+pfPVTqCGL-i$D*Gb8B$R3Zsm`*9a>ZT$SaaI48UtYN;Bqifs;uT77&cMVtOfh(aaT2G;k zT-04o26HwD2yBL~2XhaLhrLgO5y7Wud^p7?gs$qy{{S-Vbu?5JPSsOKPdVXvA`U+F zO<$6-g?YVB1xbF`fr$R02~Nfx7{zxksNns2qFTcrlTuY+Nu{QJquMdCra~HSP9e;}Cx#J6V1DGl>z+X}!m6~L50O@l9 zy@zGf;ZK!e>MB(^o|+p+)KSN@wRG{GK6HR6RXFAo0U^~v-*jAiR#FTqDPn7-mRT-2 zy59uv9Ozn4r8C0-_ut%@ttl>o-OWeT&O0B+a_K@q8)3D%BpE?GQGz*+INVv)8MT>iN@qhe=P;Q+I2zzUCc~F~*4iA4P99xb zic3&AHYkK+-bq~RIY}NL(*Uh=MwZUmMadhM1AV)~?FMi~?x7OOSaMEYiTd9Jvs>!P zF}JW;SjB|kwC@jx380Pb;O5h`JKJlgoha>0zH41u1#(MtjRL>`6lo+Ksh7*zR(~>P zDoiq#QYMY{4IXTIx%Zw!wOTNg-XgL?l3SJCA?8^qV{Q|Mxz5C@tkwo`U^`#L^7g9e zFc>gAt5Mbv`pKFfQ6{Tzd)obj-m%nKl7@p4pBAim#BmuMbh(7-XKk;{O2C^lz`{a;h|?jWOko^ZFaD zFDg-@sq*dT`y0NMV>S4lA*6FA0v9#KuNDVaGPC$gE2&|1GqW6cR0aCn*qk2s8-9r$ zovS&jNll#RxSj&!sZ_XKWUjG?svWX8G=3|s!|Z9>(~UZ5%OB4Imp5~$y=IdBd77%& zFnQq;V%1eXPTxerZLFSWk79)kxgw5DQQ_4UL>-LIE*{N-a%xu8;V{z4C~l1)y`ua! z+k%Vv29OWuRWAPkmIuvxJG)xdDsx5&*5a1+irTGQ?c z$usm?NE8wt%e;OHqLT)OCzn-Hx=6mM6LIwbrivUU<)aFWkUq5U{8oB=KD^d>QugQf z87rD;<1PFM)mf^RcNaegh2-;goBqntbaji;J(aDF2G23b=U z1T6BfsU2lU>-sVA;xc+FzfaW&Yl8~jogy(H63uY>ymqgeABv~B zR`}gijAb~2802%s%U;b_@Kq6FF#iBahrsUJJ>%xK@k>=w>QhonINv@C!7&^*{ZhA# zB^RfLJbl5F+rNqe!P=StBY^_MB6Vd8j_}VPb6gxf_D(WPHSP0iW{usCi*INvo9J~v zD^K3PTpcg&G#`6E54K6P6;%(Bn&y$^r0Fktb-dHt;Gc&wV^Oz2uaVvF0m>#GX<1Ox z>iSSOy~~FpJ3G^6l6_xB87SmqcyKFS%^I_|q>{^7&Nb`gWVWks=@naR4;ap#($+lH zjk3Yj5K22A4Za0Ti#k*=`S?{lgPCl_zZR_Ylyz(ueKeqU>Dfmh@GF%#N)HlPI!cuq zi`0JxZT|p;wAht1sHLQu7fw38o(~@Jx_1l5F#HxTA*p>pCB@Apu6O|Qy2Ro4ZF~fO zTHlJFn8m9aP4B05(WOcf;q@fesz~P6rOSOz3Fy;01UZuflnY{XjVF`j4LkEwX-`PT-(?c&$F9>ti>#Nh|j?> zOO-Puq!v@p2QXbZZ@TJ5URcb1T&}62mSX0BTpZF3jr&(qh1G?;rztL|r) zsxkAlc^T0RYGfw5wYyejHP8$pY z)Xd)2eC{4|ygLHyLdsXMTIl+mj@?^H_?GoGj&tgeKgpQW+4a_jINrDQWMX?)?-xO0ml;OTf5crLP5+%lFi?&{2q*1Mid{6jI}lyxSZ z^SHgdOB<*I*sR%|1}%fp;Zx#$d~|Ko578L-l6T~I@Lal%iS-WF@>;{snOe-ZfmGrZ z41$Uv8q;???AG{|lAYyBzvQZtiq}!nw}d@dHR`v&QAU4M10 z-jhcQVx^4f4s(l%Xx~ozZ_P@QhFXkP8eke+((r6~YzbO=47q~Qz9+JeRE;3&l1DoJ zqHXR@zT&hjs%5OXczcRlSyZ`>nj~9gY}1us$&SG>Oadt@nH-^&Z={X)F1c@~(-9-fh9)0^`^O#MR-rYuM8a z;Fmhvk35C6slgjeV=+`Xmp;PdhQ$1rR;Ll6PLv?(IJ_>I*vkuAlbocZ%)O-@UzBTj zl2J27#h%?j`jfQSRYhF`>S4iYW{Hn$9N`N=)b0a?`6ucqg{?^96OT8Z!r^OL57vvvk(_>9>nNRw zNr6utAUn?GBz#f3q6?3#a8-@_ZE- zjV55As;YC`-lxfc*B9eqs)7u>H7X4+0=YUqLE^c-6G|}R4g2ySh#w#b1otAy56 zb4xI>W4E_@t~hNQop@%%p#K1ws>&@!U89hf{!*ycdA=Zh_gSg%OPW&76Zn3kr^IPa zIM|;>o9nT;T}M+-9Bd6Q)ZXPk4$XA=e-M+cf=PA8&~)v>>Z1(H^Wl|ll1bR$=3BG=<5XtP=I9 zurgt5j5HMl`U}Pdom^LRBMC&ne{eJm58gN_V)Ewc1oiOHv6p zlh(O^M;OgSc)XES*1tpJvjg3KI_sf~LQrfa4qSVRQH1dp@N_9ua*kNMM-r`$7PXQV zmm3lrx_d4Pii&!NNcX&n2c&LQRx4j4y1Rzn2s+Wbo$_@%?r77|J8AgLE5lca5b zT6^L3aq8jw(4}=xI_KbRTP4ktjDD-_!DOlPrUN0rl(~no?Xr!IW6iCof|m@hV5WH; ze2j1owfJt9tS%ew{=Y)b>XKWVmK35_KEi6N@af~Iqn4(onbH=`46Y6~LCO zcq6vHxLgq7w|9zNTUxZc(SHK)tqJVQ6XJ#TRezSLY)+YWI$dVpNf#t~fKE3SpmQ~O zLql}xxdZ|>+s#0?y<_b(%h2`~qA@XZ_LfE1zt?&{eIM%$mlp$lY3; zC38-jio;$Ai27zLld%8(zZO zk-kJ!(o`8iC(X1*LOAVn_$n-K6m${?#K261+WeK493Cm$AG3uh4)wUK{x-(RUrPWx zIl;Fb5~(Pc$HgJw!$}5g(P;Wa2dRp9JXZ~69y@^H*#nlM_8>Q}&I}xZXC=^7Cp5DX<1k2tdn^#vOfo-nQ zdnw$4ZYX4A;jN?-hB2c0tax}7!{v04Kn!LpG@U)3ar_j?E9hoyENdKYc>eXStw_oou@MN#ODyMoH%p?PiV}F;%JZV;#}-DznW?&*Bzwm@ znH|T~Wd}Mo8orRS^&IiGSj)k49xe{#?m6&V8tg8PrTU8uPPKuF#hE%_;qfZ2TFL@qY^rB<*T@9I!w z#qkEhSa8`Ke?h>Hie@}&P(GUst7rHx@I;n&1j~Eaq9(;uoXLf;`eb{W?OJR`;PzQP z7CD8?@f&IYapdilTqL(Zq?uqzF~56xrFr8&NF>aadU$ot1KPXYWpokI%<%sJMlyF? z74)yg^F$46OQN2wj4$MMIB;4mv~96fwMiZS01b%2g;BzI#{(FALqJO=#T%s-1D6Ed z_>i*>9*xA@6tl8KL(RGp6Ubda7WCKZSUUo9#YYCs1*Y7$N)Fc@$s)%49yU%y6|=is z@;mglO|nbQV$;Ea*Y%}1T$>%rg&cA|x>r;JSjIFl!s6$0eo1L$l3en|5NVc1GgE_G zsM_T6OKI#``nsrUoq45XesPY^+pA&P`~709aH^v{D@ky3no9`=;E-*9njCP5Pb|rF z&p2jXF<){(_l0NCjWMRk*lQ0LMr8j0GmE}!E7NqYY*560xDNM~yUK8yY_*2dyV8!9 zCZ7<{n=L6Kk=COv5&Dsh!bv0!JQi`G*rhOYO-(#dL}Z!mV+R*Ftj;f)oc&PHU5(+^ z5xMZ}T5avBS9pT=nQpHsTP)4{x}DQnEowP`dM>bbuF=jU7wnU*FlF0{L68n;0af+z zP*O`*RUnQSm|piDKrcLJEKY32idW*%+7?S7V{Gq6zya<8{p)MeJ~368@whUM4Dd%; z{9-_EH4ZoKZujr-RXro&R5(5xLxoYXp^b*_aA4N+bE(Af;&^y=C}q*K$ys{?tjg3C zH8c^;ERkK#aVF;Hy=tf^s4(W1QBpwwJbJjmzlzIrUs~DnT@h=)H>rer@9-gMOugz+ zR!(%pgpRkC${Ck#DF#wGslf!tOWqO;${XhIOiNP8+o|$3gPC!#MgXvMskcg~>T&Eg z5qDQY{^mPb_b4-Fx*$G|Dv3{EWxgoD8h1>abjuw+ZK-oegAQv?Cq3l*fFBhCOm`ZT z5!SLh{g}qc44F=r<8!mLZU7#zz&8Ygy|@j}sdZW!_(mH}`RXELx|SDecn09VTavw9K!Z0rK&+ip3kN}RV;*PkYu8isN@v>##oR{NV; z^46NnI%D--B52m8w4RdlGyD$}um=TihhcS$mUd5v%0r=eVKI_5TWcQGRU;o8%VcR` z8xZY?TRrLno8QGpsf^j+gK)ES^HoCY!!zO_{ZdrsV>RvF^0q^5%bR8y$ezgcy7t@??K`RNi1sS{NxfZ7Bab-?eW0dVUskDFDr~J< z5R4m%CZeMfn+iBbmb}oJdeI3UXm;ECllUevjNp*Z78PABD3yV(Y);dv*RVJ7_og)P z?@dG;WO&}d3l7D2UMW$Orv%!6Qm%_rNj(^n99SIa05-9)9@SZlx|&J~x@#7{sdc=^ zTHs%g4oc(>L|k?h`Cu^UEgD?g5q-DrS}4|y-5I2?`ush@>P69Lh){{Z1tE@p!|OfhE(I_x?1EO8oSEf#I{3$z2h)~hvT&WYy8GFD+VL79#S zqU$M(%K?y(BmfWrw)__={EKT1ovCHdA?^2BP<;l|)yYbBt=ceq(fEIv9+LOQ7CbJ)jRG>a3m(2Qyb zVl9=^wl_A1rG4Lm=^CP3R$R zW0CB2nNa)l8n^@=43E6AQ_5}qBq84vsc=m_S^U3p>xg*7#YZ(nR_f-Fmh5o~Q!#8! zqxs~n9ga61^`ohyqnMlF@CRFR!ddPcB{wb8OGIjg1|igkXfnrg!0tA^t$T7xd@|~v zc6xhSF0Sayc>IbQ970)ZpzCR=op~Ayy1O1d#aU)Lh-0c?mYmC|A33kmLXn4Mrqs;m z(aBi<04Hx`tTV z;F`m*{7!=rg0;A9Tl$#;h!Hs7y7pYFn;Akn(^+e#e@!xUX>l3JTsF)C9z{sNTRbM-JkJNu1#_KV8gDhDM{K%Wn$F!ou@Y|62+jGgh zy=uz|!Dj1WWH3o2^8ReKq?aBZYU|lUqjpii+(Q}-eB#Blz+J8T-KV*5Z}08+e0?JJ zS(Djr+x~lzzKto?eVqk`;&SF;)59Ed&rL9i#8~Jdw(mR?sw`&`pnIdEs&rB6zg7S( zzkyvpr0AO5sZd(bG&(XZ%gzgrmtIGBt`#mbQ;Jh_lI2P4dK*g$cIz9r)YVkt^^|ba z;!zelcMm&Lb~=XpAT#DIVR~vqWA7hVDE_($CAEW{dw7*t2KQ6yP3RARNU;3HIK3#rLB)83 z2&s-dT7p;It*2wP9y18O51utC`b<*6~^JmF?hFbAwR9x_^FX*+-ZOVJ6&$p1ztjq)_ob& z=cT2e^N_^v7N%W>Vb&y4vfy{D1R3WLWrVcw$>DaDHZ1pl&QTC8#j;=In+nyb=?S&$ zY((sNyU#M@vB!H1xBF zMhLq|a66S`%hG}GJ@=Ut>5E`T1(+&Ci}tx*1^uDkG0*_HPZ zE)P%g$^QU8274i6-@&X}s;;Jm^0s<8APo)z#Mp!F3qOn1&0mPkRVyzfhQYSd14j>C zmqC&!6hWOE@>V|03FN$663Vy+}s>w)-s-cptx;Fr4=OOz8O=}CTGJl z79)lnJ(IE2xNLC9Bxaiswl*KbawWN;!02IK2oc?fyH{tTs;n7mbNXIak-bj7OH?iTtXLhvbQ}EM^ z>M>T7W8Qf%(!*5YrX0gq(e`C+$GEDh%)3!E#z$iG(>(HipijA9PUc3^_lE|CmmfE= zX)`50PkjVa;7*a#bqwnxn^Q=WnHRq%3qjncO;}Rq`rWpxnXtws-wNWs%mvvPx zc9&S$+z(>QLdM$SIgd13ZDLwIJh8XvUES4==|5qPeI-+UI3;h33SsPUJWAfOYNlUB zL;a~Pl35%&_CYYd-DE)V@mH4x#?$$Jbh3_KaUUex^5!BNcD4uiHfF5Tw66Qqv;P3q zeuJZ4n%aLqxw?a-EG^%if@XTIt2356hGrZ-3VLeBKx3DzJ!m5PTd`zL3yOnbhg7=X zW-{UKQD>y8fZA$#V?B;x3o@ZjoRpm`WB&jp`i*sEIP&xT3Gw05IlwlemG?YEpl#c| zki_sh=Dc+bZA4@1#^6V?_f-X_u^SyMXUuz>;B2+4s6FETKe3tHK9A-5ofp_eN#V=G z+2&eRXX#OsTO&nlyVj}eT>5}nG_pOnzLIuTbh(caFKZptyc2EW7xya%k|J?Eiw(83 zDdnf1cEfHfN_beoT?2`xtZ^sJXlU*i;Hc*F@wyiF%Fo&g@{+oSnmJ*qrj_n~`kL*n z*<}&YVGqQq>l;&57gZx(Sm=X+vAwTxWznxL){2U2IPvrIGSZJFPG+<_URG|50+pxi zllsM3U%g{6eCvY8M-@Bdb96S>BG*>5*|Pwc*jL8Vc{^q2CByDh?&sLk`W=;ZMfLp) z;#0J?@|Jk2sPT+3{)KcW{6SBms-|^A2&jtPy24DYYr*=^Yuv_0wK;7i@hye*p5+q} zJF~#4F}ykFNd$5rsCbdQtpIQelhF)Bd!(M!d!9(0e#DZ&w15fjRQWB1nP!x`i;R!; z8619U(r1QK%Kref8;`j_!Hc58@aksD*vO=e=`6j@kWW7Kb=@lWEQs8q0HQ8)NY+|K z>`knTtZnvi^{U{uM*=jSx3fw0rsu1th=$7hJo+sIijGzp9VHdYRE;eH!6S+g1;raq z=!ydRm5~g?;$&3x%Z5eD7)y_Jm`dNscUUYjoqtUguRHZLX5HH`eAez>DU~`eB9ZDL z#V8cn;*RNMg`M_5w&@ouHJ%`SPm(lMRKcV@-$Nm7&epw_i(7eI)l{Rq`(#yW%8VT4 z(9`L^6axjg1zVQlk{lM5l1(hG(n)-dE^)aha#cJSrYP)Rs`AG54w(6-AZ*df>9nkC zRg}~gc{dk3s&v#`zc$5I8xX0_H6g$_Hl@dLdubdHxW|5qfM<6`~8eGn^x13@%sa#{4$A{E1qei zCDM{_=W+{4H6=8S0Mo^>xH<;KR^-_FtO_gfbBn%TC3D=k{6h-nYC9%xwa;T5_!ZBf zkX*-CP|o1tZ+J}@aX6SnRjqNh-DG$8SzB7+N5mb~QA+-8YR8j7mccRXezBDgFws4+ zR6ZNDIA2M%_?2%SD_-5?!@)nJBCW_!gO|k4Z|A3yUu%lL>YMgQv080s7%H=YvzewF zO@>Yf=r|PM%+w|)2gOqnBY}*Ydk`?R@dMb9X_@v%u^i;!ZJ6#Dz+FOGazk3!%fonC*0N?i7Y8Hh zFv?jU#K`D@c|Et|bzsU!f%9dkXzc4r_@+u~NhQ#%V$*GrS|p5%M>VbX!i8Ci$3*Km z$t0HAGDp1fQu7_DFlvg3swQNOXbn2Kk{3Vg-r;u}p4%<<7BR2u%VctRpAnV2RN%F1 za(*XWS6v}=@7nt?Y|x=tqB5RfA5tA_KFRj4K?o=;MPVLVbCl)HCzg5>t_d!fa#b(H zDcOFqq9$97-U5SBnQ)knlUJ?ZpwjPn7F_ewnjW_6Dbglp7&aDQf!D04X>4{rfZ$#D zcc`_w(v}#VBWb5|8aTLy-??FE>MJqKefZT>Op%Ot(Xsa|xZo|=J}Ru0;%<64z{V4Y zy1KET*VTaM6UB_;8{^FW)pV*mkD z=9bh=hqg)QmQvXVru*Asry2%w%YKUSN=i((EOj*zK<_DW4JVeC^7Sn(PH4j-9%PbC zdm~}^n+42riv#4SjqcTBalZwoYzzon5iQG_jM`jnO-BC!85{?=T|=nMIJBbCbubRl z-ACS{1~hO9Knw6vMnq3riqwluTPYolI!C(7If2gn;dB1;iBhbEk%9=#r(om~+Z#tD z^^p;}07j4kJ0)2ikOn`bn8~1l%^L>D53nnYqD!*f74Ll(-;WZbXVwQ`%_sW6J)neH z0%0y*jh44o;?-ZwU-Fms3q6=|32JIa4Tp=&^JyJ@fb8(?QnNNHaQH+}S=BP^!NiXL z0Kpzu!c7bYdW$-)ZQg#!>_gW=7Mhh(V6}L250im3Bwfw5ta<#@?5i#Y+{I4Z#T>tr zNO+e`!MS15eA9iMWuQ#YOHVVkNedkA+itqlc#kYOCcWs%JABKT8EB!Bq6+uW)x;T0 z$G*>y0^r}W4-?$1@*O4L9^N zWq(uE+zmH2wfEuLwm3aiV4{bss&N60^IgY+bt@`YOI)qI_tT~d%Hnihn13(1IxJQy zmcJ5?H|Sv`wA%YB9LboUioxWuz+-~o`Nz&BT$zltwJvi;w%j|-usaWGtEZ>KDw@Wc z0~sUIj!PT*_FWs!7ut4eEoLhndKqY5IP>t91wYN=|Y0J*L#kXVb2+p*a8s)pblV>LsFK;g?%NV>=9S5(^M>)`(Yao&+e zUGJJ0hcRN&iy4_9{Wc4y9hhA{fmcNox*e&duPq>?oV$si%|{fYwX-+oK~SnN+UOl= zsU~Urg!~m;uIWB$C+}Z$N>nGJYuwBDb0rDVA4@X_=#YMDMK&#k2R+_U=7(~7EoG81 zxu(u(Y3w4%j>&56r6{=J069@8^JfL6ulNr_1tyCNj~RrCyeaW$@y(l~K*fVoxwOzS|Sp6sFKZ7|PbkFmW2RYx8$Z zW=KDhWO3x?{?Jwt0&w^ndyNeNc5G_OttmR1wW{`*t{|Z$%cgf2A;f|ub8L4O2I}&M zGU8NuVy7?Dz5xY9y=jn-J*L1AIa2P3=E0I&1r9%4Z*(^ z(6hRQSk1j2`G1k^rK%CNmT(*H0bP=?*`;%QOt@zJM1iQf7@P-g#4jcN zJH>A5S8$x$SbOW??>k&V){JTMDC1M1bR~w;{y}ypEeVda%IJ%pZbLuA>@mU=d_(arGK=xjJZX-##0^d8W)mwn4dh&nNO279iowjqHz8wwL9G!%DHZa{L zfO3s@b6S0l3yERvYq2QXChR2Y1nt1C-O?})$~cerB#&z0kW(_Yv9!`T&1=cIZywd? zzouXKT7Exb_et_Lx@6U2ol9`I29W6NG>hy{E!J}pbdmk;ix# zxo*+)XI4B~4|tjTAQjW_>Q``Uo8VJU!sGgqICzVyJ8Fo0c$Ejfg<1hmI9!~hYHiqc zNZ{7H!O3ooMbLC_0KY&$zbH#-A}y`K1nji(#BBC}e(+7XjvQL=Vxx;&q4B8+Y{c68 z9l~5h$LJE%+>^~6)E3jI>{S&pKU7s!5uebN&G;2#7GuYv07Z$+&^ub!d_h$==8P0& zlyso?Mw**D<8g~j=qK@x-vur{kTDj!&!MJ!dz@N+YaM>nIH=I0x2F0Vnw-%VGXrVG zE8~@?SvLe*g}v&(0nBjWIJH}9DW)+H8rQL)?{ji}qu8nN06ewFkI|oqQLFc?o9`9c zoSb-6QtKvhV{Hj>DM!L7ohg~m1amI=%MW9iLX%0Euv(XBj=r3`u)G4fF5v8pokGq5 z-XiOZgmoR7=*_1}GDSNulN;La;|BIO>{}ci%otrR{{Yal_Cn(Ev8<_pwT*4&ZR|W$ z=hs)p;>fFNV`wL4NJW=QyjP&o_OqEv-cIhMS457-CWnhSHN(Gj+o@?>Ea)O5?ghzV z$8o3#G2(N&+@xz9(%N~v1d8LuP@sNGRb5fLhtk)}9^;tSA@N6;UlSaPHK96Ey4~ z_oluD#F)VG+xc3FG{(BY|B$Y zQ%#4|Q#zUw*9rC-i5v*!vZluKRy~YiG2ytabhR+YAbTU~SOBv~zMui-MV6_i=VTrj z_LjM=aO@)Klz*rrPBBiB)6ediN${s8+MfPjxy_;sU;xDJBau8pMDo+b$IB&}#)3)g z3!uw}VeWRPQbm5@Z-R|cgHT8;A<^E~5V{fmrMG*K=Nr$X{3Ot!>K>8|%k=W+j;I-4^)-ut{9@PH;aWdhTG3lRqo%&d` zhCWTtZv<|2TZYvRBe1KvGTlYCsBeHjxl!UxZVKqBD=MF@&D7HIsJLH^!q-cP+Esk8 zsJ=M*h@GuyEXpjsuFE*h78Of~Pf0}jYNl(9og{&4*n&9jsc_sbpB2QZXignSWvZF6 zF~F7pTtFAo;-yKMu^P-O96C%~O-BgR94|XGZLsIaDEM7`_;xu%iPE|kzLG&GiO&VE za3G6-LFB6?K3B~ZEWI(0i_d^39IjT^kL*8=W%_KnnI*$tM8;Lq08Eg|=P>GMJ8C_L zv1hSrYD&CC{d~`efni_>veITOZnrSz8thuWk~(>3({UNQ4Vc#43z9fSZgk_z%{HR*JGZR<}?eu4Y)Q(Vy;4rwChJArU* zvVlFihxjg4?Fw{SF$pxe?gB49{ z00MIF6>XOCzCD}YQYb0ts$ELfx$T!rNz_jQd=hH#c)~p0rWcz1%fk7|E3p?kB;Y@tDn?B6#l#!XA;Q0@Ben z-UVl$mg+R>hO(Ak;NlOwCclF?#}Q1&4(ZwWtkSivyVO(v0P4P}4OQv%sgHX#$D|rc zifLe)mPmTW78eceaciYXn{b&VWA%c@!XgZKEgJ#8+wxdCj4Kd<#7jvtZO=`|v?%X_ zW(&SK8{*OyXG)w_B}shO>N!-K)lcd6I=%yp;Wd?Pik?`RBY^-3-s}2`zv{u^D@x+o za;`i-8G^2GOBhZ4V#|L|nJ{^Wn^@ML(gb{0I_8{tvk2jQO_XXpdd$`JFXZN*TCA*X z@gb~%Viag`I%kift%=v{kSaXKiLxfj_s%t2&9k>zp$y9doV3Z(YJnC^+hxO!X~m*9 zhPp=j)VOojKHFZTJPu9^Y`AnY>eTqB`PraV_Fi=>=UmTa`JN!|ep z!VZ#!!IFT3+6rIx`RCP=e*Aq=b$1@AmRVF8dQitIM+QNl`rP0CD{THHa~!RjaI8BP z#A{}fR~pwf$IL!$ueZHw;?CFvzxa|oLbU8Fx9X(*jeW?C)g-le_cKGfC07n(!6x)8 zm^@Ut@eDpsLbexwj4blAxQffB{tRNTN$n-^B#t&&tlcN^_Q&>OzqG9+azf*F%^3%= z!DVw#or_Ct?8brYU2o~?Z__g+vVUU_6rcDop6wc1FEWk>_C!7lA4*864}BPTMDNW&w~9waPHb;#s0ss(6ak9VAP}Z6y(Ml&aQ$O&n5)v z;=J9=xvNfIE&Wcl6^e0QNN^PgFvugot7Jb`7aw3#TB-A~EXy^-bBvE}-!!PKvhQJ> zksgexhhK?-uJ((on<&^U9!-PNQ-S%oTHkV(qE9W7mV!|bmu=A~0lRLF+oQJWKqzFS zHd>9M$-*{9(Y=sJP1!ookzx|Z6cAA7xl1r+v>#4pgX|~du(#Q3`a+b5DIpvSob&1y z9}=8XbhqegenCbz@~07mKaKY9j-3!(}XmxtGbVV{u92 z1l`TlL0aNBD;JksS(&4ZFla4s)H&>STYGU)9g9nGyCVa1q7YJ4O_0;FSU}p!Kt+(! zm1G5!0FvF2SuEHDlIGuvhSJrNmUbxfy9=-h57~Q9y=U@nAr2XrYAPjrNe}Z`a0zwY zvesq@9b1IdQ$#%Xx6Qu7;M^>BUdxqEuWUG!eO){x$){o@adqwBlIp{oMu#d><<%EC zQxBT9pyRlWog|yW!Z;7L_$^89NMo8B0I&dX2|$B%<&0=1Sc=ZZGOoS<0pg(CF!&B)n{~Xxib#k$(jt*qa|q1o1K5Wjk01lbLUa zCAQiXB?PtZv?{BPbLNhNfad(4mn1aVw=YMEHg^ZZGpVDP!Un_u?Zx=)xW#9a%ZX0- zTud6?VKOumr+pxg3oE=zTuT@%lwsX#L~dX)hk3U=Zh5E}Mhh)gHwGJDFvG;HdFv$` z4J>njIoY|`?UI-!bJ-~rr zZb?x_?&6hMa_F(Y*GvTH!fR6J2*e}AF!{?^<~fGWYmW~O)Kn~FE&@rJt3|_JE#3{) zW=(hTy0|$$An#ZteA4j@6CTB@*xGtIqJ@nN50Tm$>wRVD*AV5lg;}h8w1*MeFJQBtZr6>?A9rP4N73dXs5-9^f*?Pt)bsDM zzrr$wEerL_S5;7Z427-s55;Gr#bX4Mr?;Q%UUr$+n1Xt?u(-Xf19ER`1gKpjeCEvN z_F2~pM*J>~%+b|YaN!r+EfT43QMkM}*FU(W8j#gx5i9cDJrnw>{5KGym+Z(3NcA`=6qro7 zWdmh=bzVF0lW~5&o)2>bFu5vd%y3T(l69_i zwOelc@G2y@9w!+9QaWDFsE7O%igxOrNZ3Oj=N7kC+ncVH(Z(;9@XA`+Ja-A35b~Eb z!dRNu7;FeO4kK?`$8%#%;l?fd8q?xFoy3-M*S{wsnv$vmYKm!LJ&$2;#YS{{@jlhO zseJmLY2|yNG7#3j*VBEjJQ5t>U7~7QSz34pMn43n!TF}+8EuW%kC|nCgqvS84b_eO z(dc?^o)nY#{;1fh zJY~g=#WeAsqnbv2*AS*di()bCD&%k_simnW zlyZxo0P7O39^O6I@OkzUid?*;Swn>#1cOy9!#X9gBjK=4hh9v0$> z?52&aiZ(_q$+lcJCrH`ib8Zfy@m;bi`2;Z%#<+ED;yaf=h)xnwY28DgXw=-tMdm+n!q>_$lNI=KXFYg-XE;0nV_Nv%y)HH*-V z(aW@A6<~9Q{_vpmurHXfS#Rk_BYlVWstgY~vk#oi+&DGPZkK70kVmaQm~%E6m@261 zDmv;%Q(sF&VZh&x_fMn3>c@=6=}B^9SDbXl&kVs*;X6l3r=Mro+SW_NUh4&lI1J$- zxEkpgM}3@C@9>KWs@au{Yr7q{#br`kV#n$lU&rTY8CF>Sus& zw`%8qUDJQwWc;J_Ex@1pM7-HUn<5o>4QvMXbPpSN6~i*6<;9$2vx{RSZNRSYTDw0r+>~y4}vcY5#MFfyE$0Y8U-_jzTvx)&)cdT$;- zL}sMlYcJ=B+o6TV?us&d+7oZ0rhgcK@YzT~b6g_#M%dMyd##Y2hf|TUc=e{v7BA+` zM~hX83XX+0&~$HW$7vNW!)FK0a~wU1vqMtd^k2ZMq!Qh%2ScgH~@^tt# zq(KR(-LXp_8<(_NwF-{PoKnskGh5zKSCuB-6H5eR+J)4&Ve^5pVel!pQdAG< zo$6etN^5j=<8ww^Neo`d!M5n%6pZwvaPbKjRl{42O-W@jWyJGD zTLit*L2q~}j3ZH!j^3N-YGTiLn`|Y+Ddp`goVNbrr$wJ)BkAzD9B*jPCk!IQgT8iBaLB zZX=1Zjm|6*>_{7dYp(nxr%#{A^Ob4#Rb4&s_bqxqkjGt2&74 zT=}0y!s?SJn|JALuf5c~F9^f%sEDG6L*4*;%ZVOTGqrI2JX2fbs===SjDBT ztM@WGT=xvGWhGoR(@R@a$(&iU8yIM5@#0lwQ>{+(+!g*R@8S}AhrBH9U*;USeXQy% z&E(MG7`+iE4X-AQ69n=I{L)x&U^({${vC``R$^j?l*;LVy`JAR*;dfhzLO7dWLnJn z*mvL)Js-f~oLu=>TjrfN+lzo(s_`n6Qi5wlzJlRqs(Q^l8*0o08DvMRX%05Cn!x`6 z9f4MPXB*8qRrNAqtaKuv0P^@QG>#m5$=zz(zYYmT_Smjk{-miTij>p%{^sYxV6}{z zGI_5zV|kvMnk-%MJkq(wKCUp^>b35DL1pIpM(He&ZZ~bg_O77lA$ipJV{z>*1umBh z!|ERQO+^Ftw(YmUY@z*0Mk|-H!t!hEjPv0&8_zyWZdAo`({M~y99D$7u*e$$lA8Pm z=%@p2((>Hiufo z0;Z=Fv#h42Imek}D@=PZ-XED)!|o!INj!f#;L2ukf@!R2Q)TE4j!ed(~4TbMhdQC zqL;0nX4h8n*XkyvjeKX3=RGlud`d{0D*-K`QCDE@x!QQy!;+m_g;mf78iQuO zM#jPjI-Ga}nu-eEtFILLAr0Ne_S=cN>%#KhX>!z0-oEBJP^%ZLU%=3}CUw0YzcrQmo7rH5IgZdI@5tc9}Y>G>xyY7AJ3I*5a!;VdJ89 zANP37Y&OnZ@jXhus{wzz#vwO#{;Q&hLx$r^rXiEwOHm%bfUVa!BGTLBJ>X>*0U(q1ZCCD}WhZb=g0 zl%CD}zhR30eNr&4ide}Hjm&W32(nFy=C^RqQ1;)fY^~sKIHw|PvrNdmikf2cq%cI= zl6V2K&n^h}jQ(G^)qmq3nSdIB;!};5Vd%*td>mIo#PaZ>khc`e64Sk-&huW|dk6}x z_-+j_8fdC1$@?-B@w!6cRkwuG_rI{mc@NDNW+TKa;r!iP&EM@6tfZ`}I3JbGE2Nl0 zU#Z*Ni`f=o$tMG;Wj-tAv{FdjY-Ev(xOV%IRz%paZzqaDV!;@5w%G6{%WZ`VihS;G zD;jH&7d0tQl|^;%B4Jsd6|b+Rq^yyWu)4sO?lx0#%tf?x&5h6rnK>ceb**l~t8mO3 zN?Q15byYLNZgjE7R}S!(nW*b4T}>S{GE7<=PM+PAQdQ|$&zXAJ(@$Vevz>V1>5sVP z5t?Z*_=_QerY5z)&JCD=;yqiTRf%DEhASORdTE;G+p26}?|seo-B3$|;?WIqVf7)S ziRz7qu|snn$JxQmJ>zxF^qEts+D;2T%o_V8DDuWy9fqGfVFod&#h`3RAb_kP%(yIs ze;jK``$Efx*pNk!-3Q`}RDs~N#E>nq*{cMo+DYpMy^rscN~>LdZ;|K-F>Eqp^mVX0 z+_eQ;o+=`jF3A)!G47gJm^8ZlUKbK=RNxrmmFIhtdjacL*d}CjRWUEB|3&OF4s7F)n);W(qk0Ol9Es! zKqZA+>zsBK%&S6Fq~SG&Zl_W;Bh3~^@*nNJtw?!^%A3zhE8$P3-4b{-pGmftkS>f0mJx4_?xhajBm7G z=BO8{vyUwZuOr1@%bMnI&*%hqN#ugkV>Y;zV<;(KJeIf?XzePKD$z+_T>Ngb=gvJq zJkQ$s1s-=lGUGy;H#xL%Zt_0Q;(iNd3~Y)f#~VXh&;vl^0_Lq*>E4J@2$7u?!(O9aMKm)v`HsGSSsTky?Hnu9#dy53$iZbKLGc54L+sia@j@P&kimj~7 zI87g zTtYDBQS$ndKP2L~gbfqaJ+Ym}xh@Z=C#f==3>TiI*X)U)4{5r+q(8-Cw!=W%)SV7k zrFT&_qlN54RVei^w3KxJ0LpR88)S49IwLOP8XQO9uQ2Qn3aG;)l1eDc+zrSi9zH6z zW|m_3s|MF`f3&o%1_0~fQTG=Qde?UZ?bN#EeFajJRT?uGT|88^*n|+YHI13T4(*o9 z4mY5uBl@QN7FaYl9j|Tes$2V-w74s;hsHk5+w3J;@;~V*wbSfYi~j&LiJXw;h@s7K z&44NfV`p1|)v$Ak-vv20SkAJJR(+;8dzIkvBtzJ!$nMvH1I)Vr0ICqFC7FgUSJFMV z3NJ^mCkQM&tbQZppTzdZX^H0Me$wedf2o>%Q~DXD_-oo;5wfGjf&8T7Z|Iw2+r?gs zDoka+%52`(gmE3^Q-uEjxL&}!{4y37Oy^R)?H*{|-Msf6^gzJuN;*aXa&)}gaef>W zsw_&5pAQu7C#uhkZ|Y4-Gg-pVz-@bY z#||b#GvN4}tBmZSmPU18lN$-J8(jO96miJMHcbx!t{2<0b=-39bc-Wn!xf(lekByt z&!>D$lSybUYqf3-?W2c(6_e?EHengBG1B7r4jSw(F+|v>i$hpUf$t^WZpUKX`$?BzwK95Pxdni+1eG54{#^MDpy_YD@rAahNLc(K0)E@sBy z$Fw!Y?el}I;s{EL87Z8~(N56QaK}0#SWm*?CXAsODS3^kq}Du*H9O?e*T+)(o%oN6 zc3I|{ibu*Hs?Gyg?B=r$#T^cb9Yn_n&F;xdd$?I#;W&$NJSJ(H*;}VmEg`uU7qISL zs?O$&^9pm$?_B$OoNDxEA6iky#B4fzK6g4P@XB{RfNFN*mG`o~#d35sczq5dUx&{} zOJ2hIcf~tg9tDZp&LfD}+lr;2jwq~Wl0P^bvO%W)v&1fen)4k7C(-6!!zo;5tiyxE zvGNzataxw%L${0NQi6KxroSS4MzYkJ_nhu*28Hi)X=~owZ)sWcAA*oD>~c1>l(_3& z0NI98--xo9(${(BJ3EJ?x>uM_E2X{>Z#4Pi@Xp@+>RDdZk9Ck|k2k{>?vd^LQOL)i z)g@Bb99B!VwAH)~?I1QY8VEk{&(-;+d$+@b!15#F-<$%j41WD>S%e%ywD!G&NpQd$LYrsHmof zIVq)dZ#Ej5Y*lPIlN_cX8mHWHcvg7qE~tViqbx7K)^HpLdWDSPmDAKVSgB^1XaMzX zFR|ZaWzgbu>bXi$&NS1ZmeJGYN?6XMi1WTHr$s|qD^AA5wN}woU{OdU{GBv!A6c&s z_=R0GPKo*?jqP~i*EF`?)Z2AjpEP^d4xE)N^1>_s0IFX^bX+;Qzl$|Z#PFxK-ctC1 z`*EL=O(b-!fvt-Wf<}@^Bh4Ip)?LihZsOzIPRIs?WpT=f(Yjn-) zAZchVdmcMl;8t@KZKkLPliaE!GDdG4j$!>0La;7#>2*ApJ5J>BD{ug2&O~6%>uMY=z@hz>+ZQdsU=as-d>)XkwSa#i`SsI+UXh zY&D(J>+H!$#&MIDGMRDIlu|}m!s~Y^QRbQ#i%CT^N3=LNDJ;ckE21&ez2ZPY(&pP` zdpbC;2Be<8C2P+#+G&PZTuJA2lMzy>-%IB1PZW{N1NcE?>mb=h%ANNhseL|zJ zIVikjC{&83lVxd)q1BaQY0qLKPJ0(U#jP2kJd{;7W-Uj4$NUta-IQ!`rYvHNTE|QH z_$wwvO9tGRLgnV#pTG1GPw^y1(u&GUq9c+)jP@>Zpkl~bI#^P4p2Pr)BC>a@BydBk zC?qIc!6lyNw&QD?CV{@-gSv6lH1%6lMW~Jh8xWBkEQU73!Dh5rE`?~$HjvKEvosuC zd1G)Om1Hcr=90d&+Y+Gr?)WJ*ThW8tE6=@0uOG^k1N5KpPhrPZ3vKq@@hi7S{e->$ z0HKDbXfBrKdb$YH8FI?%*8X0;Qm+>B00H8;Y^o zPvSs+8z0n>!^d=khQE}JD#-;$Z%yfK$SmhEBD zkB=0pw@9JT@Hc7pB#>^j2@Z}o8@e_h5UA}u80hg;`J?kFxgxr=#Nm!QNhXdq2Q&l2 zy&|56t%a|ht&PqAx2rsm>}vA!rJ%*dsJDld$yqBeOPR5uhQmy60=CH8phf(Q2Ajw!+|FbLwAOuCZlH+RrUdTJ!f4F*k?NkUr2r;&9vO-qDpvtR&j_8m=iiQ&D~aSjRyN z&!TV5A}*>L+G^1gB$gQ)Ak}*k({XjE!Dwk8lB;VfDWaBG;&6@6ZOc5~Ue%J-eIWgi zk$Z6(msJ#2IdHy6?eNGqZ52IMzp1^4q$@NB;k)zGF5d+6dO?JbI}Vh1NEbeDf>7{S z`BBjHdVU8>rq5VRrqeQ_8}lr<_Z4f0XFR;X;Og?*_Qd}H!Ey9xho~8`d@74NXT?31lOGgqdzi!HPE^D4WrKE+!^e)1zvalvrv((#R zQB6ySQ&LZ2=GYzYW5&u{w3M|Uou`TCw6tAfV9oG}TInfg0jA>9Y97L^AkJ80fYv5w zixhg%mo#1(M{hh~#J>EIhCgFS&rvEonQsno+0(Rk2alLcQs#^zQ)bn%r}XytsnKH? ze53}})05aFABxip`?`JFlK8VU?5RHV7r?Eo$}}^^SlJz8#66+F`z)iJqbPTqQ4kj7 z2wFze#Qy+BKzs^xO51T-Y4N-0eti2f9JorrSp33F>I8;i>X)=R+f3rN?4Hy_T~f!YT4*7agtqS*|`V^S)}< zXkwMRWr{fmW10%ozp3!rojk8^>?QDhGN**~3X2_gviZ zy2(LUDe+o3q@{e6Zg4Y7-$}MSEPIzjb{|4oTjX#7+`tq^;COS5`nR_Hn)0hpjao%= z!AcsZrXRa-mk+MqWYmZ~Fy`DFHfDNytSrjbvW1LuBxaWbt>?UR3mcEN*mGIh=7prp z*gXXlaXuK`sqVGO2Vv@2%weKo6gxO;Cu7*X1&vr%wdSR^%RbYYaS2nciS9+g8q-%M zmVj9uw&riePsAdnjyNjk#2*8nE1?VxzRl8ae*ISUCh)337hNbC9N-4WQjb_4B}BUA zc!jRMk~K6NjqT%QYV_(#hb=jByzl=2u%|dGxsu8!G}OUYEO1mvbUd}Ew#LfBux1(t zhgCs5EhfOo#{qv7#T^x7PmZO~OEU=T0nk8qFmJz#x5aa2Eso*Fg1Vxa&<6)=^ER+}; z(_+)H_HsyJC8dxJ4CeFC@mtJS45Y?d4CS#^w0W{}e{+XF6^z8_s;KG=HSOvwE*cKb z9ia9vFL`59+FI)$6R$$zHCLH4FJo0WWn^ZWX)PGJBZb#t%Gt6!TA6^&rfY5wGPqUV z_J+6UVI29N55-BttEwsHjM6#cIE~5GV3ExV(Cq7N(e+=qkk!pjT-5#^C!UmI!bT-LlD0o|o-ZV0%s2b$yD+SIpeI;XSf{{Wr_`Zc3fX}?rU z^0sq~r8cSudO(j5p&(XnW;~tNQn>ndJ_${1E;~GXRdtaOx}I_icC!A9�vs^n5$peE|2n!KP6a~R^s}(p5jR|5=rq*h3l!u zmI-(x!PS>M9 zWRYSLzBb&Kf-X62e~Q>rzpSKz_VlA^_bkqNji#qbcAZf*aw#kVHEeZM_S>@gYi{U=QdmTofD*YYq)%qV?w01LfvRCrl&yx4F`?5EHz$a z#_GKQ%5m7xtLi1hs{a6-*2?bd?4KJsz1W#uMQn}bPAP{|n@d*}#}Ezv3aYC&VNqSN z31)ENU;sb&;IamqA-vDjdtBs)OGO^qfjz?M(XxWwig!}zUesqAr`%IfwWpDG>bj>H z#;TiV)E1A?+u*CsWOuV8J|v*o$s3z^Cq|7p?1f6K<+}~>O*Zq+_xP0L6hcSnjyFi| zz%Ekao))>Tds^Z0oJ#!ZI7No{edU}3i#t+p6f5cY_;ZPSqp6~p4DqokgjTeZK>B=RjOS#9C@ zaz=BGcQ@pV6ReD3jHhrecymiel6k1PQRT4L#74rRgSY`MdN+$zF z3H<@^Pqf^qIAgCv7j?(5l`XE_bL&Tzq?dzZpz{>Wty$BVF&3T%JC9or;H~nz^-w*Z zW<&1^s}*nh$tAy}jf3JoYJO68(bP2kru-L9uC{5v1ZR)q5GyR)(K{_`wWr)xg9SFF zaO{!GKaB#+)*trZY4#Eo?hNmtly*F;+&*gvzhPJ`Trz&7FS;V+TDM$7Xsh=l;w@x? z;*G9A8JvseLimR!kbO=nMrJOGvX)jh?ys>pX2_8&-{{YE$ zGybt-s{IUq@V{^B2E~{wE*$p2)Ao*=`vuc-(Md;98zz^md)i5Gxwo}nQ%SBE-Z``% za<9h?>M33Kfl*^~Cvh0zCFD)TCE2!B!95i-pRR~YKC%Ozb^`s1J!_10u*}dH{j|H2 zRrU&6*k#yXt#!(x$KGS8iQ9S4E%%kWoZ~8$V~xG$cA8Lm++up$%I42J(wl7P_Xpc= zf?X8SI}d!3H%!ft7FgWhYj-US7)~6(F94AJsSVHIqvMThB6f>5(cPd8wka0ZQK?gs zJyZG&t4YQ@%%H?ui@HeaE88rZ59Z%y>h7@aRJ9U`CUl{pt$DqG*mx}lAr2!n43eeN zx;GuQI05Wc6pfBrc%`vue1W#?z5f6OmV7=Hnx1g>({0TWxs(?#Ld!5^s=ONj!7-^S zX{M*fB9cf%W!jmbfabd33*Ezo`K|6BJv1Ez$xR(~HC&Y}mGVGCOq8w?Y%C4M!*6mq zs;oZ+!i6M7)QymqgQ@%R2&%H~5cr>_jjeW*V$wyI66VsKR--L&)AyrXDzvh5dcEnN zzhbt??E56fV8bS4+ z!EC6H@i=Yv8}Lu8HRmdo9HjLfPZ5ept)jtj>L|LnA$JYSGNaT$9@ zv@IVMMO)_P79Z^y@LRZjS>{VGy3F?xjuXdH9%7CxGFnGAKC`ZQVIMq9(S*f+UKCWbmy#X5H#}gfejixSMvL}y zQQZ{Ly3;YD=WW)Mf>v`pYh@KX*|BWEpJHteEmCt)lu=3AMJ%RjvHa6PUzO-^+I%6j z^)Nsonr#g_>;Tjc5Oz%TVTxebb|s4panVIhJ{u%aJ?zzYk~_9L6$1{A32?eYBy4xl z4anU)B^5j4A+6KkQeD*@V6z={>!0T3`#PA*!_dj*2VRyp18+Luj|A2kRbFZt6jI1r zea|NOw--EksaUKom663^`O9zMouZ}c_4RKobL<~&uVve*2~FtnlMG~{wMRzFm?l-k zvZXyO^6aw#LLjiH(v^Y=(t)fLGqYNC|C`CQhqm8 z_>LWfQd729SJa0&)&r^s*T;gnd!`MzfVkXTHNjS9p&YaU;xN~>;14v53vQRhEos7& zoFKBj-$9nurskt5KdD+NXq?Nl7NXyPezu?4OOcu&b2Ty67Q0Qs@G6_D)OG;+)wE3_ zg_rvUa3bQ~iw8n!<5oe^;tA472aS?!ny9U=pp}oKtCkI9-$Si?DfF1VY?TqVsqL#_ z;w58TPp;Qf*+NoFIB(1MB~D3fL@gtdpO^gF?()JFt6hdp;~TKJ+FCB{1il$A-J>ku_K0vMZoIawf8j+G|Kcm#oTIxKGEi~FGvqJ-`<3HJtiB+w1tlg45 zq;@#17AZu^%HQX3!sf6GJRSXtG0WJ4tY9?rgZ3t9{i`b9T32=DIK|<D-;&1U%qs`h?hPk1#K|Fk}EJDY~({!EG zyUDny$tJy@Y!TQ+Jgp7eajYT#0QjKW{icwmM>QPc@YO)^-0->Ho1j{&+f#k$d>`K= z_EK9&={`u}E&wC6ZkrVLc9X?(_^K))w^cNZw_}J?8H?gkfoi!8yG_*QgYjPn_9K>} z@PE&F(=bdrl3U4E3)&5X0kzhe56LWyGst2MAD{}z^m||VM+}+ujwQ@^0FGNNF=aEG zVKE*njduS4%G70)bgkmK6KhhBHOE5Qn4LopRtrwMYzb6Og`Q0tz&g(rbD3J!RK&)- zJI%fQs%1SLy`S}BLn)kNdj ztX(|V1HBR*;8tgCRyw${%B&w#GmAGyD?#BQ_XNBYYNH*RkHtNhA#*WW7il&PlIPT$ z%iy~YcJ{NrC3Wb*yEh*nx-!m7wa@dZD6tW0oY)CtNgP5WPPk4ucs8a%EhuR!S%9K2 z6&NImWL8p4B_PI712#ou>0w2o05LSN!~vDzfSs;U#`eh2<~TX0m?~6skWL7mCP)Dm z)yXpGB7x4176vdKfZ!IHTFNvPcJyx_&w#aAI*y-IB?NXt%P`U=EB?-#zM%?JQgV>Ylw8ze0=MO7U&Q-{{Tuw{{U`H^=#?! zFZU1Fjg2Lu=*tjrxFqQwu)*_Kie{PWx|R<-4tzFSdb`Kbol{5Fou9QB`7VCvRvODt zwZ>->2A;(DMNaa+{%_=wzJ*CsvxKAIO{r<6u%67n3zUfFicihR*B-!9sWRNMyRF2k z4bJ6^JKwyk`YJkVHWt%D*ummQ6YEdUByH|gR&tX{EPCS5OEQiMR1XC{j*0Wc*=idbSmxcmRl)V$IVKwy)RE)H#a5-2 zT2GcsQMCA_yw+1I5C?TM;m_puri}$lPR(n(!AW#xx*B77LR(1;B2K=VrM{F-^=_Lj ze)nk}rAHRGL!(j>+2kZFh#mJzDhmZ*jS)a>fE{e}uz8Zbz78r(7{)0BuN`#5xVq!m zR1_2t;-2Wck2K>fJJkmc!om0^@%(zL8NwvPsskfy$R+F>?cAy^i0*}>$hStD-5P8s zI^tARkWxC5mS~}kv9#-R2>@HLtA_M}O4frm8&!UNTbkkTAgm7rFR~g;qqT=arc&pO z7Ixjjq7%$MGxAQiH{cI;pUP0M{+-+LT)U410kPw{j#N?Eu8iuKthjzBhxN>^ZOc1OAdA&KUJ(q*IB@gw(nF}n_CB0 z3&}E74zZ%#1^)nYcL%7csfS%nOw$tkH(Rw>TZa6VElqt)_@olK;Po+CfQ__*NC(!t zG_NYVYEqlal6n4n&Q)7j!o1?TJbyk1R;wOKm*gBes#9-MRwtsUgq8D`jX*-}ow(h> zCT!`6RnSyY)zeauBu-|ii-oqaHYyfQ$0NX+=^sfgL6{NdJ)pOK_2w;?Eu~p?f6vtH z*QYN?rwo5Ti0PP)5lL50V=*y@>bauC6^O-f+MF_S*&0g^qDKSZuOiP_L0~hsrUT3v zi@#>+)0r~fD#HhtMnkzs*EECc3V3f7t!VjWmk+0~t>+JoT~frqo0(<+!yK|lcDQP2 zH?xNYahtMjb{&UNoWn(EsvD{)8@r)#9*esjZS|4|V+NaOu$HsNpxt z`W%BE!YVN7^;J8prg1$}*gV(x_8#Q`hrD^ms)%p{is?^$C9*>JgtV`A(a0~iFIeqa z9De|PD_>m+bxj@Y2iA=KkooXjtKJ;xF4CT{`J-LE(x0n6{-rDuygh~i)yBhpUA_yT z8(W+6`nLIRl zCZ2fbiL%7-_BdP+Qd5;xxi&9vlx5eZuEm*!F9* zr1KA&_WjM2E_vT9^q1GzOu8J>Phv4;&ov6axiwiRf{tm8?8Z6X!F1>rP;(T5EDo*5 zoJGFfzH0R2%{3nuF08p}%3cWP7rTi((S$&5je8TphshE&6p$iJr7TqfVM9$gZsEGN zKjBiNbX1U*RppvM(0Gbq(e1GdPU@xs#W5{>09nUn%jG>sHcZlx52GDZ1JAUZqphjK9xWsccY|zwp;J}5fS;j`Ncv&hefTvrv;BSS}oPe`wrcZeQA(+ z^AJ1Md0J9^2S(ze>S!}Hbp|5?YG~?Xp3E(CL#*I0_Dml)Fev<@P{KFU#DKC< zHKF0{aB%HOh5+{7a@Q8z5>DJyyL8~7-h`^nOPJc#XDm81YG~xCJ;uZ4s_JuYCo}&5 ziqbMM%HdH7sNowun|+aVhvJce^idLf_E8&ZQRoihqv}tn#Bs`^dg}MJr;A!_J)9IL zQ#tOu(@68F`lQvLqH^wrTGhK;d9#F0bI5)6Lrk~ zD<|DgxAkhp`Jr_lV9JJxnigVbanFw*sbRm+7jii@^I0m3?z)jI4b;Pl(5RT=Dm*6m+%1S4~Y5nCS-Z zCx|=qO_dec^1oGivZ{W*clHX*4R__bc{H0CEq?t<-v&EOKI!ToZT@2w5GrQU-SfIx*Vj@Vw9X z0-Vje)57icE*{XDq8f8eOntIm-Yosf+P@Eur~xjmxLB^0N{XFobAAi;Ge$hnlvjv! z5B?_({{Ydq;GtuV^XZTFgnN|eDSD{GK_#F72VhY#T1lr7^=);HTO5fYE*W(TTMv2{phupF<-L!Q;a1x9E0Gt)jwfrAr zf1sM3{{Vr}F!Oo}f9{*`QZMF}2L+R?!(lwY)9f}=F1Xc~ElDTh&5(JDW@n?}qYZp# zkslWY2QFAU5NFw_dw*HVc2T=uJZ$gd_mAS8rorXIrKz3M2ex@j$hE9VC3a{vPVbey z@$@lO^{C{#!(Dn9Pw0gz>J}0&hsWJqbyxFoE#d`-Ds#}KF;@Q5R55r)6 zMNH0ah}bSog0u^(f1`#B&&xQjWR@^o#xP_NuB=UCL zPoV17#$AVF)bl%&W`wie3HUBw(pN>f;)@O%a7k-e*XncWK9<09u-$s?jt+L*6Q^*B zRh>C0co8aUO9zK==%kAehACQEByH06o-HGh?@}YZ*I)ktNUWCy3{K7XWPMkh1#E5L z>+BzH-b;`&&S-N@+C{PcL-{@v%&-&UrEUijqhP028wYc2mt9v$ zNX>N>5yU@9)?8|d_K(_bk98Hb!IZ_$c9#%ay|>=$KZL#bg-vxdeQngz1F4(MlqZ6XKKye z0by8v17D8e*_KHnrKELFlNbYww6^7;;oSTR;>;{Gwqsn@hMNNGpK24;Rznq_t*mW4 zfVV@XBIB4K8xWd+pC$IMIYy^5(CO8OD%7dODde5Z>d5m>_u!@B@dnKqe33Lai7x>l zlek*gvR_%z(aq4}ahFGax|z(}4SF^=4S%$FIx|8m`}3 zvaP}ED=VLf%J%j0InR4q{jTG4w+<>h@XSG=hD77yKLp5Cqfy24?dQL>3zR1uu3h|y zbq%GNx@E$t4{2;+r255_X)wGE;FnRQ-MZl&NUPMd(E-pQidZav>dpdCMh7A zl2IcXG?T)|eU@^%M`D!jrE5fDj_xFz>--hnHXRNdJw-HB^3%DpmdhNQmN;xi>bnc4 zrjXTjRXU`a>~mb(X*>7t#dRMKZr+tAuP%D|GZ&?QmUCKX24kxOhRN9KYT+QqRWoha z@~x`!_9Gdz5>iZi8zBeY0nTerD!UA@gG-Duvb(w|fZW*k5b4yts= zu(wi`hh|EMVl9$}0ky2gHK301WmsYAiD8Vejv$K^p9N@M+(GxBzh9_3dQw{&XftrI zY}6b+%fE7&4rs$4w6V({ew(aik!e))ru)%P-H)aNuuEqD0G`u-4ljetYgRF!{jHS7 zx{g73>S1;GS>ba#BW=f`;yenT){EXB2lvSxl-APu9W>JexUy;V$Ly|s4P|5>nyHn& z+KrURar{~?6*|Xb+6q6@zO8$c{ZL}6@Mgu-$LypSH{i38W~%m))p zEgSy;!r)Z)G~ae>-q%{X(#mjAAi)N&JR6& zV*dcq{L1Hfo!6cLd+g)zTYTX!@um;#$Hc4#WT~f$cntEkJxz9ld{+@^Wihln9 zqbYp<0K+fmQZTb=U*Mm?+k9e-cFxb*3MqSJr!I}b&2x4mhZR2q7slv-7y5R5Li%?U z^=#??0OHRvYprQ}QK{*+PvY2YcjEaS><#=^A;j-y_%5x}Q;CxeZ!1gjYUMbF?`R#1 z&;I~R#~e17o`3%UOx?s^`k8+^8*F~i^lsK(-DIsl{z3RGc3ShXIi1+gn%ZY5{{a4) z#{LC?j%n{NS&XLE{z+_v0{1=Fb+tPc&7=qk8H?uJIJbEpS#O3>x(UY^Wd8suO^`vc1bD3iBG$Wz2gD`2 zMbVFs6@6l7@7#*_SI&YLdQmX?lSYP={{TmC@fAmFE{tq=r9P>Y_t^o(NyK1nDUHmI zN3q0%W4P>2wt><_9t)!<>b!0}iXE*~vyBJGiCedhU|^uSxu@*3pLM=yV`upJ=5)8< zrr{K?f>|E-F!`1i@lf#?WtO@}vNsmqz>ekCl{r)4DPvbJ=g{Utyee@=Ea;>B`j!}3 z48s`hwOP*hto%arKee#i+JAbzV_yywTHxSBCvI0HswgfWQ@0H*_%5YcI9t5Gsx(b|fYB!qQ_)Q5++GPc90<5Q#ZysN00dK#TglwotcJ~L zCimPCz>Z7ZSl?2-rSwz~ICu}}wF4SxXe}b~6#W4BzhO!Ah6cu3%u9Q6M_1&xu@c+@cR5heNfL^ zEU*E647H0#IOXySBbO4f33Onpgq-4Zkxs5~G@Ab2343$sNlTVo`T?{bMf z6r71#uB3J(KA0XO+_~62HcVW!Qg#&hD zS5e31RnLP+wSfoi6%7|mt2{U(YaUMps*5j|@nyYAbQyX!joWkJj#fu9F-@kG{2El4 zFu9(16}y86g_imu>{BDd<8I-AdX(}J&SGis86CT>6^%Xs!EHpXYbh894{~r*w0%+R zQ_70-8R#%tcArA(YgzDDEhJ3 zq-pL&qqzhT^dYBpAh274BF^Lh#+2Og>xJ#@X$nDYl1NgX9zm>>SPKBCowCN>v7x(> zxK{HwR4;9~w2yM5zzwUV7y6$yd{3HI+xFvd->^*$X{2XpD7?0LHA|p2*RZ|Sv{+3W z2I2C3_kIcn2IAWJo42BPcR5`bl-v7?s;f!i6t|}yT(oAhqztBtiO{jFZ&>D=4$u~Y zfP+N=<9OTfRhf>8qY|f%D6|a{2l>Z}pF+tYrj8el*w8Ky9hOv`&ywh6J!IlmYCzWPb*p$ ztBqj?Hh$V9@5UrCMqva>-j1A4$3Yl(ArR3+xf$t ztXC?@;^H3DWq-qw0C{CND})V}hc~?J;H=xSR;Q(p%iL7?`ya{6gI~o62(W z!JinTCG8P}59IcEyY`Bf0*8Yi1NQJydxE6$kP zZY5I%vUU7AUb3B!2H=aSpzN{z8{*H!@{J8++Sa_ZwcvJxWvOWgiU6w4*j#wlS;R28 zKrN_&%-?$(Y<;WA5z@Ymo=GSn@RJz_m5rCP9LAmP$$4+3JT^=(Jz`L~g{+cZx<2;1 zf5loQCUg4F_N*jwoO~3lWBINB07xixqzpxevSIL67=Q+y!0^-;a(|6M`6Hbl!=Jp$ z$0jQzeosdE^oKYB5Vt;M3VfwmQzu(jD2nFiQP%$e!D_G;Hat85&F(((rxlvw<;J9= z1eT-e)67$B$JJqR;cS*TUiXJZsF(N^Nr6|I3YwYvr#q};+R)xtwU);*w!!E76Z2VI zxdatBgz-+zL9w{BU!I$V&`nC(GM9rh?NUh zs~*F_X*xG~ZWSf(sK0Z}s2SqN82lIw92(P;j$1O~DPI%jYi`CkHOIW( z-vzeJ`H~DvCE++kcm&YZQqnR9)wzMfK-0t!E;uZuE&l)`R`0@S{tjIqEnD&>FKB7~ zqLotKE|RI&CE-=6a-5~?EeBIeO}UFHnL`Mzp0U`Zc4aj-&vT8n+;9V$>a{e~wIfYM zJTnf-7#;CVVlhP)5hQU&*x1(DtzfVorCVs?v>%!#w6ji2$g99``e~(>re<}IYlA~w zxElk(Ux8z=*;Qqscxper9{{bby4A!a-NdpY-7d`r3Q#FFD8j@%!}Nwo%otH0)yeW& zx=K2EXrO$MvBbpFdv^tu=?s4^;a_r3lG$O$`8u0_=vnX!bfqZJi%%M{I8JXQd2p&X zT!uM>@zUiMToMDkZzJyu{1wBJ2=7#d^~5%t`h%fO?urVmzWHNwTRCmTyw?y%f^YIw zY+iF3=UUdiiQBR3Q6xV7%A?t4-n954LjF+BFUfU3K?Gm3+QOF7ZwI%8iV zRVk;HEWb2naSHb0xUFq_V0;3is##kcoIr6Ta9IcdFO=MRV{h>Vv&BOrtdX&?tpM;F za#WaI4_RmROmhv($&UI@E!UxrCr%A*gP)#RNRgLgY#5~tRdcn|!%!bQG4Pf>S1sS2 zmrBhw`3kEnRLg=eQQ~AYLkpmIb@dDR!tJ%M&2!h$!q;9VxyOM3>9T&K1gw$KNXeei z$7&5}2IAZg1(u}>RHmlp(`|LrL6NB`#~i7zV8vB-BTr7pbZscLbT{sE!#Q6fKq3I$GBa&t=oqE@vrC^Cz;x z)9;ad*hxiUkY=O6UrcA7327KNE^CPQ8?5|?26M|}ma-Ar_O;{PVzpH!I;vL3RZQT( z1>2*ak2Q&rfV}yw8?GM{hh!6MdC|0&JbTY1#?jHrZ5Ay|Ph;5jd&shIV72G- zZH7_E$2$%ESGuhrH(&wqT~DHpj&OY0l5uQewx+fUiaLVE^#!D85$fHfZ@IGOb5ncA z;QskFVtpUS_>OB)MOOo~%}p~8dkSI9AlQp5?9)dk1BkYYhZ(7rp|^dUS)t!&V`H%j z12Jqtqv>B2P`?S`YyIRXk7G5I+V_9!=vrN*UK(E#xPpfETB%MIJ0k1oB92KmZ&h~O z`jN?lVA2Z*RYH4p1MVShTr#AaROFsK%>J1bj%3zI*))9}NgrvK_7zo*Z;DUiJ^^Yl zY&xESlXX>Qpge+Z%{H(-=KNJ0xK&+kRMS9Vb6oRl$N~4Qw>Gtvl{$R6X}z`a5lXZj z#HXDKX1VM)W8Bl+Y@TKzUmd!-xtnj&QzgLhRvQ`#-`^W`k9E{3>E?mjW`;j5%l9uhg$$ zPr%(}?D{aVBU?qJOKn&4xPSJw6kR7_jsdk17M{^@@e7Xxc3j(=8#}2vR-+c*T~qlq zXs1%GHHb~6@%o)}7RM6nvW5QZ|)_!49y~N|nMOPA?5=v-+7R z!lfzmV>;yMT`){vTSCvW$De5*g69)6T}*B79~EI$i`C;ADk|LPIipCqx`gRrhOM$r z9ig`LM(ZC7v{rN}&~nc&Uw%v6)pcr0c;BJ2&rc2L-Ui>&>e>hRuCdu~6QzR>Wi+mH zU#Qz_-sN25IJOy$;E>^>t<>F72JdehDA-mVhf@IRd8y`jH`HDWy-K`4hgYQ;^Dgw; z(Wj$+O_r^331i76)UqFfNf#+``OdIArvwkxYZcq#@ouuh(IXX;N*?k#3Jy95!|mGO z6}w=6HkDVJ0c9*Kw(gN&v{|haOYNPhw&gx;#2E(m{L8iqK)0RMoQR zYolXYF6X$|1$*dn$<%K~-^i^8l%TgM^pVLC3v&)u!(6}(TipK8;qXk&lUiN$HP#=b z%ly|VUDD3x-$$rpEQOYizbjT#H&bSTdm&`; zkGT5jB~0r>ZqNdx z!rCO3XSf0kuGp)Joul>ql}-u#-sg!W-qN?k9M=#5vFzD3ikr_&&9{Bj_V5dnS`qX}#jd}9xxSrRk*?&Pzxp7%43Fwc z@#A8eb{j7#8SGYwE@QW*`W*=0*N^5Y-$7E|4Qsp{n|&=yeJGvk-CiK5?P1Z5j})r6 zNtx2ujxBKYDKu4YgUlmTXNynCT%&vC>cZ>UOy4cdNgjZ#C*D-_*H=A%#D$u=AB@LY%s|{{S~Cv5Y=C_W3R& z5sIoGHIk9%G>$}b3l)oD)i`}qn_LEwy9-U)@!AUL7|ne}e6hNsXr`5nv6eR5Rrj~5 z_Vp+)WHm5(Ep%d7iP(|5PiQ=fy~T>UrmAC>f6dC%xa?%&o?>9!OoLpfOnirGa*}tl zR?f>YjN#s_aoiSMJ^gdv*V2>bJU46hIC3hr&`jve(?<3}JGiesIMky&NnxX<8AhaX zwMK@Hwx1PaMDJ`>Zs8@3pX2zcbWq6z(#Io9T-#s%`K+W)b7T`AqiZ*4?OKcyrdqld z$t#PjOIlB0PlBgbqNvs;z7N~xBJ$#-EoDbJi^scM{0js~B3lnQ9neC^Ad{kLN22NN zC}~{t(To`ONHHmP*$p-*0v}O+DJGexkW;jTT1B`ZGFLi|2MazXJa##^LMkd|Z5$CY z-HZWPY26WM>_!R65J1t$C3QW@PMx?T4MqTrB1-D*$v#>T47flHh9DZA4Bvukd8y(2 zbM>#yDg=RBJOBY(`=6icXY(46s z`LK1?Z6ML!{nUI$piM(GQL|BXyyDw)9M8#8Hin?S=G&VdtYk60w6enJbvT=c0xj63$B5KR8zP>DDcS{rEI0P4Q)F_Z zV05?=2qU>ODbm}9mma~*W#p-CJxoF%XzqOWwq06>gKuJ&4nI{J-Yq1pY1>&FeAKF* zByx{o)Rl1JQq1aiHZlmmiR7)zO^+oi7j=;}`askH<8-G+*8&_3h*~Pw?WN4hLZ2eS zGdN$Hi^$&4@$Us)=p{PLK8#d#?Pt=ScjBdWBe6E-v};XDJ_RX3!C{aDX!LEtPU?ap zz!q_Y69XOTF|P3!Rcrz*eX)Q8Za@~d5U=!a!BJP88HNHr+@6)7oSSc;}gjvHt*Z-t?GGoX$IS z$d5G-q>T2*^Im~J^+|Zd=e7PXV=XFi3QB#ZosA=x5~}Mlnrg5q;A73r%bYF2E}t#M z>D5g49y(dD{8RXa1tlIIER^sujuzZ;;;~!$G%Cj#Z!6e7r)elW=u}4hURe5yCOkwO zKyUy9ziCw!43a!nYAT2M>}hkH4e#cY@>g`Yl~gd8DdG;ZiMVMebyr(7jK$v^phs0C zdfshfMUA&z2BjIxEk$eX_L3XTr4+F)aDqNEd&mKd(DEc z%5=2&q*Bc}kmw@78(#gZPX!evJ2viL4#Q%!c&^kk9?Ktk&(k~ody%K@8-J9o9w^(;Pxn+*m-*>S7~ASfU*nSY{Ro&F zuSp<0r9UNN2;`5W+`Wuu8K$Me=-)GpmoGatqTEU3y9N!=yJ8KO8pRy$W-4ae)u{dp zHTp{8e$kfcx@f29Hx-(f60D*uE~b%z?r{398+-1$y;RM6H8s}-R#4&Cc3q}4xNQ`1 zM13)9m-wC~GcgjdzKzBwbB)|ROA|FM1FDg`mvCHbJmWu=amiT1dYWf-Z1EgQ>NV!7 zNF5u7mpje4P&+NuQ90gqH9;VL zi-rC-T`oH4>@K{|l5-5%cNOx03+dzopB}|&OI!nhy_M|+YhUJE?Ez2!}Hra-t59+`E+UrAGIQ#zqQ!CQ6y~USDk`Kx9 zGoH@(kfUc^@qynHerev1E!B`Xb!YaHGqZn|8*l2H^IY4g;rJB)#rKQU4L;P@+xDx9mlH9y`Dg1JxPAMR)EDR1~u5oBN` zSqm&_+B*l@NTP5sM00d1@P+8)*_ zKZwT)DLW0Y^Nr;iqNUPnhGNOTUho*rB-MZqJ6 z#BouxCRoR?-2VVUf@QqfT|oN5BVDd3VvUA2x!SL7FSr)lakcp_ZROObHrq8&qLRW# zGwC;@s+@<4Q&Hko@Km;{H%Sxe?{Ls<=LZ2};7ZI>nc>yswR1xmvrEBq0XE=tt$#{% zIkl2dS7t0-nB5$$b0{$gr);sv(PH2&xUoDO2R=4bBLv0iJe<#l*A`XM3dnq$cRCVm zAGPc+w)R-1QK;odCYZrDtHDcOEb=|QeEoBPuqFDe5?K+mZyDVCRGFZ3VWwD~81{!o z=dg`U?H#JyFJ#PfGE+}J3x;`Aa5y$X*;vxjMZmL&C03<+RAbLk;d#6-nH-Jii7-fF zr?iykVShwA-Ini8O86_`hpKR*IvQMDP25K2_q~bXvdle`LQHI`sfo=tAU}d)d}9;C zWBmB21Y8MN2=`ffDtuPoT*a*Kj2!7%)adhraY_J*#CYto^xbKwnc(@B*oTmBtFd2T z^1fUmIph%sOAwOGX#icI+mCv$!|>rdm>V5KpDVwkBi=2+b*eQesdO>JM$yqaD9UWj zv*Hj>RaZ^qjb~QoY2Q<q6)X4}k4@*mk1djVHe@3|VMog-& zsHwxMDp^+>SbC>47TA-+*0FhpTFU%>uMmzcFx1pbGn(La4NfEr@aMWs8j_W1dfASc z*tFu57MNvR)4?TUX{vCTq-kcMD2xGq%Zntwg9Mam6cWNu)@+XXlBKvx(PzbB#1Xno z4r3Y{q0DP`7fg+$UVB8PtLU^|AGu!H>B*JvE59INuYQ(R)J90S4qzkObcr&&r2P$j zD~Bz5ntlac>x}kW#WOP){Y8vvcpD4Yq)Ua{Nq2su72Q?T&dryxHWQ)-%m+B$54(zo zQ-@d6kEf%aG1^#cwS7@w0goUKS4)O!n0AV1ZQirbf>P-wzGGv=C)FllO~NI+eN9x1 z461B30{iui2jZg`Fz4J$g+aeI+6qq5EJIvxWfW10>X*RkhC!vewLB6J0d1BkP?~*`)$BGkBc6VRF;Xz!_Jkka z4~nyPTB~rqH1yN=2k1YoWSB5qPLj5r(^FZUV7r?3xB-6RR6^3cDlTX}*Flo6PEmN0 zC*zVnItG?Fw3#h%Y~LPI|{1aVpxrvCXHI&9eYwi9V&yq0N}2M$T+f zH8o{OmO;cJ=Cn-@&M>g?PM(^Of&~SHwMCxPMl`2z@d0@JnUd*fNN|L}E7~ATpCZA->$JHFk0d=@GBRR{+ z$9<}b^DD~=Muk1lB$yW&tec85bz@B*WuJPyf*v&(| zCPgF6aPB_@mNQL7KbqjBC#^{gS4pOw&OcS9#X-Vj(@f6x z4~b9XB({Qc`WE(-cw;Y0JGS>N97;k#}sx#%2R3O5sZz!nw_bZ zYq|CmQMfx(T-!Uph)XVLHZB03^d7+=^PF1lPV8O#E+(glRo}<%bL(4j`p^g)M(CW- z_CIGe!r^D10)I?wch>|5?A z!ndl|NA3|$a)zkImJ;n#TW%5e_pMF_$2Sd(xLCUGw`J9|@(QD_vYH9q!_QzR+r>w- zT1!~K#)Gp*06P~NRu0+cISb34)(SYlXplh%I2T5V3!o058h~PQ+oKj-^U;mz*$q3{ zIc<=zx{w>wWJS9tK)ulr0I~pYNV+XMrMb;+zW+crfdBjw1r-T!;rZzV1w*_faEuwYqeA2nx zfr7P+)X^s3w}ADt_~d<>#@_eX1iE^Ty{&B>Jk>67uo1KIKnkgzp}@JZ z#S=)m0f3R~DRD^Wz`|y68UfN>!_=llfn!xJyyBLM5jhsFrG<4Ziz%ay%BLuqbGMxPCaN?JIXUr`h;Yivfa;)PB#gwjUh`E|H` zXFN5bV=26|5s-;1;(&I@Trc5ZN=|OW1`$I^QyoKU+dJJHkGGoA03dqz_9%^5%VtZ9 zTbe2JX_6N^52TV-vDPn!65tPcT6$g&X075Gi4;Vib-zV6~&QV75S>#j1e!}Qk05PLvR^2~s zl1Lm-23n|fd8LQ8QSWMAY9uk6e@1!tq~jnrF~H%@NRPhAX4`0l_qcnmyOiE*QhqW0 z3^HD%*O`3B>W!}LXeur(=V7z7A3H|L7<0^+eLL^QHvAFS4OCO#`nP-+CZDTSc{5zK ziy-B>G)GYxw$cOb3Xd~ERQ$!|;sV$NZD4))+V}9{tuprJqi${ZD>tw|X8=cGTJCCg zcT!^MN1Bo|{T@l8o~5S$08{3*MoqY=u+zm8wK66&o+Jw=be>BcdWxjnkl06+V9S?i z&$_R%bA91%u)ICk&L>ZW2|B+jm9ZE00(h*YP6<;cG%*B`m<8(++L|7kyz%*>ddQx;dyrlhYiCinG{jCatECA&CWc^nO7q~ zRTy?NOSyuHn931~;e!yZbj4Qy^CDHSst6Uk%o z?B#}Gv%VQ5E|I)g(Q=>*>a9Dxn+EPz4a^w4IL2zk=B9HW6jF%V_M3-Woz&d#o-rI# z4ymT24y2|ry7oSM#krnK1JU#KQOn*KPplQHl|S)LUeBnR&(=uP0HjgW)Y41V($6F> z?PIK%2~FaZNw2|k)+tK%vbYM!d6Topt#tgMk5S_BiKyn)r1|W2&ToiZ6`@|_tWEev zVx_5T%VRGLlyWRP8~jvqG8HRRJdH>&LD5qKFxQ(N%bxliyPqN zj>fNB*HA%?WyYM;NxuTG2vm7Y{{T82?fI+2#c1L`?q~eyF5B>;B%(@0 zu@5nQArIwzC&EvFTHHVVolX7AJ^^KVKm+ow5>NJ@0JU@i*mXDeEd7wS)&Br(4{H-2 z#bcY>qkvHJ0Ggs%?{n7cWJRAL$ zOa04!3#)bh(vgyv<4nxrb<^SXrg~t)^R1&$*j({d<_voraeKPyBRl!tY+kNP{)Q;$ z$C@Q$Ue>lp0O4_ag`@xhVZ*UD`n+jWNk=aWQnv7Nj`|$49i*tjrga0SC61>vxvjWv zp?hu4-K$5Gb1pSYgH-jDPg5mi(pH8tO>V!M z){b6r_b!Lg)m~ktrNV2!gqU;~%`RSp4K%eCOkrs<7rm}&H`TBn2fcIZyhe(eUZ$d@ zk-Uq5<&whVFV7Ryv}%e;ZpZR)(`AyK1yefXLC&j=GpXNk8VnMTFtVBk)60e7xjuJ9 z`C@HTSivi7?FVP<-WM0LM@Ln;J{+}GjbZqf4J4HkLMk;mxO{AQ)Ejt)p%aJ)w^gpW z{+CA^mZd2*aS6@;Q}gX>q`wo3@nkaEWV#L;DK#NfLQBWLbMzz zB-7~i85)>*CS|^jGPXC8Cd^Y&;;)`s%AgrTSsVJe*!KpO>HtUufVkttEJh{KMK&dY z;f=#{R1#(!M++E*E)gv;XI#z8n%Q*`r~q~&dvfKwrNv_Y4U7j8!l^MTR?Q^rFT74< zk8yK|F_CXb>;M+oO2P457Y{f#AtnXa<9ICxkx&ES z!Rf1MFeo9ei>aBgZq5V?ZE?uk*tGe741Ec7j&{yqYemKFvAI!m8@1L<=Wgz<0C)U%aay6qYOeHC`3aSrdNT_sc>?ij0N=LBh$oTe zUb31N{D?G6F1BoVg|uHS}HI`@DB+~OAdjw1>+Ib@%>MLTym zX3r!=h6>ZFy-Hjl^y{?4joh2yndRiDA@^c&DeR zd~9o>(mF|fE0fIgT{~II zb5fks@=s0l`5CIE7~v=H_7Jf;sVSy|LG*zZJklYmigyUAXc;Jx#h^H90D*h&!5$*f zC1g6oivvkIMbD`nH9U7^Owc?&Hy1{2!s5rDV%Ie(NuhDViY)S$QJ~G0byS!(93-BV zn&xa$mRMfg?h>;HM}Ly{u4Qf>H3n;uf<|g0;A#YKU{uVVnJV(_Ow~9u=en%ZMHUf=K|^*tPUk|>*FM0s5-)3RxskM+X;p+Cd@{bqmsu?GN?!8? zQI!F=l43B){o@Ni6jazoWufzE>YUIG^utmteOFf1)SZ1HiYbgT7c_%z!Y&ifWc^fe zvTY@t^n-reqx)SNwPg-QlE%5w`!XJvQV~+-O|quv3+*ya| z1TH^z#azVd(DSBJSkQ0A;@nksEsNC0B}FT#SlJ_MOJ$NayH(FCg1Ps6Cfb5omiuYp zsiuwfY3B=XGOHBF@jOADT(cHO&9V8Q_}hxC7FKV>Vtap#;WTnO{*yH_{{ZDEy8Iqb z&e2rUn{kF+L)h=dVN@WrgqO{JnE9GJSw8Y*ZGsv?w2GexsM;(>cu)55ZS83&orW;& z7AHX=}We!_4*Y>qUbkMP1@qq+*OpAE-2l_9FK2nEaB}64Dibf zw96Ecp2swWNmHo}WST6e8Eo<{I@;Kc*;g?C02x!F%s8}Qe;aeH_J!3@-73oQxvd?f z{v}-~OLU9qb8aGB);=iSNdOVD&{fq6Xq`L3(;u8|&zi#03)MyU$Hi*%GxXsz547Mo z_pa9txSXg}ob|%{ocpR#RUsJlAE(V2RKK0;BJpCE4l#vHbh4^Do)>cGMX5RhFwHuf!TCT%F`Bp9>m9V8B8@E~4LpW)hS(&P(MLcr0_pmnjEfTr6sZyNzvCECu*h-^TF_OmqWbAYg$&z=yty3`AG|@EM(j@%8 zN~WV9!)ln==A)W6hliaG2Y*_#rOX&Kbj*pbkZJpEOuLjT#j0{oFFWJvR*IE8b48WV zvsPf3_7D}pi@8F>8%X>@O2LRzIkGsNA;dU(h*y!a8GQ+3C$jsbUeKvNUP$F7(@~Ua zMQ1FoF&@N|8SGZ3n|oX&x|Q9Fn<{dSl2|mUC$UsmMmL9}f zROeHk#6}+EU7gUS^T~mfbSX(5Ik&YVz(vF@Eq7fFiKTY>{5~qQu&9Y-j zEeE-EFDO%|ZB9vQ{>EB0B`UH_=!iI*gD54{xGgOny^?4(2qV$~xb02mOSX-Tzoj5k zaM;I@Mo#=w?e3i$bvbMQ0J4=SDbJN?4Nn|cY&oVIx|uUTjmNMkV}h+qwR5Ui!Gi9W zX3`XbE1`6=AP)o(JP<$~U;};y9-ieG)$(I4;!`qDGrqo&+$_1=$n?Cdoh;iM{B=()q0AKeg<|RaZ^=Ilce$rDfo@a4+9tk8_5gRJymPrL;dy+{+ zQSm?v6$FK3lHJnnwa~HvYDj~*C{;Lo6Squ}wZpK5WousPHV6YbHe00gj6Gxo{{U9& zdz2|~I{2DI6S8mbo4>SKY{U;D`bZt5SSI9cJYw?IMBd^Zo$ptG*?$=EqXamCcRuNz{?JbiK#$9%smQ%LP_LW;ED~Z-TG&%Qiu(`d@ij6dK z8H*~bTf^$VP`_r|B~Go z6wxrXu6J5E@Y}a!r)q`qy_%TUv~FBLb@>*ZmO1elF6zkTZu*I=#_DB0eNb)q2JWO` zcyPpP-wjPu?8a(3cfYTI@lIjX)OD2x7tuiPG@FP+H}MN~fZ(*$vH4su7R>iLIS#Vz zt#NO1=U3xull740X_cchRM-t17@MJ%7t0@4uyHQAziW2fd{wJ*)7d-^fzOIK4caDn zb3i-n2Y)3u55y&_#KszF>7#}*a^Ub>Tr6$}J8(UVKyVo6U==gVAl4+YJ6sOV#mu0TPOf>K&$K0GT^oR~zV=rgP`VvcdlJu z;~#QMqZ&x<7E>37nGIxrqO9A9VbvoJlAyYeYcbn~+}h!HI!8$*p7VR~JeQbq*)7DR zZv)jIbC;xgJ(bn@M-QlZCH+}rk9$cUC98?SM!UefNjFanyVI$bD~xoVgwI8eNmL_o zf_peP4T0=ky|Gv*7_}S~6w*Uc6J(9;AcqUA37VS8=33Uh@JPtnsm^H@y|{&4lxV8( z-EmI|vzWjf(k*dyr^M)+Ns3idFp}7%b3r|TrrOIbFskDR3~0>mIe8Wet*LRBFk!U9 zuc%>^Hcdv&^R^e;g3(}@MNTOznFFI^S#3^xtOB6vBQO3>qw($0j?4;tQjSh*LGBzcauo{eBzM`4%!q8kJB$Iv|yVpL89GUHW z%%Ts1J8N)vqkaq1Ia+#5*^qGQu}WCyg1&-2x+m@(=74?Sb1di3Omj?T%9y(;BXPN< zsRXsY4jcPZIhj1D>75Yc^x2OqK~)sZGQtE?$A3A5aKDJ%dDEt;SxwT0WQGtM9FsJV zKGADQKNZ}1BFW<-VRbF3trazuHg!YWvjw;Y{wvG9BW44QXIyI(zneW%^nK8Dk8ld~ zs=9R)*Fv+0G7u)7A(CUjIEw|(A)0p1_J;u(I|0Q! zX022{wjkHEvUyCMC0Z< zMzEgZn0_f<#(?^YhqQ1HZlFEt(<)HGT~zAl&fw_CxQY^8x36WQd`_JzD=P_jYG<7# z2InWCX_MupZZ}k6c@YSRCHrm&?NGCoOf%zAx+jM;Mgb$lr*YbPT1X#LO39fRKS`+R z-1D-n!k~i_n2`jNS(meA4d=%8S>rlan$gRbu(T?r7TZ`LVS3Efj9|DdI)LoD-E@3! zgUkK#_yv^3DI0{(8@idP*)v?%+FeE4cJ0o`)U5KZKSzdA$xk4a;m_0vZw;)buQfVO zjMf;+ZB41gYHR+gSB2_yxAF`)GSusQx1!JW%G6<6?!%#yC~cBocTba)1_$ zaZT@Zp>lvIGiURL@cha7t85=^E|mWOWZ#0P%?9(1=uQ3V!vv^}_+>a_+Tz&S2nT_; zHPpKQ0BK0cNAadW!gt3H+>_!J@eVr!WkZ6W<$~|ff5a^UrxW~taxH84W=}WZur`XT z6f;cenE_zb>x&;(dZ)pB+$H}2-H+K?%;)~>*6;N`YbS=C7!=MNM0X$M3!%cM+#6yp z)*gm~CcY|sFRPkp+V?mR3x)UvFBr@;cymZ-lBQAhBWbs-+!r;a#%dz;+SiX^Q=!a2 z3mD$d2V5?7YkAb0o_91tq++lZF^tz&SH7oAakPsygkHdRsDlukdQgIYMb`MM56bj& z->hUM$D899_SqC{!{&9sVeVv!4wYExjEqx*>NiiAD(S!tmZlc>8--y_oGO@hs%)Lo zH~#>o75IHC&zm#jZ+H&;6j`u=dooDgJN8R?>k{h^W@@Rko8B!gEG`|fy5EYjgEY`S z-DGmd+rX}N{2peXHX=_htl!{?)U{CTWSNJt*ZHXRLk!$vwUpJBGCB#`=DC&^^LMvl zT55UWk|qe|jmJnEI37!ob(L)2tYhuk)V5+Xu#3TC+Se@=t{rGb4X;#GDRN1*)a*nN zy^$_cPnj#AHWhVIj>1U_)|)t0*j}y|_c@>ypOK8p26Jw_>ko7d_?6IFEYKG<0G2>$ zzpt;3zZPyH`0Vh&(i(0JxrMH$%~*TdI@V1Y`?m{4dBxL&R_lL}NGHtOFHlwR<5+Cg z(^nf$W6qz7n-sA~+s7jwJ6b_m1`8<;*`)RaqME68nt2anPr*uBkj&;>I|#ZB>A zv;DjO0BEN&LvNX4Jc3=j#J$hxT6-Nipjl`cjVAAeeaaxe7Gf5so!P_AC z6@DEz3yAsXR^~aT%%sqXXS12DqRrfGb!{aZ=;IK{2)lu`jqlsRSY>K>{6x9*$WJ#6%Oj&Wa`UJxLa>hqs3;Vqs$V~ zc_4Gy7__5_2FBj?w!vBk6Hng2eY>m=%(}-Y z*@aR0O;t;q2zY5Nxv@RTfX5a!?2x&S0nH_+f#9WdjQHP4O-A?muX%f#H#-fFh0w#J z(}Gb|@g~6SaBaZb)SqR4DX;Q%+fJhE+o<)f?b z42KY_!y$BWvGNLWPcf0V)Bw~?#kb@|*E5PZ-y7Ynz_eW6JAu8pgmD=RQbNh}$8oG{ zA*ZtZl_g2Zt$qx=XUd+RA}|fq9>U<^!p6l~V0gYO9b|F&T+&CsnD<<8RsCD9W`OPZ zu!HbYaJc1VPGV=BXAWZq434yL-?J8#;(9B3yQ zcF9TV_UX{(IpMBMrS0lPiwnXVQ5)&xtJK#6XO@s{>IzmhPU^Zr36?mV=`PYivE#A! zrO~n%R9()q#_a*O2gKiH8#OvXO8|kT(h^RP{U?_+$8AOwrj&4=_$sQ-RU?*FzbxYR zyivh3Pc}~9nF<9?7l~7Teu{RE-4`Cwvd~2{gF3+4mgry(ZnE;?ZN03zZUd02tFl`& z0LEsfd!d?cuc*|`F_ZPR3trr8xmCDrv?I*q)_vy4UDj_HOyMqLm~8gBr?878t@Ent z2rQNu=N*zXI3H*#6>PXrd=S-PFtyX0G{qHw^|r}1_)lcA>Lu;d?p~eqo5wG}#@Qb0 zT5zliS2?0Os4Q)BC&J-2?6rqI$Ed~2dm8vbKG5WuaEq0)Pv7?#ucnoqq&moMV+i;5{E9kF@;00owIcM$JE{0Bzjl(v#82uh9eNBOxP&$?`% zsiUf={QWf1efAOYOc1mB(0&YNl0=wsJVq0E>n0!KQ(?^b#8&!NzTapngcGB8T02RU zbZNcOri(e&TGG-)d(t zGd&5UZ-abK!9ygkvz;3O zaP2Cx4C63;=B5O971iQ#ruiV{j4$WV$7#*$Ni2@$n+a)Q-;SehDs?n&7)(xSvyQ{s zscZ4PF1oFg%@pyyqT3z4iq;%fhI$xbf#ilCNOB=AG~+sOrv{!kF683en!zSkUk%ny z=HC?uwN0L;o%nQ6!6=Zl*x0C6Nl$@5KlARvX1Qe^@NAb>h3Ci|nhAU8*4 z%RN9jAdC%>?0^zNOM-JrCty%>96cf!JIaDat-3n|+|_h21Fn5Ua%!xq%=EBaMI3kJ zPURT_<&1mHn8M@S5RrmC<&liH8J-e8x11o-i0nrsCA!RTF^eQU@kt=s4$vxOwXKgrx-B$R?~y1-Zof}1<2InYk}CC3JyywT&iK<(bHtw zXqwq8s~qw-aAw2WQlqcQly7_x)KNA<2qnb7e-nEydz!G^W*QM+i&q(*=^Geho?Lfs zuMM~!^_Xjje-zHbI4REAzpXITOe1W=NVb zXwv439y{|_^|bZkk||Cj9aL_y3=g>*c6(O>d@WOWg-yIx4bi*i-Xqllq4S2vVY%I) zUf#7bO-`hwBxxF4*K6ad_PDo+Rk&6m9)~ibo_Cwwu8G06?l%N0DtfTd)-kRzNaEcj z`NH1rYaQK{dt{u}gXq_%RkapBf}I$I7bL#q2i#fW+m74b-{oCD3~+4(gOg@y_Si0Y zhRZ_fp|A{lTr}$6e^&*!!RnhP8&DeHNey+m>_+R&@eIkQgLN$tQ&!f}<`5X*8tK3+ zYqx12v>yX=f4UX3@tkJ0!aP0(L)C)kKH7uGb%S}>9nYy0)%blqYuix93YI;@YPHFC z3wH7KDRMg#mH^5~SRPtM+;#_%w=j|TnC44GyLx`)zf^%f8C6|o;uDzM=^rE6tdn9b zZYI{+);v{|3^Z}m!yj2EROT%Wo08;403$LYF&z_8Ax~23uRhA~u$wYW3OLw{}Y_y6SBXk9l%XdJ80Ns(=>2}!w z5CQ-|gu5YS05(SGfKJVT(FwV0VtAE;=#YXC^tnu1usj-TfXJ&~6gpqJ~?xGfu z(%>&)bj7HqK&{QR`C5*ljU8=d?6gb=haWhqe8rovj7Oo$Y$i6!F_&TulBf_!HU`cI z!otfDn8ic!x|wGrxzD>#Yk6DLqS3svsxV^`0`Ybp2eD?UZAy)K&Jp#V@2kdxBRM*2 zuaWJJiy)GRqkO+hFw-P-G3p(Ji~QD&@@%Pc)OGM>tU0xS3V}@@THeQL8!2*4C)g_J zl=6&UPMN3pF_0^(Vt$&Lhm+Ix>weeu3!hT)jFn`xmpjFG@ARskqPDvX!k?&9#S^Lv zM>msifcLLBR6^!P*SX*aZqdxGO44pBZ)Pplif%ng@Zuv-wt=<0;H5=e!?T|Qzr;e$ z>fXq~P3+*xb4b$}SuV{C^*-v$1I21Ehe&B!0g!G@io}AJ4|=Sv;U$JHtW{XJO=UA@ ziehybiySH>7k0-?U;?1gR8K)6eKVqIBG(riEbC^KzKo6@1uM#`KdyhZO)GUp=9|G0 zgd-kB6@vD@TAW0IQBv0cb_AW2cHxf)%yV7$Wh$+;l_P|ONM<2g#s}*qz2Oq2DLlDq zGBQzEOG3a=*0<3T`ki#4O^K1SrMKM$Kp4eXKU3J8W;HnUn8V?4j>=vk&b$Zbr>|+b zd;M{ygX$RJt*mhHAvoM|+~Pi`B^+-50MWnv$~y`8liEkXxXUw-%5jMw9Xkt2*6qOC zu&qsfTug{KVrHNdyIKQmJxiZ66pvNUSkv2Ur(K9r$-CYUix1$j+;G{}mobjDCsLx9 zJ4?V^_4B$%7e^z&CdXCM;>YVEuw$zj2V=i_X|=RGEW_f`m*q4>Z4@WhZ<^KF)W$hY zicwujEX|mUHZ?Uv!~&m-IaiIHUTykOkHJ?FML?&7Ba{FCY23i@{+Lnn;0{k zjy82H9wFsOi#RdH(o--F*1cek5<`xb#_sUzd{MTZ8g`Q;C8Tq?vOQl61{t`;Dx<|( zA4u5=EV0{xR7n0? zO4eYR)$R>tYdYo~y2k9`;KV+EXeyYKXogWznW z$BV#hAJ&22xC)I0S56OfPc)ZPN#(Q>fF2=th_UviJ{lQYc;eCC*OEQ=S)>6V+#4R` zsasQJ)oG&kS~_#HWgz~Oc1=4B+f!tMPT)vB@T*%@II!zo>nZkPRSgy)j`0a@%fXjd z52_t@8zr?0cYryz{{SS^jjD_dGrU`o-8{@x^N!vHNp3F3N2Sub-(~H%OLp~Rvn;B? z?uZSjs{+I^-Zu%@u_+=xbWa`Za+KNW9z7VH?kL0*ZyT~uc^14|;qyyzsU6XD;Pzv* zxsrGYF;!0&-XxEjwTCuUPZ^E%5Xc-Xb^xj`AdK*kTlAawCf$X|`{`_N9nJ&ds-%EKo2i);YR6c6{zYeKD-B$QvP#x9w*crS z>q(QJO+|`_%~c+q{O!Qu-kU~IO}zAfLX(T|I!6UZnp`@l8hN;%as& zuLP=m(n0cYPujP+GlwkP$KUiRbEPIq%L3SQtv#9dQks&i)&wm#!Tmz}g*mE|vkbTM@NH1`!!y{=Rmptvqf}d7&;W%?`{TM&0 z6D_3bPMdpY=$N|6D9EtOq#P}dA6f4|tW(&$on9AK?N+oe-u=Z;MUPV9cswmSrl>lI ziJW~S#PV6Hs;p};)%7maT20vmdoXvn^YJSMYBQ-;a!X9JO*tsWC=?l&GU0MIS{#19 z7Xlshbx-vYFTnSxSne%|!tVlDGg}rJpvt&W;XO(Cq3GVZXD<7*Ez3{eS zpx(_SfqQrrURBSYrLy^P(53Tvf$&qR@Vrif80sdZt)`A<2S+2C;2hJyf}~{p8TpGd zVfZ9d2wzz+h)7#-+w-?I*)z^1jpfXtRe)kg3ze;-Z8T1Sk}<<;TFxfq08b=~n)ei3 z9T;bq9;8MJS;@oORbCi0Vlsj`IgNGwXjEf0wDD?@l=A`F(g{p+QE^L}Z6Fd! z zHm?kP)n82GApzDSVenFMiWn*By2$mHIl-Vfi*CA6;uL2-$>%xnGP`-D%#8G42_LRP z{YO(l1I-rTo$BM_Eqf`LejFFaB&61ZW)MK!iyQ1xWR_@S)F&((3)`94BNsIY>n9Y$Kjqx{&0pnqi` z_!||WbP*S8x1DQWot55H%D9eXHBn*p5yj-}Yusdc7rTG}YUeU33pF}$RdzNjueh})o-wabnD;yjHU*_%HN6jJ}DsEYflS!AAd!9ZwOAJnDAuBZq>V z%(0BpG|2ig=MlMM+T-9XRCy_CVs@D+=3yPIA$jLYtvzKHz1TXu^yILbWu93dOEKkO z72#Dmaki(C#8~x~_XTaqZ=kVF9(MKSynuoTlT4wMNFabZK-}yulo;+{GDniW*|)*)FYh+=b`?i-R%s4h-VRfBCT^w5u`sbD>Wbpop%pr`apNgrsc3T(TW zaR=H)Iu@QTmD)h{aaQy>4m-8gwT^M-BW~W(r71pyWg@<_HOTvjH39k}$`dMa%t-$L z7N?#;el2Nm_V5dTLzi%BOBiUFTm`j1&OY^M#xf=bv7xOT02WqM-%_q?p~kZmRoF}I zUS4D@57DcUHcntTO?EwoKC=$$1LKs&_}*+rz=972ZPNTj)v-x!ol9Zj98b+BpxMcb z7>AHCIC}Mi@CvFcn+2Qoa%3?W@jNRMk|SSNMO7KG(J>zmN_2U@Hq*q|n_Ud;0>RQm zHG0oAE2XLyQ|2d5*Eso1LF5yA5w_mNL4s0N;wENO%LwY)9Og*t6TPVTJ785kUC1e*Q0Q(zvr86F=f|;-ima(jfkvl^L%R_9^eOKR#(?*+|5Q?%& zOuWyL8FI{lvXf`E`dlnt;OiR;k{xpLM{?&;VRdrYmI%jd&6a{Xzblou*5KPXmlJW> z?oz}e!_6MEb+(hZLkA=VQWQw$j(7&b88tcjM2lMD>%Oa z?>ZTa0b^VbouQ%5JoY?!0J@e4)|SY0H_m|a?_Ba4NalnXo;j@by^Jpe z7CFA_ubc51b46hEbhIn;Q*B)gt%b5aO#&wuhPkpIL|)gki5Iuo3jREHj+Qv!n^>qI z2D43riQoY|c=GR1aU40Agzz?L9N6NBYiJ&kBHxdKQ{976x;bjJw6yu1V~MqexN}&s z8j*}J+g=6UQ*2FEEj}#-kD{cJy+{sgUFNyO-=8hHTb~r}4l84$BM^7e-H-E*-~+HE z@hDT`?W|#znkhP$$$f5UYj}Zv)O};*sFlxq2yAX>Yk0Jc_uISNuIbcN*@Q zk23Kv*@kOp>{ad9HdtOi_L=xDm_^Fw`PvZf#QG@WO{Z=2w4Cl zN*Yq^g_Hn-)B}5yme~$y0lhn+qh#fg+hrg%r*t%@2;B=P0UA+hODJ(b2(lv7<&jwc zHK%l-)VnG9ewRf%_H0B&UySO4k-#~S@O-0{{>b;P zrw$(s5HbpAqlP98EMpsB2ZGMjV{%j0nn_}C1;9jGzM)}qdn`&`6WV23%RxOvw+ ze9^;1o5nqFK*`pDr!SLujvUz0cql_Cp zkMZO=@3QnZK}9HxQ|&v~@8T9#3evKZ138j@u2bQA%|CJf!pz$mE)uIqAlz9?<6M z8b}@of`=rU9cMMS@m!Ly^qGuwwfRR8u9kx<(aw6PVw5$K#yDM|-Ng2l(cfK82G1Mt z2?Gv2({5YMS*tt5a11rl#ZOGwEmn5Di#c?cS31%CJRN+{SBrlqo#wP-Aq>eTr zdpZi{y}~Gf?>6GRozgu6hI_^YZn$+@{VqIKnlh}@Q0OjdADGAfNx@0;sp?sNY?vE* zh(XzV1(f|3D^X9~Ek3V!Dnu+ZJ@LTiOz6#=80(OH6LPieackPfLACCe6;(to`Z|eO z;Nq;VBh_GsN?s&7tf!08mI>QV&2G}$%mvDai(q)37o*E5i#7TrI2-#Gnx7P{s(({a zFIw2fwrXhDqQ)_g%TdDG$ej_2_HvOHW}40=xZ94IIYrriM=UFcn*hVK{$~)Dc5W z2#v%M5bRmwTQ3+GQ*w8y+Kqk&VV6#3XA#Xf_}YTP+Vqxnh$W zj#|3Or0U_5Nr}5_+WVd>O-U+kr_fwiGRc@mEp^o=$vJ)ChEvTKdnR|z>zig;Xt2z7 zs)hphM-zZ)ao*MiRyCs{Tp1W5D^HZNyD`iu^^UA`4s519A(tBfZ^yk(pQHMa35ODe z{m+cI;dO4_3rv;dk&4|Y5;zjB>hjHPHXzy;%L}D+WKv z;*pXIfFuFK#`ah3B9?Mj45kxI(dNqW;NqNRWlK0rsARcsd7t^<76ix$j_MXNUf$c)0SUT z4(xZ@9jgUJDl*V~IchCP&S*T7E`+t(;&EfUN6kKh%$r)HLrInxO?Z})Og^SV98%ak zfqf^D@LCvQFk#{lc56t#Bi^x6HNnB`^oPH>T;X_>^HLgCPGgn3!r}qB`v&W)V|u9P zD=5WrLC>kN$lBY7)M~jRZT*2#biD5~4Dpx#m6Mwu#bSG+H5DT}j}8OexX-gHeYP~q5wvdGqhGkBiHs|>hV{2+6f#Fr#~ zgB7Zxjq$-TV_fTzq!6uejMrB~QP)Nwk>ptF7WgJcttiGzUT5$s>CLs^Pw9!*$7sOY z_`m%{YkEAPqN&OB#;TGD{*q75#vRYSbF9yc$(So+s;FZkbZpUR&ct5){8o=FX9zHi z9x4aY$u5UD>J9^my35Rza5crVV_lSRDyz8CHV1aX;@@`xVeORdpDgm z_L-^Dlg}}Lo|2}W+G;7~h24)~Y4@f}O6XH(tO{XiDJtAM9M+NXP^$9|PO4?4!ezf} zV{1>eIW9Mgsx0VBp@|_8;H=*K8fw`b-zkQeyUYv$&I8tdX{xC3fIo`X)RWmP01s$! zPIc+7r4=do5C!{EnIqbK*SWq1$TvuT)sS}*n3iBC>V#ptUiT{MamO}D<$1w5m6a|2 zl%E~b@#0dqXR)pw+bZ_D=*d#jd!iOmGI^?NcBO@&w{6_2>gaJzWztgAKfSFk1J*WI z*>$1KklX8gmByQIYc5q-CpK+_nu;;FKrx3I8d^!_EUjaB;s{eQ<7yWYNxO#?PYbpQ zZ$@2fsj11NKOd(S8wZ{rqE?$U*{`PqvE2L?U!%}pj^FIYdwI_t-GOUpC~0V6 z5Yt4~4rbe*6U{wLG*Up$$l5*0VREi6XIf7?8tKzfMmIls*q$F2`!Vj>x9t2l@>=>f zhS9>?4Wn3lVx+&v2 znIlchPiP{}CDxMg%Q~HvHb>?_H|N$A`ns{mc;7=3mbOc;m%ndP{H=v_qMYosWXng^KAN z`s-+5xBvp}{{Zi~VYFGQjWs)^{!VSMIElF~aU_#+Qu8qamLY%XLE(NyFD*x-1*RRX zx`&$Nw^1M7a?wR|UTNaRP2!0DTaQi~tDEF=zF#F+_Bx{;(8jsvuiOEBf#28yy1>A0 z4^Deqe|bFHh%^(x+Pq&-xD@}QesJ&PslPd#0YMx8_GPwHz z3frVPT+B8A*AVASEP3)&{T-7gD?F!*n)h+-a8mk3kQs3JKTXazRFt!GA0_uLmMOB+ z0i=q2A~(#y;^IyC9x7SFDF`Fa7w=2J*`o)gAK3%#7gJVRmvy63T(W6ox}ygblv2xp zGQ_e*p1@gecu=bH+vSEzf2km|izKmHd!cm5@jHaEC!cC6?P>JT$s8iG8#TRQ4Hh15 zuXtA(M=;}gPP%F6L`2NZFKgRkZM#nWhjnO)!K7Kh5#AP$?#@x*=ZF7JMut!pWE6 zbaFRHW0~F@erhFoLR6hNW*K?PB~qxQr7TO@UgO#KES`v2ml;zeb*+jhhPd>!kt1A1 z@7N0FRGLj*+nW`npDxnN8=CirjvziTKvXP(DC!{3A@3sp_PN^A6Oo+)Sp}D2S8H>@=(?zNYUK=E?J2Vr_^0Mwz^l^k#P?@mI zLb^vaqiD#KJ8kXTzk1^GGPR^=iLcyWeq8O|q<&MZrKfI*Dxxj8XkjA5j}TT$meu1X z<5BATHX9n0+DXL@X4(;kW(ukuTFfLh$E(muuqNY?-pZ*Qm2y|R%O&eI%jX`m()IVE zvgbD2%XvAo?=T>OB?Uu41P}*Dpvm|xFuS9Yq2JoOVd@rFbo8*&#nscn98BzEKu-={ zD?ize>y+=DQ$cx(CQl69@axtdcYH%0zDrxcg zrf#YzWSg{x=x?>wzo|rJJYnU`gH(h1M>g(1;yaZ4VS9$=%DHT^NfkMQ8k}e{I?)7d zXd$JSGB*5o;qEFnOrwV^VihwoX{Lgqftm|$;NW&Q9wlB$)Ek)fRJa=!Len80HsR|} zK|@zi?Jc5Y47cZHcH{gb@d`TRF{TWcN|f~&lc}qy;gVS6h_*o1?%nUfU@0eUE97lW zH(9BKS>9=B;tKBV%5}AL#gXDui2=BDQ2C?O4gKpGj%0{v@#ixh#$XH#)^s?T4ekKy z_+I>p;mtW$aaxP)k%gNbsFIpGc;ku|M&S1KVb{36`;*#Jm_*KW?_< zR(=TB*1VR0K_H87-UoLjqOEXt4rC|#FsW(bZtfAt``FsR3-(=Tv|P~limv%&C5}4v zG-_}j!OzvnUODY(yNCn5Rk@KjNYxZK8f-Tv{H~sg0V4yS=j!vk^Q+vtRFjIqil!x~ z-XQ2|B#nT%#>dbcI}P|I#6^q*MI3h^-cLB~&cm8_5N}sX=>y!&Anpeb0IFfCn@Kv0 z>@^)*?aZ{FWr9V-oL-A8WjN(;p=2X%M2>Kg(hF%In*rRF6MCtog{%RcL4Ah+M(REh zH=bDM3*S|T1Gf-)jg+aRjyl$BevtUy+mUg5E_W7-sbu7qx*BZXEy6;m-tApu43E@2 z?LKH|?caU?`&UWICG{}UOC$W}TsXAscDC1C`UoX-O>}HE4I@coALfD2ZO0(D*+BJF zl#_c3&w+D7|UpeAfaM1L0cOT7>%G!8ir`8Oz^-lsoz~;32H#{%H)~)i?H1$|~ zRT48_R5Y>D5KDO->^HslQ8Psx)imZfB7CnCxUr2oNxx&PZQ`Rd2(Ntw9W704u;>Nz z+2p;$Q7vt`n-RY&E@UIx(u@<=KJl&cyt4BZuAu5)NgYEv`5JTv@3`NH*l=0=vsqD9 zQCS5&)&1CKH+ZOGkOQDD?GqB8b6c|**mSDPE#)_Du=rbi4h+_EU+adxK| z*c)@(vg+FKOR2%dM+x$=}%Gkm{Be?a_7_8dOh+R=>`^MjjlQ?!>iiEN1Q zK@LlCoCN@dk$`{#30AN|+y3MdBqQ}DSqp4{3frRr9-Yv&kO5m{R^2EIg%cIT@a!r9 zEqz>YgMA>MIo<&PNlF82s5lNSM~zS#+93XC^C4=BL&CiR7Pw*Nb-GlH%5jxxfzc zxXbZ;S?2D<>redWeklhL#Cg}%pZU)GRCCkw{(H&nsC$Ep+BggH19j!LV%VcS3`(`Xc6Se&n(S{Cq+=OPiPl2O zmR@$pGnyp79acN~)1eAer#Wk;KBXT#XBFal`xMOCW|nakMk@m(dDt}S1IFAGd;yeh zhDYI;w78^U=Tn^^d1cM*upBS!UQ%U6E1EjILS_qGjeJ$kUFklzCPMl=9K|eE4%aA? zbEt42@=mK^*~6L6KOE=acJ8=*DADZ{vd+nn8{~a1XGokhnaomw?fh3wY1A&fk4@5U zywxr`Us*{#MTGhtt5Cl#?&Y(arKU>(G&8ac22U02;bU+??cH^lGwvH(ginmc6una#T;T2^ z+?6m}CHrvZ?XE{V)}C!GJYxC<`%2^3uS$5`QIWW0RP?PJH8WeS@Krf`q`J7VnQ-jG zJxx=kZR2}v15JtG6!Ne;J}sCqOd2OkU5L`thKuPh8jo-sSCTXKWv7=Mt;HJI9xCb{ z_`o~``KI%<^5Yq!%BJfgWlW~SUjPQnJ&EB}at^85tskRswNjkz;^iOBps7YR8c(4o z4aqpZH-Nh^Vbz#hrOg5g}$rk zcNCz@`C|~nv1sbQF3lVQC&pXU%J55fxwzTe?ZH!HnF~1JFT`Co7;0OGRL4tD>Q@Og z$slflVYv>}&^F>gShY_S_@Y2rIaW4PY?DsLmB zmDiF03I46+qu6e;{{U>9l62mGp7Fky6{L|_Pm1Cc_UbEVU>voZD=4#GEFyF>J~`Ot zkaahIXi0txO5j}`W8>^=*Y?murx^Gn9&+baALS27C zs}qKrwqY&ihD!iRvzziOSJCBGAB0m>Q&eIx6Dz~P(&mA+t+-TG7;S7U*EKBBx09U0 z0rjYbgcOTe{HcxoY+OKt#gE1zQ63^<)-Rw+@5M90Wl zLm0hQkZ-cTrp2jiCJ+dVW;_Yy+O%3im04lSmHcvRp-P&kIaJ*( zIv1G+DtatFS_tOW{;2)q1s41IH|PR@`^$_`Y9s& zt`0@rx}!c`Wd24aB`spK24cK3X^e5`MQQujCJvskt*LyBW?)ziwxM#vn( zC3cg0AH``ho8$b8PR_BKfUz2e$(2);yi(Wr#K%gFvfy|p7>TUD;>loGu74{Zl09}G zMOz+ z>^kC|&eXtLYj)e#lSMg>KrdicxhU3-1t#gsB$;A-GCDaLTK1a|c_LP`$N5H-&e3|9 zo9Bd4!7 zSdXS?{{X0a)@KoPMn<+cpBV$r)bQ*30J5>BneOJ}`+N=sG`bl7?TdBJB~8_tR{toN$rBJlg7^!gu>s)vNI;>v;fW%t725&;8Jr z+_8#%Sek$4xA8{IMzC<5-n8>EO5y(ih;p*Dek3~nLtnvcXOC0tnZJrZ;J!9EQ@j5F zd7p}sF^oWPocj}v#_X0YKldB>q@2Bs4m9(Boxh5*{JDt# z09-%HhvnQu`o{kNl~19|#E(c`1yk@$UA!Qh{J}EpD;8+po^^C_%Iogtf0Yxl%ERdf z6<~L0i^=PY!v2}7VwbvocvZ)XPI4rW3qttXXc~2gv z!mFK7`gcek-6uo14&b!)xP?9~HU`>o(kOIIe5tq;AI+p5*laMd7Sgg5;%6d_a$5Z0JobNKKdc!myu2@!1Ds0QV|d z!K*k-4-T+%{W7mHt3MQbM%OG0scyMv4=BIT$yZpvZ`{9T(X+Xrdy)5qWO6_?dEA{7 z513?ZnYEyunAU^s3v+#kg6GvOGv^tF{{W^9{bkTdTklpOO?iEU(eFuwN-OMziYqJw zfQsl@3n&1=MmVAY0Hl(i9Vg2?BOel{VM5q+J?+1@bybYM^|Jnm0DBd^0JRJQ`!@U+ zTJtfqq`#XOsB2nmePXl5`1LOCxQmyGTR~!0A2Xi2HE6iA8Z5K$`14iw4zBSNWrVj* z8A{RpQ+^7UHxuPC4#H$1w$v>!$wJ-e&e`qw%3m9M2d7jyu_ zgF}8ThW_iU%@fN^(sOMzhxJO;RB4Y6lwW4VN4aApe^rLpOLMh3X!Y19Sv=FJFA|lP ztrSh@;wuT|JeV3iWS^2_K9X_DMqG)8L0Ia2DGpU|qn1JVPZqdkt&g)XCF~T2RrNdBbMZ1p`*5MT- zs<eqt4cld9fU`~;8;fk` z7ZPo`^Nqd9jLJx+ir4gx!D)^4N|!2}bXj8E2*#H)@+#R&<&|=XqG>Um#lf}jZTav> z&dwNa8BY30+RBGW8?e2_xA)mk%Ogvw*IvHM1p~Bt=)}NCsO>@=Mu#Q@qLbb}m*?Nz( z$8+l?S%uH3UVQAgqSBJp9P)ZeqLs`cq_o=L@k|yxacNrYcMXCXS5!t^2zKC}H!7f0 z_4tveW7ux%YGdJGVV!w~Mm^Uwk+->GD)G!e3d87VT-c>0rPjNOK$->Q!Tf%VsOALy>lA(r{YnaSUwfDCJ!*zX4g<;gYh(q2! zv-kIKM~1;eg-}OBLkw_J#4Q|c;MAI`=;LFY=e4aINj3pbuz@sKo*!E+RW%f>d8dC@ zFITpg5yNh4Erw)jN;<(+Ek)jaEi4Z%@pJBP-T*6^<;>5B4w+P~6^E81LFSBfH(oOj6 zv`*F1qs-W-S5v8kYv_QR2y0Ib1QnQlOw4ys9@fUuV}2Ys9(}7tfD-&Hw~d@fxGdbX z)e}!3Y+!~x;2YQxdysr~RJ3yCX%?=QRzTS_%-J;?2Lb2uHrO9mHKL)gq>lVRvY~A& z8s>>-X*RZ&4MoG4Ht+Z=IGtSP7DY^3zQyO{7V1@@M?CDyI&!IwR6mIV`e!kRONkae zil4~(ttNR{6VBib+#P-(b{)-?(=fV;4yu}mjkjw}hq+o|m=liF)MeJ1?O^R~IA5EmT0V9XjlXLhiv#nC1!_f|mAs3#e9}dAPB^s%tsFufl9&2ynr&d>E8Fs!H zY9AE9#vUVf;@2YA7B{$CgI64FCBy&)_;6T!^H~NdQ%w3Ztj8H^$r^dw4b~dglxjy4 zieuqcY3R+qS;ZyAu%=eaX2|zDQ(M3gNcdS=YqxdQ8zo{gQc$*t(@OGWE-!Beg0&*7 zxL(Tk^eIt}Da9{hDN<6C%$`dE^g=*X5jiC&OjdzrGo%>9b2TjE>(hXJPAgxN=Y_$dAAlvN6I1&l<4}#)V zHMRJqW9lZE^2qW_qEQtxQ7BroVd~Wbwyd=Gs{24fd{$I+P;2i}~#`$~7C^{{TIO)pS^FPYsRKQi3-y znMfbzvWy!^ed36Z@qQ|fY(|CV{acCbb4n(NYY1gM5Dz>$w>&%IpXpRr{7L(l*r#Pb z`@f&u({(s|@cAk_x)A1nMb}8#9o`1vS5`?%Jf1lm_O2DSvMc*`r{5O z9AsZ=mW{j$6)dM|iIokgGeR_-ZfU=M+|_OGZEADFO?;N#8WeDzQ&P@6{^YuxtBd|e zj~0$1`bjUQKnUT-i0)aOLkX;>B3a>-G29$R$JVt}7=)D6l@yftg!ItV03~Q9&K-mf zAvD=p1-zc2g~RJM?LPHKYi`yQ_4#}s+;X_Av};f2^J65|5w()RveZ?_QB@R^h$$+n z15S}E+vgrvu?1#jo^}U$c3!N3`~#`nO9!ZffAcJLt>aEuQcQI^u1hTr!&5YVgt9lm z#aY9SR>sgC_DTmQG<-(tx|W(MxD1fXByI-9lR)Ns>|5_nO@~#&2B^b<7X~+nkJTkr zcF|gG@kGAOy?({)If^&7(^EN{agEpa6*>&VSL78mSai*8Jxer79@{u~j`fWp;OQWO zITdGtg6QYC^hN!}XtL$4RYIcu1=daw=8H7)O4=6ru4|g%yY+EGM#C-YGzQGZ5L`BL zpqmk*5eXW?>5F(9*cD@w>T2q0=4=wMxsBD;$SxncUE-xe9MP9Ik~$ntR4w4A2d7e( z5|_#=X&oKj=1b=gIaz4tia8m(1<2>gU(HQhQyFZqQ9{AF0lTb;$7y7hz(nQ^UXk5p zp?B#N)NzKq{7x;c-8+d(k>?%_ z&G=O{HU*cUrjr$=r>XK9i6W}%KfwD~1;`SLh#$Y=h&|D;fJvar7WBa9?JOs1BuUu;BDf`cgkyInkX4hRY>+0b%X%!w%Qy3H#YWPWww?5#W&v1 zw^Kar>C2(cU5(L0E24ED_q~vhGomL z&x>NzuuRD241H}ZqDwm4Y8+ZE+R@9oLFienF=Fj_4)deE_xnq9IHac++C@L-yw5vT z7P(#s*wzt;!Bf=3B{ZV`og}Vsw}Z#c6uD+xG~u|ToP3_LH%SJz*t`N-ZENs83!zPf zVUt|GmN0u;a7VFVu`1c&&2wO8XL%(fFHXn2lHEW!fpczTt6NImt!Uyne-EagA}Ttt zgV*`*D{(4$Dkvs`7VCTJ4VvBf6`rV|o|;V)LnKb@dq`SH;F6|8PZM=IO1E0ay=!wc z6^+Fus={!IsHn>r84DgAq=o?Fu{=m8)}482PPIhkEIfIB#b{TRIJ_*JsKV~l8p#@b z*Ezlh%Wo(SnX%Wgg~yLs5GLKcFJzoXiwvizbadWHSr~ECD22@q9tOo@mk648jK06P z^?MeziH~fRKy5tV=P5!;v;P2yoBsfCtuTU@Rlg@v`yKe8Vx?)@52d$npZ@^KTl+`JA4P&Y z5#wMJGUYsWsfEUeK^Gvo?F9E)>d>~01)hI)->DL-+H@;$TIWYATpei{xILjx#c88+ z1$8R}VR`e|KcjbNf#kErvOzHf7D&fff-FG_`wGNb);htiW5$_ccHocu=BaTGR1o4&w55RS zUrD!ZlBlqZLb_1eC?7kI0LGHKei0L+%O8?4DtRepWCess+g{-F;I8Ya%a^Ggn`@h) z%ZUva-8&h$x+&sxQ$865oW~ZCe#O+XRu6~MV3gHtri{-tA<{j<N0vF+`^O5WDr*4Q^!{fbFfnq8ur{@Q+G|RKE_TgtRc@HVwfOT zU48+AyK{!q4+dF9s=)I65tq$D2po-%$tpKVyd=qVn%5^QD3;?Pw-1u0SaA#^4Xck+ zoIIh91Y2MM4jrp;77vn(p^|*90d#CFb7Psol5A{9Cil4<`773}sXR19Amy>cxF}5C zrbGHmpmeoEfl=aA&%ki=Lr*JQOl8H!<4-f+br+-T0}I8dYUi$i$o9+{(jG;HugNZT zQ;L~*#|@(<5!mtzq+)qnEYZ_CS^C%-2N5>@`>Y*xZ3P_eIAl+dhaD2mLDSh`VZV7t zq`@Fmq`)Y;s6_aLo=X#j+8i$TkI`?;?5^@i^2+y$IGYr3$JPyI<;LJ|?O5prMO3C~ zWP9B1FLvaq_&se#7bBew&aq>2x~YLYKSP^Y^G+r1Yj;8hD{`-+_|6w#M!GY8giAhQChe59XTaYHbV6Ce8$GcZ~^VHS2eF|@nCs%>~FAz zQH4LR%rx$~I-cj7`pUJKMsXO6b?P-ER{I}2Yc8END9NdAqx||AX4A$zO({XAkwkc* zdo+{CFF16JEiewvvXQ^>q5DN{-Nj*Y_%mibXN#oO?73T`#c#ioZW!=0&`(gVi3AG3 zJS*;n(Om=pXm+3u$ifLTc#+jzIQ=^<1Z@nAH?RP+^W2r#w3qxx=7kL z-+=s9s1=fQy>@44voo_Wx)>8BP6{xwYq%)Hxh8#0Ndqj>p zi*LQQRTTtHon!<0){Y&ETO|!Fj@0NFPi?HCM}bg9Rw`l=MIhi<$l(Knl75q6o#%0wRN~c{{SSV;EZ>V)kx4#fXYZ(I0)PDRhg0h07(>mjlZ=1h zsIv>&Yd}9$gXUE@Z>@8mX*XI@!9j>)?xDlvqoVVxW|7c0uG%NEV{v^Op83(|sToXveTJyn;aX!!9 ztaCJ%>SN_%?~=shHPtn#0@GS=hRIhb)dF)UhXXeMxi z7UYfO!8Ct%K*`C{yr+xI<{!> zDhIX)%GjYB2Af{}tC@Ko&XS5+n&>`hodS~!g@akSf7*8E+M?pPrVWL>LsL=K)VSG{ zunw|kp5_hGsdBv)ERA#(c!MFcn)r=d+}8rqExEY25Ju}gk#P(v40{^F>F8sIN;;P^ zIc=%$Wym*iB%Q}^HRJo)dCm_-wid;^Wz4vfbkfr^FVfoLX{{{p?$NMktd)+%V8U?> zClze9m6KOd)`*`qMMqfj(Xci<2HnSCx=EgC!DKPH+^=nxaxAlhP(@u&iAc%(wr;q@ z(ppH`!sB~t7vzxdsnL7k`D*_Fp3*8)=2+atVL3(|LLe#ed7kErmiaEGv>qpC?c%3b zSK}E^6`_u`^z}wIYl-l>+L^-KJUxMJFicAhlCk(^8%oM}pe>EsTVUdD-MejLd-jEs z>6)KQu*$$TEp%tZZk@vTt;K8T(5>Bh-7M{%hAW$RLYF)@FY7u*#j2qOjuujaE=!KH z@Bl3KFNtHd^g^n;5T0lp;52o%ZSF^+Hsq_Yk`5aY9g6P?Z&`rXL?hgFga`0>(|eBg z{MXPWN)%wF1bOv2RFhI=B~u`q4~_8DK9-^;lTgy|MY~(T-0iZwr1FX^8xE1xjgp<}XoR*F_B|r=}7D-m&+2 z1JT%RZsXtDv&A_}Ri9|rgtrq}Bre3I>YmqIYc<+~Y);EFn`sR7kxJJF{T+_ogD#$f7x-Gae>^~J1>qD^Y^RJOKp(R(J8Wj0i-^C@4;o2X> za658D+$FMAO8KNZ9O6SSx3Nm6r!;t#RXNf$y2Lk!scf(a>6?eLM#RAKPN!Pt_w8J` z=VZ;&#_Z2f;4(B0riiz0FB(F+roce6swy_-$i==I95?b9GFz}R8+(0 z;bCz4&hwA7r=KYMUiAiAk-q9QwO|fvNGHiry@Qi4ssoGt>aJR$erhY~Nhru*xP4Q8 z*S$uS)RIZANFjT>T);}KGZK;{B&Y)NI<}30+p_9IDuEnT1ZBjjkm^At!)ggWl6pCf z@2OTq4`P=S9Br})DYM_56(rnrytm!^m7YVMHc`zBp3v^Nn-kj6_$I0{@T+li>ZBx_ z+2c?O&Wi%2iJE0DnYRYV%>v#ExOpv=j2Emaz?zHzEH(+&@K-1-asA0aZj#<9Z3;k8 z8zpo7$hjjx0C7ickO=v3K>;N+ZiyRsgaN_x%m$k}Vr>MA&Ry*Q{Lx{U)lDUvsJ*@G zOVYkxtdAI*9EFaWnr5`hT0b=419$E=2I~sfY`YfxlY-8sDw&M5Hc)1$+|3ijbeHXtUz~04YhI44jimkyhbUrs14tsjkK=C?buneN2zB zA4X*bbIicVt1%NiPb|0>>fLMm71J|CJg4dD-h4&V?Z+@@fF-4m{856j(cu*+OPVar zX-b_9Wj810W5q13^lIfECg}Q#&^D@R@r`VmE?B5Hbu<_K7rDORP&60}-m1NCSyaZ*|8AHJMI#^uBnIlp!GlZEpWgn9(>x#0PqDR`C>N$V8j|vW8)*< z30409BHb4CIx{aGFk+wHJb<+81&Ep%BE^IvPXltsmhWWBhDK^;3$2RJ!EGDy0 zT3t>D9ptygYH-ZWOvbUV5;TAZo#zi>*;)#$zlizObTB?cuybBxBfvD<+^_w;((c4X zDbj|JSTG$!&D&s5YcVK4n^nDx`vly37U9`@371i)ubL8g2edqUtf5q8is;6uC#0$$ zp(Tj*I1h4KjY@ArPc$dFl>DKH!}<7x!?mWxNTkfQ6wWBb%Q3gorNylkm4)%)_lyYOcxmn?$&AD^W-!_o)j(_G!)e=16twMZtZYprfvJ}U*I4&Si9mNPly{4K6Yyhm zLVW1viQTLyQGZ^sGgAl!%$EBOB=KBQgEv${-l?^0A>3O$O@Hl!WpOrd ztE;4CG||FZ-$n6&2=#DNR+@5hiozli!oz1|#7-YYnRBK&UykAPU^uoMVuqF~*2w8) z($ENu9wNtnJFYL#y~NnGcj6;H_kJk9DOXC}6!_qPJC9#T%MV*YKIK_bGYoEX=<8yt zJk1;03H4YXa)>M@yh2*MooTVVR^r|(kY?EHV(BVsms3sT{cA^Cpy|`33*UmPs>E@q zi4{%6b#v<+@0zc6C0PATbLxh!A<`QwQ>f`lNQVw(gQ8^WN}tQkm6Vdy!s%Y%Ss8E_ z9DS>IQCUw+;455dT6j89qDfqQbGp&#dEbarvhFp5QD8K* zm9s?oqG=BSn#U4*7cwuZ)qbuq>nIUe^K$mLz%Hw+>Zs)9_Il?BRg*I1ZC}lUk98H{qu3~W86usOqXH z=8jM==2ZUWahD{P2Ja)=7-g+>Nsq?b%A)r@&eBbe{4Tl`VcFZluQsotik#f*N}F0C z(NRj@&9+cG-Ve!5Z1mF8mYS0NtQkExa2^Kt9;KP`Cbw|+4_QlNDr5ZJLx+e3V09F? z`WsXl*t>2YOEY&d!EMX>;cI=@TjG&E3ilmURaEsXJO;4G{{WP#sUKM87CG&3Jng!E zDx%S6u=w@@2XK?B{rn8?jy11Os;DFZGESmbYS+-hm1X#nTxESsdF#sFo15YIzN0kHY+VfOuFKHC4te4 z@2DS=uW)G{skcPKzK}dNSC!JJ^Em76DyX=uMV)S+7Q(6g6Ro-5vIM%Y-=_$iazL1^#H%hHhQ-?I=p&5*IVRV|wDr%^i3k&o$ z+KAxWk;L-gtFSy|<;*5(VA$oUmQ8+ZuGWiNa;~#RTV@}INmmvm+NUwPi#d?F{Ee1( zUPES{O;Jwj6>0j9L`^jryrKM86|Ti<3+I*6yO0|-syivkv3b--4YmpB*c#9pEEDmu zRmrnTibG9|o=I$rv?&3$9F*uV$(%aMf+?f5?(ydl+}twG`2Pf^m;R#Y;cnlmJhHUKV!As8(T&Q4EJ zsH1$cwkX(K_mCO~5?4pSOC!W!CBQYhi6`!qoEt1t;S^HVR5(poA~tor*!Jc4s8Po> z?h#Wl&UwEDqSeNwI-22ZDynguOKjDOQPk4;7zRPA=G$;ka6B?hO~yyOMNJ;sTg>(W z^HDQK8~HZ{Y|=}rqFGwfT6SpO>hmbPMVFl^;deOtf?VrHl?+kwE}-Q+%%#e*_1NTh zV_G9G0^fA2dT*t!fb;7^kHu4D(R%48wSkUDk7bpn89Z^-vPnG_U*NSV|>HDscmnXdD=HN4aDV4Y1A)irL_D2c>0 z9h!2N% zJUfce@?CB&{{YJv_%GDQX1zxI!dQH=-m@N(Lw)6O;oaJ{)3`@ZB!`1qUF-`LlE%D8 ztlNvp-?=8wN>v}1=*HF7QAWY?GBK&|Ye#F1mOl(7*!&MX<$Jhz^X9en5JfW!C186` zBA^T+iITQhqjYa?0@oeAsW$avPBKq+UZfnFegtgWEvRB(K9rvToXX$qoA$`!ixI+P zs%rtOf0#B7Xd?Zo>>`e)ikLw6LgoTZ+D+3XIC?Usp1CTb`%Y$w{7%3&jjTo6hQ9 zW`_)nMTgVJZ8~FZ!0oo{A4vZID9#Qh#`E9N-ow3ZLxWp$zy*oQ?K5MJoP%vnb@_3! zO`}}3Cj36*Dy?FT$wWL;W$44#Jb6)=OHD*gxW1l#D?f+fX15V}Xwug|RF79U!;7ol zZ?i!7C$JIK;ZjHj<<57oe&ye(F2>Fn-%pv(g6FLxHANIp%;k~(XjBG{djh&Bb3o5f{9Vi7q)&DAEIPW#;islA5(aui5G}=acx{!%OPY1maHGC-_!#ag z(@>Nf+`{0K24VVNa6^r*l{|6E@hpwLgQ=HwfIbUtPnYo6$aH|p;BIbg52!5Gf2E2F zh@Av%jgE-y!C`QJcwa!B^-e^RM-CRy=FUan_&r`D180s%*#oa<--!mtVYCKL;nEp*sgGTg31st^_-}2hwH^5 zlJ~nU?>~xunsUgCLEz%3v}NCPy^^f=DALutwN<`XyO}^2xbs%YOcLa&apswH&OFi^ z6Wn>H*-GQd1dJCrqik*}!YV%2sZr7muGCg1eXtl61~3nJM<;MBm$b*Jklk0&A~S#blf!qaMaTCR0fq zZY*?#&%J;i9xGd0(cWaIb)br#t-Z0z8}4dxk+ zH-rw9XRA0xUR2DKpPs?$NIc_ckbbEuqG;uT`X`D=o!Iu4pL+Cw3zc;C%{BNuqW5%C zH`rVFr&7F>q@vKN&`D)81P#)G-9f=#e<^5jC8QmW;d&OUDCNqQG(QQYZro#UR)1E> zlD&*x8Y}4Stfp{+5DT(N(Z*eQC8DXG6Mi9A)^uBkzUG${sf7OkXvVw`Wt6MhOs}KN z<$)0kq1AL-R>pr7#vqm5l0sNLpp~Aj$+*=vCl(ws7SVb&-c1uAWAcL^RVaqQ~(jaGL%i>Ref_y?_Tbht#w8M4{7z=t`WI zQ&SdisRU|-9|EProFm(;jzRBO!Qhk?Jsz9rTeX_%ZQqaJRe#Q5jo*GP{t6~6?6)=- zQtC!V$-^$oTpQoLWQasnJrbIEMZ0;mfk+1y@FXI-L)=Fr>{_xX1lTbOs0E{$;C5YD z(`H;kAOqal0PP#B$Cb0J>hI?%QJaLep+7SIv<=1=on)swx=8QGNmC@oa7;nv^)!@| zIowN`rQXqEx$sK$rApGAiz|u5-bsYu6>*nSc}+9V4KfKC>^u9^8FH-eX>~1qG>+vQ zmyO5PcK58TQ%@1Rb4cF@5U11Qm9X1W%$DsdEckMJG4Z^8Nfo#rFGx>cK`xQ?qZ-@o zZNh-|Gj_f^IB^7SuITerk+>HJv%RHUH!@IHvqftu=Qgl2#2iO~JCAD1I|`B5d~V|R zFtSoa@nXU3QD26!uGJHq!}=!6pbR?ztMp@ZSKwinhp62#Rg~hD)saFF{3{^d_m zPb@EVbr8NcgJRdTf;~kYtk&f6MeH?6kG?i_W}m+P0R-MM*I~Tn#Sv zgYr5RewyO|;-O;j9-Lguywskv9E^ih$(ry(#&pxk# zqJ9S)1L^5&q_-DBTITyj(=MjMTd;<#AJUn>{8e2(Jv0vz0W~|v2QZtDSX0@VT~cjP zQ5{J;e;8zeI73%apKB~a@&82rPuZZ_|u`{DaxT_`ivL<0541rb^ibqLbnEs zaU4xO!BW32;!Xh5vO2A=XeDdi{!n#$lnRP!idhU*Fv$#qi)3kO9-_2H-jmoU_NUk^%U^T(aO%L(n#C#JB5)p zNhi+RiibuX1gXflsces)Ndrk~b;k}K34>wA>KiI1k9NxMZNhbx@`LBGj~(G6c2IrS zfw8%o-4E7j~rOx>RIJNq&JA@)r7WVLDIL;k>3>bbu+h%TJKegZ)B#~Ei(EKvbM+1jQmvU zTyCNk^>EhB8-C8R2e9m`I;WV-jbn=j#n#v+OK-$0DKtkaWz*|ol-_E3N&C!TA8Txz z6!cP(@1ljgLr8c&0o#vqjhCt0CMes)$p8fCe*(5|tEFTuISUOZz^xFWQgG+x z)5PCx!<~3@sbI-e;jC;zNU7>bJmZm)kJa3I)ClR|FCp~?Hx5sX{FK~sK|@^5t`{^t zn(Wi)?u1y&YJiTR@l;!2CuQ6`4-xHEbe?d`HDf9>aC@RNXku|XYhnKYE58M-%0~>K zqJ2GNvD7*5)GQ5#*6dU+hJtpJPd|~$KCG=Tjoz`i_7xJd@~$4%%QZ6$F7#%M>W$yx z8(+JM9h@sJR?HVr?V-smrQ~k$aYI=_G!)d3HSb|-T<4vxIIMum^R#*ORN(g#eo72@ zqKl~w_}~mnWttUMM5=G#}qCfS)+05JE}}e z6*9frBrm??{{Z%@-QJpTu})MW4UoRlQr(3k-66Y*Z0w{_(@$Gj6(u`$ODnXr6J^pe z=24@<^i))lRaQ6DN1DUjK4;vQQmkRQXu-25fMq($_6nN4RMgx>$DZ-rIe3+AmucaT z6sC>{!&n?$->@rbSnV{@y8Ia9?pXY=>*B2a7CYdtZBfpZSw8|QM-z%hj~Q_oZ2tgv z=Bl#EEvlK$1Q8$2SmSQ~O_#uYLa)ok ?0IG5=cCy6&HbRI0{?fVv*{{S>d8SY6d zDWQfKWg(ImIO*)(a=cJ^66OHU&!TPKMV#}*q~p_=@M^b>tr7R^ozy#3PHvry z%k@A!YIc@^er3btxUiI}RZd#!ZD~e0MRr4I@|j)@?HIp_D`j<&$ixvlgLK)kx7Go= z)ap#VZPs}!Nn#^}p>%*z-DMal1V|tQO;)>29IotPPK6&6+Yg(==9@ugtXG;VQQ{1# z!JDfPkB^$teb)J{091GY{xu=&XU$!Ow~EU*9>k3HixkMX%d}xNPX5S+o5&2jvbf*P zj^5H0=4p^cm+IYp^jLdUKS#sp=RcwpQPS3b&jR!OL~0xoNJ}V}JaB>$Te&Ep0tpjj zESF?fPy%d?9gwmBBoWyH5CI^Cw?RQ5KqImPijoE~6Q+hjJ1y^3cwj%2Tin7*vBn*y zh_?3B;1nzwx2To>7(7HO??l@YV;NH zlMr^N%=_^rEwvL<<^4@-2%QDMO-zbh3Oi8nYNVo@@)i$NhU^;9_;8hM;h&DM$Hy&sP*7qxd7asmutB(vD zXfIU0%Ggx6RTQvD#hZ!!EsQuCA^P5d!!eVh(_buoNRrlAg{zt zp9{|?SxKX3Sm%Mt$&5(G;>Iz-&TFnf-ASQ)>8|&{_cg=~hz9-~)q0ocOOBM4Dmp{h z6#6(@BrTDl)<0-isce7_NthRsx++U4QPW$Jn#e`d&4?g^BOvZivJu#%HZVCSTEqau zVotU+@M3YTQb5yVvT>|V0G0;G0WE4!Zs|a7lmY-C62L~#oM=D<&{D0kPzdd^03ehQ z2+#mW%??P=00_`W$p8{Ci<&S{0fo!@ZiF_w4XC#7iR}LXDF^F``&hx`8k{{XhShc(W6c7s+ChE#K9Lz(_&-~N>pKM@5^>p1-rv|>yr z?H_{m1q8xs3Tc7a@Ug?z0bW~f*n>yuh3QP4H(ix5nJ;_M#yyP#^45t+j1wHUyP6Vf$?(Vd4@iw zddP^eWUgE%QOD|#aRpFG< zxzz2uv{;?HM(c;*=lK?-c1eHLE4<_f^_fEreed*E_7ahD+-XIjut~|}tj97=S&s~= znhZF_BO!Cz=FI(w1dWNZ;xg5`IE%F0uS#O#_+?PH7+yWad5*91P70RwX5#e8);d}6RQQohZ_gC^~w3J z@6rwrRLqZ2S2J3`&Zvfyxx0Y*q&_C8R)spVOPQp+US2dV?Np%U6|y*lI8A)?zEuS~ zrh{2$Sk1ijyf{~;>#?}`*E`=MlQc5_~MM@3}IVox4YwIK@kv|n=*9NJ169aZ($Ev6! zs1QA{x=`ZVgJ%J6deCF}3lzd|*{kHLq?RavE)CQU=G@16DOH4hTXm z4-~NQhBboXSal6OZEZM?o~QFq4`~kC{EptWo_$b^{T#M8+xzN|H-jb_3W=l&Iuu#oFNE+6r`P z!hP3~P7;pjdO4+x-I6({#Z7}7#G;x7?!5mN3ZyG?^?VATbDIiX1*I3Ur$sI^)helb&9O4 zpsJ;InyP7GapfZjx|dIxs+n%KHn=OSKRL2C=yRlvr-&CI`j$!9O>90laeJ8u)3Lzn zYKtx%3y-3@3u`j{e)CaLyq;|u59-+_qb}gp4An_JRXdLMbDJZ6N8+(^EkXM_^gH#eJ?f^r1jQ(xMNJIM?+w)7 zXt+_&QY7-iMGJ9RB6rfj?-rY@3T(r;2BqK+t1p~AhT##3rs2&Ai76s?)e2Q_6ho)x zT8g|jsioC8T@^!!+DPvchxrHx))ZlfV3O+kdfHRSX>6n7(yFAxm=300r@1y`tY@Jq zcitCHjm|c2>MhaoE+Wfqnf8Yo!#6e^W1hq7yMAgGHGpDRRD{&Qu0N|hM_g|3aM?y> zWn?GmAr2p+Y^KrUl2S?5#Wd8i4h{s5cutMPpMw_ZCFE0*l2(T)#LQnlQ;O)#L=_H^ z(g^_p{$q{(t9^)cd4N>Bz8@&1BziGO3vb}yekroJig;qxHgXH238%a=S2vI(BW$s_ z*f?D^Nf6@4n&&k6t|P$NN{X_jwi@*h(ic&K9vasuB6B=f&Ps6q${odHoWiafFwwlL)q+jJd&as#v0PL!_~F?nRZrt;6cr9X&J? zfGiK9N3FK^tPURUp|>?Du5%8ik`HKEqe>Ej&~u!lEg7zgmX;~R^i!CgI5K0mfN=SQ zs{C4_64q5yO2-}8OF?UgalMMnMTt!nwH-sZnz+MpyT)L9TQmw-c>OJgZ_BAyoRwgt7||ETTF#Jo(K9UE=s;aX`xYBInXd;@dlSMDMcik>;vM zT*ON<#Mmo9TKSL?E*>>Sg!|{pe-C*M$t(?bs%#*l-vGt}wNRf?ynm{A5 zSyX*RxGsgv^!RQnOX*@}8&*f=uk(Z20H+&mREMLg8gIvL`0ayMev3lO52C8U%X z?&$fqv%zih{P=EQ@_HOqkCs9-!8mr8kI+4#Z=rRL_Nd8Lhk=ekbj-n)Fj@=_wrFUB zWr_#6u6x`_HrtxyWzTTk#H}&8jP^piCburI{5HD~C&?+Km;+)*0DMtqY$~q=Wj~a< zBKg4}Fq5X%;NGlU;@s25!tv+- z04KP1xuf2rWxKjcr=BE%@lx?DKC-5qF|UX?JUf_8V6=5~ZIn+I?|8MHeY;hn;;mdV zvftQ-o2s5PEC@*>boW+KIfZl01Po94l%B7MQ7}dp)lLr<0N;P^Q*l=R01v(a%VW%){jLMu`_-u_x9>L+{&XpB zu}u_t=1n*Q+kZ*zANTA@$r0$Ioz`&k!+hamuu=29R5Ia=ia7(F*9(E*RJ@yEg*w0h zWF!IL72I5Hb?U<@)GrDyXs(TO$AJV8PywKV2mm5Rf&d7LG!Oz%N=0M<5C9`a2IvC6 z96v=6{UTFXBMqj1X#jeM??;Web+NGDW|wT8!3F5a?Ri~^`n*0X#r~|t9d26&EbSCd z7Tv@(SKzJJSX@V=8TXYt9&=j+Z9Lt7VEyVo1l!b3;4A|`vYK9}50ml#0MJWYh2%xf z&(Vf5J|;;m{{Vi}jGq~041|r#{z*C6y=bS1G`hjgBiN?pX!PL`pS0&6eZEU8mo29* zH~ap>lFeL`$W2rd*qvd>@5pSW5TfE`%%pJ^+<>y%!=9{T;>J{XgJydZ+8_?VxO#$K zXp%R;$eQ3@*Vuu?a8`IzbyYdm;L8tcr_JOF=pg}s7V5VEcRzyeTvSuLQU3s{{R~uN zidpd|;6wP1z3yGjPARM!9XMiiYQsk+~`Z1FxOLozqWyk2GzJo=MhIlgR{;?BY=foa4b*$4W`y zq(@1^WMGVyY^5icxk`$RpQ^1QrnexZvJT}q43%-waNFFZM?%Z))nv$g6U>Ke*-i+_ zPx%gs@(1-HgmY(_r_y1x&;ixN3Hwgn-qNQ>5iS5W=f)VW77O#qpgMNXG)F5468 zJE+kHTQkju!-8-|FTqNd2_ccuOB2n4>Y{n|JQeB3 z^YK@@?*%P{o+}U2Onqiijo;PP#_!c|`K`I)vHdR%Su$SfqdxSiKZuE+b(|T-)y+So z7oxg2cD#>;IPvO8_Be&-T7hbiPiS6*=)u>>xM=Ka$LqOXHl`GC0N`?{DI{e@HC0X~kP^GPwJe<*ire74$}D zq*t_|!O!x2P~XyO?;RVu`6ms(*mvy}!LU$GHBaA;{?fe-(ICQ^k!WSEnbOGJKtRqu zk?j0d4l8#XOGwU1yu2iqTY8$U>AIB~Uq5kcQB6Hf9PNfDy!CFrk$Y^sy@)o#L2beL zuUO((?L%o_6%1^phe_Z8aTfLk-|ZaO!AH$BR>8Mx3tUIYl;}kHaklq}3*q(5~p0Y$FhFWpmH!wI?!1t31+0 zthKYTt#`5Cf*lnbQQ~dvs=u|P@Jr1@PIAI!zoh05)<25em->o}$%uc47wjqVa4DyJ zRTpF1=AEw4d(UOLtW1PC&fFc&>!N2U+G`}Zz4ZJSId;_&gUQElTF+AfSkjF*Xa=x^<{a?XdJqoe{p)Dn+@`7P&670-h!ns)H1nX$O` z0q1;zm!8>sNnOK}XKck$h*DAEg`t%6j20YAjjeT#B$r$cYVR^o%M`+;t#N`+t;jpE zRW*wZX58B~Q%OZ&5aTYa6Sc4I}Rb%Z0xsqAwLBK zjoen#4-(<>TA_ab05FMKugc523vomS>s6Jga!J8J#c|Db&tTRZ9L}ikvurHy6Yf-a z?L7rHFVta`!mg3c1+zO~3FK3#-_fkvT6=U8J8F>Bf@m&3`zclo96U0G|SGL`SW}S>Bu;|>$8vH{kHKC@56o#Vykdd8P9`Y7iMg~ac zToMhgI28MH9eo^TLh&tyfOmSXt-^CXY;Aot3+0AQ$CLTQ{MR<--?4fdDAw&a7_P)A zpu11fJbhOD%Al>RmZ9Ep(%bqYg41Dn12bX}NhqQ6T{{6%OAF6%x5Z}h96GlSua=Iw z6BO|}&uQMvG+`+*?nxUgp@`Pf80C(!?9u48$50pUJ1x~dX{jdih-BN7bo>`6p`6Cs z4V9EtuN=x^khdIzG~pO+H9P9*C#YnahtNC+iynHmKC61xVllnegBYFDQ9KiV9~DHw zWvZN<)>x@7T@Lq?scEWmW)~e*JTtS&nRdv);qP3(r1)T?&9XyB1a$ETm~=+a*4&Q3 zy1q*Vxkn5C0MfJIxkpN-`Od4kk&o31w43K;TF}l#Go0|!Hz#FpjNz3SU16iE&|Fv@ z=9_{Fr=o7E*jw80J?o?Nyc*ifJO+#PZ@IS}!q*AQokjH(IILwH-gXw%#K_rT;cGN; zH`z<7rl_QURaFGANpD6*7N22DqGwr-Q8)W&$Lj^(t7s@|W9sWEVyJHmNWkkVPOoPy zV6u-gN1b1~DBS3t9NsG9JW^8O*{-&ViY~4Q%XGIz zcr}By$ABH>Uz;l^jAKJxmIG?=d{Q|bc!f7u0poFUJ?l0IBV~JwVsuzW0r>+BmO7&1 z9amJ=*geC88P7=V=CicL+6JyR-E;1oT(c8RzEZ;-BXpi=MI`PPVdjr1NlhFRnu;ju zF73568!4FN^n(tTYFs_EvqS^gEMrM&zT6cS4~a)Z9E_4TyHU6;33!$!{8pFpWehU9 zY2<5K(9jEw*64OBp0d}#?Lw^|MCEN~4BBSAXx3#n%|4dNRfjfRxw`gu;<)hQ7WY3( z6c95ErkFjnRIP#JeY^R~2H!|{tttvBHPrH|S;^MADu{muT;e?{ho5i@C(`p8%yyc> z#KFJ$irjQw%{fM!Z%OhK{$ds%rZfJtLf%4^zqI9*Yf+6`3+R@ZO^Q@Vc*h3bvbtMf zEj_-agy>c42*u);JP_(>a}~s$6_gs5s$e^(ulWZLd0UklES2xr0GxuZ3v4y zl#;|WC?_RWC5Jw)0T8KlZP%{aLG1T!@lkNxGOC720CaOQLffB2fED(HWvfECnull5 zUuPa}Zlpe|xgS^Dr07Iivy2$g+=iay{*+o!16omOPiaZgpa$)_7ETtT(ts0XSC}3H zAdHeO^6iEA58kb8sIw!jQ3&i|KGL@E`14#ve>M|v@lBF4-}6Zi_NU+#i3%nF%{Xfv z_RF2`Q;18umo<66GSrqJh_g}B%x^6`Pd%<+DHQm9H8a71hB3!WTtGghGRRpeRFYUs zzbfzuBM4a?l?e$N7DC7XAdbi&0D=hA07xK$03;AW02oFQBLx6bMhR|q|qN5#9uV8dyTbh&^dQ4UIPh>2Bl&CtRHUc4UNLcaZe>-F6x_NZ^hjG>!VVz zGLjZr){Udwu-{8Cid%H}8YnC_-r`GC9A&^q6HT{$?09Uft-7hl-UP6;+TauI{{ZPH zhr^oUE8FxfaV%3{!Yo9tu=KeqEXG8*jIMNH&5QK&Giniv+}6;?qJ{?cMA7a}ke$(`paH;HB!$D9=96ah zu&`zbaxoiZ!mx54t}FN-`13JIiX{*z6ZJY04_6l{ccCz%N3 zF1kJu(UE8q<9L*CF}u1rw>JCdVeBf>8r+MQG<|hc+l>Epu`yt{4R?2U7;XcG6=&$6 zxVsIP;qDA~cXu7`?(Ph?y?MUB^ZwD3lBA){sodPp6{1(Uk7e{9w$F(yI}`;_RjPP? z3U*S%z>jKP&`QT`&jee$Q~t#lxNv)}Y{ViWm{yTqG;S`_?zuB0%Ud|~LHMPUr& zpdL+HYGU@V`4yxkw=#Ni{3$WfQE|5`>jnt+`z0G3dW8IkPk=dY4L>Y?LU7%_{(ZIi zIm!3)WXif@#)`uzZ!{F~#eHHja*Z66oIgA@wnW_A*W;qt8JV{=z}y*wpD|l2M88B& z%F{y2V^aMeKF3-AjL^M49h~#wECD|WulAb-=!?ZS3r*k#`U0@*R60%uw4e}JXaS1G zlP@5OS{!K;5J=hteHnxu16=YkkSgc6k-}!t!g@`8fiitoH}{dOpgG~EL46yHPMgy* z^N=^rj*E$BUe z?A1gT&tfg`wq%>>M%-Zq{(7nn92A4Az$_eJ>*lZoj369_9w1he2vsw3huWxTK>+Yncz)#hG)|^Z7cC`(ze1?KUA-bw%tWt;9gtx z;$^`6K_NmpABAoym9p|vWRzz#2yTIUM$4F${H6RsrrY;}iFXHl{VRAU8`hEH^crEe zAk@Yg-8ubOIPVjlSZ-1Ad{;^( zE6sU5Ayq&1PLJH^OS1&>=a`lkWbR)9mq@9%*+`JTjSQHS7*Ny`gs)RgOHyQ1Qnrxd zPmTq-#=2XpxZaq^(YCoQfQFNCHoc6grzh&Zo_QVLRXCwYMN<)JUZ3Ceuqrm#hA4}? z;lLyHtm}25u6fWqilO#anYz4*!e>Af=CwlC7{(>#l;AC?%JA*9!by_sHC}V`{O5zu zerfA?)!%-y$LvWM`){@cA?9T6O}N%&^sCDhA?;PNLRD^c+Pa>gb@y2|)}aU^^jM?i zJ@~nuI&Eu8{l=rJ!o=&Tj`8<;FPuyA-Pt))k3T3C6Y*tHw>Ly*Q1G&6gw|)8;doaE zhVeb>9C^h~6;2}3B}GOkd1J}sP0J*3zOmpDB=h{l`bnpW;F4OdKMezEEXF?GCFpJ~ zO5-S&uV1^4O5S}~Clt~+&f%{V4QiEaXiwS)9N5+tLUVY_Ay_KE#t~t}*<6n~ud+TJ^1*_< ze+hX}BsE`c+}@V=6+9!zu1kaDE%Y_ERU^}c)rwF1^^wt8t3xe|vV4wDeRO~I9#2Ck ze)v$zu`ykw4>11XLBun54L-!xuQM3ey9JXF5i$G<<_m~;vSWY?sFgn85hsw(rdnBu zqG_?Ut5@feD82aL(*-v`D~rvkQtzF^ zEc9E9t)*}R0^iP@sFaou;x^~f2}4mq;R;aakW|O~DCpdeIL3fb5i)qf;+P!lQqV}9 z#iYknZHo6eJIgU&K=#P268}S}oaXFL+_?o=cst%C0cZ!vm4IU?OR-DcXqeJTtcH&m z(tJOaK@{0?!TJZ1Mvv|6&8sa9J13p+nN}T@%0~P3q)AA@;N`%?(8cvD>9H5}O%`RC zBmIsyANBf2b{d~W2O3y4fM2**42IlN1ItGWU;lxwm!*7^hqMMikRU); zRc$F6?wGb0=G#El&k?Nqsm*J`zu^2_HX|g!r)lf<&r|wojBazrUAX?It4GSD6*IK6 zK!;5#z)WiUM6n6;=iq9oqF z33&y*1+xMQ>)rWZqN2`GY~9J=f@)OWEGoSPW#PziW=GvcE5^F!vaW=mdyKd$?WHOO zspT9**m#rZOAP^2uthc{Bzlzk0hIy|@`VG0PdWswkxD<3OE1~&+PYG8+ASOK>$ll8>+{J0 z#YbWCi6-qD96giEG=?h1$sKCko({kpDtFH

J51%{9cPo zb}a36stZrr^Ze7Cjw6XmUL2c~Jhevhb7X*MvpEjoygC;r?*;wCIH7HE{DkA4=R_IG zPPk8bx~XYRYOhuqcD}9EIPq00?$^j!y^x=8s-0x?+06KuL=srdmRXy81%DjNrNh*k zFAT~mGDOg_5^p#RZ>_1a;P{j!#Z;Gx@sY{){U zdyh*k$qZmZG!cYL&igJWq%Ug%6oHb;+pwl8rF4OKw5)~jcXM{yoxL`?489QfrX9gMlxTF0MD>}tkdyI>$sz^?qQw{R*}l}`d_jgtXlK@vqrrH5 zo5`2~iUb!Dv~9AQ92jV!;^wqf2Z|$HMJf1|af<45nZA5FUPrg^l!GgV=Jaa_>M7_) zT5{CNaKB6|eN(Fm=1^Fhl)utG7a(V=Ka*TYw>SxWx{g&hTwydR?IGM8QR{ta3G{Qz zd{AY~M3We@u=)zwwwrb`E#aappk7qD*3`tG2~N03;4H<#uf|>shhT}d{Qj_wXbC`8yX{3x=yl10y?$`FE^>ebta!AK zc7Korj4OMN%;UCfRQONqOL09`(4@P99$NbCSNi7aCl2cmElr#LdYSjh<5A1X-J;^sAR7u51W1XY45=S$?WAo)ZCUjrYtoYLQX=wl z3bu0>KVV{S_B^f;R@GT|sQJfVQl_t4%i@?uCk%rOd4p-lqWq&Q{CSV8 zyRQYIR}4Dv7Uib788omdUAi<#vi^a-lU;%}qh;76(TjTQ?ztjyjPvoe1@Up@afhyd zedSeOkkND-oGoZPt5!Z`y;H7_N+vUn=G#tmwj$K`SlqmLrdTSg5ssxNoW;SG8g=Wg z)1%(=2<|cLv#yplr6(pE-;(dIu~NdYcXqa_^9kd&t+x6N!Ij_p1!sj#07cZOs6~|E zw;ogLFns$T3@2y>&^JL2EGHFI!;BvQw`0)#5>1%?E7QzDUp2|W0)3%^s>PA=LmZ|M z1;GNzhagyUGC*C4cHj>ZmLVG<;RJytNALkPJ>it(I2;JSe7xqaMHM@0v4b+uEjUgX zafaNv#8jhhsy(~1xGgy>d^;3VcU51UAT&BL zSIHaqflW(0!t#16bLvEqz(M=K;DL}=tj%imq{EL#%K0f_4xJA= zr8)k}1kT*@KM!r~|AFLOu@l{9|A7?alRVDP=`4p|F2&1uKIZ2d#AB8k%dSby0?@c~ zZ*FOo6cX{Nz=UbLqW40$YCsj2Z_;sN=ddjz!$wY4z)s)W4zea}c@pLO3IsdvceSq` z)8nf|2|gGdW_hG?&3%2+TiNw=b098|&crKYp8Gi_`%q>k;H;B6*#4oS>$k6=zW-K62@Wyq~m^vR-hNtKaK-&y=nKVQTbrO0xaSSCr4^EX73Cg3MLZR6j_> z{lV%+T=ug=3-x*ji?UhHJWf;MC~Yv_qNId3{~ddS6&AAm`YQ7y>kKW;q)wN|IqVG^vNgI0a=*|O+U z>?QjNaxgXbfd)z0T4RDHv5+hl`zL-WsqK<+2!4u)d^VhD977k4F;gi^3|r#@h4tF@ z5}&7rLIgh7Hw+`TWMIh8Lt!(Y<)J=2U(Tw!{ds{ZaQJ>|UYPo1-Ux~ysu%OI(zL9y zvagImhp;egyQczYjtj>4Owk}ZaxSiq(<(M9ZjV>j4iUwqFh6kiNf0@#ve_m5KB(&- zXg%)!?bGHJ3n1gDp%*j_E)CC^t!L!|x*1z|R-1z+ETzIrtp6l7Cc!9us=?!`e;Vg* z9K+OQQoC_qH*RpNK%Z|Y9Co+A2;rOgx@4MOmaJ@IbV0tl>~yAs({t^GpNE<_83i$N z{teVV%uN@23{7(X6=^hHwn5U=Ji%7*)`Y6w$9@fu3TZ#Zd2pc;;u7Ye;rH~ro-RBWg?}{uyO^D zh#IWgH&xKe({5Az1N~7oIAc^*@~UW=vR+d=k{jV_P-nU|i*e%T(!!$$uiacT@5d=WXv{|2h&=Z?K2BHK_h>jT$9}nq9WKJe+`s?leSjsuB{+n zC923Pk0O&B;*sTnwbMKU@6<0{*0xzEa#Ouext47u+bht^(V8OgC2h>F}}2G27em(Cp6BE}oZ3$q`kzrgTA1LH-0YE97?b5Rx^c{xVt z29a6g&|1SaFQi&G4Y9RYGPfi(pjyOKIGNR`I(xG5i}0#164g)$z1G>n{=VCZapsxR z%y|-0!9S1yN|E^?%?P2^U`3m}BGH51V_q_GPGxD7NA*E4YdzZ;_1@oQ=86uf8vm;@ ztiSOowFE`!iXYdQ;}ms@Td?NxGba`#>|WcrHxw{^PD7u{qZZ6Ps&E{aO7Pw3*rs{v zqqR5s{PuTTu|>90^LFMQyB!WiSnd=j-|4aGay@10pjU8Qnt{l^GTj;(ywD_ejY0iY zix2nO?yQVR0VMi@CT3f4tb&9@g_Xo8W?Px^S(qwuqgN9oJiKfdBJJG|z5SpaX5t%mjaB5gXAqOoYf zRO=GOcpJxq2W%~T4>>DiN!2gGU@>d~)=x1e|%yyPX&Q7`f=y|-w&T7C^*@@tGdWXPQ>Z{JbCk-$WQZ#dGE z!!WX$G-TUnRA+e_WtqJrTU3v)zLIBl&#Nn_+m?1yK<+Ml5KFBbR2aviRjRVlijyOT zwSOB%U1w=1oLTM<9MMW^h{`L-!1MlOxockExXbRpZjw8MP;O>ksB>%_+EW&;_gF-J z6gQ!oO9&e0*rQA@{J3)L!=MpYYc0h>!N9O|L<`jsHId%zg7Ob!M6V!e8mMY37H-8Y z!(LXymp4OCPk_a<^QDE-RTt`nJ~1msPrOj7Pswllwz{UKTFt$}yc)zBopyk{Q`f6I zMxi|z;x(##$7hUeT45nGa#v51w%b8DvS4NV6;9%wE>&SOhC&Y>x*OnDaibPzy>JuHVA4`1ZoMO+Q`ZHOg?@=*20c&ln_I} zCqqX7{NP_c#l(rZHBj|{*u`WOf%_n<;=3xw{M6l0!`e8pkceg;l$6ZIrVjS-gA(k6 z@bMj{gcRpUXI>mZ*`}X6ReIH}1>FOu2!h(Z6({Q!vNHPwok+eIcNbV*v01D0MR8FW z$7Uub{xdHteN7EIBd|&k%cVt_n&ysqb&l^k3^bzs=gYrgt({qEc&m_H^Ek^7Kv4$A(ZCY5DWm#y(;Dj~`C=gEpEf+1U0V!;7-fR1Nx5X8!3NL6Q)`yd=QSLiPschK`;*g z==cRhsvL+42f}WWHsQ7Z(tovmxH6 zPQo)wE(_-oTK8iB8|2b{T8b2>+a;8IXFxv0Y^>F);LHxuxLC91$EH7qbtk~E4!dx; z6%qR^F`r3f=qM~T06*(~agq3%3^&d;ghKcIlhpN)Dut%!7keldl;G3+Y|>d(JY6jX z!J6RjhOQRdlQTMx?KT!O(YYg%jSZH(nw~5~P3Y#^xbKqjCH!8P$kxXz3XgJ z#xC3=ux*lYVvpK`W;~G7CQHL#AuE^0lRGU3hWPe~N%xA^K#c5EM1`Jm(>r1GBGRnh zEvxda&5{!?jOMq_AVU%%edZ+&p@n&|h3?;i-if9!XGUCz%{Z4BpT*S6@| z(Y#MRHT0Xi9TJ(UEEEOt2B)?Sk7YByd(rh_1*0SCk9WFrbG?~`S-R{Tgeuh+inUu8 zyRCnW*uhE+g;i(>QoLu>WEe?{=i&-NduQfkWu70-l z`F+8lAg#(o+RjWa&B~g!l~f{EfeW{Lk2jGgSmz$T^G#gwV4T59vf|HkGRq_r9`n?Og>ms1-hu@MXXda?{<_x3-=+nJQ`Hjg27m5p zN-X7H$NJ{dY~ZZd@8{JS)e7M?fsHe>uP)vSE29}foae)prut8%LNg%<_8-0oNcV81 z>b>d(tyS4N#QW8$CBoDxjq{}Cng52pj}ZhipudaQ9+C4;Nwhw|OPs&wJeEsB+AnqZ zlQh$>PA+O51HO1g!F!iiPtBBj=bgCXX$1whaRC+0<4%4gI^cUR;utZLa4pRPe^-|r zm#vmNyxG(eqPIF+@w8%Yc<4p`bMo=s9`=3T1Yjsbo{d^i(N7mRH#I2vvq=+b4Q6sc ze1NW$ZdX`Yfs(Ry(b0uwK--j$NM3gDCWKo-0~7^DSUaSPNBE_iT?oIXUcxLN7>i8| zsmt1VPZz`$A6B1p^Q?YO+xiPCWO9jhY_nn%)DRkI@)X1wf+}>@$CE@NUTE%T)ax^7 zWw7Y%6Or*$m@po+!++%u4)!m3-XAE*_tos>Y-s)2BI3pv@)b)lx46&5SbI>83hEqZ z0m|l`pwPZtxHL8v0Y+~@ZXGW8fk(|fuc|@7%qiK>4cT4T7S2Cyh?;+In#lpY^=7g( zPb83~yG(7&8DG;>&TAeHD+il&jL?+E>j#P;p|~hcYv#I1ozf)gHAc1LzCL9aFI8 zfO+9C7q9&HR~vdrhL(*)=G(R*B>{P+MKm9|+U{?zhFEmZ*d44tQD!m^_LxL{RfRT8aF%1@V` z&NycTj{QV25zM=P5x!~?Q@@w&@Nv$xAK7!yxGZg2x>Vp;#mW3Fz}$6A*_M-+;U--l z)0uYR2s2!^N%N2^;<~9%Me6p+D^8C&z4S59oXb;C0u|FRn(QSeVAm^sD$gq}w5Nn( zG!0x@x+YBWM1#EMN{95g_@vs0m&+sf8WbZ_<5AnYL0>wCQa-rB1s`36BVNQF9hWKy z7&(6TY1d1aQhdS2EL-`eYFR8Mm`Et6ZTWF^E5jbdB5`omlq5p7T{pXx@I#udBF$~b z9O9C>T60^qR*e_JXi&3tr;arz#g~ei)w@j8k21C_DR3l&kFYvo8J`rM6apdZeu$tk zj^V%Okz=y_*%6tb`vI@iVt^1G$hVZWloo(i>ak3C!vCrg9%qd)sxV2e_S{Z`SrDx! zMWRX|JpF5Rr7Ub6XmM2j%GFFLyCVl`M$YgquJlL1S*C=nXhcIN(OXC(0FFzxuM~)^ z0gPViY5;{q^AxU;G;epB<=e03p#0s*p$arc>24~`N9fzHjNcl4s+%IJh=$}e(|k$M zQ35w1sH+rCi3WuQ7CSzn*DhTSD7fLzncAzaKPq z>n>#p7tCGh)&(d}_b`}iKIswY9*!NR$8J{N(Jj5;|LF+*5TTuzeo;fyw9j9;zD3hOn`cnwMT>ctTRpWTpt$Eg?YYbwY>ym-)MwXPz za0x?XadBae(6LH|I*&cPOG+sR8ZXDE<7jQ~0-{f`a~LQ$Oa_Mx%EMPFMAfRnV#8Bk z;7nZp!>_T+_#Z*#L9saiiw2e_18^uTd97Zo7JydD0Ux9av{g`CxgWr~0T6c?2|&8o zac4m>(2T&^{}!PQe1KUJ#sgrOt>OV}Ye9|)fLA|!GRmeFArr}BDE~SCdC~^T29UI) zI+zXF7Gs40EkN}j;k^jUnDF7~ytPIdXPGFj0wC%UY5}3Z(HU@- zUF2#7NMTn^3@H9vF5~}~?!iE2Qf3nr1Eqxz>>mR~8l8so1t#qy#TQuM-ZCr(ta)q> z$e*|#u%h}Sf;jm|LnuLL;%Og26bBJ;gpaf!3W5l_+s{JQnkeXZRw7z23=~R%h6e5u z{)L7EdET!g*u$;6F7-6a11*#9HS=$=xtSHRm1eourd-q{>flpRq|g^#&|R6#RT@6} zf{{9D>>a`6fhw1KaIRF!ah|mRFS4LI@N3ZAa=}a!CJ$@F^JvjH#N zL&sxsPfyLbJe&fB=e41IdZ4k72$k;;{4?4p$Dus=e)Py2c7iCmtmLV-O5-VIv&WEMETm*yv)%Wa z#rG+yDsABwxMTnCQ8t@o(urAlQj%O&EsexrL$}r;f z{5yLm%j_)3Y_@7}y*~=rOBnMsNTr+TME7AmK`obSei3%h893g9hf(wZ>`MmX0 zGtE)#fMj^lcQ2$->SGmHe-}>5$>^G7R);Yii#Lgh6LXM=&^{T;hkDDV=GQ7lXmsOd z_==OsmqGe4^8I{Nq+C_|JVxAKs&ONo{#m@cpCTiTwHfi@;l`fD%x!+8tu6Ik=&5RU z8SSrAp>dG>ViLigsDBl`OD<)b_I;VA6+GU2QP30Z-HO^Dedcttv4(z6U`PG=#9T4J z+~w8I&5>%LUs#Qz?Zd26bH=+k`?3ErYUrEIitS3un6R}A>FUM=%w0yMK6AD!4?zg+ zlQ&u`n_}A6;&Q7@^?j!KoThPNBy_!}5d@t-2Ek%16F|%p*F@U}jt9$Y&Hb;EHB(bS zBc??=WcBBtpxZI|Mg+S>q`LF)lnjF1O+g zYMv1;&zmBwMQxpS!LR|jz~3J{Z0LE1gVbcl_jjvW+`@K6_ki{j1;~s8F?m za$=l0<_dc=spsb+3E^L(Z^XFVYRI0(CE?QPT$Qj{-H1(&OIP0s8n{;L@BJI5eA>{} zJxXG#H|2aUGIEF!hd$wY0n{+IuDaQp{&nnnCKCPBOdXlp0ylTqe(<-4RacNg622mw z6JfRQdxp&BS>a=gB1W{9~b&-?=ZBk8ml&*_ZQdXO{m!28A^5 ziAY0FsY{|eY2`KfNw54(Ug#k@;WjM`zf)ENZ62{Fb<&83u(yb%JyNH;JDq-a-Dg+t ze|%=SQf=iUIZecUom_G_U3dNJ#3MlGoNj2^@0d}^SB1{R{p`Sfh14KpHPh^4#q;M8 zkSBz?4j43EawsT-&M7%1+6^9JkWgrQA?VSt?=#}Ze=M)`Q^MR1y5Y)|ua98{n099C zpoksL%}tIS-0u^LG>khs|3KUMkJJk#yd#DQfkuB5?0;&}a5u1wSzFdcdjvT(BCD`< zR6_AIr0K*i4|T5@r}{)A6`Yz5QaBYfS&@_LD>PfVTr<#~bT6Xkq!ZZdA+eiM8)3(R zhrV-S@`5hQXAMBBb1jKFpRgUj=oJrvta3B^^PA?P225)DsQt^Z{NvmuIaS)Z zX4>o~?l?&N2?el|qOyKBC|7faX-j6>p|W73LUmi(hv_~$Eae*3?YlB>uc)T%W4XD| zFcI1~1{$_Cuf=I2fP)}>yOeTi^0b+W#oGgH!?YHJ_d}%i_J%b}nBz{`UYUMkV`*l8 zJWXPYREg}qP%KMj|b`b%t3d&28wx+kL~mUZGU$B&1`Jv^RDwL1 zv2ip4fci1JG($!4r-8dck8JobA4}J*m|&<+Q!> z!n*prkZ6lH1!gBIBBR+GU&qM356;VrNTyYgJ5PM25m&#DQwo4poumi7GAc+?eO6E4m}>@Qw%Iw^crR!a2}3J7fpMUr-ord6ZeX%6hO(l^5L-IQJT%6r`KG|A9ZLGgrY+b9TDmRWUX_7nvS%4-C z@>ES_jExw|4suNfV#Aco=741K3v4731%N~uBVj?Ju;!%DqWFYU&{jVYWd4Ug6aM2@ zYt;cTiX8*MCm;kZ7A?_)(GUH95gh=e!jS`x@v)X1<1Bm_3P)_j1YlqPw^4%h!?1*y zT_9Km#)LBIB9ja|21c@ks^pX$P(k2Orp)=zX@7(D{#Z}PuMD~Pf*!%WmFCPyH84oM z=8w=&iM38!U!r39LtOSk5}hlDs{(}VCoYzlLq1hHLUAPdTXrfq5HnuVdJ1@=vfo-r zfv6BDEhLKCk05`6|3=SfQhp%uxR0cuNLX`F3x#+b;S>}s-+x=6ftCIrr(|R(#0Eep zz&St!G0U_RI7k{hfuIQ}6lYbHRNl~fAF{Se_y*Ar;(PzXrn`v?P;pgy258P45`%2y z+KajgF!|S{>@h#=|L)sKre+eASd?;}it)iHjK&0gk!9I`YDw9g) z(irXZg%-krQf2TMO-6KkOutrveH8vI94tbY9$dD$+3k@iY3;3UcDP5?Wnv{1&R%ps zwz_U>u_JO99I^3Wa(`RmgXMhfYc$w}Ur|C88DA28R%h#rt-0$x(#jquce=8yvmzZA zh^6t@wT(lqj6a&g{o`*Z1liWPZvtnb_F_jct)T67M#(%rBlz=%s5j9^27P}M=&#z$ zCK8t`uWG_61n-Swn=ry~B%)QbDGWPZ1@dJ}_wNkWl4(yLl<~%` zgS^}8)J;+j{n8M_`kL{|( zcgY%d?UTWloMyIyqB@5L^@X?xq!9N6w|j6^1Xg1Gw^X`(ced8|P@11t+T|h1C)M_x zb^ay^AWy%&#`8Sbb(yHt96L=l40oW$C=|U{wj-e!a^IS=q>XXw#|@@;T=`jw*htJ& z3Ru;8ec=peOrx-+w%^R&=&`}Qzz9}tGhnX$-Q!P#G}C(>YD0mG4{j43_&fsfIN(m1X7ps$68cCxhiI})-DAea?B`I&( zZ*Ya{M!=4`wGk)!2uI^K0@=Ge)`_J@EyiQzzAQDu~t~R%>j@(Hvz$~ z=Cj;vOcO&Tq35Tr+vn;}_rXa47)Aa`IO1r*+Z9Cnrun<$W9adtTBg3z7i(*18eU{#;PVDB^K=AXkWTL@6 zVE0}Cf3c3k1WgZRi?8(S)#{f2TA^)s8GhPh z4!<`4kPA%pU8(M*>cVzGt)K<7Wm>nit&tueuA=nLpTFRM4}DY8UNUJHO)*7gf*2;2 zqrmyC>?Om+^uOUeCgzS-r)A*9;F??J3#_~@YW!9D2g1E&uRQZEC=`eW@0y+`OW3hm zM18RPuw}m^H@Q0(KRY9JZ2Sq;y;@LeZ6O*XCqFmGaVuh?dG2ppW@}Kk&Z|lW@bZ0- zJ_*#n?e4dC3=$VaxlMKJ9wI@l$Tbui?-ZJ-APGex+nte#14B~t+3(K_USdrH1bJx1 zrWFvPYwhByn=XU2s(G2J-ljO)i_uE@A%xf0ychPe^&ZR%7I*WDiP%KRXF=ZwRWIF^ zmp6=7Vl~)XH|saGOw|=Fl9c)fF_U*|TmX>CPixJl}@71XQK8c8dP3)Yb zKgZ+9O?RmOfi%-RKobl9W}nLgymJM1)H}uP-jQa$SHG%*jSI`ONy1xd8m5L2enHmP zCI=Ko$ThEcA^FD3l&*7xGkm3L?zuJl&q;baxUy1bxlJnoYTJ-p6;gE*xksG34M$a% z_O1U1^3!?^dw=bD`2Io{@m7irh&}n|FH-xhpZI+*vajegzV7^3(o=d+dW)+baui?M zt-K}LD!lcu&Y+!i%C1(!@EUTX>mZ4~>x_OkJHuXVy+}E_aDE~>U?xeKXzvT@-O2a;AE+e$KD@`O;|HXqI)i!_qu~85yB=3zGI4rlzqE$xJ++W}wV}Hb z5~y>NNJj~xF{FG9YM6NPk6m(BhhxR;ICIp!KB~K>OU&7)3RJ<6=i|$!JSCYQC+O}> zhB4`;ds(_d98>wTxTCd;mWEsV*&t{FXMtJfN^{X)S6^X7a3PvyKm3mjcW&;LDrSvv ze2Crt4%oxs(*=y5JxuROSu$@Z;~hVX6!{WX)8V(>=L)Uzy>qRk>gY!JW`_3Pp)=d= zx(dpqHYMK+@@Cvu4RF5Wl-{h#d33<}Ya~Oc@0CS`U53-AaassvpX^d1vq+sgdT6k^ z9SNys)wmK1`cFIwFpjr!PZX;Y&dznW&*qBn1@wGwf&1ush6_k1IKgIm!> zQta!%CfsH=twG zZr|TWDF(W`*JQiC4=q&jmpHa>-M)i+tDhV;@CZN0!}i9JYG15FDP}|UldW;ZMJ)(i zYn{;cbJj?3e=@Lchf9p)vQ`i|FpCYS7O*wims<=|iFdN3V?kh6qv3$@2171F8?1c7rA(>qj^FYNYD)HK};%k5&lZr1($xfzJUV%L5#k2#10VF=jx zNn!bc#YTVu_%g=-afVb{07saUXafEM8$JSF|L+em0buBzfDuj!k||HLo+1NLVaLS) z;Eg=_e^3rA-*^=Ie;_WX=?n5QG{!J69FG7g}03|NQ_Kjol-Y6+)v z{1A`;9EKgc3=0jyS;mfQAwTf@2#jf?R%u5_#fHy2R}+mT1Hg036J{!XciE4Oi#$$D zhLhTI41enBCi42ha$|J$pPls#pFc-sH=c(yXg3PtB2mo>j8c=R*>;7XT7>CDt1EJ< zL{mrKx(aXT7Yw{#I=Y`Msf_*uwbw_t*!%;f;lJTyHB_)Ttfdp9hkTQw?6*ieyfIjP zNSIgTC%07%nXRQpL7~_yUekZTm3iPB3Zvdf->N`?TOG@K&ntfA4u2pg+9V(NeDfx4 zVL?w%$&zIz_>eFVyXhp~CYL}bDTYAVNym&){XjDKYXIZkJx0y^#BL>X&OxpBQ__9%R|V+=A4(4ksAfslj>+LvEbsbDi}`$Y{;tJ;%LnNnQLr z39lP-Rf2&(AqH_SHHZ4h43nb|O?{=3dOz|0LT!R-9ej~Vq`4V*k*oVzyXmQBsh|t0CfRSikUXhpvYk zX=0V%tEJ>${V3iLz1}K7- zpvKx6=XB1=UkB$dI1TJYL;*|^H-7bO_ugv(TRvG4YT;%4d!lu^R0g7vy06UnyIh{DRlq&?qbuZPXw3aH zzwLb@+xvVrT2F(~8U| zYQI~IA#fKbztomxcISEzzl=$@Po%~NJqtG-DO@`oMhipq2&j+lccpOfH=9eSVaB{p@d&-G&g^^Ee7e(r*OdhIxvRSBs` zQwjM@Ku*gvoSVZr&_3pv#@xK~?>p3w8?t{Oy&RF;f1qV;{rG$Th)a*%cd)Y!NkmH=*1h0=6XXh<{P z;j~!@M0Zr9Q)@p?+_htWLMT7Iyq-=@xbB4CQDqt_&maygbquTE+07!NMr^JTEXuJJ zTs<$vy`W|JGBMIsTv5BARXWJR=c_7*9PyY;+>L3*tf#|(9WQ*}6t>5$kt+Y;^MGey z9F8P%#?^<5P5*#Yh*9nkJ%kii8XXyweuJbst!k^P@A-K$Og5tfJk{SYw>p550;t7x zW&eS)T{jq7gV*hf-rYCd3Z_rAr4Ct4OLfhdR1}mc|2Te1lP9t*_k~O9rwgKTy8XSs zLr2)X+rLzq8Iw1gKn`BNy`ejJPJJu<5(Nt{)`Lzz2$s=yK8q<&_$J}S#>I@A;zuwcuPLe}-9+k1;k2IptA zc;Ezr+3kNkD_ra89Q2v1622QHIOZ8}o`~wkQ%P>`x*s(iaw_McaLPaD(`|fD zI3vNl(cNWf0<%eMtfTIIxPX@9}MB{1ZxLe*MiBM7mS$Oj~)q02LXIP7U!lXrNx+F zmNcITdF%wNmvB&m#;w&%A7%(yww)U*hn>$LSPkcHFYJvQ*UsYV$!w)~UavwoKks(_ zhU^rw7v)C%40Gn6AW5s18%jbP?A;!65B%FPMF3Jttk6hP^c2#s5IgiD*(Ss#`ej z2l<&iyEu#IH=GQk&HW9)W4HQPJ-QB>qt$lr^hF4$>DkY{Jo}9^9A>P%Ps$du{Ywj> zKXD9?xd@mliS4NQerS!mN_zw6-`CS@1`9>sq6-n}!XF`hp7Etd!O)o*v({Mbqb%3m?14o2DOEykdjnm0_^5&cwjOKy z?G39-Y*T}@o`tiair^Hais|c2L`0t^T&xxy?4KBbUgHFioTN$Fe^8AvLF_+P?f=L% zG9)TsUHbo@2~%OAL0bXHPV~R|a3UaA{QvvaGEl7?X@SW9bNZA65=aKBg#U;-2`V5z z7n(7A#N|KSCT#*Nps!(>S~<~hDdDkP!~i{9t$@`pSf<*K05Cg<8iwLtCW-}Spfc%X zBebMqAb=-qJR*f|9M*AiSw^z)LxBAcfd6ADm3kvQy`DPK9qdsK6&;^zNw`SYc?>Qc6R!-_&1& z-7JN7FeAN>5!nF5yYzs;J9qOQX@{Y^BI@a$?!_OO8=9aRi-Z>%EtCeeUvUQA*C*tY zgiEhPHC-rHj=~8StwxEaF>5LL=s-`ow2l(_P$TF&L5=^N5m`F01mGP{h)a+b_@J0AP-X5Dw>|uQwYDB3zR!(;CMdzQ)uOa0tLRfWUMj^dh9xr3 ztT#?qbvanNEGmR=N=&6VIhrx>rrGDC*0 zcqNEXLrN_-CpSwui^IBcp(xid3opHtPK)&1@~70*qSY6MB-0EUPOzFZrIappMvU%X zZyV6Y!fLd~iD=A(;hy|sshPb&>;PI z_g&>kQdD_cNbEmQQuMvptaEd#4vy}EM*jUK%b?G=xKhiQirk;u*!T0+*8;8$Doyr{l25k`JMV1?^I3iZI_|7$ncIKw@@py4bFqH`?dGnc4qb3 z?b=UY!0gtFgc*F=Yid`s*30uV2gMpr2HX0w8^e1esafI$Pzt+2)D=kpQi zKUVMJrSE6|du1=>2fSg=Y!68RmV|3K>hKm%sisN(>wUn^VB`VVx{X|0#?&TudC zJNkwNQZ-TW52R7pWIJ5i87KN0E#dAQa4**~(Yx&8LvoK;d`#kn7`C~>b1&C}awY`O zTu~m#rQ{;{!VC)Oa5c|8Zv}5XrfWX`ucxm7i|T*AUP?+@LUL(Ikw$Xqkd_9K6zK*< zrMp|{ZV;AUP)X_T?vzGp@crQT_kSNBkjvg(mc7f&nKNh3#0FOemJEs?q8W#Kn{&MU zK{U_w)$Ng>&S|K46Dvw$5b4dv+Hb5159H8u(&iek{#`xtE?T_HCBnvzG^`y_YM;G` zkt>ZG#m!Nd|Bx4E>SY9a?a(C$y3Eqy2lu-IRljTyx!;U^ycFNjSs?if`F8B>!n{{h zya0HuNijJ84AY{src79y;LqQ=mnk&O_heMF|J^n0oaUNMZZvtb=o3IZjuUbhTuYN5NX(tjB1Wvf?F!!k{*4c-~vJw@ee(;jLC z#^GQlvA(E6>F-Yic=6~qPO(gYh$7E?9FVwJ_f~Jr7$5p7fRe`~I z9%n)KysI`Cs2rOU_O^KYtnVQq;J%fa_YyO-CGB13u*hfl ze##-7_}|0a7Ndi&3S~hs-EVXz8(eYcj5fF{8leVLv2)IDU2czpHt>`x>L{f9j=ywG zFD+Jmc}et{YgPQz>2BBn_j8>;4ox9*Ybi(U%%r8QXx)#oU@s(b2F)98i^gec<%tKb zZM#kp?CN8p5u&f1I1luh>j&jsNTiR8#vaFU@7rUGN>8Xne37tvxY$B1p#8<43l_k| zH~Ha!)ZC+PA@7WBxl=33qFMMchnu1OFLL&&s!2zxu%1Hp?%*^l=VsuzyXq2Zz2UOA zsz0(_cjKesY|nDZMrvmg&^z2Epi=6j|3NCZ&VH9tQdZZt$Dqi#Lw~WYXd&U0m^a%m zG#*`8-i4Z7c*uF?@mNx{i3+!pl#{<@*h9sN=Ndh<>F1+U*#1Ky`Xn{RK6-nFi6(93 zTEYQ;^6XFe-?(@a7uX2su{xi=Z+aNzR75r`#5@}}oHSgdtEuVBn9Ys~F(C*3-$(%_ zpfQC*4vp&Ng1{-Deq4~sJyp%|am_o^H(m2nG=6VUOKRRuM03a&G*2o-q`fjdGSJw3 zC`-e45M_Cn>TwouU1i9hw^)sX)D`~4?1Gh-l&%m0PiQReY`y5Lz5WpS>|03^MephO zH%l5{Z9*2C%!ip4H=;pH#MDhswKUsy%wpqt9Zae;;0KtKJEdg~Bgf|E?~S$^buB6N zYSb@9g>Z`;*FPf7tv2snL~dYD*u2r2IMz^7y_?&aNKM4j%-hw9ephrz!U^ttYSp+BVWd@UR<9W%OuE_O>w1Wt&^+p<+q`GU(pe|9{QHdwL2} z5}eVE^yIz^2^xuV=J*l`;C6s+0^|NiJpG>tP8y0D0{{$QrBdl=0lM*%{!a^s9e^xm z1uY_&A=Kf8$p7p6fk9IY7Z}bkL^c!^f-kuZnti+mS&(4hYy_164o710dbsjWtT0_Y zBQ02{ULK5{sjMwlymg^`(_iWzO*qh1SvJe1Lw)Wuy=Wt7a#2omc*r~2RI2)>@JpE+ z6Ly$ko9>M2^mElJ9UY63&j)YwZF4L4Hsm>Rx(HfzN6kC~pP`if(wn~h3waMpL)+E5 z=d;>dVK>#lg?)QuZf@d6e&yop*8JDv!i+Ch%%LMqzk?qw5!S|-{np~x{Y5*BM-i#4 zl#JH)Yo;Pj8TIHHVr{xWd_1n+n)J7kz#8Jti(3k=sh7)eS0LFP)=<&HR(xP)m7ZR< zwx^{bdnT+jy&2mbPQ1y0a#k4IseG;g)we1Rh6Ju7<@jss?~ZZWn${Y>(7R*=+dw~I!>k}!Ccok(K0icd4)=(*QbkS+2z@8XpdcU4V3b0Q|oE_ z%Vn)vW%;D1aimsUsERABzMs{tm^^!!9ywa`d5+jjXq(^h4+W#T+Ms!#k}w#7YG;aE zc7$F}>1GWGD!+J*aKXOo&sjW%Z`9%|JVY=QB*vk ze~4!-5;dR`+IF>?rykyHz?W%7591zx!lI`jhr{bJLWF`g1ZB^&nlUGr{=n%;Td1&Z z=kGPBEvxr*; z`W_R}s+#L$+78&Th>m5u$PG6@Qq~V>Ca^7*Je8@pUw|n_3%g3J_S;zP?2-qT4p-j{ zZu`Dt^9pwZ97SIt5ItFOVp8My4_drTTuS{@+MIsbAh2=|J>0Y@8z}|b$+Czq<0>2;0zXyv@kQwT`Nd#o}3yz&85Rtsy~AV4lt~) zhf#E=q$8={4HjIVc#nR4&IxvHnA~lu&b)LJ-d$A>9soS}~5DP1WIt@(bJo5-!bY$!L7wNgf zKj0zjeW?O}B+_E%JNXv^Ubz`o{lL$dH^BzCfCw}%Q$M5~ok_FX8W00L*{A1qJTni2 z=Zjy7fT#70=9lh)FW`vA5eL;ARD&g_@U<^%w|f=W1Gh0v*n@phTV8)5*lc&#-sbGu z3#T&5LEOXN#U;d7A0K1GPYgX+4?jB||ApW$h3%31rDmM4$*+DXpX#4ZzRO=*ti8qe zq3tmIG>6_$W@$z{|5NlYZ|2vYiI)CM!Txzp{50Q)4oD1A_w)HeE(nmBppI3Jv)f9}{=*zF0Io0jCq^a*c>yjorR#gg#0L1Eo+ zBOxku#HkMFrT41%4%Q*BxQfwN@uNfP4TX`+RNtW`wNFTTTaS4y!cON1s7FckQzjl$ zuXN4yxPOS?wJ;Q|y9>J;RUL2&q5?J9T4`T{i}y;d*rZ>o6PyjFKl4TKwqq;6pybuJ z6ujd6BcGymy>IMwK5~g2w5QHY(#&WXk%n&;dF96371Qt$+|Ug*yLmXtm{)faCz<%! zM)knJ+D#tp`@+^mbID!CJaK(3nPbcMgB|9K>Hr`qzwrT zf-00tiLcgz4NG=>Rj+JhATr2qa<#D4NNw2qet|F1_EfMc_?Jh5a+a?`6uAb5}PK=lK^<^bk>Cy1bafbqG zK-vc*2d;sE#1Ew8K$jE4@-LYJ#kjOBkT`=x3Nmc%urMPOZ~%n&N^vzfFZ=y13IDzp z29>=crhJ(XCZW1C_yiv534TIYH}VWQr`50o2b@>lMdCp)11HF*^wK^+HRFR!0s|=( zNY*i8fesxRPBKGI36#=Nb6I|p>(MC~ms!Y2*g$FnA(8AJfudA`Wr1-WM;GfZl!ExS znjd0IR0z`EnZrYs3lj`A*8armzM$Zdv+tTMn+sj>^p|!6&XOC@mF0KrIrI+tr{~6F z{8|=6O1v9uRAR=q6f8?xVIAKrHs9`^Z14{Jda^b03355Wno|9sA|Dk&cotnY&}Nzf zFZjHOx!C87?qFJ<-yg?6P!b~YvB36YI5RWl%I##S+>nB|=$@sWjeVnW-{lccMFT&O zHBBckdln(Bq?@dG^Ys#2>eenOh+mlBXacjq5{|Sc;m-(-BOBG|1-k0WqNLZC$+0PL zPDLYo8a-JD5l)*=Qw00!o`KtMwF|XHey^t->wnHCB@L=%h#ZwyF>s9|a+a9E#KjyX zve)=0fohBKg!=gWLqzzpU>H2<022*DfXQGRNwDmLktfJ_s_+CUIWozGXY=86zZ7dT zW+Sytf{Ct1`}rZpcM%#r2EnRIj|!L-+ZOCr=S~gVNqfou;Cro%81~&NXReuDBwP^` zELC)lJ1b2*n_zGgt8MOn7mb_WQe1iUh3ZiRXH(jefCo@RR{?)b&2H0@!X)&_zE`7< z%J48djy?W(etb6K^*FPdymm5zx_p}xCqKZQ+&Y9+Z&GuBvj6&pqvJVaW4)G;Fj729 zb{r-%&vymB#tz+AEp~f42vVW7=eTswG3KQr>=>K;3|cwLFjog+aMa{|#!ZS%iuU1{ z_{atZz$Bytsm;kG@7uR%T6YnI+r}Q>Ryu>+ALMnizmQZNVJ{}f^TbKGm;Xrx+2MsH z-fQ@j8-hsAbg=gaH~$5()qda-nk8~zblnBV>$}ds5aepP<{%|Yr?A@`FGjopiv3Ogb%P3R_|W?AXQV!|^;9W+r;M(hQ~eUfS=TR=*!7-CSX%4>>Zr{|MA5HOR zSa~3;TWX#wF}H>-<+|7|DV>AXM9#-aedbr+<|CUmVRQL4Wl}G^6`8lK{?*{`#r{G* zn%sdmKK$#Inm|;*r}#T0HCOMuE16q>7cA_J_anfVSGplF`m@y6T0iE+mtR)Cr0n*N zVN*TByC9AdU4L%C6D$8=^y_;r(gECq0`Ja}AXVwogK$s`zg4y10c!8kV%J-3zzvMe zyBm57Q*461?DwS^?22!5oUpX?4`Z*qYiQskZ1$Hq1+5n1=VAWWM~GHW8>LVPu1%ap zu^uj;GS6Xw0BNQgqe8!4P9bqKzDT?B8B%rT+69xgf7($PY~_(|e`Gvad2AG}O$jxO z5@X!Mpdws%YK8B;j{smR@;Dv$VO?gOhOY%TX#<`O6vSZ6->R6UU@%P{Yb8)75qwzF z%v=59fLVe2%W4%@8LXd6+4jbFzAd^&P;m#X!{U9$N$XWB?Jvzm zBM%kbMYg)-gNJQab2F-Bw`vVO!sT~-Uz(#ZtXPb4oUlr1iVE`q737IrO0K}?|0Sr&8Dahg@`*DS02dWYG7;iLF zRB)7)f?CuJ<9E}Lvx;FW;$#+4a7#+Ub!&e*Uw+)IEh0um5QOl+d)Dl;FWT4TRKcTB zt13}7=^#6$$_P@d%H-rTkYZmS&x2DrD}%Dq)PSMyEs*t3> zm^j*dvemFKD53~O+>rWfuGBPJo_pt*66`%i4^$EVSDZo<&1LR~%*jcBq*g&b1_|GfI}}35#`s7c zQWnZW2s}w6QIV{D<;>arkk=!z0k1EANo)QZ3JD1)h58Uib?@Et@38y5cO*bK0ql8k zG3eox!}1`xw_!lREbF+-p}7?)7wO+PWK7{~&yl{1jGTE(pU;Gh}~tNTgP zD(7{w72DE(P6GR!+k*UtoNQz_MQXG1nn$c&CQzFmy=8G>a6n3Q-1z}&-YSK-aI}t@ z62g|XL7u#864@!)w$0Sq4uY1zt&+cx7n+5fqrG{>eRGNDm({iobGoI(42Dw{B_XYD zynZG(p~v}$_|~pjn3Uhl__C*)xuxQpke+2T0f#CL?Ty_F&TXIc z+&!l=&cMt+jDdq`^YEjjOwsSAB{SNE72M5Xl7#ZAN)&7FY$IGKg#>jAx@SCI#_P$# z_!VgHJ=zqoTKFXuCdHSe~C#D3Q6uLjdAtSGmlk{T@;4x$_9y0kN1_%yT$m>c2KXMP1q`h#J zZ3e4a0t#l=9C~0YQLku)(JhVEm=zynsctJ-`WK=G+Py|fxmz|p4R1wq>8DpoIr~?q zBX#%gC!@v?KuzVs^%nvJlXK1!F2l+kk;tJD{1;kEUMAix#nRISrLqVigV`=T=l*)K zL0Y|L&OaQ5(x_M;q+KS5e05W*e}iv$ORLTiI-I{bOZruqz!defi>E;trGzH?RB!&1 zrSixaZ;WlX?*Y`Ome_SfOV%t^-DGvx7B4J!h)W7rqu5rxHWDofhlKjG-U?rD?k2bT zM0KLlcKHK#2N9jWklt2MLs}2@fp4%~POILq)7;gS*8nn^uF$n2_wIwJH<}UcY5Hmz zi+E-<(UDeMW}Zi=re?#pvRc04F$9}HUwl+=@~1u>QE#Hv`URrqPm;wn$INH?mE0d4 zh*P3YIK=!l<=+=@W%3eA9M>=BZ=`9SXba%H4e6akM;^B8AUKs#3N+;#k1EP9vJQF~=FRX~ML9qwK=3LtZ}m5O5^kyg<_%r76l3D}m|#$lJU?@XZ4GS2 zWv?{eut(1*9Dl1an42V%udwFqf@6-eYA9K(s57HLGjGn#l^KYK8Kp-4Law)HfEY5- zW7J`@=!5YQR&?&^T_dO@|*6{{qqF)+kmZj$1*QtL8J-8=S z9ACQgso(0{lJ0*l6hErCEhxcMP0>1+jl$_-O_L`n5czbfqxcKaU*qxoWz=*9EETtG z;FWjnC0bU0W9JF!=!$u5Q|;KPxJPxTskzqzTgIw_QzLWAkYs%LrddA2Z%(*B$|(*7 zPMP5^@Lb9>XcLaCzsZcZMte*~?1=kqT`)SeA7ijdJ#QyqR^I;@vi8de9nzoB(6N~! zxE|(SZSdJvrdiAWQFX$j+3A7R$5@t#i;s45XO!Ag9$h2GiOe)MOKFx`IGUt zSr1u=WWsf*-_CPr&pp$^&5mK`9uW7+@Yc=QdFY$CD{$3MQ$}oW+*w>%R;;>csk$3ovc>b0#(F0S%Hk5EI+Fr?amRG~*lQieR4%dA&UKxk1t0r&qgbq}aWQrJmY8a3~DE}eT89_gG zDB62S){)bmRasr&c>4JCU%KPd$XA8^|8$qBl3rf+(#v`d^JkU((S`rxH!3Te0@cmu z>;a=dF2i2&)GM;*i7G>U|4zP>*Sx)g0{oZbD{R%Eu_?W8OWV!g3v5aYU%irmW6-ZR z6-YpV^~rw&t{|sC{6MWoJAjIRKM=xjpYGV8kWkicATdO>0)rs2`1cj$eG6K$8>)bD zKcfs1AseF$AagO|_Cf+62@0T*O-RZs!`cn%!~2Yf4@1SrhWc^)V3Oa@WPsN~{xU~y z0D_(6M;Qaz1V0f4MhHeU=4n61(jy=Ypxlgsg471cTYM3Gh=@Avx8Wy{*c<^_KMCJu z!0dARu^kRjfjj6CQpSAWhCuy*Gs`6%zpOej8B+I~6jF22wE{GFZs!{}zjyvgVyXK{AMiG7!WsQxf7)?|ERQF8s^+ve97g*uAELn(Kka!QU~$fg{) zikityMl79|to1wUhl%t`)EWxhGT9wyhF99YeAgz>9A&1|7JXPh`P>N$kLE0D$sy&8 zXgi_g?%{2Jdn{Z5Thk!%j59awc19e(X!q!LemZyJv`kwvlo|BICLhXd(QbIdI3w z5ipfA!w@-^PXMErW@xZYXTha_J_u7-c2@x=1j{~x?l$&m`VHI47t$$zpsXVz(bA`z0V62ld>{Pfhoo}Wm`?A;`qLMF~h`WvtUsbrdW z&MkB3Pr~uW&W;hA^ytoT-+}G&Cna{$vY=c7<+g{pT)H9P75!EdEJo+?C)*N^v1(K|w{g>({d~|y zAdrqN)iLL47p|sm@`v}27Eb6vs-~Nxm=z6ULd@~;i?O}UE+WeFhH1W>^@Lvy1OwV> z8r=#TsRt1$xadN_WQ_1|#(MnEv*gf+c2AL8C+FF_WTAJ`psW~gI@7N$r(o9-?zeSE z(Tokxu~Ok;C3%+q1Y1tZ;a-LR@+0+O+T+`?JFM$`ngO-jtsCUr?ZXetO9#eB*w%j` z_HX_|Vs@qcz7CMw;Z9M1l=_bKvsK43_O1eCmP5b)B^L8#t#dh*FcC$>+r>%gUJb*w z795ybPfLMRxEn+scNLv)qMX{opMe_tVe2JWCVgV~KyX%h-q7}R7Hmqh3HS2RZg_xZ z963;s9#~m(c>;*58&zi&=XPjYmG(%>EB)ydg?IDuA4e~9X|zb&-@{3WWH{*!a42zY zXjq_vCtF8G0vsDxhUA|^f{K?edsIn3A(_BY98#XhE!_rlRxYD6#;vNd;1pK=OwiX{ zM~x$K%6+ajoCZf%(l=E0nUC**Nc^0$B?qraJLt(f6}*Spp9a}Qbc#_0zl{;fk9*;% zyC>yM9DqDmp745f{Kc&0AzI(C**pD^?(?dByy49ZYNw3>9aA9+LLVX}^+)?CBOWQ< zz_Y!oj8gY5ZLd19v}!ZQ5nyz9;3bh zlh-&la?m_T0y&|-253U*Vzg`cz@SJ^pnze7KtQ7p5RS-d5~x7(j4_M}q}T)j>`~ni zdJG8ojA}*Cnnn1d&y%HSOh?8FM^2$T#r3tc7r2Q}$C}X7J zKyH9Wn?vv(B~*Y%hl0uxhRPlt2caX-V*`E*Wx=;cay{74n6kh+S>I(JLfLJ-C$l?G zpmh8S&;_V|+}$R}iYKQyBL2!R zjP&*?fRAMcMl$aCm3!Nvo>byRm+IWQb-tH=#t>Wo)DOJ2XH3Z*^y8muWVO1K{Km#5qDAif!{xph_PXa14O z<{Z2WFSzkA98>B|T^O51zD8>WJ3MT69p{W{VAD&Hpz4wSi^gEpvi5srx;af52_+ zTj3w_S|d`054DARgU3hur8XGhL3>k=3}*13gw9xyIbOxPE3DQnJjz_3ZJiOkO$;rX zmKVk}Pzep;b=%)dES%S#OUx>f`R>y3j8Ytj&}6vr(53AB0jZT;Lxq{hiI(VzGEXLb zWFPRz-H)=O>o8=Uy+OgOK<|B$!;re`kaB3Hkgp}>`bxd0iz)JFyA2wDytTW3pT*Y> zI40v@`>&eAO}I&Y%_ppIsy1)B@lII_?uHKPE&aJTPcM?9ml63{jfK4=8b~wn7^+s6 zALbSQVXvn@LEC0aXH;=7dV4_klId?t=F@?gblxg>j1a0;n(#XpZzu7Nsnzx)_$V2BlhAj+^!6cQaNV zKAhaFH@)3MyF^o>oh}cSTD>TMdeJ)Ub0&7AM5!mm^*slxY9Cc{V|jjg^n%2Oj2(IX zmf8CoCsj=KGU8XrasyZdVtHBdYI|C=j~Mqd#|vZo$e3RpcSqc$uUxb#u?HcvB%pSo zaE)v9ZeIpWlkIWq$LMF$G6eYgKi3koZD)HX!Z<$ehD^GPy0|RzG2X%}pUgVp=tN5J zJ8iNPf@wDsFLr-(JE&4LEbl?GZk%xPCl{24K!cFio<@#hS~3p<=Zy*m&mt#7=4%lo zE6Yv@{kJ70kCl}Ol5|q}vfW-}`u7ExNM`6g@1Z`W-*3(HVHXYiMC`NDV8Rskr7GEp zA?{eGc4xi$##psbWpspbc4C_BCp`(3 zsu#NAiq*8x-9~s(ivE`c-XTt}h6Fb)!U>g~0{6m(YvFjM;+6|aK{v6tHtGjABFU%f zq#pK7Z3jW%2}N*ThCt|HXi!vq(5Auo7w%!GNT~P{NaPGw47>oQg98}!K0?FqJ3bgd zF#)3V{Rfx~X8<`sy6}Fxj;|WBnyg<8VOWn<0?=eA!5NUuL$ZK21vfwo6z1|dffCIP183-Y!_Pwr@$ea++UTpS(ak2No zJ+1-O_`DUA8;p9yF-Qik1hEJVFv-0c7z5a13`p!F3zSv}fpl~V;NKGD!qk_+0QwVW zx%>7PIh$6vzVD>gh``s9~mnT#)-gp{TfT1Aa!j!!lBRLJd??Tv=vZ;HaF z#P#iaBXybhrNp6Eef?uv8@=y7?CKd2h(xC?b!uoYPyC)@BBDp1v|Kxuj3y|{7nk9w zxs7ko{{1eo{$Z)#Qo&HG`NrD3uw^Oi15jM4XKsTs{@DBJR!d)F9tA#ZGu6>xKv0Z* z?iPFT-PO|@Eu&A`^N6V28$!I4F`HW*8zFZO0@sbrn(G@47R}(7N?aehdkNrl0-C=n zdw0c$#+n88k(~C`x)0DJpk!B!(dnqcV!r!XLEsRo*=8+u4 z=PRNJM{F2Ukfe=kYb@#*XwJIEThN7L<+m5lsmc#lFdbQ~S5Pj*tC;*a&{&ixFLHG~ z`u^4^ho84`Py|9biOs!5Do>*vuu#7IMnrXivROywYfcnA&RP#P!iG&{7a0O?F}kL8(EPz`<&UQORHd+$?Uy(ex_%6=>?ONZ!P*H>lVjJo->T`p$I`EZ)LD6b zX@TnN<3LYQ{Cxx-9$=CKtf5{;9ojM8$U=C;xZOg|G4$3$B2#0uf$f&(<&c-@N6bp{AI

+|#L2yaA0Iy3I`wO!{^Z&c`jZeR6p)!`GKu%GYd{NsR35dy(Icb0!hV@p zC(NRmnZO2dCEq^uJA&MJAIr=sk{q2V>L-eX4UE$Rg4Sb%p1X&FpIzimulqimLEivR zv|E7n6uH;)6%kKCXjF=Z`-eIwtAjm#NNYiho_4vt9ZAPe!Ew9D1;Jqgu+w&Ox@@EW zsbX7y*0Fbw`tLV)7n!%hcZ#R^v6gC{Kh}$`xR->yZ<_9a z#|`lptZJZKZ1M~~$Gc!3KBWC>=~S%JX^Wr|?-fiwXTJr@MyS~=h5OCc5|KQo}$Zky0M>E&82>`hb3z$Ol+EkdG$j z3*}=&W69NL*!idvAU*^$tlfJ)gkU*u8Q{&K^uWs%EdPp~xsSwX=4N5SP-F;L(~HE~ z%Z&^Gev==090f&#^>|^fR}KTme^&X54@$$JkV#?Ks*#nHP|niGL{0RV0<_g#H`cK2F$g0QLf;4y2*rl|hcTh~e46$Sdm3%rBl=Uz>IT6QmL zT@Y`T=MP^WV1{!BBQHCy3~y!BQXjVWeEM1x-c+x*;vwNDOly~%`|JTw9N-SXqK9Gs7lJ|Pgn^^~kUAj-;QUE>334~F(y90 zO3m${Oy;mErdzCMAMf(e=$$b*Gphc9rl6-J%2P8gc`>wu%eouA)zBrY)YMjjjv5l9Ga6jZs} zMq+8aAQl0$PK)7^?P1I+Mr&e7N-bj8YU~q0WW1}Ke}=FyVUK%*CmltbyIDz0sKgMY zKn*HC!!>6e!=G?>qL2ZGgUAtO`dXo5L`SAyw`n6Du}|Z;hTO~6*1`dg9~R#O71ed8 zrE{6g-m)_L6Iv(>_d7&R*#vVW!NWy##(3Y^I=!rMawIpDE47f~Bn|Dhr#{80%Gs1g`=r5$uyXTc_ z9eclcbI-}kbh5dN*Qz(d|KbI?k#PP)8T~dUGk=*EFgG*&2A*0A}2qkfojV=)&0TUb@>$D1cCfg@_wh&K@26X zUD@%Qy}*%YH?493ny+*)ndt`t3kSu04h=KODv}91nae-(eNCLAZGJF`#I*QS=rMoY z)BBEh<+%*)IcI|ozn$lO2=YeEh}*m_j=2$0s`DgeTj0Tkb;5D73+ z{b3jhl4y?13l0D@g(AxhP}NDL_+YTzH2Pht+i9uMAvGBc{Dm}<7`EaVq-wn!g@QN- zY(W~|K4b0X+BI^hHzQm8*y=eb+GSa|t^Gg1d6%((NOBLYn5<~&_>~0~@gSyZKeyHb z+}bgvrKG2^Q{Xo5i7Vlb>Neq`J^rOYfcdo$uRCh*wj(tfH4W})!GDpiOlD)Q?3j8R zh;rZa;L8ugmks?d z7~;d^0ev73xnJuDguD<``4P}E&4o#j0NJqN$N~i0HZZ0^^12uTq{fFbj#(&@-4#(Q z;793T9QdHcNNyWJ$T4E~e9#$$auBEoJ`fmowkyQ@yp=cx!Ip(0l|m}x3bAsQ#u5$( z!4aGG<1hxqS+d)RhL;e6Pm8J}@xYHQ+tKIY5sbJq1_7b25u$YrJd;44%YpOiNI)WE z4ZP!tyW0!vkv{BUo(7XxBunS?{BVv6AHu*oImd7tFiZt51Ge`< zYz_j9z>|v4N88XLKxbKmV1eKX@D$fx0KYv4z;bd<<~pPjNZ8nPxEMVo5_pvk zl`{{N`YMA_OCcCeN$6Law@t)Sj$td#hn{?i2S)lfVr+89#>F{_1NufY6eN=-y5a8| zAF7C3!pwGWdpTa|7bI90#11!AM#3yP1&eER6YV$<#S3{Kb?ixjE=Bd#jF>%*5%5LP zvpc(_5amxaVryUgky#Wrk4KQ-6hUQ!J{j%pL9DwO>nIa8rD3UCL7_unBCeMjZJKdW znWmM@5vDoI?|4~cJg!ssG&w}g^Dm^v`}1YiZ&oe0TyYmJ#-<~geCtx@y4f9Ol;7qz zlDS}|%9GBmfZAq0oUYpU*qnTz_G1kf!;?Gm&W6Ve(n;gEdHnTmrMFv1>kTtf5*xUJZ2xq&g7@w(ObVbA*ov{BYG3&W@wDwKGFj-| zbVcFFG)=3uD(T&ffqfl;{Fd&(13p!CMpV65<6!u*$v*^-Qcg4I5q%Z;z0x|j0{HW zix8&Uj0Ep`w*Dr+lzOa^g4wk993sZoF@2w(mC=>t5vbs$EY*3V6?CvJXXSDCRO_=( zMu8BsDBhKi`JO7H`aM75-Ba`__. +The warehouse command uses that same checkpoint URL explicitly. Both retain 123 observations, +eight actions, 120 Hz physics, and a 60 Hz policy rate. Checkpoints remain outside the repository. + +Train or compare backends +------------------------- + +.. code-block:: bash + + uv run isaaclab train --rl_library rsl_rl \ + --task IsaacContrib-Conveyor-Franka-Newton-v0 --num_envs 256 --device cuda:0 + + uv run --extra isaacsim isaaclab play --rl_library rsl_rl \ + --task IsaacContrib-Conveyor-Franka-PhysX-CPU-v0 \ + --checkpoint /path/to/model.pt --num_envs 1 --device cpu --viz kit --real-time \ + agent.device=cpu + +Native PhysX surface velocity requires CPU simulation for this task. GPU dynamics can drop +belt contacts in the supported Isaac Sim runtime. The PhysX variant rejects CUDA devices; +its separately named pretrained artifact is not published. Policy shape compatibility does +not imply identical behavior between backends. + +Warehouse sorting +----------------- + +The original manipulation straights and adjoining 90-degree bends remain fixed. Twenty-four +40 mm cartons circulate through the extended layout. Each reset shuffles a balanced batch of +six blue, six green, six orange, and six purple cartons. Blue/green belong on the positive-Y +loop; orange/purple belong on the negative-Y loop. Four policy slots are reassigned to arriving +parcels while an active grasp retains its identity. Destination classes are supervisory metadata; +the state-based checkpoint does not recognize colors from images. + +This is a presentation and policy-reuse demonstration, not a reliably solved 24-parcel benchmark. +The unchanged checkpoint can miss grasps and reset before finishing a batch. The original +four-cube training and CPU reference configurations retain their compact layout. + +.. raw:: html + + + +`Download the warehouse preview `__. + +The task's +`README `__ +describes the USD assets, collision ownership, slot adapter, and sorting metrics. diff --git a/source/isaaclab_tasks/changelog.d/maximiliank-conveyor-franka.minor.rst b/source/isaaclab_tasks/changelog.d/maximiliank-conveyor-franka.minor.rst index 583ae22b78e9..7f0a35c512b5 100644 --- a/source/isaaclab_tasks/changelog.d/maximiliank-conveyor-franka.minor.rst +++ b/source/isaaclab_tasks/changelog.d/maximiliank-conveyor-franka.minor.rst @@ -1,21 +1,18 @@ Added ^^^^^ -* Added a contributed manager-based environment with guarded, counter-rotating force-driven racetrack - conveyors, robust primitive and closed-mesh belt colliders, a MuJoCo Menagerie Franka, and an interactive - Newton-viewer cube-goal selector. -* Used the reusable surface-velocity physics interfaces while retaining a single, kitless Newton force owner with - CUDA-graph and hard-reset-safe lifecycle binding. -* Added a checkpoint-compatible Newton Play variant rendered with A09/A12 functional-loop visuals, a render-only - Thor robot table, packing station, pallet bays, and warehouse dressing while retaining the task's lightweight - collision and traction surfaces. -* Added the opt-in ``IsaacContrib-Conveyor-Franka-PhysX-CPU-v0`` reference task, which explicitly - rejects GPU dynamics because the supported native surface-velocity path can drop conveyor contacts. +* Added contributed Franka conveyor tasks with Newton GPU training, CPU-only native PhysX + playback, reusable surface-velocity interfaces, and an interactive Newton-viewer goal selector. +* Added a USD-authored warehouse Play variant with textured 40 mm cartons, gravity infeeds, + compact elevated returns, and 24 physical parcels mapped into the checkpoint's four policy slots. + Added seeded four-color batches, two destination loops, and sorting metrics. Preserved the + original manipulation geometry and 123-observation, eight-action policy interface. Complete-batch + reliability with the unchanged policy was not established. Use ``--viz kit`` for authored visuals + and the explicit base-task checkpoint URL documented in the conveyor guide. +* Added a user guide, preview, and environment-browser entries for the conveyor variants. Changed ^^^^^^^ -* Allowed :func:`isaaclab_tasks.utils.parse_env_cfg` callers to preserve a task's configured simulation device by - passing ``device=None``. -* Kept action-rate penalties finite for rejected NaN or infinite policy commands by tracking the sanitized - commands accepted by the task's action terms. +* Kept action-rate penalties finite for rejected NaN or infinite policy commands by tracking the + sanitized commands accepted by the task's action terms. diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/README.md b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/README.md index 05abc3b44238..3400cfb41407 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/README.md +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/README.md @@ -14,11 +14,11 @@ commands, rewards, reset recipes, 120 Hz physics step, and 60 Hz policy rate. ## Backend support -| Task | Physics device | Intended use | Conveyor actuation | -| --- | --- | --- | --- | -| `IsaacContrib-Conveyor-Franka-Newton-v0` | CUDA | Training and scalable playback | Batched Warp contact-force feedback captured with the Newton solver graph | -| `IsaacContrib-Conveyor-Franka-Newton-Play-v0` | CUDA | Warehouse-dressed Digital Twin playback | Same Newton force feedback on lightweight hidden collision surfaces | -| `IsaacContrib-Conveyor-Franka-PhysX-CPU-v0` | CPU only | Native-PhysX reference and checkpoint playback | Authored `PhysxSurfaceVelocityAPI` on kinematic belt sections | +| Variant | Device | Intended use | +| --- | --- | --- | +| Newton | CUDA | Train and play the four-cube task | +| Newton Play | CUDA | Play the 24-parcel USD warehouse demonstration | +| PhysX CPU | CPU only | Native surface-velocity reference and checkpoint playback | The PhysX task rejects CUDA during configuration validation. In the supported Isaac Sim runtime, enabling the native surface-velocity contact-modification path under GPU dynamics can drop the belt @@ -51,23 +51,119 @@ backend. To evaluate another policy, replace `pretrained` with an explicit check PhysX task resolves a different backend-specific artifact name, so transferring this Newton policy to PhysX currently requires the explicit local checkpoint path shown below. -For presentation, use the checkpoint-compatible Play variant. It replaces the procedural render -geometry with `ConveyorBelt_A09` straight sections and `ConveyorBelt_A12` 180-degree turns from -`Isaac/Props/Conveyors`. A Thor robot table, packing station, separate loaded and empty pallet bays, -safety markings, and a warehouse backdrop complete the scene. Every added USD is render-only with its -authored physics APIs and action graphs stripped locally. The same lightweight hidden surfaces remain -the sole owners of contact and conveyor forces, so scene dressing cannot introduce double contacts or -change the trained policy dynamics. Measured asset feet sit on the global `z=0` ground plane; the complete -policy workspace is elevated without changing its local coordinates. This asset-rich variant defaults to -one environment to keep interactive startup and rendering practical. +For presentation, use the checkpoint-compatible Play variant. The two parallel manipulation +straights and their adjoining 90-degree bends retain their original positions, widths, radii, +and 0.35 m/s surface speed. Beyond these fixed sections, two short rising feeds climb 0.10 m at less than 10 degrees and +join a shared elevated deck. Guides keep the two return lanes assigned through the upper split, +and separate descending conveyors deliver the parcels back to the original workcell approaches. +The new incline panels use slope-aligned traction and a 0.95 friction coefficient; the original +manipulation sections retain their trained 0.5 setting. + +Blue conveyor frames reach the floor, and the scanner faces along the background main belt after +a 90-degree counterclockwise rotation. The cubes wear SimReady cardboard meshes normalized to +**40 × 40 × 40 mm**, centered on their original colliders; mass remains 50 g. Actions and observation +ordering stay checkpoint-compatible. The Play variant widens its travel bounds and resets all parcels as a randomized mixed batch on the +raised supply belts. +The base training and PhysX tasks retain the original compact layout. + +The workcell contains **24 physical parcels**, exposed to the unchanged checkpoint through +four policy slots. The sorting command fills remote slots with misplaced arrivals; local assignments and an +active grasp stay pinned. Reassignment runs in the command manager, before the next policy observation, +and changes only identity mapping, never a physical pose. Commands, rewards, and placement checks +use the same mapping. All 24 parcels receive belt forces and participate in safety checks. +The arm parks when there is no active sorting transfer. The policy sees remote inventory in canonical waiting slots; +all local parcel poses and velocities, physical transport, rewards, and transfer checks remain +actual simulated states. This adapter is necessary because the checkpoint was trained on compact, +flat returns. Tensor ordering stays 123 observations to eight actions, but remote observation values +are intentionally adapted. Invalid actions still reach the original sanitization and termination path. +The checkpoint can still miss grasps and reset before a batch finishes. The larger randomized +inventory preserves the policy interface, but reliable complete-batch sorting is not established. + +The near loop has a rounded return; the far loop has a shorter squared return with rounded corners +and a shorter supply belt. Both retain the exact original manipulation geometry. Oversized yellow +drive blocks are omitted from the workcell. + +The batch contains six cartons in each of four clearly marked colors: **blue, orange, green, +and purple**. Colored paper bands wrap the textured cardboard without changing its 40 mm bounds. +Reset shuffles physical identities across 24 supply positions, independently +for each environment, using the simulation's seeded random generator. Counts remain balanced while +arrival order and initial conveyor assignments vary. Colors stay fixed throughout each batch. +The 0.043–0.052 m/s feeds release cartons through **12 cm gravity drops** onto the 0.35 m/s loops. +Blue and green belong on the positive-Y loop; orange and purple belong on the negative-Y loop. +The dispatcher requests only wrong-lane transfers and retains ownership through grasp and stable +release. Missed arrivals recirculate for another opportunity. Once the batch is sorted, the arm +parks and both loops continue running. No parcels are recolored, teleported, or replaced during sorting. + +Class assignment is explicit supervisory metadata, not a claim that the unchanged state-based +checkpoint recognizes color. `commands.transfer.parcel_colors` assigns appearances and +`commands.transfer.parcel_destinations` assigns loop IDs; all parcels of a color must share a +single destination. `commands.transfer.randomize_arrivals=False` uses the authored ordering. +`sorted_parcels` counts settled, correctly routed inventory; `batch_complete` indicates that all +24 parcels are settled on their assigned loops. + +The surrounding warehouse includes loaded rack aisles, packing shelves, pallet staging, scan and +outbound signs, safety markings, overhead beams, and warm/cool industrial lighting. Its additional +23.11 m parcel loop includes A29 elevated runs and A38 ramps, with twelve animated cartons traveling +along the main transport line. These background cartons remain render-only inventory and do not +add contacts or policy observations. The 24 small workcell parcels have real collision and +dynamics. A Y-divert dresses the outbound bay. + +Use **Kit/RTX** to see the authored MDL textures, USD lights, and background animation: ```bash -DISPLAY=:1 uv run isaaclab play --rl_library rsl_rl \ +DISPLAY=:1 uv run --extra isaacsim isaaclab play --rl_library rsl_rl \ --task IsaacContrib-Conveyor-Franka-Newton-Play-v0 \ - --checkpoint pretrained \ - --num_envs 1 --device cuda:0 --viz newton_gl --real-time + --checkpoint https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/6.1/Isaac/IsaacLab/PretrainedCheckpoints/rsl_rl/IsaacContrib-Conveyor-Franka-Newton-v0_newtonmjwarp_none_rsl_rl.pt --num_envs 1 --device cuda:0 --viz kit --real-time ``` +Kit renders the USD directly, so the Play configuration excludes visual-only meshes from the +Newton model (`sim.physics.load_visual_shapes=False`). This avoids importing warehouse dressing +into the physics model. For a static approximation in `--viz newton_gl`, explicitly pass +`env.sim.physics.load_visual_shapes=True`; materials and lighting are simplified in that viewer. +Presentation defaults to one environment. Asset references and textures are downloaded and cached +on first use, so the first launch takes longer. + +### Editing the presentation + +* `assets/warehouse.usda` owns the warehouse layout, referenced props, materials, lights, cameras, + and looping parcel transform samples. It can be opened in a USD authoring application with an + Omniverse-compatible asset resolver. `Cameras/Workcell` and `Cameras/Overview` provide two views. +* `assets/parcel_{blue,orange,green,purple}.usda` reference the carton, tint its cardboard, and + add a colored paper band within the original bounds. +* `assets/parcel.usda` normalizes the measured SimReady `cardbox_a1` visual bounds to a centered + 40 mm cube. Keep the shell bounds aligned with the task's original collider when editing it. +* `assets/conveyor_*_supported.usd` override the lower frame vertices of the referenced conveyor + modules. Upper-frame and belt geometry, source topology, and materials remain unchanged. +* `assets/conveyor_routes.usda` owns the elevated network's centerlines, frame meshes, material + references, ramp traction, feed speeds, and mixed-batch spawn positions. Physics reads these same paths after application startup; + USD is not imported during task discovery. Incline normals follow each panel's slope. All 24 + physical parcel spawn positions are authored in `conveyor:parcelSpawnPositions`. +* The Python adapter resolves remote references into the normal local asset cache, removes all + imported physics/action-graph ownership, and samples background animation using simulation time. + `conveyor_warehouse_geometry.py` sweeps lightweight collision proxies along the authored paths. + `mdp/sorting.py` extends the shared transfer command with class dispatch and batch metrics; + the action, observation, reward, and stable-placement contracts remain shared with the trained task. +* `env.conveyor_cube_pool.slot_ids` maps each environment's four policy slots to physical parcel IDs. + `transfer_counts` records placements per physical parcel; assignments never duplicate a parcel + within an environment, and resetting one environment does not change another's slots. + +### Asset and visual references + +The asset survey covered [SimReady Central](https://simready-central.nvidia.com/), +`omniverse://ov-isaac-dev.nvidia.com/Isaac/SimReady/Industrial/Warehouse`, +`Isaac/Environments/{Digital_Twin_Warehouse,Modular_Warehouse}`, `Isaac/Props/Conveyors`, and +`NVIDIA/Assets/DigitalTwin/Assets/Warehouse`. The composition references publicly accessible +Omniverse counterparts so playback does not require internal Nucleus credentials. + +Selected assets are the Omniverse A03/A09/A12/A24/A29/A38 conveyors, `RackLarge_A1`, SimReady +`bulkstoragerack_a01` and `cardbox_a1`, the Isaac packing table, and loaded pallets. NVIDIA assets +remain under their original licenses; this package contains the scene layout and lower-frame mesh overrides. +The layout draws on [Dematic's modular conveyor examples](https://www.dematic.com/content/dam/dematic/downloads/brochures/NA_BR_1039_MCS.pdf) +and [KION's warehouse installation photographs](https://www.kiongroup.com/en/News-Stories/Stories/Innovation/Warehousing-is-being-transformed.html): +parallel transport, elevation changes, rack storage, packing zones, and clear marked aisles. +The induction and recirculation layout also follows the concepts in +[Dematic’s sortation overview](https://www.dematic.com/content/dam/dematic/downloads/whitepapers/NA_WP-1015_Sorting-Out-Sortation.pdf). + Training uses the same task ID and defaults to 256 environments: ```bash diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/__init__.py index 1222979e24bd..158fffa2b25d 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/__init__.py @@ -20,10 +20,9 @@ ) gym.register( - # The conventional Play suffix lets the pretrained-checkpoint resolver - # reuse the base Newton task's published policy automatically. + # This presentation variant uses the base Newton checkpoint through an explicit URL. id="IsaacContrib-Conveyor-Franka-Newton-Play-v0", - entry_point=f"{__name__}.conveyor_franka_env:ConveyorFrankaEnv", + entry_point=f"{__name__}.conveyor_franka_warehouse_env:ConveyorFrankaWarehouseEnv", disable_env_checker=True, kwargs={ "env_cfg_entry_point": f"{__name__}.conveyor_franka_asset_env_cfg:ConveyorFrankaA09A12EnvCfg", diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/assets/conveyor_quarter_supported.usd b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/assets/conveyor_quarter_supported.usd new file mode 100644 index 000000000000..2e3ce9e849d4 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/assets/conveyor_quarter_supported.usd @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:281020b9d284f23e224babda0210629c64f663a74482a7962d1c511ba73bce8a +size 7997402 diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/assets/conveyor_routes.usda b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/assets/conveyor_routes.usda new file mode 100644 index 000000000000..c35390b0218f --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/assets/conveyor_routes.usda @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5ffa5fa3f772265e0d6e563b5ffdf3ea315602dd45ce4a2205a30477aee6e572 +size 1045928 diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/assets/conveyor_straight_supported.usd b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/assets/conveyor_straight_supported.usd new file mode 100644 index 000000000000..b07af0f62a85 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/assets/conveyor_straight_supported.usd @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e4a735b6c8cb283e4cda2950d2e4ef6db6b2e91048e3d4b4a4d5d285fa806e1a +size 22319911 diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/assets/parcel.usda b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/assets/parcel.usda new file mode 100644 index 000000000000..994effb5c4bd --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/assets/parcel.usda @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:9f8366dd61f93ade13f11d0e193ecd389c94f4e9493de0d97f9a2ff908b6156d +size 890 diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/assets/parcel_blue.usda b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/assets/parcel_blue.usda new file mode 100644 index 000000000000..ff92e414ee8b --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/assets/parcel_blue.usda @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:53894664629ad77181aac1416482a613a782e261b0dd801e36d2a7e5fedd01e2 +size 1926 diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/assets/parcel_green.usda b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/assets/parcel_green.usda new file mode 100644 index 000000000000..eb7320c43338 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/assets/parcel_green.usda @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:371036df39c3586de1166ad93753fab1bc45d9d8de8bd0b49077a29ad16287aa +size 1928 diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/assets/parcel_orange.usda b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/assets/parcel_orange.usda new file mode 100644 index 000000000000..de6de478fdf1 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/assets/parcel_orange.usda @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5076a92ba6b4141f7a9c64ecb7b2789fb2058f862f933c2b2217fc4696c8a9b8 +size 1927 diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/assets/parcel_purple.usda b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/assets/parcel_purple.usda new file mode 100644 index 000000000000..750fe3441703 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/assets/parcel_purple.usda @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8a39e3d6f918d99cf8a811c68161565c224da9ad8ab2889ebd08e7232807bd38 +size 1924 diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/assets/warehouse.usda b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/assets/warehouse.usda new file mode 100644 index 000000000000..c5a87bb58e21 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/assets/warehouse.usda @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1b2f7d68aa2aaddf17a67c7f44fe728456b4ce23ae6d388a6fa557c8657dcdb3 +size 458277 diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_cube_pool.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_cube_pool.py new file mode 100644 index 000000000000..2f241455277c --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_cube_pool.py @@ -0,0 +1,99 @@ +# 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 + +"""Stable physical parcel identities behind the checkpoint's four observation slots.""" + +from __future__ import annotations + +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import torch + +from .mdp.reset_events import CUBE_COUNT + +if TYPE_CHECKING: + from isaaclab.assets import RigidObject + from isaaclab.envs import ManagerBasedRLEnv + + +def cube_values(env: ManagerBasedRLEnv, attribute: str, *, all_cubes: bool = False) -> torch.Tensor: + """Gather a rigid-object data attribute in policy-slot or complete physical-inventory order. + + Values retain the attribute's units and world frame. The result has shape + ``(num_envs, num_slots_or_parcels, attribute_size)``. Tasks without a parcel pool + retain the original four fixed cube identities. + """ + pool = getattr(env, "conveyor_cube_pool", None) + assets = pool.assets if pool is not None else tuple(env.scene[f"cube_{i}"] for i in range(CUBE_COUNT)) + values = torch.stack([getattr(asset.data, attribute).torch for asset in assets], dim=1) + if pool is not None and not all_cubes: + values = values.gather(1, pool.slot_ids[..., None].expand(-1, -1, values.shape[-1])) + return values + + +class ConveyorCubePool: + """Bind four policy slots to a larger pool without moving or copying physical bodies. + + Assignments are independent per environment. Local parcels keep their slots; + the active grasp is additionally pinned even if it moves outside the workcell. + """ + + def __init__(self, assets: tuple[RigidObject, ...], num_envs: int, device: str) -> None: + if len(assets) < CUBE_COUNT: + raise ValueError("The conveyor policy requires at least four physical parcels.") + self.assets = assets + self.slot_ids = torch.arange(CUBE_COUNT, device=device).repeat(num_envs, 1) + self.assignment_counts = torch.zeros((num_envs, len(assets)), device=device, dtype=torch.long) + self.assignment_counts[:, :CUBE_COUNT] = 1 + self.transfer_counts = torch.zeros_like(self.assignment_counts) + + def reset(self, env_ids: Sequence[int] | torch.Tensor) -> None: + """Restore reset-recipe slot identities for selected environments.""" + self.slot_ids[env_ids] = torch.arange(CUBE_COUNT, device=self.slot_ids.device) + + def refresh( + self, + positions: torch.Tensor, + local: torch.Tensor, + candidates: torch.Tensor, + target_slots: torch.Tensor, + pinned: torch.Tensor, + ) -> torch.Tensor: + """Assign arriving parcels to remote slots, preserving all local and pinned identities. + + Args: + positions: Physical parcel positions in the workspace [m], shape ``(N, P, 3)``. + local: Parcels still in the manipulation region, shape ``(N, P)``. + candidates: Parcels eligible for pickup, shape ``(N, P)``. + target_slots: Current command's policy slot, shape ``(N,)``. + pinned: Whether the active parcel must retain its slot, shape ``(N,)``. + + Returns: + Environments whose slot assignments changed, shape ``(N,)``. + """ + mapped = torch.zeros_like(candidates).scatter_(1, self.slot_ids, True) + available = candidates & ~mapped + reusable = ~local.gather(1, self.slot_ids) + reusable.scatter_(1, target_slots[:, None], ~pinned[:, None] & reusable.gather(1, target_slots[:, None])) + changed = torch.zeros_like(pinned) + # Prefer parcels assigned less often, then the arrival with more belt travel remaining. + priority = positions[..., 0] - 10.0 * self.assignment_counts + for slot in range(CUBE_COUNT): + eligible = reusable[:, slot] & available.any(dim=1) + rows = eligible.nonzero(as_tuple=False).flatten() + if not rows.numel(): + continue + choices = torch.where(available, priority, -torch.inf).argmax(dim=1)[rows] + self.slot_ids[rows, slot] = choices + self.assignment_counts[rows, choices] += 1 + available[rows, choices] = False + changed[rows] = True + return changed + + def record_transfers(self, env_ids: torch.Tensor, target_slots: torch.Tensor) -> None: + """Credit stable placements to physical parcels before the command selects its next slot.""" + physical_ids = self.slot_ids[env_ids, target_slots] + self.transfer_counts[env_ids, physical_ids] += 1 diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_asset_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_asset_env_cfg.py index f734b796ab88..65669ebe0c56 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_asset_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_asset_env_cfg.py @@ -8,20 +8,27 @@ from __future__ import annotations from collections.abc import Callable +from functools import cache +from pathlib import Path from typing import TYPE_CHECKING import isaaclab.sim as sim_utils from isaaclab.assets import AssetBaseCfg +from isaaclab.physics import SurfaceVelocitySpec from isaaclab.terrains import TerrainImporterCfg -from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, retrieve_file_path from isaaclab.utils.configclass import configclass if TYPE_CHECKING: + from pxr import Sdf, Usd + from isaaclab.terrains import TerrainImporter from .conveyor_franka_env_cfg import ( ConveyorFrankaEnvCfg, ConveyorFrankaSceneCfg, + _hidden_collision_geometry, + _hidden_collision_mesh, _spawn_shape_with_display_color, ) from .conveyor_geometry import ( @@ -31,29 +38,21 @@ BELT_TOP_Z, BELT_TURN_RADIUS, ) +from .conveyor_warehouse_geometry import warehouse_belt_sections, warehouse_guard_meshes -_CONVEYOR_ASSET_DIR = f"{ISAAC_NUCLEUS_DIR}/Props/Conveyors" -_A09_ASSET_PATH = f"{_CONVEYOR_ASSET_DIR}/ConveyorBelt_A09.usd" -_A12_ASSET_PATH = f"{_CONVEYOR_ASSET_DIR}/ConveyorBelt_A12.usd" _THOR_TABLE_ASSET_PATH = f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/thor_table.usd" -_PACKING_TABLE_ASSET_PATH = f"{ISAAC_NUCLEUS_DIR}/Props/PackingTable/packing_table.usd" -_PALLET_ASSET_PATH = f"{ISAAC_NUCLEUS_DIR}/Props/Pallet/pallet.usd" -_LOADED_PALLET_ASSET_PATH = f"{ISAAC_NUCLEUS_DIR}/Props/Pallet/o3dyn_pallet.usd" # The A12 endpoints are 2.9922 m apart, its belt crown is 1.78053 m above the -# asset origin, and the lowest rendered point of both A09 and A12 is authored at -# z=0. Scale from those measured bounds so the asset feet sit on the global -# ground while the visual surface follows the existing task colliders. +# asset origin. USD point overrides extend the legs to the floor while keeping +# the upper frames unchanged; the policy workspace follows the deck elevation. _A12_ENDPOINT_SEPARATION = 2.9922 _ASSET_BELT_TOP_Z = 1.78053 -_ASSET_LOWEST_Z = 0.0 _ASSET_XY_SCALE = 2.0 * BELT_TURN_RADIUS / _A12_ENDPOINT_SEPARATION -_GROUND_PLANE_Z = 0.0 -# Preserve the assets' lateral/vertical proportions instead of stretching the -# supports to the original table height. The scaled belt crown then determines +_CONVEYOR_SUPPORT_Z = 0.55 +# Preserve the upper frames' proportions. The scaled belt crown determines # how far to elevate the policy workspace. _ASSET_Z_SCALE = _ASSET_XY_SCALE -_ASSET_ROOT_Z = _GROUND_PLANE_Z - _ASSET_LOWEST_Z * _ASSET_Z_SCALE +_ASSET_ROOT_Z = _CONVEYOR_SUPPORT_Z _ASSET_BELT_WORLD_Z = _ASSET_ROOT_Z + _ASSET_BELT_TOP_Z * _ASSET_Z_SCALE _WORKSPACE_ELEVATION = _ASSET_BELT_WORLD_Z - BELT_TOP_Z @@ -68,12 +67,80 @@ _THOR_TABLE_LOWEST_Z = -0.795 _THOR_TABLE_SCALE = _WORKSPACE_ELEVATION / -_THOR_TABLE_LOWEST_Z -_BACKDROP_COLOR = (0.075, 0.09, 0.12) -_BACKDROP_ACCENT_COLOR = (0.16, 0.20, 0.25) -_SAFETY_YELLOW = (0.95, 0.58, 0.055) _PHYSICS_SCHEMA_PREFIXES = ("Physics", "Physx", "Newton", "Mujoco") _PHYSICS_SCHEMA_NAMES = frozenset(("IsaacConveyorAPI",)) +_PRESENTATION_ASSETS = Path(__file__).parent / "assets" + + +@cache +def _presentation_layer(usd_path: str) -> Sdf.Layer: + """Resolve a local composition's remote assets without modifying its source layer.""" + from pxr import Sdf, UsdUtils + + source = Sdf.Layer.FindOrOpen(usd_path) + layer = Sdf.Layer.CreateAnonymous(Path(usd_path).name) + layer.TransferContent(source) + resolved = {} + for path in source.GetExternalReferences(): + if path.startswith("https://"): + resolved[path] = retrieve_file_path(path) + else: + local_path = Sdf.ComputeAssetPathRelativeToLayer(source, path) + resolved[path] = _presentation_layer(local_path).identifier + UsdUtils.ModifyAssetPaths( + layer, + lambda path: resolved[path] if path in resolved else Sdf.ComputeAssetPathRelativeToLayer(source, path), + ) + return layer + + +@sim_utils.clone +def _spawn_authored_visual( + prim_path: str, + cfg: sim_utils.UsdFileCfg, + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, + **kwargs, +) -> Usd.Prim: + """Compose USD scenery with cached dependencies and no simulation ownership.""" + layer = _presentation_layer(cfg.usd_path) + prim = sim_utils.create_prim( + prim_path, + translation=translation, + orientation=orientation, + scale=cfg.scale, + ) + prim.GetReferences().AddReference(layer.identifier) + sim_utils.make_uninstanceable(prim_path) + _make_usd_subtree_visual_only(prim) + return prim + + +@sim_utils.clone +def _spawn_carton_cube( + prim_path: str, + cfg: _ParcelCuboidCfg, + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, + **kwargs, +) -> Usd.Prim: + """Dress the original 40 mm collider with a centered, equally sized SimReady carton.""" + from pxr import UsdGeom + + prim = _spawn_shape_with_display_color(prim_path, cfg, translation, orientation, **kwargs) + UsdGeom.Imageable(prim.GetStage().GetPrimAtPath(f"{prim_path}/geometry/mesh")).MakeInvisible() + visual_cfg = sim_utils.UsdFileCfg(usd_path=cfg.parcel_usd_path) + _spawn_authored_visual(f"{prim_path}/CartonVisual", visual_cfg) + return prim + + +@configclass +class _ParcelCuboidCfg(sim_utils.CuboidCfg): + """Original task collider with a separately authored carton appearance.""" + + parcel_usd_path: str = str(_PRESENTATION_ASSETS / "parcel.usda") + """USD visual, normalized to the task's 40 mm cube.""" def _is_physics_schema(schema_name: str) -> bool: @@ -186,37 +253,12 @@ def _visual_usd_asset( ) -def _visual_cuboid( - prim_path: str, - size: tuple[float, float, float], - position: tuple[float, float, float], - color: tuple[float, float, float], - roughness: float = 0.72, - metallic: float = 0.0, -) -> AssetBaseCfg: - """Build one non-colliding scene-dressing cuboid.""" - spawn = sim_utils.CuboidCfg( - func=_spawn_shape_with_display_color, - size=size, - visual_material=sim_utils.PreviewSurfaceCfg( - diffuse_color=color, - roughness=roughness, - metallic=metallic, - ), - ) - return AssetBaseCfg( - prim_path=prim_path, - init_state=AssetBaseCfg.InitialStateCfg(pos=position), - spawn=spawn, - ) - - @configclass class ConveyorFrankaA09A12SceneCfg(ConveyorFrankaSceneCfg): """Checkpoint-compatible Digital Twin scene with visual warehouse dressing.""" def __post_init__(self) -> None: - """Replace procedural visuals while retaining the validated physics proxies.""" + """Dress the workcell and extend its returns around the fixed manipulation sections.""" super().__post_init__() # Raise every inherited env-scoped task component as one rigid @@ -236,12 +278,10 @@ def __post_init__(self) -> None: visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.055, 0.065, 0.082), roughness=0.82), ) self.dome_light.spawn.color = (0.70, 0.78, 0.92) - self.dome_light.spawn.intensity = 1850.0 + self.dome_light.spawn.intensity = 700.0 - left_x = BELT_CENTER_X - BELT_HALF_STRAIGHT right_x = BELT_CENTER_X + BELT_HALF_STRAIGHT straight_scale = (_A09_X_SCALE, _ASSET_XY_SCALE, _ASSET_Z_SCALE) - turn_scale = (_ASSET_XY_SCALE, _ASSET_XY_SCALE, _ASSET_Z_SCALE) # The Franka is fixed at the elevated workspace origin and does not need # support collision. Replace the temporary plinth with the purpose-built @@ -265,147 +305,93 @@ def __post_init__(self) -> None: setattr(self, f"guard_{side_key}_inner_visual", None) setattr(self, f"guard_{side_key}_outer_visual", None) - for run, y_position in ( - ("top", center_y + BELT_TURN_RADIUS), - ("bottom", center_y - BELT_TURN_RADIUS), - ): - setattr( - self, - f"conveyor_{side_key}_{run}_a09_visual", - _visual_usd_asset( - prim_path=f"{{ENV_REGEX_NS}}/Conveyor{side}{run.title()}A09Visual", - usd_path=_A09_ASSET_PATH, - position=(right_x, y_position, _ASSET_ROOT_Z), - scale=straight_scale, - ), - ) - - # A12 starts at one end of its diameter and bends toward local +X. - # The right piece uses its authored orientation. Rotating the left - # piece by 180 degrees produces the opposite semicircle without a - # negative scale or mirrored geometry. + run = "bottom" if side == "Left" else "top" + y_position = center_y + (-BELT_TURN_RADIUS if side == "Left" else BELT_TURN_RADIUS) setattr( self, - f"conveyor_{side_key}_right_a12_visual", + f"conveyor_{side_key}_{run}_a09_visual", _visual_usd_asset( - prim_path=f"{{ENV_REGEX_NS}}/Conveyor{side}RightA12Visual", - usd_path=_A12_ASSET_PATH, - position=(right_x, center_y + BELT_TURN_RADIUS, _ASSET_ROOT_Z), - scale=turn_scale, - ), - ) - setattr( - self, - f"conveyor_{side_key}_left_a12_visual", - _visual_usd_asset( - prim_path=f"{{ENV_REGEX_NS}}/Conveyor{side}LeftA12Visual", - usd_path=_A12_ASSET_PATH, - position=(left_x, center_y - BELT_TURN_RADIUS, _ASSET_ROOT_Z), - scale=turn_scale, - rotation=(0.0, 0.0, 1.0, 0.0), + prim_path=f"{{ENV_REGEX_NS}}/Conveyor{side}{run.title()}A09Visual", + usd_path=str(_PRESENTATION_ASSETS / "conveyor_straight_supported.usd"), + position=(right_x, y_position, _ASSET_ROOT_Z), + scale=straight_scale, ), ) + getattr(self, f"conveyor_{side_key}_{run}_a09_visual").spawn.func = _spawn_authored_visual - # Warehouse props provide scale and context but are intentionally - # presentation-only. Their placement stays behind the robot and outside - # the manipulation workspace. - self.packing_station_visual = _visual_usd_asset( - prim_path="{ENV_REGEX_NS}/PackingStationVisual", - usd_path=_PACKING_TABLE_ASSET_PATH, - position=(-1.05, -1.42, _GROUND_PLANE_Z), - scale=(0.42, 0.42, 0.42), - rotation=(0.0, 0.0, 0.70710678, 0.70710678), - ) - self.loaded_pallet_visual = _visual_usd_asset( - prim_path="{ENV_REGEX_NS}/LoadedPalletVisual", - usd_path=_LOADED_PALLET_ASSET_PATH, - position=(-1.08, 1.22, _GROUND_PLANE_Z), - scale=(0.56, 0.56, 0.56), - rotation=(0.0, 0.0, -0.25881905, 0.96592583), - ) - self.empty_pallet_visual = _visual_usd_asset( - prim_path="{ENV_REGEX_NS}/EmptyPalletVisual", - usd_path=_PALLET_ASSET_PATH, - position=(0.00, 1.72, _GROUND_PLANE_Z), - scale=(0.58, 0.58, 0.58), - rotation=(0.0, 0.0, 0.13052619, 0.99144486), + self.warehouse_visual = _visual_usd_asset( + prim_path="{ENV_REGEX_NS}/WarehouseVisual", + usd_path=str(_PRESENTATION_ASSETS / "warehouse.usda"), + position=(0.0, 0.0, 0.0), + scale=(1.0, 1.0, 1.0), ) + self.warehouse_visual.spawn.func = _spawn_authored_visual + for cube_id in range(4): + cube = getattr(self, f"cube_{cube_id}") + cube.spawn = _ParcelCuboidCfg(**vars(cube.spawn)) + cube.spawn.func = _spawn_carton_cube + + def _configure_route_assets( + self, parcel_colors: tuple[str, ...] = ("blue", "orange", "green", "purple") * 6 + ) -> None: + """Read USD route geometry after the application selects its USD runtime.""" + from .conveyor_warehouse_geometry import warehouse_parcel_positions + + positions = warehouse_parcel_positions() + if len(parcel_colors) != len(positions) or not set(parcel_colors) <= {"blue", "orange", "green", "purple"}: + raise ValueError("Each authored parcel requires a color: blue, orange, green, or purple.") + for cube_id, (position, color) in enumerate(zip(positions, parcel_colors, strict=True)): + cube = self.cube_0.copy() + cube.spawn.parcel_usd_path = str(_PRESENTATION_ASSETS / f"parcel_{color}.usda") + cube.prim_path = f"{{ENV_REGEX_NS}}/Cube{cube_id}" + cube.init_state.pos = (position[0], position[1], position[2] + _WORKSPACE_ELEVATION) + setattr(self, f"cube_{cube_id}", cube) + for side in ("Left", "Right"): + for key in ("top_straight", "bottom_straight", "right_turn", "left_turn"): + setattr(self, f"conveyor_{side.lower()}_{key}_collision", None) + for index, section in enumerate(warehouse_belt_sections(side)): + asset = _hidden_collision_geometry(section.belt.prim_path, section.geometry, 1.1e-5, 1) + x, y, z = asset.init_state.pos + asset.init_state.pos = (x, y, z + _WORKSPACE_ELEVATION) + setattr(self, f"warehouse_{side.lower()}_section_{index}", asset) + for boundary in ("inner", "outer"): + setattr(self, f"guard_{side.lower()}_{boundary}_collision", None) + for guard in warehouse_guard_meshes(side): + asset = _hidden_collision_mesh(f"{{ENV_REGEX_NS}}/{guard.name}Collision", guard, 1.1e-5, 1) + asset.init_state.pos = (0.0, 0.0, _WORKSPACE_ELEVATION) + setattr(self, f"warehouse_{guard.name}_collision", asset) - # A low-detail wall and safety-zone markings frame the high-detail USD - # assets without importing a full warehouse stage or adding collision. - self.warehouse_back_wall_visual = _visual_cuboid( - prim_path="{ENV_REGEX_NS}/WarehouseBackWallVisual", - size=(0.06, 4.4, 2.0), - position=(-1.72, 0.0, 1.0), - color=_BACKDROP_COLOR, - roughness=0.82, - ) - self.warehouse_side_wall_visual = _visual_cuboid( - prim_path="{ENV_REGEX_NS}/WarehouseSideWallVisual", - size=(5.3, 0.06, 2.0), - position=(0.93, 2.18, 1.0), - color=_BACKDROP_COLOR, - roughness=0.82, - ) - for index, y_position in enumerate((-1.75, -0.58, 0.58, 1.75)): - setattr( - self, - f"warehouse_wall_column_{index}_visual", - _visual_cuboid( - prim_path=f"{{ENV_REGEX_NS}}/WarehouseWallColumn{index}Visual", - size=(0.09, 0.08, 2.08), - position=(-1.66, y_position, 1.04), - color=_BACKDROP_ACCENT_COLOR, - roughness=0.55, - metallic=0.35, - ), - ) - for index, x_position in enumerate((-1.62, -0.48, 0.66, 1.80, 2.94)): - setattr( - self, - f"warehouse_side_column_{index}_visual", - _visual_cuboid( - prim_path=f"{{ENV_REGEX_NS}}/WarehouseSideColumn{index}Visual", - size=(0.08, 0.09, 2.08), - position=(x_position, 2.12, 1.04), - color=_BACKDROP_ACCENT_COLOR, - roughness=0.55, - metallic=0.35, - ), - ) - for index, (size, position) in enumerate( - ( - ((1.85, 0.025, 0.004), (0.48, 1.02, 0.002)), - ((1.85, 0.025, 0.004), (0.48, -1.02, 0.002)), - ((0.025, 2.065, 0.004), (-0.445, 0.0, 0.002)), - ((0.025, 2.065, 0.004), (1.405, 0.0, 0.002)), - ) - ): - setattr( - self, - f"safety_zone_{index}_visual", - _visual_cuboid( - prim_path=f"{{ENV_REGEX_NS}}/SafetyZone{index}Visual", - size=size, - position=position, - color=_SAFETY_YELLOW, - roughness=0.68, - ), - ) + def build_conveyor_belt_specs(self, **kwargs: float | bool) -> tuple[SurfaceVelocitySpec, ...]: + """Describe the linked warehouse surfaces to the existing Newton conveyor driver.""" + return tuple(section.belt for side in ("Left", "Right") for section in warehouse_belt_sections(side, **kwargs)) @configclass class ConveyorFrankaA09A12EnvCfg(ConveyorFrankaEnvCfg): - """Newton presentation variant with Digital Twin visuals and unchanged task physics.""" + """Newton presentation variant with Digital Twin visuals and extended return routes.""" scene: ConveyorFrankaA09A12SceneCfg = ConveyorFrankaA09A12SceneCfg( num_envs=1, - env_spacing=6.0, + env_spacing=24.0, replicate_physics=True, ) def __post_init__(self) -> None: - """Frame the complete presentation scene while retaining all task settings.""" + """Frame the presentation scene and allow packages to travel along its extended returns.""" super().__post_init__() - self.sim.default_visualizer_cfg.eye = (4.10, -3.65, 2.35) - self.sim.default_visualizer_cfg.lookat = (0.80, 0.0, 0.38) + from .mdp.sorting import ConveyorSortCommandCfg + + self.commands.transfer = ConveyorSortCommandCfg() + # Kit renders the authored USD directly; Newton needs only the physical geometry. + self.sim.physics.load_visual_shapes = False + self.conveyor_force.transported_body_count_per_env = len(self.commands.transfer.parcel_destinations) + self.sim.physics.solver_cfg.nconmax = 400 + self.sim.physics.solver_cfg.njmax = 600 + self.conveyor_force.transported_body_pattern = r"(?:^|/)Cube_?[0-9]+(?:/|$)" + self.terminations.cube_out_of_workspace.params = { + "minimum": (-0.4, -1.2, -0.05), + "maximum": (2.85, 1.2, 0.8), + } + self.sim.default_visualizer_cfg.eye = (4.8, -5.2, 3.0) + self.sim.default_visualizer_cfg.lookat = (0.9, 0.6, 0.95) + self.sim.default_visualizer_cfg.focal_length = 24.0 diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_asset_terrain.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_asset_terrain.py index da6a01614893..645b58511d27 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_asset_terrain.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_asset_terrain.py @@ -22,3 +22,9 @@ def __init__(self, cfg: _ElevatedGroundPlaneCfg): """Create the ground plane and translate its policy-facing origins.""" super().__init__(cfg) self.env_origins.add_(self.env_origins.new_tensor(cfg.workspace_origin_offset)) + # The authored warehouse floor replaces the default calibration-grid visual. + from pxr import UsdGeom + + from isaaclab.sim.utils.stage import get_current_stage + + UsdGeom.Imageable(get_current_stage().GetPrimAtPath(cfg.prim_path)).MakeInvisible() diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_warehouse_env.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_warehouse_env.py new file mode 100644 index 000000000000..85ac794aab28 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_franka_warehouse_env.py @@ -0,0 +1,151 @@ +# 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 + +"""Four-slot policy playback over a physical parcel pool and USD-authored warehouse.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import torch + +from isaaclab.envs.common import VecEnvStepReturn + +from .conveyor_cube_pool import ConveyorCubePool +from .conveyor_franka_env import ConveyorFrankaEnv + +if TYPE_CHECKING: + from pxr import Usd + + from .conveyor_franka_asset_env_cfg import ConveyorFrankaA09A12EnvCfg + + +class ConveyorFrankaWarehouseEnv(ConveyorFrankaEnv): + """Play the four-cube checkpoint over an individually tracked workcell parcel pool. + + Kit renders the authored USD materials, lights, and animation. Lightweight + Newton viewers show a static approximation of the warehouse dressing. + """ + + def __init__(self, cfg: ConveyorFrankaA09A12EnvCfg, render_mode: str | None = None, **kwargs): + self._warehouse_animation: list[tuple[Usd.Attribute, Usd.Attribute]] = [] + self._warehouse_arm_joint_ids: list[int] | None = None + cfg.scene._configure_route_assets(cfg.commands.transfer.parcel_colors) + super().__init__(cfg, render_mode=render_mode, **kwargs) + if not any(viz.cfg.visualizer_type == "kit" for viz in self.sim.visualizers): + return + self.sim.set_setting("/app/viewport/grid/enabled", False) + + from pxr import Usd + + from .conveyor_franka_asset_env_cfg import _presentation_layer + + camera = self.sim.stage.GetPrimAtPath("/OmniverseKit_Persp") + if camera: + # Kit owns this camera in its session layer; weaker root-layer edits are ignored. + viewer = cfg.sim.default_visualizer_cfg + with Usd.EditContext(self.sim.stage, self.sim.stage.GetSessionLayer()): + self.sim.set_camera_view(viewer.eye, viewer.lookat) + camera.GetAttribute("focalLength").Set(viewer.focal_length) + self._warehouse_source = Usd.Stage.Open(_presentation_layer(cfg.scene.warehouse_visual.spawn.usd_path)) + self._warehouse_period = self._warehouse_source.GetEndTimeCode() + self._warehouse_fps = self._warehouse_source.GetTimeCodesPerSecond() + for env_path in self.scene.env_prim_paths: + group = self._warehouse_source.GetPrimAtPath("/Warehouse/Parcels") + for source in group.GetChildren(): + target = self.sim.stage.GetPrimAtPath(f"{env_path}/WarehouseVisual/Parcels/{source.GetName()}") + for name in ("xformOp:translate", "xformOp:rotateXYZ"): + self._warehouse_animation.append((source.GetAttribute(name), target.GetAttribute(name))) + self.sim.add_render_callback("conveyor_warehouse_animation", self._animate_warehouse) + + def load_managers(self) -> None: + """Create slot identities before command, reward, and observation terms inspect cubes.""" + assets = tuple(self.scene[f"cube_{i}"] for i in range(self.cfg.conveyor_force.transported_body_count_per_env)) + self.conveyor_cube_pool = ConveyorCubePool(assets, self.num_envs, self.device) + super().load_managers() + + @staticmethod + def _in_workcell(positions: torch.Tensor) -> torch.Tensor: + """Identify parcels within the trained controller's local manipulation region [m].""" + return ( + (positions[..., 0] > -0.25) + & (positions[..., 0] < 1.5) + & (positions[..., 1].abs() < 0.6) + & (positions[..., 2] < 0.4) + # The low merge passes beside the placement bend but is still remote transport. + & ((positions[..., 2] < 0.12) | (positions[..., 0] < 1.05)) + ) + + def _adapt_policy_cube_state(self, positions, quaternions, velocities): + """Represent remote inventory as waiting slots while keeping local manipulation states exact.""" + origins = self.scene.env_origins[:, None, :] + local = positions - origins + remote = ~self._in_workcell(local) + waiting = local.clone() + waiting[..., 0] = 0.14 + 0.88 * torch.arange(1, 5, device=self.device)[None, :] / 5 + waiting[..., 1] = torch.where(local[..., 1] >= 0, 0.75, -0.75) + waiting[..., 2] = 0.06 + upright = torch.zeros_like(quaternions) + upright[..., 3] = 1.0 + transport_velocity = torch.zeros_like(velocities) + transport_velocity[..., 0] = self.cfg.conveyor_force.speed + return ( + torch.where(remote[..., None], waiting + origins, positions), + torch.where(remote[..., None], upright, quaternions), + torch.where(remote[..., None], transport_velocity, velocities), + ) + + def step(self, action: torch.Tensor) -> VecEnvStepReturn: + """Park while waiting for a misplaced parcel; dispatch runs through the command manager.""" + command = self.command_manager.get_term("transfer") + robot = self.scene["robot"] + if self._warehouse_arm_joint_ids is None: + self._warehouse_arm_joint_ids = robot.find_joints( + self.cfg.actions.arm_action.joint_names, preserve_order=True + )[0] + joint_ids = self._warehouse_arm_joint_ids + parked = torch.zeros_like(action) + parked[:, :7] = ( + (robot.data.default_joint_pos.torch[:, joint_ids] - robot.data.joint_pos.torch[:, joint_ids]) + / self.cfg.actions.arm_action.scale + ).clamp(-0.25, 0.25) + # Keep invalid actions visible to the existing sanitization and termination terms. + use_policy = command.has_target | ~torch.isfinite(action).all(dim=1) + return super().step(torch.where(use_policy[:, None], action, parked)) + + def _animate_warehouse(self, _event) -> None: + """Sample USD motion using policy time, independently of render frame rate.""" + from pxr import Sdf + + time_code = (self.common_step_counter * self.step_dt * self._warehouse_fps) % self._warehouse_period + with Sdf.ChangeBlock(): + for source, target in self._warehouse_animation: + target.Set(source.Get(time_code)) + + def _reset_idx(self, env_ids) -> None: + """Shuffle a mixed batch across the authored feeds in selected environments.""" + from .conveyor_warehouse_geometry import warehouse_parcel_positions + + pool = self.conveyor_cube_pool + pool.reset(env_ids) + super()._reset_idx(env_ids) + positions = torch.tensor(warehouse_parcel_positions(), device=self.device) + if self.cfg.commands.transfer.randomize_arrivals: + assignments = torch.rand((len(env_ids), len(pool.assets)), device=self.device).argsort(dim=1) + else: + assignments = torch.arange(len(pool.assets), device=self.device).expand(len(env_ids), -1) + for cube_id, cube in enumerate(pool.assets): + pose = cube.data.root_pose_w.torch[env_ids].clone() + pose[:, :3] = positions[assignments[:, cube_id]] + self.scene.env_origins[env_ids] + pose[:, 3:] = pose.new_tensor((0.0, 0.0, 0.0, 1.0)) + cube.write_root_pose_to_sim_index(root_pose=pose, env_ids=env_ids) + cube.write_root_velocity_to_sim_index(root_velocity=pose.new_zeros((len(env_ids), 6)), env_ids=env_ids) + + def close(self) -> None: + """Remove the presentation callback before releasing the shared task resources.""" + if getattr(self, "sim", None) is not None: + self.sim.remove_render_callback("conveyor_warehouse_animation") + self._warehouse_animation.clear() + super().close() diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_geometry.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_geometry.py index 3a3159c4c3e2..b218cde91949 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_geometry.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_geometry.py @@ -229,15 +229,18 @@ def _straight_collision_cuboid(name: str, center_y: float) -> CuboidSpec: ) -def _turn_collision_mesh(name: str, pivot_x: float, center_y: float, start_angle: float) -> MeshSpec: - """Build one closed annular half-turn prism with a +Z top surface.""" +def _turn_collision_mesh( + name: str, pivot_x: float, center_y: float, start_angle: float, angle_span: float = math.pi +) -> MeshSpec: + """Build a closed annular turn with a +Z top surface and an angular span [rad].""" half_width = 0.5 * BELT_WIDTH + BELT_COLLISION_OVERHANG inner_radius = BELT_TURN_RADIUS - half_width outer_radius = BELT_TURN_RADIUS + half_width angle_overlap = BELT_COLLISION_SEAM_OVERLAP / BELT_TURN_RADIUS angle_start = start_angle - angle_overlap - angle_step = (math.pi + 2.0 * angle_overlap) / TURN_SEGMENT_COUNT - angles = tuple(angle_start + index * angle_step for index in range(TURN_SEGMENT_COUNT + 1)) + segment_count = round(TURN_SEGMENT_COUNT * angle_span / math.pi) + angle_step = (angle_span + 2.0 * angle_overlap) / segment_count + angles = tuple(angle_start + index * angle_step for index in range(segment_count + 1)) inner_top = tuple( (pivot_x + inner_radius * math.cos(angle), center_y + inner_radius * math.sin(angle), BELT_TOP_Z) @@ -256,7 +259,7 @@ def _turn_collision_mesh(name: str, pivot_x: float, center_y: float, start_angle inner_bottom_offset = 2 * count outer_bottom_offset = 3 * count faces: list[tuple[int, int, int]] = [] - for index in range(TURN_SEGMENT_COUNT): + for index in range(segment_count): next_index = index + 1 inner_top_i = index inner_top_j = next_index @@ -281,7 +284,7 @@ def _turn_collision_mesh(name: str, pivot_x: float, center_y: float, start_angle ) # Close both radial ends of the annular prism. - end = TURN_SEGMENT_COUNT + end = segment_count faces.extend( ( (0, inner_bottom_offset, outer_bottom_offset), diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_warehouse_geometry.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_warehouse_geometry.py new file mode 100644 index 000000000000..17e42d8af016 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/conveyor_warehouse_geometry.py @@ -0,0 +1,226 @@ +# 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 + +"""Physical surfaces for the USD-authored elevated conveyor network.""" + +from __future__ import annotations + +import math +from dataclasses import dataclass, replace +from functools import lru_cache +from pathlib import Path + +import numpy as np + +from isaaclab.physics import SurfaceVelocitySpec + +from .conveyor_geometry import ( + BELT_CENTER_X, + BELT_CENTER_Y, + BELT_COLLISION_OVERHANG, + BELT_COLLISION_SEAM_OVERLAP, + BELT_HALF_STRAIGHT, + BELT_THICKNESS, + GUARD_BASE_OVERLAP, + GUARD_HEIGHT, + GUARD_THICKNESS, + ConveyorSectionSpec, + MeshSpec, + _turn_collision_mesh, + belt_collision_section_specs, + belt_direction, +) + +_ROUTE_ASSET = Path(__file__).parent / "assets" / "conveyor_routes.usda" + + +@lru_cache(maxsize=1) +def _route_layer(): + # Import Usd to register file formats in kitless mode, without composing asset dependencies. + from pxr import Sdf, Usd # noqa: F401 + + return Sdf.Layer.FindOrOpen(str(_ROUTE_ASSET)) + + +@dataclass(frozen=True) +class _RouteSegment: + name: str + points: tuple[tuple[float, float, float], ...] + direction: tuple[float, float, float] + width: float + pivot: tuple[float, float, float] | None + radius: float | None + friction: float | None + velocity: float | None + path: str + + +@lru_cache(maxsize=2) +def _route_segments(side: str) -> tuple[_RouteSegment, ...]: + """Read the same centerlines that author the visible conveyor modules.""" + from pxr import Sdf + + belt_direction(side) + layer = _route_layer() + root = layer.GetPrimAtPath(Sdf.Path(f"/ConveyorRoutes/{side}")) + segments = [] + for name in root.attributes["conveyor:order"].default: + prim = layer.GetPrimAtPath(root.path.AppendChild(name)) + points = layer.GetPrimAtPath(prim.path.AppendChild("Centerline")).attributes["points"].default + curved = "conveyor:pivot" in prim.attributes + segments.append( + _RouteSegment( + name=name, + velocity=prim.attributes["conveyor:velocity"].default + if "conveyor:velocity" in prim.attributes + else None, + path=prim.attributes["conveyor:path"].default if "conveyor:path" in prim.attributes else "main", + points=tuple(tuple(point) for point in points), + direction=tuple(prim.attributes["conveyor:direction"].default), + width=prim.attributes["conveyor:width"].default, + pivot=tuple(prim.attributes["conveyor:pivot"].default) if curved else None, + radius=prim.attributes["conveyor:radius"].default if curved else None, + friction=prim.attributes["conveyor:friction"].default + if "conveyor:friction" in prim.attributes + else None, + ) + ) + return tuple(segments) + + +def warehouse_parcel_positions() -> tuple[tuple[float, float, float], ...]: + """Return physical parcel infeed spawn positions in workspace coordinates [m].""" + root = _route_layer().GetPrimAtPath("/ConveyorRoutes") + return tuple(tuple(position) for position in root.attributes["conveyor:parcelSpawnPositions"].default) + + +def _normals(segment: _RouteSegment) -> np.ndarray: + if segment.pivot is None: + tangent = np.asarray(segment.direction) + normal = np.array((-tangent[1], tangent[0], 0.0)) + return np.tile(normal / np.linalg.norm(normal), (len(segment.points), 1)) + radial = np.asarray(segment.points) - segment.pivot + radial[:, 2] = 0 + return -math.copysign(1, segment.direction[2]) * radial / np.linalg.norm(radial, axis=1, keepdims=True) + + +def _prism( + name: str, + points: np.ndarray, + normals: np.ndarray, + offset: float | np.ndarray, + width: float, + bottom: float, + top: float, + *, + closed: bool = False, +) -> MeshSpec: + """Sweep a closed solid along a three-dimensional centerline [m].""" + offset = np.broadcast_to(offset, (len(points),))[:, None] + left = points + normals * (offset - width / 2) + right = points + normals * (offset + width / 2) + vertices = np.concatenate((left + (0, 0, top), right + (0, 0, top), left + (0, 0, bottom), right + (0, 0, bottom))) + count = len(points) + quads = [] + for i in range(count if closed else count - 1): + j = (i + 1) % count + quads.extend( + ( + (i, j, count + j, count + i), + (2 * count + i, 3 * count + i, 3 * count + j, 2 * count + j), + (i, 2 * count + i, 2 * count + j, j), + (count + i, count + j, 3 * count + j, 3 * count + i), + ) + ) + if not closed: + quads.extend(((0, count, 3 * count, 2 * count), (count - 1, 3 * count - 1, 4 * count - 1, 2 * count - 1))) + faces = tuple(triangle for a, b, c, d in quads for triangle in ((a, b, c), (a, c, d))) + return MeshSpec(name, tuple(tuple(vertex) for vertex in vertices), faces) + + +def warehouse_belt_sections(side: str, **kwargs: float | bool) -> tuple[ConveyorSectionSpec, ...]: + """Build linked belt surfaces from USD while preserving the original manipulation geometry.""" + sign = belt_direction(side) + sections = [] + for segment in _route_segments(side): + name = f"Conveyor{side}{segment.name}Collision" + pivot = segment.pivot + surface_kwargs = dict(kwargs) + if segment.velocity is not None: + surface_kwargs["velocity"] = segment.velocity + if segment.friction is not None: + surface_kwargs["friction_coefficient"] = segment.friction + if segment.name == "Working": + original = belt_collision_section_specs(side, **kwargs)[1 if sign > 0 else 0] + sections.append(original) + continue + if segment.name in {"PickupBend", "PlacementBend"}: + pickup = segment.name == "PickupBend" + name = f"Conveyor{side}{'Left' if pickup else 'Right'}InnerTurnCollision" + pivot = (BELT_CENTER_X + (-BELT_HALF_STRAIGHT if pickup else BELT_HALF_STRAIGHT), sign * BELT_CENTER_Y, 0.0) + geometry = _turn_collision_mesh( + name, pivot[0], BELT_CENTER_Y, math.pi if pickup else -math.pi / 2, math.pi / 2 + ) + if sign < 0: + geometry = replace( + geometry, + vertices=tuple((x, -y, z) for x, y, z in geometry.vertices), + faces=tuple((a, c, b) for a, b, c in geometry.faces), + ) + else: + points = np.array(segment.points) + # Overlap adjacent panels without exposing a vertical collision seam. + for index, neighbor, direction in ((0, 1, -1), (-1, -2, 1)): + tangent = points[neighbor] - points[index] if index == 0 else points[index] - points[neighbor] + points[index] += direction * BELT_COLLISION_SEAM_OVERLAP * tangent / np.linalg.norm(tangent) + geometry = _prism( + name, points, _normals(segment), 0, segment.width + 2 * BELT_COLLISION_OVERHANG, -BELT_THICKNESS, 0 + ) + sections.append( + ConveyorSectionSpec( + geometry, + SurfaceVelocitySpec( + prim_path=f"{{ENV_REGEX_NS}}/{geometry.name}", + direction=segment.direction, + surface_normal=(0.0, 0.0, 1.0) + if segment.pivot is not None + else tuple(np.cross(segment.direction, _normals(segment)[0])), + curved=segment.pivot is not None, + pivot_point=pivot or (0, 0, 0), + radius=segment.radius, + **surface_kwargs, + ), + ) + ) + return tuple(sections) + + +def warehouse_guard_meshes(side: str) -> tuple[MeshSpec, ...]: + """Build continuous guides for each closed circulation route and open supply belt.""" + segments = _route_segments(side) + guards = [] + for path in dict.fromkeys(segment.path for segment in segments): + points, normals, offsets = [], [], [] + route = [segment for segment in segments if segment.path == path] + for index, segment in enumerate(route): + stop = None if path != "main" and index == len(route) - 1 else -1 + for point, normal in zip(segment.points[:stop], _normals(segment)[:stop]): + points.append(point) + normals.append(normal) + offsets.append((segment.width + GUARD_THICKNESS) / 2) + for boundary, sign in (("Inner", -1), ("Outer", 1)): + guards.append( + _prism( + f"Guard{side}{path.title()}{boundary}", + np.asarray(points), + np.asarray(normals), + sign * np.asarray(offsets), + GUARD_THICKNESS, + -GUARD_BASE_OVERLAP, + GUARD_HEIGHT, + closed=path == "main", + ) + ) + return tuple(guards) diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/franka_robot_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/franka_robot_cfg.py index 89250854b6c2..12a0adba8338 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/franka_robot_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/franka_robot_cfg.py @@ -6,6 +6,7 @@ """Task-calibrated Franka configuration for conveyor manipulation.""" from isaaclab_newton.sim.schemas import MujocoJointCfg +from isaaclab_physx.sim.schemas import PhysxArticulationCfg from isaaclab.actuators import ImplicitActuatorCfg @@ -61,6 +62,8 @@ FRANKA_PANDA_CONVEYOR_PHYSX_CFG.spawn.joint_drive_props = None # Contact-rich manipulation benefits from resolving the articulation for more than # the generic asset defaults, especially with the deliberately stiff trained gains. -FRANKA_PANDA_CONVEYOR_PHYSX_CFG.spawn.articulation_props.solver_position_iteration_count = 32 -FRANKA_PANDA_CONVEYOR_PHYSX_CFG.spawn.articulation_props.solver_velocity_iteration_count = 4 +for _properties in FRANKA_PANDA_CONVEYOR_PHYSX_CFG.spawn.articulation_props: + if isinstance(_properties, PhysxArticulationCfg): + _properties.solver_position_iteration_count = 32 + _properties.solver_velocity_iteration_count = 4 """PhysX variant with the same joints, gains, action ordering, and gravity-compensated policy contract.""" diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/__init__.pyi b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/__init__.pyi index 8007e7febd58..5bd90e830c86 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/__init__.pyi +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/__init__.pyi @@ -76,4 +76,4 @@ from .terminations import ( transfer_sequence_time_out, ) from isaaclab.envs.mdp import * -from isaaclab_tasks.core.lift.mdp.events_cfg import SuccessMonitorCfg +from isaaclab_tasks.utils.success_monitor import SuccessMonitorCfg diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/commands.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/commands.py index 6d1d783e3870..2c03810c417f 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/commands.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/commands.py @@ -15,13 +15,14 @@ from isaaclab.managers import CommandTerm, CommandTermCfg from isaaclab.utils.configclass import configclass +from ..conveyor_cube_pool import cube_values from ..conveyor_geometry import BELT_CENTER_X, BELT_HALF_STRAIGHT from .kinematics import end_effector_pose from .reset_events import CUBE_COUNT, ConveyorResetRecipe, select_next_transfer_cube, side_inner_y from .rewards import current_transfer_potential, physical_cube_acquisition_mask if TYPE_CHECKING: - from isaaclab.assets import Articulation, RigidObject + from isaaclab.assets import Articulation from isaaclab.envs import ManagerBasedRLEnv @@ -62,7 +63,6 @@ def __init__(self, cfg: ConveyorTransferCommandCfg, env: ManagerBasedRLEnv) -> N raise RuntimeError("ConveyorTransferCommand requires ConveyorResetStateTable reset metadata.") self._reset_term = reset_term self._robot: Articulation = env.scene["robot"] - self._cubes: tuple[RigidObject, ...] = tuple(env.scene[f"cube_{cube_id}"] for cube_id in range(CUBE_COUNT)) self._finger_joint_ids = self._robot.find_joints("panda_finger_joint[1-2]", preserve_order=True)[0] self.target_cube_ids = torch.zeros(self.num_envs, dtype=torch.long, device=self.device) @@ -149,8 +149,8 @@ def evaluate(self) -> None: if not bool(torch.any(evaluate_mask)): return - positions = torch.stack(tuple(cube.data.root_pos_w.torch for cube in self._cubes), dim=1) - velocities = torch.stack(tuple(cube.data.root_lin_vel_w.torch for cube in self._cubes), dim=1) + positions = cube_values(self._env, "root_pos_w") + velocities = cube_values(self._env, "root_lin_vel_w") index = self.target_cube_ids.view(self.num_envs, 1, 1).expand(-1, 1, 3) active_position = torch.gather(positions, 1, index).squeeze(1) - self._env.scene.env_origins active_velocity = torch.gather(velocities, 1, index).squeeze(1) @@ -184,6 +184,9 @@ def evaluate(self) -> None: source_sides = self.source_side_ids[success_ids] self.transfer_counts[success_ids] += 1 self.direction_transfer_counts[success_ids, source_sides] += 1 + pool = getattr(self._env, "conveyor_cube_pool", None) + if pool is not None: + pool.record_transfers(success_ids, self.target_cube_ids[success_ids]) potential = current_transfer_potential(self._env, command=self) progressed = (potential >= self._target_potential) & (evaluation_steps >= self.cfg.minimum_progress_steps) @@ -213,8 +216,7 @@ def set_goal(self, target_cube_id: int, env_ids: Sequence[int] | torch.Tensor | ids = self._resolve_env_ids(env_ids) if ids.numel() == 0: return - cube = self._cubes[target_cube_id] - local_y = cube.data.root_pos_w.torch[ids, 1] - self._env.scene.env_origins[ids, 1] + local_y = cube_values(self._env, "root_pos_w")[ids, target_cube_id, 1] - self._env.scene.env_origins[ids, 1] source_side_ids = (local_y < 0.0).long() target_cube_ids = torch.full_like(ids, target_cube_id) self._assign_goal(ids, target_cube_ids, source_side_ids) @@ -241,7 +243,7 @@ def _resample_command(self, env_ids: Sequence[int]) -> None: self.subgoal_start_steps[ids] = 0 return - positions = torch.stack(tuple(cube.data.root_pos_w.torch[ids] for cube in self._cubes), dim=1) + positions = cube_values(self._env, "root_pos_w")[ids] positions -= self._env.scene.env_origins[ids].unsqueeze(1) next_source_side_ids = 1 - self.source_side_ids[ids] next_cube_ids = select_next_transfer_cube( diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/curriculums.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/curriculums.py index 3d70ad663e5c..85b0830ea909 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/curriculums.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/curriculums.py @@ -15,7 +15,7 @@ from isaaclab.managers import CurriculumTermCfg, ManagerTermBase -from isaaclab_tasks.core.lift.mdp.events_cfg import SuccessMonitorCfg +from isaaclab_tasks.utils.success_monitor import SuccessMonitorCfg from .reset_events import BELT_DEPLOYMENT_VARIANT, CUBE_COUNT, ConveyorResetRecipe, reset_variant_counts diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/observations.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/observations.py index e7f699c36bc4..8fb26f8d5460 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/observations.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/observations.py @@ -14,11 +14,12 @@ from isaaclab.managers import SceneEntityCfg from isaaclab.utils import math as math_utils +from ..conveyor_cube_pool import cube_values from .kinematics import end_effector_pose, tool_velocity from .reset_events import CUBE_COUNT, TRANSFER_X, side_inner_y if TYPE_CHECKING: - from isaaclab.assets import Articulation, RigidObject + from isaaclab.assets import Articulation from isaaclab.envs import ManagerBasedRLEnv @@ -27,19 +28,15 @@ def _transfer_command(env: ManagerBasedRLEnv, command_name: str = "transfer"): return env.command_manager.get_term(command_name) -def _cube_assets(env: ManagerBasedRLEnv) -> tuple[RigidObject, ...]: - """Return the four cubes in stable identity order.""" - return tuple(env.scene[f"cube_{cube_id}"] for cube_id in range(CUBE_COUNT)) - - def _cube_state(env: ManagerBasedRLEnv) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor]: """Stack cube world positions, orientations, and spatial velocities.""" - cubes = _cube_assets(env) - return ( - torch.stack(tuple(cube.data.root_pos_w.torch for cube in cubes), dim=1), - torch.stack(tuple(cube.data.root_quat_w.torch for cube in cubes), dim=1), - torch.stack(tuple(cube.data.root_vel_w.torch for cube in cubes), dim=1), + state = ( + cube_values(env, "root_pos_w"), + cube_values(env, "root_quat_w"), + cube_values(env, "root_vel_w"), ) + adapter = getattr(env, "_adapt_policy_cube_state", None) + return state if adapter is None else adapter(*state) def _active_cube_values(values: torch.Tensor, target_cube_ids: torch.Tensor) -> torch.Tensor: @@ -82,8 +79,9 @@ def transfer_object_observation(env: ManagerBasedRLEnv) -> torch.Tensor: """Describe all four cubes in stable identity slots. The observation contains local positions, tool-relative positions, local - up axes, and linear/angular velocities. Cube identity does not change - during an episode; :func:`target_cube_one_hot` selects the active slot. + up axes, and linear/angular velocities. The base task keeps fixed identities; + warehouse playback can refill remote slots from its physical parcel pool. + :func:`target_cube_one_hot` selects the active slot. """ positions, quaternions, velocities = _cube_state(env) local_positions = positions - env.scene.env_origins.unsqueeze(1) diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/rewards.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/rewards.py index ebcb05a69753..1c161562eaac 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/rewards.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/rewards.py @@ -13,11 +13,12 @@ from isaaclab.managers import SceneEntityCfg +from ..conveyor_cube_pool import cube_values from .kinematics import end_effector_pose -from .reset_events import CUBE_COUNT, CUBE_REST_Z, TRANSFER_X, side_inner_y +from .reset_events import CUBE_REST_Z, TRANSFER_X, side_inner_y if TYPE_CHECKING: - from isaaclab.assets import Articulation, RigidObject + from isaaclab.assets import Articulation from isaaclab.envs import ManagerBasedRLEnv from .commands import ConveyorTransferCommand @@ -63,8 +64,7 @@ def current_transfer_potential( """Gather current task state and evaluate the shaping potential.""" if command is None: command = env.command_manager.get_term(command_name) - cubes: tuple[RigidObject, ...] = tuple(env.scene[f"cube_{cube_id}"] for cube_id in range(CUBE_COUNT)) - positions = torch.stack(tuple(cube.data.root_pos_w.torch for cube in cubes), dim=1) + positions = cube_values(env, "root_pos_w") index = command.target_cube_ids.view(env.num_envs, 1, 1).expand(-1, 1, 3) active_position = torch.gather(positions, 1, index).squeeze(1) - env.scene.env_origins tool_position, _ = end_effector_pose(env) @@ -127,8 +127,7 @@ def physical_cube_acquisition_mask( raise ValueError("Physical acquisition thresholds must be positive.") if command is None: command = env.command_manager.get_term(command_name) - cubes: tuple[RigidObject, ...] = tuple(env.scene[f"cube_{cube_id}"] for cube_id in range(CUBE_COUNT)) - positions = torch.stack(tuple(cube.data.root_pos_w.torch for cube in cubes), dim=1) + positions = cube_values(env, "root_pos_w") index = command.target_cube_ids.view(env.num_envs, 1, 1).expand(-1, 1, 3) active_position = torch.gather(positions, 1, index).squeeze(1) tool_position, _ = end_effector_pose(env) diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/sorting.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/sorting.py new file mode 100644 index 000000000000..7cb92cea07d5 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/sorting.py @@ -0,0 +1,128 @@ +# 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 + +"""Parcel-class dispatch for the warehouse's unchanged four-slot transfer policy.""" + +from __future__ import annotations + +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import torch + +from isaaclab.utils.configclass import configclass + +from ..conveyor_cube_pool import cube_values +from .commands import ConveyorTransferCommand, ConveyorTransferCommandCfg +from .rewards import physical_cube_acquisition_mask + +if TYPE_CHECKING: + from ..conveyor_franka_warehouse_env import ConveyorFrankaWarehouseEnv + + +class ConveyorSortCommand(ConveyorTransferCommand): + """Transfer misplaced cartons and leave correctly sorted inventory circulating. + + Physical parcel IDs carry immutable destination classes. The dispatcher presents + one wrong-lane arrival through the pretrained policy's existing cube/side command; + color is a visual class label, not an additional policy observation. + """ + + cfg: ConveyorSortCommandCfg + + def __init__(self, cfg: ConveyorSortCommandCfg, env: ConveyorFrankaWarehouseEnv) -> None: + super().__init__(cfg, env) + if not 0.14 <= cfg.pickup_x_range[0] < cfg.pickup_x_range[1] <= 1.02: + raise ValueError("The sorting pickup window must lie within the original working straight [0.14, 1.02] m.") + if len(cfg.parcel_destinations) != len(env.conveyor_cube_pool.assets) or set(cfg.parcel_destinations) != {0, 1}: + raise ValueError("Parcel destinations must match the physical pool and include both conveyor IDs, 0 and 1.") + if len(cfg.parcel_colors) != len(cfg.parcel_destinations): + raise ValueError("Each physical parcel requires a color and a destination.") + for color in set(cfg.parcel_colors): + destinations = { + side for shade, side in zip(cfg.parcel_colors, cfg.parcel_destinations, strict=True) if shade == color + } + if len(destinations) != 1: + raise ValueError(f"All {color} parcels must share one destination conveyor.") + self.parcel_destinations = torch.tensor(cfg.parcel_destinations, device=self.device) + self.has_target = torch.zeros(self.num_envs, device=self.device, dtype=torch.bool) + self.metrics["sorted_parcels"] = torch.zeros(self.num_envs, device=self.device) + self.metrics["batch_complete"] = torch.zeros(self.num_envs, device=self.device) + + def evaluate(self) -> None: + """Credit stable placements only while the dispatcher owns an active transfer.""" + self.new_success[~self.has_target] = False + self.is_success[~self.has_target] = False + self._last_evaluation_steps[~self.has_target] = self._env.episode_length_buf[~self.has_target] + super().evaluate() + + def _resample_command(self, env_ids: Sequence[int]) -> None: + # Reset metadata remains compatible with the shared reward and observation terms. + if self._resampling_from_reset: + super()._resample_command(env_ids) + self.has_target[env_ids] = False + self.held_cube_ids[env_ids] = -1 + self.pending_success[env_ids] = False + + def _update_command(self) -> None: + """Publish slot assignments before the manager computes the next policy observation.""" + env = self._env + pool = env.conveyor_cube_pool + positions = cube_values(env, "root_pos_w", all_cubes=True) - env.scene.env_origins[:, None] + local = env._in_workcell(positions) + rows = torch.arange(self.num_envs, device=self.device) + physical_target = pool.slot_ids[rows, self.target_cube_ids] + held = physical_cube_acquisition_mask(env, command=self) & self.has_target + self.has_target &= ~self.pending_success & (local[rows, physical_target] | held) + self.pending_success.zero_() + wrong_lane = (positions[..., 1] < 0).long() != self.parcel_destinations + candidates = ( + wrong_lane + & (positions[..., 0] > self.cfg.pickup_x_range[0]) + & (positions[..., 0] < self.cfg.pickup_x_range[1]) + & (positions[..., 1].abs() > 0.20) + & (positions[..., 1].abs() < 0.36) + & (positions[..., 2] > 0.04) + & (positions[..., 2] < 0.10) + ) + pool.refresh(positions, local, candidates, self.target_cube_ids, self.has_target) + available = candidates.gather(1, pool.slot_ids) + eligible = available.any(dim=1) & ~self.has_target + choices = torch.where(available, positions[..., 0].gather(1, pool.slot_ids), -torch.inf).argmax(dim=1) + for slot in range(4): + env_ids = torch.where(eligible & (choices == slot))[0] + if env_ids.numel(): + self.set_goal(slot, env_ids) + self.has_target[env_ids] = True + self.command_counter[env_ids] += 1 + # A class is counted only after leaving the elevated supply and landing on a loop. + velocities = cube_values(env, "root_lin_vel_w", all_cubes=True) + on_loop = (positions[..., 2] > 0.04) & (positions[..., 2] < 0.20) & (velocities[..., 2].abs() < 0.15) + active = torch.zeros_like(on_loop).scatter_( + 1, pool.slot_ids[rows, self.target_cube_ids, None], self.has_target[:, None] + ) + on_loop &= ~active + sorted_parcels = (~wrong_lane & on_loop).sum(dim=1) + self.metrics["sorted_parcels"].copy_(sorted_parcels) + self.metrics["batch_complete"].copy_(sorted_parcels == len(pool.assets)) + + +@configclass +class ConveyorSortCommandCfg(ConveyorTransferCommandCfg): + """Batch sorting with fixed physical classes and checkpoint-compatible commands.""" + + class_type: type[ConveyorSortCommand] | str = "{DIR}.sorting:ConveyorSortCommand" + + parcel_destinations: tuple[int, ...] = (0, 1) * 12 + """Destination per physical parcel: 0 is the positive-Y loop, 1 the negative-Y loop.""" + + parcel_colors: tuple[str, ...] = ("blue", "orange", "green", "purple") * 6 + """Authored color per physical parcel; blue, orange, green, and purple are available.""" + + randomize_arrivals: bool = True + """Shuffle physical parcel identities across authored start positions at each reset.""" + + pickup_x_range: tuple[float, float] = (0.35, 1.02) + """Longitudinal arrival window in policy workspace coordinates [m].""" diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/terminations.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/terminations.py index 60f34adb9416..9f1f680319bb 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/terminations.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/mdp/terminations.py @@ -15,6 +15,7 @@ from isaaclab.managers import SceneEntityCfg +from ..conveyor_cube_pool import cube_values from ..conveyor_geometry import ( BELT_CENTER_X, BELT_HALF_STRAIGHT, @@ -22,10 +23,10 @@ BELT_WIDTH, GUARD_THICKNESS, ) -from .reset_events import CUBE_COUNT, CUBE_SIZE +from .reset_events import CUBE_SIZE if TYPE_CHECKING: - from isaaclab.assets import Articulation, RigidObject + from isaaclab.assets import Articulation from isaaclab.envs import ManagerBasedRLEnv @@ -93,8 +94,7 @@ def cube_out_of_workspace( ), ) -> torch.Tensor: """Terminate when any cube leaves the complete guarded racetrack workspace.""" - cubes: tuple[RigidObject, ...] = tuple(env.scene[f"cube_{cube_id}"] for cube_id in range(CUBE_COUNT)) - positions = torch.stack(tuple(cube.data.root_pos_w.torch for cube in cubes), dim=1) + positions = cube_values(env, "root_pos_w", all_cubes=True) positions -= env.scene.env_origins.unsqueeze(1) lower = positions.new_tensor(minimum) upper = positions.new_tensor(maximum) @@ -109,7 +109,5 @@ def nonfinite_scene_state( robot: Articulation = env.scene[robot_cfg.name] invalid = ~torch.all(torch.isfinite(robot.data.joint_pos.torch), dim=1) invalid |= ~torch.all(torch.isfinite(robot.data.joint_vel.torch), dim=1) - for cube_id in range(CUBE_COUNT): - cube: RigidObject = env.scene[f"cube_{cube_id}"] - invalid |= ~torch.all(torch.isfinite(cube.data.root_state_w.torch), dim=1) + invalid |= ~torch.isfinite(cube_values(env, "root_state_w", all_cubes=True)).all(dim=(1, 2)) return invalid diff --git a/source/isaaclab_tasks/pyproject.toml b/source/isaaclab_tasks/pyproject.toml index 46052dcb2186..7dca2968d29a 100644 --- a/source/isaaclab_tasks/pyproject.toml +++ b/source/isaaclab_tasks/pyproject.toml @@ -43,3 +43,4 @@ include = ["isaaclab_tasks", "isaaclab_tasks.*"] [tool.setuptools.package-data] "*" = ["*.pyi"] +"isaaclab_tasks.contrib.conveyor_franka" = ["assets/*.usda", "assets/*.usd"] diff --git a/source/isaaclab_tasks/test/contrib/test_conveyor_franka_asset_cfg.py b/source/isaaclab_tasks/test/contrib/test_conveyor_franka_asset_cfg.py index 57f39bd7da3f..70577e118509 100644 --- a/source/isaaclab_tasks/test/contrib/test_conveyor_franka_asset_cfg.py +++ b/source/isaaclab_tasks/test/contrib/test_conveyor_franka_asset_cfg.py @@ -8,13 +8,19 @@ import math import subprocess import sys +from types import SimpleNamespace import gymnasium as gym +import pytest + +import isaaclab.sim as sim_utils import isaaclab_tasks # noqa: F401 from isaaclab_tasks.contrib.conveyor_franka.conveyor_franka_asset_env_cfg import ( + _PRESENTATION_ASSETS, ConveyorFrankaA09A12EnvCfg, _make_usd_subtree_visual_only, + _presentation_layer, ) from isaaclab_tasks.contrib.conveyor_franka.conveyor_franka_env_cfg import ConveyorFrankaEnvCfg from isaaclab_tasks.contrib.conveyor_franka.conveyor_geometry import ( @@ -37,12 +43,19 @@ def test_a09_a12_play_task_reuses_the_newton_policy_contract() -> None: assert cfg.scene.num_envs == 1 assert cfg.actions == base_cfg.actions assert cfg.observations == base_cfg.observations - assert cfg.commands == base_cfg.commands + assert cfg.commands.transfer.parcel_destinations == (0, 1) * 12 + assert cfg.commands.transfer.parcel_colors == ("blue", "orange", "green", "purple") * 6 + assert cfg.commands.transfer.randomize_arrivals is True + assert cfg.commands.transfer.hold_steps == base_cfg.commands.transfer.hold_steps assert cfg.events == base_cfg.events assert cfg.rewards == base_cfg.rewards - assert cfg.terminations == base_cfg.terminations + for name, term in vars(base_cfg.terminations).items(): + if name != "cube_out_of_workspace": + assert getattr(cfg.terminations, name) == term + assert cfg.terminations.cube_out_of_workspace.func == base_cfg.terminations.cube_out_of_workspace.func assert cfg.decimation == base_cfg.decimation assert cfg.sim.dt == base_cfg.sim.dt + assert cfg.sim.physics.load_visual_shapes is False def test_a09_a12_config_import_does_not_preload_usd() -> None: @@ -90,8 +103,8 @@ def test_visual_only_usd_strips_physics_and_execution_metadata() -> None: assert not physics_scene.IsActive() -def test_digital_twin_assets_replace_only_procedural_render_geometry() -> None: - """A09/A12 visuals coexist with the unchanged lightweight collision proxies.""" +def test_digital_twin_assets_use_separate_physical_routes() -> None: + """Imported visual assets do not own contacts on the extended physical routes.""" scene = ConveyorFrankaA09A12EnvCfg().scene assert scene.conveyor_left_belt_visual is None @@ -100,22 +113,27 @@ def test_digital_twin_assets_replace_only_procedural_render_geometry() -> None: assert hasattr(scene, "conveyor_left_right_turn_collision") assert hasattr(scene, "guard_left_inner_collision") - asset_names = tuple(name for name in vars(scene) if name.endswith(("_a09_visual", "_a12_visual"))) - assert len(asset_names) == 8 - assert sum(name.endswith("_a09_visual") for name in asset_names) == 4 - assert sum(name.endswith("_a12_visual") for name in asset_names) == 4 + asset_names = tuple( + name + for name in vars(scene) + if name.endswith(("_a09_visual", "_a12_visual")) and getattr(scene, name) is not None + ) + assert len(asset_names) == 2 + assert all(name.endswith("_a09_visual") for name in asset_names) + assert len(scene.build_conveyor_belt_specs()) > 8 for name in asset_names: asset = getattr(scene, name) - assert asset.spawn.usd_path.endswith("ConveyorBelt_A09.usd" if "a09" in name else "ConveyorBelt_A12.usd") + assert asset.spawn.usd_path.endswith("conveyor_straight_supported.usd") assert asset.spawn.collision_props is None assert asset.spawn.make_uninstanceable -def test_thor_table_is_visual_only_and_all_support_feet_reach_the_ground() -> None: - """The Thor mount and measured conveyor lows sit on the common z=0 floor.""" +def test_thor_table_and_conveyors_rest_on_their_authored_supports() -> None: + """The Thor mount reaches the floor and narrow supports retain the conveyor deck elevation.""" cfg = ConveyorFrankaA09A12EnvCfg() scene = cfg.scene + scene._configure_route_assets() assert scene.tabletop.prim_path.endswith("/RobotThorTableVisual") assert scene.tabletop.spawn.usd_path.endswith("/Props/Mounts/thor_table.usd") @@ -125,67 +143,430 @@ def test_thor_table_is_visual_only_and_all_support_feet_reach_the_ground() -> No assert math.isclose(scene.tabletop.init_state.pos[2] - 0.795 * scene.tabletop.spawn.scale[2], ground_z) for name in vars(scene): - if name.endswith(("_a09_visual", "_a12_visual")): - assert math.isclose(getattr(scene, name).init_state.pos[2], ground_z) + if name.endswith(("_a09_visual", "_a12_visual")) and getattr(scene, name) is not None: + assert math.isclose(getattr(scene, name).init_state.pos[2], 0.55) assert ground_z == 0.0 workspace_z = scene.ground.workspace_origin_offset[2] - assert 0.2 < workspace_z < 0.3 + assert 0.75 < workspace_z < 0.85 assert math.isclose(scene.robot.init_state.pos[2], workspace_z) - assert math.isclose(scene.cube_0.init_state.pos[2], 0.06 + workspace_z) + assert math.isclose(scene.cube_0.init_state.pos[2], 0.28 + workspace_z) base_collision_z = ConveyorFrankaEnvCfg().scene.conveyor_left_top_straight_collision.init_state.pos[2] - assert math.isclose(scene.conveyor_left_top_straight_collision.init_state.pos[2], base_collision_z + workspace_z) + assert math.isclose(scene.warehouse_left_section_0.init_state.pos[2], base_collision_z + workspace_z) -def test_warehouse_props_are_render_only_and_pallet_bays_do_not_overlap() -> None: - """Scene dressing stays outside the policy contract and owns no collision.""" - scene = ConveyorFrankaA09A12EnvCfg().scene - assert not any(name.startswith("sorter_") for name in vars(scene)) - - prop_names = ("packing_station_visual", "loaded_pallet_visual", "empty_pallet_visual") - for name in prop_names: - asset = getattr(scene, name) - assert asset.spawn.collision_props is None - assert asset.spawn.make_uninstanceable +def test_warehouse_layout_is_usd_authored_and_preserves_cube_physics() -> None: + """The presentation adds one USD assembly and changes only the task cubes' render spawner.""" + from pxr import Sdf - # Measured, conservatively axis-aligned extents after scaling leave a clear - # aisle between the loaded and empty pallet bays even with their rotations. - loaded = scene.loaded_pallet_visual - empty = scene.empty_pallet_visual - loaded_half_extent_x = ( - 0.5 * loaded.spawn.scale[0] * (1.203 * math.cos(math.radians(30.0)) + 0.80281 * math.sin(math.radians(30.0))) + scene = ConveyorFrankaA09A12EnvCfg().scene + base = ConveyorFrankaEnvCfg().scene + assert scene.warehouse_visual.spawn.collision_props is None + layer = Sdf.Layer.FindOrOpen(scene.warehouse_visual.spawn.usd_path) + assert layer.defaultPrim == "Warehouse" + assert layer.GetPrimAtPath("/Warehouse/Lights/WorkcellSoftbox") + assert layer.GetPrimAtPath("/Warehouse/Transport/Divert") + assert layer.GetPrimAtPath("/Warehouse/Parcels/Parcel00") + assert not layer.GetPrimAtPath("/Warehouse/Cell/Riser0") + assert not layer.GetPrimAtPath("/Warehouse/Supports") + assert not layer.GetPrimAtPath("/Warehouse/NetworkParcels") + assert layer.GetPrimAtPath("/Warehouse/Scanner").attributes["xformOp:rotateZ"].default == 90 + assert all( + path.startswith("https://") + or path + in {"conveyor_straight_supported.usd", "conveyor_quarter_supported.usd", "conveyor_routes.usda", "parcel.usda"} + for path in layer.GetExternalReferences() ) - empty_half_extent_x = ( - 0.5 * empty.spawn.scale[0] * (1.213235 * math.cos(math.radians(15.0)) + 0.802298 * math.sin(math.radians(15.0))) + for cube_id in range(4): + visual_spawn = getattr(scene, f"cube_{cube_id}").spawn.to_dict() + base_spawn = getattr(base, f"cube_{cube_id}").spawn.to_dict() + visual_spawn.pop("func") + visual_spawn.pop("parcel_usd_path") + base_spawn.pop("func") + assert visual_spawn == base_spawn + scene._configure_route_assets() + for cube_id in range(4, 24): + cube = getattr(scene, f"cube_{cube_id}") + actual = cube.spawn.to_dict() + expected = scene.cube_0.spawn.to_dict() + assert actual.pop("parcel_usd_path").endswith( + f"parcel_{ConveyorFrankaA09A12EnvCfg().commands.transfer.parcel_colors[cube_id]}.usda" + ) + expected.pop("parcel_usd_path") + assert actual == expected + assert cube.prim_path.endswith(f"/Cube{cube_id}") + + +@pytest.mark.parametrize( + "parcel_asset", + [ + "parcel.usda", + "parcel_blue.usda", + "parcel_orange.usda", + "parcel_green.usda", + "parcel_purple.usda", + ], +) +def test_carton_visual_is_centered_on_the_original_40_mm_collider(tmp_path, monkeypatch, parcel_asset) -> None: + """Asset normalization changes appearance without moving or resizing the grasp surface.""" + from pxr import Usd, UsdGeom, UsdPhysics + + from isaaclab_tasks.contrib.conveyor_franka import conveyor_franka_asset_env_cfg as asset_cfg + + # Measured unscaled Cardbox_A1 bounds, used as an offline stand-in for the remote mesh. + lower = (-0.34971755743026733, -0.260576993227005, 0.0) + upper = (0.34971755743026733, 0.2605747878551483, 0.5099270939826965) + source = Usd.Stage.CreateNew(str(tmp_path / "carton.usda")) + root = UsdGeom.Xform.Define(source, "/Carton").GetPrim() + source.SetDefaultPrim(root) + UsdPhysics.RigidBodyAPI.Apply(root) + mesh = UsdGeom.Cube.Define(source, "/Carton/Mesh") + mesh.CreateSizeAttr(1.0) + mesh.AddTranslateOp().Set(tuple((a + b) / 2 for a, b in zip(lower, upper))) + mesh.AddScaleOp().Set(tuple(b - a for a, b in zip(lower, upper))) + UsdPhysics.CollisionAPI.Apply(mesh.GetPrim()) + source.GetRootLayer().Save() + monkeypatch.setattr(asset_cfg, "retrieve_file_path", lambda path: str(tmp_path / "carton.usda")) + _presentation_layer.cache_clear() + try: + stage = sim_utils.create_new_stage() + cfg = ConveyorFrankaA09A12EnvCfg().scene.cube_0.spawn + cfg.parcel_usd_path = str(_PRESENTATION_ASSETS / parcel_asset) + prim = cfg.func("/Cube", cfg) + visual = stage.GetPrimAtPath("/Cube/CartonVisual") + bounds = UsdGeom.BBoxCache(Usd.TimeCode.Default(), ["default", "render"]).ComputeWorldBound(visual) + assert tuple(bounds.ComputeAlignedRange().GetMin()) == pytest.approx((-0.02,) * 3, abs=1e-8) + assert tuple(bounds.ComputeAlignedRange().GetMax()) == pytest.approx((0.02,) * 3, abs=1e-8) + assert prim.HasAPI(UsdPhysics.RigidBodyAPI) + collider = stage.GetPrimAtPath("/Cube/geometry/mesh") + assert collider.HasAPI(UsdPhysics.CollisionAPI) + assert UsdGeom.Imageable(collider).ComputeVisibility() == "invisible" + assert sum(p.HasAPI(UsdPhysics.RigidBodyAPI) for p in stage.Traverse()) == 1 + assert sum(p.HasAPI(UsdPhysics.CollisionAPI) for p in stage.Traverse()) == 1 + assert not any(p.HasAPI(UsdPhysics.RigidBodyAPI) for p in Usd.PrimRange(visual)) + # The authored reference remains portable; cache resolution edits only an anonymous copy. + from pxr import Sdf + + assert all( + path.startswith("https://") + for path in Sdf.Layer.FindOrOpen(str(_PRESENTATION_ASSETS / "parcel.usda")).GetExternalReferences() + ) + finally: + _presentation_layer.cache_clear() + + +def test_asset_transforms_preserve_the_original_workcell() -> None: + """The working straights and adjoining quarter-turns keep their original locations and radii.""" + from pxr import Sdf + + from isaaclab_tasks.contrib.conveyor_franka.conveyor_geometry import belt_collision_section_specs + from isaaclab_tasks.contrib.conveyor_franka.conveyor_warehouse_geometry import ( + warehouse_belt_sections, ) - assert loaded.init_state.pos[0] + loaded_half_extent_x < empty.init_state.pos[0] - empty_half_extent_x - - assert scene.ground.terrain_type == "plane" - assert scene.ground.visual_material.diffuse_color == (0.055, 0.065, 0.082) - assert scene.warehouse_back_wall_visual.spawn.collision_props is None - assert scene.warehouse_side_wall_visual.spawn.collision_props is None - assert scene.safety_zone_0_visual.spawn.collision_props is None - -def test_asset_transforms_match_the_existing_racetrack_surface() -> None: - """Asset endpoints, radius, and belt crown align with the policy's original geometry.""" scene = ConveyorFrankaA09A12EnvCfg().scene - top = scene.conveyor_left_top_a09_visual - right_turn = scene.conveyor_left_right_a12_visual - left_turn = scene.conveyor_left_left_a12_visual - - assert top.init_state.pos[:2] == (BELT_CENTER_X + BELT_HALF_STRAIGHT, BELT_CENTER_Y + BELT_TURN_RADIUS) - assert right_turn.init_state.pos[:2] == top.init_state.pos[:2] - assert left_turn.init_state.pos[:2] == ( - BELT_CENTER_X - BELT_HALF_STRAIGHT, - BELT_CENTER_Y - BELT_TURN_RADIUS, + layer = Sdf.Layer.FindOrOpen(scene.warehouse_visual.spawn.usd_path) + for side, sign, original_index in (("Left", 1, 1), ("Right", -1, 0)): + sections = warehouse_belt_sections(side, velocity=0.35) + original = belt_collision_section_specs(side)[original_index].geometry + inner = sections[0].geometry + assert inner.position == original.position + assert inner.size == original.size + visual = getattr(scene, f"conveyor_{side.lower()}_{'bottom' if sign > 0 else 'top'}_a09_visual") + assert visual.init_state.pos[:2] == ( + BELT_CENTER_X + BELT_HALF_STRAIGHT, + sign * (BELT_CENTER_Y - BELT_TURN_RADIUS), + ) + assert 4 * visual.spawn.scale[0] == pytest.approx(2 * BELT_HALF_STRAIGHT) + assert visual.init_state.pos[2] + 1.78053 * visual.spawn.scale[2] == pytest.approx( + BELT_TOP_Z + scene.ground.workspace_origin_offset[2] + ) + for name, x in ( + ("RightInner", BELT_CENTER_X + BELT_HALF_STRAIGHT), + ("LeftInner", BELT_CENTER_X - BELT_HALF_STRAIGHT), + ): + turn = next(section.belt for section in sections if f"{name}Turn" in section.geometry.name) + assert turn.pivot_point == (x, sign * BELT_CENTER_Y, 0) + assert turn.radius == BELT_TURN_RADIUS + authored = layer.GetPrimAtPath(f"/Warehouse/WorkcellConveyors/{side}{name}") + position = authored.attributes["xformOp:translate"].default + scale = authored.attributes["xformOp:scale"].default + angle = math.radians(authored.attributes["xformOp:rotateZ"].default) + # A03's original circular pivot is at (0, -1.4961); the referenced arc lands on the physical bend. + radius = 1.4961 * scale[0] + assert radius == pytest.approx(BELT_TURN_RADIUS) + assert position[0] + radius * math.sin(angle) == pytest.approx(x) + assert position[1] - radius * math.cos(angle) == pytest.approx(sign * BELT_CENTER_Y) + assert any(section.belt.direction[2] > 0 for section in sections if not section.belt.curved) + assert any(section.belt.direction[2] < 0 for section in sections if not section.belt.curved) + feed_velocity = 0.043 if side == "Left" else 0.052 + assert all( + section.belt.velocity == (feed_velocity if "Supply" in section.geometry.name else 0.35) + for section in sections + ) + + +def test_warehouse_reset_loads_mixed_feeds_only_in_selected_environments(monkeypatch) -> None: + """A seeded reset shuffles all physical arrivals without disturbing other environments.""" + import torch + + from isaaclab_tasks.contrib.conveyor_franka.conveyor_cube_pool import ConveyorCubePool + from isaaclab_tasks.contrib.conveyor_franka.conveyor_franka_env import ConveyorFrankaEnv + from isaaclab_tasks.contrib.conveyor_franka.conveyor_franka_warehouse_env import ConveyorFrankaWarehouseEnv + from isaaclab_tasks.contrib.conveyor_franka.conveyor_warehouse_geometry import warehouse_parcel_positions + + origins = torch.tensor([[0.0, 0.0, 0.8], [10.0, 12.0, 0.8]]) + cubes = {} + before = [] + velocities = [] + positions = warehouse_parcel_positions() + for i in range(len(positions)): + pose = torch.tensor([[0.52, 0.27, 0.06, 0.0, 0.0, 0.0, 1.0]]).repeat(2, 1) + pose[:, :3] += origins + before.append(pose.clone()) + velocity = torch.ones(2, 6) + velocities.append(velocity) + + def write(root_pose, env_ids, state=pose): + state[env_ids] = root_pose + + def write_velocity(root_velocity, env_ids, state=velocity): + state[env_ids] = root_velocity + + cubes[f"cube_{i}"] = SimpleNamespace( + data=SimpleNamespace(root_pose_w=SimpleNamespace(torch=pose)), + write_root_pose_to_sim_index=write, + write_root_velocity_to_sim_index=write_velocity, + ) + + class Scene(dict): + env_origins = origins + + env = ConveyorFrankaWarehouseEnv.__new__(ConveyorFrankaWarehouseEnv) + env.scene = Scene(cubes) + env.conveyor_cube_pool = ConveyorCubePool(tuple(cubes.values()), 2, "cpu") + env._warehouse_animation = [] + env.sim = SimpleNamespace(device="cpu", remove_render_callback=lambda name: None) + env.cfg = ConveyorFrankaA09A12EnvCfg() + monkeypatch.setattr(ConveyorFrankaEnv, "_reset_idx", lambda self, ids: None) + monkeypatch.setattr(ConveyorFrankaEnv, "close", lambda self: None) + torch.manual_seed(42) + env._reset_idx(torch.tensor([1])) + actual_positions = [] + for i in range(len(positions)): + actual = cubes[f"cube_{i}"].data.root_pose_w.torch + torch.testing.assert_close(actual[0], before[i][0]) + torch.testing.assert_close(actual[1, 3:], before[i][1, 3:]) + actual_positions.append(actual[1, :3] - origins[1]) + torch.testing.assert_close(velocities[i][0], torch.ones(6)) + torch.testing.assert_close(velocities[i][1], torch.zeros(6)) + actual_positions = torch.stack(actual_positions) + expected = torch.tensor(positions) + distances = torch.linalg.vector_norm(actual_positions[:, None] - expected[None], dim=-1) + assert distances.min(dim=1).values.max() < 1e-5 + assert distances.argmin(dim=1).unique().numel() == len(positions) + assert not torch.allclose(actual_positions, expected) + torch.manual_seed(42) + env._reset_idx(torch.tensor([1])) + repeated = torch.stack([cube.data.root_pose_w.torch[1, :3] - origins[1] for cube in cubes.values()]) + torch.testing.assert_close(repeated, actual_positions) + + +def test_warehouse_animation_uses_active_kit_viewer_and_policy_time(tmp_path, monkeypatch) -> None: + """A CLI-selected Kit viewer animates and loops authored parcels without changing task state.""" + from pxr import Usd, UsdGeom + + from isaaclab_tasks.contrib.conveyor_franka.conveyor_franka_env import ConveyorFrankaEnv + from isaaclab_tasks.contrib.conveyor_franka.conveyor_franka_warehouse_env import ConveyorFrankaWarehouseEnv + + path = str(tmp_path / "traffic.usda") + source = Usd.Stage.CreateNew(path) + root = UsdGeom.Xform.Define(source, "/Warehouse").GetPrim() + source.SetDefaultPrim(root) + source.SetTimeCodesPerSecond(60) + source.SetEndTimeCode(120) + parcel = UsdGeom.Xform.Define(source, "/Warehouse/Parcels/Parcel00") + translate = parcel.AddTranslateOp() + translate.Set((0, 0, 1), 0) + translate.Set((2, 0, 1), 120) + parcel.AddRotateXYZOp().Set((0, 0, 0)) + source.GetRootLayer().Save() + stage = Usd.Stage.CreateInMemory() + stage.DefinePrim("/World/envs/env_0/WarehouseVisual").GetReferences().AddReference(path) + callbacks = {} + + def initialize(env, cfg, **kwargs): + env.cfg = cfg + env.common_step_counter = 60 + env.scene = SimpleNamespace(env_prim_paths=["/World/envs/env_0"]) + env.sim = SimpleNamespace( + stage=stage, + visualizers=[SimpleNamespace(cfg=SimpleNamespace(visualizer_type="kit"))], + set_setting=lambda name, value: None, + add_render_callback=lambda name, callback: callbacks.update({name: callback}), + remove_render_callback=lambda name: callbacks.pop(name, None), + ) + + monkeypatch.setattr(ConveyorFrankaEnv, "__init__", initialize) + monkeypatch.setattr(ConveyorFrankaEnv, "close", lambda env: None) + cfg = ConveyorFrankaA09A12EnvCfg() + # CLI viewer selection is resolved by SimulationContext, not written back into this list. + cfg.sim.visualizer_cfgs = [] + cfg.scene.warehouse_visual.spawn.usd_path = path + env = ConveyorFrankaWarehouseEnv(cfg) + try: + callback = callbacks["conveyor_warehouse_animation"] + callback(None) + position = stage.GetPrimAtPath("/World/envs/env_0/WarehouseVisual/Parcels/Parcel00").GetAttribute( + "xformOp:translate" + ) + assert tuple(position.Get()) == pytest.approx((1, 0, 1)) + env.common_step_counter = 180 + callback(None) + assert tuple(position.Get()) == pytest.approx((1, 0, 1)) + finally: + env.close() + _presentation_layer.cache_clear() + assert not callbacks + + +@pytest.mark.parametrize("remote_position", [(2.8, 0.1, 0.56), (1.2, 0.59, 0.16)]) +def test_warehouse_policy_view_preserves_local_states_and_physical_inventory(remote_position): + """Only remote transport is mapped to waiting slots; physical tensors remain untouched.""" + import torch + + from isaaclab_tasks.contrib.conveyor_franka.conveyor_franka_warehouse_env import ConveyorFrankaWarehouseEnv + + origin = torch.tensor([[10.0, 12.0, 0.8]]) + positions = ( + torch.tensor([[[0.52, 0.27, 0.06], [0.6, -0.1, 0.25], remote_position, [1.9, -1.1, 0.2]]]) + origin[:, None] + ) + quaternions = torch.randn(1, 4, 4) + velocities = torch.randn(1, 4, 6) + original = tuple(value.clone() for value in (positions, quaternions, velocities)) + env = SimpleNamespace( + scene=SimpleNamespace(env_origins=origin), + device="cpu", + cfg=SimpleNamespace(conveyor_force=SimpleNamespace(speed=0.35)), + _in_workcell=ConveyorFrankaWarehouseEnv._in_workcell, + ) + actual = ConveyorFrankaWarehouseEnv._adapt_policy_cube_state(env, positions, quaternions, velocities) + for result, source, before in zip(actual, (positions, quaternions, velocities), original): + torch.testing.assert_close(result[:, :2], source[:, :2]) + torch.testing.assert_close(source, before) + torch.testing.assert_close(actual[0][0, 2:, 1:] - origin[0, 1:], torch.tensor([[0.75, 0.06], [-0.75, 0.06]])) + torch.testing.assert_close(actual[1][0, 2:], torch.tensor([[0.0, 0.0, 0.0, 1.0], [0.0, 0.0, 0.0, 1.0]])) + + +def test_warehouse_idle_parks_and_preserves_invalid_actions(monkeypatch): + """Idle handling cannot mask invalid policy input or bypass an active transfer.""" + import torch + + from isaaclab_tasks.contrib.conveyor_franka.conveyor_franka_env import ConveyorFrankaEnv + from isaaclab_tasks.contrib.conveyor_franka.conveyor_franka_warehouse_env import ConveyorFrankaWarehouseEnv + + class Scene(dict): + num_envs = 3 + + scene = Scene( + robot=SimpleNamespace( + data=SimpleNamespace( + default_joint_pos=SimpleNamespace(torch=torch.zeros(3, 7)), + joint_pos=SimpleNamespace(torch=torch.full((3, 7), 0.12)), + ) + ) + ) + command = SimpleNamespace(has_target=torch.tensor([False, True, False])) + env = ConveyorFrankaWarehouseEnv.__new__(ConveyorFrankaWarehouseEnv) + env.scene = scene + env.sim = SimpleNamespace(device="cpu", remove_render_callback=lambda name: None) + env.cfg = SimpleNamespace(actions=SimpleNamespace(arm_action=SimpleNamespace(scale=0.12))) + env.command_manager = SimpleNamespace(get_term=lambda name: command) + env._warehouse_arm_joint_ids = list(range(7)) + env._warehouse_animation = [] + accepted = [] + result = ({"policy": torch.zeros(3, 123)}, None, None, None, {}) + + def step(self, action): + accepted.append(action) + return result + + monkeypatch.setattr(ConveyorFrankaEnv, "step", step) + monkeypatch.setattr(ConveyorFrankaEnv, "close", lambda self: None) + action = torch.full((3, 8), 0.5) + action[2, 0] = torch.nan + assert env.step(action) is result + actual = accepted[0] + torch.testing.assert_close(actual[0], torch.tensor([-0.25] * 7 + [0.0])) + torch.testing.assert_close(actual[1], action[1]) + torch.testing.assert_close(actual[2], action[2], equal_nan=True) + + +def test_parcel_pool_preserves_pinned_slots_and_eventually_assigns_every_physical_parcel(): + """Arrivals rotate through four distinct slots without displacing a held parcel or another environment.""" + import torch + + from isaaclab_tasks.contrib.conveyor_franka.conveyor_cube_pool import ConveyorCubePool + + pool = ConveyorCubePool((None,) * 16, 2, "cpu") + pool.slot_ids[1] = torch.tensor([3, 2, 1, 0]) + positions = torch.zeros(2, 16, 3) + positions[:, :, 0] = torch.linspace(0.4, 1.0, 16) + local = torch.zeros(2, 16, dtype=torch.bool) + local[0, 1] = True + candidates = torch.zeros_like(local) + candidates[0, 4:] = True + target_slots = torch.tensor([0, 2]) + pinned = torch.tensor([True, True]) + initial_other_environment = pool.slot_ids[1].clone() + for _ in range(6): + changed = pool.refresh(positions, local, candidates, target_slots, pinned) + assert changed.tolist() == [True, False] + assert pool.slot_ids[0, :2].tolist() == [0, 1] + assert len(pool.slot_ids[0].unique()) == 4 + torch.testing.assert_close(pool.slot_ids[1], initial_other_environment) + assert bool((pool.assignment_counts[0] > 0).all()) + physical_id = int(pool.slot_ids[0, 2]) + pool.record_transfers(torch.tensor([0]), torch.tensor([2])) + assert pool.transfer_counts[0, physical_id] == 1 + assert pool.transfer_counts.sum() == 1 + pool.reset(torch.tensor([0])) + assert pool.slot_ids[0].tolist() == [0, 1, 2, 3] + torch.testing.assert_close(pool.slot_ids[1], initial_other_environment) + + +def test_parcel_pool_gathers_per_environment_states_and_checks_unassigned_inventory(): + """All policy features use the same physical assignment; an unassigned fallen cube still terminates.""" + import torch + + from isaaclab_tasks.contrib.conveyor_franka.conveyor_cube_pool import ConveyorCubePool, cube_values + from isaaclab_tasks.contrib.conveyor_franka.mdp.observations import _cube_state + from isaaclab_tasks.contrib.conveyor_franka.mdp.terminations import cube_out_of_workspace + + assets = [] + for cube_id in range(6): + values = { + "root_pos_w": torch.tensor([[0.1 * cube_id, 0.27, 0.06], [10 + 0.1 * cube_id, 0.27, 0.06]]), + "root_quat_w": torch.full((2, 4), float(cube_id)), + "root_vel_w": torch.full((2, 6), float(cube_id)), + } + assets.append( + SimpleNamespace( + data=SimpleNamespace(**{name: SimpleNamespace(torch=value) for name, value in values.items()}) + ) + ) + pool = ConveyorCubePool(tuple(assets), 2, "cpu") + pool.slot_ids[:] = torch.tensor([[4, 1, 2, 3], [0, 5, 2, 3]]) + env = SimpleNamespace( + conveyor_cube_pool=pool, scene=SimpleNamespace(env_origins=torch.tensor([[0.0, 0.0, 0.0], [10.0, 0.0, 0.0]])) ) - assert left_turn.init_state.rot == (0.0, 0.0, 1.0, 0.0) - - a09_length = 4.0 * top.spawn.scale[0] - a12_diameter = 2.9922 * right_turn.spawn.scale[0] - asset_belt_top = right_turn.init_state.pos[2] + 1.78053 * right_turn.spawn.scale[2] - assert math.isclose(a09_length, 2.0 * BELT_HALF_STRAIGHT) - assert math.isclose(a12_diameter, 2.0 * BELT_TURN_RADIUS) - assert math.isclose(right_turn.spawn.scale[2], right_turn.spawn.scale[1]) - assert math.isclose(asset_belt_top, BELT_TOP_Z + scene.ground.workspace_origin_offset[2]) + physical_before = cube_values(env, "root_pos_w", all_cubes=True).clone() + states = _cube_state(env) + for values, attribute in zip(states, ("root_pos_w", "root_quat_w", "root_vel_w")): + for row in range(2): + expected = torch.stack([getattr(assets[i].data, attribute).torch[row] for i in pool.slot_ids[row]]) + torch.testing.assert_close(values[row], expected) + torch.testing.assert_close(cube_values(env, "root_pos_w", all_cubes=True), physical_before) + assert not cube_out_of_workspace(env).any() + assets[5].data.root_pos_w.torch[0, 2] = -1.0 # Parcel 5 is not assigned in environment 0. + assert cube_out_of_workspace(env).tolist() == [True, False] diff --git a/source/isaaclab_tasks/test/contrib/test_conveyor_franka_geometry.py b/source/isaaclab_tasks/test/contrib/test_conveyor_franka_geometry.py index 6b319bfe7ccb..c49409122aa3 100644 --- a/source/isaaclab_tasks/test/contrib/test_conveyor_franka_geometry.py +++ b/source/isaaclab_tasks/test/contrib/test_conveyor_franka_geometry.py @@ -7,9 +7,10 @@ from collections import Counter +import pytest + from isaaclab_tasks.contrib.conveyor_franka.conveyor_franka_env_cfg import _collision_properties, _cube from isaaclab_tasks.contrib.conveyor_franka.conveyor_geometry import ( - BELT_TOP_Z, BELT_TURN_RADIUS, TURN_SEGMENT_COUNT, MeshSpec, @@ -41,16 +42,35 @@ def test_racetrack_visual_meshes_are_named_watertight_loops(): assert set(_edge_use_counts(spec).values()) == {2} -def test_belt_top_faces_point_upward(): +@pytest.mark.parametrize("warehouse", [False, True]) +def test_belt_top_faces_point_upward(warehouse): """One-sided triangle-mesh surfaces support parcels from above.""" for side in ("Left", "Right"): - spec = belt_mesh_spec(side) - for face in spec.faces: - vertices = tuple(spec.vertices[index] for index in face) - if all(vertex[2] == BELT_TOP_Z for vertex in vertices): - a, b, c = vertices - cross_z = (b[0] - a[0]) * (c[1] - a[1]) - (b[1] - a[1]) * (c[0] - a[0]) - assert cross_z > 0.0 + if warehouse: + from isaaclab_tasks.contrib.conveyor_franka.conveyor_warehouse_geometry import ( + warehouse_belt_sections, + warehouse_guard_meshes, + ) + + specs = [ + section.geometry for section in warehouse_belt_sections(side) if isinstance(section.geometry, MeshSpec) + ] + specs.extend(warehouse_guard_meshes(side)) + else: + specs = [belt_mesh_spec(side)] + for spec in specs: + assert set(_edge_use_counts(spec).values()) == {2} + _assert_top_faces_point_upward(spec) + + +def _assert_top_faces_point_upward(spec): + top_z = max(vertex[2] for vertex in spec.vertices) + for face in spec.faces: + vertices = tuple(spec.vertices[index] for index in face) + if all(vertex[2] == top_z for vertex in vertices): + a, b, c = vertices + cross_z = (b[0] - a[0]) * (c[1] - a[1]) - (b[1] - a[1]) * (c[0] - a[0]) + assert cross_z > 0.0 def test_contact_configuration_uses_one_mujoco_parameterization(): @@ -78,3 +98,23 @@ def test_collision_sections_carry_schema_aligned_belt_intent(): assert tuple(section.belt.contact_threshold for section in sections) == (0.997,) * 4 assert tuple(section.belt.curved for section in sections) == (False, False, True, True) assert tuple(section.belt.radius for section in sections) == (None, None, BELT_TURN_RADIUS, BELT_TURN_RADIUS) + + +def test_elevated_belt_normals_accept_contacts_on_every_ramp_panel(): + """Inclined parcels receive traction instead of sliding into the bottom transition.""" + import numpy as np + + from isaaclab_tasks.contrib.conveyor_franka.conveyor_warehouse_geometry import warehouse_belt_sections + + for side in ("Left", "Right"): + for section in warehouse_belt_sections(side): + if section.belt.curved or abs(section.belt.direction[2]) < 1e-6: + continue + vertices = np.asarray(section.geometry.vertices) + triangles = vertices[np.asarray(section.geometry.faces)] + normals = np.cross(triangles[:, 1] - triangles[:, 0], triangles[:, 2] - triangles[:, 0]) + top = normals[normals[:, 2] > 1e-8] + top /= np.linalg.norm(top, axis=1, keepdims=True) + assert len(top) > 0 + assert np.all(top @ section.belt.surface_normal >= section.belt.contact_threshold) + assert abs(np.dot(section.belt.direction, section.belt.surface_normal)) < 1e-6 diff --git a/source/isaaclab_tasks/test/contrib/test_conveyor_franka_mdp.py b/source/isaaclab_tasks/test/contrib/test_conveyor_franka_mdp.py index 3aa4cb81bc59..9a8d50b2d4b1 100644 --- a/source/isaaclab_tasks/test/contrib/test_conveyor_franka_mdp.py +++ b/source/isaaclab_tasks/test/contrib/test_conveyor_franka_mdp.py @@ -8,6 +8,7 @@ from collections import Counter from types import SimpleNamespace +import pytest import torch from isaaclab_tasks.contrib.conveyor_franka.agents.rsl_rl_ppo_cfg import ( @@ -446,7 +447,8 @@ def test_next_transfer_cube_is_random_among_eligible_alternatives(): assert torch.all(torch.abs(frequencies[1:3] - 0.5) < 0.05) -def test_manual_transfer_goal_uses_selected_cube_current_side(): +@pytest.mark.parametrize("use_pool", [False, True]) +def test_manual_transfer_goal_uses_selected_cube_current_side(use_pool): """Viewer goal changes preserve cube identity and infer the opposite destination.""" class _Scene(dict): @@ -473,7 +475,16 @@ class _Scene(dict): scene=scene, episode_length_buf=torch.tensor((11, 19)), ) - command._cubes = cubes + for cube_id, cube in enumerate(cubes): + scene[f"cube_{cube_id}"] = cube + if use_pool: + from isaaclab_tasks.contrib.conveyor_franka.conveyor_cube_pool import ConveyorCubePool + + extra_cube = SimpleNamespace(data=SimpleNamespace(root_pos_w=SimpleNamespace(torch=selected_cube_positions))) + cubes[2].data.root_pos_w.torch = 2 * origins - selected_cube_positions + pool = ConveyorCubePool((*cubes, extra_cube), 2, "cpu") + pool.slot_ids[:, 2] = 4 + command._env.conveyor_cube_pool = pool command.target_cube_ids = torch.tensor((0, 1)) command.source_side_ids = torch.tensor((1, 0)) command.held_cube_ids = torch.tensor((0, 1)) @@ -561,3 +572,77 @@ def test_deployment_layout_uses_each_racetrack_straight_run_once(): side_x = torch.where(cube_sides == side_id, cube_x, torch.nan) expected_sum = torch.full((source_side_ids.numel(),), 2 * BELT_CENTER_X, dtype=cube_x.dtype) torch.testing.assert_close(torch.nansum(side_x, dim=1), expected_sum) + + +def test_sort_dispatch_preserves_grasp_and_never_reverses_a_sorted_parcel(monkeypatch): + """Dispatch changes logical slots, never physical state, and leaves completed batches circulating.""" + from isaaclab_tasks.contrib.conveyor_franka.conveyor_cube_pool import ConveyorCubePool + from isaaclab_tasks.contrib.conveyor_franka.conveyor_franka_warehouse_env import ConveyorFrankaWarehouseEnv + from isaaclab_tasks.contrib.conveyor_franka.mdp import sorting + + count = 6 + positions = torch.tensor([[[2.0, 0.8, 0.46]] * count] * 2) + # The wrong-class arrival is outside the initial four slots; the nearby natural carton is already sorted. + positions[0, 0] = torch.tensor([0.7, 0.27, 0.06]) + positions[0, 5] = torch.tensor([0.8, 0.27, 0.06]) + positions[1, 1] = torch.tensor([0.5, -0.1, 0.25]) # Active grasp crossing between loops. + assets = tuple( + SimpleNamespace( + data=SimpleNamespace( + root_pos_w=SimpleNamespace(torch=positions[:, i]), + root_lin_vel_w=SimpleNamespace(torch=torch.zeros(2, 3)), + ) + ) + for i in range(count) + ) + pool = ConveyorCubePool(assets, 2, "cpu") + env = SimpleNamespace( + num_envs=2, + device="cpu", + scene=SimpleNamespace(env_origins=torch.zeros(2, 3)), + conveyor_cube_pool=pool, + _in_workcell=ConveyorFrankaWarehouseEnv._in_workcell, + episode_length_buf=torch.tensor([20, 20]), + ) + command = object.__new__(sorting.ConveyorSortCommand) + command._env = env + command.cfg = sorting.ConveyorSortCommandCfg(parcel_destinations=(0, 1) * 3) + command.parcel_destinations = torch.tensor(command.cfg.parcel_destinations) + command.has_target = torch.tensor([False, True]) + command.target_cube_ids = torch.tensor([0, 1]) + command.source_side_ids = torch.tensor([0, 0]) + command.held_cube_ids = torch.full((2,), -1) + command.subgoal_start_steps = torch.zeros(2, dtype=torch.long) + command.command_counter = torch.ones(2, dtype=torch.long) + command._stable_steps = torch.zeros(2, dtype=torch.long) + command._last_evaluation_steps = torch.full((2,), -1) + command.is_success = torch.zeros(2, dtype=torch.bool) + command.new_success = torch.zeros(2, dtype=torch.bool) + command.pending_success = torch.zeros(2, dtype=torch.bool) + command.metrics = {name: torch.zeros(2) for name in ("sorted_parcels", "batch_complete")} + monkeypatch.setattr(sorting, "physical_cube_acquisition_mask", lambda *args, **kwargs: torch.tensor([False, True])) + before = positions.clone() + command._update_command() + assert command.has_target.tolist() == [True, True] + assert pool.slot_ids[0, command.target_cube_ids[0]] == 5 + assert pool.slot_ids[1, command.target_cube_ids[1]] == 1 + assert command.command[0, -2:].tolist() == [0.0, 1.0] + assert command.metrics["sorted_parcels"].tolist() == [1.0, 0.0] + torch.testing.assert_close(positions, before) + # Every parcel is now on its class's loop. Stable completion must end dispatch, not reverse direction. + positions[0, :, 0] = 0.6 + positions[0, :, 1] = torch.tensor([0.27, -0.27] * 3) + positions[0, :, 2] = 0.06 + command.pending_success[0] = True + command._update_command() + assert command.has_target.tolist() == [False, True] + assert command.metrics["batch_complete"].tolist() == [1.0, 0.0] + command._update_command() + assert not command.has_target[0] + # An idle command must not keep paying the preceding placement reward. + command.has_target.zero_() + command.new_success.fill_(True) + command.is_success.fill_(True) + command.evaluate() + assert not command.new_success.any() + assert not command.is_success.any() diff --git a/uv.lock b/uv.lock index ce54ead3ce49..f425aa60722b 100644 --- a/uv.lock +++ b/uv.lock @@ -1689,7 +1689,7 @@ wheels = [ [[package]] name = "isaaclab" -version = "26.0.0" +version = "28.0.0" source = { editable = "source/isaaclab" } [[package]] @@ -2082,7 +2082,7 @@ requires-dist = [ [[package]] name = "isaaclab-newton" -version = "6.5.0" +version = "7.0.1" source = { editable = "source/isaaclab_newton" } dependencies = [ { name = "isaaclab" }, @@ -2093,7 +2093,7 @@ requires-dist = [{ name = "isaaclab", editable = "source/isaaclab" }] [[package]] name = "isaaclab-ov" -version = "3.2.0" +version = "3.3.2" source = { editable = "source/isaaclab_ov" } dependencies = [ { name = "isaaclab" }, @@ -2108,7 +2108,7 @@ requires-dist = [ [[package]] name = "isaaclab-physx" -version = "7.2.0" +version = "7.2.2" source = { editable = "source/isaaclab_physx" } dependencies = [ { name = "isaaclab" }, @@ -2130,7 +2130,7 @@ requires-dist = [{ name = "isaaclab", editable = "source/isaaclab" }] [[package]] name = "isaaclab-rl" -version = "1.1.0" +version = "1.2.0" source = { editable = "source/isaaclab_rl" } dependencies = [ { name = "isaaclab" }, @@ -2147,7 +2147,7 @@ requires-dist = [ [[package]] name = "isaaclab-tasks" -version = "21.0.1" +version = "21.1.0" source = { editable = "source/isaaclab_tasks" } dependencies = [ { name = "isaaclab" }, @@ -2192,7 +2192,7 @@ requires-dist = [{ name = "isaaclab", editable = "source/isaaclab" }] [[package]] name = "isaaclab-visualizers" -version = "1.11.0" +version = "1.12.1" source = { editable = "source/isaaclab_visualizers" } dependencies = [ { name = "isaaclab" }, From 5b394c628f5527d9d66a6eabbfc3ef487cf80886 Mon Sep 17 00:00:00 2001 From: Maximilian Krause Date: Thu, 24 Sep 2026 12:03:30 +0200 Subject: [PATCH 20/23] Fix public conveyor documentation links --- docs/source/setup/conveyor_franka.rst | 2 +- .../isaaclab_tasks/contrib/conveyor_franka/README.md | 5 +++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/docs/source/setup/conveyor_franka.rst b/docs/source/setup/conveyor_franka.rst index f9eeee83dd8b..42b478fd2368 100644 --- a/docs/source/setup/conveyor_franka.rst +++ b/docs/source/setup/conveyor_franka.rst @@ -94,5 +94,5 @@ four-cube training and CPU reference configurations retain their compact layout. `Download the warehouse preview `__. The task's -`README `__ +:download:`README <../../../source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/README.md>` describes the USD assets, collision ownership, slot adapter, and sorting metrics. diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/README.md b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/README.md index 3400cfb41407..75d79568dfae 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/README.md +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/README.md @@ -149,11 +149,12 @@ on first use, so the first launch takes longer. ### Asset and visual references -The asset survey covered [SimReady Central](https://simready-central.nvidia.com/), +The asset survey covered SimReady Central (`simready-central.nvidia.com`), `omniverse://ov-isaac-dev.nvidia.com/Isaac/SimReady/Industrial/Warehouse`, `Isaac/Environments/{Digital_Twin_Warehouse,Modular_Warehouse}`, `Isaac/Props/Conveyors`, and `NVIDIA/Assets/DigitalTwin/Assets/Warehouse`. The composition references publicly accessible -Omniverse counterparts so playback does not require internal Nucleus credentials. +Omniverse counterparts so playback does not require internal Nucleus credentials. Public catalog +usage is described in the [SimReady Explorer documentation](https://docs.omniverse.nvidia.com/extensions/latest/ext_core/ext_browser-extensions/simready-explorer.html). Selected assets are the Omniverse A03/A09/A12/A24/A29/A38 conveyors, `RackLarge_A1`, SimReady `bulkstoragerack_a01` and `cardbox_a1`, the Isaac packing table, and loaded pallets. NVIDIA assets From db325c22f5767f92d468d48ab4ed66e2089f8440 Mon Sep 17 00:00:00 2001 From: Maximilian Krause Date: Thu, 24 Sep 2026 12:25:00 +0200 Subject: [PATCH 21/23] Document Kit geometry streaming override for warehouse playback --- docs/source/setup/conveyor_franka.rst | 7 +++++-- .../isaaclab_tasks/contrib/conveyor_franka/README.md | 6 ++++-- 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/docs/source/setup/conveyor_franka.rst b/docs/source/setup/conveyor_franka.rst index 42b478fd2368..327d1582178b 100644 --- a/docs/source/setup/conveyor_franka.rst +++ b/docs/source/setup/conveyor_franka.rst @@ -35,10 +35,13 @@ Use the standard Isaac Lab installation with the Isaac Sim extra for Kit/RTX vis uv run --extra isaacsim isaaclab play --rl_library rsl_rl \ --task IsaacContrib-Conveyor-Franka-Newton-Play-v0 \ - --checkpoint https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/6.1/Isaac/IsaacLab/PretrainedCheckpoints/rsl_rl/IsaacContrib-Conveyor-Franka-Newton-v0_newtonmjwarp_none_rsl_rl.pt --num_envs 1 --device cuda:0 --viz kit --real-time + --checkpoint https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/6.1/Isaac/IsaacLab/PretrainedCheckpoints/rsl_rl/IsaacContrib-Conveyor-Franka-Newton-v0_newtonmjwarp_none_rsl_rl.pt --num_envs 1 --device cuda:0 --viz kit --real-time \ + --kit_args=--/UJITSO/geometry=false The first launch downloads the referenced Omniverse assets. The warehouse uses USD-authored -materials and lighting; Kit/RTX is the intended viewer. For compact, lightweight playback: +materials and lighting; Kit/RTX is the intended viewer. The launch override disables experimental +geometry streaming, including saved Kit preferences, which can hide meshes updated through Fabric. +For compact, lightweight playback: .. code-block:: bash diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/README.md b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/README.md index 75d79568dfae..7304702ce77b 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/README.md +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/README.md @@ -113,14 +113,16 @@ Use **Kit/RTX** to see the authored MDL textures, USD lights, and background ani ```bash DISPLAY=:1 uv run --extra isaacsim isaaclab play --rl_library rsl_rl \ --task IsaacContrib-Conveyor-Franka-Newton-Play-v0 \ - --checkpoint https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/6.1/Isaac/IsaacLab/PretrainedCheckpoints/rsl_rl/IsaacContrib-Conveyor-Franka-Newton-v0_newtonmjwarp_none_rsl_rl.pt --num_envs 1 --device cuda:0 --viz kit --real-time + --checkpoint https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/6.1/Isaac/IsaacLab/PretrainedCheckpoints/rsl_rl/IsaacContrib-Conveyor-Franka-Newton-v0_newtonmjwarp_none_rsl_rl.pt --num_envs 1 --device cuda:0 --viz kit --real-time \ + --kit_args=--/UJITSO/geometry=false ``` Kit renders the USD directly, so the Play configuration excludes visual-only meshes from the Newton model (`sim.physics.load_visual_shapes=False`). This avoids importing warehouse dressing into the physics model. For a static approximation in `--viz newton_gl`, explicitly pass `env.sim.physics.load_visual_shapes=True`; materials and lighting are simplified in that viewer. -Presentation defaults to one environment. Asset references and textures are downloaded and cached +The launch override disables experimental geometry streaming to prevent disappearing meshes with +Fabric transforms. Presentation defaults to one environment. Asset references and textures are downloaded and cached on first use, so the first launch takes longer. ### Editing the presentation From 31f212060fb9e376a25d1ca8c310d4ceb8899ed4 Mon Sep 17 00:00:00 2001 From: Maximilian Krause Date: Thu, 24 Sep 2026 12:30:26 +0200 Subject: [PATCH 22/23] Document the validated Kit recording configuration --- docs/source/setup/conveyor_franka.rst | 3 +++ 1 file changed, 3 insertions(+) diff --git a/docs/source/setup/conveyor_franka.rst b/docs/source/setup/conveyor_franka.rst index 327d1582178b..eb6e6b5d2385 100644 --- a/docs/source/setup/conveyor_franka.rst +++ b/docs/source/setup/conveyor_franka.rst @@ -41,6 +41,9 @@ Use the standard Isaac Lab installation with the Isaac Sim extra for Kit/RTX vis The first launch downloads the referenced Omniverse assets. The warehouse uses USD-authored materials and lighting; Kit/RTX is the intended viewer. The launch override disables experimental geometry streaming, including saved Kit preferences, which can hide meshes updated through Fabric. +To record this view, append ``--video --video_length 1440 env.sim.physics.use_cuda_graph=False``. +Disable CUDA graphs for this Kit recording path; compact training retains its graph-enabled default. + For compact, lightweight playback: .. code-block:: bash From 9609dc2d6dbe391cf1b8c06e1c987cc9caac1fe3 Mon Sep 17 00:00:00 2001 From: Maximilian Krause Date: Thu, 24 Sep 2026 13:25:34 +0200 Subject: [PATCH 23/23] Clarify shared-policy racetrack and sorting task variants --- docs/source/setup/conveyor_franka.rst | 35 ++++++++------- .../maximiliank-conveyor-franka.minor.rst | 3 +- .../contrib/conveyor_franka/README.md | 45 +++++++++++-------- .../contrib/conveyor_franka/__init__.py | 4 +- .../contrib/test_conveyor_franka_asset_cfg.py | 24 ++++++++-- 5 files changed, 69 insertions(+), 42 deletions(-) diff --git a/docs/source/setup/conveyor_franka.rst b/docs/source/setup/conveyor_franka.rst index eb6e6b5d2385..5c4188b6b628 100644 --- a/docs/source/setup/conveyor_franka.rst +++ b/docs/source/setup/conveyor_franka.rst @@ -1,30 +1,33 @@ Conveyor Franka (Contrib) ========================= -A Franka transfers parcels between two moving conveyors. The compact task supports Newton -GPU training and native PhysX CPU playback. An optional USD warehouse demonstrates the same -checkpoint with textured cartons, elevated returns, gravity infeeds, and color sorting. +Choose between the original four-cube racetrack task and the warehouse sorting task. +Both use the same pretrained Franka policy and shared manipulation code. The sorter extends +the base environment with textured cartons, elevated returns, gravity infeeds, and color dispatch. .. image:: ../_static/conveyor_franka.jpg :alt: Franka sorting colored cartons between two conveyors in a warehouse :width: 100% -.. list-table:: Available variants +.. list-table:: Two tasks, one policy :header-rows: 1 :widths: 22 24 54 - * - Variant - - Physics / device - - Intended use - * - Newton - - Newton MJWarp / GPU - - Train or play the original four-cube transfer task. - * - Newton Play - - Newton MJWarp / GPU - - Play the warehouse demonstration with 24 physical parcels. - * - PhysX CPU - - Isaac Sim PhysX / CPU - - Compare native surface-velocity behavior with an explicit checkpoint. + * - Task + - Inventory + - Behavior + * - Racetrack transfer + - Four numbered cubes + - Original two closed racetracks; continuous alternating transfers. + * - Warehouse sorting + - 24 colored parcels + - Extended circulating conveyors; blue/green on one loop, orange/purple on the other. + +Both run on Newton GPU: select ``IsaacContrib-Conveyor-Franka-Newton-v0`` for the original +racetracks or ``IsaacContrib-Conveyor-Franka-Newton-Play-v0`` for sorting. The original task +also has a native PhysX CPU backend, described below. Sorting reuses the base configuration, +agent configuration, and manipulation terms; only the warehouse adds parcel-slot reassignment +and color-based dispatch. Run the pretrained policy ------------------------- diff --git a/source/isaaclab_tasks/changelog.d/maximiliank-conveyor-franka.minor.rst b/source/isaaclab_tasks/changelog.d/maximiliank-conveyor-franka.minor.rst index 7f0a35c512b5..5b78762c693f 100644 --- a/source/isaaclab_tasks/changelog.d/maximiliank-conveyor-franka.minor.rst +++ b/source/isaaclab_tasks/changelog.d/maximiliank-conveyor-franka.minor.rst @@ -9,7 +9,8 @@ Added original manipulation geometry and 123-observation, eight-action policy interface. Complete-batch reliability with the unchanged policy was not established. Use ``--viz kit`` for authored visuals and the explicit base-task checkpoint URL documented in the conveyor guide. -* Added a user guide, preview, and environment-browser entries for the conveyor variants. +* Added a user guide, preview, and environment-browser entries distinguishing the original + four-cube racetrack task from warehouse sorting with the same pretrained policy. Changed ^^^^^^^ diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/README.md b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/README.md index 7304702ce77b..82409d287c1e 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/README.md +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/README.md @@ -7,18 +7,23 @@ SPDX-License-Identifier: BSD-3-Clause # Conveyor Franka -This package provides checkpoint-compatible Newton and PhysX variants of a manager-based task in -which a Franka transfers four numbered cubes between two counter-rotating racetrack conveyors. -Both variants preserve the same ordered eight-dimensional action space, policy observations, -commands, rewards, reset recipes, 120 Hz physics step, and 60 Hz policy rate. +Choose between two tasks using the same pretrained Franka policy: + +| Task | Layout and behavior | Newton task ID | +| --- | --- | --- | +| Racetrack transfer | Original two closed racetracks and four numbered cubes; continuous alternating transfers | `IsaacContrib-Conveyor-Franka-Newton-v0` | +| Warehouse sorting | Current extended conveyors and 24 colored parcels; two colors per circulating conveyor | `IsaacContrib-Conveyor-Franka-Newton-Play-v0` | + +The sorter inherits the racetrack environment and configuration. Both reuse the same action, +observation, reward, and placement logic and the same RSL-RL agent configuration. Sorting adds +class dispatch and a four-slot parcel adapter; it needs no separately trained policy. The original +task keeps its compact geometry and four fixed cube identities. Both use 123 observations, +eight actions, 120 Hz physics, and a 60 Hz policy rate. ## Backend support -| Variant | Device | Intended use | -| --- | --- | --- | -| Newton | CUDA | Train and play the four-cube task | -| Newton Play | CUDA | Play the 24-parcel USD warehouse demonstration | -| PhysX CPU | CPU only | Native surface-velocity reference and checkpoint playback | +Both tasks run on Newton GPU. `IsaacContrib-Conveyor-Franka-PhysX-CPU-v0` provides a CPU-only +native PhysX reference for the original four-cube racetrack task. The PhysX task rejects CUDA during configuration validation. In the supported Isaac Sim runtime, enabling the native surface-velocity contact-modification path under GPU dynamics can drop the belt @@ -35,7 +40,7 @@ pipeline lives in `isaaclab_newton.physics.surface_velocity`, while PhysX schema attribute control live in `isaaclab_physx.physics.surface_velocity`. The task package owns only the racetrack geometry, backend lifecycle selection, and task-level commands. -## Newton GPU playback +## Original racetrack task Newton is kitless and supports the lightweight GL viewer: @@ -51,7 +56,17 @@ backend. To evaluate another policy, replace `pretrained` with an explicit check PhysX task resolves a different backend-specific artifact name, so transferring this Newton policy to PhysX currently requires the explicit local checkpoint path shown below. -For presentation, use the checkpoint-compatible Play variant. The two parallel manipulation +Training uses the same task ID and defaults to 256 environments: + +```bash +uv run isaaclab train --rl_library rsl_rl \ + --task IsaacContrib-Conveyor-Franka-Newton-v0 \ + --num_envs 256 --device cuda:0 +``` + +## Warehouse sorting task + +Select the `Newton-Play` task for warehouse sorting with the same checkpoint. The two parallel manipulation straights and their adjoining 90-degree bends retain their original positions, widths, radii, and 0.35 m/s surface speed. Beyond these fixed sections, two short rising feeds climb 0.10 m at less than 10 degrees and join a shared elevated deck. Guides keep the two return lanes assigned through the upper split, @@ -167,14 +182,6 @@ parallel transport, elevation changes, rack storage, packing zones, and clear ma The induction and recirculation layout also follows the concepts in [Dematic’s sortation overview](https://www.dematic.com/content/dam/dematic/downloads/whitepapers/NA_WP-1015_Sorting-Out-Sortation.pdf). -Training uses the same task ID and defaults to 256 environments: - -```bash -uv run isaaclab train --rl_library rsl_rl \ - --task IsaacContrib-Conveyor-Franka-Newton-v0 \ - --num_envs 256 --device cuda:0 -``` - ## PhysX CPU playback The native PhysX variant requires an Isaac Sim-enabled launch and an explicit CPU device. One diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/__init__.py index 158fffa2b25d..00b70df752b7 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/conveyor_franka/__init__.py @@ -3,7 +3,7 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Backend-selectable conveyor scene with a Franka robot.""" +"""Four-cube racetrack transfer and warehouse sorting tasks sharing a Franka policy.""" import gymnasium as gym @@ -20,7 +20,7 @@ ) gym.register( - # This presentation variant uses the base Newton checkpoint through an explicit URL. + # Warehouse sorting reuses the racetrack policy through an explicit checkpoint URL. id="IsaacContrib-Conveyor-Franka-Newton-Play-v0", entry_point=f"{__name__}.conveyor_franka_warehouse_env:ConveyorFrankaWarehouseEnv", disable_env_checker=True, diff --git a/source/isaaclab_tasks/test/contrib/test_conveyor_franka_asset_cfg.py b/source/isaaclab_tasks/test/contrib/test_conveyor_franka_asset_cfg.py index 70577e118509..ebff50061ca9 100644 --- a/source/isaaclab_tasks/test/contrib/test_conveyor_franka_asset_cfg.py +++ b/source/isaaclab_tasks/test/contrib/test_conveyor_franka_asset_cfg.py @@ -32,14 +32,30 @@ ) -def test_a09_a12_play_task_reuses_the_newton_policy_contract() -> None: - """The visual task is registered as a Play variant with unchanged policy-facing config.""" +def test_racetrack_and_sorting_tasks_share_the_policy_contract() -> None: + """Sorting extends the four-cube task without changing its scene or policy configuration.""" task = gym.spec("IsaacContrib-Conveyor-Franka-Newton-Play-v0") - cfg = ConveyorFrankaA09A12EnvCfg() + base_task = gym.spec("IsaacContrib-Conveyor-Franka-Newton-v0") base_cfg = ConveyorFrankaEnvCfg() + base_scene = base_cfg.scene.to_dict() + cfg = ConveyorFrankaA09A12EnvCfg() + cfg.scene._configure_route_assets(cfg.commands.transfer.parcel_colors) assert task.kwargs["env_cfg_entry_point"].endswith(":ConveyorFrankaA09A12EnvCfg") - assert task.id.replace("-Play", "") == "IsaacContrib-Conveyor-Franka-Newton-v0" + assert base_task.kwargs["env_cfg_entry_point"].endswith(":ConveyorFrankaEnvCfg") + assert task.kwargs["rsl_rl_cfg_entry_point"] == base_task.kwargs["rsl_rl_cfg_entry_point"] + assert base_cfg.scene.to_dict() == base_scene + assert base_cfg.scene.to_dict() == ConveyorFrankaEnvCfg().scene.to_dict() + assert [name for name in vars(base_cfg.scene) if name.startswith("cube_")] == [ + "cube_0", + "cube_1", + "cube_2", + "cube_3", + ] + assert base_cfg.conveyor_force.transported_body_count_per_env == 4 + assert base_cfg.commands.transfer.class_type.__name__ == "ConveyorTransferCommand" + assert cfg.commands.transfer.class_type.__name__ == "ConveyorSortCommand" + assert cfg.conveyor_force.transported_body_count_per_env == 24 assert cfg.scene.num_envs == 1 assert cfg.actions == base_cfg.actions assert cfg.observations == base_cfg.observations