256 lines
9.5 KiB
JavaScript
256 lines
9.5 KiB
JavaScript
/**
|
|
* 验证脚本:把生成的 full URDF 里每个 visual 还原回"世界 pose"(Scene 系下),
|
|
* 跟 assembly.json 里该零件的 pos 对比,列出差异 > 1mm 的 visual。
|
|
*
|
|
* - 如果所有 visual 都吻合 → URDF 写得对,问题在 urdf-loader/GLTFLoader 加载层
|
|
* - 如果有差异 → build-full-urdf 数学有 bug
|
|
*/
|
|
|
|
import { readFileSync } from 'node:fs';
|
|
import { dirname, resolve } from 'node:path';
|
|
import { fileURLToPath } from 'node:url';
|
|
import * as THREE from 'three';
|
|
|
|
const __dirname = dirname(fileURLToPath(import.meta.url));
|
|
const PROJECT_ROOT = resolve(__dirname, '..');
|
|
const FULL_URDF = resolve(PROJECT_ROOT, 'src/assets/O6/urdf/linkerhand_o6_left.full.urdf');
|
|
const ASSEMBLY_PATH = resolve(PROJECT_ROOT, 'src/data/O6/assembly.json');
|
|
|
|
function parsePos(str) { return str.split(',').map(Number); }
|
|
|
|
// ===================================================================
|
|
// 解析 full URDF —— 它本身已经在 Scene Y-up 系,直接相乘
|
|
// ===================================================================
|
|
|
|
function parseUrdf(xml) {
|
|
const links = new Map();
|
|
// 同时收集每个 link 的 visual 列表
|
|
const linkVisuals = new Map();
|
|
|
|
// 匹配每个 <link name="X"> ... </link>
|
|
for (const lm of xml.matchAll(/<link\s+name="([^"]+)">([\s\S]*?)<\/link>/g)) {
|
|
const linkName = lm[1];
|
|
const body = lm[2];
|
|
links.set(linkName, { name: linkName, parent: null, joint: null });
|
|
const visuals = [];
|
|
for (const vm of body.matchAll(/<visual>([\s\S]*?)<\/visual>/g)) {
|
|
const vb = vm[1];
|
|
const om = /<origin\s+xyz="([^"]+)"\s+rpy="([^"]+)"/.exec(vb);
|
|
const mm = /<mesh\s+filename="([^"]+)"/.exec(vb);
|
|
if (!om || !mm) continue;
|
|
visuals.push({
|
|
xyz: om[1].trim().split(/\s+/).map(Number),
|
|
rpy: om[2].trim().split(/\s+/).map(Number),
|
|
mesh: mm[1],
|
|
});
|
|
}
|
|
linkVisuals.set(linkName, visuals);
|
|
}
|
|
|
|
// 匹配 joints
|
|
for (const jm of xml.matchAll(/<joint\s+name="([^"]+)"\s+type="([^"]+)">([\s\S]*?)<\/joint>/g)) {
|
|
const body = jm[3];
|
|
const parent = /<parent\s+link="([^"]+)"/.exec(body)?.[1];
|
|
const child = /<child\s+link="([^"]+)"/.exec(body)?.[1];
|
|
const om = /<origin\s+xyz="([^"]+)"\s+rpy="([^"]+)"/.exec(body);
|
|
if (!parent || !child || !om) continue;
|
|
const node = links.get(child);
|
|
if (!node) continue;
|
|
node.parent = parent;
|
|
node.joint = {
|
|
xyz: om[1].trim().split(/\s+/).map(Number),
|
|
rpy: om[2].trim().split(/\s+/).map(Number),
|
|
};
|
|
}
|
|
|
|
return { links, linkVisuals };
|
|
}
|
|
|
|
function computeWorldMatrices(links) {
|
|
const cache = new Map();
|
|
function world(name) {
|
|
if (cache.has(name)) return cache.get(name);
|
|
const node = links.get(name);
|
|
if (!node) throw new Error(`unknown link ${name}`);
|
|
if (!node.parent) {
|
|
cache.set(name, new THREE.Matrix4().identity());
|
|
return cache.get(name);
|
|
}
|
|
const parentMat = world(node.parent).clone();
|
|
const local = new THREE.Matrix4().compose(
|
|
new THREE.Vector3(...node.joint.xyz),
|
|
new THREE.Quaternion().setFromEuler(new THREE.Euler(...node.joint.rpy, 'ZYX')),
|
|
new THREE.Vector3(1, 1, 1),
|
|
);
|
|
cache.set(name, parentMat.multiply(local));
|
|
return cache.get(name);
|
|
}
|
|
for (const name of links.keys()) world(name);
|
|
return cache;
|
|
}
|
|
|
|
// ===================================================================
|
|
// 从 assembly.json 拿零件世界 pose(Scene 系下,即 GLB 拼凑模式下的真实位置)
|
|
// ===================================================================
|
|
|
|
// 链 1 phase 的零件 assemblypos 是"4 指主体本地系",最终世界位置要 + 合装偏移
|
|
const CHAIN1_PHASES = new Set([
|
|
'phase-法兰安装', 'phase-后外壳安装', 'phase-PCB安装',
|
|
'phase-食指安装', 'phase-中指安装', 'phase-无名指安装', 'phase-小拇指安装',
|
|
]);
|
|
const COMBINE_OFFSET = [-0.000776827, -0.047601270, -0.009744326];
|
|
|
|
function collectAssemblyPoses(assembly) {
|
|
const out = [];
|
|
const seenAtomic = new Set();
|
|
const isAtomic = (n) => /^0?\d+$/.test(n);
|
|
for (const phaseObj of assembly.AllStep) {
|
|
const [phaseName, body] = Object.entries(phaseObj)[0];
|
|
const offset = CHAIN1_PHASES.has(phaseName) ? COMBINE_OFFSET : [0, 0, 0];
|
|
for (const ps of body.phases ?? []) {
|
|
for (const sub of ps.subphase ?? []) {
|
|
const pn = sub.part;
|
|
if (pn && isAtomic(pn) && !seenAtomic.has(pn) && Array.isArray(sub.pose) && sub.pose.length > 0) {
|
|
seenAtomic.add(pn);
|
|
const ap = sub.pose[0];
|
|
const pos = parsePos(ap.pos);
|
|
out.push({ phase: phaseName, part: pn, index: ap.index, pos: [pos[0]+offset[0], pos[1]+offset[1], pos[2]+offset[2]], rot: parsePos(ap.rot) });
|
|
}
|
|
if (!sub.subpart || sub.action === 'line') continue;
|
|
for (const ap of sub.assemblypos ?? []) {
|
|
const pos = parsePos(ap.pos);
|
|
out.push({ phase: phaseName, part: sub.subpart, index: ap.index, pos: [pos[0]+offset[0], pos[1]+offset[1], pos[2]+offset[2]], rot: parsePos(ap.rot) });
|
|
}
|
|
}
|
|
}
|
|
}
|
|
return out;
|
|
}
|
|
|
|
// ===================================================================
|
|
// 主流程
|
|
// ===================================================================
|
|
|
|
function main() {
|
|
const xml = readFileSync(FULL_URDF, 'utf-8');
|
|
const assembly = JSON.parse(readFileSync(ASSEMBLY_PATH, 'utf-8'));
|
|
const { links, linkVisuals } = parseUrdf(xml);
|
|
const worldMats = computeWorldMatrices(links);
|
|
|
|
// 对每个 visual,算它的世界 pose
|
|
/** @type {Array<{ link: string; part: string; worldPos: number[]; worldRpy: number[] }>} */
|
|
const urdfVisuals = [];
|
|
for (const [linkName, visuals] of linkVisuals) {
|
|
const linkWorld = worldMats.get(linkName);
|
|
for (const v of visuals) {
|
|
const local = new THREE.Matrix4().compose(
|
|
new THREE.Vector3(...v.xyz),
|
|
new THREE.Quaternion().setFromEuler(new THREE.Euler(...v.rpy, 'ZYX')),
|
|
new THREE.Vector3(1, 1, 1),
|
|
);
|
|
const world = new THREE.Matrix4().multiplyMatrices(linkWorld, local);
|
|
const p = new THREE.Vector3();
|
|
const q = new THREE.Quaternion();
|
|
world.decompose(p, q, new THREE.Vector3());
|
|
const e = new THREE.Euler().setFromQuaternion(q, 'XYZ');
|
|
// URDF 是 ROS Z-up,assembly.json 是 Scene Y-up。转 Scene 系比较:
|
|
// ROS (x,y,z) → Scene (y,z,x)
|
|
const part = v.mesh.match(/([^/\\]+?)\.glb$/i)?.[1] ?? '';
|
|
urdfVisuals.push({
|
|
link: linkName,
|
|
part,
|
|
worldPos: [p.y, p.z, p.x],
|
|
worldRpy: [e.x, e.y, e.z],
|
|
});
|
|
}
|
|
}
|
|
|
|
// 跟 assembly.json 对比
|
|
const assemPoses = collectAssemblyPoses(assembly);
|
|
console.log(`URDF visuals: ${urdfVisuals.length}, assembly placements: ${assemPoses.length}`);
|
|
console.log('');
|
|
|
|
// 用 (part, index) 匹配。同名零件多次出现按出现顺序匹配
|
|
const assemByPart = new Map();
|
|
for (const ap of assemPoses) {
|
|
if (!assemByPart.has(ap.part)) assemByPart.set(ap.part, []);
|
|
assemByPart.get(ap.part).push(ap);
|
|
}
|
|
const consumed = new Map(); // part → 已消费的 idx
|
|
|
|
let okCount = 0;
|
|
let mismatchCount = 0;
|
|
const mismatches = [];
|
|
const missing = [];
|
|
for (const u of urdfVisuals) {
|
|
const candidates = assemByPart.get(u.part);
|
|
if (!candidates) {
|
|
missing.push(u);
|
|
continue;
|
|
}
|
|
// 找未消费的最近的(world pos 最接近)
|
|
const used = consumed.get(u.part) ?? new Set();
|
|
let best = -1, bestDist = Infinity;
|
|
for (let i = 0; i < candidates.length; i++) {
|
|
if (used.has(i)) continue;
|
|
const a = candidates[i];
|
|
const d = Math.hypot(a.pos[0] - u.worldPos[0], a.pos[1] - u.worldPos[1], a.pos[2] - u.worldPos[2]);
|
|
if (d < bestDist) { bestDist = d; best = i; }
|
|
}
|
|
if (best === -1) {
|
|
missing.push(u);
|
|
continue;
|
|
}
|
|
used.add(best);
|
|
consumed.set(u.part, used);
|
|
const a = candidates[best];
|
|
const distMm = bestDist * 1000;
|
|
// rot 差异
|
|
const dRot = [
|
|
Math.abs(a.rot[0] - u.worldRpy[0]),
|
|
Math.abs(a.rot[1] - u.worldRpy[1]),
|
|
Math.abs(a.rot[2] - u.worldRpy[2]),
|
|
];
|
|
const rotMaxDeg = Math.max(...dRot) * (180 / Math.PI);
|
|
// rotation 差异预期非零:visual rpy 含 GLB→ROS 轴交换补偿(GLB Y-up → ROS Z-up),
|
|
// 跟 assembly.json 的 "原始 GLB 旋转" 差一个轴交换矩阵。只看 position 就够了。
|
|
if (distMm > 1.0) {
|
|
mismatches.push({
|
|
link: u.link,
|
|
part: u.part,
|
|
urdfWorld: u.worldPos.map((v) => +v.toFixed(4)),
|
|
assemWorld: a.pos.map((v) => +v.toFixed(4)),
|
|
urdfRpy: u.worldRpy.map((v) => +v.toFixed(3)),
|
|
assemRpy: a.rot.map((v) => +v.toFixed(3)),
|
|
distMm: +distMm.toFixed(2),
|
|
rotDeg: +rotMaxDeg.toFixed(2),
|
|
});
|
|
mismatchCount++;
|
|
} else {
|
|
okCount++;
|
|
}
|
|
}
|
|
|
|
console.log(`✅ OK (差异 ≤ 1mm/1°): ${okCount}`);
|
|
console.log(`⚠️ Mismatch (差异 > 1mm 或 > 1°): ${mismatchCount}`);
|
|
console.log(`❌ assembly.json 找不到对应: ${missing.length}`);
|
|
console.log('');
|
|
if (mismatches.length > 0) {
|
|
console.log('=== Mismatches (前 30) ===');
|
|
for (const m of mismatches.slice(0, 30)) {
|
|
console.log(` ${m.part.padEnd(6)} @ ${m.link.padEnd(28)} Δpos=${m.distMm}mm, Δrot=${m.rotDeg}°`);
|
|
console.log(` URDF→world=(${m.urdfWorld.join(', ')}) rpy=(${m.urdfRpy.join(', ')})`);
|
|
console.log(` assembly =(${m.assemWorld.join(', ')}) rpy=(${m.assemRpy.join(', ')})`);
|
|
}
|
|
}
|
|
if (missing.length > 0) {
|
|
console.log('');
|
|
console.log('=== Missing (前 10) ===');
|
|
for (const u of missing.slice(0, 10)) {
|
|
console.log(` ${u.part} @ ${u.link}`);
|
|
}
|
|
}
|
|
}
|
|
|
|
main();
|