diff --git a/motrix_envs/src/motrix_envs/locomotion/go1/walk_np.py b/motrix_envs/src/motrix_envs/locomotion/go1/walk_np.py index 39112bd..fc9e6bf 100644 --- a/motrix_envs/src/motrix_envs/locomotion/go1/walk_np.py +++ b/motrix_envs/src/motrix_envs/locomotion/go1/walk_np.py @@ -172,8 +172,10 @@ class Go1WalkTask(NpEnv): def apply_action(self, actions, state): state.info["last_dof_vel"] = self.get_dof_vel(state.data) - state.info["last_actions"] = state.info["current_actions"] - state.info["current_actions"] = actions + if "last_last_actions" in state.info: + state.info["last_last_actions"] = state.info["last_actions"].copy() + state.info["last_actions"] = state.info["current_actions"].copy() + state.info["current_actions"] = np.array(actions, copy=True) state.data.actuator_ctrls = self._compute_torques(actions, state.data) return state diff --git a/motrix_envs/tests/test_dreamwaq_state_safety.py b/motrix_envs/tests/test_dreamwaq_state_safety.py index 761a6a0..f75a0fe 100644 --- a/motrix_envs/tests/test_dreamwaq_state_safety.py +++ b/motrix_envs/tests/test_dreamwaq_state_safety.py @@ -1,6 +1,9 @@ +from types import SimpleNamespace + import numpy as np from motrix_envs.locomotion.go1.dreamwaq import _sanitize_dof_pos +from motrix_envs.locomotion.go1.walk_np import Go1WalkTask def test_sanitize_dof_pos_handles_zero_quaternion_with_nonfinite_joints(): @@ -17,3 +20,25 @@ def test_sanitize_dof_pos_handles_zero_quaternion_with_nonfinite_joints(): np.testing.assert_allclose(np.linalg.norm(clean[:, 3:7], axis=1), 1.0) assert clean[0, 7] == 0.0 assert clean[1, 8] == 0.0 + + +def test_apply_action_advances_three_frame_action_history(): + task = Go1WalkTask.__new__(Go1WalkTask) + task.get_dof_vel = lambda data: np.ones((1, 12), dtype=np.float32) + task._compute_torques = lambda actions, data: actions + state = SimpleNamespace( + data=SimpleNamespace(actuator_ctrls=None), + info={ + "current_actions": np.full((1, 12), 2.0, dtype=np.float32), + "last_actions": np.full((1, 12), 1.0, dtype=np.float32), + "last_last_actions": np.zeros((1, 12), dtype=np.float32), + }, + ) + actions = np.full((1, 12), 3.0, dtype=np.float32) + + task.apply_action(actions, state) + actions.fill(9.0) + + np.testing.assert_array_equal(state.info["last_last_actions"], 1.0) + np.testing.assert_array_equal(state.info["last_actions"], 2.0) + np.testing.assert_array_equal(state.info["current_actions"], 3.0) diff --git a/scripts/dreamwaq_sim2sim_mujoco.py b/scripts/dreamwaq_sim2sim_mujoco.py index da77da5..5297b69 100644 --- a/scripts/dreamwaq_sim2sim_mujoco.py +++ b/scripts/dreamwaq_sim2sim_mujoco.py @@ -201,6 +201,10 @@ def parse_args(): ) parser.add_argument("--timeout", type=float, default=30.0, help="validation timeout in simulation seconds") parser.add_argument("--log-interval", type=float, default=0.5, help="pose log interval in simulation seconds") + parser.add_argument( + "--realtime-factor", type=float, default=1.0, + help="viewer playback speed relative to wall time (default: 1.0)", + ) parser.add_argument("--manual", action="store_true", help="start with zero velocity and use keyboard commands") parser.add_argument("--headless", action="store_true", help="run validation without viewer or real-time delay") parser.add_argument("--no-validation", action="store_true", help="do not stop with automatic PASS/FAIL") @@ -217,6 +221,9 @@ def main(): if args.headless and args.no_validation: print("[ERROR] --headless requires automatic validation", file=sys.stderr) return 2 + if args.realtime_factor <= 0.0: + print("[ERROR] --realtime-factor must be positive", file=sys.stderr) + return 2 terrain_map = { "flat": "scene_dreamwaq_flat.xml", @@ -292,6 +299,7 @@ def main(): "down={platform_end:.2f}->{down_end:.2f}m pass_x={pass_x:.2f}m".format(**course) ) if not args.headless: + print(f"[Viewer] realtime_factor={args.realtime_factor:.2f}x") print("[CTRL] W/S forward/back, Q/E lateral, A/D yaw, Space stop, R reset, Esc quit") keyboard = None @@ -423,7 +431,7 @@ def main(): if view is not None: view.sync() - deadline = wall_start + data.time + deadline = wall_start + data.time / args.realtime_factor remaining = deadline - time.monotonic() if remaining > 0: time.sleep(remaining) diff --git a/scripts/run_dreamwaq_sim2sim.sh b/scripts/run_dreamwaq_sim2sim.sh index 74711a9..f95e8bd 100755 --- a/scripts/run_dreamwaq_sim2sim.sh +++ b/scripts/run_dreamwaq_sim2sim.sh @@ -11,6 +11,7 @@ terrain="stairs_box" level="0" forward_speed="0.5" timeout="30" +realtime_factor="1.0" export_only=false manual=false headless=false @@ -31,6 +32,8 @@ Options: -l, --level N Terrain difficulty level passed to sim2sim. Default: 0. --speed MPS Autonomous forward command. Default: 0.5. --timeout SEC Validation timeout in simulation seconds. Default: 30. + --realtime-factor X + Viewer playback speed. Default: 1.0 (real time). --manual Start at zero velocity and use keyboard commands. --headless Run validation without a viewer or real-time delay. --no-validation Run until the viewer closes instead of returning PASS/FAIL. @@ -82,6 +85,11 @@ while (($#)); do timeout="$2" shift 2 ;; + --realtime-factor) + (($# >= 2)) || die "$1 requires a number" + realtime_factor="$2" + shift 2 + ;; --manual) manual=true shift @@ -115,6 +123,8 @@ esac [[ "$level" =~ ^[0-9]+$ ]] || die "level must be a non-negative integer: ${level}" [[ "$forward_speed" =~ ^[0-9]+([.][0-9]+)?$ ]] || die "speed must be a non-negative number: ${forward_speed}" [[ "$timeout" =~ ^[0-9]+([.][0-9]+)?$ ]] || die "timeout must be a non-negative number: ${timeout}" +[[ "$realtime_factor" =~ ^[0-9]+([.][0-9]+)?$ ]] || die "realtime-factor must be a positive number: ${realtime_factor}" +[[ "$realtime_factor" != "0" && "$realtime_factor" != "0.0" ]] || die "realtime-factor must be positive" command -v uv >/dev/null 2>&1 || die "uv is not available in PATH" if [[ -z "$checkpoint" ]]; then @@ -159,6 +169,7 @@ sim_args=( --level "$level" --forward-speed "$forward_speed" --timeout "$timeout" + --realtime-factor "$realtime_factor" ) [[ "$manual" == true ]] && sim_args+=(--manual) [[ "$headless" == true ]] && sim_args+=(--headless)