同步KDL工程源码到云仓库

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

@@ -0,0 +1,151 @@
+import { describe, expect, it } from "vitest";
+import type { GrlDataDeclaration, GrlTargetDeclaration } from "../../src/grl/ast/index.js";
+import { parseGrl } from "../../src/grl/parser/index.js";
+import {
+  compileGrlDataDeclaration,
+  compileGrlTargetDeclaration,
+  compileOffsetExpression
+} from "../../src/grl/semantic/index.js";
+
+function dataDeclarations(source: string): GrlDataDeclaration[] {
+  return parseGrl(source).module.declarations.filter(
+    (declaration): declaration is GrlDataDeclaration => declaration.kind === "DataDeclaration"
+  );
+}
+
+function targetDeclarations(source: string): GrlTargetDeclaration[] {
+  return parseGrl(source).module.declarations.filter(
+    (declaration): declaration is GrlTargetDeclaration => declaration.kind === "TargetDeclaration"
+  );
+}
+
+describe("GRL data declarations and target compilation", () => {
+  it("compiles tool and frame declarations into shared structures", () => {
+    const [toolDecl, frameDecl] = dataDeclarations(`language grl 0.1
+module Main
+  persistent tool gripper = tool {
+    tcp: pose(0 mm, 0 mm, 180 mm, 0 deg, 0 deg, 0 deg),
+    mass: 2.5 kg,
+    cog: [0 mm, 0 mm, 80 mm]
+  }
+  persistent frame fixture = frame {
+    origin: pose(800 mm, 0 mm, 200 mm, 0 deg, 0 deg, 0 deg)
+  }
+end
+`);
+
+    expect(toolDecl).toMatchObject({
+      storage: "persistent",
+      typeName: "tool",
+      name: "gripper",
+      initializer: { kind: "ObjectExpression", typeName: "tool" }
+    });
+    expect(compileGrlDataDeclaration(toolDecl!)).toMatchObject({
+      name: "gripper",
+      value: {
+        tcp: {
+          position: [0, 0, 0.18],
+          quaternion: [0, 0, 0, 1]
+        },
+        mass: 2.5,
+        cog: [0, 0, 0.08]
+      }
+    });
+    expect(compileGrlDataDeclaration(frameDecl!)).toMatchObject({
+      name: "fixture",
+      value: {
+        origin: {
+          position: [0.8, 0, 0.2],
+          quaternion: [0, 0, 0, 1]
+        }
+      }
+    });
+  });
+
+  it("compiles speed and zone declarations", () => {
+    const declarations = dataDeclarations(`language grl 0.1
+module Main
+  const speed v_joint = joint(80 %)
+  const speed v_pick = linear(300 mm/s)
+  const speed v_slow = linear(100 mm/s, acc 500 mm/s2)
+  const zone z_fine = fine
+  const zone z10 = z(10 mm)
+  const zone z_cnt = cnt(30)
+  const zone z_cont = continuous
+end
+`);
+    const compiled = declarations.map(compileGrlDataDeclaration);
+
+    expect(compiled).toMatchObject([
+      { name: "v_joint", value: { kind: "joint_percent", value: 0.8 } },
+      { name: "v_pick", value: { kind: "linear", velocity: 0.3 } },
+      { name: "v_slow", value: { kind: "linear", velocity: 0.1, acceleration: 0.5 } },
+      { name: "z_fine", value: { kind: "fine" } },
+      { name: "z10", value: { kind: "distance", value: 0.01 } },
+      { name: "z_cnt", value: { kind: "cnt", value: 30 } },
+      { name: "z_cont", value: { kind: "continuous" } }
+    ]);
+  });
+
+  it("compiles joint_target and pose_target declarations", () => {
+    const [home, pick] = targetDeclarations(`language grl 0.1
+module Main
+  target home = joint_target {
+    joints: [0 deg, -30 deg, 60 deg, 0 deg, 60 deg, 0 deg]
+  }
+  target pick = pose_target {
+    pose: pose(500 mm, 120 mm, 300 mm, 180 deg, 0 deg, 90 deg),
+    config: robot_config(0, 0, 1),
+    tool: gripper,
+    frame: fixture
+  }
+end
+`);
+
+    expect(compileGrlTargetDeclaration(home!)).toMatchObject({
+      name: "home",
+      target: {
+        joints: [0, -Math.PI / 6, Math.PI / 3, 0, Math.PI / 3, 0]
+      }
+    });
+
+    const compiledPick = compileGrlTargetDeclaration(pick!);
+    expect(compiledPick.name).toBe("pick");
+    expect("pose" in compiledPick.target).toBe(true);
+    if ("pose" in compiledPick.target) {
+      expect(compiledPick.target.pose.position).toEqual([0.5, 0.12, 0.3]);
+      expect(compiledPick.target.config).toEqual({ shoulder: 0, elbow: 0, wrist: 1 });
+    }
+  });
+
+  it("parses and compiles offset expressions", () => {
+    const [declFrame, declTool] = dataDeclarations(`language grl 0.1
+module Main
+  var pose_target p2 = pick offset x 20 mm y -10 mm z 50 mm
+  var pose_target p3 = pick offset_in tool z -50 mm
+end
+`);
+
+    expect(declFrame?.initializer).toMatchObject({
+      kind: "OffsetExpression",
+      mode: "frame",
+      axes: [
+        { axis: "x" },
+        { axis: "y" },
+        { axis: "z" }
+      ]
+    });
+    if (declFrame?.initializer.kind === "OffsetExpression") {
+      expect(compileOffsetExpression(declFrame.initializer)).toEqual({
+        mode: "frame",
+        xyz: [0.02, -0.01, 0.05]
+      });
+    }
+    if (declTool?.initializer.kind === "OffsetExpression") {
+      expect(compileOffsetExpression(declTool.initializer)).toEqual({
+        mode: "tool",
+        xyz: [0, 0, -0.05]
+      });
+    }
+  });
+});