🐾 Adds foot tracing and gui panel

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

@@ -1,21 +1,22 @@
-import { LoaderUtils, Vector3 } from 'three';
+import { Color, LoaderUtils, Vector3 } from 'three';
 import URDFLoader, { type URDFRobot } from 'urdf-loader';
 import { XacroLoader } from 'xacro-parser';
 import { Result } from '$lib/utilities';
 
+let model_xml: XMLDocument;
+
 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);
 
 		xacroLoader.load(
 			url,
 			async (xml) => {
-				const urdfLoader = new URDFLoader();
-
-				urdfLoader.workingPath = LoaderUtils.extractUrlBase(url);
-
+				model_xml = xml;
 				try {
 					const model = urdfLoader.parse(xml);
 					model.rotation.x = -Math.PI / 2;
@@ -48,3 +49,15 @@ export const toeWorldPositions = (robot: URDFRobot) => {
 	});
 	return toe_positions;
 };
+
+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(', ');
+
+	return new Color(`rgb(${colorStr})`);
+};