From 01d46f283b57a54c27f521bb2d265f593d2c218a Mon Sep 17 00:00:00 2001 From: Rune Harlyk Date: Wed, 2 Jul 2025 22:55:31 +0200 Subject: [PATCH] =?UTF-8?q?=F0=9F=91=94=20Update=20model=20utils=20to=20be?= =?UTF-8?q?=20able=20to=20load=20both=20urdf=20and=20xacro?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- app/src/lib/utilities/model-utilities.ts | 155 ++++++++++++----------- 1 file changed, 79 insertions(+), 76 deletions(-) diff --git a/app/src/lib/utilities/model-utilities.ts b/app/src/lib/utilities/model-utilities.ts index e37dd61..f3eeeb5 100644 --- a/app/src/lib/utilities/model-utilities.ts +++ b/app/src/lib/utilities/model-utilities.ts @@ -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> => { - 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> => { + 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 => + 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})`) +}