From 4c2fe9a04460ffb38aa808b038f4ddedd3cccaa4 Mon Sep 17 00:00:00 2001
From: Rune Harlyk <rune@harlyk.dk>
Date: Sun, 25 Feb 2024 01:57:43 +0100
Subject: [PATCH] =?UTF-8?q?=F0=9F=93=AB=20Adds=20function=20to=20get=20the?=
 =?UTF-8?q?=20model=20toe=20positions?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 app/src/lib/utilities/model-utilities.ts | 14 +++++++++++++-
 1 file changed, 13 insertions(+), 1 deletion(-)

diff --git a/app/src/lib/utilities/model-utilities.ts b/app/src/lib/utilities/model-utilities.ts
index 7feda9c..235c46d 100644
--- a/app/src/lib/utilities/model-utilities.ts
+++ b/app/src/lib/utilities/model-utilities.ts
@@ -1,4 +1,4 @@
-import { LoaderUtils } from 'three';
+import { LoaderUtils, Vector3 } from 'three';
 import URDFLoader, { type URDFRobot } from 'urdf-loader';
 import { XacroLoader } from 'xacro-parser';
 import { Result } from '$lib/utilities';
@@ -36,3 +36,15 @@ export const loadModelAsync = async (
 		);
 	});
 };
+
+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;
+};