🐾 Adds foot tracing and gui panel

app/src/app.css

@@ -13,3 +13,8 @@
 body {
 	margin: 0;
 }
+
+#three-gui-panel {
+    top: 50px;
+    right:0px
+}

app/src/components/Views/Model.svelte

@@ -1,36 +1,55 @@
 <script lang="ts">
 	import { onDestroy, onMount } from 'svelte';
-	import { CanvasTexture, CircleGeometry, Mesh, MeshBasicMaterial } from 'three';
+	import { BufferGeometry, CanvasTexture, CircleGeometry, CubicBezierCurve3, Line, LineBasicMaterial, Mesh, MeshBasicMaterial, Vector3, type NormalBufferAttributes } from 'three';
 	import socketService from '$lib/services/socket-service';
 	import uzip from 'uzip';
 	import { model } from '$lib/stores';
-	import { ForwardKinematics } from '$lib/kinematic';
-	import { location } from '$lib/utilities';
+	import { footColor, isEmbeddedApp, location, toeWorldPositions } from '$lib/utilities';
 	import { fileService } from '$lib/services';
 	import { servoAngles, mpu, jointNames } from '$lib/stores';
 	import SceneBuilder from '$lib/sceneBuilder';
 	import { lerp, degToRad } from 'three/src/math/MathUtils';
+    import { GUI } from 'three/addons/libs/lil-gui.module.min.js';
 
-	let sceneManager: SceneBuilder;
+	let sceneManager = new SceneBuilder();
 	let canvas: HTMLCanvasElement, streamCanvas: HTMLCanvasElement, stream: HTMLImageElement;
 	let context: CanvasRenderingContext2D, texture: CanvasTexture;
 
 	let modelAngles: number[] | Int16Array = new Array(12).fill(0);
 	let modelTargetAngles: number[] | Int16Array = new Array(12).fill(0);
 
+    let feet_trace = new Array(4).fill([]);
+    let trace_lines: BufferGeometry<NormalBufferAttributes>[] = []
+
 	const videoStream = `//${location}/api/stream`;
 
 	let showStream = false;
 
+    let settings = {
+        'Trace feet':true,
+        'Trace points': 30
+    }
+
 	onMount(async () => {
-		await cacheModelFiles();
-		await createScene();
+        await cacheModelFiles()
+        await createScene();
+        if (!isEmbeddedApp) createPanel();
 	});
 
 	onDestroy(() => {
 		canvas.remove();
 	});
 
+    const createPanel = () => {
+        const panel = new GUI({width: 310});
+        panel.close();
+        panel.domElement.id = 'three-gui-panel';
+ 
+        const visibility = panel.addFolder('Visualization');
+        visibility.add(settings, 'Trace feet')
+        visibility.add(settings, 'Trace points', 1, 1000, 1)
+    }
+
 	const cacheModelFiles = async () => {
 		let data = await fetch('/stl.zip').then((data) => data.arrayBuffer());
 
@@ -54,13 +73,13 @@
 	};
 
 	const createScene = async () => {
-		sceneManager = new SceneBuilder()
+		sceneManager
 			.addRenderer({ antialias: true, canvas: canvas, alpha: true })
 			.addPerspectiveCamera({ x: -0.5, y: 0.5, z: 1 })
-			.addOrbitControls(10, 30)
+			.addOrbitControls(8, 30)
 			.addSky()
-			.addGroundPlane({ x: 0, y: -2, z: 0 })
-			.addGridHelper({ size: 250, divisions: 125, y: -2 })
+			.addGroundPlane()
+			.addGridHelper({ size: 250, divisions: 125 })
 			.addAmbientLight({ color: 0xffffff, intensity: 0.7 })
 			.addDirectionalLight({ x: 10, y: 100, z: 10, color: 0xffffff, intensity: 1 })
 			.addArrowHelper({ origin: { x: 0, y: 0, z: 0 }, direction: { x: 0, y: -2, z: 0 } })
@@ -72,6 +91,14 @@
 			.startRenderLoop();
 
 		addVideoStream();
+
+		for (let i = 0; i < 4; i++) {
+			const geometry = new BufferGeometry();
+			const material = new LineBasicMaterial({ color: footColor() });
+			const line = new Line(geometry, material);
+			trace_lines.push(geometry);
+			sceneManager.scene.add(line);
+		}
 	};
 
 	const addVideoStream = () => {
@@ -92,16 +119,30 @@
 		texture.needsUpdate = true;
 	};
 
+    const renderTraceLines = (foot_positions: Vector3[]) => {
+        if (!settings['Trace feet']) {
+            if (!feet_trace.length) return 
+            trace_lines.forEach((line, i) => line.setFromPoints(feet_trace[i].slice(-1)))
+            feet_trace = new Array(4).fill([])
+            return
+        }
+        
+        trace_lines.forEach((line, i) => {
+            feet_trace[i].push(foot_positions[i])
+            feet_trace[i] = feet_trace[i].slice(-settings['Trace points'])
+            line.setFromPoints(feet_trace[i]);
+        })
+    }
+
 	const render = () => {
 		const robot = sceneManager.model;
 		if (!robot) return;
 
-		const forwardKinematics = new ForwardKinematics();
+        const toes = toeWorldPositions(robot)
 
-		const points = forwardKinematics.calculateFootpoints(
-			modelAngles.map((ang) => degToRad(ang)) as number[]
-		);
-		robot.position.y = Math.max(...points.map((coord) => coord[0] / 100)) - 2.7;
+        renderTraceLines(toes)
+
+		robot.position.y = robot.position.y - Math.min(...toes.map(toe => toe.y));
 		robot.rotation.z = lerp(robot.rotation.z, degToRad($mpu.heading + 90), 0.1);
 		modelTargetAngles = $servoAngles;
 

app/src/lib/sceneBuilder.ts

@@ -131,11 +131,11 @@ export default class SceneBuilder {
 		return this;
 	};
 
-	public addGroundPlane = (options: position) => {
+	public addGroundPlane = (options?: position) => {
 		this.ground = new Mesh(new PlaneGeometry(), new ShadowMaterial({ side: 2 }));
 		this.ground.rotation.x = -Math.PI / 2;
 		this.ground.scale.setScalar(30);
-		this.ground.position.set(options.x ?? 0, options.y ?? 0, options.z ?? 0);
+		this.ground.position.set(options?.x ?? 0, options?.y ?? 0, options?.z ?? 0);
 		this.ground.receiveShadow = true;
 		this.scene.add(this.ground);
 		return this;

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

@@ -1,4 +1,3 @@
-export const webAppBuild = import.meta.env.MODE === 'WEB';
 export const hostname = window.location.hostname;
 
 export const isSecure = window.location.protocol === 'https:';

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})`);
+};

app/src/routes/Controller.svelte

@@ -7,9 +7,9 @@
 
 <div class="flex justify-center items-center w-full h-full">
 	{#if $emulateModel}
-		<Model />
+    <Model />
 	{:else}
-		<Stream />
+    <Stream />
 	{/if}
 	<Controls />
 </div>