同步KDL工程源码到云仓库

kdl-wasm/web/tests/kdl/trapProfile.test.ts

@@ -0,0 +1,138 @@
+import { describe, expect, it } from "vitest";
+import { createKdlWorkerRuntime } from "../../src/kdl/runtime.js";
+import { dispatchKdlRpcRequest } from "../../src/kdl/workerRpc.js";
+import type { TrapProfileResult, TrapSample } from "../../src/kdl/types.js";
+
+async function createRuntime() {
+  const runtime = createKdlWorkerRuntime();
+  await dispatchKdlRpcRequest(runtime, { id: 1, method: "init", payload: [{}] });
+  return runtime;
+}
+
+function expectMonotonic(samples: TrapSample[]) {
+  for (let index = 1; index < samples.length; index += 1) {
+    expect(samples[index]!.time).toBeGreaterThan(samples[index - 1]!.time);
+    expect(samples[index]!.s).toBeGreaterThanOrEqual(samples[index - 1]!.s);
+  }
+}
+
+describe("trapezoid velocity profile API", () => {
+  it("creates a trapezoid profile when the path can reach max velocity", async () => {
+    const runtime = await createRuntime();
+    const response = await dispatchKdlRpcRequest(runtime, {
+      id: 2,
+      method: "makeTrapProfile",
+      payload: [
+        2,
+        {
+          maxVelocity: 1,
+          maxAcceleration: 1,
+          sampleTime: 0.25
+        }
+      ]
+    });
+
+    expect(response.ok).toBe(true);
+    const profile = response.result as TrapProfileResult;
+    expect(profile).toMatchObject({
+      ok: true,
+      type: "trapezoid",
+      length: 2,
+      duration: 3,
+      tAccel: 1,
+      tConst: 1,
+      tDecel: 1,
+      vPeak: 1,
+      diagnostics: []
+    });
+    expect(profile.samples[0]).toMatchObject({ index: 0, time: 0, s: 0 });
+    expect(profile.samples.at(-1)).toMatchObject({ time: 3, s: 1 });
+    expect(profile.samples.find((sample) => sample.time === 1)?.s).toBeCloseTo(0.25);
+    expect(profile.samples.find((sample) => sample.time === 1)?.sd).toBeCloseTo(0.5);
+    expectMonotonic(profile.samples);
+  });
+
+  it("falls back to a triangle profile for short paths", async () => {
+    const runtime = await createRuntime();
+    const response = await dispatchKdlRpcRequest(runtime, {
+      id: 3,
+      method: "makeTrapProfile",
+      payload: [
+        0.5,
+        {
+          maxVelocity: 2,
+          maxAcceleration: 1,
+          sampleTime: 0.1
+        }
+      ]
+    });
+
+    expect(response.ok).toBe(true);
+    const profile = response.result as TrapProfileResult;
+    expect(profile.type).toBe("triangle");
+    expect(profile.tConst).toBe(0);
+    expect(profile.vPeak).toBeCloseTo(Math.sqrt(0.5));
+    expect(profile.duration).toBeCloseTo(2 * Math.sqrt(0.5));
+    expect(profile.samples[0]?.s).toBe(0);
+    expect(profile.samples.at(-1)?.s).toBe(1);
+    expect(profile.diagnostics).toMatchObject([
+      {
+        severity: "info",
+        code: "KDL_TRAP_TRIANGLE_PROFILE"
+      }
+    ]);
+    expectMonotonic(profile.samples);
+  });
+
+  it("returns samples from sampleTrapProfile with strict endpoint samples", async () => {
+    const runtime = await createRuntime();
+    const response = await dispatchKdlRpcRequest(runtime, {
+      id: 4,
+      method: "sampleTrapProfile",
+      payload: [
+        1,
+        {
+          maxVelocity: 1,
+          maxAcceleration: 2,
+          sampleTime: 0.2
+        }
+      ]
+    });
+
+    expect(response.ok).toBe(true);
+    const samples = response.result as TrapSample[];
+    expect(samples[0]).toMatchObject({ index: 0, time: 0, s: 0 });
+    expect(samples.at(-1)?.s).toBe(1);
+    expect(samples.at(-1)?.time).toBeCloseTo(1.5);
+    expectMonotonic(samples);
+  });
+
+  it("returns a structured diagnostic for invalid trap profile inputs", async () => {
+    const runtime = await createRuntime();
+    const response = await dispatchKdlRpcRequest(runtime, {
+      id: 5,
+      method: "makeTrapProfile",
+      payload: [
+        1,
+        {
+          maxVelocity: 0,
+          maxAcceleration: 1,
+          sampleTime: 0.01
+        }
+      ]
+    });
+
+    expect(response).toMatchObject({
+      ok: false,
+      error: {
+        code: "KDL_INVALID_TRAP_PROFILE",
+        diagnostics: [
+          {
+            severity: "error",
+            code: "KDL_INVALID_TRAP_PROFILE"
+          }
+        ]
+      }
+    });
+  });
+});