Darkages/SpotMicroESP32-Leika
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

👔 Update model utils to be able to load both urdf and xacro

Browse Source
This commit is contained in:
Rune Harlyk
2025-07-02 22:55:31 +02:00
parent 7c8c5b40a1
commit 01d46f283b
1 changed files with 79 additions and 76 deletions
Download Patch File Download Diff File Expand all files Collapse all files
app/src/lib/utilities/model-utilities.ts
+79 -76
View File
@@ -1,89 +1,92 @@
import { Color, LoaderUtils, Vector3 } from 'three';
import URDFLoader, { type URDFRobot } from 'urdf-loader';
import { XacroLoader } from 'xacro-parser';
import { Result } from '$lib/utilities';
import { jointNames, model } from '$lib/stores';
import uzip from 'uzip';
import { fileService } from '$lib/services';
import { Color, LoaderUtils, Vector3 } from 'three'
import URDFLoader, { type URDFRobot } from 'urdf-loader'
import { XacroLoader } from 'xacro-parser'
import { Result } from '$lib/utilities'
import { jointNames, model } from '$lib/stores'
import uzip from 'uzip'
import { fileService } from '$lib/services'
let model_xml: XMLDocument;
let model_xml: XMLDocument
export const populateModelCache = async () => {
await cacheModelFiles();
const modelRes = await loadModelAsync('/spot_micro.urdf.xacro');
if (modelRes.isOk()) {
const [urdf, JOINT_NAME] = modelRes.inner;
jointNames.set(JOINT_NAME);
model.set(urdf);
} else {
console.error(modelRes.inner, { exception: modelRes.exception });
}
};
await cacheModelFiles()
const modelRes = await loadModel('/yertle.URDF')
if (modelRes.isOk()) {
const [urdf, JOINT_NAME] = modelRes.inner
jointNames.set(JOINT_NAME)
model.set(urdf)
} else {
console.error(modelRes.inner, { exception: modelRes.exception })
}
}
export const cacheModelFiles = async () => {
let data = await fetch('/stl.zip');
const data = await fetch('/URDF.zip')
var files = uzip.parse(await data.arrayBuffer());
const files = uzip.parse(await data.arrayBuffer())
for (const [path, data] of Object.entries(files) as [path: string, data: Uint8Array][]) {
const url = new URL(path, window.location.href);
fileService.saveFile(url.toString(), data);
}
};
for (const [path, data] of Object.entries(files) as [path: string, data: Uint8Array][]) {
const url = new URL(path, window.location.href)
fileService?.saveFile(url.toString(), data)
}
}
export const loadModelAsync = async (
url: string
): Promise<Result<[URDFRobot, string[]], string>> => {
return new Promise((resolve, reject) => {
const xacroLoader = new XacroLoader();
const urdfLoader = new URDFLoader();
urdfLoader.workingPath = LoaderUtils.extractUrlBase(url);
export const loadModel = async (url: string): Promise<Result<[URDFRobot, string[]], string>> => {
const urdfLoader = new URDFLoader()
urdfLoader.workingPath = LoaderUtils.extractUrlBase(url)
xacroLoader.load(
url,
async (xml) => {
model_xml = xml;
try {
const model = urdfLoader.parse(xml);
model.rotation.x = -Math.PI / 2;
model.rotation.z = Math.PI / 2;
model.traverse((c) => (c.castShadow = true));
model.updateMatrixWorld(true);
model.scale.setScalar(10);
const joints = Object.entries(model.joints)
.filter((joint) => joint[1].jointType !== 'fixed')
.map((joint) => joint[0]);
let xml = url.endsWith('.xacro') ? await loadXacro(url) : await fetch(url).then(res => res.text())
resolve(Result.ok([model, joints]));
} catch (error) {
resolve(Result.err('Failed to load model', error));
}
},
(error) => resolve(Result.err('Failed to load model', error))
);
});
};
if (typeof xml === 'string') {
xml = new window.DOMParser().parseFromString(xml, 'text/xml')
}
export const toeWorldPositions = (robot: URDFRobot) => {
const toe_positions: Vector3[] = [];
robot.traverse((child) => {
if (child.name.includes('toe') && !child.name.includes('_link')) {
const worldPosition = new Vector3();
child.getWorldPosition(worldPosition);
toe_positions.push(worldPosition);
}
});
return toe_positions;
};
return new Promise(resolve => {
model_xml = xml
try {
const model = urdfLoader.parse(xml)
setupRobot(model)
const joints = Object.entries(model.joints)
.filter(joint => joint[1].jointType !== 'fixed')
.map(joint => joint[0])
export const footColor = () => {
const colorElem = model_xml.querySelector('material[name=foot_color] > color') as Element;
const colorAttrStr = colorElem.getAttribute('rgba') as string;
const colorStr = colorAttrStr
.split(' ')
.slice(0, 3)
.map((val) => Math.floor(+val * 255))
.join(', ');
resolve(Result.ok([model, joints]))
} catch (error) {
resolve(Result.err('Failed to load model', error))
}
})
}
return new Color(`rgb(${colorStr})`);
};
const loadXacro = async (url: string): Promise<XMLDocument> =>
new Promise((resolve, reject) => {
new XacroLoader().load(url, resolve, reject)
})
function setupRobot(robot: URDFRobot) {
robot.rotation.x = -Math.PI / 2
robot.rotation.z = Math.PI / 2
robot.scale.setScalar(10)
robot.traverse(c => (c.castShadow = true))
robot.updateMatrixWorld(true)
}
export function getToeWorldPositions(robot: URDFRobot): Vector3[] {
const toes: Vector3[] = []
robot.traverse(c => {
if (c.name.includes('toe') && !c.name.includes('_link'))
toes.push(c.getWorldPosition(new Vector3()))
})
return toes
}
export const extractFootColor = () => {
const colorElem = model_xml.querySelector('material[name=foot_color] > color') as Element
const colorAttrStr = colorElem.getAttribute('rgba') as string
const colorStr = colorAttrStr
.split(' ')
.slice(0, 3)
.map(val => Math.floor(+val * 255))
.join(', ')
return new Color(`rgb(${colorStr})`)
}