diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..5292519 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +logs/ \ No newline at end of file diff --git a/__pycache__/deploy_onnx_pro_sdk.cpython-310.pyc b/__pycache__/deploy_onnx_pro_sdk.cpython-310.pyc new file mode 100644 index 0000000..8f1d497 Binary files /dev/null and b/__pycache__/deploy_onnx_pro_sdk.cpython-310.pyc differ diff --git a/__pycache__/sim2sim_test_deploy.cpython-310.pyc b/__pycache__/sim2sim_test_deploy.cpython-310.pyc new file mode 100644 index 0000000..f356cf4 Binary files /dev/null and b/__pycache__/sim2sim_test_deploy.cpython-310.pyc differ diff --git a/adaptation_module_latest.jit b/adaptation_module_latest.jit new file mode 100755 index 0000000..7d624b7 Binary files /dev/null and b/adaptation_module_latest.jit differ diff --git a/body_latest.jit b/body_latest.jit new file mode 100755 index 0000000..a0ba3f6 Binary files /dev/null and b/body_latest.jit differ diff --git a/deploy_onnx_pro_sdk.py b/deploy_onnx_pro_sdk.py new file mode 100644 index 0000000..c1ee77a --- /dev/null +++ b/deploy_onnx_pro_sdk.py @@ -0,0 +1,897 @@ +#!/usr/bin/env python3 +""" +deploy_onnx_pro_sdk.py + +Deploy ONNX policy on Unitree Go1 PRO using go1_pro_sdk (no official SDK needed). +Works on non-EDU Go1 PRO. + +Uses direct MCU UDP communication with Blowfish encryption. +Joint order matches ONNX directly: FR→FL→RR→RL (hip/thigh/calf per leg). +Observation: 45-dim, matches go1_sim2sim.py. + +Setup: + cd /path/to/go1_pro_sdk && pip install -e . + pip install onnxruntime numpy + +Before running, kill the Pi's sport processes: + ssh pi@192.168.123.161 + sudo pkill -9 -f keep_sport_alive + sudo pkill -9 -f Legged_sport + sudo pkill -9 -f appTransit + +ALWAYS suspend the robot for initial testing. + +Usage: + # Step 0: check 45-dim observation vector (no motors, no ONNX) + python deploy_onnx_pro_sdk.py --obs-check --kill-sport + + # Step 1: test remote controller data (no motors, no ONNX) + python deploy_onnx_pro_sdk.py --monitor --kill-sport + + # Step 2-5: state machine with RC (R2=go/stop, L2=emergency stop) + python deploy_onnx_pro_sdk.py --onnx policy.onnx --kill-sport + + # Fixed commands, no state machine + python deploy_onnx_pro_sdk.py --onnx policy.onnx --no-rc --no-sm --kill-sport +""" + +import argparse +import json +import signal +import subprocess +import sys +import time +from datetime import datetime +from enum import Enum +from pathlib import Path + +import numpy as np +import onnxruntime as ort + +from go1_pro_sdk import ( + MCUClient, LowCmd, MotorCmd, MotorMode, + apply_safety, JOINT_NAMES, +) + +# ─── Policy constants (matches go1_sim2sim.py) ─── +NUM_OBS = 45 +NUM_ACTIONS = 12 +ACTION_SCALE = 0.05 +CLIP_ACTIONS = 23.7 +CLIP_OBS = 100.0 + +DEFAULT_ANGLES = np.array([ + -0.0, 0.9, -1.8, # FR + 0.0, 0.9, -1.8, # FL + -0.0, 0.9, -1.8, # RR + 0.0, 0.9, -1.8, # RL +], dtype=np.float32) + +EXIT = False + + +def _sig_handler(signum, frame): + global EXIT + EXIT = True + + +signal.signal(signal.SIGINT, _sig_handler) +signal.signal(signal.SIGTERM, _sig_handler) + + +# ─── State machine states ─── +class State(Enum): + IDLE = "IDLE" # full damping, wait for R2 + CALIBRATE = "CALIBRATE" # ramping to default pose + HOLD = "HOLD" # holding default pose, wait for R2 to start RL + RL = "RL" # running ONNX policy + + +# ─── Quaternion math ─── +def quat_to_rot_matrix(q): + w, x, y, z = q + return np.array([ + [1 - 2*y*y - 2*z*z, 2*x*y - 2*w*z, 2*x*z + 2*w*y], + [ 2*x*y + 2*w*z, 1 - 2*x*x - 2*z*z, 2*y*z - 2*w*x], + [ 2*x*z - 2*w*y, 2*y*z + 2*w*x, 1 - 2*x*x - 2*y*y], + ], dtype=np.float32) + + +def get_projected_gravity(quaternion): + R = quat_to_rot_matrix(quaternion) + return (R.T @ np.array([0., 0., -1.], dtype=np.float32)).astype(np.float32) + + +# ─── Observation ─── +def compute_obs(imu, motor_states, commands, last_actions): + obs = np.zeros(NUM_OBS, dtype=np.float32) + + gx, gy, gz = imu.gyroscope + obs[0:3] = np.array([gx, gy, gz], dtype=np.float32) * 0.25 + obs[3:6] = get_projected_gravity(imu.quaternion) + + dof_pos = np.array([motor_states[i].q for i in range(12)], dtype=np.float32) + dof_vel = np.array([motor_states[i].dq for i in range(12)], dtype=np.float32) + obs[6:18] = (dof_pos - DEFAULT_ANGLES) * 1.0 + obs[18:30] = dof_vel * 0.05 + + obs[30:42] = last_actions + obs[42:45] = np.array(commands, dtype=np.float32) * np.array([2.0, 2.0, 0.25], dtype=np.float32) + + obs = np.clip(obs, -CLIP_OBS, CLIP_OBS) + obs = np.nan_to_num(obs, nan=0.0, posinf=0.0, neginf=0.0) + return obs + + +def state_ok(state): + for i in range(12): + if not np.isfinite(state.motorState[i].q): + return False, f"motorState[{i}].q non-finite" + gn = float(np.linalg.norm(get_projected_gravity(state.imu.quaternion))) + if gn < 0.5 or gn > 1.5: + return False, f"gravity norm={gn:.3f}" + return True, "ok" + + +# ─── ONNX policy ─── +class OnnxPolicy: + def __init__(self, onnx_path): + self.session = ort.InferenceSession(onnx_path, providers=["CPUExecutionProvider"]) + self.input_name = self.session.get_inputs()[0].name + print(f"[INFO] ONNX: {onnx_path}") + print(f"[INFO] Input : {self.input_name} {self.session.get_inputs()[0].shape}") + print(f"[INFO] Output: {[(o.name, o.shape) for o in self.session.get_outputs()]}") + + def __call__(self, obs): + out = self.session.run(None, {self.input_name: obs.reshape(1, -1).astype(np.float32)})[0][0] + return np.asarray(out, dtype=np.float32) + + +# ─── Remote controller ─── +def get_rc_commands(state, args): + r = state.remote + vx = r.ly * args.rc_vx_scale + vy = -r.lx * args.rc_vy_scale + wz = -r.rx * args.rc_wz_scale + return np.array([vx, vy, wz], dtype=np.float32) + + +class RCEdgeDetector: + """Detect rising/falling edges on RC buttons.""" + def __init__(self): + self._prev = set() + + def update(self, state): + current = set(state.remote.pressed) + rising = current - self._prev + falling = self._prev - current + self._prev = current + return rising, falling + + def rose(self, state, button): + """True on rising edge of button.""" + current = set(state.remote.pressed) + prev = self._prev + self._prev = current + return button not in prev and button in current + + +# ─── JSONL logger ─── +class JsonlLogger: + def __init__(self, log_dir, args): + self.enabled = bool(log_dir) + self.fp = None + self.run_dir = None + self.flush_every = 50 + if not self.enabled: + return + ts = datetime.now().strftime("%Y%m%d_%H%M%S") + self.run_dir = Path(log_dir).expanduser().resolve() / f"deploy_run_{ts}" + self.run_dir.mkdir(parents=True, exist_ok=True) + meta = { + "created_at": ts, + "num_obs": NUM_OBS, + "num_actions": NUM_ACTIONS, + "action_scale": ACTION_SCALE, + "clip_actions": CLIP_ACTIONS, + "clip_obs": CLIP_OBS, + "default_angles": DEFAULT_ANGLES.tolist(), + "joint_names": list(JOINT_NAMES), + } + for k, v in vars(args).items(): + if isinstance(v, (str, int, float, bool, type(None))): + meta[k] = v + (self.run_dir / "metadata.json").write_text(json.dumps(meta, indent=2, ensure_ascii=False)) + self.fp = open(self.run_dir / "steps.jsonl", "a", encoding="utf-8", buffering=1) + print(f"[INFO] Log dir: {self.run_dir}") + + def log(self, step, **kw): + if not self.enabled: + return + rec = {"step": int(step), "time_wall": time.time()} + for k, v in kw.items(): + if isinstance(v, np.ndarray): + rec[k] = np.asarray(v, dtype=np.float32).reshape(-1).tolist() + elif isinstance(v, (np.float32, np.float64)): + rec[k] = float(v) + elif isinstance(v, (np.int32, np.int64)): + rec[k] = int(v) + else: + rec[k] = v + self.fp.write(json.dumps(rec, ensure_ascii=False) + "\n") + if step % self.flush_every == 0: + self.fp.flush() + + def close(self): + if self.fp: + self.fp.flush(); self.fp.close() + print(f"[INFO] Log saved: {self.run_dir}") + + +# ─── Display helpers ─── +def fmt_rc(state): + r = state.remote + sticks = f"lx={r.lx:+.2f} ly={r.ly:+.2f} rx={r.rx:+.2f} ry={r.ry:+.2f} L2={r.L2:.2f}" + btns = ",".join(r.pressed) if r.pressed else "none" + return f"sticks:[{sticks}] btns:[{btns}]" + + +def fmt_joints(state): + parts = [] + for i in range(12): + parts.append(f"{state.motorState[i].q:+6.3f}") + return " ".join(parts) + + +# ─── Observation check mode ─── +def run_obs_check(args): + """Compute 45-dim observation and print each segment. No inference, no motors.""" + print(""" +╔══════════════════════════════════════════════════════════════╗ +║ OBS-CHECK MODE — verify 45-dim observation vector ║ +║ No inference, no motors. Compare with sim2sim reference. ║ +║ ║ +║ Ensure: ║ +║ 1. Robot SUSPENDED (sling / foot stand) ║ +║ 2. Network connected to robot (192.168.123.x) ║ +║ 3. Use --kill-sport to auto-kill Pi sport processes ║ +╚══════════════════════════════════════════════════════════════╝ +""") + input("Press Enter to start obs check...") + + if args.kill_sport: + kill_sport_processes(args.pi_host, args.pi_user) + + print("[INFO] Connecting to MCU...") + client = MCUClient() + logger = JsonlLogger(args.log_dir, args) + + try: + print("[INFO] Waking MCU...") + client.wake_mcu(n_frames=50, dt=0.01) + + state = client.recv_state(timeout=2.0) + if state is None: + print("[ERROR] No state received.") + return 1 + + print(f"[INFO] Connected. Battery={state.bms.SOC}%") + print(f"[INFO] RPY: {np.round(np.degrees(state.imu.rpy), 1)} deg") + print("[INFO] Computing 45-dim obs each step. Compare with sim2sim reference.\n") + + # Sim2sim reference (stationary, default pose, zero cmd) + print("─" * 70) + print("OBSERVATION LAYOUT (45-dim):") + print(" obs[ 0: 3] gyro * 0.25 → ~[0, 0, 0] when stationary") + print(" obs[ 3: 6] projected gravity → ~[0, 0, -1] when upright") + print(" obs[ 6:18] (dof_pos-def) * 1.0 → ~[0]*12 when at default pose") + print(" obs[18:30] dof_vel * 0.05 → ~[0]*12 when stationary") + print(" obs[30:42] last_actions → [0]*12 initially") + print(" obs[42:45] commands*[2,2,0.25] → [0,0,0] with zero cmd") + print("─" * 70) + + last_actions = np.zeros(NUM_ACTIONS, dtype=np.float32) + step = 0 + dt = 1.0 / max(1.0, args.rate_hz) + next_t = time.perf_counter() + + while not EXIT: + new_state = client.recv_latest() + if new_state is not None: + state = new_state + if state is None: + time.sleep(0.001) + continue + + # Get commands from RC or fixed + r = state.remote + commands = np.array([r.ly * args.rc_vx_scale, + -r.lx * args.rc_vy_scale, + -r.rx * args.rc_wz_scale], dtype=np.float32) + + # Compute observation + obs = compute_obs(state.imu, state.motorState, commands, last_actions) + + dof_pos = np.array([state.motorState[i].q for i in range(12)]) + dof_vel = np.array([state.motorState[i].dq for i in range(12)]) + gx, gy, gz = state.imu.gyroscope + grav = get_projected_gravity(state.imu.quaternion) + + # Detailed print + print(f"\n{'='*70}") + print(f"[STEP {step}] bat={state.bms.SOC}% {fmt_rc(state)}") + print(f"{'='*70}") + print(f" obs[ 0: 3] gyro*0.25 = {np.round(obs[0:3], 4)}") + print(f" raw gyro (rad/s) = [{gx:+.4f}, {gy:+.4f}, {gz:+.4f}]") + print(f" obs[ 3: 6] proj gravity = {np.round(obs[3:6], 4)}") + print(f" |gravity| = {np.linalg.norm(grav):.4f} (expect ~1.0)") + print(f" obs[ 6:18] dof_pos_rel = {np.round(obs[6:18], 3)}") + diff_from_default = dof_pos - DEFAULT_ANGLES + print(f" |diff| max = {np.max(np.abs(diff_from_default)):.4f}") + print(f" actual pos = {np.round(dof_pos, 3)}") + print(f" default_angles = {np.round(DEFAULT_ANGLES, 3)}") + print(f" obs[18:30] dof_vel*0.05 = {np.round(obs[18:30], 4)}") + print(f" |dof_vel| max = {np.max(np.abs(dof_vel)):.4f}") + print(f" obs[30:42] last_actions = {np.round(obs[30:42], 4)}") + print(f" obs[42:45] commands*scale = {np.round(obs[42:45], 4)}") + print(f" raw cmd [vx,vy,wz]= {np.round(commands, 3)}") + print(f" obs min={obs.min():.3f} max={obs.max():.3f} mean={obs.mean():.3f}") + + # Warn if values look suspicious + if np.linalg.norm(grav) < 0.5 or np.linalg.norm(grav) > 1.5: + print(f" ⚠ WARN: gravity norm is off! Robot might not be upright.") + if np.max(np.abs(dof_pos)) < 1e-6: + print(f" ⚠ WARN: dof_pos is all zeros — state might be stale!") + + logger.log( + step, + mode="obs_check", + obs=obs, + dof_pos=dof_pos, + dof_vel=dof_vel, + base_ang_vel=np.array([gx, gy, gz]), + projected_gravity=grav, + imu_rpy_deg=np.degrees(state.imu.rpy), + commands=commands, + rc_buttons=r.pressed, + battery_soc=state.bms.SOC, + ) + + step += 1 + + next_t += dt + sleep = next_t - time.perf_counter() + if sleep > 0: + time.sleep(sleep) + else: + next_t = time.perf_counter() + + finally: + if logger is not None: + logger.close() + client.close() + print("[INFO] Done.") + + +# ─── Monitor mode ─── +def run_monitor(args): + """Read and display RC + IMU + joint data. No motor commands sent.""" + print(""" +╔══════════════════════════════════════════════════════════════╗ +║ MONITOR MODE — no motor commands, RC test only ║ +║ ║ +║ Ensure: ║ +║ 1. Robot SUSPENDED (sling / foot stand) ║ +║ 2. Network connected to robot (192.168.123.x) ║ +║ 3. Use --kill-sport to auto-kill Pi sport processes ║ +║ (or manually: ssh pi@192.168.123.161, pkill -9 ...) ║ +╚══════════════════════════════════════════════════════════════╝ +""") + input("Press Enter to start monitoring...") + + if args.kill_sport: + kill_sport_processes(args.pi_host, args.pi_user) + + print("[INFO] Connecting to MCU...") + client = MCUClient() + logger = JsonlLogger(args.log_dir, args) + + try: + print("[INFO] Waking MCU (damping frames)...") + client.wake_mcu(n_frames=50, dt=0.01) + + state = client.recv_state(timeout=2.0) + if state is None: + print("[ERROR] No state received. Check connection and Pi processes.") + return 1 + + print(f"[INFO] Connected. Battery={state.bms.SOC}%") + print("[INFO] Monitoring RC + IMU + joint data. Ctrl+C to exit.\n") + + edge = RCEdgeDetector() + step = 0 + dt = 1.0 / max(1.0, args.rate_hz) + next_t = time.perf_counter() + + while not EXIT: + new_state = client.recv_latest() + if new_state is not None: + state = new_state + + if state is None: + time.sleep(0.001) + continue + + rising, falling = edge.update(state) + + rpy = np.degrees(state.imu.rpy) + r = state.remote + dof_pos = np.array([state.motorState[i].q for i in range(12)]) + dof_vel = np.array([state.motorState[i].dq for i in range(12)]) + + print(f"\n─── [step {step}] bat={state.bms.SOC}% ───") + print(f" IMU roll={rpy[0]:+.1f} pitch={rpy[1]:+.1f} yaw={rpy[2]:+.1f}") + print(f" RC {fmt_rc(state)}") + if rising: + print(f" RC ↑ RISING: {sorted(rising)}") + if falling: + print(f" RC ↓ FALLING: {sorted(falling)}") + print(f" JOINT {fmt_joints(state)}") + print(f" JOINTS: {' '.join(f'{n:6s}' for n in JOINT_NAMES)}") + + logger.log( + step, + mode="monitor", + dof_pos=dof_pos, + dof_vel=dof_vel, + base_ang_vel=np.array(state.imu.gyroscope), + projected_gravity=get_projected_gravity(state.imu.quaternion), + imu_rpy_deg=np.degrees(state.imu.rpy), + rc_lx=r.lx, rc_ly=r.ly, rc_rx=r.rx, rc_ry=r.ry, + rc_buttons=r.pressed, + rc_rising=list(rising), rc_falling=list(falling), + battery_soc=state.bms.SOC, + ) + + step += 1 + + next_t += dt + sleep = next_t - time.perf_counter() + if sleep > 0: + time.sleep(sleep) + else: + next_t = time.perf_counter() + + finally: + if logger is not None: + logger.close() + client.close() + print("[INFO] Done.") + + +# ─── Ramp to default pose ─── +def ramp_to_default(client, args, state): + """Slowly interpolate from current pose to DEFAULT_ANGLES (~2s). + + Position protect is DISABLED during ramp because the movement is inherently + large (e.g. knee from -2.79 → -1.80, a 1.0 rad sweep). Only joint hard + limits and torque limiting are active. + """ + print("[INFO] Ramping to default pose (~2s)...") + current = np.array([state.motorState[i].q for i in range(12)], dtype=np.float32) + error = current - DEFAULT_ANGLES + + if np.max(np.abs(error)) < 0.05: + print("[INFO] Already near default pose.") + return state + + print(f"[INFO] Max error: {np.max(np.abs(error)):.2f} rad") + ramp_steps = 200 + step_err = error / ramp_steps + + for i in range(ramp_steps): + if EXIT: + return state + new_state = client.recv_latest() + if new_state is not None: + state = new_state + + targets = DEFAULT_ANGLES + (error - step_err * min(i + 1, ramp_steps)) + + cmd = LowCmd() + for j in range(12): + cmd.set_motor(j, MotorCmd( + mode=MotorMode.Servo, + q=float(targets[j]), dq=0.0, tau=0.0, + Kp=args.kp_cal, Kd=args.kd_cal, + )) + # Ramp: only joint limits + torque limit, NO position_protect + # (the movement is inherently >0.5 rad for many joints) + apply_safety(cmd, state, power_factor=args.power_factor, + position_limit_on=True, position_protect_limit=None) + client.send(cmd) + time.sleep(0.01) + + if i % 50 == 0: + actual = np.array([state.motorState[j].q for j in range(12)]) + print(f" ramp {i}/{ramp_steps} " + f"target_err={np.max(np.abs(targets - DEFAULT_ANGLES)):.3f} " + f"actual_err={np.max(np.abs(actual - DEFAULT_ANGLES)):.3f}") + + # Hold at default briefly (position_protect disabled — ramp continuation) + for _ in range(50): + if EXIT: + return state + new_state = client.recv_latest() + if new_state is not None: + state = new_state + + cmd = LowCmd() + for j in range(12): + cmd.set_motor(j, MotorCmd( + mode=MotorMode.Servo, + q=float(DEFAULT_ANGLES[j]), dq=0.0, tau=0.0, + Kp=args.kp, Kd=args.kd, + )) + apply_safety(cmd, state, power_factor=args.power_factor, + position_limit_on=True, position_protect_limit=None) + client.send(cmd) + time.sleep(0.01) + + print("[INFO] Default pose reached.") + return state + + +def send_hold_cmd(client, state, args): + """Send servo commands holding DEFAULT_ANGLES. + Position protect is disabled — hold is a fixed safe pose. + """ + cmd = LowCmd() + for j in range(12): + cmd.set_motor(j, MotorCmd( + mode=MotorMode.Servo, + q=float(DEFAULT_ANGLES[j]), dq=0.0, tau=0.0, + Kp=args.kp, Kd=args.kd, + )) + apply_safety(cmd, state, power_factor=args.power_factor, + position_limit_on=True, position_protect_limit=None) + client.send(cmd) + + +def send_rl_cmd(client, state, targets, args): + """Send servo commands for RL policy targets.""" + cmd = LowCmd() + for j in range(12): + cmd.set_motor(j, MotorCmd( + mode=MotorMode.Servo, + q=float(targets[j]), dq=0.0, tau=0.0, + Kp=args.kp, Kd=args.kd, + )) + pp_limit = args.position_protect_limit if args.position_protect_limit > 0 else None + apply_safety(cmd, state, power_factor=args.power_factor, + position_limit_on=True, position_protect_limit=pp_limit) + client.send(cmd) + + +# ─── Startup instructions ─── +STARTUP_BANNER = """ +╔══════════════════════════════════════════════════════════════╗ +║ Ensure: ║ +║ 1. Robot SUSPENDED (sling / foot stand) ║ +║ 2. Network connected to robot (192.168.123.x) ║ +║ 3. Use --kill-sport to auto-kill Pi sport processes ║ +║ (or manually: ssh pi@192.168.123.161, pkill -9 ...) ║ +║ ║ +║ Controls: R2=go/stop, L2=emergency-stop(→IDLE) ║ +║ Left-stick=move, Right-stick=turn ║ +╚══════════════════════════════════════════════════════════════╝ +""" + + +# ─── State machine deploy mode ─── +def run_deploy(args): + print(STARTUP_BANNER) + input("Press Enter when ready...") + + if args.kill_sport: + kill_sport_processes(args.pi_host, args.pi_user) + + print("[INFO] Connecting to MCU...") + client = MCUClient() + logger = None + + try: + print("[INFO] Waking MCU...") + client.wake_mcu(n_frames=50, dt=0.01) + + state = client.recv_state(timeout=2.0) + if state is None: + print("[ERROR] No state received. Check connection and Pi processes.") + return 1 + + print(f"[INFO] Connected. Battery={state.bms.SOC}%") + print(f"[INFO] RPY: {np.round(np.degrees(state.imu.rpy), 1)} deg") + print("[INFO] Initial joint positions (rad):") + for i in range(12): + print(f" {JOINT_NAMES[i]:6s}: {state.motorState[i].q:+7.3f}") + + # Load ONNX + policy = OnnxPolicy(args.onnx) + + # Logger + logger = JsonlLogger(args.log_dir, args) + logger.flush_every = max(1, int(args.log_flush_every)) + + # State machine + sm_state = State.IDLE + last_actions = np.zeros(NUM_ACTIONS, dtype=np.float32) + ema_filter = type('EMA', (), {'state': np.zeros(NUM_ACTIONS, dtype=np.float32)})() + step = 0 + dt = 1.0 / args.rate_hz + next_t = time.perf_counter() + edge = RCEdgeDetector() + + # Seed edge detector with initial state + edge.update(state) + + print(f"\n[INFO] State machine: IDLE → (R2) → CALIBRATE → HOLD → (R2) → RL") + print(f"[INFO] Rate: {args.rate_hz}Hz. Ctrl+C to exit.\n") + + while not EXIT: + new_state = client.recv_latest() + if new_state is not None: + state = new_state + + if state is None: + time.sleep(0.001) + continue + + # Rising edge detection (single update per tick) + rising, falling = edge.update(state) + r2_rose = "R2" in rising + l2_rose = "L2" in rising + r = state.remote + + # ── Emergency stop: L2 → IDLE (from any state) ── + if l2_rose and sm_state != State.IDLE: + print(f"\n[L2 EMERGENCY] {sm_state.value} → IDLE") + sm_state = State.IDLE + last_actions[:] = 0.0 + # Send damping + client.send(LowCmd()) # all damping by default + + # ── State machine ── + if sm_state == State.IDLE: + # Full damping, no torques. Keep stream alive with damping frames. + if step % 10 == 0: + client.send(LowCmd()) # all damping + + if r2_rose: + print("\n[R2] IDLE → CALIBRATE") + sm_state = State.CALIBRATE + state = ramp_to_default(client, args, state) + if EXIT: + break + sm_state = State.HOLD + print(f"[STATE] → HOLD (waiting for R2 to start RL)") + + elif sm_state == State.HOLD: + send_hold_cmd(client, state, args) + + if r2_rose: + print("\n[R2] HOLD → RL") + sm_state = State.RL + last_actions[:] = 0.0 + ema_filter.state[:] = 0.0 + + elif sm_state == State.RL: + if r2_rose: + print("\n[R2] RL → HOLD") + sm_state = State.HOLD + last_actions[:] = 0.0 + # Ramp back to default + state = ramp_to_default(client, args, state) + if EXIT: + break + continue + + # Velocity commands from RC + commands = get_rc_commands(state, args) + + # Observation + inference + obs = compute_obs(state.imu, state.motorState, commands, last_actions) + + ok, reason = (True, "ok") + if not args.no_state_check: + ok, reason = state_ok(state) + + if ok: + action_raw = policy(obs) + action_clipped = np.clip(action_raw, -CLIP_ACTIONS, CLIP_ACTIONS) + # EMA smooth to suppress policy's inherent high-freq oscillation + alpha = args.action_ema_alpha + if 0 < alpha < 1 and hasattr(ema_filter, 'state'): + ema_filter.state = alpha * action_clipped + (1 - alpha) * ema_filter.state + action_smoothed = ema_filter.state.copy() + else: + action_smoothed = action_clipped + last_actions = action_smoothed.copy() + targets = DEFAULT_ANGLES + action_smoothed * ACTION_SCALE + else: + # State check failed: hold default pose + targets = DEFAULT_ANGLES.copy() + + if step >= args.warmup_steps: + send_rl_cmd(client, state, targets, args) + + # Log RL data + logger.log( + step, + mode=sm_state.value, + commands=commands, + obs_isaac=obs, + action_raw=action_raw if ok else np.zeros(NUM_ACTIONS), + action_safe=action_clipped if ok else np.zeros(NUM_ACTIONS), + joint_targets=targets, + dof_pos=np.array([state.motorState[i].q for i in range(12)]), + dof_vel=np.array([state.motorState[i].dq for i in range(12)]), + base_ang_vel=np.array(state.imu.gyroscope), + projected_gravity=get_projected_gravity(state.imu.quaternion), + imu_rpy_deg=np.degrees(state.imu.rpy), + rc_lx=state.remote.lx, rc_ly=state.remote.ly, + rc_rx=state.remote.rx, rc_ry=state.remote.ry, + rc_buttons=state.remote.pressed, + state_ok=ok, state_reason=reason, + ) + else: + # Log non-RL states (minimal) + logger.log( + step, + mode=sm_state.value, + dof_pos=np.array([state.motorState[i].q for i in range(12)]), + imu_rpy_deg=np.degrees(state.imu.rpy), + rc_buttons=state.remote.pressed, + ) + + # ── Status print ── + if step % args.print_every == 0: + rpy = np.degrees(state.imu.rpy) + dof_pos = np.array([state.motorState[i].q for i in range(12)]) + print(f"\n[STEP {step}] state={sm_state.value} bat={state.bms.SOC}% " + f"rpy=[{rpy[0]:.0f},{rpy[1]:.0f},{rpy[2]:.0f}]") + print(f" RC: {fmt_rc(state)}") + print(f" joint: {np.round(dof_pos, 2)}") + if sm_state == State.RL and 'targets' in dir(): + print(f" target:{np.round(targets, 2)}") + + step += 1 + if args.max_steps > 0 and step >= args.max_steps: + print("[INFO] max_steps reached.") + break + + # Rate limit + next_t += dt + sleep = next_t - time.perf_counter() + if sleep > 0: + time.sleep(sleep) + else: + next_t = time.perf_counter() + + finally: + if logger is not None: + logger.close() + print("[INFO] Safe stopping...") + client.safe_stop(n_frames=50, dt=0.002) + client.close() + print("[INFO] Done.") + + +# ─── Auto-kill Pi sport processes ─── +def kill_sport_processes(host, user): + """SSH into the Pi and kill sport-mode processes.""" + cmds = [ + "sudo pkill -9 -f keep_sport_alive", + "sudo pkill -9 -f Legged_sport", + "sudo pkill -9 -f appTransit", + ] + ssh_target = f"{user}@{host}" if user else host + full_cmd = " && ".join(cmds) + + print(f"[INFO] Killing sport processes on {ssh_target}...") + try: + result = subprocess.run( + ["ssh", ssh_target, full_cmd], + capture_output=True, text=True, timeout=15 + ) + if result.returncode == 0: + print("[INFO] Sport processes killed successfully.") + return True + else: + stderr = result.stderr.strip() + # pkill returns non-zero if no matching process — that's also OK + if "no process" in stderr.lower() or not stderr: + print("[INFO] No sport processes found (already killed).") + return True + print(f"[WARN] SSH returned {result.returncode}: {stderr}") + return False + except FileNotFoundError: + print("[WARN] 'ssh' command not found. Please kill processes manually.") + return False + except subprocess.TimeoutExpired: + print("[WARN] SSH timed out. Check network connection to Pi.") + return False + except Exception as e: + print(f"[WARN] Failed to kill sport processes: {e}") + return False + + +# ─── CLI ─── +def main(): + parser = argparse.ArgumentParser(description="Deploy ONNX policy on Go1 PRO (no official SDK)") + parser.add_argument("--onnx", default="policy.onnx", help="Path to ONNX model") + + # Pi control + parser.add_argument("--kill-sport", action="store_true", + help="Auto-kill sport processes on Pi via SSH before starting") + parser.add_argument("--pi-host", default="192.168.123.161", help="Pi IP/hostname") + parser.add_argument("--pi-user", default="pi", help="Pi SSH user") + + # Modes + parser.add_argument("--obs-check", action="store_true", + help="Observation check mode: compute 45-dim obs, no inference, no motors") + parser.add_argument("--monitor", action="store_true", + help="Monitor mode: read RC/IMU/joint data only, no motors") + parser.add_argument("--no-rc", action="store_true", + help="Disable RC — use fixed --cmd-* velocities instead") + parser.add_argument("--no-sm", action="store_true", + help="Disable state machine — run RL continuously") + parser.add_argument("--dry-run", action="store_true", + help="Inference only, no motor commands") + + # Gains + parser.add_argument("--kp", type=float, default=30.0) + parser.add_argument("--kd", type=float, default=0.5) + parser.add_argument("--kp-cal", type=float, default=20.0) + parser.add_argument("--kd-cal", type=float, default=1.0) + parser.add_argument("--power-factor", type=int, default=7, + help="Torque limit factor 1-10. tau_lim = TAU_MAX * factor/10. hip: %.1f, thigh: %.1f, knee: %.1f Nm at factor=7" % (23.7*7/10, 23.7*7/10, 35.55*7/10)) + + # RC scale + parser.add_argument("--rc-vx-scale", type=float, default=1.0) + parser.add_argument("--rc-vy-scale", type=float, default=1.0) + parser.add_argument("--rc-wz-scale", type=float, default=1.0) + + # Fixed commands (when --no-rc) + parser.add_argument("--cmd-x", type=float, default=0.0) + parser.add_argument("--cmd-y", type=float, default=0.0) + parser.add_argument("--cmd-yaw", type=float, default=0.0) + + # Control + parser.add_argument("--rate-hz", type=float, default=100.0) + parser.add_argument("--warmup-steps", type=int, default=50) + parser.add_argument("--max-steps", type=int, default=0) + parser.add_argument("--print-every", type=int, default=50) + + # Safety + parser.add_argument("--no-state-check", action="store_true") + parser.add_argument("--position-protect-limit", type=float, default=1.0, + help="Max |target-actual| before zeroing Kp/Kd (negative=disable)") + parser.add_argument("--action-ema-alpha", type=float, default=0.3, + help="EMA smoothing factor for actions (0=no smooth, 1=no filter, default 0.3)") + + # Logging + parser.add_argument("--log-dir", default="", help="Enable JSONL logging to this directory") + parser.add_argument("--log-flush-every", type=int, default=50) + + args = parser.parse_args() + + if args.obs_check: + return run_obs_check(args) + if args.monitor: + return run_monitor(args) + else: + return run_deploy(args) + + +if __name__ == "__main__": + main() diff --git a/sim2sim_mujoco_example b/sim2sim_mujoco_example new file mode 160000 index 0000000..eaee63a --- /dev/null +++ b/sim2sim_mujoco_example @@ -0,0 +1 @@ +Subproject commit eaee63a9e8d70bf82f391e45c68ec983d79cb282 diff --git a/sim2sim_test_deploy.py b/sim2sim_test_deploy.py new file mode 100644 index 0000000..b684395 --- /dev/null +++ b/sim2sim_test_deploy.py @@ -0,0 +1,414 @@ +#!/usr/bin/env python3 +""" +sim2sim test for deploy_onnx_pro_sdk.py using sim2sim_mujoco_example's Go1 XML. + +Mirrors the deploy safety layer (joint limits, torque factor, position deviation protect) +and logs all data to JSONL when --log-dir is set. + +Usage: + conda activate free_dog_sdk + mjpython sim2sim_test_deploy.py --onnx policy.onnx + mjpython sim2sim_test_deploy.py --onnx policy.onnx --log-dir logs + +Controls: W/S=forward/back, Q/E=strafe, A/D=rotate, Space=stop, R=reset, Esc=quit +""" + +import argparse +import json +import os +import signal +import time +from datetime import datetime +from pathlib import Path + +import mujoco +import numpy as np +import onnxruntime as ort +from mujoco import viewer + +HERE = os.path.dirname(os.path.abspath(__file__)) +SIM2SIM_XML = os.path.join(HERE, "sim2sim_mujoco_example", "data", "go1", "xml", "go1.xml") + +# ─── Policy constants (matches go1_sim2sim.py + deploy_onnx_pro_sdk.py) ─── +NUM_OBS = 45 +NUM_ACTIONS = 12 +ACTION_SCALE = 0.05 +CLIP_ACTIONS = 23.7 +CLIP_OBS = 100.0 +KP_DEFAULT = 80.0 +KD_DEFAULT = 0.5 # + joint_damping(0.5) = 1.0 total + +DEFAULT_ANGLES = np.array([ + -0.0, 0.9, -1.8, # FR + 0.0, 0.9, -1.8, # FL + -0.0, 0.9, -1.8, # RR + 0.0, 0.9, -1.8, # RL +], dtype=np.float32) + +JOINT_NAMES = [ + "FR_hip", "FR_thigh", "FR_calf", + "FL_hip", "FL_thigh", "FL_calf", + "RR_hip", "RR_thigh", "RR_calf", + "RL_hip", "RL_thigh", "RL_calf", +] + +# ─── Safety limits (mirrors deploy go1_pro_sdk safety layer) ─── +JOINT_TYPE = [("hip" if i % 3 == 0 else "thigh" if i % 3 == 1 else "knee") for i in range(12)] + +JOINT_LIMITS = { + "hip": (-0.78, 0.78), + "thigh": (-0.60, 3.50), + "knee": (-2.70, -0.95), +} + +TAU_MAX = { + "hip": 23.7, + "thigh": 23.7, + "knee": 35.55, +} + +EXIT = False + + +def _sig_handler(signum, frame): + global EXIT + EXIT = True + + +signal.signal(signal.SIGINT, _sig_handler) +signal.signal(signal.SIGTERM, _sig_handler) + + +# ─── Safety functions (mirror deploy apply_safety) ─── +def clip_targets_to_limits(targets): + """PositionLimit: clamp target joint angles to JOINT_LIMITS.""" + n_clamped = 0 + safe = targets.copy() + for i in range(12): + lo, hi = JOINT_LIMITS[JOINT_TYPE[i]] + if safe[i] < lo: + safe[i] = lo; n_clamped += 1 + elif safe[i] > hi: + safe[i] = hi; n_clamped += 1 + return safe, n_clamped + + +def clip_torques(torques, power_factor): + """PowerProtect: clamp torque to TAU_MAX * power_factor / 10.""" + tau_lim = np.array([TAU_MAX[JOINT_TYPE[i]] * power_factor / 10.0 for i in range(12)], + dtype=np.float64) + return np.clip(torques, -tau_lim, tau_lim) + + +def position_protect_mask(targets, current_pos, limit_rad): + """PositionProtect: return bool mask, True where deviation <= limit.""" + return np.abs(targets - current_pos) <= limit_rad + + +# ─── Quaternion math (same as deploy_onnx_pro_sdk.py) ─── +def quat_to_rot_matrix(q): + w, x, y, z = q + return np.array([ + [1 - 2*y*y - 2*z*z, 2*x*y - 2*w*z, 2*x*z + 2*w*y], + [ 2*x*y + 2*w*z, 1 - 2*x*x - 2*z*z, 2*y*z - 2*w*x], + [ 2*x*z - 2*w*y, 2*y*z + 2*w*x, 1 - 2*x*x - 2*y*y], + ], dtype=np.float32) + + +# ─── Observations (same as deploy_onnx_pro_sdk.py) ─── +def compute_obs(model, data, commands, last_actions): + obs = np.zeros(NUM_OBS, dtype=np.float32) + + sid = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SENSOR, "Body_Gyro") + adr = model.sensor_adr[sid] + obs[0:3] = data.sensordata[adr:adr + 3] * 0.25 + + sid = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SENSOR, "Body_Quat") + adr = model.sensor_adr[sid] + quat = data.sensordata[adr:adr + 4] + R = quat_to_rot_matrix(quat) + obs[3:6] = (R.T @ np.array([0., 0., -1.], dtype=np.float64)).astype(np.float32) + + dof_pos = np.zeros(12, dtype=np.float32) + dof_vel = np.zeros(12, dtype=np.float32) + for i, name in enumerate(JOINT_NAMES): + sid = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SENSOR, f"{name}_pos") + dof_pos[i] = data.sensordata[model.sensor_adr[sid]] + sid = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SENSOR, f"{name}_vel") + dof_vel[i] = data.sensordata[model.sensor_adr[sid]] + + obs[6:18] = (dof_pos - DEFAULT_ANGLES) * 1.0 + obs[18:30] = dof_vel * 0.05 + obs[30:42] = last_actions + obs[42:45] = np.array(commands, dtype=np.float32) * np.array([2.0, 2.0, 0.25], dtype=np.float32) + + obs = np.clip(obs, -CLIP_OBS, CLIP_OBS) + obs = np.nan_to_num(obs, nan=0.0, posinf=0.0, neginf=0.0) + return obs + + +# ─── JSONL logger ─── +class JsonlLogger: + def __init__(self, log_dir, args): + self.enabled = bool(log_dir) + self.fp = None + self.run_dir = None + self.flush_every = 50 + if not self.enabled: + return + ts = datetime.now().strftime("%Y%m%d_%H%M%S") + self.run_dir = Path(log_dir).expanduser().resolve() / f"sim2sim_run_{ts}" + self.run_dir.mkdir(parents=True, exist_ok=True) + meta = { + "created_at": ts, + "args": vars(args), + "num_obs": NUM_OBS, + "num_actions": NUM_ACTIONS, + "action_scale": ACTION_SCALE, + "default_angles": DEFAULT_ANGLES.tolist(), + "joint_names": JOINT_NAMES, + "joint_limits": {k: list(v) for k, v in JOINT_LIMITS.items()}, + "tau_max": TAU_MAX, + } + (self.run_dir / "metadata.json").write_text(json.dumps(meta, indent=2, ensure_ascii=False)) + self.fp = open(self.run_dir / "steps.jsonl", "a", encoding="utf-8") + print(f"[INFO] Log dir: {self.run_dir}") + + def log(self, step, **kw): + if not self.enabled: + return + rec = {"step": int(step), "time_wall": time.time()} + for k, v in kw.items(): + if isinstance(v, np.ndarray): + rec[k] = np.asarray(v, dtype=np.float32).reshape(-1).tolist() + elif isinstance(v, (np.float32, np.float64)): + rec[k] = float(v) + elif isinstance(v, (np.int32, np.int64)): + rec[k] = int(v) + else: + rec[k] = v + self.fp.write(json.dumps(rec, ensure_ascii=False) + "\n") + if step % self.flush_every == 0: + self.fp.flush() + + def close(self): + if self.fp: + self.fp.flush(); self.fp.close() + print(f"[INFO] Log saved: {self.run_dir}") + + +# ─── Keyboard input ─── +class Keyboard: + def __init__(self): + self.held = set() + + def _on_press(self, key): + try: self.held.add(key.char.lower()) + except AttributeError: self.held.add(str(key)) + + def _on_release(self, key): + try: self.held.discard(key.char.lower()) + except AttributeError: self.held.discard(str(key)) + + def init(self): + from pynput import keyboard + self._listener = keyboard.Listener( + on_press=self._on_press, on_release=self._on_release) + self._listener.start() + + def keys(self): + return self.held.copy() + + def stop(self): + if self._listener: + self._listener.stop() + + +# ─── Main ─── +def main(): + parser = argparse.ArgumentParser(description="Sim2sim test for deploy_onnx_pro_sdk.py") + parser.add_argument("--onnx", default=os.path.join(HERE, "policy.onnx")) + + # PD gains + parser.add_argument("--kp", type=float, default=KP_DEFAULT) + parser.add_argument("--kd", type=float, default=KD_DEFAULT) + + # Safety (mirrors deploy args) + parser.add_argument("--power-factor", type=int, default=7, + help="Torque limit factor 1-10, applied as TAU_MAX * factor/10") + parser.add_argument("--position-protect-limit", type=float, default=0.5, + help="Max |target - actual| before zeroing torque (negative=disable)") + parser.add_argument("--no-joint-limit", action="store_true", + help="Disable joint limit clipping on targets") + + # Logging + parser.add_argument("--log-dir", default="", help="Enable JSONL logging to this directory") + parser.add_argument("--print-every", type=int, default=200) + + args = parser.parse_args() + + if not os.path.exists(SIM2SIM_XML): + print(f"[ERROR] XML not found: {SIM2SIM_XML}"); return 1 + if not os.path.exists(args.onnx): + print(f"[ERROR] ONNX not found: {args.onnx}"); return 1 + + print(f"[INFO] XML: {SIM2SIM_XML}") + print(f"[INFO] ONNX: {args.onnx}") + + model = mujoco.MjModel.from_xml_path(SIM2SIM_XML) + data = mujoco.MjData(model) + model.dof_damping[6:] = 0.5 + + total_kd = args.kd + model.dof_damping[6] + print(f"[INFO] Bodies={model.nbody}, DoF={model.nq}, Actuators={model.nu}") + print(f"[INFO] Timestep={model.opt.timestep}") + print(f"[INFO] KP={args.kp}, KD(active)={args.kd}, KD(passive)={model.dof_damping[6]}, " + f"total_KD={total_kd}") + print(f"[INFO] Safety: power_factor={args.power_factor}, " + f"position_protect={args.position_protect_limit}, " + f"joint_limit={not args.no_joint_limit}") + print(f"[INFO] Torque limits (factor={args.power_factor}): " + + ", ".join(f"{jt}={TAU_MAX[jt]*args.power_factor/10:.1f}" for jt in ["hip","thigh","knee"])) + + # Init pose + data.qpos[0:3] = [0.0, 0.0, 0.42] + data.qpos[3:7] = [1.0, 0.0, 0.0, 0.0] + data.qpos[7:19] = DEFAULT_ANGLES + data.qvel[:] = 0.0 + mujoco.mj_forward(model, data) + + # Load ONNX + session = ort.InferenceSession(args.onnx, providers=["CPUExecutionProvider"]) + input_name = session.get_inputs()[0].name + print(f"[INFO] ONNX input={input_name}, shape={session.get_inputs()[0].shape}") + print(f"[INFO] Controls: W/S=前后 Q/E=左右 A/D=旋转 Space=停 R=重置 Esc=退出") + + logger = JsonlLogger(args.log_dir, args) + + kb = Keyboard(); kb.init() + + view = viewer.launch_passive(model, data) + step = 0 + ctrl_dt = 0.01 + steps_per_inference = int(ctrl_dt / model.opt.timestep) + last_actions = np.zeros(NUM_ACTIONS, dtype=np.float32) + action = np.zeros(NUM_ACTIONS, dtype=np.float32) + safety_stats = {"joint_limit_clamps": 0, "position_protect_hits": 0} + + t0 = time.perf_counter() + + while view.is_running() and not EXIT: + t_loop = time.perf_counter() + + keys = kb.keys() + if 'key.esc' in keys or '\x1b' in keys: + break + + if 'r' in keys: + data.qpos[0:3] = [0.0, 0.0, 0.42] + data.qpos[3:7] = [1.0, 0.0, 0.0, 0.0] + data.qpos[7:19] = DEFAULT_ANGLES + data.qvel[:] = 0.0 + last_actions[:] = 0.0 + action[:] = 0.0 + mujoco.mj_forward(model, data) + + vx = 1.0 if 'w' in keys else (-1.0 if 's' in keys else 0.0) + vy = 1.0 if 'q' in keys else (-1.0 if 'e' in keys else 0.0) + wz = 0.5 if 'a' in keys else (-0.5 if 'd' in keys else 0.0) + if ' ' in keys: + vx = vy = wz = 0.0 + + commands = np.array([vx, vy, wz], dtype=np.float32) + + # Inference at 100 Hz + if step % steps_per_inference == 0: + obs = compute_obs(model, data, commands, last_actions) + action = session.run(None, {input_name: obs.reshape(1, -1).astype(np.float32)})[0][0] + action = np.clip(action, -CLIP_ACTIONS, CLIP_ACTIONS) + last_actions = action.copy() + + # Targets (pre-safety) + targets_raw = DEFAULT_ANGLES + action * ACTION_SCALE + + # ── Safety layer (mirrors deploy apply_safety) ── + # 1. Joint limit clipping + if not args.no_joint_limit: + targets, n_clamped = clip_targets_to_limits(targets_raw) + safety_stats["joint_limit_clamps"] += n_clamped + else: + targets = targets_raw + + current_pos = data.qpos[7:19] + current_vel = data.qvel[6:18] + + # 2. Position deviation protection (zero torque where |target - actual| > limit) + pos_ok = np.ones(12, dtype=bool) + if args.position_protect_limit > 0: + pos_ok = position_protect_mask(targets, current_pos, args.position_protect_limit) + n_hit = np.sum(~pos_ok) + safety_stats["position_protect_hits"] += n_hit + + # 3. PD control + torques = np.zeros(12, dtype=np.float64) + torques[pos_ok] = (args.kp * (targets[pos_ok] - current_pos[pos_ok]) + - args.kd * current_vel[pos_ok]) + + # 4. Torque limiting (power_protect) + torques = clip_torques(torques, args.power_factor) + + data.ctrl[:] = torques + + mujoco.mj_step(model, data) + view.sync() + + # Logging + logger.log( + step, + mode="rl", + loop_ms=(time.perf_counter() - t_loop) * 1000.0, + commands=commands, + obs=obs if step % steps_per_inference == 0 else np.zeros(0), + action_raw=action, + action_safe=action, + joint_targets_raw=targets_raw, + joint_targets_safe=targets, + dof_pos=current_pos, + dof_vel=current_vel, + torques=torques, + position_protect_hit_mask=(~pos_ok).astype(int), + ) + + step += 1 + + if step % args.print_every == 0: + z = data.qpos[2] + sid = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SENSOR, "Body_Quat") + quat = data.sensordata[model.sensor_adr[sid]:model.sensor_adr[sid] + 4] + pos_err = np.max(np.abs(targets - current_pos)) + n_pp = safety_stats["position_protect_hits"] + n_jl = safety_stats["joint_limit_clamps"] + print(f"\n[STEP {step}] z={z:.3f} cmd=[{vx:.1f},{vy:.1f},{wz:.1f}] " + f"max_err={pos_err:.3f}") + print(f" quat={np.round(quat, 3)} action_max={np.max(np.abs(action)):.2f}") + print(f" target: {np.round(targets[:4], 2)}") + print(f" actual: {np.round(current_pos[:4], 2)}") + print(f" torque: {np.round(torques[:4], 2)}") + if n_pp > 0 or n_jl > 0: + print(f" safety: pos_protect_hits={n_pp} joint_limit_clamps={n_jl}") + + # Real-time sync + expected = (step + 1) * model.opt.timestep + elapsed = time.perf_counter() - t0 + sleep = expected - elapsed + if sleep > 0: + time.sleep(sleep) + + logger.close() + kb.stop() + view.close() + print("[INFO] Done.") + + +if __name__ == "__main__": + main() diff --git a/test_deploy.md b/test_deploy.md new file mode 100644 index 0000000..7190fb0 --- /dev/null +++ b/test_deploy.md @@ -0,0 +1,138 @@ +# Go1 PRO ONNX 策略部署测试 + +## 环境准备 + +```bash +conda activate free_dog_sdk +cd /Users/chenyouyuan/cyy_ws/deploy_go1_pro +``` + +--- + +## 推荐部署命令 + +```bash +python deploy_onnx_pro_sdk.py --onnx policy.onnx --kill-sport --log-dir logs \ + --kp 80 --kd 1 --kp-cal 20 --kd-cal 1 \ + --power-factor 7 --position-protect-limit 1.0 \ + --action-ema-alpha 0.3 --print-every 10 +``` + +--- + +## 测试流程 + +### Step 0 — 观测数据查验 ✓ + +```bash +python deploy_onnx_pro_sdk.py --obs-check --kill-sport --log-dir logs +``` + +| obs 段 | 含义 | 期望 | 结果 | +|---|---|---|---| +| `[0:3]` | gyro×0.25 | ~0 | ✅ | +| `[3:6]` | 投影重力 | ~[0,0,-1] | ✅ | +| `[6:18]` | 关节偏差 | ~0（站姿时） | ⚠️ 蹲姿，正常 | +| `[18:30]` | 关节速度 | ~0 | ✅ | +| `[30:42]` | 上步动作 | [0] | ✅ | +| `[42:45]` | 指令×scale | ~0 | ✅ | + +结论：关节初始为阻尼蹲姿（膝盖 ~-2.79rad），ramp 渐变到站姿。 + +### Step 1 — 遥控器测试 ✓ + +```bash +python deploy_onnx_pro_sdk.py --monitor --kill-sport --log-dir logs +``` + +全部按钮（R2/L2/L1/A/B/X/Y/START）上升沿/下降沿检测正常，摇杆四轴 ±1.0。 + +### Step 2 — 起身 ✓ + +R2 → CALIBRATE（2s 渐变）→ HOLD（伺服站姿）。 + +### Step 3 — 急停 ✓ + +L2 → 立即 IDLE（全阻尼），恢复正常。 + +### Step 4 — RL 联调 ✓ + +R2 → RL，摇杆控制速度。 + +--- + +## 参数调优记录 + +### 最终工作参数 + +| 参数 | 值 | 说明 | +|---|---|---| +| `--kp` | **80** | 与 sim 一致，直连 MCU | +| `--kd` | **1** | sim Kd=0.5+被动0.5=1.0 | +| `--kp-cal` | 20 | ramp 阶段 Kp | +| `--kd-cal` | 1.0 | ramp 阶段 Kd | +| `--power-factor` | 7 | hip:16.6, thigh:16.6, knee:24.9 Nm | +| `--position-protect-limit` | 1.0 | RL 阶段位置偏差保护 | +| `--action-ema-alpha` | **0.3** | 平滑 action 高频换向 | + +### 调参过程 + +| 阶段 | 问题 | 原因 | 解决 | +|---|---|---|---| +| 起身 | 力矩被掐断 | `position_protect=0.5` 膝盖差 1.0rad 触发 | ramp/HOLD 阶段禁用 position_protect | +| 起身 | 力不够 | `Kp=5 / power_factor=3` 太保守 | Kp→20-30, power_factor→7 | +| RL | 高频抖动 | policy 本就会每2-3步换向（sim 和 real 一致），Kd=0.3 刹不住 | Kd→1, 加 EMA 滤波 | +| RL | 关节偏软 | Kp 不够大 | Kp→80 与 sim 对齐 | + +### Safety 策略分层 + +| 阶段 | position_protect | 原因 | +|---|---|---| +| Ramp（起身） | ❌ 禁用 | 大范围移动（膝盖 ~1.0rad） | +| HOLD（站姿） | ❌ 禁用 | 维持固定站姿，本身安全 | +| RL（行走） | ✅ limit=1.0rad | 防策略突跳 | + +### EMA 滤波 + +Policy 输出的 action 在 sim 和真机上方向切换率一致（hips ~40%, calves ~25%），这是模型本身特性。sim 中 Kp=80 硬吃掉了，真机需要用 EMA 平滑： + +- `--action-ema-alpha 0.3`：`new = 0.3×raw + 0.7×prev`，时间常数 ~30ms +- 0.2：更强平滑但响应慢 +- 1.0：不过滤 + +### Kd 选择依据 + +| 来源 | Kp | Kd | Kd/Kp | +|---|---|---|---| +| sim（含被动阻尼 0.5） | 80 | 1.0 (0.5+0.5) | 0.0125 | +| WTW LCM deploy | 20 | 0.5 | 0.025 | +| **最终直连 MCU** | **80** | **1** | **0.0125** | + +--- + +## 状态机 + +``` + ┌──────────────────────────────────────┐ + │ L2 (急停) │ + ▼ │ +IDLE ──R2──► CALIBRATE ──auto──► HOLD ──R2──► RL │ + ▲ ▲ │ │ + └──────────── L2(急停) ────────────┘ R2 ──┘ │ + └─────────────────────────────────────────────────────────┘ + +IDLE: 全阻尼，等待 R2 +CALIBRATE: 渐变到站姿（2s），自动→HOLD +HOLD: 伺服保持站姿，等待 R2 启动 RL +RL: 运行 ONNX 策略，摇杆控制 +``` + +## 遥控器 + +| 按键 | 功能 | +|---|---| +| R2 | 状态切换（IDLE→起身 / HOLD↔RL） | +| L2 | 急停（任意状态→IDLE） | +| 左杆 Y | 前进/后退 | +| 左杆 X | 平移 | +| 右杆 X | 旋转 |