diff --git a/agent_context/topics/simulation-system/physics-configuration.md b/agent_context/topics/simulation-system/physics-configuration.md index cb8986c849..4b31b386c0 100644 --- a/agent_context/topics/simulation-system/physics-configuration.md +++ b/agent_context/topics/simulation-system/physics-configuration.md @@ -31,11 +31,32 @@ contact-path policy. Expert settings use the native option groups are rejected for mappings and native config objects, and flat DexUni options are rejected by native validation. Standalone SPH is not a supported solver family; fluid settings belong under -[DexUni's nested SPH configuration](fluids.md). Gradient mode requires an explicit -`semi_implicit` solver. Read exact solver options/defaults in -`cfg/physics.py` and the installed DexSim config types; do not substitute a +[DexUni's nested SPH configuration](fluids.md). Robot presets select +`newton_dexuni` before the MuJoCo aliases and portable fallbacks. Gradient mode +requires an explicit `semi_implicit` solver or a direct/named provider supporting +the requested gradient mode. Functional Scene calls additionally require its +functional dynamics service. Python callers pass DexSim's +`ProviderNewtonSolverCfg` with a provider class/zero-argument factory directly +through `solver_cfg`; this path does not inspect installed metadata. DexSim +creates fresh instances per model build. Installed `dexsim.newton_solvers` +names pass through to DexSim's `RegisteredNewtonSolverCfg`; unregistered names +remain errors. Read exact solver options/defaults in `cfg/physics.py` and the +installed DexSim config types; do not substitute a hardcoded fallback when an expected DexSim API is absent. +The spring-slider provider lives only in +`scripts/tutorials/sim/simple_newton_solver/`; see its +[tutorial](../../../docs/source/tutorial/simple_newton_solver.rst) for functional +Scene stepping. It is not a packaged solver or registered entry point. + +Articulation initialization also calls `reset(clear_dynamics=False)` without a +later coordinated clear. Preserve its position hold targets and mimic target +propagation when the solver accepts them. Query the finalized public +`Scene.dynamics_capabilities().supported_control_fields`; only an explicit +exclusion of `joint_target_q` suppresses that command. Released SDKs predating +extensions retain their unrestricted target behavior. Extension SDKs missing +the query require an upgrade instead of guessing support from solver names. + `collision_cfg` owns the external Newton contact pipeline. Its `update_interval=None` updates at the first local solver substep; positive `k` updates at `0, k, 2k, ...`, reusing contacts between updates. `collision_cfg=None` diff --git a/docs/source/tutorial/simple_newton_solver.rst b/docs/source/tutorial/simple_newton_solver.rst new file mode 100644 index 0000000000..329d9f0a37 --- /dev/null +++ b/docs/source/tutorial/simple_newton_solver.rst @@ -0,0 +1,136 @@ +Build a custom Newton solver +============================ + +This tutorial implements a spring-slider solver with DexSim's Newton extension +interface. All of its code lives in ``scripts/tutorials/sim/simple_newton_solver/``; +the provider is an example, not an EmbodiChain solver or package entry point. +The scene contains two independent prismatic sliders, one in each environment. + +Run the example +--------------- + +Use a DexSim build that provides ``ProviderNewtonSolverCfg``, +``FunctionalDynamics`` and ``Scene.dynamics_capabilities()``. From the repository +root, run against an installed EmbodiChain environment or expose this checkout: + +.. code-block:: bash + + export PYTHONPATH="$PWD:$PWD/extensions${PYTHONPATH:+:$PYTHONPATH}" + python scripts/tutorials/sim/simple_newton_solver/demo.py --device cpu \ + --preview-path simple_newton_solver_preview.png + +Use ``--device cuda:0`` to run the same numerical example on CUDA. The script +runs headlessly. It checks native/functional parity and state isolation before +printing JSON. A successful CPU run reports approximately: + +.. code-block:: json + + { + "native_functional_parity": true, + "published_position": [0.020873, 0.031310], + "partial_reset_position": [0.0, 0.031310], + "force_gradient": [0.00012048, 0.00050749], + "force_scale_gradient": 0.00352685 + } + +The optional PNG plots the two slider trajectories. Its dashed final segment +shows that resetting ``arena_0`` returns only the first slider to zero. + +For an image rendered by EmbodiChain's native offscreen camera, run: + +.. code-block:: bash + + python scripts/tutorials/sim/simple_newton_solver/demo.py --device cpu \ + --render-path simple_newton_offscreen.png + +The resulting PNG has environment 0 and 1 in the left and right columns, with +initial and after-30-step views in the top and bottom rows. The script reserves +FastRT before constructing the World, creates a ``CameraCfg`` in each arena, +renders through ``SimulationManager.render_camera_group()``, and reads the RGB +frames from ``Camera.get_data()``. This needs a working native GPU renderer even +when the solver runs on the CPU. + +To record the same offscreen cameras as a video, run: + +.. code-block:: bash + + python scripts/tutorials/sim/simple_newton_solver/demo.py --device cpu \ + --video-path simple_newton_offscreen.mp4 + +The H.264 MP4 places the two environments side by side and records all 30 +native simulation steps at 10 fps, with short holds on the initial and final +poses. It requires ``ffmpeg`` with ``libx264`` on ``PATH``. Combine +``--render-path`` and ``--video-path`` to save both outputs in one run. + +To inspect the actual EmbodiChain scene in Viser, run: + +.. code-block:: bash + + python scripts/tutorials/sim/simple_newton_solver/demo.py --device cpu \ + --viser --viser-seconds 60 + +Open ``http://127.0.0.1:8080`` while the script runs. Viser receives both URDF +slider meshes, then shows 30 native simulation steps. The scene remains open +for the requested number of seconds after replay. Use ``--viser-port PORT`` if +8080 is in use. The ``base`` link has no visual geometry, so the visible objects +are the two cubes. This browser view uses EmbodiChain's +``SimulationManager.capture_visualization()`` and does not require a native +render window. + +Follow the data flow +-------------------- + +1. ``demo.py`` passes the local ``SimpleSolverProvider`` class to + ``NewtonPhysicsCfg`` through DexSim's ``ProviderNewtonSolverCfg``. The direct + factory path does not require package entry-point registration. +2. ``SimulationManager.prepare()`` finalizes the scene and asks DexSim to create + a fresh provider for the model. The provider validates the slider model and + declares effort-only controls, reset support and no CUDA Graph support. +3. ``_SliderDynamics.step()`` applies effort, spring force and damping, then + updates velocity and position with semi-implicit Euler. Its Torch operations + preserve gradients in the functional path. +4. The native ``SolverBase`` adapter calls that same transition under + ``torch.no_grad()`` and writes the resulting state to the next Newton state. + ``demo.py`` checks that one native step matches one functional step. +5. Functional stepping with ``publish=False`` leaves the live scene untouched. + ``publish=True`` publishes a detached physical state and advances the Scene + clock once. The returned functional tensors retain their gradient graph. + Reset selects arenas independently. + +For a slider coordinate ``q``, velocity ``v``, effort ``u``, trainable scale +``g`` and effective mass ``m``, the unconstrained transition is: + +.. code-block:: text + + force = clamp(g * u, -effort_limit, effort_limit) + acceleration = (force - stiffness * q - damping * v) / m + v_next = clamp(v + dt * acceleration, -velocity_limit, velocity_limit) + q_next = clamp(q + dt * v_next, lower_limit, upper_limit) + +The implementation also includes joint damping in ``damping`` and stops the +slider when a position limit is reached. Gradients at clamp boundaries are +piecewise-defined; the example stays away from those boundaries. + +.. dropdown:: Complete provider (solver.py) + :icon: code + + .. literalinclude:: ../../../scripts/tutorials/sim/simple_newton_solver/solver.py + :language: python + :linenos: + +.. dropdown:: Complete scene and rollout (demo.py) + :icon: code + + .. literalinclude:: ../../../scripts/tutorials/sim/simple_newton_solver/demo.py + :language: python + :linenos: + +Scope and next steps +-------------------- + +This educational solver accepts independent dynamic prismatic leaves with +fixed ancestors and zero gravity. It rejects moving-body collisions, particles, +mimic constraints, friction and position/velocity target drives. Its parameters +belong to one finalized model; save learned values explicitly before rebuilding +the scene. See the tutorial directory's ``README.md`` for SDK prerequisites, +limit behavior, and notes about differentiating Cartesian losses. diff --git a/docs/source/tutorial/simulation/index.rst b/docs/source/tutorial/simulation/index.rst index 0cfedd38cc..ecd92f8f5d 100644 --- a/docs/source/tutorial/simulation/index.rst +++ b/docs/source/tutorial/simulation/index.rst @@ -18,6 +18,7 @@ pages are ordered as a progressive learning path. ../robot ../sensor ../solver + ../simple_newton_solver ../motion_gen ../trajectory_planning ../robot_workspace diff --git a/embodichain/lab/sim/cfg/physics.py b/embodichain/lab/sim/cfg/physics.py index 3975f31980..d467e7c50a 100644 --- a/embodichain/lab/sim/cfg/physics.py +++ b/embodichain/lab/sim/cfg/physics.py @@ -310,8 +310,10 @@ class NewtonPhysicsCfg(PhysicsBackendCfg): requires_grad: bool = False """Whether to finalize the Newton model with differentiable state enabled. - EmbodiChain currently requires the Semi-implicit solver for this mode and - disables CUDA graph capture when gradients are enabled. + Requires Semi-implicit or a direct/named provider supporting the requested + gradient mode. The provider validates native support during model creation; + Scene functional calls additionally require its functional dynamics service. + CUDA graph capture is disabled when gradients are enabled. """ use_cuda_graph: bool = True @@ -387,6 +389,16 @@ class NewtonPhysicsCfg(PhysicsBackendCfg): coupling requires ``joint_mode='dynamic'``. Current DexSim supports full-surface rigid-soft feedback; native validation owns the compatible contact modes and option combinations. + + Installed ``dexsim.newton_solvers`` entry-point names are also accepted by + DexSim builds with the named-extension API. Extension names are exact; + options pass to their provider. + + Python callers can pass DexSim's ``ProviderNewtonSolverCfg`` with a + ``provider_factory`` class or zero-argument factory and provider ``options``. + That configuration passes through directly without package discovery. The + SDK creates a fresh provider for each model build; pass a factory rather + than a live provider or solver instance. """ collision_cfg: NewtonCollisionPipelineCfg | Mapping[str, Any] | None = field( @@ -456,13 +468,16 @@ def to_dexsim_cfg( solver_cfg_map=solver_cfg_map, ) - if self.requires_grad and ( - solver_cfg is None or solver_cfg.solver_type != "semi_implicit" + solver_name = "dexuni" if solver_cfg is None else str(solver_cfg.solver_type) + if ( + self.requires_grad + and solver_name in solver_cfg_map + and solver_name != "semi_implicit" ): logger.log_error( "Newton gradient mode requires an explicit " - "solver_type='semi_implicit'; the default does not select a " - "differentiable solver." + "solver_type='semi_implicit' or a provider supporting gradients; " + "the default does not select a differentiable solver." ) collision_pipeline_cfg = None @@ -537,11 +552,16 @@ def _normalize_newton_solver_type(solver_type: str) -> str: "vbdsolvercfg": "vbd", } if key not in aliases: - logger.log_error( - f"Unsupported Newton solver type '{solver_type}'. " - "Expected one of 'dexuni', 'mjwarp', 'xpbd', 'semi_implicit', " - "'featherstone', or 'vbd'." - ) + try: + from dexsim.engine.newton_physics import normalize_newton_solver_name + except ImportError as exc: + raise ValueError( + f"Solver {solver_type!r} requires a DexSim build with named solver extensions" + ) from exc + try: + return normalize_newton_solver_name(solver_type) + except ValueError as exc: + logger.log_error(f"Unsupported Newton solver type {solver_type!r}. {exc}") return aliases[key] @@ -559,6 +579,11 @@ def _newton_solver_cfg_to_dexsim( "Newton solver_cfg must be a mapping or a DexSim Newton solver " "config object with a 'solver_type' attribute." ) + from dexsim.engine import newton_physics + + provider_cfg_type = getattr(newton_physics, "ProviderNewtonSolverCfg", None) + if provider_cfg_type is not None and isinstance(solver_cfg, provider_cfg_type): + return solver_cfg _normalize_newton_solver_type(str(solver_cfg.solver_type)) return solver_cfg @@ -569,6 +594,10 @@ def _newton_solver_cfg_to_dexsim( or "dexuni" ) normalized_solver_type = _normalize_newton_solver_type(str(configured_solver_type)) + if normalized_solver_type not in solver_cfg_map: + from dexsim.engine.newton_physics import RegisteredNewtonSolverCfg + + return RegisteredNewtonSolverCfg(normalized_solver_type, solver_cfg_data) solver_cfg_type = solver_cfg_map[normalized_solver_type] return solver_cfg_type(**solver_cfg_data) diff --git a/embodichain/lab/sim/objects/articulation.py b/embodichain/lab/sim/objects/articulation.py index 3b7b856ffa..fbca5f5811 100644 --- a/embodichain/lab/sim/objects/articulation.py +++ b/embodichain/lab/sim/objects/articulation.py @@ -97,6 +97,7 @@ from embodichain.lab.sim.objects.backends.newton import ( _configure_newton_mimic_compliance, _stabilize_newton_mimic_target_write, + _supports_newton_position_targets, ) from embodichain.utils.math import ( matrix_from_euler, @@ -2828,6 +2829,11 @@ def reset( num_instances = len(local_env_ids) self.cfg: ArticulationCfg + write_hold_targets = ( + not self.body_data.is_newton_backend + or _supports_newton_position_targets(self._spawn_result) + ) + self.restore_visual_material(env_ids=local_env_ids) self._restore_default_physical_properties(local_env_ids) @@ -2876,8 +2882,11 @@ def reset( # impulse when initialized away from their equality manifold. qpos = self._project_mimic_qpos(qpos) self.set_qpos(qpos, target=False, env_ids=local_env_ids) - # Set drive target to hold position. - self.set_qpos(qpos, target=True, env_ids=local_env_ids) + # Initialization uses clear_dynamics=False with no subsequent clear. + # Keep hold targets and mimic propagation for position-capable solvers; + # effort-only solvers use native reset bookkeeping without a command. + if write_hold_targets: + self.set_qpos(qpos, target=True, env_ids=local_env_ids) if clear_dynamics: self.clear_dynamics(env_ids=local_env_ids) diff --git a/embodichain/lab/sim/objects/backends/newton.py b/embodichain/lab/sim/objects/backends/newton.py index 1763ea5287..5c875ee7d0 100644 --- a/embodichain/lab/sim/objects/backends/newton.py +++ b/embodichain/lab/sim/objects/backends/newton.py @@ -34,6 +34,23 @@ def is_newton_scene(scene: object) -> bool: return isinstance(scene, Scene) and scene.backend == "newton" +def _supports_newton_position_targets(scene: Scene) -> bool: + """Query the finalized solver contract while preserving legacy SDK support.""" + query = getattr(scene, "dynamics_capabilities", None) + if query is None: + from dexsim.engine import newton_physics + + if hasattr(newton_physics, "NewtonSolverExtension"): + raise RuntimeError( + "Newton solver extensions require a compatible DexSim SDK with " + "Scene.dynamics_capabilities() for articulation initialization." + ) + # Released SDKs predating extensions accept the existing target channels. + return True + fields = query().supported_control_fields + return fields is None or "joint_target_q" in fields + + def _create_newton_standalone_state_sync( model: Any, body_ids: Sequence[int], diff --git a/scripts/tutorials/sim/simple_newton_solver/README.md b/scripts/tutorials/sim/simple_newton_solver/README.md new file mode 100644 index 0000000000..6e43cc3610 --- /dev/null +++ b/scripts/tutorials/sim/simple_newton_solver/README.md @@ -0,0 +1,158 @@ +# Tutorial: a custom Newton solver + +This directory owns the complete spring-slider example: `solver.py`, `demo.py`, +and `slider.urdf`. The provider is tutorial code; it is not part of the +`embodichain` package or its solver registry. `demo.py` passes the local +`SimpleSolverProvider` directly to DexSim. It demonstrates `SimulationManager.update()`, +independent differentiable rollout, parameter/input gradients, publication into +the live Scene, and partial functional/native reset in two environments. + +The [narrative tutorial](../../../../docs/source/tutorial/simple_newton_solver.rst) +walks through the configuration, shared transition, and expected output. + +## Prerequisite + +Use the current DexSim extension SDK with `ProviderNewtonSolverCfg`, +`FunctionalDynamics`, `Scene.dynamics_capabilities()` and explicit control, +particle and native acceleration capabilities. The released dependency pin +may lag these development APIs; selecting the provider reports an actionable +error when the SDK is insufficient. The provider can still be inspected without +activating the extension. + +The example was validated with DexSim `0.6.0+c3e92584`, Newton 1.7.0 and Warp +1.17.0 in the conda `open` environment. Run this checkout against that environment +without reinstalling its editable package or SDK: + +```bash +conda activate open +export PYTHONPATH="$PWD:$PWD/extensions${PYTHONPATH:+:$PYTHONPATH}" +python scripts/tutorials/sim/simple_newton_solver/demo.py --device cpu +python scripts/tutorials/sim/simple_newton_solver/demo.py --device cuda:0 +python scripts/tutorials/sim/simple_newton_solver/demo.py --device cpu \ + --preview-path simple_newton_solver_preview.png +python scripts/tutorials/sim/simple_newton_solver/demo.py --device cpu \ + --render-path simple_newton_offscreen.png +python scripts/tutorials/sim/simple_newton_solver/demo.py --device cpu \ + --video-path simple_newton_offscreen.mp4 +python scripts/tutorials/sim/simple_newton_solver/demo.py --device cpu \ + --viser --viser-seconds 60 +``` + +For a separate development installation, install the checkout with `--no-deps` +and install a compatible DexSim wheel with its dependencies. The direct factory +example requires no entry-point metadata refresh. + +The command runs headlessly and prints a JSON result with positions and nonzero +gradients. It checks numerical equality and state isolation before reporting +success. The two slider positions after publication are about `0.02087 m` and +`0.03131 m`; resetting the first arena leaves the second at `0.03131 m`. +Pass `--preview-path` to save a PNG with the measured functional trajectory and +the selected-arena reset. The plot is generated only when this option is used. + +Pass `--render-path` to save **actual EmbodiChain offscreen RGB**, with the two +environments as columns and initial/after-30-step views as rows. This reserves +the native FastRT renderer before World construction, then uses `CameraCfg`, +`SimulationManager.render_camera_group()` and `Camera.get_data()["color"]`. +The physics and gradient checks remain the same. Native camera rendering needs +a working GPU renderer even when Newton physics runs on the CPU. + +Pass `--video-path` to record those two offscreen cameras side by side during +the 30-step replay. The H.264 MP4 has 40 frames at 10 fps, including short +holds on the initial and final poses. This option requires `ffmpeg` with +`libx264` on `PATH` and the same native GPU renderer as `--render-path`. +Both options may be used in one run. + +Pass `--viser` to see the actual EmbodiChain scene at `http://127.0.0.1:8080`. +The script exports both slider links, replays 30 native simulation steps, and +keeps the browser scene open for 30 seconds by default. Use `--viser-seconds 60` +to leave more time to inspect the result, or `--viser-port PORT` if 8080 is busy. +The JSON includes the visible link count and final replay positions. Viser +shows the URDF cube mesh; the `base` has no visual geometry. This mode uses the +existing `VisualizationCfg` and `SimulationManager.capture_visualization()` +without requiring a native render window. + +For local verification, first require the SDK capability, then run the tutorial +on CPU or CUDA. The default package test suite does not exercise this example: + +```bash +python - <<'PY' +from dexsim.engine.newton_physics import FunctionalDynamics +from dexsim.engine.newton_physics import ProviderNewtonSolverCfg +from dexsim.scene import Scene +assert callable(getattr(Scene, "dynamics_capabilities", None)) +PY +python scripts/tutorials/sim/simple_newton_solver/demo.py --device cpu +``` + +## Dynamics and supported model + +For each independent prismatic leaf, with coordinate `q`, velocity `v`, input +force `u`, trainable scale `g`, and effective mass `m + armature`: + +```text +f = clamp(g * u, -effort_limit, effort_limit) +a = (f - stiffness * q - (damping + joint_damping) * v) / effective_mass +v_next = clamp(v + dt * a, -velocity_limit, velocity_limit) +q_next = clamp(q + dt * v_next, lower_limit, upper_limit) +``` + +A position-limit clamp stops the slider. Body acceleration is the realized +velocity change divided by `dt`. Gradients through the clamps are piecewise; +the example stays away from these boundaries. Native and functional stepping +call the same transition implementation. Body poses use the fixed joint frames +and world-space slider axis, preserving arena placement and body orientation. + +The model must have fixed ancestors and independent prismatic leaf bodies, +zero gravity, no particles, mimic constraints, friction or position/velocity +drives. Moving-body collisions are rejected. Static ground is allowed because +the moving bodies have collision disabled. The demo uses an explicit collision +overlay: URDF visual geometry can otherwise become collision geometry during +import. Unsupported explicit control targets and external body forces remain +errors. This eager solver does not support CUDA Graph capture. + +## API boundaries + +Python configuration passes the provider factory directly: + +```python +from dexsim.engine.newton_physics import ProviderNewtonSolverCfg +from embodichain.lab.sim.cfg import NewtonPhysicsCfg +from solver import SimpleSolverProvider # local to this tutorial directory + +physics = NewtonPhysicsCfg( + solver_cfg=ProviderNewtonSolverCfg( + provider_factory=SimpleSolverProvider, + options={"stiffness": 2.0}, + ), +) +``` + +Pass a class or zero-argument factory, not an already-created provider/solver. +DexSim creates a fresh provider for every model build. The direct factory path +does not scan installed entry points. To try name-based registration in a +separate project, package and register your own provider under +`dexsim.newton_solvers`; this tutorial deliberately does not register one. +`requires_grad=True` enables the functional path. After `sim.prepare()`, use +the public `sim.spawn_result` Scene: + +- `create_functional_state()` clones physical state. +- `functional_parameters()` returns the model-scoped trainable `force_scale`. +- `functional_step(..., publish=False)` computes an isolated trajectory. +- `functional_step(..., publish=True)` also publishes detached physical state + and advances the live clock once. Do not also call `sim.update()` for that + transition. The returned tensors retain their gradient graph. +- `reset_functional_state(..., arenas=["arena_0"])` resets only the selected + environment; the global `default` arena is a separate selection. + +The service is stateless: recurrent history is empty, history commit is a no-op, +and DexSim owns native buffers, clocks and lifecycle. A topology rebuild creates +a new provider; save and restore learned parameters explicitly across rebuilds. +Training loops and optimizers remain downstream. The example adds no manager or +registry of its own. + +The demonstrated gradients are with respect to effort and `force_scale`. +Functional state includes both joint and body coordinates. To differentiate a +Cartesian loss with respect to initial joint positions, initialize body poses +through a differentiable FK graph consistent with those positions. Promoting +only `joint_q` to a trainable tensor while keeping the initial `body_q` detached +instead differentiates a redundant state with its initial body pose held fixed. diff --git a/scripts/tutorials/sim/simple_newton_solver/demo.py b/scripts/tutorials/sim/simple_newton_solver/demo.py new file mode 100644 index 0000000000..d86bc8e77c --- /dev/null +++ b/scripts/tutorials/sim/simple_newton_solver/demo.py @@ -0,0 +1,399 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +"""Run ordinary and functional stepping through public EmbodiChain/Scene APIs.""" + +from __future__ import annotations + +import argparse +import json +import subprocess +import time +from pathlib import Path + +import numpy as np +import torch +from dexsim.engine.newton_physics import ProviderNewtonSolverCfg + +from embodichain.lab.sim import SimulationManager, SimulationManagerCfg +from embodichain.lab.sim.cfg import ( + ArticulationCfg, + CollisionPropertiesCfg, + NewtonPhysicsCfg, + RenderCfg, + RigidBodyPhysicsCfg, +) +from embodichain.lab.sim.sensors import Camera, CameraCfg +from embodichain.lab.visualization import VisualizationCfg, ViserServerCfg +from solver import SimpleSolverProvider + + +def _save_preview( + positions: list[torch.Tensor], reset_position: torch.Tensor, path: Path +) -> None: + """Plot the measured functional trajectory and selected-arena reset.""" + import matplotlib + + matplotlib.use("Agg") + import matplotlib.pyplot as plt + + values = torch.stack(positions).detach().cpu().numpy() + reset_values = reset_position.detach().cpu().numpy() + figure, axis = plt.subplots(figsize=(8, 4.5)) + for arena_id, color in enumerate(("#2563eb", "#ea580c")): + axis.plot( + range(len(values)), + values[:, arena_id], + marker="o", + color=color, + label=f"arena_{arena_id}", + ) + axis.plot( + (len(values) - 1, len(values)), + (values[-1, arena_id], reset_values[arena_id]), + linestyle="--", + marker="o", + color=color, + ) + axis.set_xticks(range(len(values) + 1)) + axis.set_xticklabels([*(str(step) for step in range(len(values))), "reset"]) + axis.set_xlabel("Functional transition") + axis.set_ylabel("Joint position (m)") + axis.set_title("Spring-slider tutorial: rollout and selected-arena reset") + axis.grid(alpha=0.25) + axis.legend() + figure.tight_layout() + path.parent.mkdir(parents=True, exist_ok=True) + figure.savefig(path, dpi=160) + plt.close(figure) + + +def _capture_rgb(sim: SimulationManager, camera: Camera) -> np.ndarray: + """Read the two native offscreen views through EmbodiChain's camera API.""" + sim.render_camera_group([camera.group_id]) + camera.update(fetch_only=True) + return camera.get_data()["color"][..., :3].cpu().numpy().copy() + + +def _save_offscreen_preview(before: np.ndarray, after: np.ndarray, path: Path) -> None: + """Save initial and replayed RGB frames for both environments.""" + from PIL import Image, ImageDraw, ImageFont + + height, width = before.shape[1:3] + image = Image.new("RGB", (2 * width, 2 * height)) + for row, frames in enumerate((before, after)): + for env_id in range(2): + image.paste(Image.fromarray(frames[env_id]), (env_id * width, row * height)) + draw = ImageDraw.Draw(image) + try: + font = ImageFont.truetype("DejaVuSans.ttf", 18) + except OSError: + font = ImageFont.load_default() + for row, phase in enumerate(("Initial", "After 30 steps")): + for env_id in range(2): + x, y = env_id * width, row * height + draw.rectangle((x, y, x + width - 1, y + 32), fill="white") + draw.text( + (x + 10, y + 5), + f"{phase} - environment {env_id}", + fill="black", + font=font, + ) + path.parent.mkdir(parents=True, exist_ok=True) + image.save(path) + + +def _start_video_encoder(path: Path, width: int, height: int) -> subprocess.Popen: + """Start an H.264 encoder for the two side-by-side offscreen views.""" + path.parent.mkdir(parents=True, exist_ok=True) + try: + return subprocess.Popen( + [ + "ffmpeg", + "-loglevel", + "error", + "-y", + "-f", + "rawvideo", + "-pixel_format", + "rgb24", + "-video_size", + f"{2 * width}x{height}", + "-framerate", + "10", + "-i", + "pipe:0", + "-an", + "-c:v", + "libx264", + "-pix_fmt", + "yuv420p", + str(path), + ], + stdin=subprocess.PIPE, + stderr=subprocess.PIPE, + ) + except FileNotFoundError as exc: + raise RuntimeError("--video-path requires ffmpeg on PATH") from exc + + +def _write_video_frame(encoder: subprocess.Popen, rgb: np.ndarray) -> None: + """Append one RGB frame per environment without retaining the video in RAM.""" + combined = np.ascontiguousarray(np.concatenate((rgb[0], rgb[1]), axis=1)) + encoder.stdin.write(combined.tobytes()) + + +def _finish_video_encoder(encoder: subprocess.Popen) -> None: + """Flush the encoder and report an unsuccessful conversion.""" + encoder.stdin.close() + error = encoder.stderr.read().decode(errors="replace") + if encoder.wait() != 0: + raise RuntimeError(f"ffmpeg could not encode the tutorial video: {error}") + + +def run( + device: str, + preview_path: Path | None = None, + *, + render_path: Path | None = None, + video_path: Path | None = None, + viser: bool = False, + viser_port: int = 8080, + viser_seconds: float = 30.0, +) -> dict[str, object]: + """Demonstrate direct provider creation, parity, gradients and partial reset.""" + if viser_seconds < 0: + raise ValueError("viser_seconds must be non-negative") + config = SimulationManagerCfg( + headless=True, + render_cfg=RenderCfg( + renderer=( + "fast-rt" + if render_path is not None or video_path is not None + else "auto" + ) + ), + visualization=VisualizationCfg( + backend="viser" if viser else "none", + env_ids=[0, 1], + viser_server=ViserServerCfg(port=viser_port), + ), + num_envs=2, + enable_entity_gizmo=False, + startup_summary="off", + physics_cfg=NewtonPhysicsCfg( + device=device, + gravity=(0.0, 0.0, 0.0), + physics_dt=0.02, + num_substeps=2, + requires_grad=True, + use_cuda_graph=False, + collision_cfg=None, + solver_cfg=ProviderNewtonSolverCfg( + provider_factory=SimpleSolverProvider, + options={"stiffness": 2.0, "damping": 0.2}, + ), + ), + ) + sim = SimulationManager( + config, + requires_native_renderer=render_path is not None or video_path is not None, + ) + try: + slider = sim.add_articulation( + ArticulationCfg( + uid="slider", + fpath=str(Path(__file__).with_name("slider.urdf")), + asset_physics_mode="overlay", + attrs=RigidBodyPhysicsCfg( + collision_props=CollisionPropertiesCfg(collision_enabled=False), + ), + ) + ) + sim.prepare() + camera = None + if render_path is not None or video_path is not None: + camera = sim.add_sensor( + CameraCfg( + uid="slider_preview", + width=640, + height=480, + intrinsics=(600, 600, 320, 240), + extrinsics=CameraCfg.ExtrinsicsCfg( + eye=(0.7, -0.8, 0.55), + target=(0.25, 0.0, 0.2), + up=(0.0, 0.0, 1.0), + ), + enable_color=True, + ) + ) + if viser: + manifest = sim.refresh_visualization() + if not any(node.kind == "articulation_link" for node in manifest.nodes): + raise RuntimeError("Viser scene has no slider visual geometry") + sim.capture_visualization(force=True) + scene = sim.spawn_result + initial = scene.create_functional_state() + params = scene.functional_parameters() + force = torch.tensor([4.0, 6.0], device=device, requires_grad=True) + + predicted = scene.functional_step(initial, force, publish=False) + torch.testing.assert_close( + slider.get_qpos().flatten(), initial.physical["joint_q"] + ) + slider.set_qf(force.detach().reshape(2, 1)) + sim.update(step=1) + torch.testing.assert_close( + slider.get_qpos().flatten(), predicted.physical["joint_q"] + ) + + trajectory = initial + positions = [initial.physical["joint_q"]] + for _ in range(6): + trajectory = scene.functional_step(trajectory, force, publish=False) + positions.append(trajectory.physical["joint_q"]) + saved = trajectory.physical["joint_q"].clone() + published = scene.functional_step(trajectory, force, publish=True) + positions.append(published.physical["joint_q"]) + torch.testing.assert_close( + slider.get_qpos().flatten(), published.physical["joint_q"] + ) + torch.testing.assert_close(trajectory.physical["joint_q"], saved) + + reset = scene.reset_functional_state(published, arenas=["arena_0"]) + torch.testing.assert_close( + reset.physical["joint_q"][0], initial.physical["joint_q"][0] + ) + torch.testing.assert_close( + reset.physical["joint_q"][1], published.physical["joint_q"][1] + ) + if preview_path is not None: + _save_preview(positions, reset.physical["joint_q"], preview_path) + loss = ( + trajectory.physical["joint_q"].square().sum() + + reset.physical["joint_q"].square().sum() + ) + grad_force, grad_scale = torch.autograd.grad( + loss, (force, params["force_scale"]) + ) + assert torch.isfinite(grad_force).all() and (grad_force.abs() > 0).all() + assert torch.isfinite(grad_scale) and grad_scale.abs() > 0 + slider.reset(env_ids=[0]) + torch.testing.assert_close( + slider.get_qpos()[0].flatten(), initial.physical["joint_q"][:1] + ) + result = { + "device": device, + "native_functional_parity": True, + "published_position": published.physical["joint_q"].detach().tolist(), + "partial_reset_position": reset.physical["joint_q"].detach().tolist(), + "force_gradient": grad_force.tolist(), + "force_scale_gradient": grad_scale.item(), + } + if preview_path is not None: + result["preview_path"] = str(preview_path) + if viser or render_path is not None or video_path is not None: + # Replay in the live EmbodiChain scene for Viser and/or native RGB. + # The numerical and gradient checks above remain independent. + slider.reset() + slider.set_qf(force.detach().reshape(2, 1)) + initial_rgb = _capture_rgb(sim, camera) if camera is not None else None + encoder = ( + _start_video_encoder(video_path, camera.cfg.width, camera.cfg.height) + if video_path is not None + else None + ) + final_rgb = None + try: + if encoder is not None: + for _ in range(5): + _write_video_frame(encoder, initial_rgb) + for _ in range(30): + sim.update(step=1) + if viser: + sim.capture_visualization(force=True) + time.sleep(0.05) + if encoder is not None: + final_rgb = _capture_rgb(sim, camera) + _write_video_frame(encoder, final_rgb) + if encoder is not None: + for _ in range(5): + _write_video_frame(encoder, final_rgb) + finally: + if encoder is not None: + _finish_video_encoder(encoder) + result["visualized_position"] = slider.get_qpos().flatten().tolist() + if camera is not None: + if render_path is not None: + if final_rgb is None: + final_rgb = _capture_rgb(sim, camera) + _save_offscreen_preview(initial_rgb, final_rgb, render_path) + result["render_path"] = str(render_path) + if video_path is not None: + result["video_path"] = str(video_path) + if viser: + result["viser_url"] = f"http://127.0.0.1:{viser_port}" + result["visualized_links"] = sum( + node.kind == "articulation_link" for node in manifest.nodes + ) + print(f"Viser preview: {result['viser_url']}", flush=True) + time.sleep(viser_seconds) + return result + finally: + sim.destroy(exit_process=False) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--device", default="cpu", help="cpu or cuda:0") + parser.add_argument( + "--preview-path", type=Path, help="save the rollout and reset plot as PNG" + ) + parser.add_argument( + "--render-path", + type=Path, + help="save EmbodiChain offscreen RGB before and after replay", + ) + parser.add_argument( + "--video-path", type=Path, help="record a side-by-side offscreen MP4" + ) + parser.add_argument( + "--viser", action="store_true", help="replay in EmbodiChain Viser" + ) + parser.add_argument("--viser-port", type=int, default=8080) + parser.add_argument( + "--viser-seconds", + type=float, + default=30.0, + help="seconds to keep the browser scene open after replay", + ) + args = parser.parse_args() + try: + print( + json.dumps( + run( + args.device, + args.preview_path, + render_path=args.render_path, + video_path=args.video_path, + viser=args.viser, + viser_port=args.viser_port, + viser_seconds=args.viser_seconds, + ), + indent=2, + ) + ) + finally: + SimulationManager.flush_cleanup_queue() diff --git a/scripts/tutorials/sim/simple_newton_solver/slider.urdf b/scripts/tutorials/sim/simple_newton_solver/slider.urdf new file mode 100644 index 0000000000..0d6880fd5c --- /dev/null +++ b/scripts/tutorials/sim/simple_newton_solver/slider.urdf @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/scripts/tutorials/sim/simple_newton_solver/solver.py b/scripts/tutorials/sim/simple_newton_solver/solver.py new file mode 100644 index 0000000000..11197aa93c --- /dev/null +++ b/scripts/tutorials/sim/simple_newton_solver/solver.py @@ -0,0 +1,344 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +"""Tutorial spring-slider solver demonstrating the DexSim extension contract.""" + +from __future__ import annotations + +import math +from collections.abc import Mapping +from contextlib import nullcontext +from typing import Any + +import newton +import numpy as np +import torch +import warp as wp +from newton.solvers import SolverBase +from dexsim.engine import newton_physics as _sdk + +from embodichain.utils.math import quat_apply + +__all__ = ["SimpleSolverProvider"] + +# Catalogues and autodoc may inspect the provider with the released SDK. +# Resolve annotations when available; instance creation enforces the runtime API. +try: + from dexsim.engine.newton_physics import ( + FunctionalSolverState, + NewtonSolverCapabilities, + NewtonSolverContext, + NewtonSolverExtension, + ) +except ImportError: + FunctionalSolverState = Any + NewtonSolverCapabilities = Any + NewtonSolverContext = Any + NewtonSolverExtension = Any + + +def _torch_scope(device: wp.context.Device) -> Any: + return ( + torch.cuda.stream(wp.stream_to_torch(wp.get_stream(device))) + if device.is_cuda + else nullcontext() + ) + + +def _tensor(array: wp.array) -> torch.Tensor: + # Warp cannot expose a null pointer as a Torch tensor for empty arrays. + if array.size == 0: + return torch.as_tensor(array.numpy(), device=str(array.device)) + return wp.to_torch(array) + + +class _SliderDynamics: + def __init__(self, model: newton.Model, options: Mapping[str, Any]) -> None: + unknown = set(options) - {"stiffness", "damping", "force_scale"} + if unknown: + raise ValueError(f"Unknown simple solver options: {sorted(unknown)}") + values = {"stiffness": 1.0, "damping": 0.1, "force_scale": 1.0} + values.update(options) + for name, value in values.items(): + if not math.isfinite(float(value)) or ( + name != "force_scale" and float(value) < 0 + ): + raise ValueError(f"Invalid simple solver option {name}={value!r}") + self.model = model + self.stiffness = float(values["stiffness"]) + self.damping = float(values["damping"]) + self.force_scale = torch.tensor( + float(values["force_scale"]), device=str(model.device), requires_grad=True + ) + self.refresh() + + def refresh(self) -> None: + model = self.model + types = model.joint_type.numpy() + parents, children = model.joint_parent.numpy(), model.joint_child.numpy() + moving = np.flatnonzero(types == int(newton.JointType.PRISMATIC)) + if ( + not len(moving) + or not np.isin( + types, [int(newton.JointType.FIXED), int(newton.JointType.PRISMATIC)] + ).all() + or len(children) != model.body_count + or len(set(children.tolist())) != model.body_count + or any(int(children[j]) in parents for j in moving) + or not model.joint_enabled.numpy().all() + ): + raise ValueError( + "simple supports independent prismatic leaf joints with fixed ancestors only" + ) + if model.particle_count or getattr(model, "constraint_mimic_count", 0): + raise ValueError("simple does not support particles or mimic constraints") + moving_shapes = np.isin(model.shape_body.numpy(), children[moving]) + if np.any( + model.shape_flags.numpy()[moving_shapes] + & int(newton.ShapeFlags.COLLIDE_SHAPES) + ): + raise ValueError( + "simple does not support collision shapes on moving bodies" + ) + if np.any(model.gravity.numpy()): + raise ValueError("simple requires zero gravity") + for name in ("joint_target_ke", "joint_target_kd", "joint_friction"): + if np.any(getattr(model, name).numpy()): + raise ValueError(f"simple does not support nonzero {name}") + self.children = torch.as_tensor( + children[moving], dtype=torch.long, device=str(model.device) + ) + if np.any( + model.body_flags.numpy()[children[moving]] & int(newton.BodyFlags.KINEMATIC) + ): + raise ValueError("simple requires dynamic slider bodies") + mass = _tensor(model.body_mass)[self.children].detach().clone() + if not bool(torch.isfinite(mass).all() and (mass > 0).all()): + raise ValueError("simple requires finite positive slider mass") + self.mass = mass + _tensor(model.joint_armature).detach().clone() + if not bool(torch.isfinite(self.mass).all() and (self.mass > 0).all()): + raise ValueError("simple requires finite positive effective mass") + self.drag = _tensor(model.joint_damping).detach().clone() + self.damping + self.lower = _tensor(model.joint_limit_lower).detach().clone() + self.upper = _tensor(model.joint_limit_upper).detach().clone() + self.effort_limit = _tensor(model.joint_effort_limit).detach().clone() + self.velocity_limit = _tensor(model.joint_velocity_limit).detach().clone() + child_frames = _tensor(model.joint_X_c)[ + torch.as_tensor(moving, device=str(model.device)) + ] + self.local_axes = quat_apply( + child_frames[:, 3:], _tensor(model.joint_axis) + ).detach() + + def create_state(self, native_state: newton.State) -> FunctionalSolverState: + return FunctionalSolverState( + { + name: _tensor(getattr(native_state, name)).detach().clone() + for name in ("joint_q", "joint_qd", "body_q", "body_qd", "body_qdd") + if getattr(native_state, name, None) is not None + } + ) + + def parameters(self) -> Mapping[str, torch.Tensor]: + return {"force_scale": self.force_scale} + + def step( + self, + state: FunctionalSolverState, + control: torch.Tensor, + parameters: Mapping[str, torch.Tensor], + dt: float, + ) -> FunctionalSolverState: + q, qd = state.physical["joint_q"], state.physical["joint_qd"] + if not isinstance(control, torch.Tensor) or control.shape != q.shape: + raise ValueError( + "simple control must be a tensor shaped (joint_dof_count,)" + ) + if control.device != q.device or control.dtype != q.dtype: + raise ValueError("simple control must match the state device and dtype") + if not math.isfinite(dt) or dt <= 0: + raise ValueError("simple dt must be positive and finite") + force = (parameters["force_scale"] * control).clamp( + -self.effort_limit, self.effort_limit + ) + acceleration = (force - self.stiffness * q - self.drag * qd) / self.mass + velocity = (qd + dt * acceleration).clamp( + -self.velocity_limit, self.velocity_limit + ) + proposed = q + dt * velocity + position = proposed.clamp(self.lower, self.upper) + velocity = torch.where( + proposed != position, torch.zeros_like(velocity), velocity + ) + acceleration = (velocity - qd) / dt + # Advance the existing pose by the coordinate delta so explicit root + # placement and randomized states are preserved across transitions. + previous_pose = state.physical["body_q"] + axes = quat_apply(previous_pose[self.children, 3:], self.local_axes) + pose = previous_pose.index_copy( + 0, + self.children, + torch.cat( + ( + previous_pose[self.children, :3] + axes * (position - q)[:, None], + previous_pose[self.children, 3:], + ), + dim=1, + ), + ) + spatial = torch.zeros( + (self.model.body_count, 6), device=q.device, dtype=q.dtype + ) + zeros = torch.zeros_like(axes) + body_qd = spatial.index_copy( + 0, self.children, torch.cat((axes * velocity[:, None], zeros), dim=1) + ) + body_qdd = spatial.index_copy( + 0, + self.children, + torch.cat((axes * acceleration[:, None], zeros), dim=1), + ) + return FunctionalSolverState( + { + "joint_q": position, + "joint_qd": velocity, + "body_q": pose, + "body_qd": body_qd, + "body_qdd": body_qdd, + } + ) + + def commit(self, state: FunctionalSolverState) -> None: + # There is no recurrent history. DexSim owns physical publication. + pass + + def reset_history( + self, + state: FunctionalSolverState, + reset_state: FunctionalSolverState, + world_mask: wp.array | None, + ) -> tuple[Mapping[str, Any], None]: + return {}, None + + +class _NativeSliderSolver(SolverBase): + def __init__(self, model: newton.Model, dynamics: _SliderDynamics) -> None: + super().__init__(model) + self.dynamics = dynamics + + def step( + self, + state_in: newton.State, + state_out: newton.State, + control: newton.Control | None, + contacts: newton.Contacts | None, + dt: float, + *, + observables: SolverBase.Observables | None = None, + ) -> None: + self.validate_observables(observables, contacts) + with _torch_scope(self.model.device), torch.no_grad(): + source = self.dynamics.create_state(state_in) + force = _tensor(self.model.joint_f if control is None else control.joint_f) + result = self.dynamics.step(source, force, self.dynamics.parameters(), dt) + for name, value in result.physical.items(): + destination = getattr(state_out, name, None) + if destination is not None: + _tensor(destination).copy_(value) + + def notify_model_changed(self, flags: newton.ModelFlags | int) -> None: + with _torch_scope(self.model.device): + self.dynamics.refresh() + + +class SimpleSolverProvider: + """Create a minimal, differentiable spring-slider Newton extension. + + Each moving body must be a dynamic prismatic leaf attached to the world or + fixed ancestors. Visual shapes and static ground are allowed; moving-body + collisions, particles, mimic + constraints, gravity, target drives and friction are rejected. Joint mass, + armature, damping and effort/velocity/position limits are respected. Limits + use hard clamping; gradients at their boundaries are piecewise-defined. + + Options are ``stiffness`` (N/m, default 1), ``damping`` (N s/m, default 0.1) + and trainable ``force_scale`` (default 1). Native and functional execution + share semi-implicit Euler: first update velocity, then position. This is an + eager educational solver, not a general robot/contact solver. Parameters + belong to one finalized model and must be checkpointed across rebuilds. + """ + + def __init__(self) -> None: + """Require the extension SDK when the provider is selected. + + Raises: + ImportError: The installed SDK lacks the functional service contract. + """ + if ( + not hasattr(_sdk, "FunctionalDynamics") + or not hasattr(NewtonSolverCapabilities, "native_body_accelerations") + or not hasattr(NewtonSolverCapabilities, "supported_particle_kinds") + ): + raise ImportError( + "The simple solver requires DexSim's FunctionalDynamics extension API " + "with explicit control, particle and acceleration capabilities; " + "install a compatible DexSim build." + ) + self.capabilities = NewtonSolverCapabilities( + reset=True, + cuda_graph=False, + external_wrenches=False, + supported_control_fields=("joint_f",), + runtime_controls=False, + native_body_accelerations=True, + supported_particle_kinds=(), + ) + + def register_custom_attributes( + self, + builder: newton.ModelBuilder, + options: Mapping[str, Any], + ) -> None: + """Use standard Newton attributes; no custom registration is needed. + + Args: + builder: Builder owned by DexSim before model finalization. + options: Solver options validated during creation. + """ + + def create( + self, + context: NewtonSolverContext, + options: Mapping[str, Any], + ) -> NewtonSolverExtension: + """Validate the model and create native and functional views of one solver. + + Args: + context: Finalized model and resources borrowed from DexSim. + options: Spring stiffness, damping and initial force scale. + + Returns: + Model-scoped extension with an independent functional trajectory API. + + Raises: + ValueError: The model or options exceed the documented supported scope. + """ + with _torch_scope(context.device): + dynamics = _SliderDynamics(context.model, options) + return NewtonSolverExtension( + native_solver=_NativeSliderSolver(context.model, dynamics), + capabilities=self.capabilities, + functional=dynamics, + ) diff --git a/tests/sim/objects/test_articulation.py b/tests/sim/objects/test_articulation.py index ae573e9342..597288232a 100644 --- a/tests/sim/objects/test_articulation.py +++ b/tests/sim/objects/test_articulation.py @@ -65,6 +65,53 @@ def _inertia_tensor(inertia: torch.Tensor, com_pose: torch.Tensor) -> torch.Tens return rotation @ torch.diag_embed(inertia) @ rotation.transpose(-1, -2) +@pytest.mark.requires_sim +def test_newton_initial_hold_targets_for_multiple_articulations(tmp_path: Path) -> None: + from embodichain.lab.sim.cfg import NewtonPhysicsCfg + + asset = tmp_path / "initial_slider.urdf" + asset.write_text(""" + + + + """) + sim = SimulationManager( + SimulationManagerCfg( + headless=True, + enable_entity_gizmo=False, + physics_cfg=NewtonPhysicsCfg( + device="cpu", + gravity=(0, 0, 0), + collision_cfg=None, + solver_cfg={"solver_type": "semi_implicit"}, + ), + ), + requires_native_renderer=False, + ) + articulations = [] + art = None + try: + initial_positions = (0.2, -0.3) + for index, position in enumerate(initial_positions): + articulations.append( + sim.add_articulation( + ArticulationCfg( + uid=f"slider_{index}", fpath=str(asset), init_qpos=[position] + ) + ) + ) + sim.prepare() + for art, position in zip(articulations, initial_positions, strict=True): + expected = torch.tensor([[position]]) + torch.testing.assert_close(art.get_qpos(), expected) + torch.testing.assert_close(art.get_qpos(target=True), expected) + finally: + art = None + articulations.clear() + sim.destroy(exit_process=False) + SimulationManager.flush_cleanup_queue() + + @pytest.mark.parametrize("device", ["cpu", "cuda:0"]) @pytest.mark.parametrize("env_ids", [None, [0]], ids=["all", "selected"]) @pytest.mark.parametrize("operation", ["reset", "root_pose", "qpos"]) diff --git a/tests/sim/objects/test_backend_physics_helpers.py b/tests/sim/objects/test_backend_physics_helpers.py index b5e28e2df8..7a2140a176 100644 --- a/tests/sim/objects/test_backend_physics_helpers.py +++ b/tests/sim/objects/test_backend_physics_helpers.py @@ -49,6 +49,7 @@ ) from embodichain.lab.sim.objects.backends.newton import ( _stabilize_newton_mimic_target_write, + _supports_newton_position_targets, ) from embodichain.lab.sim.objects.backends.lifecycle import ( apply_rigid_initial_state, @@ -66,6 +67,29 @@ ) +@pytest.mark.parametrize( + "fields, expected", + [(None, True), (("joint_f",), False), (("joint_target_q",), True)], +) +def test_newton_hold_targets_follow_declared_channels(fields, expected) -> None: + scene = SimpleNamespace( + dynamics_capabilities=lambda: SimpleNamespace(supported_control_fields=fields) + ) + assert _supports_newton_position_targets(scene) is expected + + +def test_newton_hold_targets_distinguish_legacy_and_incomplete_sdks( + monkeypatch, +) -> None: + from dexsim.engine import newton_physics + + monkeypatch.delattr(newton_physics, "NewtonSolverExtension", raising=False) + assert _supports_newton_position_targets(SimpleNamespace()) + monkeypatch.setattr(newton_physics, "NewtonSolverExtension", object, raising=False) + with pytest.raises(RuntimeError, match="Scene.dynamics_capabilities"): + _supports_newton_position_targets(SimpleNamespace()) + + class _RigidEntity: def __init__(self, handle: int = 7) -> None: self.handle = handle diff --git a/tests/sim/test_sim_manager_cfg.py b/tests/sim/test_sim_manager_cfg.py index 4e30acea55..d1435aeb3b 100644 --- a/tests/sim/test_sim_manager_cfg.py +++ b/tests/sim/test_sim_manager_cfg.py @@ -175,8 +175,9 @@ def test_newton_physics_cfg_requires_dexsim_dexuni_api( @pytest.mark.no_sim -def test_newton_gradient_mode_rejects_default_solver() -> None: - cfg = NewtonPhysicsCfg(solver_cfg=None, requires_grad=True) +@pytest.mark.parametrize("solver_cfg", [None, {}]) +def test_newton_gradient_mode_rejects_default_solver(solver_cfg) -> None: + cfg = NewtonPhysicsCfg(solver_cfg=solver_cfg, requires_grad=True) with pytest.raises(RuntimeError, match="explicit.*semi_implicit"): cfg.to_dexsim_cfg(gpu_id=0) @@ -475,6 +476,8 @@ def test_newton_physics_cfg_preserves_automatic_dexuni_defaults(solver_mapping) @pytest.mark.parametrize( "solver_mapping", [ + {"solver_type": "auto"}, + {"class_type": "AutoSolverCfg"}, {"solver_type": "unknown_solver"}, {"class_type": "UnknownSolverCfg"}, ], @@ -539,6 +542,32 @@ def test_newton_physics_cfg_directly_accepts_dexsim_solver_cfg_object() -> None: assert dexsim_cfg.solver_cfg.enable_restitution is True +@pytest.mark.no_sim +def test_newton_physics_cfg_direct_provider_skips_name_discovery( + monkeypatch: pytest.MonkeyPatch, +) -> None: + from dexsim.engine import newton_physics + + provider_cfg_type = getattr(newton_physics, "ProviderNewtonSolverCfg", None) + if provider_cfg_type is None: + pytest.skip("DexSim direct-provider configuration is unavailable") + + def unexpected_discovery() -> None: + pytest.fail("A direct provider must not scan installed entry points") + + monkeypatch.setattr( + "dexsim.engine.newton_physics.solver_extensions._entry_points", + unexpected_discovery, + ) + provider_cfg = provider_cfg_type(provider_factory=lambda: object()) + + native = NewtonPhysicsCfg(solver_cfg=provider_cfg).to_dexsim_cfg(gpu_id=0) + + assert isinstance(native.solver_cfg, provider_cfg_type) + assert native.solver_cfg.provider_factory is provider_cfg.provider_factory + assert native.solver_cfg.options == provider_cfg.options + + @pytest.mark.no_sim def test_newton_physics_cfg_forwards_two_way_coupling() -> None: cfg = NewtonPhysicsCfg(