481 lines
14 KiB
JavaScript
481 lines
14 KiB
JavaScript
const fs = require("fs");
|
|
const path = require("path");
|
|
|
|
const rootDir = path.resolve(__dirname, "..");
|
|
const defaultModulePath = path.join(rootDir, "public", "wasm", "smart_math.js");
|
|
|
|
function readJson(relativePath) {
|
|
const filePath = path.join(rootDir, relativePath);
|
|
return JSON.parse(fs.readFileSync(filePath, "utf8"));
|
|
}
|
|
|
|
function ensure(condition, message) {
|
|
if (!condition) {
|
|
throw new Error(message);
|
|
}
|
|
}
|
|
|
|
function getByPath(value, pathExpression) {
|
|
const parts = pathExpression.split(".");
|
|
let current = value;
|
|
|
|
for (const part of parts) {
|
|
if (current === undefined || current === null) {
|
|
return undefined;
|
|
}
|
|
|
|
if (/^\d+$/.test(part)) {
|
|
current = current[Number(part)];
|
|
} else {
|
|
current = current[part];
|
|
}
|
|
}
|
|
|
|
return current;
|
|
}
|
|
|
|
function jointArrayToString(joints) {
|
|
return joints.join(",");
|
|
}
|
|
|
|
function poseToString(position, orientation) {
|
|
return [...position, ...orientation].join(",");
|
|
}
|
|
|
|
function positionDistance(positionA, positionB) {
|
|
const dx = positionA[0] - positionB[0];
|
|
const dy = positionA[1] - positionB[1];
|
|
const dz = positionA[2] - positionB[2];
|
|
return Math.sqrt(dx * dx + dy * dy + dz * dz);
|
|
}
|
|
|
|
function quaternionDistance(q1, q2) {
|
|
const dot = Math.abs(
|
|
q1[0] * q2[0] +
|
|
q1[1] * q2[1] +
|
|
q1[2] * q2[2] +
|
|
q1[3] * q2[3]
|
|
);
|
|
return 1 - Math.min(1, dot);
|
|
}
|
|
|
|
async function loadModule(modulePath) {
|
|
const required = require(modulePath);
|
|
|
|
if (typeof required === "function") {
|
|
return required({
|
|
noInitialRun: true,
|
|
locateFile: (fileName) => path.join(path.dirname(modulePath), fileName),
|
|
});
|
|
}
|
|
|
|
if (required && typeof required === "object") {
|
|
return required;
|
|
}
|
|
|
|
throw new Error(`Unsupported WASM loader type: ${typeof required}`);
|
|
}
|
|
|
|
function callJsonFunction(module, exportName, payload) {
|
|
const requestText = typeof payload === "string" ? payload : JSON.stringify(payload);
|
|
const requestPtr = module.allocateUTF8(requestText);
|
|
let responsePtr = 0;
|
|
|
|
try {
|
|
responsePtr = module[exportName](requestPtr);
|
|
const responseText = module.UTF8ToString(responsePtr);
|
|
return JSON.parse(responseText);
|
|
} finally {
|
|
if (responsePtr) {
|
|
module._smart_free_string(responsePtr);
|
|
}
|
|
module._free(requestPtr);
|
|
}
|
|
}
|
|
|
|
function callNoArgJsonFunction(module, exportName) {
|
|
const responsePtr = module[exportName]();
|
|
try {
|
|
const responseText = module.UTF8ToString(responsePtr);
|
|
return JSON.parse(responseText);
|
|
} finally {
|
|
module._smart_free_string(responsePtr);
|
|
}
|
|
}
|
|
|
|
function callBusinessApi(module, request) {
|
|
return callJsonFunction(module, "_func", request);
|
|
}
|
|
|
|
function buildForwardRequest(robotUuid, joints) {
|
|
return {
|
|
msg: "fk roundtrip",
|
|
req_code: "AUTO_FK",
|
|
req_from: "wasm_test",
|
|
req_cmd: "Cmd_Kinematics_forward_pose_str",
|
|
req_param: {
|
|
robot_uuid: robotUuid,
|
|
q_init_str: jointArrayToString(joints),
|
|
},
|
|
};
|
|
}
|
|
|
|
function buildInverseRequest(command, robotUuid, poseStr, qInitStr, extra = {}) {
|
|
return {
|
|
msg: "ik roundtrip",
|
|
req_code: "AUTO_IK",
|
|
req_from: "wasm_test",
|
|
req_cmd: command,
|
|
req_param: {
|
|
robot_uuid: robotUuid,
|
|
pose_str: poseStr,
|
|
q_init_str: qInitStr,
|
|
...extra,
|
|
},
|
|
};
|
|
}
|
|
|
|
function assertStaticCase(response, assertions) {
|
|
for (const assertion of assertions) {
|
|
const actual = getByPath(response, assertion.path);
|
|
|
|
if (assertion.equals !== undefined) {
|
|
ensure(
|
|
actual === assertion.equals,
|
|
`${assertion.path} expected ${JSON.stringify(assertion.equals)}, got ${JSON.stringify(actual)}`
|
|
);
|
|
}
|
|
|
|
if (assertion.exists) {
|
|
ensure(actual !== undefined && actual !== null, `${assertion.path} is missing`);
|
|
}
|
|
|
|
if (assertion.gte !== undefined) {
|
|
ensure(actual >= assertion.gte, `${assertion.path} expected >= ${assertion.gte}, got ${actual}`);
|
|
}
|
|
|
|
if (assertion.lengthEquals !== undefined) {
|
|
ensure(Array.isArray(actual), `${assertion.path} is not an array`);
|
|
ensure(
|
|
actual.length === assertion.lengthEquals,
|
|
`${assertion.path} expected length ${assertion.lengthEquals}, got ${actual.length}`
|
|
);
|
|
}
|
|
|
|
if (assertion.lengthGte !== undefined) {
|
|
ensure(Array.isArray(actual), `${assertion.path} is not an array`);
|
|
ensure(
|
|
actual.length >= assertion.lengthGte,
|
|
`${assertion.path} expected length >= ${assertion.lengthGte}, got ${actual.length}`
|
|
);
|
|
}
|
|
|
|
if (assertion.includes !== undefined) {
|
|
ensure(
|
|
String(actual).includes(assertion.includes),
|
|
`${assertion.path} expected to include ${assertion.includes}, got ${actual}`
|
|
);
|
|
}
|
|
}
|
|
}
|
|
|
|
async function runStaticCase(module, testCase) {
|
|
const request = readJson(testCase.requestFile);
|
|
const response = callBusinessApi(module, request);
|
|
assertStaticCase(response, testCase.assertions);
|
|
return response;
|
|
}
|
|
|
|
async function runRoundtripSingleCase(module, testCase) {
|
|
const seed = readJson(testCase.seedFile);
|
|
const forwardResponse = callBusinessApi(module, buildForwardRequest(seed.robot_uuid, seed.target_joints));
|
|
|
|
ensure(forwardResponse.success === true, "forward request failed");
|
|
ensure(getByPath(forwardResponse, "res_data.success") === true, "forward result is not successful");
|
|
|
|
const forwardPosition = getByPath(forwardResponse, "res_data.position");
|
|
const forwardOrientation = getByPath(forwardResponse, "res_data.orientation");
|
|
const poseStr = poseToString(forwardPosition, forwardOrientation);
|
|
|
|
const inverseResponse = callBusinessApi(
|
|
module,
|
|
buildInverseRequest(
|
|
"Cmd_Kinematics_inverse_pose_str_NoDifference",
|
|
seed.robot_uuid,
|
|
poseStr,
|
|
seed.q_init_str
|
|
)
|
|
);
|
|
|
|
ensure(inverseResponse.success === true, "inverse request failed");
|
|
ensure(getByPath(inverseResponse, "res_data.success") === true, "inverse result is not successful");
|
|
|
|
const jointSolutions = getByPath(inverseResponse, "res_data.joints");
|
|
ensure(Array.isArray(jointSolutions) && jointSolutions.length >= 1, "inverse result contains no joint solution");
|
|
|
|
const finalJoints = jointSolutions[jointSolutions.length - 1];
|
|
const forwardCheck = callBusinessApi(module, buildForwardRequest(seed.robot_uuid, finalJoints));
|
|
|
|
ensure(forwardCheck.success === true, "forward validation request failed");
|
|
ensure(getByPath(forwardCheck, "res_data.success") === true, "forward validation result is not successful");
|
|
|
|
const checkedPosition = getByPath(forwardCheck, "res_data.position");
|
|
const checkedOrientation = getByPath(forwardCheck, "res_data.orientation");
|
|
|
|
const posDiff = positionDistance(forwardPosition, checkedPosition);
|
|
const quatDiff = quaternionDistance(forwardOrientation, checkedOrientation);
|
|
|
|
ensure(
|
|
posDiff <= seed.position_tolerance,
|
|
`roundtrip position diff ${posDiff} exceeds tolerance ${seed.position_tolerance}`
|
|
);
|
|
ensure(
|
|
quatDiff <= seed.orientation_tolerance,
|
|
`roundtrip quaternion diff ${quatDiff} exceeds tolerance ${seed.orientation_tolerance}`
|
|
);
|
|
|
|
return {
|
|
response: inverseResponse,
|
|
details: {
|
|
positionDiff: posDiff,
|
|
quaternionDiff: quatDiff,
|
|
solutionCount: jointSolutions.length,
|
|
},
|
|
};
|
|
}
|
|
|
|
async function runRoundtripPathCase(module, testCase) {
|
|
const seed = readJson(testCase.seedFile);
|
|
|
|
const startForward = callBusinessApi(module, buildForwardRequest(seed.robot_uuid, seed.start_joints));
|
|
const endForward = callBusinessApi(module, buildForwardRequest(seed.robot_uuid, seed.end_joints));
|
|
|
|
const startPose = poseToString(
|
|
getByPath(startForward, "res_data.position"),
|
|
getByPath(startForward, "res_data.orientation")
|
|
);
|
|
const endPose = poseToString(
|
|
getByPath(endForward, "res_data.position"),
|
|
getByPath(endForward, "res_data.orientation")
|
|
);
|
|
|
|
const inverseResponse = callBusinessApi(
|
|
module,
|
|
buildInverseRequest(
|
|
"Cmd_Kinematics_inverse_pose_str_2PSteps",
|
|
seed.robot_uuid,
|
|
`${startPose};${endPose}`,
|
|
seed.q_init_str,
|
|
{ steps_str: String(seed.steps) }
|
|
)
|
|
);
|
|
|
|
ensure(inverseResponse.success === true, "path inverse request failed");
|
|
ensure(getByPath(inverseResponse, "res_data.success") === true, "path inverse result is not successful");
|
|
|
|
const jointSolutions = getByPath(inverseResponse, "res_data.joints");
|
|
ensure(Array.isArray(jointSolutions), "path inverse result is not an array");
|
|
ensure(
|
|
jointSolutions.length === seed.steps,
|
|
`path inverse expected ${seed.steps} solutions, got ${jointSolutions.length}`
|
|
);
|
|
|
|
const firstForwardCheck = callBusinessApi(module, buildForwardRequest(seed.robot_uuid, jointSolutions[0]));
|
|
const lastForwardCheck = callBusinessApi(
|
|
module,
|
|
buildForwardRequest(seed.robot_uuid, jointSolutions[jointSolutions.length - 1])
|
|
);
|
|
|
|
const startPosDiff = positionDistance(
|
|
getByPath(startForward, "res_data.position"),
|
|
getByPath(firstForwardCheck, "res_data.position")
|
|
);
|
|
const endPosDiff = positionDistance(
|
|
getByPath(endForward, "res_data.position"),
|
|
getByPath(lastForwardCheck, "res_data.position")
|
|
);
|
|
|
|
ensure(
|
|
startPosDiff <= seed.position_tolerance,
|
|
`path start position diff ${startPosDiff} exceeds tolerance ${seed.position_tolerance}`
|
|
);
|
|
ensure(
|
|
endPosDiff <= seed.position_tolerance,
|
|
`path end position diff ${endPosDiff} exceeds tolerance ${seed.position_tolerance}`
|
|
);
|
|
|
|
return {
|
|
response: inverseResponse,
|
|
details: {
|
|
solutionCount: jointSolutions.length,
|
|
startPositionDiff: startPosDiff,
|
|
endPositionDiff: endPosDiff,
|
|
},
|
|
};
|
|
}
|
|
|
|
const suite = [
|
|
{
|
|
id: "list_robots_after_init",
|
|
type: "static",
|
|
requestFile: "tests/testdata/kinematics/list_robots.json",
|
|
assertions: [
|
|
{ path: "success", equals: true },
|
|
{ path: "res_data.success", equals: true },
|
|
{ path: "res_data.count", gte: 2 },
|
|
],
|
|
},
|
|
{
|
|
id: "kinematics_forward_zero",
|
|
type: "static",
|
|
requestFile: "tests/testdata/kinematics/forward_zero.json",
|
|
assertions: [
|
|
{ path: "success", equals: true },
|
|
{ path: "res_data.success", equals: true },
|
|
{ path: "res_data.position", lengthEquals: 3 },
|
|
{ path: "res_data.orientation", lengthEquals: 4 },
|
|
],
|
|
},
|
|
{
|
|
id: "kinematics_forward_all_joints_path",
|
|
type: "static",
|
|
requestFile: "tests/testdata/kinematics/forward_all_joints_path.json",
|
|
assertions: [
|
|
{ path: "success", equals: true },
|
|
{ path: "res_data.success", equals: true },
|
|
{ path: "res_data.OPERATION.frames", lengthEquals: 2 },
|
|
],
|
|
},
|
|
{
|
|
id: "spc_basic_5x30",
|
|
type: "static",
|
|
requestFile: "tests/testdata/spc/basic_5x30.json",
|
|
assertions: [
|
|
{ path: "success", equals: true },
|
|
{ path: "res_data.XR.n", equals: 5 },
|
|
{ path: "res_data.XR.k", equals: 30 },
|
|
{ path: "res_data.Cpk.USL", equals: 1.7 },
|
|
{ path: "res_data.Cpk.LSL", equals: 1.5 },
|
|
{ path: "res_data.Cpk.Cpk", gte: 0.1 },
|
|
],
|
|
},
|
|
{
|
|
id: "fourbar_valid",
|
|
type: "static",
|
|
requestFile: "tests/testdata/fourbar/crank_slider_valid.json",
|
|
assertions: [
|
|
{ path: "success", equals: true },
|
|
{ path: "res_data.success", equals: true },
|
|
{ path: "res_data.points.B.x", exists: true },
|
|
{ path: "res_data.trajectory", lengthGte: 1 },
|
|
{ path: "res_data.slider_trajectory", lengthGte: 1 },
|
|
],
|
|
},
|
|
{
|
|
id: "fourbar_invalid",
|
|
type: "static",
|
|
requestFile: "tests/testdata/fourbar/crank_slider_invalid.json",
|
|
assertions: [
|
|
{ path: "success", equals: true },
|
|
{ path: "res_data.success", equals: false },
|
|
{ path: "res_data.error", includes: "Invalid parameters" },
|
|
{ path: "res_data.validation_errors", lengthGte: 1 },
|
|
],
|
|
},
|
|
{
|
|
id: "quadruped_points_from_motor_angles",
|
|
type: "static",
|
|
requestFile: "tests/testdata/quadruped/calculate_points_from_motor_angles.json",
|
|
assertions: [
|
|
{ path: "success", equals: true },
|
|
{ path: "res_data.objStates", lengthGte: 20 },
|
|
],
|
|
},
|
|
{
|
|
id: "quadruped_forward_gait",
|
|
type: "static",
|
|
requestFile: "tests/testdata/quadruped/perform_forward_kinematics.json",
|
|
assertions: [
|
|
{ path: "success", equals: true },
|
|
{ path: "res_data.GaitInfo.TotalFrames", equals: 40 },
|
|
{ path: "res_data.ConstraintData.AB1_AB_Constraint.IsValid", equals: true },
|
|
],
|
|
},
|
|
{
|
|
id: "ik_roundtrip_single_pose",
|
|
type: "roundtrip_single",
|
|
seedFile: "tests/testdata/kinematics/roundtrip_single_seed.json",
|
|
},
|
|
{
|
|
id: "ik_roundtrip_two_pose_path",
|
|
type: "roundtrip_path",
|
|
seedFile: "tests/testdata/kinematics/roundtrip_path_seed.json",
|
|
},
|
|
];
|
|
|
|
async function runCase(module, testCase) {
|
|
switch (testCase.type) {
|
|
case "static":
|
|
return runStaticCase(module, testCase);
|
|
case "roundtrip_single":
|
|
return runRoundtripSingleCase(module, testCase);
|
|
case "roundtrip_path":
|
|
return runRoundtripPathCase(module, testCase);
|
|
default:
|
|
throw new Error(`Unknown test type: ${testCase.type}`);
|
|
}
|
|
}
|
|
|
|
async function main() {
|
|
const modulePath = path.resolve(process.argv[2] || defaultModulePath);
|
|
ensure(fs.existsSync(modulePath), `WASM JS loader not found: ${modulePath}`);
|
|
|
|
console.log(`Loading module: ${modulePath}`);
|
|
const module = await loadModule(modulePath);
|
|
|
|
ensure(typeof module._init_func === "function", "_init_func is not exported");
|
|
ensure(typeof module._func === "function", "_func is not exported");
|
|
ensure(typeof module.allocateUTF8 === "function", "allocateUTF8 is not exported");
|
|
ensure(typeof module.UTF8ToString === "function", "UTF8ToString is not exported");
|
|
|
|
const initResponse = callNoArgJsonFunction(module, "_init_func");
|
|
ensure(initResponse.success === true, `init_func failed: ${JSON.stringify(initResponse)}`);
|
|
|
|
console.log("Initialization complete.");
|
|
|
|
let passed = 0;
|
|
const failures = [];
|
|
|
|
for (const testCase of suite) {
|
|
try {
|
|
const result = await runCase(module, testCase);
|
|
passed += 1;
|
|
|
|
if (result && result.details) {
|
|
console.log(`[PASS] ${testCase.id} ${JSON.stringify(result.details)}`);
|
|
} else {
|
|
console.log(`[PASS] ${testCase.id}`);
|
|
}
|
|
} catch (error) {
|
|
failures.push({ id: testCase.id, message: error.message });
|
|
console.error(`[FAIL] ${testCase.id}: ${error.message}`);
|
|
}
|
|
}
|
|
|
|
console.log("");
|
|
console.log(`Result: ${passed}/${suite.length} passed`);
|
|
|
|
if (failures.length > 0) {
|
|
console.log("Failed cases:");
|
|
for (const failure of failures) {
|
|
console.log(`- ${failure.id}: ${failure.message}`);
|
|
}
|
|
process.exitCode = 1;
|
|
}
|
|
}
|
|
|
|
main().catch((error) => {
|
|
console.error(error);
|
|
process.exitCode = 1;
|
|
});
|