diff --git a/CHANGELOG.md b/CHANGELOG.md index 355180fb..0c8b5c47 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -2,6 +2,16 @@ 本项目的重要变更记录在此文件中，版本标签沿用仓库现有的 `V主版本.次版本[.修订版本]` 格式。 +## [0.7.1] - 2026-09-01 + +### 新增 + +- 增加浏览器内仿真遥测记录模块，可配置记录 Body、采样频率和样本上限。 +- 记录位置、移动速度、机身侧倾/俯仰/偏航角、角速度、累计里程、接触与驱动指标。 +- 增加实时数据与摘要面板，并支持导出稳定列结构的 CSV 和 Schema V1 JSON。 +- 提供 `TelemetrySource`、`registerDataChannel` 及记录生命周期接口，便于扩展业务指标和其他仿真后端。 +- 仿真重置使用数据分段，避免跨重置计算错误速度；达到样本上限时自动停止。 + ## [0.6.1] - 2026-08-28 ### 新增 diff --git a/package-lock.json b/package-lock.json index 644be042..0f266f91 100644 --- a/package-lock.json +++ b/package-lock.json @@ -1,12 +1,12 @@ { "name": "mujoco-web-platform", - "version": "0.7.0", + "version": "0.7.1", "lockfileVersion": 3, "requires": true, "packages": { "": { "name": "mujoco-web-platform", - "version": "0.7.0", + "version": "0.7.1", "license": "Apache-2.0", "dependencies": { "@monaco-editor/react": "^4.7.0", diff --git a/package.json b/package.json index 249bf2af..91c4f3c3 100644 --- a/package.json +++ b/package.json @@ -1,6 +1,6 @@ { "name": "mujoco-web-platform", - "version": "0.7.0", + "version": "0.7.1", "description": "基于 MuJoCo WebAssembly 的本地机器人仿真与控制平台", "private": true, "type": "module", diff --git a/web_platform/README.md b/web_platform/README.md index d54d719a..01f6c6d9 100644 --- a/web_platform/README.md +++ b/web_platform/README.md @@ -16,6 +16,7 @@ - 导入单文件 `.py` 控制器，通过本地 Pyodide 在 `mj_step` 前按仿真时间同步执行 - 导入 mjlab 导出的 `policy.onnx`，在浏览器本地执行 Go2-W 平衡/速度策略推理 - 从图形界面向本机训练桥接服务发起 mjlab 强化学习训练、查看进度/日志、停止任务并导入训练生成的 ONNX +- 可配置仿真遥测记录，实时查看速度、机身姿态、位置、驱动力等指标并导出 CSV/JSON - FPS、物理耗时和主线程步进预算提示 ## 开发 @@ -97,6 +98,12 @@ V3 可编辑地图使用 `schemaVersion: 2`，并增加 `"authoring": { "source" 物理地图超过 2000 个 geom 会产生性能警告，超过 10000 个会被拒绝；GLB 超过 100 万三角面会警告，超过 300 万会被拒绝。当前原生 URDF 模式不支持地图，请切换到“转换为 MJCF”。 +## 数据记录 + +加载模型后打开右侧“数据”标签，可以选择需要跟踪的 Body、采样频率和样本上限。内置通道包括世界系位置/速度、水平与三维速度、机身侧倾/俯仰/偏航角及角速度、累计里程、接触数、控制输入 RMS、驱动力 RMS、绝对驱动功率和广义速度 RMS。仿真重置不会删除已有数据，而是创建新分段，避免跨重置计算出错误速度；切换记录 Body 或采样配置会清空不兼容的旧数据。 + +记录只保留在当前浏览器会话中，达到样本上限后自动停止，可导出带稳定列名的 CSV 或包含通道元数据、摘要和样本的 Schema V1 JSON。`SimulationSession`/`PhysicsAdapter` 保留 `configureDataRecorder`、`startDataRecording`、`stopDataRecording`、`clearDataRecording`、`exportDataRecording` 接口；还可通过 `registerDataChannel({ key, label, unit, read })` 在开始记录前注册业务自定义标量通道。数据源通过 `TelemetrySource` 抽象与 MuJoCo 解耦，后续可复用于 Worker 或远端仿真。 + ## Python 控制器 Python 控制器是可信的单文件脚本，必须同步定义 `step(ctx, state)`；可选定义 `NAME`、`CONTROL_HZ`（限制为 1–500 Hz）、`init(api)`、`command(name, state)`、`reset(state)` 和 `dispose(state)`。`init` 可用 `api.joint(name)`、`api.actuator(name)`、`api.sensor(name)`、`api.body(name)` 预解析 ID；`step` 可用 `ctx.qpos(id)`、`ctx.qvel(id)`、`ctx.sensor(id)`、`ctx.body_quat(id)`、`ctx.body_position(id)` 读取状态，并用 `ctx.set_control(id, value)` 写入经过有限值检查和 actuator 限幅的控制量。定义 `command` 后，界面会显示停止、前进、后退、左转、右转和起跳按钮，并分别传入 `stop`、`forward`、`backward`、`turn_left`、`turn_right`、`jump`。所有回调都必须同步；异常会自动停止控制器或显示诊断，运行期异常还会暂停仿真并清零 `ctrl`。 diff --git a/web_platform/src/app/App.tsx b/web_platform/src/app/App.tsx index 08f7d257..92116e0e 100644 --- a/web_platform/src/app/App.tsx +++ b/web_platform/src/app/App.tsx @@ -43,6 +43,7 @@ import { type UrdfLoadMode, } from '../simulation/PhysicsAdapter'; import type { ActuatorParameters } from '../simulation/SimulationSession'; +import type { DataRecorderConfig } from '../simulation/DataRecorder'; import type { ControllerCommand, ControllerStatus } from '../controller/types'; import type { RLCommand, RLPolicyStatus } from '../rl/types'; import type { MuJoCoViewer, InteractionMode, ViewerTheme } from '../viewer/MuJoCoViewer'; @@ -1328,6 +1329,42 @@ export function App() { setPolicyStatus(undefined); state.setSnapshot(adapter.current.snapshot() ?? undefined); }; + const configureDataRecorder = (patch: Partial) => { + try { + adapter.current.configureDataRecorder(patch); + state.setSnapshot(adapter.current.snapshot() ?? undefined); + } catch (error) { + state.setDiagnostic(diagnostic('仿真', error, state.selectedEntry)); + } + }; + const startDataRecording = () => { + adapter.current.startDataRecording(); + state.setSnapshot(adapter.current.snapshot() ?? undefined); + }; + const stopDataRecording = () => { + adapter.current.stopDataRecording(); + state.setSnapshot(adapter.current.snapshot() ?? undefined); + }; + const clearDataRecording = () => { + adapter.current.clearDataRecording(); + state.setSnapshot(adapter.current.snapshot() ?? undefined); + }; + const exportDataRecording = (format: 'csv' | 'json') => { + try { + const stem = + (manifest.current?.name ?? 'simulation') + .replace(/\.(?:zip|xml|urdf)$/i, '') + .replace(/[^\p{L}\p{N}._-]+/gu, '_') || 'simulation'; + downloadBytes( + adapter.current.exportDataRecording(format), + `${stem}-telemetry.${format}`, + format === 'csv' ? 'text/csv' : 'application/json', + ); + notify(`遥测 ${format.toUpperCase()} 已导出`, `${stem}-telemetry.${format}`); + } catch (error) { + state.setDiagnostic(diagnostic('仿真', error, state.selectedEntry)); + } + }; const notify = ( title: string, detail: string, @@ -1769,6 +1806,11 @@ export function App() { setLeftOpen(true); setProjectSidebarTab('assets'); }} + onDataRecorderConfigure={configureDataRecorder} + onDataRecordingStart={startDataRecording} + onDataRecordingStop={stopDataRecording} + onDataRecordingClear={clearDataRecording} + onDataRecordingExport={exportDataRecording} /> {pendingUrdfPath && ( diff --git a/web_platform/src/app/components/DataRecordingPanel.test.tsx b/web_platform/src/app/components/DataRecordingPanel.test.tsx new file mode 100644 index 00000000..b500a535 --- /dev/null +++ b/web_platform/src/app/components/DataRecordingPanel.test.tsx @@ -0,0 +1,74 @@ +import { fireEvent, render, screen } from '@testing-library/react'; +import { DataRecordingPanel } from './DataRecordingPanel'; +import type { DataRecorderStatus } from '../../simulation/DataRecorder'; + +const status: DataRecorderStatus = { + recording: false, + limitReached: false, + sampleCount: 2, + segmentCount: 1, + config: { bodyId: 1, sampleRateHz: 50, maxSamples: 30_000 }, + body: { id: 1, name: 'base' }, + latest: { + sequence: 1, + segment: 0, + simulationTime: 0.02, + values: { + speed_horizontal: 1.25, + speed_3d: 1.3, + velocity_x: 1.25, + velocity_y: 0, + velocity_z: 0.1, + pitch: 0.1, + roll: -0.05, + yaw: 0.2, + height: 0.45, + position_x: 0.1, + position_y: 0, + contact_count: 4, + control_rms: 2, + actuator_force_rms: 3, + actuator_power_abs: 8, + }, + }, + summary: { + duration: 0.02, + distanceHorizontal: 0.1, + maxHorizontalSpeed: 1.25, + maxAbsRoll: 0.05, + maxAbsPitch: 0.1, + minHeight: 0.44, + maxHeight: 0.46, + }, +}; + +describe('DataRecordingPanel', () => { + it('展示实时遥测并提供配置、记录和导出操作', () => { + const configure = vi.fn(), + start = vi.fn(), + exportData = vi.fn(); + render( + , + ); + expect(screen.getAllByText('1.250 m/s').length).toBeGreaterThan(0); + expect(screen.getAllByText('5.73°').length).toBeGreaterThan(0); + fireEvent.change(screen.getByLabelText('记录 Body'), { target: { value: '2' } }); + expect(configure).toHaveBeenCalledWith({ bodyId: 2 }); + fireEvent.click(screen.getByRole('button', { name: '开始记录' })); + expect(start).toHaveBeenCalledTimes(1); + fireEvent.click(screen.getByRole('button', { name: '导出 CSV' })); + expect(exportData).toHaveBeenCalledWith('csv'); + }); +}); diff --git a/web_platform/src/app/components/DataRecordingPanel.tsx b/web_platform/src/app/components/DataRecordingPanel.tsx new file mode 100644 index 00000000..672c6c8d --- /dev/null +++ b/web_platform/src/app/components/DataRecordingPanel.tsx @@ -0,0 +1,198 @@ +import { Circle, Download, Square, Trash2 } from 'lucide-react'; +import type { BodyInfo } from '../../simulation/SimulationSession'; +import type { DataRecorderConfig, DataRecorderStatus } from '../../simulation/DataRecorder'; +import { Badge, Button, PropertyRow, Select } from '../../components/ui'; + +const SAMPLE_RATES = [10, 20, 50, 100, 200]; +const radiansToDegrees = 180 / Math.PI; + +function number(value: number | undefined, digits = 3): string { + return Number.isFinite(value) ? value!.toFixed(digits) : '—'; +} + +function degrees(value: number | undefined): string { + return Number.isFinite(value) ? `${(value! * radiansToDegrees).toFixed(2)}°` : '—'; +} + +export function DataRecordingPanel({ + status, + bodies, + onConfigure, + onStart, + onStop, + onClear, + onExport, +}: { + status: DataRecorderStatus; + bodies: BodyInfo[]; + onConfigure: (patch: Partial) => void; + onStart: () => void; + onStop: () => void; + onClear: () => void; + onExport: (format: 'csv' | 'json') => void; +}) { + const values = status.latest?.values, + availableBodies = bodies.filter( + (body) => body.id > 0 && !body.name.startsWith('__platform_map_'), + ); + return ( +
+
+
+
+

仿真遥测记录

+

+ 数据仅保存在当前浏览器会话，可导出 CSV 或 JSON。 +

+
+ + {status.recording ? '记录中' : status.limitReached ? '已达上限' : '已停止'} + +
+ +
+ + +
+
+ {status.recording ? ( + + ) : ( + + )} + +
+
+ + + +
+
+ +
+

实时运动状态

+ + + + + + + + + + + + +
+ +
+

本次记录摘要

+ + + + + +
+ + +
+
+

+ 每次仿真重置会创建新分段，避免跨重置计算出错误速度。开发接口支持注册额外标量通道，导出列名保持稳定。 +

+
+ ); +} diff --git a/web_platform/src/app/components/SidebarPanel.tsx b/web_platform/src/app/components/SidebarPanel.tsx index 6fd8d4a9..fb3bb20d 100644 --- a/web_platform/src/app/components/SidebarPanel.tsx +++ b/web_platform/src/app/components/SidebarPanel.tsx @@ -1,5 +1,13 @@ import { useState, type ReactNode } from 'react'; -import { Box, FolderTree, Info, Map as MapIcon, Settings2, SlidersHorizontal } from 'lucide-react'; +import { + Box, + Database, + FolderTree, + Info, + Map as MapIcon, + Settings2, + SlidersHorizontal, +} from 'lucide-react'; import type { MapEntry, ModelEntry } from '../../project/types'; import { countProjectSearchResults, @@ -16,6 +24,7 @@ import type { SimulationSnapshot, } from '../../simulation/SimulationSession'; import type { UrdfBaseMode, UrdfLoadMode } from '../../simulation/PhysicsAdapter'; +import type { DataRecorderConfig } from '../../simulation/DataRecorder'; import type { ViewerSelection } from '../../viewer/MuJoCoViewer'; import type { ControllerCommand, ControllerStatus } from '../../controller/types'; import type { RLCommand, RLPolicyStatus } from '../../rl/types'; @@ -34,6 +43,7 @@ import { ProjectBreadcrumb } from './ProjectBreadcrumb'; import { PythonControllerPanel } from './PythonControllerPanel'; import { RLPolicyPanel } from './RLPolicyPanel'; import { LocalTrainingPanel } from './LocalTrainingPanel'; +import { DataRecordingPanel } from './DataRecordingPanel'; import { PhysicalMapPanel } from './PhysicalMapPanel'; import { MapAssetLibrary } from './MapAssetLibrary'; import { @@ -297,9 +307,14 @@ interface ModelControlsProps { onEditorSnapping: (translation: number | null, rotationDegrees: number | null) => void; onMapDisplay: (visual: boolean, collision: boolean) => void; onMapTabOpen?: () => void; + onDataRecorderConfigure: (patch: Partial) => void; + onDataRecordingStart: () => void; + onDataRecordingStop: () => void; + onDataRecordingClear: () => void; + onDataRecordingExport: (format: 'csv' | 'json') => void; } export function ModelControlsSidebar(props: ModelControlsProps) { - const [tab, setTab] = useState<'properties' | 'controls' | 'map'>('properties'), + const [tab, setTab] = useState<'properties' | 'controls' | 'data' | 'map'>('properties'), s = props.snapshot; if (!s) return ( @@ -535,6 +550,22 @@ export function ModelControlsSidebar(props: ModelControlsProps) { icon: , content: controls, }, + { + value: 'data', + label: '数据', + icon: , + content: ( + + ), + }, { value: 'map', label: '地图', diff --git a/web_platform/src/simulation/DataRecorder.test.ts b/web_platform/src/simulation/DataRecorder.test.ts new file mode 100644 index 00000000..d6f9f7f2 --- /dev/null +++ b/web_platform/src/simulation/DataRecorder.test.ts @@ -0,0 +1,121 @@ +import { DataRecorder, type TelemetrySource } from './DataRecorder'; + +function sourceFixture() { + let time = 0, + position: [number, number, number] = [0, 0, 0.5], + quaternion: [number, number, number, number] = [1, 0, 0, 0]; + const source: TelemetrySource = { + simulationTime: () => time, + bodyPose: () => ({ position, quaternion }), + controls: () => [3, 4], + actuatorForces: () => [2, -2], + actuatorVelocities: () => [3, -1], + generalizedVelocities: () => [0, 2], + contactCount: () => 4, + }; + return { + source, + move(nextTime: number, nextPosition: [number, number, number]) { + time = nextTime; + position = nextPosition; + }, + rotate(next: [number, number, number, number]) { + quaternion = next; + }, + }; +} + +describe('DataRecorder', () => { + it('按采样率记录位姿、移动速度、姿态和控制指标', () => { + const fixture = sourceFixture(), + recorder = new DataRecorder(fixture.source, [{ id: 1, name: 'base' }], { + sampleRateHz: 10, + }); + recorder.start(); + fixture.move(0.05, [0.05, 0, 0.5]); + recorder.capture(); + expect(recorder.status().sampleCount).toBe(1); + + fixture.move(0.1, [0.1, 0, 0.6]); + fixture.rotate([Math.cos(Math.PI / 8), 0, Math.sin(Math.PI / 8), 0]); + recorder.capture(); + const status = recorder.status(), + values = status.latest!.values; + expect(status.sampleCount).toBe(2); + expect(values.speed_horizontal).toBeCloseTo(1); + expect(values.velocity_z).toBeCloseTo(1); + expect(values.pitch).toBeCloseTo(Math.PI / 4); + expect(values.distance_horizontal).toBeCloseTo(0.1); + expect(values.control_rms).toBeCloseTo(Math.sqrt(12.5)); + expect(values.actuator_force_rms).toBe(2); + expect(values.actuator_power_abs).toBe(8); + expect(values.contact_count).toBe(4); + expect(status.summary.maxAbsPitch).toBeCloseTo(Math.PI / 4); + }); + + it('仿真重置后新建分段且不跨分段计算速度', () => { + const fixture = sourceFixture(), + recorder = new DataRecorder(fixture.source, [{ id: 1, name: 'base' }]); + recorder.start(); + fixture.move(0.02, [0.1, 0, 0.5]); + recorder.capture(true); + recorder.simulationReset(); + fixture.move(0, [0, 0, 0.5]); + recorder.capture(true); + expect(recorder.status().segmentCount).toBe(2); + expect(recorder.status().latest?.values.speed_horizontal).toBe(0); + }); + + it('支持自定义通道并导出稳定的 CSV/JSON 接口', () => { + const fixture = sourceFixture(), + recorder = new DataRecorder(fixture.source, [{ id: 1, name: 'base' }]); + recorder.registerChannel({ + key: 'stability_score', + label: '稳定性', + unit: '', + read: ({ pose }) => pose.position[2] * 2, + }); + recorder.start(); + const csv = new TextDecoder().decode(recorder.toCsv()), + json = JSON.parse(new TextDecoder().decode(recorder.toJson())) as { + schemaVersion: number; + channels: { key: string }[]; + samples: { values: Record }[]; + }; + expect(csv).toContain('simulation_time_s'); + expect(csv).toContain('stability_score'); + expect(json.schemaVersion).toBe(1); + expect(json.channels.some((channel) => channel.key === 'stability_score')).toBe(true); + expect(json.samples[0].values.stability_score).toBe(1); + }); + + it('修改采样对象或频率时清空旧数据，并在达到上限时停止', () => { + const fixture = sourceFixture(), + recorder = new DataRecorder( + fixture.source, + [ + { id: 1, name: 'base' }, + { id: 2, name: 'payload' }, + ], + { maxSamples: 100 }, + ); + recorder.start(); + recorder.stop(); + recorder.configure({ bodyId: 2, sampleRateHz: 20 }); + expect(recorder.status()).toMatchObject({ + sampleCount: 0, + body: { id: 2 }, + config: { sampleRateHz: 20 }, + }); + recorder.start(); + for (let index = 1; index < 100; index += 1) { + fixture.move(index / 20, [index / 20, 0, 0.5]); + recorder.capture(true); + } + expect(recorder.status()).toMatchObject({ + recording: false, + limitReached: true, + sampleCount: 100, + }); + }); +}); diff --git a/web_platform/src/simulation/DataRecorder.ts b/web_platform/src/simulation/DataRecorder.ts new file mode 100644 index 00000000..58aed3b5 --- /dev/null +++ b/web_platform/src/simulation/DataRecorder.ts @@ -0,0 +1,455 @@ +export interface TelemetryBody { + id: number; + name: string; +} + +export interface TelemetryPose { + position: readonly [number, number, number]; + /** MuJoCo 的 w、x、y、z 四元数顺序。 */ + quaternion: readonly [number, number, number, number]; +} + +/** + * 数据源接口刻意与 MuJoCo 类型解耦，后续可接入 Worker、远端仿真或自定义指标。 + */ +export interface TelemetrySource { + simulationTime(): number; + bodyPose(bodyId: number): TelemetryPose; + controls(): ArrayLike; + actuatorForces(): ArrayLike; + actuatorVelocities(): ArrayLike; + generalizedVelocities(): ArrayLike; + contactCount(): number; +} + +export interface TelemetryChannelContext { + source: TelemetrySource; + body: TelemetryBody; + simulationTime: number; + pose: TelemetryPose; +} + +/** 注册自定义通道时使用的稳定扩展接口。 */ +export interface TelemetryChannel { + key: string; + label: string; + unit: string; + read(context: TelemetryChannelContext): number; +} + +export interface DataRecorderConfig { + bodyId: number; + sampleRateHz: number; + maxSamples: number; +} + +export interface TelemetrySample { + sequence: number; + segment: number; + simulationTime: number; + values: Record; +} + +export interface TelemetrySummary { + duration: number; + distanceHorizontal: number; + maxHorizontalSpeed: number; + maxAbsRoll: number; + maxAbsPitch: number; + minHeight?: number; + maxHeight?: number; +} + +export interface DataRecorderStatus { + recording: boolean; + limitReached: boolean; + sampleCount: number; + segmentCount: number; + config: DataRecorderConfig; + body: TelemetryBody; + latest?: TelemetrySample; + summary: TelemetrySummary; +} + +const BUILTIN_CHANNELS: readonly Omit[] = [ + { key: 'position_x', label: '位置 X', unit: 'm' }, + { key: 'position_y', label: '位置 Y', unit: 'm' }, + { key: 'height', label: '机身高度', unit: 'm' }, + { key: 'velocity_x', label: '速度 X', unit: 'm/s' }, + { key: 'velocity_y', label: '速度 Y', unit: 'm/s' }, + { key: 'velocity_z', label: '速度 Z', unit: 'm/s' }, + { key: 'speed_horizontal', label: '水平移动速度', unit: 'm/s' }, + { key: 'speed_3d', label: '三维速度', unit: 'm/s' }, + { key: 'roll', label: '侧倾角', unit: 'rad' }, + { key: 'pitch', label: '俯仰角', unit: 'rad' }, + { key: 'yaw', label: '偏航角', unit: 'rad' }, + { key: 'angular_velocity_roll', label: '侧倾角速度', unit: 'rad/s' }, + { key: 'angular_velocity_pitch', label: '俯仰角速度', unit: 'rad/s' }, + { key: 'angular_velocity_yaw', label: '偏航角速度', unit: 'rad/s' }, + { key: 'distance_horizontal', label: '水平累计里程', unit: 'm' }, + { key: 'contact_count', label: '场景接触数', unit: '' }, + { key: 'control_rms', label: '控制输入 RMS', unit: '' }, + { key: 'actuator_force_rms', label: '驱动力 RMS', unit: '' }, + { key: 'actuator_power_abs', label: '驱动器绝对功率和', unit: 'W' }, + { key: 'joint_velocity_rms', label: '广义速度 RMS', unit: '' }, +]; + +const DEFAULT_MAX_SAMPLES = 30_000; + +function finite(value: number, fallback = 0): number { + return Number.isFinite(value) ? value : fallback; +} + +function clampInteger(value: number, minimum: number, maximum: number): number { + return Math.round(Math.min(maximum, Math.max(minimum, finite(value, minimum)))); +} + +function rootMeanSquare(values: ArrayLike): number { + if (!values.length) return 0; + let sum = 0; + for (let index = 0; index < values.length; index += 1) { + const value = finite(Number(values[index])); + sum += value * value; + } + return Math.sqrt(sum / values.length); +} + +function absoluteActuatorPower(forces: ArrayLike, velocities: ArrayLike): number { + let total = 0; + for (let index = 0; index < Math.min(forces.length, velocities.length); index += 1) + total += Math.abs(finite(Number(forces[index])) * finite(Number(velocities[index]))); + return total; +} + +function eulerFromQuaternion( + quaternion: readonly [number, number, number, number], +): [number, number, number] { + const [w, x, y, z] = quaternion.map((value) => finite(Number(value))) as [ + number, + number, + number, + number, + ], + norm = Math.hypot(w, x, y, z) || 1, + qw = w / norm, + qx = x / norm, + qy = y / norm, + qz = z / norm, + roll = Math.atan2(2 * (qw * qx + qy * qz), 1 - 2 * (qx * qx + qy * qy)), + pitch = Math.asin(Math.min(1, Math.max(-1, 2 * (qw * qy - qz * qx)))), + yaw = Math.atan2(2 * (qw * qz + qx * qy), 1 - 2 * (qy * qy + qz * qz)); + return [roll, pitch, yaw]; +} + +function angleDelta(next: number, previous: number): number { + return Math.atan2(Math.sin(next - previous), Math.cos(next - previous)); +} + +function csvCell(value: string | number): string { + const text = String(value); + return /[",\r\n]/.test(text) ? `"${text.replaceAll('"', '""')}"` : text; +} + +function initialSummary(): TelemetrySummary { + return { + duration: 0, + distanceHorizontal: 0, + maxHorizontalSpeed: 0, + maxAbsRoll: 0, + maxAbsPitch: 0, + }; +} + +export class DataRecorder { + private configValue: DataRecorderConfig; + private samplesValue: TelemetrySample[] = []; + private customChannels = new Map(); + private recordingValue = false; + private limitReachedValue = false; + private nextSampleTime?: number; + private previousTime?: number; + private previousPosition?: [number, number, number]; + private previousEuler?: [number, number, number]; + private segment = 0; + private pendingSegment = false; + private summaryValue = initialSummary(); + + constructor( + private readonly source: TelemetrySource, + private readonly bodies: readonly TelemetryBody[], + config?: Partial, + ) { + if (!bodies.length) throw new Error('数据记录器至少需要一个可记录 Body'); + const requestedBody = bodies.some((body) => body.id === config?.bodyId) + ? config!.bodyId! + : bodies[0].id; + this.configValue = { + bodyId: requestedBody, + sampleRateHz: clampInteger(config?.sampleRateHz ?? 50, 1, 1000), + maxSamples: clampInteger(config?.maxSamples ?? DEFAULT_MAX_SAMPLES, 100, 1_000_000), + }; + } + + channels(): readonly Omit[] { + return [...BUILTIN_CHANNELS, ...this.customChannels.values()].map(({ key, label, unit }) => ({ + key, + label, + unit, + })); + } + + /** + * 注册业务自定义标量。key 会成为 CSV/JSON 的稳定列名；开始记录后不可变更列结构。 + */ + registerChannel(channel: TelemetryChannel): () => void { + if (this.recordingValue || this.samplesValue.length) + throw new Error('已有记录时不能修改数据通道，请先停止并清空记录'); + if (!/^[a-z][a-z0-9_]*$/i.test(channel.key)) + throw new Error(`数据通道 key 无效：${channel.key}`); + if ( + BUILTIN_CHANNELS.some((item) => item.key === channel.key) || + this.customChannels.has(channel.key) + ) + throw new Error(`数据通道 key 重复：${channel.key}`); + this.customChannels.set(channel.key, channel); + return () => { + if (this.recordingValue || this.samplesValue.length) + throw new Error('已有记录时不能移除数据通道，请先停止并清空记录'); + this.customChannels.delete(channel.key); + }; + } + + configure(patch: Partial): DataRecorderStatus { + if (this.recordingValue) throw new Error('请先停止记录再修改采样配置'); + const next: DataRecorderConfig = { + bodyId: patch.bodyId ?? this.configValue.bodyId, + sampleRateHz: clampInteger(patch.sampleRateHz ?? this.configValue.sampleRateHz, 1, 1000), + maxSamples: clampInteger(patch.maxSamples ?? this.configValue.maxSamples, 100, 1_000_000), + }; + if (!this.bodies.some((body) => body.id === next.bodyId)) + throw new Error(`无法记录不存在的 Body：${next.bodyId}`); + const changed = Object.keys(next).some( + (key) => + next[key as keyof DataRecorderConfig] !== this.configValue[key as keyof DataRecorderConfig], + ); + this.configValue = next; + if (changed && this.samplesValue.length) this.clear(); + return this.status(); + } + + start(): DataRecorderStatus { + if (this.samplesValue.length >= this.configValue.maxSamples) return this.status(); + this.recordingValue = true; + this.limitReachedValue = false; + this.resetSamplingClock(); + this.capture(true); + return this.status(); + } + + stop(): DataRecorderStatus { + this.recordingValue = false; + return this.status(); + } + + clear(): DataRecorderStatus { + this.samplesValue = []; + this.recordingValue = false; + this.limitReachedValue = false; + this.segment = 0; + this.pendingSegment = false; + this.summaryValue = initialSummary(); + this.resetSamplingClock(); + return this.status(); + } + + /** 仿真 reset 后开启新分段，保留之前的数据且不跨分段计算速度。 */ + simulationReset(): void { + this.pendingSegment = this.samplesValue.length > 0; + this.resetSamplingClock(); + } + + capture(force = false): void { + if (!this.recordingValue) return; + if (this.samplesValue.length >= this.configValue.maxSamples) { + this.recordingValue = false; + this.limitReachedValue = true; + return; + } + const time = finite(this.source.simulationTime()); + if (this.pendingSegment) { + this.segment += 1; + this.pendingSegment = false; + } + if (this.previousTime !== undefined && time + 1e-9 < this.previousTime) { + this.segment += 1; + this.resetSamplingClock(); + } + if (!force && this.nextSampleTime !== undefined && time + 1e-9 < this.nextSampleTime) return; + + const body = this.body(), + pose = this.source.bodyPose(body.id), + position: [number, number, number] = [ + finite(Number(pose.position[0])), + finite(Number(pose.position[1])), + finite(Number(pose.position[2])), + ], + euler = eulerFromQuaternion(pose.quaternion), + dt = this.previousTime === undefined ? 0 : Math.max(0, time - this.previousTime), + sameSegment = + dt > 1e-9 && this.previousPosition !== undefined && this.previousEuler !== undefined, + velocity: [number, number, number] = sameSegment + ? (position.map((value, axis) => (value - this.previousPosition![axis]) / dt) as [ + number, + number, + number, + ]) + : [0, 0, 0], + angularVelocity: [number, number, number] = sameSegment + ? (euler.map((value, axis) => angleDelta(value, this.previousEuler![axis]) / dt) as [ + number, + number, + number, + ]) + : [0, 0, 0], + horizontalDelta = sameSegment + ? Math.hypot( + position[0] - this.previousPosition![0], + position[1] - this.previousPosition![1], + ) + : 0; + + this.summaryValue.distanceHorizontal += horizontalDelta; + const values: Record = { + position_x: position[0], + position_y: position[1], + height: position[2], + velocity_x: velocity[0], + velocity_y: velocity[1], + velocity_z: velocity[2], + speed_horizontal: Math.hypot(velocity[0], velocity[1]), + speed_3d: Math.hypot(...velocity), + roll: euler[0], + pitch: euler[1], + yaw: euler[2], + angular_velocity_roll: angularVelocity[0], + angular_velocity_pitch: angularVelocity[1], + angular_velocity_yaw: angularVelocity[2], + distance_horizontal: this.summaryValue.distanceHorizontal, + contact_count: Math.max(0, Math.round(finite(this.source.contactCount()))), + control_rms: rootMeanSquare(this.source.controls()), + actuator_force_rms: rootMeanSquare(this.source.actuatorForces()), + actuator_power_abs: absoluteActuatorPower( + this.source.actuatorForces(), + this.source.actuatorVelocities(), + ), + joint_velocity_rms: rootMeanSquare(this.source.generalizedVelocities()), + }; + const context: TelemetryChannelContext = { + source: this.source, + body, + simulationTime: time, + pose, + }; + for (const channel of this.customChannels.values()) { + try { + values[channel.key] = finite(channel.read(context), Number.NaN); + } catch { + values[channel.key] = Number.NaN; + } + } + const sample: TelemetrySample = { + sequence: this.samplesValue.length, + segment: this.segment, + simulationTime: time, + values, + }; + this.samplesValue.push(sample); + if (sameSegment) this.summaryValue.duration += dt; + this.summaryValue.maxHorizontalSpeed = Math.max( + this.summaryValue.maxHorizontalSpeed, + values.speed_horizontal, + ); + this.summaryValue.maxAbsRoll = Math.max(this.summaryValue.maxAbsRoll, Math.abs(values.roll)); + this.summaryValue.maxAbsPitch = Math.max(this.summaryValue.maxAbsPitch, Math.abs(values.pitch)); + this.summaryValue.minHeight = + this.summaryValue.minHeight === undefined + ? values.height + : Math.min(this.summaryValue.minHeight, values.height); + this.summaryValue.maxHeight = + this.summaryValue.maxHeight === undefined + ? values.height + : Math.max(this.summaryValue.maxHeight, values.height); + this.previousTime = time; + this.previousPosition = position; + this.previousEuler = euler; + this.nextSampleTime = time + 1 / this.configValue.sampleRateHz; + if (this.samplesValue.length >= this.configValue.maxSamples) { + this.recordingValue = false; + this.limitReachedValue = true; + } + } + + status(): DataRecorderStatus { + const latest = this.samplesValue.at(-1); + return { + recording: this.recordingValue, + limitReached: this.limitReachedValue, + sampleCount: this.samplesValue.length, + segmentCount: this.samplesValue.length ? this.segment + 1 : 0, + config: { ...this.configValue }, + body: { ...this.body() }, + latest: latest ? { ...latest, values: { ...latest.values } } : undefined, + summary: { ...this.summaryValue }, + }; + } + + samples(): readonly TelemetrySample[] { + return this.samplesValue; + } + + toCsv(): Uint8Array { + const keys = this.channels().map((channel) => channel.key), + rows = [ + ['sequence', 'segment', 'simulation_time_s', ...keys].map(csvCell).join(','), + ...this.samplesValue.map((sample) => + [ + sample.sequence, + sample.segment, + sample.simulationTime, + ...keys.map((key) => sample.values[key] ?? Number.NaN), + ] + .map(csvCell) + .join(','), + ), + ]; + return new TextEncoder().encode(`${rows.join('\n')}\n`); + } + + toJson(): Uint8Array { + return new TextEncoder().encode( + `${JSON.stringify( + { + schemaVersion: 1, + body: this.body(), + config: this.configValue, + channels: this.channels(), + summary: this.summaryValue, + samples: this.samplesValue, + }, + null, + 2, + )}\n`, + ); + } + + private body(): TelemetryBody { + return this.bodies.find((body) => body.id === this.configValue.bodyId) ?? this.bodies[0]; + } + + private resetSamplingClock(): void { + this.nextSampleTime = undefined; + this.previousTime = undefined; + this.previousPosition = undefined; + this.previousEuler = undefined; + } +} diff --git a/web_platform/src/simulation/PhysicsAdapter.ts b/web_platform/src/simulation/PhysicsAdapter.ts index cb601ebb..d9c1ecbb 100644 --- a/web_platform/src/simulation/PhysicsAdapter.ts +++ b/web_platform/src/simulation/PhysicsAdapter.ts @@ -16,6 +16,7 @@ import { } from './SimulationSession'; import type { ControllerCommand, ControllerStatus } from '../controller/types'; import type { RLCommand, RLPolicyStatus } from '../rl/types'; +import type { DataRecorderConfig, DataRecorderStatus, TelemetryChannel } from './DataRecorder'; import { composePhysicalMap } from '../map/physicalMap'; import { composeProjectMap } from '../map/MapComposer'; import { resolveProjectMap } from '../map/MapLoader'; @@ -64,6 +65,12 @@ export interface PhysicsAdapter { setRLPolicyEnabled(enabled: boolean): void; setRLCommand(command: RLCommand): void; removeRLPolicy(): void; + configureDataRecorder(config: Partial): DataRecorderStatus | undefined; + startDataRecording(): DataRecorderStatus | undefined; + stopDataRecording(): DataRecorderStatus | undefined; + clearDataRecording(): DataRecorderStatus | undefined; + registerDataChannel(channel: TelemetryChannel): (() => void) | undefined; + exportDataRecording(format: 'csv' | 'json'): Uint8Array; cachedSupportFiles(): ProjectFile[]; releaseRetired(): void; rollbackRetired(): void; @@ -305,6 +312,25 @@ export class MainThreadPhysicsAdapter implements PhysicsAdapter { removeRLPolicy(): void { this.session?.removeRLPolicy(); } + configureDataRecorder(config: Partial): DataRecorderStatus | undefined { + return this.session?.configureDataRecorder(config); + } + startDataRecording(): DataRecorderStatus | undefined { + return this.session?.startDataRecording(); + } + stopDataRecording(): DataRecorderStatus | undefined { + return this.session?.stopDataRecording(); + } + clearDataRecording(): DataRecorderStatus | undefined { + return this.session?.clearDataRecording(); + } + registerDataChannel(channel: TelemetryChannel): (() => void) | undefined { + return this.session?.registerDataChannel(channel); + } + exportDataRecording(format: 'csv' | 'json'): Uint8Array { + if (!this.session) throw new Error('请先加载模型'); + return this.session.exportDataRecording(format); + } cachedSupportFiles(): ProjectFile[] { return this.supportFiles.map((file) => ({ ...file, data: file.data.slice() })); } diff --git a/web_platform/src/simulation/SimulationSession.ts b/web_platform/src/simulation/SimulationSession.ts index a78578c4..0dc2796f 100644 --- a/web_platform/src/simulation/SimulationSession.ts +++ b/web_platform/src/simulation/SimulationSession.ts @@ -5,6 +5,13 @@ import type { ControllerBindings, ControllerCommand, ControllerStatus } from '.. import { Go2wPolicyBindings } from '../rl/runtime/Go2wPolicyBindings'; import type { OnnxPolicyRuntime } from '../rl/runtime/OnnxPolicyRuntime'; import type { RLCommand, RLPolicyStatus } from '../rl/types'; +import { + DataRecorder, + type DataRecorderConfig, + type DataRecorderStatus, + type TelemetryBody, + type TelemetryChannel, +} from './DataRecorder'; export interface ActuatorParameters { gear: number; @@ -63,6 +70,7 @@ export interface SimulationSnapshot { warnings: string[]; controller?: ControllerStatus; rlPolicy?: RLPolicyStatus; + telemetry: DataRecorderStatus; model: { nbody: number; njnt: number; @@ -99,6 +107,7 @@ export class SimulationSession { private controllerLoadGeneration = 0; private rlPolicy?: OnnxPolicyRuntime; private rlPolicyLoadGeneration = 0; + private dataRecorder!: DataRecorder; constructor( readonly module: MainModule, @@ -131,6 +140,36 @@ export class SimulationSession { } }); module.mj_forward(model, data); + const bodies = this.telemetryBodies(); + this.dataRecorder = new DataRecorder( + { + simulationTime: () => Number(this.data.time), + bodyPose: (bodyId) => { + const positionAddress = bodyId * 3, + quaternionAddress = bodyId * 4; + return { + position: [ + Number(this.data.xpos[positionAddress]), + Number(this.data.xpos[positionAddress + 1]), + Number(this.data.xpos[positionAddress + 2]), + ], + quaternion: [ + Number(this.data.xquat[quaternionAddress]), + Number(this.data.xquat[quaternionAddress + 1]), + Number(this.data.xquat[quaternionAddress + 2]), + Number(this.data.xquat[quaternionAddress + 3]), + ], + }; + }, + controls: () => this.data.ctrl, + actuatorForces: () => this.data.actuator_force, + actuatorVelocities: () => this.data.actuator_velocity, + generalizedVelocities: () => this.data.qvel, + contactCount: () => Number(this.data.ncon), + }, + bodies, + { bodyId: this.defaultTelemetryBody(bodies) }, + ); } catch (error) { perturb?.delete(); data?.delete(); @@ -155,11 +194,13 @@ export class SimulationSession { this.data.ctrl.fill(0); this.pythonController?.reset(Number(this.data.time)); this.rlPolicy?.reset(Number(this.data.time)); + this.dataRecorder.simulationReset(); } singleStep(): void { this.runController(); this.applyForce(); this.module.mj_step(this.model, this.data); + this.dataRecorder.capture(); } advance(now: number): FrameResult { @@ -182,6 +223,7 @@ export class SimulationSession { this.runController(); this.applyForce(); this.module.mj_step(this.model, this.data); + this.dataRecorder.capture(); this.accumulator -= dt; steps++; } @@ -240,6 +282,26 @@ export class SimulationSession { setRLCommand(command: RLCommand): void { this.rlPolicy?.setCommand(command); } + + configureDataRecorder(config: Partial): DataRecorderStatus { + return this.dataRecorder.configure(config); + } + startDataRecording(): DataRecorderStatus { + return this.dataRecorder.start(); + } + stopDataRecording(): DataRecorderStatus { + return this.dataRecorder.stop(); + } + clearDataRecording(): DataRecorderStatus { + return this.dataRecorder.clear(); + } + registerDataChannel(channel: TelemetryChannel): () => void { + return this.dataRecorder.registerChannel(channel); + } + exportDataRecording(format: 'csv' | 'json'): Uint8Array { + return format === 'csv' ? this.dataRecorder.toCsv() : this.dataRecorder.toJson(); + } + removeRLPolicy(): void { this.rlPolicyLoadGeneration += 1; this.rlPolicy?.dispose(); @@ -553,6 +615,31 @@ export class SimulationSession { this.data.xfrc_applied[offset + 2] += this.force[2]; } + private telemetryBodies(): TelemetryBody[] { + const bodies = Array.from({ length: this.model.nbody }, (_, id) => { + const body = this.model.body(id); + try { + return { id, name: body.name || (id === 0 ? 'world' : `body_${id}`) }; + } finally { + body.delete(); + } + }).filter((body) => body.id > 0 && !body.name.startsWith('__platform_map_')); + return bodies.length ? bodies : [{ id: 0, name: 'world' }]; + } + + private defaultTelemetryBody(bodies: readonly TelemetryBody[]): number { + for (const body of bodies) { + if (Number(this.model.body_parentid[body.id]) !== 0) continue; + const firstJoint = Number(this.model.body_jntadr[body.id]), + jointCount = Number(this.model.body_jntnum[body.id]); + for (let offset = 0; offset < jointCount; offset += 1) + if (this.jointLimits[firstJoint + offset]?.type === 0) return body.id; + } + return ( + bodies.find((body) => Number(this.model.body_parentid[body.id]) === 0)?.id ?? bodies[0].id + ); + } + /** 用有限几何的包围球估算视图中心与范围，忽略地面等无限平面。 */ geometryBounds(): { center: [number, number, number]; extent: number } { const lower = [Number.POSITIVE_INFINITY, Number.POSITIVE_INFINITY, Number.POSITIVE_INFINITY]; @@ -825,6 +912,7 @@ export class SimulationSession { warnings: this.warnings, controller: this.pythonController?.status(), rlPolicy: this.rlPolicy?.status(), + telemetry: this.dataRecorder.status(), model: { nbody: this.model.nbody, njnt: this.model.njnt,