139 lines
4.0 KiB
TypeScript
139 lines
4.0 KiB
TypeScript
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"
|
|
}
|
|
]
|
|
}
|
|
});
|
|
});
|
|
});
|