📫 Adds function to get the model toe positions

2024-02-25 01:57:43 +01:00
parent 2d8137772a
commit 4c2fe9a044
1 changed files with 13 additions and 1 deletions
@@ -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;
+};