同步KDL工程源码到云仓库

kdl-wasm/web/tests/grl/motionCompile.test.ts

@@ -0,0 +1,124 @@
+import { describe, expect, it } from "vitest";
+import type { GrlDataDeclaration, GrlProcedureDeclaration, GrlTargetDeclaration } from "../../src/grl/ast/index.js";
+import { parseGrl } from "../../src/grl/parser/index.js";
+import {
+  buildMotionContext,
+  compileMotionToKdlRequest,
+  parseProcedureMotionInstructions
+} from "../../src/grl/semantic/index.js";
+
+const PROGRAM = `language grl 0.1
+module Main
+  persistent tool gripper = tool {
+    tcp: pose(0 mm, 0 mm, 100 mm, 0 deg, 0 deg, 0 deg),
+    mass: 1 kg
+  }
+  persistent frame fixture = frame {
+    origin: pose(800 mm, 0 mm, 0 mm, 0 deg, 0 deg, 0 deg)
+  }
+  const speed v_joint = joint(60 %)
+  const speed v_linear = linear(300 mm/s)
+  const zone z10 = z(10 mm)
+  target home = joint_target {
+    joints: [0 deg, 0 deg]
+  }
+  target pick = pose_target {
+    pose: pose(500 mm, 0 mm, 0 mm, 0 deg, 0 deg, 0 deg),
+    tool: gripper,
+    frame: fixture
+  }
+  target mid = pose_target {
+    pose: pose(550 mm, 50 mm, 0 mm, 0 deg, 0 deg, 0 deg)
+  }
+  target arc_end = pose_target {
+    pose: pose(600 mm, 0 mm, 0 mm, 0 deg, 0 deg, 0 deg)
+  }
+  proc main()
+    set_tool gripper
+    set_frame fixture
+    set_speed v_linear
+    set_zone z10
+    movej home speed v_joint zone fine
+    movel pick
+    movec via mid target arc_end speed linear(150 mm/s) zone fine
+  end
+end
+`;
+
+function declarations() {
+  return parseGrl(PROGRAM).module.declarations;
+}
+
+describe("GRL motion instruction compilation", () => {
+  it("parses movej, movel, and movec from procedure body tokens", () => {
+    const decls = declarations();
+    const context = buildMotionContext(
+      decls.filter(
+        (decl): decl is GrlDataDeclaration | GrlTargetDeclaration =>
+          decl.kind === "DataDeclaration" || decl.kind === "TargetDeclaration"
+      )
+    );
+    const procedure = decls.find((decl): decl is GrlProcedureDeclaration => decl.kind === "ProcedureDeclaration")!;
+    const instructions = parseProcedureMotionInstructions(procedure, context);
+
+    expect(instructions.map((instruction) => instruction.kind)).toEqual(["MOVEJ", "MOVEL", "MOVEC"]);
+    expect(instructions[0]).toMatchObject({
+      kind: "MOVEJ",
+      speed: { kind: "joint_percent", value: 0.6 },
+      zone: { kind: "fine" },
+      target: { joints: [0, 0] },
+      sourceMap: { line: 32 }
+    });
+    expect(instructions[1]).toMatchObject({
+      kind: "MOVEL",
+      speed: { kind: "linear", velocity: 0.3 },
+      zone: { kind: "distance", value: 0.01 }
+    });
+    expect(instructions[1]?.tool?.position).toEqual([0, 0, 0.1]);
+    expect(instructions[1]?.frame?.position).toEqual([0.8, 0, 0]);
+    expect(instructions[2]).toMatchObject({
+      kind: "MOVEC",
+      speed: { kind: "linear", velocity: 0.15 },
+      zone: { kind: "fine" }
+    });
+  });
+
+  it("compiles motion instructions to KDL request shapes", () => {
+    const decls = declarations();
+    const context = buildMotionContext(
+      decls.filter(
+        (decl): decl is GrlDataDeclaration | GrlTargetDeclaration =>
+          decl.kind === "DataDeclaration" || decl.kind === "TargetDeclaration"
+      )
+    );
+    const procedure = decls.find((decl): decl is GrlProcedureDeclaration => decl.kind === "ProcedureDeclaration")!;
+    const [movej, movel, movec] = parseProcedureMotionInstructions(procedure, context);
+
+    expect(compileMotionToKdlRequest(movej!, { startJoints: [0, 0], sampleTime: 0.004 })).toMatchObject({
+      startJoints: [0, 0],
+      target: { joints: [0, 0] },
+      speed: { kind: "joint_percent", value: 0.6 },
+      zone: { kind: "fine" },
+      sampleTime: 0.004
+    });
+
+    expect(compileMotionToKdlRequest(movel!, { startJoints: [0, 0], sampleTime: 0.004 })).toMatchObject({
+      startJoints: [0, 0],
+      target: { pose: { position: [0.5, 0, 0] } },
+      speed: { kind: "linear", velocity: 0.3 },
+      zone: { kind: "distance", value: 0.01 },
+      tool: { position: [0, 0, 0.1] },
+      frame: { position: [0.8, 0, 0] },
+      sampleTime: 0.004
+    });
+
+    expect(compileMotionToKdlRequest(movec!, { startJoints: [0, 0], sampleTime: 0.004 })).toMatchObject({
+      startJoints: [0, 0],
+      via: { pose: { position: [0.55, 0.05, 0] } },
+      target: { pose: { position: [0.6, 0, 0] } },
+      speed: { kind: "linear", velocity: 0.15 },
+      zone: { kind: "fine" },
+      sampleTime: 0.004
+    });
+  });
+});