Files

91 lines
3.3 KiB
JavaScript

/**
* 容错 URDF 解析(支持属性跨行,SolidWorks 导出常见格式)。
* 保留 inertial / collision 原文块,解析 joint 的 origin/parent/child/axis/limit/mimic。
*/
import * as THREE from 'three';
function attr(tag, name) {
const m = new RegExp(`${name}\\s*=\\s*"([^"]*)"`).exec(tag);
return m ? m[1] : null;
}
function nums(str) {
return str.trim().split(/\s+/).map(Number);
}
export function parseUrdf(xml) {
const links = [];
const linkMap = new Map();
for (const lm of xml.matchAll(/<link\b([^>]*)>([\s\S]*?)<\/link>/g)) {
const name = attr(lm[1], 'name');
const body = lm[2];
const inertialBlocks = [...body.matchAll(/<inertial>[\s\S]*?<\/inertial>/g)].map((m) => m[0]);
const collisionBlocks = [...body.matchAll(/<collision>[\s\S]*?<\/collision>/g)].map((m) => m[0]);
const node = { name, parent: null, joint: null, inertialBlocks, collisionBlocks };
links.push(node);
linkMap.set(name, node);
}
const joints = [];
for (const jm of xml.matchAll(/<joint\b([^>]*)>([\s\S]*?)<\/joint>/g)) {
const head = jm[1];
const body = jm[2];
const name = attr(head, 'name');
const type = attr(head, 'type');
const parent = (/<parent\b([^>]*)\/?>/.exec(body)?.[1] ?? '').match(/link\s*=\s*"([^"]+)"/)?.[1];
const child = (/<child\b([^>]*)\/?>/.exec(body)?.[1] ?? '').match(/link\s*=\s*"([^"]+)"/)?.[1];
const originTag = /<origin\b([^>]*)\/?>/.exec(body)?.[1] ?? '';
const xyz = nums(attr(originTag, 'xyz') ?? '0 0 0');
const rpy = nums(attr(originTag, 'rpy') ?? '0 0 0');
const axisTag = /<axis\b([^>]*)\/?>/.exec(body)?.[1];
const axis = axisTag ? nums(attr(axisTag, 'xyz') ?? '0 0 0') : null;
const limitTag = /<limit\b([^>]*)\/?>/.exec(body)?.[1];
const limit = limitTag
? {
lower: Number(attr(limitTag, 'lower') ?? 0),
upper: Number(attr(limitTag, 'upper') ?? 0),
effort: Number(attr(limitTag, 'effort') ?? 0),
velocity: Number(attr(limitTag, 'velocity') ?? 0),
}
: null;
const mimicTag = /<mimic\b([^>]*)\/?>/.exec(body)?.[1];
const mimic = mimicTag
? {
source: attr(mimicTag, 'joint'),
multiplier: Number(attr(mimicTag, 'multiplier') ?? 1),
offset: Number(attr(mimicTag, 'offset') ?? 0),
}
: null;
joints.push({ name, type, parent, child, xyz, rpy, axis, limit, mimic });
const childNode = linkMap.get(child);
if (childNode) {
childNode.parent = parent;
childNode.joint = { xyz, rpy };
}
}
return { links, joints, linkMap };
}
/** 每个 link 在 ROS 系下的世界 4x4 矩阵(关节角=0) */
export function computeRosWorldMatrices(linkMap) {
const cache = new Map();
function world(name) {
if (cache.has(name)) return cache.get(name);
const node = linkMap.get(name);
if (!node || !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 linkMap.keys()) world(name);
return cache;
}