From d98c0279ae2a2a2c8d97eac58c87f315450312fe Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Wed, 1 Jul 2026 20:36:43 +0800 Subject: [PATCH 01/59] Refactor benchmark to pinned clip protocol --- AGENTS.md | 4 +- docs/docs/reference/architecture.md | 2 +- .../reference/training-troubleshooting.md | 33 +- docs/docs/tutorials/training.md | 15 +- .../current/reference/architecture.md | 2 +- .../reference/training-troubleshooting.md | 33 +- .../current/tutorials/training.md | 15 +- tests/test_benchmark_omnixtreme.py | 423 +++++++++ train_mimic/benchmarking.py | 343 +++++++ train_mimic/scripts/benchmark.py | 845 +++++++----------- train_mimic/tasks/tracking/mdp/commands.py | 62 +- 11 files changed, 1178 insertions(+), 599 deletions(-) create mode 100644 tests/test_benchmark_omnixtreme.py create mode 100644 train_mimic/benchmarking.py diff --git a/AGENTS.md b/AGENTS.md index 5024575d..b28a24ef 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -78,7 +78,7 @@ train_mimic/ # Training package └── scripts/ ├── train.py # Training entry point ├── play.py # Checkpoint playback - ├── benchmark.py # Policy evaluation with tracking errors + ├── benchmark.py # OmniXtreme-style policy benchmark └── save_onnx.py # Export TemporalCNN ONNX ``` @@ -203,7 +203,7 @@ The single supported training task is `General-Tracking-G1` (experiment name: `g - Training env uses `sampling_mode="rewind"` - Tracking rewards include root position/orientation/linear velocity/angular velocity, body pose/velocity, joint position/velocity, survival, action-rate, joint-limit, self-collision, and ankle acceleration terms - Supported motion sampling modes are `uniform`, `start`, and `rewind`; `rewind` restarts failed environments from the same clip after stepping back `rewind_min_steps..rewind_max_steps` with probability `rewind_prob`, otherwise it falls back to uniform sampling -- Playback/benchmark use `play=True`, which switches motion sampling to `start` +- Playback and benchmark use `play=True`, which switches motion sampling to `start`; benchmark pins exact clip ids/start times, disables clip-end resampling, and reports `MPJPE(mm)`, `delta_vel(mm/frame)`, `delta_acc(mm/frame^2)`, and `success_rate(%)` - `window_steps=[0]` - `save_onnx.py` exports dual-input TemporalCNN ONNX diff --git a/docs/docs/reference/architecture.md b/docs/docs/reference/architecture.md index f979d687..6e384ad9 100644 --- a/docs/docs/reference/architecture.md +++ b/docs/docs/reference/architecture.md @@ -58,7 +58,7 @@ train_mimic/scripts/data | Inference observation | `velcmd_history` (167D) | | ONNX signature | Dual-input `obs` (167D) + `obs_history` | | Actor/Critic | TemporalCNN (2048, 1024, 512, 256, 128) | -| Training sampling | Default `rewind`; also supports `uniform`; playback/benchmark use `start` | +| Training sampling | Default `rewind`; also supports `uniform`; playback uses `start`; benchmark pins exact clips and disables clip-end resampling | | Training `window_steps` | `[0]` | | Data format | Minimal recursive HDF5 shards (`shard_*.h5`) | diff --git a/docs/docs/reference/training-troubleshooting.md b/docs/docs/reference/training-troubleshooting.md index b683c6c4..16665e9b 100644 --- a/docs/docs/reference/training-troubleshooting.md +++ b/docs/docs/reference/training-troubleshooting.md @@ -117,38 +117,7 @@ Only modifying the robot XML is insufficient - the simulation-level `njmax` in m --- -## Issue 5: Benchmark Video Problems - -### Video has only 1 frame - -Ensure `num_eval_steps >= video_length`: - -```bash -python train_mimic/scripts/benchmark.py \ - --checkpoint logs/rsl_rl/g1_general_tracking//model_30000.pt \ - --motion_file data/datasets/_precomputed \ - --num_envs 1 --num_eval_steps 2000 \ - --video --video_length 600 -``` - -### EGL/OpenGL errors - -Install OpenGL/EGL dependencies: - -```bash -conda install -c conda-forge libopengl libglx libegl libglvnd pyopengl -``` - -If GPU EGL is unavailable, try CPU rendering: - -```bash -MUJOCO_GL=osmesa PYOPENGL_PLATFORM=osmesa \ - python train_mimic/scripts/benchmark.py ... --video -``` - ---- - -## Issue 6: Foot Sliding in Sim2Sim (Benchmark OK but ONNX Inference Slides) +## Issue 5: Foot Sliding in Sim2Sim (Benchmark OK but ONNX Inference Slides) ### Root Cause diff --git a/docs/docs/tutorials/training.md b/docs/docs/tutorials/training.md index fcf5cc2a..e26ed9fa 100644 --- a/docs/docs/tutorials/training.md +++ b/docs/docs/tutorials/training.md @@ -115,19 +115,10 @@ python train_mimic/scripts/play.py \ python train_mimic/scripts/benchmark.py \ --checkpoint logs/rsl_rl/g1_general_tracking//model_30000.pt \ --motion_file data/datasets_precomputed \ - --num_envs 1 + --num_envs 32 ``` -### Benchmark with Video - -```bash -python train_mimic/scripts/benchmark.py \ - --checkpoint logs/rsl_rl/g1_general_tracking//model_30000.pt \ - --motion_file data/datasets_precomputed \ - --num_envs 1 \ - --video \ - --video_length 600 -``` +The benchmark uses an OmniXtreme-style protocol: 10-second clips, one deterministic rollout per eligible clip, and `MPJPE(mm)`, `delta_vel(mm/frame)`, `delta_acc(mm/frame^2)`, and `success_rate(%)` outputs. It uses play-mode observations without training noise and pins exact clip ids/start times without clip-end resampling. `--motion_file` must point to a precomputed training dataset; all clips long enough for the configured clip length are evaluated. ## Training Architecture @@ -142,4 +133,4 @@ Key files: - `train_mimic/app.py` - Shared entry point for train/play/benchmark - `train_mimic/tasks/tracking/config/env.py` - General-Tracking-G1 env builder - `train_mimic/tasks/tracking/config/rl.py` - TemporalCNN PPO config -- `train_mimic/tasks/tracking/mdp/commands.py` - Supports `uniform`, `start`, and `rewind` sampling modes. Training defaults to `rewind`; playback/benchmark use `start`. +- `train_mimic/tasks/tracking/mdp/commands.py` - Supports `uniform`, `start`, and `rewind` sampling modes. Training defaults to `rewind`; playback uses `start`; benchmark pins exact clip ids and start times. diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/architecture.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/architecture.md index 5baef6ff..2000679e 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/architecture.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/architecture.md @@ -58,7 +58,7 @@ train_mimic/scripts/data | 推理观测 | `velcmd_history`（167D） | | ONNX 签名 | 双输入 `obs`（167D）+ `obs_history` | | Actor/Critic | TemporalCNN（2048、1024、512、256、128） | -| 训练采样 | 默认 `rewind`；也支持 `uniform`；播放/评估使用 `start` | +| 训练采样 | 默认 `rewind`；也支持 `uniform`；播放使用 `start`；benchmark 固定精确 clip 并禁用 clip 末尾重采样 | | 训练 `window_steps` | `[0]` | | 数据格式 | 可递归发现的最小 HDF5 shard（`shard_*.h5`） | diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/training-troubleshooting.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/training-troubleshooting.md index 14a8500a..6e2e72df 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/training-troubleshooting.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/training-troubleshooting.md @@ -117,38 +117,7 @@ self.sim.nconmax = 150_000 --- -## 问题 5：Benchmark 视频问题 - -### 视频只有 1 帧 - -确保 `num_eval_steps >= video_length`： - -```bash -python train_mimic/scripts/benchmark.py \ - --checkpoint logs/rsl_rl/g1_general_tracking//model_30000.pt \ - --motion_file data/datasets/_precomputed \ - --num_envs 1 --num_eval_steps 2000 \ - --video --video_length 600 -``` - -### EGL/OpenGL 错误 - -安装 OpenGL/EGL 依赖： - -```bash -conda install -c conda-forge libopengl libglx libegl libglvnd pyopengl -``` - -如果 GPU EGL 不可用，尝试 CPU 渲染： - -```bash -MUJOCO_GL=osmesa PYOPENGL_PLATFORM=osmesa \ - python train_mimic/scripts/benchmark.py ... --video -``` - ---- - -## 问题 6：Sim2Sim 脚滑（Benchmark 正常但 ONNX 推理脚打滑） +## 问题 5：Sim2Sim 脚滑（Benchmark 正常但 ONNX 推理脚打滑） ### 根本原因 diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/training.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/training.md index 1de8d8e1..09610633 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/training.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/training.md @@ -114,19 +114,10 @@ python train_mimic/scripts/play.py \ python train_mimic/scripts/benchmark.py \ --checkpoint logs/rsl_rl/g1_general_tracking//model_30000.pt \ --motion_file data/datasets_precomputed \ - --num_envs 1 + --num_envs 32 ``` -### 带视频的定量评估 - -```bash -python train_mimic/scripts/benchmark.py \ - --checkpoint logs/rsl_rl/g1_general_tracking//model_30000.pt \ - --motion_file data/datasets_precomputed \ - --num_envs 1 \ - --video \ - --video_length 600 -``` +benchmark 使用 OmniXtreme 风格协议：10 秒 clip、每个合格 clip 进行一次确定性 rollout，并输出 `MPJPE(mm)`、`delta_vel(mm/frame)`、`delta_acc(mm/frame^2)` 和 `success_rate(%)`。它使用无训练噪声的 play-mode 观测，并固定精确 clip id/起始时间且禁用 clip 末尾重采样。`--motion_file` 必须指向预计算训练数据集；所有长度足够满足配置 clip 时长的 clip 都会参与评测。 ## 训练架构 @@ -141,4 +132,4 @@ train_mimic/scripts - `train_mimic/app.py` - 训练/播放/评估的统一入口 - `train_mimic/tasks/tracking/config/env.py` - General-Tracking-G1 环境构建器 - `train_mimic/tasks/tracking/config/rl.py` - TemporalCNN PPO 配置 -- `train_mimic/tasks/tracking/mdp/commands.py` - 支持 `uniform`、`start` 和 `rewind` 采样模式。训练默认使用 `rewind`；播放/评估使用 `start`。 +- `train_mimic/tasks/tracking/mdp/commands.py` - 支持 `uniform`、`start` 和 `rewind` 采样模式。训练默认使用 `rewind`；播放使用 `start`；benchmark 会固定精确的 clip id 和起始时间。 diff --git a/tests/test_benchmark_omnixtreme.py b/tests/test_benchmark_omnixtreme.py new file mode 100644 index 00000000..9ad6e5d3 --- /dev/null +++ b/tests/test_benchmark_omnixtreme.py @@ -0,0 +1,423 @@ +from __future__ import annotations + +from dataclasses import dataclass +from types import SimpleNamespace + +import pytest +import numpy as np +import torch + +from train_mimic.benchmarking import ( + BenchmarkJob, + ClipSpec, + RolloutResult, + build_benchmark_plan, + compute_tracking_metrics, + summarize_rollouts, + write_benchmark_outputs, +) +from train_mimic.tasks.tracking.mdp.commands import MotionCommand +from train_mimic.scripts.benchmark import _configure_benchmark_env_cfg, _run_batch, parse_args + + +@dataclass +class _FakeAgentCfg: + clip_actions: float | None = None + + +def _clip(clip_id: int, duration_s: float) -> ClipSpec: + return ClipSpec( + clip_id=clip_id, + shard_path="shard.h5", + shard_clip_index=clip_id, + frame_offset=clip_id * 1000, + num_frames=int(duration_s * 30) + 1, + fps=30.0, + sample_start_s=0.0, + sample_end_s=duration_s, + ) + + +def test_build_benchmark_plan_uses_all_eligible_clips() -> None: + plan = build_benchmark_plan( + [_clip(0, 10.0), _clip(1, 9.9), _clip(2, 12.0)], + clip_seconds=10.0, + step_dt=0.02, + ) + + assert plan.control_steps == 500 + assert [clip.clip_id for clip in plan.eligible_clips] == [0, 2] + assert [clip.clip_id for clip in plan.skipped_short_clips] == [1] + assert plan.jobs == ( + BenchmarkJob(job_id=0, clip_id=0, rollout_index=0, start_time_s=0.0), + BenchmarkJob(job_id=1, clip_id=2, rollout_index=0, start_time_s=0.0), + ) + + +def test_build_benchmark_plan_requires_integer_control_steps() -> None: + with pytest.raises(ValueError, match="integer number of control steps"): + build_benchmark_plan( + [_clip(0, 10.0)], + clip_seconds=10.0, + step_dt=0.03, + ) + + +def test_compute_tracking_metrics() -> None: + ref = np.array( + [ + [[0.0, 0.0, 0.0]], + [[1.0, 0.0, 0.0]], + [[3.0, 0.0, 0.0]], + ], + dtype=np.float32, + ) + robot = np.array( + [ + [[0.0, 0.0, 0.0]], + [[1.1, 0.0, 0.0]], + [[3.4, 0.0, 0.0]], + ], + dtype=np.float32, + ) + + metrics = compute_tracking_metrics(ref, robot) + + assert metrics["mpjpe_mm"] == pytest.approx((0.0 + 0.1 + 0.4) / 3.0 * 1000.0) + assert metrics["delta_vel_mm_per_frame"] == pytest.approx((0.1 + 0.3) / 2.0 * 1000.0) + assert metrics["delta_acc_mm_per_frame2"] == pytest.approx(0.2 * 1000.0) + + +def test_summarize_rollouts_aggregates_success_and_metrics() -> None: + results = [ + RolloutResult(0, 0, 0, True, 500, None, None, 10.0, 2.0, 1.0), + RolloutResult( + 1, + 0, + 1, + False, + 120, + 120, + "anchor_pos", + float("nan"), + float("nan"), + float("nan"), + ), + RolloutResult(2, 1, 0, True, 500, None, None, 30.0, 6.0, 5.0), + ] + + summary = summarize_rollouts(results) + + assert summary["global"]["success_rate"] == pytest.approx(200.0 / 3.0) + assert summary["global"]["mpjpe_mm"] == pytest.approx(20.0) + assert summary["per_clip"][0]["success_rate"] == pytest.approx(50.0) + assert summary["per_clip"][0]["mpjpe_mm"] == pytest.approx(10.0) + assert summary["per_clip"][1]["success_rate"] == pytest.approx(100.0) + + +def test_parse_args_rejects_removed_legacy_flags() -> None: + with pytest.raises(SystemExit): + parse_args( + [ + "--checkpoint", + "model.pt", + "--motion_file", + "data/datasets_precomputed", + "--num_eval_steps", + "2000", + ] + ) + + +def test_parse_args_rejects_rollouts_per_clip_flag() -> None: + with pytest.raises(SystemExit): + parse_args( + [ + "--checkpoint", + "model.pt", + "--motion_file", + "data/datasets_precomputed", + "--rollouts_per_clip", + "2", + ] + ) + + +def test_benchmark_env_cfg_disables_noise_events_and_clip_resample() -> None: + motion = SimpleNamespace( + motion_file="old", + sampling_mode="rewind", + resample_on_clip_end=True, + pose_range={"x": (-1.0, 1.0)}, + velocity_range={"x": (-1.0, 1.0)}, + joint_position_range=(-0.1, 0.1), + ) + cfg = SimpleNamespace( + commands={"motion": motion}, + events={"base_com": object(), "add_joint_default_pos": object()}, + episode_length_s=1.0, + auto_reset=True, + ) + + out = _configure_benchmark_env_cfg( + cfg, + motion_file="data/datasets_precomputed", + clip_seconds=10.0, + ) + + assert out.commands["motion"].motion_file == "data/datasets_precomputed" + assert out.commands["motion"].sampling_mode == "start" + assert out.commands["motion"].resample_on_clip_end is False + assert out.commands["motion"].pose_range == {} + assert out.commands["motion"].velocity_range == {} + assert out.commands["motion"].joint_position_range == (0.0, 0.0) + assert out.events == {} + assert out.episode_length_s == 10.0 + assert out.auto_reset is False + + +def test_reset_to_motion_rejects_sample_end_time() -> None: + cmd = SimpleNamespace() + cmd.device = "cpu" + cmd.motion_times = torch.zeros(1, dtype=torch.float32) + cmd.motion_ids = torch.zeros(1, dtype=torch.long) + cmd.time_left = torch.zeros(1, dtype=torch.float32) + cmd.motion = SimpleNamespace( + num_clips=1, + clip_sample_start_s=torch.tensor([0.0], dtype=torch.float32), + clip_sample_end_s=torch.tensor([10.0], dtype=torch.float32), + ) + + with pytest.raises(ValueError, match=r"range=\[0\.000000, 10\.000000\)"): + MotionCommand.reset_to_motion( + cmd, + torch.tensor([0]), + torch.tensor([0]), + torch.tensor([10.0]), + ) + + +def test_write_benchmark_outputs_serializes_failed_metrics_as_null(tmp_path) -> None: + plan = build_benchmark_plan( + [_clip(0, 10.0)], + clip_seconds=10.0, + step_dt=0.02, + ) + result = RolloutResult( + job_id=0, + clip_id=0, + rollout_index=0, + success=False, + steps=120, + failure_step=120, + failure_reason="anchor_pos", + mpjpe_mm=float("nan"), + delta_vel_mm_per_frame=float("nan"), + delta_acc_mm_per_frame2=float("nan"), + ) + + paths = write_benchmark_outputs( + tmp_path, + text_stem="benchmark", + metadata={ + "task": "General-Tracking-G1", + "checkpoint": "model.pt", + "motion_file": "dataset", + }, + plan=plan, + results=[result], + ) + + data = paths["summary_json"].read_text() + assert "NaN" not in data + report = __import__("json").loads(data) + assert report["global"]["mpjpe_mm"] is None + assert report["per_rollout"][0]["mpjpe_mm"] is None + + +def test_run_batch_resets_inactive_done_envs_and_excludes_failed_metrics(monkeypatch) -> None: + import train_mimic.scripts.benchmark as benchmark_script + + class FakeTensor: + def __init__(self, values): + self.values = np.asarray(values) + + def __or__(self, other): + return FakeTensor(self.values | other.values) + + def detach(self): + return self + + def cpu(self): + return self + + def numpy(self): + return self.values + + def item(self): + return bool(self.values) + + class FakeTorch: + long = "long" + float32 = "float32" + + class no_grad: + def __enter__(self): + return None + + def __exit__(self, exc_type, exc, tb): + return False + + @staticmethod + def tensor(values, dtype=None, device=None): + return list(values) + + @staticmethod + def arange(n, dtype=None, device=None): + return list(range(n)) + + class FakeCmd: + def reset_to_motion(self, env_ids, motion_ids, motion_times): + return None + + class FakeCommandManager: + def __init__(self): + self.cmd = FakeCmd() + + def get_term(self, name): + return self.cmd + + def compute(self, dt): + return None + + class FakeScene: + def write_data_to_sim(self): + return None + + class FakeSim: + def forward(self): + return None + + def sense(self): + return None + + class FakeObservationManager: + def __init__(self): + self.reset_calls = [] + + def reset(self, env_ids): + self.reset_calls.append(list(env_ids)) + return {} + + def compute(self, update_history): + return {"actor": np.zeros((2, 1), dtype=np.float32)} + + class FakeTermCfg: + time_out = False + + class FakeTerminationManager: + active_terms = ("failure",) + + def get_term_cfg(self, term_name): + return FakeTermCfg() + + def get_term(self, term_name): + return [FakeTensor(False), FakeTensor(True)] + + class FakeEnv: + instances = [] + + def __init__(self, cfg, device, render_mode): + self.cfg = cfg + self.device = device + self.scene = FakeScene() + self.sim = FakeSim() + self.command_manager = FakeCommandManager() + self.observation_manager = FakeObservationManager() + self.termination_manager = FakeTerminationManager() + self.reset_calls = [] + self.step_index = 0 + FakeEnv.instances.append(self) + + def reset(self, env_ids=None): + self.reset_calls.append(None if env_ids is None else list(env_ids)) + return {"actor": np.zeros((2, 1), dtype=np.float32)}, {} + + def step(self, actions): + self.step_index += 1 + if self.step_index == 1: + terminated = FakeTensor([False, True]) + truncated = FakeTensor([False, False]) + else: + terminated = FakeTensor([False, True]) + truncated = FakeTensor([True, False]) + return ( + {"actor": np.zeros((2, 1), dtype=np.float32)}, + None, + terminated, + truncated, + {}, + ) + + def close(self): + return None + + class FakeWrapper: + def __init__(self, env, clip_actions): + self.env = env + + class FakeRunner: + def __init__(self, wrapped_env, agent_dict, log_dir, device): + return None + + def load(self, checkpoint, map_location): + return None + + def get_inference_policy(self, device): + return lambda obs: np.zeros((2, 1), dtype=np.float32) + + def fake_aligned(_cmd): + ref = np.zeros((2, 1, 3), dtype=np.float32) + robot = np.zeros((2, 1, 3), dtype=np.float32) + return ref, robot + + monkeypatch.setattr(benchmark_script, "_aligned_keybody_positions", fake_aligned) + + motion = SimpleNamespace(motion_file="dataset") + base_env_cfg = SimpleNamespace( + commands={"motion": motion}, + events={}, + episode_length_s=1.0, + auto_reset=True, + scene=SimpleNamespace(num_envs=0), + ) + agent_cfg = _FakeAgentCfg() + jobs = [ + BenchmarkJob(0, 0, 0, 0.0), + BenchmarkJob(1, 1, 0, 0.0), + ] + + results = _run_batch( + batch_index=0, + jobs=jobs, + base_env_cfg=base_env_cfg, + agent_cfg=agent_cfg, + runner_cls=FakeRunner, + fallback_runner_cls=FakeRunner, + checkpoint="model.pt", + log_dir="logs", + device="cpu", + torch_module=FakeTorch, + ManagerBasedRlEnv=FakeEnv, + RslRlVecEnvWrapper=FakeWrapper, + clip_seconds=10.0, + control_steps=2, + seed=42, + ) + + env = FakeEnv.instances[-1] + assert env.reset_calls == [None, [1]] + assert env.observation_manager.reset_calls == [[0, 1]] + assert results[0].success is True + assert results[1].success is False + assert np.isnan(results[1].mpjpe_mm) diff --git a/train_mimic/benchmarking.py b/train_mimic/benchmarking.py new file mode 100644 index 00000000..6e4477f5 --- /dev/null +++ b/train_mimic/benchmarking.py @@ -0,0 +1,343 @@ +"""OmniXtreme-style benchmark helpers for motion tracking policies.""" + +from __future__ import annotations + +import csv +import json +from dataclasses import asdict, dataclass +from pathlib import Path +from typing import Any, Iterable, Sequence + +import h5py +import numpy as np + +from train_mimic.data.dataset_lib import ( + compute_clip_sample_ranges, + find_precomputed_motion_shards, + parse_window_steps, +) + + +@dataclass(frozen=True) +class ClipSpec: + clip_id: int + shard_path: str + shard_clip_index: int + frame_offset: int + num_frames: int + fps: float + sample_start_s: float + sample_end_s: float + + +@dataclass(frozen=True) +class BenchmarkJob: + job_id: int + clip_id: int + rollout_index: int + start_time_s: float + + +@dataclass(frozen=True) +class BenchmarkPlan: + clip_seconds: float + control_steps: int + step_dt: float + eligible_clips: tuple[ClipSpec, ...] + skipped_short_clips: tuple[ClipSpec, ...] + jobs: tuple[BenchmarkJob, ...] + + +@dataclass(frozen=True) +class RolloutResult: + job_id: int + clip_id: int + rollout_index: int + success: bool + steps: int + failure_step: int | None + failure_reason: str | None + mpjpe_mm: float + delta_vel_mm_per_frame: float + delta_acc_mm_per_frame2: float + + +def _json_safe(value: Any) -> Any: + if isinstance(value, float): + return value if np.isfinite(value) else None + if isinstance(value, dict): + return {key: _json_safe(item) for key, item in value.items()} + if isinstance(value, list): + return [_json_safe(item) for item in value] + return value + + +def load_clip_specs( + motion_file: str | Path, + *, + window_steps: Sequence[int] = (0,), +) -> tuple[ClipSpec, ...]: + """Load benchmark clip specs in the same order MotionLib assigns clip ids.""" + specs: list[ClipSpec] = [] + next_clip_id = 0 + steps = parse_window_steps(window_steps) + max_future = max((step for step in steps if step > 0), default=0) + max_history = -min((step for step in steps if step < 0), default=0) + min_clip_length = max_history + 1 + max_future + 1 + for shard_path in find_precomputed_motion_shards(Path(motion_file)): + with h5py.File(shard_path, "r") as h5: + starts = np.asarray(h5["clip_starts"], dtype=np.int64) + lengths = np.asarray(h5["clip_lengths"], dtype=np.int64) + fps = np.asarray(h5["clip_fps"], dtype=np.float32) + valid_mask = lengths >= min_clip_length + if not np.any(valid_mask): + continue + valid_starts = starts[valid_mask] + valid_lengths = lengths[valid_mask] + valid_fps = fps[valid_mask] + valid_shard_clip_indices = np.nonzero(valid_mask)[0] + sample_starts, sample_ends = compute_clip_sample_ranges( + valid_lengths, + window_steps=steps, + ) + for shard_clip_index, start, length, cur_fps, sample_start, sample_end in zip( + valid_shard_clip_indices, + valid_starts, + valid_lengths, + valid_fps, + sample_starts, + sample_ends, + strict=True, + ): + specs.append( + ClipSpec( + clip_id=next_clip_id, + shard_path=str(shard_path), + shard_clip_index=int(shard_clip_index), + frame_offset=int(start), + num_frames=int(length), + fps=float(cur_fps), + sample_start_s=float(sample_start) / float(cur_fps), + sample_end_s=float(sample_end) / float(cur_fps), + ) + ) + next_clip_id += 1 + return tuple(specs) + + +def build_benchmark_plan( + clips: Sequence[ClipSpec], + *, + clip_seconds: float, + step_dt: float, +) -> BenchmarkPlan: + if clip_seconds <= 0.0: + raise ValueError(f"clip_seconds must be > 0, got {clip_seconds}") + if step_dt <= 0.0: + raise ValueError(f"step_dt must be > 0, got {step_dt}") + + control_steps_f = clip_seconds / step_dt + control_steps = int(round(control_steps_f)) + if not np.isclose(control_steps_f, control_steps, atol=1e-6): + raise ValueError( + f"clip_seconds={clip_seconds} is not an integer number of control steps " + f"for step_dt={step_dt}" + ) + + eligible: list[ClipSpec] = [] + skipped: list[ClipSpec] = [] + for clip in clips: + duration_s = clip.sample_end_s - clip.sample_start_s + if duration_s + 1e-9 >= clip_seconds: + eligible.append(clip) + else: + skipped.append(clip) + if not eligible: + raise ValueError( + f"No clips are at least {clip_seconds:.3f}s long after applying valid sample ranges." + ) + + jobs: list[BenchmarkJob] = [] + for clip in eligible: + jobs.append( + BenchmarkJob( + job_id=len(jobs), + clip_id=clip.clip_id, + rollout_index=0, + start_time_s=clip.sample_start_s, + ) + ) + return BenchmarkPlan( + clip_seconds=clip_seconds, + control_steps=control_steps, + step_dt=step_dt, + eligible_clips=tuple(eligible), + skipped_short_clips=tuple(skipped), + jobs=tuple(jobs), + ) + + +def compute_tracking_metrics(aligned_ref_pos: np.ndarray, aligned_robot_pos: np.ndarray) -> dict[str, float]: + """Compute MPJPE, delta velocity, and delta acceleration from aligned key bodies. + + Inputs are ``(T, B, 3)`` arrays in root/anchor coordinates. Velocity and + acceleration are frame differences, matching the paper's mm/frame units. + """ + ref = np.asarray(aligned_ref_pos, dtype=np.float64) + robot = np.asarray(aligned_robot_pos, dtype=np.float64) + if ref.shape != robot.shape: + raise ValueError(f"aligned position shape mismatch: {ref.shape} vs {robot.shape}") + if ref.ndim != 3 or ref.shape[-1] != 3: + raise ValueError(f"aligned positions must be (T,B,3), got {ref.shape}") + if ref.shape[0] == 0 or ref.shape[1] == 0: + raise ValueError(f"aligned positions must have non-empty T and B dimensions, got {ref.shape}") + + pos_error = np.linalg.norm(ref - robot, axis=-1) + mpjpe = float(pos_error.mean() * 1000.0) + + if ref.shape[0] >= 2: + ref_vel = np.diff(ref, axis=0) + robot_vel = np.diff(robot, axis=0) + delta_vel = float(np.linalg.norm(ref_vel - robot_vel, axis=-1).mean() * 1000.0) + else: + delta_vel = float("nan") + + if ref.shape[0] >= 3: + ref_acc = np.diff(np.diff(ref, axis=0), axis=0) + robot_acc = np.diff(np.diff(robot, axis=0), axis=0) + delta_acc = float(np.linalg.norm(ref_acc - robot_acc, axis=-1).mean() * 1000.0) + else: + delta_acc = float("nan") + + return { + "mpjpe_mm": mpjpe, + "delta_vel_mm_per_frame": delta_vel, + "delta_acc_mm_per_frame2": delta_acc, + } + + +def summarize_rollouts(results: Sequence[RolloutResult]) -> dict[str, Any]: + if not results: + raise ValueError("Cannot summarize an empty benchmark result set") + + def finite_mean(values: Iterable[float]) -> float: + arr = np.asarray(list(values), dtype=np.float64) + arr = arr[np.isfinite(arr)] + if arr.size == 0: + return float("nan") + return float(arr.mean()) + + clip_ids = sorted({result.clip_id for result in results}) + per_clip: list[dict[str, Any]] = [] + for clip_id in clip_ids: + clip_results = [result for result in results if result.clip_id == clip_id] + per_clip.append( + { + "clip_id": clip_id, + "rollouts": len(clip_results), + "success_rate": 100.0 + * sum(1 for result in clip_results if result.success) + / len(clip_results), + "mpjpe_mm": finite_mean(result.mpjpe_mm for result in clip_results), + "delta_vel_mm_per_frame": finite_mean( + result.delta_vel_mm_per_frame for result in clip_results + ), + "delta_acc_mm_per_frame2": finite_mean( + result.delta_acc_mm_per_frame2 for result in clip_results + ), + } + ) + + return { + "global": { + "clips": len(clip_ids), + "rollouts": len(results), + "success_rate": 100.0 * sum(1 for result in results if result.success) / len(results), + "mpjpe_mm": finite_mean(result.mpjpe_mm for result in results), + "delta_vel_mm_per_frame": finite_mean( + result.delta_vel_mm_per_frame for result in results + ), + "delta_acc_mm_per_frame2": finite_mean( + result.delta_acc_mm_per_frame2 for result in results + ), + }, + "per_clip": per_clip, + } + + +def write_benchmark_outputs( + output_dir: str | Path, + *, + text_stem: str, + metadata: dict[str, Any], + plan: BenchmarkPlan, + results: Sequence[RolloutResult], +) -> dict[str, Path]: + output_path = Path(output_dir) + output_path.mkdir(parents=True, exist_ok=True) + summary = summarize_rollouts(results) + + txt_path = output_path / f"{text_stem}.txt" + json_path = output_path / f"{text_stem}.json" + per_clip_path = output_path / f"{text_stem}-per_clip.csv" + per_rollout_path = output_path / f"{text_stem}-per_rollout.csv" + + global_summary = summary["global"] + lines = [ + "OmniXtreme-style Benchmark Results", + f"checkpoint: {metadata['checkpoint']}", + f"motion_file: {metadata['motion_file']}", + f"clip_seconds: {plan.clip_seconds:.6f}", + f"control_steps: {plan.control_steps}", + f"eligible_clips: {len(plan.eligible_clips)}", + f"skipped_short_clips: {len(plan.skipped_short_clips)}", + "", + f"MPJPE(mm): {global_summary['mpjpe_mm']:.6f}", + f"delta_vel(mm/frame): {global_summary['delta_vel_mm_per_frame']:.6f}", + f"delta_acc(mm/frame^2): {global_summary['delta_acc_mm_per_frame2']:.6f}", + f"success_rate(%): {global_summary['success_rate']:.6f}", + ] + txt_path.write_text("\n".join(lines) + "\n") + + report = { + "metadata": metadata, + "protocol": { + "clip_seconds": plan.clip_seconds, + "control_steps": plan.control_steps, + "step_dt": plan.step_dt, + }, + "global": global_summary, + "per_clip": summary["per_clip"], + "per_rollout": [asdict(result) for result in results], + "eligible_clips": [asdict(clip) for clip in plan.eligible_clips], + "skipped_short_clips": [asdict(clip) for clip in plan.skipped_short_clips], + } + json_path.write_text(json.dumps(_json_safe(report), indent=2, allow_nan=False)) + + with per_clip_path.open("w", newline="") as f: + writer = csv.DictWriter( + f, + fieldnames=[ + "clip_id", + "rollouts", + "success_rate", + "mpjpe_mm", + "delta_vel_mm_per_frame", + "delta_acc_mm_per_frame2", + ], + ) + writer.writeheader() + writer.writerows(summary["per_clip"]) + + with per_rollout_path.open("w", newline="") as f: + writer = csv.DictWriter(f, fieldnames=list(asdict(results[0]).keys())) + writer.writeheader() + for result in results: + writer.writerow(asdict(result)) + + return { + "summary_txt": txt_path, + "summary_json": json_path, + "per_clip_csv": per_clip_path, + "per_rollout_csv": per_rollout_path, + } diff --git a/train_mimic/scripts/benchmark.py b/train_mimic/scripts/benchmark.py index 7fdcb632..211a6bc3 100644 --- a/train_mimic/scripts/benchmark.py +++ b/train_mimic/scripts/benchmark.py @@ -1,35 +1,33 @@ #!/usr/bin/env python3 -"""Benchmark a trained tracking policy on motion clips. +"""Benchmark a G1 motion tracking policy with an OmniXtreme-style protocol. -Runs policy rollout for a fixed number of evaluation steps and reports -distribution statistics for motion-tracking errors. - -Can optionally render and save benchmark videos for qualitative inspection. +Default protocol: + * 10 second clips at the policy control rate (500 steps at 50 Hz) + * One deterministic rollout per eligible motion clip + * MPJPE, delta velocity, delta acceleration, and success rate Usage: - # Benchmark only (no video) python train_mimic/scripts/benchmark.py \ - --checkpoint logs/rsl_rl/g1_tracking/.../model_30000.pt \ + --checkpoint logs/rsl_rl/g1_general_tracking//model_30000.pt \ --motion_file data/datasets_precomputed \ - --num_envs 1 - - # Single video (one continuous clip) - python train_mimic/scripts/benchmark.py ... --video --video_length 500 - - # Multiple separate clip videos - python train_mimic/scripts/benchmark.py ... --video --num_clips 10 --video_length 250 + --num_envs 32 """ from __future__ import annotations import argparse -import json +import copy import os +import sys from pathlib import Path +from typing import Sequence -import h5py import numpy as np -from tensordict import TensorDictBase +from tensordict import TensorDict + +REPO_ROOT = Path(__file__).resolve().parents[2] +if str(REPO_ROOT) not in sys.path: + sys.path.insert(0, str(REPO_ROOT)) from train_mimic.app import ( DEFAULT_TASK, @@ -40,207 +38,272 @@ validate_checkpoint_path, validate_motion_file, ) -from train_mimic.data.dataset_lib import find_precomputed_motion_shards -from teleopit.debug.rollout_trace import RolloutTraceWriter - - -def _render_frame(unwrapped: object, split: bool = False, _cmd: object = None) -> np.ndarray: - """Render a frame using the environment's offline renderer (ghost included). - - Args: - unwrapped: The unwrapped ManagerBasedRlEnv. - split: If True, render split-screen with camera lookat on ref (left) - and robot (right). Same scene, different camera targets. - _cmd: MotionCommand term (required when split=True). - """ - if not split: - frame = unwrapped.render() - if frame is None: - raise RuntimeError("render() returned None; ensure render_mode='rgb_array'") - return frame - - import mujoco - - renderer = unwrapped._offline_renderer - cam = renderer._cam - env_idx = max(0, min(int(renderer._cfg.env_idx), int(unwrapped.sim.data.nworld) - 1)) - - # Full scene update (robot + ghost via debug vis). - debug_callback = ( - unwrapped.update_visualizers if hasattr(unwrapped, "update_visualizers") else None +from train_mimic.benchmarking import ( + BenchmarkJob, + RolloutResult, + build_benchmark_plan, + compute_tracking_metrics, + load_clip_specs, + summarize_rollouts, + write_benchmark_outputs, +) + + +def parse_args(argv: Sequence[str] | None = None) -> argparse.Namespace: + parser = argparse.ArgumentParser( + description="OmniXtreme-style benchmark for the G1 tracking policy." ) - renderer.update(unwrapped.sim.data, debug_vis_callback=debug_callback) - - # Save original camera state. - orig_type = cam.type - orig_trackbodyid = cam.trackbodyid - orig_lookat = cam.lookat.copy() - - # --- Left: camera follows ref pose --- - ref_pos = _cmd.body_pos_w[env_idx, 0].cpu().numpy() - cam.type = mujoco.mjtCamera.mjCAMERA_FREE.value - cam.trackbodyid = -1 - cam.lookat[:] = [ref_pos[0], ref_pos[1], 0.8] - renderer._renderer.update_scene(renderer._data, camera=cam) - # Re-apply ghost geoms after update_scene reset. - if debug_callback is not None: - from mjlab.viewer.native.visualizer import MujocoNativeDebugVisualizer - vis = MujocoNativeDebugVisualizer( - renderer._renderer.scene, renderer._model, env_idx=renderer._cfg.env_idx - ) - debug_callback(vis) - frame_ref = renderer._renderer.render() - - # --- Right: camera follows robot --- - robot_pos = unwrapped.sim.data.qpos[env_idx, :3].cpu().numpy() - cam.lookat[:] = [robot_pos[0], robot_pos[1], 0.8] - renderer._renderer.update_scene(renderer._data, camera=cam) - if debug_callback is not None: - vis = MujocoNativeDebugVisualizer( - renderer._renderer.scene, renderer._model, env_idx=renderer._cfg.env_idx - ) - debug_callback(vis) - frame_robot = renderer._renderer.render() - - # Restore camera. - cam.type = orig_type - cam.trackbodyid = orig_trackbodyid - cam.lookat[:] = orig_lookat - - return np.concatenate([frame_ref, frame_robot], axis=1) - - -def _to_float(value: object, torch_module: object) -> float: - if isinstance(value, torch_module.Tensor): - if value.numel() == 0: - return 0.0 - return float(value.float().mean().item()) - if isinstance(value, (float, int)): - return float(value) - raise TypeError(f"Unsupported value type: {type(value)}") - - -def _stats(values: list[float]) -> dict[str, float]: - if not values: - return { - "mean": float("nan"), - "std": float("nan"), - "p50": float("nan"), - "p95": float("nan"), - "min": float("nan"), - "max": float("nan"), - } - arr = np.asarray(values, dtype=np.float64) - return { - "mean": float(arr.mean()), - "std": float(arr.std()), - "p50": float(np.percentile(arr, 50)), - "p95": float(np.percentile(arr, 95)), - "min": float(arr.min()), - "max": float(arr.max()), - } - - -def parse_args() -> argparse.Namespace: - parser = argparse.ArgumentParser(description="Benchmark G1 tracking policy.") parser.add_argument("--checkpoint", type=str, required=True) - parser.add_argument("--motion_file", type=str, required=True, help="Path to precomputed training dataset root containing Teleopit shard_*.h5 files") - parser.add_argument("--num_envs", type=int, default=1) - parser.add_argument("--num_eval_steps", type=int, default=2000, - help="Number of rollout steps for evaluation (default: 2000)") - parser.add_argument("--warmup_steps", type=int, default=100, - help="Warmup steps ignored from metric aggregation (default: 100)") + parser.add_argument( + "--motion_file", + type=str, + required=True, + help="Precomputed training dataset root or shard produced by precompute_dataset.py", + ) + parser.add_argument("--num_envs", type=int, default=32) parser.add_argument("--seed", type=int, default=42) - parser.add_argument("--video", action="store_true", - help="Record benchmark video(s)") - parser.add_argument("--num_clips", type=int, default=1, - help="Number of separate video clips to render (default: 1)") - parser.add_argument("--video_length", type=int, default=None, - help="Steps per video clip (default: longest clip in motion file)") - parser.add_argument("--video_folder", type=str, default=None, - help="Output folder for benchmark video(s)") - parser.add_argument("--split", action="store_true", - help="Render split-screen video with two camera angles") + parser.add_argument("--clip_seconds", type=float, default=10.0) + parser.add_argument("--output_dir", type=str, default="benchmark_results") parser.add_argument("--device", type=str, default=None) - parser.add_argument("--task", type=str, default=DEFAULT_TASK, - help="Task id to benchmark (default: %(default)s)") parser.add_argument( - "--debug_trace", + "--task", type=str, - default=None, - help="Optional .npz path to dump per-step benchmark trace for comparison", + default=DEFAULT_TASK, + help="Task id to benchmark (default: %(default)s)", + ) + return parser.parse_args(argv) + + +def _chunked(seq: Sequence[BenchmarkJob], size: int) -> list[Sequence[BenchmarkJob]]: + return [seq[i : i + size] for i in range(0, len(seq), size)] + + +def _obs_tensordict(obs_dict: object, num_envs: int) -> TensorDict: + return TensorDict(obs_dict, batch_size=[num_envs]) + + +def _configure_benchmark_env_cfg( + base_cfg: object, + *, + motion_file: str, + clip_seconds: float, +) -> object: + cfg = copy.deepcopy(base_cfg) + cfg.commands["motion"].motion_file = motion_file + cfg.commands["motion"].sampling_mode = "start" + cfg.commands["motion"].resample_on_clip_end = False + cfg.commands["motion"].pose_range = {} + cfg.commands["motion"].velocity_range = {} + cfg.commands["motion"].joint_position_range = (0.0, 0.0) + cfg.events = {} + cfg.episode_length_s = clip_seconds + cfg.auto_reset = False + return cfg + + +def _reset_to_jobs( + env: object, + jobs: Sequence[BenchmarkJob], + torch_module: object, +) -> TensorDict: + obs_dict, _extras = env.reset() + cmd = env.command_manager.get_term("motion") + env_ids = torch_module.arange(len(jobs), dtype=torch_module.long, device=env.device) + motion_ids = torch_module.tensor( + [job.clip_id for job in jobs], + dtype=torch_module.long, + device=env.device, + ) + motion_times = torch_module.tensor( + [job.start_time_s for job in jobs], + dtype=torch_module.float32, + device=env.device, + ) + cmd.reset_to_motion(env_ids, motion_ids, motion_times) + env.observation_manager.reset(env_ids) + env.scene.write_data_to_sim() + env.sim.forward() + env.command_manager.compute(dt=0.0) + env.sim.sense() + obs_dict = env.observation_manager.compute(update_history=True) + env.obs_buf = obs_dict + return _obs_tensordict(obs_dict, len(jobs)) + + +def _aligned_keybody_positions(cmd: object) -> tuple[np.ndarray, np.ndarray]: + from mjlab.utils.lab_api.math import quat_apply, quat_inv + + ref_anchor_pos = cmd.anchor_pos_w[:, None, :] + robot_anchor_pos = cmd.robot_anchor_pos_w[:, None, :] + num_bodies = cmd.body_pos_w.shape[1] + + ref_anchor_inv = quat_inv(cmd.anchor_quat_w)[:, None, :].expand(-1, num_bodies, -1) + robot_anchor_inv = quat_inv(cmd.robot_anchor_quat_w)[:, None, :].expand( + -1, num_bodies, -1 + ) + ref_aligned = quat_apply(ref_anchor_inv, cmd.body_pos_w - ref_anchor_pos) + robot_aligned = quat_apply(robot_anchor_inv, cmd.robot_body_pos_w - robot_anchor_pos) + return ( + ref_aligned.detach().cpu().numpy().astype(np.float32, copy=False), + robot_aligned.detach().cpu().numpy().astype(np.float32, copy=False), ) - return parser.parse_args() - - -def _load_motion_dir_video_metadata(motion_dir: str) -> tuple[float, int]: - clip_fps: float | None = None - max_clip_frames = 0 - for shard_path in find_precomputed_motion_shards(motion_dir): - with h5py.File(shard_path, "r") as h5: - fps_arr = np.asarray(h5["clip_fps"], dtype=np.float32) - if fps_arr.size == 0: - continue - cur_fps = float(fps_arr[0]) - if np.any(fps_arr != cur_fps): - raise ValueError(f"inconsistent fps within HDF5 shard: {shard_path}") - if clip_fps is None: - clip_fps = cur_fps - elif clip_fps != cur_fps: - raise ValueError( - f"inconsistent fps across shards: {shard_path} has {cur_fps}, expected {clip_fps}" - ) - max_clip_frames = max(max_clip_frames, int(np.asarray(h5["clip_lengths"]).max())) - if clip_fps is None: - raise ValueError(f"failed reading HDF5 shard metadata from {motion_dir}") - return clip_fps, max_clip_frames -def _tensor_to_numpy(value: object, torch_module: object) -> np.ndarray: - if isinstance(value, torch_module.Tensor): - return value.detach().cpu().numpy() - return np.asarray(value) +def _failure_reason(env: object, env_index: int) -> str: + manager = env.termination_manager + for term_name in manager.active_terms: + term_cfg = manager.get_term_cfg(term_name) + if term_cfg.time_out: + continue + if bool(manager.get_term(term_name)[env_index].item()): + return term_name + for term_name in manager.active_terms: + term_cfg = manager.get_term_cfg(term_name) + if term_cfg.time_out and bool(manager.get_term(term_name)[env_index].item()): + return term_name + return "done" + + +def _run_batch( + *, + batch_index: int, + jobs: Sequence[BenchmarkJob], + base_env_cfg: object, + agent_cfg: object, + runner_cls: object, + fallback_runner_cls: object, + checkpoint: str, + log_dir: str, + device: str, + torch_module: object, + ManagerBasedRlEnv: object, + RslRlVecEnvWrapper: object, + clip_seconds: float, + control_steps: int, + seed: int, +) -> list[RolloutResult]: + env_cfg = _configure_benchmark_env_cfg( + base_env_cfg, + motion_file=base_env_cfg.commands["motion"].motion_file, + clip_seconds=clip_seconds, + ) + env_cfg.seed = seed + batch_index + env_cfg.scene.num_envs = len(jobs) + env = ManagerBasedRlEnv(cfg=env_cfg, device=device, render_mode=None) + wrapped_env = RslRlVecEnvWrapper(env, clip_actions=agent_cfg.clip_actions) + agent_dict = build_runner_cfg_dict(agent_cfg, force_tensorboard=True) + RunnerCls = runner_cls or fallback_runner_cls + runner = RunnerCls(wrapped_env, agent_dict, log_dir=log_dir, device=device) + runner.load(checkpoint, map_location=device) + policy = runner.get_inference_policy(device=device) -def _first_env_numpy(value: object, torch_module: object) -> np.ndarray: - array = _tensor_to_numpy(value, torch_module) - if array.ndim == 0: - return array.reshape(1) - return array[0].copy() + aligned_ref_by_env: list[list[np.ndarray]] = [[] for _ in jobs] + aligned_robot_by_env: list[list[np.ndarray]] = [[] for _ in jobs] + active = np.ones(len(jobs), dtype=bool) + finished: dict[int, tuple[bool, int, int | None, str | None]] = {} + try: + obs = _reset_to_jobs(env, jobs, torch_module) + cmd = env.command_manager.get_term("motion") + for step in range(control_steps): + ref_aligned, robot_aligned = _aligned_keybody_positions(cmd) + for env_index, is_active in enumerate(active): + if is_active: + aligned_ref_by_env[env_index].append(ref_aligned[env_index]) + aligned_robot_by_env[env_index].append(robot_aligned[env_index]) + + with torch_module.no_grad(): + actions = policy(obs) + if agent_cfg.clip_actions is not None: + actions = torch_module.clamp( + actions, + -agent_cfg.clip_actions, + agent_cfg.clip_actions, + ) + + obs_dict, _rewards, terminated, truncated, _extras = env.step(actions) + obs = _obs_tensordict(obs_dict, len(jobs)) + + done = (terminated | truncated).detach().cpu().numpy().astype(bool) + terminated_np = terminated.detach().cpu().numpy().astype(bool) + truncated_np = truncated.detach().cpu().numpy().astype(bool) + done_envs = [int(env_index) for env_index, is_done in enumerate(done) if is_done] + for env_index, is_done in enumerate(done): + if not active[env_index] or not is_done: + continue + reached_horizon = step + 1 >= control_steps + success = bool( + truncated_np[env_index] + and reached_horizon + and not terminated_np[env_index] + ) + failure_step = None if success else step + 1 + failure_reason = None if success else _failure_reason(env, env_index) + finished[env_index] = (success, step + 1, failure_step, failure_reason) + active[env_index] = False + + if step + 1 < control_steps and done_envs: + env_ids = torch_module.tensor( + done_envs, + dtype=torch_module.long, + device=env.device, + ) + obs_dict, _extras = env.reset(env_ids=env_ids) + obs = _obs_tensordict(obs_dict, len(jobs)) -def _extract_obs_for_trace(obs: object, torch_module: object) -> tuple[np.ndarray, np.ndarray | None]: - if isinstance(obs, TensorDictBase): - actor = _first_env_numpy(obs["actor"], torch_module).astype(np.float32, copy=False) - actor_history = None - if "actor_history" in obs.keys(): - actor_history = _first_env_numpy(obs["actor_history"], torch_module).astype( - np.float32, copy=False + if not active.any(): + break + finally: + env.close() + + results: list[RolloutResult] = [] + for env_index, job in enumerate(jobs): + success, steps, failure_step, failure_reason = finished.get( + env_index, + (True, control_steps, None, None), + ) + metrics = compute_tracking_metrics( + np.stack(aligned_ref_by_env[env_index], axis=0), + np.stack(aligned_robot_by_env[env_index], axis=0), + ) + if not success: + metrics = { + "mpjpe_mm": float("nan"), + "delta_vel_mm_per_frame": float("nan"), + "delta_acc_mm_per_frame2": float("nan"), + } + results.append( + RolloutResult( + job_id=job.job_id, + clip_id=job.clip_id, + rollout_index=job.rollout_index, + success=success, + steps=steps, + failure_step=failure_step, + failure_reason=failure_reason, + mpjpe_mm=metrics["mpjpe_mm"], + delta_vel_mm_per_frame=metrics["delta_vel_mm_per_frame"], + delta_acc_mm_per_frame2=metrics["delta_acc_mm_per_frame2"], ) - return actor, actor_history - raise TypeError(f"Unsupported observation container for debug trace: {type(obs)}") + ) + return results -def main() -> int: - args = parse_args() +def main(argv: Sequence[str] | None = None) -> int: + args = parse_args(argv) - if args.warmup_steps < 0: - raise ValueError("--warmup_steps must be >= 0") - if not args.video and args.num_eval_steps <= args.warmup_steps: - raise ValueError("--num_eval_steps must be greater than --warmup_steps") - if args.video and args.num_envs != 1: - raise ValueError("--video currently requires --num_envs 1") - if args.debug_trace is not None and args.num_envs != 1: - raise ValueError("--debug_trace currently requires --num_envs 1") - validate_motion_file(args.motion_file) + if args.num_envs <= 0: + raise ValueError("--num_envs must be > 0") + if args.clip_seconds <= 0.0: + raise ValueError("--clip_seconds must be > 0") - # Set render backend before importing modules that may initialize MuJoCo/GL. - if args.video and "MUJOCO_GL" not in os.environ: - os.environ["MUJOCO_GL"] = "egl" - print("[INFO] --video enabled, MUJOCO_GL not set. Defaulting to MUJOCO_GL=egl.") - if args.video and "PYOPENGL_PLATFORM" not in os.environ: - os.environ["PYOPENGL_PLATFORM"] = "egl" - print("[INFO] --video enabled, PYOPENGL_PLATFORM not set. Defaulting to PYOPENGL_PLATFORM=egl.") + validate_motion_file(args.motion_file) + try: + validate_checkpoint_path(args.checkpoint) + except FileNotFoundError as exc: + print(f"Error: {exc}") + return 1 ( torch, @@ -252,328 +315,98 @@ def main() -> int: _load_runner_cls, configure_torch_backends, ) = import_training_stack() - - try: - validate_checkpoint_path(args.checkpoint) - except FileNotFoundError as exc: - print(f"Error: {exc}") - return 1 - configure_torch_backends() - # Load configs (play=True disables corruption, push_robot, etc.) - task_name, env_cfg, agent_cfg, runner_cls = load_task_components( + task_name, base_env_cfg, agent_cfg, runner_cls = load_task_components( args.task, play=True, load_env_cfg=_load_env_cfg, load_rl_cfg=_load_rl_cfg, load_runner_cls=_load_runner_cls, ) - - # Configure for benchmark - env_cfg.seed = args.seed - env_cfg.scene.num_envs = args.num_envs - env_cfg.commands["motion"].motion_file = args.motion_file - env_cfg.commands["motion"].pose_range = {} - env_cfg.commands["motion"].velocity_range = {} - - # Use uniform sampling so each reset picks a different motion segment. - if args.video and args.num_clips > 1: - env_cfg.commands["motion"].sampling_mode = "uniform" - - step_dt = float(env_cfg.decimation) * float(env_cfg.sim.mujoco.timestep) - required_episode_s = args.num_eval_steps * step_dt + 1.0 - if float(env_cfg.episode_length_s) < required_episode_s: - env_cfg.episode_length_s = required_episode_s - if args.video: - env_cfg.terminations.pop("time_out", None) - env_cfg.terminations.pop("anchor_pos", None) - env_cfg.terminations.pop("anchor_ori", None) - env_cfg.terminations.pop("ee_body_pos", None) - env_cfg.terminations.pop("body_z_tracking_failure", None) - env_cfg.terminations.pop("gravity_tracking_failure", None) + base_env_cfg.commands["motion"].motion_file = args.motion_file + benchmark_env_cfg = _configure_benchmark_env_cfg( + base_env_cfg, + motion_file=args.motion_file, + clip_seconds=args.clip_seconds, + ) + step_dt = float(benchmark_env_cfg.decimation) * float( + benchmark_env_cfg.sim.mujoco.timestep + ) + clips = load_clip_specs( + args.motion_file, + window_steps=benchmark_env_cfg.commands["motion"].window_steps, + ) + plan = build_benchmark_plan( + clips, + clip_seconds=args.clip_seconds, + step_dt=step_dt, + ) device = resolve_device(args.device, torch) - - # Create env - render_mode = "rgb_array" if args.video else None - try: - env = ManagerBasedRlEnv(cfg=env_cfg, device=device, render_mode=render_mode) - except Exception as exc: - if args.video: - raise RuntimeError( - "Video renderer initialization failed. " - "Try setting MUJOCO_GL=egl (or osmesa) and make sure the corresponding " - "OpenGL backend libraries are available on this machine." - ) from exc - else: - raise - - env = RslRlVecEnvWrapper(env, clip_actions=agent_cfg.clip_actions) - - # Auto-resolve video_length from motion file if not specified. - if args.video and args.video_length is None: - clip_fps, max_clip_frames = _load_motion_dir_video_metadata(args.motion_file) - step_dt = env.unwrapped.step_dt - args.video_length = int(max_clip_frames / clip_fps / step_dt) - print(f"[INFO] Auto video_length={args.video_length} steps " - f"({max_clip_frames} frames / {clip_fps} fps / {step_dt} step_dt = " - f"{args.video_length * step_dt:.1f}s)") - elif args.video_length is None: - args.video_length = 600 - - # Auto-adjust num_eval_steps to cover all video clips. - if args.video: - min_steps = args.num_clips * args.video_length + args.warmup_steps - if args.num_eval_steps < min_steps: - print(f"[INFO] Increasing --num_eval_steps from {args.num_eval_steps} to {min_steps} " - f"to cover {args.num_clips} clips x {args.video_length} steps + {args.warmup_steps} warmup.") - args.num_eval_steps = min_steps - + batches = _chunked(plan.jobs, args.num_envs) log_dir = os.path.dirname(args.checkpoint) - agent_dict = build_runner_cfg_dict(agent_cfg, force_tensorboard=True) - RunnerCls = runner_cls or MjlabOnPolicyRunner - runner = RunnerCls(env, agent_dict, log_dir=log_dir, device=device) - runner.load(args.checkpoint, map_location=device) - policy = runner.get_inference_policy(device=device) + results: list[RolloutResult] = [] - # --- Video recording setup --- - video_writer = None - video_folder: Path | None = None - video_paths: list[Path] = [] - clip_step_counter = 0 - clip_idx = 0 - - if args.video: - import imageio.v2 as imageio - - video_folder = Path(args.video_folder or "benchmark_results/videos") - video_folder.mkdir(parents=True, exist_ok=True) - video_fps = max(1, int(round(1.0 / env.unwrapped.step_dt))) - - def _open_clip_writer(idx: int): - nonlocal video_writer, clip_step_counter - if args.num_clips == 1: - path = video_folder / "benchmark.mp4" - else: - path = video_folder / f"clip_{idx:03d}.mp4" - video_paths.append(path) - video_writer = imageio.get_writer(str(path), fps=video_fps, quality=8) - clip_step_counter = 0 - print(f"[INFO] Recording clip {idx + 1}/{args.num_clips}: {path}") - - _open_clip_writer(0) - - # --- Benchmark loop --- - obs = env.get_observations() - unwrapped = env.unwrapped - cmd = unwrapped.command_manager.get_term("motion") - metric_keys = sorted(cmd.metrics.keys()) - metric_series: dict[str, list[float]] = {k: [] for k in metric_keys} - reward_series: list[float] = [] - reset_log_series: dict[str, list[float]] = {} - trace_writer: RolloutTraceWriter | None = None - if args.debug_trace is not None: - trace_writer = RolloutTraceWriter( - args.debug_trace, - metadata={ - "source": "benchmark", - "task": args.task, - "checkpoint": args.checkpoint, - "motion_file": args.motion_file, - "num_envs": args.num_envs, - "step_dt": float(env.unwrapped.step_dt), - }, + print( + "Running OmniXtreme-style benchmark: " + f"{len(plan.eligible_clips)} clips, {len(plan.jobs)} rollouts, " + f"{plan.control_steps} steps/rollout, batch size {args.num_envs}." + ) + if plan.skipped_short_clips: + print( + f"Skipping {len(plan.skipped_short_clips)} clips shorter than " + f"{args.clip_seconds:.2f}s." ) - done_events = 0 - timeout_events = 0 - ep_len_buf = torch.zeros(args.num_envs, dtype=torch.long, device=device) - completed_episode_lengths: list[int] = [] - - try: - for step in range(args.num_eval_steps): - actor_obs, actor_history = _extract_obs_for_trace(obs, torch) - with torch.no_grad(): - actions = policy(obs) - obs, rewards, dones, extras = env.step(actions) - ep_len_buf += 1 - - # Record video frame. - if video_writer is not None and clip_idx < args.num_clips: - frame = _render_frame(env.unwrapped, split=args.split, _cmd=cmd) - video_writer.append_data(frame) - clip_step_counter += 1 - - # Close current clip and open next one. - if clip_step_counter >= args.video_length: - video_writer.close() - video_writer = None - clip_idx += 1 - if clip_idx < args.num_clips: - # Reset env to sample a new motion segment. - obs, _ = env.reset() - ep_len_buf[:] = 0 - _open_clip_writer(clip_idx) - - done_mask = dones > 0 - num_done = int(done_mask.sum().item()) - if num_done > 0: - done_events += num_done - completed_episode_lengths.extend(ep_len_buf[done_mask].detach().cpu().tolist()) - ep_len_buf[done_mask] = 0 - extras_log = extras.get("log", {}) if isinstance(extras, dict) else {} - if isinstance(extras_log, dict): - for key, value in extras_log.items(): - if key.startswith(("Episode_Reward/", "Episode_Termination/", "Metrics/motion/")): - reset_log_series.setdefault(key, []).append(_to_float(value, torch)) - - if step < args.warmup_steps: - continue - - reward_series.append(_to_float(rewards, torch)) - for key in metric_keys: - metric_series[key].append(_to_float(cmd.metrics[key], torch)) - - if isinstance(extras, dict) and "time_outs" in extras and isinstance(extras["time_outs"], torch.Tensor): - timeout_events += int(extras["time_outs"].sum().item()) - - if trace_writer is not None: - trace_writer.add_step( - step=np.int64(step), - policy_time=np.float64(step * env.unwrapped.step_dt), - obs=actor_obs, - obs_history=actor_history, - action=_first_env_numpy(actions, torch).astype(np.float32, copy=False), - reward=np.asarray(_to_float(rewards, torch), dtype=np.float32), - motion_joint_pos=_first_env_numpy(cmd.joint_pos, torch).astype(np.float32, copy=False), - motion_joint_vel=_first_env_numpy(cmd.joint_vel, torch).astype(np.float32, copy=False), - motion_anchor_pos_w=_first_env_numpy(cmd.anchor_pos_w, torch).astype(np.float32, copy=False), - motion_anchor_quat_w=_first_env_numpy(cmd.anchor_quat_w, torch).astype(np.float32, copy=False), - motion_anchor_lin_vel_w=_first_env_numpy(cmd.anchor_lin_vel_w, torch).astype(np.float32, copy=False), - motion_anchor_ang_vel_w=_first_env_numpy(cmd.anchor_ang_vel_w, torch).astype(np.float32, copy=False), - robot_joint_pos=_first_env_numpy(cmd.robot_joint_pos, torch).astype(np.float32, copy=False), - robot_joint_vel=_first_env_numpy(cmd.robot_joint_vel, torch).astype(np.float32, copy=False), - robot_anchor_pos_w=_first_env_numpy(cmd.robot_anchor_pos_w, torch).astype(np.float32, copy=False), - robot_anchor_quat_w=_first_env_numpy(cmd.robot_anchor_quat_w, torch).astype(np.float32, copy=False), - done=np.asarray(bool(dones[0].item()), dtype=np.bool_), - ) - finally: - if video_writer is not None: - video_writer.close() - if trace_writer is not None: - trace_writer.save() - env.close() - - # --- Report --- - effective_steps = args.num_eval_steps - args.warmup_steps - if effective_steps <= 0: - raise RuntimeError("No effective evaluation steps. Increase --num_eval_steps or decrease --warmup_steps.") - - metric_stats = {key: _stats(vals) for key, vals in metric_series.items()} - reward_stats = _stats(reward_series) - reset_log_stats = {key: _stats(vals) for key, vals in reset_log_series.items()} - - anchor_pos = metric_stats.get("error_anchor_pos", {}).get("mean", float("nan")) - anchor_rot = metric_stats.get("error_anchor_rot", {}).get("mean", float("nan")) - body_pos = metric_stats.get("error_body_pos", {}).get("mean", float("nan")) - total = anchor_pos + anchor_rot + body_pos - - eval_transitions = args.num_eval_steps * args.num_envs - done_rate = done_events / max(eval_transitions, 1) - timeout_rate = timeout_events / max(eval_transitions, 1) - ep_len_stats = _stats([float(v) for v in completed_episode_lengths]) - - print(f"\nBenchmark Results ({effective_steps} effective steps, warmup {args.warmup_steps}):") - print(f" total_error(anchor_pos+anchor_rot+body_pos): {total:.4f}") - print(f" error_anchor_pos: {anchor_pos:.4f}") - print(f" error_anchor_rot: {anchor_rot:.4f}") - print(f" error_body_pos: {body_pos:.4f}") - print(f" mean_step_reward: {reward_stats['mean']:.4f}") - print(f" done_rate: {done_rate:.4f}") - print(f" timeout_rate: {timeout_rate:.4f}") - print(f" completed_episodes: {len(completed_episode_lengths)}") - print(f" mean_episode_length: {ep_len_stats['mean']:.2f}") - - print("\nMetric distributions (mean / p50 / p95):") - for key in ( - "error_anchor_pos", - "error_anchor_rot", - "error_anchor_lin_vel", - "error_anchor_ang_vel", - "error_body_pos", - "error_body_rot", - "error_body_lin_vel", - "error_body_ang_vel", - "error_joint_pos", - "error_joint_vel", - ): - if key not in metric_stats: - continue - stats = metric_stats[key] - print(f" {key}: {stats['mean']:.4f} / {stats['p50']:.4f} / {stats['p95']:.4f}") - - Path("benchmark_results").mkdir(exist_ok=True) - output_path = Path("benchmark_results") / f"{args.task}-{Path(args.checkpoint).stem}.txt" - json_path = Path("benchmark_results") / f"{args.task}-{Path(args.checkpoint).stem}.json" - - lines = [ - f"checkpoint: {args.checkpoint}", - f"motion_file: {args.motion_file}", - f"num_envs: {args.num_envs}", - f"num_eval_steps: {args.num_eval_steps}", - f"warmup_steps: {args.warmup_steps}", - f"effective_steps: {effective_steps}", - "", - f"total_error(anchor_pos+anchor_rot+body_pos): {total:.6f}", - f"error_anchor_pos_mean: {anchor_pos:.6f}", - f"error_anchor_rot_mean: {anchor_rot:.6f}", - f"error_body_pos_mean: {body_pos:.6f}", - f"mean_step_reward: {reward_stats['mean']:.6f}", - f"done_rate: {done_rate:.6f}", - f"timeout_rate: {timeout_rate:.6f}", - f"completed_episodes: {len(completed_episode_lengths)}", - f"mean_episode_length: {ep_len_stats['mean']:.6f}", - "", - "metric_stats(mean,std,p50,p95,min,max):", - ] - for key in sorted(metric_stats.keys()): - s = metric_stats[key] - lines.append( - f"{key}: {s['mean']:.6f}, {s['std']:.6f}, {s['p50']:.6f}, {s['p95']:.6f}, {s['min']:.6f}, {s['max']:.6f}" + for batch_index, batch_jobs in enumerate(batches): + print( + f"[{batch_index + 1}/{len(batches)}] " + f"rollouts {batch_jobs[0].job_id}..{batch_jobs[-1].job_id}" ) - if reset_log_stats: - lines.append("") - lines.append("reset_log_stats(mean,std,p50,p95,min,max):") - for key in sorted(reset_log_stats.keys()): - s = reset_log_stats[key] - lines.append( - f"{key}: {s['mean']:.6f}, {s['std']:.6f}, {s['p50']:.6f}, {s['p95']:.6f}, {s['min']:.6f}, {s['max']:.6f}" + results.extend( + _run_batch( + batch_index=batch_index, + jobs=batch_jobs, + base_env_cfg=benchmark_env_cfg, + agent_cfg=agent_cfg, + runner_cls=runner_cls, + fallback_runner_cls=MjlabOnPolicyRunner, + checkpoint=args.checkpoint, + log_dir=log_dir, + device=device, + torch_module=torch, + ManagerBasedRlEnv=ManagerBasedRlEnv, + RslRlVecEnvWrapper=RslRlVecEnvWrapper, + clip_seconds=args.clip_seconds, + control_steps=plan.control_steps, + seed=args.seed, ) - output_path.write_text("\n".join(lines) + "\n") - - report = { - "checkpoint": args.checkpoint, - "motion_file": args.motion_file, - "num_envs": args.num_envs, - "num_eval_steps": args.num_eval_steps, - "warmup_steps": args.warmup_steps, - "effective_steps": effective_steps, - "total_error": total, - "mean_step_reward": reward_stats["mean"], - "done_rate": done_rate, - "timeout_rate": timeout_rate, - "completed_episodes": len(completed_episode_lengths), - "mean_episode_length": ep_len_stats["mean"], - "metric_stats": metric_stats, - "reset_log_stats": reset_log_stats, - } - json_path.write_text(json.dumps(report, indent=2)) - - print(f"\nSaved summary to: {output_path}") - print(f"Saved detailed json to: {json_path}") - for vp in video_paths: - print(f"Saved video: {vp}") + ) + + output_stem = f"{task_name}-{Path(args.checkpoint).stem}-omnixtreme" + paths = write_benchmark_outputs( + args.output_dir, + text_stem=output_stem, + metadata={ + "task": task_name, + "checkpoint": args.checkpoint, + "motion_file": args.motion_file, + "seed": args.seed, + "num_envs": args.num_envs, + }, + plan=plan, + results=results, + ) + + summary = summarize_rollouts(results)["global"] + print("\nBenchmark Results:") + print(f" MPJPE(mm): {summary['mpjpe_mm']:.4f}") + print(f" delta_vel(mm/frame): {summary['delta_vel_mm_per_frame']:.4f}") + print(f" delta_acc(mm/frame^2): {summary['delta_acc_mm_per_frame2']:.4f}") + print(f" success_rate(%): {summary['success_rate']:.2f}") + for label, path in paths.items(): + print(f"Saved {label}: {path}") return 0 diff --git a/train_mimic/tasks/tracking/mdp/commands.py b/train_mimic/tasks/tracking/mdp/commands.py index add39de0..9648bf6e 100644 --- a/train_mimic/tasks/tracking/mdp/commands.py +++ b/train_mimic/tasks/tracking/mdp/commands.py @@ -826,6 +826,60 @@ def _resample_command(self, env_ids: torch.Tensor): self._reset_envs_to_current_reference(env_ids) + def reset_to_motion( + self, + env_ids: torch.Tensor, + motion_ids: torch.Tensor, + motion_times: torch.Tensor, + ) -> None: + """Reset selected environments to exact motion clips/times. + + This is intended for deterministic benchmark rollouts. Normal training + and playback should continue using the configured sampling mode. + """ + if env_ids.ndim != 1: + raise ValueError(f"env_ids must be 1-D, got {tuple(env_ids.shape)}") + if motion_ids.ndim != 1 or motion_times.ndim != 1: + raise ValueError( + "motion_ids and motion_times must be 1-D, got " + f"{tuple(motion_ids.shape)} and {tuple(motion_times.shape)}" + ) + if not (len(env_ids) == len(motion_ids) == len(motion_times)): + raise ValueError( + "env_ids, motion_ids, and motion_times must have matching lengths, got " + f"{len(env_ids)}, {len(motion_ids)}, {len(motion_times)}" + ) + if len(env_ids) == 0: + return + + env_ids = env_ids.to(device=self.device, dtype=torch.long) + motion_ids = motion_ids.to(device=self.device, dtype=torch.long) + motion_times = motion_times.to(device=self.device, dtype=self.motion_times.dtype) + if torch.any(motion_ids < 0) or torch.any(motion_ids >= self.motion.num_clips): + raise ValueError( + f"motion_ids out of range [0, {self.motion.num_clips}): " + f"{motion_ids.detach().cpu().tolist()}" + ) + + sample_starts = self.motion.clip_sample_start_s[motion_ids] + sample_ends = self.motion.clip_sample_end_s[motion_ids] + invalid_times = (motion_times < sample_starts) | (motion_times >= sample_ends) + if torch.any(invalid_times): + bad = torch.where(invalid_times)[0] + first = int(bad[0].item()) + raise ValueError( + "motion_times must be inside each clip's valid sample range; " + f"motion_id={int(motion_ids[first].item())}, " + f"time={float(motion_times[first].item()):.6f}, " + f"range=[{float(sample_starts[first].item()):.6f}, " + f"{float(sample_ends[first].item()):.6f})" + ) + + self.motion_ids[env_ids] = motion_ids + self.motion_times[env_ids] = motion_times + self.time_left[env_ids] = float(self.cfg.resampling_time_range[1]) + self._reset_envs_to_current_reference(env_ids) + def _reset_envs_to_current_reference(self, env_ids: torch.Tensor) -> None: if env_ids.numel() == 0: return @@ -920,8 +974,13 @@ def _update_command(self): exceeded = self.motion_times >= end_times env_ids = torch.where(exceeded)[0] - if env_ids.numel() > 0: + if env_ids.numel() > 0 and self.cfg.resample_on_clip_end: self._resample_command(env_ids) + elif env_ids.numel() > 0: + self.motion_times[env_ids] = torch.nextafter( + end_times[env_ids], + self.motion.clip_sample_start_s[self.motion_ids[env_ids]], + ) self._refresh_frame_cache() @@ -1037,6 +1096,7 @@ class MotionCommandCfg(CommandTermCfg): velocity_range: dict[str, tuple[float, float]] = field(default_factory=dict) joint_position_range: tuple[float, float] = (-0.52, 0.52) sampling_mode: Literal["uniform", "start", "rewind"] = "rewind" + resample_on_clip_end: bool = True window_steps: tuple[int, ...] = (0,) rewind_prob: float = 0.8 rewind_min_steps: int = 25 From 9c8e65a88bfb108352d05f308c80fd807c621abd Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Thu, 2 Jul 2026 17:24:55 +0800 Subject: [PATCH 02/59] Update benchmark tracking metrics --- AGENTS.md | 2 +- docs/docs/tutorials/training.md | 2 +- .../current/tutorials/training.md | 2 +- tests/test_benchmark_omnixtreme.py | 49 ++++++--- train_mimic/benchmarking.py | 104 ++++++++++-------- train_mimic/scripts/benchmark.py | 56 ++++++++-- 6 files changed, 145 insertions(+), 70 deletions(-) diff --git a/AGENTS.md b/AGENTS.md index b28a24ef..2ce48804 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -203,7 +203,7 @@ The single supported training task is `General-Tracking-G1` (experiment name: `g - Training env uses `sampling_mode="rewind"` - Tracking rewards include root position/orientation/linear velocity/angular velocity, body pose/velocity, joint position/velocity, survival, action-rate, joint-limit, self-collision, and ankle acceleration terms - Supported motion sampling modes are `uniform`, `start`, and `rewind`; `rewind` restarts failed environments from the same clip after stepping back `rewind_min_steps..rewind_max_steps` with probability `rewind_prob`, otherwise it falls back to uniform sampling -- Playback and benchmark use `play=True`, which switches motion sampling to `start`; benchmark pins exact clip ids/start times, disables clip-end resampling, and reports `MPJPE(mm)`, `delta_vel(mm/frame)`, `delta_acc(mm/frame^2)`, and `success_rate(%)` +- Playback and benchmark use `play=True`, which switches motion sampling to `start`; benchmark pins exact clip ids/start times, disables clip-end resampling, and reports `MPJPE(m)`, `root_pos_error(m)`, `root_rot_error(rad)`, `root_vel_error(m/s)`, and `success_rate(%)` - `window_steps=[0]` - `save_onnx.py` exports dual-input TemporalCNN ONNX diff --git a/docs/docs/tutorials/training.md b/docs/docs/tutorials/training.md index e26ed9fa..b59d1e8d 100644 --- a/docs/docs/tutorials/training.md +++ b/docs/docs/tutorials/training.md @@ -118,7 +118,7 @@ python train_mimic/scripts/benchmark.py \ --num_envs 32 ``` -The benchmark uses an OmniXtreme-style protocol: 10-second clips, one deterministic rollout per eligible clip, and `MPJPE(mm)`, `delta_vel(mm/frame)`, `delta_acc(mm/frame^2)`, and `success_rate(%)` outputs. It uses play-mode observations without training noise and pins exact clip ids/start times without clip-end resampling. `--motion_file` must point to a precomputed training dataset; all clips long enough for the configured clip length are evaluated. +The benchmark uses an OmniXtreme-style protocol: 10-second clips, one deterministic rollout per eligible clip, and `MPJPE(m)`, `root_pos_error(m)`, `root_rot_error(rad)`, `root_vel_error(m/s)`, and `success_rate(%)` outputs. Root errors use the same anchor position, rotation, and linear velocity definitions as the tracking command metrics. It uses play-mode observations without training noise and pins exact clip ids/start times without clip-end resampling. `--motion_file` must point to a precomputed training dataset; all clips long enough for the configured clip length are evaluated. ## Training Architecture diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/training.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/training.md index 09610633..0b2ee359 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/training.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/training.md @@ -117,7 +117,7 @@ python train_mimic/scripts/benchmark.py \ --num_envs 32 ``` -benchmark 使用 OmniXtreme 风格协议：10 秒 clip、每个合格 clip 进行一次确定性 rollout，并输出 `MPJPE(mm)`、`delta_vel(mm/frame)`、`delta_acc(mm/frame^2)` 和 `success_rate(%)`。它使用无训练噪声的 play-mode 观测，并固定精确 clip id/起始时间且禁用 clip 末尾重采样。`--motion_file` 必须指向预计算训练数据集；所有长度足够满足配置 clip 时长的 clip 都会参与评测。 +benchmark 使用 OmniXtreme 风格协议：10 秒 clip、每个合格 clip 进行一次确定性 rollout，并输出 `MPJPE(m)`、`root_pos_error(m)`、`root_rot_error(rad)`、`root_vel_error(m/s)` 和 `success_rate(%)`。root error 使用与 tracking command metrics 相同的 anchor 位置、旋转和线速度定义。它使用无训练噪声的 play-mode 观测，并固定精确 clip id/起始时间且禁用 clip 末尾重采样。`--motion_file` 必须指向预计算训练数据集；所有长度足够满足配置 clip 时长的 clip 都会参与评测。 ## 训练架构 diff --git a/tests/test_benchmark_omnixtreme.py b/tests/test_benchmark_omnixtreme.py index 9ad6e5d3..dffdf770 100644 --- a/tests/test_benchmark_omnixtreme.py +++ b/tests/test_benchmark_omnixtreme.py @@ -81,16 +81,23 @@ def test_compute_tracking_metrics() -> None: dtype=np.float32, ) - metrics = compute_tracking_metrics(ref, robot) + metrics = compute_tracking_metrics( + ref, + robot, + root_pos_error_m=np.array([0.1, 0.2, 0.3], dtype=np.float32), + root_rot_error_rad=np.array([0.01, 0.02, 0.03], dtype=np.float32), + root_vel_error_m_s=np.array([1.0, 2.0, 3.0], dtype=np.float32), + ) - assert metrics["mpjpe_mm"] == pytest.approx((0.0 + 0.1 + 0.4) / 3.0 * 1000.0) - assert metrics["delta_vel_mm_per_frame"] == pytest.approx((0.1 + 0.3) / 2.0 * 1000.0) - assert metrics["delta_acc_mm_per_frame2"] == pytest.approx(0.2 * 1000.0) + assert metrics["mpjpe_m"] == pytest.approx((0.0 + 0.1 + 0.4) / 3.0) + assert metrics["root_pos_error_m"] == pytest.approx(0.2) + assert metrics["root_rot_error_rad"] == pytest.approx(0.02) + assert metrics["root_vel_error_m_s"] == pytest.approx(2.0) def test_summarize_rollouts_aggregates_success_and_metrics() -> None: results = [ - RolloutResult(0, 0, 0, True, 500, None, None, 10.0, 2.0, 1.0), + RolloutResult(0, 0, 0, True, 500, None, None, 0.01, 0.1, 0.01, 1.0), RolloutResult( 1, 0, @@ -102,16 +109,20 @@ def test_summarize_rollouts_aggregates_success_and_metrics() -> None: float("nan"), float("nan"), float("nan"), + float("nan"), ), - RolloutResult(2, 1, 0, True, 500, None, None, 30.0, 6.0, 5.0), + RolloutResult(2, 1, 0, True, 500, None, None, 0.03, 0.3, 0.03, 3.0), ] summary = summarize_rollouts(results) assert summary["global"]["success_rate"] == pytest.approx(200.0 / 3.0) - assert summary["global"]["mpjpe_mm"] == pytest.approx(20.0) + assert summary["global"]["mpjpe_m"] == pytest.approx(0.02) + assert summary["global"]["root_pos_error_m"] == pytest.approx(0.2) + assert summary["global"]["root_rot_error_rad"] == pytest.approx(0.02) + assert summary["global"]["root_vel_error_m_s"] == pytest.approx(2.0) assert summary["per_clip"][0]["success_rate"] == pytest.approx(50.0) - assert summary["per_clip"][0]["mpjpe_mm"] == pytest.approx(10.0) + assert summary["per_clip"][0]["mpjpe_m"] == pytest.approx(0.01) assert summary["per_clip"][1]["success_rate"] == pytest.approx(100.0) @@ -211,9 +222,10 @@ def test_write_benchmark_outputs_serializes_failed_metrics_as_null(tmp_path) -> steps=120, failure_step=120, failure_reason="anchor_pos", - mpjpe_mm=float("nan"), - delta_vel_mm_per_frame=float("nan"), - delta_acc_mm_per_frame2=float("nan"), + mpjpe_m=float("nan"), + root_pos_error_m=float("nan"), + root_rot_error_rad=float("nan"), + root_vel_error_m_s=float("nan"), ) paths = write_benchmark_outputs( @@ -231,8 +243,9 @@ def test_write_benchmark_outputs_serializes_failed_metrics_as_null(tmp_path) -> data = paths["summary_json"].read_text() assert "NaN" not in data report = __import__("json").loads(data) - assert report["global"]["mpjpe_mm"] is None - assert report["per_rollout"][0]["mpjpe_mm"] is None + assert report["global"]["mpjpe_m"] is None + assert report["global"]["root_pos_error_m"] is None + assert report["per_rollout"][0]["mpjpe_m"] is None def test_run_batch_resets_inactive_done_envs_and_excludes_failed_metrics(monkeypatch) -> None: @@ -381,7 +394,15 @@ def fake_aligned(_cmd): robot = np.zeros((2, 1, 3), dtype=np.float32) return ref, robot + def fake_root_errors(_cmd): + return ( + np.zeros(2, dtype=np.float32), + np.zeros(2, dtype=np.float32), + np.zeros(2, dtype=np.float32), + ) + monkeypatch.setattr(benchmark_script, "_aligned_keybody_positions", fake_aligned) + monkeypatch.setattr(benchmark_script, "_root_tracking_errors", fake_root_errors) motion = SimpleNamespace(motion_file="dataset") base_env_cfg = SimpleNamespace( @@ -420,4 +441,4 @@ def fake_aligned(_cmd): assert env.observation_manager.reset_calls == [[0, 1]] assert results[0].success is True assert results[1].success is False - assert np.isnan(results[1].mpjpe_mm) + assert np.isnan(results[1].mpjpe_m) diff --git a/train_mimic/benchmarking.py b/train_mimic/benchmarking.py index 6e4477f5..cfb854ce 100644 --- a/train_mimic/benchmarking.py +++ b/train_mimic/benchmarking.py @@ -57,9 +57,10 @@ class RolloutResult: steps: int failure_step: int | None failure_reason: str | None - mpjpe_mm: float - delta_vel_mm_per_frame: float - delta_acc_mm_per_frame2: float + mpjpe_m: float + root_pos_error_m: float + root_rot_error_rad: float + root_vel_error_m_s: float def _json_safe(value: Any) -> Any: @@ -177,11 +178,18 @@ def build_benchmark_plan( ) -def compute_tracking_metrics(aligned_ref_pos: np.ndarray, aligned_robot_pos: np.ndarray) -> dict[str, float]: - """Compute MPJPE, delta velocity, and delta acceleration from aligned key bodies. +def compute_tracking_metrics( + aligned_ref_pos: np.ndarray, + aligned_robot_pos: np.ndarray, + root_pos_error_m: np.ndarray, + root_rot_error_rad: np.ndarray, + root_vel_error_m_s: np.ndarray, +) -> dict[str, float]: + """Compute benchmark tracking metrics from aligned key bodies and root errors. - Inputs are ``(T, B, 3)`` arrays in root/anchor coordinates. Velocity and - acceleration are frame differences, matching the paper's mm/frame units. + Key-body inputs are ``(T, B, 3)`` arrays in root/anchor coordinates. Root + error inputs are per-frame values computed with the same anchor metrics used + by ``MotionCommand``. """ ref = np.asarray(aligned_ref_pos, dtype=np.float64) robot = np.asarray(aligned_robot_pos, dtype=np.float64) @@ -190,29 +198,29 @@ def compute_tracking_metrics(aligned_ref_pos: np.ndarray, aligned_robot_pos: np. if ref.ndim != 3 or ref.shape[-1] != 3: raise ValueError(f"aligned positions must be (T,B,3), got {ref.shape}") if ref.shape[0] == 0 or ref.shape[1] == 0: - raise ValueError(f"aligned positions must have non-empty T and B dimensions, got {ref.shape}") + raise ValueError( + f"aligned positions must have non-empty T and B dimensions, got {ref.shape}" + ) + + root_pos = np.asarray(root_pos_error_m, dtype=np.float64) + root_rot = np.asarray(root_rot_error_rad, dtype=np.float64) + root_vel = np.asarray(root_vel_error_m_s, dtype=np.float64) + for name, values in ( + ("root_pos_error_m", root_pos), + ("root_rot_error_rad", root_rot), + ("root_vel_error_m_s", root_vel), + ): + if values.size == 0: + raise ValueError(f"{name} must be non-empty") pos_error = np.linalg.norm(ref - robot, axis=-1) - mpjpe = float(pos_error.mean() * 1000.0) - - if ref.shape[0] >= 2: - ref_vel = np.diff(ref, axis=0) - robot_vel = np.diff(robot, axis=0) - delta_vel = float(np.linalg.norm(ref_vel - robot_vel, axis=-1).mean() * 1000.0) - else: - delta_vel = float("nan") - - if ref.shape[0] >= 3: - ref_acc = np.diff(np.diff(ref, axis=0), axis=0) - robot_acc = np.diff(np.diff(robot, axis=0), axis=0) - delta_acc = float(np.linalg.norm(ref_acc - robot_acc, axis=-1).mean() * 1000.0) - else: - delta_acc = float("nan") + mpjpe = float(pos_error.mean()) return { - "mpjpe_mm": mpjpe, - "delta_vel_mm_per_frame": delta_vel, - "delta_acc_mm_per_frame2": delta_acc, + "mpjpe_m": mpjpe, + "root_pos_error_m": float(root_pos.mean()), + "root_rot_error_rad": float(root_rot.mean()), + "root_vel_error_m_s": float(root_vel.mean()), } @@ -238,12 +246,15 @@ def finite_mean(values: Iterable[float]) -> float: "success_rate": 100.0 * sum(1 for result in clip_results if result.success) / len(clip_results), - "mpjpe_mm": finite_mean(result.mpjpe_mm for result in clip_results), - "delta_vel_mm_per_frame": finite_mean( - result.delta_vel_mm_per_frame for result in clip_results + "mpjpe_m": finite_mean(result.mpjpe_m for result in clip_results), + "root_pos_error_m": finite_mean( + result.root_pos_error_m for result in clip_results ), - "delta_acc_mm_per_frame2": finite_mean( - result.delta_acc_mm_per_frame2 for result in clip_results + "root_rot_error_rad": finite_mean( + result.root_rot_error_rad for result in clip_results + ), + "root_vel_error_m_s": finite_mean( + result.root_vel_error_m_s for result in clip_results ), } ) @@ -252,13 +263,18 @@ def finite_mean(values: Iterable[float]) -> float: "global": { "clips": len(clip_ids), "rollouts": len(results), - "success_rate": 100.0 * sum(1 for result in results if result.success) / len(results), - "mpjpe_mm": finite_mean(result.mpjpe_mm for result in results), - "delta_vel_mm_per_frame": finite_mean( - result.delta_vel_mm_per_frame for result in results + "success_rate": 100.0 + * sum(1 for result in results if result.success) + / len(results), + "mpjpe_m": finite_mean(result.mpjpe_m for result in results), + "root_pos_error_m": finite_mean( + result.root_pos_error_m for result in results + ), + "root_rot_error_rad": finite_mean( + result.root_rot_error_rad for result in results ), - "delta_acc_mm_per_frame2": finite_mean( - result.delta_acc_mm_per_frame2 for result in results + "root_vel_error_m_s": finite_mean( + result.root_vel_error_m_s for result in results ), }, "per_clip": per_clip, @@ -292,9 +308,10 @@ def write_benchmark_outputs( f"eligible_clips: {len(plan.eligible_clips)}", f"skipped_short_clips: {len(plan.skipped_short_clips)}", "", - f"MPJPE(mm): {global_summary['mpjpe_mm']:.6f}", - f"delta_vel(mm/frame): {global_summary['delta_vel_mm_per_frame']:.6f}", - f"delta_acc(mm/frame^2): {global_summary['delta_acc_mm_per_frame2']:.6f}", + f"MPJPE(m): {global_summary['mpjpe_m']:.6f}", + f"root_pos_error(m): {global_summary['root_pos_error_m']:.6f}", + f"root_rot_error(rad): {global_summary['root_rot_error_rad']:.6f}", + f"root_vel_error(m/s): {global_summary['root_vel_error_m_s']:.6f}", f"success_rate(%): {global_summary['success_rate']:.6f}", ] txt_path.write_text("\n".join(lines) + "\n") @@ -321,9 +338,10 @@ def write_benchmark_outputs( "clip_id", "rollouts", "success_rate", - "mpjpe_mm", - "delta_vel_mm_per_frame", - "delta_acc_mm_per_frame2", + "mpjpe_m", + "root_pos_error_m", + "root_rot_error_rad", + "root_vel_error_m_s", ], ) writer.writeheader() diff --git a/train_mimic/scripts/benchmark.py b/train_mimic/scripts/benchmark.py index 211a6bc3..85393d28 100644 --- a/train_mimic/scripts/benchmark.py +++ b/train_mimic/scripts/benchmark.py @@ -4,7 +4,7 @@ Default protocol: * 10 second clips at the policy control rate (500 steps at 50 Hz) * One deterministic rollout per eligible motion clip - * MPJPE, delta velocity, delta acceleration, and success rate + * MPJPE, root tracking errors, and success rate Usage: python train_mimic/scripts/benchmark.py \ @@ -149,6 +149,23 @@ def _aligned_keybody_positions(cmd: object) -> tuple[np.ndarray, np.ndarray]: ) +def _root_tracking_errors(cmd: object) -> tuple[np.ndarray, np.ndarray, np.ndarray]: + from mjlab.utils.lab_api.math import quat_error_magnitude + import torch + + root_pos_error = torch.norm(cmd.anchor_pos_w - cmd.robot_anchor_pos_w, dim=-1) + root_rot_error = quat_error_magnitude(cmd.anchor_quat_w, cmd.robot_anchor_quat_w) + root_vel_error = torch.norm( + cmd.anchor_lin_vel_w - cmd.robot_anchor_lin_vel_w, + dim=-1, + ) + return ( + root_pos_error.detach().cpu().numpy().astype(np.float32, copy=False), + root_rot_error.detach().cpu().numpy().astype(np.float32, copy=False), + root_vel_error.detach().cpu().numpy().astype(np.float32, copy=False), + ) + + def _failure_reason(env: object, env_index: int) -> str: manager = env.termination_manager for term_name in manager.active_terms: @@ -200,6 +217,9 @@ def _run_batch( aligned_ref_by_env: list[list[np.ndarray]] = [[] for _ in jobs] aligned_robot_by_env: list[list[np.ndarray]] = [[] for _ in jobs] + root_pos_error_by_env: list[list[float]] = [[] for _ in jobs] + root_rot_error_by_env: list[list[float]] = [[] for _ in jobs] + root_vel_error_by_env: list[list[float]] = [[] for _ in jobs] active = np.ones(len(jobs), dtype=bool) finished: dict[int, tuple[bool, int, int | None, str | None]] = {} @@ -208,10 +228,20 @@ def _run_batch( cmd = env.command_manager.get_term("motion") for step in range(control_steps): ref_aligned, robot_aligned = _aligned_keybody_positions(cmd) + root_pos_error, root_rot_error, root_vel_error = _root_tracking_errors(cmd) for env_index, is_active in enumerate(active): if is_active: aligned_ref_by_env[env_index].append(ref_aligned[env_index]) aligned_robot_by_env[env_index].append(robot_aligned[env_index]) + root_pos_error_by_env[env_index].append( + float(root_pos_error[env_index]) + ) + root_rot_error_by_env[env_index].append( + float(root_rot_error[env_index]) + ) + root_vel_error_by_env[env_index].append( + float(root_vel_error[env_index]) + ) with torch_module.no_grad(): actions = policy(obs) @@ -266,12 +296,16 @@ def _run_batch( metrics = compute_tracking_metrics( np.stack(aligned_ref_by_env[env_index], axis=0), np.stack(aligned_robot_by_env[env_index], axis=0), + np.asarray(root_pos_error_by_env[env_index], dtype=np.float32), + np.asarray(root_rot_error_by_env[env_index], dtype=np.float32), + np.asarray(root_vel_error_by_env[env_index], dtype=np.float32), ) if not success: metrics = { - "mpjpe_mm": float("nan"), - "delta_vel_mm_per_frame": float("nan"), - "delta_acc_mm_per_frame2": float("nan"), + "mpjpe_m": float("nan"), + "root_pos_error_m": float("nan"), + "root_rot_error_rad": float("nan"), + "root_vel_error_m_s": float("nan"), } results.append( RolloutResult( @@ -282,9 +316,10 @@ def _run_batch( steps=steps, failure_step=failure_step, failure_reason=failure_reason, - mpjpe_mm=metrics["mpjpe_mm"], - delta_vel_mm_per_frame=metrics["delta_vel_mm_per_frame"], - delta_acc_mm_per_frame2=metrics["delta_acc_mm_per_frame2"], + mpjpe_m=metrics["mpjpe_m"], + root_pos_error_m=metrics["root_pos_error_m"], + root_rot_error_rad=metrics["root_rot_error_rad"], + root_vel_error_m_s=metrics["root_vel_error_m_s"], ) ) return results @@ -401,9 +436,10 @@ def main(argv: Sequence[str] | None = None) -> int: summary = summarize_rollouts(results)["global"] print("\nBenchmark Results:") - print(f" MPJPE(mm): {summary['mpjpe_mm']:.4f}") - print(f" delta_vel(mm/frame): {summary['delta_vel_mm_per_frame']:.4f}") - print(f" delta_acc(mm/frame^2): {summary['delta_acc_mm_per_frame2']:.4f}") + print(f" MPJPE(m): {summary['mpjpe_m']:.4f}") + print(f" root_pos_error(m): {summary['root_pos_error_m']:.4f}") + print(f" root_rot_error(rad): {summary['root_rot_error_rad']:.4f}") + print(f" root_vel_error(m/s): {summary['root_vel_error_m_s']:.4f}") print(f" success_rate(%): {summary['success_rate']:.2f}") for label, path in paths.items(): print(f"Saved {label}: {path}") From 6d26580d06c362880c9cd59d1b9784b206e8ca90 Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Fri, 3 Jul 2026 12:22:22 +0800 Subject: [PATCH 03/59] Increase action rate penalty --- train_mimic/tasks/tracking/tracking_env_cfg.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/train_mimic/tasks/tracking/tracking_env_cfg.py b/train_mimic/tasks/tracking/tracking_env_cfg.py index ece6329a..eb1faaae 100644 --- a/train_mimic/tasks/tracking/tracking_env_cfg.py +++ b/train_mimic/tasks/tracking/tracking_env_cfg.py @@ -269,7 +269,7 @@ def make_tracking_env_cfg() -> ManagerBasedRlEnvCfg: params={"command_name": "motion", "std": 3.0}, ), "survival": RewardTermCfg(func=mdp.survival, weight=3.0), - "action_rate_l2": RewardTermCfg(func=mdp.action_rate_l2, weight=-0.3), + "action_rate_l2": RewardTermCfg(func=mdp.action_rate_l2, weight=-0.5), "joint_limit": RewardTermCfg( func=mdp.joint_pos_limits, weight=-10.0, From 3b22b0f059c4eb4429a39f8f4b943db8d2f1a14d Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Tue, 7 Jul 2026 17:12:10 +0800 Subject: [PATCH 04/59] Add optional OpenNeck sim2real worker --- AGENTS.md | 5 +- README.md | 17 ++ docs/docs/configuration/config-reference.md | 26 ++ docs/docs/getting-started/installation.md | 9 + .../current/configuration/config-reference.md | 25 ++ .../current/getting-started/installation.md | 8 + pyproject.toml | 4 + teleopit/configs/sim2real.yaml | 23 ++ teleopit/sim2real/mp/runtime.py | 80 ++++++ teleopit/sim2real/neck/__init__.py | 12 + teleopit/sim2real/neck/config.py | 106 ++++++++ teleopit/sim2real/neck/mapper.py | 122 ++++++++++ teleopit/sim2real/neck/openneck.py | 104 ++++++++ teleopit/sim2real/neck/worker.py | 98 ++++++++ tests/test_active_neck.py | 230 ++++++++++++++++++ tests/test_sim2real_multiprocess.py | 83 +++++++ 16 files changed, 951 insertions(+), 1 deletion(-) create mode 100644 teleopit/sim2real/neck/__init__.py create mode 100644 teleopit/sim2real/neck/config.py create mode 100644 teleopit/sim2real/neck/mapper.py create mode 100644 teleopit/sim2real/neck/openneck.py create mode 100644 teleopit/sim2real/neck/worker.py create mode 100644 tests/test_active_neck.py diff --git a/AGENTS.md b/AGENTS.md index 2ce48804..f2a82862 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -56,7 +56,8 @@ teleopit/ # Core inference package │ └── loop.py # SimulationLoop — PD control at 200Hz, policy at 50Hz ├── sim2real/ │ ├── mp/ # Process-isolated sim2real runtime and IPC -│ └── hands/ # Optional LinkerHand driver/mapper plugins +│ ├── hands/ # Optional LinkerHand driver/mapper plugins +│ └── neck/ # Optional OpenNeck active-vision gimbal control └── recording/ # Pico motion NPZ recording helpers scripts/ ├── run/run_sim.py # Offline sim2sim pipeline @@ -159,6 +160,8 @@ target_dof_pos = clip(action, -10, 10) × action_scale + default_dof_pos - `vr_hand_pose` defaults to a low-latency somehand path: `hands.somehand.rate_hz=60`, `max_iterations=12`, `temporal_filter_alpha=1.0`, and `output_alpha=1.0`; this prioritizes response speed over smoothing - LinkerHand control is active in all sim2real modes when `hands.enabled=true`; shutdown and hand-runtime failure must send the configured open pose - In `vr_hand_pose` mode, missing/inactive hand pose holds the last commanded pose for that side instead of opening the hand +- Optional OpenNeck active-vision gimbal control uses `neck.enabled=true` and `neck.driver=openneck`; it requires `input.provider=pico4`, reuses the existing Pico body frame stream, and must not start a second `PicoBridge` +- OpenNeck is integrated as a non-critical sim2real `neck_worker`; failures should not stop the G1 control loop, and no OpenNeck state is added to the 167D policy observation ### SimulationLoop Runtime Behavior - `realtime=true` enforces wall-clock pacing even without a viewer diff --git a/README.md b/README.md index b2990a7f..81d1da0d 100644 --- a/README.md +++ b/README.md @@ -108,6 +108,23 @@ sync metadata in the HDF5 episode. The low-dimensional HDF5 schema records reference qpos sent to the policy path, and `action.hand(12)` as the latest LinkerHand left/right 6D pose commands. +## OpenNeck Active Vision + +Pico sim2real can drive the optional OpenNeck two-axis active-vision gimbal from +the same Pico body tracking stream used for whole-body control: + +```bash +pip install -e '.[openneck]' +python scripts/run/run_sim2real.py --config-name pico4_sim2real \ + controller.policy_path=track.onnx \ + neck.enabled=true \ + neck.port=/dev/ttyACM0 +``` + +`neck.enabled=true` requires `input.provider=pico4`. The neck worker reuses the +existing Teleopit Pico receiver and does not start another `PicoBridge` or +camera pipeline. + ## Documentation Full docs at **[BotRunner64.github.io/Teleopit](https://BotRunner64.github.io/Teleopit/)**, covering installation profiles, all tutorials, configuration reference, and architecture. diff --git a/docs/docs/configuration/config-reference.md b/docs/docs/configuration/config-reference.md index f6bf965f..e1b938a5 100644 --- a/docs/docs/configuration/config-reference.md +++ b/docs/docs/configuration/config-reference.md @@ -152,6 +152,32 @@ calling somehand 0.2.0 through `somehand.api` only. | `hands.somehand.temporal_filter_alpha` | somehand input landmark smoothing alpha; `1.0` disables smoothing delay | `1.0` | | `hands.somehand.output_alpha` | somehand qpos output smoothing alpha; `1.0` disables smoothing delay | `1.0` | +### OpenNeck Active Vision (Pico sim2real) + +`neck.enabled=true` requires `input.provider=pico4` and the `openneck` extra. The +neck worker reuses Teleopit's existing Pico body-frame stream and does not start +a second `PicoBridge` or RealSense pipeline. OpenNeck runs as a non-critical +sim2real worker and does not change the policy observation. + +| Field | Description | Default | +|-------|-------------|---------| +| `neck.enabled` | Enable optional OpenNeck worker | `false` | +| `neck.driver` | Neck driver plugin; currently `openneck` | `openneck` | +| `neck.config_path` | Optional OpenNeck calibration config path | `null` | +| `neck.port` | Optional serial port override, for example `/dev/ttyACM0` | `null` | +| `neck.rate_hz` | Maximum neck command rate in Hz | `60.0` | +| `neck.frame_timeout_s` | Pico body-frame staleness threshold | `0.2` | +| `neck.active_modes` | Sim2real modes that allow neck motion | `[standing, mocap, arms, pause]` | +| `neck.head_joint` / `body_reference_joint` | Pico body joints used for relative head mapping | `Head` / `Spine3` | +| `neck.use_body_reference` | Map head motion relative to the body reference joint | `true` | +| `neck.dead_zone_deg` | Yaw/pitch dead zone in degrees | `0.5` | +| `neck.smoothing_alpha` | EMA alpha for normalized yaw/pitch commands | `0.35` | +| `neck.yaw_range_deg` / `pitch_range_deg` | Degrees mapped to normalized command magnitude `1.0` | `90.0` / `60.0` | +| `neck.invert_yaw` / `invert_pitch` | Invert OpenNeck command direction per axis | `true` / `true` | +| `neck.center_on_start` / `center_on_shutdown` | Center the gimbal at worker startup/shutdown | `true` / `true` | +| `neck.release_on_shutdown` | Release servo torque after shutdown when supported | `false` | +| `neck.dry_run` | Compute commands without opening OpenNeck hardware | `false` | + ### HDF5 Recording (Pico sim2real) `recording.enabled=true` is supported only with `input.provider=pico4`, diff --git a/docs/docs/getting-started/installation.md b/docs/docs/getting-started/installation.md index ed9daf6c..0de1e998 100644 --- a/docs/docs/getting-started/installation.md +++ b/docs/docs/getting-started/installation.md @@ -73,6 +73,15 @@ scripts/setup/download_somehand_l6_assets.sh These packages are only required when `hands.enabled=true`. +Optional OpenNeck active-vision control for Pico sim2real uses the remote +OpenNeck package: + +```bash +pip install -e '.[openneck]' +``` + +This extra includes the Pico stack and is only required when `neck.enabled=true`. + ### Sim2Real Recording ```bash diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md index 0366815c..2244dd77 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md @@ -170,6 +170,31 @@ Teleopit 会先将 Pico 手部状态转成 21 个 landmarks，再只通过 someh | `hands.somehand.temporal_filter_alpha` | somehand 输入 landmarks 平滑 alpha；`1.0` 表示关闭平滑延时 | `1.0` | | `hands.somehand.output_alpha` | somehand qpos 输出平滑 alpha；`1.0` 表示关闭平滑延时 | `1.0` | +### OpenNeck 主动视觉（Pico sim2real） + +`neck.enabled=true` 要求 `input.provider=pico4` 和 `openneck` extra。neck worker +复用 Teleopit 已有的 Pico body frame 数据流，不会启动第二个 `PicoBridge` 或 RealSense +管线。OpenNeck 作为非关键 sim2real worker 运行，不会改变策略观测。 + +| 字段 | 说明 | 默认值 | +|---|---|---| +| `neck.enabled` | 启用可选 OpenNeck worker | `false` | +| `neck.driver` | 头颈设备驱动插件；当前为 `openneck` | `openneck` | +| `neck.config_path` | 可选 OpenNeck 校准配置路径 | `null` | +| `neck.port` | 可选串口覆盖，例如 `/dev/ttyACM0` | `null` | +| `neck.rate_hz` | 最大头颈命令频率（Hz） | `60.0` | +| `neck.frame_timeout_s` | Pico body frame 过期阈值 | `0.2` | +| `neck.active_modes` | 允许头颈运动的 sim2real 模式 | `[standing, mocap, arms, pause]` | +| `neck.head_joint` / `body_reference_joint` | 用于相对头部映射的 Pico body 关节 | `Head` / `Spine3` | +| `neck.use_body_reference` | 相对于 body reference 关节映射头部运动 | `true` | +| `neck.dead_zone_deg` | yaw/pitch 死区（度） | `0.5` | +| `neck.smoothing_alpha` | 归一化 yaw/pitch 命令的 EMA alpha | `0.35` | +| `neck.yaw_range_deg` / `pitch_range_deg` | 映射到归一化命令幅值 `1.0` 的角度 | `90.0` / `60.0` | +| `neck.invert_yaw` / `invert_pitch` | 按轴反转 OpenNeck 命令方向 | `true` / `true` | +| `neck.center_on_start` / `center_on_shutdown` | worker 启动/关闭时回中云台 | `true` / `true` | +| `neck.release_on_shutdown` | 关闭后在支持时释放舵机扭矩 | `false` | +| `neck.dry_run` | 只计算命令，不打开 OpenNeck 硬件 | `false` | + ### HDF5 录制（Pico sim2real） `recording.enabled=true` 只支持 `input.provider=pico4`、 diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/installation.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/installation.md index 4f3028c0..3220900e 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/installation.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/installation.md @@ -73,6 +73,14 @@ scripts/setup/download_somehand_l6_assets.sh 只有在 `hands.enabled=true` 时才需要安装这些包。 +Pico sim2real 可选的 OpenNeck 主动视觉控制使用远程 OpenNeck 包： + +```bash +pip install -e '.[openneck]' +``` + +该 extra 包含 Pico 栈，只有在 `neck.enabled=true` 时才需要安装。 + ### Sim2Real 录制 ```bash diff --git a/pyproject.toml b/pyproject.toml index 4bb6ce39..77704031 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -53,6 +53,10 @@ pico4 = [ "pico-bridge[camera] @ https://github.com/BotRunner64/pico-bridge/releases/download/v0.2.1/pico_bridge-0.2.1-py3-none-any.whl", "teleopit[sim2real]", ] +openneck = [ + "openneck @ git+https://github.com/BotRunner64/OpenNeck.git", + "teleopit[pico4]", +] recording = [ "teleopit[pico4]", "opencv-python", diff --git a/teleopit/configs/sim2real.yaml b/teleopit/configs/sim2real.yaml index f2dbe24f..cc67b688 100644 --- a/teleopit/configs/sim2real.yaml +++ b/teleopit/configs/sim2real.yaml @@ -105,6 +105,29 @@ hands: temporal_filter_alpha: 1.0 output_alpha: 1.0 +# Optional OpenNeck active-vision gimbal control. Use only with input.provider=pico4. +neck: + enabled: false + driver: openneck + config_path: null + port: null + rate_hz: 60.0 + frame_timeout_s: 0.2 + active_modes: [standing, mocap, arms, pause] + head_joint: Head + body_reference_joint: Spine3 + use_body_reference: true + dead_zone_deg: 0.5 + smoothing_alpha: 0.35 + yaw_range_deg: 90.0 + pitch_range_deg: 60.0 + invert_yaw: true + invert_pitch: true + center_on_start: true + center_on_shutdown: true + release_on_shutdown: false + dry_run: false + # Physical robot SDK configuration real_robot: network_interface: "eth0" diff --git a/teleopit/sim2real/mp/runtime.py b/teleopit/sim2real/mp/runtime.py index f600a5bb..700e619d 100644 --- a/teleopit/sim2real/mp/runtime.py +++ b/teleopit/sim2real/mp/runtime.py @@ -62,6 +62,8 @@ from teleopit.sim2real.hands.base import HandPoseCommand from teleopit.sim2real.hands.linkerhand_l6 import parse_linkerhand_l6_config from teleopit.sim2real.hands.linkerhand_o6 import parse_linkerhand_o6_config +from teleopit.sim2real.neck.config import parse_neck_config +from teleopit.sim2real.neck.worker import body_packet_frame, build_neck_runtime, mode_packet_active from teleopit.sim2real.mp.ipc import ( BODY_TOPIC, COMMAND_TOPIC, @@ -314,6 +316,12 @@ def _validate_new_runtime_config(cfg: Any) -> None: hands_cfg = cfg_get(cfg, "hands", {}) or {} if bool(cfg_get(hands_cfg, "enabled", False)) and provider != "pico4": raise ValueError("hands.enabled=true requires input.provider=pico4") + neck_cfg = parse_neck_config(cfg) + if neck_cfg.enabled: + if provider != "pico4": + raise ValueError("neck.enabled=true requires input.provider=pico4") + if neck_cfg.driver != "openneck": + raise ValueError(f"Unsupported neck.driver={neck_cfg.driver!r}; supported drivers: openneck") if _recording_enabled(cfg): if provider != "pico4": raise ValueError("recording.enabled=true requires input.provider=pico4") @@ -412,6 +420,7 @@ def run(self) -> None: self._command_pub = ZmqPublisher(self._endpoints.command_pub) self._keyboard = TerminalKeyboardReader() operator_logger.info("keyboard recording controls active: R start, S save, D discard, Q shutdown, H help") + reported_noncritical_dead: set[str] = set() while not self._stop_event.is_set(): self._poll_terminal_recording_controls() time.sleep(0.2) @@ -435,9 +444,11 @@ def run(self) -> None: if not process.is_alive() and process.exitcode not in (None, 0) and process.name not in critical_names + and process.name not in reported_noncritical_dead ] if noncritical_dead: operator_logger.warning("non-critical worker exited: %s", ", ".join(noncritical_dead)) + reported_noncritical_dead.update(noncritical_dead) except KeyboardInterrupt: operator_logger.info("keyboard interrupt -> shutting down") self._stop_event.set() @@ -479,6 +490,9 @@ def _start_processes(self) -> None: hands_cfg = cfg_get(self.cfg, "hands", {}) or {} if bool(cfg_get(hands_cfg, "enabled", False)): specs.append(("hand_worker", _run_hand_worker)) + neck_cfg = parse_neck_config(self.cfg) + if neck_cfg.enabled: + specs.append(("neck_worker", _run_neck_worker)) if _recording_enabled(self.cfg): specs.append(("recording_worker", _run_recording_worker)) video_cfg = parse_pico_video_config(cfg_get(self.cfg, "input", {})) @@ -1999,6 +2013,72 @@ def _main() -> None: _worker_loop("recording_worker", cfg, _main) +def _run_neck_worker( + cfg: dict[str, Any], + endpoints: Sim2RealIpcEndpoints, + stop_event: MpEvent, +) -> None: + def _main() -> None: + neck_cfg = parse_neck_config(cfg) + runtime = build_neck_runtime(neck_cfg) + body_sub = LatestSubscriber(endpoints.body_pub, BODY_TOPIC) + mode_sub = LatestSubscriber(endpoints.mode_pub, MODE_TOPIC) + command_sub = LatestSubscriber(endpoints.command_pub, COMMAND_TOPIC) + latest_frame: Any | None = None + latest_frame_timestamp_s: float | None = None + latest_body_seq = -1 + latest_mode: ModeStatePacket | None = None + command_count = 0 + sleep_s = 1.0 / max(float(neck_cfg.rate_hz), 1.0) + last_status_s = 0.0 + try: + runtime.start() + while not stop_event.is_set(): + command = command_sub.recv_latest() + if isinstance(command, CommandPacket) and command.command == "shutdown": + stop_event.set() + break + body_packet = body_sub.recv_latest() + frame, frame_timestamp_s, body_seq = body_packet_frame(body_packet) + if frame is not None: + latest_frame = frame + latest_frame_timestamp_s = frame_timestamp_s + latest_body_seq = body_seq + mode_packet = mode_sub.recv_latest() + if isinstance(mode_packet, ModeStatePacket): + latest_mode = mode_packet + now_s = time.monotonic() + try: + moved = runtime.tick( + frame=latest_frame, + frame_timestamp_s=latest_frame_timestamp_s, + active=mode_packet_active(latest_mode, neck_cfg), + now_s=now_s, + ) + if moved: + command_count += 1 + except Exception: + logger.exception("OpenNeck worker tick failed; neck control continues") + if now_s - last_status_s >= 5.0: + logger.debug( + "OpenNeck worker status | body_seq=%s commands=%s active=%s", + latest_body_seq, + command_count, + mode_packet_active(latest_mode, neck_cfg), + ) + last_status_s = now_s + time.sleep(sleep_s) + finally: + try: + runtime.close() + finally: + body_sub.close() + mode_sub.close() + command_sub.close() + + _worker_loop("neck_worker", cfg, _main) + + class _HandSnapshotProxy: def __init__(self) -> None: self.hand_snapshot: Any | None = None diff --git a/teleopit/sim2real/neck/__init__.py b/teleopit/sim2real/neck/__init__.py new file mode 100644 index 00000000..32f0f3dd --- /dev/null +++ b/teleopit/sim2real/neck/__init__.py @@ -0,0 +1,12 @@ +"""Optional active-neck runtimes for sim2real.""" + +from teleopit.sim2real.neck.config import NeckConfig, parse_neck_config +from teleopit.sim2real.neck.mapper import HeadPoseMapper +from teleopit.sim2real.neck.worker import build_neck_runtime + +__all__ = [ + "HeadPoseMapper", + "NeckConfig", + "build_neck_runtime", + "parse_neck_config", +] diff --git a/teleopit/sim2real/neck/config.py b/teleopit/sim2real/neck/config.py new file mode 100644 index 00000000..05024109 --- /dev/null +++ b/teleopit/sim2real/neck/config.py @@ -0,0 +1,106 @@ +from __future__ import annotations + +from collections.abc import Iterable +from dataclasses import dataclass +from pathlib import Path +from typing import Any + +from teleopit.runtime.common import cfg_get + +VALID_NECK_ACTIVE_MODES = frozenset(("standing", "mocap", "arms", "pause")) + + +@dataclass(frozen=True) +class NeckConfig: + enabled: bool = False + driver: str = "openneck" + config_path: str | None = None + port: str | None = None + rate_hz: float = 60.0 + frame_timeout_s: float = 0.2 + active_modes: tuple[str, ...] = ("standing", "mocap", "arms", "pause") + head_joint: str = "Head" + body_reference_joint: str = "Spine3" + use_body_reference: bool = True + dead_zone_deg: float = 0.5 + smoothing_alpha: float = 0.35 + yaw_range_deg: float = 90.0 + pitch_range_deg: float = 60.0 + invert_yaw: bool = True + invert_pitch: bool = True + center_on_start: bool = True + center_on_shutdown: bool = True + release_on_shutdown: bool = False + dry_run: bool = False + + +def parse_neck_config(cfg: Any) -> NeckConfig: + neck_cfg = cfg_get(cfg, "neck", {}) or {} + active_modes = _parse_active_modes(cfg_get(neck_cfg, "active_modes", ["standing", "mocap", "arms", "pause"])) + rate_hz = float(cfg_get(neck_cfg, "rate_hz", 60.0)) + if rate_hz <= 0: + raise ValueError("neck.rate_hz must be > 0") + frame_timeout_s = float(cfg_get(neck_cfg, "frame_timeout_s", 0.2)) + if frame_timeout_s <= 0: + raise ValueError("neck.frame_timeout_s must be > 0") + smoothing_alpha = float(cfg_get(neck_cfg, "smoothing_alpha", 0.35)) + if not 0.0 < smoothing_alpha <= 1.0: + raise ValueError("neck.smoothing_alpha must be in (0, 1]") + dead_zone_deg = float(cfg_get(neck_cfg, "dead_zone_deg", 0.5)) + if dead_zone_deg < 0: + raise ValueError("neck.dead_zone_deg must be >= 0") + yaw_range_deg = float(cfg_get(neck_cfg, "yaw_range_deg", 90.0)) + pitch_range_deg = float(cfg_get(neck_cfg, "pitch_range_deg", 60.0)) + if yaw_range_deg <= 0: + raise ValueError("neck.yaw_range_deg must be > 0") + if pitch_range_deg <= 0: + raise ValueError("neck.pitch_range_deg must be > 0") + config_path = cfg_get(neck_cfg, "config_path", None) + if config_path in ("", "null"): + config_path = None + elif config_path is not None: + config_path = str(Path(str(config_path)).expanduser()) + port = cfg_get(neck_cfg, "port", None) + if port in ("", "null"): + port = None + return NeckConfig( + enabled=bool(cfg_get(neck_cfg, "enabled", False)), + driver=str(cfg_get(neck_cfg, "driver", "openneck")).strip().lower(), + config_path=config_path, + port=None if port is None else str(port), + rate_hz=rate_hz, + frame_timeout_s=frame_timeout_s, + active_modes=active_modes, + head_joint=str(cfg_get(neck_cfg, "head_joint", "Head")), + body_reference_joint=str(cfg_get(neck_cfg, "body_reference_joint", "Spine3")), + use_body_reference=bool(cfg_get(neck_cfg, "use_body_reference", True)), + dead_zone_deg=dead_zone_deg, + smoothing_alpha=smoothing_alpha, + yaw_range_deg=yaw_range_deg, + pitch_range_deg=pitch_range_deg, + invert_yaw=bool(cfg_get(neck_cfg, "invert_yaw", True)), + invert_pitch=bool(cfg_get(neck_cfg, "invert_pitch", True)), + center_on_start=bool(cfg_get(neck_cfg, "center_on_start", True)), + center_on_shutdown=bool(cfg_get(neck_cfg, "center_on_shutdown", True)), + release_on_shutdown=bool(cfg_get(neck_cfg, "release_on_shutdown", False)), + dry_run=bool(cfg_get(neck_cfg, "dry_run", False)), + ) + + +def _parse_active_modes(value: Any) -> tuple[str, ...]: + if isinstance(value, str): + modes = (value.strip().lower(),) + elif isinstance(value, Iterable): + modes = tuple(str(mode).strip().lower() for mode in value) + else: + raise ValueError("neck.active_modes must be a mode string or a list of modes") + modes = tuple(mode for mode in modes if mode) + if not modes: + raise ValueError("neck.active_modes must contain at least one mode") + unsupported = sorted(set(modes).difference(VALID_NECK_ACTIVE_MODES)) + if unsupported: + raise ValueError( + "neck.active_modes contains unsupported modes " + f"{unsupported}; supported modes: {sorted(VALID_NECK_ACTIVE_MODES)}" + ) + return modes diff --git a/teleopit/sim2real/neck/mapper.py b/teleopit/sim2real/neck/mapper.py new file mode 100644 index 00000000..bc919fce --- /dev/null +++ b/teleopit/sim2real/neck/mapper.py @@ -0,0 +1,122 @@ +from __future__ import annotations + +from dataclasses import dataclass +import math + +import numpy as np +from numpy.typing import NDArray + +from teleopit.inputs.realtime_packet import HumanFrame +from teleopit.sim2real.neck.config import NeckConfig + + +FloatArray = NDArray[np.float64] + + +@dataclass(frozen=True) +class NeckCommand: + yaw: float + pitch: float + yaw_deg: float + pitch_deg: float + roll_deg: float + + +class HeadPoseMapper: + """Map Teleopit Pico body frames to normalized active-neck yaw/pitch commands.""" + + def __init__(self, config: NeckConfig) -> None: + self._cfg = config + self._offset = np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float64) + self._calibrated = False + self._smooth_yaw = 0.0 + self._smooth_pitch = 0.0 + + @property + def calibrated(self) -> bool: + return self._calibrated + + def reset(self) -> None: + self._offset = np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float64) + self._calibrated = False + self._smooth_yaw = 0.0 + self._smooth_pitch = 0.0 + + def map_frame(self, frame: HumanFrame) -> NeckCommand | None: + q_head = _joint_quat(frame, self._cfg.head_joint) + if q_head is None: + return None + q_body = _joint_quat(frame, self._cfg.body_reference_joint) if self._cfg.use_body_reference else None + relative = self._relative(q_head, q_body) + if not self._calibrated: + self._offset = relative + self._calibrated = True + return None + + q_cmd = _qmul(relative, _qconj(self._offset)) + yaw_deg, pitch_deg, roll_deg = _openneck_yaw_pitch_roll_deg(q_cmd) + if self._cfg.invert_yaw: + yaw_deg = -yaw_deg + if self._cfg.invert_pitch: + pitch_deg = -pitch_deg + if abs(yaw_deg) < self._cfg.dead_zone_deg: + yaw_deg = 0.0 + if abs(pitch_deg) < self._cfg.dead_zone_deg: + pitch_deg = 0.0 + + yaw = yaw_deg / self._cfg.yaw_range_deg + pitch = pitch_deg / self._cfg.pitch_range_deg + alpha = self._cfg.smoothing_alpha + self._smooth_yaw += alpha * (yaw - self._smooth_yaw) + self._smooth_pitch += alpha * (pitch - self._smooth_pitch) + return NeckCommand( + yaw=float(np.clip(self._smooth_yaw, -1.0, 1.0)), + pitch=float(np.clip(self._smooth_pitch, -1.0, 1.0)), + yaw_deg=float(yaw_deg), + pitch_deg=float(pitch_deg), + roll_deg=float(roll_deg), + ) + + def _relative(self, q_head: FloatArray, q_body: FloatArray | None) -> FloatArray: + if q_body is not None: + return _qmul(_qconj(q_body), q_head) + return q_head + + +def _joint_quat(frame: HumanFrame, joint_name: str) -> FloatArray | None: + item = frame.get(joint_name) + if item is None: + return None + quat = np.asarray(item[1], dtype=np.float64).reshape(-1) + if quat.shape[0] != 4 or not np.all(np.isfinite(quat)): + return None + norm = float(np.linalg.norm(quat)) + if norm <= 1e-9: + return None + return quat / norm + + +def _qconj(q: FloatArray) -> FloatArray: + return np.array([q[0], -q[1], -q[2], -q[3]], dtype=np.float64) + + +def _qmul(a: FloatArray, b: FloatArray) -> FloatArray: + w1, x1, y1, z1 = a + w2, x2, y2, z2 = b + return np.array( + [ + w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2, + w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2, + w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2, + w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2, + ], + dtype=np.float64, + ) + + +def _openneck_yaw_pitch_roll_deg(q_wxyz: FloatArray) -> tuple[float, float, float]: + w, x, y, z = q_wxyz + yaw = math.degrees(math.atan2(2.0 * (x * z + w * y), 1.0 - 2.0 * (y * y + z * z))) + pitch = math.degrees(math.asin(float(np.clip(-2.0 * (y * z - w * x), -1.0, 1.0)))) + roll = math.degrees(math.atan2(2.0 * (x * y + w * z), 1.0 - 2.0 * (x * x + z * z))) + return yaw, pitch, roll diff --git a/teleopit/sim2real/neck/openneck.py b/teleopit/sim2real/neck/openneck.py new file mode 100644 index 00000000..d6f67971 --- /dev/null +++ b/teleopit/sim2real/neck/openneck.py @@ -0,0 +1,104 @@ +from __future__ import annotations + +import logging +from typing import Protocol + +from teleopit.sim2real.neck.config import NeckConfig + +logger = logging.getLogger(__name__) + + +class NeckDevice(Protocol): + def connect(self) -> None: ... + + def center(self) -> None: ... + + def release(self) -> None: ... + + def move_norm(self, yaw: float, pitch: float) -> None: ... + + def close(self) -> None: ... + + +class OpenNeckDevice: + def __init__(self, config: NeckConfig) -> None: + self._cfg = config + self._context = None + self._controller = None + + def connect(self) -> None: + try: + from openneck import OpenNeckController + except ModuleNotFoundError as exc: + raise ImportError( + "openneck is required for neck.driver=openneck. " + "Install with: pip install -e '.[openneck]'" + ) from exc + controller = OpenNeckController( + config=self._cfg.config_path, + port=self._cfg.port, + enable_torque_on_connect=True, + ) + entered = controller.__enter__() + self._context = controller + self._controller = controller if entered is None else entered + logger.info("OpenNeck connected on port %s", getattr(self._controller, "port", self._cfg.port)) + + def center(self) -> None: + if self._controller is not None: + self._controller.center(wait_s=0.5) + + def move_norm(self, yaw: float, pitch: float) -> None: + if self._controller is not None: + self._controller.move_norm(float(yaw), float(pitch)) + + def release(self) -> None: + if self._controller is None: + return + release = getattr(self._controller, "release", None) + if callable(release): + release() + return + disable_torque = getattr(self._controller, "disable_torque", None) + if callable(disable_torque): + disable_torque() + + def close(self) -> None: + context = self._context + controller = self._controller + self._context = None + self._controller = None + if context is not None: + exit_context = getattr(context, "__exit__", None) + if callable(exit_context): + exit_context(None, None, None) + return + if controller is not None: + close = getattr(controller, "close", None) + if callable(close): + close() + + +class DryRunNeckDevice: + def connect(self) -> None: + logger.info("OpenNeck dry-run device active") + + def center(self) -> None: + logger.info("OpenNeck dry-run center") + + def move_norm(self, yaw: float, pitch: float) -> None: + logger.debug("OpenNeck dry-run command yaw=%.3f pitch=%.3f", yaw, pitch) + + def release(self) -> None: + logger.info("OpenNeck dry-run release") + + def close(self) -> None: + logger.info("OpenNeck dry-run closed") + + +def build_neck_device(config: NeckConfig) -> NeckDevice: + if config.driver != "openneck": + raise ValueError("Unsupported neck.driver={!r}; supported drivers: openneck".format(config.driver)) + if config.dry_run: + return DryRunNeckDevice() + return OpenNeckDevice(config) diff --git a/teleopit/sim2real/neck/worker.py b/teleopit/sim2real/neck/worker.py new file mode 100644 index 00000000..2d2e671e --- /dev/null +++ b/teleopit/sim2real/neck/worker.py @@ -0,0 +1,98 @@ +from __future__ import annotations + +import logging +import time +from typing import Any + +from teleopit.inputs.realtime_packet import HumanFrame +from teleopit.sim2real.neck.config import NeckConfig, parse_neck_config +from teleopit.sim2real.neck.mapper import HeadPoseMapper +from teleopit.sim2real.neck.openneck import NeckDevice, build_neck_device + +logger = logging.getLogger(__name__) + + +class NeckRuntime: + def __init__(self, config: NeckConfig, device: NeckDevice | None = None) -> None: + self._cfg = config + self._device = device or build_neck_device(config) + self._mapper = HeadPoseMapper(config) + self._active = False + + def start(self) -> None: + self._device.connect() + if self._cfg.center_on_start: + self._device.center() + + def tick( + self, + *, + frame: HumanFrame | None, + frame_timestamp_s: float | None, + active: bool, + now_s: float | None = None, + ) -> bool: + now = time.monotonic() if now_s is None else float(now_s) + if active and not self._active: + self._mapper.reset() + self._active = bool(active) + if not self._active or frame is None or frame_timestamp_s is None: + return False + if now - float(frame_timestamp_s) > self._cfg.frame_timeout_s: + return False + command = self._mapper.map_frame(frame) + if command is None: + return False + self._device.move_norm(command.yaw, command.pitch) + return True + + def close(self) -> None: + try: + if self._cfg.center_on_shutdown: + self._device.center() + if self._cfg.release_on_shutdown: + self._device.release() + finally: + self._device.close() + + +class DisabledNeckRuntime: + def start(self) -> None: + return None + + def tick( + self, + *, + frame: HumanFrame | None, + frame_timestamp_s: float | None, + active: bool, + now_s: float | None = None, + ) -> bool: + del frame, frame_timestamp_s, active, now_s + return False + + def close(self) -> None: + return None + + +def build_neck_runtime(cfg: Any | NeckConfig, device: NeckDevice | None = None) -> NeckRuntime | DisabledNeckRuntime: + neck_cfg = cfg if isinstance(cfg, NeckConfig) else parse_neck_config(cfg) + if not neck_cfg.enabled: + return DisabledNeckRuntime() + return NeckRuntime(neck_cfg, device=device) + + +def mode_packet_active(mode_packet: object | None, config: NeckConfig) -> bool: + if mode_packet is None: + return False + mode = "pause" if bool(getattr(mode_packet, "mocap_paused", False)) else str(getattr(mode_packet, "mode", "")).strip().lower() + return mode in config.active_modes + + +def body_packet_frame(packet: object | None) -> tuple[HumanFrame | None, float | None, int]: + if packet is None or not all(hasattr(packet, attr) for attr in ("frame", "timestamp_s", "seq")): + return None, None, -1 + try: + return getattr(packet, "frame"), float(getattr(packet, "timestamp_s")), int(getattr(packet, "seq")) + except (TypeError, ValueError): + return None, None, -1 diff --git a/tests/test_active_neck.py b/tests/test_active_neck.py new file mode 100644 index 00000000..7a723f52 --- /dev/null +++ b/tests/test_active_neck.py @@ -0,0 +1,230 @@ +from __future__ import annotations + +import math +from types import ModuleType, SimpleNamespace + +import numpy as np + +from teleopit.sim2real.neck.config import NeckConfig, parse_neck_config +from teleopit.sim2real.neck.mapper import HeadPoseMapper +from teleopit.sim2real.neck.openneck import OpenNeckDevice +from teleopit.sim2real.neck.worker import NeckRuntime, body_packet_frame + + +def _quat_y(deg: float) -> np.ndarray: + rad = math.radians(deg) + return np.array([math.cos(rad / 2.0), 0.0, math.sin(rad / 2.0), 0.0], dtype=np.float64) + + +def _quat_x(deg: float) -> np.ndarray: + rad = math.radians(deg) + return np.array([math.cos(rad / 2.0), math.sin(rad / 2.0), 0.0, 0.0], dtype=np.float64) + + +def _frame(head: np.ndarray, spine: np.ndarray | None = None): + pos = np.zeros(3, dtype=np.float64) + frame = {"Head": (pos, head)} + if spine is not None: + frame["Spine3"] = (pos, spine) + return frame + + +def test_head_pose_mapper_calibrates_then_maps_yaw_pitch() -> None: + cfg = NeckConfig( + enabled=True, + smoothing_alpha=1.0, + invert_yaw=False, + invert_pitch=False, + use_body_reference=False, + dead_zone_deg=0.0, + ) + mapper = HeadPoseMapper(cfg) + + assert mapper.map_frame(_frame(_quat_y(0.0))) is None + command = mapper.map_frame(_frame(_quat_y(30.0))) + + assert command is not None + assert command.yaw_deg == pytest_approx(30.0) + assert command.yaw == pytest_approx(30.0 / 90.0) + + mapper.reset() + assert mapper.map_frame(_frame(_quat_x(0.0))) is None + command = mapper.map_frame(_frame(_quat_x(15.0))) + assert command is not None + assert command.pitch_deg == pytest_approx(15.0) + assert command.pitch == pytest_approx(15.0 / 60.0) + + +def test_head_pose_mapper_uses_body_relative_orientation() -> None: + cfg = NeckConfig( + enabled=True, + smoothing_alpha=1.0, + invert_yaw=False, + use_body_reference=True, + dead_zone_deg=0.0, + ) + mapper = HeadPoseMapper(cfg) + + assert mapper.map_frame(_frame(_quat_y(10.0), _quat_y(10.0))) is None + command = mapper.map_frame(_frame(_quat_y(40.0), _quat_y(10.0))) + + assert command is not None + assert command.yaw_deg == pytest_approx(30.0) + + +def test_neck_runtime_sends_command_after_calibration() -> None: + class FakeDevice: + def __init__(self) -> None: + self.moves: list[tuple[float, float]] = [] + self.center_calls = 0 + self.closed = False + + def connect(self) -> None: + return None + + def center(self) -> None: + self.center_calls += 1 + + def release(self) -> None: + return None + + def move_norm(self, yaw: float, pitch: float) -> None: + self.moves.append((yaw, pitch)) + + def close(self) -> None: + self.closed = True + + device = FakeDevice() + cfg = NeckConfig( + enabled=True, + smoothing_alpha=1.0, + invert_yaw=False, + use_body_reference=False, + dead_zone_deg=0.0, + center_on_start=True, + center_on_shutdown=True, + ) + runtime = NeckRuntime(cfg, device=device) + + runtime.start() + assert device.center_calls == 1 + assert not runtime.tick(frame=_frame(_quat_y(0.0)), frame_timestamp_s=1.0, active=True, now_s=1.01) + assert runtime.tick(frame=_frame(_quat_y(30.0)), frame_timestamp_s=1.02, active=True, now_s=1.03) + runtime.close() + + assert device.moves == [(30.0 / 90.0, 0.0)] + assert device.center_calls == 2 + assert device.closed is True + + +def test_neck_runtime_releases_on_shutdown_when_enabled() -> None: + class FakeDevice: + def __init__(self) -> None: + self.released = False + self.closed = False + + def connect(self) -> None: + return None + + def center(self) -> None: + return None + + def release(self) -> None: + self.released = True + + def move_norm(self, yaw: float, pitch: float) -> None: + del yaw, pitch + + def close(self) -> None: + self.closed = True + + device = FakeDevice() + runtime = NeckRuntime( + NeckConfig(enabled=True, center_on_start=False, center_on_shutdown=False, release_on_shutdown=True), + device=device, + ) + + runtime.close() + + assert device.released is True + assert device.closed is True + + +def test_body_packet_frame_ignores_incomplete_packets() -> None: + assert body_packet_frame(None) == (None, None, -1) + assert body_packet_frame(SimpleNamespace(frame=_frame(_quat_y(0.0)))) == (None, None, -1) + assert body_packet_frame(SimpleNamespace(frame=_frame(_quat_y(0.0)), timestamp_s="bad", seq=1)) == (None, None, -1) + + +def test_openneck_device_closes_context_manager(monkeypatch) -> None: + calls: list[str] = [] + + class FakeEnteredController: + port = "/dev/entered" + + def center(self, *, wait_s: float) -> None: + calls.append(f"entered-center-{wait_s}") + + def move_norm(self, yaw: float, pitch: float) -> None: + calls.append(f"entered-move-{yaw}-{pitch}") + + class FakeOpenNeckController: + port = "/dev/fake" + + def __init__(self, *, config: object, port: object, enable_torque_on_connect: bool) -> None: + del config, port, enable_torque_on_connect + self.entered = FakeEnteredController() + + def __enter__(self): + calls.append("enter") + return self.entered + + def __exit__(self, exc_type: object, exc: object, tb: object) -> None: + del exc_type, exc, tb + calls.append("exit") + + def close(self) -> None: + calls.append("close") + + module = ModuleType("openneck") + module.OpenNeckController = FakeOpenNeckController # type: ignore[attr-defined] + monkeypatch.setitem(__import__("sys").modules, "openneck", module) + + device = OpenNeckDevice(NeckConfig(enabled=True)) + device.connect() + device.center() + device.move_norm(0.25, -0.5) + device.close() + + assert calls == ["enter", "entered-center-0.5", "entered-move-0.25--0.5", "exit"] + + +def test_parse_neck_config_validates_rate() -> None: + try: + parse_neck_config({"neck": {"enabled": True, "rate_hz": 0}}) + except ValueError as exc: + assert "neck.rate_hz" in str(exc) + else: + raise AssertionError("expected ValueError") + + +def test_parse_neck_config_accepts_scalar_active_mode() -> None: + cfg = parse_neck_config({"neck": {"enabled": True, "active_modes": "mocap"}}) + + assert cfg.active_modes == ("mocap",) + + +def test_parse_neck_config_rejects_unknown_active_mode() -> None: + try: + parse_neck_config({"neck": {"enabled": True, "active_modes": ["mocap", "idle"]}}) + except ValueError as exc: + assert "neck.active_modes" in str(exc) + assert "idle" in str(exc) + else: + raise AssertionError("expected ValueError") + + +def pytest_approx(value: float): + import pytest + + return pytest.approx(value, abs=1e-6) diff --git a/tests/test_sim2real_multiprocess.py b/tests/test_sim2real_multiprocess.py index 72a7d933..58c4b16d 100644 --- a/tests/test_sim2real_multiprocess.py +++ b/tests/test_sim2real_multiprocess.py @@ -81,6 +81,16 @@ def test_sim2real_runtime_rejects_hands_without_pico_provider() -> None: Sim2RealRuntime(cfg) +def test_sim2real_runtime_rejects_neck_without_pico_provider() -> None: + cfg = { + "input": {"provider": "bvh"}, + "runtime": {"shutdown_timeout_s": 0.01}, + "neck": {"enabled": True, "driver": "openneck"}, + } + with pytest.raises(ValueError, match="neck.enabled=true requires input.provider=pico4"): + Sim2RealRuntime(cfg) + + def test_sim2real_runtime_rejects_recording_without_pico_provider() -> None: cfg = { "input": {"provider": "bvh"}, @@ -291,6 +301,79 @@ def Process(self, *, name: str, target: object, args: tuple[object, ...]) -> Fak assert started_names == ["pico_input", "reference", "robot_control", "recording_worker"] +def test_neck_enabled_adds_neck_worker() -> None: + started_names: list[str] = [] + + class FakeProcess: + def __init__(self, *, name: str, target: object, args: tuple[object, ...]) -> None: + del target, args + self.name = name + self.exitcode = 0 + + def start(self) -> None: + started_names.append(self.name) + + class FakeContext: + def Event(self) -> object: + return SimpleNamespace(set=lambda: None, is_set=lambda: False) + + def Process(self, *, name: str, target: object, args: tuple[object, ...]) -> FakeProcess: + return FakeProcess(name=name, target=target, args=args) + + cfg = { + "input": {"provider": "pico4"}, + "runtime": {"shutdown_timeout_s": 0.01}, + "neck": {"enabled": True, "driver": "openneck", "dry_run": True}, + } + runtime = Sim2RealRuntime(cfg) + runtime._ctx = FakeContext() # type: ignore[assignment] + + runtime._start_processes() + + assert started_names == ["pico_input", "reference", "robot_control", "neck_worker"] + + +def test_noncritical_worker_exit_warning_is_not_repeated(monkeypatch, caplog) -> None: + class FakeStopEvent: + def __init__(self) -> None: + self.polls = 0 + self.stopped = False + + def is_set(self) -> bool: + self.polls += 1 + if self.polls >= 4: + self.stopped = True + return self.stopped + + def set(self) -> None: + self.stopped = True + + class FakeProcess: + name = "neck_worker" + exitcode = 1 + + def is_alive(self) -> bool: + return False + + def join(self, timeout: float | None = None) -> None: + del timeout + + cfg = { + "input": {"provider": "pico4"}, + "runtime": {"shutdown_timeout_s": 0.01}, + "neck": {"enabled": True, "driver": "openneck", "dry_run": True}, + } + runtime = Sim2RealRuntime(cfg) + runtime._stop_event = FakeStopEvent() # type: ignore[assignment] + monkeypatch.setattr(runtime, "_start_processes", lambda: runtime._processes.append(FakeProcess())) # type: ignore[arg-type] + + with caplog.at_level(logging.WARNING, logger="teleopit.operator"): + runtime.run() + + warnings = [message for message in caplog.messages if "non-critical worker exited" in message] + assert warnings == ["non-critical worker exited: neck_worker"] + + def test_recording_key_mapping() -> None: assert map_recording_key_to_command("R") == "record_start" assert map_recording_key_to_command("s") == "record_save" From 33a660cb9aa27733a1c395b5e656203779b1b55a Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Tue, 7 Jul 2026 17:17:51 +0800 Subject: [PATCH 05/59] Add OpenNeck diagnostic script --- scripts/dev/test_openneck.py | 220 +++++++++++++++++++++++++++++++++++ 1 file changed, 220 insertions(+) create mode 100644 scripts/dev/test_openneck.py diff --git a/scripts/dev/test_openneck.py b/scripts/dev/test_openneck.py new file mode 100644 index 00000000..d891932f --- /dev/null +++ b/scripts/dev/test_openneck.py @@ -0,0 +1,220 @@ +#!/usr/bin/env python3 +"""Exercise optional OpenNeck active-vision control.""" + +from __future__ import annotations + +import argparse +import logging +from pathlib import Path +import sys +import time + + +REPO_ROOT = Path(__file__).resolve().parents[2] +sys.path.insert(0, str(REPO_ROOT)) + +from teleopit.inputs.pico4_provider import Pico4InputProvider # noqa: E402 +from teleopit.sim2real.neck.config import NeckConfig # noqa: E402 +from teleopit.sim2real.neck.openneck import build_neck_device # noqa: E402 +from teleopit.sim2real.neck.worker import NeckRuntime # noqa: E402 + + +DEFAULT_RATE_HZ = 60.0 +DEFAULT_FRAME_TIMEOUT_S = 0.3 +DEFAULT_STEP_MAGNITUDE = 0.25 +DEFAULT_HOLD_S = 0.8 +DEFAULT_PICO_TIMEOUT_S = 60.0 + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser(description="Test OpenNeck active-vision control") + parser.add_argument( + "--mode", + choices=["direct", "pico"], + default="direct", + help=( + "direct sends a conservative fixed motion pattern to OpenNeck; " + "pico drives OpenNeck from live Pico head/body tracking through Teleopit's active-neck mapper." + ), + ) + parser.add_argument("--port", default=None, help="Optional OpenNeck serial port, for example /dev/ttyACM0") + parser.add_argument("--config", dest="config_path", default=None, help="Optional OpenNeck calibration config path") + parser.add_argument("--dry-run", action="store_true", help="Compute/log commands without opening OpenNeck hardware") + parser.add_argument("--rate-hz", type=float, default=DEFAULT_RATE_HZ) + parser.add_argument("--frame-timeout-s", type=float, default=DEFAULT_FRAME_TIMEOUT_S) + parser.add_argument( + "--magnitude", + type=float, + default=DEFAULT_STEP_MAGNITUDE, + help="Normalized direct-test command magnitude in [0, 1]. Keep this conservative.", + ) + parser.add_argument("--hold-s", type=float, default=DEFAULT_HOLD_S, help="Seconds to hold each direct-test command") + parser.add_argument("--duration-s", type=float, default=0.0, help="Pico mode duration; 0 means until Ctrl-C") + parser.add_argument("--no-center-on-start", action="store_true") + parser.add_argument("--no-center-on-shutdown", action="store_true") + parser.add_argument("--release-on-shutdown", action="store_true") + parser.add_argument("--use-body-reference", action=argparse.BooleanOptionalAction, default=True) + parser.add_argument("--invert-yaw", action=argparse.BooleanOptionalAction, default=True) + parser.add_argument("--invert-pitch", action=argparse.BooleanOptionalAction, default=True) + parser.add_argument("--dead-zone-deg", type=float, default=0.5) + parser.add_argument("--smoothing-alpha", type=float, default=0.35) + parser.add_argument("--yaw-range-deg", type=float, default=90.0) + parser.add_argument("--pitch-range-deg", type=float, default=60.0) + parser.add_argument("--head-joint", default="Head") + parser.add_argument("--body-reference-joint", default="Spine3") + parser.add_argument("--bridge-host", default="0.0.0.0") + parser.add_argument("--bridge-port", type=int, default=63901) + parser.add_argument("--bridge-discovery", action=argparse.BooleanOptionalAction, default=True) + parser.add_argument("--bridge-advertise-ip", default=None) + args = parser.parse_args() + if args.rate_hz <= 0: + raise SystemExit("--rate-hz must be > 0") + if args.frame_timeout_s <= 0: + raise SystemExit("--frame-timeout-s must be > 0") + if args.hold_s <= 0: + raise SystemExit("--hold-s must be > 0") + if args.duration_s < 0: + raise SystemExit("--duration-s must be >= 0") + if not 0.0 <= args.magnitude <= 1.0: + raise SystemExit("--magnitude must be in [0, 1]") + return args + + +def make_neck_config(args: argparse.Namespace) -> NeckConfig: + return NeckConfig( + enabled=True, + driver="openneck", + config_path=args.config_path, + port=args.port, + rate_hz=args.rate_hz, + frame_timeout_s=args.frame_timeout_s, + active_modes=("mocap",), + head_joint=args.head_joint, + body_reference_joint=args.body_reference_joint, + use_body_reference=bool(args.use_body_reference), + dead_zone_deg=args.dead_zone_deg, + smoothing_alpha=args.smoothing_alpha, + yaw_range_deg=args.yaw_range_deg, + pitch_range_deg=args.pitch_range_deg, + invert_yaw=bool(args.invert_yaw), + invert_pitch=bool(args.invert_pitch), + center_on_start=not bool(args.no_center_on_start), + center_on_shutdown=not bool(args.no_center_on_shutdown), + release_on_shutdown=bool(args.release_on_shutdown), + dry_run=bool(args.dry_run), + ) + + +def make_pico_provider(args: argparse.Namespace) -> Pico4InputProvider: + return Pico4InputProvider( + timeout=DEFAULT_PICO_TIMEOUT_S, + pause_button=None, + arms_button=None, + bridge_host=args.bridge_host, + bridge_port=args.bridge_port, + bridge_discovery=bool(args.bridge_discovery), + bridge_advertise_ip=args.bridge_advertise_ip, + bridge_video=None, + bridge_video_enabled=False, + ) + + +def run_direct(args: argparse.Namespace) -> None: + cfg = make_neck_config(args) + device = build_neck_device(cfg) + magnitude = float(args.magnitude) + pattern = [ + ("center", 0.0, 0.0), + ("yaw right", magnitude, 0.0), + ("center", 0.0, 0.0), + ("yaw left", -magnitude, 0.0), + ("center", 0.0, 0.0), + ("pitch up", 0.0, magnitude), + ("center", 0.0, 0.0), + ("pitch down", 0.0, -magnitude), + ("center", 0.0, 0.0), + ] + + print( + f"Testing OpenNeck direct pattern | port={args.port} dry_run={args.dry_run} " + f"magnitude={magnitude:.2f}", + flush=True, + ) + try: + device.connect() + if cfg.center_on_start: + device.center() + for label, yaw, pitch in pattern: + print(f"{label}: yaw={yaw:.3f} pitch={pitch:.3f}", flush=True) + device.move_norm(yaw, pitch) + time.sleep(float(args.hold_s)) + except KeyboardInterrupt: + print("Interrupted; shutting down OpenNeck", flush=True) + finally: + try: + if cfg.center_on_shutdown: + device.center() + if cfg.release_on_shutdown: + device.release() + finally: + device.close() + + +def run_pico(args: argparse.Namespace) -> None: + cfg = make_neck_config(args) + provider = make_pico_provider(args) + runtime = NeckRuntime(cfg) + sleep_s = 1.0 / max(float(args.rate_hz), 1.0) + deadline = time.monotonic() + float(args.duration_s) if args.duration_s > 0.0 else None + last_seq = -1 + command_count = 0 + + print( + "Testing OpenNeck active vision from live Pico body tracking. " + "Hold your head neutral for the first valid body frame; press Ctrl-C to stop.", + flush=True, + ) + try: + runtime.start() + while deadline is None or time.monotonic() < deadline: + now_s = time.monotonic() + if provider.has_frame(): + frame, timestamp_s, seq = provider.get_frame_packet() + if int(seq) != last_seq: + moved = runtime.tick( + frame=frame, + frame_timestamp_s=timestamp_s, + active=True, + now_s=now_s, + ) + if moved: + command_count += 1 + last_seq = int(seq) + age_ms = max((now_s - float(timestamp_s)) * 1000.0, 0.0) + print( + f"pico seq={seq} age={age_ms:.1f}ms moved={moved} commands={command_count}", + flush=True, + ) + else: + runtime.tick(frame=None, frame_timestamp_s=None, active=True, now_s=now_s) + time.sleep(sleep_s) + except KeyboardInterrupt: + print("Interrupted; shutting down OpenNeck", flush=True) + finally: + runtime.close() + provider.close() + + +def main() -> None: + logging.basicConfig(level=logging.INFO, format="%(levelname)s:%(name)s:%(message)s") + args = parse_args() + if args.mode == "direct": + run_direct(args) + elif args.mode == "pico": + run_pico(args) + else: + raise AssertionError(f"Unhandled mode: {args.mode}") + + +if __name__ == "__main__": + main() From a7d51f54f526aae007ca0e68bfd5593f142b3357 Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Tue, 7 Jul 2026 20:47:49 +0800 Subject: [PATCH 06/59] Add OpenNeck config to Pico sim2real --- teleopit/configs/pico4_sim2real.yaml | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/teleopit/configs/pico4_sim2real.yaml b/teleopit/configs/pico4_sim2real.yaml index 9b335e58..20a46229 100644 --- a/teleopit/configs/pico4_sim2real.yaml +++ b/teleopit/configs/pico4_sim2real.yaml @@ -104,6 +104,29 @@ hands: temporal_filter_alpha: 1.0 output_alpha: 1.0 +# Optional OpenNeck active-vision gimbal control. +neck: + enabled: false + driver: openneck + config_path: null + port: null + rate_hz: 60.0 + frame_timeout_s: 0.2 + active_modes: [standing, mocap, arms, pause] + head_joint: Head + body_reference_joint: Spine3 + use_body_reference: true + dead_zone_deg: 0.5 + smoothing_alpha: 0.35 + yaw_range_deg: 90.0 + pitch_range_deg: 60.0 + invert_yaw: true + invert_pitch: true + center_on_start: true + center_on_shutdown: true + release_on_shutdown: false + dry_run: false + # Physical robot SDK configuration real_robot: network_interface: "eth0" From 474223b254d5ac3fa8a1749f39ed811e6485bbce Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Wed, 8 Jul 2026 21:41:36 +0800 Subject: [PATCH 07/59] Add O6 somehand hand-pose support --- AGENTS.md | 4 +- README.md | 2 +- docs/docs/configuration/config-reference.md | 12 +- docs/docs/getting-started/installation.md | 2 +- docs/docs/tutorials/pico-sim2real.md | 27 +++-- .../current/configuration/config-reference.md | 10 +- .../current/getting-started/installation.md | 2 +- .../current/tutorials/pico-sim2real.md | 21 +++- scripts/dev/test_linkerhand_l6.py | 13 +- ..._assets.sh => download_somehand_assets.sh} | 0 teleopit/configs/pico4_sim2real.yaml | 3 +- teleopit/configs/sim2real.yaml | 3 +- teleopit/sim2real/hands/linkerhand_l6.py | 110 +++++++++++++---- teleopit/sim2real/hands/linkerhand_o6.py | 62 +++++++++- tests/test_dexterous_hand.py | 113 +++++++++++++++++- 15 files changed, 315 insertions(+), 69 deletions(-) rename scripts/setup/{download_somehand_l6_assets.sh => download_somehand_assets.sh} (100%) diff --git a/AGENTS.md b/AGENTS.md index f2a82862..814ff8ea 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -155,8 +155,8 @@ target_dof_pos = clip(action, -10, 10) × action_scale + default_dof_pos - `gripper` mode reuses `Pico4InputProvider.get_controller_snapshot()` for Pico grip/trigger open-close control and supports LinkerHand L6 and O6 - `vr_hand_pose` mode reuses `Pico4InputProvider.get_hand_snapshot()` and somehand 0.2.0 public `somehand.api` for continuous Pico hand-pose retargeting; do not start a second `PicoBridge` for hand control - Teleopit owns Pico 26-joint hand-state to 21-landmark conversion; do not import `somehand.pico_input` -- LinkerHand O6 supports only `hands.mode=gripper`; its default `close_pose` is `[86, 73, 118, 111, 110, 111]` -- L6 `gripper` mode uses the configured `hands.linkerhand_l6.speed` (default `[50]*6`); O6 `gripper` mode uses `hands.linkerhand_o6.speed` (default `[255]*6`); `vr_hand_pose` always sets LinkerHand L6 speed to `[255]*6` +- LinkerHand O6 supports `hands.mode=gripper|vr_hand_pose`; its default `close_pose` is `[86, 73, 118, 111, 110, 111]` +- L6 `gripper` mode uses the configured `hands.linkerhand_l6.speed` (default `[50]*6`); O6 `gripper` mode uses `hands.linkerhand_o6.speed` (default `[255]*6`); `vr_hand_pose` always sets LinkerHand L6/O6 speed to `[255]*6` - `vr_hand_pose` defaults to a low-latency somehand path: `hands.somehand.rate_hz=60`, `max_iterations=12`, `temporal_filter_alpha=1.0`, and `output_alpha=1.0`; this prioritizes response speed over smoothing - LinkerHand control is active in all sim2real modes when `hands.enabled=true`; shutdown and hand-runtime failure must send the configured open pose - In `vr_hand_pose` mode, missing/inactive hand pose holds the last commanded pose for that side instead of opening the hand diff --git a/README.md b/README.md index 81d1da0d..67f8b865 100644 --- a/README.md +++ b/README.md @@ -134,7 +134,7 @@ Full docs at **[BotRunner64.github.io/Teleopit](https://BotRunner64.github.io/Te ### v0.4.0 (2026-06-25) - Improved Pico realtime control with pico-bridge 0.2.1, `ARMS` mode, armed sim2real mocap entry, and retargeter-preserving pause/arms resets. -- Added optional LinkerHand L6/O6 sim2real control, including Pico gripper input and low-latency L6 `vr_hand_pose`. +- Added optional LinkerHand L6/O6 sim2real control, including Pico gripper input and low-latency L6/O6 `vr_hand_pose`. - Added manual Pico sim2real HDF5 recording and an interactive Pico motion recorder for training NPZ clips. - Refined the training data path with minimal HDF5 shards, explicit precompute, rewind sampling, and updated tracking rewards. diff --git a/docs/docs/configuration/config-reference.md b/docs/docs/configuration/config-reference.md index e1b938a5..ba72d4ad 100644 --- a/docs/docs/configuration/config-reference.md +++ b/docs/docs/configuration/config-reference.md @@ -128,9 +128,10 @@ installs of `third_party/linkerhand-python-sdk` and `third_party/somehand`. When enabled, hand control remains active in all sim2real modes. `gripper` supports `linkerhand_l6` and `linkerhand_o6` by interpolating Pico trigger input between the configured open and close poses. `vr_hand_pose` is -L6-only: missing hand pose holds the last command for that side, L6 speed is -set to the maximum, and Teleopit converts Pico hand state to 21 landmarks before -calling somehand 0.2.0 through `somehand.api` only. +supported by `linkerhand_l6` and `linkerhand_o6`: missing hand pose holds the +last command for that side, the selected hand speed is set to the maximum, and +Teleopit converts Pico hand state to 21 landmarks before calling somehand 0.2.0 +through `somehand.api` only. | Field | Description | Default | |-------|-------------|---------| @@ -144,9 +145,10 @@ calling somehand 0.2.0 through `somehand.api` only. | `hands.linkerhand_l6.speed` | L6 speed used by `gripper`; `vr_hand_pose` overrides this to maximum speed | see config | | `hands.linkerhand_l6.open_pose` / `close_pose` | Six-value L6 open/closed poses | see config | | `hands.linkerhand_o6.left_can` / `right_can` | CAN channels for each O6 hand | `can0` / `can1` | -| `hands.linkerhand_o6.speed` | O6 speed used by `gripper` | see config | +| `hands.linkerhand_o6.speed` | O6 speed used by `gripper`; `vr_hand_pose` overrides this to maximum speed | see config | | `hands.linkerhand_o6.open_pose` / `close_pose` | Six-value O6 open/closed poses | see config | -| `hands.somehand.config_path` | Official somehand 0.2.0 bi-hand L6 config used by `vr_hand_pose` | see config | +| `hands.somehand.l6_config_path` | Official somehand 0.2.0 bi-hand L6 config used by L6 `vr_hand_pose` | see config | +| `hands.somehand.o6_config_path` | Official somehand 0.2.0 bi-hand O6 config used by O6 `vr_hand_pose` | see config | | `hands.somehand.rate_hz` | Low-latency `vr_hand_pose` command rate in Hz | `60.0` | | `hands.somehand.max_iterations` | somehand solver iteration cap for `vr_hand_pose` | `12` | | `hands.somehand.temporal_filter_alpha` | somehand input landmark smoothing alpha; `1.0` disables smoothing delay | `1.0` | diff --git a/docs/docs/getting-started/installation.md b/docs/docs/getting-started/installation.md index 0de1e998..a0a8f189 100644 --- a/docs/docs/getting-started/installation.md +++ b/docs/docs/getting-started/installation.md @@ -68,7 +68,7 @@ Install those packages directly after initializing the submodules: git submodule update --init --recursive pip install -e third_party/linkerhand-python-sdk pip install -e third_party/somehand -scripts/setup/download_somehand_l6_assets.sh +scripts/setup/download_somehand_assets.sh ``` These packages are only required when `hands.enabled=true`. diff --git a/docs/docs/tutorials/pico-sim2real.md b/docs/docs/tutorials/pico-sim2real.md index ff9395bc..0943d09e 100644 --- a/docs/docs/tutorials/pico-sim2real.md +++ b/docs/docs/tutorials/pico-sim2real.md @@ -183,11 +183,11 @@ Pico sim2real can drive LinkerHand hands from Pico input: trigger closes that hand. This mode supports `hands.driver=linkerhand_l6` and `hands.driver=linkerhand_o6`; speed and open/close poses come from the matching driver config. -- `vr_hand_pose`: L6-only mode that retargets Pico hand pose through somehand and - commands the continuous L6 hand target. If a hand pose disappears, that side - keeps its last commanded pose. This mode uses Teleopit's Pico landmark adapter - and the public `somehand.api` from somehand 0.2.0. It always sets L6 speed to - the maximum. +- `vr_hand_pose`: retargets Pico hand pose through somehand and commands the + continuous L6 or O6 hand target. If a hand pose disappears, that side keeps its + last commanded pose. This mode uses Teleopit's Pico landmark adapter and the + public `somehand.api` from somehand 0.2.0. It always sets the selected hand + speed to the maximum. When `hands.enabled=true`, hand control remains active in all sim2real modes. Shutdown and hand-runtime failure send the configured open pose. @@ -199,7 +199,7 @@ the main Pico profile: git submodule update --init --recursive pip install -e third_party/linkerhand-python-sdk pip install -e third_party/somehand -scripts/setup/download_somehand_l6_assets.sh +scripts/setup/download_somehand_assets.sh ``` Bring up the CAN interfaces before testing or running hand control: @@ -229,7 +229,8 @@ python scripts/dev/test_linkerhand_l6.py \ --right-can can1 ``` -To test O6 with live Pico gripper input, add `--mode gripper`. +To test O6 with live Pico gripper input, add `--mode gripper`. To test O6 with +live Pico hand-pose retargeting, add `--mode vr_hand_pose`. Then enable L6 gripper control in Pico sim2real: @@ -251,7 +252,7 @@ hands.linkerhand_o6.left_can=can0 hands.linkerhand_o6.right_can=can1 ``` -For continuous VR hand-pose control, use: +For continuous L6 VR hand-pose control, use: ```bash hands.enabled=true @@ -261,6 +262,16 @@ hands.linkerhand_l6.left_can=can0 hands.linkerhand_l6.right_can=can1 ``` +For continuous O6 VR hand-pose control, switch the driver and CAN keys: + +```bash +hands.enabled=true +hands.driver=linkerhand_o6 +hands.mode=vr_hand_pose +hands.linkerhand_o6.left_can=can0 +hands.linkerhand_o6.right_can=can1 +``` + ## Optional RealSense Preview Stream the G1 RealSense color camera back to the Pico headset: diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md index 2244dd77..9bdd4889 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md @@ -145,8 +145,9 @@ MuJoCo 窗口显示重定向参考；`sim2sim`、`mocap`、`camera` 和 `all` `hands.enabled=true` 要求 `input.provider=pico4`，并以本地 editable 方式安装 `third_party/linkerhand-python-sdk` 和 `third_party/somehand`。启用后，手控会在所有 sim2real 模式中保持生效。 `gripper` 支持 `linkerhand_l6` 和 `linkerhand_o6`，会用 Pico trigger 在配置的张开和闭合姿态之间插值。 -`vr_hand_pose` 只支持 L6：手部 pose 消失时，对应侧会保持上一条命令；L6 速度会设为最大值； -Teleopit 会先将 Pico 手部状态转成 21 个 landmarks，再只通过 somehand 0.2.0 公开的 `somehand.api` 调用。 +`vr_hand_pose` 支持 `linkerhand_l6` 和 `linkerhand_o6`：手部 pose 消失时，对应侧会保持上一条命令； +所选手的速度会设为最大值；Teleopit 会先将 Pico 手部状态转成 21 个 landmarks， +再只通过 somehand 0.2.0 公开的 `somehand.api` 调用。 | 字段 | 说明 | 默认值 | |---|---|---| @@ -162,9 +163,10 @@ Teleopit 会先将 Pico 手部状态转成 21 个 landmarks，再只通过 someh | `hands.linkerhand_l6.trigger_deadzone` | trigger 两端死区 | `0.05` | | `hands.linkerhand_l6.open_pose` / `close_pose` | L6 的 6 维张开/闭合姿态 | 见配置 | | `hands.linkerhand_o6.left_can` / `right_can` | 左右 O6 手 CAN 通道 | `can0` / `can1` | -| `hands.linkerhand_o6.speed` | `gripper` 使用的 O6 速度 | 见配置 | +| `hands.linkerhand_o6.speed` | `gripper` 使用的 O6 速度；`vr_hand_pose` 会覆盖为最大速度 | 见配置 | | `hands.linkerhand_o6.open_pose` / `close_pose` | O6 的 6 维张开/闭合姿态 | 见配置 | -| `hands.somehand.config_path` | `vr_hand_pose` 使用的 somehand 双手 L6 配置 | 见配置 | +| `hands.somehand.l6_config_path` | L6 `vr_hand_pose` 使用的 somehand 双手 L6 配置 | 见配置 | +| `hands.somehand.o6_config_path` | O6 `vr_hand_pose` 使用的 somehand 双手 O6 配置 | 见配置 | | `hands.somehand.rate_hz` | 低延时 `vr_hand_pose` 命令频率（Hz） | `60.0` | | `hands.somehand.max_iterations` | `vr_hand_pose` 的 somehand solver 迭代上限 | `12` | | `hands.somehand.temporal_filter_alpha` | somehand 输入 landmarks 平滑 alpha；`1.0` 表示关闭平滑延时 | `1.0` | diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/installation.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/installation.md index 3220900e..6cb001ec 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/installation.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/installation.md @@ -68,7 +68,7 @@ submodule 后，直接安装这些包： git submodule update --init --recursive pip install -e third_party/linkerhand-python-sdk pip install -e third_party/somehand -scripts/setup/download_somehand_l6_assets.sh +scripts/setup/download_somehand_assets.sh ``` 只有在 `hands.enabled=true` 时才需要安装这些包。 diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md index de155bcb..af46feed 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md @@ -172,9 +172,9 @@ Pico sim2real 可以用 Pico 输入控制 LinkerHand： - `gripper`：按住同侧 grip 作为 deadman，同侧 trigger 控制对应手闭合。 该模式支持 `hands.driver=linkerhand_l6` 和 `hands.driver=linkerhand_o6`； 速度和张开/闭合姿态来自对应 driver 配置。 -- `vr_hand_pose`：只支持 L6，通过 somehand 重定向 Pico 手部 pose，并下发连续 L6 手部目标。 +- `vr_hand_pose`：通过 somehand 重定向 Pico 手部 pose，并下发连续 L6 或 O6 手部目标。 如果某侧手部 pose 消失，该侧会保持上一条手势命令。这个模式使用 Teleopit 的 - Pico landmark 适配器和 somehand 0.2.0 公开的 `somehand.api`，并始终将 L6 + Pico landmark 适配器和 somehand 0.2.0 公开的 `somehand.api`，并始终将所选手的 速度设为最大值。默认配置使用 60 Hz 的低延时 somehand 路径并减少平滑，所以响应会更快， 但可能比标准 somehand 设置更抖。 @@ -186,7 +186,7 @@ Pico sim2real 可以用 Pico 输入控制 LinkerHand： git submodule update --init --recursive pip install -e third_party/linkerhand-python-sdk pip install -e third_party/somehand -scripts/setup/download_somehand_l6_assets.sh +scripts/setup/download_somehand_assets.sh ``` 测试或运行手控前，先开启 CAN 接口： @@ -215,7 +215,8 @@ python scripts/dev/test_linkerhand_l6.py \ --right-can can1 ``` -如果要用实时 Pico gripper 输入测试 O6，再加 `--mode gripper`。 +如果要用实时 Pico gripper 输入测试 O6，再加 `--mode gripper`。如果要用实时 Pico +手部 pose 重定向测试 O6，再加 `--mode vr_hand_pose`。 然后在 Pico sim2real 中启用 L6 gripper 控制： @@ -237,7 +238,7 @@ hands.linkerhand_o6.left_can=can0 hands.linkerhand_o6.right_can=can1 ``` -连续 VR 手部 pose 控制使用： +连续 L6 VR 手部 pose 控制使用： ```bash hands.enabled=true @@ -247,6 +248,16 @@ hands.linkerhand_l6.left_can=can0 hands.linkerhand_l6.right_can=can1 ``` +连续 O6 VR 手部 pose 控制切换 driver 和 CAN 配置键： + +```bash +hands.enabled=true +hands.driver=linkerhand_o6 +hands.mode=vr_hand_pose +hands.linkerhand_o6.left_can=can0 +hands.linkerhand_o6.right_can=can1 +``` + ## 可选 RealSense 预览 将 G1 RealSense 彩色相机推送回 Pico 头显： diff --git a/scripts/dev/test_linkerhand_l6.py b/scripts/dev/test_linkerhand_l6.py index 3b40d58c..4a854d58 100644 --- a/scripts/dev/test_linkerhand_l6.py +++ b/scripts/dev/test_linkerhand_l6.py @@ -34,7 +34,8 @@ O6_OPEN_POSE = [250, 250, 250, 250, 250, 250] O6_CLOSE_POSE = [86, 73, 118, 111, 110, 111] O6_DEFAULT_SPEED = [255, 255, 255, 255, 255, 255] -DEFAULT_SOMEHAND_CONFIG_PATH = "third_party/somehand/configs/retargeting/bihand/linkerhand_l6_bihand.yaml" +DEFAULT_L6_SOMEHAND_CONFIG_PATH = "third_party/somehand/configs/retargeting/bihand/linkerhand_l6_bihand.yaml" +DEFAULT_O6_SOMEHAND_CONFIG_PATH = "third_party/somehand/configs/retargeting/bihand/linkerhand_o6_bihand.yaml" OPEN_CLOSE_HOLD_S = 1.0 GRIPPER_RATE_HZ = 30.0 VR_HAND_POSE_RATE_HZ = 60.0 @@ -56,7 +57,7 @@ def parse_args() -> argparse.Namespace: "--driver", choices=["linkerhand_l6", "linkerhand_o6"], default="linkerhand_l6", - help="Hand driver to test. O6 currently supports open_close and gripper only.", + help="Hand driver to test.", ) parser.add_argument( "--mode", @@ -77,8 +78,6 @@ def parse_args() -> argparse.Namespace: help='RS485 serial port such as /dev/ttyUSB0; "None" uses CAN', ) args = parser.parse_args() - if args.driver == "linkerhand_o6" and args.mode == "vr_hand_pose": - raise SystemExit("hands.driver=linkerhand_o6 supports only --mode open_close or gripper") args.speed = list(O6_DEFAULT_SPEED if args.driver == "linkerhand_o6" else DEFAULT_SPEED) args.open_pose = list(O6_OPEN_POSE if args.driver == "linkerhand_o6" else OPEN_POSE) args.close_pose = list(O6_CLOSE_POSE if args.driver == "linkerhand_o6" else CLOSE_POSE) @@ -113,7 +112,8 @@ def make_config(args: argparse.Namespace, *, mode: str) -> dict[str, object]: "frame_timeout_s": FRAME_TIMEOUT_S, driver_section: driver_cfg, "somehand": { - "config_path": DEFAULT_SOMEHAND_CONFIG_PATH, + "l6_config_path": DEFAULT_L6_SOMEHAND_CONFIG_PATH, + "o6_config_path": DEFAULT_O6_SOMEHAND_CONFIG_PATH, "rate_hz": VR_HAND_POSE_RATE_HZ, "max_iterations": 12, "temporal_filter_alpha": 1.0, @@ -260,9 +260,6 @@ def run_gripper(args: argparse.Namespace) -> None: def run_vr_hand_pose(args: argparse.Namespace) -> None: - if args.hand_type != "both": - raise SystemExit("hands.mode=vr_hand_pose currently requires --hand-type both") - config = make_config(args, mode="vr_hand_pose") provider = make_pico_provider() device, mapper = build_driver_runtime(config, driver=args.driver) diff --git a/scripts/setup/download_somehand_l6_assets.sh b/scripts/setup/download_somehand_assets.sh similarity index 100% rename from scripts/setup/download_somehand_l6_assets.sh rename to scripts/setup/download_somehand_assets.sh diff --git a/teleopit/configs/pico4_sim2real.yaml b/teleopit/configs/pico4_sim2real.yaml index 20a46229..cce998ca 100644 --- a/teleopit/configs/pico4_sim2real.yaml +++ b/teleopit/configs/pico4_sim2real.yaml @@ -97,7 +97,8 @@ hands: close_pose: [86, 73, 118, 111, 110, 111] print_input: false somehand: - config_path: third_party/somehand/configs/retargeting/bihand/linkerhand_l6_bihand.yaml + l6_config_path: third_party/somehand/configs/retargeting/bihand/linkerhand_l6_bihand.yaml + o6_config_path: third_party/somehand/configs/retargeting/bihand/linkerhand_o6_bihand.yaml # Low-latency vr_hand_pose path. This favors response speed over smoothing. rate_hz: 60.0 max_iterations: 12 diff --git a/teleopit/configs/sim2real.yaml b/teleopit/configs/sim2real.yaml index cc67b688..5cf15d94 100644 --- a/teleopit/configs/sim2real.yaml +++ b/teleopit/configs/sim2real.yaml @@ -98,7 +98,8 @@ hands: close_pose: [86, 73, 118, 111, 110, 111] print_input: false somehand: - config_path: third_party/somehand/configs/retargeting/bihand/linkerhand_l6_bihand.yaml + l6_config_path: third_party/somehand/configs/retargeting/bihand/linkerhand_l6_bihand.yaml + o6_config_path: third_party/somehand/configs/retargeting/bihand/linkerhand_o6_bihand.yaml # Low-latency vr_hand_pose path. This favors response speed over smoothing. rate_hz: 60.0 max_iterations: 12 diff --git a/teleopit/sim2real/hands/linkerhand_l6.py b/teleopit/sim2real/hands/linkerhand_l6.py index d7ed00be..1525fd03 100644 --- a/teleopit/sim2real/hands/linkerhand_l6.py +++ b/teleopit/sim2real/hands/linkerhand_l6.py @@ -89,7 +89,9 @@ def parse_linkerhand_l6_config(cfg: Any) -> LinkerHandL6Config: close_pose=tuple(close_pose), fixed_thumb_yaw=thumb_yaw, print_input=bool(cfg_get(l6_cfg, "print_input", False)), - somehand_config_path=str(cfg_get(somehand_cfg, "config_path", DEFAULT_SOMEHAND_CONFIG)), + somehand_config_path=str( + cfg_get(somehand_cfg, "l6_config_path", cfg_get(somehand_cfg, "config_path", DEFAULT_SOMEHAND_CONFIG)) + ), somehand_rate_hz=_positive_float(cfg_get(somehand_cfg, "rate_hz", cfg_get(somehand_cfg, "rate", 60.0)), "somehand.rate_hz"), somehand_max_iterations=_optional_positive_int(cfg_get(somehand_cfg, "max_iterations", None), "somehand.max_iterations"), somehand_temporal_filter_alpha=_optional_alpha(cfg_get(somehand_cfg, "temporal_filter_alpha", None), "somehand.temporal_filter_alpha"), @@ -204,13 +206,24 @@ def close(self) -> None: pass -class SomehandL6Mapper(HandInputMapper): - def __init__(self, config: LinkerHandL6Config): +class SomehandRetargetMapper(HandInputMapper): + def __init__( + self, + config: Any, + *, + family: str, + joint_order: Sequence[str], + config_path: str, + config_label: str, + ): self.config = config - self._engine: Any | None = None + self.family = family.upper() + self.joint_order = tuple(joint_order) + self.config_path = config_path + self.config_label = config_label + self._engine: dict[str, Any] = {} self._hand_frame_cls: Any | None = None - self._bihand_frame_cls: Any | None = None - self._mappers: dict[str, L6RetargetPoseMapper] = {} + self._mappers: dict[str, RetargetPoseMapper] = {} self._next_tick_s = 0.0 self._active = False @@ -218,19 +231,26 @@ def start(self) -> None: _require_somehand_020() from somehand.api import HandFrame, RetargetingEngine, load_bihand_config, load_retargeting_config - config_path = _resolve_project_path(self.config.somehand_config_path) + config_path = _resolve_project_path(self.config_path) if not config_path.exists(): - raise FileNotFoundError(f"somehand L6 config not found: {config_path}") + raise FileNotFoundError(f"{self.config_label} not found: {config_path}") bihand_config = load_bihand_config(str(config_path)) self._engine = {} - for side, path in (("left", bihand_config.left_config_path), ("right", bihand_config.right_config_path)): + self._mappers = {} + config_paths = {"left": bihand_config.left_config_path, "right": bihand_config.right_config_path} + for side in self.config.sides: + path = config_paths[side] retarget_cfg = load_retargeting_config(path) self._apply_low_latency_overrides(retarget_cfg) self._engine[side] = RetargetingEngine(retarget_cfg) self._hand_frame_cls = HandFrame for side, engine in self._engine.items(): - if side in self.config.sides: - self._mappers[side] = L6RetargetPoseMapper(getattr(engine, "hand_model", None), side=side) + self._mappers[side] = RetargetPoseMapper( + getattr(engine, "hand_model", None), + side=side, + family=self.family, + joint_order=self.joint_order, + ) def map(self, *, controller_snapshot: object | None, hand_snapshot: object | None, active: bool, now_s: float) -> tuple[HandPoseCommand, ...]: del controller_snapshot @@ -277,21 +297,42 @@ def _apply_low_latency_overrides(self, cfg: object) -> None: cfg.preprocess.temporal_filter_alpha = float(self.config.somehand_temporal_filter_alpha) +class SomehandL6Mapper(SomehandRetargetMapper): + def __init__(self, config: LinkerHandL6Config): + super().__init__( + config, + family="L6", + joint_order=L6_SDK_JOINT_ORDER, + config_path=config.somehand_config_path, + config_label="somehand L6 config", + ) + + class L6RetargetPoseMapper: def __init__(self, hand_model: Any | None, *, side: str): + self._delegate = RetargetPoseMapper(hand_model, side=side, family="L6", joint_order=L6_SDK_JOINT_ORDER) + + def qpos_to_pose(self, qpos: object) -> list[int]: + return self._delegate.qpos_to_pose(qpos) + + +class RetargetPoseMapper: + def __init__(self, hand_model: Any | None, *, side: str, family: str, joint_order: Sequence[str]): if hand_model is None: - raise ValueError("somehand L6 hand model is missing") + raise ValueError(f"somehand {family} hand model is missing") get_index = getattr(hand_model, "get_joint_name_to_qpos_index", None) if not callable(get_index): - raise ValueError("somehand L6 hand model does not expose get_joint_name_to_qpos_index()") + raise ValueError(f"somehand {family} hand model does not expose get_joint_name_to_qpos_index()") joint_index = get_index() - self._indices = np.asarray([_resolve_l6_joint_index(joint_index, name, side=side) for name in L6_SDK_JOINT_ORDER], dtype=np.int64) + self.family = family.upper() + self._indices = np.asarray([_resolve_joint_index(joint_index, name, side=side, family=self.family) for name in joint_order], dtype=np.int64) mapping = _load_linkerhand_mapping_module() side_key = "l" if side == "left" else "r" self._mapping = mapping - self._arc_min = np.asarray(getattr(mapping, f"l6_{side_key}_min"), dtype=np.float64) - self._arc_max = np.asarray(getattr(mapping, f"l6_{side_key}_max"), dtype=np.float64) - self._direction = np.asarray(getattr(mapping, f"l6_{side_key}_derict"), dtype=np.int8) + mapping_prefix = self.family.lower() + self._arc_min = np.asarray(getattr(mapping, f"{mapping_prefix}_{side_key}_min"), dtype=np.float64) + self._arc_max = np.asarray(getattr(mapping, f"{mapping_prefix}_{side_key}_max"), dtype=np.float64) + self._direction = np.asarray(getattr(mapping, f"{mapping_prefix}_{side_key}_derict"), dtype=np.int8) def qpos_to_pose(self, qpos: object) -> list[int]: values = np.asarray(qpos, dtype=np.float64).reshape(-1) @@ -303,7 +344,7 @@ def qpos_to_pose(self, qpos: object) -> list[int]: scaled = self._mapping.scale_value(arc, float(self._arc_min[index]), float(self._arc_max[index]), 255.0, 0.0) else: scaled = self._mapping.scale_value(arc, float(self._arc_min[index]), float(self._arc_max[index]), 0.0, 255.0) - pose.append(_uint8(round(float(scaled)), "somehand.pose")) + pose.append(_retarget_uint8(round(float(scaled)), "somehand.pose")) return pose @@ -342,26 +383,42 @@ def _require_somehand_020() -> None: def _resolve_l6_joint_index(joint_index: dict[str, int], semantic_name: str, *, side: str) -> int: - for candidate in _l6_joint_candidates(semantic_name, side=side): + return _resolve_joint_index(joint_index, semantic_name, side=side, family="L6") + + +def _resolve_joint_index(joint_index: dict[str, int], semantic_name: str, *, side: str, family: str) -> int: + for candidate in _joint_candidates(semantic_name, side=side, family=family): if candidate in joint_index: return int(joint_index[candidate]) - suffixes = tuple(f"_{alias}" for alias in _l6_aliases(semantic_name)) + aliases = _joint_aliases(semantic_name, family=family) + suffixes = tuple(f"_{alias}" for alias in aliases) for name, index in joint_index.items(): - if name in _l6_aliases(semantic_name) or any(name.endswith(suffix) for suffix in suffixes): + if name in aliases or any(name.endswith(suffix) for suffix in suffixes): return int(index) - raise ValueError(f"Cannot resolve LinkerHand L6 SDK joint {semantic_name!r} in somehand hand model") + raise ValueError(f"Cannot resolve LinkerHand {family} SDK joint {semantic_name!r} in somehand hand model") def _l6_joint_candidates(semantic_name: str, *, side: str) -> tuple[str, ...]: + return _joint_candidates(semantic_name, side=side, family="L6") + + +def _joint_candidates(semantic_name: str, *, side: str, family: str) -> tuple[str, ...]: prefixes = ("", f"{side}_", f"{side[0]}_", f"{side[0].upper()}_", f"{'lh' if side == 'left' else 'rh'}_") - return tuple(f"{prefix}{alias}" for alias in _l6_aliases(semantic_name) for prefix in prefixes) + return tuple(f"{prefix}{alias}" for alias in _joint_aliases(semantic_name, family=family) for prefix in prefixes) def _l6_aliases(semantic_name: str) -> tuple[str, ...]: + return _joint_aliases(semantic_name, family="L6") + + +def _joint_aliases(semantic_name: str, *, family: str) -> tuple[str, ...]: + del family if semantic_name == "thumb_cmc_pitch": return ("thumb_cmc_pitch", "thumb_pitch") if semantic_name == "thumb_cmc_roll": return ("thumb_cmc_roll", "thumb_roll") + if semantic_name == "thumb_cmc_yaw": + return ("thumb_cmc_yaw", "thumb_yaw") aliases = [semantic_name] if semantic_name.endswith("_mcp_pitch"): finger = semantic_name[: -len("_mcp_pitch")] @@ -407,6 +464,13 @@ def _uint8(value: object, field_name: str) -> int: return parsed +def _retarget_uint8(value: object, field_name: str) -> int: + parsed = int(value) + if parsed < 0 or parsed > 255: + raise ValueError(f"{field_name} must be in 0-255, got {value!r}") + return parsed + + def _pose_values(value: object, field_name: str) -> list[int]: parsed = [_uint8(item, field_name) for item in value] # type: ignore[union-attr] if len(parsed) != 6: diff --git a/teleopit/sim2real/hands/linkerhand_o6.py b/teleopit/sim2real/hands/linkerhand_o6.py index 6d4de349..2697d605 100644 --- a/teleopit/sim2real/hands/linkerhand_o6.py +++ b/teleopit/sim2real/hands/linkerhand_o6.py @@ -6,13 +6,28 @@ from teleopit.runtime.common import cfg_get from teleopit.sim2real.hands.base import HAND_SIDES, HandDevice, HandInputMapper -from teleopit.sim2real.hands.linkerhand_l6 import GripperMapper +from teleopit.sim2real.hands.linkerhand_l6 import ( + GripperMapper, + SomehandRetargetMapper, + _optional_alpha, + _optional_positive_int, +) logger = logging.getLogger(__name__) +DEFAULT_SOMEHAND_CONFIG = "third_party/somehand/configs/retargeting/bihand/linkerhand_o6_bihand.yaml" OPEN_POSE = (250, 250, 250, 250, 250, 250) CLOSE_POSE = (86, 73, 118, 111, 110, 111) DEFAULT_SPEED = (255, 255, 255, 255, 255, 255) +VR_HAND_POSE_SPEED = (255, 255, 255, 255, 255, 255) +O6_SDK_JOINT_ORDER = ( + "thumb_cmc_pitch", + "thumb_cmc_yaw", + "index_mcp_pitch", + "middle_mcp_pitch", + "ring_mcp_pitch", + "pinky_mcp_pitch", +) @dataclass(frozen=True) @@ -31,17 +46,28 @@ class LinkerHandO6Config: close_pose: tuple[int, ...] fixed_thumb_yaw: int | None print_input: bool + somehand_config_path: str + somehand_rate_hz: float + somehand_max_iterations: int | None + somehand_temporal_filter_alpha: float | None + somehand_output_alpha: float | None def parse_linkerhand_o6_config(cfg: Any) -> LinkerHandO6Config: hands_cfg = cfg_get(cfg, "hands", {}) or {} o6_cfg = cfg_get(hands_cfg, "linkerhand_o6", {}) or {} + somehand_cfg = cfg_get(hands_cfg, "somehand", {}) or {} mode = str(cfg_get(hands_cfg, "mode", "gripper")).strip().lower() - if mode != "gripper": - raise ValueError(f"hands.driver=linkerhand_o6 supports only hands.mode=gripper, got {mode!r}") + if mode not in ("gripper", "vr_hand_pose"): + raise ValueError(f"hands.mode must be gripper or vr_hand_pose, got {mode!r}") sides = tuple(str(side).strip().lower() for side in cfg_get(hands_cfg, "sides", HAND_SIDES)) if not sides or any(side not in HAND_SIDES for side in sides): raise ValueError("hands.sides must contain left, right, or both sides") + speed = ( + VR_HAND_POSE_SPEED + if mode == "vr_hand_pose" + else tuple(_pose_values(cfg_get(o6_cfg, "speed", DEFAULT_SPEED), "speed")) + ) return LinkerHandO6Config( mode=mode, sides=sides, @@ -52,11 +78,25 @@ def parse_linkerhand_o6_config(cfg: Any) -> LinkerHandO6Config: frame_timeout_s=_positive_float(cfg_get(hands_cfg, "frame_timeout_s", 0.3), "frame_timeout_s"), trigger_deadzone=_deadzone(cfg_get(o6_cfg, "trigger_deadzone", 0.05)), deadman_threshold=_threshold(cfg_get(o6_cfg, "deadman_threshold", 0.5)), - speed=tuple(_pose_values(cfg_get(o6_cfg, "speed", DEFAULT_SPEED), "speed")), + speed=tuple(speed), open_pose=tuple(_pose_values(cfg_get(o6_cfg, "open_pose", OPEN_POSE), "open_pose")), close_pose=tuple(_pose_values(cfg_get(o6_cfg, "close_pose", CLOSE_POSE), "close_pose")), fixed_thumb_yaw=None, print_input=bool(cfg_get(o6_cfg, "print_input", False)), + somehand_config_path=str(cfg_get(somehand_cfg, "o6_config_path", DEFAULT_SOMEHAND_CONFIG)), + somehand_rate_hz=_positive_float( + cfg_get(somehand_cfg, "rate_hz", cfg_get(somehand_cfg, "rate", 60.0)), + "somehand.rate_hz", + ), + somehand_max_iterations=_optional_positive_int( + cfg_get(somehand_cfg, "max_iterations", None), + "somehand.max_iterations", + ), + somehand_temporal_filter_alpha=_optional_alpha( + cfg_get(somehand_cfg, "temporal_filter_alpha", None), + "somehand.temporal_filter_alpha", + ), + somehand_output_alpha=_optional_alpha(cfg_get(somehand_cfg, "output_alpha", None), "somehand.output_alpha"), ) @@ -125,7 +165,19 @@ def close(self) -> None: def build_linkerhand_o6(cfg: Any) -> tuple[HandDevice, HandInputMapper]: config = parse_linkerhand_o6_config(cfg) - return LinkerHandO6Device(config), GripperMapper(config) + mapper: HandInputMapper = SomehandO6Mapper(config) if config.mode == "vr_hand_pose" else GripperMapper(config) + return LinkerHandO6Device(config), mapper + + +class SomehandO6Mapper(SomehandRetargetMapper): + def __init__(self, config: LinkerHandO6Config): + super().__init__( + config, + family="O6", + joint_order=O6_SDK_JOINT_ORDER, + config_path=config.somehand_config_path, + config_label="somehand O6 config", + ) def _uint8(value: object, field_name: str) -> int: diff --git a/tests/test_dexterous_hand.py b/tests/test_dexterous_hand.py index 53aad28a..70c1588d 100644 --- a/tests/test_dexterous_hand.py +++ b/tests/test_dexterous_hand.py @@ -1,7 +1,7 @@ from __future__ import annotations import sys -from types import SimpleNamespace +from types import ModuleType, SimpleNamespace import numpy as np import pytest @@ -10,6 +10,7 @@ from teleopit.sim2real.hands.linkerhand_l6 import ( GripperMapper, LinkerHandL6Device, + RetargetPoseMapper, SomehandL6Mapper, parse_linkerhand_l6_config, trigger_to_pose, @@ -17,7 +18,10 @@ from teleopit.sim2real.hands.base import HandPoseCommand from teleopit.sim2real.hands.linkerhand_o6 import ( CLOSE_POSE as O6_CLOSE_POSE, + DEFAULT_SOMEHAND_CONFIG as O6_DEFAULT_SOMEHAND_CONFIG, LinkerHandO6Device, + O6_SDK_JOINT_ORDER, + SomehandO6Mapper, parse_linkerhand_o6_config, ) from teleopit.sim2real.hands.pico_landmarks import pico_hand_to_landmarks @@ -106,6 +110,12 @@ def _o6_cfg(mode: str = "gripper") -> dict[str, object]: "trigger_deadzone": 0.05, "deadman_threshold": 0.5, }, + "somehand": { + "rate_hz": 60.0, + "max_iterations": 12, + "temporal_filter_alpha": 1.0, + "output_alpha": 1.0, + }, }, } @@ -244,9 +254,104 @@ def test_linkerhand_o6_device_starts_sdk(monkeypatch) -> None: assert [hand.hand.close_calls for hand in FakeLinkerHandApi.instances] == [0, 0] -def test_linkerhand_o6_rejects_vr_hand_pose() -> None: - with pytest.raises(ValueError, match="supports only hands.mode=gripper"): - parse_linkerhand_o6_config(_o6_cfg(mode="vr_hand_pose")) +def test_linkerhand_o6_accepts_vr_hand_pose() -> None: + cfg = parse_linkerhand_o6_config(_o6_cfg(mode="vr_hand_pose")) + + assert cfg.mode == "vr_hand_pose" + assert cfg.speed == (255, 255, 255, 255, 255, 255) + assert cfg.somehand_config_path == O6_DEFAULT_SOMEHAND_CONFIG + + mapper = SomehandO6Mapper(cfg) + assert mapper.map(controller_snapshot=None, hand_snapshot=None, active=False, now_s=10.0) == () + mapper._active = True + first_inactive = mapper.map(controller_snapshot=None, hand_snapshot=None, active=False, now_s=10.1) + assert [command.force for command in first_inactive] == [True, True] + + +def test_somehand_mapper_loads_only_configured_side(monkeypatch) -> None: + class FakeHandModel: + def get_joint_name_to_qpos_index(self) -> dict[str, int]: + return { + "lh_thumb_cmc_pitch": 0, + "lh_thumb_cmc_roll": 1, + "lh_index_mcp_pitch": 2, + "lh_middle_mcp_pitch": 3, + "lh_ring_mcp_pitch": 4, + "lh_pinky_mcp_pitch": 5, + } + + class FakeRetargetingEngine: + def __init__(self, cfg: object) -> None: + self.cfg = cfg + self.hand_model = FakeHandModel() + + loaded_paths: list[str] = [] + somehand_api = ModuleType("somehand.api") + somehand_api.HandFrame = object + somehand_api.RetargetingEngine = FakeRetargetingEngine + somehand_api.load_bihand_config = lambda path: SimpleNamespace( + left_config_path="left-only.yaml", + right_config_path="right-should-not-load.yaml", + ) + + def load_retargeting_config(path: str): + loaded_paths.append(path) + if path != "left-only.yaml": + raise AssertionError(f"unexpected path loaded: {path}") + return SimpleNamespace( + solver=SimpleNamespace(max_iterations=30, output_alpha=0.7), + preprocess=SimpleNamespace(temporal_filter_alpha=0.35), + ) + + somehand_api.load_retargeting_config = load_retargeting_config + somehand_pkg = ModuleType("somehand") + somehand_pkg.__path__ = [] + monkeypatch.setitem(sys.modules, "somehand", somehand_pkg) + monkeypatch.setitem(sys.modules, "somehand.api", somehand_api) + monkeypatch.setattr("teleopit.sim2real.hands.linkerhand_l6.version", lambda name: "0.2.0") + monkeypatch.setattr("teleopit.sim2real.hands.linkerhand_l6._resolve_project_path", lambda path: SimpleNamespace(exists=lambda: True)) + monkeypatch.setattr( + "teleopit.sim2real.hands.linkerhand_l6._load_linkerhand_mapping_module", + lambda: SimpleNamespace( + l6_l_min=[0.0, -0.087266, 0.0, 0.0, 0.0, 0.0], + l6_l_max=[0.837758, 1.256637, 1.134464, 1.134464, 1.134464, 1.134464], + l6_l_derict=[-1, -1, -1, -1, -1, -1], + ), + ) + config_dict = _cfg(mode="vr_hand_pose") + config_dict["hands"]["sides"] = ["left"] # type: ignore[index] + mapper = SomehandL6Mapper(parse_linkerhand_l6_config(config_dict)) + + mapper.start() + + assert loaded_paths == ["left-only.yaml"] + + +def test_o6_retarget_pose_mapper_uses_o6_thumb_yaw_and_mapping(monkeypatch) -> None: + class FakeHandModel: + def get_joint_name_to_qpos_index(self) -> dict[str, int]: + return { + "lh_thumb_cmc_pitch": 0, + "lh_thumb_cmc_yaw": 1, + "lh_index_mcp_pitch": 2, + "lh_middle_mcp_pitch": 3, + "lh_ring_mcp_pitch": 4, + "lh_pinky_mcp_pitch": 5, + } + + mapping = SimpleNamespace( + o6_l_min=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + o6_l_max=[0.58, 1.36, 1.6, 1.6, 1.6, 1.6], + o6_l_derict=[-1, -1, -1, -1, -1, -1], + is_within_range=lambda value, lower, upper: max(lower, min(upper, value)), + scale_value=lambda value, in_min, in_max, out_min, out_max: out_min + + (value - in_min) * (out_max - out_min) / (in_max - in_min), + ) + monkeypatch.setattr("teleopit.sim2real.hands.linkerhand_l6._load_linkerhand_mapping_module", lambda: mapping) + + mapper = RetargetPoseMapper(FakeHandModel(), side="left", family="O6", joint_order=O6_SDK_JOINT_ORDER) + + assert mapper.qpos_to_pose(np.asarray([0.58, 0.0, 0.8, 1.6, 0.0, 1.6])) == [0, 255, 128, 0, 255, 0] def test_hand_runtime_closes_device_when_mapper_start_fails() -> None: From d695afd5aa35c6cced9f4e59dbdc6107f7b873d3 Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Wed, 8 Jul 2026 21:57:45 +0800 Subject: [PATCH 08/59] Rename LinkerHand dev test script --- docs/docs/getting-started/installation.md | 2 +- docs/docs/tutorials/pico-sim2real.md | 8 ++++---- .../current/getting-started/installation.md | 2 +- .../current/tutorials/pico-sim2real.md | 8 ++++---- scripts/dev/{test_linkerhand_l6.py => test_linkerhand.py} | 0 5 files changed, 10 insertions(+), 10 deletions(-) rename scripts/dev/{test_linkerhand_l6.py => test_linkerhand.py} (100%) diff --git a/docs/docs/getting-started/installation.md b/docs/docs/getting-started/installation.md index a0a8f189..89761893 100644 --- a/docs/docs/getting-started/installation.md +++ b/docs/docs/getting-started/installation.md @@ -68,7 +68,7 @@ Install those packages directly after initializing the submodules: git submodule update --init --recursive pip install -e third_party/linkerhand-python-sdk pip install -e third_party/somehand -scripts/setup/download_somehand_assets.sh +bash scripts/setup/download_somehand_assets.sh ``` These packages are only required when `hands.enabled=true`. diff --git a/docs/docs/tutorials/pico-sim2real.md b/docs/docs/tutorials/pico-sim2real.md index 0943d09e..a352e27e 100644 --- a/docs/docs/tutorials/pico-sim2real.md +++ b/docs/docs/tutorials/pico-sim2real.md @@ -199,7 +199,7 @@ the main Pico profile: git submodule update --init --recursive pip install -e third_party/linkerhand-python-sdk pip install -e third_party/somehand -scripts/setup/download_somehand_assets.sh +bash scripts/setup/download_somehand_assets.sh ``` Bring up the CAN interfaces before testing or running hand control: @@ -213,7 +213,7 @@ Before enabling full sim2real, verify the hand connection with a standalone open/close test. The test runs until Ctrl-C: ```bash -python scripts/dev/test_linkerhand_l6.py \ +python scripts/dev/test_linkerhand.py \ --hand-type both \ --left-can can0 \ --right-can can1 @@ -222,7 +222,7 @@ python scripts/dev/test_linkerhand_l6.py \ For an O6 standalone open/close test, add the O6 driver: ```bash -python scripts/dev/test_linkerhand_l6.py \ +python scripts/dev/test_linkerhand.py \ --driver linkerhand_o6 \ --hand-type both \ --left-can can0 \ @@ -323,5 +323,5 @@ input.video.enabled=true | Cannot enter debug mode | Unitree mode release failed | Stop other robot modes and press `Start` again | | Robot enters `STANDING` but not `MOCAP` | Mocap validation failed | Keep tracking active and stable; check `mocap_switch.check_frames` logs | | Pico pause does not return to `STANDING` | Expected behavior | Pico pause freezes mocap; press remote `X` for `STANDING` | -| LinkerHand does not move | `hands.enabled=false`, gripper deadman released, SDK/assets not installed, or CAN channel wrong | Enable `hands.enabled`, set `hands.mode`, run `scripts/dev/test_linkerhand_l6.py`, and check the selected driver's `left_can` / `right_can` | +| LinkerHand does not move | `hands.enabled=false`, gripper deadman released, SDK/assets not installed, or CAN channel wrong | Enable `hands.enabled`, set `hands.mode`, run `scripts/dev/test_linkerhand.py`, and check the selected driver's `left_can` / `right_can` | | Video preview is unavailable | RealSense or video source failed | Check camera permissions, `input.video.source`, and logs | diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/installation.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/installation.md index 6cb001ec..5dd89b68 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/installation.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/installation.md @@ -68,7 +68,7 @@ submodule 后，直接安装这些包： git submodule update --init --recursive pip install -e third_party/linkerhand-python-sdk pip install -e third_party/somehand -scripts/setup/download_somehand_assets.sh +bash scripts/setup/download_somehand_assets.sh ``` 只有在 `hands.enabled=true` 时才需要安装这些包。 diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md index af46feed..a33c1ea9 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md @@ -186,7 +186,7 @@ Pico sim2real 可以用 Pico 输入控制 LinkerHand： git submodule update --init --recursive pip install -e third_party/linkerhand-python-sdk pip install -e third_party/somehand -scripts/setup/download_somehand_assets.sh +bash scripts/setup/download_somehand_assets.sh ``` 测试或运行手控前，先开启 CAN 接口： @@ -199,7 +199,7 @@ sudo /usr/sbin/ip link set can1 up type can bitrate 1000000 启用完整 sim2real 前，先用独立开合测试验证灵巧手连接。测试默认一直运行到 Ctrl-C： ```bash -python scripts/dev/test_linkerhand_l6.py \ +python scripts/dev/test_linkerhand.py \ --hand-type both \ --left-can can0 \ --right-can can1 @@ -208,7 +208,7 @@ python scripts/dev/test_linkerhand_l6.py \ O6 独立开合测试需要加上 O6 driver： ```bash -python scripts/dev/test_linkerhand_l6.py \ +python scripts/dev/test_linkerhand.py \ --driver linkerhand_o6 \ --hand-type both \ --left-can can0 \ @@ -309,5 +309,5 @@ input.video.enabled=true | 无法进入 debug mode | Unitree mode 释放失败 | 停止其他机器人模式后再次按 `Start` | | 机器人进入 `STANDING` 但不进入 `MOCAP` | 动捕验证失败 | 保持追踪稳定，查看 `mocap_switch.check_frames` 日志 | | Pico 暂停没有返回 `STANDING` | 这是预期行为 | Pico 暂停只冻结 mocap；按遥控器 `X` 返回 `STANDING` | -| LinkerHand 不动 | `hands.enabled=false`、gripper deadman 未按住、SDK/资产未安装，或 CAN 通道错误 | 设置 `hands.enabled=true` 和 `hands.mode`，运行 `scripts/dev/test_linkerhand_l6.py`，并检查所选 driver 的 `left_can` / `right_can` | +| LinkerHand 不动 | `hands.enabled=false`、gripper deadman 未按住、SDK/资产未安装，或 CAN 通道错误 | 设置 `hands.enabled=true` 和 `hands.mode`，运行 `scripts/dev/test_linkerhand.py`，并检查所选 driver 的 `left_can` / `right_can` | | 视频预览不可用 | RealSense 或视频源失败 | 检查相机权限、`input.video.source` 和日志 | diff --git a/scripts/dev/test_linkerhand_l6.py b/scripts/dev/test_linkerhand.py similarity index 100% rename from scripts/dev/test_linkerhand_l6.py rename to scripts/dev/test_linkerhand.py From 9018e6f9a6504928902d04d5b1c786c2a203dd39 Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Wed, 8 Jul 2026 23:19:33 +0800 Subject: [PATCH 09/59] Configure ONNX Runtime CPU budget --- teleopit/configs/controller/rl_policy.yaml | 6 + teleopit/configs/sim2real.yaml | 7 + teleopit/controllers/rl_policy.py | 164 ++++++++++++++++++++- teleopit/sim2real/mp/runtime.py | 1 + tests/test_controller.py | 109 ++++++++++++++ 5 files changed, 286 insertions(+), 1 deletion(-) diff --git a/teleopit/configs/controller/rl_policy.yaml b/teleopit/configs/controller/rl_policy.yaml index 1bd1fe1b..9b70acdc 100644 --- a/teleopit/configs/controller/rl_policy.yaml +++ b/teleopit/configs/controller/rl_policy.yaml @@ -5,3 +5,9 @@ observation_type: "velcmd_history" action_scale: null # inherited from robot config; set explicitly to override clip_range: [-10.0, 10.0] default_dof_pos: null + +# CPU runtime controls for ONNX Runtime. Leave cpu_affinity null for no process binding. +cpu_affinity: null +intra_op_num_threads: null +inter_op_num_threads: null +intra_op_allow_spinning: null diff --git a/teleopit/configs/sim2real.yaml b/teleopit/configs/sim2real.yaml index 5cf15d94..3ea85190 100644 --- a/teleopit/configs/sim2real.yaml +++ b/teleopit/configs/sim2real.yaml @@ -22,6 +22,13 @@ console: show_timing: false timing_log_interval_s: 10.0 +# Sim2real production defaults: bind the control process to CPUs 4-7 and keep +# ONNX Runtime inside that CPU budget. +controller: + cpu_affinity: [4, 5, 6, 7] + intra_op_num_threads: 4 + inter_op_num_threads: 1 + recording: enabled: false format: hdf5 diff --git a/teleopit/controllers/rl_policy.py b/teleopit/controllers/rl_policy.py index 4a4c934f..468b2cfd 100644 --- a/teleopit/controllers/rl_policy.py +++ b/teleopit/controllers/rl_policy.py @@ -2,6 +2,7 @@ import importlib import logging +import os from collections import deque from collections.abc import Callable, Mapping, Sequence from pathlib import Path @@ -60,7 +61,16 @@ def __init__(self, cfg: object) -> None: cast(Callable[[], Sequence[str]], providers_fn), str(cfg_get(cfg, "device", "auto")), ) - self._session = cast(_OrtSession, session_ctor(str(policy_path), providers=providers)) + applied_cpus = self._apply_cpu_affinity(cfg) + session_options = self._create_session_options( + ort, + cfg, + cpu_budget=None if applied_cpus is None else len(applied_cpus), + ) + self._session = cast( + _OrtSession, + session_ctor(str(policy_path), sess_options=session_options, providers=providers), + ) onnx_inputs = self._session.get_inputs() self._input_name = onnx_inputs[0].name self._output_name = self._session.get_outputs()[0].name @@ -254,3 +264,155 @@ def _select_providers(get_available_providers: object, device: str) -> list[str] providers.append("CUDAExecutionProvider") providers.append("CPUExecutionProvider") return providers + + @staticmethod + def _create_session_options( + ort: object, + cfg: object, + *, + cpu_budget: int | None = None, + ) -> object | None: + intra_threads = RLPolicyController._parse_optional_positive_int( + cfg_get(cfg, "intra_op_num_threads", None), + field_name="controller.intra_op_num_threads", + ) + inter_threads = RLPolicyController._parse_optional_positive_int( + cfg_get(cfg, "inter_op_num_threads", None), + field_name="controller.inter_op_num_threads", + ) + allow_spinning = cfg_get(cfg, "intra_op_allow_spinning", None) + if intra_threads is None and inter_threads is None and allow_spinning is None: + return None + if ( + intra_threads is not None + and cpu_budget is not None + and cpu_budget > 0 + and intra_threads > cpu_budget + ): + _logger.warning( + "controller.intra_op_num_threads=%s exceeds effective CPU affinity budget %s; using %s", + intra_threads, + cpu_budget, + cpu_budget, + ) + intra_threads = cpu_budget + + session_options_ctor = getattr(ort, "SessionOptions", None) + if not callable(session_options_ctor): + raise ImportError("onnxruntime missing SessionOptions API") + session_options = session_options_ctor() + if intra_threads is not None: + session_options.intra_op_num_threads = intra_threads + if inter_threads is not None: + session_options.inter_op_num_threads = inter_threads + if allow_spinning is not None: + if not isinstance(allow_spinning, bool): + raise ValueError( + "controller.intra_op_allow_spinning must be true, false, or null" + ) + add_entry = getattr(session_options, "add_session_config_entry", None) + if not callable(add_entry): + raise ImportError("onnxruntime SessionOptions missing add_session_config_entry API") + add_entry("session.intra_op.allow_spinning", "1" if allow_spinning else "0") + return session_options + + @staticmethod + def _apply_cpu_affinity(cfg: object) -> tuple[int, ...] | None: + cpus = RLPolicyController._normalize_cpu_affinity( + cfg_get(cfg, "cpu_affinity", None), + field_name="controller.cpu_affinity", + ) + if cpus is None: + return None + if not hasattr(os, "sched_getaffinity") or not hasattr(os, "sched_setaffinity"): + _logger.warning("controller.cpu_affinity is configured but OS CPU affinity APIs are unavailable") + return None + + allowed = sorted(os.sched_getaffinity(0)) + target = [cpu for cpu in cpus if cpu in allowed] + if len(target) != len(cpus): + _logger.warning( + "controller.cpu_affinity requested CPUs %s but current affinity mask allows %s; using %s", + list(cpus), + allowed, + target, + ) + if not target: + _logger.warning( + "controller.cpu_affinity resolved to no available CPUs; leaving affinity unchanged " + "and using current CPU budget %s", + allowed, + ) + return tuple(allowed) if allowed else None + if set(target) != set(allowed): + os.sched_setaffinity(0, set(target)) + _logger.info("Bound CPU affinity to CPUs %s", target) + else: + _logger.debug("CPU affinity already limited to CPUs %s", target) + return tuple(target) + + @staticmethod + def _normalize_cpu_affinity(raw: object, *, field_name: str) -> tuple[int, ...] | None: + if raw is None or raw is False: + return None + if isinstance(raw, str) and raw.strip().lower() in ("", "null", "none"): + return None + + values: list[object] + if isinstance(raw, str): + values = [] + for token in raw.split(","): + token = token.strip() + if not token: + continue + if "-" in token: + parts = token.split("-") + if len(parts) != 2 or not parts[0].strip() or not parts[1].strip(): + raise ValueError(f"{field_name} contains invalid CPU range {token!r}") + try: + start = int(parts[0]) + end = int(parts[1]) + except ValueError as exc: + raise ValueError(f"{field_name} contains invalid CPU range {token!r}") from exc + if end < start: + raise ValueError(f"{field_name} CPU range must be ascending, got {token!r}") + values.extend(range(start, end + 1)) + else: + try: + values.append(int(token)) + except ValueError as exc: + raise ValueError(f"{field_name} contains invalid CPU id {token!r}") from exc + elif isinstance(raw, int) and not isinstance(raw, bool): + values = [raw] + elif isinstance(raw, Sequence) and not isinstance(raw, (bytes, bytearray)): + values = list(raw) + else: + raise ValueError(f"{field_name} must be null, an int, a list of ints, or a comma-separated string") + + cpus: list[int] = [] + seen: set[int] = set() + for value in values: + if isinstance(value, bool) or not isinstance(value, (int, float)): + raise ValueError(f"{field_name} entries must be non-negative integers, got {value!r}") + cpu = int(value) + if cpu < 0 or cpu != value: + raise ValueError(f"{field_name} entries must be non-negative integers, got {value!r}") + if cpu not in seen: + seen.add(cpu) + cpus.append(cpu) + if not cpus: + return None + return tuple(cpus) + + @staticmethod + def _parse_optional_positive_int(raw: object, *, field_name: str) -> int | None: + if raw is None: + return None + if isinstance(raw, str) and raw.strip().lower() in ("", "null", "none"): + return None + if isinstance(raw, bool) or not isinstance(raw, (int, float)): + raise ValueError(f"{field_name} must be a positive integer or null, got {raw!r}") + parsed = int(raw) + if parsed <= 0 or parsed != raw: + raise ValueError(f"{field_name} must be a positive integer or null, got {raw!r}") + return parsed diff --git a/teleopit/sim2real/mp/runtime.py b/teleopit/sim2real/mp/runtime.py index 700e619d..70f3b5e9 100644 --- a/teleopit/sim2real/mp/runtime.py +++ b/teleopit/sim2real/mp/runtime.py @@ -1068,6 +1068,7 @@ def __init__( self.policy_hz = float(cfg_get(cfg, "policy_hz", 50.0)) self.dt = 1.0 / self.policy_hz + RLPolicyController._apply_cpu_affinity(require_section(cfg, "controller")) self.robot = UnitreeG1Robot(cfg_get(cfg, "real_robot")) self.remote = UnitreeRemote() self.policy, self.obs_builder = self._build_policy_and_obs() diff --git a/tests/test_controller.py b/tests/test_controller.py index e86e41ca..832a8e66 100644 --- a/tests/test_controller.py +++ b/tests/test_controller.py @@ -79,6 +79,115 @@ def test_select_providers_auto_with_cuda(self): assert "CUDAExecutionProvider" in providers assert "CPUExecutionProvider" in providers + def test_normalize_cpu_affinity_list(self): + from teleopit.controllers.rl_policy import RLPolicyController + assert RLPolicyController._normalize_cpu_affinity( + [4, 5, 6, 7], + field_name="controller.cpu_affinity", + ) == (4, 5, 6, 7) + + def test_normalize_cpu_affinity_string_range(self): + from teleopit.controllers.rl_policy import RLPolicyController + assert RLPolicyController._normalize_cpu_affinity( + "4-7", + field_name="controller.cpu_affinity", + ) == (4, 5, 6, 7) + + def test_normalize_cpu_affinity_invalid_raises(self): + from teleopit.controllers.rl_policy import RLPolicyController + with pytest.raises(ValueError, match="non-negative integers"): + RLPolicyController._normalize_cpu_affinity( + [4, -1], + field_name="controller.cpu_affinity", + ) + + def test_create_session_options_sets_ort_threads(self): + from teleopit.controllers.rl_policy import RLPolicyController + + class FakeSessionOptions: + def __init__(self): + self.intra_op_num_threads = 0 + self.inter_op_num_threads = 0 + self.entries = {} + + def add_session_config_entry(self, key, value): + self.entries[key] = value + + class FakeOrt: + SessionOptions = FakeSessionOptions + + options = RLPolicyController._create_session_options( + FakeOrt(), + { + "intra_op_num_threads": 4, + "inter_op_num_threads": 1, + "intra_op_allow_spinning": False, + }, + ) + assert options is not None + assert options.intra_op_num_threads == 4 + assert options.inter_op_num_threads == 1 + assert options.entries["session.intra_op.allow_spinning"] == "0" + + def test_create_session_options_caps_intra_threads_to_cpu_budget(self): + from teleopit.controllers.rl_policy import RLPolicyController + + class FakeSessionOptions: + def __init__(self): + self.intra_op_num_threads = 0 + self.inter_op_num_threads = 0 + + class FakeOrt: + SessionOptions = FakeSessionOptions + + options = RLPolicyController._create_session_options( + FakeOrt(), + { + "intra_op_num_threads": 4, + "inter_op_num_threads": 1, + }, + cpu_budget=2, + ) + assert options is not None + assert options.intra_op_num_threads == 2 + assert options.inter_op_num_threads == 1 + + def test_apply_cpu_affinity_returns_effective_cpus(self, monkeypatch): + from teleopit.controllers import rl_policy + from teleopit.controllers.rl_policy import RLPolicyController + + calls = [] + + monkeypatch.setattr(rl_policy.os, "sched_getaffinity", lambda _pid: {4, 5}) + monkeypatch.setattr( + rl_policy.os, + "sched_setaffinity", + lambda pid, cpus: calls.append((pid, set(cpus))), + ) + + effective = RLPolicyController._apply_cpu_affinity({"cpu_affinity": [4, 5, 6]}) + + assert effective == (4, 5) + assert calls == [] + + def test_apply_cpu_affinity_returns_current_budget_when_request_unavailable(self, monkeypatch): + from teleopit.controllers import rl_policy + from teleopit.controllers.rl_policy import RLPolicyController + + calls = [] + + monkeypatch.setattr(rl_policy.os, "sched_getaffinity", lambda _pid: {0, 1}) + monkeypatch.setattr( + rl_policy.os, + "sched_setaffinity", + lambda pid, cpus: calls.append((pid, set(cpus))), + ) + + effective = RLPolicyController._apply_cpu_affinity({"cpu_affinity": [4, 5, 6, 7]}) + + assert effective == (0, 1) + assert calls == [] + def test_cfg_get_dict(self): from teleopit.runtime.common import cfg_get assert cfg_get({"a": 1}, "a", 0) == 1 From f457f649792dfe3b2ad7d3358fb0dd385950de12 Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Wed, 8 Jul 2026 23:24:59 +0800 Subject: [PATCH 10/59] Enable CPU pinning for Pico sim2real --- teleopit/configs/pico4_sim2real.yaml | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/teleopit/configs/pico4_sim2real.yaml b/teleopit/configs/pico4_sim2real.yaml index cce998ca..4ba4f382 100644 --- a/teleopit/configs/pico4_sim2real.yaml +++ b/teleopit/configs/pico4_sim2real.yaml @@ -18,6 +18,11 @@ reference_anchor_velocity_smoothing_alpha: 0.25 reference_steps: [0] reference_debug_log: false +controller: + cpu_affinity: [4, 5, 6, 7] + intra_op_num_threads: 4 + inter_op_num_threads: 1 + recording: enabled: false format: hdf5 From 553842df2222e1786537739e9adb23e195d1c3c2 Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Thu, 9 Jul 2026 11:48:13 +0800 Subject: [PATCH 11/59] Revert "Enable CPU pinning for Pico sim2real" This reverts commit f457f649792dfe3b2ad7d3358fb0dd385950de12. --- teleopit/configs/pico4_sim2real.yaml | 5 ----- 1 file changed, 5 deletions(-) diff --git a/teleopit/configs/pico4_sim2real.yaml b/teleopit/configs/pico4_sim2real.yaml index 4ba4f382..cce998ca 100644 --- a/teleopit/configs/pico4_sim2real.yaml +++ b/teleopit/configs/pico4_sim2real.yaml @@ -18,11 +18,6 @@ reference_anchor_velocity_smoothing_alpha: 0.25 reference_steps: [0] reference_debug_log: false -controller: - cpu_affinity: [4, 5, 6, 7] - intra_op_num_threads: 4 - inter_op_num_threads: 1 - recording: enabled: false format: hdf5 From 82ab90b7998be57319ab59f4082846317f0c820e Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Thu, 9 Jul 2026 11:48:13 +0800 Subject: [PATCH 12/59] Revert "Configure ONNX Runtime CPU budget" This reverts commit 9018e6f9a6504928902d04d5b1c786c2a203dd39. --- teleopit/configs/controller/rl_policy.yaml | 6 - teleopit/configs/sim2real.yaml | 7 - teleopit/controllers/rl_policy.py | 164 +-------------------- teleopit/sim2real/mp/runtime.py | 1 - tests/test_controller.py | 109 -------------- 5 files changed, 1 insertion(+), 286 deletions(-) diff --git a/teleopit/configs/controller/rl_policy.yaml b/teleopit/configs/controller/rl_policy.yaml index 9b70acdc..1bd1fe1b 100644 --- a/teleopit/configs/controller/rl_policy.yaml +++ b/teleopit/configs/controller/rl_policy.yaml @@ -5,9 +5,3 @@ observation_type: "velcmd_history" action_scale: null # inherited from robot config; set explicitly to override clip_range: [-10.0, 10.0] default_dof_pos: null - -# CPU runtime controls for ONNX Runtime. Leave cpu_affinity null for no process binding. -cpu_affinity: null -intra_op_num_threads: null -inter_op_num_threads: null -intra_op_allow_spinning: null diff --git a/teleopit/configs/sim2real.yaml b/teleopit/configs/sim2real.yaml index 3ea85190..5cf15d94 100644 --- a/teleopit/configs/sim2real.yaml +++ b/teleopit/configs/sim2real.yaml @@ -22,13 +22,6 @@ console: show_timing: false timing_log_interval_s: 10.0 -# Sim2real production defaults: bind the control process to CPUs 4-7 and keep -# ONNX Runtime inside that CPU budget. -controller: - cpu_affinity: [4, 5, 6, 7] - intra_op_num_threads: 4 - inter_op_num_threads: 1 - recording: enabled: false format: hdf5 diff --git a/teleopit/controllers/rl_policy.py b/teleopit/controllers/rl_policy.py index 468b2cfd..4a4c934f 100644 --- a/teleopit/controllers/rl_policy.py +++ b/teleopit/controllers/rl_policy.py @@ -2,7 +2,6 @@ import importlib import logging -import os from collections import deque from collections.abc import Callable, Mapping, Sequence from pathlib import Path @@ -61,16 +60,7 @@ def __init__(self, cfg: object) -> None: cast(Callable[[], Sequence[str]], providers_fn), str(cfg_get(cfg, "device", "auto")), ) - applied_cpus = self._apply_cpu_affinity(cfg) - session_options = self._create_session_options( - ort, - cfg, - cpu_budget=None if applied_cpus is None else len(applied_cpus), - ) - self._session = cast( - _OrtSession, - session_ctor(str(policy_path), sess_options=session_options, providers=providers), - ) + self._session = cast(_OrtSession, session_ctor(str(policy_path), providers=providers)) onnx_inputs = self._session.get_inputs() self._input_name = onnx_inputs[0].name self._output_name = self._session.get_outputs()[0].name @@ -264,155 +254,3 @@ def _select_providers(get_available_providers: object, device: str) -> list[str] providers.append("CUDAExecutionProvider") providers.append("CPUExecutionProvider") return providers - - @staticmethod - def _create_session_options( - ort: object, - cfg: object, - *, - cpu_budget: int | None = None, - ) -> object | None: - intra_threads = RLPolicyController._parse_optional_positive_int( - cfg_get(cfg, "intra_op_num_threads", None), - field_name="controller.intra_op_num_threads", - ) - inter_threads = RLPolicyController._parse_optional_positive_int( - cfg_get(cfg, "inter_op_num_threads", None), - field_name="controller.inter_op_num_threads", - ) - allow_spinning = cfg_get(cfg, "intra_op_allow_spinning", None) - if intra_threads is None and inter_threads is None and allow_spinning is None: - return None - if ( - intra_threads is not None - and cpu_budget is not None - and cpu_budget > 0 - and intra_threads > cpu_budget - ): - _logger.warning( - "controller.intra_op_num_threads=%s exceeds effective CPU affinity budget %s; using %s", - intra_threads, - cpu_budget, - cpu_budget, - ) - intra_threads = cpu_budget - - session_options_ctor = getattr(ort, "SessionOptions", None) - if not callable(session_options_ctor): - raise ImportError("onnxruntime missing SessionOptions API") - session_options = session_options_ctor() - if intra_threads is not None: - session_options.intra_op_num_threads = intra_threads - if inter_threads is not None: - session_options.inter_op_num_threads = inter_threads - if allow_spinning is not None: - if not isinstance(allow_spinning, bool): - raise ValueError( - "controller.intra_op_allow_spinning must be true, false, or null" - ) - add_entry = getattr(session_options, "add_session_config_entry", None) - if not callable(add_entry): - raise ImportError("onnxruntime SessionOptions missing add_session_config_entry API") - add_entry("session.intra_op.allow_spinning", "1" if allow_spinning else "0") - return session_options - - @staticmethod - def _apply_cpu_affinity(cfg: object) -> tuple[int, ...] | None: - cpus = RLPolicyController._normalize_cpu_affinity( - cfg_get(cfg, "cpu_affinity", None), - field_name="controller.cpu_affinity", - ) - if cpus is None: - return None - if not hasattr(os, "sched_getaffinity") or not hasattr(os, "sched_setaffinity"): - _logger.warning("controller.cpu_affinity is configured but OS CPU affinity APIs are unavailable") - return None - - allowed = sorted(os.sched_getaffinity(0)) - target = [cpu for cpu in cpus if cpu in allowed] - if len(target) != len(cpus): - _logger.warning( - "controller.cpu_affinity requested CPUs %s but current affinity mask allows %s; using %s", - list(cpus), - allowed, - target, - ) - if not target: - _logger.warning( - "controller.cpu_affinity resolved to no available CPUs; leaving affinity unchanged " - "and using current CPU budget %s", - allowed, - ) - return tuple(allowed) if allowed else None - if set(target) != set(allowed): - os.sched_setaffinity(0, set(target)) - _logger.info("Bound CPU affinity to CPUs %s", target) - else: - _logger.debug("CPU affinity already limited to CPUs %s", target) - return tuple(target) - - @staticmethod - def _normalize_cpu_affinity(raw: object, *, field_name: str) -> tuple[int, ...] | None: - if raw is None or raw is False: - return None - if isinstance(raw, str) and raw.strip().lower() in ("", "null", "none"): - return None - - values: list[object] - if isinstance(raw, str): - values = [] - for token in raw.split(","): - token = token.strip() - if not token: - continue - if "-" in token: - parts = token.split("-") - if len(parts) != 2 or not parts[0].strip() or not parts[1].strip(): - raise ValueError(f"{field_name} contains invalid CPU range {token!r}") - try: - start = int(parts[0]) - end = int(parts[1]) - except ValueError as exc: - raise ValueError(f"{field_name} contains invalid CPU range {token!r}") from exc - if end < start: - raise ValueError(f"{field_name} CPU range must be ascending, got {token!r}") - values.extend(range(start, end + 1)) - else: - try: - values.append(int(token)) - except ValueError as exc: - raise ValueError(f"{field_name} contains invalid CPU id {token!r}") from exc - elif isinstance(raw, int) and not isinstance(raw, bool): - values = [raw] - elif isinstance(raw, Sequence) and not isinstance(raw, (bytes, bytearray)): - values = list(raw) - else: - raise ValueError(f"{field_name} must be null, an int, a list of ints, or a comma-separated string") - - cpus: list[int] = [] - seen: set[int] = set() - for value in values: - if isinstance(value, bool) or not isinstance(value, (int, float)): - raise ValueError(f"{field_name} entries must be non-negative integers, got {value!r}") - cpu = int(value) - if cpu < 0 or cpu != value: - raise ValueError(f"{field_name} entries must be non-negative integers, got {value!r}") - if cpu not in seen: - seen.add(cpu) - cpus.append(cpu) - if not cpus: - return None - return tuple(cpus) - - @staticmethod - def _parse_optional_positive_int(raw: object, *, field_name: str) -> int | None: - if raw is None: - return None - if isinstance(raw, str) and raw.strip().lower() in ("", "null", "none"): - return None - if isinstance(raw, bool) or not isinstance(raw, (int, float)): - raise ValueError(f"{field_name} must be a positive integer or null, got {raw!r}") - parsed = int(raw) - if parsed <= 0 or parsed != raw: - raise ValueError(f"{field_name} must be a positive integer or null, got {raw!r}") - return parsed diff --git a/teleopit/sim2real/mp/runtime.py b/teleopit/sim2real/mp/runtime.py index 70f3b5e9..700e619d 100644 --- a/teleopit/sim2real/mp/runtime.py +++ b/teleopit/sim2real/mp/runtime.py @@ -1068,7 +1068,6 @@ def __init__( self.policy_hz = float(cfg_get(cfg, "policy_hz", 50.0)) self.dt = 1.0 / self.policy_hz - RLPolicyController._apply_cpu_affinity(require_section(cfg, "controller")) self.robot = UnitreeG1Robot(cfg_get(cfg, "real_robot")) self.remote = UnitreeRemote() self.policy, self.obs_builder = self._build_policy_and_obs() diff --git a/tests/test_controller.py b/tests/test_controller.py index 832a8e66..e86e41ca 100644 --- a/tests/test_controller.py +++ b/tests/test_controller.py @@ -79,115 +79,6 @@ def test_select_providers_auto_with_cuda(self): assert "CUDAExecutionProvider" in providers assert "CPUExecutionProvider" in providers - def test_normalize_cpu_affinity_list(self): - from teleopit.controllers.rl_policy import RLPolicyController - assert RLPolicyController._normalize_cpu_affinity( - [4, 5, 6, 7], - field_name="controller.cpu_affinity", - ) == (4, 5, 6, 7) - - def test_normalize_cpu_affinity_string_range(self): - from teleopit.controllers.rl_policy import RLPolicyController - assert RLPolicyController._normalize_cpu_affinity( - "4-7", - field_name="controller.cpu_affinity", - ) == (4, 5, 6, 7) - - def test_normalize_cpu_affinity_invalid_raises(self): - from teleopit.controllers.rl_policy import RLPolicyController - with pytest.raises(ValueError, match="non-negative integers"): - RLPolicyController._normalize_cpu_affinity( - [4, -1], - field_name="controller.cpu_affinity", - ) - - def test_create_session_options_sets_ort_threads(self): - from teleopit.controllers.rl_policy import RLPolicyController - - class FakeSessionOptions: - def __init__(self): - self.intra_op_num_threads = 0 - self.inter_op_num_threads = 0 - self.entries = {} - - def add_session_config_entry(self, key, value): - self.entries[key] = value - - class FakeOrt: - SessionOptions = FakeSessionOptions - - options = RLPolicyController._create_session_options( - FakeOrt(), - { - "intra_op_num_threads": 4, - "inter_op_num_threads": 1, - "intra_op_allow_spinning": False, - }, - ) - assert options is not None - assert options.intra_op_num_threads == 4 - assert options.inter_op_num_threads == 1 - assert options.entries["session.intra_op.allow_spinning"] == "0" - - def test_create_session_options_caps_intra_threads_to_cpu_budget(self): - from teleopit.controllers.rl_policy import RLPolicyController - - class FakeSessionOptions: - def __init__(self): - self.intra_op_num_threads = 0 - self.inter_op_num_threads = 0 - - class FakeOrt: - SessionOptions = FakeSessionOptions - - options = RLPolicyController._create_session_options( - FakeOrt(), - { - "intra_op_num_threads": 4, - "inter_op_num_threads": 1, - }, - cpu_budget=2, - ) - assert options is not None - assert options.intra_op_num_threads == 2 - assert options.inter_op_num_threads == 1 - - def test_apply_cpu_affinity_returns_effective_cpus(self, monkeypatch): - from teleopit.controllers import rl_policy - from teleopit.controllers.rl_policy import RLPolicyController - - calls = [] - - monkeypatch.setattr(rl_policy.os, "sched_getaffinity", lambda _pid: {4, 5}) - monkeypatch.setattr( - rl_policy.os, - "sched_setaffinity", - lambda pid, cpus: calls.append((pid, set(cpus))), - ) - - effective = RLPolicyController._apply_cpu_affinity({"cpu_affinity": [4, 5, 6]}) - - assert effective == (4, 5) - assert calls == [] - - def test_apply_cpu_affinity_returns_current_budget_when_request_unavailable(self, monkeypatch): - from teleopit.controllers import rl_policy - from teleopit.controllers.rl_policy import RLPolicyController - - calls = [] - - monkeypatch.setattr(rl_policy.os, "sched_getaffinity", lambda _pid: {0, 1}) - monkeypatch.setattr( - rl_policy.os, - "sched_setaffinity", - lambda pid, cpus: calls.append((pid, set(cpus))), - ) - - effective = RLPolicyController._apply_cpu_affinity({"cpu_affinity": [4, 5, 6, 7]}) - - assert effective == (0, 1) - assert calls == [] - def test_cfg_get_dict(self): from teleopit.runtime.common import cfg_get assert cfg_get({"a": 1}, "a", 0) == 1 From 34732159d7390e5f9ebc5f06ae5b83d0543ac853 Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Thu, 9 Jul 2026 16:59:05 +0800 Subject: [PATCH 13/59] Harden OpenNeck and RealSense shutdown --- docs/docs/configuration/config-reference.md | 2 +- .../current/configuration/config-reference.md | 2 +- teleopit/configs/pico4_sim2real.yaml | 2 +- teleopit/configs/sim2real.yaml | 2 +- teleopit/inputs/pico_video.py | 8 +- teleopit/sim2real/neck/config.py | 4 +- teleopit/sim2real/neck/openneck.py | 32 ++- teleopit/sim2real/neck/worker.py | 10 +- tests/test_active_neck.py | 186 ++++++++++++++++++ tests/test_pico_video.py | 31 +++ 10 files changed, 265 insertions(+), 14 deletions(-) diff --git a/docs/docs/configuration/config-reference.md b/docs/docs/configuration/config-reference.md index ba72d4ad..dce3f636 100644 --- a/docs/docs/configuration/config-reference.md +++ b/docs/docs/configuration/config-reference.md @@ -176,7 +176,7 @@ sim2real worker and does not change the policy observation. | `neck.smoothing_alpha` | EMA alpha for normalized yaw/pitch commands | `0.35` | | `neck.yaw_range_deg` / `pitch_range_deg` | Degrees mapped to normalized command magnitude `1.0` | `90.0` / `60.0` | | `neck.invert_yaw` / `invert_pitch` | Invert OpenNeck command direction per axis | `true` / `true` | -| `neck.center_on_start` / `center_on_shutdown` | Center the gimbal at worker startup/shutdown | `true` / `true` | +| `neck.center_on_start` / `center_on_shutdown` | Center the gimbal at worker startup/shutdown | `true` / `false` | | `neck.release_on_shutdown` | Release servo torque after shutdown when supported | `false` | | `neck.dry_run` | Compute commands without opening OpenNeck hardware | `false` | diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md index 9bdd4889..84d538d0 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md @@ -193,7 +193,7 @@ MuJoCo 窗口显示重定向参考；`sim2sim`、`mocap`、`camera` 和 `all` | `neck.smoothing_alpha` | 归一化 yaw/pitch 命令的 EMA alpha | `0.35` | | `neck.yaw_range_deg` / `pitch_range_deg` | 映射到归一化命令幅值 `1.0` 的角度 | `90.0` / `60.0` | | `neck.invert_yaw` / `invert_pitch` | 按轴反转 OpenNeck 命令方向 | `true` / `true` | -| `neck.center_on_start` / `center_on_shutdown` | worker 启动/关闭时回中云台 | `true` / `true` | +| `neck.center_on_start` / `center_on_shutdown` | worker 启动/关闭时回中云台 | `true` / `false` | | `neck.release_on_shutdown` | 关闭后在支持时释放舵机扭矩 | `false` | | `neck.dry_run` | 只计算命令，不打开 OpenNeck 硬件 | `false` | diff --git a/teleopit/configs/pico4_sim2real.yaml b/teleopit/configs/pico4_sim2real.yaml index cce998ca..74c6fa51 100644 --- a/teleopit/configs/pico4_sim2real.yaml +++ b/teleopit/configs/pico4_sim2real.yaml @@ -124,7 +124,7 @@ neck: invert_yaw: true invert_pitch: true center_on_start: true - center_on_shutdown: true + center_on_shutdown: false release_on_shutdown: false dry_run: false diff --git a/teleopit/configs/sim2real.yaml b/teleopit/configs/sim2real.yaml index 5cf15d94..0da0e847 100644 --- a/teleopit/configs/sim2real.yaml +++ b/teleopit/configs/sim2real.yaml @@ -125,7 +125,7 @@ neck: invert_yaw: true invert_pitch: true center_on_start: true - center_on_shutdown: true + center_on_shutdown: false release_on_shutdown: false dry_run: false diff --git a/teleopit/inputs/pico_video.py b/teleopit/inputs/pico_video.py index fd1ed25e..838025dc 100644 --- a/teleopit/inputs/pico_video.py +++ b/teleopit/inputs/pico_video.py @@ -191,6 +191,7 @@ def stop(self) -> None: self._thread.join(timeout=2.0) def _run(self) -> None: + pipeline_started = False try: import pyrealsense2 as rs @@ -206,6 +207,7 @@ def _run(self) -> None: self._config.fps, ) pipeline.start(config) + pipeline_started = True self._ready_event.set() try: while not self._stop_event.is_set(): @@ -222,7 +224,11 @@ def _run(self) -> None: else: self._pushed_frames += 1 finally: - pipeline.stop() + if pipeline_started: + try: + pipeline.stop() + except RuntimeError: + logger.exception("Failed to stop RealSense pipeline after video producer exit") except BaseException as exc: self._error = exc self._ready_event.set() diff --git a/teleopit/sim2real/neck/config.py b/teleopit/sim2real/neck/config.py index 05024109..5c5848de 100644 --- a/teleopit/sim2real/neck/config.py +++ b/teleopit/sim2real/neck/config.py @@ -29,7 +29,7 @@ class NeckConfig: invert_yaw: bool = True invert_pitch: bool = True center_on_start: bool = True - center_on_shutdown: bool = True + center_on_shutdown: bool = False release_on_shutdown: bool = False dry_run: bool = False @@ -81,7 +81,7 @@ def parse_neck_config(cfg: Any) -> NeckConfig: invert_yaw=bool(cfg_get(neck_cfg, "invert_yaw", True)), invert_pitch=bool(cfg_get(neck_cfg, "invert_pitch", True)), center_on_start=bool(cfg_get(neck_cfg, "center_on_start", True)), - center_on_shutdown=bool(cfg_get(neck_cfg, "center_on_shutdown", True)), + center_on_shutdown=bool(cfg_get(neck_cfg, "center_on_shutdown", False)), release_on_shutdown=bool(cfg_get(neck_cfg, "release_on_shutdown", False)), dry_run=bool(cfg_get(neck_cfg, "dry_run", False)), ) diff --git a/teleopit/sim2real/neck/openneck.py b/teleopit/sim2real/neck/openneck.py index d6f67971..64574ccf 100644 --- a/teleopit/sim2real/neck/openneck.py +++ b/teleopit/sim2real/neck/openneck.py @@ -68,15 +68,37 @@ def close(self) -> None: controller = self._controller self._context = None self._controller = None + close_error: BaseException | None = None if context is not None: exit_context = getattr(context, "__exit__", None) if callable(exit_context): - exit_context(None, None, None) - return - if controller is not None: - close = getattr(controller, "close", None) + try: + exit_context(None, None, None) + return + except BaseException as exc: + close_error = exc + logger.exception("OpenNeck context exit failed; trying direct close") + close_targets = [target for target in (controller, context) if target is not None] + seen_target_ids: set[int] = set() + direct_close_error: BaseException | None = None + for target in close_targets: + target_id = id(target) + if target_id in seen_target_ids: + continue + seen_target_ids.add(target_id) + close = getattr(target, "close", None) if callable(close): - close() + try: + close() + except BaseException as exc: + direct_close_error = exc + logger.exception("OpenNeck direct close failed") + if close_error is not None: + if direct_close_error is not None: + raise close_error from direct_close_error + raise close_error + if direct_close_error is not None: + raise direct_close_error class DryRunNeckDevice: diff --git a/teleopit/sim2real/neck/worker.py b/teleopit/sim2real/neck/worker.py index 2d2e671e..aa885374 100644 --- a/teleopit/sim2real/neck/worker.py +++ b/teleopit/sim2real/neck/worker.py @@ -49,9 +49,15 @@ def tick( def close(self) -> None: try: if self._cfg.center_on_shutdown: - self._device.center() + try: + self._device.center() + except Exception: + logger.exception("Failed to center OpenNeck on shutdown; closing device") if self._cfg.release_on_shutdown: - self._device.release() + try: + self._device.release() + except Exception: + logger.exception("Failed to release OpenNeck torque on shutdown; closing device") finally: self._device.close() diff --git a/tests/test_active_neck.py b/tests/test_active_neck.py index 7a723f52..54495672 100644 --- a/tests/test_active_neck.py +++ b/tests/test_active_neck.py @@ -150,6 +150,69 @@ def close(self) -> None: assert device.closed is True +def test_neck_shutdown_defaults_to_close_only() -> None: + class FakeDevice: + def __init__(self) -> None: + self.center_calls = 0 + self.released = False + self.closed = False + + def connect(self) -> None: + return None + + def center(self) -> None: + self.center_calls += 1 + + def release(self) -> None: + self.released = True + + def move_norm(self, yaw: float, pitch: float) -> None: + del yaw, pitch + + def close(self) -> None: + self.closed = True + + device = FakeDevice() + runtime = NeckRuntime(NeckConfig(enabled=True, center_on_start=False), device=device) + + runtime.close() + + assert device.center_calls == 0 + assert device.released is False + assert device.closed is True + + +def test_neck_runtime_closes_after_shutdown_center_failure() -> None: + class FakeDevice: + def __init__(self) -> None: + self.closed = False + + def connect(self) -> None: + return None + + def center(self) -> None: + raise RuntimeError("neck center failed") + + def release(self) -> None: + return None + + def move_norm(self, yaw: float, pitch: float) -> None: + del yaw, pitch + + def close(self) -> None: + self.closed = True + + device = FakeDevice() + runtime = NeckRuntime( + NeckConfig(enabled=True, center_on_start=False, center_on_shutdown=True), + device=device, + ) + + runtime.close() + + assert device.closed is True + + def test_body_packet_frame_ignores_incomplete_packets() -> None: assert body_packet_frame(None) == (None, None, -1) assert body_packet_frame(SimpleNamespace(frame=_frame(_quat_y(0.0)))) == (None, None, -1) @@ -199,6 +262,129 @@ def close(self) -> None: assert calls == ["enter", "entered-center-0.5", "entered-move-0.25--0.5", "exit"] +def test_openneck_device_direct_close_after_context_exit_failure(monkeypatch) -> None: + calls: list[str] = [] + + class FakeOpenNeckController: + port = "/dev/fake" + + def __init__(self, *, config: object, port: object, enable_torque_on_connect: bool) -> None: + del config, port, enable_torque_on_connect + + def __enter__(self): + calls.append("enter") + return self + + def __exit__(self, exc_type: object, exc: object, tb: object) -> None: + del exc_type, exc, tb + calls.append("exit") + raise RuntimeError("context exit failed") + + def close(self) -> None: + calls.append("close") + + module = ModuleType("openneck") + module.OpenNeckController = FakeOpenNeckController # type: ignore[attr-defined] + monkeypatch.setitem(__import__("sys").modules, "openneck", module) + + device = OpenNeckDevice(NeckConfig(enabled=True)) + device.connect() + try: + device.close() + except RuntimeError as exc: + assert "context exit failed" in str(exc) + else: + raise AssertionError("expected RuntimeError") + + assert calls == ["enter", "exit", "close"] + + +def test_openneck_device_direct_closes_context_when_entered_proxy_lacks_close(monkeypatch) -> None: + calls: list[str] = [] + + class FakeEnteredController: + port = "/dev/entered" + + class FakeOpenNeckController: + port = "/dev/fake" + + def __init__(self, *, config: object, port: object, enable_torque_on_connect: bool) -> None: + del config, port, enable_torque_on_connect + self.entered = FakeEnteredController() + + def __enter__(self): + calls.append("enter") + return self.entered + + def __exit__(self, exc_type: object, exc: object, tb: object) -> None: + del exc_type, exc, tb + calls.append("exit") + raise RuntimeError("context exit failed") + + def close(self) -> None: + calls.append("context-close") + + module = ModuleType("openneck") + module.OpenNeckController = FakeOpenNeckController # type: ignore[attr-defined] + monkeypatch.setitem(__import__("sys").modules, "openneck", module) + + device = OpenNeckDevice(NeckConfig(enabled=True)) + device.connect() + try: + device.close() + except RuntimeError as exc: + assert "context exit failed" in str(exc) + else: + raise AssertionError("expected RuntimeError") + + assert calls == ["enter", "exit", "context-close"] + + +def test_openneck_device_attempts_all_direct_close_targets(monkeypatch) -> None: + calls: list[str] = [] + + class FakeEnteredController: + port = "/dev/entered" + + def close(self) -> None: + calls.append("entered-close") + raise RuntimeError("entered close failed") + + class FakeOpenNeckController: + port = "/dev/fake" + + def __init__(self, *, config: object, port: object, enable_torque_on_connect: bool) -> None: + del config, port, enable_torque_on_connect + self.entered = FakeEnteredController() + + def __enter__(self): + calls.append("enter") + return self.entered + + def __exit__(self, exc_type: object, exc: object, tb: object) -> None: + del exc_type, exc, tb + calls.append("exit") + raise RuntimeError("context exit failed") + + def close(self) -> None: + calls.append("context-close") + + module = ModuleType("openneck") + module.OpenNeckController = FakeOpenNeckController # type: ignore[attr-defined] + monkeypatch.setitem(__import__("sys").modules, "openneck", module) + + device = OpenNeckDevice(NeckConfig(enabled=True)) + device.connect() + try: + device.close() + except RuntimeError as exc: + assert "context exit failed" in str(exc) + else: + raise AssertionError("expected RuntimeError") + + assert calls == ["enter", "exit", "entered-close", "context-close"] + + def test_parse_neck_config_validates_rate() -> None: try: parse_neck_config({"neck": {"enabled": True, "rate_hz": 0}}) diff --git a/tests/test_pico_video.py b/tests/test_pico_video.py index 6e0d3cb7..5867a1ce 100644 --- a/tests/test_pico_video.py +++ b/tests/test_pico_video.py @@ -199,6 +199,37 @@ def stop(self) -> None: assert stopped is True +def test_realsense_video_start_failure_does_not_stop_unstarted_pipeline(monkeypatch: pytest.MonkeyPatch) -> None: + fake_rs = ModuleType("pyrealsense2") + fake_rs.stream = SimpleNamespace(color="color") + fake_rs.format = SimpleNamespace(rgb8="rgb8") + stop_calls = 0 + + class FakeConfig: + def enable_stream(self, *_args: object) -> None: + pass + + class FakePipeline: + def start(self, _config: object) -> None: + raise RuntimeError("no device connected") + + def stop(self) -> None: + nonlocal stop_calls + stop_calls += 1 + + fake_rs.config = FakeConfig + fake_rs.pipeline = FakePipeline + monkeypatch.setitem(sys.modules, "pyrealsense2", fake_rs) + + sink = _FrameSink() + config = parse_pico_video_config({"video": {"enabled": True, "source": "realsense"}}) + runtime = PicoVideoRuntime(provider=sink, config=config, mode="sim2real") + + runtime.start() + + assert stop_calls == 0 + + def test_mujoco_video_runtime_renders_camera_frame(monkeypatch: pytest.MonkeyPatch) -> None: fake_mujoco = ModuleType("mujoco") fake_mujoco.mjtObj = SimpleNamespace(mjOBJ_CAMERA="camera") From 5b2c94ed0883909e5edab02bf7a382bf8837f16c Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Thu, 9 Jul 2026 17:38:35 +0800 Subject: [PATCH 14/59] Avoid pico-bridge camera extra --- pyproject.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pyproject.toml b/pyproject.toml index 77704031..7c70dac2 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -50,7 +50,7 @@ train = [ "tqdm>=4.65.0", ] pico4 = [ - "pico-bridge[camera] @ https://github.com/BotRunner64/pico-bridge/releases/download/v0.2.1/pico_bridge-0.2.1-py3-none-any.whl", + "pico-bridge @ https://github.com/BotRunner64/pico-bridge/releases/download/v0.2.1/pico_bridge-0.2.1-py3-none-any.whl", "teleopit[sim2real]", ] openneck = [ From ba50d77d32ddc540837a545c298c46d2ff2d362c Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Wed, 15 Jul 2026 14:00:22 +0800 Subject: [PATCH 15/59] feat: update sim2real recording dataset format --- AGENTS.md | 6 +- README.md | 17 +- docs/docs/configuration/config-reference.md | 55 +- docs/docs/tutorials/pico-sim2real.md | 11 +- .../current/configuration/config-reference.md | 48 +- .../current/tutorials/pico-sim2real.md | 12 +- teleopit/configs/robot/g1.yaml | 1 + teleopit/constants.py | 34 + teleopit/recording/hdf5.py | 611 +++++++++++++----- teleopit/sim2real/mp/messages.py | 2 +- teleopit/sim2real/mp/runtime.py | 56 +- tests/test_sim2real_multiprocess.py | 219 +++++-- 12 files changed, 792 insertions(+), 280 deletions(-) diff --git a/AGENTS.md b/AGENTS.md index 814ff8ea..149126aa 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -150,8 +150,10 @@ target_dof_pos = clip(action, -10, 10) × action_scale + default_dof_pos - Optional LinkerHand control uses `hands.enabled=true`, `hands.driver=linkerhand_l6|linkerhand_o6`, and `hands.mode=gripper|vr_hand_pose`; default is disabled - Optional Pico sim2real HDF5 recording uses `--config-name sim2real_record` or `recording.enabled=true`; it requires `input.provider=pico4`, `input.video.enabled=true`, `input.video.source=realsense`, an interactive terminal, and the `recording` extra - Recording is manual only: terminal `R` starts an episode, `S` saves, `D` discards the active episode, and `Q` shuts down; `STANDING`, `MOCAP`, `ARMS`, and paused mocap are recordable -- Recording captures `observation.images.d435i_rgb` RealSense RGB video at 30Hz plus `observation.state(68)`, `observation.mode(1)`, `action(36)`, and `action.hand(12)`; RealSense capture lives in `pico_input` through the normal `input.video` path -- HDF5 recording writes compressed MP4 sidecar videos under `recording.output_dir/videos//` while HDF5 episodes store `frame_index`, `timestamp`, low-dimensional data, and video sync attributes; raw RGB image datasets are not supported +- Recording captures `observation.images.d435i_rgb` RealSense RGB video at 30Hz plus `observation.state(68)`, scalar `observation.mode`, and `action(36)` as the root-plus-joint reference consumed by the motion tracker; `action.hand(12)` is present when LinkerHand control is enabled +- Sim2real recording uses an editable source layout: `schema.json`, `episodes.jsonl`, per-episode HDF5 files under `recording.output_dir/data/`, and compressed MP4 files under `recording.output_dir/videos/d435i_rgb/`; task prompts live only in `episodes.jsonl`, and HDF5 files contain only frame arrays with no metadata attributes or raw RGB datasets +- Recording `schema.json` stores `robot_type=unitree_g1_29dof`, `hand_type=none|linkerhand_l6|linkerhand_o6`, FPS, and feature definitions; the recording worker rejects an existing mismatched schema without writing episodes, but remains non-critical and must not stop the G1 control runtime; the previous attribute-based HDF5 layout is unsupported +- Episodes interrupted before their `episodes.jsonl` entry is committed are discarded on the next recording-worker startup and do not consume an episode index - `gripper` mode reuses `Pico4InputProvider.get_controller_snapshot()` for Pico grip/trigger open-close control and supports LinkerHand L6 and O6 - `vr_hand_pose` mode reuses `Pico4InputProvider.get_hand_snapshot()` and somehand 0.2.0 public `somehand.api` for continuous Pico hand-pose retargeting; do not start a second `PicoBridge` for hand control - Teleopit owns Pico 26-joint hand-state to 21-landmark conversion; do not import `somehand.pico_input` diff --git a/README.md b/README.md index 67f8b865..e48b07aa 100644 --- a/README.md +++ b/README.md @@ -100,13 +100,16 @@ python scripts/run/run_sim2real.py --config-name sim2real_record \ Recording uses the terminal controls `R` start, `S` save, `D` discard, and `Q` shutdown. `STANDING`, `MOCAP`, `ARMS`, and paused mocap can be recorded. Saved -episodes are written as `.h5` files under `data/recordings/sim2real_hdf5/episodes/`. -`sim2real_record.yaml` stores camera frames as compressed MP4 sidecar files under -`data/recordings/sim2real_hdf5/videos/` and keeps `frame_index` / `timestamp` -sync metadata in the HDF5 episode. The low-dimensional HDF5 schema records -`observation.state(68)`, `observation.mode(1)`, `action(36)` as the aligned -reference qpos sent to the policy path, and `action.hand(12)` as the latest -LinkerHand left/right 6D pose commands. +episodes are written under `data/recordings/sim2real_hdf5/data/`, with compressed +MP4 files under `videos/d435i_rgb/`. `schema.json` records the FPS, robot and +hand types, feature shapes, names, and groups. `episodes.jsonl` maps each episode +to its HDF5/video files and stores its editable task prompt. HDF5 contains only +frame-aligned arrays: `observation.state(68)`, scalar `observation.mode`, and +`action(36)` as the aligned reference qpos consumed by the motion tracker. +`action.hand(12)` is present when LinkerHand control is enabled. +Recording is non-critical: an incompatible output schema stops only the +recording worker while G1 control continues. Episodes interrupted before their +manifest entry is committed are discarded on the next recording startup. ## OpenNeck Active Vision diff --git a/docs/docs/configuration/config-reference.md b/docs/docs/configuration/config-reference.md index dce3f636..eb501f64 100644 --- a/docs/docs/configuration/config-reference.md +++ b/docs/docs/configuration/config-reference.md @@ -23,6 +23,7 @@ Complete reference for all configurable fields. | Field | Description | Default | |-------|-------------|---------| +| `robot.type` | Stable robot type written into recording schemas | `unitree_g1_29dof` | | `robot.num_actions` | Joint action dimension | `29` | | `robot.xml_path` | MuJoCo XML path | - | | `d435i_rgb` | Fixed RGB camera in the G1 MJCF; use `viewers=[sim2sim,camera]` to display it | - | @@ -196,7 +197,7 @@ same frames produced by `pico_input`. |-------|-------------|---------| | `recording.enabled` | Enable manual HDF5 recording | `false` | | `recording.output_dir` | Dataset root directory | `data/recordings/sim2real_hdf5` | -| `recording.task` | Task string stored with frames | `demo` | +| `recording.task` | Episode task prompt written to `episodes.jsonl` | `demo` | | `recording.fps` | Recording/video clock rate | `30` | | `recording.min_episode_seconds` | Discard saved episodes shorter than this duration | `1.0` | | `recording.record_modes` | Modes that allow recording start and frame writes | `[standing, mocap, arms, pause]` | @@ -207,32 +208,58 @@ same frames produced by `pico_input`. Camera failure behavior is controlled by `input.video.fail_on_error`. -Each saved episode has one `.h5` file under `recording.output_dir/episodes/` -and one compressed MP4 sidecar under -`recording.output_dir/videos//`. The HDF5 episode stores -`frame_index` and `timestamp` arrays, plus `video_path`, `video_fps`, and -`video_frames` root attributes for synchronization. Raw RGB image datasets are -not written. +The recorder creates an editable source dataset: + +```text +recording.output_dir/ +├── schema.json +├── episodes.jsonl +├── data/ +│ └── episode_000000.h5 +└── videos/ + └── d435i_rgb/ + └── episode_000000.mp4 +``` + +`schema.json` contains the FPS, `robot_type`, `hand_type`, and feature +definitions. `robot_type` comes from `robot.type`; `hand_type` is `none` when +hands are disabled, otherwise it is the configured `hands.driver`. +`episodes.jsonl` contains one object per saved episode with `episode_index`, +`frames`, editable `task`, HDF5 path, and video paths. Task prompts can therefore +be relabeled without rewriting HDF5 or MP4 data. Starting another recording run +with the same schema resumes at the next episode index and may use a different +`recording.task`. + +The format is intentionally not compatible with the earlier attribute-based +HDF5 layout. Use an empty `recording.output_dir`; when an existing schema does +not match, the recording worker rejects the dataset and exits without writing +episodes. Recording is non-critical, so the main sim2real control runtime +continues and reports the worker failure. An episode interrupted before its +`episodes.jsonl` entry is committed is discarded on the next recording-worker +startup and does not consume an episode index. HDF5 datasets: ```text frame_index int64[N] timestamp float64[N] -observation.state float32[68] -observation.mode float32[1] -action float32[36] -action.hand float32[12] +observation.state float32[N, 68] +observation.mode int8[N] +action float32[N, 36] +action.hand float32[N, 12] # only when hands are enabled ``` -The root attributes include the Teleopit HDF5 recording format, schema version, -task, fps, frame count, and video sync metadata. +HDF5 files contain only these frame arrays and have no recording metadata root +attributes. RGB frames remain in MP4 and are associated through +`episodes.jsonl`; raw RGB HDF5 datasets are not written. `observation.state` is ordered as `joint_pos(29)`, `joint_vel(29)`, `base_quat_wxyz(4)`, `base_ang_vel(3)`, and `projected_gravity(3)`. `observation.mode` is a numeric categorical: `standing=0`, `mocap=1`, `arms=2`, and `pause=3`. `action` is the current reference qpos: -`root_pos(3) + root_quat_wxyz(4) + joint_pos(29)`. +`root_pos(3) + root_quat_wxyz(4) + reference_joint_pos(29)`. It is the +high-level reference consumed by the motion tracker, not the tracker policy's +raw output or the final joint targets sent to G1. `action.hand` is the latest LinkerHand command from the hand worker: `left_pose(6) + right_pose(6)`, using the SDK's 0-255 pose values. diff --git a/docs/docs/tutorials/pico-sim2real.md b/docs/docs/tutorials/pico-sim2real.md index a352e27e..14a57c8c 100644 --- a/docs/docs/tutorials/pico-sim2real.md +++ b/docs/docs/tutorials/pico-sim2real.md @@ -116,10 +116,13 @@ python scripts/run/run_sim2real.py \ Terminal controls are `R` start episode, `S` save, `D` discard, and `Q` shutdown. `STANDING`, `MOCAP`, `ARMS`, and paused mocap can be recorded; saved episodes cannot be discarded afterward. Episodes are saved as `.h5` files -under `data/recordings/sim2real_hdf5/episodes/`, with compressed MP4 sidecar -videos under `data/recordings/sim2real_hdf5/videos/`. The HDF5 episode stores -`frame_index` and `timestamp` sync arrays plus `observation.state(68)`, -`observation.mode(1)`, `action(36)`, and `action.hand(12)` at 30 Hz. +under `data/recordings/sim2real_hdf5/data/`, with compressed MP4 files under +`data/recordings/sim2real_hdf5/videos/d435i_rgb/`. The dataset-level +`schema.json` records robot/hand types and feature definitions, while +`episodes.jsonl` stores file mappings and the editable task prompt for every +episode. HDF5 stores `frame_index`, `timestamp`, `observation.state(68)`, scalar +`observation.mode`, and the 36D motion-tracker reference `action` at 30 Hz. +When hand control is enabled, it also stores `action.hand(12)`. ## Operator Flow diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md index 84d538d0..c87ece85 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md @@ -25,6 +25,7 @@ sidebar_position: 2 | 字段 | 类型 | 说明 | |---|---|---| +| `type` | str | 写入录制 schema 的稳定机器人类型，G1 为 `unitree_g1_29dof` | | `num_actions` | int | 策略输出的动作维度（即受控关节数） | | `xml_path` | str | MuJoCo MJCF 模型文件路径 | | `d435i_rgb` | camera | G1 MJCF 中的固定 RGB 相机；配合 `viewers=[sim2sim,camera]` 显示画面 | @@ -211,7 +212,7 @@ MuJoCo 窗口显示重定向参考；`sim2sim`、`mocap`、`camera` 和 `all` |---|---|---| | `recording.enabled` | 启用手动 HDF5 录制 | `false` | | `recording.output_dir` | 数据集根目录 | `data/recordings/sim2real_hdf5` | -| `recording.task` | 写入 frame 的任务字符串 | `demo` | +| `recording.task` | 写入 `episodes.jsonl` 的 episode 任务 prompt | `demo` | | `recording.fps` | 录制/视频主时钟频率 | `30` | | `recording.min_episode_seconds` | 保存时短于该时长的 episode 会被丢弃 | `1.0` | | `recording.record_modes` | 允许开始录制和写帧的模式 | `[standing, mocap, arms, pause]` | @@ -222,30 +223,51 @@ MuJoCo 窗口显示重定向参考；`sim2sim`、`mocap`、`camera` 和 `all` 相机失败时的行为由 `input.video.fail_on_error` 控制。 -每个保存的 episode 会在 `recording.output_dir/episodes/` 下写入一个 `.h5` -文件，并在 `recording.output_dir/videos//` 下写入一个压缩 MP4 -sidecar。HDF5 episode 保存 `frame_index` 和 `timestamp` 数组，并在根属性中 -记录 `video_path`、`video_fps` 和 `video_frames` 用于同步。录制不会写入原始 -RGB 图像 dataset。 +录制器会创建一份便于编辑的源数据集： + +```text +recording.output_dir/ +├── schema.json +├── episodes.jsonl +├── data/ +│ └── episode_000000.h5 +└── videos/ + └── d435i_rgb/ + └── episode_000000.mp4 +``` + +`schema.json` 保存 FPS、`robot_type`、`hand_type` 和 feature 定义。 +`robot_type` 来自 `robot.type`；未启用灵巧手时 `hand_type` 为 `none`，否则为 +配置的 `hands.driver`。`episodes.jsonl` 每行对应一个已保存的 episode，包含 +`episode_index`、`frames`、可编辑的 `task`、HDF5 路径和视频路径。因此修改任务 +prompt 不需要重写 HDF5 或 MP4。使用相同 schema 再次启动录制时，会从下一个 +episode index 继续追加，并且可以使用不同的 `recording.task`。 + +该格式有意不兼容之前依赖 HDF5 根属性的布局。请使用空的 +`recording.output_dir`；如果已有 schema 不匹配，录制 worker 会拒绝该数据集并 +退出，不写入 episode。录制属于非关键进程，因此 sim2real 主控制运行时会继续 +运行并报告 worker 故障。在 `episodes.jsonl` 条目提交前中断的 episode 会在下次 +录制 worker 启动时被丢弃，并且不会占用 episode index。 HDF5 datasets： ```text frame_index int64[N] timestamp float64[N] -observation.state float32[68] -observation.mode float32[1] -action float32[36] -action.hand float32[12] +observation.state float32[N, 68] +observation.mode int8[N] +action float32[N, 36] +action.hand float32[N, 12] # 仅启用灵巧手时存在 ``` -根属性包含 Teleopit HDF5 recording format、schema version、task、fps、 -frame count 和视频同步元数据。 +HDF5 文件仅包含上述逐帧数组，不保存录制元数据根属性。RGB 帧保存在 MP4 中， +并通过 `episodes.jsonl` 与 episode 关联；不会写入原始 RGB HDF5 dataset。 `observation.state` 的顺序是 `joint_pos(29)`、`joint_vel(29)`、 `base_quat_wxyz(4)`、`base_ang_vel(3)` 和 `projected_gravity(3)`。 `observation.mode` 是数值类别：`standing=0`、`mocap=1`、 `arms=2`、`pause=3`。`action` 是当前 reference qpos： -`root_pos(3) + root_quat_wxyz(4) + joint_pos(29)`。 +`root_pos(3) + root_quat_wxyz(4) + reference_joint_pos(29)`。它是 motion tracker +消费的高层参考，不是 tracker policy 的原始输出，也不是最终下发给 G1 的关节目标。 `action.hand` 是手部 worker 最新的 LinkerHand 命令： `left_pose(6) + right_pose(6)`，使用 SDK 的 0-255 pose 数值。 diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md index a33c1ea9..a381eaaf 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md @@ -111,11 +111,13 @@ python scripts/run/run_sim2real.py \ 终端控制为：`R` 开始 episode，`S` 保存，`D` 丢弃，`Q` 关闭。可以录制 `STANDING`、`MOCAP`、`ARMS` 和暂停状态的 mocap；已经保存的 episode 不支持再丢弃。 -episode 会保存为 `data/recordings/sim2real_hdf5/episodes/` 下的 `.h5` 文件， -压缩 MP4 sidecar 视频保存在 `data/recordings/sim2real_hdf5/videos/` 下。 -HDF5 episode 以 30 Hz 保存 `frame_index` 和 `timestamp` 同步数组，以及 -`observation.state(68)`、`observation.mode(1)`、`action(36)` 和 -`action.hand(12)`。 +episode 会保存为 `data/recordings/sim2real_hdf5/data/` 下的 `.h5` 文件， +压缩 MP4 视频保存在 `data/recordings/sim2real_hdf5/videos/d435i_rgb/` 下。 +数据集级 `schema.json` 保存机器人/灵巧手类型和 feature 定义， +`episodes.jsonl` 保存每个 episode 的文件映射与可编辑任务 prompt。HDF5 以 +30 Hz 保存 `frame_index`、`timestamp`、`observation.state(68)`、标量 +`observation.mode` 和作为 motion-tracker reference 的 36D `action`。启用灵巧手 +控制时还会保存 `action.hand(12)`。 ## 操作流程 diff --git a/teleopit/configs/robot/g1.yaml b/teleopit/configs/robot/g1.yaml index 014e84f1..4553fd73 100644 --- a/teleopit/configs/robot/g1.yaml +++ b/teleopit/configs/robot/g1.yaml @@ -2,6 +2,7 @@ # Action scale computed as 0.25 * effort_limit / stiffness per actuator. # Do NOT copy from deploy.yaml (contains known waist_yaw scale error). +type: unitree_g1_29dof num_actions: 29 # Inference uses the 167D velcmd_history observation path (General-Tracking-G1). diff --git a/teleopit/constants.py b/teleopit/constants.py index 5a4269f8..2f724dd7 100644 --- a/teleopit/constants.py +++ b/teleopit/constants.py @@ -5,3 +5,37 @@ ROOT_DIM = ROOT_POS_DIM + ROOT_QUAT_DIM # 7: pos(3) + quat_wxyz(4) NUM_JOINTS = 29 # G1 actuated joints FULL_QPOS_DIM = ROOT_DIM + NUM_JOINTS # 36: root + joints + +# Canonical actuator order used by the downloaded g1_29dof.xml, policy output, +# Unitree command path, and sim2real recording schema. +G1_JOINT_NAMES = ( + "left_hip_pitch_joint", + "left_hip_roll_joint", + "left_hip_yaw_joint", + "left_knee_joint", + "left_ankle_pitch_joint", + "left_ankle_roll_joint", + "right_hip_pitch_joint", + "right_hip_roll_joint", + "right_hip_yaw_joint", + "right_knee_joint", + "right_ankle_pitch_joint", + "right_ankle_roll_joint", + "waist_yaw_joint", + "waist_roll_joint", + "waist_pitch_joint", + "left_shoulder_pitch_joint", + "left_shoulder_roll_joint", + "left_shoulder_yaw_joint", + "left_elbow_joint", + "left_wrist_roll_joint", + "left_wrist_pitch_joint", + "left_wrist_yaw_joint", + "right_shoulder_pitch_joint", + "right_shoulder_roll_joint", + "right_shoulder_yaw_joint", + "right_elbow_joint", + "right_wrist_roll_joint", + "right_wrist_pitch_joint", + "right_wrist_yaw_joint", +) diff --git a/teleopit/recording/hdf5.py b/teleopit/recording/hdf5.py index 36849179..8add4edb 100644 --- a/teleopit/recording/hdf5.py +++ b/teleopit/recording/hdf5.py @@ -1,22 +1,24 @@ -"""HDF5 recorder and schema helpers for Teleopit sim2real recording.""" +"""Editable HDF5 dataset writer for Teleopit sim2real recording.""" from __future__ import annotations from dataclasses import dataclass import json import logging +import os from pathlib import Path import re -import time from typing import Any import h5py import numpy as np -from teleopit.constants import FULL_QPOS_DIM, NUM_JOINTS +from teleopit.constants import FULL_QPOS_DIM, G1_JOINT_NAMES, NUM_JOINTS from teleopit.controllers.observation import _quat_rotate_np from teleopit.math_utils import quat_inv_np from teleopit.runtime.common import cfg_get +from teleopit.sim2real.hands.linkerhand_l6 import L6_SDK_JOINT_ORDER +from teleopit.sim2real.hands.linkerhand_o6 import O6_SDK_JOINT_ORDER logger = logging.getLogger(__name__) @@ -29,33 +31,43 @@ FRAME_INDEX_KEY = "frame_index" TIMESTAMP_KEY = "timestamp" STATE_DIM = 68 -MODE_DIM = 1 ACTION_DIM = FULL_QPOS_DIM HAND_ACTION_DIM = 12 DEFAULT_IMAGE_SHAPE = (480, 640, 3) -HDF5_RECORDING_FORMAT = "teleopit_sim2real_recording_hdf5" +HDF5_RECORDING_FORMAT = "teleopit_hdf5" HDF5_RECORDING_VERSION = 1 +DEFAULT_ROBOT_TYPE = "unitree_g1_29dof" +NO_HAND_TYPE = "none" +SUPPORTED_HAND_TYPES = (NO_HAND_TYPE, "linkerhand_l6", "linkerhand_o6") MODE_CODES = { "standing": 0, "mocap": 1, "arms": 2, "pause": 3, } +_EPISODE_HDF5_FILENAME = re.compile(r"episode_\d{6,}\.h5") +_EPISODE_MP4_FILENAME = re.compile(r"episode_\d{6,}\.mp4") @dataclass(frozen=True) class RecordingSchema: + fps: int + robot_type: str + hand_type: str image_key: str image_shape: tuple[int, int, int] state_key: str = STATE_KEY state_dim: int = STATE_DIM mode_key: str = MODE_KEY - mode_dim: int = MODE_DIM action_key: str = ACTION_KEY action_dim: int = ACTION_DIM hand_action_key: str = HAND_ACTION_KEY hand_action_dim: int = HAND_ACTION_DIM + @property + def has_hand_action(self) -> bool: + return self.hand_type != NO_HAND_TYPE + @dataclass(frozen=True) class MP4VideoConfig: @@ -64,13 +76,40 @@ class MP4VideoConfig: pixelformat: str = "yuv420p" -def build_recording_schema(camera_cfg: Any) -> RecordingSchema: - key = str(cfg_get(camera_cfg, "key", IMAGE_KEY)) +def build_recording_schema( + camera_cfg: Any, + *, + fps: int = 30, + robot_type: str = DEFAULT_ROBOT_TYPE, + hand_type: str = NO_HAND_TYPE, +) -> RecordingSchema: + key = str(cfg_get(camera_cfg, "key", IMAGE_KEY)).strip() width = int(cfg_get(camera_cfg, "width", DEFAULT_IMAGE_SHAPE[1])) height = int(cfg_get(camera_cfg, "height", DEFAULT_IMAGE_SHAPE[0])) + parsed_fps = int(fps) + parsed_robot_type = str(robot_type).strip().lower() + parsed_hand_type = str(hand_type).strip().lower() + if not key: + raise ValueError("recording.camera.key must not be empty") if width <= 0 or height <= 0: raise ValueError("recording.camera.width and recording.camera.height must be positive") - return RecordingSchema(image_key=key, image_shape=(height, width, 3)) + if parsed_fps <= 0: + raise ValueError("recording.fps must be positive") + if parsed_robot_type != DEFAULT_ROBOT_TYPE: + raise ValueError( + f"Unsupported recording robot_type={parsed_robot_type!r}; expected {DEFAULT_ROBOT_TYPE!r}" + ) + if parsed_hand_type not in SUPPORTED_HAND_TYPES: + raise ValueError( + f"Unsupported recording hand_type={parsed_hand_type!r}; expected one of {SUPPORTED_HAND_TYPES}" + ) + return RecordingSchema( + fps=parsed_fps, + robot_type=parsed_robot_type, + hand_type=parsed_hand_type, + image_key=key, + image_shape=(height, width, 3), + ) def build_mp4_video_config(video_cfg: Any) -> MP4VideoConfig: @@ -84,78 +123,68 @@ def build_mp4_video_config(video_cfg: Any) -> MP4VideoConfig: ) -def hdf5_schema( - schema: RecordingSchema, - *, - video_config: MP4VideoConfig | None = None, -) -> dict[str, object]: - video_cfg = video_config or MP4VideoConfig() - return { - "format": HDF5_RECORDING_FORMAT, - "version": HDF5_RECORDING_VERSION, - "features": { - schema.image_key: { - "type": "video", - "format": "mp4", - "codec": video_cfg.codec, - "shape": list(schema.image_shape), - "dtype": "uint8", - "sync": { - "frame_index": FRAME_INDEX_KEY, - "timestamp": TIMESTAMP_KEY, - }, - }, - FRAME_INDEX_KEY: { - "type": "index", - "shape": [], - "dtype": "int64", - }, - TIMESTAMP_KEY: { - "type": "timestamp", - "shape": [], - "dtype": "float64", - "units": "seconds", - }, - schema.state_key: { - "type": "low_dim", - "shape": [schema.state_dim], - "dtype": "float32", - "slices": { - "joint_pos": [0, 29], - "joint_vel": [29, 58], - "base_quat_wxyz": [58, 62], - "base_ang_vel": [62, 65], - "projected_gravity": [65, 68], - }, - }, - schema.mode_key: { - "type": "categorical", - "shape": [schema.mode_dim], - "dtype": "float32", - "codes": MODE_CODES, - }, - schema.action_key: { - "type": "low_dim", - "shape": [schema.action_dim], - "dtype": "float32", - "slices": { - "root_pos": [0, 3], - "root_quat_wxyz": [3, 7], - "joint_pos": [7, 36], - }, +def hdf5_schema(schema: RecordingSchema) -> dict[str, object]: + features: dict[str, object] = { + FRAME_INDEX_KEY: { + "dtype": "int64", + "shape": [], + }, + TIMESTAMP_KEY: { + "dtype": "float64", + "shape": [], + "units": "seconds", + }, + schema.state_key: { + "dtype": "float32", + "shape": [schema.state_dim], + "names": _state_names(), + "groups": { + "joint_pos": [0, 29], + "joint_vel": [29, 58], + "base_quat_wxyz": [58, 62], + "base_ang_vel": [62, 65], + "projected_gravity": [65, 68], }, - schema.hand_action_key: { - "type": "low_dim", - "shape": [schema.hand_action_dim], - "dtype": "float32", - "units": "linkerhand_uint8_pose", - "slices": { - "left_pose": [0, 6], - "right_pose": [6, 12], - }, + }, + schema.mode_key: { + "dtype": "int8", + "shape": [], + "values": MODE_CODES, + }, + schema.action_key: { + "dtype": "float32", + "shape": [schema.action_dim], + "names": _reference_action_names(), + "groups": { + "root_pos": [0, 3], + "root_quat_wxyz": [3, 7], + "reference_joint_pos": [7, 36], }, }, } + if schema.has_hand_action: + features[schema.hand_action_key] = { + "dtype": "float32", + "shape": [schema.hand_action_dim], + "names": _hand_action_names(schema.hand_type), + "groups": { + "left_hand_target": [0, 6], + "right_hand_target": [6, 12], + }, + } + features[schema.image_key] = { + "dtype": "video", + "shape": list(schema.image_shape), + "names": ["height", "width", "channel"], + } + return { + "format": HDF5_RECORDING_FORMAT, + "version": HDF5_RECORDING_VERSION, + "fps": schema.fps, + "robot_type": schema.robot_type, + "hand_type": schema.hand_type, + "features": features, + } def build_observation_state(robot_state: object) -> np.ndarray: @@ -183,7 +212,7 @@ def build_observation_state(robot_state: object) -> np.ndarray: def normalize_action_reference_qpos(reference_qpos: object) -> np.ndarray: - action = np.asarray(reference_qpos, dtype=np.float32).reshape(-1)[:ACTION_DIM] + action = np.asarray(reference_qpos, dtype=np.float32).reshape(-1) if action.shape[0] != ACTION_DIM: raise ValueError(f"recording action reference qpos must be {ACTION_DIM}D, got {action.shape[0]}") return action @@ -202,41 +231,44 @@ def normalize_hand_action(left_pose: object, right_pose: object) -> np.ndarray: return action -def build_mode_observation(mode: str) -> np.ndarray: +def build_mode_observation(mode: str) -> np.int8: normalized = str(mode).strip().lower() if normalized not in MODE_CODES: raise ValueError(f"Unsupported recording mode {mode!r}; expected one of {sorted(MODE_CODES)}") - return np.array([MODE_CODES[normalized]], dtype=np.float32) + return np.int8(MODE_CODES[normalized]) class TeleopitHDF5Recorder: - """Writes one HDF5 file per saved sim2real recording episode.""" + """Writes editable per-episode HDF5 and MP4 files plus a JSONL manifest.""" def __init__( self, *, output_dir: Path, task: str, - fps: int, schema: RecordingSchema, video_config: MP4VideoConfig | None = None, ) -> None: self._output_dir = output_dir - self._task = str(task) - self._fps = int(fps) + self._task = str(task).strip() self._schema = schema + self._fps = schema.fps self._video_config = video_config or MP4VideoConfig() self._active = False self._frames_in_episode = 0 - self._episode_index = 0 + self._next_episode_index = 0 + self._active_episode_index: int | None = None self._h5: h5py.File | None = None self._tmp_path: Path | None = None self._episode_path: Path | None = None self._tmp_video_path: Path | None = None self._episode_video_path: Path | None = None + self._data_rel_path: str | None = None self._video_rel_path: str | None = None self._video_writer: Any | None = None self._datasets: dict[str, h5py.Dataset] = {} + if not self._task: + raise ValueError("recording.task must not be empty") @classmethod def create( @@ -244,7 +276,6 @@ def create( *, output_dir: str | Path, task: str, - fps: int, schema: RecordingSchema, video_config: MP4VideoConfig | None = None, ) -> "TeleopitHDF5Recorder": @@ -253,36 +284,39 @@ def create( recorder = cls( output_dir=root, task=task, - fps=fps, schema=schema, video_config=video_config, ) - recorder._write_schema_sidecar() + recorder._initialize_dataset() return recorder def start_episode(self) -> None: if self._active: raise RuntimeError("Cannot start a new recording episode while one is active") - self._episode_index += 1 - timestamp = time.strftime("%Y%m%d_%H%M%S") - stem = f"episode_{timestamp}_{time.time_ns()}_{self._episode_index:06d}" - tmp_dir = self._output_dir / ".tmp" - episodes_dir = self._output_dir / "episodes" - tmp_dir.mkdir(parents=True, exist_ok=True) - episodes_dir.mkdir(parents=True, exist_ok=True) - self._tmp_path = tmp_dir / f"{stem}.h5" - self._episode_path = episodes_dir / f"{stem}.h5" - self._h5 = h5py.File(self._tmp_path, "w") - video_dir = self._output_dir / "videos" / _safe_path_component(self._schema.image_key) - tmp_video_dir = tmp_dir / "videos" / _safe_path_component(self._schema.image_key) - video_dir.mkdir(parents=True, exist_ok=True) - tmp_video_dir.mkdir(parents=True, exist_ok=True) + episode_index = self._next_episode_index + stem = f"episode_{episode_index:06d}" + data_dir = self._output_dir / "data" + video_storage_key = _video_storage_key(self._schema.image_key) + video_dir = self._output_dir / "videos" / video_storage_key + tmp_data_dir = self._output_dir / ".tmp" / "data" + tmp_video_dir = self._output_dir / ".tmp" / "videos" / video_storage_key + for path in (data_dir, video_dir, tmp_data_dir, tmp_video_dir): + path.mkdir(parents=True, exist_ok=True) + + self._tmp_path = tmp_data_dir / f"{stem}.h5" + self._episode_path = data_dir / f"{stem}.h5" self._tmp_video_path = tmp_video_dir / f"{stem}.mp4" self._episode_video_path = video_dir / f"{stem}.mp4" + self._data_rel_path = self._episode_path.relative_to(self._output_dir).as_posix() self._video_rel_path = self._episode_video_path.relative_to(self._output_dir).as_posix() + self._active_episode_index = episode_index + if self._episode_path.exists() or self._episode_video_path.exists(): + self._reset_episode() + raise FileExistsError(f"Recording episode {stem} already exists") + try: + self._h5 = h5py.File(self._tmp_path, "w") self._video_writer = self._create_video_writer(self._tmp_video_path) - self._write_episode_header(self._h5) self._datasets = self._create_datasets(self._h5) self._active = True self._frames_in_episode = 0 @@ -295,10 +329,9 @@ def add_frame( *, image: np.ndarray, state: np.ndarray, - mode: np.ndarray, + mode: object, action: np.ndarray, - hand_action: np.ndarray, - task: str, + hand_action: np.ndarray | None = None, ) -> None: if not self._active or self._h5 is None: raise RuntimeError("Cannot add a recording frame without an active episode") @@ -306,9 +339,19 @@ def add_frame( if tuple(image_arr.shape) != self._schema.image_shape: raise ValueError(f"{self._schema.image_key} frame shape {image_arr.shape} != {self._schema.image_shape}") state_arr = self._validate_vector(state, self._schema.state_key, self._schema.state_dim) - mode_arr = self._validate_vector(mode, self._schema.mode_key, self._schema.mode_dim) + mode_value = self._validate_mode(mode) action_arr = self._validate_vector(action, self._schema.action_key, self._schema.action_dim) - hand_action_arr = self._validate_vector(hand_action, self._schema.hand_action_key, self._schema.hand_action_dim) + hand_action_arr: np.ndarray | None = None + if self._schema.has_hand_action: + if hand_action is None: + raise ValueError(f"{self._schema.hand_action_key} is required for hand_type={self._schema.hand_type}") + hand_action_arr = self._validate_vector( + hand_action, + self._schema.hand_action_key, + self._schema.hand_action_dim, + ) + elif hand_action is not None: + raise ValueError(f"{self._schema.hand_action_key} must be omitted for hand_type={NO_HAND_TYPE}") row = self._frames_in_episode for dataset in self._datasets.values(): @@ -319,12 +362,11 @@ def add_frame( self._datasets[FRAME_INDEX_KEY][row] = row self._datasets[TIMESTAMP_KEY][row] = float(row) / float(self._fps) self._datasets[self._schema.state_key][row] = state_arr - self._datasets[self._schema.mode_key][row] = mode_arr + self._datasets[self._schema.mode_key][row] = mode_value self._datasets[self._schema.action_key][row] = action_arr - self._datasets[self._schema.hand_action_key][row] = hand_action_arr + if hand_action_arr is not None: + self._datasets[self._schema.hand_action_key][row] = hand_action_arr self._frames_in_episode += 1 - self._h5.attrs["frames"] = self._frames_in_episode - self._h5.attrs["task"] = str(task) def save_episode(self) -> None: if not self._active: @@ -333,11 +375,38 @@ def save_episode(self) -> None: episode_path = self._require_episode_path() tmp_video_path = self._tmp_video_path episode_video_path = self._episode_video_path + episode_index = self._active_episode_index + data_rel_path = self._data_rel_path + video_rel_path = self._video_rel_path + frames = self._frames_in_episode self._close_active_outputs() - if tmp_video_path is None or episode_video_path is None: - raise RuntimeError("recording episode has no video output path") - tmp_video_path.replace(episode_video_path) - tmp_path.replace(episode_path) + if ( + tmp_video_path is None + or episode_video_path is None + or episode_index is None + or data_rel_path is None + or video_rel_path is None + ): + raise RuntimeError("recording episode paths are incomplete") + try: + tmp_video_path.replace(episode_video_path) + tmp_path.replace(episode_path) + self._append_manifest_entry( + { + "episode_index": episode_index, + "frames": frames, + "task": self._task, + "data": data_rel_path, + "videos": {self._schema.image_key: video_rel_path}, + } + ) + except Exception: + for path in (tmp_path, tmp_video_path, episode_path, episode_video_path): + if path.exists(): + path.unlink() + self._reset_episode() + raise + self._next_episode_index += 1 self._reset_episode() def discard_episode(self) -> None: @@ -346,39 +415,176 @@ def discard_episode(self) -> None: tmp_path = self._tmp_path tmp_video_path = self._tmp_video_path self._close_active_outputs() - if tmp_path is not None and tmp_path.exists(): - tmp_path.unlink() - if tmp_video_path is not None and tmp_video_path.exists(): - tmp_video_path.unlink() + for path in (tmp_path, tmp_video_path): + if path is not None and path.exists(): + path.unlink() self._reset_episode() def finalize(self) -> None: if self._active: self.discard_episode() - def _write_schema_sidecar(self) -> None: - path = self._output_dir / "schema.json" - path.write_text(json.dumps(self._schema_dict(), indent=2) + "\n", encoding="utf-8") - - def _write_episode_header(self, h5: h5py.File) -> None: - h5.attrs["format"] = HDF5_RECORDING_FORMAT - h5.attrs["version"] = HDF5_RECORDING_VERSION - h5.attrs["task"] = self._task - h5.attrs["fps"] = self._fps - h5.attrs["frames"] = 0 - h5.attrs["schema_json"] = json.dumps(self._schema_dict(), sort_keys=True) - h5.attrs["video_key"] = self._schema.image_key - h5.attrs["video_path"] = self._video_rel_path or "" - h5.attrs["video_format"] = "mp4" - h5.attrs["video_codec"] = self._video_config.codec - h5.attrs["video_pixelformat"] = self._video_config.pixelformat - h5.attrs["video_fps"] = self._fps - h5.attrs["video_frames"] = 0 - h5.attrs["video_from_timestamp_s"] = 0.0 - h5.attrs["video_to_timestamp_s"] = 0.0 + def _initialize_dataset(self) -> None: + schema_path = self._output_dir / "schema.json" + expected_schema = self._schema_dict() + if schema_path.exists(): + try: + existing_schema = json.loads(schema_path.read_text(encoding="utf-8")) + except (OSError, json.JSONDecodeError) as exc: + raise ValueError(f"Invalid recording schema: {schema_path}") from exc + if existing_schema != expected_schema: + raise ValueError( + f"Recording schema mismatch at {schema_path}; use an empty output_dir for the new dataset" + ) + else: + if self._has_recorded_payload(): + raise ValueError( + f"Recording output {self._output_dir} contains data without schema.json; use an empty output_dir" + ) + schema_path.write_text( + json.dumps(expected_schema, indent=2, ensure_ascii=False) + "\n", + encoding="utf-8", + ) + + manifest_path = self._manifest_path + if not manifest_path.exists(): + if self._has_recorded_payload(): + raise ValueError( + f"Recording output {self._output_dir} contains data without episodes.jsonl; use an empty output_dir" + ) + manifest_path.touch() + entries = self._read_manifest_entries() + self._discard_uncommitted_episode_files(entries) + self._next_episode_index = len(entries) + + @property + def _manifest_path(self) -> Path: + return self._output_dir / "episodes.jsonl" + + def _has_recorded_payload(self) -> bool: + for dirname in ("data", "videos", "episodes"): + path = self._output_dir / dirname + if path.exists() and any(item.is_file() for item in path.rglob("*")): + return True + return False + + def _read_manifest_entries(self) -> list[dict[str, object]]: + entries: list[dict[str, object]] = [] + for line_number, raw_line in enumerate( + self._manifest_path.read_text(encoding="utf-8").splitlines(), + start=1, + ): + line = raw_line.strip() + if not line: + continue + try: + entry = json.loads(line) + except json.JSONDecodeError as exc: + raise ValueError(f"Invalid JSON in {self._manifest_path}:{line_number}") from exc + if not isinstance(entry, dict): + raise ValueError(f"Episode entry in {self._manifest_path}:{line_number} must be an object") + expected_index = len(entries) + if entry.get("episode_index") != expected_index: + raise ValueError( + f"Episode indices in {self._manifest_path} must be contiguous from 0; " + f"line {line_number} expected {expected_index}, got {entry.get('episode_index')!r}" + ) + data_path = entry.get("data") + videos = entry.get("videos") + if not isinstance(data_path, str) or not isinstance(videos, dict): + raise ValueError(f"Episode entry in {self._manifest_path}:{line_number} has invalid paths") + referenced_paths = [data_path, *[str(path) for path in videos.values()]] + for relative_path in referenced_paths: + if not (self._output_dir / relative_path).is_file(): + raise ValueError( + f"Episode entry in {self._manifest_path}:{line_number} references missing file {relative_path!r}" + ) + entries.append(entry) + return entries + + def _append_manifest_entry(self, entry: dict[str, object]) -> None: + current = self._manifest_path.read_bytes() + if current and not current.endswith(b"\n"): + current += b"\n" + encoded_entry = ( + json.dumps(entry, ensure_ascii=False, separators=(",", ":")) + "\n" + ).encode("utf-8") + tmp_path = self._output_dir / ".tmp" / "episodes.jsonl" + tmp_path.parent.mkdir(parents=True, exist_ok=True) + try: + with tmp_path.open("wb") as handle: + handle.write(current) + handle.write(encoded_entry) + handle.flush() + os.fsync(handle.fileno()) + tmp_path.replace(self._manifest_path) + except Exception: + if tmp_path.exists(): + tmp_path.unlink() + raise + + def _discard_uncommitted_episode_files(self, entries: list[dict[str, object]]) -> None: + committed_paths: set[Path] = set() + for entry in entries: + data_path = entry.get("data") + videos = entry.get("videos") + if isinstance(data_path, str): + committed_paths.add((self._output_dir / data_path).resolve()) + if isinstance(videos, dict): + committed_paths.update( + (self._output_dir / str(relative_path)).resolve() + for relative_path in videos.values() + ) + + video_storage_key = _video_storage_key(self._schema.image_key) + final_candidates = [ + *self._matching_episode_files( + self._output_dir / "data", + _EPISODE_HDF5_FILENAME, + ), + *self._matching_episode_files( + self._output_dir / "videos" / video_storage_key, + _EPISODE_MP4_FILENAME, + ), + ] + tmp_candidates = [ + *self._matching_episode_files( + self._output_dir / ".tmp" / "data", + _EPISODE_HDF5_FILENAME, + ), + *self._matching_episode_files( + self._output_dir / ".tmp" / "videos" / video_storage_key, + _EPISODE_MP4_FILENAME, + ), + self._output_dir / ".tmp" / "episodes.jsonl", + ] + for path in final_candidates: + if path.resolve() not in committed_paths: + self._discard_interrupted_artifact(path) + for path in tmp_candidates: + if path.is_file(): + self._discard_interrupted_artifact(path) + + @staticmethod + def _matching_episode_files(directory: Path, pattern: re.Pattern[str]) -> list[Path]: + if not directory.is_dir(): + return [] + return [ + path + for path in directory.iterdir() + if path.is_file() and pattern.fullmatch(path.name) + ] + + @staticmethod + def _discard_interrupted_artifact(path: Path) -> None: + try: + path.unlink() + except OSError as exc: + raise RuntimeError(f"Failed to discard interrupted recording artifact: {path}") from exc + logger.warning("Discarded interrupted recording artifact: %s", path) def _create_datasets(self, h5: h5py.File) -> dict[str, h5py.Dataset]: - return { + datasets = { FRAME_INDEX_KEY: h5.create_dataset( FRAME_INDEX_KEY, shape=(0,), @@ -393,13 +599,31 @@ def _create_datasets(self, h5: h5py.File) -> dict[str, h5py.Dataset]: chunks=(1024,), dtype=np.float64, ), - self._schema.state_key: self._create_vector_dataset(h5, self._schema.state_key, self._schema.state_dim), - self._schema.mode_key: self._create_vector_dataset(h5, self._schema.mode_key, self._schema.mode_dim), - self._schema.action_key: self._create_vector_dataset(h5, self._schema.action_key, self._schema.action_dim), - self._schema.hand_action_key: self._create_vector_dataset( - h5, self._schema.hand_action_key, self._schema.hand_action_dim + self._schema.state_key: self._create_vector_dataset( + h5, + self._schema.state_key, + self._schema.state_dim, + ), + self._schema.mode_key: h5.create_dataset( + self._schema.mode_key, + shape=(0,), + maxshape=(None,), + chunks=(1024,), + dtype=np.int8, + ), + self._schema.action_key: self._create_vector_dataset( + h5, + self._schema.action_key, + self._schema.action_dim, ), } + if self._schema.has_hand_action: + datasets[self._schema.hand_action_key] = self._create_vector_dataset( + h5, + self._schema.hand_action_key, + self._schema.hand_action_dim, + ) + return datasets @staticmethod def _create_vector_dataset(h5: h5py.File, key: str, dim: int) -> h5py.Dataset: @@ -418,11 +642,18 @@ def _validate_vector(value: object, key: str, dim: int) -> np.ndarray: raise ValueError(f"{key} must be {dim}D") return arr + @staticmethod + def _validate_mode(value: object) -> np.int8: + arr = np.asarray(value).reshape(-1) + if arr.shape[0] != 1: + raise ValueError(f"{MODE_KEY} must be scalar") + parsed = int(arr[0]) + if parsed not in MODE_CODES.values(): + raise ValueError(f"{MODE_KEY} must be one of {sorted(MODE_CODES.values())}, got {parsed}") + return np.int8(parsed) + def _schema_dict(self) -> dict[str, object]: - return hdf5_schema( - self._schema, - video_config=self._video_config, - ) + return hdf5_schema(self._schema) def _create_video_writer(self, path: Path) -> Any: try: @@ -443,26 +674,23 @@ def _close_active_outputs(self) -> None: self._video_writer.close() self._video_writer = None if self._h5 is not None: - self._h5.attrs["video_frames"] = self._frames_in_episode - self._h5.attrs["video_to_timestamp_s"] = ( - float(max(self._frames_in_episode - 1, 0)) / float(self._fps) - if self._frames_in_episode > 0 - else 0.0 - ) - self._h5.attrs["video_path"] = self._video_rel_path or "" self._h5.close() - self._h5 = None + self._h5 = None self._datasets = {} def _reset_episode(self) -> None: self._active = False self._frames_in_episode = 0 + self._active_episode_index = None + self._h5 = None self._tmp_path = None self._episode_path = None self._tmp_video_path = None self._episode_video_path = None + self._data_rel_path = None self._video_rel_path = None self._video_writer = None + self._datasets = {} def _cleanup_partial_episode(self) -> None: if self._video_writer is not None: @@ -477,19 +705,12 @@ def _cleanup_partial_episode(self) -> None: except Exception: logger.exception("Failed to close partial HDF5 recording file") self._h5 = None - tmp_path = self._tmp_path - tmp_video_path = self._tmp_video_path - if tmp_path is not None and tmp_path.exists(): - try: - tmp_path.unlink() - except Exception: - logger.exception("Failed to remove partial HDF5 recording file: %s", tmp_path) - if tmp_video_path is not None and tmp_video_path.exists(): - try: - tmp_video_path.unlink() - except Exception: - logger.exception("Failed to remove partial MP4 recording file: %s", tmp_video_path) - self._datasets = {} + for path in (self._tmp_path, self._tmp_video_path): + if path is not None and path.exists(): + try: + path.unlink() + except Exception: + logger.exception("Failed to remove partial recording file: %s", path) self._reset_episode() def _require_tmp_path(self) -> Path: @@ -503,6 +724,50 @@ def _require_episode_path(self) -> Path: return self._episode_path -def _safe_path_component(value: str) -> str: - safe = re.sub(r"[^A-Za-z0-9._-]+", "_", value).strip("._") +def _state_names() -> list[str]: + return [ + *[f"{name}.position" for name in G1_JOINT_NAMES], + *[f"{name}.velocity" for name in G1_JOINT_NAMES], + "base_quat.w", + "base_quat.x", + "base_quat.y", + "base_quat.z", + "base_ang_vel.x", + "base_ang_vel.y", + "base_ang_vel.z", + "projected_gravity.x", + "projected_gravity.y", + "projected_gravity.z", + ] + + +def _reference_action_names() -> list[str]: + return [ + "root_pos.x", + "root_pos.y", + "root_pos.z", + "root_quat.w", + "root_quat.x", + "root_quat.y", + "root_quat.z", + *G1_JOINT_NAMES, + ] + + +def _hand_action_names(hand_type: str) -> list[str]: + if hand_type == "linkerhand_l6": + joint_order = L6_SDK_JOINT_ORDER + elif hand_type == "linkerhand_o6": + joint_order = O6_SDK_JOINT_ORDER + else: + raise ValueError(f"hand action names are unavailable for hand_type={hand_type!r}") + return [ + *[f"left_{name}" for name in joint_order], + *[f"right_{name}" for name in joint_order], + ] + + +def _video_storage_key(value: str) -> str: + leaf = value.rsplit(".", 1)[-1] + safe = re.sub(r"[^A-Za-z0-9._-]+", "_", leaf).strip("._") return safe or "camera" diff --git a/teleopit/sim2real/mp/messages.py b/teleopit/sim2real/mp/messages.py index a56bb1e7..b2ee9c1c 100644 --- a/teleopit/sim2real/mp/messages.py +++ b/teleopit/sim2real/mp/messages.py @@ -66,7 +66,7 @@ class RecordStepPacket: mocap_active: bool recordable: bool observation_state: Float64Array - observation_mode: Float64Array + observation_mode: int action_reference_qpos: Float64Array seq: int diff --git a/teleopit/sim2real/mp/runtime.py b/teleopit/sim2real/mp/runtime.py index 700e619d..3f269c2b 100644 --- a/teleopit/sim2real/mp/runtime.py +++ b/teleopit/sim2real/mp/runtime.py @@ -43,9 +43,12 @@ from teleopit.runtime.reference_config import parse_reference_config from teleopit.runtime.terminal_keyboard import TerminalKeyboardReader from teleopit.recording.hdf5 import ( + DEFAULT_ROBOT_TYPE, + NO_HAND_TYPE, build_mode_observation, build_mp4_video_config, build_observation_state, + build_recording_schema, normalize_hand_action, normalize_action_reference_qpos, ) @@ -283,6 +286,18 @@ def _recording_camera_cfg(cfg: Any) -> Any: return cfg_get(_recording_cfg(cfg), "camera", {}) or {} +def _recording_robot_and_hand_types(cfg: Any) -> tuple[str, str]: + robot_cfg = cfg_get(cfg, "robot", {}) or {} + robot_type = str(cfg_get(robot_cfg, "type", DEFAULT_ROBOT_TYPE)).strip().lower() + hands_cfg = cfg_get(cfg, "hands", {}) or {} + hand_type = ( + str(cfg_get(hands_cfg, "driver", "linkerhand_l6")).strip().lower() + if bool(cfg_get(hands_cfg, "enabled", False)) + else NO_HAND_TYPE + ) + return robot_type, hand_type + + def _configured_open_hand_pose(cfg: Any) -> tuple[np.ndarray, np.ndarray]: hands_cfg = cfg_get(cfg, "hands", {}) or {} driver = str(cfg_get(hands_cfg, "driver", "linkerhand_l6")).strip().lower() @@ -338,6 +353,13 @@ def _validate_new_runtime_config(cfg: Any) -> None: raise ValueError("recording.camera.source must be realsense") if int(cfg_get(rec_cfg, "fps", 30)) != int(cfg_get(camera_cfg, "fps", 30)): raise ValueError("recording.fps must match recording.camera.fps") + robot_type, hand_type = _recording_robot_and_hand_types(cfg) + build_recording_schema( + camera_cfg, + fps=int(cfg_get(rec_cfg, "fps", 30)), + robot_type=robot_type, + hand_type=hand_type, + ) input_video = parse_pico_video_config(cfg_get(cfg, "input", {}) or {}) if not input_video.enabled: raise ValueError("recording.enabled=true requires input.video.enabled=true") @@ -1720,7 +1742,7 @@ def _publish_record_step(self, *, robot_state: object, reference_qpos: Float64Ar mocap_active=active, recordable=recordable, observation_state=build_observation_state(robot_state).astype(np.float32, copy=True), - observation_mode=build_mode_observation(record_mode).astype(np.float32, copy=True), + observation_mode=int(build_mode_observation(record_mode)), action_reference_qpos=normalize_action_reference_qpos(reference_qpos).astype(np.float32, copy=True), seq=self._mode_seq, ), @@ -1742,7 +1764,7 @@ def _publish_damping_record_step(self) -> None: mocap_active=False, recordable=False, observation_state=build_observation_state(robot_state).astype(np.float32, copy=True), - observation_mode=np.array([-1.0], dtype=np.float32), + observation_mode=-1, action_reference_qpos=normalize_action_reference_qpos(reference_qpos).astype(np.float32, copy=True), seq=self._mode_seq, ), @@ -1859,18 +1881,20 @@ def __init__( self._episode_started_s = 0.0 self._episode_frames = 0 - from teleopit.recording.hdf5 import ( - TeleopitHDF5Recorder, - build_recording_schema, - ) + from teleopit.recording.hdf5 import TeleopitHDF5Recorder - self._schema = build_recording_schema(self.camera_cfg) + robot_type, hand_type = _recording_robot_and_hand_types(cfg) + self._schema = build_recording_schema( + self.camera_cfg, + fps=self.fps, + robot_type=robot_type, + hand_type=hand_type, + ) self._video_config = build_mp4_video_config(cfg_get(self.rec_cfg, "video", {}) or {}) factory = recorder_factory or TeleopitHDF5Recorder.create self._recorder = factory( output_dir=cfg_get(self.rec_cfg, "output_dir", "data/recordings/sim2real_hdf5"), task=self.task, - fps=self.fps, schema=self._schema, video_config=self._video_config, ) @@ -1988,16 +2012,20 @@ def _handle_video(self, descriptor: SharedFrameDescriptor) -> None: self._discard_episode("mode not recordable") return image = self._frame_reader.read(descriptor, copy=True) + hand_action = ( + normalize_hand_action( + self._latest_hand_command.left_pose, + self._latest_hand_command.right_pose, + ) + if self._schema.has_hand_action + else None + ) self._recorder.add_frame( image=np.asarray(image, dtype=np.uint8), state=np.asarray(record.observation_state, dtype=np.float32), - mode=np.asarray(record.observation_mode, dtype=np.float32), + mode=record.observation_mode, action=np.asarray(record.action_reference_qpos, dtype=np.float32), - hand_action=normalize_hand_action( - self._latest_hand_command.left_pose, - self._latest_hand_command.right_pose, - ), - task=self.task, + hand_action=hand_action, ) self._episode_frames += 1 diff --git a/tests/test_sim2real_multiprocess.py b/tests/test_sim2real_multiprocess.py index 58c4b16d..e82f2b15 100644 --- a/tests/test_sim2real_multiprocess.py +++ b/tests/test_sim2real_multiprocess.py @@ -1,8 +1,10 @@ from __future__ import annotations import importlib.util +import json import logging from pathlib import Path +import shutil from types import SimpleNamespace import h5py @@ -383,36 +385,59 @@ def test_recording_key_mapping() -> None: def test_hdf5_recording_schema() -> None: - schema = build_recording_schema({"width": 640, "height": 480, "key": IMAGE_KEY}) + schema = build_recording_schema( + {"width": 640, "height": 480, "key": IMAGE_KEY}, + fps=30, + robot_type="unitree_g1_29dof", + hand_type="linkerhand_o6", + ) sidecar = hdf5_schema(schema) features = sidecar["features"] assert sidecar["format"] == HDF5_RECORDING_FORMAT - assert features[IMAGE_KEY]["type"] == "video" - assert features[IMAGE_KEY]["format"] == "mp4" + assert sidecar["fps"] == 30 + assert sidecar["robot_type"] == "unitree_g1_29dof" + assert sidecar["hand_type"] == "linkerhand_o6" + assert features[IMAGE_KEY]["dtype"] == "video" assert features[IMAGE_KEY]["shape"] == [480, 640, 3] assert features[FRAME_INDEX_KEY]["dtype"] == "int64" assert features[TIMESTAMP_KEY]["dtype"] == "float64" assert features[STATE_KEY]["shape"] == [68] - assert features[MODE_KEY]["shape"] == [1] + assert features[MODE_KEY]["shape"] == [] + assert features[MODE_KEY]["dtype"] == "int8" assert features[ACTION_KEY]["shape"] == [36] assert features[HAND_ACTION_KEY]["shape"] == [12] - assert sidecar["features"][STATE_KEY]["slices"]["joint_pos"] == [0, 29] - assert sidecar["features"][STATE_KEY]["slices"]["projected_gravity"] == [65, 68] - assert sidecar["features"][MODE_KEY]["codes"]["pause"] == 3 - assert sidecar["features"][ACTION_KEY]["slices"]["joint_pos"] == [7, 36] - assert sidecar["features"][HAND_ACTION_KEY]["slices"]["left_pose"] == [0, 6] - assert sidecar["features"][HAND_ACTION_KEY]["slices"]["right_pose"] == [6, 12] + assert features[STATE_KEY]["groups"]["joint_pos"] == [0, 29] + assert features[STATE_KEY]["groups"]["projected_gravity"] == [65, 68] + assert features[MODE_KEY]["values"]["pause"] == 3 + assert features[ACTION_KEY]["groups"]["reference_joint_pos"] == [7, 36] + assert features[HAND_ACTION_KEY]["groups"]["left_hand_target"] == [0, 6] + assert features[HAND_ACTION_KEY]["groups"]["right_hand_target"] == [6, 12] + assert len(features[STATE_KEY]["names"]) == 68 + assert len(features[ACTION_KEY]["names"]) == 36 + assert len(features[HAND_ACTION_KEY]["names"]) == 12 + + +def test_hdf5_recording_schema_omits_hand_action_without_hand_hardware() -> None: + schema = build_recording_schema( + {"width": 640, "height": 480, "key": IMAGE_KEY}, + hand_type="none", + ) + + assert schema.has_hand_action is False + assert HAND_ACTION_KEY not in hdf5_schema(schema)["features"] def test_hdf5_recorder_mp4_sidecar_writes_sync_metadata(tmp_path: Path) -> None: from teleopit.recording.hdf5 import MP4VideoConfig, TeleopitHDF5Recorder - schema = build_recording_schema({"width": 2, "height": 2, "key": IMAGE_KEY}) + schema = build_recording_schema( + {"width": 2, "height": 2, "key": IMAGE_KEY}, + hand_type="linkerhand_l6", + ) recorder = TeleopitHDF5Recorder.create( output_dir=tmp_path, task="walk", - fps=30, schema=schema, video_config=MP4VideoConfig(quality=5), ) @@ -425,40 +450,140 @@ def test_hdf5_recorder_mp4_sidecar_writes_sync_metadata(tmp_path: Path) -> None: mode=build_mode_observation("mocap"), action=np.arange(36, dtype=np.float32), hand_action=np.arange(12, dtype=np.float32), - task="walk", ) recorder.save_episode() recorder.finalize() - episodes = sorted((tmp_path / "episodes").glob("*.h5")) - videos = sorted((tmp_path / "videos" / "observation.images.d435i_rgb").glob("*.mp4")) + episodes = sorted((tmp_path / "data").glob("*.h5")) + videos = sorted((tmp_path / "videos" / "d435i_rgb").glob("*.mp4")) assert len(episodes) == 1 assert len(videos) == 1 assert videos[0].stat().st_size > 0 assert (tmp_path / "schema.json").exists() - assert not list((tmp_path / ".tmp").glob("*.h5")) + assert (tmp_path / "episodes.jsonl").exists() + assert not list((tmp_path / ".tmp").rglob("*.h5")) + + manifest = [json.loads(line) for line in (tmp_path / "episodes.jsonl").read_text().splitlines()] + assert manifest == [ + { + "episode_index": 0, + "frames": 2, + "task": "walk", + "data": "data/episode_000000.h5", + "videos": {IMAGE_KEY: "videos/d435i_rgb/episode_000000.mp4"}, + } + ] with h5py.File(episodes[0], "r") as h5: - assert h5.attrs["format"] == HDF5_RECORDING_FORMAT - assert h5.attrs["version"] == 1 - assert h5.attrs["task"] == "walk" - assert h5.attrs["fps"] == 30 - assert h5.attrs["frames"] == 2 - assert h5.attrs["video_path"] == videos[0].relative_to(tmp_path).as_posix() - assert h5.attrs["video_key"] == IMAGE_KEY - assert h5.attrs["video_frames"] == 2 - assert h5.attrs["video_fps"] == 30 + assert dict(h5.attrs) == {} assert IMAGE_KEY not in h5 assert h5[FRAME_INDEX_KEY].shape == (2,) assert h5[TIMESTAMP_KEY].shape == (2,) np.testing.assert_array_equal(h5[FRAME_INDEX_KEY][...], np.array([0, 1], dtype=np.int64)) np.testing.assert_allclose(h5[TIMESTAMP_KEY][...], np.array([0.0, 1.0 / 30.0], dtype=np.float64)) assert h5[STATE_KEY].shape == (2, 68) - assert h5[MODE_KEY].shape == (2, 1) + assert h5[MODE_KEY].shape == (2,) + assert h5[MODE_KEY].dtype == np.dtype(np.int8) assert h5[ACTION_KEY].shape == (2, 36) assert h5[HAND_ACTION_KEY].shape == (2, 12) +def test_hdf5_recorder_resumes_and_keeps_tasks_in_editable_manifest(tmp_path: Path) -> None: + from teleopit.recording.hdf5 import TeleopitHDF5Recorder + + schema = build_recording_schema({"width": 2, "height": 2, "key": IMAGE_KEY}) + + def write_episode(task: str, value: int) -> None: + recorder = TeleopitHDF5Recorder.create(output_dir=tmp_path, task=task, schema=schema) + recorder.start_episode() + recorder.add_frame( + image=np.full((2, 2, 3), value, dtype=np.uint8), + state=np.full(68, value, dtype=np.float32), + mode=build_mode_observation("mocap"), + action=np.full(36, value, dtype=np.float32), + ) + recorder.save_episode() + recorder.finalize() + + write_episode("pick up the box", 1) + first_entry = json.loads((tmp_path / "episodes.jsonl").read_text().strip()) + first_entry["task"] = "pick up the red box" + (tmp_path / "episodes.jsonl").write_text( + json.dumps(first_entry, ensure_ascii=False, separators=(",", ":")), + encoding="utf-8", + ) + + write_episode("把盒子放到桌上", 2) + + entries = [json.loads(line) for line in (tmp_path / "episodes.jsonl").read_text().splitlines()] + assert [entry["episode_index"] for entry in entries] == [0, 1] + assert [entry["task"] for entry in entries] == ["pick up the red box", "把盒子放到桌上"] + assert sorted(path.name for path in (tmp_path / "data").glob("*.h5")) == [ + "episode_000000.h5", + "episode_000001.h5", + ] + with h5py.File(tmp_path / "data" / "episode_000001.h5", "r") as h5: + assert HAND_ACTION_KEY not in h5 + + +def test_hdf5_recorder_discards_uncommitted_episode_files_on_resume(tmp_path: Path) -> None: + from teleopit.recording.hdf5 import TeleopitHDF5Recorder + + schema = build_recording_schema({"width": 2, "height": 2, "key": IMAGE_KEY}) + recorder = TeleopitHDF5Recorder.create(output_dir=tmp_path, task="first", schema=schema) + recorder.start_episode() + recorder.add_frame( + image=np.zeros((2, 2, 3), dtype=np.uint8), + state=np.zeros(68, dtype=np.float32), + mode=build_mode_observation("mocap"), + action=np.zeros(36, dtype=np.float32), + ) + recorder.save_episode() + + orphan_data = tmp_path / "data" / "episode_000001.h5" + orphan_video = tmp_path / "videos" / "d435i_rgb" / "episode_000001.mp4" + shutil.copyfile(tmp_path / "data" / "episode_000000.h5", orphan_data) + shutil.copyfile(tmp_path / "videos" / "d435i_rgb" / "episode_000000.mp4", orphan_video) + tmp_data = tmp_path / ".tmp" / "data" / "episode_000001.h5" + tmp_video = tmp_path / ".tmp" / "videos" / "d435i_rgb" / "episode_000001.mp4" + tmp_data.parent.mkdir(parents=True, exist_ok=True) + tmp_video.parent.mkdir(parents=True, exist_ok=True) + shutil.copyfile(orphan_data, tmp_data) + shutil.copyfile(orphan_video, tmp_video) + + resumed = TeleopitHDF5Recorder.create(output_dir=tmp_path, task="second", schema=schema) + + assert not orphan_data.exists() + assert not orphan_video.exists() + assert not tmp_data.exists() + assert not tmp_video.exists() + resumed.start_episode() + resumed.add_frame( + image=np.ones((2, 2, 3), dtype=np.uint8), + state=np.ones(68, dtype=np.float32), + mode=build_mode_observation("mocap"), + action=np.ones(36, dtype=np.float32), + ) + resumed.save_episode() + + entries = [json.loads(line) for line in (tmp_path / "episodes.jsonl").read_text().splitlines()] + assert [entry["episode_index"] for entry in entries] == [0, 1] + + +def test_hdf5_recorder_rejects_existing_incompatible_schema(tmp_path: Path) -> None: + from teleopit.recording.hdf5 import TeleopitHDF5Recorder + + no_hands = build_recording_schema({"width": 2, "height": 2}, hand_type="none") + TeleopitHDF5Recorder.create(output_dir=tmp_path, task="demo", schema=no_hands).finalize() + with_hands = build_recording_schema( + {"width": 2, "height": 2}, + hand_type="linkerhand_l6", + ) + + with pytest.raises(ValueError, match="schema mismatch"): + TeleopitHDF5Recorder.create(output_dir=tmp_path, task="demo", schema=with_hands) + + def test_hdf5_recorder_cleans_partial_episode_when_video_writer_fails(tmp_path: Path) -> None: from teleopit.recording.hdf5 import TeleopitHDF5Recorder @@ -468,16 +593,16 @@ def _create_video_writer(self, path: Path) -> object: raise RuntimeError("writer failed") schema = build_recording_schema({"width": 2, "height": 2, "key": IMAGE_KEY}) - recorder = FailingVideoRecorder.create(output_dir=tmp_path, task="walk", fps=30, schema=schema) + recorder = FailingVideoRecorder.create(output_dir=tmp_path, task="walk", schema=schema) with pytest.raises(RuntimeError, match="writer failed"): recorder.start_episode() recorder.finalize() - assert not list((tmp_path / ".tmp").glob("*.h5")) - assert not list((tmp_path / ".tmp" / "videos" / "observation.images.d435i_rgb").glob("*.mp4")) - assert not list((tmp_path / "episodes").glob("*.h5")) - assert not list((tmp_path / "videos" / "observation.images.d435i_rgb").glob("*.mp4")) + assert not list((tmp_path / ".tmp").rglob("*.h5")) + assert not list((tmp_path / ".tmp" / "videos" / "d435i_rgb").glob("*.mp4")) + assert not list((tmp_path / "data").glob("*.h5")) + assert not list((tmp_path / "videos" / "d435i_rgb").glob("*.mp4")) def test_hdf5_recorder_keeps_startup_error_when_partial_cleanup_fails(tmp_path: Path) -> None: @@ -495,13 +620,13 @@ def _create_datasets(self, h5: h5py.File) -> dict[str, h5py.Dataset]: raise RuntimeError("startup failed") schema = build_recording_schema({"width": 2, "height": 2, "key": IMAGE_KEY}) - recorder = FailingDatasetRecorder.create(output_dir=tmp_path, task="walk", fps=30, schema=schema) + recorder = FailingDatasetRecorder.create(output_dir=tmp_path, task="walk", schema=schema) with pytest.raises(RuntimeError, match="startup failed"): recorder.start_episode() recorder.finalize() - assert not list((tmp_path / ".tmp").glob("*.h5")) + assert not list((tmp_path / ".tmp").rglob("*.h5")) def test_configured_open_hand_pose_matches_linkerhand_l6_parser() -> None: @@ -1041,9 +1166,8 @@ def test_robot_worker_publish_record_step() -> None: assert packet.mocap_active is True assert packet.recordable is True assert packet.observation_state.shape == (68,) - assert packet.observation_mode.shape == (1,) + assert packet.observation_mode == int(build_mode_observation("arms")) assert packet.action_reference_qpos.shape == (36,) - np.testing.assert_allclose(packet.observation_mode, build_mode_observation("arms")) np.testing.assert_allclose(packet.action_reference_qpos, reference_qpos.astype(np.float32)) @@ -1082,7 +1206,7 @@ def test_robot_worker_enter_damping_publishes_non_recordable_packet() -> None: assert packet.mode == "damping" assert packet.recordable is False assert packet.mocap_active is False - np.testing.assert_allclose(packet.observation_mode, np.array([-1.0], dtype=np.float32)) + assert packet.observation_mode == -1 def test_recording_worker_start_save_discard_with_fake_adapter() -> None: @@ -1100,17 +1224,17 @@ def add_frame( *, image: np.ndarray, state: np.ndarray, - mode: np.ndarray, + mode: object, action: np.ndarray, - hand_action: np.ndarray, - task: str, + hand_action: np.ndarray | None = None, ) -> None: - calls.append(f"frame:{task}") + calls.append("frame") + assert hand_action is not None frames.append( { "image": image.copy(), "state": state.copy(), - "mode": mode.copy(), + "mode": np.asarray(mode).copy(), "action": action.copy(), "hand_action": hand_action.copy(), } @@ -1138,7 +1262,8 @@ def fake_factory(**_kwargs: object) -> FakeRecorder: "fps": 30, "min_episode_seconds": 0.0, "camera": {"width": 2, "height": 2, "key": IMAGE_KEY}, - } + }, + "hands": {"enabled": True, "driver": "linkerhand_l6"}, }, endpoints, stop_event, # type: ignore[arg-type] @@ -1152,7 +1277,7 @@ def fake_factory(**_kwargs: object) -> FakeRecorder: mocap_active=False, recordable=False, observation_state=np.ones(68, dtype=np.float32), - observation_mode=build_mode_observation("standing"), + observation_mode=int(build_mode_observation("standing")), action_reference_qpos=np.ones(36, dtype=np.float32), seq=1, ) @@ -1165,7 +1290,7 @@ def fake_factory(**_kwargs: object) -> FakeRecorder: mocap_active=False, recordable=True, observation_state=np.arange(68, dtype=np.float32), - observation_mode=build_mode_observation("standing"), + observation_mode=int(build_mode_observation("standing")), action_reference_qpos=np.arange(36, dtype=np.float32), seq=2, ) @@ -1187,10 +1312,10 @@ def fake_factory(**_kwargs: object) -> FakeRecorder: worker._handle_video(desc) worker._save_episode() - assert calls == ["start", "discard", "start", "frame:walk", "save"] + assert calls == ["start", "discard", "start", "frame", "save"] assert frames[0]["image"].shape == (2, 2, 3) np.testing.assert_allclose(frames[0]["state"], np.arange(68, dtype=np.float32)) - np.testing.assert_allclose(frames[0]["mode"], build_mode_observation("standing")) + assert int(frames[0]["mode"]) == int(build_mode_observation("standing")) np.testing.assert_allclose(frames[0]["action"], np.arange(36, dtype=np.float32)) np.testing.assert_allclose(frames[0]["hand_action"], np.arange(12, dtype=np.float32)) @@ -1200,7 +1325,7 @@ def fake_factory(**_kwargs: object) -> FakeRecorder: mocap_active=False, recordable=True, observation_state=np.zeros(68, dtype=np.float32), - observation_mode=build_mode_observation("pause"), + observation_mode=int(build_mode_observation("pause")), action_reference_qpos=np.zeros(36, dtype=np.float32), seq=3, ) From a3962736a9677077369fd0ba78449bebded021c6 Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Wed, 15 Jul 2026 15:13:53 +0800 Subject: [PATCH 16/59] feat: record OpenNeck actions in sim2real datasets --- AGENTS.md | 9 +- README.md | 8 +- docs/docs/configuration/config-reference.md | 12 +- docs/docs/tutorials/pico-sim2real.md | 6 +- .../current/configuration/config-reference.md | 9 +- .../current/tutorials/pico-sim2real.md | 5 +- scripts/dev/test_openneck.py | 3 +- teleopit/recording/hdf5.py | 56 ++++++- teleopit/sim2real/mp/ipc.py | 3 + teleopit/sim2real/mp/messages.py | 10 ++ teleopit/sim2real/mp/runtime.py | 106 ++++++++++--- teleopit/sim2real/neck/worker.py | 16 +- tests/test_active_neck.py | 7 +- tests/test_sim2real_multiprocess.py | 143 +++++++++++++++++- 14 files changed, 344 insertions(+), 49 deletions(-) diff --git a/AGENTS.md b/AGENTS.md index 149126aa..70e6dbe4 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -150,9 +150,9 @@ target_dof_pos = clip(action, -10, 10) × action_scale + default_dof_pos - Optional LinkerHand control uses `hands.enabled=true`, `hands.driver=linkerhand_l6|linkerhand_o6`, and `hands.mode=gripper|vr_hand_pose`; default is disabled - Optional Pico sim2real HDF5 recording uses `--config-name sim2real_record` or `recording.enabled=true`; it requires `input.provider=pico4`, `input.video.enabled=true`, `input.video.source=realsense`, an interactive terminal, and the `recording` extra - Recording is manual only: terminal `R` starts an episode, `S` saves, `D` discards the active episode, and `Q` shuts down; `STANDING`, `MOCAP`, `ARMS`, and paused mocap are recordable -- Recording captures `observation.images.d435i_rgb` RealSense RGB video at 30Hz plus `observation.state(68)`, scalar `observation.mode`, and `action(36)` as the root-plus-joint reference consumed by the motion tracker; `action.hand(12)` is present when LinkerHand control is enabled +- Recording captures `observation.images.d435i_rgb` RealSense RGB video at 30Hz plus `observation.state(68)`, scalar `observation.mode`, and `action(36)` as the root-plus-joint reference consumed by the motion tracker; `action.hand(12)` is present when LinkerHand control is enabled, and `action.neck(2)` is present when OpenNeck control is enabled - Sim2real recording uses an editable source layout: `schema.json`, `episodes.jsonl`, per-episode HDF5 files under `recording.output_dir/data/`, and compressed MP4 files under `recording.output_dir/videos/d435i_rgb/`; task prompts live only in `episodes.jsonl`, and HDF5 files contain only frame arrays with no metadata attributes or raw RGB datasets -- Recording `schema.json` stores `robot_type=unitree_g1_29dof`, `hand_type=none|linkerhand_l6|linkerhand_o6`, FPS, and feature definitions; the recording worker rejects an existing mismatched schema without writing episodes, but remains non-critical and must not stop the G1 control runtime; the previous attribute-based HDF5 layout is unsupported +- Recording `schema.json` stores `robot_type=unitree_g1_29dof`, `hand_type=none|linkerhand_l6|linkerhand_o6`, `neck_type=none|openneck`, FPS, and feature definitions; optional action fields are controlled directly by `hands.enabled` and `neck.enabled`; the recording worker rejects an existing mismatched schema without writing episodes, but remains non-critical and must not stop the G1 control runtime; the previous attribute-based HDF5 layout is unsupported - Episodes interrupted before their `episodes.jsonl` entry is committed are discarded on the next recording-worker startup and do not consume an episode index - `gripper` mode reuses `Pico4InputProvider.get_controller_snapshot()` for Pico grip/trigger open-close control and supports LinkerHand L6 and O6 - `vr_hand_pose` mode reuses `Pico4InputProvider.get_hand_snapshot()` and somehand 0.2.0 public `somehand.api` for continuous Pico hand-pose retargeting; do not start a second `PicoBridge` for hand control @@ -294,6 +294,11 @@ Critical note: align robot root orientation to the BVH human forward direction b ## Development +### Simplicity Policy +- Prefer the smallest implementation that satisfies the current requirement +- Do not add speculative configuration switches, abstraction layers, compatibility paths, or extensibility without a concrete use case +- Reuse existing enable flags and data flows when they already express the required behavior + ### Runtime Validation Policy - Fail fast for logical mismatches such as observation definition vs. ONNX signature mismatch - Do not silently pad, trim, clip, or replace invalid data/config to "make it run" diff --git a/README.md b/README.md index e48b07aa..8728d856 100644 --- a/README.md +++ b/README.md @@ -101,12 +101,14 @@ python scripts/run/run_sim2real.py --config-name sim2real_record \ Recording uses the terminal controls `R` start, `S` save, `D` discard, and `Q` shutdown. `STANDING`, `MOCAP`, `ARMS`, and paused mocap can be recorded. Saved episodes are written under `data/recordings/sim2real_hdf5/data/`, with compressed -MP4 files under `videos/d435i_rgb/`. `schema.json` records the FPS, robot and -hand types, feature shapes, names, and groups. `episodes.jsonl` maps each episode +MP4 files under `videos/d435i_rgb/`. `schema.json` records the FPS, robot, hand, +and neck types, plus feature shapes, names, and groups. `episodes.jsonl` maps each episode to its HDF5/video files and stores its editable task prompt. HDF5 contains only frame-aligned arrays: `observation.state(68)`, scalar `observation.mode`, and `action(36)` as the aligned reference qpos consumed by the motion tracker. -`action.hand(12)` is present when LinkerHand control is enabled. +`action.hand(12)` is present exactly when LinkerHand control is enabled, and +`action.neck(2)` contains the latest normalized OpenNeck yaw/pitch command exactly +when OpenNeck control is enabled. Recording is non-critical: an incompatible output schema stops only the recording worker while G1 control continues. Episodes interrupted before their manifest entry is committed are discarded on the next recording startup. diff --git a/docs/docs/configuration/config-reference.md b/docs/docs/configuration/config-reference.md index eb501f64..c029c4b8 100644 --- a/docs/docs/configuration/config-reference.md +++ b/docs/docs/configuration/config-reference.md @@ -221,9 +221,12 @@ recording.output_dir/ └── episode_000000.mp4 ``` -`schema.json` contains the FPS, `robot_type`, `hand_type`, and feature -definitions. `robot_type` comes from `robot.type`; `hand_type` is `none` when -hands are disabled, otherwise it is the configured `hands.driver`. +`schema.json` contains the FPS, `robot_type`, `hand_type`, `neck_type`, and +feature definitions. `robot_type` comes from `robot.type`; `hand_type` is `none` +when hands are disabled, otherwise it is the configured `hands.driver`. +`neck_type` is `none` when active-neck control is disabled, otherwise it is the +configured `neck.driver`. These enabled flags directly control whether their +action fields are recorded; there are no separate recording switches. `episodes.jsonl` contains one object per saved episode with `episode_index`, `frames`, editable `task`, HDF5 path, and video paths. Task prompts can therefore be relabeled without rewriting HDF5 or MP4 data. Starting another recording run @@ -247,6 +250,7 @@ observation.state float32[N, 68] observation.mode int8[N] action float32[N, 36] action.hand float32[N, 12] # only when hands are enabled +action.neck float32[N, 2] # only when OpenNeck is enabled ``` HDF5 files contain only these frame arrays and have no recording metadata root @@ -262,6 +266,8 @@ high-level reference consumed by the motion tracker, not the tracker policy's raw output or the final joint targets sent to G1. `action.hand` is the latest LinkerHand command from the hand worker: `left_pose(6) + right_pose(6)`, using the SDK's 0-255 pose values. +`action.neck` is the latest command successfully sent to OpenNeck by the neck +worker: normalized `[yaw, pitch]`, each in `[-1, 1]`. ## Critical: `default_dof_pos` diff --git a/docs/docs/tutorials/pico-sim2real.md b/docs/docs/tutorials/pico-sim2real.md index 14a57c8c..b06d34c8 100644 --- a/docs/docs/tutorials/pico-sim2real.md +++ b/docs/docs/tutorials/pico-sim2real.md @@ -118,11 +118,13 @@ shutdown. `STANDING`, `MOCAP`, `ARMS`, and paused mocap can be recorded; saved episodes cannot be discarded afterward. Episodes are saved as `.h5` files under `data/recordings/sim2real_hdf5/data/`, with compressed MP4 files under `data/recordings/sim2real_hdf5/videos/d435i_rgb/`. The dataset-level -`schema.json` records robot/hand types and feature definitions, while +`schema.json` records robot/hand/neck types and feature definitions, while `episodes.jsonl` stores file mappings and the editable task prompt for every episode. HDF5 stores `frame_index`, `timestamp`, `observation.state(68)`, scalar `observation.mode`, and the 36D motion-tracker reference `action` at 30 Hz. -When hand control is enabled, it also stores `action.hand(12)`. +When hand control is enabled, it also stores `action.hand(12)`. When OpenNeck +control is enabled, it stores the latest normalized yaw/pitch command as +`action.neck(2)`. Disabled devices do not add their action fields. ## Operator Flow diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md index c87ece85..500c691c 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md @@ -236,9 +236,11 @@ recording.output_dir/ └── episode_000000.mp4 ``` -`schema.json` 保存 FPS、`robot_type`、`hand_type` 和 feature 定义。 +`schema.json` 保存 FPS、`robot_type`、`hand_type`、`neck_type` 和 feature 定义。 `robot_type` 来自 `robot.type`；未启用灵巧手时 `hand_type` 为 `none`，否则为 -配置的 `hands.driver`。`episodes.jsonl` 每行对应一个已保存的 episode，包含 +配置的 `hands.driver`。未启用主动视觉颈部控制时 `neck_type` 为 `none`，否则为 +配置的 `neck.driver`。这些 enabled 标志直接决定是否录制对应的 action 字段；没有 +单独的录制开关。`episodes.jsonl` 每行对应一个已保存的 episode，包含 `episode_index`、`frames`、可编辑的 `task`、HDF5 路径和视频路径。因此修改任务 prompt 不需要重写 HDF5 或 MP4。使用相同 schema 再次启动录制时，会从下一个 episode index 继续追加，并且可以使用不同的 `recording.task`。 @@ -258,6 +260,7 @@ observation.state float32[N, 68] observation.mode int8[N] action float32[N, 36] action.hand float32[N, 12] # 仅启用灵巧手时存在 +action.neck float32[N, 2] # 仅启用 OpenNeck 时存在 ``` HDF5 文件仅包含上述逐帧数组，不保存录制元数据根属性。RGB 帧保存在 MP4 中， @@ -271,3 +274,5 @@ HDF5 文件仅包含上述逐帧数组，不保存录制元数据根属性。RGB 消费的高层参考，不是 tracker policy 的原始输出，也不是最终下发给 G1 的关节目标。 `action.hand` 是手部 worker 最新的 LinkerHand 命令： `left_pose(6) + right_pose(6)`，使用 SDK 的 0-255 pose 数值。 +`action.neck` 是颈部 worker 最近一次成功发送给 OpenNeck 的命令：归一化的 +`[yaw, pitch]`，两个值的范围均为 `[-1, 1]`。 diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md index a381eaaf..40deea79 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md @@ -113,11 +113,12 @@ python scripts/run/run_sim2real.py \ `STANDING`、`MOCAP`、`ARMS` 和暂停状态的 mocap；已经保存的 episode 不支持再丢弃。 episode 会保存为 `data/recordings/sim2real_hdf5/data/` 下的 `.h5` 文件， 压缩 MP4 视频保存在 `data/recordings/sim2real_hdf5/videos/d435i_rgb/` 下。 -数据集级 `schema.json` 保存机器人/灵巧手类型和 feature 定义， +数据集级 `schema.json` 保存机器人/灵巧手/颈部类型和 feature 定义， `episodes.jsonl` 保存每个 episode 的文件映射与可编辑任务 prompt。HDF5 以 30 Hz 保存 `frame_index`、`timestamp`、`observation.state(68)`、标量 `observation.mode` 和作为 motion-tracker reference 的 36D `action`。启用灵巧手 -控制时还会保存 `action.hand(12)`。 +控制时还会保存 `action.hand(12)`。启用 OpenNeck 控制时，会把最新的归一化 +yaw/pitch 命令保存为 `action.neck(2)`。未启用的设备不会添加对应的 action 字段。 ## 操作流程 diff --git a/scripts/dev/test_openneck.py b/scripts/dev/test_openneck.py index d891932f..829ec497 100644 --- a/scripts/dev/test_openneck.py +++ b/scripts/dev/test_openneck.py @@ -181,12 +181,13 @@ def run_pico(args: argparse.Namespace) -> None: if provider.has_frame(): frame, timestamp_s, seq = provider.get_frame_packet() if int(seq) != last_seq: - moved = runtime.tick( + command = runtime.tick( frame=frame, frame_timestamp_s=timestamp_s, active=True, now_s=now_s, ) + moved = command is not None if moved: command_count += 1 last_seq = int(seq) diff --git a/teleopit/recording/hdf5.py b/teleopit/recording/hdf5.py index 8add4edb..14af97aa 100644 --- a/teleopit/recording/hdf5.py +++ b/teleopit/recording/hdf5.py @@ -28,17 +28,21 @@ MODE_KEY = "observation.mode" ACTION_KEY = "action" HAND_ACTION_KEY = "action.hand" +NECK_ACTION_KEY = "action.neck" FRAME_INDEX_KEY = "frame_index" TIMESTAMP_KEY = "timestamp" STATE_DIM = 68 ACTION_DIM = FULL_QPOS_DIM HAND_ACTION_DIM = 12 +NECK_ACTION_DIM = 2 DEFAULT_IMAGE_SHAPE = (480, 640, 3) HDF5_RECORDING_FORMAT = "teleopit_hdf5" -HDF5_RECORDING_VERSION = 1 +HDF5_RECORDING_VERSION = 2 DEFAULT_ROBOT_TYPE = "unitree_g1_29dof" NO_HAND_TYPE = "none" SUPPORTED_HAND_TYPES = (NO_HAND_TYPE, "linkerhand_l6", "linkerhand_o6") +NO_NECK_TYPE = "none" +SUPPORTED_NECK_TYPES = (NO_NECK_TYPE, "openneck") MODE_CODES = { "standing": 0, "mocap": 1, @@ -56,6 +60,7 @@ class RecordingSchema: hand_type: str image_key: str image_shape: tuple[int, int, int] + neck_type: str = NO_NECK_TYPE state_key: str = STATE_KEY state_dim: int = STATE_DIM mode_key: str = MODE_KEY @@ -63,11 +68,17 @@ class RecordingSchema: action_dim: int = ACTION_DIM hand_action_key: str = HAND_ACTION_KEY hand_action_dim: int = HAND_ACTION_DIM + neck_action_key: str = NECK_ACTION_KEY + neck_action_dim: int = NECK_ACTION_DIM @property def has_hand_action(self) -> bool: return self.hand_type != NO_HAND_TYPE + @property + def has_neck_action(self) -> bool: + return self.neck_type != NO_NECK_TYPE + @dataclass(frozen=True) class MP4VideoConfig: @@ -82,6 +93,7 @@ def build_recording_schema( fps: int = 30, robot_type: str = DEFAULT_ROBOT_TYPE, hand_type: str = NO_HAND_TYPE, + neck_type: str = NO_NECK_TYPE, ) -> RecordingSchema: key = str(cfg_get(camera_cfg, "key", IMAGE_KEY)).strip() width = int(cfg_get(camera_cfg, "width", DEFAULT_IMAGE_SHAPE[1])) @@ -89,6 +101,7 @@ def build_recording_schema( parsed_fps = int(fps) parsed_robot_type = str(robot_type).strip().lower() parsed_hand_type = str(hand_type).strip().lower() + parsed_neck_type = str(neck_type).strip().lower() if not key: raise ValueError("recording.camera.key must not be empty") if width <= 0 or height <= 0: @@ -103,10 +116,15 @@ def build_recording_schema( raise ValueError( f"Unsupported recording hand_type={parsed_hand_type!r}; expected one of {SUPPORTED_HAND_TYPES}" ) + if parsed_neck_type not in SUPPORTED_NECK_TYPES: + raise ValueError( + f"Unsupported recording neck_type={parsed_neck_type!r}; expected one of {SUPPORTED_NECK_TYPES}" + ) return RecordingSchema( fps=parsed_fps, robot_type=parsed_robot_type, hand_type=parsed_hand_type, + neck_type=parsed_neck_type, image_key=key, image_shape=(height, width, 3), ) @@ -172,6 +190,14 @@ def hdf5_schema(schema: RecordingSchema) -> dict[str, object]: "right_hand_target": [6, 12], }, } + if schema.has_neck_action: + features[schema.neck_action_key] = { + "dtype": "float32", + "shape": [schema.neck_action_dim], + "names": ["yaw", "pitch"], + "units": "normalized", + "range": [-1.0, 1.0], + } features[schema.image_key] = { "dtype": "video", "shape": list(schema.image_shape), @@ -183,6 +209,7 @@ def hdf5_schema(schema: RecordingSchema) -> dict[str, object]: "fps": schema.fps, "robot_type": schema.robot_type, "hand_type": schema.hand_type, + "neck_type": schema.neck_type, "features": features, } @@ -231,6 +258,13 @@ def normalize_hand_action(left_pose: object, right_pose: object) -> np.ndarray: return action +def normalize_neck_action(yaw: object, pitch: object) -> np.ndarray: + action = np.asarray([yaw, pitch], dtype=np.float32).reshape(-1) + if action.shape[0] != NECK_ACTION_DIM: + raise ValueError(f"recording action.neck must be {NECK_ACTION_DIM}D, got {action.shape[0]}") + return action + + def build_mode_observation(mode: str) -> np.int8: normalized = str(mode).strip().lower() if normalized not in MODE_CODES: @@ -332,6 +366,7 @@ def add_frame( mode: object, action: np.ndarray, hand_action: np.ndarray | None = None, + neck_action: np.ndarray | None = None, ) -> None: if not self._active or self._h5 is None: raise RuntimeError("Cannot add a recording frame without an active episode") @@ -352,6 +387,17 @@ def add_frame( ) elif hand_action is not None: raise ValueError(f"{self._schema.hand_action_key} must be omitted for hand_type={NO_HAND_TYPE}") + neck_action_arr: np.ndarray | None = None + if self._schema.has_neck_action: + if neck_action is None: + raise ValueError(f"{self._schema.neck_action_key} is required for neck_type={self._schema.neck_type}") + neck_action_arr = self._validate_vector( + neck_action, + self._schema.neck_action_key, + self._schema.neck_action_dim, + ) + elif neck_action is not None: + raise ValueError(f"{self._schema.neck_action_key} must be omitted for neck_type={NO_NECK_TYPE}") row = self._frames_in_episode for dataset in self._datasets.values(): @@ -366,6 +412,8 @@ def add_frame( self._datasets[self._schema.action_key][row] = action_arr if hand_action_arr is not None: self._datasets[self._schema.hand_action_key][row] = hand_action_arr + if neck_action_arr is not None: + self._datasets[self._schema.neck_action_key][row] = neck_action_arr self._frames_in_episode += 1 def save_episode(self) -> None: @@ -623,6 +671,12 @@ def _create_datasets(self, h5: h5py.File) -> dict[str, h5py.Dataset]: self._schema.hand_action_key, self._schema.hand_action_dim, ) + if self._schema.has_neck_action: + datasets[self._schema.neck_action_key] = self._create_vector_dataset( + h5, + self._schema.neck_action_key, + self._schema.neck_action_dim, + ) return datasets @staticmethod diff --git a/teleopit/sim2real/mp/ipc.py b/teleopit/sim2real/mp/ipc.py index e0c3b1c9..e24487e0 100644 --- a/teleopit/sim2real/mp/ipc.py +++ b/teleopit/sim2real/mp/ipc.py @@ -13,6 +13,7 @@ BODY_TOPIC = "body" HAND_TOPIC = "hand" HAND_COMMAND_TOPIC = "hand_command" +NECK_COMMAND_TOPIC = "neck_command" CONTROLLER_TOPIC = "controller" CONTROL_EVENTS_TOPIC = "control_events" REFERENCE_TOPIC = "reference" @@ -28,6 +29,7 @@ class Sim2RealIpcEndpoints: body_pub: str hand_pub: str hand_command_pub: str + neck_command_pub: str controller_pub: str control_events_pub: str reference_pub: str @@ -46,6 +48,7 @@ def default_endpoints(*, host: str = "127.0.0.1", base_port: int = 39700) -> Sim body_pub=f"{prefix}{base_port}", hand_pub=f"{prefix}{base_port + 1}", hand_command_pub=f"{prefix}{base_port + 2}", + neck_command_pub=f"{prefix}{base_port + 12}", controller_pub=f"{prefix}{base_port + 3}", control_events_pub=f"{prefix}{base_port + 4}", reference_pub=f"{prefix}{base_port + 5}", diff --git a/teleopit/sim2real/mp/messages.py b/teleopit/sim2real/mp/messages.py index b2ee9c1c..d929b1aa 100644 --- a/teleopit/sim2real/mp/messages.py +++ b/teleopit/sim2real/mp/messages.py @@ -82,6 +82,16 @@ class HandCommandPacket: seq: int +@dataclass(frozen=True) +class NeckCommandPacket: + timestamp_s: float + driver: str + active: bool + yaw: float + pitch: float + seq: int + + @dataclass(frozen=True) class HealthPacket: worker: str diff --git a/teleopit/sim2real/mp/runtime.py b/teleopit/sim2real/mp/runtime.py index 3f269c2b..70c88f07 100644 --- a/teleopit/sim2real/mp/runtime.py +++ b/teleopit/sim2real/mp/runtime.py @@ -45,12 +45,14 @@ from teleopit.recording.hdf5 import ( DEFAULT_ROBOT_TYPE, NO_HAND_TYPE, + NO_NECK_TYPE, build_mode_observation, build_mp4_video_config, build_observation_state, build_recording_schema, - normalize_hand_action, normalize_action_reference_qpos, + normalize_hand_action, + normalize_neck_action, ) from teleopit.sim.reference_motion import OfflineReferenceMotion from teleopit.sim.reference_timeline import ReferenceTimeline, ReferenceWindow, ReferenceWindowBuilder @@ -76,6 +78,7 @@ HAND_TOPIC, HEALTH_TOPIC, MODE_TOPIC, + NECK_COMMAND_TOPIC, RECORD_TOPIC, REFERENCE_TOPIC, VIDEO_TOPIC, @@ -91,6 +94,7 @@ HandCommandPacket, HealthPacket, ModeStatePacket, + NeckCommandPacket, ReferencePacket, RecordStepPacket, SnapshotPacket, @@ -286,7 +290,7 @@ def _recording_camera_cfg(cfg: Any) -> Any: return cfg_get(_recording_cfg(cfg), "camera", {}) or {} -def _recording_robot_and_hand_types(cfg: Any) -> tuple[str, str]: +def _recording_hardware_types(cfg: Any) -> tuple[str, str, str]: robot_cfg = cfg_get(cfg, "robot", {}) or {} robot_type = str(cfg_get(robot_cfg, "type", DEFAULT_ROBOT_TYPE)).strip().lower() hands_cfg = cfg_get(cfg, "hands", {}) or {} @@ -295,7 +299,13 @@ def _recording_robot_and_hand_types(cfg: Any) -> tuple[str, str]: if bool(cfg_get(hands_cfg, "enabled", False)) else NO_HAND_TYPE ) - return robot_type, hand_type + neck_cfg = cfg_get(cfg, "neck", {}) or {} + neck_type = ( + str(cfg_get(neck_cfg, "driver", "openneck")).strip().lower() + if bool(cfg_get(neck_cfg, "enabled", False)) + else NO_NECK_TYPE + ) + return robot_type, hand_type, neck_type def _configured_open_hand_pose(cfg: Any) -> tuple[np.ndarray, np.ndarray]: @@ -353,12 +363,13 @@ def _validate_new_runtime_config(cfg: Any) -> None: raise ValueError("recording.camera.source must be realsense") if int(cfg_get(rec_cfg, "fps", 30)) != int(cfg_get(camera_cfg, "fps", 30)): raise ValueError("recording.fps must match recording.camera.fps") - robot_type, hand_type = _recording_robot_and_hand_types(cfg) + robot_type, hand_type, neck_type = _recording_hardware_types(cfg) build_recording_schema( camera_cfg, fps=int(cfg_get(rec_cfg, "fps", 30)), robot_type=robot_type, hand_type=hand_type, + neck_type=neck_type, ) input_video = parse_pico_video_config(cfg_get(cfg, "input", {}) or {}) if not input_video.enabled: @@ -1863,6 +1874,7 @@ def __init__( self._record_sub = LatestSubscriber(endpoints.record_pub, RECORD_TOPIC) self._video_sub = LatestSubscriber(endpoints.video_pub, VIDEO_TOPIC) self._hand_command_sub = LatestSubscriber(endpoints.hand_command_pub, HAND_COMMAND_TOPIC) + self._neck_command_sub = LatestSubscriber(endpoints.neck_command_pub, NECK_COMMAND_TOPIC) self._command_sub = LatestSubscriber(endpoints.command_pub, COMMAND_TOPIC) self._frame_reader = frame_reader or SharedFrameRingReader() self._latest_record: RecordStepPacket | None = None @@ -1876,6 +1888,14 @@ def __init__( right_pose=right_open.astype(np.float32, copy=True), seq=0, ) + self._latest_neck_command = NeckCommandPacket( + timestamp_s=0.0, + driver=str(cfg_get(cfg_get(cfg, "neck", {}) or {}, "driver", "openneck")).strip().lower(), + active=False, + yaw=0.0, + pitch=0.0, + seq=0, + ) self._latest_video_seq = -1 self._active = False self._episode_started_s = 0.0 @@ -1883,12 +1903,13 @@ def __init__( from teleopit.recording.hdf5 import TeleopitHDF5Recorder - robot_type, hand_type = _recording_robot_and_hand_types(cfg) + robot_type, hand_type, neck_type = _recording_hardware_types(cfg) self._schema = build_recording_schema( self.camera_cfg, fps=self.fps, robot_type=robot_type, hand_type=hand_type, + neck_type=neck_type, ) self._video_config = build_mp4_video_config(cfg_get(self.rec_cfg, "video", {}) or {}) factory = recorder_factory or TeleopitHDF5Recorder.create @@ -1917,6 +1938,10 @@ def run(self) -> None: if isinstance(hand_command, HandCommandPacket): self._latest_hand_command = hand_command + neck_command = self._neck_command_sub.recv_latest() + if isinstance(neck_command, NeckCommandPacket): + self._latest_neck_command = neck_command + video = self._video_sub.recv_latest() if isinstance(video, SharedFrameDescriptor): self._handle_video(video) @@ -1934,6 +1959,7 @@ def run(self) -> None: self._record_sub.close() self._video_sub.close() self._hand_command_sub.close() + self._neck_command_sub.close() self._command_sub.close() self._frame_reader.close() @@ -2020,13 +2046,24 @@ def _handle_video(self, descriptor: SharedFrameDescriptor) -> None: if self._schema.has_hand_action else None ) - self._recorder.add_frame( - image=np.asarray(image, dtype=np.uint8), - state=np.asarray(record.observation_state, dtype=np.float32), - mode=record.observation_mode, - action=np.asarray(record.action_reference_qpos, dtype=np.float32), - hand_action=hand_action, + neck_action = ( + normalize_neck_action( + self._latest_neck_command.yaw, + self._latest_neck_command.pitch, + ) + if self._schema.has_neck_action + else None ) + frame_kwargs = { + "image": np.asarray(image, dtype=np.uint8), + "state": np.asarray(record.observation_state, dtype=np.float32), + "mode": record.observation_mode, + "action": np.asarray(record.action_reference_qpos, dtype=np.float32), + "hand_action": hand_action, + } + if neck_action is not None: + frame_kwargs["neck_action"] = neck_action + self._recorder.add_frame(**frame_kwargs) self._episode_frames += 1 def _run_recording_worker( @@ -2052,18 +2089,44 @@ def _main() -> None: body_sub = LatestSubscriber(endpoints.body_pub, BODY_TOPIC) mode_sub = LatestSubscriber(endpoints.mode_pub, MODE_TOPIC) command_sub = LatestSubscriber(endpoints.command_pub, COMMAND_TOPIC) + neck_command_pub = ( + ZmqPublisher(endpoints.neck_command_pub) + if _recording_enabled(cfg) + else None + ) latest_frame: Any | None = None latest_frame_timestamp_s: float | None = None latest_body_seq = -1 latest_mode: ModeStatePacket | None = None command_count = 0 + command_seq = 0 sleep_s = 1.0 / max(float(neck_cfg.rate_hz), 1.0) last_status_s = 0.0 + + def _publish_neck_command(*, timestamp_s: float, active: bool, yaw: float, pitch: float) -> None: + nonlocal command_seq + if neck_command_pub is None: + return + command_seq += 1 + neck_command_pub.publish( + NECK_COMMAND_TOPIC, + NeckCommandPacket( + timestamp_s=float(timestamp_s), + driver=neck_cfg.driver, + active=bool(active), + yaw=float(yaw), + pitch=float(pitch), + seq=command_seq, + ), + ) + try: runtime.start() + if neck_cfg.center_on_start: + _publish_neck_command(timestamp_s=time.monotonic(), active=False, yaw=0.0, pitch=0.0) while not stop_event.is_set(): - command = command_sub.recv_latest() - if isinstance(command, CommandPacket) and command.command == "shutdown": + runtime_command = command_sub.recv_latest() + if isinstance(runtime_command, CommandPacket) and runtime_command.command == "shutdown": stop_event.set() break body_packet = body_sub.recv_latest() @@ -2076,15 +2139,22 @@ def _main() -> None: if isinstance(mode_packet, ModeStatePacket): latest_mode = mode_packet now_s = time.monotonic() + active = mode_packet_active(latest_mode, neck_cfg) try: - moved = runtime.tick( + neck_command = runtime.tick( frame=latest_frame, frame_timestamp_s=latest_frame_timestamp_s, - active=mode_packet_active(latest_mode, neck_cfg), + active=active, now_s=now_s, ) - if moved: + if neck_command is not None: command_count += 1 + _publish_neck_command( + timestamp_s=now_s, + active=active, + yaw=neck_command.yaw, + pitch=neck_command.pitch, + ) except Exception: logger.exception("OpenNeck worker tick failed; neck control continues") if now_s - last_status_s >= 5.0: @@ -2092,7 +2162,7 @@ def _main() -> None: "OpenNeck worker status | body_seq=%s commands=%s active=%s", latest_body_seq, command_count, - mode_packet_active(latest_mode, neck_cfg), + active, ) last_status_s = now_s time.sleep(sleep_s) @@ -2103,6 +2173,8 @@ def _main() -> None: body_sub.close() mode_sub.close() command_sub.close() + if neck_command_pub is not None: + neck_command_pub.close() _worker_loop("neck_worker", cfg, _main) diff --git a/teleopit/sim2real/neck/worker.py b/teleopit/sim2real/neck/worker.py index aa885374..891373f6 100644 --- a/teleopit/sim2real/neck/worker.py +++ b/teleopit/sim2real/neck/worker.py @@ -6,7 +6,7 @@ from teleopit.inputs.realtime_packet import HumanFrame from teleopit.sim2real.neck.config import NeckConfig, parse_neck_config -from teleopit.sim2real.neck.mapper import HeadPoseMapper +from teleopit.sim2real.neck.mapper import HeadPoseMapper, NeckCommand from teleopit.sim2real.neck.openneck import NeckDevice, build_neck_device logger = logging.getLogger(__name__) @@ -31,20 +31,20 @@ def tick( frame_timestamp_s: float | None, active: bool, now_s: float | None = None, - ) -> bool: + ) -> NeckCommand | None: now = time.monotonic() if now_s is None else float(now_s) if active and not self._active: self._mapper.reset() self._active = bool(active) if not self._active or frame is None or frame_timestamp_s is None: - return False + return None if now - float(frame_timestamp_s) > self._cfg.frame_timeout_s: - return False + return None command = self._mapper.map_frame(frame) if command is None: - return False + return None self._device.move_norm(command.yaw, command.pitch) - return True + return command def close(self) -> None: try: @@ -73,9 +73,9 @@ def tick( frame_timestamp_s: float | None, active: bool, now_s: float | None = None, - ) -> bool: + ) -> None: del frame, frame_timestamp_s, active, now_s - return False + return None def close(self) -> None: return None diff --git a/tests/test_active_neck.py b/tests/test_active_neck.py index 54495672..fcc72ab2 100644 --- a/tests/test_active_neck.py +++ b/tests/test_active_neck.py @@ -108,10 +108,13 @@ def close(self) -> None: runtime.start() assert device.center_calls == 1 - assert not runtime.tick(frame=_frame(_quat_y(0.0)), frame_timestamp_s=1.0, active=True, now_s=1.01) - assert runtime.tick(frame=_frame(_quat_y(30.0)), frame_timestamp_s=1.02, active=True, now_s=1.03) + assert runtime.tick(frame=_frame(_quat_y(0.0)), frame_timestamp_s=1.0, active=True, now_s=1.01) is None + command = runtime.tick(frame=_frame(_quat_y(30.0)), frame_timestamp_s=1.02, active=True, now_s=1.03) runtime.close() + assert command is not None + assert command.yaw == pytest_approx(30.0 / 90.0) + assert command.pitch == pytest_approx(0.0) assert device.moves == [(30.0 / 90.0, 0.0)] assert device.center_calls == 2 assert device.closed is True diff --git a/tests/test_sim2real_multiprocess.py b/tests/test_sim2real_multiprocess.py index e82f2b15..69a95735 100644 --- a/tests/test_sim2real_multiprocess.py +++ b/tests/test_sim2real_multiprocess.py @@ -19,8 +19,10 @@ FRAME_INDEX_KEY, HAND_ACTION_KEY, HDF5_RECORDING_FORMAT, + HDF5_RECORDING_VERSION, IMAGE_KEY, MODE_KEY, + NECK_ACTION_KEY, STATE_KEY, TIMESTAMP_KEY, build_mode_observation, @@ -28,8 +30,15 @@ build_recording_schema, hdf5_schema, ) -from teleopit.sim2real.mp.ipc import HEALTH_TOPIC, LatestSubscriber, ZmqPublisher -from teleopit.sim2real.mp.messages import HandCommandPacket, ModeStatePacket, RecordStepPacket, ReferencePacket, SharedFrameDescriptor +from teleopit.sim2real.mp.ipc import HEALTH_TOPIC, LatestSubscriber, ZmqPublisher, default_endpoints +from teleopit.sim2real.mp.messages import ( + HandCommandPacket, + ModeStatePacket, + NeckCommandPacket, + RecordStepPacket, + ReferencePacket, + SharedFrameDescriptor, +) from teleopit.sim.reference_timeline import ReferenceSample, ReferenceWindow from teleopit.sim2real.mp.runtime import ( ARM_MOCAP_REFERENCE_COMMAND, @@ -43,6 +52,8 @@ _configured_open_hand_pose, _hand_worker_active_for_mode, _human_frame_is_valid, + _recording_hardware_types, + _run_neck_worker, ) from teleopit.sim2real.mp.shm import SharedFrameRingReader, SharedFrameRingWriter @@ -335,6 +346,55 @@ def Process(self, *, name: str, target: object, args: tuple[object, ...]) -> Fak assert started_names == ["pico_input", "reference", "robot_control", "neck_worker"] +@pytest.mark.parametrize("recording_enabled", [False, True]) +def test_neck_command_publisher_is_only_created_for_recording(monkeypatch, recording_enabled: bool) -> None: + publisher_endpoints: list[str] = [] + published_topics: list[str] = [] + + class FakeRuntime: + def start(self) -> None: + return None + + def close(self) -> None: + return None + + class FakeSubscriber: + def __init__(self, endpoint: str, topic: str) -> None: + del endpoint, topic + + def close(self) -> None: + return None + + class FakePublisher: + def __init__(self, endpoint: str) -> None: + publisher_endpoints.append(endpoint) + + def publish(self, topic: str, payload: object) -> None: + del payload + published_topics.append(topic) + + def close(self) -> None: + return None + + monkeypatch.setattr("teleopit.sim2real.mp.runtime.build_neck_runtime", lambda _cfg: FakeRuntime()) + monkeypatch.setattr("teleopit.sim2real.mp.runtime.LatestSubscriber", FakeSubscriber) + monkeypatch.setattr("teleopit.sim2real.mp.runtime.ZmqPublisher", FakePublisher) + endpoints = default_endpoints(base_port=39870) + stop_event = SimpleNamespace(is_set=lambda: True, set=lambda: None) + + _run_neck_worker( + { + "neck": {"enabled": True, "driver": "openneck"}, + "recording": {"enabled": recording_enabled}, + }, + endpoints, + stop_event, # type: ignore[arg-type] + ) + + assert publisher_endpoints == ([endpoints.neck_command_pub] if recording_enabled else []) + assert published_topics == (["neck_command"] if recording_enabled else []) + + def test_noncritical_worker_exit_warning_is_not_repeated(monkeypatch, caplog) -> None: class FakeStopEvent: def __init__(self) -> None: @@ -384,20 +444,47 @@ def test_recording_key_mapping() -> None: assert map_recording_key_to_command("x") is None +@pytest.mark.parametrize( + ("hands_enabled", "neck_enabled", "hand_type", "neck_type"), + [ + (False, False, "none", "none"), + (True, False, "linkerhand_l6", "none"), + (False, True, "none", "openneck"), + (True, True, "linkerhand_l6", "openneck"), + ], +) +def test_recording_optional_hardware_types_follow_enabled_flags( + hands_enabled: bool, + neck_enabled: bool, + hand_type: str, + neck_type: str, +) -> None: + assert _recording_hardware_types( + { + "robot": {"type": "unitree_g1_29dof"}, + "hands": {"enabled": hands_enabled, "driver": "linkerhand_l6"}, + "neck": {"enabled": neck_enabled, "driver": "openneck"}, + } + ) == ("unitree_g1_29dof", hand_type, neck_type) + + def test_hdf5_recording_schema() -> None: schema = build_recording_schema( {"width": 640, "height": 480, "key": IMAGE_KEY}, fps=30, robot_type="unitree_g1_29dof", hand_type="linkerhand_o6", + neck_type="openneck", ) sidecar = hdf5_schema(schema) features = sidecar["features"] assert sidecar["format"] == HDF5_RECORDING_FORMAT + assert sidecar["version"] == HDF5_RECORDING_VERSION assert sidecar["fps"] == 30 assert sidecar["robot_type"] == "unitree_g1_29dof" assert sidecar["hand_type"] == "linkerhand_o6" + assert sidecar["neck_type"] == "openneck" assert features[IMAGE_KEY]["dtype"] == "video" assert features[IMAGE_KEY]["shape"] == [480, 640, 3] assert features[FRAME_INDEX_KEY]["dtype"] == "int64" @@ -407,25 +494,47 @@ def test_hdf5_recording_schema() -> None: assert features[MODE_KEY]["dtype"] == "int8" assert features[ACTION_KEY]["shape"] == [36] assert features[HAND_ACTION_KEY]["shape"] == [12] + assert features[NECK_ACTION_KEY]["shape"] == [2] assert features[STATE_KEY]["groups"]["joint_pos"] == [0, 29] assert features[STATE_KEY]["groups"]["projected_gravity"] == [65, 68] assert features[MODE_KEY]["values"]["pause"] == 3 assert features[ACTION_KEY]["groups"]["reference_joint_pos"] == [7, 36] assert features[HAND_ACTION_KEY]["groups"]["left_hand_target"] == [0, 6] assert features[HAND_ACTION_KEY]["groups"]["right_hand_target"] == [6, 12] + assert features[NECK_ACTION_KEY]["names"] == ["yaw", "pitch"] + assert features[NECK_ACTION_KEY]["units"] == "normalized" + assert features[NECK_ACTION_KEY]["range"] == [-1.0, 1.0] assert len(features[STATE_KEY]["names"]) == 68 assert len(features[ACTION_KEY]["names"]) == 36 assert len(features[HAND_ACTION_KEY]["names"]) == 12 -def test_hdf5_recording_schema_omits_hand_action_without_hand_hardware() -> None: +@pytest.mark.parametrize( + ("hand_type", "neck_type", "has_hand_action", "has_neck_action"), + [ + ("none", "none", False, False), + ("linkerhand_l6", "none", True, False), + ("none", "openneck", False, True), + ("linkerhand_o6", "openneck", True, True), + ], +) +def test_hdf5_recording_schema_optional_action_combinations( + hand_type: str, + neck_type: str, + has_hand_action: bool, + has_neck_action: bool, +) -> None: schema = build_recording_schema( {"width": 640, "height": 480, "key": IMAGE_KEY}, - hand_type="none", + hand_type=hand_type, + neck_type=neck_type, ) + features = hdf5_schema(schema)["features"] - assert schema.has_hand_action is False - assert HAND_ACTION_KEY not in hdf5_schema(schema)["features"] + assert schema.has_hand_action is has_hand_action + assert schema.has_neck_action is has_neck_action + assert (HAND_ACTION_KEY in features) is has_hand_action + assert (NECK_ACTION_KEY in features) is has_neck_action def test_hdf5_recorder_mp4_sidecar_writes_sync_metadata(tmp_path: Path) -> None: @@ -434,6 +543,7 @@ def test_hdf5_recorder_mp4_sidecar_writes_sync_metadata(tmp_path: Path) -> None: schema = build_recording_schema( {"width": 2, "height": 2, "key": IMAGE_KEY}, hand_type="linkerhand_l6", + neck_type="openneck", ) recorder = TeleopitHDF5Recorder.create( output_dir=tmp_path, @@ -450,6 +560,7 @@ def test_hdf5_recorder_mp4_sidecar_writes_sync_metadata(tmp_path: Path) -> None: mode=build_mode_observation("mocap"), action=np.arange(36, dtype=np.float32), hand_action=np.arange(12, dtype=np.float32), + neck_action=np.array([0.25, -0.5], dtype=np.float32), ) recorder.save_episode() recorder.finalize() @@ -486,6 +597,11 @@ def test_hdf5_recorder_mp4_sidecar_writes_sync_metadata(tmp_path: Path) -> None: assert h5[MODE_KEY].dtype == np.dtype(np.int8) assert h5[ACTION_KEY].shape == (2, 36) assert h5[HAND_ACTION_KEY].shape == (2, 12) + assert h5[NECK_ACTION_KEY].shape == (2, 2) + np.testing.assert_allclose( + h5[NECK_ACTION_KEY][...], + np.array([[0.25, -0.5], [0.25, -0.5]], dtype=np.float32), + ) def test_hdf5_recorder_resumes_and_keeps_tasks_in_editable_manifest(tmp_path: Path) -> None: @@ -524,6 +640,7 @@ def write_episode(task: str, value: int) -> None: ] with h5py.File(tmp_path / "data" / "episode_000001.h5", "r") as h5: assert HAND_ACTION_KEY not in h5 + assert NECK_ACTION_KEY not in h5 def test_hdf5_recorder_discards_uncommitted_episode_files_on_resume(tmp_path: Path) -> None: @@ -1227,9 +1344,11 @@ def add_frame( mode: object, action: np.ndarray, hand_action: np.ndarray | None = None, + neck_action: np.ndarray | None = None, ) -> None: calls.append("frame") assert hand_action is not None + assert neck_action is not None frames.append( { "image": image.copy(), @@ -1237,6 +1356,7 @@ def add_frame( "mode": np.asarray(mode).copy(), "action": action.copy(), "hand_action": hand_action.copy(), + "neck_action": neck_action.copy(), } ) @@ -1264,6 +1384,7 @@ def fake_factory(**_kwargs: object) -> FakeRecorder: "camera": {"width": 2, "height": 2, "key": IMAGE_KEY}, }, "hands": {"enabled": True, "driver": "linkerhand_l6"}, + "neck": {"enabled": True, "driver": "openneck"}, }, endpoints, stop_event, # type: ignore[arg-type] @@ -1308,6 +1429,14 @@ def fake_factory(**_kwargs: object) -> FakeRecorder: right_pose=np.arange(6, 12, dtype=np.float32), seq=1, ) + worker._latest_neck_command = NeckCommandPacket( + timestamp_s=2.06, + driver="openneck", + active=True, + yaw=0.25, + pitch=-0.5, + seq=1, + ) desc = writer.write(np.full((2, 2, 3), 5, dtype=np.uint8), timestamp_s=2.1) worker._handle_video(desc) worker._save_episode() @@ -1318,6 +1447,7 @@ def fake_factory(**_kwargs: object) -> FakeRecorder: assert int(frames[0]["mode"]) == int(build_mode_observation("standing")) np.testing.assert_allclose(frames[0]["action"], np.arange(36, dtype=np.float32)) np.testing.assert_allclose(frames[0]["hand_action"], np.arange(12, dtype=np.float32)) + np.testing.assert_allclose(frames[0]["neck_action"], np.array([0.25, -0.5], dtype=np.float32)) worker._latest_record = RecordStepPacket( timestamp_s=3.0, @@ -1337,5 +1467,6 @@ def fake_factory(**_kwargs: object) -> FakeRecorder: worker._record_sub.close() worker._video_sub.close() worker._hand_command_sub.close() + worker._neck_command_sub.close() worker._command_sub.close() worker._frame_reader.close() From 523b643da96502e9e790e05dd89803fe70c0fbdd Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Wed, 15 Jul 2026 19:40:50 +0800 Subject: [PATCH 17/59] Fix OpenNeck fixed-neutral head mapping --- AGENTS.md | 1 + README.md | 4 +- docs/docs/configuration/config-reference.md | 9 ++- .../current/configuration/config-reference.md | 7 +- scripts/dev/test_openneck.py | 10 +-- teleopit/configs/pico4_sim2real.yaml | 4 - teleopit/configs/sim2real.yaml | 4 - teleopit/sim2real/neck/config.py | 11 --- teleopit/sim2real/neck/mapper.py | 42 +++-------- teleopit/sim2real/neck/worker.py | 6 +- tests/test_active_neck.py | 75 ++++++++++++++----- 11 files changed, 81 insertions(+), 92 deletions(-) diff --git a/AGENTS.md b/AGENTS.md index 70e6dbe4..3c710671 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -164,6 +164,7 @@ target_dof_pos = clip(action, -10, 10) × action_scale + default_dof_pos - In `vr_hand_pose` mode, missing/inactive hand pose holds the last commanded pose for that side instead of opening the hand - Optional OpenNeck active-vision gimbal control uses `neck.enabled=true` and `neck.driver=openneck`; it requires `input.provider=pico4`, reuses the existing Pico body frame stream, and must not start a second `PicoBridge` - OpenNeck is integrated as a non-critical sim2real `neck_worker`; failures should not stop the G1 control loop, and no OpenNeck state is added to the 167D policy observation +- OpenNeck maps the absolute Pico `Head` orientation relative to `Spine3` with a fixed identity neutral pose and no neck-side EMA; it must not capture the first live frame as a runtime zero pose, so tracking can start while the operator's head is turned ### SimulationLoop Runtime Behavior - `realtime=true` enforces wall-clock pacing even without a viewer diff --git a/README.md b/README.md index 8728d856..9772a3b0 100644 --- a/README.md +++ b/README.md @@ -128,7 +128,9 @@ python scripts/run/run_sim2real.py --config-name pico4_sim2real \ `neck.enabled=true` requires `input.provider=pico4`. The neck worker reuses the existing Teleopit Pico receiver and does not start another `PicoBridge` or -camera pipeline. +camera pipeline. It maps the absolute `Head` orientation relative to `Spine3` +with a fixed neutral pose and no neck-side EMA, so tracking startup does not +require the operator to face straight ahead. ## Documentation diff --git a/docs/docs/configuration/config-reference.md b/docs/docs/configuration/config-reference.md index c029c4b8..9e642a76 100644 --- a/docs/docs/configuration/config-reference.md +++ b/docs/docs/configuration/config-reference.md @@ -160,7 +160,11 @@ through `somehand.api` only. `neck.enabled=true` requires `input.provider=pico4` and the `openneck` extra. The neck worker reuses Teleopit's existing Pico body-frame stream and does not start a second `PicoBridge` or RealSense pipeline. OpenNeck runs as a non-critical -sim2real worker and does not change the policy observation. +sim2real worker and does not change the policy observation. Head motion is +mapped as the absolute `Head` orientation relative to `Spine3`, using the fixed +PICO neutral orientation and no neck-side EMA; startup does not capture the +operator's first pose as a new zero pose, so the operator does not need to face +straight when tracking starts. | Field | Description | Default | |-------|-------------|---------| @@ -171,10 +175,7 @@ sim2real worker and does not change the policy observation. | `neck.rate_hz` | Maximum neck command rate in Hz | `60.0` | | `neck.frame_timeout_s` | Pico body-frame staleness threshold | `0.2` | | `neck.active_modes` | Sim2real modes that allow neck motion | `[standing, mocap, arms, pause]` | -| `neck.head_joint` / `body_reference_joint` | Pico body joints used for relative head mapping | `Head` / `Spine3` | -| `neck.use_body_reference` | Map head motion relative to the body reference joint | `true` | | `neck.dead_zone_deg` | Yaw/pitch dead zone in degrees | `0.5` | -| `neck.smoothing_alpha` | EMA alpha for normalized yaw/pitch commands | `0.35` | | `neck.yaw_range_deg` / `pitch_range_deg` | Degrees mapped to normalized command magnitude `1.0` | `90.0` / `60.0` | | `neck.invert_yaw` / `invert_pitch` | Invert OpenNeck command direction per axis | `true` / `true` | | `neck.center_on_start` / `center_on_shutdown` | Center the gimbal at worker startup/shutdown | `true` / `false` | diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md index 500c691c..f0c06b0c 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md @@ -177,7 +177,9 @@ MuJoCo 窗口显示重定向参考；`sim2sim`、`mocap`、`camera` 和 `all` `neck.enabled=true` 要求 `input.provider=pico4` 和 `openneck` extra。neck worker 复用 Teleopit 已有的 Pico body frame 数据流，不会启动第二个 `PicoBridge` 或 RealSense -管线。OpenNeck 作为非关键 sim2real worker 运行，不会改变策略观测。 +管线。OpenNeck 作为非关键 sim2real worker 运行，不会改变策略观测。头部运动使用固定的 +PICO 中立姿态且不进行颈部侧 EMA，按照 `Head` 相对于 `Spine3` 的绝对朝向进行映射； +启动时不会把操作者的第一帧姿态采集为新的零位，因此开始追踪时操作者不需要保持头部朝正前方。 | 字段 | 说明 | 默认值 | |---|---|---| @@ -188,10 +190,7 @@ MuJoCo 窗口显示重定向参考；`sim2sim`、`mocap`、`camera` 和 `all` | `neck.rate_hz` | 最大头颈命令频率（Hz） | `60.0` | | `neck.frame_timeout_s` | Pico body frame 过期阈值 | `0.2` | | `neck.active_modes` | 允许头颈运动的 sim2real 模式 | `[standing, mocap, arms, pause]` | -| `neck.head_joint` / `body_reference_joint` | 用于相对头部映射的 Pico body 关节 | `Head` / `Spine3` | -| `neck.use_body_reference` | 相对于 body reference 关节映射头部运动 | `true` | | `neck.dead_zone_deg` | yaw/pitch 死区（度） | `0.5` | -| `neck.smoothing_alpha` | 归一化 yaw/pitch 命令的 EMA alpha | `0.35` | | `neck.yaw_range_deg` / `pitch_range_deg` | 映射到归一化命令幅值 `1.0` 的角度 | `90.0` / `60.0` | | `neck.invert_yaw` / `invert_pitch` | 按轴反转 OpenNeck 命令方向 | `true` / `true` | | `neck.center_on_start` / `center_on_shutdown` | worker 启动/关闭时回中云台 | `true` / `false` | diff --git a/scripts/dev/test_openneck.py b/scripts/dev/test_openneck.py index 829ec497..f42390aa 100644 --- a/scripts/dev/test_openneck.py +++ b/scripts/dev/test_openneck.py @@ -53,15 +53,11 @@ def parse_args() -> argparse.Namespace: parser.add_argument("--no-center-on-start", action="store_true") parser.add_argument("--no-center-on-shutdown", action="store_true") parser.add_argument("--release-on-shutdown", action="store_true") - parser.add_argument("--use-body-reference", action=argparse.BooleanOptionalAction, default=True) parser.add_argument("--invert-yaw", action=argparse.BooleanOptionalAction, default=True) parser.add_argument("--invert-pitch", action=argparse.BooleanOptionalAction, default=True) parser.add_argument("--dead-zone-deg", type=float, default=0.5) - parser.add_argument("--smoothing-alpha", type=float, default=0.35) parser.add_argument("--yaw-range-deg", type=float, default=90.0) parser.add_argument("--pitch-range-deg", type=float, default=60.0) - parser.add_argument("--head-joint", default="Head") - parser.add_argument("--body-reference-joint", default="Spine3") parser.add_argument("--bridge-host", default="0.0.0.0") parser.add_argument("--bridge-port", type=int, default=63901) parser.add_argument("--bridge-discovery", action=argparse.BooleanOptionalAction, default=True) @@ -89,11 +85,7 @@ def make_neck_config(args: argparse.Namespace) -> NeckConfig: rate_hz=args.rate_hz, frame_timeout_s=args.frame_timeout_s, active_modes=("mocap",), - head_joint=args.head_joint, - body_reference_joint=args.body_reference_joint, - use_body_reference=bool(args.use_body_reference), dead_zone_deg=args.dead_zone_deg, - smoothing_alpha=args.smoothing_alpha, yaw_range_deg=args.yaw_range_deg, pitch_range_deg=args.pitch_range_deg, invert_yaw=bool(args.invert_yaw), @@ -171,7 +163,7 @@ def run_pico(args: argparse.Namespace) -> None: print( "Testing OpenNeck active vision from live Pico body tracking. " - "Hold your head neutral for the first valid body frame; press Ctrl-C to stop.", + "OpenNeck follows the current head pose relative to the torso; press Ctrl-C to stop.", flush=True, ) try: diff --git a/teleopit/configs/pico4_sim2real.yaml b/teleopit/configs/pico4_sim2real.yaml index 74c6fa51..12a85c8c 100644 --- a/teleopit/configs/pico4_sim2real.yaml +++ b/teleopit/configs/pico4_sim2real.yaml @@ -114,11 +114,7 @@ neck: rate_hz: 60.0 frame_timeout_s: 0.2 active_modes: [standing, mocap, arms, pause] - head_joint: Head - body_reference_joint: Spine3 - use_body_reference: true dead_zone_deg: 0.5 - smoothing_alpha: 0.35 yaw_range_deg: 90.0 pitch_range_deg: 60.0 invert_yaw: true diff --git a/teleopit/configs/sim2real.yaml b/teleopit/configs/sim2real.yaml index 0da0e847..267c296e 100644 --- a/teleopit/configs/sim2real.yaml +++ b/teleopit/configs/sim2real.yaml @@ -115,11 +115,7 @@ neck: rate_hz: 60.0 frame_timeout_s: 0.2 active_modes: [standing, mocap, arms, pause] - head_joint: Head - body_reference_joint: Spine3 - use_body_reference: true dead_zone_deg: 0.5 - smoothing_alpha: 0.35 yaw_range_deg: 90.0 pitch_range_deg: 60.0 invert_yaw: true diff --git a/teleopit/sim2real/neck/config.py b/teleopit/sim2real/neck/config.py index 5c5848de..c2e12bff 100644 --- a/teleopit/sim2real/neck/config.py +++ b/teleopit/sim2real/neck/config.py @@ -19,11 +19,7 @@ class NeckConfig: rate_hz: float = 60.0 frame_timeout_s: float = 0.2 active_modes: tuple[str, ...] = ("standing", "mocap", "arms", "pause") - head_joint: str = "Head" - body_reference_joint: str = "Spine3" - use_body_reference: bool = True dead_zone_deg: float = 0.5 - smoothing_alpha: float = 0.35 yaw_range_deg: float = 90.0 pitch_range_deg: float = 60.0 invert_yaw: bool = True @@ -43,9 +39,6 @@ def parse_neck_config(cfg: Any) -> NeckConfig: frame_timeout_s = float(cfg_get(neck_cfg, "frame_timeout_s", 0.2)) if frame_timeout_s <= 0: raise ValueError("neck.frame_timeout_s must be > 0") - smoothing_alpha = float(cfg_get(neck_cfg, "smoothing_alpha", 0.35)) - if not 0.0 < smoothing_alpha <= 1.0: - raise ValueError("neck.smoothing_alpha must be in (0, 1]") dead_zone_deg = float(cfg_get(neck_cfg, "dead_zone_deg", 0.5)) if dead_zone_deg < 0: raise ValueError("neck.dead_zone_deg must be >= 0") @@ -71,11 +64,7 @@ def parse_neck_config(cfg: Any) -> NeckConfig: rate_hz=rate_hz, frame_timeout_s=frame_timeout_s, active_modes=active_modes, - head_joint=str(cfg_get(neck_cfg, "head_joint", "Head")), - body_reference_joint=str(cfg_get(neck_cfg, "body_reference_joint", "Spine3")), - use_body_reference=bool(cfg_get(neck_cfg, "use_body_reference", True)), dead_zone_deg=dead_zone_deg, - smoothing_alpha=smoothing_alpha, yaw_range_deg=yaw_range_deg, pitch_range_deg=pitch_range_deg, invert_yaw=bool(cfg_get(neck_cfg, "invert_yaw", True)), diff --git a/teleopit/sim2real/neck/mapper.py b/teleopit/sim2real/neck/mapper.py index bc919fce..644f392f 100644 --- a/teleopit/sim2real/neck/mapper.py +++ b/teleopit/sim2real/neck/mapper.py @@ -11,6 +11,8 @@ FloatArray = NDArray[np.float64] +_PICO_HEAD_JOINT = "Head" +_PICO_BODY_REFERENCE_JOINT = "Spine3" @dataclass(frozen=True) @@ -27,33 +29,17 @@ class HeadPoseMapper: def __init__(self, config: NeckConfig) -> None: self._cfg = config - self._offset = np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float64) - self._calibrated = False - self._smooth_yaw = 0.0 - self._smooth_pitch = 0.0 - - @property - def calibrated(self) -> bool: - return self._calibrated - - def reset(self) -> None: - self._offset = np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float64) - self._calibrated = False - self._smooth_yaw = 0.0 - self._smooth_pitch = 0.0 def map_frame(self, frame: HumanFrame) -> NeckCommand | None: - q_head = _joint_quat(frame, self._cfg.head_joint) + q_head = _joint_quat(frame, _PICO_HEAD_JOINT) if q_head is None: return None - q_body = _joint_quat(frame, self._cfg.body_reference_joint) if self._cfg.use_body_reference else None - relative = self._relative(q_head, q_body) - if not self._calibrated: - self._offset = relative - self._calibrated = True + q_body = _joint_quat(frame, _PICO_BODY_REFERENCE_JOINT) + if q_body is None: return None - - q_cmd = _qmul(relative, _qconj(self._offset)) + # Head and Spine3 share the same neutral orientation in the supported + # PICO convention, so their relative identity is the fixed zero pose. + q_cmd = _qmul(_qconj(q_body), q_head) yaw_deg, pitch_deg, roll_deg = _openneck_yaw_pitch_roll_deg(q_cmd) if self._cfg.invert_yaw: yaw_deg = -yaw_deg @@ -66,22 +52,14 @@ def map_frame(self, frame: HumanFrame) -> NeckCommand | None: yaw = yaw_deg / self._cfg.yaw_range_deg pitch = pitch_deg / self._cfg.pitch_range_deg - alpha = self._cfg.smoothing_alpha - self._smooth_yaw += alpha * (yaw - self._smooth_yaw) - self._smooth_pitch += alpha * (pitch - self._smooth_pitch) return NeckCommand( - yaw=float(np.clip(self._smooth_yaw, -1.0, 1.0)), - pitch=float(np.clip(self._smooth_pitch, -1.0, 1.0)), + yaw=float(np.clip(yaw, -1.0, 1.0)), + pitch=float(np.clip(pitch, -1.0, 1.0)), yaw_deg=float(yaw_deg), pitch_deg=float(pitch_deg), roll_deg=float(roll_deg), ) - def _relative(self, q_head: FloatArray, q_body: FloatArray | None) -> FloatArray: - if q_body is not None: - return _qmul(_qconj(q_body), q_head) - return q_head - def _joint_quat(frame: HumanFrame, joint_name: str) -> FloatArray | None: item = frame.get(joint_name) diff --git a/teleopit/sim2real/neck/worker.py b/teleopit/sim2real/neck/worker.py index 891373f6..8d6ca4af 100644 --- a/teleopit/sim2real/neck/worker.py +++ b/teleopit/sim2real/neck/worker.py @@ -17,7 +17,6 @@ def __init__(self, config: NeckConfig, device: NeckDevice | None = None) -> None self._cfg = config self._device = device or build_neck_device(config) self._mapper = HeadPoseMapper(config) - self._active = False def start(self) -> None: self._device.connect() @@ -33,10 +32,7 @@ def tick( now_s: float | None = None, ) -> NeckCommand | None: now = time.monotonic() if now_s is None else float(now_s) - if active and not self._active: - self._mapper.reset() - self._active = bool(active) - if not self._active or frame is None or frame_timestamp_s is None: + if not active or frame is None or frame_timestamp_s is None: return None if now - float(frame_timestamp_s) > self._cfg.frame_timeout_s: return None diff --git a/tests/test_active_neck.py b/tests/test_active_neck.py index fcc72ab2..d7ac46cf 100644 --- a/tests/test_active_neck.py +++ b/tests/test_active_neck.py @@ -5,6 +5,7 @@ import numpy as np +from teleopit.inputs.pico4_provider import BODY_JOINT_NAMES, Pico4InputProvider from teleopit.sim2real.neck.config import NeckConfig, parse_neck_config from teleopit.sim2real.neck.mapper import HeadPoseMapper from teleopit.sim2real.neck.openneck import OpenNeckDevice @@ -29,27 +30,26 @@ def _frame(head: np.ndarray, spine: np.ndarray | None = None): return frame -def test_head_pose_mapper_calibrates_then_maps_yaw_pitch() -> None: +def test_head_pose_mapper_maps_fixed_neutral_yaw_pitch_without_startup_calibration() -> None: cfg = NeckConfig( enabled=True, - smoothing_alpha=1.0, invert_yaw=False, invert_pitch=False, - use_body_reference=False, dead_zone_deg=0.0, ) mapper = HeadPoseMapper(cfg) - assert mapper.map_frame(_frame(_quat_y(0.0))) is None - command = mapper.map_frame(_frame(_quat_y(30.0))) - + command = mapper.map_frame(_frame(_quat_y(30.0), _quat_y(0.0))) assert command is not None assert command.yaw_deg == pytest_approx(30.0) assert command.yaw == pytest_approx(30.0 / 90.0) - mapper.reset() - assert mapper.map_frame(_frame(_quat_x(0.0))) is None - command = mapper.map_frame(_frame(_quat_x(15.0))) + command = mapper.map_frame(_frame(_quat_y(0.0), _quat_y(0.0))) + assert command is not None + assert command.yaw_deg == pytest_approx(0.0) + assert command.yaw == pytest_approx(0.0) + + command = mapper.map_frame(_frame(_quat_x(15.0), _quat_x(0.0))) assert command is not None assert command.pitch_deg == pytest_approx(15.0) assert command.pitch == pytest_approx(15.0 / 60.0) @@ -58,21 +58,50 @@ def test_head_pose_mapper_calibrates_then_maps_yaw_pitch() -> None: def test_head_pose_mapper_uses_body_relative_orientation() -> None: cfg = NeckConfig( enabled=True, - smoothing_alpha=1.0, invert_yaw=False, - use_body_reference=True, dead_zone_deg=0.0, ) mapper = HeadPoseMapper(cfg) - assert mapper.map_frame(_frame(_quat_y(10.0), _quat_y(10.0))) is None command = mapper.map_frame(_frame(_quat_y(40.0), _quat_y(10.0))) assert command is not None assert command.yaw_deg == pytest_approx(30.0) -def test_neck_runtime_sends_command_after_calibration() -> None: +def test_head_pose_mapper_handles_converted_pico_neutral_and_yaw() -> None: + body_poses = np.zeros((len(BODY_JOINT_NAMES), 7), dtype=np.float64) + body_poses[:, 6] = 1.0 + mapper = HeadPoseMapper( + NeckConfig( + enabled=True, + invert_yaw=False, + invert_pitch=False, + dead_zone_deg=0.0, + ) + ) + + neutral = mapper.map_frame(Pico4InputProvider._convert_body_joints_to_frame(body_poses)) + assert neutral is not None + assert neutral.yaw_deg == pytest_approx(0.0) + assert neutral.pitch_deg == pytest_approx(0.0) + + head_idx = BODY_JOINT_NAMES.index("Head") + body_poses[head_idx, 4] = math.sin(math.radians(30.0) / 2.0) + body_poses[head_idx, 6] = math.cos(math.radians(30.0) / 2.0) + command = mapper.map_frame(Pico4InputProvider._convert_body_joints_to_frame(body_poses)) + assert command is not None + assert command.yaw_deg == pytest_approx(30.0) + assert command.pitch_deg == pytest_approx(0.0) + + +def test_head_pose_mapper_requires_spine3_joint() -> None: + mapper = HeadPoseMapper(NeckConfig(enabled=True)) + + assert mapper.map_frame(_frame(_quat_y(30.0))) is None + + +def test_neck_runtime_sends_relative_command_on_first_active_frame() -> None: class FakeDevice: def __init__(self) -> None: self.moves: list[tuple[float, float]] = [] @@ -97,9 +126,7 @@ def close(self) -> None: device = FakeDevice() cfg = NeckConfig( enabled=True, - smoothing_alpha=1.0, invert_yaw=False, - use_body_reference=False, dead_zone_deg=0.0, center_on_start=True, center_on_shutdown=True, @@ -108,14 +135,26 @@ def close(self) -> None: runtime.start() assert device.center_calls == 1 - assert runtime.tick(frame=_frame(_quat_y(0.0)), frame_timestamp_s=1.0, active=True, now_s=1.01) is None - command = runtime.tick(frame=_frame(_quat_y(30.0)), frame_timestamp_s=1.02, active=True, now_s=1.03) + command = runtime.tick( + frame=_frame(_quat_y(30.0), _quat_y(0.0)), + frame_timestamp_s=1.0, + active=True, + now_s=1.01, + ) + neutral_command = runtime.tick( + frame=_frame(_quat_y(0.0), _quat_y(0.0)), + frame_timestamp_s=1.02, + active=True, + now_s=1.03, + ) runtime.close() assert command is not None assert command.yaw == pytest_approx(30.0 / 90.0) assert command.pitch == pytest_approx(0.0) - assert device.moves == [(30.0 / 90.0, 0.0)] + assert neutral_command is not None + assert neutral_command.yaw == pytest_approx(0.0) + assert device.moves == [(30.0 / 90.0, 0.0), (0.0, 0.0)] assert device.center_calls == 2 assert device.closed is True From 434e75a09d7568dda685d623f6b2c5ca63d79845 Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Wed, 15 Jul 2026 21:43:13 +0800 Subject: [PATCH 18/59] Update OpenNeck angle control integration --- AGENTS.md | 5 +- README.md | 14 +- docs/docs/configuration/config-reference.md | 23 +- docs/docs/getting-started/installation.md | 7 +- docs/docs/tutorials/pico-sim2real.md | 5 +- .../current/configuration/config-reference.md | 18 +- .../current/getting-started/installation.md | 7 +- .../current/tutorials/pico-sim2real.md | 5 +- scripts/dev/test_openneck.py | 40 +- teleopit/configs/pico4_sim2real.yaml | 4 - teleopit/configs/sim2real.yaml | 4 - teleopit/recording/hdf5.py | 11 +- teleopit/sim2real/mp/messages.py | 4 +- teleopit/sim2real/mp/runtime.py | 35 +- teleopit/sim2real/neck/config.py | 27 +- teleopit/sim2real/neck/mapper.py | 16 +- teleopit/sim2real/neck/openneck.py | 128 +++---- teleopit/sim2real/neck/worker.py | 13 +- tests/test_active_neck.py | 359 ++++++------------ tests/test_sim2real_multiprocess.py | 16 +- 20 files changed, 316 insertions(+), 425 deletions(-) diff --git a/AGENTS.md b/AGENTS.md index 3c710671..0717d31f 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -150,7 +150,7 @@ target_dof_pos = clip(action, -10, 10) × action_scale + default_dof_pos - Optional LinkerHand control uses `hands.enabled=true`, `hands.driver=linkerhand_l6|linkerhand_o6`, and `hands.mode=gripper|vr_hand_pose`; default is disabled - Optional Pico sim2real HDF5 recording uses `--config-name sim2real_record` or `recording.enabled=true`; it requires `input.provider=pico4`, `input.video.enabled=true`, `input.video.source=realsense`, an interactive terminal, and the `recording` extra - Recording is manual only: terminal `R` starts an episode, `S` saves, `D` discards the active episode, and `Q` shuts down; `STANDING`, `MOCAP`, `ARMS`, and paused mocap are recordable -- Recording captures `observation.images.d435i_rgb` RealSense RGB video at 30Hz plus `observation.state(68)`, scalar `observation.mode`, and `action(36)` as the root-plus-joint reference consumed by the motion tracker; `action.hand(12)` is present when LinkerHand control is enabled, and `action.neck(2)` is present when OpenNeck control is enabled +- Recording captures `observation.images.d435i_rgb` RealSense RGB video at 30Hz plus `observation.state(68)`, scalar `observation.mode`, and `action(36)` as the root-plus-joint reference consumed by the motion tracker; `action.hand(12)` is present when LinkerHand control is enabled, and `action.neck(2)` stores the mechanically clamped OpenNeck `[yaw_deg, pitch_deg]` target when OpenNeck control is enabled - Sim2real recording uses an editable source layout: `schema.json`, `episodes.jsonl`, per-episode HDF5 files under `recording.output_dir/data/`, and compressed MP4 files under `recording.output_dir/videos/d435i_rgb/`; task prompts live only in `episodes.jsonl`, and HDF5 files contain only frame arrays with no metadata attributes or raw RGB datasets - Recording `schema.json` stores `robot_type=unitree_g1_29dof`, `hand_type=none|linkerhand_l6|linkerhand_o6`, `neck_type=none|openneck`, FPS, and feature definitions; optional action fields are controlled directly by `hands.enabled` and `neck.enabled`; the recording worker rejects an existing mismatched schema without writing episodes, but remains non-critical and must not stop the G1 control runtime; the previous attribute-based HDF5 layout is unsupported - Episodes interrupted before their `episodes.jsonl` entry is committed are discarded on the next recording-worker startup and do not consume an episode index @@ -164,7 +164,8 @@ target_dof_pos = clip(action, -10, 10) × action_scale + default_dof_pos - In `vr_hand_pose` mode, missing/inactive hand pose holds the last commanded pose for that side instead of opening the hand - Optional OpenNeck active-vision gimbal control uses `neck.enabled=true` and `neck.driver=openneck`; it requires `input.provider=pico4`, reuses the existing Pico body frame stream, and must not start a second `PicoBridge` - OpenNeck is integrated as a non-critical sim2real `neck_worker`; failures should not stop the G1 control loop, and no OpenNeck state is added to the 167D policy observation -- OpenNeck maps the absolute Pico `Head` orientation relative to `Spine3` with a fixed identity neutral pose and no neck-side EMA; it must not capture the first live frame as a runtime zero pose, so tracking can start while the operator's head is turned +- OpenNeck 0.2.0 is the supported runtime; Teleopit sends physical degrees through `move_deg()`, and the direct-drive OpenNeck package converts degrees to servo steps and clips them to its calibrated mechanical limits; the removed normalized API and config fields are unsupported +- OpenNeck maps the absolute Pico `Head` orientation relative to `Spine3` with a fixed identity neutral pose and no neck-side EMA; it must not capture the first live frame as a runtime zero pose, so tracking can start while the operator's head is turned; positive yaw turns left and positive pitch looks up ### SimulationLoop Runtime Behavior - `realtime=true` enforces wall-clock pacing even without a viewer diff --git a/README.md b/README.md index 9772a3b0..96eeb5e6 100644 --- a/README.md +++ b/README.md @@ -107,8 +107,8 @@ to its HDF5/video files and stores its editable task prompt. HDF5 contains only frame-aligned arrays: `observation.state(68)`, scalar `observation.mode`, and `action(36)` as the aligned reference qpos consumed by the motion tracker. `action.hand(12)` is present exactly when LinkerHand control is enabled, and -`action.neck(2)` contains the latest normalized OpenNeck yaw/pitch command exactly -when OpenNeck control is enabled. +`action.neck(2)` contains the latest mechanically clamped OpenNeck +`[yaw_deg, pitch_deg]` target when OpenNeck control is enabled. Recording is non-critical: an incompatible output schema stops only the recording worker while G1 control continues. Episodes interrupted before their manifest entry is committed are discarded on the next recording startup. @@ -130,7 +130,15 @@ python scripts/run/run_sim2real.py --config-name pico4_sim2real \ existing Teleopit Pico receiver and does not start another `PicoBridge` or camera pipeline. It maps the absolute `Head` orientation relative to `Spine3` with a fixed neutral pose and no neck-side EMA, so tracking startup does not -require the operator to face straight ahead. +require the operator to face straight ahead. Teleopit sends physical yaw/pitch +angles through the OpenNeck 0.2.0 `move_deg()` API; OpenNeck converts those +angles for its direct-drive servos and clips them to the calibrated mechanical +step limits. Positive yaw turns left and positive pitch looks up. + +OpenNeck 0.2.0 uses an angle-based calibration file and rejects the previous +normalized configuration fields. Re-run `openneck calibrate` before enabling +the neck worker, and set `neck.config_path` when the calibration file is not in +the runtime working directory. ## Documentation diff --git a/docs/docs/configuration/config-reference.md b/docs/docs/configuration/config-reference.md index 9e642a76..3e8f8258 100644 --- a/docs/docs/configuration/config-reference.md +++ b/docs/docs/configuration/config-reference.md @@ -164,20 +164,29 @@ sim2real worker and does not change the policy observation. Head motion is mapped as the absolute `Head` orientation relative to `Spine3`, using the fixed PICO neutral orientation and no neck-side EMA; startup does not capture the operator's first pose as a new zero pose, so the operator does not need to face -straight when tracking starts. +straight when tracking starts. Teleopit converts the supported PICO convention +to OpenNeck's physical convention—positive yaw turns left and positive pitch +looks up—and sends the relative angles in degrees through OpenNeck 0.2.0 +`move_deg()`. OpenNeck performs the direct-drive degree-to-step conversion and +clips each target to the mechanical step limits in its calibration file. + +OpenNeck 0.2.0 calibration files use angle-control fields such as +`yaw_center_step`, `yaw_min_step`, `yaw_max_step`, and `yaw_step_sign` (and the +corresponding pitch fields). The previous normalized OpenNeck configuration is +unsupported; run `openneck calibrate` to create a current file. Teleopit's +removed `neck.yaw_range_deg`, `neck.pitch_range_deg`, and `neck.invert_*` keys +are rejected rather than ignored. | Field | Description | Default | |-------|-------------|---------| | `neck.enabled` | Enable optional OpenNeck worker | `false` | | `neck.driver` | Neck driver plugin; currently `openneck` | `openneck` | -| `neck.config_path` | Optional OpenNeck calibration config path | `null` | +| `neck.config_path` | Optional OpenNeck 0.2.0 angle-calibration config path | `null` | | `neck.port` | Optional serial port override, for example `/dev/ttyACM0` | `null` | | `neck.rate_hz` | Maximum neck command rate in Hz | `60.0` | | `neck.frame_timeout_s` | Pico body-frame staleness threshold | `0.2` | | `neck.active_modes` | Sim2real modes that allow neck motion | `[standing, mocap, arms, pause]` | | `neck.dead_zone_deg` | Yaw/pitch dead zone in degrees | `0.5` | -| `neck.yaw_range_deg` / `pitch_range_deg` | Degrees mapped to normalized command magnitude `1.0` | `90.0` / `60.0` | -| `neck.invert_yaw` / `invert_pitch` | Invert OpenNeck command direction per axis | `true` / `true` | | `neck.center_on_start` / `center_on_shutdown` | Center the gimbal at worker startup/shutdown | `true` / `false` | | `neck.release_on_shutdown` | Release servo torque after shutdown when supported | `false` | | `neck.dry_run` | Compute commands without opening OpenNeck hardware | `false` | @@ -267,8 +276,10 @@ high-level reference consumed by the motion tracker, not the tracker policy's raw output or the final joint targets sent to G1. `action.hand` is the latest LinkerHand command from the hand worker: `left_pose(6) + right_pose(6)`, using the SDK's 0-255 pose values. -`action.neck` is the latest command successfully sent to OpenNeck by the neck -worker: normalized `[yaw, pitch]`, each in `[-1, 1]`. +`action.neck` is the latest mechanically clamped target returned by OpenNeck +after a successful command: `[yaw_deg, pitch_deg]` in degrees. Positive yaw +turns left and positive pitch looks up. The reachable range comes from the +OpenNeck calibration file and is therefore not fixed in the recording schema. ## Critical: `default_dof_pos` diff --git a/docs/docs/getting-started/installation.md b/docs/docs/getting-started/installation.md index 89761893..fc781124 100644 --- a/docs/docs/getting-started/installation.md +++ b/docs/docs/getting-started/installation.md @@ -73,14 +73,17 @@ bash scripts/setup/download_somehand_assets.sh These packages are only required when `hands.enabled=true`. -Optional OpenNeck active-vision control for Pico sim2real uses the remote -OpenNeck package: +Optional OpenNeck active-vision control for Pico sim2real uses the latest +OpenNeck angle-control package: ```bash pip install -e '.[openneck]' ``` This extra includes the Pico stack and is only required when `neck.enabled=true`. +OpenNeck 0.2.0 calibration files use `*_center_step`, `*_min_step`, +`*_max_step`, and `*_step_sign`; the previous normalized configuration format +is unsupported. Run `openneck calibrate` to create a current calibration file. ### Sim2Real Recording diff --git a/docs/docs/tutorials/pico-sim2real.md b/docs/docs/tutorials/pico-sim2real.md index b06d34c8..0093e5bb 100644 --- a/docs/docs/tutorials/pico-sim2real.md +++ b/docs/docs/tutorials/pico-sim2real.md @@ -123,8 +123,9 @@ under `data/recordings/sim2real_hdf5/data/`, with compressed MP4 files under episode. HDF5 stores `frame_index`, `timestamp`, `observation.state(68)`, scalar `observation.mode`, and the 36D motion-tracker reference `action` at 30 Hz. When hand control is enabled, it also stores `action.hand(12)`. When OpenNeck -control is enabled, it stores the latest normalized yaw/pitch command as -`action.neck(2)`. Disabled devices do not add their action fields. +control is enabled, it stores the latest mechanically clamped +`[yaw_deg, pitch_deg]` target in degrees as `action.neck(2)`. Disabled devices +do not add their action fields. ## Operator Flow diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md index f0c06b0c..433a8d8c 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md @@ -180,19 +180,26 @@ MuJoCo 窗口显示重定向参考；`sim2sim`、`mocap`、`camera` 和 `all` 管线。OpenNeck 作为非关键 sim2real worker 运行，不会改变策略观测。头部运动使用固定的 PICO 中立姿态且不进行颈部侧 EMA，按照 `Head` 相对于 `Spine3` 的绝对朝向进行映射； 启动时不会把操作者的第一帧姿态采集为新的零位，因此开始追踪时操作者不需要保持头部朝正前方。 +Teleopit 将受支持的 PICO 约定转换为 OpenNeck 的物理约定——正 yaw 向左转，正 pitch +向上看——并通过 OpenNeck 0.2.0 的 `move_deg()` 发送以度为单位的相对角度。OpenNeck +负责直驱角度到舵机步数的转换，并将每个目标裁剪到标定文件中的机械步数限位。 + +OpenNeck 0.2.0 标定文件使用 `yaw_center_step`、`yaw_min_step`、 +`yaw_max_step` 和 `yaw_step_sign` 等角度控制字段（pitch 使用对应字段）。不支持以前的 +OpenNeck 归一化配置；运行 `openneck calibrate` 创建当前格式的文件。Teleopit 已移除的 +`neck.yaw_range_deg`、`neck.pitch_range_deg` 和 `neck.invert_*` 键会被拒绝， +而不是被忽略。 | 字段 | 说明 | 默认值 | |---|---|---| | `neck.enabled` | 启用可选 OpenNeck worker | `false` | | `neck.driver` | 头颈设备驱动插件；当前为 `openneck` | `openneck` | -| `neck.config_path` | 可选 OpenNeck 校准配置路径 | `null` | +| `neck.config_path` | 可选 OpenNeck 0.2.0 角度标定配置路径 | `null` | | `neck.port` | 可选串口覆盖，例如 `/dev/ttyACM0` | `null` | | `neck.rate_hz` | 最大头颈命令频率（Hz） | `60.0` | | `neck.frame_timeout_s` | Pico body frame 过期阈值 | `0.2` | | `neck.active_modes` | 允许头颈运动的 sim2real 模式 | `[standing, mocap, arms, pause]` | | `neck.dead_zone_deg` | yaw/pitch 死区（度） | `0.5` | -| `neck.yaw_range_deg` / `pitch_range_deg` | 映射到归一化命令幅值 `1.0` 的角度 | `90.0` / `60.0` | -| `neck.invert_yaw` / `invert_pitch` | 按轴反转 OpenNeck 命令方向 | `true` / `true` | | `neck.center_on_start` / `center_on_shutdown` | worker 启动/关闭时回中云台 | `true` / `false` | | `neck.release_on_shutdown` | 关闭后在支持时释放舵机扭矩 | `false` | | `neck.dry_run` | 只计算命令，不打开 OpenNeck 硬件 | `false` | @@ -273,5 +280,6 @@ HDF5 文件仅包含上述逐帧数组，不保存录制元数据根属性。RGB 消费的高层参考，不是 tracker policy 的原始输出，也不是最终下发给 G1 的关节目标。 `action.hand` 是手部 worker 最新的 LinkerHand 命令： `left_pose(6) + right_pose(6)`，使用 SDK 的 0-255 pose 数值。 -`action.neck` 是颈部 worker 最近一次成功发送给 OpenNeck 的命令：归一化的 -`[yaw, pitch]`，两个值的范围均为 `[-1, 1]`。 +`action.neck` 是 OpenNeck 成功执行命令后返回的最新机械限位裁剪目标：以度为单位的 +`[yaw_deg, pitch_deg]`。正 yaw 向左转，正 pitch 向上看。可达范围来自 OpenNeck +标定文件，因此录制 schema 中没有固定范围。 diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/installation.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/installation.md index 5dd89b68..99998229 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/installation.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/installation.md @@ -73,13 +73,16 @@ bash scripts/setup/download_somehand_assets.sh 只有在 `hands.enabled=true` 时才需要安装这些包。 -Pico sim2real 可选的 OpenNeck 主动视觉控制使用远程 OpenNeck 包： +Pico sim2real 可选的 OpenNeck 主动视觉控制使用最新的 OpenNeck 角度控制包： ```bash pip install -e '.[openneck]' ``` -该 extra 包含 Pico 栈，只有在 `neck.enabled=true` 时才需要安装。 +该 extra 包含 Pico 栈，只有在 `neck.enabled=true` 时才需要安装。OpenNeck 0.2.0 +标定文件使用 `*_center_step`、`*_min_step`、`*_max_step` 和 +`*_step_sign`；不支持以前的归一化配置格式。运行 `openneck calibrate` +创建当前格式的标定文件。 ### Sim2Real 录制 diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md index 40deea79..7a84c19f 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md @@ -117,8 +117,9 @@ episode 会保存为 `data/recordings/sim2real_hdf5/data/` 下的 `.h5` 文件 `episodes.jsonl` 保存每个 episode 的文件映射与可编辑任务 prompt。HDF5 以 30 Hz 保存 `frame_index`、`timestamp`、`observation.state(68)`、标量 `observation.mode` 和作为 motion-tracker reference 的 36D `action`。启用灵巧手 -控制时还会保存 `action.hand(12)`。启用 OpenNeck 控制时，会把最新的归一化 -yaw/pitch 命令保存为 `action.neck(2)`。未启用的设备不会添加对应的 action 字段。 +控制时还会保存 `action.hand(12)`。启用 OpenNeck 控制时，会把最新经过机械限位裁剪的 +`[yaw_deg, pitch_deg]` 目标以度为单位保存为 `action.neck(2)`。未启用的设备不会添加 +对应的 action 字段。 ## 操作流程 diff --git a/scripts/dev/test_openneck.py b/scripts/dev/test_openneck.py index f42390aa..20ab35dc 100644 --- a/scripts/dev/test_openneck.py +++ b/scripts/dev/test_openneck.py @@ -21,7 +21,7 @@ DEFAULT_RATE_HZ = 60.0 DEFAULT_FRAME_TIMEOUT_S = 0.3 -DEFAULT_STEP_MAGNITUDE = 0.25 +DEFAULT_TEST_ANGLE_DEG = 5.0 DEFAULT_HOLD_S = 0.8 DEFAULT_PICO_TIMEOUT_S = 60.0 @@ -43,21 +43,17 @@ def parse_args() -> argparse.Namespace: parser.add_argument("--rate-hz", type=float, default=DEFAULT_RATE_HZ) parser.add_argument("--frame-timeout-s", type=float, default=DEFAULT_FRAME_TIMEOUT_S) parser.add_argument( - "--magnitude", + "--angle-deg", type=float, - default=DEFAULT_STEP_MAGNITUDE, - help="Normalized direct-test command magnitude in [0, 1]. Keep this conservative.", + default=DEFAULT_TEST_ANGLE_DEG, + help="Direct-test angle magnitude in degrees. Keep this conservative.", ) parser.add_argument("--hold-s", type=float, default=DEFAULT_HOLD_S, help="Seconds to hold each direct-test command") parser.add_argument("--duration-s", type=float, default=0.0, help="Pico mode duration; 0 means until Ctrl-C") parser.add_argument("--no-center-on-start", action="store_true") parser.add_argument("--no-center-on-shutdown", action="store_true") parser.add_argument("--release-on-shutdown", action="store_true") - parser.add_argument("--invert-yaw", action=argparse.BooleanOptionalAction, default=True) - parser.add_argument("--invert-pitch", action=argparse.BooleanOptionalAction, default=True) parser.add_argument("--dead-zone-deg", type=float, default=0.5) - parser.add_argument("--yaw-range-deg", type=float, default=90.0) - parser.add_argument("--pitch-range-deg", type=float, default=60.0) parser.add_argument("--bridge-host", default="0.0.0.0") parser.add_argument("--bridge-port", type=int, default=63901) parser.add_argument("--bridge-discovery", action=argparse.BooleanOptionalAction, default=True) @@ -71,8 +67,8 @@ def parse_args() -> argparse.Namespace: raise SystemExit("--hold-s must be > 0") if args.duration_s < 0: raise SystemExit("--duration-s must be >= 0") - if not 0.0 <= args.magnitude <= 1.0: - raise SystemExit("--magnitude must be in [0, 1]") + if args.angle_deg <= 0.0: + raise SystemExit("--angle-deg must be > 0") return args @@ -86,10 +82,6 @@ def make_neck_config(args: argparse.Namespace) -> NeckConfig: frame_timeout_s=args.frame_timeout_s, active_modes=("mocap",), dead_zone_deg=args.dead_zone_deg, - yaw_range_deg=args.yaw_range_deg, - pitch_range_deg=args.pitch_range_deg, - invert_yaw=bool(args.invert_yaw), - invert_pitch=bool(args.invert_pitch), center_on_start=not bool(args.no_center_on_start), center_on_shutdown=not bool(args.no_center_on_shutdown), release_on_shutdown=bool(args.release_on_shutdown), @@ -114,31 +106,31 @@ def make_pico_provider(args: argparse.Namespace) -> Pico4InputProvider: def run_direct(args: argparse.Namespace) -> None: cfg = make_neck_config(args) device = build_neck_device(cfg) - magnitude = float(args.magnitude) + angle_deg = float(args.angle_deg) pattern = [ ("center", 0.0, 0.0), - ("yaw right", magnitude, 0.0), + ("yaw left", angle_deg, 0.0), ("center", 0.0, 0.0), - ("yaw left", -magnitude, 0.0), + ("yaw right", -angle_deg, 0.0), ("center", 0.0, 0.0), - ("pitch up", 0.0, magnitude), + ("pitch up", 0.0, angle_deg), ("center", 0.0, 0.0), - ("pitch down", 0.0, -magnitude), + ("pitch down", 0.0, -angle_deg), ("center", 0.0, 0.0), ] print( f"Testing OpenNeck direct pattern | port={args.port} dry_run={args.dry_run} " - f"magnitude={magnitude:.2f}", + f"angle={angle_deg:.2f}deg", flush=True, ) try: device.connect() if cfg.center_on_start: device.center() - for label, yaw, pitch in pattern: - print(f"{label}: yaw={yaw:.3f} pitch={pitch:.3f}", flush=True) - device.move_norm(yaw, pitch) + for label, yaw_deg, pitch_deg in pattern: + print(f"{label}: yaw={yaw_deg:.2f}deg pitch={pitch_deg:.2f}deg", flush=True) + device.move_deg(yaw_deg, pitch_deg) time.sleep(float(args.hold_s)) except KeyboardInterrupt: print("Interrupted; shutting down OpenNeck", flush=True) @@ -147,7 +139,7 @@ def run_direct(args: argparse.Namespace) -> None: if cfg.center_on_shutdown: device.center() if cfg.release_on_shutdown: - device.release() + device.release_torque() finally: device.close() diff --git a/teleopit/configs/pico4_sim2real.yaml b/teleopit/configs/pico4_sim2real.yaml index 12a85c8c..545b3449 100644 --- a/teleopit/configs/pico4_sim2real.yaml +++ b/teleopit/configs/pico4_sim2real.yaml @@ -115,10 +115,6 @@ neck: frame_timeout_s: 0.2 active_modes: [standing, mocap, arms, pause] dead_zone_deg: 0.5 - yaw_range_deg: 90.0 - pitch_range_deg: 60.0 - invert_yaw: true - invert_pitch: true center_on_start: true center_on_shutdown: false release_on_shutdown: false diff --git a/teleopit/configs/sim2real.yaml b/teleopit/configs/sim2real.yaml index 267c296e..b63a608f 100644 --- a/teleopit/configs/sim2real.yaml +++ b/teleopit/configs/sim2real.yaml @@ -116,10 +116,6 @@ neck: frame_timeout_s: 0.2 active_modes: [standing, mocap, arms, pause] dead_zone_deg: 0.5 - yaw_range_deg: 90.0 - pitch_range_deg: 60.0 - invert_yaw: true - invert_pitch: true center_on_start: true center_on_shutdown: false release_on_shutdown: false diff --git a/teleopit/recording/hdf5.py b/teleopit/recording/hdf5.py index 14af97aa..2eb67544 100644 --- a/teleopit/recording/hdf5.py +++ b/teleopit/recording/hdf5.py @@ -37,7 +37,7 @@ NECK_ACTION_DIM = 2 DEFAULT_IMAGE_SHAPE = (480, 640, 3) HDF5_RECORDING_FORMAT = "teleopit_hdf5" -HDF5_RECORDING_VERSION = 2 +HDF5_RECORDING_VERSION = 3 DEFAULT_ROBOT_TYPE = "unitree_g1_29dof" NO_HAND_TYPE = "none" SUPPORTED_HAND_TYPES = (NO_HAND_TYPE, "linkerhand_l6", "linkerhand_o6") @@ -194,9 +194,8 @@ def hdf5_schema(schema: RecordingSchema) -> dict[str, object]: features[schema.neck_action_key] = { "dtype": "float32", "shape": [schema.neck_action_dim], - "names": ["yaw", "pitch"], - "units": "normalized", - "range": [-1.0, 1.0], + "names": ["yaw_deg", "pitch_deg"], + "units": "degrees", } features[schema.image_key] = { "dtype": "video", @@ -258,8 +257,8 @@ def normalize_hand_action(left_pose: object, right_pose: object) -> np.ndarray: return action -def normalize_neck_action(yaw: object, pitch: object) -> np.ndarray: - action = np.asarray([yaw, pitch], dtype=np.float32).reshape(-1) +def build_neck_action(yaw_deg: object, pitch_deg: object) -> np.ndarray: + action = np.asarray([yaw_deg, pitch_deg], dtype=np.float32).reshape(-1) if action.shape[0] != NECK_ACTION_DIM: raise ValueError(f"recording action.neck must be {NECK_ACTION_DIM}D, got {action.shape[0]}") return action diff --git a/teleopit/sim2real/mp/messages.py b/teleopit/sim2real/mp/messages.py index d929b1aa..2a166a6d 100644 --- a/teleopit/sim2real/mp/messages.py +++ b/teleopit/sim2real/mp/messages.py @@ -87,8 +87,8 @@ class NeckCommandPacket: timestamp_s: float driver: str active: bool - yaw: float - pitch: float + yaw_deg: float + pitch_deg: float seq: int diff --git a/teleopit/sim2real/mp/runtime.py b/teleopit/sim2real/mp/runtime.py index 70c88f07..44b42f6b 100644 --- a/teleopit/sim2real/mp/runtime.py +++ b/teleopit/sim2real/mp/runtime.py @@ -52,7 +52,7 @@ build_recording_schema, normalize_action_reference_qpos, normalize_hand_action, - normalize_neck_action, + build_neck_action, ) from teleopit.sim.reference_motion import OfflineReferenceMotion from teleopit.sim.reference_timeline import ReferenceTimeline, ReferenceWindow, ReferenceWindowBuilder @@ -1892,8 +1892,8 @@ def __init__( timestamp_s=0.0, driver=str(cfg_get(cfg_get(cfg, "neck", {}) or {}, "driver", "openneck")).strip().lower(), active=False, - yaw=0.0, - pitch=0.0, + yaw_deg=0.0, + pitch_deg=0.0, seq=0, ) self._latest_video_seq = -1 @@ -2047,9 +2047,9 @@ def _handle_video(self, descriptor: SharedFrameDescriptor) -> None: else None ) neck_action = ( - normalize_neck_action( - self._latest_neck_command.yaw, - self._latest_neck_command.pitch, + build_neck_action( + self._latest_neck_command.yaw_deg, + self._latest_neck_command.pitch_deg, ) if self._schema.has_neck_action else None @@ -2103,7 +2103,13 @@ def _main() -> None: sleep_s = 1.0 / max(float(neck_cfg.rate_hz), 1.0) last_status_s = 0.0 - def _publish_neck_command(*, timestamp_s: float, active: bool, yaw: float, pitch: float) -> None: + def _publish_neck_command( + *, + timestamp_s: float, + active: bool, + yaw_deg: float, + pitch_deg: float, + ) -> None: nonlocal command_seq if neck_command_pub is None: return @@ -2114,8 +2120,8 @@ def _publish_neck_command(*, timestamp_s: float, active: bool, yaw: float, pitch timestamp_s=float(timestamp_s), driver=neck_cfg.driver, active=bool(active), - yaw=float(yaw), - pitch=float(pitch), + yaw_deg=float(yaw_deg), + pitch_deg=float(pitch_deg), seq=command_seq, ), ) @@ -2123,7 +2129,12 @@ def _publish_neck_command(*, timestamp_s: float, active: bool, yaw: float, pitch try: runtime.start() if neck_cfg.center_on_start: - _publish_neck_command(timestamp_s=time.monotonic(), active=False, yaw=0.0, pitch=0.0) + _publish_neck_command( + timestamp_s=time.monotonic(), + active=False, + yaw_deg=0.0, + pitch_deg=0.0, + ) while not stop_event.is_set(): runtime_command = command_sub.recv_latest() if isinstance(runtime_command, CommandPacket) and runtime_command.command == "shutdown": @@ -2152,8 +2163,8 @@ def _publish_neck_command(*, timestamp_s: float, active: bool, yaw: float, pitch _publish_neck_command( timestamp_s=now_s, active=active, - yaw=neck_command.yaw, - pitch=neck_command.pitch, + yaw_deg=neck_command.yaw_deg, + pitch_deg=neck_command.pitch_deg, ) except Exception: logger.exception("OpenNeck worker tick failed; neck control continues") diff --git a/teleopit/sim2real/neck/config.py b/teleopit/sim2real/neck/config.py index c2e12bff..fb3d2128 100644 --- a/teleopit/sim2real/neck/config.py +++ b/teleopit/sim2real/neck/config.py @@ -8,6 +8,12 @@ from teleopit.runtime.common import cfg_get VALID_NECK_ACTIVE_MODES = frozenset(("standing", "mocap", "arms", "pause")) +REMOVED_NECK_CONFIG_KEYS = ( + "yaw_range_deg", + "pitch_range_deg", + "invert_yaw", + "invert_pitch", +) @dataclass(frozen=True) @@ -20,10 +26,6 @@ class NeckConfig: frame_timeout_s: float = 0.2 active_modes: tuple[str, ...] = ("standing", "mocap", "arms", "pause") dead_zone_deg: float = 0.5 - yaw_range_deg: float = 90.0 - pitch_range_deg: float = 60.0 - invert_yaw: bool = True - invert_pitch: bool = True center_on_start: bool = True center_on_shutdown: bool = False release_on_shutdown: bool = False @@ -32,6 +34,13 @@ class NeckConfig: def parse_neck_config(cfg: Any) -> NeckConfig: neck_cfg = cfg_get(cfg, "neck", {}) or {} + removed = [key for key in REMOVED_NECK_CONFIG_KEYS if cfg_get(neck_cfg, key, None) is not None] + if removed: + raise ValueError( + "Removed normalized OpenNeck config key(s): " + f"{', '.join(removed)}. Teleopit now sends head angles in degrees; " + "configure motor direction and mechanical limits in the OpenNeck calibration file." + ) active_modes = _parse_active_modes(cfg_get(neck_cfg, "active_modes", ["standing", "mocap", "arms", "pause"])) rate_hz = float(cfg_get(neck_cfg, "rate_hz", 60.0)) if rate_hz <= 0: @@ -42,12 +51,6 @@ def parse_neck_config(cfg: Any) -> NeckConfig: dead_zone_deg = float(cfg_get(neck_cfg, "dead_zone_deg", 0.5)) if dead_zone_deg < 0: raise ValueError("neck.dead_zone_deg must be >= 0") - yaw_range_deg = float(cfg_get(neck_cfg, "yaw_range_deg", 90.0)) - pitch_range_deg = float(cfg_get(neck_cfg, "pitch_range_deg", 60.0)) - if yaw_range_deg <= 0: - raise ValueError("neck.yaw_range_deg must be > 0") - if pitch_range_deg <= 0: - raise ValueError("neck.pitch_range_deg must be > 0") config_path = cfg_get(neck_cfg, "config_path", None) if config_path in ("", "null"): config_path = None @@ -65,10 +68,6 @@ def parse_neck_config(cfg: Any) -> NeckConfig: frame_timeout_s=frame_timeout_s, active_modes=active_modes, dead_zone_deg=dead_zone_deg, - yaw_range_deg=yaw_range_deg, - pitch_range_deg=pitch_range_deg, - invert_yaw=bool(cfg_get(neck_cfg, "invert_yaw", True)), - invert_pitch=bool(cfg_get(neck_cfg, "invert_pitch", True)), center_on_start=bool(cfg_get(neck_cfg, "center_on_start", True)), center_on_shutdown=bool(cfg_get(neck_cfg, "center_on_shutdown", False)), release_on_shutdown=bool(cfg_get(neck_cfg, "release_on_shutdown", False)), diff --git a/teleopit/sim2real/neck/mapper.py b/teleopit/sim2real/neck/mapper.py index 644f392f..f8486c5d 100644 --- a/teleopit/sim2real/neck/mapper.py +++ b/teleopit/sim2real/neck/mapper.py @@ -17,15 +17,13 @@ @dataclass(frozen=True) class NeckCommand: - yaw: float - pitch: float yaw_deg: float pitch_deg: float roll_deg: float class HeadPoseMapper: - """Map Teleopit Pico body frames to normalized active-neck yaw/pitch commands.""" + """Map Teleopit Pico body frames to OpenNeck yaw/pitch angles.""" def __init__(self, config: NeckConfig) -> None: self._cfg = config @@ -41,20 +39,16 @@ def map_frame(self, frame: HumanFrame) -> NeckCommand | None: # PICO convention, so their relative identity is the fixed zero pose. q_cmd = _qmul(_qconj(q_body), q_head) yaw_deg, pitch_deg, roll_deg = _openneck_yaw_pitch_roll_deg(q_cmd) - if self._cfg.invert_yaw: - yaw_deg = -yaw_deg - if self._cfg.invert_pitch: - pitch_deg = -pitch_deg + # Convert the supported PICO convention to OpenNeck's physical command + # convention: positive yaw turns left and positive pitch looks up. + yaw_deg = -yaw_deg + pitch_deg = -pitch_deg if abs(yaw_deg) < self._cfg.dead_zone_deg: yaw_deg = 0.0 if abs(pitch_deg) < self._cfg.dead_zone_deg: pitch_deg = 0.0 - yaw = yaw_deg / self._cfg.yaw_range_deg - pitch = pitch_deg / self._cfg.pitch_range_deg return NeckCommand( - yaw=float(np.clip(yaw, -1.0, 1.0)), - pitch=float(np.clip(pitch, -1.0, 1.0)), yaw_deg=float(yaw_deg), pitch_deg=float(pitch_deg), roll_deg=float(roll_deg), diff --git a/teleopit/sim2real/neck/openneck.py b/teleopit/sim2real/neck/openneck.py index 64574ccf..2db7af91 100644 --- a/teleopit/sim2real/neck/openneck.py +++ b/teleopit/sim2real/neck/openneck.py @@ -8,14 +8,31 @@ logger = logging.getLogger(__name__) +def _load_openneck_controller() -> type: + try: + from openneck import OpenNeckController + except ModuleNotFoundError as exc: + raise ImportError( + "OpenNeck 0.2.0 is required for neck.driver=openneck. " + "Install with: pip install -e '.[openneck]'" + ) from exc + if not callable(getattr(OpenNeckController, "move_deg", None)): + raise ImportError( + "OpenNeck 0.2.0 angle API is required; reinstall with: " + "pip install --force-reinstall --no-deps " + "'openneck @ git+https://github.com/BotRunner64/OpenNeck.git'" + ) + return OpenNeckController + + class NeckDevice(Protocol): def connect(self) -> None: ... def center(self) -> None: ... - def release(self) -> None: ... + def release_torque(self) -> None: ... - def move_norm(self, yaw: float, pitch: float) -> None: ... + def move_deg(self, yaw_deg: float, pitch_deg: float) -> tuple[float, float]: ... def close(self) -> None: ... @@ -23,98 +40,79 @@ def close(self) -> None: ... class OpenNeckDevice: def __init__(self, config: NeckConfig) -> None: self._cfg = config - self._context = None self._controller = None def connect(self) -> None: - try: - from openneck import OpenNeckController - except ModuleNotFoundError as exc: - raise ImportError( - "openneck is required for neck.driver=openneck. " - "Install with: pip install -e '.[openneck]'" - ) from exc + OpenNeckController = _load_openneck_controller() controller = OpenNeckController( config=self._cfg.config_path, port=self._cfg.port, - enable_torque_on_connect=True, ) - entered = controller.__enter__() - self._context = controller - self._controller = controller if entered is None else entered + controller.connect() + self._controller = controller logger.info("OpenNeck connected on port %s", getattr(self._controller, "port", self._cfg.port)) def center(self) -> None: if self._controller is not None: - self._controller.center(wait_s=0.5) - - def move_norm(self, yaw: float, pitch: float) -> None: - if self._controller is not None: - self._controller.move_norm(float(yaw), float(pitch)) + self._controller.center() - def release(self) -> None: + def move_deg(self, yaw_deg: float, pitch_deg: float) -> tuple[float, float]: if self._controller is None: - return - release = getattr(self._controller, "release", None) - if callable(release): - release() - return - disable_torque = getattr(self._controller, "disable_torque", None) - if callable(disable_torque): - disable_torque() + raise RuntimeError("OpenNeck is not connected") + applied = self._controller.move_deg(float(yaw_deg), float(pitch_deg)) + return float(applied.yaw_deg), float(applied.pitch_deg) + + def release_torque(self) -> None: + if self._controller is not None: + self._controller.release_torque() def close(self) -> None: - context = self._context controller = self._controller - self._context = None self._controller = None - close_error: BaseException | None = None - if context is not None: - exit_context = getattr(context, "__exit__", None) - if callable(exit_context): - try: - exit_context(None, None, None) - return - except BaseException as exc: - close_error = exc - logger.exception("OpenNeck context exit failed; trying direct close") - close_targets = [target for target in (controller, context) if target is not None] - seen_target_ids: set[int] = set() - direct_close_error: BaseException | None = None - for target in close_targets: - target_id = id(target) - if target_id in seen_target_ids: - continue - seen_target_ids.add(target_id) - close = getattr(target, "close", None) - if callable(close): - try: - close() - except BaseException as exc: - direct_close_error = exc - logger.exception("OpenNeck direct close failed") - if close_error is not None: - if direct_close_error is not None: - raise close_error from direct_close_error - raise close_error - if direct_close_error is not None: - raise direct_close_error + if controller is not None: + controller.close() class DryRunNeckDevice: + def __init__(self, config: NeckConfig) -> None: + self._cfg = config + self._controller = None + def connect(self) -> None: + OpenNeckController = _load_openneck_controller() + self._controller = OpenNeckController( + config=self._cfg.config_path, + port=self._cfg.port, + ) logger.info("OpenNeck dry-run device active") def center(self) -> None: logger.info("OpenNeck dry-run center") - def move_norm(self, yaw: float, pitch: float) -> None: - logger.debug("OpenNeck dry-run command yaw=%.3f pitch=%.3f", yaw, pitch) + def move_deg(self, yaw_deg: float, pitch_deg: float) -> tuple[float, float]: + controller = self._controller + if controller is None: + raise RuntimeError("OpenNeck dry-run device is not connected") + # Reuse OpenNeck's calibration conversion without writing to + # its servo driver, so dry-run reports the same clamped target. + yaw_step = controller._angle_to_step("yaw", float(yaw_deg)) + pitch_step = controller._angle_to_step("pitch", float(pitch_deg)) + applied_yaw_deg = float(controller._step_to_angle("yaw", yaw_step)) + applied_pitch_deg = float(controller._step_to_angle("pitch", pitch_step)) + logger.debug( + "OpenNeck dry-run command yaw=%.3fdeg pitch=%.3fdeg applied_yaw=%.3fdeg applied_pitch=%.3fdeg", + yaw_deg, + pitch_deg, + applied_yaw_deg, + applied_pitch_deg, + ) + return applied_yaw_deg, applied_pitch_deg - def release(self) -> None: + def release_torque(self) -> None: logger.info("OpenNeck dry-run release") def close(self) -> None: + self._controller = None logger.info("OpenNeck dry-run closed") @@ -122,5 +120,5 @@ def build_neck_device(config: NeckConfig) -> NeckDevice: if config.driver != "openneck": raise ValueError("Unsupported neck.driver={!r}; supported drivers: openneck".format(config.driver)) if config.dry_run: - return DryRunNeckDevice() + return DryRunNeckDevice(config) return OpenNeckDevice(config) diff --git a/teleopit/sim2real/neck/worker.py b/teleopit/sim2real/neck/worker.py index 8d6ca4af..f4b7cab6 100644 --- a/teleopit/sim2real/neck/worker.py +++ b/teleopit/sim2real/neck/worker.py @@ -39,8 +39,15 @@ def tick( command = self._mapper.map_frame(frame) if command is None: return None - self._device.move_norm(command.yaw, command.pitch) - return command + applied_yaw_deg, applied_pitch_deg = self._device.move_deg( + command.yaw_deg, + command.pitch_deg, + ) + return NeckCommand( + yaw_deg=applied_yaw_deg, + pitch_deg=applied_pitch_deg, + roll_deg=command.roll_deg, + ) def close(self) -> None: try: @@ -51,7 +58,7 @@ def close(self) -> None: logger.exception("Failed to center OpenNeck on shutdown; closing device") if self._cfg.release_on_shutdown: try: - self._device.release() + self._device.release_torque() except Exception: logger.exception("Failed to release OpenNeck torque on shutdown; closing device") finally: diff --git a/tests/test_active_neck.py b/tests/test_active_neck.py index d7ac46cf..7c684b60 100644 --- a/tests/test_active_neck.py +++ b/tests/test_active_neck.py @@ -8,7 +8,7 @@ from teleopit.inputs.pico4_provider import BODY_JOINT_NAMES, Pico4InputProvider from teleopit.sim2real.neck.config import NeckConfig, parse_neck_config from teleopit.sim2real.neck.mapper import HeadPoseMapper -from teleopit.sim2real.neck.openneck import OpenNeckDevice +from teleopit.sim2real.neck.openneck import DryRunNeckDevice, OpenNeckDevice from teleopit.sim2real.neck.worker import NeckRuntime, body_packet_frame @@ -30,56 +30,59 @@ def _frame(head: np.ndarray, spine: np.ndarray | None = None): return frame -def test_head_pose_mapper_maps_fixed_neutral_yaw_pitch_without_startup_calibration() -> None: - cfg = NeckConfig( - enabled=True, - invert_yaw=False, - invert_pitch=False, - dead_zone_deg=0.0, - ) - mapper = HeadPoseMapper(cfg) +class FakeDevice: + def __init__(self) -> None: + self.moves: list[tuple[float, float]] = [] + self.center_calls = 0 + self.released = False + self.closed = False + + def connect(self) -> None: + return None + + def center(self) -> None: + self.center_calls += 1 + + def release_torque(self) -> None: + self.released = True + + def move_deg(self, yaw_deg: float, pitch_deg: float) -> tuple[float, float]: + self.moves.append((yaw_deg, pitch_deg)) + return max(-20.0, min(20.0, yaw_deg)), max(-10.0, min(10.0, pitch_deg)) + + def close(self) -> None: + self.closed = True + + +def test_head_pose_mapper_maps_fixed_pico_convention_to_openneck_degrees() -> None: + mapper = HeadPoseMapper(NeckConfig(enabled=True, dead_zone_deg=0.0)) command = mapper.map_frame(_frame(_quat_y(30.0), _quat_y(0.0))) assert command is not None - assert command.yaw_deg == pytest_approx(30.0) - assert command.yaw == pytest_approx(30.0 / 90.0) + assert command.yaw_deg == pytest_approx(-30.0) command = mapper.map_frame(_frame(_quat_y(0.0), _quat_y(0.0))) assert command is not None assert command.yaw_deg == pytest_approx(0.0) - assert command.yaw == pytest_approx(0.0) command = mapper.map_frame(_frame(_quat_x(15.0), _quat_x(0.0))) assert command is not None - assert command.pitch_deg == pytest_approx(15.0) - assert command.pitch == pytest_approx(15.0 / 60.0) + assert command.pitch_deg == pytest_approx(-15.0) def test_head_pose_mapper_uses_body_relative_orientation() -> None: - cfg = NeckConfig( - enabled=True, - invert_yaw=False, - dead_zone_deg=0.0, - ) - mapper = HeadPoseMapper(cfg) + mapper = HeadPoseMapper(NeckConfig(enabled=True, dead_zone_deg=0.0)) command = mapper.map_frame(_frame(_quat_y(40.0), _quat_y(10.0))) assert command is not None - assert command.yaw_deg == pytest_approx(30.0) + assert command.yaw_deg == pytest_approx(-30.0) def test_head_pose_mapper_handles_converted_pico_neutral_and_yaw() -> None: body_poses = np.zeros((len(BODY_JOINT_NAMES), 7), dtype=np.float64) body_poses[:, 6] = 1.0 - mapper = HeadPoseMapper( - NeckConfig( - enabled=True, - invert_yaw=False, - invert_pitch=False, - dead_zone_deg=0.0, - ) - ) + mapper = HeadPoseMapper(NeckConfig(enabled=True, dead_zone_deg=0.0)) neutral = mapper.map_frame(Pico4InputProvider._convert_body_joints_to_frame(body_poses)) assert neutral is not None @@ -91,7 +94,7 @@ def test_head_pose_mapper_handles_converted_pico_neutral_and_yaw() -> None: body_poses[head_idx, 6] = math.cos(math.radians(30.0) / 2.0) command = mapper.map_frame(Pico4InputProvider._convert_body_joints_to_frame(body_poses)) assert command is not None - assert command.yaw_deg == pytest_approx(30.0) + assert command.yaw_deg == pytest_approx(-30.0) assert command.pitch_deg == pytest_approx(0.0) @@ -101,32 +104,10 @@ def test_head_pose_mapper_requires_spine3_joint() -> None: assert mapper.map_frame(_frame(_quat_y(30.0))) is None -def test_neck_runtime_sends_relative_command_on_first_active_frame() -> None: - class FakeDevice: - def __init__(self) -> None: - self.moves: list[tuple[float, float]] = [] - self.center_calls = 0 - self.closed = False - - def connect(self) -> None: - return None - - def center(self) -> None: - self.center_calls += 1 - - def release(self) -> None: - return None - - def move_norm(self, yaw: float, pitch: float) -> None: - self.moves.append((yaw, pitch)) - - def close(self) -> None: - self.closed = True - +def test_neck_runtime_sends_degrees_and_returns_applied_target() -> None: device = FakeDevice() cfg = NeckConfig( enabled=True, - invert_yaw=False, dead_zone_deg=0.0, center_on_start=True, center_on_shutdown=True, @@ -134,7 +115,6 @@ def close(self) -> None: runtime = NeckRuntime(cfg, device=device) runtime.start() - assert device.center_calls == 1 command = runtime.tick( frame=_frame(_quat_y(30.0), _quat_y(0.0)), frame_timestamp_s=1.0, @@ -150,39 +130,19 @@ def close(self) -> None: runtime.close() assert command is not None - assert command.yaw == pytest_approx(30.0 / 90.0) - assert command.pitch == pytest_approx(0.0) + assert command.yaw_deg == pytest_approx(-20.0) + assert command.pitch_deg == pytest_approx(0.0) assert neutral_command is not None - assert neutral_command.yaw == pytest_approx(0.0) - assert device.moves == [(30.0 / 90.0, 0.0), (0.0, 0.0)] + assert neutral_command.yaw_deg == pytest_approx(0.0) + np.testing.assert_allclose(device.moves, [(-30.0, 0.0), (0.0, 0.0)], atol=1e-6) assert device.center_calls == 2 assert device.closed is True -def test_neck_runtime_releases_on_shutdown_when_enabled() -> None: - class FakeDevice: - def __init__(self) -> None: - self.released = False - self.closed = False - - def connect(self) -> None: - return None - - def center(self) -> None: - return None - - def release(self) -> None: - self.released = True - - def move_norm(self, yaw: float, pitch: float) -> None: - del yaw, pitch - - def close(self) -> None: - self.closed = True - +def test_neck_runtime_releases_torque_on_shutdown_when_enabled() -> None: device = FakeDevice() runtime = NeckRuntime( - NeckConfig(enabled=True, center_on_start=False, center_on_shutdown=False, release_on_shutdown=True), + NeckConfig(enabled=True, center_on_start=False, release_on_shutdown=True), device=device, ) @@ -193,27 +153,6 @@ def close(self) -> None: def test_neck_shutdown_defaults_to_close_only() -> None: - class FakeDevice: - def __init__(self) -> None: - self.center_calls = 0 - self.released = False - self.closed = False - - def connect(self) -> None: - return None - - def center(self) -> None: - self.center_calls += 1 - - def release(self) -> None: - self.released = True - - def move_norm(self, yaw: float, pitch: float) -> None: - del yaw, pitch - - def close(self) -> None: - self.closed = True - device = FakeDevice() runtime = NeckRuntime(NeckConfig(enabled=True, center_on_start=False), device=device) @@ -225,26 +164,11 @@ def close(self) -> None: def test_neck_runtime_closes_after_shutdown_center_failure() -> None: - class FakeDevice: - def __init__(self) -> None: - self.closed = False - - def connect(self) -> None: - return None - + class CenterFailingDevice(FakeDevice): def center(self) -> None: raise RuntimeError("neck center failed") - def release(self) -> None: - return None - - def move_norm(self, yaw: float, pitch: float) -> None: - del yaw, pitch - - def close(self) -> None: - self.closed = True - - device = FakeDevice() + device = CenterFailingDevice() runtime = NeckRuntime( NeckConfig(enabled=True, center_on_start=False, center_on_shutdown=True), device=device, @@ -261,66 +185,27 @@ def test_body_packet_frame_ignores_incomplete_packets() -> None: assert body_packet_frame(SimpleNamespace(frame=_frame(_quat_y(0.0)), timestamp_s="bad", seq=1)) == (None, None, -1) -def test_openneck_device_closes_context_manager(monkeypatch) -> None: +def test_openneck_device_uses_angle_api_and_returns_applied_target(monkeypatch) -> None: calls: list[str] = [] - class FakeEnteredController: - port = "/dev/entered" - - def center(self, *, wait_s: float) -> None: - calls.append(f"entered-center-{wait_s}") - - def move_norm(self, yaw: float, pitch: float) -> None: - calls.append(f"entered-move-{yaw}-{pitch}") - class FakeOpenNeckController: port = "/dev/fake" - def __init__(self, *, config: object, port: object, enable_torque_on_connect: bool) -> None: - del config, port, enable_torque_on_connect - self.entered = FakeEnteredController() - - def __enter__(self): - calls.append("enter") - return self.entered - - def __exit__(self, exc_type: object, exc: object, tb: object) -> None: - del exc_type, exc, tb - calls.append("exit") - - def close(self) -> None: - calls.append("close") - - module = ModuleType("openneck") - module.OpenNeckController = FakeOpenNeckController # type: ignore[attr-defined] - monkeypatch.setitem(__import__("sys").modules, "openneck", module) - - device = OpenNeckDevice(NeckConfig(enabled=True)) - device.connect() - device.center() - device.move_norm(0.25, -0.5) - device.close() - - assert calls == ["enter", "entered-center-0.5", "entered-move-0.25--0.5", "exit"] - - -def test_openneck_device_direct_close_after_context_exit_failure(monkeypatch) -> None: - calls: list[str] = [] + def __init__(self, *, config: object, port: object) -> None: + calls.append(f"init-{config}-{port}") - class FakeOpenNeckController: - port = "/dev/fake" + def connect(self) -> None: + calls.append("connect") - def __init__(self, *, config: object, port: object, enable_torque_on_connect: bool) -> None: - del config, port, enable_torque_on_connect + def center(self) -> None: + calls.append("center") - def __enter__(self): - calls.append("enter") - return self + def move_deg(self, yaw_deg: float, pitch_deg: float) -> SimpleNamespace: + calls.append(f"move-{yaw_deg}-{pitch_deg}") + return SimpleNamespace(yaw_deg=-20.0, pitch_deg=10.0) - def __exit__(self, exc_type: object, exc: object, tb: object) -> None: - del exc_type, exc, tb - calls.append("exit") - raise RuntimeError("context exit failed") + def release_torque(self) -> None: + calls.append("release-torque") def close(self) -> None: calls.append("close") @@ -329,102 +214,70 @@ def close(self) -> None: module.OpenNeckController = FakeOpenNeckController # type: ignore[attr-defined] monkeypatch.setitem(__import__("sys").modules, "openneck", module) - device = OpenNeckDevice(NeckConfig(enabled=True)) - device.connect() - try: - device.close() - except RuntimeError as exc: - assert "context exit failed" in str(exc) - else: - raise AssertionError("expected RuntimeError") - - assert calls == ["enter", "exit", "close"] - - -def test_openneck_device_direct_closes_context_when_entered_proxy_lacks_close(monkeypatch) -> None: - calls: list[str] = [] - - class FakeEnteredController: - port = "/dev/entered" - - class FakeOpenNeckController: - port = "/dev/fake" - - def __init__(self, *, config: object, port: object, enable_torque_on_connect: bool) -> None: - del config, port, enable_torque_on_connect - self.entered = FakeEnteredController() - - def __enter__(self): - calls.append("enter") - return self.entered - - def __exit__(self, exc_type: object, exc: object, tb: object) -> None: - del exc_type, exc, tb - calls.append("exit") - raise RuntimeError("context exit failed") - - def close(self) -> None: - calls.append("context-close") - - module = ModuleType("openneck") - module.OpenNeckController = FakeOpenNeckController # type: ignore[attr-defined] - monkeypatch.setitem(__import__("sys").modules, "openneck", module) - - device = OpenNeckDevice(NeckConfig(enabled=True)) + device = OpenNeckDevice( + NeckConfig(enabled=True, config_path="neck.json", port="/dev/ttyACM0") + ) device.connect() - try: - device.close() - except RuntimeError as exc: - assert "context exit failed" in str(exc) - else: - raise AssertionError("expected RuntimeError") + device.center() + applied = device.move_deg(-25.0, 15.0) + device.release_torque() + device.close() - assert calls == ["enter", "exit", "context-close"] + assert applied == (-20.0, 10.0) + assert calls == [ + "init-neck.json-/dev/ttyACM0", + "connect", + "center", + "move--25.0-15.0", + "release-torque", + "close", + ] -def test_openneck_device_attempts_all_direct_close_targets(monkeypatch) -> None: +def test_dry_run_neck_device_reuses_openneck_calibration_clamp(monkeypatch) -> None: calls: list[str] = [] - class FakeEnteredController: - port = "/dev/entered" - - def close(self) -> None: - calls.append("entered-close") - raise RuntimeError("entered close failed") - class FakeOpenNeckController: - port = "/dev/fake" + def __init__(self, *, config: object, port: object) -> None: + calls.append(f"init-{config}-{port}") - def __init__(self, *, config: object, port: object, enable_torque_on_connect: bool) -> None: - del config, port, enable_torque_on_connect - self.entered = FakeEnteredController() + def move_deg(self, yaw_deg: float, pitch_deg: float) -> None: + del yaw_deg, pitch_deg + raise AssertionError("dry-run must not send a hardware command") - def __enter__(self): - calls.append("enter") - return self.entered + def _angle_to_step(self, axis: str, angle_deg: float) -> int: + calls.append(f"angle-to-step-{axis}-{angle_deg}") + low, high = (-20.0, 20.0) if axis == "yaw" else (-10.0, 10.0) + return round(max(low, min(high, angle_deg))) - def __exit__(self, exc_type: object, exc: object, tb: object) -> None: - del exc_type, exc, tb - calls.append("exit") - raise RuntimeError("context exit failed") - - def close(self) -> None: - calls.append("context-close") + def _step_to_angle(self, axis: str, step: int) -> float: + calls.append(f"step-to-angle-{axis}-{step}") + return float(step) module = ModuleType("openneck") module.OpenNeckController = FakeOpenNeckController # type: ignore[attr-defined] monkeypatch.setitem(__import__("sys").modules, "openneck", module) - device = OpenNeckDevice(NeckConfig(enabled=True)) + device = DryRunNeckDevice( + NeckConfig( + enabled=True, + config_path="neck.json", + port="/dev/ttyACM0", + dry_run=True, + ) + ) device.connect() - try: - device.close() - except RuntimeError as exc: - assert "context exit failed" in str(exc) - else: - raise AssertionError("expected RuntimeError") + applied = device.move_deg(25.0, -15.0) + device.close() - assert calls == ["enter", "exit", "entered-close", "context-close"] + assert applied == (20.0, -10.0) + assert calls == [ + "init-neck.json-/dev/ttyACM0", + "angle-to-step-yaw-25.0", + "angle-to-step-pitch--15.0", + "step-to-angle-yaw-20", + "step-to-angle-pitch--10", + ] def test_parse_neck_config_validates_rate() -> None: @@ -452,6 +305,16 @@ def test_parse_neck_config_rejects_unknown_active_mode() -> None: raise AssertionError("expected ValueError") +def test_parse_neck_config_rejects_removed_normalized_fields() -> None: + try: + parse_neck_config({"neck": {"enabled": True, "yaw_range_deg": 90.0}}) + except ValueError as exc: + assert "Removed normalized OpenNeck config" in str(exc) + assert "angles in degrees" in str(exc) + else: + raise AssertionError("expected ValueError") + + def pytest_approx(value: float): import pytest diff --git a/tests/test_sim2real_multiprocess.py b/tests/test_sim2real_multiprocess.py index 69a95735..10fdb717 100644 --- a/tests/test_sim2real_multiprocess.py +++ b/tests/test_sim2real_multiprocess.py @@ -501,9 +501,9 @@ def test_hdf5_recording_schema() -> None: assert features[ACTION_KEY]["groups"]["reference_joint_pos"] == [7, 36] assert features[HAND_ACTION_KEY]["groups"]["left_hand_target"] == [0, 6] assert features[HAND_ACTION_KEY]["groups"]["right_hand_target"] == [6, 12] - assert features[NECK_ACTION_KEY]["names"] == ["yaw", "pitch"] - assert features[NECK_ACTION_KEY]["units"] == "normalized" - assert features[NECK_ACTION_KEY]["range"] == [-1.0, 1.0] + assert features[NECK_ACTION_KEY]["names"] == ["yaw_deg", "pitch_deg"] + assert features[NECK_ACTION_KEY]["units"] == "degrees" + assert "range" not in features[NECK_ACTION_KEY] assert len(features[STATE_KEY]["names"]) == 68 assert len(features[ACTION_KEY]["names"]) == 36 assert len(features[HAND_ACTION_KEY]["names"]) == 12 @@ -560,7 +560,7 @@ def test_hdf5_recorder_mp4_sidecar_writes_sync_metadata(tmp_path: Path) -> None: mode=build_mode_observation("mocap"), action=np.arange(36, dtype=np.float32), hand_action=np.arange(12, dtype=np.float32), - neck_action=np.array([0.25, -0.5], dtype=np.float32), + neck_action=np.array([12.5, -8.0], dtype=np.float32), ) recorder.save_episode() recorder.finalize() @@ -600,7 +600,7 @@ def test_hdf5_recorder_mp4_sidecar_writes_sync_metadata(tmp_path: Path) -> None: assert h5[NECK_ACTION_KEY].shape == (2, 2) np.testing.assert_allclose( h5[NECK_ACTION_KEY][...], - np.array([[0.25, -0.5], [0.25, -0.5]], dtype=np.float32), + np.array([[12.5, -8.0], [12.5, -8.0]], dtype=np.float32), ) @@ -1433,8 +1433,8 @@ def fake_factory(**_kwargs: object) -> FakeRecorder: timestamp_s=2.06, driver="openneck", active=True, - yaw=0.25, - pitch=-0.5, + yaw_deg=12.5, + pitch_deg=-8.0, seq=1, ) desc = writer.write(np.full((2, 2, 3), 5, dtype=np.uint8), timestamp_s=2.1) @@ -1447,7 +1447,7 @@ def fake_factory(**_kwargs: object) -> FakeRecorder: assert int(frames[0]["mode"]) == int(build_mode_observation("standing")) np.testing.assert_allclose(frames[0]["action"], np.arange(36, dtype=np.float32)) np.testing.assert_allclose(frames[0]["hand_action"], np.arange(12, dtype=np.float32)) - np.testing.assert_allclose(frames[0]["neck_action"], np.array([0.25, -0.5], dtype=np.float32)) + np.testing.assert_allclose(frames[0]["neck_action"], np.array([12.5, -8.0], dtype=np.float32)) worker._latest_record = RecordStepPacket( timestamp_s=3.0, From 71c6a6e8f27948f6f03c6214e11b9f0ea470e2f3 Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Wed, 15 Jul 2026 22:43:01 +0800 Subject: [PATCH 19/59] Drive OpenNeck from Pico HMD pose --- AGENTS.md | 5 +- README.md | 17 ++- docs/docs/configuration/config-reference.md | 26 ++-- .../current/configuration/config-reference.md | 18 +-- scripts/dev/test_openneck.py | 53 ++++---- teleopit/inputs/pico4_provider.py | 72 ++++++++++ teleopit/sim2real/mp/ipc.py | 3 + teleopit/sim2real/mp/runtime.py | 96 ++++++++++--- teleopit/sim2real/neck/__init__.py | 4 +- teleopit/sim2real/neck/mapper.py | 35 ++--- teleopit/sim2real/neck/worker.py | 63 ++++++--- tests/test_active_neck.py | 127 +++++++++++------- tests/test_pico4_provider.py | 97 +++++++++++++ tests/test_sim2real_multiprocess.py | 109 +++++++++++++++ 14 files changed, 574 insertions(+), 151 deletions(-) diff --git a/AGENTS.md b/AGENTS.md index 0717d31f..8e9bc589 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -162,10 +162,11 @@ target_dof_pos = clip(action, -10, 10) × action_scale + default_dof_pos - `vr_hand_pose` defaults to a low-latency somehand path: `hands.somehand.rate_hz=60`, `max_iterations=12`, `temporal_filter_alpha=1.0`, and `output_alpha=1.0`; this prioritizes response speed over smoothing - LinkerHand control is active in all sim2real modes when `hands.enabled=true`; shutdown and hand-runtime failure must send the configured open pose - In `vr_hand_pose` mode, missing/inactive hand pose holds the last commanded pose for that side instead of opening the hand -- Optional OpenNeck active-vision gimbal control uses `neck.enabled=true` and `neck.driver=openneck`; it requires `input.provider=pico4`, reuses the existing Pico body frame stream, and must not start a second `PicoBridge` +- Optional OpenNeck active-vision gimbal control uses `neck.enabled=true` and `neck.driver=openneck`; it requires `input.provider=pico4`, reuses the existing Pico receiver, and must not start a second `PicoBridge` - OpenNeck is integrated as a non-critical sim2real `neck_worker`; failures should not stop the G1 control loop, and no OpenNeck state is added to the 167D policy observation - OpenNeck 0.2.0 is the supported runtime; Teleopit sends physical degrees through `move_deg()`, and the direct-drive OpenNeck package converts degrees to servo steps and clips them to its calibrated mechanical limits; the removed normalized API and config fields are unsupported -- OpenNeck maps the absolute Pico `Head` orientation relative to `Spine3` with a fixed identity neutral pose and no neck-side EMA; it must not capture the first live frame as a runtime zero pose, so tracking can start while the operator's head is turned; positive yaw turns left and positive pitch looks up +- OpenNeck maps the independent HMD `PicoFrame.head.rotation` relative to the same-frame full-body `Body.Spine3` orientation; it must never use the full-body `Body.Head` skeleton joint for neck control, and HMD pose updates must remain independent of duplicate-body-frame filtering +- OpenNeck uses a fixed identity neutral pose and no neck-side EMA; it must not capture the first live frame as a runtime zero pose, so tracking can start while the operator's head is turned; positive yaw turns left and positive pitch looks up ### SimulationLoop Runtime Behavior - `realtime=true` enforces wall-clock pacing even without a viewer diff --git a/README.md b/README.md index 96eeb5e6..2012406a 100644 --- a/README.md +++ b/README.md @@ -116,7 +116,7 @@ manifest entry is committed are discarded on the next recording startup. ## OpenNeck Active Vision Pico sim2real can drive the optional OpenNeck two-axis active-vision gimbal from -the same Pico body tracking stream used for whole-body control: +the same Pico receiver used for whole-body control: ```bash pip install -e '.[openneck]' @@ -128,12 +128,15 @@ python scripts/run/run_sim2real.py --config-name pico4_sim2real \ `neck.enabled=true` requires `input.provider=pico4`. The neck worker reuses the existing Teleopit Pico receiver and does not start another `PicoBridge` or -camera pipeline. It maps the absolute `Head` orientation relative to `Spine3` -with a fixed neutral pose and no neck-side EMA, so tracking startup does not -require the operator to face straight ahead. Teleopit sends physical yaw/pitch -angles through the OpenNeck 0.2.0 `move_deg()` API; OpenNeck converts those -angles for its direct-drive servos and clips them to the calibrated mechanical -step limits. Positive yaw turns left and positive pitch looks up. +camera pipeline. The neck path reads the independent HMD +`PicoFrame.head.rotation` and maps it relative to `Body.Spine3` from the same +source frame. It never uses the full-body tracker's `Body.Head` skeleton joint, +whose model constraints can under-report extreme head pitch. The mapper uses a +fixed neutral pose and no neck-side EMA, so tracking startup does not require +the operator to face straight ahead. Teleopit sends physical yaw/pitch angles +through the OpenNeck 0.2.0 `move_deg()` API; OpenNeck converts those angles for +its direct-drive servos and clips them to the calibrated mechanical step +limits. Positive yaw turns left and positive pitch looks up. OpenNeck 0.2.0 uses an angle-based calibration file and rejects the previous normalized configuration fields. Re-run `openneck calibrate` before enabling diff --git a/docs/docs/configuration/config-reference.md b/docs/docs/configuration/config-reference.md index 3e8f8258..b0fece3b 100644 --- a/docs/docs/configuration/config-reference.md +++ b/docs/docs/configuration/config-reference.md @@ -158,17 +158,21 @@ through `somehand.api` only. ### OpenNeck Active Vision (Pico sim2real) `neck.enabled=true` requires `input.provider=pico4` and the `openneck` extra. The -neck worker reuses Teleopit's existing Pico body-frame stream and does not start +neck worker reuses Teleopit's existing Pico receiver and does not start a second `PicoBridge` or RealSense pipeline. OpenNeck runs as a non-critical -sim2real worker and does not change the policy observation. Head motion is -mapped as the absolute `Head` orientation relative to `Spine3`, using the fixed -PICO neutral orientation and no neck-side EMA; startup does not capture the -operator's first pose as a new zero pose, so the operator does not need to face -straight when tracking starts. Teleopit converts the supported PICO convention -to OpenNeck's physical convention—positive yaw turns left and positive pitch -looks up—and sends the relative angles in degrees through OpenNeck 0.2.0 -`move_deg()`. OpenNeck performs the direct-drive degree-to-step conversion and -clips each target to the mechanical step limits in its calibration file. +sim2real worker and does not change the policy observation. Head motion comes +from the independent HMD `PicoFrame.head.rotation`, mapped relative to +`Body.Spine3` from the same source frame. The neck path never reads the +full-body tracker's `Body.Head` skeleton joint, whose model constraints can +under-report extreme head pitch. HMD updates remain independent of duplicate +body-frame filtering. The mapper uses the fixed PICO neutral orientation and +no neck-side EMA; startup does not capture the operator's first pose as a new +zero pose, so the operator does not need to face straight when tracking starts. +Teleopit converts the supported PICO convention to OpenNeck's physical +convention—positive yaw turns left and positive pitch looks up—and sends the +relative angles in degrees through OpenNeck 0.2.0 `move_deg()`. OpenNeck +performs the direct-drive degree-to-step conversion and clips each target to +the mechanical step limits in its calibration file. OpenNeck 0.2.0 calibration files use angle-control fields such as `yaw_center_step`, `yaw_min_step`, `yaw_max_step`, and `yaw_step_sign` (and the @@ -184,7 +188,7 @@ are rejected rather than ignored. | `neck.config_path` | Optional OpenNeck 0.2.0 angle-calibration config path | `null` | | `neck.port` | Optional serial port override, for example `/dev/ttyACM0` | `null` | | `neck.rate_hz` | Maximum neck command rate in Hz | `60.0` | -| `neck.frame_timeout_s` | Pico body-frame staleness threshold | `0.2` | +| `neck.frame_timeout_s` | Pico HMD/Spine3 pose staleness threshold | `0.2` | | `neck.active_modes` | Sim2real modes that allow neck motion | `[standing, mocap, arms, pause]` | | `neck.dead_zone_deg` | Yaw/pitch dead zone in degrees | `0.5` | | `neck.center_on_start` / `center_on_shutdown` | Center the gimbal at worker startup/shutdown | `true` / `false` | diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md index 433a8d8c..824fbc3a 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md @@ -176,13 +176,15 @@ MuJoCo 窗口显示重定向参考；`sim2sim`、`mocap`、`camera` 和 `all` ### OpenNeck 主动视觉（Pico sim2real） `neck.enabled=true` 要求 `input.provider=pico4` 和 `openneck` extra。neck worker -复用 Teleopit 已有的 Pico body frame 数据流，不会启动第二个 `PicoBridge` 或 RealSense -管线。OpenNeck 作为非关键 sim2real worker 运行，不会改变策略观测。头部运动使用固定的 -PICO 中立姿态且不进行颈部侧 EMA，按照 `Head` 相对于 `Spine3` 的绝对朝向进行映射； -启动时不会把操作者的第一帧姿态采集为新的零位，因此开始追踪时操作者不需要保持头部朝正前方。 -Teleopit 将受支持的 PICO 约定转换为 OpenNeck 的物理约定——正 yaw 向左转，正 pitch -向上看——并通过 OpenNeck 0.2.0 的 `move_deg()` 发送以度为单位的相对角度。OpenNeck -负责直驱角度到舵机步数的转换，并将每个目标裁剪到标定文件中的机械步数限位。 +复用 Teleopit 已有的 Pico receiver，不会启动第二个 `PicoBridge` 或 RealSense 管线。 +OpenNeck 作为非关键 sim2real worker 运行，不会改变策略观测。头部运动来自独立的头显 +`PicoFrame.head.rotation`，并相对于同一个源帧中的 `Body.Spine3` 进行映射。neck 路径 +绝不读取全身动捕的 `Body.Head` 骨架关节；人体模型约束可能使该关节低估极端低头角度。 +头显姿态更新不受 body 重复帧过滤影响。mapper 使用固定的 PICO 中立姿态且不进行颈部侧 +EMA；启动时不会把操作者的第一帧姿态采集为新的零位，因此开始追踪时操作者不需要保持头部 +朝正前方。Teleopit 将受支持的 PICO 约定转换为 OpenNeck 的物理约定——正 yaw 向左转， +正 pitch 向上看——并通过 OpenNeck 0.2.0 的 `move_deg()` 发送以度为单位的相对角度。 +OpenNeck 负责直驱角度到舵机步数的转换，并将每个目标裁剪到标定文件中的机械步数限位。 OpenNeck 0.2.0 标定文件使用 `yaw_center_step`、`yaw_min_step`、 `yaw_max_step` 和 `yaw_step_sign` 等角度控制字段（pitch 使用对应字段）。不支持以前的 @@ -197,7 +199,7 @@ OpenNeck 归一化配置；运行 `openneck calibrate` 创建当前格式的文 | `neck.config_path` | 可选 OpenNeck 0.2.0 角度标定配置路径 | `null` | | `neck.port` | 可选串口覆盖，例如 `/dev/ttyACM0` | `null` | | `neck.rate_hz` | 最大头颈命令频率（Hz） | `60.0` | -| `neck.frame_timeout_s` | Pico body frame 过期阈值 | `0.2` | +| `neck.frame_timeout_s` | Pico 头显/Spine3 姿态过期阈值 | `0.2` | | `neck.active_modes` | 允许头颈运动的 sim2real 模式 | `[standing, mocap, arms, pause]` | | `neck.dead_zone_deg` | yaw/pitch 死区（度） | `0.5` | | `neck.center_on_start` / `center_on_shutdown` | worker 启动/关闭时回中云台 | `true` / `false` | diff --git a/scripts/dev/test_openneck.py b/scripts/dev/test_openneck.py index 20ab35dc..25874168 100644 --- a/scripts/dev/test_openneck.py +++ b/scripts/dev/test_openneck.py @@ -34,7 +34,7 @@ def parse_args() -> argparse.Namespace: default="direct", help=( "direct sends a conservative fixed motion pattern to OpenNeck; " - "pico drives OpenNeck from live Pico head/body tracking through Teleopit's active-neck mapper." + "pico drives OpenNeck from live Pico HMD rotation relative to Spine3." ), ) parser.add_argument("--port", default=None, help="Optional OpenNeck serial port, for example /dev/ttyACM0") @@ -154,34 +154,41 @@ def run_pico(args: argparse.Namespace) -> None: command_count = 0 print( - "Testing OpenNeck active vision from live Pico body tracking. " - "OpenNeck follows the current head pose relative to the torso; press Ctrl-C to stop.", + "Testing OpenNeck active vision from the live Pico HMD rotation relative to Spine3; " + "press Ctrl-C to stop.", flush=True, ) try: runtime.start() while deadline is None or time.monotonic() < deadline: now_s = time.monotonic() - if provider.has_frame(): - frame, timestamp_s, seq = provider.get_frame_packet() - if int(seq) != last_seq: - command = runtime.tick( - frame=frame, - frame_timestamp_s=timestamp_s, - active=True, - now_s=now_s, - ) - moved = command is not None - if moved: - command_count += 1 - last_seq = int(seq) - age_ms = max((now_s - float(timestamp_s)) * 1000.0, 0.0) - print( - f"pico seq={seq} age={age_ms:.1f}ms moved={moved} commands={command_count}", - flush=True, - ) - else: - runtime.tick(frame=None, frame_timestamp_s=None, active=True, now_s=now_s) + snapshot = provider.get_head_pose_snapshot() + if snapshot is not None and int(snapshot.seq) != last_seq: + command = runtime.tick( + hmd_rotation_wxyz=snapshot.hmd_rotation_wxyz, + spine3_rotation_wxyz=snapshot.spine3_rotation_wxyz, + pose_timestamp_s=snapshot.timestamp_s, + active=True, + now_s=now_s, + ) + moved = command is not None + if moved: + command_count += 1 + last_seq = int(snapshot.seq) + age_ms = max((now_s - float(snapshot.timestamp_s)) * 1000.0, 0.0) + print( + f"pico seq={snapshot.seq} age={age_ms:.1f}ms " + f"moved={moved} commands={command_count}", + flush=True, + ) + elif snapshot is None: + runtime.tick( + hmd_rotation_wxyz=None, + spine3_rotation_wxyz=None, + pose_timestamp_s=None, + active=True, + now_s=now_s, + ) time.sleep(sleep_s) except KeyboardInterrupt: print("Interrupted; shutting down OpenNeck", flush=True) diff --git a/teleopit/inputs/pico4_provider.py b/teleopit/inputs/pico4_provider.py index 1780b03f..52b184f5 100644 --- a/teleopit/inputs/pico4_provider.py +++ b/teleopit/inputs/pico4_provider.py @@ -94,6 +94,16 @@ class PicoHandSnapshot: seq: int +@dataclass(frozen=True) +class PicoHeadPoseSnapshot: + """Synchronized HMD and torso orientations for active-neck control.""" + + hmd_rotation_wxyz: NDArray[np.float64] | None + spine3_rotation_wxyz: NDArray[np.float64] | None + timestamp_s: float + seq: int + + _PAUSE_BUTTON_MAP: dict[str, tuple[str, str]] = { "A": ("right", "primaryButton"), "B": ("right", "secondaryButton"), @@ -173,6 +183,32 @@ def _coordinate_transform_input(body_pose_dict: dict[str, list]) -> dict[str, li return body_pose_dict +def _transform_pico_native_rotation(rotation_xyzw: Any) -> NDArray[np.float64] | None: + """Convert one PICO-native xyzw orientation into Teleopit wxyz coordinates.""" + try: + rotation = np.asarray(rotation_xyzw, dtype=np.float64).reshape(-1) + except (TypeError, ValueError): + return None + if rotation.shape != (4,) or not np.all(np.isfinite(rotation)): + return None + quat_wxyz = np.array( + [rotation[3], rotation[0], rotation[1], rotation[2]], + dtype=np.float64, + ) + norm = float(np.linalg.norm(quat_wxyz)) + if norm <= 1e-9: + return None + transformed = quat_mul_np( + _INPUT_TO_TELEOPIT_QUAT, + quat_wxyz / norm, + scalar_first=True, + ) + transformed_norm = float(np.linalg.norm(transformed)) + if transformed_norm <= 1e-9 or not np.all(np.isfinite(transformed)): + return None + return np.asarray(transformed / transformed_norm, dtype=np.float64) + + class Pico4InputProvider(RealtimeInputProvider): """Realtime input provider backed by the ``pico_bridge`` receiver.""" @@ -240,6 +276,7 @@ def __init__( self._last_source_seq: int | None = None self._controller_snapshot: PicoControllerSnapshot | None = None self._hand_snapshot: PicoHandSnapshot | None = None + self._head_pose_snapshot: PicoHeadPoseSnapshot | None = None self._ground_alignment_offset: float | None = None self._bridge = bridge_cls( host=bridge_host, @@ -336,6 +373,11 @@ def get_hand_snapshot(self) -> PicoHandSnapshot | None: with self._lock: return self._hand_snapshot + def get_head_pose_snapshot(self) -> PicoHeadPoseSnapshot | None: + """Return the latest synchronized HMD/Spine3 orientation snapshot.""" + with self._lock: + return self._head_pose_snapshot + def push_video_frame(self, frame: NDArray[np.uint8]) -> int: """Push one RGB camera frame to pico-bridge 0.2.1 video output.""" push_video_frame = getattr(self._bridge, "push_video_frame", None) @@ -400,6 +442,7 @@ def _poll_loop(self) -> None: def _accept_pico_frame(self, frame: Any) -> bool: timestamp = float(getattr(frame, "receive_time_s", time.monotonic())) + self._accept_head_pose_snapshot(frame, timestamp=timestamp) self._accept_controller_snapshot(frame, timestamp=timestamp) self._accept_hand_snapshot(frame, timestamp=timestamp) self._poll_control_events(frame, timestamp=timestamp) @@ -467,6 +510,35 @@ def _accept_hand_snapshot(self, frame: Any, *, timestamp: float) -> None: with self._lock: self._hand_snapshot = snapshot + def _accept_head_pose_snapshot(self, frame: Any, *, timestamp: float) -> None: + """Capture HMD and Spine3 rotations from the same pico_bridge frame.""" + seq = int(getattr(frame, "seq", self._last_source_seq or -1)) + + head = getattr(frame, "head", None) + hmd_rotation = _transform_pico_native_rotation( + None if head is None else getattr(head, "rotation", None) + ) + + spine3_rotation: NDArray[np.float64] | None = None + body = getattr(frame, "body", None) + if body is not None and bool(getattr(body, "active", False)): + try: + body_joints = np.asarray(getattr(body, "joints"), dtype=np.float64) + except (AttributeError, TypeError, ValueError): + body_joints = np.empty((0, 0), dtype=np.float64) + if body_joints.shape == (len(BODY_JOINT_NAMES), 7): + spine3 = body_joints[BODY_JOINT_NAMES.index("Spine3")] + spine3_rotation = _transform_pico_native_rotation(spine3[[3, 4, 5, 6]]) + + snapshot = PicoHeadPoseSnapshot( + hmd_rotation_wxyz=hmd_rotation, + spine3_rotation_wxyz=spine3_rotation, + timestamp_s=float(timestamp), + seq=seq, + ) + with self._lock: + self._head_pose_snapshot = snapshot + def _poll_control_events(self, frame: Any, *, timestamp: float) -> bool: emitted = False emitted = self._poll_button_control_event( diff --git a/teleopit/sim2real/mp/ipc.py b/teleopit/sim2real/mp/ipc.py index e24487e0..21d81213 100644 --- a/teleopit/sim2real/mp/ipc.py +++ b/teleopit/sim2real/mp/ipc.py @@ -11,6 +11,7 @@ BODY_TOPIC = "body" +HEAD_POSE_TOPIC = "head_pose" HAND_TOPIC = "hand" HAND_COMMAND_TOPIC = "hand_command" NECK_COMMAND_TOPIC = "neck_command" @@ -27,6 +28,7 @@ @dataclass(frozen=True) class Sim2RealIpcEndpoints: body_pub: str + head_pose_pub: str hand_pub: str hand_command_pub: str neck_command_pub: str @@ -46,6 +48,7 @@ def default_endpoints(*, host: str = "127.0.0.1", base_port: int = 39700) -> Sim prefix = f"tcp://{host}:" return Sim2RealIpcEndpoints( body_pub=f"{prefix}{base_port}", + head_pose_pub=f"{prefix}{base_port + 13}", hand_pub=f"{prefix}{base_port + 1}", hand_command_pub=f"{prefix}{base_port + 2}", neck_command_pub=f"{prefix}{base_port + 12}", diff --git a/teleopit/sim2real/mp/runtime.py b/teleopit/sim2real/mp/runtime.py index 44b42f6b..a7c43653 100644 --- a/teleopit/sim2real/mp/runtime.py +++ b/teleopit/sim2real/mp/runtime.py @@ -68,13 +68,14 @@ from teleopit.sim2real.hands.linkerhand_l6 import parse_linkerhand_l6_config from teleopit.sim2real.hands.linkerhand_o6 import parse_linkerhand_o6_config from teleopit.sim2real.neck.config import parse_neck_config -from teleopit.sim2real.neck.worker import body_packet_frame, build_neck_runtime, mode_packet_active +from teleopit.sim2real.neck.worker import build_neck_runtime, head_pose_packet, mode_packet_active from teleopit.sim2real.mp.ipc import ( BODY_TOPIC, COMMAND_TOPIC, CONTROL_EVENTS_TOPIC, CONTROLLER_TOPIC, HAND_COMMAND_TOPIC, + HEAD_POSE_TOPIC, HAND_TOPIC, HEALTH_TOPIC, MODE_TOPIC, @@ -614,6 +615,28 @@ def _main() -> None: ) body_pub = ZmqPublisher(endpoints.body_pub) + head_pose_pub: ZmqPublisher | None = None + + def _disable_head_pose_publisher(message: str) -> None: + nonlocal head_pose_pub + logger.exception(message) + failed_publisher = head_pose_pub + head_pose_pub = None + if failed_publisher is None: + return + try: + failed_publisher.close() + except Exception: + logger.exception("Failed to close disabled OpenNeck head-pose publisher") + + if parse_neck_config(cfg).enabled: + try: + head_pose_pub = ZmqPublisher(endpoints.head_pose_pub) + except Exception: + _disable_head_pose_publisher( + "OpenNeck head-pose IPC setup failed; neck control is disabled " + "while pico_input continues" + ) hand_pub = ZmqPublisher(endpoints.hand_pub) controller_pub = ZmqPublisher(endpoints.controller_pub) events_pub = ZmqPublisher(endpoints.control_events_pub) @@ -645,6 +668,7 @@ def _publish_recording_frame(frame: NDArray[np.generic], timestamp_s: float) -> hz = float(cfg_get(_mp_cfg(cfg), "pico_input_hz", 120.0)) sleep_s = 1.0 / max(hz, 1.0) last_body_seq = -1 + last_head_pose_seq = -1 last_hand_seq = -1 last_controller_seq = -1 last_video_seq = -1 @@ -659,6 +683,28 @@ def _publish_recording_frame(frame: NDArray[np.generic], timestamp_s: float) -> break now = time.monotonic() + if head_pose_pub is not None: + try: + head_pose_snapshot = provider.get_head_pose_snapshot() + if ( + head_pose_snapshot is not None + and int(head_pose_snapshot.seq) != last_head_pose_seq + ): + head_pose_pub.publish( + HEAD_POSE_TOPIC, + SnapshotPacket( + snapshot=head_pose_snapshot, + timestamp_s=float(head_pose_snapshot.timestamp_s), + seq=int(head_pose_snapshot.seq), + ), + ) + last_head_pose_seq = int(head_pose_snapshot.seq) + except Exception: + _disable_head_pose_publisher( + "OpenNeck head-pose stream failed; neck control is disabled " + "while pico_input continues" + ) + if callable(getattr(provider, "has_frame", None)) and provider.has_frame(): try: frame, timestamp_s, seq = provider.get_frame_packet() @@ -715,6 +761,7 @@ def _publish_recording_frame(frame: NDArray[np.generic], timestamp_s: float) -> metrics={ "body_seq": last_body_seq, "body_fps": float(provider.fps), + "head_pose_seq": last_head_pose_seq, "hand_seq": last_hand_seq, "controller_seq": last_controller_seq, "video_seq": last_video_seq, @@ -728,7 +775,19 @@ def _publish_recording_frame(frame: NDArray[np.generic], timestamp_s: float) -> if frame_writer is not None: frame_writer.close(unlink=True) command_sub.close() - for publisher in (body_pub, hand_pub, controller_pub, events_pub, health_pub, video_pub): + if head_pose_pub is not None: + try: + head_pose_pub.close() + except Exception: + logger.exception("Failed to close OpenNeck head-pose publisher") + for publisher in ( + body_pub, + hand_pub, + controller_pub, + events_pub, + health_pub, + video_pub, + ): if publisher is not None: publisher.close() provider.close() @@ -2086,7 +2145,7 @@ def _run_neck_worker( def _main() -> None: neck_cfg = parse_neck_config(cfg) runtime = build_neck_runtime(neck_cfg) - body_sub = LatestSubscriber(endpoints.body_pub, BODY_TOPIC) + head_pose_sub = LatestSubscriber(endpoints.head_pose_pub, HEAD_POSE_TOPIC) mode_sub = LatestSubscriber(endpoints.mode_pub, MODE_TOPIC) command_sub = LatestSubscriber(endpoints.command_pub, COMMAND_TOPIC) neck_command_pub = ( @@ -2094,9 +2153,10 @@ def _main() -> None: if _recording_enabled(cfg) else None ) - latest_frame: Any | None = None - latest_frame_timestamp_s: float | None = None - latest_body_seq = -1 + latest_hmd_rotation: Float64Array | None = None + latest_spine3_rotation: Float64Array | None = None + latest_pose_timestamp_s: float | None = None + latest_pose_seq = -1 latest_mode: ModeStatePacket | None = None command_count = 0 command_seq = 0 @@ -2140,12 +2200,13 @@ def _publish_neck_command( if isinstance(runtime_command, CommandPacket) and runtime_command.command == "shutdown": stop_event.set() break - body_packet = body_sub.recv_latest() - frame, frame_timestamp_s, body_seq = body_packet_frame(body_packet) - if frame is not None: - latest_frame = frame - latest_frame_timestamp_s = frame_timestamp_s - latest_body_seq = body_seq + pose_packet = head_pose_sub.recv_latest() + hmd_rotation, spine3_rotation, pose_timestamp_s, pose_seq = head_pose_packet(pose_packet) + if pose_seq >= 0: + latest_hmd_rotation = hmd_rotation + latest_spine3_rotation = spine3_rotation + latest_pose_timestamp_s = pose_timestamp_s + latest_pose_seq = pose_seq mode_packet = mode_sub.recv_latest() if isinstance(mode_packet, ModeStatePacket): latest_mode = mode_packet @@ -2153,8 +2214,9 @@ def _publish_neck_command( active = mode_packet_active(latest_mode, neck_cfg) try: neck_command = runtime.tick( - frame=latest_frame, - frame_timestamp_s=latest_frame_timestamp_s, + hmd_rotation_wxyz=latest_hmd_rotation, + spine3_rotation_wxyz=latest_spine3_rotation, + pose_timestamp_s=latest_pose_timestamp_s, active=active, now_s=now_s, ) @@ -2170,8 +2232,8 @@ def _publish_neck_command( logger.exception("OpenNeck worker tick failed; neck control continues") if now_s - last_status_s >= 5.0: logger.debug( - "OpenNeck worker status | body_seq=%s commands=%s active=%s", - latest_body_seq, + "OpenNeck worker status | head_pose_seq=%s commands=%s active=%s", + latest_pose_seq, command_count, active, ) @@ -2181,7 +2243,7 @@ def _publish_neck_command( try: runtime.close() finally: - body_sub.close() + head_pose_sub.close() mode_sub.close() command_sub.close() if neck_command_pub is not None: diff --git a/teleopit/sim2real/neck/__init__.py b/teleopit/sim2real/neck/__init__.py index 32f0f3dd..29c09f94 100644 --- a/teleopit/sim2real/neck/__init__.py +++ b/teleopit/sim2real/neck/__init__.py @@ -1,11 +1,11 @@ """Optional active-neck runtimes for sim2real.""" from teleopit.sim2real.neck.config import NeckConfig, parse_neck_config -from teleopit.sim2real.neck.mapper import HeadPoseMapper +from teleopit.sim2real.neck.mapper import HmdPoseMapper from teleopit.sim2real.neck.worker import build_neck_runtime __all__ = [ - "HeadPoseMapper", + "HmdPoseMapper", "NeckConfig", "build_neck_runtime", "parse_neck_config", diff --git a/teleopit/sim2real/neck/mapper.py b/teleopit/sim2real/neck/mapper.py index f8486c5d..1c284127 100644 --- a/teleopit/sim2real/neck/mapper.py +++ b/teleopit/sim2real/neck/mapper.py @@ -6,13 +6,10 @@ import numpy as np from numpy.typing import NDArray -from teleopit.inputs.realtime_packet import HumanFrame from teleopit.sim2real.neck.config import NeckConfig FloatArray = NDArray[np.float64] -_PICO_HEAD_JOINT = "Head" -_PICO_BODY_REFERENCE_JOINT = "Spine3" @dataclass(frozen=True) @@ -22,22 +19,29 @@ class NeckCommand: roll_deg: float -class HeadPoseMapper: - """Map Teleopit Pico body frames to OpenNeck yaw/pitch angles.""" +class HmdPoseMapper: + """Map synchronized Pico HMD/Spine3 orientations to OpenNeck angles.""" def __init__(self, config: NeckConfig) -> None: self._cfg = config - def map_frame(self, frame: HumanFrame) -> NeckCommand | None: - q_head = _joint_quat(frame, _PICO_HEAD_JOINT) - if q_head is None: + def map_pose( + self, + *, + hmd_rotation_wxyz: FloatArray | None, + spine3_rotation_wxyz: FloatArray | None, + ) -> NeckCommand | None: + q_hmd = _normalized_quat(hmd_rotation_wxyz) + if q_hmd is None: return None - q_body = _joint_quat(frame, _PICO_BODY_REFERENCE_JOINT) + q_body = _normalized_quat(spine3_rotation_wxyz) if q_body is None: return None - # Head and Spine3 share the same neutral orientation in the supported + # The HMD and Spine3 share the same neutral orientation in the supported # PICO convention, so their relative identity is the fixed zero pose. - q_cmd = _qmul(_qconj(q_body), q_head) + # Deliberately do not read the full-body tracker's skeleton Head joint: + # its model constraints can under-report extreme head pitch. + q_cmd = _qmul(_qconj(q_body), q_hmd) yaw_deg, pitch_deg, roll_deg = _openneck_yaw_pitch_roll_deg(q_cmd) # Convert the supported PICO convention to OpenNeck's physical command # convention: positive yaw turns left and positive pitch looks up. @@ -55,12 +59,11 @@ def map_frame(self, frame: HumanFrame) -> NeckCommand | None: ) -def _joint_quat(frame: HumanFrame, joint_name: str) -> FloatArray | None: - item = frame.get(joint_name) - if item is None: +def _normalized_quat(value: FloatArray | None) -> FloatArray | None: + if value is None: return None - quat = np.asarray(item[1], dtype=np.float64).reshape(-1) - if quat.shape[0] != 4 or not np.all(np.isfinite(quat)): + quat = np.asarray(value, dtype=np.float64).reshape(-1) + if quat.shape != (4,) or not np.all(np.isfinite(quat)): return None norm = float(np.linalg.norm(quat)) if norm <= 1e-9: diff --git a/teleopit/sim2real/neck/worker.py b/teleopit/sim2real/neck/worker.py index f4b7cab6..7abdfb29 100644 --- a/teleopit/sim2real/neck/worker.py +++ b/teleopit/sim2real/neck/worker.py @@ -4,19 +4,22 @@ import time from typing import Any -from teleopit.inputs.realtime_packet import HumanFrame +import numpy as np +from numpy.typing import NDArray + from teleopit.sim2real.neck.config import NeckConfig, parse_neck_config -from teleopit.sim2real.neck.mapper import HeadPoseMapper, NeckCommand +from teleopit.sim2real.neck.mapper import HmdPoseMapper, NeckCommand from teleopit.sim2real.neck.openneck import NeckDevice, build_neck_device logger = logging.getLogger(__name__) +FloatArray = NDArray[np.float64] class NeckRuntime: def __init__(self, config: NeckConfig, device: NeckDevice | None = None) -> None: self._cfg = config self._device = device or build_neck_device(config) - self._mapper = HeadPoseMapper(config) + self._mapper = HmdPoseMapper(config) def start(self) -> None: self._device.connect() @@ -26,17 +29,21 @@ def start(self) -> None: def tick( self, *, - frame: HumanFrame | None, - frame_timestamp_s: float | None, + hmd_rotation_wxyz: FloatArray | None, + spine3_rotation_wxyz: FloatArray | None, + pose_timestamp_s: float | None, active: bool, now_s: float | None = None, ) -> NeckCommand | None: now = time.monotonic() if now_s is None else float(now_s) - if not active or frame is None or frame_timestamp_s is None: + if not active or pose_timestamp_s is None: return None - if now - float(frame_timestamp_s) > self._cfg.frame_timeout_s: + if now - float(pose_timestamp_s) > self._cfg.frame_timeout_s: return None - command = self._mapper.map_frame(frame) + command = self._mapper.map_pose( + hmd_rotation_wxyz=hmd_rotation_wxyz, + spine3_rotation_wxyz=spine3_rotation_wxyz, + ) if command is None: return None applied_yaw_deg, applied_pitch_deg = self._device.move_deg( @@ -72,12 +79,13 @@ def start(self) -> None: def tick( self, *, - frame: HumanFrame | None, - frame_timestamp_s: float | None, + hmd_rotation_wxyz: FloatArray | None, + spine3_rotation_wxyz: FloatArray | None, + pose_timestamp_s: float | None, active: bool, now_s: float | None = None, ) -> None: - del frame, frame_timestamp_s, active, now_s + del hmd_rotation_wxyz, spine3_rotation_wxyz, pose_timestamp_s, active, now_s return None def close(self) -> None: @@ -98,10 +106,33 @@ def mode_packet_active(mode_packet: object | None, config: NeckConfig) -> bool: return mode in config.active_modes -def body_packet_frame(packet: object | None) -> tuple[HumanFrame | None, float | None, int]: - if packet is None or not all(hasattr(packet, attr) for attr in ("frame", "timestamp_s", "seq")): - return None, None, -1 +def head_pose_packet( + packet: object | None, +) -> tuple[FloatArray | None, FloatArray | None, float | None, int]: + if packet is None or not all(hasattr(packet, attr) for attr in ("snapshot", "timestamp_s", "seq")): + return None, None, None, -1 + snapshot = getattr(packet, "snapshot") + if snapshot is None or not all( + hasattr(snapshot, attr) + for attr in ("hmd_rotation_wxyz", "spine3_rotation_wxyz", "timestamp_s", "seq") + ): + return None, None, None, -1 + try: + timestamp_s = float(getattr(packet, "timestamp_s")) + seq = int(getattr(packet, "seq")) + if int(getattr(snapshot, "seq")) != seq: + return None, None, None, -1 + hmd_rotation = _optional_quat(getattr(snapshot, "hmd_rotation_wxyz")) + spine3_rotation = _optional_quat(getattr(snapshot, "spine3_rotation_wxyz")) + return hmd_rotation, spine3_rotation, timestamp_s, seq + except (TypeError, ValueError): + return None, None, None, -1 + + +def _optional_quat(value: object | None) -> FloatArray | None: + if value is None: + return None try: - return getattr(packet, "frame"), float(getattr(packet, "timestamp_s")), int(getattr(packet, "seq")) + return np.asarray(value, dtype=np.float64).reshape(-1) except (TypeError, ValueError): - return None, None, -1 + return None diff --git a/tests/test_active_neck.py b/tests/test_active_neck.py index 7c684b60..ef640133 100644 --- a/tests/test_active_neck.py +++ b/tests/test_active_neck.py @@ -5,11 +5,12 @@ import numpy as np -from teleopit.inputs.pico4_provider import BODY_JOINT_NAMES, Pico4InputProvider +from teleopit.inputs.pico4_provider import PicoHeadPoseSnapshot from teleopit.sim2real.neck.config import NeckConfig, parse_neck_config -from teleopit.sim2real.neck.mapper import HeadPoseMapper +from teleopit.sim2real.neck.mapper import HmdPoseMapper from teleopit.sim2real.neck.openneck import DryRunNeckDevice, OpenNeckDevice -from teleopit.sim2real.neck.worker import NeckRuntime, body_packet_frame +from teleopit.sim2real.neck.worker import NeckRuntime, head_pose_packet +from teleopit.sim2real.mp.messages import SnapshotPacket def _quat_y(deg: float) -> np.ndarray: @@ -22,14 +23,6 @@ def _quat_x(deg: float) -> np.ndarray: return np.array([math.cos(rad / 2.0), math.sin(rad / 2.0), 0.0, 0.0], dtype=np.float64) -def _frame(head: np.ndarray, spine: np.ndarray | None = None): - pos = np.zeros(3, dtype=np.float64) - frame = {"Head": (pos, head)} - if spine is not None: - frame["Spine3"] = (pos, spine) - return frame - - class FakeDevice: def __init__(self) -> None: self.moves: list[tuple[float, float]] = [] @@ -54,54 +47,54 @@ def close(self) -> None: self.closed = True -def test_head_pose_mapper_maps_fixed_pico_convention_to_openneck_degrees() -> None: - mapper = HeadPoseMapper(NeckConfig(enabled=True, dead_zone_deg=0.0)) +def test_hmd_pose_mapper_maps_fixed_pico_convention_to_openneck_degrees() -> None: + mapper = HmdPoseMapper(NeckConfig(enabled=True, dead_zone_deg=0.0)) - command = mapper.map_frame(_frame(_quat_y(30.0), _quat_y(0.0))) + command = mapper.map_pose( + hmd_rotation_wxyz=_quat_y(30.0), + spine3_rotation_wxyz=_quat_y(0.0), + ) assert command is not None assert command.yaw_deg == pytest_approx(-30.0) - command = mapper.map_frame(_frame(_quat_y(0.0), _quat_y(0.0))) + command = mapper.map_pose( + hmd_rotation_wxyz=_quat_y(0.0), + spine3_rotation_wxyz=_quat_y(0.0), + ) assert command is not None assert command.yaw_deg == pytest_approx(0.0) - command = mapper.map_frame(_frame(_quat_x(15.0), _quat_x(0.0))) + command = mapper.map_pose( + hmd_rotation_wxyz=_quat_x(15.0), + spine3_rotation_wxyz=_quat_x(0.0), + ) assert command is not None assert command.pitch_deg == pytest_approx(-15.0) -def test_head_pose_mapper_uses_body_relative_orientation() -> None: - mapper = HeadPoseMapper(NeckConfig(enabled=True, dead_zone_deg=0.0)) - - command = mapper.map_frame(_frame(_quat_y(40.0), _quat_y(10.0))) - - assert command is not None - assert command.yaw_deg == pytest_approx(-30.0) - - -def test_head_pose_mapper_handles_converted_pico_neutral_and_yaw() -> None: - body_poses = np.zeros((len(BODY_JOINT_NAMES), 7), dtype=np.float64) - body_poses[:, 6] = 1.0 - mapper = HeadPoseMapper(NeckConfig(enabled=True, dead_zone_deg=0.0)) +def test_hmd_pose_mapper_uses_body_relative_orientation() -> None: + mapper = HmdPoseMapper(NeckConfig(enabled=True, dead_zone_deg=0.0)) - neutral = mapper.map_frame(Pico4InputProvider._convert_body_joints_to_frame(body_poses)) - assert neutral is not None - assert neutral.yaw_deg == pytest_approx(0.0) - assert neutral.pitch_deg == pytest_approx(0.0) + command = mapper.map_pose( + hmd_rotation_wxyz=_quat_y(40.0), + spine3_rotation_wxyz=_quat_y(10.0), + ) - head_idx = BODY_JOINT_NAMES.index("Head") - body_poses[head_idx, 4] = math.sin(math.radians(30.0) / 2.0) - body_poses[head_idx, 6] = math.cos(math.radians(30.0) / 2.0) - command = mapper.map_frame(Pico4InputProvider._convert_body_joints_to_frame(body_poses)) assert command is not None assert command.yaw_deg == pytest_approx(-30.0) - assert command.pitch_deg == pytest_approx(0.0) -def test_head_pose_mapper_requires_spine3_joint() -> None: - mapper = HeadPoseMapper(NeckConfig(enabled=True)) +def test_hmd_pose_mapper_requires_hmd_and_spine3_orientations() -> None: + mapper = HmdPoseMapper(NeckConfig(enabled=True)) - assert mapper.map_frame(_frame(_quat_y(30.0))) is None + assert mapper.map_pose( + hmd_rotation_wxyz=_quat_y(30.0), + spine3_rotation_wxyz=None, + ) is None + assert mapper.map_pose( + hmd_rotation_wxyz=None, + spine3_rotation_wxyz=_quat_y(0.0), + ) is None def test_neck_runtime_sends_degrees_and_returns_applied_target() -> None: @@ -116,14 +109,16 @@ def test_neck_runtime_sends_degrees_and_returns_applied_target() -> None: runtime.start() command = runtime.tick( - frame=_frame(_quat_y(30.0), _quat_y(0.0)), - frame_timestamp_s=1.0, + hmd_rotation_wxyz=_quat_y(30.0), + spine3_rotation_wxyz=_quat_y(0.0), + pose_timestamp_s=1.0, active=True, now_s=1.01, ) neutral_command = runtime.tick( - frame=_frame(_quat_y(0.0), _quat_y(0.0)), - frame_timestamp_s=1.02, + hmd_rotation_wxyz=_quat_y(0.0), + spine3_rotation_wxyz=_quat_y(0.0), + pose_timestamp_s=1.02, active=True, now_s=1.03, ) @@ -179,10 +174,44 @@ def center(self) -> None: assert device.closed is True -def test_body_packet_frame_ignores_incomplete_packets() -> None: - assert body_packet_frame(None) == (None, None, -1) - assert body_packet_frame(SimpleNamespace(frame=_frame(_quat_y(0.0)))) == (None, None, -1) - assert body_packet_frame(SimpleNamespace(frame=_frame(_quat_y(0.0)), timestamp_s="bad", seq=1)) == (None, None, -1) +def test_head_pose_packet_extracts_synchronized_snapshot() -> None: + snapshot = PicoHeadPoseSnapshot( + hmd_rotation_wxyz=_quat_y(20.0), + spine3_rotation_wxyz=_quat_y(5.0), + timestamp_s=1.0, + seq=4, + ) + + hmd_rotation, spine3_rotation, timestamp_s, seq = head_pose_packet( + SnapshotPacket(snapshot=snapshot, timestamp_s=1.0, seq=4) + ) + + np.testing.assert_allclose(hmd_rotation, _quat_y(20.0)) + np.testing.assert_allclose(spine3_rotation, _quat_y(5.0)) + assert timestamp_s == 1.0 + assert seq == 4 + + +def test_head_pose_packet_ignores_incomplete_or_mismatched_packets() -> None: + assert head_pose_packet(None) == (None, None, None, -1) + assert head_pose_packet(SimpleNamespace(snapshot=object(), timestamp_s=1.0, seq=1)) == ( + None, + None, + None, + -1, + ) + snapshot = PicoHeadPoseSnapshot( + hmd_rotation_wxyz=_quat_y(0.0), + spine3_rotation_wxyz=_quat_y(0.0), + timestamp_s=1.0, + seq=2, + ) + assert head_pose_packet(SnapshotPacket(snapshot=snapshot, timestamp_s=1.0, seq=3)) == ( + None, + None, + None, + -1, + ) def test_openneck_device_uses_angle_api_and_returns_applied_target(monkeypatch) -> None: diff --git a/tests/test_pico4_provider.py b/tests/test_pico4_provider.py index e785aaae..ae98f7a9 100644 --- a/tests/test_pico4_provider.py +++ b/tests/test_pico4_provider.py @@ -28,10 +28,16 @@ def _pico_frame( body_active: bool = True, right_primary: bool = False, right_secondary: bool = False, + head_rotation_xyzw: np.ndarray | None = None, ) -> SimpleNamespace: return SimpleNamespace( seq=seq, receive_time_s=timestamp, + head=( + None + if head_rotation_xyzw is None + else SimpleNamespace(rotation=np.asarray(head_rotation_xyzw, dtype=np.float64)) + ), body=SimpleNamespace(active=body_active, joints=body_poses), controllers=SimpleNamespace( left=SimpleNamespace(buttons={}), @@ -70,6 +76,7 @@ def _make_provider() -> Pico4InputProvider: provider._ground_alignment_offset = None provider._controller_snapshot = None provider._hand_snapshot = None + provider._head_pose_snapshot = None provider._closed = False return provider @@ -345,3 +352,93 @@ def test_pico4_provider_exposes_hand_snapshot_when_body_inactive() -> None: assert snapshot.right.present is True assert snapshot.right.active is False np.testing.assert_allclose(snapshot.left.joints[:, 0:3], 1.5) + + +def test_pico4_provider_exposes_hmd_rotation_separately_from_skeleton_head() -> None: + provider = _make_provider() + body_poses = _body_poses(1.0) + angle = np.deg2rad(30.0) + hmd_rotation_xyzw = np.array( + [0.0, np.sin(angle / 2.0), 0.0, np.cos(angle / 2.0)], + dtype=np.float64, + ) + + assert provider._accept_pico_frame( + _pico_frame( + body_poses, + seq=7, + timestamp=3.0, + head_rotation_xyzw=hmd_rotation_xyzw, + ) + ) is True + + snapshot = provider.get_head_pose_snapshot() + assert snapshot is not None + assert snapshot.seq == 7 + assert snapshot.timestamp_s == pytest.approx(3.0) + expected_body = body_poses.copy() + expected_body[BODY_JOINT_NAMES.index("Head"), 3:7] = hmd_rotation_xyzw + expected_frame = Pico4InputProvider._convert_body_joints_to_frame(expected_body) + np.testing.assert_allclose( + snapshot.hmd_rotation_wxyz, + expected_frame["Head"][1], + atol=1e-6, + ) + np.testing.assert_allclose( + snapshot.spine3_rotation_wxyz, + expected_frame["Spine3"][1], + atol=1e-6, + ) + skeleton_frame = Pico4InputProvider._convert_body_joints_to_frame(body_poses) + assert not np.allclose(snapshot.hmd_rotation_wxyz, skeleton_frame["Head"][1]) + + +def test_pico4_provider_updates_hmd_snapshot_when_duplicate_body_is_dropped() -> None: + provider = _make_provider() + body_poses = _body_poses(1.0) + + assert provider._accept_pico_frame( + _pico_frame( + body_poses, + seq=1, + timestamp=1.0, + head_rotation_xyzw=np.array([0.0, 0.0, 0.0, 1.0]), + ) + ) is True + first = provider.get_head_pose_snapshot() + assert first is not None + + angle = np.deg2rad(20.0) + assert provider._accept_pico_frame( + _pico_frame( + body_poses.copy(), + seq=2, + timestamp=1.01, + head_rotation_xyzw=np.array([np.sin(angle / 2.0), 0.0, 0.0, np.cos(angle / 2.0)]), + ) + ) is False + second = provider.get_head_pose_snapshot() + + assert second is not None + assert second.seq == 2 + assert second.timestamp_s == pytest.approx(1.01) + assert not np.allclose(second.hmd_rotation_wxyz, first.hmd_rotation_wxyz) + + +def test_pico4_provider_invalidates_spine3_when_body_tracking_is_inactive() -> None: + provider = _make_provider() + + assert provider._accept_pico_frame( + _pico_frame( + _body_poses(1.0), + seq=4, + timestamp=2.0, + body_active=False, + head_rotation_xyzw=np.array([0.0, 0.0, 0.0, 1.0]), + ) + ) is False + + snapshot = provider.get_head_pose_snapshot() + assert snapshot is not None + assert snapshot.hmd_rotation_wxyz is not None + assert snapshot.spine3_rotation_wxyz is None diff --git a/tests/test_sim2real_multiprocess.py b/tests/test_sim2real_multiprocess.py index 10fdb717..825ca2c7 100644 --- a/tests/test_sim2real_multiprocess.py +++ b/tests/test_sim2real_multiprocess.py @@ -54,6 +54,7 @@ _human_frame_is_valid, _recording_hardware_types, _run_neck_worker, + _run_pico_io_worker, ) from teleopit.sim2real.mp.shm import SharedFrameRingReader, SharedFrameRingWriter @@ -346,6 +347,114 @@ def Process(self, *, name: str, target: object, args: tuple[object, ...]) -> Fak assert started_names == ["pico_input", "reference", "robot_control", "neck_worker"] +@pytest.mark.parametrize("failure_stage", ["setup", "snapshot", "publish", "close"]) +def test_head_pose_ipc_failure_does_not_stop_pico_input(monkeypatch, failure_stage: str) -> None: + endpoints = default_endpoints(base_port=39890) + closed_publishers: list[str] = [] + provider_closed = False + + class FakeStopEvent: + def __init__(self) -> None: + self.polls = 0 + + def is_set(self) -> bool: + self.polls += 1 + return self.polls > 1 + + def set(self) -> None: + self.polls = 2 + + class FakeProvider: + fps = 0.0 + + def __init__(self, **_kwargs: object) -> None: + return None + + def get_head_pose_snapshot(self) -> SimpleNamespace: + if failure_stage == "snapshot": + raise RuntimeError("head-pose snapshot failed") + return SimpleNamespace( + hmd_rotation_wxyz=np.array([1.0, 0.0, 0.0, 0.0]), + spine3_rotation_wxyz=np.array([1.0, 0.0, 0.0, 0.0]), + timestamp_s=1.0, + seq=1, + ) + + def has_frame(self) -> bool: + return False + + def pop_control_events(self) -> tuple[object, ...]: + return () + + def get_controller_snapshot(self) -> None: + return None + + def get_hand_snapshot(self) -> None: + return None + + def close(self) -> None: + nonlocal provider_closed + provider_closed = True + + class FakeVideoRuntime: + pushed_frames = 0 + + def __init__(self, **_kwargs: object) -> None: + return None + + def start(self) -> None: + return None + + def tick(self) -> None: + return None + + def stop(self) -> None: + return None + + class FakeSubscriber: + def __init__(self, _endpoint: str, _topic: str) -> None: + return None + + def recv_latest(self) -> None: + return None + + def close(self) -> None: + return None + + class FakePublisher: + def __init__(self, endpoint: str) -> None: + self.endpoint = endpoint + if endpoint == endpoints.head_pose_pub and failure_stage == "setup": + raise RuntimeError("head-pose bind failed") + + def publish(self, _topic: str, _payload: object) -> None: + if self.endpoint == endpoints.head_pose_pub and failure_stage == "publish": + raise RuntimeError("head-pose publish failed") + + def close(self) -> None: + closed_publishers.append(self.endpoint) + if self.endpoint == endpoints.head_pose_pub and failure_stage == "close": + raise RuntimeError("head-pose close failed") + + monkeypatch.setattr("teleopit.sim2real.mp.runtime.Pico4InputProvider", FakeProvider) + monkeypatch.setattr("teleopit.sim2real.mp.runtime.PicoVideoRuntime", FakeVideoRuntime) + monkeypatch.setattr("teleopit.sim2real.mp.runtime.LatestSubscriber", FakeSubscriber) + monkeypatch.setattr("teleopit.sim2real.mp.runtime.ZmqPublisher", FakePublisher) + + _run_pico_io_worker( + {"input": {"provider": "pico4"}, "neck": {"enabled": True}}, + endpoints, + FakeStopEvent(), # type: ignore[arg-type] + ) + + assert provider_closed is True + assert endpoints.body_pub in closed_publishers + if failure_stage == "setup": + assert endpoints.head_pose_pub not in closed_publishers + else: + assert closed_publishers.count(endpoints.head_pose_pub) == 1 + + @pytest.mark.parametrize("recording_enabled", [False, True]) def test_neck_command_publisher_is_only_created_for_recording(monkeypatch, recording_enabled: bool) -> None: publisher_endpoints: list[str] = [] From 99285a0e8b7337b755498084b763f3307e298a57 Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Wed, 15 Jul 2026 23:08:48 +0800 Subject: [PATCH 20/59] Add OpenNeck pitch gain --- AGENTS.md | 3 +- README.md | 6 ++- docs/docs/configuration/config-reference.md | 11 +++-- .../current/configuration/config-reference.md | 7 ++- scripts/dev/test_openneck.py | 6 +++ teleopit/configs/pico4_sim2real.yaml | 1 + teleopit/configs/sim2real.yaml | 1 + teleopit/sim2real/neck/config.py | 6 +++ teleopit/sim2real/neck/mapper.py | 2 + tests/test_active_neck.py | 44 +++++++++++++++++-- 10 files changed, 75 insertions(+), 12 deletions(-) diff --git a/AGENTS.md b/AGENTS.md index 8e9bc589..fc7434ae 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -166,7 +166,8 @@ target_dof_pos = clip(action, -10, 10) × action_scale + default_dof_pos - OpenNeck is integrated as a non-critical sim2real `neck_worker`; failures should not stop the G1 control loop, and no OpenNeck state is added to the 167D policy observation - OpenNeck 0.2.0 is the supported runtime; Teleopit sends physical degrees through `move_deg()`, and the direct-drive OpenNeck package converts degrees to servo steps and clips them to its calibrated mechanical limits; the removed normalized API and config fields are unsupported - OpenNeck maps the independent HMD `PicoFrame.head.rotation` relative to the same-frame full-body `Body.Spine3` orientation; it must never use the full-body `Body.Head` skeleton joint for neck control, and HMD pose updates must remain independent of duplicate-body-frame filtering -- OpenNeck uses a fixed identity neutral pose and no neck-side EMA; it must not capture the first live frame as a runtime zero pose, so tracking can start while the operator's head is turned; positive yaw turns left and positive pitch looks up +- OpenNeck uses a fixed identity neutral pose and no neck-side EMA; it must not capture the first live frame as a runtime zero pose, so tracking can start while the operator's head is turned +- After the raw relative-angle dead zone, `neck.pitch_gain` (default `1.4`) scales pitch before `move_deg()` while yaw remains one-to-one; OpenNeck remains responsible for final mechanical clipping; positive yaw turns left and positive pitch looks up ### SimulationLoop Runtime Behavior - `realtime=true` enforces wall-clock pacing even without a viewer diff --git a/README.md b/README.md index 2012406a..a613085f 100644 --- a/README.md +++ b/README.md @@ -133,8 +133,10 @@ camera pipeline. The neck path reads the independent HMD source frame. It never uses the full-body tracker's `Body.Head` skeleton joint, whose model constraints can under-report extreme head pitch. The mapper uses a fixed neutral pose and no neck-side EMA, so tracking startup does not require -the operator to face straight ahead. Teleopit sends physical yaw/pitch angles -through the OpenNeck 0.2.0 `move_deg()` API; OpenNeck converts those angles for +the operator to face straight ahead. After the dead zone, Teleopit multiplies +the relative pitch by `neck.pitch_gain` (default `1.4`) to compensate for the +robot camera geometry; yaw remains one-to-one. It then sends physical angles +through the OpenNeck 0.2.0 `move_deg()` API. OpenNeck converts those angles for its direct-drive servos and clips them to the calibrated mechanical step limits. Positive yaw turns left and positive pitch looks up. diff --git a/docs/docs/configuration/config-reference.md b/docs/docs/configuration/config-reference.md index b0fece3b..6e909106 100644 --- a/docs/docs/configuration/config-reference.md +++ b/docs/docs/configuration/config-reference.md @@ -169,10 +169,12 @@ body-frame filtering. The mapper uses the fixed PICO neutral orientation and no neck-side EMA; startup does not capture the operator's first pose as a new zero pose, so the operator does not need to face straight when tracking starts. Teleopit converts the supported PICO convention to OpenNeck's physical -convention—positive yaw turns left and positive pitch looks up—and sends the -relative angles in degrees through OpenNeck 0.2.0 `move_deg()`. OpenNeck -performs the direct-drive degree-to-step conversion and clips each target to -the mechanical step limits in its calibration file. +convention—positive yaw turns left and positive pitch looks up. After applying +`neck.dead_zone_deg` to the raw relative angles, it multiplies pitch by +`neck.pitch_gain` (default `1.4`) while leaving yaw one-to-one. The resulting +physical angles are sent through OpenNeck 0.2.0 `move_deg()`. OpenNeck performs +the direct-drive degree-to-step conversion and clips each target to the +mechanical step limits in its calibration file. OpenNeck 0.2.0 calibration files use angle-control fields such as `yaw_center_step`, `yaw_min_step`, `yaw_max_step`, and `yaw_step_sign` (and the @@ -191,6 +193,7 @@ are rejected rather than ignored. | `neck.frame_timeout_s` | Pico HMD/Spine3 pose staleness threshold | `0.2` | | `neck.active_modes` | Sim2real modes that allow neck motion | `[standing, mocap, arms, pause]` | | `neck.dead_zone_deg` | Yaw/pitch dead zone in degrees | `0.5` | +| `neck.pitch_gain` | Gain applied to relative HMD pitch after the dead zone | `1.4` | | `neck.center_on_start` / `center_on_shutdown` | Center the gimbal at worker startup/shutdown | `true` / `false` | | `neck.release_on_shutdown` | Release servo torque after shutdown when supported | `false` | | `neck.dry_run` | Compute commands without opening OpenNeck hardware | `false` | diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md index 824fbc3a..b797d2b6 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md @@ -183,8 +183,10 @@ OpenNeck 作为非关键 sim2real worker 运行，不会改变策略观测。头 头显姿态更新不受 body 重复帧过滤影响。mapper 使用固定的 PICO 中立姿态且不进行颈部侧 EMA；启动时不会把操作者的第一帧姿态采集为新的零位，因此开始追踪时操作者不需要保持头部 朝正前方。Teleopit 将受支持的 PICO 约定转换为 OpenNeck 的物理约定——正 yaw 向左转， -正 pitch 向上看——并通过 OpenNeck 0.2.0 的 `move_deg()` 发送以度为单位的相对角度。 -OpenNeck 负责直驱角度到舵机步数的转换，并将每个目标裁剪到标定文件中的机械步数限位。 +正 pitch 向上看。对原始相对角度应用 `neck.dead_zone_deg` 后，Teleopit 将 pitch 乘以 +`neck.pitch_gain`（默认 `1.4`），而 yaw 仍保持一比一。随后通过 OpenNeck 0.2.0 的 +`move_deg()` 发送得到的物理角度。OpenNeck 负责直驱角度到舵机步数的转换，并将每个目标 +裁剪到标定文件中的机械步数限位。 OpenNeck 0.2.0 标定文件使用 `yaw_center_step`、`yaw_min_step`、 `yaw_max_step` 和 `yaw_step_sign` 等角度控制字段（pitch 使用对应字段）。不支持以前的 @@ -202,6 +204,7 @@ OpenNeck 归一化配置；运行 `openneck calibrate` 创建当前格式的文 | `neck.frame_timeout_s` | Pico 头显/Spine3 姿态过期阈值 | `0.2` | | `neck.active_modes` | 允许头颈运动的 sim2real 模式 | `[standing, mocap, arms, pause]` | | `neck.dead_zone_deg` | yaw/pitch 死区（度） | `0.5` | +| `neck.pitch_gain` | 死区后应用于头显相对 pitch 的增益 | `1.4` | | `neck.center_on_start` / `center_on_shutdown` | worker 启动/关闭时回中云台 | `true` / `false` | | `neck.release_on_shutdown` | 关闭后在支持时释放舵机扭矩 | `false` | | `neck.dry_run` | 只计算命令，不打开 OpenNeck 硬件 | `false` | diff --git a/scripts/dev/test_openneck.py b/scripts/dev/test_openneck.py index 25874168..24f7a04b 100644 --- a/scripts/dev/test_openneck.py +++ b/scripts/dev/test_openneck.py @@ -5,6 +5,7 @@ import argparse import logging +import math from pathlib import Path import sys import time @@ -21,6 +22,7 @@ DEFAULT_RATE_HZ = 60.0 DEFAULT_FRAME_TIMEOUT_S = 0.3 +DEFAULT_PITCH_GAIN = 1.4 DEFAULT_TEST_ANGLE_DEG = 5.0 DEFAULT_HOLD_S = 0.8 DEFAULT_PICO_TIMEOUT_S = 60.0 @@ -54,6 +56,7 @@ def parse_args() -> argparse.Namespace: parser.add_argument("--no-center-on-shutdown", action="store_true") parser.add_argument("--release-on-shutdown", action="store_true") parser.add_argument("--dead-zone-deg", type=float, default=0.5) + parser.add_argument("--pitch-gain", type=float, default=DEFAULT_PITCH_GAIN) parser.add_argument("--bridge-host", default="0.0.0.0") parser.add_argument("--bridge-port", type=int, default=63901) parser.add_argument("--bridge-discovery", action=argparse.BooleanOptionalAction, default=True) @@ -69,6 +72,8 @@ def parse_args() -> argparse.Namespace: raise SystemExit("--duration-s must be >= 0") if args.angle_deg <= 0.0: raise SystemExit("--angle-deg must be > 0") + if not math.isfinite(args.pitch_gain) or args.pitch_gain <= 0.0: + raise SystemExit("--pitch-gain must be finite and > 0") return args @@ -82,6 +87,7 @@ def make_neck_config(args: argparse.Namespace) -> NeckConfig: frame_timeout_s=args.frame_timeout_s, active_modes=("mocap",), dead_zone_deg=args.dead_zone_deg, + pitch_gain=args.pitch_gain, center_on_start=not bool(args.no_center_on_start), center_on_shutdown=not bool(args.no_center_on_shutdown), release_on_shutdown=bool(args.release_on_shutdown), diff --git a/teleopit/configs/pico4_sim2real.yaml b/teleopit/configs/pico4_sim2real.yaml index 545b3449..3887185e 100644 --- a/teleopit/configs/pico4_sim2real.yaml +++ b/teleopit/configs/pico4_sim2real.yaml @@ -115,6 +115,7 @@ neck: frame_timeout_s: 0.2 active_modes: [standing, mocap, arms, pause] dead_zone_deg: 0.5 + pitch_gain: 1.4 center_on_start: true center_on_shutdown: false release_on_shutdown: false diff --git a/teleopit/configs/sim2real.yaml b/teleopit/configs/sim2real.yaml index b63a608f..4adc276c 100644 --- a/teleopit/configs/sim2real.yaml +++ b/teleopit/configs/sim2real.yaml @@ -116,6 +116,7 @@ neck: frame_timeout_s: 0.2 active_modes: [standing, mocap, arms, pause] dead_zone_deg: 0.5 + pitch_gain: 1.4 center_on_start: true center_on_shutdown: false release_on_shutdown: false diff --git a/teleopit/sim2real/neck/config.py b/teleopit/sim2real/neck/config.py index fb3d2128..fbdca070 100644 --- a/teleopit/sim2real/neck/config.py +++ b/teleopit/sim2real/neck/config.py @@ -2,6 +2,7 @@ from collections.abc import Iterable from dataclasses import dataclass +import math from pathlib import Path from typing import Any @@ -26,6 +27,7 @@ class NeckConfig: frame_timeout_s: float = 0.2 active_modes: tuple[str, ...] = ("standing", "mocap", "arms", "pause") dead_zone_deg: float = 0.5 + pitch_gain: float = 1.4 center_on_start: bool = True center_on_shutdown: bool = False release_on_shutdown: bool = False @@ -51,6 +53,9 @@ def parse_neck_config(cfg: Any) -> NeckConfig: dead_zone_deg = float(cfg_get(neck_cfg, "dead_zone_deg", 0.5)) if dead_zone_deg < 0: raise ValueError("neck.dead_zone_deg must be >= 0") + pitch_gain = float(cfg_get(neck_cfg, "pitch_gain", 1.4)) + if not math.isfinite(pitch_gain) or pitch_gain <= 0: + raise ValueError("neck.pitch_gain must be finite and > 0") config_path = cfg_get(neck_cfg, "config_path", None) if config_path in ("", "null"): config_path = None @@ -68,6 +73,7 @@ def parse_neck_config(cfg: Any) -> NeckConfig: frame_timeout_s=frame_timeout_s, active_modes=active_modes, dead_zone_deg=dead_zone_deg, + pitch_gain=pitch_gain, center_on_start=bool(cfg_get(neck_cfg, "center_on_start", True)), center_on_shutdown=bool(cfg_get(neck_cfg, "center_on_shutdown", False)), release_on_shutdown=bool(cfg_get(neck_cfg, "release_on_shutdown", False)), diff --git a/teleopit/sim2real/neck/mapper.py b/teleopit/sim2real/neck/mapper.py index 1c284127..f5737ba8 100644 --- a/teleopit/sim2real/neck/mapper.py +++ b/teleopit/sim2real/neck/mapper.py @@ -51,6 +51,8 @@ def map_pose( yaw_deg = 0.0 if abs(pitch_deg) < self._cfg.dead_zone_deg: pitch_deg = 0.0 + else: + pitch_deg *= self._cfg.pitch_gain return NeckCommand( yaw_deg=float(yaw_deg), diff --git a/tests/test_active_neck.py b/tests/test_active_neck.py index ef640133..77fec184 100644 --- a/tests/test_active_neck.py +++ b/tests/test_active_neck.py @@ -47,8 +47,10 @@ def close(self) -> None: self.closed = True -def test_hmd_pose_mapper_maps_fixed_pico_convention_to_openneck_degrees() -> None: - mapper = HmdPoseMapper(NeckConfig(enabled=True, dead_zone_deg=0.0)) +def test_hmd_pose_mapper_applies_pitch_gain_to_openneck_degrees() -> None: + mapper = HmdPoseMapper( + NeckConfig(enabled=True, dead_zone_deg=0.0, pitch_gain=1.4) + ) command = mapper.map_pose( hmd_rotation_wxyz=_quat_y(30.0), @@ -69,7 +71,27 @@ def test_hmd_pose_mapper_maps_fixed_pico_convention_to_openneck_degrees() -> Non spine3_rotation_wxyz=_quat_x(0.0), ) assert command is not None - assert command.pitch_deg == pytest_approx(-15.0) + assert command.pitch_deg == pytest_approx(-21.0) + + +def test_hmd_pose_mapper_applies_dead_zone_before_pitch_gain() -> None: + mapper = HmdPoseMapper( + NeckConfig(enabled=True, dead_zone_deg=0.5, pitch_gain=2.0) + ) + + inside_dead_zone = mapper.map_pose( + hmd_rotation_wxyz=_quat_x(0.4), + spine3_rotation_wxyz=_quat_x(0.0), + ) + outside_dead_zone = mapper.map_pose( + hmd_rotation_wxyz=_quat_x(10.0), + spine3_rotation_wxyz=_quat_x(0.0), + ) + + assert inside_dead_zone is not None + assert inside_dead_zone.pitch_deg == pytest_approx(0.0) + assert outside_dead_zone is not None + assert outside_dead_zone.pitch_deg == pytest_approx(-20.0) def test_hmd_pose_mapper_uses_body_relative_orientation() -> None: @@ -324,6 +346,22 @@ def test_parse_neck_config_accepts_scalar_active_mode() -> None: assert cfg.active_modes == ("mocap",) +def test_parse_neck_config_accepts_pitch_gain() -> None: + cfg = parse_neck_config({"neck": {"enabled": True, "pitch_gain": 1.6}}) + + assert cfg.pitch_gain == pytest_approx(1.6) + + +def test_parse_neck_config_rejects_invalid_pitch_gain() -> None: + for value in (0.0, -1.0, float("nan"), float("inf")): + try: + parse_neck_config({"neck": {"enabled": True, "pitch_gain": value}}) + except ValueError as exc: + assert "neck.pitch_gain" in str(exc) + else: + raise AssertionError(f"expected ValueError for pitch_gain={value!r}") + + def test_parse_neck_config_rejects_unknown_active_mode() -> None: try: parse_neck_config({"neck": {"enabled": True, "active_modes": ["mocap", "idle"]}}) From f67cfd0305225dc04abb51df551154899350386a Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Thu, 16 Jul 2026 11:08:48 +0800 Subject: [PATCH 21/59] Add synchronized sim2real recording reviewer --- AGENTS.md | 2 + README.md | 15 + docs/docs/getting-started/installation.md | 10 + docs/docs/tutorials/pico-sim2real.md | 25 + .../current/getting-started/installation.md | 9 + .../current/tutorials/pico-sim2real.md | 20 + pyproject.toml | 4 + scripts/view/view_recording.py | 1172 +++++++++++++++++ tests/test_recording_viewer.py | 156 +++ 9 files changed, 1413 insertions(+) create mode 100644 scripts/view/view_recording.py create mode 100644 tests/test_recording_viewer.py diff --git a/AGENTS.md b/AGENTS.md index fc7434ae..6ac1b4f5 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -64,6 +64,7 @@ scripts/ ├── run/run_sim2real.py # G1 sim2real control; supports offline BVH playback and Pico4 ├── run/record_pico_motion.py # Interactive Pico recording → G1 motion NPZ clips ├── render/render_sim.py # Render single BVH → 3 MuJoCo videos (mocap input, retarget, sim2sim) +├── view/view_recording.py # Read-only synchronized sim2real recording reviewer └── dev/compute_ik_offsets.py # Compute IK quaternion offsets for new BVH formats train_mimic/ # Training package ├── app.py # Shared app helpers for train/play/benchmark @@ -154,6 +155,7 @@ target_dof_pos = clip(action, -10, 10) × action_scale + default_dof_pos - Sim2real recording uses an editable source layout: `schema.json`, `episodes.jsonl`, per-episode HDF5 files under `recording.output_dir/data/`, and compressed MP4 files under `recording.output_dir/videos/d435i_rgb/`; task prompts live only in `episodes.jsonl`, and HDF5 files contain only frame arrays with no metadata attributes or raw RGB datasets - Recording `schema.json` stores `robot_type=unitree_g1_29dof`, `hand_type=none|linkerhand_l6|linkerhand_o6`, `neck_type=none|openneck`, FPS, and feature definitions; optional action fields are controlled directly by `hands.enabled` and `neck.enabled`; the recording worker rejects an existing mismatched schema without writing episodes, but remains non-critical and must not stop the G1 control runtime; the previous attribute-based HDF5 layout is unsupported - Episodes interrupted before their `episodes.jsonl` entry is committed are discarded on the next recording-worker startup and do not consume an episode index +- Review saved sim2real recordings with `scripts/view/view_recording.py`; it validates manifest/HDF5/MP4 alignment and synchronizes camera video, an observed-vs-reference MuJoCo overlay, joint/mode plots, and optional hand/neck signals; because measured root XYZ is not recorded, the observed robot is anchored to the reference root position - `gripper` mode reuses `Pico4InputProvider.get_controller_snapshot()` for Pico grip/trigger open-close control and supports LinkerHand L6 and O6 - `vr_hand_pose` mode reuses `Pico4InputProvider.get_hand_snapshot()` and somehand 0.2.0 public `somehand.api` for continuous Pico hand-pose retargeting; do not start a second `PicoBridge` for hand control - Teleopit owns Pico 26-joint hand-state to 21-landmark conversion; do not import `somehand.pico_input` diff --git a/README.md b/README.md index a613085f..f86eadfd 100644 --- a/README.md +++ b/README.md @@ -113,6 +113,21 @@ Recording is non-critical: an incompatible output schema stops only the recording worker while G1 control continues. Episodes interrupted before their manifest entry is committed are discarded on the next recording startup. +Review saved episodes in a synchronized read-only web UI: + +```bash +pip install -e '.[review]' +python scripts/view/view_recording.py \ + --recording data/recordings/sim2real_hdf5 +``` + +The reviewer validates the manifest, HDF5 arrays, and MP4 frame count before +playback. It shows D435i video beside the observed G1 pose with a translucent +green reference overlay, plus mode, joint-tracking, LinkerHand, and OpenNeck +timelines. Because recordings do not contain measured root XYZ, the observed +pose is anchored to the reference root position; joint and root-orientation +comparisons remain valid. + ## OpenNeck Active Vision Pico sim2real can drive the optional OpenNeck two-axis active-vision gimbal from diff --git a/docs/docs/getting-started/installation.md b/docs/docs/getting-started/installation.md index fc781124..6deefc72 100644 --- a/docs/docs/getting-started/installation.md +++ b/docs/docs/getting-started/installation.md @@ -101,6 +101,16 @@ pip package: conda install -c conda-forge pyrealsense2 ``` +### Recording Review + +```bash +pip install -e '.[review]' +``` + +Adds the OpenCV and MuJoCo/Viser dependencies used by the read-only synchronized +sim2real recording reviewer. The review extra does not install Pico, RealSense, +or G1 control dependencies. + ## Verify Installation ```bash diff --git a/docs/docs/tutorials/pico-sim2real.md b/docs/docs/tutorials/pico-sim2real.md index 0093e5bb..b03bfc69 100644 --- a/docs/docs/tutorials/pico-sim2real.md +++ b/docs/docs/tutorials/pico-sim2real.md @@ -127,6 +127,31 @@ control is enabled, it stores the latest mechanically clamped `[yaw_deg, pitch_deg]` target in degrees as `action.neck(2)`. Disabled devices do not add their action fields. +### Review Saved Episodes + +Install the lightweight review dependencies and launch the read-only web +reviewer against a recording root: + +```bash +pip install -e '.[review]' +python scripts/view/view_recording.py \ + --recording data/recordings/sim2real_hdf5 +``` + +Open the printed local URL in a browser. The reviewer synchronizes the D435i +MP4 with a MuJoCo view of the observed G1 pose and a translucent green +reference pose. Use the episode selector, frame scrubber, playback speed, and +joint selector to inspect tracking. The side panel includes the mode timeline, +per-body-group joint error, optional LinkerHand channels, and optional OpenNeck +yaw/pitch. + +The reviewer validates `schema.json`, every manifest path, HDF5 shapes and +finite values, and MP4 frame count/FPS before playback. It never modifies the +recording. `observation.state` does not contain measured root XYZ, so the +observed robot is anchored to the reference root position in the overlay; +joint tracking and root-orientation comparisons remain valid, but global root +translation cannot be evaluated from this recording format. + ## Operator Flow Keep the Unitree remote in hand. `L1+R1` is the emergency stop path into diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/installation.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/installation.md index 99998229..341fb404 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/installation.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/installation.md @@ -99,6 +99,15 @@ conda-forge，而不是 pip 包： conda install -c conda-forge pyrealsense2 ``` +### 录制 Review + +```bash +pip install -e '.[review]' +``` + +该 extra 会安装只读 sim2real 录制同步 reviewer 使用的 OpenCV 和 MuJoCo/Viser 依赖， +不会安装 Pico、RealSense 或 G1 控制依赖。 + ## 验证安装 ```bash diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md index 7a84c19f..c98a1653 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md @@ -121,6 +121,26 @@ episode 会保存为 `data/recordings/sim2real_hdf5/data/` 下的 `.h5` 文件 `[yaw_deg, pitch_deg]` 目标以度为单位保存为 `action.neck(2)`。未启用的设备不会添加 对应的 action 字段。 +### Review 已保存的 Episode + +安装轻量 review 依赖，然后对录制根目录启动只读 Web reviewer： + +```bash +pip install -e '.[review]' +python scripts/view/view_recording.py \ + --recording data/recordings/sim2real_hdf5 +``` + +在浏览器中打开终端输出的本地 URL。reviewer 会同步显示 D435i MP4、MuJoCo +中的 G1 实测姿态以及绿色半透明 reference 姿态。可以通过 episode 选择器、帧拖动条、 +播放速度和关节选择器检查跟踪效果。侧栏还包含模式时间线、各身体分组的关节误差， +以及可选的 LinkerHand 通道和 OpenNeck yaw/pitch。 + +播放前，reviewer 会验证 `schema.json`、manifest 中的所有路径、HDF5 shape 和有限值， +以及 MP4 帧数/FPS；它不会修改录制数据。`observation.state` 不包含实测 root XYZ， +因此叠加画面会把实测机器人锚定到 reference root 位置。关节跟踪和 root 朝向比较仍然 +有效，但无法通过当前录制格式评价全局 root 平移。 + ## 操作流程 始终把 Unitree 遥控器拿在手里。`L1+R1` 是进入 `DAMPING` 的急停路径。 diff --git a/pyproject.toml b/pyproject.toml index 7c70dac2..6b1427dd 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -62,6 +62,10 @@ recording = [ "opencv-python", "imageio[ffmpeg]", ] +review = [ + "opencv-python", + "mjviser>=0.0.14", +] dexhand = [] [tool.setuptools.packages.find] diff --git a/scripts/view/view_recording.py b/scripts/view/view_recording.py new file mode 100644 index 00000000..accc4567 --- /dev/null +++ b/scripts/view/view_recording.py @@ -0,0 +1,1172 @@ +#!/usr/bin/env python3 +"""Read-only synchronized reviewer for Teleopit sim2real recordings.""" + +from __future__ import annotations + +import argparse +from dataclasses import dataclass +import html +import json +from pathlib import Path +import sys +import time +from typing import Any + +import h5py +import numpy as np + +PROJECT_ROOT = Path(__file__).resolve().parents[2] +if str(PROJECT_ROOT) not in sys.path: + sys.path.insert(0, str(PROJECT_ROOT)) + +from teleopit.constants import FULL_QPOS_DIM, NUM_JOINTS +from teleopit.recording.hdf5 import ( + ACTION_KEY, + FRAME_INDEX_KEY, + HAND_ACTION_KEY, + HDF5_RECORDING_FORMAT, + HDF5_RECORDING_VERSION, + MODE_KEY, + NECK_ACTION_KEY, + STATE_KEY, + TIMESTAMP_KEY, +) +from teleopit.runtime.assets import UNITREE_G1_XML, missing_gmr_assets_message + + +DEFAULT_RECORDING_ROOT = PROJECT_ROOT / "data" / "recordings" / "sim2real_hdf5" +DEFAULT_XML = UNITREE_G1_XML + +JOINT_GROUPS: tuple[tuple[str, slice], ...] = ( + ("left leg", slice(0, 6)), + ("right leg", slice(6, 12)), + ("waist", slice(12, 15)), + ("left arm", slice(15, 22)), + ("right arm", slice(22, 29)), +) +GROUP_COLORS = ("#3b82f6", "#06b6d4", "#f59e0b", "#ec4899", "#8b5cf6") + + +@dataclass(frozen=True) +class RecordingEpisode: + episode_index: int + frames: int + task: str + data_path: Path + video_path: Path + + def label(self, fps: int) -> str: + duration_s = self.frames / fps + return f"#{self.episode_index:06d} · {self.task} · {duration_s:.1f}s" + + +@dataclass(frozen=True) +class RecordingDataset: + root: Path + schema: dict[str, Any] + features: dict[str, Any] + fps: int + image_key: str + image_shape: tuple[int, int, int] + mode_names: dict[int, str] + joint_names: tuple[str, ...] + hand_names: tuple[str, ...] + has_hand_action: bool + has_neck_action: bool + episodes: tuple[RecordingEpisode, ...] + + +@dataclass(frozen=True) +class EpisodeReviewData: + episode: RecordingEpisode + frame_index: np.ndarray + timestamps: np.ndarray + state: np.ndarray + mode: np.ndarray + action: np.ndarray + hand_action: np.ndarray | None + neck_action: np.ndarray | None + joint_error: np.ndarray + group_error: dict[str, np.ndarray] + root_orientation_error_rad: np.ndarray + joint_rmse_rad: float + root_orientation_rmse_rad: float + max_joint_error_rad: float + max_joint_error_frame: int + max_joint_error_name: str + + +def _read_json_object(path: Path, *, label: str) -> dict[str, Any]: + try: + payload = json.loads(path.read_text(encoding="utf-8")) + except FileNotFoundError as exc: + raise ValueError(f"Recording {label} not found: {path}") from exc + except (OSError, json.JSONDecodeError) as exc: + raise ValueError(f"Invalid recording {label}: {path}") from exc + if not isinstance(payload, dict): + raise ValueError(f"Recording {label} must contain a JSON object: {path}") + return payload + + +def _feature_shape(features: dict[str, Any], key: str) -> tuple[int, ...]: + feature = features.get(key) + if not isinstance(feature, dict): + raise ValueError(f"Recording schema is missing feature {key!r}") + shape = feature.get("shape") + if not isinstance(shape, list) or not all(isinstance(value, int) for value in shape): + raise ValueError(f"Recording schema feature {key!r} has invalid shape {shape!r}") + return tuple(shape) + + +def _feature_dtype(features: dict[str, Any], key: str) -> np.dtype: + feature = features.get(key) + raw_dtype = feature.get("dtype") if isinstance(feature, dict) else None + if not isinstance(raw_dtype, str): + raise ValueError( + f"Recording schema feature {key!r} has invalid dtype {raw_dtype!r}" + ) + try: + return np.dtype(raw_dtype) + except TypeError as exc: + raise ValueError( + f"Recording schema feature {key!r} has invalid dtype {raw_dtype!r}" + ) from exc + + +def _feature_names(features: dict[str, Any], key: str, expected: int) -> tuple[str, ...]: + feature = features.get(key) + names = feature.get("names") if isinstance(feature, dict) else None + if not isinstance(names, list) or len(names) != expected: + raise ValueError( + f"Recording schema feature {key!r} must define {expected} names, got {names!r}" + ) + return tuple(str(name) for name in names) + + +def _resolve_recording_path(root: Path, value: object, *, label: str) -> Path: + if not isinstance(value, str) or not value.strip(): + raise ValueError(f"Recording manifest {label} must be a non-empty relative path") + relative = Path(value) + if relative.is_absolute(): + raise ValueError(f"Recording manifest {label} must be relative to {root}: {value!r}") + resolved = (root / relative).resolve() + try: + resolved.relative_to(root) + except ValueError as exc: + raise ValueError(f"Recording manifest {label} escapes dataset root: {value!r}") from exc + if not resolved.is_file(): + raise ValueError(f"Recording manifest {label} not found: {resolved}") + return resolved + + +def _validate_hdf5_episode( + data_path: Path, + *, + frames: int, + features: dict[str, Any], + keys: tuple[str, ...], +) -> None: + try: + with h5py.File(data_path, "r") as h5: + for key in keys: + if key not in h5: + raise ValueError(f"Recording episode {data_path} is missing HDF5 dataset {key!r}") + expected_shape = (frames, *_feature_shape(features, key)) + actual_shape = tuple(h5[key].shape) + if actual_shape != expected_shape: + raise ValueError( + f"Recording episode {data_path} dataset {key!r} shape {actual_shape} " + f"!= manifest/schema shape {expected_shape}" + ) + expected_dtype = _feature_dtype(features, key) + actual_dtype = h5[key].dtype + if actual_dtype != expected_dtype: + raise ValueError( + f"Recording episode {data_path} dataset {key!r} dtype {actual_dtype} " + f"!= schema dtype {expected_dtype}" + ) + except OSError as exc: + raise ValueError(f"Cannot open recording episode HDF5: {data_path}") from exc + + +def load_recording_dataset(recording_root: str | Path) -> RecordingDataset: + """Load and validate the dataset-level schema and episode manifest.""" + + root = Path(recording_root).expanduser().resolve() + if not root.is_dir(): + raise ValueError(f"Recording root not found: {root}") + + schema = _read_json_object(root / "schema.json", label="schema.json") + if schema.get("format") != HDF5_RECORDING_FORMAT: + raise ValueError( + f"Unsupported recording format {schema.get('format')!r}; expected {HDF5_RECORDING_FORMAT!r}" + ) + if schema.get("version") != HDF5_RECORDING_VERSION: + raise ValueError( + f"Unsupported recording version {schema.get('version')!r}; expected {HDF5_RECORDING_VERSION}" + ) + + fps = schema.get("fps") + if not isinstance(fps, int) or fps <= 0: + raise ValueError(f"Recording schema fps must be a positive integer, got {fps!r}") + features = schema.get("features") + if not isinstance(features, dict): + raise ValueError("Recording schema features must be an object") + + expected_shapes = { + FRAME_INDEX_KEY: (), + TIMESTAMP_KEY: (), + STATE_KEY: (68,), + MODE_KEY: (), + ACTION_KEY: (FULL_QPOS_DIM,), + } + for key, expected_shape in expected_shapes.items(): + actual_shape = _feature_shape(features, key) + if actual_shape != expected_shape: + raise ValueError( + f"Recording schema feature {key!r} shape {actual_shape} != {expected_shape}" + ) + + video_keys = [ + str(key) + for key, feature in features.items() + if isinstance(feature, dict) and feature.get("dtype") == "video" + ] + if len(video_keys) != 1: + raise ValueError( + f"Recording reviewer requires exactly one video feature, found {video_keys}" + ) + image_key = video_keys[0] + image_shape = _feature_shape(features, image_key) + if len(image_shape) != 3 or image_shape[2] != 3 or min(image_shape) <= 0: + raise ValueError( + f"Recording video feature {image_key!r} must have [height, width, 3] shape, got {image_shape}" + ) + + mode_feature = features[MODE_KEY] + raw_mode_values = mode_feature.get("values") if isinstance(mode_feature, dict) else None + if not isinstance(raw_mode_values, dict) or not raw_mode_values: + raise ValueError(f"Recording schema feature {MODE_KEY!r} must define mode values") + mode_names: dict[int, str] = {} + for name, value in raw_mode_values.items(): + if not isinstance(value, int) or value in mode_names: + raise ValueError(f"Recording schema has invalid or duplicate mode code {value!r}") + mode_names[value] = str(name) + + action_names = _feature_names(features, ACTION_KEY, FULL_QPOS_DIM) + joint_names = action_names[7:] + if len(joint_names) != NUM_JOINTS: + raise ValueError( + f"Recording action names must contain {NUM_JOINTS} reference joints, got {len(joint_names)}" + ) + + hand_type = str(schema.get("hand_type", "none")).strip().lower() + neck_type = str(schema.get("neck_type", "none")).strip().lower() + has_hand_action = hand_type != "none" + has_neck_action = neck_type != "none" + hand_names: tuple[str, ...] = () + if has_hand_action: + if _feature_shape(features, HAND_ACTION_KEY) != (12,): + raise ValueError(f"Recording schema feature {HAND_ACTION_KEY!r} must be 12D") + hand_names = _feature_names(features, HAND_ACTION_KEY, 12) + elif HAND_ACTION_KEY in features: + raise ValueError( + f"Recording schema hand_type={hand_type!r} must not define {HAND_ACTION_KEY!r}" + ) + if has_neck_action: + if _feature_shape(features, NECK_ACTION_KEY) != (2,): + raise ValueError(f"Recording schema feature {NECK_ACTION_KEY!r} must be 2D") + _feature_names(features, NECK_ACTION_KEY, 2) + elif NECK_ACTION_KEY in features: + raise ValueError( + f"Recording schema neck_type={neck_type!r} must not define {NECK_ACTION_KEY!r}" + ) + + hdf5_keys = [FRAME_INDEX_KEY, TIMESTAMP_KEY, STATE_KEY, MODE_KEY, ACTION_KEY] + if has_hand_action: + hdf5_keys.append(HAND_ACTION_KEY) + if has_neck_action: + hdf5_keys.append(NECK_ACTION_KEY) + + manifest_path = root / "episodes.jsonl" + try: + lines = manifest_path.read_text(encoding="utf-8").splitlines() + except OSError as exc: + raise ValueError(f"Recording manifest not found or unreadable: {manifest_path}") from exc + + episodes: list[RecordingEpisode] = [] + for line_number, raw_line in enumerate(lines, start=1): + if not raw_line.strip(): + continue + try: + entry = json.loads(raw_line) + except json.JSONDecodeError as exc: + raise ValueError(f"Invalid JSON in {manifest_path}:{line_number}") from exc + if not isinstance(entry, dict): + raise ValueError(f"Recording manifest entry at line {line_number} must be an object") + + expected_index = len(episodes) + episode_index = entry.get("episode_index") + if episode_index != expected_index: + raise ValueError( + f"Recording episode indices must be contiguous from 0; line {line_number} " + f"expected {expected_index}, got {episode_index!r}" + ) + frames = entry.get("frames") + if not isinstance(frames, int) or frames <= 0: + raise ValueError( + f"Recording manifest entry {episode_index} frames must be positive, got {frames!r}" + ) + task = entry.get("task") + if not isinstance(task, str) or not task.strip(): + raise ValueError(f"Recording manifest entry {episode_index} task must not be empty") + + videos = entry.get("videos") + if not isinstance(videos, dict) or image_key not in videos: + raise ValueError( + f"Recording manifest entry {episode_index} is missing video path for {image_key!r}" + ) + data_path = _resolve_recording_path( + root, + entry.get("data"), + label=f"episode {episode_index} data", + ) + video_path = _resolve_recording_path( + root, + videos[image_key], + label=f"episode {episode_index} video {image_key}", + ) + _validate_hdf5_episode( + data_path, + frames=frames, + features=features, + keys=tuple(hdf5_keys), + ) + episodes.append( + RecordingEpisode( + episode_index=episode_index, + frames=frames, + task=task.strip(), + data_path=data_path, + video_path=video_path, + ) + ) + + if not episodes: + raise ValueError(f"Recording manifest has no saved episodes: {manifest_path}") + + return RecordingDataset( + root=root, + schema=schema, + features=features, + fps=fps, + image_key=image_key, + image_shape=image_shape, + mode_names=mode_names, + joint_names=joint_names, + hand_names=hand_names, + has_hand_action=has_hand_action, + has_neck_action=has_neck_action, + episodes=tuple(episodes), + ) + + +def _normalized_quaternions(values: np.ndarray, *, label: str) -> np.ndarray: + norms = np.linalg.norm(values, axis=1, keepdims=True) + if np.any(norms < 1e-6): + bad_frame = int(np.flatnonzero(norms[:, 0] < 1e-6)[0]) + raise ValueError(f"{label} contains a zero quaternion at frame {bad_frame}") + return values / norms + + +def load_episode_review_data( + dataset: RecordingDataset, + episode: RecordingEpisode, +) -> EpisodeReviewData: + """Load one episode and compute synchronized tracking metrics.""" + + keys = [FRAME_INDEX_KEY, TIMESTAMP_KEY, STATE_KEY, MODE_KEY, ACTION_KEY] + if dataset.has_hand_action: + keys.append(HAND_ACTION_KEY) + if dataset.has_neck_action: + keys.append(NECK_ACTION_KEY) + with h5py.File(episode.data_path, "r") as h5: + arrays = {key: np.asarray(h5[key]) for key in keys} + + frame_index = arrays[FRAME_INDEX_KEY] + timestamps = arrays[TIMESTAMP_KEY].astype(np.float64, copy=False) + state = arrays[STATE_KEY].astype(np.float64, copy=False) + mode = arrays[MODE_KEY].astype(np.int64, copy=False) + action = arrays[ACTION_KEY].astype(np.float64, copy=False) + hand_action = ( + arrays[HAND_ACTION_KEY].astype(np.float64, copy=False) + if dataset.has_hand_action + else None + ) + neck_action = ( + arrays[NECK_ACTION_KEY].astype(np.float64, copy=False) + if dataset.has_neck_action + else None + ) + + expected_frame_index = np.arange(episode.frames, dtype=frame_index.dtype) + if not np.array_equal(frame_index, expected_frame_index): + raise ValueError( + f"Recording episode {episode.episode_index} frame_index must be contiguous from 0" + ) + numeric_arrays = { + TIMESTAMP_KEY: timestamps, + STATE_KEY: state, + ACTION_KEY: action, + } + if hand_action is not None: + numeric_arrays[HAND_ACTION_KEY] = hand_action + if neck_action is not None: + numeric_arrays[NECK_ACTION_KEY] = neck_action + for key, values in numeric_arrays.items(): + if not np.isfinite(values).all(): + raise ValueError( + f"Recording episode {episode.episode_index} dataset {key!r} contains NaN or Inf" + ) + if episode.frames > 1 and np.any(np.diff(timestamps) <= 0.0): + raise ValueError( + f"Recording episode {episode.episode_index} timestamps must be strictly increasing" + ) + invalid_modes = sorted(set(int(value) for value in np.unique(mode)) - set(dataset.mode_names)) + if invalid_modes: + raise ValueError( + f"Recording episode {episode.episode_index} contains unknown mode codes {invalid_modes}" + ) + + actual_joint_pos = state[:, :NUM_JOINTS] + reference_joint_pos = action[:, 7:] + joint_error = actual_joint_pos - reference_joint_pos + group_error = { + name: np.sqrt(np.mean(np.square(joint_error[:, indices]), axis=1)) + for name, indices in JOINT_GROUPS + } + + actual_quat = _normalized_quaternions( + state[:, 58:62], + label=f"episode {episode.episode_index} observation base quaternion", + ) + reference_quat = _normalized_quaternions( + action[:, 3:7], + label=f"episode {episode.episode_index} reference root quaternion", + ) + quat_dot = np.abs(np.sum(actual_quat * reference_quat, axis=1)) + root_orientation_error_rad = 2.0 * np.arccos(np.clip(quat_dot, 0.0, 1.0)) + + max_frame, max_joint = np.unravel_index( + int(np.argmax(np.abs(joint_error))), + joint_error.shape, + ) + return EpisodeReviewData( + episode=episode, + frame_index=frame_index, + timestamps=timestamps, + state=state, + mode=mode, + action=action, + hand_action=hand_action, + neck_action=neck_action, + joint_error=joint_error, + group_error=group_error, + root_orientation_error_rad=root_orientation_error_rad, + joint_rmse_rad=float(np.sqrt(np.mean(np.square(joint_error)))), + root_orientation_rmse_rad=float( + np.sqrt(np.mean(np.square(root_orientation_error_rad))) + ), + max_joint_error_rad=float(abs(joint_error[max_frame, max_joint])), + max_joint_error_frame=int(max_frame), + max_joint_error_name=dataset.joint_names[int(max_joint)], + ) + + +def aligned_qpos_pair(data: EpisodeReviewData, frame: int) -> tuple[np.ndarray, np.ndarray]: + """Return actual/reference qpos with actual root position aligned to reference.""" + + if frame < 0 or frame >= data.episode.frames: + raise IndexError(f"Frame {frame} outside episode range [0, {data.episode.frames - 1}]") + reference_qpos = data.action[frame].copy() + reference_qpos[3:7] = _normalized_quaternions( + reference_qpos[None, 3:7], + label="reference root quaternion", + )[0] + actual_qpos = reference_qpos.copy() + actual_qpos[3:7] = _normalized_quaternions( + data.state[frame : frame + 1, 58:62], + label="observation base quaternion", + )[0] + actual_qpos[7:] = data.state[frame, :NUM_JOINTS] + return actual_qpos, reference_qpos + + +class RecordingVideoReader: + def __init__(self, dataset: RecordingDataset, episode: RecordingEpisode) -> None: + try: + import cv2 + except ImportError as exc: + raise RuntimeError( + "Recording review requires OpenCV; install with pip install -e '.[review]'" + ) from exc + self._cv2 = cv2 + self._episode = episode + self._expected_shape = dataset.image_shape + self._capture = cv2.VideoCapture(str(episode.video_path)) + if not self._capture.isOpened(): + raise RuntimeError(f"Cannot open recording video: {episode.video_path}") + reported_frames = int(round(self._capture.get(cv2.CAP_PROP_FRAME_COUNT))) + reported_fps = float(self._capture.get(cv2.CAP_PROP_FPS)) + if reported_frames != episode.frames: + self.close() + raise ValueError( + f"Recording episode {episode.episode_index} MP4 frame count {reported_frames} " + f"!= manifest/HDF5 frame count {episode.frames}" + ) + if abs(reported_fps - dataset.fps) > 0.1: + self.close() + raise ValueError( + f"Recording episode {episode.episode_index} MP4 fps {reported_fps:g} " + f"!= schema fps {dataset.fps}" + ) + self._next_frame = 0 + + def read_frame(self, frame: int) -> np.ndarray: + if frame != self._next_frame: + self._capture.set(self._cv2.CAP_PROP_POS_FRAMES, frame) + ok, bgr = self._capture.read() + if not ok or bgr is None: + raise RuntimeError( + f"Failed to decode episode {self._episode.episode_index} video frame {frame}" + ) + self._next_frame = frame + 1 + rgb = self._cv2.cvtColor(bgr, self._cv2.COLOR_BGR2RGB) + if tuple(rgb.shape) != self._expected_shape: + raise ValueError( + f"Episode {self._episode.episode_index} video frame shape {rgb.shape} " + f"!= schema shape {self._expected_shape}" + ) + return rgb + + def close(self) -> None: + if getattr(self, "_capture", None) is not None: + self._capture.release() + + +class _PrefixedSceneApi: + """Prefix Viser node names so two MuJoCo scenes can share one server.""" + + def __init__(self, scene: Any, prefix: str) -> None: + self._scene = scene + self._prefix = prefix + + def __getattr__(self, name: str) -> Any: + attribute = getattr(self._scene, name) + if callable(attribute) and name.startswith("add_"): + return lambda path, *args, **kwargs: attribute( + self._prefix + str(path), + *args, + **kwargs, + ) + return attribute + + +class _PrefixedViserServer: + def __init__(self, server: Any, prefix: str) -> None: + self._server = server + self.scene = _PrefixedSceneApi(server.scene, prefix) + + def __getattr__(self, name: str) -> Any: + return getattr(self._server, name) + + +class RobotOverlayScene: + """Show observed G1 geometry with a translucent green reference overlay.""" + + def __init__(self, server: Any, xml_path: Path) -> None: + try: + import mujoco + from mjviser import ViserMujocoScene + except ImportError as exc: + raise RuntimeError( + "Recording review requires mjviser; install with pip install -e '.[review]'" + ) from exc + + self._mujoco = mujoco + self._actual_model = mujoco.MjModel.from_xml_path(str(xml_path)) + self._reference_model = mujoco.MjModel.from_xml_path(str(xml_path)) + for model in (self._actual_model, self._reference_model): + if model.nq != FULL_QPOS_DIM: + raise ValueError( + f"Recording reviewer robot XML nq={model.nq} != action dim " + f"{FULL_QPOS_DIM}: {xml_path}" + ) + + reference_color = np.array([0.1, 0.95, 0.2], dtype=np.float32) + self._reference_model.geom_rgba[:, :3] = reference_color + visible_geoms = self._reference_model.geom_rgba[:, 3] > 0.0 + self._reference_model.geom_rgba[visible_geoms, 3] = 0.38 + world_geoms = self._reference_model.geom_bodyid == 0 + self._reference_model.geom_rgba[world_geoms, 3] = 0.0 + if self._reference_model.nmat > 0: + self._reference_model.mat_rgba[:, :3] = reference_color + visible_materials = self._reference_model.mat_rgba[:, 3] > 0.0 + self._reference_model.mat_rgba[visible_materials, 3] = 0.38 + world_materials = np.unique(self._reference_model.geom_matid[world_geoms]) + world_materials = world_materials[world_materials >= 0] + self._reference_model.mat_rgba[world_materials, 3] = 0.0 + + self._actual_root = server.scene.add_frame("/actual", show_axes=False) + self._reference_root = server.scene.add_frame("/reference", show_axes=False) + self._actual_scene = ViserMujocoScene( + _PrefixedViserServer(server, "/actual"), + self._actual_model, + num_envs=1, + ) + self._reference_scene = ViserMujocoScene( + _PrefixedViserServer(server, "/reference"), + self._reference_model, + num_envs=1, + ) + self._actual_data = mujoco.MjData(self._actual_model) + self._reference_data = mujoco.MjData(self._reference_model) + + def update( + self, + actual_qpos: np.ndarray, + reference_qpos: np.ndarray, + *, + show_reference: bool, + ) -> None: + self._actual_data.qpos[:] = actual_qpos + self._reference_data.qpos[:] = reference_qpos + self._mujoco.mj_forward(self._actual_model, self._actual_data) + self._mujoco.mj_forward(self._reference_model, self._reference_data) + self._actual_scene.update_from_mjdata(self._actual_data) + self._reference_root.visible = show_reference + if show_reference: + self._reference_scene.update_from_mjdata(self._reference_data) + + def close(self) -> None: + self._actual_root.remove() + self._reference_root.remove() + + +class RecordingReviewerApp: + def __init__( + self, + *, + dataset: RecordingDataset, + xml_path: Path, + port: int, + initial_episode: int, + ) -> None: + try: + import viser + except ImportError as exc: + raise RuntimeError( + "Recording review requires Viser; install with pip install -e '.[review]'" + ) from exc + + self._dataset = dataset + self._episode_position = initial_episode + self._data = load_episode_review_data(dataset, dataset.episodes[initial_episode]) + self._video = RecordingVideoReader(dataset, self._data.episode) + self._server = viser.ViserServer(port=port, label="Recording Reviewer") + self._server.gui.configure_theme( + control_layout="fixed", + control_width="large", + dark_mode=True, + show_share_button=False, + brand_color=(34, 197, 94), + ) + self._server.scene.world_axes.visible = False + self._server.initial_camera.position = (2.4, -2.4, 1.8) + self._server.initial_camera.look_at = (0.0, 0.0, 0.8) + self._server.initial_camera.up = (0.0, 0.0, 1.0) + self._server.initial_camera.fov = 45.0 + try: + self._robot_scene = RobotOverlayScene(self._server, xml_path) + except Exception: + self._video.close() + self._server.stop() + raise + + self._playing = False + self._speed = 1.0 + self._frame_accumulator = 0.0 + self._current_frame = -1 + self._show_reference = True + self._pending_actions: list[str] = [] + self._pending_episode: int | None = None + self._pending_scrub: int | None = None + self._pending_joint: str | None = None + self._pending_hand: str | None = None + + self._joint_chart: Any | None = None + self._group_chart: Any | None = None + self._mode_chart: Any | None = None + self._hand_chart: Any | None = None + self._neck_chart: Any | None = None + self._setup_gui(self._video.read_frame(0)) + self._set_frame(0, force=True) + + def _setup_gui(self, first_camera_frame: np.ndarray) -> None: + gui = self._server.gui + episode_labels = [episode.label(self._dataset.fps) for episode in self._dataset.episodes] + + with gui.add_folder("Camera", order=0): + self._camera_image = gui.add_image( + first_camera_frame, + label="D435i RGB", + format="jpeg", + jpeg_quality=82, + ) + gui.add_markdown( + "The main view is interactive 3D: the recorded robot state uses its normal " + "appearance and the motion-tracker reference is translucent green." + ) + self._current_html = gui.add_html("") + + with gui.add_folder("Episode", order=1): + self._episode_dropdown = gui.add_dropdown( + "Episode", + options=episode_labels, + initial_value=episode_labels[self._episode_position], + ) + self._summary_html = gui.add_html("") + + @self._episode_dropdown.on_update + def _(_) -> None: + selected = episode_labels.index(self._episode_dropdown.value) + if selected != self._episode_position: + self._pending_episode = selected + + with gui.add_folder("Playback", order=2): + self._play_button = gui.add_button("Play", color="green") + self._frame_slider = gui.add_slider( + "Frame", + min=0, + max=max(0, self._data.episode.frames - 1), + step=1, + initial_value=0, + ) + self._speed_group = gui.add_button_group( + "Speed", + options=["0.25x", "0.5x", "1x", "2x"], + ) + self._speed_group.value = "1x" + self._restart_button = gui.add_button("Restart") + self._prev_button = gui.add_button("Previous episode") + self._next_button = gui.add_button("Next episode") + self._reference_checkbox = gui.add_checkbox( + "Show green reference", + initial_value=True, + ) + @self._play_button.on_click + def _(_) -> None: + self._pending_actions.append("toggle_play") + + @self._frame_slider.on_update + def _(_) -> None: + requested = int(self._frame_slider.value) + if requested != self._current_frame: + self._pending_scrub = requested + + @self._speed_group.on_click + def _(event) -> None: + self._speed = { + "0.25x": 0.25, + "0.5x": 0.5, + "1x": 1.0, + "2x": 2.0, + }.get(str(event.target.value), 1.0) + + @self._restart_button.on_click + def _(_) -> None: + self._pending_actions.append("restart") + + @self._prev_button.on_click + def _(_) -> None: + self._pending_actions.append("previous") + + @self._next_button.on_click + def _(_) -> None: + self._pending_actions.append("next") + + @self._reference_checkbox.on_update + def _(_) -> None: + self._show_reference = bool(self._reference_checkbox.value) + self._pending_actions.append("refresh") + + self._tracking_folder = gui.add_folder("Tracking", order=3) + with self._tracking_folder: + self._joint_dropdown = gui.add_dropdown( + "Joint", + options=list(self._dataset.joint_names), + initial_value=self._dataset.joint_names[0], + ) + gui.add_markdown( + "The observed robot uses the reference root position because the recording " + "does not contain measured root XYZ. Joint and root-orientation comparisons remain valid." + ) + + @self._joint_dropdown.on_update + def _(_) -> None: + self._pending_joint = str(self._joint_dropdown.value) + + self._signals_folder = gui.add_folder("Recorded signals", order=4, expand_by_default=False) + with self._signals_folder: + gui.add_markdown("Mode: `0 standing`, `1 mocap`, `2 arms`, `3 pause`.") + if self._dataset.has_hand_action: + self._hand_dropdown = gui.add_dropdown( + "Hand channel", + options=list(self._dataset.hand_names), + initial_value=self._dataset.hand_names[0], + ) + + @self._hand_dropdown.on_update + def _(_) -> None: + self._pending_hand = str(self._hand_dropdown.value) + else: + self._hand_dropdown = None + gui.add_markdown("This dataset does not contain `action.hand`.") + if not self._dataset.has_neck_action: + gui.add_markdown("This dataset does not contain `action.neck`.") + + self._refresh_summary() + self._refresh_charts() + + @staticmethod + def _chart_axes(y_label: str) -> tuple[dict[str, Any], dict[str, Any]]: + return ( + {"label": "time (s)", "stroke": "#9ca3af"}, + {"label": y_label, "stroke": "#9ca3af"}, + ) + + def _add_chart( + self, + folder: Any, + *, + data: tuple[np.ndarray, ...], + series: tuple[dict[str, Any], ...], + title: str, + y_label: str, + order: float, + ) -> Any: + with folder: + return self._server.gui.add_uplot( + data=data, + series=series, + title=title, + axes=self._chart_axes(y_label), + legend={"show": True, "live": True}, + cursor={"show": True, "x": True, "y": True}, + height=220, + order=order, + ) + + def _refresh_charts(self) -> None: + for handle in ( + self._joint_chart, + self._group_chart, + self._mode_chart, + self._hand_chart, + self._neck_chart, + ): + if handle is not None: + handle.remove() + + timestamps = self._data.timestamps.astype(np.float64, copy=False) + selected_joint = str(self._joint_dropdown.value) + joint_index = self._dataset.joint_names.index(selected_joint) + actual = self._data.state[:, joint_index] + reference = self._data.action[:, 7 + joint_index] + error = self._data.joint_error[:, joint_index] + self._joint_chart = self._add_chart( + self._tracking_folder, + data=(timestamps, actual, reference, error), + series=( + {"label": "time"}, + {"label": "actual", "stroke": "#3b82f6", "width": 2.0}, + {"label": "reference", "stroke": "#22c55e", "width": 2.0}, + {"label": "error", "stroke": "#ef4444", "width": 1.5}, + ), + title=f"Joint: {selected_joint}", + y_label="rad", + order=1, + ) + group_values = tuple(self._data.group_error[name] for name, _ in JOINT_GROUPS) + group_series: tuple[dict[str, Any], ...] = ( + {"label": "time"}, + *tuple( + {"label": name, "stroke": color, "width": 1.6} + for (name, _), color in zip(JOINT_GROUPS, GROUP_COLORS, strict=True) + ), + ) + self._group_chart = self._add_chart( + self._tracking_folder, + data=(timestamps, *group_values), + series=group_series, + title="Instantaneous group joint RMSE", + y_label="rad", + order=2, + ) + self._mode_chart = self._add_chart( + self._signals_folder, + data=(timestamps, self._data.mode.astype(np.float64)), + series=( + {"label": "time"}, + {"label": "mode", "stroke": "#f59e0b", "width": 2.0}, + ), + title="Mode timeline", + y_label="mode code", + order=1, + ) + + if self._data.hand_action is not None and self._hand_dropdown is not None: + selected_hand = str(self._hand_dropdown.value) + hand_index = self._dataset.hand_names.index(selected_hand) + self._hand_chart = self._add_chart( + self._signals_folder, + data=(timestamps, self._data.hand_action[:, hand_index]), + series=( + {"label": "time"}, + {"label": selected_hand, "stroke": "#8b5cf6", "width": 2.0}, + ), + title="LinkerHand target", + y_label="SDK pose", + order=2, + ) + else: + self._hand_chart = None + + if self._data.neck_action is not None: + self._neck_chart = self._add_chart( + self._signals_folder, + data=( + timestamps, + self._data.neck_action[:, 0], + self._data.neck_action[:, 1], + ), + series=( + {"label": "time"}, + {"label": "yaw", "stroke": "#06b6d4", "width": 2.0}, + {"label": "pitch", "stroke": "#ec4899", "width": 2.0}, + ), + title="OpenNeck target", + y_label="degrees", + order=3, + ) + else: + self._neck_chart = None + + def _mode_name(self, frame: int) -> str: + return self._dataset.mode_names[int(self._data.mode[frame])] + + def _set_frame(self, frame: int, *, force: bool = False) -> None: + frame = max(0, min(int(frame), self._data.episode.frames - 1)) + if not force and frame == self._current_frame: + return + self._camera_image.image = self._video.read_frame(frame) + actual_qpos, reference_qpos = aligned_qpos_pair(self._data, frame) + self._robot_scene.update( + actual_qpos, + reference_qpos, + show_reference=self._show_reference, + ) + self._current_frame = frame + self._frame_slider.value = frame + instant_rmse = float(np.sqrt(np.mean(np.square(self._data.joint_error[frame])))) + selected_joint = str(self._joint_dropdown.value) + joint_index = self._dataset.joint_names.index(selected_joint) + selected_error = float(self._data.joint_error[frame, joint_index]) + self._current_html.content = ( + "
" + f"Frame: {frame}/{self._data.episode.frames - 1}
" + f"Time: {self._data.timestamps[frame]:.2f}s
" + f"Mode: {html.escape(self._mode_name(frame))}
" + f"Instant joint RMSE: {instant_rmse:.3f} rad
" + f"{html.escape(selected_joint)} error: {selected_error:+.3f} rad" + "
" + ) + + def _refresh_summary(self) -> None: + episode = self._data.episode + mode_counts = [ + f"{html.escape(name)}={int(np.count_nonzero(self._data.mode == code))}" + for code, name in sorted(self._dataset.mode_names.items()) + if np.any(self._data.mode == code) + ] + self._summary_html.content = ( + "
" + f"Task: {html.escape(episode.task)}
" + f"Frames: {episode.frames} @ {self._dataset.fps} FPS " + f"({episode.frames / self._dataset.fps:.2f}s)
" + f"Modes: {', '.join(mode_counts)}
" + f"Joint RMSE: {self._data.joint_rmse_rad:.3f} rad " + f"({np.degrees(self._data.joint_rmse_rad):.2f}°)
" + f"Root orientation RMSE: " + f"{np.degrees(self._data.root_orientation_rmse_rad):.2f}°
" + f"Max joint error: {self._data.max_joint_error_rad:.3f} rad " + f"at frame {self._data.max_joint_error_frame} " + f"({html.escape(self._data.max_joint_error_name)})" + "
" + ) + + def _set_playing(self, playing: bool) -> None: + self._playing = playing + self._frame_accumulator = 0.0 + self._play_button.label = "Pause" if playing else "Play" + self._play_button.color = "red" if playing else "green" + + def _load_episode(self, position: int) -> None: + position = max(0, min(position, len(self._dataset.episodes) - 1)) + if position == self._episode_position: + return + self._set_playing(False) + new_data = load_episode_review_data( + self._dataset, + self._dataset.episodes[position], + ) + new_video = RecordingVideoReader(self._dataset, new_data.episode) + old_video = self._video + self._data = new_data + self._video = new_video + self._episode_position = position + old_video.close() + self._current_frame = -1 + self._frame_slider.max = max(0, new_data.episode.frames - 1) + self._episode_dropdown.value = new_data.episode.label(self._dataset.fps) + self._refresh_summary() + self._refresh_charts() + self._set_frame(0, force=True) + + def _process_pending(self) -> None: + if self._pending_episode is not None: + position = self._pending_episode + self._pending_episode = None + self._load_episode(position) + + if self._pending_joint is not None: + self._pending_joint = None + self._refresh_charts() + self._set_frame(self._current_frame, force=True) + if self._pending_hand is not None: + self._pending_hand = None + self._refresh_charts() + + if self._pending_scrub is not None: + frame = self._pending_scrub + self._pending_scrub = None + self._set_playing(False) + self._set_frame(frame) + + while self._pending_actions: + action = self._pending_actions.pop(0) + if action == "toggle_play": + if self._current_frame >= self._data.episode.frames - 1: + self._set_frame(0) + self._set_playing(not self._playing) + elif action == "restart": + self._set_playing(False) + self._set_frame(0, force=True) + elif action == "previous": + self._load_episode(self._episode_position - 1) + elif action == "next": + self._load_episode(self._episode_position + 1) + elif action == "refresh": + self._set_frame(self._current_frame, force=True) + + def run(self) -> None: + print(f"\nRecording reviewer ready at http://localhost:{self._server.get_port()}") + print(f"Dataset: {self._dataset.root}") + print("Press Ctrl+C to exit.\n") + previous_time = time.monotonic() + try: + while True: + now = time.monotonic() + elapsed = now - previous_time + previous_time = now + self._process_pending() + if self._playing: + self._frame_accumulator += elapsed * self._dataset.fps * self._speed + advance = int(self._frame_accumulator) + if advance > 0: + self._frame_accumulator -= advance + next_frame = self._current_frame + advance + if next_frame >= self._data.episode.frames - 1: + self._set_frame(self._data.episode.frames - 1) + self._set_playing(False) + else: + self._set_frame(next_frame) + time.sleep(1.0 / 60.0) + except KeyboardInterrupt: + print("\nShutting down...") + finally: + self.close() + + def close(self) -> None: + self._video.close() + self._robot_scene.close() + self._server.stop() + + +def main() -> None: + parser = argparse.ArgumentParser( + description="Read-only synchronized reviewer for Teleopit sim2real recordings" + ) + parser.add_argument( + "--recording", + type=str, + default=str(DEFAULT_RECORDING_ROOT), + help="Recording dataset root containing schema.json and episodes.jsonl", + ) + parser.add_argument("--xml", type=str, default=None, help="Canonical G1 MuJoCo XML path") + parser.add_argument("--episode", type=int, default=0, help="Initial episode index") + parser.add_argument("--port", type=int, default=8013, help="Viser server port") + args = parser.parse_args() + + recording_root = Path(args.recording) + if not recording_root.is_absolute(): + recording_root = (PROJECT_ROOT / recording_root).resolve() + try: + dataset = load_recording_dataset(recording_root) + except ValueError as exc: + print(f"ERROR: {exc}", file=sys.stderr) + raise SystemExit(1) from exc + + episode_positions = { + episode.episode_index: position for position, episode in enumerate(dataset.episodes) + } + if args.episode not in episode_positions: + valid = [episode.episode_index for episode in dataset.episodes] + print(f"ERROR: episode {args.episode} not found; available indices: {valid}", file=sys.stderr) + raise SystemExit(1) + + xml_path = Path(args.xml).expanduser() if args.xml else DEFAULT_XML + if not xml_path.is_absolute(): + xml_path = (PROJECT_ROOT / xml_path).resolve() + if not xml_path.is_file(): + print( + "ERROR: " + missing_gmr_assets_message(xml_path, label="Robot XML"), + file=sys.stderr, + ) + raise SystemExit(1) + + try: + app = RecordingReviewerApp( + dataset=dataset, + xml_path=xml_path, + port=args.port, + initial_episode=episode_positions[args.episode], + ) + except (ImportError, RuntimeError, ValueError) as exc: + print(f"ERROR: {exc}", file=sys.stderr) + raise SystemExit(1) from exc + app.run() + + +if __name__ == "__main__": + main() diff --git a/tests/test_recording_viewer.py b/tests/test_recording_viewer.py new file mode 100644 index 00000000..7f159b94 --- /dev/null +++ b/tests/test_recording_viewer.py @@ -0,0 +1,156 @@ +from __future__ import annotations + +import json +from pathlib import Path +import sys + +import h5py +import numpy as np +import pytest + +_PROJECT_ROOT = str(Path(__file__).resolve().parents[1]) +if _PROJECT_ROOT not in sys.path: + sys.path.insert(0, _PROJECT_ROOT) + +from scripts.view.view_recording import ( + aligned_qpos_pair, + load_episode_review_data, + load_recording_dataset, +) +from teleopit.recording.hdf5 import ( + ACTION_KEY, + FRAME_INDEX_KEY, + HAND_ACTION_KEY, + MODE_KEY, + NECK_ACTION_KEY, + RecordingSchema, + STATE_KEY, + TIMESTAMP_KEY, + hdf5_schema, +) + + +def _write_recording( + root: Path, + *, + frames: int = 4, + manifest_frames: int | None = None, + hand_type: str = "none", + neck_type: str = "none", +) -> None: + root.mkdir(parents=True, exist_ok=True) + schema = RecordingSchema( + fps=30, + robot_type="unitree_g1_29dof", + hand_type=hand_type, + neck_type=neck_type, + image_key="observation.images.d435i_rgb", + image_shape=(4, 6, 3), + ) + (root / "schema.json").write_text( + json.dumps(hdf5_schema(schema)), + encoding="utf-8", + ) + data_path = root / "data" / "episode_000000.h5" + data_path.parent.mkdir() + state = np.zeros((frames, 68), dtype=np.float32) + state[:, 58] = 1.0 + action = np.zeros((frames, 36), dtype=np.float32) + action[:, :3] = np.array([1.0, 2.0, 0.8], dtype=np.float32) + action[:, 3] = 1.0 + action[:, 7:] = 0.1 + with h5py.File(data_path, "w") as h5: + h5.create_dataset(FRAME_INDEX_KEY, data=np.arange(frames, dtype=np.int64)) + h5.create_dataset(TIMESTAMP_KEY, data=np.arange(frames, dtype=np.float64) / 30.0) + h5.create_dataset(STATE_KEY, data=state) + h5.create_dataset(MODE_KEY, data=np.ones(frames, dtype=np.int8)) + h5.create_dataset(ACTION_KEY, data=action) + if hand_type != "none": + h5.create_dataset(HAND_ACTION_KEY, data=np.zeros((frames, 12), dtype=np.float32)) + if neck_type != "none": + h5.create_dataset(NECK_ACTION_KEY, data=np.zeros((frames, 2), dtype=np.float32)) + + video_path = root / "videos" / "d435i_rgb" / "episode_000000.mp4" + video_path.parent.mkdir(parents=True) + video_path.touch() + manifest = { + "episode_index": 0, + "frames": frames if manifest_frames is None else manifest_frames, + "task": "test task", + "data": "data/episode_000000.h5", + "videos": { + "observation.images.d435i_rgb": "videos/d435i_rgb/episode_000000.mp4" + }, + } + (root / "episodes.jsonl").write_text(json.dumps(manifest) + "\n", encoding="utf-8") + + +def test_recording_viewer_loads_schema_episode_and_tracking_metrics(tmp_path: Path) -> None: + _write_recording( + tmp_path, + hand_type="linkerhand_o6", + neck_type="openneck", + ) + + dataset = load_recording_dataset(tmp_path) + data = load_episode_review_data(dataset, dataset.episodes[0]) + + assert dataset.fps == 30 + assert dataset.image_shape == (4, 6, 3) + assert dataset.has_hand_action is True + assert dataset.has_neck_action is True + assert data.hand_action is not None + assert data.neck_action is not None + assert data.joint_rmse_rad == pytest.approx(0.1) + assert data.root_orientation_rmse_rad == pytest.approx(0.0) + assert data.max_joint_error_rad == pytest.approx(0.1) + assert set(data.group_error) == { + "left leg", + "right leg", + "waist", + "left arm", + "right arm", + } + assert all(np.allclose(values, 0.1) for values in data.group_error.values()) + + +def test_recording_viewer_aligns_observed_root_position_to_reference(tmp_path: Path) -> None: + _write_recording(tmp_path) + dataset = load_recording_dataset(tmp_path) + data = load_episode_review_data(dataset, dataset.episodes[0]) + + actual_qpos, reference_qpos = aligned_qpos_pair(data, 0) + + np.testing.assert_allclose(actual_qpos[:3], [1.0, 2.0, 0.8]) + np.testing.assert_allclose(reference_qpos[:3], [1.0, 2.0, 0.8]) + np.testing.assert_allclose(actual_qpos[7:], 0.0) + np.testing.assert_allclose(reference_qpos[7:], 0.1) + + +def test_recording_viewer_rejects_manifest_hdf5_frame_mismatch(tmp_path: Path) -> None: + _write_recording(tmp_path, frames=4, manifest_frames=5) + + with pytest.raises(ValueError, match="shape .* != manifest/schema shape"): + load_recording_dataset(tmp_path) + + +def test_recording_viewer_rejects_non_finite_episode_data(tmp_path: Path) -> None: + _write_recording(tmp_path) + data_path = tmp_path / "data" / "episode_000000.h5" + with h5py.File(data_path, "r+") as h5: + h5[STATE_KEY][2, 0] = np.nan + + dataset = load_recording_dataset(tmp_path) + with pytest.raises(ValueError, match="contains NaN or Inf"): + load_episode_review_data(dataset, dataset.episodes[0]) + + +def test_recording_viewer_rejects_hdf5_dtype_mismatch(tmp_path: Path) -> None: + _write_recording(tmp_path) + data_path = tmp_path / "data" / "episode_000000.h5" + with h5py.File(data_path, "r+") as h5: + del h5[MODE_KEY] + h5.create_dataset(MODE_KEY, data=np.full(4, 1.9, dtype=np.float32)) + + with pytest.raises(ValueError, match="dtype float32 != schema dtype int8"): + load_recording_dataset(tmp_path) From 2805cd6c837f8dac6e4af78153f3c5c451f99831 Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Thu, 16 Jul 2026 23:24:52 +0800 Subject: [PATCH 22/59] feat: add high-level policy sim2real runtime --- AGENTS.md | 28 +- README.md | 38 + docs/docs/configuration/config-reference.md | 39 + docs/docs/configuration/overview.md | 1 + docs/docs/getting-started/quick-start.md | 1 + docs/docs/reference/architecture.md | 29 +- .../tutorials/high-level-policy-sim2real.md | 200 +++++ docs/docs/tutorials/pico-sim2real.md | 5 +- .../current/configuration/config-reference.md | 37 + .../current/configuration/overview.md | 1 + .../current/getting-started/quick-start.md | 1 + .../current/reference/architecture.md | 26 +- .../tutorials/high-level-policy-sim2real.md | 183 ++++ .../current/tutorials/pico-sim2real.md | 4 +- docs/sidebars.ts | 1 + pyproject.toml | 4 + scripts/run/run_high_level_policy_sim2real.py | 59 ++ .../configs/high_level_policy_sim2real.yaml | 51 ++ teleopit/high_level_policy/__init__.py | 21 + teleopit/high_level_policy/client.py | 276 ++++++ teleopit/high_level_policy/config.py | 223 +++++ .../high_level_policy/hand_calibration.json | 5 + .../high_level_policy/hand_calibration.py | 39 + teleopit/high_level_policy/protocol.py | 105 +++ teleopit/high_level_policy/scheduler.py | 502 +++++++++++ teleopit/runtime/console.py | 12 + teleopit/sim2real/__init__.py | 5 + teleopit/sim2real/mp/__init__.py | 2 + .../sim2real/mp/high_level_policy_runtime.py | 473 ++++++++++ .../sim2real/mp/high_level_policy_worker.py | 284 ++++++ teleopit/sim2real/mp/ipc.py | 9 + teleopit/sim2real/mp/messages.py | 53 ++ teleopit/sim2real/mp/runtime.py | 578 +++++++++++- tests/test_high_level_policy.py | 821 ++++++++++++++++++ 34 files changed, 4093 insertions(+), 23 deletions(-) create mode 100644 docs/docs/tutorials/high-level-policy-sim2real.md create mode 100644 docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md create mode 100644 scripts/run/run_high_level_policy_sim2real.py create mode 100644 teleopit/configs/high_level_policy_sim2real.yaml create mode 100644 teleopit/high_level_policy/__init__.py create mode 100644 teleopit/high_level_policy/client.py create mode 100644 teleopit/high_level_policy/config.py create mode 100644 teleopit/high_level_policy/hand_calibration.json create mode 100644 teleopit/high_level_policy/hand_calibration.py create mode 100644 teleopit/high_level_policy/protocol.py create mode 100644 teleopit/high_level_policy/scheduler.py create mode 100644 teleopit/sim2real/mp/high_level_policy_runtime.py create mode 100644 teleopit/sim2real/mp/high_level_policy_worker.py create mode 100644 tests/test_high_level_policy.py diff --git a/AGENTS.md b/AGENTS.md index 6ac1b4f5..70abf681 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -12,9 +12,14 @@ Config: Hydra/OmegaConf YAML files in `teleopit/configs/` ``` InputProvider (BVH file / Pico4 VR) → Retargeter (GMR) → ObservationBuilder (167D) → Controller (dual-input TemporalCNN ONNX) → Robot (MuJoCo + PD / Unitree SDK) + +Host policy service → onboard policy client/scheduler → 36D reference → same ObservationBuilder/Controller → Unitree SDK ``` -Module-internal isolation: all modules run in-process and communicate via `InProcessBus` (zero-copy). Core interfaces are defined as `typing.Protocol` in `teleopit/interfaces.py`. +Offline core modules communicate through `InProcessBus` (zero-copy). Sim2real +workers use localhost ZMQ plus shared-memory video rings, while the external +host-policy boundary uses strict msgpack/ZeroMQ without pickle. Core interfaces +are defined as `typing.Protocol` in `teleopit/interfaces.py`. ## Supported Surface @@ -22,6 +27,7 @@ Module-internal isolation: all modules run in-process and communicate via `InPro - Inference observation: `velcmd_history` (167D, dual-input ONNX with `obs` + `obs_history`) - TemporalCNN actor/critic with scaled dims (2048,1024,512,256,128) - Realtime inference uses a retargeted-reference timeline before observation build; `reference_steps=[0]` is the default production path +- Host high-level-policy deployment uses an independent script/environment boundary; its network structure is defined by the current client/server code and protocol tests, and LeRobot is not a Teleopit dependency ## Directory Structure @@ -58,10 +64,12 @@ teleopit/ # Core inference package │ ├── mp/ # Process-isolated sim2real runtime and IPC │ ├── hands/ # Optional LinkerHand driver/mapper plugins │ └── neck/ # Optional OpenNeck active-vision gimbal control +├── high_level_policy/ # Host-policy protocol, strict client, frame transform, and action scheduler └── recording/ # Pico motion NPZ recording helpers scripts/ ├── run/run_sim.py # Offline sim2sim pipeline ├── run/run_sim2real.py # G1 sim2real control; supports offline BVH playback and Pico4 +├── run/run_high_level_policy_sim2real.py # Independent host-policy deployment runtime ├── run/record_pico_motion.py # Interactive Pico recording → G1 motion NPZ clips ├── render/render_sim.py # Render single BVH → 3 MuJoCo videos (mocap input, retarget, sim2sim) ├── view/view_recording.py # Read-only synchronized sim2real recording reviewer @@ -143,7 +151,7 @@ target_dof_pos = clip(action, -10, 10) × action_scale + default_dof_pos - Pico sim2sim supports a keyboard-driven top-level mode state machine: `STANDING → MOCAP ↔ ARMS`, `X` returns to `STANDING` - Default Pico sim2sim keyboard mappings are `Y` → `MOCAP`, `A` → pause/resume mocap, `B` → toggle `MOCAP`/`ARMS`, `X` → back to `STANDING`, `Q` → quit - Pico4 sim2real pause/resume is handled as a mocap-session control event (`toggle_pause`), not as a mode switch to `STANDING` -- Default Pico pause button is `A`; resume resets policy/reference state and yaw/XY root-offset alignment while the process-isolated realtime reference worker continues its live input timeline +- Default Pico/controller pause button is `A`; Unitree remote `B` also pauses/resumes Pico sim2real. Resume resets policy/reference state and yaw/XY root-offset alignment while the process-isolated realtime reference worker continues its live input timeline - Pico4 sim2real arms the process-isolated reference worker only when entering `MOCAP`; `STANDING` and `DAMPING` disarm it so cold startup frames do not warm-start GMR before mocap entry - Pico4 sim2sim/sim2real support `ARMS` mode toggled from `MOCAP` with Pico/controller `B`; retargeting continues, while the control loop sends the motion tracker a composed reference with stand-pose body/legs/waist and live retargeted arms - `ARMS` entering/exiting/resume resets policy/reference alignment and uses Kp ramp; offline BVH sim2real does not use `ARMS`, and Unitree remote `B` remains BVH replay @@ -166,6 +174,22 @@ target_dof_pos = clip(action, -10, 10) × action_scale + default_dof_pos - In `vr_hand_pose` mode, missing/inactive hand pose holds the last commanded pose for that side instead of opening the hand - Optional OpenNeck active-vision gimbal control uses `neck.enabled=true` and `neck.driver=openneck`; it requires `input.provider=pico4`, reuses the existing Pico receiver, and must not start a second `PicoBridge` - OpenNeck is integrated as a non-critical sim2real `neck_worker`; failures should not stop the G1 control loop, and no OpenNeck state is added to the 167D policy observation + +### Host High-Level Policy +- `scripts/run/run_high_level_policy_sim2real.py` is independent from the Pico `run_sim2real.py` runtime; it must not start PicoBridge, GMR, or the realtime retarget reference worker +- The host LeRobot/ReplayPolicy service runs in the separate `lerobot-teleopit` repository and environment; Teleopit must not depend on LeRobot, Transformers, or host policy classes +- The current client/server code and protocol tests define the ZeroMQ request/response structure. During active development, Teleopit and `lerobot-teleopit` must update that structure together; no legacy network envelope is supported +- The only shared data file is `hand_calibration.json`, which contains the LinkerHand O6 open/close calibration and must stay identical in both repositories +- The host boundary uses ZeroMQ REQ/REP with msgpack and non-pickle float32 arrays. The onboard network client runs in a non-critical worker and never blocks the 50 Hz robot loop +- Policy observation is RGB JPEG plus `observation.state(68)`; only `state[58:62]` is rotated into the session-local yaw frame +- Canonical action is `float32[T,50]`: local root `xyz(3)` + local root quaternion `wxyz(4)` + G1 joint reference `29` + left/right O6 closure `12` + OpenNeck yaw/pitch degrees `2` +- The body reference `[0:36]` is yaw/XY-delocalized once and sent through the existing motion tracker. It is never sent directly as a G1 motor command and must not pass through the mocap alignment a second time +- High-level-policy formal robot modes are `IDLE`, `STANDING`, `POLICY`, and `DAMPING`. After Unitree remote `Y`, first-chunk handshake remains an internal pending condition while the robot stays in `STANDING`; do not add a `POLICY_STARTING` mode +- Unitree remote controls are `Start -> STANDING`, `Y -> request POLICY`, `B -> pause/resume`, `X -> STANDING/cancel pending`, and `L1+R1 -> DAMPING` +- Policy pause freezes the scheduler/body reference and holds the latest hand/neck command. Leaving `POLICY` opens LinkerHand and centers OpenNeck +- The onboard scheduler rejects whole chunks on shape/finiteness, session/sequence, quaternion, root displacement/height/speed/yaw-rate, G1 joint position/rate, hand closure, OpenNeck degree range, or staleness failures; never pad, trim, or safety-clip invalid host output +- A short cached-reference grace period is allowed; exhaustion, host/network failure, or loss of a required camera/client worker puts `POLICY` into the same resumable pause state used by remote `B`. The runtime never enters `STANDING` automatically; `B` resumes after a fresh valid chunk is available, while `X` remains the manual transition to `STANDING` +- Initial production hardware support requires two LinkerHand O6 hands and OpenNeck because all 50 canonical action dimensions are active - OpenNeck 0.2.0 is the supported runtime; Teleopit sends physical degrees through `move_deg()`, and the direct-drive OpenNeck package converts degrees to servo steps and clips them to its calibrated mechanical limits; the removed normalized API and config fields are unsupported - OpenNeck maps the independent HMD `PicoFrame.head.rotation` relative to the same-frame full-body `Body.Spine3` orientation; it must never use the full-body `Body.Head` skeleton joint for neck control, and HMD pose updates must remain independent of duplicate-body-frame filtering - OpenNeck uses a fixed identity neutral pose and no neck-side EMA; it must not capture the first live frame as a runtime zero pose, so tracking can start while the operator's head is turned diff --git a/README.md b/README.md index f86eadfd..e3fd3380 100644 --- a/README.md +++ b/README.md @@ -128,6 +128,44 @@ timelines. Because recordings do not contain measured root XYZ, the observed pose is anchored to the reference root position; joint and root-orientation comparisons remain valid. +## Host High-Level Policy Deployment + +Teleopit can run a host-served ReplayPolicy or LeRobot ACT policy through a +dedicated onboard runtime. The host policy remains in the independent +`lerobot-teleopit` repository/environment; Teleopit receives canonical 50D +reference chunks over ZeroMQ, validates and interpolates them onboard, and +rate-limits plan switches at 50 Hz before passing the 36D body reference +through the existing motion tracker. The host never sends G1 motor commands. + +Pico and high-level-policy deployment use separate scripts. The policy runtime +does not start PicoBridge, GMR, or the Pico reference worker: + +```bash +python scripts/run/run_high_level_policy_sim2real.py \ + controller.policy_path=track.onnx \ + high_level_policy.endpoint=tcp://192.168.1.10:5555 \ + high_level_policy.task="pick up the object" \ + real_robot.network_interface=eth0 +``` + +Use the Unitree remote: `Start` enters `STANDING`, `Y` requests policy +takeover, `B` pauses/resumes, `X` returns to `STANDING`, and `L1+R1` enters +`DAMPING`. While the first host chunk is being checked, the robot remains in +`STANDING`; there is no separate starting mode. Invalid/stale chunks and +watchdog expiry cannot block the local control loop and instead pause `POLICY` +while holding the last reference. Host/network failure and loss of a required +camera/client worker use the same ordinary pause state as remote `B`; after +recovery, press `B` to resume on a fresh valid chunk. The runtime never enters +`STANDING` automatically, and `X` remains the manual transition. + +The current client/server code and protocol tests define the network message +structure. During active development, Teleopit and `lerobot-teleopit` must be +updated together. Their only shared data file is `hand_calibration.json`, which +contains the LinkerHand O6 open/close calibration. See the +[host-policy deployment tutorial](https://BotRunner64.github.io/Teleopit/tutorials/high-level-policy-sim2real) +for the 68D observation, 50D action layout, safety envelope, host startup, and +operator procedure. + ## OpenNeck Active Vision Pico sim2real can drive the optional OpenNeck two-axis active-vision gimbal from diff --git a/docs/docs/configuration/config-reference.md b/docs/docs/configuration/config-reference.md index 6e909106..0b112ab2 100644 --- a/docs/docs/configuration/config-reference.md +++ b/docs/docs/configuration/config-reference.md @@ -106,6 +106,45 @@ and `all` are simulation-only viewer modes. | `mocap_switch.check_frames` | Consecutive valid frames required before switching to MOCAP | `10` | | `arm_mocap.controlled_joint_indices` | G1 joints driven by live retargeting in Pico `ARMS` mode | `[15..28]` | +### Host High-Level Policy (independent sim2real) + +`high_level_policy_sim2real.yaml` is used only by +`scripts/run/run_high_level_policy_sim2real.py`. It starts camera, network +client, robot-control, LinkerHand O6, and OpenNeck workers. It does not start +PicoBridge, GMR, or a retarget reference worker. The host LeRobot environment +remains separate and must track the current client/server message structure and +protocol tests. The only shared data file is `hand_calibration.json`. + +| Field | Description | Default | +|-------|-------------|---------| +| `camera.source` | Onboard policy camera: `realsense` or integration-only `test-pattern` | `realsense` | +| `camera.width` / `height` / `fps` | Exact policy image contract | `640` / `480` / `30` | +| `camera.device` | Optional RealSense serial | `null` | +| `high_level_policy.endpoint` | Host policy ZeroMQ TCP endpoint | `tcp://127.0.0.1:5555` | +| `high_level_policy.task` | Non-empty task prompt sent on reset and every observation | `demo` | +| `high_level_policy.timeout_s` | Per-request network deadline | `1.0` | +| `high_level_policy.reconnect_backoff_s` | Retry delay while establishing a new session | `1.0` | +| `high_level_policy.replan_steps` | Minimum 30 Hz source-frame interval between requests; must not exceed host horizon | `3` | +| `high_level_policy.jpeg_quality` | JPEG quality for the 640x480 RGB frame | `90` | +| `high_level_policy.max_observation_age_s` | Maximum camera/observation age before a request is skipped | `0.15` | +| `high_level_policy.max_result_age_s` | Maximum local IPC age before a received result is rejected | `0.1` | +| `high_level_policy.entry_timeout_s` | Maximum wait for the first valid chunk while remaining in `STANDING` | `3.0` | +| `high_level_policy.hold_s` | Final validated-reference grace period before the watchdog pauses `POLICY` | `0.1` | +| `high_level_policy.safety.root_height_min_m` / `root_height_max_m` | Accepted absolute root-height range | `0.55` / `1.05` | +| `high_level_policy.safety.max_root_xy_speed_m_s` | Maximum root XY speed across 30 Hz references | `2.5` | +| `high_level_policy.safety.max_root_displacement_m` | Maximum 3D root displacement between reference frames | `0.1` | +| `high_level_policy.safety.max_yaw_rate_rad_s` | Maximum root yaw rate | `2.5` | +| `high_level_policy.safety.max_joint_rate_rad_s` | Maximum per-joint reference rate | `10.0` | +| `high_level_policy.safety.neck_yaw_min_deg` / `neck_yaw_max_deg` | Accepted OpenNeck yaw command range | `-45` / `45` | +| `high_level_policy.safety.neck_pitch_min_deg` / `neck_pitch_max_deg` | Accepted OpenNeck pitch command range | `-40` / `40` | + +G1 reference joint positions are checked against +`real_robot.joint_pos_lower/upper`. The initial runtime requires +`hands.driver=linkerhand_o6`, both hand sides, and `neck.driver=openneck` because +all canonical 50D action fields are active. OpenNeck policy values go directly +to `move_deg(yaw, pitch)` after chunk validation; Pico dead-zone and pitch-gain +mapping are not applied. + ### Real Robot | Field | Description | Default | diff --git a/docs/docs/configuration/overview.md b/docs/docs/configuration/overview.md index 6d967dbf..7a149ca6 100644 --- a/docs/docs/configuration/overview.md +++ b/docs/docs/configuration/overview.md @@ -16,6 +16,7 @@ Runtime assembly is centralized in `teleopit/runtime/`. Scripts, `TeleopPipeline | `teleopit/configs/pico4_sim.yaml` | Pico 4 VR sim2sim | | `teleopit/configs/sim2real.yaml` | BVH sim2real on Unitree G1 | | `teleopit/configs/pico4_sim2real.yaml` | Pico 4 VR sim2real on Unitree G1 | +| `teleopit/configs/high_level_policy_sim2real.yaml` | Independent host-policy sim2real on Unitree G1 | These compose sub-configs: diff --git a/docs/docs/getting-started/quick-start.md b/docs/docs/getting-started/quick-start.md index bede79a1..510a6d26 100644 --- a/docs/docs/getting-started/quick-start.md +++ b/docs/docs/getting-started/quick-start.md @@ -60,4 +60,5 @@ python scripts/run/run_sim.py controller.policy_path=track.onnx 'viewers=[retarg - [Standalone Standing](../tutorials/standalone-standing) - Check G1 bridge, network, and policy standing - [Pico Sim2Real](../tutorials/pico-sim2real) - Deploy Pico teleoperation to Unitree G1 - [BVH Sim2Real](../tutorials/bvh-sim2real) - Replay offline BVH motions on Unitree G1 +- [Host Policy Sim2Real](../tutorials/high-level-policy-sim2real) - Connect an independent LeRobot policy host to the onboard motion tracker - [Training](../tutorials/training) - Train your own policy diff --git a/docs/docs/reference/architecture.md b/docs/docs/reference/architecture.md index 6e384ad9..90d323ec 100644 --- a/docs/docs/reference/architecture.md +++ b/docs/docs/reference/architecture.md @@ -18,6 +18,25 @@ InputProvider (BVH file / Pico4) Offline/online inference is assembled by `teleopit/runtime/` and `teleopit/pipeline.py`. The hardware state machine runs through the process-isolated runtime in `teleopit/sim2real/mp/`. Training is provided by `train_mimic/`. +Host-served imitation policies use a second, independent deployment path: + +```text +lerobot-teleopit host environment + policy server -> strict ZeroMQ/msgpack messages + | +Teleopit onboard environment + RealSense/state -> non-critical client worker -> validated action scheduler + -> existing 50 Hz motion tracker -> G1 joint-angle targets + -> dedicated LinkerHand O6 and OpenNeck workers +``` + +The host and onboard environments share semantic data and one identical +`hand_calibration.json`; they do not import each other's Python packages. The +current client/server code and protocol tests define the network structure, so +both repositories must change together during active development. Pico +teleoperation and host-policy deployment also have separate run scripts and +process assemblies. + ## Code Structure ```text @@ -44,6 +63,7 @@ train_mimic/scripts/data | `teleopit/runtime/` | Config parsing, path normalization, component assembly, CLI validation | | `teleopit/pipeline.py` | Lightweight facade for offline sim | | `teleopit/sim2real/mp/` | Process-isolated sim2real state machine, IPC, and robot-control loop | +| `teleopit/high_level_policy/` | Host-policy protocol, session-local frame transform, validation, and 30-to-50 Hz scheduler | | `teleopit/controllers/observation.py` | ObservationBuilder | | `teleopit/controllers/rl_policy.py` | Accepts dual-input ONNX whose observation dimension matches the runtime builder | | `train_mimic/app.py` | Shared train/play/benchmark assembly | @@ -61,6 +81,9 @@ train_mimic/scripts/data | Training sampling | Default `rewind`; also supports `uniform`; playback uses `start`; benchmark pins exact clips and disables clip-end resampling | | Training `window_steps` | `[0]` | | Data format | Minimal recursive HDF5 shards (`shard_*.h5`) | +| Host-policy observation | JPEG RGB + `observation.state(68)` | +| Host-policy action | `float32[T,50]` canonical reference at 30 Hz | +| Host-policy body control | 36D root/joint reference through the existing 50 Hz motion tracker | ## Constraints @@ -69,10 +92,14 @@ train_mimic/scripts/data - `viewers` is the sole viewer configuration entry - Observation/ONNX dimension mismatch causes immediate startup error - sim2real also requires a dual-input ONNX whose observation dimension matches the runtime builder +- Host-policy message-envelope or schema mismatches are rejected while the robot remains in `STANDING` +- Host action chunks are validated and interpolated onboard; the host cannot bypass the motion tracker or send motor commands +- Waiting for the first host chunk is not a robot mode: the formal takeover mode is only `POLICY` ## Public Surface -**Stable run modes:** offline sim2sim, offline sim2real playback, Pico4 sim2sim, G1 sim2real +**Stable run modes:** offline sim2sim, offline sim2real playback, Pico4 sim2sim, +G1 sim2real, independent host-policy G1 sim2real **Stable training entry points:** `train.py`, `play.py`, `benchmark.py`, `save_onnx.py` diff --git a/docs/docs/tutorials/high-level-policy-sim2real.md b/docs/docs/tutorials/high-level-policy-sim2real.md new file mode 100644 index 00000000..0a463637 --- /dev/null +++ b/docs/docs/tutorials/high-level-policy-sim2real.md @@ -0,0 +1,200 @@ +--- +sidebar_position: 6 +--- + +# Host Policy Deployment on Unitree G1 + +This workflow runs a LeRobot policy service on a host workstation and the +Teleopit motion tracker on the G1 onboard computer. The two repositories use +separate Python environments and communicate only through strict +ZeroMQ/msgpack messages. + +```text +Host workstation (lerobot-teleopit) + ReplayPolicy or ACT -> policy server + | + | float32 state/action + JPEG over TCP + v +G1 onboard computer (Teleopit) + RealSense + G1 state -> client -> validated 30 Hz action chunk + -> 50 Hz interpolation -> motion tracker -> G1 joint-angle targets + -> LinkerHand O6 / OpenNeck +``` + +This is a separate runtime from Pico teleoperation. Do not start PicoBridge, +GMR, or `run_sim2real.py` for this workflow. Switching between Pico control and +host-policy control means stopping one runtime and starting the other. + +## 1. Network Messages and Hand Calibration + +The current client/server code and protocol tests define the request and +response structure. During active development, changes to that structure must +be made in Teleopit and `lerobot-teleopit` together; old network envelopes are +not supported. + +The only shared data file is carried in both repositories: + +```text +lerobot-teleopit/src/lerobot_teleopit/hand_calibration.json +Teleopit/teleopit/high_level_policy/hand_calibration.json +``` + +`hand_calibration.json` defines the LinkerHand O6 raw open/close values and +range tolerance. The current `describe` response identifies the 68D +observation as `teleopit-g1-state` and the canonical 50D action as +`teleopit-g1-reference`. The action layout and physical-degree OpenNeck +commands are enforced by the current code and tests. + +The canonical action layout is: + +```text +[0:3] session-local root x/y and absolute z +[3:7] session-local root quaternion, wxyz +[7:36] G1 29D reference joint positions, radians +[36:48] left/right LinkerHand O6 closure, [0, 1] +[48:50] OpenNeck yaw/pitch, physical degrees +``` + +The host sends reference motion, never G1 motor commands. Teleopit routes the +body slice through the existing motion tracker, which produces joint-angle +targets for the local G1 controller. + +## 2. Prepare the Host + +Use the independent `lerobot-teleopit` environment on the workstation. For a +first network test, start ReplayPolicy before using ACT: + +```bash +cd /path/to/lerobot-teleopit +uv run teleopit-policy-server \ + --dataset-root data/lerobot/teleopit_v3 \ + --repo-id local/teleopit_v3 \ + --episode 0 \ + --chunk-size 15 \ + --bind tcp://0.0.0.0:5555 +``` + +For ACT, use the host repository's checkpoint command instead. Allow TCP port +`5555` only on the trusted robot network. The protocol deliberately has no +remote shutdown or motor-control endpoint. + +## 3. Prepare the Onboard Runtime + +Install Teleopit and the hardware dependencies in its own environment: + +```bash +pip install -e '.[openneck]' +git submodule update --init --recursive +pip install -e third_party/linkerhand-python-sdk +bash scripts/setup/setup_g1_bridge.sh +``` + +Install `pyrealsense2` for the onboard platform separately. On Arm systems, +the conda-forge package is usually the most reliable option. + +Bring up both LinkerHand CAN interfaces before launch: + +```bash +sudo /usr/sbin/ip link set can0 up type can bitrate 1000000 +sudo /usr/sbin/ip link set can1 up type can bitrate 1000000 +``` + +Calibrate OpenNeck with OpenNeck 0.2.0 and set `neck.config_path` if the +calibration file is not in the runtime working directory. + +## 4. Start Teleopit + +Run the dedicated onboard entry point and set the host IP, low-level tracking +policy, and G1 network interface: + +```bash +python scripts/run/run_high_level_policy_sim2real.py \ + controller.policy_path=track.onnx \ + high_level_policy.endpoint=tcp://192.168.1.10:5555 \ + high_level_policy.task="pick up the object" \ + real_robot.network_interface=eth0 +``` + +Use `high_level_policy.replan_steps=15` for a 15-frame ReplayPolicy chunk. The +initial ACT setup uses `replan_steps=3`. The value must not exceed the horizon +reported by the host. + +The production camera contract is exactly RGB `uint8[480,640,3]` at 30 Hz. +`camera.source=test-pattern` exists only for controlled integration testing; +use `camera.source=realsense` for deployment. + +## 5. Operator Flow + +Keep the Unitree remote in hand. The runtime has only the formal robot modes +`IDLE`, `STANDING`, `POLICY`, and `DAMPING`. + +| Control | Action | +|---------|--------| +| Unitree remote `Start` | Enter `STANDING` | +| Unitree remote `Y` | Request host-policy takeover | +| Unitree remote `B` | Pause or resume `POLICY` | +| Unitree remote `X` | Return to `STANDING` or cancel a pending request | +| Unitree remote `L1+R1` | Emergency transition to `DAMPING` | + +After `Y`, Teleopit creates a new session, establishes the current root XY/yaw +anchor, and waits for the first compatible, fully validated action chunk. The +robot remains formally in `STANDING` during this handshake; there is no +separate "policy starting" state. It enters `POLICY` only after that first +chunk is ready. A timeout leaves the robot in `STANDING`. + +Pause freezes the body reference and holds the last LinkerHand and OpenNeck +commands. Resume requests a fresh action chunk while continuing to hold the +paused pose. `X` stops the policy session and opens/centers the auxiliary +hardware as the runtime returns to `STANDING`. + +A watchdog, host/network, camera, or policy-client fault enters this same +ordinary pause state and keeps the current body, hand, and neck commands. Once +the input path has recovered, press `B`; Teleopit holds the paused pose until a +fresh valid action chunk arrives, then resumes `POLICY`. The runtime never +enters `STANDING` automatically; `X` remains the manual transition. + +## 6. Onboard Validation and Watchdog + +Teleopit rejects a complete chunk if any frame violates the contract. It never +pads, trims, or safety-clips a malformed host result. Checks include: + +- exact finite `float32[T,50]`, current session, and increasing source sequence; +- normalized root quaternion with temporal sign continuity; +- root height, per-frame displacement, XY speed, and yaw-rate limits; +- G1 joint position and joint-rate limits; +- LinkerHand closure `[0,1]` and configured OpenNeck degree ranges; +- observation/result age, source timestamp, and action horizon. + +Validated 30 Hz body references are interpolated and rate-limited locally at +50 Hz, including when latency skips source frames or a new chunk replaces the +old plan. A short configured grace period can reuse the final validated +reference during an inference delay. If no valid action remains, a network +exchange fails, or a required camera/client worker exits, Teleopit remains in +`POLICY`, enters the normal resumable pause state, and holds the latest body, +hand, and neck commands. After recovery, `B` requests resume; execution stays +paused until a fresh validated chunk arrives. Only `X` changes the mode to +`STANDING`. + +The default safety envelope lives under `high_level_policy.safety` in +`high_level_policy_sim2real.yaml`. Adjust it only after checking the recorded +data, G1 joint limits, and the installed OpenNeck calibration. + +## 7. Troubleshooting + +**`Y` never enters `POLICY`:** check the host endpoint, firewall, server log, +`describe` schemas, message envelope, task, checkpoint manifest, and +`replan_steps`. Teleopit stays +in `STANDING` by design when any handshake or first-chunk check fails. + +**The first chunk is rejected for rate limits:** the first predicted reference +is too far from the current G1 pose. Start from the demonstrated standing pose +or fix the policy/replay start frame; do not bypass the boundary check. + +**Policy runs briefly and becomes paused:** inspect timeout, inference +latency, stale-result, worker-exit, and safety-rejection logs. The low-level +50 Hz tracker does not block on host inference. Restore the failed input path, +then press `B` to resume. + +**Pico does not connect:** this runtime intentionally does not start Pico. Stop +it and launch the Pico-specific `run_sim2real.py --config-name +pico4_sim2real` workflow instead. diff --git a/docs/docs/tutorials/pico-sim2real.md b/docs/docs/tutorials/pico-sim2real.md index b03bfc69..8ca4ac20 100644 --- a/docs/docs/tutorials/pico-sim2real.md +++ b/docs/docs/tutorials/pico-sim2real.md @@ -161,6 +161,7 @@ Keep the Unitree remote in hand. `L1+R1` is the emergency stop path into |---------|--------| | Unitree remote `Start` | Enter `STANDING` | | Unitree remote `Y` | Enter `MOCAP` | +| Unitree remote `B` | Pause / resume live mocap | | Pico/controller `A` | Pause / resume live mocap | | Pico/controller `B` | Toggle `MOCAP` / `ARMS` | | Unitree remote `X` | Return to `STANDING` | @@ -195,7 +196,9 @@ resets policy/reference alignment and uses the same Kp ramp safety path. ## Pause / Resume -Pico pause/resume is a mocap-session control event. +Pico pause/resume is a mocap-session control event. Use either Unitree remote +`B` or Pico/controller `A`; Pico/controller `B` remains the `MOCAP` / `ARMS` +toggle. - `ACTIVE`: the pause button freezes the current reference pose. - `PAUSED`: pressing it again clears policy/reference state, warms the realtime diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md index b797d2b6..1622b670 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md @@ -125,6 +125,43 @@ MuJoCo 窗口显示重定向参考；`sim2sim`、`mocap`、`camera` 和 `all` | `mocap_switch.check_frames` | 切换到 MOCAP 前所需的连续有效帧数 | `10` | | `arm_mocap.controlled_joint_indices` | Pico `ARMS` 模式下由实时 retargeting 驱动的 G1 关节 | `[15..28]` | +### 主机 High-Level Policy（独立 sim2real） + +`high_level_policy_sim2real.yaml` 只供 +`scripts/run/run_high_level_policy_sim2real.py` 使用。它会启动 camera、network +client、robot-control、LinkerHand O6 和 OpenNeck worker；不会启动 PicoBridge、GMR +或 retarget reference worker。主机 LeRobot 环境保持独立，并且必须跟随当前 +client/server 消息结构与协议测试。唯一共享的数据文件是 `hand_calibration.json`。 + +| 字段 | 说明 | 默认值 | +|------|------|--------| +| `camera.source` | Onboard 策略相机：`realsense`，或仅供集成测试的 `test-pattern` | `realsense` | +| `camera.width` / `height` / `fps` | 精确的策略图像契约 | `640` / `480` / `30` | +| `camera.device` | 可选 RealSense 序列号 | `null` | +| `high_level_policy.endpoint` | 主机策略 ZeroMQ TCP endpoint | `tcp://127.0.0.1:5555` | +| `high_level_policy.task` | reset 和每个 observation 都会发送的非空任务 prompt | `demo` | +| `high_level_policy.timeout_s` | 单次网络请求 deadline | `1.0` | +| `high_level_policy.reconnect_backoff_s` | 建立新 session 时的重试间隔 | `1.0` | +| `high_level_policy.replan_steps` | 两次请求之间的最小 30 Hz source-frame 间隔；不能超过主机 horizon | `3` | +| `high_level_policy.jpeg_quality` | 640x480 RGB 帧的 JPEG 质量 | `90` | +| `high_level_policy.max_observation_age_s` | 跳过请求前允许的最大 camera/observation age | `0.15` | +| `high_level_policy.max_result_age_s` | 拒绝已接收结果前允许的最大本地 IPC age | `0.1` | +| `high_level_policy.entry_timeout_s` | 保持 `STANDING` 等待首个有效 chunk 的最长时间 | `3.0` | +| `high_level_policy.hold_s` | Watchdog 暂停 `POLICY` 前，最后一条有效 reference 的 grace period | `0.1` | +| `high_level_policy.safety.root_height_min_m` / `root_height_max_m` | 可接受的绝对 root 高度范围 | `0.55` / `1.05` | +| `high_level_policy.safety.max_root_xy_speed_m_s` | 30 Hz reference 之间允许的最大 root XY 速度 | `2.5` | +| `high_level_policy.safety.max_root_displacement_m` | reference 帧之间允许的最大 3D root 位移 | `0.1` | +| `high_level_policy.safety.max_yaw_rate_rad_s` | 最大 root yaw rate | `2.5` | +| `high_level_policy.safety.max_joint_rate_rad_s` | 最大单关节 reference rate | `10.0` | +| `high_level_policy.safety.neck_yaw_min_deg` / `neck_yaw_max_deg` | 可接受的 OpenNeck yaw 命令范围 | `-45` / `45` | +| `high_level_policy.safety.neck_pitch_min_deg` / `neck_pitch_max_deg` | 可接受的 OpenNeck pitch 命令范围 | `-40` / `40` | + +G1 reference joint position 会按 `real_robot.joint_pos_lower/upper` 检查。由于 canonical +50D action 的所有字段都处于启用状态，初始运行时要求 +`hands.driver=linkerhand_o6`、左右两只手以及 `neck.driver=openneck`。OpenNeck 策略值 +在 chunk 验证后直接发送给 `move_deg(yaw, pitch)`；不会应用 Pico dead-zone 或 pitch-gain +映射。 + ### 真机 SDK | 字段 | 说明 | 默认值 | diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/overview.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/overview.md index 301c7b0c..556afea1 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/overview.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/overview.md @@ -16,6 +16,7 @@ Teleopit 使用 [Hydra](https://hydra.cc/) 组合配置。大多数运行入口 | `teleopit/configs/pico4_sim.yaml` | Pico 4 VR sim2sim | | `teleopit/configs/sim2real.yaml` | BVH sim2real（Unitree G1 真机） | | `teleopit/configs/pico4_sim2real.yaml` | Pico 4 VR sim2real（Unitree G1 真机） | +| `teleopit/configs/high_level_policy_sim2real.yaml` | 独立主机策略 sim2real（Unitree G1 真机） | 它们会组合以下子配置： diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/quick-start.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/quick-start.md index 1fee2d36..f53ea91b 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/quick-start.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/quick-start.md @@ -60,4 +60,5 @@ python scripts/run/run_sim.py controller.policy_path=track.onnx 'viewers=[retarg - [独立站立测试](../tutorials/standalone-standing) - 检查 G1 bridge、网络和 policy 站立 - [Pico Sim2Real](../tutorials/pico-sim2real) - 将 Pico 遥操作部署到 Unitree G1 - [BVH Sim2Real](../tutorials/bvh-sim2real) - 在 Unitree G1 上回放离线 BVH 动作 +- [主机策略 Sim2Real](../tutorials/high-level-policy-sim2real) - 将独立 LeRobot 策略主机连接到 onboard motion tracker - [训练](../tutorials/training) - 训练你自己的策略 diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/architecture.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/architecture.md index 2000679e..d16066c2 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/architecture.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/architecture.md @@ -18,6 +18,22 @@ InputProvider（BVH 文件 / Pico4） 离线/在线推理由 `teleopit/runtime/` 和 `teleopit/pipeline.py` 装配。硬件状态机通过 `teleopit/sim2real/mp/` 中的进程隔离运行时执行。训练由 `train_mimic/` 提供。 +由主机提供服务的模仿策略使用第二条相互独立的部署路径： + +```text +lerobot-teleopit 主机环境 + policy server -> 严格的 ZeroMQ/msgpack 消息 + | +Teleopit onboard 环境 + RealSense/state -> 非关键 client worker -> 已验证的 action scheduler + -> 现有 50 Hz motion tracker -> G1 关节角目标 + -> 专用 LinkerHand O6 与 OpenNeck worker +``` + +主机与 onboard 环境共享语义数据和一份相同的 `hand_calibration.json`；它们不会导入 +对方的 Python 包。当前 client/server 代码和协议测试定义网络结构，因此活跃开发期间 +两个仓库必须同步修改。Pico 遥操作和主机策略部署也分别使用不同的运行脚本与进程装配。 + ## 代码结构 ```text @@ -44,6 +60,7 @@ train_mimic/scripts/data | `teleopit/runtime/` | 配置解析、路径规范化、组件装配、CLI 校验 | | `teleopit/pipeline.py` | 离线仿真的轻量 facade | | `teleopit/sim2real/mp/` | 进程隔离的 sim2real 状态机、IPC 和机器人控制循环 | +| `teleopit/high_level_policy/` | 主机策略协议、session-local 坐标变换、验证与 30-to-50 Hz scheduler | | `teleopit/controllers/observation.py` | ObservationBuilder | | `teleopit/controllers/rl_policy.py` | 接受观测维度与运行时 builder 匹配的双输入 ONNX | | `train_mimic/app.py` | 共享的训练/播放/benchmark 装配 | @@ -61,6 +78,9 @@ train_mimic/scripts/data | 训练采样 | 默认 `rewind`；也支持 `uniform`；播放使用 `start`；benchmark 固定精确 clip 并禁用 clip 末尾重采样 | | 训练 `window_steps` | `[0]` | | 数据格式 | 可递归发现的最小 HDF5 shard（`shard_*.h5`） | +| 主机策略 observation | JPEG RGB + `observation.state(68)` | +| 主机策略 action | 30 Hz 的 `float32[T,50]` canonical reference | +| 主机策略 body 控制 | 36D root/joint reference 通过现有 50 Hz motion tracker | ## 约束 @@ -69,10 +89,14 @@ train_mimic/scripts/data - `viewers` 是唯一的 viewer 配置入口 - 观测/ONNX 维度不匹配会在启动时立即报错 - sim2real 也要求双输入 ONNX，且观测维度必须与运行时 builder 匹配 +- 主机策略消息 envelope 或 schema 不匹配时会被拒绝，机器人保持在 `STANDING` +- 主机 action chunk 在 onboard 完成验证与插值；主机不能绕过 motion tracker 或发送电机命令 +- 等待第一个主机 chunk 不是机器人模式：正式接管模式只有 `POLICY` ## 公共接口 -**稳定运行模式：** 离线 sim2sim、离线 sim2real playback、Pico4 sim2sim、G1 sim2real +**稳定运行模式：** 离线 sim2sim、离线 sim2real playback、Pico4 sim2sim、G1 +sim2real、独立的主机策略 G1 sim2real **稳定训练入口：** `train.py`、`play.py`、`benchmark.py`、`save_onnx.py` diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md new file mode 100644 index 00000000..ccfddc56 --- /dev/null +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md @@ -0,0 +1,183 @@ +--- +sidebar_position: 6 +--- + +# 在 Unitree G1 上部署主机策略 + +该工作流在主机工作站上运行 LeRobot 策略服务，在 G1 onboard 计算机上运行 +Teleopit motion tracker。两个仓库使用相互独立的 Python 环境，只通过严格的 +ZeroMQ/msgpack 消息通信。 + +```text +主机工作站（lerobot-teleopit） + ReplayPolicy 或 ACT -> policy server + | + | 通过 TCP 传输 float32 state/action + JPEG + v +G1 onboard 计算机（Teleopit） + RealSense + G1 state -> client -> 已验证的 30 Hz action chunk + -> 50 Hz 插值 -> motion tracker -> G1 关节角目标 + -> LinkerHand O6 / OpenNeck +``` + +这是与 Pico 遥操作相互独立的运行时。该工作流不应启动 PicoBridge、GMR 或 +`run_sim2real.py`。在 Pico 控制与主机策略控制之间切换时，需要先停止一个运行时， +再启动另一个。 + +## 1. 网络消息与手部标定 + +当前 client/server 代码和协议测试定义 request 与 response 结构。活跃开发期间， +任何结构变更都必须同时修改 Teleopit 和 `lerobot-teleopit`；不支持旧网络 envelope。 + +两个仓库唯一共享的数据文件为： + +```text +lerobot-teleopit/src/lerobot_teleopit/hand_calibration.json +Teleopit/teleopit/high_level_policy/hand_calibration.json +``` + +`hand_calibration.json` 定义 LinkerHand O6 的 raw open/close 值和 range tolerance。 +当前 `describe` 响应将 68D observation 标识为 `teleopit-g1-state`，将 canonical 50D +action 标识为 `teleopit-g1-reference`。action 布局和使用物理角度的 OpenNeck 命令由 +当前代码与测试约束。 + +canonical action 布局为： + +```text +[0:3] session-local root x/y 与绝对 z +[3:7] session-local root quaternion，wxyz +[7:36] G1 29D reference joint positions，弧度 +[36:48] 左/右 LinkerHand O6 closure，[0, 1] +[48:50] OpenNeck yaw/pitch，物理角度 +``` + +主机发送的是 reference motion，而不是 G1 电机命令。Teleopit 会把 body slice 送入 +现有 motion tracker，由它为本地 G1 控制器生成关节角目标。 + +## 2. 准备主机 + +在工作站上使用独立的 `lerobot-teleopit` 环境。首次网络测试应先运行 +ReplayPolicy，再使用 ACT： + +```bash +cd /path/to/lerobot-teleopit +uv run teleopit-policy-server \ + --dataset-root data/lerobot/teleopit_v3 \ + --repo-id local/teleopit_v3 \ + --episode 0 \ + --chunk-size 15 \ + --bind tcp://0.0.0.0:5555 +``` + +使用 ACT 时，改用主机仓库中的 checkpoint 命令。只在可信的机器人网络上放行 TCP +端口 `5555`。该协议有意不提供远程关机或电机控制 endpoint。 + +## 3. 准备 Onboard 运行时 + +在 Teleopit 自己的环境中安装 Teleopit 与硬件依赖： + +```bash +pip install -e '.[openneck]' +git submodule update --init --recursive +pip install -e third_party/linkerhand-python-sdk +bash scripts/setup/setup_g1_bridge.sh +``` + +需要根据 onboard 平台单独安装 `pyrealsense2`。在 Arm 系统上，conda-forge 包通常 +最可靠。 + +启动前开启两个 LinkerHand CAN 接口： + +```bash +sudo /usr/sbin/ip link set can0 up type can bitrate 1000000 +sudo /usr/sbin/ip link set can1 up type can bitrate 1000000 +``` + +使用 OpenNeck 0.2.0 完成校准；如果校准文件不在运行目录中，请设置 +`neck.config_path`。 + +## 4. 启动 Teleopit + +运行专用 onboard 入口，并设置主机 IP、底层 tracking policy 和 G1 网卡： + +```bash +python scripts/run/run_high_level_policy_sim2real.py \ + controller.policy_path=track.onnx \ + high_level_policy.endpoint=tcp://192.168.1.10:5555 \ + high_level_policy.task="pick up the object" \ + real_robot.network_interface=eth0 +``` + +对于 15 帧 ReplayPolicy chunk，使用 `high_level_policy.replan_steps=15`。初始 ACT +配置使用 `replan_steps=3`。该值不能超过主机报告的 horizon。 + +生产相机契约固定为 30 Hz 的 RGB `uint8[480,640,3]`。 +`camera.source=test-pattern` 只用于受控集成测试；部署时应使用 +`camera.source=realsense`。 + +## 5. 操作流程 + +始终把 Unitree 遥控器拿在手中。该运行时只有 `IDLE`、`STANDING`、`POLICY` 和 +`DAMPING` 四个正式机器人模式。 + +| 控制 | 动作 | +|------|------| +| Unitree remote `Start` | 进入 `STANDING` | +| Unitree remote `Y` | 请求主机策略接管 | +| Unitree remote `B` | 暂停或恢复 `POLICY` | +| Unitree remote `X` | 返回 `STANDING`，或取消等待中的请求 | +| Unitree remote `L1+R1` | 紧急切换到 `DAMPING` | + +按下 `Y` 后，Teleopit 会创建新 session，以当前 root XY/yaw 建立锚点，并等待第一个 +兼容且完整通过验证的 action chunk。握手期间机器人在形式上仍处于 `STANDING`；没有 +单独的“policy starting”状态。只有首个 chunk 就绪后才会进入 `POLICY`。超时后机器人 +仍保持 `STANDING`。 + +暂停会冻结 body reference，并保持最后一条 LinkerHand 和 OpenNeck 命令。恢复时会请求 +新的 action chunk，并在等待期间继续保持暂停姿态。按 `X` 会停止策略 session；运行时 +返回 `STANDING` 时会张开手并让辅助硬件回中。 + +Watchdog、主机/网络、相机或 policy client 故障也会进入同一个普通暂停状态，并保持 +当前 body、hand 和 neck 命令。输入路径恢复后按 `B`；Teleopit 会继续保持暂停姿态， +直到收到新的有效 action chunk，再恢复 `POLICY`。运行时不会自动进入 `STANDING`； +`X` 仍是手动切换到 `STANDING` 的操作。 + +## 6. Onboard 验证与 Watchdog + +如果任一帧违反契约，Teleopit 会拒绝整个 chunk。它不会对错误的主机结果进行补齐、 +裁剪或安全限幅。检查包括： + +- 精确且有限的 `float32[T,50]`、当前 session，以及递增的 source sequence； +- 归一化 root quaternion 与时间连续的符号； +- root 高度、逐帧位移、XY 速度和 yaw rate 限制； +- G1 关节位置和关节 rate 限制； +- LinkerHand closure `[0,1]` 和配置的 OpenNeck 角度范围； +- observation/result 时效、source timestamp 和 action horizon。 + +通过验证的 30 Hz body reference 会在本地插值到 50 Hz 并执行 rate limit；网络延迟 +导致跳过 source frame 或新 chunk 替换旧计划时同样如此。在短暂推理延迟期间，可以在 +配置的短 grace period 内继续使用最后一条已验证 reference。如果不再有有效 action， +网络交换失败，或必要的 camera/client worker 退出，Teleopit 会保持在 `POLICY`，进入 +普通的可恢复暂停状态，并保持最后一条 body、hand 和 neck 命令。故障恢复后按 `B` +请求恢复；在收到新的有效 chunk 前，执行仍保持暂停。只有 `X` 会把模式切换到 +`STANDING`。 + +默认安全范围位于 `high_level_policy_sim2real.yaml` 的 +`high_level_policy.safety` 下。只有在检查录制数据、G1 关节限位和已安装的 OpenNeck +校准后，才应调整这些值。 + +## 7. 故障排查 + +**按 `Y` 后始终不进入 `POLICY`：** 检查主机 endpoint、防火墙、server 日志、 +`describe` schema、消息 envelope、task、checkpoint manifest 和 `replan_steps`。 +任何握手或首个 chunk 检查失败时， +Teleopit 都会按设计保持 `STANDING`。 + +**首个 chunk 因 rate limit 被拒绝：** 第一条预测 reference 距离当前 G1 姿态太远。 +请从示范的站立姿态启动，或修正 policy/replay 起始帧；不要绕过边界检查。 + +**策略短暂运行后进入暂停：** 检查 timeout、推理延迟、stale result、worker 退出和 +安全拒绝日志。底层 50 Hz tracker 不会等待主机推理。恢复故障输入路径后按 `B` 继续。 + +**Pico 无法连接：** 该运行时有意不启动 Pico。请先停止它，再改用 Pico 专用的 +`run_sim2real.py --config-name pico4_sim2real` 工作流。 diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md index c98a1653..f41ef7de 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md @@ -149,6 +149,7 @@ python scripts/view/view_recording.py \ |------|------| | Unitree remote `Start` | 进入 `STANDING` | | Unitree remote `Y` | 进入 `MOCAP` | +| Unitree remote `B` | 暂停 / 恢复实时动捕 | | Pico/controller `A` | 暂停 / 恢复实时动捕 | | Pico/controller `B` | 在 `MOCAP` / `ARMS` 之间切换 | | Unitree remote `X` | 返回 `STANDING` | @@ -179,7 +180,8 @@ policy/reference 对齐，并使用同一套 Kp ramp 安全路径。 ## 暂停 / 恢复 -Pico 暂停/恢复是 mocap-session control event。 +Pico 暂停/恢复是 mocap-session control event。可以使用 Unitree remote `B` 或 +Pico/controller `A`；Pico/controller `B` 仍用于切换 `MOCAP` / `ARMS`。 - `ACTIVE`：暂停键冻结当前参考姿态。 - `PAUSED`：再次按下会清空 policy/reference 状态，预热实时 buffer，重新居中 yaw/XY 对齐， diff --git a/docs/sidebars.ts b/docs/sidebars.ts index 5fa0eba5..96799339 100644 --- a/docs/sidebars.ts +++ b/docs/sidebars.ts @@ -21,6 +21,7 @@ const sidebars: SidebarsConfig = { 'tutorials/standalone-standing', 'tutorials/pico-sim2real', 'tutorials/bvh-sim2real', + 'tutorials/high-level-policy-sim2real', 'tutorials/training', ], }, diff --git a/pyproject.toml b/pyproject.toml index 6b1427dd..baee02e1 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -24,6 +24,7 @@ dependencies = [ "h5py", "onnxruntime", "pyzmq", + "msgpack", "rich", "loop-rate-limiters", "imageio", @@ -71,3 +72,6 @@ dexhand = [] [tool.setuptools.packages.find] where = ["."] include = ["teleopit*", "train_mimic*"] + +[tool.setuptools.package-data] +"teleopit.high_level_policy" = ["hand_calibration.json"] diff --git a/scripts/run/run_high_level_policy_sim2real.py b/scripts/run/run_high_level_policy_sim2real.py new file mode 100644 index 00000000..acc664c7 --- /dev/null +++ b/scripts/run/run_high_level_policy_sim2real.py @@ -0,0 +1,59 @@ +"""Run host high-level-policy control through Teleopit's onboard motion tracker.""" + +from __future__ import annotations + +import inspect + +import hydra +from omegaconf import DictConfig + +from teleopit.high_level_policy.config import parse_high_level_policy_config +from teleopit.runtime.cli import validate_policy_path +from teleopit.runtime.console import ( + PlainConsole, + configure_runtime_logging, + high_level_policy_operator_controls, +) +from teleopit.sim2real.mp import HighLevelPolicySim2RealRuntime + + +@hydra.main( + version_base=None, + config_path="../../teleopit/configs", + config_name="high_level_policy_sim2real", +) +def main(cfg: DictConfig) -> None: + _run_high_level_policy_sim2real(cfg) + + +def _run_high_level_policy_sim2real(cfg: DictConfig) -> None: + configure_runtime_logging(cfg, force=True) + validate_policy_path(cfg, "run_high_level_policy_sim2real.py") + policy_cfg = parse_high_level_policy_config(cfg) + console = PlainConsole(title="Teleopit high-level policy sim2real") + runtime_params = inspect.signature(HighLevelPolicySim2RealRuntime).parameters + runtime = ( + HighLevelPolicySim2RealRuntime(cfg, console=console) + if "console" in runtime_params + else HighLevelPolicySim2RealRuntime(cfg) + ) + console.start( + status=( + ("State", "IDLE"), + ("Runtime", "high-level policy"), + ("Host", policy_cfg.endpoint), + ("Task", policy_cfg.task), + ), + controls=high_level_policy_operator_controls(), + events=("Start enters STANDING; Remote Y requests host-policy takeover",), + control_section="Controls", + show_help_key=False, + ) + try: + runtime.run() + finally: + runtime.shutdown() + + +if __name__ == "__main__": + main() diff --git a/teleopit/configs/high_level_policy_sim2real.yaml b/teleopit/configs/high_level_policy_sim2real.yaml new file mode 100644 index 00000000..a9708ef3 --- /dev/null +++ b/teleopit/configs/high_level_policy_sim2real.yaml @@ -0,0 +1,51 @@ +defaults: + - sim2real + - _self_ + +# This configuration is launched only by run_high_level_policy_sim2real.py. +# It does not start PicoBridge, GMR, or the teleoperation reference worker. +input: + provider: high_level_policy + +camera: + source: realsense # realsense | test-pattern + width: 640 + height: 480 + fps: 30 + device: null + +high_level_policy: + enabled: true + endpoint: tcp://127.0.0.1:5555 + task: demo + timeout_s: 1.0 + reconnect_backoff_s: 1.0 + replan_steps: 3 # Current ACT checkpoint; use 15 for ReplayPolicy. + jpeg_quality: 90 + max_observation_age_s: 0.15 + max_result_age_s: 0.1 + entry_timeout_s: 3.0 + hold_s: 0.1 + safety: + root_height_min_m: 0.55 + root_height_max_m: 1.05 + max_root_xy_speed_m_s: 2.5 + max_root_displacement_m: 0.1 + max_yaw_rate_rad_s: 2.5 + max_joint_rate_rad_s: 10.0 + neck_yaw_min_deg: -45.0 + neck_yaw_max_deg: 45.0 + neck_pitch_min_deg: -40.0 + neck_pitch_max_deg: 40.0 + +recording: + enabled: false + +hands: + enabled: true + driver: linkerhand_o6 + sides: [left, right] + +neck: + enabled: true + driver: openneck diff --git a/teleopit/high_level_policy/__init__.py b/teleopit/high_level_policy/__init__.py new file mode 100644 index 00000000..107bb1bb --- /dev/null +++ b/teleopit/high_level_policy/__init__.py @@ -0,0 +1,21 @@ +"""Lightweight onboard client and scheduler for host high-level policies.""" + +from teleopit.high_level_policy.client import ( + HighLevelPolicyClient, + PolicyActionChunk, + PolicyDescription, +) +from teleopit.high_level_policy.hand_calibration import HandCalibration +from teleopit.high_level_policy.scheduler import ( + HighLevelPolicyScheduler, + PolicyFrameTransform, +) + +__all__ = [ + "HandCalibration", + "HighLevelPolicyClient", + "HighLevelPolicyScheduler", + "PolicyActionChunk", + "PolicyDescription", + "PolicyFrameTransform", +] diff --git a/teleopit/high_level_policy/client.py b/teleopit/high_level_policy/client.py new file mode 100644 index 00000000..412ee837 --- /dev/null +++ b/teleopit/high_level_policy/client.py @@ -0,0 +1,276 @@ +"""Synchronous ZeroMQ client used only from the onboard background worker.""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import Any, Mapping + +import numpy as np +import zmq + +from teleopit.high_level_policy.protocol import ( + ENDPOINTS, + MAX_ACTION_HORIZON, + MAX_IMAGE_BYTES, + MAX_REQUEST_BYTES, + MAX_RESPONSE_BYTES, + PolicyProtocolError, + decode_float32_array, + encode_float32_array, + pack_message, + unpack_message, +) + + +class PolicyTransportError(RuntimeError): + """The host did not complete a REQ/REP exchange before the deadline.""" + + +@dataclass(frozen=True) +class PolicyDescription: + policy_type: str + policy_id: str + dataset_fps: int + max_action_horizon: int + + +@dataclass(frozen=True) +class PolicyActionChunk: + session_id: str + source_sequence_id: int + source_onboard_monotonic_timestamp_ns: int + action_fps: int + actions: np.ndarray + policy_id: str + server_inference_ms: float + + +class HighLevelPolicyClient: + def __init__( + self, + endpoint: str, + *, + timeout_s: float, + context: zmq.Context[Any] | None = None, + ) -> None: + if not str(endpoint).startswith(("tcp://", "inproc://")): + raise ValueError("High-level policy endpoint must use tcp:// (or inproc:// in tests)") + if not np.isfinite(timeout_s) or timeout_s <= 0.0: + raise ValueError("High-level policy timeout_s must be finite and > 0") + self.endpoint = str(endpoint) + self.timeout_s = float(timeout_s) + self._own_context = context is None + self._context = zmq.Context() if context is None else context + self._socket: zmq.Socket[Any] | None = None + self._open_socket() + + def close(self) -> None: + self._close_socket() + if self._own_context: + self._context.term() + + def ping(self) -> bool: + data = self._request("ping", {}) + if set(data) != {"ready"} or not isinstance(data["ready"], bool): + raise PolicyProtocolError("invalid_response", "ping data must contain exactly boolean ready") + return bool(data["ready"]) + + def describe(self) -> PolicyDescription: + data = self._request("describe", {}) + expected_fields = { + "observation_schema", + "observation_dim", + "action_schema", + "action_dim", + "dataset_fps", + "max_action_horizon", + "policy_type", + "policy_id", + "ready", + } + if set(data) != expected_fields: + raise PolicyProtocolError("invalid_response", "describe data contains unexpected fields") + expected_schema = { + "observation_schema": "teleopit-g1-state", + "observation_dim": 68, + "action_schema": "teleopit-g1-reference", + "action_dim": 50, + } + for name, value in expected_schema.items(): + if data[name] != value: + raise PolicyProtocolError( + "schema_mismatch", f"describe {name} must be {value!r}, got {data[name]!r}" + ) + if data["dataset_fps"] != 30: + raise PolicyProtocolError( + "invalid_response", f"describe dataset_fps must be 30, got {data['dataset_fps']!r}" + ) + if data["ready"] is not True: + raise PolicyProtocolError("policy_not_ready", "Host policy reported ready=false") + if not isinstance(data["policy_type"], str) or not data["policy_type"]: + raise PolicyProtocolError("invalid_response", "describe policy_type must be non-empty") + if not isinstance(data["policy_id"], str) or not data["policy_id"]: + raise PolicyProtocolError("invalid_response", "describe policy_id must be non-empty") + horizon = _int64(data["max_action_horizon"], name="max_action_horizon") + if not 1 <= horizon <= MAX_ACTION_HORIZON: + raise PolicyProtocolError("invalid_response", "describe max_action_horizon is outside limits") + return PolicyDescription( + policy_type=data["policy_type"], + policy_id=data["policy_id"], + dataset_fps=30, + max_action_horizon=horizon, + ) + + def reset(self, session_id: str, task: str) -> None: + data = self._request("reset", {"session_id": session_id, "task": task}) + if set(data) != {"session_id", "reset"} or data["session_id"] != session_id or data["reset"] is not True: + raise PolicyProtocolError("invalid_response", "reset acknowledgement does not match the requested session") + + def get_action( + self, + *, + session_id: str, + sequence_id: int, + onboard_monotonic_timestamp_ns: int, + task: str, + jpeg_image: bytes, + state: object, + ) -> PolicyActionChunk: + if not isinstance(jpeg_image, bytes): + raise PolicyProtocolError("invalid_image", "jpeg_image must be bytes") + if not 4 <= len(jpeg_image) <= MAX_IMAGE_BYTES: + raise PolicyProtocolError("invalid_image", "jpeg_image size is outside limits") + if not jpeg_image.startswith(b"\xff\xd8") or not jpeg_image.endswith(b"\xff\xd9"): + raise PolicyProtocolError("invalid_image", "jpeg_image is missing JPEG start/end markers") + state_array = np.asarray(state, dtype=np.float32) + if state_array.shape != (68,) or not np.all(np.isfinite(state_array)): + raise PolicyProtocolError("invalid_state", f"state must be finite float32[68], got {state_array.shape}") + quaternion_norm = float(np.linalg.norm(state_array[58:62])) + if abs(quaternion_norm - 1.0) > 1e-3: + raise PolicyProtocolError( + "invalid_state", f"state base quaternion norm must be near 1, got {quaternion_norm:.6g}" + ) + request_data = { + "session_id": session_id, + "sequence_id": _int64(sequence_id, name="sequence_id"), + "onboard_monotonic_timestamp_ns": _int64( + onboard_monotonic_timestamp_ns, + name="onboard_monotonic_timestamp_ns", + ), + "task": task, + "image_encoding": "jpeg", + "image": jpeg_image, + "state": encode_float32_array(state_array), + } + data = self._request("get_action", request_data) + expected_fields = { + "session_id", + "source_sequence_id", + "source_onboard_monotonic_timestamp_ns", + "action_fps", + "actions", + "policy_id", + "server_inference_ms", + } + if set(data) != expected_fields: + raise PolicyProtocolError("invalid_response", "get_action data contains unexpected fields") + if data["session_id"] != session_id: + raise PolicyProtocolError("session_mismatch", "get_action response session_id does not match request") + source_sequence = _int64(data["source_sequence_id"], name="source_sequence_id") + source_timestamp = _int64( + data["source_onboard_monotonic_timestamp_ns"], + name="source_onboard_monotonic_timestamp_ns", + ) + if source_sequence != sequence_id or source_timestamp != onboard_monotonic_timestamp_ns: + raise PolicyProtocolError("stale_response", "get_action response does not echo the source observation") + action_fps = _int64(data["action_fps"], name="action_fps") + if action_fps != 30: + raise PolicyProtocolError("invalid_response", f"action_fps must be 30, got {action_fps}") + actions = decode_float32_array(data["actions"], name="actions", expected_shape=(None, 50)) + if not 1 <= len(actions) <= MAX_ACTION_HORIZON: + raise PolicyProtocolError("invalid_response", f"actions horizon is invalid: {len(actions)}") + if not isinstance(data["policy_id"], str) or not data["policy_id"]: + raise PolicyProtocolError("invalid_response", "policy_id must be non-empty") + inference_ms = float(data["server_inference_ms"]) + if not np.isfinite(inference_ms) or inference_ms < 0.0: + raise PolicyProtocolError("invalid_response", "server_inference_ms must be finite and >= 0") + return PolicyActionChunk( + session_id=session_id, + source_sequence_id=source_sequence, + source_onboard_monotonic_timestamp_ns=source_timestamp, + action_fps=action_fps, + actions=actions, + policy_id=data["policy_id"], + server_inference_ms=inference_ms, + ) + + def _request(self, endpoint: str, data: dict[str, Any]) -> dict[str, Any]: + if endpoint not in ENDPOINTS: + raise PolicyProtocolError("unknown_endpoint", f"Unsupported endpoint {endpoint!r}") + request = {"endpoint": endpoint, "data": data} + payload = pack_message(request, max_bytes=MAX_REQUEST_BYTES) + socket = self._socket + if socket is None: + raise PolicyTransportError("High-level policy client is closed") + try: + socket.send(payload) + reply_payload = socket.recv() + except zmq.Again as exc: + self._recreate_socket() + raise PolicyTransportError( + f"High-level policy {endpoint} timed out after {self.timeout_s:.3f}s" + ) from exc + except zmq.ZMQError as exc: + self._recreate_socket() + raise PolicyTransportError( + f"High-level policy {endpoint} transport failed: {exc}" + ) from exc + reply = unpack_message(reply_payload, max_bytes=MAX_RESPONSE_BYTES) + return self._parse_reply(reply, endpoint=endpoint) + + def _parse_reply(self, reply: Mapping[str, Any], *, endpoint: str) -> dict[str, Any]: + base = {"endpoint", "ok"} + if not base.issubset(reply): + raise PolicyProtocolError("invalid_response", "Response is missing envelope fields") + if reply["endpoint"] != endpoint: + raise PolicyProtocolError("invalid_response", f"Response endpoint {reply['endpoint']!r} != {endpoint!r}") + if not isinstance(reply["ok"], bool): + raise PolicyProtocolError("invalid_response", "Response ok must be boolean") + if reply["ok"]: + if set(reply) != base | {"data"} or not isinstance(reply["data"], dict): + raise PolicyProtocolError("invalid_response", "Successful response must contain exactly data") + return dict(reply["data"]) + if set(reply) != base | {"error"} or not isinstance(reply["error"], Mapping): + raise PolicyProtocolError("invalid_response", "Failed response must contain exactly error") + error = reply["error"] + if set(error) != {"code", "message"} or not all(isinstance(error[key], str) for key in error): + raise PolicyProtocolError("invalid_response", "Response error must contain string code/message") + raise PolicyProtocolError(error["code"], error["message"]) + + def _open_socket(self) -> None: + socket = self._context.socket(zmq.REQ) + timeout_ms = max(1, int(round(self.timeout_s * 1000.0))) + socket.setsockopt(zmq.LINGER, 0) + socket.setsockopt(zmq.RCVHWM, 1) + socket.setsockopt(zmq.SNDHWM, 1) + socket.setsockopt(zmq.RCVTIMEO, timeout_ms) + socket.setsockopt(zmq.SNDTIMEO, timeout_ms) + socket.setsockopt(zmq.MAXMSGSIZE, MAX_RESPONSE_BYTES) + socket.connect(self.endpoint) + self._socket = socket + + def _close_socket(self) -> None: + socket = self._socket + self._socket = None + if socket is not None: + socket.close(linger=0) + + def _recreate_socket(self) -> None: + self._close_socket() + self._open_socket() + + +def _int64(value: object, *, name: str) -> int: + if not isinstance(value, int) or isinstance(value, bool) or not 0 <= value <= 2**63 - 1: + raise PolicyProtocolError("invalid_value", f"{name} must be an int64 in [0, 2^63-1]") + return int(value) diff --git a/teleopit/high_level_policy/config.py b/teleopit/high_level_policy/config.py new file mode 100644 index 00000000..0d7e8db4 --- /dev/null +++ b/teleopit/high_level_policy/config.py @@ -0,0 +1,223 @@ +from __future__ import annotations + +from dataclasses import dataclass +import math +from typing import Any + +import numpy as np + +from teleopit.runtime.common import cfg_get + + +@dataclass(frozen=True) +class HighLevelPolicyConfig: + endpoint: str + task: str + timeout_s: float + reconnect_backoff_s: float + replan_steps: int + jpeg_quality: int + max_observation_age_s: float + max_result_age_s: float + entry_timeout_s: float + hold_s: float + + +@dataclass(frozen=True) +class HighLevelPolicyCameraConfig: + source: str + width: int + height: int + fps: int + device: str | None + + +@dataclass(frozen=True) +class HighLevelPolicySafetyConfig: + root_height_min_m: float + root_height_max_m: float + max_root_xy_speed_m_s: float + max_root_displacement_m: float + max_yaw_rate_rad_s: float + max_joint_rate_rad_s: float + joint_pos_lower: tuple[float, ...] + joint_pos_upper: tuple[float, ...] + neck_yaw_min_deg: float + neck_yaw_max_deg: float + neck_pitch_min_deg: float + neck_pitch_max_deg: float + + +def parse_high_level_policy_config(cfg: Any) -> HighLevelPolicyConfig: + policy_cfg = cfg_get(cfg, "high_level_policy", {}) or {} + endpoint = str(cfg_get(policy_cfg, "endpoint", "tcp://127.0.0.1:5555")).strip() + if not endpoint.startswith("tcp://"): + raise ValueError("high_level_policy.endpoint must be a tcp:// endpoint") + task = str(cfg_get(policy_cfg, "task", "")).strip() + if not task: + raise ValueError("high_level_policy.task must be a non-empty prompt") + if len(task.encode("utf-8")) > 1024: + raise ValueError("high_level_policy.task exceeds the protocol 1024-byte UTF-8 limit") + timeout_s = _positive_float(cfg_get(policy_cfg, "timeout_s", 1.0), "timeout_s") + reconnect_backoff_s = _positive_float( + cfg_get(policy_cfg, "reconnect_backoff_s", 1.0), "reconnect_backoff_s" + ) + replan_steps = int(cfg_get(policy_cfg, "replan_steps", 3)) + if not 1 <= replan_steps <= 15: + raise ValueError("high_level_policy.replan_steps must be in [1, 15]") + jpeg_quality = int(cfg_get(policy_cfg, "jpeg_quality", 90)) + if not 1 <= jpeg_quality <= 100: + raise ValueError("high_level_policy.jpeg_quality must be in [1, 100]") + max_observation_age_s = _positive_float( + cfg_get(policy_cfg, "max_observation_age_s", 0.15), "max_observation_age_s" + ) + max_result_age_s = _positive_float( + cfg_get(policy_cfg, "max_result_age_s", 0.1), "max_result_age_s" + ) + entry_timeout_s = _positive_float( + cfg_get(policy_cfg, "entry_timeout_s", 3.0), "entry_timeout_s" + ) + hold_s = float(cfg_get(policy_cfg, "hold_s", 0.1)) + if not math.isfinite(hold_s) or hold_s < 0.0: + raise ValueError("high_level_policy.hold_s must be finite and >= 0") + return HighLevelPolicyConfig( + endpoint=endpoint, + task=task, + timeout_s=timeout_s, + reconnect_backoff_s=reconnect_backoff_s, + replan_steps=replan_steps, + jpeg_quality=jpeg_quality, + max_observation_age_s=max_observation_age_s, + max_result_age_s=max_result_age_s, + entry_timeout_s=entry_timeout_s, + hold_s=hold_s, + ) + + +def parse_high_level_policy_camera_config(cfg: Any) -> HighLevelPolicyCameraConfig: + camera_cfg = cfg_get(cfg, "camera", {}) or {} + source = str(cfg_get(camera_cfg, "source", "realsense")).strip().lower() + if source not in ("realsense", "test-pattern"): + raise ValueError("camera.source must be realsense or test-pattern") + width = int(cfg_get(camera_cfg, "width", 640)) + height = int(cfg_get(camera_cfg, "height", 480)) + fps = int(cfg_get(camera_cfg, "fps", 30)) + if (width, height, fps) != (640, 480, 30): + raise ValueError( + "High-level policy camera must be exactly width=640, height=480, fps=30" + ) + device = cfg_get(camera_cfg, "device", None) + return HighLevelPolicyCameraConfig( + source=source, + width=width, + height=height, + fps=fps, + device=None if device in (None, "", "null") else str(device), + ) + + +def parse_high_level_policy_safety_config(cfg: Any) -> HighLevelPolicySafetyConfig: + policy_cfg = cfg_get(cfg, "high_level_policy", {}) or {} + safety_cfg = cfg_get(policy_cfg, "safety", {}) or {} + real_cfg = cfg_get(cfg, "real_robot", {}) or {} + + root_height_min_m = _finite_float( + cfg_get(safety_cfg, "root_height_min_m", 0.55), + "safety.root_height_min_m", + ) + root_height_max_m = _finite_float( + cfg_get(safety_cfg, "root_height_max_m", 1.05), + "safety.root_height_max_m", + ) + if root_height_min_m >= root_height_max_m: + raise ValueError( + "high_level_policy.safety.root_height_min_m must be less than root_height_max_m" + ) + + joint_pos_lower = _joint_limit_vector( + cfg_get(real_cfg, "joint_pos_lower", None), + "real_robot.joint_pos_lower", + ) + joint_pos_upper = _joint_limit_vector( + cfg_get(real_cfg, "joint_pos_upper", None), + "real_robot.joint_pos_upper", + ) + if np.any(np.asarray(joint_pos_lower) >= np.asarray(joint_pos_upper)): + raise ValueError("real_robot joint position lower limits must be below upper limits") + + neck_yaw_min_deg = _finite_float( + cfg_get(safety_cfg, "neck_yaw_min_deg", -45.0), + "safety.neck_yaw_min_deg", + ) + neck_yaw_max_deg = _finite_float( + cfg_get(safety_cfg, "neck_yaw_max_deg", 45.0), + "safety.neck_yaw_max_deg", + ) + neck_pitch_min_deg = _finite_float( + cfg_get(safety_cfg, "neck_pitch_min_deg", -40.0), + "safety.neck_pitch_min_deg", + ) + neck_pitch_max_deg = _finite_float( + cfg_get(safety_cfg, "neck_pitch_max_deg", 40.0), + "safety.neck_pitch_max_deg", + ) + if neck_yaw_min_deg >= neck_yaw_max_deg: + raise ValueError( + "high_level_policy.safety.neck_yaw_min_deg must be less than neck_yaw_max_deg" + ) + if neck_pitch_min_deg >= neck_pitch_max_deg: + raise ValueError( + "high_level_policy.safety.neck_pitch_min_deg must be less than neck_pitch_max_deg" + ) + + return HighLevelPolicySafetyConfig( + root_height_min_m=root_height_min_m, + root_height_max_m=root_height_max_m, + max_root_xy_speed_m_s=_positive_float( + cfg_get(safety_cfg, "max_root_xy_speed_m_s", 2.5), + "safety.max_root_xy_speed_m_s", + ), + max_root_displacement_m=_positive_float( + cfg_get(safety_cfg, "max_root_displacement_m", 0.1), + "safety.max_root_displacement_m", + ), + max_yaw_rate_rad_s=_positive_float( + cfg_get(safety_cfg, "max_yaw_rate_rad_s", 2.5), + "safety.max_yaw_rate_rad_s", + ), + max_joint_rate_rad_s=_positive_float( + cfg_get(safety_cfg, "max_joint_rate_rad_s", 10.0), + "safety.max_joint_rate_rad_s", + ), + joint_pos_lower=joint_pos_lower, + joint_pos_upper=joint_pos_upper, + neck_yaw_min_deg=neck_yaw_min_deg, + neck_yaw_max_deg=neck_yaw_max_deg, + neck_pitch_min_deg=neck_pitch_min_deg, + neck_pitch_max_deg=neck_pitch_max_deg, + ) + + +def _positive_float(value: object, name: str) -> float: + parsed = float(value) + if not math.isfinite(parsed) or parsed <= 0.0: + raise ValueError(f"high_level_policy.{name} must be finite and > 0") + return parsed + + +def _finite_float(value: object, name: str) -> float: + parsed = float(value) + if not math.isfinite(parsed): + raise ValueError(f"high_level_policy.{name} must be finite") + return parsed + + +def _joint_limit_vector(value: object, name: str) -> tuple[float, ...]: + if value is None: + raise ValueError( + f"{name} is required for high-level-policy action validation" + ) + array = np.asarray(value, dtype=np.float64).reshape(-1) + if array.shape != (29,) or not np.all(np.isfinite(array)): + raise ValueError(f"{name} must contain 29 finite values") + return tuple(float(item) for item in array) diff --git a/teleopit/high_level_policy/hand_calibration.json b/teleopit/high_level_policy/hand_calibration.json new file mode 100644 index 00000000..2779413f --- /dev/null +++ b/teleopit/high_level_policy/hand_calibration.json @@ -0,0 +1,5 @@ +{ + "open_raw": [250, 250, 250, 250, 250, 250], + "close_raw": [86, 73, 118, 111, 110, 111], + "range_tolerance": 0.0001 +} diff --git a/teleopit/high_level_policy/hand_calibration.py b/teleopit/high_level_policy/hand_calibration.py new file mode 100644 index 00000000..10cc7ecd --- /dev/null +++ b/teleopit/high_level_policy/hand_calibration.py @@ -0,0 +1,39 @@ +"""Load the LinkerHand calibration shared with the host policy service.""" + +from __future__ import annotations + +import json +import math +from dataclasses import dataclass +from importlib.resources import files + + +@dataclass(frozen=True) +class HandCalibration: + open_raw: tuple[float, ...] + close_raw: tuple[float, ...] + range_tolerance: float + + @classmethod + def load(cls) -> "HandCalibration": + path = files("teleopit.high_level_policy").joinpath("hand_calibration.json") + try: + document = json.loads(path.read_bytes()) + opened = tuple(float(value) for value in document["open_raw"]) + closed = tuple(float(value) for value in document["close_raw"]) + range_tolerance = float(document["range_tolerance"]) + except (OSError, json.JSONDecodeError, KeyError, TypeError, ValueError) as exc: + raise ValueError(f"Invalid hand_calibration.json: {exc}") from exc + + if len(opened) != 6 or len(closed) != 6: + raise ValueError("hand_calibration.json must define six open_raw and close_raw values") + if not all(math.isfinite(value) for value in (*opened, *closed, range_tolerance)): + raise ValueError("hand_calibration.json values must be finite") + if any(opened_value == closed_value for opened_value, closed_value in zip(opened, closed, strict=True)): + raise ValueError("Each hand_calibration.json open/close pair must differ") + + return cls( + open_raw=opened, + close_raw=closed, + range_tolerance=range_tolerance, + ) diff --git a/teleopit/high_level_policy/protocol.py b/teleopit/high_level_policy/protocol.py new file mode 100644 index 00000000..3f8813ff --- /dev/null +++ b/teleopit/high_level_policy/protocol.py @@ -0,0 +1,105 @@ +"""Serialization helpers and fixed limits for high-level-policy messages.""" + +from __future__ import annotations + +from typing import Any, Mapping + +import msgpack +import numpy as np + + +ENDPOINTS = frozenset({"ping", "describe", "reset", "get_action"}) +MAX_REQUEST_BYTES = 2_097_152 +MAX_RESPONSE_BYTES = 262_144 +MAX_IMAGE_BYTES = 1_572_864 +MAX_TASK_UTF8_BYTES = 1_024 +MAX_SESSION_ID_UTF8_BYTES = 128 +MAX_ACTION_HORIZON = 15 + + +class PolicyProtocolError(ValueError): + def __init__(self, code: str, message: str) -> None: + super().__init__(message) + self.code = str(code) + self.message = str(message) + + +def encode_float32_array(values: object) -> dict[str, object]: + array = np.asarray(values) + if not np.issubdtype(array.dtype, np.number): + raise PolicyProtocolError("invalid_array", f"Array dtype must be numeric, got {array.dtype}") + encoded = np.ascontiguousarray(array, dtype=" np.ndarray: + if not isinstance(value, Mapping) or set(value) != {"dtype", "shape", "data"}: + raise PolicyProtocolError("invalid_array", f"{name} must contain exactly dtype, shape, and data") + if value["dtype"] != " bytes: + try: + payload = msgpack.packb(dict(message), use_bin_type=True, strict_types=True) + except (TypeError, ValueError) as exc: + raise PolicyProtocolError("serialization_error", f"Cannot serialize message: {exc}") from exc + if len(payload) > max_bytes: + raise PolicyProtocolError( + "message_too_large", f"Serialized message has {len(payload)} bytes; limit is {max_bytes}" + ) + return payload + + +def unpack_message(payload: object, *, max_bytes: int) -> dict[str, Any]: + if not isinstance(payload, bytes): + raise PolicyProtocolError("invalid_message", "Protocol payload must be bytes") + if len(payload) > max_bytes: + raise PolicyProtocolError("message_too_large", f"Message has {len(payload)} bytes; limit is {max_bytes}") + try: + message = msgpack.unpackb( + payload, + raw=False, + strict_map_key=True, + max_bin_len=max_bytes, + max_str_len=max_bytes, + max_array_len=128, + max_map_len=64, + max_ext_len=0, + ) + except (msgpack.ExtraData, msgpack.FormatError, msgpack.StackError, ValueError) as exc: + raise PolicyProtocolError("invalid_msgpack", f"Cannot decode message: {exc}") from exc + if not isinstance(message, dict): + raise PolicyProtocolError("invalid_message", "Protocol message must be a map") + return message diff --git a/teleopit/high_level_policy/scheduler.py b/teleopit/high_level_policy/scheduler.py new file mode 100644 index 00000000..6e2a6176 --- /dev/null +++ b/teleopit/high_level_policy/scheduler.py @@ -0,0 +1,502 @@ +"""Session-local frame conversion and latency-aware 30 Hz action scheduling.""" + +from __future__ import annotations + +from dataclasses import dataclass +import math + +import numpy as np + +from teleopit.high_level_policy.client import PolicyActionChunk +from teleopit.high_level_policy.config import HighLevelPolicySafetyConfig +from teleopit.high_level_policy.hand_calibration import HandCalibration +from teleopit.math_utils import quat_inv_np, quat_mul_np +from teleopit.sim.reference_motion import interpolate_retarget_qpos + + +STATE_DIM = 68 +ACTION_DIM = 50 +BODY_ACTION_DIM = 36 +STATE_BASE_QUATERNION = slice(58, 62) +ROOT_QUATERNION = slice(3, 7) + + +def _normalized_quaternion(value: object, *, name: str) -> np.ndarray: + quaternion = np.asarray(value, dtype=np.float32).reshape(-1) + if quaternion.shape != (4,) or not np.all(np.isfinite(quaternion)): + raise ValueError(f"{name} must be a finite wxyz quaternion") + norm = float(np.linalg.norm(quaternion)) + if norm < 1e-8: + raise ValueError(f"{name} has a near-zero norm") + if abs(norm - 1.0) > 1e-3: + raise ValueError(f"{name} norm must be near 1, got {norm:.6g}") + return quaternion / np.float32(norm) + + +def _yaw_from_quaternion(value: object) -> float: + w, x, y, z = _normalized_quaternion(value, name="anchor quaternion") + return float(math.atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z))) + + +def _yaw_quaternion(yaw_rad: float) -> np.ndarray: + half = 0.5 * float(yaw_rad) + return np.array([math.cos(half), 0.0, 0.0, math.sin(half)], dtype=np.float32) + + +@dataclass(frozen=True) +class PolicyFrameTransform: + origin_xy: tuple[float, float] + yaw_rad: float + + @classmethod + def from_robot_pose(cls, root_xy: object, quaternion_wxyz: object) -> "PolicyFrameTransform": + xy = np.asarray(root_xy, dtype=np.float64).reshape(-1) + if xy.shape[0] < 2 or not np.all(np.isfinite(xy[:2])): + raise ValueError("Policy session root_xy must contain two finite values") + return cls( + origin_xy=(float(xy[0]), float(xy[1])), + yaw_rad=_yaw_from_quaternion(quaternion_wxyz), + ) + + def localize_state(self, state: object) -> np.ndarray: + localized = np.asarray(state, dtype=np.float32).reshape(-1).copy() + if localized.shape != (STATE_DIM,) or not np.all(np.isfinite(localized)): + raise ValueError(f"High-level policy state must be finite float32[{STATE_DIM}]") + base_quaternion = _normalized_quaternion( + localized[STATE_BASE_QUATERNION], name="state base quaternion" + ) + inverse_yaw = quat_inv_np(_yaw_quaternion(self.yaw_rad)) + localized_quaternion = quat_mul_np(inverse_yaw, base_quaternion) + localized[STATE_BASE_QUATERNION] = _normalized_quaternion( + localized_quaternion, name="localized state base quaternion" + ) + return localized + + def localize_body_action(self, action: object) -> np.ndarray: + body = np.asarray(action, dtype=np.float32).reshape(-1).copy() + if body.shape != (BODY_ACTION_DIM,) or not np.all(np.isfinite(body)): + raise ValueError(f"High-level body action must be finite float32[{BODY_ACTION_DIM}]") + world_delta = body[:2].astype(np.float64) - np.asarray(self.origin_xy, dtype=np.float64) + cosine = math.cos(self.yaw_rad) + sine = math.sin(self.yaw_rad) + body[0] = cosine * world_delta[0] + sine * world_delta[1] + body[1] = -sine * world_delta[0] + cosine * world_delta[1] + world_quaternion = _normalized_quaternion(body[ROOT_QUATERNION], name="action root quaternion") + local_quaternion = quat_mul_np(quat_inv_np(_yaw_quaternion(self.yaw_rad)), world_quaternion) + body[ROOT_QUATERNION] = _normalized_quaternion( + local_quaternion, name="localized action root quaternion" + ) + return body + + def delocalize_body_action(self, action: object) -> np.ndarray: + body = np.asarray(action, dtype=np.float32).reshape(-1).copy() + if body.shape != (BODY_ACTION_DIM,) or not np.all(np.isfinite(body)): + raise ValueError(f"High-level body action must be finite float32[{BODY_ACTION_DIM}]") + local_xy = body[:2].astype(np.float64) + cosine = math.cos(self.yaw_rad) + sine = math.sin(self.yaw_rad) + body[0] = cosine * local_xy[0] - sine * local_xy[1] + self.origin_xy[0] + body[1] = sine * local_xy[0] + cosine * local_xy[1] + self.origin_xy[1] + local_quaternion = _normalized_quaternion(body[ROOT_QUATERNION], name="action root quaternion") + world_quaternion = quat_mul_np(_yaw_quaternion(self.yaw_rad), local_quaternion) + body[ROOT_QUATERNION] = _normalized_quaternion( + world_quaternion, name="delocalized action root quaternion" + ) + return body + + +class HighLevelPolicyScheduler: + def __init__( + self, + *, + hold_s: float = 0.1, + safety: HighLevelPolicySafetyConfig | None = None, + output_hz: float = 50.0, + ) -> None: + if not np.isfinite(hold_s) or hold_s < 0.0: + raise ValueError("high_level_policy.hold_s must be finite and >= 0") + if not np.isfinite(output_hz) or output_hz <= 0.0: + raise ValueError("High-level policy scheduler output_hz must be finite and > 0") + self.hold_s = float(hold_s) + self.safety = safety + self.output_hz = float(output_hz) + self._session_id: str | None = None + self._chunk: PolicyActionChunk | None = None + self._last_source_sequence_id = -1 + self._last_source_timestamp_ns = -1 + self._paused_at_s: float | None = None + self._timestamp_shift_s = 0.0 + self._initial_action: np.ndarray | None = None + self._last_output_action: np.ndarray | None = None + + @property + def session_id(self) -> str | None: + return self._session_id + + @property + def has_chunk(self) -> bool: + return self._chunk is not None + + @property + def paused(self) -> bool: + return self._paused_at_s is not None + + def reset(self, session_id: str, *, initial_action: object | None = None) -> None: + if not isinstance(session_id, str) or not session_id: + raise ValueError("High-level policy session_id must be non-empty") + self._session_id = session_id + self._chunk = None + self._last_source_sequence_id = -1 + self._last_source_timestamp_ns = -1 + self._paused_at_s = None + self._timestamp_shift_s = 0.0 + self._initial_action = ( + None + if initial_action is None + else self._validate_single_action(initial_action, name="initial_action") + ) + self._last_output_action = ( + None if self._initial_action is None else self._initial_action.copy() + ) + + def clear(self) -> None: + self._session_id = None + self._chunk = None + self._last_source_sequence_id = -1 + self._last_source_timestamp_ns = -1 + self._paused_at_s = None + self._timestamp_shift_s = 0.0 + self._initial_action = None + self._last_output_action = None + + def accept(self, chunk: PolicyActionChunk, *, now_s: float) -> None: + if not np.isfinite(now_s): + raise ValueError("High-level policy scheduler now_s must be finite") + if self._session_id is None or chunk.session_id != self._session_id: + raise ValueError( + f"High-level policy action session mismatch: active={self._session_id!r}, " + f"received={chunk.session_id!r}" + ) + if ( + not isinstance(chunk.source_sequence_id, int) + or isinstance(chunk.source_sequence_id, bool) + or chunk.source_sequence_id < 0 + ): + raise ValueError("High-level policy source sequence must be a non-negative integer") + if chunk.source_sequence_id <= self._last_source_sequence_id: + raise ValueError( + "High-level policy source sequence must increase: " + f"last={self._last_source_sequence_id}, received={chunk.source_sequence_id}" + ) + if not isinstance(chunk.action_fps, int) or isinstance(chunk.action_fps, bool) or chunk.action_fps != 30: + raise ValueError(f"High-level policy action_fps must be 30, got {chunk.action_fps}") + if ( + not isinstance(chunk.source_onboard_monotonic_timestamp_ns, int) + or isinstance(chunk.source_onboard_monotonic_timestamp_ns, bool) + or not 0 <= chunk.source_onboard_monotonic_timestamp_ns <= 2**63 - 1 + ): + raise ValueError("High-level policy source timestamp must be a non-negative int64") + if chunk.source_onboard_monotonic_timestamp_ns <= self._last_source_timestamp_ns: + raise ValueError( + "High-level policy source timestamp must increase: " + f"last={self._last_source_timestamp_ns}, " + f"received={chunk.source_onboard_monotonic_timestamp_ns}" + ) + if not isinstance(chunk.policy_id, str) or not chunk.policy_id: + raise ValueError("High-level policy policy_id must be non-empty") + if not np.isfinite(chunk.server_inference_ms) or chunk.server_inference_ms < 0.0: + raise ValueError("High-level policy server_inference_ms must be finite and >= 0") + source_s = chunk.source_onboard_monotonic_timestamp_ns * 1e-9 + if source_s > float(now_s) + 0.001: + raise ValueError( + "High-level policy source timestamp is in the future: " + f"source={source_s:.9f}s now={float(now_s):.9f}s" + ) + boundary_action = self._sample_unlimited(source_s) + if boundary_action is None: + boundary_action = ( + self._initial_action + if self._chunk is None + else self._chunk.actions[-1].copy() + ) + actions = self._validate_actions( + chunk.actions, + action_fps=chunk.action_fps, + boundary_action=boundary_action, + ) + valid_until_s = source_s + len(actions) / float(chunk.action_fps) + self.hold_s + if float(now_s) > valid_until_s: + raise ValueError( + "High-level policy action chunk is already expired: " + f"age={float(now_s) - source_s:.3f}s horizon={len(actions) / chunk.action_fps:.3f}s" + ) + self._chunk = PolicyActionChunk( + session_id=chunk.session_id, + source_sequence_id=chunk.source_sequence_id, + source_onboard_monotonic_timestamp_ns=chunk.source_onboard_monotonic_timestamp_ns, + action_fps=chunk.action_fps, + actions=actions, + policy_id=chunk.policy_id, + server_inference_ms=chunk.server_inference_ms, + ) + self._last_source_sequence_id = chunk.source_sequence_id + self._last_source_timestamp_ns = chunk.source_onboard_monotonic_timestamp_ns + self._timestamp_shift_s = 0.0 + if self._paused_at_s is not None: + self._paused_at_s = float(now_s) + + def pause(self, now_s: float) -> None: + if self._paused_at_s is None: + self._paused_at_s = float(now_s) + + def resume(self, now_s: float) -> None: + if self._paused_at_s is None: + return + self._timestamp_shift_s += max(0.0, float(now_s) - self._paused_at_s) + self._paused_at_s = None + + def sample(self, now_s: float) -> np.ndarray | None: + desired = self._sample_unlimited(now_s) + if desired is None: + return None + previous = self._last_output_action + safety = self.safety + if previous is not None and safety is not None: + desired = self._rate_limit_output(previous, desired, safety=safety) + self._last_output_action = desired.copy() + return desired + + def _sample_unlimited(self, now_s: float) -> np.ndarray | None: + chunk = self._chunk + if chunk is None: + return None + effective_now_s = self._paused_at_s if self._paused_at_s is not None else float(now_s) + source_s = chunk.source_onboard_monotonic_timestamp_ns * 1e-9 + self._timestamp_shift_s + frame_f = (effective_now_s - source_s) * float(chunk.action_fps) + if frame_f <= 0.0: + return chunk.actions[0].copy() + last_index = len(chunk.actions) - 1 + if frame_f >= float(last_index): + valid_until_s = source_s + len(chunk.actions) / float(chunk.action_fps) + self.hold_s + if effective_now_s > valid_until_s: + return None + return chunk.actions[last_index].copy() + index0 = int(math.floor(frame_f)) + index1 = min(index0 + 1, last_index) + alpha = float(frame_f - index0) + interpolated = interpolate_retarget_qpos( + np.asarray(chunk.actions[index0], dtype=np.float64), + np.asarray(chunk.actions[index1], dtype=np.float64), + alpha, + ) + return np.asarray(interpolated, dtype=np.float32) + + def _rate_limit_output( + self, + previous: np.ndarray, + desired: np.ndarray, + *, + safety: HighLevelPolicySafetyConfig, + ) -> np.ndarray: + output = np.asarray(desired, dtype=np.float32).copy() + previous = self._validate_single_action(previous, name="previous output action") + + root_delta = output[0:3].astype(np.float64) - previous[0:3].astype(np.float64) + max_root_delta = safety.max_root_displacement_m * 30.0 / self.output_hz + root_distance = float(np.linalg.norm(root_delta)) + if root_distance > max_root_delta: + root_delta *= max_root_delta / root_distance + max_xy_delta = safety.max_root_xy_speed_m_s / self.output_hz + xy_distance = float(np.linalg.norm(root_delta[:2])) + if xy_distance > max_xy_delta: + root_delta[:2] *= max_xy_delta / xy_distance + output[0:3] = previous[0:3] + root_delta.astype(np.float32) + + previous_yaw = _yaw_from_quaternion(previous[ROOT_QUATERNION]) + desired_quaternion = _normalized_quaternion( + output[ROOT_QUATERNION], name="desired output root quaternion" + ) + desired_yaw = _yaw_from_quaternion(desired_quaternion) + yaw_delta = math.atan2( + math.sin(desired_yaw - previous_yaw), + math.cos(desired_yaw - previous_yaw), + ) + max_yaw_delta = safety.max_yaw_rate_rad_s / self.output_hz + limited_yaw = previous_yaw + float(np.clip(yaw_delta, -max_yaw_delta, max_yaw_delta)) + desired_tilt = quat_mul_np( + quat_inv_np(_yaw_quaternion(desired_yaw)), + desired_quaternion, + ) + limited_quaternion = _normalized_quaternion( + quat_mul_np(_yaw_quaternion(limited_yaw), desired_tilt), + name="rate-limited output root quaternion", + ) + if float(np.dot(previous[ROOT_QUATERNION], limited_quaternion)) < 0.0: + limited_quaternion = -limited_quaternion + output[ROOT_QUATERNION] = limited_quaternion + + max_joint_delta = safety.max_joint_rate_rad_s / self.output_hz + output[7:36] = previous[7:36] + np.clip( + output[7:36] - previous[7:36], + -max_joint_delta, + max_joint_delta, + ) + return output + + def _validate_actions( + self, + values: object, + *, + action_fps: int, + boundary_action: np.ndarray | None, + ) -> np.ndarray: + actions = np.asarray(values) + if actions.ndim != 2 or actions.shape[1] != ACTION_DIM or not 1 <= len(actions) <= 15: + raise ValueError(f"High-level policy actions must have shape [T, {ACTION_DIM}] with T in [1, 15]") + if not np.issubdtype(actions.dtype, np.number) or not np.all(np.isfinite(actions)): + raise ValueError("High-level policy actions must be finite numeric values") + validated = np.ascontiguousarray(actions, dtype=np.float32) + previous: np.ndarray | None = None + for index in range(len(validated)): + quaternion = _normalized_quaternion( + validated[index, ROOT_QUATERNION], name=f"action[{index}] root quaternion" + ) + if previous is not None and float(np.dot(previous, quaternion)) < 0.0: + quaternion = -quaternion + validated[index, ROOT_QUATERNION] = quaternion + previous = quaternion + hand = validated[:, 36:48] + if float(np.min(hand)) < 0.0 or float(np.max(hand)) > 1.0: + raise ValueError("High-level policy LinkerHand closure must be within [0, 1]") + safety = self.safety + if safety is not None: + self._validate_safety_limits( + validated, + action_fps=action_fps, + boundary_action=boundary_action, + safety=safety, + ) + return validated + + @staticmethod + def _validate_single_action(values: object, *, name: str) -> np.ndarray: + action = np.asarray(values) + if action.shape != (ACTION_DIM,) or not np.issubdtype(action.dtype, np.number): + raise ValueError(f"{name} must be a numeric float32[{ACTION_DIM}]") + action = np.ascontiguousarray(action, dtype=np.float32) + if not np.all(np.isfinite(action)): + raise ValueError(f"{name} must contain only finite values") + action[ROOT_QUATERNION] = _normalized_quaternion( + action[ROOT_QUATERNION], name=f"{name} root quaternion" + ) + return action + + @staticmethod + def _validate_safety_limits( + actions: np.ndarray, + *, + action_fps: int, + boundary_action: np.ndarray | None, + safety: HighLevelPolicySafetyConfig, + ) -> None: + root_height = actions[:, 2] + if ( + float(np.min(root_height)) < safety.root_height_min_m + or float(np.max(root_height)) > safety.root_height_max_m + ): + raise ValueError( + "High-level policy root height is outside " + f"[{safety.root_height_min_m}, {safety.root_height_max_m}] m" + ) + + joints = actions[:, 7:36] + lower = np.asarray(safety.joint_pos_lower, dtype=np.float32) + upper = np.asarray(safety.joint_pos_upper, dtype=np.float32) + violations = np.argwhere((joints < lower[None, :]) | (joints > upper[None, :])) + if len(violations): + frame, joint = (int(value) for value in violations[0]) + raise ValueError( + "High-level policy joint position exceeds real_robot limits: " + f"action[{frame}, {7 + joint}]={float(joints[frame, joint]):.6g}, " + f"range=[{float(lower[joint]):.6g}, {float(upper[joint]):.6g}]" + ) + + yaw = actions[:, 48] + pitch = actions[:, 49] + if float(np.min(yaw)) < safety.neck_yaw_min_deg or float(np.max(yaw)) > safety.neck_yaw_max_deg: + raise ValueError( + "High-level policy OpenNeck yaw is outside " + f"[{safety.neck_yaw_min_deg}, {safety.neck_yaw_max_deg}] degrees" + ) + if ( + float(np.min(pitch)) < safety.neck_pitch_min_deg + or float(np.max(pitch)) > safety.neck_pitch_max_deg + ): + raise ValueError( + "High-level policy OpenNeck pitch is outside " + f"[{safety.neck_pitch_min_deg}, {safety.neck_pitch_max_deg}] degrees" + ) + + sequence = actions + if boundary_action is not None: + baseline = HighLevelPolicyScheduler._validate_single_action( + boundary_action, + name="boundary_action", + ) + if float(np.dot(baseline[ROOT_QUATERNION], sequence[0, ROOT_QUATERNION])) < 0.0: + baseline[ROOT_QUATERNION] *= -1.0 + sequence = np.concatenate((baseline[None, :], actions), axis=0) + if len(sequence) < 2: + return + + root_delta = np.diff(sequence[:, 0:3], axis=0) + displacement = np.linalg.norm(root_delta, axis=1) + max_displacement = float(np.max(displacement)) + if max_displacement > safety.max_root_displacement_m: + raise ValueError( + "High-level policy root per-frame displacement exceeds limit: " + f"{max_displacement:.6g} > {safety.max_root_displacement_m:.6g} m" + ) + xy_speed = np.linalg.norm(root_delta[:, :2], axis=1) * float(action_fps) + max_xy_speed = float(np.max(xy_speed)) + if max_xy_speed > safety.max_root_xy_speed_m_s: + raise ValueError( + "High-level policy root XY speed exceeds limit: " + f"{max_xy_speed:.6g} > {safety.max_root_xy_speed_m_s:.6g} m/s" + ) + + yaws = np.asarray( + [_yaw_from_quaternion(row[ROOT_QUATERNION]) for row in sequence], + dtype=np.float64, + ) + yaw_delta = np.arctan2(np.sin(np.diff(yaws)), np.cos(np.diff(yaws))) + max_yaw_rate = float(np.max(np.abs(yaw_delta))) * float(action_fps) + if max_yaw_rate > safety.max_yaw_rate_rad_s: + raise ValueError( + "High-level policy root yaw rate exceeds limit: " + f"{max_yaw_rate:.6g} > {safety.max_yaw_rate_rad_s:.6g} rad/s" + ) + + joint_rate = np.abs(np.diff(sequence[:, 7:36], axis=0)) * float(action_fps) + max_joint_rate = float(np.max(joint_rate)) + if max_joint_rate > safety.max_joint_rate_rad_s: + raise ValueError( + "High-level policy joint rate exceeds limit: " + f"{max_joint_rate:.6g} > {safety.max_joint_rate_rad_s:.6g} rad/s" + ) + + +def closure_to_o6_pose( + closure: object, + calibration: HandCalibration | None = None, +) -> tuple[int, ...]: + calibration = calibration or HandCalibration.load() + values = np.asarray(closure, dtype=np.float32).reshape(-1) + if values.shape != (6,) or not np.all(np.isfinite(values)): + raise ValueError("LinkerHand O6 closure must contain six finite values") + if float(np.min(values)) < 0.0 or float(np.max(values)) > 1.0: + raise ValueError("LinkerHand O6 closure must be within [0, 1]") + opened = np.asarray(calibration.open_raw, dtype=np.float32) + closed = np.asarray(calibration.close_raw, dtype=np.float32) + raw = np.rint(opened - values * (opened - closed)).astype(np.int64) + return tuple(int(value) for value in raw) diff --git a/teleopit/runtime/console.py b/teleopit/runtime/console.py index 2b6d7b12..07b2d894 100644 --- a/teleopit/runtime/console.py +++ b/teleopit/runtime/console.py @@ -200,6 +200,7 @@ def _highlight_text(self, text: str) -> str: "MOCAP": GREEN + BOLD, "STANDING": GREEN + BOLD, "ARMS": MAGENTA + BOLD, + "POLICY": CYAN + BOLD, } for word, code in replacements.items(): highlighted = highlighted.replace(word, f"{code}{word}{RESET}") @@ -256,6 +257,7 @@ def sim2real_operator_controls(cfg: Any) -> tuple[KeyboardControl, ...]: if provider == "pico4": controls.extend( [ + KeyboardControl("Remote B", "pause/resume"), KeyboardControl("Pico/Controller A", "pause/resume"), KeyboardControl("Pico/Controller B", "arms"), ] @@ -269,3 +271,13 @@ def sim2real_operator_controls(cfg: Any) -> tuple[KeyboardControl, ...]: ) controls.extend(sim2real_keyboard_controls(cfg)) return tuple(controls) + + +def high_level_policy_operator_controls() -> tuple[KeyboardControl, ...]: + return ( + KeyboardControl("Remote Start", "standing"), + KeyboardControl("Remote Y", "policy takeover"), + KeyboardControl("Remote B", "pause/resume"), + KeyboardControl("Remote X", "standing"), + KeyboardControl("Remote L1+R1", "damping / estop"), + ) diff --git a/teleopit/sim2real/__init__.py b/teleopit/sim2real/__init__.py index 2c22fb9c..bb7a6c39 100644 --- a/teleopit/sim2real/__init__.py +++ b/teleopit/sim2real/__init__.py @@ -1,5 +1,6 @@ __all__ = [ "Sim2RealRuntime", + "HighLevelPolicySim2RealRuntime", "UnitreeG1Robot", "UnitreeRemote", "Button", @@ -11,6 +12,10 @@ def __getattr__(name: str): from teleopit.sim2real.mp import Sim2RealRuntime return Sim2RealRuntime + if name == "HighLevelPolicySim2RealRuntime": + from teleopit.sim2real.mp import HighLevelPolicySim2RealRuntime + + return HighLevelPolicySim2RealRuntime if name == "UnitreeG1Robot": from teleopit.sim2real.unitree_g1 import UnitreeG1Robot diff --git a/teleopit/sim2real/mp/__init__.py b/teleopit/sim2real/mp/__init__.py index f32f55cd..3f55ff45 100644 --- a/teleopit/sim2real/mp/__init__.py +++ b/teleopit/sim2real/mp/__init__.py @@ -3,7 +3,9 @@ from teleopit.sim2real.mp.runtime import ( Sim2RealRuntime, ) +from teleopit.sim2real.mp.high_level_policy_runtime import HighLevelPolicySim2RealRuntime __all__ = [ "Sim2RealRuntime", + "HighLevelPolicySim2RealRuntime", ] diff --git a/teleopit/sim2real/mp/high_level_policy_runtime.py b/teleopit/sim2real/mp/high_level_policy_runtime.py new file mode 100644 index 00000000..16bcf30a --- /dev/null +++ b/teleopit/sim2real/mp/high_level_policy_runtime.py @@ -0,0 +1,473 @@ +"""Independent high-level-policy sim2real process assembly. + +This runtime deliberately does not start PicoBridge, GMR, or a reference +worker. It owns one RealSense stream and sends host-policy body references +through Teleopit's existing motion tracker. +""" + +from __future__ import annotations + +import logging +import multiprocessing as mp +from multiprocessing.synchronize import Event as MpEvent +import time +from typing import Any, Callable + +import numpy as np + +from teleopit.high_level_policy.config import ( + parse_high_level_policy_camera_config, + parse_high_level_policy_config, + parse_high_level_policy_safety_config, +) +from teleopit.high_level_policy.hand_calibration import HandCalibration +from teleopit.high_level_policy.scheduler import closure_to_o6_pose +from teleopit.runtime.common import cfg_get +from teleopit.runtime.console import OPERATOR_LOGGER_NAME, PlainConsole +from teleopit.sim2real.hands.linkerhand_o6 import ( + LinkerHandO6Device, + parse_linkerhand_o6_config, +) +from teleopit.sim2real.mp.high_level_policy_worker import HighLevelPolicyWorker +from teleopit.sim2real.mp.ipc import ( + COMMAND_TOPIC, + HIGH_LEVEL_POLICY_TARGET_TOPIC, + MODE_TOPIC, + VIDEO_TOPIC, + LatestSubscriber, + Sim2RealIpcEndpoints, + ZmqPublisher, + default_endpoints, +) +from teleopit.sim2real.mp.messages import ( + CommandPacket, + HighLevelPolicyTargetPacket, + ModeStatePacket, +) +from teleopit.sim2real.mp.runtime import ( + HIGH_LEVEL_POLICY_FAULT_COMMAND, + _mp_cfg, + _plain_cfg, + _run_robot_control_worker, + _worker_loop, +) +from teleopit.sim2real.mp.shm import SharedFrameRingWriter +from teleopit.sim2real.neck.config import parse_neck_config +from teleopit.sim2real.neck.openneck import build_neck_device + + +logger = logging.getLogger(__name__) +operator_logger = logging.getLogger(OPERATOR_LOGGER_NAME) + + +class HighLevelPolicySim2RealRuntime: + def __init__(self, cfg: Any, *, console: PlainConsole | None = None) -> None: + self.cfg = _plain_cfg(cfg) + _validate_high_level_policy_runtime_config(self.cfg) + runtime_cfg = _mp_cfg(self.cfg) + self._ctx = mp.get_context(str(cfg_get(runtime_cfg, "start_method", "spawn"))) + self._stop_event = self._ctx.Event() + self._processes: list[mp.Process] = [] + self._shutdown_timeout_s = float(cfg_get(runtime_cfg, "shutdown_timeout_s", 3.0)) + self._endpoints = default_endpoints( + host=str(cfg_get(runtime_cfg, "host", "127.0.0.1")), + base_port=int(cfg_get(runtime_cfg, "base_port", 39700)), + ) + self._command_pub: ZmqPublisher | None = None + self._console = console or PlainConsole(title="Teleopit high-level policy", enabled=False) + + def run(self) -> None: + operator_logger.info("high-level policy runtime starting") + try: + self._start_processes() + self._command_pub = ZmqPublisher(self._endpoints.command_pub) + reported_dead: set[str] = set() + while not self._stop_event.is_set(): + time.sleep(0.2) + critical_dead = [ + process.name + for process in self._processes + if process.name == "robot_control" + and not process.is_alive() + and process.exitcode not in (None, 0) + ] + if critical_dead: + operator_logger.error("critical worker exited: %s", ", ".join(critical_dead)) + self._stop_event.set() + break + required_input_dead = [ + process.name + for process in self._processes + if process.name in {"camera", "high_level_policy"} + and not process.is_alive() + and process.exitcode is not None + ] + if required_input_dead and self._command_pub is not None: + detail = ( + "required high-level-policy input worker exited: " + + ", ".join(required_input_dead) + ) + self._command_pub.publish( + COMMAND_TOPIC, + CommandPacket( + command=HIGH_LEVEL_POLICY_FAULT_COMMAND, + timestamp_s=time.monotonic(), + payload={"detail": detail}, + ), + ) + noncritical_dead = [ + process.name + for process in self._processes + if process.name != "robot_control" + and not process.is_alive() + and process.exitcode is not None + and process.name not in reported_dead + ] + if noncritical_dead: + operator_logger.warning( + "non-critical worker exited: %s; G1 remains under local control", + ", ".join(noncritical_dead), + ) + reported_dead.update(noncritical_dead) + except KeyboardInterrupt: + operator_logger.info("keyboard interrupt -> shutting down") + self._stop_event.set() + finally: + self.shutdown() + + def shutdown(self) -> None: + self._stop_event.set() + if self._command_pub is not None: + self._command_pub.publish( + COMMAND_TOPIC, + CommandPacket(command="shutdown", timestamp_s=time.monotonic()), + ) + for process in self._processes: + process.join(timeout=self._shutdown_timeout_s) + for process in self._processes: + if process.is_alive(): + operator_logger.warning("terminating worker %s", process.name) + process.terminate() + process.join(timeout=1.0) + self._processes.clear() + if self._command_pub is not None: + self._command_pub.close() + self._command_pub = None + + def _start_processes(self) -> None: + if self._processes: + return + specs: list[tuple[str, Callable[..., None]]] = [ + ("camera", _run_high_level_policy_camera_worker), + ("high_level_policy", _run_high_level_policy_client_worker), + ("robot_control", _run_robot_control_worker), + ] + hands_cfg = cfg_get(self.cfg, "hands", {}) or {} + if bool(cfg_get(hands_cfg, "enabled", False)): + specs.append(("policy_hand", _run_high_level_policy_hand_worker)) + neck_cfg = parse_neck_config(self.cfg) + if neck_cfg.enabled: + specs.append(("policy_neck", _run_high_level_policy_neck_worker)) + for name, target in specs: + process = self._ctx.Process( + name=name, + target=target, + args=(self.cfg, self._endpoints, self._stop_event), + ) + process.start() + self._processes.append(process) + + +def _validate_high_level_policy_runtime_config(cfg: dict[str, Any]) -> None: + input_cfg = cfg_get(cfg, "input", {}) or {} + if str(cfg_get(input_cfg, "provider", "")).strip().lower() != "high_level_policy": + raise ValueError( + "HighLevelPolicySim2RealRuntime requires input.provider=high_level_policy" + ) + policy_cfg = cfg_get(cfg, "high_level_policy", {}) or {} + if not bool(cfg_get(policy_cfg, "enabled", False)): + raise ValueError("HighLevelPolicySim2RealRuntime requires high_level_policy.enabled=true") + parse_high_level_policy_config(cfg) + parse_high_level_policy_camera_config(cfg) + parse_high_level_policy_safety_config(cfg) + reference_steps = tuple(int(value) for value in cfg_get(cfg, "reference_steps", [0])) + if reference_steps != (0,): + raise ValueError("High-level policy sim2real requires reference_steps=[0]") + recording_cfg = cfg_get(cfg, "recording", {}) or {} + if bool(cfg_get(recording_cfg, "enabled", False)): + raise ValueError("High-level policy recording is not supported in the initial runtime") + + calibration = HandCalibration.load() + hands_cfg = cfg_get(cfg, "hands", {}) or {} + if not bool(cfg_get(hands_cfg, "enabled", False)): + raise ValueError("High-level policy action[36:48] requires hands.enabled=true") + if str(cfg_get(hands_cfg, "driver", "")).strip().lower() != "linkerhand_o6": + raise ValueError("High-level policy requires hands.driver=linkerhand_o6") + hand_config = parse_linkerhand_o6_config(cfg) + if len(hand_config.sides) != 2 or set(hand_config.sides) != {"left", "right"}: + raise ValueError("High-level policy requires hands.sides=[left, right]") + if tuple(float(value) for value in hand_config.open_pose) != calibration.open_raw: + raise ValueError("LinkerHand O6 open_pose does not match hand_calibration.json") + if tuple(float(value) for value in hand_config.close_pose) != calibration.close_raw: + raise ValueError("LinkerHand O6 close_pose does not match hand_calibration.json") + + neck_cfg = parse_neck_config(cfg) + if not neck_cfg.enabled or neck_cfg.driver != "openneck": + raise ValueError("High-level policy action[48:50] requires neck.enabled=true and driver=openneck") + + +def _run_high_level_policy_client_worker( + cfg: dict[str, Any], endpoints: Sim2RealIpcEndpoints, stop_event: MpEvent +) -> None: + def _main() -> None: + HighLevelPolicyWorker(cfg, endpoints, stop_event).run() + + _worker_loop("high_level_policy", cfg, _main) + + +def _run_high_level_policy_camera_worker( + cfg: dict[str, Any], endpoints: Sim2RealIpcEndpoints, stop_event: MpEvent +) -> None: + def _main() -> None: + camera_cfg = parse_high_level_policy_camera_config(cfg) + runtime_cfg = _mp_cfg(cfg) + publisher = ZmqPublisher(endpoints.video_pub) + command_sub = LatestSubscriber(endpoints.command_pub, COMMAND_TOPIC) + writer = SharedFrameRingWriter( + shape=(camera_cfg.height, camera_cfg.width, 3), + dtype=np.uint8, + slots=int(cfg_get(runtime_cfg, "video_slots", 3)), + ) + pipeline: Any | None = None + try: + if camera_cfg.source == "realsense": + try: + import pyrealsense2 as rs + except ImportError as exc: + raise RuntimeError( + "RealSense high-level-policy camera requires pyrealsense2" + ) from exc + pipeline = rs.pipeline() + rs_config = rs.config() + if camera_cfg.device is not None: + rs_config.enable_device(camera_cfg.device) + rs_config.enable_stream( + rs.stream.color, + camera_cfg.width, + camera_cfg.height, + rs.format.rgb8, + camera_cfg.fps, + ) + pipeline.start(rs_config) + period_s = 1.0 / float(camera_cfg.fps) + test_frame_index = 0 + while not stop_event.is_set(): + command = command_sub.recv_latest() + if isinstance(command, CommandPacket) and command.command == "shutdown": + break + started_s = time.monotonic() + if pipeline is None: + frame = _test_pattern( + camera_cfg.height, + camera_cfg.width, + test_frame_index, + ) + test_frame_index += 1 + else: + try: + frames = pipeline.wait_for_frames(timeout_ms=1000) + except RuntimeError: + continue + color = frames.get_color_frame() + if not color: + continue + frame = np.ascontiguousarray( + np.asanyarray(color.get_data()), + dtype=np.uint8, + ) + timestamp_s = time.monotonic() + descriptor = writer.write(frame, timestamp_s=timestamp_s) + publisher.publish(VIDEO_TOPIC, descriptor) + if pipeline is None: + elapsed_s = time.monotonic() - started_s + if elapsed_s < period_s: + time.sleep(period_s - elapsed_s) + finally: + if pipeline is not None: + pipeline.stop() + writer.close(unlink=True) + command_sub.close() + publisher.close() + + _worker_loop("camera", cfg, _main) + + +def _test_pattern(height: int, width: int, frame_index: int) -> np.ndarray: + x = np.linspace(0, 255, width, dtype=np.uint8) + y = np.linspace(0, 255, height, dtype=np.uint8)[:, None] + frame = np.empty((height, width, 3), dtype=np.uint8) + frame[:, :, 0] = x[None, :] + frame[:, :, 1] = y + frame[:, :, 2] = np.uint8(frame_index % 256) + return frame + + +def _policy_target_action(target: HighLevelPolicyTargetPacket) -> np.ndarray: + action = np.asarray(target.action, dtype=np.float32).reshape(-1) + if action.shape != (50,) or not np.all(np.isfinite(action)): + raise ValueError("High-level policy hardware worker received an invalid 50D target") + return action + + +def _policy_target_is_current( + target: object, + mode: ModeStatePacket | None, + *, + last_target_seq: int, + max_age_s: float, + now_s: float | None = None, +) -> bool: + if not isinstance(target, HighLevelPolicyTargetPacket): + return False + if mode is None or mode.mode != "policy" or mode.policy_paused: + return False + if mode.policy_session_id is None or target.session_id != mode.policy_session_id: + return False + if ( + not isinstance(target.seq, int) + or isinstance(target.seq, bool) + or target.seq <= last_target_seq + ): + return False + current_s = time.monotonic() if now_s is None else float(now_s) + age_s = current_s - float(target.timestamp_s) + return bool(np.isfinite(age_s) and 0.0 <= age_s <= float(max_age_s)) + + +def _apply_policy_hand_target( + device: LinkerHandO6Device, + target: HighLevelPolicyTargetPacket, + calibration: HandCalibration, +) -> None: + action = _policy_target_action(target) + device.send_pose( + "left", + closure_to_o6_pose(action[36:42], calibration), + reason="policy", + ) + device.send_pose( + "right", + closure_to_o6_pose(action[42:48], calibration), + reason="policy", + ) + + +def _apply_policy_neck_target(device: Any, target: HighLevelPolicyTargetPacket) -> None: + action = _policy_target_action(target) + device.move_deg(float(action[48]), float(action[49])) + + +def _run_high_level_policy_hand_worker( + cfg: dict[str, Any], endpoints: Sim2RealIpcEndpoints, stop_event: MpEvent +) -> None: + def _main() -> None: + config = parse_linkerhand_o6_config(cfg) + device = LinkerHandO6Device(config) + calibration = HandCalibration.load() + target_sub = LatestSubscriber( + endpoints.high_level_policy_control_pub, + HIGH_LEVEL_POLICY_TARGET_TOPIC, + ) + mode_sub = LatestSubscriber(endpoints.mode_pub, MODE_TOPIC) + command_sub = LatestSubscriber(endpoints.command_pub, COMMAND_TOPIC) + latest_mode: ModeStatePacket | None = None + last_target_seq = -1 + was_in_policy = False + sleep_s = 1.0 / max(float(cfg_get(_mp_cfg(cfg), "hand_worker_hz", 120.0)), 1.0) + try: + device.connect() + while not stop_event.is_set(): + command = command_sub.recv_latest() + if isinstance(command, CommandPacket) and command.command == "shutdown": + break + mode = mode_sub.recv_latest() + if isinstance(mode, ModeStatePacket): + latest_mode = mode + in_policy = bool(latest_mode is not None and latest_mode.mode == "policy") + if was_in_policy and not in_policy: + device.open_all(force=True, reason="policy-inactive") + was_in_policy = in_policy + target = target_sub.recv_latest() + if _policy_target_is_current( + target, + latest_mode, + last_target_seq=last_target_seq, + max_age_s=config.frame_timeout_s, + ): + _apply_policy_hand_target(device, target, calibration) + last_target_seq = int(target.seq) + time.sleep(sleep_s) + finally: + try: + device.close() + finally: + target_sub.close() + mode_sub.close() + command_sub.close() + + _worker_loop("policy_hand", cfg, _main) + + +def _run_high_level_policy_neck_worker( + cfg: dict[str, Any], endpoints: Sim2RealIpcEndpoints, stop_event: MpEvent +) -> None: + def _main() -> None: + config = parse_neck_config(cfg) + device = build_neck_device(config) + target_sub = LatestSubscriber( + endpoints.high_level_policy_control_pub, + HIGH_LEVEL_POLICY_TARGET_TOPIC, + ) + mode_sub = LatestSubscriber(endpoints.mode_pub, MODE_TOPIC) + command_sub = LatestSubscriber(endpoints.command_pub, COMMAND_TOPIC) + latest_mode: ModeStatePacket | None = None + last_target_seq = -1 + was_in_policy = False + sleep_s = 1.0 / max(config.rate_hz, 1.0) + try: + device.connect() + if config.center_on_start: + device.center() + while not stop_event.is_set(): + command = command_sub.recv_latest() + if isinstance(command, CommandPacket) and command.command == "shutdown": + break + mode = mode_sub.recv_latest() + if isinstance(mode, ModeStatePacket): + latest_mode = mode + in_policy = bool(latest_mode is not None and latest_mode.mode == "policy") + if was_in_policy and not in_policy: + device.center() + was_in_policy = in_policy + target = target_sub.recv_latest() + if _policy_target_is_current( + target, + latest_mode, + last_target_seq=last_target_seq, + max_age_s=config.frame_timeout_s, + ): + _apply_policy_neck_target(device, target) + last_target_seq = int(target.seq) + time.sleep(sleep_s) + finally: + try: + device.center() + if config.release_on_shutdown: + device.release_torque() + finally: + device.close() + target_sub.close() + mode_sub.close() + command_sub.close() + + _worker_loop("policy_neck", cfg, _main) diff --git a/teleopit/sim2real/mp/high_level_policy_worker.py b/teleopit/sim2real/mp/high_level_policy_worker.py new file mode 100644 index 00000000..13056d06 --- /dev/null +++ b/teleopit/sim2real/mp/high_level_policy_worker.py @@ -0,0 +1,284 @@ +"""Non-critical host-policy client worker for high-level-policy sim2real.""" + +from __future__ import annotations + +import logging +import time +from typing import Any, Callable + +import numpy as np + +from teleopit.high_level_policy.client import ( + HighLevelPolicyClient, + PolicyTransportError, +) +from teleopit.high_level_policy.config import parse_high_level_policy_config +from teleopit.high_level_policy.protocol import PolicyProtocolError +from teleopit.sim2real.mp.ipc import ( + COMMAND_TOPIC, + HIGH_LEVEL_POLICY_ACTION_TOPIC, + HIGH_LEVEL_POLICY_OBSERVATION_TOPIC, + HIGH_LEVEL_POLICY_SESSION_TOPIC, + HIGH_LEVEL_POLICY_STATUS_TOPIC, + LatestSubscriber, + Sim2RealIpcEndpoints, + ZmqPublisher, +) +from teleopit.sim2real.mp.messages import ( + CommandPacket, + HighLevelPolicyActionPacket, + HighLevelPolicyObservationPacket, + HighLevelPolicySessionPacket, + HighLevelPolicyStatusPacket, +) +from teleopit.sim2real.mp.shm import SharedFrameRingReader + + +logger = logging.getLogger(__name__) + + +def encode_policy_jpeg(frame: object, *, quality: int) -> bytes: + try: + import cv2 + except ImportError as exc: + raise RuntimeError( + "High-level policy image encoding requires OpenCV; install teleopit[sim2real]" + ) from exc + rgb = np.asarray(frame) + if rgb.shape != (480, 640, 3) or rgb.dtype != np.uint8: + raise ValueError( + f"High-level policy camera frame must be uint8[480,640,3], got {rgb.dtype}{rgb.shape}" + ) + bgr = cv2.cvtColor(np.ascontiguousarray(rgb), cv2.COLOR_RGB2BGR) + ok, encoded = cv2.imencode(".jpg", bgr, [cv2.IMWRITE_JPEG_QUALITY, int(quality)]) + if not ok: + raise RuntimeError("OpenCV failed to encode the high-level policy JPEG") + payload = encoded.tobytes() + if not payload.startswith(b"\xff\xd8") or not payload.endswith(b"\xff\xd9"): + raise RuntimeError("OpenCV returned an invalid JPEG payload") + return payload + + +class HighLevelPolicyWorker: + def __init__( + self, + cfg: dict[str, Any], + endpoints: Sim2RealIpcEndpoints, + stop_event: Any, + *, + client_factory: Callable[..., HighLevelPolicyClient] = HighLevelPolicyClient, + frame_reader: SharedFrameRingReader | None = None, + ) -> None: + self.cfg = cfg + self.endpoints = endpoints + self.stop_event = stop_event + self.policy_cfg = parse_high_level_policy_config(cfg) + self._client_factory = client_factory + self._frame_reader = frame_reader or SharedFrameRingReader() + self._session_sub = LatestSubscriber( + endpoints.high_level_policy_control_pub, + HIGH_LEVEL_POLICY_SESSION_TOPIC, + ) + self._observation_sub = LatestSubscriber( + endpoints.high_level_policy_control_pub, + HIGH_LEVEL_POLICY_OBSERVATION_TOPIC, + ) + self._command_sub = LatestSubscriber(endpoints.command_pub, COMMAND_TOPIC) + self._result_pub = ZmqPublisher(endpoints.high_level_policy_result_pub) + self._client: HighLevelPolicyClient | None = None + self._active_session: HighLevelPolicySessionPacket | None = None + self._ready = False + self._paused = False + self._last_session_seq = -1 + self._last_observation_seq = -1 + self._last_request_timestamp_ns: int | None = None + self._next_connect_time_s = 0.0 + self._status_seq = 0 + self._policy_type: str | None = None + self._policy_id: str | None = None + self._new_session_required = False + + def run(self) -> None: + try: + while not self.stop_event.is_set(): + command = self._command_sub.recv_latest() + if isinstance(command, CommandPacket) and command.command == "shutdown": + break + session = self._session_sub.recv_latest() + if isinstance(session, HighLevelPolicySessionPacket): + self._handle_session(session) + if self._active_session is not None and not self._ready: + self._connect_if_due() + observation = self._observation_sub.recv_latest() + if isinstance(observation, HighLevelPolicyObservationPacket): + self._handle_observation(observation) + time.sleep(0.001) + finally: + self.close() + + def close(self) -> None: + client = self._client + self._client = None + if client is not None: + client.close() + self._frame_reader.close() + self._session_sub.close() + self._observation_sub.close() + self._command_sub.close() + self._result_pub.close() + + def _handle_session(self, packet: HighLevelPolicySessionPacket) -> None: + if int(packet.seq) <= self._last_session_seq: + return + self._last_session_seq = int(packet.seq) + command = str(packet.command).strip().lower() + if command == "start": + if self._active_session is None or packet.session_id != self._active_session.session_id: + self._active_session = packet + self._ready = False + self._paused = False + self._last_observation_seq = -1 + self._last_request_timestamp_ns = None + self._next_connect_time_s = 0.0 + self._new_session_required = False + self._policy_type = None + self._policy_id = None + self._publish_status("connecting", "connecting to host policy") + return + if self._active_session is None or packet.session_id != self._active_session.session_id: + return + if command == "pause": + if not self._paused: + self._paused = True + self._publish_status("paused", "policy requests paused") + elif command == "resume": + if self._paused: + self._paused = False + self._last_request_timestamp_ns = None + if self._ready: + self._publish_status("ready", "policy requests resumed") + else: + self._new_session_required = False + self._next_connect_time_s = 0.0 + self._policy_type = None + self._policy_id = None + self._publish_status( + "connecting", + "reconnecting the paused policy session", + ) + elif command == "stop": + self._publish_status("stopped", "policy session stopped") + self._active_session = None + self._ready = False + self._paused = False + + def _connect_if_due(self) -> None: + session = self._active_session + now_s = time.monotonic() + if session is None or self._new_session_required or now_s < self._next_connect_time_s: + return + try: + if self._client is None: + self._client = self._client_factory( + self.policy_cfg.endpoint, + timeout_s=self.policy_cfg.timeout_s, + ) + description = self._client.describe() + if self.policy_cfg.replan_steps > description.max_action_horizon: + raise ValueError( + "high_level_policy.replan_steps exceeds host max_action_horizon: " + f"{self.policy_cfg.replan_steps} > {description.max_action_horizon}" + ) + self._client.reset(session.session_id, session.task) + self._policy_type = description.policy_type + self._policy_id = description.policy_id + self._ready = True + self._publish_status("ready", "host policy session reset") + except (PolicyProtocolError, PolicyTransportError, ValueError, RuntimeError) as exc: + self._ready = False + self._next_connect_time_s = now_s + self.policy_cfg.reconnect_backoff_s + self._publish_status("unavailable", str(exc)) + + def _handle_observation(self, packet: HighLevelPolicyObservationPacket) -> None: + session = self._active_session + if session is None or not self._ready or self._paused: + return + if packet.session_id != session.session_id or packet.sequence_id <= self._last_observation_seq: + return + now_s = time.monotonic() + if now_s - float(packet.timestamp_s) > self.policy_cfg.max_observation_age_s: + return + minimum_interval_ns = int(round(self.policy_cfg.replan_steps / 30.0 * 1e9)) + if ( + self._last_request_timestamp_ns is not None + and packet.onboard_monotonic_timestamp_ns - self._last_request_timestamp_ns < minimum_interval_ns + ): + return + client = self._client + if client is None: + return + try: + frame = self._frame_reader.read(packet.frame, copy=True) + jpeg = encode_policy_jpeg(frame, quality=self.policy_cfg.jpeg_quality) + chunk = client.get_action( + session_id=session.session_id, + sequence_id=int(packet.sequence_id), + onboard_monotonic_timestamp_ns=int(packet.onboard_monotonic_timestamp_ns), + task=session.task, + jpeg_image=jpeg, + state=packet.state, + ) + if self._policy_id is None or chunk.policy_id != self._policy_id: + raise PolicyProtocolError( + "policy_mismatch", + "get_action policy_id does not match the preceding describe response", + ) + self._result_pub.publish( + HIGH_LEVEL_POLICY_ACTION_TOPIC, + HighLevelPolicyActionPacket( + session_id=chunk.session_id, + source_sequence_id=chunk.source_sequence_id, + source_onboard_monotonic_timestamp_ns=chunk.source_onboard_monotonic_timestamp_ns, + action_fps=chunk.action_fps, + actions=np.asarray(chunk.actions, dtype=np.float32).copy(), + policy_id=chunk.policy_id, + server_inference_ms=chunk.server_inference_ms, + received_timestamp_s=time.monotonic(), + ), + ) + self._last_observation_seq = int(packet.sequence_id) + self._last_request_timestamp_ns = int(packet.onboard_monotonic_timestamp_ns) + except (PolicyProtocolError, PolicyTransportError, ValueError, RuntimeError) as exc: + logger.warning("High-level policy request failed: %s", exc) + self._ready = False + self._paused = True + self._new_session_required = True + self._publish_status( + "fault", + f"{exc}; POLICY paused and can be resumed with B after recovery", + ) + + def _publish_status(self, status: str, detail: str) -> None: + self._status_seq += 1 + session_id = None if self._active_session is None else self._active_session.session_id + self._result_pub.publish( + HIGH_LEVEL_POLICY_STATUS_TOPIC, + HighLevelPolicyStatusPacket( + session_id=session_id, + status=str(status), + detail=str(detail), + timestamp_s=time.monotonic(), + seq=self._status_seq, + policy_type=self._policy_type, + policy_id=self._policy_id, + ), + ) + + +def run_high_level_policy_worker( + cfg: dict[str, Any], + endpoints: Sim2RealIpcEndpoints, + stop_event: Any, +) -> None: + worker = HighLevelPolicyWorker(cfg, endpoints, stop_event) + worker.run() diff --git a/teleopit/sim2real/mp/ipc.py b/teleopit/sim2real/mp/ipc.py index 21d81213..6d3c59b0 100644 --- a/teleopit/sim2real/mp/ipc.py +++ b/teleopit/sim2real/mp/ipc.py @@ -23,6 +23,11 @@ RECORD_TOPIC = "record" HEALTH_TOPIC = "health" COMMAND_TOPIC = "command" +HIGH_LEVEL_POLICY_SESSION_TOPIC = "high_level_policy_session" +HIGH_LEVEL_POLICY_OBSERVATION_TOPIC = "high_level_policy_observation" +HIGH_LEVEL_POLICY_ACTION_TOPIC = "high_level_policy_action" +HIGH_LEVEL_POLICY_STATUS_TOPIC = "high_level_policy_status" +HIGH_LEVEL_POLICY_TARGET_TOPIC = "high_level_policy_target" @dataclass(frozen=True) @@ -41,6 +46,8 @@ class Sim2RealIpcEndpoints: health_pub: str command_pub: str reference_command_pub: str + high_level_policy_control_pub: str + high_level_policy_result_pub: str def default_endpoints(*, host: str = "127.0.0.1", base_port: int = 39700) -> Sim2RealIpcEndpoints: @@ -61,6 +68,8 @@ def default_endpoints(*, host: str = "127.0.0.1", base_port: int = 39700) -> Sim health_pub=f"{prefix}{base_port + 9}", command_pub=f"{prefix}{base_port + 10}", reference_command_pub=f"{prefix}{base_port + 11}", + high_level_policy_control_pub=f"{prefix}{base_port + 14}", + high_level_policy_result_pub=f"{prefix}{base_port + 15}", ) diff --git a/teleopit/sim2real/mp/messages.py b/teleopit/sim2real/mp/messages.py index 2a166a6d..0fffd6ef 100644 --- a/teleopit/sim2real/mp/messages.py +++ b/teleopit/sim2real/mp/messages.py @@ -13,6 +13,7 @@ Float64Array = NDArray[np.float64] +Float32Array = NDArray[np.float32] @dataclass(frozen=True) @@ -57,6 +58,8 @@ class ModeStatePacket: mocap_paused: bool timestamp_s: float seq: int + policy_paused: bool = False + policy_session_id: str | None = None @dataclass(frozen=True) @@ -116,3 +119,53 @@ class SharedFrameDescriptor: shape: tuple[int, ...] dtype: str slots: int + + +@dataclass(frozen=True) +class HighLevelPolicySessionPacket: + session_id: str + task: str + command: str + timestamp_s: float + seq: int + + +@dataclass(frozen=True) +class HighLevelPolicyObservationPacket: + session_id: str + sequence_id: int + onboard_monotonic_timestamp_ns: int + state: Float32Array + frame: SharedFrameDescriptor + timestamp_s: float + + +@dataclass(frozen=True) +class HighLevelPolicyActionPacket: + session_id: str + source_sequence_id: int + source_onboard_monotonic_timestamp_ns: int + action_fps: int + actions: Float32Array + policy_id: str + server_inference_ms: float + received_timestamp_s: float + + +@dataclass(frozen=True) +class HighLevelPolicyStatusPacket: + session_id: str | None + status: str + detail: str + timestamp_s: float + seq: int + policy_type: str | None = None + policy_id: str | None = None + + +@dataclass(frozen=True) +class HighLevelPolicyTargetPacket: + session_id: str + action: Float32Array + timestamp_s: float + seq: int diff --git a/teleopit/sim2real/mp/runtime.py b/teleopit/sim2real/mp/runtime.py index a7c43653..ccc91150 100644 --- a/teleopit/sim2real/mp/runtime.py +++ b/teleopit/sim2real/mp/runtime.py @@ -9,12 +9,19 @@ from pathlib import Path import sys import time +import uuid from typing import Any, Callable import numpy as np from numpy.typing import NDArray from teleopit.constants import FULL_QPOS_DIM, NUM_JOINTS, ROOT_DIM +from teleopit.high_level_policy.client import PolicyActionChunk +from teleopit.high_level_policy.config import ( + parse_high_level_policy_config, + parse_high_level_policy_safety_config, +) +from teleopit.high_level_policy.scheduler import HighLevelPolicyScheduler, PolicyFrameTransform from teleopit.controllers.observation import VelCmdObservationBuilder, align_motion_qpos_yaw from teleopit.controllers.rl_policy import RLPolicyController from teleopit.inputs.bvh_provider import BVHInputProvider @@ -78,6 +85,11 @@ HEAD_POSE_TOPIC, HAND_TOPIC, HEALTH_TOPIC, + HIGH_LEVEL_POLICY_ACTION_TOPIC, + HIGH_LEVEL_POLICY_OBSERVATION_TOPIC, + HIGH_LEVEL_POLICY_SESSION_TOPIC, + HIGH_LEVEL_POLICY_STATUS_TOPIC, + HIGH_LEVEL_POLICY_TARGET_TOPIC, MODE_TOPIC, NECK_COMMAND_TOPIC, RECORD_TOPIC, @@ -94,6 +106,11 @@ ControlEventsPacket, HandCommandPacket, HealthPacket, + HighLevelPolicyActionPacket, + HighLevelPolicyObservationPacket, + HighLevelPolicySessionPacket, + HighLevelPolicyStatusPacket, + HighLevelPolicyTargetPacket, ModeStatePacket, NeckCommandPacket, ReferencePacket, @@ -121,6 +138,7 @@ PROJECT_ROOT = Path(__file__).resolve().parents[3] ARM_MOCAP_REFERENCE_COMMAND = "arm_mocap_reference" DISARM_MOCAP_REFERENCE_COMMAND = "disarm_mocap_reference" +HIGH_LEVEL_POLICY_FAULT_COMMAND = "high_level_policy_fault" class RobotMode(Enum): @@ -128,6 +146,7 @@ class RobotMode(Enum): STANDING = "standing" MOCAP = "mocap" ARMS = "arms" + POLICY = "policy" DAMPING = "damping" @@ -279,6 +298,11 @@ def _input_provider_kind(cfg: Any) -> str: return str(cfg_get(cfg_get(cfg, "input", {}) or {}, "provider", "bvh")).strip().lower() +def _high_level_policy_enabled(cfg: Any) -> bool: + policy_cfg = cfg_get(cfg, "high_level_policy", {}) or {} + return bool(cfg_get(policy_cfg, "enabled", False)) + + def _recording_cfg(cfg: Any) -> Any: return cfg_get(cfg, "recording", {}) or {} @@ -336,6 +360,11 @@ def _validate_new_runtime_config(cfg: Any) -> None: "Legacy sim2real config keys are no longer supported: " f"{', '.join(legacy_keys)}. Use input.provider, runtime, and hands instead." ) + if _high_level_policy_enabled(cfg): + raise ValueError( + "high_level_policy.enabled=true requires the independent " + "scripts/run/run_high_level_policy_sim2real.py entry point" + ) provider = _input_provider_kind(cfg) if provider not in ("pico4", "bvh"): raise ValueError(f"sim2real input.provider must be pico4 or bvh, got {provider!r}") @@ -1156,6 +1185,7 @@ def __init__( self.endpoints = endpoints self.stop_event = stop_event self.provider_kind = _input_provider_kind(cfg) + self.high_level_policy_enabled = _high_level_policy_enabled(cfg) self.mode = RobotMode.IDLE self.policy_hz = float(cfg_get(cfg, "policy_hz", 50.0)) self.dt = 1.0 / self.policy_hz @@ -1185,6 +1215,8 @@ def __init__( policy_dt_s=self.dt, reference_steps=cfg_get(cfg, "reference_steps", [0]), ) + if self.high_level_policy_enabled and self._reference_window_builder.reference_steps != (0,): + raise ValueError("High-level policy sim2real currently requires reference_steps=[0]") self._ref_proc = Sim2RealReferenceProcessor( obs_builder=self.obs_builder, policy=self.policy, @@ -1208,6 +1240,41 @@ def __init__( self._mocap_reference_arm_retry_s = float(cfg_get(_mp_cfg(cfg), "mocap_reference_arm_retry_s", 0.1)) self._mocap_session = MocapSessionManager() + self._high_level_policy_cfg = ( + parse_high_level_policy_config(cfg) if self.high_level_policy_enabled else None + ) + self._high_level_policy_safety_cfg = ( + parse_high_level_policy_safety_config(cfg) + if self.high_level_policy_enabled + else None + ) + self._high_level_policy_scheduler = ( + HighLevelPolicyScheduler( + hold_s=self._high_level_policy_cfg.hold_s, + safety=self._high_level_policy_safety_cfg, + output_hz=self.policy_hz, + ) + if self._high_level_policy_cfg is not None + else None + ) + self._policy_entry_pending = False + self._policy_entry_deadline_s: float | None = None + self._policy_session_id: str | None = None + self._policy_frame_transform: PolicyFrameTransform | None = None + self._policy_paused = False + self._policy_resume_pending = False + self._policy_resume_deadline_s: float | None = None + self._policy_resume_source_timestamp_ns: int | None = None + self._policy_hold_qpos: Float64Array | None = None + self._policy_session_seq = 0 + self._policy_observation_seq = 0 + self._policy_target_seq = 0 + self._last_policy_session_publish_s = 0.0 + self._last_policy_video_seq = -1 + self._latest_policy_video: SharedFrameDescriptor | None = None + self._latest_policy_status: HighLevelPolicyStatusPacket | None = None + self._last_policy_status_seq = -1 + self._latest_reference: ReferencePacket | None = None mp_cfg = _mp_cfg(cfg) self._max_reference_age_s = float(cfg_get(mp_cfg, "max_reference_age_s", 0.25)) @@ -1217,10 +1284,42 @@ def __init__( self._last_reference_seq = -1 self._consecutive_valid_references = 0 - self._reference_sub = LatestSubscriber(endpoints.reference_pub, REFERENCE_TOPIC) - self._events_sub = LatestSubscriber(endpoints.control_events_pub, CONTROL_EVENTS_TOPIC) + self._reference_sub = ( + None + if self.high_level_policy_enabled + else LatestSubscriber(endpoints.reference_pub, REFERENCE_TOPIC) + ) + self._events_sub = ( + None + if self.high_level_policy_enabled + else LatestSubscriber(endpoints.control_events_pub, CONTROL_EVENTS_TOPIC) + ) self._command_sub = LatestSubscriber(endpoints.command_pub, COMMAND_TOPIC) - self._reference_command_pub = ZmqPublisher(endpoints.reference_command_pub) + self._reference_command_pub = ( + None + if self.high_level_policy_enabled + else ZmqPublisher(endpoints.reference_command_pub) + ) + self._policy_video_sub = ( + LatestSubscriber(endpoints.video_pub, VIDEO_TOPIC) + if self.high_level_policy_enabled + else None + ) + self._policy_action_sub = ( + LatestSubscriber(endpoints.high_level_policy_result_pub, HIGH_LEVEL_POLICY_ACTION_TOPIC) + if self.high_level_policy_enabled + else None + ) + self._policy_status_sub = ( + LatestSubscriber(endpoints.high_level_policy_result_pub, HIGH_LEVEL_POLICY_STATUS_TOPIC) + if self.high_level_policy_enabled + else None + ) + self._policy_control_pub = ( + ZmqPublisher(endpoints.high_level_policy_control_pub) + if self.high_level_policy_enabled + else None + ) self._mode_pub = ZmqPublisher(endpoints.mode_pub) self._record_pub = ZmqPublisher(endpoints.record_pub) if _recording_enabled(cfg) else None @@ -1256,6 +1355,8 @@ def run(self) -> None: self._standing_step() elif self.mode in (RobotMode.MOCAP, RobotMode.ARMS): self._mocap_step() + elif self.mode == RobotMode.POLICY: + self._high_level_policy_step() self._publish_mode_state() work_elapsed_s = time.monotonic() - t0 @@ -1264,13 +1365,15 @@ def run(self) -> None: loop_start_s=t0, work_elapsed_s=work_elapsed_s, cycle_elapsed_s=cycle_elapsed_s, - pico_age_s=self._reference_age_s(), + pico_age_s=None if self.high_level_policy_enabled else self._reference_age_s(), ) finally: self.shutdown() def shutdown(self) -> None: - if self.mode in (RobotMode.STANDING, RobotMode.MOCAP, RobotMode.ARMS): + if self.high_level_policy_enabled and self._policy_session_id is not None: + self._stop_high_level_policy_session() + if self.mode in (RobotMode.STANDING, RobotMode.MOCAP, RobotMode.ARMS, RobotMode.POLICY): try: self.robot.set_damping() time.sleep(0.5) @@ -1281,10 +1384,20 @@ def shutdown(self) -> None: except Exception: logger.exception("Failed to exit debug mode during robot_control shutdown") self._retarget_viewer.shutdown() - self._reference_sub.close() - self._events_sub.close() + for subscriber in ( + self._reference_sub, + self._events_sub, + self._policy_video_sub, + self._policy_action_sub, + self._policy_status_sub, + ): + if subscriber is not None: + subscriber.close() self._command_sub.close() - self._reference_command_pub.close() + if self._reference_command_pub is not None: + self._reference_command_pub.close() + if self._policy_control_pub is not None: + self._policy_control_pub.close() self._mode_pub.close() if self._record_pub is not None: self._record_pub.close() @@ -1310,6 +1423,20 @@ def _drain_ipc(self) -> None: if isinstance(command, CommandPacket) and command.command == "shutdown": self.stop_event.set() return + if isinstance(command, CommandPacket) and command.command == HIGH_LEVEL_POLICY_FAULT_COMMAND: + detail = str( + command.payload.get( + "detail", + "required high-level-policy input worker exited", + ) + ) + self._handle_high_level_policy_fault(detail) + return + if bool(getattr(self, "high_level_policy_enabled", False)): + self._drain_high_level_policy_ipc() + return + if self._reference_sub is None or self._events_sub is None: + raise RuntimeError("Teleoperation robot worker is missing reference/event subscribers") reference = self._reference_sub.recv_latest() if isinstance(reference, ReferencePacket): self._note_reference_packet(reference) @@ -1318,6 +1445,9 @@ def _drain_ipc(self) -> None: self._handle_mocap_control_events(events.events) def _handle_transitions(self) -> None: + if bool(getattr(self, "high_level_policy_enabled", False)): + self._handle_high_level_policy_transitions() + return if self.mode == RobotMode.IDLE: if self.remote.start.on_pressed: operator_logger.info("Start -> STANDING") @@ -1339,13 +1469,17 @@ def _handle_transitions(self) -> None: self._send_reference_command("replay_mocap") self._resume_paused_mocap_if_needed() return - if self.remote.A.on_pressed: + pause_pressed = ( + self.remote.B.on_pressed if self.provider_kind == "pico4" else self.remote.A.on_pressed + ) + if pause_pressed: + button = "B" if self.provider_kind == "pico4" else "A" if self._mocap_session.state == MocapSessionState.PAUSED: - operator_logger.info("A -> resume playback") + operator_logger.info("%s -> resume playback", button) self._send_reference_command("resume_mocap") self._resume_paused_mocap() else: - operator_logger.info("A -> pause playback") + operator_logger.info("%s -> pause playback", button) self._send_reference_command("pause_mocap") self._pause_active_mocap() return @@ -1357,6 +1491,305 @@ def _handle_transitions(self) -> None: operator_logger.info("Start -> STANDING") self._enter_standing() + def _drain_high_level_policy_ipc(self) -> None: + if self._policy_video_sub is None or self._policy_action_sub is None or self._policy_status_sub is None: + raise RuntimeError("High-level policy robot worker is missing IPC subscribers") + video = self._policy_video_sub.recv_latest() + if isinstance(video, SharedFrameDescriptor) and int(video.seq) > self._last_policy_video_seq: + self._latest_policy_video = video + status = self._policy_status_sub.recv_latest() + if isinstance(status, HighLevelPolicyStatusPacket) and int(status.seq) > self._last_policy_status_seq: + self._latest_policy_status = status + self._last_policy_status_seq = int(status.seq) + if status.status in ("fault", "unavailable"): + logger.warning("High-level policy host status=%s: %s", status.status, status.detail) + current_session = status.session_id == self._policy_session_id + terminal_fault = status.status == "fault" or ( + status.status == "unavailable" and self.mode == RobotMode.POLICY + ) + if current_session and terminal_fault: + self._handle_high_level_policy_fault(status.detail) + return + packet = self._policy_action_sub.recv_latest() + if not isinstance(packet, HighLevelPolicyActionPacket): + return + # A request may already be in flight when the operator pauses. Drain + # its result without replacing the reference frozen at the B press. + if self._policy_paused and not self._policy_resume_pending: + return + scheduler = self._high_level_policy_scheduler + policy_cfg = self._high_level_policy_cfg + if scheduler is None or policy_cfg is None: + return + now_s = time.monotonic() + result_age_s = now_s - float(packet.received_timestamp_s) + if ( + not np.isfinite(result_age_s) + or result_age_s < 0.0 + or result_age_s > policy_cfg.max_result_age_s + ): + logger.warning( + "Rejected stale high-level policy result: age=%.3fs limit=%.3fs", + result_age_s, + policy_cfg.max_result_age_s, + ) + return + minimum_source_timestamp_ns = self._policy_resume_source_timestamp_ns + if ( + self._policy_resume_pending + and minimum_source_timestamp_ns is not None + and int(packet.source_onboard_monotonic_timestamp_ns) + < minimum_source_timestamp_ns + ): + logger.warning("Discarded pre-resume high-level policy action chunk") + return + try: + scheduler.accept( + PolicyActionChunk( + session_id=packet.session_id, + source_sequence_id=int(packet.source_sequence_id), + source_onboard_monotonic_timestamp_ns=int( + packet.source_onboard_monotonic_timestamp_ns + ), + action_fps=int(packet.action_fps), + actions=np.asarray(packet.actions, dtype=np.float32), + policy_id=str(packet.policy_id), + server_inference_ms=float(packet.server_inference_ms), + ), + now_s=now_s, + ) + except ValueError as exc: + logger.warning("Rejected high-level policy action chunk: %s", exc) + return + if self._policy_resume_pending: + scheduler.resume(now_s) + self._policy_paused = False + self._policy_resume_pending = False + self._policy_resume_deadline_s = None + self._policy_resume_source_timestamp_ns = None + operator_logger.info("fresh action chunk -> resume POLICY") + + def _handle_high_level_policy_transitions(self) -> None: + if self.mode == RobotMode.IDLE: + if self.remote.start.on_pressed: + operator_logger.info("Start -> STANDING") + self._enter_standing() + return + if self.mode == RobotMode.STANDING: + if self.remote.X.on_pressed and self._policy_entry_pending: + operator_logger.info("X -> cancel high-level policy entry") + self._stop_high_level_policy_session() + return + if self.remote.Y.on_pressed and not self._policy_entry_pending: + operator_logger.info("Y -> request high-level policy") + self._begin_high_level_policy_entry() + if self._policy_entry_pending: + self._publish_high_level_policy_session("start", repeat=True) + scheduler = self._high_level_policy_scheduler + if scheduler is not None and scheduler.has_chunk: + self._transition_to_high_level_policy() + return + deadline_s = self._policy_entry_deadline_s + if deadline_s is not None and time.monotonic() > deadline_s: + operator_logger.warning("High-level policy entry timed out; remaining in STANDING") + self._stop_high_level_policy_session() + return + if self.mode == RobotMode.POLICY: + if self.remote.X.on_pressed: + operator_logger.info("X -> STANDING") + self._enter_standing() + return + if self.remote.B.on_pressed: + self._toggle_high_level_policy_pause() + return + self._publish_high_level_policy_session( + "resume" + if self._policy_resume_pending or not self._policy_paused + else "pause", + repeat=True, + ) + return + if self.mode == RobotMode.DAMPING and self.remote.start.on_pressed: + operator_logger.info("Start -> STANDING") + self._enter_standing() + + def _begin_high_level_policy_entry(self) -> None: + policy_cfg = self._high_level_policy_cfg + scheduler = self._high_level_policy_scheduler + if policy_cfg is None or scheduler is None: + raise RuntimeError("High-level policy runtime is not configured") + state = self.robot.get_state() + root_pos = self._resolve_base_pos(state) + self._policy_frame_transform = PolicyFrameTransform.from_robot_pose( + root_pos[:2], + getattr(state, "quat"), + ) + self._policy_session_id = uuid.uuid4().hex + initial_action = np.zeros(50, dtype=np.float32) + initial_action[:36] = self._policy_frame_transform.localize_body_action( + self._build_robot_state_qpos(state) + ) + scheduler.reset(self._policy_session_id, initial_action=initial_action) + self._policy_entry_pending = True + self._policy_entry_deadline_s = time.monotonic() + policy_cfg.entry_timeout_s + self._policy_paused = False + self._policy_resume_pending = False + self._policy_resume_deadline_s = None + self._policy_resume_source_timestamp_ns = None + self._policy_hold_qpos = self._build_robot_state_qpos(state) + self._policy_observation_seq = 0 + self._last_policy_video_seq = ( + -1 + if self._latest_policy_video is None + else int(self._latest_policy_video.seq) + ) + self._last_policy_session_publish_s = 0.0 + self._publish_high_level_policy_session("start", repeat=False) + + def _publish_high_level_policy_session(self, command: str, *, repeat: bool = False) -> None: + publisher = self._policy_control_pub + session_id = self._policy_session_id + policy_cfg = self._high_level_policy_cfg + if publisher is None or session_id is None or policy_cfg is None: + return + now_s = time.monotonic() + if repeat and now_s - self._last_policy_session_publish_s < 0.2: + return + self._policy_session_seq += 1 + publisher.publish( + HIGH_LEVEL_POLICY_SESSION_TOPIC, + HighLevelPolicySessionPacket( + session_id=session_id, + task=policy_cfg.task, + command=str(command), + timestamp_s=now_s, + seq=self._policy_session_seq, + ), + ) + self._last_policy_session_publish_s = now_s + + def _publish_high_level_policy_observation(self, robot_state: object) -> None: + if not (self._policy_entry_pending or self.mode == RobotMode.POLICY): + return + if self._policy_paused and not self._policy_resume_pending: + return + publisher = self._policy_control_pub + frame = self._latest_policy_video + transform = self._policy_frame_transform + session_id = self._policy_session_id + policy_cfg = self._high_level_policy_cfg + if publisher is None or frame is None or transform is None or session_id is None or policy_cfg is None: + return + if int(frame.seq) <= self._last_policy_video_seq: + return + now_s = time.monotonic() + if abs(now_s - float(frame.timestamp_s)) > policy_cfg.max_observation_age_s: + return + state = transform.localize_state(build_observation_state(robot_state)) + sequence_id = self._policy_observation_seq + publisher.publish( + HIGH_LEVEL_POLICY_OBSERVATION_TOPIC, + HighLevelPolicyObservationPacket( + session_id=session_id, + sequence_id=sequence_id, + onboard_monotonic_timestamp_ns=int(round(float(frame.timestamp_s) * 1e9)), + state=state.astype(np.float32, copy=True), + frame=frame, + timestamp_s=now_s, + ), + ) + self._policy_observation_seq += 1 + self._last_policy_video_seq = int(frame.seq) + + def _transition_to_high_level_policy(self) -> None: + state = self.robot.get_state() + resume_qpos = self._build_robot_state_qpos(state) + self._reset_policy_state() + self._last_retarget_qpos = None + self._last_commanded_motion_qpos = resume_qpos.copy() + self._policy_hold_qpos = resume_qpos.copy() + self._safety.start_kp_ramp( + duration_s=self._standing_return_ramp_duration, + floor_ratio=self._standing_return_kp_ramp_floor_ratio, + ) + self._policy_entry_pending = False + self._policy_entry_deadline_s = None + self._policy_resume_pending = False + self._policy_resume_deadline_s = None + self._policy_resume_source_timestamp_ns = None + self.mode = RobotMode.POLICY + operator_logger.info("mode -> POLICY") + + def _toggle_high_level_policy_pause(self) -> None: + scheduler = self._high_level_policy_scheduler + if scheduler is None: + return + now_s = time.monotonic() + if self._policy_paused: + if self._policy_resume_pending: + return + policy_cfg = self._high_level_policy_cfg + if policy_cfg is None: + return + self._policy_resume_pending = True + self._policy_resume_deadline_s = now_s + policy_cfg.entry_timeout_s + self._policy_resume_source_timestamp_ns = int(round(now_s * 1e9)) + self._publish_high_level_policy_session("resume") + operator_logger.info("B -> resume POLICY; waiting for a fresh action chunk") + else: + scheduler.pause(now_s) + self._policy_paused = True + self._policy_resume_pending = False + self._policy_resume_deadline_s = None + self._policy_resume_source_timestamp_ns = None + self._policy_hold_qpos = self._resolve_mocap_hold_qpos() + self._publish_high_level_policy_session("pause") + operator_logger.info("B -> pause POLICY") + + def _stop_high_level_policy_session(self) -> None: + if self._policy_session_id is not None: + self._publish_high_level_policy_session("stop") + scheduler = self._high_level_policy_scheduler + if scheduler is not None: + scheduler.clear() + self._policy_entry_pending = False + self._policy_entry_deadline_s = None + self._policy_session_id = None + self._policy_frame_transform = None + self._policy_paused = False + self._policy_resume_pending = False + self._policy_resume_deadline_s = None + self._policy_resume_source_timestamp_ns = None + self._policy_hold_qpos = None + self._latest_policy_status = None + + def _handle_high_level_policy_fault(self, detail: str) -> None: + if not bool(getattr(self, "high_level_policy_enabled", False)): + return + if self.mode == RobotMode.POLICY: + if self._policy_paused and not self._policy_resume_pending: + return + scheduler = self._high_level_policy_scheduler + if scheduler is not None: + scheduler.pause(time.monotonic()) + self._policy_paused = True + self._policy_resume_pending = False + self._policy_resume_deadline_s = None + self._policy_resume_source_timestamp_ns = None + self._policy_hold_qpos = self._resolve_mocap_hold_qpos() + self._publish_high_level_policy_session("pause") + operator_logger.warning( + "High-level policy fault -> pause POLICY: %s", + detail, + ) + return + if self._policy_entry_pending: + operator_logger.warning( + "High-level policy entry failed; remaining in STANDING: %s", + detail, + ) + self._stop_high_level_policy_session() + def _standing_step(self) -> None: robot_state = self.robot.get_state() qpos = self._standing_qpos.copy() @@ -1381,6 +1814,7 @@ def _standing_step(self) -> None: self._last_action = np.asarray(action, dtype=np.float32).reshape(-1) self._last_retarget_qpos = qpos.copy() self._last_commanded_motion_qpos = qpos.copy() + self._publish_high_level_policy_observation(robot_state) self._publish_record_step(robot_state=robot_state, reference_qpos=qpos) self._write_retarget_viewer(qpos) @@ -1407,15 +1841,106 @@ def _mocap_step(self) -> None: robot_state = self.robot.get_state() self._execute_mocap_pipeline(reference.qpos, robot_state, reference.reference_window) + def _high_level_policy_step(self) -> None: + scheduler = self._high_level_policy_scheduler + transform = self._policy_frame_transform + session_id = self._policy_session_id + if self._policy_resume_pending: + robot_state = self.robot.get_state() + self._publish_high_level_policy_observation(robot_state) + deadline_s = self._policy_resume_deadline_s + if deadline_s is not None and time.monotonic() > deadline_s: + self._handle_high_level_policy_fault("resume timed out waiting for a fresh action chunk") + hold_qpos = self._policy_hold_qpos + if hold_qpos is None: + hold_qpos = self._resolve_mocap_hold_qpos() + self._policy_hold_qpos = hold_qpos.copy() + self._run_static_mocap_step(hold_qpos) + return + if self._policy_paused: + hold_qpos = self._policy_hold_qpos + if hold_qpos is None: + hold_qpos = self._resolve_mocap_hold_qpos() + self._policy_hold_qpos = hold_qpos.copy() + self._run_static_mocap_step(hold_qpos) + return + if scheduler is None or transform is None or session_id is None: + detail = "POLICY mode is missing its scheduler/session transform" + logger.error(detail) + self._handle_high_level_policy_fault(detail) + hold_qpos = self._policy_hold_qpos + if hold_qpos is None: + hold_qpos = self._resolve_mocap_hold_qpos() + self._policy_hold_qpos = hold_qpos.copy() + self._run_static_mocap_step(hold_qpos) + return + + robot_state = self.robot.get_state() + self._publish_high_level_policy_observation(robot_state) + scheduled = scheduler.sample(time.monotonic()) + if scheduled is None: + self._handle_high_level_policy_fault("action watchdog expired") + hold_qpos = self._policy_hold_qpos + if hold_qpos is None: + hold_qpos = self._resolve_mocap_hold_qpos() + self._policy_hold_qpos = hold_qpos.copy() + self._run_static_mocap_step(hold_qpos) + return + reference_qpos = transform.delocalize_body_action(scheduled[:36]).astype(np.float64) + self._execute_reference_pipeline( + reference_qpos, + robot_state, + reference_window=None, + align_reference=False, + compose_arms=False, + ) + self._policy_hold_qpos = reference_qpos.copy() + publisher = self._policy_control_pub + if publisher is not None: + self._policy_target_seq += 1 + publisher.publish( + HIGH_LEVEL_POLICY_TARGET_TOPIC, + HighLevelPolicyTargetPacket( + session_id=session_id, + action=np.asarray(scheduled, dtype=np.float32).copy(), + timestamp_s=time.monotonic(), + seq=self._policy_target_seq, + ), + ) + def _execute_mocap_pipeline( self, reference_qpos: Float64Array, robot_state: object, reference_window: ReferenceWindow | None, + ) -> None: + self._execute_reference_pipeline( + reference_qpos, + robot_state, + reference_window=reference_window, + align_reference=True, + compose_arms=self.mode == RobotMode.ARMS, + ) + + def _execute_reference_pipeline( + self, + reference_qpos: Float64Array, + robot_state: object, + *, + reference_window: ReferenceWindow | None, + align_reference: bool, + compose_arms: bool, ) -> None: reference_window_aligned = False - reference_qpos = self._ref_proc.align_reference_yaw(reference_qpos, robot_state=robot_state) - if self.mode == RobotMode.ARMS: + if align_reference: + reference_qpos = self._ref_proc.align_reference_yaw( + reference_qpos, + robot_state=robot_state, + ) + else: + reference_qpos = np.asarray(reference_qpos, dtype=np.float64).copy() + reference_window_aligned = True + if compose_arms: reference_qpos = self._compose_arm_reference(reference_qpos) aligned_window = self._ref_proc.align_reference_window(reference_window, robot_state) reference_window = self._compose_arm_reference_window(aligned_window) @@ -1478,10 +2003,19 @@ def _compose_arm_reference_window(self, reference_window: ReferenceWindow | None def _enter_standing(self) -> None: prev_mode = self.mode + if bool(getattr(self, "high_level_policy_enabled", False)) and ( + prev_mode == RobotMode.POLICY or self._policy_entry_pending + ): + self._stop_high_level_policy_session() self._disarm_mocap_reference_if_needed() self._clear_reference_gate() self._mocap_entry_requested = False - already_in_debug = self.mode in (RobotMode.STANDING, RobotMode.MOCAP, RobotMode.ARMS) + already_in_debug = self.mode in ( + RobotMode.STANDING, + RobotMode.MOCAP, + RobotMode.ARMS, + RobotMode.POLICY, + ) if not already_in_debug: logger.info("Entering debug mode...") ok = self.robot.enter_debug_mode() @@ -1491,7 +2025,7 @@ def _enter_standing(self) -> None: time.sleep(0.5) state = self.robot.get_state() - if prev_mode not in (RobotMode.MOCAP, RobotMode.ARMS): + if prev_mode not in (RobotMode.MOCAP, RobotMode.ARMS, RobotMode.POLICY): logger.info("Locking joints to current position...") self.robot.lock_all_joints() time.sleep(0.3) @@ -1503,7 +2037,7 @@ def _enter_standing(self) -> None: self._last_commanded_motion_qpos = None self._set_default_standing_reference(state) self._reset_policy_state() - if prev_mode in (RobotMode.MOCAP, RobotMode.ARMS): + if prev_mode in (RobotMode.MOCAP, RobotMode.ARMS, RobotMode.POLICY): self._safety.start_kp_ramp( duration_s=self._standing_return_ramp_duration, floor_ratio=self._standing_return_kp_ramp_floor_ratio, @@ -1579,10 +2113,14 @@ def _resume_paused_mocap_if_needed(self) -> None: self._resume_paused_mocap() def _enter_damping(self) -> None: + if bool(getattr(self, "high_level_policy_enabled", False)) and ( + self.mode == RobotMode.POLICY or self._policy_entry_pending + ): + self._stop_high_level_policy_session() self._disarm_mocap_reference_if_needed() self._clear_reference_gate() self._mocap_entry_requested = False - if self.mode in (RobotMode.STANDING, RobotMode.MOCAP, RobotMode.ARMS): + if self.mode in (RobotMode.STANDING, RobotMode.MOCAP, RobotMode.ARMS, RobotMode.POLICY): logger.info("DAMPING: sending LowCmd damping...") self.robot.set_damping() time.sleep(0.5) @@ -1693,6 +2231,8 @@ def _resume_paused_mocap(self) -> None: logger.info("Mocap session -> ACTIVE (multiprocess episode-reset + reference realignment)") def _send_reference_command(self, command: str) -> None: + if self._reference_command_pub is None: + return self._reference_command_pub.publish( COMMAND_TOPIC, CommandPacket(command=command, timestamp_s=time.monotonic()), @@ -1793,6 +2333,10 @@ def _publish_mode_state(self) -> None: mocap_paused=paused, timestamp_s=time.monotonic(), seq=self._mode_seq, + policy_paused=self.mode == RobotMode.POLICY and self._policy_paused, + policy_session_id=( + self._policy_session_id if self.mode == RobotMode.POLICY else None + ), ), ) diff --git a/tests/test_high_level_policy.py b/tests/test_high_level_policy.py new file mode 100644 index 00000000..591a6065 --- /dev/null +++ b/tests/test_high_level_policy.py @@ -0,0 +1,821 @@ +from __future__ import annotations + +import math +import threading +import time +from types import SimpleNamespace + +import numpy as np +import pytest +import zmq + +from teleopit.high_level_policy.client import HighLevelPolicyClient, PolicyActionChunk +from teleopit.high_level_policy.config import HighLevelPolicySafetyConfig +from teleopit.high_level_policy.hand_calibration import HandCalibration +from teleopit.high_level_policy.protocol import ( + MAX_REQUEST_BYTES, + MAX_RESPONSE_BYTES, + PolicyProtocolError, + decode_float32_array, + encode_float32_array, + pack_message, + unpack_message, +) +from teleopit.high_level_policy.scheduler import ( + HighLevelPolicyScheduler, + PolicyFrameTransform, + closure_to_o6_pose, +) +from teleopit.sim2real.mp.high_level_policy_runtime import ( + HighLevelPolicySim2RealRuntime, + _apply_policy_neck_target, + _policy_target_is_current, + _test_pattern, + _validate_high_level_policy_runtime_config, +) +from teleopit.sim2real.mp.high_level_policy_worker import HighLevelPolicyWorker +from teleopit.sim2real.mp.messages import ( + HighLevelPolicyActionPacket, + HighLevelPolicySessionPacket, + HighLevelPolicyStatusPacket, + HighLevelPolicyTargetPacket, + ModeStatePacket, +) +from teleopit.sim2real.mp.runtime import RobotMode, Sim2RealRuntime, _RobotControlWorker +from teleopit.runtime.mocap_session import MocapSessionState + + +def _chunk(*, source_s: float, sequence: int = 0, frames: int = 3) -> PolicyActionChunk: + actions = np.zeros((frames, 50), dtype=np.float32) + actions[:, 2] = 0.78 + actions[:, 3] = 1.0 + actions[:, 0] = np.arange(frames, dtype=np.float32) + actions[:, 36:48] = 0.5 + actions[:, 48] = np.arange(frames, dtype=np.float32) * 10.0 + return PolicyActionChunk( + session_id="session-1", + source_sequence_id=sequence, + source_onboard_monotonic_timestamp_ns=int(round(source_s * 1e9)), + action_fps=30, + actions=actions, + policy_id="test", + server_inference_ms=1.0, + ) + + +def _safe_actions(frames: int = 3) -> np.ndarray: + actions = np.zeros((frames, 50), dtype=np.float32) + actions[:, 0] = np.arange(frames, dtype=np.float32) * 0.02 + actions[:, 2] = 0.76 + actions[:, 3] = 1.0 + actions[:, 36:48] = 0.5 + return actions + + +def _safety_config() -> HighLevelPolicySafetyConfig: + return HighLevelPolicySafetyConfig( + root_height_min_m=0.55, + root_height_max_m=1.05, + max_root_xy_speed_m_s=2.5, + max_root_displacement_m=0.1, + max_yaw_rate_rad_s=2.5, + max_joint_rate_rad_s=10.0, + joint_pos_lower=(-3.0,) * 29, + joint_pos_upper=(3.0,) * 29, + neck_yaw_min_deg=-45.0, + neck_yaw_max_deg=45.0, + neck_pitch_min_deg=-40.0, + neck_pitch_max_deg=40.0, + ) + + +def _safe_chunk(actions: np.ndarray, *, source_s: float = 1.0, sequence: int = 0) -> PolicyActionChunk: + return PolicyActionChunk( + session_id="session-1", + source_sequence_id=sequence, + source_onboard_monotonic_timestamp_ns=int(round(source_s * 1e9)), + action_fps=30, + actions=actions, + policy_id="test", + server_inference_ms=1.0, + ) + + +def test_packaged_hand_calibration_loads() -> None: + calibration = HandCalibration.load() + + assert calibration.open_raw == (250.0, 250.0, 250.0, 250.0, 250.0, 250.0) + assert calibration.close_raw == (86.0, 73.0, 118.0, 111.0, 110.0, 111.0) + assert calibration.range_tolerance == pytest.approx(0.0001) + + +def test_msgpack_float32_array_roundtrip_is_little_endian() -> None: + values = np.arange(12, dtype=np.float64).reshape(3, 4) + message = {"array": encode_float32_array(values)} + payload = pack_message(message, max_bytes=4096) + decoded_message = unpack_message(payload, max_bytes=4096) + decoded = decode_float32_array( + decoded_message["array"], + name="array", + expected_shape=(3, 4), + ) + + assert decoded.dtype == np.dtype("float32") + np.testing.assert_allclose(decoded, values.astype(np.float32)) + + +def test_policy_frame_transform_localizes_state_and_delocalizes_action() -> None: + yaw = math.pi / 2.0 + yaw_quaternion = np.array([math.cos(yaw / 2.0), 0.0, 0.0, math.sin(yaw / 2.0)], dtype=np.float32) + transform = PolicyFrameTransform.from_robot_pose([2.0, 3.0], yaw_quaternion) + state = np.zeros(68, dtype=np.float32) + state[58:62] = yaw_quaternion + + localized = transform.localize_state(state) + np.testing.assert_allclose(localized[58:62], [1.0, 0.0, 0.0, 0.0], atol=1e-6) + + body = np.zeros(36, dtype=np.float32) + body[0] = 1.0 + body[2] = 0.78 + body[3] = 1.0 + world = transform.delocalize_body_action(body) + np.testing.assert_allclose(world[:3], [2.0, 4.0, 0.78], atol=1e-6) + np.testing.assert_allclose(world[3:7], yaw_quaternion, atol=1e-6) + np.testing.assert_allclose(transform.localize_body_action(world), body, atol=1e-6) + + +def test_scheduler_uses_source_timestamp_and_interpolates_at_30hz() -> None: + scheduler = HighLevelPolicyScheduler(hold_s=0.1) + scheduler.reset("session-1") + scheduler.accept(_chunk(source_s=10.0), now_s=10.01) + + halfway = scheduler.sample(10.0 + 0.5 / 30.0) + assert halfway is not None + assert halfway[0] == pytest.approx(0.5) + assert halfway[48] == pytest.approx(5.0) + + +def test_scheduler_pause_freezes_and_resume_shifts_plan_time() -> None: + scheduler = HighLevelPolicyScheduler(hold_s=0.1) + scheduler.reset("session-1") + scheduler.accept(_chunk(source_s=20.0), now_s=20.0) + scheduler.pause(20.02) + + paused = scheduler.sample(25.0) + assert paused is not None + scheduler.resume(25.0) + resumed = scheduler.sample(25.0) + assert resumed is not None + np.testing.assert_allclose(resumed, paused) + + +def test_scheduler_rejects_wrong_session_and_expired_chunk() -> None: + scheduler = HighLevelPolicyScheduler(hold_s=0.0) + scheduler.reset("other") + with pytest.raises(ValueError, match="session mismatch"): + scheduler.accept(_chunk(source_s=1.0), now_s=1.0) + + scheduler.reset("session-1") + with pytest.raises(ValueError, match="already expired"): + scheduler.accept(_chunk(source_s=1.0), now_s=2.0) + + with pytest.raises(ValueError, match="in the future"): + scheduler.accept(_chunk(source_s=3.0), now_s=2.0) + + +def test_scheduler_rejects_nonincreasing_source_timestamp() -> None: + scheduler = HighLevelPolicyScheduler(hold_s=0.1) + scheduler.reset("session-1") + scheduler.accept(_chunk(source_s=1.0), now_s=1.0) + + with pytest.raises(ValueError, match="source timestamp must increase"): + scheduler.accept(_chunk(source_s=1.0, sequence=1), now_s=1.01) + + +def test_linkerhand_closure_uses_hand_calibration() -> None: + assert closure_to_o6_pose(np.zeros(6, dtype=np.float32)) == (250, 250, 250, 250, 250, 250) + assert closure_to_o6_pose(np.ones(6, dtype=np.float32)) == (86, 73, 118, 111, 110, 111) + + +def test_scheduler_validates_complete_chunk_against_onboard_safety_limits() -> None: + scheduler = HighLevelPolicyScheduler(hold_s=0.1, safety=_safety_config()) + initial = _safe_actions(1)[0] + scheduler.reset("session-1", initial_action=initial) + scheduler.accept(_safe_chunk(_safe_actions()), now_s=1.01) + + assert scheduler.has_chunk + + +@pytest.mark.parametrize( + ("mutate", "message"), + [ + (lambda value: value.__setitem__((1, 2), 0.4), "root height"), + (lambda value: value.__setitem__((1, 0), 0.2), "displacement"), + (lambda value: value.__setitem__((1, 7), 4.0), "joint position"), + (lambda value: value.__setitem__((1, 7), 0.5), "joint rate"), + (lambda value: value.__setitem__((1, 48), 46.0), "OpenNeck yaw"), + (lambda value: value.__setitem__((1, 49), -41.0), "OpenNeck pitch"), + ], +) +def test_scheduler_rejects_entire_unsafe_chunk(mutate, message: str) -> None: # type: ignore[no-untyped-def] + scheduler = HighLevelPolicyScheduler(hold_s=0.1, safety=_safety_config()) + scheduler.reset("session-1", initial_action=_safe_actions(1)[0]) + actions = _safe_actions() + mutate(actions) + + with pytest.raises(ValueError, match=message): + scheduler.accept(_safe_chunk(actions), now_s=1.01) + assert not scheduler.has_chunk + + +def test_scheduler_rejects_root_yaw_rate() -> None: + scheduler = HighLevelPolicyScheduler(hold_s=0.1, safety=_safety_config()) + scheduler.reset("session-1", initial_action=_safe_actions(1)[0]) + actions = _safe_actions() + yaw = 0.2 + actions[1, 3:7] = [math.cos(yaw / 2.0), 0.0, 0.0, math.sin(yaw / 2.0)] + + with pytest.raises(ValueError, match="yaw rate"): + scheduler.accept(_safe_chunk(actions), now_s=1.01) + + +def test_scheduler_rate_limits_valid_plan_at_50hz_after_latency_skip() -> None: + scheduler = HighLevelPolicyScheduler( + hold_s=0.1, + safety=_safety_config(), + output_hz=50.0, + ) + initial = _safe_actions(1)[0] + scheduler.reset("session-1", initial_action=initial) + actions = _safe_actions(2) + actions[1, 0] = 0.08 + actions[1, 7] = 0.3 + yaw = 0.08 + actions[1, 3:7] = [math.cos(yaw / 2.0), 0.0, 0.0, math.sin(yaw / 2.0)] + scheduler.accept(_safe_chunk(actions), now_s=1.01) + + output = scheduler.sample(1.0 + 1.0 / 30.0) + + assert output is not None + assert output[0] == pytest.approx(2.5 / 50.0) + assert output[7] == pytest.approx(10.0 / 50.0) + output_yaw = 2.0 * math.atan2(float(output[6]), float(output[3])) + assert output_yaw == pytest.approx(2.5 / 50.0, abs=1e-6) + + +def test_policy_client_roundtrip_matches_current_messages() -> None: + context = zmq.Context() + endpoint = "inproc://teleopit-policy-client-roundtrip" + server = context.socket(zmq.REP) + server.bind(endpoint) + requests: list[dict[str, object]] = [] + + def serve() -> None: + for _ in range(3): + request = unpack_message(server.recv(), max_bytes=MAX_REQUEST_BYTES) + requests.append(request) + name = request["endpoint"] + if name == "describe": + data = { + "observation_schema": "teleopit-g1-state", + "observation_dim": 68, + "action_schema": "teleopit-g1-reference", + "action_dim": 50, + "dataset_fps": 30, + "max_action_horizon": 3, + "policy_type": "replay", + "policy_id": "test-policy", + "ready": True, + } + elif name == "reset": + data = {"session_id": "session-1", "reset": True} + else: + observation = request["data"] + data = { + "session_id": "session-1", + "source_sequence_id": observation["sequence_id"], + "source_onboard_monotonic_timestamp_ns": observation[ + "onboard_monotonic_timestamp_ns" + ], + "action_fps": 30, + "actions": encode_float32_array(_safe_actions()), + "policy_id": "test-policy", + "server_inference_ms": 1.0, + } + server.send( + pack_message( + { + "endpoint": name, + "ok": True, + "data": data, + }, + max_bytes=MAX_RESPONSE_BYTES, + ) + ) + + thread = threading.Thread(target=serve) + thread.start() + client = HighLevelPolicyClient(endpoint, timeout_s=0.2, context=context) + try: + description = client.describe() + client.reset("session-1", "demo") + state = np.zeros(68, dtype=np.float32) + state[58] = 1.0 + chunk = client.get_action( + session_id="session-1", + sequence_id=4, + onboard_monotonic_timestamp_ns=123, + task="demo", + jpeg_image=b"\xff\xd8test\xff\xd9", + state=state, + ) + assert description.policy_id == "test-policy" + np.testing.assert_allclose(chunk.actions, _safe_actions()) + assert all(set(request) == {"endpoint", "data"} for request in requests) + finally: + client.close() + thread.join(timeout=1.0) + server.close(linger=0) + context.term() + + +def test_policy_client_rejects_extra_response_envelope_fields() -> None: + client = object.__new__(HighLevelPolicyClient) + + with pytest.raises(PolicyProtocolError, match="exactly data"): + client._parse_reply( + { + "endpoint": "describe", + "ok": True, + "data": {}, + "extra": "not allowed", + }, + endpoint="describe", + ) + + +def test_high_level_policy_runtime_is_independent_from_pico_and_gmr() -> None: + started: list[str] = [] + + class FakeProcess: + def __init__(self, *, name: str, target, args) -> None: # type: ignore[no-untyped-def] + del target, args + self.name = name + + def start(self) -> None: + started.append(self.name) + + runtime = object.__new__(HighLevelPolicySim2RealRuntime) + runtime.cfg = { + "hands": {"enabled": True}, + "neck": {"enabled": True, "driver": "openneck"}, + } + runtime._ctx = SimpleNamespace(Process=FakeProcess) + runtime._endpoints = SimpleNamespace() + runtime._stop_event = SimpleNamespace() + runtime._processes = [] + + runtime._start_processes() + + assert started == ["camera", "high_level_policy", "robot_control", "policy_hand", "policy_neck"] + assert all("pico" not in name and "reference" not in name and "retarget" not in name for name in started) + + +def test_high_level_policy_runtime_config_requires_safety_joint_limits() -> None: + cfg = { + "input": {"provider": "high_level_policy"}, + "camera": {"source": "test-pattern", "width": 640, "height": 480, "fps": 30}, + "high_level_policy": {"enabled": True, "task": "demo"}, + "reference_steps": [0], + "recording": {"enabled": False}, + "hands": {"enabled": True, "driver": "linkerhand_o6", "sides": ["left", "right"]}, + "neck": {"enabled": True, "driver": "openneck"}, + "real_robot": {}, + } + + with pytest.raises(ValueError, match="joint_pos_lower"): + _validate_high_level_policy_runtime_config(cfg) + + +def test_standard_pico_runtime_rejects_high_level_policy_flag() -> None: + cfg = { + "input": {"provider": "pico4"}, + "high_level_policy": {"enabled": True}, + } + + with pytest.raises(ValueError, match="independent.*run_high_level_policy_sim2real.py"): + Sim2RealRuntime(cfg) + + +def test_high_level_policy_test_camera_is_exact_protocol_shape() -> None: + frame = _test_pattern(480, 640, 7) + assert frame.shape == (480, 640, 3) + assert frame.dtype == np.uint8 + assert np.all(frame[:, :, 2] == 7) + + +def test_openneck_policy_target_is_sent_directly_in_physical_degrees() -> None: + calls: list[tuple[float, float]] = [] + device = SimpleNamespace(move_deg=lambda yaw, pitch: calls.append((yaw, pitch))) + action = _safe_actions(1)[0] + action[48:50] = [12.5, -7.25] + target = HighLevelPolicyTargetPacket( + session_id="session-1", + action=action, + timestamp_s=1.0, + seq=1, + ) + + _apply_policy_neck_target(device, target) + + assert calls == [(12.5, -7.25)] + + +def test_policy_hardware_target_requires_current_session_and_timestamp() -> None: + action = _safe_actions(1)[0] + target = HighLevelPolicyTargetPacket( + session_id="session-1", + action=action, + timestamp_s=10.0, + seq=5, + ) + mode = ModeStatePacket( + mode="policy", + mocap_active=False, + mocap_paused=False, + timestamp_s=10.0, + seq=1, + policy_session_id="session-1", + ) + + assert _policy_target_is_current( + target, + mode, + last_target_seq=4, + max_age_s=0.2, + now_s=10.1, + ) + assert not _policy_target_is_current( + target, + mode, + last_target_seq=5, + max_age_s=0.2, + now_s=10.1, + ) + assert not _policy_target_is_current( + HighLevelPolicyTargetPacket( + session_id="old-session", + action=action, + timestamp_s=10.0, + seq=6, + ), + mode, + last_target_seq=4, + max_age_s=0.2, + now_s=10.1, + ) + assert not _policy_target_is_current( + target, + mode, + last_target_seq=4, + max_age_s=0.2, + now_s=10.3, + ) + assert not _policy_target_is_current( + target, + ModeStatePacket( + mode="policy", + mocap_active=False, + mocap_paused=False, + timestamp_s=10.0, + seq=2, + policy_paused=True, + policy_session_id="session-1", + ), + last_target_seq=4, + max_age_s=0.2, + now_s=10.1, + ) + + +def _remote(*, a: bool = False, b: bool = False, x: bool = False, y: bool = False): # type: ignore[no-untyped-def] + button = lambda pressed=False: SimpleNamespace(on_pressed=pressed, pressed=pressed) + return SimpleNamespace( + A=button(a), + B=button(b), + X=button(x), + Y=button(y), + start=button(False), + ) + + +def test_high_level_policy_y_requests_takeover_without_starting_mode_state() -> None: + worker = object.__new__(_RobotControlWorker) + worker.mode = RobotMode.STANDING + worker.remote = _remote(y=True) + worker._policy_entry_pending = False + requests: list[str] = [] + + def begin() -> None: + requests.append("begin") + worker._policy_entry_pending = True + + worker._begin_high_level_policy_entry = begin + worker._publish_high_level_policy_session = lambda *_args, **_kwargs: None + worker._high_level_policy_scheduler = SimpleNamespace(has_chunk=False) + worker._policy_entry_deadline_s = None + + worker._handle_high_level_policy_transitions() + + assert requests == ["begin"] + assert worker.mode == RobotMode.STANDING + + +def test_high_level_policy_body_action_uses_existing_tracker_without_second_alignment() -> None: + worker = object.__new__(_RobotControlWorker) + action = _safe_actions(1)[0] + worker._high_level_policy_scheduler = SimpleNamespace(sample=lambda _now: action.copy()) + worker._policy_frame_transform = SimpleNamespace( + delocalize_body_action=lambda body: np.asarray(body, dtype=np.float32) + np.float32(1.0) + ) + worker._policy_session_id = "session-1" + worker._policy_paused = False + worker._policy_resume_pending = False + worker.robot = SimpleNamespace(get_state=lambda: SimpleNamespace()) + worker._publish_high_level_policy_observation = lambda _state: None + worker._policy_control_pub = None + worker._policy_hold_qpos = None + calls: list[tuple[np.ndarray, dict[str, object]]] = [] + + def execute(reference, _state, **kwargs) -> None: # type: ignore[no-untyped-def] + calls.append((np.asarray(reference), kwargs)) + + worker._execute_reference_pipeline = execute + + worker._high_level_policy_step() + + assert len(calls) == 1 + np.testing.assert_allclose(calls[0][0], action[:36] + 1.0) + assert calls[0][1] == { + "reference_window": None, + "align_reference": False, + "compose_arms": False, + } + + +def test_policy_and_pico_remote_b_both_toggle_pause() -> None: + policy_worker = object.__new__(_RobotControlWorker) + policy_worker.mode = RobotMode.POLICY + policy_worker.remote = _remote(b=True) + policy_toggles: list[str] = [] + policy_worker._toggle_high_level_policy_pause = lambda: policy_toggles.append("policy") + policy_worker._handle_high_level_policy_transitions() + + pico_worker = object.__new__(_RobotControlWorker) + pico_worker.mode = RobotMode.MOCAP + pico_worker.provider_kind = "pico4" + pico_worker.remote = _remote(b=True) + pico_worker._mocap_session = SimpleNamespace(state=MocapSessionState.ACTIVE) + pico_commands: list[str] = [] + pico_worker._send_reference_command = pico_commands.append + pico_worker._pause_active_mocap = lambda: pico_commands.append("paused") + pico_worker._handle_transitions() + + assert policy_toggles == ["policy"] + assert pico_commands == ["pause_mocap", "paused"] + + +def test_policy_worker_pause_resume_retransmission_is_idempotent() -> None: + worker = object.__new__(HighLevelPolicyWorker) + worker._last_session_seq = -1 + worker._active_session = None + worker._ready = False + worker._paused = False + worker._last_observation_seq = -1 + worker._last_request_timestamp_ns = None + worker._next_connect_time_s = 0.0 + worker._new_session_required = False + statuses: list[str] = [] + worker._publish_status = lambda status, _detail: statuses.append(status) + + def packet(command: str, seq: int) -> HighLevelPolicySessionPacket: + return HighLevelPolicySessionPacket( + session_id="session-1", + task="demo", + command=command, + timestamp_s=1.0, + seq=seq, + ) + + worker._handle_session(packet("start", 1)) + worker._ready = True + worker._handle_session(packet("pause", 2)) + worker._handle_session(packet("pause", 3)) + worker._handle_session(packet("resume", 4)) + worker._handle_session(packet("resume", 5)) + + assert statuses == ["connecting", "paused", "ready"] + + +def test_policy_worker_resume_reconnects_faulted_current_session() -> None: + worker = object.__new__(HighLevelPolicyWorker) + worker._last_session_seq = -1 + worker._active_session = None + worker._ready = False + worker._paused = False + worker._last_observation_seq = -1 + worker._last_request_timestamp_ns = None + worker._next_connect_time_s = 0.0 + worker._new_session_required = False + statuses: list[str] = [] + worker._publish_status = lambda status, _detail: statuses.append(status) + + def packet(command: str, seq: int) -> HighLevelPolicySessionPacket: + return HighLevelPolicySessionPacket( + session_id="session-1", + task="demo", + command=command, + timestamp_s=1.0, + seq=seq, + ) + + worker._handle_session(packet("start", 1)) + worker._new_session_required = True + worker._handle_session(packet("pause", 2)) + worker._handle_session(packet("resume", 3)) + + assert statuses == ["connecting", "paused", "connecting"] + assert not worker._paused + assert not worker._new_session_required + + +def test_policy_fault_uses_normal_pause_without_entering_standing() -> None: + worker = object.__new__(_RobotControlWorker) + worker.high_level_policy_enabled = True + worker.mode = RobotMode.POLICY + worker._policy_paused = False + worker._policy_resume_pending = False + worker._policy_resume_deadline_s = None + worker._policy_resume_source_timestamp_ns = None + paused: list[float] = [] + worker._high_level_policy_scheduler = SimpleNamespace( + pause=lambda now_s: paused.append(float(now_s)) + ) + hold_qpos = np.arange(36, dtype=np.float64) + worker._resolve_mocap_hold_qpos = lambda: hold_qpos.copy() + published: list[str] = [] + worker._publish_high_level_policy_session = published.append + worker._enter_standing = lambda: pytest.fail("fault must not enter STANDING") + + worker._handle_high_level_policy_fault("network timeout") + + assert worker.mode == RobotMode.POLICY + assert worker._policy_paused + assert not worker._policy_resume_pending + assert len(paused) == 1 + assert published == ["pause"] + np.testing.assert_array_equal(worker._policy_hold_qpos, hold_qpos) + + +def test_policy_watchdog_pauses_and_holds_last_reference() -> None: + worker = object.__new__(_RobotControlWorker) + worker.high_level_policy_enabled = True + worker.mode = RobotMode.POLICY + worker._policy_paused = False + worker._policy_resume_pending = False + worker._policy_resume_deadline_s = None + worker._policy_resume_source_timestamp_ns = None + paused: list[float] = [] + worker._high_level_policy_scheduler = SimpleNamespace( + sample=lambda _now_s: None, + pause=lambda now_s: paused.append(float(now_s)), + ) + worker._policy_frame_transform = SimpleNamespace() + worker._policy_session_id = "session-1" + worker.robot = SimpleNamespace(get_state=lambda: SimpleNamespace()) + worker._publish_high_level_policy_observation = lambda _state: None + hold_qpos = np.arange(36, dtype=np.float64) + worker._last_commanded_motion_qpos = hold_qpos.copy() + worker._last_retarget_qpos = None + worker._publish_high_level_policy_session = lambda _command: None + held: list[np.ndarray] = [] + worker._run_static_mocap_step = lambda qpos: held.append(np.asarray(qpos).copy()) + + worker._high_level_policy_step() + + assert worker.mode == RobotMode.POLICY + assert worker._policy_paused + assert not worker._policy_resume_pending + assert len(paused) == 1 + assert len(held) == 1 + np.testing.assert_array_equal(held[0], hold_qpos) + + +def test_current_policy_fault_status_is_handled_even_while_paused() -> None: + worker = object.__new__(_RobotControlWorker) + worker._policy_video_sub = SimpleNamespace(recv_latest=lambda: None) + worker._policy_action_sub = SimpleNamespace(recv_latest=lambda: None) + worker._policy_status_sub = SimpleNamespace( + recv_latest=lambda: HighLevelPolicyStatusPacket( + session_id="session-1", + status="fault", + detail="network timeout", + timestamp_s=1.0, + seq=1, + ) + ) + worker._last_policy_video_seq = -1 + worker._last_policy_status_seq = -1 + worker._policy_session_id = "session-1" + worker._policy_paused = True + worker.mode = RobotMode.POLICY + handled: list[str] = [] + worker._handle_high_level_policy_fault = handled.append + + worker._drain_high_level_policy_ipc() + + assert handled == ["network timeout"] + + +def test_policy_pause_resume_waits_for_fresh_chunk_then_resumes() -> None: + worker = object.__new__(_RobotControlWorker) + worker._policy_paused = True + worker._policy_resume_pending = False + worker._policy_resume_deadline_s = None + worker._policy_resume_source_timestamp_ns = None + resumed: list[float] = [] + accepted: list[object] = [] + worker._high_level_policy_scheduler = SimpleNamespace( + accept=lambda *args, **kwargs: accepted.append((args, kwargs)), + resume=lambda now_s: resumed.append(float(now_s)), + ) + worker._high_level_policy_cfg = SimpleNamespace( + entry_timeout_s=1.0, + max_result_age_s=0.1, + ) + session_commands: list[str] = [] + worker._publish_high_level_policy_session = session_commands.append + + worker._toggle_high_level_policy_pause() + + assert worker._policy_paused + assert worker._policy_resume_pending + assert session_commands == ["resume"] + + source_timestamp_ns = worker._policy_resume_source_timestamp_ns + assert source_timestamp_ns is not None + worker._policy_video_sub = SimpleNamespace(recv_latest=lambda: None) + worker._policy_status_sub = SimpleNamespace(recv_latest=lambda: None) + worker._policy_action_sub = SimpleNamespace( + recv_latest=lambda: HighLevelPolicyActionPacket( + session_id="session-1", + source_sequence_id=1, + source_onboard_monotonic_timestamp_ns=source_timestamp_ns, + action_fps=30, + actions=_safe_actions(1), + policy_id="test", + server_inference_ms=1.0, + received_timestamp_s=time.monotonic(), + ) + ) + worker._last_policy_video_seq = -1 + worker._last_policy_status_seq = -1 + + worker._drain_high_level_policy_ipc() + + assert len(accepted) == 1 + assert len(resumed) == 1 + assert not worker._policy_paused + assert not worker._policy_resume_pending + + +def test_paused_robot_worker_discards_inflight_policy_result() -> None: + worker = object.__new__(_RobotControlWorker) + worker._policy_video_sub = SimpleNamespace(recv_latest=lambda: None) + worker._policy_status_sub = SimpleNamespace(recv_latest=lambda: None) + action = _safe_actions(1) + worker._policy_action_sub = SimpleNamespace( + recv_latest=lambda: HighLevelPolicyActionPacket( + session_id="session-1", + source_sequence_id=1, + source_onboard_monotonic_timestamp_ns=1, + action_fps=30, + actions=action, + policy_id="test", + server_inference_ms=1.0, + received_timestamp_s=1.0, + ) + ) + worker._last_policy_video_seq = -1 + worker._last_policy_status_seq = -1 + worker._policy_paused = True + worker._policy_resume_pending = False + accepted: list[object] = [] + worker._high_level_policy_scheduler = SimpleNamespace( + accept=lambda *args, **kwargs: accepted.append((args, kwargs)) + ) + worker._high_level_policy_cfg = SimpleNamespace(max_result_age_s=0.1) + + worker._drain_high_level_policy_ipc() + + assert accepted == [] From f45a36794073234913553d0612692b2d607386ee Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Sun, 19 Jul 2026 19:38:01 +0800 Subject: [PATCH 23/59] fix: cold-start GMR on mocap entry --- AGENTS.md | 2 +- docs/docs/tutorials/pico-sim2sim.md | 6 ++ .../current/tutorials/pico-sim2sim.md | 5 ++ teleopit/retargeting/gmr/motion_retarget.py | 55 +++++++++++++++-- teleopit/sim/session.py | 4 ++ tests/test_retargeting.py | 59 +++++++++++++++++++ tests/test_sim_loop.py | 20 +++++-- 7 files changed, 141 insertions(+), 10 deletions(-) diff --git a/AGENTS.md b/AGENTS.md index 70abf681..24d2cc78 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -204,7 +204,7 @@ target_dof_pos = clip(action, -10, 10) × action_scale + default_dof_pos - Realtime inferred `motion_joint_vel`, anchor linear velocity, and anchor angular velocity can be EMA-smoothed via `reference_velocity_smoothing_alpha` and `reference_anchor_velocity_smoothing_alpha` - Sim2real Pico pause/resume uses mocap-session states `ACTIVE ↔ PAUSED`; resume clears policy/reference state, rebuilds yaw/XY root alignment, and does not interpolate retarget qpos from the paused pose - Realtime sim2sim with Pico control events uses the same mocap-session pause/resume semantics and rebuilds the realtime reference path on resume, including the configured warmup -- Realtime sim2sim `STANDING ↔ MOCAP` transitions rebuild the realtime reference path on entry; Pico sim2real `STANDING -> MOCAP` additionally rearms and resets the process-isolated reference worker before accepting fresh references +- Realtime Pico sim2sim `STANDING -> MOCAP` resets GMR, seeds its floating root from the current live pelvis target, and rebuilds the realtime reference path before accepting references; Pico sim2real performs the same GMR cold start through its rearmed process-isolated reference worker - Realtime Pico sim2sim can start directly in `STANDING` with keyboard mode control enabled via top-level `keyboard.enabled` ### Inference Observation diff --git a/docs/docs/tutorials/pico-sim2sim.md b/docs/docs/tutorials/pico-sim2sim.md index ff354dd9..cda722f0 100644 --- a/docs/docs/tutorials/pico-sim2sim.md +++ b/docs/docs/tutorials/pico-sim2sim.md @@ -79,6 +79,12 @@ enter `MOCAP`. sim2sim viewers. Use `viewers=sim2sim` or `viewers=none` when you want fewer windows. +Each `STANDING -> MOCAP` entry resets GMR, seeds its floating root from the +current live pelvis target, and rebuilds the realtime reference path. The +operator can therefore change heading while in `STANDING` without reusing the +previous mocap session's IK warm-start. Pause/resume and `MOCAP <-> ARMS` +switches retain the current IK warm-start. + ## Pause / Resume Pico pause/resume freezes the mocap session; it is not a switch back to diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2sim.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2sim.md index 02eb6143..df1c286d 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2sim.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2sim.md @@ -76,6 +76,11 @@ python scripts/run/run_sim.py \ `pico4_sim.yaml` 默认使用 `viewers=all`，会打开 mocap、retarget 和 sim2sim 三个 viewer。需要更少窗口时，可使用 `viewers=sim2sim` 或 `viewers=none`。 +每次从 `STANDING` 进入 `MOCAP` 时，Teleopit 都会重置 GMR、使用当前实时 pelvis +目标初始化其浮动根，并重建实时参考路径。因此，操作者可以在 `STANDING` 中改变朝向， +而不会复用上一次 mocap session 的 IK warm-start。暂停/恢复和 +`MOCAP <-> ARMS` 切换会保留当前 IK warm-start。 + ## 暂停 / 恢复 Pico 暂停/恢复会冻结 mocap session；它不是切回 `STANDING`。 diff --git a/teleopit/retargeting/gmr/motion_retarget.py b/teleopit/retargeting/gmr/motion_retarget.py index c97400f9..2958e0b3 100644 --- a/teleopit/retargeting/gmr/motion_retarget.py +++ b/teleopit/retargeting/gmr/motion_retarget.py @@ -165,13 +165,57 @@ def reset_configuration(self): pause/resume) so the warm-start IK solver does not get stuck in a local minimum far from the new target. - The next ``retarget()`` call will use many more iterations so the - solver can converge from the default pose to the (potentially distant) - new target. + The next ``retarget()`` call will seed the floating root from the live + target and use many more iterations so the articulated joints can + converge from their default pose. """ self.configuration.update(q=self.model.qpos0.copy()) self._warmup_needed = True + def _seed_warmup_root_from_target(self): + """Seed a floating root from the current human-root target. + + Starting every reset from the model's fixed world heading can make the + nonlinear IK solve converge to a different joint branch when the live + subject faces some directions. The root target is already known after + ``update_targets()``, so initialize only the floating root from it and + leave all articulated joints at their default values. + """ + root_body_id = mj.mj_name2id( + self.model, + mj.mjtObj.mjOBJ_BODY, + self.robot_root_name, + ) + if root_body_id < 0: + raise ValueError(f"Robot root body '{self.robot_root_name}' was not found") + + free_joint_id = None + joint_start = int(self.model.body_jntadr[root_body_id]) + joint_count = int(self.model.body_jntnum[root_body_id]) + for joint_id in range(joint_start, joint_start + joint_count): + if self.model.jnt_type[joint_id] == mj.mjtJoint.mjJNT_FREE: + free_joint_id = joint_id + break + if free_joint_id is None: + return + + root_pos, root_quat = self.scaled_human_data[self.human_root_name] + root_pos = np.asarray(root_pos, dtype=np.float64).reshape(-1) + root_quat = np.asarray(root_quat, dtype=np.float64).reshape(-1) + if root_pos.shape != (3,) or not np.all(np.isfinite(root_pos)): + raise ValueError(f"Human root position must be finite 3D, got {root_pos}") + if root_quat.shape != (4,) or not np.all(np.isfinite(root_quat)): + raise ValueError(f"Human root quaternion must be finite wxyz, got {root_quat}") + quat_norm = float(np.linalg.norm(root_quat)) + if quat_norm <= 1e-9: + raise ValueError("Human root quaternion norm must be positive") + + q_seed = self.model.qpos0.copy() + qpos_adr = int(self.model.jnt_qposadr[free_joint_id]) + q_seed[qpos_adr:qpos_adr + 3] = root_pos + q_seed[qpos_adr + 3:qpos_adr + 7] = root_quat / quat_norm + self.configuration.update(q=q_seed) + def setup_retarget_configuration(self): self.configuration = mink.Configuration(self.model) @@ -244,10 +288,11 @@ def retarget(self, human_data, offset_to_ground=False): # Update the task targets self.update_targets(human_data, offset_to_ground) - # After a reset, use a large dt and more iterations so the solver - # can converge from the default pose to a potentially distant target. + # After a reset, seed the floating root and use a large dt plus more + # iterations so the articulated joints can converge from defaults. warmup = self._warmup_needed if warmup: + self._seed_warmup_root_from_target() self._warmup_needed = False iter_limit = self._warmup_max_iter if warmup else self.max_iter dt = self._warmup_dt if warmup else self.configuration.model.opt.timestep diff --git a/teleopit/sim/session.py b/teleopit/sim/session.py index c404200c..7f740235 100644 --- a/teleopit/sim/session.py +++ b/teleopit/sim/session.py @@ -252,6 +252,10 @@ def enter_mocap_mode(self) -> bool: return False state = loop.robot.get_state() start_qpos = loop._resolve_hold_qpos(None, None, None, state) + # STANDING does not run retargeting, so the live subject may have + # changed pose or heading discontinuously since the previous MOCAP + # session. Cold-start GMR from the current live root on the next frame. + self._retargeter.reset() self.reset_policy_reference_state() self._step_runner.last_retarget_qpos = start_qpos.copy() self.last_commanded_motion_qpos = start_qpos.copy() diff --git a/tests/test_retargeting.py b/tests/test_retargeting.py index 3d0cd8a1..c2c5fb64 100644 --- a/tests/test_retargeting.py +++ b/tests/test_retargeting.py @@ -7,6 +7,7 @@ """ import json from pathlib import Path +from types import SimpleNamespace from unittest.mock import MagicMock import numpy as np @@ -170,3 +171,61 @@ def test_pico_bridge_g1_foot_ik_uses_canonical_foot_sites(self): assert ik_config[table_name]["left_foot"][0] == "Left_Foot" assert ik_config[table_name]["right_foot"][-1] == "site" assert ik_config[table_name]["right_foot"][0] == "Right_Foot" + + +@requires_mujoco +@requires_mink +class TestGmrResetWarmup: + def test_warmup_seeds_floating_root_from_current_target(self, monkeypatch): + import mujoco + + from teleopit.retargeting.gmr.motion_retarget import GeneralMotionRetargeting + + gmr = object.__new__(GeneralMotionRetargeting) + gmr.robot_root_name = "pelvis" + gmr.human_root_name = "Pelvis" + gmr.scaled_human_data = { + "Pelvis": ( + np.array([1.0, -2.0, 0.83], dtype=np.float64), + np.array([0.0, 0.0, 0.0, 2.0], dtype=np.float64), + ) + } + gmr.model = SimpleNamespace( + qpos0=np.arange(36, dtype=np.float64), + body_jntadr=np.array([0, 0], dtype=np.int32), + body_jntnum=np.array([0, 1], dtype=np.int32), + jnt_type=np.array([mujoco.mjtJoint.mjJNT_FREE], dtype=np.int32), + jnt_qposadr=np.array([0], dtype=np.int32), + ) + gmr.configuration = SimpleNamespace(update=MagicMock()) + monkeypatch.setattr(mujoco, "mj_name2id", lambda *_args: 1) + + gmr._seed_warmup_root_from_target() + + seeded = gmr.configuration.update.call_args.kwargs["q"] + np.testing.assert_allclose(seeded[:3], [1.0, -2.0, 0.83]) + np.testing.assert_allclose(seeded[3:7], [0.0, 0.0, 0.0, 1.0]) + np.testing.assert_allclose(seeded[7:], gmr.model.qpos0[7:]) + + def test_retarget_applies_root_seed_only_on_reset_warmup(self): + from teleopit.retargeting.gmr.motion_retarget import GeneralMotionRetargeting + + gmr = object.__new__(GeneralMotionRetargeting) + gmr.update_targets = MagicMock() + gmr._seed_warmup_root_from_target = MagicMock() + gmr._warmup_needed = True + gmr._warmup_max_iter = 200 + gmr._warmup_dt = 0.1 + gmr.max_iter = 10 + gmr.use_ik_match_table1 = False + gmr.use_ik_match_table2 = False + gmr.configuration = SimpleNamespace( + data=SimpleNamespace(qpos=np.zeros(36, dtype=np.float64)), + model=SimpleNamespace(opt=SimpleNamespace(timestep=0.005)), + ) + + gmr.retarget({}) + gmr.retarget({}) + + gmr._seed_warmup_root_from_target.assert_called_once_with() + assert gmr._warmup_needed is False diff --git a/tests/test_sim_loop.py b/tests/test_sim_loop.py index 11cdebd8..3f1521d7 100644 --- a/tests/test_sim_loop.py +++ b/tests/test_sim_loop.py @@ -604,6 +604,7 @@ def __init__(self) -> None: (), (TerminalKeyEvent("y"),), (TerminalKeyEvent("x"),), + (TerminalKeyEvent("y"),), ] self._idx = 0 @@ -642,16 +643,19 @@ def close(self) -> None: viewers=set(), ) + retargeter = _DummyRetargeter() result = loop.run( input_provider=_RealtimeInputProvider(), - retargeter=_DummyRetargeter(), - num_steps=3, + retargeter=retargeter, + num_steps=4, ) - assert result["steps"] == 3 + assert result["steps"] == 4 np.testing.assert_allclose(obs_builder.mimic_obs_calls[0], np.array([0.0], dtype=np.float32), atol=1e-6) np.testing.assert_allclose(obs_builder.mimic_obs_calls[1], np.array([0.3], dtype=np.float32), atol=1e-6) np.testing.assert_allclose(obs_builder.mimic_obs_calls[2], np.array([0.0], dtype=np.float32), atol=1e-6) + np.testing.assert_allclose(obs_builder.mimic_obs_calls[3], np.array([0.6], dtype=np.float32), atol=1e-6) + assert retargeter.reset_calls == 2 @requires_mujoco @@ -716,6 +720,9 @@ def get_realtime_input_packet(self): return packet class _Retargeter: + def __init__(self) -> None: + self.reset_calls = 0 + def retarget(self, frame: object) -> np.ndarray: pelvis = np.asarray(frame["Pelvis"][0], dtype=np.float64) qpos = np.zeros(36, dtype=np.float64) @@ -725,6 +732,9 @@ def retarget(self, frame: object) -> np.ndarray: qpos[8] = pelvis[0] + 10.0 return qpos + def reset(self) -> None: + self.reset_calls += 1 + class _KeyboardReader: def __init__(self) -> None: self._polls = [ @@ -770,9 +780,10 @@ def close(self) -> None: viewers=set(), ) + retargeter = _Retargeter() result = loop.run( input_provider=_RealtimeInputProvider(), - retargeter=_Retargeter(), + retargeter=retargeter, num_steps=4, ) @@ -784,6 +795,7 @@ def close(self) -> None: # Step 3 toggles back to full-body MOCAP; root XY is reanchored, while non-arm joints follow retarget again. np.testing.assert_allclose(obs_builder.motion_qpos_calls[3][0], 0.0, atol=1e-6) np.testing.assert_allclose(obs_builder.motion_qpos_calls[3][7], 1.2, atol=1e-6) + assert retargeter.reset_calls == 1 @requires_mujoco From 2fd01b1fb7b972da1174e44e2ecd198fb06ed280 Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Sun, 19 Jul 2026 20:08:34 +0800 Subject: [PATCH 24/59] Handle transient RealSense frame timeouts --- teleopit/inputs/pico_video.py | 10 +++++++++- tests/test_pico_video.py | 8 +++++++- 2 files changed, 16 insertions(+), 2 deletions(-) diff --git a/teleopit/inputs/pico_video.py b/teleopit/inputs/pico_video.py index 838025dc..c3ae173f 100644 --- a/teleopit/inputs/pico_video.py +++ b/teleopit/inputs/pico_video.py @@ -211,7 +211,15 @@ def _run(self) -> None: self._ready_event.set() try: while not self._stop_event.is_set(): - frames = pipeline.wait_for_frames() + try: + frames = pipeline.wait_for_frames() + except RuntimeError as exc: + message = str(exc).lower() + is_timeout = "timeout" in message or "timed out" in message or "frame didn't arrive" in message + if self._config.fail_on_error or not is_timeout: + raise + logger.warning("RealSense Pico video frame timeout; continuing: %s", exc) + continue color_frame = frames.get_color_frame() if not color_frame: continue diff --git a/tests/test_pico_video.py b/tests/test_pico_video.py index 5867a1ce..fb8e4fd4 100644 --- a/tests/test_pico_video.py +++ b/tests/test_pico_video.py @@ -39,7 +39,7 @@ def test_pico_video_config_rejects_enabled_unknown_source() -> None: parse_pico_video_config({"video": {"enabled": True, "source": "webcam"}}) -def test_realsense_video_runtime_pushes_rgb_frames(monkeypatch: pytest.MonkeyPatch) -> None: +def test_realsense_video_runtime_continues_after_frame_timeout(monkeypatch: pytest.MonkeyPatch) -> None: fake_rs = ModuleType("pyrealsense2") fake_rs.stream = SimpleNamespace(color="color") fake_rs.format = SimpleNamespace(rgb8="rgb8") @@ -60,11 +60,17 @@ def get_color_frame(self) -> FakeColorFrame: return FakeColorFrame() class FakePipeline: + def __init__(self) -> None: + self.calls = 0 + def start(self, _config: object) -> None: pass def wait_for_frames(self) -> FakeFrames: + self.calls += 1 time.sleep(0.005) + if self.calls == 1: + raise RuntimeError("Frame didn't arrive within 5000") return FakeFrames() def stop(self) -> None: From 850be5e267c29ff8339e13ccebf466bd8fd82484 Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Mon, 20 Jul 2026 15:02:37 +0800 Subject: [PATCH 25/59] fix: harden high-level policy entry --- AGENTS.md | 4 +- README.md | 18 +- docs/docs/configuration/config-reference.md | 2 +- docs/docs/reference/architecture.md | 2 +- .../tutorials/high-level-policy-sim2real.md | 43 +- .../current/configuration/config-reference.md | 2 +- .../current/reference/architecture.md | 2 +- .../tutorials/high-level-policy-sim2real.md | 33 +- teleopit/high_level_policy/scheduler.py | 30 +- teleopit/sim2real/mp/runtime.py | 184 +++++++-- teleopit/sim2real/safety.py | 4 + tests/test_high_level_policy.py | 368 +++++++++++++++++- 12 files changed, 621 insertions(+), 71 deletions(-) diff --git a/AGENTS.md b/AGENTS.md index 24d2cc78..1fa0c20a 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -184,10 +184,10 @@ target_dof_pos = clip(action, -10, 10) × action_scale + default_dof_pos - Policy observation is RGB JPEG plus `observation.state(68)`; only `state[58:62]` is rotated into the session-local yaw frame - Canonical action is `float32[T,50]`: local root `xyz(3)` + local root quaternion `wxyz(4)` + G1 joint reference `29` + left/right O6 closure `12` + OpenNeck yaw/pitch degrees `2` - The body reference `[0:36]` is yaw/XY-delocalized once and sent through the existing motion tracker. It is never sent directly as a G1 motor command and must not pass through the mocap alignment a second time -- High-level-policy formal robot modes are `IDLE`, `STANDING`, `POLICY`, and `DAMPING`. After Unitree remote `Y`, first-chunk handshake remains an internal pending condition while the robot stays in `STANDING`; do not add a `POLICY_STARTING` mode +- High-level-policy formal robot modes are `IDLE`, `STANDING`, `POLICY`, and `DAMPING`. After Unitree remote `Y`, entry remains internal to `STANDING`: validate one candidate chunk without its measured-pose-to-first-frame G1 joint-rate boundary, hold `action[0]` through the existing tracker for one Kp ramp, then create one new host session and require a fresh normally validated chunk before entering `POLICY`; a failure aborts entry, and there is no `POLICY_STARTING` mode - Unitree remote controls are `Start -> STANDING`, `Y -> request POLICY`, `B -> pause/resume`, `X -> STANDING/cancel pending`, and `L1+R1 -> DAMPING` - Policy pause freezes the scheduler/body reference and holds the latest hand/neck command. Leaving `POLICY` opens LinkerHand and centers OpenNeck -- The onboard scheduler rejects whole chunks on shape/finiteness, session/sequence, quaternion, root displacement/height/speed/yaw-rate, G1 joint position/rate, hand closure, OpenNeck degree range, or staleness failures; never pad, trim, or safety-clip invalid host output +- The onboard scheduler rejects whole chunks on shape/finiteness, session/sequence, quaternion, root displacement/height/speed/yaw-rate, G1 joint position/rate, hand closure, OpenNeck degree range, or staleness failures; the entry candidate exempts only its measured-pose-to-first-frame G1 joint-rate boundary, while root boundary checks and every transition inside the candidate remain mandatory; never pad, trim, or safety-clip invalid host output - A short cached-reference grace period is allowed; exhaustion, host/network failure, or loss of a required camera/client worker puts `POLICY` into the same resumable pause state used by remote `B`. The runtime never enters `STANDING` automatically; `B` resumes after a fresh valid chunk is available, while `X` remains the manual transition to `STANDING` - Initial production hardware support requires two LinkerHand O6 hands and OpenNeck because all 50 canonical action dimensions are active - OpenNeck 0.2.0 is the supported runtime; Teleopit sends physical degrees through `move_deg()`, and the direct-drive OpenNeck package converts degrees to servo steps and clips them to its calibrated mechanical limits; the removed normalized API and config fields are unsupported diff --git a/README.md b/README.md index e3fd3380..c61c300e 100644 --- a/README.md +++ b/README.md @@ -150,13 +150,17 @@ python scripts/run/run_high_level_policy_sim2real.py \ Use the Unitree remote: `Start` enters `STANDING`, `Y` requests policy takeover, `B` pauses/resumes, `X` returns to `STANDING`, and `L1+R1` enters -`DAMPING`. While the first host chunk is being checked, the robot remains in -`STANDING`; there is no separate starting mode. Invalid/stale chunks and -watchdog expiry cannot block the local control loop and instead pause `POLICY` -while holding the last reference. Host/network failure and loss of a required -camera/client worker use the same ordinary pause state as remote `B`; after -recovery, press `B` to resume on a fresh valid chunk. The runtime never enters -`STANDING` automatically, and `X` remains the manual transition. +`DAMPING`. Policy entry remains an internal `STANDING` phase with no separate +starting mode: Teleopit validates a candidate chunk, holds its first body +reference through one motion-tracker Kp ramp, then creates one fresh host +session. A normally validated chunk from that session is required before +entering `POLICY`, so Replay restarts from its configured start frame and ACT +recomputes from the post-ramp observation. Entry failure returns to `STANDING`. +Invalid/stale live chunks and watchdog expiry cannot block the local control +loop and instead pause `POLICY` while holding the last reference. Host/network +failure and loss of a required camera/client worker use the same ordinary pause +state as remote `B`; after recovery, press `B` to resume on a fresh valid chunk. +Only `X` returns active `POLICY` to `STANDING`. The current client/server code and protocol tests define the network message structure. During active development, Teleopit and `lerobot-teleopit` must be diff --git a/docs/docs/configuration/config-reference.md b/docs/docs/configuration/config-reference.md index 0b112ab2..303c55e6 100644 --- a/docs/docs/configuration/config-reference.md +++ b/docs/docs/configuration/config-reference.md @@ -128,7 +128,7 @@ protocol tests. The only shared data file is `hand_calibration.json`. | `high_level_policy.jpeg_quality` | JPEG quality for the 640x480 RGB frame | `90` | | `high_level_policy.max_observation_age_s` | Maximum camera/observation age before a request is skipped | `0.15` | | `high_level_policy.max_result_age_s` | Maximum local IPC age before a received result is rejected | `0.1` | -| `high_level_policy.entry_timeout_s` | Maximum wait for the first valid chunk while remaining in `STANDING` | `3.0` | +| `high_level_policy.entry_timeout_s` | Maximum total entry duration (candidate, Kp ramp, fresh session) and maximum fresh-chunk wait on resume | `3.0` | | `high_level_policy.hold_s` | Final validated-reference grace period before the watchdog pauses `POLICY` | `0.1` | | `high_level_policy.safety.root_height_min_m` / `root_height_max_m` | Accepted absolute root-height range | `0.55` / `1.05` | | `high_level_policy.safety.max_root_xy_speed_m_s` | Maximum root XY speed across 30 Hz references | `2.5` | diff --git a/docs/docs/reference/architecture.md b/docs/docs/reference/architecture.md index 90d323ec..72677de8 100644 --- a/docs/docs/reference/architecture.md +++ b/docs/docs/reference/architecture.md @@ -94,7 +94,7 @@ train_mimic/scripts/data - sim2real also requires a dual-input ONNX whose observation dimension matches the runtime builder - Host-policy message-envelope or schema mismatches are rejected while the robot remains in `STANDING` - Host action chunks are validated and interpolated onboard; the host cannot bypass the motion tracker or send motor commands -- Waiting for the first host chunk is not a robot mode: the formal takeover mode is only `POLICY` +- Policy entry remains internal to `STANDING`: hold one validated candidate first frame for a Kp ramp, then require a fully validated chunk from one fresh host session; the only formal takeover mode is `POLICY` ## Public Surface diff --git a/docs/docs/tutorials/high-level-policy-sim2real.md b/docs/docs/tutorials/high-level-policy-sim2real.md index 0a463637..0b547960 100644 --- a/docs/docs/tutorials/high-level-policy-sim2real.md +++ b/docs/docs/tutorials/high-level-policy-sim2real.md @@ -136,11 +136,20 @@ Keep the Unitree remote in hand. The runtime has only the formal robot modes | Unitree remote `X` | Return to `STANDING` or cancel a pending request | | Unitree remote `L1+R1` | Emergency transition to `DAMPING` | -After `Y`, Teleopit creates a new session, establishes the current root XY/yaw -anchor, and waits for the first compatible, fully validated action chunk. The -robot remains formally in `STANDING` during this handshake; there is no -separate "policy starting" state. It enters `POLICY` only after that first -chunk is ready. A timeout leaves the robot in `STANDING`. +After `Y`, Teleopit creates an entry session, establishes the current root +XY/yaw anchor, and requests one candidate chunk. All absolute limits, root +boundary limits, and transitions inside that chunk are validated. Only the G1 +joint-rate boundary from the measured pose to `action[0]` is excluded. Teleopit +then freezes `action[0]` as a static body reference and uses the existing +motion tracker for one Kp ramp. + +The robot remains formally in `STANDING` throughout entry; there is no separate +"policy starting" state. When the ramp finishes, Teleopit creates a second +session, which resets ReplayPolicy to its configured start frame (frame 0 by +default) or resets ACT state, and requests a fresh chunk from the post-ramp +observation. That fresh chunk must pass normal validation, including the +measured-pose-to-first-frame joint-rate boundary, before the runtime enters +`POLICY`. A failure or timeout safely returns to the normal standing reference. Pause freezes the body reference and holds the last LinkerHand and OpenNeck commands. Resume requests a fresh action chunk while continuing to hold the @@ -165,6 +174,14 @@ pads, trims, or safety-clips a malformed host result. Checks include: - LinkerHand closure `[0,1]` and configured OpenNeck degree ranges; - observation/result age, source timestamp, and action horizon. +The entry candidate has one narrow exception: its measured-pose-to-first-frame +G1 joint-rate boundary is handled by static tracker alignment instead of chunk +rejection. Root boundary checks and all transitions inside the candidate remain +mandatory. Host requests are paused for one Kp ramp. A new host session then +supplies the fresh chunk that will actually enter `POLICY`; the candidate chunk +is never continued as a live timeline. A rejected fresh chunk aborts entry +instead of starting another alignment cycle. + Validated 30 Hz body references are interpolated and rate-limited locally at 50 Hz, including when latency skips source frames or a new chunk replaces the old plan. A short configured grace period can reuse the final validated @@ -182,13 +199,15 @@ data, G1 joint limits, and the installed OpenNeck calibration. ## 7. Troubleshooting **`Y` never enters `POLICY`:** check the host endpoint, firewall, server log, -`describe` schemas, message envelope, task, checkpoint manifest, and -`replan_steps`. Teleopit stays -in `STANDING` by design when any handshake or first-chunk check fails. - -**The first chunk is rejected for rate limits:** the first predicted reference -is too far from the current G1 pose. Start from the demonstrated standing pose -or fix the policy/replay start frame; do not bypass the boundary check. +`describe` schemas, message envelope, task, checkpoint manifest, +`replan_steps`, and the entry logs. Teleopit stays in `STANDING` while it aligns +to the first reference and when any candidate or fresh-chunk check fails. + +**The fresh entry chunk is rejected or entry times out:** verify that the +episode starts with a stable pose, inspect the joint ordering and absolute +reference convention, and check whether the inherited +`standing_return_ramp_duration` is sufficient. Do not disable the +chunk-internal rate checks. **Policy runs briefly and becomes paused:** inspect timeout, inference latency, stale-result, worker-exit, and safety-rejection logs. The low-level diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md index 1622b670..1c602412 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md @@ -146,7 +146,7 @@ client/server 消息结构与协议测试。唯一共享的数据文件是 `hand | `high_level_policy.jpeg_quality` | 640x480 RGB 帧的 JPEG 质量 | `90` | | `high_level_policy.max_observation_age_s` | 跳过请求前允许的最大 camera/observation age | `0.15` | | `high_level_policy.max_result_age_s` | 拒绝已接收结果前允许的最大本地 IPC age | `0.1` | -| `high_level_policy.entry_timeout_s` | 保持 `STANDING` 等待首个有效 chunk 的最长时间 | `3.0` | +| `high_level_policy.entry_timeout_s` | 候选请求、Kp ramp 和新 session 的 entry 最长总时间，以及恢复时等待新鲜 chunk 的最长时间 | `3.0` | | `high_level_policy.hold_s` | Watchdog 暂停 `POLICY` 前，最后一条有效 reference 的 grace period | `0.1` | | `high_level_policy.safety.root_height_min_m` / `root_height_max_m` | 可接受的绝对 root 高度范围 | `0.55` / `1.05` | | `high_level_policy.safety.max_root_xy_speed_m_s` | 30 Hz reference 之间允许的最大 root XY 速度 | `2.5` | diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/architecture.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/architecture.md index d16066c2..54761675 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/architecture.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/architecture.md @@ -91,7 +91,7 @@ train_mimic/scripts/data - sim2real 也要求双输入 ONNX，且观测维度必须与运行时 builder 匹配 - 主机策略消息 envelope 或 schema 不匹配时会被拒绝，机器人保持在 `STANDING` - 主机 action chunk 在 onboard 完成验证与插值；主机不能绕过 motion tracker 或发送电机命令 -- 等待第一个主机 chunk 不是机器人模式：正式接管模式只有 `POLICY` +- 策略 entry 保持为 `STANDING` 内部流程：通过一次 Kp ramp 保持经过验证的候选第一帧，然后要求一个新 host session 提供完整通过验证的 chunk；正式接管模式只有 `POLICY` ## 公共接口 diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md index ccfddc56..43c5e2fd 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md @@ -128,10 +128,18 @@ python scripts/run/run_high_level_policy_sim2real.py \ | Unitree remote `X` | 返回 `STANDING`，或取消等待中的请求 | | Unitree remote `L1+R1` | 紧急切换到 `DAMPING` | -按下 `Y` 后，Teleopit 会创建新 session，以当前 root XY/yaw 建立锚点，并等待第一个 -兼容且完整通过验证的 action chunk。握手期间机器人在形式上仍处于 `STANDING`；没有 -单独的“policy starting”状态。只有首个 chunk 就绪后才会进入 `POLICY`。超时后机器人 -仍保持 `STANDING`。 +按下 `Y` 后，Teleopit 会创建 entry session，以当前 root XY/yaw 建立锚点，并请求一个 +候选 chunk。该 chunk 的所有绝对限制、root 边界限制和 chunk 内部跳变都会进行验证； +仅不检查实测姿态到 `action[0]` 的 G1 关节 rate 边界。随后 Teleopit 会冻结 +`action[0]` 作为静态 body reference，并通过现有 motion tracker 在一次 Kp ramp 期间 +跟踪该 reference。 + +整个 entry 期间，机器人在形式上仍处于 `STANDING`；没有单独的“policy starting” +状态。Kp ramp 结束后，Teleopit 会创建第二个 session：它会把 ReplayPolicy 重置到所 +配置的起始帧（默认为第 0 帧），或重置 ACT 状态，并根据 ramp 后的 observation 请求 +新 chunk。该新 chunk +必须通过包含“实测姿态到第一帧关节 rate 边界”在内的正常验证，运行时才会进入 +`POLICY`。失败或超时会安全地返回普通 standing reference。 暂停会冻结 body reference，并保持最后一条 LinkerHand 和 OpenNeck 命令。恢复时会请求 新的 action chunk，并在等待期间继续保持暂停姿态。按 `X` 会停止策略 session；运行时 @@ -154,6 +162,12 @@ Watchdog、主机/网络、相机或 policy client 故障也会进入同一个 - LinkerHand closure `[0,1]` 和配置的 OpenNeck 角度范围； - observation/result 时效、source timestamp 和 action horizon。 +entry 候选 chunk 只有一个严格限定的例外：实测姿态到第一帧的 G1 关节 rate 边界由 +静态 tracker 对齐处理，而不是直接拒绝 chunk。root 边界检查和候选 chunk 内部的所有 +跳变仍然是强制检查项。一次 Kp ramp 期间会暂停主机请求，随后新 host session 会提供 +真正进入 `POLICY` 的新鲜 chunk；候选 chunk 绝不会作为实时 timeline 继续播放。若新鲜 +chunk 被拒绝，entry 会直接终止，不会开始另一轮对齐。 + 通过验证的 30 Hz body reference 会在本地插值到 50 Hz 并执行 rate limit；网络延迟 导致跳过 source frame 或新 chunk 替换旧计划时同样如此。在短暂推理延迟期间，可以在 配置的短 grace period 内继续使用最后一条已验证 reference。如果不再有有效 action， @@ -169,12 +183,13 @@ Watchdog、主机/网络、相机或 policy client 故障也会进入同一个 ## 7. 故障排查 **按 `Y` 后始终不进入 `POLICY`：** 检查主机 endpoint、防火墙、server 日志、 -`describe` schema、消息 envelope、task、checkpoint manifest 和 `replan_steps`。 -任何握手或首个 chunk 检查失败时， -Teleopit 都会按设计保持 `STANDING`。 +`describe` schema、消息 envelope、task、checkpoint manifest、`replan_steps` 和 entry +日志。Teleopit 在对齐第一帧 reference，以及候选 chunk 或新鲜 chunk 检查失败时，都会 +保持 `STANDING`。 -**首个 chunk 因 rate limit 被拒绝：** 第一条预测 reference 距离当前 G1 姿态太远。 -请从示范的站立姿态启动，或修正 policy/replay 起始帧；不要绕过边界检查。 +**新鲜 entry chunk 被拒绝或 entry 超时：** 请确认 episode 从稳定姿态开始，检查关节 +顺序和绝对 reference 约定，并确认继承的 `standing_return_ramp_duration` 是否足够。 +不要关闭 chunk 内部 rate 检查。 **策略短暂运行后进入暂停：** 检查 timeout、推理延迟、stale result、worker 退出和 安全拒绝日志。底层 50 Hz tracker 不会等待主机推理。恢复故障输入路径后按 `B` 继续。 diff --git a/teleopit/high_level_policy/scheduler.py b/teleopit/high_level_policy/scheduler.py index 6e2a6176..2180952f 100644 --- a/teleopit/high_level_policy/scheduler.py +++ b/teleopit/high_level_policy/scheduler.py @@ -170,6 +170,27 @@ def clear(self) -> None: self._last_output_action = None def accept(self, chunk: PolicyActionChunk, *, now_s: float) -> None: + self._accept(chunk, now_s=now_s, validate_joint_boundary=True) + + def accept_entry(self, chunk: PolicyActionChunk, *, now_s: float) -> np.ndarray: + """Accept an entry candidate without comparing its first joint target. + + Root boundary limits, absolute limits, and every transition inside the + chunk are still validated. The runtime tracks the first action for one + Kp ramp, then starts a fresh policy session whose first chunk uses the + normal boundary validation path. + """ + self._accept(chunk, now_s=now_s, validate_joint_boundary=False) + assert self._chunk is not None + return self._chunk.actions[0].copy() + + def _accept( + self, + chunk: PolicyActionChunk, + *, + now_s: float, + validate_joint_boundary: bool, + ) -> None: if not np.isfinite(now_s): raise ValueError("High-level policy scheduler now_s must be finite") if self._session_id is None or chunk.session_id != self._session_id: @@ -223,6 +244,7 @@ def accept(self, chunk: PolicyActionChunk, *, now_s: float) -> None: chunk.actions, action_fps=chunk.action_fps, boundary_action=boundary_action, + validate_joint_boundary=validate_joint_boundary, ) valid_until_s = source_s + len(actions) / float(chunk.action_fps) + self.hold_s if float(now_s) > valid_until_s: @@ -349,6 +371,7 @@ def _validate_actions( *, action_fps: int, boundary_action: np.ndarray | None, + validate_joint_boundary: bool, ) -> np.ndarray: actions = np.asarray(values) if actions.ndim != 2 or actions.shape[1] != ACTION_DIM or not 1 <= len(actions) <= 15: @@ -374,6 +397,7 @@ def _validate_actions( validated, action_fps=action_fps, boundary_action=boundary_action, + validate_joint_boundary=validate_joint_boundary, safety=safety, ) return validated @@ -397,6 +421,7 @@ def _validate_safety_limits( *, action_fps: int, boundary_action: np.ndarray | None, + validate_joint_boundary: bool, safety: HighLevelPolicySafetyConfig, ) -> None: root_height = actions[:, 2] @@ -477,7 +502,10 @@ def _validate_safety_limits( f"{max_yaw_rate:.6g} > {safety.max_yaw_rate_rad_s:.6g} rad/s" ) - joint_rate = np.abs(np.diff(sequence[:, 7:36], axis=0)) * float(action_fps) + joint_sequence = sequence if validate_joint_boundary else actions + if len(joint_sequence) < 2: + return + joint_rate = np.abs(np.diff(joint_sequence[:, 7:36], axis=0)) * float(action_fps) max_joint_rate = float(np.max(joint_rate)) if max_joint_rate > safety.max_joint_rate_rad_s: raise ValueError( diff --git a/teleopit/sim2real/mp/runtime.py b/teleopit/sim2real/mp/runtime.py index ccc91150..0ce24df6 100644 --- a/teleopit/sim2real/mp/runtime.py +++ b/teleopit/sim2real/mp/runtime.py @@ -1259,6 +1259,7 @@ def __init__( ) self._policy_entry_pending = False self._policy_entry_deadline_s: float | None = None + self._policy_entry_target_qpos: Float64Array | None = None self._policy_session_id: str | None = None self._policy_frame_transform: PolicyFrameTransform | None = None self._policy_paused = False @@ -1513,6 +1514,13 @@ def _drain_high_level_policy_ipc(self) -> None: packet = self._policy_action_sub.recv_latest() if not isinstance(packet, HighLevelPolicyActionPacket): return + if packet.session_id != self._policy_session_id: + logger.debug( + "Discarded high-level policy action for inactive session: active=%r received=%r", + self._policy_session_id, + packet.session_id, + ) + return # A request may already be in flight when the operator pauses. Drain # its result without replacing the reference frozen at the B press. if self._policy_paused and not self._policy_resume_pending: @@ -1522,6 +1530,21 @@ def _drain_high_level_policy_ipc(self) -> None: if scheduler is None or policy_cfg is None: return now_s = time.monotonic() + if self.mode == RobotMode.STANDING and self._policy_entry_pending: + deadline_s = self._policy_entry_deadline_s + if deadline_s is not None and now_s > deadline_s: + operator_logger.warning( + "High-level policy entry timed out; remaining in STANDING" + ) + self._enter_standing() + return + if self.mode == RobotMode.POLICY and self._policy_resume_pending: + deadline_s = self._policy_resume_deadline_s + if deadline_s is not None and now_s > deadline_s: + self._handle_high_level_policy_fault( + "resume timed out waiting for a fresh action chunk" + ) + return result_age_s = now_s - float(packet.received_timestamp_s) if ( not np.isfinite(result_age_s) @@ -1533,6 +1556,11 @@ def _drain_high_level_policy_ipc(self) -> None: result_age_s, policy_cfg.max_result_age_s, ) + if self.mode == RobotMode.STANDING and self._policy_entry_pending: + operator_logger.warning( + "High-level policy entry failed; received a stale action result" + ) + self._enter_standing() return minimum_source_timestamp_ns = self._policy_resume_source_timestamp_ns if ( @@ -1543,21 +1571,46 @@ def _drain_high_level_policy_ipc(self) -> None: ): logger.warning("Discarded pre-resume high-level policy action chunk") return + if self.mode == RobotMode.STANDING and not self._policy_entry_pending: + return + chunk = PolicyActionChunk( + session_id=packet.session_id, + source_sequence_id=int(packet.source_sequence_id), + source_onboard_monotonic_timestamp_ns=int( + packet.source_onboard_monotonic_timestamp_ns + ), + action_fps=int(packet.action_fps), + actions=np.asarray(packet.actions, dtype=np.float32), + policy_id=str(packet.policy_id), + server_inference_ms=float(packet.server_inference_ms), + ) + if self.mode == RobotMode.STANDING: + first_chunk = self._policy_entry_target_qpos is None + try: + if first_chunk: + first_action = scheduler.accept_entry(chunk, now_s=now_s) + else: + scheduler.reset( + packet.session_id, + initial_action=self._build_high_level_policy_boundary_action( + self.robot.get_state() + ), + ) + scheduler.accept(chunk, now_s=now_s) + except (RuntimeError, ValueError) as exc: + logger.warning("Rejected high-level policy entry chunk: %s", exc) + operator_logger.warning( + "High-level policy entry failed; remaining in STANDING" + ) + self._enter_standing() + return + if first_chunk: + self._begin_policy_entry_alignment(first_action) + else: + self._transition_to_high_level_policy() + return try: - scheduler.accept( - PolicyActionChunk( - session_id=packet.session_id, - source_sequence_id=int(packet.source_sequence_id), - source_onboard_monotonic_timestamp_ns=int( - packet.source_onboard_monotonic_timestamp_ns - ), - action_fps=int(packet.action_fps), - actions=np.asarray(packet.actions, dtype=np.float32), - policy_id=str(packet.policy_id), - server_inference_ms=float(packet.server_inference_ms), - ), - now_s=now_s, - ) + scheduler.accept(chunk, now_s=now_s) except ValueError as exc: logger.warning("Rejected high-level policy action chunk: %s", exc) return @@ -1578,21 +1631,20 @@ def _handle_high_level_policy_transitions(self) -> None: if self.mode == RobotMode.STANDING: if self.remote.X.on_pressed and self._policy_entry_pending: operator_logger.info("X -> cancel high-level policy entry") - self._stop_high_level_policy_session() + self._enter_standing() return if self.remote.Y.on_pressed and not self._policy_entry_pending: operator_logger.info("Y -> request high-level policy") self._begin_high_level_policy_entry() if self._policy_entry_pending: - self._publish_high_level_policy_session("start", repeat=True) - scheduler = self._high_level_policy_scheduler - if scheduler is not None and scheduler.has_chunk: - self._transition_to_high_level_policy() - return + self._publish_high_level_policy_session( + "pause" if self._policy_paused else "start", + repeat=True, + ) deadline_s = self._policy_entry_deadline_s if deadline_s is not None and time.monotonic() > deadline_s: operator_logger.warning("High-level policy entry timed out; remaining in STANDING") - self._stop_high_level_policy_session() + self._enter_standing() return if self.mode == RobotMode.POLICY: if self.remote.X.on_pressed: @@ -1614,10 +1666,26 @@ def _handle_high_level_policy_transitions(self) -> None: self._enter_standing() def _begin_high_level_policy_entry(self) -> None: + self._policy_entry_target_qpos = None + self._start_high_level_policy_entry_session() + + def _build_high_level_policy_boundary_action(self, state: object) -> np.ndarray: + transform = self._policy_frame_transform + if transform is None: + raise RuntimeError("High-level policy entry is missing its frame transform") + initial_action = np.zeros(50, dtype=np.float32) + initial_action[:36] = transform.localize_body_action( + self._build_robot_state_qpos(state) + ) + return initial_action + + def _start_high_level_policy_entry_session(self) -> None: policy_cfg = self._high_level_policy_cfg scheduler = self._high_level_policy_scheduler if policy_cfg is None or scheduler is None: raise RuntimeError("High-level policy runtime is not configured") + if self._policy_session_id is not None: + self._publish_high_level_policy_session("stop") state = self.robot.get_state() root_pos = self._resolve_base_pos(state) self._policy_frame_transform = PolicyFrameTransform.from_robot_pose( @@ -1625,13 +1693,13 @@ def _begin_high_level_policy_entry(self) -> None: getattr(state, "quat"), ) self._policy_session_id = uuid.uuid4().hex - initial_action = np.zeros(50, dtype=np.float32) - initial_action[:36] = self._policy_frame_transform.localize_body_action( - self._build_robot_state_qpos(state) + scheduler.reset( + self._policy_session_id, + initial_action=self._build_high_level_policy_boundary_action(state), ) - scheduler.reset(self._policy_session_id, initial_action=initial_action) self._policy_entry_pending = True - self._policy_entry_deadline_s = time.monotonic() + policy_cfg.entry_timeout_s + if self._policy_entry_target_qpos is None: + self._policy_entry_deadline_s = time.monotonic() + policy_cfg.entry_timeout_s self._policy_paused = False self._policy_resume_pending = False self._policy_resume_deadline_s = None @@ -1646,6 +1714,25 @@ def _begin_high_level_policy_entry(self) -> None: self._last_policy_session_publish_s = 0.0 self._publish_high_level_policy_session("start", repeat=False) + def _begin_policy_entry_alignment(self, first_action: np.ndarray) -> None: + transform = self._policy_frame_transform + if transform is None: + raise RuntimeError("High-level policy entry is missing its frame transform") + target_qpos = np.asarray( + transform.delocalize_body_action(first_action[:36]), + dtype=np.float64, + ) + self._policy_entry_target_qpos = target_qpos + self._policy_paused = True + self._reset_policy_state() + self._last_retarget_qpos = None + self._safety.start_kp_ramp( + duration_s=self._standing_return_ramp_duration, + floor_ratio=self._standing_return_kp_ramp_floor_ratio, + ) + self._publish_high_level_policy_session("pause") + operator_logger.info("Aligning to high-level policy first reference in STANDING") + def _publish_high_level_policy_session(self, command: str, *, repeat: bool = False) -> None: publisher = self._policy_control_pub session_id = self._policy_session_id @@ -1708,12 +1795,9 @@ def _transition_to_high_level_policy(self) -> None: self._last_retarget_qpos = None self._last_commanded_motion_qpos = resume_qpos.copy() self._policy_hold_qpos = resume_qpos.copy() - self._safety.start_kp_ramp( - duration_s=self._standing_return_ramp_duration, - floor_ratio=self._standing_return_kp_ramp_floor_ratio, - ) self._policy_entry_pending = False self._policy_entry_deadline_s = None + self._policy_entry_target_qpos = None self._policy_resume_pending = False self._policy_resume_deadline_s = None self._policy_resume_source_timestamp_ns = None @@ -1754,6 +1838,7 @@ def _stop_high_level_policy_session(self) -> None: scheduler.clear() self._policy_entry_pending = False self._policy_entry_deadline_s = None + self._policy_entry_target_qpos = None self._policy_session_id = None self._policy_frame_transform = None self._policy_paused = False @@ -1788,9 +1873,22 @@ def _handle_high_level_policy_fault(self, detail: str) -> None: "High-level policy entry failed; remaining in STANDING: %s", detail, ) - self._stop_high_level_policy_session() + self._enter_standing() def _standing_step(self) -> None: + target_qpos = self._policy_entry_target_qpos + if self._policy_entry_pending and target_qpos is not None: + robot_state = self._run_static_mocap_step(target_qpos) + if self._policy_paused: + if not self._safety.kp_ramp_active: + operator_logger.info( + "High-level policy first-reference Kp ramp complete; " + "requesting fresh session" + ) + self._start_high_level_policy_entry_session() + else: + self._publish_high_level_policy_observation(robot_state) + return robot_state = self.robot.get_state() qpos = self._standing_qpos.copy() motion_joint_vel = np.zeros(self.num_actions, dtype=np.float32) @@ -2003,6 +2101,12 @@ def _compose_arm_reference_window(self, reference_window: ReferenceWindow | None def _enter_standing(self) -> None: prev_mode = self.mode + policy_entry_alignment_active = bool( + getattr(self, "high_level_policy_enabled", False) + and prev_mode == RobotMode.STANDING + and self._policy_entry_pending + and self._policy_entry_target_qpos is not None + ) if bool(getattr(self, "high_level_policy_enabled", False)) and ( prev_mode == RobotMode.POLICY or self._policy_entry_pending ): @@ -2010,6 +2114,8 @@ def _enter_standing(self) -> None: self._disarm_mocap_reference_if_needed() self._clear_reference_gate() self._mocap_entry_requested = False + if prev_mode == RobotMode.STANDING and not policy_entry_alignment_active: + return already_in_debug = self.mode in ( RobotMode.STANDING, RobotMode.MOCAP, @@ -2025,7 +2131,12 @@ def _enter_standing(self) -> None: time.sleep(0.5) state = self.robot.get_state() - if prev_mode not in (RobotMode.MOCAP, RobotMode.ARMS, RobotMode.POLICY): + if prev_mode not in ( + RobotMode.STANDING, + RobotMode.MOCAP, + RobotMode.ARMS, + RobotMode.POLICY, + ): logger.info("Locking joints to current position...") self.robot.lock_all_joints() time.sleep(0.3) @@ -2037,7 +2148,11 @@ def _enter_standing(self) -> None: self._last_commanded_motion_qpos = None self._set_default_standing_reference(state) self._reset_policy_state() - if prev_mode in (RobotMode.MOCAP, RobotMode.ARMS, RobotMode.POLICY): + if policy_entry_alignment_active or prev_mode in ( + RobotMode.MOCAP, + RobotMode.ARMS, + RobotMode.POLICY, + ): self._safety.start_kp_ramp( duration_s=self._standing_return_ramp_duration, floor_ratio=self._standing_return_kp_ramp_floor_ratio, @@ -2284,7 +2399,7 @@ def _paused_mocap_step(self) -> None: raise RuntimeError("Paused mocap session is missing a hold_qpos") self._run_static_mocap_step(hold_qpos) - def _run_static_mocap_step(self, hold_qpos: Float64Array) -> None: + def _run_static_mocap_step(self, hold_qpos: Float64Array) -> object: robot_state = self.robot.get_state() qpos = np.asarray(hold_qpos, dtype=np.float64).copy() motion_joint_vel = np.zeros(self.num_actions, dtype=np.float32) @@ -2311,6 +2426,7 @@ def _run_static_mocap_step(self, hold_qpos: Float64Array) -> None: self._last_commanded_motion_qpos = qpos.copy() self._publish_record_step(robot_state=robot_state, reference_qpos=qpos) self._write_retarget_viewer(qpos) + return robot_state def _hold_mocap_reference(self, reason: str, *, detail: str | None = None) -> None: if self._last_mocap_hold_reason != reason: diff --git a/teleopit/sim2real/safety.py b/teleopit/sim2real/safety.py index 6ce08ebe..57a5e5db 100644 --- a/teleopit/sim2real/safety.py +++ b/teleopit/sim2real/safety.py @@ -56,6 +56,10 @@ def __init__( self._joint_pos_lower = None self._joint_pos_upper = None + @property + def kp_ramp_active(self) -> bool: + return self._kp_ramp_active + def compute_kp_ramp_gains(self) -> tuple[Float32Array, Float32Array] | None: """Return (kp, kd) for current Kp-ramp step, or None if ramp inactive.""" if not self._kp_ramp_active: diff --git a/tests/test_high_level_policy.py b/tests/test_high_level_policy.py index 591a6065..7b278baf 100644 --- a/tests/test_high_level_policy.py +++ b/tests/test_high_level_policy.py @@ -41,7 +41,11 @@ HighLevelPolicyTargetPacket, ModeStatePacket, ) -from teleopit.sim2real.mp.runtime import RobotMode, Sim2RealRuntime, _RobotControlWorker +from teleopit.sim2real.mp.runtime import ( + RobotMode, + Sim2RealRuntime, + _RobotControlWorker, +) from teleopit.runtime.mocap_session import MocapSessionState @@ -206,6 +210,49 @@ def test_scheduler_validates_complete_chunk_against_onboard_safety_limits() -> N assert scheduler.has_chunk +def test_scheduler_entry_ignores_only_current_pose_boundary() -> None: + scheduler = HighLevelPolicyScheduler(hold_s=0.1, safety=_safety_config()) + initial = _safe_actions(1)[0] + initial[7] = 0.8 + actions = _safe_actions() + actions[:, 7] = -0.08 + scheduler.reset("session-1", initial_action=initial) + + with pytest.raises(ValueError, match="joint rate"): + scheduler.accept(_safe_chunk(actions), now_s=1.01) + + scheduler.reset("session-1", initial_action=initial) + first_action = scheduler.accept_entry(_safe_chunk(actions), now_s=1.01) + + assert first_action[7] == pytest.approx(-0.08) + + +def test_scheduler_entry_still_rejects_internal_chunk_discontinuity() -> None: + scheduler = HighLevelPolicyScheduler(hold_s=0.1, safety=_safety_config()) + actions = _safe_actions() + actions[0, 7] = -0.08 + actions[1:, 7] = 0.5 + scheduler.reset("session-1", initial_action=_safe_actions(1)[0]) + + with pytest.raises(ValueError, match="joint rate"): + scheduler.accept_entry(_safe_chunk(actions), now_s=1.01) + + assert not scheduler.has_chunk + + +def test_scheduler_entry_still_rejects_root_boundary_displacement() -> None: + scheduler = HighLevelPolicyScheduler(hold_s=0.1, safety=_safety_config()) + initial = _safe_actions(1)[0] + actions = _safe_actions() + actions[:, 0] += 0.2 + scheduler.reset("session-1", initial_action=initial) + + with pytest.raises(ValueError, match="root per-frame displacement"): + scheduler.accept_entry(_safe_chunk(actions), now_s=1.01) + + assert not scheduler.has_chunk + + @pytest.mark.parametrize( ("mutate", "message"), [ @@ -514,6 +561,7 @@ def test_high_level_policy_y_requests_takeover_without_starting_mode_state() -> worker.mode = RobotMode.STANDING worker.remote = _remote(y=True) worker._policy_entry_pending = False + worker._policy_paused = False requests: list[str] = [] def begin() -> None: @@ -522,7 +570,6 @@ def begin() -> None: worker._begin_high_level_policy_entry = begin worker._publish_high_level_policy_session = lambda *_args, **_kwargs: None - worker._high_level_policy_scheduler = SimpleNamespace(has_chunk=False) worker._policy_entry_deadline_s = None worker._handle_high_level_policy_transitions() @@ -531,6 +578,280 @@ def begin() -> None: assert worker.mode == RobotMode.STANDING +def test_policy_entry_first_action_starts_static_tracker_alignment() -> None: + worker = object.__new__(_RobotControlWorker) + worker._policy_entry_pending = True + worker._policy_frame_transform = PolicyFrameTransform.from_robot_pose( + [0.0, 0.0], + [1.0, 0.0, 0.0, 0.0], + ) + resets: list[str] = [] + worker._reset_policy_state = lambda: resets.append("reset") + worker._last_retarget_qpos = np.zeros(36, dtype=np.float64) + ramps: list[tuple[float, float]] = [] + worker._safety = SimpleNamespace( + start_kp_ramp=lambda *, duration_s, floor_ratio: ramps.append( + (float(duration_s), float(floor_ratio)) + ) + ) + worker._standing_return_ramp_duration = 0.5 + worker._standing_return_kp_ramp_floor_ratio = 0.5 + commands: list[str] = [] + worker._publish_high_level_policy_session = commands.append + action = _safe_actions(1)[0] + action[7:36] = 0.5 + + worker._begin_policy_entry_alignment(action) + + assert worker._policy_paused + assert worker._policy_entry_target_qpos is not None + np.testing.assert_allclose(worker._policy_entry_target_qpos[7:36], 0.5) + assert resets == ["reset"] + assert ramps == [(0.5, 0.5)] + assert commands == ["pause"] + + +def test_policy_entry_requests_fresh_session_after_kp_ramp() -> None: + worker = object.__new__(_RobotControlWorker) + worker._policy_entry_pending = True + worker._policy_paused = True + worker._policy_entry_target_qpos = np.zeros(36, dtype=np.float64) + worker._policy_entry_target_qpos[3] = 1.0 + worker._safety = SimpleNamespace(kp_ramp_active=False) + worker._run_static_mocap_step = lambda _target: SimpleNamespace() + starts: list[str] = [] + worker._start_high_level_policy_entry_session = lambda: starts.append("fresh") + + worker._standing_step() + + assert starts == ["fresh"] + + +def test_policy_transition_after_entry_alignment_does_not_restart_kp_ramp() -> None: + worker = object.__new__(_RobotControlWorker) + worker.mode = RobotMode.STANDING + worker.robot = SimpleNamespace(get_state=lambda: SimpleNamespace()) + resume_qpos = np.zeros(36, dtype=np.float64) + resume_qpos[3] = 1.0 + worker._build_robot_state_qpos = lambda _state: resume_qpos.copy() + resets: list[str] = [] + worker._reset_policy_state = lambda: resets.append("reset") + worker._last_retarget_qpos = np.ones(36, dtype=np.float64) + worker._last_commanded_motion_qpos = None + worker._policy_hold_qpos = None + worker._policy_entry_pending = True + worker._policy_entry_deadline_s = 2.0 + worker._policy_entry_target_qpos = np.ones(36, dtype=np.float64) + worker._policy_resume_pending = False + worker._policy_resume_deadline_s = None + worker._policy_resume_source_timestamp_ns = None + worker._standing_return_ramp_duration = 0.5 + worker._standing_return_kp_ramp_floor_ratio = 0.5 + worker._safety = SimpleNamespace( + start_kp_ramp=lambda **_kwargs: pytest.fail( + "POLICY transition must not start a second Kp ramp" + ) + ) + + worker._transition_to_high_level_policy() + + assert worker.mode == RobotMode.POLICY + assert resets == ["reset"] + assert not worker._policy_entry_pending + assert worker._policy_entry_target_qpos is None + np.testing.assert_array_equal(worker._policy_hold_qpos, resume_qpos) + + +def test_policy_entry_rejects_action_received_after_total_deadline() -> None: + worker = object.__new__(_RobotControlWorker) + now_s = time.monotonic() + worker.mode = RobotMode.STANDING + worker._policy_video_sub = SimpleNamespace(recv_latest=lambda: None) + worker._policy_status_sub = SimpleNamespace(recv_latest=lambda: None) + worker._policy_action_sub = SimpleNamespace( + recv_latest=lambda: HighLevelPolicyActionPacket( + session_id="session-1", + source_sequence_id=1, + source_onboard_monotonic_timestamp_ns=int(round(now_s * 1e9)), + action_fps=30, + actions=_safe_actions(1), + policy_id="test", + server_inference_ms=1.0, + received_timestamp_s=now_s, + ) + ) + worker._last_policy_video_seq = -1 + worker._last_policy_status_seq = -1 + worker._policy_session_id = "session-1" + worker._policy_paused = False + worker._policy_resume_pending = False + worker._policy_resume_source_timestamp_ns = None + worker._policy_entry_pending = True + worker._policy_entry_deadline_s = now_s - 0.01 + worker._policy_entry_target_qpos = np.zeros(36, dtype=np.float64) + accepted: list[object] = [] + worker._high_level_policy_scheduler = SimpleNamespace( + accept=lambda *args, **kwargs: accepted.append((args, kwargs)) + ) + worker._high_level_policy_cfg = SimpleNamespace(max_result_age_s=1.0) + standing: list[str] = [] + worker._enter_standing = lambda: standing.append("standing") + worker._transition_to_high_level_policy = lambda: pytest.fail( + "expired entry action must not enter POLICY" + ) + + worker._drain_high_level_policy_ipc() + + assert standing == ["standing"] + assert accepted == [] + + +def test_policy_entry_fresh_chunk_uses_current_measured_joint_boundary() -> None: + worker = object.__new__(_RobotControlWorker) + now_s = time.monotonic() + worker.mode = RobotMode.STANDING + worker._policy_video_sub = SimpleNamespace(recv_latest=lambda: None) + worker._policy_status_sub = SimpleNamespace(recv_latest=lambda: None) + worker._policy_action_sub = SimpleNamespace( + recv_latest=lambda: HighLevelPolicyActionPacket( + session_id="session-1", + source_sequence_id=1, + source_onboard_monotonic_timestamp_ns=int(round(now_s * 1e9)), + action_fps=30, + actions=_safe_actions(1), + policy_id="test", + server_inference_ms=1.0, + received_timestamp_s=now_s, + ) + ) + worker._last_policy_video_seq = -1 + worker._last_policy_status_seq = -1 + worker._policy_session_id = "session-1" + worker._policy_paused = False + worker._policy_resume_pending = False + worker._policy_resume_source_timestamp_ns = None + worker._policy_entry_pending = True + worker._policy_entry_deadline_s = now_s + 1.0 + worker._policy_entry_target_qpos = np.zeros(36, dtype=np.float64) + worker._policy_frame_transform = PolicyFrameTransform.from_robot_pose( + [0.0, 0.0], + [1.0, 0.0, 0.0, 0.0], + ) + current_qpos = np.zeros(36, dtype=np.float64) + current_qpos[2] = 0.76 + current_qpos[3] = 1.0 + current_qpos[7] = 0.8 + state = SimpleNamespace() + worker.robot = SimpleNamespace(get_state=lambda: state) + worker._build_robot_state_qpos = lambda _state: current_qpos.copy() + scheduler = HighLevelPolicyScheduler(hold_s=0.1, safety=_safety_config()) + scheduler.reset("session-1", initial_action=_safe_actions(1)[0]) + worker._high_level_policy_scheduler = scheduler + worker._high_level_policy_cfg = SimpleNamespace(max_result_age_s=1.0) + standing: list[str] = [] + worker._enter_standing = lambda: standing.append("standing") + worker._transition_to_high_level_policy = lambda: pytest.fail( + "fresh chunk must be rejected against the current measured pose" + ) + + worker._drain_high_level_policy_ipc() + + assert standing == ["standing"] + assert not scheduler.has_chunk + + +def test_policy_entry_stale_result_aborts_current_session() -> None: + worker = object.__new__(_RobotControlWorker) + now_s = time.monotonic() + worker.mode = RobotMode.STANDING + worker._policy_video_sub = SimpleNamespace(recv_latest=lambda: None) + worker._policy_status_sub = SimpleNamespace(recv_latest=lambda: None) + worker._policy_action_sub = SimpleNamespace( + recv_latest=lambda: HighLevelPolicyActionPacket( + session_id="session-1", + source_sequence_id=1, + source_onboard_monotonic_timestamp_ns=int(round(now_s * 1e9)), + action_fps=30, + actions=_safe_actions(1), + policy_id="test", + server_inference_ms=1.0, + received_timestamp_s=now_s - 1.0, + ) + ) + worker._last_policy_video_seq = -1 + worker._last_policy_status_seq = -1 + worker._policy_session_id = "session-1" + worker._policy_paused = False + worker._policy_resume_pending = False + worker._policy_resume_source_timestamp_ns = None + worker._policy_entry_pending = True + worker._policy_entry_deadline_s = now_s + 1.0 + worker._policy_entry_target_qpos = np.zeros(36, dtype=np.float64) + worker._high_level_policy_scheduler = SimpleNamespace() + worker._high_level_policy_cfg = SimpleNamespace(max_result_age_s=0.1) + standing: list[str] = [] + worker._enter_standing = lambda: standing.append("standing") + + worker._drain_high_level_policy_ipc() + + assert standing == ["standing"] + + +def test_policy_entry_alignment_abort_skips_standing_joint_lock() -> None: + worker = object.__new__(_RobotControlWorker) + worker.high_level_policy_enabled = True + worker.mode = RobotMode.STANDING + worker._policy_entry_pending = True + worker._policy_entry_target_qpos = np.zeros(36, dtype=np.float64) + worker._mocap_entry_requested = False + stops: list[str] = [] + + def stop_session() -> None: + stops.append("stop") + worker._policy_entry_pending = False + worker._policy_entry_target_qpos = None + + worker._stop_high_level_policy_session = stop_session + worker._disarm_mocap_reference_if_needed = lambda: None + worker._clear_reference_gate = lambda: None + state = SimpleNamespace() + worker.robot = SimpleNamespace( + get_state=lambda: state, + lock_all_joints=lambda: pytest.fail( + "STANDING entry abort must not lock joints or block the control loop" + ), + ) + current_qpos = np.zeros(36, dtype=np.float64) + current_qpos[3] = 1.0 + worker._build_robot_state_qpos = lambda _state: current_qpos.copy() + worker._ref_proc = SimpleNamespace(last_reference_qpos=current_qpos.copy()) + mocap_resets: list[str] = [] + worker._mocap_session = SimpleNamespace( + reset=lambda: mocap_resets.append("reset") + ) + standing_references: list[object] = [] + worker._set_default_standing_reference = standing_references.append + policy_resets: list[str] = [] + worker._reset_policy_state = lambda: policy_resets.append("reset") + ramps: list[tuple[float, float]] = [] + worker._safety = SimpleNamespace( + start_kp_ramp=lambda *, duration_s, floor_ratio: ramps.append( + (float(duration_s), float(floor_ratio)) + ) + ) + worker._standing_return_ramp_duration = 0.5 + worker._standing_return_kp_ramp_floor_ratio = 0.5 + + worker._enter_standing() + + assert stops == ["stop"] + assert worker.mode == RobotMode.STANDING + assert mocap_resets == ["reset"] + assert standing_references == [state] + assert policy_resets == ["reset"] + assert ramps == [(0.5, 0.5)] + + def test_high_level_policy_body_action_uses_existing_tracker_without_second_alignment() -> None: worker = object.__new__(_RobotControlWorker) action = _safe_actions(1)[0] @@ -739,6 +1060,8 @@ def test_current_policy_fault_status_is_handled_even_while_paused() -> None: def test_policy_pause_resume_waits_for_fresh_chunk_then_resumes() -> None: worker = object.__new__(_RobotControlWorker) + worker.mode = RobotMode.POLICY + worker._policy_session_id = "session-1" worker._policy_paused = True worker._policy_resume_pending = False worker._policy_resume_deadline_s = None @@ -789,6 +1112,46 @@ def test_policy_pause_resume_waits_for_fresh_chunk_then_resumes() -> None: assert not worker._policy_resume_pending +def test_policy_resume_rejects_action_received_after_deadline() -> None: + worker = object.__new__(_RobotControlWorker) + now_s = time.monotonic() + worker.mode = RobotMode.POLICY + worker._policy_video_sub = SimpleNamespace(recv_latest=lambda: None) + worker._policy_status_sub = SimpleNamespace(recv_latest=lambda: None) + worker._policy_action_sub = SimpleNamespace( + recv_latest=lambda: HighLevelPolicyActionPacket( + session_id="session-1", + source_sequence_id=1, + source_onboard_monotonic_timestamp_ns=int(round(now_s * 1e9)), + action_fps=30, + actions=_safe_actions(1), + policy_id="test", + server_inference_ms=1.0, + received_timestamp_s=now_s, + ) + ) + worker._last_policy_video_seq = -1 + worker._last_policy_status_seq = -1 + worker._policy_session_id = "session-1" + worker._policy_paused = True + worker._policy_resume_pending = True + worker._policy_resume_deadline_s = now_s - 0.01 + worker._policy_resume_source_timestamp_ns = int(round((now_s - 0.1) * 1e9)) + accepted: list[object] = [] + worker._high_level_policy_scheduler = SimpleNamespace( + accept=lambda *args, **kwargs: accepted.append((args, kwargs)), + resume=lambda _now_s: pytest.fail("expired resume must not resume scheduler"), + ) + worker._high_level_policy_cfg = SimpleNamespace(max_result_age_s=1.0) + faults: list[str] = [] + worker._handle_high_level_policy_fault = faults.append + + worker._drain_high_level_policy_ipc() + + assert faults == ["resume timed out waiting for a fresh action chunk"] + assert accepted == [] + + def test_paused_robot_worker_discards_inflight_policy_result() -> None: worker = object.__new__(_RobotControlWorker) worker._policy_video_sub = SimpleNamespace(recv_latest=lambda: None) @@ -808,6 +1171,7 @@ def test_paused_robot_worker_discards_inflight_policy_result() -> None: ) worker._last_policy_video_seq = -1 worker._last_policy_status_seq = -1 + worker._policy_session_id = "session-1" worker._policy_paused = True worker._policy_resume_pending = False accepted: list[object] = [] From 7f7dd274d53fbd2152b5f323d2fa444496b75465 Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Mon, 20 Jul 2026 15:31:05 +0800 Subject: [PATCH 26/59] Increase high-level policy entry timeout --- docs/docs/configuration/config-reference.md | 2 +- .../current/configuration/config-reference.md | 2 +- teleopit/configs/high_level_policy_sim2real.yaml | 2 +- teleopit/high_level_policy/config.py | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/docs/docs/configuration/config-reference.md b/docs/docs/configuration/config-reference.md index 303c55e6..abf8e8ba 100644 --- a/docs/docs/configuration/config-reference.md +++ b/docs/docs/configuration/config-reference.md @@ -128,7 +128,7 @@ protocol tests. The only shared data file is `hand_calibration.json`. | `high_level_policy.jpeg_quality` | JPEG quality for the 640x480 RGB frame | `90` | | `high_level_policy.max_observation_age_s` | Maximum camera/observation age before a request is skipped | `0.15` | | `high_level_policy.max_result_age_s` | Maximum local IPC age before a received result is rejected | `0.1` | -| `high_level_policy.entry_timeout_s` | Maximum total entry duration (candidate, Kp ramp, fresh session) and maximum fresh-chunk wait on resume | `3.0` | +| `high_level_policy.entry_timeout_s` | Maximum total entry duration (candidate, Kp ramp, fresh session) and maximum fresh-chunk wait on resume | `5.0` | | `high_level_policy.hold_s` | Final validated-reference grace period before the watchdog pauses `POLICY` | `0.1` | | `high_level_policy.safety.root_height_min_m` / `root_height_max_m` | Accepted absolute root-height range | `0.55` / `1.05` | | `high_level_policy.safety.max_root_xy_speed_m_s` | Maximum root XY speed across 30 Hz references | `2.5` | diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md index 1c602412..7b63ea91 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md @@ -146,7 +146,7 @@ client/server 消息结构与协议测试。唯一共享的数据文件是 `hand | `high_level_policy.jpeg_quality` | 640x480 RGB 帧的 JPEG 质量 | `90` | | `high_level_policy.max_observation_age_s` | 跳过请求前允许的最大 camera/observation age | `0.15` | | `high_level_policy.max_result_age_s` | 拒绝已接收结果前允许的最大本地 IPC age | `0.1` | -| `high_level_policy.entry_timeout_s` | 候选请求、Kp ramp 和新 session 的 entry 最长总时间，以及恢复时等待新鲜 chunk 的最长时间 | `3.0` | +| `high_level_policy.entry_timeout_s` | 候选请求、Kp ramp 和新 session 的 entry 最长总时间，以及恢复时等待新鲜 chunk 的最长时间 | `5.0` | | `high_level_policy.hold_s` | Watchdog 暂停 `POLICY` 前，最后一条有效 reference 的 grace period | `0.1` | | `high_level_policy.safety.root_height_min_m` / `root_height_max_m` | 可接受的绝对 root 高度范围 | `0.55` / `1.05` | | `high_level_policy.safety.max_root_xy_speed_m_s` | 30 Hz reference 之间允许的最大 root XY 速度 | `2.5` | diff --git a/teleopit/configs/high_level_policy_sim2real.yaml b/teleopit/configs/high_level_policy_sim2real.yaml index a9708ef3..e79fff11 100644 --- a/teleopit/configs/high_level_policy_sim2real.yaml +++ b/teleopit/configs/high_level_policy_sim2real.yaml @@ -24,7 +24,7 @@ high_level_policy: jpeg_quality: 90 max_observation_age_s: 0.15 max_result_age_s: 0.1 - entry_timeout_s: 3.0 + entry_timeout_s: 5.0 hold_s: 0.1 safety: root_height_min_m: 0.55 diff --git a/teleopit/high_level_policy/config.py b/teleopit/high_level_policy/config.py index 0d7e8db4..5227385d 100644 --- a/teleopit/high_level_policy/config.py +++ b/teleopit/high_level_policy/config.py @@ -75,7 +75,7 @@ def parse_high_level_policy_config(cfg: Any) -> HighLevelPolicyConfig: cfg_get(policy_cfg, "max_result_age_s", 0.1), "max_result_age_s" ) entry_timeout_s = _positive_float( - cfg_get(policy_cfg, "entry_timeout_s", 3.0), "entry_timeout_s" + cfg_get(policy_cfg, "entry_timeout_s", 5.0), "entry_timeout_s" ) hold_s = float(cfg_get(policy_cfg, "hold_s", 0.1)) if not math.isfinite(hold_s) or hold_s < 0.0: From 3dd3bfab6c59df5721e23f8933ef41cb125c9be9 Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Mon, 20 Jul 2026 15:40:30 +0800 Subject: [PATCH 27/59] Increase high-level policy entry ramp duration --- docs/docs/configuration/config-reference.md | 1 + .../current/configuration/config-reference.md | 1 + teleopit/configs/high_level_policy_sim2real.yaml | 3 +++ 3 files changed, 5 insertions(+) diff --git a/docs/docs/configuration/config-reference.md b/docs/docs/configuration/config-reference.md index abf8e8ba..4d4da384 100644 --- a/docs/docs/configuration/config-reference.md +++ b/docs/docs/configuration/config-reference.md @@ -120,6 +120,7 @@ protocol tests. The only shared data file is `hand_calibration.json`. | `camera.source` | Onboard policy camera: `realsense` or integration-only `test-pattern` | `realsense` | | `camera.width` / `height` / `fps` | Exact policy image contract | `640` / `480` / `30` | | `camera.device` | Optional RealSense serial | `null` | +| `standing_return_ramp_duration` | Kp-ramp duration while tracking the policy-entry candidate first frame | `2.0` | | `high_level_policy.endpoint` | Host policy ZeroMQ TCP endpoint | `tcp://127.0.0.1:5555` | | `high_level_policy.task` | Non-empty task prompt sent on reset and every observation | `demo` | | `high_level_policy.timeout_s` | Per-request network deadline | `1.0` | diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md index 7b63ea91..4731d5ba 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md @@ -138,6 +138,7 @@ client/server 消息结构与协议测试。唯一共享的数据文件是 `hand | `camera.source` | Onboard 策略相机：`realsense`，或仅供集成测试的 `test-pattern` | `realsense` | | `camera.width` / `height` / `fps` | 精确的策略图像契约 | `640` / `480` / `30` | | `camera.device` | 可选 RealSense 序列号 | `null` | +| `standing_return_ramp_duration` | 跟踪策略 entry 候选第一帧时的 Kp ramp 时长 | `2.0` | | `high_level_policy.endpoint` | 主机策略 ZeroMQ TCP endpoint | `tcp://127.0.0.1:5555` | | `high_level_policy.task` | reset 和每个 observation 都会发送的非空任务 prompt | `demo` | | `high_level_policy.timeout_s` | 单次网络请求 deadline | `1.0` | diff --git a/teleopit/configs/high_level_policy_sim2real.yaml b/teleopit/configs/high_level_policy_sim2real.yaml index e79fff11..911e8284 100644 --- a/teleopit/configs/high_level_policy_sim2real.yaml +++ b/teleopit/configs/high_level_policy_sim2real.yaml @@ -7,6 +7,9 @@ defaults: input: provider: high_level_policy +# Hold the entry candidate through the tracker before requesting the fresh session. +standing_return_ramp_duration: 2.0 + camera: source: realsense # realsense | test-pattern width: 640 From 97f9fa424df098be2d6da53f5bfc9a43f1a42cdd Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Mon, 20 Jul 2026 16:42:53 +0800 Subject: [PATCH 28/59] Allow discontinuous high-level policy references --- AGENTS.md | 4 +- README.md | 9 +- docs/docs/configuration/config-reference.md | 8 +- docs/docs/reference/architecture.md | 3 +- .../tutorials/high-level-policy-sim2real.md | 52 +++++----- .../current/configuration/config-reference.md | 8 +- .../current/reference/architecture.md | 3 +- .../tutorials/high-level-policy-sim2real.md | 38 +++---- teleopit/high_level_policy/scheduler.py | 99 ++----------------- teleopit/sim2real/mp/runtime.py | 15 +-- tests/test_high_level_policy.py | 86 ++++++---------- 11 files changed, 107 insertions(+), 218 deletions(-) diff --git a/AGENTS.md b/AGENTS.md index 1fa0c20a..cf2e0fc0 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -184,10 +184,10 @@ target_dof_pos = clip(action, -10, 10) × action_scale + default_dof_pos - Policy observation is RGB JPEG plus `observation.state(68)`; only `state[58:62]` is rotated into the session-local yaw frame - Canonical action is `float32[T,50]`: local root `xyz(3)` + local root quaternion `wxyz(4)` + G1 joint reference `29` + left/right O6 closure `12` + OpenNeck yaw/pitch degrees `2` - The body reference `[0:36]` is yaw/XY-delocalized once and sent through the existing motion tracker. It is never sent directly as a G1 motor command and must not pass through the mocap alignment a second time -- High-level-policy formal robot modes are `IDLE`, `STANDING`, `POLICY`, and `DAMPING`. After Unitree remote `Y`, entry remains internal to `STANDING`: validate one candidate chunk without its measured-pose-to-first-frame G1 joint-rate boundary, hold `action[0]` through the existing tracker for one Kp ramp, then create one new host session and require a fresh normally validated chunk before entering `POLICY`; a failure aborts entry, and there is no `POLICY_STARTING` mode +- High-level-policy formal robot modes are `IDLE`, `STANDING`, `POLICY`, and `DAMPING`. After Unitree remote `Y`, entry remains internal to `STANDING`: validate one candidate chunk, hold `action[0]` through the existing tracker for one Kp ramp, then create one new host session and require a fresh valid chunk before entering `POLICY`; the 50 Hz output limiter starts from the held `action[0]` reference rather than measured tracker joints, a failure aborts entry, and there is no `POLICY_STARTING` mode - Unitree remote controls are `Start -> STANDING`, `Y -> request POLICY`, `B -> pause/resume`, `X -> STANDING/cancel pending`, and `L1+R1 -> DAMPING` - Policy pause freezes the scheduler/body reference and holds the latest hand/neck command. Leaving `POLICY` opens LinkerHand and centers OpenNeck -- The onboard scheduler rejects whole chunks on shape/finiteness, session/sequence, quaternion, root displacement/height/speed/yaw-rate, G1 joint position/rate, hand closure, OpenNeck degree range, or staleness failures; the entry candidate exempts only its measured-pose-to-first-frame G1 joint-rate boundary, while root boundary checks and every transition inside the candidate remain mandatory; never pad, trim, or safety-clip invalid host output +- The onboard scheduler rejects whole chunks on shape/finiteness, session/sequence, quaternion, absolute root height, G1 joint position, hand closure, OpenNeck degree range, or staleness failures. It accepts temporal root, yaw, and joint-reference discontinuities at entry, inside chunks, and across chunks because recorded pause/resume transitions may be discontinuous; accepted root translation, yaw, and joint output is rate-limited at 50 Hz. Never pad, trim, or clip malformed/out-of-range host output into validity - A short cached-reference grace period is allowed; exhaustion, host/network failure, or loss of a required camera/client worker puts `POLICY` into the same resumable pause state used by remote `B`. The runtime never enters `STANDING` automatically; `B` resumes after a fresh valid chunk is available, while `X` remains the manual transition to `STANDING` - Initial production hardware support requires two LinkerHand O6 hands and OpenNeck because all 50 canonical action dimensions are active - OpenNeck 0.2.0 is the supported runtime; Teleopit sends physical degrees through `move_deg()`, and the direct-drive OpenNeck package converts degrees to servo steps and clips them to its calibrated mechanical limits; the removed normalized API and config fields are unsupported diff --git a/README.md b/README.md index c61c300e..ca76499d 100644 --- a/README.md +++ b/README.md @@ -153,9 +153,12 @@ takeover, `B` pauses/resumes, `X` returns to `STANDING`, and `L1+R1` enters `DAMPING`. Policy entry remains an internal `STANDING` phase with no separate starting mode: Teleopit validates a candidate chunk, holds its first body reference through one motion-tracker Kp ramp, then creates one fresh host -session. A normally validated chunk from that session is required before -entering `POLICY`, so Replay restarts from its configured start frame and ACT -recomputes from the post-ramp observation. Entry failure returns to `STANDING`. +session. A valid chunk from that session is required before entering `POLICY`; +Replay therefore restarts from its configured start frame and ACT recomputes +from the post-ramp observation. The 50 Hz output limiter starts from the held +reference, not the tracker's measured joint pose. Temporal reference jumps are +accepted so recorded pause/resume transitions can be replayed, then rate-limited +on output. Entry failure returns to `STANDING`. Invalid/stale live chunks and watchdog expiry cannot block the local control loop and instead pause `POLICY` while holding the last reference. Host/network failure and loss of a required camera/client worker use the same ordinary pause diff --git a/docs/docs/configuration/config-reference.md b/docs/docs/configuration/config-reference.md index 4d4da384..7e6439a1 100644 --- a/docs/docs/configuration/config-reference.md +++ b/docs/docs/configuration/config-reference.md @@ -132,10 +132,10 @@ protocol tests. The only shared data file is `hand_calibration.json`. | `high_level_policy.entry_timeout_s` | Maximum total entry duration (candidate, Kp ramp, fresh session) and maximum fresh-chunk wait on resume | `5.0` | | `high_level_policy.hold_s` | Final validated-reference grace period before the watchdog pauses `POLICY` | `0.1` | | `high_level_policy.safety.root_height_min_m` / `root_height_max_m` | Accepted absolute root-height range | `0.55` / `1.05` | -| `high_level_policy.safety.max_root_xy_speed_m_s` | Maximum root XY speed across 30 Hz references | `2.5` | -| `high_level_policy.safety.max_root_displacement_m` | Maximum 3D root displacement between reference frames | `0.1` | -| `high_level_policy.safety.max_yaw_rate_rad_s` | Maximum root yaw rate | `2.5` | -| `high_level_policy.safety.max_joint_rate_rad_s` | Maximum per-joint reference rate | `10.0` | +| `high_level_policy.safety.max_root_xy_speed_m_s` | Root XY speed limit applied to the 50 Hz scheduler output | `2.5` | +| `high_level_policy.safety.max_root_displacement_m` | Source-frame-equivalent 3D root step used by the 50 Hz output limiter | `0.1` | +| `high_level_policy.safety.max_yaw_rate_rad_s` | Root yaw-rate limit applied to the 50 Hz scheduler output | `2.5` | +| `high_level_policy.safety.max_joint_rate_rad_s` | Per-joint rate limit applied to the 50 Hz scheduler output | `10.0` | | `high_level_policy.safety.neck_yaw_min_deg` / `neck_yaw_max_deg` | Accepted OpenNeck yaw command range | `-45` / `45` | | `high_level_policy.safety.neck_pitch_min_deg` / `neck_pitch_max_deg` | Accepted OpenNeck pitch command range | `-40` / `40` | diff --git a/docs/docs/reference/architecture.md b/docs/docs/reference/architecture.md index 72677de8..2423e1b6 100644 --- a/docs/docs/reference/architecture.md +++ b/docs/docs/reference/architecture.md @@ -94,7 +94,8 @@ train_mimic/scripts/data - sim2real also requires a dual-input ONNX whose observation dimension matches the runtime builder - Host-policy message-envelope or schema mismatches are rejected while the robot remains in `STANDING` - Host action chunks are validated and interpolated onboard; the host cannot bypass the motion tracker or send motor commands -- Policy entry remains internal to `STANDING`: hold one validated candidate first frame for a Kp ramp, then require a fully validated chunk from one fresh host session; the only formal takeover mode is `POLICY` +- Policy entry remains internal to `STANDING`: hold one validated candidate first frame for a Kp ramp, then require one fresh-session chunk and start its rate-limited output from the held reference rather than measured tracker joints; the only formal takeover mode is `POLICY` +- Temporal root, yaw, and joint-reference discontinuities are accepted at chunk boundaries and inside chunks, then rate-limited at the 50 Hz scheduler output so recorded pause/resume transitions remain usable ## Public Surface diff --git a/docs/docs/tutorials/high-level-policy-sim2real.md b/docs/docs/tutorials/high-level-policy-sim2real.md index 0b547960..51ba11ed 100644 --- a/docs/docs/tutorials/high-level-policy-sim2real.md +++ b/docs/docs/tutorials/high-level-policy-sim2real.md @@ -137,19 +137,20 @@ Keep the Unitree remote in hand. The runtime has only the formal robot modes | Unitree remote `L1+R1` | Emergency transition to `DAMPING` | After `Y`, Teleopit creates an entry session, establishes the current root -XY/yaw anchor, and requests one candidate chunk. All absolute limits, root -boundary limits, and transitions inside that chunk are validated. Only the G1 -joint-rate boundary from the measured pose to `action[0]` is excluded. Teleopit -then freezes `action[0]` as a static body reference and uses the existing -motion tracker for one Kp ramp. +XY/yaw anchor, and requests one candidate chunk. Its structure, finite values, +quaternion, and absolute hardware ranges are validated. Temporal root, yaw, and +joint-reference jumps are accepted. Teleopit then freezes `action[0]` as a +static body reference and uses the existing motion tracker for one Kp ramp. The robot remains formally in `STANDING` throughout entry; there is no separate "policy starting" state. When the ramp finishes, Teleopit creates a second session, which resets ReplayPolicy to its configured start frame (frame 0 by default) or resets ACT state, and requests a fresh chunk from the post-ramp -observation. That fresh chunk must pass normal validation, including the -measured-pose-to-first-frame joint-rate boundary, before the runtime enters -`POLICY`. A failure or timeout safely returns to the normal standing reference. +observation. That fresh chunk must pass normal validation before the runtime +enters `POLICY`. The scheduler's 50 Hz output +limiter starts from the held `action[0]` reference rather than measured tracker +joints, which need not equal a motion reference. A failure or timeout safely +returns to the normal standing reference. Pause freezes the body reference and holds the last LinkerHand and OpenNeck commands. Resume requests a fresh action chunk while continuing to hold the @@ -169,23 +170,26 @@ pads, trims, or safety-clips a malformed host result. Checks include: - exact finite `float32[T,50]`, current session, and increasing source sequence; - normalized root quaternion with temporal sign continuity; -- root height, per-frame displacement, XY speed, and yaw-rate limits; -- G1 joint position and joint-rate limits; +- absolute root-height limits; +- absolute G1 joint-position limits; - LinkerHand closure `[0,1]` and configured OpenNeck degree ranges; - observation/result age, source timestamp, and action horizon. -The entry candidate has one narrow exception: its measured-pose-to-first-frame -G1 joint-rate boundary is handled by static tracker alignment instead of chunk -rejection. Root boundary checks and all transitions inside the candidate remain -mandatory. Host requests are paused for one Kp ramp. A new host session then -supplies the fresh chunk that will actually enter `POLICY`; the candidate chunk -is never continued as a live timeline. A rejected fresh chunk aborts entry -instead of starting another alignment cycle. +Reference continuity is not an acceptance condition. Root translation, root +yaw, and G1 joint-reference jumps are accepted at entry, inside a chunk, and +across chunks because a recorded pause/resume transition can intentionally be +discontinuous. Host requests are paused for one Kp ramp. A new host session +then supplies the fresh chunk that will actually enter `POLICY`; the candidate +chunk is never continued as a live timeline. A malformed, stale, or +out-of-range fresh chunk aborts entry instead of starting another alignment +cycle. Validated 30 Hz body references are interpolated and rate-limited locally at 50 Hz, including when latency skips source frames or a new chunk replaces the -old plan. A short configured grace period can reuse the final validated -reference during an inference delay. If no valid action remains, a network +old plan. The configured root displacement/XY speed, yaw-rate, and joint-rate +values are output limits, not chunk-rejection thresholds. A short configured +grace period can reuse the final validated reference during an inference delay. +If no valid action remains, a network exchange fails, or a required camera/client worker exits, Teleopit remains in `POLICY`, enters the normal resumable pause state, and holds the latest body, hand, and neck commands. After recovery, `B` requests resume; execution stays @@ -203,11 +207,11 @@ data, G1 joint limits, and the installed OpenNeck calibration. `replan_steps`, and the entry logs. Teleopit stays in `STANDING` while it aligns to the first reference and when any candidate or fresh-chunk check fails. -**The fresh entry chunk is rejected or entry times out:** verify that the -episode starts with a stable pose, inspect the joint ordering and absolute -reference convention, and check whether the inherited -`standing_return_ramp_duration` is sufficient. Do not disable the -chunk-internal rate checks. +**The fresh entry chunk is rejected or entry times out:** inspect the logged +contract error, joint ordering, absolute-reference convention, hardware ranges, +and host/network latency. Reference discontinuity alone does not reject a +chunk. `standing_return_ramp_duration` controls physical alignment to the +candidate first frame. **Policy runs briefly and becomes paused:** inspect timeout, inference latency, stale-result, worker-exit, and safety-rejection logs. The low-level diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md index 4731d5ba..f8727092 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md @@ -150,10 +150,10 @@ client/server 消息结构与协议测试。唯一共享的数据文件是 `hand | `high_level_policy.entry_timeout_s` | 候选请求、Kp ramp 和新 session 的 entry 最长总时间，以及恢复时等待新鲜 chunk 的最长时间 | `5.0` | | `high_level_policy.hold_s` | Watchdog 暂停 `POLICY` 前，最后一条有效 reference 的 grace period | `0.1` | | `high_level_policy.safety.root_height_min_m` / `root_height_max_m` | 可接受的绝对 root 高度范围 | `0.55` / `1.05` | -| `high_level_policy.safety.max_root_xy_speed_m_s` | 30 Hz reference 之间允许的最大 root XY 速度 | `2.5` | -| `high_level_policy.safety.max_root_displacement_m` | reference 帧之间允许的最大 3D root 位移 | `0.1` | -| `high_level_policy.safety.max_yaw_rate_rad_s` | 最大 root yaw rate | `2.5` | -| `high_level_policy.safety.max_joint_rate_rad_s` | 最大单关节 reference rate | `10.0` | +| `high_level_policy.safety.max_root_xy_speed_m_s` | 应用于 50 Hz scheduler 输出的 root XY 速度限制 | `2.5` | +| `high_level_policy.safety.max_root_displacement_m` | 50 Hz 输出 rate limiter 使用的 source-frame 等效 3D root 步长 | `0.1` | +| `high_level_policy.safety.max_yaw_rate_rad_s` | 应用于 50 Hz scheduler 输出的 root yaw rate 限制 | `2.5` | +| `high_level_policy.safety.max_joint_rate_rad_s` | 应用于 50 Hz scheduler 输出的单关节 rate 限制 | `10.0` | | `high_level_policy.safety.neck_yaw_min_deg` / `neck_yaw_max_deg` | 可接受的 OpenNeck yaw 命令范围 | `-45` / `45` | | `high_level_policy.safety.neck_pitch_min_deg` / `neck_pitch_max_deg` | 可接受的 OpenNeck pitch 命令范围 | `-40` / `40` | diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/architecture.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/architecture.md index 54761675..ea717586 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/architecture.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/architecture.md @@ -91,7 +91,8 @@ train_mimic/scripts/data - sim2real 也要求双输入 ONNX，且观测维度必须与运行时 builder 匹配 - 主机策略消息 envelope 或 schema 不匹配时会被拒绝，机器人保持在 `STANDING` - 主机 action chunk 在 onboard 完成验证与插值；主机不能绕过 motion tracker 或发送电机命令 -- 策略 entry 保持为 `STANDING` 内部流程：通过一次 Kp ramp 保持经过验证的候选第一帧，然后要求一个新 host session 提供完整通过验证的 chunk；正式接管模式只有 `POLICY` +- 策略 entry 保持为 `STANDING` 内部流程：通过一次 Kp ramp 保持经过验证的候选第一帧，然后要求新 host session 提供一个 chunk，并从所保持的 reference 而非 tracker 实测关节开始执行 rate-limited 输出；正式接管模式只有 `POLICY` +- chunk 边界和 chunk 内部的 root、yaw 与关节 reference 时间跳变都会被接受，再由 50 Hz scheduler 输出执行 rate limit，从而保留录制的 pause/resume 转换 ## 公共接口 diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md index 43c5e2fd..d7d9ddf9 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md @@ -129,17 +129,16 @@ python scripts/run/run_high_level_policy_sim2real.py \ | Unitree remote `L1+R1` | 紧急切换到 `DAMPING` | 按下 `Y` 后，Teleopit 会创建 entry session，以当前 root XY/yaw 建立锚点，并请求一个 -候选 chunk。该 chunk 的所有绝对限制、root 边界限制和 chunk 内部跳变都会进行验证； -仅不检查实测姿态到 `action[0]` 的 G1 关节 rate 边界。随后 Teleopit 会冻结 -`action[0]` 作为静态 body reference，并通过现有 motion tracker 在一次 Kp ramp 期间 -跟踪该 reference。 +候选 chunk。运行时会验证其结构、有限值、四元数和绝对硬件范围；root、yaw 和关节 +reference 的时间跳变会被接受。随后 Teleopit 会冻结 `action[0]` 作为静态 body +reference，并通过现有 motion tracker 在一次 Kp ramp 期间跟踪该 reference。 整个 entry 期间，机器人在形式上仍处于 `STANDING`；没有单独的“policy starting” 状态。Kp ramp 结束后，Teleopit 会创建第二个 session：它会把 ReplayPolicy 重置到所 配置的起始帧（默认为第 0 帧），或重置 ACT 状态，并根据 ramp 后的 observation 请求 -新 chunk。该新 chunk -必须通过包含“实测姿态到第一帧关节 rate 边界”在内的正常验证，运行时才会进入 -`POLICY`。失败或超时会安全地返回普通 standing reference。 +新 chunk。该新 chunk 必须通过正常验证，运行时才会进入 `POLICY`。scheduler 的 50 Hz +输出 limiter 从正在保持的 `action[0]` reference 开始，而不是从无需等于 motion +reference 的 tracker 实测关节开始。失败或超时会安全地返回普通 standing reference。 暂停会冻结 body reference，并保持最后一条 LinkerHand 和 OpenNeck 命令。恢复时会请求 新的 action chunk，并在等待期间继续保持暂停姿态。按 `X` 会停止策略 session；运行时 @@ -157,20 +156,21 @@ Watchdog、主机/网络、相机或 policy client 故障也会进入同一个 - 精确且有限的 `float32[T,50]`、当前 session，以及递增的 source sequence； - 归一化 root quaternion 与时间连续的符号； -- root 高度、逐帧位移、XY 速度和 yaw rate 限制； -- G1 关节位置和关节 rate 限制； +- 绝对 root 高度限制； +- 绝对 G1 关节位置限制； - LinkerHand closure `[0,1]` 和配置的 OpenNeck 角度范围； - observation/result 时效、source timestamp 和 action horizon。 -entry 候选 chunk 只有一个严格限定的例外：实测姿态到第一帧的 G1 关节 rate 边界由 -静态 tracker 对齐处理，而不是直接拒绝 chunk。root 边界检查和候选 chunk 内部的所有 -跳变仍然是强制检查项。一次 Kp ramp 期间会暂停主机请求，随后新 host session 会提供 -真正进入 `POLICY` 的新鲜 chunk；候选 chunk 绝不会作为实时 timeline 继续播放。若新鲜 -chunk 被拒绝，entry 会直接终止，不会开始另一轮对齐。 +reference 连续性不是接收条件。entry、chunk 内部和 chunk 之间的 root translation、root +yaw 与 G1 关节 reference 跳变都会被接受，因为录制的 pause/resume 转换可能有意地不 +连续。一次 Kp ramp 期间会暂停主机请求，随后新 host session 会提供真正进入 `POLICY` +的新鲜 chunk；候选 chunk 绝不会作为实时 timeline 继续播放。格式错误、过期或超出绝对 +范围的新鲜 chunk 会终止 entry，而不会开始另一轮对齐。 通过验证的 30 Hz body reference 会在本地插值到 50 Hz 并执行 rate limit；网络延迟 -导致跳过 source frame 或新 chunk 替换旧计划时同样如此。在短暂推理延迟期间，可以在 -配置的短 grace period 内继续使用最后一条已验证 reference。如果不再有有效 action， +导致跳过 source frame 或新 chunk 替换旧计划时同样如此。配置的 root displacement/XY +speed、yaw rate 和 joint rate 是输出限制，而不是 chunk 拒绝阈值。在短暂推理延迟期间， +可以在配置的短 grace period 内继续使用最后一条已验证 reference。如果不再有有效 action， 网络交换失败，或必要的 camera/client worker 退出，Teleopit 会保持在 `POLICY`，进入 普通的可恢复暂停状态，并保持最后一条 body、hand 和 neck 命令。故障恢复后按 `B` 请求恢复；在收到新的有效 chunk 前，执行仍保持暂停。只有 `X` 会把模式切换到 @@ -187,9 +187,9 @@ chunk 被拒绝，entry 会直接终止，不会开始另一轮对齐。 日志。Teleopit 在对齐第一帧 reference，以及候选 chunk 或新鲜 chunk 检查失败时，都会 保持 `STANDING`。 -**新鲜 entry chunk 被拒绝或 entry 超时：** 请确认 episode 从稳定姿态开始，检查关节 -顺序和绝对 reference 约定，并确认继承的 `standing_return_ramp_duration` 是否足够。 -不要关闭 chunk 内部 rate 检查。 +**新鲜 entry chunk 被拒绝或 entry 超时：** 请检查日志中的契约错误、关节顺序、绝对 +reference 约定、硬件范围以及 host/network 延迟。单纯的 reference 跳变不会导致 chunk +被拒绝。`standing_return_ramp_duration` 控制对候选第一帧的物理对齐。 **策略短暂运行后进入暂停：** 检查 timeout、推理延迟、stale result、worker 退出和 安全拒绝日志。底层 50 Hz tracker 不会等待主机推理。恢复故障输入路径后按 `B` 继续。 diff --git a/teleopit/high_level_policy/scheduler.py b/teleopit/high_level_policy/scheduler.py index 2180952f..8f2d8446 100644 --- a/teleopit/high_level_policy/scheduler.py +++ b/teleopit/high_level_policy/scheduler.py @@ -126,7 +126,6 @@ def __init__( self._last_source_timestamp_ns = -1 self._paused_at_s: float | None = None self._timestamp_shift_s = 0.0 - self._initial_action: np.ndarray | None = None self._last_output_action: np.ndarray | None = None @property @@ -150,13 +149,13 @@ def reset(self, session_id: str, *, initial_action: object | None = None) -> Non self._last_source_timestamp_ns = -1 self._paused_at_s = None self._timestamp_shift_s = 0.0 - self._initial_action = ( + initial_output = ( None if initial_action is None else self._validate_single_action(initial_action, name="initial_action") ) self._last_output_action = ( - None if self._initial_action is None else self._initial_action.copy() + None if initial_output is None else initial_output.copy() ) def clear(self) -> None: @@ -166,21 +165,14 @@ def clear(self) -> None: self._last_source_timestamp_ns = -1 self._paused_at_s = None self._timestamp_shift_s = 0.0 - self._initial_action = None self._last_output_action = None def accept(self, chunk: PolicyActionChunk, *, now_s: float) -> None: - self._accept(chunk, now_s=now_s, validate_joint_boundary=True) + self._accept(chunk, now_s=now_s) def accept_entry(self, chunk: PolicyActionChunk, *, now_s: float) -> np.ndarray: - """Accept an entry candidate without comparing its first joint target. - - Root boundary limits, absolute limits, and every transition inside the - chunk are still validated. The runtime tracks the first action for one - Kp ramp, then starts a fresh policy session whose first chunk uses the - normal boundary validation path. - """ - self._accept(chunk, now_s=now_s, validate_joint_boundary=False) + """Accept an entry candidate and return its validated first action.""" + self._accept(chunk, now_s=now_s) assert self._chunk is not None return self._chunk.actions[0].copy() @@ -189,7 +181,6 @@ def _accept( chunk: PolicyActionChunk, *, now_s: float, - validate_joint_boundary: bool, ) -> None: if not np.isfinite(now_s): raise ValueError("High-level policy scheduler now_s must be finite") @@ -233,19 +224,7 @@ def _accept( "High-level policy source timestamp is in the future: " f"source={source_s:.9f}s now={float(now_s):.9f}s" ) - boundary_action = self._sample_unlimited(source_s) - if boundary_action is None: - boundary_action = ( - self._initial_action - if self._chunk is None - else self._chunk.actions[-1].copy() - ) - actions = self._validate_actions( - chunk.actions, - action_fps=chunk.action_fps, - boundary_action=boundary_action, - validate_joint_boundary=validate_joint_boundary, - ) + actions = self._validate_actions(chunk.actions) valid_until_s = source_s + len(actions) / float(chunk.action_fps) + self.hold_s if float(now_s) > valid_until_s: raise ValueError( @@ -368,10 +347,6 @@ def _rate_limit_output( def _validate_actions( self, values: object, - *, - action_fps: int, - boundary_action: np.ndarray | None, - validate_joint_boundary: bool, ) -> np.ndarray: actions = np.asarray(values) if actions.ndim != 2 or actions.shape[1] != ACTION_DIM or not 1 <= len(actions) <= 15: @@ -393,13 +368,7 @@ def _validate_actions( raise ValueError("High-level policy LinkerHand closure must be within [0, 1]") safety = self.safety if safety is not None: - self._validate_safety_limits( - validated, - action_fps=action_fps, - boundary_action=boundary_action, - validate_joint_boundary=validate_joint_boundary, - safety=safety, - ) + self._validate_safety_limits(validated, safety=safety) return validated @staticmethod @@ -419,9 +388,6 @@ def _validate_single_action(values: object, *, name: str) -> np.ndarray: def _validate_safety_limits( actions: np.ndarray, *, - action_fps: int, - boundary_action: np.ndarray | None, - validate_joint_boundary: bool, safety: HighLevelPolicySafetyConfig, ) -> None: root_height = actions[:, 2] @@ -462,57 +428,6 @@ def _validate_safety_limits( f"[{safety.neck_pitch_min_deg}, {safety.neck_pitch_max_deg}] degrees" ) - sequence = actions - if boundary_action is not None: - baseline = HighLevelPolicyScheduler._validate_single_action( - boundary_action, - name="boundary_action", - ) - if float(np.dot(baseline[ROOT_QUATERNION], sequence[0, ROOT_QUATERNION])) < 0.0: - baseline[ROOT_QUATERNION] *= -1.0 - sequence = np.concatenate((baseline[None, :], actions), axis=0) - if len(sequence) < 2: - return - - root_delta = np.diff(sequence[:, 0:3], axis=0) - displacement = np.linalg.norm(root_delta, axis=1) - max_displacement = float(np.max(displacement)) - if max_displacement > safety.max_root_displacement_m: - raise ValueError( - "High-level policy root per-frame displacement exceeds limit: " - f"{max_displacement:.6g} > {safety.max_root_displacement_m:.6g} m" - ) - xy_speed = np.linalg.norm(root_delta[:, :2], axis=1) * float(action_fps) - max_xy_speed = float(np.max(xy_speed)) - if max_xy_speed > safety.max_root_xy_speed_m_s: - raise ValueError( - "High-level policy root XY speed exceeds limit: " - f"{max_xy_speed:.6g} > {safety.max_root_xy_speed_m_s:.6g} m/s" - ) - - yaws = np.asarray( - [_yaw_from_quaternion(row[ROOT_QUATERNION]) for row in sequence], - dtype=np.float64, - ) - yaw_delta = np.arctan2(np.sin(np.diff(yaws)), np.cos(np.diff(yaws))) - max_yaw_rate = float(np.max(np.abs(yaw_delta))) * float(action_fps) - if max_yaw_rate > safety.max_yaw_rate_rad_s: - raise ValueError( - "High-level policy root yaw rate exceeds limit: " - f"{max_yaw_rate:.6g} > {safety.max_yaw_rate_rad_s:.6g} rad/s" - ) - - joint_sequence = sequence if validate_joint_boundary else actions - if len(joint_sequence) < 2: - return - joint_rate = np.abs(np.diff(joint_sequence[:, 7:36], axis=0)) * float(action_fps) - max_joint_rate = float(np.max(joint_rate)) - if max_joint_rate > safety.max_joint_rate_rad_s: - raise ValueError( - "High-level policy joint rate exceeds limit: " - f"{max_joint_rate:.6g} > {safety.max_joint_rate_rad_s:.6g} rad/s" - ) - def closure_to_o6_pose( closure: object, diff --git a/teleopit/sim2real/mp/runtime.py b/teleopit/sim2real/mp/runtime.py index 0ce24df6..b05aeb95 100644 --- a/teleopit/sim2real/mp/runtime.py +++ b/teleopit/sim2real/mp/runtime.py @@ -1590,14 +1590,8 @@ def _drain_high_level_policy_ipc(self) -> None: if first_chunk: first_action = scheduler.accept_entry(chunk, now_s=now_s) else: - scheduler.reset( - packet.session_id, - initial_action=self._build_high_level_policy_boundary_action( - self.robot.get_state() - ), - ) scheduler.accept(chunk, now_s=now_s) - except (RuntimeError, ValueError) as exc: + except ValueError as exc: logger.warning("Rejected high-level policy entry chunk: %s", exc) operator_logger.warning( "High-level policy entry failed; remaining in STANDING" @@ -1673,10 +1667,11 @@ def _build_high_level_policy_boundary_action(self, state: object) -> np.ndarray: transform = self._policy_frame_transform if transform is None: raise RuntimeError("High-level policy entry is missing its frame transform") + boundary_qpos = self._policy_entry_target_qpos + if boundary_qpos is None: + boundary_qpos = self._build_robot_state_qpos(state) initial_action = np.zeros(50, dtype=np.float32) - initial_action[:36] = transform.localize_body_action( - self._build_robot_state_qpos(state) - ) + initial_action[:36] = transform.localize_body_action(boundary_qpos) return initial_action def _start_high_level_policy_entry_session(self) -> None: diff --git a/tests/test_high_level_policy.py b/tests/test_high_level_policy.py index 7b278baf..856740b4 100644 --- a/tests/test_high_level_policy.py +++ b/tests/test_high_level_policy.py @@ -210,56 +210,31 @@ def test_scheduler_validates_complete_chunk_against_onboard_safety_limits() -> N assert scheduler.has_chunk -def test_scheduler_entry_ignores_only_current_pose_boundary() -> None: +def test_scheduler_accepts_entry_boundary_and_internal_reference_discontinuities() -> None: scheduler = HighLevelPolicyScheduler(hold_s=0.1, safety=_safety_config()) initial = _safe_actions(1)[0] initial[7] = 0.8 actions = _safe_actions() - actions[:, 7] = -0.08 + actions[0, 0] = 0.2 + actions[0, 7] = -0.08 + actions[1, 0] = -0.2 + actions[1, 7] = 0.5 + yaw = 0.2 + actions[1, 3:7] = [math.cos(yaw / 2.0), 0.0, 0.0, math.sin(yaw / 2.0)] scheduler.reset("session-1", initial_action=initial) - with pytest.raises(ValueError, match="joint rate"): - scheduler.accept(_safe_chunk(actions), now_s=1.01) - - scheduler.reset("session-1", initial_action=initial) first_action = scheduler.accept_entry(_safe_chunk(actions), now_s=1.01) + assert scheduler.has_chunk + assert first_action[0] == pytest.approx(0.2) assert first_action[7] == pytest.approx(-0.08) -def test_scheduler_entry_still_rejects_internal_chunk_discontinuity() -> None: - scheduler = HighLevelPolicyScheduler(hold_s=0.1, safety=_safety_config()) - actions = _safe_actions() - actions[0, 7] = -0.08 - actions[1:, 7] = 0.5 - scheduler.reset("session-1", initial_action=_safe_actions(1)[0]) - - with pytest.raises(ValueError, match="joint rate"): - scheduler.accept_entry(_safe_chunk(actions), now_s=1.01) - - assert not scheduler.has_chunk - - -def test_scheduler_entry_still_rejects_root_boundary_displacement() -> None: - scheduler = HighLevelPolicyScheduler(hold_s=0.1, safety=_safety_config()) - initial = _safe_actions(1)[0] - actions = _safe_actions() - actions[:, 0] += 0.2 - scheduler.reset("session-1", initial_action=initial) - - with pytest.raises(ValueError, match="root per-frame displacement"): - scheduler.accept_entry(_safe_chunk(actions), now_s=1.01) - - assert not scheduler.has_chunk - - @pytest.mark.parametrize( ("mutate", "message"), [ (lambda value: value.__setitem__((1, 2), 0.4), "root height"), - (lambda value: value.__setitem__((1, 0), 0.2), "displacement"), (lambda value: value.__setitem__((1, 7), 4.0), "joint position"), - (lambda value: value.__setitem__((1, 7), 0.5), "joint rate"), (lambda value: value.__setitem__((1, 48), 46.0), "OpenNeck yaw"), (lambda value: value.__setitem__((1, 49), -41.0), "OpenNeck pitch"), ], @@ -275,18 +250,7 @@ def test_scheduler_rejects_entire_unsafe_chunk(mutate, message: str) -> None: # assert not scheduler.has_chunk -def test_scheduler_rejects_root_yaw_rate() -> None: - scheduler = HighLevelPolicyScheduler(hold_s=0.1, safety=_safety_config()) - scheduler.reset("session-1", initial_action=_safe_actions(1)[0]) - actions = _safe_actions() - yaw = 0.2 - actions[1, 3:7] = [math.cos(yaw / 2.0), 0.0, 0.0, math.sin(yaw / 2.0)] - - with pytest.raises(ValueError, match="yaw rate"): - scheduler.accept(_safe_chunk(actions), now_s=1.01) - - -def test_scheduler_rate_limits_valid_plan_at_50hz_after_latency_skip() -> None: +def test_scheduler_accepts_discontinuous_plan_and_rate_limits_output_at_50hz() -> None: scheduler = HighLevelPolicyScheduler( hold_s=0.1, safety=_safety_config(), @@ -295,9 +259,9 @@ def test_scheduler_rate_limits_valid_plan_at_50hz_after_latency_skip() -> None: initial = _safe_actions(1)[0] scheduler.reset("session-1", initial_action=initial) actions = _safe_actions(2) - actions[1, 0] = 0.08 - actions[1, 7] = 0.3 - yaw = 0.08 + actions[1, 0] = 0.2 + actions[1, 7] = 0.5 + yaw = 0.2 actions[1, 3:7] = [math.cos(yaw / 2.0), 0.0, 0.0, math.sin(yaw / 2.0)] scheduler.accept(_safe_chunk(actions), now_s=1.01) @@ -706,7 +670,7 @@ def test_policy_entry_rejects_action_received_after_total_deadline() -> None: assert accepted == [] -def test_policy_entry_fresh_chunk_uses_current_measured_joint_boundary() -> None: +def test_policy_entry_fresh_chunk_uses_held_reference_boundary() -> None: worker = object.__new__(_RobotControlWorker) now_s = time.monotonic() worker.mode = RobotMode.STANDING @@ -732,7 +696,10 @@ def test_policy_entry_fresh_chunk_uses_current_measured_joint_boundary() -> None worker._policy_resume_source_timestamp_ns = None worker._policy_entry_pending = True worker._policy_entry_deadline_s = now_s + 1.0 - worker._policy_entry_target_qpos = np.zeros(36, dtype=np.float64) + held_qpos = np.zeros(36, dtype=np.float64) + held_qpos[2] = 0.76 + held_qpos[3] = 1.0 + worker._policy_entry_target_qpos = held_qpos worker._policy_frame_transform = PolicyFrameTransform.from_robot_pose( [0.0, 0.0], [1.0, 0.0, 0.0, 0.0], @@ -745,19 +712,22 @@ def test_policy_entry_fresh_chunk_uses_current_measured_joint_boundary() -> None worker.robot = SimpleNamespace(get_state=lambda: state) worker._build_robot_state_qpos = lambda _state: current_qpos.copy() scheduler = HighLevelPolicyScheduler(hold_s=0.1, safety=_safety_config()) - scheduler.reset("session-1", initial_action=_safe_actions(1)[0]) + boundary_action = worker._build_high_level_policy_boundary_action(state) + scheduler.reset("session-1", initial_action=boundary_action) worker._high_level_policy_scheduler = scheduler worker._high_level_policy_cfg = SimpleNamespace(max_result_age_s=1.0) - standing: list[str] = [] - worker._enter_standing = lambda: standing.append("standing") - worker._transition_to_high_level_policy = lambda: pytest.fail( - "fresh chunk must be rejected against the current measured pose" + worker._enter_standing = lambda: pytest.fail( + "fresh chunk must not be compared with measured tracker joints" ) + transitions: list[str] = [] + worker._transition_to_high_level_policy = lambda: transitions.append("policy") worker._drain_high_level_policy_ipc() - assert standing == ["standing"] - assert not scheduler.has_chunk + assert boundary_action[7] == pytest.approx(0.0) + assert current_qpos[7] == pytest.approx(0.8) + assert transitions == ["policy"] + assert scheduler.has_chunk def test_policy_entry_stale_result_aborts_current_session() -> None: From 730270051b40f95a4b54271db34750e51b33bab7 Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Mon, 20 Jul 2026 17:14:16 +0800 Subject: [PATCH 29/59] Increase high-level policy reference grace period --- docs/docs/configuration/config-reference.md | 2 +- .../current/configuration/config-reference.md | 2 +- teleopit/configs/high_level_policy_sim2real.yaml | 2 +- teleopit/high_level_policy/config.py | 2 +- teleopit/high_level_policy/scheduler.py | 2 +- tests/test_high_level_policy.py | 11 ++++++++++- 6 files changed, 15 insertions(+), 6 deletions(-) diff --git a/docs/docs/configuration/config-reference.md b/docs/docs/configuration/config-reference.md index 7e6439a1..47ad796b 100644 --- a/docs/docs/configuration/config-reference.md +++ b/docs/docs/configuration/config-reference.md @@ -130,7 +130,7 @@ protocol tests. The only shared data file is `hand_calibration.json`. | `high_level_policy.max_observation_age_s` | Maximum camera/observation age before a request is skipped | `0.15` | | `high_level_policy.max_result_age_s` | Maximum local IPC age before a received result is rejected | `0.1` | | `high_level_policy.entry_timeout_s` | Maximum total entry duration (candidate, Kp ramp, fresh session) and maximum fresh-chunk wait on resume | `5.0` | -| `high_level_policy.hold_s` | Final validated-reference grace period before the watchdog pauses `POLICY` | `0.1` | +| `high_level_policy.hold_s` | Final validated-reference grace period before the watchdog pauses `POLICY` | `0.5` | | `high_level_policy.safety.root_height_min_m` / `root_height_max_m` | Accepted absolute root-height range | `0.55` / `1.05` | | `high_level_policy.safety.max_root_xy_speed_m_s` | Root XY speed limit applied to the 50 Hz scheduler output | `2.5` | | `high_level_policy.safety.max_root_displacement_m` | Source-frame-equivalent 3D root step used by the 50 Hz output limiter | `0.1` | diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md index f8727092..e70cdc1a 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md @@ -148,7 +148,7 @@ client/server 消息结构与协议测试。唯一共享的数据文件是 `hand | `high_level_policy.max_observation_age_s` | 跳过请求前允许的最大 camera/observation age | `0.15` | | `high_level_policy.max_result_age_s` | 拒绝已接收结果前允许的最大本地 IPC age | `0.1` | | `high_level_policy.entry_timeout_s` | 候选请求、Kp ramp 和新 session 的 entry 最长总时间，以及恢复时等待新鲜 chunk 的最长时间 | `5.0` | -| `high_level_policy.hold_s` | Watchdog 暂停 `POLICY` 前，最后一条有效 reference 的 grace period | `0.1` | +| `high_level_policy.hold_s` | Watchdog 暂停 `POLICY` 前，最后一条有效 reference 的 grace period | `0.5` | | `high_level_policy.safety.root_height_min_m` / `root_height_max_m` | 可接受的绝对 root 高度范围 | `0.55` / `1.05` | | `high_level_policy.safety.max_root_xy_speed_m_s` | 应用于 50 Hz scheduler 输出的 root XY 速度限制 | `2.5` | | `high_level_policy.safety.max_root_displacement_m` | 50 Hz 输出 rate limiter 使用的 source-frame 等效 3D root 步长 | `0.1` | diff --git a/teleopit/configs/high_level_policy_sim2real.yaml b/teleopit/configs/high_level_policy_sim2real.yaml index 911e8284..aec0137b 100644 --- a/teleopit/configs/high_level_policy_sim2real.yaml +++ b/teleopit/configs/high_level_policy_sim2real.yaml @@ -28,7 +28,7 @@ high_level_policy: max_observation_age_s: 0.15 max_result_age_s: 0.1 entry_timeout_s: 5.0 - hold_s: 0.1 + hold_s: 0.5 safety: root_height_min_m: 0.55 root_height_max_m: 1.05 diff --git a/teleopit/high_level_policy/config.py b/teleopit/high_level_policy/config.py index 5227385d..48516fbf 100644 --- a/teleopit/high_level_policy/config.py +++ b/teleopit/high_level_policy/config.py @@ -77,7 +77,7 @@ def parse_high_level_policy_config(cfg: Any) -> HighLevelPolicyConfig: entry_timeout_s = _positive_float( cfg_get(policy_cfg, "entry_timeout_s", 5.0), "entry_timeout_s" ) - hold_s = float(cfg_get(policy_cfg, "hold_s", 0.1)) + hold_s = float(cfg_get(policy_cfg, "hold_s", 0.5)) if not math.isfinite(hold_s) or hold_s < 0.0: raise ValueError("high_level_policy.hold_s must be finite and >= 0") return HighLevelPolicyConfig( diff --git a/teleopit/high_level_policy/scheduler.py b/teleopit/high_level_policy/scheduler.py index 8f2d8446..860c0a85 100644 --- a/teleopit/high_level_policy/scheduler.py +++ b/teleopit/high_level_policy/scheduler.py @@ -109,7 +109,7 @@ class HighLevelPolicyScheduler: def __init__( self, *, - hold_s: float = 0.1, + hold_s: float = 0.5, safety: HighLevelPolicySafetyConfig | None = None, output_hz: float = 50.0, ) -> None: diff --git a/tests/test_high_level_policy.py b/tests/test_high_level_policy.py index 856740b4..00289f3d 100644 --- a/tests/test_high_level_policy.py +++ b/tests/test_high_level_policy.py @@ -10,7 +10,10 @@ import zmq from teleopit.high_level_policy.client import HighLevelPolicyClient, PolicyActionChunk -from teleopit.high_level_policy.config import HighLevelPolicySafetyConfig +from teleopit.high_level_policy.config import ( + HighLevelPolicySafetyConfig, + parse_high_level_policy_config, +) from teleopit.high_level_policy.hand_calibration import HandCalibration from teleopit.high_level_policy.protocol import ( MAX_REQUEST_BYTES, @@ -105,6 +108,12 @@ def _safe_chunk(actions: np.ndarray, *, source_s: float = 1.0, sequence: int = 0 ) +def test_high_level_policy_default_hold_covers_transport_jitter() -> None: + config = parse_high_level_policy_config({"high_level_policy": {"task": "demo"}}) + + assert config.hold_s == pytest.approx(0.5) + + def test_packaged_hand_calibration_loads() -> None: calibration = HandCalibration.load() From a528e8178b89b65236604a9be10e3067747490fc Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Mon, 20 Jul 2026 19:52:22 +0800 Subject: [PATCH 30/59] Project small high-level policy joint limit violations --- AGENTS.md | 2 +- docs/docs/configuration/config-reference.md | 7 +++-- .../tutorials/high-level-policy-sim2real.md | 15 ++++++---- .../current/configuration/config-reference.md | 4 ++- .../tutorials/high-level-policy-sim2real.md | 11 ++++---- .../configs/high_level_policy_sim2real.yaml | 1 + teleopit/high_level_policy/config.py | 5 ++++ teleopit/high_level_policy/scheduler.py | 17 +++++++++++ tests/test_high_level_policy.py | 28 +++++++++++++++++-- 9 files changed, 73 insertions(+), 17 deletions(-) diff --git a/AGENTS.md b/AGENTS.md index cf2e0fc0..4a9a8c50 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -187,7 +187,7 @@ target_dof_pos = clip(action, -10, 10) × action_scale + default_dof_pos - High-level-policy formal robot modes are `IDLE`, `STANDING`, `POLICY`, and `DAMPING`. After Unitree remote `Y`, entry remains internal to `STANDING`: validate one candidate chunk, hold `action[0]` through the existing tracker for one Kp ramp, then create one new host session and require a fresh valid chunk before entering `POLICY`; the 50 Hz output limiter starts from the held `action[0]` reference rather than measured tracker joints, a failure aborts entry, and there is no `POLICY_STARTING` mode - Unitree remote controls are `Start -> STANDING`, `Y -> request POLICY`, `B -> pause/resume`, `X -> STANDING/cancel pending`, and `L1+R1 -> DAMPING` - Policy pause freezes the scheduler/body reference and holds the latest hand/neck command. Leaving `POLICY` opens LinkerHand and centers OpenNeck -- The onboard scheduler rejects whole chunks on shape/finiteness, session/sequence, quaternion, absolute root height, G1 joint position, hand closure, OpenNeck degree range, or staleness failures. It accepts temporal root, yaw, and joint-reference discontinuities at entry, inside chunks, and across chunks because recorded pause/resume transitions may be discontinuous; accepted root translation, yaw, and joint output is rate-limited at 50 Hz. Never pad, trim, or clip malformed/out-of-range host output into validity +- The onboard scheduler clips G1 joint references to `real_robot.joint_pos_lower/upper` when the required correction is at most `high_level_policy.safety.max_joint_projection_rad` (default `0.1` rad), then rejects whole chunks on excessive joint correction, shape/finiteness, session/sequence, quaternion, absolute root height, hand closure, OpenNeck degree range, or staleness failures. It accepts temporal root, yaw, and joint-reference discontinuities at entry, inside chunks, and across chunks because recorded pause/resume transitions may be discontinuous; accepted root translation, yaw, and joint output is rate-limited at 50 Hz. Never pad or trim malformed host output, and do not clip other out-of-range fields into validity - A short cached-reference grace period is allowed; exhaustion, host/network failure, or loss of a required camera/client worker puts `POLICY` into the same resumable pause state used by remote `B`. The runtime never enters `STANDING` automatically; `B` resumes after a fresh valid chunk is available, while `X` remains the manual transition to `STANDING` - Initial production hardware support requires two LinkerHand O6 hands and OpenNeck because all 50 canonical action dimensions are active - OpenNeck 0.2.0 is the supported runtime; Teleopit sends physical degrees through `move_deg()`, and the direct-drive OpenNeck package converts degrees to servo steps and clips them to its calibrated mechanical limits; the removed normalized API and config fields are unsupported diff --git a/docs/docs/configuration/config-reference.md b/docs/docs/configuration/config-reference.md index 47ad796b..c7fb6552 100644 --- a/docs/docs/configuration/config-reference.md +++ b/docs/docs/configuration/config-reference.md @@ -136,11 +136,14 @@ protocol tests. The only shared data file is `hand_calibration.json`. | `high_level_policy.safety.max_root_displacement_m` | Source-frame-equivalent 3D root step used by the 50 Hz output limiter | `0.1` | | `high_level_policy.safety.max_yaw_rate_rad_s` | Root yaw-rate limit applied to the 50 Hz scheduler output | `2.5` | | `high_level_policy.safety.max_joint_rate_rad_s` | Per-joint rate limit applied to the 50 Hz scheduler output | `10.0` | +| `high_level_policy.safety.max_joint_projection_rad` | Maximum correction allowed when clipping a G1 joint reference to its position limit | `0.1` | | `high_level_policy.safety.neck_yaw_min_deg` / `neck_yaw_max_deg` | Accepted OpenNeck yaw command range | `-45` / `45` | | `high_level_policy.safety.neck_pitch_min_deg` / `neck_pitch_max_deg` | Accepted OpenNeck pitch command range | `-40` / `40` | -G1 reference joint positions are checked against -`real_robot.joint_pos_lower/upper`. The initial runtime requires +G1 reference joint positions are clipped to +`real_robot.joint_pos_lower/upper` when the required correction does not exceed +`high_level_policy.safety.max_joint_projection_rad`; larger corrections reject +the chunk. The initial runtime requires `hands.driver=linkerhand_o6`, both hand sides, and `neck.driver=openneck` because all canonical 50D action fields are active. OpenNeck policy values go directly to `move_deg(yaw, pitch)` after chunk validation; Pico dead-zone and pitch-gain diff --git a/docs/docs/tutorials/high-level-policy-sim2real.md b/docs/docs/tutorials/high-level-policy-sim2real.md index 51ba11ed..2c16d574 100644 --- a/docs/docs/tutorials/high-level-policy-sim2real.md +++ b/docs/docs/tutorials/high-level-policy-sim2real.md @@ -165,13 +165,16 @@ enters `STANDING` automatically; `X` remains the manual transition. ## 6. Onboard Validation and Watchdog -Teleopit rejects a complete chunk if any frame violates the contract. It never -pads, trims, or safety-clips a malformed host result. Checks include: +Teleopit clips a G1 joint reference to the configured real-robot position +limits when the correction is at most `max_joint_projection_rad`, then rejects +a complete chunk if any frame violates the remaining contract. It never pads +or trims a malformed host result. Checks include: - exact finite `float32[T,50]`, current session, and increasing source sequence; - normalized root quaternion with temporal sign continuity; - absolute root-height limits; -- absolute G1 joint-position limits; +- G1 joint-position clipping to `real_robot.joint_pos_lower/upper`, with larger + corrections rejected; - LinkerHand closure `[0,1]` and configured OpenNeck degree ranges; - observation/result age, source timestamp, and action horizon. @@ -180,9 +183,9 @@ yaw, and G1 joint-reference jumps are accepted at entry, inside a chunk, and across chunks because a recorded pause/resume transition can intentionally be discontinuous. Host requests are paused for one Kp ramp. A new host session then supplies the fresh chunk that will actually enter `POLICY`; the candidate -chunk is never continued as a live timeline. A malformed, stale, or -out-of-range fresh chunk aborts entry instead of starting another alignment -cycle. +chunk is never continued as a live timeline. A malformed or stale chunk, an +out-of-range non-joint field, or an excessive joint correction in a fresh chunk +aborts entry instead of starting another alignment cycle. Validated 30 Hz body references are interpolated and rate-limited locally at 50 Hz, including when latency skips source frames or a new chunk replaces the diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md index e70cdc1a..652fd67d 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md @@ -154,10 +154,12 @@ client/server 消息结构与协议测试。唯一共享的数据文件是 `hand | `high_level_policy.safety.max_root_displacement_m` | 50 Hz 输出 rate limiter 使用的 source-frame 等效 3D root 步长 | `0.1` | | `high_level_policy.safety.max_yaw_rate_rad_s` | 应用于 50 Hz scheduler 输出的 root yaw rate 限制 | `2.5` | | `high_level_policy.safety.max_joint_rate_rad_s` | 应用于 50 Hz scheduler 输出的单关节 rate 限制 | `10.0` | +| `high_level_policy.safety.max_joint_projection_rad` | 将 G1 关节 reference 裁剪到位置限位时允许的最大修正量 | `0.1` | | `high_level_policy.safety.neck_yaw_min_deg` / `neck_yaw_max_deg` | 可接受的 OpenNeck yaw 命令范围 | `-45` / `45` | | `high_level_policy.safety.neck_pitch_min_deg` / `neck_pitch_max_deg` | 可接受的 OpenNeck pitch 命令范围 | `-40` / `40` | -G1 reference joint position 会按 `real_robot.joint_pos_lower/upper` 检查。由于 canonical +当所需修正量不超过 `high_level_policy.safety.max_joint_projection_rad` 时，G1 reference +joint position 会裁剪到 `real_robot.joint_pos_lower/upper`；更大的修正量会导致 chunk 被拒绝。由于 canonical 50D action 的所有字段都处于启用状态，初始运行时要求 `hands.driver=linkerhand_o6`、左右两只手以及 `neck.driver=openneck`。OpenNeck 策略值 在 chunk 验证后直接发送给 `move_deg(yaw, pitch)`；不会应用 Pico dead-zone 或 pitch-gain diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md index d7d9ddf9..2cb2377c 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md @@ -151,21 +151,22 @@ Watchdog、主机/网络、相机或 policy client 故障也会进入同一个 ## 6. Onboard 验证与 Watchdog -如果任一帧违反契约，Teleopit 会拒绝整个 chunk。它不会对错误的主机结果进行补齐、 -裁剪或安全限幅。检查包括： +当修正量不超过 `max_joint_projection_rad` 时，Teleopit 会先把 G1 关节 reference 裁剪到 +配置的真机关节位置范围；如果任一帧违反其余契约，则拒绝整个 chunk。它不会对错误的 +主机结果进行补齐或删减。检查包括： - 精确且有限的 `float32[T,50]`、当前 session，以及递增的 source sequence； - 归一化 root quaternion 与时间连续的符号； - 绝对 root 高度限制； -- 绝对 G1 关节位置限制； +- 按 `real_robot.joint_pos_lower/upper` 裁剪 G1 关节位置，并拒绝更大的修正量； - LinkerHand closure `[0,1]` 和配置的 OpenNeck 角度范围； - observation/result 时效、source timestamp 和 action horizon。 reference 连续性不是接收条件。entry、chunk 内部和 chunk 之间的 root translation、root yaw 与 G1 关节 reference 跳变都会被接受，因为录制的 pause/resume 转换可能有意地不 连续。一次 Kp ramp 期间会暂停主机请求，随后新 host session 会提供真正进入 `POLICY` -的新鲜 chunk；候选 chunk 绝不会作为实时 timeline 继续播放。格式错误、过期或超出绝对 -范围的新鲜 chunk 会终止 entry，而不会开始另一轮对齐。 +的新鲜 chunk；候选 chunk 绝不会作为实时 timeline 继续播放。格式错误、过期、非关节字段 +超出绝对范围或关节修正量过大的新鲜 chunk 会终止 entry，而不会开始另一轮对齐。 通过验证的 30 Hz body reference 会在本地插值到 50 Hz 并执行 rate limit；网络延迟 导致跳过 source frame 或新 chunk 替换旧计划时同样如此。配置的 root displacement/XY diff --git a/teleopit/configs/high_level_policy_sim2real.yaml b/teleopit/configs/high_level_policy_sim2real.yaml index aec0137b..a1893389 100644 --- a/teleopit/configs/high_level_policy_sim2real.yaml +++ b/teleopit/configs/high_level_policy_sim2real.yaml @@ -36,6 +36,7 @@ high_level_policy: max_root_displacement_m: 0.1 max_yaw_rate_rad_s: 2.5 max_joint_rate_rad_s: 10.0 + max_joint_projection_rad: 0.1 neck_yaw_min_deg: -45.0 neck_yaw_max_deg: 45.0 neck_pitch_min_deg: -40.0 diff --git a/teleopit/high_level_policy/config.py b/teleopit/high_level_policy/config.py index 48516fbf..fafd36b2 100644 --- a/teleopit/high_level_policy/config.py +++ b/teleopit/high_level_policy/config.py @@ -40,6 +40,7 @@ class HighLevelPolicySafetyConfig: max_root_displacement_m: float max_yaw_rate_rad_s: float max_joint_rate_rad_s: float + max_joint_projection_rad: float joint_pos_lower: tuple[float, ...] joint_pos_upper: tuple[float, ...] neck_yaw_min_deg: float @@ -189,6 +190,10 @@ def parse_high_level_policy_safety_config(cfg: Any) -> HighLevelPolicySafetyConf cfg_get(safety_cfg, "max_joint_rate_rad_s", 10.0), "safety.max_joint_rate_rad_s", ), + max_joint_projection_rad=_positive_float( + cfg_get(safety_cfg, "max_joint_projection_rad", 0.1), + "safety.max_joint_projection_rad", + ), joint_pos_lower=joint_pos_lower, joint_pos_upper=joint_pos_upper, neck_yaw_min_deg=neck_yaw_min_deg, diff --git a/teleopit/high_level_policy/scheduler.py b/teleopit/high_level_policy/scheduler.py index 860c0a85..67e16fe3 100644 --- a/teleopit/high_level_policy/scheduler.py +++ b/teleopit/high_level_policy/scheduler.py @@ -368,6 +368,23 @@ def _validate_actions( raise ValueError("High-level policy LinkerHand closure must be within [0, 1]") safety = self.safety if safety is not None: + joints = validated[:, 7:36] + projected = np.clip( + joints, + np.asarray(safety.joint_pos_lower, dtype=np.float32), + np.asarray(safety.joint_pos_upper, dtype=np.float32), + ) + correction = np.abs(projected - joints) + violations = np.argwhere(correction > safety.max_joint_projection_rad) + if len(violations): + frame, joint = (int(value) for value in violations[0]) + raise ValueError( + "High-level policy joint projection correction exceeds " + f"{safety.max_joint_projection_rad:.6g} rad: " + f"action[{frame}, {7 + joint}] correction=" + f"{float(correction[frame, joint]):.6g} rad" + ) + validated[:, 7:36] = projected self._validate_safety_limits(validated, safety=safety) return validated diff --git a/tests/test_high_level_policy.py b/tests/test_high_level_policy.py index 00289f3d..3151aa26 100644 --- a/tests/test_high_level_policy.py +++ b/tests/test_high_level_policy.py @@ -87,6 +87,7 @@ def _safety_config() -> HighLevelPolicySafetyConfig: max_root_displacement_m=0.1, max_yaw_rate_rad_s=2.5, max_joint_rate_rad_s=10.0, + max_joint_projection_rad=0.1, joint_pos_lower=(-3.0,) * 29, joint_pos_upper=(3.0,) * 29, neck_yaw_min_deg=-45.0, @@ -239,16 +240,39 @@ def test_scheduler_accepts_entry_boundary_and_internal_reference_discontinuities assert first_action[7] == pytest.approx(-0.08) +def test_scheduler_clips_joint_positions_to_onboard_limits() -> None: + scheduler = HighLevelPolicyScheduler(hold_s=0.1, safety=_safety_config()) + scheduler.reset("session-1", initial_action=_safe_actions(1)[0]) + actions = _safe_actions(1) + actions[0, 7] = -3.08 + actions[0, 8] = 3.08 + + first_action = scheduler.accept_entry(_safe_chunk(actions), now_s=1.01) + + assert first_action[7] == pytest.approx(-3.0) + assert first_action[8] == pytest.approx(3.0) + + +def test_scheduler_rejects_joint_projection_above_limit() -> None: + scheduler = HighLevelPolicyScheduler(hold_s=0.1, safety=_safety_config()) + scheduler.reset("session-1", initial_action=_safe_actions(1)[0]) + actions = _safe_actions(1) + actions[0, 7] = -3.11 + + with pytest.raises(ValueError, match="joint projection correction exceeds"): + scheduler.accept(_safe_chunk(actions), now_s=1.01) + assert not scheduler.has_chunk + + @pytest.mark.parametrize( ("mutate", "message"), [ (lambda value: value.__setitem__((1, 2), 0.4), "root height"), - (lambda value: value.__setitem__((1, 7), 4.0), "joint position"), (lambda value: value.__setitem__((1, 48), 46.0), "OpenNeck yaw"), (lambda value: value.__setitem__((1, 49), -41.0), "OpenNeck pitch"), ], ) -def test_scheduler_rejects_entire_unsafe_chunk(mutate, message: str) -> None: # type: ignore[no-untyped-def] +def test_scheduler_rejects_entire_unsafe_non_joint_chunk(mutate, message: str) -> None: # type: ignore[no-untyped-def] scheduler = HighLevelPolicyScheduler(hold_s=0.1, safety=_safety_config()) scheduler.reset("session-1", initial_action=_safe_actions(1)[0]) actions = _safe_actions() From 9cf046cf9390435473784ff5279e42050701f7ae Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Mon, 20 Jul 2026 22:05:18 +0800 Subject: [PATCH 31/59] Harden Pico video failure handling --- AGENTS.md | 3 + README.md | 6 + docs/docs/configuration/config-reference.md | 10 +- docs/docs/tutorials/pico-sim2real.md | 11 +- docs/docs/tutorials/pico-sim2sim.md | 3 +- .../current/configuration/config-reference.md | 7 +- .../current/tutorials/pico-sim2real.md | 8 +- .../current/tutorials/pico-sim2sim.md | 3 +- scripts/run/check_pico_signal.py | 2 +- teleopit/configs/input/pico4.yaml | 1 - teleopit/configs/sim2real_record.yaml | 1 - teleopit/inputs/pico_video.py | 121 ++++++++++-------- teleopit/pipeline.py | 1 - teleopit/sim2real/mp/runtime.py | 51 +++++++- tests/test_pico_video.py | 57 ++++++--- tests/test_sim2real_multiprocess.py | 39 +++++- 16 files changed, 226 insertions(+), 98 deletions(-) diff --git a/AGENTS.md b/AGENTS.md index 4a9a8c50..ae16523f 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -144,6 +144,8 @@ target_dof_pos = clip(action, -10, 10) × action_scale + default_dof_pos - The pico-bridge receiver runs on the Teleopit host, which can be a workstation PC or robot onboard computer; do not maintain a separate onboard Pico input mode - pico-bridge 0.2.1 is the supported runtime; camera preview uses `PicoBridge(video="frames").push_video_frame(rgb_uint8)` - Pico video preview is optional and disabled by default; sim2sim uses the MuJoCo `d435i_rgb` camera and sim2real uses RealSense when `input.video.enabled=true` +- RealSense frame timeouts and disconnects are non-critical in sim2real: the video producer rebuilds the capture pipeline in the background, and video start/tick/stop failures must never exit `pico_input` or stop G1 control +- The supervisor treats `pico_input` as non-critical; if that process exits, `robot_control` remains active, stale mocap references hold the last command, and the Unitree remote remains available for `STANDING` or manual `DAMPING` - Bone naming follows `pico_bridge_to_g1.json` - The provider applies an input-space transform to match the current retarget config - Do not hardcode that transform as a public coordinate-system contract; validate against actual retarget/sim2sim behavior when SDK or firmware changes @@ -159,6 +161,7 @@ target_dof_pos = clip(action, -10, 10) × action_scale + default_dof_pos - Optional LinkerHand control uses `hands.enabled=true`, `hands.driver=linkerhand_l6|linkerhand_o6`, and `hands.mode=gripper|vr_hand_pose`; default is disabled - Optional Pico sim2real HDF5 recording uses `--config-name sim2real_record` or `recording.enabled=true`; it requires `input.provider=pico4`, `input.video.enabled=true`, `input.video.source=realsense`, an interactive terminal, and the `recording` extra - Recording is manual only: terminal `R` starts an episode, `S` saves, `D` discards the active episode, and `Q` shuts down; `STANDING`, `MOCAP`, `ARMS`, and paused mocap are recordable +- Recording requires a fresh RealSense frame before `R` can start an episode; an active episode is discarded after one second without a fresh camera frame while Pico input and G1 control continue, and recording does not restart automatically when video recovers - Recording captures `observation.images.d435i_rgb` RealSense RGB video at 30Hz plus `observation.state(68)`, scalar `observation.mode`, and `action(36)` as the root-plus-joint reference consumed by the motion tracker; `action.hand(12)` is present when LinkerHand control is enabled, and `action.neck(2)` stores the mechanically clamped OpenNeck `[yaw_deg, pitch_deg]` target when OpenNeck control is enabled - Sim2real recording uses an editable source layout: `schema.json`, `episodes.jsonl`, per-episode HDF5 files under `recording.output_dir/data/`, and compressed MP4 files under `recording.output_dir/videos/d435i_rgb/`; task prompts live only in `episodes.jsonl`, and HDF5 files contain only frame arrays with no metadata attributes or raw RGB datasets - Recording `schema.json` stores `robot_type=unitree_g1_29dof`, `hand_type=none|linkerhand_l6|linkerhand_o6`, `neck_type=none|openneck`, FPS, and feature definitions; optional action fields are controlled directly by `hands.enabled` and `neck.enabled`; the recording worker rejects an existing mismatched schema without writing episodes, but remains non-critical and must not stop the G1 control runtime; the previous attribute-based HDF5 layout is unsupported diff --git a/README.md b/README.md index ca76499d..3cac00da 100644 --- a/README.md +++ b/README.md @@ -112,6 +112,12 @@ frame-aligned arrays: `observation.state(68)`, scalar `observation.mode`, and Recording is non-critical: an incompatible output schema stops only the recording worker while G1 control continues. Episodes interrupted before their manifest entry is committed are discarded on the next recording startup. +RealSense frame timeouts and disconnects trigger background camera reconnection +without stopping Pico input or G1 control. Recording requires a fresh camera +frame to start and discards an active episode after one second without video; +press `R` again after the camera recovers. If the entire Pico input worker exits, +G1 control remains active and holds the latest command so the operator can use +the Unitree remote to return to `STANDING` or request `DAMPING`. Review saved episodes in a synchronized read-only web UI: diff --git a/docs/docs/configuration/config-reference.md b/docs/docs/configuration/config-reference.md index c7fb6552..0cdeed30 100644 --- a/docs/docs/configuration/config-reference.md +++ b/docs/docs/configuration/config-reference.md @@ -75,7 +75,6 @@ Complete reference for all configurable fields. | `input.video.source` | Video source: `mujoco`, `realsense`, or `test-pattern` | `null` | | `input.video.width` / `height` / `fps` | Video capture/render settings | `1280` / `720` / `30` | | `input.video.device` | Optional RealSense serial | `null` | -| `input.video.fail_on_error` | Fail startup instead of disabling video on error | `false` | ### Realtime @@ -266,7 +265,14 @@ same frames produced by `pico_input`. | `recording.camera.device` | Optional RealSense serial | `null` | | `recording.video.codec` / `quality` / `pixelformat` | MP4 sidecar encoder settings | `libx264` / `8` / `yuv420p` | -Camera failure behavior is controlled by `input.video.fail_on_error`. +RealSense frame timeouts and disconnects rebuild the capture pipeline in the +background and never stop Pico input or G1 control. Recording requires a fresh +camera frame before accepting `R`. An active episode is discarded after one +second without a fresh frame, and recording remains idle after the camera +recovers until the operator presses `R` again. If the entire `pico_input` +worker exits, `robot_control` remains active and holds the latest command; the +Unitree remote remains available for returning to `STANDING` or requesting +`DAMPING`. The recorder creates an editable source dataset: diff --git a/docs/docs/tutorials/pico-sim2real.md b/docs/docs/tutorials/pico-sim2real.md index 8ca4ac20..69192c7f 100644 --- a/docs/docs/tutorials/pico-sim2real.md +++ b/docs/docs/tutorials/pico-sim2real.md @@ -127,6 +127,14 @@ control is enabled, it stores the latest mechanically clamped `[yaw_deg, pitch_deg]` target in degrees as `action.neck(2)`. Disabled devices do not add their action fields. +Recording starts only when a fresh RealSense frame is available. RealSense +timeouts or disconnects trigger background reconnection without stopping Pico +input or G1 control. If video is unavailable for one second during recording, +the active episode is discarded; press `R` again after video recovers. If the +entire Pico input worker exits, G1 control remains active and holds the latest +command so the Unitree remote can return the robot to `STANDING` or request +`DAMPING`. + ### Review Saved Episodes Install the lightweight review dependencies and launch the read-only web @@ -319,7 +327,8 @@ python scripts/run/run_sim2real.py \ input.video.device= ``` -If video fails, control continues unless `input.video.fail_on_error=true`. +RealSense frame timeouts and disconnects reconnect in the background and never +stop Pico tracking or G1 control. ## Common Parameters diff --git a/docs/docs/tutorials/pico-sim2sim.md b/docs/docs/tutorials/pico-sim2sim.md index cda722f0..a545726c 100644 --- a/docs/docs/tutorials/pico-sim2sim.md +++ b/docs/docs/tutorials/pico-sim2sim.md @@ -116,8 +116,7 @@ python scripts/run/run_sim.py \ Use `input.video.source=test-pattern` for a receiver-side video sanity check. If video startup fails, Teleopit logs the error, disables video, and keeps tracking -and control running. Set `input.video.fail_on_error=true` to fail startup -instead. +and control running. ## Common Parameters diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md index 652fd67d..c6aeecef 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md @@ -92,7 +92,6 @@ target = clip(action, clip_range) * action_scale + default_dof_pos | `video.source` | str/null | `null` | 视频源：`mujoco`、`realsense` 或 `test-pattern` | | `video.width` / `height` / `fps` | int | `1280` / `720` / `30` | 视频采集/渲染设置 | | `video.device` | str/null | `null` | 可选的 RealSense 序列号 | -| `video.fail_on_error` | bool | `false` | 视频失败时是否让启动失败，而不是关闭视频后继续 | ## Realtime 字段 @@ -272,7 +271,11 @@ OpenNeck 归一化配置；运行 `openneck calibrate` 创建当前格式的文 | `recording.camera.device` | 可选 RealSense 序列号 | `null` | | `recording.video.codec` / `quality` / `pixelformat` | MP4 sidecar 编码设置 | `libx264` / `8` / `yuv420p` | -相机失败时的行为由 `input.video.fail_on_error` 控制。 +RealSense 帧超时或断连时会在后台重建采集 pipeline，绝不会停止 Pico 输入或 G1 +控制。按 `R` 开始录制前必须存在新鲜相机帧。录制期间一秒内没有新鲜帧时，当前 +episode 会被丢弃；相机恢复后录制仍保持空闲，直到操作员再次按 `R`。 +如果整个 `pico_input` worker 退出，`robot_control` 会继续运行并保持最新命令； +Unitree 遥控器仍可用于返回 `STANDING` 或请求 `DAMPING`。 录制器会创建一份便于编辑的源数据集： diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md index f41ef7de..e4213b6a 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md @@ -121,6 +121,12 @@ episode 会保存为 `data/recordings/sim2real_hdf5/data/` 下的 `.h5` 文件 `[yaw_deg, pitch_deg]` 目标以度为单位保存为 `action.neck(2)`。未启用的设备不会添加 对应的 action 字段。 +只有存在新鲜 RealSense 帧时才能开始录制。RealSense 超时或断连会触发后台重连， +不会停止 Pico 输入或 G1 控制。录制期间视频不可用达到一秒时，当前 episode 会被 +丢弃；视频恢复后需要再次按 `R`。如果整个 Pico 输入 worker 退出，G1 控制会继续 +运行并保持最新命令，操作员仍可使用 Unitree 遥控器让机器人返回 `STANDING` 或请求 +`DAMPING`。 + ### Review 已保存的 Episode 安装轻量 review 依赖，然后对录制根目录启动只读 Web reviewer： @@ -297,7 +303,7 @@ python scripts/run/run_sim2real.py \ input.video.device= ``` -如果视频失败，控制会继续运行，除非设置了 `input.video.fail_on_error=true`。 +RealSense 帧超时或断连时会在后台重连，绝不会停止 Pico 追踪或 G1 控制。 ## 常用参数 diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2sim.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2sim.md index df1c286d..efb96155 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2sim.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2sim.md @@ -107,8 +107,7 @@ python scripts/run/run_sim.py \ ``` 使用 `input.video.source=test-pattern` 可以做 receiver 侧视频 sanity check。如果视频启动失败， -Teleopit 会记录错误、关闭视频，并继续运行追踪和控制。设置 -`input.video.fail_on_error=true` 可改为启动失败。 +Teleopit 会记录错误、关闭视频，并继续运行追踪和控制。 ## 常用参数 diff --git a/scripts/run/check_pico_signal.py b/scripts/run/check_pico_signal.py index 44eac250..d87e884a 100644 --- a/scripts/run/check_pico_signal.py +++ b/scripts/run/check_pico_signal.py @@ -222,7 +222,7 @@ def main(cfg: DictConfig) -> None: stop_event = threading.Event() _install_signal_handlers(stop_event) provider = _build_provider(cfg, video_cfg.enabled) - video_runtime = PicoVideoRuntime(provider=provider, config=video_cfg, mode="sim2real") + video_runtime = PicoVideoRuntime(provider=provider, config=video_cfg) total = 0 valid = 0 invalid_reasons: Counter[str] = Counter() diff --git a/teleopit/configs/input/pico4.yaml b/teleopit/configs/input/pico4.yaml index 8aa4d7dc..2861f955 100644 --- a/teleopit/configs/input/pico4.yaml +++ b/teleopit/configs/input/pico4.yaml @@ -22,4 +22,3 @@ video: height: 720 fps: 30 device: null - fail_on_error: false diff --git a/teleopit/configs/sim2real_record.yaml b/teleopit/configs/sim2real_record.yaml index 2fda8bf4..c598ab40 100644 --- a/teleopit/configs/sim2real_record.yaml +++ b/teleopit/configs/sim2real_record.yaml @@ -13,4 +13,3 @@ input: height: 480 fps: 30 device: null - fail_on_error: true diff --git a/teleopit/inputs/pico_video.py b/teleopit/inputs/pico_video.py index c3ae173f..ae24d3f9 100644 --- a/teleopit/inputs/pico_video.py +++ b/teleopit/inputs/pico_video.py @@ -23,7 +23,6 @@ class PicoVideoConfig: height: int = 720 fps: int = 30 device: str | None = None - fail_on_error: bool = False def parse_pico_video_config(input_cfg: Any) -> PicoVideoConfig: @@ -48,7 +47,6 @@ def parse_pico_video_config(input_cfg: Any) -> PicoVideoConfig: height=height, fps=fps, device=None if device in (None, "", "null") else str(device), - fail_on_error=bool(cfg_get(video_cfg, "fail_on_error", False)), ) @@ -68,13 +66,11 @@ def __init__( *, provider: Any, config: PicoVideoConfig, - mode: str, robot: Any | None = None, frame_callback: Callable[[np.ndarray, float], None] | None = None, ) -> None: self._provider = provider self._config = config - self._mode = mode self._robot = robot self._frame_callback = frame_callback self._producer: _VideoProducer | None = None @@ -140,9 +136,7 @@ def stop(self) -> None: self._producer = None def _handle_error(self, exc: Exception) -> None: - if self._config.fail_on_error: - raise RuntimeError("Pico video pipeline failed") from exc - logger.warning("Pico video disabled after error: %s", exc) + logger.warning("Pico video disabled after error; tracking and control continue: %s", exc) class _VideoProducer: @@ -154,6 +148,10 @@ def stop(self) -> None: ... class _RealSenseVideoProducer(_VideoProducer): + _STARTUP_WAIT_S = 5.0 + _FRAME_TIMEOUT_MS = 1000 + _RECONNECT_DELAY_S = 1.0 + def __init__( self, provider: Any, @@ -175,11 +173,14 @@ def pushed_frames(self) -> int: def start(self) -> None: self._thread.start() - self._ready_event.wait(timeout=5.0) + self._ready_event.wait(timeout=self._STARTUP_WAIT_S) if self._error is not None: raise RuntimeError("failed to start RealSense video producer") from self._error if not self._ready_event.is_set(): - raise TimeoutError("RealSense video producer did not become ready within 5s") + logger.warning( + "RealSense video producer is not ready after %.1fs; reconnecting in background", + self._STARTUP_WAIT_S, + ) def tick(self) -> None: if self._error is not None: @@ -191,52 +192,68 @@ def stop(self) -> None: self._thread.join(timeout=2.0) def _run(self) -> None: - pipeline_started = False try: import pyrealsense2 as rs - - pipeline = rs.pipeline() - config = rs.config() - if self._config.device is not None: - config.enable_device(self._config.device) - config.enable_stream( - rs.stream.color, - self._config.width, - self._config.height, - rs.format.rgb8, - self._config.fps, - ) - pipeline.start(config) - pipeline_started = True + except BaseException as exc: + self._error = exc self._ready_event.set() - try: - while not self._stop_event.is_set(): - try: - frames = pipeline.wait_for_frames() - except RuntimeError as exc: - message = str(exc).lower() - is_timeout = "timeout" in message or "timed out" in message or "frame didn't arrive" in message - if self._config.fail_on_error or not is_timeout: - raise - logger.warning("RealSense Pico video frame timeout; continuing: %s", exc) - continue - color_frame = frames.get_color_frame() - if not color_frame: - continue - rgb = np.ascontiguousarray(np.asanyarray(color_frame.get_data()), dtype=np.uint8) - timestamp_s = time.monotonic() - if self._frame_callback is not None: - self._frame_callback(rgb, timestamp_s) - if callable(getattr(self._provider, "push_video_frame", None)): - self._pushed_frames = int(self._provider.push_video_frame(rgb)) - else: - self._pushed_frames += 1 - finally: - if pipeline_started: - try: - pipeline.stop() - except RuntimeError: - logger.exception("Failed to stop RealSense pipeline after video producer exit") + logger.exception("RealSense Pico video producer could not load pyrealsense2") + return + + reconnecting = False + try: + while not self._stop_event.is_set(): + pipeline = None + pipeline_started = False + try: + pipeline = rs.pipeline() + config = rs.config() + if self._config.device is not None: + config.enable_device(self._config.device) + config.enable_stream( + rs.stream.color, + self._config.width, + self._config.height, + rs.format.rgb8, + self._config.fps, + ) + pipeline.start(config) + pipeline_started = True + self._ready_event.set() + if reconnecting: + logger.info("RealSense Pico video stream reconnected") + reconnecting = False + while not self._stop_event.is_set(): + frames = pipeline.wait_for_frames(self._FRAME_TIMEOUT_MS) + color_frame = frames.get_color_frame() + if not color_frame: + continue + rgb = np.ascontiguousarray(np.asanyarray(color_frame.get_data()), dtype=np.uint8) + timestamp_s = time.monotonic() + if self._frame_callback is not None: + self._frame_callback(rgb, timestamp_s) + if callable(getattr(self._provider, "push_video_frame", None)): + self._pushed_frames = int(self._provider.push_video_frame(rgb)) + else: + self._pushed_frames += 1 + except Exception as exc: + if self._stop_event.is_set(): + break + reconnecting = True + logger.warning( + "RealSense Pico video stream lost; reconnecting in %.1fs: %s", + self._RECONNECT_DELAY_S, + exc, + ) + finally: + if pipeline_started and pipeline is not None: + try: + pipeline.stop() + except RuntimeError as exc: + logger.warning("Failed to stop RealSense pipeline during reconnect: %s", exc) + + if not self._stop_event.is_set(): + self._stop_event.wait(self._RECONNECT_DELAY_S) except BaseException as exc: self._error = exc self._ready_event.set() diff --git a/teleopit/pipeline.py b/teleopit/pipeline.py index 181a1912..5a9e0b63 100644 --- a/teleopit/pipeline.py +++ b/teleopit/pipeline.py @@ -43,7 +43,6 @@ def __init__(self, cfg: DictConfig | dict[str, Any], *, console: PlainConsole | self.video_runtime = PicoVideoRuntime( provider=self.input_provider, config=parse_pico_video_config(input_cfg), - mode="sim2sim", robot=self.robot, ) self.loop = SimulationLoop( diff --git a/teleopit/sim2real/mp/runtime.py b/teleopit/sim2real/mp/runtime.py index b05aeb95..eac18b42 100644 --- a/teleopit/sim2real/mp/runtime.py +++ b/teleopit/sim2real/mp/runtime.py @@ -488,8 +488,6 @@ def run(self) -> None: self._poll_terminal_recording_controls() time.sleep(0.2) critical_names = {"robot_control", "reference"} - if _input_provider_kind(self.cfg) == "pico4": - critical_names.add("pico_input") critical_dead = [ process.name for process in self._processes @@ -510,7 +508,10 @@ def run(self) -> None: and process.name not in reported_noncritical_dead ] if noncritical_dead: - operator_logger.warning("non-critical worker exited: %s", ", ".join(noncritical_dead)) + operator_logger.warning( + "non-critical worker exited: %s; G1 control remains active", + ", ".join(noncritical_dead), + ) reported_noncritical_dead.update(noncritical_dead) except KeyboardInterrupt: operator_logger.info("keyboard interrupt -> shutting down") @@ -690,7 +691,6 @@ def _publish_recording_frame(frame: NDArray[np.generic], timestamp_s: float) -> video_runtime = PicoVideoRuntime( provider=provider, config=video_cfg, - mode="sim2real", frame_callback=_publish_recording_frame if _recording_enabled(cfg) else None, ) @@ -703,9 +703,19 @@ def _publish_recording_frame(frame: NDArray[np.generic], timestamp_s: float) -> last_video_seq = -1 last_health_s = 0.0 try: - video_runtime.start() + try: + video_runtime.start() + except Exception: + logger.exception( + "Pico video startup failed; video is disabled while pico_input and robot control continue" + ) while not stop_event.is_set(): - video_runtime.tick() + try: + video_runtime.tick() + except Exception: + logger.exception( + "Pico video runtime failed; video is disabled while pico_input and robot control continue" + ) command = command_sub.recv_latest() if isinstance(command, CommandPacket) and command.command == "shutdown": stop_event.set() @@ -800,7 +810,10 @@ def _publish_recording_frame(frame: NDArray[np.generic], timestamp_s: float) -> last_health_s = now time.sleep(sleep_s) finally: - video_runtime.stop() + try: + video_runtime.stop() + except Exception: + logger.exception("Failed to stop Pico video runtime during pico_input cleanup") if frame_writer is not None: frame_writer.close(unlink=True) command_sub.close() @@ -2563,6 +2576,8 @@ def _main() -> None: class _RecordingWorker: + _CAMERA_TIMEOUT_S = 1.0 + def __init__( self, cfg: dict[str, Any], @@ -2611,6 +2626,7 @@ def __init__( seq=0, ) self._latest_video_seq = -1 + self._latest_video_received_s: float | None = None self._active = False self._episode_started_s = 0.0 self._episode_frames = 0 @@ -2659,6 +2675,7 @@ def run(self) -> None: video = self._video_sub.recv_latest() if isinstance(video, SharedFrameDescriptor): self._handle_video(video) + self._discard_if_camera_stale() time.sleep(idle_sleep_s) finally: @@ -2706,6 +2723,9 @@ def _start_episode(self) -> None: record.recordable, ) return + if not self._camera_is_fresh(): + operator_logger.warning("cannot start recording: no fresh RealSense frame") + return self._recorder.start_episode() self._active = True self._episode_started_s = time.monotonic() @@ -2716,6 +2736,9 @@ def _save_episode(self) -> None: if not self._active: operator_logger.info("no active recording episode to save") return + if not self._camera_is_fresh(): + self._discard_episode("camera stream timeout") + return duration_s = time.monotonic() - self._episode_started_s if self._episode_frames <= 0: self._discard_episode("empty episode") @@ -2741,6 +2764,7 @@ def _handle_video(self, descriptor: SharedFrameDescriptor) -> None: if int(descriptor.seq) == self._latest_video_seq: return self._latest_video_seq = int(descriptor.seq) + self._latest_video_received_s = time.monotonic() if not self._active: return record = self._latest_record @@ -2780,6 +2804,19 @@ def _handle_video(self, descriptor: SharedFrameDescriptor) -> None: self._recorder.add_frame(**frame_kwargs) self._episode_frames += 1 + def _camera_is_fresh(self, *, now_s: float | None = None) -> bool: + if self._latest_video_received_s is None: + return False + now = time.monotonic() if now_s is None else float(now_s) + return now - self._latest_video_received_s <= self._CAMERA_TIMEOUT_S + + def _discard_if_camera_stale(self, *, now_s: float | None = None) -> bool: + if not self._active or self._camera_is_fresh(now_s=now_s): + return False + self._discard_episode("camera stream timeout") + return True + + def _run_recording_worker( cfg: dict[str, Any], endpoints: Sim2RealIpcEndpoints, diff --git a/tests/test_pico_video.py b/tests/test_pico_video.py index fb8e4fd4..0748f352 100644 --- a/tests/test_pico_video.py +++ b/tests/test_pico_video.py @@ -39,7 +39,7 @@ def test_pico_video_config_rejects_enabled_unknown_source() -> None: parse_pico_video_config({"video": {"enabled": True, "source": "webcam"}}) -def test_realsense_video_runtime_continues_after_frame_timeout(monkeypatch: pytest.MonkeyPatch) -> None: +def test_realsense_video_runtime_reconnects_after_frame_timeout(monkeypatch: pytest.MonkeyPatch) -> None: fake_rs = ModuleType("pyrealsense2") fake_rs.stream = SimpleNamespace(color="color") fake_rs.format = SimpleNamespace(rgb8="rgb8") @@ -59,17 +59,22 @@ class FakeFrames: def get_color_frame(self) -> FakeColorFrame: return FakeColorFrame() + pipeline_instances = 0 + wait_calls = 0 + class FakePipeline: def __init__(self) -> None: - self.calls = 0 + nonlocal pipeline_instances + pipeline_instances += 1 def start(self, _config: object) -> None: pass - def wait_for_frames(self) -> FakeFrames: - self.calls += 1 + def wait_for_frames(self, _timeout_ms: int) -> FakeFrames: + nonlocal wait_calls + wait_calls += 1 time.sleep(0.005) - if self.calls == 1: + if wait_calls == 1: raise RuntimeError("Frame didn't arrive within 5000") return FakeFrames() @@ -79,18 +84,31 @@ def stop(self) -> None: fake_rs.config = FakeConfig fake_rs.pipeline = FakePipeline monkeypatch.setitem(sys.modules, "pyrealsense2", fake_rs) + monkeypatch.setattr(pico_video._RealSenseVideoProducer, "_RECONNECT_DELAY_S", 0.005) sink = _FrameSink() - config = parse_pico_video_config({"video": {"enabled": True, "source": "realsense", "width": 3, "height": 2}}) - runtime = PicoVideoRuntime(provider=sink, config=config, mode="sim2real") + config = parse_pico_video_config( + { + "video": { + "enabled": True, + "source": "realsense", + "width": 3, + "height": 2, + } + } + ) + runtime = PicoVideoRuntime(provider=sink, config=config) runtime.start() - time.sleep(0.03) + deadline = time.monotonic() + 0.5 + while not sink.frames and time.monotonic() < deadline: + time.sleep(0.005) runtime.stop() assert sink.frames assert sink.frames[-1].shape == (2, 3, 3) assert sink.frames[-1].dtype == np.uint8 + assert pipeline_instances >= 2 def test_realsense_video_runtime_invokes_frame_callback(monkeypatch: pytest.MonkeyPatch) -> None: @@ -114,7 +132,7 @@ class FakePipeline: def start(self, _config: object) -> None: pass - def wait_for_frames(self) -> FakeFrames: + def wait_for_frames(self, _timeout_ms: int) -> FakeFrames: time.sleep(0.005) return FakeFrames() @@ -131,7 +149,6 @@ def stop(self) -> None: runtime = PicoVideoRuntime( provider=sink, config=config, - mode="sim2real", frame_callback=lambda frame, _timestamp_s: callback_frames.append(frame.copy()), ) @@ -165,7 +182,7 @@ def stop(self) -> None: sink = _FrameSink() config = parse_pico_video_config({"video": {"enabled": True, "source": "realsense"}}) - runtime = PicoVideoRuntime(provider=sink, config=config, mode="sim2real") + runtime = PicoVideoRuntime(provider=sink, config=config) runtime.start() @@ -173,7 +190,7 @@ def stop(self) -> None: assert sink.frames == [] -def test_video_runtime_stops_producer_before_reraising_tick_error(monkeypatch: pytest.MonkeyPatch) -> None: +def test_video_runtime_stops_producer_and_isolates_tick_error(monkeypatch: pytest.MonkeyPatch) -> None: stopped = False class FailingProducer: @@ -193,14 +210,11 @@ def stop(self) -> None: monkeypatch.setattr(pico_video, "_RealSenseVideoProducer", FailingProducer) sink = _FrameSink() - config = parse_pico_video_config( - {"video": {"enabled": True, "source": "realsense", "fail_on_error": True}} - ) - runtime = PicoVideoRuntime(provider=sink, config=config, mode="sim2real") + config = parse_pico_video_config({"video": {"enabled": True, "source": "realsense"}}) + runtime = PicoVideoRuntime(provider=sink, config=config) runtime.start() - with pytest.raises(RuntimeError, match="Pico video pipeline failed"): - runtime.tick() + runtime.tick() assert stopped is True @@ -226,12 +240,15 @@ def stop(self) -> None: fake_rs.config = FakeConfig fake_rs.pipeline = FakePipeline monkeypatch.setitem(sys.modules, "pyrealsense2", fake_rs) + monkeypatch.setattr(pico_video._RealSenseVideoProducer, "_STARTUP_WAIT_S", 0.02) + monkeypatch.setattr(pico_video._RealSenseVideoProducer, "_RECONNECT_DELAY_S", 0.005) sink = _FrameSink() config = parse_pico_video_config({"video": {"enabled": True, "source": "realsense"}}) - runtime = PicoVideoRuntime(provider=sink, config=config, mode="sim2real") + runtime = PicoVideoRuntime(provider=sink, config=config) runtime.start() + runtime.stop() assert stop_calls == 0 @@ -260,7 +277,7 @@ def close(self) -> None: sink = _FrameSink() robot = SimpleNamespace(model=object(), data=object()) config = parse_pico_video_config({"video": {"enabled": True, "source": "mujoco", "width": 4, "height": 3}}) - runtime = PicoVideoRuntime(provider=sink, config=config, mode="sim2sim", robot=robot) + runtime = PicoVideoRuntime(provider=sink, config=config, robot=robot) runtime.start() runtime.tick() diff --git a/tests/test_sim2real_multiprocess.py b/tests/test_sim2real_multiprocess.py index 825ca2c7..278553fd 100644 --- a/tests/test_sim2real_multiprocess.py +++ b/tests/test_sim2real_multiprocess.py @@ -5,6 +5,7 @@ import logging from pathlib import Path import shutil +import time from types import SimpleNamespace import h5py @@ -347,8 +348,11 @@ def Process(self, *, name: str, target: object, args: tuple[object, ...]) -> Fak assert started_names == ["pico_input", "reference", "robot_control", "neck_worker"] -@pytest.mark.parametrize("failure_stage", ["setup", "snapshot", "publish", "close"]) -def test_head_pose_ipc_failure_does_not_stop_pico_input(monkeypatch, failure_stage: str) -> None: +@pytest.mark.parametrize( + "failure_stage", + ["setup", "snapshot", "publish", "close", "video_start", "video_tick", "video_stop"], +) +def test_pico_auxiliary_failure_does_not_stop_pico_input(monkeypatch, failure_stage: str) -> None: endpoints = default_endpoints(base_port=39890) closed_publishers: list[str] = [] provider_closed = False @@ -403,12 +407,18 @@ def __init__(self, **_kwargs: object) -> None: return None def start(self) -> None: + if failure_stage == "video_start": + raise RuntimeError("video startup failed") return None def tick(self) -> None: + if failure_stage == "video_tick": + raise RuntimeError("video tick failed") return None def stop(self) -> None: + if failure_stage == "video_stop": + raise RuntimeError("video stop failed") return None class FakeSubscriber: @@ -504,7 +514,8 @@ def close(self) -> None: assert published_topics == (["neck_command"] if recording_enabled else []) -def test_noncritical_worker_exit_warning_is_not_repeated(monkeypatch, caplog) -> None: +@pytest.mark.parametrize("worker_name", ["neck_worker", "pico_input"]) +def test_noncritical_worker_exit_warning_is_not_repeated(monkeypatch, caplog, worker_name: str) -> None: class FakeStopEvent: def __init__(self) -> None: self.polls = 0 @@ -520,9 +531,11 @@ def set(self) -> None: self.stopped = True class FakeProcess: - name = "neck_worker" exitcode = 1 + def __init__(self) -> None: + self.name = worker_name + def is_alive(self) -> bool: return False @@ -542,7 +555,7 @@ def join(self, timeout: float | None = None) -> None: runtime.run() warnings = [message for message in caplog.messages if "non-critical worker exited" in message] - assert warnings == ["non-critical worker exited: neck_worker"] + assert warnings == [f"non-critical worker exited: {worker_name}; G1 control remains active"] def test_recording_key_mapping() -> None: @@ -1525,6 +1538,11 @@ def fake_factory(**_kwargs: object) -> FakeRecorder: seq=2, ) worker._start_episode() + assert calls == [] + + ready_desc = writer.write(np.full((2, 2, 3), 4, dtype=np.uint8), timestamp_s=2.0) + worker._handle_video(ready_desc) + worker._start_episode() worker._save_episode() assert calls == ["start", "discard"] @@ -1571,6 +1589,17 @@ def fake_factory(**_kwargs: object) -> FakeRecorder: worker._start_episode() worker._discard_episode("test") assert calls[-2:] == ["start", "discard"] + + worker._start_episode() + worker._latest_video_received_s = time.monotonic() - worker._CAMERA_TIMEOUT_S - 0.1 + assert worker._discard_if_camera_stale() is True + assert calls[-2:] == ["start", "discard"] + + worker._latest_video_received_s = time.monotonic() + worker._start_episode() + worker._latest_video_received_s = time.monotonic() - worker._CAMERA_TIMEOUT_S - 0.1 + worker._save_episode() + assert calls[-2:] == ["start", "discard"] finally: writer.close(unlink=True) worker._record_sub.close() From 68c14b7623a30b694d3bb9f191e038bad6cf9a0a Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Tue, 21 Jul 2026 16:31:56 +0800 Subject: [PATCH 32/59] Clip high-level policy neck targets --- AGENTS.md | 2 +- README.md | 4 ++- docs/docs/configuration/config-reference.md | 12 ++++--- .../tutorials/high-level-policy-sim2real.md | 11 ++++--- .../current/configuration/config-reference.md | 11 ++++--- .../tutorials/high-level-policy-sim2real.md | 10 +++--- teleopit/high_level_policy/scheduler.py | 26 ++++++---------- tests/test_high_level_policy.py | 31 ++++++++++++------- 8 files changed, 60 insertions(+), 47 deletions(-) diff --git a/AGENTS.md b/AGENTS.md index ae16523f..748e8d51 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -190,7 +190,7 @@ target_dof_pos = clip(action, -10, 10) × action_scale + default_dof_pos - High-level-policy formal robot modes are `IDLE`, `STANDING`, `POLICY`, and `DAMPING`. After Unitree remote `Y`, entry remains internal to `STANDING`: validate one candidate chunk, hold `action[0]` through the existing tracker for one Kp ramp, then create one new host session and require a fresh valid chunk before entering `POLICY`; the 50 Hz output limiter starts from the held `action[0]` reference rather than measured tracker joints, a failure aborts entry, and there is no `POLICY_STARTING` mode - Unitree remote controls are `Start -> STANDING`, `Y -> request POLICY`, `B -> pause/resume`, `X -> STANDING/cancel pending`, and `L1+R1 -> DAMPING` - Policy pause freezes the scheduler/body reference and holds the latest hand/neck command. Leaving `POLICY` opens LinkerHand and centers OpenNeck -- The onboard scheduler clips G1 joint references to `real_robot.joint_pos_lower/upper` when the required correction is at most `high_level_policy.safety.max_joint_projection_rad` (default `0.1` rad), then rejects whole chunks on excessive joint correction, shape/finiteness, session/sequence, quaternion, absolute root height, hand closure, OpenNeck degree range, or staleness failures. It accepts temporal root, yaw, and joint-reference discontinuities at entry, inside chunks, and across chunks because recorded pause/resume transitions may be discontinuous; accepted root translation, yaw, and joint output is rate-limited at 50 Hz. Never pad or trim malformed host output, and do not clip other out-of-range fields into validity +- The onboard scheduler clips G1 joint references to `real_robot.joint_pos_lower/upper` when the required correction is at most `high_level_policy.safety.max_joint_projection_rad` (default `0.1` rad), and clips OpenNeck yaw/pitch commands to their configured degree ranges. It rejects whole chunks on excessive joint correction, shape/finiteness, session/sequence, quaternion, absolute root height, hand closure, or staleness failures. It accepts temporal root, yaw, and joint-reference discontinuities at entry, inside chunks, and across chunks because recorded pause/resume transitions may be discontinuous; accepted root translation, yaw, and joint output is rate-limited at 50 Hz. Never pad or trim malformed host output, and do not clip other out-of-range fields into validity - A short cached-reference grace period is allowed; exhaustion, host/network failure, or loss of a required camera/client worker puts `POLICY` into the same resumable pause state used by remote `B`. The runtime never enters `STANDING` automatically; `B` resumes after a fresh valid chunk is available, while `X` remains the manual transition to `STANDING` - Initial production hardware support requires two LinkerHand O6 hands and OpenNeck because all 50 canonical action dimensions are active - OpenNeck 0.2.0 is the supported runtime; Teleopit sends physical degrees through `move_deg()`, and the direct-drive OpenNeck package converts degrees to servo steps and clips them to its calibrated mechanical limits; the removed normalized API and config fields are unsupported diff --git a/README.md b/README.md index 3cac00da..c9efa33d 100644 --- a/README.md +++ b/README.md @@ -164,7 +164,9 @@ Replay therefore restarts from its configured start frame and ACT recomputes from the post-ramp observation. The 50 Hz output limiter starts from the held reference, not the tracker's measured joint pose. Temporal reference jumps are accepted so recorded pause/resume transitions can be replayed, then rate-limited -on output. Entry failure returns to `STANDING`. +on output. OpenNeck yaw/pitch values are clipped to the configured degree ranges +before scheduling, so a neck-only overshoot does not reject the action chunk. +Entry failure returns to `STANDING`. Invalid/stale live chunks and watchdog expiry cannot block the local control loop and instead pause `POLICY` while holding the last reference. Host/network failure and loss of a required camera/client worker use the same ordinary pause diff --git a/docs/docs/configuration/config-reference.md b/docs/docs/configuration/config-reference.md index 0cdeed30..5802d510 100644 --- a/docs/docs/configuration/config-reference.md +++ b/docs/docs/configuration/config-reference.md @@ -136,17 +136,19 @@ protocol tests. The only shared data file is `hand_calibration.json`. | `high_level_policy.safety.max_yaw_rate_rad_s` | Root yaw-rate limit applied to the 50 Hz scheduler output | `2.5` | | `high_level_policy.safety.max_joint_rate_rad_s` | Per-joint rate limit applied to the 50 Hz scheduler output | `10.0` | | `high_level_policy.safety.max_joint_projection_rad` | Maximum correction allowed when clipping a G1 joint reference to its position limit | `0.1` | -| `high_level_policy.safety.neck_yaw_min_deg` / `neck_yaw_max_deg` | Accepted OpenNeck yaw command range | `-45` / `45` | -| `high_level_policy.safety.neck_pitch_min_deg` / `neck_pitch_max_deg` | Accepted OpenNeck pitch command range | `-40` / `40` | +| `high_level_policy.safety.neck_yaw_min_deg` / `neck_yaw_max_deg` | OpenNeck yaw clipping range | `-45` / `45` | +| `high_level_policy.safety.neck_pitch_min_deg` / `neck_pitch_max_deg` | OpenNeck pitch clipping range | `-40` / `40` | G1 reference joint positions are clipped to `real_robot.joint_pos_lower/upper` when the required correction does not exceed `high_level_policy.safety.max_joint_projection_rad`; larger corrections reject -the chunk. The initial runtime requires +the chunk. OpenNeck yaw/pitch values are clipped to their configured ranges and +do not reject a chunk solely because of neck overshoot. The initial runtime +requires `hands.driver=linkerhand_o6`, both hand sides, and `neck.driver=openneck` because all canonical 50D action fields are active. OpenNeck policy values go directly -to `move_deg(yaw, pitch)` after chunk validation; Pico dead-zone and pitch-gain -mapping are not applied. +to `move_deg(yaw, pitch)` after onboard clipping and chunk validation; Pico +dead-zone and pitch-gain mapping are not applied. ### Real Robot diff --git a/docs/docs/tutorials/high-level-policy-sim2real.md b/docs/docs/tutorials/high-level-policy-sim2real.md index 2c16d574..78936307 100644 --- a/docs/docs/tutorials/high-level-policy-sim2real.md +++ b/docs/docs/tutorials/high-level-policy-sim2real.md @@ -166,7 +166,8 @@ enters `STANDING` automatically; `X` remains the manual transition. ## 6. Onboard Validation and Watchdog Teleopit clips a G1 joint reference to the configured real-robot position -limits when the correction is at most `max_joint_projection_rad`, then rejects +limits when the correction is at most `max_joint_projection_rad`, and clips +OpenNeck yaw/pitch commands to their configured degree ranges. It then rejects a complete chunk if any frame violates the remaining contract. It never pads or trims a malformed host result. Checks include: @@ -175,7 +176,8 @@ or trims a malformed host result. Checks include: - absolute root-height limits; - G1 joint-position clipping to `real_robot.joint_pos_lower/upper`, with larger corrections rejected; -- LinkerHand closure `[0,1]` and configured OpenNeck degree ranges; +- LinkerHand closure `[0,1]`; +- OpenNeck yaw/pitch clipping to the configured degree ranges; - observation/result age, source timestamp, and action horizon. Reference continuity is not an acceptance condition. Root translation, root @@ -184,8 +186,9 @@ across chunks because a recorded pause/resume transition can intentionally be discontinuous. Host requests are paused for one Kp ramp. A new host session then supplies the fresh chunk that will actually enter `POLICY`; the candidate chunk is never continued as a live timeline. A malformed or stale chunk, an -out-of-range non-joint field, or an excessive joint correction in a fresh chunk -aborts entry instead of starting another alignment cycle. +out-of-range non-joint field other than the projected OpenNeck angles, or an +excessive joint correction in a fresh chunk aborts entry instead of starting +another alignment cycle. Validated 30 Hz body references are interpolated and rate-limited locally at 50 Hz, including when latency skips source frames or a new chunk replaces the diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md index c6aeecef..931bee8a 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md @@ -154,15 +154,16 @@ client/server 消息结构与协议测试。唯一共享的数据文件是 `hand | `high_level_policy.safety.max_yaw_rate_rad_s` | 应用于 50 Hz scheduler 输出的 root yaw rate 限制 | `2.5` | | `high_level_policy.safety.max_joint_rate_rad_s` | 应用于 50 Hz scheduler 输出的单关节 rate 限制 | `10.0` | | `high_level_policy.safety.max_joint_projection_rad` | 将 G1 关节 reference 裁剪到位置限位时允许的最大修正量 | `0.1` | -| `high_level_policy.safety.neck_yaw_min_deg` / `neck_yaw_max_deg` | 可接受的 OpenNeck yaw 命令范围 | `-45` / `45` | -| `high_level_policy.safety.neck_pitch_min_deg` / `neck_pitch_max_deg` | 可接受的 OpenNeck pitch 命令范围 | `-40` / `40` | +| `high_level_policy.safety.neck_yaw_min_deg` / `neck_yaw_max_deg` | OpenNeck yaw 裁剪范围 | `-45` / `45` | +| `high_level_policy.safety.neck_pitch_min_deg` / `neck_pitch_max_deg` | OpenNeck pitch 裁剪范围 | `-40` / `40` | 当所需修正量不超过 `high_level_policy.safety.max_joint_projection_rad` 时，G1 reference -joint position 会裁剪到 `real_robot.joint_pos_lower/upper`；更大的修正量会导致 chunk 被拒绝。由于 canonical +joint position 会裁剪到 `real_robot.joint_pos_lower/upper`；更大的修正量会导致 chunk 被拒绝。 +OpenNeck yaw/pitch 会裁剪到配置范围，单纯的 neck 越界不会导致 chunk 被拒绝。由于 canonical 50D action 的所有字段都处于启用状态，初始运行时要求 `hands.driver=linkerhand_o6`、左右两只手以及 `neck.driver=openneck`。OpenNeck 策略值 -在 chunk 验证后直接发送给 `move_deg(yaw, pitch)`；不会应用 Pico dead-zone 或 pitch-gain -映射。 +在 onboard 裁剪并完成 chunk 验证后直接发送给 `move_deg(yaw, pitch)`；不会应用 Pico +dead-zone 或 pitch-gain 映射。 ### 真机 SDK diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md index 2cb2377c..7eb3271d 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md @@ -152,21 +152,23 @@ Watchdog、主机/网络、相机或 policy client 故障也会进入同一个 ## 6. Onboard 验证与 Watchdog 当修正量不超过 `max_joint_projection_rad` 时，Teleopit 会先把 G1 关节 reference 裁剪到 -配置的真机关节位置范围；如果任一帧违反其余契约，则拒绝整个 chunk。它不会对错误的 -主机结果进行补齐或删减。检查包括： +配置的真机关节位置范围，并把 OpenNeck yaw/pitch 命令裁剪到配置的角度范围；如果任一帧 +违反其余契约，则拒绝整个 chunk。它不会对错误的主机结果进行补齐或删减。检查包括： - 精确且有限的 `float32[T,50]`、当前 session，以及递增的 source sequence； - 归一化 root quaternion 与时间连续的符号； - 绝对 root 高度限制； - 按 `real_robot.joint_pos_lower/upper` 裁剪 G1 关节位置，并拒绝更大的修正量； -- LinkerHand closure `[0,1]` 和配置的 OpenNeck 角度范围； +- LinkerHand closure `[0,1]`； +- 将 OpenNeck yaw/pitch 裁剪到配置的角度范围； - observation/result 时效、source timestamp 和 action horizon。 reference 连续性不是接收条件。entry、chunk 内部和 chunk 之间的 root translation、root yaw 与 G1 关节 reference 跳变都会被接受，因为录制的 pause/resume 转换可能有意地不 连续。一次 Kp ramp 期间会暂停主机请求，随后新 host session 会提供真正进入 `POLICY` 的新鲜 chunk；候选 chunk 绝不会作为实时 timeline 继续播放。格式错误、过期、非关节字段 -超出绝对范围或关节修正量过大的新鲜 chunk 会终止 entry，而不会开始另一轮对齐。 +超出绝对范围（已裁剪的 OpenNeck 角度除外）或关节修正量过大的新鲜 chunk 会终止 +entry，而不会开始另一轮对齐。 通过验证的 30 Hz body reference 会在本地插值到 50 Hz 并执行 rate limit；网络延迟 导致跳过 source frame 或新 chunk 替换旧计划时同样如此。配置的 root displacement/XY diff --git a/teleopit/high_level_policy/scheduler.py b/teleopit/high_level_policy/scheduler.py index 67e16fe3..4c2ddc62 100644 --- a/teleopit/high_level_policy/scheduler.py +++ b/teleopit/high_level_policy/scheduler.py @@ -385,6 +385,16 @@ def _validate_actions( f"{float(correction[frame, joint]):.6g} rad" ) validated[:, 7:36] = projected + validated[:, 48] = np.clip( + validated[:, 48], + safety.neck_yaw_min_deg, + safety.neck_yaw_max_deg, + ) + validated[:, 49] = np.clip( + validated[:, 49], + safety.neck_pitch_min_deg, + safety.neck_pitch_max_deg, + ) self._validate_safety_limits(validated, safety=safety) return validated @@ -429,22 +439,6 @@ def _validate_safety_limits( f"range=[{float(lower[joint]):.6g}, {float(upper[joint]):.6g}]" ) - yaw = actions[:, 48] - pitch = actions[:, 49] - if float(np.min(yaw)) < safety.neck_yaw_min_deg or float(np.max(yaw)) > safety.neck_yaw_max_deg: - raise ValueError( - "High-level policy OpenNeck yaw is outside " - f"[{safety.neck_yaw_min_deg}, {safety.neck_yaw_max_deg}] degrees" - ) - if ( - float(np.min(pitch)) < safety.neck_pitch_min_deg - or float(np.max(pitch)) > safety.neck_pitch_max_deg - ): - raise ValueError( - "High-level policy OpenNeck pitch is outside " - f"[{safety.neck_pitch_min_deg}, {safety.neck_pitch_max_deg}] degrees" - ) - def closure_to_o6_pose( closure: object, diff --git a/tests/test_high_level_policy.py b/tests/test_high_level_policy.py index 3151aa26..6479cd42 100644 --- a/tests/test_high_level_policy.py +++ b/tests/test_high_level_policy.py @@ -253,6 +253,23 @@ def test_scheduler_clips_joint_positions_to_onboard_limits() -> None: assert first_action[8] == pytest.approx(3.0) +def test_scheduler_clips_openneck_angles_to_onboard_limits() -> None: + scheduler = HighLevelPolicyScheduler(hold_s=0.1, safety=_safety_config()) + scheduler.reset("session-1", initial_action=_safe_actions(1)[0]) + actions = _safe_actions() + actions[:, 48] = [-46.0, 0.0, 46.0] + actions[:, 49] = [41.0, 0.0, -41.0] + + first_action = scheduler.accept_entry(_safe_chunk(actions), now_s=1.01) + final_action = scheduler.sample(1.0 + 2.0 / 30.0) + + assert first_action[48] == pytest.approx(-45.0) + assert first_action[49] == pytest.approx(40.0) + assert final_action is not None + assert final_action[48] == pytest.approx(45.0) + assert final_action[49] == pytest.approx(-40.0) + + def test_scheduler_rejects_joint_projection_above_limit() -> None: scheduler = HighLevelPolicyScheduler(hold_s=0.1, safety=_safety_config()) scheduler.reset("session-1", initial_action=_safe_actions(1)[0]) @@ -264,21 +281,13 @@ def test_scheduler_rejects_joint_projection_above_limit() -> None: assert not scheduler.has_chunk -@pytest.mark.parametrize( - ("mutate", "message"), - [ - (lambda value: value.__setitem__((1, 2), 0.4), "root height"), - (lambda value: value.__setitem__((1, 48), 46.0), "OpenNeck yaw"), - (lambda value: value.__setitem__((1, 49), -41.0), "OpenNeck pitch"), - ], -) -def test_scheduler_rejects_entire_unsafe_non_joint_chunk(mutate, message: str) -> None: # type: ignore[no-untyped-def] +def test_scheduler_rejects_entire_unsafe_non_joint_chunk() -> None: scheduler = HighLevelPolicyScheduler(hold_s=0.1, safety=_safety_config()) scheduler.reset("session-1", initial_action=_safe_actions(1)[0]) actions = _safe_actions() - mutate(actions) + actions[1, 2] = 0.4 - with pytest.raises(ValueError, match=message): + with pytest.raises(ValueError, match="root height"): scheduler.accept(_safe_chunk(actions), now_s=1.01) assert not scheduler.has_chunk From 03ce49ac0210a2595e1e7ab860e11865eb089ec0 Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Tue, 21 Jul 2026 16:50:22 +0800 Subject: [PATCH 33/59] Relax high-level policy action watchdog --- AGENTS.md | 2 +- README.md | 4 +++- docs/docs/configuration/config-reference.md | 2 +- docs/docs/tutorials/high-level-policy-sim2real.md | 5 +++-- .../current/configuration/config-reference.md | 2 +- .../current/tutorials/high-level-policy-sim2real.md | 4 ++-- teleopit/configs/high_level_policy_sim2real.yaml | 2 +- teleopit/high_level_policy/config.py | 2 +- teleopit/high_level_policy/scheduler.py | 2 +- tests/test_high_level_policy.py | 4 ++-- 10 files changed, 16 insertions(+), 13 deletions(-) diff --git a/AGENTS.md b/AGENTS.md index 748e8d51..dd21fdcc 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -191,7 +191,7 @@ target_dof_pos = clip(action, -10, 10) × action_scale + default_dof_pos - Unitree remote controls are `Start -> STANDING`, `Y -> request POLICY`, `B -> pause/resume`, `X -> STANDING/cancel pending`, and `L1+R1 -> DAMPING` - Policy pause freezes the scheduler/body reference and holds the latest hand/neck command. Leaving `POLICY` opens LinkerHand and centers OpenNeck - The onboard scheduler clips G1 joint references to `real_robot.joint_pos_lower/upper` when the required correction is at most `high_level_policy.safety.max_joint_projection_rad` (default `0.1` rad), and clips OpenNeck yaw/pitch commands to their configured degree ranges. It rejects whole chunks on excessive joint correction, shape/finiteness, session/sequence, quaternion, absolute root height, hand closure, or staleness failures. It accepts temporal root, yaw, and joint-reference discontinuities at entry, inside chunks, and across chunks because recorded pause/resume transitions may be discontinuous; accepted root translation, yaw, and joint output is rate-limited at 50 Hz. Never pad or trim malformed host output, and do not clip other out-of-range fields into validity -- A short cached-reference grace period is allowed; exhaustion, host/network failure, or loss of a required camera/client worker puts `POLICY` into the same resumable pause state used by remote `B`. The runtime never enters `STANDING` automatically; `B` resumes after a fresh valid chunk is available, while `X` remains the manual transition to `STANDING` +- A configured cached-reference grace period (`high_level_policy.hold_s`, default `3.0` seconds) covers transient host inference and transport delays; exhaustion, host/network failure, or loss of a required camera/client worker puts `POLICY` into the same resumable pause state used by remote `B`. The runtime never enters `STANDING` automatically; `B` resumes after a fresh valid chunk is available, while `X` remains the manual transition to `STANDING` - Initial production hardware support requires two LinkerHand O6 hands and OpenNeck because all 50 canonical action dimensions are active - OpenNeck 0.2.0 is the supported runtime; Teleopit sends physical degrees through `move_deg()`, and the direct-drive OpenNeck package converts degrees to servo steps and clips them to its calibrated mechanical limits; the removed normalized API and config fields are unsupported - OpenNeck maps the independent HMD `PicoFrame.head.rotation` relative to the same-frame full-body `Body.Spine3` orientation; it must never use the full-body `Body.Head` skeleton joint for neck control, and HMD pose updates must remain independent of duplicate-body-frame filtering diff --git a/README.md b/README.md index c9efa33d..0fba2622 100644 --- a/README.md +++ b/README.md @@ -168,7 +168,9 @@ on output. OpenNeck yaw/pitch values are clipped to the configured degree ranges before scheduling, so a neck-only overshoot does not reject the action chunk. Entry failure returns to `STANDING`. Invalid/stale live chunks and watchdog expiry cannot block the local control -loop and instead pause `POLICY` while holding the last reference. Host/network +loop and instead pause `POLICY` while holding the last reference. The default +three-second cached-reference grace period covers transient host inference and +transport delays before watchdog expiry. Host/network failure and loss of a required camera/client worker use the same ordinary pause state as remote `B`; after recovery, press `B` to resume on a fresh valid chunk. Only `X` returns active `POLICY` to `STANDING`. diff --git a/docs/docs/configuration/config-reference.md b/docs/docs/configuration/config-reference.md index 5802d510..2994ab6a 100644 --- a/docs/docs/configuration/config-reference.md +++ b/docs/docs/configuration/config-reference.md @@ -129,7 +129,7 @@ protocol tests. The only shared data file is `hand_calibration.json`. | `high_level_policy.max_observation_age_s` | Maximum camera/observation age before a request is skipped | `0.15` | | `high_level_policy.max_result_age_s` | Maximum local IPC age before a received result is rejected | `0.1` | | `high_level_policy.entry_timeout_s` | Maximum total entry duration (candidate, Kp ramp, fresh session) and maximum fresh-chunk wait on resume | `5.0` | -| `high_level_policy.hold_s` | Final validated-reference grace period before the watchdog pauses `POLICY` | `0.5` | +| `high_level_policy.hold_s` | Final validated-reference grace period before the watchdog pauses `POLICY`; covers transient host inference and transport delays | `3.0` | | `high_level_policy.safety.root_height_min_m` / `root_height_max_m` | Accepted absolute root-height range | `0.55` / `1.05` | | `high_level_policy.safety.max_root_xy_speed_m_s` | Root XY speed limit applied to the 50 Hz scheduler output | `2.5` | | `high_level_policy.safety.max_root_displacement_m` | Source-frame-equivalent 3D root step used by the 50 Hz output limiter | `0.1` | diff --git a/docs/docs/tutorials/high-level-policy-sim2real.md b/docs/docs/tutorials/high-level-policy-sim2real.md index 78936307..5bfd22ee 100644 --- a/docs/docs/tutorials/high-level-policy-sim2real.md +++ b/docs/docs/tutorials/high-level-policy-sim2real.md @@ -193,8 +193,9 @@ another alignment cycle. Validated 30 Hz body references are interpolated and rate-limited locally at 50 Hz, including when latency skips source frames or a new chunk replaces the old plan. The configured root displacement/XY speed, yaw-rate, and joint-rate -values are output limits, not chunk-rejection thresholds. A short configured -grace period can reuse the final validated reference during an inference delay. +values are output limits, not chunk-rejection thresholds. The configured +grace period (three seconds by default) reuses the final validated reference +during transient inference or transport delays. If no valid action remains, a network exchange fails, or a required camera/client worker exits, Teleopit remains in `POLICY`, enters the normal resumable pause state, and holds the latest body, diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md index 931bee8a..6e7f61c6 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md @@ -147,7 +147,7 @@ client/server 消息结构与协议测试。唯一共享的数据文件是 `hand | `high_level_policy.max_observation_age_s` | 跳过请求前允许的最大 camera/observation age | `0.15` | | `high_level_policy.max_result_age_s` | 拒绝已接收结果前允许的最大本地 IPC age | `0.1` | | `high_level_policy.entry_timeout_s` | 候选请求、Kp ramp 和新 session 的 entry 最长总时间，以及恢复时等待新鲜 chunk 的最长时间 | `5.0` | -| `high_level_policy.hold_s` | Watchdog 暂停 `POLICY` 前，最后一条有效 reference 的 grace period | `0.5` | +| `high_level_policy.hold_s` | Watchdog 暂停 `POLICY` 前，最后一条有效 reference 的 grace period；用于容忍短暂的主机推理和传输延迟 | `3.0` | | `high_level_policy.safety.root_height_min_m` / `root_height_max_m` | 可接受的绝对 root 高度范围 | `0.55` / `1.05` | | `high_level_policy.safety.max_root_xy_speed_m_s` | 应用于 50 Hz scheduler 输出的 root XY 速度限制 | `2.5` | | `high_level_policy.safety.max_root_displacement_m` | 50 Hz 输出 rate limiter 使用的 source-frame 等效 3D root 步长 | `0.1` | diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md index 7eb3271d..035ae1c4 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md @@ -172,8 +172,8 @@ entry，而不会开始另一轮对齐。 通过验证的 30 Hz body reference 会在本地插值到 50 Hz 并执行 rate limit；网络延迟 导致跳过 source frame 或新 chunk 替换旧计划时同样如此。配置的 root displacement/XY -speed、yaw rate 和 joint rate 是输出限制，而不是 chunk 拒绝阈值。在短暂推理延迟期间， -可以在配置的短 grace period 内继续使用最后一条已验证 reference。如果不再有有效 action， +speed、yaw rate 和 joint rate 是输出限制，而不是 chunk 拒绝阈值。在短暂的推理或传输延迟 +期间，可以在配置的 grace period（默认三秒）内继续使用最后一条已验证 reference。如果不再有有效 action， 网络交换失败，或必要的 camera/client worker 退出，Teleopit 会保持在 `POLICY`，进入 普通的可恢复暂停状态，并保持最后一条 body、hand 和 neck 命令。故障恢复后按 `B` 请求恢复；在收到新的有效 chunk 前，执行仍保持暂停。只有 `X` 会把模式切换到 diff --git a/teleopit/configs/high_level_policy_sim2real.yaml b/teleopit/configs/high_level_policy_sim2real.yaml index a1893389..78e551db 100644 --- a/teleopit/configs/high_level_policy_sim2real.yaml +++ b/teleopit/configs/high_level_policy_sim2real.yaml @@ -28,7 +28,7 @@ high_level_policy: max_observation_age_s: 0.15 max_result_age_s: 0.1 entry_timeout_s: 5.0 - hold_s: 0.5 + hold_s: 3.0 safety: root_height_min_m: 0.55 root_height_max_m: 1.05 diff --git a/teleopit/high_level_policy/config.py b/teleopit/high_level_policy/config.py index fafd36b2..f6c9accf 100644 --- a/teleopit/high_level_policy/config.py +++ b/teleopit/high_level_policy/config.py @@ -78,7 +78,7 @@ def parse_high_level_policy_config(cfg: Any) -> HighLevelPolicyConfig: entry_timeout_s = _positive_float( cfg_get(policy_cfg, "entry_timeout_s", 5.0), "entry_timeout_s" ) - hold_s = float(cfg_get(policy_cfg, "hold_s", 0.5)) + hold_s = float(cfg_get(policy_cfg, "hold_s", 3.0)) if not math.isfinite(hold_s) or hold_s < 0.0: raise ValueError("high_level_policy.hold_s must be finite and >= 0") return HighLevelPolicyConfig( diff --git a/teleopit/high_level_policy/scheduler.py b/teleopit/high_level_policy/scheduler.py index 4c2ddc62..71921f78 100644 --- a/teleopit/high_level_policy/scheduler.py +++ b/teleopit/high_level_policy/scheduler.py @@ -109,7 +109,7 @@ class HighLevelPolicyScheduler: def __init__( self, *, - hold_s: float = 0.5, + hold_s: float = 3.0, safety: HighLevelPolicySafetyConfig | None = None, output_hz: float = 50.0, ) -> None: diff --git a/tests/test_high_level_policy.py b/tests/test_high_level_policy.py index 6479cd42..61a6bc17 100644 --- a/tests/test_high_level_policy.py +++ b/tests/test_high_level_policy.py @@ -109,10 +109,10 @@ def _safe_chunk(actions: np.ndarray, *, source_s: float = 1.0, sequence: int = 0 ) -def test_high_level_policy_default_hold_covers_transport_jitter() -> None: +def test_high_level_policy_default_hold_covers_inference_and_transport_jitter() -> None: config = parse_high_level_policy_config({"high_level_policy": {"task": "demo"}}) - assert config.hold_s == pytest.approx(0.5) + assert config.hold_s == pytest.approx(3.0) def test_packaged_hand_calibration_loads() -> None: From 54c6ff81eff828a2f7cd62ddd42879f3cc5d1baa Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Tue, 21 Jul 2026 18:11:52 +0800 Subject: [PATCH 34/59] Support 50-frame policy action horizons --- AGENTS.md | 2 +- README.md | 4 +-- .../tutorials/high-level-policy-sim2real.md | 7 ++-- .../tutorials/high-level-policy-sim2real.md | 5 +-- teleopit/high_level_policy/config.py | 8 +++-- teleopit/high_level_policy/protocol.py | 2 +- teleopit/high_level_policy/scheduler.py | 12 +++++-- tests/test_high_level_policy.py | 32 +++++++++++++++++++ 8 files changed, 59 insertions(+), 13 deletions(-) diff --git a/AGENTS.md b/AGENTS.md index dd21fdcc..5e044cec 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -185,7 +185,7 @@ target_dof_pos = clip(action, -10, 10) × action_scale + default_dof_pos - The only shared data file is `hand_calibration.json`, which contains the LinkerHand O6 open/close calibration and must stay identical in both repositories - The host boundary uses ZeroMQ REQ/REP with msgpack and non-pickle float32 arrays. The onboard network client runs in a non-critical worker and never blocks the 50 Hz robot loop - Policy observation is RGB JPEG plus `observation.state(68)`; only `state[58:62]` is rotated into the session-local yaw frame -- Canonical action is `float32[T,50]`: local root `xyz(3)` + local root quaternion `wxyz(4)` + G1 joint reference `29` + left/right O6 closure `12` + OpenNeck yaw/pitch degrees `2` +- Canonical action is `float32[T,50]` with protocol horizon `T` in `[1,50]`: local root `xyz(3)` + local root quaternion `wxyz(4)` + G1 joint reference `29` + left/right O6 closure `12` + OpenNeck yaw/pitch degrees `2` - The body reference `[0:36]` is yaw/XY-delocalized once and sent through the existing motion tracker. It is never sent directly as a G1 motor command and must not pass through the mocap alignment a second time - High-level-policy formal robot modes are `IDLE`, `STANDING`, `POLICY`, and `DAMPING`. After Unitree remote `Y`, entry remains internal to `STANDING`: validate one candidate chunk, hold `action[0]` through the existing tracker for one Kp ramp, then create one new host session and require a fresh valid chunk before entering `POLICY`; the 50 Hz output limiter starts from the held `action[0]` reference rather than measured tracker joints, a failure aborts entry, and there is no `POLICY_STARTING` mode - Unitree remote controls are `Start -> STANDING`, `Y -> request POLICY`, `B -> pause/resume`, `X -> STANDING/cancel pending`, and `L1+R1 -> DAMPING` diff --git a/README.md b/README.md index 0fba2622..c2e10d3c 100644 --- a/README.md +++ b/README.md @@ -180,8 +180,8 @@ structure. During active development, Teleopit and `lerobot-teleopit` must be updated together. Their only shared data file is `hand_calibration.json`, which contains the LinkerHand O6 open/close calibration. See the [host-policy deployment tutorial](https://BotRunner64.github.io/Teleopit/tutorials/high-level-policy-sim2real) -for the 68D observation, 50D action layout, safety envelope, host startup, and -operator procedure. +for the 68D observation, 50D action layout, supported 1-to-50-frame action +horizon, safety envelope, host startup, and operator procedure. ## OpenNeck Active Vision diff --git a/docs/docs/tutorials/high-level-policy-sim2real.md b/docs/docs/tutorials/high-level-policy-sim2real.md index 5bfd22ee..e08f3a66 100644 --- a/docs/docs/tutorials/high-level-policy-sim2real.md +++ b/docs/docs/tutorials/high-level-policy-sim2real.md @@ -115,9 +115,10 @@ python scripts/run/run_high_level_policy_sim2real.py \ real_robot.network_interface=eth0 ``` -Use `high_level_policy.replan_steps=15` for a 15-frame ReplayPolicy chunk. The -initial ACT setup uses `replan_steps=3`. The value must not exceed the horizon -reported by the host. +The protocol accepts action chunks from 1 to 50 frames. The production ACT +checkpoint uses a 50-frame horizon with `high_level_policy.replan_steps=3`. +For a 15-frame ReplayPolicy chunk, `replan_steps=15` remains valid. The request +stride must not exceed the horizon reported by the host. The production camera contract is exactly RGB `uint8[480,640,3]` at 30 Hz. `camera.source=test-pattern` exists only for controlled integration testing; diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md index 035ae1c4..7c75ce05 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md @@ -108,8 +108,9 @@ python scripts/run/run_high_level_policy_sim2real.py \ real_robot.network_interface=eth0 ``` -对于 15 帧 ReplayPolicy chunk，使用 `high_level_policy.replan_steps=15`。初始 ACT -配置使用 `replan_steps=3`。该值不能超过主机报告的 horizon。 +协议接受 1 到 50 帧的 action chunk。正式 ACT checkpoint 使用 50 帧 horizon，并配置 +`high_level_policy.replan_steps=3`。对于 15 帧 ReplayPolicy chunk，仍可使用 +`replan_steps=15`。请求步长不能超过主机报告的 horizon。 生产相机契约固定为 30 Hz 的 RGB `uint8[480,640,3]`。 `camera.source=test-pattern` 只用于受控集成测试；部署时应使用 diff --git a/teleopit/high_level_policy/config.py b/teleopit/high_level_policy/config.py index f6c9accf..79c89fcd 100644 --- a/teleopit/high_level_policy/config.py +++ b/teleopit/high_level_policy/config.py @@ -6,6 +6,7 @@ import numpy as np +from teleopit.high_level_policy.protocol import MAX_ACTION_HORIZON from teleopit.runtime.common import cfg_get @@ -64,8 +65,11 @@ def parse_high_level_policy_config(cfg: Any) -> HighLevelPolicyConfig: cfg_get(policy_cfg, "reconnect_backoff_s", 1.0), "reconnect_backoff_s" ) replan_steps = int(cfg_get(policy_cfg, "replan_steps", 3)) - if not 1 <= replan_steps <= 15: - raise ValueError("high_level_policy.replan_steps must be in [1, 15]") + if not 1 <= replan_steps <= MAX_ACTION_HORIZON: + raise ValueError( + "high_level_policy.replan_steps must be in " + f"[1, {MAX_ACTION_HORIZON}]" + ) jpeg_quality = int(cfg_get(policy_cfg, "jpeg_quality", 90)) if not 1 <= jpeg_quality <= 100: raise ValueError("high_level_policy.jpeg_quality must be in [1, 100]") diff --git a/teleopit/high_level_policy/protocol.py b/teleopit/high_level_policy/protocol.py index 3f8813ff..32801787 100644 --- a/teleopit/high_level_policy/protocol.py +++ b/teleopit/high_level_policy/protocol.py @@ -14,7 +14,7 @@ MAX_IMAGE_BYTES = 1_572_864 MAX_TASK_UTF8_BYTES = 1_024 MAX_SESSION_ID_UTF8_BYTES = 128 -MAX_ACTION_HORIZON = 15 +MAX_ACTION_HORIZON = 50 class PolicyProtocolError(ValueError): diff --git a/teleopit/high_level_policy/scheduler.py b/teleopit/high_level_policy/scheduler.py index 71921f78..6cffb0e6 100644 --- a/teleopit/high_level_policy/scheduler.py +++ b/teleopit/high_level_policy/scheduler.py @@ -10,6 +10,7 @@ from teleopit.high_level_policy.client import PolicyActionChunk from teleopit.high_level_policy.config import HighLevelPolicySafetyConfig from teleopit.high_level_policy.hand_calibration import HandCalibration +from teleopit.high_level_policy.protocol import MAX_ACTION_HORIZON from teleopit.math_utils import quat_inv_np, quat_mul_np from teleopit.sim.reference_motion import interpolate_retarget_qpos @@ -349,8 +350,15 @@ def _validate_actions( values: object, ) -> np.ndarray: actions = np.asarray(values) - if actions.ndim != 2 or actions.shape[1] != ACTION_DIM or not 1 <= len(actions) <= 15: - raise ValueError(f"High-level policy actions must have shape [T, {ACTION_DIM}] with T in [1, 15]") + if ( + actions.ndim != 2 + or actions.shape[1] != ACTION_DIM + or not 1 <= len(actions) <= MAX_ACTION_HORIZON + ): + raise ValueError( + f"High-level policy actions must have shape [T, {ACTION_DIM}] " + f"with T in [1, {MAX_ACTION_HORIZON}]" + ) if not np.issubdtype(actions.dtype, np.number) or not np.all(np.isfinite(actions)): raise ValueError("High-level policy actions must be finite numeric values") validated = np.ascontiguousarray(actions, dtype=np.float32) diff --git a/tests/test_high_level_policy.py b/tests/test_high_level_policy.py index 61a6bc17..5709bac7 100644 --- a/tests/test_high_level_policy.py +++ b/tests/test_high_level_policy.py @@ -16,6 +16,7 @@ ) from teleopit.high_level_policy.hand_calibration import HandCalibration from teleopit.high_level_policy.protocol import ( + MAX_ACTION_HORIZON, MAX_REQUEST_BYTES, MAX_RESPONSE_BYTES, PolicyProtocolError, @@ -115,6 +116,29 @@ def test_high_level_policy_default_hold_covers_inference_and_transport_jitter() assert config.hold_s == pytest.approx(3.0) +def test_high_level_policy_replan_steps_uses_protocol_horizon_limit() -> None: + config = parse_high_level_policy_config( + { + "high_level_policy": { + "task": "demo", + "replan_steps": MAX_ACTION_HORIZON, + } + } + ) + + assert config.replan_steps == MAX_ACTION_HORIZON + + with pytest.raises(ValueError, match=rf"\[1, {MAX_ACTION_HORIZON}\]"): + parse_high_level_policy_config( + { + "high_level_policy": { + "task": "demo", + "replan_steps": MAX_ACTION_HORIZON + 1, + } + } + ) + + def test_packaged_hand_calibration_loads() -> None: calibration = HandCalibration.load() @@ -169,6 +193,14 @@ def test_scheduler_uses_source_timestamp_and_interpolates_at_30hz() -> None: assert halfway[48] == pytest.approx(5.0) +def test_scheduler_accepts_protocol_max_action_horizon() -> None: + scheduler = HighLevelPolicyScheduler(hold_s=0.1) + scheduler.reset("session-1") + scheduler.accept(_chunk(source_s=10.0, frames=MAX_ACTION_HORIZON), now_s=10.01) + + assert scheduler.has_chunk + + def test_scheduler_pause_freezes_and_resume_shifts_plan_time() -> None: scheduler = HighLevelPolicyScheduler(hold_s=0.1) scheduler.reset("session-1") From 296553d78d300c9750c73cf3ac5cfb5f4c0045ac Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Wed, 22 Jul 2026 15:10:50 +0800 Subject: [PATCH 35/59] Add high-level policy camera diagnostics --- .../sim2real/mp/high_level_policy_runtime.py | 64 ++++++++- teleopit/sim2real/mp/runtime.py | 121 +++++++++++++++++- tests/test_high_level_policy.py | 70 ++++++++++ 3 files changed, 250 insertions(+), 5 deletions(-) diff --git a/teleopit/sim2real/mp/high_level_policy_runtime.py b/teleopit/sim2real/mp/high_level_policy_runtime.py index 16bcf30a..e50b8414 100644 --- a/teleopit/sim2real/mp/high_level_policy_runtime.py +++ b/teleopit/sim2real/mp/high_level_policy_runtime.py @@ -60,6 +60,57 @@ operator_logger = logging.getLogger(OPERATOR_LOGGER_NAME) +class _CameraFrameDiagnostics: + """Rate-limited acquisition diagnostics for the policy camera worker.""" + + def __init__(self, *, source: str, log_interval_s: float = 5.0) -> None: + self._source = str(source) + self._log_interval_s = float(log_interval_s) + self._failure_count = 0 + self._failure_started_s: float | None = None + self._last_warning_s: float | None = None + self._last_frame_seq: int | None = None + + def note_failure(self, reason: str, *, now_s: float) -> None: + now = float(now_s) + if self._failure_count == 0: + self._failure_started_s = now + self._failure_count += 1 + started_s = self._failure_started_s if self._failure_started_s is not None else now + if ( + self._last_warning_s is not None + and now - self._last_warning_s < self._log_interval_s + ): + return + operator_logger.warning( + "High-level policy %s frame acquisition stalled: " + "consecutive_failures=%d outage_s=%.3f last_frame_seq=%s reason=%s", + self._source, + self._failure_count, + max(0.0, now - started_s), + "none" if self._last_frame_seq is None else self._last_frame_seq, + reason, + ) + self._last_warning_s = now + + def note_frame(self, sequence: int, *, now_s: float) -> None: + now = float(now_s) + if self._failure_count: + started_s = self._failure_started_s if self._failure_started_s is not None else now + operator_logger.warning( + "High-level policy %s frame acquisition recovered: " + "outage_s=%.3f failed_attempts=%d frame_seq=%d", + self._source, + max(0.0, now - started_s), + self._failure_count, + int(sequence), + ) + self._last_frame_seq = int(sequence) + self._failure_count = 0 + self._failure_started_s = None + self._last_warning_s = None + + class HighLevelPolicySim2RealRuntime: def __init__(self, cfg: Any, *, console: PlainConsole | None = None) -> None: self.cfg = _plain_cfg(cfg) @@ -239,6 +290,7 @@ def _main() -> None: slots=int(cfg_get(runtime_cfg, "video_slots", 3)), ) pipeline: Any | None = None + frame_diagnostics = _CameraFrameDiagnostics(source="RealSense") try: if camera_cfg.source == "realsense": try: @@ -276,10 +328,18 @@ def _main() -> None: else: try: frames = pipeline.wait_for_frames(timeout_ms=1000) - except RuntimeError: + except RuntimeError as exc: + frame_diagnostics.note_failure( + str(exc) or type(exc).__name__, + now_s=time.monotonic(), + ) continue color = frames.get_color_frame() if not color: + frame_diagnostics.note_failure( + "frameset contains no color frame", + now_s=time.monotonic(), + ) continue frame = np.ascontiguousarray( np.asanyarray(color.get_data()), @@ -288,6 +348,8 @@ def _main() -> None: timestamp_s = time.monotonic() descriptor = writer.write(frame, timestamp_s=timestamp_s) publisher.publish(VIDEO_TOPIC, descriptor) + if pipeline is not None: + frame_diagnostics.note_frame(descriptor.seq, now_s=timestamp_s) if pipeline is None: elapsed_s = time.monotonic() - started_s if elapsed_s < period_s: diff --git a/teleopit/sim2real/mp/runtime.py b/teleopit/sim2real/mp/runtime.py index eac18b42..2c0a80b3 100644 --- a/teleopit/sim2real/mp/runtime.py +++ b/teleopit/sim2real/mp/runtime.py @@ -150,6 +150,74 @@ class RobotMode(Enum): DAMPING = "damping" +class _PolicyObservationCameraDiagnostics: + """Report why camera-backed policy observations stop being published.""" + + def __init__(self, *, max_age_s: float, log_interval_s: float = 5.0) -> None: + self._max_age_s = float(max_age_s) + self._log_interval_s = float(log_interval_s) + self.reset() + + def reset(self) -> None: + self._blocked_started_s: float | None = None + self._last_warning_s: float | None = None + self._warning_emitted = False + self._last_reason: str | None = None + + def note_blocked( + self, + reason: str, + *, + now_s: float, + frame_seq: int | None, + frame_age_s: float | None, + ) -> None: + now = float(now_s) + if self._blocked_started_s is None: + self._blocked_started_s = now + self._last_reason = str(reason) + blocked_for_s = max(0.0, now - self._blocked_started_s) + if frame_seq is None and blocked_for_s < self._max_age_s: + return + if ( + self._last_warning_s is not None + and now - self._last_warning_s < self._log_interval_s + ): + return + operator_logger.warning( + "High-level policy observation blocked by camera: " + "reason=%s latest_frame_seq=%s frame_age_s=%s " + "freshness_limit_s=%.3f blocked_for_s=%.3f", + self._last_reason, + "none" if frame_seq is None else int(frame_seq), + "unknown" if frame_age_s is None else f"{float(frame_age_s):.3f}", + self._max_age_s, + blocked_for_s, + ) + self._last_warning_s = now + self._warning_emitted = True + + def note_published( + self, + *, + now_s: float, + frame_seq: int, + frame_age_s: float, + ) -> None: + now = float(now_s) + if self._warning_emitted: + started_s = self._blocked_started_s if self._blocked_started_s is not None else now + operator_logger.warning( + "High-level policy observation camera recovered: " + "previous_reason=%s frame_seq=%d frame_age_s=%.3f blocked_for_s=%.3f", + self._last_reason, + int(frame_seq), + float(frame_age_s), + max(0.0, now - started_s), + ) + self.reset() + + class _LoopTimingReporter: def __init__( self, @@ -1288,6 +1356,13 @@ def __init__( self._latest_policy_video: SharedFrameDescriptor | None = None self._latest_policy_status: HighLevelPolicyStatusPacket | None = None self._last_policy_status_seq = -1 + self._policy_camera_diagnostics = ( + _PolicyObservationCameraDiagnostics( + max_age_s=self._high_level_policy_cfg.max_observation_age_s + ) + if self._high_level_policy_cfg is not None + else None + ) self._latest_reference: ReferencePacket | None = None mp_cfg = _mp_cfg(cfg) @@ -1719,6 +1794,9 @@ def _start_high_level_policy_entry_session(self) -> None: if self._latest_policy_video is None else int(self._latest_policy_video.seq) ) + diagnostics = getattr(self, "_policy_camera_diagnostics", None) + if diagnostics is not None: + diagnostics.reset() self._last_policy_session_publish_s = 0.0 self._publish_high_level_policy_session("start", repeat=False) @@ -1773,12 +1851,38 @@ def _publish_high_level_policy_observation(self, robot_state: object) -> None: transform = self._policy_frame_transform session_id = self._policy_session_id policy_cfg = self._high_level_policy_cfg - if publisher is None or frame is None or transform is None or session_id is None or policy_cfg is None: + if publisher is None or transform is None or session_id is None or policy_cfg is None: return + now_s = time.monotonic() + diagnostics = getattr(self, "_policy_camera_diagnostics", None) + if frame is None: + if diagnostics is not None: + diagnostics.note_blocked( + "no_frame_received", + now_s=now_s, + frame_seq=None, + frame_age_s=None, + ) + return + frame_seq = int(frame.seq) + frame_age_s = abs(now_s - float(frame.timestamp_s)) if int(frame.seq) <= self._last_policy_video_seq: + if diagnostics is not None and frame_age_s > policy_cfg.max_observation_age_s: + diagnostics.note_blocked( + "no_new_frame", + now_s=now_s, + frame_seq=frame_seq, + frame_age_s=frame_age_s, + ) return - now_s = time.monotonic() - if abs(now_s - float(frame.timestamp_s)) > policy_cfg.max_observation_age_s: + if frame_age_s > policy_cfg.max_observation_age_s: + if diagnostics is not None: + diagnostics.note_blocked( + "stale_frame", + now_s=now_s, + frame_seq=frame_seq, + frame_age_s=frame_age_s, + ) return state = transform.localize_state(build_observation_state(robot_state)) sequence_id = self._policy_observation_seq @@ -1794,7 +1898,13 @@ def _publish_high_level_policy_observation(self, robot_state: object) -> None: ), ) self._policy_observation_seq += 1 - self._last_policy_video_seq = int(frame.seq) + self._last_policy_video_seq = frame_seq + if diagnostics is not None: + diagnostics.note_published( + now_s=now_s, + frame_seq=frame_seq, + frame_age_s=frame_age_s, + ) def _transition_to_high_level_policy(self) -> None: state = self.robot.get_state() @@ -1855,6 +1965,9 @@ def _stop_high_level_policy_session(self) -> None: self._policy_resume_source_timestamp_ns = None self._policy_hold_qpos = None self._latest_policy_status = None + diagnostics = getattr(self, "_policy_camera_diagnostics", None) + if diagnostics is not None: + diagnostics.reset() def _handle_high_level_policy_fault(self, detail: str) -> None: if not bool(getattr(self, "high_level_policy_enabled", False)): diff --git a/tests/test_high_level_policy.py b/tests/test_high_level_policy.py index 5709bac7..93ffacba 100644 --- a/tests/test_high_level_policy.py +++ b/tests/test_high_level_policy.py @@ -31,6 +31,7 @@ closure_to_o6_pose, ) from teleopit.sim2real.mp.high_level_policy_runtime import ( + _CameraFrameDiagnostics, HighLevelPolicySim2RealRuntime, _apply_policy_neck_target, _policy_target_is_current, @@ -46,6 +47,7 @@ ModeStatePacket, ) from teleopit.sim2real.mp.runtime import ( + _PolicyObservationCameraDiagnostics, RobotMode, Sim2RealRuntime, _RobotControlWorker, @@ -499,6 +501,74 @@ def test_high_level_policy_test_camera_is_exact_protocol_shape() -> None: assert np.all(frame[:, :, 2] == 7) +def test_realsense_acquisition_diagnostics_are_rate_limited_and_report_recovery( + caplog: pytest.LogCaptureFixture, +) -> None: + caplog.set_level("WARNING") + diagnostics = _CameraFrameDiagnostics(source="RealSense", log_interval_s=5.0) + diagnostics.note_frame(40, now_s=9.0) + + diagnostics.note_failure("frame timeout", now_s=10.0) + diagnostics.note_failure("frame timeout", now_s=12.0) + diagnostics.note_failure("frame timeout", now_s=15.1) + diagnostics.note_frame(41, now_s=16.0) + + stalled = [message for message in caplog.messages if "acquisition stalled" in message] + recovered = [message for message in caplog.messages if "acquisition recovered" in message] + assert len(stalled) == 2 + assert "consecutive_failures=1" in stalled[0] + assert "last_frame_seq=40" in stalled[0] + assert "consecutive_failures=3" in stalled[1] + assert recovered == [ + "High-level policy RealSense frame acquisition recovered: " + "outage_s=6.000 failed_attempts=3 frame_seq=41" + ] + + +def test_policy_observation_camera_diagnostics_wait_for_freshness_limit_and_recover( + caplog: pytest.LogCaptureFixture, +) -> None: + caplog.set_level("WARNING") + diagnostics = _PolicyObservationCameraDiagnostics( + max_age_s=0.15, + log_interval_s=5.0, + ) + + diagnostics.note_blocked( + "no_frame_received", + now_s=10.0, + frame_seq=None, + frame_age_s=None, + ) + diagnostics.note_blocked( + "no_frame_received", + now_s=10.14, + frame_seq=None, + frame_age_s=None, + ) + assert caplog.messages == [] + + diagnostics.note_blocked( + "no_frame_received", + now_s=10.16, + frame_seq=None, + frame_age_s=None, + ) + diagnostics.note_blocked( + "no_frame_received", + now_s=11.0, + frame_seq=None, + frame_age_s=None, + ) + diagnostics.note_published(now_s=11.1, frame_seq=8, frame_age_s=0.02) + + assert len(caplog.messages) == 2 + assert "reason=no_frame_received" in caplog.messages[0] + assert "freshness_limit_s=0.150" in caplog.messages[0] + assert "observation camera recovered" in caplog.messages[1] + assert "frame_seq=8" in caplog.messages[1] + + def test_openneck_policy_target_is_sent_directly_in_physical_degrees() -> None: calls: list[tuple[float, float]] = [] device = SimpleNamespace(move_deg=lambda yaw, pitch: calls.append((yaw, pitch))) From b5da98091752467f4036e567cf3b06a1d8a1095f Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Wed, 22 Jul 2026 16:03:44 +0800 Subject: [PATCH 36/59] Simplify high-level policy takeover --- AGENTS.md | 2 +- README.md | 18 +- docs/docs/configuration/config-reference.md | 4 +- docs/docs/reference/architecture.md | 2 +- .../tutorials/high-level-policy-sim2real.md | 45 ++-- .../current/configuration/config-reference.md | 4 +- .../current/reference/architecture.md | 2 +- .../tutorials/high-level-policy-sim2real.md | 31 ++- .../configs/high_level_policy_sim2real.yaml | 2 +- teleopit/high_level_policy/scheduler.py | 6 - .../sim2real/mp/high_level_policy_runtime.py | 64 +----- teleopit/sim2real/mp/runtime.py | 191 ++--------------- tests/test_high_level_policy.py | 197 +++--------------- 13 files changed, 87 insertions(+), 481 deletions(-) diff --git a/AGENTS.md b/AGENTS.md index 5e044cec..4262a5d0 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -187,7 +187,7 @@ target_dof_pos = clip(action, -10, 10) × action_scale + default_dof_pos - Policy observation is RGB JPEG plus `observation.state(68)`; only `state[58:62]` is rotated into the session-local yaw frame - Canonical action is `float32[T,50]` with protocol horizon `T` in `[1,50]`: local root `xyz(3)` + local root quaternion `wxyz(4)` + G1 joint reference `29` + left/right O6 closure `12` + OpenNeck yaw/pitch degrees `2` - The body reference `[0:36]` is yaw/XY-delocalized once and sent through the existing motion tracker. It is never sent directly as a G1 motor command and must not pass through the mocap alignment a second time -- High-level-policy formal robot modes are `IDLE`, `STANDING`, `POLICY`, and `DAMPING`. After Unitree remote `Y`, entry remains internal to `STANDING`: validate one candidate chunk, hold `action[0]` through the existing tracker for one Kp ramp, then create one new host session and require a fresh valid chunk before entering `POLICY`; the 50 Hz output limiter starts from the held `action[0]` reference rather than measured tracker joints, a failure aborts entry, and there is no `POLICY_STARTING` mode +- High-level-policy formal robot modes are `IDLE`, `STANDING`, `POLICY`, and `DAMPING`. After Unitree remote `Y`, Teleopit creates exactly one host session and remains in `STANDING` only while waiting for its first valid chunk; that chunk enters `POLICY` directly. There is no candidate-reference alignment, entry Kp ramp, second session/reset, or `POLICY_STARTING` mode. The 50 Hz output limiter starts from the measured robot reference captured when the session begins, and an entry failure remains in `STANDING` - Unitree remote controls are `Start -> STANDING`, `Y -> request POLICY`, `B -> pause/resume`, `X -> STANDING/cancel pending`, and `L1+R1 -> DAMPING` - Policy pause freezes the scheduler/body reference and holds the latest hand/neck command. Leaving `POLICY` opens LinkerHand and centers OpenNeck - The onboard scheduler clips G1 joint references to `real_robot.joint_pos_lower/upper` when the required correction is at most `high_level_policy.safety.max_joint_projection_rad` (default `0.1` rad), and clips OpenNeck yaw/pitch commands to their configured degree ranges. It rejects whole chunks on excessive joint correction, shape/finiteness, session/sequence, quaternion, absolute root height, hand closure, or staleness failures. It accepts temporal root, yaw, and joint-reference discontinuities at entry, inside chunks, and across chunks because recorded pause/resume transitions may be discontinuous; accepted root translation, yaw, and joint output is rate-limited at 50 Hz. Never pad or trim malformed host output, and do not clip other out-of-range fields into validity diff --git a/README.md b/README.md index c2e10d3c..be1d880a 100644 --- a/README.md +++ b/README.md @@ -157,16 +157,14 @@ python scripts/run/run_high_level_policy_sim2real.py \ Use the Unitree remote: `Start` enters `STANDING`, `Y` requests policy takeover, `B` pauses/resumes, `X` returns to `STANDING`, and `L1+R1` enters `DAMPING`. Policy entry remains an internal `STANDING` phase with no separate -starting mode: Teleopit validates a candidate chunk, holds its first body -reference through one motion-tracker Kp ramp, then creates one fresh host -session. A valid chunk from that session is required before entering `POLICY`; -Replay therefore restarts from its configured start frame and ACT recomputes -from the post-ramp observation. The 50 Hz output limiter starts from the held -reference, not the tracker's measured joint pose. Temporal reference jumps are -accepted so recorded pause/resume transitions can be replayed, then rate-limited -on output. OpenNeck yaw/pitch values are clipped to the configured degree ranges -before scheduling, so a neck-only overshoot does not reject the action chunk. -Entry failure returns to `STANDING`. +starting mode: Teleopit creates one host session and waits for its first valid +chunk, then enters `POLICY` directly. Entry does not align a candidate reference, +run a Kp ramp, pause/resume the host, or create a second session. The 50 Hz output +limiter starts from the measured robot reference captured at session start. +Temporal reference jumps are accepted so recorded pause/resume transitions can +be replayed, then rate-limited on output. OpenNeck yaw/pitch values are clipped +to the configured degree ranges before scheduling, so a neck-only overshoot does +not reject the action chunk. Entry failure returns to `STANDING`. Invalid/stale live chunks and watchdog expiry cannot block the local control loop and instead pause `POLICY` while holding the last reference. The default three-second cached-reference grace period covers transient host inference and diff --git a/docs/docs/configuration/config-reference.md b/docs/docs/configuration/config-reference.md index 2994ab6a..0e9313f9 100644 --- a/docs/docs/configuration/config-reference.md +++ b/docs/docs/configuration/config-reference.md @@ -119,7 +119,7 @@ protocol tests. The only shared data file is `hand_calibration.json`. | `camera.source` | Onboard policy camera: `realsense` or integration-only `test-pattern` | `realsense` | | `camera.width` / `height` / `fps` | Exact policy image contract | `640` / `480` / `30` | | `camera.device` | Optional RealSense serial | `null` | -| `standing_return_ramp_duration` | Kp-ramp duration while tracking the policy-entry candidate first frame | `2.0` | +| `standing_return_ramp_duration` | Kp-ramp duration when returning from active control to `STANDING` | `2.0` | | `high_level_policy.endpoint` | Host policy ZeroMQ TCP endpoint | `tcp://127.0.0.1:5555` | | `high_level_policy.task` | Non-empty task prompt sent on reset and every observation | `demo` | | `high_level_policy.timeout_s` | Per-request network deadline | `1.0` | @@ -128,7 +128,7 @@ protocol tests. The only shared data file is `hand_calibration.json`. | `high_level_policy.jpeg_quality` | JPEG quality for the 640x480 RGB frame | `90` | | `high_level_policy.max_observation_age_s` | Maximum camera/observation age before a request is skipped | `0.15` | | `high_level_policy.max_result_age_s` | Maximum local IPC age before a received result is rejected | `0.1` | -| `high_level_policy.entry_timeout_s` | Maximum total entry duration (candidate, Kp ramp, fresh session) and maximum fresh-chunk wait on resume | `5.0` | +| `high_level_policy.entry_timeout_s` | Maximum time to establish the entry session and receive its first valid chunk, and maximum fresh-chunk wait on resume | `5.0` | | `high_level_policy.hold_s` | Final validated-reference grace period before the watchdog pauses `POLICY`; covers transient host inference and transport delays | `3.0` | | `high_level_policy.safety.root_height_min_m` / `root_height_max_m` | Accepted absolute root-height range | `0.55` / `1.05` | | `high_level_policy.safety.max_root_xy_speed_m_s` | Root XY speed limit applied to the 50 Hz scheduler output | `2.5` | diff --git a/docs/docs/reference/architecture.md b/docs/docs/reference/architecture.md index 2423e1b6..26998e2d 100644 --- a/docs/docs/reference/architecture.md +++ b/docs/docs/reference/architecture.md @@ -94,7 +94,7 @@ train_mimic/scripts/data - sim2real also requires a dual-input ONNX whose observation dimension matches the runtime builder - Host-policy message-envelope or schema mismatches are rejected while the robot remains in `STANDING` - Host action chunks are validated and interpolated onboard; the host cannot bypass the motion tracker or send motor commands -- Policy entry remains internal to `STANDING`: hold one validated candidate first frame for a Kp ramp, then require one fresh-session chunk and start its rate-limited output from the held reference rather than measured tracker joints; the only formal takeover mode is `POLICY` +- Policy entry remains internal to `STANDING` only while one host session waits for its first valid chunk; that chunk enters `POLICY` directly, with no candidate alignment, entry Kp ramp, or second session/reset, and the 50 Hz limiter starts from the measured robot reference captured at session start - Temporal root, yaw, and joint-reference discontinuities are accepted at chunk boundaries and inside chunks, then rate-limited at the 50 Hz scheduler output so recorded pause/resume transitions remain usable ## Public Surface diff --git a/docs/docs/tutorials/high-level-policy-sim2real.md b/docs/docs/tutorials/high-level-policy-sim2real.md index e08f3a66..c2544bfd 100644 --- a/docs/docs/tutorials/high-level-policy-sim2real.md +++ b/docs/docs/tutorials/high-level-policy-sim2real.md @@ -137,21 +137,16 @@ Keep the Unitree remote in hand. The runtime has only the formal robot modes | Unitree remote `X` | Return to `STANDING` or cancel a pending request | | Unitree remote `L1+R1` | Emergency transition to `DAMPING` | -After `Y`, Teleopit creates an entry session, establishes the current root -XY/yaw anchor, and requests one candidate chunk. Its structure, finite values, -quaternion, and absolute hardware ranges are validated. Temporal root, yaw, and -joint-reference jumps are accepted. Teleopit then freezes `action[0]` as a -static body reference and uses the existing motion tracker for one Kp ramp. - -The robot remains formally in `STANDING` throughout entry; there is no separate -"policy starting" state. When the ramp finishes, Teleopit creates a second -session, which resets ReplayPolicy to its configured start frame (frame 0 by -default) or resets ACT state, and requests a fresh chunk from the post-ramp -observation. That fresh chunk must pass normal validation before the runtime -enters `POLICY`. The scheduler's 50 Hz output -limiter starts from the held `action[0]` reference rather than measured tracker -joints, which need not equal a motion reference. A failure or timeout safely -returns to the normal standing reference. +After `Y`, Teleopit creates one entry session, establishes the current root +XY/yaw anchor, and requests its first chunk. The robot remains formally in +`STANDING` while waiting; there is no separate "policy starting" state. The +chunk's structure, finite values, quaternion, and absolute hardware ranges are +validated, while temporal root, yaw, and joint-reference jumps are accepted. +A valid first chunk enters `POLICY` directly. Entry does not align a candidate +reference, run a Kp ramp, pause/resume host requests, or create/reset a second +session. The scheduler's 50 Hz output limiter starts from the measured robot +reference captured when the session begins. A failure or timeout leaves the +robot on the normal standing reference. Pause freezes the body reference and holds the last LinkerHand and OpenNeck commands. Resume requests a fresh action chunk while continuing to hold the @@ -184,12 +179,10 @@ or trims a malformed host result. Checks include: Reference continuity is not an acceptance condition. Root translation, root yaw, and G1 joint-reference jumps are accepted at entry, inside a chunk, and across chunks because a recorded pause/resume transition can intentionally be -discontinuous. Host requests are paused for one Kp ramp. A new host session -then supplies the fresh chunk that will actually enter `POLICY`; the candidate -chunk is never continued as a live timeline. A malformed or stale chunk, an -out-of-range non-joint field other than the projected OpenNeck angles, or an -excessive joint correction in a fresh chunk aborts entry instead of starting -another alignment cycle. +discontinuous. The first valid chunk from the single entry session starts live +execution immediately. A malformed or stale first chunk, an out-of-range +non-joint field other than the projected OpenNeck angles, or an excessive joint +correction aborts entry. Validated 30 Hz body references are interpolated and rate-limited locally at 50 Hz, including when latency skips source frames or a new chunk replaces the @@ -212,14 +205,12 @@ data, G1 joint limits, and the installed OpenNeck calibration. **`Y` never enters `POLICY`:** check the host endpoint, firewall, server log, `describe` schemas, message envelope, task, checkpoint manifest, -`replan_steps`, and the entry logs. Teleopit stays in `STANDING` while it aligns -to the first reference and when any candidate or fresh-chunk check fails. +`replan_steps`, and the entry logs. Teleopit stays in `STANDING` until the single +entry session returns its first valid chunk. -**The fresh entry chunk is rejected or entry times out:** inspect the logged +**The first entry chunk is rejected or entry times out:** inspect the logged contract error, joint ordering, absolute-reference convention, hardware ranges, -and host/network latency. Reference discontinuity alone does not reject a -chunk. `standing_return_ramp_duration` controls physical alignment to the -candidate first frame. +and host/network latency. Reference discontinuity alone does not reject a chunk. **Policy runs briefly and becomes paused:** inspect timeout, inference latency, stale-result, worker-exit, and safety-rejection logs. The low-level diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md index 6e7f61c6..2c79fa9c 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md @@ -137,7 +137,7 @@ client/server 消息结构与协议测试。唯一共享的数据文件是 `hand | `camera.source` | Onboard 策略相机：`realsense`，或仅供集成测试的 `test-pattern` | `realsense` | | `camera.width` / `height` / `fps` | 精确的策略图像契约 | `640` / `480` / `30` | | `camera.device` | 可选 RealSense 序列号 | `null` | -| `standing_return_ramp_duration` | 跟踪策略 entry 候选第一帧时的 Kp ramp 时长 | `2.0` | +| `standing_return_ramp_duration` | 从主动控制返回 `STANDING` 时的 Kp ramp 时长 | `2.0` | | `high_level_policy.endpoint` | 主机策略 ZeroMQ TCP endpoint | `tcp://127.0.0.1:5555` | | `high_level_policy.task` | reset 和每个 observation 都会发送的非空任务 prompt | `demo` | | `high_level_policy.timeout_s` | 单次网络请求 deadline | `1.0` | @@ -146,7 +146,7 @@ client/server 消息结构与协议测试。唯一共享的数据文件是 `hand | `high_level_policy.jpeg_quality` | 640x480 RGB 帧的 JPEG 质量 | `90` | | `high_level_policy.max_observation_age_s` | 跳过请求前允许的最大 camera/observation age | `0.15` | | `high_level_policy.max_result_age_s` | 拒绝已接收结果前允许的最大本地 IPC age | `0.1` | -| `high_level_policy.entry_timeout_s` | 候选请求、Kp ramp 和新 session 的 entry 最长总时间，以及恢复时等待新鲜 chunk 的最长时间 | `5.0` | +| `high_level_policy.entry_timeout_s` | 建立 entry session 并收到其第一份有效 chunk 的最长时间，以及恢复时等待新鲜 chunk 的最长时间 | `5.0` | | `high_level_policy.hold_s` | Watchdog 暂停 `POLICY` 前，最后一条有效 reference 的 grace period；用于容忍短暂的主机推理和传输延迟 | `3.0` | | `high_level_policy.safety.root_height_min_m` / `root_height_max_m` | 可接受的绝对 root 高度范围 | `0.55` / `1.05` | | `high_level_policy.safety.max_root_xy_speed_m_s` | 应用于 50 Hz scheduler 输出的 root XY 速度限制 | `2.5` | diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/architecture.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/architecture.md index ea717586..3b186600 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/architecture.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/architecture.md @@ -91,7 +91,7 @@ train_mimic/scripts/data - sim2real 也要求双输入 ONNX，且观测维度必须与运行时 builder 匹配 - 主机策略消息 envelope 或 schema 不匹配时会被拒绝，机器人保持在 `STANDING` - 主机 action chunk 在 onboard 完成验证与插值；主机不能绕过 motion tracker 或发送电机命令 -- 策略 entry 保持为 `STANDING` 内部流程：通过一次 Kp ramp 保持经过验证的候选第一帧，然后要求新 host session 提供一个 chunk，并从所保持的 reference 而非 tracker 实测关节开始执行 rate-limited 输出；正式接管模式只有 `POLICY` +- 策略 entry 仅在单个 host session 等待第一份有效 chunk 时保持为 `STANDING` 内部流程；该 chunk 会直接进入 `POLICY`，不进行候选 reference 对齐、不运行 entry Kp ramp，也不创建或 reset 第二个 session；50 Hz limiter 从 session 开始时捕获的机器人实测 reference 起步 - chunk 边界和 chunk 内部的 root、yaw 与关节 reference 时间跳变都会被接受，再由 50 Hz scheduler 输出执行 rate limit，从而保留录制的 pause/resume 转换 ## 公共接口 diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md index 7c75ce05..16466a30 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md @@ -129,17 +129,13 @@ python scripts/run/run_high_level_policy_sim2real.py \ | Unitree remote `X` | 返回 `STANDING`，或取消等待中的请求 | | Unitree remote `L1+R1` | 紧急切换到 `DAMPING` | -按下 `Y` 后，Teleopit 会创建 entry session，以当前 root XY/yaw 建立锚点，并请求一个 -候选 chunk。运行时会验证其结构、有限值、四元数和绝对硬件范围；root、yaw 和关节 -reference 的时间跳变会被接受。随后 Teleopit 会冻结 `action[0]` 作为静态 body -reference，并通过现有 motion tracker 在一次 Kp ramp 期间跟踪该 reference。 - -整个 entry 期间，机器人在形式上仍处于 `STANDING`；没有单独的“policy starting” -状态。Kp ramp 结束后，Teleopit 会创建第二个 session：它会把 ReplayPolicy 重置到所 -配置的起始帧（默认为第 0 帧），或重置 ACT 状态，并根据 ramp 后的 observation 请求 -新 chunk。该新 chunk 必须通过正常验证，运行时才会进入 `POLICY`。scheduler 的 50 Hz -输出 limiter 从正在保持的 `action[0]` reference 开始，而不是从无需等于 motion -reference 的 tracker 实测关节开始。失败或超时会安全地返回普通 standing reference。 +按下 `Y` 后，Teleopit 会创建一个 entry session，以当前 root XY/yaw 建立锚点，并请求 +该 session 的第一份 chunk。等待期间机器人在形式上仍处于 `STANDING`；没有单独的 +“policy starting”状态。运行时会验证 chunk 的结构、有限值、四元数和绝对硬件范围， +同时接受 root、yaw 和关节 reference 的时间跳变。第一份有效 chunk 会直接进入 +`POLICY`。entry 不会对齐候选 reference、运行 Kp ramp、暂停/恢复 host 请求，也不会 +创建或 reset 第二个 session。scheduler 的 50 Hz 输出 limiter 从 session 开始时捕获的 +机器人实测 reference 起步。失败或超时会让机器人保持普通 standing reference。 暂停会冻结 body reference，并保持最后一条 LinkerHand 和 OpenNeck 命令。恢复时会请求 新的 action chunk，并在等待期间继续保持暂停姿态。按 `X` 会停止策略 session；运行时 @@ -166,10 +162,8 @@ Watchdog、主机/网络、相机或 policy client 故障也会进入同一个 reference 连续性不是接收条件。entry、chunk 内部和 chunk 之间的 root translation、root yaw 与 G1 关节 reference 跳变都会被接受，因为录制的 pause/resume 转换可能有意地不 -连续。一次 Kp ramp 期间会暂停主机请求，随后新 host session 会提供真正进入 `POLICY` -的新鲜 chunk；候选 chunk 绝不会作为实时 timeline 继续播放。格式错误、过期、非关节字段 -超出绝对范围（已裁剪的 OpenNeck 角度除外）或关节修正量过大的新鲜 chunk 会终止 -entry，而不会开始另一轮对齐。 +连续。单个 entry session 的第一份有效 chunk 会立即开始实时执行。格式错误、过期、 +非关节字段超出绝对范围（已裁剪的 OpenNeck 角度除外）或关节修正量过大会终止 entry。 通过验证的 30 Hz body reference 会在本地插值到 50 Hz 并执行 rate limit；网络延迟 导致跳过 source frame 或新 chunk 替换旧计划时同样如此。配置的 root displacement/XY @@ -188,12 +182,11 @@ speed、yaw rate 和 joint rate 是输出限制，而不是 chunk 拒绝阈值 **按 `Y` 后始终不进入 `POLICY`：** 检查主机 endpoint、防火墙、server 日志、 `describe` schema、消息 envelope、task、checkpoint manifest、`replan_steps` 和 entry -日志。Teleopit 在对齐第一帧 reference，以及候选 chunk 或新鲜 chunk 检查失败时，都会 -保持 `STANDING`。 +日志。Teleopit 会保持 `STANDING`，直到单个 entry session 返回第一份有效 chunk。 -**新鲜 entry chunk 被拒绝或 entry 超时：** 请检查日志中的契约错误、关节顺序、绝对 +**第一份 entry chunk 被拒绝或 entry 超时：** 请检查日志中的契约错误、关节顺序、绝对 reference 约定、硬件范围以及 host/network 延迟。单纯的 reference 跳变不会导致 chunk -被拒绝。`standing_return_ramp_duration` 控制对候选第一帧的物理对齐。 +被拒绝。 **策略短暂运行后进入暂停：** 检查 timeout、推理延迟、stale result、worker 退出和 安全拒绝日志。底层 50 Hz tracker 不会等待主机推理。恢复故障输入路径后按 `B` 继续。 diff --git a/teleopit/configs/high_level_policy_sim2real.yaml b/teleopit/configs/high_level_policy_sim2real.yaml index 78e551db..1d9d867a 100644 --- a/teleopit/configs/high_level_policy_sim2real.yaml +++ b/teleopit/configs/high_level_policy_sim2real.yaml @@ -7,7 +7,7 @@ defaults: input: provider: high_level_policy -# Hold the entry candidate through the tracker before requesting the fresh session. +# Smooth an explicit return from POLICY to STANDING. standing_return_ramp_duration: 2.0 camera: diff --git a/teleopit/high_level_policy/scheduler.py b/teleopit/high_level_policy/scheduler.py index 6cffb0e6..06b9c64b 100644 --- a/teleopit/high_level_policy/scheduler.py +++ b/teleopit/high_level_policy/scheduler.py @@ -171,12 +171,6 @@ def clear(self) -> None: def accept(self, chunk: PolicyActionChunk, *, now_s: float) -> None: self._accept(chunk, now_s=now_s) - def accept_entry(self, chunk: PolicyActionChunk, *, now_s: float) -> np.ndarray: - """Accept an entry candidate and return its validated first action.""" - self._accept(chunk, now_s=now_s) - assert self._chunk is not None - return self._chunk.actions[0].copy() - def _accept( self, chunk: PolicyActionChunk, diff --git a/teleopit/sim2real/mp/high_level_policy_runtime.py b/teleopit/sim2real/mp/high_level_policy_runtime.py index e50b8414..16bcf30a 100644 --- a/teleopit/sim2real/mp/high_level_policy_runtime.py +++ b/teleopit/sim2real/mp/high_level_policy_runtime.py @@ -60,57 +60,6 @@ operator_logger = logging.getLogger(OPERATOR_LOGGER_NAME) -class _CameraFrameDiagnostics: - """Rate-limited acquisition diagnostics for the policy camera worker.""" - - def __init__(self, *, source: str, log_interval_s: float = 5.0) -> None: - self._source = str(source) - self._log_interval_s = float(log_interval_s) - self._failure_count = 0 - self._failure_started_s: float | None = None - self._last_warning_s: float | None = None - self._last_frame_seq: int | None = None - - def note_failure(self, reason: str, *, now_s: float) -> None: - now = float(now_s) - if self._failure_count == 0: - self._failure_started_s = now - self._failure_count += 1 - started_s = self._failure_started_s if self._failure_started_s is not None else now - if ( - self._last_warning_s is not None - and now - self._last_warning_s < self._log_interval_s - ): - return - operator_logger.warning( - "High-level policy %s frame acquisition stalled: " - "consecutive_failures=%d outage_s=%.3f last_frame_seq=%s reason=%s", - self._source, - self._failure_count, - max(0.0, now - started_s), - "none" if self._last_frame_seq is None else self._last_frame_seq, - reason, - ) - self._last_warning_s = now - - def note_frame(self, sequence: int, *, now_s: float) -> None: - now = float(now_s) - if self._failure_count: - started_s = self._failure_started_s if self._failure_started_s is not None else now - operator_logger.warning( - "High-level policy %s frame acquisition recovered: " - "outage_s=%.3f failed_attempts=%d frame_seq=%d", - self._source, - max(0.0, now - started_s), - self._failure_count, - int(sequence), - ) - self._last_frame_seq = int(sequence) - self._failure_count = 0 - self._failure_started_s = None - self._last_warning_s = None - - class HighLevelPolicySim2RealRuntime: def __init__(self, cfg: Any, *, console: PlainConsole | None = None) -> None: self.cfg = _plain_cfg(cfg) @@ -290,7 +239,6 @@ def _main() -> None: slots=int(cfg_get(runtime_cfg, "video_slots", 3)), ) pipeline: Any | None = None - frame_diagnostics = _CameraFrameDiagnostics(source="RealSense") try: if camera_cfg.source == "realsense": try: @@ -328,18 +276,10 @@ def _main() -> None: else: try: frames = pipeline.wait_for_frames(timeout_ms=1000) - except RuntimeError as exc: - frame_diagnostics.note_failure( - str(exc) or type(exc).__name__, - now_s=time.monotonic(), - ) + except RuntimeError: continue color = frames.get_color_frame() if not color: - frame_diagnostics.note_failure( - "frameset contains no color frame", - now_s=time.monotonic(), - ) continue frame = np.ascontiguousarray( np.asanyarray(color.get_data()), @@ -348,8 +288,6 @@ def _main() -> None: timestamp_s = time.monotonic() descriptor = writer.write(frame, timestamp_s=timestamp_s) publisher.publish(VIDEO_TOPIC, descriptor) - if pipeline is not None: - frame_diagnostics.note_frame(descriptor.seq, now_s=timestamp_s) if pipeline is None: elapsed_s = time.monotonic() - started_s if elapsed_s < period_s: diff --git a/teleopit/sim2real/mp/runtime.py b/teleopit/sim2real/mp/runtime.py index 2c0a80b3..7dabb28f 100644 --- a/teleopit/sim2real/mp/runtime.py +++ b/teleopit/sim2real/mp/runtime.py @@ -150,74 +150,6 @@ class RobotMode(Enum): DAMPING = "damping" -class _PolicyObservationCameraDiagnostics: - """Report why camera-backed policy observations stop being published.""" - - def __init__(self, *, max_age_s: float, log_interval_s: float = 5.0) -> None: - self._max_age_s = float(max_age_s) - self._log_interval_s = float(log_interval_s) - self.reset() - - def reset(self) -> None: - self._blocked_started_s: float | None = None - self._last_warning_s: float | None = None - self._warning_emitted = False - self._last_reason: str | None = None - - def note_blocked( - self, - reason: str, - *, - now_s: float, - frame_seq: int | None, - frame_age_s: float | None, - ) -> None: - now = float(now_s) - if self._blocked_started_s is None: - self._blocked_started_s = now - self._last_reason = str(reason) - blocked_for_s = max(0.0, now - self._blocked_started_s) - if frame_seq is None and blocked_for_s < self._max_age_s: - return - if ( - self._last_warning_s is not None - and now - self._last_warning_s < self._log_interval_s - ): - return - operator_logger.warning( - "High-level policy observation blocked by camera: " - "reason=%s latest_frame_seq=%s frame_age_s=%s " - "freshness_limit_s=%.3f blocked_for_s=%.3f", - self._last_reason, - "none" if frame_seq is None else int(frame_seq), - "unknown" if frame_age_s is None else f"{float(frame_age_s):.3f}", - self._max_age_s, - blocked_for_s, - ) - self._last_warning_s = now - self._warning_emitted = True - - def note_published( - self, - *, - now_s: float, - frame_seq: int, - frame_age_s: float, - ) -> None: - now = float(now_s) - if self._warning_emitted: - started_s = self._blocked_started_s if self._blocked_started_s is not None else now - operator_logger.warning( - "High-level policy observation camera recovered: " - "previous_reason=%s frame_seq=%d frame_age_s=%.3f blocked_for_s=%.3f", - self._last_reason, - int(frame_seq), - float(frame_age_s), - max(0.0, now - started_s), - ) - self.reset() - - class _LoopTimingReporter: def __init__( self, @@ -1340,7 +1272,6 @@ def __init__( ) self._policy_entry_pending = False self._policy_entry_deadline_s: float | None = None - self._policy_entry_target_qpos: Float64Array | None = None self._policy_session_id: str | None = None self._policy_frame_transform: PolicyFrameTransform | None = None self._policy_paused = False @@ -1356,13 +1287,6 @@ def __init__( self._latest_policy_video: SharedFrameDescriptor | None = None self._latest_policy_status: HighLevelPolicyStatusPacket | None = None self._last_policy_status_seq = -1 - self._policy_camera_diagnostics = ( - _PolicyObservationCameraDiagnostics( - max_age_s=self._high_level_policy_cfg.max_observation_age_s - ) - if self._high_level_policy_cfg is not None - else None - ) self._latest_reference: ReferencePacket | None = None mp_cfg = _mp_cfg(cfg) @@ -1673,12 +1597,8 @@ def _drain_high_level_policy_ipc(self) -> None: server_inference_ms=float(packet.server_inference_ms), ) if self.mode == RobotMode.STANDING: - first_chunk = self._policy_entry_target_qpos is None try: - if first_chunk: - first_action = scheduler.accept_entry(chunk, now_s=now_s) - else: - scheduler.accept(chunk, now_s=now_s) + scheduler.accept(chunk, now_s=now_s) except ValueError as exc: logger.warning("Rejected high-level policy entry chunk: %s", exc) operator_logger.warning( @@ -1686,10 +1606,7 @@ def _drain_high_level_policy_ipc(self) -> None: ) self._enter_standing() return - if first_chunk: - self._begin_policy_entry_alignment(first_action) - else: - self._transition_to_high_level_policy() + self._transition_to_high_level_policy() return try: scheduler.accept(chunk, now_s=now_s) @@ -1720,7 +1637,7 @@ def _handle_high_level_policy_transitions(self) -> None: self._begin_high_level_policy_entry() if self._policy_entry_pending: self._publish_high_level_policy_session( - "pause" if self._policy_paused else "start", + "start", repeat=True, ) deadline_s = self._policy_entry_deadline_s @@ -1748,18 +1665,16 @@ def _handle_high_level_policy_transitions(self) -> None: self._enter_standing() def _begin_high_level_policy_entry(self) -> None: - self._policy_entry_target_qpos = None self._start_high_level_policy_entry_session() def _build_high_level_policy_boundary_action(self, state: object) -> np.ndarray: transform = self._policy_frame_transform if transform is None: raise RuntimeError("High-level policy entry is missing its frame transform") - boundary_qpos = self._policy_entry_target_qpos - if boundary_qpos is None: - boundary_qpos = self._build_robot_state_qpos(state) initial_action = np.zeros(50, dtype=np.float32) - initial_action[:36] = transform.localize_body_action(boundary_qpos) + initial_action[:36] = transform.localize_body_action( + self._build_robot_state_qpos(state) + ) return initial_action def _start_high_level_policy_entry_session(self) -> None: @@ -1781,8 +1696,7 @@ def _start_high_level_policy_entry_session(self) -> None: initial_action=self._build_high_level_policy_boundary_action(state), ) self._policy_entry_pending = True - if self._policy_entry_target_qpos is None: - self._policy_entry_deadline_s = time.monotonic() + policy_cfg.entry_timeout_s + self._policy_entry_deadline_s = time.monotonic() + policy_cfg.entry_timeout_s self._policy_paused = False self._policy_resume_pending = False self._policy_resume_deadline_s = None @@ -1794,31 +1708,9 @@ def _start_high_level_policy_entry_session(self) -> None: if self._latest_policy_video is None else int(self._latest_policy_video.seq) ) - diagnostics = getattr(self, "_policy_camera_diagnostics", None) - if diagnostics is not None: - diagnostics.reset() self._last_policy_session_publish_s = 0.0 self._publish_high_level_policy_session("start", repeat=False) - def _begin_policy_entry_alignment(self, first_action: np.ndarray) -> None: - transform = self._policy_frame_transform - if transform is None: - raise RuntimeError("High-level policy entry is missing its frame transform") - target_qpos = np.asarray( - transform.delocalize_body_action(first_action[:36]), - dtype=np.float64, - ) - self._policy_entry_target_qpos = target_qpos - self._policy_paused = True - self._reset_policy_state() - self._last_retarget_qpos = None - self._safety.start_kp_ramp( - duration_s=self._standing_return_ramp_duration, - floor_ratio=self._standing_return_kp_ramp_floor_ratio, - ) - self._publish_high_level_policy_session("pause") - operator_logger.info("Aligning to high-level policy first reference in STANDING") - def _publish_high_level_policy_session(self, command: str, *, repeat: bool = False) -> None: publisher = self._policy_control_pub session_id = self._policy_session_id @@ -1851,38 +1743,12 @@ def _publish_high_level_policy_observation(self, robot_state: object) -> None: transform = self._policy_frame_transform session_id = self._policy_session_id policy_cfg = self._high_level_policy_cfg - if publisher is None or transform is None or session_id is None or policy_cfg is None: - return - now_s = time.monotonic() - diagnostics = getattr(self, "_policy_camera_diagnostics", None) - if frame is None: - if diagnostics is not None: - diagnostics.note_blocked( - "no_frame_received", - now_s=now_s, - frame_seq=None, - frame_age_s=None, - ) + if publisher is None or frame is None or transform is None or session_id is None or policy_cfg is None: return - frame_seq = int(frame.seq) - frame_age_s = abs(now_s - float(frame.timestamp_s)) if int(frame.seq) <= self._last_policy_video_seq: - if diagnostics is not None and frame_age_s > policy_cfg.max_observation_age_s: - diagnostics.note_blocked( - "no_new_frame", - now_s=now_s, - frame_seq=frame_seq, - frame_age_s=frame_age_s, - ) return - if frame_age_s > policy_cfg.max_observation_age_s: - if diagnostics is not None: - diagnostics.note_blocked( - "stale_frame", - now_s=now_s, - frame_seq=frame_seq, - frame_age_s=frame_age_s, - ) + now_s = time.monotonic() + if abs(now_s - float(frame.timestamp_s)) > policy_cfg.max_observation_age_s: return state = transform.localize_state(build_observation_state(robot_state)) sequence_id = self._policy_observation_seq @@ -1898,13 +1764,7 @@ def _publish_high_level_policy_observation(self, robot_state: object) -> None: ), ) self._policy_observation_seq += 1 - self._last_policy_video_seq = frame_seq - if diagnostics is not None: - diagnostics.note_published( - now_s=now_s, - frame_seq=frame_seq, - frame_age_s=frame_age_s, - ) + self._last_policy_video_seq = int(frame.seq) def _transition_to_high_level_policy(self) -> None: state = self.robot.get_state() @@ -1915,7 +1775,7 @@ def _transition_to_high_level_policy(self) -> None: self._policy_hold_qpos = resume_qpos.copy() self._policy_entry_pending = False self._policy_entry_deadline_s = None - self._policy_entry_target_qpos = None + self._policy_paused = False self._policy_resume_pending = False self._policy_resume_deadline_s = None self._policy_resume_source_timestamp_ns = None @@ -1956,7 +1816,6 @@ def _stop_high_level_policy_session(self) -> None: scheduler.clear() self._policy_entry_pending = False self._policy_entry_deadline_s = None - self._policy_entry_target_qpos = None self._policy_session_id = None self._policy_frame_transform = None self._policy_paused = False @@ -1965,9 +1824,6 @@ def _stop_high_level_policy_session(self) -> None: self._policy_resume_source_timestamp_ns = None self._policy_hold_qpos = None self._latest_policy_status = None - diagnostics = getattr(self, "_policy_camera_diagnostics", None) - if diagnostics is not None: - diagnostics.reset() def _handle_high_level_policy_fault(self, detail: str) -> None: if not bool(getattr(self, "high_level_policy_enabled", False)): @@ -1997,19 +1853,6 @@ def _handle_high_level_policy_fault(self, detail: str) -> None: self._enter_standing() def _standing_step(self) -> None: - target_qpos = self._policy_entry_target_qpos - if self._policy_entry_pending and target_qpos is not None: - robot_state = self._run_static_mocap_step(target_qpos) - if self._policy_paused: - if not self._safety.kp_ramp_active: - operator_logger.info( - "High-level policy first-reference Kp ramp complete; " - "requesting fresh session" - ) - self._start_high_level_policy_entry_session() - else: - self._publish_high_level_policy_observation(robot_state) - return robot_state = self.robot.get_state() qpos = self._standing_qpos.copy() motion_joint_vel = np.zeros(self.num_actions, dtype=np.float32) @@ -2222,12 +2065,6 @@ def _compose_arm_reference_window(self, reference_window: ReferenceWindow | None def _enter_standing(self) -> None: prev_mode = self.mode - policy_entry_alignment_active = bool( - getattr(self, "high_level_policy_enabled", False) - and prev_mode == RobotMode.STANDING - and self._policy_entry_pending - and self._policy_entry_target_qpos is not None - ) if bool(getattr(self, "high_level_policy_enabled", False)) and ( prev_mode == RobotMode.POLICY or self._policy_entry_pending ): @@ -2235,7 +2072,7 @@ def _enter_standing(self) -> None: self._disarm_mocap_reference_if_needed() self._clear_reference_gate() self._mocap_entry_requested = False - if prev_mode == RobotMode.STANDING and not policy_entry_alignment_active: + if prev_mode == RobotMode.STANDING: return already_in_debug = self.mode in ( RobotMode.STANDING, @@ -2269,7 +2106,7 @@ def _enter_standing(self) -> None: self._last_commanded_motion_qpos = None self._set_default_standing_reference(state) self._reset_policy_state() - if policy_entry_alignment_active or prev_mode in ( + if prev_mode in ( RobotMode.MOCAP, RobotMode.ARMS, RobotMode.POLICY, diff --git a/tests/test_high_level_policy.py b/tests/test_high_level_policy.py index 93ffacba..54a7a0f6 100644 --- a/tests/test_high_level_policy.py +++ b/tests/test_high_level_policy.py @@ -31,7 +31,6 @@ closure_to_o6_pose, ) from teleopit.sim2real.mp.high_level_policy_runtime import ( - _CameraFrameDiagnostics, HighLevelPolicySim2RealRuntime, _apply_policy_neck_target, _policy_target_is_current, @@ -47,7 +46,6 @@ ModeStatePacket, ) from teleopit.sim2real.mp.runtime import ( - _PolicyObservationCameraDiagnostics, RobotMode, Sim2RealRuntime, _RobotControlWorker, @@ -254,7 +252,7 @@ def test_scheduler_validates_complete_chunk_against_onboard_safety_limits() -> N assert scheduler.has_chunk -def test_scheduler_accepts_entry_boundary_and_internal_reference_discontinuities() -> None: +def test_scheduler_accepts_internal_reference_discontinuities() -> None: scheduler = HighLevelPolicyScheduler(hold_s=0.1, safety=_safety_config()) initial = _safe_actions(1)[0] initial[7] = 0.8 @@ -267,11 +265,9 @@ def test_scheduler_accepts_entry_boundary_and_internal_reference_discontinuities actions[1, 3:7] = [math.cos(yaw / 2.0), 0.0, 0.0, math.sin(yaw / 2.0)] scheduler.reset("session-1", initial_action=initial) - first_action = scheduler.accept_entry(_safe_chunk(actions), now_s=1.01) + scheduler.accept(_safe_chunk(actions), now_s=1.01) assert scheduler.has_chunk - assert first_action[0] == pytest.approx(0.2) - assert first_action[7] == pytest.approx(-0.08) def test_scheduler_clips_joint_positions_to_onboard_limits() -> None: @@ -281,10 +277,14 @@ def test_scheduler_clips_joint_positions_to_onboard_limits() -> None: actions[0, 7] = -3.08 actions[0, 8] = 3.08 - first_action = scheduler.accept_entry(_safe_chunk(actions), now_s=1.01) + scheduler.accept(_safe_chunk(actions), now_s=1.01) + scheduled = None + for _ in range(20): + scheduled = scheduler.sample(1.0) - assert first_action[7] == pytest.approx(-3.0) - assert first_action[8] == pytest.approx(3.0) + assert scheduled is not None + assert scheduled[7] == pytest.approx(-3.0) + assert scheduled[8] == pytest.approx(3.0) def test_scheduler_clips_openneck_angles_to_onboard_limits() -> None: @@ -294,9 +294,11 @@ def test_scheduler_clips_openneck_angles_to_onboard_limits() -> None: actions[:, 48] = [-46.0, 0.0, 46.0] actions[:, 49] = [41.0, 0.0, -41.0] - first_action = scheduler.accept_entry(_safe_chunk(actions), now_s=1.01) + scheduler.accept(_safe_chunk(actions), now_s=1.01) + first_action = scheduler.sample(1.0) final_action = scheduler.sample(1.0 + 2.0 / 30.0) + assert first_action is not None assert first_action[48] == pytest.approx(-45.0) assert first_action[49] == pytest.approx(40.0) assert final_action is not None @@ -501,74 +503,6 @@ def test_high_level_policy_test_camera_is_exact_protocol_shape() -> None: assert np.all(frame[:, :, 2] == 7) -def test_realsense_acquisition_diagnostics_are_rate_limited_and_report_recovery( - caplog: pytest.LogCaptureFixture, -) -> None: - caplog.set_level("WARNING") - diagnostics = _CameraFrameDiagnostics(source="RealSense", log_interval_s=5.0) - diagnostics.note_frame(40, now_s=9.0) - - diagnostics.note_failure("frame timeout", now_s=10.0) - diagnostics.note_failure("frame timeout", now_s=12.0) - diagnostics.note_failure("frame timeout", now_s=15.1) - diagnostics.note_frame(41, now_s=16.0) - - stalled = [message for message in caplog.messages if "acquisition stalled" in message] - recovered = [message for message in caplog.messages if "acquisition recovered" in message] - assert len(stalled) == 2 - assert "consecutive_failures=1" in stalled[0] - assert "last_frame_seq=40" in stalled[0] - assert "consecutive_failures=3" in stalled[1] - assert recovered == [ - "High-level policy RealSense frame acquisition recovered: " - "outage_s=6.000 failed_attempts=3 frame_seq=41" - ] - - -def test_policy_observation_camera_diagnostics_wait_for_freshness_limit_and_recover( - caplog: pytest.LogCaptureFixture, -) -> None: - caplog.set_level("WARNING") - diagnostics = _PolicyObservationCameraDiagnostics( - max_age_s=0.15, - log_interval_s=5.0, - ) - - diagnostics.note_blocked( - "no_frame_received", - now_s=10.0, - frame_seq=None, - frame_age_s=None, - ) - diagnostics.note_blocked( - "no_frame_received", - now_s=10.14, - frame_seq=None, - frame_age_s=None, - ) - assert caplog.messages == [] - - diagnostics.note_blocked( - "no_frame_received", - now_s=10.16, - frame_seq=None, - frame_age_s=None, - ) - diagnostics.note_blocked( - "no_frame_received", - now_s=11.0, - frame_seq=None, - frame_age_s=None, - ) - diagnostics.note_published(now_s=11.1, frame_seq=8, frame_age_s=0.02) - - assert len(caplog.messages) == 2 - assert "reason=no_frame_received" in caplog.messages[0] - assert "freshness_limit_s=0.150" in caplog.messages[0] - assert "observation camera recovered" in caplog.messages[1] - assert "frame_seq=8" in caplog.messages[1] - - def test_openneck_policy_target_is_sent_directly_in_physical_degrees() -> None: calls: list[tuple[float, float]] = [] device = SimpleNamespace(move_deg=lambda yaw, pitch: calls.append((yaw, pitch))) @@ -686,56 +620,7 @@ def begin() -> None: assert worker.mode == RobotMode.STANDING -def test_policy_entry_first_action_starts_static_tracker_alignment() -> None: - worker = object.__new__(_RobotControlWorker) - worker._policy_entry_pending = True - worker._policy_frame_transform = PolicyFrameTransform.from_robot_pose( - [0.0, 0.0], - [1.0, 0.0, 0.0, 0.0], - ) - resets: list[str] = [] - worker._reset_policy_state = lambda: resets.append("reset") - worker._last_retarget_qpos = np.zeros(36, dtype=np.float64) - ramps: list[tuple[float, float]] = [] - worker._safety = SimpleNamespace( - start_kp_ramp=lambda *, duration_s, floor_ratio: ramps.append( - (float(duration_s), float(floor_ratio)) - ) - ) - worker._standing_return_ramp_duration = 0.5 - worker._standing_return_kp_ramp_floor_ratio = 0.5 - commands: list[str] = [] - worker._publish_high_level_policy_session = commands.append - action = _safe_actions(1)[0] - action[7:36] = 0.5 - - worker._begin_policy_entry_alignment(action) - - assert worker._policy_paused - assert worker._policy_entry_target_qpos is not None - np.testing.assert_allclose(worker._policy_entry_target_qpos[7:36], 0.5) - assert resets == ["reset"] - assert ramps == [(0.5, 0.5)] - assert commands == ["pause"] - - -def test_policy_entry_requests_fresh_session_after_kp_ramp() -> None: - worker = object.__new__(_RobotControlWorker) - worker._policy_entry_pending = True - worker._policy_paused = True - worker._policy_entry_target_qpos = np.zeros(36, dtype=np.float64) - worker._policy_entry_target_qpos[3] = 1.0 - worker._safety = SimpleNamespace(kp_ramp_active=False) - worker._run_static_mocap_step = lambda _target: SimpleNamespace() - starts: list[str] = [] - worker._start_high_level_policy_entry_session = lambda: starts.append("fresh") - - worker._standing_step() - - assert starts == ["fresh"] - - -def test_policy_transition_after_entry_alignment_does_not_restart_kp_ramp() -> None: +def test_policy_transition_after_first_chunk_does_not_start_kp_ramp() -> None: worker = object.__new__(_RobotControlWorker) worker.mode = RobotMode.STANDING worker.robot = SimpleNamespace(get_state=lambda: SimpleNamespace()) @@ -749,7 +634,7 @@ def test_policy_transition_after_entry_alignment_does_not_restart_kp_ramp() -> N worker._policy_hold_qpos = None worker._policy_entry_pending = True worker._policy_entry_deadline_s = 2.0 - worker._policy_entry_target_qpos = np.ones(36, dtype=np.float64) + worker._policy_paused = True worker._policy_resume_pending = False worker._policy_resume_deadline_s = None worker._policy_resume_source_timestamp_ns = None @@ -757,7 +642,7 @@ def test_policy_transition_after_entry_alignment_does_not_restart_kp_ramp() -> N worker._standing_return_kp_ramp_floor_ratio = 0.5 worker._safety = SimpleNamespace( start_kp_ramp=lambda **_kwargs: pytest.fail( - "POLICY transition must not start a second Kp ramp" + "POLICY transition must not start an entry Kp ramp" ) ) @@ -766,11 +651,11 @@ def test_policy_transition_after_entry_alignment_does_not_restart_kp_ramp() -> N assert worker.mode == RobotMode.POLICY assert resets == ["reset"] assert not worker._policy_entry_pending - assert worker._policy_entry_target_qpos is None + assert not worker._policy_paused np.testing.assert_array_equal(worker._policy_hold_qpos, resume_qpos) -def test_policy_entry_rejects_action_received_after_total_deadline() -> None: +def test_policy_entry_rejects_action_received_after_deadline() -> None: worker = object.__new__(_RobotControlWorker) now_s = time.monotonic() worker.mode = RobotMode.STANDING @@ -796,7 +681,6 @@ def test_policy_entry_rejects_action_received_after_total_deadline() -> None: worker._policy_resume_source_timestamp_ns = None worker._policy_entry_pending = True worker._policy_entry_deadline_s = now_s - 0.01 - worker._policy_entry_target_qpos = np.zeros(36, dtype=np.float64) accepted: list[object] = [] worker._high_level_policy_scheduler = SimpleNamespace( accept=lambda *args, **kwargs: accepted.append((args, kwargs)) @@ -814,7 +698,7 @@ def test_policy_entry_rejects_action_received_after_total_deadline() -> None: assert accepted == [] -def test_policy_entry_fresh_chunk_uses_held_reference_boundary() -> None: +def test_policy_entry_first_chunk_uses_measured_reference_boundary() -> None: worker = object.__new__(_RobotControlWorker) now_s = time.monotonic() worker.mode = RobotMode.STANDING @@ -840,10 +724,6 @@ def test_policy_entry_fresh_chunk_uses_held_reference_boundary() -> None: worker._policy_resume_source_timestamp_ns = None worker._policy_entry_pending = True worker._policy_entry_deadline_s = now_s + 1.0 - held_qpos = np.zeros(36, dtype=np.float64) - held_qpos[2] = 0.76 - held_qpos[3] = 1.0 - worker._policy_entry_target_qpos = held_qpos worker._policy_frame_transform = PolicyFrameTransform.from_robot_pose( [0.0, 0.0], [1.0, 0.0, 0.0, 0.0], @@ -861,17 +741,20 @@ def test_policy_entry_fresh_chunk_uses_held_reference_boundary() -> None: worker._high_level_policy_scheduler = scheduler worker._high_level_policy_cfg = SimpleNamespace(max_result_age_s=1.0) worker._enter_standing = lambda: pytest.fail( - "fresh chunk must not be compared with measured tracker joints" + "valid first chunk must enter POLICY" ) transitions: list[str] = [] worker._transition_to_high_level_policy = lambda: transitions.append("policy") worker._drain_high_level_policy_ipc() - assert boundary_action[7] == pytest.approx(0.0) + scheduled = scheduler.sample(now_s) + assert boundary_action[7] == pytest.approx(0.8) assert current_qpos[7] == pytest.approx(0.8) assert transitions == ["policy"] assert scheduler.has_chunk + assert scheduled is not None + assert scheduled[7] == pytest.approx(0.6) def test_policy_entry_stale_result_aborts_current_session() -> None: @@ -900,7 +783,6 @@ def test_policy_entry_stale_result_aborts_current_session() -> None: worker._policy_resume_source_timestamp_ns = None worker._policy_entry_pending = True worker._policy_entry_deadline_s = now_s + 1.0 - worker._policy_entry_target_qpos = np.zeros(36, dtype=np.float64) worker._high_level_policy_scheduler = SimpleNamespace() worker._high_level_policy_cfg = SimpleNamespace(max_result_age_s=0.1) standing: list[str] = [] @@ -911,59 +793,32 @@ def test_policy_entry_stale_result_aborts_current_session() -> None: assert standing == ["standing"] -def test_policy_entry_alignment_abort_skips_standing_joint_lock() -> None: +def test_policy_entry_cancel_from_standing_only_stops_session() -> None: worker = object.__new__(_RobotControlWorker) worker.high_level_policy_enabled = True worker.mode = RobotMode.STANDING worker._policy_entry_pending = True - worker._policy_entry_target_qpos = np.zeros(36, dtype=np.float64) worker._mocap_entry_requested = False stops: list[str] = [] def stop_session() -> None: stops.append("stop") worker._policy_entry_pending = False - worker._policy_entry_target_qpos = None worker._stop_high_level_policy_session = stop_session worker._disarm_mocap_reference_if_needed = lambda: None worker._clear_reference_gate = lambda: None - state = SimpleNamespace() worker.robot = SimpleNamespace( - get_state=lambda: state, + get_state=lambda: pytest.fail("entry cancel must not rebuild STANDING"), lock_all_joints=lambda: pytest.fail( - "STANDING entry abort must not lock joints or block the control loop" + "entry cancel must not lock joints or block the control loop" ), ) - current_qpos = np.zeros(36, dtype=np.float64) - current_qpos[3] = 1.0 - worker._build_robot_state_qpos = lambda _state: current_qpos.copy() - worker._ref_proc = SimpleNamespace(last_reference_qpos=current_qpos.copy()) - mocap_resets: list[str] = [] - worker._mocap_session = SimpleNamespace( - reset=lambda: mocap_resets.append("reset") - ) - standing_references: list[object] = [] - worker._set_default_standing_reference = standing_references.append - policy_resets: list[str] = [] - worker._reset_policy_state = lambda: policy_resets.append("reset") - ramps: list[tuple[float, float]] = [] - worker._safety = SimpleNamespace( - start_kp_ramp=lambda *, duration_s, floor_ratio: ramps.append( - (float(duration_s), float(floor_ratio)) - ) - ) - worker._standing_return_ramp_duration = 0.5 - worker._standing_return_kp_ramp_floor_ratio = 0.5 worker._enter_standing() assert stops == ["stop"] assert worker.mode == RobotMode.STANDING - assert mocap_resets == ["reset"] - assert standing_references == [state] - assert policy_resets == ["reset"] - assert ramps == [(0.5, 0.5)] def test_high_level_policy_body_action_uses_existing_tracker_without_second_alignment() -> None: From 7a1baa43f53fbc836b1fc6b07d280cbcfcc750e7 Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Thu, 23 Jul 2026 16:48:49 +0800 Subject: [PATCH 37/59] Add minimal RealSense stall diagnostics --- teleopit/sim2real/mp/high_level_policy_runtime.py | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/teleopit/sim2real/mp/high_level_policy_runtime.py b/teleopit/sim2real/mp/high_level_policy_runtime.py index 16bcf30a..d60b3dc0 100644 --- a/teleopit/sim2real/mp/high_level_policy_runtime.py +++ b/teleopit/sim2real/mp/high_level_policy_runtime.py @@ -261,6 +261,7 @@ def _main() -> None: pipeline.start(rs_config) period_s = 1.0 / float(camera_cfg.fps) test_frame_index = 0 + camera_stalled = False while not stop_event.is_set(): command = command_sub.recv_latest() if isinstance(command, CommandPacket) and command.command == "shutdown": @@ -276,11 +277,22 @@ def _main() -> None: else: try: frames = pipeline.wait_for_frames(timeout_ms=1000) - except RuntimeError: + except RuntimeError as exc: + if not camera_stalled: + operator_logger.warning( + "High-level policy RealSense stalled: %s", exc + ) + camera_stalled = True continue color = frames.get_color_frame() if not color: + if not camera_stalled: + operator_logger.warning( + "High-level policy RealSense returned no color frame" + ) + camera_stalled = True continue + camera_stalled = False frame = np.ascontiguousarray( np.asanyarray(color.get_data()), dtype=np.uint8, From c0a447169f574731df7013bcd6fe3c87008396b5 Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Thu, 23 Jul 2026 17:14:19 +0800 Subject: [PATCH 38/59] Reconnect high-level policy RealSense camera --- .../sim2real/mp/high_level_policy_runtime.py | 80 +++++++++++++------ 1 file changed, 56 insertions(+), 24 deletions(-) diff --git a/teleopit/sim2real/mp/high_level_policy_runtime.py b/teleopit/sim2real/mp/high_level_policy_runtime.py index d60b3dc0..613ef93e 100644 --- a/teleopit/sim2real/mp/high_level_policy_runtime.py +++ b/teleopit/sim2real/mp/high_level_policy_runtime.py @@ -238,6 +238,7 @@ def _main() -> None: dtype=np.uint8, slots=int(cfg_get(runtime_cfg, "video_slots", 3)), ) + rs: Any | None = None pipeline: Any | None = None try: if camera_cfg.source == "realsense": @@ -247,18 +248,6 @@ def _main() -> None: raise RuntimeError( "RealSense high-level-policy camera requires pyrealsense2" ) from exc - pipeline = rs.pipeline() - rs_config = rs.config() - if camera_cfg.device is not None: - rs_config.enable_device(camera_cfg.device) - rs_config.enable_stream( - rs.stream.color, - camera_cfg.width, - camera_cfg.height, - rs.format.rgb8, - camera_cfg.fps, - ) - pipeline.start(rs_config) period_s = 1.0 / float(camera_cfg.fps) test_frame_index = 0 camera_stalled = False @@ -267,7 +256,7 @@ def _main() -> None: if isinstance(command, CommandPacket) and command.command == "shutdown": break started_s = time.monotonic() - if pipeline is None: + if camera_cfg.source != "realsense": frame = _test_pattern( camera_cfg.height, camera_cfg.width, @@ -275,23 +264,60 @@ def _main() -> None: ) test_frame_index += 1 else: + assert rs is not None + if pipeline is None: + try: + pipeline = rs.pipeline() + rs_config = rs.config() + if camera_cfg.device is not None: + rs_config.enable_device(camera_cfg.device) + rs_config.enable_stream( + rs.stream.color, + camera_cfg.width, + camera_cfg.height, + rs.format.rgb8, + camera_cfg.fps, + ) + pipeline.start(rs_config) + except Exception as exc: + pipeline = None + if not camera_stalled: + operator_logger.warning( + "High-level policy RealSense unavailable; " + "retrying in 1.0s: %s", + exc, + ) + camera_stalled = True + stop_event.wait(1.0) + continue try: frames = pipeline.wait_for_frames(timeout_ms=1000) - except RuntimeError as exc: + color = frames.get_color_frame() + if not color: + raise RuntimeError("frameset contains no color frame") + except Exception as exc: if not camera_stalled: operator_logger.warning( - "High-level policy RealSense stalled: %s", exc + "High-level policy RealSense stalled; " + "reconnecting in 1.0s: %s", + exc, ) camera_stalled = True - continue - color = frames.get_color_frame() - if not color: - if not camera_stalled: - operator_logger.warning( - "High-level policy RealSense returned no color frame" + try: + pipeline.stop() + except RuntimeError as stop_exc: + logger.warning( + "Failed to stop stalled high-level policy " + "RealSense pipeline: %s", + stop_exc, ) - camera_stalled = True + pipeline = None + stop_event.wait(1.0) continue + if camera_stalled: + operator_logger.info( + "High-level policy RealSense reconnected" + ) camera_stalled = False frame = np.ascontiguousarray( np.asanyarray(color.get_data()), @@ -300,13 +326,19 @@ def _main() -> None: timestamp_s = time.monotonic() descriptor = writer.write(frame, timestamp_s=timestamp_s) publisher.publish(VIDEO_TOPIC, descriptor) - if pipeline is None: + if camera_cfg.source != "realsense": elapsed_s = time.monotonic() - started_s if elapsed_s < period_s: time.sleep(period_s - elapsed_s) finally: if pipeline is not None: - pipeline.stop() + try: + pipeline.stop() + except RuntimeError as exc: + logger.warning( + "Failed to stop high-level policy RealSense pipeline: %s", + exc, + ) writer.close(unlink=True) command_sub.close() publisher.close() From a5696a2c017e6b8c021cc6caa8bd2d152c537531 Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Thu, 23 Jul 2026 19:32:38 +0800 Subject: [PATCH 39/59] Hardware-reset stalled policy camera --- .../sim2real/mp/high_level_policy_runtime.py | 49 +++++++++++++------ tests/test_high_level_policy.py | 11 +++++ 2 files changed, 46 insertions(+), 14 deletions(-) diff --git a/teleopit/sim2real/mp/high_level_policy_runtime.py b/teleopit/sim2real/mp/high_level_policy_runtime.py index 613ef93e..84a7de3e 100644 --- a/teleopit/sim2real/mp/high_level_policy_runtime.py +++ b/teleopit/sim2real/mp/high_level_policy_runtime.py @@ -225,6 +225,17 @@ def _main() -> None: _worker_loop("high_level_policy", cfg, _main) +def _stop_and_hardware_reset_realsense(pipeline: Any, device: Any) -> None: + try: + pipeline.stop() + except RuntimeError as exc: + logger.warning("Failed to stop stalled high-level policy RealSense pipeline: %s", exc) + try: + device.hardware_reset() + except RuntimeError as exc: + logger.warning("Failed to hardware-reset high-level policy RealSense: %s", exc) + + def _run_high_level_policy_camera_worker( cfg: dict[str, Any], endpoints: Sim2RealIpcEndpoints, stop_event: MpEvent ) -> None: @@ -239,7 +250,11 @@ def _main() -> None: slots=int(cfg_get(runtime_cfg, "video_slots", 3)), ) rs: Any | None = None + rs_context: Any | None = None pipeline: Any | None = None + pipeline_profile: Any | None = None + device: Any | None = None + device_serial = camera_cfg.device try: if camera_cfg.source == "realsense": try: @@ -248,6 +263,7 @@ def _main() -> None: raise RuntimeError( "RealSense high-level-policy camera requires pyrealsense2" ) from exc + rs_context = rs.context() period_s = 1.0 / float(camera_cfg.fps) test_frame_index = 0 camera_stalled = False @@ -267,10 +283,10 @@ def _main() -> None: assert rs is not None if pipeline is None: try: - pipeline = rs.pipeline() + pipeline = rs.pipeline(rs_context) rs_config = rs.config() - if camera_cfg.device is not None: - rs_config.enable_device(camera_cfg.device) + if device_serial is not None: + rs_config.enable_device(device_serial) rs_config.enable_stream( rs.stream.color, camera_cfg.width, @@ -278,9 +294,14 @@ def _main() -> None: rs.format.rgb8, camera_cfg.fps, ) - pipeline.start(rs_config) + pipeline_profile = pipeline.start(rs_config) + device = pipeline_profile.get_device() + if device_serial is None: + device_serial = device.get_info(rs.camera_info.serial_number) except Exception as exc: pipeline = None + pipeline_profile = None + device = None if not camera_stalled: operator_logger.warning( "High-level policy RealSense unavailable; " @@ -299,19 +320,17 @@ def _main() -> None: if not camera_stalled: operator_logger.warning( "High-level policy RealSense stalled; " - "reconnecting in 1.0s: %s", + "hardware-resetting and reconnecting: %s", exc, ) camera_stalled = True - try: - pipeline.stop() - except RuntimeError as stop_exc: - logger.warning( - "Failed to stop stalled high-level policy " - "RealSense pipeline: %s", - stop_exc, - ) + frames = None + color = None + assert device is not None + _stop_and_hardware_reset_realsense(pipeline, device) pipeline = None + pipeline_profile = None + device = None stop_event.wait(1.0) continue if camera_stalled: @@ -326,7 +345,9 @@ def _main() -> None: timestamp_s = time.monotonic() descriptor = writer.write(frame, timestamp_s=timestamp_s) publisher.publish(VIDEO_TOPIC, descriptor) - if camera_cfg.source != "realsense": + if camera_cfg.source == "realsense": + frame = color = frames = None + else: elapsed_s = time.monotonic() - started_s if elapsed_s < period_s: time.sleep(period_s - elapsed_s) diff --git a/tests/test_high_level_policy.py b/tests/test_high_level_policy.py index 54a7a0f6..6a8d2d5c 100644 --- a/tests/test_high_level_policy.py +++ b/tests/test_high_level_policy.py @@ -34,6 +34,7 @@ HighLevelPolicySim2RealRuntime, _apply_policy_neck_target, _policy_target_is_current, + _stop_and_hardware_reset_realsense, _test_pattern, _validate_high_level_policy_runtime_config, ) @@ -503,6 +504,16 @@ def test_high_level_policy_test_camera_is_exact_protocol_shape() -> None: assert np.all(frame[:, :, 2] == 7) +def test_realsense_recovery_stops_pipeline_before_hardware_reset() -> None: + calls: list[str] = [] + pipeline = SimpleNamespace(stop=lambda: calls.append("stop")) + device = SimpleNamespace(hardware_reset=lambda: calls.append("hardware_reset")) + + _stop_and_hardware_reset_realsense(pipeline, device) + + assert calls == ["stop", "hardware_reset"] + + def test_openneck_policy_target_is_sent_directly_in_physical_degrees() -> None: calls: list[tuple[float, float]] = [] device = SimpleNamespace(move_deg=lambda yaw, pitch: calls.append((yaw, pitch))) From 682bdd9c4c2598dbec31073eff0c90306bc6daba Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Thu, 23 Jul 2026 20:36:13 +0800 Subject: [PATCH 40/59] Make high-level policy chunk-synchronous --- AGENTS.md | 4 +- README.md | 17 +- docs/docs/configuration/config-reference.md | 9 +- .../tutorials/high-level-policy-sim2real.md | 62 ++-- .../current/configuration/config-reference.md | 8 +- .../tutorials/high-level-policy-sim2real.md | 46 +-- .../configs/high_level_policy_sim2real.yaml | 2 - teleopit/high_level_policy/__init__.py | 4 +- teleopit/high_level_policy/client.py | 2 +- teleopit/high_level_policy/config.py | 20 +- teleopit/high_level_policy/scheduler.py | 79 ++--- .../sim2real/mp/high_level_policy_runtime.py | 4 +- .../sim2real/mp/high_level_policy_worker.py | 34 +-- teleopit/sim2real/mp/runtime.py | 126 +++++--- tests/test_high_level_policy.py | 271 ++++++++++++------ 15 files changed, 417 insertions(+), 271 deletions(-) diff --git a/AGENTS.md b/AGENTS.md index 4262a5d0..211fbeb2 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -183,7 +183,7 @@ target_dof_pos = clip(action, -10, 10) × action_scale + default_dof_pos - The host LeRobot/ReplayPolicy service runs in the separate `lerobot-teleopit` repository and environment; Teleopit must not depend on LeRobot, Transformers, or host policy classes - The current client/server code and protocol tests define the ZeroMQ request/response structure. During active development, Teleopit and `lerobot-teleopit` must update that structure together; no legacy network envelope is supported - The only shared data file is `hand_calibration.json`, which contains the LinkerHand O6 open/close calibration and must stay identical in both repositories -- The host boundary uses ZeroMQ REQ/REP with msgpack and non-pickle float32 arrays. The onboard network client runs in a non-critical worker and never blocks the 50 Hz robot loop +- The host boundary uses ZeroMQ REQ/REP with msgpack and non-pickle float32 arrays. Policy inference is strictly chunk-synchronous: publish one observation, hold the last safe reference while the isolated client worker blocks for one response, execute the complete returned chunk at 30 Hz, then publish the next observation. There is no overlapping request, latency-based frame skipping, or async compatibility mode; process isolation keeps the 50 Hz robot loop running - Policy observation is RGB JPEG plus `observation.state(68)`; only `state[58:62]` is rotated into the session-local yaw frame - Canonical action is `float32[T,50]` with protocol horizon `T` in `[1,50]`: local root `xyz(3)` + local root quaternion `wxyz(4)` + G1 joint reference `29` + left/right O6 closure `12` + OpenNeck yaw/pitch degrees `2` - The body reference `[0:36]` is yaw/XY-delocalized once and sent through the existing motion tracker. It is never sent directly as a G1 motor command and must not pass through the mocap alignment a second time @@ -191,7 +191,7 @@ target_dof_pos = clip(action, -10, 10) × action_scale + default_dof_pos - Unitree remote controls are `Start -> STANDING`, `Y -> request POLICY`, `B -> pause/resume`, `X -> STANDING/cancel pending`, and `L1+R1 -> DAMPING` - Policy pause freezes the scheduler/body reference and holds the latest hand/neck command. Leaving `POLICY` opens LinkerHand and centers OpenNeck - The onboard scheduler clips G1 joint references to `real_robot.joint_pos_lower/upper` when the required correction is at most `high_level_policy.safety.max_joint_projection_rad` (default `0.1` rad), and clips OpenNeck yaw/pitch commands to their configured degree ranges. It rejects whole chunks on excessive joint correction, shape/finiteness, session/sequence, quaternion, absolute root height, hand closure, or staleness failures. It accepts temporal root, yaw, and joint-reference discontinuities at entry, inside chunks, and across chunks because recorded pause/resume transitions may be discontinuous; accepted root translation, yaw, and joint output is rate-limited at 50 Hz. Never pad or trim malformed host output, and do not clip other out-of-range fields into validity -- A configured cached-reference grace period (`high_level_policy.hold_s`, default `3.0` seconds) covers transient host inference and transport delays; exhaustion, host/network failure, or loss of a required camera/client worker puts `POLICY` into the same resumable pause state used by remote `B`. The runtime never enters `STANDING` automatically; `B` resumes after a fresh valid chunk is available, while `X` remains the manual transition to `STANDING` +- Finishing a chunk normally starts the next synchronous request and holds its final reference during inference. A request timeout, host/network failure, invalid result, or loss of a required camera/client worker puts `POLICY` into the same resumable pause state used by remote `B`. The runtime never enters `STANDING` automatically; `B` resumes after a fresh valid chunk is available, while `X` remains the manual transition to `STANDING` - Initial production hardware support requires two LinkerHand O6 hands and OpenNeck because all 50 canonical action dimensions are active - OpenNeck 0.2.0 is the supported runtime; Teleopit sends physical degrees through `move_deg()`, and the direct-drive OpenNeck package converts degrees to servo steps and clips them to its calibrated mechanical limits; the removed normalized API and config fields are unsupported - OpenNeck maps the independent HMD `PicoFrame.head.rotation` relative to the same-frame full-body `Body.Spine3` orientation; it must never use the full-body `Body.Head` skeleton joint for neck control, and HMD pose updates must remain independent of duplicate-body-frame filtering diff --git a/README.md b/README.md index be1d880a..95cefb6d 100644 --- a/README.md +++ b/README.md @@ -142,6 +142,10 @@ dedicated onboard runtime. The host policy remains in the independent reference chunks over ZeroMQ, validates and interpolates them onboard, and rate-limits plan switches at 50 Hz before passing the 36D body reference through the existing motion tracker. The host never sends G1 motor commands. +Inference is chunk-synchronous: Teleopit sends one observation, holds the final +safe reference while that request completes, executes the complete returned +chunk at 30 Hz, and only then sends the next observation. Requests and action +chunks never overlap. Pico and high-level-policy deployment use separate scripts. The policy runtime does not start PicoBridge, GMR, or the Pico reference worker: @@ -165,13 +169,12 @@ Temporal reference jumps are accepted so recorded pause/resume transitions can be replayed, then rate-limited on output. OpenNeck yaw/pitch values are clipped to the configured degree ranges before scheduling, so a neck-only overshoot does not reject the action chunk. Entry failure returns to `STANDING`. -Invalid/stale live chunks and watchdog expiry cannot block the local control -loop and instead pause `POLICY` while holding the last reference. The default -three-second cached-reference grace period covers transient host inference and -transport delays before watchdog expiry. Host/network -failure and loss of a required camera/client worker use the same ordinary pause -state as remote `B`; after recovery, press `B` to resume on a fresh valid chunk. -Only `X` returns active `POLICY` to `STANDING`. +The blocking network exchange runs in an isolated process, so expected +inference time holds the last reference without stopping the local 50 Hz +control loop. Invalid/stale chunks, a request timeout, host/network failure, or +loss of a required camera/client worker enters the same ordinary pause state as +remote `B`; after recovery, press `B` to resume on a fresh valid chunk. Only +`X` returns active `POLICY` to `STANDING`. The current client/server code and protocol tests define the network message structure. During active development, Teleopit and `lerobot-teleopit` must be diff --git a/docs/docs/configuration/config-reference.md b/docs/docs/configuration/config-reference.md index 0e9313f9..a1379c61 100644 --- a/docs/docs/configuration/config-reference.md +++ b/docs/docs/configuration/config-reference.md @@ -122,14 +122,12 @@ protocol tests. The only shared data file is `hand_calibration.json`. | `standing_return_ramp_duration` | Kp-ramp duration when returning from active control to `STANDING` | `2.0` | | `high_level_policy.endpoint` | Host policy ZeroMQ TCP endpoint | `tcp://127.0.0.1:5555` | | `high_level_policy.task` | Non-empty task prompt sent on reset and every observation | `demo` | -| `high_level_policy.timeout_s` | Per-request network deadline | `1.0` | +| `high_level_policy.timeout_s` | Per-request network deadline; expiry pauses `POLICY` | `1.0` | | `high_level_policy.reconnect_backoff_s` | Retry delay while establishing a new session | `1.0` | -| `high_level_policy.replan_steps` | Minimum 30 Hz source-frame interval between requests; must not exceed host horizon | `3` | | `high_level_policy.jpeg_quality` | JPEG quality for the 640x480 RGB frame | `90` | | `high_level_policy.max_observation_age_s` | Maximum camera/observation age before a request is skipped | `0.15` | | `high_level_policy.max_result_age_s` | Maximum local IPC age before a received result is rejected | `0.1` | | `high_level_policy.entry_timeout_s` | Maximum time to establish the entry session and receive its first valid chunk, and maximum fresh-chunk wait on resume | `5.0` | -| `high_level_policy.hold_s` | Final validated-reference grace period before the watchdog pauses `POLICY`; covers transient host inference and transport delays | `3.0` | | `high_level_policy.safety.root_height_min_m` / `root_height_max_m` | Accepted absolute root-height range | `0.55` / `1.05` | | `high_level_policy.safety.max_root_xy_speed_m_s` | Root XY speed limit applied to the 50 Hz scheduler output | `2.5` | | `high_level_policy.safety.max_root_displacement_m` | Source-frame-equivalent 3D root step used by the 50 Hz output limiter | `0.1` | @@ -139,6 +137,11 @@ protocol tests. The only shared data file is `hand_calibration.json`. | `high_level_policy.safety.neck_yaw_min_deg` / `neck_yaw_max_deg` | OpenNeck yaw clipping range | `-45` / `45` | | `high_level_policy.safety.neck_pitch_min_deg` / `neck_pitch_max_deg` | OpenNeck pitch clipping range | `-40` / `40` | +The request schedule is not configurable. Teleopit always issues one request, +holds the final safe reference during inference, executes the complete returned +chunk at 30 Hz, and then issues the next request. Removed `replan_steps` and +`hold_s` keys are configuration errors. + G1 reference joint positions are clipped to `real_robot.joint_pos_lower/upper` when the required correction does not exceed `high_level_policy.safety.max_joint_projection_rad`; larger corrections reject diff --git a/docs/docs/tutorials/high-level-policy-sim2real.md b/docs/docs/tutorials/high-level-policy-sim2real.md index c2544bfd..7d29f4c6 100644 --- a/docs/docs/tutorials/high-level-policy-sim2real.md +++ b/docs/docs/tutorials/high-level-policy-sim2real.md @@ -16,7 +16,8 @@ Host workstation (lerobot-teleopit) | float32 state/action + JPEG over TCP v G1 onboard computer (Teleopit) - RealSense + G1 state -> client -> validated 30 Hz action chunk + RealSense + G1 state -> one blocking request -> validated 30 Hz action chunk + -> execute complete chunk -> next request -> 50 Hz interpolation -> motion tracker -> G1 joint-angle targets -> LinkerHand O6 / OpenNeck ``` @@ -115,10 +116,16 @@ python scripts/run/run_high_level_policy_sim2real.py \ real_robot.network_interface=eth0 ``` -The protocol accepts action chunks from 1 to 50 frames. The production ACT -checkpoint uses a 50-frame horizon with `high_level_policy.replan_steps=3`. -For a 15-frame ReplayPolicy chunk, `replan_steps=15` remains valid. The request -stride must not exceed the horizon reported by the host. +The protocol accepts action chunks from 1 to 50 frames. A learned-policy host +predicts its full model horizon but returns only the checkpoint's +`n_action_steps`; the default ACT checkpoint therefore predicts 50 frames and +returns 3. ReplayPolicy returns up to its configured `--chunk-size`, including +a shorter final tail. + +Request scheduling has no mode or stride setting. Teleopit sends one +observation, waits while holding the final safe reference, executes the entire +returned chunk at 30 Hz, and then samples the next observation. It never +overlaps requests or replaces an executing chunk. The production camera contract is exactly RGB `uint8[480,640,3]` at 30 Hz. `camera.source=test-pattern` exists only for controlled integration testing; @@ -148,6 +155,10 @@ session. The scheduler's 50 Hz output limiter starts from the measured robot reference captured when the session begins. A failure or timeout leaves the robot on the normal standing reference. +Inside `POLICY`, finishing a chunk is not a watchdog event. Teleopit holds its +final body, hand, and neck targets while the isolated client process performs +the next blocking request. The local 50 Hz control loop continues running. + Pause freezes the body reference and holds the last LinkerHand and OpenNeck commands. Resume requests a fresh action chunk while continuing to hold the paused pose. `X` stops the policy session and opens/centers the auxiliary @@ -179,23 +190,23 @@ or trims a malformed host result. Checks include: Reference continuity is not an acceptance condition. Root translation, root yaw, and G1 joint-reference jumps are accepted at entry, inside a chunk, and across chunks because a recorded pause/resume transition can intentionally be -discontinuous. The first valid chunk from the single entry session starts live -execution immediately. A malformed or stale first chunk, an out-of-range -non-joint field other than the projected OpenNeck angles, or an excessive joint -correction aborts entry. +discontinuous. The 50 Hz output limiter bridges accepted discontinuities. The +first valid chunk from the single entry session starts live execution +immediately. A malformed or stale first chunk, an out-of-range non-joint field +other than the projected OpenNeck angles, or an excessive joint correction +aborts entry. Validated 30 Hz body references are interpolated and rate-limited locally at -50 Hz, including when latency skips source frames or a new chunk replaces the -old plan. The configured root displacement/XY speed, yaw-rate, and joint-rate -values are output limits, not chunk-rejection thresholds. The configured -grace period (three seconds by default) reuses the final validated reference -during transient inference or transport delays. -If no valid action remains, a network -exchange fails, or a required camera/client worker exits, Teleopit remains in -`POLICY`, enters the normal resumable pause state, and holds the latest body, -hand, and neck commands. After recovery, `B` requests resume; execution stays -paused until a fresh validated chunk arrives. Only `X` changes the mode to -`STANDING`. +50 Hz from the time each response is accepted. Source timestamps identify the +observation echoed by the response; they are not used to skip into the chunk. +The configured root displacement/XY speed, yaw-rate, and joint-rate values are +output limits, not chunk-rejection thresholds. Normal inference between chunks +holds the final validated reference without a grace timer. If a network +exchange times out, a response is invalid, or a required camera/client worker +exits, Teleopit remains in `POLICY`, enters the normal resumable pause state, +and holds the latest body, hand, and neck commands. After recovery, `B` +requests resume; execution stays paused until a fresh validated chunk arrives. +Only `X` changes the mode to `STANDING`. The default safety envelope lives under `high_level_policy.safety` in `high_level_policy_sim2real.yaml`. Adjust it only after checking the recorded @@ -204,9 +215,9 @@ data, G1 joint limits, and the installed OpenNeck calibration. ## 7. Troubleshooting **`Y` never enters `POLICY`:** check the host endpoint, firewall, server log, -`describe` schemas, message envelope, task, checkpoint manifest, -`replan_steps`, and the entry logs. Teleopit stays in `STANDING` until the single -entry session returns its first valid chunk. +`describe` schemas, message envelope, task, checkpoint manifest, and the entry +logs. Teleopit stays in `STANDING` until the single entry session returns its +first valid chunk. **The first entry chunk is rejected or entry times out:** inspect the logged contract error, joint ordering, absolute-reference convention, hardware ranges, @@ -214,8 +225,9 @@ and host/network latency. Reference discontinuity alone does not reject a chunk. **Policy runs briefly and becomes paused:** inspect timeout, inference latency, stale-result, worker-exit, and safety-rejection logs. The low-level -50 Hz tracker does not block on host inference. Restore the failed input path, -then press `B` to resume. +50 Hz tracker does not block on host inference, and expected inference between +chunks only holds the current reference. Restore the failed input path, then +press `B` to resume. **Pico does not connect:** this runtime intentionally does not start Pico. Stop it and launch the Pico-specific `run_sim2real.py --config-name diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md index 2c79fa9c..386bba79 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md @@ -140,14 +140,12 @@ client/server 消息结构与协议测试。唯一共享的数据文件是 `hand | `standing_return_ramp_duration` | 从主动控制返回 `STANDING` 时的 Kp ramp 时长 | `2.0` | | `high_level_policy.endpoint` | 主机策略 ZeroMQ TCP endpoint | `tcp://127.0.0.1:5555` | | `high_level_policy.task` | reset 和每个 observation 都会发送的非空任务 prompt | `demo` | -| `high_level_policy.timeout_s` | 单次网络请求 deadline | `1.0` | +| `high_level_policy.timeout_s` | 单次网络请求 deadline；超时会暂停 `POLICY` | `1.0` | | `high_level_policy.reconnect_backoff_s` | 建立新 session 时的重试间隔 | `1.0` | -| `high_level_policy.replan_steps` | 两次请求之间的最小 30 Hz source-frame 间隔；不能超过主机 horizon | `3` | | `high_level_policy.jpeg_quality` | 640x480 RGB 帧的 JPEG 质量 | `90` | | `high_level_policy.max_observation_age_s` | 跳过请求前允许的最大 camera/observation age | `0.15` | | `high_level_policy.max_result_age_s` | 拒绝已接收结果前允许的最大本地 IPC age | `0.1` | | `high_level_policy.entry_timeout_s` | 建立 entry session 并收到其第一份有效 chunk 的最长时间，以及恢复时等待新鲜 chunk 的最长时间 | `5.0` | -| `high_level_policy.hold_s` | Watchdog 暂停 `POLICY` 前，最后一条有效 reference 的 grace period；用于容忍短暂的主机推理和传输延迟 | `3.0` | | `high_level_policy.safety.root_height_min_m` / `root_height_max_m` | 可接受的绝对 root 高度范围 | `0.55` / `1.05` | | `high_level_policy.safety.max_root_xy_speed_m_s` | 应用于 50 Hz scheduler 输出的 root XY 速度限制 | `2.5` | | `high_level_policy.safety.max_root_displacement_m` | 50 Hz 输出 rate limiter 使用的 source-frame 等效 3D root 步长 | `0.1` | @@ -157,6 +155,10 @@ client/server 消息结构与协议测试。唯一共享的数据文件是 `hand | `high_level_policy.safety.neck_yaw_min_deg` / `neck_yaw_max_deg` | OpenNeck yaw 裁剪范围 | `-45` / `45` | | `high_level_policy.safety.neck_pitch_min_deg` / `neck_pitch_max_deg` | OpenNeck pitch 裁剪范围 | `-40` / `40` | +请求调度不可配置。Teleopit 始终只发出一个请求，在推理期间保持最后一条安全 +reference，随后以 30 Hz 完整执行返回的 chunk，执行结束后再发出下一个请求。已删除的 +`replan_steps` 和 `hold_s` 配置键会直接报错。 + 当所需修正量不超过 `high_level_policy.safety.max_joint_projection_rad` 时，G1 reference joint position 会裁剪到 `real_robot.joint_pos_lower/upper`；更大的修正量会导致 chunk 被拒绝。 OpenNeck yaw/pitch 会裁剪到配置范围，单纯的 neck 越界不会导致 chunk 被拒绝。由于 canonical diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md index 16466a30..51665787 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md @@ -15,7 +15,8 @@ ZeroMQ/msgpack 消息通信。 | 通过 TCP 传输 float32 state/action + JPEG v G1 onboard 计算机（Teleopit） - RealSense + G1 state -> client -> 已验证的 30 Hz action chunk + RealSense + G1 state -> 单个阻塞请求 -> 已验证的 30 Hz action chunk + -> 完整执行 chunk -> 下一个请求 -> 50 Hz 插值 -> motion tracker -> G1 关节角目标 -> LinkerHand O6 / OpenNeck ``` @@ -108,9 +109,13 @@ python scripts/run/run_high_level_policy_sim2real.py \ real_robot.network_interface=eth0 ``` -协议接受 1 到 50 帧的 action chunk。正式 ACT checkpoint 使用 50 帧 horizon，并配置 -`high_level_policy.replan_steps=3`。对于 15 帧 ReplayPolicy chunk，仍可使用 -`replan_steps=15`。请求步长不能超过主机报告的 horizon。 +协议接受 1 到 50 帧的 action chunk。learned-policy 主机会预测完整的模型 horizon，但只 +返回 checkpoint 的 `n_action_steps`；因此默认 ACT checkpoint 会预测 50 帧并返回 3 帧。 +ReplayPolicy 最多返回 `--chunk-size` 配置的帧数，最后一段可以更短。 + +请求调度没有模式或 stride 配置。Teleopit 会发送一份 observation，在等待期间保持最后一条 +安全 reference，以 30 Hz 完整执行返回的 chunk，然后再采样下一份 observation。请求不会 +重叠，执行中的 chunk 也不会被替换。 生产相机契约固定为 30 Hz 的 RGB `uint8[480,640,3]`。 `camera.source=test-pattern` 只用于受控集成测试；部署时应使用 @@ -137,6 +142,9 @@ python scripts/run/run_high_level_policy_sim2real.py \ 创建或 reset 第二个 session。scheduler 的 50 Hz 输出 limiter 从 session 开始时捕获的 机器人实测 reference 起步。失败或超时会让机器人保持普通 standing reference。 +在 `POLICY` 内，chunk 执行结束不是 watchdog 事件。隔离的 client 进程执行下一次阻塞 +请求时，Teleopit 会保持最后一条 body、hand 和 neck target，本地 50 Hz 控制循环继续运行。 + 暂停会冻结 body reference，并保持最后一条 LinkerHand 和 OpenNeck 命令。恢复时会请求 新的 action chunk，并在等待期间继续保持暂停姿态。按 `X` 会停止策略 session；运行时 返回 `STANDING` 时会张开手并让辅助硬件回中。 @@ -162,17 +170,18 @@ Watchdog、主机/网络、相机或 policy client 故障也会进入同一个 reference 连续性不是接收条件。entry、chunk 内部和 chunk 之间的 root translation、root yaw 与 G1 关节 reference 跳变都会被接受，因为录制的 pause/resume 转换可能有意地不 -连续。单个 entry session 的第一份有效 chunk 会立即开始实时执行。格式错误、过期、 -非关节字段超出绝对范围（已裁剪的 OpenNeck 角度除外）或关节修正量过大会终止 entry。 - -通过验证的 30 Hz body reference 会在本地插值到 50 Hz 并执行 rate limit；网络延迟 -导致跳过 source frame 或新 chunk 替换旧计划时同样如此。配置的 root displacement/XY -speed、yaw rate 和 joint rate 是输出限制，而不是 chunk 拒绝阈值。在短暂的推理或传输延迟 -期间，可以在配置的 grace period（默认三秒）内继续使用最后一条已验证 reference。如果不再有有效 action， -网络交换失败，或必要的 camera/client worker 退出，Teleopit 会保持在 `POLICY`，进入 -普通的可恢复暂停状态，并保持最后一条 body、hand 和 neck 命令。故障恢复后按 `B` -请求恢复；在收到新的有效 chunk 前，执行仍保持暂停。只有 `X` 会把模式切换到 -`STANDING`。 +连续；50 Hz 输出 limiter 会衔接这些已接受的跳变。单个 entry session 的第一份有效 +chunk 会立即开始实时执行。格式错误、过期、非关节字段超出绝对范围（已裁剪的 OpenNeck +角度除外）或关节修正量过大会终止 entry。 + +每份 response 通过验证后，其中的 30 Hz body reference 会从接收时刻开始在本地插值到 +50 Hz 并执行 rate limit。source timestamp 用于标识 response 回显的 observation，不会 +用于跳入 chunk。配置的 root displacement/XY speed、yaw rate 和 joint rate 是输出限制， +而不是 chunk 拒绝阈值。chunk 之间的正常推理会一直保持最后一条有效 reference，不使用 +grace timer。如果网络交换超时、response 无效，或必要的 camera/client worker 退出， +Teleopit 会保持在 `POLICY`，进入普通的可恢复暂停状态，并保持最后一条 body、hand 和 +neck 命令。故障恢复后按 `B` 请求恢复；在收到新的有效 chunk 前，执行仍保持暂停。只有 +`X` 会把模式切换到 `STANDING`。 默认安全范围位于 `high_level_policy_sim2real.yaml` 的 `high_level_policy.safety` 下。只有在检查录制数据、G1 关节限位和已安装的 OpenNeck @@ -181,15 +190,16 @@ speed、yaw rate 和 joint rate 是输出限制，而不是 chunk 拒绝阈值 ## 7. 故障排查 **按 `Y` 后始终不进入 `POLICY`：** 检查主机 endpoint、防火墙、server 日志、 -`describe` schema、消息 envelope、task、checkpoint manifest、`replan_steps` 和 entry -日志。Teleopit 会保持 `STANDING`，直到单个 entry session 返回第一份有效 chunk。 +`describe` schema、消息 envelope、task、checkpoint manifest 和 entry 日志。Teleopit +会保持 `STANDING`，直到单个 entry session 返回第一份有效 chunk。 **第一份 entry chunk 被拒绝或 entry 超时：** 请检查日志中的契约错误、关节顺序、绝对 reference 约定、硬件范围以及 host/network 延迟。单纯的 reference 跳变不会导致 chunk 被拒绝。 **策略短暂运行后进入暂停：** 检查 timeout、推理延迟、stale result、worker 退出和 -安全拒绝日志。底层 50 Hz tracker 不会等待主机推理。恢复故障输入路径后按 `B` 继续。 +安全拒绝日志。底层 50 Hz tracker 不会等待主机推理，chunk 之间的正常推理只会保持当前 +reference。恢复故障输入路径后按 `B` 继续。 **Pico 无法连接：** 该运行时有意不启动 Pico。请先停止它，再改用 Pico 专用的 `run_sim2real.py --config-name pico4_sim2real` 工作流。 diff --git a/teleopit/configs/high_level_policy_sim2real.yaml b/teleopit/configs/high_level_policy_sim2real.yaml index 1d9d867a..e0bae81c 100644 --- a/teleopit/configs/high_level_policy_sim2real.yaml +++ b/teleopit/configs/high_level_policy_sim2real.yaml @@ -23,12 +23,10 @@ high_level_policy: task: demo timeout_s: 1.0 reconnect_backoff_s: 1.0 - replan_steps: 3 # Current ACT checkpoint; use 15 for ReplayPolicy. jpeg_quality: 90 max_observation_age_s: 0.15 max_result_age_s: 0.1 entry_timeout_s: 5.0 - hold_s: 3.0 safety: root_height_min_m: 0.55 root_height_max_m: 1.05 diff --git a/teleopit/high_level_policy/__init__.py b/teleopit/high_level_policy/__init__.py index 107bb1bb..fec03bf3 100644 --- a/teleopit/high_level_policy/__init__.py +++ b/teleopit/high_level_policy/__init__.py @@ -7,15 +7,15 @@ ) from teleopit.high_level_policy.hand_calibration import HandCalibration from teleopit.high_level_policy.scheduler import ( - HighLevelPolicyScheduler, PolicyFrameTransform, + SynchronousPolicyScheduler, ) __all__ = [ "HandCalibration", "HighLevelPolicyClient", - "HighLevelPolicyScheduler", "PolicyActionChunk", "PolicyDescription", "PolicyFrameTransform", + "SynchronousPolicyScheduler", ] diff --git a/teleopit/high_level_policy/client.py b/teleopit/high_level_policy/client.py index 412ee837..e847ecbf 100644 --- a/teleopit/high_level_policy/client.py +++ b/teleopit/high_level_policy/client.py @@ -1,4 +1,4 @@ -"""Synchronous ZeroMQ client used only from the onboard background worker.""" +"""Synchronous ZeroMQ client used only from the isolated onboard worker.""" from __future__ import annotations diff --git a/teleopit/high_level_policy/config.py b/teleopit/high_level_policy/config.py index 79c89fcd..e309e177 100644 --- a/teleopit/high_level_policy/config.py +++ b/teleopit/high_level_policy/config.py @@ -6,7 +6,6 @@ import numpy as np -from teleopit.high_level_policy.protocol import MAX_ACTION_HORIZON from teleopit.runtime.common import cfg_get @@ -16,12 +15,10 @@ class HighLevelPolicyConfig: task: str timeout_s: float reconnect_backoff_s: float - replan_steps: int jpeg_quality: int max_observation_age_s: float max_result_age_s: float entry_timeout_s: float - hold_s: float @dataclass(frozen=True) @@ -52,6 +49,12 @@ class HighLevelPolicySafetyConfig: def parse_high_level_policy_config(cfg: Any) -> HighLevelPolicyConfig: policy_cfg = cfg_get(cfg, "high_level_policy", {}) or {} + for removed_name in ("replan_steps", "hold_s"): + if cfg_get(policy_cfg, removed_name, None) is not None: + raise ValueError( + f"high_level_policy.{removed_name} was removed; " + "policy inference is always chunk-synchronous" + ) endpoint = str(cfg_get(policy_cfg, "endpoint", "tcp://127.0.0.1:5555")).strip() if not endpoint.startswith("tcp://"): raise ValueError("high_level_policy.endpoint must be a tcp:// endpoint") @@ -64,12 +67,6 @@ def parse_high_level_policy_config(cfg: Any) -> HighLevelPolicyConfig: reconnect_backoff_s = _positive_float( cfg_get(policy_cfg, "reconnect_backoff_s", 1.0), "reconnect_backoff_s" ) - replan_steps = int(cfg_get(policy_cfg, "replan_steps", 3)) - if not 1 <= replan_steps <= MAX_ACTION_HORIZON: - raise ValueError( - "high_level_policy.replan_steps must be in " - f"[1, {MAX_ACTION_HORIZON}]" - ) jpeg_quality = int(cfg_get(policy_cfg, "jpeg_quality", 90)) if not 1 <= jpeg_quality <= 100: raise ValueError("high_level_policy.jpeg_quality must be in [1, 100]") @@ -82,20 +79,15 @@ def parse_high_level_policy_config(cfg: Any) -> HighLevelPolicyConfig: entry_timeout_s = _positive_float( cfg_get(policy_cfg, "entry_timeout_s", 5.0), "entry_timeout_s" ) - hold_s = float(cfg_get(policy_cfg, "hold_s", 3.0)) - if not math.isfinite(hold_s) or hold_s < 0.0: - raise ValueError("high_level_policy.hold_s must be finite and >= 0") return HighLevelPolicyConfig( endpoint=endpoint, task=task, timeout_s=timeout_s, reconnect_backoff_s=reconnect_backoff_s, - replan_steps=replan_steps, jpeg_quality=jpeg_quality, max_observation_age_s=max_observation_age_s, max_result_age_s=max_result_age_s, entry_timeout_s=entry_timeout_s, - hold_s=hold_s, ) diff --git a/teleopit/high_level_policy/scheduler.py b/teleopit/high_level_policy/scheduler.py index 06b9c64b..dc391e90 100644 --- a/teleopit/high_level_policy/scheduler.py +++ b/teleopit/high_level_policy/scheduler.py @@ -1,4 +1,4 @@ -"""Session-local frame conversion and latency-aware 30 Hz action scheduling.""" +"""Session-local frame conversion and synchronous 30 Hz chunk execution.""" from __future__ import annotations @@ -106,27 +106,22 @@ def delocalize_body_action(self, action: object) -> np.ndarray: return body -class HighLevelPolicyScheduler: +class SynchronousPolicyScheduler: def __init__( self, *, - hold_s: float = 3.0, safety: HighLevelPolicySafetyConfig | None = None, output_hz: float = 50.0, ) -> None: - if not np.isfinite(hold_s) or hold_s < 0.0: - raise ValueError("high_level_policy.hold_s must be finite and >= 0") if not np.isfinite(output_hz) or output_hz <= 0.0: raise ValueError("High-level policy scheduler output_hz must be finite and > 0") - self.hold_s = float(hold_s) self.safety = safety self.output_hz = float(output_hz) self._session_id: str | None = None self._chunk: PolicyActionChunk | None = None + self._chunk_started_s: float | None = None self._last_source_sequence_id = -1 self._last_source_timestamp_ns = -1 - self._paused_at_s: float | None = None - self._timestamp_shift_s = 0.0 self._last_output_action: np.ndarray | None = None @property @@ -137,19 +132,14 @@ def session_id(self) -> str | None: def has_chunk(self) -> bool: return self._chunk is not None - @property - def paused(self) -> bool: - return self._paused_at_s is not None - def reset(self, session_id: str, *, initial_action: object | None = None) -> None: if not isinstance(session_id, str) or not session_id: raise ValueError("High-level policy session_id must be non-empty") self._session_id = session_id self._chunk = None + self._chunk_started_s = None self._last_source_sequence_id = -1 self._last_source_timestamp_ns = -1 - self._paused_at_s = None - self._timestamp_shift_s = 0.0 initial_output = ( None if initial_action is None @@ -162,23 +152,18 @@ def reset(self, session_id: str, *, initial_action: object | None = None) -> Non def clear(self) -> None: self._session_id = None self._chunk = None + self._chunk_started_s = None self._last_source_sequence_id = -1 self._last_source_timestamp_ns = -1 - self._paused_at_s = None - self._timestamp_shift_s = 0.0 self._last_output_action = None def accept(self, chunk: PolicyActionChunk, *, now_s: float) -> None: - self._accept(chunk, now_s=now_s) - - def _accept( - self, - chunk: PolicyActionChunk, - *, - now_s: float, - ) -> None: if not np.isfinite(now_s): raise ValueError("High-level policy scheduler now_s must be finite") + if self._chunk is not None: + raise ValueError( + "High-level policy cannot replace an active synchronous action chunk" + ) if self._session_id is None or chunk.session_id != self._session_id: raise ValueError( f"High-level policy action session mismatch: active={self._session_id!r}, " @@ -213,19 +198,7 @@ def _accept( raise ValueError("High-level policy policy_id must be non-empty") if not np.isfinite(chunk.server_inference_ms) or chunk.server_inference_ms < 0.0: raise ValueError("High-level policy server_inference_ms must be finite and >= 0") - source_s = chunk.source_onboard_monotonic_timestamp_ns * 1e-9 - if source_s > float(now_s) + 0.001: - raise ValueError( - "High-level policy source timestamp is in the future: " - f"source={source_s:.9f}s now={float(now_s):.9f}s" - ) actions = self._validate_actions(chunk.actions) - valid_until_s = source_s + len(actions) / float(chunk.action_fps) + self.hold_s - if float(now_s) > valid_until_s: - raise ValueError( - "High-level policy action chunk is already expired: " - f"age={float(now_s) - source_s:.3f}s horizon={len(actions) / chunk.action_fps:.3f}s" - ) self._chunk = PolicyActionChunk( session_id=chunk.session_id, source_sequence_id=chunk.source_sequence_id, @@ -235,23 +208,17 @@ def _accept( policy_id=chunk.policy_id, server_inference_ms=chunk.server_inference_ms, ) + self._chunk_started_s = float(now_s) self._last_source_sequence_id = chunk.source_sequence_id self._last_source_timestamp_ns = chunk.source_onboard_monotonic_timestamp_ns - self._timestamp_shift_s = 0.0 - if self._paused_at_s is not None: - self._paused_at_s = float(now_s) - - def pause(self, now_s: float) -> None: - if self._paused_at_s is None: - self._paused_at_s = float(now_s) - def resume(self, now_s: float) -> None: - if self._paused_at_s is None: - return - self._timestamp_shift_s += max(0.0, float(now_s) - self._paused_at_s) - self._paused_at_s = None + def discard_chunk(self) -> None: + self._chunk = None + self._chunk_started_s = None def sample(self, now_s: float) -> np.ndarray | None: + if not np.isfinite(now_s): + raise ValueError("High-level policy scheduler now_s must be finite") desired = self._sample_unlimited(now_s) if desired is None: return None @@ -264,18 +231,20 @@ def sample(self, now_s: float) -> np.ndarray | None: def _sample_unlimited(self, now_s: float) -> np.ndarray | None: chunk = self._chunk - if chunk is None: + started_s = self._chunk_started_s + if chunk is None or started_s is None: + return None + elapsed_s = float(now_s) - started_s + if elapsed_s < 0.0: + raise ValueError("High-level policy scheduler time moved backwards") + if elapsed_s >= len(chunk.actions) / float(chunk.action_fps): + self.discard_chunk() return None - effective_now_s = self._paused_at_s if self._paused_at_s is not None else float(now_s) - source_s = chunk.source_onboard_monotonic_timestamp_ns * 1e-9 + self._timestamp_shift_s - frame_f = (effective_now_s - source_s) * float(chunk.action_fps) + frame_f = elapsed_s * float(chunk.action_fps) if frame_f <= 0.0: return chunk.actions[0].copy() last_index = len(chunk.actions) - 1 if frame_f >= float(last_index): - valid_until_s = source_s + len(chunk.actions) / float(chunk.action_fps) + self.hold_s - if effective_now_s > valid_until_s: - return None return chunk.actions[last_index].copy() index0 = int(math.floor(frame_f)) index1 = min(index0 + 1, last_index) diff --git a/teleopit/sim2real/mp/high_level_policy_runtime.py b/teleopit/sim2real/mp/high_level_policy_runtime.py index 84a7de3e..adfc124b 100644 --- a/teleopit/sim2real/mp/high_level_policy_runtime.py +++ b/teleopit/sim2real/mp/high_level_policy_runtime.py @@ -28,7 +28,7 @@ LinkerHandO6Device, parse_linkerhand_o6_config, ) -from teleopit.sim2real.mp.high_level_policy_worker import HighLevelPolicyWorker +from teleopit.sim2real.mp.high_level_policy_worker import SynchronousPolicyWorker from teleopit.sim2real.mp.ipc import ( COMMAND_TOPIC, HIGH_LEVEL_POLICY_TARGET_TOPIC, @@ -220,7 +220,7 @@ def _run_high_level_policy_client_worker( cfg: dict[str, Any], endpoints: Sim2RealIpcEndpoints, stop_event: MpEvent ) -> None: def _main() -> None: - HighLevelPolicyWorker(cfg, endpoints, stop_event).run() + SynchronousPolicyWorker(cfg, endpoints, stop_event).run() _worker_loop("high_level_policy", cfg, _main) diff --git a/teleopit/sim2real/mp/high_level_policy_worker.py b/teleopit/sim2real/mp/high_level_policy_worker.py index 13056d06..7dfda9ec 100644 --- a/teleopit/sim2real/mp/high_level_policy_worker.py +++ b/teleopit/sim2real/mp/high_level_policy_worker.py @@ -1,4 +1,4 @@ -"""Non-critical host-policy client worker for high-level-policy sim2real.""" +"""Blocking client worker for the chunk-synchronous host-policy loop.""" from __future__ import annotations @@ -59,7 +59,7 @@ def encode_policy_jpeg(frame: object, *, quality: int) -> bytes: return payload -class HighLevelPolicyWorker: +class SynchronousPolicyWorker: def __init__( self, cfg: dict[str, Any], @@ -91,7 +91,6 @@ def __init__( self._paused = False self._last_session_seq = -1 self._last_observation_seq = -1 - self._last_request_timestamp_ns: int | None = None self._next_connect_time_s = 0.0 self._status_seq = 0 self._policy_type: str | None = None @@ -109,9 +108,14 @@ def run(self) -> None: self._handle_session(session) if self._active_session is not None and not self._ready: self._connect_if_due() - observation = self._observation_sub.recv_latest() - if isinstance(observation, HighLevelPolicyObservationPacket): - self._handle_observation(observation) + if ( + self._active_session is not None + and self._ready + and not self._paused + ): + observation = self._observation_sub.recv_latest() + if isinstance(observation, HighLevelPolicyObservationPacket): + self._handle_observation(observation) time.sleep(0.001) finally: self.close() @@ -138,7 +142,6 @@ def _handle_session(self, packet: HighLevelPolicySessionPacket) -> None: self._ready = False self._paused = False self._last_observation_seq = -1 - self._last_request_timestamp_ns = None self._next_connect_time_s = 0.0 self._new_session_required = False self._policy_type = None @@ -154,7 +157,6 @@ def _handle_session(self, packet: HighLevelPolicySessionPacket) -> None: elif command == "resume": if self._paused: self._paused = False - self._last_request_timestamp_ns = None if self._ready: self._publish_status("ready", "policy requests resumed") else: @@ -184,11 +186,6 @@ def _connect_if_due(self) -> None: timeout_s=self.policy_cfg.timeout_s, ) description = self._client.describe() - if self.policy_cfg.replan_steps > description.max_action_horizon: - raise ValueError( - "high_level_policy.replan_steps exceeds host max_action_horizon: " - f"{self.policy_cfg.replan_steps} > {description.max_action_horizon}" - ) self._client.reset(session.session_id, session.task) self._policy_type = description.policy_type self._policy_id = description.policy_id @@ -208,12 +205,6 @@ def _handle_observation(self, packet: HighLevelPolicyObservationPacket) -> None: now_s = time.monotonic() if now_s - float(packet.timestamp_s) > self.policy_cfg.max_observation_age_s: return - minimum_interval_ns = int(round(self.policy_cfg.replan_steps / 30.0 * 1e9)) - if ( - self._last_request_timestamp_ns is not None - and packet.onboard_monotonic_timestamp_ns - self._last_request_timestamp_ns < minimum_interval_ns - ): - return client = self._client if client is None: return @@ -247,7 +238,6 @@ def _handle_observation(self, packet: HighLevelPolicyObservationPacket) -> None: ), ) self._last_observation_seq = int(packet.sequence_id) - self._last_request_timestamp_ns = int(packet.onboard_monotonic_timestamp_ns) except (PolicyProtocolError, PolicyTransportError, ValueError, RuntimeError) as exc: logger.warning("High-level policy request failed: %s", exc) self._ready = False @@ -275,10 +265,10 @@ def _publish_status(self, status: str, detail: str) -> None: ) -def run_high_level_policy_worker( +def run_synchronous_policy_worker( cfg: dict[str, Any], endpoints: Sim2RealIpcEndpoints, stop_event: Any, ) -> None: - worker = HighLevelPolicyWorker(cfg, endpoints, stop_event) + worker = SynchronousPolicyWorker(cfg, endpoints, stop_event) worker.run() diff --git a/teleopit/sim2real/mp/runtime.py b/teleopit/sim2real/mp/runtime.py index 7dabb28f..121a0e87 100644 --- a/teleopit/sim2real/mp/runtime.py +++ b/teleopit/sim2real/mp/runtime.py @@ -21,7 +21,10 @@ parse_high_level_policy_config, parse_high_level_policy_safety_config, ) -from teleopit.high_level_policy.scheduler import HighLevelPolicyScheduler, PolicyFrameTransform +from teleopit.high_level_policy.scheduler import ( + PolicyFrameTransform, + SynchronousPolicyScheduler, +) from teleopit.controllers.observation import VelCmdObservationBuilder, align_motion_qpos_yaw from teleopit.controllers.rl_policy import RLPolicyController from teleopit.inputs.bvh_provider import BVHInputProvider @@ -1262,8 +1265,7 @@ def __init__( else None ) self._high_level_policy_scheduler = ( - HighLevelPolicyScheduler( - hold_s=self._high_level_policy_cfg.hold_s, + SynchronousPolicyScheduler( safety=self._high_level_policy_safety_cfg, output_hz=self.policy_hz, ) @@ -1277,7 +1279,9 @@ def __init__( self._policy_paused = False self._policy_resume_pending = False self._policy_resume_deadline_s: float | None = None - self._policy_resume_source_timestamp_ns: int | None = None + self._policy_request_pending = False + self._policy_request_sequence_id: int | None = None + self._policy_request_deadline_s: float | None = None self._policy_hold_qpos: Float64Array | None = None self._policy_session_seq = 0 self._policy_observation_seq = 0 @@ -1533,9 +1537,25 @@ def _drain_high_level_policy_ipc(self) -> None: packet.session_id, ) return - # A request may already be in flight when the operator pauses. Drain - # its result without replacing the reference frozen at the B press. + packet_sequence_id = int(packet.source_sequence_id) + expected_sequence_id = self._policy_request_sequence_id + # A blocking request may finish after the operator pauses. Its result + # must not become the first chunk of a later resume cycle. if self._policy_paused and not self._policy_resume_pending: + if packet_sequence_id == expected_sequence_id: + self._clear_policy_request() + return + if ( + not self._policy_request_pending + or expected_sequence_id is None + or packet_sequence_id != expected_sequence_id + ): + logger.warning( + "Discarded unsolicited synchronous policy result: " + "expected_sequence=%r received_sequence=%d", + expected_sequence_id, + packet_sequence_id, + ) return scheduler = self._high_level_policy_scheduler policy_cfg = self._high_level_policy_cfg @@ -1568,20 +1588,16 @@ def _drain_high_level_policy_ipc(self) -> None: result_age_s, policy_cfg.max_result_age_s, ) + self._clear_policy_request() if self.mode == RobotMode.STANDING and self._policy_entry_pending: operator_logger.warning( "High-level policy entry failed; received a stale action result" ) self._enter_standing() - return - minimum_source_timestamp_ns = self._policy_resume_source_timestamp_ns - if ( - self._policy_resume_pending - and minimum_source_timestamp_ns is not None - and int(packet.source_onboard_monotonic_timestamp_ns) - < minimum_source_timestamp_ns - ): - logger.warning("Discarded pre-resume high-level policy action chunk") + else: + self._handle_high_level_policy_fault( + "received a stale synchronous action result" + ) return if self.mode == RobotMode.STANDING and not self._policy_entry_pending: return @@ -1601,24 +1617,29 @@ def _drain_high_level_policy_ipc(self) -> None: scheduler.accept(chunk, now_s=now_s) except ValueError as exc: logger.warning("Rejected high-level policy entry chunk: %s", exc) + self._clear_policy_request() operator_logger.warning( "High-level policy entry failed; remaining in STANDING" ) self._enter_standing() return + self._clear_policy_request() self._transition_to_high_level_policy() return try: scheduler.accept(chunk, now_s=now_s) except ValueError as exc: logger.warning("Rejected high-level policy action chunk: %s", exc) + self._clear_policy_request() + self._handle_high_level_policy_fault( + f"rejected synchronous action chunk: {exc}" + ) return + self._clear_policy_request() if self._policy_resume_pending: - scheduler.resume(now_s) self._policy_paused = False self._policy_resume_pending = False self._policy_resume_deadline_s = None - self._policy_resume_source_timestamp_ns = None operator_logger.info("fresh action chunk -> resume POLICY") def _handle_high_level_policy_transitions(self) -> None: @@ -1691,6 +1712,7 @@ def _start_high_level_policy_entry_session(self) -> None: getattr(state, "quat"), ) self._policy_session_id = uuid.uuid4().hex + self._latest_policy_status = None scheduler.reset( self._policy_session_id, initial_action=self._build_high_level_policy_boundary_action(state), @@ -1700,7 +1722,7 @@ def _start_high_level_policy_entry_session(self) -> None: self._policy_paused = False self._policy_resume_pending = False self._policy_resume_deadline_s = None - self._policy_resume_source_timestamp_ns = None + self._clear_policy_request() self._policy_hold_qpos = self._build_robot_state_qpos(state) self._policy_observation_seq = 0 self._last_policy_video_seq = ( @@ -1733,23 +1755,35 @@ def _publish_high_level_policy_session(self, command: str, *, repeat: bool = Fal ) self._last_policy_session_publish_s = now_s - def _publish_high_level_policy_observation(self, robot_state: object) -> None: + def _high_level_policy_worker_ready(self) -> bool: + status = self._latest_policy_status + return bool( + status is not None + and status.session_id == self._policy_session_id + and status.status == "ready" + ) + + def _publish_high_level_policy_observation(self, robot_state: object) -> bool: if not (self._policy_entry_pending or self.mode == RobotMode.POLICY): - return + return False if self._policy_paused and not self._policy_resume_pending: - return + return False + if self._policy_request_pending: + return False + if not self._high_level_policy_worker_ready(): + return False publisher = self._policy_control_pub frame = self._latest_policy_video transform = self._policy_frame_transform session_id = self._policy_session_id policy_cfg = self._high_level_policy_cfg if publisher is None or frame is None or transform is None or session_id is None or policy_cfg is None: - return + return False if int(frame.seq) <= self._last_policy_video_seq: - return + return False now_s = time.monotonic() if abs(now_s - float(frame.timestamp_s)) > policy_cfg.max_observation_age_s: - return + return False state = transform.localize_state(build_observation_state(robot_state)) sequence_id = self._policy_observation_seq publisher.publish( @@ -1765,6 +1799,17 @@ def _publish_high_level_policy_observation(self, robot_state: object) -> None: ) self._policy_observation_seq += 1 self._last_policy_video_seq = int(frame.seq) + self._policy_request_pending = True + self._policy_request_sequence_id = sequence_id + self._policy_request_deadline_s = ( + now_s + policy_cfg.timeout_s + policy_cfg.max_result_age_s + 0.1 + ) + return True + + def _clear_policy_request(self) -> None: + self._policy_request_pending = False + self._policy_request_sequence_id = None + self._policy_request_deadline_s = None def _transition_to_high_level_policy(self) -> None: state = self.robot.get_state() @@ -1778,7 +1823,6 @@ def _transition_to_high_level_policy(self) -> None: self._policy_paused = False self._policy_resume_pending = False self._policy_resume_deadline_s = None - self._policy_resume_source_timestamp_ns = None self.mode = RobotMode.POLICY operator_logger.info("mode -> POLICY") @@ -1793,17 +1837,18 @@ def _toggle_high_level_policy_pause(self) -> None: policy_cfg = self._high_level_policy_cfg if policy_cfg is None: return + scheduler.discard_chunk() + self._clear_policy_request() self._policy_resume_pending = True self._policy_resume_deadline_s = now_s + policy_cfg.entry_timeout_s - self._policy_resume_source_timestamp_ns = int(round(now_s * 1e9)) self._publish_high_level_policy_session("resume") operator_logger.info("B -> resume POLICY; waiting for a fresh action chunk") else: - scheduler.pause(now_s) + scheduler.discard_chunk() + self._clear_policy_request() self._policy_paused = True self._policy_resume_pending = False self._policy_resume_deadline_s = None - self._policy_resume_source_timestamp_ns = None self._policy_hold_qpos = self._resolve_mocap_hold_qpos() self._publish_high_level_policy_session("pause") operator_logger.info("B -> pause POLICY") @@ -1821,7 +1866,7 @@ def _stop_high_level_policy_session(self) -> None: self._policy_paused = False self._policy_resume_pending = False self._policy_resume_deadline_s = None - self._policy_resume_source_timestamp_ns = None + self._clear_policy_request() self._policy_hold_qpos = None self._latest_policy_status = None @@ -1833,11 +1878,11 @@ def _handle_high_level_policy_fault(self, detail: str) -> None: return scheduler = self._high_level_policy_scheduler if scheduler is not None: - scheduler.pause(time.monotonic()) + scheduler.discard_chunk() + self._clear_policy_request() self._policy_paused = True self._policy_resume_pending = False self._policy_resume_deadline_s = None - self._policy_resume_source_timestamp_ns = None self._policy_hold_qpos = self._resolve_mocap_hold_qpos() self._publish_high_level_policy_session("pause") operator_logger.warning( @@ -1910,9 +1955,15 @@ def _high_level_policy_step(self) -> None: if self._policy_resume_pending: robot_state = self.robot.get_state() self._publish_high_level_policy_observation(robot_state) + now_s = time.monotonic() deadline_s = self._policy_resume_deadline_s - if deadline_s is not None and time.monotonic() > deadline_s: + request_deadline_s = self._policy_request_deadline_s + if deadline_s is not None and now_s > deadline_s: self._handle_high_level_policy_fault("resume timed out waiting for a fresh action chunk") + elif request_deadline_s is not None and now_s > request_deadline_s: + self._handle_high_level_policy_fault( + "synchronous policy inference timed out" + ) hold_qpos = self._policy_hold_qpos if hold_qpos is None: hold_qpos = self._resolve_mocap_hold_qpos() @@ -1938,10 +1989,15 @@ def _high_level_policy_step(self) -> None: return robot_state = self.robot.get_state() - self._publish_high_level_policy_observation(robot_state) - scheduled = scheduler.sample(time.monotonic()) + now_s = time.monotonic() + scheduled = scheduler.sample(now_s) if scheduled is None: - self._handle_high_level_policy_fault("action watchdog expired") + self._publish_high_level_policy_observation(robot_state) + request_deadline_s = self._policy_request_deadline_s + if request_deadline_s is not None and now_s > request_deadline_s: + self._handle_high_level_policy_fault( + "synchronous policy inference timed out" + ) hold_qpos = self._policy_hold_qpos if hold_qpos is None: hold_qpos = self._resolve_mocap_hold_qpos() diff --git a/tests/test_high_level_policy.py b/tests/test_high_level_policy.py index 6a8d2d5c..212e467d 100644 --- a/tests/test_high_level_policy.py +++ b/tests/test_high_level_policy.py @@ -26,8 +26,8 @@ unpack_message, ) from teleopit.high_level_policy.scheduler import ( - HighLevelPolicyScheduler, PolicyFrameTransform, + SynchronousPolicyScheduler, closure_to_o6_pose, ) from teleopit.sim2real.mp.high_level_policy_runtime import ( @@ -38,13 +38,14 @@ _test_pattern, _validate_high_level_policy_runtime_config, ) -from teleopit.sim2real.mp.high_level_policy_worker import HighLevelPolicyWorker +from teleopit.sim2real.mp.high_level_policy_worker import SynchronousPolicyWorker from teleopit.sim2real.mp.messages import ( HighLevelPolicyActionPacket, HighLevelPolicySessionPacket, HighLevelPolicyStatusPacket, HighLevelPolicyTargetPacket, ModeStatePacket, + SharedFrameDescriptor, ) from teleopit.sim2real.mp.runtime import ( RobotMode, @@ -111,30 +112,30 @@ def _safe_chunk(actions: np.ndarray, *, source_s: float = 1.0, sequence: int = 0 ) -def test_high_level_policy_default_hold_covers_inference_and_transport_jitter() -> None: +def test_high_level_policy_config_has_no_async_chunk_options() -> None: config = parse_high_level_policy_config({"high_level_policy": {"task": "demo"}}) - assert config.hold_s == pytest.approx(3.0) - - -def test_high_level_policy_replan_steps_uses_protocol_horizon_limit() -> None: - config = parse_high_level_policy_config( - { - "high_level_policy": { - "task": "demo", - "replan_steps": MAX_ACTION_HORIZON, - } - } - ) + assert not hasattr(config, "replan_steps") + assert not hasattr(config, "hold_s") + for removed_name in ("replan_steps", "hold_s"): + with pytest.raises(ValueError, match=rf"{removed_name} was removed"): + parse_high_level_policy_config( + { + "high_level_policy": { + "task": "demo", + removed_name: 3, + } + } + ) - assert config.replan_steps == MAX_ACTION_HORIZON - with pytest.raises(ValueError, match=rf"\[1, {MAX_ACTION_HORIZON}\]"): +def test_high_level_policy_config_still_validates_request_timeout() -> None: + with pytest.raises(ValueError, match="timeout_s"): parse_high_level_policy_config( { "high_level_policy": { "task": "demo", - "replan_steps": MAX_ACTION_HORIZON + 1, + "timeout_s": 0.0, } } ) @@ -183,57 +184,56 @@ def test_policy_frame_transform_localizes_state_and_delocalizes_action() -> None np.testing.assert_allclose(transform.localize_body_action(world), body, atol=1e-6) -def test_scheduler_uses_source_timestamp_and_interpolates_at_30hz() -> None: - scheduler = HighLevelPolicyScheduler(hold_s=0.1) +def test_scheduler_starts_on_receipt_and_interpolates_at_30hz() -> None: + scheduler = SynchronousPolicyScheduler() scheduler.reset("session-1") scheduler.accept(_chunk(source_s=10.0), now_s=10.01) - halfway = scheduler.sample(10.0 + 0.5 / 30.0) + halfway = scheduler.sample(10.01 + 0.5 / 30.0) assert halfway is not None assert halfway[0] == pytest.approx(0.5) assert halfway[48] == pytest.approx(5.0) def test_scheduler_accepts_protocol_max_action_horizon() -> None: - scheduler = HighLevelPolicyScheduler(hold_s=0.1) + scheduler = SynchronousPolicyScheduler() scheduler.reset("session-1") scheduler.accept(_chunk(source_s=10.0, frames=MAX_ACTION_HORIZON), now_s=10.01) assert scheduler.has_chunk -def test_scheduler_pause_freezes_and_resume_shifts_plan_time() -> None: - scheduler = HighLevelPolicyScheduler(hold_s=0.1) +def test_scheduler_completes_chunk_before_accepting_the_next_one() -> None: + scheduler = SynchronousPolicyScheduler() scheduler.reset("session-1") scheduler.accept(_chunk(source_s=20.0), now_s=20.0) - scheduler.pause(20.02) - paused = scheduler.sample(25.0) - assert paused is not None - scheduler.resume(25.0) - resumed = scheduler.sample(25.0) - assert resumed is not None - np.testing.assert_allclose(resumed, paused) + with pytest.raises(ValueError, match="cannot replace an active synchronous"): + scheduler.accept(_chunk(source_s=20.01, sequence=1), now_s=20.01) + + assert scheduler.sample(20.0 + 3.0 / 30.0) is None + scheduler.accept(_chunk(source_s=25.0, sequence=1), now_s=25.0) + assert scheduler.has_chunk -def test_scheduler_rejects_wrong_session_and_expired_chunk() -> None: - scheduler = HighLevelPolicyScheduler(hold_s=0.0) +def test_scheduler_rejects_wrong_session_but_not_inference_latency() -> None: + scheduler = SynchronousPolicyScheduler() scheduler.reset("other") with pytest.raises(ValueError, match="session mismatch"): scheduler.accept(_chunk(source_s=1.0), now_s=1.0) scheduler.reset("session-1") - with pytest.raises(ValueError, match="already expired"): - scheduler.accept(_chunk(source_s=1.0), now_s=2.0) - - with pytest.raises(ValueError, match="in the future"): - scheduler.accept(_chunk(source_s=3.0), now_s=2.0) + scheduler.accept(_chunk(source_s=1.0), now_s=20.0) + first = scheduler.sample(20.0) + assert first is not None + assert first[0] == pytest.approx(0.0) def test_scheduler_rejects_nonincreasing_source_timestamp() -> None: - scheduler = HighLevelPolicyScheduler(hold_s=0.1) + scheduler = SynchronousPolicyScheduler() scheduler.reset("session-1") scheduler.accept(_chunk(source_s=1.0), now_s=1.0) + scheduler.discard_chunk() with pytest.raises(ValueError, match="source timestamp must increase"): scheduler.accept(_chunk(source_s=1.0, sequence=1), now_s=1.01) @@ -245,7 +245,7 @@ def test_linkerhand_closure_uses_hand_calibration() -> None: def test_scheduler_validates_complete_chunk_against_onboard_safety_limits() -> None: - scheduler = HighLevelPolicyScheduler(hold_s=0.1, safety=_safety_config()) + scheduler = SynchronousPolicyScheduler(safety=_safety_config()) initial = _safe_actions(1)[0] scheduler.reset("session-1", initial_action=initial) scheduler.accept(_safe_chunk(_safe_actions()), now_s=1.01) @@ -254,7 +254,7 @@ def test_scheduler_validates_complete_chunk_against_onboard_safety_limits() -> N def test_scheduler_accepts_internal_reference_discontinuities() -> None: - scheduler = HighLevelPolicyScheduler(hold_s=0.1, safety=_safety_config()) + scheduler = SynchronousPolicyScheduler(safety=_safety_config()) initial = _safe_actions(1)[0] initial[7] = 0.8 actions = _safe_actions() @@ -272,7 +272,7 @@ def test_scheduler_accepts_internal_reference_discontinuities() -> None: def test_scheduler_clips_joint_positions_to_onboard_limits() -> None: - scheduler = HighLevelPolicyScheduler(hold_s=0.1, safety=_safety_config()) + scheduler = SynchronousPolicyScheduler(safety=_safety_config()) scheduler.reset("session-1", initial_action=_safe_actions(1)[0]) actions = _safe_actions(1) actions[0, 7] = -3.08 @@ -281,7 +281,7 @@ def test_scheduler_clips_joint_positions_to_onboard_limits() -> None: scheduler.accept(_safe_chunk(actions), now_s=1.01) scheduled = None for _ in range(20): - scheduled = scheduler.sample(1.0) + scheduled = scheduler.sample(1.01) assert scheduled is not None assert scheduled[7] == pytest.approx(-3.0) @@ -289,15 +289,15 @@ def test_scheduler_clips_joint_positions_to_onboard_limits() -> None: def test_scheduler_clips_openneck_angles_to_onboard_limits() -> None: - scheduler = HighLevelPolicyScheduler(hold_s=0.1, safety=_safety_config()) + scheduler = SynchronousPolicyScheduler(safety=_safety_config()) scheduler.reset("session-1", initial_action=_safe_actions(1)[0]) actions = _safe_actions() actions[:, 48] = [-46.0, 0.0, 46.0] actions[:, 49] = [41.0, 0.0, -41.0] scheduler.accept(_safe_chunk(actions), now_s=1.01) - first_action = scheduler.sample(1.0) - final_action = scheduler.sample(1.0 + 2.0 / 30.0) + first_action = scheduler.sample(1.01) + final_action = scheduler.sample(1.01 + 2.0 / 30.0) assert first_action is not None assert first_action[48] == pytest.approx(-45.0) @@ -308,7 +308,7 @@ def test_scheduler_clips_openneck_angles_to_onboard_limits() -> None: def test_scheduler_rejects_joint_projection_above_limit() -> None: - scheduler = HighLevelPolicyScheduler(hold_s=0.1, safety=_safety_config()) + scheduler = SynchronousPolicyScheduler(safety=_safety_config()) scheduler.reset("session-1", initial_action=_safe_actions(1)[0]) actions = _safe_actions(1) actions[0, 7] = -3.11 @@ -319,7 +319,7 @@ def test_scheduler_rejects_joint_projection_above_limit() -> None: def test_scheduler_rejects_entire_unsafe_non_joint_chunk() -> None: - scheduler = HighLevelPolicyScheduler(hold_s=0.1, safety=_safety_config()) + scheduler = SynchronousPolicyScheduler(safety=_safety_config()) scheduler.reset("session-1", initial_action=_safe_actions(1)[0]) actions = _safe_actions() actions[1, 2] = 0.4 @@ -330,8 +330,7 @@ def test_scheduler_rejects_entire_unsafe_non_joint_chunk() -> None: def test_scheduler_accepts_discontinuous_plan_and_rate_limits_output_at_50hz() -> None: - scheduler = HighLevelPolicyScheduler( - hold_s=0.1, + scheduler = SynchronousPolicyScheduler( safety=_safety_config(), output_hz=50.0, ) @@ -342,7 +341,7 @@ def test_scheduler_accepts_discontinuous_plan_and_rate_limits_output_at_50hz() - actions[1, 7] = 0.5 yaw = 0.2 actions[1, 3:7] = [math.cos(yaw / 2.0), 0.0, 0.0, math.sin(yaw / 2.0)] - scheduler.accept(_safe_chunk(actions), now_s=1.01) + scheduler.accept(_safe_chunk(actions), now_s=1.0) output = scheduler.sample(1.0 + 1.0 / 30.0) @@ -648,7 +647,6 @@ def test_policy_transition_after_first_chunk_does_not_start_kp_ramp() -> None: worker._policy_paused = True worker._policy_resume_pending = False worker._policy_resume_deadline_s = None - worker._policy_resume_source_timestamp_ns = None worker._standing_return_ramp_duration = 0.5 worker._standing_return_kp_ramp_floor_ratio = 0.5 worker._safety = SimpleNamespace( @@ -689,7 +687,9 @@ def test_policy_entry_rejects_action_received_after_deadline() -> None: worker._policy_session_id = "session-1" worker._policy_paused = False worker._policy_resume_pending = False - worker._policy_resume_source_timestamp_ns = None + worker._policy_request_pending = True + worker._policy_request_sequence_id = 1 + worker._policy_request_deadline_s = now_s + 1.0 worker._policy_entry_pending = True worker._policy_entry_deadline_s = now_s - 0.01 accepted: list[object] = [] @@ -732,7 +732,9 @@ def test_policy_entry_first_chunk_uses_measured_reference_boundary() -> None: worker._policy_session_id = "session-1" worker._policy_paused = False worker._policy_resume_pending = False - worker._policy_resume_source_timestamp_ns = None + worker._policy_request_pending = True + worker._policy_request_sequence_id = 1 + worker._policy_request_deadline_s = now_s + 1.0 worker._policy_entry_pending = True worker._policy_entry_deadline_s = now_s + 1.0 worker._policy_frame_transform = PolicyFrameTransform.from_robot_pose( @@ -746,7 +748,7 @@ def test_policy_entry_first_chunk_uses_measured_reference_boundary() -> None: state = SimpleNamespace() worker.robot = SimpleNamespace(get_state=lambda: state) worker._build_robot_state_qpos = lambda _state: current_qpos.copy() - scheduler = HighLevelPolicyScheduler(hold_s=0.1, safety=_safety_config()) + scheduler = SynchronousPolicyScheduler(safety=_safety_config()) boundary_action = worker._build_high_level_policy_boundary_action(state) scheduler.reset("session-1", initial_action=boundary_action) worker._high_level_policy_scheduler = scheduler @@ -759,7 +761,7 @@ def test_policy_entry_first_chunk_uses_measured_reference_boundary() -> None: worker._drain_high_level_policy_ipc() - scheduled = scheduler.sample(now_s) + scheduled = scheduler.sample(time.monotonic()) assert boundary_action[7] == pytest.approx(0.8) assert current_qpos[7] == pytest.approx(0.8) assert transitions == ["policy"] @@ -791,7 +793,9 @@ def test_policy_entry_stale_result_aborts_current_session() -> None: worker._policy_session_id = "session-1" worker._policy_paused = False worker._policy_resume_pending = False - worker._policy_resume_source_timestamp_ns = None + worker._policy_request_pending = True + worker._policy_request_sequence_id = 1 + worker._policy_request_deadline_s = now_s + 1.0 worker._policy_entry_pending = True worker._policy_entry_deadline_s = now_s + 1.0 worker._high_level_policy_scheduler = SimpleNamespace() @@ -843,7 +847,8 @@ def test_high_level_policy_body_action_uses_existing_tracker_without_second_alig worker._policy_paused = False worker._policy_resume_pending = False worker.robot = SimpleNamespace(get_state=lambda: SimpleNamespace()) - worker._publish_high_level_policy_observation = lambda _state: None + published_observations: list[object] = [] + worker._publish_high_level_policy_observation = published_observations.append worker._policy_control_pub = None worker._policy_hold_qpos = None calls: list[tuple[np.ndarray, dict[str, object]]] = [] @@ -862,6 +867,100 @@ def execute(reference, _state, **kwargs) -> None: # type: ignore[no-untyped-def "align_reference": False, "compose_arms": False, } + assert published_observations == [] + + +def test_synchronous_policy_publishes_only_one_outstanding_observation() -> None: + worker = object.__new__(_RobotControlWorker) + now_s = time.monotonic() + worker.mode = RobotMode.POLICY + worker._policy_entry_pending = False + worker._policy_paused = False + worker._policy_resume_pending = False + worker._policy_request_pending = False + worker._policy_request_sequence_id = None + worker._policy_request_deadline_s = None + worker._policy_session_id = "session-1" + worker._latest_policy_status = HighLevelPolicyStatusPacket( + session_id="session-1", + status="ready", + detail="host policy session reset", + timestamp_s=now_s, + seq=1, + ) + worker._policy_observation_seq = 7 + worker._last_policy_video_seq = 9 + worker._latest_policy_video = SharedFrameDescriptor( + shm_name="frame", + slot=0, + seq=10, + timestamp_s=now_s, + shape=(480, 640, 3), + dtype="uint8", + slots=3, + ) + worker._policy_frame_transform = PolicyFrameTransform.from_robot_pose( + [0.0, 0.0], + [1.0, 0.0, 0.0, 0.0], + ) + worker._high_level_policy_cfg = SimpleNamespace( + task="demo", + timeout_s=1.0, + max_observation_age_s=0.15, + max_result_age_s=0.1, + ) + published: list[tuple[str, object]] = [] + worker._policy_control_pub = SimpleNamespace( + publish=lambda topic, packet: published.append((topic, packet)) + ) + robot_state = SimpleNamespace( + qpos=np.zeros(29, dtype=np.float32), + qvel=np.zeros(29, dtype=np.float32), + quat=np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float32), + ang_vel=np.zeros(3, dtype=np.float32), + ) + + assert worker._publish_high_level_policy_observation(robot_state) + assert not worker._publish_high_level_policy_observation(robot_state) + assert len(published) == 1 + assert worker._policy_request_pending + assert worker._policy_request_sequence_id == 7 + + +@pytest.mark.parametrize( + "status", + [ + None, + HighLevelPolicyStatusPacket( + session_id="session-1", + status="connecting", + detail="connecting to host policy", + timestamp_s=1.0, + seq=1, + ), + HighLevelPolicyStatusPacket( + session_id="other-session", + status="ready", + detail="host policy session reset", + timestamp_s=1.0, + seq=2, + ), + ], +) +def test_synchronous_policy_waits_for_current_session_ready_status( + status: HighLevelPolicyStatusPacket | None, +) -> None: + worker = object.__new__(_RobotControlWorker) + worker.mode = RobotMode.STANDING + worker._policy_entry_pending = True + worker._policy_paused = False + worker._policy_resume_pending = False + worker._policy_request_pending = False + worker._policy_session_id = "session-1" + worker._latest_policy_status = status + + assert not worker._publish_high_level_policy_observation(SimpleNamespace()) + assert not worker._policy_request_pending def test_policy_and_pico_remote_b_both_toggle_pause() -> None: @@ -887,13 +986,12 @@ def test_policy_and_pico_remote_b_both_toggle_pause() -> None: def test_policy_worker_pause_resume_retransmission_is_idempotent() -> None: - worker = object.__new__(HighLevelPolicyWorker) + worker = object.__new__(SynchronousPolicyWorker) worker._last_session_seq = -1 worker._active_session = None worker._ready = False worker._paused = False worker._last_observation_seq = -1 - worker._last_request_timestamp_ns = None worker._next_connect_time_s = 0.0 worker._new_session_required = False statuses: list[str] = [] @@ -919,13 +1017,12 @@ def packet(command: str, seq: int) -> HighLevelPolicySessionPacket: def test_policy_worker_resume_reconnects_faulted_current_session() -> None: - worker = object.__new__(HighLevelPolicyWorker) + worker = object.__new__(SynchronousPolicyWorker) worker._last_session_seq = -1 worker._active_session = None worker._ready = False worker._paused = False worker._last_observation_seq = -1 - worker._last_request_timestamp_ns = None worker._next_connect_time_s = 0.0 worker._new_session_required = False statuses: list[str] = [] @@ -957,10 +1054,12 @@ def test_policy_fault_uses_normal_pause_without_entering_standing() -> None: worker._policy_paused = False worker._policy_resume_pending = False worker._policy_resume_deadline_s = None - worker._policy_resume_source_timestamp_ns = None - paused: list[float] = [] + worker._policy_request_pending = True + worker._policy_request_sequence_id = 2 + worker._policy_request_deadline_s = time.monotonic() + 1.0 + discarded: list[str] = [] worker._high_level_policy_scheduler = SimpleNamespace( - pause=lambda now_s: paused.append(float(now_s)) + discard_chunk=lambda: discarded.append("discard") ) hold_qpos = np.arange(36, dtype=np.float64) worker._resolve_mocap_hold_qpos = lambda: hold_qpos.copy() @@ -973,29 +1072,31 @@ def test_policy_fault_uses_normal_pause_without_entering_standing() -> None: assert worker.mode == RobotMode.POLICY assert worker._policy_paused assert not worker._policy_resume_pending - assert len(paused) == 1 + assert discarded == ["discard"] + assert not worker._policy_request_pending assert published == ["pause"] np.testing.assert_array_equal(worker._policy_hold_qpos, hold_qpos) -def test_policy_watchdog_pauses_and_holds_last_reference() -> None: +def test_synchronous_policy_wait_holds_last_reference_without_pausing() -> None: worker = object.__new__(_RobotControlWorker) worker.high_level_policy_enabled = True worker.mode = RobotMode.POLICY worker._policy_paused = False worker._policy_resume_pending = False worker._policy_resume_deadline_s = None - worker._policy_resume_source_timestamp_ns = None - paused: list[float] = [] worker._high_level_policy_scheduler = SimpleNamespace( sample=lambda _now_s: None, - pause=lambda now_s: paused.append(float(now_s)), ) worker._policy_frame_transform = SimpleNamespace() worker._policy_session_id = "session-1" worker.robot = SimpleNamespace(get_state=lambda: SimpleNamespace()) worker._publish_high_level_policy_observation = lambda _state: None + worker._policy_request_pending = True + worker._policy_request_sequence_id = 1 + worker._policy_request_deadline_s = time.monotonic() + 1.0 hold_qpos = np.arange(36, dtype=np.float64) + worker._policy_hold_qpos = hold_qpos.copy() worker._last_commanded_motion_qpos = hold_qpos.copy() worker._last_retarget_qpos = None worker._publish_high_level_policy_session = lambda _command: None @@ -1005,9 +1106,8 @@ def test_policy_watchdog_pauses_and_holds_last_reference() -> None: worker._high_level_policy_step() assert worker.mode == RobotMode.POLICY - assert worker._policy_paused + assert not worker._policy_paused assert not worker._policy_resume_pending - assert len(paused) == 1 assert len(held) == 1 np.testing.assert_array_equal(held[0], hold_qpos) @@ -1045,12 +1145,14 @@ def test_policy_pause_resume_waits_for_fresh_chunk_then_resumes() -> None: worker._policy_paused = True worker._policy_resume_pending = False worker._policy_resume_deadline_s = None - worker._policy_resume_source_timestamp_ns = None - resumed: list[float] = [] + worker._policy_request_pending = True + worker._policy_request_sequence_id = 0 + worker._policy_request_deadline_s = time.monotonic() + 1.0 + discarded: list[str] = [] accepted: list[object] = [] worker._high_level_policy_scheduler = SimpleNamespace( accept=lambda *args, **kwargs: accepted.append((args, kwargs)), - resume=lambda now_s: resumed.append(float(now_s)), + discard_chunk=lambda: discarded.append("discard"), ) worker._high_level_policy_cfg = SimpleNamespace( entry_timeout_s=1.0, @@ -1064,9 +1166,13 @@ def test_policy_pause_resume_waits_for_fresh_chunk_then_resumes() -> None: assert worker._policy_paused assert worker._policy_resume_pending assert session_commands == ["resume"] + assert discarded == ["discard"] + assert not worker._policy_request_pending - source_timestamp_ns = worker._policy_resume_source_timestamp_ns - assert source_timestamp_ns is not None + source_timestamp_ns = int(round(time.monotonic() * 1e9)) + worker._policy_request_pending = True + worker._policy_request_sequence_id = 1 + worker._policy_request_deadline_s = time.monotonic() + 1.0 worker._policy_video_sub = SimpleNamespace(recv_latest=lambda: None) worker._policy_status_sub = SimpleNamespace(recv_latest=lambda: None) worker._policy_action_sub = SimpleNamespace( @@ -1087,9 +1193,9 @@ def test_policy_pause_resume_waits_for_fresh_chunk_then_resumes() -> None: worker._drain_high_level_policy_ipc() assert len(accepted) == 1 - assert len(resumed) == 1 assert not worker._policy_paused assert not worker._policy_resume_pending + assert not worker._policy_request_pending def test_policy_resume_rejects_action_received_after_deadline() -> None: @@ -1116,11 +1222,12 @@ def test_policy_resume_rejects_action_received_after_deadline() -> None: worker._policy_paused = True worker._policy_resume_pending = True worker._policy_resume_deadline_s = now_s - 0.01 - worker._policy_resume_source_timestamp_ns = int(round((now_s - 0.1) * 1e9)) + worker._policy_request_pending = True + worker._policy_request_sequence_id = 1 + worker._policy_request_deadline_s = now_s + 1.0 accepted: list[object] = [] worker._high_level_policy_scheduler = SimpleNamespace( accept=lambda *args, **kwargs: accepted.append((args, kwargs)), - resume=lambda _now_s: pytest.fail("expired resume must not resume scheduler"), ) worker._high_level_policy_cfg = SimpleNamespace(max_result_age_s=1.0) faults: list[str] = [] @@ -1154,6 +1261,9 @@ def test_paused_robot_worker_discards_inflight_policy_result() -> None: worker._policy_session_id = "session-1" worker._policy_paused = True worker._policy_resume_pending = False + worker._policy_request_pending = True + worker._policy_request_sequence_id = 1 + worker._policy_request_deadline_s = 2.0 accepted: list[object] = [] worker._high_level_policy_scheduler = SimpleNamespace( accept=lambda *args, **kwargs: accepted.append((args, kwargs)) @@ -1163,3 +1273,4 @@ def test_paused_robot_worker_discards_inflight_policy_result() -> None: worker._drain_high_level_policy_ipc() assert accepted == [] + assert not worker._policy_request_pending From e3084f7d52055c4a97ea19d381a19ea84eb5da8b Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Sat, 25 Jul 2026 21:25:08 +0800 Subject: [PATCH 41/59] Move Pico bridge diagnostic to dev scripts --- .../check_pico_signal.py => dev/test_pico_bridge.py} | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) rename scripts/{run/check_pico_signal.py => dev/test_pico_bridge.py} (96%) diff --git a/scripts/run/check_pico_signal.py b/scripts/dev/test_pico_bridge.py similarity index 96% rename from scripts/run/check_pico_signal.py rename to scripts/dev/test_pico_bridge.py index d87e884a..d48c14fc 100644 --- a/scripts/run/check_pico_signal.py +++ b/scripts/dev/test_pico_bridge.py @@ -1,4 +1,4 @@ -"""Pico mocap/video signal diagnostic entry point.""" +"""Pico Bridge mocap/video diagnostic entry point.""" from __future__ import annotations @@ -20,7 +20,7 @@ from teleopit.runtime.common import cfg_get -logger = logging.getLogger("teleopit.tools.check_pico_signal") +logger = logging.getLogger("teleopit.tools.test_pico_bridge") def _fmt_vec(values: tuple[float, ...] | None) -> str: @@ -94,7 +94,7 @@ def _log_summary( ) -> None: if total <= 0: logger.info( - "Pico signal summary | window=%.1fs samples=0 provider_fps=%.1f " + "Pico Bridge summary | window=%.1fs samples=0 provider_fps=%.1f " "last_seq=%s video_frames=%d", window_s, provider_fps, @@ -110,7 +110,7 @@ def _log_summary( min_pos = last_stats.get("min_pos") max_pos = last_stats.get("max_pos") logger.info( - "Pico signal summary | window=%.1fs samples=%d valid=%d invalid=%d reasons=%s " + "Pico Bridge summary | window=%.1fs samples=%d valid=%d invalid=%d reasons=%s " "provider_fps=%.1f last_seq=%s last_age_ms=%s video_frames=%d " "max_abs_pos=%s pelvis=%s extent=%s min=%s max=%s quat_norm=[%s,%s]", window_s, @@ -201,7 +201,7 @@ def main(cfg: DictConfig) -> None: summary_interval_s = float(cfg_get(diag_cfg, "summary_interval_s", 1.0)) duration_s = float(cfg_get(diag_cfg, "duration_s", 0.0)) - logger.info("Starting Pico signal diagnostic") + logger.info("Starting Pico Bridge diagnostic") logger.info( "Pico bridge | host=%s port=%s discovery=%s advertise_ip=%s", cfg_get(input_cfg, "bridge_host", "0.0.0.0"), @@ -286,7 +286,7 @@ def main(cfg: DictConfig) -> None: video_start_done = None stop_event.wait(timeout=sleep_s) except KeyboardInterrupt: - logger.info("KeyboardInterrupt -- stopping Pico signal diagnostic") + logger.info("KeyboardInterrupt -- stopping Pico Bridge diagnostic") finally: video_runtime.stop() provider.close() From 69d55418b6be8f7f15e8b3437463a056815ab127 Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Sat, 25 Jul 2026 21:36:13 +0800 Subject: [PATCH 42/59] Update Pico and G1 bridge diagnostics --- ...test_bridge_state.py => test_g1_bridge.py} | 2 +- scripts/dev/test_pico_bridge.py | 146 +++++++++++++----- 2 files changed, 106 insertions(+), 42 deletions(-) rename scripts/dev/{test_bridge_state.py => test_g1_bridge.py} (97%) diff --git a/scripts/dev/test_bridge_state.py b/scripts/dev/test_g1_bridge.py similarity index 97% rename from scripts/dev/test_bridge_state.py rename to scripts/dev/test_g1_bridge.py index 7cae1df2..b7d05e98 100644 --- a/scripts/dev/test_bridge_state.py +++ b/scripts/dev/test_g1_bridge.py @@ -1,5 +1,5 @@ #!/usr/bin/env python3 -"""Diagnostic: compare C++ bridge state vs Python SDK state.""" +"""Compare G1 C++ bridge state with Unitree Python SDK state.""" import sys import time import copy diff --git a/scripts/dev/test_pico_bridge.py b/scripts/dev/test_pico_bridge.py index d48c14fc..2d7f25a6 100644 --- a/scripts/dev/test_pico_bridge.py +++ b/scripts/dev/test_pico_bridge.py @@ -1,27 +1,48 @@ +#!/usr/bin/env python3 """Pico Bridge mocap/video diagnostic entry point.""" from __future__ import annotations +import argparse from collections import Counter import logging import os +from pathlib import Path import signal +import sys import time import threading from typing import Any -import hydra import numpy as np -from omegaconf import DictConfig -from teleopit.inputs.human_frame_validation import HumanFrameValidationResult, validate_human_frame -from teleopit.inputs.pico4_provider import Pico4InputProvider -from teleopit.inputs.pico_video import PicoVideoRuntime, bridge_video_source, parse_pico_video_config -from teleopit.runtime.common import cfg_get + +REPO_ROOT = Path(__file__).resolve().parents[2] +sys.path.insert(0, str(REPO_ROOT)) + +from teleopit.inputs.human_frame_validation import ( # noqa: E402 + HumanFrameValidationResult, + validate_human_frame, +) +from teleopit.inputs.pico4_provider import Pico4InputProvider # noqa: E402 +from teleopit.inputs.pico_video import ( # noqa: E402 + PicoVideoConfig, + PicoVideoRuntime, + bridge_video_source, +) logger = logging.getLogger("teleopit.tools.test_pico_bridge") +DEFAULT_BRIDGE_HOST = "0.0.0.0" +DEFAULT_BRIDGE_PORT = 63901 +DEFAULT_VIDEO_SOURCE = "realsense" +DEFAULT_VIDEO_WIDTH = 1280 +DEFAULT_VIDEO_HEIGHT = 720 +DEFAULT_VIDEO_FPS = 30 +DEFAULT_POLL_HZ = 120.0 +DEFAULT_SUMMARY_INTERVAL_S = 1.0 + def _fmt_vec(values: tuple[float, ...] | None) -> str: if values is None: @@ -145,24 +166,72 @@ def _fmt_float(value: Any) -> str: return f"{float(value):.4f}" -def _build_provider(cfg: DictConfig, video_enabled: bool) -> Pico4InputProvider: - input_cfg = cfg_get(cfg, "input", {}) or {} - video_cfg = parse_pico_video_config(input_cfg) +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser( + description="Test Pico Bridge body tracking and RealSense video streaming", + ) + parser.add_argument("--bridge-host", default=DEFAULT_BRIDGE_HOST) + parser.add_argument("--bridge-port", type=int, default=DEFAULT_BRIDGE_PORT) + parser.add_argument( + "--bridge-discovery", + action=argparse.BooleanOptionalAction, + default=True, + ) + parser.add_argument("--bridge-advertise-ip", default=None) + parser.add_argument( + "--video", + action=argparse.BooleanOptionalAction, + default=True, + help="Stream video to Pico; enabled by default. Use --no-video to disable it.", + ) + parser.add_argument( + "--video-source", + choices=["realsense", "test-pattern"], + default=DEFAULT_VIDEO_SOURCE, + ) + parser.add_argument("--video-width", type=int, default=DEFAULT_VIDEO_WIDTH) + parser.add_argument("--video-height", type=int, default=DEFAULT_VIDEO_HEIGHT) + parser.add_argument("--video-fps", type=int, default=DEFAULT_VIDEO_FPS) + parser.add_argument("--video-device", default=None) + parser.add_argument( + "--duration-s", + type=float, + default=0.0, + help="Diagnostic duration; 0 means until Ctrl-C.", + ) + args = parser.parse_args() + + if not 1 <= args.bridge_port <= 65535: + parser.error("--bridge-port must be in [1, 65535]") + if args.video and (args.video_width <= 0 or args.video_height <= 0 or args.video_fps <= 0): + parser.error("--video-width, --video-height, and --video-fps must be > 0") + if args.duration_s < 0.0: + parser.error("--duration-s must be >= 0") + return args + + +def _make_video_config(args: argparse.Namespace) -> PicoVideoConfig: + return PicoVideoConfig( + enabled=bool(args.video), + source=str(args.video_source) if args.video else None, + width=int(args.video_width), + height=int(args.video_height), + fps=int(args.video_fps), + device=None if args.video_device in (None, "", "null") else str(args.video_device), + ) + + +def _build_provider(args: argparse.Namespace, video_cfg: PicoVideoConfig) -> Pico4InputProvider: return Pico4InputProvider( - human_format=str(cfg_get(input_cfg, "human_format", "pico_bridge")), - timeout=float(cfg_get(input_cfg, "pico4_timeout", 60.0)), - buffer_size=int(cfg_get(input_cfg, "pico4_buffer_size", 60)), - timestamp_gap_reset_s=float(cfg_get(input_cfg, "pico4_timestamp_gap_reset_s", 0.15)), - pause_button=cfg_get(input_cfg, "pause_button", "A"), - pause_debounce_s=float(cfg_get(input_cfg, "pause_debounce_s", 0.25)), - bridge_host=str(cfg_get(input_cfg, "bridge_host", "0.0.0.0")), - bridge_port=int(cfg_get(input_cfg, "bridge_port", 63901)), - bridge_discovery=bool(cfg_get(input_cfg, "bridge_discovery", True)), - bridge_advertise_ip=cfg_get(input_cfg, "bridge_advertise_ip", None), + human_format="pico_bridge", + pause_button=None, + arms_button=None, + bridge_host=str(args.bridge_host), + bridge_port=int(args.bridge_port), + bridge_discovery=bool(args.bridge_discovery), + bridge_advertise_ip=args.bridge_advertise_ip, bridge_video=bridge_video_source(video_cfg), - bridge_video_enabled=video_enabled, - bridge_start_timeout=float(cfg_get(input_cfg, "bridge_start_timeout", 10.0)), - bridge_history_size=int(cfg_get(input_cfg, "bridge_history_size", 120)), + bridge_video_enabled=video_cfg.enabled, ) @@ -191,37 +260,32 @@ def _run() -> None: return done -@hydra.main(version_base=None, config_path="../../teleopit/configs", config_name="pico4_sim2real") -def main(cfg: DictConfig) -> None: +def main() -> None: logging.basicConfig(level=logging.INFO, format="%(levelname)s:%(name)s:%(message)s") - input_cfg = cfg_get(cfg, "input", {}) or {} - video_cfg = parse_pico_video_config(input_cfg) - diag_cfg = cfg_get(cfg, "diagnostic", {}) or {} - poll_hz = float(cfg_get(diag_cfg, "poll_hz", cfg_get(cfg_get(cfg, "runtime", {}) or {}, "pico_input_hz", 120.0))) - summary_interval_s = float(cfg_get(diag_cfg, "summary_interval_s", 1.0)) - duration_s = float(cfg_get(diag_cfg, "duration_s", 0.0)) + args = parse_args() + video_cfg = _make_video_config(args) logger.info("Starting Pico Bridge diagnostic") logger.info( "Pico bridge | host=%s port=%s discovery=%s advertise_ip=%s", - cfg_get(input_cfg, "bridge_host", "0.0.0.0"), - cfg_get(input_cfg, "bridge_port", 63901), - cfg_get(input_cfg, "bridge_discovery", True), - cfg_get(input_cfg, "bridge_advertise_ip", None), + args.bridge_host, + args.bridge_port, + args.bridge_discovery, + args.bridge_advertise_ip, ) logger.info( "Signal check | validation=finite_values poll_hz=%.1f summary_interval_s=%.1f " "duration_s=%s video_enabled=%s video_source=%s", - poll_hz, - summary_interval_s, - f"{duration_s:.1f}" if duration_s > 0.0 else "until Ctrl-C", + DEFAULT_POLL_HZ, + DEFAULT_SUMMARY_INTERVAL_S, + f"{args.duration_s:.1f}" if args.duration_s > 0.0 else "until Ctrl-C", video_cfg.enabled, video_cfg.source, ) stop_event = threading.Event() _install_signal_handlers(stop_event) - provider = _build_provider(cfg, video_cfg.enabled) + provider = _build_provider(args, video_cfg) video_runtime = PicoVideoRuntime(provider=provider, config=video_cfg) total = 0 valid = 0 @@ -231,7 +295,7 @@ def main(cfg: DictConfig) -> None: last_stats: dict[str, Any] = {} window_start_s = time.monotonic() start_s = window_start_s - sleep_s = 1.0 / max(poll_hz, 1.0) + sleep_s = 1.0 / DEFAULT_POLL_HZ video_start_done: threading.Event | None = None try: @@ -240,7 +304,7 @@ def main(cfg: DictConfig) -> None: video_start_done = _start_video_runtime_async(video_runtime) while not stop_event.is_set(): now = time.monotonic() - if duration_s > 0.0 and now - start_s >= duration_s: + if args.duration_s > 0.0 and now - start_s >= args.duration_s: break video_runtime.tick() @@ -264,7 +328,7 @@ def main(cfg: DictConfig) -> None: _log_invalid(seq, last_age_ms, result) now = time.monotonic() - if now - window_start_s >= summary_interval_s: + if now - window_start_s >= DEFAULT_SUMMARY_INTERVAL_S: _log_summary( window_s=now - window_start_s, total=total, From f5a4ed038fa6bacc33d182b80c802eb37323358c Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Mon, 27 Jul 2026 17:40:07 +0800 Subject: [PATCH 43/59] Restore asynchronous high-level policy replanning --- AGENTS.md | 4 +- README.md | 23 +- docs/docs/configuration/config-reference.md | 11 +- .../tutorials/high-level-policy-sim2real.md | 66 ++-- .../current/configuration/config-reference.md | 9 +- .../tutorials/high-level-policy-sim2real.md | 49 +-- .../configs/high_level_policy_sim2real.yaml | 2 + teleopit/high_level_policy/__init__.py | 4 +- teleopit/high_level_policy/config.py | 20 +- teleopit/high_level_policy/scheduler.py | 79 ++-- .../sim2real/mp/high_level_policy_runtime.py | 4 +- .../sim2real/mp/high_level_policy_worker.py | 35 +- teleopit/sim2real/mp/runtime.py | 126 ++----- tests/test_high_level_policy.py | 345 ++++++++---------- 14 files changed, 379 insertions(+), 398 deletions(-) diff --git a/AGENTS.md b/AGENTS.md index 211fbeb2..39a6ba4d 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -183,7 +183,7 @@ target_dof_pos = clip(action, -10, 10) × action_scale + default_dof_pos - The host LeRobot/ReplayPolicy service runs in the separate `lerobot-teleopit` repository and environment; Teleopit must not depend on LeRobot, Transformers, or host policy classes - The current client/server code and protocol tests define the ZeroMQ request/response structure. During active development, Teleopit and `lerobot-teleopit` must update that structure together; no legacy network envelope is supported - The only shared data file is `hand_calibration.json`, which contains the LinkerHand O6 open/close calibration and must stay identical in both repositories -- The host boundary uses ZeroMQ REQ/REP with msgpack and non-pickle float32 arrays. Policy inference is strictly chunk-synchronous: publish one observation, hold the last safe reference while the isolated client worker blocks for one response, execute the complete returned chunk at 30 Hz, then publish the next observation. There is no overlapping request, latency-based frame skipping, or async compatibility mode; process isolation keeps the 50 Hz robot loop running +- The host boundary uses ZeroMQ REQ/REP with msgpack and non-pickle float32 arrays. Deployment is asynchronous and receding-horizon: the isolated client worker keeps at most one request in flight, submits the latest eligible observation every `high_level_policy.replan_steps` 30 Hz source frames, and leaves the current plan executing while inference runs. The onboard scheduler uses the echoed monotonic observation timestamp to skip elapsed source frames and replace the active plan when a newer response arrives; process isolation keeps the 50 Hz robot loop running - Policy observation is RGB JPEG plus `observation.state(68)`; only `state[58:62]` is rotated into the session-local yaw frame - Canonical action is `float32[T,50]` with protocol horizon `T` in `[1,50]`: local root `xyz(3)` + local root quaternion `wxyz(4)` + G1 joint reference `29` + left/right O6 closure `12` + OpenNeck yaw/pitch degrees `2` - The body reference `[0:36]` is yaw/XY-delocalized once and sent through the existing motion tracker. It is never sent directly as a G1 motor command and must not pass through the mocap alignment a second time @@ -191,7 +191,7 @@ target_dof_pos = clip(action, -10, 10) × action_scale + default_dof_pos - Unitree remote controls are `Start -> STANDING`, `Y -> request POLICY`, `B -> pause/resume`, `X -> STANDING/cancel pending`, and `L1+R1 -> DAMPING` - Policy pause freezes the scheduler/body reference and holds the latest hand/neck command. Leaving `POLICY` opens LinkerHand and centers OpenNeck - The onboard scheduler clips G1 joint references to `real_robot.joint_pos_lower/upper` when the required correction is at most `high_level_policy.safety.max_joint_projection_rad` (default `0.1` rad), and clips OpenNeck yaw/pitch commands to their configured degree ranges. It rejects whole chunks on excessive joint correction, shape/finiteness, session/sequence, quaternion, absolute root height, hand closure, or staleness failures. It accepts temporal root, yaw, and joint-reference discontinuities at entry, inside chunks, and across chunks because recorded pause/resume transitions may be discontinuous; accepted root translation, yaw, and joint output is rate-limited at 50 Hz. Never pad or trim malformed host output, and do not clip other out-of-range fields into validity -- Finishing a chunk normally starts the next synchronous request and holds its final reference during inference. A request timeout, host/network failure, invalid result, or loss of a required camera/client worker puts `POLICY` into the same resumable pause state used by remote `B`. The runtime never enters `STANDING` automatically; `B` resumes after a fresh valid chunk is available, while `X` remains the manual transition to `STANDING` +- A newer timestamp-aligned chunk normally replaces the active plan before its horizon ends. If inference runs longer, the scheduler holds the plan's final reference for `high_level_policy.hold_s`; exhausting that grace period triggers the action watchdog. A request timeout, host/network failure, watchdog expiry, or loss of a required camera/client worker puts `POLICY` into the same resumable pause state used by remote `B`; an invalid result is rejected while the last valid plan remains available. The runtime never enters `STANDING` automatically; `B` resumes after a fresh valid chunk is available, while `X` remains the manual transition to `STANDING` - Initial production hardware support requires two LinkerHand O6 hands and OpenNeck because all 50 canonical action dimensions are active - OpenNeck 0.2.0 is the supported runtime; Teleopit sends physical degrees through `move_deg()`, and the direct-drive OpenNeck package converts degrees to servo steps and clips them to its calibrated mechanical limits; the removed normalized API and config fields are unsupported - OpenNeck maps the independent HMD `PicoFrame.head.rotation` relative to the same-frame full-body `Body.Spine3` orientation; it must never use the full-body `Body.Head` skeleton joint for neck control, and HMD pose updates must remain independent of duplicate-body-frame filtering diff --git a/README.md b/README.md index 95cefb6d..11e650ba 100644 --- a/README.md +++ b/README.md @@ -142,10 +142,11 @@ dedicated onboard runtime. The host policy remains in the independent reference chunks over ZeroMQ, validates and interpolates them onboard, and rate-limits plan switches at 50 Hz before passing the 36D body reference through the existing motion tracker. The host never sends G1 motor commands. -Inference is chunk-synchronous: Teleopit sends one observation, holds the final -safe reference while that request completes, executes the complete returned -chunk at 30 Hz, and only then sends the next observation. Requests and action -chunks never overlap. +Inference is asynchronous and receding-horizon: Teleopit submits the latest +eligible observation every configured `replan_steps` at the 30 Hz action rate +while the current plan keeps executing. The isolated client keeps at most one +ZeroMQ request in flight. Each newer response is aligned with its echoed +onboard monotonic observation timestamp and replaces the active plan. Pico and high-level-policy deployment use separate scripts. The policy runtime does not start PicoBridge, GMR, or the Pico reference worker: @@ -169,12 +170,14 @@ Temporal reference jumps are accepted so recorded pause/resume transitions can be replayed, then rate-limited on output. OpenNeck yaw/pitch values are clipped to the configured degree ranges before scheduling, so a neck-only overshoot does not reject the action chunk. Entry failure returns to `STANDING`. -The blocking network exchange runs in an isolated process, so expected -inference time holds the last reference without stopping the local 50 Hz -control loop. Invalid/stale chunks, a request timeout, host/network failure, or -loss of a required camera/client worker enters the same ordinary pause state as -remote `B`; after recovery, press `B` to resume on a fresh valid chunk. Only -`X` returns active `POLICY` to `STANDING`. +The blocking network exchange runs in an isolated process, so the current plan +continues without stopping the local 50 Hz control loop. If inference outlasts +the plan horizon, its final reference remains valid for the configured +`hold_s` grace period. A request timeout, host/network failure, action-watchdog +expiry, or loss of a required camera/client worker enters the same ordinary +pause state as remote `B`; invalid/stale chunks are rejected. After recovery, +press `B` to resume on a fresh valid chunk. Only `X` returns active `POLICY` to +`STANDING`. The current client/server code and protocol tests define the network message structure. During active development, Teleopit and `lerobot-teleopit` must be diff --git a/docs/docs/configuration/config-reference.md b/docs/docs/configuration/config-reference.md index a1379c61..5c4ca409 100644 --- a/docs/docs/configuration/config-reference.md +++ b/docs/docs/configuration/config-reference.md @@ -124,10 +124,12 @@ protocol tests. The only shared data file is `hand_calibration.json`. | `high_level_policy.task` | Non-empty task prompt sent on reset and every observation | `demo` | | `high_level_policy.timeout_s` | Per-request network deadline; expiry pauses `POLICY` | `1.0` | | `high_level_policy.reconnect_backoff_s` | Retry delay while establishing a new session | `1.0` | +| `high_level_policy.replan_steps` | Minimum interval between requests in 30 Hz source frames; must not exceed the horizon reported by the host | `3` | | `high_level_policy.jpeg_quality` | JPEG quality for the 640x480 RGB frame | `90` | | `high_level_policy.max_observation_age_s` | Maximum camera/observation age before a request is skipped | `0.15` | | `high_level_policy.max_result_age_s` | Maximum local IPC age before a received result is rejected | `0.1` | | `high_level_policy.entry_timeout_s` | Maximum time to establish the entry session and receive its first valid chunk, and maximum fresh-chunk wait on resume | `5.0` | +| `high_level_policy.hold_s` | Final-reference grace period after the active plan horizon before the action watchdog pauses `POLICY` | `3.0` | | `high_level_policy.safety.root_height_min_m` / `root_height_max_m` | Accepted absolute root-height range | `0.55` / `1.05` | | `high_level_policy.safety.max_root_xy_speed_m_s` | Root XY speed limit applied to the 50 Hz scheduler output | `2.5` | | `high_level_policy.safety.max_root_displacement_m` | Source-frame-equivalent 3D root step used by the 50 Hz output limiter | `0.1` | @@ -137,10 +139,11 @@ protocol tests. The only shared data file is `hand_calibration.json`. | `high_level_policy.safety.neck_yaw_min_deg` / `neck_yaw_max_deg` | OpenNeck yaw clipping range | `-45` / `45` | | `high_level_policy.safety.neck_pitch_min_deg` / `neck_pitch_max_deg` | OpenNeck pitch clipping range | `-40` / `40` | -The request schedule is not configurable. Teleopit always issues one request, -holds the final safe reference during inference, executes the complete returned -chunk at 30 Hz, and then issues the next request. Removed `replan_steps` and -`hold_s` keys are configuration errors. +The request loop is asynchronous and receding-horizon. The isolated client has +at most one ZeroMQ request in flight, selects the latest eligible observation +at the configured source-frame stride, and leaves the current action plan +running during host inference. A newer response replaces that plan according +to its echoed onboard monotonic observation timestamp. G1 reference joint positions are clipped to `real_robot.joint_pos_lower/upper` when the required correction does not exceed diff --git a/docs/docs/tutorials/high-level-policy-sim2real.md b/docs/docs/tutorials/high-level-policy-sim2real.md index 7d29f4c6..65e438ae 100644 --- a/docs/docs/tutorials/high-level-policy-sim2real.md +++ b/docs/docs/tutorials/high-level-policy-sim2real.md @@ -16,8 +16,8 @@ Host workstation (lerobot-teleopit) | float32 state/action + JPEG over TCP v G1 onboard computer (Teleopit) - RealSense + G1 state -> one blocking request -> validated 30 Hz action chunk - -> execute complete chunk -> next request + RealSense + G1 state -> asynchronous client -> validated 30 Hz action plan + -> timestamp-aligned receding-horizon replacement -> 50 Hz interpolation -> motion tracker -> G1 joint-angle targets -> LinkerHand O6 / OpenNeck ``` @@ -116,16 +116,19 @@ python scripts/run/run_high_level_policy_sim2real.py \ real_robot.network_interface=eth0 ``` -The protocol accepts action chunks from 1 to 50 frames. A learned-policy host -predicts its full model horizon but returns only the checkpoint's -`n_action_steps`; the default ACT checkpoint therefore predicts 50 frames and -returns 3. ReplayPolicy returns up to its configured `--chunk-size`, including -a shorter final tail. +The protocol accepts action chunks from 1 to 50 frames. The current ACT host +returns its complete checkpoint horizon (50 frames for the production +checkpoint), while ReplayPolicy returns up to its configured `--chunk-size`, +including a shorter final tail. -Request scheduling has no mode or stride setting. Teleopit sends one -observation, waits while holding the final safe reference, executes the entire -returned chunk at 30 Hz, and then samples the next observation. It never -overlaps requests or replaces an executing chunk. +Teleopit submits the latest eligible observation every +`high_level_policy.replan_steps` 30 Hz source frames; the default is three. The +stride must not exceed `max_action_horizon` from the host's `describe` +response. The isolated client permits only one REQ/REP exchange at a time, but +the active plan continues while that request is in flight. The ACT host uses +the echoed onboard monotonic timestamp to aggregate overlapping predictions, +and Teleopit uses the same timestamp to replace the active plan at the correct +source-frame position. The production camera contract is exactly RGB `uint8[480,640,3]` at 30 Hz. `camera.source=test-pattern` exists only for controlled integration testing; @@ -155,9 +158,11 @@ session. The scheduler's 50 Hz output limiter starts from the measured robot reference captured when the session begins. A failure or timeout leaves the robot on the normal standing reference. -Inside `POLICY`, finishing a chunk is not a watchdog event. Teleopit holds its -final body, hand, and neck targets while the isolated client process performs -the next blocking request. The local 50 Hz control loop continues running. +Inside `POLICY`, a newer response normally replaces the active plan before its +horizon ends. If inference takes longer, Teleopit keeps the plan's final body, +hand, and neck targets for the configured `hold_s` grace period while the local +50 Hz control loop continues running. Exhausting that grace period triggers +the action watchdog and the normal resumable pause. Pause freezes the body reference and holds the last LinkerHand and OpenNeck commands. Resume requests a fresh action chunk while continuing to hold the @@ -197,16 +202,17 @@ other than the projected OpenNeck angles, or an excessive joint correction aborts entry. Validated 30 Hz body references are interpolated and rate-limited locally at -50 Hz from the time each response is accepted. Source timestamps identify the -observation echoed by the response; they are not used to skip into the chunk. -The configured root displacement/XY speed, yaw-rate, and joint-rate values are -output limits, not chunk-rejection thresholds. Normal inference between chunks -holds the final validated reference without a grace timer. If a network -exchange times out, a response is invalid, or a required camera/client worker -exits, Teleopit remains in `POLICY`, enters the normal resumable pause state, -and holds the latest body, hand, and neck commands. After recovery, `B` -requests resume; execution stays paused until a fresh validated chunk arrives. -Only `X` changes the mode to `STANDING`. +50 Hz. The echoed source timestamp selects the current position in each +response, so host latency can skip elapsed source frames and a newer chunk can +replace an executing plan. The configured root displacement/XY speed, +yaw-rate, and joint-rate values are output limits, not chunk-rejection +thresholds. The final validated reference remains available for `hold_s` after +the plan horizon. If a network exchange times out, the watchdog expires, or a +required camera/client worker exits, Teleopit remains in `POLICY`, enters the +normal resumable pause state, and holds the latest body, hand, and neck +commands. Invalid or stale responses are rejected without replacing the active +valid plan. After recovery, `B` requests resume; execution stays paused until a +fresh validated chunk arrives. Only `X` changes the mode to `STANDING`. The default safety envelope lives under `high_level_policy.safety` in `high_level_policy_sim2real.yaml`. Adjust it only after checking the recorded @@ -215,9 +221,9 @@ data, G1 joint limits, and the installed OpenNeck calibration. ## 7. Troubleshooting **`Y` never enters `POLICY`:** check the host endpoint, firewall, server log, -`describe` schemas, message envelope, task, checkpoint manifest, and the entry -logs. Teleopit stays in `STANDING` until the single entry session returns its -first valid chunk. +`describe` schemas, message envelope, task, checkpoint manifest, +`replan_steps`, and the entry logs. Teleopit stays in `STANDING` until the +single entry session returns its first valid chunk. **The first entry chunk is rejected or entry times out:** inspect the logged contract error, joint ordering, absolute-reference convention, hardware ranges, @@ -225,9 +231,9 @@ and host/network latency. Reference discontinuity alone does not reject a chunk. **Policy runs briefly and becomes paused:** inspect timeout, inference latency, stale-result, worker-exit, and safety-rejection logs. The low-level -50 Hz tracker does not block on host inference, and expected inference between -chunks only holds the current reference. Restore the failed input path, then -press `B` to resume. +50 Hz tracker does not block on host inference; the current plan keeps running +and then uses the configured final-reference grace period. Restore the failed +input path, then press `B` to resume. **Pico does not connect:** this runtime intentionally does not start Pico. Stop it and launch the Pico-specific `run_sim2real.py --config-name diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md index 386bba79..2e902c26 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md @@ -142,10 +142,12 @@ client/server 消息结构与协议测试。唯一共享的数据文件是 `hand | `high_level_policy.task` | reset 和每个 observation 都会发送的非空任务 prompt | `demo` | | `high_level_policy.timeout_s` | 单次网络请求 deadline；超时会暂停 `POLICY` | `1.0` | | `high_level_policy.reconnect_backoff_s` | 建立新 session 时的重试间隔 | `1.0` | +| `high_level_policy.replan_steps` | 两次请求之间的最小 30 Hz source-frame 间隔；不得超过主机报告的 horizon | `3` | | `high_level_policy.jpeg_quality` | 640x480 RGB 帧的 JPEG 质量 | `90` | | `high_level_policy.max_observation_age_s` | 跳过请求前允许的最大 camera/observation age | `0.15` | | `high_level_policy.max_result_age_s` | 拒绝已接收结果前允许的最大本地 IPC age | `0.1` | | `high_level_policy.entry_timeout_s` | 建立 entry session 并收到其第一份有效 chunk 的最长时间，以及恢复时等待新鲜 chunk 的最长时间 | `5.0` | +| `high_level_policy.hold_s` | active plan horizon 结束后、action watchdog 暂停 `POLICY` 前保持最终 reference 的 grace period | `3.0` | | `high_level_policy.safety.root_height_min_m` / `root_height_max_m` | 可接受的绝对 root 高度范围 | `0.55` / `1.05` | | `high_level_policy.safety.max_root_xy_speed_m_s` | 应用于 50 Hz scheduler 输出的 root XY 速度限制 | `2.5` | | `high_level_policy.safety.max_root_displacement_m` | 50 Hz 输出 rate limiter 使用的 source-frame 等效 3D root 步长 | `0.1` | @@ -155,9 +157,10 @@ client/server 消息结构与协议测试。唯一共享的数据文件是 `hand | `high_level_policy.safety.neck_yaw_min_deg` / `neck_yaw_max_deg` | OpenNeck yaw 裁剪范围 | `-45` / `45` | | `high_level_policy.safety.neck_pitch_min_deg` / `neck_pitch_max_deg` | OpenNeck pitch 裁剪范围 | `-40` / `40` | -请求调度不可配置。Teleopit 始终只发出一个请求，在推理期间保持最后一条安全 -reference，随后以 30 Hz 完整执行返回的 chunk，执行结束后再发出下一个请求。已删除的 -`replan_steps` 和 `hold_s` 配置键会直接报错。 +请求循环采用异步 receding-horizon。隔离的 client 最多只有一个 ZeroMQ 请求在途， +按照配置的 source-frame stride 选择最新的合格 observation，并在主机推理期间继续执行 +当前 action plan。较新的 response 会依据其中回显的 onboard 单调 observation 时间戳 +替换该计划。 当所需修正量不超过 `high_level_policy.safety.max_joint_projection_rad` 时，G1 reference joint position 会裁剪到 `real_robot.joint_pos_lower/upper`；更大的修正量会导致 chunk 被拒绝。 diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md index 51665787..ad1ba908 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md @@ -15,8 +15,8 @@ ZeroMQ/msgpack 消息通信。 | 通过 TCP 传输 float32 state/action + JPEG v G1 onboard 计算机（Teleopit） - RealSense + G1 state -> 单个阻塞请求 -> 已验证的 30 Hz action chunk - -> 完整执行 chunk -> 下一个请求 + RealSense + G1 state -> 异步 client -> 已验证的 30 Hz action plan + -> 按时间戳对齐的 receding-horizon 替换 -> 50 Hz 插值 -> motion tracker -> G1 关节角目标 -> LinkerHand O6 / OpenNeck ``` @@ -109,13 +109,15 @@ python scripts/run/run_high_level_policy_sim2real.py \ real_robot.network_interface=eth0 ``` -协议接受 1 到 50 帧的 action chunk。learned-policy 主机会预测完整的模型 horizon，但只 -返回 checkpoint 的 `n_action_steps`；因此默认 ACT checkpoint 会预测 50 帧并返回 3 帧。 -ReplayPolicy 最多返回 `--chunk-size` 配置的帧数，最后一段可以更短。 +协议接受 1 到 50 帧的 action chunk。当前 ACT 主机会返回完整的 checkpoint horizon +（生产 checkpoint 为 50 帧）；ReplayPolicy 最多返回 `--chunk-size` 配置的帧数， +最后一段可以更短。 -请求调度没有模式或 stride 配置。Teleopit 会发送一份 observation，在等待期间保持最后一条 -安全 reference，以 30 Hz 完整执行返回的 chunk，然后再采样下一份 observation。请求不会 -重叠，执行中的 chunk 也不会被替换。 +Teleopit 每隔 `high_level_policy.replan_steps` 个 30 Hz source frame 提交最新的合格 +observation，默认间隔为三帧。该 stride 不得超过主机 `describe` 响应中的 +`max_action_horizon`。隔离的 client 同一时间只允许一个 REQ/REP exchange，但该请求 +在途时 active plan 会继续执行。ACT 主机使用回显的 onboard 单调时间戳聚合相互重叠的 +prediction；Teleopit 使用同一时间戳，在正确的 source-frame 位置替换 active plan。 生产相机契约固定为 30 Hz 的 RGB `uint8[480,640,3]`。 `camera.source=test-pattern` 只用于受控集成测试；部署时应使用 @@ -142,8 +144,10 @@ ReplayPolicy 最多返回 `--chunk-size` 配置的帧数，最后一段可以更 创建或 reset 第二个 session。scheduler 的 50 Hz 输出 limiter 从 session 开始时捕获的 机器人实测 reference 起步。失败或超时会让机器人保持普通 standing reference。 -在 `POLICY` 内，chunk 执行结束不是 watchdog 事件。隔离的 client 进程执行下一次阻塞 -请求时，Teleopit 会保持最后一条 body、hand 和 neck target，本地 50 Hz 控制循环继续运行。 +在 `POLICY` 内，较新的 response 通常会在 active plan 的 horizon 结束前替换它。如果 +推理耗时更长，Teleopit 会在配置的 `hold_s` grace period 内保持该计划最后一条 body、 +hand 和 neck target，同时本地 50 Hz 控制循环继续运行。超过该 grace period 会触发 +action watchdog，并进入普通的可恢复暂停。 暂停会冻结 body reference，并保持最后一条 LinkerHand 和 OpenNeck 命令。恢复时会请求 新的 action chunk，并在等待期间继续保持暂停姿态。按 `X` 会停止策略 session；运行时 @@ -174,14 +178,15 @@ yaw 与 G1 关节 reference 跳变都会被接受，因为录制的 pause/resume chunk 会立即开始实时执行。格式错误、过期、非关节字段超出绝对范围（已裁剪的 OpenNeck 角度除外）或关节修正量过大会终止 entry。 -每份 response 通过验证后，其中的 30 Hz body reference 会从接收时刻开始在本地插值到 -50 Hz 并执行 rate limit。source timestamp 用于标识 response 回显的 observation，不会 -用于跳入 chunk。配置的 root displacement/XY speed、yaw rate 和 joint rate 是输出限制， -而不是 chunk 拒绝阈值。chunk 之间的正常推理会一直保持最后一条有效 reference，不使用 -grace timer。如果网络交换超时、response 无效，或必要的 camera/client worker 退出， -Teleopit 会保持在 `POLICY`，进入普通的可恢复暂停状态，并保持最后一条 body、hand 和 -neck 命令。故障恢复后按 `B` 请求恢复；在收到新的有效 chunk 前，执行仍保持暂停。只有 -`X` 会把模式切换到 `STANDING`。 +通过验证的 30 Hz body reference 会在本地插值到 50 Hz 并执行 rate limit。回显的 +source timestamp 用于选择每份 response 中的当前位置，因此主机延迟可以跳过已经过去的 +source frame，较新的 chunk 也可以替换执行中的计划。配置的 root displacement/XY +speed、yaw rate 和 joint rate 是输出限制，而不是 chunk 拒绝阈值。plan horizon 结束后， +最后一条有效 reference 会继续保留 `hold_s`。如果网络交换超时、watchdog 到期，或必要的 +camera/client worker 退出，Teleopit 会保持在 `POLICY`，进入普通的可恢复暂停状态，并保持 +最后一条 body、hand 和 neck 命令。无效或过期 response 会被拒绝，不会替换当前仍然有效的 +plan。故障恢复后按 `B` 请求恢复；在收到新的有效 chunk 前，执行仍保持暂停。只有 `X` +会把模式切换到 `STANDING`。 默认安全范围位于 `high_level_policy_sim2real.yaml` 的 `high_level_policy.safety` 下。只有在检查录制数据、G1 关节限位和已安装的 OpenNeck @@ -190,16 +195,16 @@ neck 命令。故障恢复后按 `B` 请求恢复；在收到新的有效 chunk ## 7. 故障排查 **按 `Y` 后始终不进入 `POLICY`：** 检查主机 endpoint、防火墙、server 日志、 -`describe` schema、消息 envelope、task、checkpoint manifest 和 entry 日志。Teleopit -会保持 `STANDING`，直到单个 entry session 返回第一份有效 chunk。 +`describe` schema、消息 envelope、task、checkpoint manifest、`replan_steps` 和 entry +日志。Teleopit 会保持 `STANDING`，直到单个 entry session 返回第一份有效 chunk。 **第一份 entry chunk 被拒绝或 entry 超时：** 请检查日志中的契约错误、关节顺序、绝对 reference 约定、硬件范围以及 host/network 延迟。单纯的 reference 跳变不会导致 chunk 被拒绝。 **策略短暂运行后进入暂停：** 检查 timeout、推理延迟、stale result、worker 退出和 -安全拒绝日志。底层 50 Hz tracker 不会等待主机推理，chunk 之间的正常推理只会保持当前 -reference。恢复故障输入路径后按 `B` 继续。 +安全拒绝日志。底层 50 Hz tracker 不会等待主机推理；当前 plan 会继续执行，随后使用配置的 +最终 reference grace period。恢复故障输入路径后按 `B` 继续。 **Pico 无法连接：** 该运行时有意不启动 Pico。请先停止它，再改用 Pico 专用的 `run_sim2real.py --config-name pico4_sim2real` 工作流。 diff --git a/teleopit/configs/high_level_policy_sim2real.yaml b/teleopit/configs/high_level_policy_sim2real.yaml index e0bae81c..4f7f51d1 100644 --- a/teleopit/configs/high_level_policy_sim2real.yaml +++ b/teleopit/configs/high_level_policy_sim2real.yaml @@ -23,10 +23,12 @@ high_level_policy: task: demo timeout_s: 1.0 reconnect_backoff_s: 1.0 + replan_steps: 3 # Submit a fresh observation every three 30 Hz action frames. jpeg_quality: 90 max_observation_age_s: 0.15 max_result_age_s: 0.1 entry_timeout_s: 5.0 + hold_s: 3.0 safety: root_height_min_m: 0.55 root_height_max_m: 1.05 diff --git a/teleopit/high_level_policy/__init__.py b/teleopit/high_level_policy/__init__.py index fec03bf3..107bb1bb 100644 --- a/teleopit/high_level_policy/__init__.py +++ b/teleopit/high_level_policy/__init__.py @@ -7,15 +7,15 @@ ) from teleopit.high_level_policy.hand_calibration import HandCalibration from teleopit.high_level_policy.scheduler import ( + HighLevelPolicyScheduler, PolicyFrameTransform, - SynchronousPolicyScheduler, ) __all__ = [ "HandCalibration", "HighLevelPolicyClient", + "HighLevelPolicyScheduler", "PolicyActionChunk", "PolicyDescription", "PolicyFrameTransform", - "SynchronousPolicyScheduler", ] diff --git a/teleopit/high_level_policy/config.py b/teleopit/high_level_policy/config.py index e309e177..79c89fcd 100644 --- a/teleopit/high_level_policy/config.py +++ b/teleopit/high_level_policy/config.py @@ -6,6 +6,7 @@ import numpy as np +from teleopit.high_level_policy.protocol import MAX_ACTION_HORIZON from teleopit.runtime.common import cfg_get @@ -15,10 +16,12 @@ class HighLevelPolicyConfig: task: str timeout_s: float reconnect_backoff_s: float + replan_steps: int jpeg_quality: int max_observation_age_s: float max_result_age_s: float entry_timeout_s: float + hold_s: float @dataclass(frozen=True) @@ -49,12 +52,6 @@ class HighLevelPolicySafetyConfig: def parse_high_level_policy_config(cfg: Any) -> HighLevelPolicyConfig: policy_cfg = cfg_get(cfg, "high_level_policy", {}) or {} - for removed_name in ("replan_steps", "hold_s"): - if cfg_get(policy_cfg, removed_name, None) is not None: - raise ValueError( - f"high_level_policy.{removed_name} was removed; " - "policy inference is always chunk-synchronous" - ) endpoint = str(cfg_get(policy_cfg, "endpoint", "tcp://127.0.0.1:5555")).strip() if not endpoint.startswith("tcp://"): raise ValueError("high_level_policy.endpoint must be a tcp:// endpoint") @@ -67,6 +64,12 @@ def parse_high_level_policy_config(cfg: Any) -> HighLevelPolicyConfig: reconnect_backoff_s = _positive_float( cfg_get(policy_cfg, "reconnect_backoff_s", 1.0), "reconnect_backoff_s" ) + replan_steps = int(cfg_get(policy_cfg, "replan_steps", 3)) + if not 1 <= replan_steps <= MAX_ACTION_HORIZON: + raise ValueError( + "high_level_policy.replan_steps must be in " + f"[1, {MAX_ACTION_HORIZON}]" + ) jpeg_quality = int(cfg_get(policy_cfg, "jpeg_quality", 90)) if not 1 <= jpeg_quality <= 100: raise ValueError("high_level_policy.jpeg_quality must be in [1, 100]") @@ -79,15 +82,20 @@ def parse_high_level_policy_config(cfg: Any) -> HighLevelPolicyConfig: entry_timeout_s = _positive_float( cfg_get(policy_cfg, "entry_timeout_s", 5.0), "entry_timeout_s" ) + hold_s = float(cfg_get(policy_cfg, "hold_s", 3.0)) + if not math.isfinite(hold_s) or hold_s < 0.0: + raise ValueError("high_level_policy.hold_s must be finite and >= 0") return HighLevelPolicyConfig( endpoint=endpoint, task=task, timeout_s=timeout_s, reconnect_backoff_s=reconnect_backoff_s, + replan_steps=replan_steps, jpeg_quality=jpeg_quality, max_observation_age_s=max_observation_age_s, max_result_age_s=max_result_age_s, entry_timeout_s=entry_timeout_s, + hold_s=hold_s, ) diff --git a/teleopit/high_level_policy/scheduler.py b/teleopit/high_level_policy/scheduler.py index dc391e90..06b9c64b 100644 --- a/teleopit/high_level_policy/scheduler.py +++ b/teleopit/high_level_policy/scheduler.py @@ -1,4 +1,4 @@ -"""Session-local frame conversion and synchronous 30 Hz chunk execution.""" +"""Session-local frame conversion and latency-aware 30 Hz action scheduling.""" from __future__ import annotations @@ -106,22 +106,27 @@ def delocalize_body_action(self, action: object) -> np.ndarray: return body -class SynchronousPolicyScheduler: +class HighLevelPolicyScheduler: def __init__( self, *, + hold_s: float = 3.0, safety: HighLevelPolicySafetyConfig | None = None, output_hz: float = 50.0, ) -> None: + if not np.isfinite(hold_s) or hold_s < 0.0: + raise ValueError("high_level_policy.hold_s must be finite and >= 0") if not np.isfinite(output_hz) or output_hz <= 0.0: raise ValueError("High-level policy scheduler output_hz must be finite and > 0") + self.hold_s = float(hold_s) self.safety = safety self.output_hz = float(output_hz) self._session_id: str | None = None self._chunk: PolicyActionChunk | None = None - self._chunk_started_s: float | None = None self._last_source_sequence_id = -1 self._last_source_timestamp_ns = -1 + self._paused_at_s: float | None = None + self._timestamp_shift_s = 0.0 self._last_output_action: np.ndarray | None = None @property @@ -132,14 +137,19 @@ def session_id(self) -> str | None: def has_chunk(self) -> bool: return self._chunk is not None + @property + def paused(self) -> bool: + return self._paused_at_s is not None + def reset(self, session_id: str, *, initial_action: object | None = None) -> None: if not isinstance(session_id, str) or not session_id: raise ValueError("High-level policy session_id must be non-empty") self._session_id = session_id self._chunk = None - self._chunk_started_s = None self._last_source_sequence_id = -1 self._last_source_timestamp_ns = -1 + self._paused_at_s = None + self._timestamp_shift_s = 0.0 initial_output = ( None if initial_action is None @@ -152,18 +162,23 @@ def reset(self, session_id: str, *, initial_action: object | None = None) -> Non def clear(self) -> None: self._session_id = None self._chunk = None - self._chunk_started_s = None self._last_source_sequence_id = -1 self._last_source_timestamp_ns = -1 + self._paused_at_s = None + self._timestamp_shift_s = 0.0 self._last_output_action = None def accept(self, chunk: PolicyActionChunk, *, now_s: float) -> None: + self._accept(chunk, now_s=now_s) + + def _accept( + self, + chunk: PolicyActionChunk, + *, + now_s: float, + ) -> None: if not np.isfinite(now_s): raise ValueError("High-level policy scheduler now_s must be finite") - if self._chunk is not None: - raise ValueError( - "High-level policy cannot replace an active synchronous action chunk" - ) if self._session_id is None or chunk.session_id != self._session_id: raise ValueError( f"High-level policy action session mismatch: active={self._session_id!r}, " @@ -198,7 +213,19 @@ def accept(self, chunk: PolicyActionChunk, *, now_s: float) -> None: raise ValueError("High-level policy policy_id must be non-empty") if not np.isfinite(chunk.server_inference_ms) or chunk.server_inference_ms < 0.0: raise ValueError("High-level policy server_inference_ms must be finite and >= 0") + source_s = chunk.source_onboard_monotonic_timestamp_ns * 1e-9 + if source_s > float(now_s) + 0.001: + raise ValueError( + "High-level policy source timestamp is in the future: " + f"source={source_s:.9f}s now={float(now_s):.9f}s" + ) actions = self._validate_actions(chunk.actions) + valid_until_s = source_s + len(actions) / float(chunk.action_fps) + self.hold_s + if float(now_s) > valid_until_s: + raise ValueError( + "High-level policy action chunk is already expired: " + f"age={float(now_s) - source_s:.3f}s horizon={len(actions) / chunk.action_fps:.3f}s" + ) self._chunk = PolicyActionChunk( session_id=chunk.session_id, source_sequence_id=chunk.source_sequence_id, @@ -208,17 +235,23 @@ def accept(self, chunk: PolicyActionChunk, *, now_s: float) -> None: policy_id=chunk.policy_id, server_inference_ms=chunk.server_inference_ms, ) - self._chunk_started_s = float(now_s) self._last_source_sequence_id = chunk.source_sequence_id self._last_source_timestamp_ns = chunk.source_onboard_monotonic_timestamp_ns + self._timestamp_shift_s = 0.0 + if self._paused_at_s is not None: + self._paused_at_s = float(now_s) - def discard_chunk(self) -> None: - self._chunk = None - self._chunk_started_s = None + def pause(self, now_s: float) -> None: + if self._paused_at_s is None: + self._paused_at_s = float(now_s) + + def resume(self, now_s: float) -> None: + if self._paused_at_s is None: + return + self._timestamp_shift_s += max(0.0, float(now_s) - self._paused_at_s) + self._paused_at_s = None def sample(self, now_s: float) -> np.ndarray | None: - if not np.isfinite(now_s): - raise ValueError("High-level policy scheduler now_s must be finite") desired = self._sample_unlimited(now_s) if desired is None: return None @@ -231,20 +264,18 @@ def sample(self, now_s: float) -> np.ndarray | None: def _sample_unlimited(self, now_s: float) -> np.ndarray | None: chunk = self._chunk - started_s = self._chunk_started_s - if chunk is None or started_s is None: - return None - elapsed_s = float(now_s) - started_s - if elapsed_s < 0.0: - raise ValueError("High-level policy scheduler time moved backwards") - if elapsed_s >= len(chunk.actions) / float(chunk.action_fps): - self.discard_chunk() + if chunk is None: return None - frame_f = elapsed_s * float(chunk.action_fps) + effective_now_s = self._paused_at_s if self._paused_at_s is not None else float(now_s) + source_s = chunk.source_onboard_monotonic_timestamp_ns * 1e-9 + self._timestamp_shift_s + frame_f = (effective_now_s - source_s) * float(chunk.action_fps) if frame_f <= 0.0: return chunk.actions[0].copy() last_index = len(chunk.actions) - 1 if frame_f >= float(last_index): + valid_until_s = source_s + len(chunk.actions) / float(chunk.action_fps) + self.hold_s + if effective_now_s > valid_until_s: + return None return chunk.actions[last_index].copy() index0 = int(math.floor(frame_f)) index1 = min(index0 + 1, last_index) diff --git a/teleopit/sim2real/mp/high_level_policy_runtime.py b/teleopit/sim2real/mp/high_level_policy_runtime.py index adfc124b..84a7de3e 100644 --- a/teleopit/sim2real/mp/high_level_policy_runtime.py +++ b/teleopit/sim2real/mp/high_level_policy_runtime.py @@ -28,7 +28,7 @@ LinkerHandO6Device, parse_linkerhand_o6_config, ) -from teleopit.sim2real.mp.high_level_policy_worker import SynchronousPolicyWorker +from teleopit.sim2real.mp.high_level_policy_worker import HighLevelPolicyWorker from teleopit.sim2real.mp.ipc import ( COMMAND_TOPIC, HIGH_LEVEL_POLICY_TARGET_TOPIC, @@ -220,7 +220,7 @@ def _run_high_level_policy_client_worker( cfg: dict[str, Any], endpoints: Sim2RealIpcEndpoints, stop_event: MpEvent ) -> None: def _main() -> None: - SynchronousPolicyWorker(cfg, endpoints, stop_event).run() + HighLevelPolicyWorker(cfg, endpoints, stop_event).run() _worker_loop("high_level_policy", cfg, _main) diff --git a/teleopit/sim2real/mp/high_level_policy_worker.py b/teleopit/sim2real/mp/high_level_policy_worker.py index 7dfda9ec..462508c2 100644 --- a/teleopit/sim2real/mp/high_level_policy_worker.py +++ b/teleopit/sim2real/mp/high_level_policy_worker.py @@ -1,4 +1,4 @@ -"""Blocking client worker for the chunk-synchronous host-policy loop.""" +"""Isolated client worker for asynchronous receding-horizon policy inference.""" from __future__ import annotations @@ -59,7 +59,7 @@ def encode_policy_jpeg(frame: object, *, quality: int) -> bytes: return payload -class SynchronousPolicyWorker: +class HighLevelPolicyWorker: def __init__( self, cfg: dict[str, Any], @@ -91,6 +91,7 @@ def __init__( self._paused = False self._last_session_seq = -1 self._last_observation_seq = -1 + self._last_request_timestamp_ns: int | None = None self._next_connect_time_s = 0.0 self._status_seq = 0 self._policy_type: str | None = None @@ -108,14 +109,9 @@ def run(self) -> None: self._handle_session(session) if self._active_session is not None and not self._ready: self._connect_if_due() - if ( - self._active_session is not None - and self._ready - and not self._paused - ): - observation = self._observation_sub.recv_latest() - if isinstance(observation, HighLevelPolicyObservationPacket): - self._handle_observation(observation) + observation = self._observation_sub.recv_latest() + if isinstance(observation, HighLevelPolicyObservationPacket): + self._handle_observation(observation) time.sleep(0.001) finally: self.close() @@ -142,6 +138,7 @@ def _handle_session(self, packet: HighLevelPolicySessionPacket) -> None: self._ready = False self._paused = False self._last_observation_seq = -1 + self._last_request_timestamp_ns = None self._next_connect_time_s = 0.0 self._new_session_required = False self._policy_type = None @@ -157,6 +154,7 @@ def _handle_session(self, packet: HighLevelPolicySessionPacket) -> None: elif command == "resume": if self._paused: self._paused = False + self._last_request_timestamp_ns = None if self._ready: self._publish_status("ready", "policy requests resumed") else: @@ -186,6 +184,11 @@ def _connect_if_due(self) -> None: timeout_s=self.policy_cfg.timeout_s, ) description = self._client.describe() + if self.policy_cfg.replan_steps > description.max_action_horizon: + raise ValueError( + "high_level_policy.replan_steps exceeds host max_action_horizon: " + f"{self.policy_cfg.replan_steps} > {description.max_action_horizon}" + ) self._client.reset(session.session_id, session.task) self._policy_type = description.policy_type self._policy_id = description.policy_id @@ -205,6 +208,13 @@ def _handle_observation(self, packet: HighLevelPolicyObservationPacket) -> None: now_s = time.monotonic() if now_s - float(packet.timestamp_s) > self.policy_cfg.max_observation_age_s: return + minimum_interval_ns = int(round(self.policy_cfg.replan_steps / 30.0 * 1e9)) + if ( + self._last_request_timestamp_ns is not None + and packet.onboard_monotonic_timestamp_ns - self._last_request_timestamp_ns + < minimum_interval_ns + ): + return client = self._client if client is None: return @@ -238,6 +248,7 @@ def _handle_observation(self, packet: HighLevelPolicyObservationPacket) -> None: ), ) self._last_observation_seq = int(packet.sequence_id) + self._last_request_timestamp_ns = int(packet.onboard_monotonic_timestamp_ns) except (PolicyProtocolError, PolicyTransportError, ValueError, RuntimeError) as exc: logger.warning("High-level policy request failed: %s", exc) self._ready = False @@ -265,10 +276,10 @@ def _publish_status(self, status: str, detail: str) -> None: ) -def run_synchronous_policy_worker( +def run_high_level_policy_worker( cfg: dict[str, Any], endpoints: Sim2RealIpcEndpoints, stop_event: Any, ) -> None: - worker = SynchronousPolicyWorker(cfg, endpoints, stop_event) + worker = HighLevelPolicyWorker(cfg, endpoints, stop_event) worker.run() diff --git a/teleopit/sim2real/mp/runtime.py b/teleopit/sim2real/mp/runtime.py index 121a0e87..7dabb28f 100644 --- a/teleopit/sim2real/mp/runtime.py +++ b/teleopit/sim2real/mp/runtime.py @@ -21,10 +21,7 @@ parse_high_level_policy_config, parse_high_level_policy_safety_config, ) -from teleopit.high_level_policy.scheduler import ( - PolicyFrameTransform, - SynchronousPolicyScheduler, -) +from teleopit.high_level_policy.scheduler import HighLevelPolicyScheduler, PolicyFrameTransform from teleopit.controllers.observation import VelCmdObservationBuilder, align_motion_qpos_yaw from teleopit.controllers.rl_policy import RLPolicyController from teleopit.inputs.bvh_provider import BVHInputProvider @@ -1265,7 +1262,8 @@ def __init__( else None ) self._high_level_policy_scheduler = ( - SynchronousPolicyScheduler( + HighLevelPolicyScheduler( + hold_s=self._high_level_policy_cfg.hold_s, safety=self._high_level_policy_safety_cfg, output_hz=self.policy_hz, ) @@ -1279,9 +1277,7 @@ def __init__( self._policy_paused = False self._policy_resume_pending = False self._policy_resume_deadline_s: float | None = None - self._policy_request_pending = False - self._policy_request_sequence_id: int | None = None - self._policy_request_deadline_s: float | None = None + self._policy_resume_source_timestamp_ns: int | None = None self._policy_hold_qpos: Float64Array | None = None self._policy_session_seq = 0 self._policy_observation_seq = 0 @@ -1537,25 +1533,9 @@ def _drain_high_level_policy_ipc(self) -> None: packet.session_id, ) return - packet_sequence_id = int(packet.source_sequence_id) - expected_sequence_id = self._policy_request_sequence_id - # A blocking request may finish after the operator pauses. Its result - # must not become the first chunk of a later resume cycle. + # A request may already be in flight when the operator pauses. Drain + # its result without replacing the reference frozen at the B press. if self._policy_paused and not self._policy_resume_pending: - if packet_sequence_id == expected_sequence_id: - self._clear_policy_request() - return - if ( - not self._policy_request_pending - or expected_sequence_id is None - or packet_sequence_id != expected_sequence_id - ): - logger.warning( - "Discarded unsolicited synchronous policy result: " - "expected_sequence=%r received_sequence=%d", - expected_sequence_id, - packet_sequence_id, - ) return scheduler = self._high_level_policy_scheduler policy_cfg = self._high_level_policy_cfg @@ -1588,16 +1568,20 @@ def _drain_high_level_policy_ipc(self) -> None: result_age_s, policy_cfg.max_result_age_s, ) - self._clear_policy_request() if self.mode == RobotMode.STANDING and self._policy_entry_pending: operator_logger.warning( "High-level policy entry failed; received a stale action result" ) self._enter_standing() - else: - self._handle_high_level_policy_fault( - "received a stale synchronous action result" - ) + return + minimum_source_timestamp_ns = self._policy_resume_source_timestamp_ns + if ( + self._policy_resume_pending + and minimum_source_timestamp_ns is not None + and int(packet.source_onboard_monotonic_timestamp_ns) + < minimum_source_timestamp_ns + ): + logger.warning("Discarded pre-resume high-level policy action chunk") return if self.mode == RobotMode.STANDING and not self._policy_entry_pending: return @@ -1617,29 +1601,24 @@ def _drain_high_level_policy_ipc(self) -> None: scheduler.accept(chunk, now_s=now_s) except ValueError as exc: logger.warning("Rejected high-level policy entry chunk: %s", exc) - self._clear_policy_request() operator_logger.warning( "High-level policy entry failed; remaining in STANDING" ) self._enter_standing() return - self._clear_policy_request() self._transition_to_high_level_policy() return try: scheduler.accept(chunk, now_s=now_s) except ValueError as exc: logger.warning("Rejected high-level policy action chunk: %s", exc) - self._clear_policy_request() - self._handle_high_level_policy_fault( - f"rejected synchronous action chunk: {exc}" - ) return - self._clear_policy_request() if self._policy_resume_pending: + scheduler.resume(now_s) self._policy_paused = False self._policy_resume_pending = False self._policy_resume_deadline_s = None + self._policy_resume_source_timestamp_ns = None operator_logger.info("fresh action chunk -> resume POLICY") def _handle_high_level_policy_transitions(self) -> None: @@ -1712,7 +1691,6 @@ def _start_high_level_policy_entry_session(self) -> None: getattr(state, "quat"), ) self._policy_session_id = uuid.uuid4().hex - self._latest_policy_status = None scheduler.reset( self._policy_session_id, initial_action=self._build_high_level_policy_boundary_action(state), @@ -1722,7 +1700,7 @@ def _start_high_level_policy_entry_session(self) -> None: self._policy_paused = False self._policy_resume_pending = False self._policy_resume_deadline_s = None - self._clear_policy_request() + self._policy_resume_source_timestamp_ns = None self._policy_hold_qpos = self._build_robot_state_qpos(state) self._policy_observation_seq = 0 self._last_policy_video_seq = ( @@ -1755,35 +1733,23 @@ def _publish_high_level_policy_session(self, command: str, *, repeat: bool = Fal ) self._last_policy_session_publish_s = now_s - def _high_level_policy_worker_ready(self) -> bool: - status = self._latest_policy_status - return bool( - status is not None - and status.session_id == self._policy_session_id - and status.status == "ready" - ) - - def _publish_high_level_policy_observation(self, robot_state: object) -> bool: + def _publish_high_level_policy_observation(self, robot_state: object) -> None: if not (self._policy_entry_pending or self.mode == RobotMode.POLICY): - return False + return if self._policy_paused and not self._policy_resume_pending: - return False - if self._policy_request_pending: - return False - if not self._high_level_policy_worker_ready(): - return False + return publisher = self._policy_control_pub frame = self._latest_policy_video transform = self._policy_frame_transform session_id = self._policy_session_id policy_cfg = self._high_level_policy_cfg if publisher is None or frame is None or transform is None or session_id is None or policy_cfg is None: - return False + return if int(frame.seq) <= self._last_policy_video_seq: - return False + return now_s = time.monotonic() if abs(now_s - float(frame.timestamp_s)) > policy_cfg.max_observation_age_s: - return False + return state = transform.localize_state(build_observation_state(robot_state)) sequence_id = self._policy_observation_seq publisher.publish( @@ -1799,17 +1765,6 @@ def _publish_high_level_policy_observation(self, robot_state: object) -> bool: ) self._policy_observation_seq += 1 self._last_policy_video_seq = int(frame.seq) - self._policy_request_pending = True - self._policy_request_sequence_id = sequence_id - self._policy_request_deadline_s = ( - now_s + policy_cfg.timeout_s + policy_cfg.max_result_age_s + 0.1 - ) - return True - - def _clear_policy_request(self) -> None: - self._policy_request_pending = False - self._policy_request_sequence_id = None - self._policy_request_deadline_s = None def _transition_to_high_level_policy(self) -> None: state = self.robot.get_state() @@ -1823,6 +1778,7 @@ def _transition_to_high_level_policy(self) -> None: self._policy_paused = False self._policy_resume_pending = False self._policy_resume_deadline_s = None + self._policy_resume_source_timestamp_ns = None self.mode = RobotMode.POLICY operator_logger.info("mode -> POLICY") @@ -1837,18 +1793,17 @@ def _toggle_high_level_policy_pause(self) -> None: policy_cfg = self._high_level_policy_cfg if policy_cfg is None: return - scheduler.discard_chunk() - self._clear_policy_request() self._policy_resume_pending = True self._policy_resume_deadline_s = now_s + policy_cfg.entry_timeout_s + self._policy_resume_source_timestamp_ns = int(round(now_s * 1e9)) self._publish_high_level_policy_session("resume") operator_logger.info("B -> resume POLICY; waiting for a fresh action chunk") else: - scheduler.discard_chunk() - self._clear_policy_request() + scheduler.pause(now_s) self._policy_paused = True self._policy_resume_pending = False self._policy_resume_deadline_s = None + self._policy_resume_source_timestamp_ns = None self._policy_hold_qpos = self._resolve_mocap_hold_qpos() self._publish_high_level_policy_session("pause") operator_logger.info("B -> pause POLICY") @@ -1866,7 +1821,7 @@ def _stop_high_level_policy_session(self) -> None: self._policy_paused = False self._policy_resume_pending = False self._policy_resume_deadline_s = None - self._clear_policy_request() + self._policy_resume_source_timestamp_ns = None self._policy_hold_qpos = None self._latest_policy_status = None @@ -1878,11 +1833,11 @@ def _handle_high_level_policy_fault(self, detail: str) -> None: return scheduler = self._high_level_policy_scheduler if scheduler is not None: - scheduler.discard_chunk() - self._clear_policy_request() + scheduler.pause(time.monotonic()) self._policy_paused = True self._policy_resume_pending = False self._policy_resume_deadline_s = None + self._policy_resume_source_timestamp_ns = None self._policy_hold_qpos = self._resolve_mocap_hold_qpos() self._publish_high_level_policy_session("pause") operator_logger.warning( @@ -1955,15 +1910,9 @@ def _high_level_policy_step(self) -> None: if self._policy_resume_pending: robot_state = self.robot.get_state() self._publish_high_level_policy_observation(robot_state) - now_s = time.monotonic() deadline_s = self._policy_resume_deadline_s - request_deadline_s = self._policy_request_deadline_s - if deadline_s is not None and now_s > deadline_s: + if deadline_s is not None and time.monotonic() > deadline_s: self._handle_high_level_policy_fault("resume timed out waiting for a fresh action chunk") - elif request_deadline_s is not None and now_s > request_deadline_s: - self._handle_high_level_policy_fault( - "synchronous policy inference timed out" - ) hold_qpos = self._policy_hold_qpos if hold_qpos is None: hold_qpos = self._resolve_mocap_hold_qpos() @@ -1989,15 +1938,10 @@ def _high_level_policy_step(self) -> None: return robot_state = self.robot.get_state() - now_s = time.monotonic() - scheduled = scheduler.sample(now_s) + self._publish_high_level_policy_observation(robot_state) + scheduled = scheduler.sample(time.monotonic()) if scheduled is None: - self._publish_high_level_policy_observation(robot_state) - request_deadline_s = self._policy_request_deadline_s - if request_deadline_s is not None and now_s > request_deadline_s: - self._handle_high_level_policy_fault( - "synchronous policy inference timed out" - ) + self._handle_high_level_policy_fault("action watchdog expired") hold_qpos = self._policy_hold_qpos if hold_qpos is None: hold_qpos = self._resolve_mocap_hold_qpos() diff --git a/tests/test_high_level_policy.py b/tests/test_high_level_policy.py index 212e467d..0a944a97 100644 --- a/tests/test_high_level_policy.py +++ b/tests/test_high_level_policy.py @@ -26,8 +26,8 @@ unpack_message, ) from teleopit.high_level_policy.scheduler import ( + HighLevelPolicyScheduler, PolicyFrameTransform, - SynchronousPolicyScheduler, closure_to_o6_pose, ) from teleopit.sim2real.mp.high_level_policy_runtime import ( @@ -38,14 +38,14 @@ _test_pattern, _validate_high_level_policy_runtime_config, ) -from teleopit.sim2real.mp.high_level_policy_worker import SynchronousPolicyWorker +from teleopit.sim2real.mp.high_level_policy_worker import HighLevelPolicyWorker from teleopit.sim2real.mp.messages import ( HighLevelPolicyActionPacket, + HighLevelPolicyObservationPacket, HighLevelPolicySessionPacket, HighLevelPolicyStatusPacket, HighLevelPolicyTargetPacket, ModeStatePacket, - SharedFrameDescriptor, ) from teleopit.sim2real.mp.runtime import ( RobotMode, @@ -112,30 +112,30 @@ def _safe_chunk(actions: np.ndarray, *, source_s: float = 1.0, sequence: int = 0 ) -def test_high_level_policy_config_has_no_async_chunk_options() -> None: +def test_high_level_policy_default_hold_covers_inference_and_transport_jitter() -> None: config = parse_high_level_policy_config({"high_level_policy": {"task": "demo"}}) - assert not hasattr(config, "replan_steps") - assert not hasattr(config, "hold_s") - for removed_name in ("replan_steps", "hold_s"): - with pytest.raises(ValueError, match=rf"{removed_name} was removed"): - parse_high_level_policy_config( - { - "high_level_policy": { - "task": "demo", - removed_name: 3, - } - } - ) + assert config.hold_s == pytest.approx(3.0) + + +def test_high_level_policy_replan_steps_uses_protocol_horizon_limit() -> None: + config = parse_high_level_policy_config( + { + "high_level_policy": { + "task": "demo", + "replan_steps": MAX_ACTION_HORIZON, + } + } + ) + assert config.replan_steps == MAX_ACTION_HORIZON -def test_high_level_policy_config_still_validates_request_timeout() -> None: - with pytest.raises(ValueError, match="timeout_s"): + with pytest.raises(ValueError, match=rf"\[1, {MAX_ACTION_HORIZON}\]"): parse_high_level_policy_config( { "high_level_policy": { "task": "demo", - "timeout_s": 0.0, + "replan_steps": MAX_ACTION_HORIZON + 1, } } ) @@ -184,56 +184,71 @@ def test_policy_frame_transform_localizes_state_and_delocalizes_action() -> None np.testing.assert_allclose(transform.localize_body_action(world), body, atol=1e-6) -def test_scheduler_starts_on_receipt_and_interpolates_at_30hz() -> None: - scheduler = SynchronousPolicyScheduler() +def test_scheduler_uses_source_timestamp_and_interpolates_at_30hz() -> None: + scheduler = HighLevelPolicyScheduler(hold_s=0.1) scheduler.reset("session-1") scheduler.accept(_chunk(source_s=10.0), now_s=10.01) - halfway = scheduler.sample(10.01 + 0.5 / 30.0) + halfway = scheduler.sample(10.0 + 0.5 / 30.0) assert halfway is not None assert halfway[0] == pytest.approx(0.5) assert halfway[48] == pytest.approx(5.0) def test_scheduler_accepts_protocol_max_action_horizon() -> None: - scheduler = SynchronousPolicyScheduler() + scheduler = HighLevelPolicyScheduler(hold_s=0.1) scheduler.reset("session-1") scheduler.accept(_chunk(source_s=10.0, frames=MAX_ACTION_HORIZON), now_s=10.01) assert scheduler.has_chunk -def test_scheduler_completes_chunk_before_accepting_the_next_one() -> None: - scheduler = SynchronousPolicyScheduler() +def test_scheduler_replaces_active_plan_using_new_source_timestamp() -> None: + scheduler = HighLevelPolicyScheduler(hold_s=0.1) scheduler.reset("session-1") scheduler.accept(_chunk(source_s=20.0), now_s=20.0) - with pytest.raises(ValueError, match="cannot replace an active synchronous"): - scheduler.accept(_chunk(source_s=20.01, sequence=1), now_s=20.01) + replacement = _chunk(source_s=20.05, sequence=1) + replacement.actions[:, 0] += 10.0 + scheduler.accept(replacement, now_s=20.06) - assert scheduler.sample(20.0 + 3.0 / 30.0) is None - scheduler.accept(_chunk(source_s=25.0, sequence=1), now_s=25.0) - assert scheduler.has_chunk + scheduled = scheduler.sample(20.05 + 0.5 / 30.0) + assert scheduled is not None + assert scheduled[0] == pytest.approx(10.5) + + +def test_scheduler_pause_freezes_and_resume_shifts_plan_time() -> None: + scheduler = HighLevelPolicyScheduler(hold_s=0.1) + scheduler.reset("session-1") + scheduler.accept(_chunk(source_s=20.0), now_s=20.0) + scheduler.pause(20.02) + + paused = scheduler.sample(25.0) + assert paused is not None + scheduler.resume(25.0) + resumed = scheduler.sample(25.0) + assert resumed is not None + np.testing.assert_allclose(resumed, paused) -def test_scheduler_rejects_wrong_session_but_not_inference_latency() -> None: - scheduler = SynchronousPolicyScheduler() +def test_scheduler_rejects_wrong_session_and_expired_chunk() -> None: + scheduler = HighLevelPolicyScheduler(hold_s=0.0) scheduler.reset("other") with pytest.raises(ValueError, match="session mismatch"): scheduler.accept(_chunk(source_s=1.0), now_s=1.0) scheduler.reset("session-1") - scheduler.accept(_chunk(source_s=1.0), now_s=20.0) - first = scheduler.sample(20.0) - assert first is not None - assert first[0] == pytest.approx(0.0) + with pytest.raises(ValueError, match="already expired"): + scheduler.accept(_chunk(source_s=1.0), now_s=2.0) + + with pytest.raises(ValueError, match="in the future"): + scheduler.accept(_chunk(source_s=3.0), now_s=2.0) def test_scheduler_rejects_nonincreasing_source_timestamp() -> None: - scheduler = SynchronousPolicyScheduler() + scheduler = HighLevelPolicyScheduler(hold_s=0.1) scheduler.reset("session-1") scheduler.accept(_chunk(source_s=1.0), now_s=1.0) - scheduler.discard_chunk() with pytest.raises(ValueError, match="source timestamp must increase"): scheduler.accept(_chunk(source_s=1.0, sequence=1), now_s=1.01) @@ -245,7 +260,7 @@ def test_linkerhand_closure_uses_hand_calibration() -> None: def test_scheduler_validates_complete_chunk_against_onboard_safety_limits() -> None: - scheduler = SynchronousPolicyScheduler(safety=_safety_config()) + scheduler = HighLevelPolicyScheduler(hold_s=0.1, safety=_safety_config()) initial = _safe_actions(1)[0] scheduler.reset("session-1", initial_action=initial) scheduler.accept(_safe_chunk(_safe_actions()), now_s=1.01) @@ -254,7 +269,7 @@ def test_scheduler_validates_complete_chunk_against_onboard_safety_limits() -> N def test_scheduler_accepts_internal_reference_discontinuities() -> None: - scheduler = SynchronousPolicyScheduler(safety=_safety_config()) + scheduler = HighLevelPolicyScheduler(hold_s=0.1, safety=_safety_config()) initial = _safe_actions(1)[0] initial[7] = 0.8 actions = _safe_actions() @@ -272,7 +287,7 @@ def test_scheduler_accepts_internal_reference_discontinuities() -> None: def test_scheduler_clips_joint_positions_to_onboard_limits() -> None: - scheduler = SynchronousPolicyScheduler(safety=_safety_config()) + scheduler = HighLevelPolicyScheduler(hold_s=0.1, safety=_safety_config()) scheduler.reset("session-1", initial_action=_safe_actions(1)[0]) actions = _safe_actions(1) actions[0, 7] = -3.08 @@ -281,7 +296,7 @@ def test_scheduler_clips_joint_positions_to_onboard_limits() -> None: scheduler.accept(_safe_chunk(actions), now_s=1.01) scheduled = None for _ in range(20): - scheduled = scheduler.sample(1.01) + scheduled = scheduler.sample(1.0) assert scheduled is not None assert scheduled[7] == pytest.approx(-3.0) @@ -289,15 +304,15 @@ def test_scheduler_clips_joint_positions_to_onboard_limits() -> None: def test_scheduler_clips_openneck_angles_to_onboard_limits() -> None: - scheduler = SynchronousPolicyScheduler(safety=_safety_config()) + scheduler = HighLevelPolicyScheduler(hold_s=0.1, safety=_safety_config()) scheduler.reset("session-1", initial_action=_safe_actions(1)[0]) actions = _safe_actions() actions[:, 48] = [-46.0, 0.0, 46.0] actions[:, 49] = [41.0, 0.0, -41.0] scheduler.accept(_safe_chunk(actions), now_s=1.01) - first_action = scheduler.sample(1.01) - final_action = scheduler.sample(1.01 + 2.0 / 30.0) + first_action = scheduler.sample(1.0) + final_action = scheduler.sample(1.0 + 2.0 / 30.0) assert first_action is not None assert first_action[48] == pytest.approx(-45.0) @@ -308,7 +323,7 @@ def test_scheduler_clips_openneck_angles_to_onboard_limits() -> None: def test_scheduler_rejects_joint_projection_above_limit() -> None: - scheduler = SynchronousPolicyScheduler(safety=_safety_config()) + scheduler = HighLevelPolicyScheduler(hold_s=0.1, safety=_safety_config()) scheduler.reset("session-1", initial_action=_safe_actions(1)[0]) actions = _safe_actions(1) actions[0, 7] = -3.11 @@ -319,7 +334,7 @@ def test_scheduler_rejects_joint_projection_above_limit() -> None: def test_scheduler_rejects_entire_unsafe_non_joint_chunk() -> None: - scheduler = SynchronousPolicyScheduler(safety=_safety_config()) + scheduler = HighLevelPolicyScheduler(hold_s=0.1, safety=_safety_config()) scheduler.reset("session-1", initial_action=_safe_actions(1)[0]) actions = _safe_actions() actions[1, 2] = 0.4 @@ -330,7 +345,8 @@ def test_scheduler_rejects_entire_unsafe_non_joint_chunk() -> None: def test_scheduler_accepts_discontinuous_plan_and_rate_limits_output_at_50hz() -> None: - scheduler = SynchronousPolicyScheduler( + scheduler = HighLevelPolicyScheduler( + hold_s=0.1, safety=_safety_config(), output_hz=50.0, ) @@ -341,7 +357,7 @@ def test_scheduler_accepts_discontinuous_plan_and_rate_limits_output_at_50hz() - actions[1, 7] = 0.5 yaw = 0.2 actions[1, 3:7] = [math.cos(yaw / 2.0), 0.0, 0.0, math.sin(yaw / 2.0)] - scheduler.accept(_safe_chunk(actions), now_s=1.0) + scheduler.accept(_safe_chunk(actions), now_s=1.01) output = scheduler.sample(1.0 + 1.0 / 30.0) @@ -647,6 +663,7 @@ def test_policy_transition_after_first_chunk_does_not_start_kp_ramp() -> None: worker._policy_paused = True worker._policy_resume_pending = False worker._policy_resume_deadline_s = None + worker._policy_resume_source_timestamp_ns = None worker._standing_return_ramp_duration = 0.5 worker._standing_return_kp_ramp_floor_ratio = 0.5 worker._safety = SimpleNamespace( @@ -687,9 +704,7 @@ def test_policy_entry_rejects_action_received_after_deadline() -> None: worker._policy_session_id = "session-1" worker._policy_paused = False worker._policy_resume_pending = False - worker._policy_request_pending = True - worker._policy_request_sequence_id = 1 - worker._policy_request_deadline_s = now_s + 1.0 + worker._policy_resume_source_timestamp_ns = None worker._policy_entry_pending = True worker._policy_entry_deadline_s = now_s - 0.01 accepted: list[object] = [] @@ -732,9 +747,7 @@ def test_policy_entry_first_chunk_uses_measured_reference_boundary() -> None: worker._policy_session_id = "session-1" worker._policy_paused = False worker._policy_resume_pending = False - worker._policy_request_pending = True - worker._policy_request_sequence_id = 1 - worker._policy_request_deadline_s = now_s + 1.0 + worker._policy_resume_source_timestamp_ns = None worker._policy_entry_pending = True worker._policy_entry_deadline_s = now_s + 1.0 worker._policy_frame_transform = PolicyFrameTransform.from_robot_pose( @@ -748,7 +761,7 @@ def test_policy_entry_first_chunk_uses_measured_reference_boundary() -> None: state = SimpleNamespace() worker.robot = SimpleNamespace(get_state=lambda: state) worker._build_robot_state_qpos = lambda _state: current_qpos.copy() - scheduler = SynchronousPolicyScheduler(safety=_safety_config()) + scheduler = HighLevelPolicyScheduler(hold_s=0.1, safety=_safety_config()) boundary_action = worker._build_high_level_policy_boundary_action(state) scheduler.reset("session-1", initial_action=boundary_action) worker._high_level_policy_scheduler = scheduler @@ -761,7 +774,7 @@ def test_policy_entry_first_chunk_uses_measured_reference_boundary() -> None: worker._drain_high_level_policy_ipc() - scheduled = scheduler.sample(time.monotonic()) + scheduled = scheduler.sample(now_s) assert boundary_action[7] == pytest.approx(0.8) assert current_qpos[7] == pytest.approx(0.8) assert transitions == ["policy"] @@ -793,9 +806,7 @@ def test_policy_entry_stale_result_aborts_current_session() -> None: worker._policy_session_id = "session-1" worker._policy_paused = False worker._policy_resume_pending = False - worker._policy_request_pending = True - worker._policy_request_sequence_id = 1 - worker._policy_request_deadline_s = now_s + 1.0 + worker._policy_resume_source_timestamp_ns = None worker._policy_entry_pending = True worker._policy_entry_deadline_s = now_s + 1.0 worker._high_level_policy_scheduler = SimpleNamespace() @@ -847,8 +858,7 @@ def test_high_level_policy_body_action_uses_existing_tracker_without_second_alig worker._policy_paused = False worker._policy_resume_pending = False worker.robot = SimpleNamespace(get_state=lambda: SimpleNamespace()) - published_observations: list[object] = [] - worker._publish_high_level_policy_observation = published_observations.append + worker._publish_high_level_policy_observation = lambda _state: None worker._policy_control_pub = None worker._policy_hold_qpos = None calls: list[tuple[np.ndarray, dict[str, object]]] = [] @@ -867,100 +877,6 @@ def execute(reference, _state, **kwargs) -> None: # type: ignore[no-untyped-def "align_reference": False, "compose_arms": False, } - assert published_observations == [] - - -def test_synchronous_policy_publishes_only_one_outstanding_observation() -> None: - worker = object.__new__(_RobotControlWorker) - now_s = time.monotonic() - worker.mode = RobotMode.POLICY - worker._policy_entry_pending = False - worker._policy_paused = False - worker._policy_resume_pending = False - worker._policy_request_pending = False - worker._policy_request_sequence_id = None - worker._policy_request_deadline_s = None - worker._policy_session_id = "session-1" - worker._latest_policy_status = HighLevelPolicyStatusPacket( - session_id="session-1", - status="ready", - detail="host policy session reset", - timestamp_s=now_s, - seq=1, - ) - worker._policy_observation_seq = 7 - worker._last_policy_video_seq = 9 - worker._latest_policy_video = SharedFrameDescriptor( - shm_name="frame", - slot=0, - seq=10, - timestamp_s=now_s, - shape=(480, 640, 3), - dtype="uint8", - slots=3, - ) - worker._policy_frame_transform = PolicyFrameTransform.from_robot_pose( - [0.0, 0.0], - [1.0, 0.0, 0.0, 0.0], - ) - worker._high_level_policy_cfg = SimpleNamespace( - task="demo", - timeout_s=1.0, - max_observation_age_s=0.15, - max_result_age_s=0.1, - ) - published: list[tuple[str, object]] = [] - worker._policy_control_pub = SimpleNamespace( - publish=lambda topic, packet: published.append((topic, packet)) - ) - robot_state = SimpleNamespace( - qpos=np.zeros(29, dtype=np.float32), - qvel=np.zeros(29, dtype=np.float32), - quat=np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float32), - ang_vel=np.zeros(3, dtype=np.float32), - ) - - assert worker._publish_high_level_policy_observation(robot_state) - assert not worker._publish_high_level_policy_observation(robot_state) - assert len(published) == 1 - assert worker._policy_request_pending - assert worker._policy_request_sequence_id == 7 - - -@pytest.mark.parametrize( - "status", - [ - None, - HighLevelPolicyStatusPacket( - session_id="session-1", - status="connecting", - detail="connecting to host policy", - timestamp_s=1.0, - seq=1, - ), - HighLevelPolicyStatusPacket( - session_id="other-session", - status="ready", - detail="host policy session reset", - timestamp_s=1.0, - seq=2, - ), - ], -) -def test_synchronous_policy_waits_for_current_session_ready_status( - status: HighLevelPolicyStatusPacket | None, -) -> None: - worker = object.__new__(_RobotControlWorker) - worker.mode = RobotMode.STANDING - worker._policy_entry_pending = True - worker._policy_paused = False - worker._policy_resume_pending = False - worker._policy_request_pending = False - worker._policy_session_id = "session-1" - worker._latest_policy_status = status - - assert not worker._publish_high_level_policy_observation(SimpleNamespace()) - assert not worker._policy_request_pending def test_policy_and_pico_remote_b_both_toggle_pause() -> None: @@ -986,12 +902,13 @@ def test_policy_and_pico_remote_b_both_toggle_pause() -> None: def test_policy_worker_pause_resume_retransmission_is_idempotent() -> None: - worker = object.__new__(SynchronousPolicyWorker) + worker = object.__new__(HighLevelPolicyWorker) worker._last_session_seq = -1 worker._active_session = None worker._ready = False worker._paused = False worker._last_observation_seq = -1 + worker._last_request_timestamp_ns = None worker._next_connect_time_s = 0.0 worker._new_session_required = False statuses: list[str] = [] @@ -1017,12 +934,13 @@ def packet(command: str, seq: int) -> HighLevelPolicySessionPacket: def test_policy_worker_resume_reconnects_faulted_current_session() -> None: - worker = object.__new__(SynchronousPolicyWorker) + worker = object.__new__(HighLevelPolicyWorker) worker._last_session_seq = -1 worker._active_session = None worker._ready = False worker._paused = False worker._last_observation_seq = -1 + worker._last_request_timestamp_ns = None worker._next_connect_time_s = 0.0 worker._new_session_required = False statuses: list[str] = [] @@ -1047,6 +965,67 @@ def packet(command: str, seq: int) -> HighLevelPolicySessionPacket: assert not worker._new_session_required +def test_policy_worker_replans_on_configured_source_frame_stride(monkeypatch) -> None: + worker = object.__new__(HighLevelPolicyWorker) + worker._active_session = HighLevelPolicySessionPacket( + session_id="session-1", + task="demo", + command="start", + timestamp_s=time.monotonic(), + seq=1, + ) + worker._ready = True + worker._paused = False + worker._last_observation_seq = -1 + worker._last_request_timestamp_ns = 1_000_000_000 + worker.policy_cfg = SimpleNamespace( + replan_steps=3, + max_observation_age_s=0.15, + jpeg_quality=90, + ) + requests: list[dict[str, object]] = [] + + def get_action(**kwargs): # type: ignore[no-untyped-def] + requests.append(kwargs) + return _chunk( + source_s=int(kwargs["onboard_monotonic_timestamp_ns"]) * 1e-9, + sequence=int(kwargs["sequence_id"]), + ) + + worker._client = SimpleNamespace(get_action=get_action) + worker._policy_id = "test" + worker._frame_reader = SimpleNamespace( + read=lambda _descriptor, copy: np.zeros((480, 640, 3), dtype=np.uint8) + ) + published: list[object] = [] + worker._result_pub = SimpleNamespace( + publish=lambda _topic, packet: published.append(packet) + ) + monkeypatch.setattr( + "teleopit.sim2real.mp.high_level_policy_worker.encode_policy_jpeg", + lambda _frame, quality: b"jpeg", + ) + + def observation(sequence_id: int, timestamp_ns: int) -> HighLevelPolicyObservationPacket: + return HighLevelPolicyObservationPacket( + session_id="session-1", + sequence_id=sequence_id, + onboard_monotonic_timestamp_ns=timestamp_ns, + state=np.zeros(68, dtype=np.float32), + frame=object(), # type: ignore[arg-type] + timestamp_s=time.monotonic(), + ) + + worker._handle_observation(observation(1, 1_099_999_999)) + worker._handle_observation(observation(2, 1_100_000_000)) + + assert len(requests) == 1 + assert requests[0]["sequence_id"] == 2 + assert worker._last_request_timestamp_ns == 1_100_000_000 + assert len(published) == 1 + assert published[0].source_onboard_monotonic_timestamp_ns == 1_100_000_000 + + def test_policy_fault_uses_normal_pause_without_entering_standing() -> None: worker = object.__new__(_RobotControlWorker) worker.high_level_policy_enabled = True @@ -1054,12 +1033,10 @@ def test_policy_fault_uses_normal_pause_without_entering_standing() -> None: worker._policy_paused = False worker._policy_resume_pending = False worker._policy_resume_deadline_s = None - worker._policy_request_pending = True - worker._policy_request_sequence_id = 2 - worker._policy_request_deadline_s = time.monotonic() + 1.0 - discarded: list[str] = [] + worker._policy_resume_source_timestamp_ns = None + paused: list[float] = [] worker._high_level_policy_scheduler = SimpleNamespace( - discard_chunk=lambda: discarded.append("discard") + pause=lambda now_s: paused.append(float(now_s)) ) hold_qpos = np.arange(36, dtype=np.float64) worker._resolve_mocap_hold_qpos = lambda: hold_qpos.copy() @@ -1072,31 +1049,29 @@ def test_policy_fault_uses_normal_pause_without_entering_standing() -> None: assert worker.mode == RobotMode.POLICY assert worker._policy_paused assert not worker._policy_resume_pending - assert discarded == ["discard"] - assert not worker._policy_request_pending + assert len(paused) == 1 assert published == ["pause"] np.testing.assert_array_equal(worker._policy_hold_qpos, hold_qpos) -def test_synchronous_policy_wait_holds_last_reference_without_pausing() -> None: +def test_policy_watchdog_pauses_and_holds_last_reference() -> None: worker = object.__new__(_RobotControlWorker) worker.high_level_policy_enabled = True worker.mode = RobotMode.POLICY worker._policy_paused = False worker._policy_resume_pending = False worker._policy_resume_deadline_s = None + worker._policy_resume_source_timestamp_ns = None + paused: list[float] = [] worker._high_level_policy_scheduler = SimpleNamespace( sample=lambda _now_s: None, + pause=lambda now_s: paused.append(float(now_s)), ) worker._policy_frame_transform = SimpleNamespace() worker._policy_session_id = "session-1" worker.robot = SimpleNamespace(get_state=lambda: SimpleNamespace()) worker._publish_high_level_policy_observation = lambda _state: None - worker._policy_request_pending = True - worker._policy_request_sequence_id = 1 - worker._policy_request_deadline_s = time.monotonic() + 1.0 hold_qpos = np.arange(36, dtype=np.float64) - worker._policy_hold_qpos = hold_qpos.copy() worker._last_commanded_motion_qpos = hold_qpos.copy() worker._last_retarget_qpos = None worker._publish_high_level_policy_session = lambda _command: None @@ -1106,8 +1081,9 @@ def test_synchronous_policy_wait_holds_last_reference_without_pausing() -> None: worker._high_level_policy_step() assert worker.mode == RobotMode.POLICY - assert not worker._policy_paused + assert worker._policy_paused assert not worker._policy_resume_pending + assert len(paused) == 1 assert len(held) == 1 np.testing.assert_array_equal(held[0], hold_qpos) @@ -1145,14 +1121,12 @@ def test_policy_pause_resume_waits_for_fresh_chunk_then_resumes() -> None: worker._policy_paused = True worker._policy_resume_pending = False worker._policy_resume_deadline_s = None - worker._policy_request_pending = True - worker._policy_request_sequence_id = 0 - worker._policy_request_deadline_s = time.monotonic() + 1.0 - discarded: list[str] = [] + worker._policy_resume_source_timestamp_ns = None + resumed: list[float] = [] accepted: list[object] = [] worker._high_level_policy_scheduler = SimpleNamespace( accept=lambda *args, **kwargs: accepted.append((args, kwargs)), - discard_chunk=lambda: discarded.append("discard"), + resume=lambda now_s: resumed.append(float(now_s)), ) worker._high_level_policy_cfg = SimpleNamespace( entry_timeout_s=1.0, @@ -1166,13 +1140,9 @@ def test_policy_pause_resume_waits_for_fresh_chunk_then_resumes() -> None: assert worker._policy_paused assert worker._policy_resume_pending assert session_commands == ["resume"] - assert discarded == ["discard"] - assert not worker._policy_request_pending - source_timestamp_ns = int(round(time.monotonic() * 1e9)) - worker._policy_request_pending = True - worker._policy_request_sequence_id = 1 - worker._policy_request_deadline_s = time.monotonic() + 1.0 + source_timestamp_ns = worker._policy_resume_source_timestamp_ns + assert source_timestamp_ns is not None worker._policy_video_sub = SimpleNamespace(recv_latest=lambda: None) worker._policy_status_sub = SimpleNamespace(recv_latest=lambda: None) worker._policy_action_sub = SimpleNamespace( @@ -1193,9 +1163,9 @@ def test_policy_pause_resume_waits_for_fresh_chunk_then_resumes() -> None: worker._drain_high_level_policy_ipc() assert len(accepted) == 1 + assert len(resumed) == 1 assert not worker._policy_paused assert not worker._policy_resume_pending - assert not worker._policy_request_pending def test_policy_resume_rejects_action_received_after_deadline() -> None: @@ -1222,12 +1192,11 @@ def test_policy_resume_rejects_action_received_after_deadline() -> None: worker._policy_paused = True worker._policy_resume_pending = True worker._policy_resume_deadline_s = now_s - 0.01 - worker._policy_request_pending = True - worker._policy_request_sequence_id = 1 - worker._policy_request_deadline_s = now_s + 1.0 + worker._policy_resume_source_timestamp_ns = int(round((now_s - 0.1) * 1e9)) accepted: list[object] = [] worker._high_level_policy_scheduler = SimpleNamespace( accept=lambda *args, **kwargs: accepted.append((args, kwargs)), + resume=lambda _now_s: pytest.fail("expired resume must not resume scheduler"), ) worker._high_level_policy_cfg = SimpleNamespace(max_result_age_s=1.0) faults: list[str] = [] @@ -1261,9 +1230,6 @@ def test_paused_robot_worker_discards_inflight_policy_result() -> None: worker._policy_session_id = "session-1" worker._policy_paused = True worker._policy_resume_pending = False - worker._policy_request_pending = True - worker._policy_request_sequence_id = 1 - worker._policy_request_deadline_s = 2.0 accepted: list[object] = [] worker._high_level_policy_scheduler = SimpleNamespace( accept=lambda *args, **kwargs: accepted.append((args, kwargs)) @@ -1273,4 +1239,3 @@ def test_paused_robot_worker_discards_inflight_policy_result() -> None: worker._drain_high_level_policy_ipc() assert accepted == [] - assert not worker._policy_request_pending From e76e9472d942f16d9122ba1dffdbb74bae449573 Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Mon, 27 Jul 2026 20:26:04 +0800 Subject: [PATCH 44/59] Lower standing return Kp ramp floor --- teleopit/configs/high_level_policy_sim2real.yaml | 1 + teleopit/configs/pico4_sim2real.yaml | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/teleopit/configs/high_level_policy_sim2real.yaml b/teleopit/configs/high_level_policy_sim2real.yaml index 4f7f51d1..c3fbb247 100644 --- a/teleopit/configs/high_level_policy_sim2real.yaml +++ b/teleopit/configs/high_level_policy_sim2real.yaml @@ -9,6 +9,7 @@ input: # Smooth an explicit return from POLICY to STANDING. standing_return_ramp_duration: 2.0 +standing_return_kp_ramp_floor_ratio: 0.1 camera: source: realsense # realsense | test-pattern diff --git a/teleopit/configs/pico4_sim2real.yaml b/teleopit/configs/pico4_sim2real.yaml index 3887185e..d4435a84 100644 --- a/teleopit/configs/pico4_sim2real.yaml +++ b/teleopit/configs/pico4_sim2real.yaml @@ -60,7 +60,7 @@ startup_ramp_duration: 2.0 kp_ramp_floor_ratio: 0.1 # Faster Kp ramp used when returning from MOCAP to default STANDING with X standing_return_ramp_duration: 0.5 -standing_return_kp_ramp_floor_ratio: 0.5 +standing_return_kp_ramp_floor_ratio: 0.1 # Joint velocity safety limit (rad/s) -- trigger emergency damping if exceeded joint_vel_limit: 10.0 From 3d1be2c4b55d37eb794571be08e7375d19e78844 Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Mon, 27 Jul 2026 21:01:59 +0800 Subject: [PATCH 45/59] Restore standing return Kp ramp floors --- teleopit/configs/high_level_policy_sim2real.yaml | 1 - teleopit/configs/pico4_sim2real.yaml | 2 +- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/teleopit/configs/high_level_policy_sim2real.yaml b/teleopit/configs/high_level_policy_sim2real.yaml index c3fbb247..4f7f51d1 100644 --- a/teleopit/configs/high_level_policy_sim2real.yaml +++ b/teleopit/configs/high_level_policy_sim2real.yaml @@ -9,7 +9,6 @@ input: # Smooth an explicit return from POLICY to STANDING. standing_return_ramp_duration: 2.0 -standing_return_kp_ramp_floor_ratio: 0.1 camera: source: realsense # realsense | test-pattern diff --git a/teleopit/configs/pico4_sim2real.yaml b/teleopit/configs/pico4_sim2real.yaml index d4435a84..3887185e 100644 --- a/teleopit/configs/pico4_sim2real.yaml +++ b/teleopit/configs/pico4_sim2real.yaml @@ -60,7 +60,7 @@ startup_ramp_duration: 2.0 kp_ramp_floor_ratio: 0.1 # Faster Kp ramp used when returning from MOCAP to default STANDING with X standing_return_ramp_duration: 0.5 -standing_return_kp_ramp_floor_ratio: 0.1 +standing_return_kp_ramp_floor_ratio: 0.5 # Joint velocity safety limit (rad/s) -- trigger emergency damping if exceeded joint_vel_limit: 10.0 From b667cc45cec20a86f0a8766625fe98a78c85490d Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Wed, 29 Jul 2026 19:02:45 +0800 Subject: [PATCH 46/59] Revise documentation structure and workflows --- docs/docs/getting-started/download-assets.md | 49 --- docs/docs/getting-started/installation.md | 165 ++++--- docs/docs/getting-started/quick-start.md | 64 --- docs/docs/intro.md | 58 ++- docs/docs/reference/architecture.md | 29 +- docs/docs/reference/assets.md | 24 +- docs/docs/reference/dataset.md | 67 ++- docs/docs/tutorials/offline-sim2sim.md | 134 ++++-- docs/docs/tutorials/pico-sim2real.md | 410 +++++++----------- docs/docs/tutorials/pico-sim2sim.md | 192 ++++---- docs/docs/tutorials/training.md | 171 +++++--- docs/docusaurus.config.ts | 2 +- .../current.json | 22 + .../getting-started/download-assets.md | 49 --- .../current/getting-started/installation.md | 153 ++++--- .../current/getting-started/quick-start.md | 64 --- .../current/intro.md | 75 ++-- .../current/reference/architecture.md | 27 +- .../current/reference/assets.md | 23 +- .../current/reference/dataset.md | 61 ++- .../current/tutorials/offline-sim2sim.md | 130 ++++-- .../current/tutorials/pico-sim2real.md | 384 ++++++---------- .../current/tutorials/pico-sim2sim.md | 172 ++++---- .../current/tutorials/training.md | 163 ++++--- .../docusaurus-theme-classic/footer.json | 2 +- docs/sidebars.ts | 5 - 26 files changed, 1364 insertions(+), 1331 deletions(-) delete mode 100644 docs/docs/getting-started/download-assets.md delete mode 100644 docs/docs/getting-started/quick-start.md create mode 100644 docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current.json delete mode 100644 docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/download-assets.md delete mode 100644 docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/quick-start.md diff --git a/docs/docs/getting-started/download-assets.md b/docs/docs/getting-started/download-assets.md deleted file mode 100644 index 81854a30..00000000 --- a/docs/docs/getting-started/download-assets.md +++ /dev/null @@ -1,49 +0,0 @@ ---- -sidebar_position: 2 ---- - -# Download Assets - -Robot models, datasets, and checkpoints are hosted on ModelScope and must be downloaded before use. - -## One-Click Download - -Download all assets (models, data, GMR retargeting assets): - -```bash -pip install modelscope -python scripts/setup/download_assets.py -``` - -## Selective Download - -Download only what you need for inference: - -```bash -python scripts/setup/download_assets.py --only robots gmr ckpt bvh -``` - -## Asset Inventory - -Downloaded file sizes change as checkpoints, datasets, and asset bundles are updated. Use the repository paths below as the stable contract. - -| Local Path | Purpose | -|------------|---------| -| `track.onnx` | ONNX inference model | -| `track.pt` | PyTorch checkpoint for resume training | -| `data/datasets//shard_*.h5` | Minimal motion datasets; run precompute before training | -| `data/sample_bvh/*.bvh` | Sample motion files | -| `assets/robots/unitree_g1/` | Canonical G1 XML and meshes used by training, sim2sim, retargeting, and FK validation | -| `teleopit/retargeting/gmr/assets/` | GMR retargeting assets, IK configs, and non-canonical robot descriptions | - -## Asset Groups - -| Group | ModelScope Repo | Contents | -|-------|----------------|----------| -| `ckpt` | `BingqianWu/Teleopit-models` | `track.onnx`, `track.pt` | -| `robots` | `BingqianWu/Teleopit-models` | Canonical robot XML/meshes | -| `gmr` | `BingqianWu/Teleopit-models` | GMR retargeting assets | -| `bvh` | `BingqianWu/Teleopit-models` | Sample BVH motion files | -| `data` | `BingqianWu/Teleopit-datasets` | Minimal shards for `lafan1`, `pico_record`, `seed`, and `twist2` | - -For asset management details (uploading, versioning), see [Asset Management](../reference/assets). diff --git a/docs/docs/getting-started/installation.md b/docs/docs/getting-started/installation.md index 6deefc72..cdb79900 100644 --- a/docs/docs/getting-started/installation.md +++ b/docs/docs/getting-started/installation.md @@ -2,67 +2,116 @@ sidebar_position: 1 --- -# Installation +# Install Teleopit -Teleopit supports multiple installation profiles depending on your use case. +Install only the parts you need. All commands below run from the repository +root and require Python 3.10 or newer. -## Prerequisites - -- Python 3.10+ -- [Conda](https://docs.conda.io/) (recommended) +## 1. Get the Code ```bash -conda create -n teleopit python=3.10 -conda activate teleopit +git clone https://github.com/BotRunner64/Teleopit.git +cd Teleopit ``` -## Install Profiles +You only need Git submodules for a physical G1 or optional LinkerHand control; +those steps appear later on this page. + +## 2. Create a Python Environment + +Choose one environment tool. Do not run all three sections. -### Inference Only (sim2sim) +### uv ```bash -pip install -e . +uv venv --python 3.10 +source .venv/bin/activate ``` -This is sufficient for offline BVH playback and MuJoCo simulation. +When this page shows `pip install`, you may use `uv pip install` instead. -### Training +### pip and venv ```bash -pip install -e '.[train]' +python3.10 -m venv .venv +source .venv/bin/activate +python -m pip install --upgrade pip ``` -Adds `rsl-rl-lib`, `mjlab`, `wandb`, `swanlab`, and training dependencies. +### Conda -### Sim2Real (Hardware Deployment) +```bash +conda create -n teleopit python=3.10 +conda activate teleopit +``` + +Conda creates the environment; use `pip install` inside that environment to +install Teleopit. + +## 3. Install the Profile You Need + +Each extra includes the base Teleopit package. Start with the row matching your +goal; you can install another extra later in the same environment. + +| Goal | Install command | What it adds | +|------|-----------------|--------------| +| Run a motion controller in MuJoCo | `pip install -e .` | Core inference, GMR, MuJoCo and ONNX Runtime | +| Use Pico in simulation or on G1 | `pip install -e '.[pico4]'` | Pico receiver plus the sim2real runtime | +| Replay BVH on a physical G1 without Pico | `pip install -e '.[sim2real]'` | G1 runtime and OpenCV | +| Train a controller | `pip install -e '.[train]'` | mjlab, RSL-RL and experiment loggers | +| Record Pico sim2real episodes | `pip install -e '.[recording]'` | Pico runtime and MP4 writing | +| Review saved recordings | `pip install -e '.[review]'` | OpenCV and the MuJoCo/Viser reviewer | +| Use OpenNeck with Pico | `pip install -e '.[openneck]'` | Pico runtime and the OpenNeck driver | +| Run the test suite | `pip install -e '.[dev]'` | pytest and coverage tools | + +## 4. Download the Matching Assets + +The Python package does not contain robot meshes, policies or motion datasets. +Install the default ModelScope downloader once: ```bash -pip install -e '.[sim2real]' +pip install modelscope ``` -Adds `opencv-python`. You also need to initialize submodules and build/install the C++ `g1_bridge_sdk` bridge: +Then download the bundle for your goal: + +| Goal | Command | +|------|---------| +| Simulation, Pico VR or G1 inference | `python scripts/setup/download_assets.py --only robots gmr ckpt bvh` | +| Training from the distributed datasets | `python scripts/setup/download_assets.py --only robots data` | +| Everything | `python scripts/setup/download_assets.py` | + +Use HuggingFace instead of ModelScope when needed: ```bash -git submodule update --init --recursive -bash scripts/setup/setup_g1_bridge.sh +python scripts/setup/download_assets.py \ + --source huggingface \ + --only robots gmr ckpt bvh ``` -See [G1 Bridge SDK](../reference/g1-bridge-sdk) for details. +The inference bundle creates `track.onnx`, the canonical G1 model, GMR files and +a sample BVH under their expected project paths. See +[Asset Reference](../reference/assets) for the complete inventory and asset +group mapping. -### Pico 4 VR +## 5. Additional Setup for a Physical G1 + +Build the C++ DDS bridge on the computer that will run Teleopit: ```bash -pip install -e '.[pico4]' +git submodule update --init --recursive +bash scripts/setup/setup_g1_bridge.sh ``` -Teleopit uses the in-process `pico_bridge.PicoBridge` receiver for Pico tracking. -Teleopit targets pico-bridge 0.2.1 and its `pico_native` tracking semantics. -The receiver can run on a workstation PC or the robot onboard computer. -See [Pico Sim2Sim](../tutorials/pico-sim2sim) and -[Pico Sim2Real](../tutorials/pico-sim2real) for the full setup guides. +The bridge is required for both Pico and BVH control on a real G1. See +[G1 Bridge SDK](../reference/g1-bridge-sdk) if the build or robot connection +fails. + +## 6. Optional Hardware -Optional LinkerHand control for Pico sim2real uses local third-party packages. -Install those packages directly after initializing the submodules: +### LinkerHand L6 or O6 + +Only install these local packages when `hands.enabled=true`: ```bash git submodule update --init --recursive @@ -71,54 +120,54 @@ pip install -e third_party/somehand bash scripts/setup/download_somehand_assets.sh ``` -These packages are only required when `hands.enabled=true`. +### OpenNeck -Optional OpenNeck active-vision control for Pico sim2real uses the latest -OpenNeck angle-control package: +The `openneck` extra already includes the Pico profile. Calibrate the device +before enabling it: ```bash pip install -e '.[openneck]' +openneck calibrate ``` -This extra includes the Pico stack and is only required when `neck.enabled=true`. -OpenNeck 0.2.0 calibration files use `*_center_step`, `*_min_step`, -`*_max_step`, and `*_step_sign`; the previous normalized configuration format -is unsupported. Run `openneck calibrate` to create a current calibration file. +Teleopit uses the OpenNeck angle API. Old normalized calibration fields are not +supported. + +### RealSense Recording or Preview -### Sim2Real Recording +Install `pyrealsense2` separately when a RealSense camera is enabled. On Arm +machines, use conda-forge: ```bash -pip install -e '.[recording]' +conda install -c conda-forge pyrealsense2 ``` -Adds the Pico sim2real stack plus the video dependencies used by -`sim2real_record.yaml`. RealSense Python bindings are platform-specific: install -`pyrealsense2` manually in the active environment when using -`input.video.source=realsense`. On Arm machines, use conda-forge rather than the -pip package: +Pico body tracking itself does not require RealSense. + +## 7. Verify the Environment + +Run the core import check: ```bash -conda install -c conda-forge pyrealsense2 +python -c "import teleopit; print('teleopit OK')" ``` -### Recording Review +If you installed Pico or training dependencies, run the matching check: ```bash -pip install -e '.[review]' +python -c "from pico_bridge import PicoBridge; print('Pico OK')" +python -c "import train_mimic.tasks; print('training OK')" ``` -Adds the OpenCV and MuJoCo/Viser dependencies used by the read-only synchronized -sim2real recording reviewer. The review extra does not install Pico, RealSense, -or G1 control dependencies. - -## Verify Installation +For an inference profile with the `robots gmr ckpt bvh` assets, finish with one +sample simulation: ```bash -python -c "import teleopit; print('teleopit OK')" -python -c "import train_mimic.tasks; print('training OK')" # if training installed +python scripts/run/run_sim.py \ + controller.policy_path=track.onnx \ + input.bvh_file=data/sample_bvh/aiming1_subject1.bvh ``` -## Next Steps - -- [Download Assets](download-assets) - Download models and data -- [Quick Start](quick-start) - Run your first simulation +The installation is ready when a MuJoCo window opens and the simulated G1 +follows the sample motion. Close the window to stop, then continue with one of +the four task-based tutorials. diff --git a/docs/docs/getting-started/quick-start.md b/docs/docs/getting-started/quick-start.md deleted file mode 100644 index 510a6d26..00000000 --- a/docs/docs/getting-started/quick-start.md +++ /dev/null @@ -1,64 +0,0 @@ ---- -sidebar_position: 3 ---- - -# Quick Start - -This guide walks you through running your first sim2sim playback in under 5 minutes. - -## Prerequisites - -1. [Install Teleopit](installation) (inference profile) -2. [Download assets](download-assets) (`--only robots gmr ckpt bvh`) - -## Run Offline Sim2Sim - -```bash -python scripts/run/run_sim.py \ - controller.policy_path=track.onnx \ - input.bvh_file=data/sample_bvh/aiming1_subject1.bvh -``` - -You should see MuJoCo viewer windows showing the robot tracking the BVH motion. - -## Keyboard Controls - -When running with `playback.keyboard.enabled=true`: - -| Key | Action | -|-----|--------| -| `Space` / `P` | Pause / Resume | -| `R` | Replay from start | -| `Q` | Stop | - -```bash -python scripts/run/run_sim.py \ - controller.policy_path=track.onnx \ - input.bvh_file=data/sample_bvh/aiming1_subject1.bvh \ - playback.keyboard.enabled=true -``` - -## Viewer Modes - -Control which viewers are displayed: - -```bash -# All viewers (mocap + retarget + sim2sim) -python scripts/run/run_sim.py controller.policy_path=track.onnx viewers=all - -# No viewer (headless) -python scripts/run/run_sim.py controller.policy_path=track.onnx viewers=none - -# Specific viewers -python scripts/run/run_sim.py controller.policy_path=track.onnx 'viewers=[retarget,sim2sim]' -``` - -## What's Next - -- [Offline Sim2Sim Tutorial](../tutorials/offline-sim2sim) - Full guide with rendering -- [Pico Sim2Sim](../tutorials/pico-sim2sim) - Verify Pico tracking in MuJoCo -- [Standalone Standing](../tutorials/standalone-standing) - Check G1 bridge, network, and policy standing -- [Pico Sim2Real](../tutorials/pico-sim2real) - Deploy Pico teleoperation to Unitree G1 -- [BVH Sim2Real](../tutorials/bvh-sim2real) - Replay offline BVH motions on Unitree G1 -- [Host Policy Sim2Real](../tutorials/high-level-policy-sim2real) - Connect an independent LeRobot policy host to the onboard motion tracker -- [Training](../tutorials/training) - Train your own policy diff --git a/docs/docs/intro.md b/docs/docs/intro.md index 8555ed00..4062f8f0 100644 --- a/docs/docs/intro.md +++ b/docs/docs/intro.md @@ -3,45 +3,41 @@ sidebar_position: 1 slug: / --- -# Introduction +# Teleopit > **Looking for Chinese docs?** [中文文档点此进入](https://BotRunner64.github.io/Teleopit/zh-Hans/) -**Teleopit** is a lightweight, extensible whole-body teleoperation framework for humanoid robots. It provides real-time motion retargeting from human operators to Unitree G1 robots, supporting both MuJoCo simulation and real hardware deployment. +Teleopit is a **full-embodiment teleoperation system for the Unitree G1**. +With a Pico 4 or Pico 4 Ultra, an operator can drive the robot's whole-body +motion in real time. Optional LinkerHand hands reproduce hand gestures, and an +optional OpenNeck gimbal turns head motion into active camera control. -## Key Features +The same motion controller runs in MuJoCo first, so you can check tracking and +controls before connecting a physical robot. -- **Offline sim2sim**: Play back BVH motion capture files through RL policy in MuJoCo -- **VR teleoperation**: Real-time whole-body control via Pico 4 / Pico 4 Ultra full body tracking -- **Sim2real deployment**: Deploy to Unitree G1 hardware with the same pipeline -- **Training pipeline**: End-to-end RL training with General-Tracking-G1 task -- **Extensible design**: Protocol-based components (InputProvider, Retargeter, Controller, Robot) +## Start Here -## Pipeline Overview +If this is your first time using Teleopit: -```text -InputProvider (BVH / Pico4 VR) - -> Retargeter (GMR) - -> ObservationBuilder (167D) - -> Controller (dual-input TemporalCNN ONNX) - -> Robot (MuJoCo sim or Unitree G1) -``` +1. [Install Teleopit](getting-started/installation) for the job you want to do + and complete the check at the end of that page. +2. Continue with one of the four guides below. -## Technical Specs +| I want to... | Follow this guide | +|--------------|-------------------| +| Check a motion controller in MuJoCo | [Run a Motion Controller in Simulation](tutorials/offline-sim2sim) | +| Try Pico VR control without a real robot | [VR Teleoperation in Simulation](tutorials/pico-sim2sim) | +| Control a physical G1 with Pico VR | [VR Teleoperation on Unitree G1](tutorials/pico-sim2real) | +| Train and export my own controller | [Train a Motion Controller](tutorials/training) | -| Spec | Value | -|------|-------| -| Policy frequency | 50 Hz | -| PD control frequency | 200 Hz | -| Observation dimension | 167D | -| Action dimension | 29D (G1 joints) | -| ONNX model | Dual-input TemporalCNN | -| Retargeting | GMR (General Motion Retargeting) | -| Simulator | MuJoCo | -| Hardware | Unitree G1 (29 DOF) | +:::warning Before using a real robot +Make the Pico workflow work in simulation first. Keep the Unitree remote in +hand during hardware operation; `L1+R1` is the emergency path to `DAMPING`. +::: -## What's Next +## Looking for Implementation Details? -- [Installation](getting-started/installation) - Set up your environment -- [Quick Start](getting-started/quick-start) - Run your first sim2sim -- [Tutorials](tutorials/offline-sim2sim) - Step-by-step guides for each use case +The user guides intentionally keep internals out of the main flow. See +[Architecture](reference/architecture) for the runtime pipeline and technical +specifications, [Asset Reference](reference/assets) for every downloaded file, +or [Configuration](configuration/overview) for Hydra options. diff --git a/docs/docs/reference/architecture.md b/docs/docs/reference/architecture.md index 26998e2d..d97b95f3 100644 --- a/docs/docs/reference/architecture.md +++ b/docs/docs/reference/architecture.md @@ -4,7 +4,8 @@ sidebar_position: 1 # Architecture -System internals and technical constraints for developers. +This page collects the runtime pipeline, supported boundaries and exact +dimensions that are intentionally omitted from the task-based user guides. ## Pipeline @@ -18,6 +19,25 @@ InputProvider (BVH file / Pico4) Offline/online inference is assembled by `teleopit/runtime/` and `teleopit/pipeline.py`. The hardware state machine runs through the process-isolated runtime in `teleopit/sim2real/mp/`. Training is provided by `train_mimic/`. +## Full-Embodiment Pico Path + +One Pico frame can feed three independent control paths: + +```text +Pico full-body tracking + -> GMR retargeting -> tracking policy -> G1 whole-body joints + +Pico hand tracking or controller input + -> Teleopit hand adapter -> somehand or gripper mapping -> LinkerHand L6/O6 + +Pico HMD rotation + same-frame Spine3 rotation + -> relative yaw/pitch mapping -> OpenNeck +``` + +Whole-body control is the required path. Hands and OpenNeck are optional +process-isolated workers; their failure must not stop G1 body control. All +three paths reuse the same in-process PicoBridge receiver. + Host-served imitation policies use a second, independent deployment path: ```text @@ -74,13 +94,20 @@ train_mimic/scripts/data | Spec | Value | |------|-------| +| Supported robot | Unitree G1, 29 actuated joints | +| Simulator | MuJoCo | +| Motion retargeting | GMR (General Motion Retargeting) | +| Policy / PD rates | 50 Hz / 200 Hz | | Training task | `General-Tracking-G1` | | Inference observation | `velcmd_history` (167D) | | ONNX signature | Dual-input `obs` (167D) + `obs_history` | +| Policy action | 29D joint offsets from `default_dof_pos` | | Actor/Critic | TemporalCNN (2048, 1024, 512, 256, 128) | | Training sampling | Default `rewind`; also supports `uniform`; playback uses `start`; benchmark pins exact clips and disables clip-end resampling | | Training `window_steps` | `[0]` | | Data format | Minimal recursive HDF5 shards (`shard_*.h5`) | +| Optional hands | LinkerHand L6 or O6, gripper or Pico hand-pose input | +| Optional active vision | OpenNeck yaw/pitch in physical degrees | | Host-policy observation | JPEG RGB + `observation.state(68)` | | Host-policy action | `float32[T,50]` canonical reference at 30 Hz | | Host-policy body control | 36D root/joint reference through the existing 50 Hz motion tracker | diff --git a/docs/docs/reference/assets.md b/docs/docs/reference/assets.md index daf811a3..0972d840 100644 --- a/docs/docs/reference/assets.md +++ b/docs/docs/reference/assets.md @@ -2,9 +2,12 @@ sidebar_position: 2 --- -# Asset Management +# Asset Reference -Datasets, checkpoints, robot models, and demo media are not tracked in Git. They are distributed via ModelScope and HuggingFace. The canonical Unitree G1 model is downloaded to `assets/robots/unitree_g1/g1_29dof.xml`. +Teleopit's Git repository contains code, not large robot meshes, policies or +motion data. [Installation](../getting-started/installation) shows the shortest +download command for each user workflow; this page is the complete inventory +and maintainer reference. ## What's Not in Git @@ -13,6 +16,19 @@ Datasets, checkpoints, robot models, and demo media are not tracked in Git. They - `data/`, checkpoints, caches - Demo media (`assets/demo.gif`, `assets/demo.mp4`) +## Asset Inventory + +| Group | Local result | Used for | +|-------|--------------|----------| +| `ckpt` | `track.onnx`, `track.pt` | Ready-to-run inference and the matching PyTorch checkpoint | +| `robots` | `assets/robots/unitree_g1/g1_29dof.xml` and meshes | Training, MuJoCo inference, GMR and dataset FK | +| `gmr` | `teleopit/retargeting/gmr/assets/` | Retargeting models and IK configuration | +| `bvh` | `data/sample_bvh/*.bvh` | Sample motions used by the installation check and simulation tutorial | +| `data` | `data/datasets//shard_*.h5` | Minimal distributed motion datasets; precompute before training | + +The file `assets/robots/unitree_g1/g1_29dof.xml` is the canonical G1 entry +point. XML files inside the GMR asset directory are not replacements for it. + ## Repositories ### ModelScope (default download source) @@ -29,7 +45,7 @@ Datasets, checkpoints, robot models, and demo media are not tracked in Git. They | `12e21/Teleopit-models` | model | Checkpoints, GMR retargeting assets, sample BVH | | `12e21/Teleopit-datasets` | dataset | Training/validation datasets | -### Asset Group Mapping +### Asset Group and Repository Mapping | Group | Repository | Remote Path | |-------|-----------|-------------| @@ -39,7 +55,7 @@ Datasets, checkpoints, robot models, and demo media are not tracked in Git. They | `bvh` | Teleopit-models | `archives/sample_bvh.tar.gz` | | `data` | Teleopit-datasets | `data/datasets/*/*.h5` (`lafan1`, `pico_record`, `seed`, `twist2`) | -## Download +## Download Behavior Use the project download script (defaults to ModelScope): diff --git a/docs/docs/reference/dataset.md b/docs/docs/reference/dataset.md index 08e9178c..14bb8e9f 100644 --- a/docs/docs/reference/dataset.md +++ b/docs/docs/reference/dataset.md @@ -2,7 +2,15 @@ sidebar_position: 3 --- -# Dataset +# Dataset Reference + +Teleopit uses two separate dataset families: + +- **motion datasets** provide reference motion for controller training, and +- **sim2real episode recordings** store synchronized robot state, references + and camera video for later review or external policy work. + +They have different schemas and are not interchangeable. ## Download Pre-Built Dataset (Recommended) @@ -171,3 +179,60 @@ python train_mimic/scripts/data/check_motion_npz_fk.py \ ``` Recommended thresholds: `pos_max < 1e-3 m`, `quat_mean < 0.05 rad`, `quat_p95 < 0.10 rad`. + +## Sim2Real Episode Recordings + +The recording runtime writes an editable dataset rather than one self-contained +HDF5 file: + +```text +data/recordings/sim2real_hdf5/ +├── schema.json +├── episodes.jsonl +├── data/ +│ └── episode_000000.h5 +└── videos/ + └── d435i_rgb/ + └── episode_000000.mp4 +``` + +`schema.json` defines the dataset FPS, `robot_type`, `hand_type`, `neck_type` +and every feature's shape, dtype, names and groups. The hardware types must +match the active runtime configuration. + +`episodes.jsonl` is the editable episode manifest. Each line maps one episode +to its HDF5 and MP4 files and stores the task prompt. Task text is not copied +into HDF5 attributes. + +Each HDF5 file contains only frame-aligned arrays: + +| Field | Shape | Meaning | +|-------|-------|---------| +| `frame_index` | scalar | Camera/action frame index | +| `timestamp` | scalar | Monotonic timestamp in seconds | +| `observation.state` | `(68,)` | G1 joint state, base orientation/angular velocity and projected gravity | +| `observation.mode` | scalar | `STANDING`, `MOCAP`, `ARMS` or paused mocap code | +| `action` | `(36,)` | Root pose plus 29-joint reference consumed by the motion tracker | +| `action.hand` | `(12,)`, optional | Left/right LinkerHand target when hand control is enabled | +| `action.neck` | `(2,)`, optional | Mechanically clamped OpenNeck yaw/pitch target in degrees | + +Camera RGB is stored only in the MP4 sidecar; HDF5 does not contain a duplicate +raw image dataset. Optional action fields appear exactly when the matching +hardware is enabled. + +The recorder commits the HDF5/video files before appending the manifest entry. +An interrupted, uncommitted episode is removed on the next recording-worker +startup and does not consume an episode index. An existing incompatible +`schema.json` stops only the non-critical recording worker. + +Review the dataset with: + +```bash +python scripts/view/view_recording.py \ + --recording data/recordings/sim2real_hdf5 +``` + +The reviewer validates manifest paths, HDF5 shapes/dtypes/finite values and MP4 +alignment before playback. Measured root XYZ is not recorded, so its observed +robot view is anchored to the reference root position; global root translation +cannot be evaluated from this format. diff --git a/docs/docs/tutorials/offline-sim2sim.md b/docs/docs/tutorials/offline-sim2sim.md index 57455374..26748c74 100644 --- a/docs/docs/tutorials/offline-sim2sim.md +++ b/docs/docs/tutorials/offline-sim2sim.md @@ -2,75 +2,108 @@ sidebar_position: 1 --- -# Offline Sim2Sim +# Run a Motion Controller in Simulation -Run BVH motion capture files through the RL policy in MuJoCo simulation. +Use this guide to watch a trained controller reproduce a motion in MuJoCo. This +is the quickest way to answer two basic questions before adding VR or a real +robot: -## Basic Playback +- Does the policy load and keep the G1 stable? +- Does the retargeted motion look like the source motion? + +## Before You Start + +Complete [Installation](../getting-started/installation) with the base profile +and the `robots gmr ckpt bvh` asset bundle. + +## 1. Run the Sample Motion ```bash python scripts/run/run_sim.py \ controller.policy_path=track.onnx \ - input.bvh_file=data/sample_bvh/aiming1_subject1.bvh + input.bvh_file=data/sample_bvh/aiming1_subject1.bvh \ + playback.keyboard.enabled=true ``` -### Using hc_mocap Format +The `sim2sim` window is the result that matters: it shows the G1 produced by +physics and the policy, not just a kinematic target. + +| Key | Action | +|-----|--------| +| `Space` or `P` | Pause or resume | +| `R` | Replay from the first frame | +| `Q` | Stop | + +The run is healthy when the robot remains stable and follows the overall timing +and pose of the clip. Small tracking error is normal; falling, frozen joints or +a clearly wrong facing direction is not. + +## 2. Compare the Three Views + +Open all views when you need to find where a bad result starts: ```bash python scripts/run/run_sim.py \ controller.policy_path=track.onnx \ - input.bvh_file=data/hc_mocap/walk.bvh \ - input.bvh_format=hc_mocap + input.bvh_file=data/sample_bvh/aiming1_subject1.bvh \ + viewers=all ``` -## Keyboard Playback +| View | What you are looking at | +|------|-------------------------| +| `mocap` | The human skeleton read from the BVH file | +| `retarget` | The kinematic G1 pose produced by GMR | +| `sim2sim` | The G1 after policy inference and MuJoCo physics | + +If `mocap` is wrong, check the BVH format. If `mocap` looks right but +`retarget` does not, inspect the retargeting setup. If only `sim2sim` is wrong, +check the policy and observation configuration. -Enable interactive control for offline BVH playback: +You can also select views explicitly: ```bash +# Only the physics result python scripts/run/run_sim.py \ controller.policy_path=track.onnx \ input.bvh_file=data/sample_bvh/aiming1_subject1.bvh \ - playback.keyboard.enabled=true -``` + viewers=sim2sim -| Key | Action | -|-----|--------| -| `Space` / `P` | Pause / Resume | -| `R` | Replay from start | -| `Q` | Stop | +# No windows; useful for a server or timing test +python scripts/run/run_sim.py \ + controller.policy_path=track.onnx \ + input.bvh_file=data/sample_bvh/aiming1_subject1.bvh \ + viewers=none +``` -Additional options: +Closing every active viewer ends the simulation. -```bash -# Pause at end of motion -playback.pause_on_end=true +## 3. Try Your Own BVH -# Limit number of steps (0 = infinite) -num_steps=300 +For a LAFAN1-style file: -# Wall-clock rate limiting (even without viewer) -realtime=true +```bash +python scripts/run/run_sim.py \ + controller.policy_path=track.onnx \ + input.bvh_file=/path/to/motion.bvh \ + input.bvh_format=lafan1 ``` -## Viewer Modes - -Viewers run in separate subprocesses. Use shell quotes for list overrides. +For an `hc_mocap` file: ```bash -viewers=sim2sim # Default -viewers=all # mocap + retarget + sim2sim -viewers=none # Headless -'viewers=[retarget,sim2sim]' # Specific combination +python scripts/run/run_sim.py \ + controller.policy_path=track.onnx \ + input.bvh_file=/path/to/motion.bvh \ + input.bvh_format=hc_mocap ``` -:::note -When all active viewer windows are closed, the simulation ends automatically. -::: +Teleopit does not guess an unknown skeleton layout. A file can be valid BVH and +still need an adapter before it matches a supported format. -## Offline Rendering +## 4. Save a Video -Render simulation to video (headless): +Use the renderer when you want repeatable output instead of interactive +windows: ```bash MUJOCO_GL=egl python scripts/render/render_sim.py \ @@ -78,13 +111,30 @@ MUJOCO_GL=egl python scripts/render/render_sim.py \ --policy track.onnx ``` -For hc_mocap format: +Add `--format hc_mocap` for that input format. The renderer writes synchronized +`mocap`, `retarget` and `sim2sim` videos. + +## Useful Playback Options ```bash -MUJOCO_GL=egl python scripts/render/render_sim.py \ - --bvh data/hc_mocap/wander.bvh \ - --format hc_mocap \ - --policy track.onnx +# Hold the final pose instead of exiting +playback.pause_on_end=true + +# Stop after 300 simulation steps; 0 means no step limit +num_steps=300 + +# Keep wall-clock timing even with no viewer +realtime=true ``` -The render pipeline outputs three views (mocap input, retarget, sim2sim), all using MuJoCo rendering. +For every available field, see [Configuration](../configuration/overview). + +## Common Problems + +| Symptom | Check | +|---------|-------| +| Policy path error | Confirm that `track.onnx` exists or pass the path to your exported ONNX | +| Missing XML, mesh or GMR config | Download `robots` and `gmr` assets | +| Motion is rotated or distorted from the first view | Confirm `input.bvh_format` matches the file's skeleton | +| Robot falls only in `sim2sim` | Confirm the ONNX was exported for the current 167D `velcmd_history` observation | +| EGL/OpenGL error | Try the interactive viewer on a desktop, or configure EGL before headless rendering | diff --git a/docs/docs/tutorials/pico-sim2real.md b/docs/docs/tutorials/pico-sim2real.md index 69192c7f..76fa3997 100644 --- a/docs/docs/tutorials/pico-sim2real.md +++ b/docs/docs/tutorials/pico-sim2real.md @@ -1,81 +1,77 @@ --- -sidebar_position: 4 +sidebar_position: 3 --- -# Pico 4 VR Teleoperation on Unitree G1 +# VR Teleoperation on Unitree G1 -Use this tutorial after [Pico Sim2Sim](pico-sim2sim) is working. It deploys the -same realtime Pico input path to a physical Unitree G1. +This guide moves the Pico workflow from MuJoCo to a physical Unitree G1. The +motion input is the same; the important new pieces are the G1 network, the DDS +bridge and safe operator transitions. -```text -Pico headset -> Teleopit host -> retarget -> RL policy -> g1_bridge_sdk -> G1 -``` - -There are two deployment styles: - -| Deployment | Where Teleopit Runs | Main Difference | -|------------|---------------------|-----------------| -| Wired PC-to-G1 | External workstation or laptop | Set `real_robot.network_interface` to the PC Ethernet interface connected to G1 | -| Onboard | G1 onboard computer | Install Teleopit on the onboard computer; `eth0` is usually correct | +:::danger Keep the Unitree remote in your hand +Use `L1+R1` to enter `DAMPING` whenever motion is unexpected. Start with clear +space around the robot and an operator ready to support or stop it. +::: -Both styles use `Pico4InputProvider` and the in-process pico-bridge receiver. -There is no separate onboard Pico input mode. +## Before You Start -Teleopit targets pico-bridge 0.2.1 and its `pico_native` tracking semantics. +Do not continue until all of these are true: -## 1. Install Runtime Dependencies +- [VR Teleoperation in Simulation](pico-sim2sim) works reliably. +- You installed the `pico4` profile and built `g1_bridge_sdk` as described in + [Installation](../getting-started/installation). +- `track.onnx`, robot assets and GMR assets are present. +- The computer running Teleopit has a wired DDS connection to the G1. +- No other program is commanding the robot. -Install Pico and sim2real dependencies on the machine that will run Teleopit: +Teleopit may run on an external PC connected to G1 by Ethernet or on the G1 +onboard computer. Pico still connects directly to the machine running Teleopit. -```bash -pip install -e '.[pico4]' -git submodule update --init --recursive -bash scripts/setup/setup_g1_bridge.sh -``` +## 1. Find the G1 Network Interface -Verify Pico receiver import: +List the Linux interfaces: ```bash -python -c "from pico_bridge import PicoBridge; print('OK')" +ip -br link ``` -## 2. Choose The Network Interface +For a wired PC, use the Ethernet interface connected to G1, such as +`enp130s0`. On the onboard computer, it is usually `eth0`. -`real_robot.network_interface` is the Linux interface used for Unitree DDS -communication. +The value is passed as: -For wired PC-to-G1 deployment: +```text +real_robot.network_interface=enp130s0 +``` -1. Connect the PC to the G1 by Ethernet. -2. Run `ifconfig` on the PC. -3. Use the Ethernet interface connected to the robot, for example `enp130s0`. -4. Keep the Pico headset on a network that can reach the PC running Teleopit. +This interface is for Unitree DDS. If Pico discovery selects the wrong Wi-Fi or +Ethernet address, set `input.bridge_advertise_ip` separately. -For onboard deployment: +## 2. Check Standing Control First -1. Run Teleopit on the robot onboard computer. -2. Keep the Pico headset on a network that can reach the onboard computer. -3. Use `real_robot.network_interface=eth0` unless your robot network differs. -4. Set `input.bridge_advertise_ip=` if Pico discovery advertises the - wrong address. +Run the same standing controller used by sim2real before adding Pico: -### Onboard RealSense On Arm +```bash +python scripts/run/standalone_standing.py \ + --policy track.onnx \ + --network-interface enp130s0 \ + --dry-run +``` -The pico-bridge PC receiver supports Arm machines when the required Python -dependencies are available. On Arm onboard computers that need RealSense preview, -install `pyrealsense2` from conda-forge in the active Conda environment instead -of relying on the pip package: +The dry run checks state reception and policy timing without sending motor +commands. Then repeat without `--dry-run` in a safe hardware setup: ```bash -pip uninstall pyrealsense2 -conda install -c conda-forge pyrealsense2 +python scripts/run/standalone_standing.py \ + --policy track.onnx \ + --network-interface enp130s0 ``` -This only matters when using the optional RealSense preview path -(`input.video.enabled=true`). Pico tracking and robot control do not require -RealSense. +If this fails, stop here and use the +[Standalone Standing reference guide](standalone-standing). Pico cannot fix a +G1 bridge or policy problem. -## 3. Run The Controller +## 3. Start Pico Sim2Real Wired PC example: @@ -95,276 +91,164 @@ python scripts/run/run_sim2real.py \ real_robot.network_interface=eth0 ``` -## Optional HDF5 Recording - -Install the recording extra on the machine that owns Pico input and RealSense: - -```bash -pip install -e '.[recording]' -``` - -Run the recording config: - -```bash -python scripts/run/run_sim2real.py \ - --config-name sim2real_record \ - controller.policy_path=track.onnx \ - real_robot.network_interface=enp130s0 \ - recording.task="walk forward" -``` - -Terminal controls are `R` start episode, `S` save, `D` discard, and `Q` -shutdown. `STANDING`, `MOCAP`, `ARMS`, and paused mocap can be recorded; -saved episodes cannot be discarded afterward. Episodes are saved as `.h5` files -under `data/recordings/sim2real_hdf5/data/`, with compressed MP4 files under -`data/recordings/sim2real_hdf5/videos/d435i_rgb/`. The dataset-level -`schema.json` records robot/hand/neck types and feature definitions, while -`episodes.jsonl` stores file mappings and the editable task prompt for every -episode. HDF5 stores `frame_index`, `timestamp`, `observation.state(68)`, scalar -`observation.mode`, and the 36D motion-tracker reference `action` at 30 Hz. -When hand control is enabled, it also stores `action.hand(12)`. When OpenNeck -control is enabled, it stores the latest mechanically clamped -`[yaw_deg, pitch_deg]` target in degrees as `action.neck(2)`. Disabled devices -do not add their action fields. - -Recording starts only when a fresh RealSense frame is available. RealSense -timeouts or disconnects trigger background reconnection without stopping Pico -input or G1 control. If video is unavailable for one second during recording, -the active episode is discarded; press `R` again after video recovers. If the -entire Pico input worker exits, G1 control remains active and holds the latest -command so the Unitree remote can return the robot to `STANDING` or request -`DAMPING`. - -### Review Saved Episodes - -Install the lightweight review dependencies and launch the read-only web -reviewer against a recording root: - -```bash -pip install -e '.[review]' -python scripts/view/view_recording.py \ - --recording data/recordings/sim2real_hdf5 -``` +Starting the process does not immediately hand control to Pico. -Open the printed local URL in a browser. The reviewer synchronizes the D435i -MP4 with a MuJoCo view of the observed G1 pose and a translucent green -reference pose. Use the episode selector, frame scrubber, playback speed, and -joint selector to inspect tracking. The side panel includes the mode timeline, -per-body-group joint error, optional LinkerHand channels, and optional OpenNeck -yaw/pitch. +## 4. Hand Over Control Deliberately -The reviewer validates `schema.json`, every manifest path, HDF5 shapes and -finite values, and MP4 frame count/FPS before playback. It never modifies the -recording. `observation.state` does not contain measured root XYZ, so the -observed robot is anchored to the reference root position in the overlay; -joint tracking and root-orientation comparisons remain valid, but global root -translation cannot be evaluated from this recording format. - -## Operator Flow - -Keep the Unitree remote in hand. `L1+R1` is the emergency stop path into -`DAMPING`. +1. Press remote `Start` to enter `STANDING`. +2. Wait until the robot is stable and Pico tracking is valid. +3. Stand in a neutral pose with room to move. +4. Press remote `Y` to enter `MOCAP`. +5. Begin with small, slow movements. +6. Press remote `X` when you want to return to `STANDING`. | Control | Action | |---------|--------| | Unitree remote `Start` | Enter `STANDING` | -| Unitree remote `Y` | Enter `MOCAP` | -| Unitree remote `B` | Pause / resume live mocap | -| Pico/controller `A` | Pause / resume live mocap | -| Pico/controller `B` | Toggle `MOCAP` / `ARMS` | -| Unitree remote `X` | Return to `STANDING` | +| Unitree remote `Y` | Start whole-body VR control (`MOCAP`) | +| Unitree remote `B` | Pause or resume the current mocap session | +| Pico/controller `A` | Pause or resume the current mocap session | +| Pico/controller `B` | Switch between whole-body `MOCAP` and arm-only `ARMS` | +| Unitree remote `X` | End VR control and return to `STANDING` | | Unitree remote `L1+R1` | Emergency stop (`DAMPING`) | -Enter `MOCAP` only after Pico tracking is stable. Teleopit validates consecutive -mocap frames before switching; if validation fails, the robot stays in -`STANDING`. - -## Runtime Behavior +Teleopit checks several consecutive Pico frames before entering `MOCAP`. If the +check fails, the robot remains in `STANDING`. -Pico sim2real uses the shared realtime reference timeline: - -```text -Pico body frames -> retarget -> reference buffer -> observation -> policy -> G1 joints -``` +### Pause and Resume -When entering `STANDING`, Teleopit releases active Unitree modes, enters -debug/low-level control, locks the current joints briefly, resets policy state, -and ramps Kp without changing policy targets. +Pause holds the current reference pose; it does not return the robot to +`STANDING`. Resume rebuilds the live alignment from the current operator pose. +Resume while standing still and close to the held pose. Use remote `X` instead +when you want to end the VR session. -When entering `MOCAP`, Teleopit rearms the process-isolated reference worker, -resets its GMR state and realtime reference buffer, then waits for fresh -validated references before tracking the live mocap command. `STANDING` and -`DAMPING` keep the reference worker disarmed so cold startup frames cannot -warm-start retargeting before mocap entry. +### What Happens if Pico or Video Fails? -`ARMS` keeps the same live retargeting timeline running, but sends the motion -tracker a composed reference: body, waist, and legs stay at the standing pose -while both arms follow the live retargeted result. Entering or leaving `ARMS` -resets policy/reference alignment and uses the same Kp ramp safety path. +Pico input and camera preview are non-critical workers. If Pico input stops, the +G1 control loop keeps the last safe command and the Unitree remote remains +available. A RealSense timeout disables or reconnects video without stopping +body control. Use remote `X` or `L1+R1`; do not wait for an automatic mode +change. -## Pause / Resume - -Pico pause/resume is a mocap-session control event. Use either Unitree remote -`B` or Pico/controller `A`; Pico/controller `B` remains the `MOCAP` / `ARMS` -toggle. - -- `ACTIVE`: the pause button freezes the current reference pose. -- `PAUSED`: pressing it again clears policy/reference state, warms the realtime - buffer, re-centers yaw/XY alignment, and resumes from live mocap. - -:::warning -Resume while standing still and close to the paused pose. This reduces sudden -reference changes when live tracking resumes. -::: - -## Optional LinkerHand Control - -Pico sim2real can drive LinkerHand hands from Pico input: - -- `gripper`: hold the matching side grip as a deadman switch; the matching - trigger closes that hand. This mode supports `hands.driver=linkerhand_l6` and - `hands.driver=linkerhand_o6`; speed and open/close poses come from the matching - driver config. -- `vr_hand_pose`: retargets Pico hand pose through somehand and commands the - continuous L6 or O6 hand target. If a hand pose disappears, that side keeps its - last commanded pose. This mode uses Teleopit's Pico landmark adapter and the - public `somehand.api` from somehand 0.2.0. It always sets the selected hand - speed to the maximum. - -When `hands.enabled=true`, hand control remains active in all sim2real modes. -Shutdown and hand-runtime failure send the configured open pose. - -Install the local hand-control packages first if they were not installed with -the main Pico profile: - -```bash -git submodule update --init --recursive -pip install -e third_party/linkerhand-python-sdk -pip install -e third_party/somehand -bash scripts/setup/download_somehand_assets.sh -``` +## Optional: LinkerHand Control -Bring up the CAN interfaces before testing or running hand control: +Skip this section unless LinkerHand hardware is connected. Install the local +hand packages from [Installation](../getting-started/installation), then bring +up both CAN interfaces: ```bash sudo /usr/sbin/ip link set can0 up type can bitrate 1000000 sudo /usr/sbin/ip link set can1 up type can bitrate 1000000 ``` -Before enabling full sim2real, verify the hand connection with a standalone -open/close test. The test runs until Ctrl-C: +Test the hands before starting the robot runtime: ```bash python scripts/dev/test_linkerhand.py \ + --driver linkerhand_o6 \ --hand-type both \ --left-can can0 \ --right-can can1 ``` -For an O6 standalone open/close test, add the O6 driver: +Enable O6 hand-pose control by adding: -```bash -python scripts/dev/test_linkerhand.py \ - --driver linkerhand_o6 \ - --hand-type both \ - --left-can can0 \ - --right-can can1 +```text +hands.enabled=true +hands.driver=linkerhand_o6 +hands.mode=vr_hand_pose +hands.linkerhand_o6.left_can=can0 +hands.linkerhand_o6.right_can=can1 ``` -To test O6 with live Pico gripper input, add `--mode gripper`. To test O6 with -live Pico hand-pose retargeting, add `--mode vr_hand_pose`. +Use `hands.mode=gripper` for trigger-based open/close control. +`linkerhand_l6` is also supported; use the matching +`hands.linkerhand_l6.*` CAN keys. Hand control remains active in all robot +modes, and runtime failure opens the hands. -Then enable L6 gripper control in Pico sim2real: +## Optional: OpenNeck Active Vision + +Skip this section unless OpenNeck is installed and calibrated: ```bash -hands.enabled=true -hands.driver=linkerhand_l6 -hands.mode=gripper -hands.linkerhand_l6.left_can=can0 -hands.linkerhand_l6.right_can=can1 +pip install -e '.[openneck]' +openneck calibrate ``` -For O6 gripper control, use: +Enable it in the main command: -```bash -hands.enabled=true -hands.driver=linkerhand_o6 -hands.mode=gripper -hands.linkerhand_o6.left_can=can0 -hands.linkerhand_o6.right_can=can1 +```text +neck.enabled=true +neck.port=/dev/ttyACM0 ``` -For continuous L6 VR hand-pose control, use: +OpenNeck follows the Pico HMD relative to the operator's upper body. It uses the +same Pico receiver as body control and does not start another PicoBridge. -```bash -hands.enabled=true -hands.driver=linkerhand_l6 -hands.mode=vr_hand_pose -hands.linkerhand_l6.left_can=can0 -hands.linkerhand_l6.right_can=can1 -``` +## Optional: RealSense Preview in the Headset -For continuous O6 VR hand-pose control, switch the driver and CAN keys: +Install `pyrealsense2`, then add: -```bash -hands.enabled=true -hands.driver=linkerhand_o6 -hands.mode=vr_hand_pose -hands.linkerhand_o6.left_can=can0 -hands.linkerhand_o6.right_can=can1 +```text +input.video.enabled=true +input.video.device= ``` -## Optional RealSense Preview +RealSense reconnects in the background after a timeout. Camera failure does not +stop Pico tracking or G1 control. + +## Optional: Record and Review Episodes -Stream the G1 RealSense color camera back to the Pico headset: +Recording requires the `recording` profile and a fresh RealSense RGB frame: ```bash python scripts/run/run_sim2real.py \ - --config-name pico4_sim2real \ + --config-name sim2real_record \ controller.policy_path=track.onnx \ real_robot.network_interface=enp130s0 \ - input.video.enabled=true \ - input.video.device= + recording.task="walk forward" ``` -RealSense frame timeouts and disconnects reconnect in the background and never -stop Pico tracking or G1 control. +| Terminal key | Action | +|--------------|--------| +| `R` | Start an episode | +| `S` | Save the active episode | +| `D` | Discard the active episode | +| `Q` | Shut down | -## Common Parameters +If fresh video is missing for one second, the active episode is discarded while +robot control continues. Start a new episode manually after video recovers. -```bash -# Real G1 DDS interface -real_robot.network_interface=enp130s0 +Review saved data with: -# Pico timeout -input.pico4_timeout=30 +```bash +pip install -e '.[review]' +python scripts/view/view_recording.py \ + --recording data/recordings/sim2real_hdf5 +``` -# Override advertised Pico discovery IP -input.bridge_advertise_ip=192.168.1.20 +The reviewer synchronizes camera video, observed/reference G1 poses and optional +hand/neck signals. The recording layout and field definitions are documented +in [Dataset Reference](../reference/dataset). -# Consecutive valid mocap frames required before MOCAP -mocap_switch.check_frames=10 +## Common Problems -# Change Pico pause button -input.pause_button=right_axis_click +| Symptom | What to do | +|---------|------------| +| No `LowState` arrives | Check the Ethernet cable and `real_robot.network_interface` | +| `g1_bridge_sdk` cannot import | Re-run `scripts/setup/setup_g1_bridge.sh` in the active environment | +| `Start` cannot enter standing control | Stop other Unitree modes and programs, then try again | +| `Y` does not enter `MOCAP` | Keep Pico tracking visible and stable; inspect mocap validation logs | +| Pausing does not return to standing | This is expected; use remote `X` | +| Pico cannot discover Teleopit | Set `input.bridge_advertise_ip` to an address reachable from the headset | +| LinkerHand does not move | Check `hands.enabled`, driver/mode, CAN state and the standalone hand test | +| RealSense is unavailable on Arm | Install `pyrealsense2` from conda-forge | -# Enable LinkerHand gripper control -hands.enabled=true -hands.driver=linkerhand_l6 -hands.mode=gripper +## Other G1 Workflows -# Enable headset video preview -input.video.enabled=true -``` +The main tutorial path is Pico VR. These focused guides remain available for +less common bring-up and deployment work: -## Troubleshooting - -| Symptom | Likely Cause | Fix | -|---------|--------------|-----| -| No LowState received | Wrong interface or G1 network not connected | Check Ethernet wiring and `real_robot.network_interface` | -| `TimeoutError: No Pico4 body data` | Headset is not connected or tracking is inactive | Check headset app, network, and `input.pico4_timeout` | -| Cannot enter debug mode | Unitree mode release failed | Stop other robot modes and press `Start` again | -| Robot enters `STANDING` but not `MOCAP` | Mocap validation failed | Keep tracking active and stable; check `mocap_switch.check_frames` logs | -| Pico pause does not return to `STANDING` | Expected behavior | Pico pause freezes mocap; press remote `X` for `STANDING` | -| LinkerHand does not move | `hands.enabled=false`, gripper deadman released, SDK/assets not installed, or CAN channel wrong | Enable `hands.enabled`, set `hands.mode`, run `scripts/dev/test_linkerhand.py`, and check the selected driver's `left_can` / `right_can` | -| Video preview is unavailable | RealSense or video source failed | Check camera permissions, `input.video.source`, and logs | +- [Standalone Standing Test](standalone-standing) +- [BVH Playback on Unitree G1](bvh-sim2real) +- [Host Policy Deployment on Unitree G1](high-level-policy-sim2real) diff --git a/docs/docs/tutorials/pico-sim2sim.md b/docs/docs/tutorials/pico-sim2sim.md index a545726c..4c5d093e 100644 --- a/docs/docs/tutorials/pico-sim2sim.md +++ b/docs/docs/tutorials/pico-sim2sim.md @@ -2,61 +2,60 @@ sidebar_position: 2 --- -# Pico 4 VR Teleoperation in Simulation +# VR Teleoperation in Simulation -Use this tutorial to verify Pico 4 / Pico 4 Ultra full-body tracking in MuJoCo -before running on real Unitree G1 hardware. +Use Pico tracking to control a simulated G1 before connecting a physical robot. +Do not skip this step: it lets you fix headset, network and body-tracking +problems without putting hardware at risk. -```text -Pico headset -> pico-bridge receiver -> retarget -> RL policy -> MuJoCo G1 -``` +## Before You Start -After this works, continue with [Pico Sim2Real](pico-sim2real). +You need: -## Supported Devices +- a Pico 4 or Pico 4 Ultra with full-body tracking, +- the headset and the computer running Teleopit on the same network, +- the `pico4` install profile and `robots gmr ckpt bvh` assets, and +- a working result from + [Run a Motion Controller in Simulation](offline-sim2sim). -- Pico 4 -- Pico 4 Ultra +## 1. Prepare the Headset -## 1. Set Up The Headset +1. Download the headset APK from + [pico-bridge Releases](https://github.com/BotRunner64/pico-bridge/releases). +2. Install it: -1. Download the headset APK from [pico-bridge Releases](https://github.com/BotRunner64/pico-bridge/releases). -2. Install it with adb: ```bash adb install pico-bridge.apk ``` -3. Launch the pico-bridge headset client. -4. Enable full-body tracking. -5. Keep the headset and Teleopit host on the same network. -## 2. Install The Pico Host Extra +3. Open the pico-bridge app in the headset. +4. Turn on full-body tracking. -On the machine that will run Teleopit: +Teleopit uses pico-bridge 0.2.1. The receiver runs inside Teleopit on your +computer; there is no second relay program to start. -```bash -pip install -e '.[pico4]' -``` +## 2. Check That the Computer Receives Pico Data -Verify the receiver package: +This diagnostic prints body-frame and connection information without starting +the robot controller: ```bash -python -c "from pico_bridge import PicoBridge; print('OK')" +python scripts/dev/test_pico_bridge.py --no-video ``` -Teleopit starts `pico_bridge.PicoBridge` in-process through -`Pico4InputProvider`. The same Pico input path is used later for wired and -onboard sim2real deployment. +Move slightly and confirm that new valid frames continue to arrive. Press +`Ctrl+C` to stop the diagnostic. -Teleopit targets pico-bridge 0.2.1 and its `pico_native` tracking semantics. - -## 3. Download Assets +If discovery chooses the wrong network address, pass the address that the +headset can reach: ```bash -pip install modelscope -python scripts/setup/download_assets.py --only robots gmr ckpt bvh +python scripts/dev/test_pico_bridge.py \ + --no-video \ + --bridge-advertise-ip=192.168.1.20 ``` -## 4. Run Pico Sim2Sim +## 3. Start the Simulation ```bash python scripts/run/run_sim.py \ @@ -64,48 +63,62 @@ python scripts/run/run_sim.py \ controller.policy_path=track.onnx ``` -The simulation starts in `STANDING`. Wait until Pico tracking is active, then -enter `MOCAP`. +The robot intentionally starts in `STANDING`; live body tracking does not take +control until you ask for it. + +## 4. Complete the First VR Session + +1. Stand in a comfortable neutral pose and wait for stable tracking. +2. Press `Y` on the keyboard to enter `MOCAP`. +3. Move slowly at first and confirm that the simulated G1 follows. +4. Press `A` to pause, then press `A` again to resume. +5. Press `X` to return to `STANDING`. -| Keyboard | Action | -|----------|--------| -| `Y` | Enter `MOCAP` | -| `A` | Pause / resume live mocap | -| `B` | Toggle `MOCAP` / `ARMS` | -| `X` | Return to `STANDING` | +| Key | Action | +|-----|--------| +| `Y` | Start whole-body control (`MOCAP`) | +| `A` | Pause or resume the current mocap session | +| `B` | Switch between `MOCAP` and arm-only control (`ARMS`) | +| `X` | Stop VR control and return to `STANDING` | | `Q` | Quit | -`pico4_sim.yaml` defaults to `viewers=all`, which opens mocap, retarget, and -sim2sim viewers. Use `viewers=sim2sim` or `viewers=none` when you want fewer -windows. +The modes are simple: -Each `STANDING -> MOCAP` entry resets GMR, seeds its floating root from the -current live pelvis target, and rebuilds the realtime reference path. The -operator can therefore change heading while in `STANDING` without reusing the -previous mocap session's IK warm-start. Pause/resume and `MOCAP <-> ARMS` -switches retain the current IK warm-start. +- `STANDING`: the robot waits in its standing controller. +- `MOCAP`: the whole body follows the operator. +- `ARMS`: the body, waist and legs stay in the standing pose while both arms + continue to follow the operator. -## Pause / Resume +Each new `STANDING -> MOCAP` session recalibrates the live root pose. You may +turn to a new heading while standing, then enter `MOCAP` again. -Pico pause/resume freezes the mocap session; it is not a switch back to -`STANDING`. +:::tip Pausing is not the same as stopping VR control +`A` freezes and resumes the current mocap pose. Use `X` when you want to end the +session and return to `STANDING`. +::: -- Press keyboard `A` or the Pico/controller pause button to freeze the current - reference pose. -- Press it again to rebuild the realtime reference path, re-center yaw and - ground-plane position, and continue from the current live tracking stream. +## Choose the Viewer Layout -The default Pico pause button is `A`. Supported overrides include `B`, `X`, `Y`, -`left_axis_click`, `right_axis_click`, `left_menu_button`, and -`right_menu_button`. +Pico simulation opens the mocap, retarget and physics views by default. Use a +smaller layout when you no longer need all three: -The default Pico arms-mode button is `B`. `ARMS` keeps body, waist, and legs at -the standing pose while both arms follow the live retargeted result. +```bash +# Physics result only +python scripts/run/run_sim.py \ + --config-name pico4_sim \ + controller.policy_path=track.onnx \ + viewers=sim2sim -## Optional Headset Video Preview +# Headless +python scripts/run/run_sim.py \ + --config-name pico4_sim \ + controller.policy_path=track.onnx \ + viewers=none +``` + +## Optional Headset Video -pico-bridge 0.2.1 can show a host-side camera stream in the headset. In -simulation, Teleopit can stream the MuJoCo `d435i_rgb` camera: +To send the simulated `d435i_rgb` camera view back to the headset: ```bash python scripts/run/run_sim.py \ @@ -114,42 +127,37 @@ python scripts/run/run_sim.py \ input.video.enabled=true ``` -Use `input.video.source=test-pattern` for a receiver-side video sanity check. If -video startup fails, Teleopit logs the error, disables video, and keeps tracking -and control running. +Use `input.video.source=test-pattern` to check only the video connection. +Video failure disables the preview but does not stop tracking or control. -## Common Parameters +## Network Overrides -```bash -# Pico wait timeout for the first body frame -input.pico4_timeout=30 +Most setups only need automatic discovery. Use these overrides when the +diagnostic shows a network problem: -# Override the IP advertised to the headset during discovery +```bash +# Advertise a specific host address to the headset input.bridge_advertise_ip=192.168.1.20 # Disable discovery and bind explicitly -input.bridge_discovery=false input.bridge_host=0.0.0.0 input.bridge_port=63901 +input.bridge_discovery=false +input.bridge_host=0.0.0.0 +input.bridge_port=63901 -# Change the Pico pause button -input.pause_button=right_axis_click - -# Disable keyboard mode control -keyboard.enabled=false - -# Change policy frequency -policy_hz=30 - -# Enable headset video preview -input.video.enabled=true +# Wait longer for the first body frame +input.pico4_timeout=30 ``` -## Troubleshooting +## Common Problems + +| Symptom | What to do | +|---------|------------| +| `ImportError: pico_bridge` | Install the `pico4` profile again | +| Startup reports an old pico-bridge | Reinstall the profile so version 0.2.1 is used | +| No body frames arrive | Open the headset app, enable full-body tracking and check that UDP port 63901 is reachable | +| Discovery advertises the wrong address | Set `input.bridge_advertise_ip` to the computer address visible from the headset | +| G1 stays still in the viewer | Wait for stable tracking, then press `Y` | +| G1 follows only with its arms | Press `B` to leave `ARMS` and return to `MOCAP` | -| Symptom | Likely Cause | Fix | -|---------|--------------|-----| -| `ImportError: pico_bridge` | Pico extra not installed | Run `pip install -e '.[pico4]'` | -| Startup says pico-bridge is too old | Installed receiver does not support the required API or tracking semantics | Reinstall the Pico extra so pico-bridge 0.2.1 is used | -| `TimeoutError: No Pico4 body data` | Headset is not connected or body tracking is inactive | Check the headset app, network, and `input.pico4_timeout` | -| Discovery cannot find the host | Wrong advertised IP or blocked UDP | Set `input.bridge_advertise_ip=` and confirm UDP port `63901` is reachable | -| Sim robot does not follow | Loop is still in `STANDING` | Press `Y` after tracking is ready | -| Pico video is black or disabled | Video source failed or camera access is unavailable | Check `input.video.source` and logs | +Once this workflow is reliable, continue with +[VR Teleoperation on Unitree G1](pico-sim2real). diff --git a/docs/docs/tutorials/training.md b/docs/docs/tutorials/training.md index b59d1e8d..1460f873 100644 --- a/docs/docs/tutorials/training.md +++ b/docs/docs/tutorials/training.md @@ -1,40 +1,52 @@ --- -sidebar_position: 5 +sidebar_position: 4 --- -# Training +# Train a Motion Controller -Train a whole-body tracking policy and export it as ONNX for inference. +This guide starts with downloaded motion data and ends with an ONNX controller +that Teleopit can run in simulation or on a G1. -:::info -For data preparation, see [Dataset Reference](../reference/dataset). For common training issues, see [Training Troubleshooting](../reference/training-troubleshooting). -::: +The normal training path assumes an NVIDIA GPU. Motion data is loaded into +memory at startup, so larger combined datasets also need enough system and GPU +memory. -## Setup +## Before You Start -```bash -conda create -n teleopit python=3.10 -conda activate teleopit -pip install -e '.[train]' -``` +Follow [Installation](../getting-started/installation) with: + +- the `train` profile, and +- the `robots data` asset bundle. + +Verify the training package: -Verify: ```bash python -c "import train_mimic.tasks; print('training OK')" ``` -Download the distributed minimal datasets and generate the combined precomputed -training dataset: +## 1. Prepare the Downloaded Dataset + +Downloaded datasets are compact distribution files. Training uses a second +directory with joint velocities and body kinematics precomputed: ```bash -python scripts/setup/download_assets.py --only robots data python train_mimic/scripts/data/precompute_dataset.py \ - data/datasets --outdir data/datasets_precomputed --jobs 8 + data/datasets \ + --outdir data/datasets_precomputed \ + --jobs 8 ``` -## Training +Use `data/datasets_precomputed` for every training, playback and benchmark +command below. Pointing training at the original `data/datasets` directory is +an error, not a supported shortcut. + +For custom BVH, PKL, NPZ or Pico-recorded data, see +[Dataset Reference](../reference/dataset). -### Smoke Test +## 2. Run a Short Smoke Test + +Before starting a long job, verify that the dataset, simulator and logger work +together: ```bash python train_mimic/scripts/train.py \ @@ -43,7 +55,10 @@ python train_mimic/scripts/train.py \ --motion_file data/datasets_precomputed ``` -### Full Training +The test is successful when environments step, losses are reported and a run +directory appears under `logs/rsl_rl/g1_general_tracking/`. + +## 3. Start a Full Run ```bash python train_mimic/scripts/train.py \ @@ -52,43 +67,42 @@ python train_mimic/scripts/train.py \ --motion_file data/datasets_precomputed ``` -### Multi-GPU +Reduce `--num_envs` if GPU memory is insufficient. The default logger is +TensorBoard; choose `--logger wandb` or `--logger swanlab` when required. + +`--max_iterations` means additional iterations. For example, resuming +`model_12000.pt` with `--max_iterations 18000` continues to iteration 30000. + +## 4. Watch the Checkpoint in Simulation ```bash -python train_mimic/scripts/train.py \ - --gpu_ids 0 1 2 3 \ - --num_envs 1024 \ - --max_iterations 30000 \ +python train_mimic/scripts/play.py \ + --checkpoint logs/rsl_rl/g1_general_tracking//model_30000.pt \ --motion_file data/datasets_precomputed ``` -### Multi-Node Multi-GPU +Playback starts clips from their beginning and removes training noise. Use it +to catch an obviously unstable policy before exporting. -Use `torchrun` directly when training across multiple machines: +## 5. Run the Benchmark ```bash -torchrun \ - --nnodes=$PET_NNODES \ - --nproc_per_node=$PET_NPROC_PER_NODE \ - --node_rank=$PET_NODE_RANK \ - --master_addr=$PET_MASTER_ADDR \ - --master_port=$PET_MASTER_PORT \ - train_mimic/scripts/train.py \ - --num_envs 1024 \ - --max_iterations 1000 \ - --motion_file data/datasets_precomputed +python train_mimic/scripts/benchmark.py \ + --checkpoint logs/rsl_rl/g1_general_tracking//model_30000.pt \ + --motion_file data/datasets_precomputed \ + --num_envs 32 ``` -**Notes:** -- `--num_envs` is per-GPU in multi-GPU mode -- `--num_envs` is also per-process in multi-node mode, so total environments scale with `world_size` -- Default logger is TensorBoard. Use `--logger wandb` or `--logger swanlab` to select W&B or SwanLab; the project name defaults to `experiment_name` -- `--motion_file` accepts a precomputed training dataset root directory or a single precomputed `.h5` shard; shard discovery is recursive -- If you only have the minimal distributed shards, first run `python train_mimic/scripts/data/precompute_dataset.py --outdir ` and pass the precomputed output to training. -- Training loads all discovered precomputed motion windows into memory at startup. -- `--max_iterations` means additional iterations; resuming from `model_12000.pt` with `--max_iterations 18000` trains to `model_30000.pt` +The benchmark evaluates one deterministic 10-second rollout for every eligible +clip. It reports: + +- mean per-joint position error (`MPJPE`), +- root position, rotation and velocity error, and +- rollout success rate. -## Export ONNX +Results are written as a text summary, JSON, per-clip CSV and per-rollout CSV. + +## 6. Export ONNX ```bash python train_mimic/scripts/save_onnx.py \ @@ -97,40 +111,59 @@ python train_mimic/scripts/save_onnx.py \ --history_length 10 ``` -The exported model is a dual-input ONNX (`obs` + `obs_history`). The inference side expects a 167D dual-input ONNX policy matching the current `velcmd_history` observation. +The result must be a dual-input TemporalCNN with `obs` and `obs_history`. +Teleopit validates the 167D observation signature at startup and rejects an +incompatible export. + +Test the export in the normal runtime: -## Evaluation +```bash +python scripts/run/run_sim.py \ + controller.policy_path=track.onnx \ + input.bvh_file=data/sample_bvh/aiming1_subject1.bvh +``` -### Playback +## Scale to Multiple GPUs + +For one machine: ```bash -python train_mimic/scripts/play.py \ - --checkpoint logs/rsl_rl/g1_general_tracking//model_30000.pt \ +python train_mimic/scripts/train.py \ + --gpu_ids 0 1 2 3 \ + --num_envs 1024 \ + --max_iterations 30000 \ --motion_file data/datasets_precomputed ``` -### Benchmark +`--num_envs` is per GPU. + +For multiple machines, launch the same script with `torchrun`: ```bash -python train_mimic/scripts/benchmark.py \ - --checkpoint logs/rsl_rl/g1_general_tracking//model_30000.pt \ - --motion_file data/datasets_precomputed \ - --num_envs 32 +torchrun \ + --nnodes=$PET_NNODES \ + --nproc_per_node=$PET_NPROC_PER_NODE \ + --node_rank=$PET_NODE_RANK \ + --master_addr=$PET_MASTER_ADDR \ + --master_port=$PET_MASTER_PORT \ + train_mimic/scripts/train.py \ + --num_envs 1024 \ + --max_iterations 1000 \ + --motion_file data/datasets_precomputed ``` -The benchmark uses an OmniXtreme-style protocol: 10-second clips, one deterministic rollout per eligible clip, and `MPJPE(m)`, `root_pos_error(m)`, `root_rot_error(rad)`, `root_vel_error(m/s)`, and `success_rate(%)` outputs. Root errors use the same anchor position, rotation, and linear velocity definitions as the tracking command metrics. It uses play-mode observations without training noise and pins exact clip ids/start times without clip-end resampling. `--motion_file` must point to a precomputed training dataset; all clips long enough for the configured clip length are evaluated. +Here `--num_envs` is per process, so the total scales with the world size. -## Training Architecture +## Common Problems -```text -train_mimic/scripts - -> train_mimic/app.py - -> single task registry / env builder / runner cfg - -> mjlab + rsl_rl -``` +| Symptom | What to check | +|---------|---------------| +| Loader says the dataset is minimal | Run `precompute_dataset.py` and use its output directory | +| Out of GPU memory | Lower `--num_envs` | +| Out of system memory during startup | Train on fewer precomputed shards or add RAM | +| Exported ONNX fails the 167D check | Export with `save_onnx.py` and `--history_length 10` from the current task | +| Benchmark skips clips | The skipped clips are shorter than the configured benchmark duration | -Key files: -- `train_mimic/app.py` - Shared entry point for train/play/benchmark -- `train_mimic/tasks/tracking/config/env.py` - General-Tracking-G1 env builder -- `train_mimic/tasks/tracking/config/rl.py` - TemporalCNN PPO config -- `train_mimic/tasks/tracking/mdp/commands.py` - Supports `uniform`, `start`, and `rewind` sampling modes. Training defaults to `rewind`; playback uses `start`; benchmark pins exact clip ids and start times. +For task internals and model dimensions, see +[Architecture](../reference/architecture). For failure-specific guidance, see +[Training Troubleshooting](../reference/training-troubleshooting). diff --git a/docs/docusaurus.config.ts b/docs/docusaurus.config.ts index 8119bf29..3968c8be 100644 --- a/docs/docusaurus.config.ts +++ b/docs/docusaurus.config.ts @@ -4,7 +4,7 @@ import type * as Preset from '@docusaurus/preset-classic'; const config: Config = { title: 'Teleopit', - tagline: 'Lightweight, extensible whole-body teleoperation framework for humanoid robots', + tagline: 'Full-embodiment teleoperation for humanoid robots', favicon: 'img/favicon.ico', url: 'https://BotRunner64.github.io', diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current.json b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current.json new file mode 100644 index 00000000..5252e760 --- /dev/null +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current.json @@ -0,0 +1,22 @@ +{ + "version.label": { + "message": "当前版本", + "description": "The label for version current" + }, + "sidebar.docsSidebar.category.Getting Started": { + "message": "入门", + "description": "The label for category 'Getting Started' in sidebar 'docsSidebar'" + }, + "sidebar.docsSidebar.category.Tutorials": { + "message": "教程", + "description": "The label for category 'Tutorials' in sidebar 'docsSidebar'" + }, + "sidebar.docsSidebar.category.Configuration": { + "message": "配置", + "description": "The label for category 'Configuration' in sidebar 'docsSidebar'" + }, + "sidebar.docsSidebar.category.Reference": { + "message": "参考资料", + "description": "The label for category 'Reference' in sidebar 'docsSidebar'" + } +} diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/download-assets.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/download-assets.md deleted file mode 100644 index 9c6463ac..00000000 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/download-assets.md +++ /dev/null @@ -1,49 +0,0 @@ ---- -sidebar_position: 2 ---- - -# 下载资源 - -机器人模型、数据集和检查点托管在 ModelScope 上，使用前需要先下载。 - -## 一键下载 - -下载全部资源（模型、数据、GMR 重定向资源）： - -```bash -pip install modelscope -python scripts/setup/download_assets.py -``` - -## 按需下载 - -只下载推理所需的资源： - -```bash -python scripts/setup/download_assets.py --only robots gmr ckpt bvh -``` - -## 资源清单 - -checkpoint、数据集和资源包更新后，下载文件大小会变化。下表中的仓库路径才是稳定约定。 - -| 本地路径 | 用途 | -|----------|------| -| `track.onnx` | ONNX 推理模型 | -| `track.pt` | 用于恢复训练的 PyTorch checkpoint | -| `data/datasets//shard_*.h5` | 最小运动数据集；训练前需先预计算 | -| `data/sample_bvh/*.bvh` | 示例动捕文件 | -| `assets/robots/unitree_g1/` | 训练、sim2sim、重定向和 FK 校验共用的 G1 canonical XML 与 mesh | -| `teleopit/retargeting/gmr/assets/` | GMR 重定向资源、IK 配置和非 canonical 机器人描述 | - -## 资源分组 - -| 分组 | ModelScope 仓库 | 包含内容 | -|------|-----------------|----------| -| `ckpt` | `BingqianWu/Teleopit-models` | `track.onnx`、`track.pt` | -| `robots` | `BingqianWu/Teleopit-models` | Canonical 机器人 XML/mesh | -| `gmr` | `BingqianWu/Teleopit-models` | GMR 重定向资源 | -| `bvh` | `BingqianWu/Teleopit-models` | 示例 BVH 动捕文件 | -| `data` | `BingqianWu/Teleopit-datasets` | `lafan1`、`pico_record`、`seed`、`twist2` 的最小 shard | - -资源管理的更多细节（上传、版本控制等）请参阅 [资源管理](../reference/assets)。 diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/installation.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/installation.md index 341fb404..901aee3d 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/installation.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/installation.md @@ -2,67 +2,111 @@ sidebar_position: 1 --- -# 安装 +# 安装 Teleopit -Teleopit 提供多种安装配置，你可以根据实际使用场景选择对应的安装方式。 +只安装你真正需要的部分。下面的命令都在仓库根目录执行，并要求 Python 3.10 +或更高版本。 -## 前置条件 - -- Python 3.10+ -- [Conda](https://docs.conda.io/)（推荐） +## 1. 获取代码 ```bash -conda create -n teleopit python=3.10 -conda activate teleopit +git clone https://github.com/BotRunner64/Teleopit.git +cd Teleopit ``` -## 安装配置 +只有连接真实 G1 或使用 LinkerHand 时才需要 Git 子模块，相关命令放在本页后面。 + +## 2. 创建 Python 环境 + +下面三种方式任选一种，不要全部执行。 -### 仅推理（sim2sim） +### uv ```bash -pip install -e . +uv venv --python 3.10 +source .venv/bin/activate ``` -该配置已足够进行离线 BVH 回放和 MuJoCo 仿真。 +本页后续出现 `pip install` 时，也可以替换为 `uv pip install`。 -### 训练 +### pip 和 venv ```bash -pip install -e '.[train]' +python3.10 -m venv .venv +source .venv/bin/activate +python -m pip install --upgrade pip ``` -额外安装 `rsl-rl-lib`、`mjlab`、`wandb`、`swanlab` 等训练相关依赖。 +### Conda -### Sim2Real（硬件部署） +```bash +conda create -n teleopit python=3.10 +conda activate teleopit +``` + +Conda 负责创建环境；进入环境后，仍使用 `pip install` 安装 Teleopit。 + +## 3. 根据目标安装依赖 + +每个 extra 都包含 Teleopit 基础包。先安装与你当前目标对应的一项；以后可以在同一个 +环境中继续安装其他 extra。 + +| 目标 | 安装命令 | 增加的内容 | +|------|----------|------------| +| 在 MuJoCo 中运行运控 | `pip install -e .` | 基础推理、GMR、MuJoCo 和 ONNX Runtime | +| 在仿真或 G1 上使用 Pico | `pip install -e '.[pico4]'` | Pico 接收与真机运行环境 | +| 不使用 Pico，在真实 G1 上回放 BVH | `pip install -e '.[sim2real]'` | G1 运行环境和 OpenCV | +| 训练运控策略 | `pip install -e '.[train]'` | mjlab、RSL-RL 和实验记录工具 | +| 录制 Pico 真机数据 | `pip install -e '.[recording]'` | Pico 运行环境和 MP4 写入依赖 | +| 查看已录制的数据 | `pip install -e '.[review]'` | OpenCV 和 MuJoCo/Viser 查看工具 | +| 使用 OpenNeck | `pip install -e '.[openneck]'` | Pico 运行环境和 OpenNeck 驱动 | +| 运行测试 | `pip install -e '.[dev]'` | pytest 和覆盖率工具 | + +## 4. 下载对应资源 + +Python 包里不包含机器人 mesh、运控模型和动作数据。先安装默认的 ModelScope +下载工具： ```bash -pip install -e '.[sim2real]' +pip install modelscope ``` -额外安装 `opencv-python`。此外还需要初始化子模块并编译/安装 C++ `g1_bridge_sdk` 桥接库： +再根据目标下载： + +| 目标 | 命令 | +|------|------| +| 仿真、Pico VR 或 G1 推理 | `python scripts/setup/download_assets.py --only robots gmr ckpt bvh` | +| 使用已发布数据集训练 | `python scripts/setup/download_assets.py --only robots data` | +| 下载全部资源 | `python scripts/setup/download_assets.py` | + +需要从 HuggingFace 下载时： ```bash -git submodule update --init --recursive -bash scripts/setup/setup_g1_bridge.sh +python scripts/setup/download_assets.py \ + --source huggingface \ + --only robots gmr ckpt bvh ``` -详见 [G1 Bridge SDK](../reference/g1-bridge-sdk)。 +推理资源包会把 `track.onnx`、标准 G1 模型、GMR 文件和示例 BVH 放到代码默认查找 +的位置。完整文件清单和资源分组见[资源参考](../reference/assets)。 -### Pico 4 VR +## 5. 连接真实 G1 前的额外安装 + +在实际运行 Teleopit 的电脑上编译 C++ DDS bridge： ```bash -pip install -e '.[pico4]' +git submodule update --init --recursive +bash scripts/setup/setup_g1_bridge.sh ``` -Teleopit 使用进程内的 `pico_bridge.PicoBridge` receiver 接收 Pico 追踪数据。 -Teleopit 面向 pico-bridge 0.2.1 及其 `pico_native` tracking 语义。 -receiver 可以运行在工作站 PC，也可以运行在机器人 onboard 计算机。 -完整设置流程详见 [Pico Sim2Sim](../tutorials/pico-sim2sim) 和 -[Pico Sim2Real](../tutorials/pico-sim2real)。 +无论使用 Pico 还是真机 BVH 回放，都需要这个 bridge。如果编译失败或收不到机器人 +状态，请查看 [G1 Bridge SDK](../reference/g1-bridge-sdk)。 + +## 6. 可选硬件 -Pico sim2real 可选的 LinkerHand 控制使用本地 third-party 包。初始化 -submodule 后，直接安装这些包： +### LinkerHand L6 或 O6 + +只有设置 `hands.enabled=true` 时才需要安装： ```bash git submodule update --init --recursive @@ -71,51 +115,50 @@ pip install -e third_party/somehand bash scripts/setup/download_somehand_assets.sh ``` -只有在 `hands.enabled=true` 时才需要安装这些包。 +### OpenNeck -Pico sim2real 可选的 OpenNeck 主动视觉控制使用最新的 OpenNeck 角度控制包： +`openneck` extra 已经包含 Pico 依赖。启用前先完成标定： ```bash pip install -e '.[openneck]' +openneck calibrate ``` -该 extra 包含 Pico 栈，只有在 `neck.enabled=true` 时才需要安装。OpenNeck 0.2.0 -标定文件使用 `*_center_step`、`*_min_step`、`*_max_step` 和 -`*_step_sign`；不支持以前的归一化配置格式。运行 `openneck calibrate` -创建当前格式的标定文件。 +Teleopit 使用 OpenNeck 的角度接口，不支持旧版归一化标定字段。 + +### RealSense 录制或视频预览 -### Sim2Real 录制 +启用 RealSense 时还需要单独安装 `pyrealsense2`。Arm 设备建议使用 +conda-forge： ```bash -pip install -e '.[recording]' +conda install -c conda-forge pyrealsense2 ``` -该配置包含 Pico sim2real 栈，以及 `sim2real_record.yaml` 使用的视频依赖。 -RealSense Python 绑定与平台相关；使用 `input.video.source=realsense` 时， -需要在当前环境中手动安装 `pyrealsense2`。在 Arm 机器上，请使用 -conda-forge，而不是 pip 包： +Pico 身体追踪本身不依赖 RealSense。 + +## 7. 检查安装结果 + +先检查 Teleopit 基础包： ```bash -conda install -c conda-forge pyrealsense2 +python -c "import teleopit; print('teleopit OK')" ``` -### 录制 Review +如果安装了 Pico 或训练依赖，再运行对应检查： ```bash -pip install -e '.[review]' +python -c "from pico_bridge import PicoBridge; print('Pico OK')" +python -c "import train_mimic.tasks; print('training OK')" ``` -该 extra 会安装只读 sim2real 录制同步 reviewer 使用的 OpenCV 和 MuJoCo/Viser 依赖， -不会安装 Pico、RealSense 或 G1 控制依赖。 - -## 验证安装 +如果安装的是推理环境，并已经下载 `robots gmr ckpt bvh` 资源，最后运行一次示例仿真： ```bash -python -c "import teleopit; print('teleopit OK')" -python -c "import train_mimic.tasks; print('training OK')" # 仅在安装了训练配置时适用 +python scripts/run/run_sim.py \ + controller.policy_path=track.onnx \ + input.bvh_file=data/sample_bvh/aiming1_subject1.bvh ``` -## 下一步 - -- [下载资源](download-assets) - 下载模型和数据 -- [快速上手](quick-start) - 运行你的第一个仿真 +MuJoCo 窗口能够打开，仿真 G1 能跟随示例动作，就说明安装完成。关闭窗口即可停止， +然后进入四条任务教程之一。 diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/quick-start.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/quick-start.md deleted file mode 100644 index f53ea91b..00000000 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/quick-start.md +++ /dev/null @@ -1,64 +0,0 @@ ---- -sidebar_position: 3 ---- - -# 快速上手 - -本指南带你在 5 分钟内完成第一次 sim2sim 回放。 - -## 前置条件 - -1. [安装 Teleopit](installation)（推理配置） -2. [下载资源](download-assets)（`--only robots gmr ckpt bvh`） - -## 运行离线 Sim2Sim - -```bash -python scripts/run/run_sim.py \ - controller.policy_path=track.onnx \ - input.bvh_file=data/sample_bvh/aiming1_subject1.bvh -``` - -运行后你应该能看到 MuJoCo 查看器窗口，展示机器人跟踪 BVH 动作的过程。 - -## 键盘控制 - -在启用 `playback.keyboard.enabled=true` 时可使用以下快捷键： - -| 按键 | 功能 | -|------|------| -| `Space` / `P` | 暂停 / 继续 | -| `R` | 从头重播 | -| `Q` | 停止 | - -```bash -python scripts/run/run_sim.py \ - controller.policy_path=track.onnx \ - input.bvh_file=data/sample_bvh/aiming1_subject1.bvh \ - playback.keyboard.enabled=true -``` - -## 查看器模式 - -控制显示哪些查看器窗口： - -```bash -# 显示全部查看器（动捕 + 重定向 + sim2sim） -python scripts/run/run_sim.py controller.policy_path=track.onnx viewers=all - -# 无查看器（无头模式） -python scripts/run/run_sim.py controller.policy_path=track.onnx viewers=none - -# 指定查看器 -python scripts/run/run_sim.py controller.policy_path=track.onnx 'viewers=[retarget,sim2sim]' -``` - -## 下一步 - -- [离线 Sim2Sim 教程](../tutorials/offline-sim2sim) - 包含渲染的完整指南 -- [Pico Sim2Sim](../tutorials/pico-sim2sim) - 在 MuJoCo 中验证 Pico 追踪 -- [独立站立测试](../tutorials/standalone-standing) - 检查 G1 bridge、网络和 policy 站立 -- [Pico Sim2Real](../tutorials/pico-sim2real) - 将 Pico 遥操作部署到 Unitree G1 -- [BVH Sim2Real](../tutorials/bvh-sim2real) - 在 Unitree G1 上回放离线 BVH 动作 -- [主机策略 Sim2Real](../tutorials/high-level-policy-sim2real) - 将独立 LeRobot 策略主机连接到 onboard motion tracker -- [训练](../tutorials/training) - 训练你自己的策略 diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/intro.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/intro.md index 30fa4e3c..3b20f7fa 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/intro.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/intro.md @@ -3,43 +3,38 @@ sidebar_position: 1 slug: / --- -# 简介 - -**Teleopit** 是一个轻量、可扩展的人形机器人全身遥操作框架。它能够将人类操作者的动作实时映射到 Unitree G1 机器人上，同时支持 MuJoCo 仿真和实物硬件部署。 - -## 核心特性 - -- **离线 sim2sim**：在 MuJoCo 中回放 BVH 动捕文件，通过 RL 策略驱动机器人 -- **VR 遥操作**：基于 Pico 4 / Pico 4 Ultra 全身追踪的实时全身控制 -- **Sim2Real 部署**：使用同一套流程直接部署到 Unitree G1 实物 -- **训练流程**：基于 General-Tracking-G1 任务的端到端强化学习训练 -- **可扩展设计**：基于协议的组件体系（InputProvider、Retargeter、Controller、Robot） - -## 流程概览 - -```text -InputProvider (BVH / Pico4 VR) - -> Retargeter (GMR) - -> ObservationBuilder (167D) - -> Controller (双输入 TemporalCNN ONNX) - -> Robot (MuJoCo 仿真 或 Unitree G1) -``` - -## 技术规格 - -| 项目 | 参数 | -|------|------| -| 策略频率 | 50 Hz | -| PD 控制频率 | 200 Hz | -| 观测维度 | 167D | -| 动作维度 | 29D（G1 关节） | -| ONNX 模型 | 双输入 TemporalCNN | -| 运动重定向 | GMR（General Motion Retargeting） | -| 仿真器 | MuJoCo | -| 硬件平台 | Unitree G1（29 自由度） | - -## 下一步 - -- [安装指南](getting-started/installation) - 搭建开发环境 -- [快速上手](getting-started/quick-start) - 运行你的第一个 sim2sim 示例 -- [教程](tutorials/offline-sim2sim) - 各使用场景的详细步骤指引 +# Teleopit + +Teleopit 是一套面向 Unitree G1 的**全身具身遥操作系统**。操作者戴上 +Pico 4 或 Pico 4 Ultra 后，可以实时控制机器人的全身动作；接入可选的 +LinkerHand 后，还能控制手势；接入可选的 OpenNeck 后，头部动作可以直接控制 +机器人相机的朝向。 + +同一套运控策略会先在 MuJoCo 中运行。你可以先在仿真里确认动作和控制方式，再连接 +真实机器人。 + +## 从这里开始 + +第一次使用 Teleopit 时，建议按这个顺序： + +1. 根据自己的目标[安装 Teleopit](getting-started/installation)，并完成该页面最后的 + 安装检查。 +2. 从下面四条路径中选择一条继续。 + +| 我想做什么 | 对应教程 | +|------------|----------| +| 在 MuJoCo 中检查运控策略 | [在仿真中运行运控](tutorials/offline-sim2sim) | +| 不连接真机，先尝试 Pico VR 遥操 | [在仿真中进行 VR 遥操](tutorials/pico-sim2sim) | +| 使用 Pico VR 控制真实 G1 | [用 VR 遥操真实 G1](tutorials/pico-sim2real) | +| 训练并导出自己的运控策略 | [训练运控策略](tutorials/training) | + +:::warning 连接真机之前 +请先把 Pico 仿真遥操跑通。真机运行时始终把 Unitree 遥控器拿在手里； +`L1+R1` 是进入 `DAMPING` 的紧急停止方式。 +::: + +## 想了解实现细节？ + +主线教程只保留完成任务所需的内容。运行流程和技术规格见 +[系统架构](reference/architecture)，下载文件与资源分组见 +[资源参考](reference/assets)，Hydra 参数见[配置说明](configuration/overview)。 diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/architecture.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/architecture.md index 3b186600..cf6ca936 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/architecture.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/architecture.md @@ -4,7 +4,7 @@ sidebar_position: 1 # 架构 -面向开发者的系统内部结构和技术约束。 +本页集中说明运行流程、支持边界和精确维度。这些实现细节不会放在任务主线教程中。 ## Pipeline @@ -18,6 +18,24 @@ InputProvider（BVH 文件 / Pico4） 离线/在线推理由 `teleopit/runtime/` 和 `teleopit/pipeline.py` 装配。硬件状态机通过 `teleopit/sim2real/mp/` 中的进程隔离运行时执行。训练由 `train_mimic/` 提供。 +## Pico 全身具身控制路径 + +同一帧 Pico 数据可以同时进入三条相互独立的控制路径： + +```text +Pico 全身追踪 + -> GMR 动作重定向 -> 运控策略 -> G1 全身关节 + +Pico 手势追踪或手柄输入 + -> Teleopit 手部适配 -> somehand 或开合映射 -> LinkerHand L6/O6 + +Pico 头显旋转 + 同帧 Spine3 旋转 + -> 相对 yaw/pitch 映射 -> OpenNeck +``` + +全身控制是必需路径，手部和 OpenNeck 是可选的独立进程。它们发生故障时不能停止 G1 +身体控制。三条路径复用同一个进程内 PicoBridge。 + 由主机提供服务的模仿策略使用第二条相互独立的部署路径： ```text @@ -71,13 +89,20 @@ train_mimic/scripts/data | 项目 | 规格 | |---|---| +| 支持机器人 | Unitree G1，29 个驱动关节 | +| 仿真器 | MuJoCo | +| 动作重定向 | GMR（General Motion Retargeting） | +| 运控 / PD 频率 | 50 Hz / 200 Hz | | 训练任务 | `General-Tracking-G1` | | 推理观测 | `velcmd_history`（167D） | | ONNX 签名 | 双输入 `obs`（167D）+ `obs_history` | +| 运控输出 | 相对 `default_dof_pos` 的 29D 关节 offset | | Actor/Critic | TemporalCNN（2048、1024、512、256、128） | | 训练采样 | 默认 `rewind`；也支持 `uniform`；播放使用 `start`；benchmark 固定精确 clip 并禁用 clip 末尾重采样 | | 训练 `window_steps` | `[0]` | | 数据格式 | 可递归发现的最小 HDF5 shard（`shard_*.h5`） | +| 可选灵巧手 | LinkerHand L6 或 O6，支持手柄开合或 Pico 手势 | +| 可选主动视觉 | 使用物理角度控制 OpenNeck yaw/pitch | | 主机策略 observation | JPEG RGB + `observation.state(68)` | | 主机策略 action | 30 Hz 的 `float32[T,50]` canonical reference | | 主机策略 body 控制 | 36D root/joint reference 通过现有 50 Hz motion tracker | diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/assets.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/assets.md index 98ba4a86..a6963389 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/assets.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/assets.md @@ -2,9 +2,11 @@ sidebar_position: 2 --- -# 资源管理 +# 资源参考 -数据集、checkpoint、机器人模型和演示媒体不进 Git 历史，统一走外部下载。Unitree G1 的 canonical 模型下载到 `assets/robots/unitree_g1/g1_29dof.xml`。 +Teleopit 的 Git 仓库只保存代码，不保存大型机器人 mesh、运控模型和动作数据。 +[安装说明](../getting-started/installation)给出了每种用户场景最短的下载命令；本页提供 +完整文件清单和维护者说明。 ## 不入库的内容 @@ -13,6 +15,19 @@ sidebar_position: 2 - `data/`、checkpoint、缓存等生成产物 - 演示媒体（`assets/demo.gif`、`assets/demo.mp4`） +## 资源清单 + +| 资源组 | 下载后的路径 | 用途 | +|--------|--------------|------| +| `ckpt` | `track.onnx`、`track.pt` | 可直接运行的推理模型和对应 PyTorch checkpoint | +| `robots` | `assets/robots/unitree_g1/g1_29dof.xml` 与 mesh | 训练、MuJoCo 推理、GMR 和数据集 FK | +| `gmr` | `teleopit/retargeting/gmr/assets/` | 动作重定向模型和 IK 配置 | +| `bvh` | `data/sample_bvh/*.bvh` | 安装检查和仿真教程使用的示例动作 | +| `data` | `data/datasets//shard_*.h5` | 用于分发的精简动作数据；训练前需要预计算 | + +`assets/robots/unitree_g1/g1_29dof.xml` 是项目唯一标准的 G1 入口。GMR 资源目录中的 +XML 不能替代它。 + ## 远程仓库 ### ModelScope（默认下载源） @@ -29,7 +44,7 @@ sidebar_position: 2 | `12e21/Teleopit-models` | model | checkpoint、GMR retargeting 资源、示例 BVH | | `12e21/Teleopit-datasets` | dataset | 训练/验证数据集 | -### 资源组与仓库的对应关系 +### 资源组与仓库对应关系 | 组 | 仓库 | 远端路径 | |----|------|---------| @@ -39,7 +54,7 @@ sidebar_position: 2 | `bvh` | Teleopit-models | `archives/sample_bvh.tar.gz` | | `data` | Teleopit-datasets | `data/datasets/*/*.h5`（`lafan1`、`pico_record`、`seed`、`twist2`） | -## 下载 +## 下载行为 使用项目自带的下载脚本（默认从 ModelScope 下载）： diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/dataset.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/dataset.md index 93fb2f21..b5bd65c6 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/dataset.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/dataset.md @@ -2,7 +2,15 @@ sidebar_position: 3 --- -# 数据集 +# 数据集参考 + +Teleopit 使用两类相互独立的数据： + +- **动作数据集**为运控训练提供参考动作； +- **真机 episode 录制**保存同步的机器人状态、参考动作和相机视频，供后续检查或外部 + 策略使用。 + +两类数据的 schema 不同，不能相互替换。 ## 下载预构建数据集（推荐） @@ -166,3 +174,54 @@ python train_mimic/scripts/data/check_motion_npz_fk.py \ ``` 推荐判据：`pos_max < 1e-3 m`、`quat_mean < 0.05 rad`、`quat_p95 < 0.10 rad`。 + +## 真机 Episode 录制 + +录制程序写出的是一个可编辑数据集，而不是单个包含所有内容的 HDF5： + +```text +data/recordings/sim2real_hdf5/ +├── schema.json +├── episodes.jsonl +├── data/ +│ └── episode_000000.h5 +└── videos/ + └── d435i_rgb/ + └── episode_000000.mp4 +``` + +`schema.json` 定义数据集 FPS、`robot_type`、`hand_type`、`neck_type`，以及每个字段的 +shape、dtype、名称和分组。硬件类型必须与当前运行配置一致。 + +`episodes.jsonl` 是可编辑的 episode 清单。每一行把一条 episode 映射到对应 HDF5 +和 MP4，并保存任务描述。任务文本不会写入 HDF5 attribute。 + +每个 HDF5 只包含按帧对齐的数组： + +| 字段 | Shape | 含义 | +|------|-------|------| +| `frame_index` | scalar | 相机/动作帧序号 | +| `timestamp` | scalar | 单调时钟时间戳，单位为秒 | +| `observation.state` | `(68,)` | G1 关节状态、基座方向/角速度和投影重力 | +| `observation.mode` | scalar | `STANDING`、`MOCAP`、`ARMS` 或动捕暂停状态码 | +| `action` | `(36,)` | motion tracker 使用的根部姿态和 29 关节参考 | +| `action.hand` | `(12,)`，可选 | 启用手部控制时的左右 LinkerHand 目标 | +| `action.neck` | `(2,)`，可选 | 经过机械限位后的 OpenNeck yaw/pitch 角度 | + +相机 RGB 只保存在 MP4 中，HDF5 不再重复保存 raw image。只有启用对应硬件时，才会 +出现可选 action 字段。 + +录制器会先提交 HDF5/视频文件，再向清单追加记录。进程中断后，未提交的 episode 会在 +下次录制进程启动时删除，也不会占用 episode 序号。已有 `schema.json` 与当前配置 +不兼容时，只会停止非关键的录制进程。 + +使用下面的命令查看数据： + +```bash +python scripts/view/view_recording.py \ + --recording data/recordings/sim2real_hdf5 +``` + +播放前，查看器会检查清单路径、HDF5 shape/dtype/有限值和 MP4 对齐。录制数据不包含 +实测根部 XYZ，因此 Viewer 中的实测机器人会锚定到参考根部位置；这个格式无法评估 +全局根部平移。 diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/offline-sim2sim.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/offline-sim2sim.md index b5731659..5d947344 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/offline-sim2sim.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/offline-sim2sim.md @@ -2,75 +2,104 @@ sidebar_position: 1 --- -# 离线 Sim2Sim +# 在仿真中运行运控 -在 MuJoCo 仿真环境中，使用 BVH 动捕文件驱动 RL 策略进行全身运动复现。 +本教程让训练好的运控策略在 MuJoCo 中复现一段动作。在接入 VR 或真实机器人之前， +先用它确认两个最基本的问题： -## 基本播放 +- 运控模型能否正常加载，并让 G1 保持稳定？ +- 重定向后的机器人动作是否与原始动作一致？ + +## 开始之前 + +按照[安装说明](../getting-started/installation)安装基础依赖，并下载 +`robots gmr ckpt bvh` 资源包。 + +## 1. 运行示例动作 ```bash python scripts/run/run_sim.py \ controller.policy_path=track.onnx \ - input.bvh_file=data/sample_bvh/aiming1_subject1.bvh + input.bvh_file=data/sample_bvh/aiming1_subject1.bvh \ + playback.keyboard.enabled=true ``` -### 使用 hc_mocap 格式 +最重要的是 `sim2sim` 窗口：它显示的是运控策略和物理仿真共同产生的 G1 动作，而不是 +单纯的运动学目标。 + +| 按键 | 作用 | +|------|------| +| `Space` 或 `P` | 暂停或继续 | +| `R` | 从第一帧重新播放 | +| `Q` | 停止 | + +机器人能够保持稳定，并大致跟上动作的节奏和姿态，就说明运行正常。少量跟踪误差是正常 +的；摔倒、关节不动或朝向明显错误则不是。 + +## 2. 对比三个视图 + +动作异常时，打开全部视图可以判断问题从哪一步开始： ```bash python scripts/run/run_sim.py \ controller.policy_path=track.onnx \ - input.bvh_file=data/hc_mocap/walk.bvh \ - input.bvh_format=hc_mocap + input.bvh_file=data/sample_bvh/aiming1_subject1.bvh \ + viewers=all ``` -## 键盘交互重播 +| 视图 | 显示内容 | +|------|----------| +| `mocap` | 从 BVH 中读取的人体骨架 | +| `retarget` | GMR 生成的 G1 运动学目标 | +| `sim2sim` | 经过运控推理和 MuJoCo 物理后的 G1 | + +如果 `mocap` 就不对，先检查 BVH 格式；如果 `mocap` 正常但 `retarget` 不对，检查动作 +重定向；如果只有 `sim2sim` 不对，检查运控模型和观测配置。 -为离线 BVH 播放启用键盘交互控制： +也可以只打开需要的视图： ```bash +# 只看物理仿真结果 python scripts/run/run_sim.py \ controller.policy_path=track.onnx \ input.bvh_file=data/sample_bvh/aiming1_subject1.bvh \ - playback.keyboard.enabled=true + viewers=sim2sim + +# 不打开窗口，适合服务器或时序测试 +python scripts/run/run_sim.py \ + controller.policy_path=track.onnx \ + input.bvh_file=data/sample_bvh/aiming1_subject1.bvh \ + viewers=none ``` -| 按键 | 功能 | -|------|------| -| `Space` / `P` | 暂停 / 继续 | -| `R` | 从头重播 | -| `Q` | 停止 | +关闭所有已打开的 Viewer 后，仿真会自动结束。 -其他可选参数： +## 3. 使用自己的 BVH -```bash -# 动作播放结束后自动暂停 -playback.pause_on_end=true - -# 限制仿真步数（0 = 无限） -num_steps=300 +LAFAN1 格式： -# 按真实时间速率播放（即使无 Viewer 窗口也生效） -realtime=true +```bash +python scripts/run/run_sim.py \ + controller.policy_path=track.onnx \ + input.bvh_file=/path/to/motion.bvh \ + input.bvh_format=lafan1 ``` -## Viewer 模式 - -Viewer 以独立子进程运行。使用 shell 引号传递列表参数。 +`hc_mocap` 格式： ```bash -viewers=sim2sim # 默认模式 -viewers=all # mocap + retarget + sim2sim 三视图 -viewers=none # 无头模式（不显示窗口） -'viewers=[retarget,sim2sim]' # 自定义组合 +python scripts/run/run_sim.py \ + controller.policy_path=track.onnx \ + input.bvh_file=/path/to/motion.bvh \ + input.bvh_format=hc_mocap ``` -:::note -当所有 Viewer 窗口被关闭后，仿真会自动结束。 -::: +Teleopit 不会猜测未知的骨架布局。一个文件即使是合法 BVH，也可能需要先写适配器才能 +作为支持的格式使用。 -## 离线渲染 +## 4. 保存视频 -在无头模式下将仿真渲染为视频： +需要可重复的视频结果而不是交互窗口时： ```bash MUJOCO_GL=egl python scripts/render/render_sim.py \ @@ -78,13 +107,30 @@ MUJOCO_GL=egl python scripts/render/render_sim.py \ --policy track.onnx ``` -使用 hc_mocap 格式时： +`hc_mocap` 输入需要再加 `--format hc_mocap`。渲染脚本会输出同步的 `mocap`、 +`retarget` 和 `sim2sim` 视频。 + +## 常用播放参数 ```bash -MUJOCO_GL=egl python scripts/render/render_sim.py \ - --bvh data/hc_mocap/wander.bvh \ - --format hc_mocap \ - --policy track.onnx +# 动作结束后保持最后姿态 +playback.pause_on_end=true + +# 运行 300 个仿真 step；0 表示不限制 +num_steps=300 + +# 即使不打开 Viewer，也按照真实时间运行 +realtime=true ``` -渲染管线输出三个视角（动捕输入、重定向、sim2sim），均通过 MuJoCo 渲染。 +完整字段见[配置说明](../configuration/overview)。 + +## 常见问题 + +| 现象 | 检查内容 | +|------|----------| +| 运控模型路径报错 | 确认 `track.onnx` 存在，或传入自己导出的 ONNX 路径 | +| 缺少 XML、mesh 或 GMR 配置 | 下载 `robots` 和 `gmr` 资源组 | +| 从 `mocap` 视图开始动作就旋转或变形 | 确认 `input.bvh_format` 与文件骨架一致 | +| 只有 `sim2sim` 中机器人会摔倒 | 确认 ONNX 使用当前 167D `velcmd_history` 观测导出 | +| EGL/OpenGL 报错 | 在桌面环境使用交互 Viewer，或先配置 EGL 再进行无头渲染 | diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md index e4213b6a..84c3c241 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md @@ -1,79 +1,76 @@ --- -sidebar_position: 4 +sidebar_position: 3 --- -# Pico 4 VR 真机遥操作 +# 用 VR 遥操真实 G1 -在 [Pico Sim2Sim](pico-sim2sim) 跑通后，使用本教程把同一条实时 Pico 输入路径部署到 -真实 Unitree G1。 +本教程把已经跑通的 Pico 仿真遥操迁移到真实 Unitree G1。动作输入没有变化；新增加的 +关键环节是 G1 网络、DDS bridge 和安全的模式切换。 -```text -Pico 头显 -> Teleopit host -> retarget -> RL policy -> g1_bridge_sdk -> G1 -``` - -有两种部署方式： - -| 部署方式 | Teleopit 运行位置 | 主要区别 | -|----------|-------------------|----------| -| Wired PC-to-G1 | 外部工作站或笔记本 | 将 `real_robot.network_interface` 设置为 PC 上连接 G1 的以太网接口 | -| Onboard | G1 onboard 计算机 | 在 onboard 计算机安装 Teleopit；通常使用 `eth0` | +:::danger 始终把 Unitree 遥控器拿在手里 +动作异常时立即按 `L1+R1` 进入 `DAMPING`。第一次运行时清空机器人周围空间，并安排 +一名操作者随时扶住或停止机器人。 +::: -两种方式都使用 `Pico4InputProvider` 和进程内 pico-bridge receiver。不存在单独的 -onboard Pico 输入模式。 +## 开始之前 -Teleopit 面向 pico-bridge 0.2.1 及其 `pico_native` tracking 语义。 +下面每一项都满足后再继续： -## 1. 安装运行时依赖 +- [在仿真中进行 VR 遥操](pico-sim2sim)已经稳定运行； +- 已按照[安装说明](../getting-started/installation)安装 `pico4` 依赖并编译 + `g1_bridge_sdk`； +- `track.onnx`、机器人资源和 GMR 资源都已下载； +- 运行 Teleopit 的电脑通过有线网络连接 G1 DDS； +- 没有其他程序正在向机器人发送控制命令。 -在运行 Teleopit 的机器上安装 Pico 和 sim2real 依赖： +Teleopit 可以运行在通过网线连接 G1 的外部电脑上，也可以运行在机器人 onboard +电脑上。Pico 始终直接连接运行 Teleopit 的那台电脑。 -```bash -pip install -e '.[pico4]' -git submodule update --init --recursive -bash scripts/setup/setup_g1_bridge.sh -``` +## 1. 找到 G1 使用的网卡 -验证 Pico receiver 导入： +列出 Linux 网卡： ```bash -python -c "from pico_bridge import PicoBridge; print('OK')" +ip -br link ``` -## 2. 选择网络接口 +外部电脑使用连接 G1 的有线网卡，例如 `enp130s0`；onboard 电脑通常使用 `eth0`。 -`real_robot.network_interface` 是用于 Unitree DDS 通信的 Linux 网卡接口。 +运行参数写成： -对于 wired PC-to-G1 部署： +```text +real_robot.network_interface=enp130s0 +``` -1. 用网线连接 PC 和 G1。 -2. 在 PC 上运行 `ifconfig`。 -3. 使用连接到机器人的以太网接口，例如 `enp130s0`。 -4. 确保 Pico 头显所在网络可以访问运行 Teleopit 的 PC。 +这个网卡只负责 Unitree DDS。如果 Pico 自动发现选择了错误的 Wi-Fi 或网口地址， +需要另外设置 `input.bridge_advertise_ip`。 -对于 onboard 部署： +## 2. 先单独检查站立运控 -1. 在机器人 onboard 计算机上运行 Teleopit。 -2. 确保 Pico 头显所在网络可以访问 onboard 计算机。 -3. 除非机器人网络不同，否则使用 `real_robot.network_interface=eth0`。 -4. 如果 Pico discovery 广播了错误地址，设置 `input.bridge_advertise_ip=`。 +在接入 Pico 之前，先运行真机流程使用的同一套站立运控： -### Arm Onboard 的 RealSense 配置 +```bash +python scripts/run/standalone_standing.py \ + --policy track.onnx \ + --network-interface enp130s0 \ + --dry-run +``` -pico-bridge PC receiver 在所需 Python 依赖可用时支持 Arm 机器。对于需要 RealSense -预览的 Arm onboard 计算机，应在当前 Conda 环境中从 conda-forge 安装 `pyrealsense2`， -不要依赖 pip 包： +`--dry-run` 会检查机器人状态接收和运控时序，但不发送电机命令。在安全的硬件环境中 +确认无误后，再去掉 `--dry-run`： ```bash -pip uninstall pyrealsense2 -conda install -c conda-forge pyrealsense2 +python scripts/run/standalone_standing.py \ + --policy track.onnx \ + --network-interface enp130s0 ``` -这只影响可选的 RealSense 预览路径（`input.video.enabled=true`）。Pico 追踪和机器人控制 -本身不需要 RealSense。 +如果这一步失败，请停在这里并查看[独立站立检查](standalone-standing)。Pico 无法 +解决 G1 bridge 或运控模型本身的问题。 -## 3. 运行控制器 +## 3. 启动 Pico 真机遥操 -Wired PC 示例： +外部电脑示例： ```bash python scripts/run/run_sim2real.py \ @@ -82,7 +79,7 @@ python scripts/run/run_sim2real.py \ real_robot.network_interface=enp130s0 ``` -Onboard 示例： +onboard 电脑示例： ```bash python scripts/run/run_sim2real.py \ @@ -91,255 +88,156 @@ python scripts/run/run_sim2real.py \ real_robot.network_interface=eth0 ``` -## 可选 HDF5 录制 - -在负责 Pico 输入和 RealSense 的机器上安装 recording extra： - -```bash -pip install -e '.[recording]' -``` - -运行录制配置： - -```bash -python scripts/run/run_sim2real.py \ - --config-name sim2real_record \ - controller.policy_path=track.onnx \ - real_robot.network_interface=enp130s0 \ - recording.task="walk forward" -``` - -终端控制为：`R` 开始 episode，`S` 保存，`D` 丢弃，`Q` 关闭。可以录制 -`STANDING`、`MOCAP`、`ARMS` 和暂停状态的 mocap；已经保存的 episode 不支持再丢弃。 -episode 会保存为 `data/recordings/sim2real_hdf5/data/` 下的 `.h5` 文件， -压缩 MP4 视频保存在 `data/recordings/sim2real_hdf5/videos/d435i_rgb/` 下。 -数据集级 `schema.json` 保存机器人/灵巧手/颈部类型和 feature 定义， -`episodes.jsonl` 保存每个 episode 的文件映射与可编辑任务 prompt。HDF5 以 -30 Hz 保存 `frame_index`、`timestamp`、`observation.state(68)`、标量 -`observation.mode` 和作为 motion-tracker reference 的 36D `action`。启用灵巧手 -控制时还会保存 `action.hand(12)`。启用 OpenNeck 控制时，会把最新经过机械限位裁剪的 -`[yaw_deg, pitch_deg]` 目标以度为单位保存为 `action.neck(2)`。未启用的设备不会添加 -对应的 action 字段。 - -只有存在新鲜 RealSense 帧时才能开始录制。RealSense 超时或断连会触发后台重连， -不会停止 Pico 输入或 G1 控制。录制期间视频不可用达到一秒时，当前 episode 会被 -丢弃；视频恢复后需要再次按 `R`。如果整个 Pico 输入 worker 退出，G1 控制会继续 -运行并保持最新命令，操作员仍可使用 Unitree 遥控器让机器人返回 `STANDING` 或请求 -`DAMPING`。 +启动进程并不会立刻让 Pico 接管机器人。 -### Review 已保存的 Episode +## 4. 主动、逐步地交出控制权 -安装轻量 review 依赖，然后对录制根目录启动只读 Web reviewer： +1. 按遥控器 `Start` 进入 `STANDING`。 +2. 等待机器人稳定，同时确认 Pico 追踪有效。 +3. 操作者以中立姿态站好，周围留出足够空间。 +4. 按遥控器 `Y` 进入 `MOCAP`。 +5. 先从小幅慢动作开始。 +6. 需要回到站立时按遥控器 `X`。 -```bash -pip install -e '.[review]' -python scripts/view/view_recording.py \ - --recording data/recordings/sim2real_hdf5 -``` - -在浏览器中打开终端输出的本地 URL。reviewer 会同步显示 D435i MP4、MuJoCo -中的 G1 实测姿态以及绿色半透明 reference 姿态。可以通过 episode 选择器、帧拖动条、 -播放速度和关节选择器检查跟踪效果。侧栏还包含模式时间线、各身体分组的关节误差， -以及可选的 LinkerHand 通道和 OpenNeck yaw/pitch。 - -播放前，reviewer 会验证 `schema.json`、manifest 中的所有路径、HDF5 shape 和有限值， -以及 MP4 帧数/FPS；它不会修改录制数据。`observation.state` 不包含实测 root XYZ， -因此叠加画面会把实测机器人锚定到 reference root 位置。关节跟踪和 root 朝向比较仍然 -有效，但无法通过当前录制格式评价全局 root 平移。 - -## 操作流程 - -始终把 Unitree 遥控器拿在手里。`L1+R1` 是进入 `DAMPING` 的急停路径。 - -| 控制 | 动作 | +| 控制 | 作用 | |------|------| -| Unitree remote `Start` | 进入 `STANDING` | -| Unitree remote `Y` | 进入 `MOCAP` | -| Unitree remote `B` | 暂停 / 恢复实时动捕 | -| Pico/controller `A` | 暂停 / 恢复实时动捕 | -| Pico/controller `B` | 在 `MOCAP` / `ARMS` 之间切换 | -| Unitree remote `X` | 返回 `STANDING` | -| Unitree remote `L1+R1` | 急停（`DAMPING`） | - -只在 Pico 追踪稳定后进入 `MOCAP`。Teleopit 会在切换前验证连续动捕帧；验证失败时， -机器人会保持在 `STANDING`。 - -## 运行时行为 - -Pico sim2real 使用共享的实时参考时间线： - -```text -Pico body frames -> retarget -> reference buffer -> observation -> policy -> G1 joints -``` - -进入 `STANDING` 时，Teleopit 会释放当前 Unitree 模式，进入 debug/low-level 控制， -短暂锁住当前关节，重置 policy 状态，并在不改变 policy target 的情况下执行 Kp ramp。 - -进入 `MOCAP` 时，Teleopit 会重新 arm 进程隔离的 reference worker，重置其中的 GMR 状态 -和实时 reference buffer，然后等待新的已验证 reference，再开始跟踪实时 mocap 命令。 -`STANDING` 和 `DAMPING` 会让 reference worker 保持 disarmed，避免冷启动帧在进入 mocap -之前 warm-start retargeting。 - -`ARMS` 会保持同一条实时 retargeting 时间线继续运行，但发送给 motion tracker 的参考会被组合： -身体、腰部和腿部保持站立姿态，双臂跟随实时 retarget 结果。进入或离开 `ARMS` 时会重置 -policy/reference 对齐，并使用同一套 Kp ramp 安全路径。 - -## 暂停 / 恢复 - -Pico 暂停/恢复是 mocap-session control event。可以使用 Unitree remote `B` 或 -Pico/controller `A`；Pico/controller `B` 仍用于切换 `MOCAP` / `ARMS`。 - -- `ACTIVE`：暂停键冻结当前参考姿态。 -- `PAUSED`：再次按下会清空 policy/reference 状态，预热实时 buffer，重新居中 yaw/XY 对齐， - 并从实时 mocap 恢复。 - -:::warning -恢复时请保持静止，并尽量接近暂停时的姿态。这样可以减少实时追踪恢复时的参考突变。 -::: +| Unitree 遥控器 `Start` | 进入 `STANDING` | +| Unitree 遥控器 `Y` | 开始全身 VR 控制（`MOCAP`） | +| Unitree 遥控器 `B` | 暂停或恢复当前动捕会话 | +| Pico/controller `A` | 暂停或恢复当前动捕会话 | +| Pico/controller `B` | 在全身 `MOCAP` 和仅手臂 `ARMS` 之间切换 | +| Unitree 遥控器 `X` | 结束 VR 控制并返回 `STANDING` | +| Unitree 遥控器 `L1+R1` | 紧急停止（`DAMPING`） | -## 可选 LinkerHand 控制 +进入 `MOCAP` 之前，Teleopit 会连续检查多帧 Pico 数据。检查失败时，机器人会继续 +留在 `STANDING`。 -Pico sim2real 可以用 Pico 输入控制 LinkerHand： +### 暂停和恢复 -- `gripper`：按住同侧 grip 作为 deadman，同侧 trigger 控制对应手闭合。 - 该模式支持 `hands.driver=linkerhand_l6` 和 `hands.driver=linkerhand_o6`； - 速度和张开/闭合姿态来自对应 driver 配置。 -- `vr_hand_pose`：通过 somehand 重定向 Pico 手部 pose，并下发连续 L6 或 O6 手部目标。 - 如果某侧手部 pose 消失，该侧会保持上一条手势命令。这个模式使用 Teleopit 的 - Pico landmark 适配器和 somehand 0.2.0 公开的 `somehand.api`，并始终将所选手的 - 速度设为最大值。默认配置使用 60 Hz 的低延时 somehand 路径并减少平滑，所以响应会更快， - 但可能比标准 somehand 设置更抖。 +暂停会保持当前参考姿态，不会让机器人回到 `STANDING`。恢复时，系统会根据操作者 +当前姿态重新建立实时对齐。恢复前请站稳，并尽量保持在暂停姿态附近。需要结束 VR +会话时使用遥控器 `X`。 -`hands.enabled=true` 时，手控会在所有 sim2real 模式中保持生效。退出和手控运行时失败会发送配置的张开姿态。 +### Pico 或视频中断时会怎样？ -如果主 Pico profile 没有包含手控支持，先安装本地手控包： +Pico 输入和视频预览都不是关键控制进程。Pico 输入停止后，G1 控制循环会继续保持 +最后一个安全命令，Unitree 遥控器仍然可用。RealSense 超时只会关闭或重连视频， +不会停止身体控制。此时请主动按遥控器 `X` 或 `L1+R1`，不要等待系统自动切换模式。 -```bash -git submodule update --init --recursive -pip install -e third_party/linkerhand-python-sdk -pip install -e third_party/somehand -bash scripts/setup/download_somehand_assets.sh -``` +## 可选：LinkerHand 控制 -测试或运行手控前，先开启 CAN 接口： +没有连接 LinkerHand 时请跳过本节。先按照[安装说明](../getting-started/installation) +安装手部依赖，再启动两个 CAN 接口： ```bash sudo /usr/sbin/ip link set can0 up type can bitrate 1000000 sudo /usr/sbin/ip link set can1 up type can bitrate 1000000 ``` -启用完整 sim2real 前，先用独立开合测试验证灵巧手连接。测试默认一直运行到 Ctrl-C： +启动机器人之前先单独测试手： ```bash python scripts/dev/test_linkerhand.py \ + --driver linkerhand_o6 \ --hand-type both \ --left-can can0 \ --right-can can1 ``` -O6 独立开合测试需要加上 O6 driver： +启用 O6 手势追踪时，在主命令后增加： -```bash -python scripts/dev/test_linkerhand.py \ - --driver linkerhand_o6 \ - --hand-type both \ - --left-can can0 \ - --right-can can1 +```text +hands.enabled=true +hands.driver=linkerhand_o6 +hands.mode=vr_hand_pose +hands.linkerhand_o6.left_can=can0 +hands.linkerhand_o6.right_can=can1 ``` -如果要用实时 Pico gripper 输入测试 O6，再加 `--mode gripper`。如果要用实时 Pico -手部 pose 重定向测试 O6，再加 `--mode vr_hand_pose`。 +使用手柄扳机开合时设置 `hands.mode=gripper`。系统也支持 `linkerhand_l6`，此时使用 +对应的 `hands.linkerhand_l6.*` CAN 参数。手部控制在所有机器人模式下都保持工作； +手部进程出错时会发送张开手的命令。 -然后在 Pico sim2real 中启用 L6 gripper 控制： +## 可选：OpenNeck 主动视觉 + +没有安装和标定 OpenNeck 时请跳过本节： ```bash -hands.enabled=true -hands.driver=linkerhand_l6 -hands.mode=gripper -hands.linkerhand_l6.left_can=can0 -hands.linkerhand_l6.right_can=can1 +pip install -e '.[openneck]' +openneck calibrate ``` -O6 gripper 控制使用： +在主命令后增加： -```bash -hands.enabled=true -hands.driver=linkerhand_o6 -hands.mode=gripper -hands.linkerhand_o6.left_can=can0 -hands.linkerhand_o6.right_can=can1 +```text +neck.enabled=true +neck.port=/dev/ttyACM0 ``` -连续 L6 VR 手部 pose 控制使用： +OpenNeck 根据 Pico 头显相对操作者上半身的方向转动。它复用身体控制的 Pico 接收器， +不会再启动第二个 PicoBridge。 -```bash -hands.enabled=true -hands.driver=linkerhand_l6 -hands.mode=vr_hand_pose -hands.linkerhand_l6.left_can=can0 -hands.linkerhand_l6.right_can=can1 -``` +## 可选：在头显中预览 RealSense -连续 O6 VR 手部 pose 控制切换 driver 和 CAN 配置键： +安装 `pyrealsense2` 后，在主命令后增加： -```bash -hands.enabled=true -hands.driver=linkerhand_o6 -hands.mode=vr_hand_pose -hands.linkerhand_o6.left_can=can0 -hands.linkerhand_o6.right_can=can1 +```text +input.video.enabled=true +input.video.device=<可选的-realsense-序列号> ``` -## 可选 RealSense 预览 +RealSense 超时后会在后台重连。相机失败不会停止 Pico 追踪或 G1 控制。 + +## 可选：录制和查看数据 -将 G1 RealSense 彩色相机推送回 Pico 头显： +录制需要安装 `recording` 依赖，并且 RealSense 能提供新鲜 RGB 帧： ```bash python scripts/run/run_sim2real.py \ - --config-name pico4_sim2real \ + --config-name sim2real_record \ controller.policy_path=track.onnx \ real_robot.network_interface=enp130s0 \ - input.video.enabled=true \ - input.video.device= + recording.task="walk forward" ``` -RealSense 帧超时或断连时会在后台重连，绝不会停止 Pico 追踪或 G1 控制。 +| 终端按键 | 作用 | +|----------|------| +| `R` | 开始一条 episode | +| `S` | 保存当前 episode | +| `D` | 丢弃当前 episode | +| `Q` | 关闭程序 | -## 常用参数 +如果连续一秒没有新鲜视频帧，当前 episode 会被丢弃，但机器人控制会继续。视频恢复后 +需要手动开始新的 episode。 -```bash -# G1 DDS 网卡接口 -real_robot.network_interface=enp130s0 +查看已保存数据： -# Pico 超时时间 -input.pico4_timeout=30 +```bash +pip install -e '.[review]' +python scripts/view/view_recording.py \ + --recording data/recordings/sim2real_hdf5 +``` -# 覆盖 Pico discovery 广播 IP -input.bridge_advertise_ip=192.168.1.20 +查看器会同步显示相机视频、G1 实测/参考姿态和可选的手部/颈部信号。录制目录和字段 +定义见[数据集参考](../reference/dataset)。 -# 进入 MOCAP 前要求的连续有效动捕帧数 -mocap_switch.check_frames=10 +## 常见问题 -# 更换 Pico 暂停键 -input.pause_button=right_axis_click +| 现象 | 处理方法 | +|------|----------| +| 收不到 `LowState` | 检查网线和 `real_robot.network_interface` | +| 无法导入 `g1_bridge_sdk` | 在当前环境重新运行 `scripts/setup/setup_g1_bridge.sh` | +| 按 `Start` 无法进入站立运控 | 停止其他 Unitree 模式和控制程序后重试 | +| 按 `Y` 无法进入 `MOCAP` | 保持 Pico 追踪有效且稳定，检查动捕验证日志 | +| 暂停后没有回到站立 | 这是正常行为；请使用遥控器 `X` | +| Pico 找不到 Teleopit | 把 `input.bridge_advertise_ip` 设为头显能访问的地址 | +| LinkerHand 不动 | 检查 `hands.enabled`、driver/mode、CAN 状态和独立手部测试 | +| Arm 设备上 RealSense 不可用 | 从 conda-forge 安装 `pyrealsense2` | -# 开启 LinkerHand gripper 控制 -hands.enabled=true -hands.driver=linkerhand_l6 -hands.mode=gripper +## 其他 G1 运行方式 -# 开启头显视频预览 -input.video.enabled=true -``` +主线教程以 Pico VR 为主。以下页面保留给较少使用的硬件检查和部署场景： -## 故障排查 - -| 现象 | 可能原因 | 解决方法 | -|------|----------|----------| -| 没有收到 LowState | 网卡错误或 G1 网络未连接 | 检查网线和 `real_robot.network_interface` | -| `TimeoutError: No Pico4 body data` | 头显未连接或追踪未激活 | 检查头显 app、网络和 `input.pico4_timeout` | -| 无法进入 debug mode | Unitree mode 释放失败 | 停止其他机器人模式后再次按 `Start` | -| 机器人进入 `STANDING` 但不进入 `MOCAP` | 动捕验证失败 | 保持追踪稳定，查看 `mocap_switch.check_frames` 日志 | -| Pico 暂停没有返回 `STANDING` | 这是预期行为 | Pico 暂停只冻结 mocap；按遥控器 `X` 返回 `STANDING` | -| LinkerHand 不动 | `hands.enabled=false`、gripper deadman 未按住、SDK/资产未安装，或 CAN 通道错误 | 设置 `hands.enabled=true` 和 `hands.mode`，运行 `scripts/dev/test_linkerhand.py`，并检查所选 driver 的 `left_can` / `right_can` | -| 视频预览不可用 | RealSense 或视频源失败 | 检查相机权限、`input.video.source` 和日志 | +- [独立站立检查](standalone-standing) +- [在 Unitree G1 上回放 BVH](bvh-sim2real) +- [在 Unitree G1 上部署 Host Policy](high-level-policy-sim2real) diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2sim.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2sim.md index efb96155..f9940d1a 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2sim.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2sim.md @@ -2,60 +2,55 @@ sidebar_position: 2 --- -# Pico 4 VR 仿真遥操作 +# 在仿真中进行 VR 遥操 -使用本教程在接入真实 Unitree G1 之前，先在 MuJoCo 中验证 Pico 4 / Pico 4 Ultra -全身追踪。 +连接真实机器人之前，先用 Pico 控制仿真 G1。不要跳过这一步：头显、网络和身体追踪 +问题都可以在这里解决，不会给硬件带来风险。 -```text -Pico 头显 -> pico-bridge receiver -> retarget -> RL policy -> MuJoCo G1 -``` +## 开始之前 -此流程跑通后，再继续阅读 [Pico Sim2Real](pico-sim2real)。 +你需要： -## 支持设备 +- 支持全身追踪的 Pico 4 或 Pico 4 Ultra； +- 头显和运行 Teleopit 的电脑处于同一网络； +- 已安装 `pico4` 依赖并下载 `robots gmr ckpt bvh` 资源； +- [在仿真中运行运控](offline-sim2sim)已经正常。 -- Pico 4 -- Pico 4 Ultra +## 1. 准备头显 -## 1. 设置头显 +1. 从 [pico-bridge Releases](https://github.com/BotRunner64/pico-bridge/releases) + 下载头显 APK。 +2. 安装 APK： -1. 从 [pico-bridge Releases](https://github.com/BotRunner64/pico-bridge/releases) 下载头显 APK。 -2. 使用 adb 安装： ```bash adb install pico-bridge.apk ``` -3. 启动 pico-bridge 头显 client。 + +3. 在头显中打开 pico-bridge。 4. 开启全身追踪。 -5. 确保头显和 Teleopit host 在同一网络。 -## 2. 安装 Pico Host Extra +Teleopit 使用 pico-bridge 0.2.1。接收程序会直接运行在 Teleopit 进程中，不需要再 +启动一个单独的转发程序。 -在运行 Teleopit 的机器上执行： +## 2. 检查电脑是否收到 Pico 数据 -```bash -pip install -e '.[pico4]' -``` - -验证 receiver 包： +下面的诊断只打印身体帧和连接状态，不会启动机器人运控： ```bash -python -c "from pico_bridge import PicoBridge; print('OK')" +python scripts/dev/test_pico_bridge.py --no-video ``` -Teleopit 会通过 `Pico4InputProvider` 在进程内启动 `pico_bridge.PicoBridge`。 -后续 wired 和 onboard sim2real 部署也使用同一条 Pico 输入路径。 - -Teleopit 面向 pico-bridge 0.2.1 及其 `pico_native` tracking 语义。 +轻微移动身体，确认终端持续收到新的有效帧。按 `Ctrl+C` 结束诊断。 -## 3. 下载资源 +如果自动发现选择了错误的网卡地址，显式指定头显能够访问的地址： ```bash -pip install modelscope -python scripts/setup/download_assets.py --only robots gmr ckpt bvh +python scripts/dev/test_pico_bridge.py \ + --no-video \ + --bridge-advertise-ip=192.168.1.20 ``` -## 4. 运行 Pico Sim2Sim +## 3. 启动仿真 ```bash python scripts/run/run_sim.py \ @@ -63,41 +58,58 @@ python scripts/run/run_sim.py \ controller.policy_path=track.onnx ``` -仿真从 `STANDING` 开始。等待 Pico 追踪激活后，再进入 `MOCAP`。 +机器人会有意从 `STANDING` 开始；只有操作者主动切换后，实时身体追踪才会接管。 + +## 4. 完成第一次 VR 遥操 -| 键盘 | 动作 | +1. 以舒适的中立姿态站好，等待追踪稳定。 +2. 在键盘上按 `Y` 进入 `MOCAP`。 +3. 先做小幅慢动作，确认仿真 G1 正常跟随。 +4. 按 `A` 暂停，再按一次 `A` 恢复。 +5. 按 `X` 返回 `STANDING`。 + +| 按键 | 作用 | |------|------| -| `Y` | 进入 `MOCAP` | -| `A` | 暂停 / 恢复实时动捕 | -| `B` | 在 `MOCAP` / `ARMS` 之间切换 | -| `X` | 返回 `STANDING` | +| `Y` | 开始全身控制（`MOCAP`） | +| `A` | 暂停或恢复当前动捕会话 | +| `B` | 在全身控制 `MOCAP` 和仅手臂控制 `ARMS` 之间切换 | +| `X` | 结束 VR 控制并返回 `STANDING` | | `Q` | 退出 | -`pico4_sim.yaml` 默认使用 `viewers=all`，会打开 mocap、retarget 和 sim2sim -三个 viewer。需要更少窗口时，可使用 `viewers=sim2sim` 或 `viewers=none`。 +三个模式可以简单理解为： -每次从 `STANDING` 进入 `MOCAP` 时，Teleopit 都会重置 GMR、使用当前实时 pelvis -目标初始化其浮动根，并重建实时参考路径。因此，操作者可以在 `STANDING` 中改变朝向， -而不会复用上一次 mocap session 的 IK warm-start。暂停/恢复和 -`MOCAP <-> ARMS` 切换会保留当前 IK warm-start。 +- `STANDING`：机器人在站立运控中等待； +- `MOCAP`：机器人全身跟随操作者； +- `ARMS`：身体、腰和腿保持站立，只有双臂继续跟随。 -## 暂停 / 恢复 +每次重新从 `STANDING` 进入 `MOCAP` 时，系统都会重新对齐实时根部姿态。操作者可以 +在站立状态改变朝向，再重新进入 `MOCAP`。 -Pico 暂停/恢复会冻结 mocap session；它不是切回 `STANDING`。 +:::tip 暂停不等于结束 VR 控制 +`A` 只是冻结并恢复当前动捕姿态。需要结束会话并回到站立时，请按 `X`。 +::: -- 按键盘 `A` 或 Pico/controller 暂停键，冻结当前参考姿态。 -- 再按一次会重建实时参考路径，重新居中 yaw 和地面平面位置，然后从当前实时追踪流继续。 +## 选择 Viewer 布局 -默认 Pico 暂停键是 `A`。支持的覆盖值包括 `B`、`X`、`Y`、`left_axis_click`、 -`right_axis_click`、`left_menu_button` 和 `right_menu_button`。 +Pico 仿真默认会打开动捕、重定向和物理仿真三个视图。不再需要全部视图时，可以减少窗口： -默认 Pico 双臂模式按钮是 `B`。`ARMS` 会让身体、腰部和腿部保持站立姿态，同时双臂跟随 -实时 retarget 结果。 +```bash +# 只看物理仿真结果 +python scripts/run/run_sim.py \ + --config-name pico4_sim \ + controller.policy_path=track.onnx \ + viewers=sim2sim -## 可选头显视频预览 +# 不打开窗口 +python scripts/run/run_sim.py \ + --config-name pico4_sim \ + controller.policy_path=track.onnx \ + viewers=none +``` -pico-bridge 0.2.1 可以在头显中显示 host 侧视频流。在仿真中，Teleopit 可以推送 -MuJoCo `d435i_rgb` 相机： +## 可选：头显视频 + +把仿真的 `d435i_rgb` 相机画面发送回头显： ```bash python scripts/run/run_sim.py \ @@ -106,41 +118,35 @@ python scripts/run/run_sim.py \ input.video.enabled=true ``` -使用 `input.video.source=test-pattern` 可以做 receiver 侧视频 sanity check。如果视频启动失败， -Teleopit 会记录错误、关闭视频，并继续运行追踪和控制。 +使用 `input.video.source=test-pattern` 可以只检查视频链路。视频失败时预览会关闭，但 +身体追踪和运控会继续运行。 -## 常用参数 +## 网络参数 -```bash -# 等待第一帧 Pico body 数据的超时时间 -input.pico4_timeout=30 +大部分网络只需要自动发现。诊断显示网络有问题时再使用这些参数： -# 覆盖 discovery 广播给头显的 IP +```bash +# 向头显广播指定的电脑地址 input.bridge_advertise_ip=192.168.1.20 -# 关闭 discovery 并显式绑定 -input.bridge_discovery=false input.bridge_host=0.0.0.0 input.bridge_port=63901 - -# 更换 Pico 暂停键 -input.pause_button=right_axis_click - -# 关闭键盘模式控制 -keyboard.enabled=false +# 关闭自动发现并显式绑定 +input.bridge_discovery=false +input.bridge_host=0.0.0.0 +input.bridge_port=63901 -# 修改策略频率 -policy_hz=30 - -# 开启头显视频预览 -input.video.enabled=true +# 延长等待第一帧身体数据的时间 +input.pico4_timeout=30 ``` -## 故障排查 +## 常见问题 + +| 现象 | 处理方法 | +|------|----------| +| `ImportError: pico_bridge` | 重新安装 `pico4` 依赖 | +| 启动时提示 pico-bridge 版本过旧 | 重新安装依赖，确保使用 0.2.1 | +| 收不到身体帧 | 打开头显应用、启用全身追踪，并确认 UDP 63901 端口可达 | +| 自动发现广播了错误地址 | 把 `input.bridge_advertise_ip` 设为头显能访问的电脑地址 | +| Viewer 中 G1 不动 | 等待追踪稳定后按 `Y` | +| G1 只有手臂跟随 | 按 `B` 离开 `ARMS`，回到 `MOCAP` | -| 现象 | 可能原因 | 解决方法 | -|------|----------|----------| -| `ImportError: pico_bridge` | 未安装 Pico extra | 执行 `pip install -e '.[pico4]'` | -| 启动提示 pico-bridge 太旧 | 已安装 receiver 不支持所需 API 或 tracking 语义 | 重新安装 Pico extra，确保使用 pico-bridge 0.2.1 | -| `TimeoutError: No Pico4 body data` | 头显未连接或 body tracking 未激活 | 检查头显 app、网络和 `input.pico4_timeout` | -| discovery 找不到 host | 广播 IP 不对或 UDP 被阻断 | 设置 `input.bridge_advertise_ip=`，确认 UDP 端口 `63901` 可达 | -| 仿真机器人不跟随 | 循环仍在 `STANDING` | 追踪准备好后按 `Y` | -| Pico 视频黑屏或被关闭 | 视频源失败或相机不可访问 | 检查 `input.video.source` 和日志 | +这条流程稳定后，再继续[用 VR 遥操真实 G1](pico-sim2real)。 diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/training.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/training.md index 0b2ee359..e9bbda30 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/training.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/training.md @@ -1,39 +1,49 @@ --- -sidebar_position: 5 +sidebar_position: 4 --- -# 训练 +# 训练运控策略 -训练全身追踪策略，并导出为 ONNX 格式用于推理部署。 +本教程从已下载的动作数据开始，最终得到可以在 Teleopit 仿真和真实 G1 上运行的 +ONNX 运控模型。 -:::info -数据准备请参阅 [数据集参考](../reference/dataset)。常见训练问题请参阅 [训练故障排查](../reference/training-troubleshooting)。 -::: +常规训练流程默认使用 NVIDIA GPU。动作数据会在启动时全部加载到内存，因此合并数据集 +越大，需要的内存和显存也越多。 -## 环境安装 +## 开始之前 -```bash -conda create -n teleopit python=3.10 -conda activate teleopit -pip install -e '.[train]' -``` +按照[安装说明](../getting-started/installation)完成： + +- `train` 依赖； +- `robots data` 资源包。 + +检查训练包： -验证安装： ```bash python -c "import train_mimic.tasks; print('training OK')" ``` -下载分发的最小数据集，并生成合并后的预计算训练数据集： +## 1. 预处理已下载的数据集 + +下载的数据是便于分发的精简版本。训练需要另一个目录，其中提前计算好了关节速度和 +身体运动学信息： ```bash -python scripts/setup/download_assets.py --only robots data python train_mimic/scripts/data/precompute_dataset.py \ - data/datasets --outdir data/datasets_precomputed --jobs 8 + data/datasets \ + --outdir data/datasets_precomputed \ + --jobs 8 ``` -## 训练 +下面所有训练、回放和 benchmark 命令都使用 `data/datasets_precomputed`。把原始 +`data/datasets` 直接传给训练会报错，这不是支持的快捷方式。 + +自定义 BVH、PKL、NPZ 或 Pico 录制数据的处理方法见 +[数据集参考](../reference/dataset)。 -### 冒烟测试 +## 2. 先做短时间冒烟测试 + +开始长时间训练前，先确认数据集、仿真器和日志工具能够一起工作： ```bash python train_mimic/scripts/train.py \ @@ -42,7 +52,10 @@ python train_mimic/scripts/train.py \ --motion_file data/datasets_precomputed ``` -### 完整训练 +只要环境能够持续 step、终端输出 loss，并且 +`logs/rsl_rl/g1_general_tracking/` 下生成新的运行目录，这项检查就通过了。 + +## 3. 开始完整训练 ```bash python train_mimic/scripts/train.py \ @@ -51,43 +64,40 @@ python train_mimic/scripts/train.py \ --motion_file data/datasets_precomputed ``` -### 多卡训练 +显存不足时降低 `--num_envs`。默认日志工具是 TensorBoard；需要时可使用 +`--logger wandb` 或 `--logger swanlab`。 + +`--max_iterations` 表示继续训练多少次。例如从 `model_12000.pt` 恢复并设置 +`--max_iterations 18000`，最终会训练到第 30000 次。 + +## 4. 在仿真中查看 checkpoint ```bash -python train_mimic/scripts/train.py \ - --gpu_ids 0 1 2 3 \ - --num_envs 1024 \ - --max_iterations 30000 \ +python train_mimic/scripts/play.py \ + --checkpoint logs/rsl_rl/g1_general_tracking//model_30000.pt \ --motion_file data/datasets_precomputed ``` -### 多机多卡训练 +回放会从每段动作开头开始，并关闭训练噪声。导出前先用它排除明显不稳定的模型。 -跨多台机器训练时，直接使用 `torchrun`： +## 5. 运行 Benchmark ```bash -torchrun \ - --nnodes=$PET_NNODES \ - --nproc_per_node=$PET_NPROC_PER_NODE \ - --node_rank=$PET_NODE_RANK \ - --master_addr=$PET_MASTER_ADDR \ - --master_port=$PET_MASTER_PORT \ - train_mimic/scripts/train.py \ - --num_envs 1024 \ - --max_iterations 1000 \ - --motion_file data/datasets_precomputed +python train_mimic/scripts/benchmark.py \ + --checkpoint logs/rsl_rl/g1_general_tracking//model_30000.pt \ + --motion_file data/datasets_precomputed \ + --num_envs 32 ``` -**注意事项：** -- 多卡模式下 `--num_envs` 为每张 GPU 的环境数量 -- 多机模式下 `--num_envs` 也按每个进程计算，因此总环境数会随 `world_size` 线性增长 -- 默认日志工具为 TensorBoard。使用 `--logger wandb` 或 `--logger swanlab` 可选择 W&B 或 SwanLab；项目名默认使用 `experiment_name` -- `--motion_file` 接受预计算训练数据集根目录或单个预计算 `.h5` shard；shard 会递归发现 -- 如果只有最小分发 shard，先运行 `python train_mimic/scripts/data/precompute_dataset.py --outdir `，再把预计算输出传给训练。 -- 训练会在启动时把所有发现的预计算 motion window 全量加载到内存中。 -- `--max_iterations` 表示追加迭代次数；例如从 `model_12000.pt` 恢复训练并设置 `--max_iterations 18000`，最终将训练到 `model_30000.pt` +Benchmark 会对每个长度足够的 clip 执行一次确定性的 10 秒 rollout，并报告： + +- 平均关节位置误差（`MPJPE`）； +- 根部位置、旋转和速度误差； +- rollout 成功率。 -## 导出 ONNX +结果会保存为文本摘要、JSON、逐 clip CSV 和逐 rollout CSV。 + +## 6. 导出 ONNX ```bash python train_mimic/scripts/save_onnx.py \ @@ -96,40 +106,57 @@ python train_mimic/scripts/save_onnx.py \ --history_length 10 ``` -导出的模型为双输入 ONNX（`obs` + `obs_history`）。推理端需要与当前 `velcmd_history` 观测匹配的 167D 双输入 ONNX 策略。 +输出必须是包含 `obs` 和 `obs_history` 的双输入 TemporalCNN。Teleopit 会在启动时 +检查 167D 观测签名，不兼容的导出文件会直接报错。 + +使用正常运行入口检查导出结果： -## 评估 +```bash +python scripts/run/run_sim.py \ + controller.policy_path=track.onnx \ + input.bvh_file=data/sample_bvh/aiming1_subject1.bvh +``` -### 播放验证 +## 扩展到多张 GPU + +单机多卡： ```bash -python train_mimic/scripts/play.py \ - --checkpoint logs/rsl_rl/g1_general_tracking//model_30000.pt \ +python train_mimic/scripts/train.py \ + --gpu_ids 0 1 2 3 \ + --num_envs 1024 \ + --max_iterations 30000 \ --motion_file data/datasets_precomputed ``` -### 定量评估 +这里的 `--num_envs` 是每张 GPU 的环境数量。 + +多机训练使用 `torchrun`： ```bash -python train_mimic/scripts/benchmark.py \ - --checkpoint logs/rsl_rl/g1_general_tracking//model_30000.pt \ - --motion_file data/datasets_precomputed \ - --num_envs 32 +torchrun \ + --nnodes=$PET_NNODES \ + --nproc_per_node=$PET_NPROC_PER_NODE \ + --node_rank=$PET_NODE_RANK \ + --master_addr=$PET_MASTER_ADDR \ + --master_port=$PET_MASTER_PORT \ + train_mimic/scripts/train.py \ + --num_envs 1024 \ + --max_iterations 1000 \ + --motion_file data/datasets_precomputed ``` -benchmark 使用 OmniXtreme 风格协议：10 秒 clip、每个合格 clip 进行一次确定性 rollout，并输出 `MPJPE(m)`、`root_pos_error(m)`、`root_rot_error(rad)`、`root_vel_error(m/s)` 和 `success_rate(%)`。root error 使用与 tracking command metrics 相同的 anchor 位置、旋转和线速度定义。它使用无训练噪声的 play-mode 观测，并固定精确 clip id/起始时间且禁用 clip 末尾重采样。`--motion_file` 必须指向预计算训练数据集；所有长度足够满足配置 clip 时长的 clip 都会参与评测。 +这里的 `--num_envs` 是每个进程的环境数量，总数会随 world size 增长。 -## 训练架构 +## 常见问题 -```text -train_mimic/scripts - -> train_mimic/app.py - -> single task registry / env builder / runner cfg - -> mjlab + rsl_rl -``` +| 现象 | 检查内容 | +|------|----------| +| Loader 提示数据集是 minimal 格式 | 运行 `precompute_dataset.py`，并使用它的输出目录 | +| 显存不足 | 降低 `--num_envs` | +| 启动加载时内存不足 | 减少参与训练的 precomputed shard，或增加内存 | +| 导出的 ONNX 无法通过 167D 检查 | 使用当前版本的 `save_onnx.py` 和 `--history_length 10` 重新导出 | +| Benchmark 跳过部分 clip | 被跳过的 clip 比配置的评测时长更短 | -关键文件： -- `train_mimic/app.py` - 训练/播放/评估的统一入口 -- `train_mimic/tasks/tracking/config/env.py` - General-Tracking-G1 环境构建器 -- `train_mimic/tasks/tracking/config/rl.py` - TemporalCNN PPO 配置 -- `train_mimic/tasks/tracking/mdp/commands.py` - 支持 `uniform`、`start` 和 `rewind` 采样模式。训练默认使用 `rewind`；播放使用 `start`；benchmark 会固定精确的 clip id 和起始时间。 +任务内部结构和模型维度见[系统架构](../reference/architecture)，具体训练故障见 +[训练问题排查](../reference/training-troubleshooting)。 diff --git a/docs/i18n/zh-Hans/docusaurus-theme-classic/footer.json b/docs/i18n/zh-Hans/docusaurus-theme-classic/footer.json index 607d7fa0..211f9b02 100644 --- a/docs/i18n/zh-Hans/docusaurus-theme-classic/footer.json +++ b/docs/i18n/zh-Hans/docusaurus-theme-classic/footer.json @@ -8,7 +8,7 @@ "description": "The title of the footer links column with title=More in the footer" }, "link.item.label.Getting Started": { - "message": "快速上手", + "message": "安装", "description": "The label of footer link with label=Getting Started" }, "link.item.label.Tutorials": { diff --git a/docs/sidebars.ts b/docs/sidebars.ts index 96799339..4be1c9b7 100644 --- a/docs/sidebars.ts +++ b/docs/sidebars.ts @@ -8,8 +8,6 @@ const sidebars: SidebarsConfig = { label: 'Getting Started', items: [ 'getting-started/installation', - 'getting-started/download-assets', - 'getting-started/quick-start', ], }, { @@ -18,10 +16,7 @@ const sidebars: SidebarsConfig = { items: [ 'tutorials/offline-sim2sim', 'tutorials/pico-sim2sim', - 'tutorials/standalone-standing', 'tutorials/pico-sim2real', - 'tutorials/bvh-sim2real', - 'tutorials/high-level-policy-sim2real', 'tutorials/training', ], }, From 72fea17b8620210d2a30e85c83b767a94715f8fe Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Wed, 29 Jul 2026 19:39:44 +0800 Subject: [PATCH 47/59] docs: refresh Pico deployment guides and diagrams --- docs/docs/intro.md | 9 +- docs/docs/reference/architecture.md | 5 +- docs/docs/tutorials/offline-sim2sim.md | 10 - docs/docs/tutorials/pico-sim2real.md | 213 ++++++++---------- docs/docs/tutorials/pico-sim2sim.md | 56 +++-- docs/docs/tutorials/training.md | 5 +- .../current/intro.md | 7 +- .../current/reference/architecture.md | 5 +- .../current/tutorials/offline-sim2sim.md | 10 - .../current/tutorials/pico-sim2real.md | 194 +++++++--------- .../current/tutorials/pico-sim2sim.md | 48 ++-- .../current/tutorials/training.md | 5 +- .../img/diagrams/pico-g1-state-machine-zh.svg | 88 ++++++++ .../img/diagrams/pico-g1-state-machine.svg | 88 ++++++++ .../diagrams/pico-sim-state-machine-zh.svg | 74 ++++++ .../img/diagrams/pico-sim-state-machine.svg | 74 ++++++ 16 files changed, 575 insertions(+), 316 deletions(-) create mode 100644 docs/static/img/diagrams/pico-g1-state-machine-zh.svg create mode 100644 docs/static/img/diagrams/pico-g1-state-machine.svg create mode 100644 docs/static/img/diagrams/pico-sim-state-machine-zh.svg create mode 100644 docs/static/img/diagrams/pico-sim-state-machine.svg diff --git a/docs/docs/intro.md b/docs/docs/intro.md index 4062f8f0..9872e722 100644 --- a/docs/docs/intro.md +++ b/docs/docs/intro.md @@ -7,10 +7,11 @@ slug: / > **Looking for Chinese docs?** [中文文档点此进入](https://BotRunner64.github.io/Teleopit/zh-Hans/) -Teleopit is a **full-embodiment teleoperation system for the Unitree G1**. -With a Pico 4 or Pico 4 Ultra, an operator can drive the robot's whole-body -motion in real time. Optional LinkerHand hands reproduce hand gestures, and an -optional OpenNeck gimbal turns head motion into active camera control. +Teleopit is a **full-embodiment humanoid teleoperation system for the Unitree +G1**. With a supported Pico headset, an operator can drive the robot's +whole-body motion in real time. In onboard deployments, optional LinkerHand +hands reproduce hand gestures, and an optional OpenNeck gimbal turns head +motion into active camera control. The same motion controller runs in MuJoCo first, so you can check tracking and controls before connecting a physical robot. diff --git a/docs/docs/reference/architecture.md b/docs/docs/reference/architecture.md index d97b95f3..eb4cbaf1 100644 --- a/docs/docs/reference/architecture.md +++ b/docs/docs/reference/architecture.md @@ -35,8 +35,9 @@ Pico HMD rotation + same-frame Spine3 rotation ``` Whole-body control is the required path. Hands and OpenNeck are optional -process-isolated workers; their failure must not stop G1 body control. All -three paths reuse the same in-process PicoBridge receiver. +process-isolated workers for onboard deployment; their failure must not stop G1 +body control. External-host Pico deployment supports the whole-body path only. +All active paths reuse the same in-process PicoBridge receiver. Host-served imitation policies use a second, independent deployment path: diff --git a/docs/docs/tutorials/offline-sim2sim.md b/docs/docs/tutorials/offline-sim2sim.md index 26748c74..1c73d2a5 100644 --- a/docs/docs/tutorials/offline-sim2sim.md +++ b/docs/docs/tutorials/offline-sim2sim.md @@ -128,13 +128,3 @@ realtime=true ``` For every available field, see [Configuration](../configuration/overview). - -## Common Problems - -| Symptom | Check | -|---------|-------| -| Policy path error | Confirm that `track.onnx` exists or pass the path to your exported ONNX | -| Missing XML, mesh or GMR config | Download `robots` and `gmr` assets | -| Motion is rotated or distorted from the first view | Confirm `input.bvh_format` matches the file's skeleton | -| Robot falls only in `sim2sim` | Confirm the ONNX was exported for the current 167D `velcmd_history` observation | -| EGL/OpenGL error | Try the interactive viewer on a desktop, or configure EGL before headless rendering | diff --git a/docs/docs/tutorials/pico-sim2real.md b/docs/docs/tutorials/pico-sim2real.md index 76fa3997..be6f3238 100644 --- a/docs/docs/tutorials/pico-sim2real.md +++ b/docs/docs/tutorials/pico-sim2real.md @@ -4,52 +4,57 @@ sidebar_position: 3 # VR Teleoperation on Unitree G1 -This guide moves the Pico workflow from MuJoCo to a physical Unitree G1. The -motion input is the same; the important new pieces are the G1 network, the DDS -bridge and safe operator transitions. +This guide moves the Pico workflow from MuJoCo to a physical Unitree G1. First +choose where Teleopit will run, then verify standing control before handing the +robot over to live body tracking. :::danger Keep the Unitree remote in your hand -Use `L1+R1` to enter `DAMPING` whenever motion is unexpected. Start with clear -space around the robot and an operator ready to support or stop it. +Use `L1+R1` to enter `DAMPING` whenever motion is unexpected. Keep clear space +around the robot and have another person ready to support or stop it. ::: -## Before You Start - -Do not continue until all of these are true: - -- [VR Teleoperation in Simulation](pico-sim2sim) works reliably. -- You installed the `pico4` profile and built `g1_bridge_sdk` as described in - [Installation](../getting-started/installation). -- `track.onnx`, robot assets and GMR assets are present. -- The computer running Teleopit has a wired DDS connection to the G1. -- No other program is commanding the robot. +## Choose a Deployment -Teleopit may run on an external PC connected to G1 by Ethernet or on the G1 -onboard computer. Pico still connects directly to the machine running Teleopit. +### External host: whole-body tracking only -## 1. Find the G1 Network Interface +Run Teleopit on a workstation or laptop connected to G1 by Ethernet. The Pico +headset must be able to reach this computer over the network. -List the Linux interfaces: +This deployment is for G1 whole-body control only. Keep LinkerHand, OpenNeck, +RealSense preview and recording disabled. Find the wired interface connected +to G1: ```bash -ip -br link +ifconfig ``` -For a wired PC, use the Ethernet interface connected to G1, such as -`enp130s0`. On the onboard computer, it is usually `eth0`. +Use that interface name in the commands below. The examples use `enp130s0`. -The value is passed as: +### Onboard computer: full embodiment -```text -real_robot.network_interface=enp130s0 -``` +Run Teleopit directly on the G1 onboard computer when you also need LinkerHand, +OpenNeck, RealSense preview or data collection. The Pico headset must be able to +reach the onboard computer. -This interface is for Unitree DDS. If Pico discovery selects the wrong Wi-Fi or -Ethernet address, set `input.bridge_advertise_ip` separately. +The G1 DDS interface is `eth0` by default. Apart from the network interface and +the optional onboard hardware settings, the body-control configuration and +launch command are the same as for an external host. -## 2. Check Standing Control First +## Before You Start -Run the same standing controller used by sim2real before adding Pico: +Do not continue until all of these are true: + +- [VR Teleoperation in Simulation](pico-sim2sim) works reliably. +- You installed the `pico4` profile and built `g1_bridge_sdk` as described in + [Installation](../getting-started/installation). +- `track.onnx`, the robot files and GMR assets are present. +- The machine running Teleopit has a wired DDS connection to G1. +- No other program is commanding the robot. + +## 1. Check Standing Control + +Check state reception and policy timing without sending motor commands. On an +external host, replace `enp130s0` with the interface reported by `ifconfig`: ```bash python scripts/run/standalone_standing.py \ @@ -58,8 +63,10 @@ python scripts/run/standalone_standing.py \ --dry-run ``` -The dry run checks state reception and policy timing without sending motor -commands. Then repeat without `--dry-run` in a safe hardware setup: +On the onboard computer, use `--network-interface eth0`. + +If the dry run succeeds, repeat the command without `--dry-run` in a safe +hardware setup: ```bash python scripts/run/standalone_standing.py \ @@ -67,13 +74,12 @@ python scripts/run/standalone_standing.py \ --network-interface enp130s0 ``` -If this fails, stop here and use the -[Standalone Standing reference guide](standalone-standing). Pico cannot fix a -G1 bridge or policy problem. +Stop here if standing control is not stable. Follow the +[Standalone Standing Test](standalone-standing) before adding Pico input. -## 3. Start Pico Sim2Real +## 2. Start Pico Sim2Real -Wired PC example: +External-host example: ```bash python scripts/run/run_sim2real.py \ @@ -82,7 +88,7 @@ python scripts/run/run_sim2real.py \ real_robot.network_interface=enp130s0 ``` -Onboard example: +Onboard-computer example: ```bash python scripts/run/run_sim2real.py \ @@ -91,57 +97,51 @@ python scripts/run/run_sim2real.py \ real_robot.network_interface=eth0 ``` -Starting the process does not immediately hand control to Pico. +Starting the program does not immediately give Pico control of the robot. -## 4. Hand Over Control Deliberately +## 3. Use the G1 State Machine -1. Press remote `Start` to enter `STANDING`. -2. Wait until the robot is stable and Pico tracking is valid. -3. Stand in a neutral pose with room to move. -4. Press remote `Y` to enter `MOCAP`. -5. Begin with small, slow movements. -6. Press remote `X` when you want to return to `STANDING`. +![Pico G1 control state machine](/img/diagrams/pico-g1-state-machine.svg) -| Control | Action | -|---------|--------| -| Unitree remote `Start` | Enter `STANDING` | -| Unitree remote `Y` | Start whole-body VR control (`MOCAP`) | -| Unitree remote `B` | Pause or resume the current mocap session | -| Pico/controller `A` | Pause or resume the current mocap session | -| Pico/controller `B` | Switch between whole-body `MOCAP` and arm-only `ARMS` | -| Unitree remote `X` | End VR control and return to `STANDING` | -| Unitree remote `L1+R1` | Emergency stop (`DAMPING`) | +Labels beginning with **G1 remote** refer to the Unitree remote. Labels +beginning with **Pico controller** refer to the VR controllers. The computer +keyboard does not switch robot modes. -Teleopit checks several consecutive Pico frames before entering `MOCAP`. If the -check fails, the robot remains in `STANDING`. +Press **G1 remote** `Start` to enter `STANDING`. Wait until the robot is stable, +stand in a neutral pose and make sure Pico tracking is valid. Then press +**G1 remote** `Y` to enter `MOCAP`, and begin with small, slow movements. Press +**G1 remote** `X` when you want to end the VR session and return to `STANDING`. -### Pause and Resume +`MOCAP` follows the whole body. `ARMS` keeps the body, waist and legs in the +standing pose while both arms continue to follow. `PAUSED` holds the current +reference; resuming returns to the previous `MOCAP` or `ARMS` state. -Pause holds the current reference pose; it does not return the robot to -`STANDING`. Resume rebuilds the live alignment from the current operator pose. -Resume while standing still and close to the held pose. Use remote `X` instead -when you want to end the VR session. +Teleopit checks several consecutive Pico frames before entering `MOCAP`. If +that check fails, the robot stays in `STANDING`. -### What Happens if Pico or Video Fails? +:::tip Pause and resume +G1 remote `B` or Pico controller `A` pauses and resumes the current session. +Resume while standing still and close to the held pose. Use G1 remote `X` +instead when you want to end the session. +::: -Pico input and camera preview are non-critical workers. If Pico input stops, the -G1 control loop keeps the last safe command and the Unitree remote remains -available. A RealSense timeout disables or reconnects video without stopping -body control. Use remote `X` or `L1+R1`; do not wait for an automatic mode -change. +If Pico input stops, body control holds the last reference and the G1 remote +remains available. Use `X` to return to `STANDING`, or `L1+R1` to enter +`DAMPING`; do not wait for an automatic mode change. -## Optional: LinkerHand Control +## Onboard Only: LinkerHand -Skip this section unless LinkerHand hardware is connected. Install the local -hand packages from [Installation](../getting-started/installation), then bring -up both CAN interfaces: +Skip this section unless LinkerHand hardware is connected to the onboard +computer. Install the hand packages from +[Installation](../getting-started/installation), then bring up both CAN +interfaces: ```bash sudo /usr/sbin/ip link set can0 up type can bitrate 1000000 sudo /usr/sbin/ip link set can1 up type can bitrate 1000000 ``` -Test the hands before starting the robot runtime: +Test both hands before starting G1 control: ```bash python scripts/dev/test_linkerhand.py \ @@ -151,7 +151,7 @@ python scripts/dev/test_linkerhand.py \ --right-can can1 ``` -Enable O6 hand-pose control by adding: +Enable O6 hand-pose control by adding these overrides to the sim2real command: ```text hands.enabled=true @@ -161,31 +161,29 @@ hands.linkerhand_o6.left_can=can0 hands.linkerhand_o6.right_can=can1 ``` -Use `hands.mode=gripper` for trigger-based open/close control. -`linkerhand_l6` is also supported; use the matching -`hands.linkerhand_l6.*` CAN keys. Hand control remains active in all robot -modes, and runtime failure opens the hands. +Use `hands.mode=gripper` for trigger-based open and close. LinkerHand L6 is also +supported through the matching `hands.linkerhand_l6.*` settings. -## Optional: OpenNeck Active Vision +## Onboard Only: OpenNeck -Skip this section unless OpenNeck is installed and calibrated: +Install and calibrate OpenNeck: ```bash pip install -e '.[openneck]' openneck calibrate ``` -Enable it in the main command: +Then add these overrides to the sim2real command: ```text neck.enabled=true neck.port=/dev/ttyACM0 ``` -OpenNeck follows the Pico HMD relative to the operator's upper body. It uses the -same Pico receiver as body control and does not start another PicoBridge. +OpenNeck follows the Pico HMD relative to the operator's upper body. It reuses +the existing Pico receiver. -## Optional: RealSense Preview in the Headset +## Onboard Only: RealSense Preview Install `pyrealsense2`, then add: @@ -194,32 +192,27 @@ input.video.enabled=true input.video.device= ``` -RealSense reconnects in the background after a timeout. Camera failure does not -stop Pico tracking or G1 control. +The camera view is sent to the headset. A timeout restarts the camera in the +background without stopping Pico tracking or G1 control. -## Optional: Record and Review Episodes +## Onboard Only: Record and Review Data -Recording requires the `recording` profile and a fresh RealSense RGB frame: +Recording requires a fresh RealSense RGB frame: ```bash python scripts/run/run_sim2real.py \ --config-name sim2real_record \ controller.policy_path=track.onnx \ - real_robot.network_interface=enp130s0 \ + real_robot.network_interface=eth0 \ recording.task="walk forward" ``` -| Terminal key | Action | -|--------------|--------| -| `R` | Start an episode | -| `S` | Save the active episode | -| `D` | Discard the active episode | -| `Q` | Shut down | +Use terminal `R` to start an episode, `S` to save it, `D` to discard it and +`Q` to shut down. If no fresh camera frame arrives for one second, the active +episode is discarded while robot control continues. Start a new episode +manually after video recovers. -If fresh video is missing for one second, the active episode is discarded while -robot control continues. Start a new episode manually after video recovers. - -Review saved data with: +Review the saved recording with: ```bash pip install -e '.[review]' @@ -227,28 +220,18 @@ python scripts/view/view_recording.py \ --recording data/recordings/sim2real_hdf5 ``` -The reviewer synchronizes camera video, observed/reference G1 poses and optional -hand/neck signals. The recording layout and field definitions are documented -in [Dataset Reference](../reference/dataset). +The viewer synchronizes camera video, measured and reference G1 poses, and +optional hand and neck signals. See [Dataset Reference](../reference/dataset) +for the stored fields. ## Common Problems -| Symptom | What to do | -|---------|------------| -| No `LowState` arrives | Check the Ethernet cable and `real_robot.network_interface` | -| `g1_bridge_sdk` cannot import | Re-run `scripts/setup/setup_g1_bridge.sh` in the active environment | -| `Start` cannot enter standing control | Stop other Unitree modes and programs, then try again | -| `Y` does not enter `MOCAP` | Keep Pico tracking visible and stable; inspect mocap validation logs | -| Pausing does not return to standing | This is expected; use remote `X` | -| Pico cannot discover Teleopit | Set `input.bridge_advertise_ip` to an address reachable from the headset | -| LinkerHand does not move | Check `hands.enabled`, driver/mode, CAN state and the standalone hand test | -| RealSense is unavailable on Arm | Install `pyrealsense2` from conda-forge | +| Problem | Solution | +|---------|----------| +| RealSense does not work on Arm | Remove the PyPI wheel with `pip uninstall pyrealsense2`, then install the conda-forge Arm build with `conda install -c conda-forge pyrealsense2` | ## Other G1 Workflows -The main tutorial path is Pico VR. These focused guides remain available for -less common bring-up and deployment work: - - [Standalone Standing Test](standalone-standing) - [BVH Playback on Unitree G1](bvh-sim2real) - [Host Policy Deployment on Unitree G1](high-level-policy-sim2real) diff --git a/docs/docs/tutorials/pico-sim2sim.md b/docs/docs/tutorials/pico-sim2sim.md index 4c5d093e..be3006b7 100644 --- a/docs/docs/tutorials/pico-sim2sim.md +++ b/docs/docs/tutorials/pico-sim2sim.md @@ -8,11 +8,20 @@ Use Pico tracking to control a simulated G1 before connecting a physical robot. Do not skip this step: it lets you fix headset, network and body-tracking problems without putting hardware at risk. +## Supported Headsets + +- Pico 4 +- Pico 4 Ultra +- Pico 4 Ultra Enterprise +- Pico 4 Pro + +All headsets must have full-body tracking enabled and run a Pico system version +that supports the current body-tracking interface. + ## Before You Start You need: -- a Pico 4 or Pico 4 Ultra with full-body tracking, - the headset and the computer running Teleopit on the same network, - the `pico4` install profile and `robots gmr ckpt bvh` assets, and - a working result from @@ -66,35 +75,29 @@ python scripts/run/run_sim.py \ The robot intentionally starts in `STANDING`; live body tracking does not take control until you ask for it. -## 4. Complete the First VR Session +## 4. Use the Simulation State Machine -1. Stand in a comfortable neutral pose and wait for stable tracking. -2. Press `Y` on the keyboard to enter `MOCAP`. -3. Move slowly at first and confirm that the simulated G1 follows. -4. Press `A` to pause, then press `A` again to resume. -5. Press `X` to return to `STANDING`. +![Pico simulation control state machine](/img/diagrams/pico-sim-state-machine.svg) -| Key | Action | -|-----|--------| -| `Y` | Start whole-body control (`MOCAP`) | -| `A` | Pause or resume the current mocap session | -| `B` | Switch between `MOCAP` and arm-only control (`ARMS`) | -| `X` | Stop VR control and return to `STANDING` | -| `Q` | Quit | +Labels beginning with **Keyboard** refer to the computer keyboard. Labels +beginning with **Pico controller** refer to the VR controllers. The Unitree G1 +remote is not used in simulation. -The modes are simple: +Stand in a comfortable neutral pose and wait for stable tracking before using +**Keyboard** `Y` to enter `MOCAP`. Move slowly at first. Use **Keyboard** `X` to +end the VR session and return to `STANDING`; **Keyboard** `Q` quits the +simulation from any state. -- `STANDING`: the robot waits in its standing controller. -- `MOCAP`: the whole body follows the operator. -- `ARMS`: the body, waist and legs stay in the standing pose while both arms - continue to follow the operator. +`MOCAP` follows the whole body. `ARMS` keeps the body, waist and legs in the +standing pose while both arms continue to follow. `PAUSED` holds the current +reference and returns to the previous `MOCAP` or `ARMS` state when resumed. Each new `STANDING -> MOCAP` session recalibrates the live root pose. You may turn to a new heading while standing, then enter `MOCAP` again. :::tip Pausing is not the same as stopping VR control -`A` freezes and resumes the current mocap pose. Use `X` when you want to end the -session and return to `STANDING`. +Keyboard or Pico controller `A` freezes and resumes the current mocap pose. +Use Keyboard `X` when you want to end the session and return to `STANDING`. ::: ## Choose the Viewer Layout @@ -150,14 +153,9 @@ input.pico4_timeout=30 ## Common Problems -| Symptom | What to do | -|---------|------------| -| `ImportError: pico_bridge` | Install the `pico4` profile again | -| Startup reports an old pico-bridge | Reinstall the profile so version 0.2.1 is used | -| No body frames arrive | Open the headset app, enable full-body tracking and check that UDP port 63901 is reachable | -| Discovery advertises the wrong address | Set `input.bridge_advertise_ip` to the computer address visible from the headset | -| G1 stays still in the viewer | Wait for stable tracking, then press `Y` | -| G1 follows only with its arms | Press `B` to leave `ARMS` and return to `MOCAP` | +| Problem | Solution | +|---------|----------| +| No body frames arrive | Upgrade the Pico headset to the latest available system version, restart it, enable full-body tracking again, and rerun `scripts/dev/test_pico_bridge.py --no-video` | Once this workflow is reliable, continue with [VR Teleoperation on Unitree G1](pico-sim2real). diff --git a/docs/docs/tutorials/training.md b/docs/docs/tutorials/training.md index 1460f873..2fbb3efb 100644 --- a/docs/docs/tutorials/training.md +++ b/docs/docs/tutorials/training.md @@ -159,10 +159,9 @@ Here `--num_envs` is per process, so the total scales with the world size. | Symptom | What to check | |---------|---------------| | Loader says the dataset is minimal | Run `precompute_dataset.py` and use its output directory | -| Out of GPU memory | Lower `--num_envs` | +| Out of GPU memory | Lower `--num_envs` or train with fewer precomputed data shards | | Out of system memory during startup | Train on fewer precomputed shards or add RAM | -| Exported ONNX fails the 167D check | Export with `save_onnx.py` and `--history_length 10` from the current task | -| Benchmark skips clips | The skipped clips are shorter than the configured benchmark duration | +| Training is unexpectedly slow | Check that PyTorch detects CUDA and that the training device is a CUDA GPU | For task internals and model dimensions, see [Architecture](../reference/architecture). For failure-specific guidance, see diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/intro.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/intro.md index 3b20f7fa..c0712d78 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/intro.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/intro.md @@ -5,10 +5,9 @@ slug: / # Teleopit -Teleopit 是一套面向 Unitree G1 的**全身具身遥操作系统**。操作者戴上 -Pico 4 或 Pico 4 Ultra 后，可以实时控制机器人的全身动作；接入可选的 -LinkerHand 后，还能控制手势；接入可选的 OpenNeck 后，头部动作可以直接控制 -机器人相机的朝向。 +Teleopit 是一套面向 Unitree G1 的**全具身人形机器人遥操作系统**。操作者戴上支持的 +Pico 头显后，可以实时控制机器人的全身动作。机载部署还可以接入可选的 LinkerHand +控制手势，并通过可选的 OpenNeck 把头部动作转换为机器人相机朝向。 同一套运控策略会先在 MuJoCo 中运行。你可以先在仿真里确认动作和控制方式，再连接 真实机器人。 diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/architecture.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/architecture.md index cf6ca936..095f69a0 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/architecture.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/architecture.md @@ -33,8 +33,9 @@ Pico 头显旋转 + 同帧 Spine3 旋转 -> 相对 yaw/pitch 映射 -> OpenNeck ``` -全身控制是必需路径，手部和 OpenNeck 是可选的独立进程。它们发生故障时不能停止 G1 -身体控制。三条路径复用同一个进程内 PicoBridge。 +全身控制是必需路径。手部和 OpenNeck 是机载部署中的可选独立进程，它们发生故障时 +不能停止 G1 身体控制；外部主机部署只支持全身控制。所有启用的路径复用同一个进程内 +PicoBridge。 由主机提供服务的模仿策略使用第二条相互独立的部署路径： diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/offline-sim2sim.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/offline-sim2sim.md index 5d947344..baf39bba 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/offline-sim2sim.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/offline-sim2sim.md @@ -124,13 +124,3 @@ realtime=true ``` 完整字段见[配置说明](../configuration/overview)。 - -## 常见问题 - -| 现象 | 检查内容 | -|------|----------| -| 运控模型路径报错 | 确认 `track.onnx` 存在，或传入自己导出的 ONNX 路径 | -| 缺少 XML、mesh 或 GMR 配置 | 下载 `robots` 和 `gmr` 资源组 | -| 从 `mocap` 视图开始动作就旋转或变形 | 确认 `input.bvh_format` 与文件骨架一致 | -| 只有 `sim2sim` 中机器人会摔倒 | 确认 ONNX 使用当前 167D `velcmd_history` 观测导出 | -| EGL/OpenGL 报错 | 在桌面环境使用交互 Viewer，或先配置 EGL 再进行无头渲染 | diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md index 84c3c241..3e309141 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md @@ -4,50 +4,53 @@ sidebar_position: 3 # 用 VR 遥操真实 G1 -本教程把已经跑通的 Pico 仿真遥操迁移到真实 Unitree G1。动作输入没有变化；新增加的 -关键环节是 G1 网络、DDS bridge 和安全的模式切换。 +本教程会把已经在 MuJoCo 中验证过的 Pico 流程迁移到真实 Unitree G1。先确定 Teleopit +运行在哪里，再验证站立运控，最后才把机器人交给实时身体追踪。 :::danger 始终把 Unitree 遥控器拿在手里 -动作异常时立即按 `L1+R1` 进入 `DAMPING`。第一次运行时清空机器人周围空间，并安排 -一名操作者随时扶住或停止机器人。 +动作异常时立即按 `L1+R1` 进入 `DAMPING`。清空机器人周围空间，并安排另一名人员 +随时扶住或停止机器人。 ::: -## 开始之前 - -下面每一项都满足后再继续： - -- [在仿真中进行 VR 遥操](pico-sim2sim)已经稳定运行； -- 已按照[安装说明](../getting-started/installation)安装 `pico4` 依赖并编译 - `g1_bridge_sdk`； -- `track.onnx`、机器人资源和 GMR 资源都已下载； -- 运行 Teleopit 的电脑通过有线网络连接 G1 DDS； -- 没有其他程序正在向机器人发送控制命令。 +## 选择部署方式 -Teleopit 可以运行在通过网线连接 G1 的外部电脑上，也可以运行在机器人 onboard -电脑上。Pico 始终直接连接运行 Teleopit 的那台电脑。 +### 外部主机部署：仅全身追踪 -## 1. 找到 G1 使用的网卡 +Teleopit 运行在工作站或笔记本电脑上，电脑通过网线连接 G1；Pico 头显需要能够通过 +网络访问这台电脑。 -列出 Linux 网卡： +这种部署方式只用于 G1 全身控制，请保持 LinkerHand、OpenNeck、RealSense 画面和 +数据录制关闭。先查看连接 G1 的有线网卡： ```bash -ip -br link +ifconfig ``` -外部电脑使用连接 G1 的有线网卡，例如 `enp130s0`；onboard 电脑通常使用 `eth0`。 +在后面的命令中填写这块网卡的名称。本文以 `enp130s0` 为例。 -运行参数写成： +### 机载电脑部署：完整具身能力 -```text -real_robot.network_interface=enp130s0 -``` +如果还需要 LinkerHand、OpenNeck、RealSense 画面或数据采集，请直接在 G1 机载电脑 +上运行 Teleopit。Pico 头显需要能够访问机载电脑。 -这个网卡只负责 Unitree DDS。如果 Pico 自动发现选择了错误的 Wi-Fi 或网口地址， -需要另外设置 `input.bridge_advertise_ip`。 +G1 DDS 默认使用 `eth0`。除了网络接口和可选机载硬件配置之外，全身控制的配置和启动 +命令与外部主机部署相同。 -## 2. 先单独检查站立运控 +## 开始之前 -在接入 Pico 之前，先运行真机流程使用的同一套站立运控： +请确认以下条件全部满足： + +- [在仿真中进行 VR 遥操](pico-sim2sim)已经稳定运行； +- 已按照[安装](../getting-started/installation)安装 `pico4` 依赖并编译 + `g1_bridge_sdk`； +- 已准备好 `track.onnx`、机器人文件和 GMR 资源； +- 运行 Teleopit 的设备已经通过有线 DDS 网络连接 G1； +- 没有其他程序正在控制机器人。 + +## 1. 检查站立运控 + +先只检查机器人状态接收和策略频率，不发送电机命令。外部主机需要把 `enp130s0` +替换为 `ifconfig` 查到的有线网卡： ```bash python scripts/run/standalone_standing.py \ @@ -56,8 +59,9 @@ python scripts/run/standalone_standing.py \ --dry-run ``` -`--dry-run` 会检查机器人状态接收和运控时序，但不发送电机命令。在安全的硬件环境中 -确认无误后，再去掉 `--dry-run`： +在机载电脑上运行时，使用 `--network-interface eth0`。 + +Dry run 成功后，在确保硬件安全的情况下去掉 `--dry-run` 再运行一次： ```bash python scripts/run/standalone_standing.py \ @@ -65,12 +69,12 @@ python scripts/run/standalone_standing.py \ --network-interface enp130s0 ``` -如果这一步失败，请停在这里并查看[独立站立检查](standalone-standing)。Pico 无法 -解决 G1 bridge 或运控模型本身的问题。 +站立运控不稳定时不要继续接入 Pico，请先按照 +[单独测试站立运控](standalone-standing)排查。 -## 3. 启动 Pico 真机遥操 +## 2. 启动 Pico 真机遥操 -外部电脑示例： +外部主机示例： ```bash python scripts/run/run_sim2real.py \ @@ -79,7 +83,7 @@ python scripts/run/run_sim2real.py \ real_robot.network_interface=enp130s0 ``` -onboard 电脑示例： +机载电脑示例： ```bash python scripts/run/run_sim2real.py \ @@ -88,53 +92,44 @@ python scripts/run/run_sim2real.py \ real_robot.network_interface=eth0 ``` -启动进程并不会立刻让 Pico 接管机器人。 +程序启动后不会立即让 Pico 接管机器人。 -## 4. 主动、逐步地交出控制权 +## 3. 按 G1 状态机操作 -1. 按遥控器 `Start` 进入 `STANDING`。 -2. 等待机器人稳定，同时确认 Pico 追踪有效。 -3. 操作者以中立姿态站好，周围留出足够空间。 -4. 按遥控器 `Y` 进入 `MOCAP`。 -5. 先从小幅慢动作开始。 -6. 需要回到站立时按遥控器 `X`。 +![Pico G1 控制状态机](/img/diagrams/pico-g1-state-machine-zh.svg) -| 控制 | 作用 | -|------|------| -| Unitree 遥控器 `Start` | 进入 `STANDING` | -| Unitree 遥控器 `Y` | 开始全身 VR 控制（`MOCAP`） | -| Unitree 遥控器 `B` | 暂停或恢复当前动捕会话 | -| Pico/controller `A` | 暂停或恢复当前动捕会话 | -| Pico/controller `B` | 在全身 `MOCAP` 和仅手臂 `ARMS` 之间切换 | -| Unitree 遥控器 `X` | 结束 VR 控制并返回 `STANDING` | -| Unitree 遥控器 `L1+R1` | 紧急停止（`DAMPING`） | +图中的 **G1 遥控器**表示 Unitree 遥控器，**Pico 手柄**表示 VR 手柄。电脑键盘不负责 +切换真机状态。 -进入 `MOCAP` 之前，Teleopit 会连续检查多帧 Pico 数据。检查失败时，机器人会继续 -留在 `STANDING`。 +先按 **G1 遥控器** `Start` 进入 `STANDING`。等机器人站稳，以中立姿态站好，并确认 +Pico 追踪有效。然后按 **G1 遥控器** `Y` 进入 `MOCAP`，从缓慢的小幅动作开始。需要 +结束 VR 会话时，按 **G1 遥控器** `X` 返回 `STANDING`。 -### 暂停和恢复 +`MOCAP` 控制全身。`ARMS` 会让身体、腰和腿保持站立，只有双臂继续跟随。 +`PAUSED` 保持当前参考姿态，恢复后回到暂停前的 `MOCAP` 或 `ARMS`。 -暂停会保持当前参考姿态，不会让机器人回到 `STANDING`。恢复时，系统会根据操作者 -当前姿态重新建立实时对齐。恢复前请站稳，并尽量保持在暂停姿态附近。需要结束 VR -会话时使用遥控器 `X`。 +进入 `MOCAP` 前，Teleopit 会连续检查多帧 Pico 数据。检查没有通过时，机器人会继续 +停留在 `STANDING`。 -### Pico 或视频中断时会怎样？ +:::tip 暂停和恢复 +G1 遥控器 `B` 或 Pico 手柄 `A` 会暂停、恢复当前会话。恢复时请保持静止，并尽量接近 +暂停时的姿态。需要结束会话时，请使用 G1 遥控器 `X`。 +::: -Pico 输入和视频预览都不是关键控制进程。Pico 输入停止后，G1 控制循环会继续保持 -最后一个安全命令，Unitree 遥控器仍然可用。RealSense 超时只会关闭或重连视频， -不会停止身体控制。此时请主动按遥控器 `X` 或 `L1+R1`，不要等待系统自动切换模式。 +如果 Pico 输入中断，全身运控会保持最后一个参考，G1 遥控器仍然可用。按 `X` 返回 +`STANDING`，或按 `L1+R1` 进入 `DAMPING`；不要等待系统自动切换状态。 -## 可选：LinkerHand 控制 +## 仅机载：LinkerHand -没有连接 LinkerHand 时请跳过本节。先按照[安装说明](../getting-started/installation) -安装手部依赖，再启动两个 CAN 接口： +只有 LinkerHand 已连接到机载电脑时才需要本节。先按照 +[安装](../getting-started/installation)安装手部依赖，再启用两路 CAN： ```bash sudo /usr/sbin/ip link set can0 up type can bitrate 1000000 sudo /usr/sbin/ip link set can1 up type can bitrate 1000000 ``` -启动机器人之前先单独测试手： +启动 G1 运控前，先单独测试双手： ```bash python scripts/dev/test_linkerhand.py \ @@ -144,7 +139,7 @@ python scripts/dev/test_linkerhand.py \ --right-can can1 ``` -启用 O6 手势追踪时，在主命令后增加： +在真机启动命令后追加以下参数，即可启用 O6 手部姿态控制： ```text hands.enabled=true @@ -154,63 +149,55 @@ hands.linkerhand_o6.left_can=can0 hands.linkerhand_o6.right_can=can1 ``` -使用手柄扳机开合时设置 `hands.mode=gripper`。系统也支持 `linkerhand_l6`，此时使用 -对应的 `hands.linkerhand_l6.*` CAN 参数。手部控制在所有机器人模式下都保持工作； -手部进程出错时会发送张开手的命令。 +使用 `hands.mode=gripper` 可以通过扳机键控制开合。LinkerHand L6 也受支持，对应参数 +为 `hands.linkerhand_l6.*`。 -## 可选：OpenNeck 主动视觉 +## 仅机载：OpenNeck -没有安装和标定 OpenNeck 时请跳过本节： +安装并校准 OpenNeck： ```bash pip install -e '.[openneck]' openneck calibrate ``` -在主命令后增加： +然后在真机启动命令后追加： ```text neck.enabled=true neck.port=/dev/ttyACM0 ``` -OpenNeck 根据 Pico 头显相对操作者上半身的方向转动。它复用身体控制的 Pico 接收器， -不会再启动第二个 PicoBridge。 +OpenNeck 会根据 Pico 头显相对操作者上身的运动转动，并复用已有的 Pico 接收程序。 -## 可选：在头显中预览 RealSense +## 仅机载：RealSense 画面 -安装 `pyrealsense2` 后，在主命令后增加： +安装 `pyrealsense2`，再追加： ```text input.video.enabled=true input.video.device=<可选的-realsense-序列号> ``` -RealSense 超时后会在后台重连。相机失败不会停止 Pico 追踪或 G1 控制。 +相机画面会发送到头显。相机超时后会在后台重连，不会停止 Pico 追踪或 G1 运控。 -## 可选：录制和查看数据 +## 仅机载：录制和查看数据 -录制需要安装 `recording` 依赖，并且 RealSense 能提供新鲜 RGB 帧： +录制前必须能够收到新的 RealSense RGB 帧： ```bash python scripts/run/run_sim2real.py \ --config-name sim2real_record \ controller.policy_path=track.onnx \ - real_robot.network_interface=enp130s0 \ - recording.task="walk forward" + real_robot.network_interface=eth0 \ + recording.task="向前走" ``` -| 终端按键 | 作用 | -|----------|------| -| `R` | 开始一条 episode | -| `S` | 保存当前 episode | -| `D` | 丢弃当前 episode | -| `Q` | 关闭程序 | +在终端按 `R` 开始一个 episode，按 `S` 保存，按 `D` 丢弃，按 `Q` 关闭程序。如果 +一秒内没有收到新的相机帧，当前 episode 会被丢弃，但机器人运控会继续。视频恢复后 +需要手动重新开始录制。 -如果连续一秒没有新鲜视频帧，当前 episode 会被丢弃，但机器人控制会继续。视频恢复后 -需要手动开始新的 episode。 - -查看已保存数据： +查看已保存的数据： ```bash pip install -e '.[review]' @@ -218,26 +205,17 @@ python scripts/view/view_recording.py \ --recording data/recordings/sim2real_hdf5 ``` -查看器会同步显示相机视频、G1 实测/参考姿态和可选的手部/颈部信号。录制目录和字段 -定义见[数据集参考](../reference/dataset)。 +查看器会同步显示相机视频、G1 实测与参考姿态，以及可选的手部和头部信号。字段说明见 +[数据集参考](../reference/dataset)。 ## 常见问题 -| 现象 | 处理方法 | +| 问题 | 解决方法 | |------|----------| -| 收不到 `LowState` | 检查网线和 `real_robot.network_interface` | -| 无法导入 `g1_bridge_sdk` | 在当前环境重新运行 `scripts/setup/setup_g1_bridge.sh` | -| 按 `Start` 无法进入站立运控 | 停止其他 Unitree 模式和控制程序后重试 | -| 按 `Y` 无法进入 `MOCAP` | 保持 Pico 追踪有效且稳定，检查动捕验证日志 | -| 暂停后没有回到站立 | 这是正常行为；请使用遥控器 `X` | -| Pico 找不到 Teleopit | 把 `input.bridge_advertise_ip` 设为头显能访问的地址 | -| LinkerHand 不动 | 检查 `hands.enabled`、driver/mode、CAN 状态和独立手部测试 | -| Arm 设备上 RealSense 不可用 | 从 conda-forge 安装 `pyrealsense2` | - -## 其他 G1 运行方式 +| Arm 设备上 RealSense 无法使用 | 先运行 `pip uninstall pyrealsense2` 删除 PyPI wheel，再运行 `conda install -c conda-forge pyrealsense2` 安装 conda-forge 提供的 Arm 构建 | -主线教程以 Pico VR 为主。以下页面保留给较少使用的硬件检查和部署场景： +## 其他 G1 工作流 -- [独立站立检查](standalone-standing) +- [单独测试站立运控](standalone-standing) - [在 Unitree G1 上回放 BVH](bvh-sim2real) -- [在 Unitree G1 上部署 Host Policy](high-level-policy-sim2real) +- [在 Unitree G1 上部署主机策略](high-level-policy-sim2real) diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2sim.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2sim.md index f9940d1a..cbc5de46 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2sim.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2sim.md @@ -7,11 +7,19 @@ sidebar_position: 2 连接真实机器人之前，先用 Pico 控制仿真 G1。不要跳过这一步：头显、网络和身体追踪 问题都可以在这里解决，不会给硬件带来风险。 +## 支持的头显 + +- Pico 4 +- Pico 4 Ultra +- Pico 4 Ultra Enterprise +- Pico 4 Pro + +所有设备都需要开启全身追踪，并使用支持当前身体追踪接口的 Pico 系统版本。 + ## 开始之前 你需要： -- 支持全身追踪的 Pico 4 或 Pico 4 Ultra； - 头显和运行 Teleopit 的电脑处于同一网络； - 已安装 `pico4` 依赖并下载 `robots gmr ckpt bvh` 资源； - [在仿真中运行运控](offline-sim2sim)已经正常。 @@ -60,33 +68,26 @@ python scripts/run/run_sim.py \ 机器人会有意从 `STANDING` 开始；只有操作者主动切换后，实时身体追踪才会接管。 -## 4. 完成第一次 VR 遥操 +## 4. 按状态机操作 -1. 以舒适的中立姿态站好，等待追踪稳定。 -2. 在键盘上按 `Y` 进入 `MOCAP`。 -3. 先做小幅慢动作，确认仿真 G1 正常跟随。 -4. 按 `A` 暂停，再按一次 `A` 恢复。 -5. 按 `X` 返回 `STANDING`。 +![Pico 仿真控制状态机](/img/diagrams/pico-sim-state-machine-zh.svg) -| 按键 | 作用 | -|------|------| -| `Y` | 开始全身控制（`MOCAP`） | -| `A` | 暂停或恢复当前动捕会话 | -| `B` | 在全身控制 `MOCAP` 和仅手臂控制 `ARMS` 之间切换 | -| `X` | 结束 VR 控制并返回 `STANDING` | -| `Q` | 退出 | +图中的**键盘**表示运行 Teleopit 的电脑键盘，**Pico 手柄**表示 VR 手柄。仿真过程 +不使用 Unitree G1 遥控器。 -三个模式可以简单理解为： +以舒适的中立姿态站好，等待追踪稳定，再按**键盘** `Y` 进入 `MOCAP`。先从小幅慢动作 +开始。按**键盘** `X` 结束 VR 会话并返回 `STANDING`；在任意状态按**键盘** `Q` +退出仿真。 -- `STANDING`：机器人在站立运控中等待； -- `MOCAP`：机器人全身跟随操作者； -- `ARMS`：身体、腰和腿保持站立，只有双臂继续跟随。 +`MOCAP` 控制全身。`ARMS` 会让身体、腰和腿保持站立，只有双臂继续跟随。 +`PAUSED` 保持当前参考姿态，恢复后返回之前的 `MOCAP` 或 `ARMS`。 每次重新从 `STANDING` 进入 `MOCAP` 时，系统都会重新对齐实时根部姿态。操作者可以 在站立状态改变朝向，再重新进入 `MOCAP`。 :::tip 暂停不等于结束 VR 控制 -`A` 只是冻结并恢复当前动捕姿态。需要结束会话并回到站立时，请按 `X`。 +键盘或 Pico 手柄 `A` 只会冻结并恢复当前动捕姿态。需要结束会话并回到站立时， +请按键盘 `X`。 ::: ## 选择 Viewer 布局 @@ -140,13 +141,8 @@ input.pico4_timeout=30 ## 常见问题 -| 现象 | 处理方法 | +| 问题 | 解决方法 | |------|----------| -| `ImportError: pico_bridge` | 重新安装 `pico4` 依赖 | -| 启动时提示 pico-bridge 版本过旧 | 重新安装依赖，确保使用 0.2.1 | -| 收不到身体帧 | 打开头显应用、启用全身追踪，并确认 UDP 63901 端口可达 | -| 自动发现广播了错误地址 | 把 `input.bridge_advertise_ip` 设为头显能访问的电脑地址 | -| Viewer 中 G1 不动 | 等待追踪稳定后按 `Y` | -| G1 只有手臂跟随 | 按 `B` 离开 `ARMS`，回到 `MOCAP` | +| 收不到身体帧 | 把 Pico 头显升级到最新可用的系统版本，重启头显，重新开启全身追踪，然后再次运行 `scripts/dev/test_pico_bridge.py --no-video` | 这条流程稳定后，再继续[用 VR 遥操真实 G1](pico-sim2real)。 diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/training.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/training.md index e9bbda30..bc56109b 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/training.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/training.md @@ -153,10 +153,9 @@ torchrun \ | 现象 | 检查内容 | |------|----------| | Loader 提示数据集是 minimal 格式 | 运行 `precompute_dataset.py`，并使用它的输出目录 | -| 显存不足 | 降低 `--num_envs` | +| 显存不足 | 降低 `--num_envs`，或使用更少的预计算数据 shard | | 启动加载时内存不足 | 减少参与训练的 precomputed shard，或增加内存 | -| 导出的 ONNX 无法通过 167D 检查 | 使用当前版本的 `save_onnx.py` 和 `--history_length 10` 重新导出 | -| Benchmark 跳过部分 clip | 被跳过的 clip 比配置的评测时长更短 | +| 训练速度异常缓慢 | 检查 PyTorch 是否识别 CUDA，并确认训练设备实际使用 CUDA GPU | 任务内部结构和模型维度见[系统架构](../reference/architecture)，具体训练故障见 [训练问题排查](../reference/training-troubleshooting)。 diff --git a/docs/static/img/diagrams/pico-g1-state-machine-zh.svg b/docs/static/img/diagrams/pico-g1-state-machine-zh.svg new file mode 100644 index 00000000..a7dcc23f --- /dev/null +++ b/docs/static/img/diagrams/pico-g1-state-machine-zh.svg @@ -0,0 +1,88 @@ + + Pico 遥操 Unitree G1 状态机 + G1 遥控器进入站立并开始或结束全身追踪，Pico 手柄切换仅手臂模式或暂停，任意状态按 G1 遥控器 L1 加 R1 进入阻尼。 + + + + + + + + + + + + 真实 G1 控制 + + + G1 遥控器 + + Pico 手柄 + + + 实时 VR 会话 + + + + DAMPING + 电机阻尼 + + + + STANDING + 机器人站立 · 追踪未接管 + + + + MOCAP + 全身追踪 + + + + ARMS + 仅双臂追踪 + + + + PAUSED + 保持当前参考姿态 + + + + Start + + + + 遥控器 Y + + + + 遥控器 X · 结束 + + + + Pico 手柄 B + + + + + + + 遥控器 B · Pico 手柄 A + + + + 任意状态 · 遥控器 L1+R1 → DAMPING + diff --git a/docs/static/img/diagrams/pico-g1-state-machine.svg b/docs/static/img/diagrams/pico-g1-state-machine.svg new file mode 100644 index 00000000..bffecdcb --- /dev/null +++ b/docs/static/img/diagrams/pico-g1-state-machine.svg @@ -0,0 +1,88 @@ + + Pico teleoperation state machine on Unitree G1 + The G1 remote enters standing and starts or ends body tracking. The Pico controller switches arms mode or pauses. G1 remote L1 plus R1 enters damping from any state. + + + + + + + + + + + + Physical G1 control + + + G1 remote + + Pico controller + + + LIVE VR SESSION + + + + DAMPING + Motors damped + + + + STANDING + Robot standing · tracking idle + + + + MOCAP + Whole-body tracking + + + + ARMS + Arms-only tracking + + + + PAUSED + Hold current reference + + + + Start + + + + G1 remote Y + + + + G1 remote X · end + + + + Pico controller B + + + + + + + G1 remote B · Pico controller A + + + + Any state · G1 remote L1+R1 → DAMPING + diff --git a/docs/static/img/diagrams/pico-sim-state-machine-zh.svg b/docs/static/img/diagrams/pico-sim-state-machine-zh.svg new file mode 100644 index 00000000..81c91bca --- /dev/null +++ b/docs/static/img/diagrams/pico-sim-state-machine-zh.svg @@ -0,0 +1,74 @@ + + Pico 仿真控制状态机 + 键盘 Y 开始实时会话，键盘或 Pico 手柄 B 切换全身与仅手臂控制，键盘或 Pico 手柄 A 暂停，键盘 X 返回站立，键盘 Q 退出。 + + + + + + + + + 仿真控制 + + + 电脑键盘 + + Pico 手柄 + + + 实时 VR 会话 + + + + STANDING + 站立姿态 · 实时追踪未接管 + + + + MOCAP + 全身追踪 + + + + ARMS + 双臂跟随 · 身体保持站立 + + + + PAUSED + 保持当前参考姿态 + + + + 键盘 Y + + + + 键盘 X · 结束 + + + + 键盘 B · Pico B + + + + + + + 键盘 A · Pico A + + + 任意状态按键盘 Q · 退出仿真 + diff --git a/docs/static/img/diagrams/pico-sim-state-machine.svg b/docs/static/img/diagrams/pico-sim-state-machine.svg new file mode 100644 index 00000000..d56a4fe9 --- /dev/null +++ b/docs/static/img/diagrams/pico-sim-state-machine.svg @@ -0,0 +1,74 @@ + + Pico simulation control state machine + Keyboard Y starts a live session, keyboard or Pico controller B switches between whole-body and arms-only control, keyboard or Pico controller A pauses, keyboard X returns to standing, and keyboard Q quits. + + + + + + + + + Simulation control + + + Keyboard + + Pico controller + + + LIVE VR SESSION + + + + STANDING + Standing pose · tracking inactive + + + + MOCAP + Whole-body tracking + + + + ARMS + Live arms · standing body + + + + PAUSED + Hold the current reference + + + + Keyboard Y + + + + Keyboard X · end + + + + Keyboard B · Pico B + + + + + + + Keyboard A · Pico A + + + Keyboard Q · Quit the simulation from any state + From 439a7e52bf258ed648360c575d1d1f90f57308c8 Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Wed, 29 Jul 2026 22:53:02 +0800 Subject: [PATCH 48/59] docs: document selectable robot models --- AGENTS.md | 8 ++-- README.md | 7 ++-- docs/docs/getting-started/installation.md | 4 +- docs/docs/reference/assets.md | 16 ++++++-- .../reference/training-troubleshooting.md | 4 +- docs/docs/tutorials/training.md | 37 ++++++++++++++++--- .../current/getting-started/installation.md | 2 +- .../current/reference/assets.md | 14 +++++-- .../reference/training-troubleshooting.md | 4 +- .../current/tutorials/training.md | 34 ++++++++++++++--- 10 files changed, 102 insertions(+), 28 deletions(-) diff --git a/AGENTS.md b/AGENTS.md index 39a6ba4d..01bf9c84 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -97,7 +97,7 @@ train_mimic/ # Training package ### Sim2Sim Pipeline - Policy runs at 50Hz, PD control at 200Hz (`decimation=4`, `sim_dt=0.005`) - Action flow: `compute_action()` returns raw action → `get_target_dof_pos()` applies clip `[-10, 10]`, scale, and `default_dof_pos` -- Must use `assets/robots/unitree_g1/g1_29dof.xml` for training, sim2sim, dataset FK, and retargeting; it is the canonical G1 XML entry point +- `assets/robots/unitree_g1/g1_29dof.xml` is the default G1 XML, not a model allowlist; training can select another task-compatible model with `--robot_xml`, and each workflow should keep its robot joint/body definitions consistent ### Multi-Viewer Support `SimulationLoop` supports multiple simultaneous viewer windows controlled by the `viewers` config: @@ -277,9 +277,9 @@ python train_mimic/scripts/save_onnx.py --checkpoint logs/rsl_rl/g1_general_trac ### External Assets - Do not commit robot meshes, datasets, checkpoints, or demo media to Git; use `scripts/setup/download_assets.py` -- `assets/robots/unitree_g1/g1_29dof.xml` and its meshes are the canonical G1 robot model assets; they are downloaded from the `robots` asset group and are not tracked in Git +- G1 XML variants and their meshes are downloaded under `assets/robots/unitree_g1/` by the `robots` asset group and are not tracked in Git; `g1_29dof.xml` is the default - `teleopit/retargeting/gmr/assets/` is gitignored; downloaded at runtime -- `train_mimic/assets/` is no longer tracked; FK tooling reuses `assets/robots/unitree_g1/g1_29dof.xml` +- `train_mimic/assets/` is no longer tracked; FK tooling uses the robot assets under `assets/robots/`, with `assets/robots/unitree_g1/g1_29dof.xml` as the default G1 model - `third_party/linkerhand-python-sdk` and `third_party/somehand` support optional LinkerHand sim2real control - Run `python scripts/dev/check_large_tracked_files.py` before pushing @@ -352,4 +352,4 @@ pytest tests/ -v ## Known Issues 1. `lafan1-resolved` retargeting is still broken because it uses a different BVH skeleton layout. -2. Legacy downloaded GMR XMLs under `teleopit/retargeting/gmr/assets/unitree_g1/` are not the project entry point; use `assets/robots/unitree_g1/g1_29dof.xml`. +2. Legacy downloaded GMR XMLs under `teleopit/retargeting/gmr/assets/unitree_g1/` are separate retargeting assets, not replacements for the runtime robot bundle under `assets/robots/unitree_g1/`; `g1_29dof.xml` is the default runtime G1 model. diff --git a/README.md b/README.md index 11e650ba..b14e8631 100644 --- a/README.md +++ b/README.md @@ -35,9 +35,10 @@ pip install modelscope python scripts/setup/download_assets.py --only robots gmr ckpt bvh ``` -The canonical Unitree G1 robot model is downloaded to -`assets/robots/unitree_g1/g1_29dof.xml`. Training, sim2sim, retargeting, and FK -validation all use this same XML. +The default Unitree G1 robot model is downloaded to +`assets/robots/unitree_g1/g1_29dof.xml`, with additional model variants in the +same directory. Training can select a task-compatible XML with `--robot_xml`; +the quick-start command below uses the default model. **3. Run** diff --git a/docs/docs/getting-started/installation.md b/docs/docs/getting-started/installation.md index cdb79900..976da5a9 100644 --- a/docs/docs/getting-started/installation.md +++ b/docs/docs/getting-started/installation.md @@ -89,8 +89,8 @@ python scripts/setup/download_assets.py \ --only robots gmr ckpt bvh ``` -The inference bundle creates `track.onnx`, the canonical G1 model, GMR files and -a sample BVH under their expected project paths. See +The inference bundle creates `track.onnx`, the G1 model files, GMR files and a +sample BVH under their expected project paths. See [Asset Reference](../reference/assets) for the complete inventory and asset group mapping. diff --git a/docs/docs/reference/assets.md b/docs/docs/reference/assets.md index 0972d840..e3c8d8cb 100644 --- a/docs/docs/reference/assets.md +++ b/docs/docs/reference/assets.md @@ -21,13 +21,23 @@ and maintainer reference. | Group | Local result | Used for | |-------|--------------|----------| | `ckpt` | `track.onnx`, `track.pt` | Ready-to-run inference and the matching PyTorch checkpoint | -| `robots` | `assets/robots/unitree_g1/g1_29dof.xml` and meshes | Training, MuJoCo inference, GMR and dataset FK | +| `robots` | Robot XML variants and meshes under `assets/robots/` | Training, MuJoCo inference, GMR and dataset FK | | `gmr` | `teleopit/retargeting/gmr/assets/` | Retargeting models and IK configuration | | `bvh` | `data/sample_bvh/*.bvh` | Sample motions used by the installation check and simulation tutorial | | `data` | `data/datasets//shard_*.h5` | Minimal distributed motion datasets; precompute before training | -The file `assets/robots/unitree_g1/g1_29dof.xml` is the canonical G1 entry -point. XML files inside the GMR asset directory are not replacements for it. +The current G1 robot bundle includes: + +| Model XML | Setup | +|-----------|-------| +| `assets/robots/unitree_g1/g1_29dof.xml` | Base G1 model and the default | +| `assets/robots/unitree_g1/g1_29dof_dex3.xml` | G1 with Dex3 hand geometry and inertial properties | +| `assets/robots/unitree_g1/g1_29dof_avp_o6.xml` | G1 with AVP active vision and O6 hand models | + +The default is not a model allowlist. Training can select another +task-compatible XML with `--robot_xml`. XML files in the GMR asset directory +belong to their retargeting configurations and are separate from the runtime +robot bundle. ## Repositories diff --git a/docs/docs/reference/training-troubleshooting.md b/docs/docs/reference/training-troubleshooting.md index 16665e9b..08d20651 100644 --- a/docs/docs/reference/training-troubleshooting.md +++ b/docs/docs/reference/training-troubleshooting.md @@ -137,6 +137,8 @@ print(cfg.init_state.joint_pos) # Must match g1.yaml default_angles ### Solution -Update `teleopit/configs/robot/g1.yaml` and `assets/robots/unitree_g1/g1_29dof.xml` to match training environment values (default angles, armature, condim). +Update `teleopit/configs/robot/g1.yaml` and the robot XML selected for training +to match the training environment values (default angles, armature, condim). +The default XML is `assets/robots/unitree_g1/g1_29dof.xml`. This fix also affects the sim2real path since `default_angles` is shared by `rl_policy.py` and `observation.py`. diff --git a/docs/docs/tutorials/training.md b/docs/docs/tutorials/training.md index 2fbb3efb..07be82fb 100644 --- a/docs/docs/tutorials/training.md +++ b/docs/docs/tutorials/training.md @@ -43,7 +43,33 @@ an error, not a supported shortcut. For custom BVH, PKL, NPZ or Pico-recorded data, see [Dataset Reference](../reference/dataset). -## 2. Run a Short Smoke Test +## 2. Choose the Robot Model + +Use `--robot_xml` to select the MuJoCo model used by training. If the argument +is omitted, it defaults to: + +```text +assets/robots/unitree_g1/g1_29dof.xml +``` + +The current `robots` asset bundle includes these ready-to-use examples: + +| Model XML | Setup | +|-----------|-------| +| `assets/robots/unitree_g1/g1_29dof.xml` | Base G1 model and the default | +| `assets/robots/unitree_g1/g1_29dof_dex3.xml` | G1 with Dex3 hand geometry and inertial properties | +| `assets/robots/unitree_g1/g1_29dof_avp_o6.xml` | G1 with AVP active vision and O6 hand models | + +This table describes the models shipped in the current asset bundle; it is not +a hard-coded model allowlist. Another XML can be passed when its joint and body +definitions are compatible with the selected task configuration and dataset. + +The full-training command below explicitly selects the base model as a copyable +example. Replace that path with the model you want to train. The other commands +do not repeat this option; playback and benchmark load the robot from the +selected task configuration. + +## 3. Run a Short Smoke Test Before starting a long job, verify that the dataset, simulator and logger work together: @@ -58,10 +84,11 @@ python train_mimic/scripts/train.py \ The test is successful when environments step, losses are reported and a run directory appears under `logs/rsl_rl/g1_general_tracking/`. -## 3. Start a Full Run +## 4. Start a Full Run ```bash python train_mimic/scripts/train.py \ + --robot_xml assets/robots/unitree_g1/g1_29dof.xml \ --num_envs 4096 \ --max_iterations 30000 \ --motion_file data/datasets_precomputed @@ -73,7 +100,7 @@ TensorBoard; choose `--logger wandb` or `--logger swanlab` when required. `--max_iterations` means additional iterations. For example, resuming `model_12000.pt` with `--max_iterations 18000` continues to iteration 30000. -## 4. Watch the Checkpoint in Simulation +## 5. Watch the Checkpoint in Simulation ```bash python train_mimic/scripts/play.py \ @@ -84,7 +111,7 @@ python train_mimic/scripts/play.py \ Playback starts clips from their beginning and removes training noise. Use it to catch an obviously unstable policy before exporting. -## 5. Run the Benchmark +## 6. Run the Benchmark ```bash python train_mimic/scripts/benchmark.py \ @@ -102,7 +129,7 @@ clip. It reports: Results are written as a text summary, JSON, per-clip CSV and per-rollout CSV. -## 6. Export ONNX +## 7. Export ONNX ```bash python train_mimic/scripts/save_onnx.py \ diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/installation.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/installation.md index 901aee3d..425447a0 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/installation.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/installation.md @@ -87,7 +87,7 @@ python scripts/setup/download_assets.py \ --only robots gmr ckpt bvh ``` -推理资源包会把 `track.onnx`、标准 G1 模型、GMR 文件和示例 BVH 放到代码默认查找 +推理资源包会把 `track.onnx`、G1 模型文件、GMR 文件和示例 BVH 放到代码默认查找 的位置。完整文件清单和资源分组见[资源参考](../reference/assets)。 ## 5. 连接真实 G1 前的额外安装 diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/assets.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/assets.md index a6963389..07fa8e6c 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/assets.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/assets.md @@ -20,13 +20,21 @@ Teleopit 的 Git 仓库只保存代码，不保存大型机器人 mesh、运控 | 资源组 | 下载后的路径 | 用途 | |--------|--------------|------| | `ckpt` | `track.onnx`、`track.pt` | 可直接运行的推理模型和对应 PyTorch checkpoint | -| `robots` | `assets/robots/unitree_g1/g1_29dof.xml` 与 mesh | 训练、MuJoCo 推理、GMR 和数据集 FK | +| `robots` | `assets/robots/` 下的机器人 XML 变体与 mesh | 训练、MuJoCo 推理、GMR 和数据集 FK | | `gmr` | `teleopit/retargeting/gmr/assets/` | 动作重定向模型和 IK 配置 | | `bvh` | `data/sample_bvh/*.bvh` | 安装检查和仿真教程使用的示例动作 | | `data` | `data/datasets//shard_*.h5` | 用于分发的精简动作数据；训练前需要预计算 | -`assets/robots/unitree_g1/g1_29dof.xml` 是项目唯一标准的 G1 入口。GMR 资源目录中的 -XML 不能替代它。 +当前 G1 机器人资源包包括： + +| 模型 XML | 配置 | +|----------|------| +| `assets/robots/unitree_g1/g1_29dof.xml` | 基础 G1 模型，也是默认值 | +| `assets/robots/unitree_g1/g1_29dof_dex3.xml` | 带 Dex3 手部几何和惯性参数的 G1 | +| `assets/robots/unitree_g1/g1_29dof_avp_o6.xml` | 带 AVP 主动视觉和 O6 手部模型的 G1 | + +默认值不是模型白名单。训练可以通过 `--robot_xml` 选择其他与任务兼容的 XML。GMR +资源目录中的 XML 属于对应的重定向配置，与运行时机器人资源包是两套不同资源。 ## 远程仓库 diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/training-troubleshooting.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/training-troubleshooting.md index 6e2e72df..acaa71b8 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/training-troubleshooting.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/training-troubleshooting.md @@ -137,6 +137,8 @@ print(cfg.init_state.joint_pos) # 必须与 g1.yaml default_angles 一致 ### 解决方案 -更新 `teleopit/configs/robot/g1.yaml` 和 `assets/robots/unitree_g1/g1_29dof.xml`，使其与训练环境的值一致（default angles、armature、condim）。 +更新 `teleopit/configs/robot/g1.yaml` 和训练时选择的机器人 XML，使其与训练环境的值 +一致（default angles、armature、condim）。默认 XML 是 +`assets/robots/unitree_g1/g1_29dof.xml`。 此修复同时影响 sim2real 路径，因为 `default_angles` 被 `rl_policy.py` 和 `observation.py` 共用。 diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/training.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/training.md index bc56109b..e260ddf5 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/training.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/training.md @@ -41,7 +41,30 @@ python train_mimic/scripts/data/precompute_dataset.py \ 自定义 BVH、PKL、NPZ 或 Pico 录制数据的处理方法见 [数据集参考](../reference/dataset)。 -## 2. 先做短时间冒烟测试 +## 2. 选择机器人模型 + +使用 `--robot_xml` 指定训练所用的 MuJoCo 模型。如果省略该参数，默认使用： + +```text +assets/robots/unitree_g1/g1_29dof.xml +``` + +当前 `robots` 资源包提供了以下可以直接使用的示例： + +| 模型 XML | 配置 | +|----------|------| +| `assets/robots/unitree_g1/g1_29dof.xml` | 基础 G1 模型，也是默认值 | +| `assets/robots/unitree_g1/g1_29dof_dex3.xml` | 带 Dex3 手部几何和惯性参数的 G1 | +| `assets/robots/unitree_g1/g1_29dof_avp_o6.xml` | 带 AVP 主动视觉和 O6 手部模型的 G1 | + +这个表只是当前资源包随附的模型示例，不是写死的模型白名单。只要关节和刚体定义与所选 +训练任务配置及数据集兼容，也可以传入其他模型 XML。 + +下面“开始完整训练”的主命令会显式选择基础模型，便于直接复制。训练其他模型时， +替换 `--robot_xml` 后的路径即可。其他命令不再重复该参数；回放和 benchmark 会从 +所选任务配置中加载机器人。 + +## 3. 先做短时间冒烟测试 开始长时间训练前，先确认数据集、仿真器和日志工具能够一起工作： @@ -55,10 +78,11 @@ python train_mimic/scripts/train.py \ 只要环境能够持续 step、终端输出 loss，并且 `logs/rsl_rl/g1_general_tracking/` 下生成新的运行目录，这项检查就通过了。 -## 3. 开始完整训练 +## 4. 开始完整训练 ```bash python train_mimic/scripts/train.py \ + --robot_xml assets/robots/unitree_g1/g1_29dof.xml \ --num_envs 4096 \ --max_iterations 30000 \ --motion_file data/datasets_precomputed @@ -70,7 +94,7 @@ python train_mimic/scripts/train.py \ `--max_iterations` 表示继续训练多少次。例如从 `model_12000.pt` 恢复并设置 `--max_iterations 18000`，最终会训练到第 30000 次。 -## 4. 在仿真中查看 checkpoint +## 5. 在仿真中查看 checkpoint ```bash python train_mimic/scripts/play.py \ @@ -80,7 +104,7 @@ python train_mimic/scripts/play.py \ 回放会从每段动作开头开始，并关闭训练噪声。导出前先用它排除明显不稳定的模型。 -## 5. 运行 Benchmark +## 6. 运行 Benchmark ```bash python train_mimic/scripts/benchmark.py \ @@ -97,7 +121,7 @@ Benchmark 会对每个长度足够的 clip 执行一次确定性的 10 秒 rollo 结果会保存为文本摘要、JSON、逐 clip CSV 和逐 rollout CSV。 -## 6. 导出 ONNX +## 7. 导出 ONNX ```bash python train_mimic/scripts/save_onnx.py \ From 746baeededf821b7ea1bec473a63687714a54492 Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Wed, 29 Jul 2026 23:51:47 +0800 Subject: [PATCH 49/59] docs: reorganize reference guides and resources --- docs/docs/configuration/faq.md | 29 --- docs/docs/getting-started/installation.md | 8 +- docs/docs/intro.md | 5 +- docs/docs/reference/architecture.md | 229 ++++++++++-------- docs/docs/reference/companion-projects.md | 88 +++++++ .../configuration/fields.md} | 4 +- .../{ => reference}/configuration/overview.md | 2 +- docs/docs/reference/g1-bridge-sdk.md | 59 ----- docs/docs/reference/{ => resources}/assets.md | 6 +- .../motion-datasets.md} | 72 +----- .../resources/teleoperation-datasets.md | 98 ++++++++ .../reference/training-troubleshooting.md | 144 ----------- docs/docs/tutorials/offline-sim2sim.md | 3 +- docs/docs/tutorials/pico-sim2real.md | 3 +- docs/docs/tutorials/training.md | 5 +- docs/docusaurus.config.ts | 2 +- .../current.json | 4 + .../current/configuration/faq.md | 29 --- .../current/getting-started/installation.md | 4 +- .../current/intro.md | 3 +- .../current/reference/architecture.md | 225 +++++++++-------- .../current/reference/companion-projects.md | 77 ++++++ .../configuration/fields.md} | 4 +- .../{ => reference}/configuration/overview.md | 2 +- .../current/reference/g1-bridge-sdk.md | 59 ----- .../reference/{ => resources}/assets.md | 6 +- .../motion-datasets.md} | 65 +---- .../resources/teleoperation-datasets.md | 88 +++++++ .../reference/training-troubleshooting.md | 144 ----------- .../current/tutorials/offline-sim2sim.md | 2 +- .../current/tutorials/pico-sim2real.md | 4 +- .../current/tutorials/training.md | 5 +- .../docusaurus-theme-classic/footer.json | 6 +- docs/sidebars.ts | 33 +-- .../img/diagrams/architecture-pipeline-zh.svg | 106 ++++++++ .../img/diagrams/architecture-pipeline.svg | 106 ++++++++ 36 files changed, 881 insertions(+), 848 deletions(-) delete mode 100644 docs/docs/configuration/faq.md create mode 100644 docs/docs/reference/companion-projects.md rename docs/docs/{configuration/config-reference.md => reference/configuration/fields.md} (99%) rename docs/docs/{ => reference}/configuration/overview.md (97%) delete mode 100644 docs/docs/reference/g1-bridge-sdk.md rename docs/docs/reference/{ => resources}/assets.md (97%) rename docs/docs/reference/{dataset.md => resources/motion-datasets.md} (69%) create mode 100644 docs/docs/reference/resources/teleoperation-datasets.md delete mode 100644 docs/docs/reference/training-troubleshooting.md delete mode 100644 docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/faq.md create mode 100644 docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/companion-projects.md rename docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/{configuration/config-reference.md => reference/configuration/fields.md} (99%) rename docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/{ => reference}/configuration/overview.md (97%) delete mode 100644 docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/g1-bridge-sdk.md rename docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/{ => resources}/assets.md (97%) rename docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/{dataset.md => resources/motion-datasets.md} (70%) create mode 100644 docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/resources/teleoperation-datasets.md delete mode 100644 docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/training-troubleshooting.md create mode 100644 docs/static/img/diagrams/architecture-pipeline-zh.svg create mode 100644 docs/static/img/diagrams/architecture-pipeline.svg diff --git a/docs/docs/configuration/faq.md b/docs/docs/configuration/faq.md deleted file mode 100644 index ccc28537..00000000 --- a/docs/docs/configuration/faq.md +++ /dev/null @@ -1,29 +0,0 @@ ---- -sidebar_position: 3 ---- - -# Configuration FAQ - -## Why does it fail even though I set `policy_path`? - -1. Verify the file exists -2. Confirm the input dimension is `167` with dual inputs (`obs` + `obs_history`) - -## Why must I specify `input.bvh_file` explicitly? - -`input/bvh.yaml` no longer provides machine-specific default paths. Always specify explicitly: - -```bash -python scripts/run/run_sim.py \ - controller.policy_path=policy.onnx \ - input.bvh_file=data/sample_bvh/aiming1_subject1.bvh -``` - -## Why doesn't `viewer=true` work? - -The legacy `viewer` alias has been removed. Use `viewers` (plural): - -```bash -python scripts/run/run_sim.py controller.policy_path=policy.onnx viewers=sim2sim -python scripts/run/run_sim.py controller.policy_path=policy.onnx viewers=none -``` diff --git a/docs/docs/getting-started/installation.md b/docs/docs/getting-started/installation.md index 976da5a9..b6c6bc0d 100644 --- a/docs/docs/getting-started/installation.md +++ b/docs/docs/getting-started/installation.md @@ -91,8 +91,8 @@ python scripts/setup/download_assets.py \ The inference bundle creates `track.onnx`, the G1 model files, GMR files and a sample BVH under their expected project paths. See -[Asset Reference](../reference/assets) for the complete inventory and asset -group mapping. +[Assets](../reference/resources/assets) for the complete inventory +and asset group mapping. ## 5. Additional Setup for a Physical G1 @@ -104,8 +104,8 @@ bash scripts/setup/setup_g1_bridge.sh ``` The bridge is required for both Pico and BVH control on a real G1. See -[G1 Bridge SDK](../reference/g1-bridge-sdk) if the build or robot connection -fails. +[Companion Projects](../reference/companion-projects#g1-bridge-sdk) if the +build or robot connection fails. ## 6. Optional Hardware diff --git a/docs/docs/intro.md b/docs/docs/intro.md index 9872e722..6eb4111b 100644 --- a/docs/docs/intro.md +++ b/docs/docs/intro.md @@ -40,5 +40,6 @@ hand during hardware operation; `L1+R1` is the emergency path to `DAMPING`. The user guides intentionally keep internals out of the main flow. See [Architecture](reference/architecture) for the runtime pipeline and technical -specifications, [Asset Reference](reference/assets) for every downloaded file, -or [Configuration](configuration/overview) for Hydra options. +specifications, [Assets](reference/resources/assets) for every +downloaded file, or +[Configuration](reference/configuration/overview) for Hydra options. diff --git a/docs/docs/reference/architecture.md b/docs/docs/reference/architecture.md index eb4cbaf1..e99cbe70 100644 --- a/docs/docs/reference/architecture.md +++ b/docs/docs/reference/architecture.md @@ -1,135 +1,158 @@ --- -sidebar_position: 1 +sidebar_position: 2 --- # Architecture -This page collects the runtime pipeline, supported boundaries and exact -dimensions that are intentionally omitted from the task-based user guides. +This page defines Teleopit's runtime pipelines, repository layout, supported +technical surface, and public entry points. ## Pipeline -```text -InputProvider (BVH file / Pico4) - -> Retargeter (GMR) - -> ObservationBuilder (167D) - -> Controller (dual-input TemporalCNN ONNX) - -> Robot (MuJoCo sim or Unitree G1) -``` +![Teleopit runtime pipelines](/img/diagrams/architecture-pipeline.svg) -Offline/online inference is assembled by `teleopit/runtime/` and `teleopit/pipeline.py`. The hardware state machine runs through the process-isolated runtime in `teleopit/sim2real/mp/`. Training is provided by `train_mimic/`. +The main tracking path converts BVH or live PICO body motion into a time-aligned +G1 reference. `VelCmdObservationBuilder` combines that reference with robot +state, and the dual-input TemporalCNN ONNX controller produces 29 joint offsets. +The same observation and controller path drives MuJoCo and the real G1. -## Full-Embodiment Pico Path +Pico hand and active-vision paths are optional process-isolated workers. They +reuse the same in-process `PicoBridge` receiver and never add fields to the 167D +tracking-policy observation. A hand or neck failure must not stop G1 body +control. These optional hardware paths are supported by onboard deployment; +external-host Pico deployment supports whole-body control only. -One Pico frame can feed three independent control paths: +Host-policy deployment is independent from the Pico runtime. A separate host +environment receives JPEG RGB and `observation.state(68)`, then returns +canonical `float32[T,50]` action chunks over strict ZeroMQ/msgpack messages. +The onboard validator and scheduler convert the body portion into a 36D +reference for the existing motion tracker; host output never bypasses that +tracker or becomes a direct motor command. -```text -Pico full-body tracking - -> GMR retargeting -> tracking policy -> G1 whole-body joints +The Teleopit and host environments share semantic data and one identical +`hand_calibration.json`, but do not import each other's Python packages. The +current client/server code and protocol tests define the network structure, so +both repositories must change together when that protocol changes. -Pico hand tracking or controller input - -> Teleopit hand adapter -> somehand or gripper mapping -> LinkerHand L6/O6 +## Runtime Boundaries -Pico HMD rotation + same-frame Spine3 rotation - -> relative yaw/pitch mapping -> OpenNeck -``` +- Offline core components communicate through `InProcessBus` without copying + array payloads. +- Sim2real robot control, reference generation, camera, recording, hand, neck, + and host-policy client work are process-isolated where blocking or hardware + failure could disturb the 50 Hz control loop. +- Local sim2real workers use localhost ZeroMQ and shared-memory video rings. +- The external host-policy boundary uses msgpack and non-pickle float32 arrays. +- Shared component contracts are `typing.Protocol` definitions in + `teleopit/interfaces.py`. -Whole-body control is the required path. Hands and OpenNeck are optional -process-isolated workers for onboard deployment; their failure must not stop G1 -body control. External-host Pico deployment supports the whole-body path only. -All active paths reuse the same in-process PicoBridge receiver. - -Host-served imitation policies use a second, independent deployment path: +## Repository Layout ```text -lerobot-teleopit host environment - policy server -> strict ZeroMQ/msgpack messages - | -Teleopit onboard environment - RealSense/state -> non-critical client worker -> validated action scheduler - -> existing 50 Hz motion tracker -> G1 joint-angle targets - -> dedicated LinkerHand O6 and OpenNeck workers +teleopit/ — Core inference and deployment package +├── interfaces.py — Robot, controller, input and retargeting protocols +├── pipeline.py — Thin offline simulation facade +├── runtime/ — Config/path resolution, factories and CLI validation +├── configs/ — Hydra runtime configuration +├── bus/ — In-process zero-copy publish/subscribe +├── inputs/ — BVH, PICO and realtime input adapters +├── retargeting/gmr/ — Self-contained whole-body GMR implementation +├── controllers/ — Observation builder and ONNX policy controller +├── robots/ — MuJoCo robot adapter +├── sim/ — 200 Hz PD / 50 Hz policy simulation loop +├── sim2real/ +│ ├── mp/ — Process supervisor, IPC and robot-control state machine +│ ├── hands/ — Optional LinkerHand drivers and input mapping +│ └── neck/ — Optional OpenNeck mapping and worker +├── high_level_policy/ — Host protocol, frame transforms and action scheduler +└── recording/ — Sim2real dataset schema and recording workers + +train_mimic/ — Training package +├── app.py — Shared train/play/benchmark assembly +├── tasks/tracking/ — General-Tracking-G1 task and TemporalCNN model +├── data/ — Dataset construction and motion loading +└── scripts/ — Training, playback, benchmark and ONNX export + +scripts/ — User-facing runtime and maintenance entry points +├── run/ — Simulation, sim2real and recording commands +├── setup/ — Asset download and hardware setup +├── render/ — Offline video rendering +├── view/ — Recording review +└── dev/ — Validation and calibration utilities + +third_party/ — Optional hardware SDKs and somehand +tests/ — Unit, protocol and integration tests ``` -The host and onboard environments share semantic data and one identical -`hand_calibration.json`; they do not import each other's Python packages. The -current client/server code and protocol tests define the network structure, so -both repositories must change together during active development. Pico -teleoperation and host-policy deployment also have separate run scripts and -process assemblies. - -## Code Structure - -```text -configs / scripts - -> runtime - -> interfaces + pipeline state machines - -> adapters (inputs / retargeting / controller / robot / recording) - -train_mimic/scripts - -> train_mimic/app.py - -> single task registry / env builder / runner cfg - -> mjlab / rsl_rl - -train_mimic/scripts/data - -> train_mimic/data/dataset_builder.py - -> dataset_lib / motion_fk / convert_pkl_to_npz -``` - -## Core Boundaries - -| Module | Role | -|--------|------| -| `teleopit/interfaces.py` | Stable protocols: InputProvider, Retargeter, Controller, Robot, ObservationBuilder | -| `teleopit/runtime/` | Config parsing, path normalization, component assembly, CLI validation | -| `teleopit/pipeline.py` | Lightweight facade for offline sim | -| `teleopit/sim2real/mp/` | Process-isolated sim2real state machine, IPC, and robot-control loop | -| `teleopit/high_level_policy/` | Host-policy protocol, session-local frame transform, validation, and 30-to-50 Hz scheduler | -| `teleopit/controllers/observation.py` | ObservationBuilder | -| `teleopit/controllers/rl_policy.py` | Accepts dual-input ONNX whose observation dimension matches the runtime builder | -| `train_mimic/app.py` | Shared train/play/benchmark assembly | -| `train_mimic/tasks/tracking/config/` | Single task registration (`General-Tracking-G1`) | -| `train_mimic/data/dataset_builder.py` | Sole official dataset construction entry | - ## Technical Specifications -| Spec | Value | -|------|-------| -| Supported robot | Unitree G1, 29 actuated joints | +| Specification | Supported value | +|---------------|-----------------| +| Robot | Unitree G1 with 29 actuated joints | | Simulator | MuJoCo | -| Motion retargeting | GMR (General Motion Retargeting) | +| Whole-body retargeting | GMR (General Motion Retargeting) | | Policy / PD rates | 50 Hz / 200 Hz | | Training task | `General-Tracking-G1` | | Inference observation | `velcmd_history` (167D) | -| ONNX signature | Dual-input `obs` (167D) + `obs_history` | +| ONNX signature | Dual input: `obs` (167D) + `obs_history` | | Policy action | 29D joint offsets from `default_dof_pos` | -| Actor/Critic | TemporalCNN (2048, 1024, 512, 256, 128) | -| Training sampling | Default `rewind`; also supports `uniform`; playback uses `start`; benchmark pins exact clips and disables clip-end resampling | -| Training `window_steps` | `[0]` | -| Data format | Minimal recursive HDF5 shards (`shard_*.h5`) | -| Optional hands | LinkerHand L6 or O6, gripper or Pico hand-pose input | +| Actor / critic | TemporalCNN (2048, 1024, 512, 256, 128) | +| Training sampling | `rewind` by default; `uniform` supported; playback uses `start`; benchmark pins exact clips and disables clip-end resampling | +| Training window | `window_steps=[0]` | +| Distributed motion data | Minimal recursive HDF5 `shard_*.h5` files | +| Optional hands | LinkerHand L6/O6 with gripper or PICO hand-pose input | | Optional active vision | OpenNeck yaw/pitch in physical degrees | | Host-policy observation | JPEG RGB + `observation.state(68)` | -| Host-policy action | `float32[T,50]` canonical reference at 30 Hz | +| Host-policy action | `float32[T,50]`, 30 Hz source horizon, `T` in `[1,50]` | | Host-policy body control | 36D root/joint reference through the existing 50 Hz motion tracker | ## Constraints -- `controller.policy_path` must be explicitly provided and the file must exist -- Offline BVH runs require explicit `input.bvh_file` -- `viewers` is the sole viewer configuration entry -- Observation/ONNX dimension mismatch causes immediate startup error -- sim2real also requires a dual-input ONNX whose observation dimension matches the runtime builder -- Host-policy message-envelope or schema mismatches are rejected while the robot remains in `STANDING` -- Host action chunks are validated and interpolated onboard; the host cannot bypass the motion tracker or send motor commands -- Policy entry remains internal to `STANDING` only while one host session waits for its first valid chunk; that chunk enters `POLICY` directly, with no candidate alignment, entry Kp ramp, or second session/reset, and the 50 Hz limiter starts from the measured robot reference captured at session start -- Temporal root, yaw, and joint-reference discontinuities are accepted at chunk boundaries and inside chunks, then rate-limited at the 50 Hz scheduler output so recorded pause/resume transitions remain usable - -## Public Surface - -**Stable run modes:** offline sim2sim, offline sim2real playback, Pico4 sim2sim, -G1 sim2real, independent host-policy G1 sim2real - -**Stable training entry points:** `train.py`, `play.py`, `benchmark.py`, `save_onnx.py` - -**Stable data entry points:** `build_dataset.py`, `precompute_dataset.py` +- `controller.policy_path` must be explicit and point to an existing file. +- Offline BVH runs require an explicit, existing `input.bvh_file`. +- `viewers` is the only viewer configuration key. +- Observation definitions and ONNX signatures must match exactly; startup fails + instead of padding or trimming data. +- `default_dof_pos` must come from the selected robot's default standing angles. +- Sim2real requires the same dual-input observation contract used in simulation. +- Host message-envelope or schema mismatches are rejected while the robot + remains in `STANDING`. Shape, finiteness, session, sequence, quaternion, + staleness, and safety violations reject the whole action chunk. +- Host actions are validated, scheduled, and rate-limited onboard. The host + cannot bypass the motion tracker or send G1 motor commands. +- Policy entry remains an internal `STANDING` flow while one host session waits + for its first valid chunk. That chunk enters `POLICY` directly, with no + candidate alignment, entry Kp ramp, or second session/reset. The 50 Hz limiter + starts from the measured robot reference captured at session start. +- Temporal root, yaw, and joint-reference discontinuities are accepted at chunk + boundaries and inside chunks, then rate-limited at the 50 Hz scheduler output + so recorded pause/resume transitions remain usable. +- PICO input, RealSense preview, recording, hand, and neck failures are + non-critical; the Unitree remote and robot-control loop remain available. + +## Public Entry Points + +Supported run modes are offline sim2sim, offline sim2real playback, PICO +sim2sim, PICO G1 sim2real, and independent host-policy G1 sim2real. + +Runtime commands: + +- `scripts/run/run_sim.py` — offline BVH and live PICO sim2sim +- `scripts/run/run_sim2real.py` — BVH or PICO G1 sim2real +- `scripts/run/run_high_level_policy_sim2real.py` — independent host-policy G1 deployment +- `scripts/run/record_pico_motion.py` — record retargeted motion clips from PICO +- `scripts/render/render_sim.py` — render mocap, retargeting, and sim2sim videos +- `scripts/view/view_recording.py` — review synchronized sim2real recordings + +Training and data commands: + +- `train_mimic/scripts/train.py`, `play.py`, `benchmark.py`, `save_onnx.py` +- `train_mimic/scripts/data/build_dataset.py` +- `train_mimic/scripts/data/precompute_dataset.py` + +Public Python surfaces: + +- Protocols in `teleopit/interfaces.py` +- `TeleopPipeline` +- `VelCmdObservationBuilder` +- `RLPolicyController` diff --git a/docs/docs/reference/companion-projects.md b/docs/docs/reference/companion-projects.md new file mode 100644 index 00000000..6beb566b --- /dev/null +++ b/docs/docs/reference/companion-projects.md @@ -0,0 +1,88 @@ +--- +sidebar_position: 4 +--- + +# Companion Projects + +Teleopit integrates four focused components for robot communication, hand +retargeting, active vision, and PICO transport. They are kept outside the +`teleopit` Python package so each project can own its hardware protocol and +public API. + +| Component | Source | Function | Use in Teleopit | +|-----------|--------|----------|-----------------| +| G1 Bridge SDK | [Teleopit source tree](https://github.com/BotRunner64/Teleopit/tree/master/third_party/g1_bridge_sdk) | Native C++/pybind11 bridge over Unitree SDK2 and Cyclone DDS | Real-time G1 state, remote input, mode selection, and 200 Hz low-level commands | +| somehand | [GitHub](https://github.com/BotRunner64/somehand) | Dexterous-hand retargeting library | Maps live Pico hand landmarks to LinkerHand L6/O6 targets | +| OpenNeck | [GitHub](https://github.com/BotRunner64/OpenNeck) | Calibrated two-axis neck driver | Converts physical yaw/pitch degrees to safe servo commands | +| PICO Bridge | [GitHub](https://github.com/BotRunner64/pico-bridge) | Headset app and Python receiver for PICO tracking and video | Supplies body, controller, hand, and HMD frames and optionally returns RGB video | + +## G1 Bridge SDK + +G1 Bridge SDK is maintained directly in Teleopit under +`third_party/g1_bridge_sdk`; it is not a separate repository. Its setup script +downloads [Unitree SDK2](https://github.com/unitreerobotics/unitree_sdk2), then +builds and installs the local pybind11 extension: + +```bash +bash scripts/setup/setup_g1_bridge.sh +``` + +All DDS publish/subscribe work runs on native C++ threads. Teleopit's +`UnitreeG1` adapter reads joint state, base orientation, angular velocity, and +wireless-remote input through the bridge, and sends 29-joint position targets +with per-joint PD gains. This is the hardware boundary used by sim2real +teleoperation, the standalone standing check, and host-policy deployment. + +## somehand + +somehand provides configurable human-to-robot hand retargeting. Teleopit pins +the compatible source as the `third_party/somehand` Git submodule and uses its +0.2.0 public `somehand.api` surface. + +In `hands.mode=vr_hand_pose`, Teleopit converts PICO's 26-joint hand state to +21 landmarks, calls somehand for continuous retargeting, and sends the result +to LinkerHand L6 or O6. Teleopit owns the live Pico receiver and the landmark +conversion; it does not start somehand's standalone Pico input path. + +Install the dexterous-hand dependencies with: + +```bash +git submodule update --init --recursive +pip install -e third_party/linkerhand-python-sdk +pip install -e third_party/somehand +``` + +## OpenNeck + +OpenNeck owns serial communication, degree-to-servo-step conversion, and +calibrated mechanical limits for the two-axis active-vision gimbal. Teleopit +supports the OpenNeck 0.2.0 physical-angle API and calls `move_deg()`; removed +normalized control fields are not compatible. + +For Pico teleoperation, Teleopit computes HMD rotation relative to the +same-frame `Body.Spine3` orientation, applies the configured dead zone and +pitch gain, and sends yaw/pitch degrees from a non-critical neck worker. Host +policy deployment sends the validated neck fields from its canonical action. + +```bash +pip install -e '.[openneck]' +openneck calibrate +``` + +## PICO Bridge + +PICO Bridge contains both the headset application and the importable Python PC +receiver. Teleopit supports release 0.2.1, installed by the `pico4` extra: + +```bash +pip install -e '.[pico4]' +``` + +One in-process `PicoBridge` instance supplies full-body, controller, hand, and +independent HMD data to Teleopit. Whole-body retargeting, hand control, and +OpenNeck all reuse that receiver. When video is enabled, Teleopit can also push +MuJoCo or RealSense RGB frames back to the headset through +`push_video_frame()`. + +Download the headset APK from the +[PICO Bridge releases](https://github.com/BotRunner64/pico-bridge/releases). diff --git a/docs/docs/configuration/config-reference.md b/docs/docs/reference/configuration/fields.md similarity index 99% rename from docs/docs/configuration/config-reference.md rename to docs/docs/reference/configuration/fields.md index 5c4ca409..350a650a 100644 --- a/docs/docs/configuration/config-reference.md +++ b/docs/docs/reference/configuration/fields.md @@ -2,9 +2,9 @@ sidebar_position: 2 --- -# Config Reference +# Configuration Fields -Complete reference for all configurable fields. +Complete reference for Teleopit's Hydra configuration fields. ## Top-Level Fields diff --git a/docs/docs/configuration/overview.md b/docs/docs/reference/configuration/overview.md similarity index 97% rename from docs/docs/configuration/overview.md rename to docs/docs/reference/configuration/overview.md index 7a149ca6..722e5345 100644 --- a/docs/docs/configuration/overview.md +++ b/docs/docs/reference/configuration/overview.md @@ -76,4 +76,4 @@ Teleopit does not silently fix misconfigurations: When you encounter a configuration error, look for **which two components have inconsistent definitions**. -For the complete field reference, see [Config Reference](config-reference). +For the complete field reference, see [Configuration Fields](fields). diff --git a/docs/docs/reference/g1-bridge-sdk.md b/docs/docs/reference/g1-bridge-sdk.md deleted file mode 100644 index 2337df3c..00000000 --- a/docs/docs/reference/g1-bridge-sdk.md +++ /dev/null @@ -1,59 +0,0 @@ ---- -sidebar_position: 4 ---- - -# G1 Bridge SDK - -C++ DDS bridge library wrapping unitree_sdk2 with pybind11, providing near-zero latency (< 0.5 ms) access to Unitree G1's real-time communication interface. - -All DDS publish/subscribe runs on native C++ threads. The Python side only calls simple get/set methods. - -## Dependencies - -- CMake >= 3.10 -- GCC >= 9.4 (C++17 support) -- pybind11 >= 2.6 -- Unitree SDK2 (bundled in `third_party/g1_bridge_sdk/thirdparty/unitree_sdk2/`, no manual install needed) -- Cyclone DDS (unitree_sdk2 dependency) - -## Installation - -```bash -bash scripts/setup/setup_g1_bridge.sh -``` - -The script clones `unitree_sdk2`, installs `pybind11`, and builds the C++ bridge automatically. - -## Python API - -```python -import g1_bridge_sdk - -bridge = g1_bridge_sdk.G1Bridge( - network_interface="enp130s0", # PC Ethernet interface connected to G1 - publish_hz=200 # Command publish rate (default 200 Hz) -) -``` - -For wired PC-to-G1 control, run `ifconfig` on the PC and use the interface name for the G1 cable connection. When running onboard on the robot computer, `eth0` is usually the correct interface. - -| Method | Description | -|--------|-------------| -| `wait_for_state(timeout_sec=5.0)` | Block until first LowState frame; returns False on timeout | -| `get_state()` | Returns `(qpos[29], qvel[29], quat[4], ang_vel[3])` numpy arrays | -| `get_state_counter()` | Returns cumulative LowState frame count | -| `get_wireless_remote()` | Returns 40-byte wireless remote data | -| `get_mode_machine()` | Returns current mode_machine value | -| `set_target(target, kp, kd)` | Set target joint positions and PD gains (29 elements each) | -| `lock_joints()` | Lock current joint positions | -| `set_damping()` | Switch to damping mode (for emergency stop) | -| `start_publish()` | Start command publish thread | -| `stop_publish()` | Stop command publish thread | -| `check_mode()` | Query current motion mode, returns `(code, name)` | -| `select_mode(name)` | Switch motion mode (e.g., `"ai"`, `"normal"`) | -| `release_mode()` | Release current mode, enter low-level control | - -## Usage - -- **Pico4 hardware teleoperation**: `scripts/run/run_sim2real.py` -- **Standalone standing test**: `scripts/run/standalone_standing.py` diff --git a/docs/docs/reference/assets.md b/docs/docs/reference/resources/assets.md similarity index 97% rename from docs/docs/reference/assets.md rename to docs/docs/reference/resources/assets.md index e3c8d8cb..affff206 100644 --- a/docs/docs/reference/assets.md +++ b/docs/docs/reference/resources/assets.md @@ -1,11 +1,11 @@ --- -sidebar_position: 2 +sidebar_position: 1 --- -# Asset Reference +# Assets Teleopit's Git repository contains code, not large robot meshes, policies or -motion data. [Installation](../getting-started/installation) shows the shortest +motion data. [Installation](../../getting-started/installation) shows the shortest download command for each user workflow; this page is the complete inventory and maintainer reference. diff --git a/docs/docs/reference/dataset.md b/docs/docs/reference/resources/motion-datasets.md similarity index 69% rename from docs/docs/reference/dataset.md rename to docs/docs/reference/resources/motion-datasets.md index 14bb8e9f..185f6085 100644 --- a/docs/docs/reference/dataset.md +++ b/docs/docs/reference/resources/motion-datasets.md @@ -1,16 +1,13 @@ --- -sidebar_position: 3 +sidebar_position: 2 --- -# Dataset Reference +# Motion Datasets -Teleopit uses two separate dataset families: - -- **motion datasets** provide reference motion for controller training, and -- **sim2real episode recordings** store synchronized robot state, references - and camera video for later review or external policy work. - -They have different schemas and are not interchangeable. +Motion datasets provide reference motion for controller training. The +distributable format and the precomputed training format are separate; training +accepts only the latter. For synchronized robot, reference and camera +recordings, see [Teleoperation Datasets](teleoperation-datasets). ## Download Pre-Built Dataset (Recommended) @@ -179,60 +176,3 @@ python train_mimic/scripts/data/check_motion_npz_fk.py \ ``` Recommended thresholds: `pos_max < 1e-3 m`, `quat_mean < 0.05 rad`, `quat_p95 < 0.10 rad`. - -## Sim2Real Episode Recordings - -The recording runtime writes an editable dataset rather than one self-contained -HDF5 file: - -```text -data/recordings/sim2real_hdf5/ -├── schema.json -├── episodes.jsonl -├── data/ -│ └── episode_000000.h5 -└── videos/ - └── d435i_rgb/ - └── episode_000000.mp4 -``` - -`schema.json` defines the dataset FPS, `robot_type`, `hand_type`, `neck_type` -and every feature's shape, dtype, names and groups. The hardware types must -match the active runtime configuration. - -`episodes.jsonl` is the editable episode manifest. Each line maps one episode -to its HDF5 and MP4 files and stores the task prompt. Task text is not copied -into HDF5 attributes. - -Each HDF5 file contains only frame-aligned arrays: - -| Field | Shape | Meaning | -|-------|-------|---------| -| `frame_index` | scalar | Camera/action frame index | -| `timestamp` | scalar | Monotonic timestamp in seconds | -| `observation.state` | `(68,)` | G1 joint state, base orientation/angular velocity and projected gravity | -| `observation.mode` | scalar | `STANDING`, `MOCAP`, `ARMS` or paused mocap code | -| `action` | `(36,)` | Root pose plus 29-joint reference consumed by the motion tracker | -| `action.hand` | `(12,)`, optional | Left/right LinkerHand target when hand control is enabled | -| `action.neck` | `(2,)`, optional | Mechanically clamped OpenNeck yaw/pitch target in degrees | - -Camera RGB is stored only in the MP4 sidecar; HDF5 does not contain a duplicate -raw image dataset. Optional action fields appear exactly when the matching -hardware is enabled. - -The recorder commits the HDF5/video files before appending the manifest entry. -An interrupted, uncommitted episode is removed on the next recording-worker -startup and does not consume an episode index. An existing incompatible -`schema.json` stops only the non-critical recording worker. - -Review the dataset with: - -```bash -python scripts/view/view_recording.py \ - --recording data/recordings/sim2real_hdf5 -``` - -The reviewer validates manifest paths, HDF5 shapes/dtypes/finite values and MP4 -alignment before playback. Measured root XYZ is not recorded, so its observed -robot view is anchored to the reference root position; global root translation -cannot be evaluated from this format. diff --git a/docs/docs/reference/resources/teleoperation-datasets.md b/docs/docs/reference/resources/teleoperation-datasets.md new file mode 100644 index 00000000..4ea0c373 --- /dev/null +++ b/docs/docs/reference/resources/teleoperation-datasets.md @@ -0,0 +1,98 @@ +--- +sidebar_position: 3 +--- + +# Teleoperation Datasets + +Teleoperation datasets are manually recorded sim2real episodes. They synchronize +the G1 state, the reference consumed by the motion tracker, optional hand and +neck commands, and RealSense RGB video. This format is intended for review and +external policy work; it is not a motion dataset for controller training. + +## Record Episodes + +Recording is available only for onboard Pico sim2real deployment with an +interactive terminal and a fresh RealSense stream: + +```bash +pip install -e '.[recording]' +python scripts/run/run_sim2real.py --config-name sim2real_record \ + controller.policy_path=policy.onnx +``` + +The equivalent manual configuration requires `recording.enabled=true`, +`input.provider=pico4`, `input.video.enabled=true`, and +`input.video.source=realsense`. + +Use `R` to start an episode, `S` to save it, `D` to discard it, and `Q` to shut +down. `STANDING`, `MOCAP`, `ARMS`, and paused mocap are recordable. Recording +does not start without a fresh camera frame. If the camera stays stale for one +second during an episode, that episode is discarded; Pico input and G1 control +continue, and recording does not restart automatically when video recovers. + +## Dataset Layout + +The recording runtime writes an editable dataset rather than one self-contained +HDF5 file: + +```text +data/recordings/sim2real_hdf5/ +├── schema.json +├── episodes.jsonl +├── data/ +│ └── episode_000000.h5 +└── videos/ + └── d435i_rgb/ + └── episode_000000.mp4 +``` + +`schema.json` defines the dataset FPS, `robot_type`, `hand_type`, `neck_type`, +and every feature's shape, dtype, names, and groups. Hardware types must match +the active runtime configuration. + +`episodes.jsonl` is the editable episode manifest. Each line maps one episode +to its HDF5 and MP4 files and stores the task prompt. Task text is not copied +into HDF5 attributes, so it can be edited without rewriting frame data. + +## Frame Fields + +Each HDF5 file contains only frame-aligned arrays: + +| Field | Shape | Meaning | +|-------|-------|---------| +| `frame_index` | scalar | Camera/action frame index | +| `timestamp` | scalar | Monotonic timestamp in seconds | +| `observation.state` | `(68,)` | G1 joint state, base orientation/angular velocity, and projected gravity | +| `observation.mode` | scalar | `STANDING`, `MOCAP`, `ARMS`, or paused mocap code | +| `action` | `(36,)` | Root pose plus 29-joint reference consumed by the motion tracker | +| `action.hand` | `(12,)`, optional | Left/right LinkerHand target when hand control is enabled | +| `action.neck` | `(2,)`, optional | Mechanically clamped OpenNeck yaw/pitch target in degrees | + +`observation.state` is ordered as `joint_pos(29)`, `joint_vel(29)`, +`base_quat_wxyz(4)`, `base_ang_vel(3)`, and `projected_gravity(3)`. +`observation.mode` uses `standing=0`, `mocap=1`, `arms=2`, and `pause=3`. +`action` is `root_pos(3) + root_quat_wxyz(4) + reference_joint_pos(29)`. + +Camera RGB is stored only in the MP4 sidecar; HDF5 does not duplicate raw image +frames. Optional action fields appear exactly when the corresponding hardware +is enabled. + +## Commit and Recovery Rules + +The recorder commits HDF5 and video files before appending the manifest entry. +An interrupted, uncommitted episode is removed on the next recording-worker +startup and does not consume an episode index. An incompatible existing +`schema.json` stops only the non-critical recording worker, while G1 control +continues. + +## Review Recordings + +```bash +python scripts/view/view_recording.py \ + --recording data/recordings/sim2real_hdf5 +``` + +The reviewer validates manifest paths, HDF5 shapes, dtypes, finite values, and +MP4 alignment before playback. Measured root XYZ is not recorded, so the +observed robot is anchored to the reference root position; global root +translation cannot be evaluated from this format. diff --git a/docs/docs/reference/training-troubleshooting.md b/docs/docs/reference/training-troubleshooting.md deleted file mode 100644 index 08d20651..00000000 --- a/docs/docs/reference/training-troubleshooting.md +++ /dev/null @@ -1,144 +0,0 @@ ---- -sidebar_position: 5 ---- - -# Training Troubleshooting - -Common training issues and solutions. - -:::info -For training workflow, see [Training Tutorial](../tutorials/training). For data preparation, see [Dataset Reference](dataset). -::: - ---- - -## Issue 1: Mean Episode Length = 1.00 (Robot Terminates on First Step) - -### Symptoms - -- `Mean episode length: 1.00` -- `Episode_Termination/anchor_pos` near total parallel environment count -- `Metrics/motion/error_anchor_pos` > 0.5 m -- `Metrics/motion/error_body_rot` very large (close to pi) - -### Root Cause - -Usually not a PPO hyperparameter issue, but **motion NPZ labels inconsistent with MuJoCo FK**: - -1. **Body position coordinate error**: Local coordinates used as world coordinates -2. **Body order error**: Using PKL's 38-body order instead of mjlab G1's 30-body order -3. **Body orientation/angular velocity error**: All bodies approximated to root orientation - -The current `convert_pkl_to_npz.py` fixes these issues. - -### Quick Diagnosis - -```bash -python train_mimic/scripts/data/check_motion_npz_fk.py \ - --npz data/lafan1_clips/lafan1/.npz -``` - -Expected thresholds: `pos_max < 1e-3 m`, `quat_mean < 0.05 rad`, `quat_p95 < 0.10 rad`. - -If check fails, regenerate data and run a smoke test: - -```bash -python train_mimic/scripts/train.py \ - --num_envs 64 --max_iterations 100 \ - --motion_file data/datasets/_precomputed -``` - -Expected: `Mean episode length` significantly > 1, `error_anchor_pos` starts decreasing. - ---- - -## Issue 2: Episode Length Not Growing - -### Symptoms - -After 1000+ iterations, `Mean episode length` stays low (< 3) with no upward trend. - -### Possible Causes - -1. Poor retargeting quality (unreachable target poses) -2. Tracking reward weight too low vs regularization -3. Learning rate too high/low, clip_param mismatch -4. Termination thresholds too strict - -### Diagnosis Steps - -1. Visualize reference motion with `play.py` -2. Check reward distribution - tracking reward should dominate -3. Temporarily increase `bad_anchor_pos` threshold (0.25m -> 0.5m) -4. Compare with mjlab's built-in G1 tracking task - ---- - -## Issue 3: Slow Training - -### Symptoms - -Training speed < 1000 steps/s (expected 1500-2000 on RTX 4090). - -### Solutions - -1. Increase `--num_envs` to 4096 (needs 24 GB VRAM) -2. Disable `--video` during training -3. Use TensorBoard instead of W&B (default) - ---- - -## Issue 4: `nefc overflow - please increase njmax` - -### Symptoms - -```text -nefc overflow - please increase njmax to 257 -``` - -### Root Cause - -MuJoCo constraint buffer insufficient. When the robot falls or has many contacts, active constraints exceed `njmax`. The `mjlab` training default is `sim.njmax=250`. - -### Solution - -Already fixed in the repository. The env builder in `train_mimic/tasks/tracking/config/env.py` overrides: - -```python -self.sim.njmax = 500 -self.sim.nconmax = 150_000 -``` - -If warnings persist at higher values, increase to `njmax = 800`. - -:::note -Only modifying the robot XML is insufficient - the simulation-level `njmax` in mjlab takes precedence. -::: - ---- - -## Issue 5: Foot Sliding in Sim2Sim (Benchmark OK but ONNX Inference Slides) - -### Root Cause - -Sim2sim configuration parameters mismatch with training environment: - -1. **`default_angles` mismatch (critical)**: Different joint defaults cause action offset and observation errors -2. **Missing joint armature**: Training environment has non-zero armature; zero armature causes overshoot -3. **condim mismatch**: Different collision parameters between training and sim2sim - -### Diagnosis - -```python -from mjlab.asset_zoo.robots import get_g1_robot_cfg -cfg = get_g1_robot_cfg() -print(cfg.init_state.joint_pos) # Must match g1.yaml default_angles -``` - -### Solution - -Update `teleopit/configs/robot/g1.yaml` and the robot XML selected for training -to match the training environment values (default angles, armature, condim). -The default XML is `assets/robots/unitree_g1/g1_29dof.xml`. - -This fix also affects the sim2real path since `default_angles` is shared by `rl_policy.py` and `observation.py`. diff --git a/docs/docs/tutorials/offline-sim2sim.md b/docs/docs/tutorials/offline-sim2sim.md index 1c73d2a5..5bef642a 100644 --- a/docs/docs/tutorials/offline-sim2sim.md +++ b/docs/docs/tutorials/offline-sim2sim.md @@ -127,4 +127,5 @@ num_steps=300 realtime=true ``` -For every available field, see [Configuration](../configuration/overview). +For every available field, see +[Configuration](../reference/configuration/overview). diff --git a/docs/docs/tutorials/pico-sim2real.md b/docs/docs/tutorials/pico-sim2real.md index be6f3238..e3848993 100644 --- a/docs/docs/tutorials/pico-sim2real.md +++ b/docs/docs/tutorials/pico-sim2real.md @@ -221,7 +221,8 @@ python scripts/view/view_recording.py \ ``` The viewer synchronizes camera video, measured and reference G1 poses, and -optional hand and neck signals. See [Dataset Reference](../reference/dataset) +optional hand and neck signals. See +[Teleoperation Datasets](../reference/resources/teleoperation-datasets) for the stored fields. ## Common Problems diff --git a/docs/docs/tutorials/training.md b/docs/docs/tutorials/training.md index 07be82fb..e635e468 100644 --- a/docs/docs/tutorials/training.md +++ b/docs/docs/tutorials/training.md @@ -41,7 +41,7 @@ command below. Pointing training at the original `data/datasets` directory is an error, not a supported shortcut. For custom BVH, PKL, NPZ or Pico-recorded data, see -[Dataset Reference](../reference/dataset). +[Motion Datasets](../reference/resources/motion-datasets). ## 2. Choose the Robot Model @@ -191,5 +191,4 @@ Here `--num_envs` is per process, so the total scales with the world size. | Training is unexpectedly slow | Check that PyTorch detects CUDA and that the training device is a CUDA GPU | For task internals and model dimensions, see -[Architecture](../reference/architecture). For failure-specific guidance, see -[Training Troubleshooting](../reference/training-troubleshooting). +[Architecture](../reference/architecture). diff --git a/docs/docusaurus.config.ts b/docs/docusaurus.config.ts index 3968c8be..78dd3cd4 100644 --- a/docs/docusaurus.config.ts +++ b/docs/docusaurus.config.ts @@ -72,7 +72,7 @@ const config: Config = { items: [ {label: 'Getting Started', to: '/getting-started/installation'}, {label: 'Tutorials', to: '/tutorials/offline-sim2sim'}, - {label: 'Configuration', to: '/configuration/overview'}, + {label: 'Reference', to: '/reference/configuration/overview'}, ], }, { diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current.json b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current.json index 5252e760..8a845705 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current.json +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current.json @@ -18,5 +18,9 @@ "sidebar.docsSidebar.category.Reference": { "message": "参考资料", "description": "The label for category 'Reference' in sidebar 'docsSidebar'" + }, + "sidebar.docsSidebar.category.Resources": { + "message": "资源", + "description": "The label for category 'Resources' in sidebar 'docsSidebar'" } } diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/faq.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/faq.md deleted file mode 100644 index 65856f4e..00000000 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/faq.md +++ /dev/null @@ -1,29 +0,0 @@ ---- -sidebar_position: 3 ---- - -# 配置常见问题 - -## 为什么设置了 `policy_path` 还是启动不了？ - -1. 确认文件存在 -2. 确认输入维度是 `167`，且为双输入 ONNX（`obs` + `obs_history`） - -## 为什么必须显式指定 `input.bvh_file`？ - -`input/bvh.yaml` 已不再提供机器相关的默认路径。始终在命令行显式指定： - -```bash -python scripts/run/run_sim.py \ - controller.policy_path=policy.onnx \ - input.bvh_file=data/sample_bvh/aiming1_subject1.bvh -``` - -## 为什么 `viewer=true` 不起作用？ - -旧的 `viewer` 别名已移除。请使用 `viewers`（复数）： - -```bash -python scripts/run/run_sim.py controller.policy_path=policy.onnx viewers=sim2sim -python scripts/run/run_sim.py controller.policy_path=policy.onnx viewers=none -``` diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/installation.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/installation.md index 425447a0..2ec6d007 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/installation.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/installation.md @@ -88,7 +88,7 @@ python scripts/setup/download_assets.py \ ``` 推理资源包会把 `track.onnx`、G1 模型文件、GMR 文件和示例 BVH 放到代码默认查找 -的位置。完整文件清单和资源分组见[资源参考](../reference/assets)。 +的位置。完整文件清单和资源分组见[资产](../reference/resources/assets)。 ## 5. 连接真实 G1 前的额外安装 @@ -100,7 +100,7 @@ bash scripts/setup/setup_g1_bridge.sh ``` 无论使用 Pico 还是真机 BVH 回放，都需要这个 bridge。如果编译失败或收不到机器人 -状态，请查看 [G1 Bridge SDK](../reference/g1-bridge-sdk)。 +状态，请查看[配套项目](../reference/companion-projects#g1-bridge-sdk)。 ## 6. 可选硬件 diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/intro.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/intro.md index c0712d78..8174e7d0 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/intro.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/intro.md @@ -36,4 +36,5 @@ Pico 头显后，可以实时控制机器人的全身动作。机载部署还可 主线教程只保留完成任务所需的内容。运行流程和技术规格见 [系统架构](reference/architecture)，下载文件与资源分组见 -[资源参考](reference/assets)，Hydra 参数见[配置说明](configuration/overview)。 +[资产](reference/resources/assets)，Hydra 参数见 +[配置说明](reference/configuration/overview)。 diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/architecture.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/architecture.md index 095f69a0..6e4214c5 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/architecture.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/architecture.md @@ -1,130 +1,145 @@ --- -sidebar_position: 1 +sidebar_position: 2 --- -# 架构 +# 系统架构 -本页集中说明运行流程、支持边界和精确维度。这些实现细节不会放在任务主线教程中。 +本页定义 Teleopit 的运行时流程、仓库布局、支持的技术范围和公共入口。 ## Pipeline -```text -InputProvider（BVH 文件 / Pico4） - -> Retargeter（GMR） - -> ObservationBuilder（167D） - -> Controller（双输入 TemporalCNN ONNX） - -> Robot（MuJoCo 仿真或 Unitree G1） -``` +![Teleopit 运行时流程](/img/diagrams/architecture-pipeline-zh.svg) -离线/在线推理由 `teleopit/runtime/` 和 `teleopit/pipeline.py` 装配。硬件状态机通过 `teleopit/sim2real/mp/` 中的进程隔离运行时执行。训练由 `train_mimic/` 提供。 +全身运控主流程把 BVH 或 PICO 实时身体动作转换为时间对齐的 G1 参考。 +`VelCmdObservationBuilder` 把参考动作与机器人状态组合起来，双输入 TemporalCNN +ONNX 运控器再输出 29 维关节偏移。同一套观测和运控器路径同时用于 MuJoCo 和真机 G1。 -## Pico 全身具身控制路径 +Pico 手部和主动视觉路径是可选的进程隔离 worker。它们复用同一个进程内 +`PicoBridge` 接收器，不会向 167 维运控策略观测增加字段。手部或颈部故障不能停止 +G1 身体控制。这些可选硬件路径只支持机载部署；外部主机 Pico 部署只支持全身控制。 -同一帧 Pico 数据可以同时进入三条相互独立的控制路径： - -```text -Pico 全身追踪 - -> GMR 动作重定向 -> 运控策略 -> G1 全身关节 +主机高层策略部署与 Pico 运行时彼此独立。单独的主机环境接收 JPEG RGB 和 +`observation.state(68)`，再通过严格的 ZeroMQ/msgpack 消息返回 canonical +`float32[T,50]` action chunk。机载校验器和调度器把其中的身体部分转换为 36 维参考， +交给现有 motion tracker；主机输出不能绕过 tracker，也不能直接成为电机命令。 -Pico 手势追踪或手柄输入 - -> Teleopit 手部适配 -> somehand 或开合映射 -> LinkerHand L6/O6 +Teleopit 和主机环境共享语义数据和一份完全相同的 `hand_calibration.json`，但不会导入 +对方的 Python 包。当前 client/server 代码和协议测试定义网络结构，因此协议变化时 +两个仓库必须同步修改。 -Pico 头显旋转 + 同帧 Spine3 旋转 - -> 相对 yaw/pitch 映射 -> OpenNeck -``` +## 运行时边界 -全身控制是必需路径。手部和 OpenNeck 是机载部署中的可选独立进程，它们发生故障时 -不能停止 G1 身体控制；外部主机部署只支持全身控制。所有启用的路径复用同一个进程内 -PicoBridge。 +- 离线核心组件通过 `InProcessBus` 通信，不复制数组 payload。 +- 真机机器人控制、参考生成、相机、录制、手部、颈部和高层策略客户端在可能阻塞或 + 硬件故障影响 50 Hz 控制循环时使用进程隔离。 +- 本地 sim2real worker 使用 localhost ZeroMQ 和共享内存视频环。 +- 外部主机策略边界使用 msgpack 和非 pickle 的 float32 数组。 +- 共享组件契约是在 `teleopit/interfaces.py` 中定义的 `typing.Protocol`。 -由主机提供服务的模仿策略使用第二条相互独立的部署路径： +## 仓库布局 ```text -lerobot-teleopit 主机环境 - policy server -> 严格的 ZeroMQ/msgpack 消息 - | -Teleopit onboard 环境 - RealSense/state -> 非关键 client worker -> 已验证的 action scheduler - -> 现有 50 Hz motion tracker -> G1 关节角目标 - -> 专用 LinkerHand O6 与 OpenNeck worker +teleopit/ — 核心推理和部署包 +├── interfaces.py — 机器人、运控器、输入和重定向协议 +├── pipeline.py — 轻量离线仿真 facade +├── runtime/ — 配置/路径解析、工厂和 CLI 校验 +├── configs/ — Hydra 运行时配置 +├── bus/ — 进程内零拷贝发布/订阅 +├── inputs/ — BVH、PICO 和实时输入适配器 +├── retargeting/gmr/ — 自包含的全身 GMR 实现 +├── controllers/ — 观测构建器和 ONNX 策略运控器 +├── robots/ — MuJoCo 机器人适配器 +├── sim/ — 200 Hz PD / 50 Hz 策略仿真循环 +├── sim2real/ +│ ├── mp/ — 进程 supervisor、IPC 和机器人控制状态机 +│ ├── hands/ — 可选 LinkerHand 驱动和输入映射 +│ └── neck/ — 可选 OpenNeck 映射和 worker +├── high_level_policy/ — 主机协议、坐标变换和 action 调度器 +└── recording/ — Sim2real 数据 schema 和录制 worker + +train_mimic/ — 训练包 +├── app.py — 共享的训练/播放/benchmark 装配 +├── tasks/tracking/ — General-Tracking-G1 任务和 TemporalCNN 模型 +├── data/ — 数据集构建和动作加载 +└── scripts/ — 训练、播放、benchmark 和 ONNX 导出 + +scripts/ — 面向用户的运行和维护入口 +├── run/ — 仿真、sim2real 和录制命令 +├── setup/ — 资源下载和硬件设置 +├── render/ — 离线视频渲染 +├── view/ — 录制数据检查 +└── dev/ — 校验和标定工具 + +third_party/ — 可选硬件 SDK 和 somehand +tests/ — 单元、协议和集成测试 ``` -主机与 onboard 环境共享语义数据和一份相同的 `hand_calibration.json`；它们不会导入 -对方的 Python 包。当前 client/server 代码和协议测试定义网络结构，因此活跃开发期间 -两个仓库必须同步修改。Pico 遥操作和主机策略部署也分别使用不同的运行脚本与进程装配。 - -## 代码结构 - -```text -configs / scripts - -> runtime - -> interfaces + pipeline state machines - -> adapters（inputs / retargeting / controller / robot / recording） - -train_mimic/scripts - -> train_mimic/app.py - -> single task registry / env builder / runner cfg - -> mjlab / rsl_rl - -train_mimic/scripts/data - -> train_mimic/data/dataset_builder.py - -> dataset_lib / motion_fk / convert_pkl_to_npz -``` - -## 核心模块边界 - -| 模块 | 职责 | -|------|------| -| `teleopit/interfaces.py` | 稳定协议：InputProvider、Retargeter、Controller、Robot、ObservationBuilder | -| `teleopit/runtime/` | 配置解析、路径规范化、组件装配、CLI 校验 | -| `teleopit/pipeline.py` | 离线仿真的轻量 facade | -| `teleopit/sim2real/mp/` | 进程隔离的 sim2real 状态机、IPC 和机器人控制循环 | -| `teleopit/high_level_policy/` | 主机策略协议、session-local 坐标变换、验证与 30-to-50 Hz scheduler | -| `teleopit/controllers/observation.py` | ObservationBuilder | -| `teleopit/controllers/rl_policy.py` | 接受观测维度与运行时 builder 匹配的双输入 ONNX | -| `train_mimic/app.py` | 共享的训练/播放/benchmark 装配 | -| `train_mimic/tasks/tracking/config/` | 单一任务注册（`General-Tracking-G1`） | -| `train_mimic/data/dataset_builder.py` | 唯一官方数据集构建入口 | - ## 技术规格 -| 项目 | 规格 | -|---|---| -| 支持机器人 | Unitree G1，29 个驱动关节 | +| 规格 | 支持值 | +|------|--------| +| 机器人 | 29 个执行关节的 Unitree G1 | | 仿真器 | MuJoCo | -| 动作重定向 | GMR（General Motion Retargeting） | -| 运控 / PD 频率 | 50 Hz / 200 Hz | +| 全身重定向 | GMR（General Motion Retargeting） | +| 策略 / PD 频率 | 50 Hz / 200 Hz | | 训练任务 | `General-Tracking-G1` | -| 推理观测 | `velcmd_history`（167D） | -| ONNX 签名 | 双输入 `obs`（167D）+ `obs_history` | -| 运控输出 | 相对 `default_dof_pos` 的 29D 关节 offset | -| Actor/Critic | TemporalCNN（2048、1024、512、256、128） | -| 训练采样 | 默认 `rewind`；也支持 `uniform`；播放使用 `start`；benchmark 固定精确 clip 并禁用 clip 末尾重采样 | -| 训练 `window_steps` | `[0]` | -| 数据格式 | 可递归发现的最小 HDF5 shard（`shard_*.h5`） | -| 可选灵巧手 | LinkerHand L6 或 O6，支持手柄开合或 Pico 手势 | -| 可选主动视觉 | 使用物理角度控制 OpenNeck yaw/pitch | -| 主机策略 observation | JPEG RGB + `observation.state(68)` | -| 主机策略 action | 30 Hz 的 `float32[T,50]` canonical reference | -| 主机策略 body 控制 | 36D root/joint reference 通过现有 50 Hz motion tracker | +| 推理观测 | `velcmd_history`（167 维） | +| ONNX 签名 | 双输入：`obs`（167 维）+ `obs_history` | +| 策略动作 | 相对 `default_dof_pos` 的 29 维关节偏移 | +| Actor / critic | TemporalCNN（2048、1024、512、256、128） | +| 训练采样 | 默认 `rewind`；支持 `uniform`；播放使用 `start`；benchmark 固定精确 clip 并禁用 clip 末尾重采样 | +| 训练窗口 | `window_steps=[0]` | +| 分发动作数据 | 递归 minimal HDF5 `shard_*.h5` 文件 | +| 可选手部 | LinkerHand L6/O6，支持 gripper 或 PICO 手部姿态输入 | +| 可选主动视觉 | 使用物理角度的 OpenNeck yaw/pitch | +| 主机策略观测 | JPEG RGB + `observation.state(68)` | +| 主机策略动作 | `float32[T,50]`，30 Hz 源时间线，`T` 在 `[1,50]` 内 | +| 主机策略身体控制 | 36 维根部/关节参考，通过现有 50 Hz motion tracker | ## 约束 -- 必须显式提供 `controller.policy_path`，且文件必须存在 -- 离线 BVH 运行必须显式提供 `input.bvh_file` -- `viewers` 是唯一的 viewer 配置入口 -- 观测/ONNX 维度不匹配会在启动时立即报错 -- sim2real 也要求双输入 ONNX，且观测维度必须与运行时 builder 匹配 -- 主机策略消息 envelope 或 schema 不匹配时会被拒绝，机器人保持在 `STANDING` -- 主机 action chunk 在 onboard 完成验证与插值；主机不能绕过 motion tracker 或发送电机命令 -- 策略 entry 仅在单个 host session 等待第一份有效 chunk 时保持为 `STANDING` 内部流程；该 chunk 会直接进入 `POLICY`，不进行候选 reference 对齐、不运行 entry Kp ramp，也不创建或 reset 第二个 session；50 Hz limiter 从 session 开始时捕获的机器人实测 reference 起步 -- chunk 边界和 chunk 内部的 root、yaw 与关节 reference 时间跳变都会被接受，再由 50 Hz scheduler 输出执行 rate limit，从而保留录制的 pause/resume 转换 - -## 公共接口 - -**稳定运行模式：** 离线 sim2sim、离线 sim2real playback、Pico4 sim2sim、G1 -sim2real、独立的主机策略 G1 sim2real - -**稳定训练入口：** `train.py`、`play.py`、`benchmark.py`、`save_onnx.py` - -**稳定数据入口：** `build_dataset.py`、`precompute_dataset.py` +- `controller.policy_path` 必须显式提供，并指向现有文件。 +- 离线 BVH 运行必须显式提供现有的 `input.bvh_file`。 +- `viewers` 是唯一的 viewer 配置键。 +- 观测定义必须与 ONNX 签名完全一致；启动时会直接失败，不会 pad 或 trim 数据。 +- `default_dof_pos` 必须来自所选机器人的默认站立角度。 +- sim2real 使用与仿真相同的双输入观测契约。 +- 主机消息 envelope 或 schema 不匹配时会被拒绝，机器人保持在 `STANDING`。shape、 + 有限值、session、sequence、四元数、时效性或安全检查失败时，会拒绝整个 action + chunk。 +- 主机 action 在机载侧校验、调度并限速；主机不能绕过 motion tracker 或发送 G1 + 电机命令。 +- 策略 entry 在一个主机 session 等待第一份有效 chunk 时保持为 `STANDING` 内部流程。 + 该 chunk 会直接进入 `POLICY`，不执行候选参考对齐、entry Kp ramp 或第二次 + session/reset。50 Hz limiter 从 session 开始时捕获的机器人实测参考起步。 +- chunk 边界和内部的根部、yaw 与关节参考时间跳变都会被接受，再由 50 Hz scheduler + 输出限速，从而保留录制的 pause/resume 转换。 +- PICO 输入、RealSense 预览、录制、手部和颈部故障都是非关键故障；Unitree 遥控器 + 和机器人控制循环仍然可用。 + +## 公共入口 + +支持的运行模式包括离线 sim2sim、离线 sim2real 回放、PICO sim2sim、PICO G1 +sim2real，以及独立主机高层策略 G1 sim2real。 + +运行命令： + +- `scripts/run/run_sim.py` — 离线 BVH 和 PICO 实时 sim2sim +- `scripts/run/run_sim2real.py` — BVH 或 PICO G1 sim2real +- `scripts/run/run_high_level_policy_sim2real.py` — 独立主机高层策略 G1 部署 +- `scripts/run/record_pico_motion.py` — 从 PICO 录制重定向动作 clip +- `scripts/render/render_sim.py` — 渲染 mocap、重定向和 sim2sim 视频 +- `scripts/view/view_recording.py` — 检查同步的 sim2real 录制数据 + +训练和数据命令： + +- `train_mimic/scripts/train.py`、`play.py`、`benchmark.py`、`save_onnx.py` +- `train_mimic/scripts/data/build_dataset.py` +- `train_mimic/scripts/data/precompute_dataset.py` + +公共 Python 接口： + +- `teleopit/interfaces.py` 中的协议 +- `TeleopPipeline` +- `VelCmdObservationBuilder` +- `RLPolicyController` diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/companion-projects.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/companion-projects.md new file mode 100644 index 00000000..8d328416 --- /dev/null +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/companion-projects.md @@ -0,0 +1,77 @@ +--- +sidebar_position: 4 +--- + +# 配套项目 + +Teleopit 集成了四个职责明确的组件，分别负责机器人通信、灵巧手重定向、主动视觉和 +PICO 数据传输。它们位于 `teleopit` Python 包之外，各自维护硬件协议和公共 API。 + +| 组件 | 源码地址 | 功能 | 在 Teleopit 中的用途 | +|------|----------|------|-----------------------| +| G1 Bridge SDK | [Teleopit 源码目录](https://github.com/BotRunner64/Teleopit/tree/master/third_party/g1_bridge_sdk) | 基于 Unitree SDK2、Cyclone DDS 和 pybind11 的原生 C++ bridge | 获取 G1 实时状态和遥控器输入、切换模式，并发送 200 Hz 底层命令 | +| somehand | [GitHub](https://github.com/BotRunner64/somehand) | 灵巧手动作重定向库 | 把 Pico 实时手部 landmark 映射为 LinkerHand L6/O6 目标 | +| OpenNeck | [GitHub](https://github.com/BotRunner64/OpenNeck) | 带标定的双轴颈部驱动 | 把物理 yaw/pitch 角度转换为安全的舵机命令 | +| PICO Bridge | [GitHub](https://github.com/BotRunner64/pico-bridge) | 传输 PICO 追踪和视频的头显应用与 Python 接收器 | 提供身体、手柄、手部和 HMD 帧，并可回传 RGB 视频 | + +## G1 Bridge SDK + +G1 Bridge SDK 直接维护在 Teleopit 的 `third_party/g1_bridge_sdk` 中，不是单独的 +仓库。安装脚本会下载 [Unitree SDK2](https://github.com/unitreerobotics/unitree_sdk2)， +然后构建并安装本地 pybind11 扩展： + +```bash +bash scripts/setup/setup_g1_bridge.sh +``` + +所有 DDS 发布和订阅都运行在原生 C++ 线程中。Teleopit 的 `UnitreeG1` 适配器通过 +bridge 读取关节状态、基座方向、角速度和无线遥控器输入，并发送带逐关节 PD 增益的 +29 关节位置目标。真机遥操、独立站立测试和主机高层策略部署都使用这个硬件边界。 + +## somehand + +somehand 提供可配置的人手到机器人手部动作重定向。Teleopit 把兼容源码固定为 +`third_party/somehand` Git submodule，并使用 0.2.0 的公共 `somehand.api`。 + +在 `hands.mode=vr_hand_pose` 下，Teleopit 把 PICO 的 26 关节手部状态转换为 21 个 +landmark，调用 somehand 连续重定向，再把结果发送给 LinkerHand L6 或 O6。Pico +实时接收和 landmark 转换由 Teleopit 负责，不会启动 somehand 自带的 Pico 输入路径。 + +安装灵巧手依赖： + +```bash +git submodule update --init --recursive +pip install -e third_party/linkerhand-python-sdk +pip install -e third_party/somehand +``` + +## OpenNeck + +OpenNeck 负责双轴主动视觉云台的串口通信、角度到舵机 step 的转换和标定机械限位。 +Teleopit 支持 OpenNeck 0.2.0 的物理角度 API，并调用 `move_deg()`；已经移除的 +normalized 控制字段不兼容。 + +在 Pico 遥操中，Teleopit 计算 HMD 相对同帧 `Body.Spine3` 的旋转，应用配置的死区和 +俯仰增益，再由非关键 neck worker 发送 yaw/pitch 角度。主机高层策略部署则发送 +canonical action 中经过校验的颈部字段。 + +```bash +pip install -e '.[openneck]' +openneck calibrate +``` + +## PICO Bridge + +PICO Bridge 同时包含头显应用和可导入的 Python PC 接收器。Teleopit 支持 0.2.1 +版本，由 `pico4` extra 安装： + +```bash +pip install -e '.[pico4]' +``` + +一个进程内 `PicoBridge` 实例为 Teleopit 提供全身、手柄、手部和独立 HMD 数据。 +全身重定向、手部控制和 OpenNeck 共用这个接收器。启用视频后，Teleopit 还可以通过 +`push_video_frame()` 把 MuJoCo 或 RealSense RGB 帧推回头显。 + +头显 APK 从 [PICO Bridge Releases](https://github.com/BotRunner64/pico-bridge/releases) +下载。 diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/configuration/fields.md similarity index 99% rename from docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md rename to docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/configuration/fields.md index 2e902c26..4bdef41b 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/config-reference.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/configuration/fields.md @@ -2,9 +2,9 @@ sidebar_position: 2 --- -# 配置参考 +# 配置字段 -本页列出 Teleopit 所有可配置字段及其含义。 +本页列出 Teleopit 的全部 Hydra 配置字段。 ## 顶层字段 diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/overview.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/configuration/overview.md similarity index 97% rename from docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/overview.md rename to docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/configuration/overview.md index 556afea1..3ffacf9c 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/configuration/overview.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/configuration/overview.md @@ -76,4 +76,4 @@ Teleopit 不会静默修补配置错误： 当你遇到配置错误时，应该查找**哪两个组件的定义不一致**。 -完整字段参考请查看 [配置参考](config-reference)。 +完整字段参考请查看[配置字段](fields)。 diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/g1-bridge-sdk.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/g1-bridge-sdk.md deleted file mode 100644 index 2d2024ab..00000000 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/g1-bridge-sdk.md +++ /dev/null @@ -1,59 +0,0 @@ ---- -sidebar_position: 4 ---- - -# G1 Bridge SDK - -C++ DDS 桥接库，用 pybind11 封装 unitree_sdk2，让 Python 以接近零延迟（< 0.5 ms）访问 Unitree G1 的实时通信接口。 - -所有 DDS 发布/订阅运行在原生 C++ 线程中，Python 侧只需调用简单的 get/set 方法。 - -## 依赖 - -- CMake >= 3.10 -- GCC >= 9.4（支持 C++17） -- pybind11 >= 2.6 -- Unitree SDK2（已内置于 `third_party/g1_bridge_sdk/thirdparty/unitree_sdk2/`，无需手动安装） -- Cyclone DDS（unitree_sdk2 依赖） - -## 安装 - -```bash -bash scripts/setup/setup_g1_bridge.sh -``` - -脚本会自动克隆 `unitree_sdk2`、安装 `pybind11` 并编译 C++ 桥接库。 - -## Python API - -```python -import g1_bridge_sdk - -bridge = g1_bridge_sdk.G1Bridge( - network_interface="enp130s0", # PC 上连接 G1 的以太网接口 - publish_hz=200 # 指令发布频率（默认 200 Hz） -) -``` - -PC 通过网线连接 G1 控制时，先在 PC 上运行 `ifconfig`，填写这根 G1 网线对应的接口名。在机器人 onboard 计算机上运行时，`eth0` 通常就是正确接口。 - -| 方法 | 说明 | -|------|------| -| `wait_for_state(timeout_sec=5.0)` | 阻塞等待第一帧 LowState，超时返回 False | -| `get_state()` | 返回 `(qpos[29], qvel[29], quat[4], ang_vel[3])` numpy 数组 | -| `get_state_counter()` | 返回累计收到的 LowState 帧数 | -| `get_wireless_remote()` | 返回 40 字节无线遥控数据 | -| `get_mode_machine()` | 返回当前 mode_machine 值 | -| `set_target(target, kp, kd)` | 设置目标关节位置和 PD 增益（各 29 元素） | -| `lock_joints()` | 锁定当前关节位置 | -| `set_damping()` | 切换为阻尼模式（急停用） | -| `start_publish()` | 启动指令发布线程 | -| `stop_publish()` | 停止指令发布线程 | -| `check_mode()` | 查询当前运动模式，返回 `(code, name)` | -| `select_mode(name)` | 切换运动模式（如 `"ai"`、`"normal"`） | -| `release_mode()` | 释放当前模式，进入低级控制 | - -## 使用场景 - -- **Pico4 真机遥操作**：`scripts/run/run_sim2real.py` -- **独立站立测试**：`scripts/run/standalone_standing.py` diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/assets.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/resources/assets.md similarity index 97% rename from docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/assets.md rename to docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/resources/assets.md index 07fa8e6c..eaee1d32 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/assets.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/resources/assets.md @@ -1,11 +1,11 @@ --- -sidebar_position: 2 +sidebar_position: 1 --- -# 资源参考 +# 资产 Teleopit 的 Git 仓库只保存代码，不保存大型机器人 mesh、运控模型和动作数据。 -[安装说明](../getting-started/installation)给出了每种用户场景最短的下载命令；本页提供 +[安装说明](../../getting-started/installation)给出了每种用户场景最短的下载命令；本页提供 完整文件清单和维护者说明。 ## 不入库的内容 diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/dataset.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/resources/motion-datasets.md similarity index 70% rename from docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/dataset.md rename to docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/resources/motion-datasets.md index b5bd65c6..5fb94e24 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/dataset.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/resources/motion-datasets.md @@ -1,16 +1,12 @@ --- -sidebar_position: 3 +sidebar_position: 2 --- -# 数据集参考 +# 动作数据集 -Teleopit 使用两类相互独立的数据： - -- **动作数据集**为运控训练提供参考动作； -- **真机 episode 录制**保存同步的机器人状态、参考动作和相机视频，供后续检查或外部 - 策略使用。 - -两类数据的 schema 不同，不能相互替换。 +动作数据集为运控训练提供参考动作。用于分发的 minimal 格式与预计算训练格式彼此 +独立，训练只能读取后者。同步的机器人状态、参考动作和相机录制见 +[遥操数据集](teleoperation-datasets)。 ## 下载预构建数据集（推荐） @@ -174,54 +170,3 @@ python train_mimic/scripts/data/check_motion_npz_fk.py \ ``` 推荐判据：`pos_max < 1e-3 m`、`quat_mean < 0.05 rad`、`quat_p95 < 0.10 rad`。 - -## 真机 Episode 录制 - -录制程序写出的是一个可编辑数据集，而不是单个包含所有内容的 HDF5： - -```text -data/recordings/sim2real_hdf5/ -├── schema.json -├── episodes.jsonl -├── data/ -│ └── episode_000000.h5 -└── videos/ - └── d435i_rgb/ - └── episode_000000.mp4 -``` - -`schema.json` 定义数据集 FPS、`robot_type`、`hand_type`、`neck_type`，以及每个字段的 -shape、dtype、名称和分组。硬件类型必须与当前运行配置一致。 - -`episodes.jsonl` 是可编辑的 episode 清单。每一行把一条 episode 映射到对应 HDF5 -和 MP4，并保存任务描述。任务文本不会写入 HDF5 attribute。 - -每个 HDF5 只包含按帧对齐的数组： - -| 字段 | Shape | 含义 | -|------|-------|------| -| `frame_index` | scalar | 相机/动作帧序号 | -| `timestamp` | scalar | 单调时钟时间戳，单位为秒 | -| `observation.state` | `(68,)` | G1 关节状态、基座方向/角速度和投影重力 | -| `observation.mode` | scalar | `STANDING`、`MOCAP`、`ARMS` 或动捕暂停状态码 | -| `action` | `(36,)` | motion tracker 使用的根部姿态和 29 关节参考 | -| `action.hand` | `(12,)`，可选 | 启用手部控制时的左右 LinkerHand 目标 | -| `action.neck` | `(2,)`，可选 | 经过机械限位后的 OpenNeck yaw/pitch 角度 | - -相机 RGB 只保存在 MP4 中，HDF5 不再重复保存 raw image。只有启用对应硬件时，才会 -出现可选 action 字段。 - -录制器会先提交 HDF5/视频文件，再向清单追加记录。进程中断后，未提交的 episode 会在 -下次录制进程启动时删除，也不会占用 episode 序号。已有 `schema.json` 与当前配置 -不兼容时，只会停止非关键的录制进程。 - -使用下面的命令查看数据： - -```bash -python scripts/view/view_recording.py \ - --recording data/recordings/sim2real_hdf5 -``` - -播放前，查看器会检查清单路径、HDF5 shape/dtype/有限值和 MP4 对齐。录制数据不包含 -实测根部 XYZ，因此 Viewer 中的实测机器人会锚定到参考根部位置；这个格式无法评估 -全局根部平移。 diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/resources/teleoperation-datasets.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/resources/teleoperation-datasets.md new file mode 100644 index 00000000..12b35358 --- /dev/null +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/resources/teleoperation-datasets.md @@ -0,0 +1,88 @@ +--- +sidebar_position: 3 +--- + +# 遥操数据集 + +遥操数据集是手动录制的 sim2real episode。它同步保存 G1 状态、motion tracker +消费的参考动作、可选的手部和颈部命令，以及 RealSense RGB 视频。这个格式用于数据 +检查和外部策略开发，不是运控训练使用的动作数据集。 + +## 录制 Episode + +录制只支持带交互终端和新鲜 RealSense 画面的 Pico 机载 sim2real 部署： + +```bash +pip install -e '.[recording]' +python scripts/run/run_sim2real.py --config-name sim2real_record \ + controller.policy_path=policy.onnx +``` + +手动配置的等价条件是 `recording.enabled=true`、`input.provider=pico4`、 +`input.video.enabled=true` 和 `input.video.source=realsense`。 + +终端按 `R` 开始一条 episode，按 `S` 保存，按 `D` 丢弃，按 `Q` 关闭运行时。 +`STANDING`、`MOCAP`、`ARMS` 和动捕暂停状态都可以录制。没有新鲜相机帧时不能开始 +录制；录制过程中相机画面超过一秒未更新时，当前 episode 会被丢弃，但 Pico 输入和 +G1 控制继续运行。视频恢复后不会自动重新开始录制。 + +## 数据集目录 + +录制程序写出的是一个可编辑数据集，而不是单个包含所有内容的 HDF5： + +```text +data/recordings/sim2real_hdf5/ +├── schema.json +├── episodes.jsonl +├── data/ +│ └── episode_000000.h5 +└── videos/ + └── d435i_rgb/ + └── episode_000000.mp4 +``` + +`schema.json` 定义数据集 FPS、`robot_type`、`hand_type`、`neck_type`，以及每个字段的 +shape、dtype、名称和分组。硬件类型必须与当前运行配置一致。 + +`episodes.jsonl` 是可编辑的 episode 清单。每一行把一条 episode 映射到对应 HDF5 +和 MP4，并保存任务描述。任务文本不会写入 HDF5 attribute，因此修改任务描述不需要 +重写帧数据。 + +## 帧字段 + +每个 HDF5 只包含按帧对齐的数组： + +| 字段 | Shape | 含义 | +|------|-------|------| +| `frame_index` | scalar | 相机/动作帧序号 | +| `timestamp` | scalar | 单调时钟时间戳，单位为秒 | +| `observation.state` | `(68,)` | G1 关节状态、基座方向/角速度和投影重力 | +| `observation.mode` | scalar | `STANDING`、`MOCAP`、`ARMS` 或动捕暂停状态码 | +| `action` | `(36,)` | motion tracker 使用的根部姿态和 29 关节参考 | +| `action.hand` | `(12,)`，可选 | 启用手部控制时的左右 LinkerHand 目标 | +| `action.neck` | `(2,)`，可选 | 经过机械限位后的 OpenNeck yaw/pitch 角度 | + +`observation.state` 的顺序为 `joint_pos(29)`、`joint_vel(29)`、 +`base_quat_wxyz(4)`、`base_ang_vel(3)` 和 `projected_gravity(3)`。 +`observation.mode` 使用 `standing=0`、`mocap=1`、`arms=2` 和 `pause=3`。 +`action` 的结构是 `root_pos(3) + root_quat_wxyz(4) + reference_joint_pos(29)`。 + +相机 RGB 只保存在 MP4 sidecar 中，HDF5 不重复保存 raw image。只有启用对应硬件时， +才会出现可选 action 字段。 + +## 提交与恢复规则 + +录制器会先提交 HDF5 和视频文件，再向清单追加记录。进程中断后，未提交的 episode +会在下次录制进程启动时删除，也不会占用 episode 序号。已有 `schema.json` 与当前 +配置不兼容时，只会停止非关键的录制进程，G1 控制会继续运行。 + +## 检查录制数据 + +```bash +python scripts/view/view_recording.py \ + --recording data/recordings/sim2real_hdf5 +``` + +播放前，查看器会检查清单路径、HDF5 shape、dtype、有限值和 MP4 对齐。录制数据 +不包含实测根部 XYZ，因此实测机器人会锚定到参考根部位置；这个格式无法评估全局 +根部平移。 diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/training-troubleshooting.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/training-troubleshooting.md deleted file mode 100644 index acaa71b8..00000000 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/training-troubleshooting.md +++ /dev/null @@ -1,144 +0,0 @@ ---- -sidebar_position: 5 ---- - -# 训练问题排查 - -常见训练问题及解决方案。 - -:::info -训练流程见[训练教程](../tutorials/training)，数据准备见[数据集参考](dataset)。 -::: - ---- - -## 问题 1：Mean Episode Length = 1.00（机器人第一步就终止） - -### 现象 - -- `Mean episode length: 1.00` -- `Episode_Termination/anchor_pos` 接近并行环境总数 -- `Metrics/motion/error_anchor_pos` > 0.5 m -- `Metrics/motion/error_body_rot` 很大（接近 pi） - -### 根本原因 - -通常不是 PPO 超参数问题，而是 **motion NPZ 的监督标签与 MuJoCo FK 不一致**： - -1. **body 位置坐标系错误**：把局部坐标当世界坐标使用 -2. **body 顺序错误**：使用 PKL 的 38-body 顺序而非 mjlab G1 的 30-body 顺序 -3. **body 朝向/角速度标签错误**：所有 body 近似为 root 朝向 - -当前版本的 `convert_pkl_to_npz.py` 已修复上述问题。 - -### 快速排查 - -```bash -python train_mimic/scripts/data/check_motion_npz_fk.py \ - --npz data/lafan1_clips/lafan1/.npz -``` - -推荐判据：`pos_max < 1e-3 m`、`quat_mean < 0.05 rad`、`quat_p95 < 0.10 rad`。 - -如果检查失败，重新生成数据并做一次 smoke test： - -```bash -python train_mimic/scripts/train.py \ - --num_envs 64 --max_iterations 100 \ - --motion_file data/datasets/_precomputed -``` - -预期：`Mean episode length` 明显大于 1，`error_anchor_pos` 开始下降。 - ---- - -## 问题 2：Episode Length 不增长 - -### 现象 - -训练 1000+ 轮后，`Mean episode length` 仍然很低（< 3），无上升趋势。 - -### 可能原因 - -1. Retargeting 质量差（目标姿态不可达） -2. Tracking reward 权重过低，正则化权重过高 -3. 学习率过大/过小，clip_param 不匹配 -4. 终止条件过严 - -### 排查步骤 - -1. 用 `play.py` 可视化参考运动 -2. 检查奖励分布——tracking reward 应占主导 -3. 临时增大 `bad_anchor_pos` 阈值（0.25m → 0.5m） -4. 对比 mjlab 内置的 G1 tracking task - ---- - -## 问题 3：训练速度慢 - -### 现象 - -训练速度 < 1000 steps/s（RTX 4090 预期 1500-2000 steps/s）。 - -### 解决方案 - -1. 增加 `--num_envs` 到 4096（需要 24 GB 显存） -2. 训练时关闭 `--video` -3. 使用 TensorBoard 替代 W&B（默认即 TensorBoard） - ---- - -## 问题 4：`nefc overflow - please increase njmax` - -### 现象 - -```text -nefc overflow - please increase njmax to 257 -``` - -### 根本原因 - -MuJoCo 约束缓冲区不足。机器人跌倒或大量接触时，活跃约束数超出 `njmax`。`mjlab` 训练默认 `sim.njmax=250`。 - -### 解决方案 - -仓库中已修复。`train_mimic/tasks/tracking/config/env.py` 的 env builder 覆盖了训练仿真参数： - -```python -self.sim.njmax = 500 -self.sim.nconmax = 150_000 -``` - -如果警告仍出现在更高数值，增加到 `njmax = 800`。 - -:::note -仅修改机器人 XML 不够——`mjlab` 的仿真层 `njmax` 才是实际生效的参数。 -::: - ---- - -## 问题 5：Sim2Sim 脚滑（Benchmark 正常但 ONNX 推理脚打滑） - -### 根本原因 - -sim2sim 配置参数与训练环境不一致： - -1. **`default_angles` 不匹配（最关键）**：不同的关节默认值导致动作偏移和观测误差 -2. **缺少 joint armature**：训练环境有非零 armature，零 armature 导致过冲 -3. **condim 不一致**：训练和 sim2sim 之间碰撞参数不同 - -### 诊断方法 - -```python -from mjlab.asset_zoo.robots import get_g1_robot_cfg -cfg = get_g1_robot_cfg() -print(cfg.init_state.joint_pos) # 必须与 g1.yaml default_angles 一致 -``` - -### 解决方案 - -更新 `teleopit/configs/robot/g1.yaml` 和训练时选择的机器人 XML，使其与训练环境的值 -一致（default angles、armature、condim）。默认 XML 是 -`assets/robots/unitree_g1/g1_29dof.xml`。 - -此修复同时影响 sim2real 路径，因为 `default_angles` 被 `rl_policy.py` 和 `observation.py` 共用。 diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/offline-sim2sim.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/offline-sim2sim.md index baf39bba..3f4cff6f 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/offline-sim2sim.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/offline-sim2sim.md @@ -123,4 +123,4 @@ num_steps=300 realtime=true ``` -完整字段见[配置说明](../configuration/overview)。 +完整字段见[配置说明](../reference/configuration/overview)。 diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md index 3e309141..b2387edd 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md @@ -205,8 +205,8 @@ python scripts/view/view_recording.py \ --recording data/recordings/sim2real_hdf5 ``` -查看器会同步显示相机视频、G1 实测与参考姿态，以及可选的手部和头部信号。字段说明见 -[数据集参考](../reference/dataset)。 +查看器会同步显示相机视频、G1 实测与参考姿态，以及可选的手部和头部信号。字段说明 +见[遥操数据集](../reference/resources/teleoperation-datasets)。 ## 常见问题 diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/training.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/training.md index e260ddf5..2ea11a26 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/training.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/training.md @@ -39,7 +39,7 @@ python train_mimic/scripts/data/precompute_dataset.py \ `data/datasets` 直接传给训练会报错，这不是支持的快捷方式。 自定义 BVH、PKL、NPZ 或 Pico 录制数据的处理方法见 -[数据集参考](../reference/dataset)。 +[动作数据集](../reference/resources/motion-datasets)。 ## 2. 选择机器人模型 @@ -181,5 +181,4 @@ torchrun \ | 启动加载时内存不足 | 减少参与训练的 precomputed shard，或增加内存 | | 训练速度异常缓慢 | 检查 PyTorch 是否识别 CUDA，并确认训练设备实际使用 CUDA GPU | -任务内部结构和模型维度见[系统架构](../reference/architecture)，具体训练故障见 -[训练问题排查](../reference/training-troubleshooting)。 +任务内部结构和模型维度见[系统架构](../reference/architecture)。 diff --git a/docs/i18n/zh-Hans/docusaurus-theme-classic/footer.json b/docs/i18n/zh-Hans/docusaurus-theme-classic/footer.json index 211f9b02..abfe9792 100644 --- a/docs/i18n/zh-Hans/docusaurus-theme-classic/footer.json +++ b/docs/i18n/zh-Hans/docusaurus-theme-classic/footer.json @@ -15,9 +15,9 @@ "message": "教程", "description": "The label of footer link with label=Tutorials" }, - "link.item.label.Configuration": { - "message": "配置", - "description": "The label of footer link with label=Configuration" + "link.item.label.Reference": { + "message": "参考资料", + "description": "The label of footer link with label=Reference" }, "link.item.label.GitHub": { "message": "GitHub", diff --git a/docs/sidebars.ts b/docs/sidebars.ts index 4be1c9b7..d6fff6f4 100644 --- a/docs/sidebars.ts +++ b/docs/sidebars.ts @@ -20,27 +20,32 @@ const sidebars: SidebarsConfig = { 'tutorials/training', ], }, - { - type: 'category', - label: 'Configuration', - items: [ - 'configuration/overview', - 'configuration/config-reference', - 'configuration/faq', - ], - }, { type: 'category', label: 'Reference', items: [ + { + type: 'category', + label: 'Configuration', + items: [ + 'reference/configuration/overview', + 'reference/configuration/fields', + ], + }, 'reference/architecture', - 'reference/assets', - 'reference/dataset', - 'reference/g1-bridge-sdk', - 'reference/training-troubleshooting', + { + type: 'category', + label: 'Resources', + items: [ + 'reference/resources/assets', + 'reference/resources/motion-datasets', + 'reference/resources/teleoperation-datasets', + ], + }, + 'reference/companion-projects', + 'contributing', ], }, - 'contributing', ], }; diff --git a/docs/static/img/diagrams/architecture-pipeline-zh.svg b/docs/static/img/diagrams/architecture-pipeline-zh.svg new file mode 100644 index 00000000..040b9537 --- /dev/null +++ b/docs/static/img/diagrams/architecture-pipeline-zh.svg @@ -0,0 +1,106 @@ + + Teleopit 运行时流程 + 全身运控主流程从 BVH 或 PICO 输入开始，依次经过 GMR、参考时间线、观测构建、ONNX 运控和 MuJoCo 或 Unitree G1。主机高层策略可以注入经过校验的参考，PICO 可选路径负责灵巧手和 OpenNeck。 + + + + + + + + + Teleopit 运行时流程 + + 全身运控 + + 输入提供器 + BVH · PICO 身体 + + + GMR 重定向 + 人体动作 → G1 动作 + + + 参考时间线 + 根部 + 29 关节 + 时间对齐 · 平滑 + + + 观测构建器 + velcmd_history + 167 维 + + + ONNX 运控器 + TemporalCNN + 29 维偏移 + + + 机器人 + MuJoCo · Unitree G1 + + + + + + + + 独立主机高层策略部署 + + 机载观测 + RealSense JPEG + state(68) + + + 主机策略服务 + ZeroMQ · msgpack + + + 校验器 + 调度器 + 时间戳对齐 · 安全检查 + 36 维身体参考 + + + + + 接入现有 motion tracker + + 可选 PICO 机载路径 · 共用一个 PICOBRIDGE 接收器 + + 手部 / 手柄 + PICO 追踪帧 + + + 手部映射 + somehand · gripper + + + LinkerHand + L6 · O6 + + + + + + HMD + Spine3 + 同帧旋转 + + + 偏航 / 俯仰映射 + 死区 · 俯仰增益 + + + OpenNeck + 物理角度 + + + + diff --git a/docs/static/img/diagrams/architecture-pipeline.svg b/docs/static/img/diagrams/architecture-pipeline.svg new file mode 100644 index 00000000..75183389 --- /dev/null +++ b/docs/static/img/diagrams/architecture-pipeline.svg @@ -0,0 +1,106 @@ + + Teleopit runtime pipelines + The main whole-body tracking pipeline runs from BVH or PICO input through GMR, a reference timeline, observation building, ONNX control, and a MuJoCo or Unitree G1 robot. A host policy can inject validated references, while optional PICO paths control hands and OpenNeck. + + + + + + + + + Teleopit runtime pipelines + + WHOLE-BODY TRACKING + + InputProvider + BVH · PICO body + + + GMR Retargeter + Human → G1 motion + + + Reference timeline + Root + 29 joints + time-aligned · smoothed + + + ObservationBuilder + velcmd_history + 167D + + + ONNX Controller + TemporalCNN + 29D offsets + + + Robot + MuJoCo · Unitree G1 + + + + + + + + INDEPENDENT HOST-POLICY DEPLOYMENT + + Onboard observation + RealSense JPEG + state(68) + + + Host policy server + ZeroMQ · msgpack + + + Validator + scheduler + Timestamp alignment · safety + 36D body reference + + + + + Feeds the existing motion tracker + + OPTIONAL PICO-BASED ONBOARD PATHS · ONE SHARED PICOBRIDGE RECEIVER + + Hands / controllers + PICO tracking frame + + + Hand mapping + somehand · gripper + + + LinkerHand + L6 · O6 + + + + + + HMD + Spine3 + Same-frame rotations + + + Yaw / pitch mapping + Dead zone · pitch gain + + + OpenNeck + Physical degrees + + + + From 9b080a282cd3fe9197b1b9cbfbd27439243b0ed5 Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Thu, 30 Jul 2026 15:17:07 +0800 Subject: [PATCH 50/59] Update G1 tracking asset variants --- .gitignore | 1 + AGENTS.md | 6 +++-- README.md | 14 ++++++----- docs/docs/getting-started/installation.md | 7 +++--- docs/docs/reference/configuration/fields.md | 14 ++++++----- docs/docs/reference/resources/assets.md | 24 ++++++++++++------- docs/docs/tutorials/bvh-sim2real.md | 2 +- .../tutorials/high-level-policy-sim2real.md | 2 +- docs/docs/tutorials/offline-sim2sim.md | 14 +++++------ docs/docs/tutorials/pico-sim2real.md | 20 ++++++++++------ docs/docs/tutorials/pico-sim2sim.md | 8 +++---- docs/docs/tutorials/standalone-standing.md | 8 +++---- docs/docs/tutorials/training.md | 6 ++--- .../current/getting-started/installation.md | 7 +++--- .../current/reference/configuration/fields.md | 4 +++- .../current/reference/resources/assets.md | 23 +++++++++++------- .../current/tutorials/bvh-sim2real.md | 2 +- .../tutorials/high-level-policy-sim2real.md | 2 +- .../current/tutorials/offline-sim2sim.md | 14 +++++------ .../current/tutorials/pico-sim2real.md | 21 +++++++++------- .../current/tutorials/pico-sim2sim.md | 8 +++---- .../current/tutorials/standalone-standing.md | 8 +++---- .../current/tutorials/training.md | 6 ++--- scripts/dev/bench_policy_onnx.py | 6 ++--- teleopit/retargeting/gmr/params.py | 14 +++++------ teleopit/runtime/assets.py | 2 +- teleopit/runtime/external_assets.py | 14 +++++++++-- tests/test_download_assets.py | 14 +++++++++++ 28 files changed, 166 insertions(+), 105 deletions(-) diff --git a/.gitignore b/.gitignore index 33034add..0be09449 100644 --- a/.gitignore +++ b/.gitignore @@ -74,6 +74,7 @@ outputs/ benchmark_results/ logs/ ckpt/ +ckpt_restore/ wandb/ *.log # Training - downloaded reference assets (re-downloadable) diff --git a/AGENTS.md b/AGENTS.md index 01bf9c84..e415a9b6 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -167,7 +167,7 @@ target_dof_pos = clip(action, -10, 10) × action_scale + default_dof_pos - Recording `schema.json` stores `robot_type=unitree_g1_29dof`, `hand_type=none|linkerhand_l6|linkerhand_o6`, `neck_type=none|openneck`, FPS, and feature definitions; optional action fields are controlled directly by `hands.enabled` and `neck.enabled`; the recording worker rejects an existing mismatched schema without writing episodes, but remains non-critical and must not stop the G1 control runtime; the previous attribute-based HDF5 layout is unsupported - Episodes interrupted before their `episodes.jsonl` entry is committed are discarded on the next recording-worker startup and do not consume an episode index - Review saved sim2real recordings with `scripts/view/view_recording.py`; it validates manifest/HDF5/MP4 alignment and synchronizes camera video, an observed-vs-reference MuJoCo overlay, joint/mode plots, and optional hand/neck signals; because measured root XYZ is not recorded, the observed robot is anchored to the reference root position -- `gripper` mode reuses `Pico4InputProvider.get_controller_snapshot()` for Pico grip/trigger open-close control and supports LinkerHand L6 and O6 +- `gripper` mode reuses `Pico4InputProvider.get_controller_snapshot()` and supports LinkerHand L6 and O6; the side grip trigger is a deadman enable, the index trigger controls closure while it is held, and releasing the side grip opens that hand - `vr_hand_pose` mode reuses `Pico4InputProvider.get_hand_snapshot()` and somehand 0.2.0 public `somehand.api` for continuous Pico hand-pose retargeting; do not start a second `PicoBridge` for hand control - Teleopit owns Pico 26-joint hand-state to 21-landmark conversion; do not import `somehand.pico_input` - LinkerHand O6 supports `hands.mode=gripper|vr_hand_pose`; its default `close_pose` is `[86, 73, 118, 111, 110, 111]` @@ -278,6 +278,8 @@ python train_mimic/scripts/save_onnx.py --checkpoint logs/rsl_rl/g1_general_trac ### External Assets - Do not commit robot meshes, datasets, checkpoints, or demo media to Git; use `scripts/setup/download_assets.py` - G1 XML variants and their meshes are downloaded under `assets/robots/unitree_g1/` by the `robots` asset group and are not tracked in Git; `g1_29dof.xml` is the default +- The neck-and-O6 runtime variant is `assets/robots/unitree_g1/g1_29dof_neck_o6.xml` +- Released tracking assets download under `ckpt/` as the matching `track_g1.{pt,onnx}` and `track_g1_neck_o6.{pt,onnx}` pairs - `teleopit/retargeting/gmr/assets/` is gitignored; downloaded at runtime - `train_mimic/assets/` is no longer tracked; FK tooling uses the robot assets under `assets/robots/`, with `assets/robots/unitree_g1/g1_29dof.xml` as the default G1 model - `third_party/linkerhand-python-sdk` and `third_party/somehand` support optional LinkerHand sim2real control @@ -301,7 +303,7 @@ python scripts/setup/prepare_modelscope_assets.py --only data # 2. Upload to each repo modelscope upload --repo-type model BingqianWu/Teleopit-models \ - data/modelscope_upload/checkpoints checkpoints + data/modelscope_upload/checkpoints checkpoints --sync modelscope upload --repo-type model BingqianWu/Teleopit-models \ data/modelscope_upload/archives archives modelscope upload --repo-type dataset BingqianWu/Teleopit-datasets \ diff --git a/README.md b/README.md index b14e8631..30ea458a 100644 --- a/README.md +++ b/README.md @@ -38,13 +38,15 @@ python scripts/setup/download_assets.py --only robots gmr ckpt bvh The default Unitree G1 robot model is downloaded to `assets/robots/unitree_g1/g1_29dof.xml`, with additional model variants in the same directory. Training can select a task-compatible XML with `--robot_xml`; -the quick-start command below uses the default model. +the quick-start command below uses the default model and its matching +`ckpt/track_g1.onnx` policy. The neck-and-O6 variant uses +`g1_29dof_neck_o6.xml` with `ckpt/track_g1_neck_o6.onnx`. **3. Run** ```bash python scripts/run/run_sim.py \ - controller.policy_path=track.onnx \ + controller.policy_path=ckpt/track_g1.onnx \ input.bvh_file=data/sample_bvh/aiming1_subject1.bvh ``` @@ -54,7 +56,7 @@ To show the simulated D435i RGB camera view, add the explicit `camera` viewer: ```bash python scripts/run/run_sim.py \ - controller.policy_path=track.onnx \ + controller.policy_path=ckpt/track_g1.onnx \ input.bvh_file=data/sample_bvh/aiming1_subject1.bvh \ 'viewers=[sim2sim,camera]' ``` @@ -95,7 +97,7 @@ pip install -e '.[recording]' # On Arm machines, prefer conda-forge: # conda install -c conda-forge pyrealsense2 python scripts/run/run_sim2real.py --config-name sim2real_record \ - controller.policy_path=track.onnx \ + controller.policy_path=ckpt/track_g1.onnx \ recording.task="walk forward" ``` @@ -154,7 +156,7 @@ does not start PicoBridge, GMR, or the Pico reference worker: ```bash python scripts/run/run_high_level_policy_sim2real.py \ - controller.policy_path=track.onnx \ + controller.policy_path=ckpt/track_g1_neck_o6.onnx \ high_level_policy.endpoint=tcp://192.168.1.10:5555 \ high_level_policy.task="pick up the object" \ real_robot.network_interface=eth0 @@ -196,7 +198,7 @@ the same Pico receiver used for whole-body control: ```bash pip install -e '.[openneck]' python scripts/run/run_sim2real.py --config-name pico4_sim2real \ - controller.policy_path=track.onnx \ + controller.policy_path=ckpt/track_g1_neck_o6.onnx \ neck.enabled=true \ neck.port=/dev/ttyACM0 ``` diff --git a/docs/docs/getting-started/installation.md b/docs/docs/getting-started/installation.md index b6c6bc0d..16b72638 100644 --- a/docs/docs/getting-started/installation.md +++ b/docs/docs/getting-started/installation.md @@ -89,8 +89,9 @@ python scripts/setup/download_assets.py \ --only robots gmr ckpt bvh ``` -The inference bundle creates `track.onnx`, the G1 model files, GMR files and a -sample BVH under their expected project paths. See +The inference bundle creates the `track_g1` and `track_g1_neck_o6` ONNX/checkpoint +pairs under `ckpt/`, plus the G1 model files, GMR files and a sample BVH under +their expected project paths. See [Assets](../reference/resources/assets) for the complete inventory and asset group mapping. @@ -164,7 +165,7 @@ sample simulation: ```bash python scripts/run/run_sim.py \ - controller.policy_path=track.onnx \ + controller.policy_path=ckpt/track_g1.onnx \ input.bvh_file=data/sample_bvh/aiming1_subject1.bvh ``` diff --git a/docs/docs/reference/configuration/fields.md b/docs/docs/reference/configuration/fields.md index 350a650a..8b2a4415 100644 --- a/docs/docs/reference/configuration/fields.md +++ b/docs/docs/reference/configuration/fields.md @@ -177,12 +177,14 @@ Realtime Pico resume re-centers heading and ground-plane position before trackin `hands.enabled=true` requires `input.provider=pico4` plus local editable installs of `third_party/linkerhand-python-sdk` and `third_party/somehand`. When enabled, hand control remains active in all sim2real modes. -`gripper` supports `linkerhand_l6` and `linkerhand_o6` by interpolating Pico -trigger input between the configured open and close poses. `vr_hand_pose` is -supported by `linkerhand_l6` and `linkerhand_o6`: missing hand pose holds the -last command for that side, the selected hand speed is set to the maximum, and -Teleopit converts Pico hand state to 21 landmarks before calling somehand 0.2.0 -through `somehand.api` only. +`gripper` supports `linkerhand_l6` and `linkerhand_o6`. The corresponding +controller's side grip trigger is a deadman enable: while it is held, the index +trigger interpolates between the configured open and close poses; releasing +the side grip trigger commands that hand to open. `vr_hand_pose` is supported +by `linkerhand_l6` and `linkerhand_o6`: missing hand pose holds the last command +for that side, the selected hand speed is set to the maximum, and Teleopit +converts Pico hand state to 21 landmarks before calling somehand 0.2.0 through +`somehand.api` only. | Field | Description | Default | |-------|-------------|---------| diff --git a/docs/docs/reference/resources/assets.md b/docs/docs/reference/resources/assets.md index affff206..2a8315f3 100644 --- a/docs/docs/reference/resources/assets.md +++ b/docs/docs/reference/resources/assets.md @@ -13,14 +13,14 @@ and maintainer reference. - `assets/robots/` - Canonical robot XML/meshes - `teleopit/retargeting/gmr/assets/` - GMR retargeting assets, IK configs, and non-canonical robot descriptions -- `data/`, checkpoints, caches +- `data/`, `ckpt/`, checkpoints, caches - Demo media (`assets/demo.gif`, `assets/demo.mp4`) ## Asset Inventory | Group | Local result | Used for | |-------|--------------|----------| -| `ckpt` | `track.onnx`, `track.pt` | Ready-to-run inference and the matching PyTorch checkpoint | +| `ckpt` | `ckpt/track_g1.{onnx,pt}`, `ckpt/track_g1_neck_o6.{onnx,pt}` | Ready-to-run inference models and matching PyTorch checkpoints | | `robots` | Robot XML variants and meshes under `assets/robots/` | Training, MuJoCo inference, GMR and dataset FK | | `gmr` | `teleopit/retargeting/gmr/assets/` | Retargeting models and IK configuration | | `bvh` | `data/sample_bvh/*.bvh` | Sample motions used by the installation check and simulation tutorial | @@ -32,12 +32,13 @@ The current G1 robot bundle includes: |-----------|-------| | `assets/robots/unitree_g1/g1_29dof.xml` | Base G1 model and the default | | `assets/robots/unitree_g1/g1_29dof_dex3.xml` | G1 with Dex3 hand geometry and inertial properties | -| `assets/robots/unitree_g1/g1_29dof_avp_o6.xml` | G1 with AVP active vision and O6 hand models | +| `assets/robots/unitree_g1/g1_29dof_neck_o6.xml` | G1 with neck active vision and O6 hand models | The default is not a model allowlist. Training can select another task-compatible XML with `--robot_xml`. XML files in the GMR asset directory belong to their retargeting configurations and are separate from the runtime -robot bundle. +robot bundle. Use the `track_g1` policy pair with the base model and the +`track_g1_neck_o6` pair with the neck-and-O6 model. ## Repositories @@ -59,7 +60,7 @@ robot bundle. | Group | Repository | Remote Path | |-------|-----------|-------------| -| `ckpt` | Teleopit-models | `checkpoints/track.onnx`, `checkpoints/track.pt` | +| `ckpt` | Teleopit-models | `checkpoints/track_g1.{onnx,pt}`, `checkpoints/track_g1_neck_o6.{onnx,pt}` | | `robots` | Teleopit-models | `archives/robot_assets.tar.gz` | | `gmr` | Teleopit-models | `archives/gmr_assets.tar.gz` | | `bvh` | Teleopit-models | `archives/sample_bvh.tar.gz` | @@ -87,8 +88,10 @@ Local paths after download: | Remote | Local | |--------|-------| -| `checkpoints/track.onnx` | `track.onnx` | -| `checkpoints/track.pt` | `track.pt` | +| `checkpoints/track_g1.onnx` | `ckpt/track_g1.onnx` | +| `checkpoints/track_g1.pt` | `ckpt/track_g1.pt` | +| `checkpoints/track_g1_neck_o6.onnx` | `ckpt/track_g1_neck_o6.onnx` | +| `checkpoints/track_g1_neck_o6.pt` | `ckpt/track_g1_neck_o6.pt` | | `archives/robot_assets.tar.gz` | `assets/robots/` (extracted) | | `archives/gmr_assets.tar.gz` | `teleopit/retargeting/gmr/assets/` (extracted) | | `archives/sample_bvh.tar.gz` | `data/sample_bvh/` (extracted) | @@ -110,7 +113,7 @@ Output goes to `data/modelscope_upload/`. ```bash # Model repo modelscope upload --repo-type model BingqianWu/Teleopit-models \ - data/modelscope_upload/checkpoints checkpoints + data/modelscope_upload/checkpoints checkpoints --sync modelscope upload --repo-type model BingqianWu/Teleopit-models \ data/modelscope_upload/archives archives @@ -119,6 +122,11 @@ modelscope upload --repo-type dataset BingqianWu/Teleopit-datasets \ data/modelscope_upload/data data ``` +The checkpoint upload intentionally uses `--sync`. Its deletion scope is the +remote `checkpoints/` directory, so obsolete policy names are removed without +touching `archives/`. Do not add `--sync` to the archive upload unless the local +staging directory contains every remote archive that must be retained. + ### Step 3: Tag Version Only the model repo supports tags (dataset repo does not). diff --git a/docs/docs/tutorials/bvh-sim2real.md b/docs/docs/tutorials/bvh-sim2real.md index 75c9a83b..55548494 100644 --- a/docs/docs/tutorials/bvh-sim2real.md +++ b/docs/docs/tutorials/bvh-sim2real.md @@ -36,7 +36,7 @@ For onboard deployment, `eth0` is usually correct. ```bash python scripts/run/run_sim2real.py \ - controller.policy_path=track.onnx \ + controller.policy_path=ckpt/track_g1.onnx \ real_robot.network_interface=enp130s0 \ input.bvh_file=data/sample_bvh/aiming1_subject1.bvh ``` diff --git a/docs/docs/tutorials/high-level-policy-sim2real.md b/docs/docs/tutorials/high-level-policy-sim2real.md index 65e438ae..7beb45c4 100644 --- a/docs/docs/tutorials/high-level-policy-sim2real.md +++ b/docs/docs/tutorials/high-level-policy-sim2real.md @@ -110,7 +110,7 @@ policy, and G1 network interface: ```bash python scripts/run/run_high_level_policy_sim2real.py \ - controller.policy_path=track.onnx \ + controller.policy_path=ckpt/track_g1_neck_o6.onnx \ high_level_policy.endpoint=tcp://192.168.1.10:5555 \ high_level_policy.task="pick up the object" \ real_robot.network_interface=eth0 diff --git a/docs/docs/tutorials/offline-sim2sim.md b/docs/docs/tutorials/offline-sim2sim.md index 5bef642a..99cf69b0 100644 --- a/docs/docs/tutorials/offline-sim2sim.md +++ b/docs/docs/tutorials/offline-sim2sim.md @@ -20,7 +20,7 @@ and the `robots gmr ckpt bvh` asset bundle. ```bash python scripts/run/run_sim.py \ - controller.policy_path=track.onnx \ + controller.policy_path=ckpt/track_g1.onnx \ input.bvh_file=data/sample_bvh/aiming1_subject1.bvh \ playback.keyboard.enabled=true ``` @@ -44,7 +44,7 @@ Open all views when you need to find where a bad result starts: ```bash python scripts/run/run_sim.py \ - controller.policy_path=track.onnx \ + controller.policy_path=ckpt/track_g1.onnx \ input.bvh_file=data/sample_bvh/aiming1_subject1.bvh \ viewers=all ``` @@ -64,13 +64,13 @@ You can also select views explicitly: ```bash # Only the physics result python scripts/run/run_sim.py \ - controller.policy_path=track.onnx \ + controller.policy_path=ckpt/track_g1.onnx \ input.bvh_file=data/sample_bvh/aiming1_subject1.bvh \ viewers=sim2sim # No windows; useful for a server or timing test python scripts/run/run_sim.py \ - controller.policy_path=track.onnx \ + controller.policy_path=ckpt/track_g1.onnx \ input.bvh_file=data/sample_bvh/aiming1_subject1.bvh \ viewers=none ``` @@ -83,7 +83,7 @@ For a LAFAN1-style file: ```bash python scripts/run/run_sim.py \ - controller.policy_path=track.onnx \ + controller.policy_path=ckpt/track_g1.onnx \ input.bvh_file=/path/to/motion.bvh \ input.bvh_format=lafan1 ``` @@ -92,7 +92,7 @@ For an `hc_mocap` file: ```bash python scripts/run/run_sim.py \ - controller.policy_path=track.onnx \ + controller.policy_path=ckpt/track_g1.onnx \ input.bvh_file=/path/to/motion.bvh \ input.bvh_format=hc_mocap ``` @@ -108,7 +108,7 @@ windows: ```bash MUJOCO_GL=egl python scripts/render/render_sim.py \ --bvh data/sample_bvh/aiming1_subject1.bvh \ - --policy track.onnx + --policy ckpt/track_g1.onnx ``` Add `--format hc_mocap` for that input format. The renderer writes synchronized diff --git a/docs/docs/tutorials/pico-sim2real.md b/docs/docs/tutorials/pico-sim2real.md index e3848993..a23983b6 100644 --- a/docs/docs/tutorials/pico-sim2real.md +++ b/docs/docs/tutorials/pico-sim2real.md @@ -36,6 +36,10 @@ Run Teleopit directly on the G1 onboard computer when you also need LinkerHand, OpenNeck, RealSense preview or data collection. The Pico headset must be able to reach the onboard computer. +When the onboard setup includes both O6 hands and OpenNeck, set the low-level +tracking policy to +`controller.policy_path=ckpt/track_g1_neck_o6.onnx`. + The G1 DDS interface is `eth0` by default. Apart from the network interface and the optional onboard hardware settings, the body-control configuration and launch command are the same as for an external host. @@ -47,7 +51,7 @@ Do not continue until all of these are true: - [VR Teleoperation in Simulation](pico-sim2sim) works reliably. - You installed the `pico4` profile and built `g1_bridge_sdk` as described in [Installation](../getting-started/installation). -- `track.onnx`, the robot files and GMR assets are present. +- `ckpt/track_g1.onnx`, the robot files and GMR assets are present. - The machine running Teleopit has a wired DDS connection to G1. - No other program is commanding the robot. @@ -58,7 +62,7 @@ external host, replace `enp130s0` with the interface reported by `ifconfig`: ```bash python scripts/run/standalone_standing.py \ - --policy track.onnx \ + --policy ckpt/track_g1.onnx \ --network-interface enp130s0 \ --dry-run ``` @@ -70,7 +74,7 @@ hardware setup: ```bash python scripts/run/standalone_standing.py \ - --policy track.onnx \ + --policy ckpt/track_g1.onnx \ --network-interface enp130s0 ``` @@ -84,7 +88,7 @@ External-host example: ```bash python scripts/run/run_sim2real.py \ --config-name pico4_sim2real \ - controller.policy_path=track.onnx \ + controller.policy_path=ckpt/track_g1.onnx \ real_robot.network_interface=enp130s0 ``` @@ -93,7 +97,7 @@ Onboard-computer example: ```bash python scripts/run/run_sim2real.py \ --config-name pico4_sim2real \ - controller.policy_path=track.onnx \ + controller.policy_path=ckpt/track_g1.onnx \ real_robot.network_interface=eth0 ``` @@ -161,7 +165,9 @@ hands.linkerhand_o6.left_can=can0 hands.linkerhand_o6.right_can=can1 ``` -Use `hands.mode=gripper` for trigger-based open and close. LinkerHand L6 is also +With `hands.mode=gripper`, hold the controller's side grip trigger to enable +that hand, then use the index trigger to control how far it closes. Releasing +the side grip trigger commands that hand to open. LinkerHand L6 is also supported through the matching `hands.linkerhand_l6.*` settings. ## Onboard Only: OpenNeck @@ -202,7 +208,7 @@ Recording requires a fresh RealSense RGB frame: ```bash python scripts/run/run_sim2real.py \ --config-name sim2real_record \ - controller.policy_path=track.onnx \ + controller.policy_path=ckpt/track_g1.onnx \ real_robot.network_interface=eth0 \ recording.task="walk forward" ``` diff --git a/docs/docs/tutorials/pico-sim2sim.md b/docs/docs/tutorials/pico-sim2sim.md index be3006b7..461035f5 100644 --- a/docs/docs/tutorials/pico-sim2sim.md +++ b/docs/docs/tutorials/pico-sim2sim.md @@ -69,7 +69,7 @@ python scripts/dev/test_pico_bridge.py \ ```bash python scripts/run/run_sim.py \ --config-name pico4_sim \ - controller.policy_path=track.onnx + controller.policy_path=ckpt/track_g1.onnx ``` The robot intentionally starts in `STANDING`; live body tracking does not take @@ -109,13 +109,13 @@ smaller layout when you no longer need all three: # Physics result only python scripts/run/run_sim.py \ --config-name pico4_sim \ - controller.policy_path=track.onnx \ + controller.policy_path=ckpt/track_g1.onnx \ viewers=sim2sim # Headless python scripts/run/run_sim.py \ --config-name pico4_sim \ - controller.policy_path=track.onnx \ + controller.policy_path=ckpt/track_g1.onnx \ viewers=none ``` @@ -126,7 +126,7 @@ To send the simulated `d435i_rgb` camera view back to the headset: ```bash python scripts/run/run_sim.py \ --config-name pico4_sim \ - controller.policy_path=track.onnx \ + controller.policy_path=ckpt/track_g1.onnx \ input.video.enabled=true ``` diff --git a/docs/docs/tutorials/standalone-standing.md b/docs/docs/tutorials/standalone-standing.md index 772b0e91..73e730cc 100644 --- a/docs/docs/tutorials/standalone-standing.md +++ b/docs/docs/tutorials/standalone-standing.md @@ -36,7 +36,7 @@ Use `--dry-run` first for timing checks without sending motor commands: ```bash python scripts/run/standalone_standing.py \ - --policy track.onnx \ + --policy ckpt/track_g1.onnx \ --network-interface enp130s0 \ --dry-run ``` @@ -47,7 +47,7 @@ Run the standing controller after confirming the network interface: ```bash python scripts/run/standalone_standing.py \ - --policy track.onnx \ + --policy ckpt/track_g1.onnx \ --network-interface enp130s0 ``` @@ -55,7 +55,7 @@ For onboard deployment, the interface is usually `eth0`: ```bash python scripts/run/standalone_standing.py \ - --policy track.onnx \ + --policy ckpt/track_g1.onnx \ --network-interface eth0 ``` @@ -67,7 +67,7 @@ this startup behavior: ```bash python scripts/run/standalone_standing.py \ - --policy track.onnx \ + --policy ckpt/track_g1.onnx \ --network-interface eth0 \ --kp-ramp-duration 2.0 \ --kp-ramp-floor-ratio 0.1 diff --git a/docs/docs/tutorials/training.md b/docs/docs/tutorials/training.md index e635e468..852cd273 100644 --- a/docs/docs/tutorials/training.md +++ b/docs/docs/tutorials/training.md @@ -58,7 +58,7 @@ The current `robots` asset bundle includes these ready-to-use examples: |-----------|-------| | `assets/robots/unitree_g1/g1_29dof.xml` | Base G1 model and the default | | `assets/robots/unitree_g1/g1_29dof_dex3.xml` | G1 with Dex3 hand geometry and inertial properties | -| `assets/robots/unitree_g1/g1_29dof_avp_o6.xml` | G1 with AVP active vision and O6 hand models | +| `assets/robots/unitree_g1/g1_29dof_neck_o6.xml` | G1 with neck active vision and O6 hand models | This table describes the models shipped in the current asset bundle; it is not a hard-coded model allowlist. Another XML can be passed when its joint and body @@ -134,7 +134,7 @@ Results are written as a text summary, JSON, per-clip CSV and per-rollout CSV. ```bash python train_mimic/scripts/save_onnx.py \ --checkpoint logs/rsl_rl/g1_general_tracking//model_30000.pt \ - --output track.onnx \ + --output ckpt/track_g1.onnx \ --history_length 10 ``` @@ -146,7 +146,7 @@ Test the export in the normal runtime: ```bash python scripts/run/run_sim.py \ - controller.policy_path=track.onnx \ + controller.policy_path=ckpt/track_g1.onnx \ input.bvh_file=data/sample_bvh/aiming1_subject1.bvh ``` diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/installation.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/installation.md index 2ec6d007..29238b23 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/installation.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/getting-started/installation.md @@ -87,8 +87,9 @@ python scripts/setup/download_assets.py \ --only robots gmr ckpt bvh ``` -推理资源包会把 `track.onnx`、G1 模型文件、GMR 文件和示例 BVH 放到代码默认查找 -的位置。完整文件清单和资源分组见[资产](../reference/resources/assets)。 +推理资源包会把 `track_g1` 和 `track_g1_neck_o6` 两组 ONNX/checkpoint 放到 `ckpt/`， +并把 G1 模型文件、GMR 文件和示例 BVH 放到代码默认查找的位置。完整文件清单和资源分组 +见[资产](../reference/resources/assets)。 ## 5. 连接真实 G1 前的额外安装 @@ -156,7 +157,7 @@ python -c "import train_mimic.tasks; print('training OK')" ```bash python scripts/run/run_sim.py \ - controller.policy_path=track.onnx \ + controller.policy_path=ckpt/track_g1.onnx \ input.bvh_file=data/sample_bvh/aiming1_subject1.bvh ``` diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/configuration/fields.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/configuration/fields.md index 4bdef41b..da79fcef 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/configuration/fields.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/configuration/fields.md @@ -190,7 +190,9 @@ dead-zone 或 pitch-gain 映射。 `hands.enabled=true` 要求 `input.provider=pico4`，并以本地 editable 方式安装 `third_party/linkerhand-python-sdk` 和 `third_party/somehand`。启用后，手控会在所有 sim2real 模式中保持生效。 -`gripper` 支持 `linkerhand_l6` 和 `linkerhand_o6`，会用 Pico trigger 在配置的张开和闭合姿态之间插值。 +`gripper` 支持 `linkerhand_l6` 和 `linkerhand_o6`。对应手柄侧面的握持扳机键（grip） +是安全使能键：保持按住时，食指扳机键（trigger）会在配置的张开和闭合姿态之间插值； +松开侧面握持扳机键会让该侧手张开。 `vr_hand_pose` 支持 `linkerhand_l6` 和 `linkerhand_o6`：手部 pose 消失时，对应侧会保持上一条命令； 所选手的速度会设为最大值；Teleopit 会先将 Pico 手部状态转成 21 个 landmarks， 再只通过 somehand 0.2.0 公开的 `somehand.api` 调用。 diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/resources/assets.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/resources/assets.md index eaee1d32..a5de0ab2 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/resources/assets.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/resources/assets.md @@ -12,14 +12,14 @@ Teleopit 的 Git 仓库只保存代码，不保存大型机器人 mesh、运控 - `assets/robots/` — canonical 机器人 XML/mesh - `teleopit/retargeting/gmr/assets/` — GMR 重定向资源、IK 配置和非 canonical 机器人描述 -- `data/`、checkpoint、缓存等生成产物 +- `data/`、`ckpt/`、checkpoint、缓存等生成产物 - 演示媒体（`assets/demo.gif`、`assets/demo.mp4`） ## 资源清单 | 资源组 | 下载后的路径 | 用途 | |--------|--------------|------| -| `ckpt` | `track.onnx`、`track.pt` | 可直接运行的推理模型和对应 PyTorch checkpoint | +| `ckpt` | `ckpt/track_g1.{onnx,pt}`、`ckpt/track_g1_neck_o6.{onnx,pt}` | 可直接运行的推理模型和对应 PyTorch checkpoint | | `robots` | `assets/robots/` 下的机器人 XML 变体与 mesh | 训练、MuJoCo 推理、GMR 和数据集 FK | | `gmr` | `teleopit/retargeting/gmr/assets/` | 动作重定向模型和 IK 配置 | | `bvh` | `data/sample_bvh/*.bvh` | 安装检查和仿真教程使用的示例动作 | @@ -31,10 +31,11 @@ Teleopit 的 Git 仓库只保存代码，不保存大型机器人 mesh、运控 |----------|------| | `assets/robots/unitree_g1/g1_29dof.xml` | 基础 G1 模型，也是默认值 | | `assets/robots/unitree_g1/g1_29dof_dex3.xml` | 带 Dex3 手部几何和惯性参数的 G1 | -| `assets/robots/unitree_g1/g1_29dof_avp_o6.xml` | 带 AVP 主动视觉和 O6 手部模型的 G1 | +| `assets/robots/unitree_g1/g1_29dof_neck_o6.xml` | 带颈部主动视觉和 O6 手部模型的 G1 | 默认值不是模型白名单。训练可以通过 `--robot_xml` 选择其他与任务兼容的 XML。GMR -资源目录中的 XML 属于对应的重定向配置，与运行时机器人资源包是两套不同资源。 +资源目录中的 XML 属于对应的重定向配置，与运行时机器人资源包是两套不同资源。基础 +模型使用 `track_g1` 模型对，颈部加 O6 模型使用 `track_g1_neck_o6` 模型对。 ## 远程仓库 @@ -56,7 +57,7 @@ Teleopit 的 Git 仓库只保存代码，不保存大型机器人 mesh、运控 | 组 | 仓库 | 远端路径 | |----|------|---------| -| `ckpt` | Teleopit-models | `checkpoints/track.onnx`、`checkpoints/track.pt` | +| `ckpt` | Teleopit-models | `checkpoints/track_g1.{onnx,pt}`、`checkpoints/track_g1_neck_o6.{onnx,pt}` | | `robots` | Teleopit-models | `archives/robot_assets.tar.gz` | | `gmr` | Teleopit-models | `archives/gmr_assets.tar.gz` | | `bvh` | Teleopit-models | `archives/sample_bvh.tar.gz` | @@ -84,8 +85,10 @@ python scripts/setup/download_assets.py --source huggingface | 远端路径 | 本地路径 | |---------|---------| -| `checkpoints/track.onnx` | `track.onnx` | -| `checkpoints/track.pt` | `track.pt` | +| `checkpoints/track_g1.onnx` | `ckpt/track_g1.onnx` | +| `checkpoints/track_g1.pt` | `ckpt/track_g1.pt` | +| `checkpoints/track_g1_neck_o6.onnx` | `ckpt/track_g1_neck_o6.onnx` | +| `checkpoints/track_g1_neck_o6.pt` | `ckpt/track_g1_neck_o6.pt` | | `archives/robot_assets.tar.gz` | `assets/robots/`（自动解压） | | `archives/gmr_assets.tar.gz` | `teleopit/retargeting/gmr/assets/`（自动解压） | | `archives/sample_bvh.tar.gz` | `data/sample_bvh/`（自动解压） | @@ -107,7 +110,7 @@ python scripts/setup/prepare_modelscope_assets.py --only data ```bash # 模型仓库 modelscope upload --repo-type model BingqianWu/Teleopit-models \ - data/modelscope_upload/checkpoints checkpoints + data/modelscope_upload/checkpoints checkpoints --sync modelscope upload --repo-type model BingqianWu/Teleopit-models \ data/modelscope_upload/archives archives @@ -116,6 +119,10 @@ modelscope upload --repo-type dataset BingqianWu/Teleopit-datasets \ data/modelscope_upload/data data ``` +checkpoint 上传有意使用 `--sync`。它只会在远端 `checkpoints/` 目录内删除本地不存在的 +旧模型名，不会影响 `archives/`。除非本地 staging 中包含所有需要保留的远端归档，否则 +不要给归档上传命令添加 `--sync`。 + ### 第三步：打版本 tag ModelScope 仅模型仓库支持 tag，数据集仓库不支持。 diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/bvh-sim2real.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/bvh-sim2real.md index 59acadaa..7ad1a507 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/bvh-sim2real.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/bvh-sim2real.md @@ -34,7 +34,7 @@ real_robot.network_interface=enp130s0 ```bash python scripts/run/run_sim2real.py \ - controller.policy_path=track.onnx \ + controller.policy_path=ckpt/track_g1.onnx \ real_robot.network_interface=enp130s0 \ input.bvh_file=data/sample_bvh/aiming1_subject1.bvh ``` diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md index ad1ba908..79fb0055 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md @@ -103,7 +103,7 @@ sudo /usr/sbin/ip link set can1 up type can bitrate 1000000 ```bash python scripts/run/run_high_level_policy_sim2real.py \ - controller.policy_path=track.onnx \ + controller.policy_path=ckpt/track_g1_neck_o6.onnx \ high_level_policy.endpoint=tcp://192.168.1.10:5555 \ high_level_policy.task="pick up the object" \ real_robot.network_interface=eth0 diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/offline-sim2sim.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/offline-sim2sim.md index 3f4cff6f..b33ef0ff 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/offline-sim2sim.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/offline-sim2sim.md @@ -19,7 +19,7 @@ sidebar_position: 1 ```bash python scripts/run/run_sim.py \ - controller.policy_path=track.onnx \ + controller.policy_path=ckpt/track_g1.onnx \ input.bvh_file=data/sample_bvh/aiming1_subject1.bvh \ playback.keyboard.enabled=true ``` @@ -42,7 +42,7 @@ python scripts/run/run_sim.py \ ```bash python scripts/run/run_sim.py \ - controller.policy_path=track.onnx \ + controller.policy_path=ckpt/track_g1.onnx \ input.bvh_file=data/sample_bvh/aiming1_subject1.bvh \ viewers=all ``` @@ -61,13 +61,13 @@ python scripts/run/run_sim.py \ ```bash # 只看物理仿真结果 python scripts/run/run_sim.py \ - controller.policy_path=track.onnx \ + controller.policy_path=ckpt/track_g1.onnx \ input.bvh_file=data/sample_bvh/aiming1_subject1.bvh \ viewers=sim2sim # 不打开窗口，适合服务器或时序测试 python scripts/run/run_sim.py \ - controller.policy_path=track.onnx \ + controller.policy_path=ckpt/track_g1.onnx \ input.bvh_file=data/sample_bvh/aiming1_subject1.bvh \ viewers=none ``` @@ -80,7 +80,7 @@ LAFAN1 格式： ```bash python scripts/run/run_sim.py \ - controller.policy_path=track.onnx \ + controller.policy_path=ckpt/track_g1.onnx \ input.bvh_file=/path/to/motion.bvh \ input.bvh_format=lafan1 ``` @@ -89,7 +89,7 @@ python scripts/run/run_sim.py \ ```bash python scripts/run/run_sim.py \ - controller.policy_path=track.onnx \ + controller.policy_path=ckpt/track_g1.onnx \ input.bvh_file=/path/to/motion.bvh \ input.bvh_format=hc_mocap ``` @@ -104,7 +104,7 @@ Teleopit 不会猜测未知的骨架布局。一个文件即使是合法 BVH， ```bash MUJOCO_GL=egl python scripts/render/render_sim.py \ --bvh data/sample_bvh/aiming1_subject1.bvh \ - --policy track.onnx + --policy ckpt/track_g1.onnx ``` `hc_mocap` 输入需要再加 `--format hc_mocap`。渲染脚本会输出同步的 `mocap`、 diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md index b2387edd..5d1c4f34 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md @@ -33,6 +33,9 @@ ifconfig 如果还需要 LinkerHand、OpenNeck、RealSense 画面或数据采集，请直接在 G1 机载电脑 上运行 Teleopit。Pico 头显需要能够访问机载电脑。 +机载配置同时使用 O6 双手和 OpenNeck 时，请将底层运控策略设为 +`controller.policy_path=ckpt/track_g1_neck_o6.onnx`。 + G1 DDS 默认使用 `eth0`。除了网络接口和可选机载硬件配置之外，全身控制的配置和启动 命令与外部主机部署相同。 @@ -43,7 +46,7 @@ G1 DDS 默认使用 `eth0`。除了网络接口和可选机载硬件配置之外 - [在仿真中进行 VR 遥操](pico-sim2sim)已经稳定运行； - 已按照[安装](../getting-started/installation)安装 `pico4` 依赖并编译 `g1_bridge_sdk`； -- 已准备好 `track.onnx`、机器人文件和 GMR 资源； +- 已准备好 `ckpt/track_g1.onnx`、机器人文件和 GMR 资源； - 运行 Teleopit 的设备已经通过有线 DDS 网络连接 G1； - 没有其他程序正在控制机器人。 @@ -54,7 +57,7 @@ G1 DDS 默认使用 `eth0`。除了网络接口和可选机载硬件配置之外 ```bash python scripts/run/standalone_standing.py \ - --policy track.onnx \ + --policy ckpt/track_g1.onnx \ --network-interface enp130s0 \ --dry-run ``` @@ -65,7 +68,7 @@ Dry run 成功后，在确保硬件安全的情况下去掉 `--dry-run` 再运 ```bash python scripts/run/standalone_standing.py \ - --policy track.onnx \ + --policy ckpt/track_g1.onnx \ --network-interface enp130s0 ``` @@ -79,7 +82,7 @@ python scripts/run/standalone_standing.py \ ```bash python scripts/run/run_sim2real.py \ --config-name pico4_sim2real \ - controller.policy_path=track.onnx \ + controller.policy_path=ckpt/track_g1.onnx \ real_robot.network_interface=enp130s0 ``` @@ -88,7 +91,7 @@ python scripts/run/run_sim2real.py \ ```bash python scripts/run/run_sim2real.py \ --config-name pico4_sim2real \ - controller.policy_path=track.onnx \ + controller.policy_path=ckpt/track_g1.onnx \ real_robot.network_interface=eth0 ``` @@ -149,8 +152,10 @@ hands.linkerhand_o6.left_can=can0 hands.linkerhand_o6.right_can=can1 ``` -使用 `hands.mode=gripper` 可以通过扳机键控制开合。LinkerHand L6 也受支持，对应参数 -为 `hands.linkerhand_l6.*`。 +使用 `hands.mode=gripper` 时，需要按住对应手柄侧面的握持扳机键（grip）才会启用 +该侧手部控制；保持按住后，再用食指扳机键（trigger）控制闭合程度。松开侧面握持 +扳机键会让该侧手张开。LinkerHand L6 也受支持，对应参数为 +`hands.linkerhand_l6.*`。 ## 仅机载：OpenNeck @@ -188,7 +193,7 @@ input.video.device=<可选的-realsense-序列号> ```bash python scripts/run/run_sim2real.py \ --config-name sim2real_record \ - controller.policy_path=track.onnx \ + controller.policy_path=ckpt/track_g1.onnx \ real_robot.network_interface=eth0 \ recording.task="向前走" ``` diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2sim.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2sim.md index cbc5de46..ce3f6276 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2sim.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2sim.md @@ -63,7 +63,7 @@ python scripts/dev/test_pico_bridge.py \ ```bash python scripts/run/run_sim.py \ --config-name pico4_sim \ - controller.policy_path=track.onnx + controller.policy_path=ckpt/track_g1.onnx ``` 机器人会有意从 `STANDING` 开始；只有操作者主动切换后，实时身体追踪才会接管。 @@ -98,13 +98,13 @@ Pico 仿真默认会打开动捕、重定向和物理仿真三个视图。不再 # 只看物理仿真结果 python scripts/run/run_sim.py \ --config-name pico4_sim \ - controller.policy_path=track.onnx \ + controller.policy_path=ckpt/track_g1.onnx \ viewers=sim2sim # 不打开窗口 python scripts/run/run_sim.py \ --config-name pico4_sim \ - controller.policy_path=track.onnx \ + controller.policy_path=ckpt/track_g1.onnx \ viewers=none ``` @@ -115,7 +115,7 @@ python scripts/run/run_sim.py \ ```bash python scripts/run/run_sim.py \ --config-name pico4_sim \ - controller.policy_path=track.onnx \ + controller.policy_path=ckpt/track_g1.onnx \ input.video.enabled=true ``` diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/standalone-standing.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/standalone-standing.md index 54bb06e6..02ba0151 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/standalone-standing.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/standalone-standing.md @@ -34,7 +34,7 @@ bash scripts/setup/setup_g1_bridge.sh ```bash python scripts/run/standalone_standing.py \ - --policy track.onnx \ + --policy ckpt/track_g1.onnx \ --network-interface enp130s0 \ --dry-run ``` @@ -45,7 +45,7 @@ python scripts/run/standalone_standing.py \ ```bash python scripts/run/standalone_standing.py \ - --policy track.onnx \ + --policy ckpt/track_g1.onnx \ --network-interface enp130s0 ``` @@ -53,7 +53,7 @@ python scripts/run/standalone_standing.py \ ```bash python scripts/run/standalone_standing.py \ - --policy track.onnx \ + --policy ckpt/track_g1.onnx \ --network-interface eth0 ``` @@ -64,7 +64,7 @@ standalone standing 复用 sim2real standing 组件：`UnitreeG1Robot`、 ```bash python scripts/run/standalone_standing.py \ - --policy track.onnx \ + --policy ckpt/track_g1.onnx \ --network-interface eth0 \ --kp-ramp-duration 2.0 \ --kp-ramp-floor-ratio 0.1 diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/training.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/training.md index 2ea11a26..98c44861 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/training.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/training.md @@ -55,7 +55,7 @@ assets/robots/unitree_g1/g1_29dof.xml |----------|------| | `assets/robots/unitree_g1/g1_29dof.xml` | 基础 G1 模型，也是默认值 | | `assets/robots/unitree_g1/g1_29dof_dex3.xml` | 带 Dex3 手部几何和惯性参数的 G1 | -| `assets/robots/unitree_g1/g1_29dof_avp_o6.xml` | 带 AVP 主动视觉和 O6 手部模型的 G1 | +| `assets/robots/unitree_g1/g1_29dof_neck_o6.xml` | 带颈部主动视觉和 O6 手部模型的 G1 | 这个表只是当前资源包随附的模型示例，不是写死的模型白名单。只要关节和刚体定义与所选 训练任务配置及数据集兼容，也可以传入其他模型 XML。 @@ -126,7 +126,7 @@ Benchmark 会对每个长度足够的 clip 执行一次确定性的 10 秒 rollo ```bash python train_mimic/scripts/save_onnx.py \ --checkpoint logs/rsl_rl/g1_general_tracking//model_30000.pt \ - --output track.onnx \ + --output ckpt/track_g1.onnx \ --history_length 10 ``` @@ -137,7 +137,7 @@ python train_mimic/scripts/save_onnx.py \ ```bash python scripts/run/run_sim.py \ - controller.policy_path=track.onnx \ + controller.policy_path=ckpt/track_g1.onnx \ input.bvh_file=data/sample_bvh/aiming1_subject1.bvh ``` diff --git a/scripts/dev/bench_policy_onnx.py b/scripts/dev/bench_policy_onnx.py index b40b7c2a..9a018c95 100644 --- a/scripts/dev/bench_policy_onnx.py +++ b/scripts/dev/bench_policy_onnx.py @@ -4,9 +4,9 @@ It does not require MuJoCo, robot hardware, GMR assets, or Pico input. Examples: - python scripts/dev/bench_policy_onnx.py --policy track.onnx - python scripts/dev/bench_policy_onnx.py --policy track.onnx --runs 20000 --device cpu - python scripts/dev/bench_policy_onnx.py --policy track.onnx --mode direct + python scripts/dev/bench_policy_onnx.py --policy ckpt/track_g1.onnx + python scripts/dev/bench_policy_onnx.py --policy ckpt/track_g1.onnx --runs 20000 --device cpu + python scripts/dev/bench_policy_onnx.py --policy ckpt/track_g1.onnx --mode direct """ from __future__ import annotations diff --git a/teleopit/retargeting/gmr/params.py b/teleopit/retargeting/gmr/params.py index 1110eef0..1be0b883 100644 --- a/teleopit/retargeting/gmr/params.py +++ b/teleopit/retargeting/gmr/params.py @@ -1,8 +1,8 @@ from pathlib import Path from teleopit.runtime.assets import ( - UNITREE_G1_AVP_O6_XML, UNITREE_G1_DEX3_XML, + UNITREE_G1_NECK_O6_XML, UNITREE_G1_XML, ) @@ -19,7 +19,7 @@ def _resolve_path(relative_path): ROBOT_XML_DICT = { "unitree_g1": UNITREE_G1_XML, "unitree_g1_with_hands": UNITREE_G1_DEX3_XML, - "unitree_g1_avp_o6": UNITREE_G1_AVP_O6_XML, + "unitree_g1_neck_o6": UNITREE_G1_NECK_O6_XML, "unitree_h1": _resolve_path("assets/unitree_h1/h1.xml"), "unitree_h1_2": _resolve_path("assets/unitree_h1_2/h1_2_handless.xml"), "booster_t1": _resolve_path("assets/booster_t1/T1_serial.xml"), @@ -43,7 +43,7 @@ def _resolve_path(relative_path): "smplx": { "unitree_g1": _resolve_path("ik_configs/smplx_to_g1.json"), "unitree_g1_with_hands": _resolve_path("ik_configs/smplx_to_g1.json"), - "unitree_g1_avp_o6": _resolve_path("ik_configs/smplx_to_g1.json"), + "unitree_g1_neck_o6": _resolve_path("ik_configs/smplx_to_g1.json"), "unitree_h1": _resolve_path("ik_configs/smplx_to_h1.json"), "unitree_h1_2": _resolve_path("ik_configs/smplx_to_h1_2.json"), "booster_t1": _resolve_path("ik_configs/smplx_to_t1.json"), @@ -63,7 +63,7 @@ def _resolve_path(relative_path): "bvh_lafan1": { "unitree_g1": _resolve_path("ik_configs/bvh_lafan1_to_g1.json"), "unitree_g1_with_hands": _resolve_path("ik_configs/bvh_lafan1_to_g1.json"), - "unitree_g1_avp_o6": _resolve_path("ik_configs/bvh_lafan1_to_g1.json"), + "unitree_g1_neck_o6": _resolve_path("ik_configs/bvh_lafan1_to_g1.json"), "booster_t1_29dof": _resolve_path("ik_configs/bvh_lafan1_to_t1_29dof.json"), "fourier_n1": _resolve_path("ik_configs/bvh_lafan1_to_n1.json"), "stanford_toddy": _resolve_path("ik_configs/bvh_lafan1_to_toddy.json"), @@ -83,7 +83,7 @@ def _resolve_path(relative_path): "fbx": { "unitree_g1": _resolve_path("ik_configs/fbx_to_g1.json"), "unitree_g1_with_hands": _resolve_path("ik_configs/fbx_to_g1.json"), - "unitree_g1_avp_o6": _resolve_path("ik_configs/fbx_to_g1.json"), + "unitree_g1_neck_o6": _resolve_path("ik_configs/fbx_to_g1.json"), }, "fbx_offline": { "unitree_g1": _resolve_path("ik_configs/fbx_offline_to_g1.json"), @@ -97,7 +97,7 @@ def _resolve_path(relative_path): ROBOT_BASE_DICT = { "unitree_g1": "pelvis", "unitree_g1_with_hands": "pelvis", - "unitree_g1_avp_o6": "pelvis", + "unitree_g1_neck_o6": "pelvis", "unitree_h1": "pelvis", "unitree_h1_2": "pelvis", "booster_t1": "Waist", @@ -119,7 +119,7 @@ def _resolve_path(relative_path): VIEWER_CAM_DISTANCE_DICT = { "unitree_g1": 2.0, "unitree_g1_with_hands": 2.0, - "unitree_g1_avp_o6": 2.0, + "unitree_g1_neck_o6": 2.0, "unitree_h1": 3.0, "unitree_h1_2": 3.0, "booster_t1": 2.0, diff --git a/teleopit/runtime/assets.py b/teleopit/runtime/assets.py index 00d2e8e7..66041dbb 100644 --- a/teleopit/runtime/assets.py +++ b/teleopit/runtime/assets.py @@ -8,7 +8,7 @@ GMR_ASSETS_ROOT = PROJECT_ROOT / "teleopit" / "retargeting" / "gmr" / "assets" UNITREE_G1_XML = ROBOT_ASSETS_ROOT / "unitree_g1" / "g1_29dof.xml" UNITREE_G1_DEX3_XML = ROBOT_ASSETS_ROOT / "unitree_g1" / "g1_29dof_dex3.xml" -UNITREE_G1_AVP_O6_XML = ROBOT_ASSETS_ROOT / "unitree_g1" / "g1_29dof_avp_o6.xml" +UNITREE_G1_NECK_O6_XML = ROBOT_ASSETS_ROOT / "unitree_g1" / "g1_29dof_neck_o6.xml" UNITREE_G1_MJLAB_XML = UNITREE_G1_XML diff --git a/teleopit/runtime/external_assets.py b/teleopit/runtime/external_assets.py index 1f63c929..52c97f4d 100644 --- a/teleopit/runtime/external_assets.py +++ b/teleopit/runtime/external_assets.py @@ -21,8 +21,18 @@ class AssetEntry: ASSET_GROUPS: dict[str, list[AssetEntry]] = { "ckpt": [ - AssetEntry("checkpoints/track.onnx", "track.onnx", repo="model"), - AssetEntry("checkpoints/track.pt", "track.pt", repo="model"), + AssetEntry("checkpoints/track_g1.onnx", "ckpt/track_g1.onnx", repo="model"), + AssetEntry("checkpoints/track_g1.pt", "ckpt/track_g1.pt", repo="model"), + AssetEntry( + "checkpoints/track_g1_neck_o6.onnx", + "ckpt/track_g1_neck_o6.onnx", + repo="model", + ), + AssetEntry( + "checkpoints/track_g1_neck_o6.pt", + "ckpt/track_g1_neck_o6.pt", + repo="model", + ), ], "gmr": [ AssetEntry( diff --git a/tests/test_download_assets.py b/tests/test_download_assets.py index 9b7a0f26..b7f48782 100644 --- a/tests/test_download_assets.py +++ b/tests/test_download_assets.py @@ -56,6 +56,20 @@ def test_robot_asset_group_uses_archive_layout() -> None: assert entries[0].mode == "extract" +def test_checkpoint_asset_group_uses_named_ckpt_directory() -> None: + entries = ASSET_GROUPS["ckpt"] + + assert [(entry.remote_path, entry.local_path) for entry in entries] == [ + ("checkpoints/track_g1.onnx", "ckpt/track_g1.onnx"), + ("checkpoints/track_g1.pt", "ckpt/track_g1.pt"), + ( + "checkpoints/track_g1_neck_o6.onnx", + "ckpt/track_g1_neck_o6.onnx", + ), + ("checkpoints/track_g1_neck_o6.pt", "ckpt/track_g1_neck_o6.pt"), + ] + + def test_data_asset_group_downloads_only_hdf5_shards() -> None: entries = ASSET_GROUPS["data"] From 89c8f4164942be3c0f3795a5a75b2775630f5643 Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Thu, 30 Jul 2026 17:01:24 +0800 Subject: [PATCH 51/59] Bump somehand to 0.3.0 --- AGENTS.md | 2 +- docs/docs/reference/companion-projects.md | 2 +- docs/docs/reference/configuration/fields.md | 6 +++--- .../current/reference/companion-projects.md | 2 +- .../current/reference/configuration/fields.md | 6 +++--- teleopit/sim2real/hands/linkerhand_l6.py | 10 +++++----- tests/test_dexterous_hand.py | 2 +- third_party/somehand | 2 +- 8 files changed, 16 insertions(+), 16 deletions(-) diff --git a/AGENTS.md b/AGENTS.md index e415a9b6..87d8b795 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -168,7 +168,7 @@ target_dof_pos = clip(action, -10, 10) × action_scale + default_dof_pos - Episodes interrupted before their `episodes.jsonl` entry is committed are discarded on the next recording-worker startup and do not consume an episode index - Review saved sim2real recordings with `scripts/view/view_recording.py`; it validates manifest/HDF5/MP4 alignment and synchronizes camera video, an observed-vs-reference MuJoCo overlay, joint/mode plots, and optional hand/neck signals; because measured root XYZ is not recorded, the observed robot is anchored to the reference root position - `gripper` mode reuses `Pico4InputProvider.get_controller_snapshot()` and supports LinkerHand L6 and O6; the side grip trigger is a deadman enable, the index trigger controls closure while it is held, and releasing the side grip opens that hand -- `vr_hand_pose` mode reuses `Pico4InputProvider.get_hand_snapshot()` and somehand 0.2.0 public `somehand.api` for continuous Pico hand-pose retargeting; do not start a second `PicoBridge` for hand control +- `vr_hand_pose` mode reuses `Pico4InputProvider.get_hand_snapshot()` and somehand 0.3.0 public `somehand.api` for continuous Pico hand-pose retargeting; do not start a second `PicoBridge` for hand control - Teleopit owns Pico 26-joint hand-state to 21-landmark conversion; do not import `somehand.pico_input` - LinkerHand O6 supports `hands.mode=gripper|vr_hand_pose`; its default `close_pose` is `[86, 73, 118, 111, 110, 111]` - L6 `gripper` mode uses the configured `hands.linkerhand_l6.speed` (default `[50]*6`); O6 `gripper` mode uses `hands.linkerhand_o6.speed` (default `[255]*6`); `vr_hand_pose` always sets LinkerHand L6/O6 speed to `[255]*6` diff --git a/docs/docs/reference/companion-projects.md b/docs/docs/reference/companion-projects.md index 6beb566b..a43d26aa 100644 --- a/docs/docs/reference/companion-projects.md +++ b/docs/docs/reference/companion-projects.md @@ -37,7 +37,7 @@ teleoperation, the standalone standing check, and host-policy deployment. somehand provides configurable human-to-robot hand retargeting. Teleopit pins the compatible source as the `third_party/somehand` Git submodule and uses its -0.2.0 public `somehand.api` surface. +0.3.0 public `somehand.api` surface. In `hands.mode=vr_hand_pose`, Teleopit converts PICO's 26-joint hand state to 21 landmarks, calls somehand for continuous retargeting, and sends the result diff --git a/docs/docs/reference/configuration/fields.md b/docs/docs/reference/configuration/fields.md index 8b2a4415..5c1d236f 100644 --- a/docs/docs/reference/configuration/fields.md +++ b/docs/docs/reference/configuration/fields.md @@ -183,7 +183,7 @@ trigger interpolates between the configured open and close poses; releasing the side grip trigger commands that hand to open. `vr_hand_pose` is supported by `linkerhand_l6` and `linkerhand_o6`: missing hand pose holds the last command for that side, the selected hand speed is set to the maximum, and Teleopit -converts Pico hand state to 21 landmarks before calling somehand 0.2.0 through +converts Pico hand state to 21 landmarks before calling somehand 0.3.0 through `somehand.api` only. | Field | Description | Default | @@ -200,8 +200,8 @@ converts Pico hand state to 21 landmarks before calling somehand 0.2.0 through | `hands.linkerhand_o6.left_can` / `right_can` | CAN channels for each O6 hand | `can0` / `can1` | | `hands.linkerhand_o6.speed` | O6 speed used by `gripper`; `vr_hand_pose` overrides this to maximum speed | see config | | `hands.linkerhand_o6.open_pose` / `close_pose` | Six-value O6 open/closed poses | see config | -| `hands.somehand.l6_config_path` | Official somehand 0.2.0 bi-hand L6 config used by L6 `vr_hand_pose` | see config | -| `hands.somehand.o6_config_path` | Official somehand 0.2.0 bi-hand O6 config used by O6 `vr_hand_pose` | see config | +| `hands.somehand.l6_config_path` | Official somehand 0.3.0 bi-hand L6 config used by L6 `vr_hand_pose` | see config | +| `hands.somehand.o6_config_path` | Official somehand 0.3.0 bi-hand O6 config used by O6 `vr_hand_pose` | see config | | `hands.somehand.rate_hz` | Low-latency `vr_hand_pose` command rate in Hz | `60.0` | | `hands.somehand.max_iterations` | somehand solver iteration cap for `vr_hand_pose` | `12` | | `hands.somehand.temporal_filter_alpha` | somehand input landmark smoothing alpha; `1.0` disables smoothing delay | `1.0` | diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/companion-projects.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/companion-projects.md index 8d328416..3f2bae22 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/companion-projects.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/companion-projects.md @@ -31,7 +31,7 @@ bridge 读取关节状态、基座方向、角速度和无线遥控器输入， ## somehand somehand 提供可配置的人手到机器人手部动作重定向。Teleopit 把兼容源码固定为 -`third_party/somehand` Git submodule，并使用 0.2.0 的公共 `somehand.api`。 +`third_party/somehand` Git submodule，并使用 0.3.0 的公共 `somehand.api`。 在 `hands.mode=vr_hand_pose` 下，Teleopit 把 PICO 的 26 关节手部状态转换为 21 个 landmark，调用 somehand 连续重定向，再把结果发送给 LinkerHand L6 或 O6。Pico diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/configuration/fields.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/configuration/fields.md index da79fcef..b2e43afb 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/configuration/fields.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/configuration/fields.md @@ -195,7 +195,7 @@ dead-zone 或 pitch-gain 映射。 松开侧面握持扳机键会让该侧手张开。 `vr_hand_pose` 支持 `linkerhand_l6` 和 `linkerhand_o6`：手部 pose 消失时，对应侧会保持上一条命令； 所选手的速度会设为最大值；Teleopit 会先将 Pico 手部状态转成 21 个 landmarks， -再只通过 somehand 0.2.0 公开的 `somehand.api` 调用。 +再只通过 somehand 0.3.0 公开的 `somehand.api` 调用。 | 字段 | 说明 | 默认值 | |---|---|---| @@ -213,8 +213,8 @@ dead-zone 或 pitch-gain 映射。 | `hands.linkerhand_o6.left_can` / `right_can` | 左右 O6 手 CAN 通道 | `can0` / `can1` | | `hands.linkerhand_o6.speed` | `gripper` 使用的 O6 速度；`vr_hand_pose` 会覆盖为最大速度 | 见配置 | | `hands.linkerhand_o6.open_pose` / `close_pose` | O6 的 6 维张开/闭合姿态 | 见配置 | -| `hands.somehand.l6_config_path` | L6 `vr_hand_pose` 使用的 somehand 双手 L6 配置 | 见配置 | -| `hands.somehand.o6_config_path` | O6 `vr_hand_pose` 使用的 somehand 双手 O6 配置 | 见配置 | +| `hands.somehand.l6_config_path` | L6 `vr_hand_pose` 使用的 somehand 0.3.0 官方双手 L6 配置 | 见配置 | +| `hands.somehand.o6_config_path` | O6 `vr_hand_pose` 使用的 somehand 0.3.0 官方双手 O6 配置 | 见配置 | | `hands.somehand.rate_hz` | 低延时 `vr_hand_pose` 命令频率（Hz） | `60.0` | | `hands.somehand.max_iterations` | `vr_hand_pose` 的 somehand solver 迭代上限 | `12` | | `hands.somehand.temporal_filter_alpha` | somehand 输入 landmarks 平滑 alpha；`1.0` 表示关闭平滑延时 | `1.0` | diff --git a/teleopit/sim2real/hands/linkerhand_l6.py b/teleopit/sim2real/hands/linkerhand_l6.py index 1525fd03..fb5e66be 100644 --- a/teleopit/sim2real/hands/linkerhand_l6.py +++ b/teleopit/sim2real/hands/linkerhand_l6.py @@ -228,7 +228,7 @@ def __init__( self._active = False def start(self) -> None: - _require_somehand_020() + _require_somehand_030() from somehand.api import HandFrame, RetargetingEngine, load_bihand_config, load_retargeting_config config_path = _resolve_project_path(self.config_path) @@ -373,13 +373,13 @@ def trigger_to_pose( return pose -def _require_somehand_020() -> None: +def _require_somehand_030() -> None: try: installed = version("somehand") except PackageNotFoundError as exc: - raise ImportError("somehand==0.2.0 is required for hands.mode=vr_hand_pose") from exc - if installed != "0.2.0": - raise ImportError(f"somehand==0.2.0 is required for hands.mode=vr_hand_pose, found {installed}") + raise ImportError("somehand==0.3.0 is required for hands.mode=vr_hand_pose") from exc + if installed != "0.3.0": + raise ImportError(f"somehand==0.3.0 is required for hands.mode=vr_hand_pose, found {installed}") def _resolve_l6_joint_index(joint_index: dict[str, int], semantic_name: str, *, side: str) -> int: diff --git a/tests/test_dexterous_hand.py b/tests/test_dexterous_hand.py index 70c1588d..d9333057 100644 --- a/tests/test_dexterous_hand.py +++ b/tests/test_dexterous_hand.py @@ -308,7 +308,7 @@ def load_retargeting_config(path: str): somehand_pkg.__path__ = [] monkeypatch.setitem(sys.modules, "somehand", somehand_pkg) monkeypatch.setitem(sys.modules, "somehand.api", somehand_api) - monkeypatch.setattr("teleopit.sim2real.hands.linkerhand_l6.version", lambda name: "0.2.0") + monkeypatch.setattr("teleopit.sim2real.hands.linkerhand_l6.version", lambda name: "0.3.0") monkeypatch.setattr("teleopit.sim2real.hands.linkerhand_l6._resolve_project_path", lambda path: SimpleNamespace(exists=lambda: True)) monkeypatch.setattr( "teleopit.sim2real.hands.linkerhand_l6._load_linkerhand_mapping_module", diff --git a/third_party/somehand b/third_party/somehand index 0e9adba4..f0a6b42e 160000 --- a/third_party/somehand +++ b/third_party/somehand @@ -1 +1 @@ -Subproject commit 0e9adba4e193540279f8e5803a9339a49666499a +Subproject commit f0a6b42e151ca10a6eec3e24c24c10cd13c40314 From e6ee6fb80e50a80f397af11b0db131ff282ac220 Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Thu, 30 Jul 2026 17:51:52 +0800 Subject: [PATCH 52/59] feat: expose hand and neck state readback --- teleopit/sim2real/hands/base.py | 2 ++ teleopit/sim2real/hands/linkerhand_l6.py | 11 +++++++++++ teleopit/sim2real/hands/linkerhand_o6.py | 11 +++++++++++ teleopit/sim2real/neck/openneck.py | 15 +++++++++++++-- tests/test_active_neck.py | 16 ++++++++++++++++ tests/test_dexterous_hand.py | 8 ++++++++ 6 files changed, 61 insertions(+), 2 deletions(-) diff --git a/teleopit/sim2real/hands/base.py b/teleopit/sim2real/hands/base.py index 76257789..b8506444 100644 --- a/teleopit/sim2real/hands/base.py +++ b/teleopit/sim2real/hands/base.py @@ -18,6 +18,8 @@ class HandPoseCommand: class HandDevice(Protocol): def connect(self) -> None: ... + def get_state(self, side: str) -> tuple[float, ...]: ... + def send_pose(self, side: str, pose: Sequence[int], *, force: bool = False, reason: str = "") -> None: ... def open_all(self, *, force: bool = False, reason: str = "") -> None: ... diff --git a/teleopit/sim2real/hands/linkerhand_l6.py b/teleopit/sim2real/hands/linkerhand_l6.py index fb5e66be..44f5624c 100644 --- a/teleopit/sim2real/hands/linkerhand_l6.py +++ b/teleopit/sim2real/hands/linkerhand_l6.py @@ -130,6 +130,17 @@ def connect(self) -> None: raise self.open_all(force=True, reason="startup") + def get_state(self, side: str) -> tuple[float, ...]: + if side not in self.config.sides: + raise ValueError(f"LinkerHand L6 side is not configured: {side!r}") + hand = self._hands.get(side) + if hand is None: + raise RuntimeError(f"LinkerHand L6 {side} is not connected") + state = tuple(float(value) for value in hand.get_state()) + if len(state) != 6: + raise RuntimeError(f"LinkerHand L6 {side} state must contain 6 values, got {len(state)}") + return state + def send_pose(self, side: str, pose: Sequence[int], *, force: bool = False, reason: str = "") -> None: del reason next_pose = tuple(_uint8(value, f"{side}.pose") for value in pose) diff --git a/teleopit/sim2real/hands/linkerhand_o6.py b/teleopit/sim2real/hands/linkerhand_o6.py index 2697d605..7f593113 100644 --- a/teleopit/sim2real/hands/linkerhand_o6.py +++ b/teleopit/sim2real/hands/linkerhand_o6.py @@ -131,6 +131,17 @@ def connect(self) -> None: raise self.open_all(force=True, reason="startup") + def get_state(self, side: str) -> tuple[float, ...]: + if side not in self.config.sides: + raise ValueError(f"LinkerHand O6 side is not configured: {side!r}") + hand = self._hands.get(side) + if hand is None: + raise RuntimeError(f"LinkerHand O6 {side} is not connected") + state = tuple(float(value) for value in hand.get_state()) + if len(state) != 6: + raise RuntimeError(f"LinkerHand O6 {side} state must contain 6 values, got {len(state)}") + return state + def send_pose(self, side: str, pose: Sequence[int], *, force: bool = False, reason: str = "") -> None: del reason next_pose = tuple(_uint8(value, f"{side}.pose") for value in pose) diff --git a/teleopit/sim2real/neck/openneck.py b/teleopit/sim2real/neck/openneck.py index 2db7af91..a02c6b1e 100644 --- a/teleopit/sim2real/neck/openneck.py +++ b/teleopit/sim2real/neck/openneck.py @@ -16,9 +16,9 @@ def _load_openneck_controller() -> type: "OpenNeck 0.2.0 is required for neck.driver=openneck. " "Install with: pip install -e '.[openneck]'" ) from exc - if not callable(getattr(OpenNeckController, "move_deg", None)): + if not all(callable(getattr(OpenNeckController, name, None)) for name in ("move_deg", "read_deg")): raise ImportError( - "OpenNeck 0.2.0 angle API is required; reinstall with: " + "OpenNeck 0.2.0 move_deg/read_deg angle API is required; reinstall with: " "pip install --force-reinstall --no-deps " "'openneck @ git+https://github.com/BotRunner64/OpenNeck.git'" ) @@ -34,6 +34,8 @@ def release_torque(self) -> None: ... def move_deg(self, yaw_deg: float, pitch_deg: float) -> tuple[float, float]: ... + def read_deg(self) -> tuple[float, float]: ... + def close(self) -> None: ... @@ -62,6 +64,12 @@ def move_deg(self, yaw_deg: float, pitch_deg: float) -> tuple[float, float]: applied = self._controller.move_deg(float(yaw_deg), float(pitch_deg)) return float(applied.yaw_deg), float(applied.pitch_deg) + def read_deg(self) -> tuple[float, float]: + if self._controller is None: + raise RuntimeError("OpenNeck is not connected") + state = self._controller.read_deg() + return float(state.yaw_deg), float(state.pitch_deg) + def release_torque(self) -> None: if self._controller is not None: self._controller.release_torque() @@ -108,6 +116,9 @@ def move_deg(self, yaw_deg: float, pitch_deg: float) -> tuple[float, float]: ) return applied_yaw_deg, applied_pitch_deg + def read_deg(self) -> tuple[float, float]: + raise RuntimeError("OpenNeck dry-run has no hardware state to read") + def release_torque(self) -> None: logger.info("OpenNeck dry-run release") diff --git a/tests/test_active_neck.py b/tests/test_active_neck.py index 77fec184..73a21247 100644 --- a/tests/test_active_neck.py +++ b/tests/test_active_neck.py @@ -255,6 +255,10 @@ def move_deg(self, yaw_deg: float, pitch_deg: float) -> SimpleNamespace: calls.append(f"move-{yaw_deg}-{pitch_deg}") return SimpleNamespace(yaw_deg=-20.0, pitch_deg=10.0) + def read_deg(self) -> SimpleNamespace: + calls.append("read") + return SimpleNamespace(yaw_deg=-18.5, pitch_deg=9.5) + def release_torque(self) -> None: calls.append("release-torque") @@ -271,15 +275,18 @@ def close(self) -> None: device.connect() device.center() applied = device.move_deg(-25.0, 15.0) + state = device.read_deg() device.release_torque() device.close() assert applied == (-20.0, 10.0) + assert state == (-18.5, 9.5) assert calls == [ "init-neck.json-/dev/ttyACM0", "connect", "center", "move--25.0-15.0", + "read", "release-torque", "close", ] @@ -296,6 +303,9 @@ def move_deg(self, yaw_deg: float, pitch_deg: float) -> None: del yaw_deg, pitch_deg raise AssertionError("dry-run must not send a hardware command") + def read_deg(self) -> None: + raise AssertionError("dry-run must not read hardware state") + def _angle_to_step(self, axis: str, angle_deg: float) -> int: calls.append(f"angle-to-step-{axis}-{angle_deg}") low, high = (-20.0, 20.0) if axis == "yaw" else (-10.0, 10.0) @@ -319,6 +329,12 @@ def _step_to_angle(self, axis: str, step: int) -> float: ) device.connect() applied = device.move_deg(25.0, -15.0) + try: + device.read_deg() + except RuntimeError as exc: + assert "no hardware state" in str(exc) + else: + raise AssertionError("expected dry-run state read to fail") device.close() assert applied == (20.0, -10.0) diff --git a/tests/test_dexterous_hand.py b/tests/test_dexterous_hand.py index d9333057..d1f99f40 100644 --- a/tests/test_dexterous_hand.py +++ b/tests/test_dexterous_hand.py @@ -51,6 +51,7 @@ def __init__(self, *, hand_joint: str, hand_type: str, modbus: str, can: str) -> self.hand = FakeInnerHand() self.speed: list[int] | None = None self.poses: list[list[int]] = [] + self.state = [1, 2, 3, 4, 5, 6] if hand_type == "left" else [11, 12, 13, 14, 15, 16] self.close_can_calls = 0 FakeLinkerHandApi.instances.append(self) @@ -60,6 +61,9 @@ def set_speed(self, speed: list[int]) -> None: def finger_move(self, pose: list[int]) -> None: self.poses.append(list(pose)) + def get_state(self) -> list[int]: + return list(self.state) + def close_can(self) -> None: self.close_can_calls += 1 @@ -206,8 +210,10 @@ def test_linkerhand_l6_device_starts_sdk(monkeypatch) -> None: device.connect() device.send_pose("left", cfg.close_pose) + state = device.get_state("left") device.close() + assert state == (1.0, 2.0, 3.0, 4.0, 5.0, 6.0) assert [hand.can for hand in FakeLinkerHandApi.instances] == ["can0", "can1"] assert FakeLinkerHandApi.instances[0].speed == [50, 50, 50, 50, 50, 50] assert FakeLinkerHandApi.instances[0].poses[-2] == list(cfg.close_pose) @@ -243,8 +249,10 @@ def test_linkerhand_o6_device_starts_sdk(monkeypatch) -> None: device.connect() device.send_pose("left", cfg.close_pose) + state = device.get_state("right") device.close() + assert state == (11.0, 12.0, 13.0, 14.0, 15.0, 16.0) assert [hand.hand_joint for hand in FakeLinkerHandApi.instances] == ["O6", "O6"] assert [hand.can for hand in FakeLinkerHandApi.instances] == ["can0", "can1"] assert FakeLinkerHandApi.instances[0].speed == [255, 255, 255, 255, 255, 255] From b805c745e9958ab9734b27281f4ae97523b871ee Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Thu, 30 Jul 2026 19:46:09 +0800 Subject: [PATCH 53/59] feat: record hand and neck state --- AGENTS.md | 4 +- README.md | 8 +- docs/docs/reference/configuration/fields.md | 8 +- .../resources/teleoperation-datasets.md | 10 ++- .../current/reference/configuration/fields.md | 10 ++- .../resources/teleoperation-datasets.md | 7 +- scripts/view/view_recording.py | 75 +++++++++++----- teleopit/recording/hdf5.py | 88 +++++++++++-------- teleopit/sim2real/hands/worker.py | 7 ++ teleopit/sim2real/mp/messages.py | 4 + teleopit/sim2real/mp/runtime.py | 76 ++++++++++++++-- teleopit/sim2real/neck/worker.py | 6 ++ tests/test_active_neck.py | 4 + tests/test_dexterous_hand.py | 5 ++ tests/test_recording_viewer.py | 16 +++- tests/test_sim2real_multiprocess.py | 68 +++++++++++++- 16 files changed, 316 insertions(+), 80 deletions(-) diff --git a/AGENTS.md b/AGENTS.md index 87d8b795..c4f586f9 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -162,9 +162,9 @@ target_dof_pos = clip(action, -10, 10) × action_scale + default_dof_pos - Optional Pico sim2real HDF5 recording uses `--config-name sim2real_record` or `recording.enabled=true`; it requires `input.provider=pico4`, `input.video.enabled=true`, `input.video.source=realsense`, an interactive terminal, and the `recording` extra - Recording is manual only: terminal `R` starts an episode, `S` saves, `D` discards the active episode, and `Q` shuts down; `STANDING`, `MOCAP`, `ARMS`, and paused mocap are recordable - Recording requires a fresh RealSense frame before `R` can start an episode; an active episode is discarded after one second without a fresh camera frame while Pico input and G1 control continue, and recording does not restart automatically when video recovers -- Recording captures `observation.images.d435i_rgb` RealSense RGB video at 30Hz plus `observation.state(68)`, scalar `observation.mode`, and `action(36)` as the root-plus-joint reference consumed by the motion tracker; `action.hand(12)` is present when LinkerHand control is enabled, and `action.neck(2)` stores the mechanically clamped OpenNeck `[yaw_deg, pitch_deg]` target when OpenNeck control is enabled +- Recording captures `observation.images.d435i_rgb` RealSense RGB video at 30Hz plus `observation.state(68)`, scalar `observation.mode`, and `action(36)` as the root-plus-joint reference consumed by the motion tracker; when LinkerHand control is enabled, `observation.state.hand(12)` stores the left/right hardware joint readback and `action.hand(12)` stores the target; when OpenNeck control is enabled, `observation.state.neck(2)` stores the servo `[yaw_deg, pitch_deg]` readback and `action.neck(2)` stores the mechanically clamped target - Sim2real recording uses an editable source layout: `schema.json`, `episodes.jsonl`, per-episode HDF5 files under `recording.output_dir/data/`, and compressed MP4 files under `recording.output_dir/videos/d435i_rgb/`; task prompts live only in `episodes.jsonl`, and HDF5 files contain only frame arrays with no metadata attributes or raw RGB datasets -- Recording `schema.json` stores `robot_type=unitree_g1_29dof`, `hand_type=none|linkerhand_l6|linkerhand_o6`, `neck_type=none|openneck`, FPS, and feature definitions; optional action fields are controlled directly by `hands.enabled` and `neck.enabled`; the recording worker rejects an existing mismatched schema without writing episodes, but remains non-critical and must not stop the G1 control runtime; the previous attribute-based HDF5 layout is unsupported +- Recording `schema.json` stores `robot_type=unitree_g1_29dof`, `hand_type=none|linkerhand_l6|linkerhand_o6`, `neck_type=none|openneck`, FPS, and feature definitions; optional hand/neck state and action fields are controlled directly by `hands.enabled` and `neck.enabled`; the recording worker rejects an existing mismatched schema without writing episodes, but remains non-critical and must not stop the G1 control runtime; the previous attribute-based HDF5 layout is unsupported - Episodes interrupted before their `episodes.jsonl` entry is committed are discarded on the next recording-worker startup and do not consume an episode index - Review saved sim2real recordings with `scripts/view/view_recording.py`; it validates manifest/HDF5/MP4 alignment and synchronizes camera video, an observed-vs-reference MuJoCo overlay, joint/mode plots, and optional hand/neck signals; because measured root XYZ is not recorded, the observed robot is anchored to the reference root position - `gripper` mode reuses `Pico4InputProvider.get_controller_snapshot()` and supports LinkerHand L6 and O6; the side grip trigger is a deadman enable, the index trigger controls closure while it is held, and releasing the side grip opens that hand diff --git a/README.md b/README.md index 30ea458a..7f520786 100644 --- a/README.md +++ b/README.md @@ -109,9 +109,11 @@ and neck types, plus feature shapes, names, and groups. `episodes.jsonl` maps ea to its HDF5/video files and stores its editable task prompt. HDF5 contains only frame-aligned arrays: `observation.state(68)`, scalar `observation.mode`, and `action(36)` as the aligned reference qpos consumed by the motion tracker. -`action.hand(12)` is present exactly when LinkerHand control is enabled, and -`action.neck(2)` contains the latest mechanically clamped OpenNeck -`[yaw_deg, pitch_deg]` target when OpenNeck control is enabled. +When LinkerHand control is enabled, `observation.state.hand(12)` contains the +left/right hardware joint readback and `action.hand(12)` contains the target. +When OpenNeck control is enabled, `observation.state.neck(2)` contains the +servo `[yaw_deg, pitch_deg]` readback and `action.neck(2)` contains the latest +mechanically clamped target. Recording is non-critical: an incompatible output schema stops only the recording worker while G1 control continues. Episodes interrupted before their manifest entry is committed are discarded on the next recording startup. diff --git a/docs/docs/reference/configuration/fields.md b/docs/docs/reference/configuration/fields.md index 5c1d236f..b2831724 100644 --- a/docs/docs/reference/configuration/fields.md +++ b/docs/docs/reference/configuration/fields.md @@ -302,7 +302,7 @@ feature definitions. `robot_type` comes from `robot.type`; `hand_type` is `none` when hands are disabled, otherwise it is the configured `hands.driver`. `neck_type` is `none` when active-neck control is disabled, otherwise it is the configured `neck.driver`. These enabled flags directly control whether their -action fields are recorded; there are no separate recording switches. +state and action fields are recorded; there are no separate recording switches. `episodes.jsonl` contains one object per saved episode with `episode_index`, `frames`, editable `task`, HDF5 path, and video paths. Task prompts can therefore be relabeled without rewriting HDF5 or MP4 data. Starting another recording run @@ -323,6 +323,8 @@ HDF5 datasets: frame_index int64[N] timestamp float64[N] observation.state float32[N, 68] +observation.state.hand float32[N, 12] # only when hands are enabled +observation.state.neck float32[N, 2] # only when OpenNeck is enabled observation.mode int8[N] action float32[N, 36] action.hand float32[N, 12] # only when hands are enabled @@ -335,6 +337,10 @@ attributes. RGB frames remain in MP4 and are associated through `observation.state` is ordered as `joint_pos(29)`, `joint_vel(29)`, `base_quat_wxyz(4)`, `base_ang_vel(3)`, and `projected_gravity(3)`. +`observation.state.hand` is the latest LinkerHand hardware readback: +`left_state(6) + right_state(6)`, using the SDK's 0-255 joint values. +`observation.state.neck` is the latest OpenNeck servo position returned by +`read_deg()`: `[yaw_deg, pitch_deg]` in degrees. `observation.mode` is a numeric categorical: `standing=0`, `mocap=1`, `arms=2`, and `pause=3`. `action` is the current reference qpos: `root_pos(3) + root_quat_wxyz(4) + reference_joint_pos(29)`. It is the diff --git a/docs/docs/reference/resources/teleoperation-datasets.md b/docs/docs/reference/resources/teleoperation-datasets.md index 4ea0c373..7fca39c5 100644 --- a/docs/docs/reference/resources/teleoperation-datasets.md +++ b/docs/docs/reference/resources/teleoperation-datasets.md @@ -63,6 +63,8 @@ Each HDF5 file contains only frame-aligned arrays: | `frame_index` | scalar | Camera/action frame index | | `timestamp` | scalar | Monotonic timestamp in seconds | | `observation.state` | `(68,)` | G1 joint state, base orientation/angular velocity, and projected gravity | +| `observation.state.hand` | `(12,)`, optional | Left/right LinkerHand hardware joint readback | +| `observation.state.neck` | `(2,)`, optional | OpenNeck servo yaw/pitch readback in degrees | | `observation.mode` | scalar | `STANDING`, `MOCAP`, `ARMS`, or paused mocap code | | `action` | `(36,)` | Root pose plus 29-joint reference consumed by the motion tracker | | `action.hand` | `(12,)`, optional | Left/right LinkerHand target when hand control is enabled | @@ -70,12 +72,16 @@ Each HDF5 file contains only frame-aligned arrays: `observation.state` is ordered as `joint_pos(29)`, `joint_vel(29)`, `base_quat_wxyz(4)`, `base_ang_vel(3)`, and `projected_gravity(3)`. +`observation.state.hand` uses the LinkerHand SDK's 0-255 joint values, ordered +as six left-hand channels followed by six right-hand channels. +`observation.state.neck` is `[yaw_deg, pitch_deg]` returned by OpenNeck +`read_deg()`. `observation.mode` uses `standing=0`, `mocap=1`, `arms=2`, and `pause=3`. `action` is `root_pos(3) + root_quat_wxyz(4) + reference_joint_pos(29)`. Camera RGB is stored only in the MP4 sidecar; HDF5 does not duplicate raw image -frames. Optional action fields appear exactly when the corresponding hardware -is enabled. +frames. Optional state and action fields appear exactly when the corresponding +hardware is enabled. ## Commit and Recovery Rules diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/configuration/fields.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/configuration/fields.md index b2e43afb..98a8f937 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/configuration/fields.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/configuration/fields.md @@ -301,8 +301,8 @@ recording.output_dir/ `schema.json` 保存 FPS、`robot_type`、`hand_type`、`neck_type` 和 feature 定义。 `robot_type` 来自 `robot.type`；未启用灵巧手时 `hand_type` 为 `none`，否则为 配置的 `hands.driver`。未启用主动视觉颈部控制时 `neck_type` 为 `none`，否则为 -配置的 `neck.driver`。这些 enabled 标志直接决定是否录制对应的 action 字段；没有 -单独的录制开关。`episodes.jsonl` 每行对应一个已保存的 episode，包含 +配置的 `neck.driver`。这些 enabled 标志直接决定是否录制对应的 state 和 action +字段；没有单独的录制开关。`episodes.jsonl` 每行对应一个已保存的 episode，包含 `episode_index`、`frames`、可编辑的 `task`、HDF5 路径和视频路径。因此修改任务 prompt 不需要重写 HDF5 或 MP4。使用相同 schema 再次启动录制时，会从下一个 episode index 继续追加，并且可以使用不同的 `recording.task`。 @@ -319,6 +319,8 @@ HDF5 datasets： frame_index int64[N] timestamp float64[N] observation.state float32[N, 68] +observation.state.hand float32[N, 12] # 仅启用灵巧手时存在 +observation.state.neck float32[N, 2] # 仅启用 OpenNeck 时存在 observation.mode int8[N] action float32[N, 36] action.hand float32[N, 12] # 仅启用灵巧手时存在 @@ -330,6 +332,10 @@ HDF5 文件仅包含上述逐帧数组，不保存录制元数据根属性。RGB `observation.state` 的顺序是 `joint_pos(29)`、`joint_vel(29)`、 `base_quat_wxyz(4)`、`base_ang_vel(3)` 和 `projected_gravity(3)`。 +`observation.state.hand` 是最新的 LinkerHand 硬件回读： +`left_state(6) + right_state(6)`，使用 SDK 的 0-255 关节数值。 +`observation.state.neck` 是 OpenNeck `read_deg()` 返回的最新舵机位置： +以度为单位的 `[yaw_deg, pitch_deg]`。 `observation.mode` 是数值类别：`standing=0`、`mocap=1`、 `arms=2`、`pause=3`。`action` 是当前 reference qpos： `root_pos(3) + root_quat_wxyz(4) + reference_joint_pos(29)`。它是 motion tracker diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/resources/teleoperation-datasets.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/resources/teleoperation-datasets.md index 12b35358..f215c378 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/resources/teleoperation-datasets.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/resources/teleoperation-datasets.md @@ -57,6 +57,8 @@ shape、dtype、名称和分组。硬件类型必须与当前运行配置一致 | `frame_index` | scalar | 相机/动作帧序号 | | `timestamp` | scalar | 单调时钟时间戳，单位为秒 | | `observation.state` | `(68,)` | G1 关节状态、基座方向/角速度和投影重力 | +| `observation.state.hand` | `(12,)`，可选 | 左右 LinkerHand 硬件关节回读 | +| `observation.state.neck` | `(2,)`，可选 | 以度为单位的 OpenNeck 舵机 yaw/pitch 回读 | | `observation.mode` | scalar | `STANDING`、`MOCAP`、`ARMS` 或动捕暂停状态码 | | `action` | `(36,)` | motion tracker 使用的根部姿态和 29 关节参考 | | `action.hand` | `(12,)`，可选 | 启用手部控制时的左右 LinkerHand 目标 | @@ -64,11 +66,14 @@ shape、dtype、名称和分组。硬件类型必须与当前运行配置一致 `observation.state` 的顺序为 `joint_pos(29)`、`joint_vel(29)`、 `base_quat_wxyz(4)`、`base_ang_vel(3)` 和 `projected_gravity(3)`。 +`observation.state.hand` 使用 LinkerHand SDK 的 0-255 关节数值，顺序是左手六个 +通道，然后是右手六个通道。`observation.state.neck` 是 OpenNeck `read_deg()` +返回的 `[yaw_deg, pitch_deg]`。 `observation.mode` 使用 `standing=0`、`mocap=1`、`arms=2` 和 `pause=3`。 `action` 的结构是 `root_pos(3) + root_quat_wxyz(4) + reference_joint_pos(29)`。 相机 RGB 只保存在 MP4 sidecar 中，HDF5 不重复保存 raw image。只有启用对应硬件时， -才会出现可选 action 字段。 +才会出现可选 state 和 action 字段。 ## 提交与恢复规则 diff --git a/scripts/view/view_recording.py b/scripts/view/view_recording.py index accc4567..e8dbc9ed 100644 --- a/scripts/view/view_recording.py +++ b/scripts/view/view_recording.py @@ -24,10 +24,12 @@ ACTION_KEY, FRAME_INDEX_KEY, HAND_ACTION_KEY, + HAND_STATE_KEY, HDF5_RECORDING_FORMAT, HDF5_RECORDING_VERSION, MODE_KEY, NECK_ACTION_KEY, + NECK_STATE_KEY, STATE_KEY, TIMESTAMP_KEY, ) @@ -84,7 +86,9 @@ class EpisodeReviewData: state: np.ndarray mode: np.ndarray action: np.ndarray + hand_state: np.ndarray | None hand_action: np.ndarray | None + neck_state: np.ndarray | None neck_action: np.ndarray | None joint_error: np.ndarray group_error: dict[str, np.ndarray] @@ -266,27 +270,29 @@ def load_recording_dataset(recording_root: str | Path) -> RecordingDataset: has_neck_action = neck_type != "none" hand_names: tuple[str, ...] = () if has_hand_action: + if _feature_shape(features, HAND_STATE_KEY) != (12,): + raise ValueError(f"Recording schema feature {HAND_STATE_KEY!r} must be 12D") + _feature_names(features, HAND_STATE_KEY, 12) if _feature_shape(features, HAND_ACTION_KEY) != (12,): raise ValueError(f"Recording schema feature {HAND_ACTION_KEY!r} must be 12D") hand_names = _feature_names(features, HAND_ACTION_KEY, 12) - elif HAND_ACTION_KEY in features: - raise ValueError( - f"Recording schema hand_type={hand_type!r} must not define {HAND_ACTION_KEY!r}" - ) + elif HAND_STATE_KEY in features or HAND_ACTION_KEY in features: + raise ValueError(f"Recording schema hand_type={hand_type!r} must not define hand features") if has_neck_action: + if _feature_shape(features, NECK_STATE_KEY) != (2,): + raise ValueError(f"Recording schema feature {NECK_STATE_KEY!r} must be 2D") + _feature_names(features, NECK_STATE_KEY, 2) if _feature_shape(features, NECK_ACTION_KEY) != (2,): raise ValueError(f"Recording schema feature {NECK_ACTION_KEY!r} must be 2D") _feature_names(features, NECK_ACTION_KEY, 2) - elif NECK_ACTION_KEY in features: - raise ValueError( - f"Recording schema neck_type={neck_type!r} must not define {NECK_ACTION_KEY!r}" - ) + elif NECK_STATE_KEY in features or NECK_ACTION_KEY in features: + raise ValueError(f"Recording schema neck_type={neck_type!r} must not define neck features") hdf5_keys = [FRAME_INDEX_KEY, TIMESTAMP_KEY, STATE_KEY, MODE_KEY, ACTION_KEY] if has_hand_action: - hdf5_keys.append(HAND_ACTION_KEY) + hdf5_keys.extend((HAND_STATE_KEY, HAND_ACTION_KEY)) if has_neck_action: - hdf5_keys.append(NECK_ACTION_KEY) + hdf5_keys.extend((NECK_STATE_KEY, NECK_ACTION_KEY)) manifest_path = root / "episodes.jsonl" try: @@ -387,9 +393,9 @@ def load_episode_review_data( keys = [FRAME_INDEX_KEY, TIMESTAMP_KEY, STATE_KEY, MODE_KEY, ACTION_KEY] if dataset.has_hand_action: - keys.append(HAND_ACTION_KEY) + keys.extend((HAND_STATE_KEY, HAND_ACTION_KEY)) if dataset.has_neck_action: - keys.append(NECK_ACTION_KEY) + keys.extend((NECK_STATE_KEY, NECK_ACTION_KEY)) with h5py.File(episode.data_path, "r") as h5: arrays = {key: np.asarray(h5[key]) for key in keys} @@ -398,11 +404,21 @@ def load_episode_review_data( state = arrays[STATE_KEY].astype(np.float64, copy=False) mode = arrays[MODE_KEY].astype(np.int64, copy=False) action = arrays[ACTION_KEY].astype(np.float64, copy=False) + hand_state = ( + arrays[HAND_STATE_KEY].astype(np.float64, copy=False) + if dataset.has_hand_action + else None + ) hand_action = ( arrays[HAND_ACTION_KEY].astype(np.float64, copy=False) if dataset.has_hand_action else None ) + neck_state = ( + arrays[NECK_STATE_KEY].astype(np.float64, copy=False) + if dataset.has_neck_action + else None + ) neck_action = ( arrays[NECK_ACTION_KEY].astype(np.float64, copy=False) if dataset.has_neck_action @@ -419,8 +435,12 @@ def load_episode_review_data( STATE_KEY: state, ACTION_KEY: action, } + if hand_state is not None: + numeric_arrays[HAND_STATE_KEY] = hand_state if hand_action is not None: numeric_arrays[HAND_ACTION_KEY] = hand_action + if neck_state is not None: + numeric_arrays[NECK_STATE_KEY] = neck_state if neck_action is not None: numeric_arrays[NECK_ACTION_KEY] = neck_action for key, values in numeric_arrays.items(): @@ -468,7 +488,9 @@ def load_episode_review_data( state=state, mode=mode, action=action, + hand_state=hand_state, hand_action=hand_action, + neck_state=neck_state, neck_action=neck_action, joint_error=joint_error, group_error=group_error, @@ -925,37 +947,50 @@ def _refresh_charts(self) -> None: order=1, ) - if self._data.hand_action is not None and self._hand_dropdown is not None: + if ( + self._data.hand_state is not None + and self._data.hand_action is not None + and self._hand_dropdown is not None + ): selected_hand = str(self._hand_dropdown.value) hand_index = self._dataset.hand_names.index(selected_hand) self._hand_chart = self._add_chart( self._signals_folder, - data=(timestamps, self._data.hand_action[:, hand_index]), + data=( + timestamps, + self._data.hand_state[:, hand_index], + self._data.hand_action[:, hand_index], + ), series=( {"label": "time"}, - {"label": selected_hand, "stroke": "#8b5cf6", "width": 2.0}, + {"label": "state", "stroke": "#3b82f6", "width": 2.0}, + {"label": "target", "stroke": "#8b5cf6", "width": 2.0}, ), - title="LinkerHand target", + title=f"LinkerHand state vs target: {selected_hand}", y_label="SDK pose", order=2, ) else: self._hand_chart = None - if self._data.neck_action is not None: + if self._data.neck_state is not None and self._data.neck_action is not None: self._neck_chart = self._add_chart( self._signals_folder, data=( timestamps, + self._data.neck_state[:, 0], self._data.neck_action[:, 0], + self._data.neck_state[:, 1], self._data.neck_action[:, 1], ), series=( {"label": "time"}, - {"label": "yaw", "stroke": "#06b6d4", "width": 2.0}, - {"label": "pitch", "stroke": "#ec4899", "width": 2.0}, + {"label": "yaw state", "stroke": "#3b82f6", "width": 2.0}, + {"label": "yaw target", "stroke": "#06b6d4", "width": 2.0}, + {"label": "pitch state", "stroke": "#f59e0b", "width": 2.0}, + {"label": "pitch target", "stroke": "#ec4899", "width": 2.0}, ), - title="OpenNeck target", + title="OpenNeck state vs target", y_label="degrees", order=3, ) diff --git a/teleopit/recording/hdf5.py b/teleopit/recording/hdf5.py index 2eb67544..723157fb 100644 --- a/teleopit/recording/hdf5.py +++ b/teleopit/recording/hdf5.py @@ -25,6 +25,8 @@ IMAGE_KEY = "observation.images.d435i_rgb" STATE_KEY = "observation.state" +HAND_STATE_KEY = "observation.state.hand" +NECK_STATE_KEY = "observation.state.neck" MODE_KEY = "observation.mode" ACTION_KEY = "action" HAND_ACTION_KEY = "action.hand" @@ -32,12 +34,14 @@ FRAME_INDEX_KEY = "frame_index" TIMESTAMP_KEY = "timestamp" STATE_DIM = 68 +HAND_STATE_DIM = 12 +NECK_STATE_DIM = 2 ACTION_DIM = FULL_QPOS_DIM HAND_ACTION_DIM = 12 NECK_ACTION_DIM = 2 DEFAULT_IMAGE_SHAPE = (480, 640, 3) HDF5_RECORDING_FORMAT = "teleopit_hdf5" -HDF5_RECORDING_VERSION = 3 +HDF5_RECORDING_VERSION = 4 DEFAULT_ROBOT_TYPE = "unitree_g1_29dof" NO_HAND_TYPE = "none" SUPPORTED_HAND_TYPES = (NO_HAND_TYPE, "linkerhand_l6", "linkerhand_o6") @@ -63,6 +67,10 @@ class RecordingSchema: neck_type: str = NO_NECK_TYPE state_key: str = STATE_KEY state_dim: int = STATE_DIM + hand_state_key: str = HAND_STATE_KEY + hand_state_dim: int = HAND_STATE_DIM + neck_state_key: str = NECK_STATE_KEY + neck_state_dim: int = NECK_STATE_DIM mode_key: str = MODE_KEY action_key: str = ACTION_KEY action_dim: int = ACTION_DIM @@ -181,6 +189,15 @@ def hdf5_schema(schema: RecordingSchema) -> dict[str, object]: }, } if schema.has_hand_action: + features[schema.hand_state_key] = { + "dtype": "float32", + "shape": [schema.hand_state_dim], + "names": _hand_action_names(schema.hand_type), + "groups": { + "left_hand_state": [0, 6], + "right_hand_state": [6, 12], + }, + } features[schema.hand_action_key] = { "dtype": "float32", "shape": [schema.hand_action_dim], @@ -191,6 +208,12 @@ def hdf5_schema(schema: RecordingSchema) -> dict[str, object]: }, } if schema.has_neck_action: + features[schema.neck_state_key] = { + "dtype": "float32", + "shape": [schema.neck_state_dim], + "names": ["yaw_deg", "pitch_deg"], + "units": "degrees", + } features[schema.neck_action_key] = { "dtype": "float32", "shape": [schema.neck_action_dim], @@ -364,6 +387,8 @@ def add_frame( state: np.ndarray, mode: object, action: np.ndarray, + hand_state: np.ndarray | None = None, + neck_state: np.ndarray | None = None, hand_action: np.ndarray | None = None, neck_action: np.ndarray | None = None, ) -> None: @@ -375,28 +400,19 @@ def add_frame( state_arr = self._validate_vector(state, self._schema.state_key, self._schema.state_dim) mode_value = self._validate_mode(mode) action_arr = self._validate_vector(action, self._schema.action_key, self._schema.action_dim) - hand_action_arr: np.ndarray | None = None - if self._schema.has_hand_action: - if hand_action is None: - raise ValueError(f"{self._schema.hand_action_key} is required for hand_type={self._schema.hand_type}") - hand_action_arr = self._validate_vector( - hand_action, - self._schema.hand_action_key, - self._schema.hand_action_dim, - ) - elif hand_action is not None: - raise ValueError(f"{self._schema.hand_action_key} must be omitted for hand_type={NO_HAND_TYPE}") - neck_action_arr: np.ndarray | None = None - if self._schema.has_neck_action: - if neck_action is None: - raise ValueError(f"{self._schema.neck_action_key} is required for neck_type={self._schema.neck_type}") - neck_action_arr = self._validate_vector( - neck_action, - self._schema.neck_action_key, - self._schema.neck_action_dim, - ) - elif neck_action is not None: - raise ValueError(f"{self._schema.neck_action_key} must be omitted for neck_type={NO_NECK_TYPE}") + optional_vectors: dict[str, np.ndarray] = {} + for enabled, value, key, dim in ( + (self._schema.has_hand_action, hand_state, self._schema.hand_state_key, self._schema.hand_state_dim), + (self._schema.has_hand_action, hand_action, self._schema.hand_action_key, self._schema.hand_action_dim), + (self._schema.has_neck_action, neck_state, self._schema.neck_state_key, self._schema.neck_state_dim), + (self._schema.has_neck_action, neck_action, self._schema.neck_action_key, self._schema.neck_action_dim), + ): + if enabled and value is None: + raise ValueError(f"{key} is required when its device is enabled") + if not enabled and value is not None: + raise ValueError(f"{key} must be omitted when its device is disabled") + if value is not None: + optional_vectors[key] = self._validate_vector(value, key, dim) row = self._frames_in_episode for dataset in self._datasets.values(): @@ -409,10 +425,8 @@ def add_frame( self._datasets[self._schema.state_key][row] = state_arr self._datasets[self._schema.mode_key][row] = mode_value self._datasets[self._schema.action_key][row] = action_arr - if hand_action_arr is not None: - self._datasets[self._schema.hand_action_key][row] = hand_action_arr - if neck_action_arr is not None: - self._datasets[self._schema.neck_action_key][row] = neck_action_arr + for key, value in optional_vectors.items(): + self._datasets[key][row] = value self._frames_in_episode += 1 def save_episode(self) -> None: @@ -664,18 +678,14 @@ def _create_datasets(self, h5: h5py.File) -> dict[str, h5py.Dataset]: self._schema.action_dim, ), } - if self._schema.has_hand_action: - datasets[self._schema.hand_action_key] = self._create_vector_dataset( - h5, - self._schema.hand_action_key, - self._schema.hand_action_dim, - ) - if self._schema.has_neck_action: - datasets[self._schema.neck_action_key] = self._create_vector_dataset( - h5, - self._schema.neck_action_key, - self._schema.neck_action_dim, - ) + for enabled, key, dim in ( + (self._schema.has_hand_action, self._schema.hand_state_key, self._schema.hand_state_dim), + (self._schema.has_hand_action, self._schema.hand_action_key, self._schema.hand_action_dim), + (self._schema.has_neck_action, self._schema.neck_state_key, self._schema.neck_state_dim), + (self._schema.has_neck_action, self._schema.neck_action_key, self._schema.neck_action_dim), + ): + if enabled: + datasets[key] = self._create_vector_dataset(h5, key, dim) return datasets @staticmethod diff --git a/teleopit/sim2real/hands/worker.py b/teleopit/sim2real/hands/worker.py index e9fe323a..27d9e01f 100644 --- a/teleopit/sim2real/hands/worker.py +++ b/teleopit/sim2real/hands/worker.py @@ -37,6 +37,9 @@ def start(self) -> tuple[HandPoseCommand, ...]: finally: raise + def get_state(self, side: str) -> tuple[float, ...]: + return self._device.get_state(side) + def tick( self, *, @@ -90,6 +93,10 @@ class DisabledHandRuntime: def start(self) -> tuple[HandPoseCommand, ...]: return () + def get_state(self, side: str) -> tuple[float, ...]: + del side + raise RuntimeError("Dexterous hand control is disabled") + def tick( self, *, diff --git a/teleopit/sim2real/mp/messages.py b/teleopit/sim2real/mp/messages.py index 0fffd6ef..909bd451 100644 --- a/teleopit/sim2real/mp/messages.py +++ b/teleopit/sim2real/mp/messages.py @@ -83,6 +83,8 @@ class HandCommandPacket: left_pose: Float64Array right_pose: Float64Array seq: int + left_state: Float64Array | None = None + right_state: Float64Array | None = None @dataclass(frozen=True) @@ -93,6 +95,8 @@ class NeckCommandPacket: yaw_deg: float pitch_deg: float seq: int + state_yaw_deg: float | None = None + state_pitch_deg: float | None = None @dataclass(frozen=True) diff --git a/teleopit/sim2real/mp/runtime.py b/teleopit/sim2real/mp/runtime.py index 7dabb28f..cab07eea 100644 --- a/teleopit/sim2real/mp/runtime.py +++ b/teleopit/sim2real/mp/runtime.py @@ -380,6 +380,12 @@ def _validate_new_runtime_config(cfg: Any) -> None: if _recording_enabled(cfg): if provider != "pico4": raise ValueError("recording.enabled=true requires input.provider=pico4") + if bool(cfg_get(hands_cfg, "enabled", False)): + hand_sides = {str(side).strip().lower() for side in cfg_get(hands_cfg, "sides", ("left", "right"))} + if hand_sides != {"left", "right"}: + raise ValueError("hand-state recording requires hands.sides=[left, right]") + if neck_cfg.enabled and neck_cfg.dry_run: + raise ValueError("neck-state recording requires neck.dry_run=false") rec_cfg = _recording_cfg(cfg) if str(cfg_get(rec_cfg, "format", "hdf5")) != "hdf5": raise ValueError("Only recording.format=hdf5 is supported") @@ -2725,7 +2731,25 @@ def _handle_video(self, descriptor: SharedFrameDescriptor) -> None: logger.warning("Recording stopped because mode is no longer recordable: %s", record.mode) self._discard_episode("mode not recordable") return + if self._schema.has_hand_action and ( + self._latest_hand_command.left_state is None + or self._latest_hand_command.right_state is None + ): + return + if self._schema.has_neck_action and ( + self._latest_neck_command.state_yaw_deg is None + or self._latest_neck_command.state_pitch_deg is None + ): + return image = self._frame_reader.read(descriptor, copy=True) + hand_state = ( + normalize_hand_action( + self._latest_hand_command.left_state, + self._latest_hand_command.right_state, + ) + if self._schema.has_hand_action + else None + ) hand_action = ( normalize_hand_action( self._latest_hand_command.left_pose, @@ -2734,6 +2758,14 @@ def _handle_video(self, descriptor: SharedFrameDescriptor) -> None: if self._schema.has_hand_action else None ) + neck_state = ( + build_neck_action( + self._latest_neck_command.state_yaw_deg, + self._latest_neck_command.state_pitch_deg, + ) + if self._schema.has_neck_action + else None + ) neck_action = ( build_neck_action( self._latest_neck_command.yaw_deg, @@ -2747,8 +2779,11 @@ def _handle_video(self, descriptor: SharedFrameDescriptor) -> None: "state": np.asarray(record.observation_state, dtype=np.float32), "mode": record.observation_mode, "action": np.asarray(record.action_reference_qpos, dtype=np.float32), + "hand_state": hand_state, "hand_action": hand_action, } + if neck_state is not None: + frame_kwargs["neck_state"] = neck_state if neck_action is not None: frame_kwargs["neck_action"] = neck_action self._recorder.add_frame(**frame_kwargs) @@ -2815,6 +2850,11 @@ def _publish_neck_command( nonlocal command_seq if neck_command_pub is None: return + try: + state_yaw_deg, state_pitch_deg = runtime.read_deg() + except Exception: + logger.exception("OpenNeck state read failed") + state_yaw_deg = state_pitch_deg = None command_seq += 1 neck_command_pub.publish( NECK_COMMAND_TOPIC, @@ -2825,12 +2865,14 @@ def _publish_neck_command( yaw_deg=float(yaw_deg), pitch_deg=float(pitch_deg), seq=command_seq, + state_yaw_deg=state_yaw_deg, + state_pitch_deg=state_pitch_deg, ), ) try: runtime.start() - if neck_cfg.center_on_start: + if neck_cfg.center_on_start or neck_command_pub is not None: _publish_neck_command( timestamp_s=time.monotonic(), active=False, @@ -2932,6 +2974,11 @@ def _main() -> None: hand_mode = str(cfg_get(hands_cfg, "mode", "gripper")).strip().lower() left_pose, right_pose = _configured_open_hand_pose(cfg) command_seq = 0 + recording_enabled = _recording_enabled(cfg) + state_interval_s = ( + 1.0 / float(cfg_get(_recording_cfg(cfg), "fps", 30)) + if recording_enabled else 0.0 + ) def _apply_hand_commands(commands: tuple[HandPoseCommand, ...]) -> bool: nonlocal left_pose, right_pose @@ -2951,8 +2998,20 @@ def _apply_hand_commands(commands: tuple[HandPoseCommand, ...]) -> bool: logger.warning("Ignoring hand command with unsupported side %r", hand_command.side) return changed - def _publish_hand_command(*, timestamp_s: float, active_state: bool) -> None: + def _publish_hand_command( + *, + timestamp_s: float, + active_state: bool, + read_state: bool = True, + ) -> None: nonlocal command_seq + left_state = right_state = None + if recording_enabled and read_state: + try: + left_state = np.asarray(runtime.get_state("left"), dtype=np.float32) + right_state = np.asarray(runtime.get_state("right"), dtype=np.float32) + except Exception: + logger.exception("LinkerHand state read failed") command_seq += 1 hand_command_pub.publish( HAND_COMMAND_TOPIC, @@ -2964,6 +3023,8 @@ def _publish_hand_command(*, timestamp_s: float, active_state: bool) -> None: left_pose=np.asarray(left_pose, dtype=np.float32).copy(), right_pose=np.asarray(right_pose, dtype=np.float32).copy(), seq=command_seq, + left_state=None if left_state is None else left_state.copy(), + right_state=None if right_state is None else right_state.copy(), ), ) @@ -2972,6 +3033,7 @@ def _publish_hand_command(*, timestamp_s: float, active_state: bool) -> None: startup_s = time.monotonic() _apply_hand_commands(startup_commands) _publish_hand_command(timestamp_s=startup_s, active_state=False) + last_state_s = startup_s while not stop_event.is_set(): command = command_sub.recv_latest() if isinstance(command, CommandPacket) and command.command == "shutdown": @@ -2994,9 +3056,11 @@ def _publish_hand_command(*, timestamp_s: float, active_state: bool) -> None: active=active, now_s=now_s, ) - if commands: - if _apply_hand_commands(commands): - _publish_hand_command(timestamp_s=now_s, active_state=active) + commands_changed = bool(commands) and _apply_hand_commands(commands) + state_due = recording_enabled and now_s - last_state_s >= state_interval_s + if commands_changed or state_due: + _publish_hand_command(timestamp_s=now_s, active_state=active) + last_state_s = now_s except Exception: logger.exception("Dexterous hand worker tick failed; hand control continues") time.sleep(sleep_s) @@ -3005,7 +3069,7 @@ def _publish_hand_command(*, timestamp_s: float, active_state: bool) -> None: shutdown_commands = runtime.close() shutdown_s = time.monotonic() if _apply_hand_commands(shutdown_commands): - _publish_hand_command(timestamp_s=shutdown_s, active_state=False) + _publish_hand_command(timestamp_s=shutdown_s, active_state=False, read_state=False) finally: hand_sub.close() controller_sub.close() diff --git a/teleopit/sim2real/neck/worker.py b/teleopit/sim2real/neck/worker.py index 7abdfb29..7e63b504 100644 --- a/teleopit/sim2real/neck/worker.py +++ b/teleopit/sim2real/neck/worker.py @@ -26,6 +26,9 @@ def start(self) -> None: if self._cfg.center_on_start: self._device.center() + def read_deg(self) -> tuple[float, float]: + return self._device.read_deg() + def tick( self, *, @@ -76,6 +79,9 @@ class DisabledNeckRuntime: def start(self) -> None: return None + def read_deg(self) -> tuple[float, float]: + raise RuntimeError("OpenNeck control is disabled") + def tick( self, *, diff --git a/tests/test_active_neck.py b/tests/test_active_neck.py index 73a21247..f7f52b2c 100644 --- a/tests/test_active_neck.py +++ b/tests/test_active_neck.py @@ -43,6 +43,9 @@ def move_deg(self, yaw_deg: float, pitch_deg: float) -> tuple[float, float]: self.moves.append((yaw_deg, pitch_deg)) return max(-20.0, min(20.0, yaw_deg)), max(-10.0, min(10.0, pitch_deg)) + def read_deg(self) -> tuple[float, float]: + return -18.5, 9.5 + def close(self) -> None: self.closed = True @@ -130,6 +133,7 @@ def test_neck_runtime_sends_degrees_and_returns_applied_target() -> None: runtime = NeckRuntime(cfg, device=device) runtime.start() + assert runtime.read_deg() == (-18.5, 9.5) command = runtime.tick( hmd_rotation_wxyz=_quat_y(30.0), spine3_rotation_wxyz=_quat_y(0.0), diff --git a/tests/test_dexterous_hand.py b/tests/test_dexterous_hand.py index d1f99f40..c3c3b321 100644 --- a/tests/test_dexterous_hand.py +++ b/tests/test_dexterous_hand.py @@ -408,6 +408,10 @@ class FakeDevice: def connect(self) -> None: calls.append(("connect", None, None)) + def get_state(self, side: str) -> tuple[float, ...]: + start = 1.0 if side == "left" else 11.0 + return tuple(start + index for index in range(6)) + def send_pose(self, side, pose, *, force=False, reason="") -> None: calls.append((side, tuple(pose), reason)) @@ -436,6 +440,7 @@ def close(self) -> None: runtime = HandRuntime(FakeDevice(), mapper, open_commands=open_commands) startup = runtime.start() + assert runtime.get_state("right") == (11.0, 12.0, 13.0, 14.0, 15.0, 16.0) ticked = runtime.tick(controller_snapshot=None, hand_snapshot=None, active=True, now_s=1.0) mapper.fail = True failure = runtime.tick(controller_snapshot=None, hand_snapshot=None, active=True, now_s=2.0) diff --git a/tests/test_recording_viewer.py b/tests/test_recording_viewer.py index 7f159b94..6e802264 100644 --- a/tests/test_recording_viewer.py +++ b/tests/test_recording_viewer.py @@ -21,8 +21,10 @@ ACTION_KEY, FRAME_INDEX_KEY, HAND_ACTION_KEY, + HAND_STATE_KEY, MODE_KEY, NECK_ACTION_KEY, + NECK_STATE_KEY, RecordingSchema, STATE_KEY, TIMESTAMP_KEY, @@ -66,9 +68,17 @@ def _write_recording( h5.create_dataset(MODE_KEY, data=np.ones(frames, dtype=np.int8)) h5.create_dataset(ACTION_KEY, data=action) if hand_type != "none": + h5.create_dataset(HAND_STATE_KEY, data=np.full((frames, 12), 20.0, dtype=np.float32)) h5.create_dataset(HAND_ACTION_KEY, data=np.zeros((frames, 12), dtype=np.float32)) if neck_type != "none": - h5.create_dataset(NECK_ACTION_KEY, data=np.zeros((frames, 2), dtype=np.float32)) + h5.create_dataset( + NECK_STATE_KEY, + data=np.tile(np.array([11.5, -7.5], dtype=np.float32), (frames, 1)), + ) + h5.create_dataset( + NECK_ACTION_KEY, + data=np.tile(np.array([12.5, -8.0], dtype=np.float32), (frames, 1)), + ) video_path = root / "videos" / "d435i_rgb" / "episode_000000.mp4" video_path.parent.mkdir(parents=True) @@ -99,8 +109,12 @@ def test_recording_viewer_loads_schema_episode_and_tracking_metrics(tmp_path: Pa assert dataset.image_shape == (4, 6, 3) assert dataset.has_hand_action is True assert dataset.has_neck_action is True + assert data.hand_state is not None assert data.hand_action is not None + assert data.neck_state is not None assert data.neck_action is not None + np.testing.assert_allclose(data.hand_state[0], 20.0) + np.testing.assert_allclose(data.neck_state[0], [11.5, -7.5]) assert data.joint_rmse_rad == pytest.approx(0.1) assert data.root_orientation_rmse_rad == pytest.approx(0.0) assert data.max_joint_error_rad == pytest.approx(0.1) diff --git a/tests/test_sim2real_multiprocess.py b/tests/test_sim2real_multiprocess.py index 278553fd..6436108d 100644 --- a/tests/test_sim2real_multiprocess.py +++ b/tests/test_sim2real_multiprocess.py @@ -19,11 +19,13 @@ ACTION_KEY, FRAME_INDEX_KEY, HAND_ACTION_KEY, + HAND_STATE_KEY, HDF5_RECORDING_FORMAT, HDF5_RECORDING_VERSION, IMAGE_KEY, MODE_KEY, NECK_ACTION_KEY, + NECK_STATE_KEY, STATE_KEY, TIMESTAMP_KEY, build_mode_observation, @@ -126,6 +128,26 @@ def test_sim2real_runtime_rejects_recording_without_input_video() -> None: Sim2RealRuntime(cfg) +@pytest.mark.parametrize( + ("device_cfg", "message"), + [ + ({"hands": {"enabled": True, "sides": ["left"]}}, "hands.sides"), + ({"neck": {"enabled": True, "dry_run": True}}, "neck.dry_run"), + ], +) +def test_sim2real_runtime_rejects_recording_without_device_readback( + device_cfg: dict[str, object], + message: str, +) -> None: + cfg = { + "input": {"provider": "pico4"}, + "recording": {"enabled": True}, + **device_cfg, + } + with pytest.raises(ValueError, match=message): + Sim2RealRuntime(cfg) + + def test_shared_frame_ring_roundtrip() -> None: writer = SharedFrameRingWriter(shape=(2, 3, 1), dtype=np.uint8, slots=2) reader = SharedFrameRingReader() @@ -469,11 +491,15 @@ def close(self) -> None: def test_neck_command_publisher_is_only_created_for_recording(monkeypatch, recording_enabled: bool) -> None: publisher_endpoints: list[str] = [] published_topics: list[str] = [] + published_packets: list[object] = [] class FakeRuntime: def start(self) -> None: return None + def read_deg(self) -> tuple[float, float]: + return 1.0, -2.0 + def close(self) -> None: return None @@ -489,8 +515,8 @@ def __init__(self, endpoint: str) -> None: publisher_endpoints.append(endpoint) def publish(self, topic: str, payload: object) -> None: - del payload published_topics.append(topic) + published_packets.append(payload) def close(self) -> None: return None @@ -512,6 +538,11 @@ def close(self) -> None: assert publisher_endpoints == ([endpoints.neck_command_pub] if recording_enabled else []) assert published_topics == (["neck_command"] if recording_enabled else []) + if recording_enabled: + packet = published_packets[0] + assert isinstance(packet, NeckCommandPacket) + assert packet.state_yaw_deg == 1.0 + assert packet.state_pitch_deg == -2.0 @pytest.mark.parametrize("worker_name", ["neck_worker", "pico_input"]) @@ -602,7 +633,7 @@ def test_hdf5_recording_schema() -> None: features = sidecar["features"] assert sidecar["format"] == HDF5_RECORDING_FORMAT - assert sidecar["version"] == HDF5_RECORDING_VERSION + assert sidecar["version"] == HDF5_RECORDING_VERSION == 4 assert sidecar["fps"] == 30 assert sidecar["robot_type"] == "unitree_g1_29dof" assert sidecar["hand_type"] == "linkerhand_o6" @@ -615,19 +646,26 @@ def test_hdf5_recording_schema() -> None: assert features[MODE_KEY]["shape"] == [] assert features[MODE_KEY]["dtype"] == "int8" assert features[ACTION_KEY]["shape"] == [36] + assert features[HAND_STATE_KEY]["shape"] == [12] assert features[HAND_ACTION_KEY]["shape"] == [12] + assert features[NECK_STATE_KEY]["shape"] == [2] assert features[NECK_ACTION_KEY]["shape"] == [2] assert features[STATE_KEY]["groups"]["joint_pos"] == [0, 29] assert features[STATE_KEY]["groups"]["projected_gravity"] == [65, 68] assert features[MODE_KEY]["values"]["pause"] == 3 assert features[ACTION_KEY]["groups"]["reference_joint_pos"] == [7, 36] + assert features[HAND_STATE_KEY]["groups"]["left_hand_state"] == [0, 6] + assert features[HAND_STATE_KEY]["groups"]["right_hand_state"] == [6, 12] assert features[HAND_ACTION_KEY]["groups"]["left_hand_target"] == [0, 6] assert features[HAND_ACTION_KEY]["groups"]["right_hand_target"] == [6, 12] assert features[NECK_ACTION_KEY]["names"] == ["yaw_deg", "pitch_deg"] + assert features[NECK_STATE_KEY]["names"] == ["yaw_deg", "pitch_deg"] + assert features[NECK_STATE_KEY]["units"] == "degrees" assert features[NECK_ACTION_KEY]["units"] == "degrees" assert "range" not in features[NECK_ACTION_KEY] assert len(features[STATE_KEY]["names"]) == 68 assert len(features[ACTION_KEY]["names"]) == 36 + assert len(features[HAND_STATE_KEY]["names"]) == 12 assert len(features[HAND_ACTION_KEY]["names"]) == 12 @@ -655,7 +693,9 @@ def test_hdf5_recording_schema_optional_action_combinations( assert schema.has_hand_action is has_hand_action assert schema.has_neck_action is has_neck_action + assert (HAND_STATE_KEY in features) is has_hand_action assert (HAND_ACTION_KEY in features) is has_hand_action + assert (NECK_STATE_KEY in features) is has_neck_action assert (NECK_ACTION_KEY in features) is has_neck_action @@ -681,7 +721,9 @@ def test_hdf5_recorder_mp4_sidecar_writes_sync_metadata(tmp_path: Path) -> None: state=np.arange(68, dtype=np.float32), mode=build_mode_observation("mocap"), action=np.arange(36, dtype=np.float32), + hand_state=np.arange(12, dtype=np.float32) + 20.0, hand_action=np.arange(12, dtype=np.float32), + neck_state=np.array([11.5, -7.5], dtype=np.float32), neck_action=np.array([12.5, -8.0], dtype=np.float32), ) recorder.save_episode() @@ -718,8 +760,14 @@ def test_hdf5_recorder_mp4_sidecar_writes_sync_metadata(tmp_path: Path) -> None: assert h5[MODE_KEY].shape == (2,) assert h5[MODE_KEY].dtype == np.dtype(np.int8) assert h5[ACTION_KEY].shape == (2, 36) + assert h5[HAND_STATE_KEY].shape == (2, 12) assert h5[HAND_ACTION_KEY].shape == (2, 12) + assert h5[NECK_STATE_KEY].shape == (2, 2) assert h5[NECK_ACTION_KEY].shape == (2, 2) + np.testing.assert_allclose( + h5[NECK_STATE_KEY][...], + np.array([[11.5, -7.5], [11.5, -7.5]], dtype=np.float32), + ) np.testing.assert_allclose( h5[NECK_ACTION_KEY][...], np.array([[12.5, -8.0], [12.5, -8.0]], dtype=np.float32), @@ -761,7 +809,9 @@ def write_episode(task: str, value: int) -> None: "episode_000001.h5", ] with h5py.File(tmp_path / "data" / "episode_000001.h5", "r") as h5: + assert HAND_STATE_KEY not in h5 assert HAND_ACTION_KEY not in h5 + assert NECK_STATE_KEY not in h5 assert NECK_ACTION_KEY not in h5 @@ -1465,10 +1515,14 @@ def add_frame( state: np.ndarray, mode: object, action: np.ndarray, + hand_state: np.ndarray | None = None, + neck_state: np.ndarray | None = None, hand_action: np.ndarray | None = None, neck_action: np.ndarray | None = None, ) -> None: calls.append("frame") + assert hand_state is not None + assert neck_state is not None assert hand_action is not None assert neck_action is not None frames.append( @@ -1477,6 +1531,8 @@ def add_frame( "state": state.copy(), "mode": np.asarray(mode).copy(), "action": action.copy(), + "hand_state": hand_state.copy(), + "neck_state": neck_state.copy(), "hand_action": hand_action.copy(), "neck_action": neck_action.copy(), } @@ -1546,7 +1602,6 @@ def fake_factory(**_kwargs: object) -> FakeRecorder: worker._save_episode() assert calls == ["start", "discard"] - worker._start_episode() worker._latest_hand_command = HandCommandPacket( timestamp_s=2.05, driver="linkerhand_l6", @@ -1555,6 +1610,8 @@ def fake_factory(**_kwargs: object) -> FakeRecorder: left_pose=np.arange(6, dtype=np.float32), right_pose=np.arange(6, 12, dtype=np.float32), seq=1, + left_state=np.arange(20, 26, dtype=np.float32), + right_state=np.arange(26, 32, dtype=np.float32), ) worker._latest_neck_command = NeckCommandPacket( timestamp_s=2.06, @@ -1563,7 +1620,10 @@ def fake_factory(**_kwargs: object) -> FakeRecorder: yaw_deg=12.5, pitch_deg=-8.0, seq=1, + state_yaw_deg=11.5, + state_pitch_deg=-7.5, ) + worker._start_episode() desc = writer.write(np.full((2, 2, 3), 5, dtype=np.uint8), timestamp_s=2.1) worker._handle_video(desc) worker._save_episode() @@ -1573,7 +1633,9 @@ def fake_factory(**_kwargs: object) -> FakeRecorder: np.testing.assert_allclose(frames[0]["state"], np.arange(68, dtype=np.float32)) assert int(frames[0]["mode"]) == int(build_mode_observation("standing")) np.testing.assert_allclose(frames[0]["action"], np.arange(36, dtype=np.float32)) + np.testing.assert_allclose(frames[0]["hand_state"], np.arange(20, 32, dtype=np.float32)) np.testing.assert_allclose(frames[0]["hand_action"], np.arange(12, dtype=np.float32)) + np.testing.assert_allclose(frames[0]["neck_state"], np.array([11.5, -7.5], dtype=np.float32)) np.testing.assert_allclose(frames[0]["neck_action"], np.array([12.5, -8.0], dtype=np.float32)) worker._latest_record = RecordStepPacket( From 438f804744fb28f085ec3d2700da6ff8453e7cd7 Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Fri, 31 Jul 2026 00:01:52 +0800 Subject: [PATCH 54/59] feat: update high-level policy observation protocol --- AGENTS.md | 3 +- README.md | 10 +- docs/docs/reference/architecture.md | 15 +- .../tutorials/high-level-policy-sim2real.md | 43 ++- .../current/reference/architecture.md | 13 +- .../tutorials/high-level-policy-sim2real.md | 37 ++- .../img/diagrams/architecture-pipeline-zh.svg | 5 +- .../img/diagrams/architecture-pipeline.svg | 5 +- teleopit/high_level_policy/client.py | 45 ++- teleopit/high_level_policy/scheduler.py | 109 +++++-- .../sim2real/mp/high_level_policy_runtime.py | 114 ++++++- .../sim2real/mp/high_level_policy_worker.py | 5 +- teleopit/sim2real/mp/messages.py | 5 +- teleopit/sim2real/mp/runtime.py | 120 +++++++- tests/test_high_level_policy.py | 283 ++++++++++++++++-- 15 files changed, 718 insertions(+), 94 deletions(-) diff --git a/AGENTS.md b/AGENTS.md index c4f586f9..9f307b5a 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -184,7 +184,8 @@ target_dof_pos = clip(action, -10, 10) × action_scale + default_dof_pos - The current client/server code and protocol tests define the ZeroMQ request/response structure. During active development, Teleopit and `lerobot-teleopit` must update that structure together; no legacy network envelope is supported - The only shared data file is `hand_calibration.json`, which contains the LinkerHand O6 open/close calibration and must stay identical in both repositories - The host boundary uses ZeroMQ REQ/REP with msgpack and non-pickle float32 arrays. Deployment is asynchronous and receding-horizon: the isolated client worker keeps at most one request in flight, submits the latest eligible observation every `high_level_policy.replan_steps` 30 Hz source frames, and leaves the current plan executing while inference runs. The onboard scheduler uses the echoed monotonic observation timestamp to skip elapsed source frames and replace the active plan when a newer response arrives; process isolation keeps the 50 Hz robot loop running -- Policy observation is RGB JPEG plus `observation.state(68)`; only `state[58:62]` is rotated into the session-local yaw frame +- Policy `get_action` input is RGB JPEG plus G1 joint positions `float32[29]`, raw measured left/right LinkerHand O6 readback `float32[12]`, measured OpenNeck yaw/pitch degrees `float32[2]`, and the observation-time active reference root pose `float32[7]` (`xyz + quaternion wxyz`) in the session-local frame. The first three arrays form the 43D model state; the source reference pose is used only to reconstruct source-relative root output and is not a model input +- The scheduler keeps a short history of the rate-limited active session-local reference and queries/interpolates it at each camera timestamp. Session reset seeds this history from the initial active reference; the source anchor must never be reconstructed from the robot's measured root pose - Canonical action is `float32[T,50]` with protocol horizon `T` in `[1,50]`: local root `xyz(3)` + local root quaternion `wxyz(4)` + G1 joint reference `29` + left/right O6 closure `12` + OpenNeck yaw/pitch degrees `2` - The body reference `[0:36]` is yaw/XY-delocalized once and sent through the existing motion tracker. It is never sent directly as a G1 motor command and must not pass through the mocap alignment a second time - High-level-policy formal robot modes are `IDLE`, `STANDING`, `POLICY`, and `DAMPING`. After Unitree remote `Y`, Teleopit creates exactly one host session and remains in `STANDING` only while waiting for its first valid chunk; that chunk enters `POLICY` directly. There is no candidate-reference alignment, entry Kp ramp, second session/reset, or `POLICY_STARTING` mode. The 50 Hz output limiter starts from the measured robot reference captured when the session begins, and an entry failure remains in `STANDING` diff --git a/README.md b/README.md index 7f520786..f6fd7af9 100644 --- a/README.md +++ b/README.md @@ -152,6 +152,13 @@ eligible observation every configured `replan_steps` at the 30 Hz action rate while the current plan keeps executing. The isolated client keeps at most one ZeroMQ request in flight. Each newer response is aligned with its echoed onboard monotonic observation timestamp and replaces the active plan. +Each `get_action` request carries the camera JPEG, measured G1 joint positions, +raw measured O6 readback, measured OpenNeck angles, and the active +session-local reference root pose at that camera timestamp. The first three +state arrays form the host's 43D model observation. The source pose is not a +model input; it anchors reconstruction of the model's source-relative root +output. Teleopit's scheduler obtains it from a short history of references +actually sent to the motion tracker, never from the measured robot root. Pico and high-level-policy deployment use separate scripts. The policy runtime does not start PicoBridge, GMR, or the Pico reference worker: @@ -189,7 +196,8 @@ structure. During active development, Teleopit and `lerobot-teleopit` must be updated together. Their only shared data file is `hand_calibration.json`, which contains the LinkerHand O6 open/close calibration. See the [host-policy deployment tutorial](https://BotRunner64.github.io/Teleopit/tutorials/high-level-policy-sim2real) -for the 68D observation, 50D action layout, supported 1-to-50-frame action +for the 43D model observation, source-reference request anchor, 50D action +layout, supported 1-to-50-frame action horizon, safety envelope, host startup, and operator procedure. ## OpenNeck Active Vision diff --git a/docs/docs/reference/architecture.md b/docs/docs/reference/architecture.md index e99cbe70..66d3719c 100644 --- a/docs/docs/reference/architecture.md +++ b/docs/docs/reference/architecture.md @@ -23,11 +23,14 @@ control. These optional hardware paths are supported by onboard deployment; external-host Pico deployment supports whole-body control only. Host-policy deployment is independent from the Pico runtime. A separate host -environment receives JPEG RGB and `observation.state(68)`, then returns -canonical `float32[T,50]` action chunks over strict ZeroMQ/msgpack messages. -The onboard validator and scheduler convert the body portion into a 36D -reference for the existing motion tracker; host output never bypasses that -tracker or becomes a direct motor command. +environment receives JPEG RGB, measured G1 joint positions, raw measured O6 +readback, measured OpenNeck angles, and an observation-time source reference +root pose. The body/hand/neck arrays form the 43D model observation; the +session-local source pose only anchors reconstruction of source-relative root +output. The host returns canonical `float32[T,50]` action chunks over strict +ZeroMQ/msgpack messages. The onboard validator and scheduler convert the body +portion into a 36D reference for the existing motion tracker; host output never +bypasses that tracker or becomes a direct motor command. The Teleopit and host environments share semantic data and one identical `hand_calibration.json`, but do not import each other's Python packages. The @@ -102,7 +105,7 @@ tests/ — Unit, protocol and integration tests | Distributed motion data | Minimal recursive HDF5 `shard_*.h5` files | | Optional hands | LinkerHand L6/O6 with gripper or PICO hand-pose input | | Optional active vision | OpenNeck yaw/pitch in physical degrees | -| Host-policy observation | JPEG RGB + `observation.state(68)` | +| Host-policy observation | JPEG RGB + G1 joint position (29D) + raw O6 readback (12D) + OpenNeck degrees (2D); request also carries the camera-time active reference root pose (7D) | | Host-policy action | `float32[T,50]`, 30 Hz source horizon, `T` in `[1,50]` | | Host-policy body control | 36D root/joint reference through the existing 50 Hz motion tracker | diff --git a/docs/docs/tutorials/high-level-policy-sim2real.md b/docs/docs/tutorials/high-level-policy-sim2real.md index 7beb45c4..50f6754b 100644 --- a/docs/docs/tutorials/high-level-policy-sim2real.md +++ b/docs/docs/tutorials/high-level-policy-sim2real.md @@ -13,10 +13,11 @@ ZeroMQ/msgpack messages. Host workstation (lerobot-teleopit) ReplayPolicy or ACT -> policy server | - | float32 state/action + JPEG over TCP + | float32 observations/actions + JPEG over TCP v G1 onboard computer (Teleopit) - RealSense + G1 state -> asynchronous client -> validated 30 Hz action plan + RealSense + G1/O6/OpenNeck state -> asynchronous client + -> validated 30 Hz action plan -> timestamp-aligned receding-horizon replacement -> 50 Hz interpolation -> motion tracker -> G1 joint-angle targets -> LinkerHand O6 / OpenNeck @@ -41,10 +42,26 @@ Teleopit/teleopit/high_level_policy/hand_calibration.json ``` `hand_calibration.json` defines the LinkerHand O6 raw open/close values and -range tolerance. The current `describe` response identifies the 68D -observation as `teleopit-g1-state` and the canonical 50D action as -`teleopit-g1-reference`. The action layout and physical-degree OpenNeck -commands are enforced by the current code and tests. +range tolerance. The current `describe` response identifies the 43D model +observation as `teleopit-g1-joint-pos-dex-neck-state` and the canonical 50D +action as `teleopit-g1-reference`. The action layout and physical-degree +OpenNeck commands are enforced by the current code and tests. + +Every `get_action` request contains: + +```text +body_joint_positions float32[29] measured G1 joint positions, radians +dex_state float32[12] raw measured left/right O6 readback +neck_state float32[2] measured yaw/pitch, physical degrees +source_reference_root_pose float32[7] session-local xyz + quaternion wxyz +image JPEG RGB 640x480 camera observation +``` + +The host calibrates the raw O6 values and combines the first three arrays into +the 43D model state. `source_reference_root_pose` is the active reference root +at the camera timestamp. It is not a model input: the host uses it to +reconstruct session-local absolute root poses from source-relative model +output. The canonical action layout is: @@ -125,9 +142,10 @@ Teleopit submits the latest eligible observation every `high_level_policy.replan_steps` 30 Hz source frames; the default is three. The stride must not exceed `max_action_horizon` from the host's `describe` response. The isolated client permits only one REQ/REP exchange at a time, but -the active plan continues while that request is in flight. The ACT host uses -the echoed onboard monotonic timestamp to aggregate overlapping predictions, -and Teleopit uses the same timestamp to replace the active plan at the correct +the active plan continues while that request is in flight. Each learned-policy +response is independently reconstructed from its request's source reference; +the host does not aggregate overlapping chunks. Teleopit uses the echoed +onboard monotonic timestamp to replace the active plan at the correct source-frame position. The production camera contract is exactly RGB `uint8[480,640,3]` at 30 Hz. @@ -214,6 +232,13 @@ commands. Invalid or stale responses are rejected without replacing the active valid plan. After recovery, `B` requests resume; execution stays paused until a fresh validated chunk arrives. Only `X` changes the mode to `STANDING`. +The scheduler records its rate-limited session-local reference at 50 Hz. +For each camera frame it looks up or interpolates that history at the camera's +monotonic timestamp and sends the resulting root pose as +`source_reference_root_pose`. Session start seeds the history from the active +standing reference, and pause/resume records held-reference boundaries, so the +anchor never comes from measured robot root pose. + The default safety envelope lives under `high_level_policy.safety` in `high_level_policy_sim2real.yaml`. Adjust it only after checking the recorded data, G1 joint limits, and the installed OpenNeck calibration. diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/architecture.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/architecture.md index 6e4214c5..ee24882f 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/architecture.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/reference/architecture.md @@ -18,10 +18,13 @@ Pico 手部和主动视觉路径是可选的进程隔离 worker。它们复用 `PicoBridge` 接收器，不会向 167 维运控策略观测增加字段。手部或颈部故障不能停止 G1 身体控制。这些可选硬件路径只支持机载部署；外部主机 Pico 部署只支持全身控制。 -主机高层策略部署与 Pico 运行时彼此独立。单独的主机环境接收 JPEG RGB 和 -`observation.state(68)`，再通过严格的 ZeroMQ/msgpack 消息返回 canonical -`float32[T,50]` action chunk。机载校验器和调度器把其中的身体部分转换为 36 维参考， -交给现有 motion tracker；主机输出不能绕过 tracker，也不能直接成为电机命令。 +主机高层策略部署与 Pico 运行时彼此独立。单独的主机环境接收 JPEG RGB、G1 实测关节 +位置、O6 原始实测 readback、OpenNeck 实测角度，以及 observation 时刻的 source +reference root pose。身体、手部和颈部数组组成 43 维模型观测；session-local source +pose 只用于重建 source-relative root 输出。主机再通过严格的 ZeroMQ/msgpack 消息返回 +canonical `float32[T,50]` action chunk。机载校验器和调度器把其中的身体部分转换为 +36 维参考，交给现有 motion tracker；主机输出不能绕过 tracker，也不能直接成为电机 +命令。 Teleopit 和主机环境共享语义数据和一份完全相同的 `hand_calibration.json`，但不会导入 对方的 Python 包。当前 client/server 代码和协议测试定义网络结构，因此协议变化时 @@ -92,7 +95,7 @@ tests/ — 单元、协议和集成测试 | 分发动作数据 | 递归 minimal HDF5 `shard_*.h5` 文件 | | 可选手部 | LinkerHand L6/O6，支持 gripper 或 PICO 手部姿态输入 | | 可选主动视觉 | 使用物理角度的 OpenNeck yaw/pitch | -| 主机策略观测 | JPEG RGB + `observation.state(68)` | +| 主机策略观测 | JPEG RGB + G1 关节位置（29 维）+ O6 原始 readback（12 维）+ OpenNeck 角度（2 维）；请求还携带相机时刻的 active reference root pose（7 维） | | 主机策略动作 | `float32[T,50]`，30 Hz 源时间线，`T` 在 `[1,50]` 内 | | 主机策略身体控制 | 36 维根部/关节参考，通过现有 50 Hz motion tracker | diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md index 79fb0055..abaf5abe 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md @@ -12,10 +12,11 @@ ZeroMQ/msgpack 消息通信。 主机工作站（lerobot-teleopit） ReplayPolicy 或 ACT -> policy server | - | 通过 TCP 传输 float32 state/action + JPEG + | 通过 TCP 传输 float32 observations/actions + JPEG v G1 onboard 计算机（Teleopit） - RealSense + G1 state -> 异步 client -> 已验证的 30 Hz action plan + RealSense + G1/O6/OpenNeck state -> 异步 client + -> 已验证的 30 Hz action plan -> 按时间戳对齐的 receding-horizon 替换 -> 50 Hz 插值 -> motion tracker -> G1 关节角目标 -> LinkerHand O6 / OpenNeck @@ -38,9 +39,24 @@ Teleopit/teleopit/high_level_policy/hand_calibration.json ``` `hand_calibration.json` 定义 LinkerHand O6 的 raw open/close 值和 range tolerance。 -当前 `describe` 响应将 68D observation 标识为 `teleopit-g1-state`，将 canonical 50D -action 标识为 `teleopit-g1-reference`。action 布局和使用物理角度的 OpenNeck 命令由 -当前代码与测试约束。 +当前 `describe` 响应将 43 维模型 observation 标识为 +`teleopit-g1-joint-pos-dex-neck-state`，将 canonical 50 维 action 标识为 +`teleopit-g1-reference`。action 布局和使用物理角度的 OpenNeck 命令由当前代码与测试 +约束。 + +每个 `get_action` 请求都包含： + +```text +body_joint_positions float32[29] G1 实测关节位置，弧度 +dex_state float32[12] 左/右 O6 原始实测 readback +neck_state float32[2] 实测 yaw/pitch，物理角度 +source_reference_root_pose float32[7] session-local xyz + quaternion wxyz +image JPEG RGB 640x480 相机观测 +``` + +主机会标定 O6 原始值，并把前三个数组组合成 43 维模型 state。 +`source_reference_root_pose` 是相机时间戳对应的 active reference root。它不是模型 +输入；主机用它从 source-relative 模型输出重建 session-local absolute root pose。 canonical action 布局为： @@ -116,8 +132,9 @@ python scripts/run/run_high_level_policy_sim2real.py \ Teleopit 每隔 `high_level_policy.replan_steps` 个 30 Hz source frame 提交最新的合格 observation，默认间隔为三帧。该 stride 不得超过主机 `describe` 响应中的 `max_action_horizon`。隔离的 client 同一时间只允许一个 REQ/REP exchange，但该请求 -在途时 active plan 会继续执行。ACT 主机使用回显的 onboard 单调时间戳聚合相互重叠的 -prediction；Teleopit 使用同一时间戳，在正确的 source-frame 位置替换 active plan。 +在途时 active plan 会继续执行。每份 learned-policy response 都基于其请求中的 source +reference 独立重建；主机不会聚合相互重叠的 chunk。Teleopit 使用回显的 onboard 单调 +时间戳，在正确的 source-frame 位置替换 active plan。 生产相机契约固定为 30 Hz 的 RGB `uint8[480,640,3]`。 `camera.source=test-pattern` 只用于受控集成测试；部署时应使用 @@ -188,6 +205,12 @@ camera/client worker 退出，Teleopit 会保持在 `POLICY`，进入普通的 plan。故障恢复后按 `B` 请求恢复；在收到新的有效 chunk 前，执行仍保持暂停。只有 `X` 会把模式切换到 `STANDING`。 +scheduler 会以 50 Hz 记录经过限速的 session-local reference。对于每个相机帧，它会 +在相机单调时间戳处查询该历史或进行插值，并把得到的 root pose 作为 +`source_reference_root_pose` 发送。session 启动时用当时的 active standing reference +初始化历史；pause/resume 会记录 held-reference 边界，因此锚点绝不会来自机器人实测 +root pose。 + 默认安全范围位于 `high_level_policy_sim2real.yaml` 的 `high_level_policy.safety` 下。只有在检查录制数据、G1 关节限位和已安装的 OpenNeck 校准后，才应调整这些值。 diff --git a/docs/static/img/diagrams/architecture-pipeline-zh.svg b/docs/static/img/diagrams/architecture-pipeline-zh.svg index 040b9537..0b916ca6 100644 --- a/docs/static/img/diagrams/architecture-pipeline-zh.svg +++ b/docs/static/img/diagrams/architecture-pipeline-zh.svg @@ -56,8 +56,9 @@ 独立主机高层策略部署 - 机载观测 - RealSense JPEG + state(68) + 机载观测 + JPEG + 43 维实测状态 + + 相机时刻参考根部 主机策略服务 diff --git a/docs/static/img/diagrams/architecture-pipeline.svg b/docs/static/img/diagrams/architecture-pipeline.svg index 75183389..15c01875 100644 --- a/docs/static/img/diagrams/architecture-pipeline.svg +++ b/docs/static/img/diagrams/architecture-pipeline.svg @@ -56,8 +56,9 @@ INDEPENDENT HOST-POLICY DEPLOYMENT - Onboard observation - RealSense JPEG + state(68) + Onboard observation + JPEG + 43D measured state + + camera-time reference root Host policy server diff --git a/teleopit/high_level_policy/client.py b/teleopit/high_level_policy/client.py index e847ecbf..66ef776c 100644 --- a/teleopit/high_level_policy/client.py +++ b/teleopit/high_level_policy/client.py @@ -91,8 +91,8 @@ def describe(self) -> PolicyDescription: if set(data) != expected_fields: raise PolicyProtocolError("invalid_response", "describe data contains unexpected fields") expected_schema = { - "observation_schema": "teleopit-g1-state", - "observation_dim": 68, + "observation_schema": "teleopit-g1-joint-pos-dex-neck-state", + "observation_dim": 43, "action_schema": "teleopit-g1-reference", "action_dim": 50, } @@ -134,7 +134,10 @@ def get_action( onboard_monotonic_timestamp_ns: int, task: str, jpeg_image: bytes, - state: object, + body_joint_positions: object, + dex_state: object, + neck_state: object, + source_reference_root_pose: object, ) -> PolicyActionChunk: if not isinstance(jpeg_image, bytes): raise PolicyProtocolError("invalid_image", "jpeg_image must be bytes") @@ -142,13 +145,24 @@ def get_action( raise PolicyProtocolError("invalid_image", "jpeg_image size is outside limits") if not jpeg_image.startswith(b"\xff\xd8") or not jpeg_image.endswith(b"\xff\xd9"): raise PolicyProtocolError("invalid_image", "jpeg_image is missing JPEG start/end markers") - state_array = np.asarray(state, dtype=np.float32) - if state_array.shape != (68,) or not np.all(np.isfinite(state_array)): - raise PolicyProtocolError("invalid_state", f"state must be finite float32[68], got {state_array.shape}") - quaternion_norm = float(np.linalg.norm(state_array[58:62])) + body_array = _finite_float32_vector( + body_joint_positions, + name="body_joint_positions", + size=29, + ) + dex_array = _finite_float32_vector(dex_state, name="dex_state", size=12) + neck_array = _finite_float32_vector(neck_state, name="neck_state", size=2) + source_pose_array = _finite_float32_vector( + source_reference_root_pose, + name="source_reference_root_pose", + size=7, + ) + quaternion_norm = float(np.linalg.norm(source_pose_array[3:7])) if abs(quaternion_norm - 1.0) > 1e-3: raise PolicyProtocolError( - "invalid_state", f"state base quaternion norm must be near 1, got {quaternion_norm:.6g}" + "invalid_reference_pose", + "source_reference_root_pose quaternion norm must be near 1, " + f"got {quaternion_norm:.6g}", ) request_data = { "session_id": session_id, @@ -160,7 +174,10 @@ def get_action( "task": task, "image_encoding": "jpeg", "image": jpeg_image, - "state": encode_float32_array(state_array), + "body_joint_positions": encode_float32_array(body_array), + "dex_state": encode_float32_array(dex_array), + "neck_state": encode_float32_array(neck_array), + "source_reference_root_pose": encode_float32_array(source_pose_array), } data = self._request("get_action", request_data) expected_fields = { @@ -274,3 +291,13 @@ def _int64(value: object, *, name: str) -> int: if not isinstance(value, int) or isinstance(value, bool) or not 0 <= value <= 2**63 - 1: raise PolicyProtocolError("invalid_value", f"{name} must be an int64 in [0, 2^63-1]") return int(value) + + +def _finite_float32_vector(value: object, *, name: str, size: int) -> np.ndarray: + array = np.asarray(value, dtype=np.float32) + if array.shape != (size,) or not np.all(np.isfinite(array)): + raise PolicyProtocolError( + "invalid_observation", + f"{name} must be finite float32[{size}], got {array.shape}", + ) + return np.ascontiguousarray(array, dtype=np.float32) diff --git a/teleopit/high_level_policy/scheduler.py b/teleopit/high_level_policy/scheduler.py index 06b9c64b..205c0115 100644 --- a/teleopit/high_level_policy/scheduler.py +++ b/teleopit/high_level_policy/scheduler.py @@ -2,6 +2,8 @@ from __future__ import annotations +from bisect import bisect_right +from collections import deque from dataclasses import dataclass import math @@ -15,11 +17,11 @@ from teleopit.sim.reference_motion import interpolate_retarget_qpos -STATE_DIM = 68 ACTION_DIM = 50 BODY_ACTION_DIM = 36 -STATE_BASE_QUATERNION = slice(58, 62) ROOT_QUATERNION = slice(3, 7) +REFERENCE_HISTORY_SIZE = 256 +MAX_REFERENCE_INTERPOLATION_GAP_PERIODS = 2.5 def _normalized_quaternion(value: object, *, name: str) -> np.ndarray: @@ -59,20 +61,6 @@ def from_robot_pose(cls, root_xy: object, quaternion_wxyz: object) -> "PolicyFra yaw_rad=_yaw_from_quaternion(quaternion_wxyz), ) - def localize_state(self, state: object) -> np.ndarray: - localized = np.asarray(state, dtype=np.float32).reshape(-1).copy() - if localized.shape != (STATE_DIM,) or not np.all(np.isfinite(localized)): - raise ValueError(f"High-level policy state must be finite float32[{STATE_DIM}]") - base_quaternion = _normalized_quaternion( - localized[STATE_BASE_QUATERNION], name="state base quaternion" - ) - inverse_yaw = quat_inv_np(_yaw_quaternion(self.yaw_rad)) - localized_quaternion = quat_mul_np(inverse_yaw, base_quaternion) - localized[STATE_BASE_QUATERNION] = _normalized_quaternion( - localized_quaternion, name="localized state base quaternion" - ) - return localized - def localize_body_action(self, action: object) -> np.ndarray: body = np.asarray(action, dtype=np.float32).reshape(-1).copy() if body.shape != (BODY_ACTION_DIM,) or not np.all(np.isfinite(body)): @@ -128,6 +116,9 @@ def __init__( self._paused_at_s: float | None = None self._timestamp_shift_s = 0.0 self._last_output_action: np.ndarray | None = None + self._reference_history: deque[tuple[float, np.ndarray]] = deque( + maxlen=REFERENCE_HISTORY_SIZE + ) @property def session_id(self) -> str | None: @@ -141,9 +132,20 @@ def has_chunk(self) -> bool: def paused(self) -> bool: return self._paused_at_s is not None - def reset(self, session_id: str, *, initial_action: object | None = None) -> None: + def reset( + self, + session_id: str, + *, + initial_action: object | None = None, + initial_reference: object | None = None, + initial_timestamp_s: float | None = None, + ) -> None: if not isinstance(session_id, str) or not session_id: raise ValueError("High-level policy session_id must be non-empty") + if (initial_reference is None) != (initial_timestamp_s is None): + raise ValueError( + "High-level policy initial reference and initial_timestamp_s must be provided together" + ) self._session_id = session_id self._chunk = None self._last_source_sequence_id = -1 @@ -158,6 +160,10 @@ def reset(self, session_id: str, *, initial_action: object | None = None) -> Non self._last_output_action = ( None if initial_output is None else initial_output.copy() ) + self._reference_history.clear() + if initial_reference is not None: + assert initial_timestamp_s is not None + self._record_reference(initial_timestamp_s, initial_reference) def clear(self) -> None: self._session_id = None @@ -167,6 +173,7 @@ def clear(self) -> None: self._paused_at_s = None self._timestamp_shift_s = 0.0 self._last_output_action = None + self._reference_history.clear() def accept(self, chunk: PolicyActionChunk, *, now_s: float) -> None: self._accept(chunk, now_s=now_s) @@ -243,16 +250,21 @@ def _accept( def pause(self, now_s: float) -> None: if self._paused_at_s is None: - self._paused_at_s = float(now_s) + timestamp_s = self._validated_timestamp(now_s, name="pause now_s") + self._paused_at_s = timestamp_s + self._record_last_output_reference(timestamp_s) def resume(self, now_s: float) -> None: if self._paused_at_s is None: return - self._timestamp_shift_s += max(0.0, float(now_s) - self._paused_at_s) + timestamp_s = self._validated_timestamp(now_s, name="resume now_s") + self._timestamp_shift_s += max(0.0, timestamp_s - self._paused_at_s) self._paused_at_s = None + self._record_last_output_reference(timestamp_s) def sample(self, now_s: float) -> np.ndarray | None: - desired = self._sample_unlimited(now_s) + timestamp_s = self._validated_timestamp(now_s, name="sample now_s") + desired = self._sample_unlimited(timestamp_s) if desired is None: return None previous = self._last_output_action @@ -260,8 +272,65 @@ def sample(self, now_s: float) -> np.ndarray | None: if previous is not None and safety is not None: desired = self._rate_limit_output(previous, desired, safety=safety) self._last_output_action = desired.copy() + self._record_reference(timestamp_s, desired) return desired + def reference_root_pose_at(self, timestamp_s: float) -> np.ndarray | None: + """Return the active session-local reference root at a camera timestamp.""" + + query_s = self._validated_timestamp(timestamp_s, name="reference timestamp_s") + history = tuple(self._reference_history) + if not history: + return None + if query_s <= history[0][0]: + return history[0][1].copy() + if query_s >= history[-1][0]: + return history[-1][1].copy() + timestamps = [sample[0] for sample in history] + right_index = bisect_right(timestamps, query_s) + left_timestamp, left_pose = history[right_index - 1] + right_timestamp, right_pose = history[right_index] + if ( + right_timestamp - left_timestamp + > MAX_REFERENCE_INTERPOLATION_GAP_PERIODS / self.output_hz + ): + return left_pose.copy() + alpha = (query_s - left_timestamp) / (right_timestamp - left_timestamp) + return np.asarray( + interpolate_retarget_qpos(left_pose, right_pose, alpha), + dtype=np.float32, + ) + + def _record_last_output_reference(self, timestamp_s: float) -> None: + if self._last_output_action is not None: + self._record_reference(timestamp_s, self._last_output_action) + + def _record_reference(self, timestamp_s: float, action: object) -> None: + timestamp = self._validated_timestamp( + timestamp_s, + name="reference history timestamp_s", + ) + validated = self._validate_single_action(action, name="reference history action") + root_pose = validated[:7].copy() + if self._reference_history: + latest_timestamp = self._reference_history[-1][0] + if timestamp < latest_timestamp: + raise ValueError( + "High-level policy reference history timestamp must not decrease: " + f"last={latest_timestamp:.9f}s received={timestamp:.9f}s" + ) + if timestamp == latest_timestamp: + self._reference_history[-1] = (timestamp, root_pose) + return + self._reference_history.append((timestamp, root_pose)) + + @staticmethod + def _validated_timestamp(value: object, *, name: str) -> float: + timestamp = float(value) + if not np.isfinite(timestamp) or timestamp < 0.0: + raise ValueError(f"High-level policy {name} must be finite and >= 0") + return timestamp + def _sample_unlimited(self, now_s: float) -> np.ndarray | None: chunk = self._chunk if chunk is None: diff --git a/teleopit/sim2real/mp/high_level_policy_runtime.py b/teleopit/sim2real/mp/high_level_policy_runtime.py index 84a7de3e..5788107b 100644 --- a/teleopit/sim2real/mp/high_level_policy_runtime.py +++ b/teleopit/sim2real/mp/high_level_policy_runtime.py @@ -31,8 +31,10 @@ from teleopit.sim2real.mp.high_level_policy_worker import HighLevelPolicyWorker from teleopit.sim2real.mp.ipc import ( COMMAND_TOPIC, + HAND_COMMAND_TOPIC, HIGH_LEVEL_POLICY_TARGET_TOPIC, MODE_TOPIC, + NECK_COMMAND_TOPIC, VIDEO_TOPIC, LatestSubscriber, Sim2RealIpcEndpoints, @@ -41,8 +43,10 @@ ) from teleopit.sim2real.mp.messages import ( CommandPacket, + HandCommandPacket, HighLevelPolicyTargetPacket, ModeStatePacket, + NeckCommandPacket, ) from teleopit.sim2real.mp.runtime import ( HIGH_LEVEL_POLICY_FAULT_COMMAND, @@ -214,6 +218,8 @@ def _validate_high_level_policy_runtime_config(cfg: dict[str, Any]) -> None: neck_cfg = parse_neck_config(cfg) if not neck_cfg.enabled or neck_cfg.driver != "openneck": raise ValueError("High-level policy action[48:50] requires neck.enabled=true and driver=openneck") + if neck_cfg.dry_run: + raise ValueError("High-level policy neck_state requires neck.dry_run=false") def _run_high_level_policy_client_worker( @@ -413,23 +419,35 @@ def _apply_policy_hand_target( device: LinkerHandO6Device, target: HighLevelPolicyTargetPacket, calibration: HandCalibration, -) -> None: +) -> tuple[np.ndarray, np.ndarray]: action = _policy_target_action(target) + left_pose = closure_to_o6_pose(action[36:42], calibration) + right_pose = closure_to_o6_pose(action[42:48], calibration) device.send_pose( "left", - closure_to_o6_pose(action[36:42], calibration), + left_pose, reason="policy", ) device.send_pose( "right", - closure_to_o6_pose(action[42:48], calibration), + right_pose, reason="policy", ) + return ( + np.asarray(left_pose, dtype=np.float32), + np.asarray(right_pose, dtype=np.float32), + ) -def _apply_policy_neck_target(device: Any, target: HighLevelPolicyTargetPacket) -> None: +def _apply_policy_neck_target( + device: Any, + target: HighLevelPolicyTargetPacket, +) -> tuple[float, float]: action = _policy_target_action(target) - device.move_deg(float(action[48]), float(action[49])) + yaw_deg = float(action[48]) + pitch_deg = float(action[49]) + device.move_deg(yaw_deg, pitch_deg) + return yaw_deg, pitch_deg def _run_high_level_policy_hand_worker( @@ -445,10 +463,45 @@ def _main() -> None: ) mode_sub = LatestSubscriber(endpoints.mode_pub, MODE_TOPIC) command_sub = LatestSubscriber(endpoints.command_pub, COMMAND_TOPIC) + state_pub = ZmqPublisher(endpoints.hand_command_pub) latest_mode: ModeStatePacket | None = None last_target_seq = -1 was_in_policy = False + state_seq = 0 + last_state_s = float("-inf") + state_period_s = 1.0 / 30.0 + left_pose = np.asarray(config.open_pose, dtype=np.float32) + right_pose = np.asarray(config.open_pose, dtype=np.float32) sleep_s = 1.0 / max(float(cfg_get(_mp_cfg(cfg), "hand_worker_hz", 120.0)), 1.0) + + def _publish_state(*, active: bool, timestamp_s: float) -> None: + nonlocal state_seq + try: + left_state = np.asarray(device.get_state("left"), dtype=np.float32) + right_state = np.asarray(device.get_state("right"), dtype=np.float32) + except Exception as exc: + logger.warning("LinkerHand O6 policy state read failed: %s", exc) + left_state = right_state = None + state_seq += 1 + state_pub.publish( + HAND_COMMAND_TOPIC, + HandCommandPacket( + timestamp_s=float(timestamp_s), + driver="linkerhand_o6", + mode="policy", + active=bool(active), + left_pose=left_pose.copy(), + right_pose=right_pose.copy(), + seq=state_seq, + left_state=( + None if left_state is None else left_state.copy() + ), + right_state=( + None if right_state is None else right_state.copy() + ), + ), + ) + try: device.connect() while not stop_event.is_set(): @@ -461,6 +514,8 @@ def _main() -> None: in_policy = bool(latest_mode is not None and latest_mode.mode == "policy") if was_in_policy and not in_policy: device.open_all(force=True, reason="policy-inactive") + left_pose = np.asarray(config.open_pose, dtype=np.float32) + right_pose = np.asarray(config.open_pose, dtype=np.float32) was_in_policy = in_policy target = target_sub.recv_latest() if _policy_target_is_current( @@ -469,8 +524,16 @@ def _main() -> None: last_target_seq=last_target_seq, max_age_s=config.frame_timeout_s, ): - _apply_policy_hand_target(device, target, calibration) + left_pose, right_pose = _apply_policy_hand_target( + device, + target, + calibration, + ) last_target_seq = int(target.seq) + now_s = time.monotonic() + if now_s - last_state_s >= state_period_s: + _publish_state(active=in_policy, timestamp_s=now_s) + last_state_s = now_s time.sleep(sleep_s) finally: try: @@ -479,6 +542,7 @@ def _main() -> None: target_sub.close() mode_sub.close() command_sub.close() + state_pub.close() _worker_loop("policy_hand", cfg, _main) @@ -495,10 +559,39 @@ def _main() -> None: ) mode_sub = LatestSubscriber(endpoints.mode_pub, MODE_TOPIC) command_sub = LatestSubscriber(endpoints.command_pub, COMMAND_TOPIC) + state_pub = ZmqPublisher(endpoints.neck_command_pub) latest_mode: ModeStatePacket | None = None last_target_seq = -1 was_in_policy = False + state_seq = 0 + last_state_s = float("-inf") + state_period_s = 1.0 / 30.0 + yaw_deg = 0.0 + pitch_deg = 0.0 sleep_s = 1.0 / max(config.rate_hz, 1.0) + + def _publish_state(*, active: bool, timestamp_s: float) -> None: + nonlocal state_seq + try: + state_yaw_deg, state_pitch_deg = device.read_deg() + except Exception as exc: + logger.warning("OpenNeck policy state read failed: %s", exc) + state_yaw_deg = state_pitch_deg = None + state_seq += 1 + state_pub.publish( + NECK_COMMAND_TOPIC, + NeckCommandPacket( + timestamp_s=float(timestamp_s), + driver="openneck", + active=bool(active), + yaw_deg=float(yaw_deg), + pitch_deg=float(pitch_deg), + seq=state_seq, + state_yaw_deg=state_yaw_deg, + state_pitch_deg=state_pitch_deg, + ), + ) + try: device.connect() if config.center_on_start: @@ -513,6 +606,8 @@ def _main() -> None: in_policy = bool(latest_mode is not None and latest_mode.mode == "policy") if was_in_policy and not in_policy: device.center() + yaw_deg = 0.0 + pitch_deg = 0.0 was_in_policy = in_policy target = target_sub.recv_latest() if _policy_target_is_current( @@ -521,8 +616,12 @@ def _main() -> None: last_target_seq=last_target_seq, max_age_s=config.frame_timeout_s, ): - _apply_policy_neck_target(device, target) + yaw_deg, pitch_deg = _apply_policy_neck_target(device, target) last_target_seq = int(target.seq) + now_s = time.monotonic() + if now_s - last_state_s >= state_period_s: + _publish_state(active=in_policy, timestamp_s=now_s) + last_state_s = now_s time.sleep(sleep_s) finally: try: @@ -534,5 +633,6 @@ def _main() -> None: target_sub.close() mode_sub.close() command_sub.close() + state_pub.close() _worker_loop("policy_neck", cfg, _main) diff --git a/teleopit/sim2real/mp/high_level_policy_worker.py b/teleopit/sim2real/mp/high_level_policy_worker.py index 462508c2..cdacf06c 100644 --- a/teleopit/sim2real/mp/high_level_policy_worker.py +++ b/teleopit/sim2real/mp/high_level_policy_worker.py @@ -227,7 +227,10 @@ def _handle_observation(self, packet: HighLevelPolicyObservationPacket) -> None: onboard_monotonic_timestamp_ns=int(packet.onboard_monotonic_timestamp_ns), task=session.task, jpeg_image=jpeg, - state=packet.state, + body_joint_positions=packet.body_joint_positions, + dex_state=packet.dex_state, + neck_state=packet.neck_state, + source_reference_root_pose=packet.source_reference_root_pose, ) if self._policy_id is None or chunk.policy_id != self._policy_id: raise PolicyProtocolError( diff --git a/teleopit/sim2real/mp/messages.py b/teleopit/sim2real/mp/messages.py index 909bd451..41ee11ab 100644 --- a/teleopit/sim2real/mp/messages.py +++ b/teleopit/sim2real/mp/messages.py @@ -139,7 +139,10 @@ class HighLevelPolicyObservationPacket: session_id: str sequence_id: int onboard_monotonic_timestamp_ns: int - state: Float32Array + body_joint_positions: Float32Array + dex_state: Float32Array + neck_state: Float32Array + source_reference_root_pose: Float32Array frame: SharedFrameDescriptor timestamp_s: float diff --git a/teleopit/sim2real/mp/runtime.py b/teleopit/sim2real/mp/runtime.py index cab07eea..ead22db8 100644 --- a/teleopit/sim2real/mp/runtime.py +++ b/teleopit/sim2real/mp/runtime.py @@ -1324,6 +1324,16 @@ def __init__( if self.high_level_policy_enabled else None ) + self._policy_hand_state_sub = ( + LatestSubscriber(endpoints.hand_command_pub, HAND_COMMAND_TOPIC) + if self.high_level_policy_enabled + else None + ) + self._policy_neck_state_sub = ( + LatestSubscriber(endpoints.neck_command_pub, NECK_COMMAND_TOPIC) + if self.high_level_policy_enabled + else None + ) self._policy_action_sub = ( LatestSubscriber(endpoints.high_level_policy_result_pub, HIGH_LEVEL_POLICY_ACTION_TOPIC) if self.high_level_policy_enabled @@ -1341,6 +1351,8 @@ def __init__( ) self._mode_pub = ZmqPublisher(endpoints.mode_pub) self._record_pub = ZmqPublisher(endpoints.record_pub) if _recording_enabled(cfg) else None + self._latest_policy_hand_state: HandCommandPacket | None = None + self._latest_policy_neck_state: NeckCommandPacket | None = None viewers = _parse_sim2real_viewers(cfg) self._retarget_viewer = _Sim2RealRetargetViewer( @@ -1407,6 +1419,8 @@ def shutdown(self) -> None: self._reference_sub, self._events_sub, self._policy_video_sub, + self._policy_hand_state_sub, + self._policy_neck_state_sub, self._policy_action_sub, self._policy_status_sub, ): @@ -1683,6 +1697,14 @@ def _build_high_level_policy_boundary_action(self, state: object) -> np.ndarray: ) return initial_action + def _build_high_level_policy_reference_action(self, reference_qpos: object) -> np.ndarray: + transform = self._policy_frame_transform + if transform is None: + raise RuntimeError("High-level policy entry is missing its frame transform") + initial_reference = np.zeros(50, dtype=np.float32) + initial_reference[:36] = transform.localize_body_action(reference_qpos) + return initial_reference + def _start_high_level_policy_entry_session(self) -> None: policy_cfg = self._high_level_policy_cfg scheduler = self._high_level_policy_scheduler @@ -1696,18 +1718,28 @@ def _start_high_level_policy_entry_session(self) -> None: root_pos[:2], getattr(state, "quat"), ) + active_reference = ( + np.asarray(self._last_commanded_motion_qpos, dtype=np.float64).copy() + if self._last_commanded_motion_qpos is not None + else self._standing_qpos.copy() + ) + session_started_s = time.monotonic() self._policy_session_id = uuid.uuid4().hex scheduler.reset( self._policy_session_id, initial_action=self._build_high_level_policy_boundary_action(state), + initial_reference=self._build_high_level_policy_reference_action( + active_reference + ), + initial_timestamp_s=session_started_s, ) self._policy_entry_pending = True - self._policy_entry_deadline_s = time.monotonic() + policy_cfg.entry_timeout_s + self._policy_entry_deadline_s = session_started_s + policy_cfg.entry_timeout_s self._policy_paused = False self._policy_resume_pending = False self._policy_resume_deadline_s = None self._policy_resume_source_timestamp_ns = None - self._policy_hold_qpos = self._build_robot_state_qpos(state) + self._policy_hold_qpos = active_reference.copy() self._policy_observation_seq = 0 self._last_policy_video_seq = ( -1 @@ -1746,17 +1778,36 @@ def _publish_high_level_policy_observation(self, robot_state: object) -> None: return publisher = self._policy_control_pub frame = self._latest_policy_video - transform = self._policy_frame_transform + scheduler = self._high_level_policy_scheduler session_id = self._policy_session_id policy_cfg = self._high_level_policy_cfg - if publisher is None or frame is None or transform is None or session_id is None or policy_cfg is None: + if ( + publisher is None + or frame is None + or scheduler is None + or session_id is None + or policy_cfg is None + ): return if int(frame.seq) <= self._last_policy_video_seq: return now_s = time.monotonic() if abs(now_s - float(frame.timestamp_s)) > policy_cfg.max_observation_age_s: return - state = transform.localize_state(build_observation_state(robot_state)) + self._drain_high_level_policy_hardware_state() + hardware_state = self._high_level_policy_hardware_state( + now_s=now_s, + max_age_s=policy_cfg.max_observation_age_s, + ) + if hardware_state is None: + return + dex_state, neck_state = hardware_state + source_reference_root_pose = scheduler.reference_root_pose_at( + float(frame.timestamp_s) + ) + if source_reference_root_pose is None: + return + body_joint_positions = build_observation_state(robot_state)[:NUM_JOINTS] sequence_id = self._policy_observation_seq publisher.publish( HIGH_LEVEL_POLICY_OBSERVATION_TOPIC, @@ -1764,7 +1815,13 @@ def _publish_high_level_policy_observation(self, robot_state: object) -> None: session_id=session_id, sequence_id=sequence_id, onboard_monotonic_timestamp_ns=int(round(float(frame.timestamp_s) * 1e9)), - state=state.astype(np.float32, copy=True), + body_joint_positions=body_joint_positions.astype( + np.float32, + copy=True, + ), + dex_state=dex_state, + neck_state=neck_state, + source_reference_root_pose=source_reference_root_pose, frame=frame, timestamp_s=now_s, ), @@ -1772,6 +1829,57 @@ def _publish_high_level_policy_observation(self, robot_state: object) -> None: self._policy_observation_seq += 1 self._last_policy_video_seq = int(frame.seq) + def _drain_high_level_policy_hardware_state(self) -> None: + hand_subscriber = self._policy_hand_state_sub + neck_subscriber = self._policy_neck_state_sub + if hand_subscriber is None or neck_subscriber is None: + return + hand_state = hand_subscriber.recv_latest() + if isinstance(hand_state, HandCommandPacket): + self._latest_policy_hand_state = hand_state + neck_state = neck_subscriber.recv_latest() + if isinstance(neck_state, NeckCommandPacket): + self._latest_policy_neck_state = neck_state + + def _high_level_policy_hardware_state( + self, + *, + now_s: float, + max_age_s: float, + ) -> tuple[np.ndarray, np.ndarray] | None: + hand = self._latest_policy_hand_state + neck = self._latest_policy_neck_state + if ( + hand is None + or hand.left_state is None + or hand.right_state is None + or neck is None + or neck.state_yaw_deg is None + or neck.state_pitch_deg is None + ): + return None + hand_age_s = float(now_s) - float(hand.timestamp_s) + neck_age_s = float(now_s) - float(neck.timestamp_s) + if not ( + np.isfinite(hand_age_s) + and 0.0 <= hand_age_s <= float(max_age_s) + and np.isfinite(neck_age_s) + and 0.0 <= neck_age_s <= float(max_age_s) + ): + return None + left_state = np.asarray(hand.left_state, dtype=np.float32).reshape(-1) + right_state = np.asarray(hand.right_state, dtype=np.float32).reshape(-1) + dex_state = np.concatenate((left_state, right_state), dtype=np.float32) + neck_state = np.asarray( + [neck.state_yaw_deg, neck.state_pitch_deg], + dtype=np.float32, + ) + if dex_state.shape != (12,) or not np.all(np.isfinite(dex_state)): + return None + if neck_state.shape != (2,) or not np.all(np.isfinite(neck_state)): + return None + return dex_state.copy(), neck_state.copy() + def _transition_to_high_level_policy(self) -> None: state = self.robot.get_state() resume_qpos = self._build_robot_state_qpos(state) diff --git a/tests/test_high_level_policy.py b/tests/test_high_level_policy.py index 0a944a97..4a53cf9e 100644 --- a/tests/test_high_level_policy.py +++ b/tests/test_high_level_policy.py @@ -40,12 +40,15 @@ ) from teleopit.sim2real.mp.high_level_policy_worker import HighLevelPolicyWorker from teleopit.sim2real.mp.messages import ( + HandCommandPacket, HighLevelPolicyActionPacket, HighLevelPolicyObservationPacket, HighLevelPolicySessionPacket, HighLevelPolicyStatusPacket, HighLevelPolicyTargetPacket, ModeStatePacket, + NeckCommandPacket, + SharedFrameDescriptor, ) from teleopit.sim2real.mp.runtime import ( RobotMode, @@ -164,15 +167,10 @@ def test_msgpack_float32_array_roundtrip_is_little_endian() -> None: np.testing.assert_allclose(decoded, values.astype(np.float32)) -def test_policy_frame_transform_localizes_state_and_delocalizes_action() -> None: +def test_policy_frame_transform_localizes_and_delocalizes_action() -> None: yaw = math.pi / 2.0 yaw_quaternion = np.array([math.cos(yaw / 2.0), 0.0, 0.0, math.sin(yaw / 2.0)], dtype=np.float32) transform = PolicyFrameTransform.from_robot_pose([2.0, 3.0], yaw_quaternion) - state = np.zeros(68, dtype=np.float32) - state[58:62] = yaw_quaternion - - localized = transform.localize_state(state) - np.testing.assert_allclose(localized[58:62], [1.0, 0.0, 0.0, 0.0], atol=1e-6) body = np.zeros(36, dtype=np.float32) body[0] = 1.0 @@ -231,6 +229,36 @@ def test_scheduler_pause_freezes_and_resume_shifts_plan_time() -> None: np.testing.assert_allclose(resumed, paused) +def test_scheduler_interpolates_active_reference_history_at_camera_timestamp() -> None: + scheduler = HighLevelPolicyScheduler(hold_s=0.1) + initial = _safe_actions(1)[0] + scheduler.reset("session-1", initial_action=initial) + assert scheduler.reference_root_pose_at(1.0) is None + scheduler.reset( + "session-1", + initial_reference=initial, + initial_timestamp_s=1.0, + ) + actions = _safe_actions(2) + actions[1, 0] = 1.0 + yaw = math.pi / 2.0 + actions[1, 3:7] = [ + math.cos(yaw / 2.0), + 0.0, + 0.0, + math.sin(yaw / 2.0), + ] + scheduler.accept(_safe_chunk(actions), now_s=1.0) + scheduler.sample(1.0 + 1.0 / 30.0) + + source_pose = scheduler.reference_root_pose_at(1.0 + 0.5 / 30.0) + + assert source_pose is not None + assert source_pose[0] == pytest.approx(0.5) + source_yaw = 2.0 * math.atan2(float(source_pose[6]), float(source_pose[3])) + assert source_yaw == pytest.approx(math.pi / 4.0) + + def test_scheduler_rejects_wrong_session_and_expired_chunk() -> None: scheduler = HighLevelPolicyScheduler(hold_s=0.0) scheduler.reset("other") @@ -288,7 +316,10 @@ def test_scheduler_accepts_internal_reference_discontinuities() -> None: def test_scheduler_clips_joint_positions_to_onboard_limits() -> None: scheduler = HighLevelPolicyScheduler(hold_s=0.1, safety=_safety_config()) - scheduler.reset("session-1", initial_action=_safe_actions(1)[0]) + scheduler.reset( + "session-1", + initial_action=_safe_actions(1)[0], + ) actions = _safe_actions(1) actions[0, 7] = -3.08 actions[0, 8] = 3.08 @@ -305,7 +336,10 @@ def test_scheduler_clips_joint_positions_to_onboard_limits() -> None: def test_scheduler_clips_openneck_angles_to_onboard_limits() -> None: scheduler = HighLevelPolicyScheduler(hold_s=0.1, safety=_safety_config()) - scheduler.reset("session-1", initial_action=_safe_actions(1)[0]) + scheduler.reset( + "session-1", + initial_action=_safe_actions(1)[0], + ) actions = _safe_actions() actions[:, 48] = [-46.0, 0.0, 46.0] actions[:, 49] = [41.0, 0.0, -41.0] @@ -324,7 +358,10 @@ def test_scheduler_clips_openneck_angles_to_onboard_limits() -> None: def test_scheduler_rejects_joint_projection_above_limit() -> None: scheduler = HighLevelPolicyScheduler(hold_s=0.1, safety=_safety_config()) - scheduler.reset("session-1", initial_action=_safe_actions(1)[0]) + scheduler.reset( + "session-1", + initial_action=_safe_actions(1)[0], + ) actions = _safe_actions(1) actions[0, 7] = -3.11 @@ -335,7 +372,10 @@ def test_scheduler_rejects_joint_projection_above_limit() -> None: def test_scheduler_rejects_entire_unsafe_non_joint_chunk() -> None: scheduler = HighLevelPolicyScheduler(hold_s=0.1, safety=_safety_config()) - scheduler.reset("session-1", initial_action=_safe_actions(1)[0]) + scheduler.reset( + "session-1", + initial_action=_safe_actions(1)[0], + ) actions = _safe_actions() actions[1, 2] = 0.4 @@ -382,8 +422,8 @@ def serve() -> None: name = request["endpoint"] if name == "describe": data = { - "observation_schema": "teleopit-g1-state", - "observation_dim": 68, + "observation_schema": "teleopit-g1-joint-pos-dex-neck-state", + "observation_dim": 43, "action_schema": "teleopit-g1-reference", "action_dim": 50, "dataset_fps": 30, @@ -424,19 +464,73 @@ def serve() -> None: try: description = client.describe() client.reset("session-1", "demo") - state = np.zeros(68, dtype=np.float32) - state[58] = 1.0 + body_joint_positions = np.linspace(-0.2, 0.2, 29, dtype=np.float32) + dex_state = np.arange(12, dtype=np.float32) + 100.0 + neck_state = np.array([5.0, -7.0], dtype=np.float32) + source_reference_root_pose = np.array( + [1.0, 2.0, 0.76, 0.9995, 0.0, 0.0, 0.0], + dtype=np.float32, + ) chunk = client.get_action( session_id="session-1", sequence_id=4, onboard_monotonic_timestamp_ns=123, task="demo", jpeg_image=b"\xff\xd8test\xff\xd9", - state=state, + body_joint_positions=body_joint_positions, + dex_state=dex_state, + neck_state=neck_state, + source_reference_root_pose=source_reference_root_pose, ) assert description.policy_id == "test-policy" np.testing.assert_allclose(chunk.actions, _safe_actions()) assert all(set(request) == {"endpoint", "data"} for request in requests) + get_action_data = requests[2]["data"] + assert isinstance(get_action_data, dict) + assert set(get_action_data) == { + "session_id", + "sequence_id", + "onboard_monotonic_timestamp_ns", + "task", + "image_encoding", + "image", + "body_joint_positions", + "dex_state", + "neck_state", + "source_reference_root_pose", + } + np.testing.assert_array_equal( + decode_float32_array( + get_action_data["body_joint_positions"], + name="body_joint_positions", + expected_shape=(29,), + ), + body_joint_positions, + ) + np.testing.assert_array_equal( + decode_float32_array( + get_action_data["dex_state"], + name="dex_state", + expected_shape=(12,), + ), + dex_state, + ) + np.testing.assert_array_equal( + decode_float32_array( + get_action_data["neck_state"], + name="neck_state", + expected_shape=(2,), + ), + neck_state, + ) + np.testing.assert_array_equal( + decode_float32_array( + get_action_data["source_reference_root_pose"], + name="source_reference_root_pose", + expected_shape=(7,), + ), + source_reference_root_pose, + ) finally: client.close() thread.join(timeout=1.0) @@ -763,7 +857,10 @@ def test_policy_entry_first_chunk_uses_measured_reference_boundary() -> None: worker._build_robot_state_qpos = lambda _state: current_qpos.copy() scheduler = HighLevelPolicyScheduler(hold_s=0.1, safety=_safety_config()) boundary_action = worker._build_high_level_policy_boundary_action(state) - scheduler.reset("session-1", initial_action=boundary_action) + scheduler.reset( + "session-1", + initial_action=boundary_action, + ) worker._high_level_policy_scheduler = scheduler worker._high_level_policy_cfg = SimpleNamespace(max_result_age_s=1.0) worker._enter_standing = lambda: pytest.fail( @@ -783,6 +880,136 @@ def test_policy_entry_first_chunk_uses_measured_reference_boundary() -> None: assert scheduled[7] == pytest.approx(0.6) +def test_policy_session_seeds_source_history_from_active_reference() -> None: + worker = object.__new__(_RobotControlWorker) + state = SimpleNamespace( + qpos=np.linspace(-0.2, 0.2, 29, dtype=np.float32), + quat=np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float32), + base_pos=np.array([10.0, 20.0, 0.76], dtype=np.float64), + ) + worker.robot = SimpleNamespace(get_state=lambda: state) + worker.num_actions = 29 + worker._default_root_pos = np.array([0.0, 0.0, 0.76], dtype=np.float64) + worker._high_level_policy_cfg = SimpleNamespace(entry_timeout_s=5.0) + scheduler = HighLevelPolicyScheduler() + worker._high_level_policy_scheduler = scheduler + worker._policy_session_id = None + worker._latest_policy_video = None + worker._last_commanded_motion_qpos = np.zeros(36, dtype=np.float64) + worker._last_commanded_motion_qpos[:7] = [ + 12.0, + 24.0, + 0.82, + 1.0, + 0.0, + 0.0, + 0.0, + ] + worker._standing_qpos = np.zeros(36, dtype=np.float64) + worker._standing_qpos[3] = 1.0 + worker._publish_high_level_policy_session = lambda *_args, **_kwargs: None + + worker._start_high_level_policy_entry_session() + + source_pose = scheduler.reference_root_pose_at(time.monotonic()) + assert source_pose is not None + np.testing.assert_allclose(source_pose[:3], [2.0, 4.0, 0.82], atol=1e-6) + np.testing.assert_array_equal( + worker._policy_hold_qpos, + worker._last_commanded_motion_qpos, + ) + + +def test_policy_observation_uses_active_reference_and_measured_hardware_state() -> None: + worker = object.__new__(_RobotControlWorker) + now_s = time.monotonic() + frame = SharedFrameDescriptor( + shm_name="camera", + slot=0, + seq=7, + timestamp_s=now_s, + shape=(480, 640, 3), + dtype="uint8", + slots=3, + ) + worker._policy_entry_pending = True + worker.mode = RobotMode.STANDING + worker._policy_paused = False + worker._policy_resume_pending = False + worker._policy_session_id = "session-1" + worker._high_level_policy_cfg = SimpleNamespace(max_observation_age_s=0.15) + worker._latest_policy_video = frame + worker._last_policy_video_seq = -1 + worker._policy_observation_seq = 0 + worker._policy_hand_state_sub = SimpleNamespace(recv_latest=lambda: None) + worker._policy_neck_state_sub = SimpleNamespace(recv_latest=lambda: None) + worker._latest_policy_hand_state = HandCommandPacket( + timestamp_s=now_s, + driver="linkerhand_o6", + mode="policy", + active=False, + left_pose=np.full(6, 250.0, dtype=np.float32), + right_pose=np.full(6, 250.0, dtype=np.float32), + seq=1, + left_state=np.arange(6, dtype=np.float32) + 10.0, + right_state=np.arange(6, dtype=np.float32) + 20.0, + ) + worker._latest_policy_neck_state = NeckCommandPacket( + timestamp_s=now_s, + driver="openneck", + active=False, + yaw_deg=0.0, + pitch_deg=0.0, + seq=1, + state_yaw_deg=12.0, + state_pitch_deg=-8.0, + ) + scheduler = HighLevelPolicyScheduler() + initial_action = _safe_actions(1)[0] + active_reference = initial_action.copy() + active_reference[:7] = [4.0, 5.0, 0.82, 1.0, 0.0, 0.0, 0.0] + scheduler.reset( + "session-1", + initial_action=initial_action, + initial_reference=active_reference, + initial_timestamp_s=now_s, + ) + worker._high_level_policy_scheduler = scheduler + published: list[tuple[str, object]] = [] + worker._policy_control_pub = SimpleNamespace( + publish=lambda topic, packet: published.append((topic, packet)) + ) + robot_state = SimpleNamespace( + qpos=np.linspace(-0.2, 0.2, 29, dtype=np.float32), + qvel=np.zeros(29, dtype=np.float32), + quat=np.array([0.0, 0.0, 0.0, 1.0], dtype=np.float32), + ang_vel=np.zeros(3, dtype=np.float32), + base_pos=np.array([99.0, 98.0, 97.0], dtype=np.float32), + ) + + worker._publish_high_level_policy_observation(robot_state) + + assert len(published) == 1 + packet = published[0][1] + assert isinstance(packet, HighLevelPolicyObservationPacket) + np.testing.assert_array_equal(packet.body_joint_positions, robot_state.qpos) + np.testing.assert_array_equal( + packet.dex_state, + np.concatenate( + ( + worker._latest_policy_hand_state.left_state, + worker._latest_policy_hand_state.right_state, + ) + ), + ) + np.testing.assert_array_equal(packet.neck_state, [12.0, -8.0]) + np.testing.assert_array_equal( + packet.source_reference_root_pose, + active_reference[:7], + ) + assert packet.source_reference_root_pose[0] != robot_state.base_pos[0] + + def test_policy_entry_stale_result_aborts_current_session() -> None: worker = object.__new__(_RobotControlWorker) now_s = time.monotonic() @@ -1011,7 +1238,13 @@ def observation(sequence_id: int, timestamp_ns: int) -> HighLevelPolicyObservati session_id="session-1", sequence_id=sequence_id, onboard_monotonic_timestamp_ns=timestamp_ns, - state=np.zeros(68, dtype=np.float32), + body_joint_positions=np.arange(29, dtype=np.float32), + dex_state=np.arange(12, dtype=np.float32) + 100.0, + neck_state=np.array([5.0, -2.0], dtype=np.float32), + source_reference_root_pose=np.array( + [1.0, 2.0, 0.76, 1.0, 0.0, 0.0, 0.0], + dtype=np.float32, + ), frame=object(), # type: ignore[arg-type] timestamp_s=time.monotonic(), ) @@ -1021,6 +1254,22 @@ def observation(sequence_id: int, timestamp_ns: int) -> HighLevelPolicyObservati assert len(requests) == 1 assert requests[0]["sequence_id"] == 2 + np.testing.assert_array_equal( + requests[0]["body_joint_positions"], + np.arange(29, dtype=np.float32), + ) + np.testing.assert_array_equal( + requests[0]["dex_state"], + np.arange(12, dtype=np.float32) + 100.0, + ) + np.testing.assert_array_equal( + requests[0]["neck_state"], + np.array([5.0, -2.0], dtype=np.float32), + ) + np.testing.assert_array_equal( + requests[0]["source_reference_root_pose"], + np.array([1.0, 2.0, 0.76, 1.0, 0.0, 0.0, 0.0], dtype=np.float32), + ) assert worker._last_request_timestamp_ns == 1_100_000_000 assert len(published) == 1 assert published[0].source_onboard_monotonic_timestamp_ns == 1_100_000_000 From 046dc668ba35d674ae452e80b0d0ecd79e67a98b Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Fri, 31 Jul 2026 19:56:19 +0800 Subject: [PATCH 55/59] docs: trim advanced workflows from README --- README.md | 168 ------------------------------------------------------ 1 file changed, 168 deletions(-) diff --git a/README.md b/README.md index f6fd7af9..cd41f288 100644 --- a/README.md +++ b/README.md @@ -64,174 +64,6 @@ python scripts/run/run_sim.py \ For sim2real, viewers are disabled by default. Add `viewers=retarget` to show the retargeted reference in an optional MuJoCo window. -## Pico Motion Recording - -Record many Pico clips as training-ready G1 motion NPZ files: - -```bash -pip install -e '.[pico4]' -python scripts/run/record_pico_motion.py -``` - -The recorder starts the Pico receiver and live Retarget viewer before waiting -for clip names, so preview keeps running while the terminal is idle. Enter a -semantic clip name, then use `R` to start, `S` to save, `D` to discard, `N` for -a new name, and `Q` to quit. Saved clips are written to -`data/pico_motion/clips/` using the semantic label in the filename, with no -sidecar JSON. - -Merge recorded clips into the standard HDF5 shard dataset: - -```bash -python train_mimic/scripts/data/build_dataset.py \ - --spec data/pico_motion/pico_recorded.yaml --force -``` - -## Sim2Real HDF5 Recording - -Pico sim2real can also record manual HDF5 episodes from the real G1: - -```bash -pip install -e '.[recording]' -# If you use RealSense video, install pyrealsense2 manually for your platform. -# On Arm machines, prefer conda-forge: -# conda install -c conda-forge pyrealsense2 -python scripts/run/run_sim2real.py --config-name sim2real_record \ - controller.policy_path=ckpt/track_g1.onnx \ - recording.task="walk forward" -``` - -Recording uses the terminal controls `R` start, `S` save, `D` discard, and `Q` -shutdown. `STANDING`, `MOCAP`, `ARMS`, and paused mocap can be recorded. Saved -episodes are written under `data/recordings/sim2real_hdf5/data/`, with compressed -MP4 files under `videos/d435i_rgb/`. `schema.json` records the FPS, robot, hand, -and neck types, plus feature shapes, names, and groups. `episodes.jsonl` maps each episode -to its HDF5/video files and stores its editable task prompt. HDF5 contains only -frame-aligned arrays: `observation.state(68)`, scalar `observation.mode`, and -`action(36)` as the aligned reference qpos consumed by the motion tracker. -When LinkerHand control is enabled, `observation.state.hand(12)` contains the -left/right hardware joint readback and `action.hand(12)` contains the target. -When OpenNeck control is enabled, `observation.state.neck(2)` contains the -servo `[yaw_deg, pitch_deg]` readback and `action.neck(2)` contains the latest -mechanically clamped target. -Recording is non-critical: an incompatible output schema stops only the -recording worker while G1 control continues. Episodes interrupted before their -manifest entry is committed are discarded on the next recording startup. -RealSense frame timeouts and disconnects trigger background camera reconnection -without stopping Pico input or G1 control. Recording requires a fresh camera -frame to start and discards an active episode after one second without video; -press `R` again after the camera recovers. If the entire Pico input worker exits, -G1 control remains active and holds the latest command so the operator can use -the Unitree remote to return to `STANDING` or request `DAMPING`. - -Review saved episodes in a synchronized read-only web UI: - -```bash -pip install -e '.[review]' -python scripts/view/view_recording.py \ - --recording data/recordings/sim2real_hdf5 -``` - -The reviewer validates the manifest, HDF5 arrays, and MP4 frame count before -playback. It shows D435i video beside the observed G1 pose with a translucent -green reference overlay, plus mode, joint-tracking, LinkerHand, and OpenNeck -timelines. Because recordings do not contain measured root XYZ, the observed -pose is anchored to the reference root position; joint and root-orientation -comparisons remain valid. - -## Host High-Level Policy Deployment - -Teleopit can run a host-served ReplayPolicy or LeRobot ACT policy through a -dedicated onboard runtime. The host policy remains in the independent -`lerobot-teleopit` repository/environment; Teleopit receives canonical 50D -reference chunks over ZeroMQ, validates and interpolates them onboard, and -rate-limits plan switches at 50 Hz before passing the 36D body reference -through the existing motion tracker. The host never sends G1 motor commands. -Inference is asynchronous and receding-horizon: Teleopit submits the latest -eligible observation every configured `replan_steps` at the 30 Hz action rate -while the current plan keeps executing. The isolated client keeps at most one -ZeroMQ request in flight. Each newer response is aligned with its echoed -onboard monotonic observation timestamp and replaces the active plan. -Each `get_action` request carries the camera JPEG, measured G1 joint positions, -raw measured O6 readback, measured OpenNeck angles, and the active -session-local reference root pose at that camera timestamp. The first three -state arrays form the host's 43D model observation. The source pose is not a -model input; it anchors reconstruction of the model's source-relative root -output. Teleopit's scheduler obtains it from a short history of references -actually sent to the motion tracker, never from the measured robot root. - -Pico and high-level-policy deployment use separate scripts. The policy runtime -does not start PicoBridge, GMR, or the Pico reference worker: - -```bash -python scripts/run/run_high_level_policy_sim2real.py \ - controller.policy_path=ckpt/track_g1_neck_o6.onnx \ - high_level_policy.endpoint=tcp://192.168.1.10:5555 \ - high_level_policy.task="pick up the object" \ - real_robot.network_interface=eth0 -``` - -Use the Unitree remote: `Start` enters `STANDING`, `Y` requests policy -takeover, `B` pauses/resumes, `X` returns to `STANDING`, and `L1+R1` enters -`DAMPING`. Policy entry remains an internal `STANDING` phase with no separate -starting mode: Teleopit creates one host session and waits for its first valid -chunk, then enters `POLICY` directly. Entry does not align a candidate reference, -run a Kp ramp, pause/resume the host, or create a second session. The 50 Hz output -limiter starts from the measured robot reference captured at session start. -Temporal reference jumps are accepted so recorded pause/resume transitions can -be replayed, then rate-limited on output. OpenNeck yaw/pitch values are clipped -to the configured degree ranges before scheduling, so a neck-only overshoot does -not reject the action chunk. Entry failure returns to `STANDING`. -The blocking network exchange runs in an isolated process, so the current plan -continues without stopping the local 50 Hz control loop. If inference outlasts -the plan horizon, its final reference remains valid for the configured -`hold_s` grace period. A request timeout, host/network failure, action-watchdog -expiry, or loss of a required camera/client worker enters the same ordinary -pause state as remote `B`; invalid/stale chunks are rejected. After recovery, -press `B` to resume on a fresh valid chunk. Only `X` returns active `POLICY` to -`STANDING`. - -The current client/server code and protocol tests define the network message -structure. During active development, Teleopit and `lerobot-teleopit` must be -updated together. Their only shared data file is `hand_calibration.json`, which -contains the LinkerHand O6 open/close calibration. See the -[host-policy deployment tutorial](https://BotRunner64.github.io/Teleopit/tutorials/high-level-policy-sim2real) -for the 43D model observation, source-reference request anchor, 50D action -layout, supported 1-to-50-frame action -horizon, safety envelope, host startup, and operator procedure. - -## OpenNeck Active Vision - -Pico sim2real can drive the optional OpenNeck two-axis active-vision gimbal from -the same Pico receiver used for whole-body control: - -```bash -pip install -e '.[openneck]' -python scripts/run/run_sim2real.py --config-name pico4_sim2real \ - controller.policy_path=ckpt/track_g1_neck_o6.onnx \ - neck.enabled=true \ - neck.port=/dev/ttyACM0 -``` - -`neck.enabled=true` requires `input.provider=pico4`. The neck worker reuses the -existing Teleopit Pico receiver and does not start another `PicoBridge` or -camera pipeline. The neck path reads the independent HMD -`PicoFrame.head.rotation` and maps it relative to `Body.Spine3` from the same -source frame. It never uses the full-body tracker's `Body.Head` skeleton joint, -whose model constraints can under-report extreme head pitch. The mapper uses a -fixed neutral pose and no neck-side EMA, so tracking startup does not require -the operator to face straight ahead. After the dead zone, Teleopit multiplies -the relative pitch by `neck.pitch_gain` (default `1.4`) to compensate for the -robot camera geometry; yaw remains one-to-one. It then sends physical angles -through the OpenNeck 0.2.0 `move_deg()` API. OpenNeck converts those angles for -its direct-drive servos and clips them to the calibrated mechanical step -limits. Positive yaw turns left and positive pitch looks up. - -OpenNeck 0.2.0 uses an angle-based calibration file and rejects the previous -normalized configuration fields. Re-run `openneck calibrate` before enabling -the neck worker, and set `neck.config_path` when the calibration file is not in -the runtime working directory. - ## Documentation Full docs at **[BotRunner64.github.io/Teleopit](https://BotRunner64.github.io/Teleopit/)**, covering installation profiles, all tutorials, configuration reference, and architecture. From be5434e8c2f27fa623cd4ed560495823776725b5 Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Mon, 3 Aug 2026 11:42:36 +0800 Subject: [PATCH 56/59] docs: publish imitation learning deployment guide --- .../tutorials/high-level-policy-sim2real.md | 395 ++++++++---------- docs/docs/tutorials/pico-sim2real.md | 2 +- .../tutorials/high-level-policy-sim2real.md | 342 ++++++++------- .../current/tutorials/pico-sim2real.md | 2 +- docs/sidebars.ts | 1 + 5 files changed, 348 insertions(+), 394 deletions(-) diff --git a/docs/docs/tutorials/high-level-policy-sim2real.md b/docs/docs/tutorials/high-level-policy-sim2real.md index 50f6754b..e7b03b41 100644 --- a/docs/docs/tutorials/high-level-policy-sim2real.md +++ b/docs/docs/tutorials/high-level-policy-sim2real.md @@ -1,265 +1,234 @@ --- -sidebar_position: 6 +sidebar_position: 4 --- -# Host Policy Deployment on Unitree G1 +# From Teleoperation Data to Imitation Learning / VLA Deployment -This workflow runs a LeRobot policy service on a host workstation and the -Teleopit motion tracker on the G1 onboard computer. The two repositories use -separate Python environments and communicate only through strict -ZeroMQ/msgpack messages. +This guide connects the complete workflow: record Pico demonstrations with +Teleopit, train an ACT or GR00T N1.7 policy in `lerobot-teleopit`, and run the +result on a physical Unitree G1. ```text -Host workstation (lerobot-teleopit) - ReplayPolicy or ACT -> policy server - | - | float32 observations/actions + JPEG over TCP - v -G1 onboard computer (Teleopit) - RealSense + G1/O6/OpenNeck state -> asynchronous client - -> validated 30 Hz action plan - -> timestamp-aligned receding-horizon replacement - -> 50 Hz interpolation -> motion tracker -> G1 joint-angle targets - -> LinkerHand O6 / OpenNeck +Pico demonstration + -> Teleopit v4 recording + -> LeRobot Dataset + -> ACT or GR00T checkpoint + -> host policy server + -> Teleopit onboard motion tracker + -> G1 + LinkerHand O6 + OpenNeck ``` -This is a separate runtime from Pico teleoperation. Do not start PicoBridge, -GMR, or `run_sim2real.py` for this workflow. Switching between Pico control and -host-policy control means stopping one runtime and starting the other. +Teleopit owns recording and real-time robot control. +[`lerobot-teleopit`](https://github.com/BotRunner64/lerobot-teleopit) owns +dataset conversion, model training and the host policy server. Keep their +Python environments separate; the host sends reference motion, not G1 motor +commands. + +## Before You Start + +- [VR Teleoperation on Unitree G1](pico-sim2real) works reliably. +- The onboard setup has two LinkerHand O6 hands, OpenNeck and a RealSense RGB + camera. The current training and deployment path requires all of them. +- The onboard computer is prepared through + [Installation](../getting-started/installation) with recording, OpenNeck, + LinkerHand and somehand support, plus `ckpt/track_g1_neck_o6.onnx`. +- The [Standalone Standing Test](standalone-standing) is stable with the same + G1 network interface and low-level tracking policy. +- The host workstation is prepared separately through the + [`lerobot-teleopit` installation guide](https://github.com/BotRunner64/lerobot-teleopit#installation). + +:::danger Keep the Unitree remote in your hand +Use `L1+R1` to enter `DAMPING` whenever motion is unexpected. Keep clear space +around the robot, have another person ready to support or stop it, and never +run two programs that can command the G1 at the same time. +::: + +## 1. Record and Review Demonstrations + +Run the recording configuration on the G1 onboard computer. This example uses +Pico hand-pose retargeting; use `hands.mode=gripper` when demonstrations should +use the controller triggers instead. -## 1. Network Messages and Hand Calibration +```bash +python scripts/run/run_sim2real.py \ + --config-name sim2real_record \ + controller.policy_path=ckpt/track_g1_neck_o6.onnx \ + real_robot.network_interface=eth0 \ + hands.enabled=true \ + hands.driver=linkerhand_o6 \ + hands.mode=vr_hand_pose \ + neck.enabled=true \ + recording.output_dir=data/recordings/my_task \ + recording.task="pick up the object" +``` -The current client/server code and protocol tests define the request and -response structure. During active development, changes to that structure must -be made in Teleopit and `lerobot-teleopit` together; old network envelopes are -not supported. +Use the G1 remote to enter `MOCAP` or `ARMS`, then use the recording terminal: -The only shared data file is carried in both repositories: +| Key | Action | +|-----|--------| +| `R` | Start an episode after a fresh RealSense frame is available | +| `S` | Save the active episode | +| `D` | Discard the active episode | +| `Q` | Shut down the runtime | -```text -lerobot-teleopit/src/lerobot_teleopit/hand_calibration.json -Teleopit/teleopit/high_level_policy/hand_calibration.json -``` +Record one task per dataset and keep `recording.task` consistent. Save only +successful demonstrations, while varying useful factors such as starting pose, +object position and execution speed. -`hand_calibration.json` defines the LinkerHand O6 raw open/close values and -range tolerance. The current `describe` response identifies the 43D model -observation as `teleopit-g1-joint-pos-dex-neck-state` and the canonical 50D -action as `teleopit-g1-reference`. The action layout and physical-degree -OpenNeck commands are enforced by the current code and tests. +Review the synchronized video, measured state and reference before training: -Every `get_action` request contains: - -```text -body_joint_positions float32[29] measured G1 joint positions, radians -dex_state float32[12] raw measured left/right O6 readback -neck_state float32[2] measured yaw/pitch, physical degrees -source_reference_root_pose float32[7] session-local xyz + quaternion wxyz -image JPEG RGB 640x480 camera observation +```bash +python scripts/view/view_recording.py \ + --recording data/recordings/my_task ``` -The host calibrates the raw O6 values and combines the first three arrays into -the 43D model state. `source_reference_root_pose` is the active reference root -at the camera timestamp. It is not a model input: the host uses it to -reconstruct session-local absolute root poses from source-relative model -output. +Discard episodes with tracking loss, camera interruption or unsafe references. +For the recording schema and recovery rules, see +[Teleoperation Datasets](../reference/resources/teleoperation-datasets). + +## 2. Hand the Dataset to `lerobot-teleopit` -The canonical action layout is: +Copy the complete recording directory to the host without flattening or +renaming its contents. A typical source directory is: ```text -[0:3] session-local root x/y and absolute z -[3:7] session-local root quaternion, wxyz -[7:36] G1 29D reference joint positions, radians -[36:48] left/right LinkerHand O6 closure, [0, 1] -[48:50] OpenNeck yaw/pitch, physical degrees +lerobot-teleopit/data/raw/my_task/ +├── schema.json +├── episodes.jsonl +├── data/ +└── videos/d435i_rgb/ ``` -The host sends reference motion, never G1 motor commands. Teleopit routes the -body slice through the existing motion tracker, which produces joint-angle -targets for the local G1 controller. +The current converter requires a Teleopit v4 dataset with LinkerHand O6 and +OpenNeck state/action fields. Missing fields are rejected rather than padded. +If the same task was recorded in several directories, use the host repository's +`merge_raw_datasets.py` tool before conversion. -## 2. Prepare the Host +Run all remaining host commands inside the independent `lerobot-teleopit` +environment. Its +[Dataset Conversion and Training guide](https://github.com/BotRunner64/lerobot-teleopit/blob/main/docs/training-entrypoint.md) +covers dependencies, merge options, training scales, multi-GPU settings and +logging. -Use the independent `lerobot-teleopit` environment on the workstation. For a -first network test, start ReplayPolicy before using ACT: +## 3. Convert and Train on the Host + +The shortest conversion command is: ```bash -cd /path/to/lerobot-teleopit -uv run teleopit-policy-server \ - --dataset-root data/lerobot/teleopit_v3 \ - --repo-id local/teleopit_v3 \ - --episode 0 \ - --chunk-size 15 \ - --bind tcp://0.0.0.0:5555 +python scripts/convert_dataset.py \ + --source data/raw/my_task \ + --output data/lerobot/my_task \ + --repo-id local/my_task \ + --workers 4 ``` -For ACT, use the host repository's checkpoint command instead. Allow TCP port -`5555` only on the trusted robot network. The protocol deliberately has no -remote shutdown or motor-control endpoint. +Choose one training command. For ACT: -## 3. Prepare the Onboard Runtime +```bash +python scripts/train_policy.py \ + --policy act \ + --dataset-root data/lerobot/my_task \ + --devices 0 +``` -Install Teleopit and the hardware dependencies in its own environment: +For GR00T N1.7: ```bash -pip install -e '.[openneck]' -git submodule update --init --recursive -pip install -e third_party/linkerhand-python-sdk -bash scripts/setup/setup_g1_bridge.sh +python scripts/train_policy.py \ + --policy groot \ + --dataset-root data/lerobot/my_task \ + --devices 0,1,2,3 ``` -Install `pyrealsense2` for the onboard platform separately. On Arm systems, -the conda-forge package is usually the most reliable option. +Append `--dry-run` to verify the resolved launch without starting training. +Unless `--output-dir` is set, runs are created under `outputs/train/`. The +deployable artifact is the run's `checkpoints/last/pretrained_model/` +directory. + +## 4. Validate the Robot Path with ReplayPolicy -Bring up both LinkerHand CAN interfaces before launch: +Before loading a learned checkpoint, replay a recorded episode from the host: ```bash -sudo /usr/sbin/ip link set can0 up type can bitrate 1000000 -sudo /usr/sbin/ip link set can1 up type can bitrate 1000000 +python scripts/run_policy_server.py \ + --backend replay \ + --dataset-root data/lerobot/my_task \ + --repo-id local/my_task \ + --episode 0 \ + --start-frame 0 \ + --chunk-size 15 \ + --bind tcp://0.0.0.0:5555 ``` -Calibrate OpenNeck with OpenNeck 0.2.0 and set `neck.config_path` if the -calibration file is not in the runtime working directory. - -## 4. Start Teleopit +Bind to `0.0.0.0` only on the trusted robot network. The service has no +authentication and must not be exposed to the public internet. -Run the dedicated onboard entry point and set the host IP, low-level tracking -policy, and G1 network interface: +On the G1 onboard computer, start Teleopit's dedicated runtime. Replace +`HOST_IP` with the workstation address and use the same task wording as the +dataset: ```bash python scripts/run/run_high_level_policy_sim2real.py \ controller.policy_path=ckpt/track_g1_neck_o6.onnx \ - high_level_policy.endpoint=tcp://192.168.1.10:5555 \ + high_level_policy.endpoint=tcp://HOST_IP:5555 \ high_level_policy.task="pick up the object" \ real_robot.network_interface=eth0 ``` -The protocol accepts action chunks from 1 to 50 frames. The current ACT host -returns its complete checkpoint horizon (50 frames for the production -checkpoint), while ReplayPolicy returns up to its configured `--chunk-size`, -including a shorter final tail. +Starting the process leaves the robot in `IDLE`. Use the Unitree remote: -Teleopit submits the latest eligible observation every -`high_level_policy.replan_steps` 30 Hz source frames; the default is three. The -stride must not exceed `max_action_horizon` from the host's `describe` -response. The isolated client permits only one REQ/REP exchange at a time, but -the active plan continues while that request is in flight. Each learned-policy -response is independently reconstructed from its request's source reference; -the host does not aggregate overlapping chunks. Teleopit uses the echoed -onboard monotonic timestamp to replace the active plan at the correct -source-frame position. +| Control | Action | +|---------|--------| +| `Start` | Enter `STANDING` | +| `Y` | Start a policy session; the first valid chunk enters `POLICY` | +| `B` | Pause or resume after a fresh chunk is available | +| `X` | End the session and return to `STANDING` | +| `L1+R1` | Immediately enter `DAMPING` | -The production camera contract is exactly RGB `uint8[480,640,3]` at 30 Hz. -`camera.source=test-pattern` exists only for controlled integration testing; -use `camera.source=realsense` for deployment. +ReplayPolicy should reproduce the recorded reference closely enough to verify +the network, action convention and onboard execution path. Stop here if it +does not. A learned policy cannot fix a recording, conversion, coordinate or +low-level tracking problem. -## 5. Operator Flow +## 5. Deploy the Trained Policy -Keep the Unitree remote in hand. The runtime has only the formal robot modes -`IDLE`, `STANDING`, `POLICY`, and `DAMPING`. +Press `X` to return the G1 to `STANDING`, then stop ReplayPolicy. On the host, +start the learned-policy server with the `pretrained_model` directory itself: -| Control | Action | -|---------|--------| -| Unitree remote `Start` | Enter `STANDING` | -| Unitree remote `Y` | Request host-policy takeover | -| Unitree remote `B` | Pause or resume `POLICY` | -| Unitree remote `X` | Return to `STANDING` or cancel a pending request | -| Unitree remote `L1+R1` | Emergency transition to `DAMPING` | - -After `Y`, Teleopit creates one entry session, establishes the current root -XY/yaw anchor, and requests its first chunk. The robot remains formally in -`STANDING` while waiting; there is no separate "policy starting" state. The -chunk's structure, finite values, quaternion, and absolute hardware ranges are -validated, while temporal root, yaw, and joint-reference jumps are accepted. -A valid first chunk enters `POLICY` directly. Entry does not align a candidate -reference, run a Kp ramp, pause/resume host requests, or create/reset a second -session. The scheduler's 50 Hz output limiter starts from the measured robot -reference captured when the session begins. A failure or timeout leaves the -robot on the normal standing reference. - -Inside `POLICY`, a newer response normally replaces the active plan before its -horizon ends. If inference takes longer, Teleopit keeps the plan's final body, -hand, and neck targets for the configured `hold_s` grace period while the local -50 Hz control loop continues running. Exhausting that grace period triggers -the action watchdog and the normal resumable pause. - -Pause freezes the body reference and holds the last LinkerHand and OpenNeck -commands. Resume requests a fresh action chunk while continuing to hold the -paused pose. `X` stops the policy session and opens/centers the auxiliary -hardware as the runtime returns to `STANDING`. - -A watchdog, host/network, camera, or policy-client fault enters this same -ordinary pause state and keeps the current body, hand, and neck commands. Once -the input path has recovered, press `B`; Teleopit holds the paused pose until a -fresh valid action chunk arrives, then resumes `POLICY`. The runtime never -enters `STANDING` automatically; `X` remains the manual transition. - -## 6. Onboard Validation and Watchdog - -Teleopit clips a G1 joint reference to the configured real-robot position -limits when the correction is at most `max_joint_projection_rad`, and clips -OpenNeck yaw/pitch commands to their configured degree ranges. It then rejects -a complete chunk if any frame violates the remaining contract. It never pads -or trims a malformed host result. Checks include: - -- exact finite `float32[T,50]`, current session, and increasing source sequence; -- normalized root quaternion with temporal sign continuity; -- absolute root-height limits; -- G1 joint-position clipping to `real_robot.joint_pos_lower/upper`, with larger - corrections rejected; -- LinkerHand closure `[0,1]`; -- OpenNeck yaw/pitch clipping to the configured degree ranges; -- observation/result age, source timestamp, and action horizon. - -Reference continuity is not an acceptance condition. Root translation, root -yaw, and G1 joint-reference jumps are accepted at entry, inside a chunk, and -across chunks because a recorded pause/resume transition can intentionally be -discontinuous. The 50 Hz output limiter bridges accepted discontinuities. The -first valid chunk from the single entry session starts live execution -immediately. A malformed or stale first chunk, an out-of-range non-joint field -other than the projected OpenNeck angles, or an excessive joint correction -aborts entry. - -Validated 30 Hz body references are interpolated and rate-limited locally at -50 Hz. The echoed source timestamp selects the current position in each -response, so host latency can skip elapsed source frames and a newer chunk can -replace an executing plan. The configured root displacement/XY speed, -yaw-rate, and joint-rate values are output limits, not chunk-rejection -thresholds. The final validated reference remains available for `hold_s` after -the plan horizon. If a network exchange times out, the watchdog expires, or a -required camera/client worker exits, Teleopit remains in `POLICY`, enters the -normal resumable pause state, and holds the latest body, hand, and neck -commands. Invalid or stale responses are rejected without replacing the active -valid plan. After recovery, `B` requests resume; execution stays paused until a -fresh validated chunk arrives. Only `X` changes the mode to `STANDING`. - -The scheduler records its rate-limited session-local reference at 50 Hz. -For each camera frame it looks up or interpolates that history at the camera's -monotonic timestamp and sends the resulting root pose as -`source_reference_root_pose`. Session start seeds the history from the active -standing reference, and pause/resume records held-reference boundaries, so the -anchor never comes from measured robot root pose. - -The default safety envelope lives under `high_level_policy.safety` in -`high_level_policy_sim2real.yaml`. Adjust it only after checking the recorded -data, G1 joint limits, and the installed OpenNeck calibration. - -## 7. Troubleshooting - -**`Y` never enters `POLICY`:** check the host endpoint, firewall, server log, -`describe` schemas, message envelope, task, checkpoint manifest, -`replan_steps`, and the entry logs. Teleopit stays in `STANDING` until the -single entry session returns its first valid chunk. - -**The first entry chunk is rejected or entry times out:** inspect the logged -contract error, joint ordering, absolute-reference convention, hardware ranges, -and host/network latency. Reference discontinuity alone does not reject a chunk. - -**Policy runs briefly and becomes paused:** inspect timeout, inference -latency, stale-result, worker-exit, and safety-rejection logs. The low-level -50 Hz tracker does not block on host inference; the current plan keeps running -and then uses the configured final-reference grace period. Restore the failed -input path, then press `B` to resume. - -**Pico does not connect:** this runtime intentionally does not start Pico. Stop -it and launch the Pico-specific `run_sim2real.py --config-name -pico4_sim2real` workflow instead. +```bash +python scripts/run_policy_server.py \ + --backend lerobot \ + --checkpoint outputs/train//checkpoints/last/pretrained_model \ + --device cuda \ + --bind tcp://0.0.0.0:5555 +``` + +ACT and GR00T use the same server command. Press `Y` on the Unitree remote to +create a new policy session. Begin with a familiar scene from the training +distribution and small, recoverable motions. + +To record the observations and actions exchanged during a run, add this host +option: + +```bash +--record-dir outputs/policy-recordings +``` + +Teleopit validates and rate-limits each returned plan before the 50 Hz motion +tracker consumes it. Malformed output is rejected rather than padded or +trimmed. A host, network, camera or action-watchdog fault pauses the session +and holds the latest commands; it does not automatically enter `STANDING`. +Restore the failed path and press `B` to resume, or use `X` or `L1+R1` as +appropriate. + +## Common Problems + +| Symptom | What to check | +|---------|---------------| +| Pressing `Y` never enters `POLICY` | Host IP and firewall, server logs, a fresh RealSense frame, matching code versions and identical `hand_calibration.json` files | +| `POLICY` becomes paused | Host inference latency, request timeout, stale camera/result, action watchdog or a required worker exit | + +For model action coordinates and host-side behavior, see the +[`lerobot-teleopit` Action Space guide](https://github.com/BotRunner64/lerobot-teleopit/blob/main/docs/planar-relative-root-actions.md). +For onboard timing and safety settings, see +[Configuration Fields](../reference/configuration/fields#host-high-level-policy-independent-sim2real). diff --git a/docs/docs/tutorials/pico-sim2real.md b/docs/docs/tutorials/pico-sim2real.md index a23983b6..e8a9feb0 100644 --- a/docs/docs/tutorials/pico-sim2real.md +++ b/docs/docs/tutorials/pico-sim2real.md @@ -241,4 +241,4 @@ for the stored fields. - [Standalone Standing Test](standalone-standing) - [BVH Playback on Unitree G1](bvh-sim2real) -- [Host Policy Deployment on Unitree G1](high-level-policy-sim2real) +- [From Teleoperation Data to Imitation Learning / VLA Deployment](high-level-policy-sim2real) diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md index abaf5abe..0e700042 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/high-level-policy-sim2real.md @@ -1,233 +1,217 @@ --- -sidebar_position: 6 +sidebar_position: 4 --- -# 在 Unitree G1 上部署主机策略 +# 从遥操数据到模仿学习 / VLA 真机部署 -该工作流在主机工作站上运行 LeRobot 策略服务，在 G1 onboard 计算机上运行 -Teleopit motion tracker。两个仓库使用相互独立的 Python 环境，只通过严格的 -ZeroMQ/msgpack 消息通信。 +本教程串联完整工作流：使用 Teleopit 录制 Pico 示教，在 `lerobot-teleopit` 中训练 +ACT 或 GR00T N1.7 策略，再把训练结果运行到 Unitree G1 真机上。 ```text -主机工作站（lerobot-teleopit） - ReplayPolicy 或 ACT -> policy server - | - | 通过 TCP 传输 float32 observations/actions + JPEG - v -G1 onboard 计算机（Teleopit） - RealSense + G1/O6/OpenNeck state -> 异步 client - -> 已验证的 30 Hz action plan - -> 按时间戳对齐的 receding-horizon 替换 - -> 50 Hz 插值 -> motion tracker -> G1 关节角目标 - -> LinkerHand O6 / OpenNeck +Pico 示教 + -> Teleopit v4 录制数据 + -> LeRobot Dataset + -> ACT 或 GR00T checkpoint + -> 主机策略服务 + -> Teleopit onboard motion tracker + -> G1 + LinkerHand O6 + OpenNeck ``` -这是与 Pico 遥操作相互独立的运行时。该工作流不应启动 PicoBridge、GMR 或 -`run_sim2real.py`。在 Pico 控制与主机策略控制之间切换时，需要先停止一个运行时， -再启动另一个。 +Teleopit 负责录制和实时机器人控制； +[`lerobot-teleopit`](https://github.com/BotRunner64/lerobot-teleopit) +负责数据转换、模型训练和主机策略服务。两个仓库使用相互独立的 Python 环境；主机发送 +reference motion，而不是 G1 电机命令。 -## 1. 网络消息与手部标定 +## 开始之前 -当前 client/server 代码和协议测试定义 request 与 response 结构。活跃开发期间， -任何结构变更都必须同时修改 Teleopit 和 `lerobot-teleopit`；不支持旧网络 envelope。 +- [Unitree G1 VR 遥操作](pico-sim2real)已经可靠运行。 +- Onboard 配置包含两只 LinkerHand O6、OpenNeck 和一台 RealSense RGB 相机。 + 当前训练与部署路径要求这些硬件全部存在。 +- Onboard 计算机已经按照[安装指南](../getting-started/installation)安装 recording、 + OpenNeck、LinkerHand 和 somehand 支持，并且存在 + `ckpt/track_g1_neck_o6.onnx`。 +- 使用相同 G1 网络接口和底层 tracking policy 时， + [独立站立测试](standalone-standing)已经稳定运行。 +- 主机工作站已经单独按照 + [`lerobot-teleopit` 安装指南](https://github.com/BotRunner64/lerobot-teleopit#installation) + 准备完成。 -两个仓库唯一共享的数据文件为： +:::danger 始终把 Unitree 遥控器拿在手中 +动作异常时立即按 `L1+R1` 进入 `DAMPING`。确保机器人周围有足够的安全空间， +安排另一人随时准备扶住或停止机器人，并且不要同时运行两个可能向 G1 发送命令的程序。 +::: -```text -lerobot-teleopit/src/lerobot_teleopit/hand_calibration.json -Teleopit/teleopit/high_level_policy/hand_calibration.json +## 1. 录制并检查示教 + +在 G1 onboard 计算机上运行录制配置。下面的示例使用 Pico 手部姿态重定向； +如果示教需要使用手柄扳机，请改用 `hands.mode=gripper`。 + +```bash +python scripts/run/run_sim2real.py \ + --config-name sim2real_record \ + controller.policy_path=ckpt/track_g1_neck_o6.onnx \ + real_robot.network_interface=eth0 \ + hands.enabled=true \ + hands.driver=linkerhand_o6 \ + hands.mode=vr_hand_pose \ + neck.enabled=true \ + recording.output_dir=data/recordings/my_task \ + recording.task="pick up the object" ``` -`hand_calibration.json` 定义 LinkerHand O6 的 raw open/close 值和 range tolerance。 -当前 `describe` 响应将 43 维模型 observation 标识为 -`teleopit-g1-joint-pos-dex-neck-state`，将 canonical 50 维 action 标识为 -`teleopit-g1-reference`。action 布局和使用物理角度的 OpenNeck 命令由当前代码与测试 -约束。 +使用 G1 遥控器进入 `MOCAP` 或 `ARMS`，然后操作录制终端： -每个 `get_action` 请求都包含： +| 按键 | 动作 | +|------|------| +| `R` | RealSense 有新鲜画面后，开始一条 episode | +| `S` | 保存当前 episode | +| `D` | 丢弃当前 episode | +| `Q` | 关闭运行时 | -```text -body_joint_positions float32[29] G1 实测关节位置，弧度 -dex_state float32[12] 左/右 O6 原始实测 readback -neck_state float32[2] 实测 yaw/pitch，物理角度 -source_reference_root_pose float32[7] session-local xyz + quaternion wxyz -image JPEG RGB 640x480 相机观测 +每个 dataset 只录制一项任务，并保持 `recording.task` 一致。只保存成功示教， +同时覆盖有意义的初始姿态、物体位置和执行速度变化。 + +训练前检查同步视频、实测状态和 reference： + +```bash +python scripts/view/view_recording.py \ + --recording data/recordings/my_task ``` -主机会标定 O6 原始值，并把前三个数组组合成 43 维模型 state。 -`source_reference_root_pose` 是相机时间戳对应的 active reference root。它不是模型 -输入；主机用它从 source-relative 模型输出重建 session-local absolute root pose。 +出现追踪丢失、相机中断或不安全 reference 时，应丢弃对应 episode。录制 schema +和恢复规则见[遥操数据集](../reference/resources/teleoperation-datasets)。 + +## 2. 将 Dataset 交给 `lerobot-teleopit` -canonical action 布局为： +把完整录制目录复制到主机，不要展开目录层级或修改其中的名称。典型 source 目录为： ```text -[0:3] session-local root x/y 与绝对 z -[3:7] session-local root quaternion，wxyz -[7:36] G1 29D reference joint positions，弧度 -[36:48] 左/右 LinkerHand O6 closure，[0, 1] -[48:50] OpenNeck yaw/pitch，物理角度 +lerobot-teleopit/data/raw/my_task/ +├── schema.json +├── episodes.jsonl +├── data/ +└── videos/d435i_rgb/ ``` -主机发送的是 reference motion，而不是 G1 电机命令。Teleopit 会把 body slice 送入 -现有 motion tracker，由它为本地 G1 控制器生成关节角目标。 +当前转换器要求 Teleopit v4 dataset 包含 LinkerHand O6 和 OpenNeck 的 state/action +字段；缺失字段会被拒绝，不会自动补齐。如果同一任务录制在多个目录中，请在转换前使用 +主机仓库的 `merge_raw_datasets.py` 工具合并。 + +后续所有主机命令都应在独立的 `lerobot-teleopit` 环境中运行。其 +[数据转换与训练指南](https://github.com/BotRunner64/lerobot-teleopit/blob/main/docs/training-entrypoint.zh-CN.md) +包含依赖、合并选项、训练规模、多 GPU 设置和实验日志说明。 -## 2. 准备主机 +## 3. 在主机上转换并训练 -在工作站上使用独立的 `lerobot-teleopit` 环境。首次网络测试应先运行 -ReplayPolicy，再使用 ACT： +最短的数据转换命令为： ```bash -cd /path/to/lerobot-teleopit -uv run teleopit-policy-server \ - --dataset-root data/lerobot/teleopit_v3 \ - --repo-id local/teleopit_v3 \ - --episode 0 \ - --chunk-size 15 \ - --bind tcp://0.0.0.0:5555 +python scripts/convert_dataset.py \ + --source data/raw/my_task \ + --output data/lerobot/my_task \ + --repo-id local/my_task \ + --workers 4 ``` -使用 ACT 时，改用主机仓库中的 checkpoint 命令。只在可信的机器人网络上放行 TCP -端口 `5555`。该协议有意不提供远程关机或电机控制 endpoint。 +选择一种训练命令。训练 ACT： -## 3. 准备 Onboard 运行时 +```bash +python scripts/train_policy.py \ + --policy act \ + --dataset-root data/lerobot/my_task \ + --devices 0 +``` -在 Teleopit 自己的环境中安装 Teleopit 与硬件依赖： +训练 GR00T N1.7： ```bash -pip install -e '.[openneck]' -git submodule update --init --recursive -pip install -e third_party/linkerhand-python-sdk -bash scripts/setup/setup_g1_bridge.sh +python scripts/train_policy.py \ + --policy groot \ + --dataset-root data/lerobot/my_task \ + --devices 0,1,2,3 ``` -需要根据 onboard 平台单独安装 `pyrealsense2`。在 Arm 系统上，conda-forge 包通常 -最可靠。 +追加 `--dry-run` 可以检查最终启动配置，而不实际开始训练。如果没有设置 +`--output-dir`，训练结果会写入 `outputs/train/`。可部署产物是对应 run 下的 +`checkpoints/last/pretrained_model/` 目录。 + +## 4. 使用 ReplayPolicy 验证真机链路 -启动前开启两个 LinkerHand CAN 接口： +加载 learned checkpoint 前，先从主机回放一条已经录制的 episode： ```bash -sudo /usr/sbin/ip link set can0 up type can bitrate 1000000 -sudo /usr/sbin/ip link set can1 up type can bitrate 1000000 +python scripts/run_policy_server.py \ + --backend replay \ + --dataset-root data/lerobot/my_task \ + --repo-id local/my_task \ + --episode 0 \ + --start-frame 0 \ + --chunk-size 15 \ + --bind tcp://0.0.0.0:5555 ``` -使用 OpenNeck 0.2.0 完成校准；如果校准文件不在运行目录中，请设置 -`neck.config_path`。 +只在可信的机器人网络上绑定 `0.0.0.0`。该服务没有身份验证，不得暴露到公网。 -## 4. 启动 Teleopit - -运行专用 onboard 入口，并设置主机 IP、底层 tracking policy 和 G1 网卡： +在 G1 onboard 计算机上启动 Teleopit 专用运行时。将 `HOST_IP` 替换为工作站地址， +并使用与 dataset 一致的任务描述： ```bash python scripts/run/run_high_level_policy_sim2real.py \ controller.policy_path=ckpt/track_g1_neck_o6.onnx \ - high_level_policy.endpoint=tcp://192.168.1.10:5555 \ + high_level_policy.endpoint=tcp://HOST_IP:5555 \ high_level_policy.task="pick up the object" \ real_robot.network_interface=eth0 ``` -协议接受 1 到 50 帧的 action chunk。当前 ACT 主机会返回完整的 checkpoint horizon -（生产 checkpoint 为 50 帧）；ReplayPolicy 最多返回 `--chunk-size` 配置的帧数， -最后一段可以更短。 +进程启动后，机器人保持 `IDLE`。使用 Unitree 遥控器操作： -Teleopit 每隔 `high_level_policy.replan_steps` 个 30 Hz source frame 提交最新的合格 -observation，默认间隔为三帧。该 stride 不得超过主机 `describe` 响应中的 -`max_action_horizon`。隔离的 client 同一时间只允许一个 REQ/REP exchange，但该请求 -在途时 active plan 会继续执行。每份 learned-policy response 都基于其请求中的 source -reference 独立重建；主机不会聚合相互重叠的 chunk。Teleopit 使用回显的 onboard 单调 -时间戳，在正确的 source-frame 位置替换 active plan。 +| 操作 | 动作 | +|------|------| +| `Start` | 进入 `STANDING` | +| `Y` | 创建策略 session；第一份有效 chunk 会进入 `POLICY` | +| `B` | 暂停，或在新鲜 chunk 可用后恢复 | +| `X` | 结束 session 并返回 `STANDING` | +| `L1+R1` | 立即进入 `DAMPING` | -生产相机契约固定为 30 Hz 的 RGB `uint8[480,640,3]`。 -`camera.source=test-pattern` 只用于受控集成测试；部署时应使用 -`camera.source=realsense`。 +ReplayPolicy 应当足够准确地重现录制 reference，以验证网络、action 约定和 onboard +执行链路。如果回放不正确，请在这里停止。Learned policy 无法修复录制、转换、坐标或 +底层 tracking 问题。 -## 5. 操作流程 +## 5. 部署训练好的策略 -始终把 Unitree 遥控器拿在手中。该运行时只有 `IDLE`、`STANDING`、`POLICY` 和 -`DAMPING` 四个正式机器人模式。 +按 `X` 让 G1 返回 `STANDING`，然后停止 ReplayPolicy。在主机上使用 +`pretrained_model` 目录本身启动 learned-policy server： -| 控制 | 动作 | -|------|------| -| Unitree remote `Start` | 进入 `STANDING` | -| Unitree remote `Y` | 请求主机策略接管 | -| Unitree remote `B` | 暂停或恢复 `POLICY` | -| Unitree remote `X` | 返回 `STANDING`，或取消等待中的请求 | -| Unitree remote `L1+R1` | 紧急切换到 `DAMPING` | - -按下 `Y` 后，Teleopit 会创建一个 entry session，以当前 root XY/yaw 建立锚点，并请求 -该 session 的第一份 chunk。等待期间机器人在形式上仍处于 `STANDING`；没有单独的 -“policy starting”状态。运行时会验证 chunk 的结构、有限值、四元数和绝对硬件范围， -同时接受 root、yaw 和关节 reference 的时间跳变。第一份有效 chunk 会直接进入 -`POLICY`。entry 不会对齐候选 reference、运行 Kp ramp、暂停/恢复 host 请求，也不会 -创建或 reset 第二个 session。scheduler 的 50 Hz 输出 limiter 从 session 开始时捕获的 -机器人实测 reference 起步。失败或超时会让机器人保持普通 standing reference。 - -在 `POLICY` 内，较新的 response 通常会在 active plan 的 horizon 结束前替换它。如果 -推理耗时更长，Teleopit 会在配置的 `hold_s` grace period 内保持该计划最后一条 body、 -hand 和 neck target，同时本地 50 Hz 控制循环继续运行。超过该 grace period 会触发 -action watchdog，并进入普通的可恢复暂停。 - -暂停会冻结 body reference，并保持最后一条 LinkerHand 和 OpenNeck 命令。恢复时会请求 -新的 action chunk，并在等待期间继续保持暂停姿态。按 `X` 会停止策略 session；运行时 -返回 `STANDING` 时会张开手并让辅助硬件回中。 - -Watchdog、主机/网络、相机或 policy client 故障也会进入同一个普通暂停状态，并保持 -当前 body、hand 和 neck 命令。输入路径恢复后按 `B`；Teleopit 会继续保持暂停姿态， -直到收到新的有效 action chunk，再恢复 `POLICY`。运行时不会自动进入 `STANDING`； -`X` 仍是手动切换到 `STANDING` 的操作。 - -## 6. Onboard 验证与 Watchdog - -当修正量不超过 `max_joint_projection_rad` 时，Teleopit 会先把 G1 关节 reference 裁剪到 -配置的真机关节位置范围，并把 OpenNeck yaw/pitch 命令裁剪到配置的角度范围；如果任一帧 -违反其余契约，则拒绝整个 chunk。它不会对错误的主机结果进行补齐或删减。检查包括： - -- 精确且有限的 `float32[T,50]`、当前 session，以及递增的 source sequence； -- 归一化 root quaternion 与时间连续的符号； -- 绝对 root 高度限制； -- 按 `real_robot.joint_pos_lower/upper` 裁剪 G1 关节位置，并拒绝更大的修正量； -- LinkerHand closure `[0,1]`； -- 将 OpenNeck yaw/pitch 裁剪到配置的角度范围； -- observation/result 时效、source timestamp 和 action horizon。 - -reference 连续性不是接收条件。entry、chunk 内部和 chunk 之间的 root translation、root -yaw 与 G1 关节 reference 跳变都会被接受，因为录制的 pause/resume 转换可能有意地不 -连续；50 Hz 输出 limiter 会衔接这些已接受的跳变。单个 entry session 的第一份有效 -chunk 会立即开始实时执行。格式错误、过期、非关节字段超出绝对范围（已裁剪的 OpenNeck -角度除外）或关节修正量过大会终止 entry。 - -通过验证的 30 Hz body reference 会在本地插值到 50 Hz 并执行 rate limit。回显的 -source timestamp 用于选择每份 response 中的当前位置，因此主机延迟可以跳过已经过去的 -source frame，较新的 chunk 也可以替换执行中的计划。配置的 root displacement/XY -speed、yaw rate 和 joint rate 是输出限制，而不是 chunk 拒绝阈值。plan horizon 结束后， -最后一条有效 reference 会继续保留 `hold_s`。如果网络交换超时、watchdog 到期，或必要的 -camera/client worker 退出，Teleopit 会保持在 `POLICY`，进入普通的可恢复暂停状态，并保持 -最后一条 body、hand 和 neck 命令。无效或过期 response 会被拒绝，不会替换当前仍然有效的 -plan。故障恢复后按 `B` 请求恢复；在收到新的有效 chunk 前，执行仍保持暂停。只有 `X` -会把模式切换到 `STANDING`。 - -scheduler 会以 50 Hz 记录经过限速的 session-local reference。对于每个相机帧，它会 -在相机单调时间戳处查询该历史或进行插值，并把得到的 root pose 作为 -`source_reference_root_pose` 发送。session 启动时用当时的 active standing reference -初始化历史；pause/resume 会记录 held-reference 边界，因此锚点绝不会来自机器人实测 -root pose。 - -默认安全范围位于 `high_level_policy_sim2real.yaml` 的 -`high_level_policy.safety` 下。只有在检查录制数据、G1 关节限位和已安装的 OpenNeck -校准后，才应调整这些值。 - -## 7. 故障排查 - -**按 `Y` 后始终不进入 `POLICY`：** 检查主机 endpoint、防火墙、server 日志、 -`describe` schema、消息 envelope、task、checkpoint manifest、`replan_steps` 和 entry -日志。Teleopit 会保持 `STANDING`，直到单个 entry session 返回第一份有效 chunk。 - -**第一份 entry chunk 被拒绝或 entry 超时：** 请检查日志中的契约错误、关节顺序、绝对 -reference 约定、硬件范围以及 host/network 延迟。单纯的 reference 跳变不会导致 chunk -被拒绝。 - -**策略短暂运行后进入暂停：** 检查 timeout、推理延迟、stale result、worker 退出和 -安全拒绝日志。底层 50 Hz tracker 不会等待主机推理；当前 plan 会继续执行，随后使用配置的 -最终 reference grace period。恢复故障输入路径后按 `B` 继续。 - -**Pico 无法连接：** 该运行时有意不启动 Pico。请先停止它，再改用 Pico 专用的 -`run_sim2real.py --config-name pico4_sim2real` 工作流。 +```bash +python scripts/run_policy_server.py \ + --backend lerobot \ + --checkpoint outputs/train//checkpoints/last/pretrained_model \ + --device cuda \ + --bind tcp://0.0.0.0:5555 +``` + +ACT 和 GR00T 使用相同的 server 命令。按 Unitree 遥控器 `Y` 创建新的策略 session。 +开始时使用训练分布内熟悉的场景，并只允许幅度小、容易恢复的动作。 + +如果需要记录一次运行中交换的 observation 和 action，增加以下主机参数： + +```bash +--record-dir outputs/policy-recordings +``` + +Teleopit 会在 50 Hz motion tracker 使用之前校验并限速每一份 plan。格式错误的输出 +会被拒绝，不会被补齐或删减。主机、网络、相机或 action watchdog 故障会暂停 session +并保持最后一条命令，不会自动进入 `STANDING`。恢复故障链路后按 `B` 继续，或根据情况 +使用 `X` 或 `L1+R1`。 + +## 常见问题 + +| 现象 | 检查项 | +|------|--------| +| 按 `Y` 后始终不进入 `POLICY` | 主机 IP 和防火墙、server 日志、新鲜的 RealSense 画面、两边匹配的代码版本，以及完全一致的 `hand_calibration.json` | +| `POLICY` 进入暂停 | 主机推理延迟、请求超时、相机/结果过期、action watchdog，或必要 worker 退出 | + +模型 action 坐标和主机端行为见 +[`lerobot-teleopit` Action Space 指南](https://github.com/BotRunner64/lerobot-teleopit/blob/main/docs/planar-relative-root-actions.zh-CN.md)。 +Onboard 时序和安全设置见 +[配置字段](../reference/configuration/fields#主机-high-level-policy独立-sim2real)。 diff --git a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md index 5d1c4f34..e5bd2d81 100644 --- a/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md +++ b/docs/i18n/zh-Hans/docusaurus-plugin-content-docs/current/tutorials/pico-sim2real.md @@ -223,4 +223,4 @@ python scripts/view/view_recording.py \ - [单独测试站立运控](standalone-standing) - [在 Unitree G1 上回放 BVH](bvh-sim2real) -- [在 Unitree G1 上部署主机策略](high-level-policy-sim2real) +- [从遥操数据到模仿学习 / VLA 真机部署](high-level-policy-sim2real) diff --git a/docs/sidebars.ts b/docs/sidebars.ts index d6fff6f4..756bfad2 100644 --- a/docs/sidebars.ts +++ b/docs/sidebars.ts @@ -17,6 +17,7 @@ const sidebars: SidebarsConfig = { 'tutorials/offline-sim2sim', 'tutorials/pico-sim2sim', 'tutorials/pico-sim2real', + 'tutorials/high-level-policy-sim2real', 'tutorials/training', ], }, From cb7a256c4022d93a4438c93a53174fd4727f8127 Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Mon, 3 Aug 2026 11:55:52 +0800 Subject: [PATCH 57/59] docs: replace project logo with teleoperation photo --- README.md | 2 +- assets/teleopit.png | Bin 0 -> 405925 bytes 2 files changed, 1 insertion(+), 1 deletion(-) create mode 100644 assets/teleopit.png diff --git a/README.md b/README.md index cd41f288..007eca1d 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,5 @@

- Teleopit + Teleopit whole-body teleoperation demo

Teleopit

diff --git a/assets/teleopit.png b/assets/teleopit.png new file mode 100644 index 0000000000000000000000000000000000000000..271b166d14bf359c912bd9a34852582497bb84d7 GIT binary patch literal 405925 zcmX`SbySo8`#(NHN-06QU(!fPh@?0`I;6X6bazON&XLk3F}h=jgu)0B7~S0+Bfoop z&iVbGXK}VacJJKxbzjd%UD0YPa`<>ucmMzZU;YbF0|3B?0RYf(anMoUICN<`qTX;_ zzUa9F00jO2d!j)&38(=827o;9lcvx2!*0Ds<{?*>Z93vC-}KJuZY?8DKN;Z;vAcz0x&cfCWF%QEiQ`g`&fGzZ`2B09&d(53)5Z$;`lt7 z)7wfAe$Dz&ZBD@K!UA>TZ!x=tNh!jmYOmqgx(6Xr9V_-jR#oYT#jBW}9L<09|LHYY zlE)6r&CPjyl<5x(!^q>lVe;wQ^jkN16x{nNCfTu~5n-TQA(?-4-q`=l1wU^b zm$={UOVliD;96)te|t6BS2KRUUEaDhwb_Pm9>=ICjRzcQP-2$>_S_EeBqStA+|Sw2 z2VF8UN%)-pq`LQ(yz8WYI*mfiS5`hSuide$-LA0BsVX8~t55wGYTIqp7q~r6%qwne zCt-s_X3XpSPLWubR0ef8IJg=yxbi|8hcQBmR2n^`dGIx*<1H(D1W^j8#)CNm*dKgX z9(;G&M_5WaI9tTlRKu0Ln%)rynt4Y=G(HH@b^|FKA7nREDFQX**-PrJ`An*{t8#L3 z5@8n;hue`hap2(w8r8wkEZI-FgV`RP>9@0Ze!mV_x1*-2yI0%xHWUNLPr`{@oAs4h zNdTEVWF|q#Ef0R*>q9*)J3x{`%?ybBHvG+Bnuxthzia28QA_eiSjQ^C05y%X&yE7m zo9VW%=xF96kaI5x#{CYqCJ9htG2+C*g2-SXkIS>kTqmNEU}LR|&14S^>bI zIB%QB2_Vuy4-v!yydoq@u8Ocu)U}`;Ljy3r^$i#r%@LBtQ&;_#hTmF0I6rOQY+P#s z3-I;yi~wh3WMpY<3~d%CN>mQJIR^(jJJ(ingt9^K$T)%(gmr;Uh>qa}WU_`p7!#9` z)-i}X9gg||L+YcQ8a&2$1Ox#{Vz+`C z)y_1|RaXyp?-{E__l&cnV*|aq5l#A@b^J_(NyNOs&Bl2@8=>(>JJ&`^USfLW9M;@T zt||ddqU;9OSA;&axkdo+LkT(q0;Y{})Fbk8b15xgS5W7HCD;o-(u6FA!Ke!c{J!K6 zd3?2WD=jUpvVXGgTir~V#xJv{bYuJ;6Q?R4sz#jma^ zPjp{PYnGt{B4WT2Pq!Pv55I#Sp2}AUmp#qLYmS~iC0o4gQt(x_hsN`13~#w1PuD&> zdZ?3H+BNITv*(NXy7~Ea^z`shyjIT7%||s?kb^quWOp~$*Xl2;ewtUNX6xPQNrO=%R= zH0^OPL|511tm({~AcGh9=FpVo@yp^PWSLGxhrW}dv**$fL##^Erse@5Y0{wF9i#{U zw5$QBG)*sE1~28h6X#?#8p1YIOH?c{QxD5WuK)%cH%Yp{yl;^)W&;D!JfX@EYz~ zn27k}?Z%0rkN{paS1ghN zIb9nhNEJVG;Hi}^catDnJn{7ORHai(b}^7zuKMjDHXc24g~qJgPt==TXt7*uI*9Pz z%XOBl$Y009w~R=deQ0s`q~|#*?VIRe=Ja0EgL6rIy0o#8k+HE+;7#X>5qG+8UWV1) zY<8lz|4vENo6Z4}L&RnfrSY|Q#L?Y$d-d?0sq9n-RU)e-a!6I;@u~ckyixrv|8T0n zFG$j{l(%pHytj$#mcp{Wzn>_gzZp7nP{hVx8rO`NuQlmvYtIr7ytCxb9qe98xr3d% zNQR}>`y)IyLn&A>O)RClsgj6Nyz^^&nwGx$@Z~ouoTF;^@A=12^avuAL~wk+0byZi z`7$7a2ii{7D;nFbVjPONg*Pna)#S%{Et2F1T^7@$4(Yw(f3lZaJ`{Ry^XS(-cJZCh zR!?CU(KEJ#3>o&IW4LaGi?P&c? z2ybve!jjOF-@FF@(zj^hLnshd+!ljHWNa=jFW*}mNF0JpoFyP8?yvAH_H%X`xN2)_ zAsVSeC2V-wt)w!&wpt7g+*rgj2X5WQitRa1wTa@M4E|qbzoggxV)?f5E^^OxO@h~K z=$u!l>P>O#m0zH%Yhl_y{gb1kdMXGwO_Sqm)pU%YYV}W{!8XO1%eEH8&Em1?ZkSx4 zL$_*0B3K_Z;Ucd6%%fA~3`5@euiP(hZ80+?$o`kE+Jx}_R#zB2rSdO=RBG>j_>5%2 z>XHwcu{W4KVe1hX_;?fieA77hFqHZ`_);!dlXPHjZ*OR5C^7h9qvtlZ=l1TPmNgX@ z$cgvj<3<)twqkB(=*vWxb5p>cQSQf9YP0qv19 z{>C!c>6t*}74cvdpH!1HPEAF<)UH&6*8yx#G3zMWUx+)Vfos}Fj4lygX*fb|vtxsL z-22~GXSELEu~QVE?wet}Gd44Xj05kezL_41SM`jWn`M3l7nwLEZK{gke=$6#KAh82b( z2zvCiebB6*GzE6x$c5Or+cMUhsn#qFJfNn+Kt@W<)g`9P*H6SQ zt``6daCy`cb*eRUpi4SLpPEKxYSO9zupsm_f5Dk0&ywj~i^Lh%~U-=80 zPL9aWGwXl#Kxi9@%5rUd2|FGhoD z!EL%+;Y6u>K%IFz<0WupR9))qq0g90uIpB9w5`S9vG6A#e9KLY?)kDjI5;4{&Bdk8 zkUM1{(qON15T02 zKiS72-Wu*bN@0C|HLCwg(iC3%DmAyj7661`TQRE9ZPZC8pI(&nM?1UNb{f@Z*E@cr zVa+yM32E)y8rTZpvpIJ4@o8&q&9FhDnpEw~#u<r@*qpgh2Z?+Y7SyJEX4~)ndxKa*fiW#DUN4$b z=$$QLIroO5TUi@ci;!~qS#TwN%r&%*mG*un+^LpJk$rP*%}g&HzIDF-P{bC)HU6SV zv$?xxLiS5?MfE?e%6nX0nkEQv>=6oXY7OLkIvA$-PZt4+DRgdN9SX%Jn|2+)*Y=mnGlzvW<`#+9>jUt`i22A|j<%kaJ@m%vp~WYUW@+dpe! z*cNkT>ZiaD<=I=$)_PtpE^6u1VfTcy!28uz26*Ga!GT;NUAN!WKI&r)RH!0jSLNVs z(Rn)1;ZfTF8y7ep-u&EBXcF*d&5z{O4Md~5g>SHfa+UhIWL-#JSXekpE=oEE#Y3j; zt)Lad0@y$9SH2d;8-ANt9C~6-4)}Kyi0wBmSHBj8Vf?8RF+1cToo?LalV>&8Xfw`X zgv%J!e?m&y=5Bf?D%H^&{t_tR!aBLfBk}wq`7u=znL6ZAR8{#e`=9xFk3hp{cbDdC zDxE0QpRTInteCn#>5G4eXiB(|0uQ7v>N(vJBE73k6GP85ZO$ z$Ziz3T2O^?_z?U9h9->Q(DQ$CafE#wRCnVXC68@vY)tIApkSvhcCQ61arcv!JzO+x;Ch*9+P zTl4|H?mV7t)SdrvDMy7B_%>WkL#u-Wt}!H_v86+ZN-S{%Si67UbbMp^ zr7j#&e9e96%J<5)T{mx068Ug3E{WVsjg3`274}SY=kutmB?2?3;h-^zE$I~x-JC2X zGj$3KTwsW&uy#(j9^rXB-Cj-BMEv+c`y&}2>l?Li4_$G5^Y;3B3YeRFO!8?n6;%Xh zwwmeFmi%UN7CO+X-o5Z5q?Nn7?qs`;-HzRhz$?6 zYMm-o+}xOFDd}AkKld8o;lg2#VJv!cems+1u7?4rbQnl%JknVK^seZhCsl&xnrZkbbE%!$w4O^+GdK_RVlD zxaLIP^aJ$*8dfqrx&=mX*u8ZxL3@WCeaGU8r`f<21Jg?q3@v7*LcJ_TDfCTW>!vl% z_e}a+8L4L!fS4jRjdZJaeJB~+D!n3gBen7$qUIpfDvR`&pFQpj-{rB>?QY26k5YDG z{9z5Au7$grpb+9$3_4KHbSp7T5cEYAzZPjkihaFdwKjan=U16E_!pxPz|_E^Twv)6 zWWGH@O+{s8!U-)3A5`lN?1F_D*_X`)ufM93Iz0afE zYeQrAP=|nDX_$*cIxc^2&ER=zlG_$k@v;gPLzeoa1HHWxKH2jnB0fjxR@95*H`I|z ziR$4!D(E!rTVOy)@h4yWb62Z)K}21&;yfQ`W;k#ZVP6cbPo$H~URo3jxV1&eKk1xC zc1#BK%U@#N5=3%S;LCQns=-d{wkjsb7QPvz*tN^yBamUT$%_xcU&2vr|6)A&X$SR) zMWROnT3652X&0v@iVK#!Twmub zJ6+GVFSCEeuwmzq|EN|#ibrKSpI-l~mQu#w>s3B^&(m}*B+WOyCjfcg8-_hrJvmUc ze~HlZMeSyBf)yyp)HoLhPB>W-6%*6vCYL=p*un$ozF{lb{Hcr3UH0To36aEAW*FN} z02}2dfpHuHpEuHmN6-(*DSx@`5{9Bf3`P)C4gHe1jQU4=fa1B=bG*-eW2vJowlH?3 zMeJ+FzmV(kAa3jEv@|vsK9do0_vVFebiRB^q+%Dt%_E!*Ba z{gcKqvU~Dx)_+I#GSr+O3Ox&;4I^~@-XkMb6#UfMul3}8wsS$Yj_+;2K5#5CNKx*) zo_{W86YOO%0?*(nO}j}rKGVQTbS3l%(z$JWWV^eD*PKp8dGkTEhRz_{vB-gefyVOj zv9WO!%p=OUB&?&=UqM&9Q)6MTe^p0N{;ZhtqpzUJWOB|NtKV(LL5L@&iw%$6s!H!L z3f>D~_cZv`M+pv|n?b%sjw1geTIF4;SK2Rx#rGl<5V{rueJeaY59i%C%YuU(rP~+n zd{=_s&ol*#5Y^0OKL}fd0uLU)UF|I*D+m&|Bxzhmq=6NdCST^auL@0QDIJ5(Q}Y8( z{8tuMR!q3IM@}2X5_@#$yXX*=E$1oEccz9mUqa_+X7cXFQ>nsHTI&eK1xkYY9blsj zES~r?v*0=W{Bk-^8$zc#Xwu{3>g$U{7&z1Be|N!Aq;t-*iQB0boTsqnM_0F^JRwP# zI^Mus@$DfN|0ARDO>J_eQqy9bgDe`N3DajZoeA{s`Pab~vuCN1&g{P>s?3Y6kkE5t zE7mY*Xkvm=J2I?%U0hsFc6V1-L!gRAU~Q&`!4dLUC9Alvff;4ibo2735EG|W0wPhL zd0{3KTw1~&MBUT)^V7;)-o~QxA_C@y?iE!YSZ;Q-$AlSGrA)Ru>?nA}S?V(mAWc|I96SgIs zjt@Gb*BVJncUG&cx#Muz2HeDrwf@HYx` z(2*BJ3uE9VCVBjNbWzjqfa4J=mn~ZBHTpZ(tZ~}fFcKV2D*1Gl7<`-m?9!|s&uRgU zui=PiG8X7U5Q&R~O2J>|G;!#_%f63nOHxgM^y>C5JB6Xkq2v#fZYgIAEw7k((!;1Ad zteUvt8q^m^R0lhq_%B@t)n;Pn(X4%o!`oPkVno{4Pp`^?cN@i z)myUxtJ)cY>0qejBY`NK378fy80@xpy;es-@!`ktz+!g~k54O>WFfwM8^k_G8~S@A zI!RhyFC8T$JD9&}QqZeWkoOI8bq(@jfqXKtbh6>d zLIkp6j%+CF>5l#}KF4dGeYMInHXeg{(J-1{Ys(kSMbMbI_d5PlmL#eEQB%5|- zQM&kys#4D2SRlqwb9d_2C9fDQ+xVuOj`FXsp}%zdPA^e7>iO8a2fl34c}=1*%XIwj z$P2jgmUqVnF0>YUCCl7C$2gP~p;v@*2TY##b^d2?C@eF};^%a{!MvMzSIS<)QTLXG z$)ei%@Idi)LGwO)^R_N&{o%p9KGTAv!%Zj$d7LBvXl}YiL%w{@No~PtboBx^6wCOw zd_mpM;JcYrd2+TW2EkC9(uGw_@IoaTP9OoyUkuuKypqxuy+by5_S#S2CEh3FY1 zW<0YWWLALL?pEzUifepZUJMJpUl&Nq7(@nFzx>?(ma|rI?`)I^z0gKUgI9jcYDza{ z#S=d`II)^F6efY9pqyPgj?mV7C~+R4t(SOB%Bev03i&T0Vpoa$9u#*&>4VCzR|LeA zuUk%VCu<%l^MqR|Vv26Jc!I6so>s#MtsC{e8^rEiHJV*PuhAq+ZclFWF$klg!MgvQ zKOir<|F^`2x`NzqD1t_Zff;C=T(vZ4}JDP^xHM_EQ8>W`2heX;`QpTa{l>|3$m+ z2xIC0*v>i1u>0ZQ!O%ByO3>G_xh@(EQK-qkm{YQ%l*{y9B&0G6EB`j#x^b2%{?}6r zqP0a(OtC4I8lA?+)&{0GYA2>r+tTj5ABu{c#dO1SYr^`;op)rGz8g+77CGGEQrt}R zZ(b$9domnk$W2D08O)zcM@w4|MHdobUu1Q!NB7ASanI~Ks9{B298=OK-2fQrTNc|4 z?GobCb~&3zoc`eZmB-fV{_7c%GSYB^e|1})m(26acT|%SJ~8a#hQqM(gn85Wtu|Z$ zFI7AN0fA;ZWwBA76L}(Tb_JtUOuTBRb#={~;Hki;E?#h5*cf4&QeMWW;17uMv38I| z&UiY!RKxyb?(xY{?F`7VDb)$XC&R&sl|4Ab3mWY2qkj_2NP%rz+;g|jSvvK!{M5bS zUo~xC9&|l%>SOZEdt7Dav;4QyazyhwmIwCXqga=B0E*!Z`7|h-mrHb{Hr2?dro@N% z&uIJf)XW_GA7n*U%?ClDR7s4$D%;G+&$jr$ApAl2Ro!4~(D9f6V|M4GT9UhsagoLH zC>7Jw(aO^jVbE#5E3c{dgOsG$)&5+-0hO(p=xGz@w~oHqjGvF^u3N3kq6vX~LXRjx zO#->OG32qoSYtug5IA~8Z-65L+pik$<;E%te3VE3v?EF9%>~z-+pI?JBQH?Y>K+AF zW07K!nTN3rpYSY`pecMV*eG3dmeTCRh=Wo{0_K}!E$79bQ| zkGX<+lPZ|~nT$Lv_O~@T=9dCw^T17-Tb3uJWM^*UbWA#|d0R&PRzZ!uR7y-#(#9nY zTwL@gR@&=C+R^>xJBZuLm=%UAdRU4Eb(rpsfSIZxGYQ&!z|RhG?YK2|!F}n;?g$1Z z&mh-cg_G`XyS!;{rdEtHB_aJgn6j1Tq zauJ(gc5FnzF0MsQ#Gm&MAF8VrGGg#MvtC6(W4ud^B~T@$IhI*HDX_5H0DX%kdkAaU){Z;ei z3{iq*5?8sjE$#h;*0q(U&H76t*6@M#`~0Bc8*6?@Lil6BPz#!>Me7@!?GljgoP;xw zlRo+W!#~{vCN2-skHC)6a6J$zS}@T~{zyRE;jfM;hFA?LFf*6dRGK{A&7iwFJEIUV z*y0PyahC0FuReRVa~r|ZK>~1rF@xf)8$(%wyvS{tdyEL^eSEd+JPCT{GDs*ffMruWe8RA=*$DJZZ$xU`gi z%*Z9`kcaK2*G06m>5piJyd@r+f6?=|B)w({2J(C?rTK z!<_?UF{I1(%V z)%{KG8_XlV2ocK5t^wruvQ-2jt4v^52m_Qu5zk+mB-BuIzjW!&VDy#e@0?}9_f6c9 z^E}C5i7X`oLPFNm=-_(l)&_iht~cf+*GmIJYevqlc;<7r4Tk;l-$&JDC_h+u=vk$2=cM@@fHH1M2-3hBa3@g|SZM5gi z$ZOvX?{(2!satmMNyK|Ho)~kX{Kx%_9uxrfeb}Aw>t1fod)s)WrQE4G{hy?8_OINJ zWF$;?YQ=1L#iJ+zC$_eSJ7&Gxd&NCHnZQ~mm-5%|KI`O9^_hM5RWs@VnlFYmXI09N zhq=<>N><-_@(4#!j>1J_V=?N~CnT_Jxay_Tw4{cOe|B%G?}G`-{$Y9aE*=)t7bZ@d ze|!En!my@F<$kvuqDs(?z3)&ZsM=xuttU7~73J`wxL>;=bg+2h5u1=@qkfoQ1vzGr zjZq}}HRO)$Js)d|umQv-RlHnXuGcBI4(R z;0qHsqWd#T!}Mvl_8%OM*`&*3>ZF~m7(w0msZg_%?mq$xziWvtm;mJS>P z+_OMETZ>0e1eR(4i&AF$oBvrmBj|B-CIAC<`D_KWD>t@j4#F4v37XawHOF3KbjNiZ z&j1NeNdI81ViAvzkE3jio-1hU?_zaz(&J1*%) z-<+Vhb-ErRTga(ae95y*JZf_6l(eN&L+RSR+c@}YS|>l?T%KM?xn)5Pnl*_Cl5YfTjFj_QC&*zOF8sc-cfc3Dj%s zR>~QPff6bLvlj!ty_OsT1V*i{%1fzCM;LkQ#vC6+H5Ue6u?~dH;(TE1o6D^mnvD~Jl{i^2k{Po4Mm8Kx$6gm=S62LjNa9#N#c`qf<;ls> zXGayY>gG_|FvJ_5sPM1!>0F?+(q?UiEUWh6mrp;74wl(~@BEOrk&Wjc0vIqCLYr&| zGcG6pn8T0jc?hI<`>x8*f7U<0yH8IDCc%wv@K2 zS?!ScSD!GS3o*6`TvpI&x>%H1Qc+4C$~mK#Tv_|YXt`rxw(aJbbML9o&h58S^>(2byPAM{|(9FqzCY84%`M9Sl`E+aX z{9&=-qED!dvGc!w^Oza$C2**nKpQR1C~J8rbULnnXUFEa{LIIYbO3dEgRUF%U1{oB zRr4MmcJhPo_UK!V zjg4Zp4{747njWUQINz#Ll|cHybPtuwxxM~fcb*V5mv=*amKpm5T*i;9WX`#Gs?K-v z1BI3r{O>l|^FwI-CT+VXQhmPC3!R1^@4tbW5AgX80sucNhueg>`)=R?;i7_Xr}+;W6PRS7aPm``;pFx(ixfG-EBQYx!psS?9w}O|Ni+>##x3 z0&!zB^_ zAYWN;b7Be^=~=t%OJDWeoK{$iYqeig*WQzynNJ(8yqBt-tYF`gsw7Qu&u=g(#z0i- zj$RllRFr6B{bIonys;=u;|Q5Tqxr4e+SJ!H*@^Kdv@?vMa8S%Z4ju`oI1F^KQ zSb9Y8$Ka-SWfqNTEvn<+|0I=@70E1>^3b;!#Htnd0pvG{Pb(Ow0xqw=af=-tL|-}` z{G1`ye+h8t{l$)#JI)>vYSfZ@>hquI`a60HZQ&RRA=_NKsT9Is-N#lr6#m|yx;|x_O z1g}Vg|9em)pG$Ev?tWp?a`Jn(-6TlZkzN#|+vn`z@n%JGR`Snz z7l-14kB&o6N-FcwlAS8{{6GyIH_KkTFr~$!Bo9w*5E337bh$gR@jEe|&o_@J1Kz;3 zl*_TTI zj(6q&-tYd=3+GNH?X9n*L}foL0Z5Pl64jSTa_r1+}W+(Gr$P(g17}RBHBBh zR-e!4pM-b04?+_s2nwoRQ{z0eBvT|inFLAAgW>(dVfYXXBBm0t8;jWq@3jxF)Xd)` z_qbT>N(LOtXz5ufZK4Td-qhCAtUt9iO8QD;s|Ez{grBPN$aXhSvtGyxh9~jx zTEQxqjz;wo?Ghj=+mEu`IN%NUHz&WHKG&-9B~lwZ&rbJ#){$Euc5uiTb@T;34Edzj zpyD~sHHirc*Ecs7Qb~tW4XHzB1a&yii63%8#w+hU(lOqm8-4TkEo$bIl?;A*Fj(>P z=chzv!eIS>=Pl>3ph(_@t5MB@(DKj)*vb4Xw6O7{y1r%{pr&ovN4Xt3XrE07((ccs zw5K(ifvQ2UIA*A6wKolcJBJO<_SD;dNGD;W&f{G@+F}B$(#^Ep(QrDW^6r zk^lm%zyJM~uM@f5fPL~ii;9RKJ^VX~5NVssh%Fs%5GpDsg47tp0`#=*_9j_Ty0EZt zBTBgS+*23GoR<$y*T5d84x^0LUQP}* zXw+69PlXlyhds2EhyyMV_D)^rwV@D!`$YT?hS~YEw-=wRo1)9bgZ|lA@86wl_fm;c zC017IkC4NsQ8psic=;r5XXxPL zI{GcZHwF_N_$~PvGr=mCR66DAt?1AWli)nphOcy+qHxAhGt*-vG{i-$nvex?iAUjn4<^H&n^+r{9hg`G@DY)rtoX(7Xc%;lT#=}j1 z@jt7#xY&C+DC&=~v7xn|HiTKQb#2vojaE>mwGmQFiEd+bBcsg30OGP!1kxakk^92O zje^&AcPLSj9fI~vP+Or_dfTs?E|E2B1W+Kbg|eP@8j(*=%f(TN)q-RF+VTU(f28pm zFiAP&h?BY6F_R>8D#F-2v5&`tN208LCRGFtM5HR`33S31GFG4VnSE$4^El?T55`|?B%bO(M3B4YUL zAQk^ljp?j@shGOGPz<2FZma9?99k63oNDHMoC2-%o>Sz80h>I}!xp z0uU*|5geslG2cE*O9WoO0$?KaXmWjhojF$odNv$tOqIzb3sMIgW1Cst` zlgaGp3Pw9nQi`@H1APh{6}OA)g~=l zjB!3$Y$@M1%6YQ_(FFbAFD;QCWBy_Z-BKDr4OV21@318m4}F;{t0XLW0R(2XVIPE* zD|NST5dWO~K;ZE2_Cx}2OyVGo1_ia%&fTSsY78uA1@eM6>?IiuSH39e?W1)<6p zt=sZ1H3IkDWfJk1*9*l_O?Gn=6_-r-jf)=%JjCyUSm|kLjraITlj=iM;VFHtN_cbgMW`Mlrw zPrvVV>+pv}QY{*!;uq3C2|R~f&HCIk2ZA>GS;t3P zzT6SB`$g}qWv5;onya0?@4`AXzkF?11YvxYbPb%Wf{q5ZCCRY5T5N(&4Q%Ik|4U&;H? z9y`|%P(6SeM$nacr9eSFw`m13AdIRM`rm}eY}Q}~26?OusvdS58+8Dt@AO*kdCOX6 z4l8RPhN-2+_^g(T&pjM6Jzwx&CS6RDK`1;kghQPq~HC`*jhU?g)og+u#4jtwv&lXhk#0Th8 z18&E>VT~wt1|D(hKu9zZJoCDp6E@`I=I-w96{MZctmpUqcyn`IX4qIGF#QqUFLFRI z2V?dVjhTsbf92L{kQXIyAd6R_8R@yplwbYTO~(ospulc18UWLEpYNNt=g$q;nUe#X zcNK73STE%qbf3m?J=0fL^+u#cYyYy`es+JXKzu^#1;SBm*sk zB>vL==?6&Zy;M3*2fZj?m*}@od_VU&U1G)MG2e1I*x=TICSmKJWXA#n=VOf;Z!hgX zd1S9GbARvsw=^TaW+4T6FYH4vtkN_G zp`*`QD?d5y28e^Xw4*Wyn-R|^sA;P(Y{C-K(ABr4)u^E*fx%N`6u25~JyDQOHL@^( z9{FJ7kEfx^?7R?==j?U;`3K3rA89*b`zKHnA1T_O!5**OlU~dP4WGVIbm5F0Nr1`X zF}eeYe#QOXxbk~fx;e`aBmUOfX1`kGX%&r9(O`z$@q#z1$Mkfx((kKKxiSXx;<68q zz?sKjwbp%IKH16R+SFS^m(GIs{+1M;$c^C=2E{>&e~xeAvNr7CKl&D2uH4EED1?YY z74dvgQBkV-5^tn!ja&hO4gIpji}a37f-V2v0P3t-Rz5SKzr>Ql>1B8W06Q?~wtVuX z^b?AHyR<_zQBt^5^feYO-Lf_B4ieO;Qc9YvrOc6q608TzVTMqC2rUgw9IFMprIwb1 z*DEq@I3(@YmOr5co5T8pJY{w5Dc~v>7)TL^u~wlZGS|=|{nZVo8*p)jnoY2SQpU@o zlyj7&T>{XMm*8ren0C{mHenq~e6#)%k2*4$N~kC#&E=I;tKv@z9LFFZ@ah`iNrm`& z?)s*;e}NL00_+C5a-DUAv@83vvX&^xn!7mnq66mtW=$a#q68i2J0cWgras?{|Ebv^ zlgbjYI5Z+fh}n+RflRZPwT=6jS3Y&LNE+C>ej|3oP<@pAphM%Glk4^%PoX-vT!=md zYbSAP)_0w{Y*SC*{7$3(*Cs*agg$@Bm6nzkX+fwFi4qC2^}XcVgP%7iilgZMZMON^ z#!YwNrLDjs7l;Kw#nlQrDdpUM!PrguQ1s-uLadeP296mX=QxN49=k_{xXYqcFk?wS zthpZl$b(DdsG>OD>#FFtM^9_CFv~}2Ij!v%Z>YLnb<^%9=ChUl-=NHs>hn$Te#xgj zVN>3D-qVi~FxK#ZyGvZah?7%+2;4qY83%^#jTZNvj!OLx3x8N`<)K%tFs#D)CG|+8 zs78BGNP^a9wY(D*4rPtHf(((*U@~;32F+T|z`N1C0151n3ERtSQRlFX9eIW1Es-7& zbXpeVNRfh?Xo6Hs^Cx%BhuuehxhN@=ihif>8R1{-=;6smsyBO~OY@Ct_2*NH#rqPJ zCg$lWXfi_V`Y}tw_W{U0w7)#831%z#3z6-g@adO*;bbU}Z@R+Pa@pZJWz)Z&XztFu z?5pm7tp7e|6qWNtZlPjuLmqwsZeqDg>ZV#+R$3{{jPDlC&-?M%^^fd+QB@3)zX$-# zLI>yio|G`)(nUwFg!{DjVeJz4tTFI>kg1&4X8;4RT3n`|YH(l#?DhCymwA6TKZ!;! zj>9u0JrE`uR+>iJSGExF-HATi>;=X8RcOI~7V)o#Q;yzKxa6LtIp)qS|KqWto`1Sk zrPJgFa$j;q`-mS_ihDYq(L-v8k|jL?kB*&+iQo-!Cefi5?&)79E&E;aitTY@z2!DUBr(?r^ zn)lYYXAf-z9`kOkNJN9kc5JOl^~?cE_P89HEMl{lX{bjaR^!Ocpvp}p6$qjk+2=F4 zeVhA0R{eZmRhsc>9{S+F5|o6*u|-50T;%Y^i^get=7^El|4o;0z|+!zQw17@C745> z4&EV>^@!Df6n0Yd-HkE(H9bP#X--{%DlQsZC!LPg`<~G#+CPb`P#= zGzEZpB80k|FPgKZ3^O27Q!L`GpVn>*J$WClRzDmD*{#Ro_kOe!)-x$ zMQEPO8Er`^(CCddj%xQd7$<~CX1}3GVkDa<$51S~vDf(pK>t%YhM+^iz?w%yEb_ZZ zXObH~<%*%n?+jeQS}bMUMN;KiV=T(*Y?)`7ZUpPwS4{V{CNr3W6 zrU$2jw4P!d0eZWRgDowKRTDp?`Ue7F`p^gW?-<@yT=2$M5e2)0gU>bvF0K!g!4yg| zfX>HXy?T^QbSJjKPSn%7ZB%n*w%y$E6TewmQF^7VK=!CjBa(|E!}d-U=#>6JlLjx@ z7ipEP^WBKTysOiHY$4pOU%&xsqs4l#hk-Fa8ykzVO{oDNK79D!lt)=At`uz)B)XW0 zm*29U^|o5!279Xlt@FNhO?5#Z|1&iMMj`&0DH?dA{q{JO``(wAS7O$X@BIhzJ|kt= z>~F_&uKZCO;O6?sXOh%>BDUk%G`C=Rw}Wzi6v%@@ zlz9i%{;Di!9%n3Spr3u6cdDgMaOVUF;BZlDa3A+DlH$AzTTsi>2(8V*@;|QV^MA0m zLMKWBln*IpR;zK=@~bg0;Eafyt;RU=+Ea9#b%a4xj=-kx(32)gCp1jaVMOXWX-7}9 z*FW6xUZb0uSXZp#&hZLBim&{jX~PZGLX{vGn3Rbp#y{)V%eR0u^l($=&#DFterX3AC__#d|qpRT+)rgd``PPZdKc2ab-Nj5#PjDc2 z{K62lbG;n;Rk^~2zWeEskYu6jZtAjbGOK0jHkv|X*hy7lm7451z=!r>A6_)%7g?d= zw$ZSptEjFmvzlfNFXbBqG#sd=Zo_chn5@#@ba2$xUO7m>)7JmECTq_gvNHYuSpd}w zp#n$>dI=x|WfBUDUML zP0G`yQx9KDD5xHRpNq3L=NFa{{Wh)Jp(qt>+l+fRK7j?s@z7srt#9pX;uQ0w{i*;( zfPmEu0vIuNA7U1AFKzCBwX)@3&R+{22AS%pOjDDe>@yq0N0^kAx8|N>zOB13w-rP? zO1FGZhba{@+_5?{eT&9PuncyM~5aemLCD2JaSR03hvJ0+52mvBL6osBfLP zGfp5XDTAALzYKVrW!JCF2#x>KR=dfVp*=ymk%@bhbQ?Or|NY>D_nRj>Y-Ap|i6Fjz zli<>G@N*vwbWK4za~s?~4R1qkBKVYkIEw~l8O~v%=8HugVWsxB6{~U$06@SvSu}h_p!!y?Fj&zL=L~IU0?d z%NMkkDs7FmKJVffG#rd30C_$<_2cX~c16-!1fGf=rES-9HZk0CpZ96Y`s&}lKY>hgS8AbvlKmghsBO$ykaT<6k`Tbw#yR{y zzjXp2_T~johO7{{ zu^hIV+o*YW2K)Q_%jJ?|?>ht5mdDv0OUH#J659scf0N=zkF5tH`i8Kb7w#xla398Xl{evo!q#?jBw5Wb?wCK2;Ofx-EcKC)0CC5-$h&G``z)!0sbF|&Ef6Jt zYTk-c5XV8F`0w?4ZLN_~DU}H!+G|51;;*KfMJ~{fT(;%7`smHW(_NJdcLYx<%{1Z& zrgizA2m`N=rs_cM0tARV!e(`~!v*|6q;G;6kEnF^gt}cDmPf=p0nD!A0Wg=)Onbm9 z0FFD6cN!-A0sJy15?MR_Nts^QO=nj9ZrkoHKXIGd$|YQ-+P-z*tpQu@grnFNwk7Qs zk;2taMC%Y^6xrWyDVPj~c$B!O8d-m*Rb|V_6IRKCzaCBHU zaJ&F7otGQr=1&!!SlHZX#!IJW_tANrWoj}R-xzJQK@^Bq2uU2K(ON(eTyG{ZhZ`D3 zsT~tXSI>puU6JjF=}9=!r#%%71<&KE9Ep?>IzLwlQ*QJd^&9eWysOjMOJG+UJ+;c=3&u@(F5tWkb zb-kjb6haC=oXu!7S}vE)|9W6_+tVK!JhfZ9e%`L#JGu1ViC7;jz*)Ng zBA;wJ_yZhv=hSQm8|YAd|C88Nnuu2reIwyN&6kB{);VjC(E zn|XNR-G-~&F7{&bpLLyn#;l(E(pb!{XVr4Z6n-MOsw9(PTJ$nFHUngea#@1t7f&p@O;zY!Wi+r>l{MXTz+PEa@ z8f(Ry5vF_HiA@qNF*-{?vP?}2OI6FF&QU*B(eY~ z2?(sw#uCRs@*UhZm_@c4Vr%)T4j_aYZ&Z64!@=Zv3@MoCAZY-s2O1i9=>P3Q&p@H* zPf?R$vQ2*p-Fd5#LS#YP*Pj96p#`0Z4K#fa8T7{$cB2bk$JWFyvAu#ZiLZDDT*PnkSy4FY+0hdvX3`IrC2a3#MN3ceR>eS`6~%dP%@w_%wnS|Q|m zy$+;S+XTKCgr{vJX05fT<6ok+0_w2J!a^%f6Ooc?Lk6QA(bQdecK6Y)VB$@~>pu8v zdnV4maJ(IP=bF^Il2Q?R*)P-WhWTY+ch1JNO}gwIY74&(A%+hT(WcgwbMtyb+T7m_ z*=QFt;h@&PpNHRp5I)@WHuVqGFcg-*Pohh;*I$3)r;ew)_gqEo9U0T@RyksAsJ3WsPO+UIAyQf( z_b^p+A0XIO6%>EXW>xzJ6>Z!%-DDh1zSUj`)@hp&0L!v;xuC!UrqC5II`GsIxWG5f z1iI|u0OTd?X;(s7mK+)o8XZ+UIemev{=Y`ajYC91SXoC&*lL^j=a`5`wJlM-P!JJ~ zv2_})X7$+=S{6`;FBNY<`*=I*;kOj;CML9@p>3Rl>i?%d1VTjro;{%WwE(Zfe5(3sjf*%D}NjI};Ib#SB)%0P2k*XhMm zNt>rTf@}A?rXHuh z3c%SdVEygj=|=J=e9Qj$v*9K|b%U$-)rQ^$JwPHXsOwMM|J7>c$~1PLGG6d;>+ZX6 zKj&OCe?B1uv`$e3K%B~DG#X9DV~EUxxI+S4?Q#dKdn2x)9%o(J@%!2s6C>A|HcC-a zIJ`_jF79<%4gR8&5Tb$a~ZTj?l>0?|LtOd`64j@6h+}yphSp$z#W=N zf}SM3X;s$tJ!xLJn|6m~Y5!Ze9h$(b-OOaSBaRd%PzWJ-r#LwTF`awa9m!oaQtJ+M z>(1PQh7*1Y^a4CLTR=!bwvFxDRAc}^;OoX_=*!adCdlWFZ?2W!FOf4BN!p1EUjttP(c%1rmlpK(k@6%&~^*%mg}-nod&|( z{{B8gZA(<-9FB#+GgXwaWB>V4Ik}S4yKDQ}+yYl?UHm@Wg*Cv+ANZmtwH4Q|Aif1NX z8}V08))0sj+w3m~e4PagAOlkk0M=No&C_Skj_)0BtO~ji!54=R#jXL~9mN^nm}1yk zx9d4xGp^->JN}bR!6LrxymhTEqs#u53aIr!Q9ugpj=BlK1bvSj&GHs56CPPmqB(M1Df4 zh?|jZj(A?ce5$&LOG&}|PI~obYKo1fFWv_9s}igg-3*lC=i#+6_G89f;Vnl&Vm(#ibQTjDcJ)3p7sVR@%D9S zUDFFs->~iJ-NM9xoIK+rb=J;t+|zALLD<-RF zNPq!E78otq)PfzJi(1bomL;&lDuy_;jw8`qJU$dTFTm0R?$KMU?9w2vi zwxUs&MGR|Ccb%o3_>X$=;)Tn0+9>o8B6#X|h078VS%#aBTR3@0F>6B_j=!w&}rVgL!&*k%`==XHF&x+*O zWVFvGx=(h%t-LKZ6WLnYVGXDiP?e<+G7%)CiN+X&xd>V1cIWyX__^2IfMGzZIr9Rj zQQeSm3|vCbmw)~BMxL3?n~Uliw8%mJUmxplq+9qoy&-DVsNKY4EzeKQ#;(RfJ7qOf zdwYA_yvtYXgJp+D_}uhEuGr3D>`Vy-HeufAF1~J3C)x+#dMl}`3PmDo?07W#>}Nka zJ~{cp4}QR#vC_uaj{8PEKU+YJWcfgk6@!=nO!zQmd!<^?dJF{ppZK4Bx+1?(t|!pl zZY&Rj$FmQz$um5Fp1@eaSbRjAgx84a2mnE`WdB`os35@VfHE>3-SDrjSH7GO!ak4s z-im=pnp&l&&(#)ly;5k zvxxL#)4mQ7k&xo-{OsP*Jw!x83xH^SFxoVku;p;lMVr`SyK{Y`FdT5jn5}vIq283^ z@#N;_#u%+s)V(Ju9_#?5P8J$UfI4YhN*54GNu6G2fEF$Dk^rv}TK zcOw@TC>qGy=91o?;ogEaR?goVya|#@3Mu%1x~ilQ0I0IO0A$tat@o2^{8|DK-z+an z-LRKs+2$Y5^Bg?Pv6J@BFB$wdc7%$X2x@RII_kq`ekWZSYPEoA?Va@)RyR$2qLmKN zX$sqpja0%fJFXm3O9j2{+OF_=2+iGA7Yz@rx6Mnjt1u3hW%BTd!}uqY6zmF5eH-O% zdY-D80612FCbHTXT~&`CJw7-%@GBHuKE((%V1nX~lnCy!KKdmMz7cEP?0V-$$e?j&6dXD^=v5n$YP(Jjn>Y9hrsj{(D8k{SMr_5!V%bz)vJ}eQYtVD= zV%y+VvmMo;zV0+&)^J-qK(b_wy zKL+7f*KnN*eP`21e6d8K9poeF#t31oH)}J>396=3Rw(kBLL^h?ac5@r;DgQBy zf&B78yZpGPhasSBeobhU!G-v-;q$)o$*w}`v$l+fLI`e&01&7XOltUB3@@9$z6Ca& zer@%lZ59Uip{lBFBn?OzGsNP=#zFNDr=vaPfS|MatPg@WBf3RX+tRM73pwGOC5r;L zVMe>Fn0tv`B&Cpubx4bNs$7`s(xx20Vd(oU`tJJs(;HccGTUc3IB+{yeP zqP0+!)$`}i9eMx&S!eA?UI}h%lR8YV4^Qg^6CSS{kH?Q5KYH-sL8dxUco9(ushW(t zDemLCwbLLvv`HiIx*O6L18Svbb23)C;^zVdqVg4-?!_eVX!_jvqR1MO@{EDE4m+3C z-Fq7%%L+brOn1Q4eYK{oj)|QQE{!&2S!z~zH@5;lD&>rHqtQqy_IXVsAHU8dw&^@c zsf~Pq9@N94BfBuMO+{0aRavGyF{vvJ)LC>18>?rBi`_xa%bGLq2?Rrc0-=#G$bvwG z!41;~#`}cDTN^+)Waj=UYHp#lHv9W~03d{aeBM%A6SXKK92;uT0|A0laDgpbvzM~vDVm|*^S={hmM4v^F;`eWtoe{;|ezo2_Zu7 zkp#SJ?>vdIgl`K6-6H_JLooQVeQ{@nKsIRXjFGDmo8_fLRY#2*;`avb5kNBQc@VBEm_GiS{-7L>_N4IwYeL5ADh_O z{ptw9lhxh#V`g@Moxuu=PeMQd5C{sAC@V}uFY!^x{PK6c_Sj!!bq;T|t}3~FV!eKPf+&~hn<>u?ceh8AVX)_RQAomkO2xiH zLNsh33*1w#7XSd7;aodA1cP5D-JWHHjW6G8UBTe-U=pc?gG$kQbMxx@)n>ENx_$ik z@!$UK-!2x7QjJm#05q!c{0hifxj#hgAtqdKgE*jAb4)~ll5b1ac)-WHH^3o69csc+ z6rcX|r`xs_E8DZPvtc__dkhHd*KclaE-o$rfQSqj&`d;~0c={sK<#NKcNw&dmueth zXsQ@Vv{ZdP--byjQ{@70`4{3%OI4$MuX=Y)q0W=-hPV9w`|sbsf8SlCg`%cWL@F)p zI~ks=v^H(K)!JIDTH>wGvvl<&w?o3_q|9h4+GsY`^I^GM5UGpHi;K$(S2H3KGb1Vh z%l*s2Gxr|s`#&Jd!48?q_B5!Fb*o9y+1c4wzxLH;vG8d)*tsxDc9L|m`azy*`Lgd^ zYu&c(&M<=6Bg8;?pQ0QbmhBA#mum?D*ce2oxl1enBd!p=uZS_+SIll{_nuhwxRWy)AZayy3I-soP=hB&#Ql$IQ?aL|K9rJ+j@8))cMt3ifsUm|DM@_O zun(pATTM?8N({R+yhBY!ym;X*!jhxG0dCI_VV)bt%nZ{v)1A< z&FC_%fDm{6qpV@pT91zp%4{UxQK?s9f|4?JbpMZ6`MAiL?8o)e{4uG7^T@vZ3~6$&xDL(nDbG zYra_0#9{GNT3k@F+we&Dh2-AUAbq)9G>c}bmy6|+nTnFq6yOuT_?ups4n%Ag4I;jJb^YSS zRoiYugAM-+Gxy!#>e=Z*8LF)?4$1T$*~eoyS-MqXnM^@od*>5xeuKXf+87L^)`MBKTS%jJUy55Dr1ul)I+|M_ycY}+n1l$bE8M%l28HiukKYLjb> zq@U)Hu-)}~Eg$9GcR%yw>66uJb@||dykVm?*vVUPwVZT)4&muY%a9I_60+e|Ym-}2 z1}yiOSvzwS=LVk{MPdJ?u+%^XeqReg9Qc%&_y6Cv2cG#vsz!+1ZQd8)il#HYSOj z(Dh$zI*{Y{8!3Wo$ok?;$-W$BMunVZ9kQ?q0g;3wX`1H!pTEDia^J7uNQlnj)TesS zY(=^6!ihVD?Pkwb?4VgQ)k;+h_(tY!>iIvZ4Am0adiYqRzDp5MV`f!|!dsi>R16S5 z``OP*WFmV}QKRBOKQacO;0a#)DMQMtyRq+xlR$x<2u)~=`Shnhy;v?3spWF{_S?jKHYfu=N2s7B-D!wOX~UzP^6>^5vCS54T!3lF!*Oj-_YG@|VuDKI8X1 z{Kz|xB}r}M7Zne9GS!B@|E5FRsK96sgxs;Yk2S+yNtz)K1x>OBrZ_$X+`bGX1i$>+ zRb6~>1jryZn?i*$x>sh^QDn_kU2Fgs<=&3kVfwviJKS>+tMFtTZUO^r>M#1V=8bBY zx!txBt}M~@TdA^YACjr!K9{OLjT!D`y6#NpGF>Ni@N|#pp;fYXhM`eX8Bfg_ZuT}s zu{8BpMEw^nGQ(iSlYb1Ahbhc}U6B$S$CV$LNExGzG4KEEeI$J6op;a~1y;$^r%%vD z<{?B`UBPqANhkf!-u1pF&dZeWbSh(#c}{H+iYYOSu^)RE+ThCeef9EcwOU=hxZ3b~y+ zJ>GNGjkm+qg2R%&2jAkzz)}OUU)Ko?BT5RHj;HBDis10KLxiGVZ~Zqf)s}@97R?}x z555_OV{6-Xu~>98sN~KWw+BCahUDW$I_G_B3Y9)oY3UeYF?Ap&m~7rn=K@s{E=N>I zVJkDI++IMb9QxFV=mTPCZ49$w1prL3QWc%gt_0p6*>ovhY@PN)n{B%_+PkoHL8jVm zD@gk7M~}|V&d$!x#NTn-Zo$P!3LiGkvgn%tfcaO&X+;~;0K9ndVzpX5csx2IGFc7svf-k{bsWj6Fsf9JUWc#Ihv;Nxpoi{2n3-H^fe%h$#%n#I#6Prv+?FQ1*CgZoA* zOqwtV@*)Ypbya^-%TBpotIOrUz7rtypH54T{Z=7#<~|(v^{`=vp2k0CHluytV%yW` zeW@Vhk-OX7Kmew*C}lQ8R61yeT0EJ2K88!uxjcBCB<`u(x8q)cU~57oRHP)7M(5`^ zrw#)5<){vA_gE(9$I{79B3KVkZ+#^msWBfbRCkyV^G(G{xFR29UX~jqk_9B{WVc=q zzF(Ovz`suv`eH6+c*zSAwX3UIu2%Bt?y5D`hD+SO_$ zo8XXgKRDRzJP%kJ%teMj#0`#JWPnt~fAi|*>C>k#UtY1{o12@Po10cQjcQh_RX??l zRgPkAsr1@_)87Qb_a`#^vK*;cf0n8j`C<^g+zP+Gqx5 zFzDh^!C>e6iF_%$zB4fOWAnK(`I(euB}2%vH*>R}91drFc;-=6?eg4DZF95Mx?RME z0|Q<8;))D2DQX&Z&xJni{yONb-(3%4NYxT`UW9v-Tsoehje?@POgkNWx@ZlK#2GpxA zaBIjdCCCz=-!NLeU2_M z7eL=LG%DGT+gz$`+U~)q6NWf^2!~qSaTz!?e!w7ds=rcX5DCG7RHak|XECseiq1F# z<`!92vcKcrDmI!G?9Kl_F}I=)oaDe*RtnuqOCPkeT#K zmDteH&fqvr(3J`n4mQJ;RDB4{LEas=)ByCDJm`A|z6XM2eb4bAEntb#!`b6csaP75(0>X~deDFJL(* zLSxuEJ`;s-7Cjx+-^_R%Uow2IK23O9(o+qfd@sA*_S3mIkuktdQKYoiqMb|EOux&N z*i(SecdE`QqQu3@xeEJHQKfPMV*=wn_g9PljnPUq0^K6*rBdYM)fc*Rd~`l6y|Wx4 zA}$uqw%u;qZPP3;NF(WO{|Xb$n&Hkl(I_NS$4IU-Lq^t>rzva0%AM#hEK9{KCCaueH7G@jh?x|r<#HkH zsgpTp)XagU=9c0##3hWoX>RDs}>s-IQZd;9r z7nc_{HB^v;O8yzds-$A@ixKbk6b$D=qdujYcD-i9N|9Legut|PK<#GtYXa zu}#|u30wD0V5(7y)OOnr55CFdj>EN&A3Qzv)Bm(mwNa$ZUw7Sb(==_{iUVa=e~r~t zZ47~-It$k=Yz@{Qe^58ffSndd{)#RR%7BaOFnj9CklXTU55=)>XlL98k4op;HOGh< zKaHjXi#q`;r}0n^Xg&PCf(4Z#)%3!fdv&5e$mM(PfFKMqtf6?UIdC!wqghF3TzQHJ z)cg1EYuzH^{d@N#Yzo0FKqR!GvM9fJ1W_i3dwwkG`=!~B^7QEw-#6R^Om9uHDMpVd z5xF_C@ObG0OJzxpjyR$ip^~EQwrwOruOe$_lNe5MACE;{?B-H}HH!~Jw4*>lVyy%z zBt+vAf}2vw35CnX1|>P}d`PAC?`OPn}NmT*q2~#kl9HJTQTLf4 z!%RdaA)_SRay0Eulo^U) z-bpUk`qsqlPBfm!Sj&N|!vm?ad@&cwFzexvLiWykF9NKXq=PC37#MmFA^h@gL&g{) z<=IpJGemothWe`f65CqBwb_0~Z3Stf!*&%ii2nAa!EvNvFVY4X(Y!zYNj&N%jsOBis8Hafu-CJKm_4R0f0z_@i?Nx z=$MXY4t)C>cx6hay>jOvLR5-KQD|5;pB4-gyuMTJ^gv_)EF>a2)^qLN&wEl2PbZQ| zTIJy9NKsYa7s2B?zfplRh7D5X&`h>2#pQrL;JQzyW3GF6et zo-)QO3QfBYYd0{7QZTyT7IN#mG6laC-$@o=?c3fi1J-f1$jzQ`K_p{zF-Jp@-WYvz zb8~ifMx-Q>UjKQA2=ART3Az~Xij+!_=xgY&Bc+x`sj5G=J>4lXtBgBT$%3n_f_|(? zdstiT`DxEZHW#OwNNLbYDWw{%TceFqoDNozsm^()1~+FF_bN+|oG}cZwLf6r|M*W? z4Np&j5Aae;)kDqvVrHK>4v~Ou+|$LP8F&fdOdPYw9s-Yc1`p}X?>aRR0e5KBD}TiY z+&Y6!bbR$dM1oQoD7%psV@aV%_R1d8j z$UY`y_3lmEcDq`w5WpBCPAuqcCtTsuFuesM??ak61lp!D%$=tttE6L3i^JN*Jp}+D z;4p}(YOKObktyv2nAzcrg#(!^iKz4;IgGO5rcvz{m&@g9wS4}3z1eKe&Q{7sAsFAt zykl|x-K4I8+U>14hq2W3%nw|>{N2X!=`IR-L%Pl)9aN*v&d#-NN#bG<1AyeAN)dx0 zgS_K=9|HFWj0$`KS&KMenl0c400x8}+9-g7-N944z0Lh1&imf-0SUPAz9T-@WI|Rci*%0li ziOHo44NX7e!qP)(z`mK0b`(tFxr|}L9XlEkalKwQP4no{qwpqoJY&OVu?Tf7)7a?8 zczlFRGDO1{+bkO0wr6K&0AP$!E+L$6OAZ)OtNTI5x#Nyyl8Aa^^j!((M%6zU)&9Wx ztJ%{~2r+b)vd#Cm1Rs_^y3zk$l?CcR$?8pr)MBwPMz^{}8!MZ0GbH6T?oy90p;Vv}VH`%HE#$;3r3NAIozsXHZXI`Y%E(a}Q~-S30{hx-9C40i(4a9`W` zAkj%lD%H8wSX`r$%fbA0nX9~M+?WmwO2xhNjbw3^UM`oyzG2W@Z@DWGdM6O+)=SzM zzq(Gs^3F;X7unO$y4*!a>%~2dj-is5BFc#>pK;jQFZ(5@9C=Qkl4+U<00?o>EVkQr zv)L}1#*E8y5!5~}`#7^y_dkn7%hdaAbg8=bS-$}H`oDL4sJ*?pQ>l7hQ>prqIc^XY zsjb^eeGWh(%!V(Cx)%qQ>SgNOXEf}H8XF{bmLCQ>^TF;fiN93#TM~|R`{@a@QHlsj zvjzY}Bt?7J3YVm)+^^31Ht%}XqbI6|JoW$4-5&)C@T6uG!^;;qa9Os~9034~jed{D znDFEVmUZhk*5O8x5I8=ZWe*;n@&u=;s1!tEZ#XL_F=m!SrcZAc5UX9g-tHyp%+7!k z1D&budS(5)g%qvV>(y$djq%TJwb(Kv9Hm|8(n)RcsT>nM4+K1k1d)naqq`-~2?Lz=k2nnAte)t~vvC)}r1rwV9z_=uYc4J_(-E8(~alWxg=-onYkL}xC zdf(@r5AoN@?b?R!{kkTJds=3%%k9=AWo7TDtYT`X6udgLX0wOb1VH;PFz!jl_(N2d zz3v$-*4nZdDD+hWjiThPrKNG2=d5P9V*DMWB7QPb+2V+g&@=hKx~&h z&~$1x>NpfDky};jO2y1O(?*P!5R)mi0U6r1wY)g95bd5ijUFxhr;lyq|Af%pl82|k zdn2WW4>GR3}GZH$D7>n5!0HEEJb6u0|HrvM@dAg4T{vVmgF_G~A1SU%EH|bax%zs?c zshxs>a=B@m4BD6|4Z*Nu_=$*D&3QB#D(HQ&F}9=wP!#|O5(MmN^68ZOQ75(5q=*z5 z?TH%;Ya*;k3D)cxe&pb3Sopv`_mW^ZgK}mT_II&dMo%ZbB}XXw8p1cZ_b@w_YiV>s zSg+TY7Z+?874`#5S?9>9dU;aw@zlh!r=@ph{bSzA2Zp*mJ||O)yN<% zs$URr7k#N8T@?ZMh&(ea+UQB@$;xm%{{iWaVE_Q}i?vq~jWIGc!mL;53~2qt_iz@1 zGO$0&#PRF%jCxB_o6Y9o!-vM0PD>K%Ne&w%A7@Bf_3ZZeo*C;hiY*}uU?ALz!gy(l zNDhZa8(I0B0rC-yD(3lL%CD(@s->fGI*Mtkq zFaH6saZCX?_z0Fpoes+)l4QIjq%r<;Qz_+7dutqPOX{vxSDwaF?ZQ9~#!?*vM}DqXQp}zLKuDjs_{T<@rdfD(n5bStR0{S+A~LWmu+)BG`FEPOCk4p+ zM;QQ2ytN#7wR*meQL)pLOY4Tc-4FnX7nHq1=+G%bz_Oi`+b`S}oUOJct+!PTK6JK# z>Fg;JvKGd@hkkTza;{MlMwAedp$j_&0ZfT*^aa?S&?E#EyTCGq))4{NHxX`i{w3u4 zOpZ1tU>y=q#%Q5VyHG@Sj0aQL?fX&P`*SO`bWdGFWqn>>wwsNGr%5&+`^!WVWA6Di z_i8dL{Oy$cvDv<8ThIuB3IwbzrH{@66~wlklr|a>B@RW%?IWRVKk@}KVTwz0X1Z^N z*co$0!TG(cRx3jM=%Z&~SBHF;3UN=WchCAA=u`kz`1oU=q5uyj>CYZg%Z|?~T()Q| zy`bu;Q9E`DrWl!v3IV`qBb$>AbgGEis37w&GZSHLK!5{f;Di|#z#wU=U)-GHzvtpz zp8osykQ~SzL!j0+eK>n+^|*$s5I}9v@2EfHZAs;q@wTd^z*4TcF7;6q?cdkA^I0PL zL%gq;&E8mznSEGNi$41}zW12ko(`M=0|11v5zto1hQY<^#Xwsf`9Qa=K08~4{V~k4 z?OQ(~MaJGW>N}2-%H??uc*ktoRyV3aL^h0yyrX9rSZbJ-zMXU3>n%msC=m=otJq^8 z5Cqvx{OU@IbZaA|^wx3g%roMv883Zg!nbyVdQmpd{m0 za6COWWzYWPK3^X8%nHYkmCHUa16a9$Tq`&Ks?zoKdsxtSG?8D#Egcc9Kk@iF!&6XY ze?)d3dXMFS;UzpClOkkf!&ABKPbjxzVifNE{^GDLDb(Al!r*rg>)daZh~AZ9(_k-j zBO-8}=Gt+nF42^G-VSjFCHrr$UkN}Ev0p;{_7xX0Paz=qjWH^x8w}mHhI@WTg_$MT z!oX7u4x~H$>?b8V;3)tgDP{&RLi;O3ZNO6SXnhh9kw~?=MI>e&Eivgw=0g6J9X;9a zs|PoQ>m4D=@ko(6J3m`Ai;q6~XrLi~X3DLV^9@I7^n`}{9_$^~-8k>_o)*|Ii#Za@ zJCNu+71JG4JKuLt@DLK?FyojH%O47Y*<*T*_p>K)4(vIMxB$-uXHX(F$stE6wM6GB z-@h@cVb^r)s3jXAz~v}K{|I7%s$(tkR?5jTWm#r(VVIYycpTd>5~G zB5KbXH}rjE&gzRK=j8jEI;z0tGu7E>5k$#A0zvBpcL)}DhYx?0ss-QVRi{leVL(% zr7j!09{>^^76~PB6gpd2r^b!t_Om^W(cxxW-lMHsjeEN1hPwN?LgKS+wK0YbgTf$c zrqjYF!A-N3bZy6MYle2+*|$DIb@gzsjP%uzeX-}~7ap)u&@szcVfbj%>{GSrNcLI} zv+%te7LqKAi1s>2RD2OQ0V|_ZmLc}kpHLBjOpH)L2uM=K=_hMy0w7SA95sleM@S9; z9W+)Y=&3A3J=dsQty8g{Q?am93%UPf#@~LL`l$X60S^p^!>FJLBB$jb;BWk1eidpaIEp^58H^?YV9lc zYgqWy3_u4*=Fgr(c1rylo|nYMvQxp~A*G8+O#wQyN8uGbLPcc%$b_hwi4-F0Rx=w# z3VNPT-9+hyO}#BiKfjDOa514&;6Ur#i_ z*`5;k*iu-)p~s4QIyXEG)$|eT+0ovKemPD4l-8D9$#_#mG93DJp#Nc`@{883bFaxn zgR1_Tg;hF(YYDUZeIG!>|H%d`HJ!bwuLsdWQX6~G*_Z;bHHrz928iC@>+sQX`)p$G z-5Nkb(W8dv?U>A_38EkFr{B0!aZC?I#(DNMb^R$_L3gS^>a8HXEN!mN?9#JS4xuLp zTJhax!;NYh)ojeBZCm-PQZ#a&OoBd#W;ISvC{BJn7+#gKf8)Ym#z5*Z(!zGk+0t*dANi4*`dN|~H%lHgPm=+HeQ5%rOKzt_1Trqgy^JZ4B_!(y z#^`7#3dnv=I^NY6b;kFkX_Rk5RQ$t__F76`^Va)Pp*>Z!m!~!Wenh!jdVnd6dpd2N zu+o5RTP;kDFXnua0K87$|J99i%bR%cmyf_2TAIdxbon##1CIzz5Q?W1;~0Q-+a^nE zj6pqs6EoIHWbXBFV85^jMG5%YG>vM~Te63ZlFVV4TkWbic@O$BYXpNld)l3i0KkWf z3r1%*+X)uUf|)@z#%Tn`$n6*oM+T$PXGJOutBnem%jIgd+H5y%yOl>%N*QA$u5vbD z-gs1cTiUUTrVojVRb4*EEinLctQ;8zno~jQQ!|Y{HMqRD5Ta@nA+A=do12?#8N(d0 zj2j36ag4W4D2IYh;Z%`l3^g#FoQ9Iwml9AL2BBFTQdnk zcPq(qx$GkJITe3tVf0lWIAO=E35Uqx%E}zXj4@>z)YC_4EmacKAA1puk(8s!mhr!D zwi`)#<5hFE&j{e6gmzE^(IHerL;?NTIahh5U)WHG6az$C>O7)=T|^XzX5m>uh-I-i zwH*Vzxp_qh?|$Z8K+l-Q&)6s>=H}qu>Jhlly77i7V2f%{M4QcKxmd1Nt8KeoE|-n{ zjyn)bwfW6nJJG4_7X$DA@WIgRbaCzaK?u5V-2epvu1`>g$Chf8ct>rwEx3ii#%xTq zC$|_?>a~%0U=R{d^xi51i&zRE7w+?*5}6+WRI@W4NDq-#tr~N% zr~TXW_sRqI*g>jXO(gi)MK5*nPy^LG zhXvDZD7-?BnhnattD-=4W4vQdrEX0zGs(SbSVWJXO>ar20d&CQ;TIF?}Oe*LNS51mg; zDey<(@~ms$_<+PNlVKOH2a&Wf;5>egB_#}z9Vh()#B7LgAZDr*H^vkex)7nFqlWw4 zb1W$b>1R3wQ+{(;@aP{XpTBk(!6h0 z@o{q1%Dc5d$7t~8#f~}+LEg@V3lXFBKO{|i=5t9-)Ix;>01R3@Mi3F$AlcYzWGA(a z-fy%9U`42i3~H&>bqN4Kc6^C+E`2yPVsAeoQtS14v0O?%ya=1__6(-X_nNeSmBsHW z4+D_G(|B-W=kMncDq|NrEfmY6#E$1(=$6wFzaL24L8ThiD8t-tw_u44blbe9rj?iF=_*~oIXjG5*6{*r7k^F~HF^af$_?PG| z7}O(!GmaO83IHaE1Lbt1gaLt67(yKUl0V0yLgXGQm6yj(Mm}lP$oI=~5pJr|{F*oJ zso=j*DW5tU?O_1Wv8qA+7@DPxo7$Pwpwe6dQ#$p!Q*+x?nC?H)W{6GufucwbASrl_ zK7nA<5+vP?dI^1uGyAp?fEm4$U}veqh=3S632~^z_mv2CJt!l??Fr?yc?j+kt43`$ zo5fh&K&l|WoyX#Z-Q)`pPykn4{C5kc<0z1^qy2AithIIo({+Bj{y2VQMe z-4*5Z5}^Vx_XwG4dnOHJKnr{7v?WB`*BXe5;NtRBv$L~Sx1o2`9vi_t4mOob>)kjr(2xOxfp-*?iI%u3v{71$ zN|Iv_XgQ27mkUKUaTHi!+ClIXq}DdEckd2kPj{=P_LxF-`o3J*2|xLP?B;mB@Qq}} z5)T02LV2q}R=w;{6UF|$wPc*tlRhNLNeif=E!Aak-H=B)h_;!}`ar9*Xcw=BX zzclGAjRrRY00IFqIyerL%r6M;7@ zSY8o!O6lF2y^d7F&Y|^k5c2M7c}=P76017@^VYDlY;1p+LyaLiwWhci!k`)G;97zE zS+W{4G-3PPTWEjc-9*40)?8U(^KfI9ckpk+r&fx@o zlYl9L@B;*VYO(Tt6PyN3m>?!-4@Yjd+mUZ0B2zK0(AF6yj;h6`;q{SQvpB6UmrL)D z5e_gyl$>cU9#*FfE5%qi5!u-tL|m`e%jHrQv=p1}x_$0-dphIpla~gdQ(=4Q#yS81 zFaa>gPGgf7yG$#c4NBjwZzt-#JqemkXQm=*wL!$iqCrHh#dF^;vc<rr$^hS-TWI& z17L7hy0`i(J*qVobRLalQ$4vGNix6Mf9kdc=jKTLu75k+K7N?%ibPkvl1Wa-U=TE3)iqNGJ3rIRt5-iGUdelLn*O?n#QE zg{!NV=jZ3(R)?_KQ&6EsM1?TePv z?OGo!oPNqQVsff`X zUHbRlei}6Af6rqna0a9PDRz3ukG;UcPz)L@!}7=vLr1Q z4Uv)~qhBCoOLGM3ZHwJdPjs>$D#w~m19BpVs3XeW(P6yRDI#v$?Q*dkU>%Yxb$soq ziL>FWzOjpYN?lUZowD~~_;wC{16<>nyLNiwi)777VZ>M}F{#~e!gttPg@6)uB-8c{tC3&`_VQiYS zvoodW#np>2zm)&wh>;wz(v_rlZ|Sw~^*GTxIZUvmq#TGknUzVB%I@9z9gv#W!-2+g zRNZcK@*AWg_44ZK-o1Oi0CMEBGRsmh#+1mH=`*P|oQ_xc^zL;tg^VRo(`xs&u5)V( z8DoT?4eIRcA2GhGww3*FKYV`eKc|Z8)M2)&Lorj`a2V0MKs+|}SJD9)lyS+KLT%n| z9bL!%_+LU`N@O$cLg6cuKT1K#A*z#Y+b);OMbk)V4uDw2FMFt*tB}x|0G_aP`&Tri zt)(I$qSh@U>b702R?yig<)a%7w=y7e*yG-njjQU}^Z8xEi_z914ny(>PoO%(ny`jA zHtJCj>M*H+gh~+-64BY&IT5Yb>(HDiGEvyo)+`Trh~>bvU78l2p5{t6E!Z6+^8#Y5 z<1qH%k4f3|GsaoStk>&@4?p4eW9SW-;@(-3183jx62vwOdFcNX#X7ZYoTS@{DJ&;o z=P3uKECLjYwx>iW7*sN|GqWd9<@)Xb;CoR!wQ#dKEPqOE47X?iih-gA(B5cU)&Bg7 z>^FRE*+5ji8wZ{3!hrlfGN?y&eN36frlR8SW`}QxXw;PP3l~zi=f!ZDp+7_+1eAy< z=BEN(H=7M2E|yDM|A@suqOR|jRs<1|(6o$%%)D51L5*@0p8P)}r{)Saw^aMCRKe5C z{YeP4uwF}`T2pP{{u!}QR#1gZf9U;^)UA;8qfmLf!nUSv50Ihh?>LaQSoO-0;9>(mpT{T<%kpk}DuK0KYY zC*4jkQQXrKaH<<8vn_wOG+gI~<2jG=AsJsg`e1>Md&v5>Lxw%gHd1{as6xG^zKIZZ zTK?j_g;yi~l(oZ9-WUbCnh=;qUB&6b^4GH+^G*aC_tZBsktKjfne~yRda?bpugeVD zz$D*}z}p#9cVBqg=Oi0h$A<1wZ-(@$LturD5fG!c-XlB=h?$v5y(b+*`uoiH0toDacQD5Vc1Le zqI0a=ZOT^`r;dt&>F(8LTpa%RuG-^GKrK!mRCSI2fYHwJ~qH*OG;V&<|GN#b^xxq^R32iPAn|f9v=4 zq-na2F76W)yTr_jsNJ?gT^VOppI>KoH}tWP@mhu2RA&It;4bSlbRL2-pM6m~7W>94 zTF)Mmcb`c(B4e^5Y8Fj<(;A~|&Gl;gb5c^fUG2rbWSr9q)af4Z?pC+YopNgZAtXM= zfkYc4PCt-LHwm!VFP`cmWms10*9MeLJ2fFu;ncfDHn9CN1*G-MF+S>Bx4;I4Sqh9E zS$EC=ZrFZwz&Ag{soC!<=Z!AILLqxFyF<|7=LXcOQL#G2sl#GxgdjX z%$#E8#d0aJII`u%ZRoWTatdB9ctD=GOofT}wptn4jVip+APlRe#IzsT2j8}l6p5iD zP^G9+P1|m}lbkbc8Sd}x422%Q>y-J5lp>{w46|y~+1dHatEOOx{TZDNqs-r z_erzZ?zE&lC~L3JJ|5<-e~+p zY%Ed$z~VtB+Z%AsnmdTqYys(&X!*lkDs<{^7L8Jhh}yOt-K~sx-3>E~g|0N$9i+o0 z3o@kh$TU$WE)heSPZpX$)e-Dz`0WV#G%D<3QnQDg6sJ=3$=tn2cyn_@&RH=L;gy@}zvVHR$ z>Va@)D(RmC<1WdkX;fp32KNQQKuNQo#r?jOyE66ia+tB%z!7eb*OFK6hZ4o(6Ux;6 z=#4)@jF#-si?Enr^E7Nz5NWY!v@vbl%A^%9Mk3VOm&p&x^!G_6b^;in9!JW4^Jl-L zrdYrinMDEs-ms?=RomTF^Sl$j8VTz?uFY<*_0pkLzv7G$%Q0ehm8@h>J1xJ*rp&BJ zP05}*9aM-CSk~2k`MW}hRt?lWCZpFH`{uGtYS--P@O|iB)UFSNnC6x1HCJK+c+Nij2991^Ti;=hqG(m|y@jhZD;qxo2Xygm1+s4ZD4 zUtPZf=Mfd$<(o)|xBgt|gHenq(lmTW&*pWk>Y#T3v&F=ohFN=;Xk#RnO;<8t3Wwo- z_U)aZgpt3t>sPDQ$}V0MuV6lTSJ}or4zhVWGF78T|5D+D?1rcLDlGMl>4F~4bNRMB zxZcjngS$1Rcd?t~qI@A_XEMvoZL57?si?MLBymS6{c2m%Ny9O|5s|z6(kPoEE`q0n z#tAXMe#jC@gbHn#HCm zZ#w+Nu?td;M9>kal2bJ~o26V#|97YBH$LrlUQ1>;W?qja)H@!;;@7KI>*aFcuTb*T zwzgzC#$@UVJn~(ad|3WK5QqWLt(_f@h=dB19nh4qs6E-w<0qjuS}AgAwNVD&dc7uh zSX4>@^z{3XXHRb}2oVB+PYrU*lDct>x+}XDOsYqI4dm!&*14sMBtB&6unUqKt67uQ z{)Fw0rr{3l!}Vu7HV1o3NQ@^PRpZnIlZ>WW6H+KxkxDSJkU;eXMk8HI%DP;GJ?4xg zBn(6hC69@68i7{~6#9_ggZz~cPDRhs*_+yG9f2xfy&K+I8)N?7JMZ|)103wRUW?y? zb~73YF+DeTWK@%)SJ$sD@7*IrP`$9PJ9l|&VSEAsH1D+3v8RZXEAsMCIM?@g-1Zt?u6Q5H6OQ4!0DBaF?Ir|*$pCBU<<^F;e#+XI3 zSTxPnna_5rH+M$N1w;(l{##v)j%9Mb^Lth3IOcqU6Y&@wDJOz>YCB<=J&{6)K8$(a zz*A+X0H8;LX1blHRC{R!*kzOQH^HRJeMxF6c7D-V9dOnjFQxM1;lk~V z$y)dlZO>uU{cM!Y5)X=q00gYF-sLHiS{k>(b#g=9^nbTBYEUwc34;#n1lKtVbQ)2o z0i78Q$grQ`?`$6#j2qiR9!b464v+k7wf6rEsMCO#bs=5*tZfmD#^F1TA)dX?|CNxL z8`W$!o7HLsf(;613_KDxeGQ~)RoSiFccE$6|F64+SS(PjUJQ&l>MK&(bfN5r7ob@e z!JV6;Qq}@Ysb;lWsirX|_Baj}kzUB|E~s+GP<_so-Pt7FvV@E2?YCYlNAnn+NQ-qW z)?p7=BTlbqu=744h4$*0=|8Y_;a)E^6D%+6Mn5TDaF*^NRQ4r}>Hb7M4v<6T005p* z4R&xb!YMs{iPM_fQs_=h-u+}Cq<RA^od;` zDE+J8POD5OB;a1+hG;bo#$WV(dz)8Cm)Z^k00Qe!gOA3Th-pG=h1u0_(HkKfaWc!i zmXi`c60)6?AkB!_ZrerUY)@Sx%<*z+5jLvX)|vV-D5XyBWKX#p_r%0tjP>l3zXCD> z6WS!|Tdl29k2@qsL3pW519NV}GUHM|JJ0xp7tb%wiRk_J-#0Ffh9A(9Lp(}4ATpeN zZaz8ILa?*l1l$+a{+S(=?~1ikNltB{Py2(2$ZVLys5H#x@#Dv0CsVpHlo9M{Hm$=@ zmDy^;UiAJdJP0$QY+YVS!v3e^5s0?-1BHl$uwlc-+W+;~vZVj7CwrR8F0-+G@=_QK z>0f>uLp_OtScUauHc+DRVpBJ}If+ zAdj!mc_X(V{;-FLhPhF9E1e7{KXZb!c=O#Y6O`9XL}>=Hru(3?+>72nrV@7}cWgR< z_d>)0S?!1<-8RNFC>gkI&idhl1}8INZb|4pd-xM= zg)v50((qs!{$%Dby=#hM=k9*`Hg9vz?9o;_bL=`fr_j71%Zg$uvu+H?QLxJpyO3qG z2$6vh4I(0~*Xz}4RYTE{9|{xhaOqQ&Jg;S(7&2@k<$mv8+1)3cXcFGv29FH1%SYM& zPIl8x#V^9{pMjdrJ;gA%OqIOy-CdnsY*brYb|GfqQO(c4Y#_o$;~{Bm>))7CpPrC+LKBN&!}4(+)LU6>^Kr*T<}cE&wS z+Uk5^cjj4z{t0|I3aOw^5)9fw%+RjG_L+zzm8~abF){yeYW)tpmKqJ-Faiwf2@r)n zwTVn)f+iom3ta@?xVlpVtVsuDB5R{bO!$lu+tXAiH(^gj3h%kPpQrS_ni0{wOsM__ zJ4swDH?YZ|5y0(FEiadd2n5K%bSlIHE5X+Jq`#cS)3j|{XM0LCNb-+aMsG)MmpwiD z`RcWj0gJJtn9mL@XIVBk-1$h++4~iJY~Sx`h!>%cO?Lo#0y3bT5dv$=TwcGt4sYq@_=zg9IVO+`@kFs!TVQOjl6vy& zX}8S>T7Jl}r&!&UA;BOBfbRwZq}k7>mAisO_gGg$jXwYkr-sV9`A2m+QYM~;K7d+K zo}vPBF@4*DP1B3LON^=3VC3?i?d_NwgBq}N4kpU_Zy^#XYj2tj?BB_0u^IC6^Iu#( zmN8n93Kx*v8}Na^PUi6n>wI+&r!!7FT$-KHI!zptFJMv=?>|J;#;{>(sEf5RFm~%H zlRe-_dP9hu{9#nY7!z(wSwF`E`!k75H8E=I8FPq?j`Ha%Dq&Ok3v+sJsnPsebLV9I zD5{+=`(fOXcQF8h5iW>G`LSe-?kH8X#N;qADvZNDzi;|JOYCW^!I$)+& zirVxr3D1XVW=k=0_U29jLz#OnZb&P$tY)U(gDM z;Vew7_3nuC+%cZ~z>s3tGZ?@?p%4AVwh%Elh}}zo+;as^OQKWmdlSr*LJIqTw`7=L zMkH-aIJL9b6p)T(VOQ?|$pGbgDNUliY){#KnskycPMT?8Ot9}xxoiCMHLk@j8 z7nheWUc8XBD~1hOBkzupEwHn*f0Q{ikIa&YWOwqBH*%X&f|ZBV1~C*a?r}u4Z8bRk zp}p6z3uQl|R~hy+x@)cxM@j*JHb#CaFiI5!+v%a$9ZZvW?t{k(Nf~w5m}f4plO+N4_Li4RegB2`leq{x=953l zk2+ole`l+c1^-I3R6!($nTtsdr}1R0g&sJwZlr3{qeqXT zu-*WmQ2ARrc6J08U$q%~?lV(S&J;Br4_?a;5IPEo^1X28F_IsT0ecw%ursbTCCV!5 z;Yf$e?v`EZuaOHynFX9{K!+jm_YP7qpph%R>XKT2CvTd?sY%mjxjb91*Pi|mYcW#P zPqUQjBmsN`et=*N3SIX+;U9x;{!(Z5N@^l0MN-08Yw_ByuqeqXTWn2v3v@t$w z)O^F^Hl;g7u-R-kp1o44LBwzia5{ZU(jAy8l7|$DbutyYI0}7J>iiU*`$-$52(-O& zf@D=sv4Tisqkbq-P16{oP2jDSEDC!xcS6UJjMR+kozyApJ5P|%A;BIzO;YCtona;f z0~C7i`TAUcCy?>AoElL8d%S`wG}|xh_o_v+h_tQ)aM0a5wxi z8~0Rxu^)v1P0xgXV8SCIoNDN4nbjTuk)m+dN!GZ|IqZ!i zv04ZK5uKx`kSoTd@PZT>m%&RS?3NsBO7iAA&Mjle!cgvtF$UV?9j0j-VWIX@?klBg ztyka3c<}_-?Tnvb^h(~AUf#j5m_HWv{U1owl1Xj5;ldnv!WctJNg!|lU}L=bDVF(y z0k6u_7r$Xi#5c=e$Di+XC+uBWKdWSa_EhE`-=gh=53x}Td}tgF>BBYp%E4U03ZNKL_t)Xe1g2i!Yu^4uiY)~^zso&6oHbE40;ceTLKCp zTQlI2s{A<~s3;i*T|0B{U@NQe?@--q~3iy5tnDD@$+OYVjhc}9SuFyY6_ zhcZ(9uAPcJuhrU-icS}Z)e4h0a-fW2Ph}O=x{HB|h`@|UNGzEsQd6^$f>@V)eexlK zgh~;JYgE$!zeqp`xbN-t;!5_U!$77$ZiXX5{tmujN67&RWbDc57t^hJqmcoE;Q>tAL>_0OJJ- zjke3|dcD3lKVK{sfoqrXQTmyUY8u1rv{i1VD>Ym8}{ zg;J_*%^&~qAOG-&KYa4xlV;H)`wjveMv$MU~>fr1S<3uXMS>?4r>?RM!#{fXiHG0##y7t8 zt#AGPFa7sday+R<8CQe<$Fj9Z44 zHO9ny?f1U-z4!j^y&wF?AN=&apRU*I2M-?n>OcL}Z+`Qezx>T_Zns-G8+_%aE-~GY&fAFO*ed(Y5v;XQJ{=;v5 z;^D*f&CTWIB_WZbR%;hZnzdm7gQoX28JLj9GoqOH&K#1|{Nwmi(U+PhChlUYC*i$B zS*DaaWH#+#5>Qf<#{sSSV#s9GVBV0<5Q~un1-9uUyOb3oB6qAh9K^#2TrvzCry8+U zpF50iA4&FcytQZwANT5iF&&f#so83|pYS&9LzM6({wE?JTU+M9EY(^>_5i5GrWSN| zmZs>jY53siEL+nK%R`zVwL~O+X4D{|HbxPVQXB`daUeKQk=bmE{rPiR3*avCM*^d_ zimVHSTvSBf2yOUw`}>`G@DrEawx{6^K?H0F!7zbMPzw+ds4rf;&?cCEQwNmE<$Eg^ z&Rq;=BO)qg6PY$mLquX3nS#_X!kOtC)mI%@t8Lvz!C4G2NL~GLK0RM!fjtGcK!#=H z3{8k1K6$!WE-x<5l~RBAckg}Y5B}hXKl3_ggs~+a#SPbKwvtp)Njuf@m8a_AHDLqbu~+8 zmae4YlUl{s6I*KekUQ6Qao!N4QUK0=xRNbt#JQH-p$48S;dDj6j|gDgU*){CUauF6 z1rZr%bsEk@M54fvpP8MTROcLam)KJW{h5u?#;fv)hzhvVZnynUG)#(@WbPL=x6bx( zdzb+^z;mCFb)X0vG(nq$J@qo}`w$-mVNVs2Qq5+wnM8kgTY>->#0!R*%@bo0t(96R z)u_jhAK$xw|4;tpPyWMy`29cq(?2!Zw7LZVNznZETW>vm_RJXb>%ac%zw}FAyncE8 z;>C*x58nFy@BaP|zW;-do<40FWi$gbI#L0^8vq5X9`lnYPk!(Be((F=|Nd|O=5PMe z7r&^H9^Ai=4rf>jt>Y9rxjUcrZi`DPAM=8z_WrH=)M)l}!gdi$XTvx08j9mStJvkJ z{O%E0(i>Ds*7MKxzq2AR9Y&{9o7Z~?BDK0DBAa0Z(HJH*_MN}sO(&NM<-Yqj0FYNt z*xww1YWdP$K4KG3vAF)a9#1j|y691aKDt|@8d7SpSZp@i&1Q3XdAZ$g!@DM#fm12> zPecTT$YHus&3e85zrXjr-}O;L^VzrWeoPDD~;g8>2+b}4+r!*QQl z6FZaO3L$c^D|K*(39T!W+iz_@qn-?;FR|BahWd89y}Y<^di*%MDH1BN%VZt3xF3Uf zt5WEf9HfmlVteXH?s7GWeTA3tgykuE-d4)2dYL-*G-64|6R4*zR?S94MBD8qyUF>j zmk7O+`1rB-+5uBU6#q_Aq_okC*~lGP=zL*HnvpXk}7Z{AB#<` z69@{m>l)Ik?(++<`Phpr9sjC++{_qdK`|R1X2M->!+ilwl zk;$HcKt!$E>sQyG|H9|rdh0EJ`+xdR|54lQP&m?biJ3Q>4FD__i|5au|6l+1-+t!Z z&-{ab@by>M*URN{iwFSP7!+sBTU;`tEX+G&UA<<4*d_GJ7pOg?Rig1>_SE(vH^!_F za?i%D`<)OCPr3L=tmWw3JE2U4O7~;CUmFQk*rC{@cYWWffw5iCmCIcYZe~FzVIQI9 z@ep=}V-e4KPb6i=<@r5F=eyt6C-7cJD$vUZH$~7cMjtL zU|Kcx&?%kos?4b!9HZRtxq5^wDIx+vNT5dVS=RK^7bHX`QSLLdSl;-9BNSAGVceNp z?r_+bTW!p}5k(4vg$e+ctJPor<&S>+#%qRO5s}$UMGYP< z12}aU!VXjz4toHbw>eGHJ1j)@g#RY=>4}T=V0r|ye-uZY;N?mO)79MztVP2~<9SFo36te2PTjT}YlmHR=OxQ3w(yl45u7Vdmi*PLEOErZcl0)??JYEB~R-&d+}K z{`)`q$xnj}W9z~!KsElaeV;q=Ya}M0im@z}!=b7)0c2hD z>3B->X#9`S@}tBkXA`20Xs-eQqTNwp(U}|WT-HXCH2cW_e{*WvZMi?0N9Y1SjeBp{ zSJaM^Ub~)`iEV@u>OCsp$i))MgpI>;He<=~N&@qYR}%oH|(U9R|^$eEw}Gt~l0 zs}AjW7Kse);++xXpr`DW&>v>mEPViwcR8GHKVFa7HcX*l$4uVkK*=&GIiz&`*p7%k#%*Qv32bVfwDzYeCB3= z|MHWc{N!){*0AC3$bkT845RzX2!sHaXO~O0(57v5ivVnl-nK>?lqd?~H=qxu7r#UMUpNlB2reNSOe>JewYL!|)bK*-Kw7!iR;y7m0Ui@*5M4>d4Yg%NIBz23AJ z%OwC9Kw@ZAg8*Op!WUP~(y+R2wL-etZeP8+*4wsOEX4>I0hWse6161pN*I6nW?#84euz_Dvug$9488;8@gSYc+R8e>{;2QcbE2Z@X(hcuGdkZCt0 zwP`mvbFOS0{Ot9o^dQzq=5-VAS)U@atWv?-hVDJPOCdNq-zn-!N|~l_>w#A7bc{%eh51IoeW1RwSr5ee3O%K|BBN zU;ovIPoA)RbOvJ#Fszyek!+|jLNw+M1S;!QxN5Q2x!2|K=b4lYfL{V@jp^S=;yVbp0t3Q@3!u9jQ45 zh16;-A!g?8(}6O^gl@lTvsWf>D_h+g>U3B_r_3?sdEcn=&OgcKqi=VmrAX~ZJ2TMu z{5jNfEPF)l=l~%ZVTa-r1189_n_WT?ap3%U44HDSQTC%0q@OQpAAB;mG2R|BzC&|& zWh^`;LalpCJQgyffeY$;A~w+j8W4}6{lM}%3uE$B@yA>|tIdg_(O>=bU;pC6C+BA? z)`k#Qi-lS>%%EG%hS>lbzPWyNhRfArxxQJSov$u0F1EUT_R&XM4GjSyC_)5YE*Hy1 zqZAot1TdOG6!pyaF3($p>+Pmt)wY@$2)ZC&-}vS?5Mi@fBN9*QXZ=P>f=Hy8*=Xi) zCo{%+779Cf)IQEGdpw{dtDB=U)5@Lbto`+pXm2-uaA&1JgIMQ>M2)1?Q0sMszDXzFe(o@Kgpj;Sf}cw*8B`GCbDDM?&^g zUZ0qve)!>si$$YI=~g#@ONR5s0vL=^#&EmcZf>>=tcaQh5e*_VN*ORWuWrDq2pEBv zjam>d8(1xyrfCRK5fKp(0wNH$2tWPs17nOBn|Na>!{%@P=5N0K^{+LHg`DgK-9~~$ z`2fASVR#G6aJ4Z;eIVqTCU!diM4dqe$jl~4fFyv%=uG07<4Q`5^*o<|tRaETUz$%b zU{!xXnn#Di?Q9s?nLBH}_eOS$Ha!9o?9S2!F@-|V(~^`@m;ylPrIfAHjpS0zu1;4% zu=RNFk^N+kxQ2kSeXft0B&yDx9{Kk`j@}*@cx7Q_wTjZM#yL251u^v;KN^h z>s#MAKVKS@?8flH2Om6q>)zRN_167+m*=Yom*?lJMWg8aY$fg{Bm^BGBfeU1)~5aW z2Olhus9ChFMp7+krPTe)%U}Jczbeo{g9e)xwXZ8fC?$_5P2y8>blR(Y&Q4}Ri8YZZ zmTVj@^rbO&6gd&a&b@p0#7_6v9eE=MN$Ri<0Nc&BP<&Tyf!la+{)v&|a2)_?^$b{rK|y z?Be`vv04H$0~>@-eDbY#KK-eC_b)f?29-jvp7?J+dUUp2zIFfJ#d4w0oS!W&&(AK; z&z?Vj4vYjO3D229F(}51<=J+#U7J=RF~j-U*%!X>OaJ}9{Fe{kdI*6jm2(`6yIP>Q zUenKAXPgP7=epo| z)7$M=oyfrw02I+c7z#qvLM56#`MHvdyOb9s>nWv_B2tLB-R>6|-9Ure$ng$sSFTdxj? zMt3dMobAc@8j3Yj=kVxfARAUnp$k56cq^~HaPW&~PnU~E0kYPn)eJ@x8v;PUg)(fe zuU@{qxn7-JkV3&P2H~q;|N7Z#`PRM5)j}-^8wAj;G5W=e7tn>>LBq^I2&7Ogo2AyQ z+s&_h^H=_d|M7qNJS)^)w1|cs-Ey}|PFH^qybjvoALd`Ay zH$-CQX3-dJd?ZxN7yxIaXiTyff=o43S-|S4(>+@`fG<&v%I^KewrT=Py^&*yKlY%5 zInkd00J_9c-Jdup<93$Q9O-1cUPve;MDgxWN-0H%D8UD!GE=oxRXohbVG0bNKj%pV{Db=E z8_v~(&TZc=rbPbq`nk`2?)zk|c8oCwKqDALP1vH*$joNbZm+Jd&o3^YK7ao3;U~aV z_0fyx4?pqwkFKv5hz;^m(dEU(^6c!Bmk+M5US7SpTHo9#Y7{BesLR#z;>Edsf2+6K z|LuSIU;pcGe|vRt@rx%P8s?^H{_M~GtZACBeeG-8?Nx5>h_BwM48oLIoVa{FLfsEu zhxa~u^yn{t^p_9r-}e`@#fGceuRjWvb<9fG=n`P{%M>%qT_KPS@hj${%rF~?7HPQ7 z%9re9u}kI7$Han7*a-xle0{`^)!gTp%oroPvtZc?B!!;QEn`}gF-BIyNp>?ay^wIs zM$w|(ZWYnPhY#&7Nq-bswCYG=LF9QGp3=d4I+!)(Vs4Bkm#Zm=sOCkvM!Gi}%kugRy+-x_XNUc_Y z+=4b>Uanu=tZz2k4IpXe%X{}Ug9I*RfN%WrFMsy4pMCY}nzcdQ>h1Q~N6(%=zXE{O zV)^*p&#*Q(uhzf#;DgKM>VpqH_{A@tFvGw8*Z=$f&)%CgNs?V>V&|NDeTyyE%B;$) zt-GpMER80b4T=Ovp)n+kBLW&Nd>M%{(->{^!e>26-HqO>tE#*9B`f#Xe7pA^JzRHB&&Y_Zh|Gwptm^y>sE&&AaKG+;mvhhg&UY?c zd`|j)oF;)EXk#js%3E)}Jv}{r@#1rXobQkL;e4skV`eckVV0(LWN~2EDIC&!Dg+S@ znL9cj2hlMeAud(4+TYmTupHRNf_9o{9Y|q(np^q}Ez%;42*bz#5K^2b zfA^pN?&oj+e0h2Khd=zUvps#*&fV{>Uq2csyfW4&_K3lyKOX^hws|5elC;<5^JuWw zXLEp*i?Xk}h+Q&s5&E=xCI=$|mE)&R#2)?_!@zcxKDwtPzT(FvrHMpZDI)THf6y^P z%H2P~Je6>S+Z-2}XN*xzL^RB@MEN@&n2#6Z%-rWZyNGSIL&T!v?(?g@&h34A=vek` z{p1V_ku0)>qd2cLXcv-Wq5eT&HiE=gUVeFYX6EzDUv#&dGqt+$99a#x%kzu#sWFL4 z4fA%h*=RIUZCuYILY+U;TD?9yKTn8`kd(2~kr0^#3MqtRlr~zmnyqVJeDUH-FaQ4U z|2_(#3ocTbo;d`Imo1NNK8!*(vI1*g)AsHlX|T-6kKPfas_*+TdW~7>*y2 zcnSc>hpzubR$Iz&CM=a#Jw0l}XKzWNB6A-GCs*}MXnO;%L!wYlwWalr@iajjoma;s z+RgYx!Xdb)y-HH~N`$~@%kmX+r{x~lcQG}kGYmqe24{t(j1fddWPi|ti?}1fJgRNF zUrRW(-2DNk23003GVQU;$cxZC!FnBokv{!+JkTsW6_zUC*So=WlBGVjIg+I)5 z^n@TbD2Nb|h}PED2JL_kDS$evpbVC{r-RCm_Ym;M@}LLaBLdifKC`X1%gmnZoI7{^ z_kQq$|Km^oM7Z8Jzy0f?QQJzQnT0uJ3;c`KJg-)(BjG>)-QOi4 zjMkY9qyYdl#xNVB4Kt%*1o+uc|A`TP@WUUz{`%`u2msU!%x1bV^I&zg)9(E5fAS|c zZ`>duY|@F9iMPJ49VYRPNA(!vM-h}LN^%1L8y?jJKQKy?c4K?XrBQEJa27(?jg>J* z2uf16bmo!nZ#M~w;Biap9Ahl7Q;jV(vKP4%gvu%BcBU#V#Ql}tsXWP-r1-~(%!6o5 zq5s_%J&OIf!_OXXbA|%q1Z%!O0vybpar7}3H{$qpPX$rJ#%M#<2l*se1DPA2zz$2= zBZCS6g>?ThEwwci@MINk_fPQ2-+KZRJk9Wk4R=F~)+2 zo2O-O1n$mb?RPNQf89BZkO++)-QzgOc{hPUSm4xD` zHAm0@03ZNKL_t)`FTd!yPOLQmAh5|S)WQ%PkYGKSdpc5w8_A`?00fgaOh&qap_2i6 zGTiM4&-3Cqc3n4A;ZE?nG*Cv(&>v|T!@-%?gDO{*Gq8>SH^m%b_%8zRzrARP=3k?K2SlLq~WA0D?KPh|fg*XZ|Qdvi06qJQtZ{f3DIr48t@@RI1`AtX8Yv{qA>-(OR2SYi+b<2F#dzVHmoO%V?d`TBXYJ ztArqAkWwPU?A+X~JGVDBx1PKBoFzP1JLJ#)^xysb7e8;*YSn5r3d4l?)cm|hq}T{4 zQ*Eqpa6v8^_4$lGSVu9Za^a_moV~cp2(nTmL_13U9s~J=UZg)BMo;u8qp(NEcy|vy z)jt0=(dT=FmgOUgU60=q!vH`lRTwdInvqZfWqva z*eXBLX~)(Eo8u^mdSze7b%A4yh!HaPGl4+EMUpexKt&;<22i}pyu#aXx+$YzB=#T< zmLe?g5~Pr}wSs7Ver{!DC641mZX^0`K#K?0=br`+buN9{@6Pw`zlS;h$h+0jY3whh zbX><>xp)5${)hkZ*=L_!TU~87n@JpNr88AXfFw;236M~LKtLw6zGnf|a`!7;uE?l@kNO2th^v$1KyLMd=Ng~uL5W=t3S8v_=Ws-jF z>t7R6vexx_z13=Ewkc-2k-g7id=u@6yVa4l{-d8CM)JmuQUb(MCbBL^h)e*Ww33dK zxiANaX7s=EIQr)SxTjeo^W?3vABB*L5P$wX0-`P6QG|&~j;kJba`}J#`T9`y_*xIy zV13O252*j~gXg_Lr|)+Xesm$@Lt;=n4FQajsBxfapRE~+1cZWFM^WfFHU&KMpJIt z?&p&U^O0}nBl%9a?Pu~wl8@yC--;;my_CpXdGo6Wod$ROd$a`+U}pk=&`uk6V!_GJ zAvttuj2H~Vcahm1^1jbvj#4)i2_Q5VeSoSJ3jV7>5r$-Zg+3b@S1Xb&x;Vn}u8k6G!x<3DN5&dT6W!EwGB z%cRFr&k(L@X`u>Z2pA3JiyaRN=oKucet{-^46(2r6;lV@S9%`@E^z-jK-x;E6{^~w z?o6MzOy~BayKbJJ2K@E$A6~sP@_cTG70je%W@G+IHt}ml^?aNxye%!YGWC2XGjC+- zV*Kj-Qk=$mOX{`Mn$o8IeT2emZc;P`DnY$fVgNUXwNYvs!!!$pQ?u#^-Y)JL!5GYb+e?FC0QBcMY5>7 zG-6r^YLuFWQdqn$ZaCs*Vp%do1pqYlJhyWX)&swuhM#XA`(u0`cr6GgjK;qY^heg< z@L7$CBa21AHZFs_?x59=9Fi}fX_->CcFXfSLuQn(-0#q)%>bwEH>)2YlI`{NmrHBU znK=;UjV3Der2{(_s;jV#e=?`tseHw+)KM=oIrNV!Dm-I+uGj0(mrL}tlOPseYt-I& z;}`U+za#0Ss46x>h}4d0*$L@-wgFeWKX{ei*BG5KpeqwO$4XsMScH)Gb8)z605UFI zyMDg55Ckzc8u3V34v5pP$q!%;xFrBJZK%+b5DH6to+Es>Q@}A(gWJB2L?Rgc?)RGH zOQC&jLXZz=b;`Ut{7fRJ|MDL*C5YVve6L1qkV=%Ej)5g5f$HKi-L&&mEL*!rv;Av0 znEMbz=8rG|W52V#R1Og!AcF_&vDg*RP$?&f8$0XWNJ`Jg>>@yC7Jw|ofH~k2Bb(S5 z8Fko0Vio>AAg1^FZcApqrKf*%WR70PRblzCl#Zp4PS(W=yXXipXnD*kKBb6$oJi4Q zszG7)H;vF+*NNSA`yfeN=N^X|+R7&>!hxEqEV*TYi8Bt?V0G6a3nuoc`ffo}FyI;T zCzB8hK+l?B7-G+%6>`;_=$id#o3gI;sPOW~oDy6n7N}fQRK(3?)zT;zpYt^ewQk+u zaj{Ji;)$i27*^v?Ki5_1yi%V1q8%k6U$QTnN-bz;T%1XwCj1}^qFOoeim;&?ol_!Od>zCN0i>YyU#^0?xt=qmO z8g!m{HJ)ME^7UvR*VLB*1r{ejjbUb{fG&+@5K~Vs9HoE%r)xr#Qh9S6D;r7~LSm7^^iGYLeR>NK1NvXZA!2V0D^YZPo zeHdlEAVkgtVslg59a>G+RUNj~A!qjRvTU*iQvNylU1ahr3oxV;Ess9;94BtWdr*nL zEfiAk@GltCWqBwuqe#`uIM+{mn79#47#!#15YJ?%wK~Oux)o1h5Wrpa@ot*E?T@f$ ziT(UnoKPYeA_|K`7ry=+1;Fo$qY$+qbWOO{8<~YVIMT2S7SXb`SQqZ_y>}-I{r(*{ ziGDYXL3+&Wvn2+}r}|NmFZ=L)+twLJfdt_S^y7vMqh1}is0{#z2f7}I9c(D=*XtI{NhfVn{*B)Dd#Z1$7qYtO!Bvk)c#pY>>%40W)QGL4IT-E27rN9OFcYQDm!6h!yXtyMPE(&4k4ufxUl zU2th=bMr#;d+HuD&t%;JKtLCC`X|QC7}KR+jjD?n$*x1hTNp5;ehJ4@)xtlIVuV&% z@j7~(W+M`V&(5+CF7y|d99aM0q5z~w`qoGC*2 z0@@>~A*wK(Un{JoJvq2tN?z=_duV@fWeCP0Cc)}$H(c0Hp>hZ{nqoVICVCAdf*z@tF)$n|_@TC6;| z=bkW40gws60_nUz{7yMWAs3-vW#Z5Xx$<7&rnWSze-=9~5PZ+K%C%%ou|+6s8U`m`G-46FUZcakbN74~ zS$7Sie-X=}nlvo<%~wT|0DBmGf=9hbciGQ!yd)*tS{EElG|I#_D)0$%R5IAk(k@W= zZRGeli-?%>2i;v&_X{puKt0b^Z6S2f3>b<&(!-3N03WuE#iv{IH!4W~%k{9PcB&K+!Vk&{)Hy^j0r#CNOaHaKtLXqkHymhWLpIcEAB#Zj7 zcnYR#7tvKA^{_QS_jKOgx3Dn9@`}L$8t+(9VP65moVklkVA;{ggoyh>Tm(NSJN-aG zXJm@$mXJ8zpv1|qv?%(NtG@og)crE$h!!hrhrBXkb<3gmNibeHFjyOfAmQUUA`$lr za!3cE#E4-!!qax#m;Sgc>EzS9L^Zl{by+!L;j#bfWWpMj#Qsdmm!8F51ZrhIS|+LY zFZ46t%8D?(O)>6V*VeyF@C4d<+M0tVjVf~vmtPkZYHFJx!Ib1t^rGo>z8Qalnl&SmI^7<>cg68Vdp9mrG$BGrLdnjltNbqL{O(>hui|pGFGrFJ z;_11z)Q|BVzFRrxR`m&dy(h6*u<-Z86}Jei#@x!wvcWs>$DFY!9)VeY)JS^#0;U}G zL2wMz8-I|6Bx7oe(7#CHq(O*RuvbF?KvFXsmKseA3XPo2&$2W$&=57Ys&bA*T7+aj z2}X!fMf9B7{orAU(i32ZYG4UBg;lxzu+oxgl_LrLtw8x>)vPeX?&CA0LjdGztPd_t zRFB`}&PwU<^^W3MU>H!9B&Z3G(9#1-Q4D`QP!i_jbGg_a{m3`k?^*c)o8A9kxW7S4 zaT2|fE>&opVE<-qYP^N&w*W!DRjpX)+v(SMo;(RzsS1~Gx~)Z@ z0Q`U5dbOaDu_Dl`pLMzQ4cPa=-&UkP;^7?A{m}_`?S)__;m^@*@Z=S%MD=Yl<^h$v zS}da2#QugU$i%NLWuc8J=zYkKa3Ba8O7uw$p-99Ac@f8)w|E68-ZUwd>(nZHcL*sY zi_h{G?C$X}hX+G7>Wp8Y@K6B)x-nh=eiyFug!fkF^sjmz^>FVuIn(H4nMdgrBFzW% z;$%0jzcmz%U<7+lG~W%pZ-FvzOY}JG?Ci33Zae7YH5-@Ug=*zVH~{|i(U}=t*~Az* zkjL?=vNpMbdB1@G9zYgn&JEq1W&mRMX@1_oRMc%BF&RoNRJqdOb#xs~O_0M>V1~D} z1pii*INQ&S9eb=$?sB*Ay?89cL9wO=#0mm&+Y8Sj;BNVzRdNQhm&jre*q>=Xa-}>y5$(7%k z>VC&iU~J`GJ{)NCao=>Vy`ER5KGm$Q50^2#`LDcawU2=bG^Rf-!$J;%l>4Z^6244X zJNQ5==518$OECD7Bp)u=|2`5pogKlKLwEE1`n!}j&qEFuugGgBI`*YE5h=i-?(X%Y zB@aPCK}$=^dM!;d7>}?PNgQ~}k-}Xp6Im!d3WGToZ#2D|2fhc+xfLW{i!-WF$TMJ3 zVmV(uwP%Yr`5!Injw8R&mL0EnactEJB0SQ(9ia_iN~-aR>o~>?I`?E6@q&7_b>Sf; zfZ}(xIX(3i-MJvk4PNx$5a^!AhZTik6&%(SH>25mR*de7sD#JZaK7|43fk5VMu5abH%5X)h$mU5m$Vw@$ks1yYV+6CA zLXY4`{h?x}I*VF74EA=m-z9VO>Z+O=BME7kahzk_g03zurkNkRw#L3l!lP!Em0=A< zzQr9UxFlx$fim2cWF2Jhc=toliF2d{MKjaV_d;?}?GaEg4hcn{h!=`JCtAsStA=eb$))0EIRQIB z4SSksB*Wo%el80UZL4#~Lu5Q7YsFgXG>o?S;Tc|vS1B=z#={P*uK%EmvrHN05=996 zC}LUqyy)I57q>&InpTZgyEP+6P3PQ)eayEde8=L&x$hQ#=nw#$X`opw1)dwxLP#G$ zJCcR@GX`54&k$4W%1Tsjgx=q|LE9`XQjU5m^x3kp2Xe;BaG(d<|GsNV(t`MJ%;XOPH&k2TS4nBxPJH~wfXH)#5sYm+{k&lTFmStpo@FN zV##%xf{jWrQj2th3O7Q&H_w#e;)CB(k!$d{WvF!(E z6vX1*?khR@EaGRc(+ywTN#cMw-nmXY+*m|?<5wfRY!#oM!VNKgHruIrsA?5YlK@T! zb8l4uXH94eR~sbTGaFcwKW`ah{^ePH{>0^*h<6^IT6`NcCMMT~Qyj>Hce@N}c#i@g z77`RpPX0-l@K3O^czUA~(Jz|Z#)Q{JA6oxhkkk;@>ezT+jj?E&xV2kC@-O|vhnS5o z+l_JnPMu+%duEIM&r&0t+fUB(?c#IoF+Xdm;Yv`hG@XZrlSJPAZIp|b$$o>#{vjMW zd8MFF@xV&5@eHY(zAcRny@P24ANl6&tb6fFOcE+c?vK)Bnr=LI91$u$2_u8BEIqXa zKO^p+v>o)AFE2>A8HlOJlYqaS-wg!?Tx8OYvl z<~QsB(w}9v&rdO>6ryb1$g`^DOg%bxv`zN%Sg9_1WGb}NA9Ps%ByHa=f53iUt6hVI zb#*au{O_vH1Uy(Ytv|H6fG`XoM5v#@qdkmYx$h{ETk}&vokrii!{m$b2&PN^8ytvJ-7MQHs;L`K5j8`nttE7m8rrM;s?%M(Z#x5mfW zhc7&AxaH*X5v5uH`s;ZU3D^tIhvw#lfY1A%P&8yjXWV~@iFz(({p+9*6evRsd$Zd6 zJSdIU`OXb?6u6Z`zeOzAY3jkT3A)zKhSAXA9+;Nk^^ey=4?Mw&2gNY$CrM+; z%;3~1gPWpe+^q?83^2ZGQf*%TM2{g>yOxmi{=y z9hYLHzQeTIC>5EaG{ai$^*GCAjTn5|t@-H0lCvV$SWR2!|K_W4a%%WR)^P^u1=!jD zVZfhepoS;SWBQ2?mpmYWU&Q11knZ8vGdkMg26iqM`IXt>h`C{Oxk;6NP)=)&I?#wM zSv|`>%(%Td$rcvvjE*x0SE+UVl&{Bxo=-+i>d_kTrzz${L@TySH9IQy4FgX6r^qr{ zYNTMc9gld_v6jC}7?+X8g6M7*oy#W#(;|;fb*A%BZY5ntxN@|P_A;W!z4B`a7|Yo{ zQqni)mvK1_&f0~9&6!~$6L~sOjrb%nIqMS z${aw4)^BAbd8@FG-Y2cdtpTMe@h$dD5JNAAmu8hK zgdtQl_dAbm>UqZ+o!OCUeD94fRGk5fxR8je{c>^~=|^la2cn?lOjz5?*O*7R+PS;m zAx$tVp`m@qu8!UR^nrrMu<3nxUjlAFYoV6qsDf^A3lnR!oEaJ-h(4w}b;0H%Mt-;Z zh%+iESNE*;>!gi>#rpai+6`i9YF}3{?2z=r3;l0qMr`;C{7kHG%6)FecCgw^CQiEc zoWZC+b*UBSLp;}*mcQ0$xjem?SrEK`+-W;^_!xOEg)r~^rfvdi+6*1OMrJWoVgaOo z7L7x=`9O&ptt+};H4XS-X(?-mlGaixyTGpM^bdix zm;akUMqZOBzcAhd*DvepXF9|QK;P{JD>h~Ep6s?~8Dt*|{Hp7wu`xZ!%h^#K)uh>V zZy7la|IIc&J)QWsH8))USv7V@+-a7bOm(r!bF>CB@&(BNKE(!!9yJ_z#cZJB$YPTO zs`=p^!xfJRJy?$EEVSnBZfP-ZZh|80$bNn#brsdM%r~*Z|3o z<4>g6zrf0LJPK<{U0)ZjEoktgq?(*eT?(}(6mAl+#D;wvX+j&bqj`Qx-@Z@6AM8PC z$XIFYyvS_yf4W|9u_{m6@9vf@guIR0y1k1&DAwQ-qQbAKa_lRRZncV4ygvx z*VMq9v-*cJml`|HhB-3bFIGI(MBttf4)Os8rh%1r*1Biy-%dyvQAtUte}e~zI2>pO zm~xE!y#<#rexp*;r;-oOpVrM{Jn~9@Z-10pxlX z+rD}H1D>^J&TdF>vdqXog6_Nq+Y-eCWGRvr?D->UO}vg&1| z(n3M4eLsd<3%dbtKnnan%~!a*{Xp*tA}&vDI14Kbtiu~&(G$Q=`4cJG>CZEBer~qN z%=(2o?J4cbjqT#Fo3-c9X~0&8Sw{4gs?7Tb*v?X0U<6j%LD5&=ZB^HTf7pyn$!p-B z$O2ZZr~pP50$iZBpppt<&#cu)GltK^h8poYU>tE%jtk`4>IADOtG>UhdcpypZDc?6 zdSoJ9Xsbx#e0Xc5SR)%r@Hw{YJ+4waRnH1J8hM1AQfo(4&n%)^yg%r2#^Yr`L&Z@| zA_0J@r5R|Vu?AJULh#J6LdR5+&96P(KPxj3Ur>b2rg&fE_zC<(RQ^&wHoD#A-c@Pg zEEpvi4;SP9HUaT`gP&eISDf=Y+It%fUF-g}QLh!JK6KiWW@E6MVv4kk{U-IizU~06 zJB!QXvfwZogca4QI4dj^7(p>`_T-%%TXo*dDV;hl@heUe8y?>IR%i zZWR6R8#U+gQBU@h#v2+16DL>H(}~`hwEf#-$^O?-g^c8%b9O6hEyWSb>!vyG=JhP}dUVyAAC<4&A>D^qPYyYnWAV%31`}8hP zME(cwW2<#l_WbIhK@qtn+HBKt`>`}*_aLscdeqV*Efrsr7=CotYr81-0@5(^b`us7 zS)#3)vGZ*1;g5X#mqNR@ms`2V5-9dv4j`gZ~9Lw934 zBvMK7?rr8G2d8e=o!&?b&299BWN%oH#c%wWURSHSkvE!84rN*v^^ftx<34|U&OLn` zn3hyXZ53l8WBQ%@lj{})O`qXhRhY>GVQn=p6hfzerV=M zuF4Q^_VnNKYu5}uGe`0$s-x%~8US-LllzDLp^I@q zoVpto2OEt7s_ZId-eABdiwAqv4ka=SlwV`p`IjyCl1nvBf;UpnO~0QX60F--qWYXS z@BP*S^-R(STptI5d0OzZ$XZV+2<870tUXtEXomW|n;H0{?QiW=|LqZawi&Y<6$ zjQgrw#K1|t(^TS4jf6IWK}7R}P1W;GG|@}cIz|Bq%1=}Pk?4SKe~dLWhP!HAD#^Oi z48+A8hyu*|a=&FCJ~QKd-f%3gYj3~mdo>4vT^EkOQ_~o4s+>AVvM*-WEd>GrFSQuIqye$$i43S2fgt7U&R!4>3Z;<(HaY-@ z!VW%NJ?rg3gZ_PxEMDya1VRNm7=RVy(HX2>srx;qSGC`INVWJ~{M-=T8O!jyn$chN zy;*7XIDW&P8Er{n=4Tz2`q)wL-+uim4yqg`Bk0qP( zkFrF+A3aG7lUU~Q;UPkjAcciWsRVAhEvK;Q@ijN=CG#2&K$<$Ch>dt*D<@pg{W5nl zF#>0-%Y-i1m4-rG=(v3$7$kKkQC&lr(CzUPc}ImfeNTJ+t8un5Vs0ur^3x7c-ens? zH-_{rvlQ>H`Na!jY_@_#s11Be+W&cqmsB$zh(umH-H#G9B$X3BIr>R{>vk+&vgQ|E zIlfu-+*oeDK=*cAU~1`vE79fev!xwXp7DI_%&;4u7(k2FAVF&1wlkp>wB;M)^-y3zTVZrXOC zo(8J&y-2@cQTOq?y5QG^l!FV)w8#hIV%I%7kn;Vf@=g-1f<=l6Hfj^@adWaeK-6KN|T1vH?I&TwP#!WoWI80#zMfjTNWR$4@DiGjh$qN7~aZt@i zNdRz2(p?ULXx>soPc^9`e_psS$hME`#QxQiS3dLYEnbs9LyK}_uiv~xlebq1rolyp zuJ^y05imoKiPoX>fcMA12t(PopX-L7Fq5TjA{r0J*cI5pDaY4GNuz5$)bA)~Kv*O? z88E9j60c{W#hk&$o8YNcMLIS#yfwu)`ucta{ihRgLt=m|>p3=6^`CSA&YMJ{(T=!b zP0X2JFT*DLcNwG_{fHUCQL*gr6!JX^^GJ=a>Q03`CIW z1Ob(4*m_vO9=J~y3iB%rJ4xA*Y&D7)2U(IvNec8qf|X25M+D3U*g?SP$k^2U>JPZ6 z0jCk3g0|!BQP+a zpQ7~cdxT1*?o(J32ZvtL8o& z&0ZcJPL>v~-Tx(gx_f@KBP-DwRZu)~B$H21Pyc{~|6giwzT-r?eKo*}N~lD5=+a<~ z=G9S^_R8kfx1Ulwd`rxjJ0l?g zeciVqTSJ2H#Cuert7>_Cbnt?LiJ6{LKJ{-rRAVm!bQNBsX3nx^x7jg>fa!6ZJ{|}; zKCJp*LH+NF{r8nHiH}@Z6+(FyDS8y-ioTvIk?eB9=YhNP_KfnWh&>ZUVx0W*0WP)Px z&=?H8IpTgtMW-5$yX3pKG}Jux->hPNSdw~P7>A6)l98Xq<++VaeZA{`a_2)KePYc0 z*#I3%*ZKM61(*2Eg@+%*8xbVAr=U4+3$NGHZ2VBAr<;aDVz~Vhz@sEhD8F(>QQdt0 z2VCd1bG%rXq>h`hp#6PB#ie-1caB(^e6I;nF*!{D8ONW7o z{`xx@!D-Qn_&v{Z7fV}R&Uvnm3CA(Ke~9H)!4N`RT)6zlGQ%OrSIgXz;RPcl?C|B( z@T+3wvPu5vDSqXGM6uLvPzE%;|6ZY12Nc$I5f8-nd46h#mJ=Rli$RMZ<*q&43hMdq z3-?w&G;-?(CZhgbg?*s&KmCP25Xx`7W^iLCAY&bX4Xc~vrM0(n=*3Kh6`|pfEKFI* z7EPx(LS4%CMGACjo|D&Z$6(`C%T0cd7v=h{8Hlklsw7F5#^U%PYBBhh@Qx35Iv&%&UF=-S?zb6!pV5=j`*3Z!^s08>`Y|Rq*2V;NxI_YnW`VE zy~pKKmr6~)XzzKeBEnuM^Xf8^$t`?fc`EJ~;PZ}5+_=5*)~vH9sM^Dr^XI#Up~Fw5 zW}Jo9XTF6=y5p;usl6B^07cEqz2a=-;fo6{&onWqRU;07&w?rQj|gTIx*Yb^MFNYL z5t$mn-rE7G-5s>`9)vWZaZJ5%4t+__=VXQA_^Y{&rIfN$1(dLvlSWh#wzkgp!PiJ|l-jKG0ly|6%sBjti z98VT!12xImwwg`S3eu^muOKsSM(E{@+0E*azbKf2k>~)>x(uK9&PKT=cVkkR%Xi$F z#pyyqE(f8t`&;PTc((uXRx&bx(Zs|6S*ao%vo8EH@y24z8<&xrdW=k#xQonJ!W6ro zSNqbfgLaTfXz`W>)$(25sz$!Y!f;8=qcr1MJN*r9@VzU^ay#qmjZzdToMM~ir9-}- zD$rtksz33aou)qoRAuq11K(52Ed-@FIXX0s3k5*^Yo8r1r8-X9oIUHJFqJj{^zntB zSD)2rK3na}@$408Os~X>oQ`zzgpH8c`EKIX>Zdf5Q&#L~)R7N<>W|7QOaOY7^q=H| zQnf4^gFg=mk#rMq&$+Rc52cccX!SXH@VdJcm+F;%F`jda(#!TaKchxUbGg=_KhTrz zz4d%j{|;|)WScfq=>&n_Pe4#8&;c0f+o{d#7dzQ|AGdq_%#O5&)5EJb4tr>q$S8;+ z(Ss+DU*}WifoZS#+>6IWp$_-2j)L|Az5^5BnF9e@`IwWj@$tv-=ZAd$D0Fk7L!`;6 zaL2^Qxo$=eBO)TU0&T{P)-qxPs0QC@o^<{I(<2fNG_`4LrSrX|8b$uk<^ghPhVn z79yKP1R+>yX=%v&8!2uv1jfuuOoYN1VD=@p-W)`VZjup3yPa*)OeZwP-%~;wHsW`* zx-zQ1doR-t$?j;Z^|~LE;^gEMf9}2*H*Pb4>4;bT1oAL)AMhhX(6w-uc>)$}u>joI z=^NvEL{6Ruc_-Wr9|Mv38N&m36xU3mOV#yOhs~E*M~B~KNY#kYlezo_HP8D-MkqDjGHm_4-2Hh{5BtZFgqEMr#T;x<_Cx;X zE+A$1by0-<@d}3OZi|wR(yu62>@oTLi&C@S2M1TPOCy%gG9g-VOuW_s#B~WflVxbM z&&V7CV<4(ZWI#lLR%?!hc@|^jAz*)vP;tkTkP0napgtDlJkrXE0t7{EG!5Rd_lr$r z?MM6|e?&9UU?VzX`1QDMcqvivud*$gmC;gC7~6;5Z=>36jDd=J0p;Hb9RlE#oF3o3{W0@29FByN3VkXQO_r5E}a zw{Pi)``Xjs$dWQLV8L4ZtZjRH`)=4baQk@lfW+L_V)UvFj>$24q{dhwxKHvIsTFO9 zUTo{Yhq1cjtj025fEYWC z;Rn-$`Y8vSOj%nRHbZ*U9@g~-UlHlPT@L5mqKC%HfarQ(gktoRJ%#4ShGLW#xZum9_IY z>-0E#i9lpOwMre;h_DtDAe|^&nW8hcaHODgki_VGVp8sBKrW`x_LU`6R7FsV($Xw& z7iuDaMT7d$+R4d5+@N&Yu5!oUeQ)w^(YB|mdRCLH^L~u0L2$O%%1SF=qU!74?mC#e zwo-EHZ=xW8H0baBJM78fg3>Y_TRXdh6XZmiDbHq_?C-xGN+>>AX=Tq)*u<1|aH?=Z zcxR=v=TGv-{HBnNtz;@`sOgXt67sl4q#n#wWlFldi(h=8jjJ-Tc*BSz4W60k(J4xh=|Ph$rJ>Mx<99 zwaWJhrS9q{tc52p9`=`b~kro!+m|lMvE!y`z<#r>VR-sHTJ!Q@HmscKy9}c zch?)IWF>~rhBw}aZ)rYt;I;fNS=wu#KJA`yu!4P@@@#YRIZVm%y6c50%VVJd3KF%z z<@6TM@E_v%#N5JKvvyZV5l8mZ<*04pT`=9lpW9bi{s*u|D@!YnO!2i=szUfA5=(dL zxJ@|mf4+}UPkrT-S@@Y`|8F=}ojJX9HQZ+`TWk)}I%nh1T0aa{J5{1 z2~-e{Wk&&&rd7^}s0DY4`QNFwLqa*tv4~&9q0zrrVkqG0zgU6vw0^z15L)y3ugWmm z8x5fBtyz(XGUtYG-b!lH;&UZdWdtCnQpe*McS#U-r*$gR{Ly_pJ6m+nq8z{6=}{#) zgT<~3n+Z-z+A1teKrRJKAFd!0x;F2zGurGUb#WsS<}Z8{Y?K0aoI>t;#!s|9Ge?Rj zuhW<4l)I@&o%)(mBL$_UI(A5nahPUMV3W&^35}xCm;KG5C1k2Vlk*2G$^lI{M8wzI z%lxVLV%#*NaujGY=avcFW42*bVg%f}KR_ad{D(uz59S`h4uMhUFuryNBFeAJ!flpW6>wZuIqZ(s0;{ zsjg<-4~{d%Mn?XV0935l7J}Q6=Yn#4PR`oDu|{Q_9z5}muL%-D-&qTZK*&A)cD?RL zdq}7J!VbtPd;J%!~g>$o3WyhwbpUUr;ma8yf`; zDE0<{Ge|+m#A5y8Td9(FTyMjjIYXu)ea=x`NNtvg?_GVSFwLG$IUEf4{^)p(+}u$D z@QFma&$Olg&WWiyJ)J+odR3H|hpY9Xyq zuu{*?%FMJAK;DFD(cic5iShAC_{y@i_}>2-P4^hj`p-zqr#(e(fu4s}0iUI;y1{Zz zh`|vbho1fkG`GOi{F@}w&yxjC+#AtwY)Kcy0H|k*e|8spRkyo-%&Q`imX`n3;RjVc zPY@Pyw4m(qrX1e|MQv@2Grf;j@cnx#&s+B|;TY;EFK%#8O85Ro?HJxJQVhh6S|&sY z3JJ-sX~Y*y9osuNI65F5hfRLl2uijBdRvdhBXy{o8+^vG&?Ya5z$beFSo&>a{?${j zV}d=#hl~QDgJSl{1Op1lIF{i&6a! zXaA#LOH#LNO<8#Rhk9IoM{aM=##W$0jw=g`i)C{M>FG;1db238A#2!E{W`z#d%%S< zK)mITfZ!5McJ~Icp$b!Hj=-3OD{&x8%mxx7&N9NQQgm^t`_7VFR8%Z*(Z9VtyKOmt zRA3Wd=4$B;#>v%0IxVRg3W>DXENCttzo8grrr8v2KdR{KH+;P2fN@U*Xo1%V>;DR? zi_a+*=#`hkAqEB|a|cK2LTXbA?S5wqxKn(5bbhBu1h#=`bA5e1l9a_3ZcC#W1ggkW zFTTIs5i*|hm-NKwGVehiC@Z$SDH8luT*Pj^M5?8Kc2?gn^2D4?Z(ze`>a`TozYei} z@BZg*>>;GTN@lBQS^aZ4AxP`zKK)UUg}9a~ZL993&GM~M@7jMu$Fxw(IBgN-EDdYkIecoPsaMFrU$k)V1g-uURPA)H3>@?{C z{oNdnuG^tgQ&Z+7NA1H(i`%L8-LEB7Fn%Khv3a5x)BALtl~_uF;Jh#H$XH=)DgvKz zw@+p9!cmngO=l0dT+7Wa(6inL+;Nk6f_%*MqnYTLnd_xIUeY3Oi}E3avVF=>oF5HK zC8?^~Xv5OqNK)L3i|^Fb>Fl3n-k~m>>zw$V4taFk`c$YQ5$UKC-Ew%^3Py&6hr7F@ z307)Bsq1oSDVyT3-jo|nDsv4}ArS?Ve`k-en7ar)DC-54qPj+c>PUURK8_`E9qVXh zEblW%D)hWB74YdyWBx|Tkz`vu6}?ffSB?_F4L0JEjxQe1z%9Fi!M5G&kh7Od5-I~m z+N8X9##Tm0@$>nlg6LcP9%pC9vv0TUsxrhnY2;~v$*~FiwTQw)avE0RKczB1$#vGV zBz<#h&S~R_H1h85c$hH@$;4~!YlLG`qv_AZt4>g$q@jD71wj%x2CXz%^By-kjC>j+ zHP}?r+Z4PVP9nN^_9^g*>u^Q(r&KY?NEJQdx3zH^dMv&D^b*wVA`mAx?I<9k_G`DA+7{-x+U9(J4Z|Hb1~|O8G5yws zf>XwW%~&>9+>$wV?fzXUL&T+W#+GkmeZ5pA+oWu!80AggVHkEmby*3<7ABql!^3T8 z9TGd)8S_*xq?%6Dg6Wk{?YXw*?s%;@qRYnDv#WCp2*BH1%T^X+HdygmZPF-Ua4p~) zCbr)Sbv(8EYQC2XN#l;o*JV?q?s$`Dz6;0hzQ}L`o3hX^MDi30h%e`xT6GKPpQPLC zT_J49q$rQ$<E)-DQ?Zp%JYgID)}p{`s+{bc%5(yM!{@bTT`#atn>m2RVJuy;CuL38Zit+NV*D*W=LR@-=7=3;)P@Afs0p9TEN4Cz|c` z3u1@^TOCVlr%(K$49|dbeM5t&loV!HM1Ub&!`*nmjaC}t55F2fvrgGTZjMIG#e@bx`{CKPA^=rRWpUPXZJoZR((FYp&|Xqy;!S! zd^G#FPpO_0eY_w*now`{=g(w{*yvMsHw=aqaG7v)tH()2uk)doN%@Qx_(c+!Leb-R zP0HiRhfH3XrZoncn^2wHs;l^JLlT%)~lOYK@vRa zz>j(g#bME5^d${TnU-~iuo=-KLB{rf>na~UsBx7PltK)Ix9-W4%8)-;RUz(qooa=A zZD~Vu4_a2#i^~_>R_IIAG@y&s9|gpqvP!6WbpAeGc6Rqj=H)GrRGyfxz-C|gk|zvxiPkg=(|s^)zGU>T`4SxhOE_~`^{v&OkC$WMc)=c z7H>lQ6j{!mF4V9jTnauZZQK&h#(jUPb@(~RN*wT7e?42?ki1zP^ zL-yYF?Kf?-uV0Dz|Ax}d*uPDm5XpWS$Boz8$MfUsxJd1z_MQ0dYWDQe#B0@`&@y=1 z?(M+$?rexz%1k0UPdfKuOC{-m^Ir|NvXN?@umx^3-a2QAs)ZF>fyTAFJq`a0A%AV6 znS(2thCK@NlveW+;-w??@)<|?;*lP8;EOo=_=ZEaTs>w@Hk`5-ZWtyXJ|N>)JBJ~- zUEg-BVLD*V0z50nMSk^sR!2vHrIq7I(%H-*cHNi*+`pbC(yTsJ@yw;J52bq|QY9RH zN33Mh&he}kIuy*MlMvtYz2XShoz+L6n(rDWV9muy(K-oB*792XruRV{&AoL3$p&yiNzvWuBe~_i2|H%z16L`Y-k3aLK-pXyb^V1eepjLO+~6tJ!pD zv&5|}LJ}ctd&2f7ZyMhd4^?Ue4T3{t<7Q4{p}xGY<`y}1H_J1s&BHp#EcE&vJSK``su;2dHxMAU1R zDqAEGkto-!*b7ocBUvzhW#2iJ02naZqNb+#=$;&dDAF3fh=fq!4K41+hhAo84&9EM zttNjk$+^t(6yP;Uf@o_@-`aD&QPjz}7P!*%WEAa1rwak-@c?awng7=U^z=`=k_|LE zt|5^{>45jKOiIP?7=X&ikb6%z_i= z1^hj`9bkO+i$%-})vOpGO3hRtK76sn^O^@s^J>b|OG`8otUZ2_C+z5%wcx^}V8?)r zO#qfY7O6Vc$U&>5PLvm#3)$UhB9(xHdOn2DBcbRnic4`*r414F#;NAZ1*@Cm zmAWG%6B9uN%q04l1V@T8esZiV4jIgl-}Zug&sHa+X>VF9`D);^t5$=z>H_&VFNo0C zf&kc>AHUlVH57blTz;IaU3NFHm1=riy#9Va^q52U$8Y+(SU0h|0Rhi+Of_e?yuyc< z`65VRYb-r|EGQ6Q+oF53P0M?MfL6gDs`=__kKdQzPJeOA?HDW>9EP56O70w*AHh4W z-h%9+tju#KtT(B4BotvkqcK|-3T$mbWYTg?_4OG$eaF_zd)Mb}6nH-|f(wxz4fo;% zE3Z!*E<0RN{iJgBaCkz|+nqGwn8wCNX#4TlwIF4S=h?^NrO`UZ7*b2DK0D$#qThKg zZDLfu|u&)mZzYKx3KU481o|>$xCFMlY zk2Nl^yX5eWspV>`GjZ4|HKbW2iF9>EcFM}MbQwuB$7%X_>JsHsi%PB5YGKkUM-5^s zHg<(l6a?Wm$nw>@_fELP3;_(^@N;iq{P6ZH370VAV7?_c-q+1V@4(4B3psNh(HDnv zv$H11jRxr%IJp_vI77zI=#|nL(VnQ6P2U#T=*& zaT)W2l+%dJi&szpFO{Rt4udP~TUQoWS8d>sMyE9-JD`k&tw%&&DgGpCXlOW(+;dx$ zku~Q{od=SK7FC+wE|2@}+&d*7rWy}r5#K$&X7u7R+r2)Db}zPo8Jid$ySE@{{Av!ULmJlVjQ7o!~>F(o6(Sp+Je zf1XhC;Z&unlSKY!YKF_KAJvx9wk%)ZVMIv(eBT{MZU>dP%+HLncpe!kl@y-ph1VYC zS?e{y|D)+FqoVxYuRrvFATvmV#DJ85fFLb3LrKFBN=k<)($YP2BMlBcG>Eix!yw&V zUs^!Ap7}j%{nwh8^X|USxzDxFXYU;awqiB3Xv7WN*!VQ-BMWsP=ZTo&4kqDOe2Fi+ z2T635FOZz0-20em{k!gI$9>O!x;PU9dC+Z1nir`A_nAv!ARECaiEKWV0bzG#70%!e zhp2#(>64#Uw2+W57e~|RzZap!VrCRDNkQ@3Rf)gi8#`z$q(8IKp(Fe99RN;oJm*D$ zrC|<)>NAV@v|5|(lBT@39rTrjYSObS3DBxX@gu9Mt36+zF&fiNpGX5xD9ZS{DIwjF z2xQw&=rp7@94VqoyR%kr9WnI%Rt7x-6OKqBW)zN@Sd3>gEhaqLC1E2Ilaq;F>$L59IMMD#l4@OT|--Do= zpvBnrRA3yF>N9DRnUR~3^Cwhln8Sg@!i%V&Rh-OC+Z%Hh&{$xxtLCQq-=B1fGLVna zX`va=k)UGpYjSp4k5P`lfo0qLFwA9XLa;^ny&s8~L>1^GiCcz>(k!XfxEF5WT|mb- zD68BWVS){xR2RjOXPFCV;+T^G|EQtzaw9RvC!Q;}Z`d`$jKyOma!~R!5IdI$;lC%? z6JZce%pA@E2|yA)T4MQQf}LOQjB@&~8#N~0j(Vwc_yhQ`Srp@rJJW{8?&_{gG?;O5 zDK6scLJ@Pq(WM!ou$C{CE7sd0s%lxHXJ;l4ryoAI`D|GqFD=iK_q_X_Q?DsO8*9+O zL1d4u-S;N?-Nbasgf+Pa`8q2NH_uE(i4hJ!lRl9jwU=IqLOIsw(RQDki7cjD7*tN& zZdLjop7y-M2wLOLqefQSkXNz+rE6bGxg&K;TbtbMJT2>H7!iT|zn;c?K+gRB8#2qJ z_>4vo(pB-*6>G0l-~;VHvBM3UE<5;#35DSomwxt+_?7n#|;o@P}dcHh}# zASC_F`j(dI2ddktrLmEw3=TYdNc`BO3O}=&m|xFJ6L4hw6#uVC#1Y64A!+W0Hq2>T zk%G}%rSNR;N;&Ajrw-lL;?5Oe1F82sjrB50XHR~&&1f!ocTY^Tz$1+hr6XQLol`nL zzIz*zRBj#^`!}26LjS1f*+{rZA7j9C@k>9@n1s+^qgN!U3W?N?K6&6Z8M-6e+V6Bt zm6~DtJ!GRbXY+b|UfBTtWk1Ek?}|rKKUEG3l;lr{O922TkxN@in816QNwh)GDHNBH z0l8{lc=!5l>X-4IuUiY&22%O3m6)*C;+OI7=YlzxbI zu&VZyF-s7T9@h`lam{w+D4wNDufXh7&P`;sqO1+Z4=L!-vQRaezBhKawCs7`+}MaG z4WR+NupQEW2L+-a2UAMsioF{x6-E;}PqJ zo&Th0IY!zYMBZd8@JoFHQJxx!NM zSF|U)sw!0Rzx3^@<+mm^AgBB7o15};RwWgSJV9N@k6*xCoCpqBdBK*pQ?qdP=Dn{; zYIJojfQxxO3ZH^CT(yfgh6e(*HH_{<3mXT+q4{Nl7(saamG?Cja3Jm;UEk(>){7Au zE*H0`XoZ{Z?uI@;OlACHL1;`-L{?!Jzlk2ISvx&drdI%@Zo!PiFER^lr*y8%7ezle z3_F&`y8qIHOl&?|*fVbREY)$%4RUlZn6c+M&nT?6NMxl^XPdLgEG%(%6r`=)NuHwE zkp%T<26K0<{pl`$D5;xxc|YPrlTlm}i9)?UR7stOCaw5JofI_8i+MQOkMz=@Uv(CSGtfQX&UBt#2=W=xY!Y z*}@emro@6Q+1|V! zrJKX-4VYG=v)@izp@aW2e1uxY%5_!e@@3Ca8mlGc(}=PxwdcM5dn!S9 zHaabm#`==MecyEM(OoICdHCNM$;J1pR(xi~3K<)Z4wRg3gIj-=J>_^1mOL|S$`^E% z=6+j~mN0D8#D7b!-X&UOGUxmiii=IQ8X{Qeb*VJEc$5eV8#QtDEU}*KL{_ttpsj~G(u&Ect5a*< z%Wsl=@kQOG<#fAl;&gO$?}7q^I>Z^&%27mvKq98#Rl|)rq2;NHlTQCXyXV=hF1uS) zCW!Uoii{SHPnkN=wbje>v$VvkhRDDg&V!Tt-?~~MM0dPsg_{>zvmZb`^W}*CT3wPBF zsGog@8zm)2f0Waiaov#gGXVx8i`I~>y+sk@HS6AsS#VVXtnKOJtqnD)KA!MqM{W;P%y9^nh zk0e{ocYSN#`6gs)<)~e)#*2z&;E~49W-b)w{It9b>t}Ev3%#h~Lm!bI=i-&N@ z3Fi-sFMaB*Ps)3BLJlAma%KdlZKb};!RM|Z3PpsBIIMxkuAhEAxw@ySv(6BCrOG_q zG5f+?7R1LVkJtW$uAx+nXkBset@E zS_)y|&rABB>)v;jucf`VF5_3^I3S5fp@_h!h%;3qI8oe=r{)a6ANoDV>&Q9h_8@nL z?(m3j=;G*TeDJGrvQuf>YliyfA&c_r-ciF1gr<#|k=~mKHUq~zMA^WpicTUcVUjM` zX;^o2a@X1T-(KDRxksPrT?m!Q@*>SctDeb+$JMUUwmYrebeVb7N85UFPa_)pp&HW* zOHHn;6V}K?%-LvO)QtIws?k>B*mvXmyB_OT3#?1jjy@Z7D;gE(EYX=S&jfh!DB3w9 z0=Xin7}aTne{B`a#NUSNodT+0Vt#a!vTo*47`UOqj!nSJHwTy=R{8WCLa)^!e@ zQp9e`C{(XXwX|~+Z_Ei_*$d1`rI`6>FITGqzeX-S z2eT&*?3KXjV68H3750GCnPLC{*@JR=9U|J~{2-05@hb6fjY(jrTqi6;>fxeZUFs3Z zo0H}-T$7II-M}nqo}rqRW9C47hEOj5P|iZGAr!Zy^M)j#iVsWTY#7gK`S~&SM@@M1 zhST_`QhxEolfl!pvWl7v;r&7|U04GT*kg1$A$q+ejFKs4!!(`Fe%to8S3l^4qmr#a zdtr}fPA!wo>D0le!h9oVx#>P+^!Gt2ZRW9?mqS11Xy4Z?E?yjG-H6zH`!11Oh`gQ% zkJKAiWR5OW{$NpIjKaylw+X{V#tjKVCnZ*fLnh>ll29*+Z zcEI!Az5r_VA@nyx_$Sb#1h&aiwBDOP9Li+zx@!G9cZ)Fs<4?cJ3@ERPZ z?{h+&j>>r)v{qx_Z447d82IQn2>r2>_iXewNsF|J!k8@8Bs(|JTYkCm(a*iS_T(Iu(*Qr35G} zqtJ}fo)8cWOP0xBvipQZZ9XpeJes|8!UEM{`ses^zi{9*Fr;;(VPfwxh1X~Mx_(J?9d!^Og$6QOW>h-{WA!U3astjUH(`#$TQCIar ztv#^vbi#gU$t~S7(Xm->n#c>lr>P@6GJR3(MoimKL=AJZwe|G0p^g2l_&Jr{3=@4FlPrQ#tTpk5kQs)&FUIsV^X8QGkCU8$0pRQ`Y zk%(omG&oX^ZhF?uZul>M5##B~onTI0lfqL@Qg>?eDDLnf>hKX!Mz)*xP3E?IK$|~i z{7im|KO}Y4LE3f_{W%3@kg~}SNNG)3P1Ie_F>n<=(^tbU^7Y>6ZUat zaVXQP*91G&#Ssu>xHwMq>io9F{q_hIy0ZM|L1RJ3X|D6pX|0s!&f)7KwH~E)EZ~?Y zN(It?aEqMBbDgx;@M@HI+r9gwN} zk(GI_*^uG#msC%6uhPG4qBFD8R*|hD$-3t`>tky_@Gc1h;K49f50cng&xjvKL)^vS ze&-(@%)E_ZI{Bz_Oz5lgnb*qgaKPd7ghPq)fc{#|7x0b{)BW-M4Cye9vJ066RaDz+0-E=ZN@!o^&o=@5x{^ruw^ybwdOm(S7cl;# zSTb?5I{i)jlaOnco0zK+yHbKG8zQc7U~q8UM)0XC>1<>!M9T=CxU{4u4%5LS9bEQ; zqS%FK?H$sw>*iDXjo-h;ae4ZX_sTK)Y-GvXE7PRabAPYyYPx&@=^DoM{WIBnji1u( z#KtP*PAu&z{JCHx5X74)>2;kJxiR2jZS7_6oM=<9aF{LTN>qrfUF*ndvD}rx0*9+) zZ@oU#QCo-jJf@mtp*oeA=A?SMOq%q^ez(_h~|b2Jlp7)tnW!e0>!7%;jkM1vH!UM9rKZAy~^MXE(V{^FTM$I{l*R)qf7hjNJ_tFU;K{PeH+!=F=43W++CUdM1bnMuhXjFgVF?48hFYJpx% z5z?H}8{6iZ{h@#m-;br^)!*Gva7pRw9d16V3_NIk_E}}}2r83+1BuQP z0Syx#Ux=}6C(HeLpGF5`H-L+FhzS&-E21KiT&TJ&%S2YT=nX+T!yx?bcatK2Y^2oS9d;!SrB1`dJs zon8-#QrC(0GZhMNQ2X=ZZLPKDR!W1t>29U>T^I%ob1+-y_;$JEUe3?h^7ycFceoP6 z#zL2-`w~+FsRD;9kv{`){heIe#Ym;69~*=g=jR~;p4qNqOWH+huFY9bsCa)a9O^bL zPQpf~@85P)#})_FOTRxh+&SJR9mH7cqy@*s~h!nl};AO1&`n6M;B|vQ}rbLN5~svySEOny>Tn z;K)G5xUw7za{vsRQl97t7sp?E(&W9YQ?5qRuUeO7d!HwJhZz|=8Aup*>yew$(~r() z7p_1x%=MjSXH6MiHFs&ReL}u%^HxX)Kdhy$HMtNRIU4EUX-h*=f-Fwwka(g4!p?u zOdO(FYu(St+5GXy&je&b9nU1i_Fr3FPdO$|PauRTYMWR8E|9vz|4m^sDmmpj1!xWY0F!L79kYg-}k$AExMn7)kWlpTZk zo$oo#Wx1A`lrUafu51#`i3ZHTkFo!X*H3?jIXQgdsFn`}5>^^ElEL1@Z0H$#Zb?0y zn<&`-oZ+NV$W!7+d)dEybw|+Vy%t&N=HOLg*LL1fzShg8RF7sq?Ed-QNw|7hC|I8Y zo~>HL{pu9S%{96X)0yH(f!9_y*1cx1wA{thpG}5sot3SJhJ$Go{b4_BX=MdN(+krI zDxS4Je+|(me+}ayhre#+VO7k6Oh95FW8&pI@x;vF@*k`*532;$qgf;)HtD^z8HFX) zg%$SDbQLzwHv{Z>ZJs+>wa?Fz^^AS4=dAnkvc8*K-$k0-LGlL@R7W$=XLRCz_ZWBN zz`|!9_X%B<{DUE4umgE`=^kovd5-54#epjEqU)gDhjZysSsp?EbZ4vT-VDmIY_@aI zU^F;9u0hn)F0Fa(z~*q8!9a6J^YiYrPKM{E#&98tw|V63wkAoiIP!RvjrgRwcg6>_ z=lCMwsuw{So ziyR<*c`6+S!hpm5w9jZFQ*=6l2*YiFl(ONk@Ic)KY73rHv|CwQ7A0OM;~^ac!qe`3 z`yaHP>W=ml=W`Ew6mfJVin;pW+H4C8gzC;rr+TV|dM>JpyGhHMF|)pUC^5S2_D_qg zeM|+y{#7w^qA>qB@0OI#zeg3b)BZ4AlyIYUp~zMypl6hzKi*wxee72K9m%~bnT|Na zVM|oXraIKUtx6QxiGZ*}W&~l`H~=sSJ`QW~{>|cC?f*b+OxV}uUa5(`fdM$asRAza z>XduPRzC3jAhL!PY91{lNKO3Sb_itDTIUobSYIA#a#P^3*u6@-ON7JvfS z@-4-oPE6`cBrk6XV|Z@g5Vswk_P2wj~N_JwuCQVk5kE{DbK^^a2{l@qj(+hVmZ&f!ESi>P0VV-6V2*xV%nr^BXLfyFg-B|C_ zEnvXpm4_kW&kmL|Zl!+z=-R67dXOXa3iXvszkLsb7U~_9lN&!KL5tUAdXUoxLp2Rw zx0H><0kY;AGEm%jEVzhpc3~dLXJ4>c6g|uMf*za*XXuOs>-|ceA$^&QM`1 zcT<2-`&+?uh~p!~oZ_SZ9Yqi$I${Zf_d+zwd&4jFxpINMV3hNEW=({RD7yq9M4ygX zzSGc4^F`1b{>e|U5QUtnd*9FI?4>mH`!)&g&4+HZuJ6M;3~-=?0}unc>dRr{ZgE{( ziL}NRN-&ZipCrVkV%1cb3tuRrrMPN46p4v}uL1vVtafU8glQ4vmZ1Q_uc^MSI%Ewy z`spkNX=6?JoNG7 z=eEbzV{dDB>q|_h!1TLUtr!^}NYMNxyC47o#V4HL$Wp1nSj3fddu$Wk@G)%sz0~8w z%xHGozn3Q0UO}uSeRJofQq4|hsM06rby)cLFvhb_gw ztodqHEt6}u^YuK9iwe{a;w)CsFuOPi>aiz|sL@ti?-#Gv{`A5rlUmEt!~@5bHmzJU zg>7K_0_|aG>)L1{^-j&LX_FAjWOVl*26h1c(-Vd0dcikkk4dT58`jfxaQ zjD~RJQ5~cBfkfZPo#eXnVn)|*cD=Rk07QQ(ju#swfq(Vh+;^Muhq$BN!ATaA#0 z(LKL}s6Epk(b?zI!0V>nBwq33c5sp+$tvp~F2)_Gm z=|L?d7+8>(Do{#p05t?->2KDws5EKgC0cImrL_YMM&p>|yc!q1Q7S)Iu?mD#OmqJY`f#gs)>vZA3DJ0mC|j;2m=uXn zr3Z8RLq1MeB)`H_EnTuQsToyj1+Y{l5M0bSR>bz@oms03FFkE8w5?BIRa@r}m>$Ly zxL{Rsp@5LGPDli(r9A>DQhuo5pO5hO2P$H>D*|ky0%Yo)ABN{Y9JkyjKmL7*m@vHJ zoT4BE=vc}_yRNam`0-B3wF3K6C93+)YZ{uLe2$r_x#xEn#9EqhxmWw_6Q3>bjB|FtfMQ{Q$9Sq%w z&07jQ5IWxk*#aEkKg%8L+s<8vc(vQOU{6+}_%n zSUFOt8l(d!{!z!KmWXn*Je*Klc}WCjIn|*aE>_zIl=z=9030mNAbJ$$DjIYh;*MmQ zMTw*GUW(lbyLA4m<>c@e7I0llXi>Y38HD59Fk{%w>M-`jX!DAN~>2 z{bD4P+kU{oGb<7Qd)|;s(5fbO@lBod%=zF_5b>X6O#O7Q)Re2wEDb~S`fk8crYLlZ z8<%B6gm(}dE1KVlUVP`N1pDhgC;9_AO5c+nDhyck&hMXspNsQ-XqCSCLYCTupvlcR zRr8XYkshM|Kxb?I+}=_XBT>jFo&?9;v=;dK{hczo*~{_XptBy$Z1wTl*!pg~yh#5h znsnO~lQfXXs+3Sr@B$o;EH1G({{$y5LTAgIFX?*kZ`UpD&#-9N$Atu)vQ>HX!3;hf z9xOMt8l<38p~={)m%XLyC%eUGn9|H$mJs2NG?a5MHm;>1R2qBg4>jD$I7$dfb`z8PO$PwH zV8YsVE#S(NsN8R=ccDcBRaHI?4RaR0flN1HoQ+7JmE{J#*G_7Wr`3*;Vf}b8F|Dke zw6=9(Y%EsBs5Qh1>_545JX^Sv@YOI;{2I7WeGvUpgY3Z3KmI5;|;-AG-}O;+9c z^Zfxv`(eYa7BN~~UkI8B=t`+CLH}pC>mXLZ#n8Lel*j(XBv)?z^wL&wZTJuqCWsQ- zNnlNj&GRbrB*=bbD51V|91RGbVG(sARMI)R!Y2K=I1;WB3OwH4C_h61XfD;F+CuT*?iI~43Nvj4bVujteAHG*62G?aL*o;%-~|f^d8|C?ZV@Y{9C7r-Mb|^xMI%6Ef=l z%l&3)I(NZ%&;52OrkYI^DHuL1cvK;_aLHxY?*eKyS9$G~RrnW&2t*@`iziD?LA+s- zR495Y&}Nf*^OTBu_VuFV#`8B(r0M&y?q8&siq9!63Dq+%T}JD?B?y0j$aXK9ywDeu zi8mIeq&-{t?U=p(?aA{5kSlMjs{0#?OA2Dq&91()RYqrb@dK-azA@UHcC9>+W-^Yj z)5#eK-%F$IG*?>W{PR*>2CINfo{p_rk_BZdJTB0u`6b+z_VUho)NAzR(91+6c?6Ua za0+20#z*b7kgn8e>dXl7lDB562*9%}5(hjoS(V~kd@(Z-u_oW1K{pxpkK$rGzd!Xh zm{|oyD?`HEynUB8k@L7XTWk+mUI7XB8})opkS*s#*lE z7I%z~NOVKSg&%JElHcL3BT?Pruwo1e9&RnqDvJ%$DBn+)WcgS>!5N7>(&aG^6lp`V z4q7J%OT(c!&s}L{a<&^rsf&RK#565jD0I7v-kRnTMcW5vJ~%px_?s(?fRqknc1xme zYz|Xg!VWH!BIh!H6_#83ax-LfsD?X2Ko#jG{t5Hl{uRox$cy^|3W9INu~3Ksn6bwf z>4*TTOByP@?*(&*I|ZKX2KW67hQi-6$j&w|9bsPCdcS{^uRqvJ-iB49B`dbHsrVj` z@4cNy1!oW4u%xZH9NeFZ4;whkI)o(Jq)BoVqxeSQIgI_~vm0Xfo9q&xA6tCpY{N5s zuwaL=ut|S~pyrfgyfzaK`m^A}f*gPZ)ZoE?!vw}Me5>sk7lPt}mU##bvrx%vn9(Ysc{>#qm=M`)3-8d0HzPrLCRr6VNMpf0ezNRJ)?J{dy zsWc*qQ*qtJ($V1o0BU0)rOWl})1|NcQfNo_xYq^m4JmY5FDslpvoX_(#8B51r*st< zF4sxgA>p6d4;9vw zYw{xD8;+I2^Urx)#o<}5e|)!+^`4Cq#{aMtXhC?mxf*?k8>CojZo=Vs3xW~^@E5F5 zrkWx>bf&l~8}`A^L?uz*`>9dl`p|_uE$>6>4r;((wD?Y7I6la5eNnKk?pHFKXZ&|d zhQ9b}N+)I3td+7|9!lKw8Z}IubqlFCG)qWHw$9yBy0hwKZabV00mRtTT#evxdW_{~ zVzQOQbpu8M@T{|dAa#fZpALS^kR~&SK+~$p4OvKpEtgaxY8D(_#STSch<~6EIc)F1 zfy@HhTczzwnW2;Md;+=~lj6ok1`G6#bGoDY&L``z81u?lo?p%?{=slK^GEu=ys?U^ zVV}%CyDLTl`v2|=;=()rlEGF8^JrOCuu)KII3N)L)tXx43cAuyR~$4Sycp z`=P6RDQhWrixa{usM34)UwJ(yA|?oPJ$&iI@I0q zF%q`aRAn2Qr1ilb)b%2n?Gfyaw3r6smYFofkNYr|U?mZgn?8E7UE=dxmU1?JD zLNz$@&vFq2e8apox~cn35e|axxe$&(8%0hccizQupW&e@dKR^=9HLzA*S75W2I5$` zC}sO7ezwgV&*K(rOkrWTqMUb5KeM1|J)5OPe#U%vZUPvN?}BYSRXzEe8&zl|l;zeu zrOX2k1QRN22dJTe+AMTM<&@Bfs840JY#$2-uSHHv?TZq0-(v;H{)u`2H)e&oOzO{5 zFBSen-O!6oa)w8m)iLu&$+V0D_NDc^Nm-bhqyH64YteRmzJQbjJ>^`RpD$Wl;-iG& z8M)k4afs|iABh*4?(pzuS^!}A_Wt!#aCv2AZB;Q@8sptx0+aiBJq)<>HNDW)DClDX zYuDIS@A>|I{WMnlwRq|I;Hh%a0v0w!blLrx)Z^Lz1XL$_VO8^JQTL0*BeWx2S9PMS zqTHy>69-@brL{(+HvGfn$NhUZ`1QW4^uI+dKfdL;F}BF)^nNaxE@K_KnIyP&ifeXq zfa@Sqo0BE;!M>~x8k(Cd-|m|kjc4ZN;pN(Rnb+3*aafver6E+YZ?0it7l0@x0M+CV zqMHRj@jIt?Cvw}|l|UkA%v7d_d`>y~SsVm}ha50fN2i4yW+(&?TJI~xAR9oOum z&hFxVg2^6o6M@dZ7Qrcs1I9M$o@8nTxi4#&dNUN;*Z%r-k#+3N{zn*N%ZR2EUiRHT z92zzHrKywj0`NqTT6{u5UT2EVh6m7kiE$`glKpD3?;D(}|a z+s>9&9#{PQTrDZUT^~oY!>dqSa-pCcp9=x6#*4Lcx1xGMYU&_keZuEnqk|}J!ztD5 z?|CPzFO~bSp$DQi4=S}Wh%)~8YDH6GUHF`Bz5A(>!RC@<5vnGC>6tB*K2k)F`4^)9 z79Z?j5!};^Pv~EiQtZwvqLC+(CaPq)E!ZQ@w!8GQ+O8i>1(N+^OKfCK^k{)l(O5rO z##56eV@FVvIm*_`rAR`6!4Qq#j`S}DXbJL(hJ|fHV*bN{j>sDQ?h0QFN*j)Efi6)N^N}hKrvv{c&FerlrxU7#aQ4lN-Is&pM}O zu~9CuGp+LOL@G0eOa2>Cz)3H<;ng5MZ*XkOSqIC(7(D7Twa*yw&z&6;3D3%OmSx}X zVXHM=3Yy0LeNZV<@4UTj>jDzw(?|>?-r{tDIlf#9sPAOHlaPM#l+u!WjkoLR4jRzO z8UE#FNpjr3!C_@G++Xk=hS2SpGdrhIJsPm*qR{aQ`4w5yk}k_^baSDkJX5O z-y|X8X%_$M^SM%gJoCHR%6YtYuQW7h^BjshIpAr3ye#s&8GKnuZaH`8(w|zx;pXlZ zu424-{P1t-ad+h|1E)Ay`qe*JPVMUmI=yF?qg(NM)%V2ue~YS=gsF7h9^Z zkH8G&&c@_D9F-`EqlOl0LTXAZGh9u#V_g5E7nlKl-6Y^ywrZz*4DPP z-2JIIs9tN3T<@B%twJ7x%aU214iEq`mEw4L^UNxikH;7E=KkZ`weXKU-5(X!XIAG> zN@y*AA6LX={#IVKjHP_&Qslvm$wCmt1zDPVHySk=``^H~s(9DQX~qk>*r9RBhBKDc zjv24Xs#u1Pvt0ptcN~l74OPm|omZ2N5NE0u{jXOhuA-x(YdGMTg?@R9BPX~>`_N-@ z*Mo`J`MmUC@-UNgf6(RI#yH0jbFzIiQt5Y3`fwt3)5q*a3P9a#xG!H%*Z)4hZ@YOw zU!ccNwc@FQ3a|iWWfio@+8uPs;Lxj9@0)+NQ0_fDO^G?rwz@h=ddC--XdO!A_*JNP z-e9ubp^wpw>W1BnpsF_9)nLkXqjWw*d5}xiqRXNi0y8KtulNS;L!#<7YA)*Q-9DGC z98?%;xYm`HN9Q37wd47NBS8Iixm?^1Q8L7Png@mxf~xd5NH739&Nhy2>qrP-tK4cb zoS{hp>?UH>1W^hoYNG((cAz2>* z`)|_PQT29?t6AVdl=R0ZfoJTNt1D8JB)}%~Aci@%cN2mwp0soSBUY*?gWuAhB3D5} z#H3moVvRr=(trifThVcqbEv9aEcvB01Flh>(i>%^iFvi9c>#d3+`uphz@J%kAj)#K;u{ooeC)Ixv_vh$U;S$^y&~c zJOL5z=-b}+LB-?|JWIQ7VNZ9`&Xg!)TFlo*7*#iDHS2oOid5PonzF zHuMTk+}O@5IjkI6=5u=gBNd$UFZ1{&|(h7~Ziz>0W z9c3<Fg`PJP(l4=RwU zpKx8S-g&84@pJs&JCi#t%=xK&ZEt?Qw>MO1Tj6tSUCYm(zWXWe6;qDYJBNqQGrIZc zV9Y{AGyAA@wBvS*`><|Tv`HZ-l!UE)W|jNwu$84Gr8 zem1|RjXraz@9%|qI-3vmYHFrvW|{09CQyR zesW9?DYQGQIpX1ojilEnFXSpFlJRi+maMIPu9PuFiog6R`-u6D`?qLggEoHDe;eS$ z|7@rKK8x^@QeLe0Y~EKd4gd)~TF6BBeP*5@u$LWka8im22ofpOUWw*czunW8P?q?& z`Wx|A<$>=H3FE~pUs;45M~)9^<*1FIZ1EDbOLboYn#E7>Y+2HDo0PXVpQl{5RvmesyzY|PsR(?>t&YO4#Ej&v}e zLavB1F8npr$jF+B?_=fkOxUL&SiBtnGzO5j+<3Qh*2J4$Xm%n{KB8nJD@_Xyj|v`K zu^88FUOM~~TNAy8W*zFYtWLDV1c!cJo)>hgsS_9Ts-R=HHPBg$RLtcD{GjJlv7dOd zfauTV=jc>5!UC`r+nM$*ECe<7XQBblF-dBzu|Fb;L8ypJ?qOA<(%`>|Rfq^ZI3))2 zOTgd&!Lqax~Lms&gvAeo@vdbCCi@8!Aonb)ZCnSf<{M2Qsr>&BT<9K zMMf#GqNVJuV;nBXtf|p=RfF#@njdpog-L#GG@x-uMbqk*&JRhuyVT+!GT=pAEeK4B z)pFn$>pMFGmbvE>#+&ghO??wbsn=3wecdXTP>;f;!>T-%mM}?Uk+EM}44;xL!;@YZ zEMg@vRfBF9W}2rH(yV7t9iQN!-cu+ zjCZ?J?^26q;ScO@O=dgXJUpT?=7|LnSieI;dK`rm%w);R zbFPbPKi0;^#xU2?%qk@*3bQ0(pfs)hvI735`F)Wf&#c(c67y8Pw&&I|x z23g^#&g9T15z%{WsElG=;i#x-;_zQ8sx>WE*to3HlY>k@b{biO1-u=?r(ZZPgf(qv+ zVu;vywNVxa1$(vDdy!zps0*oqP3ok<)j{Q#;pUd67&saz_C`1M%@pU9c?fF^;aFm~ zJYBB7Jx3=_ULl?N>gND(cv;Kz2<4v@vX{vIDj3G0^QLn73r01dfzX!o zY<)ZHI=HLiQlbwCh>W+Sv&R3m>2)wFY{Kr&o|X8je(qYyrMF9_+c$y%U~#VYnfA8& zd9iR@>$BS*@aBR2Si~>Qi%^io3~6m)P5bzG&4!33xtXpZ5rEiv^)@drui5R`Dc@W& zpCck_Y*>XA7Ysp&Mg!eEWOXqul2l)8HsHND5+GD z+!3lJsXdPTm%Lxb^o)*e>^RsE^7zGhuH-}Fa>|EMNh9~-tq74)e4muMui-O%^5WxT zGZVaRm7LR5%(9_68|G*K5rYT0$7KSwrt`;?Uw%?)Ui$SBZ`ff;J7Z#_DF*b_#>2zK z<)>Ep_Hh*xGoX_XlWDaxmZxz1_KT^Tui6VjCN{1PC?lcyGx5%m!W$#h^RWo>2MJlhj(cmHK|srY)LcNsR{YuPK!(O_gT9%Q3HZ8|-ma z7~yGl8XhG3l)2_u`b+?_<@kZJa;P6F>Ai$bYE|_*IS%~%9;m`^0nz?0iTJrkwZoe} zcC-CQk|*7a5+QHWEvzbjKi)c8niEx7%ehJvHVMx-NLJCWjCutzD1-n6OjOReM~qe1 z_!9}q*~7m+ch7tE>EC2MmrdP(!Rfo%=X_LJx#Qf(z;mCOOPp>BOhN4oAI?W|ShoGw_ZpTZJ!R2ZSAUv6p_vIoLUJsp{inEe8+hdI(o-3DB z%|@}%(j`u;Mc+Q(8P^Zxg~zu_^k7V)8+PrRvH@4A^Vc~ywNlp`A=lR;SZ{`HYMnJ~ zLlgmpI%h^Rujhx+Bi{!pqOBaAPKHCCm{HRO5qA^E=#57?rFy7IzYhYPjg6AH5MAqRr*C-JF+T&2O)z%0A+x@kme zF>BoKPxv_4g!tsF`79vL12=9%6j?ISH1?B(E;$yhhCtTZF}7X#a283^$?5tR0kQ%S z6V{`8r4*nTc^OB4>GzyLv;hG^?_covizv<7;BJVb{T>TU(q@}qeblKQXA*%Y2e$aW zw)%fGon=&%kJq(_kXE`uQt9pv0cm8!L5A)IY3WXB=@u9fap>-n?ijj6N@D1a_x}Cg z_3*_nE|)OFJ?HGb&%Vl(1H5ibIGqqYJTJ!Zf(F`ljWk~b!+kBw=N$u^Sw{^K({y45RYVbUBUWRKVWWV?RGM8v@I z?B1!-4EKpYrvkQ*LtN?LV9Ex5AMdnXnp&1bGy?N5j#%F^YoWZxM37oI7%5&(uBcsV zfZYN#KBd|T;kB@9t8%av3sd>LD*W>JW>qHLYtxkC#f#?`M;t|e7jRkObC5W8Nn8Vu ztv`JGH>4nteo9#N#*GN?ek^|ty(-ZwgX%2aMAF4dtx9sN#pCY%Co6j!jqK(3-_~NT zjA(4j){-u$Yo$$Eq06;id9yREdDAutP~^S8ZX3S}zc+TFNaHr9y?sSP{MR^TNaoXR zNc$Z$rY(?xL7v^(rssCQxg`E;dzo@CUTi@eaP3q=^~NN~QCZ;))Qer}S&}Kxx}llO z8spbd6nE*rh6|J??Rkncw;3VbTcFfvIX{@S2mglS=HJh-B+y9v-OiLAE;+0Hv2k%} z*I}Csl9Wqrdh_9d97u7jsIKhf31SYx+w~jia{>r*hm$VQ~@ACbXb*p2rAC2@Yq6DO(NL2ohn&#IZGII z|4-@&(0b#G%8-#c3lN4GNvuVXAQOlkO0IhOyn1L^MMol*h;ms9w z9kAuHM$d+O&E3vl9@m_W9hcP)8{gvMaM--ko%S)W8kSR+yNR6?NZYcALr@AXs)+Z$ zU7CtTDKm$WHd_nlkY(uh*_cP66H*Fe_1?5uAF-j?6zS(`mP}ttI=i@>^bUSCrSM!l z1jfAJ;Kfn_YhazGm^Ll1YXahmz#D*hcfh_vL){@^vv_*%wmktWptP8fG$!x{_rKcz zm!G$`(zD`7L9YRX3ME50@(uujoP~yB;FKRRr*!oRdg~~V*k*89*5wZ-Qz-u7Mno@i*1rU z+pefa@Z%Hm&!7*YWF8sWuSRuRh;tHO64M9OVRfT? z%&?th9FM3e%=)j36-Ab&ALgo;NI~^dzWAo;2KIaF=EhqgCpY(DTO_~fOv+2zZ+9a) zI$E_ZM!|$4`oLlt_tbc7-`RW_WU}(UtbBS$C6S(pktfvkx1qZsrBw_R>G>wW;8$Zq zQwpPn2=hSI)=RrdoUVuGx)AlUJhBbA$#FV1tu>#MS|EbT1x8V7^#>0N##j0R#%B9}7B-Ol1!j7h0%)4GgnwF~{krQ46g3 z8Qmo`eT&oMyFyph_@40Pro3)v9{yZq4)dS&cpgd+aDI-Q=L=RoJHy32`*NsB4q{;` zfgU0vrO&^m8~N-VmqjMf1RUZTUw5P7ORkH8gX5_5c>mg6!EUl!wlSv3GjSU?jX4TNjB?_FBug(oc6jA`__tX#}dDd zN1&Yjy8mrtDRsIUU`t45R$i!E`7@p*$1d91rgzU$xr^c^vohl*3#J$o8JC2MkJ=Fccj5=5Tv}#0 zNxLz7>d=Tu6G|o3p#DCcf;My1KQuHvV}0K;y}g2~3M~HPq(THJI~Wf5d$G zG_eUBI_m4`0kfDrhrP#S)L}yOf?RxiYV6eSKnT3fwZA zmzOCWa4)EVYrc`vYFz)ARYHEsHZTs)_mRK3vujpv6tTfS5R0u^wi&k}z`q1wZ zQlL{dYcOZo$Es~=YGv!O~jc`8#<*t=4myv2O{d(5qx(OLL-{p z4k`5^27i^=YJ7YY3a?W|RbnA+in;H1b#V5SZDS!Zb&%gNrF-^?D{?29+ao_k>Rec5V$Y zb+z-cva+?s22rfT(v5Lc-hb~7Dv0?H~{$OKDo8-3(jbhTKXj2qziE1Q&Okw820_-=8 zm_-vE)|oWc?tjOg8YA31X_Mn~H`9HO3UY5*LBl+HU6{0yUZB8ZSLIOaFLgFWt%>YhGJ}s%A#?q|H{A|P~un1za!3g3vMfq7I&kM~R zKH5LmBM{p}sd-h20ZTSTh)*g})^j(!)|dH05QLqyK#z*e$OspLfg!JB3O6>oqNA4# z-X3)Lmk<#2Cn0_sa07?Lz_FoflX?{1NNXBaen9n&WP4rrMYi2SYmpN3!P=aPZ~d|> z-X4w<7>-W%OO zBT{>9?bK1N500c(drVQo-Rdz{STr^Y8V{NOSoJnK|&|1Dcu%71n&VGlRRj)E0IO-dvIJ$Y~#l z*ZOI8sk^TIA|w-zfTgI~GAL#pzW@M?IjrzaQQ;BS3mj$`??|qvwX5CfdFFJgD-2wU z4g*cLzcl-7(Ku_@=5YWQC>g(>(Rw0btY3uVlKb4=k1X5Op#()4%;5sR*z98QDcy2P zjKMC&T?7Ccj~|KyY;BvocICgqR*r!L)v*+^ht3-{V^j3yOUj2{78x<&_J{eChMk>s zGP1P9iK_=pI-lLuP^5=JgYdijjQQ0)&p)EC70{K!?(?t&UY7iR$FLHSOpu7Ed}cM# z_inuCuw?JN5^-?GtZhoM_?Q+?eBHj^oOY6u7CS|$V_WTtJ%T425{m*SQb>tcV~!J6 zPGpY$F!NS{fwuSMAnC7`pn62oL?0GI9UtC1c1(G2*~*U(Ln?(>D2Z{1FKS=zVeLRCqR)%i!UVa9^8I zb2wr-)+$3KO6RsV(uaZ0nuRt&?Q5aVv=YJ}%UXe|@t6MR zd@=yd8+QX#FlA@!0_@q3=URb(9`BIp@hOD`ZKL03?buTPa?TIii4j@87eoP74C+;3 zE(>lVe3o9%j)2o~$%-F7eRb~>5?s=*v-9xrxotxFI|F|lwH%29HP9VEMlhD|qk4_e z_=l8za*vM}l?dI$qZ+fII;1WzPRy!aK+z=A@8q|!>l0rgit-JgA13;5Flks`Hzd7@2+bOs1xI5UJE-9P+>7Mf&23wwcD?jIv0+uIWd?r_r zO4vsaTm@mRi^k&!eapzq%FD{JYk*349en;X$g;_s)%xk1Q%$M3A?*AzI@)cg1o%`O z&zJUxVp$_srJ^Xxm{n5veJm{(TxeyE3FPdz#5G?ZD0`mqpraHDLq8cMB@F&kp)9KX zrhpw8Ovykx%X7;ijDZn8+YDy&4VaKH2~|hM&7~?LoUD9cp%6A%(P9WoSyW}_rLa=6 zLRaWwE4PheeTGKhFOwMG(%Oor02M$3Mpzo3>u;)c4!DA#5I!XV>LPf3%;-dQ*q>Rn zP&a(iB!^-NM>iSVL)C3zpc`c;T;1CjZoeOE6r;kBf)L>ok5VRMZdvA8Es(x_zId=K zjI(C=rMabSZS*lWdu5pKlL(&S8ppE$AgSVcH`=heC|$KV+Y`Aa>FMC&eIxU@_N4pj zQwevDvsbggv_SkmT5nWH;=cUk0OE^PY!X;J zUa(m=7z=>@Mn$bSdBLZ(n1O#u7p^lW$Yxp8QvSYF=X=TbE96?>4z1VY81jDXY+MG3 z8Bn(3v+IvvmlekwQw#2+cq^`;!~U#(YI%JkQBx=R2S7()_wu%UbsQ=h+i`Q@+zc>D z9;FCYlPe>#e^ksNh&zTo> zX+|RFx=Up^~tS9BrSLOhzU2?P6js!vBM$UEEQu2y2K_ED_oM-wI(`rc&CIB22;+)!f{5oGfgTUpgsR^27tirt z%!b$gTQq_7fSMizkWzOS2LLz^99X2RW2?bnUh}Pb|j068N;mx|6%=DRrfsi`^Tc2gF#0Pjn5lYm}6f-W$Kkftok}_0C{mzYo0cN2Y~zx~y0gP40QpG88j8 z9_!e@f?@e}Na`GUcE2(>h}bwXY99(Mkh3VUW-_g0RlMBy`&*-2ukS`Wy;S0YR}4 zF#{g~WvCMMn}}LrNG=`D{cVmbH1>en3Y{z}krrPz2t+oB5#$##F!nm!1YyB%hmw@o z?Lw2CH<%NW7lrj~lw6^pwmpdfU9J~3b_&83w19^Pyz(<~njrX#j4JNZhoU;Ubq8ew z3x}O*y&D!~tnZPS#u^&`;yA)VAb8t3g33)RQPEl}3_a;vwtm`Eb)l^N!eArx>rvW@ zAYy0tFSYHR(`_&h?5+)18hQsCs7JTwujk`)B?df-!sxu8FcXij0+3hS>ZAAB#&>m0 zGPl-yK_o=l^kTB88K-AgrnQ%t3ZM62yndCC^-k*H5}lK@IU<87ZZ?jl*!4=TxhY@TE82$nl2yD&bRoYOW5mC%Y|?iSyLQ- zD6Y%=kT#&qTq`W>l=tj*PJ6%d0`G2&AcBF>vnLU56TauDLcYD@_^Mg*^MqHoQ-kl) z<VhFdRRFr0E)E_@3As zKiHM5Waj2NHb}Hw9y|hbM3bYRpQYt9!Y)=T40-d~`q`Y?ytYrKpFhvLv_-!3*=@h0 zqoSa|LBuQ zMvf?r$Nmd8G~LPOlPs3SdxZgtN?k|xQJ0PTuU=z_TqUmQ0%WE1>&`kSQUT&Z`l+-F zm#@(Fj^?bdQKJ`}P76ZyIe2KgP$!p7fgNh++7>gmFytNiYnG=QY951(tY$~E{-OkF zAEf`)ZxtHP9;S*8?egdM_#L-pD^6Dx`Hz9mYrxGqJVlzFAT%l~j!;ySG772|UYJl$ z7%0fK6j;4!>Lqk9+l^zaJq5g-h3j0n?#$d;GJV!_`53EblB~+MM8|+F#RZ6|14#Q@ zx#MsiKcIiw;2cjM-}SF44?HfV8-w0~^bc1DBJHDb&I`i0NiYs*0?ML7)bFx(IoTYc zO_Ew{VP4}NJJ~ms?z!j)s!XzkKd+fo^6CMU|EMi6CCF|Tn$rtViLgToj4JMejmth5 ze|`)Hke&DVPsa(Ek^kr(c7f!j9fc<>5nu9CV9l&e9dzxwPvjEWLL2OLknxEoT^WlF z6C*Q(I`-LEn=mE)WFG&X0#2r@Tpx$V9Kwdv(pDto+s| zZ%3S)NKv~uj?Z?5ZK&~SFZ6pzehilH)j_UwVBY)dnS=Q%G?2p}udko9e64UNMzEvo z1TWgT4pw>BaVs;&>(DF0vGNr~LPxV4ORaO%+b9t+AD?Fs16;yM{Ckgt;DeLQ_9)_> zS33FaQ44X)i;g#lMN5_~11s}#MNX&THG{F_e7`kk!7Ssu+qs?XZNQrwe=l=F-0ec? z;^~RtZ7DCR3pgDt0ZQVhWgezszkfSXI8oF3Af>LZ<82zm6^O!dSsilN@mfMd-*_70 z_f(Rg%dy!A>&x8L@*Vok4lq=!o6<1c|4nyGIL)z zOD~t8FlSVCD=RuE`}M3>2Ei0Xy-gv3=3~>8sxo1JOw=#V4!*wc4ZeBSvq_TU@1afQ zWpziM651p{T%MjaSIYNtR@eA>4*{rl3TQr^v>6#4MFG6&WTZEiN$rUw&#@G&c2jx$ zEImEF+o{&~Di2to)jA%QI#!m}1g1unoD_~!Uc9?0-(PDc3BsA3rY;qAGRn6AjxF8v&Z9^3?`q@i*(+yRoMv8nRCfF4%#uQCSnADXHFHt_aS z5EmDRvZwEmla1v{?KdCI&wqT!O&pydeKC2}x-%Adf7xI-r)QMYvFthZJ8FjPXArfF z){6ro9&Cqa(Ck}>jNS{z5wB?q>~E>3lFk+ue}R#3Bdl+!6^3|Un8QEG~r6JV(IX@-Fez&DoC(U zHJP3j&Z+ga06;&p#*#QRLQ5fKvr0u9&GBHsvG1c`qg_&Tq0!RN_`sf8RaJE>+XgLV zH2crJM+a%UtBmF!59bnAR`7-9j*M<)O8&X= z#*h@XpkG}!=CM`BKlYpBH?fGCe6y_cE~em#?*FWDKq#_dctPcK^vMu$ydpc;cO4sb z)RD{V43s*3>5PV6V}lEyOKk`}DP`mLg>&!P87CQq2=yKzqbY=V*W93k`RMBdQkkyj zF-oh|Uc=?u6Gaf9sR@Xc0s-ZK?V9|B8ecDw*mqI2A%X=fhhEL5ZPJx)JLOhocSA{Y zN`3teaq%x(cchqvI%Yre_uWs@J=nG^m?U~mi-`z5Y@ZN=hiD~U>(J)BOBS-J~T&4y6pREYjusH#d;GNZv>59&U;!z2?^;YYT9 zm+#hAK-lQ0dA}TZ*=}ya0i2Tb#msw9L2|DPO~Gi`JMJzi!!n@>;#+mD)DhWihLsjD z*;WT|&@nfL(D*)<@4mL9Nw!ki`4a)fB2=t82reP$$7ZmDKZ|}x%{J&^2r>~APQ?L* zB_a&w;B%OX?{I>nu9n@#FAtX*oIAL91h<-<+Kjwtti5f$&F+Nd4fKpk=j`MLy$rp8 zWp`!|@Q-tRpYCJ|;^P0<_$^%nu?V%T>Z#kmgAHrqp1%y(BTawA=^5ElSi!~Ozn<-| z03Yl)J;!;zM+!!aia`n5}h$^=#- z9=(Q)grKXL$TEir@@L*=>3L4G|4Vq@OH4?=(381PS;d2A%?ue>vMc3MWy>WSPmzZf zEEe0UcRJwN2bo0>Sji9s$&y47$)XjclwUBX0FodvUv}$4PD?}C3htd#amTVvuD(QC zy^Rt_dl(z@^qryBSKR92aDyqJr5N;$jk5 zCLdV-KGdkpvi9lNnWZ|8;#`ki220qLM z+ILuk9&Tj|FUSLcgmnJX;}ZxDTE9tu@s$2_>I=lFgs9b1j*PQ;;$C$1P4q>z=XOnr zT~Bpai^S-OBU@Hts23*8L?6idu|ZVZfHHZ38F-Aj-AEf!@3;O3mE)!;A+pnjQ}dQJ%~i2hsh-?%yBD?<^S z9zzMQmjKC=6%Q%#*|=r;pS#4c+HgYP@d@2PV8G_?WdBS5UqB-wHy2zmYlYXru)Z<1dB&R0*=SRf;O*JvQpr)?dpL|IfX7rEO|*27x)brU#C#3tDU8V%U>mId9g@Ich?439JuCn;U} zSf-b+-5IIkA6*jQSg>oga9`b3Jw0fHa9Ln`Xj?GY=BWNdKwL z8CrnoHC$-E7MwV4-e_z&k8V4gt=VUU?lE;8?AYWKx7!`D&4Ny3^^sl$k{PV+kM=x>UY0+|6e+aaL-EQCqVg;4&1J|~C84`K% z#U%1kH2H3ZBL5=dH#vEDQbUNyj^t}Oq)Z5|bI?09w@Bf5-nmW>0+epmrsN-go2>JJmIf?UuQiXW!3M3PwZOX_nWvg~ zcbYl1l;n6cP!w}9y6oHLstJ=R&W4mpS@awtFA4{THP!Ps;uZ&ZmS60vx?-sYe|JwT z{OZRFE8NM3#$+IFXks9PbPE)K)`IuOJeCPIN;Evo3X)CkRb?4z7BzBEA_I;m9zt0` zc%Zc55Bq!oB#AW!14Btt7i6ESp!BNS>hUVla*i@+zx~pFV)_H|5R`NDGS&)6?Gsj| zOQ)fx;C23Hb4lH47&>_dtApd9*qcbI3p69@uco;2`3OPiNp#mxwmFYA%A4YV-MKWY zM((1Rbjs-t&%=)Lf+4TI@%m z3%8h?5?me+0ao`W>yFDhL*mV1stdd0dGmzvaW1rC5En?k#c{c7C*Zp0>16HE2ipVa zYqN%2uC;3^FT~6D6yPcUQ|#eh8Tq~^_YN!Kmq^4Nt&fqjv!_N}O$Z;kR5g;~%VMWQ z%Dj&dZMEK6%$b-&CB}t_<#3+nljb6~k(-;FTxoIY?J3}i_Df$@b=(c<_VxEgL`FWH z@;nyKI;_)ZAl}+-9Ys7sL{M)QU%UGaVYqK z=2g7BsTF7KmcCXp$>?giC3z zXHi+H8yQ)Lro)Gj&Ay*~!X>DLT=Ge;v2bon0LS4tiRo#&;vtKqdh#PBEp45>BNY~^ z6?*-`A6Wn*%m>0*Z1Y0S*PnciO!yp3-f-PL6TV~YXKEVo`$EbBb1*t{<#4IX14Ai7 z=IJ-j(Mw7SN~)VNeB-!opmrg=w6q-YP1rKnpHYmQvaF9}g-ST0SkQiX!i?8qrHhT9 zlwm+s%lU}2Jp0N!6;W1Svj=nDo@e+ZhKrDTNs{J^#)4_xDNR4jY< z(*iXz>1kTx>G-VhD zCi%_8#6)*D-%rC<#44=R@A9kGDu^EIzB6I-BMxPMl`0V|831#GOG)}qdV4+hM9*-~ zikQq@k{k2#FO<^aHCUq(2bGz7qH?-Mq{w@}BWw7;So*=yggS*8LUw}1b#e;N)s<7q zc0MK~WTH&2-#XJKeMpuEy%TR^10{l0`>Q5OpFkxMuSZZSR%HF_ij;2JaT@UMl@Jk91p`@<#ZE(T? z6`d{D2wvLN!IIEPYR7|bk-HGa8`!;E<&4@Hq!iIpD)KSe=s z2#fYFO78T*VTiPpVu~>OI}y|Gi%6XQNf(DeO&!K?<^i?yi=&m7fi5>mb@}8S)qUB! zMd${Q*SpUf0#|A1nlwkoT-jBUMWy}zDz_Qs*TyFbqlKa^_4P}QGbw@xfV5B*z}0r? zrv>qfi)Z~{9^F!04)VnT=}Kj8t5vKxJ6g^*&T=g;3paN?H^9RPVvXT^CoU>F4?KR9 zlxh)qJ6plm;L_UG)@7##+C2*M`E3qd{zc|!{=UXM5izmSTGwLp(z5?FJA=8q^AZ^V zxJiJ%xw*MBp5HJCMnwJ4VZUjw_@FwQDleElstPlU6$AdKFr8}0GDumCTMghJk$|O7 z2bjDLIyQHyokujsYbA04k;(twTt<(iiUSQKOVST3(5aY1wr* z)^yeK{Z+5Et`2}~dKV6yQMxvTD9qXc-;WT(Z9CoXh%OsbXX)cNb?SCDEx zvSv&WhoBXT&WsXM@`^ZlULJR`)J0JVn^Ckp4!rh4qlnc^c&f^dE1pDN8QZq9Px_go z`%rLwJ1sxe*BaeBkt^aQ0Aan@ZH5kt1QglKaX-f62i6#xJ$i~F5>T->N@lU_+Qbke z=gqRMYl#9mZvNCy=PjH=#Wfc$lrNcGpkm(VpL${HTkTsE zIqLiESW2E+)#ZD)FH)+}yg!~d z2AzR0dX~hEF=>t%fW5%LddgxAwY$$A&5^$Ed``c;effDKa*dAeKXUimh<)>?N0Fy1 z*=y6XVPyl3hU}9!bWgjOt5+sOiYgiI^0PLz$d)T_wW`z9F)cc5_YGY|#uO!*MxmA3U7u zd7xfUG%Q+8@?JWwF1^6^lC$L*VhvPP#9eC+4=@w3D~* zF8=1t8?pFjptG|xz%B*CYXMbF55h7fNSL_J2I`M;SQSqXs{3P z<>$AI@tq?T=kLRtKMOl~Qotb$@-E)J7NJ68KPpGi>oVey0-jb=1E{o57sUDZ3N$SL z`Jg~Pl>~j$Ywbb5<3gV$o;E)tV?JOBa0>vW=D%qznU%wHcO{B%lMaq+0ZU)Cr2m&l z20kP9|C>uHxVgCrw3~Fq1McUYpZcFZ(oxV*(BKp`P9N#8lO8A@2cs9D5D`r)yc!DA zo+k07W~e5w&z@lae(|hQym4*EmZ8uTq59k9R~;i!x&1;_MuM$* ztZIsCvfnK#t`n`=M=(5Vew?S?+i48=s?J#Er85NWF1=Q?YzBns?h> z9Mi__kTF28YD7UE2OHFmCGSTejZzpy1ZGo|8H{ zJz8wuY_(|`xASPn)~0^XoeI=I&uTW@{pJaf)Q%R;S_opx;dQfv7YL)JsJ*gcdIsm@ zIr@q8Szkq4dhK-g<>~euT;tMzc-O6rKl}C)xhw*q&B@QS_mmiRJM&s~TaS;9W*E=c zv*lNrd(Lf;A^!Oo7I)|h zz}hZ?orF$^y<`aFjekNo$~W^QyA@0_7hq zE30*GJMXomFD^?PwDKfxy3cr0IJQ{wKmWNtF-JoSl_U5#F!D$B6W33ri_F7kMMBH; zN2UACbcxWgg(MmH}7-n=d^trgCUfxsbXAV%#nL=19d zcNb79)zsALph!1;)b&eX;=LEZupjJvT}mxJsT@*-&C8V#TBE`hhZksuu3g6Kkp@JW~7e zk)^oSvNo0vqppgWk$yoEMM=TTq_6!KZEXV^=rC~>s^|BD5JpfxB}Ygl=#T<&N&ah8 zdhrdeW=8;G#dL6GA;Jc_8QY9x0`oGOR_p>F_KlaC%q2qjRL~t_A)U7IX&#TiI=g=o zY0TKWjYS0s9qlPLl~AYZbGmA&n6zVKeB2V)GPFjvTyHEIlXkQuZ8h3%kh$LYe^D#Z zHQnYtKn+4-0$sfY$h*be>_L&bKEH5_+ts`F=3RSy@w(b+`^0+VMucS{T&C~nV9{mi zs`}#xG6lOj;G?glk@jd8;A7{&hrrdRRkTA>cK!d3B&Z+9up8G(rGDIy55gq7k62gl2Ci^TAmR7FC)uwDrnFeNsQGOKVSJ9z} zU{=P7W~1vXcCT<4n<1AI@u(XAvb1N$|8-@AyhlzkW4$o#=g}{2SNrTG%y<&5NA4`d zqRWbN;N6l7%@PHhrBP`cA?Op$4Iol!=8MB>5vrsjI(a+KriiZm*@w0Im~SP)zjdSyJc52GH4inhpg2xWXo&OT!F*rcRT0KEthuL22n} zDF|>(BtGpBa-d#CeC+Rx1&LzX3s{|5PdpQ{d0$FP>ZaJ;0%pLLs+Q_{fhlvn;g#<@ zc7-N$3@GNT7xDmtjt~1aKSpCBd3d5q%HJiY+~gxZa|>P&1zd=c5I|<2OJQ;!ilCJm z5Su~&z9g(uopVlQhQ8`l!eM}omb@+N2OP_f97RNyGF2#?z$H4D-#bG7hq${$cqTWGr$pPd6Ck{#b8;26=mW)iDsqF8^|k0n-#n!u*E z9D(uO%Gx+!Yj$vN?>}P9n%mp;W|tYM+uQw;ot>o>o73fU1&>5=710{MyIoKG zL0%wR)?)qC&Z-hrJf+n92xK?`cacc0gtjS<#WGTlkiOG0rMpTt_CxHT%)y%W5~Zt< zLuqrn$H;co zp+Cvtvn9-Qx+K>0OQ8Q>Cb|y(&RNEm z0#`FinL%NCZPKQQ(ForI@v){^z1JqUugzuB5P{ftJ+E#nus~8rt1$N|C7UiKuyvn} zLTq7FRLW2zEhUa+AXW%HR!H3ytk%p*e!0PGZIjNAhVT&dQ-?jEfOZeKBmgiWGXM2o zcmW|kQBj+U%-0bunc)_J@0r zc-Dc%wUNHNT|RA>fa+pyY3pM1NzjagNHMC{#Pv|4hcj$oJ_Vtnf->_=Rs`~xpBMr)zFG)mUCZJ9Z~ZmKhaAdDo1@9V(?nvxX+sz0SIE{`Zy zCNIfNnN^L7D&Ks8pMWLjpk4}wLZM)Gwwo=d5|y00k!-y+?v8PPgZ#>4zStq1KVPY=uOC8d@3S3i7e-aj{96jjPzAXN8ry!F&6>bhCf$_P6ov@BiYz4INKI z9ZOI9UvP2Fd)@@^ue4uDv1sP4HrPM04(vSUcjN_bD(ETe(i1-JE+ALVHu}W}m^O>z zG-qst5@@7u7xEv>;OOI$ci@&uK?mH;^Ax(XKZG}Hug4u5%JEQ8o8tu}B(f6*J^Ei_ zoAPD(**62fO5=ue(O(-E8~|xpZv&MLJ!zNOQGqe3^si-Ur+1PVkr^;g`|H4FL`d1L zah{c=iE-FHf6bB+yjO~GQODax-pd+Oyh#r(S&~yA%6t)eh*jBdpL)F2<6yq2W+!7I8RV!i=|VGp7B#T_TVje&$p!{muo7+c{CnZr zWKc2Zzm~8$Zv@4fTC{-!3FcG?AVLDGY8#j>(o_UijZ-~jGx9o~_>17?_gUGR6RQ+N zI}&h&dtD?&1xb_$89aMfQDVf=M=Z z5iIyqeeXG>2PwR02t&6^}!fySIB*PQ|H3D-twR{XcD;o@WOc)2;Se6ww zx3(H^r~A&0YFgI1(6HbY`P5m}Tu6b#${o6jDjX`h5K=tkZI~*|~p;uwMo;)qe>k_v9)WsePf5 z7w=_IqUB}kRS@q4wGOdWV>Tp;+9X`uoHn`tFrYf}r3y>Qu}b6W zjKK`ST1EOP z{US6WyGUYgOn8xsj2dXz0Yaug*7vQ#qRrowQ}bAesvf+bHRg56Yd||c>Oek1o9l=QTQ^a1Hc^w(CmS18tubj$QAb(!fkbJYHe+m z6y%?JUSw6HcOXI(7I9j00pS^H?fw7VCkM?#=yQv{`6uzt0b&V5>Z9Gs4#NnGg=$Ml zl=`AhZEk9`m5BKAct+IS*Qm`lsD)=pGXPp3eV)P2)3_Y8`z5PO0H#Lb*Q{AhJKlJ4EwJC ztY2JCKT<8bg&b^#ayTE~_@HYW^GPw8s~h{LJXn4qlq=^&OwWUGW(DRu_0uZpWl|#p z=l@)H!ilgR=c+vxBu8Cc^I7sf7m}lUt$33s2_sZSy2;Lb_2Fsz&FzOQs(I4hz|iv7 zJI|4+2Z&BwwLRUQ+4jA8t5l8G9+ zOcadOIV`j9TK-;Mm^qVkod(#_xqCiZT8RV6U{auAIx;X-3 zpaPSX!|St$xH;~6MJy^7Nxpvy=q2rUv9huTt~&q9C`k$_`3=_(!>a!mv|(^qs&;7k zo34UcPe>+8F{UG$hU5)qInE6&Nxt9Q`nSiwbQOkSMW zJioh)qcDrOa8 ztZG+))q$aI08UquSOwEp%NZ*08f3FRESX|G(67vHt0#|z0$MRjuZjkOh4140Gz^PNPpXl8cH z=kxh85o52O$iVZ^z*83E@`L%US=9<2gDCAQQJgWM9c+HZfkfYco9jvQGjg8Ce{ZE* zUl5?v_7O%Cu+?xHhv(SQmoux3YzJjNjZav5+sw@V`@yucG_Ys)f|F4L$JNlsO(TYF zno!UGm-HXo(6zsJXx6cA!FWBq%o?xpZh-()ZcYxTQYkg~kToVnr=r!hcKYyRH$Qvi zdbiF69gehGvcjitsh4U2fN1mkeP82#^F>uN@+Qr3q5g0_-f^jXpGq%8S*z_z8jXV3 z7FK(L|DxxUqrb+d4evaHR!A8T&yr9p%57G+(E#_S42kMUTBjn=j<`dm@#8m{!Fgi- zP_`z3?k927xCz|$q^iOU*eUORsuO|Uf&aYze>9ziTT}1D#z%@0(ycVo9izJjh=g=^ z!$_s2q!k!lf(%4Tx>LG)zz8L!yX!sQ_qu-m19-u9&hy;yxe?5*d@I_xZpw&p_Y&Ak zEIw?81iskc%qKRbRos{H?Wovux-93qxU2Vukm$Ty&3MB-v7^?rVY5d*=}d-X+IWBa z24psG`Ni(;ZeL{jvJe$!J~LaIs@AS)WrP3Kp=(1+Rh0$pu(&TVJ%m+#JN>9m7h&u< z+Om34@g^^zz_gT4e*Elp&_<%!eWKCZ97&*`=z^>_p`qCG_HW;2P?E>-lW9Ph7#Ijz z++~b|L%OB_r~|gVJm$BkDC|V#5}1}21?Tk0z}p6=Q&gDh1sD}0lQ*Fg>uWW`-2E^` z14kkww8F}Hu7jI*bOiRS9G@VH;yFna`9=BfkM9B`p@|fOJHh29t8KNmh_56ADCo%^(2a+#gU9n1sf@IIgBm14= zvb)2HFmedx&*#z)FB3r7rgx14v91*n*x!Sz8!leqqfQX75sOC z+}F_Cpy;T_n2r}OcLrSsaS5zk6* zF-~-hZy>C9zuy;q5#Ezk8SJ(B^)pdy#6~qp_3$_ZeE&J)cT+hIt`jTOzXMs9>DD)K zClI)^F`c$E3${8}ySZnIXwIRZm5IvcO>P$@640KJnaV0D^oiXbCTVnsi*>P4aR;tY zsEkv}=B^?tA0DVFjV&&ThRdX4r~!%f)&Uf^2S6R2EdCb2ssVxs9b{8$kh`hE2?(JW zWn;^4hCUkz5cbbY((T^t*+;yNtID-hY=qg79Ut!9n-J zfYU%l6%Pmc_EQsE5~apq`g^my|HRlsk$IU{l^s4Y}oyY#>0Ll zIFTtadarq%i0F&dv?GZoqG8bOZKI9}iZ2#E&kL z-*672st1+)7xh>=WpKL#()hseM)2QDv6Tm48EQ4~$^=L<+)q8-J9YS+r|5an_Oqqf zP-y7w8m>cHZq`b)_biUDt-h>2J-)oZ{<7LJ6_p!&m{D>$YDR%a^?gN}b*M<8s}}6w+1;U8#{^O)Y^rZB+A;Q7?mzKW*pwkICOMpnX4MEmzmCT?e0?kzF0F+F9-jCY~hidx=V$8>H^} z<-8>$=sXU#>{C>=rnM_!Bv`D!?s*fmwt8P0Jbma@2h-M01_~|oAwW5D`_TU|Cp~wi zDzJF2c|Waos{P1CKnOO6`M_5qdFH6CD3<0c2y#!grI|IP%0(8ij!uW#BQrE%VLDaVlZ_z);@R_A!(pt=JzpsGy8!?;S}C5M{h!Xh{YY1r<$}zLA-ciHXRE0&Tpz4MfB(K?S_5}@!9-mkXN$;Bsc#)~mf~lPq)DLV!l6Uf8wv440%Oa*?_!OY zV;zqOGw|)yWW#dM-D=?8(P7|tTgRDE$63eymfK@*n3>6ysHJn+a5ycf^sys&lYJd& z8+3d2G-~&>^G3+<^ltUewM|P?g??h9$T$#tuuNyhT8N99H8&IQ8_Yt-%x7{V`At7@ zB!?bP;{)E~pyGWq>*IK}`~_gO zV*~g~aabsm07Cr8kRTtsZJ9;xHiIR7GPMl<`_llor+Hw&n}!pjqldUjV-I^-xUaU4 z%uCUMYejpyenSt-GE8Mz8#N-ChG#>kbhy$Q|A_z5QbE?kVFLt-teonPXTW!^XC{2_ zBT;g)EEY$min=;X7ee{sEkj?K@Z9?(>9=HP90>*Bvv2ON>@1yIw=ZdtJlam$KL4D; za!m`A2&&^cAMhYg%U)yzUzuINFFfbvR5rm0T51jtKn3h?PxN{N@N|;8(3TPu5y|L~ zb8r!VhP?qlqRl4fun*rH6+FbQ1ptPyO8(*lK?y zqo&2@fQ|Bx8bq#K**$TP#6b)=ayco2zEObC@U?18C&YatokRphs#+U2=7sDJ{0@QS z(yHfbV4JJ2)%Pxn;QOln*Zv!m7Z(?153^<@*>`Y48;f*iruQqBV+K_}wIgV|;!H9*Y>^|N9Cd$TpZLu8zJP0LH% zAgdnKp@iU6xs=`(HDfwk%Yre6W@4u3Y8NGTSz;}%mSsUH!UX)s$Wj+;9`(-aEgzd| zR-ERDzf89lUiqJ*sQd1cXi6;JxPc}hs!1YJT=vq^jJ1uMuvY{1n5!3cnx?la#BnHu z8)AGl5jI+WS#P9cFha*7!NI)4btsC3Iu6u9S;zct-nsyEsDjR%YIaCGuFeNHyh*k&j{5jJxq*uMce`()Mw6w+fU-JfGDK- z%$7PGY1D}K@-vzMAS4?Gz=YlsMh__O_>~{}!+B!Xc(_s^%FV{*@7Yt7+8>Vd<&@el z>)dk0Pj0#IySeX&fy(Jipt%PiYG|52Ja(UU(=((mN&4RfUbWxl`r9%0ZEqpkudaRZ z2Q2kZ?+Xz^+wY<5t)V{T)SpnksWEa#t40-vQGL{GqLNT7bYO2tZjIEdNWJQ?us?U< z3hxRr$B*WTfV0ZVGMxX56Cimp&k15FkK1rO6jT!8+i+MUz|1&{izZ?VO^j-6uP2Sn zH#@axjB`fJnhO}+Cq2An_H2X}5QZ|Kl7+_Bu9bXM%%m`S{8LJR=}{?~AXg7GJH~f2 zIO2*1MV?qd;TC`y{(QI|AQh~GH$8^HjE%>}2*OksI?yjjFm1it>efd|w0&>?IzwE( zTV{DubO|H<}d0j!yG83zCxH3cjsvrcf2XyvRodpmEZP`#qSe~DGLM3_sDcxMv3_ELs;bGdS;xCen#RUPd3kaF<+bka z^Yb`-dKE&ta1|9t-9$yyi3A4GoC@8I8@TfV1BjO{$@H7$f}xp_nMv#6ahKzQ|K?!v zZ+^fnyLDj4sy{$>fPJBXHk|(ywUddS_NVi>QrOVV4GIiP%F8%{@BRXYmPxs;j^mG6 zr3#7DA@=?_vqYvc9vm1O5&R6$VK&)iGCdGAt+A zAT_^!?V?F^SdSei=jqFUucfPT6W|weytZwQH_U!0cgKoD;-KO0oWmD(8HaxS$@o-( zljDZhSTIy_QH1t*+BUAzwtU| zBOxVIVQilMxTL-BLaKl|}J}#fF_JhTd{VFls=AXh|!nx?-5_BPaQrLa+)g z6|L>_X+>aJgj|~mrRKMD&HhTiDmnc#;%wgBpgfmBgMB*qyp$C+SW#2nkciTlB90O* zDbafM7U>rJ@Apf5^7o82s#Bc=xwxIujR>ZV8R~{_Bjl(LB`MK6u(3XB(a|g3Sc9U^ z1e3@9o#sP7EHTVMT$IX8@6!#V!sum#Ft=$U4S&rNkFiB(d1QSFIAR)-!Wqkq9K>yj zkN+{Uc|Ot}u==KeW<$n24minc7r%WN;(m&G&b49u=UXr(+5LiDo`I3s-TarpvSr7N zeYQQP_#`t@W7yJ+QKAt&-uJgP%l7r{7ZY-odEz%q+T%Ik%UiqP+Zx1c(SYCHh{gIr zNz1savPnmYbG=}uMiU=r`f{CY^yO^z7!SE2CZqV06BwP+MB#Hk88H7kDoJsFh&W|r zq8XbF$PqKZxhjxOuB7k}y%}X>+RO(S-Y7kU)Al=jc67^usB+r10R?k^h9{l7lX@Wv z3kQ6T`zq3y|4ZeRA<0gLcC-4htUM7O#tX1jj|c-Hty;;yR>W~8?XFl{oA$TzK|LI)V}yeLE2ys_A0@8$wto9qc zcGVTjN>n;^MT{^O9a*rGx1KeO$JVUSIbGrDJ!_@K0-Cn#;3)mlS#mo=fY<;)kjKxm zMs+8!V@T}lGruvu<1^q^Z=4=&n#L|yY_0y0Roa#UZ#G*Hk~|@lsa$ycv+O&<%|*Rb zqXB~ejoPNk83=%F?pqkyuT{ZyT*48}!J}s@|NgBwMHg}rYMMJ*T~+lLEVQnsViU&@ zt){FEe9F8;>(>Q@4Rep+?|{J~o<0H_%@DQmTG&gHnpbF@jRF>$bFKGHb(?=bvZ``# z?$E_l4!BPnf7M#}tg{lTGo{|ZNJMRW187-p4OuD$_!mn{OEVi6Uki4;qDc#}QTe*j zM-p&G*!gGdcp!K*CV*uy*Nof?+EFzF@Hf=tPm^l8XA?YuHo5!T^-+$4ZOUMX?_0No%z^BJxu0wIMxdypHAQ zAnxkLC)%MqYV-5Kw5u;DLga+=#=y+9Hu`JKwrO(ubz=f1NI_9}Eu)q+WmL#0H-{^- zt$~P%YP9bP=t#VB>oEDmkp^jVeTGfMA=UHitLX*=;u_lAz4t6Dqnogo|03pi(b*4I zY9gd#;pO*)=D%6zOlcND?zW7+fq|9w8EwvimHXAIutfT~_icuD;~)31`2y}w`d5SQ zkP@rdVeU22eX#Ak;70)dqpQ_ifem6>t<){ubJ{ZtJmvlnPwcFiH64WFv&JJ;ZayqSMRJnl4A^AU zA!K{wLIynU+G;5TWuG1OHyWDNx`g@iu%9V&rQuL};&fp}fUL#$F;`{fVQw!zc6N4} z%I-u)VyF|xvYKMsexf6@Sh9>s&o?kJ8NC`L#%3C&5WX42d1racud$~;a~XdsMJWO2 zl>Ggz6Ox{vRQSBp+r$g%Z4<#e=Paa=BYIH>Y$-#xz;A12+YaiDB(IvrBP<5UU;gHi z@CFmrkCH3NGo}2YPmG35$y=qf;X#XbQ6DE)*BYYAD7!!3nlw4^5?AHmD8jqUKp>c| zxN04|PV&Ps?bF;UD?B~}1YC&v^5uXUvvr({vjBLz+^5u-&F+gV;v(AI+q+JOD=b?0 z%_x59#fu--L65LvqNe6rUf=eHdM%G^Y|rg9QNOzcs8}kpxM(le_m^?j0@u%qQtJL{ z7lbFkT?C4N0oTXC*nw-`fV=p|cDIGg1y4J5oE+wLMm+Y?QXtfCtxrd;ZJ6cm%+Ct6 z+YwZInLo(FRp?C+I)MXBQJcwNgn6t*h8jO^8Z+mjW0R@lXEwi4(*pC<1$tjYJ_b{- z1G_N0k4cOdemT67BD{i9!IBAUhTi-4H`s1>iBxL;;uEVd^Wi8gZxbQC{5_T&I?UM@ zNTrA#-6nfzG_&WJEVl^N=C>|(8 z6?*F-!)dY;N7uF2ufc@f5``+(7hRbWwXcJ3}ki4mz>a(PA1f_cfpm$5%?ehb0-e&-* zj2=xooff@sDskD~IlIICZ(o)xa5l^ws1Ii<)BEx&nNP%PR1m0^#;#ueY2ujS1agNq;C0RbONFAts` zhoM7ccvQUNchkyoH4QSeasfsaOXMZZ(+SOT;JAbZ{-s|RaIE&ILkEfFtX$7uU-xXp z3om^*-PnsnGjlgm__?4Tc(a4>vbvaSYw5>$`!?aK;JAq>)&y@8;_U+6T$KBq8B1o8 zgN1OcsJzQ1>#wpgfRwcPUj_X^qrnR8Y*isb`Nr(GwP4qN-5YFHo~lkQYL!h#MMZNr zqY*fC)%w3V={l`5qTog)aTbo9TNvGQC~V!bj)v zfvpNa_@1?~CJ50|zbxw`z$^;wc*ZyKpe=AQ>qoSyrN5o7Y`~U=o1kAhH;0(3`BCY` zwG}HQn6UB3z5~xs8ynM(n51ZFQ*^+1;=t=o|C`LwBBt<>fKZBpgJxK?u>kou{uzG} zu|Vb&2eblqxBx4scVyAG=m@sMz!Lk?TZ7jlym2_5Rv%v+QD750fcyYC=JIe^LPAip z1eQfL%E3^u0xJ@%QKHp0;RLas$9pQ)TQ1+pO2>+jpuChGPM zx5AcIF+}~kt+INxOXPWWrdjsUM(vFR-ivrWkm{&g5fSJvnuuGNc;O#G;+$KgLpWc5 zNVj+ZHe+$AjIh$Ie{*|ji7qXC+!i{tRSG-c@_F+`KW!G zMIpVZAmd=%*b`vPQMomC{qt0zMmSy2#^w6$vck_-+WY}!a>cO@yxne=X{I03JyGwB z@JRx5rIlKcqskB~YU0z8$pbbes#Td*DSF$bqSbdT)?)UTn#qw~zbY!|fQv)AR$yi< zar>GY+h8J@_dq_?s$%!mBvxA*yx+3rl%B@i*J33GOA&GF~_hVXPUW zzMjD)?=&Cd0rk!Q`XK6&OX8bafCct=OItAaQ>P;C%a?}C^0V#neignJm&=X84&Hf(Xs}&VpDtqAXM=--j4GEj;LfJ6S``2E+-;FU zpw{a(Rhq{ECCTefG>|*M@q$6%t_|DZLx5g;It7WGTI&i!v_9UR?9VGA>w)*sUYs-1 zv=!Xo>*(R^$Ly7#CC#7)94agk?Gm4_oX>{l))sf85nqltQ-p+s))-T@az(v&yPnZx zXJ-TZuXg|lh*+WT_f}6dQ8LkbAGekYQ;JY^#I`?3I+J5qON)zx>o(W-bVCKfA+5kF}^EYif{uCPTw7{V5sYCnX5QYpvnm-D{yR2oWa-N5br$ zXkA!7mpjb%AyU~K$_&Q|8&E6?Q+9Xt>;j?F5)QH)V*CYCHr?Pw`?_v$<`2YT`DL#w zq8!$6w9;EhYRHa)J*rszs%OMT?L)zmX4z1qd6X7^BWmf!%kr=J8j6bY*|BR^Zd+9) zZw5)IsT5JdIL8#)c0z`?vOCuUXe1PjM7px~F#{^MoD1VB{UzDp-# zidLhZjU|fs=S4-#L=;XRGhC^}C#C~aqE3nV<&br1jN3rojY^-4;5*Aj)6!4-aK~ zD^%~km8*?s$}|PPYa#!&ZUfbP*BZO+vwPm$C1iZ7afZwPixN_=T?YOgy9l#9qUgsfGO4O;|)kTqJFV_^SPwrt> zK!HYvZN($M& zD^Y0{OXXBNZt_pLZXU&L=?Fh$T|lf{u|K#(FgOTWB~qvU-VsWz76|xDA*3;FswX=1sqqGuF zuqt6(-|t=uXeSr)^}Y;Dy=teG;$zIQ6j8Vu8phS75j_aQUuc;pN$`iN5U28*4!H@m zD6*@^k#8rgvymWEryYF->C*K@mfd7AH|Rm1nBj~>q1b)+B64G0)mq%B^b;{r%7fxf zOk3M%#(cbOJL(DKUjBuE&GR2pv~CtvnXVJw6_@6^vQK| z?*tejLnDiCRd84X)1gA@<@Z@%Zj8GQjwC^ag0h*l^m;mAB~%^eS-B(@rl;FIv#Wq$ z56hvm2OuujY&v-59XVuf+pnaqLe6W=7Xv2iZg3uU6r_}C&@0vP*+)JD#pN?`sE8&F z?$3DrNKz~&dPeu6Z`8w9&ExfVj80B;)Gp9FC&tEm#%sE?R(@gR;)-?C&zxTaJi!rP zXl`77dg0FG2lwrj%4>{@lH4hkR^ zj=lc?cs&~%3;f$F7Ztu2%@Q3tmTK9e-na8rdTwnO^|P~e#d$%ue?Qasm|T5&_8P0( z{rhAi0NZ(+EnfHQ*LC!l;O^?|;@lvJO2j~O8$ePru#`LxbaQm%!1L?>(&4urk5Jc8 z=NK_NmOxxdAnVt_eO>(QtL|$3)^jszMq^`*bedE5B`HEx>WZ!PE~PqL-eRw;%K$bQ zA8Xo>)1DK<-SOwHOX-La__3Thbdf`4yCT(AYUGtQZp}(3o&nFqp5_>i-`992&^vMi zphVLgxyJmNcfTO6@%><-TuwjfOBHyp6Q(PNbZ@Vs3lpL9dqoqGkX+OduBf{kVfx}w z2!lp!f<_oNiRaiVbsz>N4or76e(S3J>{YoJIl2Wt6V%eca&;vz^^e)p;*-@vy^6wj znA(~=I}?#X2`qTuN~({Ojw2S^l>)Zh@-UnA0X9qrka@m@ zKJ7KNy^jU?X*~`vnl(QTh{rqR@JhaUAk4M0s@@j*79vQ6i||gKucIEm%66^%Ui%`H z7H0#%=&yqp+WfZx-cWQDb^^{iBi9X6KeU@iTEIsOJ!A)XI_&IhZZdUjK=DPpOj=$J zy1t3jAAwQk91izu)DTL!Z$!po53@tv`^%5oPj1l?xP6_S!SDm1m8E;tw3P=w^jV& zpx27D*W07TVNV(Axt6EyuQ00TNF>mS5BP9Av>rR)4Y9z~^Ld-@6t@*6*Fii} zA1Ya{w2#qbnjZZ6hOeB}^E-q{$;JW0J;0eBtJ#A5%1;xYDTGjSI|v?`h7I~8z_2k; zI;(~6rI(f(_N}3+YD}cYzOaLL{V6@-nRgtW9Y|zWbAKg z;*2{{{H@V6h? zQB+Q`YDV~UU#1s=#3x8?vX_C6|q3Z_xp&gG{xlctk znF&FGu=Y-Ixc;acli*qjXo}gk?jqFVki*eHhRO=oDl3(plzQWQ^$YpnB=6#mo~?Ub z6l`(wn5^tZT2YsI5RarrM~htJfh9Qc_5UtHsB&GBbbb><#oRoNZqnE}xxFelK+@ zZCLgzW%fHA1p3Y%-2YGr8LFKL6ejPN6x+1OsDZ*B@@mQ3u3QUf5NAWX^tbxsqHtL9 zQsfQM$jP))v%YyY$IWyK+P`BiC@Cqit!WDE_mR_D+3|X%1RUig??$E(rv4jqMs9mv zVn*IpTzc`;{c7zqQR1)2k`v)zd~5QG=q~ANO~+ZfsmQz&$JCcp_`igbwZ50b9(BUb2@S7dMl0|vP1vI;$*Po zpBu}i(4c}`mlQ^}rPsaRqx6t|NXZ|Xq_Gap`iR!qEz899?gtowoJ5MMx9FzXj;hO6 zU{vm&dXOc-*>WdUxy*d8Ax`W>JOzJifAhV&_ z>!qeTZMw+lRRI6WD<$p+0082DYZv9Sj^ltJsMm2jJ0ARawuR;FyI8R&l;tAtPThZ{ z>bhWJWD>a3OBVmO491pnPa6enha@w~$kU-)Ir|{Xwo?GQub4#JD9B1a(~cOr(}CeD z%x{Ei?>Oc*svZiK8xSb_OW?vB&yvd$Q66r?!&&4px8*d&UV{rZA{4Qx&JYj03n~hr zwdP+8s1zKOU#$O5!V;gL2O%!{7jeP@+6WO>SRe_JooIWj+LtKJCgS5@OZf?h2^GXr zg8vo8%*e2N*+yc{$tD(!QQ#x|nrZ zw*Ht$-EbgYo-t(xb62KFg-Sh?*Y@;`RM2_dEtA<{?&rVU0elKR%&)u$C~^Sr&$?@? z6DFS&LqNUMzztb@TTG_fR9m~`&;UyX!a&K%$qJUAfkz5U?JK>0LKtDFQD|6w;Oeu4 zSa#K@e;>xkskqrGhUKyHYrUvjkb79;RRLdkS6rRFJ`$Uwc0z2v0G{3N#9s1YU&GYC z6zPgQIhwybY{SR<=$<3*9BF!5t+N&1;kzph#mbjtQPNZXtRy%he;;RbAJSQOV|AkN zy^t3|VOO!Z;uq1|s*@D@DA{I%t0a}rJz+*CbVeFBq0ngWX%Pi+lHT`j0iL4>bc26F z!+469Kol&VQKjHtFrYp*06lVC#41=wNRpSg0tNs#fl}(9HUM7_d^TQ71>FP9`P;VD zyNA5%>wc>EIfgG1ujMBLk6MDS4g=l%k>!;gL7`Cz)ROn3pW9J>h~@}^Sm7x;xqIHC zqx{7hDMOD_-S_^0+b{SGPl$4_jqxKu6C$Em^&DSb*r$@#sH5-mWXETqOzrjbBjqm{ zg&v;)W9mFOZjFB<&7LmIjxL)!PN+JdfV`uvtqqXef_X%Et4f|?(}hiC1Da;=t=R`A ztYy0tdIF*cuvDD99ZYA-A&Gqz$D2`pj`Xgn!pNyUo47>`^HVmyPBTMKsztf#7w=wE zeT}t*8CXSLOL#ahg^W65TQ>x&USivr&KMSB#ai7gJun&fXp~{Hn!9OCOq=WW=h?Y-Nx10czPMCW4(oEz1C@5(n*aw@R<>@L{05 z#V99%WSW0mQk9tENM9ZXIRYA!0hT~bd>N3KanL;XvqWo=2eWmvkS5folBoH^Wii1L zS{!_+hvRMpjd*dLxLq`mWCMz`ikdU_b%3xA^iLW;KzH1p4(ZC*bNA~Ob6kMp$bG-m zNB4mJDyfeF-xr*no$q`4gV&I&w?6ooaW!>ybtx&c5V*%t+m&Cg9WgCk83euy9@Ey4 z$QcOd&1FmiVf{kxIfUAlS{ZZ$V}af&b2+X`P9%{sO&g6B-(jdMBgtLMF3O1t@?D(e zwdkJ4xNqfAmulagatm?Osl9&@=C?VcjyBH8_xvj}aEBKymd#}EWcj&(i_WSp!Ka1J z8qE$)l@O~r02CApMtJ1YiQbb9$M34>+xml=>1pj}mq${yPa&X(+P@b3!`YA(I%s4tWdg~A@nEz{9T9ib~xHpD|1JO~nj7+$%W7NPrjlzy>^%*}1D2>_CBN420V#n~xzMtsAQt^ZGvdH7 z)#f>`?_swfun4y;aiySn-F77LTpDl(zBro^ccGFsXwCQ2)3%h zftORn#?;*gk<4)h{MMo9T6dcDrMAcJp5HU5==D2tDTSLFl6VC!o@T~BAlh&N3APU>Uf^t%F$Zg zLEm0E`X_#F|F^3WxsnvBFm`^tHRi*Z)S9GF1q}- z|E!IljLR#`+|jXl)-g}aKb2pXt7TmB2@O&6;89{+E+}N!_U)=0`^{_reIRJwTH9J( zUHu;ld3%eLi!0oMB^=97re#^^G@iL!aCCH(NTzt1tNUxJOFwkV(Kp;?A>rVTX_(N^ z!CIdyC16}mzRw_1CgJ$qUEw^1Z{}?>UI4Xag$RphPcrP?-_}6BHMg0Q&lf1n% z3&#aG|DH>YQ!*niB@Zg{4uWN_>`L$~?DZPB-sMe<Q3NnC zC0mTF0k$3KVv_xjn*DwJc2lWd0?-Q1%DEwWdtnU@Zfby0a@;P#7t?Y8{2>fkW2|z7 zh~-K`X$8`o55Pf9-OjPRIx6&aws8*0$>3qihDm#Hf;J76k)d(G4Ll7nBLGcnJXb7> zQ)Q~~xuPLs{BP%uE1dIEI$Q-l)Qma&H(G292VN<2Fig()xyQf`Yb2=y28i`E?c=(G zJl8X}Pm#$liHp%Q07-yK9>)c7@oM~exB1hf#5Q;Kvk6V|$WJuSEU3fT5}N48t6z;7 zuU3JTbTMbnuss1N?>L7wdPpZeIbRkheP=91w=RVhfzE5B?LOXa*Wwo1}ZT87i8fVt8bVTd>P$ykT z7Vb;H=H{}svvajUhOYdLS+=Mo1%I?^aP~fe)4X>!q?}L7AmhF(@G{7V=AEayvF6z= z*uDXBj+xonF$Imwhd!bb^x3(&3pIADZVz*AQa3<|G$D^RRJ(X=$^R&jjGODf6%6*Y zVJHV+%s(xYJuYAO^jy!+U*+Al=ZHB3_@=wuBmeopx31uQTAt0CCd%sSVLkFPek<*6L0$U`9LEGZBWEgX1z_b0RmcmYpO zB_PiZ2=I9~BT{z{t523mj2cpB=eWX4#~MMWdE$5bHJZdG#`@CMJor|k#H&r4UIiSR zD8rrx>FMM`6Hl2~UXIN}t}~fYC|`vh5APiTY`^(n;-uxEGxR|V=O=ge7ZKV(61Ne& z1Hje9snYpQdMd#m8BRq3dsw` zW+VxbowOYVJbPjEQ)KowcDfb@OyI_y@Zf;gE4C?r=Z&>aqm$(TbQ}b*FNj3w>J7uE zuIKfs`>ifS9n>LooxUAs%Q1W2dUNa}jsldxoI>|WvU8=HpG;st97C!q+_ZAr&$LIM zIFkAub2VZoeK8|Q=RfRGVJ(L$1PUeqCk zC~-S71U3kHY@!}5@lkuCdfCGpj13QPye zlpak&B}3IiKTYC*Oa468G}X+1c~4hQ;wl^cQYR;3tAWMqU|{s`etGp_S>tg-V@ZREnqA>$j7FFm|!U2hCxO#4ma$M>uEI!`0$v6?{uY@u3Xu{*5!_^e) zIj;TgqnT$j)n$w-0APonXq|Qs$btF&ZtRb+_;-hA$8qs}esLL{K*S2-G=R!Tv7YV1 z+td(wE9@qkeq)96-khaCDChQ@V3@_xEWuH++SdabskjqbtkT(C{XqJJ+U+!gpW9;< zf>dj_d#*-}AF4JEQB-}!O!Y6gO!u#SK06p28J67^kO^pX+@4O7CHSRkv+D(p`eOj{>g#yYx`e zQ2F03*gfx}wbW9fqK5R!phqg2hZE;TQe#;gyrc&?l*3?JOpr@JrTY!L&?U?9jf}NI z@+C@Jx5gOQPK-}#uM=6K3rI)L+sto;kTQ&WWX6Lnx;=&f;Z1wm3ZCS7kQi33tGbt7 z3-{*W;P_}$1wnY|zoYZPgsPVkIOxd)NH%VZ;e*3>H09P8vFg?+uf+3L#Efdpo ztsha0CFuK85h*ELo`OB0=2La#P?^wV*>5QC!q9*$Y>L{C5PehK1m5Qy#)!IbmUo0j z15RTfzVWkUzDUfGFNEOxO@0Ke46W^-XX$Q|%)T#K!cRC{~ss$lN+bC1$GS7gOtP<3l-z@I;Vt$@M8jq`J{ zm5)lr%a7;d_al9ZR#&;pdABpg39tNZ92`CgC^JbQu#-Zn)e`CtK1G7#LI7r9WkuP4 zK~11ur5dXW+^hi^KL}Wg#d{^{ahw^ZN$j#(zbwxLaQ)-s<1u7@0Wani{p~o6W!EO- zTYy4nQi%Xo=JKD8X&&8eY2l-e&C5Tg1D`Zj`YFDwfAI6!>dbfIKtvao={mL1DzDAslM_sy zpR3h05kOQjUU7OWqJZSk7-y%ZrgnE7C$J%#Nos{V5PC>bkGvcH&h7mCS~n_z3$i_M zwLMVlr)lLdxhmmD;13%gk2^4v04Xs(eeh@s0GeZcADeXifKbG-ghnzT3{}S}|0GD-`4CsCqwFbWC!ld@=TYxLjDUt;y&i zKwU`X4FB=Ibkz22si`LTR4I7ou-44~V&24e)x~)*>IVvOqM~IS0V!4_d1lT`q45+T zt>*cn$|vu{*D_D3P%Bw{?cqg*O3Q}IEZ(;ofoS(Hz7FW46|k@^7E$mG*VnL9Bq~ne zD8DmDX^H~eKK;;!2!1pTQ z{ZAB)>yquj1~ltlcs)jsWo!!f0CPcC0)D#(}L19;=^uUTRI-89t~Ls4)shw?&(YY zdf<$Wr8HRiCHO?+!kX^;dH9``h*jT%L_%_@fA1XisiJvx`0Rl?{_V-2<)u)d*zUH#bvUYJi63s>Q-vtF}pfH*i8tYiklkOO7MLPt#3m*2`Ct0)i4u@cW6+QURwI$-rjJ=a)2e1%Bl~`I9}RdQQN!k|`Pp&T>_b zjGJZWW_VC<1A2=uz+MGX8XecY+^?me_lW@hyEDts3`fQXB#856kjoo@%?;lSpCU$X2x^~W%yDu}BO zz>GfKE)OVTq>_-4ISj4iuOzXV7{-vZmcNK03Y|%rn7G;~!tbN73Rr!7u)1ibQ{XSc zwH{D-aYIN*FPh+aeVm`=+FCNbW8CNt?6aBbK-jF>riK_pc*_p5t0Y={j~45wCa0=E_1slxJE(Veyz!A50u3`^Z zg<@Ez(oXn=zHW?_z=B5&{^jB`R|s7*{*Y_;ftTFOhuY51uMe6n&)nwPfY>3_EMCS0 zy>paE`0`r9QIr(KWenbK4i#t%S{qUHbiKlHb_Tt$cSY~y?E5jjLvM+Xes_8F1dI)N zoQ*c@FSlH+-HTf-or$m0@kk2kmgoRgiy6rH$e4TISgsgi?~=M7z=YBjd?Ooi%{uV* z2Q*D!VG9_6ZoBAfrug}E2`G)5X1AN%4}a5w9?ztX(gNt)+Us@8Z~GelZW+1qMqAt2 z+5wfssUfX8Q?Z6taY>N{hX%2>l^f4(|7BBdEHF@+#Z`ylg={xl;14lECG?!i8A#qF zR;|!i^TYwID5Ave8$c&cOE-;*Q{MW=EN7kVN%x0MBT87;D|K$+eh|NM?kbMg)*w~4 zha$GX-qGkc3wu?K{L9LeLY|^-=r#hk)T&+{NwJ!Pi|eh`=i`*T;D! zNm1XwmU=@x{{*u}fkK93-Lx{gb?zP;Yu2bOad@;EmizbF-1J-#uRqc@&aIL{a}I(S zzn{|DgyXCCe~zJFNp3Iw9pwekzt$V#wD7NA@v+pSDYUTo=c}EahtTG^sQk;>^knM% zKXSIDes*;yDRtK_UH0)_aQ)`;qnjA>`HOILLS+^v-~gg6$EGc!sMsgodb9jDyXLOH z2`C4dwhqWZa2pq}WfQp%TdtP)4<8QNI@Ejk>P=XP0XN~lm)>usgltLg7!+JwoDJ-V zb}daJ0((m}{$uNj?Mu}p9DJ+kTnew=>l_p;^p+d2pAg<1^x^6_4!GalK7!m%;GN%c=D(f=9l}?&ien@C#0(knib7gM1-fo z_vboh4#^51JnHB{Ukyef>X4iB!D((Vp7JXpBMjm7MIWv-PwF>W!!oU?DAG=w&+_Nk zvPNyrn%lw`FCeRdM1?9}dNeoYcqo|Eac6h@bv>P;h)s@~*|2SDNsJ@*kj<4^;fo{28fFlDw!1qNaT-@pQfOWD==I`}*YBt6v*eyhhW5NcTbB3aYqN<0 zTy+)2ccG;J*o##04@kMhF!f2D=RQ@`wdp> ztU7eOQ=G>v3tAFrx*5A#0XVYEe#1tqx2X*)KMKo)ccXs5VKVmg#UfUu+c?naqCpm2 z!hudj*(hMGBQo-J&~;)}RXl+Dl+~};n{&L-D$#NCE7#rI&!{QZ$Z-_f+dLf_7{CGn zbbMSKoYos)YT$O~_h{tRf7L^xyzB72X4gKKx=bM1c6AhRl=I)-^r(nGPzRK}SEX?k zqQZZ}(r`xuOo{apGf}df_w5T#`qlYzWh0OMQZHe)(P4Mq`KE7%>GMRyDeCqU zedYo80LI=N|Bt4#jH;>&xA38)8|g-n?(R4sDFRA&cS<)B2Lz;%7AcYL29@rRPT|mq zbk|+qz2pAzCyufA*=xNq=QCRZwnxaI_2H?5)`GFl1D1Sg)%1Rs%z81hMOfnu_ zYpyG*Ov&qRIHurw5h9n9`|ttjLy;y+N)p|{(E%56Oi-B>4k3VE?k@^;@@z~XEVf#K zSXXT}Ns?x}+F&y?FYCs?QE#0D4MOwggL8LydO|~g{gd4hJ#>|06q6>E7b5eQCAcE*UNiUa)x05v@08w>>YF;W1VeFzME$WSE)EKh749JH~_IEDKn z&gfrq(O4nmEhQrGG^>kaoENWmRgzWoR`;&Y)7hatD7YVfc=9zmZSjJzSZWl!!bG~? z`gTQuYdWm;?90OX!t2v(?r?+qzH>r#YRMqS?;0+goE#A{^Y;7}&!sSkHXO_7bj<$a#3Pp;-Ntk7B21;E>^g* z^_eekv@zO`>Mj2`Z8mr-ld6pCBo2o_Rq}f(Ie93L3v3EZ4jc5LSfpuUVp956wgLsd zY!0`eP&(MUiLh^I74I^2%N?~;8)KnUtf-&5P>?`VrBAPQ@^3I~q_lB;Z)q98&}1ZE zP94dpavUm&{i|vw>Mkli+jn-*Enc6%f$vrS*A$Gxw|(PDfAS8PRjnl=l?C@bpGUG{ zb<-XPt~ELiCiE6XYYwnMJU=Tz3#ZvIGcQxdUK*FS7F0R#pqsfSwhoab!P3MZGe#}b z(sjhglk@rUFPTavYN2&|pJIW?v-NNn^OeD9+R@#!n9MEOq z&J&O^t?xkSNgOUi8c9e<7#Ju|FZzCW0IUrE7r+U*y+Z}8JgLW%Pq?_BAYTrw&2lAS9TjSvrsqdFv*X-Uk|LYYMv&3d|7S{ zd~bQaJlz70K7(JHn#x5ZudC37d4IN8H!s?AOn;{ka)v@*<0H_S5s8m@pfGxphU-s) zLIMmS$MU4A3IW-8)Up;F4H>G|SCuQuta7X@$hct%_1d+EdU8+$U0Zoit+n@uSznC& z^2Of0iBv{2;Uvn|f|ETf|?1EoXcuDl#(GSEA8OuJkQ1SVhRL13xxqSO_Ml zbWd*r3RdybNNLt|tAwWXo)r3o;)Ac#CGUH=0#ANFo&3JLTzULJBc45KnWCLb!oMHq z?(6C1R%%=(K)#w^lDFFGdv@dT`)b8o3UR)X`^fxFetahOi36gYirrX-l|lCUzyNIS zG;o}PnjHPU`t68~VcR-OZ09u9to0BMKE4Gz2;1pS8e7QRi^H3P?b02H9&xt5Zk5&G zVuzB8UZggDR;e>RqJLDA#F$SNzSkeG{+{c21>+V1S;y7td3poeZl$&29lB%0?T=HhX`Kzy7|Va) zl3Ik|ACj|3a0~D13bHbie){%^oi<2jG1BM+<)=0v9N9z(`ck;9+DyijO_5EBp$_4$@#`8c8FSqcb3JUMA$V=N^p$PhAQEZT$CO86Hh92oQ) z;k%Vh2HOzQ3bhLyvLH@5r&$Zx%YTFEv?P*eNr9)D&KLV^zqw3W=MzoB$}a>QzA6{h zS{bdb?>cZU%LW8A7M9hFz{$v^SXme`2~Qa z^Stq=y}bK9n){zLAWIY%8(Up12gII%H!e)H@ zg()I^yfY~kId$b6c|3f(Rs>?pPhtG~%s|p&x$*F3p#{K2O7tuIj4|sui7{!R4f+B% zAxc=;bGH1fseCpiV>wyYuh(qr!p z@xsmUlIZ8$fL*inElM@)#mjwXi1Z&-v@z@>KTWkLkN7agG23;0tVzqe%r`pYXDPFh zLafI?PD6CsVi;M&oyWF)w&2+5vl(Gyy>0XJZ*-3$n;O3`x6cyDWsa|_Z9OgG3flV- ziuCaC0Q5t5e^8}#nTfv7?2r5{pR;ethy{nW4xO@5n)^TN3Uy`>KBy0ioMjF$STEMnu+-v6hMe5jd;a(LBySY%? z+lA>Z9wh1y4g;h|6}g(5x`zbHXfMTN%6FxW+CC?1x^E-qP`4*!DtFmcCvrYa!}yE& zn^nk$9G%D)L6`a64l5DexMBu1DzcsXyq%bq7*nLQ&|(Sr&E1NOe%|!-)%;h67zn8^ z#%oo~S=bjUeqv8&Goy7#CI8gI#ZD)@gGENN%uZ(MrphX0xq=nqW_A+A^ z{N15i72k^!FOCG+Q?pU>Uk$S%@V27Q!_}!z_U(gOM4NU=P*>7uQclaOs%$CI%t*oB zTP%@LDLbV8Q1}I7A6&K&pOgWSz7#Nyn)jcREZ1^`##;}Bo60gmlXH zlgjZRv(PYG_=k@FoSu$PFNKNL+^s{ej$_wy1CLu8l7NiJePAi<9#1J!14AH&{Cq1_ z0y{w$2#u`FK&y^CdwQpX;Lp*{Mr?MeKew%Hx61XTThz?#R$#mn24Dh*1M8q-RDsZT zbEo7d$q1$oS}$*cJ3F(iPcgC)kAonWTQu!?@EBc~86GCiQy zMe&SS)^WAF{(S`it@Z8Z=w%69oY{F!g5=pXiTUgLMWVdoo1mfaG@47)j0p&>$9Q;C z_GZZNkH0|KhVN_5QWe&{o7~t*au91RannNxYM7gV)vFHdf`Z%~x3MPYm0CCW{Kf_t zGcYx6o|kH=i{FG}ZOP$E_<)Vf`1trzqkxc*2|F=>vRCSNSfd|o4r=w9?`1ly3JW%h z3kex;>IgYglF;Xus5Tc(ImB@~p2)r*>CISis<2yeA)^X0>p@SJWHgB&h5Xj1uV_%d z6XdE7;})^6e9Q3Fg0{6|j~(#~j7NCk=Qo`?O=JTH{l(BVJD5F2ryxBg8xt>-Hl#c& z+F#i+zbv5rmEtZATc2M67?Z*{xQ63&0jN5pSu0$&SKK?=5GfaUFF04l9MS6Ds z%jpH#e8@?q+DSnH1j*7s$VHbM-;K9*e7UPe@-9EAwUvN?=5C6-TRp8b*V{b*P- zQR|IE08yhI`&5`EWl2v)bd}A7cF4CXmme=W5QZE%t7L3V<1Oqebjy)H>kx+>t@w|g zo(=)gX)H`=)dybsyCmrN1C8s?r!AMq;)B+ig_iYovoYNo-Hm08fVGqXOsnShTEo)m zRzC`SEdS;EWpRiw4k#_t7)fnn`S6Dn3Vg38=vb`&paw={J1yx)HZu10E5u59IXWVd z>ihJm{YAEkt;hL5dt{AAwlh-eI7TLoVE*ZNV`F2Gkjq|PbI+l&!@2ufo0}HjqL4^o z^T)SnVP3>(%ORUDOi7X6lp^qiNJoY?j?|DT7IY84l2-&Fn)t7yPrqv&6ZIt{o7W+4 z2sjjHwBX~@g)w2I5-sz3e86BTffkak#I6RTkeBz3#X1t#Mx2#j)|x>PJ|uYm#ai%gf(sXD>{jh zi@w~XiW9X?rdWw~h{U~9AwQ|;Sl6scDGKa?p8OXzC6$T7_Qo4CJbrFxtHLnl28EEy ztii2XRoA|jg)RIa8^1hPCNNV{++vdARffMzV3aR+_>BpV3(pIgpl@P->03h*n=o<-$VEX_TBk z&mmX82zkyeN+nPr{Hg!F2DFyEuH(g4L#qcq8kbT_+c(x0;`;;YaRH$gq8}Hj2|62N z(XC8AHZ%V`^ySeOn3+1a z?m0<4ok=~-R0Umtg8G1Fmn1AtLTpQTtl*PmtKIKnl22WqhqtmuXP3cmU(El~k#q>& zZ_*BY8P?X;Dn;gnM0OFejHv$hwPdFPh>(eDV~l`u{EYH@uZsd*RZ6LJ*0=f#BQcHx zn5RgFrpxQ}Y>e)L#ZpR??=@LUQ=ArzoYQ<-vWok?|FMTv8(+T}H@xF~tzF=b2(eWg zfS_ajX2r*2VTtxgWOcCVWa&=8l*uHudDf$M$E?c2eOOGuQp#OqSi~p-PfB~Ww0(gP_t66?J^Rm3G0rgirVN5l`a489Vy`q~d9Ahwv)|ZOzyPG3 zUrbEAshbhM-u&qje6WQ3^k-e_Ztc9f2Z)bXAFQuGL5q~l(P_v})Qd;%o8B%r1VD8A_c?uHzXUQd2_-F1J+S1H{9&O~@ zu4>ofc0Y_k5=$OgNVIB}8a=(+90DR;D4muSjH#s$(!bXb=cdE~m4?za!>ybDY#W?x z$Z3fSes$uVDd89B_<`%0vM&P*sVKHF1nJ1Ul}jQ9Nn^U|x>fH#Uu54UCg*(3je1>& zGtk^C&})Gb4l%@XB5lkrhtK1sn*SMK?~@ly23|1TG$*}Ar&oc9Us9)(m*VA=*(yfY zll@ps*&|kx1m(-VtZ3*C%zxwM)LGu}U21IkGXM$w_|Kk)R6W{~v-C54)J8po5b%qq zW9nz62k=3GS+{yD{)rEvydzMfegVQTRUC!K5b%;oG~#0ptSY1Y_gA*0)^pb3;q$*Q zlLoC*Q>6M(6R&vHvl~TU4VKMV2Su@s+(`Unlztt}MXTx29Q?h7ecfHA8 zI`xkSUU|^u&)yS{7SA(4YWAs|)*PmPgOWik{_JKv(AdGsT}Q+MEu4tz9R!bxJA_9- z*uH3dd@N}YRi#Bc`OYHyl{^$Hk-@{Uf8w)(jo_!8YBb0ffl}c!CJ~OJx3HMreb3Fa z!?#O>hh*mFrMt8*p3iU}73DI0i(%4~#^5$!7me@cC|h-O?Nf@1+z+<1Ygw=b%}{Fo za;@h_xbV?+Y2NL=&eLAi)38aNq^ErV+=p3i?MejR;uz1fEgXNeRP6O*Q9kQ0{$P+< zQWY?KcX|%b0;^OBFo9z;6pGP?mwo};Q02u|*vf%Ovzgj1=kP@6ab?jhWHNIqWipp0A`VTwHFzyKxk%Iu zM}N02@A}0jx32jQYJYqMf8SGd0_^+vi-WYy?^jwCa{m0pYqkC(;) zSr&?&Fz+*I&@)ktK!I3F&Y_+>tB>#|iGgg+&8Z8fS&07JR4^GG9c>_C>SaNZB~*6x zrq>qrAVR}z{5I{t>Fb4G_im^XkJ*_5zTWqBdsFd~Jj>d#(0*#v=k$0slo^w&$!K%C zRAivh#c=3t7lWahB3AJ*kz1U2#0Almp4P~!Lq?#>h>$N+L%4y`^+x>!vaV*rdI;4< zEj13x&^u#RCNbmd?5rJL!k0CznwLO(fLKv~a6CXp_Q^dwx6iB``5ViSyu-6m%PsQL zX)h#Wgy+ZZ8iQ;zkvcdwfj90gPK4P)6xa*4uT@nyYg@F-G6y%2NkFStR#rBu{gEek zc{HDEC{>t zVwY0ae?B~|OWn5|oF>0i7moJ;ZGeQo7h5UfM}x+1LiR0=vu=)63qQWe^05?Q`({@| zjW?6JNgx;zhGjDZveWi6+*rH|BSM|NRZdIbE*E?mk7vF#dk7j;rcoDj^Shp&@amqC zn+z&3FCUIzN5}V|eU&{165hZ#1qWAG*Y4eK`7oBMG|<9nYwLWVP0)0YhbU%-$ZJ!p z%P4ZGah@Cyp}y}B^-zC>g2HTmyQ@g2OqH`&RUX{L_A6ti7qP5fd$EVwRrOMNRg#|} z4$ZH`9-0<>cZGAlGeeXZ(q7{QgE2RwoM_BNri*SF-CYF(d0-76Cz&Fdw7tC`IoVvV zuqIvtVaR$J?>l41EEPs2LIQO~|F%gjHCsc+%ywE+Sff)uP-^y(hFQzn(if$OB&nHS z>uZa9B-u?3h?Fk`w;d~bSiAS(H9KKWiN^flxl&zAqk}Ys&ik<|^u>`cQd zsvxejeI)r9;a8A2YPlTSX6OEUvsg{`7~l#}IjY{Wc9UZ+|3cUCpLs_iZ0O5k}G%K(SP+R;iY{9qomf4Pw+ zbN*E*rV%NpNANwFc;=NXdF!eMgx1~E+&T!q8Dg1cg%qA-S?(WNvra2eMmHpV@Hv04 zVh8adnK-sHB0*C%?wh4|{3@m}IZ6C+k$d=aWJzYbVhHq}_glm3m@&YQved9G8eRUt z{12jw#cV-9g0HgkbK>z$vLFS&!mfmS-)KEUk_>*lpE{ia72)^#){8U)9UUF= zQH9~NloSvJ)2Mmc^>oE0bv|*WQk3*xOkct3wFNk@tvd?@5%U&;+Lni`1zqb z4O9A^*Bc-I6I!j_&I-LNs{qS|`ua`O#}BwcM<@L>XMtSl<5q@Opm_yBcBpWy=dzZ0 zY=nX4w9orQ2;17+xSO-suHFo7%wEgd#3`+bZ?ai@IcdeTiJ={`Ivln02t z?(Rvj{~%lH!MDFJ^bf&W@2u;Zga*e-Sd8z#yv1l3YwCBrOYcsPU`ZX)CE>=})+8>X z$AjR9E}$X$ZtY=ETJ;56NF;w2W72UzhxY!;lTE4pL4L;E?;R!6bPEl6rV|39E^Z3; z7qNAg3*x@Mc>ZIZ)&&y0N$$?;9(hbi4zp}wmRINM@v&>ejL&+{N7A+#MI^)!D__l* zYbyWJZ#&MMEPDBK^w!aSCLmZq$dRjrDAr#ieY{tf!OJjO}{g9S|tGe=t@g4M?93N^Z()>^+ z+$$=wf(ZNm=h(9Wv-BgdfILX6Fkliocq93=>>(9!KAC2N=*2^(O0V1&M6knD^o&Wu zZ1hO~fEz?IyEJ5yd@fH!C|Rr!5RKAcDb#k$`jlplr5w)+7c$GdWJWHhXC2H zPts%k%H6V*%Aw(pN(-Be%twq#0RZi6(t?{%>fDEyhfD}v0BjMFWCzH zmCnFBP_wD=^n!uW-BDN98)l#xBUJ%vlE=G7dKzrrcXp{OBk$~A}r^E+P{3J+SCO|D4Npk+Lj*d^}HSRg!WZTIMa@BOb)1q)BB zi|ZPT|I|N2`^rqoS=rgkpjP?y>sNkvl(vrc@K$Y$ue)sK!{+>n1;q9K`Xn?o^gl*? zN9(kXA~m^{Yu`?7=Lq%-px#1{I<6|vmf)vmHr;q z6Su8Syr?laJTUpVj#t4Qa#121=0MM@IC>`=ycYetB#_ZUft1VwMMF}rX}A&o(la22 zYM+HbQAIdyG{Q6t3D4D;nc{hv!W*<6G6d$o!ufMCWYc)E`h*fI4I#@^!S`U z&nTiyvk^%c?|HS#XP32H&Gd+Q{~jEby=4W?K;Ur`ao>a;v}UM!IeXzGN}LXKE0?Kd z3932gmTR*OwwKOi$&mVRjE^>hAQGcb+T_fv8E9!JhA8F8sjD^6ia zd-p5Q|7yL7tO|$nnV|ilOd!-E$44Vj41r)dlQ(eb_wsPq)#Gu>NObfFbMWyhpd0w7 z8yjU_zclXDlyxior1xH9Rr9_iUg`OvAN%wRITRUQ%R@LvZrlO2yxqk)V&I$^62QG- zs?hFaDmqvxDrgwH(l-Z^Yx4Mj0{ENz)(!x{mD$P=U@=@)w z=gJx0*^W$FRYD`7^Nt{-yHDaO;DPGso+Wt3p@BR?*6c{Y|LL~s@w>Z(*E>aTrM19YaL&?7|7}m=sM*tT8@**yi-HfGTBwvjQ z4Xc{Z8?#_=MMT3QrB^r;PE9A>7n)TN=>T=sEB44>Oz*d2X!QKJqtMXix3hWxppvc-!|OOX15Xf5_O3Rd#h> zO-8*QVTL{gxiHl_yI0wM2Ubnm&Gd!TjPZpUvfJx-nsNjLYfX0hnW4%XNV9BDAXFok2Jw1W~wEvfR1f6Jno7wyNuBxLZsmq!y7r!ME z0CEJ8+D{%&RCg{NhqHbKleYZQuh?}8Q)XlM8J!8~a|GX0#90?GO9sqFe^Aq>kYRmLh~yaeP#FDR}ALfqS|4c zmys3su|HEy21!FOSwIP~V`Isr^$jKI<#)3(iAiDzOiz-a`;3v*m1^-2r7F5HL{=jnqU7UlNMv)l6GBl4yqXSA9)ntpzGQJeD>$_9()CpMWl6wDVse)^h2 z*unH`5uLcSB&EF4^1Yg}jF|mp%*aUI!1EML%F77Z)hmhq!PMc8fTxy~k%JGqtmPh* zwcviLr#-2_?H>iG&h00C|D|o+U3NTxT@MIQ3HG=hu^X$HNu6PM6%TYnN2?7>qs9l>I-ZHOyix1n zt)cJ6het0hwPO!) zeaHrlj?NAGU?HdPK&IU91}5%ZX0%qjt^ay@dP0#tp__HUsg6Wl|43J8w}B)H z;A$@;N>vV2R_a5>OH&Ti55+U;<({nCY9gD_}MjrAaph!{!He_S5_vc z)O3EC5tT2A1C)6+Ub%m;%hPjAw>8BrxK{6qM->^;TKqsrNooTP<8Kx;StviL zeN<8FSb%@goW^*4!h6x+Ti&UVa)T`Nc&hD@GF69)Ic*?MDuxB=|jC5Y=5w z8_lvEYrhECD^Jf%%I>p!X)T%V(3nYtFQ*XV8lF)vMw>x1_*bbb;BOL=v?lU{S}HZE#7Xia2|M1$xU zdCmX_x|FD>A5h1~#>Za*6|l#+1TcnzuO{UGLZ|?JDyA0^pJyyuwAfvpI}fe-Poq^+ zW`$_6)dkOl-Md#%s>A@o|t|8Rp;Md8mM7#mLlqJJdN}tu1WJz*0(V! zQ`sNqOY+`JmuJ5iqLkg3==Gs0v`Cy2{BowvMfSFv$a=w{n8N9Sz~-EMh5zP*2#w#( zxj&>kGAzrlKfY1SQo@{9@s?LxGm)<^gpL$(a_+^=~W3)_Ahh> z&TVWg1GO48DdqjIoi1+4PB&N6KLs>51ATGu8G9#BPfs78&Hth^aH#f!>Zi^;&M?n# znbn{P!`o=pTNQYAW_k}u53x0%Jbvi4k@R@-{?`URrq0vUs%7uBrn45qx&`icZJ;Hr zYzQAfP=P@7RmdWFr+G0Dj-HRIYO;(wfFL`Cx|-TnUWq`-^wF`=vFE2KYgZ^F%0nRV zchr)@EJwnB8+HOAw9zSfZK!c&xks&`=2Lq; zEP{;@5do{RSgFdp5ury5=M9^HprE#~UK~s{UyepFW{*6!87?)k!1FmoeUL>xyrwMY zeL$b>s2UnV&u;SUyo0%yK-W%7Tu0C=h*krE+V|_;{!as@OcQ+!Rb7vM<7bY;g1;AS zpGLKR*Omh-PZYJEA{vjNU?WHz1i6ftnGTg1xw)OdszE}$4T1w-Lv&SGo=dSPT${K2 zTReD%imDFcjisbY3N;hqWGP8H9qZr!(~R-;f1+~W&kDtguu6PpUaVX8Z(GKjg#bO94^VY6%-P2G!7dud9ZOBs4+mXdVpHua)p1 zdE~%EEb3U+Hj|o;N5%ohBkdD%ftfnZ+-BXZw(CFw3Bp+Q2#eU|_dbHQqmNU$_35*V z!%JZ-OdHlUCo0lR7Ogv5Ilo1nW}cwND0&VwmCuJ8C913z1etXW4U?4hf1S1yTLb_J zzFZXU;|ZW416|5nc4DcUuHaKO7r);&l4lwE9YC=O#H7J@hfUbn*sYbrx~SjH%{yZA z$H?HnYQrDuyjL!A?z#?8gF3VsLO8OTvhy->vocDx*}&qg$*7-Y`%GD>;yi2hy!h!< zC;0ibq=u>tZU~GQ#8aYun5fo#KRLq~Qyyck`mS#BT->F5ch^YD`zrWuDp<_tSP4Rx zT9^Rn4qF~^3f*qm-BGx0m15%JhU~!aP8_h7my^{H4#HRfVM>r32t+5O-joR_d2Awv z@K|2>sW{yH#JxY?+*E*oSinVt^VD~$WM5z7GW$0KZbbLoJns?x5Wv|zq^9&za_X)q zY4=U@ylUqL0ULGIXwY*;N>i*9FBN4azNeW?jF#*j-@Td7xaQ~ou z{Za#ci2Fq>z?c!jj@`pm0z8HDYWoe?AsnT5-CGspUY&_OEOXjo^*`O<<04p>+Q+3D_<5 z1hn3y|2lJQMI*COmCrvk!Z6V>>B`pgizeuQ)kGKISXR1Hal|5}p>84-Hb81S+=v8@p=3!npV#n?>f(XKwzhJ5!rPP;g)gE&9m z25^kO5^~fvHs&P4_>b0bxd7%i3XmV!v%Th%o-6Jxj`P$10T_FrE$yOWV#!>aRQ-BK zGFUulU#AnWEf~^3>@rw<-qAD%ySceJcF)h~n5;-m1cNEP4c~%IZeS#j*KkFX<6`|@ z=iNX5^P|&d+j~0RNhwZ$QAqKwu$a5zdsMeXI zt82A(nH#6JcA7{R0eiNTWYW-62UZ7uBEs))CMU-|I`pvg??3r~ z?f}Hd-HaqM6q&pE?Jwu9`2PzS_a7>=8b6CW!7e?qhf5uF7@dB=(=v-sXVhn#j>LUF zNF)&FlSKxP{o>^F;t?Ni`6;*_j_h}N@5j;QdiX7uz-^uY0kShP@f8oh$#Ko4R*+NC zw47IKXuJoL!TgT@Q?7$v8hpOd2|Rpemm(~RxNWP3{r1i}fV8&dexnna1{4MNU9HXP-Tl7Q8wpo5A)DTo_FbMPzW7 z;Jc-+>mn{+&Uko**0 z?405?`om=%O`g8`sTRkvMvkf01zR+_)rU&&6|aMeNZgsLz@qc}`}?`hOM$)xAVvVI zy{buDVhohXZw;#r4mLHq0gZZFU-Q&V-!%rcQi>}zpadR7}~g?iWjM)a<*34_AKgDge!Nqp0M zpQWK=#WZ#d$}@9lMN<*@!QJSQC5%c2dGZ4(y`5UABNkF2K7M94W^AS7j=9k>{+?Vl z;acnR^I6r_0)VoK)zxZp5}iU4MlB%|SAl)=An&KA2YKf(d8iR^l!$a1 zol0_`Mk~A%Qrz=v!Cv6=VUOifKy=;*ZQ zAL|c3zm^Vu+T0$^K2hjMyfpOo9_vL$NF>GMjrA$we(swb)=yh$WT&8p6p}Jz8^gpa z5Zz^%6`Q_Wpf4X?s;pep5RJ4e%UDp~qhc!#MUXqxXTqzxiB~LbMnG?%{>t(TBO;)* zUDHF2kuRC+)1)^Vgp5y82zfLCspf@rI#2GPB~?EeIk{!Ffh;p4S9$LK+i!BW$>HjM zJj=eB_4C8t`mXGqz-qQCb-py@5$x5gM`pZdTRB_`mcW#o#%j>iQ00U4gxoqTRnL_e z^5>j9C+J()*r11CVV(PPa3zmYiu)V`#NH*+THqfrh7x94g@>qhof5={nF=I}y5664~?HudfeZ+bnAl=^c zu040KzHeUlY>s9w*FV}F|Drzdk%BZ1q8g)^-Pox8u4B34g*Nd>sSAd2y~hTY@kgGv zte^jccd~g1DV^-+Cq!%Nn#Xn|!f8F%i$)M*xV7?;|MPkq|nvnXYDG7*<$ zM^YhY&0u5F42?2`;aJ!M1K|g<4r9Qkl7U_pl;hH`D1y-m%dWb`?nwU zSxZx`%aPdBk5zW5eo#J`aV%!;t0%`oW9jyq=l#TC`gU4fzkAwgzrwd=c+A;Z&mf9W zjIXykz`9vsU8c>66q>~X;Z3fT5EtLDUJiYf{4k2AQLymYcCr4hwlS-i4el`TqXwLKc5t6))GAM5ky?%osN z)E}KqU6u)qu()YlP=8V{+h!_Xp11yM9!*t6^Zfo|Dqcr3o?2p2hqc8SsA0X}eKl1N zzvnu^p-L^~)ecVL1k=WG?I=(>cLlSxT@;v zK%BbzaJTj_k_ZOli+}#m+TFIa9R;o3t$P3YVUeL)+}^r42gWA6%l3lK9T(dtaTJxU z``=qyM5K;UZ}0drMgX~?oA#SFTj!CC>NjZ0CWQt)3wde$768DBYMNoYSg~Xgrje47 z8S4T!%xQ%-5?~`$W`%y{-7$IGcCoHos0uuEDBGG@SXkgNXuR)px$ny_!38+xE%z4g zb1<{lAKED?F@_p7iD2R4IA!iG)ep+(X$Ps@HLNy&6v+RoYdG zZKq8f-oxFgIYsZ=J32FnWa!|tsHQM8JSKgd9@Hn5-3i545C}3$5uWS;EF_hV74kDe zJs17u8%=uN2}{g%Ota3{(aGH@YkzMrNR}<($($PAv)JK>w~!E0i)T2sP84L%%)ap7 zbh<2Qo0xP(;}IBsK3Z(7cIvNXhi+zHU0ux`TmY#^7Kn@JIH=6?q%6^_3^)5MT@vk_ zBYCSI*S9Vgl}1bqwm+Y&mPh{HoSoHqyj0Jg=WaxRfOctUtCsQl#G*g=dLG^q``dQ< zpn)6`HrZhw+$55L0O3+>wwo5Dmo`^KR3x2=r6X{+CiqVFaYI>#!5v~A+rMPJ=|Ug( zLzSEm-;T`wCw7jlfjoB`D}XB4uh4e2^_(ea4&QuLn>RlV9J` zrc9+})@#smV?DQEO_mwss1Trk6}jL@lOTW6Oh`*-r-rq6OX2)&&OX)+D(Pvh9HGxP zWLCVW!1c!o14Q^1}2p`0b0qW`6k%W9*i7|E{*oW({X&Z^r^o2ARy z(PWdmeI20X&kI^wS{<_Ze#CtoZENMp=&MG!4TQv*;V`duushRMkHZ)y)lyb2jiyae zW|I&Wwp~g~a`*9hW0%^%ZtD^aL(6J%be=u{$MJtMo%{aa`vcw!eH%QPuI! zf9=7z#gHbC6!5cS>zt>OE+Wjg_bva#N^zX@DORM;67LX^nC%J7&U+X$#_rz(<4IV5*#g6=1{~f-J@SI=EJpN@4+v-^kLC+wDsV9jw->5 z!&)=)PN$t;?G*)AmZn|*-{H6Yql1frsGdD9;vv~}mv%+`JJR9VZdws2_^ED-eI7Q4 z_=%fAMw#Uzi=vP9mqWY4>T(R4EPc%f7|Akr_gPq z;+ZCS@z86=`+<;Z)i;9m`ii@+!qzJ0T{CX07ktjIMyuvPe@Y zQZv%s*7mrU@a2CECr!o7$jIMOm$tn)K+^}|dvu`g4F)=flV12v@fZgO*K>@BqM|KO z{F(XrEm9(Y_?z>b|MZk$K30h_8tqBya+QBJ2`)+~{*wOm-%)1~ z@vO0BqI@E~d^CN7OMQo{QiU<{tJcV@663d ziYN6+7YH00)j~6QNou~~Dm4#7XwH1Dl-hHp1&Sg?Q^YsHNE~u6NG|Y{?_#c|i zDkzSw-NJ(eCk*bv-JKx8-6goYySux)lVp(KGDw0$a19XLf(LiE)8AipxZnz^V7hzv z-s@fKS#m&Wab3W_aI(i;0%M_i!e1=|-U z?S{J^2OYjmdLr)w(6JfREY+=Ql)|A#{Y|H;lcn=1KDm~T5(la<0;adYb*JjA?obU( z0cBX25Lt3q6bO0}@wlP`I%NLtPp>6)vJz9jpO(0lJ2v&_^s^4&?5iTMZ6r|LQ`IYGhyB*>Nlkkjd! zd#BM9lPk?pvu0&kv*saL_gymC>+fh7a;0Wbpg&02tbGRXZHZ9Zs^ep)gB&&w0)ll0 z>t`wktP%seW5kJBz%jZ4874WADJUVio_R!)6&Yz*pl|e)=1m;*;33*gDN~K_QMm&x z{JR})2O7KCIYL(ADR<(vwAHT9^_V77d?Ch>r_dt@9Q9C^zFS(Wf{8dZYh+OVya1}_MDWCUn4So zE;?v}*p_zgmJIL0BZ9NL(P$I(;c`&li59@wtL8FW@K<{0auaxw>_&VFUSCHscd_=` z4oJq7mDF7d8zFX_)A}`c_Bt|Bve2}xqC75N3ERTOoQ10THtVN*Uibz zx?kPGk;gI+%F}15fW7boJ+^%c27>9-f`65Fk-@(kTbL z4X6GBu2t=vo!1UOB~LP#rc&=5Rbyllm7%oNKOqV-P$~tPxEyus7O`p7b0twwTZWsq zeL8;Q(N9bcxTY#s>2B$2X>qfB*v&Qq5Qr*MlM}r96M)l(3P{LzUx<@n^q|r3eUy^- z^;%H!_wEd|dR%5%0}JKvNXW{ zW;eFJiSq_vg`sYUwk)e-*53<{gbf~kXb7FE<6GwdLE))QX!~5X1cpr_hWTk*t9-MY z8@$&M;+8^K2#yT*`v%-hj*==Dj%1ua`2h#jJO4n6h^f6uCCd>@Zs7Zcnn+b&gc_+$x(n zr0pFq%e_DY<{I8Zj`ihBo&+Ph%#rSMtxutRSrO=3>O}IT37{CUM{P$}vtZww)uXzF z3;;}WPCDcqyeUJspzZJQMJ=A_=3iF6a}W?GzT0S$k$d=M9aP`dMj0Yllhyr>FnzE= zD()lcRxvAIyG3=}7`3Vq%sZ6#e~Va#_612=pg)p%9#ztmzH7xpC+^+gN)1Dq{(Oq) zqNk65&%ae$M(JdAsr(`E6=HV9z(C7lPcgpc_w|Ro3gsf@wk|4TYXja_*qr;=jsx|o zIU~WV-E!N4{*9MY?-_2=L@bc3hGX{D!T;)ruJ9+5r6Kvr>(v^8fMXRX z6c>JYq0^txY85P`mJ>OE6K!27TxqV_I64C`-F=aKlSRVbug|{b;|B}MWQ)O~ay&%u zpI(fhYQ|?Zb4Sfg+OkyV8=kdq*%ujj^TI^rRn-g`l>1I@cHY zQ{YH)!w-k{o&ES3{OMZ>G*CspS3~uODpIs8?NYgq0U6%9YbJjEkV{lJSe1^UWe7C7 zde&#CN^a#l^Mau2Ae7y;tf;fU@{B;N(~{kGcm`2Zw1^R7G}&@$Ws0zAT?Ur%Q4@ZC zud#;x;$Tx>U*ES^IKUCz4ovc|chXfTx59y_<$tbwfJ+}PwxOQ(pEGG*hl)9ya1TIIr)f{?COnAnSqqGH3~YFayt~f=K`xKi2MbdL7v2A+*aWxe zkdu!!RgO@~NU7NkS_7ijXD1W$B?4C`LvgHOUOD(kM^cI7CRG{!c+sndkjz-*#YN`i ztgz;>mQII*G8lZ#6XqwIkFW2GJmL+T#1<5^$}LZDsE6 z0xSzeUg`86@N?lfx zTXNY3XTq-}#uotoX;shaz+%&hJO{ z$QH<&S^4x$+Zu2OXB+)3+0XN^q-RYv`0W{YjuQfd6qAGeW6epfqU%~D*!E%c@~Eyj z@P0EI(02d|4;ib0&-TA*2ynxVt*#nCrUPCd@SEm=q2Aqp09t7XMd|ZD8Ik3)JK&@J zqlx$q?GY|M`Wbo-e<=s*IPXIRNRnjlqnh;+6V%lHd#EO-jI)9$hRG}feiQ7Ytwnrt z)r(JSoOVdLnr7Dg)Qs(U`GhqoCg}arGFn#LItU8k&?63ykhJKA0cE@jmBfg01_pP(|4FUUJp>RCnu;@jCLS zkk2_;#r*GMPjUS7CNykcRwCBKz9Ol-L$!jX{p*sod(N#tmfOsLHLi?UTVF0T$oRKE zG7Bfj1=0Ca6DI)=&@ZSAvXYL_s`{G2?Ii0QUhxvEY0-W-PYqPm-8}%a(~jHa@p0zV zpN#>#dl0^}CFp-}Qz5fi_FtPOmcF|{o=D&LY>C>t>wBXnrxF%Lq$s!E-O~%T91y1f zrn5GFR(n9hXN=2P`)SPrF+eyy^lDFxD!gO{$HvAS-7KH3wM#?PI*p*dN4-^!{k^Ii z5$15^zaTf2Q}fFXJUM^|1n|#%lZ6F?FbP4tlD28LPvQK^LD!J|Is@HLFNamyS#|Uk z5jg&LrvQZCyL>L_us@xx%O?%3gh9k}3o%*fGA2W_4(5xIv9VlQla{@*(!`12C2#4W z_$Xo6!ejxaJgNW5;&gs8aC$vi?tB6`Kz}ne%9A#{^n=-lv0RV8hiSUGE#}&5nU(N< z;tvqizoudgjXOvAFeln_f3f)M^<#`3U%shB5WPWYxj2f%9|z!dRmLM75tdsRub{t0 zTWH@(m@g@9QMy>FzRlUT3#TR@8KZ{7kuC~@mgQRQ)}vohU=!K#D>|5xPr3?osAV%x zv0yMSK~qh$3h0Ox0&3hMR51b{DGCp{>WT>mE}})AzZO4MB;P%SPtzxD?*Nws=$r=r z{i)rt|FJax3pD~h*$!q zkEd?u*5$c_eld#3G?Uvs3F_q2hZJNmE@3YS__;xFePrwogYb%N`d&Sp+~22pQw_$6 z07+Bxs6`f2F&E0^OyM82nj+c7etkRMWbe$9_Bn-z2L2G6=Mc~&NdNNdtl2tl*{X-e;6)a2sF+@hu)3X-JXfgdgotPW${;n|PI|HiJQ0QXsb3G7+r z{`f%Lrvb9ZvwjL_VUj;e5&ctU*f!LdS|5%3j%?CN&2tY^GbEid== zNx~;Ym`0gP&>n%eh)42YedKVxj*r)?OGw^FO;{-qJG%erzP<#kC641t{&&^YwL$ys z8z+iNCri-sg(E&9^i;O7vW26k7F{YK5VfsY!t`==ZEffCC2+!a3<`S9%10q)+12Vbj2;2FKxZUx}M|Afze)~%f$10gbg5=yhGrDJ8!sPNw7 z)mpLO`+jUXfS^a_|9s)s$qLbe7>NX2P5TLBVZ|cp{L{pEe3{hDhS6G>pX_$soZoIO zwUrxZp|L- z%+?RDL=`c^^*r}h7c@4CRtM*3F<^0d)0#zC$@d0~D|23@qeWUG;YTC`)KsNQ*U{k? zDW3pf&y>(rpP~1fDL~ZpJE{TR#$0nt?cUA+`~7`1K(Fk+2dtmM)Ys$ zy=7oDnD{t$Iewltqq?Drm(O3oCXK4Q=!{$p3@|>-5)7g#@f7IQJvJ1O4Yc~5agL}R zR_us*t@og$c;=1eU=w_nBTtM}n%PpY0!)aqCuAC4N5&*y5jAF!09Oi0w-yI~hPSvY4b zyx6qhrj;zmo5{o!zH(vl;i;h8{kMCSS863JPR+W7h532bsVS~sGc&k2KkS_r%#($^ z{{5!F@wu1*dc#qBBXGsjrenTmG{qxem($A4UT116+dBGnegJW5)Xdv(6a6#Whgl?| zkExh*LPrRskV2=@k{_-PaZllnYk^g%fuD{>14VsbvSVdo4YTK-b=Hqh* zcx024lR=m1B2e!$C7{pgm#LzpZC;WzRn06PtHD37cJ+~mhq0G+0oZHAUxx1OA}hjyBc@G~C;zuRdobYX+bU?Ws`d`a{ zzP6zD$bl&6a*F6N2XDog{I8fl+ zAv7@)E{@`cq?9cqRKtWYbWsG?vXM8zP_r=f<3U!O&9LD>R7h%*Em9WXqknd_It!kV zL>%!pMG^|l3vWzIQMk=D1sA&2+72z8pj?^_F?K~Fewsp6t=D@%#}n@Z7JJ~cj|w+! zT~SIa**Ftf_Yc_UdckKiQq*{vQD9>zEh{V2tCv0Po=9H?UgM(1b#XZ{;k ze>CWff?pCy#qoUO$%B>@Na(7n?2|=a?~hjjD*gU{N5c_(6-qN4hL5%1?tI>}_>X)& zt4MU2aznx<$cPEck(1+*cfhtRB%ecAVC`%YNaZ(*17-}ZGt`uWBkB`Qsqor0a*JZl3b zqW-~nIypF)IMp`_+4_K0MCzaR=sXi%C>K#?O>6-&w>}(IM2sA_DRuoNbjJ!Fal!u z)ag|Yt*@^KH1@gOIfZOiU2Ss;NfJ8siV*%gCKgbL=xV3EH^~qzW@gipA%Wvxwp|qH zgo4DjjPqCPh?_k+&Ux&@n*AO_@9r0KvL~1J*(Az}40w~OATIazwI+XBLi4qkS#BE6 zo5zCTkOYgp-Y?zG8?XCCfnJuLo_9rt74yF~e-k*$qQXl^%Mq53xZsd=pWJ$PY!^w$ zB#pDNq<(#Q!l;$*3h$F9-USgr)~35pJb_EOSi33>{;4g^e}rxMOBaQNCBZPt-*2`2K1F zqA1x85%}c*y%vm$)3nO4B+^9T`gRi2v!ykF-GxuvDa8hCQ8tUm9h8b)Nsctpq9yzz zx#hr@G_C#AmH>KE<=wnFmIBeM9*h9asSz`{6a=t|GsN+nJG;7qfKv`&9{FkL4uno# zdjF2jWb!M|&(8zgrnWUF=u*It;QB==Tc9mFL6crhZY}`fwBer7vv&-z7H7|sIJolz zeAT`;bF+YS@Fqy8Q8ibh4$JW&@hcad>Flu#8nR~7?1bHBKM{y(4&Fq8pojl4nvPg^ zb@dARjrwDD7E6YKs=vI6Q@b38(B0q8cR3R|uMAA$Ez%6gqWKv<52KAKGgoBChLANK z8DdlEhyu&7i-i`Zig$G&Um4{6Th*}gtm$ScrxR;4-sSRDSbEA!MHQs3{F2y*sSlsF z;8_!M(RWUZK6?-wX&`ZzX)fXc{X`U8M2TtCrdAA=n(-h}c{ztn7)?g;5TG)H8c!Sq z1^1UMX6PjvxNLQlWFn}vl1|0j%N0FZ2Kd{!z@vv5?QtbZ~;dWiz+ckQUJ2c-9a zzd#SEfa_L8I-RlK;{jQ9-CfStwNvhV^iWv~hgA$vItTg7k7j#jg;rR>-;tal#;_q$ zNhWaz=f^RZ`uZ)OSHR!-KadJ|oKFg#f@cuQ(>REJ-+kP#dJp>}Hz|~5Y~75V#Kv*` zVbc3~!1&p+djTpsZ-WksNC=6_A!y>I17LP9h#11J{prB7{P6G~=+ylJtj2)TtI)QR zFK~KpVdvxfx15#0g$21`)yi3-`RY0Ugl*%~4h#L_qXL@MgDzw;B?5dGamg523vgzu zpd%@R+v%@SQBkvkJBV?T{?9>S1Y$8j^Akf+ol;b z@xJ_avS$-$u_QUB{Cn#3cHL1pBsVvTxb$oeim!+HN?>jjR!MW)w3AZAVe#>u4LXEb z60c;pl}eIb(!z$FfZPn&XtypWN}B_Bn72t@Uf!XHfNdV<6;Mw;RV=b`;DHd{>%!l( z^fw*NoeLyOzw_}j(UzMid=rc*c;b~-76T-Pi4q4NmzNh`SCe)W%1>m?XXqRVo#Fj)$s1*=bW&iZT7pF*RGQL|MjceaNr@XDI+}{H>g`k zG_7goU}fBxct%6=WQ1RHbGy==MW%OgCqEBY!%qE&o5Sla>mlK&%fbW|BVkEqoVKs^2EbW7UD-!(I*XE?SaeC#0_RI@_0D~uS@Pm~ zR^)Ly8CXX2PHz32Uw}?nrb;-!#!iz&cLTtQU2G z62c0%gixa($Hsm1hUwW$HMf!&q|VEAwCe)AM!*nEmL_q2+`>C;)atB7p0-tz3d9a0 zJBEM0daM}C{=B;X8r}Vx->qw)Qx6?%=tJwyK=mfwE1!BZyK0g23C>LY`%kdvX%%(fc41A!Q|TR87| zx+AHIT6mVf(PN1(exZL_4){WaDCa~7de`X5)Sp0&(pI4}S+nFIm+R4Pcr*woEQuVzP4DN|ws8g+ z0Cj!pI0nrFjjPH~W=A5qWaME)^U7@VPJhd~#kGqPH!zl_$S0D`n@`<67i=Rba54nC z%Cb7`>hgx@I`OdmBZ$%Wga78>fh=joJ1ASh8FIh|Hb5ziVea}MIg!jTmX}gzpl2h3 zpQ9pp^gC-rihG$oN3;C>044k0->Azcr`CVFC2M9(fO4k$VdJSt73D+~7&igp!6&u4 z1!#_*yzSa_&QfAPdxG0^3P-{D+A8=OP@C z8s{O0nBTjsV08n_x$nUfV z9V!A95u{PFC=?OL9?QjVef!B|BFd8TqrEPRPR%equesC*wTABF1+*hB{4tu2R$hCd zh#&6z={`KScR$SoqJzSTrT!Kn$`O{uYKO|WG02*-RSYJ6Rr`~{7D~5HoL$5pr z7*QT?ogNFx&HUjVi6X{$geyGM(~SRy^o6RlNe&~8S|O+7#}6A0@68Y6NTU6X-z6X0 zcg8Q2Rg@9wBX?gSz>+Sr%o=$ zkyk(-i6OG-7IL^-CxBz6C<$yxbi55LwKyxvcr)c_6 z+$hM5h1c|Jh28y%8pmt7`p`y<)U%y|5({d&ZIp$?C$*UHjkuU}YQO+)Q9&0J`d8Cn zTqnJ{F+;^)9k$uQP=?p3w|V4d-L3ZVG2iL->zjbVT-*NBQXSl~O<|d{0q|-3;O7&J zj#)cZtlRJh2Bz6{)hbg>x%aj0r;l^SFLU|HAC%I8sPepej1}~uxi0u*bt&F^-FIaq zTRCu-$@t-C@zYNgp?h*{V#+_XrA#q zfUcI1$~DWSi6sUfX<4*laQE5)U{3`$>;JW?sC>HU2YA;sH63A? zUG{=cNR4^YrAB_xFZ*SUy)NkYze!Yd{l4ue5cxqK{{~WwCN$1I*L^N;M#2TrU(K)y zh0)`A{T%c^iGe?FS~S|IDZMaF#~>5(VHYA_KqjeP{T7@lGVF~gFv`2?Git3Xbht`o zFc#Fn+XBhkKQWu@Yd zlAS;FYr-xu<>-_8A_EF&z-X#0Vp_T)Z+o_t<#P~eBWa+}Oy(A&@VGoY7fyvngXzQh zrJoYckHv2sWoxIPn8XKjE84#es(GJ#@y89ms2 z-#t<>%Weguc9a%71i~H79=M$;P&~be>Fm!Em@E9XsW}3OjZ3QPOPxzrMdJG*M=Sd1 zN!LJ)n)vx!1Rf$@+Fu6@LKsmU;}RB(R(U<^6{K6uHb>6zW^pU;tsZ1W6oxwC@`c2- z6TeX>@4Y=Pk1^kXTFk~T-b!W6{kNOZ0F~w-K!NsrR65W?f z-ReZqHCj(IrSTiD^Q~Sdheaa6o51AWpsVpYJNQOBS@?EwNaQtk#Yu48t_6_T0H{5X zZ>Z7RlHu3+^GPtnu-IrZ3?}|Ao`Jo955ZW0dpePa z5&h(~$3)PeJ(ar^e6@?|J8dBEd88#id#L%VNN3-X>Lw!`* zL?aFcfl@1%YK8~K>_IrvXji4Nc7n)-B=njCZu+=j1%e6PjSa=?NO=#2Sx zq=WaI)}K>zkl>BF0&J&>gn7Tp=rme{^Kcm(lJYhm2qjG0R>mgq5$Y~7mLL_#yCa5d zy$dCzA8RPsrp~`71aaCW#~=avEns{NdTI@RIx>EGT=l=h?Ip`Zf#D!}ZOj@hNJMzY?;(<8Lk|u0{f}`+2^%?+(;6{SHm(p}LOUvY zGHhyUI8VBfqqmcWz1aHAJ>XQH7Qa&1Cnj-(iD_-rqMokIYwU1$dPX+Hvsefh zTA$X=g1mt{p!?PN&9Qy1g)vY4u=_dt?eTs)d1`XfZ#^_7LevVh1;{{lt%snv6-?-()1kFuba0mVHzr#>ZlMui$h^-dUCrhl_LP zC2rdAa|yo;yj{e5uf5!gJSM+gY!N$OBSH?APS=Ioi4zAkfDGC>faTk3<~s2@y6V1G z;DU0EC!onyLS!EX$?@ie7YMQjPTJbOf}ySMc-~cEip#NHDCU;TRIW%bT-1j=@T^YQ zy1SV6daj!9KfZjixiFvfK=)YaMJE;swI|p>o(r$Kc-l-m-FB20*Tyg7b^1m8)aJI` zW$o6RRODKwYNf2<*aVE~G|qzGp2+-<$!dr+x%C~K7ivG)TQ|-Nq{SI_IhAH;loOS7Y1)k%k)bjP?Be8h?ipa4Nb#Q0mz?djB1JG=*w?-es(}op!U@OiR`_M}QIM$y=j-hae^PGJYAi|Ttb<}WAN%T4`!T7B$jA;3h5DfaVRw89t@9Lk~8EEUAd+pGT9 zwZNmS4aBP@y6(M!_}Yz~OwJh+wW`={n@#o8&uTrAJ~%FM3LexrxkL{h0izHOh109c zS4LnF%b+Pk+_m3}8Le}fcI%RDOz3@RvRL&ow?dVU1jj<7t&$nseueS2wXPrCJ>B@N zEjK4}ApLe~p4ZX>kdT0^3F`vR`S)Z^e8k%0C2H-plf@EE(@A*`^UlO{ag6zJtX6^9 z29cBUB*x%Ob7iG1h#_Cb$B6)0wdTJ5MqiPr70iquV>7&%paxnd)XRjPkZxi;6 z-S^`m=$Hr0A3M`Vb7HGLTN>eb?5Q?h2-R*pcbIF-3viuB(GQd6UTlFxMZ+XbH*&AF z2HC>rd`HIz#NVU+TjTb9&dkwe^xW(t#t=I15eYi;FU@9(OPDO%TL1i@kGeqqg%nli z!(|gEp`YcbJTz+k+Y+3k8RL4>SAxGKm+nhVS4%@Z&>VWrm>R?xjNp4_t_}aQ&Ow6G zKliAo1%9o#6{jyGKi*8|XJJ+64r};f5Jql~@xncFEmryZfH2 zV~~(OcjOz7hVh1OatO@DWU;s8sK79Rj`$3;VrXS!-Bu2d-h1PW{dlCZZyrXN6RUS$ z*;&8!fhf78I%L&f30$^cmi)W9NLIT!vP1}xhHwxiI(RKs)979|n0T=DfkF2i#>YhK z*HXq9LY~b~w7`~SRa(mlA^5w&~Kfye)-^W1|X0vtxKpOKe12n-^+t_;uYT2~;`s&dF;%)#>| z;A(fHQC8>xv0K+#P=3=9=eJ#tlHsv^Eyv%v@g8L+h^RrEzK#LqbsQ8+v_g$0#EBe$ z!}4)h<`WtSEv28TW@7qRG$$Hp_3Vb6JQ^JNE^=HmPK?@kmViQtMx@8r8Y>+q?kaq} zK#n(y$K%CdM*{Xjr$2o8_-RxMoSN8>Q<&h&4AQ#vHA>8wKNm+OWlDe@`JJEttE%3V zTqiAfE9y%ko;V}JRG}cglpRpRst_N)GGp0Fx&Gb^nf;9p#NNR6HG|BZ zf0kSL}P}_w7oDq17p9dN@{&V3rs_t(y%MM_3PB@jDn3wDGGGvCjJ|? zGDAHjs;Ic$Wi0hwXsLwNU^vf(1VBzIK=d@Z(9QXza-#*1DX~Nwlur)SQP4D2y50q_ z*7fC@>ZjfOq{6TE-Jy#utxUU8TzE)KXul!2-V^uX%woBo`*iUqR*yxWHHk#nY#8u9 z8FhFZ;0JBuzg`8uigdr;)CE4y79+<8Wgq6CMTg@X2Xt!11Mw`ar$pXCGkFB*P_Y#G zL03OZ{{W3D@?Y~xG>x9i0R~p7!V{VsauMD32Q`N1&gH+`-*nAW(d@g#=-G+ zqQ$9RiE!n$Az)CFLLzn43gGH|@*Y}Qzx4q4Y41@(l~zb$GJmo35xd{XH4Mfe_3bgS ztdTOYHs`N(1njcRkoQXBiYi7mI_iW0#%muxKRx-~wVgsH7JW@g)?&mCxpN%aD`7N( zxkLJr*>81q2HPlG+d2vh{o}l*8bY{flpsZ8vKZ8W3t>cm_8 z=mKXvUILLg%HmP(_N1n8l0;rzaT1Esj5%)g*QsiRU$;-?eig)QMW`f4=qid>oPS@< zaefAO4rGgMlBC1*-&;mn$gDE2FwqLg5+1g+ST&|`ez=IZK`uYx(q7D@57XMe( zdkO4aTScCrB1ddnRCl>QS5APAN+@6m`tO0Hj5IJduA8&7K>Ha`j)zWMe}U|a-aX=D z763g#tV!5?|po9Kc>~Rveixnarb3zv@a+93;koH5d9Y5@z=`8cJF0pT86AcGhGzDt( z5mUB#xw6BOY7sGj8yR(qGpT}aawI%Ppm1{j%->v6jsc#R=!SIl2?&hAhSFzOEk?zB z3l3C9#v!$IC}S)>@T->KDdgxKq5^G#iSt*=6JS5^`s)(TfvxIu){~{?1=zy~a zMa_%60%0sk_7Egg%A z@(gvQlm&gIl)4aGk?$n-zm^x_`m*n!uPIHgll_n%!$jz5ju zaO*iy#aox(gSM2-W~<-?{-wFX5?XBHupopmjcNZkFHX}nEdFl-*xU@DaS=eS1wdz7 z;Hi5taA}{Qn=XLK{YA5V?PI`w{rvJ|%(Tt@%bCdZ(anc|b8Nr?nXgNT39|^63!Udo zy^)(FFrYD@lE_sX^J2sP!o=T7C^-hhYF2yFuC*g%jHwq{f1{*{<6OV^o>%gdoH)4q zOrz7PNLkek!c^kpgJykOTP@N&on8qt@*GpxSe%&VZI9@#V~McdBIQfDIQK}4fMZSC z25f9({66Q+v*lF>b#+ymL!IRx6?_KlinXV|R2AuI?cS8S{W=>u;4oJNIfvFTSrlM` z0Gwn_j-$0DVq%9RUI?x<6=l>Sw;-oTR87(^v95Rdn7>(>B&j14Db4h=w2*u|?Xfc? z<#DCrhC`HYqQcOYG884HWpU>y`^hQ3&27H}Sy&_ldPR=XndMtBRdbfZ+!?rtcpfvr ziee4fRyy*V0CrG0&XST#i?F+Yt$Sf)S2ah6(N(00&Y>S)v6p0pqOEJ5q_G8wE-y>R zZi;!NFB)^68TDmREJi3=;YN6*LRyIObSb4=!#n!yPm;Oscw9bB%v^5le>4 z>qde~FVoT-Be#n)Fo%RL-(Obf-)Rq^@B6~EC3@2lpUtRR^7YwA4fQm4zBG78yPmfO z0Kw4_Tj&J*+znr0mZkr>CJr}a(8WOdf1DMjx(=bqVrMEKT{R;Yv(l%8X&Cxcjee`G zFOT!Ctv;s)2~j$Nbb~oIA)`+Kib_#)OI1pLkoP)Ouy0*talcJIPUht?n00DSS0?o= z*%Nazi33WK{?))W!Rj*DONyHO%n=ir{o`uFvooYNYca<>Ml9v7lA-XuXi8}+r&r4uleV#5JBcQa65vNTCU^k-&OTR*`7jf|Bo? z?>*DDzSlvT>3Xs{**<_w^EsH(nc)I%#y1RwSnV9J``efn^X3t@t<|PbDq%@lAV`v2 zBTi^tEd;$R6g|Fb0BdEi_vPxtw9TIruQPxGY(C_an|BBlyTOTe8Xm_u+vN#!Gj_qOzJ?NUMsHe3I8!W=#w=5EJe>YJYJY&y@&Zg zPmQ&HY6u1`w!8myrd2U!4$}m-ugb{T*)Eli)*}LBP8XTcTO8GPEwWZqM{#ROp6!e_ zp!7jF+_UyxKYKx5@3OTl;Q*QJi<92j`gcH|8a`VTcL)-1P2yA}c`Cd<`eZR`=sTFk z2!W6=s1Y`=1p9KMdJdVBuZ9KYUhF2icbD9TwkxzXFRD93xy3ZLUlg#>f;szvdNZ?X z2KcbmzvDu*JwGpzh(o^Glab}@4&0@a9o-;Ud_bywmYZ$@7-R?p3X2|FECKgioLs0l z@Yus0EDHQhgUsmgr%r(LA&Tz!zp?!8_S1Bugqth#C6z_Y^jtkj%dE z6z^*eG73E7cq(73@Yjf*;h-*$i!{;!FOIVtKL7_BWfK$>bhe%kL@@FvXt6O6e<6PG z`TK8Lzw711nNE(A@4qiZP?m3VWnG56ynZu248c2jeKYjo&9pu@%=bd5s<~szadQ`P z`2&QG3F2`}xQq(+@BTE>73KC@C~(8FC+E@;cR#Vi+1UIgrozUv>?bzpzlXg3(T3ovg8R85TKah2TOd;D5S$d`yD5^yfoLgO*$P*8|iF{L$YW9s-4% zCMgt#6b=K{l9Cj)8WpX~W$T$8ROWgUcp%11rlUd(P_xw5WHSfnTnS8cXD;Caz0iOmC$a`=ncUj7(5z>1^)-roj%}vM z2HD!IIliSC?3|rbVBuV0L>IqomL-K$JYkT}N&r(mReYmGGPReCKe>KtVf|2;auRiI zc*gHnJlKn2SarJ^!#+MiT>urAbxer+gRLTD8cZpXng-}L^Y}JaRtO-#aQ(s~mktE! zfC-{kdmzw84gw5toY-Rh;yUdS+m~Q>oW5g2w0P|3m?f)0aLgC#atHIU*kMP}2NajF z26NzGedNOaeU8$~D?;U#3DZ=%k`tH7I?Kuc)1N|9m4Ux33tc}+7pdVuQIY_^`g874 zBo<4CrPNJ*quwH?pnV@{D_~HDe}?wD5Y*}s8&Gs>Mnai=3$v1t`Mhx`=kx^D zN9mL=PI4`J$ySQYh{LA59CjAQkv%u|Vby*V{G$;_i|wZ>L@BWj-79ZdNa&s8-?dgLHkgt^Fho_Ofy+Pui^-SI0>Tm3!zV2#id7bb4H?U3bH47-W>PEZ8*h)H zkGR%hPcpA4A&p*#a)qp|v}?BrsXY)gT*8tBgaZ@@18K^3^c!ej${ zjzozW8N8X!<+Ck_i|>})S@A{}J)eNI1Y76zj0?Yku5RxCoFC#y{o+_={$#_3POxM- zuy#1GvPcvqVCtMDo9zqXZz;oaX@OUg1A$g?Z*7nvVe?Zt10n&bld%aa4(sb>m;146RH*`P{C(o zdcvbCd~;MyegHd7kO6DchxeF}yi@^lh=$ekTM%QR1VjLN1_|jeDdMyOVmPUBGu$^I z?ueWqzxzD(rN9?*>~DBdzcy7-zJ8n#0 zEI>GlbF`HeY+f%@yYG8vGxWgGkfJM2hM~iyliSM<16p4AFjhI7GG75pkQgPG_s6bs z$tLA1;7fh?2NSs?7&ZjpjSY-Lrt9(MenmYjG6&CAiJFVwuZG6Ns8@0;$w1U@k9p5J zhwGecN}c40x&y4&czRRx{w7umnVpl*?{fDSip!>#tz&BuvqaRtN0v(i6PjT$k6WC! zP!pr=*+;YxXG~Ta>3&fvm~6~DCFgsyFuQVIgcYw3RK6?DU%fXdU%}A$|r#POIy5)CJ@OsKe zYbZoX1c9zLC<@Rpz<;yhC2}Qk7QWM%OwhW%;@DKpG`Db4rBSJh=C(ftVc#IZDxTNvAQP5Odg}Nz7i8h$}zCXpz7gWzUej761*oN zEBhu(ZdXGQQ9sb$Ht)nD`~ie-i1Yha`}9Z1e^W7-6vb&+O#`Y39H*Y>sol&QJ{j@F zwu?Bc1F#4kObjIYD&*r56x6kGh7tX%qJlvsYKgQ) zlvoh@mCmxyc8fr1ldw^BqXkW=Y8{A{gm03^X+oy|*fN&L&QhZ$x|vAJAe}nK!@>G1e+9vj&AJkrti@3JdA|Ao~PE3MP^bQn5$&i`n#0}Gv(y)O| zI97UJQouzJ4`u*VX*U4lvxMx3!T}BB=%*Ui{X=rMhZTakn zYd{|-eW#3qI4S4pEK6T0Ml&0Nb~)}_2F7fjmzT#prq5Dr{3VQvMA3*eN2b1fBj3)$ zDNo}_R0L*4tfR|GIc*n;{+VIX8N7;=Uvxn0N+JdH3%E5OqoFwP64~ZG#+C7YHmdmS z)r=*+lSu2tOZ02g#nG{5_5cj!kcQRSn4J;bbM@~o25sJ{BT>M^**L}!l5|G*fMk1Y z)3^^O#DX>Lz49lrrizD_$ZxCa>kr-TrTd68_deHN8CpLTwhmi>ow#16p8iBF<8ZVd z-b~wgod4b29T?zNBD#r6!%BVrfu-IAiOHSOR4AR%#3pRl4Y~9DI-`%oMQm}xY^p1n zAy4Fp@XNHzEz}twT6JJTYPe{X+APw}{rA*p!PR!`H$|F6Ibk~_7d)N9Ax8KKmqCPM z1Pohldx+X3-lB?A+wE$CNJ+$N(rlSp!)~5j!fYBeZ5>}d>(-0Ryd#ME)npu0U6Qb! z>LSl+B}D}jhwR!R5-!Cy{$@~rpg+oru~0v@oQV!{`2CfFO6=3+Fx4xF()~EgJpF9! zsPjgTMSk|W7m=(3$fC~7%=Gn%L#A``@&Hbl&Uw$sbr+*3Lzp(XRY^eJ3S5RdYfC5) zUa{eBb9sG+!w@p*P2UfH&g7ppuTWHm7ysx$$x3Zs@S|vl;Q-1~1cuKFBnTRp)S255 z16k3U=kU)lU}F<1p!~3&@(2-E`SBg=t!-$g_8~6OQldlOqS5JE=uV5eLper}PQ81; z;!hCYMn&~gz8%|U&0OdUV&g3B`#*>1F{HkblEWrJzLWL}tt!I8&pgbDQX66)abb_@OTE4M1E^ zL6zy7Ha?cSamqZDA3g=-5SEo&mt^QRpl@^)2$9c%>$8UYAH}?Od>8$_h{NFf!m^hP1_jGrg?q)ityPN6G zVVJJz=ICyw+jLD&n~vdoe}4G-3wSu^zTfZbdcB@cmdpNqektj$?jG};kjuxF=5Z2 z6W?Tzu+S9q)eQffr_BTmB1C%4TTq;MWhN__)Dy57-;3QK2SGG^Y?T!Yia5*#RUM78 z*=gL(>qsbyZ@za8et(e+_&gYom;>tfLN&>bgGCoC`0ir=9pWk?`L81q??{~Gtd)Dcx;VEFiAH`-CDjA!jMZ91 zpeV({*=6n)#`s;J;=G|&4&>0veX=e;A>MIxjadsdXKAHryJvC`QB!W$J6qsUHT%}U z$EhKiHe>iD*I6XE5n`$Iu!k|2DXyJqwX9Xka7#|~(|@(Ean5KI5{d;{+8;jh*iU7D zyk5p6 zHL0X<`w~FS0JR9Mt54wu1~WYd9F*vFFrp09;mk1SG2f%AKvi&(LBLGH(JM~%tlR)3 z4#wG}i2VfvA}`KK*FlRNI4!G2ajDF=@L5Hp_|?1b?#gLD9-fS0T;fA-G6Erx@bU1l zaI}<`#aOUat!9Czge{PSvlI^a*9yT+FG!vnHqs5J9q%N;$Bl};C}!~S3`9cz1gy+Y zjLH@>@qt8s5|vz%Jbh7expFb&bki5C25pfgk7GCN8MA5`(Gpc)T7G!ZYOo>c4#rR4 z;S&y6bC9p-%Y$l!rAS3>#Pi#T9To@=O)Q4z`NV1`Nz^pD`)q&da7rOAhl!Q@u4@0a zH#|-fciTg}n(~)Kb9eZgPwtemFWB-8$S2o@Z#Qk2JzM2tHqyUuXU!QZfb7fM;cO2t~+^YV87#lL9r^dp_RGz< zVelSm2$1o!`n@Kr=yx<%+;BJr%$2#6I_Bqr+^1E~7PPyhZp}fAh!Qr4m&Fj6%5l!m z-%7uXU15Td>BABjV!{&S!EnSVi0T_3gN8J`F8bBJk+V7+<_dKrLYjqB?N}c^$ZT*V~s*XOkxO{msra0ZnKO81C>N}0Sp_e`& z5F3g(mAdEHLNv?{lBJFuAZOpF)<;@SMt)$rtX#nj%aLqC5-l#U=WaoO+1+SGem58I z%t537IeM2**xC5))&Ds%RLruRi9+MTRlsI#3itCN?|-c}trr-1f#5A5A1RllQNA_W z&#z>}?_IxO3)Cwsycb8*+wf@Q_zC0sQKjWHm4Z7{^sl*{PTx{9!bBK*IPsZ4ONf8c?M)Z9jf2|8XwG)CJ z%}dy>%4D;>`IM@MOk;<$)D_L*dal-BIShvjX|?JNP+30RspZ7#Avbd;{>EgWG&Ja9 zAI0>&B97ebns$%ceyD|&bMR2*iu1>7tktB$r&av&y7SX~q`pDjj#-Cy=lJP-O;BFY z#t@N4;&o8tn)swPva{3p@vCMLYZ3meEV#VL=fZchqq`fx;w+wdLYbH~&y(`IsN@`1 zTQ2@2QpzPQw=7um%xQ%B?%!GHYs}d+DGR=Eu_2pEQb{P3fF0X ziqKZ|Y;gV)X}jO!3lNV1tSZTK@n5nyr!6HL^h>&urQZJQKkV|I-}}fVb-MiB0g8Zg zRKk7_Dh;ah(k*L~+icyBe-gDi3e_E=8fWE@npRK$_4@>-H+IBnoTw64%H}7qRB{A) zgeNxl0&NQ1&0uF2El|dL`n4W6sNNVFx-?TLWkyUy>H0e|KKwpH6Xf)vF1}{)0y>UB z!kK3EFjGgB@kBOZb)2%p6d9cKVo`DOge1)*QU+4II;W68i{HMul6-mZA8gPrAAmFp zP^!hFBEk(#eWkI?S0Nmjz?K>#17|g+2vuh-y?Yl##r@Z3{oI|!pmPsxWNwN*h=*Rj ziudI1L9gq;qttngQleaW6oNreKqWVVPgVK%@O>N2(O-i1Okq-UTE&v&_2^OJW3mfX zSx`|$bq65_kSK0L9ou(oHKfEr%EBhK7)77MHAfMjz1mqz%l69T`VoQ8PRxSLFehr# zqR*)CXoib}mW`to#(1i&C&l&SPRoyXft9ik<9Z@j3kwV1S#!)075!DxM27A}FbK?m z!jU6q!|YJ2y;z*3gj|XWNAolZqWV9~-?k?;c!La9PW0%ZC#ZofYj!a7hP}4s%JWpJ zYBkl>D-vj@z#hc^o*e@myF1$|U;)&VW1e1aLjfYza!;VQg$Ho)nzde@#aXXB{ips! z9FT{C%MOM_Q3a*T6q0lD9^az-{|*J3M?MpC=_11ENxgXwUjR=m>}bShrnPP>j8sE0jhJ(VpNU6)$!e4+R7 zqM?$7br58+_eF3qWXfl67r3oV?NSS2P3x+6NW;s$7 z<%{SrC}bgI;RvEkanctXPCfb#8HW!NS_jn~T>%>)m?UxO;gkF0#p;yHi)i$S+`EDNNuBv0vYiI=aez-IN zZEmixxHq@^~w53<6zfB-KDHF+<*`{Yj?Y!sbjx$sBwD3xRR;(sv7H8Sc&%t$B$Dr5&g z!SQvx?B<>4hHV0L)ZqzYd0`3SNE}qeiSQsmA>;|KT=jFd!Tb3njW8a*#1(Ep^V{?M zX?x>y@sCmUFWy~eed*GLxh3?RB!aC7Q>OAmvNkEX7&%21*S?kaY}cXHf$LP8?xR$VwMg$f&sys z5)X?5HpCJDN zx5-kv!9mN1oUyCAE%>9oWv-6_h}Y>Ps|~34MX(#s@(XraEd5xeD-0{tyFP7qFeCWE ztql#RIMe1fXLO-7+0oMnS48hPnM%w8wSJ6T{06B&7o30j=uO5GHsr^1Qj((wwI&<> zKMO#e?gp8(SW`5RwjW8sUhGXh{f4aeozQ?XSotK9Ppi`t;d{t0T#lC%hQAlH@wrO8G z!ft=q6!C{{oj{$c5XC?$sSHxVn~kA3G#I37WDdVe6iBxd^Go_wGBfR#px%m1DcR;s zJ4l?VPO8}Zp?yYNR_Y^SC}$IW&8wcq1ngqYo_d@Ck|H;jY3p#oYz@3v+})t%&t)q^dYe5x(m z&Wj;*38+{E5OFfLU>>Q|;wb!xzT>`7fCJrvdw~7!s#WzX$wdg~8M-R_H+LvpU@M|= z?S7t5*XztPO*RgpYU^pGt#I}tLa$D#2qzIo`%Hu)H)O2l^Ekr(H(#(6_1afZ*5?Z} zSkU(xkc2BN0ZcF9YdFM(cx{j)JzrFTK5j9}`C%r%sfn8dA&8lm;kp$tgKfTi>|}g! zTqd=&7V@CQ(r!_d(6}38qu_{r{yB7T_-l$2773-lRtuWK-kg$6D>8Yi$HTHyOju?c z-D`jEXVF1=a2I;Mjjl)5nWBZHfl23VBV6%oGVF(PGw-{7fnp?)si4e4-z4LDXSJV8h+MQ=%Lxr!g$q>LlOlf9oCceK-f$ z&qb9~X&TCl+Sy~GA?uzKn$pa{O1_t4EK^qe5%kSIfXUL-95;Z zh=tE5Q^E9-^Z{OQBXi%-k3gn>{m30?SeC{CC@uZZdMN#u2TxA`1PRz&4zTIzeFoBp zAmfqc>P*Jx1kKD!UdJ6^EOO^o7G(5ny?gz)EV_!RcV zjw_4#?Eq2v|82FpupZcix=A>{FA zm9fEs!U*z(sVy%$8xG&0s5gyCvNX<&>*kFgBNWeSFhrNZ6+k$^{kDr~mvH9FvpcHr z5&wPjwU)Fid*3l#iwCerdbE`v5;_NhRGkWSN?h0{UH|5uoLj*z$(LGKpq!N1} z71GT?@^3meANDbeCW6djej}$~&sZC! zt@Y1kLfM_9>Tl>s{JWUOemlv!E8WE|i!k`l%jkKPbIXT@pa`T=IgBoN)7vKz3Amy@ z`2Fq^$5R{0;{C6J13)>=THL0szD3C;B}+gx7SiQwf(vh^>rr5?1TQ>00EUN6CdwkB z7(^$3p7f@Wj$(f-Qf~*gK(w4o$!W8*t*s3QPM#s<&^xolL%TLiQkO_*TwO9ZD+>!Y zjvfTe7T`^o!eGgfC~)9RAU-MNVA$%W#i2)g75dTBkHT{Ds{1;k!6OAopS&M@N-06S zsLNk}y|gg!Fym-ND!F8)_*W&8lbh#ZgZ>oIG3CQaw5;jtq*pGj#aUuu_-?T9n2`?# zgfxsG`ogepe{dBU?eIqjMR%L7d>_L4tw#p zgu0!h%S^BFq@N|xifE2R-)&!W_-6Az=g`RYPl(|sZ+@b$fKFVPPpw3z?k#(X=GD4T zrr2+JiMu`LJBa+T+7iGEsMj=liJE*bALs*#xcf<93CHq2KUx*7@{f{FOHqPNCJPJm z2LYd(v4u+D$ttMrK1rjyT3%Lk^(S(D@a~;OV4cM>sMk7_E{TeODip8{4!Lx8HEC84 z9PLei8zLq`9)QOmtI7ABt)T%?@$ig>h)8dDSV;bllt;ndJhmiySskRK zZ_wrWvbx&GO>T}Y1?xfp%3cmvwR7`E23~itsze6ZpRrd1RLL;GM-O5`rVTlSL?_~S z+g!@Ux(*^AxLVR4HsjCTs8o!TXh5Rot7lrdhky+Mgdlen=OkDNYt5Badft9=Fqw7V z^(JiU$VI4ftd9vQ!j}A8`EwKJ4>@*6RF{0Z;MxeVaD@X<1?$Vpib-OHo51h&UB1Mo zqUmwG2e_PdHP_aD*?IW8zrU|l2`{Oe>OehuaA28PVuC;9vVL}lWWYs)3T0S9gAkCz zB~H1KNCg8*1Mw4LXI~Q&-8>ao49!fcaF+UHTNf&Ll%%lhkh<*hlxUOedJD5kg%!x!poAocR3R=C3)6aQw?S7=2MT6`bKSA&Jg^;dDca2it@)$Pi5ASjfh@Ihepa-I$RvkcrI`zs>qiv|)7mu?$E z0jek>2k$jQ0H4HRn*mVn5)rrA9>Nwmgl$oRK_Ry_>=`Ei9H4N*`9`YebnRD`KKFZ3`S01>h zA2;zQlAC4;Y9D>eapKWWOCL85B^S)e3lD(4%<%ATfoJEafZ-w^3G|o(>Zr+b)UY$@ zwlDzz$Lf|u?uUVQDS#Yr(qjh0W?6w*isua19p#1+YKn-ofX+AY@uwz;5Q8o;UhV9S zeqVI2NWLO6vXCM2=dZV@+sa?zL;kUGF149DwZ2H=RJkSrpVDSa(8WAX2ujyY#Kh5| zE}8+>{@D`gG^)g$Fz*^BV^^I>Ps=Y*Zng@`=hC$u->YQINa%f`Yxo`cjTxf7q{SNe z?1~=|&hpfitHBX(CyZ@>%Hvr{w(CNR91J^att0_0ZtTD&Fq<@H<0lci`Qe9>TlR*4 zyAdy(=oEl;rKkFn*>NDhQrnED*S_%Y&1|7eKU?jny9vHIKXNFW(%h0!APL3~pytrl z_83X(vut^?rUm6q_jYt-L6NAudUC17V8+cQG&?o4^X3gDgG5PZ1@tW0b7?|!GT}Mk z5|&QleujsO<8ApJWYNa|XnlKqE@V5s^I57f1c)fWK@wc#*4_XnOJ^vq;6&C&~)v(X*qk#T^D&B$m}h*Ee`3Raga0FnP@gKTQ!lBhx5Q0gUFQB(rFe;NirsavUfhhT9z?6 z6NG8<&WOD3iU7qb|GU1g>zRAc@)c|-fO9LLe8VaZxrXh#uzYg2M@EJEA(tzKjtsBjpV*B5$ehsr~ z6-t(BMkcR-Peh@=y@-a7WmnCN`?v1EOvn%V;>;sJ4GNmT-r3t0yt!rhGG-I-N!L&h zV5bYa15{n!Ex-9OxZ-3DHRE!}_w-D#%{}^r^`nLaX7<|@^inMVYS8uacOL*4qLPz$ zk@>6w^LqkjkSr)~ud6|3__Y2!I-*Uev^hnM5Fbz0dfqB0KB05cI-_D#`s)h7P=DTw9iP{I)24r4y3@}{g zk1Rwo$%GJ?VjjiS({uL_HcX1jg{(=RYW;6lpN(3b9TJKdqC(gJ3)I{aMM2}H5jTqS zjcUaH=EMZtkXr~C#`tBG4@u4jJ8RR&fsCT{rV#0a6mbsQL~3{bV96_WTCy{p%I}?i zfXQou4zpJv>P8=}%!Ai^16SKY;MsQm}ayq@bI5=igb*;@-2>avIsa0(pDQyLemQTybn! z13WMQeNmk!rli8QewbjSI1u;#Bi<&n!dvqC7Wqjt5GUBC{FKf)^Y+DeRpmFCyu=W{ zqV!_n*9TT!!AP}nDEp^4c9M&`*x6be`lB4$h`RXXLt-7Nem*HA9K9BUr z%PAiC->!ccz&S$X)XDwAatJQOScUs4g04Fw#R_=&{Th0eOT8;$R@~2Dx(v}lttjuX z(2Aad8Ci`B|D=0x>u>SUh+7#lSe=dB>9YTBGa7>aK0Q+`BI(n?N0`XQgV$N&O3t4d zN`O$HXyhJBibt(}|MWV@IW5@G*0ykQi_fMa523cPY?<}xs;jFriM8Q6E=C7q5XYGk z_u(3nbAII@xmIS;)mJ++1ItmtU(8c|gZ zm4Td`B)JM(Xz@TJIo;y1ocUGfpZkst$KjHeFcW+_&94JwDkgLe_UGq!caKX(C-?0+ z8%nN6XG}5_hw^h5SzsjF2ehIv7wb&j+VD;}Wt2)nAqeIXMnl zi%2X^1}7Y`w9_2)5&Y3oU%$>ik)B%6bozr%cG3|bK=*AXUn>|#G4YA>s z*Py`sv7(e(HDh8yox)?O<+3}MUGzRX=!zu$ip6r;LK0>wSPqBJLVHCvO1Uk6#=K^G ztx2cVmHeJ8+u{hkCnIL>cUtZt;rnoLwTBuT1C1{I@V6sQj)y%p`p60HPSxLBLsKx- zX;OFYz6q)G%YB6Sk>bT-Pa~;sMKuA!rn$hXBO~c2jPGT8XrFi_8|jsJs*vE5w%NcA zUx+|D1EU{CGQIaQs^1p;_jE9$hSTORnYUv_3aQAj{5|!eE`N3u1{=3Rbz(2J`s>8Qz%iX}# zLLLgM`gBSSI_bL)w5?_qoUETHky!Zz%=_P}0oC3B>FXsWz<>tCkTPncOyjOhkXgGJ zn@pbLIKgS7Kw$M^hX^(7;{S6GfR{2$BCK6ZOl)lE<rvlq1S}$#Mi746_z$H7@h(bcZ8Snj*>K* zJr3nDK1;K7U7~@As#Jj2$aFnltOh%)<%$$4O0N(=v%`lrkcTT1oCNK99XmJ}nwm-_ zr&I~cj}iZ=2)2NQP+|tNFFu{{GQZomB}KrGN{S}*A4%Kb8!Ut4HIIO~0*++z@pE%= zf7Irsv*=^kP*5-+-}5KT&0bHE{&?|KQ1aeZ&Z#e>iEw|Q2-%N2_T--hE9uzWEA{#%H~82Bq|OU|CS0*ZP8 zUlnQknIU(#{dr-a!G2dt z$&U}eqT_SM9A-j0JT5d$?TGe1TmE93>(j*GhLsC_-nXGktN4I6{vZwNU`tC?y|l_8 zt7iJ8M>y&7;Z3EJBXdTVQc>qJbQh|Bjhoi?7pJE=hHO3wf{hdu^bzkF8$PT&#W&bP zqp<8gu?Bh}4k!H4$&Rurr3L7nNFi(}z8mL&>N9N6v%}-KM!i|19NB_Ny@xrTd@_q@ zBk*Z+Tyn-yW>V`?|LwKqDO3#q=k+yms2C}Tm$${x?{BdfN9_^)t=@*-MKV%R@(@1y z{Iy_<$k&2ef^Ht9*Q@<;7vewwx8!}^b{HeLBxSOT;eZO?BP@^;k6jPEzBs6WlR->j z+1lA6QJi$whvC2d{UM}>;&ukuqsEXWTgj3Uw`w;IJ^6DF0r0xxbOzJf*$oe`SHn7$ zO>@_Vkn5>N?k&_Xpm2$Fug#JBaK{HZJf>i|UL8=ONhPX@rCegF>b_w}i7}&%!kg&| z*r{T#Vylg;<1Kto{+VE6iY)Cvfo$#nMr?i7SSP;mZcooer@UYvkgOzR?QlVq(FH zy(f=2;)rTat!`9>bQuQj9-L1S@%p?d_jE8~FCI7X#qaRZ%oUC5^`zsEFVAl$wbk}Z z1Fc(}>f3++1UY-2jv0*ry9s)7OhK7prS?T@dpi=yCezW$2?&x^PgehT)j|j=&Go5R zv4s__{=j{V3PA0sY$_Zv_(M_xoAq~HJ!In~nM$Z5wru@24=gPz;e8D|25gphNvstW zIYH6rLOc0RKNNU38uzZ~Cx2)XcpTAjgrf#!=9jG|dYO-z;kK)O;TMAnub@Liw;P=& zy{gJ}n|+)S_Q+IhlH`ja81jzT8a4c#9{J)@A1>IhX+pwOt3ueo5;$daF}A_>P5SZ-lJA4O4I>amGsHAYhGLm>kBc%@*xRnq zrNh&#Mq1Q>D%31klG=$M81PL_WD%|DrkOZf*aX2-pW&&)l3^thc`DMR@617hu;oLYya<=36@Wg(X zq2GYx>CNqu5%4Lw|ItPI84}MUVfb=oXg-p<-l%mrR%+(LubXPr=DP2@^~^$++x2$Z zCxMuvk($UHZ$`b4b_L(~1dj4>$~D2ak;CD3;Ysxh)AP91 zgdjxF)%jbDx^RST#&vucwAka@_ia0`L6i$M%=>DUUmf!kT;k8=aU@!|fm8f$ z-;jN!Kbmo9LD&@PbxI&n|pZIao>Hh=@fXfhOL@=2xN+^RAtE*nogErey>dn zR~Gr)rf6@yb>BampNYbY8#4n+!m6)ZG?%M>n;RPjmd%EiX0d9vK(2%i$?gJE4UWON zoTxvj`I9!b9#UOH14)^&mzOr!q~Z1?PP$qPIob38fVP8jl)VB>Hmx@ZPzvbe5ki^9 zVW~mJfPu)$s&eZ{wm7r8jjm=Iu%$C+Z7AQ5HJu1_5CygIz=p@08jJ3jP~VU@i<&Qj z+GlK1)uC2`pW*cJ<-nrJs@M?#Tn~MqY{4E;3_k*R*yBft%c|0(#MmC|YvN_cTCgv+ zL7RBsG4{P{;&@hd=18}qP_W*8XQ>w?{X+q44GbEKf2i{8v~4D{_(rIPfs(06=q(^{ zYGf`Z?;zoBSISjcwnS(cdYzvgz9Xcn{)>zH@$`$;pxwQ0>=5GCoQj!3I`#1M6;A$@=K6Y| zjJagnbR}q{r`I@Jtj0hejN)+a$djq0kDr=R&x8l-o*&BAsP6CPhBcOQo@0UIVH%;- zD<8vtazv$^vpO`Gh?v9K!xpVVl9u31Z~FYn+_F~A&&;QfxkzoNd#pF`JG+Yih~S_CRED^8LD7`%E~yG{bv$THGgFu8NdM zSk>tgZC~HDqGO#i!B6AKfsy(irSm_rb6LGs!L2O~rnV&_r|8#-8vQ;6RPRHM`3)4zZhavv8cK^<*dS+mX-@$v+#Y%_s(`4@@K;)jl!3mT{&7hZUPb)WGcmrBQ?$+IqOv8ePd4MbZY#(<8o!;Y-6>1hPFmZTXvdTf7f%#LilR8ob@UZ3DT`POt?zg{5UNu_~B? zFDuKxy?_q3IHmybD9O_Ujc+=7#I6tWiBxhOE3U0X5DW!EPE$}Z6#<0=A(A`QvHX}= zPvHL5KE?C}kP!jke_ozmhX;rDg8-?r2k_4TC(kD7!cAK)acZbRSD5HvHnm*R5#4di zs#}u}WlTTf)Mih2)9u_iD;ACk9Y!dZQgz)FVMXO}(@U4RRvpaF=b8#Qb?y_EnyB{7tHZ4uRT$JY!#$i%={@}P83UzXFgbj+3_{{_kVHz)D6p0e= zW&6-_%#&%#e@nXj-SY8Q#V&#bOVqL0+MRf+U}JtN8bx4mjPQ2NZyhpehr@x;(e7oQ(jFd~_mw}E-zs1ya|t50udc5CQxyQ_0$?IoTx=nUEq>sg?}j>%y~{ySvKdvZ zk)v$o?fp34^K`@p2C^*OdexqQCTza+LRaTA{te<{q$gqyVi4vHBmqlMISyM{ZkH> zIpp{>6bQ}R%{)dJFm?6s$ZKeFRRc?&Yl@IIU3f$A#0^&$4ZNvM3n>Hx+28UUf@4X$rE(!hif<>3ca)DT?r$R zkgS5QK(T-U1u@MBaiF|ICLBh|-mEv|jt`j^@~favK7|?hzM&2G&^jc4{ZHnukfTN$ zOhnobuC^SH;{;1O(4h>lp^QRO%rOFpqayNQO#xFwBWaq-Vrm?VnV(JeyRBCOzy#sZ zAw({PE$yAiSe{M~{*_26Qv^XoC!vxMg8&edmNhpiRX>1a((qTs)4Gg;`c7TTC5nYH znEm;A=+Eq!`oxndfld@?Za@hth79nVR?~*yRO(gU3)sNa_WO;b%%GKR`u`xh^F52~ zSxISXiA!CcD}fQsaHI@Vk$-uDzVTCZ{~aRw4>jwxJJiIb1lT)NjPb)Hf1TVmvYdB6 z=K>>%FWce$dq7M$s{8Vhl^9S*)(w9+L4Ue-yG9rKcbO+*e4tmod2xH(wCrUsvhm}e zwzSTuYjYaxfRxkK5t4SyT8VXNUpSe&a3lP&2(W+Y;>Gyk>7v@e5x#%mgsLhRE_J$tvWCVOzHTVj&> zstsY)=l}G1q#-!CSI6sDtd?bCZS5tL7Xwn^XhxjL#F|>qm6;7pwBotNO+IJ6eGAfE zXrNH4l#vJ&j(i1?mX>A{c(Me8OVR?RWx6A$&mmC&@&X87sa9)EBN=EJNy30!m?!F) zLs%A%aFwI8#S?2WKixDP>fz(w+u^Y%~IHZvaD8^^3$F3zJ(L$1!Sd5xl$NJR~ zq{+5j80wldrSn;#C??oKSAPm=m9zQ0&ew|-(%N&TfK3fhc4lw4=Gs9%G-8($gLg3r zDr57ut0@mDOQQ-WA0P`Sy!pQ2=;h`s5VPx3jTYO)rfnYLhI9QN-^drM*sH^*w*?Xa0 z3_q>XRMc=PI5<&*2XUpv-;NRZ_Q5k%{ql>m0Ei|$<1?irt@FzrGXa(7 z4{#wk`(!1zAQ?OAw_R|&Am5jLEY)RO4=syJ#FaE*2IgC=s1;P4f~ei}jBrtXyfJaE z6I>&`&qxEcYiB@qLw%zrMm*AlO>NA?9P;I2WXe@pvpd4RfiIZJi=#QJquoY(KcErYQHhmaNTlxYoJY}Rcj;v4lCqiy{4vD z^>Mb>XDRT^iHDs_HSVyaw7iY>aGnVGAVhHbJC)o#*Yvd^ls3$|S+9vt8XFM11DfsD zfZZn`oO?HQ*HsEAKy7&)*TJxwPQNn1PVDqUbE|b68P#rR`6xTElDe*ZPZ|{~>Qf@! zUeXunc&7_~YRNA-j9Lp`0SCTbEB5K}Q@y&cAO9+>RPFGaTN6-kufFqacw-mjD5kCP zJ)JInuU&fk^vy304x}MJzGXs$HBK4JK3^l))_R&aC(kuLh5WnPbKwSyyjZqD5q_+- zerNal&w!up$QuhY2i|;0d>t@Yb%)Gut=(MeT!gpBf93HiUYbNBGju4B;K+^HWapAY zL1@G}o%gxw6QBQtxTAqEV?nTUqG3Vy%Xk^gE(~CzXQp>Q2kR!Yq{-+`IuIB@J6atU zQC^+`Vh}=8>5g=Eq}{ETjW;kzE@`kSJtIX;41z7iQk;OY>S`Ss0 zlVC2~+|U;2uho2ROFA-rIZ&dP#FrG!gMDutLU(%N$M%O3$&7D2CU(NK!2r6D@lg&7 zE~@{so73t8V1xelIx(%&Jqmr>*m_y{@v;*5Q2n+))Ay6&D3Y#a$;-(9aZxw=a=4$T?4uY z&W1&VZ1RkZ?@Z+jW(La~s^4odhbaVx`BlaUY7)5S*+e~m$SHgm-`e^)#fV^T#ctvJ zh7ZyK=X1X=$4|geU>gIGfmU8^qN0PvAVm5N%HWJFVo*O@W8DQH5J+`%ZteRdC=hdG zGS#B+_e+Ok2PtMxh12f&cuR+GF0uRRla4M^fvfqCrJL5NdXYR)OG=EBrq&V8LXV^y zg5SW54ff~MRD91XFvmNUa?gi4Zj|xv09}JVvd5RLx0m4${-+ZeoV3ch_$Ua~TH~8z z!?vFsl#hO5HnVY3gw{42m_iERN~)rlQ)`Jiw}c+h?+O3*GQjD0d$klOqNiUf1bzn>p(My+$~CgJo3 z5<{;UD;Yr`4Uk(R+G#3TdO~Au7P!tmG%AWL0TNWVVP7~%ML@QbCsG@rX=%yep;-Pa zgY!P{Ek!%7q!u^P%};wvMiqPU!sdJ0D1OIL;;cM4nk2;mMO?rK;X>B z;bpZcB{b3#z>mnY(R@m3{cvvYwC`;QT>?DqYDm;Y?Ez=2S67%A)HWeeVi3Q_aiiCr zv~G>PN8o`4eLXIn?=!xCY7zm=z|hiwt9Q4cPalK=Y`u?Wjb0@oEw7Vr4;elG#?pFj z-d=9KjQr{ZWDhdWZi!B+r5jY8D8OV%;(A{!y~K&NyEp6zc6>q3MG8E*_+Mhs_H12K zLnC-xVPI4)A5vh1x-&a3zEl@>WnH|Lf$0Qx5BC3T&^nD%vbE{Gx7S0z=Qv0~5W5Rp zjr=+G15N&$snIsSWzNB^9x6=?I$(z!;d58f>D1ZrVRvVUwnc4_lSmvlv;rlFgTSVi zg{q(ge~i6te;3cq7N-Vejk#0!r@`U(qADpF}hUDo8-&RG^t3TyXW z6i|b6HlkoQUe!&tYL($w-u0il)6_rYHA<~8t}$^z+n1vb*Bnj*V~PD(<$ueR)oop$ z?nM<`9+md|eVh*b5Uf9m`%aeLUJCp?K9ZJ!M$AW6T(tyDbA@hy$$ZM;2fVc_?AFR(&I*(8vYb0+q>YPGuDJN8igjlnxxCQi=+D^>0mme}AHx+>MCEG-1{L7dOr zxO(p@ggTc%KJVmimj_un8Dv*<4&+BeM`?y8126QEcV&OQ3AYd*>N^^5p#!m&S77bGW<_`Dx3@j`ZP= z`X`~Qy-$X%o@YINuV1JAp9OUN+3=}`m9s};1kdh^)t>9}T9-6tWyf~Iuo}v;@e2zz zK4iCQMx$9mS6&X)2dcF<)45ibxM!PQvUzR`D^?bMXua5cO6MY}qpSq_4k^y;IAWhAOomjh@c!*5yea$f#1KC zxO;w&`HwyHTU42Ja=RY)vv*ASw|5}AqJ($>SPYfrBbC+H*`dTVmoiGh&%PmMHG`>g z^j3pZol7MxEgO9H&!%*Veu{H-UYFnhvHXz z9@ax+t6gJZMV?ot15N=k8XyQpQbt?XJsfUX8Y_0(nt8pOf8x+iU3RX48!%MzD|z$nQketw7rxB2AX1%Bn~*B zT$txa9$TrOA4&4YO$lYS37Y(K*G|X2|L~2Yp^m$o@i)AT)o8i|7Io5QW%sXT%E=v} zran*&KQdW^pG(s_{7r zCis|%Nen0w42_eO3E~lAW%jDd#xwxFC8`lOSW+Hw+@x2aLp9gVl!jx3wafWD9ItF4 z*RDA;jn!!K`ZE%S{H#4s0i7UlFYVtkEnWM%`=66=FuUYvp>he0R9ItPN_}*LXbFnW z^P%(pp?@yhzJ=A|Y5%aN&TsGLYPRFp#rfef|Lu0`Ez(Ko`ZNu|g#Meqo?arH?3}MV zm-W;Pw@%*li?L=5{EO8)7EkXY&RGHjJN+neVLFK)9;pL|ZUw)}ed( z>l==MxQ+Xf`gg|hx1;Z6Lm!+m8{*HX)xF9Y+N&4mBnNE|Qt`rD^qtg^`skcx)wu`n z#iQtA`pG0E$&}~KaYaehUGh1aq;XSN7fgw~G0V1nJFBM#${f`Xezg7L!Sm3*L&Q)R zx7B^MD{?0M#${z=QNh8leV0XQ+j(o)P<_0Z6&?(kg@lgNyVHKNzG*12z>fgZk3eJ? ze>@lx$U10J+=8Jr=M2nu{+2f6punP>K;zsn=t`lGf zgYnEsIGDru0{4@&bey=@G`^0HM~vWqw;&*hFIc|X1=z#i0{16$=OCFBk}OGG%z6|&?GQ`HAHzB%jn=B z46WS(n)kjQecwcGF2#LK%zO_!SUdkm(^W-9*>&NehmdZRPDQ%AySqcWQMy5D=r2qH3__<=SfC~<1pS_>`L^cOeXl^~snFQtR_y)m%WC$<_VSgOx z^*yR62evSoSy?3lnUWAKMu&UAU)Id`asXb}3#fXZw@jJz^_OC+NZ8E%8DL!=HPo_Z zJ-3rVBWm3pA9wHI`c747I2#_eCFUY`izAdFf0&+=UapFsi(Ve(4Ko*=I74eb>wjQm zqjMYf`tlYgiAz~61P{EP%G@mA85eqaHk-m9kwPhkqCh)Vw#hUxOI+OO488XB_iq+W zUH+;kZG20&+NFqk^SwGtWF}`BMw90s6molX4NdtuJakWSx6@Y6+?8d(f)Wj~&%brD z@v?Dp!UVOp`Q%TJP1~6r)^lV1`Y9&U#Vs^A=`3jSMqxdqsQ>GH!|R{h$lkG%Hq*lD zqD^(m$6spr^RpYRq9Bk_(1n4e7!xeGy*0Y6Vg0m3wP(}kqx-N&=8*4gB|vH@!ZC?X z;Im@PNICKTUZw=9Z+Y9yXCH^hML`?j-fmPL>0(rl%zgV1Bwe=d%H;5=E0Xf`&l^J@ z#H3Nw67e&azD zL<+SYr<*@n)nKWktCx$+zn5Rmwfr@l;6LOv@-3sCXeYd+?Bo)kz!%s$;1sWRoEv0$DIsxpy(=j^NHvm@qNkIY6L?i1A=K2|qf4C&Ijk5}=EEl-A zoiW0CI^xI1mxYs)YV)(8tzV5QXzb9(AflzVy5EFL_tY0sq2qUJH9) z1(l!qo%cWN@2^bRawl3LkI#oGqN4f+TV7m2Z$&O^+@4Q1pQqB)gWIo1r9En3{X;OA z6Ph~GSyQV1SxX9&#kPkS9mRy~fE$E@&<-dM1>fA6?||FA07a2RYoYfvZB zb79-@6zBsR`x$PSxlD^K$stOXUmio>`?7p3fU0%&WxLtnIaO{v`>sRKSh0Qda&B5x zYy~%02yq=sUlH^a@0=#EbtCzf1I)=qFuvVVtc9ifE+lnsxLneJg8#!<%CTq@v&K1x ztGzFOqfvhVY{BvY;nh$93hm|vG`oWt10)gFXpMrJJ*}>|VIfX>6^Yw9D+2`l%Kb3L zuk~9re(u&5Y4EA|db?s!k}7fEdbfU7y$(aw|6CQLKZgB5_tD7BV}CO@(tcc4B&8mo z*d>~m^YK7xdlgGK}KBsKV$H?H`6NYI75#BIRU zSsQBc1H$}^l#f1=8NaE~;w32MC z+xv2Df`qc;ENp}_;3`Y(sjlaF;Bu{+N+j4s_9HF|um@voYvltZP@0kQ>KiB^r3&DG z@208_$~XGApE?H4i&~@^ri9{<`NqHyAuN8D>IzsVbgiFSa2$Ac6`1+7Ev=t;l_pc$ zlzF+Tq}t`cP%Q%j;P=<>v@Yk6`cuNuWZcdV%^xGGd8eWGMqul2&iuKn4$nF2A$yz8 zT_NSjd=#8gDcJs+go>r$FC6r^wWb|^@7N#5^m}hwZhK#zPP}(8&Hu5FB)+~U5q}u7 z?rIgGq6|1Fe8(RA(4lWG=y4ww?j&Cre6co{<`eR07c7LNgcfd8mBHvabNE2z{|{+1 z@c!&GH8_2|1$D$=SOlFPp&Q3@JTOP_qaCT5E7UjJ&`I5}b2BoGZ_571yvEnT&Wv~; z;zpWHuB`t2>Xw^i8yo{emj}VG{ws?^_HhxM9yNM-$0y$`rFG&zo1`A)9j!mzS-E>x z7yazQ;(m(emjxlS;C&gryu0b{U{eyS0(6qlGc1B88bsA73NM@mh-}lKnR?{Og93 z4{YOoJOl1~E@mPTL}J!&)kZuCyj27xxC6^32og#yot@she{G}*nOmsHEjCw9ysMTS zTXWxP7-kx-x@0I_*6=u$T>s3lE8r`OP^R%HP!A=_XEZ8TGTGP9Sz(|+^b=OUXypfj z*f#Yv3|Z7S0B_@H-IAMwU-5x~%@YF-TpB5&1ho@fyUi*h>j8XR1jYG|27&JqbBPH z7xc6@s3h|ICvDCIPQa9SmF+KENN9vA39uQ8*VosFb~CJ)&hBT^sQ8T9jA}65%zXdS ziC@bXJ*<)RUdb|KEE-jYl4UC@G^{T3Mp63zyZRfJK|ci=0_-O)ixrw!r~dLxq=~5i z1-irCW!Ji{g_F;r1;CR1%+~PJEg-$GW)t_mUo;|&IXTVS43Sl6A&sJXnMGP^^?A97 z%u;(T{8#*?N3zCzPBC1N034v+5?*-1w?WSnR_CQIJ*!?fG(>lwK zpIi=SUj8H%J#OtpFJXJS-L&$-W<+k%uD^5y2L<@)Qi=!uDUBm}&QpIvoTgf%k+qIU zUm*mf`Qo<;A^-BcJ*IUUJD!Y5^VXUgD#UyLjyU_gFJ?j~?IO)`eMuch3(9)?v9`^K zN&D$=!*-9=rU|xckmXz*zn&J)DXaO*>(7|0=-2u8{@27tnuOl7103Bp(M)+~mb@HR zM_!EEf32&&i2ER?J#d59lTz)d~M(Rmvc6ZbrL&{$FbJpTh!pTjB zC`g#v6NSqKeCx#6%W>0Y&!;!;R-P7P(?b zw~aJuEo{DfZ#Leul}3|BHcF9=#)n}-bB8|0NAO>W_NhpfeA@wcqDOXjS^`#vZ`nYY z(9hr08p+huq!WrYt#8w*2JbhtcIdypTw(JQ;#hvQt|Qz0an`=R((EChkeyMg+eOsf zE^D<109s6izP~9p|DjGERGEej9{?a^(fcdNXraGY3i?3(+Q_(1WW2@bmA@$i5qC5B zQCO25@&0P~w4P|9gdNCv2erL=*JeT=@dLWG#(1+h5R{j&s$32bt2ZAfd!J)QiXeHa zI+eO{w4tb)u1^eriCJM@TR|W|UHh+D?YUWT0tOZ{6O+fQa`AQeb?@`7ISCiXk0>n8 zl4RN3uw*xr_VXjBx|iLMrx1Y6_CAq>m?RRHJwKp z=z2Cq9Y_F<#;JQF-o3Wf-R|Ad?q`Z2_B4lsQGm`D5rzBwkR)#6)T-ZVPTuXi>%|o` zEKtc$Q9})Es3x17ecBcrr$UbBn)Z)Abg*ss-cWh7o8=c7+vk~yw5OP@x`O!Om`??N z4gf9b3CTvguPA}eRT@%UoIWZ#+DEPf2?VmainNj69OIWJN(j>h{U!ymVtBE{AB36z z&jlz~D-@QnW$4aU;&Col+DG^xp~ zAf8_v{?zP5-rZRzQ-eRHd1snaXfm_3FqYDT@Wax<@yH9Rh1xgqT85OA*ZGekYe9^) z(h&qvq01N8+%-*f2eTQ2uOA>W33EzswJ)CTAA}?mWJ+m;ya#@rl^V_}b~}ur3OpKe zd8y<11&m7MimVo%$@Mxed8M|h{FS~UC?9{zO=)CWT0!m|==u1r;Mauk5kK9_egVh1 zYO8-qPftT4qEm}eDt(h$T(O1|An_9;j^vxerJiV1E9X|pXQhhEnRaZr~ar?W1Oh+mAAFsTFRKs7O}&eesTg*BdRZ4xsS zahCeCjuMI{AqzvPj#O;bQtg>{fKO4+EkV&^q}50w6`=3>S@^>Lve2tK53GBDhl58M zExaG_AkHlVOy}(EgeMyR`=V+BA3gI-94mW-7CbLct3on$&EtM!XehGlREW2?UIBuL z&WW~HQhvJf%KhPJcOnlinSxp!^NCAH5g!zI`LNGLc17M&vj+NZWK#F@sFhJ~gX#xr zB46%Vz%d?$KyUD)J#?-+cNF)S*Rcqef-qgcFMxu}X=c|bSOU}AlTc~~V zEjNZn8X~BSN7Z;r?)8X6reLhEW`k8m@=J|9ZyH)f^_bXCjXz9-=W%VjlfUP@xQ!#0 zaiURnmEuP0XRuVP<551dcb^Zet(o8=6NehP+g{`tU)nUC1F=^Dmq#0~OrLv3D*Ot8 zHxr=y^XCC8m7PzlSHP=S#a|ix@eCfoY@3&_r`YbF;usN{J}>4b=)OBRKF%8*#35(i z>+NE5-S;gyH#W$Qm#u^q>o|!gID5{BGyMaDUF6UxYB(fwYJ@_&s<1@@IF(nZ;UY5t z*aJ1Y6)BS$mR;?_L0iHneWCczFp0piL}@qgjs01u|5 zD4pHK*_A~0;ww(j0dP&!GqjW{1QHP}TwPstE_;D;T_T@!dVCzXeOctjmnQ{rTBNPkVm6)mhfmq9^W{64# zu$#V7BDfu?&{MCn?9-r344HGJE=fh}w<;O$S48TkoT6Lgj3(%@nWzrRZ$m3Yefx{@ z*sRz4>Bm?aJM`c0>IGn+YrM*_E4*7->S-PMIa^Aa{MEbbBoPsWgv)8;$BuC~8E$tf z=EXtiJm0LTtwbHx9BJ}RVh|kqBOw zZrzO!re~|rzoRu>XbT@o_=Yi!C5Z(>P9IC-vTkqP+G&5|jHe+B^Ub&IIQcp;mC40L zJo-B3|9;mSu5^BQQgtCe^(NgsWP4km=eY$nehoh!;z+~T$Z6I1&fR8| zI`wwDuIHb=uCC-;;mrFlk)zzIqFyS}t2EkcCCcl`^AZIPSX~BpvNYI-Wir;)EU^R0 zilxXO(1@?8bkUDGYvG;Dv5l-vpE*IFJ>EE$;%OzH-`G9-I6k!Yl_*bG_g?Y z_rX{QTd|L)tNgi7EAoQR_m5koeQBKW7IGxN{A#mv&eR(d+KIizXF1+pP1fZ;VZ!U3 zxfa+WU$L$|7P&_7Am)E$(Y)t{W*87U`o*Tg70tExo+#%?k=lC+cE_o)VJjbwfKs|1JWWeOZG=95NDG#tE>6dZCq$N)UnqLq zE(-k9z*KSUC$R)KT_Xjm3?(Hcxs$1(Uz8z*5xdm50OfwZBPDbL1d`jq!<({gx+Gys zNKC43Qq@*xENR8$%2O*#yNH0y8ZFc?Nb3;FY7#arJ9~JfLF7vC@G+258@^d%vr<#m zkYOdge_n^CSS_t~jAKPMWKISYo*LUtG;*M(90ZK)WSQM}0qVNIKl0BzRrz#KiaQ-0Pd{wA$$XLgQy!iQNXyA4B8Wu)U z?JHK!w4?Rp6o@GITS|w$;ev6r9@GV{4$;_t$x%c$)TaLOo*(2ynt&=Tf(#Uj2 zWhFK?wzs$Ud>xCMN=;t5iFt~XklE@yXu7Fi4U99pfY;zaK6_uN5MzT5B7B32y;QeJ znv_H}>ffoBDpT~Zva@u<5p1Ez#74D{LrlNFZRCNuMv7(El=B&M7PQ)W^jjZJqG^PE z$&@Q@`bRz(98(*z+2fT9iI?BWFvUUGTO8=b%JB;bc-Yzk%t72-U4g^nQ!PQ^tygXk zvT_2(C=U-StMDA?ZC0JhDb9Tz^gh>|W^dB&3nOp2#x=uKlgnzE!}IaX^Dz+NJ+A(= zs2;S1L}i~_wn>$~n>%~6`02X3>SGV?+slHb_?8SW&KT{H8W*hyPE3jh2%!|U)SI-H zkJ?VgsoRcc0_(@~VFagIUo3vsBYNP>ujR(&f`{H2yci!sMPK_K8K32M1?}I+i{H<+ zI=~nmL4DiXUukwGpK46jGgV-}inGt^XphON8^mu)#QAdK-{84}WKcO_pA`tmsRF^0 zogc!neuRD|9erdHzvT`;fqG<7F0ZUt8TU4%g`ql4gia)nTzO`0l^EN?Hgmv1y-S;l`49A#b=g?UWmYD}FPWtdV zj)(=bt*gr_{`#7=sQ=@cEFyAjMYFcFCVMV+s8CRjjSrwXNjcz0KOc@x?#F_O|LS-wl15v`)RH zMxLRKQd#A(F+_w^+cQ>mD%D2h&)=chI}H%S9-FN-)!g+4Q+Sh`YvIezneij{Qdy{I zczDP$TX|CS|EBDfbNkv%Cy27`(~Srm@RE^`@GA3KF!7@@J|n$lMNmPH zxzK!od3$j!{J$8}-lx@GmxPlw^eEVcNLg7J8<2h5>p9N3d8#T?Yua^_Q5P8b^SPx_ zI084f64bG9osyn0fKX7tLeEzuysV(#Mk7ut7QH&noIE1*31!w%R@j z6NrdH&xFz=e(|}8=Dms@JhXN^<{HGjB>#K=nF>Glf6YE&ouIT?g$OE<9LLzC5AjZx>9 zfMX$T>R;oRhQ_odg3ea}WrDK_vGaUfvAlJ1Dzf=WRiMn!(rB+FmS5>oZqGEOGLQ8> zUbArf$|K8&@C&Am8?0}(O$58+4!0?6YSqY;Vf~))+<(~nMyNO@rexV;?uUHO87N#zigJjwq24$<+ zLCtJxE;sm9-@QrmDKnobcz=i#`ufcqps8MMa}(aLZgS+;J=7!|vghF%X7!WOQU+gN z*%?+(yk*7u8h0ahz1HDH&CnV$!i#s87V^Sm{yd*mzHsQeD|U~Gq?h@eR@%u9JaBCZ?&W-c2?`w9K1Wb;@YyJK%L;Ml3J}>R2|TPxVgTN?4periaG^dQUFeu{BCA1GtUb}Q2*=kXWz#m z_8=gU`Gx?19(ykKro`_DUv5MGoxO~7x<3D97sCWm-S?aci-~1s4?$?^Ia{lhI6N=r z>7dV{ycPojp7Bu-RuI4|+WQXX&X?2Asc^;VMyNCPJ<|dQdc#r5Qgx6|PUe0Vz+iGJ zY`$SLpmudm%1x3Lww6*a|M3pxx5TVH{VLbL)8y3k8V}VD?N;!3vw6jzB`07-K|G|s z@AamMCN6yQv3|v>USHRA8;+U7wZg{Fy7{tB?Btj>%e-NWf8~cGBOW@uT)1+s9-aWp z?!?6Dg4NZvHNZ-WQ+0t^B%v#bwJ5t{TUlX?)5X;e7zJpF=O!NkLczT*wCeQ#Yrl%D{H5Vh7x-6vIv@|r#!N-__9|?pkfZY2};IL{Ie_+XWpY2grJ>IDQ)hkpf zKT#qoC7tb)5G4VGgfV46=q%_qk)Hi^ON12XGj}wvucK1Sv^WEaaSjqhq2B-0LBz!^ zga$Y-lG?WK=0k29neHZB6?-}|v|3Btkbk84OY%{3^Dt*AwBriJhDwL$?W+tFm# z=V`OO$Y4gMMe+#pt_I4Fw4gtCqkM7;a;7iNrk|T;2=FZML0^bYc$Nn}uND={(Jjl( z(;|twBKpe9J+?pp2zvJv(tZ+lcHbAb<5E-hQ20nJ_g*@u5hAdF{e6KH)I7j#RDFzqr zBORQCE4*$j>ZiTV2{Ui)PZ+(6F86a*k&5SWA@Odj{(L95{x$%yk8_HSgV5feO6nYB zB);>5Cflc={`x&N1}L1G9Kre=mf?)tF7ff&Iv(+fG6M9W@0!nFSCQETK9a_Q)*^|I z$<^m85^%SxPZe@chHmCl#D*#X;wh(~88k$fr~Q31&8O@hX@fpFPjN-Q2?~9WfjP87 zLJKM+!JoDn^UB@EwrKQ+``5{pOl`;oIordKOM~zt!Tj6md59R(f>M<{`UXnWSrzwxm3RGouzL+-6wR`r$c6ERJ@ zcX1j2gppsjuyZ2l>N0m?AJ^_s3|@Y(=)~&7 zTBV_WSG8Y)*|BPw%hOoMQb~;%0|Ekce+B?Fy2?sGENL{q()o9I>S?*U$9?-Vh;O6K zMMB1^21J_*0#oD1a7EyTfpIGp&L?et5~P^9QHOf10y7i8>#2FW=i$xCDhZouB|P_u zD@5TBx~qlEcdCIVbh<$+zmVLl{Hi?v+2 zuksvTjna&pt)1F+e<{Qw18-j8R_(nq_@(ntf6}}?EF$P z2*)D-K!)QUpQsxl#83|Fn#1e+ow-qRX^8b+3KAk{i)@)`#H3yQD!X{_3nQtFX6Nry zb5vw&Tr6@(45NgFJHqRCdu1B21=HkUY6OE&gjMPdZV*?TSp*6)b9gzK)D&ZwHS0@# z@TEXnx=uFbr_?6bH4nNR508{}_-(UPKd^)AY-zdP54kB{+U)M>Y6`d;&j2>hUFN}u zrT#ErTif;VX}HCs4j32BdOsq8YB(tdElroKRZN$3>qczuNB|w{QqQyQ{&C2&`ZEh{ z&_!Ix^`A6#SkrQvy7)R3c2Z(u#B$rKHZOZ)nX8R=!AG;0h+AZB?~V&^)4vLAPo$|C z$$rY-SQ1|GM0u+^`u4TiXGRCt>fejHTVSxFs&ck^v@nrWd@riRKp&|OD`1dBRGI0T zZq5PiE~VTf-!rQ3TFm5mcvL@;@b>w6kUoS?Q-(F&78v^absTqVcsM2l*}jC#l{&(# zNUFBirS=Xj?DH`%<31Zgj%?#?m$SlzL-^l`>DxVnt2zRIP}pElb@{EGb%?Qx%UE3W z&vWN6W6UQ%3HcMG{#~D*ArjN}-)nUHCX?wL0MM_Pn*-K>GpV3deyN$g0`}YI{bdULl5JMv>)&gdjExUZ3J{F*I7K z&&6Da(8Bq%0TCP~^;>NR+h+~YOt66hL#UiRD}+4^H^a};HWr^fM$*Z&c*^h;$!F_U ztmJ*@7&%nJBhG2~zJ|3HO~meGdjdF{>}AIqOqp~doN~GJ52#=FET74SoVYG^bX(fm z+IoBIhz~{ji1BKgpp*!pcMQMATuxjulKbcyscDgkWh^9+E$gLlBS1BFo)XrxLd+3; zNL-;LHS_u>lHT*sX9Qe;hRPV##hIGd@zd%P>(=UiF$7||ft zzSSq3ez5iM_$N*E1Yq0e;gvXno6$XI}lKHhk)MONcO;Oq|K+WkjEzRfQ$K|!SIq*{#$mlZUt-a+iCuqI(XTUuFHAuPH74_ z^mn+t4rk06BCbfOtY>2=QhVCcnVE7~<1~*z64_6js@jd${neaJ4jI{NGP!usEne?V}sS8cXhd7Im2p;|ex$lp&V)5qD#{#tC5AWE5l zq5pjs$K|(05?{dN8sc{%+%Byd7LwSU47ROKleUX*lmw(llv@P2Ef-}Wz1r#mCxgsMfTlJ{w%F)R+6)lV)31qz!J!ea9fqkGImV~v>+KcaWBBY(zo{Ym zb~g~7fPe^&d$U6I*bl(4%g}&**VR^NFcoN&)U*?3i16J%gDl6a_x9b(W!<8eXl}e& z;Nh-3rQeJoJR1pfQI#%Kb#>BK+bS6stJq9NLI9cT8-p=AkHD?*xc|i|ui-baf;-30 zW|Kyxy=>k$y*&PxecIfc37-4R6Iunhhg+wgJvNu8^7y^<-KQNOPmadACa%xiyh zg$MUyct~fpwO771&a0ebQwR-ooh%<$@={9SrP%LU-Vhu{a>1devi@jy2Scl9Y$Qq? zDH)*(dCUU*7=3(v088ryo}G(!hrBE^|3fG9rxlW(^GLu5({w`bK&vW`v7S&iOaTCF zxeBmrWa)`UR<5NmMCXj|bjh1Jzk1h4G3S|_W?{{Aq>@U$^{O+3yfJ$IAOt43fr)cD_1Rl>z-Qc-M1cD@M*W>HKq^ zB`zTRdoN`QuHd=i!u!2pIz3%45uqO6!{U_>Ks`VPsV*2eMKEx1qlNj6)(`i)-#%X@ zB46@zqAiLsM=;P@$~oa`M#R84B~bXHaIFPiqi!ZLYFk@?r03n~5O*&A!D@`N>UyHX zNM(1j!@!814DSSs*vJ+`AM?5y-5PBfGzv+2!-)K#LaVr(#6Ki|B5{e85{V}BZO zwe4Mk>xxQy?(C`J@>v%G?xGjgA8K^>LO>QWlC$^%y1HamK*c@V=t`YW6%h|QlOvwK| zhv_{RdtAMLq)CavJ#PeVakOMzxBE!o)$in3j(}4T8VzS?%T4Z$_} zS4Rz6f}aN>FULhnu=F(jBiNFg^S*nTzKebYayZp-OzyDlA@&Kp!+b99COb5wcsi0e z5f+(MCi6WU8_<8$_xm9_b(X=pgC^eB?dP*qttTeyoc&V`auQ;F{W(U7gpQonF~}*d z*n(IBOaq=~$o-%_us|BA(k;}|!@$)g+=UsrPW{O~!-fU-390k@A}3x!s3qIkr#Kcg z*Py%{odxxIGA2^Iv^zo>DGO<9^u(m39ZCfIYEhvcpS$s&z)JJ}=x5baf~5uO?m+Az zMlm*?>;(kG48cqV;|b&M9R#2Jkhi5b)~bXZsyGH2{yp^~$&=oA90B0SGbB80)NtGZ z;M9dEy_izpVW3OGl{$+-GPZoM)_y8VN)^e|L9|95tGZR5WU- zqt0Zwd)S~g*BB+02GVeE#rXTUd@h>Jf=oHOZH0qL%2}FnWOahrNH2H!yCE3^OhKe* z!n9fr&%=N1(Ow&umq5TpsK;r#eU8Z=FO=I%ygy_``tQr`7nNbZ8ECRyE59w!An&gM zH_3Df#@4@Z_uk^;!&xmVDBE0n^{t3$KuT$kWZRn7i~5Sjl2JhdCoN+a^(T_OW&>fS zLnH*~_qy;%!(Us6%k98=FPM<;kIO3{jc*KGG`?QwTuq#M zNLcwm{@ICXx9>-R*Jd4`I+ox?;vpO4+Ucb&-gb6&90ws!@Q};pndja0)i$8;0&vt6 ziWJufJ;4@!sZV0_Q71Od8Z_Yc_mHxU;U2}kj{!FfD|&E%NZE;M?+6*+FdOp-1Ts=p z(k_50n)0qq!MW&bN*r?A(A8UNUZG4A4G1b^Xgg(fbJtL${TjnvilmXj1j&rz!^u1) zXJ60hS!4C63yX-bPW){cq55yU{(F|f@eh|1UaUHotZC=}B~nHDmA8v7!#;SXzJ?)emPz(pXz1ctw7JdhH)DiQSD+-OZ^VpA^(pR$kC}&WR%8 zlN#t0S!lkaEqh(T5Xget6z_4L#2c`^#*r3@Uq&Z=ZigdAfJbX}lAk!p4(_{6U|7DDVSN69yYbMz*dP`ob zt-89vZC8Jv>$1xDEnAFrosC5C?)F}WWm^%}F`Z=_5PT7#bjc3yJiYcGBU$EL{E^Ns z5T04mP<|)Pc2Ha7mSe9*uR@G;;W7sv!d}0E$bjUae zpMCLCw%x6O<=P3Dy`{xt?^xAZaM<;v+TcM--QmYAf0>xZW8Wa}NZ}%J!;%ACuXh3~ z6jLT}CxL&%cPHVUKHFxe7kq20utAOtIRKjN2=seJfIzKEzpiH=dUp4;`4Dsly^RN} z!U?vTUY+~OBT%&`_^u;=e* zCHM4JUoE70Tf5Wmckno z@o|6TgURAC6?lGLIa3@5+6*PASqkbF1Qu&Lnd*1~)>Y_=8UkQ$caYV4yz9zJ=KQL+ zT)Qt6t*FBi@xOB(`@mVgdR0r! zB{Mpi!WNF3p-MQll2GrEJLW4DHIFE8c+%U_YEdbAJ;B}F!X%oHU5++0u(q$?!3V{_YoS6X+2*3x)A+kOfB=x0F_0cyw{(Vc)T!ptQdwE%fOi-)6SB1D4sQ#Fw*d`kFD+N@ zYu(I@`sNxpV;qocN&Rr+Zj{k5sVyUCWFJkj8wJAbZ#LD1Se~b63lT~j{wSqsw<)<3 zr~Ng1o{i;YKrDivvh3^~C;~7o{LTQ2v#{F`3DKP4U&Zb_bG{)Vy_@i-|I(FupO;>i z3Rc^gg_I&?Lv!5y{(AnMLX=1ic31EP&U@hbYEexC^{kPtR}URRGOA12+aPnNpRG%{LJ#(eAv9%Tsl zA-vMwBpM@AUA(!9Y@5~X#S$fjh#A_{df7b_ zlbqMy7=rq6bBY(C@0oRm{L-UNv|2zGvv7n7Pazv@a%XB9YT9UAgJp!6(9gq-w(0@C zzU|N^uiN46kn7E7pFCq@ERI4oGEfYgX@>}@b83Tv9tZ_8$`xJaAh*Lw(>_oNC5~}j z?_^-L(FJqkgX8?&aX*HHya4WTc6-3-EF9k4@cpL^0z3z~xuvbIG$`gh`C5i)KP>P} z3xVu|;vmyPTz`yslV5POUJ6WbLP9nyKOb6pE2BS$&v;r9yEXA-+z>nK(iDk zusE(<;Uk1GA1l0bMUehN{Ta-!y535u5jBX=RGuhbzj@ZO;Q@w7Aj@RR9(BS};Q0j9 zugNNj@?yX1@oko8@g4JsONZiqR!9q;pC=eLaC+sbs5YEkRufatTGi@;@aKA|j^@M9 zDiF1=m)8_JUXzqIW?~%n%OQUQljkS@=asXp4_^6A!=zk{B(rzZDS`yQ`G*g7(#o`C z5k`T6XQPn~LG-}dKT(;hERk!hKeRSF(Nh<{CS;ZBynB>1x(`0#t`)1ZCQfg8Q1TBd z$5}cubh3miCct(RSz8z+usG>&zjg1m{j@G%vOM`-dl2m;$KC=IiFKfn0C=K1z~6f# zz{-;##u>kWnWHmS_74zC8B%xjQdjrlh88W~)QNyg#R&Pc+8)NIs;n@a`{x-Lxm{N? zhms|!rTAoiAihRKWadeLz4H zV0bn;sVwv@mIT5Z%T>D@IGA}Ak}~}%olwHSB#-q2*UWHNj3EFi-rP(Z!qb(_Z|zI> z=(!-V+9|RVkbk~dY4ymtPH!f{LP!&23dSVp_xk1l(_pT=sl5dOrD4A;;;U8GU!cw@ zo}TLC`gE@52#tk0au~W|sz%#qV(dKyQU5*OfZQLjaw*H8Ud_%Zzu<6i8(B#mSK$Yy z3iwvmhr^|R}H=myo(9MugPlGpA&62dzpUok-ZN&AZhFLqD}BImNO=yj4Wt0_)} z@BQN%G8$?RtZKpUkn5OxBmcK$k(bKurH^f{VyOI z;Ke_!HO&}W!8?0A9;PwbyDbV%^pT{TgQ}h{3JQQ;=Zw?}H5>s?wR~aQoz$ng7@uFi zX-_NfcTW;xj77hpekKDM;t%Wo)y>T!l`@5P&R?Vn@~R8p7o~L`?Eh#ToA5cZFgan- z63P0x`}&@&w3L*TG`jbN;f5=$wgTvJ@a|tC5YVykPYa;TY``!wv~B$xJKwtoJ880c zV%}{l)Y>y2{g#g_R)~>gAt0XK;+f>_^W-2yqK;D06o6OrdZV|!H|@6a(qr!sZ~|uF zho@kRUHxS1?+ZBo#WWbWp&pl4{_5I@b%H!g<_n_)#pBwLF7S-)0xl_fO~d#z1bIMZ zx+2fGp&fD+@YEM}BBI4Tay!d1`=*qN{xs-^U%vFIo1uTyqn7fB=YyD$%QRxB8#K{d ztVS(3!|5$I9Rf{gKklqx)pn?dZE*YrdNG2I48z7XR%o%o9Q9bb%uY@r38Z$7Vv$SZ6_KUN63 zulh?{|2p`4rpC0NFL4!65BY$q8!W)1y6|=sP(0Ts1MH<5AK2mW#mviH$i-OEKXjSE ziy;zKLPId9=9XL@GzyHn>Y|rd56>lM_p7~kdFeqoDc*qyd9jm3HH&^;N%;9eATtr$(Dy5pRu%+} zecfO>7(>3^h2sRY%cz6v&1*41v z$FMs!!t_#VgEwE^^Xy14;1KiF6@B1OVQg~qmbjkt>(Ixw8!V90mSk`5<)Su(nOdPi z9N0kN9idq69a?t+`7Mu^Gv&W+)y3|<(OmwrN-7d=gI5#a=}MRf-|YhPPq+Aio3k@G zu3<0fZUsnzM{qM(9F8bebSA6Ibu{41VjVGb`sA2RuCnylK;o?zy*<0+-W| zd_T}0&HJ`?lr3J}>TMiDVa@mMlkwaUCkt5Q2@r{g+#Zx9(H3*kPaD~37u@r%;9(lA zt*qSt8WFnzY+TU7D5#_snt_q0Ylt#X^U4$LLNaIi*8_<9v&Tf@ZGVS-uN~EnOxJ3v zmQPv(0G%(du0AlmT;ah(; z(k_qAIA0Co+mK%-Pr=MJ1Soe-_lygODt6jVm1G&TG{L>fFX-ghf!FB=;K)Lu}#fT6~%n{QM@>1#&5%5)T^;hGFV|Px{1H z0K9ekMImg7Zv09C`!_mRGiBTv0-G;Sn=_LmBXpasHEvavN4IDZ0tHhmWglg~`sn*& z#6*8)F%ochb=zE9(-{)w;8)#&_6EUW>jAeiKfHB`zc$Ukll~+n$co6CcIeDk@P?Ao zJKda`RrNcj6VD$7>1(_c!Nrq*P)i%dQy8J(_*)7`;sj#plLpK)z0 z#_FgIA62lL5Ap?MtX(D>KTUTQ&jw8g;D8Vi&$&1lV!u($j-?!0SF*)bRsx9Lc%zKe zra#voqw+tX3s0_JmWYv+n0Z%$F*TaYbXZJNn(-lI(!;;51pG0*ZaAZHf3-ay94O#0 zPs|He)f;CSx0x09Gd7XQJ5VE&&yq#+v)=L*C3k%E*?zUs;b$yPD$UZ(l0;yD*xpf` zy~*#oUTm3lIoXQa;))D%N`QdrJRl=YSg7HZmBH8gA=l#>@7i3~jr=iWJg0Sae=^w1 zmqkRd=;yVRF=LD;O+(((7-{X3`KLFq(W`VsI9*1!z>cmj^D6c2>Nz=%)@kMym7~%( z6n4yDt&4->I&vxtjs1CVwDBc)>usUaXOQX&G<_5a;3o@cz_C2ad_=en+Q zzNZo!wA4mHVq4RU63@Y78bpsRbeyHhrej#^6gw#jp}82on;r1^Gyd*cIV$zYE;5ZM z0Uh1qyKtr&LVS0F{_gZsr{6>$?avjvd3pK8En6E1GCbT)J#@Kc@A;+-a`Ah! zl_^y}75LQ0{JdT$htC@mq%e1Fh@0|C0b}n{QNi?<;r1fLLpm zitcwM(dOzKq&JF0d}}%Ty0P)@h0cq_MlEgu97nfx;*XplU?e-+eipJAE%1X{WRjlX z4(GeCg3YgDE)fxt!0y$QXvj!O=9R%`+PA`G8S7OiBq=Nk%9DZFCz>#BZBr_B5Of&u z>aTW$m?@H{GcqxKw(scdtiH8j!TJe)S<~pYjM>8C3E#}HIoEcnHhUxV#40+YNhaOS zN|<)z@853{>cw(p%C!}BPm5=ZbXB!J{lu8^n8bWp$$!^^r>!@c-oR@YfZHKRXFD|d zVOvsa?)I@W@tzuyoS-Zws7yS(DEvcK$lpHI2vP>9k4)Xg#^t54E&2eVh5CB+rx9F{ zVP|puqs*i$J-)bkpfmU&k|O8djJ5iuhFk^acGm|5`dIPB z6PMh5QqwHn6_u)__hAcfHbIA#s#Zd(g+i$aDHkWj26D=AXn=~ncrizQQ_S@bs{eU( zK64(`@YfUQd6ozT!ln0@vtbZo7KS)aD?z)MnnpV=@j>#o^T2Ksh7MZcA3pb%yh;+D zBqTA0jQy6qq7u^gRRz~$_g00!+SjE3b-zl|)Y6>g2}&!^iX^USW_q|B>HW9;tdnrE zHE-lM#0I{W!J#cUSQ%Tq_784}<9J*udg8XS+I3$JyF{DqNU$)&vghOqnbUSUDp)Q> z8vG13vG;|n9PCpK4pr@$eepF9vf*}OVfS572VhOHaFjPw{kzOFuy^Hxw4{u32)_?L zvYUhcb@%B}u}d!&(;Kh`IV-sVl^Uf*WJO`)I3fq)?I?fhX0~wl>Gtmv946%wD4XuF zIgSOZC6%$3-?5oO9hXM?sDk4t7Ra_nn4yS|6r%dsz1Zc=N(-derORhOkPLGA+h-~n zDvjl6VQ^+Fn@19*ir_q0cfKar*AQn3@XY6W)IzTr)JxSTEr2DS5aaoy!ZA3pAH)m< zG^)F*jHvVjbDOyi8055q6@?S~xIel!nZ5H~0YRP1SW3$pq$m&0ZZD-gu+ueN$~0OOiOe?G5`kispE=;8=VD838tYY{p<;sqfxrw zy(hRI!konn^X+ltQ;O`Q=2?Z$jB^^JLgE>oP0R!_eef{zc=ONe0?zZ!k1Lu#LyPp2 z0BEm?DDMe|VOQB+5u?+?#8VnbN~7}KBM`A(9%XGmcZPa|Q)GKtXCe!$&ki;uxw6w- z6>~&r{5B2P*#hJ5pQEGdDSjHFR843Y$@gTlq0HRhH+7#p?4!k5D>X;IG_&#>^Zzok zMe9$O{YBFoA3$R$#3uhR&5RAut-?qTK1BN6?XeS6Ygc^ z36TT-{E{ojX6Gl<2uz>>c4?Qs{8yce&_p%PB=BkZ)nRJbpKGaMrLT>1c!b0SD`DhGv~2=a;&1|I+iGt0R6lsrMgOR#!2N>u4z| zSO){wuP$gsu|Vh!fO)h9@PMyp3*Mp}Mh3UD3n}cf{dpi|U({BzhiM%xQlwcj;tTkF zC3?*@9%1d68yKQ#6yBYUvlH<}M}6G+_B*dvEDPE7WPbRYiDaHIs8WX^0y~L$#*`Sg z+TQs#_}_PlmToKE$KmX-EQSvKFjp2K%PhLP~wB}&d4TM${&Tk{P&1Z09W`tE8PY&HNKqjjfi#axW!WFe6E*-71{)VoH zz}C#(SEp(w;=aR_&F3(^wQ6r@(A0@E82r_T@@HO~0fhjlp*PG&A`j7T9iLaR;rXQz z`l?~xJ138n>~#yH8Mx;R7s9KM&|K2(q)_IBkxe%YUM}~tuWFQBZ3lGuJwnKosE!Lk z_z7%0X-6xR>KU%4o7fnWxr)?%oYu-GwZLmwQ{rja5 zPRH}oWXC{U={owTD%-t=+t=x;0r!+|rSER#X3k2+e5PGGeHX9vjp+-@${(X7xQIVe z`J9ggzyC;^EuR5TH^Im$&(d^!KVT&=dK3Q?9#?}8t~x-tJJ_4-COQ4%(Dh}8VYw1Q znBOZYM6kR&_#Jr~wD#`Mp#RGqG3Mk!@vddn;8Bk=9S9edI5Gk>Qbk{PyNKlItM$iD z-z1?@M$7O(#j=JW@l2P0j_&i;uJUSYYZIEti2_u^`91kGAH{jcyeFhCyO-KidXaXH z=D1aOLz{0`+l-=#jqnE+gIL8qn6?S?Vu7xC=v_Q+h?(MK1*K0K&;RoR^c6V+ zl16u-87)dLKP%+6Jm*)EGYZ^8^%l^G<`?hyuTOBZoNX?=vYjQlMYdMxgXi9$`b3n% z$lywvskW4YtiB(ulZk_8g=odElRpLjjHI%>l)0bpMfhBx(CD{;;fltEWD=2oINBpw zFee|ZFv2G%C;MuiD%M*i$n_U{qK#L8?=yj6O27bTiFl{;Tpx@ZNnEv`jwV(G_vB4- z(3`HI+uhgbG|OjAnmf$GVc9>DV6DH-Zk1wulLFl)ba|qiD?@Bsa34p+ByttULDr6!oq#%sL4o3xi*%Ts`w$&Sk_4dx%^%5Auq~l4D~Q z$W^3GOz86o$_4>_6LRB6=lV&*zu_6!R&Mg#PEOM3;QrRFq69V$MmsOPQ&g*uX#J=n zH2lmXhgR0XZ`mlWpe$IA3Fp#x!s}ir%5QMSm+fvEzS_;*V5Scy_yzq=qQYE+^)147 z;~VXxkD-x$;0Q1>J{gq1Dl0~ffV!%kBGI?^Iq3^U@`1R61vW!d zLo=dl`Jb15{rctUGi$PNZ~;Kd=hq*vXt6-P`sj(OiOvbtl9EX55H}K9+AZd7cGn-6 zfGq>i2~Z@88he_pUw}3t#z~BxQUpCBZtvtaDH=X9JOKDZM&Y7<(vZK4ZVw+~IjVJ#f72o;1 z2pVq%K)kpE~@Lo1@8ri4`87B%ec0v48%p<&gfC z3$~BN^vKVQwR)*GWg_!W_#fd?@UhZFv3o2&^4Vg2-q+TN4;taZsqt|w4{@ooV`Szg zXurej5Jg(;VU>{)V3}lk(`Rf(QRWM;El-}ziv;D-5*8{E#mhyAzV1C=679iI#eB7p zP*`ee%HuqTzpRgfF})x6++q0Y6qz}o$LCjjY@V$@fn@$B(qoj)T#Wm-?5X@Rr1%qkWfTF4 zspXHe(_EE1=NGgF2HN4aYKjNd#rko!PG*086oQzu_^4u*+6Y%Ka6XC%)rV}+MSvku z1kayfe_YASo1w^NSlgHpchs9HRf-_agIdK0a(;-Qu6|S@oLe$;70fzsC+L;4n-u)l zMNz-z*T=Q*)Vs58M{f=;me^Zq9&OdsF1HCYIJ&s3fcTjgN+0_fV9O_dlH@E;v(L)V zJ?Q2jwe82ax;Utpc0-6Pb5n_U*|I(B= z`oe_1$nQ$;uwBhYmed$^bXFr5bleZyP#8b2UfLjIO!NYxDSy&KAm7ty>o?nd^7)fN zgCOacCOKo%DF%)>qg3!-2CguRJlEF_&sc?IbF7}TZp+u!(GB_r+aWgc$m`yQ#~=s~ zp*i?)R$HpEqvH&4(H6Nma^rlUNljQeSVi_U*nu^uO4YDP0ax>$Nli175d;np;KEn$ zsm0yCJ`;6cTU}dSwR0ySQ_VZIVIwr0gcum`gw#t|1DW}N&?EDD-Z2UGoeb(b()K4H zFiQ%%O3c9K^7+h=dIQ8?Cjl1;Q_uPOa#r%s8?UFYn!H}lN%?I3b9r^yZ=eMwh3>F{ zs!hjV9=002CQAlr3VB-fy#Z1&f`=5Ch0H8xK_Rw4M3Yhpnx)|Z4DK8Kk8Km6*m;Dj zQvk$|Ue9#jc;6%;0`QQux#a+EkoR|hAl}&6sDA)T9DwD(ZS$0LXJaD<1E13_eGGQ; z=@t5}KxvL=!Gz_6B5-L*d#gbrTs@gC`OY@raUS(kf6_Aj_JwDcFHP3~5pQNa{TSOM zSJQ+X$moDj60QU~LauSDHIyGOv!;Th2oB$PxDwe~EMynwjHFdBDog5Ql*K6eYrr&T z|JwXag0Db@G}#<-N%7)7?Zwsz5i05YkQC!Ge;@9`eeu;b8z#-oLR-976L2-Z5S1e- z7!Ab&D`vpVb&{ahG+geTnA80@ec{+7d79f~CWo)9rqrJ)e_WKC%|W3_!4tm}qhCq- z=o5V~DuOeHUCnHZt_E)b#>LsiH!m0$G&SwUi|52Qvh+;N$IYkBz|ThJS&G47dlRqo zHOlQ^?6gSVI{(IOOu^6V2n3nxWME~cAEOQxA5$@sqp?3jA{c`itG@NQY%v}GYC>wV zo|<4^Jd$_-h9O7(+%=Ic**P0qRxpp>g#n?hnAQ!`EUG5bR-_%J2!2Iimd^fmtELLO zQ&RHUH+v1|8x8r(c7pih>Wm$LA*S(@{<^B2Rm!-SV*>yzNPb< zKpiLTe>!u^-?CEJJq7I8n(D@D>hgRd`To&?oI@t)KeP+a(`SlP+vYVxz6l+^5z*`@ zl71(XBdj^Wi(VrB9xG-=-^H_r;(8piiQ7)3RufMnxjAY2f57JLC1oAYhaze}{~bZ( z54t3SF`stXBZ5JF@XaNuVHM4=QG@mRd34cfb&P#(e{K9&{6x0QfV*OIb`-4JKQ9bO;L-Q zq%X7LbBC8N(x&CpQf6Tek6ooLXo1{%SxUnoHW&n->MK^mWlsX8d#qFL<~gX&oH70J=eYX*vF>JLmobAiQam9t~|5+3_j zwk1Xez{ahA=2}F6P67CB^`?n;WDDxQy~}ll-)UK9Z3=n-C2>B0dm^F8&qx|v8m<0v z$1hadD`b>RU595u3eWXR!Za$VNRpd)tKq=Icj%3XYYs;;$7P$_GQc)y=E6J3* ztEH2zhx^{>%Op<9W*TPUSu9dmB*m&sG!aM>Pk&*ozB5o@+=Bc*c``~(B$u$`rsE{x zxxTJ2DwbYacd&K5`{%2*hro~Ckj_&TMX};w5tc!6Nl=6nDMa|uOS``;-6g-iRp_&W zzArA$%LyrxP?8a0PdiI9eoCP}91i(C~y&41O=N`x|>8Sp=d9T;q z^%>pGjVMOpaj1LDZ$T^Y%i<1!*-*-tk;LJ!;J;=$1OG+RDsvE?wA_{K7Qdc9A;GdA z`&{7Ko{2N3;9wi`qK2za`jYug9{%snACIBf>P8@+zZepH9q#*mHXsoZYz$(>@g`yt zjGsSB_})=Pg=6j$Slfzxk*9^xr>r$@6+_&}8`t->-K~CYd0EoP zAl>kbP1^1+Ya3w6_GkOU{X zD|eo6EQ|r~4>%u4m^Nh{APhqbin%xh(OZ+T9ZCt`bd(#N-i5=QOx-_x`jMP3csf0u zEKkq(j2yp!-?A;I9v?CLvc570 z$$6N^mnYI8r@~rz&X)jv3g`1$Y z(f;x#)~U_)n!7=Tr(?Nh?3!CqDG=6NBCE|E`qq##6uL_#Rxt z)RO0w-%F-Bq9GH+B!qf#`A~rHcWpXEpazDiPn-CIj(YN>ad5=2FfiHJ<=E^^nsoWa z6H8-X+i=^P1k8r1p8R$ke%Gh}lR5U#fGw!;np4cHi+W z__G>h@ykB`MSKU^awUX_O*y5cM!x9U_h`b@XHs}awC6a%p2S1(G|nDmMyq0xTEk*b z?edAeRzhz=6J8g7u8_Ic=DWJgBnuti)a$1v*+3c?By2g6a1W2_#h0jOo_DGYQp>lm zz^@_|D*A-BHDGA*1Nxodj%AE+k-nM()ARs{ytwn>{+n2Gy}2;1_1PEFU%Y`SMv&YV zqHomZD&3g=uJ4X|MIY|lmauWv$38<5Kb-h*X`0Z&eurJ{=vyU5 zk#0{=5-;S(Ap!}QirM9oXh3b9hwcj)vYjqwJHBxSaP1-y;_;Si%N(vC$CIX-+ybdY zmfXAoK5dVVk>Ag9|HkP3l@M83prT3vjRV1caH*8fyb!_sD`xyAV+wPK#Ll19&IKV& z@Oxt0*yr)9hk=iaJ`Alju45^Vi4#IfI76 zl61SioF<2ywu3xJZ8_w3O-32o1SlrSY51AT4y8NDoz*bB;=lcJ=HJmsu?w9puJjYZ zBPS3)BK}G{k!E6dpW5>nG-@}Xh<^9?doeuDw_{c@zNsPB-c|#}NzynB_l^-a0?cIz zP?r(zp(Z<3KKa(yPifSk%^()2`hs6z8K{sg#57HVkCXESE~Au}s`PoiwwOc2#mD~H z-ce}SuT;=QzUbXV@KKirpIedDiDjlb73DJl&=C3FdCW$FUl3+8S3I*eW!RJn z#%%|wJ-%EMcJFY!?MwoD)WMml+)Mu9+3lfgg?O)S%b+9O_~w#{lgAT}X-Ei*seL>f z8qs1$@~_w#sq_q773(_KHHPd2A1td233QfQXd{X4UX2DQ`23Vvu`>tFa{ib#*}(>x z$VMa-{sn@q)-}4lc(g}v76BkAWOi;SAakOeAShGq;r z?r(FZxFCcD4k+y0G3>3k_enqJYgySHdJyhyd`1kh1H7d%AvZGjoYOMS;o|U`-tapr}FP zNBT%2b-FBak|YpcQhVD9{TmUw1K+dgbpI@$h36!~ev=Ta@1M2+&@Qyjh1M2Eu#F4{ z%bU{Z7n{mbyfwF-R#zq^xyW|{XZnmoVc?`=bObTx*SyqJ$Aa#Mu^pKrFLkwP;uwLV zS^O#csJKRDkhhi?-BPm7jq*-*+KGERJ7njLgBG`mD<@GrhePl8im9j51n!D#JKu#S zUFKWd+~OHVbeQmyyXYTYiV{H`42Gy(Tfw*-Up!e$Em2eKW>u}rrshxL-h6R5tgUyz zr|?r^QP?+;(rQ8xZ{FSHPy@aRu~vcx6e)nW2jKkB=m4$szd$q?}ySYMk z;g6d^LDzU*Hd`5qcg+d6!Mk*T#gG5lGXr$};FNp2Yj*hoV*6^36S-3&)N4vyd73Rh zU`?&7wtOfOApkU?X`Ci(bFm*n+dN#((jcBQ)4*u5aqI4ngsc&$zbTwG8^BND~-CbRZrQC8{0!HZ=5!7#2v~nIz9ZzlM zUPRj8tf0T$0X)KKraK)<@YcJ40P(~k<=9um@Wtx6hzJK|O>x$4Np#0ns%@U>Ffo|9 zh(-6a)g>? zcRH66pWA_WVXh9Yb+p=w3OG_G8TcV|_+N{R&wWS1jW|ib>7=NhR@j5g!_8?tJ6nf{ zdJN1b#_tu!r%0(^;os{9T@uwGJc&XJbW6#FKZ|--Rm7BOlXTo z9!t=QrDD#fE-}%_t=iCWKmfMuEfZtoZ5Zv6wVQUAYx{*V8WR-m^A;lS^h?h1VCh6U z1gyJsp~qMI#-m^Q;0G{tZ=J>g1j}ck`$O8@KsV?Xbxkv7rItX}bILdJ`c{+V)R-^! zuF zx9qbe;&EmDG(XquGexXEY6(fnn|-4m-Fng7W;^OA3^~idF{ z{;T%SF`F@?qEyKTX7~9{D(OAc71~^m<+ut+z+WdN2&LJ-F z^&*;j3!3rPFA4vwjV%!K{aJoRH9@O54VOD&V6n8cRGa?knURy>C}2O6=NDCJH_zGT zO)JIlo?A4%Y}v2i3hCH$b|{KrMIA*fKP!T8lxTS_Yt&U;rUFyOQ85lrlXCkfEuxA4 z>Y94W-_5l67oh9jo#1_sKV0n1M4j(_M*G!W@|9k^HapS@9n zOuwP->kc`>=|uCuLH(aDJsnoVzw7H$3}o)1IS=W>_$N-5xtvQi#VG3r2quBg6&^qB z^H`XVNX74NfAq_5k&r+W*cgaIihmXnaoXg9?xO+CtzTU~d8DB_P#NY^E%2>*#DR;ViH>Ney+K!C2 zF^uL{rM|ZAT$b+naybux16~8JSO5@!kEiCVFu%bQe|(0YBSS?pg+jHgKwveZ2inpP z!OKFc%i}?ZpRD4Aqk;|g4vARI-RiIF*8a2!UL_Ue6|`!S*&+~KY*z9u5ujI+pO~>D ziD88&?#(U-;~pZed9dOC|zEf#z_aChwaZ=+D9)g2Os@P6DMSGN7GUM zx#6#fPz3eoR-7MO={HI2u{1SB3?r#Glr{5-7PnzY$IC(t zj;cMZ9p0?RV%5kzT##;r9YjI}7GVcmNcdY;o;Pf!VVtzQC2?e&6$ok7!?MY1M*q}c z8SG<)&;qPo44~SSS_vyTzE!-!uM?izj;dQm2h}^ts<*CFzOHPMpy7S|H2aA|zNZoH z$)WzZ_<%3lCNiSXQ>{TZ#$4m9xo5?AR-~3E-H|IM@`@$q+Bk{BoEu5@+ z*tR|U;)v+TK_|x3?T62^2b3@PYAJ2v-Ij-qc^0Xj)R236CX8jHQk#47#Sao=R9Ob= zjzkX<-uC3tWVm9w@{HjqluEo9HtF+Bt=PT!^Ap9;t(krB{ZCuS8UrJPH_BEG6|^A~ zrXCZfBwH)uEzf2pfUZLy6zibk^())A^CQ{$dA1C7hB7GffkzMEt$4T%yWf#Hx0N~X z2dw>(QYsovtUvxU@J;r@0Of7 zoOVow+)J(n+?1$1IcT#^F1fkTMOmy>5Vw#RW681z_F#vt-E3`bdGuZ#Mn7Q!d^5qD zWKXiowZ!K&-Qxcf5f48BOWpoH+vhc%Ete+WN= z8M|;m{1D69D;NUX(7@dN&8myS;z>B}WqpL8%JMV(r-7l$S4^(* zu+BQNjdC|yr~i1V3cqfTw^rU(r&^A6(RidMsMm*W#(MBiHh1gCtBwY&ZQzkyu+M

WNG@)PMxz|u%(8+;>q1eDcUTDbK-tX@3hf2Mu3nn%+TcB}p{ z`D=ZYb^?=4wAAkuEWL3)n%cDELOZb_`p!c5tR%O2!cvmV{kzQRFf|{0(OA#Eb9B#; z*zDO#NwdnQXZ*%L7aa>i4r>3)q&<^$-fs0T&3WW$`bI#YHa=?i;kNzZE&T`(w2=0H z=z%)vKMmgpTWE2V(Qg^X;6O}UtG7y6m8wryej#^u9GSWv?$1v?5L3v$+uyi~x9;7!UBqaz6)TO_B(9ZF&a5vRn{@Dw{V*B!tGlm4FzfYuV8?mYbC z9po44wHWOMhCZvK`gL!WTgBUm#a?L#>Od@A!zIMUCB$9?{>pMpOe9)vQ&Pkhv0G}} zL#%Ai4~Zp4=X^{%EWU1qL1C&`-s4dA+$HWkGP_a5_opiX%XNuFpY#iA(X}_5J1uLy!69o^;Cb1+QAu5Mesw`O~jXF_w?&Lr4~hwGS3pikxj`9 z$qbfMUGxh-REJ`ZVzwT7qP}Zn&wK7zrhGS-UD5h2dftFSVKmnO;|VBx+sK8}1RuZL zE^$7FE9#wJK)?=)8?G7Q_eI%O18hwlH9OmBOYJj0OlwUu60T}s5Z4E{*b~h0-u#iJ zME9L;E4Nvq0(B+?0 zMU1G^v$ToIBy*fiH!lH2Vo@<+Po(1}Mu0%}DE#TWw z?^sVibz3&n4q|Lf)6HiigyA)0y;?D>^lNpY$z&f+b8o1aO{}cETf!|u5NVQ=7h&kz zqvq$+rPY94O!d4=X!LKsKNllN%jUPk6dRHMBFp2$VyfRAdHDDQ>ncY_#Y;bf#5*~( zN92z(xR?6}4q<00^4QLzl(xIYE;9)1SdJADUm2 z78y%v6?yONMaCIPA78q%JQNOEDHq%!7ilLTA#2-d3y0U2mx>Vu9S=yKDXAboKMJ2e z*Z1C?D{niSM?NWlp-SD@V;a2H)Coo9&O{>7TVnZA5;ymqN1-P}+Q7$8<&3g1YHErw zOSI>H7$~e`$`1hEzy;l5@7`T1$lMOB2KY5tp|aofh0LVuAD@vkOzS;!vwUjRC_7ok z5VkUM%g2IMz0ikTKGYJvQ4~sKZLV3U4yA^}Vbb$w-pKzt}M1PvekK-AszeB^%VRx^owJ`a!WLn3vM@+M_LxaL$2dqqAW^dr<{ z&t*b`SN&I}kEb1)r6nZ1duWFR;D&Yp#qHaCq;>2jc;}fJ76DlYOcpDys@Rmr&$%-Go}75k­h zI402-DG5Q?W&)3hh!REUSg!w04`iNZ>Z+M+Fl$UuXywIrU7#1=c1w5o1n&H6abF@x z@iAQCq>6hAO6ocCrK&&RDHXV&`=8v5Z`Zw-Cn;~BmewuPQl!DqgL2&h1wRcxR%G! zB1T^CClWp|=N@f;tw|YtTAH^+*kJAvR~E4vj48fjO>GmuCC2m8gTUmVrWJUvC`B6> zV5l1e4SfR67l@VfkSX2)0cu;|p2U-1s=eH$Bs=M4aAi!Ki#H#scS1cfu z`wR>~Y?a1Pons;@|DKcRusUYyrlP-(-LZ0)7cg~qJYsWcCERdnor&cKeWSLN3%|G@ zcp_uY{}{%GIUrzYz#xwl-^*~Q)p;2&Y{*2ZFpLoNQB-7m`eX4_17xbwYEH({gH@R+ z_XyL|(C3qPwR^4hmrEM*S*RtuKfAkJWmXo`WXO8P5o5)jD43Pax5r*beCg`1 zRb+)y|I)??IP(8Zu5{neZ3<1Jq}u29($R{8Q7a(mQqV%k&&u~>vxx4Vx6NLn)J z66f_TabfQ1e^D)<-vZ|eV~uDIFhjw>4QnF?wmIJYxfrjoI(%`JNj5nhg&l7LZ!XoX zxk=u4hftGDFMD){9M*4ZtIm3o&}a1%h}eDKl>b7fihD+3j~Kt)txQaT%Y7)wjzbe>b9mex8Lxo7jC&uW36|$mMkJCDp@3 z*v;DAg?+wx!S`VGgp1=xQV;jaEi6q&Dg+7-e>&G5_KkWa9?sH@ET`4N5~i);egK2E z2Yqn|bg10)JT%MPJU4*+q6V%1TYWBb1F+cw0uD**$Xs zS?(d(&x)!yuwu9exExQZwf}EmHe8xI^|;$I0CrSrowe2O}xjs zd0dsyr1Imqt#yXREjzJbPqi&B@e%=uwbm$-+aY<_MBND_s3Sb~wzXUlq@TW>SD%e= z$(anC7xCL<1FI72eRJ+ZvW@B@F&*((wga~173b7?oiWneoaI$Dmw8tA-*%>*9v_b> zs(b3bwpS!c9AYkh!#7J6bN+Rfu5_EVdVZX`%F&~wQ!y?p??Z1b3DGox)h3#;;((YIV8z&_)?oN zcB^RM_{-Wb1p<`#PVQ=AT1wp;1B1P$ayL9Bw(Y?fKV?@5fFov*9;-?Kt@v@#o$c*} z=~zzMeQ|xIx%_S?OMJLyqD7UXc12!)DlH^|mIZ=($mJ;((odoZb8Ubwa>$|gk$#Vo zY))0uY?Lv_YDgCk=gaoBZPajZ4hr9EYD9p_1$>0WulXnrBX+wgBFj^KENjg|bw$F7 z1zB|r^dFT0h6wyGzZsYzE)0^~KAxd16-JLhVh4gdi;1?3v^(11!V8lK-0b679i8lQA6Kn_J)zv9Dyzz@d~ z+L1e1S(BUUdCl1k!&N*OBniS2@)O!ze2nE~10A}PH1abIG`Lcv8Fg5HE5fcTphKIH zzu(NZM5{#mS*&&Xd)y;&p5$BP=bwCoy?gf#tc_T$=$q|T%hyg`JX;$j_nd+7fO4%C z#S(3kQ#!A*S`~+lAfa#?rOeEj7mM^Ue>c8jFf%eR?BNxjkh;xt_aC~XXYIre#DBl& zh6V4bzn+N-05w?FSMNDx4+nW9}Q9drVU*jh|l{Qm0o<(2NocNW@T zc=>NJRF0~UE1UHTf!F^A)@xjIhR+9N^(s3}JmQ6EmsUIdpTs$pXUgqgk0Ql6ty|FC zFS#L_%TAQVJtrId*FNrOQjt%V7J~azKw*t(B2fi}+5a zY{dJFCnm4nN)Rl1<56cFNrOuU!t)9R*+HU95)n zx)}k_T7CEVe(%i+(@o^-%X&brSz9aN3TPI6y6KnMF0bRi0Kna^nt}p!zmETq9ulh`Q*jU(CN|KV-9HA*S;|=-_q4+TTTRaflDr`+#%fPpO1|ve{=Q8BH0U+}!8?uw`V+UG85zurB=}Qu2tMMl z4c1(Cp04+YrOtRk=Z%%ZU1sLy&K$b6Y*nx#Y;XyR1q&l=_%Kva;-G_)Kec2STH+Ni z5^eXwz;@HdADA^j*hRHZ)m0(bz>6a7k47Mzh2ptU2qc$AE}=lt_x?x?25bgl6g2tCR&31K+|}6 z*tDG>&)%yqb#oo}&Pm5cJVRzDcRQbDEcXNn$sUP5No3ionaK1(A&lv40osb%wBw>ysdM(cOWD6#A|eQ_ZL6W$ zd&R1|sh50}LXH*gYynF_<>Jg_Qh|iHlX$jL?Ww7$EABW+6caA7tUe~&&T!-)w?Iw| z|ChFRc)#(PzY}wu)qGJ_!-XK@A3=BoQQT0B(X>%Ra3y;bGi7}Kf7C>(*6Uqt1~29#>KjjpEQn!lX-a850XfcrC> zIG^ZO=axDOuc@LBczGxK$6e9COq%Szi>5xsN@&Ffyc1M|Zl)weET-(|wJdH!?(uHf%944x1qFNAe{w}#79Y`& z*v4Da>uK!U9M%Y<@znEYppgEM`l63`U3U9SCA_M!$x4V7t!6)1rB~*MrQDQFeT6{? zqTDqJN4tt+V6(LY=Q2u_V9(9WL*#YX*1_xGvnOG{dM`W99NfC`S#E2Y9?Vdk!~<_T zCwLDXPM3S{%))Lw9tNk6y$CQU_dWPB5ZK1`VP~`MW4Gj*hKG@0xl%9s-m_W{l8qmI zC+q7V(A=bc8onYq$%@#*fv}dARN!lX*()RSP{ef2qG|8mz6|Wv;qc6k1B8)f!1lb+ z>yVv1f6LG-c3UIyQ|gbVX*+2PoCh9ZH_NZ@Pd-1aJe(bAX#-@JDypfesXE32rh*^9 z$iAB_AixUaSJ6MYaT6@O!{y=8pO}39qZRSO_>Guy%;?BG3fTdjex1^{e=+1E;{rsCsr* zZ=1?MPV8Bn8tBtjCaK0V0hUCc)wO`n;$)Pkf(qqsrjfo#UKf*!Zb}1_IC-E+-NdjO zxTuYf1|I3D9l{x|znV%CC}=cNr~Ud3?53pO$2=}zeV+_Ck}W%(`TEr|}QN<+>hF+^NQT2yg3JJ%a#J&s@HpkrHf*XeZfa()`j zLZ176Hi9H8ZKF_o{P%JD<7!0kA6AQ)7{uY2WC zQOv$J(FS5$oAY!$qoCf&&!o1Kd~aN@G^u)?XXi~`-n=HH-<<7=^7R!#}x32M%LUZW|S0JQ)?^h$27CM7(KP1 z+xZ6B7JI&FM?hj?`}d`T^faJ3f;Wg>AmRwPoIcR zO+7>BXf8bAE(iXo-jk=jC))R84-Eu%N}0qMuRb?1?dGSe8(Zm3zGL2Wt-ssoTnm`c zPq^1p6zVOBxIIzWNAw^Nfj|No4yTRU*xcOA6J5h{?Y_u5y}P-HQFVK-t*t$O_;3+= zbU}Q4wNvnL-rjrnpSXF}dz+&5_0YYV`7#KoLp`YPy-I)oS=0#GJYzJeW2hf&Mp1qqHT*e_A1+aB^E7Pu*}}N?=G1hc0o@y=<&v$0Sks znKvK(V!Jd}!#)K(O)dWsl;W+++K}dx@o$}M3Zrkx?2}0oGrl*aR?#EbZMNLAW}|GD zIgY16Fj+7-@fCpq$`G9bWzMw~LV2Nhoke6xP-6HBCZkUFr5cMW*~L3AOIRgvCnQ48 zca!tmE>>v=BK_wcW9AAY=2qI$7iXXF0+;Wc&!kz0?#upn!P>>9LD7ZI3GcSA$AtMBSiiIaHaNuK zVxPvKXpAdx8*-=l0sihTe;bfZh`krsrRpP_v@ni;O&}&8_QNgX!*yF zALB+oBRpMWB2EZhl2`nesY|>Q;40&}Bi3^myFgW-L@Q6}7*R};@lBKyKEDFmxM8CYXUGf>3!Wao7k{z@2Pve!i1?ZH2sg z1R?Fn^2z8n)YjWEIm_E;HZw>^8__c|o?E;&0IHx3)UH;$gI2E&-E+iu2!@T#3k#f+ zbg(D{NoPximvG`RzqpwF$`!OQHz&AzWk5kwJ5a(oj>TE#T=lsLWoXd4Y|(wO8U!n( zD9{dEAQWu<{Yf1t4j~Q;U_k^%{g0;ej;Hef|35-PIbD#P98JsqSoM}pYI;8O5h@g6e5=x(KJW>@y^TJ7K4q_mt^*YSgtOxJ~s>BtbC z(aP82k9%mRx8gBzu`BdC1hMaGbxxPPva+5Vxceyd@1wSwz`dpJZ{POMiJ_kBgq)hW z>RE~Pi`_QjtG4-4%L_)x)fjMTb>v+I=AB=x*SwlDn(Mqe+XhOfu0BYG>1EHlV(9VV z;XRL_mF&ecV;$!Xlo=XSGO&;+-(Wz`KoGYy3_xp}vcC=+c@SK-7gSNMq^zz!o%u_| zG{CV?Mm>%mKs~+G=Kwub5^+?Ll2SlG$!veorYHb$U63%i3O@Nv+NmN1G-0k>$ioto zmelTAS@HJ{?X55_v4~sbppHfa$w@0)|5GSf#qLQh0-68`c294IH0=bm&C`!dQ3A}U zA{^c;=HP~|x{Dj0_H69*R-wF`2e%mzR&{Rqkd!!2JBUOK5~wtLC~G`koP|52qz5r9 z{B2&jjNWr^la*0ujVdUCx>Y-8MVZZ+*=uV;?Rzq%F^m7QTqd>b%WtwEoV8$6+6nMp z;dq;>>;N#O(WKORYt6(^Cz){uww#)(;YzYZJqjm2?fgU`G6_y4BUOPk+N_B2TTg&x zB}|D3nDMilirBeU@2i=S3sM+lCj8PNDchD=muvHKw$wFoClSV*TUdx|7w!xKy*uD=G*r-`Jly9_#wy+KYB{MJ&Y@htANf(Y{;z685qIZUlRsX%bd?;i z_uA(iL)C^~;ITNLurMPY2wRG8Th6|Fhh?pLto{w*YK7eD74Zjx0%*2IAGZh5()Xo_ zC3iDYKs952!p_e%0lh%s4L+Zd;;ym3W~1AJg4hgkV=n@kDR*PXkHo0I_e48QJ{6Du zURg(zRstn*n*E8F`!5}V!_Hb?o zj;PmsPHor_aOXL}SCzZisoHf`?fKePx(nEUPB3J=E7hxE8a(0DP$lpgH!$-gnJr|b zT1E_E(v+K7EnfMGB6PT4E?JW{c)Jw|en#xD&~>J`4BySgXWG^$BMBfpY_(`pT6OV% zaf`u=>sYb3g`L6+r&!DoK-7;HfA!>((V7ylB~<|wUoSTh*WYh+JmZk$L-142vr_jbs*l1r`((gjwEl-LVk_ua5*GeZ zJhhE+_5p@y-gWakv*Fro1{O1ojWFHkN_*l{!buLo~qP~ zv8Td~Dnbbd+hS45`fJT4!fD4WPcODiKJz8(jEN6o(L5nEfCDtBQuP%~jWA{{S*tZ* zYV%b;HAwQ^y}Mdd-l$rF{i9WEU8AvAqVdA#f1ha&rRiYBNM}nUreyILid=1pO^2;t zl%mbtkb(qkEGKCR5O@`VAwadX3shC&0eNEX2eil&k9AP%pQ}2G@@1pB={B8)phpwZs6|YLT`DVBrDa ztUV=_f;$FJyTOeiC-m`S!;IjC2ggizkoUJm%5J2E?HZiAP%Q=*K}A-FRY!c1A0 z*mN3ov_`aW;G@CnuXZICp&!t>{E{I;LjwX|z~V&I?}|0liPuVR?#Z%eWr`r9?fFfq z5&y`%oLtfR<>0vCOtVp49`2n_7R{CzsjN~@yzKf*$io5*GYMJ?l7yU;;>Pb0WEbV- z<>z1gdDW&$W@WS>>srOtDsTJ=tafRNGZnUY;7yp#R5Pst<{Z$6g3f?AkPhdQ1uv@@ zkVeOkZTRYxI{AHg-bW|w61oYn9ODKD!-ro_-P4=#8TjV!SgoM~7`U@m6Jd9h`%^T4 zHkTk{RQ+oH30}-s*Q@EbaykVPraIpcQ|3GihBH6?QZok?q$Zg*4Q7R`<>&SOfCdx> z1D_lcJvdLvms6q9AmgQu<3$!yxSXPF;BNN~Vlb`E;Ll0^Z+VW7U7mj+0wa#atDVKG zVymaYnYX}Za4$At;e(NpQ!-9It;^sjvYkJb5nCyR~C$Z7BsOt0OpS6A>F%@Z#VkkQ7i_=sQVxRe-b^dk8K|sqtt1d zohk3i@Wqt<6JcRVe2XyV+9_D8GI(M!ddT{12%j|q^dpr_DglFY@tu+u~`daU90Hy!|n{T}Ka^=Ou**Z-GMl3NWU z1SZU35O(o@as(V-#2(1!5N8jwdh%UbOttde=62kDJv})ouHR?AVo@=Adv)3(y=qW0 z*`$9{N!28aj-2GVX}Gt7W*=3-n3>B9e-dYExN;1HlT2B$>?@LK>ryzvONo+xbZW@N zhfq$82Ep|sH(La49UQA?xt%ZXveKge5{Rui5XAwz$=4aIKuzA`e`))XtDg0G>%qGA zQ+?}r4VGorLQm*2kZ~-n+S z$=)H3nWRcwGvN=1eJns{siN|6b`oWg*6y1J=J<|#cKq-eS=lblfA2LT<+=D4tw1A_ z>ILy(l(mQo?CpCo>giJSwF_x@69Xlw+vO{&TWZvI&D`3GN?8e$%C+Qy1ud z681MvHvVoQ*POm&z1*IoA9KFp=5Pa5pUZ?G*u_pqx$q(`|JZD4Ui^y7%R2s1Ql1)D zwS9I}EU(Xbwa(tGH>9;cU=_CSK!EbPR^RXMmUa}lpU1wrSV7(Deh&|ThYIT zuDMCrRf$LO6~BnNaG{vs*83<#-_qKObS!JFIp$H|;wxjJcg%U!c(lOv##dq$hrx~q zn8pp<%=kb;B0MNIcszI$!f%kernZOQIQmH!9wW6brMbF|od6<&0v?3y9^?Tr6fRj3 zrN2b0bV*aohr0fGTmi7MQOEzw>OkF;KGat0LUV^tB?X4o6ej81`pOdSqdkX)|9hpG@TV z-$KyCaEUfFTGZwb=VxR2%m&paISH*7*Y1HHYc_%1WUZyKOwI4~(7=i%p){ z{s%Nv3JQR>-GYi_lHF2_0@~9`W1Ko7otu|O`k7XXR7rP4dgl1jy?Gmmdkb{P4JK6A zL!(L-egpP*x5R#&vYwBJVfd$hmc*RKZ6BIZm%*AEA_n10T#%Pl6uO)G1GU{wd?o83 zE%m(!JNF7Gb*GwMF^D~G9j|%^lUOf8l(xFhHrsNVO}%Qn5#|oe&Ktkw^NwcLvla14wwvcLK6cIe9{9`-_)cw?GyyHy&ZLI2 z6~Oq8hpkrK)iM=(%4&eq_dxtWO^69X6heZQ+dZ<^03Ti}eA=hlxP%1QwPhlWG3LPA zWQxYsLwR$9b|WH1A2U@mEmlO1evxDVZf!dU*FVCCcJU0%3$heJD7Ihi;UIdbw>M$) zSE!GVc%SEm;|)BwKV`n`h>@9y}0|HiHl^}e=bJ^ zKFAj3#qQH1=_dLPO^8yRZLNFJY+?c=f6Rh^qA(#L?l}oupv=NTv`<_d6ot-tyx@ky zmfW$gd{x}KUKbpA{PXhsf&7INdsO~*Fm$;c*C+dE zlCQXRCQDv+AxLKZTAlb>JnT-dwfFR<EQl?()TtV3jhy@0ZdCiVEl4`sf-|;iBdRk^lHmIE^)h=~MnUuN>UdKwcI% zmaR9UwBaNt1C{@kf4wg*F%G{&%00fYwyohCLADh6K;h4)bbFH{B8OKyo4H!XG z0$(Ol6&KgP{g0YC=BXKDMtr2t@OWPTCOwxjlQCe2*Oo5M064j4ArO`Kw5+ZrY@DP? z18mCLn|=g#bo=#?=!(=)?}sfOwDw%nLHyVUz1sH+B*E@e?P#5W-$T;tucjd5&po=B z&|-K6zuoQwq4uP@td^xO3Az-ncDA%#r`gwl+;Z>t?{`tUbxV3QaEjp{im53|WXq5w z76-WL{TIMR*`au~qUb#xuPn(U@RCd!8ceQ#+;F)bCgoUaNA?`8u)p*J@Qi#l^-26u z@4G622>}F`$armw%EUkJ&5hH71uP`mohc69z?Lp>1lQh(3XQ3#t>*r7K1{YBmIF(W9~D$*XXtsvI0@H(J_G4*}wtcp-J|aR8K1%D=HubjA(E_ zFre^a$LNUp)Ejym*8X#-pI$etL>}I8$$SKP6`Y6pODM%(2+Z(Lz0961ZxeJ56mqst7jh1#{%ne)j%VE?xmsN4RLN`5l;LGaQRf^ z#P~G1&1U>MJ#ESD9lybYc{OceVbSPT*<+Bp2+)Bgsw)(71COK~7TlXvUIMosasJ0? z=JC52Qvh{4?9egi!^Y&=3&T!gCzB+g4=$KS2EiCTefA9~O z-{$JAGJkKX>?CHFGgSXDw2Crf~kCnHM+sEL7~Oq@|?3`&djFO7pCDnq`@hZrh9ZrkR0Qo}~j4-?5;R(kI>p_hjS#T5PWioipAlka(>7*hPYZ<0( z=MHbIY?Pi+>d`|ZziNs)aj}@m8dT&Y^}tl)0+@y*i*&T#JY1&Oncpf;vGe=RtB4F+ z0bU@SFxi-uMgOYZOzY(z5!LdiJ*%s$o0~cPEG&)X;v?nXw6cx@DB2Jofv4?HG-@oM zi338M+5hLDaaK-hLD3_OpNZf4WLd2YRxPUDF25~wL#h1l?5tk(ob^ z0?i+1GH}=Efv()z{;mI|fcbWI#Znvsn<5~c>;}oDFmRYiKd&B9lf;HbhCE1Rd#*wS zivrh4e15#7`&{%P7as6<&+W*#J*^79cfjZ|;uxv2-RiZ_?6#6K(>`6KR|l!uzyQW? z+E#NX4&LIbqKbD1ui84=FGAG=Q+PG&wzdBJ9P2-}Y>)Z+b$~RI%#7Tg+=;9>?R`U3 zz#@`pUlMtBHk~nLr+di_?1|2x#4fX3vmMzhbx<>aj8~Ha3D)Oz)Ix>}Mkpr3c zI$O_d_EYWO29E|lKu5WW2LU$vg;N{@2l#7-ZPpxT5UE@RNxI9pN>Kyg$g*3@NX}?s zQU4biO5I!cpP&SiN+1}*Us;DT-~aH+sj89aeNB!In{ZLdYicaZyp zP`T!!rCsd_naHDJAF3;%uFG`Ai{+csX>zivihRmEW66=dbWZ=^F-_HPPyFQK(N>Jc z(B+>IE=C$pzW^>LUe~HA#3g{BVmsrnW>k1`x87Q~Ig;^BBqP$*P0J=;Fy091sRLJn z7Y!6tRRBDjI{u2fB&(u1GW>C-)pIH&qP*@C!i;B$wwRkX2m6)vb~nG(=Kv5Dfj5@M zNQa7xXH_nd{@jb-cdpL41dPj$ZC%(o779H(z}2ORtTRfv0;Jz*HsX%d9Wb60Y983n z^C1;UElraW&mS?#_Un?5!8s)pK7e9fXOBwtV|61H_AKFEOe=Mhz2|E6=Xm$YQ|q}8~eG>aY;~Hz@Byt6>gV~#hA^43bJ|E zanmlLK)hnhk}|D=AEzZAoUa;0wq^K4NTuZh&#H)lwDr~%x-^IZnn3Xqs^3fbz5+ExI9HYDX{!KG6Um@a z%#=a9)TMTe1;@;ZH1Y4Xj`D+d7N5uE)(4u5$ zB>3*uwP3uf>7n9qttYRg2OUcdXX8->4O~;H@CLtjQ0w>be%;FGa z`lpLe9Vy&W8p|GQpBixVgg5=5;#+6MJFo7c_2^>4psRqIgtZeTb$?&awL1S=Sgp7H zgTP=#g)ve{LIZLK26l^oKkE1!o7j)mH~m>s;uZ&j$15-PMwkM27S~uvly$??!VTQ> z4Q3aKgm2K(q6Th=`oq|QFs|#K4;em(-GOcm8xUFh>p)S&jjH8DaZv?aDH-OKmSL|(|GJi9%IY}36WHbx5<{ z^t1RA7j3ZkxB==PS1!c&fss7WLjZ zR)w)$8N!hu(Ok|XqkTz6r&3SxT}{2(Y+KS>X{icRGt_JS{@;$g*=+m&@y zTP11$v9ya{PDRek^OF4h4@&RgUN;xOSn$RT`B3$3|P*fX;(jj60l+?dA7&np9 zCvdgQBfgbx@$H)KqZMxgq`*0ApknfCc$2D^Ob+nkt+BGtLeWF7p09r)G2zr?AQQg{ z6|7aP2`k3(_fcs#CySI6H9T3>a-xD^?aPa{qB&BFF?Z_!wpPSAf%|b2TRV;oW7EHJ z)~bVazLzC=`jhyi^P# zddevTA6-o5EH7PUZnlFU#bwyD1I24mt_|PNW8e1^4W?PV*pYCj>r(i2mp#7S#rrNV|T6tA*Fdxt{Ln?vsz z)Y+}gqLLWl&d>;@g{(B{YNuEmguaGz0}|2wCa`TT)#$YhCB=?ou8rKa_YZ#-`4n~N zy~4qQX{0AD9CTL$%GjN%p%)w0(R~`8s0$3JoR?{0GpSUiX&^|Hq}$`hnn5J1GSa=N z?fHQ>u4F)9mTL3!lTT^L27TU`82aY89R77i>6W+25~?<-#;9`8%GIx)Vq7sbH(4!1 zY$g_&J?!9C*FD?)%a9IIl!~1(nE?z!H$*jg%=R3S!1^Ao-2G=RaFI^FWC#I$(BAl} zc`iri5#d5l^<8Cx3@sg}5>>-{g0$Z>=fVAliV$ipJ|NN%0W1UMn;+vD2mKiEo}d}P zsjZs_BFpWzVgJGnyT2BH$R@A7BBuR6XvOJ|F~oL2nt^~l>x=ltWd7B|jWZynmLOf7 z!1#A~XJ>d=yK;7ApX$T_%8IV481Rn{Hg>)JEY~?}Z`sCx|B4f3p zOO=548Z8!50(jr-OdoZaEO!h@U)V_TMCQlaswa*V2D76cNQA)iB@lp zR9<8@zBj`_N8}eV+Qw zBa;nXzOC&q1Tw}AT_n{Wdse*_`mep6{8PfB`&3Woe6YgsxN>n^-ka-5RG0q&r64n< z)j{-454)Dl6EAO=3k4xsb`>{)`q=WH3U^TkzLu{VR8W4;?NCfRjH|?XTrEyR%*U$8PT!)(1%04HZ!z$I<)OBC*v~n@5a!@Um_ig3~?SpJi+U%hQVklLf|uSG@W!q-*FbRt0> z>(Mkincf`u<(x;vDw^Dw60bKee{cPD0iSt+J>1>j^`ohtx-!9&5b({_aF6UGKRKUZ z6BBWM2fKU?-&BO^D^+i{|K06CO+gcpSq1<&!qpNZW$Jda^r0T|#vnJ>K zVj#syhnuR6AxLJ=*xrnwAazjeXaKGrmeR@Aj%$+@NWA?n2tp0B4vA=ypfAP(0!+oz zE1+c!Jx}n^A_O0fJ?lC-6~cRA&*Ju7phgH$+#XBlJ_`_B9{Se4V)D6S`|)Dv1FIL+ z)!Pn{ouMm|od^}rmvcL>INyVSmunq!{NSqhdVg&gxZD*dhN=@ug?3f9^WTuW6{X$3 z6Yb1aj3u-zUU-CHn&V#ieQ!~$4t(j$8L0oAvgdcNQ0e9e97YF~4N^w@ za7yqPDwC|`7rwim`KW&gig8dYgcjTJLN!ES2dg29hvA=Q1Jl(3|1tn+CFC#{Vec&X za*-2X2L`V})^YD0IBxv58X8iyxsAONdCX9H9{!{fTsZjcd(XK#w@x(L@K*kL@RQ0sYD!C;S4EE1$@0jM2r_lAiue%+9I$CpR~?wa>!9?}&`l%O^@zZ-Op z>aAYf=;-?IKR%$sA?Xz0C9X)JT$7f}OpF%k_)L{)eGFvRLAv}Ad)+4Xnclf6jb58W zbAH08?JnF(0qYDZ5(GGMN9l65w^}W2HE$JgI!vpvmE|b*G2p^*Ejk9@*2o(nT zIc$y$FRjJPmrbJK-|iX2bJcvMR(Kv?TZK?|U@OHU%~I8}|IN@3#@v0S4*O!H{)xF{ z+vL|+ufUyc8*zqQ!5`=^%#T*(Gy+}gg+rGb?xd4=Xz-{qsu-qCwd)y7B8F*`q-2j= zi$^8Na*h^sCmNh3seHcdQcG=lN;PPQGgBx^_LVhKY6O6zqZLR@uTLx@K+ux>2evV8 z6AJ7tH7u{wm3Rr5KLuHiFJJzr_`%MRp!yX4mC-=QpY0Kv_KVi1Ut4E2{F>{yfTYE9 z&Wfw_?FuZWi1u|g(wqWExpvg+cO`nx(L&nsd|Tpm9;HrGgOB8NN@Y3w5}_~?pOBUUJ5Y-C%HdcebLV#G;prtu6E zW0BfEFPmBpaKwgV6-q9DeJA zbRnxJ?oyA52L^f@sIUCmS`h62*8)h>-G03x7jSqIq!7A%dA`bF?F<4Eq+()nU&5T<_4J#@d`>SFo9)iNMAb@b^_9f$!%#Y8f| z2{NVs4EUj`SXAYX1G?Hddm8LUL(|V!!jl-0wwCDp6hhJIt16JCN^W5UJYk%{yL`P! zqzFMwAV0&nYag5W79EfHb}Dx3{kpuPd&sm8t*2Al90fGxLk~*3zhzW);m!B9T7DX;kNZka#p>z?%+UuL_jWRQ)(iOlYCWWnEvw=GFL5}! zmJ@iJUPMb*+BJLMN#t)#xi05;2bKr-D zTdqyAcd`rk2I~Ht5uCzhUI^ZdE7*w6oQRTqL^XpPfF?R8!#Vx^*rUDJ;W2J=pXG0G zNKS}V>&LplDkf)_ev7Ip1{_i`>kNQkldcAN5NuIVY#u41e|< z%##Q|IgYAE7M64HUDJ${&F-5}Zpt@YYh6 z$6!52zHzRj7% zhXpHh^>Q4>fc88NsFM+7MY^nZ`#&C2w?TrsZR3&ZRpeoZsBKl_g7*!qaFA`2(NnVN zU!BelpOyJ7t=F)39r5j!_vB5|j3PD33Ez5|?+73vvpcUg+zMg=*i-pg=S>BaF^_FW z>m;ebP0rYhY9J82C>iCd#IO=8Cph*oiGfZzx=tbLzpkG`R&rST_XZ*;(Ryrw8_EIS z&kI@q`KpZ~7nNpjX6#|vrIXnKa*d3gC?y8$B}OK}$cf6yR09NP{dnsGN{GIZ(Ozy$ zp9`iNmzf}ps9<_$c8BHk?B6!;kk}Rb+ls9PA!+vK&neKJCzM>o)lFvte$BP-@^10+i{igXvsR!zHRN zUIsJ5#U==SRUmurb#3xAFFpf_`)OBIJ8f<ez3w`?9QCiqSqDz?E=6ElcJ{r7Ae5j&KGzrQ66b5nJDU5zp-k+LF4_YrALbfS&R5n{ zM7-xsIZ3~L68aYMY;>J^Vq}q1%vnXucMk7V8T&8F$et&{laR1d)aYO-(k~Uax=#aJ zayC3b5#U!EE>)kV}=JI@rIIx%{Mj@5@WrZ z=-k<&vGx2E^wN^h@`xnQsiaH{fs2kWQv0;c=eCI=5^nl#(KxJb_nRudy;qw4(XwxYNOT9Z zR;8^q3qLzMJ6opl!j3PSuMfJ;eLq@tT!bs0gezWmT?wo=ELv%D6(ObH8^RH{{0l$_ zddeVrgS8j&<&^=UUSnd~HiDwEivAC(#Nis|;P$D%UW;s8%lEy|Z;tWc!bV10wkWLa zq2-z#7U%sA=F~leHDJV1=o7dx?_gEwuQQ7cXHCT)^;O~w2EN@)^y>r_q6sR9!K8!J zAc<+-wG80A*SOm1K;KO$X|`O0&TR-P@exSqS3aXuRRa^_N?{#n%+h`58^6>nn;ibE zZy4E9QC*CxxfUAeOo)}>^4bq{5MFH0T(x63GClfEGa_KvlYGOE4I_unxGm&1Er0qshA4+ zfOt<5@U3|Kt*)X17~hDAE07$Gg2Iypx!GWSnZbiqsT`%Zl93MhnZj4JTt62UsG(V{ zq|v}{10$DXQ2sMWYiFmLuVtz%3BQtCg81i-FOb!i|NaY;9N2tWH+`p9SbJh>im8*b ztIpE4CQRnzY^b3RS43syV@5R+FE7d$vp^>Tq>ppGYNOHG@KhgyC-o(*Y$ zeF^*PCuMAZD}jdI$i5uAdrnH>u0{W6Zs>;-Pbwm$ME^4>Ba60@CQ?>+%Z);%oCHBwYK3^MjVFzv@sJUG>`LT^a}*g2zyeD%e8v&)_N`3wf@ z$Tzvy%_S2+MS#5e1=if;zNjNFF&rj=cXI0<>ftYLFlM8U1lwxeGWoA!3!FR_U)sWl zfFvV4ngMHW5z=Ss3phFiMEB(W743p@DwUk`eBSlc_1?#LiP0xPKyZEGk9OBI??S$A zSrV^!t^i`=xV^!fgebzJ5~qZY?>o%VYwYlfQQt9@Bw`@!#TDV;<`%*&8vP3rfK&kl8Ssz3e5g53M#lXyQj z?eEnDQJ@HvDY{Z6oui{Y50o!u64hYjaO*%eZ@Xk&cLv8mk@09o*&kJq;Kq-XB zQB3~?2rc-|Xn~IlM0$BYnVi@Hs)b{V#U^c|v}r&A6#^(|fxCY&d$Y zjIQpq|V%Rsq;dHvkvpuvJ1@PPKC=O=;L)2Qf?@&jezX~gSi?rVXt z%{nXPnrGz~cN>`pU z7>TW`{=(aKJjX?M>H?G*PX*th)3dX)yz9-p&dVHPgIk~!dtAZEUTE8!kjx$q>H0WOvEAmJ6>t3h%VZusB2Ri%eJtF!5HqO)uKnBz!wT`&TY)Fvg4`dHJS0WE%E)CX!94|p#pODuI3nPpU^=CWJEENXDQ8g|brIcTY$CLDY?JFjG)Gh27|EP<2+)X!A)tzFd;LjrxDoA^-aBbd6f61cs+AFrptmintk{+%1OdD#gWtGi4Gy>= zl9FDpb7Wi85&h@%C#!3fATYFSsp5y)XYpbKq#yBXRyWl3Z3g{S;L9maLTzI2Zq&QA zWtQmQdl+%J@Z8Ps$HI&Wf0^N=plI!!Mj5|=pCu}O&R~! zM^)Jafj7AUWALs-TOA+$u1n+V|2Q-k`;)v;LT?P=8s(0N{pWTjRHSdRFuT8gk?pHD zH#g(KP?f{o;W%A8T9lrz98UKcPjQ+^6zH-&ZS)`vRL~$X`@O)>b;y~DaOw`mgYvFt zO`f>B<1?@T)x@-VyTDLO@nR|b;BtAaPu0Ry=RIEvBdqUWpB+9F#~hIn$yvzl z+YFQyTdipbBcHj3>=1BCM#$A!g1cm0g$A3e78NwAtbr10KOD0%RC)l7@C**V1itbZ z-?e|^9~6K66ua#L#qF!=J2-$vB(=kDDKJyoA>lDVbY3MXse|I7_WY7!b>osbAzgC- z<6AR8E%41KJ>kg@LCaAiqmBRoz%W-*v;VLNxgxAy{F$fkztWm*V7w^B9$TP<&M_Fr z|Hc7MUxZCr^oFO_O_<%6@!=sVBN4oz(Ui}81;JR@B_aP4h3Gk`s`j(%U9#dLyR{~_ z2+)dD!DLjc|5UWi5Lqgdp#R%4S?L3rQ!=Nm@MsXnzRnB4)s1v6~vD4!g7BP|u1d1;vb0<3vG@ zEHg-0VHjc9WmkIW?Q}gh2AFw1Svh@)@_>onARH>N-#Ko#`{si7Y@P85mj*!7x$=RO zvzbX`(xKA!F9l${U)~Q@4d9kvMbs9MKNV=aaV;0(4ow3*?t|slm##e-nkjq)qY|J~ZFB!8E&MjH^NleMYdTu5RFrz~A3eQm&=_OyQ8 zv)2#Iy0=WRq#dkt-vX2Q6#)0<^z3=><=K_>N2SUq5mXC~YdZjP z<^oqo$HBY!-xbPP?voTT%FEh;*t_o+boxtGpAooytj=d=JUA9PJ#7R#y?!XiPly%t z=J##$t{?tNVHsledn6?~y8(tP9um8*x3w)67v}k)-A(?| zZhm7qgZ0)_CT6ZFx_9~SUxug~ir1$?zsbP){P&{(#ESS`sgmzLm$F2;t1XkzgXWNJ zSCuyh<~F6cqn%R`9LeiSM|=N&MsSPtCi`{VrTX-@jh%+Cmk$+ zh^D#}CMSmO$5|a-lHgaQ;%%S2q+sMl^1Er70efzW*tf=VgEr`Q-=r6Pb1-*$?rrFe zH7+bJULSc}OGMNIWELZ%Rcu_vmmCYnTop}PiCx2iZEtN@s*#ydD$ZhAMAWsM`uTs( zRTTlv^F2mvc7hIfxb7$`CZrhF@oRoLmtK$ZQkPZ?xj4B5Rvu@EP#GiK89YX)N@gZ` z>EQbCF=9%$>+cz_@3C*_;j>Ui<;m$RQ19jP0$2F7~NAPd{G)$uyeaJ-~1vse@ zye;?L(-BQCr9TEwIAQO(E%R*PR)44ibEZ=o)#y&P zq~g^^;m6<|SKtI{3R!ht{NAm3a5hxMNX5-|*~U$pPK^yHJEaPu>sB=*nVYjRmkU|< zGD<_SLW{4hR;A4v)>m4oq@iQ#b zOv5>Wi{NHts(~d72u3#;$AW+8XpyUmN=5SVFTCP(mEX8A{y9=##^vFcDlPu(wc+g< z1$>FXUV3IdAQzhV0|ebrGTtFI1X|2dLT{t&bmRRO_Hf?V1)up%fcw4RZt7&q01*~t zfW#K)9ZCcS^tu_DXwTCs3w;sT!(~Pyj8!R+<;d1;f{zjXt?rk#bJ~P%pxIz@NuAhi zn0MFL*&p(Lr*=*(FU_FLvalA=zYqnH3uRw}#z&opCLmqNt zi5%ASy8i9!3;0L*~TCcwHu;K!Llp^2v%qg#>;;>D*tH? zO*beyB&@)Rt-rM<;i=_)S3s5noFmU%HoQ&avoKLULHFgol5SO{r%`62yD_ye}u#T;N|Cg;$#D8sV*(LOrpAtDN79Z?B z#o7`$4XaK-;Mff={jX2{J}krnipc2Rr%-)t+E<1#RSYa)V*U~^QP6SR7JfqWPZ74P*cf-?`sT$I#alh4Khq4LsPrVd!WTVPBuFSQClIl|L)6b3BU_@+c(3xo z@EadyK!JSgrqfkK5=^Zei$qanVkBmZGC03H-HbY?I7OMI1uy<9UA7H<%@7*el7|Nb zNe#DgdTpKc_%+D+QmS#B^@TzF&`ohtG-DX0h+wNT7q|CA#cPCgUkrDnKa?t!o02xS;$f{Y4h4=h=>&SZ+qQfFUCME72f(&16!i7&qZ?Qm2`*|xtvKw3bYUv@q?vte(oS;0l; zv%oFC#{ql<`O)&H8d-S2^V8(^x=UugqBZF3@Y`|Mb$H(8q2vFU9A^vZU4O66yY$nh zuDq>Ge13?5>AzrO9B$qkfYLyC%k7*8yx1KFk8nDr6H<3VFZx4+WO6zIF6p~oPv1~Q zZ3CV12T^Tv&s99+5x$-IRakvfj?e;jyzeCUXa;X$*Hwu*k8XcYfb7g%-pb!H*c0dI9uhpB1CnU~W z#Ln~Glt)CcYw9%AhC_Y(CrJWns7{pbU}#X(@_D5doXYjjmF)=b-?FD^j=PfCN@PL9 z-$LhqG*~iiY8uj8&-ED@Qw)=(U5g_b>mPouqI!#;Eh%rS!Ss$#QjgKek6wAPBM~c7 zk5~7jRP~p-Jqe_5pSBq=Qc#iu>ov;i+w=+OF0s%znIr3>+%I61j1%14w_p<$#zy7F zT9DNj@zB^FS)qkHv?NTU6^vXE4XukGYf2Xk$4MhyYY-y=0wS-I^~5SD#FV5)b?>KI z^xn#Y>y5Sj!rh?VRty2QZir>tW0<5?3UCZ52O%I~fu|l%hs-61SF)eYB-UiNYCsw4 zK4r$J?r|)VRj_k$c!qdD99-k4qsU9aEUCdY+vDG>joDttIAha@0*!xr0GpvP4UmttL&JgZq4GVakcFi@#I35F@UQP z08iS)klx@ow?%7PuzGZ0IM(c8oGEHf=!CSd=1kaxx=~GNIifI?ab#hK=!&Wj8Bb5k*5>#tTH*9J+^F0CA5Z7~Pv!sraXW;Nak3p5 z$38aMqa?)PWM^gXz4sm&k-cRdduEStY?3-wR^r5oL&_eX>;1#`>klY(yRO&udOn`_ z$K#P@@icx&$7~UlRBlMHfpfiha&i4jHyL}h zAq}B+d^ZA9$r5)QEc1{m(Agc9J(*#hXzXBpu%8g$6P0qG^IS{|hUl(|uf|?l{%UM! zOe&LQI(!fpWXli>l!*7UCeQQD;DEf$zWvULw{1|@PK(|~(W}k>EBvCtxZua!wtf~j z?D{e4*OO5oK-tSxlF|BY8eUu(udN~l0RXV{VEdE0T|j}`8Wal&^?`d2KLIJffbzc0*m@~xf&9AixUsINR%$-ZqKf_7lQwH?j{W&d!=U{gyZ zDYUpwz{d;+#VEfoV9+GL})3q@WdJ?$cx>9*@o{?IM2nD1oNjKMo z>a{i%1^WsMCSRYf0R<*RH=@RK_CQDosF3>i(QREe?6$YBn+dS0-2a}ku$QT~ejteS z7?%bDrv9|ry{ynzQCMfC*tnSWi&r(8u_`(64FW{jukGS=18;0Ot&O>bRW`Pf6d?-9 z!@wamG?gu+033qb#~@%~PPNF?Q-SJUcg{p%I6vt+B}+4F_hEV(qI{U?dL=4(URb*< zh{0-vLiS$3&CJcs#eK>@JjF0tvw`b@?Am5jC1q@=|Lyhi#?9JB$XUlcKpqrYFtM}EcsD#f01R=nf&59=Nn+iTi5 zgw5uxf3-2n=2We&+NQD_`*z#Ho4CFA*8uppo!xF>`yQmay8hQbYG!7x5O&KXE*a?d z4km0GOGh_~zd&ugB>xw(w@lxQstA=Sl*sSk=M$)ip4UuFPiRAJvdoL57S%?Nd{8IN z<>9OF_(j)2sf^=>zE_vBb3Cg|%oDTt5~rO@=aE#aIwaR?;|%Cy76@(2q{btL>^R1-N8Z=-ZO27Aza%L*4SEiN*ykv#S?w z@A{+9oof>1LZp9=+ZTHEc6a~oK5V?(f5QHTy}B;(Kl1noCyl?nXiTu837slgZohtH zLXlk9x>&n}1^*FXOq%}#_2NFXQn&)>YFvV4}MVw)u7(B3LDcSi6P44 zLqTb=xYm3Xy?&5Ut@ecimE>17lXUY}pvYp5HS{8{wyAuVMJT5D-Td!4#re7)W4$V* zM*5ubcThF8Q2ea_V*#kFjf|9khEp&7Z7%3#v(B8K5VbU+Gx{KQ8x12; zD227x=6WIjk&xv(B|<(K>G3y~vc^~QBzC(3iGMPh z1@qM7g!QU&d7c{5L9A^7N$J}}(&$}Xw|L;}<=0&%z*-*N{+j2YWXbDxSRqoNk39bpW|@WDp3Pp z;#wJjmZ^2ga3S_kRlFvy48xXq;W1N{pGxQ$_h@qG(BCC4u6A`eP8gA#$-!fG88sZ>9Cbx zTi5p;TT1+<=o?KsbZ0!ruhd5pX*I-GvC=VW`F8ddf9Yh;8r`=~b78k9RWFMSogSQ> z4Zi!hIwPh%EY+bUs&xolR)JluJkv`J#MIQy!GX(BXsd&lM!r^l?&?e>q879*71%U~ zeQ-8v$P1}8qT~`3JZ%y-&@easI@`4>>6YebA}Mac{NsWEP!@Ln`+LMCghq+oOC1EZzW#`L+2BJ2hv3tk)a(kMu!jI~~O<)wYybZuA#nvJeGrJu=H0uQx`Sa3gW8 zR?!~wtI5VKF5ddL?XZa)QLn?RQvRs}UlY1-$L<7E#LGO21Z}Ujg(bAS4txG9Qi8*8 zfRK|J?Dg558GWEdg^kflnL4!2hDR_7^%Rl|(}vihVUvK-imZ$>k~R4AkfTd`O)V)7 zZtBss(OUG|7AA$PR1)%Sd&_KWteH5~b_bsR+}pYR^W6`o?V8tr)`wNP zFp&HQV8VKmJxOboYi;@D!T4Fs+KDzt&re2je&J4>84Lg)FYpgB->mThPoi8~(7j=R zl`SE^N2D!vblCm{8vRX#M3wXRlcjB7#PQeR6V~OO`oi!;>*4jF;?H*)yP1=uxojuS z^Y)_2_)9~`oDP@VM~?3}-^8&0ehtx*Qq`M8f<}0iJ?iwqWp#F5T3lb0=lQEgjKESE z^Sk@|UjPGg2$_6r0nB$H{->Go8*h!?1Ca<*N&oiOxOwTm#Vw( zrj_N*oTg>+Q}gId@`}50s%*S)K_Fr?wWh_H6Gn4cf%qyv7D+9=_b~!V-3>_yv8OH{ zS80#m!;D-8?~O}Wfkb50l{(?dLv>=DyWjoHW@}uGf9LotfUK?Jx2606bi`$WGChm> za>yfZAAv}wUDwLx)U(kPoJ@cI&VJt*@)r4b==SzSPb4Vv3-1_{il%ggqf^_f$7y2Q zg&ESQmM@7`8Xw5194ZvyK#OL1CR^>dZd@hO!D-8McI;WULdcD+xg>(;y^hN&-faTZ z!>Z1!zV%&;%Sz8xP`$n`O-@xbWlz7;K7%)~P}uXYHtp=KZ{9MlP&uJ$(^Ow37Hs8yp!WmtqNy%ww4` z`L7@@LOKHmz&dOpcml#mrqpLlOC_s7SX&mS&FcVqH+Cpq0BHtoLL5zcDTH&0#-g`J zq-&9$<@IOA5?|%*p=lim@Ysb^Rh60^wf;Qa-Hx zFV>3@zmBScm$IKGBkf}QrF;kKQ}Nd6DNsV$W}>EB_p8QB+ksXkoCLq01yywa3)dq5 zj0cl0rysi2I53Cb^L5*YPCaA3VBYV)G9ShP?pp|*7CH7l7>HWbk#y9E-v`blAq@9` zYZHXaZ2B{a#+RHjR=IKE&s0Bpa5$vVgY=2)4Rp}yOsh#=zIvp`ZyDlx>N;Oq#TlAK z&Td$%i0IB^BOvvaHRNykRnOH_?F%OtIwOaelT*>z@LCAJE2-@>EH5;+kxK zz{##em3~&m7X}YmoLM(Fx7uzB_J041PsSXE7W()4>lKL`R(XljEpfQ8S*Z<7!RqOID`!U-NK&pIO zmBu+El4Mg-#r;aNXbamigw>my?Mu0A~K2 z(j88-AK#`7@FpZ4UU7(3Blmyb!Z-u&bidfglR=jQ}aNspJQ#Nk^c zdifkzI@9ZKnmzY8O%_4L^7Y7MCfjMFvq$slL7pws*uvgV6-pQ>WXwU;@)i8OKq3hI zc&eMn&6QVQbwO;DCck6Z{16~o4|*R!vTPE{SkrM?uNUrNE+L6!i2a+}GXAM&sVt=# zBcBe1I6Ys3-(R z=D${lrr4nkDfIh1>oe4Yjud8qeAWfj;i^n3E=J{OJKoAGp63A_Xv#rdx-MQ5YmruH zH_vY|inR+dt>y-&uVP+l2h;_s#a#IfLzaJqf&}K7`I0P--8ht#_lr# zz=sqh8B!S!a4K1$QlIy|W=e9XSv+nINPiO{7%5GrXgVQ+CZi%Frd<;4JA2B={?l;~ zFf4B+C*HHS0W1X*)4eW?!fOlEp|5BNQ3BPv%6;M(TD(ysqCY=FEFveKE2+Bv@iFTS z5Z&N_R5W+EII;#O*EpECko7ivcDIrQ(0erjG)ScyLlmEP){ngeKuHg#vLr(XSbPhA z)EV?m--lzMmq|bJ{f2FwT1}uVOZp{#uC|{P;}g4!vMdmTVqiSr#PneWOye zpL6!Q&z1_t&3x+Ccv-aoNQ=N=ZRd7ge=>dDn#DZ<Z8a`K%>g&$n#E-V1A$w>pt10D|B?$9bD^+ioMiy@*#T$$*9a zEl({{22i)6dc(uA__pt)vQDdXVP=KI7Pv$ZB2V- z0CM#oLTbAkcDY&HWUVgsH=*k=QtQ3R<5C`j@S@QDxxek-R~P4(&*%(hpOS@ z{U_(m5)2CT_k0cCe3+Hi`H?*D*?fbJp#!vZr1M7;cGXz`e-oQzS4Ho-PZa>&Vy2I! zG}}Z9^k$8p(?zQDfFbJ7**iy@pZ^ zS%53V9(+uQNZ_p(7|}o~^MqSJKsT%I)PH~h94D^+OJZFbLaID}DaOy&pK;VvJZ5_C zvk&@gfHI`33O)tXy?Zt3Sf0uMB7vB)uBqK_u$aVo6qJyl2cCtFC_k^yWXp=oL_xXH z!CYJ#(_Vu2sTNF_vHNK8xqCLRZaxdquu?0FB;>R_pu)fW88Vwo6GIu3^RLT`D+?vP zx2ZBqlJdb=z?X6w%1ITKiNMNGm(@7^2$nCLjmk$lv@Vy)<5Zi%eHM$06~b7oyZd@( zARk@40m5YUN(4rKFHC z;C7Ar@&Oa*G_&)KmI47>P^pzkWu>OcJObl~)VF&dDvN31Us30vhy1?;4Qya<-VL{9 z&4xGbcNe{lXh%J*{d*^T%A)nod7>+aRRR3Q_Emdt;vgALq~Wn8F2h@e7~Ahh+3)xm zQ2wbe$(+NR*>@3#)VYuT5c>)C$NwsN#_-mrwR+FQtng7l^cA|eDMv~rp8S~!{4Len zo@E1VQcuC0g?}E3`LhYMg{?rGl+hT%KU(U?n z@Ih`M{JdWs8v8=~4sI~uXB4f$P`)(}vTZ=fAjPz|cRZW3wsPX{#ph0lX{nkx=FvoW z)U*noQ|gI1m&6hvzILn)Tw?~9=1Jgir8gag(mjcIZQ9PP%fY%L+O4n_&g{pflj&f# zO6%LhI;T83X~is;<*Jj4Hxr~Tm{?a0$npJ)x^aW7Ho7WF6L4li+yVaedo~L~O;9io z45(cxo*RmrHd0kAh;Jdhz3p*aTYCQ?}jHxtnYyeU%?6EDo;yC@tS4@0lLo0I>i)e`Ufa(7*r%|I$-w1>B3(5 zn0@5qqApk+inzF}C@Wb`MoX}Yy>xIkT$IdIXlP(qYLN~ z0DB86AfgLG{CQjWL#D5CK>us=T7>OxKUPokMeYl#;mQU^^vywNSU?D2@KJWy1-%;3 zFn$#Ra+6$9d+W=J+5VXg-bDX?-?m)v93eb{!b1CO&bbeNAI{cSCuJ=;QLq5(tC-KEJzFW_F;E{M~r-Y5+_E za?-Ni2J|9lrt!qH^yF|5XcGFfm zw~&fv7yRh{3ELdO?H_B-J$tH5HJYf7Ba)vpABTvKj5Gf5izv}eVj+KTskB5AX-qLo zC&63sM#*I|Yf(I-b@j9AC=af;3EIfZb2{}wcDsWQ<9wc#YI5%G#c*7z&zh{9oR8Vr z`7R#H=iAp4vGOs|SnM$KS=n-KVTpGTw$fplp9P*cwSDsU{JgUvEt4uG*(O&ng`L38 zWpZzC4-(A=>jjLLz7kd6zy9kM^jWi~O11&S@`5uhv+{J~?L4dBmxloN>at2bzvJn# z9x?aXS75r{+u-3VAGIK@1Yai(Q{EWL&D>{M{G$2wo}TL}DfI}QM}asO$idFZC-?5Z zr|k6;L>fxYh-octZXT6R6reAF^nXRl0Z=wVam*{H2+u^WTWO7hZpM{{@FV}Z2^H0E zb?fgU2Hc<@f1c`w*L3a>8D6CmulNYOdBhW$l(~AaTiCOKMdtV@c$Eg2Brl)Gcb}zA z+eat#ziT;sy>cS^V0-DSxbTPKunS<GIMUMPMGrl$I{~F2 zbVt$|X^D)jcP1=nU3*_?&g!wMG-k3LA&<*4=>>nEVR5wN$;Vu|M`8c+d zxx-X*NT_X=V*9fOUkfe4W^&E`7VchME2^^KOXlVwaB9nHMI(n-Y;q5Zh&xA9ghlLa z*)-#(_F|s}%iTWB?KjbLL4PYwI3wRDz|}J+=onKa+Qh4&f}FH;WOvX^R2c5U60M){ zbnX!A5Z)LJ$*?MF01Z}~evBDg<%ngr`bm0$ zsn&<~9P;+NeNmof!>-PGV3yH6Z|Vu?Q*0scqc5y?f49xAN6q6iLE9~L3u5A0SMC8h zyIt|8WC0 zU0cuEbZ($+fL_J1?N{(Mm_Ica{t*&ycnd&aTowzp`&yfPfCo<+UAmnB65U>s}=y9V~UK>N-4`PQ)HM)1ku+LJUxPYv{m4MP=BwpGCE&*@CI z)zX%xm9$Jc85I;@dOU)R&nynTK6bm^b?XHn_#iZF7B+biyt2UQLoaD!LkE9Qsx}F+ z5##lVDGypl_BE;65TOO?YYkVwic`rA$lm?I2my!n34kJnT>gCy@AI-)e=uwYmx~^D zhI2LqOmhTt?|1W7e8+xV#MC}01tN#npPs5Z*2Cjf>PAQX*!aOM(^GM&tSK3|sU<)8 zk%07GcLB22DeDSZ&G8I>fLghEk{1*Nin%sEcjkk$v2%`eA!}FmR!MP(7w3GCV{|!$2bw zjs)))MkT#Ygk+mO!Oq&PPx-RN3>I%z$G*v(Q*UffuoOZWGxT2X68<}u zeW|0Q!iWDgOHy?I)3mViLwE^(8)Iah)0>VsH5?g(^fFRwgM+J-c*G{c-4|sH%9Wv5 z&lkY1e+43}#7*uw&%z+42+c1U3mygv(e zZH6MfknCi_{#d4MG%!cCG(f5>L2schi=TK!4klgU%vH@F3bNHaPdmQBGAEv#&hF4W zPiplS)qpscBC^)*dgM?HewBAMC- zLX^DL$K^ZQ(Vt!^D4ore`?fu}zrf0LYaHicf5XA9IDB1tcuKfnBgB56&d96G_#1iC zTY;HR_b_W!b1#p!7-DgcB|ZX^dN|6r%XFn9$%9ZLoR;q$c6jE=%Ml9?sv336Q&3f9 zCp9oZbj}Y|YeS5u;It^80DG1<9G3KGe+F*!+RZ7$r!4Ql9<$U|HZ++Mb%z}s{qskq_^Amo?4rm3TnF{G6OoiA_Jn>peQxFqN@8 z?0BADf-$-Jg+2!rw6up;Hb%h*512A@=WDFq-CkdlwWd@$WVPUgO7s|yS8CmMzIeRT z6vZ4nBrQ4Twdiw>OH;ntZv8#=2jecz_?I@+4%5M*&aZ+Pr&PrELjzkAPtu{9U&Gr# z?ALO78H0TuN*E63!D}vN?@;~ZJYdQ@ou&lZwRyehb8xl(L?=fCzL|^Omt99irQe;8hSK zOsB98@U`vfXpPm;0hEPS8exIK$o~D06GSKOtb>1-v`vOLvxfSChFVrx#-iN`(CaLt z_az(HmBW0ufgz3XTAS52=u=Q~Ii)7F^u|5t1iz^`9=2QHX||ZQVQKg*0tZVA8I}U6 zaH0{UuoeU)24zFe?j7Q`T<7GSrKcUc=Lrl+P6pYgp2=;iaS;<1K%Xl*a}Mn_LC9K` zC~P9oV)uA~UV7-&Y?jSi$7CQO+44!POtgU!Mn3pF$BJnzy+21Y3|higW**ulgDp+| zy+T*qiAWjl12T($EG*!iQlukkfQS6wKV!-Ev)6EaqojA`auI#Oa&h`#&1XhNYNckY zuJUlJc-2 z#>v0H_SwYosgslKVHJ&p@iv!_8+(_B2XVj$@L#|7A_gNO)Y#bA*2+PFg=NljqHT

Q`Gc7D40A40SWBAzcDcW-QEmb-~KsrXLa>iOI~o7FND9Z7YFWOYM!sZc(9jIeR5Ob zfktR(AO*YB^ivgL^b}|1fBz3E=%iK)qpT>xPtzu`TLuc7DoZED z$RZHcIWJUC)(GK_^*(aq4m_ zB_hfwo^_Ck?@u;Qf-?m{=&1dYqFBi1aQlx3H({a7{^4s{Oo=oWP9A1nf;wP>DYAFtGqTxGha-aGpOHZgJCd6V0K*xbn`XFuuVt=jYwV9;YRhK;v|))PV86>LqPLEW;lKb9sq$IPC`aXXVG(Wuj3v6SQqp8(Y4`vPi4 zgGKTW>=15N<%RKxmI2uT5I7nzu|FtvTKRdCnPDfvi683g9O>ma4$3HO&1p`G*GR5~ zYED0i$jZzltI-vSE!=TVeJS9ftD^bXw$bWiyT4e~xe5+a<3sGkJ4?5y#^*8W@4YwT z%8cWu-3LfK%wjqkdi{n107iJtd9BDkxh9?osxCAqo1uqv3gO8mj=>pjasXwjf)Ni zlp!RCT#<(Lu89^)=`8xXnWen>Pw&|bT?DhlQF&T}l|NHaRTLa;v7{Vh97{JXt%dcQ zQ8AQ*tfSB49^wlHfrvhIP{%ecI2XF)pB7(ru`6Vog?!D|E1Cl`;A(S*;YoyigyREDL<&W5DvmBYQsc*5H$*4yIn zfg1!E_ET1+oyc!Ab3rTxCA~%%6Hm@9Qgy2je@_Fv(#sEU@jI3*X?x9m8M(IOzv|3Z zAUf{6<%==hSp_f6uo9|Tjf;zMQ}}__G@2V>S8|^N#s5yt4YZjhZ6!dtcCPnfJwC4-mLoteOjBPDcy#$_5pd+P)-wFOL&iso9^TT<+jZnYe-zyz}mvDumg;&)p zN+Y^ld=!9Jb_3q>*=HXu0VqgXmT6W10z>3VBFw?K^MVrM0EA@6rrJsJ1uihJP7g%Ea;4g0w z5;%eHZg!5T+8=%bR=u9(?X6QmYr@B_huf)^8)d{3y_@uJhYH=2Q zhVm}{Wt(3u?0pU%KI@Lpe3xGt5EzQUbnnT$;wl3@vqfG^#J+xQ-pqQ{pPxM%afWm* zE-J=Nv1*B(V-%;8_X&;~`+!&ww&>3Ek|_*ye(>4G>ic&nf9nJGgU@HfJCUr4%ug9j zzDN16crlF8JGq!it|FmG8tSwqcx#Iik9IqPz6?TK>H@mlO0wXp?sy_GA+FbLH-3^O2mQ%&@3N_ZrMsZhi zjkRH&{>{cMiBfh-i`Ejcj7~RsKNvi|xVXL^!?)w&?EHw}z$P9|j&ZA*AAOVSv>tQu z6*XqRvH?!b9+66YIAQdc3sNgIsG@<_nnwzVAg*<1B>FQGa;s+lVx|UzdV*DPPbu(e zGNexj?vCX&S3nGfeki;Cl*k6wkKaPGKv$wv9eD0iY5HC*`e^Ar7!JRrP0j1iX}Cvx zfsnfw&wLh=S{!#7T3`pnLx()Sk%(1`9^Y(}bLKDg?m)friK!zv(s~PZTkn4k9pdAV zYUHh2fSiYy9Bh9IUt5)(i~C07Sm{7kYf*5$F8tD73K@gFR zZiQldLb-k5YNWfou#pP^d|R`{N(UWRB+b^ov7}Er-fS%V$xBf>_eI~S7Mc##uWedz z2@c2KF_|rA6s%6#2?9y(5TkD@tI<%JL<9Zmc>72{SV6hE<&HzqN@d^cMmC$W%9oIX zFRbgSlQh=TTv6{lnm)0WgTa_OyMg=Me*qnYr!(q7*BkYzh}tbQ_AF>sOzLMUIq}XD zy5NaXe`_)hlT_bOV58XNqx}~8R1(V@jF0<}DrVE#`^aD@_c>xHZfYmeDm~Xak+W(w z1m`opEwB6nSPkAZ+qhV}R^Ky8@vKIJ*s_32_`T9U#Hq-`37SrG&sme!kE^$v{dy|Z z#15>FKgDeS+Zko684}qy1HQXeTcM2zO2mF>kGM+|#euhcRY8$_Sm?2J#M`K;}-KQ}!Ku)`1DVTHP>Z5vK>F>kj@j1v=KRJto=hGdVXoN3k3 zTb@UEnEPlD)|Bmrgj}6(@MGL84yMitOwPtG~N`^lo#3d2?`E)-rr<@gg#t9)^+rg6KaxA9m}%KuX7nGRtgO-!I8b zhL&-R_9bQc6ajRlDI6DvqpTkTeMMm`Z8yXbtvnhxIQv6uDDQ9e05CCMQc#nEyG;c_ z-?xRAYp6UaacKimQLYblovf2{eh=>xR^21}5N|kT>IgTavo1CkS3` z=Vn~w#)dl1y_{n#+WLOkTeBGTpozt~C*>_fn$bEjp2jGv9l`6~i}F!xKmPmo1H!q> z=?U$qE=AGjmSjZt^y+6+@`CIfyFXK_;_P4{TiK=x9FAVN2<(e|up&l2`DCEgq2324 zb?ePnKRN#_^RWoyj%~5B@mc_(&jTRAz zlJB3QndMuqvf~|XG~saM(a@4pzAtq5_!ux!`b$;a5-3ZM_}ZEe|uJT)p1;aPn_XSabprI%zwXj7o%3R(JY z|7$T!8<69!fytBkP-!i8R#mlUBtz$lkWpg-!X6}Uy{qG?WQxZx-1O-!6xyJ=(!M54 z#z~K5{_c3pa`w?Q{JhSo4w2?5fyBYVb58xJ=~%h9(y>P8Xd_`p&=#YvSs_RK_x$fd z%4jCIq+HQS9UOk43pKGr@9p=#l%}V$_BYD%F)Q!_CIENI>wk;pC)SQV+VG{72A4%~ zfY9;$gA5;NZr~gLwIr09>b4tjTf1@VLkwR$<831xyB|$7VmnnjSSudn`pBDJ@sTH_ zghoYBcIM>xFU2|DHu28`trPvtx~~!-LL7^h_I77ANjUfw!}a^4nFoLtIy2@5n;aIt z{WB%R9vyahcGu>h;w$!s@fd>}lDDu&&=1g+VK<9mF=p35aV|ebM0?@%PaS6&Xwid# z1TPrtHQ}HEORQ!P0gBe^UNZNJ==h+R0#z^066KBh?UhE3`?0 zj!Ll@tEj6j`gqKKbQ$h#-c7?HMFH6!Iplf$#kr+t7dji95{Q9|fVQA1aDUY`pd|7bPx- ze|^K5PDHA5@+xbBH~Z)aGCRLLvYEOIf5rwRYLl`CC!y|=(EHulVhq(fdb zDy4Pjp-7a^lQ}0iJUlEJI^B_(9793;aBY3}frSJG1WUAeRIUUG`2BL3+$~i>snq?N z``Vmz$@<+!NkUzq*FMO?{Cgs z9vvO+{CZYw(X3GC*MDFuVZofY6#jZe@OHBPT9Lg4mw>iwx#?u8Ek8{{Eu!rNwPno!SmX(F2VG%QR zvashV2(wBq`*OzIVtTB^IP9~g_$+1z_YVt8O=2P`v@Yw~HSPQ^Qi*A;9GY*#B}>3B z%8+cD2qkg{x-R?&8u(1^!|v&uqok?d0$YToKt;pPRTTxBYo!RCbY&r8Y|6n`0lYMM zWF2BXs6c(y$$FdL?N#!slZ3g``A)3}zUM>-);bZEXbE_R)d(3+{$7)5QXXY3ztvNh z94$&TbQ;>{0vA-<+Sv~OHHN==5uiCp9G*a#{*x@{0ryYYmxvV~<{y<_f`)O<0+pEA zo=R7khT2cx9(I!DC%@^tUpM}$$jv3^DbZ}J5w3D5sM3Nv)bA8F#@LByEr@BkZOBt> z<1jK63~Q}+=2$IAK%a{2R_mZi@FzGr`0p`WQmTkYtUBvrPmJ#NDKY7vkoY zA+XqAgIO^?ofX35ozM+LG*c0!@H!R>II*jg+&qF+_e@th0N+cO)!5Da4v+N&SA$(Z zJxZD3iP=oD*y-#*(043|VQPdb{I5m>Pp6*77^JtB)#V6L+$=XOKP#)6*X?MAo zu||oUW>{BK!|s0J?810?*N6m0Q3}b~1xJ5);C4QzOvM z6}JAQrHRz1eVS$}G4PlCYUFJ|T9&&@od=!h=^U5M5Nk#A3YmL1t zmPI5G9gKIB*>ohlPs;wYpkQa%mY9<3hw^%Fi0GdBlhA46B$|uUwH}zp5$?c&{P)84 zX_TbA%SAJ!l6YT2l^ynmfm^YNlU_gTX*(tedoBA?4YakVE+$tzCrz(^vCz4EfQv^Q z)V8Mkr z(Y}$Q_@q?x2t?kCKK>UI6^HpgX8Le-9HOF%At>O>TCT%_Fxdl{-6WLP3F3uZK0S@;kM+DF7=Vq6x7r3HJ7PI=_Ri^j1$$vCq|o&*)t1+ z-XuI=r<&H71!`-8123;1BXXRq4If&!p?(im;Kh=$XM#~HnG!+>mk}f#KM$HY1sjl8 z74T=>&+P8{dSn_wIBgz*@|?0*T0aVHFal2+oi03&kjCeNs3p*(dvF4(%lPD$>xlD2 z^}F~i(<1ZN$x!&WK>@3xU2r6~19My7u9L7PTy3O98gH<}#%`JI-Y3wqds*EREHR;2 z$HT<04on4SS%h%sL};tDx4^THTp^i{2)p^|?3WwkeFnI=corRpnH zw(MVXQeQogO zUExtfS$5o%rHfb32Fm8&5V{~DAAInziipOUO&&U<(KKC;!3DG<$x;j z{54B_b)qbtjepXd)bp^}>?sVU){CWUx+zL&Zo*mj_QyW?kjXj^_()<_T25B>XJ&pS zkfmk!f+`*&#Qqfy{~)H-GxD#RV`wGTd;}1T05czAcPTI@!nePixx*V}x#XB8w9)~G z(}g7n=QjTD1_ zXhG><%zrs{zRXB<`f020dcc(I^ow@sKTpBkRVI)~k187uu(XZ(uilgM`23DhXCT~s zsw3V|^>KXlJ3ql_N26C%4yzsUEJ`)cVy)J`LO*TD9Yhx&V45;N`82UB#SP?p4GSYk z$3L5`Zf!CkmGpBjfDK8Np82AWpiOY-UN^-OoylQ7%fh^A$ELYnZUOam6L7~c4Xstv z(ACA7UY#Rls@|j&+}Eikc~z}ym3Tz&(F#R=6K5H)5k$SZ;-O~UP-0h?j-$-0^m+1# z4VihLnA#)|wdWnFAQ*^)z7Y}9*;Qgh`TrPYTQw>mi8hYhJw|38PY;3Rla)~rbcms!BVSGUy328Wj2Y-ms0rT z>9tPP$a){O4|TQe4K60u>K}d<huLRj&4%S*O$fHhpq^eVr^x zG=ewhn#YBeyDEttWDo>vnQ_eCtHot~T+IlFOJj%LsY*xYe>2Xuy1qac_nnw6%Ahd_ ztEAs_HonkVcKr$(G@Gs+0Dmgn6r>nRbf_cSRnaGpoX35DDRTXuHQDs4&g6lVuP18L zuz4Ca_-rk<6oj>R2M7b*#onI(=P(I~l3$nvWo4~=pAT zzDNgco9=40wp{gXMLnF@mWrGmn_idJao2$86yUrp3GkhT!_}oPHnVMvwl8-NRzZ@)s@}w- zY=(x+_5VlHS%x+FzkOI5=@LdrOhE<+j7AZOfj9wa>6(-unyVdJg2U6PknOvE}ttQQ^C7}%Hv?Y1qr7-<+A{u7WNTTgD8_psK?)x|D zXx3+mRw!T`O#{9aAm5i2aw~?(hZKWWfeoFku=RlIyB>Jv*vTOsM~>Im4@ZCs>W6m( zxjx~ERb>GZQi?Jwot#10lMVBBduxWknEQ@IpQ^K#xqDdjO zdDf&`6u+8^31=l>vMhFx_21bhsU%Sh%kdri)WomZInv6WWOf;|&)mr>YCyibIP61< zSC1i;wgU&6D+L-xjxUUX>PvFcis5>>2J|!3E&kIHl=sYo6}XaYP~{TDLEb?yJ1xle z!^q;#P*44JQyq1u3hzZDGO{2kIh3;I52I%-1I~03B4C^-doLY_j37IWzccmxg6}iO z=wI%>x9F)=7MV{%29*nuk2gG3uI`6Eo}{%c@N{Kl`Hl&saB=u11ZO^@fR#8q-6 z9d}}b!wRPg%+qYi+yx-zgz|*_X+(=vi3jfj57OoJ&B1{d+-QTu+~9=ae_Q}Ewr$OQ zx!X@nIYni{{VhK*1l$R&%B1*vg>|%l@Dx-_&@^B6+A*n=5ZC8{|dj@uz1(N?lmzJjJ zxWb3=PAKR!o%Xv&@jWu}y6`RFl>#zt9E#kZr>R$)#OJ}gzZ_6*g42!LOrb*}xEqT~ zS^dQq`Uup~&H8zI{MPJfV~|k_kZ@Jzp6u9hJ~c6u4qlmS*u8Mh0HSdjrOXm~@Uv*E z-mn+cNh5$expKM{{gF?y^?+q>hL?q;NwC%Z#grHqSila|D;j1(qBu(sMTtvI~~ zyN`|@1vH!D+$b$vTVMZh+Uq5}Gtp9smze%kn(3F$qAi);6=9fyp}CWiRebTYuo#Mp z8Jx*|{8sR_9_IZI+^I!sQJ%(RC7lB6o9tuZ@|_I*u+zonnVDB@#^YJRtq;blDmJry z)nNK&&mpo8^e_-W@LVtI=z%j**c1&{TIl8R*^xRkr6|Ai-95JV5!vlCQJUh5?@o-> z%gXpVLeUp@6e0S>it(Jedn9@?w zD!ZX0Ib>0Vjctc1CwyS2rmLh-Eu`GeXy$U4lyzJ}bNYvG=)1>+-AIVouA7}<>u5`# zbfT6dO%K0LmM4#1sj<7JtiHY|uG|4RSBd6O0!7k`e4joRr7@ce(W?2t*=8|Qzcb5J zaFnbTa)vNx-Yoki?!_PQ8^p%p0bB^5<$Y_hRmdnn*^Q|280J&H(KAER6KfQx4 z{%)<7h>x;i87LEidYCsYH@oXRqE$v%>*YT~TN{}&vkMUEh|!wIZmeR9&cjN!b6!Br z>1|`q4-HTnY$}{i2So=3-EaECVhW4+ejt+&KEN>{q3L9Vp@Y;8$d(MYV6!Mk;XU(b zs(RVVmT$X<<%wmFA86O7l$9FX(N>}efIOnoovT|`1H$?_ja$Laa-&o-MsJO#t+IlT zKH(33sk&>r0gk2*e@p$^%`57Gz*uEwWE;x|)4`n~!8aY3f#1bHTF;#Bw9wH57QtSd zbm7#v`jY^8C_6aN*SBQnP1BP2?b>7g2+fPP+X*mAWi};#*_Zfl^$)87GO@|bBfFPH zr!zbC`@sG)gDa97CUKw#N9YN1RSyZx z0Y@%}obD%>WF^yKfLE!~+4HZ|?_6)IPYS1&?#p9Ac5%rID_`n`5oWC7~cq60y3msb=a^7naFKflg%gbr5)(MwQP*?d~ zo=NovYy;$SEU29_Pwu()YowBQerXDF^RX+16~^eil;*fb=-{PIkE{*9U?}aBmo5{1 z{0>+EvQgmNQ+*sD|8ZMvgbcc|kSYD|YV4notRSCgTEzAFePcNN;+jQq`f?^|1as3? z^iu1BGZfe8veecF)JPdiXT?N@LYvg~7w~v|VADO}vfbIreeOWrsg4~)?ESjU!+`W7 zh6$IOKhpnz37P?rEzra;TW!Hj85IqJ2T_QW1OcrRx7+l+o*@;6AMP$hYwNdps|>CK zPJ=0Z1@8W>w|>4s4Z}0OfZNfsIvE{$>Z#w}(Z)hwTo@XsPZv{H3K0fD}WG#`w}o%wtajYy96o~Ku%15L8;CdI`-R-{xI@? zB-uDet)v5jcm+NvZC*=yURTWH^PmWweX3vmd%i}f^urIJaN(VIzhrwi=xOfgs%SY$ z?;JA%I-)!0MHdf3=t1K8TQ2BQn!fxT5F^_M%J1%Pf_?Hwl+~S=j-k(%lNV2&b3U;l z5UlJ;(Xh1}cWK=Gp0u6xk=^-Vlo0Oh{D?83ZEhFm6p~~82!H%!H$fB!a_a2Z|0LA? znz-C!h@)+1Ryg(Q>JP;W5<^n!dD9c4tCq^6tKNO0*|CoOuBLZEekVLKcDg!?A3%3R zQ`rlFucD*P|J0e|0ZbfG5A+0`(^FeQ3QE_|DZUP^%qgtgg;8|CAi9F?2mXngMvPf^T{0B7V&GR2!3oT5-s`}&mKl46QUDU!M zk!p?A!fPxa3D)>Z-JAB3co-l>y8qr?|2YCy>KC}`_t37*j(KL4tRq8^|B!vTWYola zXuq(i@mG*n!im5*>Ap&gQ6azTz@oq7!qHJ&Y>wswt}iviEn@}oRyx|BI(8I!Du8J~ z!-&~xhdD2^0*{jmKLAlT-2!ZMe=N+T)8c$LcUqJR*4gZ`cV|akc8z{8Myi$GCGzX0}y6k}G0LZ~TA< zg2%CCwNgdjBu3<3=KDR$jcK_3M<&5+20(f{{s0N6WHfZ=g+)iM(3sKZfSR<1agU`n zYHdbYk?d7RVjA}b69Dn^>7_K+e|Q`N{a)M}7%$V|8*op@9sElr`u->YRe$XGLfF-Q zQ5A|)_!d!oC)z|slK9`=f{Fe1TtjV2_Z5+oR;(zyhxkDeZ)zY3P#96F|89c=_FJxpJJ#i$!3RwMqikg_4${0hQ*UT==QTx_k}-)?Cb1-^mO?UJY6`j$qvGWVCU zM5APLPIAw`F}CGWu`38Pk0@hg4z0h4bH z9*e%VW$;noeWH!F*2jevrzyY31az-|va4B8%h8@sxZP*ao~tWmXcKH!gQ#ge`xxBI z_3G>ZMEBP%ic?bf_49*xrHorgSZ==jECv*zCuOXz9HY=N;*w5@?nD|8E~#{jmb|$W zfV%B5lT>?jlbOgB!w)2Uk0f5c$9AjVDx%{IQlE&Rcy-0+W~=KiTepE5ZxIIf8tcx) z7q6jHZGyA z|MVUxU(aJN9?(1qoRm=_bcbBerm)6h-O`kkG(3KPd zLwmtCK;_U>_}q2+tNMLR_?nHrjqQ8v^V>J{!GnDc#2=+`-z9ceR{`X;*>uYf+h~D{ zvkl+{89Op{=TUA1__M758Qg1kbg6IfdzQ_CZAQy&{adT-O+uRQ(KX&3Oaw;Su$Ibp zbBl?h(BY_Fp8jPqtlCyfjv8V%yfQT^PJlAbQp#_mX66SvqAh(EpCk?2;jLm}UT(<1LEB@{(OJv0G; zzuTf?jGyjF@vYmVSR-4Zwr0${B|9ypf{xIE5$7Qox!Fug)@N*yo9XHxt}tubCT2$< zTCJb8SSps5-fi^prn}~UTh))sR7>08@J2(Ui2R*w%xtq<%*~??LRL^X_ zdkjKlw!^)tHeTKS`?qcTp|QnKY@t&>U5#Dm*FdN^VqzL06tS8*ZH!xr?cQHZXPGR`B10y$ZnJWQ-n z^f@veLFH+74^4w%;1`7AY~1j2kP77k7)I-d?sOh(SXKkSHc$A5j^4$TPXHqWOs!lV z`vE%4UJ0t68lmW8V5;hlz_#fAlZ}471oeNmq~q(@=Mrl&!(YDggquz$3%6!JV62Wm z&P$vCEqsfI@};8i9M~7enPz5WS3;=(TGu+7ME;-bCc3QU%L6RB29!ZrfHaSnO*3lJ zD%lx)49q`Xc(u8Y>YLSTJl4=1Fmf1Cmt`?^lC{I=Y09Re4p%j?ebE7@B1fb9q)cWr zjGyb7U&my`w>;KHHdCeh_Xk5Ci@s?dmNC8{H_u+S_W)!wsv zal`)q3JiEQ<;KJ%z{~9F__yumO91D_q2=EJ&sq&mh@t1QYtLoitsgM>1SiUBRE&!D>_q?%iaFS(M9@zk_| z?_@YE+4YPB;n5VF{Ne@1me|W{=L)ycFh4ceP@B+Dl8(`aES5>xbupx6wJj4D6H^Bj z0!HKJ#WZS93G(%S)&~n?x@M?Qfx&s;Nwt1iz+v z3^WIC#{BDGak0$!dD6#)L8Fr@_jyXfEr^pDtixF!e43kUWcG`5V=F6tGk4Iz*H9h!jJMg`2>$Bk#JOSwqd;?Y zc1FH+OOv@>zEdqI(B&y1(HZXwXKDQB2Ew?1Qj4>d{yqT#fq~tUq%H*XvYd(U`}VkP zsH`$Jv6YHH|1K^$H+Jl}G|tUFGtiH{eVd~UFBX}@!>=t$-Sv%12VW-@;a@1Qx>p2a zmtsxq3!B{D-HkBXZqbzeJl>|RmH;qnI4UC2L@CRCvpjx%(TxRe8pZ<5(we;VtdRsS zc+TFxz;U1Xw_3nQStGs#HWiG-@(5J8!^_ZOEKZ3wMl@0K#Fl2~t}<`U-m#PFQ&2h< zta9ka0x>(lp?6dxXDZydbNugL?!Tk&US@M0ftLcvWPZUEbiU}Y23}4Cq~LUL#Af(R z=xjKa0jz2NE?uQYw1OR-4rfmJLggqsmxfM~dKAuIZv$x~tok}fG_HyWxTBkI%6+L( zGQPc1-SDH0#=<=o>f8|t;d`c8qK~_u87~U>44>-vl}=C_U{5<7GTWQTo)|@~f(j=ZXSo}SjobidV+3BlIOIObno5P=Dn=kNEt4BUE9OL=pQXD;k zm@J(|#b=uw)?JnrB<&#{l{=63dt_*;qeD-aR5h#eAJJz(JlXBdUjj`~)3E&f#x;!cOHk-Wq3 z+kj^T*~8!Pww{S{E65l*es4>cVF-^_KI4N<`$@exMnJvy{;E8FcxT7!C;p!>Pmeaw6Jn}feh z)=f+Mm^Fa(9N6h)mifwB9iaEqy%_jH6~i;!P~@Nk*Eo7D4da-50~B~OQgljG6h)$W zZA3TeI)unv~(<;&eN=di52gofI}w?c_xJ%?`HK&V?gj&IB4-#%83J z0TI-?(vs?b4(KJ2VGA)+2_z^$_Y^Ard>OAa&mGMRI;(@5o4P} zGE^bA#K|hC*^N?u_>1HpG}J=@*H?||w=Qxrur9h9oMbJRLjz9IblJm)nOXg_P4&N6K}Q%^3uF;zz3f@LE3XAYsZ<429AftmyB&U1x^{)|6a zreoP?DOp-tTzQM*IA&{mzA9tn_(AfPU}RF94nwp8ri%eUi{nH!YIRylnYKl*zB0WE zVDeb=%~)wkqZ;^sD^M8u>d4UrcC zRhV`Hkp-E0x(8IXs>g993D~mEDim+sAvtni^m@`W-zD4FwEo96}LU^$!I0wSG>6Z=ceLT^v(mV z&H2mSSY=j`E_YeGHG;h!HaV*3{*wp>MCb%WQ@@L}%N-7ZksERD7z?KYqC1^IFx(MP z{lX!AtMooq^~|5%@w4fSLMzRT=e&#s_n&-!v8+9iLZkFmHPgmz_SaZqF7qZZdi0Zk zN00|BcgFP5aO%`dQ#SloTX`WNr^I%mPHaUf{ycoCtg^JX8A#`@gsvbhiCg}@Ny{f* z-f?DS)P*$(wn)#L4AuM%^lx1_35Ms<1NRRMl0BH9JL+`jPX0n^X&bE!{_vcrY1SJu zs%xn&b&NIy9?(tr6vb3GP-NLfE!6Ey&Yg`Y%=~Z za=_5fpuMQ}a5+g#e6@4ZfS-whz9L@g5fboxzMC`<7Ntxkoa$8iL)S$Vte|61@R(U! zz$L;GFQqa!AxlqvAws{;=OJk3DOh-L6SAV1(a`hHlK9Ja@+2?1#nY~r{b`nR@@E7v z9T_g3*Pr;i+IIrE^V4om@}pV~w3q1!o|AiWa@ILneF`QXueAJLHhEYr& zyNDx|pE|@CF;{uLD7LNyL*!VKd+Oe^nvx}}RV^=j(o&0_lz8|@AVy7)nh#nVaD8#7 zo!Ig+OT2dY$SJ(1 z>G?!vDctpppuoEGY+10xI(C2;^Qx?yE=>F9yfq3rN4mqsB_BQWS2^r-m2&Ci?W5Er ztL)1HUdGzIJ!u?P2}VqF$K2#YOLtJFi9mwl{HLQDVlFzrgmXau;i;RrdUo~a&(`WQ zJp_CUs4O;~HfH@f^tn1Gk@S1=zbwFJF0c*fTI4*H3mR7ijAFMX=Or{|gWK-o<$!#^ z&H3C@?N2`?Rv)r+l(9v8xk!=PgK)#aI^h3i?A_r!k=V+5so=3FE3Mcf2b%7%kMZ$U zOcdiyfzCy~IUWPFEXrTU#>Qln0jKm1{_qTtTDF^i?t3xN{Mt&YL2TBvsuAm8^!*`? zAr<{6@*laBE(s}?-M3@sDtN|T0H@c2cqwvl3?$$5Q2rY`^c5u8BZIG~-aG{c14$4_ zkC?P1<+gk59q-;jV7NGFK{>(I%ZeIR$$PZdJ42M>Po6x{z<+DVW{}j-MGDS3&18G% zQPj$8c9qj%bVh}!=Glk?x#TE5t;pLK%wb1l(|ZU|^!r)Sz+eY(w zyStulG=C}guWQQRi;x05@?{xSB7!cSaQ-hmuv>b8OSzhWyx|j}GnjlUaHgBLb)VMW zFSt^N4IxsRYip6ukh5L0b3)JJw`EIa>WjBtRQ-TUksz8-cV!g*cEGtAAKBr4g*Z~ zcnKj>cdSys%+##47@_!Vsri(wBihI;RkA?v(=!6f*SbC3K00ya{6aYH=2*AT)Fk?j zQUFUiH~6z+?==hHjcBRN%z6R;ars_H>F50FD!_6)1h8ccKmvNQzM^`5acQoFHR@6Z zRS!ioD$l1*3fGd$kEfjd4%C8gq7{pHZ6kcBQj|D!pJT$)bc7+B@BWGWBXb|t75-WX zD;;znwFtV5cbDh9$P$;5V@V@*v^D=><$L`AAFC0%7(S~RH#r??ilqx20NPN~kE+sm zox-MHp$?x3Q3rEW;S^AwaWy!5BJB@Q(klQIS*-Da&vF(*cLp?fNIrN(a?`$pn+UCV z3A5yTeuJ-Wc)@cKYkT_?N~zl{@cAmb0n= z=TD|&=e>%y6ZGVp6Qh2El=MwsTkER@tTWbOR92hktqr`yA*0)13Aq8(9&4JC#Kjwt zul_vRC(?(iCNjF@Y`SrL5Ls^!yQ}u_=AMilyu`*F>+r>^C(YUpPRzAogQM(!Hj38O zgg~{`-E!M}6M+ zN4(X4Wxv;T=9EF&wJnnX`wGM52~3SsfAJqRkivj;h4i?uLRT}VA4LY6`O0w^HSPP? zuG5!41Y3Q(dGb6Yo$^k#;`ouN3ouz3d6{=?ob0!$Opce+<^j_8=8%X6Vw=FZdYQ#d z-^v}*kSLzW(o$e`>x$PlPW5a^i`nXlqMe>CElnswES>Uq(+1K$Qk6aiLZ2-xdEV{dF^}q@uf*x=r*dV5on$0V-{wPTdz#rbj(gau zju3NxS)?KHse?$)@9%sbCYqfQq$dJU8*aS*pwGflH1RKz5gVjFEu$=In5F z>um;hYv$cei>_!r6p61?uf3*E(Ju?DfMdEcnY|n|^r=s+%xx{-#8IX?8M zn{GhHwfP6UM4%>Tjs0afXB(#A5DZGHjO^oNHAejGsf?%~NuS%_9`$)HnltBV;{uJD zQw+LiWxSz+L0=m96R^0TX4CTe-9`*deOjTwNB8x z{-XO}TR22^AI-q=scvf<2=s2#5f1maR1z7Gf6`o>e5a!h-Z@X?4Hvnw8m(oMwf;*n z%YR1^Y^bub^9e+6=`+6F9up-4^=e~)l|;iD_K zqTirKZc4#@cUSQ(SiQ+U+8PCiy$kSzL+0WhQq{SSAo8XhME0L7jSfLDw;q`z&)c5{ zosAz~uLsaT*Xq(Q`6J@aedNxT)@JC4 zHo&YMtrPr~0I35FLq&O#Fn!B(D6yjkRojEOa8NWQRu7aa&Mt?jjp$wd`eBM65~l_q z^iQ;>QoHLu3Up{Cc||AYb^(@(Y$`BT*g~FZAM-vDV}%(~kah_3%~R`fh+eq~O8m)$ zZyR(GL_qVpe`udrK7XjXbKC;|01v5{YLn4+f0Rud6~qDTz#G0DSz{2a0|-?-mMh=h z-nNld)&iGJO(W#Bsf+&dEk{L1_`+YSFx1;pwg0iUFzbKc*i{ckMjIS~9$i-aU0Vpq0h^JUfkSo7_7W7fq zZ8SvMwmbs$+3~T75jqpcwjcPHJ}R%DqF4}vv+7!8)&?zGbBiVD>dx@b(p{1!-QqtQ z??kpfZy2h|yR0guP)n}&D!8E&ls#$ZZ(iDm>&qW*!Q4!4plB7zY z;HnDak`FL6SEMcMS7x^hV6hPM&&kalOZ8CuY_9*Em0EyQ zT@)P8E%p_dP`m<|jj2F&m!Ee!!;LM~rbK`pk+oT}b75l?e*X4R1Ze;Cap{^B2as%( zU2>kWN$uJVFX^3!4*QGV=z+h=S)FFW=gUEYEj?f`ZXu5N}4cSSgf(!zQ zB7l`(9du*!=(!T3+~}C4@{ESq!l-;8;g<0PZJ+9`c$gXJuY`<7xKmBVw3A;nFE~dj zzl0std>e_y0<{(%1ik@@r|Rh+cC1m}PXGe|Zhj=4`2GkXeV7eS~mo%V>{8YMf=j^1vmES-HQ zyr~|cR#^CTILYQ;>#7XKw1ABsQzxY{m#+_O61whbwAP8gt4;9|r#E<{?X%aZNSVeb}|l$HV#*kzxWvbhO;fV@v?Rc3zQo1dcmb8BgMP_WCX1#_R^ zjZl#~W;u^_?-c9sLi6Rzmp~D*Zr}^*Gk`cT|GfWLn*vPzdR`E-Y2&uIie<*+3y z#Fu#Z01o8JdvyE;r7c*m-kqBpt%QOUeIYJ$nEln?%;Z=xIXk$mEH^?)519=cWGeTB zJ^z%X3on`92lxW8-+^?t?E|ba#XFZwt1|H!5`RPUG8rc%G#X0ziTmwh$VYN$Z*n_t z09wDB#aBAO&n_{hPz5Wwyp|sU=74aKaM>A8$uYcyCYX>#ljn}LIz$jtZ_xaDfbgi> zA}EL^LDyQDjiWDQL!fj*;nF^d95SyR5s|@n!yiw`roM=L znDN6LOc-wq2QAViDW(~X;ZYuKTP-`s?T#7L?iM0{{{Q?g056xyUyC~Mel82CxVdQT z%+cHgx(hGqFqMDJC!&-^z@s9(siyg&eW2i@B2YR6=MLxwem}@h8@jwSnenpbEusPs zUCWm{0}~7hM0;`wi3x$*zh_V0$`cK$(9O?XngAC^47TKHP{75>F3>x8$ZLYQIqQ46 zW{QFWJ`YLN$I?&b%~Nb-Ia;tK_0zzwtWsJ^Vv6@48=5hEDEt-3&%4h?vL6_$FdCgs z1E^X9;q-N)PRZEHWOJk1Y8)>ZS;x@46or4TJht*AynWuYyxuP`kWt$cx!q#MQx@Nx z$lO*cP;7^qka<0HG6|p2C#fN$dps_);frHRPWKpuK68AiRIRQKv8$tmeUm^AmX!XM zNGQKws7E2G!LGyB_ruU%)}p2IU6AidnZ!t8+J0l^R0f=N&l~v{0LK>n+kbbJSUt>u zfH%@gF`nZw1o;afQ;T=-kb z^`n14069-MI$?9@$mL&Fw#&|7>wD{8nJr9kh=aDQ?SudZZO}%iG!z{zBEX+;n-eOJ zJ8{>Z{i*{)e_juetxD{Wf~>C|s(tl+VNf^aM^JsR74Um@)V~V7W?DCm`{heIq11M| zTD`Z;^pPQix=;S~YpZ?6EWwKovdYfPiZ`{f4^fh85AAF#>S#IBDHr_>1bWdg_Gq8Y zt_wZ_6 z!XFj12j}0Um;6;Ez83VR+e3%K1Yy)Lwsx znWST+;m!W$h(EExEmq8J@hFI)qqZ|!fi8fPF|H9skdxPvREt_z>}kxcp;wSe9snzL z{$YNN(9xmKLAPQyA7NYWc8d-`mKfHX^2%Y+hKqX#N{@KpCFDszBFPoe#_hK~RJG4$?v53eRb^RROKAspXyFVe-r7M54Xbw8_u=*Al%Tm&V8RJiMTIuozxi zx=|qfSGM?c9dfnZKWLQ4hrjWCcGw4|_wMK~{>WJ42bimi3sN7ak8?W>Nasd&C7iDC zu&Z?BGmi^%@;|fgn@ai%)jG4^nGig&JUou$#z&T_^I&!Pp@a31zLM9u#Z+ z!*34G$7pkVzKZ%$f7^*K2c`(Ce(+@v!QKoYLh8~`RCgck9w4?mGp2XOJ^!8z z4R6C>kKjLEVVG)1ZXY(54qEy&Eln&&`5IZrPG_`KB1R2%KuSD=vSu#Q91}-1bRQUu z+LYr9lXK=_Fu2}dO16ilWUCU`XbGP;v!q9WWz7aDOrj~n%VG`1rRX#`aL{Ydd69Qa z;(ptaTrg*ytV(2S7AdVEoMu&F5p+)af!nimV2OFNnMN}~a61R8;mXW}^u==5aF+#|hS=)kh;1W<&HhF@`XmM6Lpb?gU>InBgQGG>JW$;$AsOQkH-FmsUPvJ``;hgVF zl;a$8fVh+SzMPW+90Dm!P8rNUMCrnblpwFXM2!X0X40afDEax!<}~pA!~dncFZ%lW zUgpZPA`}#Iu#h6+EdUe}rJM`MIU<*LutEO*L~!p`jp&yGycSib9Z#uxg4-U?@}*51 zE^O2F%!pWouut{*WvHU=PKYZG-}nUG3bf+D0k^FsbLc*(h~BB%BIvx=KkDV0d$Y`D z9!FFt!Luajv_EZ!%ADs;S{Q#N)`oSN`7E?fRiPyQiv6;qXQs2gJ#>uhgiw4lslX(X z=QsH4ihMM~Z;3@EzEcE!9BC5e0Y#St$#a%yKk^l?Rx{z$##3p2&AJJDX~y|?DG^e0 zy@5HGH#Z4J+j3*c72If}lwMiUIrMB(Byj1g`9rS)HM)F$d8B<2|xTQ)co37;A@0Kc9-t+ z=KBI)1%L_qlFW@Qd=dV<9OcRL4au(56*d&H-4Gw4^U1W|K$6`AZflG=hw$<$eN`~M zMHV`jqf5il$bCy-nCEN8cLHVR30#nW5a~JX-bj^8)&BmzYE}A-U|()Czy>*z)950f z%3z~Ig-}^z9{BBho1OT51PYk5X-r`H_V&)DsLNCl3%Ibq(<7#+x)q{2o4$9sc*VPI z18+M#R-j&kBtdqUuYitC0}?2SJO6d#ASfq9!6H>KrDzY~bsFOaou(DE&XI&*1wAwv zO#AWv@VLid`X3oh%o<>w7ovQ`Yw5&K^DD}&dJ$!O*cwjf+$bpSa`K!tp@Jt8)T5a| z#xvzl>!v??=f?9gvX0(|K|2JgG}o})wX#XG>LbI+C>E6NN(A*l4SHojL++2^F$jAW~BvL;8){ItWwTgP48 z20b_%BgcfdV_1+Pr0c@ub2qYTh3Qb31p>&3-d@?WV`e_}oinnYkn861 zL}i&rsD-1S0IX0Xb>~`B;kB|>=BAvIO~jU0Yt9%;zStOdU_(z}aPu4xLc-&9M&$M;BK*7HACKR_W5_ z+CL?T3yDTa@;_H52VSj8N6>dG?Ct5hfh?=3*SQCM+_CQo3>}Ny1|cj05zTbk7c6U@ z?8jO>z(7UmHgM9!R%Uzfu1gw9R<*;IyqgN`8LPg6{v85jc;JV4Y(L0*dKREwDkMhT zR0|DNl6_J93kdP#vMwrnT)CuaR0S_v;j&g%zjbYI6qREdVRLmrceAuqNB!x)t0Pe= zUjESPy=Oq@ltRlTzq~~^u@wMZ z8$o0!NJr4Ml4I_{uuvM6)9*P+{s-$+RFq2u80Ib?EP_Y*LM6gP$mK|I5HbNg|j zl3_F0Pe$fJoV z3d9|Jb@B&Q*1VRi7KR}?Gl)(Hl2l2?n}mm<6?c(`5q+XU-qk>XNyz*+nxWG{-r7e3<55` z=<)$mn+n1{w{OOjlu$*F-K^RMU{nYX6mW1`ry>?X|E{IqYnHZDI$j>GKIor!AFZ|t zY3JA*1sOaisQGd@v21xKX&}pw6Z37S)C8^CLU0*C^=F zPv9?|0yp%>`xTNJc(l?|N6_uXxPCmIQHB+$pZB!;d_wE@3(h*U9;11pd*QU6y%DpasI_bG zR28fjl-{&|nPsj2>P+Q;CRs-VFQq}v&vsfCP5y#PQ82mju=q$A^LKrSMc@Zo2_l7D zod*jwqv=-PcvnZJGH4Diq>#MXR!75QFkf|;_m}n?JFj+KHlw0U2Y++K(J^i~t2>pS z@6K3cmQHqgj+?MpgA3|uIR}_Q&DO3KaK4`-tuJ6OhgFZD_9fw$T(vqtQqT+zaO^4t z(!_o~a%yE}Q`i0i^VG*M#Y=+KVM=9*wDS?%P4p>Phg)62(?kJGo392-Y7AnGe`wD! zKa4>0^TtL%c|~ANmX@8d$~;H|^s?k(l9xnr}nY$vE))#OQ(zs-f7*=7PqVB)>{~<{!;|+ zw@CqG@}UL3*(95&liy@6Rmt7%^10Anob5bo&@hM6E#6yXhocb>Gk<|z5Ym#oP?qBD zgfd6TbUd)PV`~Yan`AcT7lJ!t>yw;kr$J%ITnN*@*YX7J_=cZ!hZP>nKe!dCr=LR=}*ZdL_eVB5dgX$_DZgB z%M!~~hXMpDg4~$UN7l@TPw$8+uPei z-o@ySf{t!(78`c2<$*xLhN;1G_y9LE(a_i4LiC#qRY)&5ZdRn($?qqI`m@(Q_%V&dH>J7QkeH!QPsjH0{SEs@-~=5@c; z(?BlWFH7B&->f9M;rA2~7x>&8J-93f4Pd;?s?LM+^9DBRrXUseEP(UY^d6lyiKPKiO`twGTs>kP9E^B%AE;PpmT$D z;En_W?>nu728KL+tJ<(TU?EvB1Ey}0$H#tFN3*I*#PH*i|CZ8p`DXBR3vxDj@(pSN z*Ua1V2TxX)XOG+PrIGJ4*M-3b)&sPSNw}uVUK7>6fKaf2sgmnR8zPRy$8^=Tq}?GT z#^BBQwqd<~S+bOSC(CWa)88AA6IxJlrT)mvanC37#wQet({h^BiE(%DeP3E^P@|xj zM^DO)Efc*uq;YDdwGo$*V(62(zC4jiHW3lra zjzezTNor=3T6B4;CWl_bo?ukw==z+~n8krY8i_z{wT$}HzSJt9ru1HpidoD0fU1A> zQ3DSyw9D9JprEhUxCiIQs-ry!8LN%&6#;_=uiSo;X0A+!{36bzF6yHAVO+}s{qF7) zL#}eMII%I6V)axpFRc4>4W)L-?2Q(exUkCm@y7E|W9Y4J{@6FVjuqYnSW(1rG`Yha z1o1OsD$&jPb@%S?Bt&Qj;0__KDxU0{6XuzPyQJhiJ_Lo9*;xw5eKj9n_J26hN~(1#@THZp3qj3yV5pJOd;y+3xsZ*lWr7L0np-Z0aQN`NADM?`z0%U4+q=VX_~Z zn3=!_)-QtY@M@T?eu#-?7%+|Qk40vZ{!Y~q13fNIt@vyotQ_67S^jxhOYDW(qQM>h zz`QC41hT z6*%RR`gvx}0b6K=w(+OZgT+Q<9AnL_94_1Apt?ul*02wl&_9SPwLk^PAO&?I=G{6M zP8f9@j{p4mIiCSBe3fh$!mk0#Xh}i-%NLu5W6)T)@|}yJRFNs>k-YfyB35QHEjML< zU~QJun6)PkvmzNiqRL~D>nz z<_Km8;K04RY<%bc?%kMaQ)S8jvH&V|2AJ1Wai>?if$EAoqN-jTf=DV&Ua|Ro8T4$! z0EVBJC!DT8>XA<(t_lrQ&jWm$gpRE1(u;gKmSDHVY*12TOMqtc0yXWH!53XvuY;G^ z&=D|UMOer7Cu_kUZyrA9`+5-R+TJ1Ud3QB~fJVI4*T{Xai46L*dq1LAmdg9eOSD}Z zzKj`U7o@!}{o4_9-zNcTP~X1bd{FK>O1)YgTZR(Gpz!3iR%zMy!dQM@v^ZPG<`+7! zcToc?{}5zGgId{$zWbj-!0fp@00&bASO0j%7`l`i{eLu_c{G&&|NiaSV#qQi3@JmF zK^i47B#pHpF_tjNI#se!i~fbv>?*Zg|R=4QaGo2dirE$t|_P_g-G~Wc4K==OmB#-R|%#G&HoqaV7w1 zj4#|m#5||eZCi>>?p7werJ)^J`8htZFUhMXzm7Ps^Tl%f@x*0V#o}phAgz!sCpzio z1!lO!72oXYr#wH1wEryj&^NK-pz36C9lGdh+t6H4rbSayboFaHr497VRR}FdMt_YJ zexfZ2sJbh`p7fwxhLAoULIX&Iz|hUTE&r#(7-~e|(&@f%^bGdQces|RH9I)>g4L#z zRf$jr>JpxR1RZu&5@?wAZm>iSYXf)ZSED?T5^wbzfXS_Sd0HEyrao(s`K=5B|-uo$5U&eL=#UC&PqTjsr&|-gN-zjVC^DCv8^s z?O752LDR4R*R`yfs4~J?FW~@7$YPDt>$K)+$9dP&m9di2;diNLZvf|k8R ztpNqo2s_JslfJG3PtjsuM3;GVrJ2lXPro>EH9#eyVv4{PhJ6DTFLalYaUhzh!O3dY z$=y+)UMzK2I!&qfaBm}8fHzh6yYp)&1gGw)w%zP3v1Bq}40-WH>JA0l*tnU@S(|RK zs!Z-5?dgOS0GAAA6LiWf1fWwpEEy^PSuh~-RSAp>dM}r zC3b6lp(7}FNd!ZyK~MLW=|c?`0&Q_95$?FK2w9f#_lBy{}3Us&&VZ%Hg zD926`TmZ!WFrwZTTSTBwRL+8ikqcW->_R!IysGaGjwEBcE{bDnGXi_(r_g<5Qv4Rc zI$DEVtiY0l-?u{Ja$x zO+LtG_woyyyB4i;>>Dy@2$Y#=^shYT9yn7ayzx*{rctcB;U|??1o&Xgm_Ghp3c_=& zZ*B@?8`CM%Aqbhu9qy$oG{YPP_I2#B4#<_MhcDYL4)9U9+H*+0-?_z^d5#r0@^+0fr} zcFA!`GGONro&6iOZmbLm7835pZRrHBcliOzAhh)U@g~o2wQRb|7Eqw3Q|>nBny;UT zUI8y092~$?6%~pu|L{BL>C-yXPO|F+i`J>CD+@U`{=92*x(ed*>BGIzQH|8HVXZ9t zZ`@37<3V>Fp8n>%ZjJnawM$zK&fCT;E|g*Noqqjnp^B!Wy&im;W0Hz*17n7)IyfJ& zaZJM*Lv5O-XDrcO5jV1(t}=oa63=yZ-IdFG3sqN#mDYq_!+z5`)m}KuRVmK4;xf*^ z073vMA6_806LxQo*yvb=Tmi;ntGl+&snF(@7H6kZPlTnDfqcH+TClkfAQh)9ura3Z zHkJt=Y)_Mf_coni$(*zQ01dOox6}i$H<@cOvo%clT(kWzak7-%V-&3<11!&N`2NPq z{5g&ko&$wL#ZM^PJSEoMd7Px9&AgNZaKhy;sWq8X z$n22+sN@onx_Wyxwh{uTsrEE#V8*R*bylaUk!Q5M;UmIervk`?TvgTfEyvt&!mOue z2ZOdWz9a_>+)sn4n*r@5K^GZ`vR~1FUEc~B5)#7v{1j0s8f2J9ka?X0=t9`Zgsv`% z4Ycz>IwCLPK_>CNJagdFI;O>x$RzD6>68e!M0ILbTE+4t+%zXdqN~Psy^3wn5B_qX zy_ct2zOx+k+0!TYO0=fvJmReDwX9Dcga))2Y*m*(Yb~;|<{+%@-F>Ku1du_gX9#~5 zRAtO1E*hGA7C&n_f^4@J8DZ1CgsOH3_`T-^votTj`Gd;cJQJJgB zAh=8pM-z;kKb_*iYhxe!Pq-|byBCKSf*7YCzzi2)Pvy!fg?GkY3E1P9kGh@%xbYJJ z^9{`6Ilu&v1Q`nqkctns{9pLU`2wS2M|S)}u>hSNR(_ zcIrV21pu_ZYv6_;Zt*`VG(|1ZV87m0duR6T`JlbzJ>s2t+ivJYGW84joV8h-KImPn z(Z8TNtw?gma0@)V498Ybu;agNMAaDt+kmN&zKV=7!v*3~zq_B=+aJGMwrK4X3b^#D z!@W3rGrl?`M{d#^&zw9$W`0K{d|iHc^Lhr2lc%L>>;eN<4W!6?S7vw$o$H;KdAZii zLgQx!>2y0NyN30v^T?%;$l&0Pr(fcV8UX!CT$d zn!-05YeH))SCjnXSsvzII3Kwk!LN2yD&|HsEf!}mX~^Op^C*$2JjGZ0=iI{UwB9aZHO4~2A=}?R3wr!AtM+Q6&2?7lQx-@ z(-LtIq*LIMWXQ`40|?-gm+kcSg0*R29iSd^vG*Q9FQHw}%K`cOK(cIE=P7ga6jjM2 z@cTpVRLjJitPs8HIHcFP`Q%yKU#?<0wFDG5*VouLZw61js&pDN&`{xtC*|C`A7H{8 zdA#EGP%=U9?C_K8sV~9*jd_l~_sqBw8tQOm`k=1v-h|(GAHV{W+psyE0DgF*{UNi_ zo1~80>*77yRRL6 zAHBI=5%!iX@{+&%YF4)GQOn8E3A2Qn3J)p>3GGAV&1r$H07@}9Mwd+*h}h9UmS+EB zAm;@8l-+%z%WfX(P)lB(A)4Ra|S z?TEdN`jdD-AKcO|{^3a%;a6+twYBkDYpPm(Q@MJyz#78ys4q+^#@z0bnnEa(;8s)L z@UQPEf7lq>3+aeGB)mo;O<;SoO|12CP>$N#<);3nI5UH6(J}(JUKe)5BU-iLd77_s z@uQ8lSYz=|;$4&*+8W8U-1fO(_}{%zDp#jx3}=_Yd3iN;YwhoVU{S!z15|(|u)Xur4A7s)PIOySH`!xCg1W86i;%gBrqH zER=zr$fr7$`wAH8*B9|`{fE%Eq}ZIvR*B<^AlHta*=o-G{lQT))L^?m%!M`3ZBs0glZ3Cysfe8(-0ONZPza zmw;{f?tueT*b=BKyAg5hAz{b>d@eHH-UUp`Aqzx{gq0^TH#iq?d^ zo@3HaW5qM+sUNa%W$%NSL%&|!nMErxnXLe9O{D=2+iB_#^83)ZOHou`PX2Y7~(PF_GWeQl11cYYn70$l{Nd<#EaU80z` zd1hm(_D_=-o|mFYs0Ovmwk4u|>Z{Uz5X3N%$^R<6{t|=`PO)6JtkyPJy>Zz4{4TdS zpQ^`eUvsZkJB??w zjNt#l9?rNQz8Q!8wHeHI!U4MMkCHC}((GGFfgr~cdb7o4-!R#W9`CPyCII}o=Vg|3 zbRt^?g;R<_)B~C)>Sn0eF8KZ7*hq1yo1UKmlqb?5^QJ-wfRzVqEB|AQnMeH_-aiB~ z!_K8V$@Yv)P!qpS2a{d9(!i7!qv=V>Smi2gNdSQ)!xMC~e2oJJER%w(*hJSEyQ|w* zzo^@iF5>(u2?%7^bhBiiScMP@A2A;2P=J!LyQ=+Djr+gd=FCsvz<~^+mMi|N!^|nE zr4VZb3K`U>)A>5%A>NYa8xI?eK8b(io_n|JWk9pN3SMpG3E&BR14DBTu-`*wqFP6v zIx_A!Rwsnd!ruzYx~84mwEsc$V=oGzjLo;WQ~{R+& zljn(EA1mLz3!PjGvdQFYXF{(XCrAxq#{H~b@$LzR|8!9E4Xp@whZq@s?u3T7&=kbgwhoh_SN zTT4+N3p!UCj$9G>((01b^rzILfvTVL*oRqHxakp==liQcmU}mB(m)7Ls7Yd3+3=Hs zW(@IJPk@<5x)5uvI_bec+!*@I(eiJY|J>Lxnz6=VUv_2yiZS3ckRCxnFG?HVLEvmK zN=W4ar4&l=Mm7csMRLyYmLNZa@^&Nm?YmAzwe+jH;`}wUYQ(UEbCFUBPnj9y;7-;j1i}CV{n7BO#zi9=j)9=bP%z_Kfv8TNhtq;Jt_ee{L&@)6!_Yr zNx*#0Sq!(UPG{f#QREjyng<{B1%k84-ghVH!dn^Oh6oo;WIThR>9c{MQiQa+)f8A# zL4AL*$l1H@6P9M}@W*SDo&L)HW?8+iwwc5;YQef}BWU%3U}5P?pK8Fk4`5laM>bh1 zT=3KQfeh)#%l}ZBXf2Emhq01gkh~k}al2|Qw)aEyTPBk%1xRBg|C+eGkJdd}aR0Ia zENBZYFqsb*ErluGD1V1i_^)zi4@wI73^81f7!S%w>(uh1 zd*rXTN8cv~2v~kaXEmXDjmvAtpk=FT@fsLfz2Q$;225_&WI~~o_^8e+QQ)2=le*0) z6W-RA8$UTF^B-5TC|d!X0cX4FO=-ee7N&j8E*)V}do?ymwKnr_nQ8x4e25DHbRlp5 zUqn>TXDxH+>VZ$L8=X4g0Wuu7tsUzRPtvN}18?Nl;$M zcaY_phQ5*rT=E<%7Z1b=p-==gKY)pLfHr93^_i4rZ~XcB;Hf=9Rl~VeH*9ie1o<+K zP5;WoR(tFPQa5{52@;C?5k0u%nu#3Pa=&t66!W%}668~DYRT14Dj~K#KE;|Xm}@LT zq?exU`W&J{au+{1jub%|$%$bahGLTIwiHSDkHf<8QJ_Z8qoMdZehA(ySmxTzr1BJ}X_+ zNgnu&;6O#p=i>E{0!{RF0A=bL1U019JJ~~X0t|0KN;o0us5YEmvZ5BN{KuK~nB+<< zkQjyLz|4?Z=lG{jxqCwAXv-j(L&fFX_4kv(Jj$L44z_xP*rBUg-gz8Wf4bIrbeCzGK10V3-zS)CZhIRq?DWZ&4;-oeybH znuo+e<*8GGMXUk?={6FhXwCdkXk2SgPnd=<3w3S!8bkW@qY8(+8CrRzV(~@QR6XJk zS(Z0mWg)=w{eu_J(%R8uDt&+O-BQ5=WVMoJ6XDaz@X5cHT5RLYdO?1E zL$f~r>PvIzx3QfgGfo)quAhX!TutAW1hEVCd_-KoTZ=N5*%YZD$3Tyb zj86HCKbhL}H7NW99GR@$Y;l43qRdZP0X}+Qz-Wl|!XyIyi=B@n!xXiob`H?S8_sp~ z+imN0jcGNR;+XKrP%4mHLg8%^776}Htk=h_UNr{c`}~qee42%?Bqp0-t7s~w^nr80 zyo;68p?SLLqFGn$*c&hUkxvz21X!{eDZvZ|zNvoK!Vs&`Ndc+ql_eTzFOsJmC6oB4 za-A}kaP&J^`wr^G(ZnPbOU_IWpw6s4IjY32ZTe2E@1|&WEo`nlF2OZ7Y1aYii}j%E zLf<21xM*S|R5wsUu;VZT#?*Q=W(G*=>q;jGHLYKDcWA66;pCwKKvaXKf#jwI_v{D5 zHODep(BMVy#mTD-*qx#)u6E$-Zr`aQc~G0_?+H&q{u5H+oF%A%ef}HanW+5KuDQ^_ z1sh{)WGJI4fbuZdZl2?_*21VD!s!rKB1E%Qh*K4@RHih5cDfd}60$t}>=UV8MLsSB z1!!zQSG(Tr{ezjK%sW8paA2!Xeh$A#u%6_#KCBf%8I!}$nLM*XBE?)vh+XLy8J2Qg z{8I9wYzEH2|0-f!Pq20sP-mWKOOUat2eGgXWJSVJGliV~6^sQE9YHPY8R-!-WM?hh z-01Z0D-dwBGIDj%AGoU$xymDEo-XesM*nPV9s%Mc=ghHmaXPScAJcME{oo9WfA%6R zaCv4e2>%`oz!~g`82oJ#XH5m@LQc&Sojp}vR)`fc!%{My+2~Kho=7_C>KC3L?D*>5 zMNbpf5>x0+&Oo3H?-aWo9*X1q>+j{5{8gNQ#pDu`E&Pc~!bf?LoQ6}xGxwL3hrlsSOt3(&-r-pg7TfB32RtQB!J_Ou#ux5?C zOmineN$tL0pejB^I3!d7Ldp!u5y)DMH9w1SK(d?aKsrKZ)`RK}t~0dFWJOJHjK_^;XWQW_6n6}cFdae)9L_eqgoukizMX+qERS}e zSX;2qzLG8REEeFPv719j(MTm=kzlXY*eqM(^C3COFTVaUIRp50#en8NzNKi4SvZZy z>^7cb5Cyq8=3+}AB*DD@v>4i1-ed~d6c%mR2M?GYMaMn?ib9EOjQhvmvf3lwktr#c zQ$j3CO%CZ+^T0)`r8w#u01bw!9x{bIVYgF2?m<2<25 zD8Ub4Z?s3$1I&IpFya}PM*_;fI?ZG_+Ln(_zqM=W&m0Xjh#dS*kkV1<{Yu65tmNH@ zV6FVr+Xc9U|AD}6BK^;^<_^rJasq`Bb~BQ~Ch#A-nv6l7iP;3&Zv|etncJF$A7)BvMYU9w@Z+YhmuW z4F!;oh1BUrA=wxv|8{-+ zj0aW@gB-sLh9|<{4CV3UCy}|p41q>I&G6tI(z>FeULW!<>}*%{d=9ew2X9pdO_MU> zYCDTis&Czj3HGIl1-G{P#dhgA-2MWIC#n@0aN8O$5_co+Ddv=N-6k`+z$l*N+j4{V zAa~D3W+5v}`Uz(q?hyhKU!dmoZOB}TyaDoRw|x6qcw@|U@Iq`16jYh5yCegoy3M?t ziNX=^0Ug&XFFPP1*3G96uPoDwTRC`@8twJ7WUfB?5%x+WVZT*s3 zCOZVM9 z)d>hOu^wb}`x)%%Rnu=jgGMH)9=~cX0pGsyE5q7At1s~FwlrMX+6ut09EA^%G-w?J z0uYM}jJS`?Df5shTbOHQZ)qwJbF1qO1fOyTilZdsUx5A`9uiy(jrufFVK?jGbZXmy zvxGcpj2g3baI_QwvVTP}2Ow$+`1A8`bPOvS#u_rO;@fLLa>7o=7!##Y_j8Co4-jh7 za1rGVw2EFqbkF%(A(glOL`Is!SpK8+f2Hm`kJ`iVGC#B%&@$*YdNzd%KAHT*_SvNGM(ARVhMBYqW5j+)9bO{IpvY79^y08l`1Oc zHd3r?h{tX-yPyr14s8iOxE&C00pCe${Kr`(mK^5pj68N zLMb*~F>vbrFw5#-#*ceQD6n)%RY^j%(h%KfBxe!8)Q^I{d=;>u(FNABY#OuYF2NXjs7^o|8v###S`qA*# z)?%nBOrepFt-=c5(h7!Y(^he5agoa`_I83esjHV<=W00@_5pe8Ge37;Y~vBwlU zm2~HK>|+-y!+_1%-_VbJy)jqR*>autbXV4fl{$@}mrI$ap9i>9u(KZ)wRCtXf9Md} zbhkvF1wsP>NXG6K{JV450J!T<8AWMinZMyd`CYFR8xqw^lmR-{=pUxr z4LF@yY{<_c#}bgvML&R~B;UA`+z1Uji^;6mvl7{KKE z{8O>#uN4rSO|g#o1qpL})h}SVAf&=j-hVI%&I;}S0wKwF04_#Nh9%6EV9C3Y;-W^9 zaUdv34)|PJnhb@l$F?6^b2H~Ves<-yNtVSfw29N{uFYa+&T^}M9j#ZRf_4X}K zUJx{dy;AS74htWceaLBkm6lT~o@TI-JQmtuf--UsL%AIJT;auhzH7wt2n6Bp9v;bQu zBQAOw;9iJ#)z#Gv5C4Z1M{q6>(n3Gl>+?p8dyz3o1T{>q2T@lEsl_u8M}V;@XTuDU zu+;wuuYRN(KfQQEM66d$Kaa(Qb$~6A8)cN8N2M+z1QA})X~1FV4@G-H*+eS{m*O;1 zm}Rd$`X;gVrL5Sb#UF?Y~eC+T$%f6vGVf>0w5*;9q%2Xmoj1Zhe ziQ2Kc-pldPQD3wyQT+=W{o_C;<)ec2?S7&alAFw~u8h&{m~H=OT7sgCfMUOL2$U+b z6T0GdALMa_Irt@w>)0Cn;Kk)Z@Gxa+{|N7R>W4%0pz6vcyL7d0B5@XmUrmFV z%LJ?0^)at?pYW-DQ{|Q3d0Naxx;TO1kC(vkTK$)xcFv{8Blll(ccqD*xvJWW5ZKG`8q}(*R7>9X)pASzE(;= zAuN#kiLd`=2WO-!?}5+!c)qs;y%@Z9-CM)=Nyzh!i;qlVcbSF7SMLdF60Y_v??m9z zTqJ%gYBs^i+bb&s%lNnAzfbX+5xzQu{h<>L^Gh9-R07dhZ_PKEm6^jj7wS3;URFYI zmJL_0UA1zpDHmnZ_#JfZPj^%dwcW<>pN5NU7U_K%CDM;)7=*aFhT zGsyn}ccV;~Ct{qW;_g+JD*MhdW`ZrWDyJ%VE+lE9?4re0`9e8>*|t{<2R~6NtRGQf^-nu_psLe9NVn z1+GsAvGyDgcAuNlqEI(OTTU1Wmj{}18V}s6A(+nZyeo>{(S^pT+iDhbz|7BxBB0;a zgB+GYe7#zYGopvC(Hg|S5yK@LM3&QTpbIG(-AHWDbV~>V7S-( zVu|Ov2(+jt-b5tYPj%mO19_CB=hjdV;hXOZ1Q5#OZ+DeBp8t=pyH-~qPnu1+Tq=Ni zncCI07<0@YG>S4JU!F)ouZ~SROZbi}mWOX3koDD)&&xtC*If7@1Szp@sJ7<5tYSQPW;`@On&6*#RyGOrSb`Yxxio1BfA#osY3^PtH-X_>*8I2Hd6Y7EIoa7vqL{|+Sw z($NE*HGL3+LuZzN>(2z<%x(tPpJ&Mi;$mL-XC5j3D|ID$6W7ilmRvpXZCcHaDg*mg zo-^AY3$e#o=C6=E46>bK* z@R&{!p-*&v$UehuGHnvh_2*{VsE@b_l3xz43EXgAWd;K^6kf=3EZrk_l6sTYF!m1s z^=c2IF|qM?FZ*0YLN!G8)Kv$Fr7Mez>@SC#`h3g(hbc|~vmEm2-40~}qTn{pt=~_t ztE<1eQbx*@xaTz|G;FfuYRe`{sT5V7-ssw5-#kVu0B$Gq1uL%%q*@PB3BTnUgzIny z?;Ko5E_s%>-D~-DDhVY)s!Aqgx?j>NHK*L5f>iC6{f%)Ejx|2xzh`H=>mj zqhft{wFfA-+1+A52F-d>6~c(00UOQhVZ0|X$?bEhDVFp=`F`CX=|?g+dAY{QMf<+3 z^BJF3JaZp@WN77;1#aP4dHcRnMS`xpxcFF)^gwm0I1O6XEj#fAsW7kyPlZ;Merk(D zC8cv;VdVzx>^)-CB}^}EQoEIOc|LNBcieiT`7P#KcP3l1HJha9o+9*uYBcidG8$-yWo)5dUAZ*cQ!x~QP6%Wpv4SH}=#XR@w zwu&u}m+~H|+9{GyJ$QBGw|OEV=FXd34%LcR3Z3w)>s;le!s+ET#ZI^iPmt~hf>iSf zGwm)4DsKnGUbJ+{;it`PvOZA8c6-CT?3Q_#JVL*7ddC7ACv)?Q-^-k|ZkK0<43IiN z-Oq*5okE#;$q-JUCN5^HZp&_X!k#-#a9PfeQgluf#Ui=hAW|G9&;04`kOq8toPOFm zH*58-Z|Y|bC0Wn2N%u_zB0VBz7Rd3E7Th{t&V1^RE<%a7u3dYXN*Q@{BO;kNN?k+Y zgIg^)pK`Ex*H2dhD3j%bXxZ14eA*B-C~5+*_X;5R4nFQ419%MhZ;~}V zXshP2gPqsW-<4~7Dc9<>Z`@$kxXECSLQbGPih*qdfGa!N*@B&DTjyqBc)X-j|C>RZs1b0B*rhOQh?35?LCHQwkLLx(byaROpN7P93 zcU(#bT32b=<};BsV=OXQY#0!GQq&jZBlj&H{rU4pO(mdb<=srdmv#KsEe_t_T?OJw zfe!bOFH2ukX!iE5g0CuKQLTQ2fa;h)RQ-!K ztNy2=)uMev-zNh_HC+%Z#>a?O)pD371q8M5XZt#lW!UOWSwk)@m|VGlv(O}SeACda z2VI*KK=f(yId7igKybpc@aL6mbe_tyJaUMZIXG&bs;Eby-O$>h8<$yGZe5{p_W&4* z_hh9H8Z_Uw|IBIG9{DL_8$~4q&6JzBTV*YrT{4%o`)}lSa8;JPp?s=}#Rf5>J%+)C z9^cWm(yT%APypIyB9JH)l&H&cxU}od+u{56x!^=Q#89`>=wVM@z~w>lLv;vSSyli| zt5+_$$4e4N&F{iqxS750h#B-|xfwKW-+;TE0C4XU zua!swnUd@OIE*slgkpGF&+OJU1|+AUyr!O<`!lDhW{+!Rm(|#lPvM0)4PGvNf%<_8 zN;tY8Lm1XxB(zEBW{F`~7$3Yp~4wAFOzu$FKcoxX~gJuEJ(NIQ~ zPgUN}3~>}`3mMJ^9ZH>S0G=pdLoNn6Uv~ZqUCb#h1vU(~G-J*JAvQc%)(OZv1ID%s zgG|rK*%9UF)lQ{$RjLDz8b8RE({bzJ6QK1iWegZrsPaEt15~BKR$vkVz$R%@VYH^^ zW=Xz;DWBb)9qP>b5M_byxS zKCd%`zQ(w44O62k3eq_r@=lw(RS8HAOPpWIY4Ff4-y^9XM)i-`Y!f~|# z_ROS&2Chwapo_UIg&<+%RRYVnI-j!M=bM~5-`xxXqjO@P@S8e&*=3st0^X2z4-apB zF?Oy!zta~AQWIqN)C1KiagQd~B2Rw4t?Cx}?u5S$b*vokJ-Asu8pAO-tKCnr_Ev8K zjLfWLjg*jUEx@#h2a>#lH!o9}zw#$5{#B~iN;r54;}hkx*W=u`$tHhEefj}@ z(|!X`feVZ@H|s#!G$G5TsY+=@m(J=-y@1URf$EV|9IFW#rU@~B=UCTe?}$Png{}Q< zBKY4Wo-rDPQmoUx$a_xR4|(v|Z44Bzez6|N^DACFe-%g#8hV?QQ+#Qqe)d;>xt#9Q zlHgyy_pV#nj_Ed;{RmF|f=gplQJ~$q!IbYs7Gvb0niZNWsam2mPI!6=98*p(eu0Y) zUHn*vcPDFMplg{7W!v9@7<*H3TDfgs zmCpt`(Cd=N!TnEWZ!Q?vC(K0t+vzQc5JO~VJ>zl4CQEVCn}GfhL^l9t)Ga3_AQsLWrc__ zZrTB>^tO~t7d-5;sk-SxQ=d`o8AQ{|nL*Q@%feS%fc($LkC1!fY_P>*_ovmmyY-MJqtg|1BX|-%$Bbk>vwj zob%=N1g%oL_#uUfpMI@u&PSoef%R`|#VDGg)X#bS^l!_(pRwnen1oByacNWv z6~=Va1mnbo%dGUt+XcUxY<>^=)RkCp*Q+!ClwogoI~!cUvZ9(w?*Gtwd#zfnJ^x$_ z58(`|yJz)Zfs6f`PHiRvVJswT$EAua+Iq+@7-uK>$+;%tj^Vu@8J`el7D!iR)ph z-b3lVKx)0Za}MBap%n*glt0i=lE-oO9e$d$#1tYES(v+punMESN`rA+vAz`3_Jg&} zNqAz3rQv_Ufs4QNNofOGJ@VcQs;#q)0Kicqkw-ZWXKKTluZ~6r`8=Ig?DTD%=Idg$ zaUM_ssKZTq`QEnmhS53Q7Y1L_+J63JMC!JPy~TQ4)xCR{EdnlbQSSyBXGq z?#*BvL59&Zv;NXf`cV~$rQIj@;qYC{$EU%eUSyIL#DMq3;G4&+`Yhq{_P&??7966Am$EZ;x@WpD`vM95JuIhjB>q*_3H({-RjR2@mtmNG)~1I?bMVRv~x`Em$Ln zW#5zrG?tum{UXDzNu+{-Z0x&X@h$;2aQS&Q0uIy{JQKdv4^-o(KvsQlXECaQMo=a` zP1+BXe-EkKo}2V5sBun*8ODL3$0pBTdA|m7A-fV~LTwXU{BCps@u&@x&Y7OhIo05G z4;v5O?s`6mOtlEsj#p1X*o7(VBBO-TY+vO>YWF_~*YUzi>IYA~o&b*wu`R&dh)-n) z;QTaZji;ZbPo6|=jf-)oq7}+f0NPBO1p{_X!2*ZtzH(l*7DOYSDJgp_cjOHLg^HV% z`qocK3k)q4TD^!`ozFzS9pgw*shlDRDxj6)EMf{`QkYCkyr530r`F3tKJjc!-$9~~ zS-Zvqt`cRp9PPc7`gj^E`+?T)4AcSOoEXv&Ng4-$6@byVZ)|eP=N4yN<_+BJOJI01 z>!NAYf4A(*NFxX~IPx0eE4R5abBDFdh^?uGxG{9m4{SvzTg#>7w3vY3#Hbqu6zg`? ziBe`F{^5e(?LY_7cU<$|!?M&R?m`(7|3XO*#siTnV9f9YqpS?Y!}v5w)1-QcOCjV8 zkPoba8JPzWuKij46~SQIcBDowEHj`8$G3Y8@;1mh-!1%V{V#wVDw6TY!(p?a#eZxR6-*zbbtLPQbh%WU) zQ&qKu>z4YBmu7$#dUW)0qzAmM1I7Xs3K%?5oyiG!rrB$6&5nAIXXbJlyEaP9?tjNV)BxkEeI9 z$>`O71x5Q0I5Pozd{y2=*U!+-`uJyr^fHe>(^Y9vWc$RBd<96d-So^Cs52PvYitCq z{OwQJpVcd29u#g;p|lpUHy;Jxpuomg10~`s@3)0@pEyHJ#jser+lwxDbG^uom8%Vs z3YCBi^MmLdPJ@4e!Uh6*RBhsh&tI4Ze0sY@1kVZp6rDqpFE0`5TJXzttAmq-L|=I| z0ywLk;WBfUJZpr1l5h7uz2%60S4WP7uBza+Ouf|`r=snG0}b3n3LIEekSW#!Ap_s& z^nKv3)Q=98)hM}5gq@7uyFYiH>FQM9q~@iD3hx^OV21Xdu6$0J)}jPP`}#}GLJjj_ zxaq=Cq*Ryl|7Y(SEPs#TC!l^#%{O5PnJ7!M=@4l))gc^uM9CO005hHrGtAjN4r&Qu z0G-Ajm(K(6Q7~@_0XLX{y@((0gv^(NFf*6!z&MXAje{^*?B&VT@g_hMsPaJ z%n7#gI@z7YRFGs;M#Apj?$nMy$CE1Gd?OQ+f(2>{a|$`j0IM5%)f* z811!?nqGc%ZvQ^I3ED5`MIJ3R(k-U3Uzx-Y*I(jUFyW4Nj69e~#8#yDpx@T&NIthf zgUOVyxcL`m^)H!+I)Og`z`A`0Yv$E5(uUJSEyFvVXG<(mxf7Enlp#};Q$dJUF77a0{6W{FZD2LSgn>Y69{ae#0N zjOsu~D`tE(v_wFQUDadSZkbpwe8HVEDIXoULh2A}Zx+5T=Jwc+r|ie}mfIC`{2iox z@p-sPz0Ik8z?saJA+L*bk8Y z$Y9?4^-M6KXub4&L`q>weeQ2NJg! zO&cJb1g3@ez-n>&t=#@jcUShM^jEO?1edeyQ}CV(*- z4TTDWGU-RuT+^`(K~=u?&BuQw8=FoWX?2C}ls{#n%Ox5Ro#Mp`es}kP#UlKnp zMmi6q!H(~oNJe?vij=VEJhrk?^&kV*C`?}n;AP7|8J@NBYBg${O7-~~2(pR#a|ZfL z010jPy+x2ixXJ^D<&Hj=^Rq-*NtOj`ynti~__Z!iG4Kv+F}k1Yg|}*$F9#Mr%s_BM zaUdIlOpXePGxc1|cWae$6qtdAW=pf|{O^7AR|bu=ri#7;Zq*$&NGPRMrz@dVD>ma4 zNyq{)38pTtXFZunbN&TzaS0YAD_&+ML6a9lo_$+Q96XlR3f7TwS-)OBnbs?eo@3rO z#RYh86lC8p+VEmkEL;s zu?hLN0Zrs8LFQXGcE{U{kl!94dfFtzi z-_Apmt+zK$w(cL!XCdEYWo0eQ?TvnEQ6NPEpHZZk9Eq}4aQwIb$xOuav9y4A{*S8- zQCs(-G6HC-_l+SXI`JY}rAR5|Aan7p4l?yOd|aQOUL_FBz6`~#fB z*Y~pcY0guKZ3Rg54Kk$+;D4^#^OOrGJA^x;o)Ojfv>h`*=`d@g;fz=m9#P_Is!n_J z;I!Q?WDk_KnHLZb_RB3x=i#_BoLyS2__{znR1Mpwt+b#KvJSUc{fu=9PCU-Kb5#s^LsNZgxOlDB)aBQzU6{OM$H?A zr`Ku$$J6ECjp^JkK#W8GkeMU*^312Ag7TZr+=g7PBg+{(KQlWIqQ^)C8O@J7+>|u5 zrfU5Usg!$X=>)7w2YXjx;+%*eXZd;m2l7Q3Rs-O)Hc-r0?hHAfoG-8oHqkPAC6y$e zXKit?3ndjKRhl!==`rgu%)9J1*>so_nlk3z0IV3L=G=>PF--cu31hiDI3ai+*V#MS zC4~K`!-5k4C`}>&!gCx71$8Bp@TI^HNFtz?hUPQ&Bd89Atm5F%&GNGtZ#ojn?8p-4 zP8T`2EJw0NE7Y4H?W3q3k}=o3u=p2(G8rT21vwa!fNG4GL9c$7!R`HgdxI4@vK;}X zz?OUgC`Z^K5TU3YlXGj*BUIN8w8iO*-!v*0=4c^_Fb?O>WsXA`AslWo33tnHi4_-S zVxBFDbcvlosT#gmS2J7q(((_9)RXUr7MTvA`FHgc#KI@mo=~hy7$2L$Es*zW1_&@( zRu=zki#2LUc^nOO#nS(Qh`U;DhUKWMq>@^Wv%H-Zf-L5j$^EjvccGN}PH?*zKP(*$ z1#ZJtCmRU|zI9lhT@kXdNHG#xUa|#@8I@ic=~N1Sh`I(~z(=~+ac+_q0lT4Wd$9-a z8y(04KVuWF>sMSK19zd%=#?$HVy!(%$Pj$#q0zS{8_BtcUNzc6JG?*t`8gtx8GoX- zt4;!4n?PvjTil%;BTw5iRm{Q?&&$9Ibp<*w8mFy;ro0^fPeb~nw5QFb0zNkq1EBgn zgsta*|FXAB#gLJIBeKcyk^CC!v%M$gEYwwbaeM79XHkiQRuM65{r}N)?(s~&{~LF1 z4lz>ZP%J4!nDff1Db-MniXn$3G30zcn?o9*Iix9KB&NueGhvF1(1@IpR^(6;IsWd? z_wo4ot3Udq=DqvlfN>Jaw8@MrL8s+N0XcI`DT@Q#@mfFHYt#=w1EEr?9T?9FtTjf8aUOlp;~&)ZDTbb?+DVkQkpn{HpR%jI5@3H4CWU#gRc?&Y@z? z7T}#o5p1_6#lNicF7x=gm2Rma6aoa^cQqRAVdo|K!+Z4UJ4CO=$HVpZ==ULq80_JI9Ki*I4#qdB+&@&4VS zd7qh&ZF7Re*)`L>F_MHR%KnWgEc*5<<)$iz2MFu3Ig_~+mT&b9 z7=NV2wTq?EN;zBTSVUgIYGcs(pWS}`y&A&B;=fzDfetx5vFv+2Ku$bVLO&;7A zQ|}-J^m$T38eTs8E9vy|z+RVlQPJ7P=U?;RG^pGa{~f9?hG`1nJ(BK=eXi^&j9t^} zM-nN5c5_`kh8)>8dET6nX$XG?UI^hvoGl?&Guoko$Dbm{P$P85A(%1ywWT!^MdMc) z|B6;Jw?x!3yiSggPx`z=%%;MUz*WJ`?{!7M?gxL_6TTGFuX-64EBh?KC2FKM8>%71 zUF!#03#c^u_Vc|yCj$c`Hbr{etFIxDX*Vtaf8_M&VQE~SG+t)>sKT7*Str6s5H@A@ z2a)JN%VwoQ{q<$mw-Tngp8OvAF}%J5I$)&6>kji-zNPFO8mk^(j%6bmjdk8qLm^S+ zyqskW7?qw?104TV67d~_PZx2B8w#DcijrXrP34ME0lHJ1ibX?~JGAi?{2xCM;okS$dldKO*Il2ZDH6a2ezp{(-Z zJK-0Kb^lU|N~OGy>2oQ&kX<64)LjTfP`!oZZ#*ai+}y>G)5|gyHvVUn+N)Yw7*9K0 zPQ72#d)B7>R8s4set34Ncg)Vi*7ww_41pqI4dy$G^`y6iXoImMx!C1|*)4UXA(e>J z#@_E(T~7{{5@*>^8{2kj29AvZLxAHx0k)n+HWYCMQY|*P){N|ndd=O$L|lZ&DI13a zD*lVH-g;-R3{3onpPmeaia~6U!A|;O8uD=ZKFe3Kdh2@WNYk?(CVtBW*Kil%n^l0d zMh1c(hDo%cecXvQl1VpR2z#_OAuN4=e22B(4Bg)To7C9$&n@3H(0^oMF1VQs^Wa+9bVTn_T7jXejyz{gW3tOZ*1NjO*_>*9AWfFCTr7Mtx6`_PK?97ZV;r~-eZNaRXBU*z`G zjb^)hH_qoP+}!e)a5{QaUi00U#8tH`n$|;(*4hJwTW4V3FW%b!M}#j=cjp4V@Kw?P z%wVn1hWAQHruoeqXJ2nQdg%^z)Gt-WQFt-1sy?k3EUUz63O|!|fU(0`%q4^* z@xUHU&LYPLRMCZz=0lx|cac)XzsAi}Q782L6Vf=o8xU9i{6Vt06x+vy6-7-48F^v( z1tEFk;j);M|AjWnvxjp6>M8w&X!C@=fIl@7c z(M868Yq!P;7#_#!ia5b@qJ*~4PnSEa3uUjLlpb`WZH~G-G6CW z01)&X?YZ(ur`PAg_2sGJi@wY#VB9@2)%333eD!zusgnx=Euhp%KJN(nZPfzl13D%8 zJDs&P+p^HmIC~iXb7@JqslQ{FV>!xZWur5e`4_CJ4$ab8DbUw$oMaA}#<5$$^!RzN zkqgD+>TxT5s`-L&ER&c;>CI7d5I7D)u_yFE9_sPqxJSmhLY{`Nmv4p8=#wb+=YjRn z{Y?QP>+A6Wk+G~`b%azBoDutq`#|*gCyz&Rd~g29cHA(PJZ1ssGySA44Bp^dJJ%6z zY3G)d=ih|o2R9l}Yri_!kF)B?q?(~0+H+0PA?mfl$XeFG`lEhn#R|IV-yb@~PrY9b zoG5Xm?<>4W#^t!xXTFvT;t0o?d0gv@MjRyS4yOZ@OBhBnQyE9_eX`;ufXLjAUkgZJ zry*Cy+g}wr)}J&QRv&Fqq`o?HpwP+xKHVy55=N3xd>V`U?X>ez%?b|B<(l_mb~RM7 z^1a0@9$Gddv_JTdgMp07>3&ZRaqGHlX?4~ouwr$x zo2+gjh^%7rE)xheQ9yodsZ-+dI*q~bk8?WjofJDI?-^!1*3(dhG)3^@w*yZZe4gqU zr(X{{l|1XxYGm1Prj;nh{`@0QiK=gC$bjLz(=tdea4OL8Hh-cbIfty0NyvO3dxv)p;hqf&n0#c7&C3kiSb{VL2-N5B zZdqLtwNe6}AvgDvY>FsGx@f-?RoN#z$|dH!F?4e6C*`t-zRZ!Sn5+BZ7sCLw)GT||xgG+$2wqj)a`JM3LL{gu&oV>KT)VxWHuz8^ z27wkRwxH3!zE6BjO$JHRnz%n0<#Uo9__dS94+Z6}BNPTXE30{0>Pa)CW8|-7qeI zL#X3vTDz6Y>C?jUp{wM9*@+3C&%aLgo=S!RjUMKX?8ITFM27TT)D|058G1{m?+ZSR zI`h1mTFmt2uHp?B0$378B21RsC506DK&v^&kH^?CP3LA$oc-V61ytaSu z>^`%7GJO@xEFqHw+j&3rSeqguBG>;u-dj;?-I`bi_??)o@hJhOpFv8|kF8xWJKqM! zJ`$&-p!bS}t~|+`oK_EtKqAN((ke|}`#8@s>tIb!QqFh!z+03xKeNku@e&x@JYLi`tut=HM)2WH zs>;RYUaP-y0(ua5HINNgI5T&Q_@9xjQr%vzc349gf9aopy*i^17ngNjInCdJkXUDl zI432F4Rkz_c!Pu^)K5<@dr&2d8F{P}?6XWAArpwD#=+2J=XMC3rY-k*y(33Dbx8J4 z_#niMTzh-g^mdaddEa;E=cA>ah}g%|&F62EJhQRSN(M1_I6O+g+HOM!>>TP-5|Z}5 zJ>T;btouSjrvLCH9Nc&L-8l~`eSYi6KK=}JJL zNqofFSJ1^VZh_S|s$C&R`{-xz$&cl!2>&Hi>?rjDNca;wBBuQu4i!^DADJ^!QabyGnZZ zEC3mo_+j9URra~OQ1Q;AIBEQQ{eI&ElCzCFm*ff6;`sN!UUygJVow!ZO719H-U7LA z4JN0sQ}V5LsYc0I!IG{3TUBdJ_r$1_3rT}%?+r+$HYC4ZIhwtz9F;VXR%EumKRr{0SB-)Xnzers_~Y|O=jm}|;?(pg!DiN*(18#v421E)3};VO zm&e5k>>)C`(mvR}$G#g=a&Hp#S2jW6Gc;;ho+m?n-&*9Hhj{kkq`gjCl^buGROM8S zMoPNpedgR70Q-}xud+<1hrcJ3k+N9Mq}1@Gt9h=-I+pXCaH7H^KI)Yf7g#t0<-xdLBR=JMaIs%5{xOqQNaC&i3P(gye6_kR={8(Uv8u%@(X z>LJ2O*PP6jOzXex(%xvHx7T%9kNVj#6U-gU2E;kV#Gs`H735iUT`2o0y_A^Vk&+{{ zrVDeKc15bfW;4d0(2f#?IyTmlpA%@0qyBy}+8K%=lpsxkAiC1w@;q(#TI)t)H3K&8 zX`h4QdmPZfV2U1l4eaw+0ZfM}4)L%`{BoanTcX=EC^Hh-TWj5&f~ihj)y|O#T?BxD zfB!%YUF34a)Lr{$$M(HL&JC}F1Z!`@lQ5@mcc2<-ngdVC)(C|zakT9>-aZ|8ZQZxe z^9i@7@z^nx>`oDsj{pp~y$VWX3!|c?LwjJ=pZ*f>2Be#tmO|Z_= z{5BSg=ctJd=Mgr2=N2I9Nywa-P=bvPM%Q@4>v&es2>R3T3U2# zsAVmshrsTXRc2cQ1*RQlZANC%A%mWABm0 z{wOWZ!>>HgIXwFJ*X7;in4JoX3v{dBo{dQlIUCx1600X_R0?}Msp@AnS?L{?m}2Uvir+6y`xX?`be|A91jtbB9>0A6Pz&SFy$Xa#@i6+@Qf!EPDGp>3se)7sFskiysccYstI!oT7hF5?pp= zcUSg}?!7T#qC)g!iV#WEYu@{>^9+=?m2yyA88EzlsTN5-R0+wr-SUs)?cz{b+3F7& zYaYYzl=Vp~bknN(CGLDIX0buI3CXN)8`dyo*dh+XudM86C>~;PaYU>{#-H}zdBC&! zorOE8-8Jf&4Fvsf9!dy5w0${g((AYM&jQ56H`lM>vWQu9PgjKOY%37uw%`9+o8Yt3ORr;J$SQtf&#Io+KAz|7AT=%DG3S|9th$ z0Ta*q}U4Mj{ zT@MLCui&ITKRvwb2`L&5ucrhcU}t^X0j(d?j>=chpO$vKUk2JADgv;Wk~=H=C+d$r zE<~bZ{O(qH+_-*|PzinGh@`bFyO8CnXLT-5tq<&$Hjmk#=kI?uWGPo(^dJkai3 z#nVX+VD8;eNsPJ!9bC6B>GAnIQ{yK$mvvEoRlhgw*vo_KIAsiibnW-Y^Iq1{!}+dn z&X?74>2aQ&UfLDLD>y5~UW(I>5A0MlacgqBmv{j;go{`%$v|{D2=x4czGN3XMX0G}KTh&_5bf(QhdM zst>RYr>=R%<)4AXdFjabE98QSFvxM_KcC5DqeuP)c?aJ=&aWTZGAIFz*V;Lj^9lR* z)gB6U(8}=i-3Y3Sh~BMjxS|~Ok~vK>34t{wXUpGwy|Sf)+k#%;cB6s^1xbX$=kzTo zzn}>rlfeZcmk<-4RNa^Mv^c3c*0lXugc*{>Qrsu9JLcwnT5{n(g_C``e^!0>SFceL zRBH&hl;n_E}wzV&cR;oCfvEN&43;f85ANSlSV>16@Bf=oQ!aY9Djv0lHIOR8NjIa za#hJol~4|tp9DjeYv&1QTT*W`)}Ackj{2EhSM=gN*gFS+A%JrZ znAlTTz4br;o(>I8pXCtXc{VD3K!J$-mLOtCrW8rOfu(9DVMOo0zq-(*k}|= za{ZrCKZ9Cyt@gx0ffH)Qm6@g4e2L+X*Mu(nGZ$(=39TY^vnY90Q62wcCO7C8^JK67 z<)MH6?+}TwiFw81_ANI%R}%GWkmE&pfXx$354&rB*zj?XW8yS|-8+}%w~RiEj7{^i z_fl`Gq-Q2A-KzLp;^l4jtJ#A6fxm4#+viiS^rbkX*vzBGyfcyZ@9FXJ@$wuCIeVMW z_q;#d+*AO4%QvfXylTC<`@Bd8DdU8n_6?pHX0-MQXR+xm`0zvMn?!Zfh-DIsA;o-M z&4N{Aze$zA-W**sy9nX?6^A{Zt-*N0ZOT_pc6~K2O;BGm&Lp17h8|5kYlE#1hk2h! zgCb5exkP>k@2$W7z^uhYjjZoP$LKx23{aRRQzJOIs7XJkGANpR4c!?CP}O6nWngRU z9TB`o-I!r1)x%Fpl;{{lHjDN#EFZ--nd*jtuSj9-Qu0isp@}fpef=gdWthD~r}WM} z*+2MKzgXz8rivy8;cQ+;#?dH9>YBQck7KSIKl}~hU2rgL-wXe2EaR4b1-ZIt8Hw~` zmWj8|p-;A`03eBMLBOhh%xYiqv}xex@O2~7+(f?7NcfL*$<7r+#fZS z1PJHTYkUj(T`=yd&nJ5|F3Yy;ddi%V=S7^e>`~-JroQyJ~JoaDh|$;dX$SO#VK8~hF!Y|R}VAJHj$h|8>htn;3%uKxS@3TFq4d>_eeAMCG zTy|sQR;0FZ(83r_8y6A<@UD1YA+?#ZUYit;T?~6PcZE-Q62b`lFE7Dm)+jx6bDWCW`scJOQ#=a%9TEh@3Dy5Hy%Hxrv#dAQ2c60ooZFK2J@FbWAsOAUPccoDY}V zq`nKz(aLgFqUK;5T8fDR@W06kSMvo8g-m)bV#4YvjVLU{Q9^Opvw2A$Q2@-tre9s2 zXgH<+2tnn`4mQ74Y5bGtTv1)>2YqL+pOm%B+fZtj-Xji%6=thNm-S?qkDlOgAn!Jm zgURL9HKgTI@%Y;v%_PG*hXAH0Z9DfGzMR11o)%-ktg&L4>?}p6G$i#aEWzHIX&{EiwFH%S+1) zKT8Z92e`eZXIR0G`|c_bPM+lh1f&BugB?c%5x<1r))6cjwmqo1_oFv?5>g)pTtaxz z&KxyJ2bh4V%7s&Z4BRW}&*(?G3y@N0?KhIsX4U_2yOA}cTO3$!+QBb_oWasR8%wXN zTHdTmaA!5BpfS?|tNP#~=l9AQQ)GXRM)LUJ^J?GW761By1v5xo_J<%Hp4+wc$9Im! z?yq}Et}&JR?OknxS{^v8^EECpBjA)wIy!Z~3|bZ=X~crY&gnQ#9W3l+)-vdS2QWiqkYOg(GZ| ze-&vB{tYYb_R=0PdwZ%i7h~O{$R>U;^{9ApteBp2l7q@(U0^2GG9)!|E&4{8xC4w^r~vB9hyaO~>^6e1b@RH%#xF0_m@QT7ljBk5uGM3u-lz!-mP2!WD2~ z%&vt6LHF&YB~{{!@iaOMi2rfjcYasZ9kM>VB0tPQ&4F^wbBtoRd2l(ru}dGF{LKO` zK5Sr}W*6zVkt)CbG_f7&x;0zSfqWTMHg5N&4cB4!`adg+?vd4b78E@a6dRy!)gE^S z!5htYyse>1&&oLg4LRc-_sw0fqUn@vTBDy`jz#!Ap3}UP8sXVC)B%dD@OfGNTLBbg zu|=w%Nh0E0qJWLG5s!rgM4iiphvY)m5i$tWNqG#9)f(6FR~X6U!kGJ0wG$jBad`VF zx<@bATYrQd2(+PJE#A*}m}M)p#d13sUq;?+>7L=adWr!az-$M($YbOS^#&vo5;Eaj z_bmG+Srn5K#y@#^h`1uE*kEn$E#C|TEOwabp}H*W#RslU4##b)-?*DY+VzS_82HLv zSJKwRbpe#c0Yh`l9juhWv%#K-=Ow=t5X4<#DFIG1ump| zjDlJT6g~|qcNezPMb=}(i88C+LpMp?P$~F@x~;w3vREDPmCJQ2V3(7B&vfj&0QfW} zQ+tb3yC)}}jIY)mMSL)Q)u6Ec#=DG_dY~Jo`N!l^n4^T=$NQ#Mu>cyOFW;fAHJIeL zv9lZ_V!Lx$l-RP|y75KOlC-j#S4RP(YjrnM`~MP_p$x~ z&bRhU3%h8|kzU@so^y3vU1(*GiLxW0*ki^Y6mFM}1r6y5G<>j;GPVm(a|&j1($kQ< zvi_<_YLJ~=^iyGwWo{o?yWhKw zCS{kE;9Nzu7&|-}OBPFgY6Mw9yksq{Hx|8OeZPEJ*2&4R3!1*wJZ;Q}xUqdrhqH*R zg;91%%VOkNZuT{XAlVK**~u2C3Atex5lZFOsUjjGC3QiL%S%h@-wrq^mV?&l8ZvHT zz81+|t!F*U=KCK1#lOLvJm{7$velf0h19EnR=G225nv`Te4G=Mi7lmAamKJuPyN_Z z?JB9A-*Ryrneb_HP^Hza{`(wMfYf0C_J~ZUZ>0dU%;PEsiBZcH^c7w<@Hk8O7<9PS z8s&zTQZmEv!*I^AL5E1=!n|$>g)*MMI`CtqDOWHEa0lIQGfJv<(6H%;={-q3N^~~o z69gHDxI*VbtNEM6cPMsz$58=I?Vom|t-z&&95!hc&K7gVS-v*D=}Mu(zzVo6z9Q&w zLFpYauIohlp-u!w*9%Js&D`&|!WbeEf<`b1+$;z{LiJ|)^{ z!~j927S4k>R>U*#cFx~!gcln~Ft4wB4p!j;MbA}bz+nu8@d0w7j2POM5HkBcUIP8) zu7|ua0ZU@x5MMR=)h%mTSafCeTg9sSS{AMx&iB>O*@WZlVYKat0wYvUh9gIrswYA6 zj!q*3A1xe&Pyi|I&G(*&Da@i*GQa$+jFPTiUxM$^#1*()T4F#>z##`kny6c@wGPgn zpa^H!4vuwnB!qyI;aeU$g>Hj+IxTaYTfK^jAQaQ8%ZBbCO)r=f_^p0&3AC=BK${M< zt~tJX|I&WswqfL3lr(LtYU^MB-tRJ_9e#)NB{ok5k#bAPMK5zS()XRe-n^+R!G&L1 zTAGnZE(MbZDOs#iI-hi>pNu5C4CJtex-FWGMP>W8T8w0bonm$XHFSslIOaNG0+u*# zFK4Vk)27ptw<0$DSUA+U1MyPXHELjbJIU&NVn1PQ7;Yy7c{uAV>IiDpG*|pgccP zOK))m>zOFmAJN)p!Xuk3N|8m(`Ml!pZ(L|_0aIn8pL^p{U&q6pXpt}7>gmQCn^moi zjd+UY*MlP%MNvvOd0?Xb)8S&V;{8QDSee*A*Z7!MH>dc@o(jD9TKD2JPIufRFk}RJ zdLZ-6ed8cb+J`>NdU8!-9TdIyYVY@BWNF#RBm~j+DsnWO*9=B&oTyqj-EIrlOYfoc_Skdgwv5D>VJ?7iWtws zHY@BH={e_ZIeb3!_upTcnvby8zdi5^4mbG3d=jL0WVvY_b5VVf;kOL}-XlRs#^8V2 z_E^us*Y3n$IE0Hnd)@?aMsOptF~7R*0q~YK17lZQ3B_VQ;?|=;<~H}7^4bXjn$|yf+&^Z1jEl9Wn^GL zCtTXuKi}3if|5X&kej2x8NCr6zVq{CKLkFBQr!TzjzGufWN~!l>dZsGSIQt@GtmH+ zq-uiXyS@6d%S$}$*ZP$LH0|SM7z|C@bR5CX3NWb;=%cmujNsNC&r1@5?uyj&_ZD)S z;7xTRz!JF-INW2?p=jeAZ(5hQ8e8!9f zihtbiPFrhe*xG9RSWglA!a)gefH*%{r~$4REQam&qcR8}47^8N2v5lV@WLyp$T22y z`pjJ{rai)eaNqxW?%Ks$meJ?w_FeEHP-scHk&RiTFAnb4-zMgon-bjcKnnp%q@>~9Zpj%?o) z4ea<>-!UUc%q!)ugF&0##I4nJ6rM=7fEQOf-YEl1rpy&w!b*k)br@*4M!T~{u8szV zE&o2;iymD&RMzjUGNj|TNliRIkDK3oVH9hxnLhjD&#AwTG~V4$ z&fQ4BNkY!y?jrfbycsBbB_J3&egiTc-2$|f906`@-U1}`=jXAY9rwr0BGQ6pGk4J7 z1Q(--%<5e};HLhq^G}Uhp=Y)^#0r;iDd1GWfYQC$(0T{UYCy;uTJVrL-}aA;wY_ol z05e#J7<(>}XCmxDZ%()qwr3-}b}l8wBwRFF{1Jqq+YrO#4IzRRntzb;t>V!GXvpaL z%1N?DTuBIHtLOxAI)B8iE9{T&o+U=IdnV9u1nW0&i&kJyAgpSMqPLuIT&)U%)e@}MDm zbFI6)g*YuwuZ{;g`+;N~|EOiTE$KaKDl%IV&+#?Mg`?}dQaOFTQh_YMl%(Q3U{_u! z;|6ft2nc?`0mI~vGUD?jLcPH#0r9EjycPoE8f03&7a(pzhKFK8Y z-J9=$cl`t%QmS6ESLY&6*%9t_pDrJs^9TGkN&t0O|FYWi=dlVL070HODt?3r?YuuJ zCnI$BD!XYN<&D&D19aKxM_b@zym#-OArb8Lxv&i`KfwLQ45ac*`44YC5)^KAfFB#S z^6fMZ;3n77y|3Tl7d`H@sC43uki>*oef0B?BaN_?FctXnc64jE)g}Okjf{W{O_C}e};gICsCIPo;v+k$5Kj_w1>UWXI2s+JL_8xW zxbI#x=Eqv#QJP0_Hnw1~7E8UsrmS;H1rHaiRBQ49U zdCZv;#nJrGkMq2a>c+oFzHVXSK3M1HZKm(t+Xdm6ix|{pO*sZw%Gg$IOwXL-@H$O& zCYee^NyCwh;KsxG#`-FlcR_Y8tg9HiYq_pG*@w2Wu&H8iV97=tcp=kE7$4McP=}^i z*dSHY(|5ZG?k7=W_g#n8757gH|Fe?!?oHiv-w6pcgo%e_^gKbfNyf@}GhkVvnDUhq zB8JAr&Zh+hlGi6sKgOPY^ahAB$3~rhF<|{{O{x&WPVstsJMs zniZ^hm)0`!MMMdiE-voAu~L0x;K>L<>nYul`9LfF=p*d2>@SpD2jX;lMDg03Mp5}q zGCX`3x3w9LzWRioqXocAHB-xGNsCEuQL}|zw^E=_$lIF>A~s__N5 z9{x|o^A_C5T;tTgrQ6Y~AR;o6wKVQ6W}!1b>3gc+!;4HK)YIEEDPs&Ns`?z*(Vlev z$jx<(sNWJbj8j|v(k#0#qQzlpDcxwJT?AZYrdlKRc6TED3&QVLeAx7sd( zn&Uyz1nM#po?uoxD5a!G3o^gxkr=1pF7uDV`0fG|b)k-m-%nF(UJr@f;#qbUfR*1N5lCQ5el>W;_BPu<Rs{jLi^hFdF$d=$KPHuI%Xo1Fedo^L1MGY=;6et0_Y>G%f?GFWob_wQ zUN#?{F6S3ID5m=K`HQ<>2y!RdOw*EU9bR@$CtK*cC!`8&&BN2hKg6dAKZnWhEjaBi zNJn=h8cU?0tughV9$rKc)yE(R7+%r1?ONeTjv~;zGO{$;4iME6OT{$%i6^|&X|$zj zn$E^DLu&LlKRgODkobuiFPf0I&siVLwFlx`6Uim?kF*Z;H@at~hm^j#>OKJnTb41vbYQVh<3y)mUPKkX|Rx7d`y`K2YW6@A{a`OLx2Zs0AOlBOLW z0D&OzTOD!OaMa`G0LBrtbF=J+2zZ+rtNy^ zE0JAssZskJJAvP?Q06~E5*E{;o;1yw3{;k2`CCU{)I!m*N zUj!yhez;t!gyw)pK4zC?bnm*;_p%}*bYe(yZR@K$lAm$-P_TB*)c8Q*Q8w>ys$BYFJ2zt+Er@2^hxpf#3V{V zVe~)$EpfoXUV&_O1xQmUwgj6ZVpwYWX=@p@y%}wM$aRVtx)-5(kncK$pQ-2Gw9o*S zG5~VC<`@zlr9iVnqlFkS%=g_F=Ns<~SDB9eh&<9QDk~Q{q1!c+Cf8Nq`6e$|l@3F{7=J6Sk&JgqR` z7QH~t9rJWi#b<6UH#Nv{<4TMYl`BEUNj*IQDa9;Z^gNIUYNBSHxy|;4iGJ5V1L1BH z@|IL26fAmr>vnUS>)(Y>Cnv1fNJ_s~OA#SAK!KNDWGzz4cp_B^H-wWe< zE^E5Jf(}KyXw_F$<>1WDNGyI#Qt} zR-ZY42boo&Br);+>N~3*)GVbaf9$pLinZrrsevnn+mz?12`*v2nvPM-Mt8!B@;*P! zD{;{mysIeQPNRF|lWoSUKI(Y>1}8=>vc!sH0S)xWTYFU!n44AS_X|vOA35t~CVE>p zOzT;&s$P=K2I@Pky|68`B=e978RNR%AqbI>pzLiq?HsI>opxMqHVGi*u1{`3^_7J7 zJ*PDB3SKeC&VPIc?a0NNdGD{|HGLT5h)fx_^}=G4#{8z6_g4NpQE$D8Og#uTNhRY0Z!kzuiR7{)pL;Q_p7g^5p22FMCfEw z*fjXRHH*9rWN6BweJeUPmnKfbyR`a!f<$f~QLkyjc3q^+?d~2?)o04eLecD}C!&8r}BrMYYXw^2@v#p8dL zCc9EJu1g2OlNVyKtW~J({sh^fmYB$`rPcn{t>)#u=@U-dKg*o9KY?)4NOH2S zT3PQ<@4|ZYMzLv_N2hHC`BPI`b`1-M51!2nxCsD*O|>jCi#+HD^LBSJE2m#VN#dQZ zUsuHI-Lty;`F%DKiYW(&D>y(U@y4x;S#Dz~taW(7&(6FTQ5;;kXy&Q|!H>DPo*ec_ z?ozktc-2>rJoV|ufyae^-yr!eh(J$!o)+?1af9;|9iv)OG%*m3S^lE#j%sq2KhTvL zCoW;`APtIt`sz97b6elFuTD`q;J&S+BkBs{Q^|9I`COF;0HRR+!HPz`XMg>4|2p1- zbKEdhlwrH^;qK{?qy-g-awWzboqFl3`fT!MG{;Mw4UR8+(X9{Jc8H+fD<`g9yAWx4 z?YjJ*H_~s?B9f)V=MD>pQcLU)U$_mbJf#bV+YGlhHw6w#G?ofvV{4JfPH>tj#$kJA z^^RsBcy6D*D;;POjFM2inCr$FW=a4#v=7i696pt0`}q${UMGqcbdmq|40zN0h)lGbWlsi1}%;46G zFCP`x1HrNVym3PK9#~T$-0)gD7gw@>nc@g3(Yy=gWGOtyH}E$Ax|kkzec-n7={Hpk z2I3T=6_;25!El3oGzsINKJ9|`{po(8tn+?^o(ztVH*1BFhOg>3hlQ^G_-9SKBHf2N zZvz-?DIJO^O~waxbtc%(+MJHro|Pq3bf|9wKbj|X%Vk4kuOnvT-Q{Qvi=q95(Y8Ak zPb%mZ_W?*LauqUs=sRO3G?pU;TQ|P!tO3wauvu`%imZ!_eJ_$7n4k8$id$UY>Nnhq zd^IkKPt#VPNvf6?5B+}AMS2iYP6sn#ovEHXoVW@oL4d9bz;%?nSS6d8vF|v~OZCHC zCcl6`&*Krq1L5|n0opVubwOuEdznHO7HR}GeHqzLn=$dSa-km!f^ zhXo1`H(?d;myF8%$j;Io=HtF)DhWTy#(Ii6#9q=&mp3JcUTQa(w!iu|Mgsftj5>|$ zVPpC}JJJW*!mh94L+Pr{3=vm2-iyYI2*DA`OSE&4hpcaqIXRLp$liiL`bgC{=_k*e zm)^n7FD94fBXxwhAKkJMEo5Y4|I2AG^x8xtkf;4J))*`xq^YqHG{=cR5%^O^fBwrT3@nFi&CbMTEKT^hS03*E zU^^bG}se6I|d454S=tknFA^qhl43M*c$xBzQUkz6QT)^$^!1+bRB8j-( z8dVfKIeFjU*uO|W=Do(la#g_xeij1#{2==C!~}@t zfFHcYY)`h$bXM`%Hc15atfIb#G!?w8rQo`{+zgn2TkRoRWj*;{#Z}yWXhqJ*yEHpU z5nVEg^NTCx%R+-E0fJ8yd-L!0lprEE2vM(vCjGz^(=%-@Ds`4=G`?nRZ+v2x1)=&@ z-5ga{w?BonXfCbWtk`0_4_3IzOV;u!7PO61KpKU9h?wkQx1P-Hvc0?u+Wh{$T>iN{ zsjuI%#{@@gg6z)M^G5%EIPHF!iu_x!y!**%dwXMLV@G!P>)yW|S%-|uKzcE%#s5A z;v404M*7r|d$yBpD=@dxmqh4z(5#P4(#+D*Zr|*~j$P;HAG1F^ye)n-vf3Fy%o%cu z?ff2f;|>QoPZ#}b$Fww_=AyjbAL|n!OTB{NeR;c30#vAeXa2g6pxz4iYk#U4n-xEL zl4vPlSJ*Cf*Y%vE#q;??-cNwFbE+vU;GyGJ2XYpo)K8`$;FgkQ+8MRPi1Uzlh>nPq z0O7FE-2?q(RtnveVEx?XVboMM))~LAW{=+CDJNu!nfX7O&i$Y1{{Q37%^~KH=1{C6 zbDrZSNmJ4sVkG7emK@gPka9lG;bJ7sc_bMrlro2$$rQB?YRYnskwi(zq3`SS!}ky9 zcB|`p@BMzgp3leQexCp-HaS7Hjf5p+00`$YU!jkZqm&xii_dbdmj0SwT9ttFn4vp5 ztDruVVFRrpDGLfx1OYmen=zd`p4$RnQ7{5#WwtoXzs@7yw`hOO*VHqbLs@Tb`(&&r z+&Cy?f?ph7xCZ8kAIF^R6P{Tkh7ImX;ztnghERHj#?+ z)GolOJDWYYzRgSIS=@Vj&u=MOn3t9Rm6cQN!1vxNBpGA0 zzPeML6C@EQh55}=5>La4erHd;NR-natO<8bk#YG4;^nq=Ydc%()VEz5nOzYvG2gem zSB3xn(ObxK;;8w8zR#L^wG1Hv?W^?>rd6r<_wI69czDS_?U~W}5XDsQ;D`Ld%9Z65 zCJe_aUBVNGo+a4Vzi!eECRRe-uheg`T4fIVmOpw%$)0W- z7}BdZ{i5GxRWT_q-6w;ViRYd3u;=r_+rm17ZalmwR@TvFBWA3pZ&wyw{K)n7mf7;~ z5n^p_#{XW?wQGobv@nvrzo6+NP2cBEEo0cUF7p<2a2}8jaCT&;l}P2=83?$Z)c9_W zaotop5QM?IRLSM)_SjkXx6Bz`-Y+>#F(=z~=BE$N3Zy4(c$HMto;vAuFT}c5=8jpM z7}6{s5z#V?>LEiedBaB@^I`++ib$_+a9o;EO!Ul`bB-^ILR3-J6wTG#&<8J`)}M!D zALl3M;+|>hH_aMkGhxAIl`>R4G1IC@Z51w!8E}ntKzt9SF#h*FB@b;sM1N3o$z1Cq ze(rOD&P9R!X?;KAx}-6cP&9pY7I@ySl%$H@WlU2>@TG3386e5?4Ag?pfzM46ociwjC^< z!r8x{UmZBpUd{rX)ydif;Ii_o`7obfzZ^P9UhNw`>s+A*G<7vs2FFu}-_@5A}k&QmejG(7TzQ3`i za;gpU!_3Y-4FfGK-r#=8KR;*AU=jP06fHlt}`?-u|&MRhDgcbqr72A zR8n!W1hN`b3fKj9;?Cf@cOM@m^KiR+0x5|K2fpgzUNpH8#?^dn339{N=%M*l0a&@L zhV?BEY$9KjtE|C)tkxzXh!3+Z|9h+{zWfU%e#LP3EuHQDf)Bqr?+d69Frun8^oOes=JS5C&De=ZmPq z`GT0;ZDDiOKv%-6CoN08_xg0z1sC$f;MYAe-S)!Nx0AKG)?I*c-$uI~W*6+;c3ClP zaaIH;{Ylb==Wz%i+M$h}E4OzCfe{p#1Vz!J(ct)LtYLT?TKn)Xzsk?lR;pp^=GMl| zHOk+U0WV-0C$`(y4`%O=XvHE7^I*vUC01r_b`Wf1Lwky1^E^N{sPgQ2i3TPgev1>he|i5o~O6{i-QKrH+9- z?e6mMvFg#D{-GgJznJ2qb>UhJl0x|}4~_%YJ2|1%(azQNAtM_>d_74&4Jl$uK6pYC zfj@_d)=BJl58G?-929jdALaT~QW5=NIS$b1IMdQHwR`JRL3M&H(Ves#*PADnxd8yL z33!xItx?O?+7$ieorQ={)-1JkGD17zQ-U=iOQ32q^>$5K9p$WZraFn4Z0CMc+NpGrncva)%!0wNoV@Ix9(*x^fbeS+YF8$Jn9{(~6u?Q1#4%P9?_DqA>&%cM2 zh5Cxx>0UmUGd=g;5m0RN@uuA+sw8;Xh+&BBb|oX7y(b+G1}SR3E;LZsTk}Ra0Orybqz`Mb*1;^ z`fQC;ugut*$FKo+D6*O3@#up2uWO8dM-i$2I6VajPOb-c-!rH`tPKO*gU@IQb(L`-(H$zzFT z)X>g;7&(Zt!z)izZqd1L*cu;NK-905vv$ zZj0!rZ~%VqV($Zw(;tp?3i%#lMPUQdcc=RPCB*&wHkJAFMM}=RzT;ralVCW5)JCSy z@ilA%gj~Zn`;-&)Ilit5{?GcuV=oX7q=J95dd-|Jom$7ZW7c6+Q)}40=4jLM2S2RG zoT8?&!Gifu$LT}3Bq7%3(2z!N&QFmvEvhv!w)M-;CtmvuZHJn+54PN&#)PvmeVoeXCxvP4|mXXve#YS|L?eJ9a z^UR6$jxcd*>depdZ#g)V>|shS@&YA8@E9e-QPEj^g@2KucNJ{;tfu`card zZ}+F->QMlNQv4!@I8ra+q+dNI{NnVm5egan1GpeV>uUaCAVbS$fu(>c~%4z3%+(z$(*BTpGo%O^u;C|&KHwCtlB=QZe9 zEoetp-wVM3wz(`nfpT^J`rv>1mXH${S!y;6c5khkLi+(K4)uqS@wkY89{4jlJp+Ad zdG0Nf;BEygE4%YQ97ZS;si)QYTs|I>|5WLko{JGfS5p`RxnE!SH?#-Hu_o5BL@upC z$eYk|SC>Z^-fhmaVxcbVwX_qbR1N$EAB65HVC&d0!uQXmQ;1AwKmM|P+J^9mF3wpv z_~RVLcv@t!ePYD2q++rU&%{B;T#Q#ldWQ}@Mp2Na-y+RJiMX=2DPV0Qe)rDKPt(`k z685rCOmR~pC^n@!6oXV#Qjo-Ziut z$9;RE=`o>?EvaHKb3UGtHuLR`tY*B&w`y(J1(Z7O{<*qS=dFmEX?ZhRzc?D?N^=r9 z!b>oSleud^r&@Q8Yp-vu%&u~)0(`_kdhiHbuq?QRzv*x?iYt+JJWl3$SGh0MJ=I&8 z8p;&kI$jam_>S_VDgeAv(?Z&5Z`h-0*}qDzb~@lhdH@mV-Ls`pYVN9?)#n>yX*ZQ_ zIfTR=UyuD?&;txb6KyxTnLI}m28?W_t^V2$YYu_LdRjX@#yLp~)hTBhDWGz7C+!xdpzv7yfYXD%A;@z6;|}}fj>sh9-Q!4 z0NnAhDKGdP%cX&=Cu-oQcDd>hdXEx2qw=e72)Y^@$O#|Pn+P%{*+Al&IY7oi#UXbs zxGNTVLOzBRq%Eg6bI)?z*oU0AO99}^KElze64(qHHZVuMRdFNS?Shw#9e8zZLH@ME z;J5b|-u88;@H~WE4`9oi_ac1N%IVa}n8SNvrfDIHH?|It-R&C_CX!5k@v51XsTS6% zDZP90$UwMG7--W%;FD+grSB}Rf!QNGiJ6|zE1Qm7K0aegns$6*bfkiMsPamQP~9yz zS<_>taVV2}dP8W5KCs$Ifh11Tl|Myf9HpIt)UA}E zElcd7rosiR3e@53V&4!Amc!X$qYV$Fko)`jw2;}!u?tu?VhR+^t!jv!aO3=ry;^r3 zi$#=Foz(ehA$1z^qH7H|*tx7kJM;`a8gz*F0ydGLBCTixpeGK6sZjBO8ND}60lI$o z$-3C)sW=3^k<=Cmv3%MOkVgX{SFSiNjy9B!8eKl`9L%~oFk|t=5;3bN*+NCy89O64 z6GIKE467JYhv>8dbgHtzo<*J+59YzjJ}klk+e8{^%*xHc<>D&I&}Z+Z?|uoCcf;mk zS*w>yE&~QK=#g5%N0)9CE(H?eeqQK$oRtzSsT@P1yjSNgA3rJVYP*Sjxse{`Olmn21H7vi1WInM zW0NdDxzYBirhd&4?j@=$*j>mFLct|_nnF0E&jPRgm(|>ArV%T0L;Pf17A2KB9M^nt zPbwy~Cw2wx%k-}i+*TnOu;W?u<-Y@waY{m`7@Vre`Rm;+hx4w)zW$f_{7PewodcF( zI|Mrn^c;K+H?@y|x_;8h#AF%P*q}gE;e&xI{hqI;C|JmnS zS5f5eS%`T|)FpR)L_!9w?bOcWB{6j*%nEZf(lus2Oqn3z8>S6Rm>z4_eMM{7gzw0fhR{o~s4`8w{ZD^wus3aI7ZmBL`{3M|F3NBZq970gef(r*taPN)?pGVfmQXZM;ERRt&x!0rKT)T1nugW~brqbD}E7o=4T z$s51^esiQCK)v`40F079T-;gH@d+MT9WouyazJjtX4-BnZv_n$rXwS`z_5)*oYIag zW0qkB zba~n$xN(iO5Mi2hk8;pSVQ(vpTN_YH`IwfKnXkS^Pb?cxKI+-?Vk)Zv;k(>dRX$42DXG}z zHLx3Mzx;>F=ksGX*t~f!)?YhC)RVY$<2d}30{@~|9fNdb*kGXVUGyEC5&Mvnn6lz? z6PG)KN608 z;2Scsc-|_26xIR86Med)Rf!Xoxw)Z13Ph5$W23Dj@jOl%<6v~$eR#@b0396{S5{RM zAb*X5WIic{F^g#cJBJ&jhH=723!(foz38;y7yZ$6!>@C!t$rL`-{a@C;2TPytDGJ z-la90PprRra}|&nIZb0r>m-g5Y0<);;~YTGij0aHJs=Lqk6jWy#v~1L4)*mbAjJ`| z2rZMcCZg(GcW=K`8)Qg05?ez)8f*hYP3N%^!IsAwRae%=hX{x!!W_ezc0dxLjBf|ch^a* zg}5$|m!Lfr_N9QBPUo@-eV?Buvl|UW_mcl@?ffWt)b-Ai5ceA9hWs%6a?4!J0k`z$ zn9V=pOWJA^3HYlyKmHuMb`$qtX3D?L5U)`~p7a>5@V7jq4cwrY>S|gmVJVZy)Ou2Y zO42H`7$1neC8>_8rdlD2nNm0+0{;*{v=C8C7hx%Y?`G8AWG@_+W6-l_`M95A!R}Oq zGMNmcAU#GLEFEpo^gL3{dEkW{TaH3h;_yUUwV$lRIPp}9c{*xl1_io0cZUt~aNwbR zqDgs*7tw5K;9Zh*}O*`BWm2#Lj3dc%nIE5v`Ti zPSYVnThTVhcWaL%LW6B5KRrkBCGZI?#|%|H8wZtawX&HdZ$+FAxt8nz!yNbxi9 znIU)&QSms6*I#`m?BeX#mWY*mAs%uBWx-OeoOekupF*`{Rjv6A2eh|{G~v{nqOjC^ zJj!?K7wJ@lD&(p1m^!1ofHy^+87Mf{yYe~XF3IgvFch1G1TQ5j4y>KKdVXPqDBkT11;-Q6w1+f1AVN5V^3xQ;g_Kpe(^|MF8JvZIK9#E7E z1WC}777~BS_|vt1CBS4XY{F#dIZ+anxRkn!W7T2DbNaytC?o z&&PG0_@yrC`=8Q%I=Ix2mz zjqQrv62B=Sb^AQ~@+$*7(lCtF@|84Fl}mX?7cV;Ol@5kunB0p@P z9h@25;ZZ+gT{k;3AaP>r^P9$Z%MuqMhzH>NuKSqRNiV-uv$%dZj=?LqIwnX^+Js5K)$GImvV*Ss7`pteV zkgjp>Eegxl*RHfe^j+cau^LaK9?H2w@#$F4gA%R`%2ghUsG!q)GAQq1Gt>F?ix)4x z!7fr)4YJ5idoB#o?03A3aKw&@A35~sWx%@DAMKkgb&w?}R!jb>+0)lS?Nzh5z4!jX zRbH>Y$JO1xlK+Tr2hdUxDsYr?N`PV*Yt?|idO%X()@u_fpYNfaF2v@U%%Da*U#cai zNyAH#@5d1=;x2jUOsK)>xT?vAr$rBKkYyyok~wR5qF@Q1d&-`-qUoYb#(!jdUq$-W zzD77Q1M6s+k!5Z5>*ED@*v!zAS3|vO(io{Du1G(U9dDsl`>DnHi1VK57Z^C7#I;h}+@8&ZzK>qc7rb!zw-QzA%i#TGWTt=^<&Ej+)y~#) zlVUMP&~R1eRyW#@c2Dt$!aoIDLA`aHU8@Xn2_+8iuWGYF@IK4hc z8cy~n0jG}nG_4;BcC3x;p0ojq;Eu%jDT2W^*U_$0N~t|pWG37^s+>5(=8=tSn?P}s zR&nYX??~qhrpz=~`37v4W5tmDr-Z#}kz5Ds_1JjS_qL{fgGUp9azsLX`uofZ(N<5C z&y02^s-y(7gjw830mVgGzq8{eDt?lU0Qw|RZSgcf z%|-h~+~PxNs%X$H#NJ-*84e7|Y^Z85;#N?ZW+ul=A#igSIluJA=O0%Sq8~e3Yf*LKHOS3&nh9a2N z)wiLcp83_)el$d%?)UX*)`qUQe_+F?`OQ9UI*sobf^IV-;PGxyIbUj}7-t=+j;;q{s z7zaSlbz^&*YzvO*-Nxqd>;RNM6?RL3fUU74vgDi<2&VLpvJG=dL=^W3z1%6Fu4h*k z7wVjcBMk$qpRxN&?ogV95*lQNccWUx`=E|7adGwZD#oj6kBM_Ls5iIQcQ+SY)o0re z15-6{dw~qx9B$B5=f!(XdbyAx^T~NcJMP`dzLoL5Lp*n`mdFatv>co0ikC5KL#5J&56 zktWTQKJ$flu!nW^IV$-!5LEpoot8ae#JHlzkLDqAuI(o zOJg+0gdJ+U)$Vt3)(zig*pvui^q z{_Q`p^)&vEpO>!#wiTMzXU2s&epz0hF%aq5>I`EcQ~>gST)zRYnCuwc>IqknfPc+B zDTcY0#IV)gy!F}Ih#l}*EbA;hK^wFbBCS%rpM-UI(m~8+#kD$8oe{BIje>il$UxHr zvgiZeR_Om_VsW|%=h6U*q{)d|kyBnSobrb!E8qSMwm~mlKeim3@e?c^nSbw6@dO_m zTMk4!unr?4ux-MnJJIvc{NsoHZ|aXKK+t0+l0`re>Sx3UJd){fJ-GACjccu*f9oGH z4*j=ZSuW+R2I~JmhTHOHhO`7!x_57zB-WB~p+S2N}7GoiUN?KMs_Y z8(uEkUpR?X83SNe0RR2=eSZ&9b?~v0G|U zGfnG3;mjPQQ8xd)F)d_IgY^i&{bt3sy@$ecyYu+f1kZtI$Bmf$CW1Fa<*E-#P@X@k z5U+k@Ps@8OAcDEut0lJ36;#-6-_CZs{J+K1tE z8JDO;uMC`mwyFOD{H&)=!Pt+rH5I17fh5MCRj$sPtK;>btA3|3af?dV_$KRo_SX8H z9;7;OWuNKsyKoo;g#E}=UA%wB5wOooo>yin^O6&ZK&<;=VKmO%ybk;EH%QEFB z4um5EcB0JOh=N>(%rg;N?CidQk-xey4Y4`C-#s7g5P5hx6@YuKbk<|?tX>DN_pGE$ z+%H|R4G(e-EtJDp)q68sOU0s7paT1qL#k*1uW6B5hNyP8_P)07x!3I~+Lza^0I=)k zGfXG7JyY@OYiMkIZ4enR_l^m0eo3y$WIhZp+NZhB1I6T#2X z=MMqJ5#}+>$6r}o;}kZsZR`&y{flfRLHQP^f&D};2<59v7F$)OM{NBmHM|~@nNzV( z3u~jY@4IS(quq?a!Vz&1rc1_u;Kg-D%a0V+HPNx|yND}pDq`M}7T@pvdmQ{15roUU zlCQnQLe0wTm=D(E|Fy4XkTPo{dQN2sTYE?$$mVr7;~sqoH92tdeZO30+G``u75b6f zWFHDhQ>Fbo*eFTj%+jzgarI2|)~}t*L=|)2#{-qr;85j@C7{S9)>Z#nq|r}0WQQ7Uq|xJpn3 zqyAFPn4LO#B6gQ-Ie@>7i;9fAORYE>H-E2V6`}I!3(s7@6=}!k8V3mmmP9%|H+E!s zy)Duc0!OPX$Be84+V1S3%CC#%_6DRObeHENpd=$(>J`Hp5@mOV_7X zx##c!y!uv8^v+ntqmw>|ttqtwkPE2;Zu>v#*I?||5VvDJ}?`;nrj$*DrM z{H!9k)-$(}QQ>y`XjE(%M4#11gZ^uO8tycAW&?9qC<>dBdV-L{VO zb-GSWbQv@$k$oSz->1c0hw^kwH@GM3od*X`Y)ej2y+z!Y4xnf z6M+;&*ibtHK?`BZL0V=ElI{uB8T=Jt7CJqUvfqow9en#>`u@J@tuO%w0RBlmIq`i; z(SQPQ2QgsutVUno%9(y3?hNFwM;~YXYzyw>$?RQ7eR>Yk@UlL0oi~=ik~6#ZDGIBO zPzjaf4dmN+{+vfelOT4@#U}2ZF7cic_Z$V>TfFP0ul0>ka~-h^nO291?R?%kMryU@ zMmmY3_iQi^`YQdtF6f~U^E-fjP+{de*d4U5uWyn18P0kd`j?krH>+>wzDM6MJ(x#b z-|7e`rs=0c)u4Be@=M*#V+#BiBqH1-w1H%yrIHi*B{5#3=hU)7vZ{Rm?wDA!h*H_W z2U1R26*;O>Q(*}MlUb|49cqI(`9M+D6N2Q@tX1<|2rn!>b=+7}Pih^}S1dPDr`r-z zeeP69Gxu!nStym*{|Uf17265UmL^cY@x zwwgZ`DWvJwXPu0fBK#f?k3ILT7$fCaNj8%*$T6GF#2lgRKAvHK|T_!3!9Q@GXW~} z{3aq2Y0UOPpG6RB9+$sCOkF6ily#pBI3z2oFNaAt&p%4QWKdUULyeeLjO;aG6}k*g z_?~7JL}nqe65~bwCFE#JSqOPsXLXS^Y!Ib{AqVDHQ~AZ@Lo<*kh0N8?;~4)NVs&o- zPHT;Qs`nbEHpwI5KOoIwzJT=&pThcU?JIdkO}Z^&3{h5yp->E#kMCUFUHm7~@yK2z zUA+D*UFNUC_v4-D$q#r*+#byX#(R%+-*gooJGAHIc_CwmD@`f$h^h7$O;yr6ihs+z zVM+&r2_)_Q=!-<&UX$0uOL2Q<+DIIndW&+)dwC9JIHJRyQ%|hzaM$q|39JKGkgNuw zgR*jw?^J@=6p23_NZBt4cl~(}yXs**z$^`AlBp3#7;K%3ooXr}W7B{lJ7aL@wJh#A z_1BEyZ_cZ0%^bKgEJ6D1ttMjWIxU2-_c{w4dV1EosqkQ6JO2lJLEvG*k8 z=eQwhrh}*;T1Q|1arDIQPovEe zaRS8I|1I?o6uz%tag8Ziv(&5ReFRAh^_EU_{ajT*Q z!$9;sK>iKade~@QSxJM}U!;W_xc7|bnTIX`pF|NVES2mRcp?Xo5|SsUXkDK59sv4_ z)oubKT$bG9X92|_8UR8&`M=EmD#bS$zqif{pni)(Ew3vImS*cGxRfh-m%@l&rh{#2 zZ=84CnG8j!6hM6@mRCOL+MV`@Fg>XBLW3aoDH+@*Hs$WCw<`yigtAV=?}9O|=^K3( zv|B32T6+i#7j+7(N5Tt+z&YymYWs=r1(6L|vy?ZhipB(P0y;{1i4(4d6%;cC|D63J>Ldl!BP?K&mb0-Wr_q;7rwzUI|i7DN>#ZmYbo;5xS|p zBa@ML9c+Nv8^;7Mf^aZ5Wn!Na$4BK<#g#r%C7 z5xNq&IFn`w0vJ$FgIn$0fKgx9(fT}$Dr&7vr=>%U{6*8_Q zRwuztRlp!~J_#DQGoP^_%ia6X3UNf`Qkm+C-@%#F4Vxv0K1=y3f#`wgE33h% z@I3hrrlw%4g&WkRD4E4Yt^e3LtSrI;y1J9DZ!Pd;zT!PqUcUtLeOk0kDF67GE9Wp)!$z?a7IWty;B7f^mL7_To%jd7&yk;M zQviaxq%Usb`hnoz1A@&N{DS-4uau-%^fO*%QfauT4$qaGMm9y?hVxl&5Jm>o5_0LI*3z#b)j>0BaKeGH&;P%s#^#dGI9V07+ztl-DHT@Bs z+`W4?IB`J8URvy2PJ$4q zp8Tr7VKw#|?N9n`kEiDD=LQElUQ)aF< zJUh7G)vjpuV>!Pe7)J2q{cNuac#j`|z0KiaP#$)i=&MWHRf;}|73Nz{K4D?>H1b2y zO)VS9-G4jr>l18}Pg0pwd=~aMZ;Dy{_~Xy8y9j}Ge}!9>|C?43O2%HzC^$UkYGvt? z--aI_RKB;SrxWA&(0lEFkNxd1YlxKzu;4bgV9$>891$z7Gn_%4C@ov*0FiLIjPkZQ zML8nM%oNcxYt8`7jvP#rswcPm5;jOkEc=&?Hk!%i&7~$Fo^;U+MFuHy|%bLQ9Q3s1-E$l zRn%5fvxl9mW4Co8H=K2|HxF3d@!$9ycmlzxe@*W;5pO$SjRnk=RB4o|yn8zSp&Q7B zH1l~fn*yv~wxn9W)ZNWO8jxnbCw{AB;yDZ+zpz!G zZ~EF%+c$WZb7%WEN9o^@xo4;equ0S5a&>ORI#RwbGegVp@X72D)x7uM^K%wKfoja2 z*c?6~+1lhzg;fX8=5C49*V(CDz)y6>Duf9)Z1>#!*R&L+{*_cX-U(JtpeTU|?-)zT z$y}xAe=2`9l(g8j{FgcnLW&rpEDxFndf0F@M+)0RgYls#j z>0{c((QuG7+uGrc1#O`K1@4!r;E2e`rDxX_t9^@lOu++vOo(EjcCVo?@xocUSWXE| zw=fgwEp#FD^U`%Vd1O`N-tX5X#_&@ve^^6&og1K5`DJ=8G8bzN|Ji{X3{g~)y8NYc zKEOe7h5{naJRxv$(#b07ueG&(V*z#Hif0IW{kB6}!9I&Rgnt;2L~6qrh}y+C5m4WC zAA-FYdvs_vsZ_jx2Zf)c4s^f>M;IbK)A<)g!Y?4`S7z*(D4z@qNK*jS30)nc2vlE8 zXIu|4tVs3Jbr@koc3LE6e|GYvFxlrh&vJ^xB$KQem*bCd|JD!a#9uLPI*{;CcvR%- zajX0L#Im+_#+zRE$tBwl3RY9SYg4UFs>Mcys?MtIr?BDA$AWIOdQQEi+=bH(s+_E| z5Yy!>4QXn>OkNI43n2D73mKVv7k7Uljgv2ABUQ=i#SzgpM93~UjU$rwXEJw@k=cENy+cYxkel zIgPulX#4r{!$n+&U{(4gMe0d$Hgfm43Qy7Zhy|D*?I&JTsz3Cl*O)z)L?A#AzA<~@ zmF(#b9R{A0uoOty9E#u6ncA}_E*<$NZFa%Aq{7N$>I0OYQ~8Ar1IARnY5`{r5nI@w zy?xzsCytlmY><}PIky`-7r<@_4DkUyr6=VD%Hma?luw!8r^R2kT|?~OI-bNltR5EZ z@7o(m$rVtEeHlo))38xbG$#(@znW-T+;JuMeD}=fS9B5Y>_z%f$dU%1#_!EOpTXKMp z*%M2{_~6m7w7$5T*?e?UI*9Ml^i}pj?t!BPIY;m6{S3CeP$m}9xEBY}oP%P9n-G?? zA{8!GU&u$r4;5EMvqs<`7zn#M_kCJ7n5-mryArj~`kng>*rV8y9amyc`1DTTUmlD> z)Igd-5><*|`^9BEa!}3xL(Z4!?|pl5y6SXZZ+ z*Mkt6yju%&NKd1VQy(20E9mE6Jl8kH^NOElN*N7%sa8jGE5Bwj;8l0p@qKuQYqHv2 z7N8v@Y+!2+&YIF~u)?P~qrab9K|U`_bo2YzcnJpBGDGy3Hh_bbNv&`uQDS3{SHsq~ zj=P}dogVL0<`7^V>);dX@*AX5Umu&e4nBMh`3|FCDn}SLDh$h`d0u=IJ0Xd=Peu28 zhCd1T{EZC+RRewSYKlPsJOXmdI@_qRVebR!!Uk93n*z}v7k+or8fem2G}U}5ppiFh zfXjek&PKY*5^W&fibtE78nspH0H2T7`107{ytHK~Vk6Dr@8(Q(WXr#auB{-sGyQUh zQ@Go&FDz$y2zn1q2e4=jxwzy8h%b^!eM;iUBuOBjA4@f3kKz;bKjseAFXA%DP;K9{ z@7n0dXWG`)zEvrw-pwB;P(vS3uqwA@dZ*R*c^74oXgBeQ`9837fHdTPi|uZeC1|VM z##RRmw!Rw1jP!(@MQCO6OAZwcNLrSd;+*c?prMk!`f-UE7TFctS@*%;$IdHY$P`J<@x<0pf17=!c18XB2y%K9!~1XSH*JRn zPE&N5~X-xU|3jj8kEffXqGDH5wmOC8B!0Hqv0xS-H6+i zCk*AWQNk4q?84?Y9_vj3@5ujoM$=}Hbj90!FI};7?%gtcw$`eVD*De)Aw0sb0H{Lq zi+!%4Qy43Z^-JtG^-HcVOOue7v-ZMuRZ(0v45QrZJKWeva zJeNkSLHNRtoN|D*?d_Wfkkacaec#h{olymt+Rq|nsQ-(Vi_BAYa%|w4_a?KWijUTO zHGMOT0+Z}UZ{&f-=Yab_?A_GP5%!-d32N-kB3C35WyE+f*l$#3lS z&=pV(0%Hv^8O$q{X4?`UJnbRFxhwFfxn$7*oOz_OTVZcOZ7i|qhO4pc&o^!$&||A`G~jbQ7h#N`pQ1JoZweo zNX?z+5Ga5axsOEo4=K_p+T5)xh6UF3;l`Edq?4*}GEx#_g=CO)=sDPXvI3GxI7jSD zG~5eK|FV9JKw*&5d5exv8X`Tv&HdSH^I$PriQn$S6W;@(Jp}>z7+vfEyAM>>qepaV zKbmNxE!YZn<}$a#enholmWr*J8;7`LJ14BLEXwzT7J%c)Ctc2iPA7YP;nVv6EWnwR zEGfBBc2z(^482fUNS7JhFqc7?)IX=sy3bh>>aiFE!HG9RViMBb6$MjQNQWzR$q5j^ zU%j@5hLbf{0m!W*jF0CG0>D--Aq*pK#%5^#hVl7P)ai7b+ui?q&jqv8JcRMoULBoT zn$IP){-D*_nb=QCf0%`fvl(}2KF@pROJ^vy!<1*#f64!Tb>}SMdHNYu-mGdVUbSHsuYT;T`RMa#L#CJL;T#4P1Cuij`-RjR9 z1b3b7{zJNuz{}7+*8vMD%f-?ARH(+Zc20Kfiz5AQraF`ekC&7zRz=)RT!W*1RuIiiDUP zNXcQ3KI)8;NQ~!Pzj-FxO^a@Y&_W;M);5g#+nB4tA9DS*5U0TW_7ucSCu}4f55hmz zEUNvoA~y!4weH;zm!E?5>55VGm})hru&&ttoCCD~EpcBAFMKj0v0RD6O{J>`S%Bk% zeK6UphW2=5pyEk9Ji8jV{%7I2)jq0-C7M7(;vO~_&HZYQ|9f@m+&SA3M<$gD2%Q1N zLraI~wF+&wpl@A-30LF?hBo!YEHb?1Qy;&IQ``qP5bC=>X`_oJs;Y!po;-A=b{~$h zV;9h|G4H5X)njdveVc?|=5I6{-eHw z(2$t#*{mto9Rahg*c68!369Zl%?I&|Fre#LT^lOc?}Fg%8wTA?Vv1}=3N#DJkb<8Y zX@WCx8id;A8~%no{1CXL9H_gQDW6nGzoO;ae1dlIVn|C^4-o-#>Cyjny$exe?yp_q z&J+9H;+NR;-pf@$t&s%>B<{NNqtl~~cK86Vqv`POS%Fd|I`wMj<%Ar>UFN5j9S3Yh zjd)M;Du3mjFtEpK48hzV25&Q74^sZ*4p)l3d$Lm$MJ5$H7Q<~reya-dm$XgCDTpH z&w8dzF+y1cF^tubr&kT9sn&9-iO^ykc}fU=sRdNzlm`(iD;MObYc)A|1RY4z&ug7l zPw_F}Vgd#Gi;$k7-v9dr`8(Tpy%i!u%kF@sI)ZK%L8vvFyBmx*=A*0Zhkt#d8Gd@< zpqv1MEkfOIE7N|*hk~;b6!KO^?^xO>x4?6Nq3)Pt=;WYL^%?mBxU3z~~3O1%(X@rgyJ0anGNJ zpSEb>d9BKI$M(Cs5R)?{j^yA5QQ4Fb8HwOZEP_r|TEJd9AY9)EWu30`*W}NZ)YSbT zW|hOIRNyd|49bo%Pp$eiavZTp!R6Aho-G-IT|ij@l+UK>U1BtM<-(9@^ruO(z4cjS z#hmbXvMq02Uf9si^|XaVhAb<&VDVLH=byRF*2QN{0o{t=*ctq3-Q(ur zXCw~OXk(N16kv}Qw~I75d^c2_>`k&Q2e7TSV(v5P}XSr3rBY zN6*u`U`b@?T;z4u;b+V(`_Sv-v$RsTgKRYJ!E)!8oknb{XLn-D*fJ|xDXAeJ=^QAG zG~w17a$Z{h4=f!n(9?Oe3HYH$l3_laCf!}~_lM5YUy?qHyy{EXb2y)Qtc$a3FXt7O zk2@HYUii4&y?g7FTnRIvtP1;pVbuRZ?p#3l8#o!LWdXQOSL}vmkUM=Yw3#yY` zbf384^(h!0Jm!df`}Ql&M{HoGCLarM=3?ic ziGQ1B%3<9mjSrxSJor@P)C*mIC4sh)tC(pii)m0TNz=Sd>4U|>f!=|z7i5(i` zB_l}!D_mFC8&YaNh;G~;J|9)*1a5m|)!L7NdB2C;W4(~#c$1naA{vf3J$T<4@PR8I zkkv@7IQN>uCpROYfOcA-PW(+b8K+I?bU6opP@NEQ}8J_9;7RiWwr1m5+~-~rD87o1*sT|xY!k! ziTqM;e1&Y0UMC5wyjpTDU_!FlQu#LpsZIy(xOVZrbg5#TdaLJEp%`7J{L`=+I+fED#F?|0+#cytuz2qKo*B%{G8*6W z0g8o0{a7SP;rxMZ6^L#Znj$f0CnOkbXoYi>;u$6?Z8z0vNdXO(R7?H#(uyYtqczkhwQPR_9xBBCPWByqnzBx)E= zzpk0=-Fds-@l^asXw8ugA;9^({0t9TVc<8jZho&cT=&e@!i}zB@E22lzr#50>#o30 z`S$bD(#-Mihi@3VL`KDZd)6rnZB+NwoVAhHqT{dw-f9fYuh0wa4Za@40Bpc3;TON^ z7*R^p56IJ4grCjXe)I&JvCS-hJiZqA=?iGweiEtH-PWT2A5G^T&h-EPf68GFWe#Z$ zVU@BDGN(*VO^J~-5Wm%X;v^YOUfZ#Ob<>sGS-{u^#>Iy8_l`q4tfu9hyI#EU`9k(E|P92MeU zwY+}i!^0tzl^}mK_fYF5!6c?6HRAe#4@Mu#NXU(!b^3B*W|h!SlK7$&$WyQvMl-8# zz9qk={zB`0um>9I&%=*yaFWunTfW@aPqIs&`=JA%juKRPcHo@~z#=p9yyJ~PJ++B3 zE&p*QvJ;cUD#wyaU}(Y1gzJNfrm;wa1slR(&9Gu0fg-;yLdzE?l`V(_g{Uy0AlY%fsvrkeW|vK%!>==v4GHaf-0u@o7_F+?M!x@ zvREfJ-Yc)Reb4yqYv*lBw|GRommt;OqwFQ$xY>M+v|?4>Me!jY4EsY8aXGf~xKqWA zrr|PJGymWjE6-vsev4e%F<9w-^|T$vHShZW$k-FXHhs3StXPr9{-~m=ehg?;G`@sI zbhlSCqxBFaj3}u3LJF&OqA5s7{S4d5?qcq0sbZe6A%%ow)!4Szcz{%Qp4!^xX7$20 z=tKGbhrLtWm+S*7AGiKi_~{u@$w(dqwJSgRIO=UL^aD#c zPh(YlPa~F7oCL!w7!n6VZH-*XZ8|Zwj}ZZ1EIl#zzwkX#wUjCpn>aW9%iOeDwO!%p zu97$0xG-Up_XZi6zp(8Ht0@M>a21a(N7x_jLyp$s3m1QT!|?jLEBXmN+>?4sS(7i?5xz32qi}z4PACVFaSZ*qiL&Y(K+7 zxu~!5{>(@y%@^SkTFGtIcWAJm@^5?l(iTU|Pt-NF4~^XzgcXnW$x18PTn!jO=8X6Q zKH2BKp$W1(&9eKA60L_UF?UkAA^bS({kMt#_-}D0Hq5SW$!Q^|sm_slMBw}@AnpF^ zxPXoc%(xub%GUj^!he|fcsqjPgP9o*;HtXimV|8~|F+BLxurZ=7a${%OH0a;ZKOq) zUW4!*$wscN0<$Wc-8{Ylm>!_@7_ztH3~OcjHY;h&+V*i04}Q3e0Fu?Lle)Wq3vy(O zd<_*eGP6J@75q(TJz@J_wBwlV&PvY$_;*dQ4-1RQoc*vCc>iD?7Bdswl10A<0}%YO z!`7K-Lk&@2sf1mtGW&rITNg!Z;7MlGwPYANto2Of)0btNGLq~fo5~}{2VtVI2dR&; z?kQM!GxLPu+;Yu08;HzB9(mOQ>EJiCU(b{nCOur^npG`G1Exj}mr6SH9nWL_ z4KlKgFY|~a?BnKy=kRR%d;@oX$jwAK{U)Db@(y&M@Gtxojx zVj@C!x73JGC)-pi-4e6fRLR1c^mV9Bk+Ce$ssE@baF1i_?8EfGhXcqXsBeNoXbj)S z6HS}oH>ueVsD2^)OS?N>PlC3SuwnYT>&3KYv(uL9KT~h5a~VG?;$v!tlDb%l&ECcHE3mz2`gzfi`ScfNC^x z=RC3#C(Ux~gH5b2@1X19q3t+xON?J}^GbX|B=Xc}nUFj%vBz^0i@8<7=c3%%g^>Q? zOMJrxq+~LDSY*0i|>M%NE|4dqb62YSHWT-<$onlyH8QV zBAhFQ%<7(*6|nYRTf0XCI3~+z$AHIAK3N~GGK7p%{=?MQKT&2B&Bk7rhv-p!U`2+7 zp1M@A^~up>z8u8qq2ps+VAN*Hmnt|R_tGPrp+i4Bn%K7oSZ#KE-sTlC24~#kIEY zDJm|3^ zm(q&-l&uYozJd5wJdOIJLh&8cDZ@o8nsl=%FeVAA93NdH6l`*K*e$N9`Du;MF+XS+ zF0%}A%HxAd zO<|`=sJ`<$q=M7)H5O+kw5BGX+OvEIgGs_Vw&iO4T!m_+VQzPecFdPeZz|$gCavkL zmDaSFe$|Bv!xJYf4XP3u-i3Uk@S_V!ZxCDc3pIBThhEuPydxh^7BDRk#mUBmreK?u zBy$}r=u$32jvQL&h>5tj79vmIvCc5kow^MsX7skZeUE(@49psD|KB{-iamA$DcC&j zDTak&pvE@|1Ryybzx(8Zc$`lZ+0;Nnyt7}I&taUmMESjFN9Ihl63E?H{>dK- zdKyA`JGYXc=-)LSSuSj8%S}KO3(#sr3&CG3tBw}lj7-*kBIsfOXt!<{LT^Z$j`x?u z%ujy~@7A%}qZ<{ZqO>{tQI(rCvUjyD*^=D7yeYjl+ZGAszEno`W{yNBF9XJ;f$B%- z8x9&hFssi!a`|w824n6zz^@B51tp#DjB+|zftz@6pS}roh?D{@8=V0$=H}r^h!;@2 zvQ>7X;5^+1Y%(76WDo)e*^uMF-!!i-jQ+q=edj{e@#T*zcI-O)%0scym+CHuy$S14 zWCUntHY#=D3Sh=Gxl7?GBh?;*>atn^jRW@|sdP4J*}ln+{twH-g5ZB1eCRXkwz2QA zq0%j^a;~5gWa##64H@g_B60tZ4^PSa$b*boEE|ze5il_w z>ug0``=k;KUfr;J9O>b{9Hs{_TRMoyuA+PUD$$>;+;x4L`RhGr0f-0aGx=o7G4Bj;q%Nz2amIcA>FW^q zLtUU$-(Oo=TH4!-=T(Y=s9Yw@mntGZr~m4EZ-4nixcLpp26^L`^d%fc9Z!+_i-ip)| zY_y{s?Q-X{(*okMzm;b^9$mdD2oDN-DC`l0u_n-2Yw1tmQBBLwU(8=zO%*|dJ;W~= zW8eezNiY^gdlnlb9?QyEma+UYeTrwd4?BG;o`t59239|OyNMA3l@D#(-*F%bD7Q7C z0Y*OaKTWZJ`!iXMmaksDx+&7d4d;TH9@seKa9_`rKWy%iW;kjS;7Y5yQ0qH8MQY-w zw-YqjX1QSn*~7=8IElxVln~CpBP8(Px!z=y;@C@XXk|&h2D;ck&8E<32OMDn3=`*2 zm*p?96>_$}v1I7+T91B^yP3Xlg9p^$hW2Xz(yu0FmRG;PZ}Bk)`MIOBJbdu&)u zP_?ZBbtFi&@Hc(C_#wTnu5SF_i7bIuchbxwO*tlAc58O^Eq!lm^{n64o0&O}s#sUP z`d`t_!jgp@Q>0qib@{ABcbgO}@&QGlX;UNuTk131x`~Hvt?u=5&JC%*r&q85zYGnT zFyS9rA6C8oHNAK?vkY?v-m>~QGU+rc1~OrZ;XJKB$bWHr$TYcV>u8s-sXX-ecC(V6*O$m_)T;|e z@g(xEiQQwy-RZvuw{6ZJ$)HUvahdHn{y1Eto!v?H{_^7I@bxGGrt$>&(-7Ay6u(5L zbJYbJd(~5O8VAe0qSOs>k?x4kS>VEZpC%ZhW6sFuk)_6oVi-JPyVuZF5WwA>ifBlK zYCSiVDN-}2rD*B3~NaE5;&|8Z)adLkZ=Fql=%H0lU*hz zf{kMOR?b8r`3%3z&b}M_60u>iEFuI2TXk7F@p=9ojxi(mjyBxjc8S^s*dxAq**HnH`N2`2Dr)(2<;?P5b~Lym~!Pjq&9wwT-y z@bH<;Jkp9Cz56R%TEyv%|Lv+`A9Pz_Mz^-$F(;cxD*v+Zyju^xILE8}Ig)`7$aGNR znc~npcPAA5MhyTZ&)TpFwhyF^m9Oe4qUgj!ao)1%boS==&^?u^XXqH$3~-*>N(Qd? zpwDR!Xec}Y*>_M=j)TKD13I>RprFio5C(ee;3mrvVoJOZpR$$n>coY943*ovMD6ue zqz+$lTGrWs>O*an#Qnt*kw6eE5+yTRA%a%Ehh|P``--dVPd#q2*7&|W6F~fF$-D|? ztI_Lc&P{$?xAn~ah=vez$k>+5PD|qyjJBYEMU8LqbEIhx*_>xzFh6UjILs33)pT(I zP@PCfrbb?Sv(jf+L-1y)yAAW1O1--c#ya*j1%m)Uq|cxvUo}>Q3{WYs@TeRF@WFBx zAVK-n0vhh?YDfRum&A}ok31@EbOrf;CakwyB`xhNT}G@v=|HrI)fT^+Mx+Ub9_*xO z_R$94l9j({^i7eK1UceCiIh&p3Y^_((j0zwgJ5QobDYzlkd}h058gvf_50gF_SA=q z=q#%-&o@yw3Qhw>`1s@M3KsZI?3(6rL&%G1J`bI~+5g2_zA9f(`oZfvzdMmB;8>)c zWr~{t-j~qLWItcA>cRTG{U>x52diRuBa*Y8Q)dfy0eRgDIHQP2hE<<0sAr#XEw8^* zc}?`=*XJxx*i?X?j*d>JlqXYSqF*SR(Dawv=+GNuYU8kv|F33AULj?iCnhwm{7M>+ zx-Oh6lsrs7P6h3{dA~ANxj4iT*x>wX?cN*rXUZ?Nn2pcat)Tw~pA0^Q#lY-6nVCMM zo3@}*sew*sO)T%M&DUa;TZo;~g0~m7q3dl5y|>7Z7xVRMS{i1yvygjhYdkNqkxoFu zExpX-nIs`C5_ro6Q zv+18FY)e-632`j^XZAO}3j!fV=c%cf-yW}dRD|L3JJxN-M*0TirUi;>uT6;H%NY@R z>fc43SGLy|kOq)*o*q*a$Jft;j#ko>`G2Re3<$m&(q6KC$r2MW0f&(HX$rY;M*V_9 zZuS4dGIF9f|KA?d% zTk{oH*q3%R#vmaY-{(t8`ryv4l+Ebw%1BLiuy6%RXp-9hy5c^CC3jUnRws?_|FBod zQ8-#W`^vPSd8rX5fDGQ+;r`sidsbE&#zf6MbQ%hYgi$Fz@+m$Z($~ILrC;|t-d)S< zHo6X#lS#O?a|ZAK>eRW*TaFyFZMb)2nP@O$QV91~ZESNNF1Ms5!O-FA5y&t<(rc|5 z+}R01pYwk%bwYMT-yFo8j9sYDjPCd1YO@NHLk_u@D z{30ED))z+M$pJev1_LMhNj~8bhoR1PeU7kh?c!y>S|pFnMV^M!4niL$IOf97XbI}~ zp*r0np#Z@!rVd9A`5vxqs9(szC1IM{4@O?fH{`pH&|X6mp?E>fd$wlh1|V>3du3(i z`no@;zGq-1@p>xPB~pUO>IX#)Ff$apzTN(8e|HTyS7X800g|v-_rwg@DUaxMQy}A1 zPPtk#HX_o{^$6kih5~}-_7n9>&UCD?IU6jv$wNyGxY)$x6751R>x@hOx<`xB!AtLo zynK--Y9dJNs)?t-g=!*<+&!{gyx>JlJCJqupz0@u&qdq23AtSF*#I!T(cg8R4_?ds zVd*ns_Z_2XmE4O(UN=zJ#W08Ko|ARJbF#8z%^^%J2!ORn2H^HHr^`)Z?yi7 zYOOXz9B%g9(|9 ze>y=dakowl%mQOck+V)Qvg~E8@5IE}RrTv->u~DIlGmNoO}{$3_kFXgu`?IvzjV(F z*T!nBK%EdMu6^(TAFQ*XsEvOf=h6N(9ob($(-3sU;hi(u8M$j?LFMzTZnQD-VX?EyN4&q%F2?w zHd^LR{-&^bURPEy;dV~~26s*44tgSf<7p~U`)49eh*)??9qn(TnuVFx&;7GH%N zxs#$Y<}rKj&n1sE*9{s41L4m`RMI4^!H*dC-9GJbmNaQwPtw z5!2QaRIYcEzJogzwAqaH)z_gxq4umJbxm5|sHyJD2uAS&oi$rpj)Rj8>fKLV2Zp#) zdG0S;vwlrXJXPi=dYV!$x zPQy4=E1|<7LWf6{;_rsOf#nMN`w;TTv2i0aGhFp@RI@ebAJg) z{4Pz_R!7g~O;i3m1G~sNL#uThLo30nEc0g{=vuaie2q*@H!i-rJpLHCT+**Mpg znKv2Syst(5z3zR|d2Dl0qd({8abg%WqIfFJ{z-U#XVCuAQpvfF&0tek_Q7$);>dpn zEcdJFr!jXRC3L^KJ2$fc^}AFN>{nKK9y=4x6F2&;3xcrKSpEm5y}z&m$%%BhaVp~O$=)|^<*9&WC{tdRvpCu9n^?VZvxrAi6!t^ z88g?sbg*v)ceYn#?4Z{c=)ukDwOLsB!!ah$k4}&L43R(HQ|A85+6N zISYyZ3~b2AD}D2KekH`Xnl*1`a$TVQj@FsamIJpO$L++J=Xi%f^|{DA3pyE$eXTL@ zsyCkQ?=B@(vIbVQ;fH#E49=fgSHC?zjga?Y>X8RFeNg_z#;Ey^FNq&}RuW%kOCuRY zCDnKP5AehF7prtDzY%CyjPKFf9k*m%!KYmYkdnYxPFJ1+00z@Ji|uSDXA&5!L8BMX zdUYR+jDTxVdo`hVuAaEg&$pgVE~Vd-7J~9QTy3mwR7=LRl#BX&$#lgm6yEHYaZn3C zJL0zavSCxoB@;^V$tlO;@K@u76*i#ek65cAU;{$$Vp&Q1g&Aj-fs@(g1Pc}EwGUFw z*|UEOkIS<`oB}qxCBv@8{wewQ$!AL{ebK(WfMkX?bZ->z6#k?V?jRC%j{@y(`@0r) zEyX7XbaivX!g9lZikuYfmLzpEyaXMi)scvt!{w|+!L4s6X!;mOD9S^14?6G*cNViW ztU=`%yj+tV^XUYohoRpid;1JkoL#g-N?`9iu@A3wcx zNHK0b9NO*cA>Hb02ONY_e6)(Or_gQ`uY*U{bn}wj znnQ!Qz9&<;eLC<|oCcUz?lkPa-!qo`*e+*QuQz%3*h%49w%>_E-B8<(i6zJx*cMFs z37Vf7;C{=9yZnX7<9bSMeTVC5j8s`|2B#w`gLjR~}d#*@Bf`ih&T z{yw8rav2no&eA_aFQ1fbZJr7)reO;2+{vPn0m1)!1^n*%)#GHZ5kq4nk{O2#-RhBT zb*viBXG!*6jWc6PsH@S$M7C(|$r z)@f>NK^wIuZugUnbo?(eglTTlQGsGRfyyZYbpjxdsBZEEcpQ>}I&(LdWP~VZ+4afZ z7!GJ7D#F($l9ZaL$r!#IVq&&$g#qj+$_0IYR0wN?#`XEGpC|fx6fA^Ev;O8B$yVT8 z;pPVEJZpqnJYEn<GB{(*JFe=X(Tg=86({Y3}=Mv`YF98}PHyS=Bz~587CE=j+P@$oJ zye`X4Lxkl!ymP9B8KmDp$ZVcIMmq3_x+(Kuqu*$}ry0M!wB)6kNgSgI!~OOPXVg~# ziWa~tnl(Sz1J9=@ zP0~{RZhimL&oNQ#2Ystd6ycbG{al8bN+eWweC7i(49@Lo(#Ym5`Ag7M4t%D0(I*Vbub5>fD$e#seIUe&Gv zZIt+JPWQ8TvWTSlxoQ}p@yt8g0ezdkL*!0yE9_eg?Q@*cIJ&A+$ziX=;^=-yZzKsd zo{YN73;+zy>IlAr3}Z;n!sE}OkL;r1KVcynGCs`cX2|9A2J>;*M`YgbQr}cRzLTk2 z^^oxZ=h@ofQxI#j73k%!61;JCki&m~Bgsd7_>|mHtdUg{q~!g-Cvh5^Y4eDa(5hUw z^GF8v-$LjXAFDUrs!{nam@LE>;J-}u%$|jd^_=i9oZ}*?MK5v4+p}wc>_S7jG43Yr z2e!ypXT#vP(*L+~AmF*R{A!QuvnCN@&rP+UMJBFo=(g-7?(ygQ*|kbU`y8)ZFZLbv zw@A-xVyaVvM_-rSx3Bg1b(J#JB{$Y@Dbfx38+F9&<3 zh-=~bQ0z}Zb4727L|JP;JABjRQ+R<|!~-8ySpU(SJWoZv8xirIqvw0;UY#O3ScflM zdpIDOgpC;EocnJf%8WZ%F85nVlY;+`Dz%rR*={1}UR)9VzBWR>$`{$@SX_LhcQ*El ziQnl-axIEO5yNb;?(+{RobtM>w-%T7C~ZCE(m>tIkqOsG?nC(=I}YwgtaLUHT|Qed z7s7FnPCjs#e?9&~qIbfh#oV>oCa4A-CcREt+ zw=bE6j&Zh&dZKr*S4bOvcli#c%XjzH{!T$~C9*jhLbW2$CJ#s<;KXDQ&pnUbHG4y^3TQ-dHH+^nwX0gLyjoD6>j!P zqLTF}R^TYEZDfkDIm5A=zO_#_TG8BVVa=QQ5=F!A_yXi=QS1iu~r%ogh}@*L&RU!~;X`A*fuTp} z;LunB<_lv0df=-QfdD!WSipOR*gulqQLthBZnYH1{H7mz=zJ(4MZmG#5`#5Ja(1SD zK^EgPArU1r|K9%M?}J3>vZ_GSMDsmmm#7^R zm)hcJK6SI^Z`;0LqVhybSNHq!kkHdI7ECn4|FCLE_*v!n6L=G;6P=$R@Vu~7SkL&H z*gXyQ>%4A^qu!qynM~Qk4&@V8^a5Cq>4?Ld#15_8yV`%06nQOnG*U`575r7Q$=ymT z{vHHIeT>$*Msy~H+mJe`t9R5s{JXcTw(y=}`+U0E); zfh3ZTi$zyY!ami|7`O*V@}SjsF)y)yf_2W)%DY1PzAb-6H7{n*2WAn);5=~dtpKzUf_upS z!Bs2>UkbC{e8x^VvgydF+yo;lNVsVf7LAB}$?0^>TM-%MCGDd) z62#UduhcJas=B$uawjw-@t5h{17_&3A%8nwq-bJH>ww?rso^gExl)-y7&DN>*FVbC ztxmI5T@To4LYr+8V-C9}dz z6bi{?TTcX0EwvnjIASZ-MvZOAvjWg41{H2&@{y3;m63RPpz7(u^PcsIuDCu_;G6Q=^8Ru||a9zC5xyTGx>Cl8ih zTlC~epV;rihO2kuna1ee(MU4kp3&|fkI=# zyk@lODcg*1+sS1X7(^jCitqJ$mRUROHm;8a0*@-P5$aK1rVtvPG|`4ypFKl$Ao5{Q z&dE#eFOJAp2zNvd-LNs0ETjvgeF|y9?Ey5}>=u~%^Odl?4d_@DRSe&8m7fXuhG5|$ zfrF^<2%M~Jz?TyTG1x7Yq><=YsaOA6RrhCRD|JJ+*9z?G_gZ!LmvwiSUhU5afoEm= z`g(g0f?*ur8KSbO)CpoTkpCtcm#}U!7Wo&}+{9$M7PU(l>fQ0tc)+T6T^Xt}0ti47 zr(}!3*I3>+99uIzJt|@-Y&)m{a&Uwm$O$HuOpLd|X2C_?a)dbv(r^8yFPt+{7d%Go z-M)o^rw}i;e%IX(t{rB0nIL6^u5F~t<;e)iIg~+9RN!qtxZ6G1+gwK&Fn&(~8c;}J z=r)`RjkwX1M@Ijr`CXKCcjV#ye4kk0E0mR8VE%Nsp|hGh%^9Ah$2){;<^qJKa}utG z77kW;H9hQmN&dGvwFyR%)pwcI$xb&w4+CcBMkOmTZD=35?vbor%!zvZ%p<6@(yuCP zl20*R5oP}{4cyahMK%O-5)40D9hiY(GOcMxavw=Y74*3N-o{}FKO}qnGwJz1FbL~3 zXS-BNcjtQFY^3TH-b#?{rHVPg)AjR)n!SS|0hR1dA~-LOffPc@#@$75h{UUN=Hruk^io zmvD}%cTh)A8-I6jOj4ZTRrRXqWWkl+@ou^I!86g=YyHA6sMb%y9?Nv`NiQW+MvMr#s67U{R}<(A2b4DK`x297}(%m!wGbJxR40_`gQxF)ZqKjFwEF9V_zTd|P+ z#^OM>7+!3s46K&LkY$%HjZD--I3t=tGw9qbK=Q~>KHS@S@AIX#=cd%~dPwXuA0|pl zDA$K||C_>81ek1+)79gUi8qi8J*}{t29WXDzV}EXYy$M?h()u0^|n|Ng6(Xrf!|+A z*bmUk#J!^>?|)t=XB){A6W2a`b|Qn<5@M>Nc;1vNw5ZMNyqTYEpRk}0t%9Y)ba7X0 zqiaAV9Ke2tYAf0r_{4BSRFPyq=1Cjy9 zQ?l4bQs$U=q~Ea2a*P@mwHMZb8cmHv*1T*`Ye(S89`Ht+e39i1Pj!Ds%ULmF;05z6&fg-=Q&=BV9^Cz2ev+1i<7N-mSP}L&FNAKc@9%|f zx9aYU1LNrad)>W%9l9Xw*pX0~FgqWdIr_ct;ppi8Mw8VUK-D!x9R0DOE+jaZj04MJ<`h`QDzYiwv95Qf0X>L>I8|li;Co;|;>^Rk zf*H9IGz59QXK)ZiHeX)5ui>!%{2>&p7jodlnWjLDFjp<~z#DGF{NA~aOu6`bX-J*; zXe=g{FTQ=S1_(jvxVme1qu3O7vbl-A?$>3C)1*`1&GD0RG*Fg5K~W(bn>*%^adTF2 zY>Xn6%fZO5>a(PQh)p*Rv*fMQY;gMX#?qc6DTj3YT}Jt&{(Gs$_4XVX$1`_ zoK(HRkTm&}Sg9Wss&%Z={iR2xqjck15{6$tj?buFba>Ha!sH<<9_%TteQENqXU-mS$fgSkn_=#AwE znFnuhc%E2m3+bEJ*KtihJx)4BJx=~~Ut*P0~(cACl=0sAUbfW^C8% z+e}@&Aa95`^hY)^P48)wqbqHkVhmRY?V3uQF+$=^I?@1v65Ks3h|`++C?E=zMzXcz1pIMik1dD#@TdoUiv zlHl8^a26&L9w1AU0D=$Af z>Hpn=i_6LR24tz}Y1?6Yl+`@QfcSihI`sCLuU#6Lj&t`D*Ety){R3xdHt*1XcbGT= z#B0!UA%t)wk;nBU)ynOQ>-Qg5CoC`q{+x`>37*EEhhjiB-WX36Te;eQn*+3vA1~ld zC}m7kpD*o**df9c%^%|p!`lefU& zB06-rvidGQE>YyUN@5c7BA&OhxK^CA^n!zKSawqUfUow@HVjA>t&Vbjxq%4T-f~}- zVhHzxg;-N;qiB2|NCBun%S&<}%$BONRRj%*gMVG`0z#Ny?FdmUFKIwhge#9nw z>3lOM35z~FK~yn?@F9z5f7OLP3Eb0og^&hdB`hrV9Ne13?2Dm1ybhg}o zV3Kh(a0&pnLcyk!bb7aB8z1`Zz*}FYMnTaOT7d>}hDsYMmUlB?G?JMJ1leYf_B>B5 z%O{`I*tXvvZrF|v-QbnY?R~6~Y0~FA@jJ>(BgGH_utrD262`#tsm7M2jWhrrN1tz7+FS;f#o5<^G_`=4O>bcShX|Z>pjL! zd~>C?zSdR|LB)0iz?x3}hvtXlN&VbUWt6Uq9sqJ4%wMl-H*VaZTN23})vem^$!PID z@FYI;w?u$Cr&@@cP5b`pVI`lkdrT(s`N96Dy+Yx z<`2T?7=&WSueuuuC#NO}GYn(OfrK%@!Odih*<31IG0UBSK z)(J;D7OWW%k3ULj`PUA`*%1EhJ`DeUC{?UCh*|AyU2;rE=7ymQ+g2{Y#qRbM@T3?O z^%WVmS3tU&3xEtcrf}6%hK%@#tU?;X=(%$ns1suJxWn_v0rs~ z5iWh0m$ssMBVbBj*MTO4LroIkTv1k=(bbypyd)gqo%BA6#r(3|GQ39OH2)1+%QJ_iVEI16t7*M=Y|A$(VI)=UyA2wRyY!{9( zt>ioDAb77o{_okJ(F;c+9C246qX$DiKLh*L@#Ki!VyvB zJo-%&^TPn2{{!dMPVSzq8EbMOY4%+n77`Wu@ESp-izB!{3$|@t6^P-K@`SxPkS??7 zVV1eH)g*d#n>&@8Yjj=8wXgV*9q-2nc6~iFlXVw^L_~IJP*JfP4m%%Kt`SCjwefaJelmF85H6c~02I!b5=T<81e#%84jE36Ea33uWmF|9 z6ZQC%*fxOtM-u3m1>>+K`MNxF)y|9@&3+0xR zMGhpXL74^dHzE-IX%90%IZ4l#hF>6ki@4PHKSqLW+Uhec;kC2RwoN@xF|59D59xJv zq*3D3NM29A&$r1}vh?uLYT7R6(jJh;(VmQQ7EPV;LXIo-6^hy}F;MGkEkOkZ#90Qn z5d33Zoekk(&&h>bR&#u!2mE&Z@2x8~ABcTc5d~HLUWNs_$D0^h3H@()Z?n?ZWi%q> zNnrg^CwL@rCQ~d~fb)~Q2_KGNCHgsL1ntlmffcDd>T#VS-~Z$_I9WT`R8GvKt0Z9? zd9xR`T(f@vJ%025v*D4aFolV=Cyp^;odnO&+}j#&bCGM?mG2{-e}C2B$?&G33!62j zZIjorh4T5fXI`!f9Xn3T*-xs=i8(TyztC@f` z!nncGBo1Yn7QoC&fuTwg?sNoo+WBv-A|vKzoXUvrvEQFq1MBORbrEwtEgy={vgiB& zrKM3p6l*mU&{0H>NBK0pGQE-3adjcza4A45umR@+H%(iAWKR$r|GG`<{Zq6Xu%C!E zYrmgIDn$de;n4-nf{Zi!Le>ul?pk|iMNee~Rs=nC*RK1z!tn_WIZ2hfKA5d=<_hYh zO%5db#13II+_B^G3r~*AoLqv3#lHz^q|f~Q^R}kL+WY%TG<)dzGtUq?DV3gEzTdx0 zg}hp~#_%YXel<2ps=oE#a}MAm?dj=J64d+j5j+())_u>?z9fAZcC{e8xi!5tJwAS!aBSMlo3o^d9P#jovDV+)p;H~?>Dg9@;q6t5X&xPL z!OebzS#z;3^d2`1czYXfWSC1QyX#gFUAtA)Kw0c7ka?;~o* z;l96XlRGrKCIr=9A-^eK7@TJXHJn@as061}aPh5a$^>7g-KcHzcB&66T_%4^uH&)n zR-Dn(i}VKqI}%rQ`yX;p z%|88VwRU;B?oh|OFqqFBMmA1GA8tMq-FQ*!SOPXCFiTce=4AzEaAfP%nNJD=NF~PD z_@OJ)2c9-9!wmP`E)T1+P!{140%-4?Na(MK<&n^}!O{B1t8ZDFz|(BZES!Ag?|(54 zaQ>HOZt{1P13W-dGW38M17xz)^22ZQ@GCR3?-96;65nSLI$MJlBwUEy^|LTGirT!i z5tpDFmCzm=|Eah7ZiQ)ywb5+5by*eHHKsI1j|TE4V}5B^NRt}QyIqN)NF5p;MzKu` z%uQcFlZ~T(T-;*U&%(8@bQ;i{Gw#uR73B*+oQ?exhNq=q88B~yo%ec++9T)wAqeNG zcNbLJJyvcvR9+JkIj!BI~K|&;`wi4qbOEY_>cKFCwGg>6p z?a((Tl57I+Sj7ETpzI&*q|uB@!>)$-Ye;YW{LU5feqH9pmlVrXi7Y|*Nyd*U{_`~! z-FNXOqIg4q`uvS5$Yy1TAmhZa91W2H#xo)x3&@<>;SJE3EC#Fx)^z*(rInFv8P2;W z0Y-`{hK@;py(}XtxFKzS*Z`{JCeKKRIx(MlsmJ&1xsD3$YF+U6uN;YXmGoNe#W~)5 z+*Oz3HDB|@T0Bo|iZbuSN$*ctw044+s4!ce>|X(GuxGIU!Xk6r>eic;?$bDn|N7Y= zerVkw-DGRySHSC<1MOfESCH_Q0CJ!?ZnO~H&oDEo90ytTm#~A2Ps=#_A6F?c9Ehsd zMfIx0H*wXF>glc&+rJA7Ctio87G^bz{O=;8`yE4v1hpFv0*VskWhu&hi^(jEI9mvd zZdH^AwbHtERUSsw`p5ux2@vw7eq`ygP>C-nK3M+Rep~(uu8ajTh~Pmtqo4zpMKQ(* zjjg1{@M3vlSf#JOf0|P_PB&;xFu$EHepjItR$N@nJmmCe;4mG5*UE=EDO5ugYbuTv z0Ku@M)i)0qzB?u#xhHKFRnIRwxwO=k&Z7-N&1FAH6CnOnrsO$Z&@;33r)FWRV7HzX zQE(0N;HVyq&d%bkKJXJ(rhG8G|9OSjZE;34LRf_IUyPz)I5x{Qhs=j)vU^9I#NN*- z;ldP_5pVvx7`lA%;mD|N=;~|abYH_R$p_;8E56S-|jb_OVQ4i)%)KM4{o6jMLd|l$)f}W(^hYK1;>lT~H^92xm2gq{PqpWvEaZ z*hksgzdzc5oPPJderxO}udN}KT&9@+MryuDYKyaa5>x2EFyh!2kkk-%1V@PU?QL#; ziaZnn*EkwqMJaRrsu8~>edXhwGpGrLE?hznBo09F{RQD%p1m5T&7H>7#Xba8Xkql? zbsINM?co9LeR)bY?@JTrJzj!ptYlf~8X~t*83K0*L{x3u1?aKycmo|>SuwXxdzJ}s~vVN_k5N*0ymXopbGBE`IaK+%Xmt<3D~UlBJZs(wCD zfBCJsx%qU%PM$lCdbe*K%OUEO0)@AHJQv~fnGm$V&6ZD_Sb7v1y1&!%eApwc=co4! z$p zkGj3Y&HB{_1qX8|0B#4Cv-IjQq2ObqTmf27kb+GgzIx8t|86)Md$__xu*6kT`ZB5H z+r9sm7xb~6fyrfpC325(i`aLQf^q;hCHAT9?HMD4c!_3$pWU_Zzs^(en;-cuS>X&Xc<{EAaYx8u&c2(}&D+bnqXZ*daiu|AA*N>)jqRM)dO(Mfs4wK=j z77&mw596}6x6e)C^x_qa9vmE;SSmkPMlM~ban8#0eP`rNb03tzixc-*!B*m z|3}lAM??L-f7os;F(ih3Xh<1^iE2oeQDUr(U3SUV$WF=irpOku^LxI3{LXRw>71JPyr28I@7H}@{unrmtHo(VM%^@*PYJTf!=T4?gnMo5 zPh-VG6yq#dp@6pkr8ZEMsj3it)85IZfxF4r*cxwLgeyiR>d@2MDkrpIO&eTZDm} zG{DH4%UrUQAYNSPU+Yuk2MKIfYcRE6aLp{L!>J!`wjFkU4*%DEJeve~0h29lmI)0F zEz4K53AeliUNs^Iyo13ii4JepV_yncS4Fnmp*iHGb5sY;;Asi~+!$5y&yu2%XDzO- ziTx@;&Z=z>2pEO$)T6E`Oq|W=PFE4|ZsHo4+uIYFu%WVkG^I9t9fM_?e%0}nr>x`G zmAzL|^m`E$NE|zN9&q6>LVVN=f7SZvN#@_VuH%Y;$;Ux2XBm#yK=(jhQQ&(SNhvg& zeta+1@P8~S8!i{olz*hZK}|7SHD)PT&xu7&_Sc_b%wy5Xe~%`v!dd^&8TfFQPg{|u zV+)8zG!{5S$QGeucoJVy0E-PnZLTQd4;2 zBnt@0%b%LaDgbLj;%1-|T91rElr_Tj%g?GP7K`R?}qHbHKVtpTP zmV9hz-wDrj!#+)7q^n$GFVfhb9t{CZyrMzc*eesF>GXA{%jLBc*(b=Yuw4Jgx4|qA z6gurffS&r7r1(SQ#mS*Fwha26iiA1Hz;#>ySWf)8KcXJdoH^J=IYBb>wf$eUxO%y2 zZ0{JW#v!*}8BaQDHv##Z>8R>bD3m{!*;qKC`wHxt0i7t3iGwk5y>lk=COjr2RhdHM)LnoKzWgzH3X&fLV5 z5;wkV%2N}sEFsBt^o+(Z5F{AIQ~kDl01w| zd{v>xUiy{i_4(HV?o3tqn!!Q~$7ICM@A?@M9M({Dp&86H&q5!OFD2+oj7>}i-$zvfPohL@Fp8W$MP#A{1!DbB z&C!5xtNcXx3!g*_9Zp5hg3A6V3#4%dWATwG&O? zF)8mzyD)^*QrY&T^=&VYtTZI&(pj9rcuk14O^LVRZC4^TFKhCxS_q^WA+=*m@<-od z5v+SNYtdl!1c47(*EYAAn4FYo|2H$kNIRdgrRs(t>l#KQ6o@x8-5{t$BBy-RHUV6i zs4?F%{f!U~YQKv-W+S%JBl1((TCy*)ZSs9r&C8x!m!=L%CV#qDpM~Y5MIv_r zI#&WN_5^%>;a}bA!;c{{<&}x47B(J;Q~cH)b;>#~Fguoo=alS4!jr2*{64M2{d^p|G2OfeGNbhtSq;odZ z>T&FM(X`7#xAc3uD#o6$G{-6ZtB=@s5Ss-5ZvX8+D+~XqH3@?W*wE_n<%)`?fuIrZ zm&DYIOW>$pZVHBz>sI~AFQNGf-nYLiBht+O>xEDbzb#Nb_P8*+w4<^^1rOV zXaEi-e0;(JQJaRqq+(sTHNTb9NmipPinL1Tudww0HMblcd*JQFrH`S z=%iSuPY4-!?GW)xKJZ{+Fphzayh+6;L1gWNF!RT#Q>-!>&<_vl_l8UwM_7e9caDsW z_v@DbI5*X0(S={N+g|KfG#SylSQ|7+2wg?dePYoPBGvKLA^u2;Wut7G+0)ib=o?ORolI2u34K&@3=n2xHU#{7VLC}=LmTpghL=KE)7RN`_A@0#g@~(sGBx;mG?A>D8Y^D z_DDfn0Q|1wkJ%taigbDH5RRs%t{$757}u%%xzeOrptxb4>8>8)Ex^33&+gu>Uf#F& zY8hV^Y&sLQ{xzJdV|2Jbl*7(&H6gajeO*1Tbp~BXRCx4bI@$~ix#cNU>(>i&Vc6Yk z-JaY!9A-Wz!=@E7m~QhR=PpiJP!n;=uuSzT(Iwm}QYW{~j|$XA*E1j3JDQ|gYDa#5 zo#=ndivv!1FPJ@F1{<`gPJN;1fkU||LVxW~+QcOtT7>F9zxbEML=jB9oaOdtTqZpV2bFHDa7+PtkE(!&~S7dV)WY(Ce>B$G`#aaN?WqpCw6QMnRC?I?Zk(XTcj{lqsw^29BO zo^CCbvrtM~Mo3NSqcU5w975{2G-5+&42CY@osk$~W)yz>c>bPMg0GrLI@s8kr#lm< zD&josu*=uf)z#G0+P4{xz6XBaN+;3oRzI$btVkf9^~yOB77`*SRZPtLTQ$Tc4siL^ z0w-O|(q_v}Aoiv^1}U~;d<(ddx{ zjmGxEa0by4?wOP_eqH3}>aV?5#Z8$I12F$E|AF=r6@@d<*Px`ac5`cH!A~tMEqUa4 zA=bW%ck{_eQyy?@VX8*&nLaC~J@pUw_pk2f`gCPFt=2ABu;s2C0N+TMf8oub3oLxzkJ3aTPpGyq{^kbqo#;UBL0UvQJ`J#OeXfQuQD2srd;xi#c{$?ZL% zJoxq$kT{F&+sd}cZGbz&-b|?_j`lwWL`1r6sYlbq_e-ao@u5;X8J3v+iwZPliQd;ki_P_Y&gEULKkmhfAOE_E z|5k(~6NMD)A=H|btb8wGO4KbZ0*+)^B5hQB6T$wlbEwDBTWHwq#BQR}KSHoN7&$x& z3CYD8sA$bQ5fYJ_IUGmQ=h`%b6@#vy@kGS%b58Oqnwf0*#(bs`7}wQ4bH4N|M&=-H zQhtHrAIfx3-OotV)hB2(j9rB7(`_&a^q79=44}f& zW1WVTFj`LOXcHZ}aFu`QGl`$dpZ(YJk!+RqYyigXiy_(}1pR1vM zImM?=%c|fj<*J85t?$BFtWGd|%lcmW*g=P6bxc?X-HbY%_L}=(3$M*@fs+e4hAt%k zT7d(1v~JH!Umd+v?AO%2w_fE0U(p?RwZ@9hj9>r$6|Fv0Caa`RcyIsxDGNt&UQ$-- zRJ2-(b;1%dJQDm%+lAg>pDTZ_A@=0piu6Jsns5cPxve-bDdvT<}bMnv84 z>#-Y;nKm`Bo8m%$nyI{>nFi)%{f&0dmNJy9I@r^aflOsT%suW6b7V zLnGy>0tYz_DAGF;SXu$v5Xy7o zLr_z69@Z~!+x@Trr9nsw|?&$BoJg&as|w8jhLQ9D_*CPe^FpAdG1KVrDlgTYt9FCOK%KJ z3=-2^6A^P8X|_cV8e3Xi4M09odc?DQd^u6`kzLN-9Fw8Zrx@F-(j+O z{#lAHmlNqkrwMiLn=OWXhs-sMmW9_=_DVS&{~4b;?9@1ztZ&;IaQemSlH!k9d^cjV zq!G6MGve^KF-Pgt(Mmm9rtM(s{#Zq@yxhj$+1+?-?{8)PUaZlYM#TC-OtJ*q=dK{p zn;5;REvQG-iI^}(iQ&?x&`kH+uf#obK)m$#qt05MjB-*w=`v`u zjq}PbRgr|zq$g^Z)r>O&BQ->{iC9K4bxGq}u>qo`p#?{HyztuxHE-*eSSuhDy=~;xDLw98zte<%27LtuL2k6}b;yJD zzg^*N8reok#z`ZD!8oU1ZJ`_I=w;-Zp~;CK7mez(2Il4p{43`D)I^L{q|AMj65P(_fPR z8J~whvT%O$ZhKATWzX{M)fHhE@Jr)hO}iI{jjbQg9$LK8wp;9)GpP6Kz1Dq?OdT&= z_EMZ-?+w$-1OE1Q`0SxfUo8k@b1$rzND**(hCyI%^pL%-zAjvL*6%q~JVQ%`e%-pJ zG0sqH7#;7RS3AD!{EU~E*UE3+%fZHrbo^eLk5|Mwvfr_7%0=4N({M5lhMHS0Isx~g zsLV~&ML(8=7kbRBf2bWoP;B%KI3}Oi$cmsLld9vh99G>8?;9JVo~=kc?p5J%nwFk! zQA&s%m0>Hi4*?#*7g67^93{V?L@cQaW{Lq7mEUAH44eQgZebeDVf#U_*bR>MTT#(L zQM25V@qYh;Z%Vlkw2}-VgP98kt?LQ*K^g04&J69!%YZV~obHlMl(ccb@EsN{afvSK z!Vkoz@Xx$0sy^a=)U}gn@5}rsA@%#TKsXEOG;Ww5z2I?|a0wTUUw)8TH*O~>Ldg)g zNSSUCFIFimB=$>r={TWc95S-q7DE2E59dw(-WSV$^Hr5xSoq;p6wY>B8E=L@(O+EU zJ##itHCvHbIkZv{ghHD>+fdVkBP1hd^m|}+(&Vu^?cBA06XU?4R!&3x{okI-DS;`U zTXjR8KqqWS%iCaxf3?)O9Q05%hiLI-MJ?ip$>itqGL9;2HVb5V#_=dYgG3*3AZvyc z)A#}e(YnJyfNYiX%>lrB#_+TwAYNIJ@{Qjc@FhsNCV&$^b)e-xG^c1&xxk7B>EZA)J}9J`wi)&W1TlksmO{0xY_G=ltMOK=l%|&m{v8!c}be#TPd(%?txnt zQ;%%zP#w0NAeZVZB?MA@t8WK=bz&CN+>niEWG zh7euAI+X!)>3x3Dv*tM#0G{c!A0A{yR>_e6PKqbp}WGA$ARfn|^h6YCq|)d8qcE7sBc+wZS8E z&{CQ#W9>r8EP9UN>HJbVaZ!PKwO<>&f@BP4elCu%UZ0B8I79W#)7JPCWC?WBm)kOm z^rSf8AX`#eJ4@)=eOky(Iu#;>8&`FUfr1RT5c*3f8A!m(@n);c1_n+!oX2NSdjJ_v zr6~Uzip)sEIzKTSm#`8Y436S*yC7X0Hsyu$1nUM|aHO^YA{eKXIM>`iJ|Osq3ynM~ zBEEZ&sd*bOdbVr63^xu+?~b0A`6u#^1!j9>wjC|CwYIdJ0G$FJK{j;6d-oJ2|628?MfP;)mX** zi+-oi_^@Y!Ex9(wji-V!sTUd38-lTNeBVdfZd$%fd7Y4k-kW9> zn!h)3%CyX_Smw115CYRNy5q1l9A3Z;t@RxH&n`kWLHA{&&o-kEF$I2)KPuPgCSt}) zh-^kcx`@1&f?~6)b-b4TC?`%fh5u6Mzu}UJ?FB0*b)9o_F0>jngcu5IivIV8NN~Hk z2qvs|T=X=vk{-ikl$srs7oRvVtvu1Wl5_Y*uxUCwO=&=rbYmuYYW+J~qLyfs$Nmj+ zZe%5;5ft9fatqZ>~4wmT}-{htEdwD#6 zTt%N3H5vCQ+4HGj>b*dAZWlA{SMjGL6-yG+-Cc)mnp3*uVb_57K$^j-sJ3Bb@_zm8 z*FE6;2h_H6cw3JS!bvpDt?%91@z1H_rqa^?-T{$nLsJto2k0z1<6{&8JU7%_szAyH z<;eFk+1v#!;7^{O z;rorzq=;7x@9wDoPZhMQ7ywbxV@1mR4=CAwxs57)FI$pDkc<|k8K;P{; zXWGyQgcf)s=v`TuS^q5_ezbu>xXRI$6X$MBgQLsceGfySmPo-S-31k__@w8fcdI$n&EB;V`%F&XD^AgW;v`xOeT9{wnCXF* zJ<$a1MfeREeirY29yImKeo4}5*MbPgo-jH=`E|oKssce_sV|t;hNx2tQ(HAF3G2{< ziwY=b`R$w6G_FkRWDNvar%ktfy-3~xoiBK*i4CJS0qM-b5zEa2{)@fr2=AwrRL0Z) z1Pv!UcJgx-*h3rEPLheq^OB*<0V41mX>#P^2^jNGY{&FYXR~jlJc zqGc$N3S!?2iAb@e?&&ZiYsiZBuO{`Ma+YsRpDDAT+{d~uQl+JE!!}-U{d#K0KPT7U z5O84B4mu(^-2J>!vCC927;jsM&==py;Zy27%4%&SUr9J$JdK7%byB$6&S#jR*8Z2CEfcrZ zp^n zH~#i$gkMZNf96uH1$XxFdW*yQnT(bYH%l6SN6QAL1S4Zp3+v!oqx`~pmXKp^gH7fE z6RYgPTRc2rN&Xx$;WXvA&Q@HOz8C%^|6A@ty^J0Pg?OYVj^tm2#D4O~+-v=ea1j05 zlNn$hIW+g0fK0^>e)GWPl}X^yPe}fk>DK{*j(~*Wi-}d2c1Cl!<+sTKUqGh^7S)*q zArPSwd%`<3r$Il8{so5XP7FyS0{xEk2Sr3oNwdES-POMRQzp>)xQQPq8-kVVs~U;L zoii}g8Y%_4;6aJ)m3o1ptl!D5?F;?rG@BF#bQO0;G_ytm|69~G095HOJJH9{<(>x2 z4i|N-pRA=5t$QdBp2l*#QvhMeXszfVvWkWE1x<>e zDaT32hq;cYq3E%S8-YpsSAKpA%pPL9eZ%X-l8ujc0utoKK)_;sov)xDzyP@KB^2E2 z_Wjey^Hwl*OObnL2}!BLF<#1dZL3_@E$>vkE)H&s8O7Z<>Dhso$8R-4 z$JjF;-}iGdz15-xQ3p1djQpIM+H-UyVi4djgfV|(OI|Fv@%|Gl1WrT-oW;n(%GktF z^Kcj$%GPVz;blFpS}XXIGPaHy@#HMlugcb ze%!uJkKt5n#P+|1(CyjL3Gqkt+f$(CZCl^#9A7G(x8te6aTlBo@yd2Z_Ya-I#1P45 zC0nx-{}vZV*@HnAP1dE~{X)za8`<*bT4#nKswIVrSQsLFd$f3Jcj3xPkal97SL&^= z?!=E}{30%Tiw$c<~HgHQOk})@?;Z>jS*3 z^0bSij5>ET6buvt?XDOqwHr}FE|8aPs3^dcetK{RiYxYDY1fm{y7r7+nq|AAbBXUt z0&k8|>$qJi7A!0d2cnPA~5YJ5Y}im+3#0WBng@u+k5FP?}OY5H0G7KelViK8jr#P_8=!5I`r%GupHl-qOo! z(LJWq{&lWJiytsVkCFP=#UHIjYaBEFFCW$% zy0y5s*SPvn9#!%^?hMPY+XX%`$YQ>)sP-B|-RI9Yqwjc>$K0s|*n=Ch+h2Hb<99~x z0#ou6L~vN#Z)d4OuiFJ%&J$`12_EYEf0Qu451Y<=(T}(38pm7r!#7GD4i`c4BW`YH z<=@J~seQ4Wan_F?jJijo)EOas=K^aj$f?ylJl?qBI>UHu!BSJB>H|w`&pSCA1R-A= zVzVT0l3P^qgU*c+Ugm38A2{_fH<0M8;WO!y0_ku4>pV_Kx54qPOIpNN#K&2QZ_IKN zG&tbI4`C%Jx~43+z0RC0V1U|yO>0b5_25VA%gk+DOyUfRr!Bd2)+<&W4UaI;E6Pd!gnXh*OurOY$KyOcG^uv% zH5Rkhp>oBjpcX_nx5{ugR$E5B*UrMvu(Qg?)`q@KO}0rLA|O!@T>86?X~zevVUV~^ zr=|(HzQ3EBEI_H~2ixO9KfT4bV2B%cUU4i>Ku%6fbitD1B-c&{vyS1jd-4MTN9 zoKIw>WRq^LTrK;Slpot!X*i4RLpXlhN%ZD7dAQVct&n*$Yas@zW!nBEd5FE`Z0I9Ekr$57eTX0 z1#@StF82q9Dg#W;y=QUo2K@z zM6A<~|0cB_e{Nmh8y)!my~rzJ?B8-J{d4Qq!0?7gTmN~J0l)HYk2~Nxcw2UWT$u9G zak&1<#ydHx>MAKvOsoO9%j8|fq%}Os7rWNh-y1n9B$j& zZ#(V*3AlLi<3r={&AHrz9f0#H5I>&ZP-x8*Jf8|Dl1W1J$kbS9#eHx3gS#Z=HSPGM zHP`Y@#d{2}g8lB%ZYm>mr?H`d@#oy)A}8T1=)eMKcH{Kzzr`H z&^90<8GNLKlBYiZd5w{oiAtLLMOtbUGv%SG6v)!K3&G6H8He$*)JB+N>T$F{TsaGC z+V*4ryX8y!iM|!Ju|@k}X3F$00wCO=?cvncX1>eXU-Dz21}=Y`Q?ZIY8e+g&*4=&6 zEV=Jp-mXVxEWc;@)?|Lx_N1hjr|8X9wY-}L+w`I~=g1uY%K*4@qLU5wd9}pFH(P{m zi*lo6EC59nGg-b)_Q6Kkn7p4Wo+z3OlizdENrFP?!lJRc;D* zuF~aFdXn#g4K%A_s?4iuu)g}$pl|}4y~iEyVbjEoxE*_ z%lD@yC;jB@TOR>`;Njx;Qp?W{&47EZbt3kM2W3QYvo?h!VZdR*uiFF3{!rV;`WAYG zea12FdR2*vp(0s)@`i6cBp{_)--$gx=;4VgxV%R**4UyRF0~!RgBApUnnS<@;OKnB z-}4dw+#>2m?@1S^NKtowN;jRo@Ly6lAV({c-4+kVRycWN(9byBm5uZfu7FTh!y}D% z9~bl^+mu3?s$Na6NH#H^hK_YR!PZW{^3O)Es5U@ zi_5O1-j8`-`Qgl5>7CTIVH+f+!zup9CGPMTXeCcS8H#S_h^@I&#IkW!XE@B#uLt<4 zt0{c4srr-_O;GQBcqhiAg?`V9loRl9*&mVca6a!*k z8~TK+in$QL6;VYB-ShqXsZ0(j*Wp^4np4^>P~39Kj}>JXapah;%`1@8Qna5?eie70 z#~_TBZM;g%IRR+ycR;_2DQ8n0lPRy&i?WLh zt+hayBczHyf@|%-#-rLM&v6T%qw0vAlIr;EZk?ychamaZw!3~ar5Z-6j<45Wt4Oo; z$8-#n?A4B^J1yS=DQZU(RF-U@n;!rHWN9>px_fo%&F=ipe zKXpyZcW-;yEs%*{YjEXf>4XuTGs1T49fn;aW?+Z4Lyd2{BdCc?w70X9%;hE|_Nf7< z!Nsxy&dMv446G_klCZRhx2+krw{`V7PwFkM1-ZiZKc}wP4rPF} zK)k>@vdxBl0d4lMHA+UsagvvY!<=F=3bi5ygL)@N24z`S`KN`m1|rEzB(Q_^K71tei#`^U z_j$I5bkp(z9qkpZXdV!adJmQnek)TqSgyNS3(CJoo(N^r^!?S3Y_ThKYB={Pi*m0n zOP~CzHU+DtbAkK=%-?)dvB}@aDd}tT6W=S+dR5qKYj8I04~J|F^cjBfWNCRAav;aP zVaDocHZ#I#MKg@nb9R%uWYKGyVQmMRRv%g^48r)RXGj|}KbkgzP^fooI zBumTT*RRxf=U)tqM{vI=!aD}jGwVR$Y0TYRNG`rsza&O!a=QgNwx0(YnO#s90v-#Q z_bNY>abRvwbq4ZLx9J!6FKg43Cso3!@)!hg{L~KRM*MrZeZ(GdL`OdXvZCYPxktZ) zTMz#uiM_RNz1L&}?755yMCfeGc6h>MOmjawIO%jv5i!0LY38wXfA4Y7kiUXEsa-bm zl+*={?Beb;;pkE6bVgUKoG^z_VcM0K8=quIJffMHq*Qk*o6w2;e)-bmBhl125uqk# z$?lCW{Ie5m)eoD~i#+mFm{lukJu~lSQUU}4)tN7pKu+9Xv2#_)V!KGWoW*#l2G}BA z0w{+vEbPiDBgC_hCtm9KT`{|wa%KU;^=9TFb-=3GfcM` zQh&To`ug&OIlzy-v$0sbAqDDGKlxfLCjCKchFDY^abs~e63PgCJLaT~OJ^c)!CCUJ zLp%tB7)e<(glkR5=IJYz|3gWP-qUQ=5KCJBk!0uKz*yam?bwu~Hw(u?UCQsk*syx? zFOk&axrm)y5f=(*JH4wWW?-K+ld8*8M2+XZZ<%9&!Yj(SPEP4Sp~|A&`Xfh-Mp7|f zJBRJ_b?;9qTc^^@!7Q{>F;A^)KMm6bZK?oxW%Q@4tZ`RsPej%-TiMO(M1dF&gW;g} zMvjJ$0&7pl4g@u^jvg#5gjSCdu{FpmSL*R5HRZ!shVfr2mYTp%_{cpG7XWmfsCLVk z{Ab5*5&gAsmA%{$AzbT$F!z|ad|`HCP;2(n+B?YqX8|rabEGwA%G=r5rAH05-MIB+ z&c(irn5R(6su$`e@tgupFp~=oF!BlhQ22H1hF*-7P|XfG4fZ0KM^Hb#RkW>bNQQ-% zjh8HN_E}$ZZ1S@^kU#7Pp=XK|foXYixlZRa-41a>_bC;NxnTjh(`Ca#mm=F>rnWn- zqI)%tW`zK)Tv)1n5EQ;gYgyuLv3@)?cpDih#RPdIOS?waK;3Gb!c=pZ8j0v(T7vv( zqu1N;?W^*_ZajUhm#Wg8j5UA*B*+iVXx=<0hSw{P!lv zlOLp{NQ(B58?$Bx>z(Neadli&g^j;QM@P%!!G!gtB>-n6XJHVatkD0RWUYuXf23L? zF*MfQ%}xkmIG!$RXlv`f@k<+4{Jd;Nyvv5GL=ZntQo^^omwQPeS$f?8d+}@`cZYVv zhlWTJpEdta(cKdgd^RG45A^CFnA#JY9HP6PNJgIUiug61Q67FBA%(v;PJu<6@}%?R7p{hKKhj5JDE@Pcm4JU#Ev>B}NA~g( zc~Z1`M~*yrrG-jaZ!~PkfAm>T`FKZ7>ix%zt#P0|sS9cVBE^u1q`Un0#e**kk(BH) z9LR4r%e|pV8*1qoNQo_k?sWG$X!QEZJ2^&qZp{t9-FJH5)HLpWtnp^Da5Uv?3(p47 z5*gj^?O&8v7V&&_I$AgKL=>KoCk*k)*WkQxX7D3xEp*`eO9najbCHoY?gAsG=B};_ z4_B#IB-B3KykPVre&{xl?Ap?-ef|CZcJ?YaSHF6ZFGl6DN2b07)*`jMtd=5?0hP`tTSc39MeS*878Gh6D=<>hy{r!F$4oz~}u>I>t$wBHiboa2114nZwcW z(EFWM?W*qoh+px^jTLBa9IancsH8p76yu4*74e0_paAoaR++azO_WrMgv2vde!!BxI6oVlNun>prcD z{F;fh!OQ?c&hg)`fn*|4sY0RY?@+2)Mgxj2*w?lD$hVUL`=94UFOcKEK$c7I`|inV z`Q)~-OBrJtIQ8NqBUlMSb~|&OlFv4Y{IU6lv>IHstz2efKB41{W0e(Qj-~Eoc(;fw z9z0Q{_hOzW3w(87{=pE+P@pUkX>^*LdYbLZCdCHYGh;ub95@`TavuFOnaNG;?lo_b zKJz|q$7mCfHODTHY6k(1?zI{O@OSu>f(0UUjTz4|?)9a%^GTC2vh&v6uxDIFZG8t=~g>AG|ZGHz$Q8E%{7SX~W8t_n~?gAhurv;|l za(?>L?&#P1bFt{LR}`-Sq(>^9Bs_Ph82K{sj@fPVFQ!Tu{~iJs`)pc`#TZ0pd(o_dJX4x(o6KMJ$jWOX0F+RIsvPaG9%9KZ8FdFo=(pD;&)7x|pV zH8piBuLI>&5{3o?x}Q)H~O46zbzC87Kg z4Ur`9Q4lWhz4=3rrik4&Ur{#lJbmiuvb&BWf;h@z^Z{%3I$p18ghC=r}%{moDwAE60#@o!M$haH3yzIft&SH6L&PRm$Cs%h+7RoSa(}ALLv@g$26r)c)v~Bs3->QQ z_lCR>E+CJ5?m}ZgdSit{{o6ph@m{f{^W<;d_1!932-dlu3%W5a9CtQhlKd=qXcU)t z)V4hWfiEmH2yZqtTWUo6n^$l^FePs)6u)0+&?QS3n0R6kpXvIh!+qanex7|FKn0T~ zKe4?W)OLV?-r=~>aG&&L?IPJE2jq_)tH>?0bp{+}&G9fv3rpQgA#@zx1 zSWnWq)t1wCM!>Z=F@^K%`C@+B>rF3Hs0oHg+Zdsx*9(izjmOH6pGdMR!W%^%1=C5} zpl{-yyS6;7m0y!GoP6ioTQJiXQE`PPc$ZyWy+*zJtyS7qx}b9!{e+lOv6Q?w7sg;C zjs|4>a;JZZ*W$3fs5n;_MBy75@u(F3G1U9)n&nqSZXq0*I5^h8$7V%9VxNHRQ}*_G z6`_!8X``qC4*#QE&3a`J)9WmdiEm6h-odpe2n$)oiyp{d>P ztL_EDZedg1&>E~C&oqSa6&eNoaia4VDYvMiNs$_r`$G|mp$tw;;O=4*wh`yczwHJ% z6F8}))6Ka=qDpbal47l<@9*T`8(D?j#q zxTZvAo9c!3NWlprR_TackkN%(;EZlO#iLj8c!31P`iY%Cj}o%X4#03szQlearuZjb z#Bgdw7)fW}K#;)GN>UoqluzN(Vj*mwe4iA3y1gZ6E*Z;KQnSkLlE4e!VK8wd*~*W? zOwCW-<)SHb9wl-6X>udZCyT&b9Lr|3Sj{OaaS@5pIY{rj_M+bo#d^MLjvnz@-j$<2 z($Dv&PW)Z_@`e#ywA!PEz0T4}{tHo4XnQQ?2}f7!S-fbh`X0l$I)O>?iU~#!k^JH& zXeE)1&iBW~K&6DrL;|9r363vaZtFbPL3B;DX{~1D-oDMsGR~<59$%Ss?ku6zFpM=M zF;dTL23ljg-A}}ht@6@PdQyB9^5jS1;gk1P%4+$pE~7Arrj@8?q~ng&V`vkvk)-b- zBRVp(zGCTq>n_`0u1&%EA3Or->zRrc7$?gv@pJBQ7Bd^GuTk+27O!x>FCq(w^0o`H z3dJP%kpYLGA;^$+ul_CilQ1QZ0L}D&Zu=heuRY1x{%;i&=9DQJM#Ykge`vq2>;B&B z3vXME6JyUJI*ZyN5R{R?bkd6yNfYf%F_yXbNO^EQ1$gyuXS6kv-qLPlj+WApm z&iiII^X_CGi7?ez$vNtm)}F(^YVR8Tqn(Vth$;5Xq}-Ctc5b$nAMvl+0@EPw71hVt z6yHqC6KO4Ky0TTgp1fh+)QYK68KghTl5)^s$^@}=Sh^WiP? z@BHU3<}W)p*uEtXiK~ZsW)+n$2GS0aA~w5o+ronDZFua?qm~-EbaE?&s8SC#0`1%g ziF}M1)#iVH=?EC_tXpnt0co3APv+WgfPojmRN2PR^zCrgVYtTo4iQt`$Kql+R^`ePKY%w!;Vt-R-jvZm zasfvuDLK7*&H0Q^{>sXV&J{lNPVHuJXQbK4#_wV^giQIG@|v7R6X7l^B| z@qOs>Pz{b&v+S&e=!>a|ztM|GM+^T%dy?Nnzl?yu{bSj;M5Y5x#0}}$Kr0LHac}Aa zQW3*~9<@_Zl;@^n1%rB(-mC+H434R1d{O_we<~-UifP%YJsL0sL!gN#4d)?tGnoS}<-*+@je+X>U3bvZ^s*(@Gdy7$CK!GA9O_@E zBQ8cydI!#+{-xbPHuBMbEU9(Y(_wt?IR)4)Pn~4g`EAl0m4xC0$*pVS|2fP2WSWa#QEhy&71FtQ zz)~OA^*(cRvg#HeUU-`56=~Cic9IrSC?~(=H0&H17{|{8XKuEOLONYwQnd_vA z$Ex)S9Nm#5(>j(kheUmKvTI*6tH3J)8?%Ss1F?VA^!xcQg`Y-Nd|;H@ymM>gvb9={?X*zId(0~XB) zb7x}&NXB!Yq`MBd_&Q+@K#2f-kqZnYb-?g2FCh8x1QA=V%z4x0#2E9Rzjz{O_R0IS zbeU!Q8vPPwPqGngH-@u3uj|+s2(z<9uYo~Fn+eu{2B|_`cK;5gn==b zAdZNhoo!X>s$$5%W1u_?e)pF)&#;_FBM>ar)aqPKNThAe{9l{<>=ozbll_GK4j5R( zy>4s@z4u*^g0tu|w|h7(UZ6yt5?)j5SGlWbM*yuzTEamf57#{^>ouKXl;K@;es!Ke zCSf2kk)G{Xm!_UUd2jY{(E?skyn5@ z^ZFFMI}FMsH`!FxvaZchwWD`sf>6T&P9xR!AJh^x55h9GF2O@I5RZkMR(z&+Z zsK9{b|XUv~`o=?*|6~ zAl1A`lKcH^4Jt`4R(YWq)s9{A|M9B2n8<_$Em{qY^Z2uPbgnu~z2Y9CBeYAuWxYN}oqB~$Kdj!odXLFxZ?%zkd= zu&Cxq_x#CHUFq(W>|Du99R$8-J%xy&n8H~XvgsSSd3nHeC=s)E!f?!NaO?V4Db$+MaMpKEZ{`FU zbn{0e$ZM~Y7^a&MrJnFG*(g{>AZ%hLIh!v;^xj1>QD$flksUeN`0Sp9>6hghHZt+P z(?B^oH|ZStTSO!59s<*q*A+WBcW_f6tWsa_jo}ZJw3bHnVl@|y#5Eh*t{N++1#uiZ zeGMNG!G4y@%sDP;K*0c=fn+OG1BL)NuAqpnuV0LY(WI@JxnHnGwlL8@mz~xjfgfLA z*yJX<&uNr>U>3?O*yO`_5$6w`nbyCrv(I| z?yr<`jJrcGNA7dMElaFLWXLyGSz2a0E!WWr*R!#)G3&06zM6RFtd9C0`f8_u6tpAu zY&?oD8VfYt9&_rqLnb-J6;Jp1>@k%HstN-{Mk+($<~+DHG{>R361!PP8R;7WjuGXb zNzvtwICQ}9%W|^uau{H93!q$*tBF4j9uO#WQc9AACt?WpK(;Lj{Vv-v9>Yx)z)7U7 zS`)gV8_ZP51{8;AWrhW~we?x#=S!}jczog&SMy#$MG!ujyYE=o3gRSVb z@Zh7cJ#9qIxz=;!9kC%ZklC)y-U6pVIZAk-vhXXvn-cJja*^<?i8`}ZLhN3rY2qM>q)@+l%~1=25|2i zHbX^K)f!;tcf@lBWQxqlp#!q`?ws`@eg*ER zcAnyQ@sjvpc0rUTed1}I%y2=6XPJ2Ns3qZiyiJ`IPsJ0xP@t-J~MD_1E= zlWy!S2&rv9k<7c=y-m+e(E>$Mdfaat0~KGjT8C&u?c^!al)}R5dQFXDiy}AN3hcbm z(!!IkW@K2?T}iSpl zz?ZQI3RDx~Iv#kiIZR1f@mwEd(Y;6&{5$f*h{=QUP)P~Sa;5`DOR0G5uAMF%=E>zL zO7SBw>Ys}m%gIsdSf2BW19IrIK`b=f-}}L7XFrpL7gO*&d%2F;n7a zZDl1vrYYNagQIz&0{{NtMh@%;cx=!ht0S~oZODtX^KS+9(z@s4Wnt#5i+kgZb5X(6 zQ&8~-S8%L4-p)+0p2^g?h}PCnR)E@6N9;{F7GbTwrO|*RbAgA3WZI{>vA_$z3_YL0&? z4i?T?KwD)UNb93n=~cw)JQ!|r-t(F;z<7^OB!KT?IUL&cQm*22!i<<)_MZY$Zh<{1 z(ZVM3;gzhfKmNORnr@L5SUU4dPy|LbB`}^E2EhEu0Q3nS6SS9xqJx6qw|Q~2;BXZZ}WlLQ6bOd=EEm3g4tuNTw- zGp`$I2y#mA1uO5re$Mb)kSX>CN2QO=Uesa)^1WXE_cI{@ks&qRVRBS~%mRiYV@yU< zzgf#e?ssvhF5-{=O-x6&8$m$^zwG0D$=NlmO~|mdRDy+W(GE+mVeyH+Q!?o`&`0KV zgm3s=I)NVj=WlM>tsNNPzccXaka8wqVs%ROI=Hu%z@hX}Xu?a(%`7MimLZc!+#uJ? z-NzutTV{_AenbbtILTr0BropQI;Ki7vT_9|^l6Mmoz2ONQ!%drh2NfN^)6jml2bfv zZX&JEEGp;L-lfNBhu?XQ76vD)_f~ul_tlQlwz_+LS5{h;CDjV~xyW-SvU1r96va*Q z>fzseM>#yAYJq@{^K^8DtcY@ydURJN!tTl|M7%a6LxwtW`Qx;^7>&Coi|c{R2;xS- zF3TW}DhLg*k>_d_uQW>OowO@S3h)6j=&gg=&AWqQS!eNc@k@DDg~n=oqkXkJx`u}6 z6HU-&e8S$!qzVOgzZx6`OHD4jGthKe)SZ$qd>h^9Og5b6`CT25#cOjTINeOnxyTT$ z7`A@R0qrS;1A%mpcDW;rG{=Smp-}U^yfE(i8Fu41jF>c0`Nh{v#1A*x#OZo?5GuED% zQO6=G+;;uAszj!366k!?F*+>K5mR z&t2JhZiLYt#5baXuPUE5NcvG|s0T7OK*lYk=u0A#FnV6xJnxt8d+J803Go+w=@>KlzPX1+&8%2QSrb z5oOg=Q0P__+I@*Cclg>Z3G(#wjTqve!%euq z6IrGk?Uk3ei201cH2SF$GG)yWpfY_r8qDt4DA5``_%{Ln<(6hI%t}a-wZ${;2OJ$9 zsHStT&(E`pBLl1U7HgRYYbuTE@)$1PWRmNy{mcREjmrgKFvta3*Z*6qK>G zhO7tZz4I_qiW#aV_P~NR{^EO?Vk)=li!Rd&9GwvJfejHlQDs=)Du&xE3jYpMA@F-S zxJmi2ILG8S2AG=<6K*ruOmBW0?0q=+C1&Qsn*c1<45ari<4Rt)nM#{y5u@9fzB53H z?h1oVg9vh~g-hvZuwRs`@SExO_V#SY-9UB!M6aTh3+;1t66GjbP2hu)j3M{FxrvH5mO#;6nE13%=X5BS+KN9B^D96c|Z(|F;|W%_ZQP; zSEBse2k5n|14o7LsVLP3j=47MfN2UU5^xSuHh10XWGJ8*2VBO#cjj5|a=W@nwY`6y zHFx`HZEK31g5*OoBBR=${QVO?dpbZ z2_MS>Yg`V00b)jTRNhVag6QFzZ|Zc=Ba?D*DRG+N^bmzJ6ZqThNY+x5iPixDTieBx z!v7*PwRa{Rrf;O`4Gm$PJstrskQUP~E`9l#9w-y^$&3^hD@Pe#Qb?`Nc667<*+mCM zYMoYpHm>-{|8{E$u^FZ&^ba<_J(E)He{@(k^P$d}9QW~UaYs}9#Ojvv{k*OZ36zVr zghHdGjh%@tho0-}BeWYo_yD44gLvv!IjppK2+-nXLC@J3J=z7(A>8-YIF__FHG`@w z#>$UP^|6n8cdt|*e>Bm{%tgE6e5F-LyQXOI%Va~++H?}iGm1zlOmc!3ht#|nwIUb$8tweze{5R>93aDsm zBmA2$AdW4UpY^m@_Z&f&Dm_k(bBI)Az@^o%T3cJMlkQ_#_3|2iB40ZX4J_B*^wL9= z4ym`jGXF_Z5sICX@Ohc3J0j#3XifY(g2iZaY4taQ0-{ni+gm|G7?zK>&*%u zhqy^=VbESqiTB}t2tG0b&v3kAsM>L{NM&Bt_x48wqesSr$K$7Qx#@HE`b-LH^Wm2p~mf&dJsD@#eU2kIPlHwYAb{oBLq<$Ek}OBc5^~ z5aTfgOL?MCsXGS;Ae>4>*FW1ke#-oB$@UM1mun;&#Ilb$s>7*lWjDj3yMG*^2?0%* zF88MOpjQzza?O6}^p5cA!J+#sq3~@t%+TWd^7UTS+-|(j6(?!QhQ6MT5*qu4wply}ydCVWbz~ z-ByOE6@PRXuV>H*mlV1)5?5FtylY!CjuKOcWFP^%HMYfs0vqS4!#D}Ozb|P~nmZua z3dK;Ho>7ofUtG=O-xY$J?#L!&JA@07E{_UhCRtIlubmK5UDZUFiW(3Q6lw0`d!=P; z>w$PE{z1n?aJQO{;ENsBo$7Lv)bv-&i zRP6E`Lv-WHv1O&P8R?4!oI;}8RuZIO{d7}>wa_;x=a_5CWP$RUfpwX`SLOPWEdXI|Tkd-cO@_ zztgG2CL#hl80q4cQpWJ@`qkOra=LF6-pVGwc8J3a%6nTfyIvD{1%h*sdo5S*WiV4xJ3x3+?4qD<)>6X3Uk(oY<-Mu7c_}ca=T?h( zY`bOSZv4@#WEc09C)eglFq)fCnL;qQWZg{gX5nxhFdN?d;XXo}xGs9n()V2(doT)V ze%Y*TXWsAJjeY9l{hz*vlNCnYZPthL*5G|BmH3AaY9jNCUn{C=%pVWsXDgVgtQv~o zsIPgg!2}X2ng!81~!}5KoX0|Md0}3h{=ZkA{KF&}sc~nWQx$Mmh5< zab-Av)_uEo^}zQ|rX#HEBtTaBm+nwgln^%&txwD1&>V&2LQ2wM#nIoiY|PDhnF`^?bKkui-$WukCKE{J8VP6cpXdEMl<1U5KC!EU0Tz#>YNTtiCxiUA8Wkd+A zf#T&wCAN^pkBxdhZUswD4bwS9avUX@^f~TaYw)iJ6y-BpmrK)@MHG0q9#5$@aqFSM z{%t6i_U3WLeP660NSRG<~`?eYCH7>>${7!mf(N1h|AO$gc8qD3)@a>Fmy1ln+OIdxysY*J zDnWezu6BAiUE>~&cxTA$k~sLm+dX(uMM}=KOnKM1IqSBDL;GJS#126;DdUk05q6Mu zcjXXsBQ&E8;mzuWz22%U`K4^oN1_fLQ@ZX6VeFXi=fPSs2Cq+P!msHjmb;P!ZN>1@ zwZb1i+JmK_zr3*Q-%mB*e1Is3M{J$*a?6XzL2y1gTZ8EWL)qDuh>FM4o>apwH}~}L zS8f)C#x^RyeN_H3S7sp{?vGmzpF(koOAzW0cbCA4>PccO$Q>1lh^D>mo}Yb!x{szJ zTBSwaZ;7;gsZj~Mk%phiX)!>UDuI0Kil+>l$n0y(?w>!H?|e-A!6lHJgXixt_;lmi2)IR#b?0<)H-?V^#g^vTAYbH4mhIbbhQF6c~ zt4iknsZ_KkUzPNsEJ4QS7riYcp8xF}k~|PY9O!)tV4wWax3I?f-xoJGzw@}2S;4>T zJil@Nwz~V_&))HJ*Yp^Fp28I+z(zuZaR3f#OsXl(7{nLwWw;a2TL=8%qLtk~PQ3Cikc zI9?}Ub~ES}3|F+At|#59a6H_+1_3lVa374thB>^1dRAGuVdT`savK?#mZu4)1pIS`T(r%lbZ48G@r3Vr7e0N*N zIVQ|Z>Dbe=R2aWdJKL|M^3O<(A5O3w@&lzx;v1r8MFN~nb)sTW_=-2$@z0IW5~5uc zN9|pNSoR=V@^>)eA`6F;eIBP?l~9~5dZ6k=C%gLgV~a-fP}%B|yi3$Dui#%XTT{o} zNZFfD!&(GAg!c}t(AZ&6f5h9y2nu~TTL_5UAv>4(OkFmf`zZuCMlY{{? z0z-64ucf7Nhe=+ZQ^mMld}NsGB|*)bu}_CLCA^UQi1XTShOoWL;81Q-XoSUjTZ-b8 zg@8H!?nUX|##Gin3_V;4m@VHe>wp6V8uuA~YSj?u^Wg7J3WMT?u`--ZF7_|xUU1}O z{fOs+!|dVVc}2(fG!eHTsdiaJE`(iVdNH#zh%1l$0_6U3Kocv`mhqs9rip~*G zm+awlBW)>L3x2q}JNBhlNwl8P0a|x@mt~t-J)LbjzvWP0sm$xV`)!Hdc1Ne=r`2pT zS58MKo%1Pvt`N%H5v9d@QAlg{m#W6~X1msHl{`OH;m1*aB&CI9_sCqX$|N{MjL>IR zk~Hzlj=;cW7X@>(i{z;x+n07BUl>i5Ux8<>vZFel)=_{Y_N2lS(#HOn?JwM2ti4wk zz?Jqh&GjLH>C|;RT!w@6({tf?#C59Ve)iCr%p_pNc;H=Rl+QZ|5Vvj>fc3aNR8jxk zd$JFNeK0@q7@M_-mf4*h0SOh6W{)TRWZOIjC)r5-HO*qf%pSHWj4h9Fgu1BH+j%Qq*Is}ZI5iTm`U z{U9*Jm{~0w>brV%bvlSfd8yx3RoT$MGv%c>DIF14=ITr+T67waUu_%4TI#^No=$GQ z2gl`0YYJ5}`rR+2Uas&SG|u{|&w?nUi~Suq<~5UF7`z2oLepWld6=IsQOvhH?*)4L z27wd%-t-ZF15dUnCx3vhcHYu%Z%WR=VX&br-D1?K2psQfo@5G=d2bf;;?%gku?AYN zB0>e(81BvD8dU?5v(pa$=8WtXcezLY{->fkJCu~M>WaM{^z>)Q;~~NS;``x{e)({B z)%X3!ZI@f2i-32guNAr#;h5*bI@W00;9$&g9zDD4b_a3AQiJYu3G{5VHCtyz01)N) zE6($f?nklh=e2jv7Nx&$>9?>d718id#F*50xT%Wppb=|MS@$~>)5<1g?8-@oS|i8S zs9<~+mAK+5{!P3wAKG~=bR)1?dr=G4Hpvnh+<1i|X2j_xKK2vC`TGk7-Nc?WoR>c! zSam4fWm6jSNsasXPiYCRPKAVHfz>TD>7Q+gjAtABVOSx$5ccx+gp3xI6@@Ih(&&47 z`Bc{g@CS97J^QZ=@ZF4y%xlk!J(_i|R7g%gXnuBj`vvsJU_0;+H~M}23_q-VJy%`C zJpbg$d5R7Y9(?Abao>~yI*g-+--eOHZoDoKk|w0A?D2HH1|QaVb4KX7*z7e%l}G&; zB+#uo=fjvXi5}rSjhhbxZlsxypPk{0_Da407n9yFQy^fZfIX^;G4ouP08Elhgq{1Q z3K_gqWmARvU1f5aAQRDI0O81yFXV%BzB%D?j?jBEkh`>41j-r4!S4rW)d_GJLTWq; znLL$rNhl=Ps}<_mC4(m!MsXZBB9*kPo1lx97Uy>W>DSye&Ty}YhB{l!LjJe#1cS*i z&L#meb$xw3lk7#~0g30Y-PhbH5g8E?{(1t(#O;f>Ic$ZHa8-xb(Kh&}NB8PC@^AS0 zsvZtB91eH){&c@~&_$ot0_fU>PoT`VlOlSgoGsr5^E!^W<623S5xcUJHOhFW{D`q@3zSCC~j? z7?d;9w6O%Ra&2xMF+9Vk--4}rp9o){_ck;Qm0EooaKfE<@wyX9x0u;^#{riy!Kq(h z%Y9g4^4nL1Jom7U@!d%V9x}K8WBuyjt^I@5AknQ8Y@1lDPHym=$i0jIorlo~yJSb3 zH_qJspTVSRs&wclE$0=_7SG;+o}_qKJO*IQdY6`r5S_}tEtMvhr4pqmE%0AOi--n%r6r)f*8!(`R#hpWwQ3i6Jl7d}`XV zdDcEzUas9*ZSYsif5Gt*;gbk-Cvhj!2L~7M*=|)PjrOIMmoWsQj1srad zMn=lrMKfxibiPs`C@ns0d{xBdEb;4ir^zi#w19F%ILHsS*<}!0n}VlXf5v%fC8CNf zrc%D1wo}pWZsKVXh{JqNsO$YKlY-(P6LU6nn;Vz%+5|5?0gtR zdS`Umfc+E-PaGe1g)|ur!Blitq+{dW{@@(RWAgrvWUmUiKMHnI4|)sD8XDQfSh6P;onZw)H`0#8b(`iPZTq5U-baFeOIG3c}L9`vC z8u|Q+5S|?N#$wAlkTFss_i|A5cU%&76?Mw=&nB0_6?*aHgr8%Q{Of#8&UZN%8H1PS zEM$wwo2$;8IWrQR7Z13?ha$xIpLaTmq4 z2N3Xp;DD1K^o&)`SWeS4awWNR#aPVMna);0Ek5SYGIgn-A`2h>gpLsfA170tOkX-A z_f=7LHaD5W%MD2qaFWcUobW&!l8&h;&uw#4#9TLMDOJ0j(OOvdtGre;4}BrQOW6in zFP}Z(*p05S`qjr*h(pyyfXIe7eW^^`I=<34NH>sy_!JpdEFAhm103v7m10vB$HR+(sV##Jm4 z{>2xkselzAJ-HQyC!n4McN4OV>tZf%7dK5 z=vOkpd6C+8>Y5B)XylT+a~}d=e|sM?>mL(jRhw=XreIN1IHB8P|1pWueQiEws_SRL`4KkG4Vn`P@bUy>IN zT~d_dn5^u8(xyDJ9Hz$5e)Th?@3!j%{}btGo%ggQgj#@uN0sPy{%n53nPL@o@_WD` z^pu;5H+Fhl{q&*^e#yjn;9q(y%bSxq7mh35r=xR0LW|)R$SO{siS&dt!J;jemLf|h zqCC%@bN_Y@uP&Q_0SqUJmGuA_f0eX8Sb@Fx9fa5^ItAA9w65Ob8bKIFQ&LHRGQmC$ zPrU+RRlpYc{mfpk0Ftic9;yAV2ifxC<5CUDE6qVS9BNlHeg^l@UjR4q1|BiC`pjm zlP*|uBFJ~&+-@_(zQtS+r=y4nDC)gn4q^fT=2!3KoF@yz#pW%Hv5?lPrZJ}y6TQKk z0_-f1=N@GiZSll%5bS$$?O2O@m2zR9O<%FxDlCI{N$S+zUb7ZmPD2mS{5<-|Kbw4Ay5KgTaVx#mHkcsPxfKlq{9KgOb( zhFC6F!4cvX-BrtU0rJK6xIG^@~v;32^@FnR_jA|f)QWsKz` zb9+4`O|CDlbNgnlJ5^l)6r)fhmX&jM7R7xwDa|*5{*@50@v|3uQ=XwY-8d9N2+*I- zjBQZ(sFX2KXmiV$aGi6ht|j?X!^IeMN8Qn32!xk6Pi!a!}Gh}e17uRXGhiJ#}D+;@AeB4 zi5LP6zgICjVe9lW^qx!`OnRBAai#8)6PM>bYTHIe9Smd z@Mpzd&VznDHFGg!vP$%+Y)%831$7_G_gwk?J)jPl{rE)>=|RQwy1COg{GZU|^#J}E z`4+S#f5`8UPCX6X*x^nR-f|NXcH-aGG(Z^=0+WH7TuMOlMa!YPs)!f0y=AsNXe(1* zcY5b{X?sl!5p+lHHsPAM;7ip8Xj5E4iP6%GCRQOk@94MM{wI9&(~=p~Nt7n|Mg=9& zK2;W%jq<)c3k(qOsM8>yYqZ4m=F<5M;fFTc@#YcOO+_o+QpWTLmHrNF+5qdRIHP#S8 zUVTQVK$+xO)URD~rg^)CN%2~3jWomw--0yhBf>}H&75N*?^7y?oWEJ=6!S=}ajE2N zBXmZ0JI@WcJZFWPdi70koC9VwIK^xg#J@E?99uuw<uuqQOu<_skYiDIK!D;MX=fuA+JVy->EP+E{z~oncE9|l?lW*y5Tb&(85WD- z^gm`xCxQA3Z?sQIKtKW}9jlKPF!WcOjlF|F*dpoGMYh9FeSLkG&S651e_p=<0fxH2 zoKuEfV_+rd(;9Es&2QNKnerGUSpxsONi)fp8EyajY3ZpFTjspzQD@p+S_dq#_>*mc zX!yLH2X;hS)tg{o;Y7C>R76~WF0p}dgqW?rl5rmdv1n8zIv#UN#cnIr6~J1;e5fe3S7bLM~isV!&be$HZ2oMTWlViPg!a!(()nX(ZAf!RL7? zc68R=qUU67B08o8W@mAlxo;%fv0~hnDi&TjJuTgp6;HWr#t*z5Z$DfH+ya^=HendAt70SQBC*)vB*T`AEFk(Oi4x%5}X9ZbGR$%CX9ga2DL<T&ay#->6Dp7fszbxbwoO&AU%N4z|5lw@xV$EF^al^Ay>OO z_bi}?Vt7as$80-|k=1!ne$D(3)q{c{H$6*|8k<6sD<334ZQg2=`zza_kn_5R%vHa~ z-a?xnxqvZ53h5MFKmwAt2>KIAr?;iZ_a(`orF^_)#O)fxm8*SM$9vgQBnUFO->MUy z8<=a$yok67K??CmcWXR&GG)+}@{n9{KAWdSe|S?_FKlQ~{sTMg70o-r1kGWuFn7xt z6vRE)MwrVK+?$0A$Gx$pC~p!}_IS(e{7l`|t`J~QTyac|>)Ea6kzqhLMZo#5%Uxix zJEzr2Qy%g@7S4wE3f0&wEj+0S{HVVW|T*m$?uNZAeAcz7rOXXE=E;%0y-st1g6!gz9@_ys&m)5nbo*-Q+ zRmm%B>7(sMMRBU;e+C~gDQ^!|krX!6{iUK{8Ls_zyY;tWH|2tQvp-f#Qz=)QPclGn z;)jesS-t2BhVyr+q!$j2(bgF_vX*_uw+;Y?-+cD+Q*Zo;SM;$+oW@Qq}E zb^9~Gk9ODE)b<$NYf3T<1PK`*tq70CXIGuxW8~B23AnJHT6un1fxwgRQo$V zIRqrR6Z3r1@K$9)$Ck;-h}E<7f;?av1EYkm_P6WZlJklV!S&ldKvt|~! zIjcVJ_&A$_l4P{-`6Ecn1!WyxU=~l!0oMp-aVMKsH4tFyzr2+e`0rgD(?bD#|G400 z+Tmatc-J?ehBPfuuCsMVyAh&htKp7O2pu@%R`bF&%A>b!Sia*T#e67F^f$BU;9zmF zA05iEQSIoIToxK@zb#wpE{_d=Ijq`Nijv}4c&}mT%q0no1*TDIh zIG(nUMGVP%C6-lyexO6niaS}KJ+j{Z^%gbv@+EgzfBt=M);Fr6mlR;R--QF3ha zfXq3lxFMVAQMkk@8zg3b_q&09EbUwQF(+pVlTO}>v^kY zEaqIcZ@HeTfO*MLE3097nR9@k?e188+9mkB#x?db;m8{o#3cMroRi3jJH6g?{sgQv zFA{^UDTc!{DctB1}l%J(4M zw8R33@8o36M(wBz3}<}l#AO4!1$Gx$L5~WZv=Pz5^@Az(w}O^6$jbh`dNP0)FH&6{ ze-h3b|4UvsZnEkAyq_wA0Yt`SES5_q1_vr_<5swNR-a?}7IxHlF1o5zPxpq`^w#)f z`Q=M=m+3UXZr+J$I;F=z*^+SATx+N4AV$kw@lSyTu6x3p2aYG6(!tA#NT|HmQx?)T z0~h7?;6iFGp5pOGo<~Ai|N~lhzLhg@e_jBZG?S@ z<(8v#k>nSx&9|PZMxo5He200L`2j!AWKJ|^0Cie#t@ECV2*91`o~Mh2^$=Cn)VjN` zNs>Vh=uu@e#u?>2eb&l& zmhy%sS40oHgS?|>3C_9hzZ#zJ{TsYGeYii|P#ttI1$#ct!$^uCBH(8HXnwRL)&GK6 zRuU5;3MKo_GjH)4St6ke{s5)Sk9=cm;;OAoS2n{eIHtyGek${XtETG7uspbvEXVR& zAQ4pzj%zk;8Z~Yk#SJ@D-};SB9<_ZiMa`?uCY?f6p!cBQPei>9`KzUIvh*Gxq_W|c zq)1dI(RVj`2q)|AjTn_OyWs@hRn1>2sH3w0or@z`sVQ5I8lA1nN)Y#cvpK2JON9>3@qrC;`6CXYwvb;xcbcCIN0dl(RTs&qld`>=JEX=CcqqL1Dr<6F?S~Bg zj6Z>8mx!QV8W}Fc@`$+UC)*GBoy=U_W+`@K%f9fNXaDY{Tn~O8+=_>KdqD0WIN;r% zC8C)2pxZ0z(O~0Lp`?5e)?E|@Kkj4^sdPD~U-f|NwF*FcIUX3Z&;)afzlBJez3Ca}uo*mn4Rn8g z>hDiEBaq2Gp+YfTv(iquI_|yZz{Dz`aWbfW_xq=lsQ(bnh&Iuq?e@5WhSTh@WP=dj ztT>-_1_T-2T3uu$t>eBrsoF>4qDav5$4>NLW?*mFY+@f&P3xmWe}JWGqq`e~rh+<= z4FP-qLV0d%Pi@t2+LOb1vd0KKB%t!jUwT#_8qFrQxJ^GZl{t zF?uHcHv>6jiaTadTie?!-}9d>F60}3`eSczjs}T&f)bc+b|RiFYS@V5gO6Ff6?fPN z@k%*2>b6hK_k1gn_FEV5HnlCvhM4c0dsBNJXI4d5ay(GW>r*LPwHt$~-j?7NaB(ru zzG{3~R75`AIr7Jg)We^3O6Xi$Au!l#HU6}cA=fLwevuQ_q)f1Izsv{2*+qUwU;b;c z_PL<{YkKpjtFKJk9VM#aUG;MwTO4reX#G92rRpLubkT%hOEbpM!f-Vf+DGt zBzvmBl7z8N+>#hOeAw)ny+N}ZuK}yVyfQ&|Jh;+Z95OpK+x|h4m4M+5tBiEdc&jPY zXe@D)vECH>UHak=QbVm!7B6s=?(DpSe)#f`fV!8Rx45xG#5Bj9nkqzTySw+=*>#PO z8TsVstw(FOr>9qZkPh(e-eq7C8?+AM+{mfU`&6>|{2FsExPiNR^{Na$Vvn70OI;F! z)<_oA<}hPi<2$=RSeB!}CS2rBzmn3P62Hj_kMnrx8(1;@UeAGUG3+?P@}nptlTaQrkHSKI(jp6<`ht)g|WhgheCFfInX#1fA)OV#C6 zG~6%>uiy2N_s@^&J9WN8)rS+B5-dI=gwG_eiiNT4Ua>nFkc^J`B*fXgsi`TcZVQ_! zYu;@s*Kv&-TG|G0=O+Q{fYHdWU+H5WXL!u8M}lvx{z=@}vX8xK#){+k+c|FTPZD>~ zqyKF>A1hf7%O9h0Gr7Q73VH3nNN~YhYg13Y_;VtSrTHys!=F^UHNKqPf9nDgzqWX2 z1vq&Z-ubP9JLekTRT9suzCQ1YE6M*7(;5z<8xB7OZEXY{mLAdjj#kxu*X_z#_U{sV3BDHH>5V~ZaNH^r#1g@6%(5i@`^%+!8q(GW1}Oqq%pIA zRqLQ5+N*{Gm4>|<(SVJVy%ckr#a+`x5GK2201Agjgp$vah>WRcrI)87id+rRCYL8y zw}=LmB!zNU5X8MNvbFbJ4uKz$zw&QrRx|Rw-Kr+`t;BgK09bW@R;K6YrDk*3-a@z% z-cfWUJ0f<7MSkHb+CiC+suidBp&eO;vydPrx%&3<-iOh zPsC$T?o<Ou_O@;z|Qio?g}plHH zR^>}B4O3~0x~(v&+N~K7dP@@ld*ie!;YDqr@-I>A{m>vlzmu@1Zw{D>tCM(V|{PSXQD+XX1c7Thr(fC zB{8r*6a~|9(c|qOB&O;E%!k3l`cu&EVA{F+Z*Nsix+&Exs~yt%SRB;0XL*QU;VkiT zCfC7y{2cd3Rj9Q`d5&X9FTz5COeB;N4YAnvme;bJo&N<;?O@@bC5B&D6Dw=MUHs`5 z)`;iqf4aN;X1RagSk=5cVCrl6bkNMXu$Bi7{vtV^s82FMX-{vV);>ri`ZwL_6yvXs zl==GIJMsjIV5zXC@wFGh%Z7e-a^P z=T~p?hm%UT_n_v$437V}$#}16zWG=gRc{cov)k*#%)o_ZtQepMgu z9|pcEB(I5F%Uqhktr;Xy%8ul~bucEOUmgH80u=cBj%2fu80!VG#h z2>CC*#WKW%Ye6`crt8>*Gt(r<`D;;2=<jVh{(q)4jQ}6B zUrbpPr7UDs;N2n(ji>wD<{Kt52@IGp|LsQRWPPCwdAr7VOuf0gSbe$rtaw#!YElVVBS|9)V3!9Pih zmg(JDS$TN?2=`8gfZ~*D?bhzI|D);LE@HHImEZwh+tQa-X^6 zl1q|Y(_CUAjl`7O$o;O|lT5jl)KtP;rz8m>x8M2x@%yXCqaKyC?Q_oi{dzs0Q+s*3 zyQ^taF>9GMQOiq)xL_4`GsMv{YkM1n7y5g|^-0%qZhe_Dx#%;PA5eD%u@q?-hX(Yq zM_`zq&G{2)q>+-c2gNl4sYFY-8oMzl1(JgN>bUEYv!46s&h%SMQ{0}a5*c}kTQy-% zuvu6(9ggzN_%#RGCqX9`b2Cu_*XIIL7;@FPD=Z=+7eIU!n=GD%^pDl^+vYlc{PRAi zFr5>@+byuaL0N0|ugesa1A@Hen_zcT(dr{Ej~hL^I7M$bNyo;Xwi(N0NW?-)4$$0k z3zU0Jwck9F8XS~hL^`;y!UuwK7JLMBxLND$SQDhq3fIv%P6>diUE#{1Kp}3J<)6!< z^MT4K@A=ET^JWSvTImE~K4Q~2j%S7^BeM+e zln5x@(Dd@n+pKUtmk?@b)#yK%0=3r~X!Mf<2 z)qk~BV=JZGJEFr7sb^dfjsBZoyq@lX*q`XFkLj*pbK0Y(M*BAZ4jfgo2&+1Kj6_`s zwuEd#`;;zi=8~hw<)et1cCnnXDcn>V$WTuvl(klaCknUxtQwiH{dajBI=gy<1QX$O zKOYqt$u>MgIh|5KP5_?>v+g2r*TFT3D~?&c$}}xLHqu%^PAS2@IDK+QwR4LtzkW^N zs>EJPjAlE$YQ#e7{-GBqEzx~JEb)HOgz(3pXtYKVf|0E}MP!Rn5@2Y=;`1iFgv2Ud zJ3P~Y*Za8Hv$l4Hgm_q44Cte3;&_R~2bLcMFQhB`9`29o-Uc4A_pRlSv#UjxCtYPv zj3bCI|AxmUuwU!+$)PkLxo#LIX=Ym-0yH%dG{1ohz}&`@c_a63eCY{j8;;PuKfAmv zDf;UCQ*~Maf z=EJpZqOmw$Pu6KH6|#FnD_jaf>6VbyR2`@!&**NiO3jgtbtu&>lZ(8Wkqx6 z4vAo8QU6{jG8Rd*z)KbP2x%l*i?!o*a;%(o91g7XyoviVN3hQ#+#BHXp)J*3MNBjg z4Go=wiTy=mDuj#9G%tS@KKi8T%uVr=zJ~+QW)IRS?s1(()_?x~6>#7mwl_(<>BOX* z>VKu_GB&h*Ln{wd3oB`t*Il9yRB`hsrIVTW9fc433J5(c zcJV{6t%Y0tb9+{^udv|)o`~WHwawSmLuiRXs76*q+U&z6)z*`Y0 zK&1Hd_ZDiL5O{Mv;9mCwVJt5~%s0W!OmlBBQMa|N?FxoZg+mlP6U!k=v%M?FBg2PL zZ><4ukb*RNiwPPK9cwaEXHxi9|84X{cOB4-r?!@-_WwI^uP>){t*-V-MKi-%-;S+#LmP}efF^k`E*_|f}m<(o>Pu$D7@RV_s!(#9z0t*2;Vb* zk_vHe_(Ft~_<&dec!2kx7~vhf@lnNgoUfbn=LznD&*et#mk{QoanW2ozAG2x8w0L+ zO=y_ExCi4<*;-pmfv}14wq=1F!q1K5krds>VJBL$0I+k$@ug66xgv&#`dTaH=o@G* zGd!l@5Kb7DiRI-MnKS%$M7aKa-rL81&yPzuN%YpkGi~4nC>L3EB8>I+JS8t=GO@%5 zUVw9&m64WLY6`YPewHULP~rsXpMxY~Ylsl4@GJ^~uAPk+>hd2aABFlY$}P1DAoYwz zE~qiTX{9`mb?81+Q~U>J)+0?7iYt=tX)}Q5GI^wBiugYoy}aI|_H#3$u?=a2w&C>q z+>@WhhAnJL*Z^gR5H1aO`z&&>lWmQQuAX;Ec|Szv36%Ci^w-4~mCH3d5RWF+(sV8K zb&>|C@PHcleaBG^0M^jqfZZCt7nJadFGY6CYFIN{b&eu-^|8~-L6$FUOI78HG&v5{ z0ikFmOjQbEE4N3@wcrqwh%CtjPM^te<@tOSfdk9Te7c5Q>5 zqmng0wo-8(8K@T$zPQeOUrcH|V=XuZ7sEwNrvj6lmSuQX9`Sn4b7tM(tL=YXiZ{>P!m527&Uhq(kqzhCE^w28?lhpUE)#Z^sAyl$l&8-g_=HR!VuDw8u{g&@} zJ35-JPMNt2(1(5hu?v`>Yi+;e4&}lh_8|OdHAV8O(0mjt`W=9NkMxkjB&a0MMC#ZQ zn!3LDR)G)F;p=CCgGMV3rDF~JN4 z*3XVK#Gp>3z4&3L*#6m@GwCs&>o!Wlon)ZPXyJqXXdQ7Vcze{*(f9QPu6XzVSpXl3 zNP~0^#iBGZam;&hq)qlGo54mcaeB}}mOA(7nC;RxcM8)HlO9Y*QJ-T>ZPJv_Ky^C8(4MjPG>hjOhP z^iJN+@wA3z2fr-#=$bmtu1|FYEgW0BGqw&9E5!8G8Ls<3)^0bCH**?(I3Lqp7o3b_ zz7`vE2wy=lF}`KO-zLSF(z>p>hs-0uzqrJmbun&`6RrM_*W-o<-)@@ke31D`g8lT^ zvG~pad;RZa{>N({j)Y)5(+S$PQ1Ro#_WV!OG3xh_u{mfH zkvMjW0#W)>3AsItCf+dk`RvuKZ9tYNgsf?Yz(`Hw%XIObgZ&Mc7yzNp_5&L4FfDM* zd_56x@cXw`@9t{f6bQ^kO@;ZlO$5Nk<%&^~X|)z$mq4lEeC8SnHReyLs3#UkdijZi zo#R3{TS0AEZhd*+C_dRh_hs2h>T6BkuO;s}6&H4PBKp0rjK!-IlLy+>Xd(^y{Jkk6 zXo`g~gL_m1U1}=Y3Sd-awki-WGzOEDKJ)8Wb!9mejl<FDynsYb>Ku;S30mK4w_%xl?JTh@+iX=7(&x zhXbyfy&oHwcJtCR&$8?vE9rm6#{+YE?vo>4CKVxz#jKB~H_H)8J$9dt-7IG@ak1)B zTAjt17$zpL3CAl?6h612-oWMIF-V=>ZI?0~k5nEQu@F8`&$3kH^A+6EZYU?mscXIC zi{*I_m`Q8eUEymARF03aAtG0P>~vQ4Q`AjJN#k~T8lK-<&3`*9zC9S%-JMqBe|Pz^ zqq~0Pdj~)PDLjP5yjv^IEU!pD9U!tca!$vc6wZu?Y}PnSItw_IwU%dEuW4jZ@Z6NT z(N(B{-#FJ@6NG%@;&;wK41zUVA15IBy%#I4t$csdF{gt~uFf7cWI z0P?}1Fg%In2e+HJu{>e?CNVwyam_p@Tmcj&Uh>1_IHb@L3BX_LW6zJ00Y6uvM#ie3 zVuMJR7JUA8yU5!v_!xD>S&wKOzENw{22`26!xOGMXZ#-qb2a{oVxa9-h{BKO3YFu@ zt#CBAC4Uw+7(rPJ9WADaG3lBr96zC!N8DfaY9<;f1UQX1->Vc}D6Dk1NLS=|9;?nZ z+f)x<@Bt{Ge!0GEz2|>mx1}hdbUUxg2HN>YXkikQ67Y8?t!uk#YHwm{+xB3n3pBuq zLMz%71$CJDc*3GV66Lh;Re+Z%NB@>PDtr|zMYq(}2{_N+GbS??MaRl2cyB+RMLFOb zKQc!j3-Ok4;6K{8eLtIm%s(P(Sdb!fG)_qDg+PY;SZL^|tKQ?&LBnIAdG2zLS%S)z z7BX}fwTEuyN-sK^CX+@W^cEh)5wL48y88@fsRu!?(wbE8q<9s|rl);*D=*Sw^cfk0 zxV7T;5YFL65^oTW7w9%PmV~@k^#IF_z*-DwM@T_sL5%0#Zuwv3bj9{C0BwX{<_?dE z=^9@S{0!NujY(BBcM?9$QZIxNWA>mR7%{KO2>juK^+0X4VWyM zO#_VBu}>Q2*fMa31)&kNNpg@qKb3!DvwkpK{&D3@n780$N5n~RMz|Fo{#G-h_8;mb z!W6+*HA-kH_YwE~sd1UIDqJKSXCj{QXTuje zUsBy?yOSPH2Zt5`hWQ4Lz=q=G=ckoZBn$iUBY27u~igJfa$`ySN z7amoYe<9{NB#tN5@jNL6u#l>T$8+V?V))@5%9Vc8@2|pZQqYJ@?x4x@D+v7icBSN3 zeehyoY%w{LtmffPF{Rd>3&;b@D)=QiMz#>My|TQ-Khe$7P~RkBIJO;roGpjo7oW_E zx?_FU`f#Y+@Rru}VODDkvH)1AbGA25t>F3LvhADZqqOYQCY{$Vk$(nUqW*m!8d_Gn z?;slJ^9N?JrhWh62hLfuKo$9wccs^(*Hc@|GRec?u=vnsW!_=$ejbVv3`OOT9LCkL zzR!U;V)WImFriMeR1;0x`PNs#QL}xstNWk!x^|}aCD(WHq`>`)kI{&Zwup`Ob?e2r z)BC%-!8&<6AzjJ~3kx*+6d#;@rkodAeLdEIpVB}PDr!`||gfA#rpJI;(Nny8<4M(XFGc@73-rp{J8dQVQQ2njuSt z$BM;hZZF@wayYNLwtB0Ujy0mWF#uAEoYB1lokf`wA^mXDsyk`eJ~F4IddJN!i_kD( zhfhUf-uj4xv8qc)>(de+B;Q*Pz!XyMh8oD-tJwJ3+MJDj<&=QtZhb*`V*AR}Of={H zwbKy<+bbAkj`M>@atsm(a33eQNzw*o3eAMKPRy>R3%(aR+u(>Z0t}~{R zEqr=q=H~^CUW{toxs}X$X8r6l7kvRTR=^-FNP@Z!Hu&BMM1f-x70zm;MhkvsxUhwy?rLs34GX^JB82& z3iY_p(kbth_FMuZgHObj5|urVpy$k_mp*G8_Lk4yP5@7@S(Ll$MRlJX8}91H+dF@& zuv%W~=6EL@&WQ)H>|VPOto3xdK2|2%LkM3_cRxk9=i@P@-f*&w<8u;*B@mu;a|@$U zOho+S@}rK$l-g}>kB`e}tX$9WLy6KqSNn#wNm7L`JSOv=Sfq)6wqWs491!x% zK+=XhubvxP9;ylz=&*7+#|UKk*U`y!+x}4rQ3!LRpgW_w{Mv;tj!?XG-MySaCt}%y z=^rIXI+`m3$)yj;;K3`ucAehnjsM_)`Ev7ANh^aN_NAymUy+jsI;N*&4)^&=0|?tw z6}*g#R>E_IB?<4+62a#1Q^518AGhrXQRejUlpnOO;dQiDitJmD=i|}4RoD0C(hjzO z(0FpJ^KaAm_|@XW%C_#2X%)SwkXlz~W_zpdQ>zCzoD$`|9l?~17WCesl%mNr(DhEm zt(%GE^_8h8-Tv}Q=jl$ssQD}hwhxs`u7?z=@G|$*?uSljU%22?g}}4a1rMV!Mq=qB zg>b9|IhJHNP;UlOa%#LDqC94P3*8uA;UiSxN#$FTTQRX9Wj*9>Io=HiT3@hEkfBm| z9+!+9x}GYTa0e+KH=xPh&a{&(LRnN4>Tpr)|qvs9kP7oL~{OVW!V%&$4kR$#Xas z7u#uR<`Bq+tTR*q(|ANx+~njT;XH3Bm}r0KbvOYL9$a`2mHkspMgvwOUL?|w2BkbJ z3EvkjQRku)kmgr#pV0ly{?Bv$0SB_YR$LA1Bs}5SFyV&`o>;yK28NI`cz&5OoIMWr zsc{IP0bTWdaKePK(C1-cxkH{g3ct;p=mdL(j4uS$D&MQcAL=VzQ;*`m`JF{`2lA`Y zU%t~3?x&ZTJ||$l zl2zv|%S%|0oGuNB9`x^|o@ z-MG84B%)mZA}ZDMv1kJd4_F;;KBT($;!jf@n+qSAjI!M<=8a&L53*RTYNY5U5+EC_ii z8A;W!khOcoy+OCfBrz&phI5aECqj~YPmpx72x5IBJMaCqsMZSM_F3hMYlimxk0tMM zRs4IV+FcR8^LKe^Y{h&u>4Er*7+W?aX(8q}byYf8?&Uus>C5oi#{~U)4duAQc{Bk! z`~3#ip}@dWbV$S!zN)xXV&OAEU*73dz7#tTpd6>pF=y@ycJ^y-c9)XjMWY%)z&r95 zae3{vrm?d@gh*WJ!N9?;ZnzXT4B-_Uo;#G_eD;LpT{TR>znfZ58tJusv!rc?K~jDf zw88H==t2$qG<=~!1p4rp)u9MbX-S+#mV9nJg_R8S@$g;gV7NOkfLG-BsTay}4=FO# zpWnYDZq+med=*(yr@m=fZ2dK$+F45vXg-dZa|dlPa2)R*A@jjdi9B+1R^_ciju3Q$ z+p{0ye3}F5mtY{KHov#W4i7_JOX`i90{O10b$1ek5)9s__wIeGBy;c`L7Zg%1O*r> z8Fp7)peTK&f2r@Cu?&Yk?|A(1c|Uf&%Pa=Ze0B5|iBQFMjX(`#H2ReQj4`6WE0L}3 zai&I>Ln8;mW<-naWNDnEXwIfz(eq%P*~vvW!w(Pe)pJXnq^l4wRCGug6;TT0$~8T2(T-zzJQI zdo9Sxn<;tYP~90r^#WqCFsBzX0qQ>b5m8d&zth>}a5&T{Oy_A|5Ku%TLD>cNN)K`9 ziN9?ciYJt?i*MZ;n5SU5|FDI+bf5CLXkzDj{BdgA*mOH5tG zqodtNW4RZpi>pMB=p)DbY6MuTkHTnj)Va8)m%TokK2wtp0z+1bu;K9MHOH)P?tIm=yl}*?aQY%0QLiG;w`I2%gi8s$d)PpQC?nA{#vt-=glW$_s`%h6S!L9<0|`8bT(aJcinJTSyb zc+oMOrYfIv*s$j7mtyZUty=#^nBe1Tnmz3DJV|FlABJ$%2}Y90A2cr8v9j_Ss-j&H z3R3N10#ctNE^~{~3YJb|jW+R-e$4@(X3iyZ1?)HvpWfduLc~8$$Fv zKd<~n_HIta9Dp1K3jlcw;blmtX!~ED?zvo_>S*igvc&*7B{|rv%>TUa^4i+z!I|f| z0i$OI)OWXlICCg>6U}dE6{{pel@`;>UmuPKY6_O#km2-v;|Ykn-fu9h%x0OCJ>X?M zbye5=w8Y5%a@U?q=0`LJWC_YuK3$7kTZmfe8CbhC<_~3#YT$SNp48obyK=C2Rpiont*){bYAaoXvexKI;Qo&}%#^PHQU` ze+A58KPr2+jSr8KssznQE&{6L4>ooV>DGb%BMSQG#iZF7+u=E>$WiTMhE}$r z_L&$(>%?JAy8;@SQisY~)9!^(Gd!ByuhmT@#BiwA!IMEN^kXEZ%qIudd>LWL3c+9G zc($ZSOmgx^zbOS9`{_g}^hdUoHH0Bx<{g0x6?R`|^VoHF6{t@pmscw>5VrlOeRU60 zOPv{R)*A*F4SyRu>*dMLU-L*-fLsa=b>i!G7ElE_(FXSD4-SdxBqTJRHFrGV+-_R* z`d({!dW> zum#4~YZ(v=QK{fY%-7A4sL5J+6?}qU! zi_LCK1YAaBVlW7NKfGkt5F)_t=;WPYt4jBs_aMzv@Kl!T+?;oSeJz6Yfe0}lm$-lE z+cghkxGC!rTZoiD60^1>tQW{S!oI?qc?I_RW}D$DCDm6ENnJnPzrFTQp;-=&kA8|K1{do2x>34a5v+9Dr;YEh!s$Zkf#YN~kj?^!YVY~&+Hgw;w4ab+g`6{c z0SUr6*i0sle*Y=aevVr&Cvx@0c))Z2vAFV2jfYY17aKs(7=R0};9o4LPOby4>a1Gu zup5W^eY{i8efxK7m3*nFm+<4IN4k%siQm*uR~klu8g%F{jT-k^$Qcn|Jx_!{c0^wDSX4BkjioBQ^fVWQv+=C>fVuh z*T#A;qf^{37*hD4`r8Zy0nLlxC>aH&!)I&Grkd|gHx`W~T4M4x4Xs>#KcCk%9~~M> z-E=mst$F#y&Oecrc?qEc@eqJ%d4(tt?B~pCPbS~EU&#)IHk_^hDt^W9B1^P)j`)WA zs0Lje9gNHLA4hxpoU(c4P?QMy7jjuuU{;($i3T zHwoj{jcCzZ8(WWE6U2QX53&daq=gic#yfH(~_lPEHmUlPdyFLl$v@(oKHAQp(x|Dp+>g>Ca(Z(d=a5yRpCR0F3Mp|NO*YkK*yGtqk<(1{) zb$pS!vHA?&nI+c_ABn!T?e@V-Np3kzXwjLQ62^;m$bLpx_GzB(%^n8jJjh7S2IY9+ zNntuP7K@w=Me5w$QsS+ZX~{euR>LPj3R{Y=?mkZoOs+78>zB}O$|l8`)ciJPIVdPE zAM!L1})dl{g8V%`=c>@gYdZ5G`T7CF3Y&i4O;@^GiVWW1)swim8=%RpX|> zjcO3HRe1mH5~4tG>ZX6pFRKkDVD>};(z|V?`x7tiUpGeV3J1nN5OW+%L>0~LrVv+VL6aH{e50z@C^J<_HMg(2z z#`%65yzU)7pxaZ@9KwPPeHoXjNsasheYq&n^j*UoLb^C| z?>=V%Zvinxh5D*n&)}72pQoT=Y-@Fh%s_oHh2^EF;p$|>%9+sR_8w8z{fST~oDJPU zp32z$yI_O)5-Q1TFR!Rqyt8Oc$aGiiFD%O~g#Tz=AlZucG0p0+^~!Z4sxUUek(oj` z%Nx_O1s{5Rbm(SsP(YRbJ^M4}_F2ycYn9le^xs49+z5i&3{p4qQIZ5+?z8s*$MPAx zpR?z)+Y=L$Sp<7B=iD+IV2 zC!#l^^7HiwoOkga^AiDyNH!Qp%?Wn~{A;_QQoPo!MU?`ngw0v7J@h+^vsZ_;n)gAk z4!#oWtCA|U3ba7*AyK|ORD{CH#PsjD_%R6EuJzNWPr5sncA}qnEh*nmUmFfT)UHY8 zpdVc{x)F+_5-nA>rf5u+<`C$FmsulP4$(TPU11o`z*gj3PZhRSVZBTfOsr)?ZuT<1* zPi}4$=B+)zC}WXE9bs_|xD_)K;WC7mSDs6g#pu`L4aZ)J00#)MKtA8bL}X^;!Ua&F zzMxF5B_v7Q*(y^3pjI7%{r;kH%KRIq z2JEwn!(iNxaC;Fy^KAy6geWDCf>@FfR3*-axi4WL41heWjUeV5?ywM#gSox$DR_=RiU`dU1Qy?U%YIqxV>yF=&~oS z-WqSrcmFmqNyt!@*43VR_5Un@P2FI_rn91+?aze=2-@Bq&weyh|1Rz0tr#|qcscD*%~3wF%@u@5+e7xbET^3*>w_ftbmtb ztrQ=C*oysSZ2AS5&~Fhf7bVdi(p2x%AUY$Mi+OhwQM#loXt<23-ZwyV1K#@g56@Od zzk}sQTAwTngJA#L?MJH($ZVzcvBTyLyndKxDUT1Y)qXnHcEQ}!hCQ7{g~vgJa8;(N z)-N8-8!`9$<-558<7lMTE0I%&l(BL_LAmAq+RFj9E8Wj8V>R_yHbu(o|NQKrAHXo~jEyA&;p*TL+Bf=@F^f=ohV~5R^C}}V zTLZfn&{l%tM*8BX&{G^Wx#E8Z$Cr~6&(43_=m6MqmEva=ty3F}@?tU=$uEsJ6DS2c z9Gpk7+`DSX8EzPyu+%D3{mi$GA~dVa{C4)#EJ>)4T#w_FKI(Xa>hXLk9zv>vU0lf{ ztJ8$huT7H+{6{%L?t6$~%0dL*uFYB9#M!r1U&8R5Y(r+LE1u<#*ufy2Rct*G_Ju~=zWwie=~JDU z{m3ps*G;{i>CMQemaB=_)=v>LO4LSWrN#VBP3VYK_aXoF=q_D}`+c)@D-pW^w8d1> z58~fhO@`~~fhvnBmD)FWD~`0({TXSCs^mYRLG_b*nSP z0nSC1qMs9oXrC1lBA9lQk0N*f`3vVNq@R4M8`C)vV3H`+_Z={6KOJn_!i*4~Ty}e1 zP|Qw{qTCo5+q|e2`SfaqB<%V4eA`O&+Vt$9qQg`(MN_d~K6eCdP4sz@Mn^G098BY; zP}tQk|1>T`^9#^fSRR-Gw@HVqAai5&)_(ygrjG&{f*I&JU8{U)YSU@VrRGrv-#Fw3 z*Wpdw-5%ZDg>zjcWMQ_W$_Wl}C_7A92CuOQVLiCTwW?Q(<0HEP@XtO6w1l0g+OM%6 z8fo?ZMU2qHQ~#8SflChOhBj^&ki?2=o+|-z@Ehp)ym#1sW~tXRC%0rULnPIGI%Pfz zCbi7}=%0h5)}L_vtRK$5yu)d6Qw6*YF_+4MF_8n*w5TyEROzY}?ML~mxr>NX^0v*2 z@`9UfkUzD$0LKR#(0;eZKinst(v2YIlPSOeX4JkvnHU5e3pGWW&vIw5rvA<5Rl~Zd zJPMz*R5Q8p^0j-fI<2yzr$UY@9M=b}J9hWW(X?6NmsMC^$v33B&Xs;yAu>drKTJY& z%b)2(+b21M#+HFx2r@vO|U#sweyE=x`Pc%|@u*ztF3 zc}}w%c}B*Aaz`_6Rg)O|eh14<9Te~36m@KGkF>-bFLHyT>n}tlJt#<=rMw%Jd!Yto zSOH_c{H#{&SZ>U-P3KEx#g%@aK5l3a1%zUg`Y7d4LgL;NAvDhr;vwMNk7}&kjB@%! zL6#La86jE*Q^*-Yuf&*?s-^N`VMkZK2iQ2>Q;aGS4$Grrnf>63isn`PQtJ$bN2SDP zkoup#j0}NY9J{#$95e1ox7^`W3R2?&rW;W}6L&Cq8$WWYQsS>VoCqb*ysOS|nVXyy z!v{E}BFQ4pTOpuIPi}vF6p;#3YvwTvI``2$@6xd5bZim%_DXRE3o$>XS0Z7cGyRp= zEJbyIa^=)l~a@8G$Q#0EOZ>hq*3>4cXkrsJF=MEbdwu~-k3m4h=D{(1pv zbkR>QgYU#eM7(qC!1jm6!-`)KRD_|~NibB>zIK|Ws-wphgYeg2xS3e>1-)>t8K@`C zYLH~9!qYXIJIm2smq#OYp5|RGh_}m{2N!ggu4u_$21)x5UX+ytDQcc%=^*Rp0{{~x zq(@D8K&agqD&e;e%2;0)3BW$bavf#NrDF;tMofbJ%NP3M@j-``2Q}XHpxQe-+``0Z zuv_oMo-)N=?NUbPgQL!O%xC1bs>E-F0>Z_?O^H4?Xoyjuxme%#@v-aKwX5NpD|Es{ zehS5Mhlu}b2U=na;n5c*BdX5K<^J|~$rg9vBr*MxPY%t`G)r#&BJVw<%=<9*2=c4% z=fKv>wuKyO&m$k|6as6cBTC5F-_E}55IEBe5#gebHzWGdBmO|jPSYCCA`=psde9>M z<>|u0G{(tTH+R1Duhxte=6Pl?IY+|z+lw^Duzq|k_!yb6a_Fd>_lr=cnlayD zu|UQ$Xl9+}?*Y#<=Nj;YnPfdJM%>y;%%2T9?;5PMLO+bQk^33Z)sahuV6s9Vm+&A? zlive#Wif|yDC$Hp8HZc`2PiXk6K&N?JNu)hm)km5TBBn=Z#qqFICU+xRo}J!K0iMn zh_ds|DT)8}MSSP`S}7Vc-VBXdDz9EDum2l}0(WCPI5(=QN?{P8PLcwN@1E*{7}+K` zwF8M+WjSxOa=MjwdiVj_=}VxLG-)}L#=OyD@`TwsDKQQnV=v0 z$`9psZcQ(>txzWbb)@jAR0=;N|M~vi2iiw)`VvErOL;uIy$v>5gwWXSD`rS)wSUZJ z+Z3oCZbTVNZQGDPpiNZbh0*E-X>fOb8B1e^6N~ot;#oW~y`&m%X0wC_-Zftb=7pZ$ zxoaqZd@VQ&bbQshX#OuuqNG{Z@<`NV*BB}Ew|Dn^97|kY=9&_4q56~RaMFP)poo;5qO2c z_qSZ#LAIYIQCnCPvy{Q7Q03HBL<;qfKuI*xsSs@%4eF?pz1%NLKRm{p)V8yyLIf$2v{tQuS6U8fOlEP3`)w>@ME~$NU%+2EnL(h`n$@QeTQi#4s2r#d7H6hO2oH zij}x~<&rp0UZD8>(bwBXx9oizCsmp+Kh@E{vY7V}ebZmI_=Tg&WCR8dliyO~533^E zXi>?*&A49psh0MR4)EVr6Cj_oQ~t9w?I}aqexg+w_`VVG->S!&eE|7EH1hYGY6n|m zgnd>fwsmS*x_aL6TYxQ$!>e*xYw@$<)p@csnscfuHT?SVL#z)%R1gMS0Aiu$jKd)m z9FIkfeG2W-Y@oTn$|na8ORJruCqOEZ(3wmQ(Z`8EPLYmA;jv{I!6aBh(nT@Lc_g4f zj-Q>>+_m4_IR$EXB9trW5X@q068JVju24eyK`~-bjJ@>ew-SjQ8W?@8r}aWo>6A4< zP1jiecKsaD^93Q^a4&BaMwR%RNN_6nsd)mF$9!^L7X>C+UTbC?k2|Y;N`Cz-1&Iwd zvnq^B@pmgkoJ278=CH}1ssh=90FiC~@AC1$KEmVp^p;}ax1?_FEdx3@X<)Z(OGSK66#?f6u^mgcVeo0y7;fJY(vTArluw>t_) zHzwLYXrv0y-0lE()moXgdOgeOsO(2yG?`H*2m;nUnfQTmL1pByMv+EB(k`G8a!=oG zbHC(90W3W7t;w^-ffyJEM?yN5)C8jeB@ki3QIBHGt^aH&x+~`SiJ2F2WQ=?e=ut-f zX51_ZS8T}-Q)DUws{g_bouSd<#uBp3;9?+(VVtV`j&k~|0tz3WPCgo)0YS0;in}o< z)nZMvY#|*vOa0iKN9Wy?r*eFeW%;W&$J)xE@WGJsFN-;$z^5^GI4J@?FlFp!_RzBZ z!bQ)kN{DKB%co0Rj?WnP-A=I`%L);Ip}XUAcXYm8Nv5Vl&{wV>w!`)loTX33OP+Y4 zAgVooQ7TWIag`a4{@`z+NfkDDgJA6#&^*85&W}al&EIlP@RE>5K2sYo4iggi3@Xoo zWB}-H5nqmwOz|MXZMhhj%%?SbrZ_#X{hpm!{j?c1ESA3?-6e{aNV~jNzq{dbpo^F< zuRi#rdXRy=-Qf)BZm#r2fCmEsdI-TuHOVxI*=HUQ#=`tZEsd0XYn~rx3{FSaREWRR zE)qUtf_+opyWlU`0#Y949d!p}{YgvP+;}UHs&FS4z>gV!D+P}EKO1~cWR!Oufc@;x z`a0kNCL9URN*{utN%UEb=-n-S%!5k`#f+8-BODt0^_PDBpB}Zy-L2K&7RCu-bXI*h z*WVp>??nnQj@{C|FS}=aqKu_T--oVYPKW0I!S+wLs{jj`O{GZFieL)hxHi1$D zQF;+EP0Dm^_KmCClw|*T=*Np<>laFo!fBbs#me&MnOOv*dN+)ZljtxMK_$mUeSp~# zoLH=uxCCyDvtR+xK{=R8i| zSv9_T?oDiIk)u`mX=oQKVsA~cD4aAL8 zUoZ7@`>Y*aO-nYp(|?@8cdcql{u=yI=hXE#dhtKsJLFz5(lHqQ2z^Emjkf$_*+HdP3mWy&-WlA(ds&2pt62@S3^H&E9gz0v- zM$!Dwe1PG5fGx8)Y|;kD?Iv-O>T%`Vn!0aQSlb2Ej7#LcBL?I?h9YWGC&3pxH}DQ@ zXg<|Y{^zE6T}ZL2mj7(9kn1!0Bf17dlDfln1CDl)u|NZj?XOZH)+(>f2TxNber*OxyqE2T zAHz;ok1c^Sh12!t=WWtM?MuR6@jVRbbgn`D!jtGoJ)Av@n#hWPlcYu~p>PQD<|xGUzm}Z^)pO63>P{#GgoDt-lee zmpI!UF&0|2W`U6BP^FS)rR5v*Na-I|{w;0uQ`;f+)k_20aZ0*OwsDSip6nY^vv?J% zhWVO>uRC|Fv&e19BpH@sNr66sv#U_wLYfQdA{T#5Qc!|&8EBrN25Csz(nhtE5YCZv z7Nz+-Aw3HK0SqD@Vw;=YNqj)F1iPp_Mf)7)M-xvu#03@N=VfT6AL098d3>TNZ%u^? z#ziBW3E?vv%V$dEGCyLvXX9W7K6%07G%IdX&8vAz9(=KQQZKm}+k@h*J8ZyzPh>KY z;iA)S9V&U{$3K<%Z#VtTcv_RPL(G6DUWyH7X!7>#N&74e70}bU*IC*)f)oRlvjyET zEpz4N1(tGT?aC>81R`D-BYqVG_oxntvL*mg=xg?k;>Py-NbmtBDU72)wz? zbG+OktEJ3$QD3YGRWy}Y5b7j`e-kfFlt}9XVP0HjFaJLQny-Jb3Z%Ar=^xNb&93sC zjm7e2pYe;z4_InV_^3!s57ED)s1xu_goLD?Z5eN?7R_vV|LXQiAjW|7tHX_{dV`EqitKD2N$;M@%n$LqGPjR0Iqm5Xnm`t{7wSXrA&4k>74dw#!XE?UY0 zl;a}fE-4UEq@FQVm&EB`HXv`42O^|T{`)1O`RvU$!5&7tEoLI|ydrxfKXZWaMB$|s zUTs^cT*tL8)ejFuYUMe1bu47mi~+BB-{P8W%!ciMhi-I#=L%rx zNAHZq?5*Ujftwe&Yawan)lcvDt$i;}9Y&N0>&EPFx$HIQs%nBg7cYGe^ z2d8g>qvr@hei#L_M))hG)bgdjSuxW}a(JEPMo)CSBOK><06hw8@%h@dJ>C^%tN~zQ zC6oc(1LlGI6Xw!Jd1eXePVD~H>TJxv%P~9aH$cjqYbZ`Na2|^Nk7{b-o>Q_q$pb66 zvN_p(seB_M#`u*&kLK7{z3WDZ_<&;n^B1p5`+&=e)0Ayi} zOMjy>0CHD^-5P7AL#rAy`W=+w?H25nhTv7CPnaWUvT$Y<&291P-vWV?(8EU(8gzdK zrdeW$4D%~^#)CMi`-#myd~Ydrd(mDbu|5EySHf2vi5m+&hJGV|CRDPJNivkG9^;#N zmP$wS(lq6T&09WC!j2yW-Img&4c=4~qZz`+WP{wJ|65)*R~kY^Y}VQNN@=59k3aUI!LA&|15lS*>IfghbDqb zx1o6t>O%cl7xiwdP+yyl-vm*FU47pu?Gpz4CT+$ynPW9mt)0jFjwF{=RnJWAFC9EV z8i@_GFp33WdXpOb?2UYz@$y&_GE9l(7=hO{d} zVj}lV%ZQxU!>3(&d|FjVw>z1))KSaAlc@1?RSweW+ z=L4>vsWsVEORb$#E3>G4ZX7r38O~U@pYzgTlrS|d@2X2G^16<5M5sR_G=96I-0#X5 z4o9WL3p-8W_YNb~X(KHg|CYyBZ*i36-C-dQid`L>k8Z;?CnFZfwahz9`mJItBuax| zfseSWD;oGKQ_tK|&kG?6fq>F&drv@#6^iW;Qm27wPr%(KP;6Jur=$?aa!nYrHPZGV0CVr5RYuX(tVO32_KZuOqH z%7P@+o)og|NltWEq)-BVPkBi=p8T91wr!Qn^R{y`qKX=0-NCNvlha&YiESJ>j!N4pO#r6|}@c~43`!_?!pI0h%`qx_b zpynLm@;z$mM%Tx#TJTimj%xG@ACXCVUO)Hq?$}oefkbQIJUR}>=}@L6(C+O;=;&Tk z|1Xh)r4^H5#OUX;WGMbei9WsljEE{YJ@G~BxCGw6(9X?fs7e?ASg)(&*c-3rpacTD z|D);LPh=!Ff$aStt*h4 zS@sK6ow&&(Ge!Qd&eL~I`V@O>XIwWy%HQHZWTCou2Pf^zTJ@B_%tLM=1`K{9cQ#8@ zmBS`&OAsaODZ7+SXRD{|bG?F=ytEz3HK#9m>K^Ga~@&TOUU@SZ?jzqx6p}IT3HtSy;SDc zAq;9r5@i9zAgs_6p!ewU6f!UhRVVH9umRXVE^^#F%#_gKy5ZIkc?bZx_m&b2jii&W zcfsy8P{cR?22I7{=}5C7!s(Q*?*SXr*LRkWoi$=UgU6*|0($Aq(0yF#uAhd#mCb{1 ziS{Wi=`-*;XB$Y$N686|$hykE)7K}z$A<31JOg5Our2dnp&QR5;yUisztiaTaLyW8 z$N~2F%kxdK|84%=#O2cX^tJj2%+ZPmOO~DX3e@_M&yAmNCtq;Q zDJD3lsq)sjN%ip`))34r@mwZ+Bx0_W88uy$pNLgV`4e^<< z&y7Pwa`06?3Hn6YqKYj3!vj1~R*Q#!+-5mVEXtlpE>9MBRFQqd-XP8R{^(z3)Md0z z_>A8zOkRtWF@tlvXMH+WCSYZ5Yr7q_UBAcss%lW+V?6kvt&d%~H~Z9gP6{PS9c(H? zE160bNN6J)v}hZ1PNb@d1+{`mP*v@R-C#AyA~HBv$$_vK!|;&MGp+peEJ937APgYe zS!ES-1m};cac7yAcV{m1BQS59Dz3PFT5EQmP(t_)OS{@ouIuCj@LjjW^vF-;_{0zV zeLmi0pY&LP?V_-4fGJ5QmZzQl^m?N38ae&PFZAO3MJMJ;&yH{whmpwJ69FnA>(Ec; z3Wzu>R0DJ17DoYin-M0_XAQAu<%t+nKre#Y^zvKrLenLIm^-575~m_D<}Z$a`p*Pn z6;NY2q6tRnz4hf|eQGuTLPhV400A(<7$Rs8i75J%^GR8G^vnk^ksS+JjQLdMVV_gi z+sO2c4^oVO4wdLoICm?foXBLWf9OxgxRV9ok-(vvmGv)HfHOyy9tznFFeub7e5q7< zAjE5;u>_-j!^8t`DTX*VqV;ORCF7ITB*$gk902?BT*c_Jiv799buZ%)lB<`HrthZS zUfcsR%OxoY9FqiO#)#ySU`lb*i)G?|jL!7PR;C8;VfoKctpfm z*_7u-L=9IM@2KQ2u}lzhd>0bN^GGf~md}go_s*Y&r_;WH>q5JCbKQJX*JM6fvB;bg zp*NwfvMF2YNAl0ro;|$cGZCz23+bG=X9(cx&JV6BB>FEu;zKYrm1UJfbi2BEQe&jk z1BDJY+|BfW$8iie%*toUxHmXeM1O9@ll(^~Fy<%$*3@**WuuoJGtbloCU|X>fKtUa z?t&CVPWDY?eV1xil`NJ++NY*70P;D6{?#tX*$(l&*z^dsIs@$RYmgt21Mg?S1bpUG z^PeHL5`VrjaFh~`j6mP_x4T*^3QWb4@8zet(o!w>-%Z6Hn?1!;G4r)k2_-m2o(^Ed zt^tb1+L_I=f^~poB4fOjsqgGE@wpgH+B>#|B*Os?;YXHsD3^i1AMKi^^f2MHkcsm! zoZCs+Tmh{9s+`_lHgcm?pBne&xk|>Ye1cC+J3Ep}s{jc9hyDk^ys7wd@FGe zYWgho;CVQru6N%qm_l?8%{N{6tIK-+l^ETr8>N@mYvOnTq0}goa{0sM4~9D{cto5b zCLt|`(Clm?6!mA!7o>!yJHzcP5()NO6+Lr~<`d9fd80O)j%6 z{6WW;&u@_mCJt=N(r)XpDR|ghfP~`fznh0Qu4~bfvO7+16g6N82^^)_n0r2X?{Qm6 z8e8`3?+>n+_*WcG+?TGP_+=xpr&^Y>%a_UGsOb0LLP>C@tkut#>u-xz3PYGaC0lbzc^128qCUT(>7&O%`oN+Er#X? zw$10&k&%&&@Z6I7f+E%8x+z27>gJCazKL@{H#)x}V!V?vzc*Ww*QFDVrvXbb;NP(@ zfALb_{oM}Q(q=8LM^dQ>v}AR-xuzF$Nzl#h?K`B8=+BnexAI)8_hkAim zq9{RU-M8$APiK`C#s2BNdy=R??6|=m(0b?rr1Uy1Eo@!+4p*RwzFS_SrAK|IYj*Xn z(TX!9fB0&BvBcyJO;bEA90GedStr}W|5+%lyI}wGg>QA91?{wfBSeVX#o`s>o|RtU#r5%?sK1OE$vT^=_7Dov-l#ME-?<(t5jVQjmXvwDV?Fqk z3}o6WJ+{}UFFSa{-)LGEr9tnl`PdKFEhtbUhCQirpY0?Jw<{Jg7Rd7ytN!YvD#}zF zTLOu5@*VPow=hRMMSSG}4R&X6=v&jvE3IrFPc97?^GVVc6Aq!JD*3ju?`xzeb>E%P zN{+I5j&0n)<|I(lmxkynk}5?Ed62Vt#FB9SuyK4dhsf8%Fo;Jb*A!i3Dek z5Ntx@#NzP$0WXRhj0jbzHpu$`dCo_>SH%o#HAvT-F*UBB^NFhOgth9`&r*8)DOqt> z^ho;->AxW^-AvoXF^=xXIfFf$L%;<@Yg=0})vwE=IA!T^vuGg_ajqZ3T2y@j#f>aLOCfdF<36A0=jy zX-WSX*>2n`yzV7q)?&?K>-uEe>B>^Qj=-wQ8Ek)4jk4DUx%c}_rLlqI(%yjh8cMTI zdO=Rd&o%u`-Tje;Fopa{yLJd=v|ndV=H8kU#AvX;I$ubV%Sq^LX99cUon+5kEI0p) z88s5ADlyhhC3?FuvS$(8Hbyh>3=r4m(NHWetGZ;aFq zAnq&ICc6o~5#=j{owCxoxVE*UdAaBgKLhsqTU|tGXlPg%AWyuIrok?uM$b79Uyz@F znTZL)0kU(cgW`QNFxmXHfpEUIUj2dZoFrw51PU`7YpxXbH&yr$kliCYyk%A7ZP3Eg z8}TlX`}_I1hZlkp-bt34hpyjW?!9VDXmm11Y<2Cft{yPz(n)C^+XWX}o^#;hd+`O( z8;98jd^Am+Uf5IiAdt)pcGC)UP8LDh?Q zYj9GXh*)#(4b8cUG9M;TdZPV5d9bMK%5a#}#^IL8U#`Sce+{&n74*{*r_K-AcqTBO)GNH|M67AVTr zOaOdg_&TL}$jjxRwvYIjT|G^lVXKRRN%B;wh=r{s&zuV?zSlG`0xddq#Cx_MksX4E1|$OKr5dP7p~(VJ@*-tokwuRFd8zJc##z z$Y6{hCjy1&zRegst)9j&9|y&1{m8&zwXo$Zi$I^ItQARlSjZZO#NdOkb}CEK=^f~=@D9S@0Ixf=)V;3ME} zl`qrr@c6B>G)Nt1*ZZF<;1YxOgz-MVil>vRD;8fD_K3FyBzB7Brv4`?Eh2CU4HR3# zuR~|vc~b?95hm&4eh)&f^GCl$`DV%T=X4k#V=WjYvnFzY-cwX!VpP=e|MY~EvifSOQ(*Nm=5Pm59h;bz6g1)0yP77V@2va_Dw~v5v?pCPgQs~pDziX>f+(_2vL$C zq8dP-R6;=md+iMa6xA$`R#2g_Xp{Whto{Sb6oe^!W;Mt;lSky^{6f+e;*h4)L48G1 zRi7ON&vN!C^>CgIoh7NxS5GXq;vEpodE5vQ`gdyw>&yRKjtI}i!M($s53_y+Gy>f{ zSjx76K2}-2eJi{wWr)yPA%b+wzEni+QWNPq>l^y~IE%;ujBS;n3f>P3wss3$Q|+=r zBF`&+Vm7J|B_Ml=a`XE#wMK<#^zLNFphK;5bA=D%m~k`as+H~gexZ}@N%!&by6Kp2 zHNt7Hn|$(9du|M9mDdj42A*x5WL^32JrLn@gQ2B^&9DpV-#lR@I76Y@T+fHk$SFIIq${zE{eTpc$Z@>z@H+_xTaH=6$zvjNZOM+22E%hQZs%x4|AOOFDV3w zir5zB_mRc0$gL5>v$B8T@i6StxPy{T!qU?#P45JmUI^lv?BY#C+!a%VJXwtnePE0z zelese&>jAn654>r?|u!MXL3;qRg zVQ?y`YpTJ4NiA!BHK2W>H5lYW|b-9^4%KDMN%ecUmXpm{3sA^>Y^dHe;r%N3MR>JgrjJ zws_qj+^EOXS-aW!6?yZ|ci?DMh}&HGx!*?#YcUaQqSy&2 zM`DRk@<+zV1xSqk^!AU5nEem-N~1Qi$mt~|tk1gC!DG&G85p3a&kX$wqB4p6pZF#; zLES*yq7E)ZYS5Ud_qxFmA9TIvW>;4&FMM6y8GdzrcPT+G;8OeMXE_a8TWg!%;!<10 z#%3)!)`_2be2{BE$2a%_V4mES8=Whwjc9*_31M9>VJwFPj)QubdOKo$Y4V8EMs28wd?dXK z6@5tj$17&6U_nt?={LIrU)|M9F#!{V#+jy_!XDXqQHoRL48wQ++M-`1|20sfTayycAc)bW!1(0k4nfNQ0CM}a z5n;COiHpb2pgwZZd5D6CWURvaByW}hxv)O9EJFkyeXv{SsLuY_X2N?%6;ZyeE(I#E zLL%2QFh0X0tn1jN zpoa)8`&aBo&KFfE0r~I8e3O~a zxDGa`D1~@!=VHv!3WFQbRbu?H${uqavSBWA{$Qes%r(g}6CNY6Bz{iyaY2!ND$Em=AzP%sLe#-Q;k zzy*qP96o@`_qz?lek%qcL-+O{u5n6i$$+1P^Mv1RP=|Rq%saQ2zU;fk-rjyE39B5` z6wWj$6mBY9{uV&zmnVO$SP)6r=VGf8GE9hl0IdC7C&bGG$2134t^)D3lCE{%K=1)d zLS$h5$fWHXHxW+LkvhvUC_2rB&tIfc?$jgQ@1D7Pv%b4pw3-RI&9;Qfg@58`Fl}++ zK0CbfQ3hAyebSl8%@4c3>N~eQ9=EQID6Td{18wUBAsaU9A=@K{J|K!CrT-LT+?$4+ z;_N;U;+mG;4^wJ1I*>#Z4!`^M>K-Ua)u)i>kHqzK`PYv;!2L4k@F%Au&Le<-appA- zP?JOKpOiHE?>zfR z`{o6UCm)6LJf4F2E!+5V@zUdl31s2WJvX{(UV z7k4lj&IOUlv}&M@r{66(SDhC?NrSV{AzDkatk0FoUN#Ds^ktO73cvcl|em!sR%qhvwQPj}s~ zaL1T?q7DdbXF^w`FNs!~GB7}Jo(EB!)_o{QQCcxhcTSs%arm#CqNs1!4ldgMu-(bE zy@BAf*@>#x+a0{i$_3!)3+1(uzPx_D9Z{}}UE7L? z(lIj2z{lSxsLr&~`=Ak!QtbK3GyzKfq?`4!8h!_9iW||yG1kt~b%c`?xUU=bZYUkj#BTrPPHP!Jj9IhTnDlhHZojTMWjjMZE z8k#fTghnV*{oB;gh{D(h+2S?+;~OydiM8Q*50{(S)r=wQ4@~>cGx3ubyx;R&AF{pB zc3O8+los)qh?(*(NEyznQ{u z3p&_aibo|8?D6ywnDI{#yqv=;2DAbkiO2&ckJ+W#je3 z$)dMk!ed z$bXRa?*W|mZjflp+4xvlemxbTLL`my0584A!EXndG#J&r#gSa2)!(wt^gsQMO$=0C zJUjg>M4#*OYwMHTW@EP#JM9iGoa}F69w~p>7n)@q@63PZvh#491m>+@yUBtH#6EMQ zKkL`f{i#vrspWdbWPb0PXu+zf|T zrCWQEYiAbsNuUv?RC4BAf0imKO`;HrLiEC(mh_fir2iZS9g%P3YN@{r*p}b!yX|cI zg(De>fdm%B!N@jgM>Q>vWmLSJ8@r#n_S@MAK~x(GS&WmQu$jkZUOGreH-LNS-CH;? zXOkn=#sI-NI7<66p7aq`T9b~EEN6K%I?-WYp|>1|KQC9_B1Hg`47=pR_=bo5Q+#t< zkIy?;>AX}SSAoRC4y*HyAQUUnw_o?JT-oVG7?i`NfnQF5JAPY0rexr@9?L!H3-3tX zJ!yVL^}5G;%(pUWe@--H&%UVs9MQ11%>dU-c^c%dj=tVRTZG9HFlvAf-yID?zWMup z28yHFo?ZgYh*Xt%duuBQNnzRjD4q=!K}IXk2Nu(h7Ab_5CJ#8AVY#6ngKp$RxG0k7 znKNwWBpPT91Ot~&NsL~e=FaYF%+UW8ji7?GB6933LFVL)&?N@gI{>wg8C**Q=yu#- z%(y8E$h)CGhH{n5{1}SN`lMU`d+qM_aRhaEzIJg-^?Dog&ED_k#j@J|>W)JYsqJu( z@ZbPpZp5=7$+dc65srm)R0+3b!{b2#*t0dw$RiWY95WtERd%7plVCd%)1FBVqGNVo7 z37j(i^o@lDf(&1wrR^{Tx&T7|3{=_GDzR*k{Ox*k=$phND3>skgn{!Qc%E-D7-H^0 zw67y07Xa1dan^bBkAtVFHlEJtUMj5cB4X*PIEsP?31UDmLemXyb>UF+qf4^-o7@W{ z&OG%Qo^LsIq`TmR)9?Y^Vsop6Yus=9rgiq;w(O3IP3>=j0K~OuccJkukMo?a*D~LP-on*Zc&o^N{}s^@ zX{ahVm_!tR{?_=6UcEZ~sA8}+8SPIGP(h>NH_%Ad(KrR76}rfV;9TB6XJS+|SOJv( zpl~W>Tx=Wr-P2W2{drF2)}5SNfl|g<_ZJWDoTW;h|*3Te6XtDArS9>jJ;#yTsoo+S@CoLL;5 zb5D9Z`D$M3*{fntM&Z%gqIaKh#0gX{)e7M*$rZ|h6rjq?k$ihAzZ1D#v6`c>c1 z8)rH)xu2YPXX3h#Oy0iY#8pghtbM77y1=N(=Z@L{$$$X6VOG5KGjMZrbzVp4gDSuT z12;s(rs{65A<}g3Z++Az`-7oeiW$6n4G8YL5=4=v@FL{FnCKxp63)LLgGhS~Kle)! zSg?xA=0TwL?-!x=L}*5B-OG44C?*|Kgh9yLP?>_f41%-Rc*?py?aOiq0C%`x6Gbof zIuDCsHpePFN9sJWByuH~R`bPG*N;$*0&b1e5ysT$bqB@kIytSfZQy=;y^ep85xh9<}4 zVxFxYT;f0NK)ND5Mi`r)FNJcccv&1-4@YL+F2~uvfg#`Ksu_-BO*F)s!x%S-b&Qtlu1Z>iJ3p8lgS;;=f0 z%qiSRv4r+=DU^(Cp-19 zcWo~j8QDgO1F%tGRq${g_OM0cY>9D3g>@Ve&Y4XWMQMmzPLCXisUQRz$blrWKnqyI z3GW6@4oYJ}*KSzjl)`8k90|i>AzNMJTO6*ECYsp@tOfcZa$IwDH9(%oOa*+My)>vh zuJ#nZH%?H@1pG$#7cpBVRvDVsrX`+#3{LgGgJ}nKy_1yxXS9AFcD%P*qpDf8u3a+ zqmOdE_l^+RUnH)>o|a>U`qSaRozR?{#YkR$%94h=la*-!{BGjc7isC8i4}Ht#J!aj zEOb3c-cAq%cVta{{hFI0ht9)>X#THy8v>7Jgbr#r4Y|_QQ}*wgdOw+-jBdYZ-UtlM zja79+VT~?$bPvN*%B<3~qW;N{l;LWV3h-JpG!vjr6+CZQ6zTXFtE7)S8zDbJD(_k% z{W~*kz{QGKQ0#fAXgbp=&%X=ti(~BV|7!s*BA{zYvOKfu>=tvx#oiKzAbcjQ7075q zbFmht-sXBnJ3b@7Cw?_}Kh5Gf2tx^&dXjzy>bX=5Q~ubwkY9UVMF-HZkR!Kg*_j?d zn7O&af3T7K=_H}d^U3h=aK%CZ;-8&G!9!Qx#t0;Uo{IjBRspLoG7<>JkNOyyE4TqO zmO=9LWJrg95*&*9R&h_Xe^QOf7aGRXVI@>%{3DHm_bURLmdzEhK`svQ+bLCjUsp^h z`|_rrbf!fMKwHmvt!P@3k>OfDlpCPB@-N7Ey4JxbAd6?+g8a#f`L7o?5AO_f0Ajd!2Hp*s+fzNB)!u-O@Cb~683psrJ8S=Vtacg&BLD=*o zL%;LF!elV*_E=eE-tU!s73L+B31Ff)gk$fNgDxQuMojZ~9P>;Jh3P9{h+Xf7;oB#G zj{$N*{>#m@sqsZDi8H07qZDMa!e^Qn+wO+>7ZUafNPp(V4qcRh7s4sgBX_#%HSbF)6eOgkeguF(M|Y0+as+qqQZP~`w{s4T_-)%` zNgu;x z4$c)oh{RyKWEcB?S@^v+0vrGh%zk~j6vwJ^>g!CoJn84}B8F2LKU&SI6dTDHSN<%_ zO^SBi6?CVRQ1s4f*nhaK&CN`V0CM~9^{6difBZ156DAk$mS;WW5ViZed2J0yEYvQR z&Hn*@OF$bdnE5lYJF*A-dYIFIq&#`1e#rF^|9zoPsdyFG7Vb>vb>2_bx~#V8vplpSAcg_)`nVC!`)b-=*|%PLg{iafW53QV#CMFk1WyU$isEB; zZgTw9&$?z1etsu|hnN{BKN?C1LCxFwm73{sDc{&StbO81G1&H1;%H^H$a5?^&BPxw zV6h4<7Cs@Ij{m`Ru@~)aS`Sd^7ArW1`TF|c76Z)7`R{!?`+WJHBA?GNGu-|he{%Qs zuVC;nj2+KFhV1>B+~;Gv7%`&RNdAy#@RgSPA**8kZ`Tqd)$n)FnZ+HQl&u}kWNxYj zS`0fyf&1{Ek^B)@6?*RwbZ95~WbRY{&=4Lg(`9M3woSev-V!efO&JnwU!($jeIpz} z1>PXAgHrA%4T)Xb+6a$1*@{ou8;ja?47n_TF!4i|5*}yRVG=O`E0fa+sq@u(EM3;{ z{3@`zj##0^!;u~9`@uQpMF#Tk764-b2dX7wmf~BlRA$s%_vVGe(~F4TLCp*Cu0p+@ z^8meGN#Tj`g7IEkoKh_SDw&H2N>$r@2FBNSap{i@`fy5*M=t0$^ew3pa1g*a-|S)k zn{mxc7}-r801IkZA^i-hcmL7X1{LH}>vlJcn|dY{sA_ecgy(PbootR<3AX9+c|k^z zN{QKDc(P*qKG&6}an;R~Ojj*9l&`q8*IVvG7UHB-xTj@+d`aBSOzl7(>#_O}VXV3W z{Q{<~T)x&eh*IF0F;)1wxaJ7GK8z@@MMk@90=z{E^GZR2>4E6!!XXFgV{tk%skNDa zVF6&!D?1f9DVn5f27BZ16%$}0Lnu}wZ|0N~2x79@wYstX$q2bR{ zA#y!a_paO|B_qpxz+6a!!DeF=)9^muf(R*A?q!{d!kqU=Ra*<=b>+2|h&MR5qJYO) z4oVS3j$<>2!QyRUwj~QazxU-vp3a7B#PEr4hb8GXRds94;p@HTe-wh%^d(2@!hlL47YBXwVk2xu^AG9M() zoFGO|2}vLO62f`NEI1wid$Y|GR3qsFAf+pRbAu_Ga11MyHBsp4T{}d(O>4lN`cgwB zhvr;XJ&8)hE9o17ePZrKd4l0Dez=3@ag81*vzGhE;f-rwPM1g^up5)FVO^O`+-Tl( zXCn%aAlEa!Xd}m4sv(I5>3uMhw{A)Nm)@H1Lmk=L_DhJauUq5Xw@+l>5{w4Zxp8e# z-{jnt9#?gqsYBmR2wh|IK`bhZUec0@%#OU(f)TE zDTd+e9oi4%9t3ed6TTA?Gs>ZBjRApdLkom``H7wz;Rdhzxm169Au zWez6gseC6!rxFNGexnmaXwh9iTKVa&$m;>U^2bi|B}k&~1bUVR6OSIaFI~6=N~^Ug z{?hoCXKrWo%i3--^aP*%BCYIkMs z{0RV2-H{tVr%$3w$z0bwm)xA~F#HAzi>*j0kU?~9I5-W+w~8<%;ODmJ>#TKzxmT^VshY9dP)s zn2&=dbX9quaf+f`K_jv~QHd3c14rtd54n%7Hdic;)Ln6qyIhj&si?iBk9#)>hM5cS zC&iPfQ~gaN!l=Hn$RELHsI{ku5{w|raD{TX9xxh|B;NLJnGQJ(;ty;JK~8Lw?U#pW zAmO+(qO~3^p8Qb!{f~QCtlH!0c`MrQ>1(ng=MC1kbTS>vs%Y7D^Jv5?Utd~w#XJfk ze@pFSa#h+7V+ktx+!#kc*Hzxz6TrB5Lo^jtDP>&5o7C=We z7Kr!FM4KZ&I1(5R+R0-Jn_ys<+g=WFn$^uJ%b>+>1?oSGTG;x%vvZhKla;mq*D^nF zv15I7StxYOz#HT+!=H)eiJYdel>ASBm*wlH4+*Do0c;Ozdb^C!0YCHyah$& z)bZSFoB0!IpxRIJI2iUmU63`Z`vE459CXMd@plcm@fs?zTQbKbl5AEB!K-c|~=N&Vt`#3AqP{`Ff zX0%r_0k5IZe~n9jppT6Al6r8!g~X3i$-e9K>Ja=Koqw>ZB$Mlzss455_iN>`@~O__ zYC^X|C(~9rc$BC&a#>)iyz?V7RBMRY4^|PV{4_jBC71a8XDOVcVEd;6k+m+P%GoqS zQ9A?_H>Lf$_rOHUTOuqcXmrSGGg`86S-#c_`}SB+rAC1e?{-&@?9d$rqjedlx3k8gED z?#XUXqRL~&56Rch90detm3hoXEz@J%kTzcu9#Mq%F6Z6{FCf~uCqK=SeDJdl7qQfk zu9g=aC91DO`%*hLH@Y0+aJ57te6U|Na+z(oS(w*#@pt%Po|C#);X3fsG?K|w#Cq!9 za@6MT)}sGeez=;x0yWbk{-HI*&G{-_a;SlMVv;R}(t-S`Yk2WqYQMAVi}M#QK+aG8 zp~DxUU$HvQfHece9rJj(xThVfVVttM$&9UyorMKuU(0Jxvvg^YPcw5v|2x3|JLN*8 zpg|GE`cekeX=mu$a^$Owb!Y%Oxt5%k{Ru>0UGlWgTp+#IkomJX{S+EoRQ0)B{?IQ* zhqKEtA=i{1n)LCmC{@aUI}nRJU#~aV!aI15TEL?H!jbH=2%{@g0>8Wz4mG#!N2N@+ zE;#70Y*3IFk{`nWu1Qm7;K&u9!J#CmU+r7TmZ&-uVc+b|cj8?L`H95TMy6dsn0;N>FcXsN8JO89yROWeR zNcO_- zqe{>kBG=!Bv5fn8u~JF$^bq+OW5rh_e?)yRZ;OSd+s#WOZB+k?HZ2^2L9#0Gamj@#be7D-vYd<<1w@xu zSN|r$I}fHCI9+V2C?vM!Y3Jd~wF7eTBHcnYVjZeub4&E;m!>A6Ix+(^M0pF_0TJz$ zVMCRenBC9Y7lq(?a3zphT!k<#7dD7`$7>|fwzqqin(>ha(-;S)ST~h>K`ld@fBs;+ zYiV58+!AA&%D42_L69$Qf=@>dtQCxsL zgmU>~NN+3i8$hn*CXidL?`~C$-0;I#p)x)(qkKziFarw5NF(JU18){04>`SK+#to6 za$=Or&Rs;5IvUrxj0yby=JVwWiihG?p`l>jXtBUX!+XH zTEyt_{tX;EH(-j=MQitzQd3eWfb;5P2hc^g&Z?yYZp0+wzmZGK>6u}Nm;;=)Z+tsu z=c@`W6i%ZN@Aw_|`H9mT0mnvcmZ1<$gg!TngJ$)ED2LXeFgf591uPu!q4vyKx_a-5 zp5?LM7l~f6FXFlnlEq{qS)~Nc+;9n_YA8wT z*$c(N6p&hZF&GMdtIN&4ZV82Y3r3>pvc)aK4Tj;)n-5XD8w}7Gs77tD#|H8)m0aJR zVi|6ImD>eBoi^N^HZ)p^e{_tfAU{ztOtAkr!8B5*Et$IN2bF3JSxm2VEk>!%9Q+pA zJf=`sza<>{6Csb`Zy{W%Pp-9^dZzTcm7|8|d;gt}?{9<%i{}jaI>Hg0#0uha5 z)P*?w_}XFCYX`tA7((4SYq`I$h8$+lz7`?cfTNtap(zDeRp6B1EnWHPA|g>!bi60q zRXU+cQtq_4aFy`i@nfSfyW{=U3gzyJ#KDA5hmt;vP0OP&HA8-_98Y=cTgU>}HE2gN z)M?=Ro2^L3G%_*f3C)AJV)>E}vEr>H0$6)Rp z+LKNDKE|#U;G_{ed$0Ds%eAbLK1J3yJE?TU19c-D&U}cl3r(bvEhEG%e?en6LmjvvM-V z%l?r0!+5t;{VGh~M!?UlOAs+Eku$*t+&Og?KmNKjow%RbsvV?Zw!U_mic^lPK`Va2 z>daSK(Y_s})l^j2Rac1EEm1Ve55TI@4(47mqv_@v(;p+}A}WUzJ&q7rqSq(taKX*a z^!ruMrf6`kgz zx@HRUwQb=c#x;5!vVqkUL4Cv**k^}wB&K(1z0*4B?Mr~K`cEpT4c=W}f3W8O?{TLGaPdJn%&xRILCbiR)cLZ0m zY`rbLP6Ge|H7z?ME{&~8t-*O&xfyTP;T*k{Lm$WxJzIwKz@Rckc>_hiKv3&=`fRg8 zO*if~>_I|I!xsH#$iNwqh6XGXT`Gb8B0Wi{6(fk%XT8#0CD z{|(2IY!cG%9tLfUVUH&AtGFj7dKmd`79Ds9M1+>eaWK~IxS)Zy0v{79#Daq))DEL{ z&2Hm|gHNP`YKD#uP!|Ef4So*Ib4JAC8?2(Q>**Pm%Y^D|9=hPUjPAYIBO$*Llc^DYwr3#bT zD)G#S=vTOrTGJ;L-U(wqhC&e&asVv5d*pK_h)losJqJ%$m?ph}Etoviu(U=v&I0|# z?fuq8zbp0OF1uF>*64Aw5%ov5qUak<6=J>Gy`bK!$Am7scD%|kmz}3T-d;S-#{m5|R z5##3y*i&o*>hdw$|ZRD17CuOni(Lmi(s2JNx!BhTQ?bPR-5e*k{I+8#8LkIZtt9_;qP9sj< zF|Tx*rRKcAM66<@^fxR}G^G>2qF_)v*M zyvnEk9#9yB?asIV7sn}k|JX??@hlhhhLjl=sP{^#;%Fv8A>&TY%^cJ6z!$Y-FOvPN zhkX_hb#o|cazLN?(0zSKrXJze|Aop z^nmdWLb!oeChc47j?_1^(b$1Z0l1Ng4f-_hNr3uL5ll?KAfpfb=f4%-yUPLfusZgw z?mSt8X7l{4UA=$`Y6%pyQx}#T%WAtK{pP)r$2tb97t&R-1cB9_7H5Tq-xaivI^vFo zPx^BB@_8rE95sCQk_f$B3v#kAc2>lxC*6)GTVIkqCj}yvt8qCLJVKZFLmNM&$o&Cf z$aLFpodgeK)o9(eH6h^`Agbrfwpb_Vm*DHSDj%W@1@c9%Lrr)Ry+exUn~2QLhgEei zGTyPzL5x-S?>5xgd!rG$S)sZ9VtTCBV563fwzy$&s}TZWI;4Hfmzd@+!~X(O2ed-^ zvtg|mG+6jetX290j^ptBX9wxn9*PM_9n0T2N{(J3@ra1iIYM9G6{(#EmqqK9jR4N? z%z(zx3T?QK2v|)F80h^Mn!^;=dk;{pFH&txY1bxJby<}CLgiSZmuAI$l2c!uCw~E= zWE-;hxCD{)PO{xe)u_OAC8m|LFC>vusI;cB+pIirIGE#8Kp(6q#@gM&Lsa#6!xHbJ z{~v2K^*BOa=r%tU16e$)UuU!#`~1rP)pYLROuzphcQ&WYq0AwSlp$G;lN?6UN?3AO z2usW-zcAU#`pce&6@&^?W`a zg3|aJIs>%VKjEINo88S^A2Z16PxZ$L%p60Uf}c{cKs+=X?vox!!(xKY3v~U2sB`)F z+MQGkNU4zfCO8}McY9rEO700)4#LHJp>`(jra2`~ru@Crj>WDto*c}0uqJ>0O2D`l zqiHQE8A{8=6_)W9n?r!m^l9}1d07U8VrMI7gL0W zC^TL=sA5rg^c@{{+j<_T;rUjtAs1x$LCIB4R(b_UP<)YiqDLO}bD*P`DM z3dYJZk$Sba2+2zTN5+Skt5UEHDG%;U3w1uwxR`(6Qeji?l!S}$hzgaiK9)}- za2=Csv&hZ%I<`>FV1EVoE@BQYtub3sJtV6T$Xw?hc}g2LI0mM zNMI02h8k@J8OaNjw3%$&ovFa=JVItL9r{Wa3Qh zd4wnXWTu1<-_Mz8YTj|nZotBVUR#ef$$WC@;J_Bnx95R_2aGF0<7=&fm<7{;}hK{=%@X_f>2FF(?W$u)78h#4}s)4^sa5= z^0hy|GK7r5ZDrz5YnNVq0P4boe2OR-WOLi_zk&Bg0A^n1Ao3f;$&K($Z< zWtIE-^)Wv_3Y4I{H&&J~2$2TPR^O>*j-8|IcGbdi>7qXo*N_46I~xIVrw0m9K-!k1 zk2)_=agjjDOUw8hWRyhUx&0ZNvw?U)Q%vHneMv*>-csjT^P4y>c8&_%uwky7+uQY( zLgdkO57UM-h?8>rKwka2o$Ab)ngDWMsekA*t~LnOWU`Or^lcD#^AcV3=s`tf=j%%v z`yUEruET-DyWr|xX7C4iZWE4jdVkuRbJtJXET9Aw9PXoxPEskd+<4|+wy;eNh&#YJ_MRG)DN+sRV90{nd~b>lveEa?Me5mT@B4nYCVdoZq#UEc@z;Pw z6Q`@{VBfZzjdpE4l+VKF3xX=&)HN(7LhBPI=xI^CjU0>$&QKj{+`2;$!&56 zYp~qv%ws3Nc%h@JJSB)h$$fjkj^Ib_?hw0Dn*QYkfx5aG!a#X1^k&j}8y`bBZdi$) zR?|86ukBZHF~}{XG6h6K=vX@rQ(bx|EGs?vMx3i-(94p(*r*c;`l&y-;hljn17gMn zW8jYq?9{W+zXm8Vt!6-Et=&>j)`sslx{5dyCpy66f?e?ixemqCCw;C;;|1u#f6m*R zXmqNoB#4@}kKnX1|A>HXyr2`{;3r~ik51^CCqcZhR&-UC(p+gx5&f3962*6nPyhJW ztF>Sa;=>q_lLaWfqqH1M%*x_SYSM-6>pSgF7h|<7lZlCQ!tkv>sUtF@Ut$~6p@p^Y zQQWByS--Mbw>1Cep}=Jok}xjZ{F6WM;OUhJD^>}eu-|i;%`z8EH@$#NS2im zLKttx_pd83B`CkvSpAI1J<-3t@$H^ywfC@{jcsamUwx(345%4R%>i0ju&*n#%I6LO_rZs?e7-7MA?{%_M=16w zPZADc5S7^W)YcM>yolv*_&jbAE-QI*C{NJ^dPooSG^mOY)aq&i(q*|$at7YSe)ots z-6|nHCXDUIxf8o3Q*(mQloYOa6%?+VWTdVkcdLCw#t)Cp%o4fX%W<8~Gsio=3Uoat z$S}ITUBuz-)OjAUoH5m6HjexsqUBDLt~SCqIQJ)C3!pyiey`_% zx#*WFBL6D9x_XY}v0pK@wVE?hlWwYNjVig-9T?o0T^d*?LquZU?Z$yqQ-VTmz9ysp zzP7(Zjb2_3S!j!9`noiCNN2pv+0na@4+j{~=!_wwV7lAo%gnG$q``6w#5e^E1|xwR zyL4dXQ>O7n;EoAAaTy)=XYt5ro+Db5sqQVjFBON_dTo3(-BeXL+n)FOp<;DT@&*Gh zqeoAuyN#dH={tTb@UxNgqi=)F?0b<6)a~y22f^VjAZUAsYv)+6sxBR^{I$sjiBU

%r0&;QX}>DkdWl`z?^+6)8Z=)t9AAswhVUPE0`VCGND=mUM_kmIau`(UBoFgVE&kvS6vr^ zD=wNFy?PbGXmHK$&#=wCu;H;*uvjmD_?yj|GRuX^yJUxuM)Tu^HE(sytfa?pr6;^n zOq!hk1EY;#k_@}d+`0wYo~>3yn05}Yl>tnR>hY=u{u9vXlhiO~7yul;8Rs19&f$K! zGt~PYcUwtR^D2Wku)$@8P>?Y4M5KU)x}STtGEWPw4xcqZqDvecu_QJPQxJ*(bebuI@XR3DS^{8&H%YB7m4Lr3_#5J@ z;h7PPUik{dz(lv4UeINN&MAmoXzwCk;;{Z+hE=>&M<3InL^MoP6{=IVpaVvB^io@& zFOP&oG&ne|lHbdq7wQ@O*;if=3jFca-xIm=p<|mToB1U_=DyZ5q;n{ULyIrmR3x!T z?(lT!w6RiVqMl0)i6oxw)A0h`LX+pWN={ z1=D1Pctbv(^3I&}!I?6`}{WgR>BCNpsO(oqae<2Txyc^9rThTY$phk(= z{M`V<-%PaieGxErAk0S4*)QyQ_s%Ds%#VT{<)Qn65HhP+%+zvm*bEuJm?)&)X@%%h z;aAv5j~%Z?&Fxn8Bk-?VKAdHI*4qeg0({Ha?%CiXRvEx{qYh?c5fPWaQu*IcM#tB= z1RYL`OJ_clV{m2cEbI%T38COwHRH|zT>_xnvXFNgHx z4X^D;>n%@bd@gHC$Z9IT^pG|Z*^jz3!YdLDF~baQHuTQ@S)J|9{}{+*;u>nzDZe`Q zc|=(ceHHKGlh`hTN%$UszU_N*2O9K|nSSD9T)Zg^TLC!tAU)V3igeR-)QlJk!#>Lw zFVUfpLq9KznjZH>sLm|_{vvpP$SWYcJLL?P{eaQMvLbXcVWZVx_5}e4n*V$U5|EZR zr`-b+I(V#TBymqjq573%4U3z1x9(-P4|JSda_0cqp7FB%7~cSYyg*6VTG2EXf%)9{ zfnK=WfR)Dn@2TngW3||+ zWk)&i;WO#u+FZ=&vH;~@+`WI970A5Mlhv(kxL3=*?V5I4X)jUVKBg% zQK`sl&p(t$!Wb^AdZBwFo!yKZzmaHz#I;)X;f1ef1(3%Xs$NJIfDmRpJ!rfI9!Y@O zu)F${blsSQyKo*!kAuamWYQ?4<>sBrfClPY+u9dG#;uV>H#+u}O4)J`wh}0?lAB%_ zup&x@CWq2Uxc5f=>{q2ERe;MUi#zG!3d3A?>yOic!sbR+PQlq+^mW%>)laxPxTa8) z_xn>vPx%eO0IdtWReM#BigL<%6K%(6M=d)e3qV^~ydT9%3dIP-KnEig#e?3A^Rf=r zUqpietdTWM@|U22$%5WM+w^a)KuNBOUvWiJfTNrQaz^XV@gzY81?nT^OC?_u;?&7n zdHpZuZhu#hY1TfpF5t z6MtmJN2uz7*Xc04T$$X$U#;+YvVr%AURyMR=i4nnj=Dfyk826ltm0(i@GR97Rf^#o z0irLpYT>KsbG%cPM|IZOlST($D!>i%7kn#By9EF_ItYprXiXh=`%-dTLZ)bwC(zGy^(a;XAlEZ3=7hs+Fir|O^`Vd$l()fi6eagH zEe-qX!`iD4mTcTv;hAoQc+v8s(4$qKlZVAQdL&fZYz}(1cHTbE^vMjOxg4*!V)8@U ztL}=HZqsHg^EZr&M0Tys2?l%gfE7q3qf63Hh4`BE@&3!pO>4}Z%-}$x0CFBHE_eDu z7Tl0kSf-MK0n#HRWw<6lllOTV_W4L5zjlEUBS%lXY0WC{0hrS&csxJ!c422Ps;NWW z8UZf1t?bk*Ml7tl80g`%A_Z2=4|S$PxZ?-Tt-d;G!H2G>|Itc?|$#@8czn`{Zux`U4w$_0OVK<3#4h3VJ+cUX2)Q~Sobcu=Nw;@BO!|d#N%tcoaSgHCTLSF=M2{)N{p}FNR7AQ#4Q`((Y~Mdp;Gs_( zg0$ovp&UHy`hXuLq&wP`ce< z(0{Ez+N{&mMFR9oz9qKPXJ?A@4xqId)6$8j*~v#p#e98h!rLgN!B5H#tU8A2^Nd&t z19O8BGz9$=`q;0571NL6CxAGgrolgLDB@5_r;{wcQ#j*MaLZK3a&oKSp2x^L)#s&Z z)Jo>_Y6n4qi$XA*^>`WSdj}S#VaOs^Uv+h$?3X`1qj0v$N-3S^fcbWq5?@l{Za^H?C`kCZ2Kbp+H|Dy3O{1^2_vggaXU!DoLax;p6Md z=fi4UHHJ6p+%QMc?>0Br?iv*-Acsa*#4(O6aOz#HWvs<0bU$j2sh}OVgeSH`6+D9` zFZ2LKD$~n#;?)PrFg9az25@~Nz9LnAkFwOLj?1bgKKh*N0xEtLqj4iRv1*agA055? z|80dC$W?tvvW|k71r3Hh%zZ|(t{<+Yj?8QQ2tETe**6YKJ+3~`D*IJS@z0O(eZ@N- zEq_;Lc)w48M$|9YZq$?PrPq>J%pI&{Xk8!PI(hkkzw3+BL0qAXw&)*BD=NaW@ia}m zad<@UkH;MmM=68_z?mgbv|)Eu`;dY5N((m!OV1BkSalf+%RjKCOou0)l!o(&^eu(= zqB3U<`p;6=L4Xk^$`Ynj7=1Wkd&bnEZIsy2sQFw^=K8+IF|93-uECa(>@lvIQZ+3w z3K0nv`kIz3EViF-uesPXA?RKxxXXacAvp1T&UIyO#9kvt>h;k^JYahuAoZ3MW!MBd zeIBDH<4S(Z1hp~8!Q0C-N_?96Il80NY6%f^QrSXGMaRC#_WiM1IT z>6GYz^{beT5QQg+v6HX)hc=UK%B5IfJyV*CSw#C{-s^xq&zI#*66nQRoY1rwssRK# zx)b7=Lt2nfMu6OZ&C9`L;1)e@K6cxvfP8kmmNG&<%M2^bbXOTNnj;ydWVyJVG-U?GMsUua%4Um z{8Aiy;a>_cJ&^@0uSy#FrJ=RN0q-CPbL{3)D*^E-ka-_lM&JUaRK7sLsHymD8z+1T zHoc68tz9D&@k?>FxUW4`FzrAyWCf=IU9IhKIVd^*Yt+pBdj?^Mvy>FRn}x-xgQGR+ zV|k2xJmRTZ@dI)6^Ho31v zSuy^`fBm$8h%pI!FLFLfyNBeE6uNAFRF{5*ntI}){i|WYri1<`w8EzMqR%V7g(Wm` zzIjR;qi-}c54RiK$Dlh0bVM?aOL@K-M^H^-x39>`;{9dDM-Q`dLwyVG3B`>nr0r^D z-u)=ZyL;V8O=YvAm~;thT%6EhNS8V{M zqIlB|lV@Mt>F#bIpMCRgt1|`x=9zIV{7|4|8F}mw!Vp6iC=Bm&7tS}|Ym!yvq7EYU`l0(SDtR#cZ2&?lbh;3xUJ2|VoXnS+i*x{cWAX5J!MBx?1%JhuA-S?#N+m*bUkt~ zT0rUxOr2sX{{-q;bcE$1JihN>kC#EBEuE+fTvXg|qkK~1H_}ankUE*xNCYA{+tX5w zQYwj>W!HoGZcMHd);ya_dI*2y_T|52Rdn6#dG2Vfn5bf(*&)sBn1htzz&C}g2X+Hk zX;okWpXScDdK*x(@`RLwS$TX00Y-%+jPJQSSiuz=gi5J9iOS9}$-{ocwFOlz;p9vZ z`?I`=cR~~26z67SiY4@C=Bum3!!k=4$#;_YMRYh{*rFjui$%Zq$ZkAC_@Af7Gd@Yc zujmU;MpAVGDJ+(Q@9&LuHmiSXCL<;?a)x{dljG~=m%93zgu!s`+z)d$iQfcI2oi25 zj!8Qj_@I*QhtM(%V~?$${CR@%NlSRQlHYI0=|Tvh;Xv%)D%twV=tvaJ^;l|WUI}ed zVIPCKWq;?L7d1fUc7h82SMTC4a0>=ebHJPg{j4z`^v%J;C8;^E$Eh?CO@o2C)niDA zuC+O$BSb;{G6MeDcG2T!gTltm#1ygeOA`P?bl8HpJzWRwZFF}^3O=!AqB!>urO)o{6oyXfWkTEeTPmNAsLg3m3g~z2LmH+$#&_m= z!2%`*UJ4k%2ONiL){HJ6lk!{Au_0DB&gD`&0u$u#VUu~KX~dwUn4 zhZu+%*9_!i@2K$iBym$)(U#r5yHmzGx!9gE0!YOy{sJiBSFhNHcz?zsh?d%oGs{;n>Met7(ytesfB?xO!BBJqTLh|-JfJB3XqKSFy_a)wrUlJs8Gw`QT) z3HqQgVpNA2-+UzWU8qx^A{mq~ua{XEOT6Jm(Cyczka7ydBk+V+i&pMwF{}^8fQ2}m zl6w{*=FX{H=z>|1Hz^ny+u=ygqwB2*rq=W3#Kb}pS_i;xC0cMY@e%|+R(lKaRj@64 z&FB8wZn*jW9Bdl&UgY9VoU>Hpb=x`oty^gVw-n#?snO`Lm;V6DD&C_XPL^xHd2+d` z1Y0*FjpS(`wF2$2c`n#R9RZ*jV!}&jd1!3zw!$3Z|x(&l)Gn$vD+@_|Xzo7HiqMbTpplIOpojmU+vJzdy z!o6RQTG}~SKSMe`Texg^-AH&NXXLS@J6|PDv-fzv1dI#TRSm#NN7uM`Wm)8pnOVQ{ zI|0QEv)H^`{qQ->_lZZ?2j28W_|wx36O{o##}1qA_mf|Pt(1CC$ahzKOY*r@>xcXU z^ZWCCU{W{RN#fTX*b&=~P)3JV3be4y>`ef*EAGI6@x32D$&H&GS}rdgzl%Eg~%tC$T@ZXjS_3=Biqgi#wXyN@iPlOiGQQGdU zCe!?~X`FxmD!3F@>19Q|?7@Yso7?Ag@tY~WyJ~ZB-uY|it#_<7LV@=;kW%oU419BG zVt8Z}$mebx-a`V0pd+-%a5K|79&l|4h?ZAE}7xbDuE$(ESkTLAj}4u&Ta zDE;Ny3$s--&>ba2Gq^@0=EGbc^&f#%TDt<%73Q??3wA+pR=vnVS#GYjo4&4!yzSf3Pq>3H!I0Owx706S!JQ%f*i+UxJyQlZ+FGfEsBqC#gZ z%gbzjRjpR8|>jJ!aw=TX z5*n#ih>qj=&~_Lvd@II~mAjn1GZnWbJ0Fa&T;`q-(N4JeB>F63M#&C?wCyPPR$u9) zGTD14GCflAzYMAx+@KZybie0sNiA+J9WM>~R)i6Jk7I;$_|gg-D8h0{f=V3BUfr^Y z?>5BO{!5i#khZdCeyq;w}pIgQ#IDf?`$cN=by%zfG5C z$(JrA%C*<2s$ABL7X-ck?C#jRX78kl$SOP0z>fx5;M14+WD`g_%@aK3y8Ft#i%OJV z@9(WE207sMkUEXtuVsDhg7`Vvu|w(Ra<2gr2R;FN(f02hm6A?>RL-& zkel$yoJ+dz;?M#v4Us>RZ!pXh24LKLIL3qHILEM_TgL@A<>*^ReMprWi7Jb>qrE8e zt&=Pl3|(pXcMNVG7%LgP?O8NFsLaA(?<Fr_X{UYCXtQBfKj}$gATQ$9|tL z-%H86jE00?DUVEB-z+1^=^+<;=dhplFGRv-1mQ$o>j_1Od;3O&5m^?04hzC$VCvo& zvCP?P<{=^__*my|=$7+{S0<7!*&Xa(81Bl>`s{~@}|J5?WoNo`B^>`H`DLtN!PR~?%Iz^4?Se)reKsYGi}biLHY>(kb?&gPe%L`8o(yRFj^JHe7Px#NHNnY zfV&s9LCoHo`M?ZIMFDq?annG>ztGgQWB=c|M}HTufA`2sEAYR2!WmTDpFfK-`q|;9 z6{Kt>ognQ^hHkTuwHW9Z^__;C(Mfbd(%jX1hZYaJwpGBy?WmkN-mkIEw-c>Bkj zUz%3E4!)q^BNfr)miTl=%JM+$swy>Y8p0IkMDJ=|K?ynGf8b=p#gH;_E3X0hhs$JEIe;XIg2B*@kK z4C8xCoz0(PS3;{i6~a?RTDI#eRz*Exh|rV_kmDJM!NXpE56G;ADr{{M8na z$l+i7S?}~6UkheiEjvg>)G8(<@dA5{BS@NVhE4cT$7)ql-BJ6Oq7b}ym7pFMFj{-6 z4fb-AZ$mYCUXr*z3z+b#F-hX*Nh7|%#!SPjk`u(qSbAMoDAY0uG z)oo6eT;-pU4=gOkgM5^YW$NCeb6nD*4;bG;CzzRT^Q}5SI9=PHR22K)CO^$Xc{8k4 zv8!KJc(EBB25NP6gO6hc;ai`Sr>#^5=VvaC#O z;x*m{NyUfIkdVOZ$_a)pf>mNcQFoMRBP-{(M zBY(wbxe27-QbMT9+xWqB+l1y}hwy?jQ2M|GURPzh^-EE%tQUnkFs*?`YQ$mx2%zJk zfW44O+qw~)9Ml+!Xyd0FT#dz^Y2&>KPL_6b)e^u|46A)6!*k^Wiu32!^NSM(e5wZ9 zZ^%PN)*y4P5oahIgob4A9Bj^A(5j|S>R1!aAgF$Ai%TBreVeqX81HE=go=}6IXQWA zv|2u0Obx$gB!HN%OSN{P>U8JodLs@~KGj#cGC5y6%|E~T1NltJ*2^BCID-&pJ6$lB1Zto?$9D5=Rt!0_<*DjX)$85FX2KBpSmXCKe(G zx>bK}(4AJ`WuuSIujxDq(;9ICCTl;)Dj@XE19bgY@@$fM{~Yu0E^nqCGb}|CamWWe ztu6wBVPa^_;JmIdOx=&_F;T6dJ!x1kjX341pObp+I~^wBSxlUF0S}@TnY3VNgM0`P zdHI561{RYfM3+|5sAW}U`PLvPA9{%w zl4eu2sOx{ttwW~nn$t3TWrZ52Llm!L(V578){xBYHvTa)lEaFN2q$fCLojPiWOWUn zQcAM$O$3Q|o=eWe>Q6f`CA@H>^gt_Zur#N?-zG=OTo^WNsQ*>d*{>e}pnk;|&$ud! z9wesX9pv#w5%ycw!ixU|Iz8H$)4OsZziu)1*8XuTt5HPx@}sFxjP>}}%Vrl-{_{}o zIJ3AbSiij#u3ood&1v__^_>h0dJ(aAuZ_TZO zXCbxd*>;gT^6^Z0gsjl$j*;jjl?fH`TT-g)DBRd~+LnSoI zf(~E6Aj<$;w$o~#hLoZcyWR@%P}a_<tufLtxk4|yznbjOk_7kJ*BYpEEy_6#_w!TADEn-%qiI}+Fq|JB0xRW5~Bq#De>Rt;TM zPcg{iRy+IU7>p0t6Dg%woL{QHoghMmI}LH-)42+2c)Et;#f!a-L5#eQWNo7>7*&o# zF9`BowqC$BDC{~{c`=Do3@4>8M z3}C8g*7$H9T=<8Jut`jnecm_CFt->FLOD%rN{LzXE;m-(wI&NznC`mzZF0x4X1^a9W)q;5OHcb8L>=&A}FrcRPbj z{WJd+&SQb`F0#mjZ@O;QdTQjqux`Q^~fn14BbyiIoiZbDWDzj`qkW~ybn_8xYk=MaxH{zLr>t=`r%ZucmaY~T*V|M_oD zh=Y06PyG0H3>u;|s$(5C@nP*(gwYFKXe|2YUG!s;H&Fsxqc<}4mwU@|H=_zR`;r6v NoH=#gq7v<%^glazjUoU5 literal 0 HcmV?d00001 From 2b3fc01cf138eef9ee499907db2404b7e3944759 Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Mon, 3 Aug 2026 13:33:26 +0800 Subject: [PATCH 58/59] chore: release v0.5.0 --- CHANGELOG.md | 14 ++++++++++++++ README.md | 14 ++++++++++++++ pyproject.toml | 2 +- 3 files changed, 29 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index e72a1b9e..cabeab9e 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,5 +1,19 @@ # Changelog +## [0.5.0] - 2026-08-03 + +- 新增独立的 host high-level-policy sim2real 运行时：使用严格的 msgpack/ZeroMQ 协议、异步 receding-horizon replanning、时间戳对齐调度，以及 50 Hz 输出安全校验和限速。 +- 扩展 G1 外设支持：加入 OpenNeck 0.2.0 物理角度控制、Pico HMD 主动视觉映射、LinkerHand O6 somehand 0.3.0 手势控制，以及手部和颈部状态回读。 +- 更新 sim2real 录制与审阅流程：采用 `schema.json`、`episodes.jsonl`、逐 episode HDF5 和压缩 MP4 布局，记录可选手部/颈部状态与动作，并新增同步 recording viewer。 +- 新增匹配的 G1 模型/策略组合：默认 `g1_29dof.xml` 配合 `ckpt/track_g1.{pt,onnx}`，neck-and-O6 版本配合 `g1_29dof_neck_o6.xml` 和 `ckpt/track_g1_neck_o6.{pt,onnx}`。 +- 更新 OmniXtreme-style benchmark，并增强 Pico/RealSense 故障恢复、GMR mocap-entry cold start、high-level-policy watchdog 和引用安全处理。 + +### 迁移说明 + +- v0.4 的根目录 `track.{pt,onnx}` 路径已替换为 `ckpt/track_g1.{pt,onnx}`；neck-and-O6 运行时必须使用对应的模型和策略组合。 +- 旧的 attribute-based sim2real HDF5 格式不再支持；录制、转换和审阅工具使用当前 manifest-based source layout。 +- Host-policy 网络协议不提供旧 envelope 兼容，OpenNeck 旧 normalized API 也不再支持；Teleopit 与 companion runtime 必须使用匹配版本。 + ## [0.4.0] - 2026-06-25 - 改进 Pico 实时控制：支持 pico-bridge 0.2.1、`ARMS` 模式，以及保留 retargeter warm-start 的模式切换/暂停恢复。 diff --git a/README.md b/README.md index 007eca1d..1be6ad7f 100644 --- a/README.md +++ b/README.md @@ -70,6 +70,20 @@ Full docs at **[BotRunner64.github.io/Teleopit](https://BotRunner64.github.io/Te ## Changelog +### v0.5.0 (2026-08-03) + +- Added an independent host high-level-policy sim2real runtime with a strict msgpack/ZeroMQ protocol, asynchronous receding-horizon replanning, timestamp-aligned scheduling, and validated, rate-limited 50 Hz output. +- Extended G1 peripheral support with OpenNeck 0.2.0 physical-angle control, Pico HMD active-vision mapping, LinkerHand O6 hand-pose control through somehand 0.3.0, and hand/neck state readback. +- Updated sim2real recording and review around `schema.json`, `episodes.jsonl`, per-episode HDF5 files, compressed MP4 video, optional hand/neck state and action fields, and a synchronized recording viewer. +- Added matched G1 model/policy pairs: `g1_29dof.xml` with `ckpt/track_g1.{pt,onnx}`, and `g1_29dof_neck_o6.xml` with `ckpt/track_g1_neck_o6.{pt,onnx}`. +- Updated the OmniXtreme-style benchmark and hardened Pico/RealSense recovery, GMR mocap-entry cold start, the high-level-policy watchdog, and reference safety handling. + +#### Migration notes + +- The v0.4 root-level `track.{pt,onnx}` paths are replaced by `ckpt/track_g1.{pt,onnx}`; the neck-and-O6 runtime requires its matching robot model and policy. +- The old attribute-based sim2real HDF5 format is unsupported; recording, conversion, and review use the current manifest-based source layout. +- The host-policy protocol has no legacy envelope compatibility, and the old normalized OpenNeck API is unsupported; Teleopit and its companion runtimes must use matching versions. + ### v0.4.0 (2026-06-25) - Improved Pico realtime control with pico-bridge 0.2.1, `ARMS` mode, armed sim2real mocap entry, and retargeter-preserving pause/arms resets. diff --git a/pyproject.toml b/pyproject.toml index baee02e1..db70cc8a 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta" [project] name = "teleopit" -version = "0.4.0" +version = "0.5.0" description = "Teleoperation framework for humanoid robots with motion retargeting" authors = [ {name = "Teleopit Team"} From 559b60a348ad1b40cca02653e22e731b83e7a2c4 Mon Sep 17 00:00:00 2001 From: Wu Bingqian Date: Mon, 3 Aug 2026 14:07:37 +0800 Subject: [PATCH 59/59] docs: refresh README navigation links --- README.md | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index 1be6ad7f..08f9350f 100644 --- a/README.md +++ b/README.md @@ -11,11 +11,9 @@

+ Project Homepage • Documentation • - 中文文档 • - Pico Sim2Sim • - Pico Sim2Real • - Training + 中文文档

---