From fbfc4bd11dd209d135b30e93ef68464d3b20fa1e Mon Sep 17 00:00:00 2001 From: zilch Date: Sat, 19 Sep 2026 00:56:06 +0800 Subject: [PATCH] =?UTF-8?q?refactor!:=20state.info=20ownership=20=E2=80=94?= =?UTF-8?q?=20episode=20state=20on=20env=20buffers,=20time=5Fouts=20derive?= =?UTF-8?q?d=20in=20RSLRL,=20info=20replaced=20by=20typed=20reward=5Fterms?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Complete cleanup of the state.info channel in three moves: 1. Framework info ownership: ArrayEnv/TorchEnv stop maintaining info["time_outs"]; the RSLRL wrappers derive it from truncated & ~terminated for value bootstrapping (SKRL already used state.truncated, fastsac its own terminated/truncated returns). stewart stops overwriting the framework flag with a local float copy. 2. Episode-scoped task state migrates from info dicts to env-instance full-batch buffers across all environments (anymal_c, stewart, shadow_hand, franka x2, rm65 x2, quadruped walk, go1, bounce_ball, manipulator, basic quadruped, finger, pendulum): __init__ allocates the buffers, reset writes done rows in place, consumers read them directly with no info.get()/membership fallbacks. Dead writes removed (target_info, phase2_mask, handle_pose_override path, max_consecutive_bounces, action_delta, ...). Fixes partial-reset shape bugs in stewart's disturbance writes and walk_np's _randomize_params, and hopper's hop branch never writing its reward terms. 3. state.info is removed: ArrayEnvState/TorchEnvState gain a typed reward_terms field for the per-term reward breakdown (freshly written every transition) alongside metrics; reset() -> None so the reset-info merge machinery (_merge_reset_info/_replace_info_values) is deleted; the manager frontend writes reward_terms from the kernel layout; motrix_rl's env_infos() composes the RL-boundary dict from the typed fields; deploy source_rollout pins commands via the env buffer (silently broken since the walk_np migration). Reset-time Reward zeroing dropped (quadruped's three static init tables included). Tests and bilingual docs updated. Episode state lives on env buffers, lifecycle flags and reward terms on the typed state object, reduced diagnostics in metrics; info no longer exists. --- .../humanoid_velocity_tracking/env_design.md | 2 +- .../config_tuning.md | 2 +- .../quadruped_velocity_tracking/env_design.md | 2 +- .../whole_body_tracking/adding_wbt_task.md | 2 +- .../envs/whole_body_tracking/env_design.md | 4 +- .../envs/whole_body_tracking/index.md | 4 +- .../tutorial/building_envs/direct_env.md | 4 +- .../tutorial/building_envs/index.md | 2 +- .../tutorial/training/training_and_result.md | 2 +- .../humanoid_velocity_tracking/env_design.md | 2 +- .../config_tuning.md | 2 +- .../quadruped_velocity_tracking/env_design.md | 2 +- .../whole_body_tracking/adding_wbt_task.md | 2 +- .../envs/whole_body_tracking/env_design.md | 4 +- .../envs/whole_body_tracking/index.md | 2 +- .../tutorial/building_envs/direct_env.md | 5 +- .../tutorial/building_envs/index.md | 3 +- .../tutorial/training/training_and_result.md | 2 +- .../tests/test_training_parity.py | 4 +- .../src/motrix_env_core/array/env.py | 37 +- .../src/motrix_env_core/numba/manager/env.py | 8 +- .../tests/test_direct_env_sim_backend.py | 1 - motrix_env_core/tests/test_numba_manager.py | 4 +- motrix_env_core/tests/test_registry.py | 8 +- .../src/motrix_env_motrixsim/torch_env.py | 27 +- motrix_env_motrixsim/tests/test_torch_env.py | 9 +- .../motrix_envs/basic/acrobot/acrobot_np.py | 4 +- .../basic/bounce_ball/bounce_ball_np.py | 91 ++-- .../src/motrix_envs/basic/bounce_ball/cfg.py | 3 +- .../motrix_envs/basic/cartpole/cartpole_np.py | 3 +- .../motrix_envs/basic/cheetah/cheetah_np.py | 4 +- .../src/motrix_envs/basic/finger/finger_np.py | 58 +-- .../src/motrix_envs/basic/hopper/hopper_np.py | 11 +- .../motrix_envs/basic/humanoid/humanoid_np.py | 5 +- .../src/motrix_envs/basic/lqr/lqr_np.py | 6 +- .../basic/manipulator/manipulator_np.py | 36 +- .../motrix_envs/basic/pendulum/pendulum_np.py | 10 +- .../basic/point_mass/point_mass_np.py | 3 +- .../basic/quadruped/quadruped_np.py | 99 +---- .../motrix_envs/basic/reacher/reacher_np.py | 11 +- .../motrix_envs/basic/stewart/stewart_np.py | 341 +++++++-------- .../src/motrix_envs/basic/walker/walker_np.py | 16 +- .../locomotion/anymal_c/anymal_c_np.py | 65 ++- .../locomotion/go1/walk_stairs_terrain.py | 95 ++--- .../humanoid/walk_manager_mdp/command.py | 2 +- .../locomotion/quadruped/walk_np.py | 152 +++---- .../franka_lift_cube/franka_lift_cube_np.py | 43 +- .../franka_open_cabinet_np.py | 34 +- .../rm65_insert_peg/insert_peg_np.py | 191 +++++---- .../rm65_open_cabinet/rm65_open_cabinet_np.py | 391 +++++++----------- .../shadow_hand/shadow_hand_np.py | 74 ++-- motrix_envs/src/motrix_envs/motion/sampler.py | 8 +- .../tests/test_basic_direct_contract.py | 26 +- motrix_envs/tests/test_basic_env_lifecycle.py | 3 +- .../test_manipulation_direct_contract.py | 56 ++- motrix_envs/tests/test_quadruped_walk.py | 86 ++-- .../src/motrix_rl/deploy/source_rollout.py | 6 +- motrix_rl/src/motrix_rl/fastsac/wrap_np.py | 2 +- motrix_rl/src/motrix_rl/fastsac/wrap_torch.py | 2 +- .../src/motrix_rl/rslrl/torch/wrap_np.py | 5 +- .../src/motrix_rl/rslrl/torch/wrap_torch.py | 5 +- motrix_rl/src/motrix_rl/skrl/jax/wrap_np.py | 2 +- .../src/motrix_rl/skrl/jax/wrap_torch.py | 2 +- motrix_rl/src/motrix_rl/skrl/torch/wrap_np.py | 2 +- .../src/motrix_rl/skrl/torch/wrap_torch.py | 2 +- motrix_rl/src/motrix_rl/utils.py | 9 +- motrix_rl/tests/test_rl_sim_backend.py | 4 - 67 files changed, 928 insertions(+), 1186 deletions(-) diff --git a/docs/source/en/user_guide/envs/humanoid_velocity_tracking/env_design.md b/docs/source/en/user_guide/envs/humanoid_velocity_tracking/env_design.md index 314e32ac..c55fd7c5 100644 --- a/docs/source/en/user_guide/envs/humanoid_velocity_tracking/env_design.md +++ b/docs/source/en/user_guide/envs/humanoid_velocity_tracking/env_design.md @@ -73,7 +73,7 @@ sole every step, so clearance remains relative to the local surface. Each raw term is multiplied by its `RewardScales` weight and the control timestep; negative weights turn constraint measurements such as `penalty_*` and `pose` into penalties. Curriculum-selected penalties are also multiplied by the current `penalty_scale` according to completed episode length. This factor is exposed through -`info["metrics"]["penalty_scale"]`, and the final weighted terms through `info["Reward"]`. +`state.metrics["penalty_scale"]`, and the final weighted terms through `state.reward_terms`. ## Termination conditions diff --git a/docs/source/en/user_guide/envs/quadruped_velocity_tracking/config_tuning.md b/docs/source/en/user_guide/envs/quadruped_velocity_tracking/config_tuning.md index f5b156d7..468622ef 100644 --- a/docs/source/en/user_guide/envs/quadruped_velocity_tracking/config_tuning.md +++ b/docs/source/en/user_guide/envs/quadruped_velocity_tracking/config_tuning.md @@ -128,7 +128,7 @@ reward_config = RewardConfig( `base_height_target` and `initial_base_position[2]` should normally be close to the robot's default standing height. Choose `target_foot_height` according to leg length and terrain variation: too little encourages dragging, while too much -can require unreasonable joint motion. When tuning a reward weight, inspect the matching value in `info["Reward"]` +can require unreasonable joint motion. When tuning a reward weight, inspect the matching value in `state.reward_terms` rather than comparing configuration numbers alone. ## 7. `sensor`: sensor-name mapping diff --git a/docs/source/en/user_guide/envs/quadruped_velocity_tracking/env_design.md b/docs/source/en/user_guide/envs/quadruped_velocity_tracking/env_design.md index 5b8f0be8..94d3b71f 100644 --- a/docs/source/en/user_guide/envs/quadruped_velocity_tracking/env_design.md +++ b/docs/source/en/user_guide/envs/quadruped_velocity_tracking/env_design.md @@ -79,7 +79,7 @@ a body reference frame. Contact matching, swing clearance, and early swing-conta | `swing_contact` | Measure the fraction of feet still touching the ground during swing | Penalize dragging and early touchdown | Each raw term is first multiplied by its `RewardScales` weight; negative weights turn non-negative measurements into -penalties. `info["Reward"]` stores these weighted values before timestep scaling. Their sum is multiplied by `ctrl_dt` to +penalties. `state.reward_terms` stores these weighted values before timestep scaling. Their sum is multiplied by `ctrl_dt` to produce the reward returned to the training algorithm. The current environment has no reward curriculum. On rough terrain, `base_height` uses terrain height beneath the robot as its zero point. `swing_feet_z` consumes foot diff --git a/docs/source/en/user_guide/envs/whole_body_tracking/adding_wbt_task.md b/docs/source/en/user_guide/envs/whole_body_tracking/adding_wbt_task.md index ca8c3b59..3397eb2a 100644 --- a/docs/source/en/user_guide/envs/whole_body_tracking/adding_wbt_task.md +++ b/docs/source/en/user_guide/envs/whole_body_tracking/adding_wbt_task.md @@ -139,7 +139,7 @@ python scripts/train.py task=g1-wbt-dance1-subject1/motrix.fastsac \ ``` Check for missing motion, joint, or body names; systematic NaNs, joint-limit violations, or immediate bad-tracking after -reset; and verify that `info["Reward"]` and `info["metrics"]` reach the logs. Then start the default training run: +reset; and verify that `state.reward_terms` and `state.metrics` reach the logs. Then start the default training run: ```bash python scripts/train.py task=g1-wbt-dance1-subject1/motrix.fastsac algo.asynchronous=true diff --git a/docs/source/en/user_guide/envs/whole_body_tracking/env_design.md b/docs/source/en/user_guide/envs/whole_body_tracking/env_design.md index 19659f91..379749e3 100644 --- a/docs/source/en/user_guide/envs/whole_body_tracking/env_design.md +++ b/docs/source/en/user_guide/envs/whole_body_tracking/env_design.md @@ -81,7 +81,7 @@ This makes the relative body-configuration reward independent of the current hor | `undesired_contacts` | Count robot links whose net contact force exceeds the threshold and are absent from `allowed_contact_links` | Suppress body contacts not required by the motion | Each raw term is multiplied by its `WbtRewardScales` weight and then by `ctrl_dt`. Negative weights turn `action_rate_l2`, -`limits_dof_pos`, and `undesired_contacts` into penalties. Final weighted terms are written to `info["Reward"]`. +`limits_dof_pos`, and `undesired_contacts` into penalties. Final weighted terms are written to `state.reward_terms`. ## Termination conditions @@ -96,7 +96,7 @@ Each raw term is multiplied by its `WbtRewardScales` weight and then by `ctrl_dt | Motion final frame | Neither | `motion_steps` reaches the clip end | Resample and reset motion state during training; restart at frame 0 during play | `undesired_contacts` contributes only a reward penalty and does not terminate the episode. Termination rates and error means -are written to `info["metrics"]`. +are written to `state.metrics`. ## Reset logic diff --git a/docs/source/en/user_guide/envs/whole_body_tracking/index.md b/docs/source/en/user_guide/envs/whole_body_tracking/index.md index f65be639..31bdddd8 100644 --- a/docs/source/en/user_guide/envs/whole_body_tracking/index.md +++ b/docs/source/en/user_guide/envs/whole_body_tracking/index.md @@ -160,8 +160,8 @@ python scripts/train.py task=g1-wbt-dance/motrix.fastsac \ ``` During training, environments start at different points in the motion, and failure records increase the sampling probability -of difficult regions. Weighted reward terms are written to `info["Reward"]`; bad-tracking rates, motion progress, and -adaptive-sampling statistics are written to `info["metrics"]`. See [Task Environment Design](env_design.md) for definitions. +of difficult regions. Weighted reward terms are written to `state.reward_terms`; bad-tracking rates, motion progress, and +adaptive-sampling statistics are written to `state.metrics`. See [Task Environment Design](env_design.md) for definitions. ### Play the policy diff --git a/docs/source/en/user_guide/tutorial/building_envs/direct_env.md b/docs/source/en/user_guide/tutorial/building_envs/direct_env.md index d80c7868..26cc94b7 100644 --- a/docs/source/en/user_guide/tutorial/building_envs/direct_env.md +++ b/docs/source/en/user_guide/tutorial/building_envs/direct_env.md @@ -146,7 +146,7 @@ Hooks a subclass implements: | Hook | Responsibility | | ------------------------------------------- | -------------------------------------------------------------------------------------- | -| `reset(env_ids)` | Write reset state (randomized initial poses, ...) for the selected rows and return an info dict; observations are produced afterwards by `compute_observation` | +| `reset(env_ids)` | Write reset state (randomized initial poses, ...) for the selected rows; observations are produced afterwards by `compute_observation` | | `apply_action(actions, state)` | Write the action into the simulator (usually ctrl targets) | | `compute_transition(state)` | Execute the read program and derive `state.reward`, `state.terminated`, ...; this is the only full data refresh of a step and must **not** write `state.obs` | | `compute_observation(state)` | Assemble `state.obs` purely from already refreshed simulator data, without further reads | @@ -156,7 +156,7 @@ Semantics: - `terminated` marks episode-ending conditions such as task failure; `truncated` marks the time limit at `max_episode_steps`. `ArrayEnv` combines both into `done` and - triggers auto-reset; `info["time_outs"]` flags rows that timed out without failing. + triggers auto-reset. - The environment dimension must use vectorized NumPy operations; plain loops are only allowed over a fixed number of joints, feet, or terms. - Constants (initial poses, space definitions, query names) are precomputed in diff --git a/docs/source/en/user_guide/tutorial/building_envs/index.md b/docs/source/en/user_guide/tutorial/building_envs/index.md index cb38035c..19638c73 100644 --- a/docs/source/en/user_guide/tutorial/building_envs/index.md +++ b/docs/source/en/user_guide/tutorial/building_envs/index.md @@ -34,7 +34,7 @@ Every stage is orchestrated by the `ArrayEnv` base class; environment implementa hooks and must not re-implement the lifecycle. Semantics: - `terminated` marks episode-ending conditions such as task failure; `truncated` marks - the time limit, and `info["time_outs"]` flags rows that timed out without failing; + the time limit; - environments that are done are reset automatically at the end of each step, and observations are recomputed after the reset. diff --git a/docs/source/en/user_guide/tutorial/training/training_and_result.md b/docs/source/en/user_guide/tutorial/training/training_and_result.md index b743457d..383186bc 100644 --- a/docs/source/en/user_guide/tutorial/training/training_and_result.md +++ b/docs/source/en/user_guide/tutorial/training/training_and_result.md @@ -95,7 +95,7 @@ TensorBoard logs are written under the run directory and can be viewed per envir tensorboard --logdir runs/cartpole ``` -Besides the standard return and loss curves, if an environment exposes per-term rewards via `info["Reward"]`, they are also logged to TensorBoard during training. +Besides the standard return and loss curves, if an environment exposes per-term rewards via `state.reward_terms`, they are also logged to TensorBoard during training. ## Model Evaluation and Testing diff --git a/docs/source/zh_CN/user_guide/envs/humanoid_velocity_tracking/env_design.md b/docs/source/zh_CN/user_guide/envs/humanoid_velocity_tracking/env_design.md index 93f91718..6b09a496 100644 --- a/docs/source/zh_CN/user_guide/envs/humanoid_velocity_tracking/env_design.md +++ b/docs/source/zh_CN/user_guide/envs/humanoid_velocity_tracking/env_design.md @@ -66,7 +66,7 @@ $$ 每个原始项先乘以 `RewardScales` 中的权重,再乘以控制步长;`penalty_*` 和 `pose` 等约束项通过负权重成为惩罚。 课程指定的惩罚项还会根据已结束回合的平均长度乘以当前 `penalty_scale`。该缩放记录在 -`info["metrics"]["penalty_scale"]`,最终的各加权项记录在 `info["Reward"]`。 +`state.metrics["penalty_scale"]`,最终的各加权项记录在 `state.reward_terms`。 ## 终止条件 diff --git a/docs/source/zh_CN/user_guide/envs/quadruped_velocity_tracking/config_tuning.md b/docs/source/zh_CN/user_guide/envs/quadruped_velocity_tracking/config_tuning.md index 56b92e3b..25d2513e 100644 --- a/docs/source/zh_CN/user_guide/envs/quadruped_velocity_tracking/config_tuning.md +++ b/docs/source/zh_CN/user_guide/envs/quadruped_velocity_tracking/config_tuning.md @@ -122,7 +122,7 @@ reward_config = RewardConfig( `base_height_target` 和 `initial_base_position[2]` 通常应接近机器人的默认站立高度。`target_foot_height` 应结合 腿长和地形起伏设置;过低容易拖脚,过高可能要求超出合理关节范围。调节某个奖励权重时,应查看 -`info["Reward"]` 中对应项的量级,而不只比较配置数值。 +`state.reward_terms` 中对应项的量级,而不只比较配置数值。 ## 7. `sensor`:传感器名称映射 diff --git a/docs/source/zh_CN/user_guide/envs/quadruped_velocity_tracking/env_design.md b/docs/source/zh_CN/user_guide/envs/quadruped_velocity_tracking/env_design.md index 9a84b40a..9665b526 100644 --- a/docs/source/zh_CN/user_guide/envs/quadruped_velocity_tracking/env_design.md +++ b/docs/source/zh_CN/user_guide/envs/quadruped_velocity_tracking/env_design.md @@ -73,7 +73,7 @@ Actor 与 Critic 共享的字段使用同一份带噪值;当前实现没有额 | `swing_feet_z` | 在摆动且未接触时,对足端高度误差应用指数核并按脚数平均 | 鼓励摆动脚达到目标离地高度 | | `swing_contact` | 计算摆动期仍接触地面的脚所占比例 | 惩罚拖脚和过早落足 | -每个原始项先乘以 `RewardScales` 中的权重;负权重将非负度量转为惩罚。`info["Reward"]` 保存这些尚未 +每个原始项先乘以 `RewardScales` 中的权重;负权重将非负度量转为惩罚。`state.reward_terms` 保存这些尚未 乘控制步长的加权项,所有项求和后再乘 `ctrl_dt`,得到返回给训练算法的单步奖励。当前环境没有奖励课程。 粗糙地形上,`base_height` 使用机器人当前位置的地形高度作为零点。`swing_feet_z` 的足端位置来自机体参考系, diff --git a/docs/source/zh_CN/user_guide/envs/whole_body_tracking/adding_wbt_task.md b/docs/source/zh_CN/user_guide/envs/whole_body_tracking/adding_wbt_task.md index e7e36614..ca37d889 100644 --- a/docs/source/zh_CN/user_guide/envs/whole_body_tracking/adding_wbt_task.md +++ b/docs/source/zh_CN/user_guide/envs/whole_body_tracking/adding_wbt_task.md @@ -136,7 +136,7 @@ python scripts/train.py task=g1-wbt-dance1-subject1/motrix.fastsac \ ``` 检查 motion/joint/body 名称没有缺失,reset 后没有系统性 NaN、joint limit 违规或立即 bad-tracking,并确认 -`info["Reward"]` 与 `info["metrics"]` 能进入日志。随后使用默认规模训练: +`state.reward_terms` 与 `state.metrics` 能进入日志。随后使用默认规模训练: ```bash python scripts/train.py task=g1-wbt-dance1-subject1/motrix.fastsac algo.asynchronous=true diff --git a/docs/source/zh_CN/user_guide/envs/whole_body_tracking/env_design.md b/docs/source/zh_CN/user_guide/envs/whole_body_tracking/env_design.md index 8d106324..24c09d61 100644 --- a/docs/source/zh_CN/user_guide/envs/whole_body_tracking/env_design.md +++ b/docs/source/zh_CN/user_guide/envs/whole_body_tracking/env_design.md @@ -77,7 +77,7 @@ Actor 的参考身体姿态、基座角速度、关节位置和关节速度会 | `undesired_contacts` | 统计净接触力超过阈值且不在 `allowed_contact_links` 中的机器人 links | 抑制动作不需要的身体接触 | 每个原始项先乘以 `WbtRewardScales` 中的权重,再乘以 `ctrl_dt`;`action_rate_l2`、`limits_dof_pos` 和 -`undesired_contacts` 通过负权重成为惩罚。最终加权项写入 `info["Reward"]`。 +`undesired_contacts` 通过负权重成为惩罚。最终加权项写入 `state.reward_terms`。 ## 终止条件 @@ -91,7 +91,7 @@ Actor 的参考身体姿态、基座角速度、关节位置和关节速度会 | 时间上限 | `truncated` | 训练回合达到 `max_episode_seconds`,内置配置为 10 s | 正常达到训练时限,不表示 bad tracking | | Motion 末帧 | 两者都不是 | `motion_steps` 到达 clip 末尾 | 训练时重采样起始帧并重置 motion 状态;play 时从第 0 帧重播 | -`undesired_contacts` 只产生奖励惩罚,不直接终止回合。各类终止比例与误差均值记录在 `info["metrics"]`。 +`undesired_contacts` 只产生奖励惩罚,不直接终止回合。各类终止比例与误差均值记录在 `state.metrics`。 ## 重置逻辑 diff --git a/docs/source/zh_CN/user_guide/envs/whole_body_tracking/index.md b/docs/source/zh_CN/user_guide/envs/whole_body_tracking/index.md index 69be464f..77db7939 100644 --- a/docs/source/zh_CN/user_guide/envs/whole_body_tracking/index.md +++ b/docs/source/zh_CN/user_guide/envs/whole_body_tracking/index.md @@ -154,7 +154,7 @@ python scripts/train.py task=g1-wbt-dance/motrix.fastsac \ ``` 训练期间,环境从 motion 的不同时间点开始,并利用失败记录提高困难片段的采样概率。奖励分项写入 -`info["Reward"]`,bad-tracking 比例、motion 进度和自适应采样统计写入 `info["metrics"]`;各项定义见 +`state.reward_terms`,bad-tracking 比例、motion 进度和自适应采样统计写入 `state.metrics`;各项定义见 [任务环境设计](env_design.md)。 ### 回放策略 diff --git a/docs/source/zh_CN/user_guide/tutorial/building_envs/direct_env.md b/docs/source/zh_CN/user_guide/tutorial/building_envs/direct_env.md index 42f3c0de..0eb1d64e 100644 --- a/docs/source/zh_CN/user_guide/tutorial/building_envs/direct_env.md +++ b/docs/source/zh_CN/user_guide/tutorial/building_envs/direct_env.md @@ -139,7 +139,7 @@ class CartPoleEnv(DirectEnv): | 钩子 | 职责 | | ------------------------------------ | ---------------------------------------------------------------------------------------- | -| `reset(env_ids)` | 为选中的环境写入重置状态(随机化初始姿态等),返回 info 字典;观测交给后续的 `compute_observation` | +| `reset(env_ids)` | 为选中的环境写入重置状态(随机化初始姿态等);观测交给后续的 `compute_observation` | | `apply_action(actions, state)` | 将动作写入仿真(通常是 ctrl 目标) | | `compute_transition(state)` | 执行读取程序并派生 `state.reward`、`state.terminated` 等;这是每步唯一的全量数据刷新点,**不得**写 `state.obs` | | `compute_observation(state)` | 纯粹用已刷新的仿真数据拼装 `state.obs`,不再执行读取 | @@ -148,8 +148,7 @@ class CartPoleEnv(DirectEnv): 语义约定: - `terminated` 表示任务失败等回合终止条件;`truncated` 表示达到 `max_episode_steps` - 的时间截断。两者由 `ArrayEnv` 合成 `done` 并触发 auto-reset; - `info["time_outs"]` 标记"截断但未失败"的行。 + 的时间截断。两者由 `ArrayEnv` 合成 `done` 并触发 auto-reset。 - 环境维度必须使用 NumPy 向量化操作;只有遍历固定数量的关节、脚或 term 时才允许普通循环。 - 常量(初始姿态、空间定义、query 名称)在 `__init__` 或配置构造阶段预计算, 不在 step 循环中重复创建。 diff --git a/docs/source/zh_CN/user_guide/tutorial/building_envs/index.md b/docs/source/zh_CN/user_guide/tutorial/building_envs/index.md index 37690384..36d72620 100644 --- a/docs/source/zh_CN/user_guide/tutorial/building_envs/index.md +++ b/docs/source/zh_CN/user_guide/tutorial/building_envs/index.md @@ -28,8 +28,7 @@ 所有阶段都由 `ArrayEnv` 基类编排,环境实现只填充钩子,不要重复实现生命周期。语义约定: -- `terminated` 是任务失败等回合终止条件;`truncated` 是达到回合时长上限的时间截断, - `info["time_outs"]` 标记"截断但未失败"的行; +- `terminated` 是任务失败等回合终止条件;`truncated` 是达到回合时长上限的时间截断; - done 的环境在每步末尾被自动重置,观测在重置之后重新计算。 各阶段在两种工作流中分别由谁实现:DirectEnv 在 diff --git a/docs/source/zh_CN/user_guide/tutorial/training/training_and_result.md b/docs/source/zh_CN/user_guide/tutorial/training/training_and_result.md index dd611ac7..70aee9d5 100644 --- a/docs/source/zh_CN/user_guide/tutorial/training/training_and_result.md +++ b/docs/source/zh_CN/user_guide/tutorial/training/training_and_result.md @@ -95,7 +95,7 @@ TensorBoard 日志写在 run 目录下,可按环境查看: tensorboard --logdir runs/cartpole ``` -除标准的回报、损失曲线外,若环境通过 `info["Reward"]` 暴露了各 reward 分项,训练时也会将其记录到 TensorBoard。 +除标准的回报、损失曲线外,若环境通过 `state.reward_terms` 暴露了各 reward 分项,训练时也会将其记录到 TensorBoard。 ## 模型评估和测试 diff --git a/motrix_deploy_tasks/tests/test_training_parity.py b/motrix_deploy_tasks/tests/test_training_parity.py index 5b182558..cfe0ab5b 100644 --- a/motrix_deploy_tasks/tests/test_training_parity.py +++ b/motrix_deploy_tasks/tests/test_training_parity.py @@ -27,7 +27,7 @@ def test_go2_training_and_deployment_task_golden_probe(env_name: str) -> None: context = PolicyContext( step=0, elapsed_time_s=0.0, - command=PlanarVelocityCommand(env_state.info["commands"]), + command=PlanarVelocityCommand(env._commands), ) task = create_task(profile.task, profile.robot) command_scale = np.asarray(profile.task.config["command_scale"], dtype=np.float32) @@ -53,7 +53,7 @@ def test_go2_training_and_deployment_task_golden_probe(env_name: str) -> None: next_context = PolicyContext( step=1, elapsed_time_s=profile.control.period_s, - command=PlanarVelocityCommand(stepped.info["commands"]), + command=PlanarVelocityCommand(env._commands), ) np.testing.assert_allclose( task.build_observation(next_state, next_context), diff --git a/motrix_env_core/src/motrix_env_core/array/env.py b/motrix_env_core/src/motrix_env_core/array/env.py index 8f81e3ac..0e0697d9 100644 --- a/motrix_env_core/src/motrix_env_core/array/env.py +++ b/motrix_env_core/src/motrix_env_core/array/env.py @@ -40,7 +40,10 @@ class ArrayEnvState: terminated: np.ndarray truncated: np.ndarray episode_steps: np.ndarray - info: dict + # Per-term reward breakdown for the latest transition: term name to a + # ``(num_envs,)`` array. Freshly written every transition; empty when the + # environment does not decompose its reward. + reward_terms: dict[str, np.ndarray] = dataclasses.field(default_factory=dict) # Live diagnostics view: per-environment quantities stay unreduced as # ``(num_envs,)`` arrays and are views into manager buffers that kernels # overwrite every step; batch-level gauges are scalars. Values always @@ -150,8 +153,7 @@ def init_state(self) -> ArrayEnvState: terminated = np.ones((self._num_envs,), dtype=bool) truncated = np.zeros((self._num_envs,), dtype=bool) episode_steps = np.zeros((self._num_envs,), dtype=np.uint64) - info = {"time_outs": np.zeros((self._num_envs,), dtype=bool)} - self._state = self._new_state(obs, reward, terminated, truncated, episode_steps, info) + self._state = self._new_state(obs, reward, terminated, truncated, episode_steps) self._reset_done_envs() with self.perf.scope("observation"): self._state = self.compute_observation(self._state) @@ -166,7 +168,6 @@ def _new_state( terminated: np.ndarray, truncated: np.ndarray, episode_steps: np.ndarray, - info: dict, ) -> ArrayEnvState: """Assemble the environment state; subclasses add simulator-owned fields.""" return ArrayEnvState( @@ -175,7 +176,6 @@ def _new_state( terminated=terminated, truncated=truncated, episode_steps=episode_steps, - info=info, ) @property @@ -223,31 +223,11 @@ def _reset_done_envs(self) -> None: np.putmask(state.episode_steps, done, 0) env_ids = np.flatnonzero(done) with self.perf.scope("reset_envs"): - info1 = self.reset(env_ids) - self._merge_reset_info(state, info1, done) - - def _merge_reset_info(self, state: ArrayEnvState, info1: dict, done: np.ndarray) -> None: - """Merge one selected-row reset's info entries into the state info.""" - if not info1: - return - - def replace_dict_values(dst, new_values, mask): - for key, value in new_values.items(): - if key not in dst: - dst[key] = value - else: - if isinstance(value, np.ndarray): - dst[key][mask] = value - elif isinstance(value, dict): - assert isinstance(dst[key], dict) - replace_dict_values(dst[key], value, mask) - - with self.perf.scope("merge_info"): - replace_dict_values(state.info, info1, done) + self.reset(env_ids) @abc.abstractmethod - def reset(self, env_ids: np.ndarray) -> dict: - """Write reset rows for the selected environments and return reset info. + def reset(self, env_ids: np.ndarray) -> None: + """Write reset rows for the selected environments. Subclasses address their simulator's rows themselves. Observation generation is deferred until :meth:`compute_observation`, after reset @@ -265,7 +245,6 @@ def _update_truncate(self): if not max_episode_steps: return self._state.truncated = self._state.episode_steps >= max_episode_steps - self._state.info["time_outs"] = self._state.truncated & ~self._state.terminated @abc.abstractmethod def create_renderer(self, config: RenderConfig) -> SimRenderer: diff --git a/motrix_env_core/src/motrix_env_core/numba/manager/env.py b/motrix_env_core/src/motrix_env_core/numba/manager/env.py index 96c7556f..6ab8d216 100644 --- a/motrix_env_core/src/motrix_env_core/numba/manager/env.py +++ b/motrix_env_core/src/motrix_env_core/numba/manager/env.py @@ -685,10 +685,9 @@ def _reset_done_envs(self) -> None: with self.perf.scope("select_done"): np.putmask(state.episode_steps, done, 0) with self.perf.scope("reset_envs"): - info1 = self.reset(env_ids, sim_reset_ids) - self._merge_reset_info(state, info1, done) + self.reset(env_ids, sim_reset_ids) - def reset(self, env_ids: np.ndarray, sim_reset_ids: np.ndarray | None = None) -> dict[str, Any]: + def reset(self, env_ids: np.ndarray, sim_reset_ids: np.ndarray | None = None) -> None: """Reset episode lanes fully and sim-reset the rest in one pass. Episode lanes (``env_ids``) run the host lifecycle resets (command @@ -717,7 +716,6 @@ def reset(self, env_ids: np.ndarray, sim_reset_ids: np.ndarray | None = None) -> env_ids = np.concatenate([env_ids, sim_reset_ids]) if env_ids.size: self._reset_sim_rows(env_ids, self._kernel_inputs) - return {} def _make_metrics_view(self) -> dict[str, Any]: """Assemble the persistent live metrics view for the current state. @@ -815,7 +813,7 @@ def _make_kernel_buffers(self, state: ArrayEnvState) -> tuple[np.ndarray, ...]: reward_terms = np.empty((self.num_envs, len(layout.rewards)), dtype=np.float32) weighted_reward_terms = np.empty_like(reward_terms) termination_masks = np.empty((self.num_envs, len(layout.terminations)), dtype=bool) - state.info["Reward"] = {term.name: weighted_reward_terms[:, term.index] for term in layout.rewards} + state.reward_terms = {term.name: weighted_reward_terms[:, term.index] for term in layout.rewards} buffers = ( reward_terms, weighted_reward_terms, diff --git a/motrix_env_core/tests/test_direct_env_sim_backend.py b/motrix_env_core/tests/test_direct_env_sim_backend.py index c7df8723..c7d431da 100644 --- a/motrix_env_core/tests/test_direct_env_sim_backend.py +++ b/motrix_env_core/tests/test_direct_env_sim_backend.py @@ -224,7 +224,6 @@ def reset(self, env_ids: np.ndarray): self._reset_program.buffer("state_velocity")[env_ids] = dof_vel self._reset_program.execute(env_ids) self.sim_data.execute(env_ids) - return {} def _make_env(num_envs: int = 3) -> _FakeDirectEnv: diff --git a/motrix_env_core/tests/test_numba_manager.py b/motrix_env_core/tests/test_numba_manager.py index 0b948ffc..4df2b0d5 100644 --- a/motrix_env_core/tests/test_numba_manager.py +++ b/motrix_env_core/tests/test_numba_manager.py @@ -608,7 +608,7 @@ def test_step_perf_records_standard_and_numba_manager_phases() -> None: env = _ManagerEnv() state = env.init_state() assert state.episode_steps.shape == (env.num_envs,) - assert "steps" not in state.info + assert set(state.reward_terms) == {"source"} actions = np.zeros((env.num_envs, 1), dtype=np.float32) env.step(actions) @@ -686,7 +686,7 @@ def test_manager_context_is_injected_once_and_reused_across_all_term_kinds() -> np.testing.assert_allclose(env.metrics["source_at_termination"][:, 0], [0.25, 0.75]) np.testing.assert_array_equal(state.metrics["limit"], [False, True]) np.testing.assert_allclose(np.ravel(state.metrics["double"]), [0.5, 1.5]) - np.testing.assert_allclose(state.info["Reward"]["source"], [0.005, 0.015]) + np.testing.assert_allclose(state.reward_terms["source"], [0.005, 0.015]) np.testing.assert_array_equal(state.metrics["limit"], [False, True]) assert env._compiled_manager_program is not None assert sum(source.count("ctx =") for source in env._compiled_manager_program.sources) == 3 diff --git a/motrix_env_core/tests/test_registry.py b/motrix_env_core/tests/test_registry.py index 6fe78998..ae61ff3e 100644 --- a/motrix_env_core/tests/test_registry.py +++ b/motrix_env_core/tests/test_registry.py @@ -188,8 +188,8 @@ def apply_action(self, actions: np.ndarray, state: ArrayEnvState) -> ArrayEnvSta def compute_transition(self, state: ArrayEnvState) -> ArrayEnvState: return state - def reset(self, env_ids: np.ndarray): - return {} + def reset(self, env_ids: np.ndarray) -> None: + pass env = registry.make(env_name, num_envs=2, sim="fake-registry") @@ -247,8 +247,8 @@ def apply_action(self, actions: np.ndarray, state: ArrayEnvState) -> ArrayEnvSta def compute_transition(self, state: ArrayEnvState) -> ArrayEnvState: return state - def reset(self, env_ids: np.ndarray): - return {} + def reset(self, env_ids: np.ndarray) -> None: + pass env = registry.make(env_name, num_envs=2, sim="fake-registry") diff --git a/motrix_env_motrixsim/src/motrix_env_motrixsim/torch_env.py b/motrix_env_motrixsim/src/motrix_env_motrixsim/torch_env.py index 1cf70bfd..29094a1e 100644 --- a/motrix_env_motrixsim/src/motrix_env_motrixsim/torch_env.py +++ b/motrix_env_motrixsim/src/motrix_env_motrixsim/torch_env.py @@ -43,7 +43,10 @@ class TorchEnvState: terminated: torch.Tensor truncated: torch.Tensor episode_steps: torch.Tensor - info: dict + # Per-term reward breakdown for the latest transition: term name to a + # ``(num_envs,)`` tensor. Empty when the environment does not decompose + # its reward. + reward_terms: dict[str, torch.Tensor] = dataclasses.field(default_factory=dict) # Instantaneous diagnostics snapshot: scalar per key, reduced across all # environments. Recomputed every step by ``update_state``; resets do not # clear or recompute it. @@ -187,9 +190,8 @@ def init_state(self) -> TorchEnvState: terminated = torch.ones((self._num_envs,), dtype=torch.bool, device=self._device) truncated = torch.zeros((self._num_envs,), dtype=torch.bool, device=self._device) episode_steps = torch.zeros((self._num_envs,), dtype=torch.int64, device=self._device) - info = {"time_outs": torch.zeros((self._num_envs,), dtype=torch.bool, device=self._device)} data = mtx.SceneData(self._model, batch=[self._num_envs]) - self._state = TorchEnvState(data, obs, reward, terminated, truncated, episode_steps, info) + self._state = TorchEnvState(data, obs, reward, terminated, truncated, episode_steps) self._reset_done_envs() self._state.validate(self._device, self._num_envs) return self._state @@ -224,16 +226,6 @@ def _assign_obs(self, dst: TorchObs, mask: torch.Tensor, src: TorchObs | torch.T else: assert src.value is None - def _replace_info_values(self, dst: dict, new_values: dict, mask: torch.Tensor) -> None: - for key, value in new_values.items(): - if key not in dst: - dst[key] = value - elif isinstance(value, torch.Tensor): - dst[key][mask] = value - elif isinstance(value, dict): - assert isinstance(dst[key], dict) - self._replace_info_values(dst[key], value, mask) - def _reset_done_envs(self) -> None: assert self._state is not None state = self._state @@ -244,10 +236,8 @@ def _reset_done_envs(self) -> None: state.episode_steps[done] = 0 data = state.data[done.detach().cpu().numpy()] - obs, info = self.reset(data) + obs = self.reset(data) self._assign_obs(state.obs, done, obs) - if info: - self._replace_info_values(state.info, info, done) def _max_episode_steps(self) -> int | None: return self._cfg.max_episode_steps @@ -261,7 +251,6 @@ def _update_truncate(self): if not max_episode_steps: return self._state.truncated = self._state.episode_steps >= max_episode_steps - self._state.info["time_outs"] = self._state.truncated & ~self._state.terminated @abc.abstractmethod def apply_action(self, actions: torch.Tensor, state: TorchEnvState) -> TorchEnvState: @@ -286,7 +275,7 @@ def update_state(self, state: TorchEnvState) -> TorchEnvState: def reset( self, data: mtx.SceneData, - ) -> tuple[TorchObs | torch.Tensor, dict]: + ) -> TorchObs | torch.Tensor: """ Reset the environment for the done envs @@ -294,7 +283,7 @@ def reset( data (mtx.SceneData): The scene data to reset Returns: - tuple[torch.Tensor, dict]: The initial observations and info after reset + TorchObs | torch.Tensor: The initial observations after reset """ pass diff --git a/motrix_env_motrixsim/tests/test_torch_env.py b/motrix_env_motrixsim/tests/test_torch_env.py index daef6071..9fd95c2a 100644 --- a/motrix_env_motrixsim/tests/test_torch_env.py +++ b/motrix_env_motrixsim/tests/test_torch_env.py @@ -23,7 +23,6 @@ def test_torch_environment_state_matches_numpy_state_contract() -> None: terminated=torch.tensor([False, True]), truncated=torch.tensor([True, False]), episode_steps=torch.zeros(2, dtype=torch.int64), - info={}, ) state.validate() @@ -65,8 +64,8 @@ def update_state(self, state: TorchEnvState) -> TorchEnvState: terminated=self._actions[:, 0] > 0.5, ) - def reset(self, data) -> tuple[torch.Tensor, dict]: - return torch.full((*data.shape, 1), -1.0, dtype=torch.float64, device=self.device), {} + def reset(self, data) -> torch.Tensor: + return torch.full((*data.shape, 1), -1.0, dtype=torch.float64, device=self.device) def test_torch_environment_rejects_gpu_until_gpu_simulation_is_available() -> None: @@ -87,13 +86,12 @@ def test_np_simulation_places_torch_environment_lifecycle_on_cpu() -> None: assert device == torch.device("cpu") assert initial.obs.policy.dtype == torch.float64 assert initial.obs.policy.device == device - assert "steps" not in initial.info + assert initial.reward_terms == {} terminated = env.step(torch.tensor([[1.0], [0.25]], dtype=torch.float32, device=device)) torch.testing.assert_close(terminated.reward, torch.tensor([1.0, 0.25], device=device)) torch.testing.assert_close(terminated.terminated, torch.tensor([True, False], device=device)) torch.testing.assert_close(terminated.truncated, torch.tensor([False, False], device=device)) - torch.testing.assert_close(terminated.info["time_outs"], torch.tensor([False, False], device=device)) torch.testing.assert_close(terminated.episode_steps, torch.tensor([0, 1], device=device)) torch.testing.assert_close( terminated.obs.policy[:, 0], @@ -103,7 +101,6 @@ def test_np_simulation_places_torch_environment_lifecycle_on_cpu() -> None: truncated = env.step(torch.tensor([[0.2], [0.2]], dtype=torch.float32, device=device)) torch.testing.assert_close(truncated.terminated, torch.tensor([False, False], device=device)) torch.testing.assert_close(truncated.truncated, torch.tensor([False, True], device=device)) - torch.testing.assert_close(truncated.info["time_outs"], torch.tensor([False, True], device=device)) torch.testing.assert_close(truncated.episode_steps, torch.tensor([1, 0], device=device)) torch.testing.assert_close( truncated.obs.policy[:, 0], diff --git a/motrix_envs/src/motrix_envs/basic/acrobot/acrobot_np.py b/motrix_envs/src/motrix_envs/basic/acrobot/acrobot_np.py index f2b7ee82..880a14d1 100644 --- a/motrix_envs/src/motrix_envs/basic/acrobot/acrobot_np.py +++ b/motrix_envs/src/motrix_envs/basic/acrobot/acrobot_np.py @@ -135,7 +135,7 @@ def compute_transition(self, state: ArrayEnvState) -> ArrayEnvState: state.terminated = terminated return state - def reset(self, env_ids: np.ndarray) -> dict: + def reset(self, env_ids: np.ndarray) -> None: num_reset = len(env_ids) shoulder_angle = np.random.uniform(-np.pi, np.pi, size=num_reset).astype(np.float32) @@ -149,8 +149,6 @@ def reset(self, env_ids: np.ndarray) -> dict: self._reset_program.execute(env_ids) self.sim_data.execute(np.asarray(env_ids, np.int64)) - return {} - def _reset_done_envs(self): """ Reset the environments that are done diff --git a/motrix_envs/src/motrix_envs/basic/bounce_ball/bounce_ball_np.py b/motrix_envs/src/motrix_envs/basic/bounce_ball/bounce_ball_np.py index 8dba25c1..c6ed0e89 100644 --- a/motrix_envs/src/motrix_envs/basic/bounce_ball/bounce_ball_np.py +++ b/motrix_envs/src/motrix_envs/basic/bounce_ball/bounce_ball_np.py @@ -66,6 +66,15 @@ def __init__(self, cfg: BounceBallEnvCfg, num_envs=1, backend: str | None = None # Action space: 6D joint position control self._action_space = gym.spaces.Box(-1.0, 1.0, (6,), dtype=np.float32) + # Episode-scoped task state: full-batch buffers, reset writes the + # done rows in place. + num_envs = self._num_envs + self._ball_was_upward = np.zeros(num_envs, dtype=bool) + self._consecutive_bounces = np.zeros(num_envs, dtype=np.int32) + self._target_heights = np.zeros(num_envs, dtype=np.float32) + self._current_actions = np.zeros((num_envs, 6), dtype=np.float32) + self._last_actions = np.zeros((num_envs, 6), dtype=np.float32) + # Observation space: joint states + paddle position + target height (29D) self._observation_space = gym.spaces.Box(-np.inf, np.inf, (29,), dtype=np.float32) @@ -126,11 +135,11 @@ def _compute_reward( dof_pos: np.ndarray, dof_vel: np.ndarray, paddle_pos: np.ndarray, - consecutive_bounces: np.ndarray = None, - bounce_detected: np.ndarray = None, - target_heights: np.ndarray = None, - current_actions: np.ndarray = None, - last_actions: np.ndarray = None, + consecutive_bounces: np.ndarray, + bounce_detected: np.ndarray, + target_heights: np.ndarray, + current_actions: np.ndarray, + last_actions: np.ndarray, ) -> tuple: """ Compute reward based on ball position, velocity, and paddle alignment. @@ -307,9 +316,6 @@ def _compute_reward( # Encourages paddle to actively move directly below ball # Extra reward at bounce moment to reinforce correct hitting behavior # ============================================================================ - if bounce_detected is None: - bounce_detected = np.zeros(ball_x.shape[0], dtype=bool) - ball_xy = np.stack([ball_x, ball_y], axis=1) paddle_ball_xy_error = np.linalg.norm(ball_xy - paddle_xy, axis=1) @@ -349,12 +355,6 @@ def _compute_reward( # Penalizes drastic action changes and excessive joint velocities # Encourages smooth and energy-efficient control # ============================================================================ - num_envs = dof_pos.shape[0] - if current_actions is None: - current_actions = np.zeros((num_envs, 6), dtype=np.float32) - if last_actions is None: - last_actions = np.zeros((num_envs, 6), dtype=np.float32) - action_diff = current_actions - last_actions action_penalty = np.sum(np.square(action_diff), axis=-1) @@ -457,9 +457,9 @@ def _compute_terminated(self, dof_pos: np.ndarray, dof_vel: np.ndarray, target_h def apply_action(self, actions: np.ndarray, state: ArrayEnvState) -> ArrayEnvState: """Apply action to control paddle position""" - # Store last actions for penalty calculation - state.info["last_actions"] = state.info.get("current_actions", np.zeros_like(actions)) - state.info["current_actions"] = actions + # Save last actions for penalty calculation + self._last_actions[:] = self._current_actions + self._current_actions[:] = actions # Get current joint positions current_joint_pos = self.sim_data["dof_pos"][:, :6] # First 6 DOFs are arm joints @@ -478,13 +478,10 @@ def apply_action(self, actions: np.ndarray, state: ArrayEnvState) -> ArrayEnvSta def compute_observation(self, state: ArrayEnvState) -> ArrayEnvState: inputs = self.sim_data - target_heights = state.info.get("target_heights") - if target_heights is None: - target_heights = np.full(self._num_envs, np.mean(self._cfg.target_height_range), dtype=np.float32) # Observation: joint states + paddle position + target height (29D) # Concatenate: DOF pos (13) + DOF vel (12) + paddle xyz (3) + target height (1) obs = np.concatenate( - [inputs["dof_pos"], inputs["dof_vel"], inputs["paddle_pos"], target_heights[:, np.newaxis]], axis=-1 + [inputs["dof_pos"], inputs["dof_vel"], inputs["paddle_pos"], self._target_heights[:, np.newaxis]], axis=-1 ) return state.replace(obs=obs.astype(np.float32)) @@ -492,14 +489,11 @@ def compute_transition(self, state: ArrayEnvState) -> ArrayEnvState: """Update state with rewards and termination flags""" self.sim_data.execute() inputs = self.sim_data - num_envs = self._num_envs - # Get bounce tracking and target heights from info - consecutive_bounces = state.info.get("consecutive_bounces", np.zeros(num_envs, dtype=np.int32)) - ball_was_upward = state.info.get("ball_was_upward", np.zeros(num_envs, dtype=bool)) - # Use mean of target_height_range as fallback - default_height = np.mean(self._cfg.target_height_range) - target_heights = state.info.get("target_heights", np.full(num_envs, default_height, dtype=np.float32)) + # Episode-scoped bounce tracking and target heights live in env buffers. + consecutive_bounces = self._consecutive_bounces + ball_was_upward = self._ball_was_upward + target_heights = self._target_heights # Detect bounces and update consecutive bounce count. # The ball's free joint contributes (x, y, z) at dof 6:9. @@ -518,18 +512,9 @@ def compute_transition(self, state: ArrayEnvState) -> ArrayEnvState: # Reset count if ball is falling too much (not bouncing properly) falling = (current_ball_vz < -0.5) & (current_ball_z < 0.4) - consecutive_bounces = np.where(falling, 0, consecutive_bounces) - - # Update tracking variables in info - state.info["consecutive_bounces"] = consecutive_bounces - state.info["ball_was_upward"] = moving_upward - - # Track maximum bounces achieved - max_current = np.max(consecutive_bounces) - if "max_consecutive_bounces" not in state.info: - state.info["max_consecutive_bounces"] = 0 - if max_current > state.info["max_consecutive_bounces"]: - state.info["max_consecutive_bounces"] = max_current + consecutive_bounces[falling] = 0 + self._consecutive_bounces = consecutive_bounces + self._ball_was_upward = moving_upward # Compute reward and termination from simulator quantities reward, reward_details = self._compute_reward( @@ -539,8 +524,8 @@ def compute_transition(self, state: ArrayEnvState) -> ArrayEnvState: consecutive_bounces, bounce_detected=bounce_detected, target_heights=target_heights, - current_actions=state.info.get("current_actions"), - last_actions=state.info.get("last_actions"), + current_actions=self._current_actions, + last_actions=self._last_actions, ) terminated = self._compute_terminated(inputs["dof_pos"], inputs["dof_vel"], target_heights=target_heights) @@ -549,12 +534,11 @@ def compute_transition(self, state: ArrayEnvState) -> ArrayEnvState: # Store reward details for debugging if self._cfg.store_reward_details: - state.info["Reward"] = reward_details - state.info["target_heights"] = target_heights # Ensure target_heights persists across steps + state.reward_terms = reward_details return state - def reset(self, env_ids: np.ndarray) -> dict: + def reset(self, env_ids: np.ndarray) -> None: """Reset environment to initial state with randomized target heights""" cfg: BounceBallEnvCfg = self._cfg num_reset = len(env_ids) @@ -612,14 +596,9 @@ def reset(self, env_ids: np.ndarray) -> dict: # compute_observation (which never executes) sees post-reset state. self.sim_data.execute(np.asarray(env_ids, dtype=np.int64)) - # Initialize info dict with bounce tracking variables - info = { - "consecutive_bounces": np.zeros(num_reset, dtype=np.int32), - "ball_was_upward": np.zeros(num_reset, dtype=bool), - "max_consecutive_bounces": 0, - "target_heights": new_target_heights.copy(), # Return target heights for this reset batch - "current_actions": np.zeros((num_reset, 6), dtype=np.float32), - "last_actions": np.zeros((num_reset, 6), dtype=np.float32), - } - - return info + # Write episode-scoped state for the reset rows + self._consecutive_bounces[env_ids] = 0 + self._ball_was_upward[env_ids] = False + self._target_heights[env_ids] = new_target_heights + self._current_actions[env_ids] = 0.0 + self._last_actions[env_ids] = 0.0 diff --git a/motrix_envs/src/motrix_envs/basic/bounce_ball/cfg.py b/motrix_envs/src/motrix_envs/basic/bounce_ball/cfg.py index b0500a11..d1c08051 100644 --- a/motrix_envs/src/motrix_envs/basic/bounce_ball/cfg.py +++ b/motrix_envs/src/motrix_envs/basic/bounce_ball/cfg.py @@ -133,7 +133,8 @@ class BounceBallEnvCfg(DirectEnvCfg): action_bias: list = None # Debug options - store_reward_details: bool = False # Whether to store detailed reward breakdown in state.info, it's very slow + # Whether to store the detailed reward breakdown in state.reward_terms (very slow) + store_reward_details: bool = False def __post_init__(self): if self.ball_init_pos is None: diff --git a/motrix_envs/src/motrix_envs/basic/cartpole/cartpole_np.py b/motrix_envs/src/motrix_envs/basic/cartpole/cartpole_np.py index 1f71c117..a2859536 100644 --- a/motrix_envs/src/motrix_envs/basic/cartpole/cartpole_np.py +++ b/motrix_envs/src/motrix_envs/basic/cartpole/cartpole_np.py @@ -82,7 +82,7 @@ def compute_observation(self, state: ArrayEnvState): assert obs.shape == (self._num_envs, 4) return state.replace(obs=obs) - def reset(self, env_ids: np.ndarray): + def reset(self, env_ids: np.ndarray) -> None: cfg: CartPoleEnvCfg = self._cfg rows = len(env_ids) noise_pos = np.random.uniform( @@ -103,4 +103,3 @@ def reset(self, env_ids: np.ndarray): self._reset_velocity[env_ids] = dof_vel self._reset_program.execute(env_ids) self.sim_data.execute(env_ids) - return {} diff --git a/motrix_envs/src/motrix_envs/basic/cheetah/cheetah_np.py b/motrix_envs/src/motrix_envs/basic/cheetah/cheetah_np.py index eb84f1a4..f38214ee 100644 --- a/motrix_envs/src/motrix_envs/basic/cheetah/cheetah_np.py +++ b/motrix_envs/src/motrix_envs/basic/cheetah/cheetah_np.py @@ -109,7 +109,7 @@ def compute_transition(self, state: ArrayEnvState) -> ArrayEnvState: terminated=terminated, ) - def reset(self, env_ids: np.ndarray) -> dict: + def reset(self, env_ids: np.ndarray) -> None: num = len(env_ids) qpos = np.zeros((num, self._reset_position.shape[1]), dtype=np.float32) @@ -120,5 +120,3 @@ def reset(self, env_ids: np.ndarray) -> dict: row_ids = np.asarray(env_ids, np.int64) self.sim_data.execute(row_ids) - - return {} diff --git a/motrix_envs/src/motrix_envs/basic/finger/finger_np.py b/motrix_envs/src/motrix_envs/basic/finger/finger_np.py index caf80f23..0cc83dfe 100644 --- a/motrix_envs/src/motrix_envs/basic/finger/finger_np.py +++ b/motrix_envs/src/motrix_envs/basic/finger/finger_np.py @@ -119,6 +119,11 @@ def _init_action_space(self): ctrl_ranges[:, 0], ctrl_ranges[:, 1], (self.num_actuators,), dtype=np.float32 ) + # Episode-scoped action history: full-batch buffers, reset writes the + # done rows in place. + self._actions = np.zeros((self._num_envs, self.num_actuators), dtype=np.float32) + self._last_actions = np.zeros((self._num_envs, self.num_actuators), dtype=np.float32) + @property def observation_space(self) -> gym.spaces.Box: return self._observation_space @@ -129,12 +134,8 @@ def action_space(self) -> gym.spaces.Box: def apply_action(self, actions: np.ndarray, state: ArrayEnvState) -> ArrayEnvState: # Keep track of actions for reward shaping (e.g., smoothness penalties) - if "actions" not in state.info: - state.info["actions"] = np.zeros_like(actions, dtype=np.float32) - if "last_actions" not in state.info: - state.info["last_actions"] = np.zeros_like(actions, dtype=np.float32) - state.info["last_actions"] = state.info["actions"] - state.info["actions"] = actions + self._last_actions[:] = self._actions + self._actions[:] = actions ctrl = self._ctrl_writes.buffer("ctrl") ctrl[:] = np.asarray(actions, dtype=np.float32) self._ctrl_writes.execute() @@ -192,7 +193,7 @@ def _reset_collision_free_joint_angles(self, env_ids: np.ndarray): # reproduces the legacy global ``num_contacts > 0`` check exactly. pending = self.sim_data["colliding"][env_ids].max(axis=-1) > 0 - def reset(self, env_ids: np.ndarray) -> dict: + def reset(self, env_ids: np.ndarray) -> None: raise NotImplementedError @@ -259,7 +260,7 @@ def compute_transition(self, state: ArrayEnvState) -> ArrayEnvState: spin = np.clip(spin + touch_bonus + approach_reward, 0.0, 1.0).astype(np.float32) rwd = spin - state.info["Reward"] = { + state.reward_terms = { "hinge_velocity": hinge_velocity.copy(), "spin": spin.copy(), "spin_sparse": spin_sparse.copy(), @@ -272,25 +273,12 @@ def compute_transition(self, state: ArrayEnvState) -> ArrayEnvState: rwd[terminated] = 0.0 return state.replace(reward=rwd, terminated=terminated) - def reset(self, env_ids: np.ndarray) -> dict: - num = len(env_ids) + def reset(self, env_ids: np.ndarray) -> None: self._reset_collision_free_joint_angles(env_ids) - info: dict = {"Reward": {}} - info["actions"] = np.zeros((num, self.num_actuators), dtype=np.float32) - info["last_actions"] = np.zeros((num, self.num_actuators), dtype=np.float32) - info["Reward"] = { - "hinge_velocity": np.zeros((num,), dtype=np.float32), - "spin": np.zeros((num,), dtype=np.float32), - "spin_sparse": np.zeros((num,), dtype=np.float32), - "touch_raw": np.zeros((num,), dtype=np.float32), - "touch_bonus": np.zeros((num,), dtype=np.float32), - "approach_dist": np.zeros((num,), dtype=np.float32), - "approach_reward": np.zeros((num,), dtype=np.float32), - } - + self._actions[env_ids] = 0.0 + self._last_actions[env_ids] = 0.0 self.sim_data.execute(np.asarray(env_ids, np.int64)) - return info @registry.env("dm-finger-turn-easy") @@ -362,8 +350,8 @@ def compute_transition(self, state: ArrayEnvState) -> ArrayEnvState: touch_bonus = self._cfg.turn_touch_bonus_scale * np.tanh(touch_raw / self._cfg.turn_touch_bonus_tanh_scale) # Reduce jitter: penalize large actions and action changes - actions = state.info.get("actions", inputs["actuator_ctrls"]).astype(np.float32) - last_actions = state.info.get("last_actions", actions).astype(np.float32) + actions = self._actions + last_actions = self._last_actions action_l2 = np.mean(np.square(actions), axis=-1).astype(np.float32) action_delta_l2 = np.mean(np.square(actions - last_actions), axis=-1).astype(np.float32) @@ -379,7 +367,7 @@ def compute_transition(self, state: ArrayEnvState) -> ArrayEnvState: turn = turn_sparse rwd = turn - state.info["Reward"] = { + state.reward_terms = { "dist_to_target": dist_to_target.copy(), "turn": turn.copy(), "turn_sparse": turn_sparse.copy(), @@ -390,12 +378,11 @@ def compute_transition(self, state: ArrayEnvState) -> ArrayEnvState: "action_l2": action_l2.copy(), "action_delta_l2": action_delta_l2.copy(), } - state.info["target_info"] = {"positions": self._target_xyz.copy(), "radius": self._target_radius} rwd[terminated] = 0.0 return state.replace(reward=rwd, terminated=terminated) - def reset(self, env_ids: np.ndarray) -> dict: + def reset(self, env_ids: np.ndarray) -> None: num = len(env_ids) self._reset_collision_free_joint_angles(env_ids) @@ -420,14 +407,5 @@ def reset(self, env_ids: np.ndarray) -> dict: self._reset_target(env_ids, self._target_xyz[env_ids]) self.sim_data.execute(np.asarray(env_ids, np.int64)) - info: dict = {"Reward": {}} - info["actions"] = np.zeros((num, self.num_actuators), dtype=np.float32) - info["last_actions"] = np.zeros((num, self.num_actuators), dtype=np.float32) - info["target_info"] = {"positions": self._target_xyz.copy(), "radius": self._target_radius} - info["Reward"] = { - "dist_to_target": np.zeros((num,), dtype=np.float32), - "turn": np.zeros((num,), dtype=np.float32), - "turn_sparse": np.zeros((num,), dtype=np.float32), - } - - return info + self._actions[env_ids] = 0.0 + self._last_actions[env_ids] = 0.0 diff --git a/motrix_envs/src/motrix_envs/basic/hopper/hopper_np.py b/motrix_envs/src/motrix_envs/basic/hopper/hopper_np.py index f737297d..8287f9ce 100644 --- a/motrix_envs/src/motrix_envs/basic/hopper/hopper_np.py +++ b/motrix_envs/src/motrix_envs/basic/hopper/hopper_np.py @@ -156,6 +156,7 @@ def compute_transition(self, state: ArrayEnvState) -> ArrayEnvState: contact_reward = np.clip(contact_strength, 0.0, 1.0) * 0.1 * standing rwd = standing * 0.8 + effective_hop_reward * 0.8 + leg_bonus * 0.5 + extend_reward + contact_reward + state.reward_terms = {"stand": standing, "hop": effective_hop_reward, "total": rwd} if np.average(rwd) > 1000: print( "standing", @@ -182,7 +183,7 @@ def compute_transition(self, state: ArrayEnvState) -> ArrayEnvState: small_control = (small_control + 4) / 5 rwd = standing * small_control - state.info["Reward"] = {"stand": standing, "control": small_control, "total": rwd} + state.reward_terms = {"stand": standing, "control": small_control, "total": rwd} rwd[terminated] = 0.0 @@ -191,7 +192,7 @@ def compute_transition(self, state: ArrayEnvState) -> ArrayEnvState: terminated=terminated, ) - def reset(self, env_ids: np.ndarray): + def reset(self, env_ids: np.ndarray) -> None: num_reset = len(env_ids) dof_pos = np.zeros((num_reset, self._reset_position.shape[1])) @@ -211,9 +212,3 @@ def reset(self, env_ids: np.ndarray): self._reset_velocity[env_ids] = dof_vel self._reset_program.execute(env_ids) self.sim_data.execute(np.asarray(env_ids, np.int64)) - - rewards = {"stand": np.zeros((num_reset,))} - if self._hop_speed > 0.0: - rewards["hop"] = np.zeros((num_reset,)) - - return {"Reward": rewards} diff --git a/motrix_envs/src/motrix_envs/basic/humanoid/humanoid_np.py b/motrix_envs/src/motrix_envs/basic/humanoid/humanoid_np.py index e0f4d460..de63e876 100644 --- a/motrix_envs/src/motrix_envs/basic/humanoid/humanoid_np.py +++ b/motrix_envs/src/motrix_envs/basic/humanoid/humanoid_np.py @@ -219,7 +219,7 @@ def update_reward(self, state: ArrayEnvState) -> ArrayEnvState: torso_upright = self._get_torso_upright(slice(None)) rwd, reward_components = self._compute_reward(head_height, torso_upright, pelvis_height) rwd, reward_components = self._apply_termination_mask(terminated, rwd, reward_components) - state.info["Reward"] = reward_components + state.reward_terms = reward_components return state.replace(reward=rwd) def _apply_termination_mask( @@ -233,9 +233,8 @@ def _apply_termination_mask( reward_components[k] = np.where(terminated, 0.0, v).astype(np.float32) return rwd, reward_components - def reset(self, env_ids: np.ndarray) -> dict: + def reset(self, env_ids: np.ndarray) -> None: self._randomize_joints(env_ids) - return {} def _get_head_height(self, rows) -> np.ndarray: return self.sim_data["head_pos"][rows][:, 2] diff --git a/motrix_envs/src/motrix_envs/basic/lqr/lqr_np.py b/motrix_envs/src/motrix_envs/basic/lqr/lqr_np.py index 3cade2da..8448e711 100644 --- a/motrix_envs/src/motrix_envs/basic/lqr/lqr_np.py +++ b/motrix_envs/src/motrix_envs/basic/lqr/lqr_np.py @@ -120,7 +120,7 @@ def compute_transition(self, state: ArrayEnvState) -> ArrayEnvState: "success": success.astype(np.float32), "out_of_bounds": out_of_bounds.astype(np.float32), } - state.info["Reward"] = { + state.reward_terms = { "state_cost": (-state_cost).astype(np.float32), "velocity_cost": (-velocity_cost).astype(np.float32), "control_cost": (-control_cost).astype(np.float32), @@ -133,7 +133,7 @@ def compute_transition(self, state: ArrayEnvState) -> ArrayEnvState: terminated=terminated, ) - def reset(self, env_ids: np.ndarray) -> dict: + def reset(self, env_ids: np.ndarray) -> None: num_envs = len(env_ids) qpos = np.random.standard_normal((num_envs, self._nq)).astype(np.float32) @@ -149,5 +149,3 @@ def reset(self, env_ids: np.ndarray) -> dict: self._reset_velocity[env_ids] = qvel self._reset_program.execute(env_ids) self.sim_data.execute(np.asarray(env_ids, np.int64)) - - return {} diff --git a/motrix_envs/src/motrix_envs/basic/manipulator/manipulator_np.py b/motrix_envs/src/motrix_envs/basic/manipulator/manipulator_np.py index 3368f004..67c1be4e 100644 --- a/motrix_envs/src/motrix_envs/basic/manipulator/manipulator_np.py +++ b/motrix_envs/src/motrix_envs/basic/manipulator/manipulator_np.py @@ -134,6 +134,12 @@ def __init__(self, cfg: BringBallCfg, num_envs=1, backend: str | None = None): self._init_action_space() self._init_obs_space() + # Episode-scoped action history: full-batch buffers, reset writes the + # done rows in place. + self._actions = np.zeros((self._num_envs, self.num_actuators), dtype=np.float32) + self._last_actions = np.zeros((self._num_envs, self.num_actuators), dtype=np.float32) + self._prev_move_dist = np.zeros(self._num_envs, dtype=np.float32) + def _init_action_space(self): ctrl_ranges = np.asarray([spec.ctrl_range for spec in self.model.actuators], dtype=np.float32) self._action_space = gym.spaces.Box( @@ -156,8 +162,8 @@ def apply_action(self, actions: np.ndarray, state: ArrayEnvState) -> ArrayEnvSta actions = np.asarray(actions, dtype=np.float32) # Enforce actuator control limits to avoid out-of-range impulses. actions = np.clip(actions, self._action_space.low, self._action_space.high).astype(np.float32) - state.info["last_actions"] = state.info["actions"] - state.info["actions"] = actions + self._last_actions[:] = self._actions + self._actions[:] = actions ctrl = self._ctrl_writes.buffer("ctrl") ctrl[:] = actions self._ctrl_writes.execute() @@ -325,15 +331,13 @@ def initialize_episode(self, env_ids: np.ndarray) -> None: self._set_target_mocap(env_ids, target_x, target_z, target_angle) self.sim_data.execute(row_ids) - def reset(self, env_ids: np.ndarray) -> dict: - num = len(env_ids) + def reset(self, env_ids: np.ndarray) -> None: self.initialize_episode(env_ids) - info = { - "actions": np.zeros((num, self.num_actuators), dtype=np.float32), - "last_actions": np.zeros((num, self.num_actuators), dtype=np.float32), - } - return info + # Write episode-scoped state for the reset rows + self._actions[env_ids] = 0.0 + self._last_actions[env_ids] = 0.0 + self._prev_move_dist[env_ids] = 0.0 @registry.env("dm-manipulator-bring-ball") @@ -438,8 +442,7 @@ def compute_transition(self, state: ArrayEnvState) -> ArrayEnvState: r_pause = (r_pause * post_grasp_scale).astype(np.float32) # R4: Close - default_actions = np.zeros((self._num_envs, self.num_actuators), dtype=np.float32) - grasp_action = state.info.get("actions", default_actions)[:, self._grasp_act_i].astype(np.float32) + grasp_action = self._actions[:, self._grasp_act_i].astype(np.float32) r_close_intent = _tolerance( grasp_action, bounds=(0.8, 1.0), margin=1.0, sigmoid="linear", value_at_margin=0.01 ).astype(np.float32) @@ -473,18 +476,13 @@ def compute_transition(self, state: ArrayEnvState) -> ArrayEnvState: lift_norm = max(lift_height_weight + transport_weight, 1e-6) r_lift = ((lift_height_weight * r_lift_height + transport_weight * r_transport) / lift_norm).astype(np.float32) - prev_move_dist = state.info.get("prev_move_dist") - if prev_move_dist is None: - prev_move_dist = move_dist - else: - prev_move_dist = np.asarray(prev_move_dist, dtype=np.float32) first_step = state.episode_steps == 0 - prev_move_dist = np.where(first_step, move_dist, prev_move_dist) + prev_move_dist = np.where(first_step, move_dist, self._prev_move_dist) progress_clip = float(cfg.transport_progress_clip) progress = (prev_move_dist - move_dist) / max(progress_clip, 1e-6) progress = np.clip(progress, -1.0, 1.0).astype(np.float32) r_progress = (progress * float(cfg.transport_progress_scale) * grasp_mask).astype(np.float32) - state.info["prev_move_dist"] = move_dist.astype(np.float32) + self._prev_move_dist[:] = move_dist.astype(np.float32) # --- Penalties --- all_touch = self._touch_raw(slice(None)) @@ -521,7 +519,7 @@ def compute_transition(self, state: ArrayEnvState) -> ArrayEnvState: reward = np.where(terminated, 0.0, reward) - state.info["Reward"] = { + state.reward_terms = { "reach": r_reach, "orient": r_orient, "close": r_close, diff --git a/motrix_envs/src/motrix_envs/basic/pendulum/pendulum_np.py b/motrix_envs/src/motrix_envs/basic/pendulum/pendulum_np.py index d8770501..918f6d9d 100644 --- a/motrix_envs/src/motrix_envs/basic/pendulum/pendulum_np.py +++ b/motrix_envs/src/motrix_envs/basic/pendulum/pendulum_np.py @@ -46,6 +46,8 @@ def __init__(self, cfg: PendulumEnvCfg, num_envs=1, backend: str | None = None): self._action_high = float(ctrl_limits[1, 0]) self._action_space = gym.spaces.Box(-1.0, 1.0, (1,), dtype=np.float32) self._observation_space = gym.spaces.Box(-np.inf, np.inf, (3,), dtype=np.float32) + # Episode-scoped control history for the control-delta penalty. + self._prev_ctrl = np.zeros(self._num_envs, dtype=np.float32) @property def observation_space(self): @@ -84,7 +86,7 @@ def compute_transition(self, state: ArrayEnvState): # In this model, zero angle corresponds to the hanging-down position. # Shift the target by pi to encourage the upright (inverted) posture. upright = (1.0 + np.cos(angle_wrapped)) * 0.5 - prev_ctrl = state.info.get("prev_ctrl", np.zeros_like(ctrl)) + prev_ctrl = self._prev_ctrl ctrl_delta = ctrl - prev_ctrl vel_penalty = 0.2 * (ang_vel**2) energy = 0.5 * ang_vel**2 + (1.0 - np.cos(angle_wrapped)) @@ -105,10 +107,10 @@ def compute_transition(self, state: ArrayEnvState): state.reward = reward state.terminated = terminated - state.info["prev_ctrl"] = ctrl + self._prev_ctrl[:] = ctrl return state - def reset(self, env_ids: np.ndarray): + def reset(self, env_ids: np.ndarray) -> None: cfg: PendulumEnvCfg = self._cfg reset_noise_scale = getattr(cfg, "reset_noise_scale", 0.0) num_reset = len(env_ids) @@ -122,4 +124,4 @@ def reset(self, env_ids: np.ndarray): self._reset_velocity[env_ids] = dof_vel self._reset_program.execute(env_ids) self.sim_data.execute(np.asarray(env_ids, np.int64)) - return {"prev_ctrl": np.zeros((num_reset,), dtype=np.float32)} + self._prev_ctrl[env_ids] = 0.0 diff --git a/motrix_envs/src/motrix_envs/basic/point_mass/point_mass_np.py b/motrix_envs/src/motrix_envs/basic/point_mass/point_mass_np.py index 4b84792d..3f84069e 100644 --- a/motrix_envs/src/motrix_envs/basic/point_mass/point_mass_np.py +++ b/motrix_envs/src/motrix_envs/basic/point_mass/point_mass_np.py @@ -149,7 +149,7 @@ def compute_transition(self, state: ArrayEnvState) -> ArrayEnvState: state.terminated = terminated return state - def reset(self, env_ids: np.ndarray) -> dict: + def reset(self, env_ids: np.ndarray) -> None: num_reset = len(env_ids) # Random initial position within a range for the point mass (only x, y) @@ -176,4 +176,3 @@ def reset(self, env_ids: np.ndarray) -> dict: self._in_target_steps[env_ids] = 0 self.sim_data.execute(np.asarray(env_ids, np.int64)) - return {} diff --git a/motrix_envs/src/motrix_envs/basic/quadruped/quadruped_np.py b/motrix_envs/src/motrix_envs/basic/quadruped/quadruped_np.py index da03aed2..3c2da8c7 100644 --- a/motrix_envs/src/motrix_envs/basic/quadruped/quadruped_np.py +++ b/motrix_envs/src/motrix_envs/basic/quadruped/quadruped_np.py @@ -235,6 +235,11 @@ def _init_action_space(self): ctrl_ranges[:, 0], ctrl_ranges[:, 1], (self.num_actuators,), dtype=np.float32 ) + # Episode-scoped action history: full-batch buffers, reset writes the + # done rows in place. + self._actions = np.zeros((self._num_envs, self.num_actuators), dtype=np.float32) + self._last_actions = np.zeros((self._num_envs, self.num_actuators), dtype=np.float32) + @property def observation_space(self) -> gym.spaces.Box: return self._observation_space @@ -247,12 +252,8 @@ def apply_action(self, actions: np.ndarray, state: ArrayEnvState) -> ArrayEnvSta if self._cfg.clip_env_actions: actions = np.clip(actions, self._action_space.low, self._action_space.high) actions = actions.astype(np.float32) - if "actions" not in state.info: - state.info["actions"] = np.zeros_like(actions, dtype=np.float32) - if "last_actions" not in state.info: - state.info["last_actions"] = np.zeros_like(actions, dtype=np.float32) - state.info["last_actions"] = state.info["actions"] - state.info["actions"] = actions + self._last_actions[:] = self._actions + self._actions[:] = actions ctrl = self._ctrl_writes.buffer("ctrl") ctrl[:] = np.asarray(actions, dtype=np.float32) self._ctrl_writes.execute() @@ -497,11 +498,7 @@ def _lateral_reward(self, torso_vel: np.ndarray) -> np.ndarray: ) def _smooth_reward(self, state: ArrayEnvState) -> np.ndarray: - smooth_reward = np.zeros((self._num_envs,), dtype=np.float32) - if "actions" not in state.info or "last_actions" not in state.info: - return smooth_reward - - delta = state.info["actions"] - state.info["last_actions"] + delta = self._actions - self._last_actions delta_norm = np.linalg.norm(delta, axis=-1) return reward.tolerance( delta_norm, @@ -597,50 +594,6 @@ def compute_observation(self, state: ArrayEnvState) -> ArrayEnvState: return state.replace(obs=np.concatenate(parts, axis=-1).astype(np.float32)) - def _locomotion_reward_info(self, num_envs: int) -> dict: - return { - "upright": np.zeros((num_envs,), dtype=np.float32), - "move": np.zeros((num_envs,), dtype=np.float32), - "backward": np.zeros((num_envs,), dtype=np.float32), - "height": np.zeros((num_envs,), dtype=np.float32), - "lateral": np.zeros((num_envs,), dtype=np.float32), - "heading": np.zeros((num_envs,), dtype=np.float32), - "smooth": np.zeros((num_envs,), dtype=np.float32), - "lin_vel_z": np.zeros((num_envs,), dtype=np.float32), - "ang_vel_xy": np.zeros((num_envs,), dtype=np.float32), - "similar_to_default": np.zeros((num_envs,), dtype=np.float32), - "total": np.zeros((num_envs,), dtype=np.float32), - } - - def _escape_reward_info(self, num_envs: int) -> dict: - info = self._locomotion_reward_info(num_envs) - info.update( - { - "escape": np.zeros((num_envs,), dtype=np.float32), - "radial": np.zeros((num_envs,), dtype=np.float32), - } - ) - return info - - def _fetch_reward_info(self, num_envs: int) -> dict: - return { - "upright": np.zeros((num_envs,), dtype=np.float32), - "stage_move": np.zeros((num_envs,), dtype=np.float32), - "stage_reach": np.zeros((num_envs,), dtype=np.float32), - "stability": np.zeros((num_envs,), dtype=np.float32), - "behind_align": np.zeros((num_envs,), dtype=np.float32), - "face_ball": np.zeros((num_envs,), dtype=np.float32), - "near_ball": np.zeros((num_envs,), dtype=np.float32), - "ready": np.zeros((num_envs,), dtype=np.float32), - "ready_gate": np.zeros((num_envs,), dtype=np.float32), - "fetch": np.zeros((num_envs,), dtype=np.float32), - "push": np.zeros((num_envs,), dtype=np.float32), - "away": np.zeros((num_envs,), dtype=np.float32), - "leg_ball": np.zeros((num_envs,), dtype=np.float32), - "backward": np.zeros((num_envs,), dtype=np.float32), - "total": np.zeros((num_envs,), dtype=np.float32), - } - def _base_locomotion_components(self, state: ArrayEnvState) -> dict[str, np.ndarray]: torso_vel = self._torso_velocity(slice(None)) return { @@ -670,13 +623,6 @@ def _locomotion_reward(self, upright_reward: np.ndarray, components: dict[str, n ) return self._sum_scaled_rewards(reward_terms, self._locomotion_reward_scales()) - def _build_reset_info(self, num_envs: int) -> dict: - return { - "Reward": self._init_reward_info(num_envs), - "actions": np.zeros((num_envs, self.num_actuators), dtype=np.float32), - "last_actions": np.zeros((num_envs, self.num_actuators), dtype=np.float32), - } - def _random_quaternion(self, num: int) -> np.ndarray: q = np.random.randn(num, 4).astype(np.float32) q /= np.linalg.norm(q, axis=-1, keepdims=True) @@ -720,23 +666,20 @@ def _lift_non_contacting(self, env_ids: np.ndarray, dof_pos: np.ndarray, dof_vel z[pending] += 0.01 return dof_pos - def _finish_reset(self, env_ids: np.ndarray, dof_pos: np.ndarray, dof_vel: np.ndarray) -> dict: + def _finish_reset(self, env_ids: np.ndarray, dof_pos: np.ndarray, dof_vel: np.ndarray) -> None: dof_pos = self._lift_non_contacting(env_ids, dof_pos, dof_vel) self._execute_reset( env_ids, np.ascontiguousarray(dof_pos, dtype=np.float32), np.ascontiguousarray(dof_vel, dtype=np.float32) ) self.sim_data.execute(np.asarray(env_ids, dtype=np.int64)) - info = self._build_reset_info(len(env_ids)) - return info + self._actions[env_ids] = 0.0 + self._last_actions[env_ids] = 0.0 @registry.env("dm-quadruped-walk") @registry.env("dm-quadruped-run") class QuadrupedLocomotionEnv(QuadrupedEnv): - def _init_reward_info(self, num_envs: int) -> dict: - return self._locomotion_reward_info(num_envs) - def compute_transition(self, state: ArrayEnvState) -> ArrayEnvState: self.sim_data.execute() inputs = self.sim_data @@ -751,11 +694,11 @@ def compute_transition(self, state: ArrayEnvState) -> ArrayEnvState: terminated = np.isnan(inputs["dof_pos"]).any(axis=-1) | np.isnan(inputs["dof_vel"]).any(axis=-1) rwd = np.where(terminated, 0.0, rwd).astype(np.float32) - state.info["Reward"] = reward_components + state.reward_terms = reward_components return state.replace(reward=rwd, terminated=terminated) - def reset(self, env_ids: np.ndarray): + def reset(self, env_ids: np.ndarray) -> None: num = len(env_ids) dof_pos = np.tile(self._init_dof_pos, (num, 1)) dof_vel = np.zeros((num, self.num_dof_vel), dtype=np.float32) @@ -765,14 +708,11 @@ def reset(self, env_ids: np.ndarray): else: dof_pos[:, 3:7] = self._random_quaternion(num) - return self._finish_reset(env_ids, dof_pos, dof_vel) + self._finish_reset(env_ids, dof_pos, dof_vel) @registry.env("dm-quadruped-escape") class QuadrupedEscapeEnv(QuadrupedLocomotionEnv): - def _init_reward_info(self, num_envs: int) -> dict: - return self._escape_reward_info(num_envs) - def compute_transition(self, state: ArrayEnvState) -> ArrayEnvState: self.sim_data.execute() inputs = self.sim_data @@ -811,16 +751,13 @@ def compute_transition(self, state: ArrayEnvState) -> ArrayEnvState: terminated = np.isnan(inputs["dof_pos"]).any(axis=-1) | np.isnan(inputs["dof_vel"]).any(axis=-1) rwd = np.where(terminated, 0.0, rwd).astype(np.float32) - state.info["Reward"] = reward_components + state.reward_terms = reward_components return state.replace(reward=rwd, terminated=terminated) @registry.env("dm-quadruped-fetch") class QuadrupedFetchEnv(QuadrupedEnv): - def _init_reward_info(self, num_envs: int) -> dict: - return self._fetch_reward_info(num_envs) - def compute_transition(self, state: ArrayEnvState) -> ArrayEnvState: self.sim_data.execute() inputs = self.sim_data @@ -980,11 +917,11 @@ def compute_transition(self, state: ArrayEnvState) -> ArrayEnvState: rwd = np.where(terminated, 0.0, rwd).astype(np.float32) for key, value in reward_components.items(): reward_components[key] = np.where(terminated, 0.0, value).astype(np.float32) - state.info["Reward"] = reward_components + state.reward_terms = reward_components return state.replace(reward=rwd, terminated=terminated) - def reset(self, env_ids: np.ndarray): + def reset(self, env_ids: np.ndarray) -> None: num = len(env_ids) dof_pos = np.tile(self._init_dof_pos, (num, 1)) dof_vel = np.zeros((num, self.num_dof_vel), dtype=np.float32) @@ -1009,4 +946,4 @@ def reset(self, env_ids: np.ndarray): ball_qvel = self._ball_vel_slice dof_vel[:, ball_qvel.start : ball_qvel.stop] = 0.0 - return self._finish_reset(env_ids, dof_pos, dof_vel) + self._finish_reset(env_ids, dof_pos, dof_vel) diff --git a/motrix_envs/src/motrix_envs/basic/reacher/reacher_np.py b/motrix_envs/src/motrix_envs/basic/reacher/reacher_np.py index 9fb6f30d..5a67044e 100644 --- a/motrix_envs/src/motrix_envs/basic/reacher/reacher_np.py +++ b/motrix_envs/src/motrix_envs/basic/reacher/reacher_np.py @@ -102,11 +102,11 @@ def compute_transition(self, state: ArrayEnvState) -> ArrayEnvState: ) rwd[terminated] = 0.0 - state.info["Reward"] = {"distance": dist, "tolerance": rwd.copy()} + state.reward_terms = {"distance": dist, "tolerance": rwd.copy()} return state.replace(reward=rwd, terminated=terminated) - def reset(self, env_ids: np.ndarray) -> dict: + def reset(self, env_ids: np.ndarray) -> None: """Reset environment with randomized target position in xy plane (z=0).""" num_reset = len(env_ids) @@ -131,10 +131,3 @@ def reset(self, env_ids: np.ndarray) -> dict: target_pose = self.sim_data["target_pos"][env_ids] self._target_xyz[env_ids] = target_pose self._target_xyz[env_ids, 2] = 0.0 - - rewards = {"distance": np.zeros((num_reset,)), "tolerance": np.zeros((num_reset,))} - info = { - "Reward": rewards, - } - - return info diff --git a/motrix_envs/src/motrix_envs/basic/stewart/stewart_np.py b/motrix_envs/src/motrix_envs/basic/stewart/stewart_np.py index 9a29346e..4fa3b977 100644 --- a/motrix_envs/src/motrix_envs/basic/stewart/stewart_np.py +++ b/motrix_envs/src/motrix_envs/basic/stewart/stewart_np.py @@ -165,6 +165,39 @@ def __init__(self, cfg: StewartBaseEnvCfg, num_envs=1, backend: str | None = Non self._top_connect_offsets = np.asarray(top_connect_offsets, dtype=np.float32) self._leg_length_init = np.asarray(leg_length_init, dtype=np.float32) + # Episode-scoped task state: full-batch buffers, reset writes the done + # rows in place. + num_envs = self._num_envs + self._target_pos = np.zeros((num_envs, 3), dtype=np.float32) + self._target_quat = _identity_quat((num_envs,)) + self._target_tilt_cmd = np.zeros((num_envs, 2), dtype=np.float32) + self._prev_rel = np.zeros((num_envs, 3), dtype=np.float32) + self._filtered_rel_vel = np.zeros((num_envs, 3), dtype=np.float32) + self._last_rel_vel = np.zeros((num_envs, 3), dtype=np.float32) + self._prev_top_quat = _identity_quat((num_envs,)) + self._filtered_top_ang_vel = np.zeros((num_envs, 3), dtype=np.float32) + self._last_top_ang_vel = np.zeros((num_envs, 3), dtype=np.float32) + self._initial_rel_xy = np.zeros(num_envs, dtype=np.float32) + self._prev_zero_vel_rel_xy = np.zeros(num_envs, dtype=np.float32) + self._still_steps = np.zeros(num_envs, dtype=np.int32) + self._still_window_active = np.zeros(num_envs, dtype=bool) + self._policy_action = np.zeros((num_envs, self._action_dim), dtype=np.float32) + self._prev_action_exec = np.zeros((num_envs, self._action_dim), dtype=np.float32) + self._action_exec = np.zeros((num_envs, self._action_dim), dtype=np.float32) + self._disturb_time = np.zeros(num_envs, dtype=np.float32) + self._disturb_pos = np.zeros((num_envs, 3), dtype=np.float32) + self._disturb_lin_vel = np.zeros((num_envs, 3), dtype=np.float32) + self._disturb_rot_deg = np.zeros((num_envs, 2), dtype=np.float32) + self._disturb_ang_vel_deg = np.zeros((num_envs, 2), dtype=np.float32) + self._disturb_pos_alpha = np.ones((num_envs, 3), dtype=np.float32) + self._disturb_pos_limit_scale = np.ones((num_envs, 3), dtype=np.float32) + self._disturb_pos_noise_scale = np.zeros((num_envs, 3), dtype=np.float32) + self._disturb_pos_jitter_scale = np.zeros((num_envs, 3), dtype=np.float32) + self._disturb_rot_alpha = np.ones((num_envs, 2), dtype=np.float32) + self._disturb_rot_limit_scale = np.ones((num_envs, 2), dtype=np.float32) + self._disturb_rot_noise_scale = np.zeros((num_envs, 2), dtype=np.float32) + self._disturb_rot_jitter_scale = np.zeros((num_envs, 2), dtype=np.float32) + @property def observation_space(self) -> gym.spaces.Box: return self._observation_space @@ -174,18 +207,14 @@ def action_space(self) -> gym.spaces.Box: return self._action_space def apply_action(self, actions: np.ndarray, state: ArrayEnvState) -> ArrayEnvState: - state.info["policy_action"] = _normalize_actions(actions, self._num_envs, self._action_dim) + self._policy_action[:] = _normalize_actions(actions, self._num_envs, self._action_dim) return state - def _smooth_actions(self, raw_actions: np.ndarray, info: dict) -> tuple[np.ndarray, np.ndarray]: - prev_action = info["prev_action_exec"].astype(np.float32) + def _smooth_actions(self, raw_actions: np.ndarray) -> None: alpha = float(self._cfg.action_smooth) - action_exec = alpha * raw_actions + (1.0 - alpha) * prev_action - action_delta = action_exec - prev_action - info["prev_action_exec"] = action_exec.astype(np.float32) - info["action_exec"] = action_exec.astype(np.float32) - info["action_delta"] = action_delta.astype(np.float32) - return info["action_exec"], info["action_delta"] + action_exec = (alpha * raw_actions + (1.0 - alpha) * self._prev_action_exec).astype(np.float32) + self._prev_action_exec[:] = action_exec + self._action_exec[:] = action_exec def _write_body_state( self, @@ -234,7 +263,7 @@ def _write_ctrl_rows(self, env_ids: np.ndarray, values: np.ndarray) -> None: ctrl[:] = ctrls self._ctrl_writes.execute() - def _apply_pose_delta(self, info: dict, actions: np.ndarray) -> None: + def _apply_pose_delta(self, actions: np.ndarray) -> None: rel_xy_now = self._compute_rel_xy(slice(None)) if self._cfg.center_control_radius > 0.0 and self._cfg.center_control_min_gain < 1.0: ratio = np.clip(rel_xy_now / max(self._cfg.center_control_radius, 1e-6), 0.0, 1.0) @@ -253,39 +282,38 @@ def _apply_pose_delta(self, info: dict, actions: np.ndarray) -> None: target_euler_rad[..., 0], target_euler_rad[..., 1], target_euler_rad[..., 2] ).astype(np.float32) - info["target_pos"] = target_pos - info["target_quat"] = target_quat.astype(np.float32) - info["target_tilt_cmd"] = target_tilt_cmd - - def _clear_disturbance_state(self, info: dict) -> None: - num = info["target_pos"].shape[0] - info["disturb_time"] = np.zeros((num,), dtype=np.float32) - info["disturb_pos"] = np.zeros((num, 3), dtype=np.float32) - info["disturb_lin_vel"] = np.zeros((num, 3), dtype=np.float32) - info["disturb_rot_deg"] = np.zeros((num, 2), dtype=np.float32) - info["disturb_ang_vel_deg"] = np.zeros((num, 2), dtype=np.float32) - info["_disturb_pos_alpha"] = np.ones((num, 3), dtype=np.float32) - info["_disturb_pos_limit_scale"] = np.ones((num, 3), dtype=np.float32) - info["_disturb_pos_noise_scale"] = np.zeros((num, 3), dtype=np.float32) - info["_disturb_pos_jitter_scale"] = np.zeros((num, 3), dtype=np.float32) - info["_disturb_rot_alpha"] = np.ones((num, 2), dtype=np.float32) - info["_disturb_rot_limit_scale"] = np.ones((num, 2), dtype=np.float32) - info["_disturb_rot_noise_scale"] = np.zeros((num, 2), dtype=np.float32) - info["_disturb_rot_jitter_scale"] = np.zeros((num, 2), dtype=np.float32) - - def _reset_episode_disturbance(self, info: dict) -> None: - self._clear_disturbance_state(info) + self._target_pos[:] = target_pos + self._target_quat[:] = target_quat + self._target_tilt_cmd[:] = target_tilt_cmd + + def _clear_disturbance_state(self, env_ids: np.ndarray) -> None: + self._disturb_time[env_ids] = 0.0 + self._disturb_pos[env_ids] = 0.0 + self._disturb_lin_vel[env_ids] = 0.0 + self._disturb_rot_deg[env_ids] = 0.0 + self._disturb_ang_vel_deg[env_ids] = 0.0 + self._disturb_pos_alpha[env_ids] = 1.0 + self._disturb_pos_limit_scale[env_ids] = 1.0 + self._disturb_pos_noise_scale[env_ids] = 0.0 + self._disturb_pos_jitter_scale[env_ids] = 0.0 + self._disturb_rot_alpha[env_ids] = 1.0 + self._disturb_rot_limit_scale[env_ids] = 1.0 + self._disturb_rot_noise_scale[env_ids] = 0.0 + self._disturb_rot_jitter_scale[env_ids] = 0.0 + + def _reset_episode_disturbance(self, env_ids: np.ndarray) -> None: + self._clear_disturbance_state(env_ids) if (not self._cfg.disturbance_enabled) or self._cfg.disturbance_scale <= 0.0: return - num = info["target_pos"].shape[0] + num = len(env_ids) freq_min = max(1e-4, float(self._cfg.disturb_freq_min_hz)) freq_max = max(freq_min, float(self._cfg.disturb_freq_max_hz)) pos_freq = np.random.uniform(freq_min, freq_max, size=(num, 3)).astype(np.float32) rot_freq = np.random.uniform(freq_min, freq_max, size=(num, 2)).astype(np.float32) - info["_disturb_pos_alpha"] = np.exp(-2.0 * np.pi * pos_freq * self._cfg.ctrl_dt).astype(np.float32) - info["_disturb_rot_alpha"] = np.exp(-2.0 * np.pi * rot_freq * self._cfg.ctrl_dt).astype(np.float32) + self._disturb_pos_alpha[env_ids] = np.exp(-2.0 * np.pi * pos_freq * self._cfg.ctrl_dt).astype(np.float32) + self._disturb_rot_alpha[env_ids] = np.exp(-2.0 * np.pi * rot_freq * self._cfg.ctrl_dt).astype(np.float32) pos_limit_scale = np.ones((num, 3), dtype=np.float32) if self._cfg.disturb_pos_xy_max > 0.0: xy_min_ratio = np.clip( @@ -296,27 +324,27 @@ def _reset_episode_disturbance(self, info: dict) -> None: pos_limit_scale[:, :2] = np.random.uniform(xy_min_ratio, 1.00, size=(num, 2)).astype(np.float32) if self._cfg.disturb_pos_z_max > 0.0: pos_limit_scale[:, 2] = np.random.uniform(0.75, 1.00, size=(num,)).astype(np.float32) - info["_disturb_pos_limit_scale"] = pos_limit_scale - info["_disturb_rot_limit_scale"] = np.random.uniform(0.75, 1.00, size=(num, 2)).astype(np.float32) - info["_disturb_pos_noise_scale"] = np.random.uniform(0.35, 0.60, size=(num, 3)).astype(np.float32) - info["_disturb_rot_noise_scale"] = np.random.uniform(0.35, 0.60, size=(num, 2)).astype(np.float32) - info["_disturb_pos_jitter_scale"] = np.random.uniform(0.02, 0.06, size=(num, 3)).astype(np.float32) - info["_disturb_rot_jitter_scale"] = np.random.uniform(0.03, 0.08, size=(num, 2)).astype(np.float32) - - def _update_disturbance_state(self, info: dict, advance: bool) -> None: - num = info["target_pos"].shape[0] + self._disturb_pos_limit_scale[env_ids] = pos_limit_scale + self._disturb_rot_limit_scale[env_ids] = np.random.uniform(0.75, 1.00, size=(num, 2)).astype(np.float32) + self._disturb_pos_noise_scale[env_ids] = np.random.uniform(0.35, 0.60, size=(num, 3)).astype(np.float32) + self._disturb_rot_noise_scale[env_ids] = np.random.uniform(0.35, 0.60, size=(num, 2)).astype(np.float32) + self._disturb_pos_jitter_scale[env_ids] = np.random.uniform(0.02, 0.06, size=(num, 3)).astype(np.float32) + self._disturb_rot_jitter_scale[env_ids] = np.random.uniform(0.03, 0.08, size=(num, 2)).astype(np.float32) + + def _update_disturbance_state(self, advance: bool) -> None: + num = self._num_envs if advance: - info["disturb_time"] = info["disturb_time"] + self._cfg.ctrl_dt + self._disturb_time += self._cfg.ctrl_dt if (not self._cfg.disturbance_enabled) or self._cfg.disturbance_scale <= 0.0: - info["disturb_pos"] = np.zeros((num, 3), dtype=np.float32) - info["disturb_lin_vel"] = np.zeros((num, 3), dtype=np.float32) - info["disturb_rot_deg"] = np.zeros((num, 2), dtype=np.float32) - info["disturb_ang_vel_deg"] = np.zeros((num, 2), dtype=np.float32) + self._disturb_pos[:] = 0.0 + self._disturb_lin_vel[:] = 0.0 + self._disturb_rot_deg[:] = 0.0 + self._disturb_ang_vel_deg[:] = 0.0 return ramp = np.ones((num,), dtype=np.float32) if self._cfg.disturb_ramp_seconds > 1e-8: - ramp = np.clip(info["disturb_time"] / self._cfg.disturb_ramp_seconds, 0.0, 1.0).astype(np.float32) + ramp = np.clip(self._disturb_time / self._cfg.disturb_ramp_seconds, 0.0, 1.0).astype(np.float32) base_pos_limit = self._cfg.disturbance_scale * np.array( [self._cfg.disturb_pos_xy_max, self._cfg.disturb_pos_xy_max, self._cfg.disturb_pos_z_max], @@ -326,39 +354,40 @@ def _update_disturbance_state(self, info: dict, advance: bool) -> None: [self._cfg.disturb_rot_max_deg, self._cfg.disturb_rot_max_deg], dtype=np.float32, ) - pos_limit = ramp[:, None] * base_pos_limit[None, :] * info["_disturb_pos_limit_scale"] - rot_limit = ramp[:, None] * base_rot_limit[None, :] * info["_disturb_rot_limit_scale"] - pos_noise_std = ramp[:, None] * base_pos_limit[None, :] * info["_disturb_pos_noise_scale"] - rot_noise_std = ramp[:, None] * base_rot_limit[None, :] * info["_disturb_rot_noise_scale"] - pos_jitter_std = ramp[:, None] * base_pos_limit[None, :] * info["_disturb_pos_jitter_scale"] - rot_jitter_std = ramp[:, None] * base_rot_limit[None, :] * info["_disturb_rot_jitter_scale"] - - prev_pos = info["disturb_pos"].copy() - prev_rot = info["disturb_rot_deg"].copy() + pos_limit = ramp[:, None] * base_pos_limit[None, :] * self._disturb_pos_limit_scale + rot_limit = ramp[:, None] * base_rot_limit[None, :] * self._disturb_rot_limit_scale + pos_noise_std = ramp[:, None] * base_pos_limit[None, :] * self._disturb_pos_noise_scale + rot_noise_std = ramp[:, None] * base_rot_limit[None, :] * self._disturb_rot_noise_scale + pos_jitter_std = ramp[:, None] * base_pos_limit[None, :] * self._disturb_pos_jitter_scale + rot_jitter_std = ramp[:, None] * base_rot_limit[None, :] * self._disturb_rot_jitter_scale + + prev_pos = self._disturb_pos.copy() + prev_rot = self._disturb_rot_deg.copy() pos_noise = np.random.standard_normal((num, 3)).astype(np.float32) rot_noise = np.random.standard_normal((num, 2)).astype(np.float32) pos_jitter = np.random.standard_normal((num, 3)).astype(np.float32) rot_jitter = np.random.standard_normal((num, 2)).astype(np.float32) - pos_blend = np.sqrt(np.maximum(1.0 - info["_disturb_pos_alpha"] ** 2, 0.0)).astype(np.float32) - rot_blend = np.sqrt(np.maximum(1.0 - info["_disturb_rot_alpha"] ** 2, 0.0)).astype(np.float32) + pos_blend = np.sqrt(np.maximum(1.0 - self._disturb_pos_alpha**2, 0.0)).astype(np.float32) + rot_blend = np.sqrt(np.maximum(1.0 - self._disturb_rot_alpha**2, 0.0)).astype(np.float32) disturb_pos = ( - info["_disturb_pos_alpha"] * prev_pos + pos_blend * pos_noise_std * pos_noise + pos_jitter_std * pos_jitter + self._disturb_pos_alpha * prev_pos + pos_blend * pos_noise_std * pos_noise + pos_jitter_std * pos_jitter ) disturb_rot_deg = ( - info["_disturb_rot_alpha"] * prev_rot + rot_blend * rot_noise_std * rot_noise + rot_jitter_std * rot_jitter + self._disturb_rot_alpha * prev_rot + rot_blend * rot_noise_std * rot_noise + rot_jitter_std * rot_jitter ) disturb_pos = np.clip(disturb_pos, -pos_limit, pos_limit) disturb_rot_deg = np.clip(disturb_rot_deg, -rot_limit, rot_limit) - info["disturb_pos"] = disturb_pos.astype(np.float32) - info["disturb_rot_deg"] = disturb_rot_deg.astype(np.float32) - info["disturb_lin_vel"] = ((disturb_pos - prev_pos) / max(self._cfg.ctrl_dt, 1e-8)).astype(np.float32) - info["disturb_ang_vel_deg"] = ((disturb_rot_deg - prev_rot) / max(self._cfg.ctrl_dt, 1e-8)).astype(np.float32) + self._disturb_pos[:] = disturb_pos.astype(np.float32) + self._disturb_rot_deg[:] = disturb_rot_deg.astype(np.float32) + self._disturb_lin_vel[:] = ((disturb_pos - prev_pos) / max(self._cfg.ctrl_dt, 1e-8)).astype(np.float32) + self._disturb_ang_vel_deg[:] = ((disturb_rot_deg - prev_rot) / max(self._cfg.ctrl_dt, 1e-8)).astype(np.float32) - def _apply_disturbance_to_stage(self, env_ids: np.ndarray, info: dict) -> None: + def _apply_disturbance_to_stage(self, env_ids: np.ndarray) -> None: + num = len(env_ids) disturb_rot_deg = np.concatenate( - [info["disturb_rot_deg"], np.zeros((info["target_pos"].shape[0], 1), dtype=np.float32)], + [self._disturb_rot_deg[env_ids], np.zeros((num, 1), dtype=np.float32)], axis=-1, ) disturb_rot_rad = np.deg2rad(disturb_rot_deg).astype(np.float32) @@ -379,10 +408,10 @@ def _apply_disturbance_to_stage(self, env_ids: np.ndarray, info: dict) -> None: stage_quat = quaternion.from_euler( stage_euler_rad[..., 0], stage_euler_rad[..., 1], stage_euler_rad[..., 2] ).astype(np.float32) - stage_pos = self._stage_pos_init[None, :] + info["disturb_pos"] + stage_pos = self._stage_pos_init[None, :] + self._disturb_pos[env_ids] stage_ang_vel = np.deg2rad( np.concatenate( - [info["disturb_ang_vel_deg"], np.zeros((info["target_pos"].shape[0], 1), dtype=np.float32)], + [self._disturb_ang_vel_deg[env_ids], np.zeros((num, 1), dtype=np.float32)], axis=-1, ) ).astype(np.float32) @@ -391,32 +420,32 @@ def _apply_disturbance_to_stage(self, env_ids: np.ndarray, info: dict) -> None: env_ids, stage_pos, stage_quat, - info["disturb_lin_vel"], + self._disturb_lin_vel[env_ids], stage_ang_vel, ) - def _get_disturbance_obs(self, info: dict) -> np.ndarray | None: + def _get_disturbance_obs(self) -> np.ndarray | None: if not (self._cfg.disturbance_enabled and self._cfg.disturbance_include_obs): return None rot_obs_scale = max(float(self._cfg.disturb_rot_limit_deg), 1e-6) ang_vel_obs_scale = max(float(self._cfg.disturb_ang_vel_obs_scale_deg_per_s), 1e-6) return np.concatenate( [ - info["disturb_pos"], - info["disturb_lin_vel"], - info["disturb_rot_deg"] / rot_obs_scale, - info["disturb_ang_vel_deg"] / ang_vel_obs_scale, + self._disturb_pos, + self._disturb_lin_vel, + self._disturb_rot_deg / rot_obs_scale, + self._disturb_ang_vel_deg / ang_vel_obs_scale, ], axis=-1, ).astype(np.float32) - def _update_ball_kinematics(self, info: dict) -> dict[str, np.ndarray]: + def _update_ball_kinematics(self) -> dict[str, np.ndarray]: """Advance the ball-on-platform state estimates for the full batch. Reads fresh simulator quantities (already refreshed by the ``sim_data.execute()`` at the top of ``compute_transition``), updates - the filtered velocity estimates tracked in ``info``, and returns the - physical quantities shared by reward and termination. + the filtered velocity estimates tracked on the instance, and returns + the physical quantities shared by reward and termination. """ inputs = self.sim_data top_pos = inputs["top_pos"] @@ -425,20 +454,20 @@ def _update_ball_kinematics(self, info: dict) -> dict[str, np.ndarray]: quat_flat, rel_flat, rel_shape = _broadcast_quat_vec(top_quat, ball_pos - top_pos) rel = quaternion.rotate_inverse(quat_flat, rel_flat).reshape(*rel_shape, 3).astype(np.float32) - rel_vel = (rel - info["prev_rel"]) / self._cfg.ctrl_dt - info["prev_rel"] = rel.copy() - filtered_rel_vel = self._cfg.vel_smooth * rel_vel + (1.0 - self._cfg.vel_smooth) * info["filtered_rel_vel"] - info["filtered_rel_vel"] = filtered_rel_vel.astype(np.float32) - info["last_rel_vel"] = filtered_rel_vel.astype(np.float32) + rel_vel = (rel - self._prev_rel) / self._cfg.ctrl_dt + self._prev_rel[:] = rel + filtered_rel_vel = self._cfg.vel_smooth * rel_vel + (1.0 - self._cfg.vel_smooth) * self._filtered_rel_vel + self._filtered_rel_vel[:] = filtered_rel_vel.astype(np.float32) + self._last_rel_vel[:] = filtered_rel_vel.astype(np.float32) - quat_delta = quaternion.mul(top_quat, quaternion.conjugate(info["prev_top_quat"])).astype(np.float32) + quat_delta = quaternion.mul(top_quat, quaternion.conjugate(self._prev_top_quat)).astype(np.float32) top_ang_vel = _quat_to_rotvec(quat_delta) / self._cfg.ctrl_dt filtered_top_ang_vel = ( - self._cfg.vel_smooth * top_ang_vel + (1.0 - self._cfg.vel_smooth) * info["filtered_top_ang_vel"] + self._cfg.vel_smooth * top_ang_vel + (1.0 - self._cfg.vel_smooth) * self._filtered_top_ang_vel ) - info["filtered_top_ang_vel"] = filtered_top_ang_vel.astype(np.float32) - info["last_top_ang_vel"] = filtered_top_ang_vel.astype(np.float32) - info["prev_top_quat"] = top_quat.astype(np.float32) + self._filtered_top_ang_vel[:] = filtered_top_ang_vel.astype(np.float32) + self._last_top_ang_vel[:] = filtered_top_ang_vel.astype(np.float32) + self._prev_top_quat[:] = top_quat.astype(np.float32) roll_rad, pitch_rad, _ = quaternion.get_euler_xyz(top_quat) roll_deg = np.rad2deg(roll_rad).astype(np.float32) @@ -450,42 +479,40 @@ def _update_ball_kinematics(self, info: dict) -> dict[str, np.ndarray]: "tilt_deg": np.maximum(np.abs(roll_deg), np.abs(pitch_deg)).astype(np.float32), } - def _prepare_control_step(self, info: dict) -> None: - raw_action = np.asarray(info["policy_action"], dtype=np.float32) - action_exec, _ = self._smooth_actions(raw_action, info) - self._apply_pose_delta(info, action_exec) - self._update_disturbance_state(info, advance=True) + def _prepare_control_step(self) -> None: + self._smooth_actions(self._policy_action) + self._apply_pose_delta(self._action_exec) + self._update_disturbance_state(advance=True) all_ids = np.arange(self._num_envs, dtype=np.int64) - self._apply_disturbance_to_stage(all_ids, info) + self._apply_disturbance_to_stage(all_ids) self.sim_data.execute(all_ids) ctrl = self._ctrl_writes.buffer("ctrl") - ctrl[:] = self._compute_leg_ctrls(slice(None), info["target_pos"], info["target_quat"]) + ctrl[:] = self._compute_leg_ctrls(slice(None), self._target_pos, self._target_quat) self._ctrl_writes.execute() - def _simulate_control_step(self, info: dict) -> None: + def _simulate_control_step(self) -> None: all_ids = np.arange(self._num_envs, dtype=np.int64) for _ in range(self._cfg.sim_substeps): - self._apply_disturbance_to_stage(all_ids, info) + self._apply_disturbance_to_stage(all_ids) self.sim.step(1) - self._apply_disturbance_to_stage(all_ids, info) + self._apply_disturbance_to_stage(all_ids) def physics_step(self) -> None: # Stewart interleaves floating-base disturbance writes with per-substep # physics and FK-refreshed leg reads, so it owns the whole control step; # the post-step simulator refresh happens in compute_transition. - info = self._state.info - self._prepare_control_step(info) - self._simulate_control_step(info) + self._prepare_control_step() + self._simulate_control_step() - def _update_stillness(self, info: dict, rel_xy: np.ndarray, vel_xy: np.ndarray) -> np.ndarray: + def _update_stillness(self, rel_xy: np.ndarray, vel_xy: np.ndarray) -> np.ndarray: still_xy_enter = float(self._cfg.still_xy) still_vel_enter = float(self._cfg.still_vel) still_xy_exit = float(self._cfg.still_xy * self._cfg.still_xy_hysteresis) still_vel_exit = float(self._cfg.still_vel * self._cfg.still_vel_hysteresis) - still_window_active = info["still_window_active"].copy() - still_steps = info["still_steps"].copy() + still_window_active = self._still_window_active + still_steps = self._still_steps keep_mask = still_window_active & (rel_xy <= still_xy_exit) & (vel_xy <= still_vel_exit) break_mask = still_window_active & ~keep_mask @@ -499,27 +526,22 @@ def _update_stillness(self, info: dict, rel_xy: np.ndarray, vel_xy: np.ndarray) still_steps[enter_mask] = 1 still_steps[idle_mask] = 0 - info["still_window_active"] = still_window_active - info["still_steps"] = still_steps return still_steps def compute_observation(self, state: ArrayEnvState) -> ArrayEnvState: - info = state.info inputs = self.sim_data top_quat = _normalize_quat(inputs["top_quat"]) roll_rad, pitch_rad, _ = quaternion.get_euler_xyz(top_quat) roll_deg = np.rad2deg(roll_rad).astype(np.float32) pitch_deg = np.rad2deg(pitch_rad).astype(np.float32) - quat_flat, ang_vel_flat, ang_vel_shape = _broadcast_quat_vec( - top_quat, info["filtered_top_ang_vel"].astype(np.float32) - ) + quat_flat, ang_vel_flat, ang_vel_shape = _broadcast_quat_vec(top_quat, self._filtered_top_ang_vel) top_ang_vel_local = ( quaternion.rotate_inverse(quat_flat, ang_vel_flat).reshape(*ang_vel_shape, 3).astype(np.float32) ) obs_parts = [ - info["prev_rel"].astype(np.float32), - info["filtered_rel_vel"].astype(np.float32), + self._prev_rel, + self._filtered_rel_vel, np.stack( [ roll_deg / self._cfg.target_rotation_limit_deg, @@ -528,26 +550,25 @@ def compute_observation(self, state: ArrayEnvState) -> ArrayEnvState: axis=-1, ).astype(np.float32), top_ang_vel_local.astype(np.float32), - (info["target_tilt_cmd"] / max(self._cfg.target_rotation_limit_deg, 1e-6)).astype(np.float32), - info["action_exec"].astype(np.float32), + (self._target_tilt_cmd / max(self._cfg.target_rotation_limit_deg, 1e-6)).astype(np.float32), + self._action_exec, ] - disturb_obs = self._get_disturbance_obs(info) + disturb_obs = self._get_disturbance_obs() if disturb_obs is not None: obs_parts.append(disturb_obs) return state.replace(obs=np.concatenate(obs_parts, axis=-1).astype(np.float32)) def compute_transition(self, state: ArrayEnvState) -> ArrayEnvState: self.sim_data.execute() - info = state.info - state_cache = self._update_ball_kinematics(info) + state_cache = self._update_ball_kinematics() top_pos = state_cache["top_pos"] ball_pos = state_cache["ball_pos"] rel_xy = state_cache["rel_xy"] tilt_deg = state_cache["tilt_deg"] - rel_vel = info["last_rel_vel"].astype(np.float32) - top_ang_vel = info["last_top_ang_vel"].astype(np.float32) + rel_vel = self._last_rel_vel + top_ang_vel = self._last_top_ang_vel vel_xy = np.linalg.norm(rel_vel[:, :2], axis=-1).astype(np.float32) top_ang_mag = np.linalg.norm(top_ang_vel, axis=-1).astype(np.float32) @@ -557,9 +578,8 @@ def compute_transition(self, state: ArrayEnvState) -> ArrayEnvState: center_score = np.clip(1.0 - rel_xy / max(self._cfg.platform_radius, 1e-6), 0.0, 1.0).astype(np.float32) term_center = (self._cfg.k_center * center_score).astype(np.float32) - prev_zero_vel_rel_xy = info["prev_zero_vel_rel_xy"].astype(np.float32) - initial_rel_xy = info["initial_rel_xy"].astype(np.float32) - zero_reference = np.where(np.isfinite(prev_zero_vel_rel_xy), prev_zero_vel_rel_xy, initial_rel_xy) + prev_zero_vel_rel_xy = self._prev_zero_vel_rel_xy + zero_reference = np.where(np.isfinite(prev_zero_vel_rel_xy), prev_zero_vel_rel_xy, self._initial_rel_xy) zero_event = vel_xy <= self._cfg.zero_vel_thresh zero_closer_mask = zero_event & (rel_xy < zero_reference) zero_improve = np.maximum(zero_reference - rel_xy, 0.0) @@ -568,11 +588,9 @@ def compute_transition(self, state: ArrayEnvState) -> ArrayEnvState: np.float32 ) - next_prev_zero = prev_zero_vel_rel_xy.copy() - next_prev_zero[zero_event] = rel_xy[zero_event] - info["prev_zero_vel_rel_xy"] = next_prev_zero.astype(np.float32) + self._prev_zero_vel_rel_xy[zero_event] = rel_xy[zero_event] - still_steps = self._update_stillness(info, rel_xy, vel_xy) + still_steps = self._update_stillness(rel_xy, vel_xy) success = still_steps >= self._cfg.still_steps_needed term_still_bonus = np.where(success, self._cfg.k_still, 0.0).astype(np.float32) @@ -582,11 +600,12 @@ def compute_transition(self, state: ArrayEnvState) -> ArrayEnvState: reward = np.where(fallen, self._cfg.fall_penalty, reward).astype(np.float32) term_terminal[fallen] = self._cfg.fall_penalty + # Timeout flag for logging only; the framework owns truncation via its + # terminated/truncated bookkeeping. timeout = np.zeros((self._num_envs,), dtype=bool) if self._cfg.max_episode_steps is not None and self._cfg.max_episode_steps > 0: timeout = (state.episode_steps + 1) >= self._cfg.max_episode_steps timeout &= ~(fallen | success) - state.info["time_outs"] = timeout.astype(np.float32) terminated = (fallen | success).astype(bool) @@ -599,10 +618,10 @@ def compute_transition(self, state: ArrayEnvState) -> ArrayEnvState: "success": success.astype(np.float32), "fallen": fallen.astype(np.float32), "timeout": timeout.astype(np.float32), - "disturb_pos_norm": np.linalg.norm(info["disturb_pos"], axis=-1).astype(np.float32), - "disturb_rot_norm_deg": np.linalg.norm(info["disturb_rot_deg"], axis=-1).astype(np.float32), + "disturb_pos_norm": np.linalg.norm(self._disturb_pos, axis=-1).astype(np.float32), + "disturb_rot_norm_deg": np.linalg.norm(self._disturb_rot_deg, axis=-1).astype(np.float32), } - state.info["Reward"] = { + state.reward_terms = { "center": term_center.astype(np.float32), "zero_vel_reference": zero_reference.astype(np.float32), "zero_vel_improve": zero_improve.astype(np.float32), @@ -613,10 +632,9 @@ def compute_transition(self, state: ArrayEnvState) -> ArrayEnvState: return state.replace(reward=reward, terminated=terminated) - def reset(self, env_ids: np.ndarray) -> dict: + def reset(self, env_ids: np.ndarray) -> None: num = len(env_ids) row_ids = np.asarray(env_ids, dtype=np.int64) - zeros2 = np.zeros((num, 2), dtype=np.float32) zeros3 = np.zeros((num, 3), dtype=np.float32) roll_deg = np.random.uniform(self._cfg.min_init_tilt_deg, self._cfg.init_tilt_deg, size=(num,)).astype( @@ -637,27 +655,20 @@ def reset(self, env_ids: np.ndarray) -> dict: target_euler_rad[..., 0], target_euler_rad[..., 1], target_euler_rad[..., 2] ).astype(np.float32) - info = { - "target_pos": target_pos.copy(), - "target_quat": target_quat.copy(), - "target_tilt_cmd": np.stack([roll_deg, pitch_deg], axis=-1).astype(np.float32), - "prev_rel": np.zeros((num, 3), dtype=np.float32), - "filtered_rel_vel": np.zeros((num, 3), dtype=np.float32), - "last_rel_vel": np.zeros((num, 3), dtype=np.float32), - "prev_top_quat": _identity_quat((num,)), - "filtered_top_ang_vel": np.zeros((num, 3), dtype=np.float32), - "last_top_ang_vel": np.zeros((num, 3), dtype=np.float32), - "initial_rel_xy": np.zeros((num,), dtype=np.float32), - "prev_zero_vel_rel_xy": np.zeros((num,), dtype=np.float32), - "still_steps": np.zeros((num,), dtype=np.int32), - "still_window_active": np.zeros((num,), dtype=bool), - "policy_action": zeros2.copy(), - "prev_action_exec": zeros2.copy(), - "action_exec": zeros2.copy(), - "action_delta": zeros2.copy(), - "time_outs": np.zeros((num,), dtype=np.float32), - } - self._clear_disturbance_state(info) + # Write episode-scoped state for the reset rows + self._target_pos[env_ids] = target_pos + self._target_quat[env_ids] = target_quat + self._target_tilt_cmd[env_ids] = np.stack([roll_deg, pitch_deg], axis=-1) + self._prev_rel[env_ids] = 0.0 + self._filtered_rel_vel[env_ids] = 0.0 + self._last_rel_vel[env_ids] = 0.0 + self._filtered_top_ang_vel[env_ids] = 0.0 + self._last_top_ang_vel[env_ids] = 0.0 + self._still_steps[env_ids] = 0 + self._still_window_active[env_ids] = False + self._policy_action[env_ids] = 0.0 + self._prev_action_exec[env_ids] = 0.0 + self._action_exec[env_ids] = 0.0 stage_pose = np.concatenate( [np.tile(self._stage_pos_init, (num, 1)), _normalize_quat(np.tile(self._stage_quat_init, (num, 1)))], @@ -681,7 +692,7 @@ def reset(self, env_ids: np.ndarray) -> dict: # lengths follow analytically from the refreshed geometry — reset # never advances physics. self.sim_data.execute(row_ids) - leg_lengths = self._compute_leg_ctrls(env_ids, info["target_pos"], info["target_quat"]) + leg_lengths = self._compute_leg_ctrls(env_ids, target_pos, target_quat) self._reset_program.buffer("legs_position")[env_ids] = leg_lengths self._reset_program.execute(env_ids) @@ -709,15 +720,15 @@ def reset(self, env_ids: np.ndarray) -> dict: ball_pos = self.sim_data["ball_pos"][env_ids] top_pos = self.sim_data["top_pos"][env_ids] - info["prev_top_quat"] = _normalize_quat(self.sim_data["top_quat"][env_ids]) - quat_flat, rel_flat, rel_shape = _broadcast_quat_vec(info["prev_top_quat"], ball_pos - top_pos) - info["prev_rel"] = quaternion.rotate_inverse(quat_flat, rel_flat).reshape(*rel_shape, 3).astype(np.float32) + self._prev_top_quat[env_ids] = _normalize_quat(self.sim_data["top_quat"][env_ids]) + quat_flat, rel_flat, rel_shape = _broadcast_quat_vec(self._prev_top_quat[env_ids], ball_pos - top_pos) + self._prev_rel[env_ids] = ( + quaternion.rotate_inverse(quat_flat, rel_flat).reshape(*rel_shape, 3).astype(np.float32) + ) initial_rel_xy = self._compute_rel_xy(env_ids) - info["initial_rel_xy"] = initial_rel_xy.astype(np.float32) - info["prev_zero_vel_rel_xy"] = initial_rel_xy.astype(np.float32) + self._initial_rel_xy[env_ids] = initial_rel_xy + self._prev_zero_vel_rel_xy[env_ids] = initial_rel_xy - self._reset_episode_disturbance(info) - self._apply_disturbance_to_stage(env_ids, info) + self._reset_episode_disturbance(env_ids) + self._apply_disturbance_to_stage(env_ids) self.sim_data.execute(row_ids) - - return info diff --git a/motrix_envs/src/motrix_envs/basic/walker/walker_np.py b/motrix_envs/src/motrix_envs/basic/walker/walker_np.py index e317d838..86888c72 100644 --- a/motrix_envs/src/motrix_envs/basic/walker/walker_np.py +++ b/motrix_envs/src/motrix_envs/basic/walker/walker_np.py @@ -138,7 +138,7 @@ def compute_transition(self, state: ArrayEnvState) -> ArrayEnvState: rwd = rwd_stand - state.info["Reward"] = { + reward_terms = { "height": rwd_height, "upright": rwd_upright, "stand": rwd_stand, @@ -152,9 +152,10 @@ def compute_transition(self, state: ArrayEnvState) -> ArrayEnvState: value_at_margin=0.5, sigmoid="linear", ) - state.info["Reward"]["move"] = rwd_move + reward_terms["move"] = rwd_move rwd = rwd_stand * (5 * rwd_move + 1) / 6 + state.reward_terms = reward_terms rwd[terminated] = 0.0 return state.replace( @@ -162,7 +163,7 @@ def compute_transition(self, state: ArrayEnvState) -> ArrayEnvState: terminated=terminated, ) - def reset(self, env_ids: np.ndarray) -> dict: + def reset(self, env_ids: np.ndarray) -> None: num_reset = len(env_ids) dof_pos = np.zeros((num_reset, self._reset_position.shape[1])) @@ -178,12 +179,3 @@ def reset(self, env_ids: np.ndarray) -> dict: self._reset_velocity[env_ids] = dof_vel self._reset_program.execute(env_ids) self.sim_data.execute(np.asarray(env_ids, np.int64)) - rewards = { - "height": np.zeros((num_reset,)), - "upright": np.zeros((num_reset,)), - "stand": np.zeros((num_reset,)), - } - if self._move_speed > 0.0: - rewards["move"] = np.zeros((num_reset,)) - - return {"Reward": rewards} diff --git a/motrix_envs/src/motrix_envs/locomotion/anymal_c/anymal_c_np.py b/motrix_envs/src/motrix_envs/locomotion/anymal_c/anymal_c_np.py index 704d6468..25f8a1d0 100644 --- a/motrix_envs/src/motrix_envs/locomotion/anymal_c/anymal_c_np.py +++ b/motrix_envs/src/motrix_envs/locomotion/anymal_c/anymal_c_np.py @@ -114,6 +114,15 @@ def _init_buffer(self): self._init_joint_position = self.default_angles.copy() + # Episode-scoped navigation state: full-batch buffers, reset writes the + # done rows in place. + num_envs = self._num_envs + self._pose_commands = np.zeros((num_envs, 3), dtype=np.float32) + self._last_actions = np.zeros((num_envs, self._num_action), dtype=np.float32) + self._current_actions = np.zeros((num_envs, self._num_action), dtype=np.float32) + self._ever_reached = np.zeros(num_envs, dtype=bool) + self._min_distance = np.zeros(num_envs, dtype=np.float32) + def _init_contact_geometry(self): """Initialize geometry name pairs required for contact detection""" cfg = self._cfg @@ -180,10 +189,8 @@ def action_space(self): def apply_action(self, actions: np.ndarray, state: ArrayEnvState): # Save current action for incremental control - if "current_action" not in state.info: - state.info["current_actions"] = np.zeros_like(actions) - state.info["last_actions"] = state.info["current_actions"] - state.info["current_actions"] = actions + self._last_actions[:] = self._current_actions + self._current_actions[:] = actions # Position control mode: directly input target angles actions_scaled = actions * self._cfg.control_config.action_scale @@ -192,7 +199,7 @@ def apply_action(self, actions: np.ndarray, state: ArrayEnvState): self._ctrl_writes.execute() return state - def _navigation_state(self, info: dict): + def _navigation_state(self, pose_commands: np.ndarray): """Derive navigation commands from cached sim reads and pose commands. Pure physics-derived quantities shared by ``compute_transition`` @@ -201,7 +208,6 @@ def _navigation_state(self, info: dict): """ root_pos = self.sim_data["root_pos"] root_quat = self.sim_data["root_quat"] - pose_commands = info["pose_commands"] robot_position = root_pos[:, :2] robot_heading = quaternion.get_yaw(root_quat) @@ -232,7 +238,7 @@ def _navigation_state(self, info: dict): return position_error, heading_diff, distance_to_target, reached_all, desired_vel_xy, velocity_commands def compute_observation(self, state: ArrayEnvState) -> ArrayEnvState: - """Build the full observation from cached sim reads and info.""" + """Build the full observation from cached sim reads and episode state.""" inputs = self.sim_data gyro = inputs["base_gyro"] projected_gravity = self._compute_projected_gravity(inputs["root_quat"]) @@ -240,7 +246,7 @@ def compute_observation(self, state: ArrayEnvState) -> ArrayEnvState: # Navigation quantities recomputed from the same cached reads, so # freshly reset rows observe their post-reset command state. (position_error, heading_diff, distance_to_target, reached_all, _, velocity_commands) = self._navigation_state( - state.info + self._pose_commands ) # Normalize observations @@ -249,7 +255,7 @@ def compute_observation(self, state: ArrayEnvState) -> ArrayEnvState: noisy_joint_angle = (inputs["robot_joint_pos"] - self.default_angles) * self._cfg.normalization.dof_pos noisy_joint_vel = inputs["robot_joint_vel"] * self._cfg.normalization.dof_vel command_normalized = velocity_commands * self.commands_scale - last_actions = state.info["current_actions"] + last_actions = self._current_actions # Calculate task-related observations position_error_normalized = position_error / 5.0 # Normalize to reasonable range @@ -289,18 +295,17 @@ def compute_transition(self, state: ArrayEnvState): base_lin_vel = self.sim_data["base_linvel"][:, :3] # Navigation quantities derived directly from the cached reads - (_, _, _, _, desired_vel_xy, velocity_commands) = self._navigation_state(state.info) - state.info["desired_vel_xy"] = desired_vel_xy + (_, _, _, _, desired_vel_xy, velocity_commands) = self._navigation_state(self._pose_commands) # Update target position marker num_envs = self._num_envs - self._update_target_marker(np.arange(num_envs, dtype=np.int64), state.info["pose_commands"]) + self._update_target_marker(np.arange(num_envs, dtype=np.int64), self._pose_commands) # Update arrow visualization (no physical effect) base_lin_vel_xy = base_lin_vel[:, :2] self._update_heading_arrows(np.arange(num_envs, dtype=np.int64), root_pos, desired_vel_xy, base_lin_vel_xy) # Calculate reward - state.reward = self._compute_reward(state.info, velocity_commands) + state.reward = self._compute_reward(velocity_commands) # Calculate termination conditions state = self._compute_terminated(state) @@ -340,7 +345,7 @@ def _update_heading_arrows( # Both heading markers go to the backend in one crossing. self._heading_writes.execute(env_ids) - def _compute_reward(self, info: dict, velocity_commands: np.ndarray) -> np.ndarray: + def _compute_reward(self, velocity_commands: np.ndarray) -> np.ndarray: """ Velocity tracking reward mechanism velocity_commands: [num_envs, 3] - (vx, vy, vyaw) @@ -382,20 +387,17 @@ def _compute_reward(self, info: dict, velocity_commands: np.ndarray) -> np.ndarr # Get robot position and heading for arrival determination, derived # directly from the cached reads and pose commands - (_, _, distance_to_target, reached_all, _, _) = self._navigation_state(info) + (_, _, distance_to_target, reached_all, _, _) = self._navigation_state(self._pose_commands) # One-time reward for first time reaching position - info["ever_reached"] = info.get("ever_reached", np.zeros(num_envs, dtype=bool)) - first_time_reach = np.logical_and(reached_all, ~info["ever_reached"]) - info["ever_reached"] = np.logical_or(info["ever_reached"], reached_all) + first_time_reach = np.logical_and(reached_all, ~self._ever_reached) + self._ever_reached |= reached_all arrival_bonus = np.where(first_time_reach, 10.0, 0.0) # Distance approach reward: incentivize getting closer to target # Use historical minimum distance to calculate progress - if "min_distance" not in info: - info["min_distance"] = distance_to_target.copy() - distance_improvement = info["min_distance"] - distance_to_target - info["min_distance"] = np.minimum(info["min_distance"], distance_to_target) + distance_improvement = self._min_distance - distance_to_target + np.minimum(self._min_distance, distance_to_target, out=self._min_distance) approach_reward = np.clip(distance_improvement * 4.0, -1.0, 1.0) # Reward 5 points for every 1 meter closer # 3. Orientation stability reward (penalize deviation from normal standing posture) @@ -428,7 +430,7 @@ def _compute_reward(self, info: dict, velocity_commands: np.ndarray) -> np.ndarr dof_vel_penalty = np.sum(np.square(joint_vel), axis=1) # 8. Action change penalty - action_diff = info["current_actions"] - info["last_actions"] + action_diff = self._current_actions - self._last_actions action_rate_penalty = np.sum(np.square(action_diff), axis=1) # Combined reward @@ -506,7 +508,7 @@ def _compute_terminated(self, state: ArrayEnvState) -> ArrayEnvState: return state.replace(terminated=terminated) - def reset(self, env_ids: np.ndarray) -> dict: + def reset(self, env_ids: np.ndarray) -> None: cfg: AnymalCEnvCfg = self._cfg num_envs = len(env_ids) @@ -606,15 +608,12 @@ def reset(self, env_ids: np.ndarray) -> dict: if desired_yaw_rate.ndim > 1: desired_yaw_rate = desired_yaw_rate.flatten() - info = { - "pose_commands": pose_commands, - "last_actions": np.zeros((num_envs, self._num_action), dtype=np.float32), - "current_actions": np.zeros((num_envs, self._num_action), dtype=np.float32), - "ever_reached": np.zeros(num_envs, dtype=bool), - "min_distance": distance_to_target.copy(), # Initialize minimum distance - } - - return info + # Write episode-scoped state for the reset rows + self._pose_commands[rows] = pose_commands + self._last_actions[rows] = 0.0 + self._current_actions[rows] = 0.0 + self._ever_reached[rows] = False + self._min_distance[rows] = distance_to_target def _compute_projected_gravity(self, quat: np.ndarray) -> np.ndarray: gravity = np.array([0.0, 0.0, -1.0], dtype=np.float32) diff --git a/motrix_envs/src/motrix_envs/locomotion/go1/walk_stairs_terrain.py b/motrix_envs/src/motrix_envs/locomotion/go1/walk_stairs_terrain.py index aa5ffbe2..ed06b8be 100644 --- a/motrix_envs/src/motrix_envs/locomotion/go1/walk_stairs_terrain.py +++ b/motrix_envs/src/motrix_envs/locomotion/go1/walk_stairs_terrain.py @@ -165,6 +165,17 @@ def _init_buffer(self): self.num_check = self.sim_data["termination_colliding"].shape[-1] self.foot_check_num = self.sim_data["foot_colliding"].shape[-1] + # Episode-scoped task state: full-batch buffers, reset writes the done + # rows in place. + num_envs = self._num_envs + self._commands = np.zeros((num_envs, 3), dtype=np.float32) + self._contacts = np.zeros((num_envs, self.foot_check_num), dtype=np.bool_) + self._contact_force = np.zeros((num_envs, 12), dtype=np.float32) + self._feet_air_time = np.zeros((num_envs, self.foot_check_num), dtype=np.float32) + self._last_dof_vel = np.zeros((num_envs, self._num_action), dtype=np.float32) + self._current_actions = np.zeros((num_envs, self._num_action), dtype=np.float32) + self._last_actions = np.zeros((num_envs, self._num_action), dtype=np.float32) + spacing = 2.0 cols = int(np.ceil(np.sqrt(self._num_envs))) offsets = [] @@ -178,11 +189,11 @@ def _init_buffer(self): self.offsets = np.array(offsets) def apply_action(self, actions, state): - # Copy: the inputs slice is a view over the shared read buffer, which - # the upcoming physics-step read overwrites in place. - state.info["last_dof_vel"] = self.get_dof_vel().copy() - state.info["last_actions"] = state.info["current_actions"] - state.info["current_actions"] = actions + # Copy values: the read buffer is a view that the upcoming physics-step + # read overwrites in place. + self._last_dof_vel[:] = self.get_dof_vel() + self._last_actions[:] = self._current_actions + self._current_actions[:] = actions ctrl = self._ctrl_writes.buffer("ctrl") ctrl[:] = np.asarray(self._compute_torques(actions), dtype=np.float32) self._ctrl_writes.execute() @@ -202,7 +213,7 @@ def get_gyro(self) -> np.ndarray: return self.sim_data["gyro"] def compute_observation(self, state: ArrayEnvState): - """Build the full observation from cached sim reads and info.""" + """Build the full observation from cached sim reads and episode state.""" inputs = self.sim_data linear_vel = inputs["local_linvel"] gyro = inputs["gyro"] @@ -213,9 +224,9 @@ def compute_observation(self, state: ArrayEnvState): noisy_gyro = gyro * self.cfg.normalization.ang_vel noisy_joint_angle = diff * self.cfg.normalization.dof_pos noisy_joint_vel = inputs["robot_joint_vel"] * self.cfg.normalization.dof_vel - command = state.info["commands"] * self.commands_scale - last_actions = state.info["current_actions"] - contact_force = state.info["contact_force"] + command = self._commands * self.commands_scale + last_actions = self._current_actions + contact_force = self._contact_force obs = np.hstack( [ @@ -234,9 +245,9 @@ def compute_observation(self, state: ArrayEnvState): def compute_transition(self, state): self.sim_data.execute() # Contact bookkeeping is reward state derived from the refreshed cache. - state.info["contacts"] = self.sim_data["foot_colliding"].astype(bool) - state.info["feet_air_time"] = self.update_feet_air_time(state.info) - state.info["contact_force"] = self.update_contact_force(state) + self._contacts[:] = self.sim_data["foot_colliding"] + self.update_feet_air_time() + self.update_contact_force() state = self.update_terminated(state) state = self.update_reward(state) return state @@ -251,13 +262,11 @@ def update_terminated(self, state: ArrayEnvState) -> ArrayEnvState: terminated=terminated, ) - def update_feet_air_time(self, info: dict): - feet_air_time = info["feet_air_time"] - feet_air_time += self.cfg.ctrl_dt - feet_air_time *= ~info["contacts"] - return feet_air_time + def update_feet_air_time(self): + self._feet_air_time += self.cfg.ctrl_dt + self._feet_air_time *= ~self._contacts - def update_contact_force(self, state: ArrayEnvState): + def update_contact_force(self): base_quat = self.sim_data["root_quat"] foot_forces = self.sim_data["foot_contact_forces"] force = [] @@ -265,7 +274,7 @@ def update_contact_force(self, state: ArrayEnvState): contact_force = foot_forces[:, 3 * k : 3 * k + 3] contact_force = quaternion.rotate_inverse(base_quat, contact_force) force.append(contact_force) - return np.concatenate(force, axis=1) + self._contact_force[:] = np.concatenate(force, axis=1) def resample_commands(self, num_envs: int): commands = np.random.uniform( @@ -278,7 +287,7 @@ def resample_commands(self, num_envs: int): def update_reward(self, state: ArrayEnvState) -> ArrayEnvState: terminated = state.terminated - reward_dict = self._get_reward(state.info) + reward_dict = self._get_reward() rewards = {k: v * self.cfg.reward_config.scales[k] for k, v in reward_dict.items()} rwd = sum(rewards.values()) @@ -291,7 +300,7 @@ def update_reward(self, state: ArrayEnvState) -> ArrayEnvState: return state.replace(reward=rwd) - def reset(self, env_ids: np.ndarray) -> dict: + def reset(self, env_ids: np.ndarray) -> None: num_reset = len(env_ids) base_pose = np.tile(self._init_base_pose, (num_reset, 1)) @@ -311,36 +320,30 @@ def reset(self, env_ids: np.ndarray) -> dict: self._reset_program.execute(env_ids) self.sim_data.execute(np.asarray(env_ids, dtype=np.int64)) - info = { - "current_actions": np.zeros((num_reset, self._num_action), dtype=np.float32), - "last_actions": np.zeros((num_reset, self._num_action), dtype=np.float32), - "commands": self.resample_commands(num_reset), - "last_dof_vel": np.zeros((num_reset, self._num_action), dtype=np.float32), - "feet_air_time": np.zeros((num_reset, self.foot_check_num), dtype=np.float32), - "contacts": np.zeros((num_reset, self.foot_check_num), dtype=np.bool_), - "contact_force": np.zeros((num_reset, 12), dtype=np.float32), - } - return info + self._commands[env_ids] = self.resample_commands(num_reset) + self._current_actions[env_ids] = 0.0 + self._last_actions[env_ids] = 0.0 + self._last_dof_vel[env_ids] = 0.0 + self._feet_air_time[env_ids] = 0.0 + self._contacts[env_ids] = False + self._contact_force[env_ids] = 0.0 - def _get_reward( - self, - info: dict, - ) -> dict[str, np.ndarray]: - commands = info["commands"] + def _get_reward(self) -> dict[str, np.ndarray]: + commands = self._commands return { "lin_vel_z": self._reward_lin_vel_z(), "ang_vel_xy": self._reward_ang_vel_xy(), "orientation": self._reward_orientation(), "torques": self._reward_torques(), "dof_vel": self._reward_dof_vel(), - "dof_acc": self._reward_dof_acc(info), - "action_rate": self._reward_action_rate(info), + "dof_acc": self._reward_dof_acc(), + "action_rate": self._reward_action_rate(), "tracking_lin_vel": self._reward_tracking_lin_vel(commands), "tracking_ang_vel": self._reward_tracking_ang_vel(commands), "stand_still": self._reward_stand_still(commands), "hip_pos": self._reward_hip_pos(commands), "calf_pos": self._reward_calf_pos(commands), - "feet_air_time": self._reward_feet_air_time(commands, info), + "feet_air_time": self._reward_feet_air_time(commands), "feet_stumble": self._reward_feet_stumble(), } @@ -367,26 +370,26 @@ def _reward_dof_vel(self): # Penalize dof velocities return np.sum(np.square(self.get_dof_vel()), axis=1) - def _reward_dof_acc(self, info): + def _reward_dof_acc(self): # Penalize dof accelerations return np.sum( - np.square((info["last_dof_vel"] - self.get_dof_vel()) / self.cfg.ctrl_dt), + np.square((self._last_dof_vel - self.get_dof_vel()) / self.cfg.ctrl_dt), axis=1, ) - def _reward_action_rate(self, info: dict): + def _reward_action_rate(self): # Penalize changes in actions - action_diff = info["current_actions"] - info["last_actions"] + action_diff = self._current_actions - self._last_actions return np.sum(np.square(action_diff), axis=1) def _reward_termination(self, done): # Terminal reward / penalty return done - def _reward_feet_air_time(self, commands: np.ndarray, info: dict): + def _reward_feet_air_time(self, commands: np.ndarray): # Reward long steps - feet_air_time = info["feet_air_time"] - first_contact = (feet_air_time > 0.0) * info["contacts"] + feet_air_time = self._feet_air_time + first_contact = (feet_air_time > 0.0) * self._contacts # reward only on first contact with the ground rew_airTime = np.sum((feet_air_time - 0.5) * first_contact, axis=1) # no reward for zero command diff --git a/motrix_envs/src/motrix_envs/locomotion/humanoid/walk_manager_mdp/command.py b/motrix_envs/src/motrix_envs/locomotion/humanoid/walk_manager_mdp/command.py index ceec8c71..aa63ab7e 100644 --- a/motrix_envs/src/motrix_envs/locomotion/humanoid/walk_manager_mdp/command.py +++ b/motrix_envs/src/motrix_envs/locomotion/humanoid/walk_manager_mdp/command.py @@ -58,7 +58,7 @@ def _lane_resample_command(ctx, commands, low, high, stand_prob) -> None: class WalkCommand(CommandTerm): """Per-environment velocity command and gait-phase clock. - Mirrors the direct env's ``info["commands"]`` / ``info["phase"]`` state: + Mirrors the direct env's ``commands`` / ``phase`` episode state: commands resample every ``resample_steps`` transitions, the phase advances by ``phase_dt`` per step from a per-env offset, and standing commands pin the phase to ``pi``. diff --git a/motrix_envs/src/motrix_envs/locomotion/quadruped/walk_np.py b/motrix_envs/src/motrix_envs/locomotion/quadruped/walk_np.py index e1690404..3a252459 100644 --- a/motrix_envs/src/motrix_envs/locomotion/quadruped/walk_np.py +++ b/motrix_envs/src/motrix_envs/locomotion/quadruped/walk_np.py @@ -221,6 +221,16 @@ def __init__(self, cfg: QuadrupedWalkEnvCfg, num_envs=1, backend: str | None = N self.feet_contact = np.zeros((num_envs, self._num_feet), dtype=bool) self.feet_pos = np.zeros((num_envs, self._num_feet, 3), dtype=np.float32) + # Episode-scoped task state: full-batch buffers, reset writes the done + # rows in place. + self._commands = np.zeros((num_envs, 3), dtype=np.float32) + self._command_resampling_time = np.full(num_envs, np.inf, dtype=np.float32) + self._phase = np.zeros(num_envs, dtype=np.float32) + self._feet_phase = np.zeros((num_envs, self._num_feet), dtype=np.float32) + self._current_actions = np.zeros((num_envs, self._num_action), dtype=np.float32) + self._last_actions = np.zeros((num_envs, self._num_action), dtype=np.float32) + self._action_delay_steps = np.zeros(num_envs, dtype=np.int32) + # ---- obs/action space plumbing ---------------------------------------- def _policy_obs_dim(self) -> int: @@ -287,21 +297,20 @@ def _read_feet_contact(self, env_ids: np.ndarray | None = None) -> np.ndarray: contacts[rows, i] = value[:, 0] > 0.0 return contacts - def _update_feet_buffers(self, env_ids: np.ndarray | None = None) -> np.ndarray: + def _update_feet_buffers(self, env_ids: np.ndarray | None = None) -> None: contacts = self._read_feet_contact(env_ids) rows = slice(None) if env_ids is None else env_ids self.feet_contact[rows, :] = contacts[rows, :] for i, name in enumerate(self._feet_position_sensors): self.feet_pos[rows, i, :] = self.sim_data[name][rows] - return contacts if env_ids is None else contacts[env_ids] - def _advance_phase(self, info: dict): - commands = info["commands"] + def _advance_phase(self): + commands = self._commands standing = np.linalg.norm(commands, axis=1) < self.cfg.commands.velocity.standing_threshold - phase = info["phase"] - phase = np.fmod(phase + self.cfg.ctrl_dt * self.cfg.gait_frequency, 1.0).astype(np.float32, copy=False) + phase = self._phase + np.fmod(phase + self.cfg.ctrl_dt * self.cfg.gait_frequency, 1.0, out=phase) phase[standing] = 0.0 - feet_phase = info["feet_phase"] + feet_phase = self._feet_phase # Each trot pair swings together; pair ``n`` is offset by half a cycle # relative to pair ``n-1``. for pair_idx, (i, j) in enumerate(self.cfg.trot_pairs): @@ -310,20 +319,18 @@ def _advance_phase(self, info: dict): feet_phase[:, i] = value feet_phase[:, j] = value feet_phase[standing] = 0.0 - info["phase"] = phase - info["feet_phase"] = feet_phase # ---- env loop --------------------------------------------------------- def apply_action(self, actions: np.ndarray, state: ArrayEnvState) -> ArrayEnvState: actions = np.asarray(actions, dtype=np.float32) - state.info["last_actions"] = state.info["current_actions"] - state.info["current_actions"] = actions + self._last_actions[:] = self._current_actions + self._current_actions[:] = actions if self._randomize_action_delay: - use_last = state.info["action_delay_steps"] == 1 - exec_actions = np.where(use_last[:, None], state.info["last_actions"], actions) + use_last = self._action_delay_steps == 1 + exec_actions = np.where(use_last[:, None], self._last_actions, actions) else: - exec_actions = state.info["last_actions"] if self.cfg.control_config.simulate_action_latency else actions + exec_actions = self._last_actions if self.cfg.control_config.simulate_action_latency else actions targets = exec_actions * self.cfg.control_config.action_scale + self.default_angles ctrl = self._ctrl_writes.buffer("ctrl") ctrl[:] = targets.astype(np.float32, copy=False) @@ -338,9 +345,9 @@ def compute_observation(self, state: ArrayEnvState) -> ArrayEnvState: noisy_diff = self._obs_noise(diff, noise_cfg.scale_joint_angle) noisy_dof_vel = self._obs_noise(self.get_dof_vel(), noise_cfg.scale_joint_vel) noisy_linvel = self._obs_noise(self.get_local_linvel(), noise_cfg.scale_linvel) - command = state.info["commands"] - last_actions = state.info["current_actions"] - feet_phase = state.info["feet_phase"] + command = self._commands + last_actions = self._current_actions + feet_phase = self._feet_phase policy = np.concatenate( [ @@ -367,15 +374,17 @@ def compute_observation(self, state: ArrayEnvState) -> ArrayEnvState: def compute_transition(self, state: ArrayEnvState) -> ArrayEnvState: self.sim_data.execute() - self._update_commands(state.info) - self._advance_phase(state.info) - state.info["contacts"] = self._update_feet_buffers() + self._update_commands() + self._advance_phase() + self._update_feet_buffers() terminated = self.get_gravity()[:, 2] <= 0.5 - reward = self._compute_reward(state.info, self.get_local_linvel(), self.get_gyro(), self.get_dof_pos()) + reward = self._compute_reward(state, self.get_local_linvel(), self.get_gyro(), self.get_dof_pos()) return state.replace(reward=reward, terminated=terminated) - def _compute_reward(self, info: dict, linvel: np.ndarray, gyro: np.ndarray, dof_pos: np.ndarray) -> np.ndarray: + def _compute_reward( + self, state: ArrayEnvState, linvel: np.ndarray, gyro: np.ndarray, dof_pos: np.ndarray + ) -> np.ndarray: reward = np.zeros((self._num_envs,), dtype=np.float32) reward_cfg = self.cfg.reward_config reward_fns = { @@ -395,15 +404,15 @@ def _compute_reward(self, info: dict, linvel: np.ndarray, gyro: np.ndarray, dof_ scale = getattr(reward_cfg.scales, name) if scale == 0: continue - rew = reward_fn(info, linvel, gyro, dof_pos) + rew = reward_fn(linvel, gyro, dof_pos) weighted = (rew * scale).astype(np.float32, copy=False) reward += weighted reward_items[name] = weighted - info["Reward"] = reward_items + state.reward_terms = reward_items return reward * self.cfg.ctrl_dt - def reset(self, env_ids: np.ndarray): + def reset(self, env_ids: np.ndarray) -> None: num_reset = len(env_ids) base_pose = np.tile(self._init_base_pose, (num_reset, 1)) base_linear_velocity = np.zeros((num_reset, 3), dtype=np.float32) @@ -429,23 +438,17 @@ def reset(self, env_ids: np.ndarray): self._reset_joint_position[env_ids] = joint_position self._reset_joint_velocity[env_ids] = joint_velocity self._reset_program.execute(env_ids) - action_delay_steps = self._randomize_params(env_ids) + self._randomize_params(env_ids) self.sim_data.execute(np.asarray(env_ids, dtype=np.int64)) - contacts = self._update_feet_buffers(env_ids) - - info = { - "current_actions": np.zeros((num_reset, self._num_action), dtype=np.float32), - "last_actions": np.zeros((num_reset, self._num_action), dtype=np.float32), - "commands": self.resample_commands(num_reset), - "command_resampling_time": self._sample_command_resampling_time(num_reset), - "phase": np.zeros((num_reset,), dtype=np.float32), - "feet_phase": np.zeros((num_reset, self._num_feet), dtype=np.float32), - "contacts": contacts, - } - if action_delay_steps is not None: - info["action_delay_steps"] = action_delay_steps - return info + self._update_feet_buffers(env_ids) + + self._commands[env_ids] = self.resample_commands(num_reset) + self._command_resampling_time[env_ids] = self._sample_command_resampling_time(num_reset) + self._phase[env_ids] = 0.0 + self._feet_phase[env_ids] = 0.0 + self._current_actions[env_ids] = 0.0 + self._last_actions[env_ids] = 0.0 def resample_commands(self, num_envs: int) -> np.ndarray: return self._velocity_command_binding.read_command(batch_size=num_envs).values.copy() @@ -456,16 +459,16 @@ def _sample_command_resampling_time(self, num_envs: int) -> np.ndarray: return np.full((num_envs,), np.inf, dtype=np.float32) return self._command_rng.uniform(*interval, size=num_envs).astype(np.float32) - def _update_commands(self, info: dict) -> None: + def _update_commands(self) -> None: if self.cfg.commands.velocity.resampling_seconds_range is None: return - remaining = info["command_resampling_time"] - self.cfg.ctrl_dt + remaining = self._command_resampling_time + remaining -= self.cfg.ctrl_dt due = remaining <= 0.0 num_due = int(np.count_nonzero(due)) if num_due: - info["commands"][due] = self.resample_commands(num_due) + self._commands[due] = self.resample_commands(num_due) remaining[due] = self._sample_command_resampling_time(num_due) - info["command_resampling_time"] = remaining.astype(np.float32, copy=False) def _randomize_dof_noise( self, @@ -512,26 +515,25 @@ def _randomize_dof_noise( -base_ang_vel_noise, base_ang_vel_noise, size=(num_reset, 3) ).astype(np.float32) - def _randomize_params(self, env_ids: np.ndarray) -> np.ndarray | None: + def _randomize_params(self, env_ids: np.ndarray) -> None: num_reset = len(env_ids) randomization = self.cfg.randomization if not randomization.enabled: - return None + return rng = self._randomization_rng num_action = self._num_action - action_delay_steps = None if self._randomize_action_delay: delay_low, delay_high = randomization.action_delay_steps if delay_low == delay_high: - action_delay_steps = np.full((num_reset,), delay_low, dtype=np.int32) + self._action_delay_steps[env_ids] = delay_low else: - action_delay_steps = rng.integers(delay_low, delay_high + 1, size=num_reset, dtype=np.int32) + self._action_delay_steps[env_ids] = rng.integers(delay_low, delay_high + 1, size=num_reset) # All override writes for this reset batch into one program execution. program = self._randomize_writes if program is None: - return action_delay_steps + return if self._randomize_kp: kp_scale = rng.uniform(*self.cfg.randomization.kp_scale_range, size=(num_reset, num_action)).astype( @@ -560,52 +562,50 @@ def _randomize_params(self, env_ids: np.ndarray) -> np.ndarray | None: program.buffer("com")[env_ids, 0] = np.tile(self.model.others["base_com"], (num_reset, 1)) + base_com_offset program.execute(env_ids) - return action_delay_steps - # ---- reward functions ------------------------------------------------- - def _reward_tracking_lin_vel(self, info, linvel, gyro, dof_pos) -> np.ndarray: + def _reward_tracking_lin_vel(self, linvel, gyro, dof_pos) -> np.ndarray: del gyro, dof_pos - commands = info["commands"] + commands = self._commands error = np.sum(np.square(commands[:, :2] - linvel[:, :2]), axis=1) return np.exp(-error / self.cfg.reward_config.tracking_lin_vel_sigma) - def _reward_tracking_ang_vel(self, info, linvel, gyro, dof_pos) -> np.ndarray: + def _reward_tracking_ang_vel(self, linvel, gyro, dof_pos) -> np.ndarray: del linvel, dof_pos - commands = info["commands"] + commands = self._commands error = np.square(commands[:, 2] - gyro[:, 2]) return np.exp(-error / self.cfg.reward_config.tracking_ang_vel_sigma) - def _reward_lin_vel_z(self, info, linvel, gyro, dof_pos) -> np.ndarray: - del info, gyro, dof_pos + def _reward_lin_vel_z(self, linvel, gyro, dof_pos) -> np.ndarray: + del gyro, dof_pos return np.square(linvel[:, 2]) - def _reward_ang_vel_xy(self, info, linvel, gyro, dof_pos) -> np.ndarray: - del info, linvel, dof_pos + def _reward_ang_vel_xy(self, linvel, gyro, dof_pos) -> np.ndarray: + del linvel, dof_pos return np.sum(np.square(gyro[:, :2]), axis=1) - def _reward_base_height(self, info, linvel, gyro, dof_pos) -> np.ndarray: - del info, linvel, gyro, dof_pos + def _reward_base_height(self, linvel, gyro, dof_pos) -> np.ndarray: + del linvel, gyro, dof_pos base_pos = self.sim_data["base_pos"] env_ids = np.arange(self._num_envs, dtype=np.int64) ground_height = self.sim.sample_terrain_height(self.cfg.ground_geom_name, env_ids, base_pos[:, None, :2])[:, 0] base_height = base_pos[:, 2].astype(np.float32) - ground_height return np.square(base_height - self.cfg.reward_config.base_height_target) - def _reward_action_rate(self, info, linvel, gyro, dof_pos) -> np.ndarray: + def _reward_action_rate(self, linvel, gyro, dof_pos) -> np.ndarray: del linvel, gyro, dof_pos - current = info["current_actions"] - last = info["last_actions"] + current = self._current_actions + last = self._last_actions return np.sum(np.square(current - last), axis=1) - def _reward_similar_to_default(self, info, linvel, gyro, dof_pos) -> np.ndarray: - del info, linvel, gyro + def _reward_similar_to_default(self, linvel, gyro, dof_pos) -> np.ndarray: + del linvel, gyro return np.sum(np.abs(dof_pos - self.default_angles), axis=1) - def _reward_swing_feet_z(self, info, linvel, gyro, dof_pos) -> np.ndarray: + def _reward_swing_feet_z(self, linvel, gyro, dof_pos) -> np.ndarray: del linvel, gyro, dof_pos - feet_phase = info["feet_phase"] - contacts = info["contacts"] + feet_phase = self._feet_phase + contacts = self.feet_contact valid_swing = (feet_phase >= 0.6) & ~contacts reward_cfg = self.cfg.reward_config # feet_pos is body-relative (framepos with ref=imu). The foot lifts @@ -618,20 +618,20 @@ def _reward_swing_feet_z(self, info, linvel, gyro, dof_pos) -> np.ndarray: swing_rew = np.exp(-height_error / sigma_sq) * valid_swing return np.sum(swing_rew, axis=1) / self._num_feet - def _reward_contact(self, info, linvel, gyro, dof_pos) -> np.ndarray: + def _reward_contact(self, linvel, gyro, dof_pos) -> np.ndarray: del linvel, gyro, dof_pos res = np.zeros((self._num_envs,), dtype=np.float32) - feet_phase = info["feet_phase"] - contacts = info["contacts"] + feet_phase = self._feet_phase + contacts = self.feet_contact for i in range(self._num_feet): target_contact = feet_phase[:, i] < 0.6 res += (contacts[:, i] == target_contact).astype(np.float32) return res / self._num_feet - def _reward_swing_contact(self, info, linvel, gyro, dof_pos) -> np.ndarray: + def _reward_swing_contact(self, linvel, gyro, dof_pos) -> np.ndarray: del linvel, gyro, dof_pos - is_swing = info["feet_phase"] >= 0.6 - swing_contacts = info["contacts"] & is_swing + is_swing = self._feet_phase >= 0.6 + swing_contacts = self.feet_contact & is_swing return np.sum(swing_contacts, axis=1) / self._num_feet diff --git a/motrix_envs/src/motrix_envs/manipulation/franka_lift_cube/franka_lift_cube_np.py b/motrix_envs/src/motrix_envs/manipulation/franka_lift_cube/franka_lift_cube_np.py index 9604dc9c..fd9e9f77 100755 --- a/motrix_envs/src/motrix_envs/manipulation/franka_lift_cube/franka_lift_cube_np.py +++ b/motrix_envs/src/motrix_envs/manipulation/franka_lift_cube/franka_lift_cube_np.py @@ -82,6 +82,13 @@ def __init__(self, cfg: FrankaLiftCubeEnvCfg, num_envs=1, backend: str | None = self.count = 0 + # Episode-scoped task state: full-batch buffers, reset writes the done + # rows in place. + num_envs = self._num_envs + self._commands = np.zeros((num_envs, 3), dtype=np.float32) + self._current_actions = np.zeros((num_envs, self._action_dim), dtype=np.float32) + self._last_actions = np.zeros((num_envs, self._action_dim), dtype=np.float32) + @property def observation_space(self): return self._observation_space @@ -91,8 +98,8 @@ def action_space(self): return self._action_space def apply_action(self, actions: np.ndarray, state: ArrayEnvState): - state.info["last_actions"] = state.info["current_actions"] - state.info["current_actions"] = actions + self._last_actions[:] = self._current_actions + self._current_actions[:] = actions # no gripper old_joint_pos = self.get_dof_pos(slice(None))[:, : self._action_dim - 1] @@ -107,7 +114,6 @@ def apply_action(self, actions: np.ndarray, state: ArrayEnvState): sampled_gripper_action = np.where(probabilities > np.random.rand(*probabilities.shape), 0, 0.04)[ :, None ] # Close 0, Open 0.04 - state.info["current_gripper_action"] = sampled_gripper_action.squeeze(axis=-1) new_pos = np.concatenate([new_joint_pos, sampled_gripper_action], axis=-1) @@ -126,10 +132,9 @@ def compute_observation(self, state: ArrayEnvState): """Build the full observation batch from cached simulator data. Reads only the cache left by the last read-program execution in the - transition; never performs reads itself and never touches reward, - termination, or info. + transition; never performs reads itself and never touches reward or + termination. """ - info = state.info dof_pos = self.get_dof_pos(slice(None)) dof_vel = self.get_dof_vel(slice(None)) dof_pos_rel = self._get_joint_pos_rel(dof_pos) @@ -137,9 +142,9 @@ def compute_observation(self, state: ArrayEnvState): object_pick_pose = self.get_cube_pose(slice(None)) - object_lift_pos = info["commands"] + object_lift_pos = self._commands - last_actions = info["current_actions"] + last_actions = self._current_actions obs = np.concatenate([dof_pos_rel, dof_vel_rel, object_pick_pose, object_lift_pos, last_actions], axis=-1) @@ -170,7 +175,7 @@ def compute_transition(self, state: ArrayEnvState): return state - def reset(self, env_ids): + def reset(self, env_ids) -> None: num_reset = len(env_ids) row_ids = np.asarray(env_ids, dtype=np.int64) @@ -199,17 +204,13 @@ def reset(self, env_ids): self._reset_program.execute(row_ids) self.sim_data.execute(row_ids) - info = { - "current_actions": np.zeros((num_reset, self._action_dim), dtype=np.float32), - "last_actions": np.zeros((num_reset, self._action_dim), dtype=np.float32), - "commands": self._generated_commands(num_reset), # - "current_gripper_action": np.zeros(num_reset, dtype=np.float32), # 1D - } - - # Check for nan - assert not np.isnan(info["commands"]).any(), "commands contain nan" + commands = self._generated_commands(num_reset) + assert not np.isnan(commands).any(), "commands contain nan" - return info + # Write episode-scoped state for the reset rows + self._commands[row_ids] = commands + self._current_actions[row_ids] = 0.0 + self._last_actions[row_ids] = 0.0 def _check_termination(self, state: ArrayEnvState): cube_height = self.get_cube_pose(slice(None))[:, 2] @@ -240,7 +241,7 @@ def _compute_reward(self, state: ArrayEnvState, truncated: np.ndarray): lifted = lift_height > minimal_height # object_command_tracking reward - object_command_dist = np.linalg.norm(cube_pos - state.info["commands"], axis=-1) + object_command_dist = np.linalg.norm(cube_pos - self._commands, axis=-1) def shifted_sigmoid_reward(d, k=8, center=0.3): # Sigmoid(-k * (d - center)) @@ -262,7 +263,7 @@ def shifted_sigmoid_reward(d, k=8, center=0.3): ) # action_diff_sq: Sum of squares of action changes - action_diff_sq = np.sum(np.square(state.info["current_actions"] - state.info["last_actions"]), axis=-1) + action_diff_sq = np.sum(np.square(self._current_actions - self._last_actions), axis=-1) # joint_vel_sq: Sum of squares of joint velocities joint_vel_sq = np.sum(np.square(self.get_dof_vel(slice(None))[:, : self._num_dof_vel]), axis=1) diff --git a/motrix_envs/src/motrix_envs/manipulation/franka_open_cabinet/franka_open_cabinet_np.py b/motrix_envs/src/motrix_envs/manipulation/franka_open_cabinet/franka_open_cabinet_np.py index 6c80bed4..ce71ad77 100755 --- a/motrix_envs/src/motrix_envs/manipulation/franka_open_cabinet/franka_open_cabinet_np.py +++ b/motrix_envs/src/motrix_envs/manipulation/franka_open_cabinet/franka_open_cabinet_np.py @@ -108,6 +108,13 @@ def __init__(self, cfg: FrankaOpenCabinetEnvCfg, num_envs=1, backend: str | None # Set print options to 2 decimal places np.set_printoptions(precision=2) + # Episode-scoped task state: full-batch buffers, reset writes the done + # rows in place. + num_envs = self._num_envs + self._current_actions = np.zeros((num_envs, self._action_dim), dtype=np.float32) + self._last_actions = np.zeros((num_envs, self._action_dim), dtype=np.float32) + self._current_gripper_action = np.zeros(num_envs, dtype=np.float32) + @property def observation_space(self): return self._observation_space @@ -119,8 +126,8 @@ def action_space(self): def apply_action(self, actions: np.ndarray, state: ArrayEnvState): assert not np.isnan(actions).any(), "actions contain nan" - state.info["last_actions"] = state.info["current_actions"] - state.info["current_actions"] = actions + self._last_actions[:] = self._current_actions + self._current_actions[:] = actions # no gripper old_joint_pos = self.get_robot_joint_pos(slice(None))[:, : self._action_dim - 1] @@ -135,7 +142,7 @@ def apply_action(self, actions: np.ndarray, state: ArrayEnvState): sampled_gripper_action = np.where(probabilities > np.random.rand(*probabilities.shape), 0, 0.04)[ :, None ] # 0 for closed, 0.04 for open - state.info["current_gripper_action"] = sampled_gripper_action.squeeze(-1) + self._current_gripper_action[:] = sampled_gripper_action.squeeze(-1) new_pos = np.concatenate([new_joint_pos, sampled_gripper_action], axis=-1) @@ -155,8 +162,8 @@ def compute_observation(self, state: ArrayEnvState): """Build the full observation batch from cached simulator data. Reads only the cache left by the last read-program execution in the - transition; never performs reads itself and never touches reward, - termination, or info. + transition; never performs reads itself and never touches reward or + termination. """ rows = slice(None) num_envs = self.num_envs @@ -214,7 +221,7 @@ def compute_transition(self, state: ArrayEnvState): return state - def reset(self, env_ids): + def reset(self, env_ids) -> None: num_reset = len(env_ids) row_ids = np.asarray(env_ids, dtype=np.int64) @@ -232,13 +239,10 @@ def reset(self, env_ids): self._reset_program.execute(row_ids) self.sim_data.execute(row_ids) - info = { - "current_actions": np.zeros((num_reset, self._action_dim), dtype=np.float32), - "last_actions": np.zeros((num_reset, self._action_dim), dtype=np.float32), - "phase2_mask": np.zeros(num_reset, dtype=bool), # 1D array - "current_gripper_action": np.zeros(num_reset, dtype=np.float32), # 1D array - } - return info + # Write episode-scoped state for the reset rows + self._current_actions[row_ids] = 0.0 + self._last_actions[row_ids] = 0.0 + self._current_gripper_action[row_ids] = 0.0 def _compute_reward(self, state: ArrayEnvState, truncated: np.ndarray): robot_grasp_pose = self._grasp_pose(slice(None), "gripper") @@ -259,7 +263,7 @@ def _compute_reward(self, state: ArrayEnvState, truncated: np.ndarray): # When gripper distance > 0.025, closing gripper gets penalty # When gripper distance > 0.025 or < 0.025, opening gripper gets no reward open_gripper = np.where(gripper_drawer_dist < 0.025, 100.0, -20) * ( - 0.04 - state.info["current_gripper_action"] + 0.04 - self._current_gripper_action ) # dist_reward * 0 or 0.04 ## open drawer reward @@ -278,7 +282,7 @@ def _compute_reward(self, state: ArrayEnvState, truncated: np.ndarray): ##################### Penalty Terms #####################" ## Action penalty ## Joint velocity penalty - sometimes some joints rotate more while others rotate less - action_penalty = np.sum(np.square(state.info["current_actions"] - state.info["last_actions"]), axis=-1) + action_penalty = np.sum(np.square(self._current_actions - self._last_actions), axis=-1) joint_vel_penalty = np.sum(np.square(self.sim_data["robot_joint_vel"][:, : self._action_dim]), axis=-1) ## finger position penalty diff --git a/motrix_envs/src/motrix_envs/manipulation/rm65_insert_peg/insert_peg_np.py b/motrix_envs/src/motrix_envs/manipulation/rm65_insert_peg/insert_peg_np.py index 94e557b9..9ae014ed 100644 --- a/motrix_envs/src/motrix_envs/manipulation/rm65_insert_peg/insert_peg_np.py +++ b/motrix_envs/src/motrix_envs/manipulation/rm65_insert_peg/insert_peg_np.py @@ -77,10 +77,28 @@ def __init__(self, cfg: PegInsertEnvCfg, num_envs=1, backend: str | None = None) self.gripper_closed = self._cfg.action_config.gripper_closed self.gripper_open = self._cfg.action_config.gripper_open + # Episode-scoped task state: full-batch buffers, reset writes the done + # rows in place. + self._current_actions = np.zeros((num_envs, self._action_dim), dtype=np.float32) + self._last_actions = np.zeros_like(self._current_actions) + self._socket_pos = np.zeros((num_envs, 3), dtype=np.float32) + self._initial_peg_pos = np.zeros((num_envs, 3), dtype=np.float32) + self._current_gripper_action = np.zeros(num_envs, dtype=np.float32) + self._gripper_is_closed = np.zeros(num_envs, dtype=bool) self._grasp_success = np.zeros(num_envs, dtype=bool) + self._phase = np.zeros(num_envs, dtype=np.int32) self._prev_hand_to_peg_dist = np.zeros(num_envs, dtype=np.float32) self._prev_peg_to_socket_dist = np.zeros(num_envs, dtype=np.float32) + self._prev_gripper_closure = np.zeros(num_envs, dtype=np.float32) + self._prev_midpoint_xy_dist = np.zeros(num_envs, dtype=np.float32) + self._prev_pregrasp_height_error = np.zeros(num_envs, dtype=np.float32) + self._prev_peg_height = np.zeros(num_envs, dtype=np.float32) + self._prev_socket_xy_dist = np.zeros(num_envs, dtype=np.float32) + self._prev_insert_depth = np.zeros(num_envs, dtype=np.float32) + self._prev_socket_entry_gap = np.full(num_envs, 0.2, dtype=np.float32) + self._consecutive_capture_steps = np.zeros(num_envs, dtype=np.int32) self._consecutive_grasp_steps = np.zeros(num_envs, dtype=np.int32) + self._consecutive_pregrasp_open_steps = np.zeros(num_envs, dtype=np.int32) def _sample_peg_xy(self, socket_xy: np.ndarray) -> np.ndarray: peg_init = self._cfg.peg_init_config @@ -133,8 +151,8 @@ def action_space(self): return self._action_space def apply_action(self, actions: np.ndarray, state: ArrayEnvState): - state.info["last_actions"] = state.info["current_actions"] - state.info["current_actions"] = actions.copy() + self._last_actions[:] = self._current_actions + self._current_actions[:] = actions old_joint_pos = self.get_dof_pos(slice(None))[:, : self._action_dim - 1] @@ -181,13 +199,7 @@ def apply_action(self, actions: np.ndarray, state: ArrayEnvState): close_request = gripper_raw > 0.2 - grasp_locked = state.info.get( - "grasp_success", - np.zeros( - actions.shape[0], - dtype=bool, - ), - ) + grasp_locked = self._grasp_success gripper_is_closed = np.where( grasp_locked, @@ -201,9 +213,8 @@ def apply_action(self, actions: np.ndarray, state: ArrayEnvState): self.gripper_open, ).astype(np.float32) - state.info["gripper_is_closed"] = gripper_is_closed.copy() - - state.info["current_gripper_action"] = gripper_action.copy() + self._gripper_is_closed[:] = gripper_is_closed + self._current_gripper_action[:] = gripper_action new_pos = np.concatenate( [ @@ -229,11 +240,10 @@ def compute_observation(self, state: ArrayEnvState): """Build the full observation batch from cached simulator data. Reads only the cache left by the last read-program execution in the - transition; never performs reads itself and never touches reward, - termination, or info. + transition; never performs reads itself and never touches reward or + termination. """ rows = slice(None) - info = state.info dof_pos = self.get_dof_pos(rows) dof_vel = self.get_dof_vel(rows) @@ -244,7 +254,7 @@ def compute_observation(self, state: ArrayEnvState): peg_pos = inputs["peg_pos"][rows] peg_axis = quaternion.rotate_vector(inputs["peg_quat"][rows], np.array([0.0, 0.0, 1.0], dtype=np.float32)) - socket_pos = info["socket_pos"] + socket_pos = self._socket_pos hand_pos = inputs["hand_pos"][rows] left_finger_pos = inputs["left_finger_pos"][rows] @@ -270,8 +280,8 @@ def compute_observation(self, state: ArrayEnvState): peg_to_socket_dist = np.linalg.norm(peg_to_socket, axis=-1, keepdims=True) peg_to_socket_dir = peg_to_socket / (peg_to_socket_dist + 1e-6) - gripper_state = info.get("current_gripper_action", np.zeros(peg_pos.shape[0], dtype=np.float32)) - grasp_success = info.get("grasp_success", np.zeros(peg_pos.shape[0], dtype=bool)).astype(np.float32) + gripper_state = self._current_gripper_action + grasp_success = self._grasp_success.astype(np.float32) peg_to_socket_dir = peg_to_socket_dir * grasp_success[:, None] peg_to_socket_dist = peg_to_socket_dist * grasp_success[:, None] @@ -326,7 +336,7 @@ def compute_transition(self, state: ArrayEnvState): return state - def reset(self, env_ids): + def reset(self, env_ids) -> None: num_reset = len(env_ids) row_ids = np.asarray(env_ids, dtype=np.int64) @@ -400,31 +410,28 @@ def reset(self, env_ids): gripper_closure = np.clip(-gripper_joint_pos / max(abs(self.gripper_closed), 1e-6), 0.0, 1.0) socket_xy_dist = np.linalg.norm(peg_pos[:, :2] - socket_pos[:, :2], axis=-1) - info = { - "current_actions": np.zeros((num_reset, self._action_dim), dtype=np.float32), - "last_actions": np.zeros((num_reset, self._action_dim), dtype=np.float32), - "socket_pos": socket_pos, - "initial_peg_pos": peg_pos.copy(), - "current_gripper_action": np.zeros(num_reset, dtype=np.float32), - "success": np.zeros(num_reset, dtype=bool), - "grasp_success": np.zeros(num_reset, dtype=bool), - "phase": np.zeros(num_reset, dtype=np.int32), - "prev_hand_to_peg_dist": np.linalg.norm(peg_pos - hand_pos, axis=-1), - "prev_peg_to_socket_dist": np.linalg.norm(peg_pos - socket_pos, axis=-1), - "prev_midpoint_xy_dist": midpoint_to_peg_xy_dist, - "prev_pregrasp_height_error": pregrasp_height_error, - "prev_peg_height": peg_pos[:, 2].copy(), - "prev_gripper_closure": gripper_closure.copy(), - "prev_socket_xy_dist": socket_xy_dist, - "prev_insert_depth": np.zeros(num_reset, dtype=np.float32), - "prev_socket_entry_gap": np.full(num_reset, 0.2, dtype=np.float32), - "consecutive_capture_steps": np.zeros(num_reset, dtype=np.int32), - "consecutive_grasp_steps": np.zeros(num_reset, dtype=np.int32), - "consecutive_pregrasp_open_steps": np.zeros(num_reset, dtype=np.int32), - "gripper_is_closed": np.zeros(num_reset, dtype=bool), - } - - return info + # Seed the episode-scoped state buffers for the rows being reset; the + # step pipeline owns them afterwards (never per-step state). + self._current_actions[row_ids] = 0.0 + self._last_actions[row_ids] = 0.0 + self._socket_pos[row_ids] = socket_pos + self._initial_peg_pos[row_ids] = peg_pos + self._current_gripper_action[row_ids] = 0.0 + self._gripper_is_closed[row_ids] = False + self._grasp_success[row_ids] = False + self._phase[row_ids] = 0 + self._prev_hand_to_peg_dist[row_ids] = np.linalg.norm(peg_pos - hand_pos, axis=-1) + self._prev_peg_to_socket_dist[row_ids] = np.linalg.norm(peg_pos - socket_pos, axis=-1) + self._prev_gripper_closure[row_ids] = gripper_closure + self._prev_midpoint_xy_dist[row_ids] = midpoint_to_peg_xy_dist + self._prev_pregrasp_height_error[row_ids] = pregrasp_height_error + self._prev_peg_height[row_ids] = peg_pos[:, 2] + self._prev_socket_xy_dist[row_ids] = socket_xy_dist + self._prev_insert_depth[row_ids] = 0.0 + self._prev_socket_entry_gap[row_ids] = 0.2 + self._consecutive_capture_steps[row_ids] = 0 + self._consecutive_grasp_steps[row_ids] = 0 + self._consecutive_pregrasp_open_steps[row_ids] = 0 def _check_termination(self, state: ArrayEnvState): inputs = self.sim_data @@ -432,7 +439,7 @@ def _check_termination(self, state: ArrayEnvState): terminated = peg_pos[:, 2] < -0.15 - prev_grasp = state.info.get("grasp_success", np.zeros(inputs["dof_pos"].shape[0], dtype=bool)) + prev_grasp = self._grasp_success if np.any(prev_grasp): hand_pos = inputs["hand_pos"] hand_to_peg_dist = np.linalg.norm(peg_pos - hand_pos, axis=-1) @@ -447,12 +454,12 @@ def _check_termination(self, state: ArrayEnvState): return terminated - def _collect_reward_features(self, state: ArrayEnvState) -> dict: + def _collect_reward_features(self) -> dict: """Collect the current geometry and gripper state shared by all reward terms.""" inputs = self.sim_data peg_pos = inputs["peg_pos"] peg_quat = inputs["peg_quat"] - socket_pos = state.info["socket_pos"] + socket_pos = self._socket_pos hand_pos = inputs["hand_pos"] left_finger_pos = inputs["left_finger_pos"] right_finger_pos = inputs["right_finger_pos"] @@ -495,7 +502,7 @@ def _collect_reward_features(self, state: ArrayEnvState) -> dict: peg_bottom_z = peg_pos[:, 2] - self._cfg.peg_config.peg_length * 0.5 raw_insert_depth = np.clip(socket_top_z - peg_bottom_z, 0.0, self._cfg.peg_config.socket_depth) - gripper_state = state.info.get("current_gripper_action", np.zeros(inputs["dof_pos"].shape[0], dtype=np.float32)) + gripper_state = self._current_gripper_action is_gripper_command_closed = gripper_state < -0.5 is_gripper_command_open = gripper_state > -0.3 gripper_joint_pos = self.get_dof_pos(slice(None))[:, 6] @@ -507,7 +514,7 @@ def _collect_reward_features(self, state: ArrayEnvState) -> dict: ) actual_gripper_narrow = (gripper_closure > 0.42) | (finger_gap < 0.065) - initial_peg_pos = state.info["initial_peg_pos"] + initial_peg_pos = self._initial_peg_pos lift_height = np.maximum(0.0, peg_pos[:, 2] - initial_peg_pos[:, 2]) peg_xy_shift = np.linalg.norm(peg_pos[:, :2] - initial_peg_pos[:, :2], axis=-1) peg_drop_height = np.maximum(0.0, initial_peg_pos[:, 2] - peg_pos[:, 2]) @@ -548,36 +555,31 @@ def _collect_reward_features(self, state: ArrayEnvState) -> dict: "peg_speed": peg_speed, } - def _update_reward_progress(self, state: ArrayEnvState, features: dict) -> dict: + def _update_reward_progress(self, features: dict) -> dict: """Update one-step progress signals that depend on the previous simulator state.""" - num_envs = self._num_envs - info = state.info + # Snapshot the pre-step grasp state: _update_task_tracking overwrites + # the buffer in place, and downstream reads (first_grasp, + # pregrasp_open_active) must still see the prior values. + prev_grasp = self._grasp_success.copy() - prev_grasp = info.get("grasp_success", np.zeros(num_envs, dtype=bool)) + gripper_closure_progress = features["gripper_closure"] - self._prev_gripper_closure + self._prev_gripper_closure[:] = features["gripper_closure"] - prev_gripper_closure = info.get("prev_gripper_closure", features["gripper_closure"]) - gripper_closure_progress = features["gripper_closure"] - prev_gripper_closure - info["prev_gripper_closure"] = features["gripper_closure"].copy() + self._prev_hand_to_peg_dist[:] = features["hand_to_peg_dist"] - info["prev_hand_to_peg_dist"] = features["hand_to_peg_dist"].copy() + xy_progress = self._prev_midpoint_xy_dist - features["midpoint_xy_dist"] + self._prev_midpoint_xy_dist[:] = features["midpoint_xy_dist"] - prev_midpoint_xy_dist = info.get("prev_midpoint_xy_dist", features["midpoint_xy_dist"]) - xy_progress = prev_midpoint_xy_dist - features["midpoint_xy_dist"] - info["prev_midpoint_xy_dist"] = features["midpoint_xy_dist"].copy() + height_progress = self._prev_pregrasp_height_error - features["pregrasp_height_error"] + self._prev_pregrasp_height_error[:] = features["pregrasp_height_error"] - prev_pregrasp_height_error = info.get("prev_pregrasp_height_error", features["pregrasp_height_error"]) - height_progress = prev_pregrasp_height_error - features["pregrasp_height_error"] - info["prev_pregrasp_height_error"] = features["pregrasp_height_error"].copy() + peg_height_progress = features["peg_pos"][:, 2] - self._prev_peg_height + self._prev_peg_height[:] = features["peg_pos"][:, 2] - prev_peg_height = info.get("prev_peg_height", features["peg_pos"][:, 2]) - peg_height_progress = features["peg_pos"][:, 2] - prev_peg_height - info["prev_peg_height"] = features["peg_pos"][:, 2].copy() + self._prev_peg_to_socket_dist[:] = features["peg_to_socket_dist"] - info["prev_peg_to_socket_dist"] = features["peg_to_socket_dist"].copy() - - prev_socket_xy_dist = info.get("prev_socket_xy_dist", features["xy_dist"]) - xy_socket_progress = prev_socket_xy_dist - features["xy_dist"] - info["prev_socket_xy_dist"] = features["xy_dist"].copy() + xy_socket_progress = self._prev_socket_xy_dist - features["xy_dist"] + self._prev_socket_xy_dist[:] = features["xy_dist"] return { "prev_grasp": prev_grasp, @@ -588,15 +590,9 @@ def _update_reward_progress(self, state: ArrayEnvState, features: dict) -> dict: "xy_socket_progress": xy_socket_progress, } - def _update_task_tracking(self, state: ArrayEnvState, features: dict, progress: dict) -> dict: + def _update_task_tracking(self, features: dict, progress: dict) -> dict: """Update reward-related task phase state such as grasp, transport, and insertion.""" - num_envs = self._num_envs - info = state.info - prev_grasp = progress["prev_grasp"] - consecutive_capture = info.get("consecutive_capture_steps", np.zeros(num_envs, dtype=np.int32)) - consecutive_grasp = info.get("consecutive_grasp_steps", np.zeros(num_envs, dtype=np.int32)) - consecutive_pregrasp_open = info.get("consecutive_pregrasp_open_steps", np.zeros(num_envs, dtype=np.int32)) pregrasp_ready = ( (features["midpoint_xy_dist"] < 0.032) @@ -619,8 +615,8 @@ def _update_task_tracking(self, state: ArrayEnvState, features: dict, progress: & (features["peg_uprightness"] > 0.9) ) - consecutive_capture = np.where(grasp_candidate, consecutive_capture + 1, 0) - info["consecutive_capture_steps"] = consecutive_capture + consecutive_capture = np.where(grasp_candidate, self._consecutive_capture_steps + 1, 0) + self._consecutive_capture_steps[:] = consecutive_capture grasp_candidate = ( features["is_gripper_command_closed"] @@ -633,7 +629,7 @@ def _update_task_tracking(self, state: ArrayEnvState, features: dict, progress: ever_grasped = prev_grasp | confirmed_grasp - info["grasp_success"] = ever_grasped + self._grasp_success[:] = ever_grasped is_grasping = ( ever_grasped @@ -644,26 +640,24 @@ def _update_task_tracking(self, state: ArrayEnvState, features: dict, progress: consecutive_grasp = np.where( is_grasping, - consecutive_grasp + 1, + self._consecutive_grasp_steps + 1, 0, ) - info["consecutive_grasp_steps"] = consecutive_grasp + self._consecutive_grasp_steps[:] = consecutive_grasp pregrasp_open_active = pregrasp_ready & features["is_gripper_command_open"] & (~prev_grasp) - consecutive_pregrasp_open = np.where(pregrasp_open_active, consecutive_pregrasp_open + 1, 0) - info["consecutive_pregrasp_open_steps"] = consecutive_pregrasp_open + consecutive_pregrasp_open = np.where(pregrasp_open_active, self._consecutive_pregrasp_open_steps + 1, 0) + self._consecutive_pregrasp_open_steps[:] = consecutive_pregrasp_open insert_ready = ever_grasped & (features["xy_dist"] < 0.008) & (features["peg_uprightness"] > 0.97) insert_depth = np.where(insert_ready, features["raw_insert_depth"], 0.0) - prev_insert_depth = info.get("prev_insert_depth", insert_depth) - insert_depth_progress = insert_depth - prev_insert_depth - info["prev_insert_depth"] = insert_depth.copy() + insert_depth_progress = insert_depth - self._prev_insert_depth + self._prev_insert_depth[:] = insert_depth socket_entry_gap = np.maximum(0.0, features["peg_bottom_z"] - features["socket_top_z"]) - prev_socket_entry_gap = info.get("prev_socket_entry_gap", socket_entry_gap) - socket_entry_gap_progress = prev_socket_entry_gap - socket_entry_gap - info["prev_socket_entry_gap"] = socket_entry_gap.copy() + socket_entry_gap_progress = self._prev_socket_entry_gap - socket_entry_gap + self._prev_socket_entry_gap[:] = socket_entry_gap touch_socket = ( ever_grasped & (insert_depth > 0.008) & (features["xy_dist"] < 0.006) & (features["peg_uprightness"] > 0.97) @@ -673,14 +667,13 @@ def _update_task_tracking(self, state: ArrayEnvState, features: dict, progress: & (features["xy_dist"] < self._cfg.peg_config.success_threshold) & (features["peg_uprightness"] > 0.98) ) - info["success"] = success - phase = np.zeros(num_envs, dtype=np.int32) + phase = self._phase + phase[:] = 0 phase[(~pregrasp_ready) & (~ever_grasped)] = 1 phase[pregrasp_ready | grasp_channel_ready | ever_grasped] = 2 phase[touch_socket] = 3 phase[success] = 4 - info["phase"] = phase return { "pregrasp_ready": pregrasp_ready, @@ -1110,11 +1103,11 @@ def _compute_safety_penalties(self, reward_cfg, features: dict, tracking: dict) def _compute_reward(self, state: ArrayEnvState, terminated: np.ndarray): # 1) Gather geometry and actuation features shared across all reward terms. reward_cfg = self._cfg.reward_config - features = self._collect_reward_features(state) + features = self._collect_reward_features() # 2) Update history-dependent progress signals and the task phase tracker. - progress = self._update_reward_progress(state, features) - tracking = self._update_task_tracking(state, features, progress) + progress = self._update_reward_progress(features) + tracking = self._update_task_tracking(features, progress) # 3) Compute reward components by task stage. The final sum order below # intentionally mirrors the pre-refactor implementation for reproducibility. @@ -1123,7 +1116,7 @@ def _compute_reward(self, state: ArrayEnvState, terminated: np.ndarray): insert_rewards = self._compute_insert_rewards(reward_cfg, features, tracking) safety_penalties = self._compute_safety_penalties(reward_cfg, features, tracking) - action_diff_sq = np.sum(np.square(state.info["current_actions"] - state.info["last_actions"]), axis=-1) + action_diff_sq = np.sum(np.square(self._current_actions - self._last_actions), axis=-1) joint_vel_sq = np.sum(np.square(self.get_dof_vel(slice(None))[:, : self._num_dof_vel]), axis=1) peg_vel_sq = np.sum(np.square(features["peg_linear_vel"]), axis=-1) @@ -1203,7 +1196,7 @@ def _compute_reward(self, state: ArrayEnvState, terminated: np.ndarray): reward = np.clip(reward, -100.0, 3000.0) reward_details["total"] = reward.astype(np.float32) - state.info["Reward"] = reward_details + state.reward_terms = reward_details return reward.astype(np.float32) diff --git a/motrix_envs/src/motrix_envs/manipulation/rm65_open_cabinet/rm65_open_cabinet_np.py b/motrix_envs/src/motrix_envs/manipulation/rm65_open_cabinet/rm65_open_cabinet_np.py index c0836325..5e4e77ad 100755 --- a/motrix_envs/src/motrix_envs/manipulation/rm65_open_cabinet/rm65_open_cabinet_np.py +++ b/motrix_envs/src/motrix_envs/manipulation/rm65_open_cabinet/rm65_open_cabinet_np.py @@ -98,12 +98,12 @@ def __init__(self, cfg: RM65OpenCabinetEnvCfg, num_envs=1, backend: str | None = np.set_printoptions(precision=2) def _init_obs_buffers(self) -> None: - """Allocate env-owned observation caches. + """Allocate env-owned episode and observation state buffers. - These back :meth:`compute_observation` (finite-difference velocities and - observation-noise state). They live on the environment, never in - ``state.info``, so computing observations never mutates info; reset only - overwrites the rows being reset. + The episode-scoped task state (action pipeline, gripper logic, grasp + tracking, arm randomization) and the observation caches (finite-difference + velocities, observation-noise state) live on the environment, never in + per-step state; reset only overwrites the rows being reset. """ num_envs = self.num_envs # None until the first observation: the first finite-difference @@ -118,6 +118,43 @@ def _init_obs_buffers(self) -> None: np.zeros((num_envs, latency_steps + 1, handle_dim), dtype=np.float32) if latency_steps > 0 else None ) + # Action pipeline state + action_dim = self._action_dim + self._current_actions = np.zeros((num_envs, action_dim), dtype=np.float32) + self._last_actions = np.zeros((num_envs, action_dim), dtype=np.float32) + buffer_len = max(int(self._arm_action_delay_buffer_len), 1) + self._action_delay_buffer = np.zeros((num_envs, buffer_len, action_dim), dtype=np.float32) + if self._action_history_len > 0: + self._action_history = np.zeros((num_envs, self._action_history_len, action_dim), dtype=np.float32) + + # Per-episode arm randomization + self._arm_action_delay_steps_per_env = np.zeros(num_envs, dtype=np.int32) + self._arm_actuator_lag_alpha_per_env = np.zeros(num_envs, dtype=np.float32) + self._arm_max_step_per_env = np.zeros(num_envs, dtype=np.float32) + self._arm_max_acc_step_per_env = np.zeros(num_envs, dtype=np.float32) + + # Arm action filtering state + arm_dim = self._arm_action_dim + self._arm_target_smooth = np.zeros((num_envs, arm_dim), dtype=np.float32) + self._arm_prev_delta = np.zeros((num_envs, arm_dim), dtype=np.float32) + self._arm_actuator_target = np.zeros((num_envs, arm_dim), dtype=np.float32) + + # Gripper logic state + self._gripper_target_smooth = np.zeros(num_envs, dtype=np.float32) + self._gripper_binary_closed = np.zeros(num_envs, dtype=bool) + self._gripper_closed_cmd = np.zeros(num_envs, dtype=bool) + self._prev_gripper_closed_cmd = np.zeros(num_envs, dtype=bool) + self._gripper_steps_since_switch = np.zeros(num_envs, dtype=np.int32) + self._gripper_close_ratio = np.zeros(num_envs, dtype=np.float32) + self._current_gripper_action = np.zeros(num_envs, dtype=np.float32) + + # Grasp / opening task state + self._grasp_hold_steps = np.zeros(num_envs, dtype=np.int32) + self._grasped = np.zeros(num_envs, dtype=bool) + self._phase2_mask = np.zeros(num_envs, dtype=bool) + self._prev_open_dist = np.zeros(num_envs, dtype=np.float32) + self._open_bonus_progress = np.zeros(num_envs, dtype=np.int32) + def _init_action_spaces(self) -> None: self._action_dim = len(self._cfg.action_scale) + 1 self._arm_action_dim = self._action_dim - 1 @@ -296,53 +333,26 @@ def _sample_arm_delay_lag(self, num_envs: int) -> tuple[np.ndarray, np.ndarray, max_acc_step = np.full((num_envs,), float(self._arm_max_acc_step), dtype=np.float32) return delay_steps, lag_alpha, max_step, max_acc_step - def _apply_action_delay(self, actions: np.ndarray, episode_steps: np.ndarray, info: dict) -> np.ndarray: + def _apply_action_delay(self, actions: np.ndarray) -> np.ndarray: num_envs = actions.shape[0] - delay_steps = info.get("arm_action_delay_steps") - if not isinstance(delay_steps, np.ndarray) or delay_steps.shape != (num_envs,): - delay_steps = np.full((num_envs,), int(self._arm_action_delay_steps), dtype=np.int32) - else: - delay_steps = delay_steps.astype(np.int32, copy=False) - delay_steps = np.maximum(delay_steps, 0) - buffer_len = max(int(self._arm_action_delay_buffer_len), 1) - delay_steps = np.minimum(delay_steps, buffer_len - 1) + buffer_len = self._action_delay_buffer.shape[1] + delay_steps = np.clip(self._arm_action_delay_steps_per_env, 0, buffer_len - 1) - buffer = info.get("action_delay_buffer") - expected_shape = (num_envs, buffer_len, actions.shape[1]) - if buffer is None or buffer.shape != expected_shape: - buffer = np.repeat(actions[:, None, :], buffer_len, axis=1) - else: - buffer = np.roll(buffer, 1, axis=1) - buffer[:, 0, :] = actions - reset_mask = episode_steps == 0 - if np.any(reset_mask): - buffer[reset_mask] = np.repeat(actions[reset_mask][:, None, :], buffer_len, axis=1) - info["action_delay_buffer"] = buffer + buffer = self._action_delay_buffer + buffer = np.roll(buffer, 1, axis=1) + buffer[:, 0, :] = actions + self._action_delay_buffer = buffer return buffer[np.arange(num_envs), delay_steps, :] - def _update_action_history( - self, - raw_actions: np.ndarray, - delayed_actions: np.ndarray, - episode_steps: np.ndarray, - info: dict, - ) -> None: + def _update_action_history(self, raw_actions: np.ndarray) -> None: if self._action_history_len <= 0: return - hist = info.get("action_history") - expected_shape = (delayed_actions.shape[0], self._action_history_len, delayed_actions.shape[1]) - if hist is None or hist.shape != expected_shape: - hist = np.repeat(raw_actions[:, None, :], self._action_history_len, axis=1) - else: - hist = np.roll(hist, 1, axis=1) - hist[:, 0, :] = raw_actions - reset_mask = episode_steps == 0 - if np.any(reset_mask): - hist[reset_mask] = np.repeat(raw_actions[reset_mask][:, None, :], self._action_history_len, axis=1) - info["action_history"] = hist - - def _apply_arm_action(self, arm_action: np.ndarray, old_joint_pos: np.ndarray, info: dict) -> np.ndarray: + hist = np.roll(self._action_history, 1, axis=1) + hist[:, 0, :] = raw_actions + self._action_history = hist + + def _apply_arm_action(self, arm_action: np.ndarray, old_joint_pos: np.ndarray) -> np.ndarray: arm_min_limit = self.robot_joint_pos_min_limit[: self._arm_action_dim] arm_max_limit = self.robot_joint_pos_max_limit[: self._arm_action_dim] smoothing_active = self._arm_action_mode == "joint_target" and self._arm_target_smoothing_alpha > 0.0 @@ -361,9 +371,7 @@ def _apply_arm_action(self, arm_action: np.ndarray, old_joint_pos: np.ndarray, i target_joint_pos = arm_action target_joint_pos = np.clip(target_joint_pos, arm_min_limit, arm_max_limit) if smoothing_active: - prev_target = info.get("arm_target_smooth", old_joint_pos) - if not isinstance(prev_target, np.ndarray) or prev_target.shape != target_joint_pos.shape: - prev_target = old_joint_pos + prev_target = self._arm_target_smooth target_joint_pos = ( 1.0 - self._arm_target_smoothing_alpha ) * prev_target + self._arm_target_smoothing_alpha * target_joint_pos @@ -372,83 +380,47 @@ def _apply_arm_action(self, arm_action: np.ndarray, old_joint_pos: np.ndarray, i action_delta = target_joint_pos - old_joint_pos if self._arm_use_speed_limit: - max_step = info.get("arm_max_step") - if isinstance(max_step, np.ndarray) and max_step.shape == (action_delta.shape[0],): - max_step_vec = np.maximum(max_step.astype(np.float32, copy=False), 0.0) - action_delta = np.clip(action_delta, -max_step_vec[:, None], max_step_vec[:, None]) - else: - action_delta = np.clip(action_delta, -self._arm_max_step, self._arm_max_step) + max_step_vec = np.maximum(self._arm_max_step_per_env, 0.0) + action_delta = np.clip(action_delta, -max_step_vec[:, None], max_step_vec[:, None]) if self._arm_use_acc_limit: - prev_delta = info.get("arm_prev_delta", np.zeros_like(action_delta)) - if not isinstance(prev_delta, np.ndarray) or prev_delta.shape != action_delta.shape: - prev_delta = np.zeros_like(action_delta) - max_delta_change = info.get("arm_max_acc_step") - if isinstance(max_delta_change, np.ndarray) and max_delta_change.shape == (action_delta.shape[0],): - max_delta_change_vec = np.maximum(max_delta_change.astype(np.float32, copy=False), 0.0) - delta_change = np.clip( - action_delta - prev_delta, - -max_delta_change_vec[:, None], - max_delta_change_vec[:, None], - ) - action_delta = prev_delta + delta_change - else: - max_delta_change_scalar = float(self._arm_max_acc_step) - if max_delta_change_scalar > 0.0: - delta_change = np.clip( - action_delta - prev_delta, - -max_delta_change_scalar, - max_delta_change_scalar, - ) - action_delta = prev_delta + delta_change - else: - action_delta = prev_delta + prev_delta = self._arm_prev_delta + max_delta_change_vec = np.maximum(self._arm_max_acc_step_per_env, 0.0) + delta_change = np.clip( + action_delta - prev_delta, + -max_delta_change_vec[:, None], + max_delta_change_vec[:, None], + ) + action_delta = prev_delta + delta_change - info["arm_prev_delta"] = action_delta + self._arm_prev_delta[:] = action_delta target_joint_pos = old_joint_pos + action_delta if smoothing_active: - info["arm_target_smooth"] = target_joint_pos + self._arm_target_smooth[:] = target_joint_pos - lag_alpha = info.get("arm_actuator_lag_alpha", self._arm_actuator_lag_alpha) - if isinstance(lag_alpha, np.ndarray) and lag_alpha.shape == (target_joint_pos.shape[0],): - lag_alpha_vec = np.clip(lag_alpha.astype(np.float32, copy=False), 0.0, 1.0) - else: - lag_alpha_vec = np.full((target_joint_pos.shape[0],), float(self._arm_actuator_lag_alpha), dtype=np.float32) + lag_alpha_vec = np.clip(self._arm_actuator_lag_alpha_per_env, 0.0, 1.0) if np.any(lag_alpha_vec > 0.0): - prev_cmd = info.get("arm_actuator_target", old_joint_pos) - if not isinstance(prev_cmd, np.ndarray) or prev_cmd.shape != target_joint_pos.shape: - prev_cmd = old_joint_pos + prev_cmd = self._arm_actuator_target target_joint_pos = (1.0 - lag_alpha_vec[:, None]) * prev_cmd + (lag_alpha_vec[:, None] * target_joint_pos) target_joint_pos = target_joint_pos.astype(np.float32, copy=False) - info["arm_actuator_target"] = target_joint_pos + self._arm_actuator_target[:] = target_joint_pos return target_joint_pos - def _apply_gripper_action(self, gripper_action: np.ndarray, info: dict) -> np.ndarray: + def _apply_gripper_action(self, gripper_action: np.ndarray) -> np.ndarray: close_ratio = raw_action_to_close_ratio(gripper_action, use_sigmoid=self._gripper_use_sigmoid) gripper_closed_cmd = None if self._gripper_action_mode == "binary": - prev_closed = info.get("gripper_binary_closed") - if not isinstance(prev_closed, np.ndarray) or prev_closed.shape != close_ratio.shape: - prev_closed = close_ratio > self._gripper_close_on_threshold - steps_since_switch = info.get("gripper_steps_since_switch") - if not isinstance(steps_since_switch, np.ndarray) or steps_since_switch.shape != close_ratio.shape: - steps_since_switch = np.full( - close_ratio.shape, - self._gripper_min_switch_interval_steps, - dtype=np.int32, - ) gripper_closed_cmd, switched = binary_hysteresis_step( close_ratio=close_ratio, - prev_closed=prev_closed, - steps_since_switch=steps_since_switch, + prev_closed=self._gripper_binary_closed, + steps_since_switch=self._gripper_steps_since_switch, close_on_threshold=self._gripper_close_on_threshold, open_off_threshold=self._gripper_open_off_threshold, min_switch_interval_steps=self._gripper_min_switch_interval_steps, ) - steps_since_switch = np.where(switched, 0, steps_since_switch + 1).astype(np.int32) - info["gripper_binary_closed"] = gripper_closed_cmd - info["gripper_steps_since_switch"] = steps_since_switch + self._gripper_binary_closed[:] = gripper_closed_cmd + self._gripper_steps_since_switch[:] = np.where(switched, 0, self._gripper_steps_since_switch + 1) gripper_pos = np.where(gripper_closed_cmd, self.gripper_closed_pos, self.gripper_open_pos) elif self._gripper_action_mode == "continuous": gripper_pos = self.gripper_open_pos + (self.gripper_closed_pos - self.gripper_open_pos) * close_ratio @@ -456,9 +428,7 @@ def _apply_gripper_action(self, gripper_action: np.ndarray, info: dict) -> np.nd else: raise ValueError(f"Unsupported gripper action mode: {self._gripper_action_mode}") - prev_gripper = info.get("gripper_target_smooth", gripper_pos) - if not isinstance(prev_gripper, np.ndarray) or prev_gripper.shape != gripper_pos.shape: - prev_gripper = gripper_pos + prev_gripper = self._gripper_target_smooth if self._gripper_use_speed_limit and self._gripper_max_step > 0.0: delta = np.clip(gripper_pos - prev_gripper, -self._gripper_max_step, self._gripper_max_step) gripper_pos = prev_gripper + delta @@ -468,25 +438,25 @@ def _apply_gripper_action(self, gripper_action: np.ndarray, info: dict) -> np.nd ) gripper_pos = gripper_pos.astype(np.float32, copy=False) - info["gripper_target_smooth"] = gripper_pos + self._gripper_target_smooth[:] = gripper_pos if gripper_closed_cmd is not None: - info["gripper_closed_cmd"] = np.asarray(gripper_closed_cmd, dtype=bool) - info["gripper_close_ratio"] = np.asarray(close_ratio, dtype=np.float32) - info["current_gripper_action"] = gripper_pos + self._gripper_closed_cmd[:] = gripper_closed_cmd + self._gripper_close_ratio[:] = close_ratio + self._current_gripper_action[:] = gripper_pos return gripper_pos[:, None] def apply_action(self, actions: np.ndarray, state: ArrayEnvState): assert not np.isnan(actions).any(), "actions contain nan" raw_actions = np.array(actions, copy=True) - delayed_actions = self._apply_action_delay(actions, state.episode_steps, state.info) - self._update_action_history(raw_actions, delayed_actions, state.episode_steps, state.info) - state.info["last_actions"] = state.info["current_actions"] - state.info["current_actions"] = delayed_actions + delayed_actions = self._apply_action_delay(actions) + self._update_action_history(raw_actions) + self._last_actions[:] = self._current_actions + self._current_actions[:] = delayed_actions old_joint_pos = self.get_robot_joint_pos(slice(None))[:, : self._arm_action_dim] - target_joint_pos = self._apply_arm_action(delayed_actions[:, : self._arm_action_dim], old_joint_pos, state.info) - gripper_action_cmd = self._apply_gripper_action(delayed_actions[:, -1], state.info) + target_joint_pos = self._apply_arm_action(delayed_actions[:, : self._arm_action_dim], old_joint_pos) + gripper_action_cmd = self._apply_gripper_action(delayed_actions[:, -1]) new_pos = np.concatenate([target_joint_pos, gripper_action_cmd], axis=-1) @@ -507,10 +477,9 @@ def compute_observation(self, state: ArrayEnvState): Reads only the cache left by the last read-program execution in the transition or reset; never performs reads itself and never touches - reward, termination, or info. Observation-only state (finite-difference + reward, termination, or reward terms. Observation-only state (finite-difference velocities, noise caches) lives in env-owned buffers. """ - info = state.info episode_steps = state.episode_steps num_envs = self.num_envs obs_noise_cfg = self._obs_noise_cfg @@ -549,7 +518,7 @@ def compute_observation(self, state: ArrayEnvState): # relative pose: position delta + relative quaternion (target * current.inverse) robot_grasp_pose = self._grasp_pose(slice(None)) - drawer_grasp_pose = self._resolve_handle_pose(slice(None), info) + drawer_grasp_pose = self._handle_pose(slice(None)) if obs_noise_cfg.enabled and obs_noise_cfg.handle_pose_noise_enabled: drawer_grasp_pose = self._get_noisy_handle_pose(drawer_grasp_pose) pos_delta = drawer_grasp_pose[:, :3] - robot_grasp_pose[:, :3] @@ -565,13 +534,7 @@ def compute_observation(self, state: ArrayEnvState): obs = np.concatenate([dof_pos_scaled, dof_vel_rel, to_target], axis=-1) if self._action_history_len > 0: - history = info.get("action_history") - expected_shape = (num_envs, self._action_history_len, self._action_dim) - if history is None or history.shape != expected_shape: - history = np.zeros(expected_shape, dtype=np.float32) - else: - history = history.astype(np.float32, copy=False) - obs = np.concatenate([obs, history.reshape(num_envs, -1)], axis=-1) + obs = np.concatenate([obs, self._action_history.reshape(num_envs, -1)], axis=-1) assert obs.shape == (num_envs, self._obs_dim) assert not np.isnan(obs).any(), "obs contain nan" @@ -588,7 +551,7 @@ def compute_transition(self, state: ArrayEnvState): Observations are built separately by :meth:`compute_observation`. """ self.sim_data.execute() - self._enforce_drawer_grasp_constraint(state) + self._enforce_drawer_grasp_constraint() # compute truncated truncated = self._check_termination(state) @@ -602,14 +565,14 @@ def compute_transition(self, state: ArrayEnvState): return state - def _enforce_drawer_grasp_constraint(self, state: ArrayEnvState): + def _enforce_drawer_grasp_constraint(self): reward_cfg = self._cfg.reward robot_grasp_pose = self._grasp_pose(slice(None)) drawer_grasp_pose = self._handle_pose(slice(None)) gripper_drawer_dist = np.linalg.norm(drawer_grasp_pose[:, :3] - robot_grasp_pose[:, :3], axis=-1) gripper_range = max(abs(self.gripper_open_pos - self.gripper_closed_pos), 1e-6) close_ratio = np.clip( - (self.gripper_open_pos - state.info["current_gripper_action"]) / gripper_range, + (self.gripper_open_pos - self._current_gripper_action) / gripper_range, 0.0, 1.0, ) @@ -622,20 +585,14 @@ def _enforce_drawer_grasp_constraint(self, state: ArrayEnvState): ) grasp_candidate = np.logical_and(grasp_candidate, align_mask) - hold_steps = state.info.get("grasp_hold_steps") - if not isinstance(hold_steps, np.ndarray) or hold_steps.shape != grasp_candidate.shape: - hold_steps = np.zeros_like(grasp_candidate, dtype=np.int32) - hold_steps = np.where(grasp_candidate, hold_steps + 1, 0) + hold_steps = np.where(grasp_candidate, self._grasp_hold_steps + 1, 0) required_steps = max(int(getattr(reward_cfg, "grasp_hold_steps", 1)), 1) + self._grasp_hold_steps[:] = hold_steps grasped = hold_steps >= required_steps + self._grasped[:] = grasped + self._phase2_mask |= grasped - state.info["grasp_hold_steps"] = hold_steps - state.info["grasped"] = grasped - phase2_mask = state.info.get("phase2_mask", grasped) - phase2_mask = np.logical_or(phase2_mask, grasped) - state.info["phase2_mask"] = phase2_mask - - def reset(self, env_ids): + def reset(self, env_ids) -> None: num_reset = len(env_ids) row_ids = np.asarray(env_ids, dtype=np.int64) @@ -666,7 +623,7 @@ def reset(self, env_ids): bias_quat = self._sample_quat_bias(num_reset, obs_noise_cfg.target_rot_bias_std) handle_pose = self._handle_pose(row_ids).astype(np.float32) # Seed the env-owned observation caches for the rows being reset; the - # observation pipeline owns them afterwards (never state.info). + # observation pipeline owns them afterwards (never per-step state). self._obs_handle_bias_pos[row_ids] = bias_pos self._obs_handle_bias_quat[row_ids] = bias_quat self._obs_handle_pose_last[row_ids] = handle_pose @@ -674,75 +631,34 @@ def reset(self, env_ids): self._obs_handle_pose_buffer[row_ids] = np.repeat( handle_pose[:, None, :], self._obs_handle_pose_buffer.shape[1], axis=1 ) - info = { - "current_actions": hold_action.copy(), - "last_actions": hold_action.copy(), - "phase2_mask": np.zeros(num_reset, dtype=bool), # 1D array - "grasped": np.zeros(num_reset, dtype=bool), - "grasp_hold_steps": np.zeros(num_reset, dtype=np.int32), - "current_gripper_action": np.full(num_reset, self.gripper_open_pos, dtype=np.float32), # 1D array - "handle_pose_override": np.zeros((num_reset, handle_pose.shape[1]), dtype=np.float32), - "handle_pose_override_mask": np.zeros(num_reset, dtype=bool), - "arm_action_delay_steps": arm_delay_steps, - "arm_actuator_lag_alpha": arm_lag_alpha, - "arm_max_step": arm_max_step, - "arm_max_acc_step": arm_max_acc_step, - "arm_target_smooth": dof_pos[:, : self._arm_action_dim], - "arm_prev_delta": np.zeros((num_reset, self._arm_action_dim), dtype=np.float32), - "arm_actuator_target": dof_pos[:, : self._arm_action_dim], - "gripper_target_smooth": gripper_left.copy(), - "gripper_binary_closed": init_binary_closed.astype(bool, copy=False), - "gripper_closed_cmd": init_binary_closed.astype(bool, copy=False), - "gripper_steps_since_switch": np.full( - num_reset, - self._gripper_min_switch_interval_steps, - dtype=np.int32, - ), - "gripper_close_ratio": init_close_ratio.copy(), - "prev_gripper_closed_cmd": init_binary_closed.astype(bool, copy=True), - "prev_open_dist": np.zeros(num_reset, dtype=np.float32), - "open_bonus_progress": np.zeros(num_reset, dtype=np.int32), - "Reward": { - "dist": np.zeros(num_reset, dtype=np.float32), - "quat": np.zeros(num_reset, dtype=np.float32), - "close_gripper": np.zeros(num_reset, dtype=np.float32), - "open_reward": np.zeros(num_reset, dtype=np.float32), - "open_delta_reward": np.zeros(num_reset, dtype=np.float32), - "slip_penalty": np.zeros(num_reset, dtype=np.float32), - "finger_penalty": np.zeros(num_reset, dtype=np.float32), - "action_penalty": np.zeros(num_reset, dtype=np.float32), - "joint_vel_penalty": np.zeros(num_reset, dtype=np.float32), - "gripper_switch_penalty": np.zeros(num_reset, dtype=np.float32), - "truncation_penalty": np.zeros(num_reset, dtype=np.float32), - }, - } - buffer_len = max(int(self._arm_action_delay_buffer_len), 1) - info["action_delay_buffer"] = np.repeat(hold_action[:, None, :], buffer_len, axis=1) + # Seed the env-owned episode state buffers for the rows being reset; + # the action/reward pipeline owns them afterwards (never per-step state). + self._current_actions[row_ids] = hold_action + self._last_actions[row_ids] = hold_action + self._action_delay_buffer[row_ids] = np.repeat( + hold_action[:, None, :], self._action_delay_buffer.shape[1], axis=1 + ) if self._action_history_len > 0: - info["action_history"] = np.repeat(hold_action[:, None, :], self._action_history_len, axis=1) - return info - - def _resolve_handle_pose(self, rows, info: dict): - handle_pose = self._handle_pose(rows) - override = info.get("handle_pose_override") - if override is not None: - override_pose = np.asarray(override, dtype=np.float32) - if override_pose.ndim == 1 and override_pose.shape[0] == handle_pose.shape[1]: - override_pose = np.tile(override_pose, (handle_pose.shape[0], 1)) - if override_pose.shape != handle_pose.shape: - return handle_pose - override_mask = info.get("handle_pose_override_mask") - if override_mask is not None: - mask = np.asarray(override_mask, dtype=bool) - if mask.shape != (handle_pose.shape[0],): - return handle_pose - if np.any(mask): - return np.where(mask[:, None], override_pose, handle_pose) - if np.any(override_pose): - return override_pose - return handle_pose - return override_pose - return handle_pose + self._action_history[row_ids] = np.repeat(hold_action[:, None, :], self._action_history_len, axis=1) + self._arm_action_delay_steps_per_env[row_ids] = arm_delay_steps + self._arm_actuator_lag_alpha_per_env[row_ids] = arm_lag_alpha + self._arm_max_step_per_env[row_ids] = arm_max_step + self._arm_max_acc_step_per_env[row_ids] = arm_max_acc_step + self._arm_target_smooth[row_ids] = dof_pos[:, : self._arm_action_dim] + self._arm_prev_delta[row_ids] = 0.0 + self._arm_actuator_target[row_ids] = dof_pos[:, : self._arm_action_dim] + self._gripper_target_smooth[row_ids] = gripper_left + self._gripper_binary_closed[row_ids] = init_binary_closed + self._gripper_closed_cmd[row_ids] = init_binary_closed + self._prev_gripper_closed_cmd[row_ids] = init_binary_closed + self._gripper_steps_since_switch[row_ids] = self._gripper_min_switch_interval_steps + self._gripper_close_ratio[row_ids] = init_close_ratio + self._current_gripper_action[row_ids] = self.gripper_open_pos + self._grasp_hold_steps[row_ids] = 0 + self._grasped[row_ids] = False + self._phase2_mask[row_ids] = False + self._prev_open_dist[row_ids] = 0.0 + self._open_bonus_progress[row_ids] = 0 def _sample_quat_bias(self, num_envs: int, rot_std: float): identity = np.array([0.0, 0.0, 0.0, 1.0], dtype=np.float32) @@ -835,7 +751,6 @@ def _get_noisy_handle_pose(self, handle_pose: np.ndarray): def _compute_distance_alignment_terms( self, - state: ArrayEnvState, reward_cfg, robot_grasp_pose: np.ndarray, drawer_grasp_pose: np.ndarray, @@ -854,7 +769,7 @@ def _compute_distance_alignment_terms( align_mask = np.logical_and(lfinger_dist >= 0.0, rfinger_dist >= 0.0) gripper_range = max(abs(self.gripper_open_pos - self.gripper_closed_pos), 1e-6) - close_amount_raw = self.gripper_open_pos - state.info["current_gripper_action"] + close_amount_raw = self.gripper_open_pos - self._current_gripper_action close_amount_raw = np.clip(close_amount_raw, 0.0, gripper_range) close_ratio = close_amount_raw / gripper_range close_amount = close_amount_raw * (0.04 / gripper_range) @@ -880,10 +795,8 @@ def _compute_distance_alignment_terms( def _compute_open_reward_terms( self, - state: ArrayEnvState, reward_cfg, gripper_drawer_dist: np.ndarray, - align_mask: np.ndarray, ) -> dict[str, np.ndarray]: open_dist = self.sim_data["drawer_pos"][:, 0] open_dist = np.asarray(open_dist).reshape(-1) @@ -896,12 +809,8 @@ def _compute_open_reward_terms( wrong_open = np.zeros_like(open_dist, dtype=bool) open_reward = np.where(np.logical_not(wrong_open), open_reward, 0.0) - grasped = state.info.get("grasped") - if grasped is None: - grasped = align_mask - phase2_mask = state.info.get("phase2_mask") - if not isinstance(phase2_mask, np.ndarray) or phase2_mask.shape != grasped.shape: - phase2_mask = grasped + grasped = self._grasped + phase2_mask = self._phase2_mask strict_open_gate = np.logical_or(grasped, phase2_mask) strict_open_dist = float(getattr(reward_cfg, "open_reward_strict_dist", 0.0)) @@ -913,15 +822,12 @@ def _compute_open_reward_terms( open_gate = np.logical_and(strict_open_gate, near_mask) open_reward = np.where(open_gate, open_reward, 0.0) - prev_open_dist = state.info.get("prev_open_dist") - if not isinstance(prev_open_dist, np.ndarray) or prev_open_dist.shape != open_dist.shape: - prev_open_dist = np.zeros_like(open_dist, dtype=np.float32) - open_delta = np.clip(open_dist - prev_open_dist, 0.0, None) + open_delta = np.clip(open_dist - self._prev_open_dist, 0.0, None) open_delta_reward = open_delta * reward_cfg.open_delta_reward_scale open_delta_reward = np.where(open_gate, open_delta_reward, 0.0) open_delta_reward = np.where(np.logical_not(wrong_open), open_delta_reward, 0.0) - state.info["prev_open_dist"] = open_dist.astype(np.float32, copy=True) + self._prev_open_dist[:] = open_dist return { "open_dist": open_dist, @@ -934,7 +840,6 @@ def _compute_open_reward_terms( def _compute_progress_reward_terms( self, - state: ArrayEnvState, reward_cfg, open_dist: np.ndarray, grasped: np.ndarray, @@ -944,9 +849,7 @@ def _compute_progress_reward_terms( grasp_hold_open_scale = float(getattr(reward_cfg, "grasp_hold_open_scale", 0.0)) grasp_hold_reward = np.where(grasped, grasp_hold_reward_scale + grasp_hold_open_scale * open_dist, 0.0) - prev_open_bonus = state.info.get("open_bonus_progress") - if not isinstance(prev_open_bonus, np.ndarray) or prev_open_bonus.shape != open_dist.shape: - prev_open_bonus = np.zeros_like(open_dist, dtype=np.int32) + prev_open_bonus = self._open_bonus_progress bonus1_dist = float(getattr(reward_cfg, "open_bonus_dist_1", 0.0)) bonus1_reward = float(getattr(reward_cfg, "open_bonus_reward_1", 0.0)) @@ -961,7 +864,7 @@ def _compute_progress_reward_terms( open_bonus_reward = np.where(grasped, open_bonus_reward, 0.0) bonus_progress = np.where(pass_bonus1, 1, bonus_progress) bonus_progress = np.where(pass_bonus2, 2, bonus_progress) - state.info["open_bonus_progress"] = bonus_progress.astype(np.int32) + self._open_bonus_progress[:] = bonus_progress slip_open_dist_thresh = float(np.clip(reward_cfg.slip_open_dist_thresh, 0.0, 1.0)) slipped = np.logical_and( @@ -983,7 +886,6 @@ def _compute_progress_reward_terms( def _compute_penalty_terms( self, - state: ArrayEnvState, reward_cfg, gripper_drawer_dist: np.ndarray, lfinger_dist: np.ndarray, @@ -993,7 +895,7 @@ def _compute_penalty_terms( close_ratio: np.ndarray, open_dist: np.ndarray, ) -> dict[str, np.ndarray]: - action_penalty = np.sum(np.square(state.info["current_actions"] - state.info["last_actions"]), axis=-1) + action_penalty = np.sum(np.square(self._current_actions - self._last_actions), axis=-1) joint_vel_penalty = np.sum(np.square(self.sim_data["robot_joint_vel"][:, : self._action_dim]), axis=-1) finger_penalty = np.zeros_like(lfinger_dist) @@ -1010,16 +912,8 @@ def _compute_penalty_terms( 0.0, ) - gripper_closed_cmd = state.info.get("gripper_closed_cmd") - if not isinstance(gripper_closed_cmd, np.ndarray) or gripper_closed_cmd.shape != close_ratio.shape: - gripper_closed_cmd = close_ratio > self._gripper_close_threshold - gripper_closed_cmd = np.asarray(gripper_closed_cmd, dtype=bool) - prev_gripper_closed_cmd = state.info.get("prev_gripper_closed_cmd") - if not isinstance(prev_gripper_closed_cmd, np.ndarray) or ( - prev_gripper_closed_cmd.shape != gripper_closed_cmd.shape - ): - prev_gripper_closed_cmd = gripper_closed_cmd.copy() - switch_mask = gripper_closed_cmd != prev_gripper_closed_cmd + gripper_closed_cmd = self._gripper_closed_cmd + switch_mask = gripper_closed_cmd != self._prev_gripper_closed_cmd switch_penalty_dist = float(getattr(reward_cfg, "gripper_switch_penalty_dist", 0.0)) if switch_penalty_dist > 0.0: switch_gate = gripper_drawer_dist < switch_penalty_dist @@ -1031,7 +925,7 @@ def _compute_penalty_terms( -gripper_switch_penalty_scale, 0.0, ).astype(np.float32) - state.info["prev_gripper_closed_cmd"] = gripper_closed_cmd.copy() + self._prev_gripper_closed_cmd[:] = gripper_closed_cmd if self.count < reward_cfg.action_penalty_switch_step: action_penalty_rate = reward_cfg.action_penalty_rate_early @@ -1054,7 +948,7 @@ def _compute_penalty_terms( "joint_vel_penalty_rate": np.full_like(open_dist, joint_vel_penalty_rate, dtype=np.float32), } - def _update_reward_info( + def _update_reward_terms( self, state: ArrayEnvState, *, @@ -1067,7 +961,7 @@ def _update_reward_info( truncation_penalty: np.ndarray, ) -> None: grasped = open_terms["grasped"] - state.info["Reward"] = { + state.reward_terms = { "dist": alignment_terms["dist_reward"], "quat": alignment_terms["quat_reward"], "close_gripper": alignment_terms["close_gripper"], @@ -1106,27 +1000,22 @@ def _compute_reward(self, state: ArrayEnvState, truncated: np.ndarray): gripper_drawer_dist = np.linalg.norm(drawer_grasp_pose[:, :3] - robot_grasp_pose[:, :3], axis=-1) reward_cfg = self._cfg.reward alignment_terms = self._compute_distance_alignment_terms( - state, reward_cfg, robot_grasp_pose, drawer_grasp_pose, gripper_drawer_dist, ) open_terms = self._compute_open_reward_terms( - state, reward_cfg, gripper_drawer_dist, - alignment_terms["align_mask"], ) progress_terms = self._compute_progress_reward_terms( - state, reward_cfg, open_terms["open_dist"], open_terms["grasped"], open_terms["phase2_mask"], ) penalty_terms = self._compute_penalty_terms( - state, reward_cfg, gripper_drawer_dist, alignment_terms["lfinger_dist"], @@ -1154,7 +1043,7 @@ def _compute_reward(self, state: ArrayEnvState, truncated: np.ndarray): truncation_penalty = np.where(truncated, -reward_cfg.truncation_penalty, 0.0) reward = reward + truncation_penalty - self._update_reward_info( + self._update_reward_terms( state, reward_cfg=reward_cfg, alignment_terms=alignment_terms, diff --git a/motrix_envs/src/motrix_envs/manipulation/shadow_hand/shadow_hand_np.py b/motrix_envs/src/motrix_envs/manipulation/shadow_hand/shadow_hand_np.py index 0d9fdfb8..8f2c6dae 100644 --- a/motrix_envs/src/motrix_envs/manipulation/shadow_hand/shadow_hand_np.py +++ b/motrix_envs/src/motrix_envs/manipulation/shadow_hand/shadow_hand_np.py @@ -129,6 +129,14 @@ def __init__(self, cfg: ShadowHandReposeEnvCfg, num_envs=1, backend: str | None # Initial cube position (in hand) self._in_hand_pos = np.array(cfg.cube_initial_pos, dtype=np.float32) + # Episode-scoped task state: full-batch buffers, reset writes the done + # rows in place. + num_envs = self._num_envs + self._goal_pos = np.tile(self._in_hand_pos, (num_envs, 1)) + self._goal_rot = np.zeros((num_envs, 4), dtype=np.float32) + self._prev_actions = np.zeros((num_envs, self._num_actuators), dtype=np.float32) + self._successes = np.zeros(num_envs, dtype=np.int32) + @property def observation_space(self): return self._observation_space @@ -170,7 +178,7 @@ def apply_action(self, actions: np.ndarray, state: ArrayEnvState): # Apply action moving average for smoothness if cfg.act_moving_average < 1.0: - targets = cfg.act_moving_average * targets + (1.0 - cfg.act_moving_average) * state.info["prev_actions"] + targets = cfg.act_moving_average * targets + (1.0 - cfg.act_moving_average) * self._prev_actions # Clamp to control limits targets = np.clip(targets, self._actuator_ctrl_lower, self._actuator_ctrl_upper) @@ -179,7 +187,7 @@ def apply_action(self, actions: np.ndarray, state: ArrayEnvState): ctrl = self._ctrl_writes.buffer("ctrl") ctrl[:] = np.asarray(targets, dtype=np.float32) self._ctrl_writes.execute() - state.info["prev_actions"] = targets.copy() + self._prev_actions[:] = targets return state @@ -187,11 +195,10 @@ def compute_observation(self, state: ArrayEnvState): """Build the full 157-dim observation batch from cached simulator data. Reads only the cache left by the last read-program execution in the - transition; never performs reads itself and never touches reward, - termination, or info. + transition; never performs reads itself and never touches reward or + termination. """ cfg = self._cfg - info = state.info rows = slice(None) # Get hand DOF states @@ -218,7 +225,7 @@ def compute_observation(self, state: ArrayEnvState): ) # Total: 65 # Compute relative quaternion - relative_quat = quaternion.mul(cube_quat, quaternion.conjugate(info["goal_rot"])) + relative_quat = quaternion.mul(cube_quat, quaternion.conjugate(self._goal_rot)) scaled_hand_pos = utils.unscale(hand_dof_pos, self._hand_dof_lower_limits, self._hand_dof_upper_limits) # Build observation (157 dims) @@ -230,11 +237,11 @@ def compute_observation(self, state: ArrayEnvState): cube_quat, # 4 cube_linvel, # 3 cfg.vel_obs_scale * cube_angvel, # 3 - info["goal_pos"], # 3 - info["goal_rot"], # 4 + self._goal_pos, # 3 + self._goal_rot, # 4 relative_quat, # 4 fingertip_state, # 65 - info["prev_actions"], # 20 + self._prev_actions, # 20 ], axis=-1, ) @@ -252,23 +259,22 @@ def compute_transition(self, state: ArrayEnvState): method never touches the observation field. """ self.sim_data.execute() - info = state.info # Compute reward and termination from the refreshed simulator cache - reward, terminated, goal_reached = self._compute_reward(info) + reward, terminated, goal_reached = self._compute_reward() if np.any(goal_reached): reset_goal_indices = np.where(goal_reached)[0] - self._reset_goal_pose(info, reset_goal_indices) + self._reset_goal_pose(reset_goal_indices) # Update the goal mocap body so viewers track the current goal pose # (a write program, not an observation read). - self._update_target_visualization(info) + self._update_target_visualization() state.reward = reward state.terminated = terminated return state - def _compute_reward(self, info: dict): + def _compute_reward(self): """ Reward components (3 core items): 1. Position distance penalty @@ -287,15 +293,15 @@ def _compute_reward(self, info: dict): cube_pos, cube_quat, _ = self._extract_cube_states(slice(None)) # Distance from cube to goal position - goal_dist = np.linalg.norm(cube_pos - info["goal_pos"], axis=-1) + goal_dist = np.linalg.norm(cube_pos - self._goal_pos, axis=-1) # Rotation distance - rot_dist = quaternion.rotation_distance(cube_quat, info["goal_rot"]) + rot_dist = quaternion.rotation_distance(cube_quat, self._goal_rot) # Core reward components dist_rew = goal_dist * cfg.dist_reward_scale rot_rew = 1.0 / (np.abs(rot_dist) + cfg.rot_eps) * cfg.rot_reward_scale - action_penalty = np.sum(info["prev_actions"] ** 2, axis=-1) * cfg.action_penalty_scale + action_penalty = np.sum(self._prev_actions**2, axis=-1) * cfg.action_penalty_scale # Base reward reward = dist_rew + rot_rew + action_penalty @@ -304,7 +310,7 @@ def _compute_reward(self, info: dict): goal_reached = np.abs(rot_dist) <= cfg.success_tolerance # Update success counter - info["successes"] += goal_reached * 1 + self._successes += goal_reached * 1 # Success bonus reward = np.where(goal_reached, reward + cfg.reach_goal_bonus, reward) @@ -323,8 +329,8 @@ def _compute_reward(self, info: dict): new_pos = np.zeros(num_envs, dtype=bool) if cfg.max_consecutive_successes > 0: # Reset progress on goal reached when max consecutive successes reached - new_pos = info["successes"] >= cfg.max_consecutive_successes - info["successes"] *= 1 - new_pos + new_pos = self._successes >= cfg.max_consecutive_successes + self._successes *= 1 - new_pos # 3. NaN protection terminated = np.logical_or(terminated, np.isnan(rot_dist)) @@ -332,22 +338,22 @@ def _compute_reward(self, info: dict): return reward, terminated, new_pos - def _update_target_visualization(self, info: dict): + def _update_target_visualization(self): """Update the target mocap body to visualize the goal pose.""" cfg = self._cfg # Compute visualization position (offset from goal position) - viz_pos = info["goal_pos"] + np.array(cfg.viz_target_offset, dtype=np.float32) + viz_pos = self._goal_pos + np.array(cfg.viz_target_offset, dtype=np.float32) # Combine into pose array: [x, y, z, qx, qy, qz, qw] - viz_pose = np.concatenate([viz_pos, info["goal_rot"]], axis=-1) + viz_pose = np.concatenate([viz_pos, self._goal_rot], axis=-1) # Update mocap body pose all_ids = np.arange(self._num_envs, dtype=np.int64) self._target_writes.buffer("target")[all_ids, 0] = np.asarray(viz_pose, dtype=np.float32) self._target_writes.execute(all_ids) - def reset(self, env_ids: np.ndarray): + def reset(self, env_ids: np.ndarray) -> None: """Reset environments.""" cfg = self._cfg @@ -392,22 +398,18 @@ def reset(self, env_ids: np.ndarray): self._reset_program.execute(row_ids) self.sim_data.execute(row_ids) - # Reset goal pose - # Note: goal_pos and goal_rot are indexed by original env indices - info = { - "goal_pos": np.tile(self._in_hand_pos, num_resets).reshape(num_resets, 3), - "goal_rot": quaternion.generate_random_shoemake(num_resets), - "prev_actions": np.zeros((num_resets, self._num_actuators), dtype=np.float32), - "successes": np.zeros((num_resets), dtype=np.int32), - } - - return info + # Reset episode-scoped task state for the reset rows. The goal position + # is fixed; the goal orientation is sampled uniformly on SO(3). + self._goal_pos[row_ids] = self._in_hand_pos + self._goal_rot[row_ids] = quaternion.generate_random_shoemake(num_resets) + self._prev_actions[row_ids] = 0.0 + self._successes[row_ids] = 0 - def _reset_goal_pose(self, info, env_ids): + def _reset_goal_pose(self, env_ids): """Reset goal pose to random orientation with fixed position.""" num_resets = len(env_ids) # Goal position is fixed # Randomize goal orientation using Shoemake method for uniform SO(3) sampling - info["goal_rot"][env_ids] = quaternion.generate_random_shoemake(num_resets) + self._goal_rot[env_ids] = quaternion.generate_random_shoemake(num_resets) diff --git a/motrix_envs/src/motrix_envs/motion/sampler.py b/motrix_envs/src/motrix_envs/motion/sampler.py index 4d5023be..a72b5d6c 100644 --- a/motrix_envs/src/motrix_envs/motion/sampler.py +++ b/motrix_envs/src/motrix_envs/motion/sampler.py @@ -22,9 +22,11 @@ class AdaptiveTimestepsSampler: histogram (with an exponential spatial kernel and a uniform floor), then draws start-frame indices. - Typical per-step usage:: + Typical per-step usage (``motion_steps`` tracks each env's current + frame index into the clip; ``terminated`` marks failed episodes):: - sampler.record_failures(state.info["motion_steps"][state.terminated]) + failed = motion_steps[env_ids][terminated[env_ids]] + sampler.record_failures(failed) sampler.update() start_steps = sampler.sample(num_resets) """ @@ -88,7 +90,7 @@ def record_failures(self, failed_steps: np.ndarray) -> None: Args: failed_steps: 1D array of motion-step indices where episodes terminated this step (typically - ``state.info["motion_steps"][state.terminated]``). + ``motion_steps[env_ids][terminated[env_ids]]``). """ if failed_steps.size == 0: return diff --git a/motrix_envs/tests/test_basic_direct_contract.py b/motrix_envs/tests/test_basic_direct_contract.py index 7eb46920..9f771ddb 100644 --- a/motrix_envs/tests/test_basic_direct_contract.py +++ b/motrix_envs/tests/test_basic_direct_contract.py @@ -5,12 +5,12 @@ Every ``*_np`` basic environment must follow the split: -- ``reset(env_ids)`` only writes reset rows and returns a plain dict; +- ``reset(env_ids)`` only writes reset rows and returns None; - ``compute_transition`` executes the read program exactly once at the top and - only fills reward / terminated / truncated / info / metrics — never obs; + only fills reward / terminated / truncated / reward_terms / metrics — never obs; - ``compute_observation`` fully rebuilds obs from the cache left by the last read-program execution — it must not execute reads itself and must not - modify reward, termination, or info. + modify reward, termination, or reward_terms. """ from collections.abc import Iterable @@ -126,7 +126,9 @@ def test_compute_observation_reads_cache_without_executing(env_name: str) -> Non reward_before = state.reward.copy() terminated_before = state.terminated.copy() truncated_before = state.truncated.copy() - info_before = {key: np.copy(value) if isinstance(value, np.ndarray) else value for key, value in state.info.items()} + reward_terms_before = { + key: np.copy(value) if isinstance(value, np.ndarray) else value for key, value in state.reward_terms.items() + } calls = _spy_execute(env) observed = env.compute_observation(state) @@ -136,19 +138,19 @@ def test_compute_observation_reads_cache_without_executing(env_name: str) -> Non obs = observed.obs.policy if isinstance(observed.obs, NpObs) else observed.obs assert obs.shape == (num_envs, *env.observation_space.shape) assert not np.isnan(obs).any() - # Reward / termination / info must not be modified by observation. + # Reward / termination / reward_terms must not be modified by observation. np.testing.assert_array_equal(observed.reward, reward_before) np.testing.assert_array_equal(observed.terminated, terminated_before) np.testing.assert_array_equal(observed.truncated, truncated_before) - assert observed.info is state.info - for key, value in info_before.items(): - assert key in observed.info + assert observed.reward_terms is state.reward_terms + for key, value in reward_terms_before.items(): + assert key in observed.reward_terms if isinstance(value, np.ndarray): - np.testing.assert_array_equal(observed.info[key], value) + np.testing.assert_array_equal(observed.reward_terms[key], value) @pytest.mark.parametrize("env_name", _BASIC_ENVS) -def test_reset_only_writes_reset_state_and_returns_dict(env_name: str) -> None: +def test_reset_only_writes_reset_state(env_name: str) -> None: num_envs = 2 env = registry.make(env_name, num_envs=num_envs) rng = np.random.default_rng(3) @@ -158,9 +160,9 @@ def test_reset_only_writes_reset_state_and_returns_dict(env_name: str) -> None: env_ids = np.array([0], dtype=np.int64) obs_before = env.state.obs.policy.copy() - info = env.reset(env_ids) + result = env.reset(env_ids) - assert isinstance(info, dict) + assert result is None # Reset writes simulator rows (and refreshes their cache) but must not # touch the published observation batch. np.testing.assert_array_equal(env.state.obs.policy, obs_before) diff --git a/motrix_envs/tests/test_basic_env_lifecycle.py b/motrix_envs/tests/test_basic_env_lifecycle.py index 8f7a7817..7ddd4e8a 100644 --- a/motrix_envs/tests/test_basic_env_lifecycle.py +++ b/motrix_envs/tests/test_basic_env_lifecycle.py @@ -26,4 +26,5 @@ def test_basic_environment_step_preserves_numpy_observation_contract(env_name: s assert isinstance(state.obs, NpObs) assert state.obs.policy.shape == (num_envs, *env.observation_space.shape) if env_name == "franka-open-cabinet": - assert state.info["current_gripper_action"].shape == (num_envs,) + # Episode-scoped gripper state lives on the env instance, not in info. + assert env._current_gripper_action.shape == (num_envs,) diff --git a/motrix_envs/tests/test_manipulation_direct_contract.py b/motrix_envs/tests/test_manipulation_direct_contract.py index ca2233a9..25666da8 100644 --- a/motrix_envs/tests/test_manipulation_direct_contract.py +++ b/motrix_envs/tests/test_manipulation_direct_contract.py @@ -5,12 +5,12 @@ Every ``*_np`` manipulation environment must follow the split: -- ``reset(env_ids)`` only writes reset rows and returns a plain dict; +- ``reset(env_ids)`` only writes reset rows and returns ``None``; - ``compute_transition`` executes the read program exactly once at the top and - only fills reward / terminated / truncated / info / metrics — never obs; + only fills reward / terminated / truncated / reward_terms / metrics — never obs; - ``compute_observation`` fully rebuilds obs from the cache left by the last read-program execution — it must not execute reads itself and must not - modify reward, termination, or info. + modify reward, termination, or reward terms. """ from collections.abc import Iterable @@ -88,7 +88,9 @@ def test_compute_transition_executes_once_and_never_touches_obs(env_name: str) - state = env.state obs_before = state.obs.policy.copy() - info_before = {key: np.copy(value) if isinstance(value, np.ndarray) else value for key, value in state.info.items()} + reward_terms_before = { + key: np.copy(value) if isinstance(value, np.ndarray) else value for key, value in state.reward_terms.items() + } calls = _spy_execute(env) transitioned = env.compute_transition(state) @@ -101,10 +103,10 @@ def test_compute_transition_executes_once_and_never_touches_obs(env_name: str) - assert transitioned.terminated.shape == (num_envs,) assert transitioned.obs is state.obs np.testing.assert_array_equal(transitioned.obs.policy, obs_before) - # Info may gain bookkeeping entries, but pre-existing arrays keep contents. - for key, value in info_before.items(): - if isinstance(value, np.ndarray) and key in transitioned.info: - assert transitioned.info[key] is value or transitioned.info[key].shape == value.shape + # Reward terms may be rebuilt, but pre-existing entries keep their shape. + for key, value in reward_terms_before.items(): + if isinstance(value, np.ndarray) and key in transitioned.reward_terms: + assert transitioned.reward_terms[key] is value or transitioned.reward_terms[key].shape == value.shape @pytest.mark.parametrize("env_name", _MANIPULATION_ENVS) @@ -118,7 +120,9 @@ def test_compute_observation_reads_cache_without_executing(env_name: str) -> Non state = env.state reward_before = state.reward.copy() terminated_before = state.terminated.copy() - info_before = {key: np.copy(value) if isinstance(value, np.ndarray) else value for key, value in state.info.items()} + reward_terms_before = { + key: np.copy(value) if isinstance(value, np.ndarray) else value for key, value in state.reward_terms.items() + } calls = _spy_execute(env) observed = env.compute_observation(state) @@ -130,18 +134,18 @@ def test_compute_observation_reads_cache_without_executing(env_name: str) -> Non obs = observed.obs.policy if isinstance(observed.obs, NpObs) else observed.obs assert obs.shape == (num_envs, *env.observation_space.shape) assert not np.isnan(obs).any() - # Reward / termination / info must not be modified by observation. + # Reward / termination / reward terms must not be modified by observation. np.testing.assert_array_equal(observed.reward, reward_before) np.testing.assert_array_equal(observed.terminated, terminated_before) - assert observed.info is state.info - for key, value in info_before.items(): - assert key in observed.info + assert observed.reward_terms is state.reward_terms + for key, value in reward_terms_before.items(): + assert key in observed.reward_terms if isinstance(value, np.ndarray): - np.testing.assert_array_equal(observed.info[key], value) + np.testing.assert_array_equal(observed.reward_terms[key], value) @pytest.mark.parametrize("env_name", _MANIPULATION_ENVS) -def test_reset_only_writes_reset_state_and_returns_dict(env_name: str) -> None: +def test_reset_only_writes_reset_state(env_name: str) -> None: num_envs = 2 env = registry.make(env_name, num_envs=num_envs) rng = np.random.default_rng(3) @@ -151,9 +155,27 @@ def test_reset_only_writes_reset_state_and_returns_dict(env_name: str) -> None: env_ids = np.array([0], dtype=np.int64) obs_before = env.state.obs.policy.copy() - info = env.reset(env_ids) + result = env.reset(env_ids) - assert isinstance(info, dict) + assert result is None # Reset writes simulator rows (and refreshes their cache) but must not # touch the published observation batch. np.testing.assert_array_equal(env.state.obs.policy, obs_before) + + +def test_rm65_open_cabinet_arm_randomization_fallbacks_use_scalar_config() -> None: + # Regression: the per-env arm randomization buffers must not shadow the + # scalar config fallbacks consumed by _sample_arm_delay_lag; with both + # randomization toggles off and num_envs > 1, reset previously raised + # TypeError converting a (num_envs,) array to a scalar (PR #59 review). + env = registry.make("rm65-open-cabinet", num_envs=3) + # The toggles are cached on the env at construction time; flip the cached + # copies to exercise the scalar-fallback branches. + env._arm_delay_lag_randomization_enabled = False + env._arm_speed_acc_randomization_enabled = False + env.init_state() + env.reset(np.arange(3, dtype=np.int64)) + np.testing.assert_array_equal(env._arm_action_delay_steps_per_env, env._arm_action_delay_steps) + np.testing.assert_allclose(env._arm_actuator_lag_alpha_per_env, env._arm_actuator_lag_alpha) + np.testing.assert_allclose(env._arm_max_step_per_env, env._arm_max_step) + np.testing.assert_allclose(env._arm_max_acc_step_per_env, env._arm_max_acc_step) diff --git a/motrix_envs/tests/test_quadruped_walk.py b/motrix_envs/tests/test_quadruped_walk.py index 596af02d..fd01b9e8 100644 --- a/motrix_envs/tests/test_quadruped_walk.py +++ b/motrix_envs/tests/test_quadruped_walk.py @@ -56,11 +56,9 @@ def quadruped_env(): def _swing_reward(quadruped_env, contacts: np.ndarray) -> np.ndarray: - info = { - "feet_phase": np.full((1, quadruped_env._num_feet), 0.75, dtype=np.float32), - "contacts": contacts, - } - return quadruped_env._reward_swing_feet_z(info, None, None, None) + quadruped_env._feet_phase[:] = 0.75 + quadruped_env.feet_contact[:] = contacts + return quadruped_env._reward_swing_feet_z(None, None, None) def test_quadruped_reward_scales_are_structured_and_not_shared(): @@ -75,8 +73,8 @@ def test_quadruped_reward_scales_are_structured_and_not_shared(): def test_go2_velocity_commands_remain_constant_before_resampling_interval(): np.random.seed(7) env = registry.make("go2-walk-flat", num_envs=64, mode="train") - state = env.init_state() - commands = state.info["commands"].copy() + env.init_state() + commands = env._commands.copy() velocity_cfg = env.cfg.commands.velocity assert commands.shape == (64, 3) @@ -86,7 +84,7 @@ def test_go2_velocity_commands_remain_constant_before_resampling_interval(): env.step(np.zeros((env.num_envs, *env.action_space.shape), dtype=np.float32)) - np.testing.assert_array_equal(state.info["commands"], commands) + np.testing.assert_array_equal(env._commands, commands) def test_go2_randomization_is_enabled_for_training_and_disabled_for_play(): @@ -115,7 +113,7 @@ def test_go2_uses_position_actuators_for_pd_randomization(env_name: str): @pytest.mark.parametrize("env_name", ["go2-walk-flat", "go2-walk-rough"]) def test_go2_reset_randomization_stays_within_configured_ranges(env_name: str): env = registry.make(env_name, num_envs=32, mode="train") - state = env.init_state() + env.init_state() randomization = env.cfg.randomization joint_pos_diff = env.get_dof_pos() - env.default_angles dof_vel = env.sim_data["dof_vel"] @@ -129,8 +127,8 @@ def test_go2_reset_randomization_stays_within_configured_ranges(env_name: str): assert np.unique(joint_pos_diff, axis=0).shape[0] > 1 command_interval = env.cfg.commands.velocity.resampling_seconds_range if command_interval is not None: - assert np.all(state.info["command_resampling_time"] >= command_interval[0]) - assert np.all(state.info["command_resampling_time"] <= command_interval[1]) + assert np.all(env._command_resampling_time >= command_interval[0]) + assert np.all(env._command_resampling_time <= command_interval[1]) def test_go2_pd_and_friction_randomization_is_per_env_and_episode_constant(): @@ -189,8 +187,8 @@ def test_go2_action_delay_selects_current_or_previous_action_per_env(): state = env.init_state() previous = np.full((2, env._num_action), -0.2, dtype=np.float32) current = np.full((2, env._num_action), 0.3, dtype=np.float32) - state.info["current_actions"] = previous - state.info["action_delay_steps"][:] = (0, 1) + env._current_actions[:] = previous + env._action_delay_steps[:] = (0, 1) env.apply_action(current, state) env.sim_data.execute() @@ -202,19 +200,19 @@ def test_go2_action_delay_selects_current_or_previous_action_per_env(): def test_go2_command_resampling_only_updates_due_environments(monkeypatch: pytest.MonkeyPatch): env = registry.make("go2-walk-flat", num_envs=3, mode="train") - state = env.init_state() + env.init_state() commands = np.array([[0.1, 0.0, 0.0], [0.2, 0.1, 0.0], [0.3, 0.0, -0.1]], dtype=np.float32) replacement = np.array([[0.4, -0.2, 0.3], [0.5, 0.2, -0.3]], dtype=np.float32) - state.info["commands"][:] = commands - state.info["command_resampling_time"][:] = (0.0, 1.0, 0.0) + env._commands[:] = commands + env._command_resampling_time[:] = (0.0, 1.0, 0.0) monkeypatch.setattr(env, "resample_commands", lambda num_envs: replacement[:num_envs].copy()) - env._update_commands(state.info) + env._update_commands() - np.testing.assert_array_equal(state.info["commands"][[0, 2]], replacement) - np.testing.assert_array_equal(state.info["commands"][[1]], commands[[1]]) - assert state.info["command_resampling_time"][1] == pytest.approx(1.0 - env.cfg.ctrl_dt) - assert np.all(state.info["command_resampling_time"][[0, 2]] > 0.0) + np.testing.assert_array_equal(env._commands[[0, 2]], replacement) + np.testing.assert_array_equal(env._commands[[1]], commands[[1]]) + assert env._command_resampling_time[1] == pytest.approx(1.0 - env.cfg.ctrl_dt) + assert np.all(env._command_resampling_time[[0, 2]] > 0.0) def test_random_planar_velocity_binding_is_seeded_vectorized_and_task_specific(): @@ -260,8 +258,8 @@ def test_go2_partial_reset_only_resamples_finished_environments(monkeypatch: pyt ], dtype=np.float32, ) - state.info["commands"][:] = commands - action_delay_steps = state.info["action_delay_steps"].copy() + env._commands[:] = commands + action_delay_steps = env._action_delay_steps.copy() _, kp, damping, friction = _read_param_overrides(env) mass, center_of_mass = _read_mass_overrides(env) state.terminated[:] = (False, True, False) @@ -270,9 +268,9 @@ def test_go2_partial_reset_only_resamples_finished_environments(monkeypatch: pyt env._reset_done_envs() - np.testing.assert_array_equal(state.info["commands"][[0, 2]], commands[[0, 2]]) - np.testing.assert_array_equal(state.info["commands"][[1]], replacement) - np.testing.assert_array_equal(state.info["action_delay_steps"][[0, 2]], action_delay_steps[[0, 2]]) + np.testing.assert_array_equal(env._commands[[0, 2]], commands[[0, 2]]) + np.testing.assert_array_equal(env._commands[[1]], replacement) + np.testing.assert_array_equal(env._action_delay_steps[[0, 2]], action_delay_steps[[0, 2]]) _, kp_after, damping_after, friction_after = _read_param_overrides(env) mass_after, com_after = _read_mass_overrides(env) np.testing.assert_array_equal(kp_after[[0, 2]], kp[[0, 2]]) @@ -298,7 +296,7 @@ def test_go2_play_reset_preserves_nominal_joint_state_and_runtime_parameters(): "sliding_friction", "base_mass_scale", "base_com_offset", - }.isdisjoint(env.state.info) + }.isdisjoint(env.state.reward_terms) mass, center_of_mass = _read_mass_overrides(env) nominal_mass, nominal_center_of_mass = _nominal_masses(env) np.testing.assert_array_equal(mass, nominal_mass) @@ -335,16 +333,15 @@ def test_quadruped_randomization_config_rejects_invalid_ranges(update, message): def test_zero_velocity_command_freezes_gait_phase(): env = registry.make("go2-walk-flat", num_envs=2, mode="train") - info = { - "commands": np.zeros((2, 3), dtype=np.float32), - "phase": np.full(2, 0.5, dtype=np.float32), - "feet_phase": np.full((2, env._num_feet), 0.5, dtype=np.float32), - } + env.init_state() + env._commands[:] = 0.0 + env._phase[:] = 0.5 + env._feet_phase[:] = 0.5 - env._advance_phase(info) + env._advance_phase() - np.testing.assert_array_equal(info["phase"], np.zeros(2, dtype=np.float32)) - np.testing.assert_array_equal(info["feet_phase"], np.zeros((2, env._num_feet), dtype=np.float32)) + np.testing.assert_array_equal(env._phase, np.zeros(2, dtype=np.float32)) + np.testing.assert_array_equal(env._feet_phase, np.zeros((2, env._num_feet), dtype=np.float32)) @pytest.mark.parametrize( @@ -383,7 +380,7 @@ def test_quadruped_rough_walk_reset_and_base_height_are_terrain_relative(env_nam assert np.all(relative_height >= env.cfg.initial_base_position[2] - 1e-6) assert np.all(relative_height <= env.cfg.initial_base_position[2] + height_scale + 1e-6) expected = np.square(relative_height - env.cfg.reward_config.base_height_target) - np.testing.assert_allclose(env._reward_base_height(None, None, None, None), expected) + np.testing.assert_allclose(env._reward_base_height(None, None, None), expected) next_state = env.step(np.zeros((env.num_envs, *env.action_space.shape), dtype=np.float32)) assert next_state.reward.shape == (env.num_envs,) @@ -442,18 +439,11 @@ def test_swing_contact_penalty_detects_dragging_feet(quadruped_env): no_contacts = np.zeros((1, quadruped_env._num_feet), dtype=bool) all_contacts = np.ones((1, quadruped_env._num_feet), dtype=bool) - no_drag = quadruped_env._reward_swing_contact( - {"feet_phase": feet_phase, "contacts": no_contacts}, - None, - None, - None, - ) - all_drag = quadruped_env._reward_swing_contact( - {"feet_phase": feet_phase, "contacts": all_contacts}, - None, - None, - None, - ) + quadruped_env._feet_phase[:] = feet_phase + quadruped_env.feet_contact[:] = no_contacts + no_drag = quadruped_env._reward_swing_contact(None, None, None) + quadruped_env.feet_contact[:] = all_contacts + all_drag = quadruped_env._reward_swing_contact(None, None, None) np.testing.assert_array_equal(no_drag, np.zeros((1,), dtype=np.float32)) np.testing.assert_array_equal(all_drag, np.ones((1,), dtype=np.float32)) diff --git a/motrix_rl/src/motrix_rl/deploy/source_rollout.py b/motrix_rl/src/motrix_rl/deploy/source_rollout.py index 70d5f9fe..1257b87a 100644 --- a/motrix_rl/src/motrix_rl/deploy/source_rollout.py +++ b/motrix_rl/src/motrix_rl/deploy/source_rollout.py @@ -35,7 +35,9 @@ def validate_motrixsim_source_rollout( env.cfg.noise_config.level = 0.0 env.cfg.spawn_xy_range = 0.0 state = env.init_state() - state.info["commands"][0] = command + # Walk-flavor tasks keep their velocity commands in the episode-scoped + # ``_commands`` buffer; pin row 0 so the rollout drives a fixed command. + env._commands[0] = command observation = np.asarray(state.obs.policy[0], dtype=np.float32) reset_observation = observation.tolist() first_outputs: list[list[float]] = [] @@ -55,7 +57,7 @@ def validate_motrixsim_source_rollout( if bool(state.terminated[0]): exit_reason = "terminated" break - state.info["commands"][0] = command + env._commands[0] = command observation = np.asarray(state.obs.policy[0], dtype=np.float32) if not np.isfinite(observation).all(): exit_reason = "invalid_observation" diff --git a/motrix_rl/src/motrix_rl/fastsac/wrap_np.py b/motrix_rl/src/motrix_rl/fastsac/wrap_np.py index d5baa56a..1937cb5d 100644 --- a/motrix_rl/src/motrix_rl/fastsac/wrap_np.py +++ b/motrix_rl/src/motrix_rl/fastsac/wrap_np.py @@ -34,7 +34,7 @@ def _to_torch(self, arr: np.ndarray, dtype=torch.float32) -> torch.Tensor: def reset(self) -> tuple[torch.Tensor, torch.Tensor]: state = self._env.init_state() - self.last_info = state.info + self.last_info = env_infos(state) return self._to_torch(state.obs.policy), self._to_torch(state.obs.value_or_policy) def step(self, actions: torch.Tensor): diff --git a/motrix_rl/src/motrix_rl/fastsac/wrap_torch.py b/motrix_rl/src/motrix_rl/fastsac/wrap_torch.py index 22a05e80..764231c3 100644 --- a/motrix_rl/src/motrix_rl/fastsac/wrap_torch.py +++ b/motrix_rl/src/motrix_rl/fastsac/wrap_torch.py @@ -35,7 +35,7 @@ def _to_torch(self, tensor: torch.Tensor, dtype=torch.float32) -> torch.Tensor: def reset(self) -> tuple[torch.Tensor, torch.Tensor]: state = self._env.init_state() - self.last_info = state.info + self.last_info = env_infos(state) return self._to_torch(state.obs.policy), self._to_torch(state.obs.value_or_policy) def step(self, actions: torch.Tensor): diff --git a/motrix_rl/src/motrix_rl/rslrl/torch/wrap_np.py b/motrix_rl/src/motrix_rl/rslrl/torch/wrap_np.py index 745c09c8..4283a933 100644 --- a/motrix_rl/src/motrix_rl/rslrl/torch/wrap_np.py +++ b/motrix_rl/src/motrix_rl/rslrl/torch/wrap_np.py @@ -102,9 +102,8 @@ def step(self, actions: torch.Tensor) -> tuple[TensorDict, torch.Tensor, torch.T obs = self._to_tensordict(state.obs) # Build extras dict (RSLRL calls it "extras" not "infos") - extras = {} - if "time_outs" in state.info: - extras["time_outs"] = torch.from_numpy(state.info["time_outs"]).to(self._device) + # time_outs: rows truncated without failing, for value bootstrapping. + extras = {"time_outs": torch.from_numpy(state.truncated & ~state.terminated).to(self._device)} return obs, rewards, dones, extras diff --git a/motrix_rl/src/motrix_rl/rslrl/torch/wrap_torch.py b/motrix_rl/src/motrix_rl/rslrl/torch/wrap_torch.py index 7f8cbc31..71476aa7 100644 --- a/motrix_rl/src/motrix_rl/rslrl/torch/wrap_torch.py +++ b/motrix_rl/src/motrix_rl/rslrl/torch/wrap_torch.py @@ -99,9 +99,8 @@ def step(self, actions: torch.Tensor) -> tuple[TensorDict, torch.Tensor, torch.T obs = self._to_tensordict(state.obs) # Build extras dict (RSLRL calls it "extras" not "infos") - extras = {} - if "time_outs" in state.info: - extras["time_outs"] = torch.as_tensor(state.info["time_outs"], device=self._device) + # time_outs: rows truncated without failing, for value bootstrapping. + extras = {"time_outs": (state.truncated & ~state.terminated).to(self._device)} return obs, rewards, dones, extras diff --git a/motrix_rl/src/motrix_rl/skrl/jax/wrap_np.py b/motrix_rl/src/motrix_rl/skrl/jax/wrap_np.py index c6a22d07..8dc34104 100644 --- a/motrix_rl/src/motrix_rl/skrl/jax/wrap_np.py +++ b/motrix_rl/src/motrix_rl/skrl/jax/wrap_np.py @@ -29,7 +29,7 @@ def __init__(self, env: DirectEnv, render: RenderConfig | None = None): def reset(self) -> tuple[jax.Array, Any]: state = self._env.init_state() - return jnp.asarray(state.obs.policy, dtype=jnp.float32), state.info + return jnp.asarray(state.obs.policy, dtype=jnp.float32), env_infos(state) def step( self, actions: jax.Array diff --git a/motrix_rl/src/motrix_rl/skrl/jax/wrap_torch.py b/motrix_rl/src/motrix_rl/skrl/jax/wrap_torch.py index 4ffb19b0..06fb8f29 100644 --- a/motrix_rl/src/motrix_rl/skrl/jax/wrap_torch.py +++ b/motrix_rl/src/motrix_rl/skrl/jax/wrap_torch.py @@ -37,7 +37,7 @@ def _to_jax(self, tensor: torch.Tensor, dtype) -> jax.Array: def reset(self) -> tuple[jax.Array, Any]: state = self._env.init_state() - return self._to_jax(state.obs.policy, jnp.float32), state.info + return self._to_jax(state.obs.policy, jnp.float32), env_infos(state) def step( self, actions: jax.Array diff --git a/motrix_rl/src/motrix_rl/skrl/torch/wrap_np.py b/motrix_rl/src/motrix_rl/skrl/torch/wrap_np.py index b8acf000..6304f266 100644 --- a/motrix_rl/src/motrix_rl/skrl/torch/wrap_np.py +++ b/motrix_rl/src/motrix_rl/skrl/torch/wrap_np.py @@ -27,7 +27,7 @@ def __init__(self, env: DirectEnv, render: RenderConfig | None = None): def reset(self) -> tuple[torch.Tensor, Any]: state = self._env.init_state() - return torch.tensor(state.obs.policy, dtype=torch.float32, device=self.device), state.info + return torch.tensor(state.obs.policy, dtype=torch.float32, device=self.device), env_infos(state) def step( self, actions: torch.Tensor diff --git a/motrix_rl/src/motrix_rl/skrl/torch/wrap_torch.py b/motrix_rl/src/motrix_rl/skrl/torch/wrap_torch.py index b07dc833..e697c3b4 100644 --- a/motrix_rl/src/motrix_rl/skrl/torch/wrap_torch.py +++ b/motrix_rl/src/motrix_rl/skrl/torch/wrap_torch.py @@ -30,7 +30,7 @@ def _to_train_device(self, tensor: torch.Tensor, dtype: torch.dtype) -> torch.Te def reset(self) -> tuple[torch.Tensor, Any]: state = self._env.init_state() - return self._to_train_device(state.obs.policy, torch.float32), state.info + return self._to_train_device(state.obs.policy, torch.float32), env_infos(state) def step( self, actions: torch.Tensor diff --git a/motrix_rl/src/motrix_rl/utils.py b/motrix_rl/src/motrix_rl/utils.py index 47743cc0..84072fe3 100644 --- a/motrix_rl/src/motrix_rl/utils.py +++ b/motrix_rl/src/motrix_rl/utils.py @@ -73,8 +73,9 @@ def class_to_dict(obj) -> dict | list | Any: def env_infos(state) -> dict: """Compose Gym-style step infos for the RL-library boundary. - The env state keeps metrics as a first-class field; RL libraries receive - their reduced batch-level scalars merged under ``infos["metrics"]`` alongside - the raw info dict. + The env state keeps the per-term reward breakdown (``reward_terms``) and + metrics as first-class fields; RL libraries receive the breakdown under + ``infos["Reward"]`` and reduced batch-level scalar metrics under + ``infos["metrics"]``. """ - return {**state.info, "metrics": state.process_metrics()} + return {"Reward": state.reward_terms, "metrics": state.process_metrics()} diff --git a/motrix_rl/tests/test_rl_sim_backend.py b/motrix_rl/tests/test_rl_sim_backend.py index b76a2f55..30d6e8f4 100644 --- a/motrix_rl/tests/test_rl_sim_backend.py +++ b/motrix_rl/tests/test_rl_sim_backend.py @@ -46,7 +46,6 @@ def _make_env(sim_backend: str): terminated=np.zeros(_NUM_ENVS, dtype=bool), truncated=np.zeros(_NUM_ENVS, dtype=bool), episode_steps=np.zeros(_NUM_ENVS, dtype=np.uint64), - info={"time_outs": np.zeros(_NUM_ENVS, dtype=bool)}, ) else: env = Mock(spec=TorchEnv) @@ -60,7 +59,6 @@ def _make_env(sim_backend: str): terminated=torch.zeros(_NUM_ENVS, dtype=torch.bool, device=env.device), truncated=torch.zeros(_NUM_ENVS, dtype=torch.bool, device=env.device), episode_steps=torch.zeros(_NUM_ENVS, dtype=torch.int64, device=env.device), - info={"time_outs": torch.zeros(_NUM_ENVS, dtype=torch.bool, device=env.device)}, ) env.cfg = SimpleNamespace(max_episode_steps=100) @@ -207,7 +205,6 @@ def _make_state(self, value: float) -> ArrayEnvState: terminated=np.zeros(self.num_envs, dtype=bool), truncated=np.zeros(self.num_envs, dtype=bool), episode_steps=np.zeros(self.num_envs, dtype=np.uint64), - info=_async_info(lambda data: np.asarray(data, dtype=np.float32)), metrics={"progress": 0.5}, ) @@ -221,7 +218,6 @@ def _make_state(self, value: float) -> TorchEnvState: terminated=torch.zeros(self.num_envs, dtype=torch.bool), truncated=torch.zeros(self.num_envs, dtype=torch.bool), episode_steps=torch.zeros(self.num_envs, dtype=torch.int64), - info=_async_info(lambda data: torch.as_tensor(data, dtype=torch.float32)), metrics={"progress": 0.5}, )