#!/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()