diff --git a/docs/source/overview/gym/action_functors.md b/docs/source/overview/gym/action_functors.md index 375c0a3c9..900f84e4f 100644 --- a/docs/source/overview/gym/action_functors.md +++ b/docs/source/overview/gym/action_functors.md @@ -16,6 +16,51 @@ This page lists all available action terms that can be used with the Action Mana **Using an AI coding agent?** Use the **`/add-functor`** skill to scaffold a new action term with the correct class structure, `ActionTermCfg` registration, and module placement in `actions.py`. ```` +## Policy and Command Contract + +The Action Manager exposes one flat {class}`gymnasium.spaces.Box` to the +policy. Each ``pre`` term owns a contiguous slice in configuration order. The +manager processes those slices into typed ``qpos``, ``qvel``, or ``qf`` +commands and applies each command to that term's selected joints. + +By default, every term's policy range is ``[-1, 1]``. Use ``scale`` to map that +normalized value to a useful physical magnitude, or use +{class}`~actions.QposDenormalizedTerm` to map the full normalized range to +joint position limits. Physical commands are clipped to the robot's qpos, +qvel, or qf limits by default. + +The following parameters are common to pre-processing terms: + +- ``joint_ids``: Static active-joint indices controlled by the term. +- ``control_part``: Named robot control part; use this instead of + ``joint_ids``. +- ``action_range``: Two finite policy-space bounds. The default is + ``[-1, 1]``. ``QposDenormalizedTerm`` uses its existing ``range`` parameter. +- ``clip``: Clip the processed physical command to robot limits. Defaults to + ``true``. +- ``allow_overlap``: Permit two terms to address the same joint only when it + is explicitly ``true`` on both terms. Disjoint groups are recommended + because overlapping position, velocity, and effort semantics are otherwise + ambiguous. + +Flat tensors are the standard RL interface. A mapping may also address terms +by their configuration names, which is useful for scripted controllers: + +```python +env.step({ + "arm_velocity": arm_velocity_action, + "gripper_effort": gripper_effort_action, +}) +``` + +Without an Action Manager, ``env.step(tensor)`` remains a qpos command for +backward compatibility. Direct typed commands are also accepted: + +```python +env.step({"qvel": target_velocity}) +env.step({"qf": target_effort}) +``` + ## Joint Position Control ```{list-table} Joint Position Action Terms @@ -46,7 +91,7 @@ This page lists all available action terms that can be used with the Action Mana - Normalize action from qpos limits -> [range[0], range[1]]. Maps joint positions to a normalized range based on joint limits. Typically used for post-processing action outputs. ```json - {"func": "QposNormalizedTerm", "params": {"range": [0.0, 1.0]}} + {"func": "QposNormalizedTerm", "mode": "post", "params": {"range": [0.0, 1.0]}} ``` ``` @@ -75,13 +120,13 @@ This page lists all available action terms that can be used with the Action Mana * - Action Term - Description * - {class}`~actions.QvelTerm` - - Joint velocity action: scale * action -> qvel. The policy outputs target joint velocities. + - Joint velocity action: scale * action -> qvel. The policy outputs target joint velocities. Configure zero position stiffness on these joints when a position drive should not oppose the velocity target. ```json {"func": "QvelTerm", "params": {"scale": 1.0}} ``` * - {class}`~actions.QfTerm` - - Joint force/torque action: scale * action -> qf. The policy outputs target joint torques/forces. + - Joint force/torque action: scale * action -> qf. The policy outputs target joint torques/forces. The command is reapplied before every physics substep so it is held across control decimation. ```json {"func": "QfTerm", "params": {"scale": 1.0}} @@ -117,6 +162,7 @@ actions = { actions = { "normalize_qpos": ActionTermCfg( func="QposNormalizedTerm", + mode="post", params={ "range": [0.0, 1.0], # Normalize to [0, 1] range }, @@ -133,11 +179,46 @@ actions = { }, ), } + +# Example: one flat RL action controlling disjoint joint groups +actions = { + "arm_velocity": ActionTermCfg( + func="QvelTerm", + params={ + "joint_ids": [0, 1, 2, 3, 4, 5], + "scale": 1.5, + }, + ), + "gripper_effort": ActionTermCfg( + func="QfTerm", + params={ + "control_part": "gripper", + "scale": 20.0, + }, + ), +} ``` +For the mixed example, the policy outputs +``[arm_velocity..., gripper_effort...]``. The environment retains that exact +flat action in RL and trajectory buffers while routing the processed commands +to ``set_qvel`` and ``set_qf`` respectively. + +````{attention} +Velocity and effort commands do not automatically change the robot's drive +configuration. A qvel term on joints with non-zero stiffness may fight the +position drive. A qf term with non-zero stiffness or damping is additive to +the active drive rather than pure torque control. The Action Manager emits a +warning for these combinations; configure the robot drive properties to match +the intended control mode. +```` + ## Action Term Properties All action terms provide the following properties: - ``action_dim``: The dimension of the action space (number of values the policy should output) +- ``action_space``: The per-term policy-space bounds +- ``joint_ids``: The resolved robot joints controlled by the term +- ``command_key``: The physical output type (``qpos``, ``qvel``, or ``qf``) - ``process_action(action)``: Method to convert raw policy output to robot control format diff --git a/docs/source/overview/gym/env.md b/docs/source/overview/gym/env.md index 95c067f95..cf550e16d 100644 --- a/docs/source/overview/gym/env.md +++ b/docs/source/overview/gym/env.md @@ -291,6 +291,8 @@ The dataset manager is called automatically during {meth}`~envs.Env.step()`, ens For RL tasks, EmbodiChain uses the **Action Manager** integrated into {class}`~envs.EmbodiedEnv`: * **Action Preprocessing**: Configurable via ``actions`` in {class}`~envs.EmbodiedEnvCfg`. Supports DeltaQposTerm, QposTerm, QposDenormalizedTerm, EefPoseTerm, QvelTerm, QfTerm. For a complete list of available action terms, please refer to {doc}`action_functors`. +* **Flat RL Interface**: The Action Manager concatenates all ``pre`` terms into one flat ``Box`` policy action space, then routes the slices to typed qpos, qvel, or qf commands on their selected joints. +* **Command Safety**: Processed commands are checked for batch shape and finite values and are clipped to robot limits by default. Effort commands are held across every physics substep. * **Standardized Info Structure**: {class}`~envs.EmbodiedEnv` provides ``compute_task_state``, ``get_info``, and ``evaluate`` for task-specific success/failure and metrics. * **Episode Management**: Configurable episode length and truncation logic. @@ -325,6 +327,29 @@ In a gym config file, use the ``actions`` section: } ``` +Multiple terms may control disjoint joint groups. Their dimensions are +concatenated in configuration order, so standard continuous-control policies +still produce one tensor: + +```json +"actions": { + "arm_velocity": { + "func": "QvelTerm", + "params": {"joint_ids": [0, 1, 2, 3, 4, 5], "scale": 1.5} + }, + "gripper_effort": { + "func": "QfTerm", + "params": {"control_part": "gripper", "scale": 20.0} + } +} +``` + +When no Action Manager is configured, a bare tensor passed to ``step`` remains +a qpos command. A direct mapping such as ``{"qvel": value}`` or +``{"qf": value}`` selects another physical command explicitly. For RL and +trajectory recording, prefer the Action Manager because its flat action space +and raw-action ordering are stable. + ## Creating a Custom Task diff --git a/docs/source/overview/rl/models.md b/docs/source/overview/rl/models.md index 9c85cd9f3..6a250b7d1 100644 --- a/docs/source/overview/rl/models.md +++ b/docs/source/overview/rl/models.md @@ -15,10 +15,12 @@ This module contains RL policy networks and related model implementations, suppo ### ActorCritic - Typical actor-critic policy, includes actor (action distribution) and critic (value function). Used with PPO. +- Supports ``squash_actions`` for a tanh-bounded Gaussian. The sampled and reevaluated log probabilities include the tanh Jacobian correction required by PPO. ### ActorOnly - Actor-only policy without Critic. Used with GRPO (Group Relative Policy Optimization), which estimates advantages via group-level return comparison instead of a value function. - Supports Gaussian action distributions, learnable log_std, suitable for continuous action spaces. +- Supports the same corrected ``squash_actions`` path as ``ActorCritic``. - Key methods: - `forward`: Actor network outputs mean, samples action, and writes policy outputs into a `TensorDict`. - `evaluate_actions`: Used for loss calculation in PPO/GRPO algorithms. @@ -52,6 +54,12 @@ log_prob = step_td["sample_log_prob"] value = step_td["value"] ``` +For simulator RL, training enables ``squash_actions`` automatically when an +Action Manager exposes the standard ``[-1, 1]`` action range. Set it explicitly +to ``false`` to retain an unbounded Gaussian. Custom Action Manager bounds do +not enable automatic tanh squashing; use a policy distribution whose support +matches those bounds. + ## Extension and Customization - Supports custom network architectures (e.g., CNN, Transformer) by implementing the Policy interface. - Can extend to multi-head policies, distributional actors, hybrid action spaces, etc. diff --git a/docs/source/tutorial/rl.rst b/docs/source/tutorial/rl.rst index 54ce4590f..7c4c71c6b 100644 --- a/docs/source/tutorial/rl.rst +++ b/docs/source/tutorial/rl.rst @@ -140,6 +140,7 @@ The ``policy`` section defines the neural network policy: - **name**: Policy name (e.g., "actor_critic", "vla") - **action_dim**: Optional policy output action dimension. If omitted, it is inferred from ``env.action_space``. +- **squash_actions**: Optional tanh bounding for built-in Gaussian policies. Simulator training enables it by default when the Action Manager uses ``[-1, 1]`` bounds; log probabilities include the tanh Jacobian correction. - **actor**: Actor network configuration (required for actor_critic) - **critic**: Critic network configuration (required for actor_critic) diff --git a/embodichain/lab/gym/envs/base_env.py b/embodichain/lab/gym/envs/base_env.py index d8348a818..4d0d4bba8 100644 --- a/embodichain/lab/gym/envs/base_env.py +++ b/embodichain/lab/gym/envs/base_env.py @@ -20,7 +20,7 @@ import numpy as np import gymnasium as gym -from typing import Dict, List, Union, Tuple, Any, Sequence +from typing import Any, Callable, Dict, List, Sequence, Tuple, Union from functools import cached_property from tensordict import TensorDict @@ -589,6 +589,31 @@ def _postprocess_action(self, action: EnvAction) -> EnvAction: """ return action + def _before_sim_step(self, substep_index: int) -> None: + """Hook invoked immediately before every physics substep. + + Args: + substep_index: Zero-based substep within the current environment + control step. + + .. tip:: + Override this hook for commands, such as generalized efforts, that + must be reapplied throughout action decimation. + """ + del substep_index + + def _get_before_sim_step_callback(self) -> Callable[[int], None] | None: + """Return an optional callback for physics-substep control updates. + + Returns: + The overridden :meth:`_before_sim_step` hook, or ``None`` when the + hook is unchanged so ordinary environments do not pay a Python + callback cost during action decimation. + """ + if type(self)._before_sim_step is BaseEnv._before_sim_step: + return None + return self._before_sim_step + def _step_action(self, action: EnvAction) -> EnvAction: """Set action control command into simulation. @@ -666,7 +691,11 @@ def step( action = self._step_action(action=action) with self._profiler.section("sim_update"): - self.sim.update(self.sim_cfg.physics_dt, self.cfg.sim_steps_per_control) + self.sim.update( + self.sim_cfg.physics_dt, + self.cfg.sim_steps_per_control, + before_step_callback=self._get_before_sim_step_callback(), + ) with self._profiler.section("update_sim_state"): self._update_sim_state(**kwargs) diff --git a/embodichain/lab/gym/envs/embodied_env.py b/embodichain/lab/gym/envs/embodied_env.py index a8e717008..349e2295b 100644 --- a/embodichain/lab/gym/envs/embodied_env.py +++ b/embodichain/lab/gym/envs/embodied_env.py @@ -17,6 +17,7 @@ from __future__ import annotations from math import log +from collections.abc import Mapping from functools import wraps from datetime import datetime import os @@ -25,7 +26,7 @@ import gymnasium as gym from dataclasses import MISSING -from typing import Dict, Union, Sequence, Tuple, Any, List, Optional +from typing import Any, Callable, Dict, List, Optional, Sequence, Tuple, Union from tensordict import TensorDict from embodichain.lab.sim.cfg import ( @@ -290,6 +291,8 @@ def __init__(self, cfg: EmbodiedEnvCfg, **kwargs): self.reward_manager: RewardManager | None = None self.action_manager: ActionManager | None = None self.dataset_manager: DatasetManager | None = None + self._last_raw_action: torch.Tensor | None = None + self._pending_direct_qf: torch.Tensor | None = None super().__init__(cfg, **kwargs) @@ -325,7 +328,7 @@ def __init__(self, cfg: EmbodiedEnvCfg, **kwargs): # rollout_buffer so async parallel envs and ActionManager are supported. self._traj_buffer: TensorDict | None = None self._traj_steps: torch.Tensor | None = None - self._traj_raw_action: EnvAction | None = None + self._traj_raw_action: torch.Tensor | None = None self._traj_save_count = 0 self._traj_run_id = datetime.now().strftime("%Y%m%d_%H%M%S") if self.cfg.record_trajectory: @@ -652,6 +655,14 @@ def _initialize_episode( with self._profiler.section("reward_reset"): self.reward_manager.reset(env_ids=env_ids) + action_manager = getattr(self, "action_manager", None) + if action_manager is not None and action_manager.get_terms_by_mode("pre"): + with self._profiler.section("action_reset"): + action_manager.reset(env_ids=env_ids) + self._last_raw_action = None + self._traj_raw_action = None + self._pending_direct_qf = None + # Dataset saving can be disabled while the dataset configuration remains # present. In that mode no DatasetManager is created in __init__, so # reset must not dereference the optional manager. @@ -684,24 +695,26 @@ def _write_episode_rollout_step( self.rollout_buffer["obs"][:, self.current_rollout_step, ...].copy_( obs.to(buffer_device), non_blocking=True ) - if isinstance(action, TensorDict): - action_to_store = ( - action["qpos"] - if "qpos" in action - else (action["qvel"] if "qvel" in action else action["qf"]) - ) - elif isinstance(action, torch.Tensor): + action_to_store = self._last_raw_action + if action_to_store is None and isinstance(action, (TensorDict, torch.Tensor)): action_to_store = action - else: + if action_to_store is None: logger.log_warning( f"Unexpected action type {type(action)} in _hook_after_sim_step; " "skipping action storage in rollout buffer." ) - action_to_store = None if action_to_store is not None: - self.rollout_buffer["actions"][:, self.current_rollout_step, ...].copy_( - action_to_store.to(buffer_device), non_blocking=True - ) + destination = self.rollout_buffer["actions"][ + :, self.current_rollout_step, ... + ] + if tuple(action_to_store.shape) != tuple(destination.shape): + logger.log_warning( + "Raw action shape does not match the episode rollout buffer: " + f"action={tuple(action_to_store.shape)}, " + f"buffer={tuple(destination.shape)}. Skipping action storage." + ) + else: + destination.copy_(action_to_store.to(buffer_device), non_blocking=True) self.rollout_buffer["rewards"][:, self.current_rollout_step].copy_( rewards.to(buffer_device), non_blocking=True ) @@ -738,7 +751,15 @@ def _write_trajectory_step(self) -> None: idx, st ] = obj.get_local_pose()[idx] if self._traj_raw_action is not None: - self._traj_buffer["actions"][idx, st] = self._traj_raw_action[idx] + action_buffer = self._traj_buffer["actions"] + if self._traj_raw_action.shape[-1] == action_buffer.shape[-1]: + action_buffer[idx, st] = self._traj_raw_action[idx] + else: + logger.log_warning( + "Raw action dimension does not match the trajectory buffer: " + f"action={self._traj_raw_action.shape[-1]}, " + f"buffer={action_buffer.shape[-1]}. Skipping action storage." + ) self._traj_steps = (self._traj_steps + 1).clamp(max=max_steps) def _write_rl_rollout_step( @@ -900,28 +921,103 @@ def _step_action(self, action: EnvAction) -> EnvAction: Returns: The action return. """ - if isinstance(action, TensorDict): - # Support multiple control modes simultaneously - if "qpos" in action: - self.robot.set_qpos( - qpos=action["qpos"].to(self.device), joint_ids=self.active_joint_ids - ) - if "qvel" in action: - self.robot.set_qvel( - qvel=action["qvel"].to(self.device), joint_ids=self.active_joint_ids - ) - if "qf" in action: - self.robot.set_qf( - qf=action["qf"].to(self.device), joint_ids=self.active_joint_ids + self._pending_direct_qf = None + if self.action_manager is not None and self.action_manager.get_terms_by_mode( + "pre" + ): + # Term-specific joint selections and command types are retained by + # the manager. Efforts are applied in _before_sim_step so they are + # held across every physics substep. + self.action_manager.apply_action(command_keys={"qpos", "qvel"}) + return action + + if isinstance(action, (TensorDict, Mapping)): + unsupported_keys = set(action.keys()) - {"qpos", "qvel", "qf"} + if unsupported_keys: + raise KeyError( + f"Unsupported direct control command keys: {sorted(unsupported_keys)}. " + "Expected qpos, qvel and/or qf." ) - elif isinstance(action, torch.Tensor): - self.robot.set_qpos( - qpos=action.to(self.device), joint_ids=self.active_joint_ids + if not any(key in action for key in ("qpos", "qvel", "qf")): + raise ValueError("Direct control action contains no qpos, qvel or qf.") + + commands = {} + for key in ("qpos", "qvel", "qf"): + if key not in action: + continue + command = self._coerce_direct_command(action[key], key) + commands[key] = command + if key == "qpos": + self.robot.set_qpos(qpos=command, joint_ids=self.active_joint_ids) + elif key == "qvel": + self.robot.set_qvel(qvel=command, joint_ids=self.active_joint_ids) + else: + self.robot.set_qf(qf=command, joint_ids=self.active_joint_ids) + self._pending_direct_qf = command + return TensorDict(commands, batch_size=[self.num_envs], device=self.device) + + command = self._coerce_direct_command(action, "qpos") + self.robot.set_qpos(qpos=command, joint_ids=self.active_joint_ids) + return command + + def _before_sim_step(self, substep_index: int) -> None: + """Reapply effort commands across every physics substep. + + Args: + substep_index: Zero-based physics substep index. + """ + del substep_index + if self.action_manager is not None and self.action_manager.get_terms_by_mode( + "pre" + ): + self.action_manager.apply_action(command_keys={"qf"}) + elif self._pending_direct_qf is not None: + self.robot.set_qf( + qf=self._pending_direct_qf, joint_ids=self.active_joint_ids ) - else: - logger.log_error(f"Unsupported action type: {type(action)}") - return action + def _get_before_sim_step_callback(self) -> Callable[[int], None] | None: + """Enable substep callbacks only while an effort command is active.""" + if self._pending_direct_qf is not None: + return self._before_sim_step + if self.action_manager is None: + return None + has_effort_term = any( + term.command_key == "qf" + for _, term in self.action_manager.get_terms_by_mode("pre") + ) + return self._before_sim_step if has_effort_term else None + + def _coerce_direct_command(self, value: Any, command_key: str) -> torch.Tensor: + """Convert and validate a direct physical robot command. + + Args: + value: Tensor, NumPy array or sequence containing a batched command. + command_key: One of ``qpos``, ``qvel`` or ``qf``. + + Returns: + Validated and limit-clipped command tensor. + """ + command = torch.as_tensor(value, dtype=torch.float32, device=self.device) + if command.ndim == 1 and self.num_envs == 1: + command = command.unsqueeze(0) + expected_shape = (self.num_envs, len(self.active_joint_ids)) + if tuple(command.shape) != expected_shape: + raise ValueError( + f"Invalid {command_key} command shape: expected {expected_shape}, " + f"got {tuple(command.shape)}." + ) + if not bool(torch.isfinite(command).all()): + raise ValueError(f"{command_key} command contains NaN or infinite values.") + + if command_key == "qpos": + limits = self.robot.body_data.qpos_limits[:, self.active_joint_ids, :] + return command.clamp(limits[..., 0], limits[..., 1]) + limit_name = "qvel_limits" if command_key == "qvel" else "qf_limits" + limits = getattr(self.robot.body_data, limit_name)[ + :, self.active_joint_ids + ].abs() + return command.clamp(-limits, limits) def compute_task_state( self, **kwargs @@ -975,23 +1071,57 @@ def evaluate(self, **kwargs) -> Dict[str, Any]: return eval_dict def _preprocess_action(self, action: EnvAction) -> EnvAction: - """Delegate to ActionManager when configured; stash raw action for trajectory.""" + """Convert policy action and retain the exact raw action for learning/recording.""" + if self.action_manager is not None and self.action_manager.get_terms_by_mode( + "pre" + ): + processed = self.action_manager.process_action(action, mode="pre") + raw_action = self.action_manager.raw_action + else: + processed = super()._preprocess_action(action) + raw_action = self._coerce_raw_action(action) + self._last_raw_action = raw_action if self._traj_buffer is not None: - self._traj_raw_action = action - if self.action_manager is not None: - return self.action_manager.process_action(action, mode="pre") - return super()._preprocess_action(action) + self._traj_raw_action = raw_action + return processed def _postprocess_action(self, action): if self.action_manager is not None: return self.action_manager.process_action(action, mode="post") return super()._postprocess_action(action) + def _coerce_raw_action(self, action: EnvAction) -> torch.Tensor: + """Normalize an unmanaged raw action into a stable flat tensor.""" + if isinstance(action, (TensorDict, Mapping)): + unsupported_keys = set(action.keys()) - {"qpos", "qvel", "qf"} + if unsupported_keys: + raise KeyError( + f"Unsupported direct control command keys: {sorted(unsupported_keys)}." + ) + command_values = [] + for key in ("qpos", "qvel", "qf"): + if key not in action: + continue + value = torch.as_tensor( + action[key], dtype=torch.float32, device=self.device + ) + if value.ndim == 1 and self.num_envs == 1: + value = value.unsqueeze(0) + command_values.append(value) + if not command_values: + raise ValueError("Direct control action contains no qpos, qvel or qf.") + return torch.cat(command_values, dim=-1) + value = torch.as_tensor(action, dtype=torch.float32, device=self.device) + if value.ndim == 1 and self.num_envs == 1: + value = value.unsqueeze(0) + return value + def _setup_robot(self, **kwargs) -> Robot: """Setup the robot in the environment. - Currently, only joint position control is supported. Would be extended to support joint velocity and torque - control in the future. + The default action space uses joint positions. Configuring an + :class:`ActionManager` replaces it with the manager's flat policy action + space and enables position, velocity, effort or mixed control terms. Returns: Robot: The robot instance added to the scene. diff --git a/embodichain/lab/gym/envs/managers/action_manager.py b/embodichain/lab/gym/envs/managers/action_manager.py index 26975f451..c423efb9a 100644 --- a/embodichain/lab/gym/envs/managers/action_manager.py +++ b/embodichain/lab/gym/envs/managers/action_manager.py @@ -25,20 +25,21 @@ from __future__ import annotations -import inspect -import torch -import numpy as np -import gymnasium as gym - -from functools import cached_property from abc import abstractmethod +from collections.abc import Mapping, Sequence +from functools import cached_property +import inspect from typing import TYPE_CHECKING, Any, Literal + +import gymnasium as gym +import numpy as np from prettytable import PrettyTable from tensordict import TensorDict +import torch from embodichain.lab.sim.types import EnvAction -from embodichain.utils.string import string_to_callable from embodichain.utils import logger +from embodichain.utils.string import string_to_callable from .cfg import ActionTermCfg from .manager_base import Functor, ManagerBase @@ -57,8 +58,11 @@ class ActionTerm(Functor): """ SUPPORTED_TYPES = ["qpos", "qvel", "qf", "eef_pose"] - """The supported action types. Each term must specify one of these as its output type, which - determines how the processed action is applied to the robot. + """Known policy input and physical command types. + + ``eef_pose`` is a policy input type that resolves to a ``qpos`` command; + the physical command types accepted by the manager are ``qpos``, ``qvel`` + and ``qf``. """ def __init__(self, cfg: ActionTermCfg, env: EmbodiedEnv): @@ -69,22 +73,72 @@ def __init__(self, cfg: ActionTermCfg, env: EmbodiedEnv): env: The environment instance. """ super().__init__(cfg, env) + self._joint_ids = self._resolve_joint_ids() + self._clip_command = bool(cfg.params.get("clip", True)) @property @abstractmethod def input_key(self) -> str: - """The output type of the action term, which determines how the processed action is applied to the robot. + """Structured policy-action key consumed by this term. - Must be one of the supported types defined in SUPPORTED_TYPES. + This property is retained for compatibility with existing action configs. + The physical output type is exposed separately through + :attr:`command_key`. """ ... + @property + def command_key(self) -> str: + """Physical robot command produced by this term. + + Returns: + One of ``"qpos"``, ``"qvel"`` or ``"qf"``. + """ + return self.input_key + @property @abstractmethod def action_dim(self) -> int: """Dimension of the action term (policy output dimension).""" ... + @property + def joint_ids(self) -> list[int]: + """Robot joint indices controlled by this term.""" + return self._joint_ids + + @property + def clip_command(self) -> bool: + """Whether physical commands are clipped to the robot limits.""" + return self._clip_command + + @property + def action_space(self) -> gym.spaces.Box: + """Normalized per-term policy action space. + + The default range is ``[-1, 1]``. It can be overridden with the + ``action_range`` config parameter, specified as ``[low, high]`` where + either bound may be a scalar or one value per action dimension. + """ + action_range = self.cfg.params.get("action_range", (-1.0, 1.0)) + try: + low_value, high_value = action_range + except (TypeError, ValueError) as error: + raise ValueError( + "ActionTermCfg.params['action_range'] must contain [low, high]." + ) from error + low = self._expand_action_bound(low_value, "low") + high = self._expand_action_bound(high_value, "high") + if np.any(low >= high): + raise ValueError( + f"Invalid policy action range for {type(self).__name__}: low must be smaller than high." + ) + if not np.isfinite(low).all() or not np.isfinite(high).all(): + raise ValueError( + f"Policy action range for {type(self).__name__} must be finite." + ) + return gym.spaces.Box(low=low, high=high, dtype=np.float32) + @abstractmethod def process_action(self, action: torch.Tensor) -> EnvAction | torch.Tensor: """Process raw action from policy into robot control format. @@ -93,7 +147,7 @@ def process_action(self, action: torch.Tensor) -> EnvAction | torch.Tensor: action: Raw action tensor from policy, shape (num_envs, action_dim). Returns: - Processed action tensor ready for robot control, shape depends on input_key. + Processed action tensor or typed payload ready for robot control. """ ... @@ -101,13 +155,63 @@ def __call__(self, *args, **kwargs) -> Any: """Not used for ActionTerm; use process_action instead.""" return self.process_action(*args, **kwargs) + def _resolve_joint_ids(self) -> list[int]: + """Resolve the static joint selection from term parameters.""" + params = self.cfg.params + joint_ids = params.get("joint_ids") + control_part = params.get("control_part") + if joint_ids is not None and control_part is not None: + raise ValueError( + "Specify either 'joint_ids' or 'control_part' for an action term, not both." + ) + if control_part is not None: + joint_ids = self._env.robot.get_joint_ids( + name=control_part, remove_mimic=True + ) + elif joint_ids is None: + joint_ids = self._env.active_joint_ids + + resolved = [int(joint_id) for joint_id in joint_ids] + if len(resolved) == 0: + raise ValueError(f"{type(self).__name__} must control at least one joint.") + if len(set(resolved)) != len(resolved): + raise ValueError( + f"Duplicate joint ids are not allowed for {type(self).__name__}: {resolved}." + ) + active_joint_ids = set(int(joint_id) for joint_id in self._env.active_joint_ids) + invalid_joint_ids = [ + joint_id for joint_id in resolved if joint_id not in active_joint_ids + ] + if invalid_joint_ids: + raise ValueError( + f"Action term joint ids {invalid_joint_ids} are not active environment joints. " + f"Active joint ids: {sorted(active_joint_ids)}." + ) + return resolved + + def _expand_action_bound(self, value: Any, name: str) -> np.ndarray: + """Expand a scalar or vector action bound to ``action_dim``.""" + bound = np.asarray(value, dtype=np.float32) + if bound.ndim == 0: + return np.full((self.action_dim,), float(bound), dtype=np.float32) + bound = bound.reshape(-1) + if bound.shape != (self.action_dim,): + raise ValueError( + f"Policy action {name} bound for {type(self).__name__} must have " + f"shape ({self.action_dim},), got {bound.shape}." + ) + return bound + class ActionManager(ManagerBase): """Manager for processing actions sent to the environment. - The action manager handles the interpretation and preprocessing of raw actions - from the policy into the format expected by the robot. It supports a single - active action term per environment (matching current RL usage). + The manager separates the flat action sampled by a policy from typed physical + robot commands. Each pre-processing term owns a stable slice of the policy + action, produces a ``qpos``, ``qvel`` or ``qf`` command, and records the + joints to which that command applies. The environment can therefore expose a + conventional flat :class:`gymnasium.spaces.Box` while still supporting mixed + control modes on disjoint joint groups. """ def __init__(self, cfg: object, env: EmbodiedEnv): @@ -124,7 +228,11 @@ def __init__(self, cfg: object, env: EmbodiedEnv): "pre": [], "post": [], } + self._raw_action: torch.Tensor | None = None + self._processed_action: TensorDict | None = None + self._processed_term_actions: dict[str, TensorDict] = {} super().__init__(cfg, env) + self._validate_pre_terms() def __str__(self) -> str: """Returns: A string representation for action manager.""" @@ -168,7 +276,8 @@ def total_action_dim(self) -> int: return sum(term.action_dim for _, term in terms) @cached_property - def single_action_space(self) -> torch.Tensor | gym.Space: + def single_action_space(self) -> gym.spaces.Box: + """Flat policy action space formed by concatenating all pre terms.""" terms = self.get_terms_by_mode("pre") if len(terms) == 0: qpos_limits = ( @@ -176,84 +285,41 @@ def single_action_space(self) -> torch.Tensor | gym.Space: .cpu() .numpy() ) - single_action_space = gym.spaces.Box( + return gym.spaces.Box( low=qpos_limits[:, 0], high=qpos_limits[:, 1], dtype=np.float32 ) - return single_action_space - else: - # Create dict action space for multiple terms. - spaces = {} - for name, term in terms: - if term.input_key == "qpos": - qpos_limits = ( - self._env.robot.body_data.qpos_limits[ - 0, self._env.active_joint_ids - ] - .cpu() - .numpy() - ) - spaces[term.input_key] = gym.spaces.Box( - low=qpos_limits[:, 0], high=qpos_limits[:, 1], dtype=np.float32 - ) - elif term.input_key == "qvel": - qvel_limits = ( - self._env.robot.body_data.qvel_limits[ - 0, self._env.active_joint_ids - ] - .cpu() - .numpy() - ) - spaces[term.input_key] = gym.spaces.Box( - low=-qvel_limits, high=qvel_limits, dtype=np.float32 - ) - elif term.input_key == "qf": - qf_limits = ( - self._env.robot.body_data.qf_limits[ - 0, self._env.active_joint_ids - ] - .cpu() - .numpy() - ) - spaces[term.input_key] = gym.spaces.Box( - low=-qf_limits, high=qf_limits, dtype=np.float32 - ) - else: - spaces[term.input_key] = gym.spaces.Box( - low=-np.inf, - high=np.inf, - shape=(term.action_dim,), - dtype=np.float32, - ) - if len(spaces) == 1 and "qpos" in spaces: - return spaces["qpos"] - else: - return gym.spaces.Dict(spaces) + + low = np.concatenate([term.action_space.low.reshape(-1) for _, term in terms]) + high = np.concatenate([term.action_space.high.reshape(-1) for _, term in terms]) + return gym.spaces.Box(low=low, high=high, dtype=np.float32) + + @property + def raw_action(self) -> torch.Tensor | None: + """Most recent flat policy action, before term processing.""" + return self._raw_action + + @property + def processed_action(self) -> TensorDict | None: + """Most recent typed physical command payload.""" + return self._processed_action def convert_policy_action_to_env_action(self, action: torch.Tensor) -> EnvAction: - """Convert raw action from policy into robot control format. + """Validate a flat policy action before passing it to ``env.step``. - This is a convenience method for processing a raw action tensor through the active terms. - It assumes the input action is ordered according to the active terms and concatenated into a single tensor. + .. attention:: + Action conversion now belongs to :meth:`process_action`, which is + called by the environment. This method remains as a compatibility + shim for external collectors and returns the validated flat action. Args: action: Raw action tensor from policy, shape (num_envs, total_action_dim). Returns: - Processed action tensor ready for robot control, shape depends on active terms. + The validated flat policy action on the environment device. """ - terms = self.get_terms_by_mode("pre") - if len(terms) == 0 or len(terms) == 1: - return action - else: - action_dict = {} - current_dim = 0 - for _, term in terms: - term_action = action[:, current_dim : current_dim + term.action_dim] - action_dict[term.input_key] = term_action - current_dim += term.action_dim - return TensorDict( - action_dict, batch_size=[action.shape[0]], device=action.device - ) + return self._coerce_action_tensor( + action, expected_dim=self.total_action_dim, label="policy action" + ) def get_action_dim_by_mode(self, mode: Literal["pre", "post"]) -> int: """Get total action dimension for terms of a specific mode. @@ -272,9 +338,8 @@ def process_action( ) -> EnvAction: """Process raw action from policy into robot control format. - Supports: - 1. Tensor input: Passed to the active (first) term of the specified mode. - 2. Dict/TensorDict input: Uses key matching term name; raises an error if no match. + A flat tensor is split according to term order. Structured mappings may + instead provide values by term name or by the term's ``input_key``. Args: action: Raw action from policy (tensor or dict). @@ -282,23 +347,337 @@ def process_action( for postprocessing. When "post", only terms with mode="post" are applied. Returns: - TensorDict action ready for robot control. + Typed physical commands. Pre-processing returns a TensorDict keyed by + command type for one term, or by term name for multiple terms. A + single metadata-free ``qpos`` term retains its historical tensor + return type; the typed payload is always available through + :attr:`processed_action`. """ - # Filter terms by mode - mode_terms = self._mode_term_names[mode] - - if not mode_terms: + terms = self.get_terms_by_mode(mode) + if not terms: return action - if len(mode_terms) == 1: - term_name = mode_terms[0] + if mode == "post": + return self._process_post_action(action, terms) + + term_actions = self._split_policy_action(action, terms) + self._raw_action = torch.cat(term_actions, dim=-1) + self._processed_term_actions = {} + for (term_name, term), term_action in zip(terms, term_actions): + processed = term.process_action(term_action) + self._processed_term_actions[term_name] = self._normalize_term_command( + term_name, term, processed + ) + + if len(terms) == 1: + self._processed_action = next(iter(self._processed_term_actions.values())) + else: + self._processed_action = TensorDict( + self._processed_term_actions, + batch_size=[self.num_envs], + device=self.device, + ) + # Preserve the established single-qpos return type for task reward code, + # while keeping the canonical typed payload in ``processed_action``. + if len(terms) == 1 and set(self._processed_action.keys()) == {"qpos"}: + return self._processed_action["qpos"] + return self._processed_action + + def apply_action(self, command_keys: set[str] | None = None) -> None: + """Apply the most recently processed commands to the robot. + + Args: + command_keys: Optional subset of ``{"qpos", "qvel", "qf"}`` to + apply. If omitted, all physical command types are applied. + + Raises: + RuntimeError: If no policy action has been processed yet. + """ + if not self._processed_term_actions: + raise RuntimeError("No processed action is available to apply.") + selected_keys = {"qpos", "qvel", "qf"} if command_keys is None else command_keys + unsupported_keys = selected_keys - {"qpos", "qvel", "qf"} + if unsupported_keys: + raise ValueError( + f"Unsupported physical command keys: {sorted(unsupported_keys)}." + ) + for term_name, command in self._processed_term_actions.items(): term = self._terms[term_name] - return term.process_action(action) + for command_key in ("qpos", "qvel", "qf"): + if command_key not in selected_keys or command_key not in command: + continue + value = command[command_key] + if command_key == "qpos": + self._env.robot.set_qpos(qpos=value, joint_ids=term.joint_ids) + elif command_key == "qvel": + self._env.robot.set_qvel(qvel=value, joint_ids=term.joint_ids) + else: + self._env.robot.set_qf(qf=value, joint_ids=term.joint_ids) + + def reset(self, env_ids: Sequence[int] | None = None) -> dict[str, float]: + """Clear cached action state. + + Args: + env_ids: Ignored because action caches are shared batched payloads. + + Returns: + Empty logging information. + """ + del env_ids + self._raw_action = None + self._processed_action = None + self._processed_term_actions = {} + return {} + + def _split_policy_action( + self, + action: EnvAction, + terms: list[tuple[str, ActionTerm]], + ) -> list[torch.Tensor]: + """Split flat or structured policy actions into per-term tensors.""" + if not isinstance(action, (TensorDict, Mapping)): + flat_action = self._coerce_action_tensor( + action, + expected_dim=sum(term.action_dim for _, term in terms), + label="policy action", + ) + return list( + torch.split( + flat_action, + [term.action_dim for _, term in terms], + dim=-1, + ) + ) + + input_key_counts: dict[str, int] = {} + for _, term in terms: + input_key_counts[term.input_key] = ( + input_key_counts.get(term.input_key, 0) + 1 + ) + + term_actions: list[torch.Tensor] = [] + for term_name, term in terms: + if term_name in action: + value = action[term_name] + elif term.input_key in action and input_key_counts[term.input_key] == 1: + value = action[term.input_key] + else: + raise KeyError( + f"Missing policy action for term '{term_name}'. Provide key " + f"'{term_name}'" + + ( + f" or the unambiguous input key '{term.input_key}'." + if input_key_counts[term.input_key] == 1 + else "." + ) + ) + term_actions.append( + self._coerce_action_tensor( + value, + expected_dim=term.action_dim, + label=f"policy action term '{term_name}'", + ) + ) + return term_actions + + def _coerce_action_tensor( + self, + action: Any, + *, + expected_dim: int, + label: str, + ) -> torch.Tensor: + """Convert an action-like value and validate its batched shape.""" + action_tensor = torch.as_tensor(action, dtype=torch.float32, device=self.device) + if action_tensor.ndim == 1 and self.num_envs == 1: + action_tensor = action_tensor.unsqueeze(0) + expected_shape = (self.num_envs, expected_dim) + if tuple(action_tensor.shape) != expected_shape: + raise ValueError( + f"Invalid {label} shape: expected {expected_shape}, got " + f"{tuple(action_tensor.shape)}." + ) + if not bool(torch.isfinite(action_tensor).all()): + raise ValueError(f"{label.capitalize()} contains NaN or infinite values.") + return action_tensor + + def _normalize_term_command( + self, + term_name: str, + term: ActionTerm, + processed: EnvAction | Mapping[str, Any], + ) -> TensorDict: + """Normalize one term result into a typed physical command TensorDict.""" + if isinstance(processed, torch.Tensor): + values: Mapping[str, Any] = {term.command_key: processed} + elif isinstance(processed, (TensorDict, Mapping)): + values = processed else: - for name in mode_terms: - term = self._terms[name] - action[term.input_key] = term.process_action(action[term.input_key]) - return action + raise TypeError( + f"Action term '{term_name}' returned unsupported type " + f"{type(processed)!r}." + ) + + data: dict[str, torch.Tensor] = {} + physical_keys = set() + for key, value in values.items(): + if key in {"qpos", "qvel", "qf"}: + command = self._coerce_action_tensor( + value, + expected_dim=len(term.joint_ids), + label=f"{key} command from term '{term_name}'", + ) + data[key] = self._clip_to_robot_limits(term, key, command) + physical_keys.add(key) + else: + metadata = torch.as_tensor(value, device=self.device) + if metadata.ndim == 0: + if self.num_envs == 1: + metadata = metadata.unsqueeze(0) + else: + raise ValueError( + f"Scalar metadata '{key}' from action term '{term_name}' " + f"cannot represent {self.num_envs} environments." + ) + if metadata.shape[0] != self.num_envs: + raise ValueError( + f"Metadata '{key}' from action term '{term_name}' must " + f"have leading dimension {self.num_envs}, got " + f"{tuple(metadata.shape)}." + ) + data[key] = metadata + + if not physical_keys: + raise ValueError( + f"Action term '{term_name}' did not produce a qpos, qvel or qf command." + ) + if term.command_key not in physical_keys: + raise ValueError( + f"Action term '{term_name}' declares command_key='{term.command_key}' " + f"but returned {sorted(physical_keys)}." + ) + return TensorDict(data, batch_size=[self.num_envs], device=self.device) + + def _clip_to_robot_limits( + self, + term: ActionTerm, + command_key: str, + command: torch.Tensor, + ) -> torch.Tensor: + """Clip a physical command to per-joint robot limits when available.""" + if not term.clip_command: + return command + + body_data = getattr(self._env.robot, "body_data", None) + if body_data is None: + return command + joint_ids = term.joint_ids + if command_key == "qpos": + limits = getattr(body_data, "qpos_limits", None) + if limits is None: + return command + limits = limits[:, joint_ids, :].to(command.device) + return command.clamp(limits[..., 0], limits[..., 1]) + if command_key == "qvel": + limits = getattr(body_data, "qvel_limits", None) + else: + limits = getattr(body_data, "qf_limits", None) + if limits is None: + return command + limits = limits[:, joint_ids].to(command.device).abs() + return command.clamp(-limits, limits) + + def _process_post_action( + self, + action: EnvAction, + terms: list[tuple[str, ActionTerm]], + ) -> EnvAction: + """Apply post terms without modifying cached physical commands.""" + if isinstance(action, torch.Tensor): + if len(terms) != 1: + raise ValueError( + "A flat post-process action is only valid with one post term." + ) + return terms[0][1].process_action(action) + + if not isinstance(action, (TensorDict, Mapping)): + raise TypeError(f"Unsupported post-process action type: {type(action)!r}.") + result = ( + action.clone() + if isinstance(action, TensorDict) + else TensorDict(action, batch_size=[self.num_envs], device=self.device) + ) + for term_name, term in terms: + candidate_keys = (term_name, term.command_key, term.input_key) + key = next((key for key in candidate_keys if key in result), None) + if key is None: + raise KeyError( + f"Post action term '{term_name}' could not find any of " + f"{candidate_keys} in the action payload." + ) + result[key] = term.process_action(result[key]) + return result + + def _validate_pre_terms(self) -> None: + """Validate output types, policy spaces and overlapping joint groups.""" + occupied_joints: dict[int, tuple[str, ActionTerm]] = {} + for term_name, term in self.get_terms_by_mode("pre"): + if term.command_key not in {"qpos", "qvel", "qf"}: + raise ValueError( + f"Action term '{term_name}' has unsupported physical command " + f"type '{term.command_key}'." + ) + if term.action_space.shape != (term.action_dim,): + raise ValueError( + f"Action term '{term_name}' space shape {term.action_space.shape} " + f"does not match action_dim={term.action_dim}." + ) + for joint_id in term.joint_ids: + previous = occupied_joints.get(joint_id) + if previous is None: + occupied_joints[joint_id] = (term_name, term) + continue + previous_name, previous_term = previous + overlap_allowed = bool( + term.cfg.params.get("allow_overlap", False) + and previous_term.cfg.params.get("allow_overlap", False) + ) + if not overlap_allowed: + raise ValueError( + f"Action terms '{previous_name}' and '{term_name}' both " + f"control joint {joint_id}. Use disjoint joint_ids/control_part " + "selections, or explicitly set allow_overlap=true on both terms." + ) + + self._warn_for_drive_mismatch() + + def _warn_for_drive_mismatch(self) -> None: + """Warn when velocity/effort commands conflict with configured drives.""" + body_data = getattr(self._env.robot, "body_data", None) + if body_data is None: + return + stiffness = getattr(body_data, "joint_stiffness", None) + damping = getattr(body_data, "joint_damping", None) + for term_name, term in self.get_terms_by_mode("pre"): + joint_ids = term.joint_ids + if term.command_key == "qvel" and stiffness is not None: + if bool((stiffness[:, joint_ids] != 0).any()): + logger.log_warning( + f"Velocity action term '{term_name}' controls joints with " + "non-zero stiffness; the position drive may oppose the velocity target." + ) + elif term.command_key == "qf": + has_stiffness = stiffness is not None and bool( + (stiffness[:, joint_ids] != 0).any() + ) + has_damping = damping is not None and bool( + (damping[:, joint_ids] != 0).any() + ) + if has_stiffness or has_damping: + logger.log_warning( + f"Effort action term '{term_name}' controls joints with an " + "active position/velocity drive; qf will be additive rather than pure torque control." + ) def get_term(self, name: str) -> ActionTerm: """Get action term by name.""" diff --git a/embodichain/lab/gym/envs/managers/actions.py b/embodichain/lab/gym/envs/managers/actions.py index bdecd3e57..7d5ab36cc 100644 --- a/embodichain/lab/gym/envs/managers/actions.py +++ b/embodichain/lab/gym/envs/managers/actions.py @@ -37,6 +37,8 @@ from typing import TYPE_CHECKING +import gymnasium as gym +import numpy as np import torch from tensordict import TensorDict @@ -94,10 +96,11 @@ def input_key(self) -> str: @property def action_dim(self) -> int: - return len(self._env.active_joint_ids) + return len(self.joint_ids) def process_action(self, action: torch.Tensor) -> torch.Tensor: - return action * self._scale + self._env.robot.get_qpos() + current_qpos = self._env.robot.get_qpos()[:, self.joint_ids] + return action * self._scale + current_qpos class QposTerm(ActionTerm): @@ -127,7 +130,7 @@ def input_key(self) -> str: @property def action_dim(self) -> int: - return len(self._env.active_joint_ids) + return len(self.joint_ids) def process_action(self, action: torch.Tensor) -> torch.Tensor: qpos = action * self._scale @@ -161,7 +164,6 @@ class QposDenormalizedTerm(ActionTerm): def __init__(self, cfg: ActionTermCfg, env: EmbodiedEnv): super().__init__(cfg, env) self._scale = cfg.params.get("scale", 1.0) - self._joint_ids = cfg.params.get("joint_ids", self._env.active_joint_ids) self._range = cfg.params.get("range", [-1.0, 1.0]) @property @@ -170,17 +172,31 @@ def input_key(self) -> str: @property def action_dim(self) -> int: - return len(self._env.active_joint_ids) + return len(self.joint_ids) + + @property + def action_space(self) -> gym.spaces.Box: + """Policy action space matching the configured normalized range.""" + if len(self._range) != 2: + raise ValueError("QposDenormalizedTerm 'range' must contain [low, high].") + low = self._expand_action_bound(self._range[0], "low") + high = self._expand_action_bound(self._range[1], "high") + if np.any(low >= high): + raise ValueError( + "QposDenormalizedTerm policy range low must be smaller than high." + ) + if not np.isfinite(low).all() or not np.isfinite(high).all(): + raise ValueError("QposDenormalizedTerm policy range must be finite.") + return gym.spaces.Box(low=low, high=high, dtype=np.float32) def process_action(self, action: torch.Tensor) -> torch.Tensor: scaled = action * self._scale - qpos_limits = self._env.robot.body_data.qpos_limits[0, self._joint_ids] - low = qpos_limits[:, 0] - high = qpos_limits[:, 1] - scaled[:, self._joint_ids] = low + ( - scaled[:, self._joint_ids] - self._range[0] - ) / (self._range[1] - self._range[0]) * (high - low) - return scaled + qpos_limits = self._env.robot.body_data.qpos_limits[:, self.joint_ids] + low = qpos_limits[..., 0] + high = qpos_limits[..., 1] + return low + (scaled - self._range[0]) / (self._range[1] - self._range[0]) * ( + high - low + ) class QposNormalizedTerm(ActionTerm): @@ -203,7 +219,6 @@ class QposNormalizedTerm(ActionTerm): def __init__(self, cfg: ActionTermCfg, env: EmbodiedEnv): super().__init__(cfg, env) - self._joint_ids = cfg.params.get("joint_ids", self._env.active_joint_ids) self._range = cfg.params.get("range", [0.0, 1.0]) @property @@ -212,16 +227,15 @@ def input_key(self) -> str: @property def action_dim(self) -> int: - return len(self._env.active_joint_ids) + return len(self.joint_ids) def process_action(self, action: torch.Tensor) -> torch.Tensor: - qpos_limits = self._env.robot.body_data.qpos_limits[0, self._joint_ids] - low = qpos_limits[:, 0] - high = qpos_limits[:, 1] - action[:, self._joint_ids] = (action[:, self._joint_ids] - low) / ( - high - low - ) * (self._range[1] - self._range[0]) + self._range[0] - return action + qpos_limits = self._env.robot.body_data.qpos_limits[:, self.joint_ids] + low = qpos_limits[..., 0] + high = qpos_limits[..., 1] + return (action - low) / (high - low) * ( + self._range[1] - self._range[0] + ) + self._range[0] class EefPoseTerm(ActionTerm): @@ -263,13 +277,17 @@ def __init__(self, cfg: ActionTermCfg, env: EmbodiedEnv): def input_key(self) -> str: return "eef_pose" + @property + def command_key(self) -> str: + return "qpos" + @property def action_dim(self) -> int: return self._pose_dim def process_action(self, action: torch.Tensor) -> EnvAction: scaled = action * self._scale - current_qpos = self._env.robot.get_qpos() + current_qpos = self._env.robot.get_qpos()[:, self.joint_ids] batch_size = scaled.shape[0] target_pose = ( torch.eye(4, device=self.device).unsqueeze(0).repeat(batch_size, 1, 1) @@ -285,10 +303,14 @@ def process_action(self, action: torch.Tensor) -> EnvAction: f"EEF pose action must be 6D or 7D, got {scaled.shape[-1]}D" ) # Batch IK: robot.compute_ik supports (n_envs, 4, 4) pose and (n_envs, dof) seed - ret, qpos_ik = self._env.robot.compute_ik( - pose=target_pose, - joint_seed=current_qpos, - ) + ik_kwargs = { + "pose": target_pose, + "joint_seed": current_qpos, + } + control_part = self.cfg.params.get("control_part") + if control_part is not None: + ik_kwargs["name"] = control_part + ret, qpos_ik = self._env.robot.compute_ik(**ik_kwargs) # Fallback to current_qpos where IK failed result_qpos = torch.where( ret.unsqueeze(-1).expand_as(qpos_ik), qpos_ik, current_qpos @@ -327,7 +349,7 @@ def input_key(self) -> str: @property def action_dim(self) -> int: - return len(self._env.active_joint_ids) + return len(self.joint_ids) def process_action(self, action: torch.Tensor) -> torch.Tensor: return action * self._scale @@ -360,7 +382,7 @@ def input_key(self) -> str: @property def action_dim(self) -> int: - return len(self._env.active_joint_ids) + return len(self.joint_ids) def process_action(self, action: torch.Tensor) -> torch.Tensor: return action * self._scale diff --git a/embodichain/lab/gym/envs/managers/datasets.py b/embodichain/lab/gym/envs/managers/datasets.py index 4ae14dacb..911f08fc3 100644 --- a/embodichain/lab/gym/envs/managers/datasets.py +++ b/embodichain/lab/gym/envs/managers/datasets.py @@ -481,12 +481,21 @@ def _build_features(self) -> Dict: "names": joint_names, } - # Use full qpos dimension for action (includes gripper) - action_dim = state_dim + # Actions are policy-space values and may represent qpos, qvel, qf, + # end-effector poses, or mixed disjoint control terms. + action_space = getattr(self._env, "single_action_space", None) + action_dim = ( + int(np.prod(action_space.shape)) if action_space is not None else state_dim + ) + action_names = ( + joint_names + if action_dim == len(joint_names) + else [f"action_{index}" for index in range(action_dim)] + ) features[LeRobotKey.ACTION.value] = { "dtype": "float32", "shape": (action_dim,), - "names": joint_names, + "names": action_names, } # Setup sensor observation features based env.observation.sensor diff --git a/embodichain/lab/gym/envs/managers/rewards.py b/embodichain/lab/gym/envs/managers/rewards.py index edee82b93..adb6ce6ce 100644 --- a/embodichain/lab/gym/envs/managers/rewards.py +++ b/embodichain/lab/gym/envs/managers/rewards.py @@ -169,10 +169,18 @@ def action_smoothness_penalty( } ``` """ - if isinstance(action, torch.Tensor): - current_action = action - else: - current_action = action["qpos"] + action_manager = getattr(env, "action_manager", None) + current_action = action_manager.raw_action if action_manager is not None else None + if current_action is None: + if isinstance(action, torch.Tensor): + current_action = action + else: + command_values = [ + action[key] for key in ("qpos", "qvel", "qf") if key in action + ] + if not command_values: + return torch.zeros(env.num_envs, device=env.device) + current_action = torch.cat(command_values, dim=-1) buffer = getattr(env, "rollout_buffer", None) has_rl_prev_step = ( diff --git a/embodichain/lab/gym/utils/gym_utils.py b/embodichain/lab/gym/utils/gym_utils.py index 4f0dfce39..5c18ea353 100644 --- a/embodichain/lab/gym/utils/gym_utils.py +++ b/embodichain/lab/gym/utils/gym_utils.py @@ -702,6 +702,7 @@ class ComponentCfg: ) action_term = ActionTermCfg( func=term_func, + mode=term_params_modified.get("mode", "pre"), params=term_params_modified.get("params", {}), ) setattr(env_cfg.actions, term_name, action_term) diff --git a/embodichain/lab/sim/sim_manager.py b/embodichain/lab/sim/sim_manager.py index c6e8ce9e5..e7789f8c3 100644 --- a/embodichain/lab/sim/sim_manager.py +++ b/embodichain/lab/sim/sim_manager.py @@ -793,12 +793,21 @@ def render_camera_group(self, group_ids: list[int]) -> None: self._world.render_camera_group(group_ids) - def update(self, physics_dt: float | None = None, step: int = 10) -> None: + def update( + self, + physics_dt: float | None = None, + step: int = 10, + before_step_callback: Callable[[int], None] | None = None, + ) -> None: """Update the physics. Args: physics_dt (float | None, optional): the time step for physics simulation. Defaults to None. step (int, optional): the number of steps to update physics. Defaults to 10. + before_step_callback: Optional callback invoked immediately before + each physics substep with the zero-based substep index. This is + used by control environments to reapply effort commands across + action decimation. """ with self.profiler.section("sim_update", is_root=True): with self.profiler.section("gpu_physics_check"): @@ -816,6 +825,9 @@ def update(self, physics_dt: float | None = None, step: int = 10) -> None: with self.profiler.section("resolve_physics_dt"): physics_dt = self.sim_config.physics_dt for i in range(step): + if before_step_callback is not None: + with self.profiler.section("before_step_callback"): + before_step_callback(i) with self.profiler.section("gizmo_update"): self.update_gizmos() with self.profiler.section("world_update"): diff --git a/embodichain/lab/sim/types.py b/embodichain/lab/sim/types.py index c727ea830..4b29a9c10 100644 --- a/embodichain/lab/sim/types.py +++ b/embodichain/lab/sim/types.py @@ -14,15 +14,26 @@ # limitations under the License. # ---------------------------------------------------------------------------- +from __future__ import annotations + +from collections.abc import Mapping, Sequence + import numpy as np import torch -from typing import Sequence, Union from tensordict import TensorDict -Array = Union[torch.Tensor, np.ndarray, Sequence] -Device = Union[str, torch.device] +__all__ = ["Array", "Device", "EnvObs", "EnvAction"] + +Array = torch.Tensor | np.ndarray | Sequence +Device = str | torch.device -EnvObs = TensorDict[str, Union[torch.Tensor, TensorDict[str, torch.Tensor]]] +EnvObs = TensorDict[str, torch.Tensor | TensorDict[str, torch.Tensor]] -EnvAction = Union[torch.Tensor, TensorDict[str, torch.Tensor]] +EnvAction = ( + torch.Tensor + | np.ndarray + | Sequence + | Mapping[str, Array] + | TensorDict[str, torch.Tensor] +) diff --git a/embodichain/learning/rl/collector/sync_collector.py b/embodichain/learning/rl/collector/sync_collector.py index 7aef02b97..3bce0cd04 100644 --- a/embodichain/learning/rl/collector/sync_collector.py +++ b/embodichain/learning/rl/collector/sync_collector.py @@ -121,11 +121,13 @@ def _reset_env(self) -> TensorDict: return dict_to_tensordict(obs, self.device) def _to_action_dict(self, action: torch.Tensor) -> TensorDict | torch.Tensor: - am = getattr(self.env, "action_manager", None) - if am is None: - return action - else: - return am.convert_policy_action_to_env_action(action) + """Return the policy action unchanged. + + Action interpretation belongs to ``env.step`` so collectors do not need + to know whether an environment uses position, velocity, effort or mixed + control. The helper remains for compatibility with external subclasses. + """ + return action def _write_step( self, diff --git a/embodichain/learning/rl/evaluation.py b/embodichain/learning/rl/evaluation.py index 974a2ab7c..dbaf69767 100644 --- a/embodichain/learning/rl/evaluation.py +++ b/embodichain/learning/rl/evaluation.py @@ -39,15 +39,9 @@ def _flat_observation(observation: Any, device: torch.device) -> torch.Tensor: def _action_for_env(env: Any, action: torch.Tensor) -> Any: - action_manager = getattr(env, "action_manager", None) - if action_manager is None and hasattr(env, "get_wrapper_attr"): - try: - action_manager = env.get_wrapper_attr("action_manager") - except AttributeError: - action_manager = None - if action_manager is None: - return action - return action_manager.convert_policy_action_to_env_action(action) + """Pass policy action through; the environment owns action interpretation.""" + del env + return action def _selected_values(value: Any, indices: torch.Tensor) -> list[float]: diff --git a/embodichain/learning/rl/models/__init__.py b/embodichain/learning/rl/models/__init__.py index fa6d46ae6..11a4b942a 100644 --- a/embodichain/learning/rl/models/__init__.py +++ b/embodichain/learning/rl/models/__init__.py @@ -92,6 +92,7 @@ def build_policy( device=device, actor=actor, critic=critic, + squash_actions=bool(policy_block.get("squash_actions", False)), ) elif name == "actor_only": if actor is None: @@ -103,6 +104,7 @@ def build_policy( action_dim=action_dim, device=device, actor=actor, + squash_actions=bool(policy_block.get("squash_actions", False)), ) init_params = inspect.signature(policy_cls.__init__).parameters diff --git a/embodichain/learning/rl/models/actor_critic.py b/embodichain/learning/rl/models/actor_critic.py index 6bc47c5bb..527d7b214 100644 --- a/embodichain/learning/rl/models/actor_critic.py +++ b/embodichain/learning/rl/models/actor_critic.py @@ -40,6 +40,10 @@ class ActorCritic(Policy): Implements TensorDict-native interfaces while preserving `get_action()` compatibility for evaluation and legacy call-sites. + + When ``squash_actions`` is enabled, Gaussian samples are transformed with + tanh into ``[-1, 1]`` and log probabilities include the transform's + Jacobian correction. """ def __init__( @@ -49,7 +53,8 @@ def __init__( device: torch.device, actor: nn.Module, critic: nn.Module, - ): + squash_actions: bool = False, + ) -> None: super().__init__() self.obs_dim = obs_dim self.action_dim = action_dim @@ -57,6 +62,7 @@ def __init__( self.actor = actor self.critic = critic + self.squash_actions = squash_actions self.actor.to(self.device) self.critic.to(self.device) @@ -70,6 +76,22 @@ def _distribution(self, obs: torch.Tensor) -> Normal: std = log_std.exp().expand(mean.shape[0], -1) return Normal(mean, std) + def _action_log_prob( + self, + distribution: Normal, + action: torch.Tensor, + pre_squash_action: torch.Tensor | None = None, + ) -> torch.Tensor: + """Compute action log probability with the tanh Jacobian correction.""" + if not self.squash_actions: + return distribution.log_prob(action).sum(dim=-1) + epsilon = torch.finfo(action.dtype).eps + bounded_action = action.clamp(-1.0 + epsilon, 1.0 - epsilon) + if pre_squash_action is None: + pre_squash_action = torch.atanh(bounded_action) + log_det_jacobian = torch.log(1.0 - bounded_action.square() + epsilon) + return (distribution.log_prob(pre_squash_action) - log_det_jacobian).sum(dim=-1) + def forward( self, tensordict: TensorDict, deterministic: bool = False ) -> TensorDict: @@ -100,13 +122,18 @@ def _sample_action( dist = self._distribution(obs) mean = dist.mean if deterministic: - action = mean + pre_squash_action = mean elif reparameterized: - action = dist.rsample() + pre_squash_action = dist.rsample() else: - action = dist.sample() + pre_squash_action = dist.sample() + action = ( + torch.tanh(pre_squash_action) if self.squash_actions else pre_squash_action + ) tensordict["action"] = action - tensordict["sample_log_prob"] = dist.log_prob(action).sum(dim=-1) + tensordict["sample_log_prob"] = self._action_log_prob( + dist, action, pre_squash_action + ) if reparameterized: tensordict["entropy"] = dist.entropy().sum(dim=-1) tensordict["value"] = self.critic(obs).squeeze(-1) @@ -122,7 +149,7 @@ def evaluate_actions(self, tensordict: TensorDict) -> TensorDict: dist = self._distribution(obs) return TensorDict( { - "sample_log_prob": dist.log_prob(action).sum(dim=-1), + "sample_log_prob": self._action_log_prob(dist, action), "entropy": dist.entropy().sum(dim=-1), "value": self.critic(obs).squeeze(-1), }, diff --git a/embodichain/learning/rl/models/actor_only.py b/embodichain/learning/rl/models/actor_only.py index 96d0d0a1f..68a7ea582 100644 --- a/embodichain/learning/rl/models/actor_only.py +++ b/embodichain/learning/rl/models/actor_only.py @@ -31,6 +31,10 @@ class ActorOnly(Policy): Same interface as ActorCritic: get_action and evaluate_actions return (action, log_prob, value), but value is always zeros since no critic is used. + + When ``squash_actions`` is enabled, Gaussian samples are transformed with + tanh into ``[-1, 1]`` and log probabilities include the transform's + Jacobian correction. """ def __init__( @@ -39,13 +43,15 @@ def __init__( action_dim: int, device: torch.device, actor: nn.Module, - ): + squash_actions: bool = False, + ) -> None: super().__init__() self.obs_dim = obs_dim self.action_dim = action_dim self.device = device self.actor = actor + self.squash_actions = squash_actions self.actor.to(self.device) self.log_std = nn.Parameter(torch.zeros(self.action_dim, device=self.device)) @@ -58,6 +64,22 @@ def _distribution(self, obs: torch.Tensor) -> Normal: std = log_std.exp().expand(mean.shape[0], -1) return Normal(mean, std) + def _action_log_prob( + self, + distribution: Normal, + action: torch.Tensor, + pre_squash_action: torch.Tensor | None = None, + ) -> torch.Tensor: + """Compute action log probability with the tanh Jacobian correction.""" + if not self.squash_actions: + return distribution.log_prob(action).sum(dim=-1) + epsilon = torch.finfo(action.dtype).eps + bounded_action = action.clamp(-1.0 + epsilon, 1.0 - epsilon) + if pre_squash_action is None: + pre_squash_action = torch.atanh(bounded_action) + log_det_jacobian = torch.log(1.0 - bounded_action.square() + epsilon) + return (distribution.log_prob(pre_squash_action) - log_det_jacobian).sum(dim=-1) + def forward( self, tensordict: TensorDict, deterministic: bool = False ) -> TensorDict: @@ -88,13 +110,18 @@ def _sample_action( dist = self._distribution(obs) mean = dist.mean if deterministic: - action = mean + pre_squash_action = mean elif reparameterized: - action = dist.rsample() + pre_squash_action = dist.rsample() else: - action = dist.sample() + pre_squash_action = dist.sample() + action = ( + torch.tanh(pre_squash_action) if self.squash_actions else pre_squash_action + ) tensordict["action"] = action - tensordict["sample_log_prob"] = dist.log_prob(action).sum(dim=-1) + tensordict["sample_log_prob"] = self._action_log_prob( + dist, action, pre_squash_action + ) if reparameterized: tensordict["entropy"] = dist.entropy().sum(dim=-1) tensordict["value"] = torch.zeros( @@ -115,7 +142,7 @@ def evaluate_actions(self, tensordict: TensorDict) -> TensorDict: dist = self._distribution(obs) return TensorDict( { - "sample_log_prob": dist.log_prob(action).sum(dim=-1), + "sample_log_prob": self._action_log_prob(dist, action), "entropy": dist.entropy().sum(dim=-1), "value": torch.zeros(obs.shape[0], device=self.device, dtype=obs.dtype), }, diff --git a/embodichain/learning/rl/train.py b/embodichain/learning/rl/train.py index 41c44a3f1..0713fb3aa 100644 --- a/embodichain/learning/rl/train.py +++ b/embodichain/learning/rl/train.py @@ -499,10 +499,37 @@ def train_from_config( ) # Build Policy via registry + action_manager = env.get_wrapper_attr("action_manager") + managed_action_dim = ( + action_manager.total_action_dim if action_manager is not None else 0 + ) + if managed_action_dim > 0: + manager_space = action_manager.single_action_space + has_normalized_bounds = bool( + np.allclose(manager_space.low, -1.0) + and np.allclose(manager_space.high, 1.0) + ) + configured_squashing = policy_block.get("squash_actions") + if configured_squashing is None: + policy_block = { + **policy_block, + "squash_actions": has_normalized_bounds, + } + if not has_normalized_bounds: + logger.log_warning( + "ActionManager policy bounds are not [-1, 1]; automatic tanh " + "action squashing is disabled. Configure a policy distribution " + "that matches the custom action_range." + ) + elif bool(configured_squashing) and not has_normalized_bounds: + raise ValueError( + "policy.squash_actions=true requires ActionManager bounds [-1, 1]. " + "Use normalized action terms or a custom bounded distribution." + ) policy_name = policy_block["name"] env_action_dim = ( - env.get_wrapper_attr("action_manager").total_action_dim - if env.get_wrapper_attr("action_manager") is not None + managed_action_dim + if managed_action_dim > 0 else len(env.get_wrapper_attr("active_joint_ids")) ) action_dim = policy_block.get("action_dim", env_action_dim) diff --git a/tests/gym/envs/managers/test_action_manager.py b/tests/gym/envs/managers/test_action_manager.py index c617fee58..5d62a7ca4 100644 --- a/tests/gym/envs/managers/test_action_manager.py +++ b/tests/gym/envs/managers/test_action_manager.py @@ -16,8 +16,13 @@ from __future__ import annotations +from types import SimpleNamespace + +import gymnasium as gym +import numpy as np import pytest import torch +from tensordict import TensorDict from embodichain.lab.gym.envs.managers import ActionManager from embodichain.lab.gym.envs.managers.actions import ( @@ -87,6 +92,34 @@ def compute_ik(self, pose, joint_seed): return ret, joint_seed.clone() +class MockControlEnv(MockEnv): + """Mock env that records typed robot control commands.""" + + def __init__(self, num_envs: int = 2, action_dim: int = 4): + super().__init__(num_envs, action_dim) + self.command_calls: list[tuple[str, torch.Tensor, list[int]]] = [] + self._body_data = SimpleNamespace( + qpos_limits=torch.tensor([[[-1.0, 1.0]] * action_dim]), + qvel_limits=torch.full((1, action_dim), 2.0), + qf_limits=torch.full((1, action_dim), 5.0), + joint_stiffness=torch.zeros(1, action_dim), + joint_damping=torch.zeros(1, action_dim), + ) + + @property + def body_data(self): + return self._body_data + + def set_qpos(self, qpos: torch.Tensor, joint_ids: list[int]) -> None: + self.command_calls.append(("qpos", qpos.clone(), list(joint_ids))) + + def set_qvel(self, qvel: torch.Tensor, joint_ids: list[int]) -> None: + self.command_calls.append(("qvel", qvel.clone(), list(joint_ids))) + + def set_qf(self, qf: torch.Tensor, joint_ids: list[int]) -> None: + self.command_calls.append(("qf", qf.clone(), list(joint_ids))) + + def test_delta_qpos_term_process_action(): """DeltaQposTerm: qpos = current_qpos + scale * action.""" env = MockEnv(num_envs=4, action_dim=6) @@ -133,6 +166,28 @@ def test_qpos_denormalized_term_process_action(): assert term.action_dim == 2 +def test_qpos_denormalized_term_uses_local_selected_joint_columns() -> None: + """Subset actions map directly to their selected joints' limits.""" + env = MockControlEnv(num_envs=2, action_dim=4) + env.body_data.qpos_limits = torch.tensor( + [[[-1.0, 1.0], [-2.0, 2.0], [-3.0, 3.0], [-4.0, 4.0]]] + ) + term = QposDenormalizedTerm( + ActionTermCfg( + func=QposDenormalizedTerm, + params={"joint_ids": [1, 3]}, + ), + env, + ) + + result = term.process_action(torch.tensor([[-1.0, 1.0], [0.0, 0.0]])) + + torch.testing.assert_close( + result, + torch.tensor([[-2.0, 4.0], [0.0, 0.0]]), + ) + + def test_eef_pose_term_process_action_6d(): """EefPoseTerm: 6D pose (x,y,z,euler) -> IK -> qpos.""" env = MockEnvForEef(num_envs=2, action_dim=6) @@ -352,3 +407,166 @@ def test_qpos_normalized_term_from_qpos(): # [-1, 0, 1] -> [0, 0.5, 1] when normalized to [0, 1] expected = torch.tensor([[0.0, 0.5, 1.0], [0.0, 0.5, 1.0]]) torch.testing.assert_close(result, expected) + + +def test_qpos_normalized_term_uses_local_selected_joint_columns() -> None: + """Post-processing a subset does not index the local action as full qpos.""" + env = MockControlEnv(num_envs=2, action_dim=4) + env.body_data.qpos_limits = torch.tensor( + [[[-1.0, 1.0], [-2.0, 2.0], [-3.0, 3.0], [-4.0, 4.0]]] + ) + term = QposNormalizedTerm( + ActionTermCfg( + func=QposNormalizedTerm, + params={"joint_ids": [1, 3]}, + mode="post", + ), + env, + ) + + result = term.process_action(torch.tensor([[-2.0, 4.0], [0.0, 0.0]])) + + torch.testing.assert_close( + result, + torch.tensor([[0.0, 1.0], [0.5, 0.5]]), + ) + + +def test_action_manager_routes_single_velocity_term() -> None: + """A velocity term remains typed and reaches ``set_qvel``.""" + env = MockControlEnv(num_envs=2, action_dim=4) + manager = ActionManager( + { + "arm_velocity": ActionTermCfg( + func=QvelTerm, + params={"scale": 2.0, "joint_ids": [0, 2]}, + ) + }, + env, + ) + + action = torch.tensor([[0.25, -0.5], [0.5, 0.75]]) + processed = manager.process_action(action) + manager.apply_action() + + assert isinstance(manager.single_action_space, gym.spaces.Box) + assert manager.single_action_space.shape == (2,) + assert isinstance(processed, TensorDict) + torch.testing.assert_close(processed["qvel"], action * 2.0) + command_type, command, joint_ids = env.command_calls[0] + assert command_type == "qvel" + assert joint_ids == [0, 2] + torch.testing.assert_close(command, action * 2.0) + + +def test_action_manager_clips_effort_to_selected_joint_limits() -> None: + """Effort commands are clipped before they reach the robot.""" + env = MockControlEnv(num_envs=2, action_dim=3) + manager = ActionManager( + { + "effort": ActionTermCfg( + func=QfTerm, + params={"scale": 10.0, "joint_ids": [1]}, + ) + }, + env, + ) + + processed = manager.process_action(torch.tensor([[1.0], [-1.0]])) + manager.apply_action(command_keys={"qf"}) + + expected = torch.tensor([[5.0], [-5.0]]) + torch.testing.assert_close(processed["qf"], expected) + command_type, command, joint_ids = env.command_calls[0] + assert command_type == "qf" + assert joint_ids == [1] + torch.testing.assert_close(command, expected) + + +def test_action_manager_splits_flat_mixed_control_action() -> None: + """One flat policy action can target disjoint position and effort groups.""" + env = MockControlEnv(num_envs=2, action_dim=4) + manager = ActionManager( + { + "arm_position": ActionTermCfg( + func=QposTerm, + params={"joint_ids": [0, 1]}, + ), + "gripper_effort": ActionTermCfg( + func=QfTerm, + params={"scale": 4.0, "joint_ids": [2, 3]}, + ), + }, + env, + ) + action = torch.tensor([[0.1, 0.2, 0.3, 0.4], [-0.1, -0.2, -0.3, -0.4]]) + + processed = manager.process_action(action) + manager.apply_action() + + assert manager.single_action_space.shape == (4,) + np.testing.assert_allclose(manager.single_action_space.low, -1.0) + np.testing.assert_allclose(manager.single_action_space.high, 1.0) + torch.testing.assert_close(manager.raw_action, action) + torch.testing.assert_close(processed["arm_position", "qpos"], action[:, :2]) + torch.testing.assert_close(processed["gripper_effort", "qf"], action[:, 2:] * 4.0) + assert [(kind, ids) for kind, _, ids in env.command_calls] == [ + ("qpos", [0, 1]), + ("qf", [2, 3]), + ] + + +def test_action_manager_accepts_structured_term_actions() -> None: + """Structured inputs may address terms by their stable config names.""" + env = MockControlEnv(num_envs=2, action_dim=4) + manager = ActionManager( + { + "left": ActionTermCfg(func=QvelTerm, params={"joint_ids": [0, 1]}), + "right": ActionTermCfg(func=QfTerm, params={"joint_ids": [2, 3]}), + }, + env, + ) + left = torch.tensor([[0.1, 0.2], [0.3, 0.4]]) + right = torch.tensor([[0.5, 0.6], [0.7, 0.8]]) + + manager.process_action({"left": left, "right": right}) + + torch.testing.assert_close(manager.raw_action, torch.cat((left, right), dim=-1)) + + +def test_action_manager_rejects_overlapping_joint_groups() -> None: + """Ambiguous mixed control over the same joint fails during setup.""" + env = MockControlEnv(num_envs=2, action_dim=3) + + with pytest.raises(ValueError, match="both control joint 1"): + ActionManager( + { + "position": ActionTermCfg(func=QposTerm, params={"joint_ids": [0, 1]}), + "effort": ActionTermCfg(func=QfTerm, params={"joint_ids": [1, 2]}), + }, + env, + ) + + +def test_action_manager_rejects_nonfinite_policy_action() -> None: + """NaN policy outputs are rejected before reaching physics.""" + env = MockControlEnv(num_envs=2, action_dim=2) + manager = ActionManager( + {"velocity": ActionTermCfg(func=QvelTerm)}, + env, + ) + + with pytest.raises(ValueError, match="NaN or infinite"): + manager.process_action(torch.tensor([[0.0, float("nan")], [0.0, 0.0]])) + + +def test_action_manager_rejects_wrong_policy_action_shape() -> None: + """The manager reports the expected batched policy shape.""" + env = MockControlEnv(num_envs=2, action_dim=2) + manager = ActionManager( + {"velocity": ActionTermCfg(func=QvelTerm)}, + env, + ) + + with pytest.raises(ValueError, match=r"expected \(2, 2\)"): + manager.process_action(torch.zeros(2, 1)) diff --git a/tests/gym/envs/managers/test_dataset_functors.py b/tests/gym/envs/managers/test_dataset_functors.py index d1c4aeac0..d5da9b118 100644 --- a/tests/gym/envs/managers/test_dataset_functors.py +++ b/tests/gym/envs/managers/test_dataset_functors.py @@ -96,6 +96,7 @@ def __init__( self.active_joint_ids = list(range(num_joints)) self.robot = MockRobot(num_joints) + self.single_action_space = Mock(shape=(num_joints,)) # Mock has_sensors self.has_sensors = has_sensors @@ -248,6 +249,31 @@ def test_build_features_creates_correct_structure(self, mock_lerobot_dataset): assert features[LeRobotKey.OBS_STATE.value]["shape"] == (6,) assert features[LeRobotKey.ACTION.value]["shape"] == (6,) + @patch("embodichain.lab.gym.envs.managers.datasets.LeRobotDataset") + def test_build_features_uses_policy_action_dimension(self, mock_lerobot_dataset): + """Recorded actions need not have the same dimension as robot qpos.""" + env = MockEnvForDataset(num_joints=6) + env.single_action_space = Mock(shape=(8,)) + mock_dataset_instance = Mock() + mock_dataset_instance.meta = Mock() + mock_dataset_instance.meta.info = {"fps": 30} + mock_lerobot_dataset.create.return_value = mock_dataset_instance + cfg = MockFunctorCfg( + params={ + "save_path": "/tmp/test_dataset", + "robot_meta": {"robot_type": "test_robot", "control_freq": 30}, + "instruction": {"lang": "test task"}, + "extra": {"task_description": "test"}, + "use_videos": False, + } + ) + + features = LeRobotRecorder(cfg, env)._build_features() + + action_feature = features[LeRobotKey.ACTION.value] + assert action_feature["shape"] == (8,) + assert action_feature["names"] == [f"action_{index}" for index in range(8)] + @patch("embodichain.lab.gym.envs.managers.datasets.LeRobotDataset") def test_build_features_with_sensor(self, mock_lerobot_dataset): """Test that _build_features includes sensor features when sensors exist.""" diff --git a/tests/gym/envs/managers/test_reward_functors.py b/tests/gym/envs/managers/test_reward_functors.py index cab8b6b29..089e5952c 100644 --- a/tests/gym/envs/managers/test_reward_functors.py +++ b/tests/gym/envs/managers/test_reward_functors.py @@ -17,11 +17,12 @@ from __future__ import annotations +from types import SimpleNamespace +from unittest.mock import MagicMock, Mock + import pytest import torch -from unittest.mock import MagicMock, Mock - class MockRobot: """Mock robot for reward functor tests.""" @@ -293,6 +294,23 @@ def test_handles_dict_action(self): # All have negative penalty from action difference assert torch.all(result < 0) + def test_uses_flat_raw_action_from_action_manager(self): + """Mixed physical commands are compared in their policy-space order.""" + env = MockEnv(num_envs=4) + env.current_rollout_step = 1 + env.rollout_buffer["action"][:4, 0, :] = torch.zeros(4, 6) + env.rollout_buffer["done"][:4, 0] = False + raw_action = torch.ones(4, 6) + env.action_manager = SimpleNamespace(raw_action=raw_action) + processed_action = { + "velocity": {"qvel": torch.zeros(4, 3)}, + "effort": {"qf": torch.zeros(4, 3)}, + } + + result = action_smoothness_penalty(env, {}, processed_action, {}) + + torch.testing.assert_close(result, torch.full((4,), -(6.0**0.5))) + def test_zero_when_expert_buffer_lacks_rl_keys(self): """Expert buffers without done/action keys yield zero penalty.""" env = MockEnv(num_envs=4) diff --git a/tests/gym/envs/test_embodied_env.py b/tests/gym/envs/test_embodied_env.py index 77b8334bf..d919c0c18 100644 --- a/tests/gym/envs/test_embodied_env.py +++ b/tests/gym/envs/test_embodied_env.py @@ -26,8 +26,10 @@ from embodichain.lab.sim.cfg import RenderCfg from embodichain.lab.gym.envs import EmbodiedEnv, EmbodiedEnvCfg +from embodichain.lab.gym.envs.managers import ActionManager +from embodichain.lab.gym.envs.managers.actions import QfTerm, QvelTerm +from embodichain.lab.gym.envs.managers.cfg import ActionTermCfg, EventCfg from embodichain.lab.sim.objects import RigidObject, Robot -from embodichain.lab.gym.envs.managers.cfg import EventCfg from embodichain.lab.gym.envs.managers.randomization.visual import ( randomize_visual_material, set_rigid_object_visual_material, @@ -140,6 +142,118 @@ def test_visual_randomization_filter_keeps_deterministic_material_events(): assert events.set_material is not None +class _CommandRobot: + """Small robot stub for direct control-command routing tests.""" + + def __init__(self) -> None: + self.body_data = SimpleNamespace( + qpos_limits=torch.tensor([[[-1.0, 1.0], [-2.0, 2.0]]]), + qvel_limits=torch.tensor([[3.0, 4.0]]), + qf_limits=torch.tensor([[5.0, 6.0]]), + ) + self.calls: list[tuple[str, torch.Tensor, list[int]]] = [] + + def set_qpos(self, qpos: torch.Tensor, joint_ids: list[int]) -> None: + self.calls.append(("qpos", qpos.clone(), list(joint_ids))) + + def set_qvel(self, qvel: torch.Tensor, joint_ids: list[int]) -> None: + self.calls.append(("qvel", qvel.clone(), list(joint_ids))) + + def set_qf(self, qf: torch.Tensor, joint_ids: list[int]) -> None: + self.calls.append(("qf", qf.clone(), list(joint_ids))) + + +def _make_direct_control_env() -> EmbodiedEnv: + """Build an uninitialized env with only direct command dependencies.""" + env = EmbodiedEnv.__new__(EmbodiedEnv) + env.sim = SimpleNamespace(device=torch.device("cpu")) + env._num_envs = 1 + env.active_joint_ids = [0, 1] + env.robot = _CommandRobot() + env.action_manager = None + env._pending_direct_qf = None + return env + + +def test_step_action_routes_direct_velocity_and_effort_commands() -> None: + """Direct mappings retain their command type and enforce robot limits.""" + env = _make_direct_control_env() + + processed = env._step_action({"qvel": [[8.0, -8.0]], "qf": np.array([[9.0, -9.0]])}) + + torch.testing.assert_close(processed["qvel"], torch.tensor([[3.0, -4.0]])) + torch.testing.assert_close(processed["qf"], torch.tensor([[5.0, -6.0]])) + assert [(kind, ids) for kind, _, ids in env.robot.calls] == [ + ("qvel", [0, 1]), + ("qf", [0, 1]), + ] + + +def test_effort_command_is_reapplied_before_physics_substep() -> None: + """The latest direct effort is held throughout control decimation.""" + env = _make_direct_control_env() + assert env._get_before_sim_step_callback() is None + env._step_action({"qf": [[1.0, 2.0]]}) + + callback = env._get_before_sim_step_callback() + assert callback is not None + callback(0) + callback(1) + + effort_calls = [call for call in env.robot.calls if call[0] == "qf"] + assert len(effort_calls) == 3 + for _, command, joint_ids in effort_calls: + torch.testing.assert_close(command, torch.tensor([[1.0, 2.0]])) + assert joint_ids == [0, 1] + + +def test_step_action_preserves_bare_tensor_qpos_compatibility() -> None: + """A bare tensor remains the legacy joint-position command.""" + env = _make_direct_control_env() + + processed = env._step_action(torch.tensor([[2.0, -3.0]])) + + torch.testing.assert_close(processed, torch.tensor([[1.0, -2.0]])) + assert env.robot.calls[0][0] == "qpos" + + +def test_single_velocity_action_term_is_not_misrouted_as_qpos() -> None: + """The complete manager/env path preserves a single term's qvel type.""" + env = _make_direct_control_env() + env._traj_buffer = None + env.action_manager = ActionManager( + {"velocity": ActionTermCfg(func=QvelTerm, params={"scale": 2.0})}, + env, + ) + + processed = env._preprocess_action(torch.tensor([[0.5, -0.5]])) + env._step_action(processed) + + assert [kind for kind, _, _ in env.robot.calls] == ["qvel"] + torch.testing.assert_close(env.robot.calls[0][1], torch.tensor([[1.0, -1.0]])) + + +def test_managed_effort_action_is_applied_at_substep_rate() -> None: + """The complete manager/env path holds a qf term across decimation.""" + env = _make_direct_control_env() + env._traj_buffer = None + env.action_manager = ActionManager( + {"effort": ActionTermCfg(func=QfTerm, params={"scale": 4.0})}, + env, + ) + + processed = env._preprocess_action(torch.tensor([[0.5, -0.5]])) + env._step_action(processed) + callback = env._get_before_sim_step_callback() + assert callback is not None + callback(0) + callback(1) + + assert [kind for kind, _, _ in env.robot.calls] == ["qf", "qf"] + for _, command, _ in env.robot.calls: + torch.testing.assert_close(command, torch.tensor([[2.0, -2.0]])) + + class EmbodiedEnvTest: """Shared test logic for CPU and CUDA.""" @@ -182,6 +296,20 @@ def test_env_rollout(self): ), f"Expected truncated shape ({self.env.get_wrapper_attr('num_envs')},), got {truncated.shape}" assert obs.get("robot") is not None, "Expected 'robot' info in the info dict" + def test_typed_velocity_and_effort_rollout(self): + """Typed qvel and qf commands execute through a real simulation step.""" + self.env.reset() + num_envs = self.env.get_wrapper_attr("num_envs") + action_dim = len(self.env.get_wrapper_attr("active_joint_ids")) + device = self.env.get_wrapper_attr("device") + command = torch.zeros(num_envs, action_dim, device=device) + + _, velocity_reward, _, _, _ = self.env.step({"qvel": command}) + _, effort_reward, _, _, _ = self.env.step({"qf": command}) + + assert velocity_reward.shape == (num_envs,) + assert effort_reward.shape == (num_envs,) + def teardown_method(self): """Clean up resources after each test method.""" if hasattr(self, "env") and self.env is not None: diff --git a/tests/gym/utils/test_gym_utils.py b/tests/gym/utils/test_gym_utils.py index c520ddd18..62fb4de1f 100644 --- a/tests/gym/utils/test_gym_utils.py +++ b/tests/gym/utils/test_gym_utils.py @@ -508,6 +508,39 @@ def test_different_max_episode_steps(): class TestConfigToCfgFromFile: + def test_action_mode_parses_from_gym_config(self): + """ActionManager post-processing mode is preserved by config parsing.""" + config = { + "id": "EmbodiedEnv-v1", + "env": { + "events": {}, + "observations": {}, + "rewards": {}, + "actions": { + "normalize": { + "func": "QposNormalizedTerm", + "mode": "post", + "params": {"range": [0.0, 1.0]}, + } + }, + }, + "robot": { + "uid": "TestRobot", + "urdf_cfg": { + "components": [ + { + "component_type": "arm", + "urdf_path": "UniversalRobots/UR5/UR5.urdf", + } + ] + }, + }, + } + + cfg = config_to_cfg(config, manager_modules=DEFAULT_MANAGER_MODULES) + + assert cfg.actions.normalize.mode == "post" + def test_yaml_gym_config_parses_to_cfg(self, tmp_path): config = { "id": "EmbodiedEnv-v1", diff --git a/tests/learning/test_squashed_policy.py b/tests/learning/test_squashed_policy.py new file mode 100644 index 000000000..7fad02213 --- /dev/null +++ b/tests/learning/test_squashed_policy.py @@ -0,0 +1,144 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Tests for bounded Gaussian policy actions.""" + +from __future__ import annotations + +import pytest +import torch +from tensordict import TensorDict + +from embodichain.learning.rl.models import ActorCritic, ActorOnly, build_policy + + +def _make_policy( + policy_type: type[ActorCritic] | type[ActorOnly], + *, + bias: float = 0.25, + squash_actions: bool = True, +) -> ActorCritic | ActorOnly: + """Create a deterministic small policy for distribution tests.""" + device = torch.device("cpu") + actor = torch.nn.Linear(2, 2) + torch.nn.init.zeros_(actor.weight) + torch.nn.init.constant_(actor.bias, bias) + if policy_type is ActorCritic: + critic = torch.nn.Linear(2, 1) + policy: ActorCritic | ActorOnly = ActorCritic( + obs_dim=2, + action_dim=2, + device=device, + actor=actor, + critic=critic, + squash_actions=squash_actions, + ) + else: + policy = ActorOnly( + obs_dim=2, + action_dim=2, + device=device, + actor=actor, + squash_actions=squash_actions, + ) + with torch.no_grad(): + policy.log_std.fill_(-1.0) + return policy + + +@pytest.mark.parametrize("policy_type", [ActorCritic, ActorOnly]) +def test_squashed_policy_actions_stay_in_normalized_bounds(policy_type) -> None: + """Both built-in Gaussian policies obey the manager's normalized range.""" + policy = _make_policy(policy_type, bias=5.0) + tensordict = TensorDict( + {"obs": torch.zeros(8, 2)}, + batch_size=[8], + ) + + result = policy(tensordict, deterministic=True) + + assert bool((result["action"] <= 1.0).all()) + assert bool((result["action"] >= -1.0).all()) + assert bool((result["action"] > 0.99).all()) + + +@pytest.mark.parametrize("policy_type", [ActorCritic, ActorOnly]) +def test_squashed_policy_evaluation_reproduces_sample_log_prob(policy_type) -> None: + """PPO-style reevaluation uses the same tanh Jacobian correction.""" + policy = _make_policy(policy_type) + sample = policy( + TensorDict({"obs": torch.zeros(16, 2)}, batch_size=[16]), + deterministic=False, + ) + evaluation_input = TensorDict( + { + "obs": sample["obs"].clone(), + "action": sample["action"].clone(), + }, + batch_size=[16], + ) + + evaluated = policy.evaluate_actions(evaluation_input) + + torch.testing.assert_close( + evaluated["sample_log_prob"], + sample["sample_log_prob"], + atol=1e-5, + rtol=1e-5, + ) + + +def test_squashed_actor_only_keeps_pathwise_gradients() -> None: + """Tanh bounding remains compatible with differentiable RL training.""" + policy = _make_policy(ActorOnly) + result = policy.get_differentiable_action( + TensorDict({"obs": torch.ones(4, 2)}, batch_size=[4]), + deterministic=True, + ) + + result["action"].sum().backward() + + assert policy.actor.weight.grad is not None + assert bool(torch.isfinite(policy.actor.weight.grad).all()) + assert bool((policy.actor.weight.grad != 0).any()) + + +def test_unsquashed_policy_keeps_legacy_action_behavior() -> None: + """Existing non-simulator users remain unbounded unless explicitly enabled.""" + policy = _make_policy(ActorOnly, bias=2.0, squash_actions=False) + + result = policy( + TensorDict({"obs": torch.zeros(1, 2)}, batch_size=[1]), + deterministic=True, + ) + + torch.testing.assert_close(result["action"], torch.full((1, 2), 2.0)) + + +def test_policy_factory_forwards_squash_actions_option() -> None: + """Configuration can enable the bounded distribution through the factory.""" + actor = torch.nn.Linear(2, 2) + + policy = build_policy( + {"name": "actor_only", "squash_actions": True}, + obs_space=2, + action_space=2, + device=torch.device("cpu"), + actor=actor, + ) + + assert isinstance(policy, ActorOnly) + assert policy.squash_actions is True diff --git a/tests/sim/test_sim_manager.py b/tests/sim/test_sim_manager.py index 6a4fd5e4f..67730662a 100644 --- a/tests/sim/test_sim_manager.py +++ b/tests/sim/test_sim_manager.py @@ -214,6 +214,21 @@ def test_sim_update_refreshes_dirty_visualization_and_captures_current_state() - assert all(call["overlays"] is None for call in runtime.capture_calls) +def test_sim_update_invokes_callback_before_every_physics_substep() -> None: + """Control callbacks run once per substep and before the world update.""" + sim, _ = _make_visualization_sim_manager() + callback_events: list[tuple[int, int]] = [] + + sim.update( + step=3, + before_step_callback=lambda index: callback_events.append( + (index, len(sim._world.physics_updates)) + ), + ) + + assert callback_events == [(0, 0), (1, 1), (2, 2)] + + def test_sim_manager_persists_overlays_across_automatic_captures() -> None: sim, runtime = _make_visualization_sim_manager() overlays = SceneOverlays(