diff --git a/scripts/run_ovphysx.sh b/scripts/run_ovphysx.sh new file mode 100755 index 000000000000..72d18a1bec82 --- /dev/null +++ b/scripts/run_ovphysx.sh @@ -0,0 +1,71 @@ +#!/bin/bash +# Run ovphysx in Kit's Python with its bundled libcarb.so preloaded. +# Use when ovphysx is installed into Kit's Python. +# +# Usage: ./scripts/run_ovphysx.sh [your_script.py or -m pytest ...] +set -e + +ISAACLAB_PATH="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)" +ISAAC_DIR="${ISAACLAB_PATH}/_isaac_sim" + +# Match python.sh so Kit extensions/resources resolve during training too. +export CARB_APP_PATH="${CARB_APP_PATH:-${ISAAC_DIR}/kit}" +export ISAAC_PATH="${ISAAC_PATH:-${ISAAC_DIR}}" +export EXP_PATH="${EXP_PATH:-${ISAAC_DIR}/apps}" + +# Source the Python environment setup (sets PYTHONPATH, LD_LIBRARY_PATH) +# but do NOT use python.sh which sets LD_PRELOAD +source "${ISAAC_DIR}/setup_python_env.sh" + +# Preload ovphysx's own libcarb.so so its Carbonite framework wins the +# SONAME race against any other libcarb.so present in the process. +_ovphysx_libcarb="" +for _sp in "${ISAAC_DIR}"/kit/python/lib/python3.*/site-packages/ovphysx/plugins/libcarb.so; do + if [ -f "${_sp}" ]; then + _ovphysx_libcarb="${_sp}" + break + fi +done +if [ -n "${_ovphysx_libcarb}" ]; then + export LD_PRELOAD="${_ovphysx_libcarb}" +else + export LD_PRELOAD="" +fi +unset _ovphysx_libcarb + +# Ensure pxr (OpenUSD Python bindings) is on PYTHONPATH. +# setup_python_env.sh may not include the packman USD path after rebuilds. +# Search order: IsaacSim's bundled site-packages first, then the Python +# environment's own site-packages (covers pip-installed OpenUSD). +_pxr_found=false +for usd_dir in "${ISAAC_DIR}"/kit/python/lib/python3.*/site-packages/usd_core.libs/../.. \ + "${ISAAC_DIR}"/kit/python/lib/python3.*/site-packages; do + if [ -d "${usd_dir}/pxr" ]; then + export PYTHONPATH="${usd_dir}:${PYTHONPATH}" + _pxr_found=true + break + fi +done +if [ "${_pxr_found}" = false ]; then + # Last resort: ask Python itself where pxr lives + _pxr_path=$("${ISAAC_DIR}/kit/python/bin/python3" -c "import pxr, os; print(os.path.dirname(os.path.dirname(pxr.__file__)))" 2>/dev/null) + if [ -n "${_pxr_path}" ] && [ -d "${_pxr_path}/pxr" ]; then + export PYTHONPATH="${_pxr_path}:${PYTHONPATH}" + fi +fi +unset _pxr_found _pxr_path + +# Add all isaaclab source packages to PYTHONPATH so editable installs work +for pkg in isaaclab isaaclab_ovphysx isaaclab_tasks isaaclab_rl isaaclab_physx isaaclab_newton isaaclab_assets isaaclab_contrib; do + if [ -d "${ISAACLAB_PATH}/source/${pkg}" ]; then + export PYTHONPATH="${ISAACLAB_PATH}/source/${pkg}:${PYTHONPATH}" + fi +done + +# Match python.sh default for Kit app resource discovery. +export RESOURCE_NAME="${RESOURCE_NAME:-IsaacSim}" + +# Use the Python binary directly +PYTHON_EXE="${ISAAC_DIR}/kit/python/bin/python3" + +exec "${PYTHON_EXE}" "$@" diff --git a/source/isaaclab/isaaclab/assets/asset_base.py b/source/isaaclab/isaaclab/assets/asset_base.py index 654d199f0f99..f5f121c5ad2b 100644 --- a/source/isaaclab/isaaclab/assets/asset_base.py +++ b/source/isaaclab/isaaclab/assets/asset_base.py @@ -359,10 +359,10 @@ def _invoke(callback_name, event): PhysicsEvent.STOP, order=10, ) - # Optional: prim deletion (only supported by PhysX backend) + # Optional: prim deletion (only supported by Kit PhysX backend, not ovphysx) self._prim_deletion_handle = None physics_backend = physics_mgr_cls.__name__.lower() - if "physx" in physics_backend: + if physics_backend.startswith("physx"): from isaaclab_physx.physics import IsaacEvents self._prim_deletion_handle = physics_mgr_cls.register_callback( diff --git a/source/isaaclab/isaaclab/scene/interactive_scene.py b/source/isaaclab/isaaclab/scene/interactive_scene.py index 7cd0eee7433e..a4ac4424f485 100644 --- a/source/isaaclab/isaaclab/scene/interactive_scene.py +++ b/source/isaaclab/isaaclab/scene/interactive_scene.py @@ -142,11 +142,15 @@ def __init__(self, cfg: InteractiveSceneCfg): self.physics_backend = self.sim.physics_manager.__name__.lower() visualizer_clone_fn = None requested_viz_types = set(self.sim.resolve_visualizer_types()) - if "physx" in self.physics_backend: + if self.physics_backend.startswith("ovphysx"): + from isaaclab_ovphysx.cloner import ovphysx_replicate + + physics_clone_fn = ovphysx_replicate + elif self.physics_backend.startswith("physx"): from isaaclab_physx.cloner import physx_replicate physics_clone_fn = physx_replicate - elif "newton" in self.physics_backend: + elif self.physics_backend.startswith("newton"): from isaaclab_newton.cloner import newton_physics_replicate physics_clone_fn = newton_physics_replicate @@ -163,6 +167,10 @@ def __init__(self, cfg: InteractiveSceneCfg): device=self.device, physics_clone_fn=physics_clone_fn, visualizer_clone_fn=None, + # For ovphysx: env_1..N are created by physx.clone() in the physics + # runtime after add_usd(). USD replication of the asset hierarchy + # to env_1..N is skipped — only env_0 needs physics prims in the USD. + clone_usd=not self.physics_backend.startswith("ovphysx"), ) # create source prim @@ -206,6 +214,7 @@ def __init__(self, cfg: InteractiveSceneCfg): if has_scene_cfg_entities: self.clone_environments(copy_from_source=(not self.cfg.replicate_physics)) # Collision filtering is PhysX-specific (PhysxSchema.PhysxSceneAPI) + # Intentionally matches both physx and ovphysx (both are PhysX-based) if self.cfg.filter_collisions and "physx" in self.physics_backend: self.filter_collisions(self._global_prim_paths) @@ -218,6 +227,7 @@ def clone_environments(self, copy_from_source: bool = False): may increase). Defaults to False. """ # PhysX-only: set env id bit count for replicated physics. Newton handles env separation in its own API. + # Intentionally matches both physx and ovphysx (both are PhysX-based) if self.cfg.replicate_physics and "physx" in self.physics_backend: prim = self.stage.GetPrimAtPath("/physicsScene") prim.CreateAttribute("physxScene:envIdInBoundsBitCount", Sdf.ValueTypeNames.Int).Set(4) @@ -235,12 +245,12 @@ def clone_environments(self, copy_from_source: bool = False): self._default_env_origins, ) - if not copy_from_source: - # skip physx cloning, this means physx will walk and parse the stage one by one faithfully + if not copy_from_source and self.cloner_cfg.physics_clone_fn is not None: self.cloner_cfg.physics_clone_fn(self.stage, *replicate_args, device=self.cloner_cfg.device) if self.cloner_cfg.visualizer_clone_fn is not None: self.cloner_cfg.visualizer_clone_fn(self.stage, *replicate_args, device=self.cloner_cfg.device) - cloner.usd_replicate(self.stage, *replicate_args) + if self.cloner_cfg.clone_usd: + cloner.usd_replicate(self.stage, *replicate_args) def _sensor_renderer_types(self) -> list[str]: """Return renderer type names used by scene sensors.""" diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index a7fd7d632d3e..5e05dab92a46 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -289,10 +289,13 @@ def _init_usd_physics_scene(self) -> None: UsdGeom.SetStageMetersPerUnit(self.stage, 1.0) UsdPhysics.SetStageKilogramsPerUnit(self.stage, 1.0) - # Find and delete any existing physics scene - for prim in self.stage.Traverse(): - if prim.GetTypeName() == "PhysicsScene": - sim_utils.delete_prim(prim.GetPath().pathString, stage=self.stage) + # Find and delete any existing physics scene. + # Collect paths first to avoid iterator invalidation during deletion. + physics_scene_paths = [ + prim.GetPath().pathString for prim in self.stage.Traverse() if prim.GetTypeName() == "PhysicsScene" + ] + for path in physics_scene_paths: + sim_utils.delete_prim(path, stage=self.stage) # Create a new physics scene if self.stage.GetPrimAtPath(cfg.physics_prim_path).IsValid(): diff --git a/source/isaaclab/isaaclab/utils/backend_utils.py b/source/isaaclab/isaaclab/utils/backend_utils.py index 2e4ee55c62df..9f69a66aa04d 100644 --- a/source/isaaclab/isaaclab/utils/backend_utils.py +++ b/source/isaaclab/isaaclab/utils/backend_utils.py @@ -44,9 +44,11 @@ def _get_backend(cls, *args, **kwargs) -> str: from isaaclab.sim.simulation_context import SimulationContext manager_name = SimulationContext.instance().physics_manager.__name__.lower() - if "newton" in manager_name: + if manager_name.startswith("newton"): return "newton" - if "physx" in manager_name: + if manager_name.startswith("ovphysx"): + return "ovphysx" + if manager_name.startswith("physx"): return "physx" else: raise ValueError(f"Unknown physics manager: {manager_name}") diff --git a/source/isaaclab/test/assets/test_articulation_iface.py b/source/isaaclab/test/assets/test_articulation_iface.py index 64842ed7273f..14af782c2a5b 100644 --- a/source/isaaclab/test/assets/test_articulation_iface.py +++ b/source/isaaclab/test/assets/test_articulation_iface.py @@ -13,16 +13,25 @@ The setup is a bit convoluted so that we can run these tests without requiring Isaac Sim or GPU simulation. """ -"""Launch Isaac Sim Simulator first.""" +"""Launch Isaac Sim Simulator first (when available).""" -from isaaclab.app import AppLauncher +import os +import sys +from unittest.mock import MagicMock -HEADLESS = True +# When running kitless (e.g., ovphysx backend via run_ovphysx.sh), AppLauncher +# will try to boot Kit and hang. Skip it entirely when LD_PRELOAD is cleared +# (the signature of run_ovphysx.sh) or when EXP_PATH is not set. +_kitless = os.environ.get("LD_PRELOAD", "") == "" and "EXP_PATH" not in os.environ -# launch omniverse app -simulation_app = AppLauncher(headless=True).app +if not _kitless: + from isaaclab.app import AppLauncher -from unittest.mock import MagicMock + simulation_app = AppLauncher(headless=True).app +else: + simulation_app = None + for _mod in ("isaacsim.core", "isaacsim.core.simulation_manager"): + sys.modules.setdefault(_mod, MagicMock()) import numpy as np import pytest @@ -32,9 +41,10 @@ from isaaclab.assets.articulation.articulation_cfg import ArticulationCfg from isaaclab.test.mock_interfaces.utils import MockWrenchComposer -# Mock SimulationManager.get_physics_sim_view() to return a mock object with gravity -# This is needed because the Data classes call SimulationManager.get_physics_sim_view().get_gravity() -# but there's no actual physics scene when running unit tests +# Mock SimulationManager.get_physics_sim_view() to return a mock object with gravity. +# This is needed because the PhysX Data classes call +# SimulationManager.get_physics_sim_view().get_gravity() but there's no actual +# physics scene when running unit tests. _mock_physics_sim_view = MagicMock() _mock_physics_sim_view.get_gravity.return_value = (0.0, 0.0, -9.81) @@ -66,6 +76,15 @@ except ImportError: pass +try: + from isaaclab_ovphysx.assets.articulation.articulation import Articulation as OvPhysxArticulation + from isaaclab_ovphysx.assets.articulation.articulation_data import ArticulationData as OvPhysxArticulationData + from isaaclab_ovphysx.test.mock_interfaces.views import MockOvPhysxBindingSet + + BACKENDS.append("ovphysx") +except ImportError: + pass + def create_physx_articulation( num_instances: int = 2, @@ -169,6 +188,87 @@ def create_physx_articulation( return articulation, mock_view +def create_ovphysx_articulation( + num_instances: int = 2, + num_joints: int = 6, + num_bodies: int = 7, + num_fixed_tendons: int = 0, + num_spatial_tendons: int = 0, + device: str = "cuda:0", +): + """Create a test OvPhysX Articulation instance with mocked tensor bindings.""" + joint_names = [f"joint_{i}" for i in range(num_joints)] + body_names = [f"body_{i}" for i in range(num_bodies)] + + articulation = object.__new__(OvPhysxArticulation) + + articulation.cfg = ArticulationCfg( + prim_path="/World/Robot", + soft_joint_pos_limit_factor=1.0, + actuators={}, + ) + + # Create mock binding set + mock_bindings = MockOvPhysxBindingSet( + num_instances=num_instances, + num_joints=num_joints, + num_bodies=num_bodies, + is_fixed_base=False, + joint_names=joint_names, + body_names=body_names, + num_fixed_tendons=num_fixed_tendons, + num_spatial_tendons=num_spatial_tendons, + ) + mock_bindings.set_random_data() + + fixed_tendon_names = [f"fixed_tendon_{i}" for i in range(num_fixed_tendons)] + spatial_tendon_names = [f"spatial_tendon_{i}" for i in range(num_spatial_tendons)] + + object.__setattr__(articulation, "_device", device) + object.__setattr__(articulation, "_ovphysx", MagicMock()) + object.__setattr__(articulation, "_bindings", mock_bindings.bindings) + object.__setattr__(articulation, "_num_instances", num_instances) + object.__setattr__(articulation, "_num_joints", num_joints) + object.__setattr__(articulation, "_num_bodies", num_bodies) + object.__setattr__(articulation, "_is_fixed_base", False) + object.__setattr__(articulation, "_joint_names", joint_names) + object.__setattr__(articulation, "_body_names", body_names) + object.__setattr__(articulation, "_fixed_tendon_names", fixed_tendon_names) + object.__setattr__(articulation, "_spatial_tendon_names", spatial_tendon_names) + object.__setattr__(articulation, "_num_fixed_tendons", num_fixed_tendons) + object.__setattr__(articulation, "_num_spatial_tendons", num_spatial_tendons) + + # Create ArticulationData + data = OvPhysxArticulationData(mock_bindings.bindings, device) + data._num_instances = num_instances + data._num_joints = num_joints + data._num_bodies = num_bodies + data._num_fixed_tendons = num_fixed_tendons + data._num_spatial_tendons = num_spatial_tendons + data._is_fixed_base = False + data.body_names = body_names + data.joint_names = joint_names + data.fixed_tendon_names = fixed_tendon_names + data.spatial_tendon_names = spatial_tendon_names + data._create_buffers() + object.__setattr__(articulation, "_data", data) + + # Wrench composers + mock_inst_wrench = MockWrenchComposer(articulation) + mock_perm_wrench = MockWrenchComposer(articulation) + object.__setattr__(articulation, "_instantaneous_wrench_composer", mock_inst_wrench) + object.__setattr__(articulation, "_permanent_wrench_composer", mock_perm_wrench) + + # Prevent __del__ / _clear_callbacks from raising + object.__setattr__(articulation, "_initialize_handle", None) + object.__setattr__(articulation, "_invalidate_initialize_handle", None) + object.__setattr__(articulation, "_prim_deletion_handle", None) + object.__setattr__(articulation, "_debug_vis_handle", None) + object.__setattr__(articulation, "actuators", {}) + + return articulation, mock_bindings + + def create_newton_articulation( num_instances: int = 2, num_joints: int = 6, @@ -326,6 +426,10 @@ def get_articulation( return create_physx_articulation( num_instances, num_joints, num_bodies, num_fixed_tendons, num_spatial_tendons, device ) + elif backend == "ovphysx": + return create_ovphysx_articulation( + num_instances, num_joints, num_bodies, num_fixed_tendons, num_spatial_tendons, device + ) elif backend == "newton": return create_newton_articulation(num_instances, num_joints, num_bodies, device) elif backend.lower() == "mock": diff --git a/source/isaaclab/test/scene/test_interactive_scene.py b/source/isaaclab/test/scene/test_interactive_scene.py index 087474baa59e..20b96aaacd4f 100644 --- a/source/isaaclab/test/scene/test_interactive_scene.py +++ b/source/isaaclab/test/scene/test_interactive_scene.py @@ -159,6 +159,7 @@ def _usd_replicate(stage, *args, **kwargs): device="cpu", physics_clone_fn=_physics_clone_fn, visualizer_clone_fn=_visualizer_clone_fn, + clone_usd=True, ) monkeypatch.setattr("isaaclab.scene.interactive_scene.cloner.usd_replicate", _usd_replicate) diff --git a/source/isaaclab_ovphysx/config/extension.toml b/source/isaaclab_ovphysx/config/extension.toml new file mode 100644 index 000000000000..11b3322f2f1d --- /dev/null +++ b/source/isaaclab_ovphysx/config/extension.toml @@ -0,0 +1,21 @@ +[package] + +# Note: Semantic Versioning is used: https://semver.org/ +version = "0.1.0" + +# Description +title = "OvPhysX simulation interfaces for IsaacLab core package" +description = "Extension providing IsaacLab with ovphysx/TensorBindingsAPI specific abstractions." +readme = "docs/README.md" +repository = "https://github.com/isaac-sim/IsaacLab" +category = "robotics" +keywords = ["robotics", "simulation", "ovphysx", "tensorbindingsapi"] + +[dependencies] +"isaaclab" = {} + +[core] +reloadable = false + +[[python.module]] +name = "isaaclab_ovphysx" diff --git a/source/isaaclab_ovphysx/docs/CHANGELOG.rst b/source/isaaclab_ovphysx/docs/CHANGELOG.rst new file mode 100644 index 000000000000..b177752442d3 --- /dev/null +++ b/source/isaaclab_ovphysx/docs/CHANGELOG.rst @@ -0,0 +1,10 @@ +Changelog +--------- + +0.1.0 (2026-04-20) +~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Initial release of the ``isaaclab_ovphysx`` extension. diff --git a/source/isaaclab_ovphysx/isaaclab_ovphysx/__init__.py b/source/isaaclab_ovphysx/isaaclab_ovphysx/__init__.py new file mode 100644 index 000000000000..69b5f5512333 --- /dev/null +++ b/source/isaaclab_ovphysx/isaaclab_ovphysx/__init__.py @@ -0,0 +1,25 @@ +# 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 + +"""Package containing the ovphysx/TensorBindingsAPI simulation interfaces for IsaacLab.""" + +import os + +import toml + +ISAACLAB_OVPHYSX_EXT_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), "../")) +"""Path to the extension source directory.""" + +# Find config/extension.toml: bundled inside the package (wheel install) or in the +# parent directory (editable install). +_pkg_dir = os.path.dirname(os.path.abspath(__file__)) +_toml_path = os.path.join(_pkg_dir, "config", "extension.toml") +if not os.path.isfile(_toml_path): + _toml_path = os.path.join(ISAACLAB_OVPHYSX_EXT_DIR, "config", "extension.toml") + +ISAACLAB_OVPHYSX_METADATA = toml.load(_toml_path) +"""Extension metadata dictionary parsed from the extension.toml file.""" + +__version__ = ISAACLAB_OVPHYSX_METADATA["package"]["version"] diff --git a/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/__init__.py b/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/__init__.py new file mode 100644 index 000000000000..c644c05de13c --- /dev/null +++ b/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/__init__.py @@ -0,0 +1,10 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-package for ovphysx-backed assets.""" + +from isaaclab.utils.module import lazy_export + +lazy_export() diff --git a/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/__init__.pyi b/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/__init__.pyi new file mode 100644 index 000000000000..516e15c5ef6a --- /dev/null +++ b/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/__init__.pyi @@ -0,0 +1,11 @@ +# 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__ = [ + "Articulation", + "ArticulationData", +] + +from .articulation import Articulation, ArticulationData diff --git a/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/articulation/__init__.py b/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/articulation/__init__.py new file mode 100644 index 000000000000..d3798bf9652b --- /dev/null +++ b/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/articulation/__init__.py @@ -0,0 +1,10 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module for ovphysx-backed articulated assets.""" + +from isaaclab.utils.module import lazy_export + +lazy_export() diff --git a/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/articulation/__init__.pyi b/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/articulation/__init__.pyi new file mode 100644 index 000000000000..9e482491fe54 --- /dev/null +++ b/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/articulation/__init__.pyi @@ -0,0 +1,12 @@ +# 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__ = [ + "Articulation", + "ArticulationData", +] + +from .articulation import Articulation +from .articulation_data import ArticulationData diff --git a/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/articulation/articulation.py b/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/articulation/articulation.py new file mode 100644 index 000000000000..4c00dc839ca1 --- /dev/null +++ b/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/articulation/articulation.py @@ -0,0 +1,2703 @@ +# 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 + +"""Articulation implementation backed by ovphysx TensorBindingsAPI.""" + +from __future__ import annotations + +import fnmatch +import logging +import re +from collections.abc import Sequence +from typing import TYPE_CHECKING, Any + +import numpy as np +import torch +import warp as wp + +from isaaclab.assets.articulation.base_articulation import BaseArticulation +from isaaclab.physics import PhysicsManager +from isaaclab.utils.wrench_composer import WrenchComposer + +from isaaclab_ovphysx import tensor_types as TT +from isaaclab_ovphysx.physics import OvPhysxManager + +from .articulation_data import ArticulationData +from .kernels import _body_wrench_to_world, _scatter_rows_partial, update_soft_joint_pos_limits + +if TYPE_CHECKING: + from isaaclab.actuators import ActuatorBase + from isaaclab.assets.articulation.articulation_cfg import ArticulationCfg + +logger = logging.getLogger(__name__) + + +class Articulation(BaseArticulation): + """Articulation backed by the ovphysx TensorBindingsAPI. + + Reads and writes simulation state through ovphysx.TensorBinding objects created + from the OvPhysxManager's PhysX instance. + """ + + __backend_name__ = "ovphysx" + + cfg: ArticulationCfg + + def __init__(self, cfg: ArticulationCfg): + super().__init__(cfg) + + """ + Properties + """ + + @property + def data(self) -> ArticulationData: + """Data container with simulation state for this articulation.""" + return self._data + + @property + def num_instances(self) -> int: + """Number of articulation instances (environments).""" + return self._num_instances + + @property + def is_fixed_base(self) -> bool: + """Whether the articulation is a fixed-base or floating-base system.""" + return self._is_fixed_base + + @property + def num_joints(self) -> int: + """Number of joints in the articulation.""" + return self._num_joints + + @property + def num_fixed_tendons(self) -> int: + """Number of fixed tendons in the articulation.""" + return getattr(self, "_num_fixed_tendons", 0) + + @property + def num_spatial_tendons(self) -> int: + """Number of spatial tendons in the articulation.""" + return getattr(self, "_num_spatial_tendons", 0) + + @property + def num_bodies(self) -> int: + """Number of bodies (links) in the articulation.""" + return self._num_bodies + + @property + def joint_names(self) -> list[str]: + """Ordered names of joints in the articulation.""" + return self._joint_names + + @property + def fixed_tendon_names(self) -> list[str]: + """Ordered names of fixed tendons in the articulation.""" + return getattr(self, "_fixed_tendon_names", []) + + @property + def spatial_tendon_names(self) -> list[str]: + """Ordered names of spatial tendons in the articulation.""" + return getattr(self, "_spatial_tendon_names", []) + + @property + def body_names(self) -> list[str]: + """Ordered names of bodies (links) in the articulation.""" + return self._body_names + + @property + def root_view(self) -> Any: + """Root articulation view (not available for ovphysx backend).""" + return None + + @property + def instantaneous_wrench_composer(self) -> WrenchComposer | None: + """Wrench composer for forces applied only during the current step.""" + return self._instantaneous_wrench_composer + + @property + def permanent_wrench_composer(self) -> WrenchComposer | None: + """Wrench composer for forces applied persistently every step.""" + return self._permanent_wrench_composer + + """ + Operations. + """ + + def reset(self, env_ids: Sequence[int] | None = None, env_mask: wp.array | None = None) -> None: + """Reset the articulation. + + .. caution:: + If both `env_ids` and `env_mask` are provided, then `env_mask` takes precedence over `env_ids`. + + Args: + env_ids: Environment indices. If None, then all indices are used. + env_mask: Environment mask. If None, then all the instances are updated. Shape is (num_instances,). + """ + # use ellipses object to skip initial indices. + if (env_ids is None) or (env_ids == slice(None)): + env_ids = slice(None) + # reset actuators + for actuator in self.actuators.values(): + actuator.reset(env_ids) + # reset external wrenches. + self._instantaneous_wrench_composer.reset(env_ids, env_mask) + self._permanent_wrench_composer.reset(env_ids, env_mask) + + def write_data_to_sim(self) -> None: + """Apply external wrenches, actuator model, and write commands into the simulation.""" + # Apply external wrenches (before actuators, same as PhysX backend). + self._apply_external_wrenches() + + self._apply_actuator_model() + # Write effort tensor to simulation. + if self._effort_binding is not None: + self._effort_binding.write(self._effort_write_view) + # Write position and velocity targets in one shot (not per-actuator). + if self._has_implicit_actuators: + if self._pos_target_binding is not None: + self._pos_target_binding.write(self._pos_target_write_view) + if self._vel_target_binding is not None: + self._vel_target_binding.write(self._vel_target_write_view) + + def update(self, dt: float) -> None: + """Update internal data buffers after a simulation step. + + Args: + dt: The simulation time step [s] used for finite-difference quantities. + """ + self._data.update(dt) + + """ + Operations - Finders. + """ + + def find_bodies(self, name_keys: str | Sequence[str], preserve_order: bool = False) -> tuple[list[int], list[str]]: + """Find bodies in the articulation based on the name keys. + + Please check the :func:`isaaclab.utils.string.resolve_matching_names` function for more + information on the name matching. + + Args: + name_keys: A regular expression or a list of regular expressions to match the body names. + preserve_order: Whether to preserve the order of the name keys in the output. Defaults to False. + + Returns: + A tuple of lists containing the body indices and names. + """ + return self._find_names(self._body_names, name_keys, preserve_order) + + def find_joints( + self, + name_keys: str | Sequence[str], + joint_subset: list[int] | None = None, + preserve_order: bool = False, + ) -> tuple[list[int], list[str]]: + """Find joints in the articulation based on the name keys. + + Please check the :func:`isaaclab.utils.string.resolve_matching_names` function for more + information on the name matching. + + Args: + name_keys: A regular expression or a list of regular expressions to match the joint names. + joint_subset: A subset of joint indices to search within. Defaults to None, which means all joints + in the articulation are searched. + preserve_order: Whether to preserve the order of the name keys in the output. Defaults to False. + + Returns: + A tuple of lists containing the joint indices and names. + """ + names = [self._joint_names[i] for i in joint_subset] if joint_subset is not None else self._joint_names + indices, matched = self._find_names(names, name_keys, preserve_order) + if joint_subset is not None: + indices = [joint_subset[i] for i in indices] + return indices, matched + + def find_fixed_tendons( + self, + name_keys: str | Sequence[str], + tendon_subsets: list[str] | None = None, + preserve_order: bool = False, + ) -> tuple[list[int], list[str]]: + """Find fixed tendons in the articulation based on the name keys. + + Args: + name_keys: A regular expression or a list of regular expressions + to match the joint names with fixed tendons. + tendon_subsets: A subset of joints with fixed tendons to search + for. Defaults to None, which means all joints in the + articulation are searched. + preserve_order: Whether to preserve the order of the name keys in + the output. Defaults to False. + + Returns: + A tuple of lists containing the tendon indices and names. + """ + if tendon_subsets is None: + tendon_subsets = self.fixed_tendon_names + if not tendon_subsets: + return [], [] + return self._find_names(tendon_subsets, name_keys, preserve_order) + + def find_spatial_tendons( + self, + name_keys: str | Sequence[str], + tendon_subsets: list[str] | None = None, + preserve_order: bool = False, + ) -> tuple[list[int], list[str]]: + """Find spatial tendons in the articulation based on the name keys. + + Args: + name_keys: A regular expression or a list of regular expressions + to match the tendon names. + tendon_subsets: A subset of tendons to search for. Defaults to + None, which means all tendons in the articulation are searched. + preserve_order: Whether to preserve the order of the name keys in + the output. Defaults to False. + + Returns: + A tuple of lists containing the tendon indices and names. + """ + if tendon_subsets is None: + tendon_subsets = self.spatial_tendon_names + if not tendon_subsets: + return [], [] + return self._find_names(tendon_subsets, name_keys, preserve_order) + + """ + Operations - State Writers. + """ + + def write_root_pose_to_sim_index( + self, + *, + root_pose: torch.Tensor | wp.array, + env_ids: Sequence[int] | wp.array | None = None, + ) -> None: + """Set the root pose over selected environment indices into the simulation. + + The root pose comprises of the cartesian position and quaternion orientation in (x, y, z, w). + + Args: + root_pose: Root poses in simulation frame. Shape is (len(env_ids),) with dtype wp.transformf. + env_ids: Environment indices. If None, then all indices are used. + """ + n = self._n_envs_index(env_ids) + self.assert_shape_and_dtype(root_pose, (n,), wp.transformf, "root_pose") + self._write_root_state(TT.ROOT_POSE, root_pose, env_ids) + + def write_root_pose_to_sim_mask( + self, + *, + root_pose: torch.Tensor | wp.array, + env_mask: wp.array | None = None, + ) -> None: + """Set the root pose over masked environments into the simulation. + + The root pose comprises of the cartesian position and quaternion orientation in (x, y, z, w). + + Args: + root_pose: Root poses in simulation frame. Shape is (num_instances,) with dtype wp.transformf. + env_mask: Environment mask. If None, then all instances are updated. + """ + self.assert_shape_and_dtype(root_pose, (self._num_instances,), wp.transformf, "root_pose") + self._write_root_state(TT.ROOT_POSE, root_pose, mask=env_mask) + + def write_root_link_pose_to_sim_index( + self, + *, + root_pose: torch.Tensor | wp.array, + env_ids: Sequence[int] | wp.array | None = None, + ) -> None: + """Set the root link pose over selected environment indices into the simulation. + + The root pose comprises of the cartesian position and quaternion orientation in (x, y, z, w). + + Args: + root_pose: Root link poses in simulation frame. Shape is (len(env_ids),) with dtype wp.transformf. + env_ids: Environment indices. If None, then all indices are used. + """ + n = self._n_envs_index(env_ids) + self.assert_shape_and_dtype(root_pose, (n,), wp.transformf, "root_pose") + self._write_root_state(TT.ROOT_POSE, root_pose, env_ids) + + def write_root_link_pose_to_sim_mask( + self, + *, + root_pose: torch.Tensor | wp.array, + env_mask: wp.array | None = None, + ) -> None: + """Set the root link pose over masked environments into the simulation. + + The root pose comprises of the cartesian position and quaternion orientation in (x, y, z, w). + + Args: + root_pose: Root link poses in simulation frame. Shape is (num_instances,) with dtype wp.transformf. + env_mask: Environment mask. If None, then all instances are updated. + """ + self.assert_shape_and_dtype(root_pose, (self._num_instances,), wp.transformf, "root_pose") + self._write_root_state(TT.ROOT_POSE, root_pose, mask=env_mask) + + def write_root_com_pose_to_sim_index( + self, + *, + root_pose: torch.Tensor | wp.array, + env_ids: Sequence[int] | wp.array | None = None, + ) -> None: + """Set the root center of mass pose over selected environment indices into the simulation. + + The root pose comprises of the cartesian position and quaternion orientation in (x, y, z, w). + The orientation is the orientation of the principal axes of inertia. + + Args: + root_pose: Root center of mass poses in simulation frame. Shape is (len(env_ids),) + with dtype wp.transformf. + env_ids: Environment indices. If None, then all indices are used. + """ + n = self._n_envs_index(env_ids) + self.assert_shape_and_dtype(root_pose, (n,), wp.transformf, "root_pose") + self._write_root_state(TT.ROOT_POSE, root_pose, env_ids) + + def write_root_com_pose_to_sim_mask( + self, + *, + root_pose: torch.Tensor | wp.array, + env_mask: wp.array | None = None, + ) -> None: + """Set the root center of mass pose over masked environments into the simulation. + + The root pose comprises of the cartesian position and quaternion orientation in (x, y, z, w). + The orientation is the orientation of the principal axes of inertia. + + Args: + root_pose: Root center of mass poses in simulation frame. Shape is (num_instances,) + with dtype wp.transformf. + env_mask: Environment mask. If None, then all instances are updated. + """ + self.assert_shape_and_dtype(root_pose, (self._num_instances,), wp.transformf, "root_pose") + self._write_root_state(TT.ROOT_POSE, root_pose, mask=env_mask) + + def write_root_velocity_to_sim_index( + self, + *, + root_velocity: torch.Tensor | wp.array, + env_ids: Sequence[int] | wp.array | None = None, + ) -> None: + """Set the root velocity over selected environment indices into the simulation. + + The velocity comprises linear velocity (x, y, z) and angular velocity (x, y, z) in that order. + + Args: + root_velocity: Root velocities in simulation world frame. Shape is (len(env_ids),) + with dtype wp.spatial_vectorf. + env_ids: Environment indices. If None, then all indices are used. + """ + n = self._n_envs_index(env_ids) + self.assert_shape_and_dtype(root_velocity, (n,), wp.spatial_vectorf, "root_velocity") + self._write_root_state(TT.ROOT_VELOCITY, root_velocity, env_ids) + + def write_root_velocity_to_sim_mask( + self, + *, + root_velocity: torch.Tensor | wp.array, + env_mask: wp.array | None = None, + ) -> None: + """Set the root velocity over masked environments into the simulation. + + The velocity comprises linear velocity (x, y, z) and angular velocity (x, y, z) in that order. + + Args: + root_velocity: Root velocities in simulation world frame. Shape is (num_instances,) + with dtype wp.spatial_vectorf. + env_mask: Environment mask. If None, then all instances are updated. + """ + self.assert_shape_and_dtype(root_velocity, (self._num_instances,), wp.spatial_vectorf, "root_velocity") + self._write_root_state(TT.ROOT_VELOCITY, root_velocity, mask=env_mask) + + def write_root_com_velocity_to_sim_index( + self, + *, + root_velocity: torch.Tensor | wp.array, + env_ids: Sequence[int] | wp.array | None = None, + ) -> None: + """Set the root center of mass velocity over selected environment indices into the simulation. + + The velocity comprises linear velocity (x, y, z) and angular velocity (x, y, z) in that order. + + Args: + root_velocity: Root center of mass velocities in simulation world frame. + Shape is (len(env_ids),) with dtype wp.spatial_vectorf. + env_ids: Environment indices. If None, then all indices are used. + """ + n = self._n_envs_index(env_ids) + self.assert_shape_and_dtype(root_velocity, (n,), wp.spatial_vectorf, "root_velocity") + self._write_root_state(TT.ROOT_VELOCITY, root_velocity, env_ids) + + def write_root_com_velocity_to_sim_mask( + self, + *, + root_velocity: torch.Tensor | wp.array, + env_mask: wp.array | None = None, + ) -> None: + """Set the root center of mass velocity over masked environments into the simulation. + + The velocity comprises linear velocity (x, y, z) and angular velocity (x, y, z) in that order. + + Args: + root_velocity: Root center of mass velocities in simulation world frame. + Shape is (num_instances,) with dtype wp.spatial_vectorf. + env_mask: Environment mask. If None, then all instances are updated. + """ + self.assert_shape_and_dtype(root_velocity, (self._num_instances,), wp.spatial_vectorf, "root_velocity") + self._write_root_state(TT.ROOT_VELOCITY, root_velocity, mask=env_mask) + + def write_root_link_velocity_to_sim_index( + self, + *, + root_velocity: torch.Tensor | wp.array, + env_ids: Sequence[int] | wp.array | None = None, + ) -> None: + """Set the root link velocity over selected environment indices into the simulation. + + The velocity comprises linear velocity (x, y, z) and angular velocity (x, y, z) in that order. + + .. note:: + This sets the velocity of the root's frame rather than the root's center of mass. + + Args: + root_velocity: Root frame velocities in simulation world frame. + Shape is (len(env_ids),) with dtype wp.spatial_vectorf. + env_ids: Environment indices. If None, then all indices are used. + """ + n = self._n_envs_index(env_ids) + self.assert_shape_and_dtype(root_velocity, (n,), wp.spatial_vectorf, "root_velocity") + self._write_root_state(TT.ROOT_VELOCITY, root_velocity, env_ids) + + def write_root_link_velocity_to_sim_mask( + self, + *, + root_velocity: torch.Tensor | wp.array, + env_mask: wp.array | None = None, + ) -> None: + """Set the root link velocity over masked environments into the simulation. + + The velocity comprises linear velocity (x, y, z) and angular velocity (x, y, z) in that order. + + .. note:: + This sets the velocity of the root's frame rather than the root's center of mass. + + Args: + root_velocity: Root frame velocities in simulation world frame. + Shape is (num_instances,) with dtype wp.spatial_vectorf. + env_mask: Environment mask. If None, then all instances are updated. + """ + self.assert_shape_and_dtype(root_velocity, (self._num_instances,), wp.spatial_vectorf, "root_velocity") + self._write_root_state(TT.ROOT_VELOCITY, root_velocity, mask=env_mask) + + """ + Operations - Joint State Writers. + """ + + def write_joint_state_to_sim_mask( + self, + joint_pos: torch.Tensor | wp.array, + joint_vel: torch.Tensor | wp.array, + env_mask: wp.array | None = None, + joint_mask: wp.array | None = None, + ) -> None: + """Write joint positions and velocities over masked environments into the simulation. + + Args: + joint_pos: Joint positions. Shape is (num_instances, num_joints). + joint_vel: Joint velocities. Shape is (num_instances, num_joints). + env_mask: Environment mask. If None, then all instances are updated. + joint_mask: Joint mask. If None, then all joints are updated. + """ + self.write_joint_position_to_sim_mask(position=joint_pos, env_mask=env_mask, joint_mask=joint_mask) + self.write_joint_velocity_to_sim_mask(velocity=joint_vel, env_mask=env_mask, joint_mask=joint_mask) + + def write_joint_position_to_sim_index( + self, + *, + position: torch.Tensor | wp.array, + joint_ids: Sequence[int] | None = None, + env_ids: Sequence[int] | wp.array | None = None, + ) -> None: + """Write joint positions over selected environment and joint indices into the simulation. + + Args: + position: Joint positions. Shape is (len(env_ids), len(joint_ids)). + joint_ids: Joint indices. If None, then all joints are used. + env_ids: Environment indices. If None, then all indices are used. + """ + n = self._n_envs_index(env_ids) + d = len(joint_ids) if joint_ids is not None else self._num_joints + self.assert_shape_and_dtype(position, (n, d), wp.float32, "position") + self._write_flat_tensor(TT.DOF_POSITION, position, env_ids, joint_ids) + self.data._joint_pos_buf.timestamp = -1.0 + + def write_joint_position_to_sim_mask( + self, + *, + position: torch.Tensor | wp.array, + joint_mask: wp.array | None = None, + env_mask: wp.array | None = None, + ) -> None: + """Write joint positions over masked environments into the simulation. + + Args: + position: Joint positions. Shape is (num_instances, num_joints). + joint_mask: Joint mask. If None, then all joints are updated. + env_mask: Environment mask. If None, then all instances are updated. + """ + self.assert_shape_and_dtype(position, (self._num_instances, self._num_joints), wp.float32, "position") + self._write_flat_tensor_mask(TT.DOF_POSITION, position, env_mask, joint_mask) + self.data._joint_pos_buf.timestamp = -1.0 + + def write_joint_velocity_to_sim_index( + self, + *, + velocity: torch.Tensor | wp.array, + joint_ids: Sequence[int] | None = None, + env_ids: Sequence[int] | wp.array | None = None, + ) -> None: + """Write joint velocities over selected environment and joint indices into the simulation. + + Args: + velocity: Joint velocities. Shape is (len(env_ids), len(joint_ids)). + joint_ids: Joint indices. If None, then all joints are used. + env_ids: Environment indices. If None, then all indices are used. + """ + n = self._n_envs_index(env_ids) + d = len(joint_ids) if joint_ids is not None else self._num_joints + self.assert_shape_and_dtype(velocity, (n, d), wp.float32, "velocity") + self._write_flat_tensor(TT.DOF_VELOCITY, velocity, env_ids, joint_ids) + self.data._joint_vel_buf.timestamp = -1.0 + + def write_joint_velocity_to_sim_mask( + self, + *, + velocity: torch.Tensor | wp.array, + joint_mask: wp.array | None = None, + env_mask: wp.array | None = None, + ) -> None: + """Write joint velocities over masked environments into the simulation. + + Args: + velocity: Joint velocities. Shape is (num_instances, num_joints). + joint_mask: Joint mask. If None, then all joints are updated. + env_mask: Environment mask. If None, then all instances are updated. + """ + self.assert_shape_and_dtype(velocity, (self._num_instances, self._num_joints), wp.float32, "velocity") + self._write_flat_tensor_mask(TT.DOF_VELOCITY, velocity, env_mask, joint_mask) + self.data._joint_vel_buf.timestamp = -1.0 + + """ + Operations - Simulation Parameters Writers. + """ + + def write_joint_stiffness_to_sim_index( + self, + *, + stiffness: torch.Tensor | wp.array, + joint_ids: Sequence[int] | None = None, + env_ids: Sequence[int] | wp.array | None = None, + ) -> None: + """Write joint stiffness over selected indices into the simulation. + + Args: + stiffness: Joint stiffness. Shape is (len(env_ids), len(joint_ids)). + joint_ids: Joint indices. If None, then all joints are used. + env_ids: Environment indices. If None, then all indices are used. + """ + n = self._n_envs_index(env_ids) + d = len(joint_ids) if joint_ids is not None else self._num_joints + self.assert_shape_and_dtype(stiffness, (n, d), wp.float32, "stiffness") + self._write_flat_tensor(TT.DOF_STIFFNESS, stiffness, env_ids, joint_ids) + + def write_joint_stiffness_to_sim_mask( + self, + *, + stiffness: torch.Tensor | wp.array, + joint_mask: wp.array | None = None, + env_mask: wp.array | None = None, + ) -> None: + """Write joint stiffness over masked environments into the simulation. + + Args: + stiffness: Joint stiffness. Shape is (num_instances, num_joints). + joint_mask: Joint mask. If None, then all joints are updated. + env_mask: Environment mask. If None, then all instances are updated. + """ + self.assert_shape_and_dtype(stiffness, (self._num_instances, self._num_joints), wp.float32, "stiffness") + self._write_flat_tensor_mask(TT.DOF_STIFFNESS, stiffness, env_mask, joint_mask) + + def write_joint_damping_to_sim_index( + self, + *, + damping: torch.Tensor | wp.array, + joint_ids: Sequence[int] | None = None, + env_ids: Sequence[int] | wp.array | None = None, + ) -> None: + """Write joint damping over selected indices into the simulation. + + Args: + damping: Joint damping. Shape is (len(env_ids), len(joint_ids)). + joint_ids: Joint indices. If None, then all joints are used. + env_ids: Environment indices. If None, then all indices are used. + """ + n = self._n_envs_index(env_ids) + d = len(joint_ids) if joint_ids is not None else self._num_joints + self.assert_shape_and_dtype(damping, (n, d), wp.float32, "damping") + self._write_flat_tensor(TT.DOF_DAMPING, damping, env_ids, joint_ids) + + def write_joint_damping_to_sim_mask( + self, + *, + damping: torch.Tensor | wp.array, + joint_mask: wp.array | None = None, + env_mask: wp.array | None = None, + ) -> None: + """Write joint damping over masked environments into the simulation. + + Args: + damping: Joint damping. Shape is (num_instances, num_joints). + joint_mask: Joint mask. If None, then all joints are updated. + env_mask: Environment mask. If None, then all instances are updated. + """ + self.assert_shape_and_dtype(damping, (self._num_instances, self._num_joints), wp.float32, "damping") + self._write_flat_tensor_mask(TT.DOF_DAMPING, damping, env_mask, joint_mask) + + def write_joint_position_limit_to_sim_index( + self, + *, + limits: torch.Tensor | wp.array, + joint_ids: Sequence[int] | None = None, + env_ids: Sequence[int] | wp.array | None = None, + warn_limit_violation: bool = True, + ) -> None: + """Write joint position limits over selected environment indices into the simulation. + + Args: + limits: Joint position limits [rad or m]. Shape is (len(env_ids), len(joint_ids)) + with dtype wp.vec2f. + joint_ids: Joint indices. If None, then all joints are used. + env_ids: Environment indices. If None, then all indices are used. + warn_limit_violation: Whether to use warning or info level logging when default joint + positions exceed the new limits. Defaults to True. + """ + if isinstance(limits, (int, float)): + raise ValueError("Float scalars are not supported for position limits (vec2f dtype)") + n = self._n_envs_index(env_ids) + d = len(joint_ids) if joint_ids is not None else self._num_joints + self.assert_shape_and_dtype(limits, (n, d), wp.vec2f, "limits") + self._write_flat_tensor(TT.DOF_LIMIT, limits, env_ids, joint_ids) + + def write_joint_position_limit_to_sim_mask( + self, + *, + limits: torch.Tensor | wp.array, + joint_mask: wp.array | None = None, + env_mask: wp.array | None = None, + warn_limit_violation: bool = True, + ) -> None: + """Write joint position limits over masked environments into the simulation. + + Args: + limits: Joint position limits [rad or m]. Shape is (num_instances, num_joints) + with dtype wp.vec2f. + joint_mask: Joint mask. If None, then all joints are updated. + env_mask: Environment mask. If None, then all instances are updated. + warn_limit_violation: Whether to use warning or info level logging when default joint + positions exceed the new limits. Defaults to True. + """ + if isinstance(limits, (int, float)): + raise ValueError("Float scalars are not supported for position limits (vec2f dtype)") + self.assert_shape_and_dtype(limits, (self._num_instances, self._num_joints), wp.vec2f, "limits") + self._write_flat_tensor_mask(TT.DOF_LIMIT, limits, env_mask, joint_mask) + + def write_joint_velocity_limit_to_sim_index( + self, + *, + limits: torch.Tensor | wp.array, + joint_ids: Sequence[int] | None = None, + env_ids: Sequence[int] | wp.array | None = None, + ) -> None: + """Write joint max velocity over selected environment indices into the simulation. + + The velocity limit is used to constrain the joint velocities in the physics engine. + + Args: + limits: Joint max velocity [rad/s or m/s]. Shape is (len(env_ids), len(joint_ids)). + joint_ids: Joint indices. If None, then all joints are used. + env_ids: Environment indices. If None, then all indices are used. + """ + n = self._n_envs_index(env_ids) + d = len(joint_ids) if joint_ids is not None else self._num_joints + self.assert_shape_and_dtype(limits, (n, d), wp.float32, "limits") + self._write_flat_tensor(TT.DOF_MAX_VELOCITY, limits, env_ids, joint_ids) + + def write_joint_velocity_limit_to_sim_mask( + self, + *, + limits: torch.Tensor | wp.array, + joint_mask: wp.array | None = None, + env_mask: wp.array | None = None, + ) -> None: + """Write joint max velocity over masked environments into the simulation. + + The velocity limit is used to constrain the joint velocities in the physics engine. + + Args: + limits: Joint max velocity [rad/s or m/s]. Shape is (num_instances, num_joints). + joint_mask: Joint mask. If None, then all joints are updated. + env_mask: Environment mask. If None, then all instances are updated. + """ + self.assert_shape_and_dtype(limits, (self._num_instances, self._num_joints), wp.float32, "limits") + self._write_flat_tensor_mask(TT.DOF_MAX_VELOCITY, limits, env_mask, joint_mask) + + def write_joint_effort_limit_to_sim_index( + self, + *, + limits: torch.Tensor | wp.array, + joint_ids: Sequence[int] | None = None, + env_ids: Sequence[int] | wp.array | None = None, + ) -> None: + """Write joint effort limits over selected environment indices into the simulation. + + The effort limit is used to constrain the computed joint efforts in the physics engine. + + Args: + limits: Joint effort limits [N or N*m]. Shape is (len(env_ids), len(joint_ids)). + joint_ids: Joint indices. If None, then all joints are used. + env_ids: Environment indices. If None, then all indices are used. + """ + n = self._n_envs_index(env_ids) + d = len(joint_ids) if joint_ids is not None else self._num_joints + self.assert_shape_and_dtype(limits, (n, d), wp.float32, "limits") + self._write_flat_tensor(TT.DOF_MAX_FORCE, limits, env_ids, joint_ids) + + def write_joint_effort_limit_to_sim_mask( + self, + *, + limits: torch.Tensor | wp.array, + joint_mask: wp.array | None = None, + env_mask: wp.array | None = None, + ) -> None: + """Write joint effort limits over masked environments into the simulation. + + The effort limit is used to constrain the computed joint efforts in the physics engine. + + Args: + limits: Joint effort limits [N or N*m]. Shape is (num_instances, num_joints). + joint_mask: Joint mask. If None, then all joints are updated. + env_mask: Environment mask. If None, then all instances are updated. + """ + self.assert_shape_and_dtype(limits, (self._num_instances, self._num_joints), wp.float32, "limits") + self._write_flat_tensor_mask(TT.DOF_MAX_FORCE, limits, env_mask, joint_mask) + + def write_joint_armature_to_sim_index( + self, + *, + armature: torch.Tensor | wp.array, + joint_ids: Sequence[int] | None = None, + env_ids: Sequence[int] | wp.array | None = None, + ) -> None: + """Write joint armature over selected environment indices into the simulation. + + The armature is directly added to the corresponding joint-space inertia. It helps improve the + simulation stability by reducing the joint velocities. + + Args: + armature: Joint armature [kg*m^2 or kg]. Shape is (len(env_ids), len(joint_ids)). + joint_ids: Joint indices. If None, then all joints are used. + env_ids: Environment indices. If None, then all indices are used. + """ + n = self._n_envs_index(env_ids) + d = len(joint_ids) if joint_ids is not None else self._num_joints + self.assert_shape_and_dtype(armature, (n, d), wp.float32, "armature") + self._write_flat_tensor(TT.DOF_ARMATURE, armature, env_ids, joint_ids) + + def write_joint_armature_to_sim_mask( + self, + *, + armature: torch.Tensor | wp.array, + joint_mask: wp.array | None = None, + env_mask: wp.array | None = None, + ) -> None: + """Write joint armature over masked environments into the simulation. + + The armature is directly added to the corresponding joint-space inertia. It helps improve the + simulation stability by reducing the joint velocities. + + Args: + armature: Joint armature [kg*m^2 or kg]. Shape is (num_instances, num_joints). + joint_mask: Joint mask. If None, then all joints are updated. + env_mask: Environment mask. If None, then all instances are updated. + """ + self.assert_shape_and_dtype(armature, (self._num_instances, self._num_joints), wp.float32, "armature") + self._write_flat_tensor_mask(TT.DOF_ARMATURE, armature, env_mask, joint_mask) + + def write_joint_friction_coefficient_to_sim_index( + self, + *, + joint_friction_coeff: torch.Tensor | wp.array, + joint_ids: Sequence[int] | None = None, + env_ids: Sequence[int] | wp.array | None = None, + ) -> None: + """Write joint friction coefficients over selected indices into the simulation. + + Args: + joint_friction_coeff: Joint friction coefficients. Shape is (len(env_ids), len(joint_ids)). + joint_ids: Joint indices. If None, then all joints are used. + env_ids: Environment indices. If None, then all indices are used. + """ + n = self._n_envs_index(env_ids) + d = len(joint_ids) if joint_ids is not None else self._num_joints + self.assert_shape_and_dtype(joint_friction_coeff, (n, d), wp.float32, "joint_friction_coeff") + self._write_friction_column(joint_friction_coeff, env_ids, joint_ids) + + def write_joint_friction_coefficient_to_sim_mask( + self, + *, + joint_friction_coeff: torch.Tensor | wp.array, + joint_mask: wp.array | None = None, + env_mask: wp.array | None = None, + ) -> None: + """Write joint friction coefficients over masked environments into the simulation. + + Args: + joint_friction_coeff: Joint friction coefficients. Shape is (num_instances, num_joints). + joint_mask: Joint mask. If None, then all joints are updated. + env_mask: Environment mask. If None, then all instances are updated. + """ + self.assert_shape_and_dtype( + joint_friction_coeff, (self._num_instances, self._num_joints), wp.float32, "joint_friction_coeff" + ) + self._write_friction_column_mask(joint_friction_coeff, env_mask, joint_mask) + + """ + Operations - Setters. + """ + + def set_masses_index( + self, + *, + masses: torch.Tensor | wp.array, + body_ids: Sequence[int] | None = None, + env_ids: Sequence[int] | wp.array | None = None, + ) -> None: + """Set masses of all bodies using indices. + + Args: + masses: Masses of all bodies [kg]. Shape is (len(env_ids), len(body_ids)). + body_ids: The body indices to set the masses for. Defaults to None (all bodies). + env_ids: The environment indices to set the masses for. Defaults to None (all environments). + """ + n = self._n_envs_index(env_ids) + b = len(body_ids) if body_ids is not None else self._num_bodies + self.assert_shape_and_dtype(masses, (n, b), wp.float32, "masses") + self._write_flat_tensor(TT.BODY_MASS, masses, env_ids, body_ids) + + def set_masses_mask( + self, + *, + masses: torch.Tensor | wp.array, + body_mask: wp.array | None = None, + env_mask: wp.array | None = None, + ) -> None: + """Set masses of all bodies using masks. + + Args: + masses: Masses of all bodies [kg]. Shape is (num_instances, num_bodies). + body_mask: Body mask. If None, then all bodies are used. + env_mask: Environment mask. If None, then all the instances are updated. Shape is (num_instances,). + """ + self.assert_shape_and_dtype(masses, (self._num_instances, self._num_bodies), wp.float32, "masses") + self._write_flat_tensor_mask(TT.BODY_MASS, masses, env_mask, body_mask) + + def set_coms_index( + self, + *, + coms: torch.Tensor | wp.array, + body_ids: Sequence[int] | None = None, + env_ids: Sequence[int] | wp.array | None = None, + ) -> None: + """Set center of mass pose of all bodies using indices. + + Args: + coms: Center of mass pose of all bodies. Shape is (len(env_ids), len(body_ids)) + with dtype wp.transformf. + body_ids: The body indices to set the center of mass pose for. Defaults to None (all bodies). + env_ids: The environment indices to set the center of mass pose for. + Defaults to None (all environments). + """ + n = self._n_envs_index(env_ids) + b = len(body_ids) if body_ids is not None else self._num_bodies + self.assert_shape_and_dtype(coms, (n, b), wp.transformf, "coms") + self._write_flat_tensor(TT.BODY_COM_POSE, coms, env_ids, body_ids) + + def set_coms_mask( + self, + *, + coms: torch.Tensor | wp.array, + body_mask: wp.array | None = None, + env_mask: wp.array | None = None, + ) -> None: + """Set center of mass pose of all bodies using masks. + + Args: + coms: Center of mass pose of all bodies. Shape is (num_instances, num_bodies) + with dtype wp.transformf. + body_mask: Body mask. If None, then all bodies are used. + env_mask: Environment mask. If None, then all the instances are updated. Shape is (num_instances,). + """ + self.assert_shape_and_dtype(coms, (self._num_instances, self._num_bodies), wp.transformf, "coms") + self._write_flat_tensor_mask(TT.BODY_COM_POSE, coms, env_mask, body_mask) + + def set_inertias_index( + self, + *, + inertias: torch.Tensor | wp.array, + body_ids: Sequence[int] | None = None, + env_ids: Sequence[int] | wp.array | None = None, + ) -> None: + """Set inertias of all bodies using indices. + + Args: + inertias: Inertias of all bodies [kg*m^2]. Shape is (len(env_ids), len(body_ids), 9). + body_ids: The body indices to set the inertias for. Defaults to None (all bodies). + env_ids: The environment indices to set the inertias for. Defaults to None (all environments). + """ + n = self._n_envs_index(env_ids) + b = len(body_ids) if body_ids is not None else self._num_bodies + self.assert_shape_and_dtype(inertias, (n, b, 9), wp.float32, "inertias") + self._write_flat_tensor(TT.BODY_INERTIA, inertias, env_ids, body_ids) + + def set_inertias_mask( + self, + *, + inertias: torch.Tensor | wp.array, + body_mask: wp.array | None = None, + env_mask: wp.array | None = None, + ) -> None: + """Set inertias of all bodies using masks. + + Args: + inertias: Inertias of all bodies [kg*m^2]. Shape is (num_instances, num_bodies, 9). + body_mask: Body mask. If None, then all bodies are used. + env_mask: Environment mask. If None, then all the instances are updated. Shape is (num_instances,). + """ + self.assert_shape_and_dtype(inertias, (self._num_instances, self._num_bodies, 9), wp.float32, "inertias") + self._write_flat_tensor_mask(TT.BODY_INERTIA, inertias, env_mask, body_mask) + + """ + Operations - Target Setters. + """ + + def set_joint_position_target_index( + self, + *, + target: torch.Tensor | wp.array, + joint_ids: Sequence[int] | None = None, + env_ids: Sequence[int] | wp.array | None = None, + ) -> None: + """Set joint position targets into internal buffers using indices. + + This function does not apply the joint targets to the simulation. It only fills the buffers with + the desired values. To apply the joint targets, call the :meth:`write_data_to_sim` function. + + Args: + target: Joint position targets [rad or m]. Shape is (len(env_ids), len(joint_ids)). + joint_ids: Joint indices. If None, then all joints are used. + env_ids: Environment indices. If None, then all indices are used. + """ + self._set_target_into_buffer(self._data._joint_pos_target, target, env_ids, joint_ids) + + def set_joint_position_target_mask( + self, + *, + target: torch.Tensor | wp.array, + joint_mask: wp.array | None = None, + env_mask: wp.array | None = None, + ) -> None: + """Set joint position targets into internal buffers using masks. + + Args: + target: Joint position targets [rad or m]. Shape is (num_instances, num_joints). + joint_mask: Joint mask. If None, then all joints are updated. + env_mask: Environment mask. If None, then all instances are updated. + """ + self._set_target_into_buffer_mask(self._data._joint_pos_target, target, env_mask, joint_mask) + + def set_joint_velocity_target_index( + self, + *, + target: torch.Tensor | wp.array, + joint_ids: Sequence[int] | None = None, + env_ids: Sequence[int] | wp.array | None = None, + ) -> None: + """Set joint velocity targets into internal buffers using indices. + + This function does not apply the joint targets to the simulation. It only fills the buffers with + the desired values. To apply the joint targets, call the :meth:`write_data_to_sim` function. + + Args: + target: Joint velocity targets [rad/s or m/s]. Shape is (len(env_ids), len(joint_ids)). + joint_ids: Joint indices. If None, then all joints are used. + env_ids: Environment indices. If None, then all indices are used. + """ + self._set_target_into_buffer(self._data._joint_vel_target, target, env_ids, joint_ids) + + def set_joint_velocity_target_mask( + self, + *, + target: torch.Tensor | wp.array, + joint_mask: wp.array | None = None, + env_mask: wp.array | None = None, + ) -> None: + """Set joint velocity targets into internal buffers using masks. + + Args: + target: Joint velocity targets [rad/s or m/s]. Shape is (num_instances, num_joints). + joint_mask: Joint mask. If None, then all joints are updated. + env_mask: Environment mask. If None, then all instances are updated. + """ + self._set_target_into_buffer_mask(self._data._joint_vel_target, target, env_mask, joint_mask) + + def set_joint_effort_target_index( + self, + *, + target: torch.Tensor | wp.array, + joint_ids: Sequence[int] | None = None, + env_ids: Sequence[int] | wp.array | None = None, + ) -> None: + """Set joint efforts into internal buffers using indices. + + This function does not apply the joint targets to the simulation. It only fills the buffers with + the desired values. To apply the joint targets, call the :meth:`write_data_to_sim` function. + + Args: + target: Joint effort targets [N or N*m]. Shape is (len(env_ids), len(joint_ids)). + joint_ids: Joint indices. If None, then all joints are used. + env_ids: Environment indices. If None, then all indices are used. + """ + self._set_target_into_buffer(self._data._joint_effort_target, target, env_ids, joint_ids) + + def set_joint_effort_target_mask( + self, + *, + target: torch.Tensor | wp.array, + joint_mask: wp.array | None = None, + env_mask: wp.array | None = None, + ) -> None: + """Set joint efforts into internal buffers using masks. + + Args: + target: Joint effort targets [N or N*m]. Shape is (num_instances, num_joints). + joint_mask: Joint mask. If None, then all joints are updated. + env_mask: Environment mask. If None, then all instances are updated. + """ + self._set_target_into_buffer_mask(self._data._joint_effort_target, target, env_mask, joint_mask) + + """ + Operations - Tendons. + """ + + def set_fixed_tendon_stiffness_index( + self, + *, + stiffness: float | torch.Tensor | wp.array, + fixed_tendon_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + ) -> None: + """Set fixed tendon stiffness into internal buffers using indices. + + This function does not apply the tendon stiffness to the simulation. It only fills the buffers with + the desired values. To apply the tendon stiffness, call the + :meth:`write_fixed_tendon_properties_to_sim_index` method. + + Args: + stiffness: Fixed tendon stiffness. Shape is (len(env_ids), len(fixed_tendon_ids)). + fixed_tendon_ids: The tendon indices to set the stiffness for. Defaults to None (all fixed tendons). + env_ids: Environment indices. If None, then all indices are used. + """ + n = self._n_envs_index(env_ids) + t = len(fixed_tendon_ids) if fixed_tendon_ids else self._nft() + self.assert_shape_and_dtype(stiffness, (n, t), wp.float32, "stiffness") + if self._data._fixed_tendon_stiffness is not None: + self._set_target_into_buffer(self._data._fixed_tendon_stiffness, stiffness, env_ids, fixed_tendon_ids) + + def set_fixed_tendon_stiffness_mask( + self, + *, + stiffness: float | torch.Tensor | wp.array, + fixed_tendon_mask: wp.array | None = None, + env_mask: wp.array | None = None, + ) -> None: + """Set fixed tendon stiffness into internal buffers using masks. + + Args: + stiffness: Fixed tendon stiffness. Shape is (num_instances, num_fixed_tendons). + fixed_tendon_mask: Tendon mask. If None, then all fixed tendons are used. + env_mask: Environment mask. If None, then all the instances are updated. + """ + self.assert_shape_and_dtype(stiffness, (self._num_instances, self._nft()), wp.float32, "stiffness") + if self._data._fixed_tendon_stiffness is not None: + self._set_target_into_buffer_mask( + self._data._fixed_tendon_stiffness, stiffness, env_mask, fixed_tendon_mask + ) + + def set_fixed_tendon_damping_index( + self, + *, + damping: float | torch.Tensor | wp.array, + fixed_tendon_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + ) -> None: + """Set fixed tendon damping into internal buffers using indices. + + Args: + damping: Fixed tendon damping. Shape is (len(env_ids), len(fixed_tendon_ids)). + fixed_tendon_ids: The tendon indices to set the damping for. Defaults to None (all fixed tendons). + env_ids: Environment indices. If None, then all indices are used. + """ + n = self._n_envs_index(env_ids) + t = len(fixed_tendon_ids) if fixed_tendon_ids else self._nft() + self.assert_shape_and_dtype(damping, (n, t), wp.float32, "damping") + if self._data._fixed_tendon_damping is not None: + self._set_target_into_buffer(self._data._fixed_tendon_damping, damping, env_ids, fixed_tendon_ids) + + def set_fixed_tendon_damping_mask( + self, + *, + damping: float | torch.Tensor | wp.array, + fixed_tendon_mask: wp.array | None = None, + env_mask: wp.array | None = None, + ) -> None: + """Set fixed tendon damping into internal buffers using masks. + + Args: + damping: Fixed tendon damping. Shape is (num_instances, num_fixed_tendons). + fixed_tendon_mask: Tendon mask. If None, then all fixed tendons are used. + env_mask: Environment mask. If None, then all the instances are updated. + """ + self.assert_shape_and_dtype(damping, (self._num_instances, self._nft()), wp.float32, "damping") + if self._data._fixed_tendon_damping is not None: + self._set_target_into_buffer_mask(self._data._fixed_tendon_damping, damping, env_mask, fixed_tendon_mask) + + def set_fixed_tendon_limit_stiffness_index( + self, + *, + limit_stiffness: float | torch.Tensor | wp.array, + fixed_tendon_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + ) -> None: + """Set fixed tendon limit stiffness into internal buffers using indices. + + Args: + limit_stiffness: Fixed tendon limit stiffness. Shape is (len(env_ids), len(fixed_tendon_ids)). + fixed_tendon_ids: The tendon indices. Defaults to None (all fixed tendons). + env_ids: Environment indices. If None, then all indices are used. + """ + n = self._n_envs_index(env_ids) + t = len(fixed_tendon_ids) if fixed_tendon_ids else self._nft() + self.assert_shape_and_dtype(limit_stiffness, (n, t), wp.float32, "limit_stiffness") + if self._data._fixed_tendon_limit_stiffness is not None: + self._set_target_into_buffer( + self._data._fixed_tendon_limit_stiffness, limit_stiffness, env_ids, fixed_tendon_ids + ) + + def set_fixed_tendon_limit_stiffness_mask( + self, + *, + limit_stiffness: float | torch.Tensor | wp.array, + fixed_tendon_mask: wp.array | None = None, + env_mask: wp.array | None = None, + ) -> None: + """Set fixed tendon limit stiffness into internal buffers using masks. + + Args: + limit_stiffness: Fixed tendon limit stiffness. Shape is (num_instances, num_fixed_tendons). + fixed_tendon_mask: Tendon mask. If None, then all fixed tendons are used. + env_mask: Environment mask. If None, then all the instances are updated. + """ + self.assert_shape_and_dtype(limit_stiffness, (self._num_instances, self._nft()), wp.float32, "limit_stiffness") + if self._data._fixed_tendon_limit_stiffness is not None: + self._set_target_into_buffer_mask( + self._data._fixed_tendon_limit_stiffness, limit_stiffness, env_mask, fixed_tendon_mask + ) + + def set_fixed_tendon_position_limit_index( + self, + *, + limit: torch.Tensor | wp.array, + fixed_tendon_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + ) -> None: + """Set fixed tendon position limits into internal buffers using indices. + + Args: + limit: Fixed tendon position limits. Shape is (len(env_ids), len(fixed_tendon_ids)) + with dtype wp.vec2f. + fixed_tendon_ids: The tendon indices. Defaults to None (all fixed tendons). + env_ids: Environment indices. If None, then all indices are used. + """ + n = self._n_envs_index(env_ids) + t = len(fixed_tendon_ids) if fixed_tendon_ids else self._nft() + self.assert_shape_and_dtype(limit, (n, t), wp.vec2f, "limit") + if self._data._fixed_tendon_pos_limits is not None: + self._set_target_into_buffer(self._data._fixed_tendon_pos_limits, limit, env_ids, fixed_tendon_ids) + + def set_fixed_tendon_position_limit_mask( + self, + *, + limit: torch.Tensor | wp.array, + fixed_tendon_mask: wp.array | None = None, + env_mask: wp.array | None = None, + ) -> None: + """Set fixed tendon position limits into internal buffers using masks. + + Args: + limit: Fixed tendon position limits. Shape is (num_instances, num_fixed_tendons) + with dtype wp.vec2f. + fixed_tendon_mask: Tendon mask. If None, then all fixed tendons are used. + env_mask: Environment mask. If None, then all the instances are updated. + """ + self.assert_shape_and_dtype(limit, (self._num_instances, self._nft()), wp.vec2f, "limit") + if self._data._fixed_tendon_pos_limits is not None: + self._set_target_into_buffer_mask(self._data._fixed_tendon_pos_limits, limit, env_mask, fixed_tendon_mask) + + def set_fixed_tendon_rest_length_index( + self, + *, + rest_length: float | torch.Tensor | wp.array, + fixed_tendon_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + ) -> None: + """Set fixed tendon rest length into internal buffers using indices. + + Args: + rest_length: Fixed tendon rest length. Shape is (len(env_ids), len(fixed_tendon_ids)). + fixed_tendon_ids: The tendon indices. Defaults to None (all fixed tendons). + env_ids: Environment indices. If None, then all indices are used. + """ + n = self._n_envs_index(env_ids) + t = len(fixed_tendon_ids) if fixed_tendon_ids else self._nft() + self.assert_shape_and_dtype(rest_length, (n, t), wp.float32, "rest_length") + if self._data._fixed_tendon_rest_length is not None: + self._set_target_into_buffer(self._data._fixed_tendon_rest_length, rest_length, env_ids, fixed_tendon_ids) + + def set_fixed_tendon_rest_length_mask( + self, + *, + rest_length: float | torch.Tensor | wp.array, + fixed_tendon_mask: wp.array | None = None, + env_mask: wp.array | None = None, + ) -> None: + """Set fixed tendon rest length into internal buffers using masks. + + Args: + rest_length: Fixed tendon rest length. Shape is (num_instances, num_fixed_tendons). + fixed_tendon_mask: Tendon mask. If None, then all fixed tendons are used. + env_mask: Environment mask. If None, then all the instances are updated. + """ + self.assert_shape_and_dtype(rest_length, (self._num_instances, self._nft()), wp.float32, "rest_length") + if self._data._fixed_tendon_rest_length is not None: + self._set_target_into_buffer_mask( + self._data._fixed_tendon_rest_length, rest_length, env_mask, fixed_tendon_mask + ) + + def set_fixed_tendon_offset_index( + self, + *, + offset: float | torch.Tensor | wp.array, + fixed_tendon_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + ) -> None: + """Set fixed tendon offset into internal buffers using indices. + + Args: + offset: Fixed tendon offset. Shape is (len(env_ids), len(fixed_tendon_ids)). + fixed_tendon_ids: The tendon indices. Defaults to None (all fixed tendons). + env_ids: Environment indices. If None, then all indices are used. + """ + n = self._n_envs_index(env_ids) + t = len(fixed_tendon_ids) if fixed_tendon_ids else self._nft() + self.assert_shape_and_dtype(offset, (n, t), wp.float32, "offset") + if self._data._fixed_tendon_offset is not None: + self._set_target_into_buffer(self._data._fixed_tendon_offset, offset, env_ids, fixed_tendon_ids) + + def set_fixed_tendon_offset_mask( + self, + *, + offset: float | torch.Tensor | wp.array, + fixed_tendon_mask: wp.array | None = None, + env_mask: wp.array | None = None, + ) -> None: + """Set fixed tendon offset into internal buffers using masks. + + Args: + offset: Fixed tendon offset. Shape is (num_instances, num_fixed_tendons). + fixed_tendon_mask: Tendon mask. If None, then all fixed tendons are used. + env_mask: Environment mask. If None, then all the instances are updated. + """ + self.assert_shape_and_dtype(offset, (self._num_instances, self._nft()), wp.float32, "offset") + if self._data._fixed_tendon_offset is not None: + self._set_target_into_buffer_mask(self._data._fixed_tendon_offset, offset, env_mask, fixed_tendon_mask) + + def write_fixed_tendon_properties_to_sim_index( + self, + *, + fixed_tendon_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + ) -> None: + """Write fixed tendon properties into the simulation using indices. + + Args: + fixed_tendon_ids: The fixed tendon indices to write the properties for. Defaults to None + (all fixed tendons). + env_ids: Environment indices. If None, then all indices are used. + """ + if self._nft() == 0: + return + for tt, buf in [ + (TT.FIXED_TENDON_STIFFNESS, self._data._fixed_tendon_stiffness), + (TT.FIXED_TENDON_DAMPING, self._data._fixed_tendon_damping), + (TT.FIXED_TENDON_LIMIT_STIFFNESS, self._data._fixed_tendon_limit_stiffness), + (TT.FIXED_TENDON_LIMIT, self._data._fixed_tendon_pos_limits), + (TT.FIXED_TENDON_REST_LENGTH, self._data._fixed_tendon_rest_length), + (TT.FIXED_TENDON_OFFSET, self._data._fixed_tendon_offset), + ]: + if buf is not None: + self._write_flat_tensor(tt, buf, env_ids, fixed_tendon_ids) + + def write_fixed_tendon_properties_to_sim_mask( + self, + *, + fixed_tendon_mask: wp.array | None = None, + env_mask: wp.array | None = None, + ) -> None: + """Write fixed tendon properties into the simulation using masks. + + Args: + fixed_tendon_mask: Fixed tendon mask. If None, then all fixed tendons are used. + env_mask: Environment mask. If None, then all the instances are updated. + """ + if self._nft() == 0: + return + for tt, buf in [ + (TT.FIXED_TENDON_STIFFNESS, self._data._fixed_tendon_stiffness), + (TT.FIXED_TENDON_DAMPING, self._data._fixed_tendon_damping), + (TT.FIXED_TENDON_LIMIT_STIFFNESS, self._data._fixed_tendon_limit_stiffness), + (TT.FIXED_TENDON_LIMIT, self._data._fixed_tendon_pos_limits), + (TT.FIXED_TENDON_REST_LENGTH, self._data._fixed_tendon_rest_length), + (TT.FIXED_TENDON_OFFSET, self._data._fixed_tendon_offset), + ]: + if buf is not None: + self._write_flat_tensor_mask(tt, buf, env_mask, fixed_tendon_mask) + + def set_spatial_tendon_stiffness_index( + self, + *, + stiffness: float | torch.Tensor | wp.array, + spatial_tendon_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + ) -> None: + """Set spatial tendon stiffness into internal buffers using indices. + + Args: + stiffness: Spatial tendon stiffness. Shape is (len(env_ids), len(spatial_tendon_ids)). + spatial_tendon_ids: The tendon indices. Defaults to None (all spatial tendons). + env_ids: Environment indices. If None, then all indices are used. + """ + n = self._n_envs_index(env_ids) + t = len(spatial_tendon_ids) if spatial_tendon_ids else self._nst() + self.assert_shape_and_dtype(stiffness, (n, t), wp.float32, "stiffness") + if self._data._spatial_tendon_stiffness is not None: + self._set_target_into_buffer(self._data._spatial_tendon_stiffness, stiffness, env_ids, spatial_tendon_ids) + + def set_spatial_tendon_stiffness_mask( + self, + *, + stiffness: float | torch.Tensor | wp.array, + spatial_tendon_mask: wp.array | None = None, + env_mask: wp.array | None = None, + ) -> None: + """Set spatial tendon stiffness into internal buffers using masks. + + Args: + stiffness: Spatial tendon stiffness. Shape is (num_instances, num_spatial_tendons). + spatial_tendon_mask: Tendon mask. If None, then all spatial tendons are used. + env_mask: Environment mask. If None, then all the instances are updated. + """ + self.assert_shape_and_dtype(stiffness, (self._num_instances, self._nst()), wp.float32, "stiffness") + if self._data._spatial_tendon_stiffness is not None: + self._set_target_into_buffer_mask( + self._data._spatial_tendon_stiffness, stiffness, env_mask, spatial_tendon_mask + ) + + def set_spatial_tendon_damping_index( + self, + *, + damping: float | torch.Tensor | wp.array, + spatial_tendon_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + ) -> None: + """Set spatial tendon damping into internal buffers using indices. + + Args: + damping: Spatial tendon damping. Shape is (len(env_ids), len(spatial_tendon_ids)). + spatial_tendon_ids: The tendon indices. Defaults to None (all spatial tendons). + env_ids: Environment indices. If None, then all indices are used. + """ + n = self._n_envs_index(env_ids) + t = len(spatial_tendon_ids) if spatial_tendon_ids else self._nst() + self.assert_shape_and_dtype(damping, (n, t), wp.float32, "damping") + if self._data._spatial_tendon_damping is not None: + self._set_target_into_buffer(self._data._spatial_tendon_damping, damping, env_ids, spatial_tendon_ids) + + def set_spatial_tendon_damping_mask( + self, + *, + damping: float | torch.Tensor | wp.array, + spatial_tendon_mask: wp.array | None = None, + env_mask: wp.array | None = None, + ) -> None: + """Set spatial tendon damping into internal buffers using masks. + + Args: + damping: Spatial tendon damping. Shape is (num_instances, num_spatial_tendons). + spatial_tendon_mask: Tendon mask. If None, then all spatial tendons are used. + env_mask: Environment mask. If None, then all the instances are updated. + """ + self.assert_shape_and_dtype(damping, (self._num_instances, self._nst()), wp.float32, "damping") + if self._data._spatial_tendon_damping is not None: + self._set_target_into_buffer_mask( + self._data._spatial_tendon_damping, damping, env_mask, spatial_tendon_mask + ) + + def set_spatial_tendon_limit_stiffness_index( + self, + *, + limit_stiffness: float | torch.Tensor | wp.array, + spatial_tendon_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + ) -> None: + """Set spatial tendon limit stiffness into internal buffers using indices. + + Args: + limit_stiffness: Spatial tendon limit stiffness. Shape is (len(env_ids), len(spatial_tendon_ids)). + spatial_tendon_ids: The tendon indices. Defaults to None (all spatial tendons). + env_ids: Environment indices. If None, then all indices are used. + """ + n = self._n_envs_index(env_ids) + t = len(spatial_tendon_ids) if spatial_tendon_ids else self._nst() + self.assert_shape_and_dtype(limit_stiffness, (n, t), wp.float32, "limit_stiffness") + if self._data._spatial_tendon_limit_stiffness is not None: + self._set_target_into_buffer( + self._data._spatial_tendon_limit_stiffness, limit_stiffness, env_ids, spatial_tendon_ids + ) + + def set_spatial_tendon_limit_stiffness_mask( + self, + *, + limit_stiffness: float | torch.Tensor | wp.array, + spatial_tendon_mask: wp.array | None = None, + env_mask: wp.array | None = None, + ) -> None: + """Set spatial tendon limit stiffness into internal buffers using masks. + + Args: + limit_stiffness: Spatial tendon limit stiffness. Shape is (num_instances, num_spatial_tendons). + spatial_tendon_mask: Tendon mask. If None, then all spatial tendons are used. + env_mask: Environment mask. If None, then all the instances are updated. + """ + self.assert_shape_and_dtype(limit_stiffness, (self._num_instances, self._nst()), wp.float32, "limit_stiffness") + if self._data._spatial_tendon_limit_stiffness is not None: + self._set_target_into_buffer_mask( + self._data._spatial_tendon_limit_stiffness, limit_stiffness, env_mask, spatial_tendon_mask + ) + + def set_spatial_tendon_offset_index( + self, + *, + offset: float | torch.Tensor | wp.array, + spatial_tendon_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + ) -> None: + """Set spatial tendon offset into internal buffers using indices. + + Args: + offset: Spatial tendon offset. Shape is (len(env_ids), len(spatial_tendon_ids)). + spatial_tendon_ids: The tendon indices. Defaults to None (all spatial tendons). + env_ids: Environment indices. If None, then all indices are used. + """ + n = self._n_envs_index(env_ids) + t = len(spatial_tendon_ids) if spatial_tendon_ids else self._nst() + self.assert_shape_and_dtype(offset, (n, t), wp.float32, "offset") + if self._data._spatial_tendon_offset is not None: + self._set_target_into_buffer(self._data._spatial_tendon_offset, offset, env_ids, spatial_tendon_ids) + + def set_spatial_tendon_offset_mask( + self, + *, + offset: float | torch.Tensor | wp.array, + spatial_tendon_mask: wp.array | None = None, + env_mask: wp.array | None = None, + ) -> None: + """Set spatial tendon offset into internal buffers using masks. + + Args: + offset: Spatial tendon offset. Shape is (num_instances, num_spatial_tendons). + spatial_tendon_mask: Tendon mask. If None, then all spatial tendons are used. + env_mask: Environment mask. If None, then all the instances are updated. + """ + self.assert_shape_and_dtype(offset, (self._num_instances, self._nst()), wp.float32, "offset") + if self._data._spatial_tendon_offset is not None: + self._set_target_into_buffer_mask(self._data._spatial_tendon_offset, offset, env_mask, spatial_tendon_mask) + + def write_spatial_tendon_properties_to_sim_index( + self, + *, + spatial_tendon_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + ) -> None: + """Write spatial tendon properties into the simulation using indices. + + Args: + spatial_tendon_ids: The spatial tendon indices to write the properties for. Defaults to None + (all spatial tendons). + env_ids: Environment indices. If None, then all indices are used. + """ + if self._nst() == 0: + return + for tt, buf in [ + (TT.SPATIAL_TENDON_STIFFNESS, self._data._spatial_tendon_stiffness), + (TT.SPATIAL_TENDON_DAMPING, self._data._spatial_tendon_damping), + (TT.SPATIAL_TENDON_LIMIT_STIFFNESS, self._data._spatial_tendon_limit_stiffness), + (TT.SPATIAL_TENDON_OFFSET, self._data._spatial_tendon_offset), + ]: + if buf is not None: + self._write_flat_tensor(tt, buf, env_ids, spatial_tendon_ids) + + def write_spatial_tendon_properties_to_sim_mask( + self, + *, + spatial_tendon_mask: wp.array | None = None, + env_mask: wp.array | None = None, + ) -> None: + """Write spatial tendon properties into the simulation using masks. + + Args: + spatial_tendon_mask: Spatial tendon mask. If None, then all spatial tendons are used. + env_mask: Environment mask. If None, then all the instances are updated. + """ + if self._nst() == 0: + return + for tt, buf in [ + (TT.SPATIAL_TENDON_STIFFNESS, self._data._spatial_tendon_stiffness), + (TT.SPATIAL_TENDON_DAMPING, self._data._spatial_tendon_damping), + (TT.SPATIAL_TENDON_LIMIT_STIFFNESS, self._data._spatial_tendon_limit_stiffness), + (TT.SPATIAL_TENDON_OFFSET, self._data._spatial_tendon_offset), + ]: + if buf is not None: + self._write_flat_tensor_mask(tt, buf, env_mask, spatial_tendon_mask) + + """ + Deprecated methods. + """ + + def write_root_state_to_sim( + self, + root_state: torch.Tensor | wp.array, + env_ids: Sequence[int] | wp.array | None = None, + ) -> None: + """Deprecated in base class. Use :meth:`write_root_pose_to_sim_index` and + :meth:`write_root_velocity_to_sim_index` instead.""" + self._write_root_state(TT.ROOT_POSE, root_state[:, :7], env_ids) + self._write_root_state(TT.ROOT_VELOCITY, root_state[:, 7:], env_ids) + + def write_root_com_state_to_sim( + self, + root_state: torch.Tensor | wp.array, + env_ids: Sequence[int] | wp.array | None = None, + ) -> None: + """Deprecated in base class. Use :meth:`write_root_com_pose_to_sim_index` and + :meth:`write_root_com_velocity_to_sim_index` instead.""" + self._write_root_state(TT.ROOT_POSE, root_state[:, :7], env_ids) + self._write_root_state(TT.ROOT_VELOCITY, root_state[:, 7:], env_ids) + + def write_root_link_state_to_sim( + self, + root_state: torch.Tensor | wp.array, + env_ids: Sequence[int] | wp.array | None = None, + ) -> None: + """Deprecated in base class. Use :meth:`write_root_link_pose_to_sim_index` and + :meth:`write_root_link_velocity_to_sim_index` instead.""" + self._write_root_state(TT.ROOT_POSE, root_state[:, :7], env_ids) + self._write_root_state(TT.ROOT_VELOCITY, root_state[:, 7:], env_ids) + + def write_joint_state_to_sim( + self, + position: torch.Tensor | wp.array, + velocity: torch.Tensor | wp.array, + joint_ids: Sequence[int] | None = None, + env_ids: Sequence[int] | wp.array | None = None, + ) -> None: + """Deprecated in base class. Use :meth:`write_joint_position_to_sim_index` and + :meth:`write_joint_velocity_to_sim_index` instead.""" + self.write_joint_position_to_sim_index(position=position, joint_ids=joint_ids, env_ids=env_ids) + self.write_joint_velocity_to_sim_index(velocity=velocity, joint_ids=joint_ids, env_ids=env_ids) + + """ + Internal helper. + """ + + def _initialize_impl(self) -> None: + physx_instance = OvPhysxManager.get_physx_instance() + if physx_instance is None: + raise RuntimeError("OvPhysxManager has not been initialized yet.") + + prim_path = self.cfg.prim_path + # Convert IsaacLab prim-path notation to the glob patterns ovphysx expects. + # IsaacLab uses two conventions: + # /World/envs/env_.*/Robot -- regex dot-star for "any env index" + # /World/envs/{ENV_REGEX_NS}/Robot -- explicit placeholder + # ovphysx create_tensor_binding() uses fnmatch-style globs, so both map to '*'. + pattern = re.sub(r"\{ENV_REGEX_NS\}", "*", prim_path) + pattern = re.sub(r"\.\*", "*", pattern) # env_.* -> env_* + + # The pattern above points to the ArticulationCfg prim (e.g. /World/envs/env_*/Robot). + # However, PhysicsArticulationRootAPI may be on a CHILD prim (e.g. /Robot/torso) + # rather than on the prim itself. create_tensor_binding() only matches prims that + # *have* PhysicsArticulationRootAPI, so we need to extend the pattern to the actual + # articulation root. Mirror the PhysX backend's discovery logic: find the first + # matching prim in the USD stage, walk its subtree for the articulation root, and + # append the relative suffix to the glob pattern. + from pxr import UsdPhysics + + from isaaclab.sim.utils.queries import find_first_matching_prim, get_all_matching_child_prims + + stage = PhysicsManager._sim.stage + first_prim = find_first_matching_prim(prim_path, stage=stage) + if first_prim is None: + raise RuntimeError(f"OvPhysxManager: no prim found for path '{prim_path}'.") + first_prim_path = first_prim.GetPath().pathString + + root_prims = get_all_matching_child_prims( + first_prim_path, + predicate=lambda p: p.HasAPI(UsdPhysics.ArticulationRootAPI), + traverse_instance_prims=False, + ) + if len(root_prims) == 0: + raise RuntimeError( + f"No prim with PhysicsArticulationRootAPI found under '{first_prim_path}'." + " Check that the articulation has 'PhysicsArticulationRootAPI' applied." + ) + if len(root_prims) > 1: + raise RuntimeError( + f"Multiple articulation roots found under '{first_prim_path}': {root_prims}." + " There must be exactly one articulation root per prim path." + ) + self._articulation_root_path = root_prims[0].GetPath().pathString + root_relative = self._articulation_root_path[len(first_prim_path) :] + if root_relative: + # e.g. first_prim_path=/World/envs/env_0/Robot, root_relative=/torso + # pattern becomes /World/envs/env_*/Robot/torso + pattern = pattern + root_relative + logger.info("OvPhysxManager: articulation root at '%s' (pattern extended to '%s')", root_relative, pattern) + + # Bindings are created lazily (on first access) to avoid allocating + # handles for tensor types the user never queries. Only the root-pose + # binding is created eagerly because we need it to read articulation + # metadata (joint count, body count, names, fixed-base flag). + self._bindings: dict[int, Any] = {} + self._physx_instance = physx_instance + self._binding_pattern = pattern + + eager_types = [ + TT.ROOT_POSE, + TT.DOF_POSITION, + TT.DOF_STIFFNESS, + TT.DOF_DAMPING, + TT.DOF_LIMIT, + TT.DOF_MAX_VELOCITY, + TT.DOF_MAX_FORCE, + TT.DOF_ARMATURE, + TT.DOF_FRICTION_PROPERTIES, + TT.BODY_MASS, + TT.BODY_COM_POSE, + TT.BODY_INERTIA, + ] + for tt in eager_types: + try: + self._bindings[tt] = physx_instance.create_tensor_binding(pattern=pattern, tensor_type=tt) + except Exception: + logger.debug("Could not create tensor binding for type %s on pattern %s", tt, pattern) + + sample = next(iter(self._bindings.values())) + self._num_instances = sample.count + self._num_joints = sample.dof_count + self._num_bodies = sample.body_count + self._is_fixed_base = sample.is_fixed_base + self._joint_names = list(sample.dof_names) + self._body_names = list(sample.body_names) + + # Create data container. + self._data = ArticulationData(self._bindings, self._device, binding_getter=self._get_binding) + + # Discover tendon counts/names before buffer allocation so that + # _create_buffers can size the tendon property arrays. + self._process_tendons() + + self._create_buffers() + + self._process_cfg() + self._process_actuators_cfg() + self._validate_cfg() + self._log_articulation_info() + + # Cache the effort binding and a stable float32 view of the applied_torque + # buffer for write_data_to_sim(). The binding's internal write cache + # (keyed on object identity) handles the fast path automatically. + self._effort_binding = self._get_binding(TT.DOF_ACTUATION_FORCE) + if self._effort_binding is not None: + torque = self._data.applied_torque + shape = self._effort_binding.shape + self._effort_write_view = wp.array( + ptr=torque.ptr, + shape=shape, + dtype=wp.float32, + device=str(torque.device), + copy=False, + ) + else: + self._effort_write_view = None + + # Cache position/velocity target bindings + views for one-shot writes. + def _make_write_view(tt, buf): + b = self._get_binding(tt) + if b is None or buf is None: + return None, None + v = wp.array(ptr=buf.ptr, shape=b.shape, dtype=wp.float32, device=str(buf.device), copy=False) + return b, v + + self._pos_target_binding, self._pos_target_write_view = _make_write_view( + TT.DOF_POSITION_TARGET, self._data.joint_pos_target + ) + self._vel_target_binding, self._vel_target_write_view = _make_write_view( + TT.DOF_VELOCITY_TARGET, self._data.joint_vel_target + ) + + # Let the articulation data know that it is fully instantiated and ready to use. + self.data.is_primed = True + + def _create_buffers(self) -> None: + self._data._create_buffers() + + self._ALL_INDICES = wp.array(np.arange(self._num_instances, dtype=np.int32), device=self._device) + + self._instantaneous_wrench_composer = WrenchComposer(self) + self._permanent_wrench_composer = WrenchComposer(self) + self._wrench_buf = wp.zeros((self._num_instances, self._num_bodies, 9), dtype=wp.float32, device=self._device) + + # Joint-index arrays for each actuator (filled by _process_actuators_cfg). + self._joint_ids_per_actuator: dict[str, list[int]] = {} + self._write_scratch: dict[int, wp.array] = {} + + def _process_cfg(self) -> None: + """Process the articulation configuration (initial state, soft limits, etc.).""" + cfg = self.cfg + N = self._num_instances + D = self._num_joints + dev = self._device + + # Default root state from config (matching PhysX pattern). + default_root_pose = tuple(cfg.init_state.pos) + tuple(cfg.init_state.rot) + default_root_vel = tuple(cfg.init_state.lin_vel) + tuple(cfg.init_state.ang_vel) + np_pose = np.tile(np.array(default_root_pose, dtype=np.float32), (N, 1)) + np_vel = np.tile(np.array(default_root_vel, dtype=np.float32), (N, 1)) + wp.copy( + self._data._default_root_pose, + wp.from_numpy(np_pose, dtype=wp.transformf, device=dev), + ) + wp.copy( + self._data._default_root_vel, + wp.from_numpy(np_vel, dtype=wp.spatial_vectorf, device=dev), + ) + + # Default joint positions / velocities from config patterns. + self._resolve_joint_values(cfg.init_state.joint_pos, self._data._default_joint_pos) + self._resolve_joint_values(cfg.init_state.joint_vel, self._data._default_joint_vel) + + # Keep soft-limit computation on-device, matching the PhysX/Newton path. + wp.launch( + update_soft_joint_pos_limits, + dim=(N, D), + inputs=[self._data.joint_pos_limits, cfg.soft_joint_pos_limit_factor], + outputs=[self._data._soft_joint_pos_limits], + device=dev, + ) + + def _invalidate_initialize_callback(self, event) -> None: + self._is_initialized = False + + def _process_actuators_cfg(self) -> None: + """Build actuator instances from the config and write drive properties to PhysX. + + Mirrors what the legacy PhysX backend does in its own _process_actuators_cfg: + - For ImplicitActuator: write the configured stiffness / damping to the PhysX + drive so the solver uses exactly the values from the actuator config. + - For all explicit actuators: zero out PhysX stiffness / damping so the + USD-authored drive gains cannot interfere with the explicit torque path. + - For all actuators: write effort_limit_sim and velocity_limit_sim. + + These writes happen via TensorBinding (GPU-resident) after warmup has + allocated the GPU buffers (MODEL_INIT fires post-warmup). + """ + from isaaclab.actuators import ImplicitActuator + + self.actuators: dict[str, ActuatorBase] = {} + self._has_implicit_actuators = False + for name, act_cfg in self.cfg.actuators.items(): + joint_ids, joint_names = self.find_joints(act_cfg.joint_names_expr) + if not joint_ids: + logger.warning("Actuator '%s': no joints matched '%s'", name, act_cfg.joint_names_expr) + continue + act_cfg_copy = act_cfg.copy() + act = act_cfg_copy.class_type( + act_cfg_copy, + joint_names=joint_names, + joint_ids=joint_ids, + num_envs=self._num_instances, + device=self._device, + ) + self.actuators[name] = act + self._joint_ids_per_actuator[name] = joint_ids + + # Write drive gains and limits to PhysX to match the actuator config. + # Without this, PhysX retains whatever stiffness/damping was authored in the + # USD file, which can produce large restoring forces if the USD gains differ + # from the actuator config (e.g. a position-controlled robot exported with + # non-zero drive stiffness but configured with ImplicitActuator(stiffness=0)). + jids = list(joint_ids) + if isinstance(act, ImplicitActuator): + self._has_implicit_actuators = True + stiffness = act.stiffness # torch (N, J) + damping = act.damping # torch (N, J) + else: + stiffness = wp.zeros((self._num_instances, len(jids)), dtype=wp.float32, device=self._device) + damping = wp.zeros((self._num_instances, len(jids)), dtype=wp.float32, device=self._device) + self.write_joint_stiffness_to_sim_index(stiffness=stiffness, joint_ids=jids) + self.write_joint_damping_to_sim_index(damping=damping, joint_ids=jids) + self.write_joint_effort_limit_to_sim_index(limits=act.effort_limit_sim, joint_ids=jids) + self.write_joint_velocity_limit_to_sim_index(limits=act.velocity_limit_sim, joint_ids=jids) + + def _process_tendons(self) -> None: + """Discover tendon counts from binding metadata and names from USD. + + Tendon counts come from the ovphysx binding metadata. Tendon names are + recovered from the exported USD articulation subtree because ovphysx + exposes joint names/counts, but not the per-joint USD paths that the + PhysX backend can query directly. + """ + self._fixed_tendon_names = [] + self._spatial_tendon_names = [] + + sample = next(iter(self._bindings.values())) + self._num_fixed_tendons = getattr(sample, "fixed_tendon_count", 0) + self._num_spatial_tendons = getattr(sample, "spatial_tendon_count", 0) + + if self._num_fixed_tendons > 0 or self._num_spatial_tendons > 0: + stage_path = OvPhysxManager._stage_path + if stage_path is not None: + try: + from pxr import Usd, UsdPhysics + + from isaaclab.sim.utils.queries import get_all_matching_child_prims + + stage = Usd.Stage.Open(stage_path) + articulation_root_path = getattr(self, "_articulation_root_path", None) + if articulation_root_path is None: + joint_prims = stage.Traverse() + else: + joint_prims = get_all_matching_child_prims( + articulation_root_path, + predicate=lambda p: p.IsA(UsdPhysics.Joint), + stage=stage, + traverse_instance_prims=False, + ) + for prim in joint_prims: + if not prim.IsA(UsdPhysics.Joint): + continue + schema_names = list(prim.GetAppliedSchemas()) + metadata = prim.GetMetadata("apiSchemas") + if metadata is not None: + for field in ("prependedItems", "appendedItems", "explicitItems"): + items = getattr(metadata, field, None) + if items: + schema_names.extend(str(item) for item in items) + schemas_str = " ".join(schema_names) + name = prim.GetPath().name + if "PhysxTendonAxisRootAPI" in schemas_str: + self._fixed_tendon_names.append(name) + elif ( + "PhysxTendonAttachmentRootAPI" in schemas_str + or "PhysxTendonAttachmentLeafAPI" in schemas_str + ): + self._spatial_tendon_names.append(name) + except Exception: + logger.debug("Could not parse USD stage for tendon names at %s", stage_path) + + self._data._num_fixed_tendons = self._num_fixed_tendons + self._data._num_spatial_tendons = self._num_spatial_tendons + self._data.fixed_tendon_names = self._fixed_tendon_names + self._data.spatial_tendon_names = self._spatial_tendon_names + + def _apply_external_wrenches(self) -> None: + """Compose and write external wrenches to the LINK_WRENCH binding. + + WrenchComposer accumulates forces/torques in body (link) frame. + The LINK_WRENCH binding expects world-frame [fx,fy,fz,tx,ty,tz,px,py,pz]. + We rotate the body-frame vectors to world frame using the link quaternion + and pack them into the [N, L, 9] tensor with application position = origin. + """ + inst = self._instantaneous_wrench_composer + perm = self._permanent_wrench_composer + if not inst.active and not perm.active: + return + if inst.active: + if perm.active: + inst.add_forces_and_torques_index( + forces=perm.composed_force, + torques=perm.composed_torque, + body_ids=list(range(self._num_bodies)), + env_ids=list(range(self._num_instances)), + ) + force_b = inst.composed_force + torque_b = inst.composed_torque + else: + force_b = perm.composed_force + torque_b = perm.composed_torque + + poses = self._data.body_link_pose_w + wp.launch( + _body_wrench_to_world, + dim=(self._num_instances, self._num_bodies), + inputs=[force_b, torque_b, poses], + outputs=[self._wrench_buf], + device=self._device, + ) + wrench_binding = self._get_binding(TT.LINK_WRENCH) + if wrench_binding is not None: + wrench_binding.write(self._wrench_buf) + inst.reset() + + def _apply_actuator_model(self) -> None: + """Run the actuator model to compute torques from user targets. + + IsaacLab actuators are torch-based. We convert warp -> torch via + DLPack (zero-copy on GPU), run the actuator, then write results back. + """ + from isaaclab.utils.types import ArticulationActions + + for name, act in self.actuators.items(): + jids = act.joint_indices + if jids is None: + continue + jids_t = jids if isinstance(jids, list) else list(jids) + all_joints = len(jids_t) == self._num_joints + + # warp -> torch (zero-copy on same device via DLPack) + jp_target_full = wp.to_torch(self._data.joint_pos_target) + jv_target_full = wp.to_torch(self._data.joint_vel_target) + je_target_full = wp.to_torch(self._data.joint_effort_target) + jp_target = jp_target_full if all_joints else jp_target_full[:, jids_t] + jv_target = jv_target_full if all_joints else jv_target_full[:, jids_t] + je_target = je_target_full if all_joints else je_target_full[:, jids_t] + + control_action = ArticulationActions( + joint_positions=jp_target, + joint_velocities=jv_target, + joint_efforts=je_target, + ) + + jp_cur_full = wp.to_torch(self._data.joint_pos) + jv_cur_full = wp.to_torch(self._data.joint_vel) + jp_cur = jp_cur_full if all_joints else jp_cur_full[:, jids_t] + jv_cur = jv_cur_full if all_joints else jv_cur_full[:, jids_t] + + control_action = act.compute(control_action, jp_cur, jv_cur) + + if act.computed_effort is not None: + ct = wp.to_torch(self._data._computed_torque) + at = wp.to_torch(self._data._applied_torque) + if all_joints: + ct[:] = act.computed_effort + at[:] = act.applied_effort + else: + ct[:, jids_t] = act.computed_effort + at[:, jids_t] = act.applied_effort + + def _validate_cfg(self) -> None: + pass + + def _log_articulation_info(self) -> None: + """Log information about the articulation. + + .. note:: We purposefully read the values from the simulator to ensure that the values are configured as + expected. + """ + from prettytable import PrettyTable + + def format_large_number(_, v: float) -> str: + if abs(v) >= 1e3: + return f"{v:.1e}" + return f"{v:.3f}" + + def format_limits(_, v: tuple[float, float]) -> str: + if abs(v[0]) >= 1e3 or abs(v[1]) >= 1e3: + return f"[{v[0]:.1e}, {v[1]:.1e}]" + return f"[{v[0]:.3f}, {v[1]:.3f}]" + + stiffnesses = self.data.joint_stiffness.numpy()[0].tolist() + dampings = self.data.joint_damping.numpy()[0].tolist() + armatures = self.data.joint_armature.numpy()[0].tolist() + frictions = self.data.joint_friction_coeff.numpy()[0].tolist() + pos_limits_np = self.data.joint_pos_limits.numpy().reshape(self._num_instances, self._num_joints, 2) + position_limits = [tuple(pos_limits_np[0, j].tolist()) for j in range(self._num_joints)] + velocity_limits = self.data.joint_vel_limits.numpy()[0].tolist() + effort_limits = self.data.joint_effort_limits.numpy()[0].tolist() + + joint_table = PrettyTable() + joint_table.title = f"Simulation Joint Information (Prim path: {self.cfg.prim_path})" + joint_table.field_names = [ + "Index", + "Name", + "Stiffness", + "Damping", + "Armature", + "Friction", + "Position Limits", + "Velocity Limits", + "Effort Limits", + ] + joint_table.custom_format["Stiffness"] = format_large_number + joint_table.custom_format["Damping"] = format_large_number + joint_table.custom_format["Armature"] = format_large_number + joint_table.custom_format["Friction"] = format_large_number + joint_table.custom_format["Position Limits"] = format_limits + joint_table.custom_format["Velocity Limits"] = format_large_number + joint_table.custom_format["Effort Limits"] = format_large_number + joint_table.align["Name"] = "l" + + for index, name in enumerate(self.joint_names): + joint_table.add_row( + [ + index, + name, + stiffnesses[index], + dampings[index], + armatures[index], + frictions[index], + position_limits[index], + velocity_limits[index], + effort_limits[index], + ] + ) + logger.info(f"Simulation parameters for joints in {self.cfg.prim_path}:\n" + joint_table.get_string()) + + if self.num_fixed_tendons > 0: + ft_stiffnesses = self.data.fixed_tendon_stiffness.numpy()[0].tolist() + ft_dampings = self.data.fixed_tendon_damping.numpy()[0].tolist() + ft_limit_stiffnesses = self.data.fixed_tendon_limit_stiffness.numpy()[0].tolist() + ft_limits_np = self.data.fixed_tendon_pos_limits.numpy().reshape( + self._num_instances, self.num_fixed_tendons, 2 + ) + ft_limits = [tuple(ft_limits_np[0, t].tolist()) for t in range(self.num_fixed_tendons)] + ft_rest_lengths = self.data.fixed_tendon_rest_length.numpy()[0].tolist() + ft_offsets = self.data.fixed_tendon_offset.numpy()[0].tolist() + + tendon_table = PrettyTable() + tendon_table.title = f"Simulation Fixed Tendon Information (Prim path: {self.cfg.prim_path})" + tendon_table.field_names = [ + "Index", + "Stiffness", + "Damping", + "Limit Stiffness", + "Limits", + "Rest Length", + "Offset", + ] + tendon_table.custom_format["Stiffness"] = format_large_number + tendon_table.custom_format["Damping"] = format_large_number + tendon_table.custom_format["Limit Stiffness"] = format_large_number + tendon_table.custom_format["Limits"] = format_limits + tendon_table.custom_format["Rest Length"] = format_large_number + tendon_table.custom_format["Offset"] = format_large_number + for index in range(self.num_fixed_tendons): + tendon_table.add_row( + [ + index, + ft_stiffnesses[index], + ft_dampings[index], + ft_limit_stiffnesses[index], + ft_limits[index], + ft_rest_lengths[index], + ft_offsets[index], + ] + ) + logger.info( + f"Simulation parameters for fixed tendons in {self.cfg.prim_path}:\n" + tendon_table.get_string() + ) + + if self.num_spatial_tendons > 0: + st_stiffnesses = self.data.spatial_tendon_stiffness.numpy()[0].tolist() + st_dampings = self.data.spatial_tendon_damping.numpy()[0].tolist() + st_limit_stiffnesses = self.data.spatial_tendon_limit_stiffness.numpy()[0].tolist() + st_offsets = self.data.spatial_tendon_offset.numpy()[0].tolist() + + tendon_table = PrettyTable() + tendon_table.title = f"Simulation Spatial Tendon Information (Prim path: {self.cfg.prim_path})" + tendon_table.field_names = [ + "Index", + "Stiffness", + "Damping", + "Limit Stiffness", + "Offset", + ] + tendon_table.float_format = ".3" + for index in range(self.num_spatial_tendons): + tendon_table.add_row( + [ + index, + st_stiffnesses[index], + st_dampings[index], + st_limit_stiffnesses[index], + st_offsets[index], + ] + ) + logger.info( + f"Simulation parameters for spatial tendons in {self.cfg.prim_path}:\n" + tendon_table.get_string() + ) + + """ + Internal helpers -- Bindings. + """ + + def _get_binding(self, tensor_type: int): + """Return a cached TensorBinding, creating it on first access. + + Bindings are lightweight handles (a pointer + shape metadata into + PhysX's shared GPU buffer). Creating one does NOT allocate new GPU + memory -- the underlying simulation buffers are allocated once by PhysX + regardless of how many bindings point into them. Still, we defer + creation so that tensor types the user never queries are never looked up. + """ + binding = self._bindings.get(tensor_type) + if binding is not None: + return binding + try: + binding = self._physx_instance.create_tensor_binding(pattern=self._binding_pattern, tensor_type=tensor_type) + self._bindings[tensor_type] = binding + return binding + except Exception: + logger.debug("Could not create tensor binding for type %s", tensor_type) + return None + + """ + Internal helpers -- Write. + """ + + def _to_flat_f32(self, data, target_shape: tuple[int, ...] | None = None) -> wp.array | np.ndarray: + """Ensure data is a contiguous float32 tensor suitable for binding I/O. + + State tensor bindings (positions, velocities, poses) live on the + simulation device (GPU in GPU mode). We always return data on + self._device so the binding device check passes. + + For structured warp dtypes (transformf, spatial_vectorf, etc.) a + zero-copy flat float32 view is created instead of roundtripping + through CPU numpy. + """ + dev = self._device + if isinstance(data, wp.array): + if str(data.device) != dev: + data = wp.clone(data, device=dev) + if data.dtype == wp.float32: + return data + # Structured dtype: zero-copy flat float32 view. + # transformf -> [N, 7], spatial_vectorf -> [N, 6], etc. + floats_per_elem = data.strides[0] // 4 + return wp.array( + ptr=data.ptr, + shape=(data.shape[0], floats_per_elem), + dtype=wp.float32, + device=dev, + copy=False, + ) + elif isinstance(data, torch.Tensor): + if data.is_cuda and dev.startswith("cuda"): + return wp.from_torch(data.detach().contiguous().float()) + np_data = data.detach().cpu().numpy().astype(np.float32) + return wp.from_numpy(np_data, dtype=wp.float32, device=dev) + elif isinstance(data, np.ndarray): + return wp.from_numpy(data.astype(np.float32), dtype=wp.float32, device=dev) + elif isinstance(data, (int, float)): + return wp.from_numpy(np.array(data, dtype=np.float32), dtype=wp.float32, device=dev) + return wp.from_numpy(np.asarray(data, dtype=np.float32), dtype=wp.float32, device=dev) + + def _as_gpu_f32_2d(self, data, cols: int) -> wp.array: + """View/convert data as 2D [rows, cols] float32 on self._device. + + For warp arrays with structured dtypes (transformf, spatial_vectorf), + creates a zero-copy flat float32 view. For torch/numpy, converts to + warp on the simulation device. + """ + dev = self._device + if isinstance(data, wp.array): + if str(data.device) != dev: + data = wp.clone(data, device=dev) + if data.dtype == wp.float32 and data.ndim == 2: + return data + n = data.shape[0] + return wp.array( + ptr=data.ptr, + shape=(n, cols), + dtype=wp.float32, + device=dev, + copy=False, + ) + if isinstance(data, torch.Tensor) and data.is_cuda and dev.startswith("cuda"): + return wp.from_torch(data.detach().contiguous().float().reshape(-1, cols)) + np_data = self._to_cpu_numpy(data).reshape(-1, cols) + return wp.from_numpy(np_data, dtype=wp.float32, device=dev) + + def _get_write_scratch(self, tensor_type: int, binding) -> wp.array: + """Return a cached GPU scratch buffer for read-modify-write.""" + if not hasattr(self, "_write_scratch"): + self._write_scratch = {} + buf = self._write_scratch.get(tensor_type) + if buf is None: + buf = wp.zeros(binding.shape, dtype=wp.float32, device=self._device) + self._write_scratch[tensor_type] = buf + return buf + + def _write_root_state(self, tensor_type: int, data, env_ids=None, mask=None, _ids_gpu=None) -> None: + """GPU-native write for root pose [N,7] or velocity [N,6]. + + Three paths, fastest first: + - Full write (no env_ids, no mask): zero-copy DLPack. + - Indexed write with full-size data: zero-copy view + indices. + The binding API only copies the indexed rows from the full buffer, + so no read-modify-write is needed when data is already [N,...]. + - Indexed write with partial data [K,...]: scatter kernel into a GPU + scratch buffer, then write with indices. + - Masked write: data is always full [N,...], pass directly with mask. + + Args: + _ids_gpu: Pre-converted GPU warp int32 array of env indices. + When provided, skips the per-call GPU->CPU->GPU conversion + of env_ids. + """ + binding = self._get_binding(tensor_type) + if binding is None: + return + N, C = binding.shape + + if env_ids is None and _ids_gpu is None and mask is None: + binding.write(self._to_flat_f32(data)) + self._invalidate_root_caches(tensor_type) + return + + src = self._as_gpu_f32_2d(data, C) + + if env_ids is not None or _ids_gpu is not None: + if _ids_gpu is None: + _ids_gpu = self._env_ids_to_gpu_warp(env_ids) + K = _ids_gpu.shape[0] + if src.shape[0] == N: + binding.write(src, indices=_ids_gpu) + else: + scratch = self._get_write_scratch(tensor_type, binding) + binding.read(scratch) + wp.launch( + _scatter_rows_partial, + dim=(K, C), + inputs=[scratch, src, _ids_gpu], + device=self._device, + ) + binding.write(scratch, indices=_ids_gpu) + else: + mask_u8 = wp.from_numpy( + self._to_cpu_numpy(mask).astype(np.uint8), + device=self._device, + ) + binding.write(src, mask=mask_u8) + self._invalidate_root_caches(tensor_type) + + def _invalidate_root_caches(self, tensor_type: int) -> None: + """Force re-read from GPU on next property access after a binding write.""" + if tensor_type == TT.ROOT_POSE: + self.data._root_link_pose_w.timestamp = -1.0 + self.data._root_com_pose_w.timestamp = -1.0 + elif tensor_type == TT.ROOT_VELOCITY: + self.data._root_link_vel_w.timestamp = -1.0 + self.data._root_com_vel_w.timestamp = -1.0 + + def _write_flat_tensor(self, tensor_type: int, data, env_ids=None, joint_ids=None, _ids_gpu=None) -> None: + """Write a 2-D tensor to a binding, with optional env/joint index subsetting.""" + if isinstance(data, (int, float)): + return + binding = self._get_binding(tensor_type) + if binding is None: + return + from isaaclab_ovphysx.tensor_types import _CPU_ONLY_TYPES + + is_cpu_only = tensor_type in _CPU_ONLY_TYPES + + # CPU-only types or column scatter must go through numpy. + if is_cpu_only or joint_ids is not None: + target_device = "cpu" if is_cpu_only else self._device + np_data = self._to_cpu_numpy(data) + if joint_ids is not None: + if is_cpu_only: + full = np.zeros(binding.shape, dtype=np.float32) + binding.read(full) + else: + scratch = self._get_write_scratch(tensor_type, binding) + binding.read(scratch) + full = scratch.numpy() + jids = self._to_cpu_indices(joint_ids, np.intp) + if env_ids is not None: + eids = self._to_cpu_indices(env_ids, np.intp) + full[np.ix_(eids, jids)] = np_data.reshape(len(eids), len(jids), *np_data.shape[2:]) + else: + full[:, jids] = np_data.reshape(full.shape[0], len(jids), *np_data.shape[2:]) + binding.write(wp.from_numpy(full, dtype=wp.float32, device=target_device)) + elif env_ids is not None: + if is_cpu_only: + full = np.zeros(binding.shape, dtype=np.float32) + binding.read(full) + else: + scratch = self._get_write_scratch(tensor_type, binding) + binding.read(scratch) + full = scratch.numpy() + eids = self._to_cpu_indices(env_ids, np.intp) + full[eids] = np_data if np_data.shape[0] == len(eids) else np_data[eids] + flat = wp.from_numpy(full.astype(np.float32), dtype=wp.float32, device=target_device) + idx = _ids_gpu if _ids_gpu is not None else self._env_ids_to_gpu_warp(env_ids) + binding.write(flat, indices=idx) + else: + binding.write(wp.from_numpy(np_data.astype(np.float32), dtype=wp.float32, device=target_device)) + return + + # GPU path: data stays on device. + if env_ids is None and _ids_gpu is None: + binding.write(self._to_flat_f32(data)) + return + + N, C = binding.shape[0], binding.shape[1] + src = self._as_gpu_f32_2d(data, C) + if _ids_gpu is None: + _ids_gpu = self._env_ids_to_gpu_warp(env_ids) + K = _ids_gpu.shape[0] + if src.shape[0] == N: + binding.write(src, indices=_ids_gpu) + else: + scratch = self._get_write_scratch(tensor_type, binding) + binding.read(scratch) + wp.launch( + _scatter_rows_partial, + dim=(K, C), + inputs=[scratch, src, _ids_gpu], + device=self._device, + ) + binding.write(scratch, indices=_ids_gpu) + + def _write_flat_tensor_mask(self, tensor_type: int, data, env_mask=None, joint_mask=None) -> None: + """Write a 2-D tensor to a binding, with optional env/joint mask subsetting.""" + if isinstance(data, (int, float)): + return + binding = self._get_binding(tensor_type) + if binding is None: + return + from isaaclab_ovphysx.tensor_types import _CPU_ONLY_TYPES + + is_cpu_only = tensor_type in _CPU_ONLY_TYPES + + # CPU-only types or column-mask scatter must go through numpy. + if is_cpu_only or joint_mask is not None: + target_device = "cpu" if is_cpu_only else self._device + np_data = self._to_cpu_numpy(data) + if joint_mask is not None: + # GPU bindings cannot read into numpy directly; read into GPU + # scratch first, then pull to CPU for column scatter. + if is_cpu_only: + full = np.zeros(binding.shape, dtype=np.float32) + binding.read(full) + else: + scratch = self._get_write_scratch(tensor_type, binding) + binding.read(scratch) + full = scratch.numpy() + jmask = self._to_cpu_numpy(joint_mask).astype(bool) + cols = np.where(jmask)[0] + if env_mask is not None: + emask = self._to_cpu_numpy(env_mask).astype(bool) + rows = np.where(emask)[0] + full[rows[:, None], cols] = np_data[rows[:, None], cols] + else: + full[:, cols] = np_data[:, cols] + binding.write(wp.from_numpy(full.astype(np.float32), dtype=wp.float32, device=target_device)) + elif env_mask is not None: + flat = wp.from_numpy(np_data.astype(np.float32), dtype=wp.float32, device=target_device) + mask_u8 = wp.from_numpy( + self._to_cpu_numpy(env_mask).astype(np.uint8), + device=target_device, + ) + binding.write(flat, mask=mask_u8) + else: + binding.write(wp.from_numpy(np_data.astype(np.float32), dtype=wp.float32, device=target_device)) + return + + # GPU path: data stays on device. + if env_mask is None: + binding.write(self._to_flat_f32(data)) + return + + # Data is full [N, D], the binding API selects rows via the mask. + mask_u8 = wp.from_numpy( + self._to_cpu_numpy(env_mask).astype(np.uint8), + device=self._device, + ) + binding.write(self._to_flat_f32(data), mask=mask_u8) + + def _write_friction_column(self, data, env_ids=None, joint_ids=None) -> None: + """Write static friction coefficient into column 0 of DOF_FRICTION_PROPERTIES [N,D,3].""" + binding = self._get_binding(TT.DOF_FRICTION_PROPERTIES) + if binding is None: + return + full = np.zeros(binding.shape, dtype=np.float32) + binding.read(full) + if isinstance(data, (int, float)): + if env_ids is not None and joint_ids is not None: + eids = self._to_cpu_numpy(env_ids).astype(np.intp) + jids = self._to_cpu_indices(joint_ids, np.intp) + full[np.ix_(eids, jids, [0])] = data + elif env_ids is not None: + eids = self._to_cpu_numpy(env_ids).astype(np.intp) + full[eids, :, 0] = data + elif joint_ids is not None: + jids = self._to_cpu_indices(joint_ids, np.intp) + full[:, jids, 0] = data + else: + full[..., 0] = data + binding.write(wp.from_numpy(full.astype(np.float32), dtype=wp.float32, device="cpu")) + return + np_data = self._to_cpu_numpy(data) + if env_ids is not None and joint_ids is not None: + eids = self._to_cpu_numpy(env_ids).astype(np.intp) + jids = self._to_cpu_indices(joint_ids, np.intp) + full[np.ix_(eids, jids, [0])] = np_data.reshape(len(eids), len(jids), 1) + elif env_ids is not None: + eids = self._to_cpu_numpy(env_ids).astype(np.intp) + full[eids, :, 0] = np_data.reshape(len(eids), -1) + elif joint_ids is not None: + jids = self._to_cpu_indices(joint_ids, np.intp) + full[:, jids, 0] = np_data.reshape(full.shape[0], len(jids)) + else: + full[..., 0] = np_data.reshape(full.shape[0], full.shape[1]) + binding.write(wp.from_numpy(full.astype(np.float32), dtype=wp.float32, device="cpu")) + + def _write_friction_column_mask(self, data, env_mask=None, joint_mask=None) -> None: + """Write static friction coefficient via mask into column 0 of DOF_FRICTION_PROPERTIES.""" + binding = self._get_binding(TT.DOF_FRICTION_PROPERTIES) + if binding is None: + return + full = np.zeros(binding.shape, dtype=np.float32) + binding.read(full) + if isinstance(data, (int, float)): + new_col = np.full((full.shape[0], full.shape[1]), data, dtype=np.float32) + else: + new_col = self._to_cpu_numpy(data).reshape(full.shape[0], full.shape[1]) + if env_mask is not None: + emask = self._to_cpu_numpy(env_mask).astype(bool) + if joint_mask is not None: + jmask = self._to_cpu_numpy(joint_mask).astype(bool) + rows = np.where(emask)[0] + cols = np.where(jmask)[0] + full[rows[:, None], cols, 0] = new_col[rows[:, None], cols] + else: + full[emask, :, 0] = new_col[emask] + elif joint_mask is not None: + jmask = self._to_cpu_numpy(joint_mask).astype(bool) + full[:, jmask, 0] = new_col[:, jmask] + else: + full[..., 0] = new_col + binding.write(wp.from_numpy(full.astype(np.float32), dtype=wp.float32, device="cpu")) + + def _write_joint_subset(self, tensor_type: int, buffer: wp.array, joint_ids: list[int]) -> None: + """Write a full-width joint buffer into the simulation for an actuator's joints.""" + binding = self._get_binding(tensor_type) + if binding is None: + return + if not hasattr(self, "_write_dltensor_cache"): + self._write_dltensor_cache = {} + cache_key = (tensor_type, buffer.ptr) + cached = self._write_dltensor_cache.get(cache_key) + if cached is None: + flat = self._to_flat_f32(buffer) + from ovphysx._dlpack_utils import acquire_dltensor + + dl, keepalive = acquire_dltensor(flat) + self._write_dltensor_cache[cache_key] = (dl, keepalive, flat) + cached = self._write_dltensor_cache[cache_key] + binding.write(cached[0]) + + @staticmethod + def _to_cpu_numpy(data) -> np.ndarray: + """Convert data (warp, torch, numpy, scalar) to a CPU numpy array.""" + if isinstance(data, wp.array): + return data.numpy().astype(np.float32) + if isinstance(data, torch.Tensor): + return data.detach().cpu().numpy().astype(np.float32) + return np.asarray(data, dtype=np.float32) + + @staticmethod + def _to_cpu_indices(data, dtype=np.int32) -> np.ndarray: + """Convert index array (warp, torch, list, numpy) to CPU numpy int array.""" + if isinstance(data, torch.Tensor): + return data.detach().cpu().numpy().astype(dtype) + if isinstance(data, wp.array): + return data.numpy().astype(dtype) + return np.asarray(data, dtype=dtype) + + def _env_ids_to_gpu_warp(self, env_ids) -> wp.array: + """Convert env_ids to a GPU int32 warp array, with single-entry caching. + + The cache avoids repeated GPU -> CPU -> GPU round-trips when the same + ``env_ids`` object is passed to multiple binding writes in a single step + (e.g. reset writes root_pose, root_vel, joint_pos, joint_vel). A new + object identity (``id()``) or shape change invalidates the cache. + """ + if hasattr(env_ids, "data_ptr"): + key = (env_ids.data_ptr(), env_ids.shape[0]) + elif isinstance(env_ids, wp.array): + key = (env_ids.ptr, env_ids.shape[0]) + else: + key = None + + if key is not None and hasattr(self, "_ids_cache_key") and self._ids_cache_key == key: + return self._ids_cache_val + + result = wp.array(self._to_cpu_indices(env_ids, np.int32), device=self._device) + if key is not None: + self._ids_cache_key = key + self._ids_cache_val = result + return result + + def _set_target_into_buffer(self, buffer: wp.array, data, env_ids=None, joint_ids=None) -> None: + """Set user-provided target data into a warp command buffer. + + For the common case (no index subset), this uses wp.copy to stay on + the simulation device. Subset writes (specific env_ids or joint_ids) + fall back to CPU because warp does not support scatter indexing. + """ + # Fast path: all-joints shortcut. When joint_ids covers every joint + # and env_ids is None, the subset is equivalent to a full copy. + if joint_ids is not None and env_ids is None: + n_joints = buffer.shape[1] if len(buffer.shape) > 1 else 1 + if hasattr(joint_ids, "__len__") and len(joint_ids) == n_joints: + joint_ids = None + if env_ids is None and joint_ids is None: + src = self._to_flat_f32(data) + if isinstance(src, np.ndarray): + src = wp.from_numpy(src, dtype=wp.float32, device=buffer.device) + wp.copy(buffer, src) + else: + np_data = self._to_cpu_numpy(data) + buf_np = buffer.numpy() + env_idx = self._to_cpu_numpy(env_ids).astype(np.intp) if env_ids is not None else None + jnt_idx = self._to_cpu_numpy(joint_ids).astype(np.intp) if joint_ids is not None else None + if env_idx is not None and jnt_idx is not None: + buf_np[np.ix_(env_idx, jnt_idx)] = np_data + elif env_idx is not None: + buf_np[env_idx] = np_data + else: + buf_np[:, jnt_idx] = np_data + wp.copy(buffer, wp.from_numpy(buf_np, dtype=wp.float32, device=buffer.device)) + + def _set_target_into_buffer_mask(self, buffer: wp.array, data, env_mask=None, joint_mask=None) -> None: + """Set user-provided target data into a warp command buffer using masks.""" + if env_mask is None: + src = self._to_flat_f32(data) + if isinstance(src, np.ndarray): + src = wp.from_numpy(src, dtype=wp.float32, device=buffer.device) + wp.copy(buffer, src) + else: + np_data = self._to_cpu_numpy(data) + buf_np = buffer.numpy() + mask_np = self._to_cpu_numpy(env_mask).astype(bool) + buf_np[mask_np] = np_data[mask_np] + wp.copy(buffer, wp.from_numpy(buf_np, dtype=wp.float32, device=buffer.device)) + + """ + Internal helpers -- Utilities. + """ + + def _n_envs_index(self, env_ids): + """Return the number of environments from an env_ids argument.""" + if env_ids is None: + return self._num_instances + if isinstance(env_ids, (list, tuple)): + return len(env_ids) + return env_ids.shape[0] if hasattr(env_ids, "shape") else len(env_ids) + + def _nft(self): + """Return the number of fixed tendons (0 if none).""" + return getattr(self, "_num_fixed_tendons", 0) + + def _nst(self): + """Return the number of spatial tendons (0 if none).""" + return getattr(self, "_num_spatial_tendons", 0) + + @staticmethod + def _find_names(names: list[str], keys: str | Sequence[str], preserve_order: bool) -> tuple[list[int], list[str]]: + if isinstance(keys, str): + keys = [keys] + matched_indices: list[int] = [] + matched_names: list[str] = [] + if preserve_order: + for key in keys: + for idx, name in enumerate(names): + if fnmatch.fnmatch(name, key) or re.fullmatch(key, name): + if idx not in matched_indices: + matched_indices.append(idx) + matched_names.append(name) + else: + for idx, name in enumerate(names): + for key in keys: + if fnmatch.fnmatch(name, key) or re.fullmatch(key, name): + matched_indices.append(idx) + matched_names.append(name) + break + return matched_indices, matched_names + + def _resolve_joint_values(self, pattern_dict: dict[str, float], buffer: wp.array) -> None: + """Resolve a {pattern: value} dict into a per-joint buffer. + + Builds values on CPU then copies to buffer's device (GPU arrays' + .numpy() returns a read-only copy, not a writable view). + """ + buf_np = buffer.numpy() + modified = False + for pattern, value in pattern_dict.items(): + for j, name in enumerate(self._joint_names): + if re.fullmatch(pattern, name): + buf_np[:, j] = value + modified = True + if modified: + wp.copy(buffer, wp.from_numpy(buf_np, dtype=buffer.dtype, device=str(buffer.device))) diff --git a/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/articulation/articulation_data.py b/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/articulation/articulation_data.py new file mode 100644 index 000000000000..84c06675a022 --- /dev/null +++ b/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/articulation/articulation_data.py @@ -0,0 +1,1517 @@ +# 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 + +"""Articulation data backed by ovphysx TensorBindingsAPI.""" + +from __future__ import annotations + +import warnings +from typing import Any + +import numpy as np +import warp as wp + +from isaaclab.assets.articulation.base_articulation_data import BaseArticulationData +from isaaclab.utils.buffers import TimestampedBufferWarp as TimestampedBuffer + +from isaaclab_ovphysx import tensor_types as TT + +from .kernels import ( + _compose_body_com_poses, + _compose_root_com_pose, + _compute_heading, + _copy_first_body, + _fd_joint_acc, + _projected_gravity, + _world_vel_to_body_ang, + _world_vel_to_body_lin, +) + + +class ArticulationData(BaseArticulationData): + """Data container for an articulation backed by ovphysx tensor bindings. + + This class contains the data for an articulation in the simulation. The data includes the state of + the root rigid body, the state of all the bodies in the articulation, and the joint state. The data is + stored in the simulation world frame unless otherwise specified. + + An articulation is comprised of multiple rigid bodies or links. For a rigid body, there are two frames + of reference that are used: + + - Actor frame: The frame of reference of the rigid body prim. This typically corresponds to the Xform prim + with the rigid body schema. + - Center of mass frame: The frame of reference of the center of mass of the rigid body. + + Depending on the settings, the two frames may not coincide with each other. In the robotics sense, the actor frame + can be interpreted as the link frame. + + Uses ovphysx :class:`TensorBinding` objects to lazily read simulation state into warp + arrays. Writes happen via the :class:`Articulation` class. + """ + + __backend_name__: str = "ovphysx" + """The name of the backend for the articulation data.""" + + def __init__(self, bindings: dict[int, Any], device: str, binding_getter=None): + """Initialize the articulation data. + + Args: + bindings: Mapping from ovphysx tensor type constant to a + live TensorBinding for this articulation. + device: The compute device (``"cpu"`` or ``"cuda:N"``). + binding_getter: Optional callable(tensor_type) -> TensorBinding + that lazily creates bindings on first access. When provided, + ``_get_binding()`` delegates to this instead of only checking + the static ``bindings`` dict. + """ + super().__init__(root_view=None, device=device) + self._bindings = bindings + self._binding_getter = binding_getter + self._sim_timestamp: float = 0.0 + self._is_primed = False + + # Metadata from an arbitrary articulation binding. + sample = next(iter(bindings.values())) + self._num_instances = sample.count + self._num_joints = sample.dof_count + self._num_bodies = sample.body_count + self._is_fixed_base = sample.is_fixed_base + + self.body_names = list(sample.body_names) + self.joint_names = list(sample.dof_names) + self.fixed_tendon_names: list[str] = [] + self.spatial_tendon_names: list[str] = [] + + self._num_fixed_tendons = 0 + self._num_spatial_tendons = 0 + + # Initialize parametric gravity and forward vectors (matching PhysX/Newton pattern). + # Guard against None sim context (e.g. mock/test environments). + from isaaclab.physics import PhysicsManager + + gravity = (0.0, 0.0, -9.81) + if PhysicsManager._sim is not None and hasattr(PhysicsManager._sim, "cfg"): + gravity = PhysicsManager._sim.cfg.gravity + gravity_np = np.array(gravity, dtype=np.float32) + gravity_mag = np.linalg.norm(gravity_np) + if gravity_mag == 0.0: + gravity_dir = np.array([0.0, 0.0, -1.0], dtype=np.float32) + else: + gravity_dir = gravity_np / gravity_mag + gravity_dir_tiled = np.tile(gravity_dir, (self._num_instances, 1)) + forward_tiled = np.tile(np.array([1.0, 0.0, 0.0], dtype=np.float32), (self._num_instances, 1)) + + self.GRAVITY_VEC_W = wp.from_numpy(gravity_dir_tiled, dtype=wp.vec3f, device=device) + self.FORWARD_VEC_B = wp.from_numpy(forward_tiled, dtype=wp.vec3f, device=device) + + def update(self, dt: float) -> None: + """Update the data for the articulation. + + Args: + dt: The time step for the update [s]. This must be a positive value. + """ + self._sim_timestamp += dt + + # Finite-difference joint acceleration from velocity. + if dt > 0.0 and self._previous_joint_vel is not None: + cur_vel = self.joint_vel + wp.launch( + _fd_joint_acc, + dim=(self._num_instances, self._num_joints), + inputs=[cur_vel, self._previous_joint_vel, 1.0 / dt], + outputs=[self._joint_acc.data], + device=self.device, + ) + self._joint_acc.timestamp = self._sim_timestamp + + @property + def is_primed(self) -> bool: + """Whether the articulation data is fully instantiated and ready to use.""" + return self._is_primed + + @is_primed.setter + def is_primed(self, value: bool) -> None: + """Set whether the articulation data is fully instantiated and ready to use. + + .. note:: + Once this quantity is set to True, it cannot be changed. + + Args: + value: The primed state. + + Raises: + ValueError: If the articulation data is already primed. + """ + if self._is_primed: + raise ValueError("The articulation data is already primed.") + self._is_primed = True + + """ + Names. + """ + + body_names: list[str] = None + """Body names in the order parsed by the simulation view.""" + + joint_names: list[str] = None + """Joint names in the order parsed by the simulation view.""" + + fixed_tendon_names: list[str] = None + """Fixed tendon names in the order parsed by the simulation view.""" + + spatial_tendon_names: list[str] = None + """Spatial tendon names in the order parsed by the simulation view.""" + + """ + Defaults - Initial state. + """ + + @property + def default_root_pose(self) -> wp.array: + """Default root pose ``[pos, quat]`` in the local environment frame. + + The position and quaternion are of the articulation root's actor frame. + Shape is (num_instances,), dtype = wp.transformf. In torch this resolves to (num_instances, 7). + """ + return self._default_root_pose + + @default_root_pose.setter + def default_root_pose(self, value: wp.array) -> None: + """Set the default root pose. + + Args: + value: The default root pose. Shape is (num_instances, 7). + + Raises: + ValueError: If the articulation data is already primed. + """ + if self.is_primed: + raise ValueError("The articulation data is already primed.") + self._default_root_pose.assign(value) + + @property + def default_root_vel(self) -> wp.array: + """Default root velocity ``[lin_vel, ang_vel]`` in the local environment frame. + + The linear and angular velocities are of the articulation root's center of mass frame. + Shape is (num_instances,), dtype = wp.spatial_vectorf. In torch this resolves to (num_instances, 6). + """ + return self._default_root_vel + + @default_root_vel.setter + def default_root_vel(self, value: wp.array) -> None: + """Set the default root velocity. + + Args: + value: The default root velocity. Shape is (num_instances, 6). + + Raises: + ValueError: If the articulation data is already primed. + """ + if self.is_primed: + raise ValueError("The articulation data is already primed.") + self._default_root_vel.assign(value) + + @property + def default_joint_pos(self) -> wp.array: + """Default joint positions of all joints [m or rad, depending on joint type]. + + Shape is (num_instances, num_joints), dtype = wp.float32. In torch this resolves to + (num_instances, num_joints). + + This quantity is configured through the :attr:`isaaclab.assets.ArticulationCfg.init_state` parameter. + """ + return self._default_joint_pos + + @default_joint_pos.setter + def default_joint_pos(self, value: wp.array) -> None: + """Set the default joint positions. + + Args: + value: The default joint positions. Shape is (num_instances, num_joints). + + Raises: + ValueError: If the articulation data is already primed. + """ + if self.is_primed: + raise ValueError("The articulation data is already primed.") + self._default_joint_pos.assign(value) + + @property + def default_joint_vel(self) -> wp.array: + """Default joint velocities of all joints [m/s or rad/s, depending on joint type]. + + Shape is (num_instances, num_joints), dtype = wp.float32. In torch this resolves to + (num_instances, num_joints). + + This quantity is configured through the :attr:`isaaclab.assets.ArticulationCfg.init_state` parameter. + """ + return self._default_joint_vel + + @default_joint_vel.setter + def default_joint_vel(self, value: wp.array) -> None: + """Set the default joint velocities. + + Args: + value: The default joint velocities. Shape is (num_instances, num_joints). + + Raises: + ValueError: If the articulation data is already primed. + """ + if self.is_primed: + raise ValueError("The articulation data is already primed.") + self._default_joint_vel.assign(value) + + """ + Joint commands -- Set into simulation. + """ + + @property + def joint_pos_target(self) -> wp.array: + """Joint position targets commanded by the user [m or rad, depending on joint type]. + + Shape is (num_instances, num_joints), dtype = wp.float32. In torch this resolves to + (num_instances, num_joints). + + For an implicit actuator model, the targets are directly set into the simulation. + For an explicit actuator model, the targets are used to compute the joint torques + (see :attr:`applied_torque`), which are then set into the simulation. + """ + return self._joint_pos_target + + @property + def joint_vel_target(self) -> wp.array: + """Joint velocity targets commanded by the user [m/s or rad/s, depending on joint type]. + + Shape is (num_instances, num_joints), dtype = wp.float32. In torch this resolves to + (num_instances, num_joints). + + For an implicit actuator model, the targets are directly set into the simulation. + For an explicit actuator model, the targets are used to compute the joint torques + (see :attr:`applied_torque`), which are then set into the simulation. + """ + return self._joint_vel_target + + @property + def joint_effort_target(self) -> wp.array: + """Joint effort targets commanded by the user [N or N*m, depending on joint type]. + + Shape is (num_instances, num_joints), dtype = wp.float32. In torch this resolves to + (num_instances, num_joints). + + For an implicit actuator model, the targets are directly set into the simulation. + For an explicit actuator model, the targets are used to compute the joint torques + (see :attr:`applied_torque`), which are then set into the simulation. + """ + return self._joint_effort_target + + """ + Joint commands -- Explicit actuators. + """ + + @property + def computed_torque(self) -> wp.array: + """Joint torques computed from the actuator model (before clipping) [N*m]. + + Shape is (num_instances, num_joints), dtype = wp.float32. In torch this resolves to + (num_instances, num_joints). + + This quantity is the raw torque output from the actuator model, before any clipping is applied. + It is exposed for users who want to inspect the computations inside the actuator model. + For instance, to penalize the learning agent for a difference between the computed and applied torques. + """ + return self._computed_torque + + @property + def applied_torque(self) -> wp.array: + """Joint torques applied from the actuator model (after clipping) [N*m]. + + Shape is (num_instances, num_joints), dtype = wp.float32. In torch this resolves to + (num_instances, num_joints). + + These torques are set into the simulation, after clipping the :attr:`computed_torque` based on the + actuator model. + """ + return self._applied_torque + + """ + Joint properties + """ + + @property + def joint_stiffness(self) -> wp.array: + """Joint stiffness provided to the simulation. + + Shape is (num_instances, num_joints), dtype = wp.float32. In torch this resolves to + (num_instances, num_joints). + + In the case of explicit actuators, the value for the corresponding joints is zero. + """ + return self._joint_stiffness + + @property + def joint_damping(self) -> wp.array: + """Joint damping provided to the simulation. + + Shape is (num_instances, num_joints), dtype = wp.float32. In torch this resolves to + (num_instances, num_joints). + + In the case of explicit actuators, the value for the corresponding joints is zero. + """ + return self._joint_damping + + @property + def joint_armature(self) -> wp.array: + """Joint armature provided to the simulation. + + Shape is (num_instances, num_joints), dtype = wp.float32. In torch this resolves to + (num_instances, num_joints). + """ + return self._joint_armature + + @property + def joint_friction_coeff(self) -> wp.array: + """Joint static friction coefficient provided to the simulation. + + Shape is (num_instances, num_joints), dtype = wp.float32. In torch this resolves to + (num_instances, num_joints). + """ + return self._joint_friction_coeff + + @property + def joint_pos_limits(self) -> wp.array: + """Joint position limits provided to the simulation. + + Shape is (num_instances, num_joints), dtype = wp.vec2f. In torch this resolves to + (num_instances, num_joints, 2). + + The limits are in the order :math:`[lower, upper]`. + """ + return self._joint_pos_limits + + @property + def joint_vel_limits(self) -> wp.array: + """Joint maximum velocity provided to the simulation [m/s or rad/s, depending on joint type]. + + Shape is (num_instances, num_joints), dtype = wp.float32. In torch this resolves to + (num_instances, num_joints). + """ + return self._joint_vel_limits + + @property + def joint_effort_limits(self) -> wp.array: + """Joint maximum effort provided to the simulation [N or N*m, depending on joint type]. + + Shape is (num_instances, num_joints), dtype = wp.float32. In torch this resolves to + (num_instances, num_joints). + """ + return self._joint_effort_limits + + """ + Joint properties - Custom. + """ + + @property + def soft_joint_pos_limits(self) -> wp.array: + r"""Soft joint position limits for all joints. + + Shape is (num_instances, num_joints), dtype = wp.vec2f. In torch this resolves to + (num_instances, num_joints, 2). + + The limits are in the order :math:`[lower, upper]`. The soft joint position limits are computed as + a sub-region of the :attr:`joint_pos_limits` based on the + :attr:`~isaaclab.assets.ArticulationCfg.soft_joint_pos_limit_factor` parameter. + + Consider the joint position limits :math:`[lower, upper]` and the soft joint position limits + :math:`[soft\_lower, soft\_upper]`. The soft joint position limits are computed as: + + .. math:: + + soft\_lower = (lower + upper) / 2 - factor * (upper - lower) / 2 + soft\_upper = (lower + upper) / 2 + factor * (upper - lower) / 2 + + The soft joint position limits help specify a safety region around the joint limits. It isn't used by the + simulation, but is useful for learning agents to prevent the joint positions from violating the limits. + """ + return self._soft_joint_pos_limits + + @property + def soft_joint_vel_limits(self) -> wp.array: + """Soft joint velocity limits for all joints. + + Shape is (num_instances, num_joints), dtype = wp.float32. In torch this resolves to + (num_instances, num_joints). + + These are obtained from the actuator model. It may differ from :attr:`joint_vel_limits` if the actuator model + has a variable velocity limit model. For instance, in a variable gear ratio actuator model. + """ + return self._soft_joint_vel_limits + + @property + def gear_ratio(self) -> wp.array: + """Gear ratio for relating motor torques to applied joint torques. + + Shape is (num_instances, num_joints), dtype = wp.float32. In torch this resolves to + (num_instances, num_joints). + """ + return self._gear_ratio + + """ + Fixed tendon properties. + """ + + @property + def fixed_tendon_stiffness(self) -> wp.array: + """Fixed tendon stiffness provided to the simulation. + + Shape is (num_instances, num_fixed_tendons), dtype = wp.float32. In torch this resolves to + (num_instances, num_fixed_tendons). + """ + return self._fixed_tendon_stiffness + + @property + def fixed_tendon_damping(self) -> wp.array: + """Fixed tendon damping provided to the simulation. + + Shape is (num_instances, num_fixed_tendons), dtype = wp.float32. In torch this resolves to + (num_instances, num_fixed_tendons). + """ + return self._fixed_tendon_damping + + @property + def fixed_tendon_limit_stiffness(self) -> wp.array: + """Fixed tendon limit stiffness provided to the simulation. + + Shape is (num_instances, num_fixed_tendons), dtype = wp.float32. In torch this resolves to + (num_instances, num_fixed_tendons). + """ + return self._fixed_tendon_limit_stiffness + + @property + def fixed_tendon_rest_length(self) -> wp.array: + """Fixed tendon rest length provided to the simulation. + + Shape is (num_instances, num_fixed_tendons), dtype = wp.float32. In torch this resolves to + (num_instances, num_fixed_tendons). + """ + return self._fixed_tendon_rest_length + + @property + def fixed_tendon_offset(self) -> wp.array: + """Fixed tendon offset provided to the simulation. + + Shape is (num_instances, num_fixed_tendons), dtype = wp.float32. In torch this resolves to + (num_instances, num_fixed_tendons). + """ + return self._fixed_tendon_offset + + @property + def fixed_tendon_pos_limits(self) -> wp.array: + """Fixed tendon position limits provided to the simulation. + + Shape is (num_instances, num_fixed_tendons), dtype = wp.vec2f. In torch this resolves to + (num_instances, num_fixed_tendons, 2). + """ + return self._fixed_tendon_pos_limits + + """ + Spatial tendon properties. + """ + + @property + def spatial_tendon_stiffness(self) -> wp.array: + """Spatial tendon stiffness provided to the simulation. + + Shape is (num_instances, num_spatial_tendons), dtype = wp.float32. In torch this resolves to + (num_instances, num_spatial_tendons). + """ + return self._spatial_tendon_stiffness + + @property + def spatial_tendon_damping(self) -> wp.array: + """Spatial tendon damping provided to the simulation. + + Shape is (num_instances, num_spatial_tendons), dtype = wp.float32. In torch this resolves to + (num_instances, num_spatial_tendons). + """ + return self._spatial_tendon_damping + + @property + def spatial_tendon_limit_stiffness(self) -> wp.array: + """Spatial tendon limit stiffness provided to the simulation. + + Shape is (num_instances, num_spatial_tendons), dtype = wp.float32. In torch this resolves to + (num_instances, num_spatial_tendons). + """ + return self._spatial_tendon_limit_stiffness + + @property + def spatial_tendon_offset(self) -> wp.array: + """Spatial tendon offset provided to the simulation. + + Shape is (num_instances, num_spatial_tendons), dtype = wp.float32. In torch this resolves to + (num_instances, num_spatial_tendons). + """ + return self._spatial_tendon_offset + + """ + Root state properties. + """ + + @property + def root_link_pose_w(self) -> wp.array: + """Root link pose ``[pos, quat]`` in simulation world frame. + Shape is (num_instances,), dtype = wp.transformf. In torch this resolves to (num_instances, 7). + + This quantity is the pose of the articulation root's actor frame relative to the world. + The orientation is provided in (x, y, z, w) format. + """ + self._read_transform_binding(TT.ROOT_POSE, self._root_link_pose_w) + return self._root_link_pose_w.data + + @property + def root_link_vel_w(self) -> wp.array: + """Root link velocity ``[lin_vel, ang_vel]`` in simulation world frame. + Shape is (num_instances,), dtype = wp.spatial_vectorf. In torch this resolves to (num_instances, 6). + + This quantity contains the linear and angular velocities of the articulation root's actor frame + relative to the world. + """ + # ovphysx ROOT_VELOCITY is COM velocity; link velocity comes from the first + # element of the per-link velocity tensor. + self._read_spatial_vector_binding(TT.LINK_VELOCITY, self._body_link_vel_w) + if self._root_link_vel_w.timestamp < self._sim_timestamp: + wp.launch( + _copy_first_body, + dim=self._num_instances, + inputs=[self._body_link_vel_w.data], + outputs=[self._root_link_vel_w.data], + device=self.device, + ) + self._root_link_vel_w.timestamp = self._sim_timestamp + return self._root_link_vel_w.data + + @property + def root_com_pose_w(self) -> wp.array: + """Root center of mass pose ``[pos, quat]`` in simulation world frame. + Shape is (num_instances,), dtype = wp.transformf. In torch this resolves to (num_instances, 7). + + This quantity is the pose of the articulation root's center of mass frame relative to the world. + The orientation is provided in (x, y, z, w) format. + """ + if self._root_com_pose_w.timestamp < self._sim_timestamp: + wp.launch( + _compose_root_com_pose, + dim=self._num_instances, + inputs=[self.root_link_pose_w, self.body_com_pose_b], + outputs=[self._root_com_pose_w.data], + device=self.device, + ) + self._root_com_pose_w.timestamp = self._sim_timestamp + return self._root_com_pose_w.data + + @property + def root_com_vel_w(self) -> wp.array: + """Root center of mass velocity ``[lin_vel, ang_vel]`` in simulation world frame. + Shape is (num_instances,), dtype = wp.spatial_vectorf. In torch this resolves to (num_instances, 6). + + This quantity contains the linear and angular velocities of the articulation root's center of mass frame + relative to the world. + """ + self._read_spatial_vector_binding(TT.ROOT_VELOCITY, self._root_com_vel_w) + return self._root_com_vel_w.data + + """ + Body state properties. + """ + + @property + def body_mass(self) -> wp.array: + """Body mass in the world frame [kg]. + + Shape is (num_instances, num_bodies), dtype = wp.float32. In torch this resolves to + (num_instances, num_bodies). + """ + return self._body_mass + + @property + def body_inertia(self) -> wp.array: + """Flattened body inertia in the world frame [kg*m^2]. + + Shape is (num_instances, num_bodies, 9), dtype = wp.float32. In torch this resolves to + (num_instances, num_bodies, 9). + + Stored as a flattened 3x3 inertia matrix per body. + """ + return self._body_inertia + + @property + def body_link_pose_w(self) -> wp.array: + """Body link pose ``[pos, quat]`` in simulation world frame. + Shape is (num_instances, num_bodies), dtype = wp.transformf. In torch this resolves to + (num_instances, num_bodies, 7). + + This quantity is the pose of the articulation links' actor frame relative to the world. + The orientation is provided in (x, y, z, w) format. + """ + self._read_transform_binding(TT.LINK_POSE, self._body_link_pose_w) + return self._body_link_pose_w.data + + @property + def body_link_vel_w(self) -> wp.array: + """Body link velocity ``[lin_vel, ang_vel]`` in simulation world frame. + Shape is (num_instances, num_bodies), dtype = wp.spatial_vectorf. In torch this resolves to + (num_instances, num_bodies, 6). + + This quantity contains the linear and angular velocities of the articulation links' actor frame + relative to the world. + """ + self._read_spatial_vector_binding(TT.LINK_VELOCITY, self._body_link_vel_w) + return self._body_link_vel_w.data + + @property + def body_com_pose_w(self) -> wp.array: + """Body center of mass pose ``[pos, quat]`` in simulation world frame. + Shape is (num_instances, num_bodies), dtype = wp.transformf. In torch this resolves to + (num_instances, num_bodies, 7). + + This quantity is the pose of the center of mass frame of the articulation links relative to the world. + The orientation is provided in (x, y, z, w) format. + """ + if self._body_com_pose_w.timestamp < self._sim_timestamp: + wp.launch( + _compose_body_com_poses, + dim=(self._num_instances, self._num_bodies), + inputs=[self.body_link_pose_w, self.body_com_pose_b], + outputs=[self._body_com_pose_w.data], + device=self.device, + ) + self._body_com_pose_w.timestamp = self._sim_timestamp + return self._body_com_pose_w.data + + @property + def body_com_vel_w(self) -> wp.array: + """Body center of mass velocity ``[lin_vel, ang_vel]`` in simulation world frame. + Shape is (num_instances, num_bodies), dtype = wp.spatial_vectorf. In torch this resolves to + (num_instances, num_bodies, 6). + + This quantity contains the linear and angular velocities of the articulation links' center of mass frame + relative to the world. + + .. note:: + This is currently approximated using the link velocity. A proper COM velocity derivation + accounting for the COM offset is not yet implemented. + """ + return self.body_link_vel_w + + @property + def body_com_acc_w(self) -> wp.array: + """Acceleration of all bodies center of mass ``[lin_acc, ang_acc]`` [m/s^2, rad/s^2]. + Shape is (num_instances, num_bodies), dtype = wp.spatial_vectorf. In torch this resolves to + (num_instances, num_bodies, 6). + + All values are relative to the world. + """ + self._read_spatial_vector_binding(TT.LINK_ACCELERATION, self._body_com_acc_w) + return self._body_com_acc_w.data + + @property + def body_com_pose_b(self) -> wp.array: + """Center of mass pose ``[pos, quat]`` of all bodies in their respective body's link frames. + Shape is (num_instances, num_bodies), dtype = wp.transformf. In torch this resolves to + (num_instances, num_bodies, 7). + + This quantity is the pose of the center of mass frame of the rigid body relative to the body's link frame. + The orientation is provided in (x, y, z, w) format. + """ + self._read_transform_binding(TT.BODY_COM_POSE, self._body_com_pose_b) + return self._body_com_pose_b.data + + @property + def body_incoming_joint_wrench_b(self) -> wp.array: + """Incoming joint wrenches on each body in the body frame [N, N*m]. + + Shape is (num_instances, num_bodies), dtype = wp.spatial_vectorf. In torch this resolves to + (num_instances, num_bodies, 6). + + All body reaction wrenches are provided including the root body to the world of an articulation. + """ + self._read_spatial_vector_binding( + TT.LINK_INCOMING_JOINT_FORCE, + self._body_incoming_joint_wrench_buf, + ) + return self._body_incoming_joint_wrench_buf.data + + """ + Joint state properties. + """ + + @property + def joint_pos(self) -> wp.array: + """Joint positions of all joints [m or rad, depending on joint type]. + + Shape is (num_instances, num_joints), dtype = wp.float32. In torch this resolves to + (num_instances, num_joints). + """ + self._read_binding_into_buf(TT.DOF_POSITION, self._joint_pos_buf) + return self._joint_pos_buf.data + + @property + def joint_vel(self) -> wp.array: + """Joint velocities of all joints [m/s or rad/s, depending on joint type]. + + Shape is (num_instances, num_joints), dtype = wp.float32. In torch this resolves to + (num_instances, num_joints). + """ + self._read_binding_into_buf(TT.DOF_VELOCITY, self._joint_vel_buf) + return self._joint_vel_buf.data + + @property + def joint_acc(self) -> wp.array: + """Joint acceleration of all joints [m/s^2 or rad/s^2, depending on joint type]. + + Shape is (num_instances, num_joints), dtype = wp.float32. In torch this resolves to + (num_instances, num_joints). + + .. note:: + This quantity is computed via finite differencing of joint velocities. + """ + return self._joint_acc.data + + """ + Derived Properties. + """ + + @property + def projected_gravity_b(self) -> wp.array: + """Projection of the gravity direction on base frame. + Shape is (num_instances,), dtype = wp.vec3f. In torch this resolves to (num_instances, 3). + """ + if self._projected_gravity_b.timestamp < self._sim_timestamp: + wp.launch( + _projected_gravity, + dim=self._num_instances, + inputs=[self.GRAVITY_VEC_W, self.root_link_pose_w], + outputs=[self._projected_gravity_b.data], + device=self.device, + ) + self._projected_gravity_b.timestamp = self._sim_timestamp + return self._projected_gravity_b.data + + @property + def heading_w(self) -> wp.array: + """Yaw heading of the base frame (in radians). + Shape is (num_instances,), dtype = wp.float32. + + .. note:: + This quantity is computed by assuming that the forward-direction of the base + frame is along x-direction, i.e. :math:`(1, 0, 0)`. + """ + if self._heading_w.timestamp < self._sim_timestamp: + wp.launch( + _compute_heading, + dim=self._num_instances, + inputs=[self.FORWARD_VEC_B, self.root_link_pose_w], + outputs=[self._heading_w.data], + device=self.device, + ) + self._heading_w.timestamp = self._sim_timestamp + return self._heading_w.data + + @property + def root_link_lin_vel_b(self) -> wp.array: + """Root link linear velocity in base frame [m/s]. + Shape is (num_instances,), dtype = wp.vec3f. In torch this resolves to (num_instances, 3). + + This quantity is the linear velocity of the articulation root's actor frame with respect to its actor frame. + """ + if self._root_link_lin_vel_b.timestamp < self._sim_timestamp: + wp.launch( + _world_vel_to_body_lin, + dim=self._num_instances, + inputs=[self.root_link_pose_w, self.root_link_vel_w], + outputs=[self._root_link_lin_vel_b.data], + device=self.device, + ) + self._root_link_lin_vel_b.timestamp = self._sim_timestamp + return self._root_link_lin_vel_b.data + + @property + def root_link_ang_vel_b(self) -> wp.array: + """Root link angular velocity in base frame [rad/s]. + Shape is (num_instances,), dtype = wp.vec3f. In torch this resolves to (num_instances, 3). + + This quantity is the angular velocity of the articulation root's actor frame with respect to its actor frame. + """ + if self._root_link_ang_vel_b.timestamp < self._sim_timestamp: + wp.launch( + _world_vel_to_body_ang, + dim=self._num_instances, + inputs=[self.root_link_pose_w, self.root_link_vel_w], + outputs=[self._root_link_ang_vel_b.data], + device=self.device, + ) + self._root_link_ang_vel_b.timestamp = self._sim_timestamp + return self._root_link_ang_vel_b.data + + @property + def root_com_lin_vel_b(self) -> wp.array: + """Root center of mass linear velocity in base frame [m/s]. + Shape is (num_instances,), dtype = wp.vec3f. In torch this resolves to (num_instances, 3). + + This quantity is the linear velocity of the articulation root's center of mass frame + with respect to its actor frame. + """ + if self._root_com_lin_vel_b.timestamp < self._sim_timestamp: + wp.launch( + _world_vel_to_body_lin, + dim=self._num_instances, + inputs=[self.root_link_pose_w, self.root_com_vel_w], + outputs=[self._root_com_lin_vel_b.data], + device=self.device, + ) + self._root_com_lin_vel_b.timestamp = self._sim_timestamp + return self._root_com_lin_vel_b.data + + @property + def root_com_ang_vel_b(self) -> wp.array: + """Root center of mass angular velocity in base frame [rad/s]. + Shape is (num_instances,), dtype = wp.vec3f. In torch this resolves to (num_instances, 3). + + This quantity is the angular velocity of the articulation root's center of mass frame + with respect to its actor frame. + """ + if self._root_com_ang_vel_b.timestamp < self._sim_timestamp: + wp.launch( + _world_vel_to_body_ang, + dim=self._num_instances, + inputs=[self.root_link_pose_w, self.root_com_vel_w], + outputs=[self._root_com_ang_vel_b.data], + device=self.device, + ) + self._root_com_ang_vel_b.timestamp = self._sim_timestamp + return self._root_com_ang_vel_b.data + + """ + Sliced properties. + """ + + @property + def root_link_pos_w(self) -> wp.array: + """Root link position in simulation world frame. + Shape is (num_instances,), dtype = wp.vec3f. In torch this resolves to (num_instances, 3). + + This quantity is the position of the actor frame of the root rigid body relative to the world. + """ + return self._get_pos_from_transform(self.root_link_pose_w) + + @property + def root_link_quat_w(self) -> wp.array: + """Root link orientation (x, y, z, w) in simulation world frame. + Shape is (num_instances,), dtype = wp.quatf. In torch this resolves to (num_instances, 4). + + This quantity is the orientation of the actor frame of the root rigid body. + """ + return self._get_quat_from_transform(self.root_link_pose_w) + + @property + def root_link_lin_vel_w(self) -> wp.array: + """Root linear velocity in simulation world frame [m/s]. + Shape is (num_instances,), dtype = wp.vec3f. In torch this resolves to (num_instances, 3). + + This quantity is the linear velocity of the root rigid body's actor frame relative to the world. + """ + return self._get_lin_vel_from_spatial_vector(self.root_link_vel_w) + + @property + def root_link_ang_vel_w(self) -> wp.array: + """Root link angular velocity in simulation world frame [rad/s]. + Shape is (num_instances,), dtype = wp.vec3f. In torch this resolves to (num_instances, 3). + + This quantity is the angular velocity of the actor frame of the root rigid body relative to the world. + """ + return self._get_ang_vel_from_spatial_vector(self.root_link_vel_w) + + @property + def root_com_pos_w(self) -> wp.array: + """Root center of mass position in simulation world frame. + Shape is (num_instances,), dtype = wp.vec3f. In torch this resolves to (num_instances, 3). + + This quantity is the position of the center of mass frame of the root rigid body relative to the world. + """ + return self._get_pos_from_transform(self.root_com_pose_w) + + @property + def root_com_quat_w(self) -> wp.array: + """Root center of mass orientation (x, y, z, w) in simulation world frame. + Shape is (num_instances,), dtype = wp.quatf. In torch this resolves to (num_instances, 4). + + This quantity is the orientation of the principal axes of inertia of the root rigid body relative to the world. + """ + return self._get_quat_from_transform(self.root_com_pose_w) + + @property + def root_com_lin_vel_w(self) -> wp.array: + """Root center of mass linear velocity in simulation world frame [m/s]. + Shape is (num_instances,), dtype = wp.vec3f. In torch this resolves to (num_instances, 3). + + This quantity is the linear velocity of the root rigid body's center of mass frame relative to the world. + """ + return self._get_lin_vel_from_spatial_vector(self.root_com_vel_w) + + @property + def root_com_ang_vel_w(self) -> wp.array: + """Root center of mass angular velocity in simulation world frame [rad/s]. + Shape is (num_instances,), dtype = wp.vec3f. In torch this resolves to (num_instances, 3). + + This quantity is the angular velocity of the root rigid body's center of mass frame relative to the world. + """ + return self._get_ang_vel_from_spatial_vector(self.root_com_vel_w) + + @property + def body_link_pos_w(self) -> wp.array: + """Positions of all bodies in simulation world frame. + Shape is (num_instances, num_bodies), dtype = wp.vec3f. In torch this resolves to + (num_instances, num_bodies, 3). + + This quantity is the position of the articulation bodies' actor frame relative to the world. + """ + return self._get_pos_from_transform(self.body_link_pose_w) + + @property + def body_link_quat_w(self) -> wp.array: + """Orientation (x, y, z, w) of all bodies in simulation world frame. + Shape is (num_instances, num_bodies), dtype = wp.quatf. In torch this resolves to + (num_instances, num_bodies, 4). + + This quantity is the orientation of the articulation bodies' actor frame relative to the world. + """ + return self._get_quat_from_transform(self.body_link_pose_w) + + @property + def body_link_lin_vel_w(self) -> wp.array: + """Linear velocity of all bodies in simulation world frame [m/s]. + Shape is (num_instances, num_bodies), dtype = wp.vec3f. In torch this resolves to + (num_instances, num_bodies, 3). + + This quantity is the linear velocity of the articulation bodies' actor frame relative to the world. + """ + return self._get_lin_vel_from_spatial_vector(self.body_link_vel_w) + + @property + def body_link_ang_vel_w(self) -> wp.array: + """Angular velocity of all bodies in simulation world frame [rad/s]. + Shape is (num_instances, num_bodies), dtype = wp.vec3f. In torch this resolves to + (num_instances, num_bodies, 3). + + This quantity is the angular velocity of the articulation bodies' actor frame relative to the world. + """ + return self._get_ang_vel_from_spatial_vector(self.body_link_vel_w) + + @property + def body_com_pos_w(self) -> wp.array: + """Positions of all bodies' center of mass in simulation world frame. + Shape is (num_instances, num_bodies), dtype = wp.vec3f. In torch this resolves to + (num_instances, num_bodies, 3). + + This quantity is the position of the articulation bodies' center of mass frame. + """ + return self._get_pos_from_transform(self.body_com_pose_w) + + @property + def body_com_quat_w(self) -> wp.array: + """Orientation (x, y, z, w) of the principal axes of inertia of all bodies in simulation world frame. + Shape is (num_instances, num_bodies), dtype = wp.quatf. In torch this resolves to + (num_instances, num_bodies, 4). + + This quantity is the orientation of the articulation bodies' principal axes of inertia. + """ + return self._get_quat_from_transform(self.body_com_pose_w) + + @property + def body_com_lin_vel_w(self) -> wp.array: + """Linear velocity of all bodies in simulation world frame [m/s]. + Shape is (num_instances, num_bodies), dtype = wp.vec3f. In torch this resolves to + (num_instances, num_bodies, 3). + + This quantity is the linear velocity of the articulation bodies' center of mass frame. + """ + return self._get_lin_vel_from_spatial_vector(self.body_com_vel_w) + + @property + def body_com_ang_vel_w(self) -> wp.array: + """Angular velocity of all bodies in simulation world frame [rad/s]. + Shape is (num_instances, num_bodies), dtype = wp.vec3f. In torch this resolves to + (num_instances, num_bodies, 3). + + This quantity is the angular velocity of the articulation bodies' center of mass frame. + """ + return self._get_ang_vel_from_spatial_vector(self.body_com_vel_w) + + @property + def body_com_lin_acc_w(self) -> wp.array: + """Linear acceleration of all bodies in simulation world frame [m/s^2]. + Shape is (num_instances, num_bodies), dtype = wp.vec3f. In torch this resolves to + (num_instances, num_bodies, 3). + + This quantity is the linear acceleration of the articulation bodies' center of mass frame. + """ + return self._get_lin_vel_from_spatial_vector(self.body_com_acc_w) + + @property + def body_com_ang_acc_w(self) -> wp.array: + """Angular acceleration of all bodies in simulation world frame [rad/s^2]. + Shape is (num_instances, num_bodies), dtype = wp.vec3f. In torch this resolves to + (num_instances, num_bodies, 3). + + This quantity is the angular acceleration of the articulation bodies' center of mass frame. + """ + return self._get_ang_vel_from_spatial_vector(self.body_com_acc_w) + + @property + def body_com_pos_b(self) -> wp.array: + """Center of mass position of all of the bodies in their respective link frames. + Shape is (num_instances, num_bodies), dtype = wp.vec3f. In torch this resolves to + (num_instances, num_bodies, 3). + + This quantity is the center of mass location relative to its body's link frame. + """ + return self._get_pos_from_transform(self.body_com_pose_b) + + @property + def body_com_quat_b(self) -> wp.array: + """Orientation (x, y, z, w) of the principal axes of inertia of all of the bodies in their + respective link frames. + Shape is (num_instances, num_bodies), dtype = wp.quatf. In torch this resolves to + (num_instances, num_bodies, 4). + + This quantity is the orientation of the principal axes of inertia relative to its body's link frame. + """ + return self._get_quat_from_transform(self.body_com_pose_b) + + """ + Deprecated in base class (required by ABC for backward compatibility). + """ + + @property + def default_root_state(self) -> wp.array: + """Deprecated. Use :attr:`default_root_pose` and :attr:`default_root_vel` instead.""" + warnings.warn( + "default_root_state is deprecated. Use default_root_pose and default_root_vel.", + DeprecationWarning, + stacklevel=2, + ) + if self._root_state_w_buf is None: + self._root_state_w_buf = wp.zeros( + self._num_instances, dtype=wp.types.vector(13, wp.float32), device=self.device + ) + return self._root_state_w_buf + + @property + def root_state_w(self) -> wp.array: + """Deprecated. Use :attr:`root_link_pose_w` and :attr:`root_com_vel_w` instead.""" + warnings.warn( + "root_state_w is deprecated. Use root_link_pose_w and root_com_vel_w.", + DeprecationWarning, + stacklevel=2, + ) + return self.root_link_pose_w + + @property + def root_link_state_w(self) -> wp.array: + """Deprecated. Use :attr:`root_link_pose_w` and :attr:`root_link_vel_w` instead.""" + warnings.warn( + "root_link_state_w is deprecated. Use root_link_pose_w and root_link_vel_w.", + DeprecationWarning, + stacklevel=2, + ) + return self.root_link_pose_w + + @property + def root_com_state_w(self) -> wp.array: + """Deprecated. Use :attr:`root_com_pose_w` and :attr:`root_com_vel_w` instead.""" + warnings.warn( + "root_com_state_w is deprecated. Use root_com_pose_w and root_com_vel_w.", + DeprecationWarning, + stacklevel=2, + ) + return self.root_com_pose_w + + @property + def body_state_w(self) -> wp.array: + """Deprecated. Use :attr:`body_link_pose_w` and :attr:`body_com_vel_w` instead.""" + warnings.warn( + "body_state_w is deprecated. Use body_link_pose_w and body_com_vel_w.", + DeprecationWarning, + stacklevel=2, + ) + return self.body_link_pose_w + + @property + def body_link_state_w(self) -> wp.array: + """Deprecated. Use :attr:`body_link_pose_w` and :attr:`body_link_vel_w` instead.""" + warnings.warn( + "body_link_state_w is deprecated. Use body_link_pose_w and body_link_vel_w.", + DeprecationWarning, + stacklevel=2, + ) + return self.body_link_pose_w + + @property + def body_com_state_w(self) -> wp.array: + """Deprecated. Use :attr:`body_com_pose_w` and :attr:`body_com_vel_w` instead.""" + warnings.warn( + "body_com_state_w is deprecated. Use body_com_pose_w and body_com_vel_w.", + DeprecationWarning, + stacklevel=2, + ) + return self.body_com_pose_w + + """ + Internal helper. + """ + + def _create_buffers(self) -> None: # noqa: C901 + super()._create_buffers() + # Scratch buffers for _read_binding_into_* methods, allocated lazily + # on first use and reused every subsequent step to avoid per-step + # allocation overhead on the hot RL path. + self._read_scratch: dict = {} + + N = self._num_instances + D = self._num_joints + L = self._num_bodies + dev = self.device + + # -- Root state buffers + self._root_link_pose_w = TimestampedBuffer(N, dev, wp.transformf) + self._root_link_vel_w = TimestampedBuffer(N, dev, wp.spatial_vectorf) + self._root_com_pose_w = TimestampedBuffer(N, dev, wp.transformf) + self._root_com_vel_w = TimestampedBuffer(N, dev, wp.spatial_vectorf) + + # -- Body state buffers + self._body_link_pose_w = TimestampedBuffer((N, L), dev, wp.transformf) + self._body_link_vel_w = TimestampedBuffer((N, L), dev, wp.spatial_vectorf) + self._body_com_pose_b = TimestampedBuffer((N, L), dev, wp.transformf) + self._body_com_pose_w = TimestampedBuffer((N, L), dev, wp.transformf) + self._body_com_vel_w = TimestampedBuffer((N, L), dev, wp.spatial_vectorf) + self._body_com_acc_w = TimestampedBuffer((N, L), dev, wp.spatial_vectorf) + self._body_incoming_joint_wrench_buf = TimestampedBuffer((N, L), dev, wp.spatial_vectorf) + + # -- Joint state buffers + self._joint_pos_buf = TimestampedBuffer((N, D), dev, wp.float32) + self._joint_vel_buf = TimestampedBuffer((N, D), dev, wp.float32) + self._joint_acc = TimestampedBuffer((N, D), dev, wp.float32) + self._previous_joint_vel = wp.zeros((N, D), dtype=wp.float32, device=dev) + + # -- Joint properties + self._joint_stiffness = wp.zeros((N, D), dtype=wp.float32, device=dev) + self._joint_damping = wp.zeros((N, D), dtype=wp.float32, device=dev) + self._joint_armature = wp.zeros((N, D), dtype=wp.float32, device=dev) + self._joint_friction_coeff = wp.zeros((N, D), dtype=wp.float32, device=dev) + self._joint_pos_limits = wp.zeros((N, D), dtype=wp.vec2f, device=dev) + self._joint_vel_limits = wp.zeros((N, D), dtype=wp.float32, device=dev) + self._joint_effort_limits = wp.zeros((N, D), dtype=wp.float32, device=dev) + + # -- Body properties + self._body_mass = wp.zeros((N, L), dtype=wp.float32, device=dev) + self._body_inertia = wp.zeros((N, L, 9), dtype=wp.float32, device=dev) + + # -- Soft limits / custom properties + self._soft_joint_pos_limits = wp.zeros((N, D), dtype=wp.vec2f, device=dev) + self._soft_joint_vel_limits = wp.zeros((N, D), dtype=wp.float32, device=dev) + self._gear_ratio = wp.ones((N, D), dtype=wp.float32, device=dev) + + # -- Command buffers + self._joint_pos_target = wp.zeros((N, D), dtype=wp.float32, device=dev) + self._joint_vel_target = wp.zeros((N, D), dtype=wp.float32, device=dev) + self._joint_effort_target = wp.zeros((N, D), dtype=wp.float32, device=dev) + self._computed_torque = wp.zeros((N, D), dtype=wp.float32, device=dev) + self._applied_torque = wp.zeros((N, D), dtype=wp.float32, device=dev) + + # -- Default state + self._default_root_pose = wp.zeros(N, dtype=wp.transformf, device=dev) + self._default_root_vel = wp.zeros(N, dtype=wp.spatial_vectorf, device=dev) + self._default_joint_pos = wp.zeros((N, D), dtype=wp.float32, device=dev) + self._default_joint_vel = wp.zeros((N, D), dtype=wp.float32, device=dev) + + # -- Derived property buffers + self._projected_gravity_b = TimestampedBuffer(N, dev, wp.vec3f) + self._heading_w = TimestampedBuffer(N, dev, wp.float32) + self._root_link_lin_vel_b = TimestampedBuffer(N, dev, wp.vec3f) + self._root_link_ang_vel_b = TimestampedBuffer(N, dev, wp.vec3f) + self._root_com_lin_vel_b = TimestampedBuffer(N, dev, wp.vec3f) + self._root_com_ang_vel_b = TimestampedBuffer(N, dev, wp.vec3f) + + # -- Deprecated combined state buffers + self._root_state_w_buf = None + self._root_link_state_w_buf = None + self._root_com_state_w_buf = None + self._body_state_w_buf = None + self._body_link_state_w_buf = None + self._body_com_state_w_buf = None + + # -- Tendon property buffers + T_fix = getattr(self, "_num_fixed_tendons", 0) + T_spa = getattr(self, "_num_spatial_tendons", 0) + if T_fix > 0: + self._fixed_tendon_stiffness = wp.zeros((N, T_fix), dtype=wp.float32, device=dev) + self._fixed_tendon_damping = wp.zeros((N, T_fix), dtype=wp.float32, device=dev) + self._fixed_tendon_limit_stiffness = wp.zeros((N, T_fix), dtype=wp.float32, device=dev) + self._fixed_tendon_rest_length = wp.zeros((N, T_fix), dtype=wp.float32, device=dev) + self._fixed_tendon_offset = wp.zeros((N, T_fix), dtype=wp.float32, device=dev) + self._fixed_tendon_pos_limits = wp.zeros((N, T_fix), dtype=wp.vec2f, device=dev) + else: + self._fixed_tendon_stiffness = None + self._fixed_tendon_damping = None + self._fixed_tendon_limit_stiffness = None + self._fixed_tendon_rest_length = None + self._fixed_tendon_offset = None + self._fixed_tendon_pos_limits = None + if T_spa > 0: + self._spatial_tendon_stiffness = wp.zeros((N, T_spa), dtype=wp.float32, device=dev) + self._spatial_tendon_damping = wp.zeros((N, T_spa), dtype=wp.float32, device=dev) + self._spatial_tendon_limit_stiffness = wp.zeros((N, T_spa), dtype=wp.float32, device=dev) + self._spatial_tendon_offset = wp.zeros((N, T_spa), dtype=wp.float32, device=dev) + else: + self._spatial_tendon_stiffness = None + self._spatial_tendon_damping = None + self._spatial_tendon_limit_stiffness = None + self._spatial_tendon_offset = None + + # Read initial joint properties from bindings + self._read_initial_properties() + + def _read_initial_properties(self) -> None: + """Read static/initial joint and body properties from ovphysx bindings. + + These are one-time reads at init. Property tensors (stiffness, + damping, limits, mass, etc.) are CPU-resident in PhysX even in GPU + mode, so we read them via CPU numpy buffers and then copy to the + simulation device. + """ + + # Property reads always use CPU numpy (property tensors are host-side). + def _read_cpu(tensor_type): + binding = self._get_binding(tensor_type) + if binding is None: + return None + np_buf = np.zeros(binding.shape, dtype=np.float32) + binding.read(np_buf) + return np_buf + + for tt, dst in [ + (TT.DOF_STIFFNESS, self._joint_stiffness), + (TT.DOF_DAMPING, self._joint_damping), + (TT.DOF_ARMATURE, self._joint_armature), + (TT.DOF_MAX_VELOCITY, self._joint_vel_limits), + (TT.DOF_MAX_FORCE, self._joint_effort_limits), + (TT.BODY_MASS, self._body_mass), + ]: + np_buf = _read_cpu(tt) + if np_buf is not None: + wp.copy(dst, wp.from_numpy(np_buf, dtype=wp.float32, device=self.device)) + + # Joint position limits: [N, D, 2] -> (N, D) wp.vec2f + np_lim = _read_cpu(TT.DOF_LIMIT) + if np_lim is not None: + self._joint_pos_limits = wp.from_numpy( + np_lim.reshape(self._num_instances, self._num_joints, 2), dtype=wp.vec2f, device=self.device + ) + + # Body inertia: [N, L, 9] + np_iner = _read_cpu(TT.BODY_INERTIA) + if np_iner is not None: + self._body_inertia = wp.from_numpy(np_iner, dtype=wp.float32, device=self.device) + + # Friction: [N, D, 3] -> extract static friction (column 0) + np_fric = _read_cpu(TT.DOF_FRICTION_PROPERTIES) + if np_fric is not None: + self._joint_friction_coeff = wp.from_numpy(np_fric[..., 0].copy(), dtype=wp.float32, device=self.device) + + # Fixed tendon properties (CPU-side, read once) + T_fix = getattr(self, "_num_fixed_tendons", 0) + if T_fix > 0: + for tt, dst in [ + (TT.FIXED_TENDON_STIFFNESS, self._fixed_tendon_stiffness), + (TT.FIXED_TENDON_DAMPING, self._fixed_tendon_damping), + (TT.FIXED_TENDON_LIMIT_STIFFNESS, self._fixed_tendon_limit_stiffness), + (TT.FIXED_TENDON_REST_LENGTH, self._fixed_tendon_rest_length), + (TT.FIXED_TENDON_OFFSET, self._fixed_tendon_offset), + ]: + np_buf = _read_cpu(tt) + if np_buf is not None and dst is not None: + wp.copy(dst, wp.from_numpy(np_buf, dtype=wp.float32, device=self.device)) + # Fixed tendon limits: [N, T, 2] -> (N, T) wp.vec2f + np_tlim = _read_cpu(TT.FIXED_TENDON_LIMIT) + if np_tlim is not None and self._fixed_tendon_pos_limits is not None: + self._fixed_tendon_pos_limits = wp.from_numpy( + np_tlim.reshape(self._num_instances, T_fix, 2), dtype=wp.vec2f, device=self.device + ) + + # Spatial tendon properties (CPU-side, read once) + T_spa = getattr(self, "_num_spatial_tendons", 0) + if T_spa > 0: + for tt, dst in [ + (TT.SPATIAL_TENDON_STIFFNESS, self._spatial_tendon_stiffness), + (TT.SPATIAL_TENDON_DAMPING, self._spatial_tendon_damping), + (TT.SPATIAL_TENDON_LIMIT_STIFFNESS, self._spatial_tendon_limit_stiffness), + (TT.SPATIAL_TENDON_OFFSET, self._spatial_tendon_offset), + ]: + np_buf = _read_cpu(tt) + if np_buf is not None and dst is not None: + wp.copy(dst, wp.from_numpy(np_buf, dtype=wp.float32, device=self.device)) + + """ + Internal helpers -- Bindings. + """ + + def _get_binding(self, tensor_type: int): + """Return a binding, lazily creating it if a binding_getter was provided.""" + b = self._bindings.get(tensor_type) + if b is not None: + return b + if self._binding_getter is not None: + b = self._binding_getter(tensor_type) + if b is not None: + self._bindings[tensor_type] = b + return b + return None + + def _get_read_scratch(self, tensor_type: int) -> wp.array | None: + """Return a pre-allocated flat float32 scratch buffer for a binding. + + Allocated once on first use, then reused every step. CPU-only + bindings (body properties, DOF properties) get CPU scratch; GPU + bindings get GPU scratch. wp.copy handles cross-device transfer + when the destination buffer lives on a different device. + """ + if tensor_type in self._read_scratch: + return self._read_scratch[tensor_type] + binding = self._get_binding(tensor_type) + if binding is None: + return None + from isaaclab_ovphysx.tensor_types import _CPU_ONLY_TYPES + + dev = "cpu" if tensor_type in _CPU_ONLY_TYPES else self.device + buf = wp.zeros(binding.shape, dtype=wp.float32, device=dev) + self._read_scratch[tensor_type] = buf + return buf + + def _get_read_view(self, tensor_type: int, wp_array: wp.array, floats_per_elem: int = 0) -> wp.array | None: + """Return a stable float32 view of a warp buffer for reading from a binding. + + For structured-dtype buffers (transformf, spatial_vectorf), the view + reinterprets the same GPU memory as a flat float32 array matching the + binding's shape. For plain float32 buffers, returns the array as-is. + + The returned view is cached so that ``binding.read(view)`` sees the + same object on every call, enabling the binding's internal read cache. + """ + if not hasattr(self, "_read_view_cache"): + self._read_view_cache = {} + cache_key = (tensor_type, wp_array.ptr) + cached = self._read_view_cache.get(cache_key) + if cached is not None: + return cached + + binding = self._get_binding(tensor_type) + if binding is None: + self._read_view_cache[cache_key] = None + return None + + if floats_per_elem > 0: + view = wp.array( + ptr=wp_array.ptr, + shape=binding.shape, + dtype=wp.float32, + device=str(wp_array.device), + copy=False, + ) + else: + view = wp_array + + self._read_view_cache[cache_key] = view + return view + + def _read_binding_into_flat(self, tensor_type: int, wp_array: wp.array) -> None: + """Read a flat binding (no structured dtype) into an existing warp array. + + Reads directly into the target array -- no scratch buffer, no extra copy. + """ + binding = self._get_binding(tensor_type) + if binding is None: + return + binding.read(wp_array) + + def _read_binding_into_buf(self, tensor_type: int, buf: TimestampedBuffer) -> None: + """Read from an ovphysx binding into a TimestampedBuffer, skipping if fresh.""" + if buf.timestamp >= self._sim_timestamp: + return + view = self._get_read_view(tensor_type, buf.data) + if view is None: + return + self._get_binding(tensor_type).read(view) + buf.timestamp = self._sim_timestamp + + def _read_transform_binding(self, tensor_type: int, buf: TimestampedBuffer) -> None: + """Read a pose binding (float32 view of transformf buffer), skipping if fresh.""" + if buf.timestamp >= self._sim_timestamp: + return + view = self._get_read_view(tensor_type, buf.data, 7) + if view is None: + return + self._get_binding(tensor_type).read(view) + buf.timestamp = self._sim_timestamp + + def _read_spatial_vector_binding(self, tensor_type: int, buf: TimestampedBuffer) -> None: + """Read a velocity binding (float32 view of spatial_vectorf buffer), skipping if fresh.""" + if buf.timestamp >= self._sim_timestamp: + return + view = self._get_read_view(tensor_type, buf.data, 6) + if view is None: + return + self._get_binding(tensor_type).read(view) + buf.timestamp = self._sim_timestamp + + """ + Internal helpers -- Extraction. + """ + + def _get_pos_from_transform(self, transform: wp.array) -> wp.array: + return wp.array( + ptr=transform.ptr, + shape=transform.shape, + dtype=wp.vec3f, + strides=transform.strides, + device=self.device, + ) + + def _get_quat_from_transform(self, transform: wp.array) -> wp.array: + return wp.array( + ptr=transform.ptr + 3 * 4, + shape=transform.shape, + dtype=wp.quatf, + strides=transform.strides, + device=self.device, + ) + + def _get_lin_vel_from_spatial_vector(self, sv: wp.array) -> wp.array: + return wp.array( + ptr=sv.ptr, + shape=sv.shape, + dtype=wp.vec3f, + strides=sv.strides, + device=self.device, + ) + + def _get_ang_vel_from_spatial_vector(self, sv: wp.array) -> wp.array: + return wp.array( + ptr=sv.ptr + 3 * 4, + shape=sv.shape, + dtype=wp.vec3f, + strides=sv.strides, + device=self.device, + ) diff --git a/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/articulation/kernels.py b/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/articulation/kernels.py new file mode 100644 index 000000000000..b93c4e6d4b41 --- /dev/null +++ b/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/articulation/kernels.py @@ -0,0 +1,217 @@ +# 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 + +"""Warp kernels for the ovphysx articulation.""" + +import warp as wp + + +@wp.kernel +def _body_wrench_to_world( + force_b: wp.array(dtype=wp.vec3f, ndim=2), + torque_b: wp.array(dtype=wp.vec3f, ndim=2), + poses: wp.array(dtype=wp.transformf, ndim=2), + wrench_out: wp.array(dtype=wp.float32, ndim=3), +): + """Rotate body-frame force/torque to world frame and pack into [N, L, 9].""" + i, j = wp.tid() + q = wp.transform_get_rotation(poses[i, j]) + f_w = wp.quat_rotate(q, force_b[i, j]) + t_w = wp.quat_rotate(q, torque_b[i, j]) + wrench_out[i, j, 0] = f_w[0] + wrench_out[i, j, 1] = f_w[1] + wrench_out[i, j, 2] = f_w[2] + wrench_out[i, j, 3] = t_w[0] + wrench_out[i, j, 4] = t_w[1] + wrench_out[i, j, 5] = t_w[2] + p_w = wp.transform_get_translation(poses[i, j]) + wrench_out[i, j, 6] = p_w[0] + wrench_out[i, j, 7] = p_w[1] + wrench_out[i, j, 8] = p_w[2] + + +@wp.kernel +def _scatter_rows_partial( + dst: wp.array2d(dtype=wp.float32), + src: wp.array2d(dtype=wp.float32), + ids: wp.array(dtype=wp.int32), +): + """dst[ids[i], j] = src[i, j] -- scatter partial [K,C] into full [N,C] on GPU.""" + i, j = wp.tid() + dst[ids[i], j] = src[i, j] + + +@wp.func +def compute_soft_joint_pos_limits_func( + joint_pos_limits: wp.vec2f, + soft_limit_factor: wp.float32, +): + """Compute soft joint position limits from hard limits.""" + joint_pos_mean = (joint_pos_limits[0] + joint_pos_limits[1]) / 2.0 + joint_pos_range = joint_pos_limits[1] - joint_pos_limits[0] + return wp.vec2f( + joint_pos_mean - 0.5 * joint_pos_range * soft_limit_factor, + joint_pos_mean + 0.5 * joint_pos_range * soft_limit_factor, + ) + + +@wp.kernel +def update_soft_joint_pos_limits( + joint_pos_limits: wp.array2d(dtype=wp.vec2f), + soft_limit_factor: wp.float32, + soft_joint_pos_limits: wp.array2d(dtype=wp.vec2f), +): + """Update soft joint position limits from hard limits and a scale factor.""" + i, j = wp.tid() + soft_joint_pos_limits[i, j] = compute_soft_joint_pos_limits_func(joint_pos_limits[i, j], soft_limit_factor) + + +""" +Data-layer kernels (used by ArticulationData). +""" + + +@wp.kernel +def _fd_joint_acc( + cur_vel: wp.array2d(dtype=wp.float32), + prev_vel: wp.array2d(dtype=wp.float32), + inv_dt: float, + out: wp.array2d(dtype=wp.float32), +): + """Compute joint acceleration via finite differencing and update previous velocity. + + Args: + cur_vel: Current joint velocities. Shape is (num_envs, num_joints). + prev_vel: Previous joint velocities (updated in-place). Shape is (num_envs, num_joints). + inv_dt: Inverse time step (1/dt) [1/s]. + out: Output joint accelerations. Shape is (num_envs, num_joints). + """ + i, j = wp.tid() + out[i, j] = (cur_vel[i, j] - prev_vel[i, j]) * inv_dt + prev_vel[i, j] = cur_vel[i, j] + + +@wp.kernel +def _copy_first_body( + body_vel: wp.array(dtype=wp.spatial_vectorf, ndim=2), + root_vel: wp.array(dtype=wp.spatial_vectorf), +): + """Copy the first body's velocity to the root velocity buffer. + + Args: + body_vel: Body velocities. Shape is (num_envs, num_bodies). + root_vel: Output root velocities. Shape is (num_envs,). + """ + i = wp.tid() + root_vel[i] = body_vel[i, 0] + + +@wp.kernel +def _compose_root_com_pose( + link_pose: wp.array(dtype=wp.transformf), + com_pose_b: wp.array(dtype=wp.transformf, ndim=2), + com_pose_w: wp.array(dtype=wp.transformf), +): + """Compose root link pose with body-frame CoM offset to get world-frame root CoM pose. + + Args: + link_pose: Root link poses in world frame. Shape is (num_envs,). + com_pose_b: Body-frame CoM offsets. Shape is (num_envs, num_bodies). + com_pose_w: Output world-frame root CoM poses. Shape is (num_envs,). + """ + i = wp.tid() + com_pose_w[i] = wp.transform_multiply(link_pose[i], com_pose_b[i, 0]) + + +@wp.kernel +def _compose_body_com_poses( + link_pose: wp.array(dtype=wp.transformf, ndim=2), + com_pose_b: wp.array(dtype=wp.transformf, ndim=2), + com_pose_w: wp.array(dtype=wp.transformf, ndim=2), +): + """Compose body link poses with body-frame CoM offsets to get world-frame CoM poses. + + Args: + link_pose: Body link poses in world frame. Shape is (num_envs, num_bodies). + com_pose_b: Body-frame CoM offsets. Shape is (num_envs, num_bodies). + com_pose_w: Output world-frame body CoM poses. Shape is (num_envs, num_bodies). + """ + i, j = wp.tid() + com_pose_w[i, j] = wp.transform_multiply(link_pose[i, j], com_pose_b[i, j]) + + +@wp.kernel +def _projected_gravity( + gravity_vec_w: wp.array(dtype=wp.vec3f), + root_pose: wp.array(dtype=wp.transformf), + out: wp.array(dtype=wp.vec3f), +): + """Project world-frame gravity direction into the root body frame. + + Args: + gravity_vec_w: Gravity unit vector per instance in world frame. Shape is (num_envs,). + root_pose: Root link poses in world frame. Shape is (num_envs,). + out: Output projected gravity in body frame. Shape is (num_envs,). + """ + i = wp.tid() + q = wp.transform_get_rotation(root_pose[i]) + out[i] = wp.quat_rotate_inv(q, gravity_vec_w[i]) + + +@wp.kernel +def _compute_heading( + forward_vec_b: wp.array(dtype=wp.vec3f), + root_pose: wp.array(dtype=wp.transformf), + out: wp.array(dtype=wp.float32), +): + """Compute yaw heading angle from the forward direction rotated into the world frame. + + Args: + forward_vec_b: Forward direction in body frame per instance. Shape is (num_envs,). + root_pose: Root link poses in world frame. Shape is (num_envs,). + out: Output heading angles [rad]. Shape is (num_envs,). + """ + i = wp.tid() + q = wp.transform_get_rotation(root_pose[i]) + forward = wp.quat_rotate(q, forward_vec_b[i]) + out[i] = wp.atan2(forward[1], forward[0]) + + +@wp.kernel +def _world_vel_to_body_lin( + root_pose: wp.array(dtype=wp.transformf), + vel_w: wp.array(dtype=wp.spatial_vectorf), + out: wp.array(dtype=wp.vec3f), +): + """Rotate world-frame linear velocity into the root body frame. + + Args: + root_pose: Root link poses in world frame. Shape is (num_envs,). + vel_w: Spatial velocities in world frame. Shape is (num_envs,). + out: Output linear velocity in body frame. Shape is (num_envs,). + """ + i = wp.tid() + q = wp.transform_get_rotation(root_pose[i]) + lin = wp.spatial_top(vel_w[i]) + out[i] = wp.quat_rotate_inv(q, lin) + + +@wp.kernel +def _world_vel_to_body_ang( + root_pose: wp.array(dtype=wp.transformf), + vel_w: wp.array(dtype=wp.spatial_vectorf), + out: wp.array(dtype=wp.vec3f), +): + """Rotate world-frame angular velocity into the root body frame. + + Args: + root_pose: Root link poses in world frame. Shape is (num_envs,). + vel_w: Spatial velocities in world frame. Shape is (num_envs,). + out: Output angular velocity in body frame. Shape is (num_envs,). + """ + i = wp.tid() + q = wp.transform_get_rotation(root_pose[i]) + ang = wp.spatial_bottom(vel_w[i]) + out[i] = wp.quat_rotate_inv(q, ang) diff --git a/source/isaaclab_ovphysx/isaaclab_ovphysx/cloner/__init__.py b/source/isaaclab_ovphysx/isaaclab_ovphysx/cloner/__init__.py new file mode 100644 index 000000000000..3b9a12007792 --- /dev/null +++ b/source/isaaclab_ovphysx/isaaclab_ovphysx/cloner/__init__.py @@ -0,0 +1,8 @@ +# 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 .ovphysx_replicate import ovphysx_replicate + +__all__ = ["ovphysx_replicate"] diff --git a/source/isaaclab_ovphysx/isaaclab_ovphysx/cloner/ovphysx_replicate.py b/source/isaaclab_ovphysx/isaaclab_ovphysx/cloner/ovphysx_replicate.py new file mode 100644 index 000000000000..7c46a6060b88 --- /dev/null +++ b/source/isaaclab_ovphysx/isaaclab_ovphysx/cloner/ovphysx_replicate.py @@ -0,0 +1,103 @@ +# 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 + +"""OvPhysX replication hook for IsaacLab's cloning pipeline. + +Called by :func:`isaaclab.cloner.clone_from_template` in place of the PhysX +or Newton replicators. Unlike those replicators, ovphysx.PhysX does not exist +yet at this point in the scene setup — it is created lazily on the first +:meth:`~isaaclab_ovphysx.physics.OvPhysxManager.reset` call. + +This function records a *pending clone* on :class:`OvPhysxManager`. When +:meth:`~isaaclab_ovphysx.physics.OvPhysxManager._warmup_and_load` eventually +creates the ``PhysX`` instance and loads the USD stage (which contains only +``env_0`` physics — env_1..N are empty Xform containers), it replays every +pending clone via ``physx.clone(source, targets)`` to create the remaining +environments entirely inside the physics runtime without touching USD. +""" + +from __future__ import annotations + +import torch + +from pxr import Usd + + +def ovphysx_replicate( + stage: Usd.Stage, + sources: list[str], + destinations: list[str], + env_ids: torch.Tensor, + mapping: torch.Tensor, + positions: torch.Tensor | None = None, + quaternions: torch.Tensor | None = None, + device: str = "cpu", +) -> None: + """Record a physics clone for later execution by OvPhysxManager. + + Translates the generic IsaacLab source/destination/mapping representation + into ``(source_path, [target_paths])`` pairs and registers them on + :class:`~isaaclab_ovphysx.physics.OvPhysxManager`. The actual + ``physx.clone()`` calls happen in ``_warmup_and_load()`` after the USD + stage has been loaded. + + The ``positions`` parameter contains the 2-D grid world positions for all + environments. They are forwarded to the C++ clone plugin so that the + parent Xform prim for each clone (e.g. ``/World/envs/env_N``) is placed at + the correct grid location in Fabric. The exported USD stage only contains + ``env_0``; without explicit positions all clone parents would be created at + the origin, causing all articulations to pile up and the GPU solver to + diverge on the first warmup step. + + Args: + stage: USD stage (not modified by this function). + sources: Source prim paths (one per prototype). + destinations: Destination path templates with ``"{}"`` for env index. + env_ids: Environment indices tensor. + mapping: ``(num_sources, num_envs)`` bool tensor; True selects which + environments receive each source. + positions: World (x, y, z) positions for every environment, shape + ``[num_envs, 3]``. Used to place clone parent Xform prims in + Fabric at correct grid locations. + quaternions: Ignored — orientations are set at first reset. + device: Torch device (unused; kept for API compatibility). + """ + # Deferred import to avoid circular dependency at module load time. + from isaaclab_ovphysx.physics.ovphysx_manager import OvPhysxManager + + for i, src in enumerate(sources): + active_env_ids = env_ids[mapping[i]].tolist() + + # Exclude the source environment from its own target list. + # physx.clone() is only needed for *other* envs; the source env_0 is + # already loaded from USD. We detect self by matching the source path + # against the destination template. + pre, _, suf = destinations[i].partition("{}") + self_env_id: int | None = None + candidate = src.removeprefix(pre).removesuffix(suf) + if candidate.isdigit(): + self_env_id = int(candidate) + + # Build parallel (targets, parent_positions) lists for non-self envs. + # parent_positions[j] is the world (x,y,z) for the parent Xform of + # targets[j] (e.g. /World/envs/env_N). These positions are passed to + # the C++ clone plugin so that env_N Xform prims — absent from the + # exported USD stage — are created at the correct 2-D grid location + # rather than the origin. Without this, all clones pile up at env_0's + # position during the warmup physics step and the GPU solver diverges. + targets: list[str] = [] + parent_positions: list[tuple[float, float, float]] = [] + for e in active_env_ids: + if e == self_env_id: + continue + targets.append(destinations[i].format(e)) + if positions is not None and e < len(positions): + pos = positions[e] + parent_positions.append((float(pos[0]), float(pos[1]), float(pos[2]))) + else: + parent_positions.append((0.0, 0.0, 0.0)) + + if targets: + OvPhysxManager.register_clone(src, targets, parent_positions) diff --git a/source/isaaclab_ovphysx/isaaclab_ovphysx/physics/__init__.py b/source/isaaclab_ovphysx/isaaclab_ovphysx/physics/__init__.py new file mode 100644 index 000000000000..853ebc18725a --- /dev/null +++ b/source/isaaclab_ovphysx/isaaclab_ovphysx/physics/__init__.py @@ -0,0 +1,10 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""OvPhysX physics manager implementation.""" + +from isaaclab.utils.module import lazy_export + +lazy_export() diff --git a/source/isaaclab_ovphysx/isaaclab_ovphysx/physics/__init__.pyi b/source/isaaclab_ovphysx/isaaclab_ovphysx/physics/__init__.pyi new file mode 100644 index 000000000000..bfae28a137eb --- /dev/null +++ b/source/isaaclab_ovphysx/isaaclab_ovphysx/physics/__init__.pyi @@ -0,0 +1,12 @@ +# 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__ = [ + "OvPhysxManager", + "OvPhysxCfg", +] + +from .ovphysx_manager import OvPhysxManager +from .ovphysx_manager_cfg import OvPhysxCfg diff --git a/source/isaaclab_ovphysx/isaaclab_ovphysx/physics/ovphysx_manager.py b/source/isaaclab_ovphysx/isaaclab_ovphysx/physics/ovphysx_manager.py new file mode 100644 index 000000000000..9063078e45b3 --- /dev/null +++ b/source/isaaclab_ovphysx/isaaclab_ovphysx/physics/ovphysx_manager.py @@ -0,0 +1,330 @@ +# 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 + +"""OvPhysX Manager for Isaac Lab. + +This module manages an ovphysx-based physics simulation lifecycle without Kit dependencies. +It exports the current USD stage to disk, loads it into ovphysx, and steps the simulation +using the ovphysx C/Python API. +""" + +from __future__ import annotations + +import atexit +import logging +import os +import tempfile +from typing import TYPE_CHECKING, Any, ClassVar + +from isaaclab.physics import PhysicsEvent, PhysicsManager + +if TYPE_CHECKING: + from isaaclab.sim.simulation_context import SimulationContext + + from .ovphysx_manager_cfg import OvPhysxCfg + +__all__ = ["OvPhysxManager"] + +logger = logging.getLogger(__name__) + + +class OvPhysxManager(PhysicsManager): + """Manages an ovphysx-backed physics simulation lifecycle. + + Unlike PhysxManager, this manager does not depend on Kit, Carbonite, or the + Omniverse timeline. It drives the simulation entirely through the ovphysx + Python wheel. + + Lifecycle: initialize() -> reset() -> step() (repeated) -> close() + """ + + _cfg: ClassVar[OvPhysxCfg | None] = None + _physx: ClassVar[Any] = None # ovphysx.PhysX (lazy import) + _usd_handle: ClassVar[Any] = None + _stage_path: ClassVar[str | None] = None + _warmup_done: ClassVar[bool] = False + _tmp_dir: ClassVar[tempfile.TemporaryDirectory | None] = None + # Pending (source, targets, parent_positions) triples registered by + # ovphysx_replicate() before the PhysX instance exists. Replayed via + # physx.clone() in _warmup_and_load(). + # parent_positions is a list of (x, y, z) tuples — one per target. + _pending_clones: ClassVar[list[tuple[str, list[str], list[tuple[float, float, float]]]]] = [] + _atexit_registered: ClassVar[bool] = False + + @classmethod + def register_clone( + cls, source: str, targets: list[str], parent_positions: list[tuple[float, float, float]] | None = None + ) -> None: + """Register a (source, targets, parent_positions) triple for replay via physx.clone(). + + Called by :func:`~isaaclab_ovphysx.cloner.ovphysx_replicate` during + scene setup, before the PhysX instance exists. The clone operations + are executed in :meth:`_warmup_and_load` immediately after + ``physx.add_usd()``. + + Args: + source: Source prim path (env_0 articulation root). + targets: Target prim paths for env_1..N. + parent_positions: World positions (x, y, z) for each target's parent + Xform prim (e.g. /World/envs/env_N). When provided the clone + plugin sets those transforms in Fabric so all environments start + at their correct grid locations, preventing solver divergence + during the warmup step. + """ + cls._pending_clones.append((source, targets, parent_positions or [])) + + @classmethod + def initialize(cls, sim_context: SimulationContext) -> None: + """Initialize the physics manager with simulation context. + + This stores the config and device but does not create the ovphysx + instance yet -- the USD stage may not be fully populated at this point. + The actual creation happens lazily in :meth:`reset`. + """ + super().initialize(sim_context) + cls._warmup_done = False + cls._physx = None + cls._usd_handle = None + cls._stage_path = None + cls._pending_clones = [] + + @classmethod + def reset(cls, soft: bool = False) -> None: + """Reset physics simulation. + + On the first (non-soft) reset the method: + - Exports the current USD stage to a temp file + - Creates the ovphysx.PhysX instance + - Loads the exported USD + - Warms up GPU buffers (if on CUDA) + - Dispatches PHYSICS_READY + """ + if not soft: + if not cls._warmup_done: + cls._warmup_and_load() + cls.dispatch_event(PhysicsEvent.PHYSICS_READY, payload={}) + + @classmethod + def forward(cls) -> None: + """No-op -- ovphysx does not have a fabric/rendering pipeline.""" + pass + + @classmethod + def step(cls) -> None: + """Step the simulation by one physics timestep.""" + if cls._physx is None: + return + dt = cls.get_physics_dt() + sim_time = PhysicsManager._sim_time + cls._physx.step_sync(dt=dt, sim_time=sim_time) + PhysicsManager._sim_time += dt + + @classmethod + def close(cls) -> None: + """Release ovphysx resources and clean up.""" + cls._release_physx() + + cls._usd_handle = None + cls._stage_path = None + cls._warmup_done = False + + if cls._tmp_dir is not None: + cls._tmp_dir.cleanup() + cls._tmp_dir = None + + super().close() + + @classmethod + def _release_physx(cls) -> None: + """Release the ovphysx instance if it exists. Safe to call multiple times. + + With ovphysx<=0.3.7 and Kit's pxr in the same process, physx.release() + deadlocks due to dual-Carbonite static destructor races. Skip the + native release and let os._exit() (registered via atexit) terminate the + process; GPU resources are reclaimed by the driver. + """ + if cls._physx is not None: + cls._physx = None + + @classmethod + def get_physx_instance(cls) -> Any: + """Return the underlying ovphysx.PhysX instance (or None if not yet created).""" + return cls._physx + + # ------------------------------------------------------------------ + # Internal helpers + # ------------------------------------------------------------------ + + @classmethod + def _warmup_and_load(cls) -> None: + """Export the USD stage, create the ovphysx instance, and load the scene.""" + sim = PhysicsManager._sim + if sim is None: + raise RuntimeError("OvPhysxManager: SimulationContext is not set.") + + device_str = PhysicsManager._device + if "cuda" in device_str: + parts = device_str.split(":") + gpu_index = int(parts[1]) if len(parts) > 1 else 0 + ovphysx_device = "gpu" + else: + gpu_index = 0 + ovphysx_device = "cpu" + + scene_prim = sim.stage.GetPrimAtPath(sim.cfg.physics_prim_path) + if scene_prim.IsValid() and ovphysx_device == "gpu": + cls._configure_physx_scene_prim(scene_prim, PhysicsManager._cfg) + + # Export the current USD stage to a temporary file so ovphysx can load it. + cls._tmp_dir = tempfile.TemporaryDirectory(prefix="isaaclab_ovphysx_") + stage_file = os.path.join(cls._tmp_dir.name, "scene.usda") + sim.stage.Export(stage_file) + cls._stage_path = stage_file + logger.info("OvPhysxManager: exported USD stage to %s", stage_file) + + # HACK (temporary): hide pxr from sys.modules during ovphysx bootstrap. + # IsaacSim's pxr reports version 0.25.5 (pip convention) while ovphysx + # expects 25.11 (OpenUSD release convention). Hiding pxr causes + # ovphysx.check_usd_compatibility() to skip the Python-side version + # check. This should go away once ovphysx ships a namespaced USD + # copy with isolated symbols (same "import pxr" API, no collision). + import sys as _sys + + _hidden_pxr = {k: _sys.modules.pop(k) for k in list(_sys.modules) if k == "pxr" or k.startswith("pxr.")} + try: + import ovphysx as _ovphysx_bootstrap + + _ovphysx_bootstrap.bootstrap() + finally: + _sys.modules.update(_hidden_pxr) + + import ovphysx + + cls._physx = ovphysx.PhysX(device=ovphysx_device, gpu_index=gpu_index) + + # Without worker threads the stepper runs simulate()+fetchResults() + # synchronously, blocking the calling thread for the full GPU step time. + # + # COMPAT(ovphysx<=0.3.7): The public 0.3.7 wheel exposes typed config + # setters (set_config_int32 etc.) rather than the Carbonite-settings-based + # set_setting() added in newer internal builds. This guard keeps both + # working. REVERT once the public wheel ships set_setting(). + if hasattr(cls._physx, "set_setting"): + cls._physx.set_setting("/persistent/physics/numThreads", "8") + cls._physx.set_setting("/physics/physxDispatcher", "true") + cls._physx.set_setting("/physics/updateToUsd", "false") + cls._physx.set_setting("/physics/updateVelocitiesToUsd", "false") + cls._physx.set_setting("/physics/updateParticlesToUsd", "false") + else: + cls._physx.set_config_int32(ovphysx.ConfigInt32.NUM_THREADS, 8) + + # FIXME(malesiani): re-evaluate this when carbonite ships an isolated copy. + # At process exit, two Carbonite instances are in memory: + # 1. ovphysx's bundled libcarb.so (RPATH $ORIGIN/../plugins/) + # 2. kit's libcarb.so (pulled in via LD_LIBRARY_PATH by Fabric/usdrt plugins) + # + # Why does kit's libcarb end up here even though we skip AppLauncher? + # Note: AppLauncher always starts the full Kit runtime — even headless=True + # still loads Kit. "Kitless" in IsaacLab means AppLauncher is not used at all. + # But we still import `pxr` from IsaacSim's Kit USD build. The moment `import pxr` runs, the Kit USD + # runtime loads Fabric infrastructure (omni.physx.fabric.plugin, usdrt.population.plugin) + # from kit's plugin directories, which are on LD_LIBRARY_PATH via setup_python_env.sh. + # Those plugins link against kit's libcarb.so, so kit's Carbonite lands in memory + # purely from `import pxr`, regardless of whether the Kit App is launched. + # + # Both Carbonite instances register C++ static destructors. At process exit those + # destructors race and segfault. The workaround is to release ovphysx cleanly + # (so GPU resources are freed) and then call os._exit() to skip the static destructor + # phase entirely. os._exit() terminates the process without running C++ atexit + # handlers or static destructors, sidestepping the conflict. + # + # Proper long-term fix: ovphysx ships a fully namespace-isolated Carbonite + # (different soname / hidden visibility) so its symbols never collide with kit's. + if not cls._atexit_registered: + + def _atexit_release_and_exit(): + # Skip physx.release() -- it deadlocks due to dual-Carbonite + # static destructor races (ovphysx's bundled libcarb vs Kit's). + # GPU resources are reclaimed by the driver at process exit. + os._exit(0) + + atexit.register(_atexit_release_and_exit) + cls._atexit_registered = True + + usd_handle, op_idx = cls._physx.add_usd(stage_file) + cls._physx.wait_op(op_idx) + cls._usd_handle = usd_handle + logger.info("OvPhysxManager: loaded USD into ovphysx (device=%s)", ovphysx_device) + + # Replay pending physics clones registered by ovphysx_replicate(). + # The USD stage contains only env_0's physics; env_1..N are empty + # Xform containers. physx.clone() creates the remaining environments + # in the physics runtime without modifying the USD file. + if cls._pending_clones: + # ovphysx_replicate() only registers pending clones when clone_usd=False, + # meaning the USD contains only env_0 physics and physx.clone() is required + # to populate env_1..N in the physics runtime. Execute unconditionally — + # no USD content heuristic is needed. + for source, targets, parent_positions in cls._pending_clones: + logger.info( + "OvPhysxManager: cloning %s -> %d targets (%s ... %s)", + source, + len(targets), + targets[0], + targets[-1], + ) + if parent_positions: + transforms = [(x, y, z, 0.0, 0.0, 0.0, 1.0) for x, y, z in parent_positions] + else: + transforms = None + op_idx = cls._physx.clone(source, targets, transforms) + cls._physx.wait_op(op_idx) + cls._pending_clones = [] + + if ovphysx_device == "gpu": + cls._physx.warmup_gpu() + + cls.dispatch_event(PhysicsEvent.MODEL_INIT, payload={}) + cls._warmup_done = True + + @staticmethod + def _configure_physx_scene_prim(scene_prim, cfg) -> None: + """Apply PhysxSceneAPI schema and GPU dynamics attributes to a scene prim. + + The PhysxSchema USD plugin may not be loaded in standalone ovphysx mode, + so we write the apiSchemas list entry and scene attributes directly via + raw Sdf metadata manipulation instead of using the high-level USD API. + + Without these attributes PhysX defaults to CPU broadphase even when + ovphysx is created with device="gpu". + """ + from pxr import Sdf + + schemas = Sdf.TokenListOp() + current = scene_prim.GetMetadata("apiSchemas") or Sdf.TokenListOp() + items = list(current.prependedItems) if current.prependedItems else [] + if "PhysxSceneAPI" not in items: + items.append("PhysxSceneAPI") + schemas.prependedItems = items + scene_prim.SetMetadata("apiSchemas", schemas) + scene_prim.CreateAttribute("physxScene:enableGPUDynamics", Sdf.ValueTypeNames.Bool).Set(True) + scene_prim.CreateAttribute("physxScene:broadphaseType", Sdf.ValueTypeNames.String).Set("GPU") + + if cfg is not None: + for attr, val in [ + ("gpuMaxRigidContactCount", cfg.gpu_max_rigid_contact_count), + ("gpuMaxRigidPatchCount", cfg.gpu_max_rigid_patch_count), + ("gpuFoundLostPairsCapacity", cfg.gpu_found_lost_pairs_capacity), + ("gpuFoundLostAggregatePairsCapacity", cfg.gpu_found_lost_aggregate_pairs_capacity), + ("gpuTotalAggregatePairsCapacity", cfg.gpu_total_aggregate_pairs_capacity), + ("gpuCollisionStackSize", cfg.gpu_collision_stack_size), + ]: + scene_prim.CreateAttribute(f"physxScene:{attr}", Sdf.ValueTypeNames.UInt).Set(val) + + # Propagate scene query support from SimulationCfg so omni.physx creates + # the scene with the correct query mode. OvPhysxCfg does not carry this field. + sim_cfg = PhysicsManager._sim.cfg if PhysicsManager._sim is not None else None + enable_sq = getattr(sim_cfg, "enable_scene_query_support", False) + scene_prim.CreateAttribute("physxScene:enableSceneQuerySupport", Sdf.ValueTypeNames.Bool).Set(enable_sq) diff --git a/source/isaaclab_ovphysx/isaaclab_ovphysx/physics/ovphysx_manager_cfg.py b/source/isaaclab_ovphysx/isaaclab_ovphysx/physics/ovphysx_manager_cfg.py new file mode 100644 index 000000000000..c74dc56209ea --- /dev/null +++ b/source/isaaclab_ovphysx/isaaclab_ovphysx/physics/ovphysx_manager_cfg.py @@ -0,0 +1,41 @@ +# 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 + +"""Configuration for the OvPhysX physics manager.""" + +from __future__ import annotations + +from isaaclab.physics import PhysicsCfg +from isaaclab.utils import configclass + + +@configclass +class OvPhysxCfg(PhysicsCfg): + """Configuration for the ovphysx physics manager. + + PhysX scene-level parameters (solver iterations, GPU buffer sizes, etc.) are + read from the USD PhysicsScene prim. Only ovphysx-specific settings that are + not captured in USD live here. + """ + + class_type = "{DIR}.ovphysx_manager:OvPhysxManager" + + gpu_max_rigid_contact_count: int = 2**23 + """Size of the GPU rigid-body contact buffer.""" + + gpu_max_rigid_patch_count: int = 5 * 2**15 + """Size of the GPU rigid-body patch buffer.""" + + gpu_found_lost_pairs_capacity: int = 2**21 + """Capacity for GPU found/lost broadphase pairs.""" + + gpu_found_lost_aggregate_pairs_capacity: int = 2**25 + """Capacity for GPU found/lost *aggregate* broadphase pairs.""" + + gpu_total_aggregate_pairs_capacity: int = 2**21 + """Capacity for total GPU aggregate broadphase pairs.""" + + gpu_collision_stack_size: int = 2**26 + """GPU collision stack size in bytes.""" diff --git a/source/isaaclab_ovphysx/isaaclab_ovphysx/tensor_types.py b/source/isaaclab_ovphysx/isaaclab_ovphysx/tensor_types.py new file mode 100644 index 000000000000..44a5cadeeb0a --- /dev/null +++ b/source/isaaclab_ovphysx/isaaclab_ovphysx/tensor_types.py @@ -0,0 +1,334 @@ +# 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 + +"""IsaacLab re-exports of ovphysx TensorType with short backward-compat aliases. + +Import TensorType directly for new code: + from ovphysx.types import TensorType + +Or use the module-level short aliases (existing code pattern): + import isaaclab_ovphysx.tensor_types as TT + TT.DOF_STIFFNESS # resolves to TensorType.ARTICULATION_DOF_STIFFNESS + +ovphysx.types is pure Python with zero native dependencies, so this module is +always safe to import regardless of USD state or native library loading. +""" + +from ovphysx.types import TensorType # noqa: F401 — re-exported for new code + +_TT = TensorType # shorter reference for alias block + +# Short aliases -- existing code using ``TT.DOF_STIFFNESS`` etc. continues to work. +# All values are IntEnum members (== plain ints) of TensorType. + +# fmt: off -- aligned columns are intentional; do not reformat + +""" +Root state (GPU) +""" + +ROOT_POSE = _TT.ARTICULATION_ROOT_POSE +"""Root pose of each articulation instance. + +Shape is ``[N, 7]``, dtype ``float32`` (px, py, pz, qx, qy, qz, qw). +""" + +ROOT_VELOCITY = _TT.ARTICULATION_ROOT_VELOCITY +"""Root velocity of each articulation instance. + +Shape is ``[N, 6]``, dtype ``float32`` (vx, vy, vz, wx, wy, wz). +""" + +""" +Link (body) state (GPU) +""" + +LINK_POSE = _TT.ARTICULATION_LINK_POSE +"""Pose of every link (body) in each articulation instance. + +Shape is ``[N, L, 7]``, dtype ``float32``. +""" + +LINK_VELOCITY = _TT.ARTICULATION_LINK_VELOCITY +"""Velocity of every link (body) in each articulation instance. + +Shape is ``[N, L, 6]``, dtype ``float32``. +""" + +LINK_ACCELERATION = _TT.ARTICULATION_LINK_ACCELERATION +"""Acceleration of every link (body) in each articulation instance. + +Shape is ``[N, L, 6]``, dtype ``float32``. +""" + +""" +DOF state (GPU) +""" + +DOF_POSITION = _TT.ARTICULATION_DOF_POSITION +"""DOF (joint) positions. + +Shape is ``[N, D]``, dtype ``float32`` [m or rad]. +""" + +DOF_VELOCITY = _TT.ARTICULATION_DOF_VELOCITY +"""DOF (joint) velocities. + +Shape is ``[N, D]``, dtype ``float32`` [m/s or rad/s]. +""" + +""" +DOF command targets (GPU, write-only) +""" + +DOF_POSITION_TARGET = _TT.ARTICULATION_DOF_POSITION_TARGET +"""DOF position targets for the PD controller. + +Shape is ``[N, D]``, dtype ``float32``. +""" + +DOF_VELOCITY_TARGET = _TT.ARTICULATION_DOF_VELOCITY_TARGET +"""DOF velocity targets for the PD controller. + +Shape is ``[N, D]``, dtype ``float32``. +""" + +DOF_ACTUATION_FORCE = _TT.ARTICULATION_DOF_ACTUATION_FORCE +"""DOF actuation (effort) forces applied directly. + +Shape is ``[N, D]``, dtype ``float32`` [N or N·m]. +""" + +""" +DOF properties (CPU) +""" + +DOF_STIFFNESS = _TT.ARTICULATION_DOF_STIFFNESS +"""DOF stiffness (spring constant for PD controller). + +Shape is ``[N, D]``, dtype ``float32``. +""" + +DOF_DAMPING = _TT.ARTICULATION_DOF_DAMPING +"""DOF damping (damper constant for PD controller). + +Shape is ``[N, D]``, dtype ``float32``. +""" + +DOF_LIMIT = _TT.ARTICULATION_DOF_LIMIT +"""DOF position limits (lower, upper). + +Shape is ``[N, D, 2]``, dtype ``float32``. +""" + +DOF_MAX_VELOCITY = _TT.ARTICULATION_DOF_MAX_VELOCITY +"""DOF maximum velocity. + +Shape is ``[N, D]``, dtype ``float32``. +""" + +DOF_MAX_FORCE = _TT.ARTICULATION_DOF_MAX_FORCE +"""DOF maximum force. + +Shape is ``[N, D]``, dtype ``float32``. +""" + +DOF_ARMATURE = _TT.ARTICULATION_DOF_ARMATURE +"""DOF armature (added inertia on the diagonal of the joint-space mass matrix). + +Shape is ``[N, D]``, dtype ``float32``. +""" + +DOF_FRICTION_PROPERTIES = _TT.ARTICULATION_DOF_FRICTION_PROPERTIES +"""DOF friction properties (static, dynamic, viscous). + +Shape is ``[N, D, 3]``, dtype ``float32``. +""" + +""" +External wrench (GPU, write-only) +""" + +LINK_WRENCH = _TT.ARTICULATION_LINK_WRENCH +"""External wrench applied to each link. + +Shape is ``[N, L, 9]``, dtype ``float32`` (fx, fy, fz, tx, ty, tz, px, py, pz). +""" + +""" +Body properties (CPU) +""" + +BODY_MASS = _TT.ARTICULATION_BODY_MASS +"""Mass of each body (link). + +Shape is ``[N, L]``, dtype ``float32`` [kg]. +""" + +BODY_COM_POSE = _TT.ARTICULATION_BODY_COM_POSE +"""Center-of-mass pose of each body in local frame. + +Shape is ``[N, L, 7]``, dtype ``float32``. +""" + +BODY_INERTIA = _TT.ARTICULATION_BODY_INERTIA +"""Inertia tensor of each body. + +Shape is ``[N, L, 9]``, dtype ``float32`` [kg·m^2]. +""" + +BODY_INV_MASS = _TT.ARTICULATION_BODY_INV_MASS +"""Inverse mass of each body. + +Shape is ``[N, L]``, dtype ``float32``. +""" + +BODY_INV_INERTIA = _TT.ARTICULATION_BODY_INV_INERTIA +"""Inverse inertia tensor of each body. + +Shape is ``[N, L, 9]``, dtype ``float32``. +""" + +""" +Dynamics tensors (GPU) +""" + +JACOBIAN = _TT.ARTICULATION_JACOBIAN +"""Jacobian matrix of each articulation instance. + +Shape is ``[N, L, 6, D+6]``, dtype ``float32``. +""" + +MASS_MATRIX = _TT.ARTICULATION_MASS_MATRIX +"""Generalized mass (inertia) matrix. + +Shape is ``[N, D+6, D+6]``, dtype ``float32``. +""" + +CORIOLIS = _TT.ARTICULATION_CORIOLIS_AND_CENTRIFUGAL_FORCE +"""Coriolis and centrifugal force vector. + +Shape is ``[N, D]``, dtype ``float32``. +""" + +GRAVITY_FORCE = _TT.ARTICULATION_GRAVITY_FORCE +"""Generalized gravity force vector. + +Shape is ``[N, D]``, dtype ``float32``. +""" + +""" +Joint force feedback (GPU) +""" + +LINK_INCOMING_JOINT_FORCE = _TT.ARTICULATION_LINK_INCOMING_JOINT_FORCE +"""Incoming joint force (constraint force) on each link. + +Shape is ``[N, L, 6]``, dtype ``float32``. +""" + +DOF_PROJECTED_JOINT_FORCE = _TT.ARTICULATION_DOF_PROJECTED_JOINT_FORCE +"""DOF-projected joint force. + +Shape is ``[N, D]``, dtype ``float32``. +""" + +""" +Fixed tendon properties (CPU) +""" + +FIXED_TENDON_STIFFNESS = _TT.ARTICULATION_FIXED_TENDON_STIFFNESS +"""Stiffness of each fixed tendon. + +Shape is ``[N, T_fix]``, dtype ``float32``. +""" + +FIXED_TENDON_DAMPING = _TT.ARTICULATION_FIXED_TENDON_DAMPING +"""Damping of each fixed tendon. + +Shape is ``[N, T_fix]``, dtype ``float32``. +""" + +FIXED_TENDON_LIMIT_STIFFNESS = _TT.ARTICULATION_FIXED_TENDON_LIMIT_STIFFNESS +"""Limit stiffness of each fixed tendon. + +Shape is ``[N, T_fix]``, dtype ``float32``. +""" + +FIXED_TENDON_LIMIT = _TT.ARTICULATION_FIXED_TENDON_LIMIT +"""Position limits of each fixed tendon (lower, upper). + +Shape is ``[N, T_fix, 2]``, dtype ``float32``. +""" + +FIXED_TENDON_REST_LENGTH = _TT.ARTICULATION_FIXED_TENDON_REST_LENGTH +"""Rest length of each fixed tendon. + +Shape is ``[N, T_fix]``, dtype ``float32``. +""" + +FIXED_TENDON_OFFSET = _TT.ARTICULATION_FIXED_TENDON_OFFSET +"""Offset of each fixed tendon. + +Shape is ``[N, T_fix]``, dtype ``float32``. +""" + +""" +Spatial tendon properties (CPU) +""" + +SPATIAL_TENDON_STIFFNESS = _TT.ARTICULATION_SPATIAL_TENDON_STIFFNESS +"""Stiffness of each spatial tendon. + +Shape is ``[N, T_spa]``, dtype ``float32``. +""" + +SPATIAL_TENDON_DAMPING = _TT.ARTICULATION_SPATIAL_TENDON_DAMPING +"""Damping of each spatial tendon. + +Shape is ``[N, T_spa]``, dtype ``float32``. +""" + +SPATIAL_TENDON_LIMIT_STIFFNESS = _TT.ARTICULATION_SPATIAL_TENDON_LIMIT_STIFFNESS +"""Limit stiffness of each spatial tendon. + +Shape is ``[N, T_spa]``, dtype ``float32``. +""" + +SPATIAL_TENDON_OFFSET = _TT.ARTICULATION_SPATIAL_TENDON_OFFSET +"""Offset of each spatial tendon. + +Shape is ``[N, T_spa]``, dtype ``float32``. +""" + +# fmt: on +# DOF/body property tensor types are CPU-resident even in GPU simulations. +# Write helpers check this set to route data through CPU, not self._device. +_CPU_ONLY_TYPES: frozenset[TensorType] = frozenset( + { + DOF_STIFFNESS, + DOF_DAMPING, + DOF_LIMIT, + DOF_MAX_VELOCITY, + DOF_MAX_FORCE, + DOF_ARMATURE, + DOF_FRICTION_PROPERTIES, + BODY_MASS, + BODY_COM_POSE, + BODY_INERTIA, + BODY_INV_MASS, + BODY_INV_INERTIA, + FIXED_TENDON_STIFFNESS, + FIXED_TENDON_DAMPING, + FIXED_TENDON_LIMIT_STIFFNESS, + FIXED_TENDON_LIMIT, + FIXED_TENDON_REST_LENGTH, + FIXED_TENDON_OFFSET, + SPATIAL_TENDON_STIFFNESS, + SPATIAL_TENDON_DAMPING, + SPATIAL_TENDON_LIMIT_STIFFNESS, + SPATIAL_TENDON_OFFSET, + } +) diff --git a/source/isaaclab_ovphysx/isaaclab_ovphysx/test/__init__.py b/source/isaaclab_ovphysx/isaaclab_ovphysx/test/__init__.py new file mode 100644 index 000000000000..460a30569089 --- /dev/null +++ b/source/isaaclab_ovphysx/isaaclab_ovphysx/test/__init__.py @@ -0,0 +1,4 @@ +# 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_ovphysx/isaaclab_ovphysx/test/mock_interfaces/__init__.py b/source/isaaclab_ovphysx/isaaclab_ovphysx/test/mock_interfaces/__init__.py new file mode 100644 index 000000000000..2c6c0e67ffc4 --- /dev/null +++ b/source/isaaclab_ovphysx/isaaclab_ovphysx/test/mock_interfaces/__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 + +from .views import MockTensorBinding, MockOvPhysxBindingSet diff --git a/source/isaaclab_ovphysx/isaaclab_ovphysx/test/mock_interfaces/views/__init__.py b/source/isaaclab_ovphysx/isaaclab_ovphysx/test/mock_interfaces/views/__init__.py new file mode 100644 index 000000000000..6f133a18202c --- /dev/null +++ b/source/isaaclab_ovphysx/isaaclab_ovphysx/test/mock_interfaces/views/__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 + +from .mock_ovphysx_bindings import MockTensorBinding, MockOvPhysxBindingSet diff --git a/source/isaaclab_ovphysx/isaaclab_ovphysx/test/mock_interfaces/views/mock_ovphysx_bindings.py b/source/isaaclab_ovphysx/isaaclab_ovphysx/test/mock_interfaces/views/mock_ovphysx_bindings.py new file mode 100644 index 000000000000..29472ce74fd0 --- /dev/null +++ b/source/isaaclab_ovphysx/isaaclab_ovphysx/test/mock_interfaces/views/mock_ovphysx_bindings.py @@ -0,0 +1,276 @@ +# 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 + +"""Mock implementations of ovphysx TensorBinding objects for unit testing.""" + +from __future__ import annotations + +import numpy as np + +from isaaclab_ovphysx import tensor_types as TT + + +class MockTensorBinding: + """Mock of ovphysx.TensorBinding that stores data in numpy arrays. + + Mimics the real TensorBinding API: ``read(tensor)`` fills the tensor from + the internal buffer, ``write(tensor, indices, mask)`` copies into it. + """ + + def __init__( + self, + tensor_type: int, + shape: tuple[int, ...], + count: int, + dof_count: int = 0, + body_count: int = 0, + joint_count: int = 0, + is_fixed_base: bool = False, + dof_names: list[str] | None = None, + body_names: list[str] | None = None, + joint_names: list[str] | None = None, + fixed_tendon_count: int = 0, + spatial_tendon_count: int = 0, + write_only: bool = False, + ): + self.tensor_type = tensor_type + self._shape = shape + self._count = count + self._dof_count = dof_count + self._body_count = body_count + self._joint_count = joint_count + self._is_fixed_base = is_fixed_base + self._dof_names = dof_names or [] + self._body_names = body_names or [] + self._joint_names = joint_names or [] + self._fixed_tendon_count = fixed_tendon_count + self._spatial_tendon_count = spatial_tendon_count + self._write_only = write_only + self._data = np.zeros(shape, dtype=np.float32) + + @property + def shape(self) -> tuple[int, ...]: + return self._shape + + @property + def ndim(self) -> int: + return len(self._shape) + + @property + def count(self) -> int: + return self._count + + @property + def dof_count(self) -> int: + return self._dof_count + + @property + def body_count(self) -> int: + return self._body_count + + @property + def joint_count(self) -> int: + return self._joint_count + + @property + def is_fixed_base(self) -> bool: + return self._is_fixed_base + + @property + def dof_names(self) -> list[str]: + return self._dof_names + + @property + def body_names(self) -> list[str]: + return self._body_names + + @property + def joint_names(self) -> list[str]: + return self._joint_names + + @property + def fixed_tendon_count(self) -> int: + return self._fixed_tendon_count + + @property + def spatial_tendon_count(self) -> int: + return self._spatial_tendon_count + + def read(self, tensor) -> None: + """Copy internal data into the provided array (numpy or warp).""" + if self._write_only: + raise RuntimeError("write-only tensor binding does not support read()") + try: + import warp as wp + + if isinstance(tensor, wp.array): + tmp = wp.from_numpy(self._data, dtype=wp.float32, device=tensor.device) + wp.copy(tensor, tmp) + return + except ImportError: + pass + np_dst = np.asarray(tensor) + np.copyto(np_dst, self._data.reshape(np_dst.shape)) + + @staticmethod + def _to_numpy(arr) -> np.ndarray: + """Convert warp/torch/numpy array to numpy, handling GPU arrays.""" + try: + import warp as wp + + if isinstance(arr, wp.array): + return arr.numpy() + except ImportError: + pass + try: + import torch + + if isinstance(arr, torch.Tensor): + return arr.detach().cpu().numpy() + except ImportError: + pass + return np.asarray(arr) + + def write(self, tensor, indices=None, mask=None) -> None: + """Copy from the provided array (numpy or warp) into internal data.""" + np_src = self._to_numpy(tensor).astype(np.float32) + if indices is not None: + idx = self._to_numpy(indices) + if np_src.shape[0] == self._data.shape[0]: + self._data[idx] = np_src[idx] + else: + self._data[idx] = np_src.reshape(len(idx), *self._data.shape[1:]) + elif mask is not None: + np_mask = self._to_numpy(mask).astype(bool) + self._data[np_mask] = np_src[np_mask] + else: + np.copyto(self._data, np_src.reshape(self._data.shape)) + + def destroy(self) -> None: + pass + + def set_random_data(self, low: float = -1.0, high: float = 1.0) -> None: + """Fill internal buffer with random data.""" + self._data = np.random.uniform(low, high, self._shape).astype(np.float32) + + +class MockOvPhysxBindingSet: + """Factory that creates a full set of MockTensorBinding objects + for a given articulation configuration. + + Mirrors the tensor types that ``Articulation._initialize_impl`` creates. + """ + + def __init__( + self, + num_instances: int, + num_joints: int, + num_bodies: int, + is_fixed_base: bool = False, + joint_names: list[str] | None = None, + body_names: list[str] | None = None, + num_fixed_tendons: int = 0, + num_spatial_tendons: int = 0, + ): + N = num_instances + D = num_joints + L = num_bodies + T_fix = num_fixed_tendons + T_spa = num_spatial_tendons + + if joint_names is None: + joint_names = [f"joint_{i}" for i in range(D)] + if body_names is None: + body_names = [f"body_{i}" for i in range(L)] + + common = dict( + count=N, + dof_count=D, + body_count=L, + joint_count=D, + is_fixed_base=is_fixed_base, + dof_names=joint_names, + body_names=body_names, + joint_names=joint_names, + fixed_tendon_count=T_fix, + spatial_tendon_count=T_spa, + ) + + self.bindings: dict[int, MockTensorBinding] = { + TT.ROOT_POSE: MockTensorBinding(TT.ROOT_POSE, (N, 7), **common), + TT.ROOT_VELOCITY: MockTensorBinding(TT.ROOT_VELOCITY, (N, 6), **common), + TT.LINK_POSE: MockTensorBinding(TT.LINK_POSE, (N, L, 7), **common), + TT.LINK_VELOCITY: MockTensorBinding(TT.LINK_VELOCITY, (N, L, 6), **common), + TT.LINK_ACCELERATION: MockTensorBinding(TT.LINK_ACCELERATION, (N, L, 6), **common), + TT.DOF_POSITION: MockTensorBinding(TT.DOF_POSITION, (N, D), **common), + TT.DOF_VELOCITY: MockTensorBinding(TT.DOF_VELOCITY, (N, D), **common), + TT.DOF_POSITION_TARGET: MockTensorBinding(TT.DOF_POSITION_TARGET, (N, D), **common), + TT.DOF_VELOCITY_TARGET: MockTensorBinding(TT.DOF_VELOCITY_TARGET, (N, D), **common), + TT.DOF_ACTUATION_FORCE: MockTensorBinding(TT.DOF_ACTUATION_FORCE, (N, D), **common), + TT.DOF_STIFFNESS: MockTensorBinding(TT.DOF_STIFFNESS, (N, D), **common), + TT.DOF_DAMPING: MockTensorBinding(TT.DOF_DAMPING, (N, D), **common), + TT.DOF_LIMIT: MockTensorBinding(TT.DOF_LIMIT, (N, D, 2), **common), + TT.DOF_MAX_VELOCITY: MockTensorBinding(TT.DOF_MAX_VELOCITY, (N, D), **common), + TT.DOF_MAX_FORCE: MockTensorBinding(TT.DOF_MAX_FORCE, (N, D), **common), + TT.DOF_ARMATURE: MockTensorBinding(TT.DOF_ARMATURE, (N, D), **common), + TT.DOF_FRICTION_PROPERTIES: MockTensorBinding(TT.DOF_FRICTION_PROPERTIES, (N, D, 3), **common), + TT.LINK_WRENCH: MockTensorBinding(TT.LINK_WRENCH, (N, L, 9), write_only=True, **common), + TT.BODY_MASS: MockTensorBinding(TT.BODY_MASS, (N, L), **common), + TT.BODY_COM_POSE: MockTensorBinding(TT.BODY_COM_POSE, (N, L, 7), **common), + TT.BODY_INERTIA: MockTensorBinding(TT.BODY_INERTIA, (N, L, 9), **common), + TT.BODY_INV_MASS: MockTensorBinding(TT.BODY_INV_MASS, (N, L), **common), + TT.BODY_INV_INERTIA: MockTensorBinding(TT.BODY_INV_INERTIA, (N, L, 9), **common), + TT.LINK_INCOMING_JOINT_FORCE: MockTensorBinding(TT.LINK_INCOMING_JOINT_FORCE, (N, L, 6), **common), + TT.DOF_PROJECTED_JOINT_FORCE: MockTensorBinding(TT.DOF_PROJECTED_JOINT_FORCE, (N, D), **common), + } + + # Fixed tendon bindings (only when tendons are present) + if T_fix > 0: + self.bindings.update( + { + TT.FIXED_TENDON_STIFFNESS: MockTensorBinding(TT.FIXED_TENDON_STIFFNESS, (N, T_fix), **common), + TT.FIXED_TENDON_DAMPING: MockTensorBinding(TT.FIXED_TENDON_DAMPING, (N, T_fix), **common), + TT.FIXED_TENDON_LIMIT_STIFFNESS: MockTensorBinding( + TT.FIXED_TENDON_LIMIT_STIFFNESS, (N, T_fix), **common + ), + TT.FIXED_TENDON_LIMIT: MockTensorBinding(TT.FIXED_TENDON_LIMIT, (N, T_fix, 2), **common), + TT.FIXED_TENDON_REST_LENGTH: MockTensorBinding(TT.FIXED_TENDON_REST_LENGTH, (N, T_fix), **common), + TT.FIXED_TENDON_OFFSET: MockTensorBinding(TT.FIXED_TENDON_OFFSET, (N, T_fix), **common), + } + ) + + # Spatial tendon bindings + if T_spa > 0: + self.bindings.update( + { + TT.SPATIAL_TENDON_STIFFNESS: MockTensorBinding(TT.SPATIAL_TENDON_STIFFNESS, (N, T_spa), **common), + TT.SPATIAL_TENDON_DAMPING: MockTensorBinding(TT.SPATIAL_TENDON_DAMPING, (N, T_spa), **common), + TT.SPATIAL_TENDON_LIMIT_STIFFNESS: MockTensorBinding( + TT.SPATIAL_TENDON_LIMIT_STIFFNESS, (N, T_spa), **common + ), + TT.SPATIAL_TENDON_OFFSET: MockTensorBinding(TT.SPATIAL_TENDON_OFFSET, (N, T_spa), **common), + } + ) + + def set_random_data(self) -> None: + """Fill all bindings with random data.""" + for b in self.bindings.values(): + if not b._write_only: + b.set_random_data() + lim = self.bindings[TT.DOF_LIMIT] + lim._data[..., 0] = -3.14 + lim._data[..., 1] = 3.14 + for tt in (TT.ROOT_POSE, TT.LINK_POSE, TT.BODY_COM_POSE): + b = self.bindings[tt] + b._data[..., 3:6] = 0.0 + b._data[..., 6] = 1.0 + self.bindings[TT.BODY_MASS]._data = np.abs(self.bindings[TT.BODY_MASS]._data) + 0.1 + self.bindings[TT.DOF_MAX_VELOCITY]._data = np.abs(self.bindings[TT.DOF_MAX_VELOCITY]._data) + 1.0 + self.bindings[TT.DOF_MAX_FORCE]._data = np.abs(self.bindings[TT.DOF_MAX_FORCE]._data) + 1.0 + # Set sensible defaults for fixed tendon limits + if TT.FIXED_TENDON_LIMIT in self.bindings: + tlim = self.bindings[TT.FIXED_TENDON_LIMIT] + tlim._data[..., 0] = -1.0 + tlim._data[..., 1] = 1.0 diff --git a/source/isaaclab_ovphysx/pyproject.toml b/source/isaaclab_ovphysx/pyproject.toml new file mode 100644 index 000000000000..31dce8d230ec --- /dev/null +++ b/source/isaaclab_ovphysx/pyproject.toml @@ -0,0 +1,3 @@ +[build-system] +requires = ["setuptools<82.0.0", "wheel", "toml"] +build-backend = "setuptools.build_meta" diff --git a/source/isaaclab_ovphysx/setup.py b/source/isaaclab_ovphysx/setup.py new file mode 100644 index 000000000000..f590ce02140c --- /dev/null +++ b/source/isaaclab_ovphysx/setup.py @@ -0,0 +1,50 @@ +# 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 + +"""Installation script for the 'isaaclab_ovphysx' python package.""" + +import os + +import toml +from setuptools import setup + +EXTENSION_PATH = os.path.dirname(os.path.realpath(__file__)) +EXTENSION_TOML_DATA = toml.load(os.path.join(EXTENSION_PATH, "config", "extension.toml")) + +INSTALL_REQUIRES = [ + "ovphysx", +] + +setup( + name="isaaclab_ovphysx", + author="Isaac Lab Project Developers", + maintainer="Isaac Lab Project Developers", + url=EXTENSION_TOML_DATA["package"]["repository"], + version=EXTENSION_TOML_DATA["package"]["version"], + description=EXTENSION_TOML_DATA["package"]["description"], + keywords=EXTENSION_TOML_DATA["package"]["keywords"], + license="BSD-3-Clause", + include_package_data=True, + package_data={"": ["*.pyi"]}, + python_requires=">=3.11", + install_requires=INSTALL_REQUIRES, + packages=[ + "isaaclab_ovphysx", + "isaaclab_ovphysx.assets", + "isaaclab_ovphysx.assets.articulation", + "isaaclab_ovphysx.cloner", + "isaaclab_ovphysx.physics", + "isaaclab_ovphysx.test", + "isaaclab_ovphysx.test.mock_interfaces", + "isaaclab_ovphysx.test.mock_interfaces.views", + ], + classifiers=[ + "Natural Language :: English", + "Programming Language :: Python :: 3.11", + "Programming Language :: Python :: 3.12", + "Isaac Sim :: 6.0.0", + ], + zip_safe=False, +) diff --git a/source/isaaclab_ovphysx/test/__init__.py b/source/isaaclab_ovphysx/test/__init__.py new file mode 100644 index 000000000000..460a30569089 --- /dev/null +++ b/source/isaaclab_ovphysx/test/__init__.py @@ -0,0 +1,4 @@ +# 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_ovphysx/test/assets/__init__.py b/source/isaaclab_ovphysx/test/assets/__init__.py new file mode 100644 index 000000000000..460a30569089 --- /dev/null +++ b/source/isaaclab_ovphysx/test/assets/__init__.py @@ -0,0 +1,4 @@ +# 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_ovphysx/test/assets/test_articulation.py b/source/isaaclab_ovphysx/test/assets/test_articulation.py new file mode 100644 index 000000000000..52998a2ec5f6 --- /dev/null +++ b/source/isaaclab_ovphysx/test/assets/test_articulation.py @@ -0,0 +1,116 @@ +# 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 ovphysx articulation helpers.""" + +from __future__ import annotations + +from types import SimpleNamespace + +import pytest +import warp as wp + +from pxr import Sdf, Usd, UsdPhysics + +# The CI isaaclab_ov* pattern unintentionally collects isaaclab_ovphysx tests, +# but the ovphysx wheel is not installed in that environment. Skip gracefully +# so the isaaclab_ov CI pipeline is not blocked by an unrelated dependency. +pytest.importorskip("ovphysx.types", reason="ovphysx wheel not installed") + +from isaaclab_ovphysx.assets.articulation.articulation import Articulation # noqa: E402 +from isaaclab_ovphysx.physics import OvPhysxManager # noqa: E402 +from isaaclab_ovphysx.test.mock_interfaces.views import MockOvPhysxBindingSet # noqa: E402 + +wp.init() + + +def _define_tendon_joint(stage: Usd.Stage, path: str, schema_name: str) -> None: + """Define a revolute joint prim with a tendon schema marker.""" + joint = UsdPhysics.RevoluteJoint.Define(stage, path) + schemas = Sdf.TokenListOp() + schemas.explicitItems = [schema_name] + joint.GetPrim().SetMetadata("apiSchemas", schemas) + + +def _make_articulation_root_stage(tmp_path) -> str: + """Create a stage with one relevant articulation subtree and unrelated joints elsewhere.""" + stage = Usd.Stage.CreateInMemory() + stage.DefinePrim("/World", "Xform") + stage.DefinePrim("/World/envs", "Xform") + stage.DefinePrim("/World/envs/env_0", "Xform") + stage.DefinePrim("/World/envs/env_0/Robot", "Xform") + stage.DefinePrim("/World/envs/env_0/Robot/root", "Xform") + stage.DefinePrim("/World/unrelated", "Xform") + + _define_tendon_joint( + stage, + "/World/envs/env_0/Robot/root/fixed_joint", + "PhysxTendonAxisRootAPI:inst0", + ) + _define_tendon_joint( + stage, + "/World/envs/env_0/Robot/root/spatial_joint", + "PhysxTendonAttachmentRootAPI:inst0", + ) + _define_tendon_joint( + stage, + "/World/unrelated/unrelated_fixed_joint", + "PhysxTendonAxisRootAPI:inst0", + ) + _define_tendon_joint( + stage, + "/World/unrelated/unrelated_spatial_joint", + "PhysxTendonAttachmentLeafAPI:inst0", + ) + + stage_path = tmp_path / "scene.usda" + stage.Export(str(stage_path)) + return str(stage_path) + + +def _make_articulation_shell() -> Articulation: + """Create a minimal ovphysx articulation shell for tendon processing tests.""" + articulation = object.__new__(Articulation) + bindings = MockOvPhysxBindingSet( + num_instances=1, + num_joints=2, + num_bodies=2, + num_fixed_tendons=1, + num_spatial_tendons=1, + ) + object.__setattr__(articulation, "_bindings", bindings.bindings) + object.__setattr__(articulation, "_articulation_root_path", "/World/envs/env_0/Robot/root") + object.__setattr__(articulation, "_initialize_handle", None) + object.__setattr__(articulation, "_invalidate_initialize_handle", None) + object.__setattr__(articulation, "_prim_deletion_handle", None) + object.__setattr__(articulation, "_debug_vis_handle", None) + object.__setattr__( + articulation, + "_data", + SimpleNamespace( + _num_fixed_tendons=0, + _num_spatial_tendons=0, + fixed_tendon_names=[], + spatial_tendon_names=[], + ), + ) + return articulation + + +def test_process_tendons_scopes_to_articulation_root(tmp_path): + """Tendon discovery should ignore joints that live outside the current articulation subtree.""" + articulation = _make_articulation_shell() + stage_path = _make_articulation_root_stage(tmp_path) + old_stage_path = OvPhysxManager._stage_path + OvPhysxManager._stage_path = stage_path + try: + articulation._process_tendons() + finally: + OvPhysxManager._stage_path = old_stage_path + + assert articulation.fixed_tendon_names == ["fixed_joint"] + assert articulation.spatial_tendon_names == ["spatial_joint"] + assert articulation._data.fixed_tendon_names == ["fixed_joint"] + assert articulation._data.spatial_tendon_names == ["spatial_joint"] diff --git a/source/isaaclab_ovphysx/test/assets/test_articulation_data.py b/source/isaaclab_ovphysx/test/assets/test_articulation_data.py new file mode 100644 index 000000000000..de17b611eda4 --- /dev/null +++ b/source/isaaclab_ovphysx/test/assets/test_articulation_data.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 + +"""Unit tests for ovphysx articulation data helpers.""" + +import numpy as np +import pytest +import warp as wp + +# The CI isaaclab_ov* pattern unintentionally collects isaaclab_ovphysx tests, +# but the ovphysx wheel is not installed in that environment. Skip gracefully +# so the isaaclab_ov CI pipeline is not blocked by an unrelated dependency. +pytest.importorskip("ovphysx.types", reason="ovphysx wheel not installed") + +from isaaclab_ovphysx import tensor_types as TT # noqa: E402 +from isaaclab_ovphysx.assets.articulation.articulation_data import ArticulationData +from isaaclab_ovphysx.test.mock_interfaces.views import MockOvPhysxBindingSet + +wp.init() + + +class TestArticulationData: + """Unit tests for deterministic ArticulationData behavior.""" + + def test_joint_acc_uses_inverse_dt(self): + """Finite-difference joint acceleration should divide by ``dt``.""" + mock_bindings = MockOvPhysxBindingSet(num_instances=1, num_joints=2, num_bodies=1) + data = ArticulationData(mock_bindings.bindings, device="cpu") + data._create_buffers() + + mock_bindings.bindings[TT.DOF_VELOCITY]._data[...] = np.array([[1.0, -2.0]], dtype=np.float32) + + data.update(dt=0.25) + + np.testing.assert_allclose( + data.joint_acc.numpy(), + np.array([[4.0, -8.0]], dtype=np.float32), + atol=1e-6, + err_msg="Joint acceleration should be computed as delta_velocity / dt.", + ) diff --git a/source/isaaclab_ovphysx/test/physics/__init__.py b/source/isaaclab_ovphysx/test/physics/__init__.py new file mode 100644 index 000000000000..460a30569089 --- /dev/null +++ b/source/isaaclab_ovphysx/test/physics/__init__.py @@ -0,0 +1,4 @@ +# 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_ovphysx/test/sensors/__init__.py b/source/isaaclab_ovphysx/test/sensors/__init__.py new file mode 100644 index 000000000000..460a30569089 --- /dev/null +++ b/source/isaaclab_ovphysx/test/sensors/__init__.py @@ -0,0 +1,4 @@ +# 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_ovphysx/test/sensors/check_contact_sensor.py b/source/isaaclab_ovphysx/test/sensors/check_contact_sensor.py new file mode 100644 index 000000000000..8bcd9f0941f6 --- /dev/null +++ b/source/isaaclab_ovphysx/test/sensors/check_contact_sensor.py @@ -0,0 +1,38 @@ +# 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 + +"""Contact sensor parity tests for the ovphysx backend. + +Mirrors the structure of isaaclab_physx/test/sensors/check_contact_sensor.py. +Contact sensors are not yet supported by the ovphysx backend, so all tests +are skipped with an explanatory message. +""" + +import pytest + + +def test_contact_sensor_creation(): + """Verify contact sensor can be created on the ovphysx backend.""" + pytest.skip("Contact sensor not yet supported by ovphysx backend.") + + +def test_contact_sensor_data_reading(): + """Verify contact sensor data can be read after a simulation step.""" + pytest.skip("Contact sensor not yet supported by ovphysx backend.") + + +def test_contact_sensor_reset(): + """Verify contact sensor state resets correctly.""" + pytest.skip("Contact sensor not yet supported by ovphysx backend.") + + +def test_contact_sensor_air_time_tracking(): + """Verify contact sensor air time tracking.""" + pytest.skip("Contact sensor not yet supported by ovphysx backend.") + + +def test_contact_sensor_friction_forces(): + """Verify contact sensor friction force reporting.""" + pytest.skip("Contact sensor not yet supported by ovphysx backend.") diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/ant/ant_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/ant/ant_env_cfg.py index b52f62dee9ea..10072b44e71a 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/ant/ant_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/ant/ant_env_cfg.py @@ -6,6 +6,7 @@ from __future__ import annotations from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg +from isaaclab_ovphysx.physics import OvPhysxCfg from isaaclab_physx.physics import PhysxCfg import isaaclab.sim as sim_utils @@ -36,6 +37,7 @@ class AntPhysicsCfg(PresetCfg): num_substeps=1, debug_mode=False, ) + ovphysx: OvPhysxCfg = OvPhysxCfg() @configclass diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/cartpole_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/cartpole_env_cfg.py index 2b38a7e0991a..90b1f70a1a22 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/cartpole_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/cartpole_env_cfg.py @@ -6,6 +6,7 @@ from __future__ import annotations from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg +from isaaclab_ovphysx.physics import OvPhysxCfg from isaaclab_physx.physics import PhysxCfg from isaaclab.assets import ArticulationCfg @@ -35,6 +36,7 @@ class CartpolePhysicsCfg(PresetCfg): debug_mode=False, use_cuda_graph=True, ) + ovphysx: OvPhysxCfg = OvPhysxCfg() @configclass diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid/humanoid_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid/humanoid_env_cfg.py index d68c6699e72c..fbd1a09b0202 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid/humanoid_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid/humanoid_env_cfg.py @@ -6,6 +6,7 @@ from __future__ import annotations from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg +from isaaclab_ovphysx.physics import OvPhysxCfg from isaaclab_physx.physics import PhysxCfg import isaaclab.sim as sim_utils @@ -37,6 +38,7 @@ class HumanoidPhysicsCfg(PresetCfg): num_substeps=2, debug_mode=False, ) + ovphysx: OvPhysxCfg = OvPhysxCfg() @configclass diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/locomotion/locomotion_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/locomotion/locomotion_env.py index f613efb02d86..87d7a6d8b3d8 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/locomotion/locomotion_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/locomotion/locomotion_env.py @@ -8,6 +8,7 @@ import torch import warp as wp from isaaclab_newton.physics import NewtonCfg +from isaaclab_ovphysx.physics import OvPhysxCfg from isaaclab_physx.physics import PhysxCfg import isaaclab.sim as sim_utils @@ -79,7 +80,7 @@ def __init__(self, cfg: DirectRLEnvCfg, render_mode: str | None = None, **kwargs self.action_scale = self.cfg.action_scale # Resolve the joint gears based on the physics type, since they do not have the same joint ordering. if isinstance(self.cfg.joint_gears, dict): - if isinstance(self.cfg.sim.physics, PhysxCfg): + if isinstance(self.cfg.sim.physics, (PhysxCfg, OvPhysxCfg)): joint_gears = self.cfg.joint_gears["physx"] elif isinstance(self.cfg.sim.physics, NewtonCfg): joint_gears = self.cfg.joint_gears["newton"] diff --git a/source/isaaclab_tasks/isaaclab_tasks/utils/sim_launcher.py b/source/isaaclab_tasks/isaaclab_tasks/utils/sim_launcher.py index 1296873b1537..964027858dae 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/utils/sim_launcher.py +++ b/source/isaaclab_tasks/isaaclab_tasks/utils/sim_launcher.py @@ -66,9 +66,9 @@ def _visit(node): return results -def _is_newton_physics(node) -> bool: - """True when the node is a Newton physics config (Kit is not required).""" - return isinstance(node, PhysicsCfg) and type(node).__name__ == "NewtonCfg" +def _is_kitless_physics(node) -> bool: + """True when the node is a kitless physics config (Newton or OvPhysX).""" + return isinstance(node, PhysicsCfg) and type(node).__name__ in ("NewtonCfg", "OvPhysxCfg") def _get_visualizer_types(launcher_args: argparse.Namespace | dict | None) -> set[str]: @@ -141,8 +141,8 @@ def compute_kit_requirements( Returns: (needs_kit, has_kit_cameras, visualizer_types) """ - is_newton, has_kit_cameras = _scan_config(env_cfg, [_is_newton_physics, _is_kit_camera]) - needs_kit = has_kit_cameras or not is_newton + is_kitless, has_kit_cameras = _scan_config(env_cfg, [_is_kitless_physics, _is_kit_camera]) + needs_kit = has_kit_cameras or not is_kitless visualizer_types = _get_visualizer_types(launcher_args) if "kit" in visualizer_types: needs_kit = True