91 lines
3.3 KiB
JavaScript
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;
|
|
}
|