From 2f03dc4965a00e5d8c8b1ca6e1297236b7e44f90 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Wed, 4 Nov 2026 18:40:54 +0000 Subject: [PATCH 1/5] feat(gym): share substep EEF control between policy and expert Signed-off-by: yuecideng --- .../topics/env-framework/env-framework.md | 15 +- .../topics/manager-functor/manager-functor.md | 9 +- .../embodichain.lab.gym.envs.managers.rst | 24 ++ docs/source/api_reference/public_api.rst | 19 ++ embodichain/lab/gym/envs/base_env.py | 16 ++ embodichain/lab/gym/envs/embodied_env.py | 32 ++- embodichain/lab/gym/envs/expert_trajectory.py | 10 + .../lab/gym/envs/managers/action_manager.py | 67 +++++ embodichain/lab/gym/envs/managers/actions.py | 230 +++++++++++++++++- .../gym/envs/managers/observation_manager.py | 8 +- .../lab/gym/envs/managers/reward_manager.py | 8 +- .../gym/envs/managers/test_action_manager.py | 112 +++++++++ .../envs/managers/test_action_manager_flat.py | 34 +++ .../managers/test_observation_functors.py | 33 +++ .../gym/envs/managers/test_reward_functors.py | 27 ++ tests/gym/envs/test_expert_trajectory.py | 8 + tests/gym/envs/test_replay_actions.py | 40 +++ tests/gym/envs/test_substep_sensors.py | 47 ++++ 18 files changed, 722 insertions(+), 17 deletions(-) diff --git a/agent_context/topics/env-framework/env-framework.md b/agent_context/topics/env-framework/env-framework.md index 28e67d1f39..d3cc8c9b54 100644 --- a/agent_context/topics/env-framework/env-framework.md +++ b/agent_context/topics/env-framework/env-framework.md @@ -127,9 +127,11 @@ flat action validation/slicing → term processing → term application Raw policy input is one ordered flat floating tensor. `ActionManager` validates it before state changes, processes every slice, and applies each term's owned -resources. `ControllerAction` stays wrapped through preprocessing, bypasses the -manager entirely, and validates qpos/qvel/qf keys, batch, joint width, floating -dtype, and device at the direct controller boundary. Both paths retain the +resources. `ControllerAction` stays wrapped through preprocessing and normally validates +qpos/qvel/qf keys, batch, joint width, floating dtype and device at the direct +controller boundary. The position-only `expert_trajectory.use_action_manager` +opt-in instead processes joint targets through the configured policy terms, +retaining exact FK targets and expert recording schema. Both paths retain the ordinary observation, reward, termination, and rollout lifecycle. Read final task success before resetting scene/bridge state. A Task Program @@ -200,7 +202,10 @@ the traceback can retain native resources not yet registered with the manager. ### Stateful sensor and action lifecycle -BaseEnv starts one simulation-manager update per control interval and samples -participating sensors through its `after_substep` observer. See the sensor-owned [contact history contract](../sensor-system/contact-history.md) +BaseEnv normally starts one simulation-manager update per control interval and +samples participating sensors through its `after_substep` observer. Opted-in +action terms instead refresh between individual physics updates; sensor samples +remain after each completed substep. Direct controller-ready commands bypass +this refresh path; the explicit expert ActionManager route participates. See the sensor-owned [contact history contract](../sensor-system/contact-history.md) for interval accumulation, counterpart filtering and selective reset. EmbodiedEnv resets action and event managers before applying reset-mode events. diff --git a/agent_context/topics/manager-functor/manager-functor.md b/agent_context/topics/manager-functor/manager-functor.md index 85085763a4..eeb9ca9832 100644 --- a/agent_context/topics/manager-functor/manager-functor.md +++ b/agent_context/topics/manager-functor/manager-functor.md @@ -98,7 +98,14 @@ The config attribute name is the stable functor identity. For a flat policy tensor, the environment calls manager processing, manager application, physics, event `interval`, observation, and reward in that order. -`ControllerAction` bypasses the manager and uses the direct controller boundary; +A term can opt into physics-substep refresh. Its first command is processed and +applied normally; subsequent substeps call `update_substep` and apply only the +opted-in terms. Inactive demo rows retain measured holds on every refresh. +`expert_trajectory.use_action_manager` routes position-only expert commands +through `ActionManager.process_joint_targets`. Relative substep DLS and binary +gripper terms support that route, retaining exact FK targets and the expert +storage schema while using the policy controller. +`ControllerAction` otherwise bypasses the manager for direct controller writes; the full loop and reset ordering are owned by [environment execution](../env-framework/execution.md). diff --git a/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.managers.rst b/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.managers.rst index 2eef440df3..4d4d16f4e3 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.managers.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.managers.rst @@ -59,6 +59,7 @@ functors), :class:`ActionManager` (``process_action(action)`` then JointPositionToLimitsAction JointVelocityAction ParallelGripperAction + RelativeEefDlsAction RelativeEefPoseAction RelativeJointPositionAction LeRobotRecorder @@ -194,6 +195,29 @@ The action descriptor records its method, damping, bound, and success semantics. ``joint_delta``, ``joint_step_limited``, and ``joint_limit_clipped_mask`` expose the applied increment and both forms of limiting; resets clear selected rows. +``RelativeEefDlsAction`` keeps an unbounded xyz/rotation-vector request as one +fixed arena-frame target for the control interval. It recomputes the damped +Jacobian solve from measured joints before each physics substep and clips only +joint-position limits. Its ``eef_pose.relative_substep_dls@1`` contract selects +this behavior separately from the bounded, single-step relative-pose contract. +The unchanged physics loop remains batched for terms without substep updates. + +Configure the same arm and binary gripper terms for expert and policy execution. +Set ``env.expert_trajectory.use_action_manager: true`` to route position-only +``ControllerAction`` commands through ``ActionManager.process_joint_targets``. +The DLS term retains the expert's exact FK target while recording the +corresponding relative request, and keeps the existing expert storage schema. +The default controller-ready route stays direct. Every configured term must +support joint-target processing. +For binary grippers, ``open_on_zero=False`` makes zero a closing request; +the default retains nonnegative opening requests. The descriptor records this +boundary together with the gripper mode. + +.. autoclass:: RelativeEefDlsAction + :members: + :inherited-members: + :show-inheritance: + .. autoclass:: RelativeEefPoseAction :members: :inherited-members: diff --git a/docs/source/api_reference/public_api.rst b/docs/source/api_reference/public_api.rst index ab55951783..95d14d25dc 100644 --- a/docs/source/api_reference/public_api.rst +++ b/docs/source/api_reference/public_api.rst @@ -1299,6 +1299,7 @@ embodichain.lab.gym.envs.managers.actions JointPositionToLimitsAction JointVelocityAction ParallelGripperAction + RelativeEefDlsAction RelativeEefPoseAction RelativeJointPositionAction @@ -1313,6 +1314,24 @@ embodichain.lab.gym.envs.managers.action_types ActionTermDescriptor ActionTrace +embodichain.lab.gym.envs.managers.observation_manager +-------------------------------------------------- + +.. currentmodule:: embodichain.lab.gym.envs.managers.observation_manager + +.. autosummary:: + + ObservationManager + +embodichain.lab.gym.envs.managers.reward_manager +--------------------------------------------- + +.. currentmodule:: embodichain.lab.gym.envs.managers.reward_manager + +.. autosummary:: + + RewardManager + embodichain.lab.gym.envs.objectives ---------------------------------- diff --git a/embodichain/lab/gym/envs/base_env.py b/embodichain/lab/gym/envs/base_env.py index 76cf5551be..3c38674d42 100644 --- a/embodichain/lab/gym/envs/base_env.py +++ b/embodichain/lab/gym/envs/base_env.py @@ -629,6 +629,22 @@ def _advance_physics(self) -> None: sensors = [ sensor for sensor in self.sensors.values() if sensor.requires_substep_update ] + manager = getattr(self, "action_manager", None) + update_actions = ( + getattr(self, "_step_uses_action_manager", False) + and manager is not None + and manager.requires_substep_update + ) + if update_actions: + for sensor in sensors: + sensor.begin_control_step() + for substep in range(self.cfg.sim_steps_per_control): + if substep: + manager.apply_substep_actions() + self.sim.update(self.physics_dt, 1, render_final_step=False) + for sensor in sensors: + sensor.update_physics_step(self.physics_dt) + return if not sensors: self.sim.update( self.physics_dt, diff --git a/embodichain/lab/gym/envs/embodied_env.py b/embodichain/lab/gym/envs/embodied_env.py index 940a22b861..b75f2b9225 100644 --- a/embodichain/lab/gym/envs/embodied_env.py +++ b/embodichain/lab/gym/envs/embodied_env.py @@ -2473,6 +2473,10 @@ def _step_action(self, action: EnvAction | ControllerAction) -> EnvAction: """ if isinstance(action, ControllerAction): + if getattr(self, "_step_uses_action_manager", False): + self.action_manager.apply_action() + self._last_action_trace = self.action_manager.action_trace() + return action.value return EmbodiedEnv._apply_controller_action(self, action.value) action_manager = getattr(self, "action_manager", None) if action_manager is not None: @@ -2618,6 +2622,17 @@ def _preprocess_action( self._last_action_manager_qpos = None self._last_action_trace = None is_controller_action = isinstance(action, ControllerAction) + expert_cfg = getattr(getattr(self, "cfg", None), "expert_trajectory", None) + managed_expert = is_controller_action and bool( + getattr(expert_cfg, "use_action_manager", False) + ) + if managed_expert and self.action_manager is None: + raise ValueError( + "ActionManager expert execution requires configured action terms." + ) + self._step_uses_action_manager = self.action_manager is not None and ( + not is_controller_action or managed_expert + ) controller_metadata = action.metadata if is_controller_action else None policy_trajectory = self._traj_buffer is not None and ( getattr(self, "_trajectory_action_kind", None) == "policy" @@ -2653,7 +2668,9 @@ def _preprocess_action( if retain_raw_action else action ) - if self.action_manager is not None and not is_controller_action: + if self.action_manager is not None and ( + not is_controller_action or managed_expert + ): if policy_recording: explicit_policy = getattr(self, "_explicit_policy_rollout", False) if explicit_policy: @@ -2664,8 +2681,17 @@ def _preprocess_action( raise RuntimeError( "Policy action recording requires an initialized DatasetManager." ) - self.action_manager.process_action(action) - if getattr(self, "_demo_no_auto_reset", False): + if managed_expert: + action = self._prepare_controller_action(action) + if getattr(self, "_demo_no_auto_reset", False): + action = self._mask_controller_demo_action(action) + qpos = action if isinstance(action, torch.Tensor) else action["qpos"] + full_targets = self.robot.get_qpos().clone() + full_targets[:, self.active_joint_ids] = self._active_qpos_command(qpos) + self.action_manager.process_joint_targets(full_targets) + else: + self.action_manager.process_action(action) + if getattr(self, "_demo_no_auto_reset", False) and not managed_expert: self.action_manager.mask_inactive(self._demo_active_mask) if self.dataset_manager is not None and not policy_recording: processed_qpos = getattr(self.action_manager, "_processed_qpos", None) diff --git a/embodichain/lab/gym/envs/expert_trajectory.py b/embodichain/lab/gym/envs/expert_trajectory.py index f7e9384bbe..a98f5febf8 100644 --- a/embodichain/lab/gym/envs/expert_trajectory.py +++ b/embodichain/lab/gym/envs/expert_trajectory.py @@ -61,13 +61,23 @@ class ExpertTrajectoryCfg: Args: joint_command_mode: Position-only or position-velocity expert targets. + use_action_manager: Route position targets through configured policy terms. """ joint_command_mode: JointCommandMode = "position" """Joint targets emitted and stored by expert trajectory generation.""" + use_action_manager: bool = False + """Execute expert position targets through configured policy action terms.""" + def __post_init__(self) -> None: self.joint_command_mode = _validate_joint_command_mode(self.joint_command_mode) + if type(self.use_action_manager) is not bool: + raise ValueError("use_action_manager must be boolean.") + if self.use_action_manager and self.joint_command_mode != "position": + raise ValueError( + "ActionManager expert execution requires position-only targets." + ) @dataclass(frozen=True, slots=True) diff --git a/embodichain/lab/gym/envs/managers/action_manager.py b/embodichain/lab/gym/envs/managers/action_manager.py index f0645529b3..358a449609 100644 --- a/embodichain/lab/gym/envs/managers/action_manager.py +++ b/embodichain/lab/gym/envs/managers/action_manager.py @@ -50,6 +50,7 @@ class ActionTerm(Functor): """ contract_id: ClassVar[str | None] = None + requires_substep_update: ClassVar[bool] = False def __init__(self, cfg: ActionTermCfg, env: EmbodiedEnv): """Initialize an action term. @@ -78,6 +79,22 @@ def resolved_contract_id(self) -> str | None: ) return self.cfg.contract or self.contract_id + def update_substep(self) -> None: + """Refresh a processed command from current physical state.""" + + def process_joint_targets(self, qpos: torch.Tensor) -> None: + """Process full-joint expert targets through this term's controller. + + Args: + qpos: Full robot joint targets, shaped ``(num_envs, dof)``. + + Raises: + NotImplementedError: If the term has no expert-target conversion. + """ + raise NotImplementedError( + f"{type(self).__name__} cannot process joint targets." + ) + @property @abstractmethod def action_dim(self) -> int: @@ -152,6 +169,10 @@ def __init__(self, cfg: object, env: EmbodiedEnv): device=env.device, ) self._previous_action = torch.zeros_like(self._action) + self._substep_terms = tuple( + term for term in self._terms.values() if term.requires_substep_update + ) + self._substep_active_mask: torch.Tensor | None = None def __str__(self) -> str: """Return a table describing the ordered flat action layout.""" @@ -194,6 +215,48 @@ def previous_action(self) -> torch.Tensor: """Return previous complete raw policy actions.""" return self._previous_action + @property + def requires_substep_update(self) -> bool: + """Whether any term refreshes its command between physics substeps.""" + return bool(self._substep_terms) + + def _validate_joint_targets(self, qpos: torch.Tensor) -> None: + expected = self._env.robot.get_qpos().shape + if ( + not isinstance(qpos, torch.Tensor) + or not qpos.is_floating_point() + or qpos.shape != expected + or qpos.device != torch.device(self._env.device) + or not bool(torch.isfinite(qpos).all()) + ): + raise ValueError( + f"Expected finite joint targets with shape {expected} on {self._env.device}." + ) + + def process_joint_targets(self, qpos: torch.Tensor) -> None: + """Process expert targets while preserving exact FK target poses. + + Args: + qpos: Finite full-joint position targets on the environment device. + """ + self._validate_joint_targets(qpos) + self._previous_action.copy_(self._action) + self._substep_active_mask = None + for term in self._terms.values(): + term.process_joint_targets(qpos) + self._action.copy_( + torch.cat([term.raw_actions for term in self._terms.values()], dim=-1) + ) + + def apply_substep_actions(self) -> None: + """Refresh and apply opted-in terms while preserving inactive-row holds.""" + for term in self._substep_terms: + term.update_substep() + if self._substep_active_mask is not None: + self.mask_inactive(self._substep_active_mask) + for term in self._substep_terms: + term.apply_actions() + @property def descriptors(self) -> tuple[ActionDescriptor, ...]: """Return ordered action-term descriptors bound to flat slices.""" @@ -277,6 +340,7 @@ def process_action(self, action: torch.Tensor) -> None: ) self._previous_action.copy_(self._action) + self._substep_active_mask = None self._action.copy_(action) for name, term in self._terms.items(): term.process_actions(self._action[:, self._slices[name]]) @@ -333,6 +397,7 @@ def mask_inactive(self, active_mask: torch.Tensor) -> None: raise ValueError( f"active_mask must be on {expected_device}, got {active_mask.device}." ) + self._substep_active_mask = active_mask.clone() measured_qpos: torch.Tensor | None = None for name, term in self._terms.items(): @@ -371,6 +436,8 @@ def reset( ids = slice(None) if env_ids is None else env_ids self._action[ids] = 0 self._previous_action[ids] = 0 + if self._substep_active_mask is not None: + self._substep_active_mask[ids] = True for term in self._terms.values(): term.reset(env_ids=env_ids) return {} diff --git a/embodichain/lab/gym/envs/managers/actions.py b/embodichain/lab/gym/envs/managers/actions.py index 396d9c14e5..36955b2fb9 100644 --- a/embodichain/lab/gym/envs/managers/actions.py +++ b/embodichain/lab/gym/envs/managers/actions.py @@ -48,6 +48,7 @@ "JointVelocityAction", "ParallelGripperAction", "RelativeEefPoseAction", + "RelativeEefDlsAction", "RelativeJointPositionAction", "register_action_contract", "resolve_action_contract", @@ -620,6 +621,203 @@ def reset(self, env_ids: list[int] | torch.Tensor | None = None) -> None: self._ik_success[ids] = False +class RelativeEefDlsAction(EefPoseAction): + """Track an unbounded relative EEF request on every physics substep. + + Translation and rotation-vector inputs are scaled in the arena frame. + The target is held for the control interval; each substep solves the + damped Jacobian system from measured joints and applies joint limits. + """ + + contract_id: ClassVar[str] = "eef_pose.relative_substep_dls@1" + requires_substep_update: ClassVar[bool] = True + + def __init__(self, cfg: ActionTermCfg, env: EmbodiedEnv) -> None: + """Bind an arm and its fixed control-interval target. + + Args: + cfg: Term parameters: ``part_name``, positive ``translation_scale`` + and ``rotation_scale`` (both 0.5), and ``damping`` (0.01). + env: Environment owning the robot and physics loop. + """ + if "pose_representation" in cfg.params or "scale" in cfg.params: + raise ValueError( + "RelativeEefDlsAction uses xyz/rotation-vector inputs and separate scales." + ) + super().__init__(cfg, env) + self._translation_scale = float(cfg.params.get("translation_scale", 0.5)) + self._rotation_scale = float(cfg.params.get("rotation_scale", 0.5)) + self._damping = float(cfg.params.get("damping", 0.01)) + for name, value in ( + ("translation_scale", self._translation_scale), + ("rotation_scale", self._rotation_scale), + ("damping", self._damping), + ): + if not np.isfinite(value) or value <= 0: + raise ValueError(f"{name} must be positive and finite; found {value}.") + self._solver = env.robot.get_solver(self._part_name) + self._target_pose = torch.eye(4, device=self.device).repeat(self.num_envs, 1, 1) + self._identity = torch.eye(6, device=self.device) + self._descriptor = ActionTermDescriptor( + representation="relative_eef_pose", + action_dim=6, + feature_names=( + "delta_x", + "delta_y", + "delta_z", + "delta_rx", + "delta_ry", + "delta_rz", + ), + units=("unitless",) * 6, + normalization=None, + joint_names=self._joint_names, + metadata={ + "frame": "arena", + "rotation": "rotation_vector", + "translation_scale": self._translation_scale, + "rotation_scale": self._rotation_scale, + "damping": self._damping, + "update": "physics_substep", + "request_clipping": False, + }, + contract=self.resolved_contract_id(), + ) + + def _measured_pose(self) -> torch.Tensor: + return self._env.robot.compute_fk( + self._env.robot.get_qpos()[:, list(self._joint_ids)], + name=self._part_name, + to_matrix=True, + ) + + def _action_from_joint_targets(self, qpos: torch.Tensor) -> torch.Tensor: + """Express a joint-position expert target as the same relative request. + + Args: + qpos: Full robot joint targets, shaped ``(num_envs, dof)``. + + Returns: + Unbounded xyz/rotation-vector actions, shaped ``(num_envs, 6)``. + """ + from scipy.spatial.transform import Rotation + + current = self._measured_pose() + target = self._env.robot.compute_fk( + qpos[:, list(self._joint_ids)], name=self._part_name, to_matrix=True + ) + delta = Rotation.from_matrix( + (target[:, :3, :3] @ current[:, :3, :3].mT).detach().cpu().numpy() + ).as_rotvec() + rotation = torch.as_tensor(delta, dtype=current.dtype, device=self.device) + return torch.cat( + ( + (target[:, :3, 3] - current[:, :3, 3]) / self._translation_scale, + rotation / self._rotation_scale, + ), + dim=-1, + ) + + def process_actions(self, actions: torch.Tensor) -> None: + """Set a target from measured pose without clipping the request. + + Args: + actions: Finite floating actions shaped ``(num_envs, 6)``. + """ + from scipy.spatial.transform import Rotation + + if ( + not isinstance(actions, torch.Tensor) + or not actions.is_floating_point() + or actions.shape != self._raw_actions.shape + or actions.device != self._raw_actions.device + or not bool(torch.isfinite(actions).all()) + ): + raise ValueError( + f"Expected finite floating actions with shape {tuple(self._raw_actions.shape)} on {self.device}." + ) + self._previous_raw_actions.copy_(self._raw_actions) + self._raw_actions.copy_(actions) + current = self._measured_pose() + self._target_pose.copy_(current) + self._target_pose[:, :3, 3] += actions[:, :3] * self._translation_scale + delta = Rotation.from_rotvec( + (actions[:, 3:] * self._rotation_scale).detach().cpu().numpy() + ) + rotation = torch.as_tensor( + delta.as_matrix(), dtype=current.dtype, device=self.device + ) + self._target_pose[:, :3, :3] = rotation @ current[:, :3, :3] + self.update_substep() + + def process_joint_targets(self, qpos: torch.Tensor) -> None: + """Retain the expert's exact FK target and record its relative action. + + Args: + qpos: Full robot joint-position expert targets. + """ + actions = self._action_from_joint_targets(qpos) + self._previous_raw_actions.copy_(self._raw_actions) + self._raw_actions.copy_(actions) + self._target_pose.copy_( + self._env.robot.compute_fk( + qpos[:, list(self._joint_ids)], + name=self._part_name, + to_matrix=True, + ) + ) + self.update_substep() + + def update_substep(self) -> None: + """Recompute bounded joint targets toward the held EEF target.""" + from scipy.spatial.transform import Rotation + + current = self._measured_pose() + qpos = self._env.robot.get_qpos().clone() + delta = Rotation.from_matrix( + (self._target_pose[:, :3, :3] @ current[:, :3, :3].mT) + .detach() + .cpu() + .numpy() + ).as_rotvec() + error = torch.cat( + ( + self._target_pose[:, :3, 3] - current[:, :3, 3], + torch.as_tensor(delta, dtype=qpos.dtype, device=self.device), + ), + dim=-1, + ) + jacobian = self._env.robot.compute_jacobian( + qpos, + end_link_name=self._solver.end_link_name, + root_link_name=self._solver.root_link_name, + ) + transpose = jacobian.mT + increment = transpose @ torch.linalg.solve( + jacobian @ transpose + self._damping**2 * self._identity, + error.unsqueeze(-1), + ) + limits = self._env.robot.get_qpos_limits()[:, list(self._joint_ids)] + self._processed_actions.copy_( + (qpos[:, list(self._joint_ids)] + increment.squeeze(-1)).clamp( + limits[..., 0], limits[..., 1] + ) + ) + self._ik_success.copy_(torch.isfinite(self._processed_actions).all(dim=-1)) + if not bool(self._ik_success.all()): + raise ValueError("RelativeEefDlsAction produced nonfinite joint targets.") + + def reset(self, env_ids: list[int] | torch.Tensor | None = None) -> None: + """Discard target history for selected rows. + + Args: + env_ids: Selected rows, or all rows. + """ + super().reset(env_ids) + ids = slice(None) if env_ids is None else env_ids + self._target_pose[ids] = torch.eye(4, device=self.device) + + class RelativeEefPoseAction(EefPoseAction): """Convert normalized arena-frame pose deltas into selected-arm targets. @@ -914,6 +1112,9 @@ def __init__(self, cfg: ActionTermCfg, env: EmbodiedEnv) -> None: self._joint_names = tuple( env.robot.joint_names[index] for index in self._joint_ids ) + self._open_on_zero = params.get("open_on_zero", True) + if type(self._open_on_zero) is not bool: + raise ValueError("open_on_zero must be boolean.") self._command_mode = str(params.get("command_mode", "continuous")) if self._command_mode not in {"continuous", "binary"}: raise ValueError("command_mode must be 'continuous' or 'binary'.") @@ -968,7 +1169,10 @@ def __init__(self, cfg: ActionTermCfg, env: EmbodiedEnv) -> None: units=("normalized",), normalization="minus_one_to_one", joint_names=self._joint_names, - metadata={"command_mode": self._command_mode}, + metadata={ + "command_mode": self._command_mode, + "open_on_zero": self._open_on_zero, + }, contract=self.resolved_contract_id(), ) @@ -1006,6 +1210,23 @@ def part_name(self) -> str: """Return the robot control part owning the gripper joints.""" return self._part_name + def process_joint_targets(self, qpos: torch.Tensor) -> None: + """Encode a binary expert gripper target as an opening/closing request. + + Args: + qpos: Full robot joint-position targets. + + The encoded opening/closing request becomes the term's raw action. + """ + if self._command_mode != "binary": + raise NotImplementedError( + "Joint-target conversion requires a binary gripper." + ) + selected = qpos[:, list(self._joint_ids)] + opened = (selected - self._open_command).square().sum(-1) + closed = (selected - self._close_command).square().sum(-1) + self.process_actions(torch.where(opened < closed, 1.0, -1.0).unsqueeze(-1)) + @property def command_mode(self) -> str: """Return the continuous or binary scalar command mode.""" @@ -1072,7 +1293,11 @@ def process_actions(self, actions: torch.Tensor) -> None: else: self._processed_actions.copy_( torch.where( - self._raw_actions >= 0.0, + ( + (self._raw_actions >= 0.0) + if self._open_on_zero + else (self._raw_actions > 0.0) + ), self._open_command, self._close_command, ) @@ -1148,6 +1373,7 @@ def resolve_action_contract(contract_id: str) -> type[ActionTerm]: "joint_effort@1": JointEffortAction, "eef_pose.absolute@1": EefPoseAction, "eef_pose.relative@1": RelativeEefPoseAction, + "eef_pose.relative_substep_dls@1": RelativeEefDlsAction, "gripper.parallel@1": ParallelGripperAction, }.items(): register_action_contract(_contract_id, _action_class) diff --git a/embodichain/lab/gym/envs/managers/observation_manager.py b/embodichain/lab/gym/envs/managers/observation_manager.py index f5e015abe9..3c2ecc57e1 100644 --- a/embodichain/lab/gym/envs/managers/observation_manager.py +++ b/embodichain/lab/gym/envs/managers/observation_manager.py @@ -18,7 +18,6 @@ from __future__ import annotations -import inspect import torch from collections.abc import Sequence from prettytable import PrettyTable @@ -30,13 +29,16 @@ fetch_data_from_dict, assign_data_to_dict, ) -from .manager_base import ManagerBase +from .manager_base import Functor, ManagerBase from .cfg import ObservationCfg if TYPE_CHECKING: from embodichain.lab.gym.envs import EmbodiedEnv +__all__ = ["ObservationManager"] + + class ObservationManager(ManagerBase): """Manager for orchestrating operations based on different simulation observations. @@ -213,5 +215,5 @@ def _prepare_functors(self): self._mode_functor_cfgs[functor_cfg.mode].append(functor_cfg) # check if the functor is a class - if inspect.isclass(functor_cfg.func): + if isinstance(functor_cfg.func, Functor): self._mode_class_functor_cfgs[functor_cfg.mode].append(functor_cfg) diff --git a/embodichain/lab/gym/envs/managers/reward_manager.py b/embodichain/lab/gym/envs/managers/reward_manager.py index 1bdac38b1f..1d69b650ec 100644 --- a/embodichain/lab/gym/envs/managers/reward_manager.py +++ b/embodichain/lab/gym/envs/managers/reward_manager.py @@ -18,20 +18,22 @@ from __future__ import annotations -import inspect import torch from collections.abc import Sequence from prettytable import PrettyTable from typing import TYPE_CHECKING, Union from embodichain.utils import logger -from .manager_base import ManagerBase +from .manager_base import Functor, ManagerBase from .cfg import RewardCfg if TYPE_CHECKING: from embodichain.lab.gym.envs import EmbodiedEnv +__all__ = ["RewardManager"] + + class RewardManager(ManagerBase): """Manager for orchestrating reward computation in reinforcement learning tasks. @@ -225,5 +227,5 @@ def _prepare_functors(self): self._mode_functor_cfgs[functor_cfg.mode].append(functor_cfg) # check if the functor is a class - if inspect.isclass(functor_cfg.func): + if isinstance(functor_cfg.func, Functor): self._mode_class_functor_cfgs[functor_cfg.mode].append(functor_cfg) diff --git a/tests/gym/envs/managers/test_action_manager.py b/tests/gym/envs/managers/test_action_manager.py index 33c7c4a605..3858461140 100644 --- a/tests/gym/envs/managers/test_action_manager.py +++ b/tests/gym/envs/managers/test_action_manager.py @@ -985,3 +985,115 @@ def test_relative_eef_solver_maps_fk_seed_and_returned_joint_order( torch.testing.assert_close(term.processed_actions, expected_commands) assert env.robot.set_qpos.call_args.kwargs["joint_ids"] == [2, 0] assert term.descriptor.joint_names == ("joint_2", "joint_0") + + +@pytest.mark.parametrize("batch", [1, 8, 32]) +def test_substep_dls_preserves_unbounded_targets_and_expert_conversion(batch): + """Expert and policy requests follow the same held-pose DLS trajectory.""" + from scipy.spatial.transform import Rotation + from embodichain.lab.gym.envs.managers.actions import RelativeEefDlsAction + + qpos = torch.zeros(batch, 7) + qpos[:, :3] = torch.tensor([0.2, -0.1, 0.3]) + limits = torch.tensor([-3.0, 3.0]).expand(batch, 7, 2).clone() + jacobian = torch.eye(6, 7).expand(batch, 6, 7).clone() + + def fk(values, *, name, to_matrix): + pose = torch.eye(4).repeat(batch, 1, 1) + pose[:, :3, 3] = values[:, :3] + pose[:, :3, :3] = torch.tensor( + Rotation.from_rotvec(values[:, 3:6].numpy()).as_matrix(), dtype=values.dtype + ) + return pose + + robot = SimpleNamespace( + get_joint_ids=lambda part, **kwargs: list(range(7)), + joint_names=tuple(f"joint{i}" for i in range(7)), + get_qpos=lambda: qpos, + get_qpos_limits=lambda: limits, + compute_fk=fk, + compute_jacobian=lambda values, **kwargs: jacobian, + get_solver=lambda part: SimpleNamespace( + end_link_name="tool", root_link_name="base" + ), + set_qpos=lambda **kwargs: None, + ) + env = SimpleNamespace(num_envs=batch, device="cpu", robot=robot) + term = RelativeEefDlsAction(make_cfg(RelativeEefDlsAction, part_name="arm"), env) + action = torch.tensor([1.5, -1.2, 0.1, 0.2, -0.1, 0.3]).expand(batch, 6).clone() + current = fk(qpos, name="arm", to_matrix=True) + target = current.clone() + target[:, :3, 3] += action[:, :3] * 0.5 + target[:, :3, :3] = ( + torch.tensor( + Rotation.from_rotvec((action[:, 3:] * 0.5).numpy()).as_matrix(), + dtype=qpos.dtype, + ) + @ current[:, :3, :3] + ) + planned = qpos.clone() + planned[:, :3] = target[:, :3, 3] + planned[:, 3:6] = torch.tensor( + Rotation.from_matrix(target[:, :3, :3].numpy()).as_rotvec(), dtype=qpos.dtype + ) + term.process_joint_targets(planned) + torch.testing.assert_close(term.raw_actions, action) + target = fk(planned, name="arm", to_matrix=True) + assert np.isinf(term.action_space.high).all() + for _ in range(5): + pose = fk(qpos, name="arm", to_matrix=True) + error = torch.cat( + ( + target[:, :3, 3] - pose[:, :3, 3], + torch.tensor( + Rotation.from_matrix( + (target[:, :3, :3] @ pose[:, :3, :3].mT).numpy() + ).as_rotvec(), + dtype=qpos.dtype, + ), + ), + -1, + ) + expected = ( + qpos + + ( + jacobian.mT + @ torch.linalg.solve( + jacobian @ jacobian.mT + 0.01**2 * torch.eye(6), error.unsqueeze(-1) + ) + ).squeeze(-1) + ).clamp(limits[..., 0], limits[..., 1]) + term.update_substep() + torch.testing.assert_close(term.processed_actions, expected, rtol=0, atol=0) + qpos.copy_(qpos + 0.2 * (expected - qpos)) + preserved = term.raw_actions[1:].clone() + term.reset([0]) + torch.testing.assert_close(term.raw_actions[1:], preserved) + + +def test_binary_gripper_conversion_preserves_closed_zero_boundary(): + """Legacy zero closes while existing default binary semantics stay unchanged.""" + env = make_action_env( + num_envs=3, joint_names=("left", "right"), parts={"hand": (0, 1)} + ) + term = ParallelGripperAction( + make_cfg( + ParallelGripperAction, + part_name="hand", + command_mode="binary", + open_command={"left": 0.04, "right": 0.04}, + close_command={"left": 0.0, "right": 0.0}, + open_on_zero=False, + ), + env, + ) + term.process_actions(torch.tensor([[-1.0], [0.0], [1.0]])) + torch.testing.assert_close( + term.processed_actions, torch.tensor([[0.0, 0.0], [0.0, 0.0], [0.04, 0.04]]) + ) + qpos = env.robot.get_qpos().clone() + qpos[:, list(term.controlled_joint_ids)] = torch.tensor( + [[0.0, 0.0], [0.02, 0.02], [0.04, 0.04]] + ) + term.process_joint_targets(qpos) + torch.testing.assert_close(term.raw_actions, torch.tensor([[-1.0], [-1.0], [1.0]])) diff --git a/tests/gym/envs/managers/test_action_manager_flat.py b/tests/gym/envs/managers/test_action_manager_flat.py index eeabed24a2..3f7ace4a2c 100644 --- a/tests/gym/envs/managers/test_action_manager_flat.py +++ b/tests/gym/envs/managers/test_action_manager_flat.py @@ -285,3 +285,37 @@ def test_manager_reset_preserves_unselected_rows() -> None: torch.testing.assert_close(manager.action[1], action[1]) torch.testing.assert_close(manager.get_term("arm").raw_actions[0], torch.zeros(2)) torch.testing.assert_close(manager.get_term("arm").raw_actions[1], action[1]) + + +def test_substep_refresh_preserves_inactive_position_holds(): + """A refreshed command cannot move a completed demo row.""" + from action_test_utils import FakeAction, make_action_env + from embodichain.lab.gym.envs.managers import ActionManager, ActionTermCfg + + class TrackingAction(FakeAction): + requires_substep_update = True + + def update_substep(self): + self.processed_actions.fill_(0.8) + + env = make_action_env(num_envs=2, joint_names=("joint",)) + manager = ActionManager( + { + "arm": ActionTermCfg( + func=TrackingAction, + params={"action_dim": 1, "low": -1.0, "high": 1.0, "joint_ids": (0,)}, + ) + }, + env, + ) + manager.process_action(torch.tensor([[0.2], [0.3]])) + manager.mask_inactive(torch.tensor([True, False])) + manager.apply_substep_actions() + torch.testing.assert_close( + manager.get_term("arm").processed_actions, torch.tensor([[0.8], [0.0]]) + ) + manager.process_action(torch.tensor([[0.4], [0.5]])) + manager.apply_substep_actions() + torch.testing.assert_close( + manager.get_term("arm").processed_actions, torch.full((2, 1), 0.8) + ) diff --git a/tests/gym/envs/managers/test_observation_functors.py b/tests/gym/envs/managers/test_observation_functors.py index ea146413d6..c21c2bb725 100644 --- a/tests/gym/envs/managers/test_observation_functors.py +++ b/tests/gym/envs/managers/test_observation_functors.py @@ -954,3 +954,36 @@ def test_different_num_envs(self): # All envs have identical mask data, so results should match for i in range(1, num_envs): assert result[i].equal(result[0]) + + +def test_stateful_observation_manager_reset_clears_only_selected_rows() -> None: + from types import SimpleNamespace + from tensordict import TensorDict + from embodichain.lab.gym.envs.managers import ObservationCfg, ObservationManager + from embodichain.lab.gym.envs.managers.manager_base import Functor + + class Counter(Functor): + def __init__(self, cfg, env): + super().__init__(cfg, env) + self.value = torch.zeros(env.num_envs) + + def __call__(self, env, obs): + self.value += 1 + return self.value.clone() + + def reset(self, env_ids=None): + self.value[slice(None) if env_ids is None else env_ids] = 0 + + env = SimpleNamespace(num_envs=2, device=torch.device("cpu")) + manager = ObservationManager( + { + "counter": ObservationCfg( + func=Counter, mode="add", name="trace/count", params={} + ) + }, + env, + ) + manager.compute(TensorDict({}, batch_size=[2])) + manager.reset([0]) + value = manager.compute(TensorDict({}, batch_size=[2])) + torch.testing.assert_close(value["trace"]["count"], torch.tensor([1.0, 2.0])) diff --git a/tests/gym/envs/managers/test_reward_functors.py b/tests/gym/envs/managers/test_reward_functors.py index cab8b6b292..281b6cb167 100644 --- a/tests/gym/envs/managers/test_reward_functors.py +++ b/tests/gym/envs/managers/test_reward_functors.py @@ -578,3 +578,30 @@ def test_positive_when_getting_closer(self): assert result.shape == (4,) # Distance decreased from 0.5 to 0.25, so should be positive assert torch.any(result > 0) + + +def test_stateful_reward_manager_reset_clears_only_selected_rows() -> None: + from types import SimpleNamespace + from embodichain.lab.gym.envs.managers import RewardCfg, RewardManager + from embodichain.lab.gym.envs.managers.manager_base import Functor + + class Counter(Functor): + def __init__(self, cfg, env): + super().__init__(cfg, env) + self.value = torch.zeros(env.num_envs) + + def __call__(self, env, obs, action, info): + self.value += 1 + return self.value.clone() + + def reset(self, env_ids=None): + self.value[slice(None) if env_ids is None else env_ids] = 0 + + env = SimpleNamespace(num_envs=2, device=torch.device("cpu")) + manager = RewardManager( + {"counter": RewardCfg(func=Counter, weight=1.0, mode="add", params={})}, env + ) + manager.compute({}, torch.zeros(2, 1), {}) + manager.reset([0]) + value, _ = manager.compute({}, torch.zeros(2, 1), {}) + torch.testing.assert_close(value, torch.tensor([1.0, 2.0])) diff --git a/tests/gym/envs/test_expert_trajectory.py b/tests/gym/envs/test_expert_trajectory.py index aac3d965fc..b8353a2b53 100644 --- a/tests/gym/envs/test_expert_trajectory.py +++ b/tests/gym/envs/test_expert_trajectory.py @@ -218,3 +218,11 @@ def test_expert_action_encoding_rejects_nonfinite_targets() -> None: with pytest.raises(ValueError, match="finite"): encode_expert_action(action, spec=spec, active_joint_ids=[0]) + + +def test_expert_action_manager_route_requires_position_targets() -> None: + assert ExpertTrajectoryCfg(use_action_manager=True).use_action_manager + with pytest.raises(ValueError, match="position-only"): + ExpertTrajectoryCfg( + joint_command_mode="position_velocity", use_action_manager=True + ) diff --git a/tests/gym/envs/test_replay_actions.py b/tests/gym/envs/test_replay_actions.py index 833433ba36..525e7eebdc 100644 --- a/tests/gym/envs/test_replay_actions.py +++ b/tests/gym/envs/test_replay_actions.py @@ -416,3 +416,43 @@ def test_kinematic_task_replay_does_not_claim_measured_goal_acceptance(): _TaskReplayEnv(), _trajectory(kind="raw_policy"), mode="kinematic" ) assert replay._task_outcome_replay is False + + +@pytest.mark.parametrize("completed", [False, True]) +def test_expert_opt_in_uses_policy_controller_with_full_joint_target( + completed: bool, +) -> None: + from unittest.mock import MagicMock + from embodichain.lab.gym.envs.expert_trajectory import ExpertTrajectoryCfg + + env = _ReplayEnv(num_envs=1) + env._demo_no_auto_reset = completed + env._demo_active_mask = torch.tensor([not completed]) + from types import MethodType + + env._mask_controller_demo_action = MethodType( + EmbodiedEnv._mask_controller_demo_action, env + ) + env.cfg = SimpleNamespace( + expert_trajectory=ExpertTrajectoryCfg(use_action_manager=True) + ) + manager = SimpleNamespace( + process_joint_targets=MagicMock(), + apply_action=MagicMock(), + action_trace=lambda: None, + action=torch.zeros(1, 7), + ) + env.action_manager = manager + prepared = env._preprocess_action(ControllerAction(torch.tensor([[0.2, 0.3]]))) + manager.process_joint_targets.assert_called_once() + torch.testing.assert_close( + manager.process_joint_targets.call_args.args[0], + ( + torch.tensor([[0.0, 0.0, 0.0]]) + if completed + else torch.tensor([[0.0, 0.2, 0.3]]) + ), + ) + env._step_action(prepared) + manager.apply_action.assert_called_once() + assert env._step_uses_action_manager diff --git a/tests/gym/envs/test_substep_sensors.py b/tests/gym/envs/test_substep_sensors.py index 4593222f49..23837f6dee 100644 --- a/tests/gym/envs/test_substep_sensors.py +++ b/tests/gym/envs/test_substep_sensors.py @@ -50,3 +50,50 @@ def update(dt, n, *, after_substep=None, render_final_step=True): else [("manager", 0.01, 3)] + [("physics", 0.01)] * 3 ) assert events == expected + + +@pytest.mark.parametrize("controller_ready", [False, True]) +def test_substep_action_updates_interleave_with_physics_and_sensor_observation( + controller_ready, +): + """Policy controllers refresh between steps; ready joint commands bypass them.""" + calls = [] + env = BaseEnv.__new__(BaseEnv) + env.cfg = SimpleNamespace(sim_steps_per_control=3) + env.sim_cfg = SimpleNamespace(physics_dt=0.01) + env._step_uses_action_manager = not controller_ready + env.action_manager = SimpleNamespace( + requires_substep_update=True, + apply_substep_actions=lambda: calls.append("control"), + ) + + def update(dt, n, *, after_substep=None, render_final_step=True): + for _ in range(n): + calls.append("physics") + if after_substep is not None: + after_substep(dt) + + env.sim = SimpleNamespace(update=update) + env.sensors = { + "contact": SimpleNamespace( + requires_substep_update=True, + begin_control_step=lambda: None, + update_physics_step=lambda dt: calls.append("sensor"), + ) + } + env._advance_physics() + expected = ( + ["physics", "sensor"] * 3 + if controller_ready + else [ + "physics", + "sensor", + "control", + "physics", + "sensor", + "control", + "physics", + "sensor", + ] + ) + assert calls == expected From 1f53f7419f3d5f39382b158807b1fa39a9b0f101 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Wed, 4 Nov 2026 18:57:36 +0000 Subject: [PATCH 2/5] docs(gym): correct manager API heading widths Signed-off-by: yuecideng --- docs/source/api_reference/public_api.rst | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/source/api_reference/public_api.rst b/docs/source/api_reference/public_api.rst index 95d14d25dc..19a70879a4 100644 --- a/docs/source/api_reference/public_api.rst +++ b/docs/source/api_reference/public_api.rst @@ -1315,7 +1315,7 @@ embodichain.lab.gym.envs.managers.action_types ActionTrace embodichain.lab.gym.envs.managers.observation_manager --------------------------------------------------- +----------------------------------------------------- .. currentmodule:: embodichain.lab.gym.envs.managers.observation_manager @@ -1324,7 +1324,7 @@ embodichain.lab.gym.envs.managers.observation_manager ObservationManager embodichain.lab.gym.envs.managers.reward_manager ---------------------------------------------- +------------------------------------------------ .. currentmodule:: embodichain.lab.gym.envs.managers.reward_manager From e9cfe070af41c2db5a83335ca56e1c2c2a97b012 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Thu, 5 Nov 2026 11:24:02 +0000 Subject: [PATCH 3/5] refactor(gym): keep action manager scheduling in embodied env Signed-off-by: yuecideng --- .../topics/env-framework/env-framework.md | 8 +++-- .../topics/manager-functor/manager-functor.md | 3 +- embodichain/lab/gym/envs/base_env.py | 16 ---------- embodichain/lab/gym/envs/embodied_env.py | 22 +++++++++++++ tests/gym/envs/test_substep_sensors.py | 32 ++++++++++++++----- 5 files changed, 53 insertions(+), 28 deletions(-) diff --git a/agent_context/topics/env-framework/env-framework.md b/agent_context/topics/env-framework/env-framework.md index d3cc8c9b54..b8223b1c30 100644 --- a/agent_context/topics/env-framework/env-framework.md +++ b/agent_context/topics/env-framework/env-framework.md @@ -202,9 +202,11 @@ the traceback can retain native resources not yet registered with the manager. ### Stateful sensor and action lifecycle -BaseEnv normally starts one simulation-manager update per control interval and -samples participating sensors through its `after_substep` observer. Opted-in -action terms instead refresh between individual physics updates; sensor samples +`BaseEnv._advance_physics()` starts one simulation-manager update per control +interval and samples participating sensors through its `after_substep` observer. +`EmbodiedEnv._advance_physics()` owns ActionManager substep scheduling and delegates +ordinary actions to the base implementation. Opted-in action terms refresh +between individual physics updates; sensor samples remain after each completed substep. Direct controller-ready commands bypass this refresh path; the explicit expert ActionManager route participates. See the sensor-owned [contact history contract](../sensor-system/contact-history.md) for interval accumulation, counterpart filtering and selective reset. diff --git a/agent_context/topics/manager-functor/manager-functor.md b/agent_context/topics/manager-functor/manager-functor.md index eeb9ca9832..90eeda3547 100644 --- a/agent_context/topics/manager-functor/manager-functor.md +++ b/agent_context/topics/manager-functor/manager-functor.md @@ -98,7 +98,8 @@ The config attribute name is the stable functor identity. For a flat policy tensor, the environment calls manager processing, manager application, physics, event `interval`, observation, and reward in that order. -A term can opt into physics-substep refresh. Its first command is processed and +A term can opt into physics-substep refresh, scheduled by `EmbodiedEnv`. +Its first command is processed and applied normally; subsequent substeps call `update_substep` and apply only the opted-in terms. Inactive demo rows retain measured holds on every refresh. `expert_trajectory.use_action_manager` routes position-only expert commands diff --git a/embodichain/lab/gym/envs/base_env.py b/embodichain/lab/gym/envs/base_env.py index 3c38674d42..76cf5551be 100644 --- a/embodichain/lab/gym/envs/base_env.py +++ b/embodichain/lab/gym/envs/base_env.py @@ -629,22 +629,6 @@ def _advance_physics(self) -> None: sensors = [ sensor for sensor in self.sensors.values() if sensor.requires_substep_update ] - manager = getattr(self, "action_manager", None) - update_actions = ( - getattr(self, "_step_uses_action_manager", False) - and manager is not None - and manager.requires_substep_update - ) - if update_actions: - for sensor in sensors: - sensor.begin_control_step() - for substep in range(self.cfg.sim_steps_per_control): - if substep: - manager.apply_substep_actions() - self.sim.update(self.physics_dt, 1, render_final_step=False) - for sensor in sensors: - sensor.update_physics_step(self.physics_dt) - return if not sensors: self.sim.update( self.physics_dt, diff --git a/embodichain/lab/gym/envs/embodied_env.py b/embodichain/lab/gym/envs/embodied_env.py index b75f2b9225..11b27cbd95 100644 --- a/embodichain/lab/gym/envs/embodied_env.py +++ b/embodichain/lab/gym/envs/embodied_env.py @@ -2485,6 +2485,28 @@ def _step_action(self, action: EnvAction | ControllerAction) -> EnvAction: return action return EmbodiedEnv._apply_controller_action(self, action) + def _advance_physics(self) -> None: + """Refresh opted-in action terms between physics substeps.""" + manager = getattr(self, "action_manager", None) + if ( + not getattr(self, "_step_uses_action_manager", False) + or manager is None + or not manager.requires_substep_update + ): + return super()._advance_physics() + + sensors = [ + sensor for sensor in self.sensors.values() if sensor.requires_substep_update + ] + for sensor in sensors: + sensor.begin_control_step() + for substep in range(self.cfg.sim_steps_per_control): + if substep: + manager.apply_substep_actions() + self.sim.update(self.physics_dt, 1, render_final_step=False) + for sensor in sensors: + sensor.update_physics_step(self.physics_dt) + @property def last_action_trace(self) -> ActionTrace | None: """Return the most recent ActionManager request/command trace.""" diff --git a/tests/gym/envs/test_substep_sensors.py b/tests/gym/envs/test_substep_sensors.py index 23837f6dee..7fb7e14230 100644 --- a/tests/gym/envs/test_substep_sensors.py +++ b/tests/gym/envs/test_substep_sensors.py @@ -18,13 +18,14 @@ from types import SimpleNamespace import pytest -from embodichain.lab.gym.envs import BaseEnv +from embodichain.lab.gym.envs import BaseEnv, EmbodiedEnv +@pytest.mark.parametrize("env_type", (BaseEnv, EmbodiedEnv)) @pytest.mark.parametrize("enabled", (False, True)) -def test_standard_step_schedules_sensor_after_each_physics_substep(enabled): +def test_standard_step_schedules_sensor_after_each_physics_substep(env_type, enabled): events = [] - env = BaseEnv.__new__(BaseEnv) + env = env_type.__new__(env_type) env.cfg = SimpleNamespace(sim_steps_per_control=3) env.sim_cfg = SimpleNamespace(physics_dt=0.01) @@ -52,22 +53,36 @@ def update(dt, n, *, after_substep=None, render_final_step=True): assert events == expected -@pytest.mark.parametrize("controller_ready", [False, True]) +@pytest.mark.parametrize( + "env_type,controller_ready,substep_actions,refreshes", + [ + (EmbodiedEnv, False, True, True), + (EmbodiedEnv, True, True, False), + (EmbodiedEnv, False, False, False), + (BaseEnv, False, True, False), + ], +) def test_substep_action_updates_interleave_with_physics_and_sensor_observation( + env_type, controller_ready, + substep_actions, + refreshes, ): - """Policy controllers refresh between steps; ready joint commands bypass them.""" + """Only opted-in EmbodiedEnv policy controllers refresh between steps.""" calls = [] - env = BaseEnv.__new__(BaseEnv) + updates = [] + env = env_type.__new__(env_type) env.cfg = SimpleNamespace(sim_steps_per_control=3) env.sim_cfg = SimpleNamespace(physics_dt=0.01) env._step_uses_action_manager = not controller_ready env.action_manager = SimpleNamespace( - requires_substep_update=True, + requires_substep_update=substep_actions, apply_substep_actions=lambda: calls.append("control"), ) def update(dt, n, *, after_substep=None, render_final_step=True): + assert render_final_step is False + updates.append(n) for _ in range(n): calls.append("physics") if after_substep is not None: @@ -84,7 +99,7 @@ def update(dt, n, *, after_substep=None, render_final_step=True): env._advance_physics() expected = ( ["physics", "sensor"] * 3 - if controller_ready + if not refreshes else [ "physics", "sensor", @@ -97,3 +112,4 @@ def update(dt, n, *, after_substep=None, render_final_step=True): ] ) assert calls == expected + assert updates == ([1, 1, 1] if refreshes else [3]) From bc6e9cebea47dfe1ba8bf520b05c6e1e50b7b2b4 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Thu, 5 Nov 2026 11:48:29 +0000 Subject: [PATCH 4/5] fix(gym): align substep controllers with solver contracts Signed-off-by: yuecideng --- .../topics/env-framework/env-framework.md | 7 +- .../topics/manager-functor/manager-functor.md | 7 +- .../embodichain.lab.gym.envs.managers.rst | 7 +- embodichain/lab/gym/envs/embodied_env.py | 22 +- embodichain/lab/gym/envs/managers/__init__.py | 1 + .../lab/gym/envs/managers/action_manager.py | 14 +- embodichain/lab/gym/envs/managers/actions.py | 182 +++++++------- .../gym/envs/managers/test_action_manager.py | 234 +++++++++++++----- tests/gym/envs/test_replay_actions.py | 58 +++-- tests/gym/envs/test_substep_sensors.py | 2 +- tests/sim/test_sim_manager.py | 37 +++ 11 files changed, 365 insertions(+), 206 deletions(-) diff --git a/agent_context/topics/env-framework/env-framework.md b/agent_context/topics/env-framework/env-framework.md index b8223b1c30..d4fbf01b86 100644 --- a/agent_context/topics/env-framework/env-framework.md +++ b/agent_context/topics/env-framework/env-framework.md @@ -205,9 +205,10 @@ the traceback can retain native resources not yet registered with the manager. `BaseEnv._advance_physics()` starts one simulation-manager update per control interval and samples participating sensors through its `after_substep` observer. `EmbodiedEnv._advance_physics()` owns ActionManager substep scheduling and delegates -ordinary actions to the base implementation. Opted-in action terms refresh -between individual physics updates; sensor samples -remain after each completed substep. Direct controller-ready commands bypass +ordinary actions to the base implementation. It uses the same batched simulation +update, sampling sensors before refreshing actions for the next substep. Intermediate +recording and visualization frames retain the simulation manager's publication cadence. +Direct controller-ready commands bypass this refresh path; the explicit expert ActionManager route participates. See the sensor-owned [contact history contract](../sensor-system/contact-history.md) for interval accumulation, counterpart filtering and selective reset. EmbodiedEnv resets action and event managers before applying reset-mode events. diff --git a/agent_context/topics/manager-functor/manager-functor.md b/agent_context/topics/manager-functor/manager-functor.md index 90eeda3547..5e7bc5ead8 100644 --- a/agent_context/topics/manager-functor/manager-functor.md +++ b/agent_context/topics/manager-functor/manager-functor.md @@ -99,13 +99,14 @@ The config attribute name is the stable functor identity. For a flat policy tensor, the environment calls manager processing, manager application, physics, event `interval`, observation, and reward in that order. A term can opt into physics-substep refresh, scheduled by `EmbodiedEnv`. -Its first command is processed and -applied normally; subsequent substeps call `update_substep` and apply only the +Its first command is processed and applied normally; subsequent substeps +call `update_substep` and apply only the opted-in terms. Inactive demo rows retain measured holds on every refresh. `expert_trajectory.use_action_manager` routes position-only expert commands through `ActionManager.process_joint_targets`. Relative substep DLS and binary gripper terms support that route, retaining exact FK targets and the expert -storage schema while using the policy controller. +storage schema while using the policy controller. Completed rows are masked +after target conversion so binary grippers also retain measured joint holds. `ControllerAction` otherwise bypasses the manager for direct controller writes; the full loop and reset ordering are owned by [environment execution](../env-framework/execution.md). diff --git a/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.managers.rst b/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.managers.rst index 4d4d16f4e3..e39d073a73 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.managers.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.managers.rst @@ -198,15 +198,18 @@ the applied increment and both forms of limiting; resets clear selected rows. ``RelativeEefDlsAction`` keeps an unbounded xyz/rotation-vector request as one fixed arena-frame target for the control interval. It recomputes the damped Jacobian solve from measured joints before each physics substep and clips only -joint-position limits. Its ``eef_pose.relative_substep_dls@1`` contract selects +joint-position limits. It shares solver-order mapping and arena/TCP Jacobian +adaptation with the single-step relative controller. Its ``eef_pose.relative_substep_dls@1`` contract selects this behavior separately from the bounded, single-step relative-pose contract. -The unchanged physics loop remains batched for terms without substep updates. +``EmbodiedEnv`` refreshes action terms through the batched simulation callback, +preserving intermediate recording and visualization frames. Configure the same arm and binary gripper terms for expert and policy execution. Set ``env.expert_trajectory.use_action_manager: true`` to route position-only ``ControllerAction`` commands through ``ActionManager.process_joint_targets``. The DLS term retains the expert's exact FK target while recording the corresponding relative request, and keeps the existing expert storage schema. +Completed demo rows retain measured joint holds after conversion and refresh. The default controller-ready route stays direct. Every configured term must support joint-target processing. For binary grippers, ``open_on_zero=False`` makes zero a closing request; diff --git a/embodichain/lab/gym/envs/embodied_env.py b/embodichain/lab/gym/envs/embodied_env.py index 11b27cbd95..2d6716d05c 100644 --- a/embodichain/lab/gym/envs/embodied_env.py +++ b/embodichain/lab/gym/envs/embodied_env.py @@ -2500,12 +2500,22 @@ def _advance_physics(self) -> None: ] for sensor in sensors: sensor.begin_control_step() - for substep in range(self.cfg.sim_steps_per_control): - if substep: - manager.apply_substep_actions() - self.sim.update(self.physics_dt, 1, render_final_step=False) + substep = 0 + + def observe_substep(dt: float) -> None: + nonlocal substep for sensor in sensors: - sensor.update_physics_step(self.physics_dt) + sensor.update_physics_step(dt) + substep += 1 + if substep < self.cfg.sim_steps_per_control: + manager.apply_substep_actions() + + self.sim.update( + self.physics_dt, + self.cfg.sim_steps_per_control, + after_substep=observe_substep, + render_final_step=False, + ) @property def last_action_trace(self) -> ActionTrace | None: @@ -2713,7 +2723,7 @@ def _preprocess_action( self.action_manager.process_joint_targets(full_targets) else: self.action_manager.process_action(action) - if getattr(self, "_demo_no_auto_reset", False) and not managed_expert: + if getattr(self, "_demo_no_auto_reset", False): self.action_manager.mask_inactive(self._demo_active_mask) if self.dataset_manager is not None and not policy_recording: processed_qpos = getattr(self.action_manager, "_processed_qpos", None) diff --git a/embodichain/lab/gym/envs/managers/__init__.py b/embodichain/lab/gym/envs/managers/__init__.py index ea9a255ffe..5482d682b6 100644 --- a/embodichain/lab/gym/envs/managers/__init__.py +++ b/embodichain/lab/gym/envs/managers/__init__.py @@ -72,6 +72,7 @@ "JointPositionToLimitsAction", "JointVelocityAction", "ParallelGripperAction", + "RelativeEefDlsAction", "RelativeEefPoseAction", "RelativeJointPositionAction", "register_action_contract", diff --git a/embodichain/lab/gym/envs/managers/action_manager.py b/embodichain/lab/gym/envs/managers/action_manager.py index 358a449609..5c426c3ad3 100644 --- a/embodichain/lab/gym/envs/managers/action_manager.py +++ b/embodichain/lab/gym/envs/managers/action_manager.py @@ -220,7 +220,12 @@ def requires_substep_update(self) -> bool: """Whether any term refreshes its command between physics substeps.""" return bool(self._substep_terms) - def _validate_joint_targets(self, qpos: torch.Tensor) -> None: + def process_joint_targets(self, qpos: torch.Tensor) -> None: + """Process expert targets while preserving exact FK target poses. + + Args: + qpos: Finite full-joint position targets on the environment device. + """ expected = self._env.robot.get_qpos().shape if ( not isinstance(qpos, torch.Tensor) @@ -233,13 +238,6 @@ def _validate_joint_targets(self, qpos: torch.Tensor) -> None: f"Expected finite joint targets with shape {expected} on {self._env.device}." ) - def process_joint_targets(self, qpos: torch.Tensor) -> None: - """Process expert targets while preserving exact FK target poses. - - Args: - qpos: Finite full-joint position targets on the environment device. - """ - self._validate_joint_targets(qpos) self._previous_action.copy_(self._action) self._substep_active_mask = None for term in self._terms.values(): diff --git a/embodichain/lab/gym/envs/managers/actions.py b/embodichain/lab/gym/envs/managers/actions.py index 36955b2fb9..a66c63233d 100644 --- a/embodichain/lab/gym/envs/managers/actions.py +++ b/embodichain/lab/gym/envs/managers/actions.py @@ -577,6 +577,57 @@ def part_name(self) -> str: """Return the robot control part used for IK and FK.""" return self._part_name + def _prepare_solver(self, *, dls: bool) -> None: + self._solver = self._env.robot.get_solver(self._part_name) + solver_names = tuple(getattr(self._solver, "joint_names", ()) or ()) + if ( + len(solver_names) != len(self._joint_names) + or len(set(solver_names)) != len(solver_names) + or set(solver_names) != set(self._joint_names) + ): + raise ValueError( + "Relative EEF actions require solver joint_names matching selected joints." + ) + chain = getattr(self._solver, "pk_serial_chain", None) + chain_names = getattr(chain, "get_joint_parameter_names", None) + if callable(chain_names) and tuple(chain_names()) != solver_names: + raise ValueError("Solver joint_names must match its kinematic chain order.") + self._solver_order = [self._joint_names.index(name) for name in solver_names] + self._selection_order = [solver_names.index(name) for name in self._joint_names] + if not dls: + return + tcp = torch.as_tensor( + self._solver.tcp_xpos, dtype=torch.float32, device=self.device + ) + if tcp.shape != (4, 4) or not bool(torch.isfinite(tcp).all()): + raise ValueError("DLS requires a finite 4-by-4 solver TCP transform.") + self._tcp_offset = tcp[:3, 3] + self._tcp_rotation = tcp[:3, :3] + self._has_tcp_offset = bool(self._tcp_offset.any()) + + def _arena_tcp_jacobian( + self, current: torch.Tensor, current_pose: torch.Tensor + ) -> torch.Tensor: + """Return solver-order Jacobian columns at the arena-frame TCP.""" + # Ask for the end-link Jacobian without PK's locations adapter, whose + # Transform3d construction can allocate CPU tensors for CUDA offsets. + jacobian = self._solver.get_jacobian(current[:, self._solver_order]) + if tuple(jacobian.shape) != (self.num_envs, 6, len(self._joint_ids)): + raise ValueError("DLS requires a batched full 6-by-DOF Jacobian.") + rotation = self._env.robot.get_link_pose( + link_name=self._solver.root_link_name, to_matrix=True + )[:, :3, :3] + # Preserve solver strides to retain CUDA DLS reduction order. + converted = torch.empty_like(jacobian) + converted[:, :3] = rotation @ jacobian[:, :3] + converted[:, 3:] = rotation @ jacobian[:, 3:] + if self._has_tcp_offset: + link_rotation = current_pose[:, :3, :3] @ self._tcp_rotation.T + offset = (link_rotation @ self._tcp_offset).unsqueeze(1) + shift = torch.cross(converted[:, 3:].transpose(1, 2), offset, dim=-1) + converted[:, :3] += shift.transpose(1, 2) + return converted + def process_actions(self, actions: torch.Tensor) -> None: expected = (self.num_envs, self.action_dim) if tuple(actions.shape) != expected: @@ -655,7 +706,7 @@ def __init__(self, cfg: ActionTermCfg, env: EmbodiedEnv) -> None: ): if not np.isfinite(value) or value <= 0: raise ValueError(f"{name} must be positive and finite; found {value}.") - self._solver = env.robot.get_solver(self._part_name) + self._prepare_solver(dls=True) self._target_pose = torch.eye(4, device=self.device).repeat(self.num_envs, 1, 1) self._identity = torch.eye(6, device=self.device) self._descriptor = ActionTermDescriptor( @@ -684,40 +735,14 @@ def __init__(self, cfg: ActionTermCfg, env: EmbodiedEnv) -> None: contract=self.resolved_contract_id(), ) - def _measured_pose(self) -> torch.Tensor: + def _compute_pose(self, qpos: torch.Tensor) -> torch.Tensor: + selected = qpos[:, list(self._joint_ids)] return self._env.robot.compute_fk( - self._env.robot.get_qpos()[:, list(self._joint_ids)], + qpos=selected[:, self._solver_order], name=self._part_name, to_matrix=True, ) - def _action_from_joint_targets(self, qpos: torch.Tensor) -> torch.Tensor: - """Express a joint-position expert target as the same relative request. - - Args: - qpos: Full robot joint targets, shaped ``(num_envs, dof)``. - - Returns: - Unbounded xyz/rotation-vector actions, shaped ``(num_envs, 6)``. - """ - from scipy.spatial.transform import Rotation - - current = self._measured_pose() - target = self._env.robot.compute_fk( - qpos[:, list(self._joint_ids)], name=self._part_name, to_matrix=True - ) - delta = Rotation.from_matrix( - (target[:, :3, :3] @ current[:, :3, :3].mT).detach().cpu().numpy() - ).as_rotvec() - rotation = torch.as_tensor(delta, dtype=current.dtype, device=self.device) - return torch.cat( - ( - (target[:, :3, 3] - current[:, :3, 3]) / self._translation_scale, - rotation / self._rotation_scale, - ), - dim=-1, - ) - def process_actions(self, actions: torch.Tensor) -> None: """Set a target from measured pose without clipping the request. @@ -738,7 +763,7 @@ def process_actions(self, actions: torch.Tensor) -> None: ) self._previous_raw_actions.copy_(self._raw_actions) self._raw_actions.copy_(actions) - current = self._measured_pose() + current = self._compute_pose(self._env.robot.get_qpos()) self._target_pose.copy_(current) self._target_pose[:, :3, 3] += actions[:, :3] * self._translation_scale delta = Rotation.from_rotvec( @@ -756,24 +781,32 @@ def process_joint_targets(self, qpos: torch.Tensor) -> None: Args: qpos: Full robot joint-position expert targets. """ - actions = self._action_from_joint_targets(qpos) + from scipy.spatial.transform import Rotation + + current = self._compute_pose(self._env.robot.get_qpos()) + target = self._compute_pose(qpos) + delta = Rotation.from_matrix( + (target[:, :3, :3] @ current[:, :3, :3].mT).detach().cpu().numpy() + ).as_rotvec() + actions = torch.cat( + ( + (target[:, :3, 3] - current[:, :3, 3]) / self._translation_scale, + torch.as_tensor(delta, dtype=current.dtype, device=self.device) + / self._rotation_scale, + ), + dim=-1, + ) self._previous_raw_actions.copy_(self._raw_actions) self._raw_actions.copy_(actions) - self._target_pose.copy_( - self._env.robot.compute_fk( - qpos[:, list(self._joint_ids)], - name=self._part_name, - to_matrix=True, - ) - ) + self._target_pose.copy_(target) self.update_substep() def update_substep(self) -> None: """Recompute bounded joint targets toward the held EEF target.""" from scipy.spatial.transform import Rotation - current = self._measured_pose() qpos = self._env.robot.get_qpos().clone() + current = self._compute_pose(qpos) delta = Rotation.from_matrix( (self._target_pose[:, :3, :3] @ current[:, :3, :3].mT) .detach() @@ -787,21 +820,19 @@ def update_substep(self) -> None: ), dim=-1, ) - jacobian = self._env.robot.compute_jacobian( - qpos, - end_link_name=self._solver.end_link_name, - root_link_name=self._solver.root_link_name, - ) + selected = qpos[:, list(self._joint_ids)] + jacobian = self._arena_tcp_jacobian(selected, current) transpose = jacobian.mT - increment = transpose @ torch.linalg.solve( - jacobian @ transpose + self._damping**2 * self._identity, - error.unsqueeze(-1), - ) + increment = ( + transpose + @ torch.linalg.solve( + jacobian @ transpose + self._damping**2 * self._identity, + error.unsqueeze(-1), + ) + ).squeeze(-1)[:, self._selection_order] limits = self._env.robot.get_qpos_limits()[:, list(self._joint_ids)] self._processed_actions.copy_( - (qpos[:, list(self._joint_ids)] + increment.squeeze(-1)).clamp( - limits[..., 0], limits[..., 1] - ) + (selected + increment).clamp(limits[..., 0], limits[..., 1]) ) self._ik_success.copy_(torch.isfinite(self._processed_actions).all(dim=-1)) if not bool(self._ik_success.all()): @@ -875,7 +906,7 @@ def __init__(self, cfg: ActionTermCfg, env: EmbodiedEnv) -> None: self._joint_limit_clipped_mask = torch.zeros_like( self._processed_actions, dtype=torch.bool ) - self._prepare_solver() + self._prepare_solver(dls=self._ik_method == "dls") dls_metadata = {} if self._ik_method == "dls": dls_metadata = { @@ -945,53 +976,10 @@ def joint_limit_clipped_mask(self) -> torch.Tensor: """Return DLS joint components clipped to position limits.""" return self._joint_limit_clipped_mask - def _prepare_solver(self) -> None: - self._solver = self._env.robot.get_solver(self._part_name) - solver_names = tuple(getattr(self._solver, "joint_names", ()) or ()) - if ( - len(solver_names) != len(self._joint_names) - or len(set(solver_names)) != len(solver_names) - or set(solver_names) != set(self._joint_names) - ): - raise ValueError( - "Relative EEF actions require solver joint_names matching selected joints." - ) - chain = getattr(self._solver, "pk_serial_chain", None) - chain_names = getattr(chain, "get_joint_parameter_names", None) - if callable(chain_names) and tuple(chain_names()) != solver_names: - raise ValueError("Solver joint_names must match its kinematic chain order.") - self._solver_order = [self._joint_names.index(name) for name in solver_names] - self._selection_order = [solver_names.index(name) for name in self._joint_names] - if self._ik_method != "dls": - return - tcp = torch.as_tensor( - self._solver.tcp_xpos, dtype=torch.float32, device=self.device - ) - if tcp.shape != (4, 4) or not bool(torch.isfinite(tcp).all()): - raise ValueError("DLS requires a finite 4-by-4 solver TCP transform.") - self._tcp_offset = tcp[:3, 3] - self._tcp_rotation = tcp[:3, :3] - self._has_tcp_offset = bool(self._tcp_offset.any()) - def _dls_step( self, current: torch.Tensor, error: torch.Tensor ) -> tuple[torch.Tensor, torch.Tensor]: - # Ask for the end-link Jacobian without PK's locations adapter, whose - # Transform3d construction can allocate CPU tensors for CUDA offsets. - jacobian = self._solver.get_jacobian(current[:, self._solver_order]) - if tuple(jacobian.shape) != (self.num_envs, 6, len(self._joint_ids)): - raise ValueError("DLS requires a batched full 6-by-DOF Jacobian.") - rotation = self._env.robot.get_link_pose( - link_name=self._solver.root_link_name, to_matrix=True - )[:, :3, :3] - jacobian = torch.cat( - (rotation @ jacobian[:, :3], rotation @ jacobian[:, 3:]), dim=1 - ) - if self._has_tcp_offset: - link_rotation = self._current_pose[:, :3, :3] @ self._tcp_rotation.T - offset = (link_rotation @ self._tcp_offset).unsqueeze(1) - shift = torch.cross(jacobian[:, 3:].transpose(1, 2), offset, dim=-1) - jacobian[:, :3] += shift.transpose(1, 2) + jacobian = self._arena_tcp_jacobian(current, self._current_pose).contiguous() finite = torch.isfinite(jacobian).all(dim=(1, 2)) jacobian = torch.where(finite[:, None, None], jacobian, 0.0) transpose = jacobian.transpose(-1, -2) diff --git a/tests/gym/envs/managers/test_action_manager.py b/tests/gym/envs/managers/test_action_manager.py index 3858461140..8791e5db3e 100644 --- a/tests/gym/envs/managers/test_action_manager.py +++ b/tests/gym/envs/managers/test_action_manager.py @@ -33,6 +33,7 @@ JointPositionToLimitsAction, JointVelocityAction, ParallelGripperAction, + RelativeEefDlsAction, RelativeEefPoseAction, RelativeJointPositionAction, resolve_action_contract, @@ -696,33 +697,66 @@ def dls_eef_env(relative_eef_env: SimpleNamespace) -> SimpleNamespace: env.solver = SimpleNamespace( joint_names=names, root_link_name="solver_root", + end_link_name="solver_tool", pk_serial_chain=SimpleNamespace(get_joint_parameter_names=lambda: names), tcp_xpos=np.eye(4), get_jacobian=MagicMock(return_value=jacobian), ) env.robot.get_solver = MagicMock(return_value=env.solver) env.robot.get_link_pose = MagicMock(return_value=torch.eye(4).repeat(2, 1, 1)) + env.robot.get_qpos_limits = lambda: env.robot.body_data.qpos_limits + + def full_state_jacobian(qpos, **kwargs): + solver_ids = [ + env.robot.joint_names.index(name) for name in env.solver.joint_names + ] + return env.solver.get_jacobian(qpos[:, solver_ids]) + + env.robot.compute_jacobian = full_state_jacobian env.robot.get_control_part_base_pose = MagicMock( return_value=torch.eye(4).repeat(2, 1, 1) ) return env +@pytest.mark.parametrize( + "action_type,damping", + [(RelativeEefPoseAction, 0.05), (RelativeEefDlsAction, 0.01)], +) def test_dls_rotates_linear_and_angular_jacobian_into_arena( dls_eef_env: SimpleNamespace, + action_type: type, + damping: float, ) -> None: """A root rotated by pi about Z reverses both arena-X Jacobian blocks.""" env = dls_eef_env env.robot.get_link_pose.return_value[:, :3, :3] = torch.diag( torch.tensor([-1.0, -1.0, 1.0]) ) - term = RelativeEefPoseAction( - make_cfg(RelativeEefPoseAction, part_name="arm", ik_method="dls"), env + env.robot.compute_fk.return_value[:, :3, :3] = torch.diag( + torch.tensor([-1.0, -1.0, 1.0]) + ) + params = ( + {"damping": damping} + if action_type is RelativeEefDlsAction + else {"ik_method": "dls", "dls_damping": damping} + ) + term = action_type( + make_cfg( + action_type, + part_name="arm", + translation_scale=0.02, + rotation_scale=0.10, + **params, + ), + env, ) term.process_actions(torch.tensor([[1.0, 0, 0, 1.0, 0, 0]]).repeat(2, 1)) - expected = torch.tensor([[-0.02, -0.10]]).repeat(2, 1) / (1 + 0.05**2) - torch.testing.assert_close(term.joint_delta, expected) + expected = torch.tensor([[-0.02, -0.10]]).repeat(2, 1) / (1 + damping**2) + torch.testing.assert_close( + (term.processed_actions - env.robot.get_qpos()[:, [2, 0]]), expected + ) env.robot.compute_ik.assert_not_called() torch.testing.assert_close( env.solver.get_jacobian.call_args.args[0], @@ -736,8 +770,14 @@ def test_dls_rotates_linear_and_angular_jacobian_into_arena( assert term.ik_success.all() +@pytest.mark.parametrize( + "action_type,damping", + [(RelativeEefPoseAction, 0.05), (RelativeEefDlsAction, 0.01)], +) def test_dls_shifts_nonzero_tcp_jacobian_without_pk_locations( dls_eef_env: SimpleNamespace, + action_type: type, + damping: float, ) -> None: """TCP rotation is removed before shifting the arena Jacobian to its offset.""" env = dls_eef_env @@ -750,16 +790,31 @@ def test_dls_shifts_nonzero_tcp_jacobian_without_pk_locations( env.robot.compute_fk.return_value[:, :3, :3] = torch.diag( torch.tensor([-1.0, -1.0, 1.0]) ) - term = RelativeEefPoseAction( - make_cfg(RelativeEefPoseAction, part_name="arm", ik_method="dls"), env + params = ( + {"damping": damping} + if action_type is RelativeEefDlsAction + else {"ik_method": "dls", "dls_damping": damping} + ) + term = action_type( + make_cfg( + action_type, + part_name="arm", + translation_scale=0.02, + rotation_scale=0.10, + **params, + ), + env, ) term.process_actions(torch.tensor([[1.0, 0, 0, 0, 0, 0]]).repeat(2, 1)) # End-link Rz(pi/2) puts its +X offset along arena +Y, so angular +Z # induces linear -X. The scalar DLS denominator includes both Jacobian rows. - expected = torch.tensor([[-0.2 * 0.02 / (1 + 0.2**2 + 0.05**2), 0.0]]) - torch.testing.assert_close(term.joint_delta, expected.repeat(2, 1)) + expected = torch.tensor([[-0.2 * 0.02 / (1 + 0.2**2 + damping**2), 0.0]]) + torch.testing.assert_close( + (term.processed_actions - env.robot.get_qpos()[:, [2, 0]]), + expected.repeat(2, 1), + ) assert env.solver.get_jacobian.call_args.kwargs == {} assert term.ik_success.all() @@ -859,22 +914,42 @@ def test_dls_nonfinite_jacobian_holds_row_and_partial_reset_preserves_others( torch.testing.assert_close(getattr(term, name)[1], untouched[name]) +@pytest.mark.parametrize( + "action_type,damping", + [(RelativeEefPoseAction, 0.05), (RelativeEefDlsAction, 0.01)], +) def test_dls_maps_joint_order_between_selected_joints_and_solver_chain( dls_eef_env: SimpleNamespace, + action_type: type, + damping: float, ) -> None: """A permuted configured selection cannot change Jacobian column semantics.""" env = dls_eef_env names = ("joint_0", "joint_2") env.solver.joint_names = names env.solver.pk_serial_chain.get_joint_parameter_names = lambda: names - term = RelativeEefPoseAction( - make_cfg(RelativeEefPoseAction, part_name="arm", ik_method="dls"), env + params = ( + {"damping": damping} + if action_type is RelativeEefDlsAction + else {"ik_method": "dls", "dls_damping": damping} + ) + term = action_type( + make_cfg( + action_type, + part_name="arm", + translation_scale=0.02, + rotation_scale=0.10, + **params, + ), + env, ) term.process_actions(torch.tensor([[1.0, 0, 0, 0, 0, 0]]).repeat(2, 1)) term.apply_actions() - expected = torch.tensor([[0.0, 0.02]]).repeat(2, 1) / (1 + 0.05**2) - torch.testing.assert_close(term.joint_delta, expected) + expected = torch.tensor([[0.0, 0.02]]).repeat(2, 1) / (1 + damping**2) + torch.testing.assert_close( + (term.processed_actions - env.robot.get_qpos()[:, [2, 0]]), expected + ) torch.testing.assert_close( env.robot.compute_fk.call_args.kwargs["qpos"], torch.tensor([[0.1, 0.3], [0.4, 0.6]]), @@ -889,32 +964,56 @@ def test_dls_maps_joint_order_between_selected_joints_and_solver_chain( @pytest.mark.parametrize( "names", [None, ("joint_0", "unknown"), ("joint_0", "joint_0")] ) -@pytest.mark.parametrize("ik_method", ["solver", "dls"]) +@pytest.mark.parametrize( + "action_type,ik_method", + [ + (RelativeEefPoseAction, "solver"), + (RelativeEefPoseAction, "dls"), + (RelativeEefDlsAction, None), + ], +) def test_dls_rejects_unknown_or_ambiguous_solver_joint_order( dls_eef_env: SimpleNamespace, names: tuple[str, ...] | None, - ik_method: str, + action_type: type, + ik_method: str | None, ) -> None: dls_eef_env.solver.joint_names = names with pytest.raises(ValueError, match="joint_names"): - RelativeEefPoseAction( - make_cfg(RelativeEefPoseAction, part_name="arm", ik_method=ik_method), + action_type( + make_cfg( + action_type, + part_name="arm", + **({"ik_method": ik_method} if ik_method else {}), + ), dls_eef_env, ) -@pytest.mark.parametrize("ik_method", ["solver", "dls"]) +@pytest.mark.parametrize( + "action_type,ik_method", + [ + (RelativeEefPoseAction, "solver"), + (RelativeEefPoseAction, "dls"), + (RelativeEefDlsAction, None), + ], +) def test_dls_rejects_configured_names_that_disagree_with_chain_order( dls_eef_env: SimpleNamespace, - ik_method: str, + action_type: type, + ik_method: str | None, ) -> None: dls_eef_env.solver.pk_serial_chain.get_joint_parameter_names = lambda: ( "joint_0", "joint_2", ) with pytest.raises(ValueError, match="kinematic chain order"): - RelativeEefPoseAction( - make_cfg(RelativeEefPoseAction, part_name="arm", ik_method=ik_method), + action_type( + make_cfg( + action_type, + part_name="arm", + **({"ik_method": ik_method} if ik_method else {}), + ), dls_eef_env, ) @@ -989,20 +1088,19 @@ def test_relative_eef_solver_maps_fk_seed_and_returned_joint_order( @pytest.mark.parametrize("batch", [1, 8, 32]) def test_substep_dls_preserves_unbounded_targets_and_expert_conversion(batch): - """Expert and policy requests follow the same held-pose DLS trajectory.""" + """An identity Jacobian gives a closed-form held-target controller step.""" from scipy.spatial.transform import Rotation - from embodichain.lab.gym.envs.managers.actions import RelativeEefDlsAction qpos = torch.zeros(batch, 7) qpos[:, :3] = torch.tensor([0.2, -0.1, 0.3]) limits = torch.tensor([-3.0, 3.0]).expand(batch, 7, 2).clone() jacobian = torch.eye(6, 7).expand(batch, 6, 7).clone() - def fk(values, *, name, to_matrix): + def fk(qpos, *, name, to_matrix): pose = torch.eye(4).repeat(batch, 1, 1) - pose[:, :3, 3] = values[:, :3] + pose[:, :3, 3] = qpos[:, :3] pose[:, :3, :3] = torch.tensor( - Rotation.from_rotvec(values[:, 3:6].numpy()).as_matrix(), dtype=values.dtype + Rotation.from_rotvec(qpos[:, 3:6].numpy()).as_matrix(), dtype=qpos.dtype ) return pose @@ -1013,64 +1111,70 @@ def fk(values, *, name, to_matrix): get_qpos_limits=lambda: limits, compute_fk=fk, compute_jacobian=lambda values, **kwargs: jacobian, + get_link_pose=lambda **kwargs: torch.eye(4).repeat(batch, 1, 1), get_solver=lambda part: SimpleNamespace( - end_link_name="tool", root_link_name="base" + joint_names=tuple(f"joint{i}" for i in range(7)), + end_link_name="tool", + root_link_name="base", + tcp_xpos=np.eye(4), + get_jacobian=lambda values: jacobian, ), set_qpos=lambda **kwargs: None, ) env = SimpleNamespace(num_envs=batch, device="cpu", robot=robot) term = RelativeEefDlsAction(make_cfg(RelativeEefDlsAction, part_name="arm"), env) - action = torch.tensor([1.5, -1.2, 0.1, 0.2, -0.1, 0.3]).expand(batch, 6).clone() - current = fk(qpos, name="arm", to_matrix=True) - target = current.clone() - target[:, :3, 3] += action[:, :3] * 0.5 - target[:, :3, :3] = ( - torch.tensor( - Rotation.from_rotvec((action[:, 3:] * 0.5).numpy()).as_matrix(), - dtype=qpos.dtype, - ) - @ current[:, :3, :3] - ) + action = torch.tensor([1.5, -1.2, 0.1, 0.2, 0.0, 0.0]).expand(batch, 6).clone() planned = qpos.clone() - planned[:, :3] = target[:, :3, 3] - planned[:, 3:6] = torch.tensor( - Rotation.from_matrix(target[:, :3, :3].numpy()).as_rotvec(), dtype=qpos.dtype - ) + planned[:, :6] += action * 0.5 term.process_joint_targets(planned) torch.testing.assert_close(term.raw_actions, action) - target = fk(planned, name="arm", to_matrix=True) + expert_commands = term.processed_actions.clone() + term.process_actions(action) + torch.testing.assert_close(term.processed_actions, expert_commands) assert np.isinf(term.action_space.high).all() for _ in range(5): - pose = fk(qpos, name="arm", to_matrix=True) - error = torch.cat( - ( - target[:, :3, 3] - pose[:, :3, 3], - torch.tensor( - Rotation.from_matrix( - (target[:, :3, :3] @ pose[:, :3, :3].mT).numpy() - ).as_rotvec(), - dtype=qpos.dtype, - ), - ), - -1, - ) - expected = ( - qpos - + ( - jacobian.mT - @ torch.linalg.solve( - jacobian @ jacobian.mT + 0.01**2 * torch.eye(6), error.unsqueeze(-1) - ) - ).squeeze(-1) - ).clamp(limits[..., 0], limits[..., 1]) + expected = qpos.clone() + expected[:, :6] += (planned[:, :6] - qpos[:, :6]) / (1 + 0.01**2) term.update_substep() - torch.testing.assert_close(term.processed_actions, expected, rtol=0, atol=0) + torch.testing.assert_close(term.processed_actions, expected, rtol=0, atol=1e-6) qpos.copy_(qpos + 0.2 * (expected - qpos)) preserved = term.raw_actions[1:].clone() term.reset([0]) torch.testing.assert_close(term.raw_actions[1:], preserved) +def test_substep_dls_expert_targets_preserve_solver_and_selected_joint_order( + dls_eef_env: SimpleNamespace, +) -> None: + """Named FK consumes solver order and joint commands retain configured order.""" + env = dls_eef_env + names = ("joint_0", "joint_2") + env.solver.joint_names = names + env.solver.pk_serial_chain.get_joint_parameter_names = lambda: names + + def fk(qpos, *, name, to_matrix): + pose = torch.eye(4).repeat(2, 1, 1) + pose[:, 0, 3] = qpos[:, 0] + pose[:, 2, 3] = qpos[:, 1] + return pose + + env.robot.compute_fk.side_effect = fk + term = RelativeEefDlsAction(make_cfg(RelativeEefDlsAction, part_name="arm"), env) + planned = env.robot.get_qpos().clone() + planned[:, 0] += 0.05 + planned[:, 2] += 0.02 + term.process_joint_targets(planned) + + torch.testing.assert_close( + term.raw_actions, torch.tensor([[0.1, 0.0, 0.04, 0.0, 0.0, 0.0]]).repeat(2, 1) + ) + expected = env.robot.get_qpos()[:, [2, 0]].clone() + expected[:, 1] += 0.05 / (1 + 0.01**2) + torch.testing.assert_close(term.processed_actions, expected) + term.apply_actions() + assert env.robot.set_qpos.call_args.kwargs["joint_ids"] == [2, 0] + + def test_binary_gripper_conversion_preserves_closed_zero_boundary(): """Legacy zero closes while existing default binary semantics stay unchanged.""" env = make_action_env( diff --git a/tests/gym/envs/test_replay_actions.py b/tests/gym/envs/test_replay_actions.py index 525e7eebdc..de93d21224 100644 --- a/tests/gym/envs/test_replay_actions.py +++ b/tests/gym/envs/test_replay_actions.py @@ -422,37 +422,53 @@ def test_kinematic_task_replay_does_not_claim_measured_goal_acceptance(): def test_expert_opt_in_uses_policy_controller_with_full_joint_target( completed: bool, ) -> None: + """Converted expert targets keep measured holds on completed demo rows.""" + from types import MethodType from unittest.mock import MagicMock - from embodichain.lab.gym.envs.expert_trajectory import ExpertTrajectoryCfg - env = _ReplayEnv(num_envs=1) - env._demo_no_auto_reset = completed - env._demo_active_mask = torch.tensor([not completed]) - from types import MethodType + from embodichain.lab.gym.envs.expert_trajectory import ExpertTrajectoryCfg + from embodichain.lab.gym.envs.managers import ( + ActionManager, + ActionTermCfg, + ParallelGripperAction, + ) + env = _ReplayEnv(num_envs=2) + env._demo_no_auto_reset = True + env._demo_active_mask = torch.tensor([not completed, True]) env._mask_controller_demo_action = MethodType( EmbodiedEnv._mask_controller_demo_action, env ) env.cfg = SimpleNamespace( expert_trajectory=ExpertTrajectoryCfg(use_action_manager=True) ) - manager = SimpleNamespace( - process_joint_targets=MagicMock(), - apply_action=MagicMock(), - action_trace=lambda: None, - action=torch.zeros(1, 7), + measured = torch.tensor([[0.0, 0.015, 0.025], [0.0, 0.01, 0.02]]) + env.robot.get_qpos = lambda: measured + env.robot.get_joint_ids = lambda part, **kwargs: [1, 2] + env.robot.body_data = SimpleNamespace( + qpos_limits=torch.tensor([[[-1.0, 1.0], [0.0, 0.04], [0.0, 0.04]]]) ) - env.action_manager = manager - prepared = env._preprocess_action(ControllerAction(torch.tensor([[0.2, 0.3]]))) - manager.process_joint_targets.assert_called_once() - torch.testing.assert_close( - manager.process_joint_targets.call_args.args[0], - ( - torch.tensor([[0.0, 0.0, 0.0]]) - if completed - else torch.tensor([[0.0, 0.2, 0.3]]) - ), + env.robot.set_qpos = MagicMock() + env.action_manager = ActionManager( + { + "gripper": ActionTermCfg( + func=ParallelGripperAction, + params={ + "part_name": "hand", + "command_mode": "binary", + "open_command": {"elbow": 0.04, "wrist": 0.04}, + "close_command": {"elbow": 0.0, "wrist": 0.0}, + }, + ) + }, + env, ) + requested = torch.full((2, 2), 0.04) + prepared = env._preprocess_action(ControllerAction(requested)) env._step_action(prepared) - manager.apply_action.assert_called_once() + expected = requested.clone() + if completed: + expected[0] = measured[0, 1:] + torch.testing.assert_close(env.robot.set_qpos.call_args.kwargs["qpos"], expected) + torch.testing.assert_close(prepared.value, expected) assert env._step_uses_action_manager diff --git a/tests/gym/envs/test_substep_sensors.py b/tests/gym/envs/test_substep_sensors.py index 7fb7e14230..a27578cb83 100644 --- a/tests/gym/envs/test_substep_sensors.py +++ b/tests/gym/envs/test_substep_sensors.py @@ -112,4 +112,4 @@ def update(dt, n, *, after_substep=None, render_final_step=True): ] ) assert calls == expected - assert updates == ([1, 1, 1] if refreshes else [3]) + assert updates == [3] diff --git a/tests/sim/test_sim_manager.py b/tests/sim/test_sim_manager.py index 00b3c6bc44..2dcb4d7352 100644 --- a/tests/sim/test_sim_manager.py +++ b/tests/sim/test_sim_manager.py @@ -1593,6 +1593,43 @@ def test_final_substep_consumers_share_publication(camera, record, viser): assert len(runtime.capture_calls) == (3 if viser else 0) +def test_embodied_substep_actions_preserve_intermediate_window_frames(): + """Action refresh keeps physics-rate recording without extra camera captures.""" + from embodichain.lab.gym.envs import EmbodiedEnv + + sim, _, state, publications, rendered = _make_render_frame_sim() + sim._window_record_state = _WindowRecordState( + time_step=0.01, + max_memory_bytes=1024, + output_dir="unused", + video_name="unused", + save_kwargs={}, + capture_from_sim_update=True, + ) + recorded = [] + refreshed = [] + sim._capture_window_record_frame = lambda _: recorded.append(state["published"]) + env = EmbodiedEnv.__new__(EmbodiedEnv) + env.cfg = SimpleNamespace(sim_steps_per_control=3) + env.sim_cfg = SimpleNamespace(physics_dt=0.01) + env.sim = sim + env.sensors = {} + env._step_uses_action_manager = True + env.action_manager = SimpleNamespace( + requires_substep_update=True, + apply_substep_actions=lambda: refreshed.append(state["value"]), + ) + + env._advance_physics() + with sim.render_frame(): + sim.render_camera_group([3]) + + assert recorded == [1, 2, 3] + assert publications == [1, 2, 3] + assert rendered == [3] + assert refreshed == [1, 2] + + def test_render_frames_and_independent_reads_refresh_direct_state_writes(): sim, _, state, publications, rendered = _make_render_frame_sim() for value in (4, 7): From 7ef56bd5317f92f7d7a5d2ea61cdc0ef38e1fbeb Mon Sep 17 00:00:00 2001 From: yuecideng Date: Thu, 5 Nov 2026 12:12:15 +0000 Subject: [PATCH 5/5] refactor(gym): move substep scheduling into action manager Signed-off-by: yuecideng --- .../topics/env-framework/env-framework.md | 7 +-- .../topics/manager-functor/manager-functor.md | 11 +++-- .../embodichain.lab.gym.envs.managers.rst | 6 ++- embodichain/lab/gym/envs/embodied_env.py | 17 +++---- .../lab/gym/envs/managers/action_manager.py | 48 ++++++++++++++----- .../envs/managers/test_action_manager_flat.py | 21 +++++++- tests/gym/envs/test_substep_sensors.py | 11 +++-- tests/sim/test_sim_manager.py | 12 +++-- 8 files changed, 92 insertions(+), 41 deletions(-) diff --git a/agent_context/topics/env-framework/env-framework.md b/agent_context/topics/env-framework/env-framework.md index d4fbf01b86..d8e6c7fd79 100644 --- a/agent_context/topics/env-framework/env-framework.md +++ b/agent_context/topics/env-framework/env-framework.md @@ -204,9 +204,10 @@ the traceback can retain native resources not yet registered with the manager. `BaseEnv._advance_physics()` starts one simulation-manager update per control interval and samples participating sensors through its `after_substep` observer. -`EmbodiedEnv._advance_physics()` owns ActionManager substep scheduling and delegates -ordinary actions to the base implementation. It uses the same batched simulation -update, sampling sensors before refreshing actions for the next substep. Intermediate +`EmbodiedEnv._advance_physics()` connects an ActionManager control-interval callback +to the batched simulation update after sensor sampling, and delegates ordinary +actions to the base implementation. ActionManager owns the substep count, term +refresh and inactive-row holds. Intermediate recording and visualization frames retain the simulation manager's publication cadence. Direct controller-ready commands bypass this refresh path; the explicit expert ActionManager route participates. See the sensor-owned [contact history contract](../sensor-system/contact-history.md) diff --git a/agent_context/topics/manager-functor/manager-functor.md b/agent_context/topics/manager-functor/manager-functor.md index 5e7bc5ead8..d3905f7abb 100644 --- a/agent_context/topics/manager-functor/manager-functor.md +++ b/agent_context/topics/manager-functor/manager-functor.md @@ -98,10 +98,13 @@ The config attribute name is the stable functor identity. For a flat policy tensor, the environment calls manager processing, manager application, physics, event `interval`, observation, and reward in that order. -A term can opt into physics-substep refresh, scheduled by `EmbodiedEnv`. -Its first command is processed and applied normally; subsequent substeps -call `update_substep` and apply only the -opted-in terms. Inactive demo rows retain measured holds on every refresh. +A term can opt into physics-substep refresh. +`ActionManager.get_substep_callback(num_substeps)` creates a callback scoped to +one control interval, or returns None when no refresh is needed. The manager +counts remaining steps, refreshes opted-in terms only before another physics +step, and preserves inactive demo rows' measured holds. The first command is +processed and applied normally. `EmbodiedEnv` connects the callback after sensor +sampling; simulation advancement and publication remain environment-owned. `expert_trajectory.use_action_manager` routes position-only expert commands through `ActionManager.process_joint_targets`. Relative substep DLS and binary gripper terms support that route, retaining exact FK targets and the expert diff --git a/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.managers.rst b/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.managers.rst index e39d073a73..97c8a75b5a 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.managers.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.gym.envs.managers.rst @@ -201,8 +201,10 @@ Jacobian solve from measured joints before each physics substep and clips only joint-position limits. It shares solver-order mapping and arena/TCP Jacobian adaptation with the single-step relative controller. Its ``eef_pose.relative_substep_dls@1`` contract selects this behavior separately from the bounded, single-step relative-pose contract. -``EmbodiedEnv`` refreshes action terms through the batched simulation callback, -preserving intermediate recording and visualization frames. +``ActionManager.get_substep_callback(num_substeps)`` owns the remaining-step +count, action refresh and inactive-row holds for one control interval. +``EmbodiedEnv`` connects that callback after sensor sampling through the batched +simulation update, preserving intermediate recording and visualization frames. Configure the same arm and binary gripper terms for expert and policy execution. Set ``env.expert_trajectory.use_action_manager: true`` to route position-only diff --git a/embodichain/lab/gym/envs/embodied_env.py b/embodichain/lab/gym/envs/embodied_env.py index 2d6716d05c..69dbd2620d 100644 --- a/embodichain/lab/gym/envs/embodied_env.py +++ b/embodichain/lab/gym/envs/embodied_env.py @@ -2486,13 +2486,12 @@ def _step_action(self, action: EnvAction | ControllerAction) -> EnvAction: return EmbodiedEnv._apply_controller_action(self, action) def _advance_physics(self) -> None: - """Refresh opted-in action terms between physics substeps.""" + """Connect manager callbacks to physics and sensor sampling.""" manager = getattr(self, "action_manager", None) - if ( - not getattr(self, "_step_uses_action_manager", False) - or manager is None - or not manager.requires_substep_update - ): + if not getattr(self, "_step_uses_action_manager", False) or manager is None: + return super()._advance_physics() + refresh_actions = manager.get_substep_callback(self.cfg.sim_steps_per_control) + if refresh_actions is None: return super()._advance_physics() sensors = [ @@ -2500,15 +2499,11 @@ def _advance_physics(self) -> None: ] for sensor in sensors: sensor.begin_control_step() - substep = 0 def observe_substep(dt: float) -> None: - nonlocal substep for sensor in sensors: sensor.update_physics_step(dt) - substep += 1 - if substep < self.cfg.sim_steps_per_control: - manager.apply_substep_actions() + refresh_actions(dt) self.sim.update( self.physics_dt, diff --git a/embodichain/lab/gym/envs/managers/action_manager.py b/embodichain/lab/gym/envs/managers/action_manager.py index 5c426c3ad3..df525820eb 100644 --- a/embodichain/lab/gym/envs/managers/action_manager.py +++ b/embodichain/lab/gym/envs/managers/action_manager.py @@ -19,6 +19,7 @@ from __future__ import annotations from abc import abstractmethod +from collections.abc import Callable from dataclasses import replace from functools import cached_property import inspect @@ -215,10 +216,40 @@ def previous_action(self) -> torch.Tensor: """Return previous complete raw policy actions.""" return self._previous_action - @property - def requires_substep_update(self) -> bool: - """Whether any term refreshes its command between physics substeps.""" - return bool(self._substep_terms) + def get_substep_callback(self, num_substeps: int) -> Callable[[float], None] | None: + """Create action refresh scheduling for one control interval. + + Args: + num_substeps: Positive number of physics steps in the interval. + + Returns: + An after-substep callback, or None when no refresh is needed. + The initial command must already be applied; the callback refreshes + only when another physics step remains. + + Raises: + ValueError: If num_substeps is less than one. + """ + if num_substeps < 1: + raise ValueError(f"Expected positive num_substeps, found {num_substeps}.") + if not self._substep_terms or num_substeps == 1: + return None + remaining = num_substeps + + def refresh_actions(dt: float) -> None: + nonlocal remaining + del dt + remaining -= 1 + if remaining <= 0: + return + for term in self._substep_terms: + term.update_substep() + if self._substep_active_mask is not None: + self.mask_inactive(self._substep_active_mask) + for term in self._substep_terms: + term.apply_actions() + + return refresh_actions def process_joint_targets(self, qpos: torch.Tensor) -> None: """Process expert targets while preserving exact FK target poses. @@ -246,15 +277,6 @@ def process_joint_targets(self, qpos: torch.Tensor) -> None: torch.cat([term.raw_actions for term in self._terms.values()], dim=-1) ) - def apply_substep_actions(self) -> None: - """Refresh and apply opted-in terms while preserving inactive-row holds.""" - for term in self._substep_terms: - term.update_substep() - if self._substep_active_mask is not None: - self.mask_inactive(self._substep_active_mask) - for term in self._substep_terms: - term.apply_actions() - @property def descriptors(self) -> tuple[ActionDescriptor, ...]: """Return ordered action-term descriptors bound to flat slices.""" diff --git a/tests/gym/envs/managers/test_action_manager_flat.py b/tests/gym/envs/managers/test_action_manager_flat.py index 3f7ace4a2c..4cf48f7a93 100644 --- a/tests/gym/envs/managers/test_action_manager_flat.py +++ b/tests/gym/envs/managers/test_action_manager_flat.py @@ -310,12 +310,29 @@ def update_substep(self): ) manager.process_action(torch.tensor([[0.2], [0.3]])) manager.mask_inactive(torch.tensor([True, False])) - manager.apply_substep_actions() + refresh = manager.get_substep_callback(3) + assert manager.get_substep_callback(1) is None + refresh(0.01) + refresh(0.01) + refresh(0.01) torch.testing.assert_close( manager.get_term("arm").processed_actions, torch.tensor([[0.8], [0.0]]) ) + assert manager.get_term("arm").apply_count == 2 manager.process_action(torch.tensor([[0.4], [0.5]])) - manager.apply_substep_actions() + refresh = manager.get_substep_callback(2) + refresh(0.01) + refresh(0.01) torch.testing.assert_close( manager.get_term("arm").processed_actions, torch.full((2, 1), 0.8) ) + assert manager.get_term("arm").apply_count == 3 + + +def test_manager_skips_substep_callback_for_static_actions(): + manager = make_manager( + ("arm", {"action_dim": 1, "low": -1.0, "high": 1.0, "joint_ids": (0,)}) + ) + assert manager.get_substep_callback(5) is None + with pytest.raises(ValueError, match="positive num_substeps"): + manager.get_substep_callback(0) diff --git a/tests/gym/envs/test_substep_sensors.py b/tests/gym/envs/test_substep_sensors.py index a27578cb83..5b2435d855 100644 --- a/tests/gym/envs/test_substep_sensors.py +++ b/tests/gym/envs/test_substep_sensors.py @@ -19,6 +19,7 @@ from types import SimpleNamespace import pytest from embodichain.lab.gym.envs import BaseEnv, EmbodiedEnv +from embodichain.lab.gym.envs.managers import ActionManager @pytest.mark.parametrize("env_type", (BaseEnv, EmbodiedEnv)) @@ -75,10 +76,14 @@ def test_substep_action_updates_interleave_with_physics_and_sensor_observation( env.cfg = SimpleNamespace(sim_steps_per_control=3) env.sim_cfg = SimpleNamespace(physics_dt=0.01) env._step_uses_action_manager = not controller_ready - env.action_manager = SimpleNamespace( - requires_substep_update=substep_actions, - apply_substep_actions=lambda: calls.append("control"), + manager = ActionManager.__new__(ActionManager) + term = SimpleNamespace( + update_substep=lambda: calls.append("control"), + apply_actions=lambda: None, ) + manager._substep_terms = (term,) if substep_actions else () + manager._substep_active_mask = None + env.action_manager = manager def update(dt, n, *, after_substep=None, render_final_step=True): assert render_final_step is False diff --git a/tests/sim/test_sim_manager.py b/tests/sim/test_sim_manager.py index 2dcb4d7352..7affce5ef9 100644 --- a/tests/sim/test_sim_manager.py +++ b/tests/sim/test_sim_manager.py @@ -1596,6 +1596,7 @@ def test_final_substep_consumers_share_publication(camera, record, viser): def test_embodied_substep_actions_preserve_intermediate_window_frames(): """Action refresh keeps physics-rate recording without extra camera captures.""" from embodichain.lab.gym.envs import EmbodiedEnv + from embodichain.lab.gym.envs.managers import ActionManager sim, _, state, publications, rendered = _make_render_frame_sim() sim._window_record_state = _WindowRecordState( @@ -1615,10 +1616,15 @@ def test_embodied_substep_actions_preserve_intermediate_window_frames(): env.sim = sim env.sensors = {} env._step_uses_action_manager = True - env.action_manager = SimpleNamespace( - requires_substep_update=True, - apply_substep_actions=lambda: refreshed.append(state["value"]), + manager = ActionManager.__new__(ActionManager) + manager._substep_terms = ( + SimpleNamespace( + update_substep=lambda: refreshed.append(state["value"]), + apply_actions=lambda: None, + ), ) + manager._substep_active_mask = None + env.action_manager = manager env._advance_physics() with sim.render_frame():