[software] 添加16DOF早期训练仿真与Sim2Real闭环
This commit is contained in:
@@ -0,0 +1,171 @@
|
||||
"""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 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
|
||||
|
||||
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")
|
||||
|
||||
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,
|
||||
)
|
||||
obs_mean = runner.policy.obs_mean.detach().cpu().numpy()
|
||||
obs_std = runner.policy.obs_std.detach().cpu().numpy()
|
||||
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_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")
|
||||
|
||||
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 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 .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,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,239 @@
|
||||
"""Logging helpers for sim2real runs.
|
||||
|
||||
Each session writes:
|
||||
- `state.csv`: high-rate state stream
|
||||
- `events.jsonl`: event / milestone stream
|
||||
"""
|
||||
|
||||
import json
|
||||
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._write_state_header()
|
||||
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 += ["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,
|
||||
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 += [_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._state_fp.write(",".join(parts) + "\n")
|
||||
|
||||
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._events_fp.write(json.dumps(record, ensure_ascii=False) + "\n")
|
||||
self._events_fp.flush()
|
||||
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._state_fp.flush()
|
||||
self._events_fp.flush()
|
||||
|
||||
def close(self):
|
||||
if self._closed:
|
||||
return
|
||||
self.event("LOG_END")
|
||||
self._state_fp.flush()
|
||||
self._state_fp.close()
|
||||
self._events_fp.flush()
|
||||
self._events_fp.close()
|
||||
self._closed = True
|
||||
print(f"[Log] saved -> {self.dir}")
|
||||
|
||||
|
||||
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,
|
||||
"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,108 @@
|
||||
"""数学工具 — 与 rc_mjlab/sim2sim/tools/math_utils.py 数值完全一致。"""
|
||||
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) -> np.ndarray:
|
||||
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 * self.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 * self.dt
|
||||
self.q /= (np.linalg.norm(self.q) + 1e-9)
|
||||
return self.q
|
||||
@@ -0,0 +1,97 @@
|
||||
"""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",
|
||||
"policies/model_rough.pt",
|
||||
"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",
|
||||
"torch",
|
||||
"serial",
|
||||
]
|
||||
|
||||
OPTIONAL_IMPORTS = [
|
||||
("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}")
|
||||
|
||||
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()
|
||||
Reference in New Issue
Block a user