360 lines
14 KiB
JavaScript
360 lines
14 KiB
JavaScript
/**
|
|
* 完成"拇指 URDF 反推 + 归类"一站式脚本:
|
|
* 1. 算拇指零件群的 PCA(几何中心 + 主轴 + 端点)
|
|
* 2. 反推 lh_thumb_cmc_yaw / lh_thumb_cmc_pitch / lh_thumb_ip 3 个 joint 的新 origin
|
|
* 3. 写出 linkerhand_o6_left.patched.urdf
|
|
* 4. 用 patched URDF 重新跑归类
|
|
* 5. 输出最终报告
|
|
*/
|
|
|
|
import { readFileSync, mkdirSync, writeFileSync } 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 SRC_URDF = resolve(PROJECT_ROOT, 'src/assets/O6/urdf/linkerhand_o6_left.urdf');
|
|
const PATCHED_URDF = resolve(PROJECT_ROOT, 'src/assets/O6/urdf/linkerhand_o6_left.patched.urdf');
|
|
const ASSEMBLY_PATH = resolve(PROJECT_ROOT, 'src/data/O6/assembly.json');
|
|
const OUT_DIR = resolve(__dirname, 'out');
|
|
|
|
// ===================================================================
|
|
// 通用工具
|
|
// ===================================================================
|
|
|
|
function parsePos(str) { return str.split(',').map(Number); }
|
|
function sceneToUrdf([sx, sy, sz]) { return [sz, sx, sy]; }
|
|
function urdfToScene([ux, uy, uz]) { return [uy, uz, ux]; }
|
|
|
|
function collectPlacements(assembly) {
|
|
const out = [];
|
|
for (const phaseObj of assembly.AllStep) {
|
|
const [phaseName, phaseBody] = Object.entries(phaseObj)[0];
|
|
for (const phaseStep of phaseBody.phases ?? []) {
|
|
for (const sub of phaseStep.subphase ?? []) {
|
|
if (!sub.subpart || sub.action === 'line') continue;
|
|
for (const ap of sub.assemblypos ?? []) {
|
|
out.push({
|
|
phase: phaseName,
|
|
step: sub.step ?? phaseStep.step ?? '',
|
|
part: sub.subpart,
|
|
index: ap.index,
|
|
pos: parsePos(ap.pos),
|
|
rot: parsePos(ap.rot),
|
|
});
|
|
}
|
|
}
|
|
}
|
|
}
|
|
return out;
|
|
}
|
|
|
|
// ===================================================================
|
|
// 1) 拇指 PCA
|
|
// ===================================================================
|
|
|
|
function pcaAnalyze(points) {
|
|
const n = points.length;
|
|
const center = [0, 0, 0];
|
|
for (const p of points) for (let i = 0; i < 3; i++) center[i] += p[i] / n;
|
|
const c = [[0,0,0],[0,0,0],[0,0,0]];
|
|
for (const p of points) {
|
|
const d = [p[0]-center[0], p[1]-center[1], p[2]-center[2]];
|
|
for (let i = 0; i < 3; i++) for (let j = 0; j < 3; j++) c[i][j] += d[i] * d[j];
|
|
}
|
|
for (let i = 0; i < 3; i++) for (let j = 0; j < 3; j++) c[i][j] /= n;
|
|
let v = [1, 0, 0];
|
|
for (let iter = 0; iter < 100; iter++) {
|
|
const nv = [
|
|
c[0][0]*v[0] + c[0][1]*v[1] + c[0][2]*v[2],
|
|
c[1][0]*v[0] + c[1][1]*v[1] + c[1][2]*v[2],
|
|
c[2][0]*v[0] + c[2][1]*v[1] + c[2][2]*v[2],
|
|
];
|
|
const len = Math.hypot(...nv);
|
|
v = nv.map((x) => x / len);
|
|
}
|
|
let tMin = Infinity, tMax = -Infinity;
|
|
for (const p of points) {
|
|
const d = [p[0]-center[0], p[1]-center[1], p[2]-center[2]];
|
|
const t = d[0]*v[0] + d[1]*v[1] + d[2]*v[2];
|
|
if (t < tMin) tMin = t;
|
|
if (t > tMax) tMax = t;
|
|
}
|
|
const tipBack = center.map((x, i) => x + tMin * v[i]);
|
|
const tipFront = center.map((x, i) => x + tMax * v[i]);
|
|
return { n, center, axis: v, length: tMax - tMin, tipBack, tipFront, tMin, tMax };
|
|
}
|
|
|
|
// ===================================================================
|
|
// 2) 生成 patched URDF (替换 thumb 3 个 joint origin)
|
|
// ===================================================================
|
|
|
|
function buildPatchedUrdf(srcXml, suggestion) {
|
|
// 替换 lh_thumb_cmc_yaw 的 origin xyz
|
|
// 不动 rpy(yaw 关节本来就 rpy="0 0 0",不需要改)
|
|
let xml = srcXml;
|
|
|
|
// cmc_yaw
|
|
xml = xml.replace(
|
|
/(<joint\s+name="lh_thumb_cmc_yaw"[\s\S]*?<origin\s+xyz=")[^"]+("\s+rpy=")[^"]+(")/,
|
|
`$1${suggestion.cmcYawXyz.map((x) => x.toFixed(6)).join(' ')}$20 0 0$3`,
|
|
);
|
|
// cmc_pitch:rpy 改成 0 0 0 (PCA 反推时简化了)
|
|
xml = xml.replace(
|
|
/(<joint\s+name="lh_thumb_cmc_pitch"[\s\S]*?<origin\s+xyz=")[^"]+("\s+rpy=")[^"]+(")/,
|
|
`$1${suggestion.cmcPitchXyz.map((x) => x.toFixed(6)).join(' ')}$20 0 0$3`,
|
|
);
|
|
// ip:rpy 保持 0 0 0(本来就 0)
|
|
xml = xml.replace(
|
|
/(<joint\s+name="lh_thumb_ip"[\s\S]*?<origin\s+xyz=")[^"]+("\s+rpy=")[^"]+(")/,
|
|
`$1${suggestion.ipXyz.map((x) => x.toFixed(6)).join(' ')}$20 0 0$3`,
|
|
);
|
|
|
|
return xml;
|
|
}
|
|
|
|
// ===================================================================
|
|
// 3) URDF 解析 + 算 link 世界变换
|
|
// ===================================================================
|
|
|
|
function parseUrdf(xml) {
|
|
const links = new Map();
|
|
for (const m of xml.matchAll(/<link\s+name="([^"]+)"/g)) {
|
|
links.set(m[1], { name: m[1], parent: null, joint: null });
|
|
}
|
|
const jointRe = /<joint\s+name="([^"]+)"[\s\S]*?<\/joint>/g;
|
|
for (const m of xml.matchAll(jointRe)) {
|
|
const block = m[0];
|
|
const parent = /<parent\s+link="([^"]+)"/.exec(block)?.[1];
|
|
const child = /<child\s+link="([^"]+)"/.exec(block)?.[1];
|
|
const origin = /<origin\s+xyz="([^"]+)"\s+rpy="([^"]+)"/.exec(block);
|
|
if (!parent || !child || !origin) continue;
|
|
const xyz = origin[1].trim().split(/\s+/).map(Number);
|
|
const rpy = origin[2].trim().split(/\s+/).map(Number);
|
|
const node = links.get(child);
|
|
if (!node) continue;
|
|
node.parent = parent;
|
|
node.joint = { xyz, rpy };
|
|
}
|
|
return links;
|
|
}
|
|
|
|
function computeUrdfWorldMatrices(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, 'XYZ')),
|
|
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;
|
|
}
|
|
|
|
function linksInScene(urdfWorlds) {
|
|
const out = [];
|
|
for (const [name, mat] of urdfWorlds) {
|
|
const p = new THREE.Vector3();
|
|
const q = new THREE.Quaternion();
|
|
const s = new THREE.Vector3();
|
|
mat.decompose(p, q, s);
|
|
out.push({ link: name, pos: urdfToScene([p.x, p.y, p.z]) });
|
|
}
|
|
return out;
|
|
}
|
|
|
|
// ===================================================================
|
|
// 4) 归类(同 analyze-urdf-mapping.mjs)
|
|
// ===================================================================
|
|
|
|
function projectOnSegment(p, a, b) {
|
|
const ab = [b[0]-a[0], b[1]-a[1], b[2]-a[2]];
|
|
const ap = [p[0]-a[0], p[1]-a[1], p[2]-a[2]];
|
|
const ab2 = ab[0]*ab[0] + ab[1]*ab[1] + ab[2]*ab[2];
|
|
const t = (ap[0]*ab[0] + ap[1]*ab[1] + ap[2]*ab[2]) / ab2;
|
|
return { t };
|
|
}
|
|
|
|
function buildFingerChains(linkPoses) {
|
|
const map = new Map(linkPoses.map((lp) => [lp.link, lp.pos]));
|
|
const four = ['index', 'middle', 'ring', 'pinky'].map((name) => ({
|
|
name,
|
|
segments: [
|
|
{ link: `lh_${name}_proximal`, from: map.get(`lh_${name}_proximal`), to: map.get(`lh_${name}_distal`) },
|
|
(() => {
|
|
const a = map.get(`lh_${name}_proximal`), b = map.get(`lh_${name}_distal`);
|
|
const dir = [b[0]-a[0], b[1]-a[1], b[2]-a[2]];
|
|
const len = Math.hypot(...dir);
|
|
const u = dir.map((v) => v / len);
|
|
const tip = [b[0]+u[0]*len, b[1]+u[1]*len, b[2]+u[2]*len];
|
|
return { link: `lh_${name}_distal`, from: b, to: tip };
|
|
})(),
|
|
],
|
|
}));
|
|
const thumb = {
|
|
name: 'thumb',
|
|
segments: [
|
|
{ link: 'lh_thumb_metacarpals_base2', from: map.get('lh_thumb_metacarpals_base2'), to: map.get('lh_thumb_metacarpals') },
|
|
{ link: 'lh_thumb_metacarpals', from: map.get('lh_thumb_metacarpals'), to: map.get('lh_thumb_distal') },
|
|
(() => {
|
|
const a = map.get('lh_thumb_metacarpals'), b = map.get('lh_thumb_distal');
|
|
const dir = [b[0]-a[0], b[1]-a[1], b[2]-a[2]];
|
|
const len = Math.hypot(...dir);
|
|
const u = dir.map((v) => v / len);
|
|
const tip = [b[0]+u[0]*len, b[1]+u[1]*len, b[2]+u[2]*len];
|
|
return { link: 'lh_thumb_distal', from: b, to: tip };
|
|
})(),
|
|
],
|
|
};
|
|
return [thumb, ...four];
|
|
}
|
|
|
|
function classify(placements, linkPoses) {
|
|
const fingers = buildFingerChains(linkPoses);
|
|
const FAR = 0.022;
|
|
const STATIC_T = -0.1;
|
|
return placements.map((pl) => {
|
|
let bestFinger = null, bestSegIdx = -1, bestT = 0, bestDist = Infinity, bestUnT0 = 0;
|
|
for (const f of fingers) {
|
|
const seg0 = f.segments[0];
|
|
const r0 = projectOnSegment(pl.pos, seg0.from, seg0.to);
|
|
for (let i = 0; i < f.segments.length; i++) {
|
|
const seg = f.segments[i];
|
|
const { t } = projectOnSegment(pl.pos, seg.from, seg.to);
|
|
const ct = Math.max(0, Math.min(1, t));
|
|
const proj = [
|
|
seg.from[0] + (seg.to[0]-seg.from[0])*ct,
|
|
seg.from[1] + (seg.to[1]-seg.from[1])*ct,
|
|
seg.from[2] + (seg.to[2]-seg.from[2])*ct,
|
|
];
|
|
const d = Math.hypot(pl.pos[0]-proj[0], pl.pos[1]-proj[1], pl.pos[2]-proj[2]);
|
|
if (d < bestDist) {
|
|
bestDist = d; bestFinger = f; bestSegIdx = i; bestT = ct; bestUnT0 = r0.t;
|
|
}
|
|
}
|
|
}
|
|
if (!bestFinger || bestDist > FAR) {
|
|
return { ...pl, link: 'lh_hand_base_link', reason: 'no-close-link', distMm: +(bestDist*1000).toFixed(2) };
|
|
}
|
|
if (bestSegIdx === 0 && bestUnT0 < STATIC_T) {
|
|
return { ...pl, link: 'lh_hand_base_link', reason: 'behind-mcp', t: +bestUnT0.toFixed(3), distMm: +(bestDist*1000).toFixed(2), finger: bestFinger.name };
|
|
}
|
|
return { ...pl, link: bestFinger.segments[bestSegIdx].link, reason: 'on-segment', segIdx: bestSegIdx, t: +bestT.toFixed(3), distMm: +(bestDist*1000).toFixed(2), finger: bestFinger.name };
|
|
});
|
|
}
|
|
|
|
// ===================================================================
|
|
// 5) main
|
|
// ===================================================================
|
|
|
|
function main() {
|
|
const srcXml = readFileSync(SRC_URDF, 'utf-8');
|
|
const assembly = JSON.parse(readFileSync(ASSEMBLY_PATH, 'utf-8'));
|
|
|
|
// PCA 拇指
|
|
const thumbPoints = [];
|
|
for (const phaseObj of assembly.AllStep) {
|
|
const [phaseName, body] = Object.entries(phaseObj)[0];
|
|
if (phaseName !== 'phase-拇指安装') continue;
|
|
for (const ps of body.phases ?? []) for (const sub of ps.subphase ?? []) {
|
|
if (!sub.subpart || sub.action === 'line') continue;
|
|
for (const ap of sub.assemblypos ?? []) thumbPoints.push(parsePos(ap.pos));
|
|
}
|
|
}
|
|
const pca = pcaAnalyze(thumbPoints);
|
|
console.log(`拇指零件 ${thumbPoints.length} 个,几何长度 ${(pca.length*1000).toFixed(1)} mm`);
|
|
|
|
// 反推 3 段 link 位置(URDF 系)
|
|
const base2_scene = pca.tipBack;
|
|
const meta_scene = pca.center;
|
|
const distal_scene = pca.center.map((x, i) => x + 0.6 * (pca.tipFront[i] - pca.center[i]));
|
|
|
|
const base2_urdf = sceneToUrdf(base2_scene);
|
|
const meta_urdf = sceneToUrdf(meta_scene);
|
|
const distal_urdf = sceneToUrdf(distal_scene);
|
|
|
|
// joint origin 相对父 link
|
|
const cmcYawXyz = base2_urdf;
|
|
const cmcPitchXyz = [meta_urdf[0]-base2_urdf[0], meta_urdf[1]-base2_urdf[1], meta_urdf[2]-base2_urdf[2]];
|
|
const ipXyz = [distal_urdf[0]-meta_urdf[0], distal_urdf[1]-meta_urdf[1], distal_urdf[2]-meta_urdf[2]];
|
|
|
|
// 生成 patched URDF
|
|
const patched = buildPatchedUrdf(srcXml, { cmcYawXyz, cmcPitchXyz, ipXyz });
|
|
writeFileSync(PATCHED_URDF, patched, 'utf-8');
|
|
console.log(`patched URDF 写入:${PATCHED_URDF}`);
|
|
|
|
// 用 patched URDF 重跑归类
|
|
const links = parseUrdf(patched);
|
|
const urdfWorlds = computeUrdfWorldMatrices(links);
|
|
const linkPoses = linksInScene(urdfWorlds);
|
|
|
|
console.log('\n=== Patched URDF 各 link 的 Scene 位置 ===');
|
|
for (const lp of linkPoses) {
|
|
console.log(` ${lp.link.padEnd(32)} (${lp.pos.map((v) => v.toFixed(4)).join(', ')})`);
|
|
}
|
|
|
|
const placements = collectPlacements(assembly);
|
|
const classified = classify(placements, linkPoses);
|
|
|
|
const byLink = {};
|
|
for (const lp of linkPoses) byLink[lp.link] = [];
|
|
for (const c of classified) byLink[c.link].push(c);
|
|
for (const link of Object.keys(byLink)) byLink[link].sort((a,b) => (a.t??Infinity) - (b.t??Infinity));
|
|
|
|
console.log('\n=== Patched 后归类统计 ===');
|
|
for (const link of Object.keys(byLink)) {
|
|
const uniq = new Set(byLink[link].map((x) => x.part));
|
|
console.log(` ${link.padEnd(32)} ${byLink[link].length.toString().padStart(3)} placements / ${uniq.size} 种零件`);
|
|
}
|
|
|
|
// 拇指详情(用户最关心)
|
|
console.log('\n=== 拇指 3 段归类详情 ===');
|
|
for (const link of ['lh_thumb_metacarpals_base2','lh_thumb_metacarpals','lh_thumb_distal']) {
|
|
const items = byLink[link];
|
|
console.log(`\n● ${link} (${items.length} 项)`);
|
|
for (const it of items) {
|
|
const tag = `t=${it.t!==null?it.t.toFixed(3):'---'}, dist=${it.distMm}mm`;
|
|
console.log(` ${it.part} ${tag} pos=(${it.pos.map((v)=>v.toFixed(4)).join(', ')})`);
|
|
}
|
|
}
|
|
|
|
// 5 指汇总
|
|
console.log('\n=== 5 指零件归类(去重)===');
|
|
const fingerSummary = {};
|
|
for (const link of Object.keys(byLink)) {
|
|
const m = /^lh_(thumb|index|middle|ring|pinky)_(.*)$/.exec(link);
|
|
if (!m) continue;
|
|
fingerSummary[m[1]] ??= {};
|
|
fingerSummary[m[1]][m[2]] = Array.from(new Set(byLink[link].map((x) => x.part))).sort();
|
|
}
|
|
console.log(JSON.stringify(fingerSummary, null, 2));
|
|
|
|
// 写完整报告
|
|
mkdirSync(OUT_DIR, { recursive: true });
|
|
writeFileSync(resolve(OUT_DIR, 'urdf-mapping-final.json'), JSON.stringify({
|
|
meta: {
|
|
patchedUrdf: 'src/assets/O6/urdf/linkerhand_o6_left.patched.urdf',
|
|
thumb_pca: { length_mm: +(pca.length*1000).toFixed(1), n: pca.n },
|
|
links: linkPoses.map((lp) => ({ link: lp.link, scenePos: lp.pos.map((v)=>+v.toFixed(4)) })),
|
|
totalPlacements: classified.length,
|
|
},
|
|
fingerSummary,
|
|
byLink: Object.fromEntries(Object.entries(byLink).map(([k, v]) => [k, v.map((x) => ({
|
|
part: x.part, phase: x.phase, t: x.t ?? null, distMm: x.distMm, pos: x.pos.map((v)=>+v.toFixed(4)),
|
|
}))])),
|
|
}, null, 2), 'utf-8');
|
|
console.log(`\n完整报告写入:scripts/out/urdf-mapping-final.json`);
|
|
}
|
|
|
|
main();
|