Files
robot-assembly/scripts/verify-urdf-positions.mjs

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();