[real] 整理 Python Sim2Real v2
This commit is contained in:
@@ -0,0 +1,244 @@
|
||||
"""Offline deployment alignment check for the current 53-D rough policy."""
|
||||
|
||||
import argparse
|
||||
from pathlib import Path
|
||||
import sys
|
||||
|
||||
import numpy as np
|
||||
import torch
|
||||
import yaml
|
||||
|
||||
sys.path.insert(0, str(Path(__file__).resolve().parents[1]))
|
||||
|
||||
from interface.motor_mapping import MotorMapping # noqa: E402
|
||||
from policy.policy_runner import PolicyRunner # noqa: E402
|
||||
|
||||
|
||||
def _load_manifest(manifest_path: Path) -> dict:
|
||||
with open(manifest_path, "r", encoding="utf-8") as f:
|
||||
return yaml.safe_load(f)
|
||||
|
||||
|
||||
def _load_config() -> dict:
|
||||
config_path = Path(__file__).resolve().parents[1] / "config.yaml"
|
||||
with open(config_path, "r", encoding="utf-8") as f:
|
||||
return yaml.safe_load(f)
|
||||
|
||||
|
||||
def check(policy_path: Path, manifest_path: Path | None = None) -> int:
|
||||
issues: list[tuple[str, str]] = []
|
||||
manifest = _load_manifest(manifest_path) if manifest_path is not None else None
|
||||
config = _load_config()
|
||||
|
||||
expected = (
|
||||
("fl", "hip_abduction"), ("fl", "hip_pitch"), ("fl", "knee"),
|
||||
("fr", "hip_abduction"), ("fr", "hip_pitch"), ("fr", "knee"),
|
||||
("rl", "hip_abduction"), ("rl", "hip_pitch"), ("rl", "knee"),
|
||||
("rr", "hip_abduction"), ("rr", "hip_pitch"), ("rr", "knee"),
|
||||
("fl", "wheel"), ("fr", "wheel"), ("rl", "wheel"), ("rr", "wheel"),
|
||||
)
|
||||
if MotorMapping.SIM_JOINT_ORDER != expected:
|
||||
issues.append(("joint_order", "MotorMapping.SIM_JOINT_ORDER mismatch"))
|
||||
else:
|
||||
print("[Check] joint order: PASS")
|
||||
expected_joint_names = [f"{leg}_{joint}" for leg, joint in expected]
|
||||
|
||||
manifest_enable_zero_cmd = True
|
||||
if manifest is not None:
|
||||
manifest_enable_zero_cmd = bool(
|
||||
manifest.get("model", {}).get("enable_zero_cmd_suppression", True)
|
||||
)
|
||||
|
||||
runner = PolicyRunner(
|
||||
policy_path,
|
||||
device=torch.device("cpu"),
|
||||
enable_zero_cmd_suppression=manifest_enable_zero_cmd,
|
||||
clip_obs=float(config.get("policy", {}).get("clip_obs", 100.0)),
|
||||
)
|
||||
obs_mean = np.asarray(runner.policy.obs_mean.detach().cpu().numpy(), dtype=np.float32)
|
||||
obs_std = np.asarray(runner.policy.obs_std.detach().cpu().numpy(), dtype=np.float32)
|
||||
if np.allclose(obs_mean, 0.0) and np.allclose(obs_std, 1.0):
|
||||
print("[Check] obs normalizer: PASS (identity)")
|
||||
else:
|
||||
print(
|
||||
f"[Check] obs normalizer: PASS "
|
||||
f"(mean range=[{obs_mean.min():.3f},{obs_mean.max():.3f}], "
|
||||
f"std range=[{obs_std.min():.3f},{obs_std.max():.3f}])"
|
||||
)
|
||||
if (obs_std < 1e-6).any():
|
||||
issues.append(
|
||||
(
|
||||
"normalizer_zero_std",
|
||||
f"obs_std has near-zero entries: {np.where(obs_std < 1e-6)[0].tolist()}",
|
||||
)
|
||||
)
|
||||
|
||||
raw_zero = np.zeros(runner.BASE_OBS_DIM, dtype=np.float32)
|
||||
runner.reset(prime_obs=raw_zero)
|
||||
_, raw = runner.step(raw_zero)
|
||||
if np.max(np.abs(raw)) > 5.0:
|
||||
issues.append(
|
||||
(
|
||||
"output_range",
|
||||
f"raw action too large under zero obs: {np.max(np.abs(raw)):.3f}",
|
||||
)
|
||||
)
|
||||
else:
|
||||
print(f"[Check] zero-obs output range: PASS (max|raw|={np.max(np.abs(raw)):.3f})")
|
||||
|
||||
expected_default = np.array([0.0, 0.9, -1.8] * 4 + [0.0] * 4, dtype=np.float32)
|
||||
if not np.allclose(runner.default_dof_pos, expected_default):
|
||||
issues.append(("default_pose_mismatch", f"default_dof_pos mismatch: {runner.default_dof_pos}"))
|
||||
else:
|
||||
print("[Check] default_dof_pos: PASS")
|
||||
|
||||
if runner.BASE_OBS_DIM != 53:
|
||||
issues.append(("obs_dim", f"base obs dim {runner.BASE_OBS_DIM} != 53"))
|
||||
else:
|
||||
print("[Check] actor obs dim: PASS (53)")
|
||||
|
||||
if manifest is not None:
|
||||
declared_model = manifest.get("model", {})
|
||||
declared_action = manifest.get("action", {})
|
||||
declared_safety = manifest.get("safety", {})
|
||||
declared_control = manifest.get("control", {})
|
||||
|
||||
if int(declared_model.get("obs_dim", -1)) != runner.policy.expected_obs_dim:
|
||||
issues.append(
|
||||
(
|
||||
"manifest_obs_dim",
|
||||
f"manifest obs_dim {declared_model.get('obs_dim')} != policy {runner.policy.expected_obs_dim}",
|
||||
)
|
||||
)
|
||||
else:
|
||||
print("[Check] manifest obs_dim: PASS")
|
||||
|
||||
if int(declared_model.get("action_dim", -1)) != runner.policy.expected_action_dim:
|
||||
issues.append(
|
||||
(
|
||||
"manifest_action_dim",
|
||||
f"manifest action_dim {declared_model.get('action_dim')} != policy {runner.policy.expected_action_dim}",
|
||||
)
|
||||
)
|
||||
else:
|
||||
print("[Check] manifest action_dim: PASS")
|
||||
|
||||
declared_clip_obs = float(declared_model.get("clip_obs", -1.0))
|
||||
config_clip_obs = float(config.get("policy", {}).get("clip_obs", -2.0))
|
||||
if declared_clip_obs != runner.clip_obs or config_clip_obs != runner.clip_obs:
|
||||
issues.append(
|
||||
(
|
||||
"clip_obs",
|
||||
f"clip_obs mismatch: manifest={declared_clip_obs}, config={config_clip_obs}, runner={runner.clip_obs}",
|
||||
)
|
||||
)
|
||||
else:
|
||||
print("[Check] clip_obs: PASS")
|
||||
|
||||
declared_default = np.asarray(declared_action.get("default_dof_pos", []), dtype=np.float32)
|
||||
if declared_default.shape != runner.default_dof_pos.shape or not np.allclose(
|
||||
declared_default, runner.default_dof_pos
|
||||
):
|
||||
issues.append(("manifest_default_pose", "manifest default_dof_pos mismatch"))
|
||||
else:
|
||||
print("[Check] manifest default_dof_pos: PASS")
|
||||
|
||||
declared_scale = np.asarray(declared_action.get("scale", []), dtype=np.float32)
|
||||
if declared_scale.shape != runner.action_scale.shape or not np.allclose(
|
||||
declared_scale, runner.action_scale
|
||||
):
|
||||
issues.append(("manifest_action_scale", "manifest action scale mismatch"))
|
||||
else:
|
||||
print("[Check] manifest action scale: PASS")
|
||||
|
||||
config_scale = np.asarray(config.get("policy", {}).get("action_scale", []), dtype=np.float32)
|
||||
if config_scale.shape != runner.action_scale.shape or not np.allclose(config_scale, runner.action_scale):
|
||||
issues.append(("config_action_scale", "config policy.action_scale mismatch"))
|
||||
else:
|
||||
print("[Check] config action scale: PASS")
|
||||
|
||||
declared_joint_order = list(declared_action.get("joint_order", []))
|
||||
if declared_joint_order != expected_joint_names:
|
||||
issues.append(("manifest_joint_order", "manifest action.joint_order mismatch"))
|
||||
else:
|
||||
print("[Check] manifest joint order: PASS")
|
||||
|
||||
declared_wheel_indices = list(declared_action.get("wheel_indices", []))
|
||||
if declared_wheel_indices != [12, 13, 14, 15]:
|
||||
issues.append(("manifest_wheel_indices", "manifest wheel_indices must be [12,13,14,15]"))
|
||||
else:
|
||||
print("[Check] manifest wheel indices: PASS")
|
||||
|
||||
if float(declared_safety.get("zero_cmd_lin_thresh", -1.0)) != runner.zero_cmd_lin_thresh:
|
||||
issues.append(("manifest_zero_cmd_lin_thresh", "manifest zero_cmd_lin_thresh mismatch"))
|
||||
if float(declared_safety.get("zero_cmd_yaw_thresh", -1.0)) != runner.zero_cmd_yaw_thresh:
|
||||
issues.append(("manifest_zero_cmd_yaw_thresh", "manifest zero_cmd_yaw_thresh mismatch"))
|
||||
if float(declared_safety.get("zero_yaw_rate_thresh", -1.0)) != runner.zero_yaw_rate_thresh:
|
||||
issues.append(("manifest_zero_yaw_rate_thresh", "manifest zero_yaw_rate_thresh mismatch"))
|
||||
if bool(declared_model.get("enable_zero_cmd_suppression", True)) != runner.enable_zero_cmd_suppression:
|
||||
issues.append(("manifest_zero_cmd_switch", "manifest zero-command suppression switch mismatch"))
|
||||
else:
|
||||
print("[Check] manifest zero-command suppression: PASS")
|
||||
|
||||
if int(declared_control.get("control_freq_hz", -1)) != 50:
|
||||
issues.append(("manifest_control_freq", "manifest control_freq_hz must be 50"))
|
||||
else:
|
||||
print("[Check] manifest control freq: PASS")
|
||||
|
||||
if int(config.get("control_freq", -1)) != int(declared_control.get("control_freq_hz", -2)):
|
||||
issues.append(("config_control_freq", "config control_freq != manifest control_freq_hz"))
|
||||
else:
|
||||
print("[Check] config control freq: PASS")
|
||||
|
||||
controller_cfg = config.get("controller", {}) or {}
|
||||
gain_pairs = (
|
||||
("runtime_kp_leg", "kp_leg"),
|
||||
("runtime_kd_leg", "kd_leg"),
|
||||
("hold_kp_leg", "hold_kp_leg"),
|
||||
("hold_kd_leg", "hold_kd_leg"),
|
||||
("kd_wheel", "kd_wheel"),
|
||||
)
|
||||
for manifest_key, config_key in gain_pairs:
|
||||
manifest_value = float(declared_control.get(manifest_key, -9999.0))
|
||||
config_value = float(controller_cfg.get(config_key, -9998.0))
|
||||
if not np.isclose(manifest_value, config_value):
|
||||
issues.append(
|
||||
(
|
||||
"control_gains",
|
||||
f"{manifest_key}/{config_key} mismatch: manifest={manifest_value}, config={config_value}",
|
||||
)
|
||||
)
|
||||
if not any(tag == "control_gains" for tag, _ in issues):
|
||||
print("[Check] control gains: PASS")
|
||||
|
||||
manifest_filter = declared_control.get("command_filter", {}) or {}
|
||||
config_filter = config.get("command_filter", {}) or {}
|
||||
for key in ("enabled", "max_vx_acc", "max_vy_acc", "max_yaw_acc"):
|
||||
if manifest_filter.get(key) != config_filter.get(key):
|
||||
issues.append(("command_filter", f"command_filter.{key} mismatch"))
|
||||
if not any(tag == "command_filter" for tag, _ in issues):
|
||||
print("[Check] command filter config: PASS")
|
||||
|
||||
if issues:
|
||||
print("\n" + "=" * 60)
|
||||
print(f"Alignment check failed: {len(issues)} issue(s)")
|
||||
for tag, msg in issues:
|
||||
print(f" [{tag}] {msg}")
|
||||
return 1
|
||||
|
||||
print("\n" + "=" * 60)
|
||||
print("All offline alignment checks passed.")
|
||||
return 0
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("--policy", type=str, required=True, help="Path to policy .onnx or .pt")
|
||||
parser.add_argument("--manifest", type=str, default=None, help="Optional deployment manifest yaml")
|
||||
args = parser.parse_args()
|
||||
manifest = Path(args.manifest) if args.manifest else None
|
||||
sys.exit(check(Path(args.policy), manifest))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,138 @@
|
||||
"""Summarize real-run logs for startup/stand/runtime diagnosis."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import csv
|
||||
import json
|
||||
import math
|
||||
from pathlib import Path
|
||||
import statistics
|
||||
|
||||
LEG_JOINTS = (
|
||||
"fl_hip_abd", "fl_hip_pitch", "fl_knee",
|
||||
"fr_hip_abd", "fr_hip_pitch", "fr_knee",
|
||||
"rl_hip_abd", "rl_hip_pitch", "rl_knee",
|
||||
"rr_hip_abd", "rr_hip_pitch", "rr_knee",
|
||||
)
|
||||
|
||||
|
||||
def _f(row: dict[str, str], key: str, default: float = 0.0) -> float:
|
||||
try:
|
||||
return float(row.get(key, default))
|
||||
except Exception:
|
||||
return default
|
||||
|
||||
|
||||
def _stats(values: list[float]) -> str:
|
||||
if not values:
|
||||
return "--"
|
||||
return (
|
||||
f"mean={statistics.mean(values):.3f} "
|
||||
f"std={statistics.pstdev(values):.3f} "
|
||||
f"min={min(values):.3f} max={max(values):.3f}"
|
||||
)
|
||||
|
||||
|
||||
def _pitch_deg(row: dict[str, str]) -> float:
|
||||
gx = _f(row, "pgrav_x")
|
||||
gy = _f(row, "pgrav_y")
|
||||
gz = _f(row, "pgrav_z")
|
||||
return math.degrees(math.atan2(gx, math.sqrt(max(1e-9, gy * gy + gz * gz))))
|
||||
|
||||
|
||||
def summarize(log_dir: Path) -> int:
|
||||
state_path = log_dir / "state.csv"
|
||||
events_path = log_dir / "events.jsonl"
|
||||
if not state_path.exists():
|
||||
print(f"[Analyze] missing {state_path}")
|
||||
return 1
|
||||
|
||||
if events_path.exists():
|
||||
print("[Analyze] key events:")
|
||||
for line in events_path.read_text(encoding="utf-8", errors="replace").splitlines():
|
||||
try:
|
||||
ev = json.loads(line)
|
||||
except Exception:
|
||||
continue
|
||||
if ev.get("kind") in {
|
||||
"STARTUP_PLAN",
|
||||
"STARTUP_REACHED",
|
||||
"STAND_BALANCE_STABLE",
|
||||
"RUNTIME_BEGIN",
|
||||
"POLICY_TARGET_STALE",
|
||||
"POLICY_TIMEOUT",
|
||||
"SAFETY_BRAKE",
|
||||
"GUARD_STOP",
|
||||
"POSE_INIT_FAILED",
|
||||
}:
|
||||
detail = {k: v for k, v in ev.items() if k not in ("t", "t_rel")}
|
||||
print(f" t={ev.get('t_rel', 0):.2f}s {detail}")
|
||||
|
||||
with state_path.open(newline="", encoding="utf-8") as f:
|
||||
rows = list(csv.DictReader(f))
|
||||
print(f"\n[Analyze] state rows: {len(rows)}")
|
||||
global_notes: list[str] = []
|
||||
for phase in sorted({r.get("phase", "") for r in rows}):
|
||||
phase_rows = [r for r in rows if r.get("phase") == phase]
|
||||
if not phase_rows:
|
||||
continue
|
||||
pitch = [_pitch_deg(r) for r in phase_rows]
|
||||
loop = [_f(r, "loop_dt_ms") for r in phase_rows]
|
||||
imu = [_f(r, "imu_age_ms") for r in phase_rows]
|
||||
print(f"\n[Phase] {phase} n={len(phase_rows)} t={phase_rows[0].get('t_rel')}..{phase_rows[-1].get('t_rel')}")
|
||||
print(f" pitch_deg {_stats(pitch)}")
|
||||
print(f" loop_ms {_stats(loop)}")
|
||||
print(f" imu_age {_stats(imu)}")
|
||||
if "stand_pitch_corr" in phase_rows[0]:
|
||||
stand_pitch = [_f(r, "stand_pitch_deg") for r in phase_rows]
|
||||
stand_corr = [_f(r, "stand_pitch_corr") for r in phase_rows]
|
||||
stand_enabled = [_f(r, "stand_pitch_comp_enabled") for r in phase_rows]
|
||||
print(f" stand_pitch_deg {_stats(stand_pitch)}")
|
||||
print(f" stand_pitch_corr {_stats(stand_corr)} enabled_mean={statistics.mean(stand_enabled):.3f}")
|
||||
for joint in ("fl_hip_pitch", "fr_hip_pitch", "rl_hip_pitch", "rr_hip_pitch", "fl_knee", "fr_knee", "rl_knee", "rr_knee"):
|
||||
pos_key = f"{joint}_pos"
|
||||
tgt_key = f"{joint}_tgt"
|
||||
tau_key = f"{joint}_tau"
|
||||
if pos_key in phase_rows[0] and tgt_key in phase_rows[0]:
|
||||
err = [_f(r, pos_key) - _f(r, tgt_key) for r in phase_rows]
|
||||
tau = [_f(r, tau_key) for r in phase_rows] if tau_key in phase_rows[0] else []
|
||||
print(f" {joint}_err {_stats(err)} tau {_stats(tau)}")
|
||||
high_tau = []
|
||||
high_err = []
|
||||
for joint in LEG_JOINTS:
|
||||
tau_key = f"{joint}_tau"
|
||||
pos_key = f"{joint}_pos"
|
||||
tgt_key = f"{joint}_tgt"
|
||||
if tau_key in phase_rows[0]:
|
||||
tau_abs_mean = statistics.mean(abs(_f(r, tau_key)) for r in phase_rows)
|
||||
if tau_abs_mean > 6.0:
|
||||
high_tau.append((joint, tau_abs_mean))
|
||||
if pos_key in phase_rows[0] and tgt_key in phase_rows[0]:
|
||||
err_abs_mean = statistics.mean(abs(_f(r, pos_key) - _f(r, tgt_key)) for r in phase_rows)
|
||||
if err_abs_mean > 0.08:
|
||||
high_err.append((joint, err_abs_mean))
|
||||
if high_tau:
|
||||
text = ", ".join(f"{name}:{value:.2f}Nm" for name, value in sorted(high_tau, key=lambda x: -x[1])[:4])
|
||||
print(f" high_tau_mean {text}")
|
||||
global_notes.append(f"{phase}: high mean torque -> {text}")
|
||||
if high_err:
|
||||
text = ", ".join(f"{name}:{value:.3f}rad" for name, value in sorted(high_err, key=lambda x: -x[1])[:4])
|
||||
print(f" high_err_mean {text}")
|
||||
global_notes.append(f"{phase}: high tracking error -> {text}")
|
||||
if global_notes:
|
||||
print("\n[Analyze] notes:")
|
||||
for note in global_notes:
|
||||
print(f" - {note}")
|
||||
return 0
|
||||
|
||||
|
||||
def main() -> int:
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("log_dir", type=str)
|
||||
args = parser.parse_args()
|
||||
return summarize(Path(args.log_dir))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,163 @@
|
||||
"""零位偏移标定向导。
|
||||
|
||||
用途：把机器人摆到 sim2sim/训练侧的 stand 默认姿态（人工摆好），
|
||||
跑这个脚本，它会读 16 个电机的当前位置，反算每个电机的 ZERO_OFFSET。
|
||||
|
||||
关键公式（与 motor_mapping.py 一致）：
|
||||
real = sign * sim + offset
|
||||
当 sim = stand_default 时：
|
||||
offset = real - sign * stand_default
|
||||
|
||||
⚠️ 使用前置条件：
|
||||
1. 已运行过 motor_driver_direction_test 类的脚本，确认每个电机的 sign 是对的；
|
||||
sign 错的话本工具会算出错误的 offset 看起来很对，但发动作时机器人会反向冲撞
|
||||
2. 机器人物理上摆到 stand 姿态：四条腿微弯曲、轮子接地、机身水平
|
||||
3. 电机已 enable 并清除告警
|
||||
|
||||
输出：把打印出来的 ZERO_OFFSET_MAP 字段直接覆盖 motor_mapping.py 中的对应字典。
|
||||
"""
|
||||
import argparse
|
||||
import sys
|
||||
import time
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
sys.path.insert(0, str(Path(__file__).resolve().parents[1]))
|
||||
|
||||
from interface.motor_mapping import MotorMapping # noqa: E402
|
||||
from policy.policy_runner import PolicyRunner # noqa: E402
|
||||
|
||||
|
||||
def main():
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("--can1-port", default="/dev/can1")
|
||||
parser.add_argument("--can2-port", default="/dev/can2")
|
||||
parser.add_argument("--motor-model", default="rs-02")
|
||||
parser.add_argument("--samples", type=int, default=100,
|
||||
help="平均采样帧数（去抖动）")
|
||||
parser.add_argument("--target-pose", default="stand", choices=["stand", "crawl"],
|
||||
help="标定时机器人摆的物理姿态")
|
||||
parser.add_argument("--no-enable", action="store_true",
|
||||
help="不主动 enable 电机（仅读取，适合手动转关节标定）")
|
||||
args = parser.parse_args()
|
||||
|
||||
# 真机驱动注入（路径优先级与 main.py 一致：vendored/drivers > /home/rc2/...）
|
||||
sim2real_root = Path(__file__).resolve().parents[1]
|
||||
for path in (sim2real_root / "vendored",
|
||||
"/home/rc2/work/rcwork/control",
|
||||
"/home/rc2/work/rcwork"):
|
||||
sp = str(path)
|
||||
if sp not in sys.path and Path(path).exists():
|
||||
sys.path.append(sp)
|
||||
from drivers.motor_driver import RobStrideDriver # type: ignore
|
||||
|
||||
mapper = MotorMapping()
|
||||
drv1 = RobStrideDriver(args.can1_port, debug=False)
|
||||
drv2 = RobStrideDriver(args.can2_port, debug=False)
|
||||
drv1.connect()
|
||||
drv2.connect()
|
||||
|
||||
for jk in mapper.SIM_JOINT_ORDER:
|
||||
leg, joint = jk
|
||||
bus, mid = mapper.CAN_ID_MAP[jk]
|
||||
name = f"{leg}_{joint}"
|
||||
(drv1 if bus == 1 else drv2).add_motor(name, mid, args.motor_model)
|
||||
|
||||
if not args.no_enable:
|
||||
print("[Calib] Enable 电机以读取状态...（已就位则可加 --no-enable 跳过）")
|
||||
for drv in (drv1, drv2):
|
||||
for name in drv.motors:
|
||||
drv.clear_warnings(name)
|
||||
drv.enable(name)
|
||||
time.sleep(0.5)
|
||||
|
||||
# 选择标定姿态
|
||||
if args.target_pose == "stand":
|
||||
sim_pose = PolicyRunner.DEFAULT_STAND_POSE.copy() # [0,0.9,-1.8] x4 + zeros
|
||||
else:
|
||||
sim_pose = np.array([
|
||||
0.4, 1.65, -2.55, -0.4, 1.65, -2.55,
|
||||
0.4, 1.65, -2.55, -0.4, 1.65, -2.55,
|
||||
0.0, 0.0, 0.0, 0.0,
|
||||
], dtype=np.float32)
|
||||
|
||||
print(f"\n[Calib] 请把机器人物理摆成 {args.target_pose.upper()} 姿态：")
|
||||
if args.target_pose == "stand":
|
||||
print(" 四条腿髋外展=0, 髋俯仰=0.9rad(~52°), 膝=-1.8rad(~-103°), 轮接地")
|
||||
else:
|
||||
print(" 内收外展 ±0.4rad, 髋俯仰=1.65rad, 膝=-2.55rad（深蹲下趴）")
|
||||
print(" 轮子可以保持任意角度，offset 强制为 0")
|
||||
print(" 按回车开始采样...")
|
||||
try:
|
||||
input()
|
||||
except EOFError:
|
||||
pass
|
||||
|
||||
print(f"\n[Calib] 开始采样 {args.samples} 帧并平均...")
|
||||
pos_acc = np.zeros(16, dtype=np.float64)
|
||||
valid = 0
|
||||
for i in range(args.samples):
|
||||
drv1.process_messages()
|
||||
drv2.process_messages()
|
||||
real_pos = {}
|
||||
for drv_idx, drv in enumerate((drv1, drv2)):
|
||||
bus = drv_idx + 1
|
||||
for name, motor in drv.motors.items():
|
||||
parts = name.split("_", 1)
|
||||
if len(parts) != 2:
|
||||
continue
|
||||
key = (parts[0], parts[1])
|
||||
if key not in mapper.CAN_ID_MAP:
|
||||
continue
|
||||
_, mid = mapper.CAN_ID_MAP[key]
|
||||
real_pos[(bus, mid)] = motor.state.position
|
||||
if len(real_pos) == 16:
|
||||
ordered = np.array([real_pos[mapper.CAN_ID_MAP[jk]]
|
||||
for jk in mapper.SIM_JOINT_ORDER], dtype=np.float64)
|
||||
pos_acc += ordered
|
||||
valid += 1
|
||||
time.sleep(0.02)
|
||||
|
||||
if valid < args.samples * 0.5:
|
||||
print(f"[Calib] 警告: 只收到 {valid}/{args.samples} 帧反馈，标定可能不可靠")
|
||||
real_avg = pos_acc / max(valid, 1)
|
||||
|
||||
# 反算 offset：offset = real - sign * sim
|
||||
sign = mapper._sign
|
||||
offsets = real_avg - sign * sim_pose
|
||||
|
||||
# 轮子 offset 强制 0
|
||||
for i, jk in enumerate(mapper.SIM_JOINT_ORDER):
|
||||
if jk[1] == "wheel":
|
||||
offsets[i] = 0.0
|
||||
|
||||
# 打印结果（按 motor_mapping.py 的字典格式）
|
||||
print("\n" + "=" * 64)
|
||||
print(f"[Calib] 标定完成（{valid} 帧平均）")
|
||||
print("=" * 64)
|
||||
print("把以下字典覆盖 sim2real/interface/motor_mapping.py 中的 ZERO_OFFSET_MAP:")
|
||||
print()
|
||||
print(" ZERO_OFFSET_MAP = {")
|
||||
for i, jk in enumerate(mapper.SIM_JOINT_ORDER):
|
||||
leg, joint = jk
|
||||
cur = offsets[i]
|
||||
old = mapper.ZERO_OFFSET_MAP[jk]
|
||||
delta = cur - old
|
||||
marker = " *" if abs(delta) > 0.01 else ""
|
||||
print(f' ("{leg}", "{joint:13s}"): {cur:>+8.4f}, '
|
||||
f'# old={old:+.4f} delta={delta:+.4f}{marker}')
|
||||
print(" }")
|
||||
print("\n标记 * 的项与现表偏离 > 0.01 rad，请重点核对该关节的 sign 是否正确。\n")
|
||||
|
||||
# Disable
|
||||
if not args.no_enable:
|
||||
for drv in (drv1, drv2):
|
||||
for name in drv.motors:
|
||||
drv.disable(name)
|
||||
drv1.disconnect()
|
||||
drv2.disconnect()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,80 @@
|
||||
"""Export the current PyTorch actor checkpoint to ONNX and verify parity."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
from pathlib import Path
|
||||
import sys
|
||||
|
||||
import numpy as np
|
||||
import torch
|
||||
|
||||
sys.path.insert(0, str(Path(__file__).resolve().parents[1]))
|
||||
|
||||
from policy.policy_runner import load_policy # noqa: E402
|
||||
|
||||
|
||||
def export_onnx(pt_path: Path, onnx_path: Path, opset: int = 14) -> None:
|
||||
device = torch.device("cpu")
|
||||
model = load_policy(pt_path, device)
|
||||
if getattr(model, "backend", "torch") != "torch":
|
||||
raise ValueError(f"export source must be a .pt policy, got {pt_path}")
|
||||
|
||||
obs_dim = int(model.expected_obs_dim)
|
||||
dummy = torch.randn(1, obs_dim, dtype=torch.float32, device=device)
|
||||
onnx_path.parent.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
torch.onnx.export(
|
||||
model,
|
||||
dummy,
|
||||
str(onnx_path),
|
||||
export_params=True,
|
||||
opset_version=opset,
|
||||
do_constant_folding=True,
|
||||
input_names=["obs"],
|
||||
output_names=["action"],
|
||||
dynamic_axes={"obs": {0: "batch_size"}, "action": {0: "batch_size"}},
|
||||
)
|
||||
|
||||
try:
|
||||
import onnxruntime as ort
|
||||
except ImportError:
|
||||
print("[Export] onnxruntime not installed; export done but parity check skipped.")
|
||||
return
|
||||
|
||||
opts = ort.SessionOptions()
|
||||
opts.intra_op_num_threads = 1
|
||||
opts.inter_op_num_threads = 1
|
||||
opts.execution_mode = ort.ExecutionMode.ORT_SEQUENTIAL
|
||||
session = ort.InferenceSession(str(onnx_path), sess_options=opts, providers=["CPUExecutionProvider"])
|
||||
with torch.no_grad():
|
||||
torch_out = model(dummy).detach().cpu().numpy()
|
||||
ort_out = session.run([session.get_outputs()[0].name], {session.get_inputs()[0].name: dummy.cpu().numpy()})[0]
|
||||
max_diff = float(np.max(np.abs(torch_out - ort_out)))
|
||||
mean_diff = float(np.mean(np.abs(torch_out - ort_out)))
|
||||
print(f"[Export] ONNX parity max_diff={max_diff:.8f}, mean_diff={mean_diff:.8f}")
|
||||
if max_diff > 1e-4:
|
||||
raise RuntimeError(f"ONNX parity check failed: max_diff={max_diff:.8f}")
|
||||
|
||||
|
||||
def main() -> int:
|
||||
root = Path(__file__).resolve().parents[1]
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("--pt", default=str(root / "policies" / "model_rough.pt"), help="Source .pt checkpoint")
|
||||
parser.add_argument("--onnx", default=None, help="Destination .onnx path; default replaces .pt suffix")
|
||||
parser.add_argument("--opset", type=int, default=14)
|
||||
args = parser.parse_args()
|
||||
|
||||
pt_path = Path(args.pt)
|
||||
onnx_path = Path(args.onnx) if args.onnx else pt_path.with_suffix(".onnx")
|
||||
if not pt_path.exists():
|
||||
print(f"[Export] missing source policy: {pt_path}")
|
||||
return 1
|
||||
|
||||
export_onnx(pt_path, onnx_path, args.opset)
|
||||
print(f"[Export] wrote {onnx_path}")
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,296 @@
|
||||
"""Logging helpers for sim2real runs.
|
||||
|
||||
Each session writes:
|
||||
- `state.csv`: high-rate state stream
|
||||
- `events.jsonl`: event / milestone stream
|
||||
"""
|
||||
|
||||
import json
|
||||
import queue
|
||||
import threading
|
||||
import time
|
||||
from datetime import datetime
|
||||
from pathlib import Path
|
||||
from typing import Any, Dict, Optional
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
class LogBundle:
|
||||
"""One session directory containing state CSV and event JSONL."""
|
||||
|
||||
JOINT_LABELS = (
|
||||
"fl_hip_abd", "fl_hip_pitch", "fl_knee",
|
||||
"fr_hip_abd", "fr_hip_pitch", "fr_knee",
|
||||
"rl_hip_abd", "rl_hip_pitch", "rl_knee",
|
||||
"rr_hip_abd", "rr_hip_pitch", "rr_knee",
|
||||
"fl_wheel", "fr_wheel", "rl_wheel", "rr_wheel",
|
||||
)
|
||||
|
||||
def __init__(self, log_root: str = "logs"):
|
||||
timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
|
||||
self.dir = Path(log_root) / timestamp
|
||||
self.dir.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
self.state_path = self.dir / "state.csv"
|
||||
self.events_path = self.dir / "events.jsonl"
|
||||
|
||||
self._state_fp = open(self.state_path, "w", encoding="utf-8")
|
||||
self._events_fp = open(self.events_path, "w", encoding="utf-8")
|
||||
self._t0 = time.time()
|
||||
self._closed = False
|
||||
self._queue: "queue.Queue[tuple]" = queue.Queue(maxsize=20000)
|
||||
self._dropped_state_rows = 0
|
||||
self._writer_thread = threading.Thread(target=self._writer_loop, name="sim2real-log-writer", daemon=True)
|
||||
|
||||
self._write_state_header()
|
||||
self._writer_thread.start()
|
||||
self.event("LOG_START", session_dir=str(self.dir))
|
||||
print(f"[Log] {self.dir}")
|
||||
|
||||
def _write_state_header(self):
|
||||
cols = ["t", "t_rel", "phase"]
|
||||
cols += [f"{joint}_pos" for joint in self.JOINT_LABELS]
|
||||
cols += [f"{joint}_vel" for joint in self.JOINT_LABELS]
|
||||
cols += [f"{joint}_tau" for joint in self.JOINT_LABELS]
|
||||
cols += [f"{joint}_tgt" for joint in self.JOINT_LABELS]
|
||||
cols += [f"{joint}_raw" for joint in self.JOINT_LABELS]
|
||||
cols += ["gyro_x", "gyro_y", "gyro_z"]
|
||||
cols += ["accel_x", "accel_y", "accel_z"]
|
||||
cols += ["quat_w", "quat_x", "quat_y", "quat_z"]
|
||||
cols += ["pgrav_x", "pgrav_y", "pgrav_z"]
|
||||
cols += ["cmd_vx", "cmd_vy", "cmd_yaw"]
|
||||
cols += ["imu_age_ms", "loop_dt_ms"]
|
||||
cols += ["safety_level", "guard_level"]
|
||||
cols += ["holdover", "stale_max", "fresh_count"]
|
||||
cols += ["kp_scale", "nan_flag"]
|
||||
cols += ["kp_leg_cmd", "kd_leg_cmd", "kd_wheel_cmd"]
|
||||
cols += ["runtime_release_alpha", "runtime_release_hold_s", "runtime_blend_ratio"]
|
||||
cols += ["hold_target_max_err", "policy_target_max_err", "hold_policy_max_gap"]
|
||||
cols += [
|
||||
"stand_roll_deg",
|
||||
"stand_pitch_deg",
|
||||
"stand_roll_corr",
|
||||
"stand_pitch_corr",
|
||||
"stand_pitch_comp_enabled",
|
||||
]
|
||||
cols += ["target_source_code"]
|
||||
cols += [
|
||||
"clip_primary_joint_index",
|
||||
"clip_primary_joint",
|
||||
"clip_primary_target",
|
||||
"clip_primary_measured",
|
||||
"clip_primary_default",
|
||||
"clip_primary_pos_err",
|
||||
"clip_primary_raw",
|
||||
"clip_primary_scaled",
|
||||
]
|
||||
cols += ["safety_reason", "guard_reason"]
|
||||
self._state_fp.write(",".join(cols) + "\n")
|
||||
self._state_fp.flush()
|
||||
|
||||
def state(
|
||||
self,
|
||||
phase: str,
|
||||
joint_pos: np.ndarray,
|
||||
joint_vel: np.ndarray,
|
||||
joint_torque: np.ndarray,
|
||||
target_pose: np.ndarray,
|
||||
raw_action: Optional[np.ndarray],
|
||||
gyro: np.ndarray,
|
||||
accel: np.ndarray,
|
||||
quat: np.ndarray,
|
||||
proj_gravity: np.ndarray,
|
||||
command: np.ndarray,
|
||||
imu_age_ms: float,
|
||||
loop_dt_ms: float,
|
||||
safety_level: int = 0,
|
||||
guard_level: int = 0,
|
||||
holdover: int = 0,
|
||||
stale_max: int = 0,
|
||||
fresh_count: int = 16,
|
||||
kp_scale: float = 1.0,
|
||||
nan_flag: int = 0,
|
||||
kp_leg_cmd: float = 0.0,
|
||||
kd_leg_cmd: float = 0.0,
|
||||
kd_wheel_cmd: float = 0.0,
|
||||
runtime_release_alpha: float = 0.0,
|
||||
runtime_release_hold_s: float = 0.0,
|
||||
runtime_blend_ratio: float = 0.0,
|
||||
hold_target_max_err: float = 0.0,
|
||||
policy_target_max_err: float = 0.0,
|
||||
hold_policy_max_gap: float = 0.0,
|
||||
stand_roll_deg: float = 0.0,
|
||||
stand_pitch_deg: float = 0.0,
|
||||
stand_roll_corr: float = 0.0,
|
||||
stand_pitch_corr: float = 0.0,
|
||||
stand_pitch_comp_enabled: bool = False,
|
||||
target_source: str = "",
|
||||
clip_primary_joint: str = "",
|
||||
clip_primary_target: float = 0.0,
|
||||
clip_primary_measured: float = 0.0,
|
||||
clip_primary_default: float = 0.0,
|
||||
clip_primary_pos_err: float = 0.0,
|
||||
clip_primary_raw: float = 0.0,
|
||||
clip_primary_scaled: float = 0.0,
|
||||
safety_reason: str = "",
|
||||
guard_reason: str = "",
|
||||
):
|
||||
if self._closed:
|
||||
return
|
||||
if target_pose is None:
|
||||
target_pose = np.zeros(16, dtype=np.float32)
|
||||
if raw_action is None:
|
||||
raw_action = np.zeros(16, dtype=np.float32)
|
||||
|
||||
now = time.time()
|
||||
numeric_values = []
|
||||
numeric_values += joint_pos.tolist()
|
||||
numeric_values += joint_vel.tolist()
|
||||
numeric_values += joint_torque.tolist()
|
||||
numeric_values += target_pose.tolist()
|
||||
numeric_values += raw_action.tolist()
|
||||
numeric_values += gyro.tolist()
|
||||
numeric_values += accel.tolist()
|
||||
numeric_values += quat.tolist()
|
||||
numeric_values += proj_gravity.tolist()
|
||||
numeric_values += command.tolist()
|
||||
numeric_values += [imu_age_ms, loop_dt_ms]
|
||||
numeric_values += [safety_level, guard_level, holdover, stale_max, fresh_count, kp_scale, nan_flag]
|
||||
numeric_values += [kp_leg_cmd, kd_leg_cmd, kd_wheel_cmd]
|
||||
numeric_values += [runtime_release_alpha, runtime_release_hold_s, runtime_blend_ratio]
|
||||
numeric_values += [hold_target_max_err, policy_target_max_err, hold_policy_max_gap]
|
||||
numeric_values += [
|
||||
stand_roll_deg,
|
||||
stand_pitch_deg,
|
||||
stand_roll_corr,
|
||||
stand_pitch_corr,
|
||||
1.0 if stand_pitch_comp_enabled else 0.0,
|
||||
]
|
||||
numeric_values += [_target_source_code(target_source)]
|
||||
numeric_values += [_csv_numeric_joint_index(clip_primary_joint)]
|
||||
numeric_values += [
|
||||
clip_primary_target,
|
||||
clip_primary_measured,
|
||||
clip_primary_default,
|
||||
clip_primary_pos_err,
|
||||
clip_primary_raw,
|
||||
clip_primary_scaled,
|
||||
]
|
||||
|
||||
parts = [f"{now:.6f}", f"{now - self._t0:.6f}", phase]
|
||||
parts += [f"{value:.6f}" for value in numeric_values]
|
||||
parts += [_csv_escape(clip_primary_joint), _csv_escape(safety_reason), _csv_escape(guard_reason)]
|
||||
self._enqueue(("state", ",".join(parts) + "\n"), drop_if_full=True)
|
||||
|
||||
def event(self, kind: str, **fields: Any):
|
||||
if self._closed:
|
||||
return
|
||||
record = {"t": time.time(), "t_rel": time.time() - self._t0, "kind": kind}
|
||||
for key, value in fields.items():
|
||||
if isinstance(value, np.ndarray):
|
||||
record[key] = value.tolist()
|
||||
elif isinstance(value, (np.integer, np.floating)):
|
||||
record[key] = value.item()
|
||||
else:
|
||||
record[key] = value
|
||||
self._enqueue(("event", json.dumps(record, ensure_ascii=False) + "\n", kind != "STATE_TICK"), drop_if_full=False)
|
||||
if kind != "STATE_TICK":
|
||||
print(f"[Event {record['t_rel']:7.2f}s] {kind} {_short_fields(fields)}")
|
||||
|
||||
def flush(self):
|
||||
if not self._closed:
|
||||
self._queue.join()
|
||||
self._state_fp.flush()
|
||||
self._events_fp.flush()
|
||||
|
||||
def close(self):
|
||||
if self._closed:
|
||||
return
|
||||
self.event("LOG_END")
|
||||
self._queue.join()
|
||||
self._closed = True
|
||||
self._enqueue(("close",), drop_if_full=False, allow_after_closed=True)
|
||||
self._writer_thread.join(timeout=2.0)
|
||||
self._state_fp.flush()
|
||||
self._state_fp.close()
|
||||
self._events_fp.flush()
|
||||
self._events_fp.close()
|
||||
print(f"[Log] saved -> {self.dir}")
|
||||
|
||||
def _enqueue(self, item: tuple, drop_if_full: bool, allow_after_closed: bool = False):
|
||||
if self._closed and not allow_after_closed:
|
||||
return
|
||||
try:
|
||||
if drop_if_full:
|
||||
self._queue.put_nowait(item)
|
||||
else:
|
||||
self._queue.put(item, timeout=0.2)
|
||||
except queue.Full:
|
||||
if item and item[0] == "state":
|
||||
self._dropped_state_rows += 1
|
||||
|
||||
def _writer_loop(self):
|
||||
while True:
|
||||
item = self._queue.get()
|
||||
try:
|
||||
kind = item[0]
|
||||
if kind == "close":
|
||||
return
|
||||
if kind == "state":
|
||||
self._state_fp.write(item[1])
|
||||
elif kind == "event":
|
||||
self._events_fp.write(item[1])
|
||||
if item[2]:
|
||||
self._events_fp.flush()
|
||||
finally:
|
||||
self._queue.task_done()
|
||||
|
||||
|
||||
def _csv_escape(text: str) -> str:
|
||||
if not text:
|
||||
return ""
|
||||
return text.replace(",", ";").replace("\n", " ").replace("\r", " ")
|
||||
|
||||
|
||||
def _csv_numeric_joint_index(joint_name: str) -> float:
|
||||
if not joint_name:
|
||||
return -1.0
|
||||
try:
|
||||
return float(LogBundle.JOINT_LABELS.index(joint_name))
|
||||
except ValueError:
|
||||
return -1.0
|
||||
|
||||
|
||||
def _target_source_code(target_source: str) -> float:
|
||||
mapping = {
|
||||
"": -1.0,
|
||||
"startup_hold": 0.0,
|
||||
"stand_balance": 1.0,
|
||||
"stand_hold": 1.5,
|
||||
"runtime_hold": 2.0,
|
||||
"runtime_blend": 3.0,
|
||||
"runtime_policy": 4.0,
|
||||
}
|
||||
return mapping.get(target_source, 99.0)
|
||||
|
||||
|
||||
def _short_fields(fields: Dict[str, Any]) -> str:
|
||||
parts = []
|
||||
for key, value in fields.items():
|
||||
if isinstance(value, (list, tuple, np.ndarray)):
|
||||
arr = np.asarray(value).ravel()
|
||||
if arr.size > 4:
|
||||
continue
|
||||
try:
|
||||
parts.append(f"{key}=[{','.join(f'{float(x):.2f}' for x in arr)}]")
|
||||
except (TypeError, ValueError):
|
||||
parts.append(f"{key}={list(arr)[:4]}")
|
||||
elif isinstance(value, float):
|
||||
parts.append(f"{key}={value:.3f}")
|
||||
else:
|
||||
parts.append(f"{key}={value}")
|
||||
return " ".join(parts)
|
||||
|
||||
|
||||
SimpleLogger = LogBundle
|
||||
@@ -0,0 +1,111 @@
|
||||
"""数学工具 — 与 rc_mjlab/sim2sim/tools/math_utils.py 数值完全一致。"""
|
||||
from typing import Optional
|
||||
import numpy as np
|
||||
|
||||
|
||||
def get_gravity_orientation(quat_wxyz: np.ndarray) -> np.ndarray:
|
||||
qw, qx, qy, qz = quat_wxyz
|
||||
gx = 2.0 * (-qz * qx + qw * qy)
|
||||
gy = -2.0 * (qz * qy + qw * qx)
|
||||
gz = 1.0 - 2.0 * (qw * qw + qz * qz)
|
||||
return np.array([gx, gy, gz], dtype=np.float32)
|
||||
|
||||
|
||||
def quat_rotate_inverse(quat_wxyz: np.ndarray, v: np.ndarray) -> np.ndarray:
|
||||
q_w = quat_wxyz[0]
|
||||
q_vec = quat_wxyz[1:]
|
||||
a = v * (2.0 * q_w * q_w - 1.0)
|
||||
b = np.cross(q_vec, v) * q_w * 2.0
|
||||
c = q_vec * np.dot(q_vec, v) * 2.0
|
||||
return a - b + c
|
||||
|
||||
|
||||
def quat_from_accel(accel: np.ndarray) -> np.ndarray:
|
||||
"""用静止重力方向初始化机身姿态四元数。
|
||||
|
||||
思想：仿真启动时 quat = [1,0,0,0] 隐含"机身完全水平"，但真机摆在地面上
|
||||
pitch/roll 通常各自有几度偏差，会让 projected_gravity 一开始就错。
|
||||
用加速度计读数与 [0,0,-1] 的最短旋转作为初值，可以把首步重力误差
|
||||
降到 IMU 噪声级。
|
||||
"""
|
||||
g_meas = accel / (np.linalg.norm(accel) + 1e-9)
|
||||
g_ref = np.array([0.0, 0.0, 1.0], dtype=np.float32)
|
||||
cross = np.cross(g_ref, g_meas)
|
||||
dot = float(np.dot(g_ref, g_meas))
|
||||
if dot < -0.999999:
|
||||
return np.array([0.0, 1.0, 0.0, 0.0], dtype=np.float32)
|
||||
s = float(np.sqrt((1.0 + dot) * 2.0))
|
||||
q = np.array([s * 0.5, cross[0] / s, cross[1] / s, cross[2] / s], dtype=np.float32)
|
||||
return q / (np.linalg.norm(q) + 1e-9)
|
||||
|
||||
|
||||
class LowPassFilter:
|
||||
"""一阶 IIR 低通，alpha 公式与训练侧 rc_mjlab/src/robot/mdp/lowpass_actions.py
|
||||
`_lowpass_weights` 完全一致：
|
||||
|
||||
alpha = 1 - exp(-2π · cutoff_freq / control_freq)
|
||||
= 1 - exp(-2π · cutoff_freq · dt)
|
||||
|
||||
注意：这与 rc_mjlab/sim2sim/interface/mujoco_io.py 用的近似公式
|
||||
(dt / (dt + 1/(2π·fc))) 数值上不同，在 15Hz 截止时差约 30%。
|
||||
我们以训练侧为准，因为策略是在那个滤波下学的。
|
||||
"""
|
||||
|
||||
def __init__(self, cutoff_freq: float, dt: float, dim: int):
|
||||
self.alpha = float(1.0 - np.exp(-2.0 * np.pi * cutoff_freq * dt))
|
||||
self.y_prev = None
|
||||
|
||||
def filter(self, x: np.ndarray) -> np.ndarray:
|
||||
if self.y_prev is None:
|
||||
self.y_prev = x.copy()
|
||||
y = self.alpha * x + (1.0 - self.alpha) * self.y_prev
|
||||
self.y_prev = y.copy()
|
||||
return y
|
||||
|
||||
def reset(self):
|
||||
self.y_prev = None
|
||||
|
||||
|
||||
class MahonyFilter:
|
||||
"""互补滤波器：高频用陀螺仪积分，低频用加速度计修正。"""
|
||||
|
||||
def __init__(self, kp: float = 2.0, ki: float = 0.0, dt: float = 0.02):
|
||||
self.kp = kp
|
||||
self.ki = ki
|
||||
self.dt = dt
|
||||
self.q = np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float32)
|
||||
self.e_int = np.zeros(3, dtype=np.float32)
|
||||
|
||||
def reset_with_accel(self, accel: np.ndarray):
|
||||
self.q = quat_from_accel(accel)
|
||||
self.e_int.fill(0.0)
|
||||
|
||||
def update(self, accel: np.ndarray, gyro: np.ndarray, dt: Optional[float] = None) -> np.ndarray:
|
||||
if dt is None:
|
||||
dt = self.dt
|
||||
norm_a = float(np.linalg.norm(accel))
|
||||
if norm_a > 1e-6:
|
||||
a = accel / norm_a
|
||||
q = self.q
|
||||
v = np.array([
|
||||
2.0 * (q[1] * q[3] - q[0] * q[2]),
|
||||
2.0 * (q[0] * q[1] + q[2] * q[3]),
|
||||
q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3],
|
||||
], dtype=np.float32)
|
||||
e = np.cross(a, v)
|
||||
if self.ki > 0.0:
|
||||
self.e_int += e * dt
|
||||
else:
|
||||
self.e_int.fill(0.0)
|
||||
gyro = gyro + self.kp * e + self.ki * self.e_int
|
||||
|
||||
q = self.q
|
||||
q_dot = 0.5 * np.array([
|
||||
-q[1] * gyro[0] - q[2] * gyro[1] - q[3] * gyro[2],
|
||||
q[0] * gyro[0] + q[2] * gyro[2] - q[3] * gyro[1],
|
||||
q[0] * gyro[1] - q[1] * gyro[2] + q[3] * gyro[0],
|
||||
q[0] * gyro[2] + q[1] * gyro[1] - q[2] * gyro[0],
|
||||
], dtype=np.float32)
|
||||
self.q += q_dot * dt
|
||||
self.q /= (np.linalg.norm(self.q) + 1e-9)
|
||||
return self.q
|
||||
@@ -0,0 +1,61 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import sys
|
||||
import time
|
||||
from pathlib import Path
|
||||
|
||||
sys.path.insert(0, str(Path(__file__).resolve().parents[1]))
|
||||
|
||||
from input_dev.remote_uart import RemoteCommandSource # noqa: E402
|
||||
|
||||
|
||||
def main() -> None:
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("--port", required=True)
|
||||
parser.add_argument("--max-vx", type=float, default=0.8)
|
||||
parser.add_argument("--max-vy", type=float, default=0.3)
|
||||
parser.add_argument("--max-yaw", type=float, default=0.5)
|
||||
parser.add_argument("--hz", type=float, default=20.0)
|
||||
parser.add_argument("--baudrate", type=int, default=100000)
|
||||
parser.add_argument("--timeout", type=float, default=0.02)
|
||||
parser.add_argument("--deadzone", type=int, default=50)
|
||||
args = parser.parse_args()
|
||||
|
||||
remote = RemoteCommandSource(
|
||||
port=args.port,
|
||||
baudrate=args.baudrate,
|
||||
timeout=args.timeout,
|
||||
axis_deadzone=args.deadzone,
|
||||
active_threshold=args.deadzone,
|
||||
max_vx=args.max_vx,
|
||||
max_vy=args.max_vy,
|
||||
max_yaw=args.max_yaw,
|
||||
)
|
||||
remote.open()
|
||||
print(f"[remote-test] listening on {args.port}")
|
||||
try:
|
||||
period = 1.0 / max(args.hz, 1.0)
|
||||
while True:
|
||||
remote.poll()
|
||||
status = remote.get_status()
|
||||
print(
|
||||
"cmd=({:+.3f}, {:+.3f}, {:+.3f}) active={} estop={} raw=({}, {}, {}, {})".format(
|
||||
status["cmd"][0],
|
||||
status["cmd"][1],
|
||||
status["cmd"][2],
|
||||
status["command_active"],
|
||||
status["estop_requested"],
|
||||
status["ch1"],
|
||||
status["ch2"],
|
||||
status["ch3"],
|
||||
status["ch4"],
|
||||
)
|
||||
)
|
||||
time.sleep(period)
|
||||
finally:
|
||||
remote.close()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,105 @@
|
||||
"""Check whether `sim2real/` is self-contained enough for direct deployment."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import importlib
|
||||
from pathlib import Path
|
||||
import sys
|
||||
|
||||
sys.path.insert(0, str(Path(__file__).resolve().parents[1]))
|
||||
|
||||
|
||||
REQUIRED_FILES = [
|
||||
"config.yaml",
|
||||
"deployment_manifest.yaml",
|
||||
"main.py",
|
||||
"policy/policy_runner.py",
|
||||
"interface/real_io.py",
|
||||
"interface/imu_client.py",
|
||||
"interface/motor_driver.py",
|
||||
"vendored/drivers/motor_driver.py",
|
||||
"vendored/drivers/usb_can_adapter.py",
|
||||
"vendored/odin1_imu/odin1_imu.py",
|
||||
"vendored/odin1_imu/build/libodin1_imu_bridge.so",
|
||||
"mjcf/wheelleg.xml",
|
||||
]
|
||||
|
||||
REQUIRED_IMPORTS = [
|
||||
"numpy",
|
||||
"yaml",
|
||||
"serial",
|
||||
]
|
||||
|
||||
OPTIONAL_IMPORTS = [
|
||||
("onnxruntime", "required when deploying the default ONNX policy"),
|
||||
("torch", "required only for exporting/checking .pt policies"),
|
||||
("pynput", "only needed for CLI keyboard control"),
|
||||
]
|
||||
|
||||
|
||||
def check() -> int:
|
||||
root = Path(__file__).resolve().parents[1]
|
||||
issues: list[str] = []
|
||||
warnings: list[str] = []
|
||||
|
||||
print(f"[Check] sim2real root: {root}")
|
||||
|
||||
for rel in REQUIRED_FILES:
|
||||
path = root / rel
|
||||
if path.exists():
|
||||
print(f"[Check] file: PASS {rel}")
|
||||
else:
|
||||
issues.append(f"missing required file: {rel}")
|
||||
|
||||
if (root / "policies" / "model_rough.onnx").exists():
|
||||
print("[Check] file: PASS policies/model_rough.onnx")
|
||||
elif (root / "policies" / "model_rough.pt").exists():
|
||||
warnings.append("policies/model_rough.onnx not found; runtime will fall back to .pt unless exported")
|
||||
print("[Check] file: PASS policies/model_rough.pt fallback")
|
||||
else:
|
||||
issues.append("missing policy file: policies/model_rough.onnx or policies/model_rough.pt")
|
||||
|
||||
for module_name in REQUIRED_IMPORTS:
|
||||
try:
|
||||
importlib.import_module(module_name)
|
||||
print(f"[Check] import: PASS {module_name}")
|
||||
except Exception as exc:
|
||||
issues.append(f"missing python dependency `{module_name}`: {exc}")
|
||||
|
||||
for module_name, note in OPTIONAL_IMPORTS:
|
||||
try:
|
||||
importlib.import_module(module_name)
|
||||
print(f"[Check] optional import: PASS {module_name}")
|
||||
except Exception:
|
||||
warnings.append(f"optional dependency `{module_name}` not found ({note})")
|
||||
|
||||
index_html = (root / "web" / "static" / "index.html").read_text(encoding="utf-8")
|
||||
if "https://unpkg.com/three@" in index_html:
|
||||
warnings.append(
|
||||
"web 3D viewer depends on remote three.js CDN; CLI/web backend are standalone, "
|
||||
"but full offline 3D viewer is not bundled yet"
|
||||
)
|
||||
|
||||
if issues:
|
||||
print("\n" + "=" * 60)
|
||||
print("Standalone deployment check: FAIL")
|
||||
for item in issues:
|
||||
print(f"- {item}")
|
||||
else:
|
||||
print("\n" + "=" * 60)
|
||||
print("Standalone deployment check: PASS")
|
||||
|
||||
if warnings:
|
||||
print("\nWarnings:")
|
||||
for item in warnings:
|
||||
print(f"- {item}")
|
||||
|
||||
return 1 if issues else 0
|
||||
|
||||
|
||||
def main():
|
||||
raise SystemExit(check())
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,45 @@
|
||||
"""Offline checks for RS02 multi-turn angle wrapping in startup/control."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
import sys
|
||||
|
||||
import numpy as np
|
||||
|
||||
sys.path.insert(0, str(Path(__file__).resolve().parents[1]))
|
||||
|
||||
from interface.motor_mapping import MotorMapping # noqa: E402
|
||||
from startup.pose_initializer import STAND_POSE, _periodic_leg_delta # noqa: E402
|
||||
|
||||
|
||||
def main() -> int:
|
||||
mapper = MotorMapping()
|
||||
|
||||
# Real log example: startup saw rl_knee sim angle 4.648rad while the stand
|
||||
# target was -1.8rad. Those are close modulo 2*pi and must not plan a full turn.
|
||||
raw_sim = STAND_POSE.copy()
|
||||
raw_sim[8] = 4.648097991943359
|
||||
raw_real = mapper.sim_to_real(raw_sim)
|
||||
canonical = mapper.real_to_sim({(2, 3): raw_real[(2, 3)]})
|
||||
delta = _periodic_leg_delta(canonical, STAND_POSE)
|
||||
real_target = mapper.sim_to_real(STAND_POSE, current_real_pos={(2, 3): raw_real[(2, 3)]})[(2, 3)]
|
||||
real_move = real_target - raw_real[(2, 3)]
|
||||
|
||||
print(f"[WrapCheck] canonical_sim_idx8={canonical[8]:.6f}")
|
||||
print(f"[WrapCheck] startup_delta_idx8={delta[8]:.6f}")
|
||||
print(f"[WrapCheck] real_move_idx8={real_move:.6f}")
|
||||
|
||||
if abs(float(delta[8])) > 0.25:
|
||||
print("[WrapCheck] FAIL: periodic startup delta is too large")
|
||||
return 1
|
||||
if abs(float(real_move)) > 0.25:
|
||||
print("[WrapCheck] FAIL: real target would command a long-path move")
|
||||
return 1
|
||||
|
||||
print("[WrapCheck] PASS")
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,54 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
sys.path.insert(0, str(Path(__file__).resolve().parents[1]))
|
||||
|
||||
from input_dev.remote_uart import ( # noqa: E402
|
||||
RemoteCommandMapper,
|
||||
RemoteControlState,
|
||||
RemoteSwitchState,
|
||||
SWITCH_HIGH,
|
||||
SWITCH_MID,
|
||||
)
|
||||
|
||||
|
||||
def assert_close(actual: float, expected: float, tol: float = 1e-6) -> None:
|
||||
if abs(actual - expected) > tol:
|
||||
raise AssertionError(f"expected {expected}, got {actual}")
|
||||
|
||||
|
||||
def test_deadzone() -> None:
|
||||
mapper = RemoteCommandMapper(max_vx=0.8, max_vy=0.3, max_yaw=0.5, active_threshold=50)
|
||||
state = RemoteControlState(ch1=40, ch2=-49, ch4=50, switches=RemoteSwitchState(ch7=SWITCH_MID), frame_ok=True)
|
||||
cmd = mapper.map_command(state)
|
||||
assert_close(float(cmd[0]), 0.0)
|
||||
assert_close(float(cmd[1]), 0.0)
|
||||
assert_close(float(cmd[2]), 0.0)
|
||||
if mapper.is_command_active(state):
|
||||
raise AssertionError("deadzone values should not be active")
|
||||
|
||||
|
||||
def test_mapping() -> None:
|
||||
mapper = RemoteCommandMapper(max_vx=0.8, max_vy=0.3, max_yaw=0.5, active_threshold=50)
|
||||
state = RemoteControlState(ch1=330, ch2=-660, ch4=165, switches=RemoteSwitchState(ch7=SWITCH_MID), frame_ok=True)
|
||||
cmd = mapper.map_command(state)
|
||||
assert_close(float(cmd[0]), -0.8)
|
||||
assert_close(float(cmd[1]), 0.075)
|
||||
assert_close(float(cmd[2]), 0.25)
|
||||
if not mapper.is_command_active(state):
|
||||
raise AssertionError("mapped command should be active")
|
||||
|
||||
|
||||
def test_soft_estop_flag() -> None:
|
||||
state = RemoteControlState(ch1=0, ch2=0, ch4=0, switches=RemoteSwitchState(ch7=SWITCH_HIGH), frame_ok=True)
|
||||
if not state.estop_requested:
|
||||
raise AssertionError("switch high should request estop")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
test_deadzone()
|
||||
test_mapping()
|
||||
test_soft_estop_flag()
|
||||
print("remote command mapping tests passed")
|
||||
Reference in New Issue
Block a user