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

app/src/lib/utilities/model-utilities.ts

@@ -1,89 +1,92 @@
-import { Color, LoaderUtils, Vector3 } from 'three';
+import { Color, LoaderUtils, Vector3 } from 'three'
-import URDFLoader, { type URDFRobot } from 'urdf-loader';
+import URDFLoader, { type URDFRobot } from 'urdf-loader'
-import { XacroLoader } from 'xacro-parser';
+import { XacroLoader } from 'xacro-parser'
-import { Result } from '$lib/utilities';
+import { Result } from '$lib/utilities'
-import { jointNames, model } from '$lib/stores';
+import { jointNames, model } from '$lib/stores'
-import uzip from 'uzip';
+import uzip from 'uzip'
-import { fileService } from '$lib/services';
+import { fileService } from '$lib/services'
 
-let model_xml: XMLDocument;
+let model_xml: XMLDocument
 
 export const populateModelCache = async () => {
-	await cacheModelFiles();
+  await cacheModelFiles()
-	const modelRes = await loadModelAsync('/spot_micro.urdf.xacro');
+  const modelRes = await loadModel('/yertle.URDF')
-	if (modelRes.isOk()) {
+  if (modelRes.isOk()) {
-		const [urdf, JOINT_NAME] = modelRes.inner;
+    const [urdf, JOINT_NAME] = modelRes.inner
-		jointNames.set(JOINT_NAME);
+    jointNames.set(JOINT_NAME)
-		model.set(urdf);
+    model.set(urdf)
-	} else {
+  } else {
-		console.error(modelRes.inner, { exception: modelRes.exception });
+    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][]) {
+  for (const [path, data] of Object.entries(files) as [path: string, data: Uint8Array][]) {
-		const url = new URL(path, window.location.href);
+    const url = new URL(path, window.location.href)
-		fileService.saveFile(url.toString(), data);
+    fileService?.saveFile(url.toString(), data)
-	}
+  }
-};
+}
 
-export const loadModelAsync = async (
+export const loadModel = async (url: string): Promise<Result<[URDFRobot, string[]], string>> => {
-	url: string
+  const urdfLoader = new URDFLoader()
-): Promise<Result<[URDFRobot, string[]], string>> => {
+  urdfLoader.workingPath = LoaderUtils.extractUrlBase(url)
-	return new Promise((resolve, reject) => {
-		const xacroLoader = new XacroLoader();
-		const urdfLoader = new URDFLoader();
-		urdfLoader.workingPath = LoaderUtils.extractUrlBase(url);
 
-		xacroLoader.load(
+  let xml = url.endsWith('.xacro') ? await loadXacro(url) : await fetch(url).then(res => res.text())
-			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]);
 
-					resolve(Result.ok([model, joints]));
+  if (typeof xml === 'string') {
-				} catch (error) {
+    xml = new window.DOMParser().parseFromString(xml, 'text/xml')
-					resolve(Result.err('Failed to load model', error));
+  }
-				}
-			},
-			(error) => resolve(Result.err('Failed to load model', error))
-		);
-	});
-};
 
-export const toeWorldPositions = (robot: URDFRobot) => {
+  return new Promise(resolve => {
-	const toe_positions: Vector3[] = [];
+    model_xml = xml
-	robot.traverse((child) => {
+    try {
-		if (child.name.includes('toe') && !child.name.includes('_link')) {
+      const model = urdfLoader.parse(xml)
-			const worldPosition = new Vector3();
+      setupRobot(model)
-			child.getWorldPosition(worldPosition);
+      const joints = Object.entries(model.joints)
-			toe_positions.push(worldPosition);
+        .filter(joint => joint[1].jointType !== 'fixed')
-		}
+        .map(joint => joint[0])
-	});
-	return toe_positions;
-};
 
-export const footColor = () => {
+      resolve(Result.ok([model, joints]))
-	const colorElem = model_xml.querySelector('material[name=foot_color] > color') as Element;
+    } catch (error) {
-	const colorAttrStr = colorElem.getAttribute('rgba') as string;
+      resolve(Result.err('Failed to load model', error))
-	const colorStr = colorAttrStr
+    }
-		.split(' ')
+  })
-		.slice(0, 3)
+}
-		.map((val) => Math.floor(+val * 255))
-		.join(', ');
 
-	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})`)
+}