222 lines
9.4 KiB
JavaScript
222 lines
9.4 KiB
JavaScript
/**
|
|
* 生成 A7 双臂"完全体 URDF"(ROS Z-up 标准 + GLB visual + 原 inertial/collision)。
|
|
*
|
|
* 思路(对照 build-full-urdf.mjs):
|
|
* - 原 A7 双臂 URDF(SolidWorks 导出):保留 base_link + L1-L8 + R1-R8 的
|
|
* inertial / collision / 关节 / 限位 / 轴。base_link 的 visual(STL)也保留。
|
|
* - 单臂装配 assembly.json:用移植版装配引擎算出整条臂每个零件的世界位姿(Scene Y-up)。
|
|
* - 对齐:A = T(t)·R_x(90°),把装配 Scene 系刚体映射到 ROS URDF 系(L 链)。
|
|
* - 归类:连杆/外壳按名字 → LN;五金件按 ROS z 轴向就近分箱 → LN。
|
|
* - visual 局部位姿 = inv(link_world_ros) · A · M_scene。
|
|
* - 右臂 R1-R8:复用左臂各 link 的局部 visual(180° 旋转副本近似)。
|
|
*
|
|
* web 加载:UrdfRobot 给整机包 ROS→Scene 四元数,臂沿 -Z 下垂 → scene 里竖直悬挂。
|
|
*/
|
|
import { readFileSync, writeFileSync } from 'node:fs';
|
|
import { dirname, resolve } from 'node:path';
|
|
import { fileURLToPath } from 'node:url';
|
|
import * as THREE from 'three';
|
|
import { normalizeKeys, buildPlans, collectPlacedModels } from './lib/compile-assembly.mjs';
|
|
import { parseUrdf, computeRosWorldMatrices } from './lib/urdf.mjs';
|
|
import { readStlVertices } from './lib/stl.mjs';
|
|
|
|
const __dirname = dirname(fileURLToPath(import.meta.url));
|
|
const ROOT = resolve(__dirname, '..');
|
|
const SRC_URDF = resolve(ROOT, 'src/assets/A7/urdf/urdf/lkls73_i1_dual_arm_description.urdf');
|
|
const OUT_URDF = resolve(ROOT, 'src/assets/A7/urdf/a7_dual_arm.full.urdf');
|
|
const ASSEMBLY = resolve(ROOT, 'src/data/A7/assembly.json');
|
|
|
|
// 对齐变换(check-a7-align.mjs 求得)
|
|
const ALIGN_R = new THREE.Matrix4().makeRotationX(Math.PI / 2);
|
|
const ALIGN_T = new THREE.Vector3(0.0081, 0.1337, -0.0436);
|
|
const ALIGN = new THREE.Matrix4().makeTranslation(ALIGN_T.x, ALIGN_T.y, ALIGN_T.z).multiply(ALIGN_R);
|
|
|
|
// ===================================================================
|
|
// 归类:零件名/轴向 → 左臂 link
|
|
// ===================================================================
|
|
const LINK_NAMES = ['L1_Link','L2_Link','L3_Link','L4_Link','L5_Link','L6_Link','L7_Link','L8_Link'];
|
|
|
|
// 连杆 backbone 名字规则 —— 这是「link 本体」的可见结构,归类最可靠。
|
|
// 末端段归 L8。其余所有零件(外壳/电机/螺丝/轴承/支撑柱/...)都不按名字,
|
|
// 而是归到「3D 距离最近的连杆锚点」所在 link,保证同一物理段的壳+内部件+五金
|
|
// 在同一 link 上一起转,避免转关节时内部件被甩出来。
|
|
function backboneLink(name) {
|
|
if (/连杆1[AB]|^安装机械臂连杆1B$/.test(name)) return 'L1_Link';
|
|
if (/连杆2[ABC]/.test(name)) return 'L2_Link';
|
|
if (/连杆3[ABC]/.test(name)) return 'L3_Link';
|
|
if (/连杆4[AB]|4BR|4DR/.test(name)) return 'L4_Link';
|
|
if (/连杆5[AB]/.test(name)) return 'L5_Link';
|
|
if (/连杆6[AB]/.test(name)) return 'L6_Link';
|
|
if (/连杆7[AB]/.test(name)) return 'L7_Link';
|
|
if (/末端/.test(name)) return 'L8_Link';
|
|
return null;
|
|
}
|
|
|
|
/** 每个 link 的 STL 在 ROS 世界系(关节角=0)下的 AABB。 */
|
|
function buildLinkAabbs(worldMats, meshesDir) {
|
|
const aabbs = [];
|
|
for (const link of LINK_NAMES) {
|
|
const stlPath = resolve(meshesDir, `${link.replace('_Link', '')}_Link.STL`);
|
|
let verts;
|
|
try { verts = readStlVertices(stlPath); } catch { continue; }
|
|
const w = worldMats.get(link);
|
|
const box = new THREE.Box3();
|
|
const v = new THREE.Vector3();
|
|
for (const [x, y, z] of verts) {
|
|
v.set(x, y, z).applyMatrix4(w);
|
|
box.expandByPoint(v);
|
|
}
|
|
aabbs.push({ link, box });
|
|
}
|
|
return aabbs;
|
|
}
|
|
|
|
/** 点到 AABB 的距离(在盒内为 0)。 */
|
|
function pointToBox(p, box) {
|
|
const dx = Math.max(box.min.x - p.x, 0, p.x - box.max.x);
|
|
const dy = Math.max(box.min.y - p.y, 0, p.y - box.max.y);
|
|
const dz = Math.max(box.min.z - p.z, 0, p.z - box.max.z);
|
|
return Math.hypot(dx, dy, dz);
|
|
}
|
|
|
|
/** 零件归到「STL 体积最近/包含它」的 link。 */
|
|
function nearestStlLink(posRos, aabbs) {
|
|
let best = aabbs[0], bestD = Infinity;
|
|
for (const a of aabbs) {
|
|
const d = pointToBox(posRos, a.box);
|
|
if (d < bestD) { bestD = d; best = a; }
|
|
}
|
|
return best.link;
|
|
}
|
|
|
|
/** 连杆/末端按名字(它们就是 link 本体);其余按原 URDF 各 link 的 STL 体积归类。 */
|
|
function classify(name, posRos, aabbs) {
|
|
return backboneLink(name) ?? nearestStlLink(posRos, aabbs);
|
|
}
|
|
|
|
// ===================================================================
|
|
// pose 工具
|
|
// ===================================================================
|
|
function sceneMatrix(pos, rot) {
|
|
return new THREE.Matrix4().compose(
|
|
new THREE.Vector3(...pos),
|
|
new THREE.Quaternion().setFromEuler(new THREE.Euler(...rot, 'XYZ')),
|
|
new THREE.Vector3(1, 1, 1),
|
|
);
|
|
}
|
|
function matrixToUrdfPose(m) {
|
|
const p = new THREE.Vector3();
|
|
const q = new THREE.Quaternion();
|
|
m.decompose(p, q, new THREE.Vector3());
|
|
const e = new THREE.Euler().setFromQuaternion(q, 'ZYX');
|
|
return { xyz: [p.x, p.y, p.z], rpy: [e.x, e.y, e.z] };
|
|
}
|
|
function fmt(n) {
|
|
if (!Number.isFinite(n) || Math.abs(n) < 1e-9) return '0';
|
|
return n.toFixed(6);
|
|
}
|
|
|
|
// 左 link → 右 link 名(L1_Link → R1_Link)
|
|
function mirrorLinkName(lname) {
|
|
return lname.replace(/^L/, 'R');
|
|
}
|
|
|
|
// ===================================================================
|
|
// 主流程
|
|
// ===================================================================
|
|
function main() {
|
|
const cfg = normalizeKeys(JSON.parse(readFileSync(ASSEMBLY, 'utf-8')));
|
|
const { products, finalOutput } = buildPlans(cfg);
|
|
const placed = collectPlacedModels(products.get(finalOutput), { position: [0, 0, 0], rotation: [0, 0, 0] });
|
|
|
|
const srcXml = readFileSync(SRC_URDF, 'utf-8');
|
|
const { links, joints, linkMap } = parseUrdf(srcXml);
|
|
const worldMats = computeRosWorldMatrices(linkMap);
|
|
|
|
// 先算每个零件的 ROS world 矩阵
|
|
const items = placed.map((part) => {
|
|
const mRos = ALIGN.clone().multiply(sceneMatrix(part.position, part.rotation));
|
|
const pos = new THREE.Vector3(mRos.elements[12], mRos.elements[13], mRos.elements[14]);
|
|
return { name: part.name, mRos, pos };
|
|
});
|
|
|
|
// 原 URDF 各 link 的 STL 世界 AABB(归类基准)
|
|
const meshesDir = resolve(ROOT, 'src/assets/A7/urdf/meshes');
|
|
const aabbs = buildLinkAabbs(worldMats, meshesDir);
|
|
|
|
// 每个 L link 的 visual 列表(局部 pose)。R link 复用 L 的局部 pose。
|
|
const visualsByLink = new Map();
|
|
for (const n of LINK_NAMES) visualsByLink.set(n, []);
|
|
|
|
const counts = {};
|
|
for (const it of items) {
|
|
const link = classify(it.name, it.pos, aabbs);
|
|
const linkWorld = worldMats.get(link);
|
|
const local = linkWorld.clone().invert().multiply(it.mRos);
|
|
const { xyz, rpy } = matrixToUrdfPose(local);
|
|
visualsByLink.get(link).push({ part: it.name, xyz, rpy });
|
|
counts[link] = (counts[link] ?? 0) + 1;
|
|
}
|
|
|
|
const xml = renderUrdf(links, joints, visualsByLink);
|
|
writeFileSync(OUT_URDF, xml, 'utf-8');
|
|
console.log(`A7 双臂 full URDF 写入: ${OUT_URDF}`);
|
|
console.log(`零件实例: ${placed.length}`);
|
|
console.log('\n=== 各 link visual 数(左臂;右臂镜像复用)===');
|
|
for (const n of LINK_NAMES) console.log(` ${n.padEnd(10)} ${counts[n] ?? 0}`);
|
|
}
|
|
|
|
function renderUrdf(links, joints, visualsByLink) {
|
|
let xml = '<?xml version="1.0" encoding="utf-8"?>\n';
|
|
xml += '<!-- Auto-generated by scripts/build-a7-full-urdf.mjs -->\n';
|
|
xml += '<!-- 双臂:base_link 保留原 STL;L/R 各 link 用 GLB 细节零件替换 visual。inertial/collision/joint 沿用原 URDF。 -->\n';
|
|
xml += '<robot name="a7_dual_arm_full">\n';
|
|
xml += ' <material name="silver"><color rgba="0.6 0.62 0.66 1.0"/></material>\n\n';
|
|
|
|
const indent = (block) => ' ' + block.replace(/\n\s*/g, '\n ') + '\n';
|
|
|
|
for (const link of links) {
|
|
xml += ` <link name="${link.name}">\n`;
|
|
for (const b of link.inertialBlocks) xml += indent(b);
|
|
for (const b of link.collisionBlocks) xml += indent(b);
|
|
|
|
if (link.name === 'base_link') {
|
|
// 中柱:保留原 STL visual
|
|
xml += ' <visual>\n';
|
|
xml += ' <origin xyz="0 0 0" rpy="0 0 0"/>\n';
|
|
xml += ' <geometry><mesh filename="meshes/base_link.STL"/></geometry>\n';
|
|
xml += ' <material name="silver"/>\n';
|
|
xml += ' </visual>\n';
|
|
} else {
|
|
// L/R link:GLB visual。左臂直接用,右臂取镜像 link 的左臂局部 pose。
|
|
const isRight = /^R\d/.test(link.name);
|
|
const sourceLink = isRight ? link.name.replace(/^R/, 'L') : link.name;
|
|
const visuals = visualsByLink.get(sourceLink) ?? [];
|
|
for (const v of visuals) {
|
|
xml += ' <visual>\n';
|
|
xml += ` <origin xyz="${v.xyz.map(fmt).join(' ')}" rpy="${v.rpy.map(fmt).join(' ')}"/>\n`;
|
|
xml += ` <geometry><mesh filename="glb://${v.part}"/></geometry>\n`;
|
|
xml += ' </visual>\n';
|
|
}
|
|
}
|
|
xml += ' </link>\n';
|
|
}
|
|
xml += '\n';
|
|
|
|
for (const j of joints) {
|
|
xml += ` <joint name="${j.name}" type="${j.type}">\n`;
|
|
xml += ` <origin xyz="${j.xyz.map(fmt).join(' ')}" rpy="${j.rpy.map(fmt).join(' ')}"/>\n`;
|
|
xml += ` <parent link="${j.parent}"/>\n`;
|
|
xml += ` <child link="${j.child}"/>\n`;
|
|
if (j.axis) xml += ` <axis xyz="${j.axis.map(fmt).join(' ')}"/>\n`;
|
|
if (j.limit) xml += ` <limit lower="${j.limit.lower}" upper="${j.limit.upper}" effort="${j.limit.effort}" velocity="${j.limit.velocity}"/>\n`;
|
|
if (j.mimic) xml += ` <mimic joint="${j.mimic.source}" multiplier="${j.mimic.multiplier}" offset="${j.mimic.offset}"/>\n`;
|
|
xml += ' </joint>\n';
|
|
}
|
|
|
|
xml += '</robot>\n';
|
|
return xml;
|
|
}
|
|
|
|
main();
|