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:
@@ -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()
|
||||
Reference in New Issue
Block a user