feat: checkpoint Go2 FL simulation performance v0.4.2

Include callable skill and live-GUI prerequisites, epoch sensor reads, packed snapshots, sampled bit-exact kinematics, and verified-tick rendering without changing the physical/control schedule.

Record bounded analytic S1 GUI/headless qualification, original failures and CPU evidence (1557 passed, 6 skipped). Preserve accepted_skill=false and sub-real-time limitations. Exclude native authorities, caches, large media and local delivery copies; retain sealed artifact digest.
This commit is contained in:
2026-10-16 15:46:01 +08:00
parent 9fe9d27865
commit c528dd268f
136 changed files with 15553 additions and 15 deletions
+135
View File
@@ -0,0 +1,135 @@
#!/usr/bin/env python3
"""Offline evidence: measured-state FK drawings/animation, NOT Kit rendered video.
No simulator, trajectory interpolation, reset, state write, control or optimizer.
Raw 500Hz traces stay intact; only display frames are sampled at 10fps.
"""
import argparse
import importlib.util
import json
import sys
from pathlib import Path
import matplotlib
matplotlib.use('Agg')
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation, PillowWriter
import numpy as np
import torch
ROOT = Path(__file__).resolve().parents[1]
PHASES = ('support', 'unload', 'lift', 'swing', 'landing', 'reload')
ENDS = (0, 3, 4, 4.6, 6, 6.8, 9)
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument('--trace', type=Path, required=True)
parser.add_argument('--output', type=Path, required=True)
parser.add_argument('--label', required=True)
parser.add_argument('--gif', action='store_true')
args = parser.parse_args()
args.output.mkdir(parents=True, exist_ok=False)
spec = importlib.util.spec_from_file_location('SkillFactory', ROOT / 'source/SkillFactory/SkillFactory/__init__.py',
submodule_search_locations=[str(ROOT / 'source/SkillFactory/SkillFactory')])
sys.modules['SkillFactory'] = importlib.util.module_from_spec(spec)
from SkillFactory.legs.urdf import forward
bundle = json.loads((ROOT / 'assets/generated/legs/go2_fl/manifest.json').read_text())
d = {k: v[:, 0].numpy() for k, v in torch.load(args.trace, map_location='cpu', weights_only=True).items()}
t, foot, root = d['time'], d['foot'], d['root_state']
fn, gap = d['normal'][:, 2], d['gap']
error = d['policy_snapshot'][:, 32:35]
cycles = int(np.ceil(len(t) / 4500))
fig, axes = plt.subplots(4, 1, figsize=(11, 9), sharex=True)
axes[0].plot(t, fn); axes[0].set_ylabel('Actual Fn [N]')
axes[1].plot(t, gap * 1000); axes[1].set_ylabel('Gap [mm]')
for j, label in enumerate(('x', 'y', 'z')):
axes[2].plot(t, (foot[:, j] - foot[0, j]) * 1000, label=label)
axes[2].set_ylabel('Foot delta [mm]'); axes[2].legend(ncol=3)
axes[3].plot(t, np.linalg.norm(error[:, :2], axis=-1) * 1000)
axes[3].axhline(30, color='r', linestyle='--', label='30mm diagnostic only')
axes[3].set_ylabel('Root XY error [mm]'); axes[3].legend()
for ax in axes:
ax.grid(alpha=.25)
for c in range(cycles):
for p in range(6):
ax.axvspan(9*c + ENDS[p], min(t[-1], 9*c + ENDS[p+1]), color=f'C{p}', alpha=.07)
axes[-1].set_xlabel('Actual global simulation time [s]')
fig.suptitle(args.label + ' | measured 500Hz PhysX trace, no joined episodes')
fig.tight_layout(); fig.savefig(args.output / 'timeline.png', dpi=140); plt.close(fig)
def rotate(q, v):
return v + 2 * np.cross(q[:3], np.cross(q[:3], v) + q[3] * v)
max_fk_error = [0.]
def joints(i):
q = {name: float(value) for name, value in zip(bundle['joint_order'], d['joint_pos'][i])}
links, pivots = forward(bundle['tree'], q)
points = [root[i, :3]]
for joint in ('FL_thigh_joint', 'FL_calf_joint'):
local = np.array([pivots[joint][j][3] for j in range(3)])
points.append(root[i, :3] + rotate(root[i, 3:7], local))
matrix = np.array(links['FL_foot'])
fk_foot = root[i, :3] + rotate(root[i, 3:7], matrix[:3, :3] @ bundle['foot']['xyz'] + matrix[:3, 3])
max_fk_error[0] = max(max_fk_error[0], float(np.linalg.norm(fk_foot - foot[i])))
if max_fk_error[0] > .001:
raise ValueError('FK reconstruction disagrees with measured foot by >1mm')
points.append(foot[i]) # actual measured foot is always used
return np.array(points)
extent = np.concatenate((root[:, :3], foot), axis=0)
lo, hi = extent.min(0), extent.max(0)
center = (lo + hi) / 2
span = max(.4, float((hi - lo).max()) + .12)
def setup(ax):
ax.set_xlim(center[0]-span/2, center[0]+span/2)
ax.set_ylim(center[1]-span/2, center[1]+span/2)
ax.set_zlim(-.01, span)
ax.set_box_aspect((1, 1, 1)); ax.view_init(20, -55)
ax.set_xlabel('X m', fontsize=7); ax.set_ylabel('Y m', fontsize=7)
ax.tick_params(labelsize=6)
x = [center[0]-span/2, center[0]+span/2, center[0]+span/2, center[0]-span/2, center[0]-span/2]
y = [center[1]-span/2, center[1]-span/2, center[1]+span/2, center[1]+span/2, center[1]-span/2]
ax.plot(x, y, [0]*5, color='gray', alpha=.5, linewidth=.5)
fig = plt.figure(figsize=(18, 3.8*cycles))
samples = (2.8, 3.95, 4.55, 5.3, 6.78, 8.95)
for c in range(cycles):
for p, local in enumerate(samples):
ax = fig.add_subplot(cycles, 6, c*6+p+1, projection='3d'); setup(ax)
target = 9*c + local
if target > t[-1]:
ax.set_title('not evaluated'); continue
i = int(np.argmin(np.abs(t-target)))
pts = joints(i)
ax.plot(pts[:, 0], pts[:, 1], pts[:, 2], '-o', color='C0', linewidth=3, markersize=4)
ax.scatter(*foot[i], color='green' if fn[i] > 2 else 'orange', s=45)
ax.set_title(f'cycle {c+1}: {PHASES[p]}\nt={t[i]:.2f}s Fn={fn[i]:.1f}N gap={gap[i]*1000:.1f}mm', fontsize=9)
fig.suptitle(args.label + ' | FK from measured root/joints + measured foot; not Kit screenshots', fontsize=12)
fig.tight_layout(); fig.savefig(args.output / 'measured_state_keyframes.png', dpi=120); plt.close(fig)
if args.gif:
fig = plt.figure(figsize=(7, 6)); ax = fig.add_subplot(projection='3d'); setup(ax)
line, = ax.plot([], [], [], '-o', linewidth=4, markersize=6)
path, = ax.plot([], [], [], color='C1', linewidth=1, alpha=.7)
force, = ax.plot([], [], [], color='green', linewidth=3)
title = ax.set_title('')
fig.text(.03, .02, 'Measured-state FK reconstruction, NOT Kit-rendered video.\nGreen line: actual normal contact force (display scaled).', fontsize=8)
indices = list(range(0, len(t), 50)) + [len(t)-1]
def update(i):
pts = joints(i)
line.set_data_3d(pts[:, 0], pts[:, 1], pts[:, 2])
path.set_data_3d(foot[:i+1:10, 0], foot[:i+1:10, 1], foot[:i+1:10, 2])
force.set_data_3d([foot[i, 0]]*2, [foot[i, 1]]*2,
[foot[i, 2], foot[i, 2]+max(0, fn[i])*.002])
title.set_text(f'{args.label}\nt={t[i]:.2f}s cycle={int(d["cycle_index"][i])+1} {PHASES[int(d["phase"][i])]}\nFn={fn[i]:.1f}N gap={gap[i]*1000:.1f}mm')
return line, path, force, title
ani = FuncAnimation(fig, update, frames=indices, interval=100, blit=False)
ani.save(args.output / 'measured_state_animation.gif', writer=PillowWriter(fps=10), dpi=90)
plt.close(fig)
(args.output / 'scope.json').write_text(json.dumps({'trace':str(args.trace), 'display_only':True,
'new_simulation_steps':0,'fk_max_error_vs_measured_foot_m_on_display_frames':max_fk_error[0],
'not_kit_rendered_video':True,'raw_trace_unchanged':True},indent=2)+'\n')
if __name__ == '__main__':
main()