From f7fc59264fe8db6751725ea6a2830c7fe8aff3a1 Mon Sep 17 00:00:00 2001 From: acrlw <13927622+acrlw@users.noreply.github.com> Date: Wed, 21 Oct 2026 17:26:22 +0800 Subject: [PATCH 01/14] perf(rl): reduce rollout bookkeeping and batch physics updates --- .../topics/env-framework/execution.md | 8 + .../topics/sensor-system/contact-history.md | 5 + .../topics/simulation-system/lifecycle.md | 15 ++ docs/source/overview/rl/locomotion.md | 18 ++ embodichain/lab/gym/envs/embodied_env.py | 41 +++-- .../lab/gym/envs/managers/action_manager.py | 41 ++++- embodichain/lab/sim/_joint_motor.py | 155 ++++++++++++++++ embodichain/lab/sim/cfg/rigid.py | 7 + embodichain/lab/sim/objects/articulation.py | 33 +++- embodichain/lab/sim/objects/backends/base.py | 27 +++ embodichain/lab/sim/objects/backends/scene.py | 45 +++++ embodichain/lab/sim/sensors/_warp/contact.py | 40 +++++ embodichain/lab/sim/sensors/contact_sensor.py | 40 ++++- embodichain/lab/sim/sim_manager.py | 59 ++++++ .../gym/envs/managers/test_dataset_manager.py | 55 ++++++ tests/gym/envs/test_demo.py | 43 +++++ tests/gym/envs/test_recording_provenance.py | 25 +++ .../test_articulation_inertia_frames.py | 18 +- tests/sim/objects/test_scene_backend.py | 51 ++++++ .../sim/sensors/test_contact_query_sensor.py | 26 +++ tests/sim/test_joint_motor.py | 170 ++++++++++++++++++ tests/sim/test_sim_manager.py | 25 +++ 22 files changed, 921 insertions(+), 26 deletions(-) create mode 100644 embodichain/lab/sim/_joint_motor.py create mode 100644 tests/sim/test_joint_motor.py diff --git a/agent_context/topics/env-framework/execution.md b/agent_context/topics/env-framework/execution.md index c6413be2d5..4ffd297a35 100644 --- a/agent_context/topics/env-framework/execution.md +++ b/agent_context/topics/env-framework/execution.md @@ -326,3 +326,11 @@ All normal output selections also intersect context eligibility. Explicit and are never dataset commits. Closing only drains committed writes and discards live buffers. Camera recordings use independent row buffers and files, with owned context sidecars; asynchronous jobs do not wait for another row or a mosaic. + +## Recording cost during RL rollouts + +`EmbodiedEnv` allocates raw/controller action histories on the first recording +sample. Demo provenance is created when recording metadata is requested; +episode initialization refreshes it once it has been used. Ordinary RL resets +do not enumerate empty recording histories on the host. Metadata snapshots +retain deep-copy and episode-context ownership. diff --git a/agent_context/topics/sensor-system/contact-history.md b/agent_context/topics/sensor-system/contact-history.md index 91049ba8d6..fc71305250 100644 --- a/agent_context/topics/sensor-system/contact-history.md +++ b/agent_context/topics/sensor-system/contact-history.md @@ -23,3 +23,8 @@ across the control interval. Query counts stay on the device until this diagnost is read. A standalone sensor without histories reports the latest update. See `tests/sim/sensors/test_contact_history.py`, `test_contact_query_sensor.py` and `tests/gym/envs/test_substep_sensors.py`. + +Normal-force-only consumers can use `ContactSensor._reduce_normal_impulses()` +to reduce packed contact impulses directly into a task-owned snapshot. It +retains dropped-contact accounting and does not replace the row-buffer/history +path used by consumers requiring per-contact information. diff --git a/agent_context/topics/simulation-system/lifecycle.md b/agent_context/topics/simulation-system/lifecycle.md index e696ada674..7374b649ba 100644 --- a/agent_context/topics/simulation-system/lifecycle.md +++ b/agent_context/topics/simulation-system/lifecycle.md @@ -105,3 +105,18 @@ camera attachment, replacement, control registration, reset and teardown. `tests/sim/spawn/test_scene.py` covers declaration/source/binding state; `tests/sim/test_runtime_controls.py` covers nodal runtime behavior. Physical baseline tests live with the corresponding objects under `tests/sim/objects/`. + +## Batched RL controls and property writes + +`_register_joint_dc_motor_control()` installs a graph-compatible motor before +scene preparation, resolving public joint names in Arena order. Effort is held +through solver substeps and refreshed at the next outer physics step. +`configure_ground_plane_collision()` sets ground geometry and Newton collision +options before preparation; calls after preparation fail. + +Spawn-bound Newton articulations forward unique selected mass, principal +inertia and COM-pose writes through Scene batches. Public COM poses remain +XYZ + XYZW; the adapter converts the batch pose convention. Duplicate selections +retain ordered per-entity behavior. Matching DexSim selected-link batch methods +are required. `preserve_mesh_transform` retains authored collision mesh pose +and scale when explicitly selected in Newton collision configuration. diff --git a/docs/source/overview/rl/locomotion.md b/docs/source/overview/rl/locomotion.md index e2cd5664b0..0d328ac7c4 100644 --- a/docs/source/overview/rl/locomotion.md +++ b/docs/source/overview/rl/locomotion.md @@ -57,3 +57,21 @@ embodichain train-rl --config embodichain_tasks/configs/tasks/locomotion/velocit Local smoke checks cover environment construction, stepping, finite tensors, selective reset and policy forward execution, with zero training updates. See the [task API reference](../../api_reference/locomotion_tasks.rst) for configuration and MDP types. + +## Batched simulation updates + +RL rollouts allocate demonstration action histories only when recording starts. +Requesting demonstration metadata still produces an owned snapshot with episode +provenance; recording and replay retain their existing interfaces. + +For spawn-bound Newton articulations, `set_mass`, `set_inertia`, and +`set_com_pose` use selected-row batch writes when row/link selections are +unique. Inertia values remain principal moments in the applied COM frame; COM +poses use XYZ followed by XYZW. The paired DexSim version must expose selected +link writes. Duplicate selections preserve ordered per-entity updates. + +Before scene preparation, `SimulationManager.configure_ground_plane_collision` +can replace the global ground geometry and collision descriptor. Newton +collision configuration also accepts `preserve_mesh_transform` to retain the +authored collision mesh pose and scale. These options do not change the default +settings of existing tasks. diff --git a/embodichain/lab/gym/envs/embodied_env.py b/embodichain/lab/gym/envs/embodied_env.py index 940a22b861..68349e51d2 100644 --- a/embodichain/lab/gym/envs/embodied_env.py +++ b/embodichain/lab/gym/envs/embodied_env.py @@ -477,7 +477,7 @@ def __init__( self._record_raw_actions = self._explicit_policy_rollout self._last_action_manager_qpos: torch.Tensor | None = None self._last_action_trace: ActionTrace | None = None - self._expert_controller_qpos_history: list[list[torch.Tensor]] = [] + self._expert_controller_qpos_history: list[list[torch.Tensor]] | None = None super().__init__(cfg, **kwargs) @@ -549,10 +549,10 @@ def __init__( self._traj_steps: torch.Tensor | None = None self._traj_raw_action: EnvAction | None = None self._trajectory_action_kind: str | None = None - self._raw_action_history: list[list[EnvAction]] = [ - [] for _ in range(self.num_envs) - ] - self._expert_controller_qpos_history = [[] for _ in range(self.num_envs)] + # Recording methods allocate row histories on their first sample. + # Training resets then avoid downloading IDs to clear empty lists. + self._raw_action_history: list[list[EnvAction]] | None = None + self._expert_controller_qpos_history = None self._traj_save_count = 0 self._traj_run_id = datetime.now().strftime("%Y%m%d_%H%M%S") if self.cfg.record_trajectory: @@ -597,9 +597,9 @@ def __init__( self._demo_segment_participants = self._demo_active_mask.clone() self._demo_active_segment_start_steps = self._demo_steps.clone() self._demo_active_rollout_start_steps = self.rollout_steps.clone() + self._demo_metadata_initialized = False self._demo_episode_metadata: list[dict[str, Any]] = [ - self._new_demo_episode_metadata(env_id) - for env_id in range(self.num_envs) + {} for _ in range(self.num_envs) ] self.episode_success_status: torch.Tensor = torch.zeros( @@ -1439,12 +1439,12 @@ def _before_episode_reset( active_segment_ids = getattr(self, "_demo_active_segment_ids", None) if active_segment_ids is not None: demo_ids = env_ids_to_process.to(active_segment_ids.device) - active_segment_ids[demo_ids] = 0 - self._demo_active_mask[demo_ids] = True - self._demo_segment_participants[demo_ids] = False - self._demo_active_segment_start_steps[demo_ids] = 0 - self._demo_active_rollout_start_steps[demo_ids] = 0 - self._demo_steps[demo_ids] = 0 + active_segment_ids.index_fill_(0, demo_ids, 0) + self._demo_active_mask.index_fill_(0, demo_ids, True) + self._demo_segment_participants.index_fill_(0, demo_ids, False) + self._demo_active_segment_start_steps.index_fill_(0, demo_ids, 0) + self._demo_active_rollout_start_steps.index_fill_(0, demo_ids, 0) + self._demo_steps.index_fill_(0, demo_ids, 0) self.episode_success_status.index_fill_(0, env_ids_to_process, False) @@ -1453,7 +1453,9 @@ def _initialize_episode( ) -> None: env_ids_to_process = self.episode_context._ids(env_ids) episode_metadata = getattr(self, "_demo_episode_metadata", None) - if episode_metadata is not None: + if episode_metadata is not None and getattr( + self, "_demo_metadata_initialized", True + ): for env_id in env_ids_to_process.cpu().tolist(): episode_metadata[env_id] = self._new_demo_episode_metadata(env_id) # Stateful managers reset selected rows before reset-mode events run. @@ -1612,6 +1614,7 @@ def _begin_demo_episode_recording( self._demo_segment_participants.fill_(False) self._demo_active_segment_start_steps = self._demo_steps.clone() self._demo_active_rollout_start_steps = self.rollout_steps.clone() + self._demo_metadata_initialized = True self._demo_episode_metadata = [ self._new_demo_episode_metadata(env_id) for env_id in range(self.num_envs) ] @@ -1794,6 +1797,11 @@ def get_demo_episode_metadata(self, env_id: int) -> dict[str, Any]: Returns: A JSON-compatible metadata dictionary. """ + if "episode_uuid" not in self._demo_episode_metadata[env_id]: + metadata = EmbodiedEnv._new_demo_episode_metadata(self, env_id) + metadata.update(self._demo_episode_metadata[env_id]) + self._demo_episode_metadata[env_id] = metadata + self._demo_metadata_initialized = True metadata = copy.deepcopy(self._demo_episode_metadata[env_id]) metadata["episode_context"] = self.episode_context.snapshot([env_id])[0] metadata["segments"] = [ @@ -2674,7 +2682,10 @@ def _preprocess_action( if callable(processed_qpos) else None ) - else: + elif self.dataset_manager is not None or ( + getattr(self, "rollout_buffer", None) is not None + and getattr(self, "_rollout_buffer_mode", None) != "rl" + ): processed_qpos = getattr(self.action_manager, "_processed_qpos", None) try: self._last_action_manager_qpos = ( diff --git a/embodichain/lab/gym/envs/managers/action_manager.py b/embodichain/lab/gym/envs/managers/action_manager.py index f0645529b3..9726e8b782 100644 --- a/embodichain/lab/gym/envs/managers/action_manager.py +++ b/embodichain/lab/gym/envs/managers/action_manager.py @@ -22,7 +22,7 @@ from dataclasses import replace from functools import cached_property import inspect -from typing import TYPE_CHECKING, ClassVar +from typing import TYPE_CHECKING, ClassVar, Sequence import gymnasium as gym import numpy as np @@ -152,6 +152,9 @@ def __init__(self, cfg: object, env: EmbodiedEnv): device=env.device, ) self._previous_action = torch.zeros_like(self._action) + self._snapshot_joint_indices: dict[ + tuple[str, str], tuple[tuple[int, ...], torch.Tensor] + ] = {} def __str__(self) -> str: """Return a table describing the ordered flat action layout.""" @@ -295,15 +298,33 @@ def _processed_qpos(self) -> torch.Tensor: in active-joint order without exposing the removed pre/post protocol. """ qpos = self._env.robot.get_qpos().clone() - for term in self._terms.values(): + for name, term in self._terms.items(): if term.command_type != "qpos": raise ValueError( "Default joint-position dataset recording requires every " "action term to produce qpos commands; declare a policy " "action_contract for non-qpos actions." ) - qpos[:, list(term.controlled_joint_ids)] = term.processed_actions - return qpos[:, self._env.active_joint_ids] + columns = self._snapshot_columns(("term", name), term.controlled_joint_ids) + qpos[:, columns] = term.processed_actions + columns = self._snapshot_columns(("active", ""), self._env.active_joint_ids) + return qpos[:, columns] + + def _snapshot_columns( + self, key: tuple[str, str], values: Sequence[int] | torch.Tensor + ) -> torch.Tensor: + """Reuse device selectors while retaining live host selection changes.""" + if isinstance(values, torch.Tensor): + return values.to(device=self._action.device, dtype=torch.long) + indices = tuple(int(index) for index in values) + cached = self._snapshot_joint_indices.get(key) + if cached is None or cached[0] != indices: + cached = ( + indices, + torch.tensor(indices, device=self._action.device, dtype=torch.long), + ) + self._snapshot_joint_indices[key] = cached + return cached[1] def mask_inactive(self, active_mask: torch.Tensor) -> None: """Replace inactive vector rows with resource-safe commands. @@ -368,9 +389,15 @@ def reset( Returns: Empty diagnostic mapping. """ - ids = slice(None) if env_ids is None else env_ids - self._action[ids] = 0 - self._previous_action[ids] = 0 + if env_ids is None: + self._action.zero_() + self._previous_action.zero_() + else: + ids = torch.as_tensor(env_ids, dtype=torch.long).to( + device=self._action.device, non_blocking=True + ) + self._action.index_fill_(0, ids, 0) + self._previous_action.index_fill_(0, ids, 0) for term in self._terms.values(): term.reset(env_ids=env_ids) return {} diff --git a/embodichain/lab/sim/_joint_motor.py b/embodichain/lab/sim/_joint_motor.py new file mode 100644 index 0000000000..09dcc288c2 --- /dev/null +++ b/embodichain/lab/sim/_joint_motor.py @@ -0,0 +1,155 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- +"""Manager-owned explicit joint motor using the Newton control lifecycle.""" + +from __future__ import annotations + +from typing import Any + +import numpy as np +import warp as wp + +from dexsim.engine.newton_physics.runtime_control import ( + GraphRuntimeControl, + RuntimeControlContext, +) + +__all__: list[str] = [] + +wp.set_module_options({"enable_fp_fusion": False, "enable_backward": False}) + + +@wp.kernel(enable_backward=False) +def _dc_motor( + joint_q: wp.array(dtype=wp.float32), + joint_qd: wp.array(dtype=wp.float32), + q_indices: wp.array2d(dtype=wp.int32), + qd_indices: wp.array2d(dtype=wp.int32), + targets: wp.array2d(dtype=wp.float32), + kp: wp.array(dtype=wp.float32), + kd: wp.array(dtype=wp.float32), + limits: wp.array(dtype=wp.float32), + speeds: wp.array(dtype=wp.float32), + model_force: wp.array(dtype=wp.float32), + control_force: wp.array(dtype=wp.float32), + effort: wp.array2d(dtype=wp.float32), + previous_velocity: wp.array2d(dtype=wp.float32), +): + row, column = wp.tid() + dof = qd_indices[row, column] + velocity = joint_qd[dof] + desired = ( + kp[column] * (targets[row, column] - joint_q[q_indices[row, column]]) + - kd[column] * velocity + ) + speed = wp.clamp(velocity, -2.0 * speeds[column], 2.0 * speeds[column]) + maximum = wp.min(limits[column] * (1.0 - speed / speeds[column]), limits[column]) + minimum = wp.max(limits[column] * (-1.0 - speed / speeds[column]), -limits[column]) + value = wp.clamp(desired, minimum, maximum) + if wp.isnan(desired) or wp.isnan(velocity): + value = desired + velocity + model_force[dof] = value + control_force[dof] = value + effort[row, column] = value + previous_velocity[row, column] = velocity + + +class _BatchedJointDCMotorControl(GraphRuntimeControl): + """Hold DC motor effort over each existing outer physics step.""" + + def __init__(self, robot: dict[str, Any], target_paths: tuple[str, ...]) -> None: + self.robot = robot + self.target_paths = target_paths + self.num_envs = len(target_paths) + self.targets = self.effort = self.velocity = None + self._claims = () + + def initialize(self, context: RuntimeControlContext) -> None: + """Resolve scalar joints once in the manager-provided arena order.""" + names = self.robot["joint_names"] + q, qd, claims = [], [], [] + for path in self.target_paths: + articulation = context.result.get_articulation(path)._physics_binding + joints = {joint.name: joint for joint in articulation.public_joints} + selected = [joints[name] for name in names] + if any(joint.q_size != 1 or joint.qd_size != 1 for joint in selected): + raise ValueError("DC motor control requires scalar joints") + q.append([joint.q_start for joint in selected]) + qd.append([joint.qd_start for joint in selected]) + claims.extend(("newton_joint", int(joint.joint_id)) for joint in selected) + if len(q) != self.num_envs: + raise ValueError("Runtime control rows differ from the policy batch") + self.q_indices = wp.array(q, dtype=wp.int32, device=context.device) + self.qd_indices = wp.array(qd, dtype=wp.int32, device=context.device) + self.kp, self.kd, self.limits, self.speeds = ( + wp.array(self.robot[name], dtype=wp.float32, device=context.device) + for name in ("stiffness", "damping", "effort_limit", "velocity_limit") + ) + self.targets = wp.array( + np.tile(self.robot["default_joint_position"], (self.num_envs, 1)).astype( + np.float32 + ), + device=context.device, + ) + self.effort = wp.zeros_like(self.targets) + self.velocity = wp.zeros_like(self.targets) + self._claims = tuple(claims) + + def exclusive_resource_claims(self) -> tuple[object, ...]: + """Reserve the driven joints through the existing control registry.""" + return self._claims + + def __call__( + self, + context: RuntimeControlContext, + substep_index: int, + substep_count: int, + substep_dt: float, + ) -> None: + """Update once before the first solver substep of an outer step.""" + if substep_index: + return + control_force = context.control.joint_f + model_force = getattr(context.model, "joint_f", None) + if model_force is None: + model_force = control_force + wp.launch( + _dc_motor, + dim=self.targets.shape, + inputs=[ + context.current_state.joint_q, + context.current_state.joint_qd, + self.q_indices, + self.qd_indices, + self.targets, + self.kp, + self.kd, + self.limits, + self.speeds, + model_force, + control_force, + self.effort, + self.velocity, + ], + device=context.device, + ) + + def close(self) -> None: + """Release scene-owned device buffers before teardown or rebuilding.""" + self.q_indices = self.qd_indices = None + self.kp = self.kd = self.limits = self.speeds = None + self.targets = self.effort = self.velocity = None + self._claims = () diff --git a/embodichain/lab/sim/cfg/rigid.py b/embodichain/lab/sim/cfg/rigid.py index 2d32f5b4af..f662e0bf70 100644 --- a/embodichain/lab/sim/cfg/rigid.py +++ b/embodichain/lab/sim/cfg/rigid.py @@ -273,6 +273,13 @@ class NewtonCollisionPropertiesCfg(CollisionPropertiesCfg): ``margin + gap``; increasing the gap detects approaching contact earlier. """ + preserve_mesh_transform: bool | None = None + """Keep authored mesh pose and scale separate from collision vertices. + + ``None`` preserves the source setting. Set before scene preparation when + reproducing a reference solver model built from authored USD meshes. + """ + @configclass class RigidBodyMaterialCfg: diff --git a/embodichain/lab/sim/objects/articulation.py b/embodichain/lab/sim/objects/articulation.py index dc735e71d0..3afb498ada 100644 --- a/embodichain/lab/sim/objects/articulation.py +++ b/embodichain/lab/sim/objects/articulation.py @@ -2086,7 +2086,7 @@ def set_mass( """ env_index = self._resolve_env_ids(env_ids) env_list = env_index.detach().cpu().tolist() - names, _ = self._resolve_link_names(link_names) + names, link_index = self._resolve_link_names(link_names) mass = torch.as_tensor(mass, dtype=torch.float32, device=self.device) expected_shape = (len(env_list), len(names)) if tuple(mass.shape) != expected_shape: @@ -2094,6 +2094,15 @@ def set_mass( f"Expected mass shape {expected_shape}, got {tuple(mass.shape)}." ) + if ( + self.is_spawn_bound + and self._data.is_newton_backend + and len(set(env_list)) == len(env_list) + and len(set(names)) == len(names) + ): + self._data.articulation_view.apply_link_mass(mass, env_index, link_index) + return + for i, env_idx in enumerate(env_list): entity = self._entities[env_idx] for j, name in enumerate(names): @@ -2144,6 +2153,17 @@ def set_inertia( f"got {tuple(inertia.shape)}." ) + if ( + self.is_spawn_bound + and self._data.is_newton_backend + and len(set(env_list)) == len(env_list) + and len(set(names)) == len(names) + ): + self._data.articulation_view.apply_link_inertia( + inertia, env_index, link_index + ) + return + frames = ( self.body_data.com_pose[env_index[:, None], link_index[None, :], 3:7] .detach() @@ -2202,6 +2222,17 @@ def set_com_pose( f"got {tuple(com_pose.shape)}." ) + if ( + self.is_spawn_bound + and self._data.is_newton_backend + and len(set(env_list)) == len(env_list) + and len(set(names)) == len(names) + ): + self._data.articulation_view.apply_link_com_pose( + com_pose, env_index, link_index + ) + return + inertia = ( self.body_data.inertia[env_index[:, None], link_index[None, :]] if self.is_spawn_bound diff --git a/embodichain/lab/sim/objects/backends/base.py b/embodichain/lab/sim/objects/backends/base.py index d7fc258def..25278935bb 100644 --- a/embodichain/lab/sim/objects/backends/base.py +++ b/embodichain/lab/sim/objects/backends/base.py @@ -291,6 +291,33 @@ def select_articulation_ids( """Return backend articulation ids for the given environment ids.""" ... + def apply_link_mass( + self, + values: torch.Tensor, + env_ids: Sequence[int] | torch.Tensor, + link_ids: Sequence[int] | torch.Tensor, + ) -> None: + """Write masses for selected rows and public link columns.""" + raise NotImplementedError("This view does not expose batch link writes.") + + def apply_link_inertia( + self, + values: torch.Tensor, + env_ids: Sequence[int] | torch.Tensor, + link_ids: Sequence[int] | torch.Tensor, + ) -> None: + """Write principal moments in the current applied COM frames.""" + raise NotImplementedError("This view does not expose batch link writes.") + + def apply_link_com_pose( + self, + values: torch.Tensor, + env_ids: Sequence[int] | torch.Tensor, + link_ids: Sequence[int] | torch.Tensor, + ) -> None: + """Write selected COM poses in xyz + xyzw order.""" + raise NotImplementedError("This view does not expose batch link writes.") + def fetch_link_physical_properties( self, mass: torch.Tensor, diff --git a/embodichain/lab/sim/objects/backends/scene.py b/embodichain/lab/sim/objects/backends/scene.py index b59a8863f8..847a4f78f1 100644 --- a/embodichain/lab/sim/objects/backends/scene.py +++ b/embodichain/lab/sim/objects/backends/scene.py @@ -578,6 +578,51 @@ def select_articulation_ids( ) -> torch.Tensor: return self._articulation_ids[env_ids] + def _apply_link_property( + self, + method_name: str, + values: torch.Tensor, + env_ids: Sequence[int] | torch.Tensor, + link_ids: Sequence[int] | torch.Tensor, + ) -> None: + """Forward selected property writes through the public Scene batch.""" + rows = self._select_rows(env_ids) + if len(rows) and len(link_ids): + _checked_batch_call( + self.batch.select(rows), method_name, values, link_ids=link_ids + ) + + def apply_link_mass( + self, + values: torch.Tensor, + env_ids: Sequence[int] | torch.Tensor, + link_ids: Sequence[int] | torch.Tensor, + ) -> None: + """Write masses with shape (selected environments, selected links).""" + self._apply_link_property("apply_link_mass", values, env_ids, link_ids) + + def apply_link_inertia( + self, + values: torch.Tensor, + env_ids: Sequence[int] | torch.Tensor, + link_ids: Sequence[int] | torch.Tensor, + ) -> None: + """Write three principal moments per selected environment and link.""" + self._apply_link_property( + "apply_link_inertia_diagonal", values, env_ids, link_ids + ) + + def apply_link_com_pose( + self, + values: torch.Tensor, + env_ids: Sequence[int] | torch.Tensor, + link_ids: Sequence[int] | torch.Tensor, + ) -> None: + """Convert selected COM poses from xyz + xyzw to batch xyzw + xyz.""" + self._apply_link_property( + "apply_link_com_local_pose", _batch_pose(values), env_ids, link_ids + ) + def fetch_link_physical_properties( self, mass: torch.Tensor, diff --git a/embodichain/lab/sim/sensors/_warp/contact.py b/embodichain/lab/sim/sensors/_warp/contact.py index f5924f3f21..5ca9b5d747 100644 --- a/embodichain/lab/sim/sensors/_warp/contact.py +++ b/embodichain/lab/sim/sensors/_warp/contact.py @@ -162,3 +162,43 @@ def scatter_contact_rows( True, dropped, ) + + +@wp.kernel(enable_backward=False) +def reduce_normal_impulses( + data: wp.array2d(dtype=wp.float32), + actor_ids: wp.array2d(dtype=wp.int32), + env_ids: wp.array(dtype=wp.int32), + valid: wp.array2d(dtype=wp.bool), + count: wp.array(dtype=wp.int32), + keys: wp.array(dtype=wp.int64), + destinations: wp.array(dtype=wp.int64), + inverse_dt: float, + forces: wp.array2d(dtype=wp.float32), +): + row = wp.tid() + if row < count[0] and valid[row, 0]: + force = ( + wp.vec3(data[row, 3], data[row, 4], data[row, 5]) + * data[row, 9] + * inverse_dt + ) + for side in range(2): + key = wp.int64(env_ids[row]) * wp.int64(4294967296) + wp.int64( + actor_ids[row, side] + ) + lo = int(0) + hi = int(keys.shape[0]) + while lo < hi: + middle = (lo + hi) // 2 + if keys[middle] < key: + lo = middle + 1 + else: + hi = middle + if lo < keys.shape[0] and keys[lo] == key: + destination = destinations[lo] + sign = float(-1.0) + if side == 1: + sign = 1.0 + for axis in range(3): + wp.atomic_add(forces, destination, axis, sign * force[axis]) diff --git a/embodichain/lab/sim/sensors/contact_sensor.py b/embodichain/lab/sim/sensors/contact_sensor.py index 08ce036eab..d30d0b8f0f 100644 --- a/embodichain/lab/sim/sensors/contact_sensor.py +++ b/embodichain/lab/sim/sensors/contact_sensor.py @@ -26,7 +26,10 @@ from tensordict import TensorDict from embodichain.lab.sim.sensors import BaseSensor, SensorCfg -from embodichain.lab.sim.sensors._warp.contact import scatter_contact_rows +from embodichain.lab.sim.sensors._warp.contact import ( + reduce_normal_impulses, + scatter_contact_rows, +) from embodichain.lab.sim.sensors.contact_history import ContactHistory from embodichain.utils import configclass, logger from embodichain.utils.device_utils import current_warp_stream @@ -354,6 +357,41 @@ def _read_contacts(self) -> ContactBuffer: self._sync_filter_actor_metadata() return buffer + def _reduce_normal_impulses( + self, + actor_keys: torch.Tensor, + actor_rows: torch.Tensor, + out: torch.Tensor, + dt: float, + ) -> None: + """Reduce a task-owned force snapshot without materializing contact rows. + + This internal path is for consumers that need only normal forces and + have no row-buffer or substep-history consumers in the current step. + Sorted keys encode environment and actor identities; rows index out. + """ + buffer = self._read_contacts() + out.zero_() + wp.launch( + reduce_normal_impulses, + dim=buffer.capacity, + inputs=[ + wp.from_torch(buffer.data), + wp.from_torch(buffer.actor_ids), + wp.from_torch(buffer.env_ids), + wp.from_torch(buffer.quantity_valid), + wp.from_torch(buffer.count_device), + wp.from_torch(actor_keys), + wp.from_torch(actor_rows), + 1.0 / dt, + wp.from_torch(out.view(-1, 3)), + ], + device=str(self.device), + stream=current_warp_stream(self.device), + ) + self._query_dropped_count.copy_(buffer.dropped_count_device) + self._scatter_dropped_count.zero_() + def _scatter_contacts(self, buffer: ContactBuffer) -> None: assert self._num_contacts_per_env is not None self._num_contacts_per_env.zero_() diff --git a/embodichain/lab/sim/sim_manager.py b/embodichain/lab/sim/sim_manager.py index e132652652..fef37dc22f 100644 --- a/embodichain/lab/sim/sim_manager.py +++ b/embodichain/lab/sim/sim_manager.py @@ -147,6 +147,7 @@ def _is_usd_path(path: object | None) -> bool: ) if TYPE_CHECKING: + from dexsim.spawn import GeometryDesc, NewtonCollisionDesc from dexsim.engine import PhysicsScene from dexsim.scene import Scene from dexsim.interaction import EntityGizmoConfig, EntityGizmoManipulator @@ -1333,6 +1334,24 @@ def _init_sim_resources(self) -> None: self._default_resources = SimResources() + def _register_joint_dc_motor_control( + self, uid: str, robot_parameters: dict[str, Any] + ) -> Any: + """Install one batched explicit motor before scene preparation.""" + if not self.is_newton_backend: + raise RuntimeError("Graph joint motors require the Newton backend.") + if uid not in self._robots and uid not in self._articulations: + raise KeyError(f"Robot or articulation {uid!r} is not declared.") + scene = self._spawn_scene + if scene.builder.is_finalized: + raise RuntimeError("Joint motors must be registered before prepare().") + from embodichain.lab.sim._joint_motor import _BatchedJointDCMotorControl + + paths = tuple(f"{arena}/{uid}" for arena in scene.arena_names) + control = _BatchedJointDCMotorControl(robot_parameters, paths) + scene.builder.add_runtime_control(control) + return control + def register_kinematic_joint_trajectory( self, uid: str, @@ -2209,6 +2228,7 @@ def _declare_spawn_default_plane(self) -> None: physics=RigidBodyPhysicsDesc.static(), per_env=False, ) + self._spawn_default_plane_descriptor = descriptor self._spawn_scene.declare( "rigid_object", @@ -2225,6 +2245,45 @@ def _bind_default_plane(self, plane: Any) -> None: if self.has_native_renderer: plane.set_visible(self._spawn_default_plane_visibility) + def configure_ground_plane_collision( + self, + *, + enabled: bool = True, + newton: NewtonCollisionDesc | None = None, + geometry: GeometryDesc | None = None, + ) -> None: + """Configure ground collision before preparing scene physics. + + Args: + enabled: Whether the default plane participates in collision. + newton: Optional DexSim NewtonCollisionDesc for the plane. + geometry: Optional DexSim GeometryDesc replacing the global ground + geometry for both collision and rendering. + + Raises: + RuntimeError: The scene physics has already been prepared. + """ + if self.spawn_result is not None: + raise RuntimeError("Ground collision must be configured before prepare().") + if geometry is not None: + from dataclasses import fields + from dexsim.spawn import GeometryDesc + + for field in fields(GeometryDesc): + value = getattr(geometry, field.name) + setattr( + self._spawn_default_plane_descriptor.collisions[0], + field.name, + value, + ) + setattr( + self._spawn_default_plane_descriptor.renders[0], field.name, value + ) + collision = self._spawn_default_plane_descriptor.collisions[0] + collision.enable_collision = enabled + if newton is not None: + collision.newton = newton + def set_default_global_lighting(self) -> None: """Set default global lighting for the scene. diff --git a/tests/gym/envs/managers/test_dataset_manager.py b/tests/gym/envs/managers/test_dataset_manager.py index 3f82eaa1e8..9cc6cda0be 100644 --- a/tests/gym/envs/managers/test_dataset_manager.py +++ b/tests/gym/envs/managers/test_dataset_manager.py @@ -281,6 +281,61 @@ def test_initialize_episode_saves_failed_reset_envs_when_enabled() -> None: assert torch.equal(manager.saved_env_ids, torch.tensor([1, 2])) +def test_reset_does_not_create_unused_demo_metadata() -> None: + env, _ = make_env_for_episode_selection( + save_failed_episodes=False, successful_env_ids=[] + ) + env.dataset_manager = None + env._demo_metadata_initialized = False + env._demo_episode_metadata = [{}, {}, {}] + env._new_demo_episode_metadata = MagicMock(return_value={"episode_uuid": "new"}) + + EmbodiedEnv._initialize_episode(env, env_ids=[1, 2], save_data=False) + + env._new_demo_episode_metadata.assert_not_called() + assert env._demo_episode_metadata == [{}, {}, {}] + + +def test_reset_refreshes_only_selected_recording_metadata() -> None: + env, _ = make_env_for_episode_selection( + save_failed_episodes=False, successful_env_ids=[] + ) + env.dataset_manager = None + env._demo_metadata_initialized = True + env._demo_episode_metadata = [{"episode_uuid": str(row)} for row in range(3)] + env._new_demo_episode_metadata = lambda row: {"episode_uuid": f"next-{row}"} + + EmbodiedEnv._initialize_episode(env, env_ids=[1], save_data=False) + + assert env._demo_episode_metadata == [ + {"episode_uuid": "0"}, + {"episode_uuid": "next-1"}, + {"episode_uuid": "2"}, + ] + + +@pytest.mark.parametrize("recorded", [False, True]) +def test_initialize_episode_preserves_unselected_action_histories( + recorded: bool, +) -> None: + """Lazy histories remain absent; populated histories clear selected rows.""" + env, _ = make_env_for_episode_selection( + save_failed_episodes=False, successful_env_ids=[] + ) + env.dataset_manager = None + env._raw_action_history = [["raw-0"], ["raw-1"], ["raw-2"]] if recorded else None + env._expert_controller_qpos_history = ( + [["qpos-0"], ["qpos-1"], ["qpos-2"]] if recorded else None + ) + EmbodiedEnv._initialize_episode(env, env_ids=torch.tensor([2, 0]), save_data=False) + if recorded: + assert env._raw_action_history == [[], ["raw-1"], []] + assert env._expert_controller_qpos_history == [[], ["qpos-1"], []] + else: + assert env._raw_action_history is None + assert env._expert_controller_qpos_history is None + + def test_initialize_episode_saves_row_with_accepted_segment_fragment() -> None: """Fragment eligibility is independent of whole-episode success.""" env, manager = make_env_for_episode_selection( diff --git a/tests/gym/envs/test_demo.py b/tests/gym/envs/test_demo.py index 1d0ab9cfd9..c6179f2c16 100644 --- a/tests/gym/envs/test_demo.py +++ b/tests/gym/envs/test_demo.py @@ -227,6 +227,25 @@ def test_controller_action_refreshes_executed_qpos_snapshot() -> None: torch.testing.assert_close(env._last_action_manager_qpos, torch.ones(2, 3)) +@pytest.mark.parametrize("expert_recording", [False, True]) +def test_action_snapshot_requires_a_recorder(expert_recording: bool) -> None: + """Training processes actions without reading an unused qpos snapshot.""" + env = _controller_action_env() + env.dataset_manager = None + env.rollout_buffer = object() if expert_recording else None + env._rollout_buffer_mode = "expert" + env.action_manager._processed_qpos.return_value = torch.ones(2, 3) + + env._preprocess_action(torch.ones(2, 3)) + + env.action_manager.process_action.assert_called_once() + assert env.action_manager._processed_qpos.call_count == int(expert_recording) + if expert_recording: + torch.testing.assert_close(env._last_action_manager_qpos, torch.ones(2, 3)) + else: + assert env._last_action_manager_qpos is None + + def test_policy_history_snapshots_manager_owned_flat_action() -> None: """Dataset history captures manager state after validation and owns it.""" env = _controller_action_env() @@ -254,6 +273,30 @@ def test_controller_action_does_not_append_policy_history() -> None: assert env.get_raw_action_history(0).shape == (0, 0) +def test_action_histories_allocate_on_first_recorded_sample() -> None: + """Disabled recording stays empty; first samples retain owned snapshots.""" + env = _controller_action_env() + env._raw_action_history = None + env._expert_controller_qpos_history = None + env._record_raw_actions = False + action = torch.arange(6, dtype=torch.float32).reshape(2, 3) + env._record_raw_action(action) + env._record_expert_controller_qpos(None, torch.tensor([1])) + assert env._raw_action_history is None + assert env._expert_controller_qpos_history is None + assert env.get_raw_action_history(0).shape == (0, 0) + assert env.get_expert_controller_qpos_history(1).shape == (0, 3) + + env._record_raw_actions = True + env._record_raw_action(action) + env._record_expert_controller_qpos(action, torch.tensor([1])) + expected = action.clone() + action.fill_(99) + torch.testing.assert_close(env.get_raw_action_history(0), expected[0:1]) + torch.testing.assert_close(env.get_expert_controller_qpos_history(1), expected[1:2]) + assert env.get_expert_controller_qpos_history(0).shape == (0, 3) + + def test_policy_recorder_rejects_controller_action_before_execution() -> None: """A controller-ready expert cannot enter a policy dataset layout.""" env = _controller_action_env() diff --git a/tests/gym/envs/test_recording_provenance.py b/tests/gym/envs/test_recording_provenance.py index c54c249801..42ba96dd7a 100644 --- a/tests/gym/envs/test_recording_provenance.py +++ b/tests/gym/envs/test_recording_provenance.py @@ -83,6 +83,31 @@ def test_environment_identity_is_unique_per_episode_and_shared_per_run() -> None assert first["config_hash"] == second["config_hash"] +def test_metadata_request_materializes_only_requested_row() -> None: + env = make_env() + env.num_envs = 2 + env.episode_context = EpisodeContext(2) + env.rollout_buffer = None + env.rollout_steps = torch.zeros(2, dtype=torch.long) + env._demo_steps = torch.zeros(2, dtype=torch.long) + env.episode_success_status = torch.zeros(2, dtype=torch.bool) + env._demo_metadata_initialized = False + env._demo_episode_metadata = [{}, {"instruction": "Execute the selected program"}] + env._new_demo_episode_metadata = lambda row: EmbodiedEnv._new_demo_episode_metadata( + env, row + ) + + first = EmbodiedEnv.get_demo_episode_metadata(env, 1) + second = EmbodiedEnv.get_demo_episode_metadata(env, 1) + + assert first["episode_uuid"] == second["episode_uuid"] + assert UUID(first["episode_uuid"]) + assert first["env_id"] == 1 + assert first["instruction"] == "Execute the selected program" + assert env._demo_episode_metadata[0] == {} + assert env._demo_metadata_initialized + + def test_episode_override_program_hash_uses_executed_program() -> None: env = make_env() env.cfg.task_program = {"program_id": "configured_default"} diff --git a/tests/sim/objects/test_articulation_inertia_frames.py b/tests/sim/objects/test_articulation_inertia_frames.py index e2b100d557..b27f7fb7a9 100644 --- a/tests/sim/objects/test_articulation_inertia_frames.py +++ b/tests/sim/objects/test_articulation_inertia_frames.py @@ -22,7 +22,7 @@ import torch from embodichain.lab.sim import SimulationManager, SimulationManagerCfg -from embodichain.lab.sim.cfg import ArticulationCfg, physics_cfg_for_backend +from embodichain.lab.sim.cfg import ArticulationCfg, RenderCfg, physics_cfg_for_backend from embodichain.utils.math import matrix_from_quat pytestmark = [pytest.mark.gpu, pytest.mark.requires_sim] @@ -53,6 +53,7 @@ def test_live_inertia_frames_and_partial_reset(tmp_path: Path, backend: str) -> sim = SimulationManager( SimulationManagerCfg( headless=True, + render_cfg=RenderCfg(renderer="no-render"), device="cuda:0", num_envs=3, physics_cfg=physics_cfg_for_backend(backend), @@ -61,7 +62,12 @@ def test_live_inertia_frames_and_partial_reset(tmp_path: Path, backend: str) -> art = None try: art = sim.add_articulation( - ArticulationCfg(uid="inertial", fpath=str(source), init_pos=(0, 0, 1)) + ArticulationCfg( + uid="inertial", + fpath=str(source), + init_pos=(0, 0, 1), + build_pk_chain=False, + ) ) sim.prepare() data = art.body_data @@ -129,6 +135,14 @@ def test_live_inertia_frames_and_partial_reset(tmp_path: Path, backend: str) -> atol=1e-5, rtol=1e-4, ) + # Empty and repeated selections preserve the scalar API behavior. + art.set_mass(torch.empty((1, 0), device=art.device), [], [2]) + art.set_inertia(torch.empty((1, 0, 3), device=art.device), [], [2]) + art.set_com_pose(torch.empty((1, 0, 7), device=art.device), [], [2]) + art.set_mass(torch.tensor([[3.0], [4.0]], device=art.device), names, [2, 2]) + torch.testing.assert_close( + art.get_mass(names, [2]), torch.tensor([[4.0]], device=art.device) + ) sim.update() assert torch.isfinite(art.root_state).all() finally: diff --git a/tests/sim/objects/test_scene_backend.py b/tests/sim/objects/test_scene_backend.py index 6cbc7f7096..4d895b1e73 100644 --- a/tests/sim/objects/test_scene_backend.py +++ b/tests/sim/objects/test_scene_backend.py @@ -1295,3 +1295,54 @@ def test_root_velocity_view_rejects_shape_before_writing( assert batch.selections == [] torch.testing.assert_close(batch.root_linear_velocity, linear_before) torch.testing.assert_close(batch.root_angular_velocity, angular_before) + + +@pytest.mark.parametrize( + ("method", "batch_method", "width"), + [ + ("apply_link_mass", "apply_link_mass", None), + ("apply_link_inertia", "apply_link_inertia_diagonal", 3), + ("apply_link_com_pose", "apply_link_com_local_pose", 7), + ], +) +def test_selected_link_property_writes_preserve_layout_and_errors( + method: str, + batch_method: str, + width: int | None, +) -> None: + batch = _ArticulationBatch() + view = SceneArticulationView(_Scene("newton"), batch, torch.device("cpu")) + selected = Mock() + call = getattr(selected, batch_method) + call.return_value = 1 + batch.select = Mock(return_value=selected) + values = torch.arange(width or 1, dtype=torch.float32).reshape( + (1, 1) if width is None else (1, 1, width) + ) + getattr(view, method)(values, [1], [0]) + torch.testing.assert_close(batch.select.call_args.args[0], torch.tensor([1])) + expected = _batch_pose(values) if width == 7 else values + torch.testing.assert_close(call.call_args.args[0], expected) + assert call.call_args.kwargs["link_ids"] == [0] + call.return_value = -1 + with pytest.raises(RuntimeError, match=batch_method): + getattr(view, method)(values, [1], [0]) + batch.select.reset_mock() + getattr(view, method)(values[:0], [], [0]) + batch.select.assert_not_called() + + +@pytest.mark.parametrize( + "method", ["apply_link_mass", "apply_link_inertia", "apply_link_com_pose"] +) +def test_empty_link_property_selection_is_noop(method: str) -> None: + batch = _ArticulationBatch() + view = SceneArticulationView(_Scene("newton"), batch, torch.device("cpu")) + batch.select = Mock() + width = { + "apply_link_mass": (), + "apply_link_inertia": (3,), + "apply_link_com_pose": (7,), + }[method] + getattr(view, method)(torch.empty((1, 0) + width), [1], []) + batch.select.assert_not_called() diff --git a/tests/sim/sensors/test_contact_query_sensor.py b/tests/sim/sensors/test_contact_query_sensor.py index 3072117ca2..db3dc28c33 100644 --- a/tests/sim/sensors/test_contact_query_sensor.py +++ b/tests/sim/sensors/test_contact_query_sensor.py @@ -54,6 +54,32 @@ def fetch(self) -> ContactBuffer: return self.buffer +def test_normal_force_snapshot_uses_valid_compact_rows_and_clears_output() -> None: + """Exclude friction, stale capacity rows and unavailable force quantities.""" + wp.init() + query = _FakeQuery() + buffer = query.buffer + buffer.count_device[0] = 3 + buffer.quantity_valid[:2, 0] = True + buffer.quantity_valid[2:, 0] = False + buffer.data[2:].fill_(float("nan")) + sensor = object.__new__(ContactSensor) + sensor.device = torch.device("cpu") + sensor._read_contacts = lambda: buffer + sensor._query_dropped_count = torch.zeros(1, dtype=torch.int32) + sensor._scatter_dropped_count = torch.ones(2, dtype=torch.int32) + keys = torch.tensor([4, 2**32 + 7], dtype=torch.int64) + rows = torch.tensor([1, 0], dtype=torch.int64) + out = torch.full((2, 1, 3), 999.0) + sensor._reduce_normal_impulses(keys, rows, out, 0.1) + torch.testing.assert_close( + out, torch.tensor([[[0.0, 7.0, 0.0]], [[0.0, 0.0, -4.0]]]) + ) + buffer.count_device.zero_() + sensor._reduce_normal_impulses(keys, rows, out, 0.1) + torch.testing.assert_close(out, torch.zeros_like(out)) + + def test_contact_sensor_consumes_scene_query_and_explicit_env_ids() -> None: wp.init() query = _FakeQuery() diff --git a/tests/sim/test_joint_motor.py b/tests/sim/test_joint_motor.py new file mode 100644 index 0000000000..caee4bde1c --- /dev/null +++ b/tests/sim/test_joint_motor.py @@ -0,0 +1,170 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- +"""Explicit DC effort preserves reference braking and joint ownership.""" + +from __future__ import annotations + +import pytest +from types import SimpleNamespace +import torch +import warp as wp + +from embodichain.lab.sim._joint_motor import _BatchedJointDCMotorControl, _dc_motor + +__all__: list[str] = [] +pytestmark = pytest.mark.no_sim + + +def test_motor_binds_public_names_and_holds_effort_between_solver_substeps() -> None: + """Arena order and scalar q/qd offsets remain distinct from policy order.""" + wp.init() + parameters = dict( + joint_names=["b", "a"], + stiffness=[2.0, 3.0], + damping=[1.0, 1.0], + effort_limit=[20.0, 20.0], + velocity_limit=[30.0, 30.0], + default_joint_position=[1.0, -1.0], + ) + paths = ("arena_1/robot", "arena_0/robot") + articulations = {} + for arena in range(2): + joints = [ + SimpleNamespace( + name=name, + q_start=20 * arena + q, + qd_start=20 * arena + qd, + q_size=1, + qd_size=1, + joint_id=10 * arena + index, + ) + for index, (name, q, qd) in enumerate([("a", 7, 6), ("b", 9, 8)]) + ] + articulations[f"arena_{arena}/robot"] = SimpleNamespace( + _physics_binding=SimpleNamespace(public_joints=joints) + ) + context = SimpleNamespace( + result=SimpleNamespace(get_articulation=articulations.__getitem__), + device="cpu", + model=SimpleNamespace(joint_f=wp.zeros(40, dtype=float, device="cpu")), + control=SimpleNamespace(joint_f=wp.zeros(40, dtype=float, device="cpu")), + current_state=SimpleNamespace( + joint_q=wp.zeros(40, dtype=float, device="cpu"), + joint_qd=wp.zeros(40, dtype=float, device="cpu"), + ), + ) + control = _BatchedJointDCMotorControl(parameters, paths) + control.initialize(context) + assert control.q_indices.numpy().tolist() == [[29, 27], [9, 7]] + assert control.qd_indices.numpy().tolist() == [[28, 26], [8, 6]] + assert control.exclusive_resource_claims() == ( + ("newton_joint", 11), + ("newton_joint", 10), + ("newton_joint", 1), + ("newton_joint", 0), + ) + control(context, 0, 2, 0.0025) + torch.testing.assert_close( + wp.to_torch(control.effort), torch.tensor([[2.0, -3.0], [2.0, -3.0]]) + ) + control.targets.zero_() + control(context, 1, 2, 0.0025) + torch.testing.assert_close( + wp.to_torch(control.effort), torch.tensor([[2.0, -3.0], [2.0, -3.0]]) + ) + control(context, 0, 2, 0.0025) + torch.testing.assert_close(wp.to_torch(control.effort), torch.zeros(2, 2)) + control.close() + assert control.targets is None and control.exclusive_resource_claims() == () + + +def test_dc_motor_matches_reference_for_scalar_dof_routing() -> None: + """Cover reordered free-root DOFs, overspeed braking and nonfinite inputs.""" + wp.init() + device = "cpu" + torch.manual_seed(42) + rows, columns = 128, 12 + order = torch.randperm(columns, device=device) + q_indices = torch.arange(rows, device=device)[:, None] * (columns + 7) + 7 + order + qd_indices = torch.arange(rows, device=device)[:, None] * (columns + 6) + 6 + order + q = torch.randn(rows * (columns + 7) + 13, device=device) + qd = torch.randn(rows * (columns + 6) + 13, device=device) * 100 + targets = torch.randn(rows, columns, device=device) + kp = torch.rand(columns, device=device) * 100 + kd = torch.rand(columns, device=device) * 10 + limits = torch.rand(columns, device=device) * 200 + 1 + speeds = torch.rand(columns, device=device) * 80 + 1 + qd[qd_indices[0]] = torch.stack( + ( + speeds[0] * 2, + -speeds[1] * 2, + speeds[2] * 2.1, + -speeds[3] * 2.1, + speeds[4], + -speeds[5], + speeds[6] * 0, + speeds[7] * 0.5, + speeds[8] * -0.5, + speeds[9] * float("inf"), + speeds[10] * -float("inf"), + speeds[11] * float("nan"), + ) + ) + targets[1, 0] = float("nan") + # Independent torque-speed envelope in Torch; no task-package dependency. + speed = qd[qd_indices].clamp(-2 * speeds, 2 * speeds) + upper = torch.minimum(limits * (1 - speed / speeds), limits) + lower = torch.maximum(limits * (-1 - speed / speeds), -limits) + requested = kp * (targets - q[q_indices]) - kd * qd[qd_indices] + expected = torch.clamp(requested, min=lower, max=upper) + model_force = torch.full_like(qd, 2.3) + control_force = torch.full_like(qd, 2.3) + effort = torch.zeros_like(targets) + velocity = torch.zeros_like(targets) + indices = [ + wp.from_torch(value.to(torch.int32)) for value in (q_indices, qd_indices) + ] + wp.launch( + _dc_motor, + dim=(rows, columns), + inputs=[ + wp.from_torch(q), + wp.from_torch(qd), + *indices, + wp.from_torch(targets), + wp.from_torch(kp), + wp.from_torch(kd), + wp.from_torch(limits), + wp.from_torch(speeds), + wp.from_torch(model_force), + wp.from_torch(control_force), + wp.from_torch(effort), + wp.from_torch(velocity), + ], + device=device, + ) + wp.synchronize_device(device) + torch.testing.assert_close(effort, expected, atol=2e-5, rtol=1e-6, equal_nan=True) + torch.testing.assert_close(velocity, qd[qd_indices], atol=0, rtol=0, equal_nan=True) + mask = torch.ones_like(qd, dtype=torch.bool) + mask[qd_indices] = False + for owner in (model_force, control_force): + torch.testing.assert_close( + owner[qd_indices], expected, atol=2e-5, rtol=1e-6, equal_nan=True + ) + torch.testing.assert_close( + owner[mask], torch.full_like(owner[mask], 2.3), atol=0, rtol=0 + ) diff --git a/tests/sim/test_sim_manager.py b/tests/sim/test_sim_manager.py index 0aee1b1b39..fba59ff788 100644 --- a/tests/sim/test_sim_manager.py +++ b/tests/sim/test_sim_manager.py @@ -1647,6 +1647,31 @@ def test_failed_publication_retries_within_the_same_frame(): assert sim.physics.sync_render_state.call_count == 2 +def test_joint_motor_registration_preserves_manager_arena_order() -> None: + sim, builder = _make_runtime_control_sim_manager() + control = sim._register_joint_dc_motor_control("robot", {}) + builder.add_runtime_control.assert_called_once_with(control) + assert control.target_paths == ("arena_0/robot", "arena_1/robot") + + +@pytest.mark.parametrize( + "backend, uid, finalized, error", + [ + ("default", "robot", False, RuntimeError), + ("newton", "missing", False, KeyError), + ("newton", "robot", True, RuntimeError), + ], +) +def test_joint_motor_registration_rejects_invalid_lifecycle( + backend, uid, finalized, error +) -> None: + sim, builder = _make_runtime_control_sim_manager(backend=backend) + builder.is_finalized = finalized + with pytest.raises(error): + sim._register_joint_dc_motor_control(uid, {}) + builder.add_runtime_control.assert_not_called() + + def test_register_kinematic_joint_trajectory_expands_each_arena() -> None: sim, builder = _make_runtime_control_sim_manager() frame_count = 3 From d9420ac6fed316e478cec22960611c028de8d935 Mon Sep 17 00:00:00 2001 From: acrlw <13927622+acrlw@users.noreply.github.com> Date: Wed, 21 Oct 2026 20:44:02 +0800 Subject: [PATCH 02/14] fix(rl): pass vector gravity to Newton planar reach builders --- .../learning/rl/experimental/newton/planar_reach.py | 4 ++-- tests/learning/test_newton_planar_reach.py | 9 +++++++++ 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/embodichain/learning/rl/experimental/newton/planar_reach.py b/embodichain/learning/rl/experimental/newton/planar_reach.py index 41c3e5adf9..81d3f2f04c 100644 --- a/embodichain/learning/rl/experimental/newton/planar_reach.py +++ b/embodichain/learning/rl/experimental/newton/planar_reach.py @@ -351,7 +351,7 @@ def close(self) -> None: """No-op; Warp/Newton resources are released with the Python object.""" def _build_model(self) -> tuple[Any, wp.array]: - template = newton.ModelBuilder(gravity=0.0, up_axis=newton.Axis.Y) + template = newton.ModelBuilder(gravity=(0.0, 0.0, 0.0), up_axis=newton.Axis.Y) first_link = template.add_link(label="planar_first_link") template.add_shape_sphere(first_link, radius=0.01) second_link = template.add_link(label="planar_end_link") @@ -388,7 +388,7 @@ def _build_model(self) -> tuple[Any, wp.array]: ) template.add_articulation([first_joint, second_joint]) - builder = newton.ModelBuilder(gravity=0.0, up_axis=newton.Axis.Y) + builder = newton.ModelBuilder(gravity=(0.0, 0.0, 0.0), up_axis=newton.Axis.Y) builder.replicate( template, world_count=self.num_envs, diff --git a/tests/learning/test_newton_planar_reach.py b/tests/learning/test_newton_planar_reach.py index b6c210b34d..f6395c51e7 100644 --- a/tests/learning/test_newton_planar_reach.py +++ b/tests/learning/test_newton_planar_reach.py @@ -118,6 +118,15 @@ def _feature_policy_objective_at( return segmented_discounted_return(rollout, gamma=0.95).mean().detach().item() +def test_newton_model_preserves_zero_gravity_for_all_worlds() -> None: + env = _make_env() + gravity = torch.as_tensor(env._model.gravity.numpy()) + + assert env._model.world_count == env.num_envs + assert gravity.shape == (env.num_envs + 1, 3) + assert torch.equal(gravity, torch.zeros_like(gravity)) + + def test_newton_env_satisfies_contract_and_fk_matches_analytical_pose() -> None: env = _make_env() observation, _ = env.reset(seed=3) From 668aa338464fe5c4d13d4431a0cd1b2378e1619d Mon Sep 17 00:00:00 2001 From: acrlw <13927622+acrlw@users.noreply.github.com> Date: Thu, 22 Oct 2026 10:15:14 +0800 Subject: [PATCH 03/14] perf(gym): make per-step episode context snapshots optional --- .../topics/env-framework/env-framework.md | 6 +++- embodichain/lab/gym/envs/base_env.py | 14 ++++++-- embodichain/lab/gym/envs/embodied_env.py | 3 +- embodichain/lab/gym/utils/gym_utils.py | 6 ++++ tests/gym/envs/test_env_seed.py | 36 +++++++++++++++++-- tests/gym/envs/test_objectives.py | 11 ++++-- tests/gym/utils/test_gym_utils.py | 25 +++++++++++++ 7 files changed, 93 insertions(+), 8 deletions(-) diff --git a/agent_context/topics/env-framework/env-framework.md b/agent_context/topics/env-framework/env-framework.md index 28e67d1f39..4e421b6b7c 100644 --- a/agent_context/topics/env-framework/env-framework.md +++ b/agent_context/topics/env-framework/env-framework.md @@ -156,7 +156,11 @@ physical-objective initialization and final observations; it is not forwarded to functors. `BaseEnv` owns `EpisodeContext`; successful initialization seeds recording from the final observation. `EmbodiedEnv` consumes old records before physical reset. Failed checks remain attached to buffers until the selected rows are consumed -and a new episode begins. Info and recorder metadata carry owned context snapshots. The concrete preparation/restore host and +and a new episode begins. Info and recorder metadata carry owned context snapshots. +`EnvCfg.episode_context_in_info` defaults to true. Batched RL deployments may +disable automatic info snapshots while retaining context lifecycle, recorder +metadata and explicit snapshots. Reset reuses the snapshot from `get_info()`; +overrides that omit it receive one when the option is enabled. The concrete preparation/restore host and snapshot limitations belong to [scene expansion](../scene-expansion/scene-expansion.md). ## Change sites and focused validation diff --git a/embodichain/lab/gym/envs/base_env.py b/embodichain/lab/gym/envs/base_env.py index 76cf5551be..9f6531fafb 100644 --- a/embodichain/lab/gym/envs/base_env.py +++ b/embodichain/lab/gym/envs/base_env.py @@ -99,6 +99,14 @@ class EnvCfg: only end when the task is successfully completed or failed. """ + episode_context_in_info: bool = True + """Include owned episode-context snapshots in step and reset info. + + Disable for batched RL consumers that do not read per-row provenance. + Episode lifecycle tracking, recorder metadata, and explicit + ``episode_context.snapshot()`` calls remain available. + """ + profiler: EnvProfilerCfg | None = None """Optional profiler for reset/step wall-time breakdown. ``None`` keeps the profiler disabled unless one is configured directly on ``sim_cfg``. @@ -823,7 +831,8 @@ def get_info(self, **kwargs) -> TensorDict[str, Any]: evaluate = self.evaluate(**kwargs) if evaluate: info.update(evaluate) - info["episode_context"] = self.episode_context.snapshot() + if self.cfg.episode_context_in_info: + info["episode_context"] = self.episode_context.snapshot() return info def check_truncated(self, obs: EnvObs, info: TensorDict[str, Any]) -> torch.Tensor: @@ -1039,7 +1048,8 @@ def reset( context._finish_initialization(reset_ids) with self._profiler.section("get_info"): info = self.get_info(**options) - info["episode_context"] = context.snapshot() + if self.cfg.episode_context_in_info and "episode_context" not in info: + info["episode_context"] = context.snapshot() except BaseException as error: context._abort_initialization(reset_ids, error) raise diff --git a/embodichain/lab/gym/envs/embodied_env.py b/embodichain/lab/gym/envs/embodied_env.py index 68349e51d2..5812964e59 100644 --- a/embodichain/lab/gym/envs/embodied_env.py +++ b/embodichain/lab/gym/envs/embodied_env.py @@ -2577,7 +2577,8 @@ def get_info(self, **kwargs) -> Dict[str, Any]: outcome = getattr(self, "termination_manager", None) if outcome is not None: info["task_outcome"] = outcome.snapshot() - info["episode_context"] = self.episode_context.snapshot() + if self.cfg.episode_context_in_info: + info["episode_context"] = self.episode_context.snapshot() return info def check_truncated(self, obs: EnvObs, info: dict[str, Any]) -> torch.Tensor: diff --git a/embodichain/lab/gym/utils/gym_utils.py b/embodichain/lab/gym/utils/gym_utils.py index 32ffae7365..78539a809f 100644 --- a/embodichain/lab/gym/utils/gym_utils.py +++ b/embodichain/lab/gym/utils/gym_utils.py @@ -967,6 +967,12 @@ class ComponentCfg: if type(ignore_terminations) is not bool: raise TypeError("env.ignore_terminations must be a bool.") env_cfg.ignore_terminations = ignore_terminations + episode_context_in_info = config["env"].get( + "episode_context_in_info", env_cfg.episode_context_in_info + ) + if type(episode_context_in_info) is not bool: + raise TypeError("env.episode_context_in_info must be a bool.") + env_cfg.episode_context_in_info = episode_context_in_info task_instruction = config["env"].get("task_instruction") if task_instruction is not None and ( type(task_instruction) is not str diff --git a/tests/gym/envs/test_env_seed.py b/tests/gym/envs/test_env_seed.py index 869f38c3be..2b72d09699 100644 --- a/tests/gym/envs/test_env_seed.py +++ b/tests/gym/envs/test_env_seed.py @@ -28,7 +28,7 @@ import torch import embodichain.lab.gym.envs.base_env as base_env_module -from embodichain.lab.gym.envs import BaseEnv +from embodichain.lab.gym.envs import BaseEnv, EnvCfg, EmbodiedEnv from embodichain.lab.sim import SimulationManager # This module calls one unbound manager method on registry stubs; no World is @@ -48,7 +48,7 @@ class _ResetEnv(BaseEnv): def __init__(self, num_envs: int = 1) -> None: self.reset_events: list[str] = [] - self.cfg = SimpleNamespace(seed=None) + self.cfg = EnvCfg(seed=None) self._num_envs = num_envs self.episode_context = EpisodeContext(num_envs) self.sim = SimpleNamespace( @@ -241,3 +241,35 @@ def test_explicit_zero_seed_is_reproducible(): first.cfg.seed = second.cfg.seed = 0 a, b = first.get_generator("commands"), second.get_generator("commands") assert torch.equal(torch.rand(8, generator=a), torch.rand(8, generator=b)) + + +@pytest.mark.parametrize("enabled", [True, False]) +@pytest.mark.parametrize("info_owner", [BaseEnv, EmbodiedEnv]) +def test_episode_context_info_is_optional_without_changing_lifecycle( + enabled: bool, info_owner: type, monkeypatch: pytest.MonkeyPatch +) -> None: + """Opting out avoids snapshots while partial reset and provenance stay valid.""" + env = _ResetEnv(num_envs=2) + assert env.cfg.episode_context_in_info is True + env.cfg.episode_context_in_info = enabled + env.compute_task_state = lambda **_: ( + torch.zeros(2, dtype=torch.bool), + torch.zeros(2, dtype=torch.bool), + {}, + ) + env.get_info = lambda **kwargs: info_owner.get_info(env, **kwargs) + snapshot = MagicMock(wraps=env.episode_context.snapshot) + monkeypatch.setattr(env.episode_context, "snapshot", snapshot) + _, info = env.reset() + assert ("episode_context" in info) is enabled + assert snapshot.call_count == int(enabled) + # Recording remains eligible and can still request an owned snapshot. + assert env.episode_context.filter_recordable([0, 1]).tolist() == [0, 1] + saved = snapshot() + env.get_info() + _, partial_info = env.reset(options={"reset_ids": [1]}) + assert ("episode_context" in partial_info) is enabled + assert snapshot.call_count == 1 + 3 * int(enabled) + assert [row["episode"] for row in saved] == [0, 0] + assert [row["episode"] for row in snapshot()] == [0, 1] + assert env.episode_context.filter_recordable([0, 1]).tolist() == [0, 1] diff --git a/tests/gym/envs/test_objectives.py b/tests/gym/envs/test_objectives.py index e8e8d59c56..91c0bdf2ea 100644 --- a/tests/gym/envs/test_objectives.py +++ b/tests/gym/envs/test_objectives.py @@ -216,7 +216,8 @@ def test_absent_objective_preserves_info_keys(): env.episode_context = EpisodeContext(2) env.sim = SimpleNamespace(device=torch.device("cpu")) env._elapsed_steps = torch.zeros(2, dtype=torch.int32) - assert EmbodiedEnvCfg().objective is None + env.cfg = EmbodiedEnvCfg() + assert env.cfg.objective is None assert set(env.get_info()) == { "success", "fail", @@ -278,6 +279,7 @@ def __init__(self): self.max_episode_steps = 100 self.cfg = SimpleNamespace( objective=_runtime().cfg, + episode_context_in_info=True, sim_steps_per_control=1, ignore_terminations=False, events=True, @@ -366,8 +368,12 @@ def test_control_step_samples_after_interval_events_and_info_is_pure(): assert not info["success"].any() -def test_reset_preserves_terminal_snapshot_and_resets_after_recorder_consumption(): +@pytest.mark.parametrize("include_context", [True, False]) +def test_reset_preserves_terminal_snapshot_and_resets_after_recorder_consumption( + include_context: bool, +): env = _LifecycleEnv() + env.cfg.episode_context_in_info = include_context for x in [0.0, 0.0, 1.0, 1.0, 0.0, 0.0]: env.interval_x = x env.step(None) @@ -379,6 +385,7 @@ def test_reset_preserves_terminal_snapshot_and_resets_after_recorder_consumption apply=lambda **kwargs: saved.append(env.physical_objective.snapshot_row(0)), ) _, info = env.reset(options={"reset_ids": torch.tensor([0])}) + assert ("episode_context" in info) is include_context assert saved[0]["success"] is True assert env.reset_snapshot["success"].tolist() == [True, True] assert info["physical_objective"]["progress"].tolist() == [0, 3] diff --git a/tests/gym/utils/test_gym_utils.py b/tests/gym/utils/test_gym_utils.py index 9fa68a3ebd..271a53c8b5 100644 --- a/tests/gym/utils/test_gym_utils.py +++ b/tests/gym/utils/test_gym_utils.py @@ -963,6 +963,31 @@ def test_different_max_episode_steps(): class TestConfigToCfgFromFile: + @pytest.mark.parametrize("value", [True, False, None]) + def test_episode_context_info_config(self, value: bool | None) -> None: + config = { + "id": "EpisodeContextInfo-v0", + "physics": "default", + "env": {}, + "robot": {"uid": "ConfigOnly"}, + } + if value is not None: + config["env"]["episode_context_in_info"] = value + assert config_to_cfg(config).episode_context_in_info is ( + True if value is None else value + ) + + @pytest.mark.parametrize("value", ["false", 0, None]) + def test_episode_context_info_rejects_non_boolean(self, value: object) -> None: + config = { + "id": "EpisodeContextInfo-v0", + "physics": "default", + "env": {"episode_context_in_info": value}, + "robot": {"uid": "ConfigOnly"}, + } + with pytest.raises(TypeError, match="episode_context_in_info must be a bool"): + config_to_cfg(config) + @pytest.mark.parametrize("value", (True, False)) def test_config_to_cfg_forwards_ignore_terminations(self, value: bool) -> None: """The explicit environment field reaches the public typed config.""" From fc3c1dcc8f01ce22bc5e8b58c20563028661ede7 Mon Sep 17 00:00:00 2001 From: acrlw <13927622+acrlw@users.noreply.github.com> Date: Thu, 22 Oct 2026 12:13:00 +0800 Subject: [PATCH 04/14] perf(sim): expose mesh BVH construction for Newton terrain queries --- docs/source/overview/sim/sim_manager/physics/newton.md | 8 ++++++++ embodichain/lab/sim/cfg/physics.py | 9 +++++++++ tests/sim/test_cfg.py | 9 +++++++++ 3 files changed, 26 insertions(+) diff --git a/docs/source/overview/sim/sim_manager/physics/newton.md b/docs/source/overview/sim/sim_manager/physics/newton.md index 663c182fc1..d1ca0aab87 100644 --- a/docs/source/overview/sim/sim_manager/physics/newton.md +++ b/docs/source/overview/sim/sim_manager/physics/newton.md @@ -126,3 +126,11 @@ and [solver tuning reference](https://newton-physics.github.io/newton/stable/con newton/dexuni newton/runtime_qualification ``` + +## Mesh query acceleration + +`NewtonPhysicsCfg.mesh_bvh_constructor` selects the mesh BVH constructor used +for mesh and heightfield ray queries. Its default `None` preserves the engine +configuration. Large CUDA terrain scans can use `"cubql"` with a DexSim build +supporting this field. This setting changes the acceleration structure, while +preserving mesh geometry and physical contact parameters. diff --git a/embodichain/lab/sim/cfg/physics.py b/embodichain/lab/sim/cfg/physics.py index 314eca508a..d630c7766f 100644 --- a/embodichain/lab/sim/cfg/physics.py +++ b/embodichain/lab/sim/cfg/physics.py @@ -302,6 +302,13 @@ class NewtonPhysicsCfg(PhysicsBackendCfg): on CUDA unless the caller explicitly supplies another device. """ + mesh_bvh_constructor: Literal["sah", "median", "lbvh", "cubql"] | None = None + """Optional mesh and heightfield query BVH constructor. + + ``None`` preserves the engine default. CUDA terrain ray workloads can select + ``"cubql"`` through a DexSim build exposing this configuration field. + """ + num_substeps: int = 10 """Number of Newton solver substeps per EmbodiChain physics step. @@ -446,6 +453,8 @@ def to_dexsim_cfg( "collision_pipeline_cfg": collision_pipeline_cfg, "sync_to_dexsim": self.sync_to_renderer, } + if self.mesh_bvh_constructor is not None: + newton_cfg_args["mesh_bvh_constructor"] = self.mesh_bvh_constructor if solver_cfg is not None: newton_cfg_args["solver_cfg"] = solver_cfg diff --git a/tests/sim/test_cfg.py b/tests/sim/test_cfg.py index c63dd25ad0..aed6c2a012 100644 --- a/tests/sim/test_cfg.py +++ b/tests/sim/test_cfg.py @@ -596,6 +596,8 @@ def names(config_type: type) -> set[str]: "is_site", # Asset-loading adapter option, not a native ShapeConfig parameter. "use_native_mesh_loader", + # Descriptor-only mesh-to-heightfield import resolution. + "heightfield_resolution", } assert ( newton_fields == names(NewtonCollisionDesc) - intentionally_unowned_shape_fields @@ -1543,3 +1545,10 @@ def test_nrd_rejects_invalid_settings(field_name: str, invalid_value: object) -> """Malformed NRD values fail before entering the native renderer.""" with pytest.raises(ValueError, match=field_name): sim_cfg.NRDCfg(**{field_name: invalid_value}) + + +@pytest.mark.parametrize("constructor", [None, "sah", "cubql"]) +def test_newton_mesh_bvh_constructor_round_trip(constructor: str | None) -> None: + cfg = NewtonPhysicsCfg(mesh_bvh_constructor=constructor) + assert cfg.to_dict()["mesh_bvh_constructor"] == constructor + assert cfg.to_dexsim_cfg(gpu_id=0).mesh_bvh_constructor == constructor From 7d49f6a93855ad61d3275a0198c0b3c626854ba9 Mon Sep 17 00:00:00 2001 From: acrlw <13927622+acrlw@users.noreply.github.com> Date: Thu, 22 Oct 2026 13:17:46 +0800 Subject: [PATCH 05/14] fix: Support custom asset caches and require compatible DexSim packages --- .../sim/sim_manager/physics/newton.md | 10 +++ embodichain/data/dataset.py | 48 ++++++----- pyproject.toml | 2 +- tests/data/test_dataset.py | 83 +++++++++++++++++++ 4 files changed, 122 insertions(+), 21 deletions(-) diff --git a/docs/source/overview/sim/sim_manager/physics/newton.md b/docs/source/overview/sim/sim_manager/physics/newton.md index d1ca0aab87..b075e0e67e 100644 --- a/docs/source/overview/sim/sim_manager/physics/newton.md +++ b/docs/source/overview/sim/sim_manager/physics/newton.md @@ -8,6 +8,16 @@ EmbodiChain's shared scene, object, sensor, and stepping APIs. Select it with {class}`~cfg.NewtonPhysicsCfg`, or with `physics: newton` in a Gym environment configuration. +## Package compatibility + +This integration requires `dexsim_engine>=0.5.1,<0.6`. The earlier `0.5.1rc1` +package does not contain the batched RL and contact fixes required here. +When testing locally built candidates, install the EmbodiChain and DexSim wheels +in the same `python -m pip install` command, with dependency resolution enabled, +then run `python -m pip check`. A local version such as `0.5.1+` satisfies +the requirement. Use the paired candidate wheels until DexSim 0.5.1 is published +to the package index. + ```{important} For scenes that combine an articulated robot with cloth or a volume soft body, start with **DexUni**. It is the DexSim coupled solver configuration for this diff --git a/embodichain/data/dataset.py b/embodichain/data/dataset.py index 41b1b592ce..f41051bf65 100644 --- a/embodichain/data/dataset.py +++ b/embodichain/data/dataset.py @@ -29,11 +29,29 @@ import hashlib import time from contextlib import contextmanager +from pathlib import Path from typing import Iterator import open3d as o3d from embodichain.utils import logger +__all__ = ["EmbodiChainDataset", "get_data_class", "get_data_path"] + + +def _dataset_cache_paths(path: str, prefix: str) -> tuple[Path, Path]: + """Resolve one cache entry and reject paths outside its configured root.""" + if not prefix or prefix in {".", "..", ".locks"} or "/" in prefix or "\\" in prefix: + raise ValueError(f"Dataset prefix must name one cache entry: {prefix!r}") + root = Path(path).resolve() + entries = [] + for category in ("download", "extract"): + parent = root / category + entry = parent / prefix + if parent.resolve() != parent or entry.resolve() != entry: + raise ValueError(f"Dataset cache entry must stay within {parent}: {entry}") + entries.append(entry) + return entries[0], entries[1] + @contextmanager def _dataset_download_lock(path: str, prefix: str) -> Iterator[None]: @@ -47,7 +65,12 @@ def _dataset_download_lock(path: str, prefix: str) -> Iterator[None]: path: Root of the EmbodiChain data cache. prefix: Dataset cache directory name. """ - lock_dir = os.path.join(path, "download", ".locks") + download_path, _ = _dataset_cache_paths(path, prefix) + lock_dir = download_path.parent / ".locks" + if lock_dir.resolve() != lock_dir: + raise ValueError( + f"Dataset cache lock directory must not be a symlink: {lock_dir}" + ) os.makedirs(lock_dir, exist_ok=True) lock_path = os.path.join(lock_dir, f"{prefix}.lock") @@ -92,28 +115,13 @@ def __init__(self, prefix, data_descriptor, path): def check_zip(self, prefix, data_descriptor, path): """Check the integrity of the zip file and its extracted contents.""" - # Path to the downloaded zip file + # Validate both entries before removing an archive or extracted data. + zip_dir_path, extracted_path = _dataset_cache_paths(path, prefix) zip_file_name = os.path.split(data_descriptor.urls[0])[1] - zip_dir_path = os.path.join(path, "download", f"{prefix}") - zip_path = os.path.join(path, "download", f"{prefix}", f"{zip_file_name}") - # Path to the extracted directory - extracted_path = os.path.join(path, "extract", prefix) - - def is_safe_path(path_to_check): - """Verify if the path is within safe directory boundaries""" - return ( - "embodichain_data/download" in path_to_check - or "embodichain_data/extract" in path_to_check - ) + zip_path = zip_dir_path / zip_file_name def safe_remove_directory(dir_path): - """Safely remove a directory after path validation""" - if not is_safe_path(dir_path): - logger.log_warning( - f"Safety check failed, refusing to delete directory: {dir_path}" - ) - return False - + """Remove an entry whose configured cache boundary was validated.""" if os.path.exists(dir_path): try: shutil.rmtree(dir_path) diff --git a/pyproject.toml b/pyproject.toml index 43b40c7d92..f536663bf4 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -25,7 +25,7 @@ dynamic = ["version"] # Core install dependencies (kept from requirements.txt). dependencies = [ - "dexsim_engine==0.5.1rc1", + "dexsim_engine>=0.5.1,<0.6", # DexSim and LeRobot require different OpenCV distributions sharing cv2. # Keep their bindings on the same release to avoid mixed Python/native APIs. "opencv-python==4.12.0.88; python_version < '3.12'", diff --git a/tests/data/test_dataset.py b/tests/data/test_dataset.py index 6853e74d65..86bb9eee2f 100644 --- a/tests/data/test_dataset.py +++ b/tests/data/test_dataset.py @@ -20,6 +20,7 @@ import multiprocessing as mp import time +from types import SimpleNamespace from pathlib import Path import pytest @@ -33,6 +34,88 @@ PROCESS_START_TIMEOUT_SECONDS = 60.0 +def _check_cache( + root: Path, prefix: str = "TestAsset", expected: str = "valid" +) -> None: + """Run cache preparation without starting a network download.""" + descriptor = SimpleNamespace( + urls=["https://example.invalid/model.zip"], md5=expected + ) + dataset = SimpleNamespace(calculate_md5=lambda path: "valid") + dataset_module.EmbodiChainDataset.check_zip(dataset, prefix, descriptor, str(root)) + + +@pytest.mark.no_sim +@pytest.mark.parametrize("has_zip", [False, True]) +def test_custom_cache_removes_stale_extraction(tmp_path: Path, has_zip: bool) -> None: + """An orphaned or invalid download clears only its own extraction.""" + root = tmp_path / "asset-cache" + extracted = root / "extract/TestAsset" + extracted.mkdir(parents=True) + (extracted / "stale.txt").write_text("stale") + neighbor = root / "extract/AnotherAsset" + neighbor.mkdir() + (neighbor / "keep.txt").write_text("keep") + download = root / "download/TestAsset/model.zip" + if has_zip: + download.parent.mkdir(parents=True) + download.write_bytes(b"invalid") + + _check_cache(root, expected="different") + + assert not extracted.exists() + assert not download.exists() + assert (neighbor / "keep.txt").read_text() == "keep" + + +@pytest.mark.no_sim +def test_custom_cache_preserves_valid_download(tmp_path: Path) -> None: + """A valid archive with extracted contents is reused.""" + root = tmp_path / "asset-cache" + download = root / "download/TestAsset/model.zip" + download.parent.mkdir(parents=True) + download.write_bytes(b"archive") + extracted = root / "extract/TestAsset" + extracted.mkdir(parents=True) + (extracted / "model.xml").write_text("model") + + _check_cache(root) + + assert download.read_bytes() == b"archive" + assert (extracted / "model.xml").read_text() == "model" + + +@pytest.mark.no_sim +def test_cache_rejects_extraction_symlink_outside_root(tmp_path: Path) -> None: + """Invalid extraction paths fail before an archive or outside data is removed.""" + root = tmp_path / "asset-cache" + outside = tmp_path / "outside" + outside.mkdir() + (outside / "keep.txt").write_text("keep") + (root / "extract").mkdir(parents=True) + (root / "extract/TestAsset").symlink_to(outside, target_is_directory=True) + download = root / "download/TestAsset/model.zip" + download.parent.mkdir(parents=True) + download.write_bytes(b"archive") + + with pytest.raises(ValueError, match="cache"): + _check_cache(root, expected="different") + + assert download.read_bytes() == b"archive" + assert (outside / "keep.txt").read_text() == "keep" + + +@pytest.mark.no_sim +@pytest.mark.parametrize( + "prefix", + ["", ".", "..", "../outside", "/absolute", "nested/name", "nested\\name", ".locks"], +) +def test_cache_rejects_non_entry_prefix(tmp_path: Path, prefix: str) -> None: + """A dataset owns one cache entry, never a parent or reserved lock directory.""" + with pytest.raises(ValueError, match="prefix"): + _check_cache(tmp_path, prefix) + + @pytest.mark.no_sim def test_get_data_path_accepts_dataset_root_without_subpath( tmp_path: Path, monkeypatch: pytest.MonkeyPatch From 0635df1d2c8f3659f8712e2fe1325fd00f888eb7 Mon Sep 17 00:00:00 2001 From: acrlw <13927622+acrlw@users.noreply.github.com> Date: Fri, 23 Oct 2026 11:32:06 +0800 Subject: [PATCH 06/14] test: track explicit terrain conversion descriptor option --- tests/sim/test_cfg.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/tests/sim/test_cfg.py b/tests/sim/test_cfg.py index aed6c2a012..ff66a26de1 100644 --- a/tests/sim/test_cfg.py +++ b/tests/sim/test_cfg.py @@ -596,7 +596,8 @@ def names(config_type: type) -> set[str]: "is_site", # Asset-loading adapter option, not a native ShapeConfig parameter. "use_native_mesh_loader", - # Descriptor-only mesh-to-heightfield import resolution. + # Descriptor-only mesh-to-heightfield conversion options. + "convert_to_heightfield", "heightfield_resolution", } assert ( From c1fc67b2bb5c95cfcf9675ce40860eb6a18bea35 Mon Sep 17 00:00:00 2001 From: acrlw <13927622+acrlw@users.noreply.github.com> Date: Mon, 2 Nov 2026 09:48:29 +0800 Subject: [PATCH 07/14] Improve batched physics controls and lazy recording metadata --- .../topics/env-framework/env-framework.md | 4 +- .../articulation-adapters.md | 11 ++ .../physics-configuration.md | 21 ++-- .../embodichain/embodichain.lab.sim.cfg.rst | 21 ++++ .../embodichain.lab.sim.shapes.rst | 15 ++- embodichain/lab/gym/envs/embodied_env.py | 51 +++++++- embodichain/lab/sim/cfg/articulation.py | 27 +++++ embodichain/lab/sim/cfg/physics.py | 22 +++- embodichain/lab/sim/objects/articulation.py | 22 +++- .../lab/sim/objects/backends/newton.py | 6 +- embodichain/lab/sim/objects/backends/scene.py | 18 ++- embodichain/lab/sim/objects/rigid_object.py | 28 +++-- embodichain/lab/sim/objects/robot.py | 7 ++ embodichain/lab/sim/shapes.py | 15 +++ embodichain/lab/sim/spawn/descriptors.py | 26 +++- tests/gym/envs/test_episode_context.py | 34 ++++++ tests/gym/envs/test_recording_provenance.py | 22 ++++ .../objects/test_articulation_mass_setters.py | 50 +++++++- .../test_articulation_property_batch.py | 39 +++++- tests/sim/objects/test_articulation_state.py | 72 ++++++++++- tests/sim/objects/test_scene_backend.py | 59 ++++++++- tests/sim/spawn/test_descriptors.py | 114 +++++++++++++++++- tests/sim/test_sim_manager_cfg.py | 61 +++++++--- 23 files changed, 678 insertions(+), 67 deletions(-) diff --git a/agent_context/topics/env-framework/env-framework.md b/agent_context/topics/env-framework/env-framework.md index 4e421b6b7c..4b4bc0a804 100644 --- a/agent_context/topics/env-framework/env-framework.md +++ b/agent_context/topics/env-framework/env-framework.md @@ -159,7 +159,9 @@ Failed checks remain attached to buffers until the selected rows are consumed and a new episode begins. Info and recorder metadata carry owned context snapshots. `EnvCfg.episode_context_in_info` defaults to true. Batched RL deployments may disable automatic info snapshots while retaining context lifecycle, recorder -metadata and explicit snapshots. Reset reuses the snapshot from `get_info()`; +metadata and explicit snapshots. Demo recording retains a reset-time metadata +template and materializes independent rows on access; row UUIDs and mutable +segment lists are owned by each episode. Reset reuses the snapshot from `get_info()`; overrides that omit it receive one when the option is enabled. The concrete preparation/restore host and snapshot limitations belong to [scene expansion](../scene-expansion/scene-expansion.md). diff --git a/agent_context/topics/simulation-system/articulation-adapters.md b/agent_context/topics/simulation-system/articulation-adapters.md index 2d449a0c08..92803f2ffc 100644 --- a/agent_context/topics/simulation-system/articulation-adapters.md +++ b/agent_context/topics/simulation-system/articulation-adapters.md @@ -49,6 +49,13 @@ DexSim's batch fetch and converts the root-pose layout once. Returned tensors are borrowed buffers, not snapshots; clone values that must survive later reads. Fetch after reset or direct writes rather than caching across them. +`Articulation.set_state()` writes selected root, joint and drive fields in one +backend operation. Current joint positions are clipped to joint limits. +Position targets are also clipped by default; `clamp_target_qpos=False` retains +raw PD targets while physical joint limits remain enabled. `set_qpos()` exposes +the same target-only option. Keep this distinction when a task records motor +commands for learned dynamics. + `Articulation.set_root_velocity()` writes selected world-frame linear and angular velocities as `(N, 6)` rows. The Scene adapter validates the complete input before writing one selected batch; other environment rows are preserved. @@ -75,6 +82,10 @@ output tensors. Public inertia/COM remain principal moments plus `xyz + xyzw`; scalar descriptors expose body-frame mat33. Runtime writes compose the pair before DexSim, and reset restores the saved pair together to avoid reordering principal axes between two writes. +COM writes pass environment rows and link columns directly to +`ArticulationBatch.apply_link_com_local_pose(rows=..., link_ids=...)`. Property +selection does not need a selected batch's joint/link state routing buffers; +DexSim retains descriptor updates and solver-constant refresh ownership. ## Mimic coupling diff --git a/agent_context/topics/simulation-system/physics-configuration.md b/agent_context/topics/simulation-system/physics-configuration.md index 2d243afe7f..1561fdcf5c 100644 --- a/agent_context/topics/simulation-system/physics-configuration.md +++ b/agent_context/topics/simulation-system/physics-configuration.md @@ -6,7 +6,7 @@ Return to the [simulation overview](simulation-system.md) for lifecycle entry. ## Backend, solver and device resolution `SimulationManagerCfg.physics_cfg` selects the backend by config type. -`cfg/simulation.py` owns `PhysicsBackendCfg`, `DefaultPhysicsCfg` and +`cfg/physics.py` owns `PhysicsBackendCfg`, `DefaultPhysicsCfg` and `NewtonPhysicsCfg`; do not add another backend string to the manager config. Gym's file-owned selector and composition rules belong to [environment configuration](../env-framework/configuration.md). @@ -21,13 +21,18 @@ device, not a separate environment tensor device. `None` retains the native default. Gym accepts the former in `physics_config` and the latter at the top level through `config_to_cfg()`. -Newton `solver_cfg=None` preserves DexSim's scene-aware `AutoSolverCfg`. -The concrete solver is resolved from the complete Spawn scene, then exposed -through `NewtonPhysicsBackend.solver_type`. Do not guess a solver-specific robot -preset while it is unresolved. Gradient mode requires an explicit -`semi_implicit` solver. Read exact solver options/defaults in -`cfg/simulation.py` and the installed DexSim config types; do not substitute a -hardcoded fallback when an expected DexSim API is absent. +Newton `solver_cfg=None` leaves the solver argument unset and preserves the +installed DexSim default. Current DexSim uses scene-aware `DexUniSolverCfg`; +older builds use `AutoSolverCfg`. Explicit `auto` mappings are accepted only +when that class is available. After its removal, migrate to `solver_cfg=None` +or `solver_type="dexuni"`; callers depending on an older automatic selection +must explicitly select that concrete solver. + +The scene-resolved solver is exposed through `NewtonPhysicsBackend.solver_type`. +Do not guess a solver-specific robot preset while it is unresolved. Gradient +mode requires an explicit `semi_implicit` solver. Read exact options/defaults in +`cfg/physics.py` and the installed DexSim config types. DexUni's +`joint_mode="dynamic"` remains a supported top-level solver option. `collision_cfg` owns the external Newton contact pipeline. Its `update_interval=None` updates at the first local solver substep; positive `k` diff --git a/docs/source/api_reference/embodichain/embodichain.lab.sim.cfg.rst b/docs/source/api_reference/embodichain/embodichain.lab.sim.cfg.rst index 6cfdcaa858..dfe3e4c175 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.sim.cfg.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.sim.cfg.rst @@ -177,6 +177,27 @@ without consumers. physics_backend_from_cfg validate_physics_cfg +Articulation configuration module +--------------------------------- + +Articulation configuration types also live in +``embodichain.lab.sim.cfg.articulation``. ``JointDrivePropertiesCfg`` separates +drive gains from Newton joint-limit gains through ``limit_stiffness`` and +``limit_damping``. Each accepts a scalar or joint-rule mapping; ``None`` keeps +the source/backend value. With Newton's MuJoCo solver, setting both to zero +selects the default MuJoCo limit response while retaining joint bounds. + +.. currentmodule:: embodichain.lab.sim.cfg.articulation + +.. autosummary:: + + ArticulationRootPropertiesCfg + LinkPhysicsOverrideCfg + link_attrs_from_dict + JointDrivePropertiesCfg + NewtonJointDrivePropertiesCfg + ArticulationCfg + Compatibility facade -------------------- diff --git a/docs/source/api_reference/embodichain/embodichain.lab.sim.shapes.rst b/docs/source/api_reference/embodichain/embodichain.lab.sim.shapes.rst index f2cf39198e..73938411a6 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.sim.shapes.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.sim.shapes.rst @@ -8,8 +8,8 @@ Overview Geometry configuration objects used to build collision, visual, and deformable simulation shapes. :class:`ShapeCfg` is the common base; :class:`MeshCfg`, -:class:`CubeCfg`, and :class:`SphereCfg` describe triangle-mesh, box, and -sphere primitives respectively. :class:`MeshCollisionCfg` explicitly selects +:class:`CubeCfg`, :class:`PlaneCfg`, and :class:`SphereCfg` describe triangle-mesh, +box, plane, and sphere primitives respectively. :class:`MeshCollisionCfg` explicitly selects the collision representation and its cooking settings, while :class:`LoadOption` controls mesh loading. @@ -18,6 +18,10 @@ surface deformables, the array-backed form preserves node order for particle flags and kinematic trajectories. Volume deformables generate a separate tetrahedral simulation mesh during voxelization. +``PlaneCfg`` creates an infinite collision plane on a static rigid object. +Its ``size`` controls only the visual width and length. The plane's local normal +is +Z; the rigid object's initial pose sets its world position and orientation. + .. rubric:: Type aliases .. autosummary:: @@ -32,6 +36,7 @@ tetrahedral simulation mesh during voxelization. LoadOption MeshCollisionCfg MeshCfg + PlaneCfg ShapeCfg SphereCfg @@ -61,6 +66,12 @@ tetrahedral simulation mesh during voxelization. :show-inheritance: :exclude-members: __init__, copy, replace, to_dict, validate +.. autoclass:: PlaneCfg + :members: + :undoc-members: + :show-inheritance: + :exclude-members: __init__, copy, replace, to_dict, validate + .. autoclass:: SphereCfg :members: :undoc-members: diff --git a/embodichain/lab/gym/envs/embodied_env.py b/embodichain/lab/gym/envs/embodied_env.py index 5812964e59..54456a0dc6 100644 --- a/embodichain/lab/gym/envs/embodied_env.py +++ b/embodichain/lab/gym/envs/embodied_env.py @@ -362,6 +362,40 @@ class EnvLightCfg: """ +class _EpisodeMetadataRows(Sequence): + """Retain reset-time metadata, materializing row copies when recording uses them.""" + + def __init__(self, count: int, template: dict[str, Any]) -> None: + pending = (copy.deepcopy(template),) + self._rows = [pending] * count + + def __len__(self) -> int: + return len(self._rows) + + def __getitem__(self, index: int | slice) -> dict[str, Any] | list[dict[str, Any]]: + if isinstance(index, slice): + return [self[i] for i in range(*index.indices(len(self)))] + record = self._rows[index] + if isinstance(record, tuple): + record = copy.deepcopy(record[0]) + episode_uuid = str(uuid4()) + record.update( + env_id=index % len(self), + episode_uuid=episode_uuid, + source_episode_uuid=episode_uuid, + ) + self._rows[index] = record + return record + + def __setitem__(self, index: int, record: dict[str, Any]) -> None: + self._rows[index] = record + + def reset_rows(self, ids: Sequence[int], template: dict[str, Any]) -> None: + pending = (copy.deepcopy(template),) + for index in ids: + self._rows[index] = pending + + @register_env("EmbodiedEnv-v1") class EmbodiedEnv(BaseEnv): """Embodied AI environment that is used to simulate the Embodied AI tasks. @@ -1456,8 +1490,15 @@ def _initialize_episode( if episode_metadata is not None and getattr( self, "_demo_metadata_initialized", True ): - for env_id in env_ids_to_process.cpu().tolist(): - episode_metadata[env_id] = self._new_demo_episode_metadata(env_id) + metadata_ids = env_ids_to_process.cpu().tolist() + if isinstance(episode_metadata, _EpisodeMetadataRows): + if metadata_ids: + episode_metadata.reset_rows( + metadata_ids, self._new_demo_episode_metadata(metadata_ids[0]) + ) + else: + for env_id in metadata_ids: + episode_metadata[env_id] = self._new_demo_episode_metadata(env_id) # Stateful managers reset selected rows before reset-mode events run. action_manager = getattr(self, "action_manager", None) if action_manager is not None: @@ -1615,9 +1656,9 @@ def _begin_demo_episode_recording( self._demo_active_segment_start_steps = self._demo_steps.clone() self._demo_active_rollout_start_steps = self.rollout_steps.clone() self._demo_metadata_initialized = True - self._demo_episode_metadata = [ - self._new_demo_episode_metadata(env_id) for env_id in range(self.num_envs) - ] + self._demo_episode_metadata = _EpisodeMetadataRows( + self.num_envs, self._new_demo_episode_metadata(0) + ) bridge = getattr(self, "_active_task_program_bridge", None) program = getattr(bridge, "_program", None) if program is not None: diff --git a/embodichain/lab/sim/cfg/articulation.py b/embodichain/lab/sim/cfg/articulation.py index aaee49cb9c..66b492c661 100644 --- a/embodichain/lab/sim/cfg/articulation.py +++ b/embodichain/lab/sim/cfg/articulation.py @@ -35,6 +35,15 @@ _rigid_body_physics_from_dict, ) +__all__ = [ + "ArticulationRootPropertiesCfg", + "LinkPhysicsOverrideCfg", + "link_attrs_from_dict", + "JointDrivePropertiesCfg", + "NewtonJointDrivePropertiesCfg", + "ArticulationCfg", +] + def _normalize_joint_target_mode(value: object) -> int: """Normalize a portable joint target mode to its backend integer value.""" @@ -280,6 +289,24 @@ class JointDrivePropertiesCfg: * For angular joints, the unit is kg-m^2/s/rad (N-m-s/rad). """ + limit_stiffness: Dict[str, float] | float | None = None + """Newton joint-limit restoring gain, separate from the drive stiffness. + + Scalars and joint-rule mappings follow :attr:`stiffness`. ``None`` keeps + the source/backend limit gain. With Newton's MuJoCo solver, explicitly + setting both limit gains to zero selects MuJoCo's default limit response + (``solref=[0.02, 1.0]``); it does not remove the joint limits. Other Newton + solvers interpret these gains according to their joint-limit model. + """ + + limit_damping: Dict[str, float] | float | None = None + """Newton joint-limit damping gain, separate from the drive damping. + + ``None`` keeps the source/backend value. See :attr:`limit_stiffness` for + the MuJoCo solver's explicit-zero behavior. The Default backend does not + consume either Newton limit gain. + """ + max_effort: Dict[str, float] | float | None = None """Maximum drive effort [N for prismatic, N*m for revolute joints]. diff --git a/embodichain/lab/sim/cfg/physics.py b/embodichain/lab/sim/cfg/physics.py index d630c7766f..e18f9c6e13 100644 --- a/embodichain/lab/sim/cfg/physics.py +++ b/embodichain/lab/sim/cfg/physics.py @@ -354,8 +354,9 @@ class NewtonPhysicsCfg(PhysicsBackendCfg): A mapping is converted to the matching DexSim Newton solver config. Include ``solver_type`` or ``class_type`` to select the solver, then add any parameters accepted by that DexSim solver config. If omitted, EmbodiChain - preserves DexSim's scene-aware ``AutoSolverCfg`` default. A DexSim build - exporting ``AutoSolverCfg`` is required; no concrete-solver fallback is used. + preserves the installed DexSim default. Current DexSim selects through + ``DexUniSolverCfg``; older builds use ``AutoSolverCfg``. Explicit ``auto`` + mappings require an installed DexSim build that still exports that class. """ collision_cfg: NewtonCollisionPipelineCfg | Mapping[str, Any] | None = field( @@ -379,8 +380,8 @@ def to_dexsim_cfg( gpu_id: int, ) -> NewtonCfg: """Convert this config to ``dexsim.engine.newton_physics.NewtonCfg``.""" + from dexsim.engine import newton_physics from dexsim.engine.newton_physics import ( - AutoSolverCfg, DexUniSolverCfg, FeatherstoneSolverCfg, MJWarpSolverCfg, @@ -401,7 +402,6 @@ def to_dexsim_cfg( ) solver_cfg_map: dict[str, type] = { - "auto": AutoSolverCfg, "dexuni": DexUniSolverCfg, "mujoco_warp": MJWarpSolverCfg, "xpbd": XPBDSolverCfg, @@ -409,6 +409,9 @@ def to_dexsim_cfg( "featherstone": FeatherstoneSolverCfg, "vbd": VBDSolverCfg, } + auto_solver_cfg = getattr(newton_physics, "AutoSolverCfg", None) + if auto_solver_cfg is not None: + solver_cfg_map["auto"] = auto_solver_cfg solver_cfg = _newton_solver_cfg_to_dexsim( solver_cfg=self.solver_cfg, solver_cfg_map=solver_cfg_map, @@ -419,8 +422,8 @@ def to_dexsim_cfg( ): logger.log_error( "Newton gradient mode requires an explicit " - "solver_type='semi_implicit'; AutoSolver does not select a " - "differentiable solver." + "solver_type='semi_implicit'; automatic solver selection does " + "not select a differentiable solver." ) collision_pipeline_cfg = None @@ -534,6 +537,13 @@ def _newton_solver_cfg_to_dexsim( or "auto" ) normalized_solver_type = _normalize_newton_solver_type(str(configured_solver_type)) + if normalized_solver_type == "auto" and "auto" not in solver_cfg_map: + logger.log_error( + "Installed DexSim does not provide AutoSolverCfg. Use " + "solver_cfg=None for its current scene-aware default, or select " + "solver_type='dexuni'. To preserve an older automatic choice, " + "select that concrete solver explicitly." + ) solver_cfg_type = solver_cfg_map[normalized_solver_type] if normalized_solver_type == "dexuni": try: diff --git a/embodichain/lab/sim/objects/articulation.py b/embodichain/lab/sim/objects/articulation.py index 3afb498ada..72637cc231 100644 --- a/embodichain/lab/sim/objects/articulation.py +++ b/embodichain/lab/sim/objects/articulation.py @@ -1124,6 +1124,7 @@ def _stabilize_newton_mimic_target_write( joint_ids: torch.Tensor, *, velocity: bool, + clamp_target_qpos: bool = True, ) -> None: """Update weak follower-drive targets for written mimic leaders. @@ -1137,6 +1138,7 @@ def _stabilize_newton_mimic_target_write( env_ids=env_ids, joint_ids=joint_ids, velocity=velocity, + clamp_target_qpos=clamp_target_qpos, mimic_ids=self.mimic_ids, mimic_parents=self.mimic_parents, mimic_multipliers=self.mimic_multipliers, @@ -1864,6 +1866,7 @@ def set_state( qf: torch.Tensor | None = None, root_velocity: torch.Tensor | None = None, clear_dynamics: bool = False, + clamp_target_qpos: bool = True, ) -> None: """Write selected physical state in one backend operation. @@ -1872,7 +1875,7 @@ def set_state( joint_ids: Joint columns for all supplied joint tensors. root_pose: Environment-local xyz + xyzw, shape (N, 7). qpos: Current joint positions, shape (N, J), clipped to limits. - target_qpos: Position targets, shape (N, J), clipped to limits. + target_qpos: Position targets, shape (N, J). qvel: Current joint velocities, shape (N, J). target_qvel: Velocity targets, shape (N, J). qf: Generalized joint efforts, shape (N, J). @@ -1880,6 +1883,9 @@ def set_state( clear_dynamics: Clear velocities, forces and solver history for selected environments and hold their final joint positions. Explicit fields override these defaults. + clamp_target_qpos: Clip position targets to joint limits. Set False + to preserve PD targets outside the joint range. Current joint + position writes remain clipped; physical limits stay enabled. Unspecified positions and unselected environments retain their values. Scene backends validate all field shapes before changing physics state. @@ -1910,7 +1916,7 @@ def set_state( raise ValueError( f"Expected {name} shape {expected}, got {tuple(value.shape)}." ) - if name in ("qpos", "target_qpos"): + if name == "qpos" or (name == "target_qpos" and clamp_target_qpos): if limits is None: limits = self.body_data.qpos_limits[local_env_ids][ :, local_joint_ids @@ -1938,7 +1944,11 @@ def set_state( for name, velocity in (("target_qpos", False), ("target_qvel", True)): if name in fields: self._stabilize_newton_mimic_target_write( - fields[name], local_env_ids, local_joint_ids, velocity=velocity + fields[name], + local_env_ids, + local_joint_ids, + velocity=velocity, + clamp_target_qpos=clamp_target_qpos, ) if ( (root_pose is not None or qpos is not None) @@ -1956,6 +1966,8 @@ def set_qpos( joint_ids: Sequence[int] | None = None, env_ids: Sequence[int] | None = None, target: bool = True, + *, + clamp_target_qpos: bool = True, ) -> None: """Set the joint positions (qpos) or target positions for the articulation. @@ -1964,6 +1976,9 @@ def set_qpos( joint_ids (Sequence[int] | None, optional): Joint indices to apply the positions. If None, applies to all joints. env_ids (Sequence[int] | None): Environment indices to apply the positions. Defaults to all environments. target (bool): If True, sets target positions for simulation. If False, updates current positions directly. + clamp_target_qpos: Clip position targets to joint limits. False + preserves out-of-range PD targets. Direct position writes + (target=False) are always clipped to joint limits. Raises: ValueError: If the length of `env_ids` does not match the length of `qpos`. @@ -1971,6 +1986,7 @@ def set_qpos( self.set_state( env_ids=env_ids, joint_ids=joint_ids, + clamp_target_qpos=clamp_target_qpos, **{"target_qpos" if target else "qpos": qpos}, ) diff --git a/embodichain/lab/sim/objects/backends/newton.py b/embodichain/lab/sim/objects/backends/newton.py index 33b1644044..af550741d8 100644 --- a/embodichain/lab/sim/objects/backends/newton.py +++ b/embodichain/lab/sim/objects/backends/newton.py @@ -90,6 +90,7 @@ def _stabilize_newton_mimic_target_write( qvel_limits: torch.Tensor, articulation_view: Any, device: torch.device, + clamp_target_qpos: bool = True, ) -> None: """Keep Newton mimic follower-drive targets on the commanded relation.""" if not configured: @@ -137,8 +138,9 @@ def _stabilize_newton_mimic_target_write( ) return - limits = qpos_limits[env_ids][:, follower_ids_tensor, :] - targets = targets.clamp(limits[..., 0], limits[..., 1]) + if clamp_target_qpos: + limits = qpos_limits[env_ids][:, follower_ids_tensor, :] + targets = targets.clamp(limits[..., 0], limits[..., 1]) articulation_view.apply_qpos( targets, env_ids, diff --git a/embodichain/lab/sim/objects/backends/scene.py b/embodichain/lab/sim/objects/backends/scene.py index 847a4f78f1..1a7967773b 100644 --- a/embodichain/lab/sim/objects/backends/scene.py +++ b/embodichain/lab/sim/objects/backends/scene.py @@ -618,10 +618,20 @@ def apply_link_com_pose( env_ids: Sequence[int] | torch.Tensor, link_ids: Sequence[int] | torch.Tensor, ) -> None: - """Convert selected COM poses from xyz + xyzw to batch xyzw + xyz.""" - self._apply_link_property( - "apply_link_com_local_pose", _batch_pose(values), env_ids, link_ids - ) + """Write selected COM frames through the batch's row-update interface. + + Inputs use ``xyz + xyzw`` in selection order. Passing rows directly + reuses the native batch binding and retains current principal moments. + """ + rows = self._select_rows(env_ids) + if len(rows) and len(link_ids): + _checked_batch_call( + self.batch, + "apply_link_com_local_pose", + _batch_pose(values), + link_ids=link_ids, + rows=rows, + ) def fetch_link_physical_properties( self, diff --git a/embodichain/lab/sim/objects/rigid_object.py b/embodichain/lab/sim/objects/rigid_object.py index e56ca6abc1..bc9c11d4ab 100644 --- a/embodichain/lab/sim/objects/rigid_object.py +++ b/embodichain/lab/sim/objects/rigid_object.py @@ -468,6 +468,7 @@ def _initialize_spawn_bound(self, result: Scene) -> None: cfg = deepcopy(self.cfg) self.__dict__.pop("user_ids", None) + self.__dict__.pop("_static_friction_batch", None) self._spawn_result = result self.body_type = cfg.body_type self._world = result.world @@ -1190,6 +1191,14 @@ def get_mass(self, env_ids: Sequence[int] | None = None) -> torch.Tensor: return torch.as_tensor(masses, dtype=torch.float32, device=self.device) + def _static_friction_view(self) -> SceneRigidBodyView: + """Create a shape-property view for Spawn bodies without dynamic state.""" + if not hasattr(self, "_static_friction_batch"): + self._static_friction_batch = SceneRigidBodyView.from_entities( + self._spawn_result, self._entities, self.device + ) + return self._static_friction_batch + def set_friction( self, friction: torch.Tensor, env_ids: Sequence[int] | None = None ) -> None: @@ -1206,6 +1215,13 @@ def set_friction( f"Length of env_ids {len(local_env_ids)} does not match friction length {len(friction)}." ) + if self.is_spawn_bound and self._data is None: + ids = torch.as_tensor(local_env_ids, dtype=torch.int32, device=self.device) + self._static_friction_view().apply_friction( + friction.to(dtype=torch.float32, device=self.device).unsqueeze(-1), ids + ) + return + if self._data is not None and self._data.body_view.is_ready: body_ids = self._data.body_ids_for(local_env_ids) self._data.body_view.apply_friction( @@ -1237,13 +1253,11 @@ def get_friction(self, env_ids: Sequence[int] | None = None) -> torch.Tensor: """ local_env_ids = self._all_indices if env_ids is None else env_ids - if self.is_spawn_bound and self.is_static: - return torch.full( - (len(local_env_ids),), - float(self.cfg.attrs.to_dexsim_physical_attr().dynamic_friction), - dtype=torch.float32, - device=self.device, - ) + if self.is_spawn_bound and self._data is None: + ids = torch.as_tensor(local_env_ids, dtype=torch.int32, device=self.device) + result = torch.empty((len(local_env_ids), 1), device=self.device) + self._static_friction_view().fetch_friction(result, ids) + return result.squeeze(-1) if self._data is not None and self._data.body_view.is_ready: body_ids = self._data.body_ids_for(local_env_ids) diff --git a/embodichain/lab/sim/objects/robot.py b/embodichain/lab/sim/objects/robot.py index be7bf157df..305a86aa12 100644 --- a/embodichain/lab/sim/objects/robot.py +++ b/embodichain/lab/sim/objects/robot.py @@ -595,6 +595,8 @@ def set_qpos( env_ids: Sequence[int] | None = None, target: bool = True, name: str | None = None, + *, + clamp_target_qpos: bool = True, ) -> None: """Set the joint positions (qpos) or target positions for the articulation. @@ -604,6 +606,9 @@ def set_qpos( env_ids (Sequence[int] | None): Environment indices to apply the positions. Defaults to all environments. target (bool): If True, sets target positions for simulation. If False, updates current positions directly. name (str | None): The name of the control part to set the qpos for. If None, the default part is used. + clamp_target_qpos: Clip position targets to joint limits. False + preserves out-of-range PD targets; direct position writes + remain clipped. Raises: ValueError: If the length of `env_ids` does not match the length of `qpos`. @@ -614,6 +619,7 @@ def set_qpos( joint_ids=joint_ids, env_ids=env_ids, target=target, + clamp_target_qpos=clamp_target_qpos, ) else: if not self.control_parts or name not in self.control_parts: @@ -629,6 +635,7 @@ def set_qpos( joint_ids=part_joint_ids, env_ids=env_ids, target=target, + clamp_target_qpos=clamp_target_qpos, ) def get_qpos(self, name: str | None = None, target: bool = False) -> torch.Tensor: diff --git a/embodichain/lab/sim/shapes.py b/embodichain/lab/sim/shapes.py index 2c2e4612b4..b4864342ec 100755 --- a/embodichain/lab/sim/shapes.py +++ b/embodichain/lab/sim/shapes.py @@ -36,6 +36,7 @@ "ShapeCfg", "MeshCfg", "CubeCfg", + "PlaneCfg", "SphereCfg", ] @@ -462,6 +463,20 @@ class CubeCfg(ShapeCfg): """Size of the cube (in m) as [length, width, height].""" +@configclass +class PlaneCfg(ShapeCfg): + """Infinite static collision plane with a finite visual surface. + + The plane lies in local XY with normal +Z. Use the rigid object's initial + pose to position or rotate it. Configure ``body_type="static"``. + """ + + shape_type: str = "Plane" + + size: List[float] = [1.0, 1.0] + """Visual width and length in metres; collision remains infinite.""" + + @configclass class SphereCfg(ShapeCfg): """Configuration parameters for a sphere shape.""" diff --git a/embodichain/lab/sim/spawn/descriptors.py b/embodichain/lab/sim/spawn/descriptors.py index a580e12397..b629ce3e7f 100644 --- a/embodichain/lab/sim/spawn/descriptors.py +++ b/embodichain/lab/sim/spawn/descriptors.py @@ -81,7 +81,13 @@ SurfaceDeformableObjectCfg, VolumeDeformableObjectCfg, ) -from embodichain.lab.sim.shapes import CubeCfg, MeshCfg, MeshCollisionCfg, SphereCfg +from embodichain.lab.sim.shapes import ( + CubeCfg, + MeshCfg, + MeshCollisionCfg, + PlaneCfg, + SphereCfg, +) from embodichain.utils import logger from embodichain.utils.string import ( resolve_matching_names, @@ -1175,6 +1181,8 @@ def _compile_joint_properties( "max_velocity", "friction", "armature", + "limit_stiffness", + "limit_damping", ): configured = getattr(source, property_name) if configured is None: @@ -1195,6 +1203,10 @@ def _compile_joint_properties( default_desc, newton_desc = joint_properties[joint_name] if property_name == "armature": joint_common[joint_name]["armature"] = scalar + elif property_name == "limit_stiffness": + newton_desc.limit_ke = scalar + elif property_name == "limit_damping": + newton_desc.limit_kd = scalar elif property_name == "max_effort": default_desc.max_force = scalar joint_common[joint_name]["effort_limit"] = scalar @@ -1688,6 +1700,16 @@ def _compile_geometry( raise ValueError("CubeCfg.size must contain three positive values.") return GeometryDesc.cube(size), CollisionApproximation.NONE, 1, "coacd" + if isinstance(shape, PlaneCfg): + if cfg.body_type != "static": + raise ValueError("PlaneCfg collision requires body_type='static'.") + size = tuple(float(value) for value in shape.size) + if len(size) != 2 or any( + not math.isfinite(value) or value <= 0 for value in size + ): + raise ValueError("PlaneCfg.size must contain two finite positive values.") + return GeometryDesc.plane(size), CollisionApproximation.NONE, 1, "coacd" + if isinstance(shape, SphereCfg): if shape.radius <= 0: raise ValueError("SphereCfg.radius must be positive.") @@ -1700,7 +1722,7 @@ def _compile_geometry( raise NotImplementedError( f"RigidObjectCfg shape {type(shape).__name__!r} is not supported by " - "the Spawn converter; supported shapes are MeshCfg, CubeCfg, and SphereCfg." + "the Spawn converter; supported shapes are MeshCfg, CubeCfg, PlaneCfg, and SphereCfg." ) diff --git a/tests/gym/envs/test_episode_context.py b/tests/gym/envs/test_episode_context.py index 32445ed8eb..7539be433a 100644 --- a/tests/gym/envs/test_episode_context.py +++ b/tests/gym/envs/test_episode_context.py @@ -161,3 +161,37 @@ def test_nested_empty_checks_leave_current_buffers_unchanged() -> None: inner(accepted=True) outer(accepted=False) assert context.snapshot() == before + + +@pytest.mark.parametrize("embodied", [False, True]) +def test_info_can_omit_provenance_without_disabling_episode_checks(embodied): + from types import SimpleNamespace + from embodichain.lab.gym.envs.base_env import BaseEnv, EnvCfg + from embodichain.lab.gym.envs.embodied_env import EmbodiedEnv + + assert EnvCfg().episode_context_in_info is True + context = ready_context() + env = SimpleNamespace( + cfg=SimpleNamespace(episode_context_in_info=True), + num_envs=4, + device=torch.device("cpu"), + _elapsed_steps=torch.zeros(4), + episode_context=context, + evaluate=lambda **kwargs: {}, + compute_task_state=lambda **kwargs: ( + torch.zeros(4, dtype=torch.bool), + torch.zeros(4, dtype=torch.bool), + {}, + ), + ) + get_info = EmbodiedEnv.get_info if embodied else BaseEnv.get_info + assert "episode_context" in get_info(env) + env.cfg.episode_context_in_info = False + with context.check("layout", [1]) as finish: + finish(accepted=False, reasons="invalid placement") + assert "episode_context" not in get_info(env) + assert context.filter_recordable().tolist() == [0, 2, 3] + assert ( + context.snapshot([1])[0]["rejections"]["layout"]["reason"] + == "invalid placement" + ) diff --git a/tests/gym/envs/test_recording_provenance.py b/tests/gym/envs/test_recording_provenance.py index 42ba96dd7a..3c03151031 100644 --- a/tests/gym/envs/test_recording_provenance.py +++ b/tests/gym/envs/test_recording_provenance.py @@ -269,3 +269,25 @@ def test_diagnostic_export_does_not_make_rejected_buffers_normal_recordings(tmp_ with pytest.raises(RuntimeError, match="rejected"): EmbodiedEnv.save_trajectory(env, str(tmp_path / "normal.pt"), env_ids=[0]) assert not (tmp_path / "normal.pt").exists() + + +def test_episode_metadata_rows_preserve_reset_time_context_and_row_isolation(): + from embodichain.lab.gym.envs.embodied_env import _EpisodeMetadataRows + + template = {"seed": 12, "provenance": {"labels": ["initial"]}, "segments": []} + rows = _EpisodeMetadataRows(3, template) + original = rows[0] + original["segments"].append({"name": "first"}) + template["seed"] = 35 + template["provenance"]["labels"].append("reset") + rows.reset_rows([0, 2], template) + template["seed"] = 99 + assert [row["seed"] for row in rows] == [35, 12, 35] + assert rows[0]["segments"] == [] and rows[1]["segments"] == [] + assert original["segments"] == [{"name": "first"}] + rows[0]["provenance"]["labels"].append("row zero") + assert rows[2]["provenance"]["labels"] == ["initial", "reset"] + assert len({row["episode_uuid"] for row in rows}) == 3 + for index, row in enumerate(rows): + assert row["env_id"] == index + assert row["source_episode_uuid"] == row["episode_uuid"] diff --git a/tests/sim/objects/test_articulation_mass_setters.py b/tests/sim/objects/test_articulation_mass_setters.py index a251131283..292f37f0af 100644 --- a/tests/sim/objects/test_articulation_mass_setters.py +++ b/tests/sim/objects/test_articulation_mass_setters.py @@ -24,11 +24,23 @@ import torch from embodichain.lab.sim.objects import Articulation +from embodichain.lab.sim.objects.backends.scene import SceneArticulationView from embodichain.utils.math import matrix_from_quat pytestmark = pytest.mark.no_sim +def property_view(count: int, status: int = 1) -> SceneArticulationView: + view = object.__new__(SceneArticulationView) + view.device = torch.device("cpu") + view._row_indices = torch.arange(count) + view.batch = Mock() + view.batch.select.return_value = view.batch + view.batch.apply_link_inertia_diagonal.return_value = status + view.batch.apply_link_com_local_pose.return_value = status + return view + + @pytest.mark.parametrize("newton", [False, True]) @pytest.mark.parametrize("selected", [False, True]) @pytest.mark.parametrize("property_name,width", [("inertia", 3), ("com_pose", 7)]) @@ -41,6 +53,7 @@ def test_spawn_mass_setters_preserve_selection_and_quaternion_order( articulation._data = SimpleNamespace( is_newton_backend=newton, link_names=["base", "tip"], + articulation_view=property_view(3), inertia=torch.tensor([1.0, 2.0, 3.0]).expand(3, 2, 3).clone(), com_pose=torch.tensor([0.0, 0.0, 0.0, 1.0, 2.0, 3.0, 4.0]) .expand(3, 2, 7) @@ -73,6 +86,33 @@ def test_spawn_mass_setters_preserve_selection_and_quaternion_order( values, link_names=link_names, env_ids=env_ids ) + if newton: + batch = articulation._data.articulation_view.batch + method = ( + "apply_link_inertia_diagonal" + if property_name == "inertia" + else "apply_link_com_local_pose" + ) + call = getattr(batch, method).call_args + expected = ( + original + if property_name == "inertia" + else torch.cat((original[..., 3:7], original[..., :3]), dim=-1) + ) + torch.testing.assert_close(call.args[0], expected) + assert torch.as_tensor(call.kwargs["link_ids"]).tolist() == ( + [1, 0] if selected else [0, 1] + ) + native_rows = ( + batch.select.call_args.args[0] + if property_name == "inertia" + else call.kwargs["rows"] + ) + assert torch.as_tensor(native_rows).tolist() == rows + assert all(not entity.mock_calls for entity in articulation._entities) + torch.testing.assert_close(values, original) + return + for i, env_id in enumerate(rows): entity = articulation._entities[env_id] calls = entity.set_link_inertia.call_args_list @@ -132,6 +172,7 @@ def test_spawn_mass_setters_raise_for_negative_native_status( articulation._data = SimpleNamespace( is_newton_backend=newton, link_names=["base"], + articulation_view=property_view(2, status), inertia=torch.ones(2, 1, 3), com_pose=torch.tensor([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]) .expand(2, 1, 7) @@ -146,9 +187,12 @@ def test_spawn_mass_setters_raise_for_negative_native_status( ).return_value = status articulation._entities = [Mock(), entity] - with pytest.raises( - RuntimeError, match=f"set_link_{property_name}.*env 1.*base.*{status}" - ): + message = ( + f"apply_link_.*{status}" + if newton + else f"set_link_{property_name}.*env 1.*base.*{status}" + ) + with pytest.raises(RuntimeError, match=message): getattr(articulation, "set_" + property_name)( torch.ones(1, 1, width), link_names="base", env_ids=[1] ) diff --git a/tests/sim/objects/test_articulation_property_batch.py b/tests/sim/objects/test_articulation_property_batch.py index df2cb79456..1ef8a2617f 100644 --- a/tests/sim/objects/test_articulation_property_batch.py +++ b/tests/sim/objects/test_articulation_property_batch.py @@ -22,7 +22,7 @@ import pytest import torch -from embodichain.lab.sim.objects.articulation import ArticulationData +from embodichain.lab.sim.objects.articulation import Articulation, ArticulationData from embodichain.lab.sim.objects.backends.scene import SceneArticulationView pytestmark = pytest.mark.no_sim @@ -113,3 +113,40 @@ def test_other_backend_keeps_one_scalar_read_per_link() -> None: torch.testing.assert_close( pose, torch.tensor([4.0, 5.0, 6.0, 0.0, 0.0, 0.0, 1.0]).expand(2, 2, 7) ) + + +@pytest.mark.parametrize("status", [2, -1, -2]) +def test_newton_com_write_preserves_selection_pose_and_native_status( + status: int, +) -> None: + art = object.__new__(Articulation) + art.device = torch.device("cpu") + art._spawn_result = object() + art._declared_num_instances = 3 + art._entities = [Mock(), Mock(), Mock()] + view = object.__new__(SceneArticulationView) + view.device = art.device + view.batch = Mock() + view._row_indices = torch.arange(3) + view.batch.apply_link_com_local_pose.return_value = status + art._data = SimpleNamespace( + is_newton_backend=True, + link_names=["base", "tip"], + articulation_view=view, + ) + pose = torch.tensor([1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.4]).repeat(2, 2, 1) + pose[..., 3:] /= torch.linalg.vector_norm(pose[..., 3:], dim=-1, keepdim=True) + original = pose.clone() + if status < 0: + with pytest.raises(RuntimeError, match=f"apply_link_com_local_pose.*{status}"): + art.set_com_pose(pose, link_names=["tip", "base"], env_ids=[2, 0]) + else: + art.set_com_pose(pose, link_names=["tip", "base"], env_ids=[2, 0]) + args, kwargs = view.batch.apply_link_com_local_pose.call_args + assert torch.as_tensor(kwargs["link_ids"]).tolist() == [1, 0] + assert torch.as_tensor(kwargs["rows"]).tolist() == [2, 0] + view.batch.select.assert_not_called() + torch.testing.assert_close(args[0][..., :4], original[..., 3:]) + torch.testing.assert_close(args[0][..., 4:], original[..., :3]) + torch.testing.assert_close(pose, original) + assert all(not entity.mock_calls for entity in art._entities) diff --git a/tests/sim/objects/test_articulation_state.py b/tests/sim/objects/test_articulation_state.py index f271ef50fa..5c3973f5b2 100644 --- a/tests/sim/objects/test_articulation_state.py +++ b/tests/sim/objects/test_articulation_state.py @@ -23,11 +23,13 @@ import torch from embodichain.lab.sim.objects.articulation import Articulation +from embodichain.lab.sim.objects.robot import Robot pytestmark = pytest.mark.no_sim -def test_combined_state_clamps_both_positions_before_one_write() -> None: +@pytest.mark.parametrize("clamp_targets", [True, False]) +def test_combined_state_position_limits(clamp_targets: bool) -> None: art = object.__new__(Articulation) art.device = torch.device("cpu") art._entities = [object()] * 3 @@ -45,6 +47,7 @@ def test_combined_state_clamps_both_positions_before_one_write() -> None: qpos=torch.tensor([[5.0, -3.0], [-5.0, 3.0]]), target_qpos=torch.tensor([[-4.0, 4.0], [4.0, -4.0]]), clear_dynamics=True, + clamp_target_qpos=clamp_targets, ) call = art._data.articulation_view.apply_state call.assert_called_once() @@ -56,7 +59,12 @@ def test_combined_state_clamps_both_positions_before_one_write() -> None: call.call_args.kwargs["qpos"], torch.tensor([[2.0, -1.0], [-2.0, 1.0]]) ) torch.testing.assert_close( - call.call_args.kwargs["target_qpos"], torch.tensor([[-2.0, 1.0], [2.0, -1.0]]) + call.call_args.kwargs["target_qpos"], + ( + torch.tensor([[-2.0, 1.0], [2.0, -1.0]]) + if clamp_targets + else torch.tensor([[-4.0, 4.0], [4.0, -4.0]]) + ), ) call.reset_mock() art._world.update.reset_mock() @@ -96,3 +104,63 @@ def test_cpu_transform_publication_only_for_direct_pose_writes( art._world.update.assert_called_once_with(0.0) else: art._world.update.assert_not_called() + + +@pytest.mark.parametrize("target", [True, False]) +@pytest.mark.parametrize("route", ["articulation", "robot", "robot_part"]) +def test_set_qpos_limits_with_unclamped_targets(target: bool, route: str) -> None: + art = object.__new__(Articulation if route == "articulation" else Robot) + art.device = torch.device("cpu") + art._entities = [object()] + art.__dict__["dof"] = 2 + art._data = SimpleNamespace( + is_newton_backend=True, + qpos_limits=torch.tensor([[[-1.0, 1.0], [-2.0, 2.0]]]), + articulation_view=SimpleNamespace(apply_state=Mock()), + ) + art._stabilize_newton_mimic_target_write = Mock() + selection = {"joint_ids": [1]} + if route == "robot_part": + art.cfg = SimpleNamespace(control_parts={"arm": ["elbow"]}) + art._joint_ids = {"arm": [1]} + selection = {"name": "arm"} + art.set_qpos(torch.tensor([[3.0]]), **selection) + torch.testing.assert_close( + art._data.articulation_view.apply_state.call_args.kwargs["target_qpos"], + torch.tensor([[2.0]]), + ) + art.set_qpos( + torch.tensor([[3.0]]), **selection, target=target, clamp_target_qpos=False + ) + torch.testing.assert_close( + art._data.articulation_view.apply_state.call_args.kwargs[ + "target_qpos" if target else "qpos" + ], + torch.tensor([[3.0 if target else 2.0]]), + ) + + +@pytest.mark.parametrize("clamp_targets", [True, False]) +def test_mimic_follower_preserves_target_limit_selection(clamp_targets: bool) -> None: + art = object.__new__(Articulation) + art.device = torch.device("cpu") + art._entities = [object()] + art.__dict__["dof"] = 2 + art._data = SimpleNamespace( + is_newton_backend=True, + qpos_limits=torch.tensor([[[-1.0, 1.0], [-2.0, 2.0]]]), + qvel_limits=torch.tensor([[10.0, 10.0]]), + articulation_view=SimpleNamespace(apply_state=Mock(), apply_qpos=Mock()), + ) + art._newton_mimic_compliance_configured = True + art._mimic_info = SimpleNamespace( + mimic_id=torch.tensor([1]), + mimic_parent=torch.tensor([0]), + mimic_multiplier=torch.tensor([3.0]), + mimic_offset=torch.tensor([0.0]), + ) + art.set_qpos(torch.tensor([[1.5]]), joint_ids=[0], clamp_target_qpos=clamp_targets) + torch.testing.assert_close( + art._data.articulation_view.apply_qpos.call_args.args[0], + torch.tensor([[2.0 if clamp_targets else 4.5]]), + ) diff --git a/tests/sim/objects/test_scene_backend.py b/tests/sim/objects/test_scene_backend.py index 4d895b1e73..742db8c1b1 100644 --- a/tests/sim/objects/test_scene_backend.py +++ b/tests/sim/objects/test_scene_backend.py @@ -117,6 +117,7 @@ def test_scene_views_match_installed_dexsim_batch_surface() -> None: "fetch_link_pose", "fetch_link_linear_velocity", "fetch_link_angular_velocity", + "apply_link_com_local_pose", "compute_kinematics", } articulation_metadata = { @@ -1313,14 +1314,18 @@ def test_selected_link_property_writes_preserve_layout_and_errors( batch = _ArticulationBatch() view = SceneArticulationView(_Scene("newton"), batch, torch.device("cpu")) selected = Mock() - call = getattr(selected, batch_method) - call.return_value = 1 + call = Mock(return_value=1) + setattr(batch if width == 7 else selected, batch_method, call) batch.select = Mock(return_value=selected) values = torch.arange(width or 1, dtype=torch.float32).reshape( (1, 1) if width is None else (1, 1, width) ) getattr(view, method)(values, [1], [0]) - torch.testing.assert_close(batch.select.call_args.args[0], torch.tensor([1])) + if width == 7: + batch.select.assert_not_called() + torch.testing.assert_close(call.call_args.kwargs["rows"], torch.tensor([1])) + else: + torch.testing.assert_close(batch.select.call_args.args[0], torch.tensor([1])) expected = _batch_pose(values) if width == 7 else values torch.testing.assert_close(call.call_args.args[0], expected) assert call.call_args.kwargs["link_ids"] == [0] @@ -1346,3 +1351,51 @@ def test_empty_link_property_selection_is_noop(method: str) -> None: }[method] getattr(view, method)(torch.empty((1, 0) + width), [1], []) batch.select.assert_not_called() + + +def test_spawn_static_friction_uses_live_batch_and_selected_rows(monkeypatch): + from embodichain.lab.sim.objects.rigid_object import RigidObject + + values = torch.tensor([[0.3], [0.6], [0.9]]) + + class View: + def apply_friction(self, data, ids): + values[ids.long()] = data + + def fetch_friction(self, out, ids): + out.copy_(values[ids.long()]) + + factory = Mock(return_value=View()) + monkeypatch.setattr(SceneRigidBodyView, "from_entities", factory) + rigid = RigidObject.__new__(RigidObject) + rigid._spawn_result = object() + rigid._entities = [object(), object(), object()] + rigid._data = None + rigid.device = torch.device("cpu") + rigid._all_indices = [0, 1, 2] + rigid.set_friction(torch.tensor([1.2, 0.4]), env_ids=[2, 0]) + torch.testing.assert_close(rigid.get_friction(), torch.tensor([0.4, 0.6, 1.2])) + first = rigid.get_friction([0]) + rigid.set_friction(torch.tensor([0.7]), env_ids=[0]) + torch.testing.assert_close(first, torch.tensor([0.4])) + assert factory.call_count == 1 + + # Rebinding a facade after a topology rebuild replaces its native batch. + from embodichain.lab.sim.objects import rigid_object as rigid_module + from embodichain.lab.sim.common import BatchEntity + + class RebuiltScene: + world = object() + + monkeypatch.setattr(rigid_module, "Scene", RebuiltScene) + monkeypatch.setattr(BatchEntity, "__init__", lambda *args, **kwargs: None) + rigid.cfg = SimpleNamespace(body_type="static") + rigid._declared_num_instances = 3 + rigid._apply_spawn_visual_uv = Mock() + rigid._initialize_existing_visual_material = Mock() + rigid._apply_initial_state = Mock() + rebuilt = RebuiltScene() + rigid._initialize_spawn_bound(rebuilt) + rigid.get_friction([0]) + assert factory.call_count == 2 + assert factory.call_args.args[0] is rebuilt diff --git a/tests/sim/spawn/test_descriptors.py b/tests/sim/spawn/test_descriptors.py index e441bfc913..093f162c1a 100644 --- a/tests/sim/spawn/test_descriptors.py +++ b/tests/sim/spawn/test_descriptors.py @@ -71,7 +71,7 @@ VolumeDeformableMeshingCfg, VolumeDeformableObjectCfg, ) -from embodichain.lab.sim.shapes import CubeCfg, LoadOption, MeshCfg +from embodichain.lab.sim.shapes import CubeCfg, LoadOption, MeshCfg, PlaneCfg from embodichain.lab.sim.objects import Articulation, RigidObject, RigidObjectGroup from embodichain.lab.sim.spawn.descriptors import ( articulation_desc_from_cfg, @@ -91,6 +91,49 @@ DEFORMABLE_MESH_PATH = "/assets/deformable.obj" +def test_plane_config_roundtrip_preserves_static_per_environment_surface() -> None: + from dexsim.spawn import GeometryType + + width, length, friction = 12.0, 8.0, 0.7 + cfg = RigidObjectCfg.from_dict( + { + "uid": "floor", + "shape": {"shape_type": "Plane", "size": [width, length]}, + "body_type": "static", + "init_pos": [0.0, 0.0, 0.0], + "attrs": {"material_props": {"dynamic_friction": friction}}, + } + ) + assert isinstance(cfg.shape, PlaneCfg) + descriptor, _ = rigid_desc_from_cfg(cfg, newton_solver_type="mujoco_warp") + collision = descriptor.collisions[0] + assert descriptor.per_env is True + assert descriptor.physics.actor_type == RigidBodyPhysicsDesc.static().actor_type + assert collision.geometry_type == GeometryType.PLANE + assert collision.size == (width, length, 0.0) + assert collision.axis == "z" + assert collision.approximation == CollisionApproximation.NONE + assert collision.newton.mu == pytest.approx(friction) + assert descriptor.renders[0].geometry_type == GeometryType.PLANE + np.testing.assert_array_equal(descriptor.pose[:3, 3], [0.0, 0.0, 0.0]) + + +@pytest.mark.parametrize("body_type", ["dynamic", "kinematic"]) +def test_plane_collision_rejects_moving_body(body_type: str) -> None: + cfg = RigidObjectCfg(uid="floor", shape=PlaneCfg(), body_type=body_type) + with pytest.raises(ValueError, match="requires body_type='static'"): + rigid_desc_from_cfg(cfg) + + +@pytest.mark.parametrize("size", [[1.0], [0.0, 1.0], [1.0, float("inf")]]) +def test_plane_visual_extent_requires_two_finite_positive_dimensions( + size: list[float], +) -> None: + cfg = RigidObjectCfg(uid="floor", shape=PlaneCfg(size=size), body_type="static") + with pytest.raises(ValueError, match="two finite positive"): + rigid_desc_from_cfg(cfg) + + def test_soft_descriptor_uses_newton_particle_schema() -> None: youngs = 1.0e5 poissons = 0.4 @@ -1640,6 +1683,75 @@ def test_articulation_array_qpos_limits_compile_before_backend_build() -> None: assert joint.upper_limit == pytest.approx(0.75) +def test_joint_limit_gains_preserve_source_when_unspecified() -> None: + descriptor = _resolved_articulation_desc() + joint = descriptor.get_joint_desc("arm_joint") + joint.newton = NewtonJointDesc(limit_ke=123.0, limit_kd=4.0) + cfg = ArticulationCfg( + uid="robot", + fpath="robot.urdf", + asset_physics_mode="overlay", + joint_drive_props=JointDrivePropertiesCfg(stiffness=10.0, damping=2.0), + ) + + configure_articulation_desc(descriptor, cfg, newton_solver_type="mjwarp") + + assert joint.newton.limit_ke == 123.0 + assert joint.newton.limit_kd == 4.0 + assert joint.newton.target_ke == 10.0 + assert joint.newton.target_kd == 2.0 + + +@pytest.mark.parametrize("solver_type", [None, "mjwarp"]) +def test_joint_limit_gains_keep_explicit_zero_and_joint_bounds( + solver_type: str | None, +) -> None: + descriptor = _resolved_articulation_desc() + joint = descriptor.get_joint_desc("arm_joint") + joint.newton = NewtonJointDesc(limit_ke=123.0, limit_kd=4.0) + original_bounds = (joint.lower_limit, joint.upper_limit) + cfg = ArticulationCfg( + uid="robot", + fpath="robot.urdf", + asset_physics_mode="overlay", + joint_drive_props=JointDrivePropertiesCfg( + stiffness=10.0, damping=2.0, limit_stiffness=0.0, limit_damping=0.0 + ), + ) + + configure_articulation_desc(descriptor, cfg, newton_solver_type=solver_type) + + assert joint.newton.limit_ke == 0.0 + assert joint.newton.limit_kd == 0.0 + assert (joint.lower_limit, joint.upper_limit) == original_bounds + assert joint.newton.target_ke == joint.dexsim.stiffness == 10.0 + assert joint.newton.target_kd == joint.dexsim.damping == 2.0 + + +def test_joint_limit_gain_rules_round_trip_and_override_control_parts() -> None: + properties = JointDrivePropertiesCfg.from_dict( + { + "limit_stiffness": {"arm": 100.0, "arm_.*": 20.0}, + "limit_damping": {"arm": 10.0, "arm_joint": 0.0}, + } + ) + restored = JointDrivePropertiesCfg.from_dict(properties.to_dict()) + cfg = RobotCfg( + uid="robot", + fpath="robot.urdf", + control_parts={"arm": ["arm_joint"]}, + joint_drive_props=restored, + ) + descriptor = _resolved_articulation_desc() + + configure_articulation_desc(descriptor, cfg, newton_solver_type="mjwarp") + + joint = descriptor.get_joint_desc("arm_joint") + assert joint.newton.limit_ke == 20.0 + assert joint.newton.limit_kd == 0.0 + assert restored.to_dict() == properties.to_dict() + + @pytest.mark.parametrize("solver_type", [None, "auto"]) @pytest.mark.parametrize("authored_velocity", [0.05, None]) def test_robot_default_drive_preserves_authored_velocity_limit( diff --git a/tests/sim/test_sim_manager_cfg.py b/tests/sim/test_sim_manager_cfg.py index 6adf8c2945..9faaaf24c4 100644 --- a/tests/sim/test_sim_manager_cfg.py +++ b/tests/sim/test_sim_manager_cfg.py @@ -124,15 +124,16 @@ def test_newton_physics_cfg_uses_device() -> None: @pytest.mark.no_sim -def test_newton_physics_cfg_preserves_dexsim_auto_solver_default() -> None: - from dexsim.engine.newton_physics import AutoSolverCfg +def test_newton_physics_cfg_preserves_dexsim_solver_default() -> None: + from dexsim.engine.newton_physics import NewtonCfg cfg = NewtonPhysicsCfg() dexsim_cfg = cfg.to_dexsim_cfg(gpu_id=0) - assert isinstance(dexsim_cfg.solver_cfg, AutoSolverCfg) - assert dexsim_cfg.solver_cfg.solver_type == "auto" + native_default = NewtonCfg().solver_cfg + assert type(dexsim_cfg.solver_cfg) is type(native_default) + assert dexsim_cfg.solver_cfg.solver_type == native_default.solver_type @pytest.mark.no_sim @@ -158,18 +159,22 @@ def test_newton_physics_cfg_forwards_render_sync_policy( @pytest.mark.no_sim -def test_newton_physics_cfg_requires_dexsim_auto_solver_api( +@pytest.mark.parametrize("solver_cfg", [None, {"solver_type": "mujoco_warp"}]) +def test_newton_physics_cfg_converts_without_auto_solver_api( monkeypatch: pytest.MonkeyPatch, + solver_cfg: dict[str, str] | None, ) -> None: from dexsim.engine import newton_physics - monkeypatch.delattr(newton_physics, "AutoSolverCfg") + monkeypatch.delattr(newton_physics, "AutoSolverCfg", raising=False) - with pytest.raises( - ImportError, - match="cannot import name 'AutoSolverCfg'", - ): - NewtonPhysicsCfg().to_dexsim_cfg(gpu_id=0) + native_cfg = NewtonPhysicsCfg(solver_cfg=solver_cfg).to_dexsim_cfg(gpu_id=0) + expected = ( + newton_physics.NewtonCfg().solver_cfg.solver_type + if solver_cfg is None + else "mujoco_warp" + ) + assert native_cfg.solver_cfg.solver_type == expected @pytest.mark.no_sim @@ -292,7 +297,7 @@ def test_newton_backend_exposes_resolved_solver_type() -> None: @pytest.mark.no_sim -def test_newton_backend_reports_scene_resolved_auto_solver( +def test_newton_backend_reports_scene_resolved_solver( monkeypatch: pytest.MonkeyPatch, ) -> None: world = object() @@ -308,7 +313,12 @@ def test_newton_backend_reports_scene_resolved_auto_solver( backend.configure_world(world_config, sim_config) - assert world_config.newton_cfg.solver_cfg.solver_type == "auto" + from dexsim.engine.newton_physics import NewtonCfg + + assert ( + world_config.newton_cfg.solver_cfg.solver_type + == NewtonCfg().solver_cfg.solver_type + ) assert backend.solver_type == "mujoco_warp" @@ -450,14 +460,31 @@ def test_newton_physics_cfg_converts_mapping_solver_cfg_to_dexsim_cfg() -> None: @pytest.mark.no_sim -def test_newton_physics_cfg_accepts_explicit_auto_solver_mapping() -> None: - from dexsim.engine.newton_physics import AutoSolverCfg +def test_newton_physics_cfg_explicit_auto_uses_installed_api() -> None: + from dexsim.engine import newton_physics cfg = NewtonPhysicsCfg(solver_cfg={"class_type": "AutoSolverCfg"}) + auto_solver_cfg = getattr(newton_physics, "AutoSolverCfg", None) + if auto_solver_cfg is None: + with pytest.raises(RuntimeError, match="AutoSolverCfg.*solver_cfg=None"): + cfg.to_dexsim_cfg(gpu_id=0) + else: + assert isinstance(cfg.to_dexsim_cfg(gpu_id=0).solver_cfg, auto_solver_cfg) - dexsim_cfg = cfg.to_dexsim_cfg(gpu_id=0) - assert isinstance(dexsim_cfg.solver_cfg, AutoSolverCfg) +@pytest.mark.no_sim +@pytest.mark.parametrize( + "solver_cfg", [{"solver_type": "auto"}, {"class_type": "AutoSolverCfg"}] +) +def test_newton_physics_cfg_reports_removed_auto_solver( + monkeypatch: pytest.MonkeyPatch, + solver_cfg: dict[str, str], +) -> None: + from dexsim.engine import newton_physics + + monkeypatch.delattr(newton_physics, "AutoSolverCfg", raising=False) + with pytest.raises(RuntimeError, match="AutoSolverCfg.*solver_cfg=None"): + NewtonPhysicsCfg(solver_cfg=solver_cfg).to_dexsim_cfg(gpu_id=0) @pytest.mark.no_sim From ce1e0834efdb49cf3eeea10685a4fe1b2e482e51 Mon Sep 17 00:00:00 2001 From: acrlw <13927622+acrlw@users.noreply.github.com> Date: Mon, 2 Nov 2026 10:29:52 +0800 Subject: [PATCH 08/14] test(gym): preserve task outcomes with optional episode context --- tests/gym/envs/test_configured_rl.py | 7 +++++-- tests/gym/envs/test_episode_context.py | 7 ++++++- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/tests/gym/envs/test_configured_rl.py b/tests/gym/envs/test_configured_rl.py index 85b64fed78..d93bb05b73 100644 --- a/tests/gym/envs/test_configured_rl.py +++ b/tests/gym/envs/test_configured_rl.py @@ -78,7 +78,10 @@ def _env(): env = object.__new__(EmbodiedEnv) env._num_envs = 2 env.cfg = SimpleNamespace( - rl_task=_task(), task_program=None, ignore_terminations=False + rl_task=_task(), + task_program=None, + ignore_terminations=False, + episode_context_in_info=True, ) env.pose = torch.eye(4).repeat(2, 1, 1) env.sim = SimpleNamespace( @@ -169,7 +172,7 @@ def test_empty_success_set_never_accepts(): def test_unconfigured_host_keeps_legacy_and_task_program_acceptance(task_program): env = _env() env.termination_manager = None - env.cfg = SimpleNamespace(task_program=task_program) + env.cfg = SimpleNamespace(task_program=task_program, episode_context_in_info=True) env.initialize_task_outcome([0]) info = env.get_info() assert "task_outcome" not in info diff --git a/tests/gym/envs/test_episode_context.py b/tests/gym/envs/test_episode_context.py index 7539be433a..6f2df6ba44 100644 --- a/tests/gym/envs/test_episode_context.py +++ b/tests/gym/envs/test_episode_context.py @@ -184,12 +184,17 @@ def test_info_can_omit_provenance_without_disabling_episode_checks(embodied): {}, ), ) + if embodied: + env.termination_manager = SimpleNamespace(snapshot=lambda: {"accepted": True}) get_info = EmbodiedEnv.get_info if embodied else BaseEnv.get_info assert "episode_context" in get_info(env) env.cfg.episode_context_in_info = False with context.check("layout", [1]) as finish: finish(accepted=False, reasons="invalid placement") - assert "episode_context" not in get_info(env) + info = get_info(env) + assert "episode_context" not in info + if embodied: + assert info["task_outcome"] == {"accepted": True} assert context.filter_recordable().tolist() == [0, 2, 3] assert ( context.snapshot([1])[0]["rejections"]["layout"]["reason"] From 9ed54ed42d6f9476f33b6180a68dc40ce4fcabda Mon Sep 17 00:00:00 2001 From: acrlw <13927622+acrlw@users.noreply.github.com> Date: Tue, 3 Nov 2026 18:10:51 +0800 Subject: [PATCH 09/14] fix(gym): preserve episode seed when recording is enabled --- embodichain/lab/gym/envs/embodied_env.py | 8 +++++ tests/gym/envs/test_demo.py | 33 ++++++++++++++----- .../test_articulation_inertia_frames.py | 2 +- 3 files changed, 33 insertions(+), 10 deletions(-) diff --git a/embodichain/lab/gym/envs/embodied_env.py b/embodichain/lab/gym/envs/embodied_env.py index 54456a0dc6..9d6007dabe 100644 --- a/embodichain/lab/gym/envs/embodied_env.py +++ b/embodichain/lab/gym/envs/embodied_env.py @@ -1487,6 +1487,14 @@ def _initialize_episode( ) -> None: env_ids_to_process = self.episode_context._ids(env_ids) episode_metadata = getattr(self, "_demo_episode_metadata", None) + if not getattr(self, "_demo_metadata_initialized", True) and ( + self.dataset_manager is not None + or getattr(self, "rollout_buffer", None) is not None + ): + episode_metadata = self._demo_episode_metadata = _EpisodeMetadataRows( + self.num_envs, self._new_demo_episode_metadata(0) + ) + self._demo_metadata_initialized = True if episode_metadata is not None and getattr( self, "_demo_metadata_initialized", True ): diff --git a/tests/gym/envs/test_demo.py b/tests/gym/envs/test_demo.py index c6179f2c16..af2c7928ba 100644 --- a/tests/gym/envs/test_demo.py +++ b/tests/gym/envs/test_demo.py @@ -21,7 +21,7 @@ from embodichain.lab.gym.envs.episode_context import EpisodeContext import threading -from types import SimpleNamespace +from types import MethodType, SimpleNamespace from typing import Any from unittest.mock import Mock @@ -1663,19 +1663,34 @@ def test_legacy_metadata_reports_consistent_episode_success() -> None: assert metadata["segments"][0]["instruction"] == task_instruction -@pytest.mark.parametrize("seed", (None, 0, 4101)) +@pytest.mark.parametrize("seed", (None, 0, 12)) def test_demo_metadata_exposes_the_effective_environment_seed(seed: int | None) -> None: - """Recorded metadata carries the effective environment seed for provenance.""" + """Finalizing an episode retains its seed after the next reset reseeds.""" env = _RolloutWriterStub() - env.cfg = SimpleNamespace(seed=seed) - env._demo_episode_metadata = [ - EmbodiedEnv._new_demo_episode_metadata(env, row) for row in range(env.num_envs) - ] - + env.cfg = SimpleNamespace( + seed=seed, events=None, observations=None, rewards=None, dataset=None + ) + env.dataset_manager = None + env._demo_metadata_initialized = False + env._demo_episode_metadata = [{} for _ in range(env.num_envs)] + env._new_demo_episode_metadata = MethodType( + EmbodiedEnv._new_demo_episode_metadata, env + ) + EmbodiedEnv._initialize_episode(env) + env.cfg.seed = 35 metadata = EmbodiedEnv.get_demo_episode_metadata(env, 1) - assert metadata["seed"] == seed assert metadata["env_id"] == 1 + EmbodiedEnv._initialize_episode(env, [1]) + assert EmbodiedEnv.get_demo_episode_metadata(env, 1)["seed"] == 35 + assert EmbodiedEnv.get_demo_episode_metadata(env, 0)["seed"] == seed + + env.rollout_buffer = None + env._demo_metadata_initialized = False + env._demo_episode_metadata = [{} for _ in range(env.num_envs)] + env._new_demo_episode_metadata = Mock(side_effect=AssertionError("unused metadata")) + EmbodiedEnv._initialize_episode(env) + assert not env._demo_metadata_initialized def test_explicit_segment_instructions_are_preserved() -> None: diff --git a/tests/sim/objects/test_articulation_inertia_frames.py b/tests/sim/objects/test_articulation_inertia_frames.py index b27f7fb7a9..690b5dd63f 100644 --- a/tests/sim/objects/test_articulation_inertia_frames.py +++ b/tests/sim/objects/test_articulation_inertia_frames.py @@ -53,7 +53,7 @@ def test_live_inertia_frames_and_partial_reset(tmp_path: Path, backend: str) -> sim = SimulationManager( SimulationManagerCfg( headless=True, - render_cfg=RenderCfg(renderer="no-render"), + render_cfg=RenderCfg(), device="cuda:0", num_envs=3, physics_cfg=physics_cfg_for_backend(backend), From 59c678c9b10e5e751ce419189bd792e31f969412 Mon Sep 17 00:00:00 2001 From: acrlw <13927622+acrlw@users.noreply.github.com> Date: Mon, 9 Nov 2026 09:33:52 +0800 Subject: [PATCH 10/14] fix(env): preserve episode context for existing configurations --- embodichain/lab/gym/envs/base_env.py | 7 +++++-- embodichain/lab/gym/envs/embodied_env.py | 2 +- tests/gym/envs/test_episode_context.py | 2 +- 3 files changed, 7 insertions(+), 4 deletions(-) diff --git a/embodichain/lab/gym/envs/base_env.py b/embodichain/lab/gym/envs/base_env.py index 1be7567e63..ea26e06e06 100644 --- a/embodichain/lab/gym/envs/base_env.py +++ b/embodichain/lab/gym/envs/base_env.py @@ -810,7 +810,7 @@ def get_info(self, **kwargs) -> TensorDict[str, Any]: evaluate = self.evaluate(**kwargs) if evaluate: info.update(evaluate) - if self.cfg.episode_context_in_info: + if getattr(self.cfg, "episode_context_in_info", True): info["episode_context"] = self.episode_context.snapshot() return info @@ -1029,7 +1029,10 @@ def reset( self._record_initial_observation(observation_frame, reset_ids) with self._profiler.section("get_info"): info = self.get_info(**options) - if self.cfg.episode_context_in_info and "episode_context" not in info: + if ( + getattr(self.cfg, "episode_context_in_info", True) + and "episode_context" not in info + ): info["episode_context"] = context.snapshot() except BaseException as error: context._abort_initialization(reset_ids, error) diff --git a/embodichain/lab/gym/envs/embodied_env.py b/embodichain/lab/gym/envs/embodied_env.py index 40076bcb50..1be56c950b 100644 --- a/embodichain/lab/gym/envs/embodied_env.py +++ b/embodichain/lab/gym/envs/embodied_env.py @@ -2736,7 +2736,7 @@ def get_info(self, **kwargs) -> Dict[str, Any]: outcome = getattr(self, "termination_manager", None) if outcome is not None: info["task_outcome"] = outcome.snapshot() - if self.cfg.episode_context_in_info: + if getattr(self.cfg, "episode_context_in_info", True): info["episode_context"] = self.episode_context.snapshot() return info diff --git a/tests/gym/envs/test_episode_context.py b/tests/gym/envs/test_episode_context.py index c0b574b96b..97d43af73c 100644 --- a/tests/gym/envs/test_episode_context.py +++ b/tests/gym/envs/test_episode_context.py @@ -173,7 +173,7 @@ def test_info_can_omit_provenance_without_disabling_episode_checks(embodied): assert EnvCfg().episode_context_in_info is True context = ready_context() env = SimpleNamespace( - cfg=SimpleNamespace(episode_context_in_info=True), + cfg=SimpleNamespace(), num_envs=4, device=torch.device("cpu"), _elapsed_steps=torch.zeros(4), From 75e4466e6a4f56999ff2e842d6ee2147942aa46b Mon Sep 17 00:00:00 2001 From: acrlw <13927622+acrlw@users.noreply.github.com> Date: Mon, 9 Nov 2026 10:58:40 +0800 Subject: [PATCH 11/14] fix(rl): preserve recording metadata and native scene settings --- embodichain/data/dataset.py | 2 - embodichain/lab/gym/envs/embodied_env.py | 1 + .../lab/sim/objects/backends/rigid_physics.py | 16 ++++ embodichain/lab/sim/objects/rigid_object.py | 14 ++++ embodichain/lab/sim/sim_manager.py | 9 +++ tests/data/test_dataset.py | 7 ++ tests/gym/envs/test_demo.py | 10 ++- tests/sim/objects/test_rigid_object.py | 73 +++++++++++++++++++ tests/sim/objects/test_scene_backend.py | 5 +- 9 files changed, 132 insertions(+), 5 deletions(-) diff --git a/embodichain/data/dataset.py b/embodichain/data/dataset.py index f41051bf65..7fb2f844ae 100644 --- a/embodichain/data/dataset.py +++ b/embodichain/data/dataset.py @@ -35,8 +35,6 @@ from embodichain.utils import logger -__all__ = ["EmbodiChainDataset", "get_data_class", "get_data_path"] - def _dataset_cache_paths(path: str, prefix: str) -> tuple[Path, Path]: """Resolve one cache entry and reject paths outside its configured root.""" diff --git a/embodichain/lab/gym/envs/embodied_env.py b/embodichain/lab/gym/envs/embodied_env.py index 1be56c950b..e6c0e61754 100644 --- a/embodichain/lab/gym/envs/embodied_env.py +++ b/embodichain/lab/gym/envs/embodied_env.py @@ -1563,6 +1563,7 @@ def _initialize_episode( if not getattr(self, "_demo_metadata_initialized", True) and ( self.dataset_manager is not None or getattr(self, "rollout_buffer", None) is not None + or getattr(self, "_traj_buffer", None) is not None ): episode_metadata = self._demo_episode_metadata = _EpisodeMetadataRows( self.num_envs, self._new_demo_episode_metadata(0) diff --git a/embodichain/lab/sim/objects/backends/rigid_physics.py b/embodichain/lab/sim/objects/backends/rigid_physics.py index 8bd3c20750..7fb294af2d 100644 --- a/embodichain/lab/sim/objects/backends/rigid_physics.py +++ b/embodichain/lab/sim/objects/backends/rigid_physics.py @@ -136,6 +136,22 @@ def get_legacy_friction(entity: object) -> Any: return entity.get_physical_body().get_dynamic_friction() +def _set_default_spawn_friction(entity: Any, value: float) -> None: + """Update a Default Spawn surface and retain its material across rebuilds.""" + from dexsim.spawn import DexsimCollisionDesc + + attr = entity.get_physical_attr() + attr.dynamic_friction = value + attr.static_friction = value + entity.set_physical_attr(attr) + entity.notify_desc_write() + for collision in entity.object_desc.collisions: + if collision.dexsim is None: + collision.dexsim = DexsimCollisionDesc() + collision.dexsim.dynamic_friction = value + collision.dexsim.static_friction = value + + def set_legacy_damping(entity: object, linear: float, angular: float) -> None: """Set legacy linear and angular damping.""" body = entity.get_physical_body() diff --git a/embodichain/lab/sim/objects/rigid_object.py b/embodichain/lab/sim/objects/rigid_object.py index 29eca60e94..e6a1a1e642 100644 --- a/embodichain/lab/sim/objects/rigid_object.py +++ b/embodichain/lab/sim/objects/rigid_object.py @@ -37,6 +37,7 @@ ) from embodichain.lab.sim.objects.backends.base import RigidBodyViewBase from embodichain.lab.sim.objects.backends.rigid_physics import ( + _set_default_spawn_friction, apply_legacy_physical_attr, can_use_newton_entity_dynamics_fallback, get_legacy_damping, @@ -1220,6 +1221,10 @@ def set_friction( ) if self.is_spawn_bound and self._data is None: + if self._spawn_result.backend != "newton": + for env_id, value in zip(local_env_ids, friction.tolist()): + _set_default_spawn_friction(self._entities[env_id], float(value)) + return ids = torch.as_tensor(local_env_ids, dtype=torch.int32, device=self.device) self._static_friction_view().apply_friction( friction.to(dtype=torch.float32, device=self.device).unsqueeze(-1), ids @@ -1258,6 +1263,15 @@ def get_friction(self, env_ids: Sequence[int] | None = None) -> torch.Tensor: local_env_ids = self._all_indices if env_ids is None else env_ids if self.is_spawn_bound and self._data is None: + if self._spawn_result.backend != "newton": + return torch.tensor( + [ + self._entities[env_id].get_physical_attr().dynamic_friction + for env_id in local_env_ids + ], + dtype=torch.float32, + device=self.device, + ) ids = torch.as_tensor(local_env_ids, dtype=torch.int32, device=self.device) result = torch.empty((len(local_env_ids), 1), device=self.device) self._static_friction_view().fetch_friction(result, ids) diff --git a/embodichain/lab/sim/sim_manager.py b/embodichain/lab/sim/sim_manager.py index 789e954d73..62af4f8d60 100644 --- a/embodichain/lab/sim/sim_manager.py +++ b/embodichain/lab/sim/sim_manager.py @@ -2320,6 +2320,15 @@ def configure_ground_plane_collision( collision.enable_collision = enabled if newton is not None: collision.newton = newton + # Default materializes the ground during declaration. Redeclaring also + # updates its native collision and render geometry before prepare(). + self._spawn_scene.remove("default_plane") + self._spawn_scene.declare( + "rigid_object", "default_plane", self._spawn_default_plane_descriptor + ) + handles = self._spawn_scene.handles("default_plane") + if handles: + self._bind_default_plane(handles[0]) def set_default_global_lighting(self) -> None: """Set default global lighting for the scene. diff --git a/tests/data/test_dataset.py b/tests/data/test_dataset.py index 86bb9eee2f..49f2808e51 100644 --- a/tests/data/test_dataset.py +++ b/tests/data/test_dataset.py @@ -34,6 +34,13 @@ PROCESS_START_TIMEOUT_SECONDS = 60.0 +@pytest.mark.no_sim +def test_default_data_modules_remains_a_public_export() -> None: + from embodichain.data import DEFAULT_DATA_MODULES + + assert DEFAULT_DATA_MODULES is dataset_module.DEFAULT_DATA_MODULES + + def _check_cache( root: Path, prefix: str = "TestAsset", expected: str = "valid" ) -> None: diff --git a/tests/gym/envs/test_demo.py b/tests/gym/envs/test_demo.py index acee74a368..b8f9434f7a 100644 --- a/tests/gym/envs/test_demo.py +++ b/tests/gym/envs/test_demo.py @@ -1793,13 +1793,20 @@ def test_legacy_metadata_reports_consistent_episode_success() -> None: @pytest.mark.parametrize("seed", (None, 0, 12)) -def test_demo_metadata_exposes_the_effective_environment_seed(seed: int | None) -> None: +@pytest.mark.parametrize("recorder", ("rollout", "trajectory")) +def test_demo_metadata_exposes_the_effective_environment_seed( + seed: int | None, recorder: str +) -> None: """Finalizing an episode retains its seed after the next reset reseeds.""" env = _RolloutWriterStub() env.cfg = SimpleNamespace( seed=seed, events=None, observations=None, rewards=None, dataset=None ) env.dataset_manager = None + if recorder == "trajectory": + env._traj_buffer = env.rollout_buffer + env.rollout_buffer = None + env._demo_steps = env.rollout_steps.clone() env._demo_metadata_initialized = False env._demo_episode_metadata = [{} for _ in range(env.num_envs)] env._new_demo_episode_metadata = MethodType( @@ -1815,6 +1822,7 @@ def test_demo_metadata_exposes_the_effective_environment_seed(seed: int | None) assert EmbodiedEnv.get_demo_episode_metadata(env, 0)["seed"] == seed env.rollout_buffer = None + env._traj_buffer = None env._demo_metadata_initialized = False env._demo_episode_metadata = [{} for _ in range(env.num_envs)] env._new_demo_episode_metadata = Mock(side_effect=AssertionError("unused metadata")) diff --git a/tests/sim/objects/test_rigid_object.py b/tests/sim/objects/test_rigid_object.py index b064bdf3e9..6338fea50e 100644 --- a/tests/sim/objects/test_rigid_object.py +++ b/tests/sim/objects/test_rigid_object.py @@ -52,6 +52,79 @@ NEWTON_INERTIA_ROUND_TRIP_ATOL = 2e-4 +@pytest.mark.requires_sim +@pytest.mark.gpu +@pytest.mark.parametrize("backend", ["default", "newton"]) +def test_static_friction_preserves_selected_rows_across_rebuild(backend: str) -> None: + sim = SimulationManager( + SimulationManagerCfg( + headless=True, + device="cuda:0", + num_envs=3, + physics_cfg=physics_cfg_for_backend(backend), + ) + ) + surface = extra = None + try: + surface = sim.add_rigid_object( + RigidObjectCfg(uid="surface", body_type="static", shape=CubeCfg()) + ) + sim.prepare() + expected = surface.get_friction().clone() + selected = torch.tensor([0.7, 0.2], device=sim.device) + surface.set_friction(selected, env_ids=[2, 0]) + expected[[2, 0]] = selected + torch.testing.assert_close(surface.get_friction(), expected) + torch.testing.assert_close(surface.get_friction([2, 0]), selected) + extra = sim.add_rigid_object( + RigidObjectCfg(uid="extra", shape=CubeCfg(), init_pos=(0, 0, 2)) + ) + sim.prepare() + torch.testing.assert_close(surface.get_friction(), expected) + finally: + surface = extra = None + sim.destroy(exit_process=False) + SimulationManager.flush_cleanup_queue() + + +@pytest.mark.requires_sim +@pytest.mark.gpu +@pytest.mark.parametrize("backend", ["default", "newton"]) +@pytest.mark.parametrize("change", ["disabled", "geometry"]) +def test_ground_configuration_changes_live_collision(backend: str, change: str) -> None: + from dexsim.spawn import GeometryDesc + + sim = SimulationManager( + SimulationManagerCfg( + headless=True, + device="cuda:0", + physics_cfg=physics_cfg_for_backend(backend), + ) + ) + falling = None + try: + if change == "disabled": + sim.configure_ground_plane_collision(enabled=False) + else: + sim.configure_ground_plane_collision(geometry=GeometryDesc.cube((2, 2, 1))) + falling = sim.add_rigid_object( + RigidObjectCfg( + uid="falling", shape=CubeCfg(size=(0.1, 0.1, 0.1)), init_pos=(0, 0, 1) + ) + ) + sim.prepare() + sim.update(step=240) + height = float(falling.get_local_pose()[0, 2]) + if change == "disabled": + assert height < -1.0 + else: + assert height == pytest.approx(0.55, abs=0.02) + finally: + falling = None + sim.destroy(exit_process=False) + SimulationManager.flush_cleanup_queue() + + @pytest.mark.parametrize("device", ["cpu", "cuda:0"]) @pytest.mark.parametrize("operation", ["initialization", "reset", "pose"]) def test_rigid_state_writes_do_not_advance_other_bodies( diff --git a/tests/sim/objects/test_scene_backend.py b/tests/sim/objects/test_scene_backend.py index 742db8c1b1..ede7d95d4e 100644 --- a/tests/sim/objects/test_scene_backend.py +++ b/tests/sim/objects/test_scene_backend.py @@ -1354,6 +1354,7 @@ def test_empty_link_property_selection_is_noop(method: str) -> None: def test_spawn_static_friction_uses_live_batch_and_selected_rows(monkeypatch): + from embodichain.lab.sim.objects import rigid_object as rigid_module from embodichain.lab.sim.objects.rigid_object import RigidObject values = torch.tensor([[0.3], [0.6], [0.9]]) @@ -1368,7 +1369,7 @@ def fetch_friction(self, out, ids): factory = Mock(return_value=View()) monkeypatch.setattr(SceneRigidBodyView, "from_entities", factory) rigid = RigidObject.__new__(RigidObject) - rigid._spawn_result = object() + rigid._spawn_result = SimpleNamespace(backend="newton") rigid._entities = [object(), object(), object()] rigid._data = None rigid.device = torch.device("cpu") @@ -1381,11 +1382,11 @@ def fetch_friction(self, out, ids): assert factory.call_count == 1 # Rebinding a facade after a topology rebuild replaces its native batch. - from embodichain.lab.sim.objects import rigid_object as rigid_module from embodichain.lab.sim.common import BatchEntity class RebuiltScene: world = object() + backend = "newton" monkeypatch.setattr(rigid_module, "Scene", RebuiltScene) monkeypatch.setattr(BatchEntity, "__init__", lambda *args, **kwargs: None) From 2c2baa4bae7443f9c48fdcc4118c12a3a5cd2819 Mon Sep 17 00:00:00 2001 From: acrlw <13927622+acrlw@users.noreply.github.com> Date: Tue, 10 Nov 2026 09:52:52 +0800 Subject: [PATCH 12/14] fix(sim): preserve episode provenance and reset graph motors --- .../topics/env-framework/env-framework.md | 5 +- embodichain/lab/gym/envs/embodied_env.py | 19 ++- embodichain/lab/sim/_joint_motor.py | 67 +++++++++- tests/gym/envs/test_env_seed.py | 34 +++++ tests/sim/test_joint_motor.py | 21 ++- tests/sim/test_joint_motor_reset.py | 123 ++++++++++++++++++ 6 files changed, 262 insertions(+), 7 deletions(-) create mode 100644 tests/sim/test_joint_motor_reset.py diff --git a/agent_context/topics/env-framework/env-framework.md b/agent_context/topics/env-framework/env-framework.md index 0d69113dc2..e9f9e494b5 100644 --- a/agent_context/topics/env-framework/env-framework.md +++ b/agent_context/topics/env-framework/env-framework.md @@ -207,8 +207,9 @@ Failed checks remain attached to buffers until the selected rows are consumed and a new episode begins. Info and recorder metadata carry owned context snapshots. `EnvCfg.episode_context_in_info` defaults to true. Batched RL deployments may disable automatic info snapshots while retaining context lifecycle, recorder -metadata and explicit snapshots. Demo recording retains a reset-time metadata -template and materializes independent rows on access; row UUIDs and mutable +metadata and explicit snapshots. Reset retains each row's seed and shared +provenance snapshot even without a recorder. Demo recording materializes +independent metadata rows on access; row UUIDs and mutable segment lists are owned by each episode. Reset reuses the snapshot from `get_info()`; overrides that omit it receive one when the option is enabled. The concrete preparation/restore host and snapshot limitations belong to [scene expansion](../scene-expansion/scene-expansion.md). diff --git a/embodichain/lab/gym/envs/embodied_env.py b/embodichain/lab/gym/envs/embodied_env.py index e6c0e61754..f4a73e3777 100644 --- a/embodichain/lab/gym/envs/embodied_env.py +++ b/embodichain/lab/gym/envs/embodied_env.py @@ -1559,6 +1559,20 @@ def _initialize_episode( self, env_ids: Sequence[int] | torch.Tensor | None = None, **kwargs: Any ) -> None: env_ids_to_process = self.episode_context._ids(env_ids) + # Keep one shared provenance snapshot per seed epoch, even without a + # recorder. Individual episode dictionaries and UUIDs stay lazy. + seed = getattr(self.cfg, "seed", None) + snapshot = getattr(self, "_demo_reset_provenance", None) + if snapshot is None or snapshot["seed"] != seed: + self._recording_provenance = build_recording_provenance(self) + snapshot = {"seed": seed, **self._recording_provenance} + self._demo_reset_provenance = snapshot + rows = getattr(self, "_demo_episode_provenance", None) + if rows is None: + rows = self._demo_episode_provenance = [snapshot] * self.num_envs + metadata_ids = env_ids_to_process.cpu().tolist() + for env_id in metadata_ids: + rows[env_id] = snapshot episode_metadata = getattr(self, "_demo_episode_metadata", None) if not getattr(self, "_demo_metadata_initialized", True) and ( self.dataset_manager is not None @@ -1572,7 +1586,6 @@ def _initialize_episode( if episode_metadata is not None and getattr( self, "_demo_metadata_initialized", True ): - metadata_ids = env_ids_to_process.cpu().tolist() if isinstance(episode_metadata, _EpisodeMetadataRows): if metadata_ids: episode_metadata.reset_rows( @@ -1711,7 +1724,9 @@ def _new_demo_episode_metadata(self, env_id: int) -> dict[str, Any]: "terminal_reason": "unknown", "segments": [], } - metadata.update(copy.deepcopy(self._recording_provenance)) + rows = getattr(self, "_demo_episode_provenance", None) + provenance = rows[env_id] if rows is not None else self._recording_provenance + metadata.update(copy.deepcopy(provenance)) expert_action_spec = getattr(self, "expert_action_spec", None) if expert_action_spec is not None: metadata.update(expert_action_spec.metadata(step_dt=self.step_dt)) diff --git a/embodichain/lab/sim/_joint_motor.py b/embodichain/lab/sim/_joint_motor.py index 09dcc288c2..b054dc675d 100644 --- a/embodichain/lab/sim/_joint_motor.py +++ b/embodichain/lab/sim/_joint_motor.py @@ -17,6 +17,7 @@ from __future__ import annotations +from collections.abc import Callable from typing import Any import numpy as np @@ -67,6 +68,21 @@ def _dc_motor( previous_velocity[row, column] = velocity +@wp.kernel(enable_backward=False) +def _reset_motor_rows( + rows: wp.array(dtype=wp.int32), + initial_targets: wp.array(dtype=wp.float32), + targets: wp.array2d(dtype=wp.float32), + effort: wp.array2d(dtype=wp.float32), + velocity: wp.array2d(dtype=wp.float32), +): + selected, column = wp.tid() + row = rows[selected] + targets[row, column] = initial_targets[column] + effort[row, column] = 0.0 + velocity[row, column] = 0.0 + + class _BatchedJointDCMotorControl(GraphRuntimeControl): """Hold DC motor effort over each existing outer physics step.""" @@ -76,13 +92,17 @@ def __init__(self, robot: dict[str, Any], target_paths: tuple[str, ...]) -> None self.num_envs = len(target_paths) self.targets = self.effort = self.velocity = None self._claims = () + self._arena_indices = () + self._initial_targets = None def initialize(self, context: RuntimeControlContext) -> None: """Resolve scalar joints once in the manager-provided arena order.""" names = self.robot["joint_names"] - q, qd, claims = [], [], [] + q, qd, claims, arenas = [], [], [], [] for path in self.target_paths: - articulation = context.result.get_articulation(path)._physics_binding + spawned = context.result.get_articulation(path) + arenas.append(spawned._arena_index) + articulation = spawned._physics_binding joints = {joint.name: joint for joint in articulation.public_joints} selected = [joints[name] for name in names] if any(joint.q_size != 1 or joint.qd_size != 1 for joint in selected): @@ -104,6 +124,12 @@ def initialize(self, context: RuntimeControlContext) -> None: ), device=context.device, ) + self._arena_indices = tuple(arenas) + self._initial_targets = wp.array( + self.robot["default_joint_position"], + dtype=wp.float32, + device=context.device, + ) self.effort = wp.zeros_like(self.targets) self.velocity = wp.zeros_like(self.targets) self._claims = tuple(claims) @@ -112,6 +138,41 @@ def exclusive_resource_claims(self) -> tuple[object, ...]: """Reserve the driven joints through the existing control registry.""" return self._claims + def prepare_episode_reset( + self, arena_indices: tuple[int, ...] + ) -> Callable[[], None]: + """Prepare an in-place reset of selected arenas without changing state. + + Args: + arena_indices: Scene arena indices, independent of policy row order. + + Returns: + Callback restoring targets and clearing selected motor history. + """ + if self.targets is None: + raise RuntimeError("DC motor control is not initialized") + selected = set(arena_indices) + rows = [i for i, arena in enumerate(self._arena_indices) if arena in selected] + if not rows: + return lambda: None + row_indices = wp.array(rows, dtype=wp.int32, device=self.targets.device) + + def reset() -> None: + wp.launch( + _reset_motor_rows, + dim=(len(rows), self.targets.shape[1]), + inputs=[ + row_indices, + self._initial_targets, + self.targets, + self.effort, + self.velocity, + ], + device=self.targets.device, + ) + + return reset + def __call__( self, context: RuntimeControlContext, @@ -153,3 +214,5 @@ def close(self) -> None: self.kp = self.kd = self.limits = self.speeds = None self.targets = self.effort = self.velocity = None self._claims = () + self._arena_indices = () + self._initial_targets = None diff --git a/tests/gym/envs/test_env_seed.py b/tests/gym/envs/test_env_seed.py index 2b72d09699..f1f8669b9d 100644 --- a/tests/gym/envs/test_env_seed.py +++ b/tests/gym/envs/test_env_seed.py @@ -273,3 +273,37 @@ def test_episode_context_info_is_optional_without_changing_lifecycle( assert [row["episode"] for row in saved] == [0, 0] assert [row["episode"] for row in snapshot()] == [0, 1] assert env.episode_context.filter_recordable([0, 1]).tolist() == [0, 1] + + +@pytest.mark.parametrize("recording", [False, True]) +@pytest.mark.parametrize("read_order", [(0, 1), (1, 0)]) +def test_lazy_metadata_retains_each_rows_reset_seed(recording, read_order): + from types import MethodType + from embodichain.data_pipeline.recording import stable_config_hash + + env = _ResetEnv(2) + env.cfg.events = env.cfg.observations = env.cfg.rewards = env.cfg.dataset = None + env.dataset_manager = object() if recording else None + env.rollout_buffer = env._traj_buffer = None + env._demo_episode_metadata = [{}, {}] + env._demo_metadata_initialized = False + env._demo_steps = torch.zeros(2, dtype=torch.long) + env._initialize_episode = MethodType(EmbodiedEnv._initialize_episode, env) + env._new_demo_episode_metadata = MagicMock( + wraps=MethodType(EmbodiedEnv._new_demo_episode_metadata, env) + ) + env.reset(seed=12) + original_hash = stable_config_hash(env.cfg) + env.reset(seed=35, options={"reset_ids": [0]}) + if not recording: + env._new_demo_episode_metadata.assert_not_called() + records = {i: EmbodiedEnv.get_demo_episode_metadata(env, i) for i in read_order} + assert [records[i]["seed"] for i in range(2)] == [35, 12] + assert records[1]["config_hash"] == original_hash + assert records[0]["config_hash"] == stable_config_hash(env.cfg) + assert records[0]["episode_uuid"] != records[1]["episode_uuid"] + old_uuid = records[1]["episode_uuid"] + env.reset(options={"reset_ids": [0]}) + assert EmbodiedEnv.get_demo_episode_metadata(env, 1)["episode_uuid"] == old_uuid + records[1]["provenance"].clear() + assert EmbodiedEnv.get_demo_episode_metadata(env, 1)["provenance"] diff --git a/tests/sim/test_joint_motor.py b/tests/sim/test_joint_motor.py index caee4bde1c..8224270d5b 100644 --- a/tests/sim/test_joint_motor.py +++ b/tests/sim/test_joint_motor.py @@ -54,7 +54,7 @@ def test_motor_binds_public_names_and_holds_effort_between_solver_substeps() -> for index, (name, q, qd) in enumerate([("a", 7, 6), ("b", 9, 8)]) ] articulations[f"arena_{arena}/robot"] = SimpleNamespace( - _physics_binding=SimpleNamespace(public_joints=joints) + _arena_index=arena, _physics_binding=SimpleNamespace(public_joints=joints) ) context = SimpleNamespace( result=SimpleNamespace(get_articulation=articulations.__getitem__), @@ -87,6 +87,25 @@ def test_motor_binds_public_names_and_holds_effort_between_solver_substeps() -> ) control(context, 0, 2, 0.0025) torch.testing.assert_close(wp.to_torch(control.effort), torch.zeros(2, 2)) + # Arena 0 maps to policy row 1. Preflight must not mutate captured buffers. + control.targets.fill_(7.0) + control.effort.fill_(8.0) + control.velocity.fill_(9.0) + buffers = (control.targets, control.effort, control.velocity) + pointers = [value.ptr for value in buffers] + commit = control.prepare_episode_reset((0,)) + assert [value.numpy().tolist() for value in buffers] == [ + [[7.0, 7.0], [7.0, 7.0]], + [[8.0, 8.0], [8.0, 8.0]], + [[9.0, 9.0], [9.0, 9.0]], + ] + for _ in range(2): + commit() + control.prepare_episode_reset(())() + assert [value.ptr for value in buffers] == pointers + assert control.targets.numpy().tolist() == [[7.0, 7.0], [1.0, -1.0]] + assert control.effort.numpy().tolist() == [[8.0, 8.0], [0.0, 0.0]] + assert control.velocity.numpy().tolist() == [[9.0, 9.0], [0.0, 0.0]] control.close() assert control.targets is None and control.exclusive_resource_claims() == () diff --git a/tests/sim/test_joint_motor_reset.py b/tests/sim/test_joint_motor_reset.py new file mode 100644 index 0000000000..1e957dcc5b --- /dev/null +++ b/tests/sim/test_joint_motor_reset.py @@ -0,0 +1,123 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- +"""Selective SDK scene reset preserves captured DC motor buffers.""" + +from __future__ import annotations + +from pathlib import Path +import pytest +import torch +import warp as wp +from dexsim.engine.newton_physics.backend_registry import get_newton_backend +from embodichain.lab.sim import SimulationManager, SimulationManagerCfg +from embodichain.lab.sim.cfg import ( + ArticulationCfg, + JointDrivePropertiesCfg, + NewtonPhysicsCfg, + RenderCfg, +) + +pytestmark = [pytest.mark.gpu, pytest.mark.requires_sim] + + +def test_scene_reset_with_graph_motor_preserves_unselected_arena( + tmp_path: Path, +) -> None: + source = tmp_path / "motor.urdf" + link = """ + + + + + """ + source.write_text( + '' + + link.format(name="base") + + link.format(name="tip") + + '' + '' + ) + sim = SimulationManager( + SimulationManagerCfg( + headless=True, + render_cfg=RenderCfg(renderer="no-render"), + device="cuda:0", + num_envs=2, + physics_cfg=NewtonPhysicsCfg( + use_cuda_graph=True, solver_cfg={"solver_type": "dexuni"} + ), + ) + ) + art = motor = None + try: + art = sim.add_articulation( + ArticulationCfg( + uid="motor", + fpath=str(source), + init_pos=(0, 0, 1), + init_qpos=[0.0], + build_pk_chain=False, + asset_physics_mode="overlay", + joint_drive_props=JointDrivePropertiesCfg(drive_type="none"), + ) + ) + motor = sim._register_joint_dc_motor_control( + "motor", + dict( + joint_names=["hinge"], + stiffness=[4.0], + damping=[0.1], + effort_limit=[5.0], + velocity_limit=[5.0], + default_joint_position=[0.05], + ), + ) + sim.prepare() + motor.targets.fill_(0.4) + for _ in range(4): + sim.update() + assert sim.physics.cuda_graph_status == "captured" + backend = get_newton_backend(sim._world) + q_indices = wp.to_torch(motor.q_indices).long() + before_q = wp.to_torch(backend._runtime.current_state.joint_q)[ + q_indices + ].clone() + buffers = (motor.targets, motor.effort, motor.velocity) + before = [wp.to_torch(value).clone() for value in buffers] + pointers = [value.ptr for value in buffers] + scene = sim._spawn_scene.builder.result + selected_arena = scene.get_articulation("arena_1/motor")._arena_index + for _ in range(2): + assert scene.reset_arenas([selected_arena]) == 1 + assert [value.ptr for value in buffers] == pointers + for value, saved in zip(buffers, before): + torch.testing.assert_close( + wp.to_torch(value)[0], saved[0], rtol=0, atol=0 + ) + assert wp.to_torch(motor.targets)[1, 0].item() == pytest.approx(0.05) + assert wp.to_torch(motor.effort)[1, 0] == 0 + assert wp.to_torch(motor.velocity)[1, 0] == 0 + q = wp.to_torch(backend._runtime.current_state.joint_q)[q_indices] + torch.testing.assert_close(q[0], before_q[0], rtol=0, atol=0) + assert q[1, 0].item() == 0.0 + for _ in range(3): + sim.update() + assert sim.physics.cuda_graph_status == "captured" + assert torch.isfinite(wp.to_torch(motor.effort)).all() + finally: + art = motor = None + sim.destroy(exit_process=False) + SimulationManager.flush_cleanup_queue() From 21afa5e3b0b63c63619505470d5fce16a580f73c Mon Sep 17 00:00:00 2001 From: acrlw <13927622+acrlw@users.noreply.github.com> Date: Tue, 10 Nov 2026 10:34:21 +0800 Subject: [PATCH 13/14] Restart joint stability intervals after target re-entry --- embodichain/compute/trajectory/stability.py | 12 ++++++++-- tests/compute/test_position_stability.py | 23 +++++++++++++++++++ .../task_program/test_simulation_policies.py | 9 +++++--- 3 files changed, 39 insertions(+), 5 deletions(-) diff --git a/embodichain/compute/trajectory/stability.py b/embodichain/compute/trajectory/stability.py index 6f6df6f601..816558359e 100644 --- a/embodichain/compute/trajectory/stability.py +++ b/embodichain/compute/trajectory/stability.py @@ -34,7 +34,8 @@ class PositionRangeMonitor: position_drift: Positive maximum range in one candidate window. The caller supplies a separate pointwise target predicate. Tick zero does - not count as an elapsed interval. Success is current, not latched: a later + not count as an elapsed interval. Re-entering the target starts a new window + at zero completed intervals. Success is current, not latched: a later violation resets the row even if an earlier window succeeded. Duplicate step observations are idempotent; missing intervals cannot prove stability. This numerical monitor reads no Robot, clock, simulator or normative spec. @@ -63,6 +64,7 @@ def __init__( self._high = initial.clone() self._counts = torch.zeros_like(initial, dtype=torch.long) self._satisfied = torch.zeros_like(initial, dtype=torch.bool) + self._previous_good = torch.zeros_like(initial, dtype=torch.bool) self._required = required_steps self._drift = float(position_drift) self._last_step = -1 @@ -110,11 +112,17 @@ def observe( good = within_target & torch.isfinite(values) low = torch.minimum(self._low, values) high = torch.maximum(self._high, values) - reset = ~good | ~torch.isfinite(self._low) | ((high - low) > self._drift) + reset = ( + ~good + | (~self._previous_good & (elapsed_steps > 0)) + | ~torch.isfinite(self._low) + | ((high - low) > self._drift) + ) self._low = torch.where(reset, values, low) self._high = torch.where(reset, values, high) self._counts = torch.where(reset, 0, self._counts + int(elapsed_steps > 0)) self._satisfied = good & (self._counts >= self._required) + self._previous_good = good self._last_step = elapsed_steps return self._satisfied.clone() diff --git a/tests/compute/test_position_stability.py b/tests/compute/test_position_stability.py index 862ebb482d..c3711bce32 100644 --- a/tests/compute/test_position_stability.py +++ b/tests/compute/test_position_stability.py @@ -61,6 +61,29 @@ def test_position_window_validity_and_target_predicate_are_independent(): assert result.tolist() == [True, False, False] +def test_position_window_starts_at_zero_when_each_row_enters_target(): + monitor = PositionRangeMonitor( + torch.tensor([0.11, 0.09]), required_steps=2, position_drift=0.1 + ) + samples = [ + ([0.11, 0.09], [False, False], [0, 0]), + ([0.09, 0.09], [False, False], [0, 1]), + ([0.09, 0.09], [False, True], [1, 2]), + ([0.09, 0.11], [True, False], [2, 0]), + ([0.09, 0.09], [True, False], [3, 0]), + ([0.09, 0.09], [True, False], [4, 1]), + ([0.09, 0.09], [True, True], [5, 2]), + ] + for step, (positions, expected, counts) in enumerate(samples): + values = torch.tensor(positions) + result = monitor.observe(values, values.abs() <= 0.1, elapsed_steps=step) + assert result.tolist() == expected + assert monitor.stable_counts.tolist() == counts + if step in (1, 4): + torch.testing.assert_close(monitor.minimum, values) + torch.testing.assert_close(monitor.maximum, values) + + def test_position_window_recovers_range_tracking_after_nonfinite_sample(): monitor = PositionRangeMonitor( torch.tensor([float("nan")]), required_steps=2, position_drift=0.001 diff --git a/tests/gym/envs/task_program/test_simulation_policies.py b/tests/gym/envs/task_program/test_simulation_policies.py index 568cb109e3..9f87fec43d 100644 --- a/tests/gym/envs/task_program/test_simulation_policies.py +++ b/tests/gym/envs/task_program/test_simulation_policies.py @@ -1383,13 +1383,16 @@ def test_joint_stability_uses_joint_window_and_original_observed_hold(): assert not any(robot.qpos_reads) -def test_joint_stability_resets_the_full_range_and_does_not_count_tick_zero(): - port, art, _, segment, policy = _joint_condition_port(torch.tensor([[0.1]])) +@pytest.mark.parametrize("initial,changed", [(0.1, 0.1011), (0.1304, 0.1296)]) +def test_joint_stability_requires_full_duration_after_drift_or_target_entry( + initial, changed +): + port, art, _, segment, policy = _joint_condition_port(torch.tensor([[initial]])) steps = 0 for _ in port.actions(policy, segment=segment, active_mask=torch.tensor([True])): steps += 1 if steps == 10: - art._qpos[0, 0] = 0.1011 + art._qpos[0, 0] = changed assert steps == 35 assert port.post_policy_result(policy, segment=segment).tolist() == [True] From 38594254dcb6d545471b098c656f10f9b2503a62 Mon Sep 17 00:00:00 2001 From: acrlw <13927622+acrlw@users.noreply.github.com> Date: Wed, 11 Nov 2026 14:22:02 +0800 Subject: [PATCH 14/14] Fix locomotion benchmark checks for DexUni --- scripts/benchmark/rl/locomotion_ppo.py | 22 ++++--- tests/benchmark/test_locomotion_ppo.py | 83 ++++++++++++++++++++++++-- 2 files changed, 91 insertions(+), 14 deletions(-) diff --git a/scripts/benchmark/rl/locomotion_ppo.py b/scripts/benchmark/rl/locomotion_ppo.py index 2e98c14f98..e7987c0588 100644 --- a/scripts/benchmark/rl/locomotion_ppo.py +++ b/scripts/benchmark/rl/locomotion_ppo.py @@ -31,7 +31,7 @@ BACKENDS = { "default": "Dexsim Default CUDA", - "newton": "Dexsim Newton/MJWarp CUDA", + "newton": "Dexsim Newton CUDA", } TASK_DIR = ( Path(__file__).resolve().parents[3] @@ -90,11 +90,13 @@ def _check_backend(env: Any, name: str) -> Any: from dexsim.engine.newton_physics.backend_registry import get_newton_backend backend = get_newton_backend(sim._world) + solver = backend.mujoco_solver if ( - backend.solver.use_mujoco_cpu - or not backend.solver.mjw_data.qpos.device.is_cuda + solver is None + or solver.use_mujoco_cpu + or not solver.mjw_data.qpos.device.is_cuda ): - raise RuntimeError("Newton/MJWarp is not using CUDA") + raise RuntimeError("Newton MuJoCo solver is not using CUDA") return backend if not (sim._world_config.enable_gpu_sim and sim._world_config.direct_gpu_api): raise RuntimeError("Default backend is not using direct GPU simulation") @@ -250,9 +252,13 @@ def _run(args: argparse.Namespace, result: dict) -> None: env_config = yaml.safe_load(Path(config["trainer"]["gym_config"]).read_text()) env_config["num_envs"] = args.envs if args.backend == "newton": - env_config["physics_config"]["solver_cfg"][ - "iterations" - ] = args.solver_iterations + solver_cfg = env_config["physics_config"]["solver_cfg"] + solver_options = ( + solver_cfg.setdefault("mujoco_options", {}) + if solver_cfg.get("solver_type") == "dexuni" + else solver_cfg + ) + solver_options["iterations"] = args.solver_iterations env_path = args.output / "env.yaml" env_path.write_text(yaml.safe_dump(env_config, sort_keys=False)) config["trainer"]["gym_config"] = str(env_path.resolve()) @@ -313,7 +319,7 @@ def _run(args: argparse.Namespace, result: dict) -> None: if not all(math.isfinite(float(value)) for value in losses.values()): raise RuntimeError("Nonfinite PPO loss") if backend is not None and bool( - backend.solver.mjw_data.overflow.numpy().any() + backend.mujoco_solver.mjw_data.overflow.numpy().any() ): raise RuntimeError("MJWarp capacity or solver iteration limit reached") result["updates"].append( diff --git a/tests/benchmark/test_locomotion_ppo.py b/tests/benchmark/test_locomotion_ppo.py index 0d9d038825..eaa51be380 100644 --- a/tests/benchmark/test_locomotion_ppo.py +++ b/tests/benchmark/test_locomotion_ppo.py @@ -120,6 +120,52 @@ def test_config_rejects_incomplete_minibatches(envs: int, steps: int) -> None: _load_config(TASK_DIR, "default", envs, steps) +@pytest.mark.no_sim +@pytest.mark.parametrize("solver_type", ["mujoco_warp", "dexuni"]) +@pytest.mark.parametrize( + "use_cpu,device_is_cuda", [(False, True), (True, False), (False, False)] +) +def test_newton_backend_checks_active_mujoco_solver( + monkeypatch: pytest.MonkeyPatch, + solver_type: str, + use_cpu: bool, + device_is_cuda: bool, +) -> None: + """Both solver routes must use GPU integration and GPU state buffers.""" + from dexsim.engine.newton_physics import backend_registry + + mujoco = SimpleNamespace( + use_mujoco_cpu=use_cpu, + mjw_data=SimpleNamespace( + qpos=SimpleNamespace(device=SimpleNamespace(is_cuda=device_is_cuda)) + ), + ) + backend = SimpleNamespace( + solver_type=solver_type, + solver=( + mujoco + if solver_type == "mujoco_warp" + else SimpleNamespace(mujoco_solver=mujoco) + ), + mujoco_solver=mujoco, + ) + monkeypatch.setattr(backend_registry, "get_newton_backend", lambda world: backend) + env = SimpleNamespace( + unwrapped=SimpleNamespace( + sim=SimpleNamespace( + physics=SimpleNamespace(name="newton"), + device="cuda:0", + _world=object(), + ) + ) + ) + if not use_cpu and device_is_cuda: + assert benchmark._check_backend(env, "newton") is backend + else: + with pytest.raises(RuntimeError, match="not using CUDA"): + benchmark._check_backend(env, "newton") + + @pytest.mark.parametrize("status", ["passed", "failed"]) def test_report_does_not_rank_training_as_task_success(tmp_path, status: str) -> None: result = _result(status) @@ -220,9 +266,20 @@ def read_text(path: Path, *args, **kwargs): @pytest.mark.no_sim -@pytest.mark.parametrize("renderer", [None, "auto", "no-render", "hybrid"]) +@pytest.mark.parametrize( + "renderer,solver_type", + [ + (None, "mujoco_warp"), + ("auto", "dexuni"), + ("no-render", "mujoco_warp"), + ("hybrid", "dexuni"), + ], +) def test_creation_failure_retains_resource_samples( - tmp_path: Path, monkeypatch: pytest.MonkeyPatch, renderer: str | None + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, + renderer: str | None, + solver_type: str, ) -> None: """Write failure reports with samples taken before runtime construction.""" from embodichain.lab.gym.utils import registration @@ -230,7 +287,9 @@ def test_creation_failure_retains_resource_samples( from scripts.benchmark.rl import runtime as benchmark_runtime env_config = tmp_path / "input.yaml" - env_config.write_text("physics_config:\n solver_cfg: {}\n") + env_config.write_text( + f"physics_config:\n solver_cfg:\n solver_type: {solver_type}\n" + ) output = tmp_path / "result" monkeypatch.setattr( sys, @@ -243,6 +302,8 @@ def test_creation_failure_retains_resource_samples( str(output), "--envs", "32", + "--solver-iterations", + "37", ], ) trainer_cfg = {"gym_config": str(env_config)} @@ -280,7 +341,19 @@ def sample(resources: ProcessResources, label: str) -> dict: failure = RuntimeError("runtime construction failed") def fail_creation(*args, **kwargs): + import yaml + assert kwargs["renderer"] == renderer + saved = yaml.safe_load((output / "env.yaml").read_text()) + expected_options = ( + {"mujoco_options": {"iterations": 37}} + if solver_type == "dexuni" + else {"iterations": 37} + ) + assert saved["physics_config"]["solver_cfg"] == { + "solver_type": solver_type, + **expected_options, + } raise failure monkeypatch.setattr(policy_runtime, "build_gym_policy_runtime", fail_creation) @@ -292,9 +365,7 @@ def fail_creation(*args, **kwargs): assert result["resources"]["samples"] == [ {"label": "before_create", "cpu_rss_mib": 50.0} ] - assert ( - "| Dexsim Newton/MJWarp CUDA | failed |" in (output / "report.md").read_text() - ) + assert "| Dexsim Newton CUDA | failed |" in (output / "report.md").read_text() @pytest.mark.no_sim