diff --git a/agent_context/topics/env-framework/env-framework.md b/agent_context/topics/env-framework/env-framework.md index 28e67d1f39..d8e6c7fd79 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,14 @@ 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._advance_physics()` starts one simulation-manager update per control +interval and samples participating sensors through its `after_substep` observer. +`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) 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..d3905f7abb 100644 --- a/agent_context/topics/manager-functor/manager-functor.md +++ b/agent_context/topics/manager-functor/manager-functor.md @@ -98,7 +98,19 @@ 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. +`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 +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 2eef440df3..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 @@ -59,6 +59,7 @@ functors), :class:`ActionManager` (``process_action(action)`` then JointPositionToLimitsAction JointVelocityAction ParallelGripperAction + RelativeEefDlsAction RelativeEefPoseAction RelativeJointPositionAction LeRobotRecorder @@ -194,6 +195,34 @@ 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. 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. +``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 +``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; +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..19a70879a4 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/embodied_env.py b/embodichain/lab/gym/envs/embodied_env.py index 940a22b861..69dbd2620d 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: @@ -2481,6 +2485,33 @@ def _step_action(self, action: EnvAction | ControllerAction) -> EnvAction: return action return EmbodiedEnv._apply_controller_action(self, action) + def _advance_physics(self) -> None: + """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: + 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 = [ + sensor for sensor in self.sensors.values() if sensor.requires_substep_update + ] + for sensor in sensors: + sensor.begin_control_step() + + def observe_substep(dt: float) -> None: + for sensor in sensors: + sensor.update_physics_step(dt) + refresh_actions(dt) + + 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: """Return the most recent ActionManager request/command trace.""" @@ -2618,6 +2649,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 +2695,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,7 +2708,16 @@ def _preprocess_action( raise RuntimeError( "Policy action recording requires an initialized DatasetManager." ) - self.action_manager.process_action(action) + 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): self.action_manager.mask_inactive(self._demo_active_mask) if self.dataset_manager is not None and not policy_recording: 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/__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 f0645529b3..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 @@ -50,6 +51,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 +80,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 +170,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 +216,67 @@ def previous_action(self) -> torch.Tensor: """Return previous complete raw policy actions.""" return self._previous_action + 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. + + Args: + qpos: Finite full-joint position targets on the environment device. + """ + 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}." + ) + + 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) + ) + @property def descriptors(self) -> tuple[ActionDescriptor, ...]: """Return ordered action-term descriptors bound to flat slices.""" @@ -277,6 +360,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 +417,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 +456,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..a66c63233d 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", @@ -576,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: @@ -620,6 +672,183 @@ 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._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( + 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 _compute_pose(self, qpos: torch.Tensor) -> torch.Tensor: + selected = qpos[:, list(self._joint_ids)] + return self._env.robot.compute_fk( + qpos=selected[:, self._solver_order], + name=self._part_name, + to_matrix=True, + ) + + 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._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( + (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. + """ + 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_(target) + self.update_substep() + + def update_substep(self) -> None: + """Recompute bounded joint targets toward the held EEF target.""" + from scipy.spatial.transform import Rotation + + 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() + .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, + ) + 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), + ) + ).squeeze(-1)[:, self._selection_order] + limits = self._env.robot.get_qpos_limits()[:, list(self._joint_ids)] + self._processed_actions.copy_( + (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()): + 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. @@ -677,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 = { @@ -747,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) @@ -914,6 +1100,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 +1157,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 +1198,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 +1281,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 +1361,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..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, ) @@ -985,3 +1084,120 @@ 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): + """An identity Jacobian gives a closed-form held-target controller step.""" + from scipy.spatial.transform import Rotation + + 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(qpos, *, name, to_matrix): + pose = torch.eye(4).repeat(batch, 1, 1) + pose[:, :3, 3] = qpos[:, :3] + pose[:, :3, :3] = torch.tensor( + Rotation.from_rotvec(qpos[:, 3:6].numpy()).as_matrix(), dtype=qpos.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_link_pose=lambda **kwargs: torch.eye(4).repeat(batch, 1, 1), + get_solver=lambda part: SimpleNamespace( + 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.0, 0.0]).expand(batch, 6).clone() + planned = qpos.clone() + planned[:, :6] += action * 0.5 + term.process_joint_targets(planned) + torch.testing.assert_close(term.raw_actions, action) + 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): + 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=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( + 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..4cf48f7a93 100644 --- a/tests/gym/envs/managers/test_action_manager_flat.py +++ b/tests/gym/envs/managers/test_action_manager_flat.py @@ -285,3 +285,54 @@ 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])) + 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]])) + 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/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..de93d21224 100644 --- a/tests/gym/envs/test_replay_actions.py +++ b/tests/gym/envs/test_replay_actions.py @@ -416,3 +416,59 @@ 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: + """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 + 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) + ) + 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.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) + 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 4593222f49..5b2435d855 100644 --- a/tests/gym/envs/test_substep_sensors.py +++ b/tests/gym/envs/test_substep_sensors.py @@ -18,13 +18,15 @@ from types import SimpleNamespace import pytest -from embodichain.lab.gym.envs import BaseEnv +from embodichain.lab.gym.envs import BaseEnv, EmbodiedEnv +from embodichain.lab.gym.envs.managers import ActionManager +@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) @@ -50,3 +52,69 @@ 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( + "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, +): + """Only opted-in EmbodiedEnv policy controllers refresh between steps.""" + calls = [] + 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 + 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 + updates.append(n) + 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 not refreshes + else [ + "physics", + "sensor", + "control", + "physics", + "sensor", + "control", + "physics", + "sensor", + ] + ) + assert calls == expected + assert updates == [3] diff --git a/tests/sim/test_sim_manager.py b/tests/sim/test_sim_manager.py index 00b3c6bc44..7affce5ef9 100644 --- a/tests/sim/test_sim_manager.py +++ b/tests/sim/test_sim_manager.py @@ -1593,6 +1593,49 @@ 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 + from embodichain.lab.gym.envs.managers import ActionManager + + 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 + 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(): + 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):