🐾 Adds foot tracing and gui panel

app/public/spot_micro.urdf.xacro

@@ -1,13 +1,13 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="spot_micro_rviz">
 
-  <material name="white">
+    <material name="shell_color">
         <color rgba="1 1 1 1" />
     </material>
-  <material name="black">
+    <material name="body_color">
         <color rgba="0.1 0.1 0.1 1" />
     </material>
-  <material name="blue">
+    <material name="foot_color">
         <color rgba="0 0.75 1 1" />
     </material>
 
@@ -50,7 +50,7 @@
                     </geometry>
                     <origin rpy="0 0 3.14159" xyz="0.135 0.095 -0.01" />
                 </xacro:unless>
-        <material name="black"/>
+                <material name="body_color" />
             </visual>
             <collision>
                 <geometry>
@@ -91,7 +91,7 @@
                     </geometry>
                     <origin rpy="0 -0.139 3.14159" xyz="0.130 0.15 -0.025" />
                 </xacro:unless>
-        <material name="white"/>
+                <material name="shell_color" />
             </visual>
         </link>
         <link name="${name}">
@@ -108,12 +108,12 @@
                     </geometry>
                     <origin rpy="0 -0.139 3.14159" xyz="0.130 0.15 -0.025" />
                 </xacro:unless>
-        <material name="black"/>
+                <material name="body_color" />
                 <!--        <geometry>
           <box size="0.028 0.036 ${leg_length}"/>
         </geometry>
         <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.050"/>
-        <material name="white"/>-->
+        <material name="shell_color"/>-->
       </visual>
       <collision>
         <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.050"/>
@@ -160,7 +160,7 @@
         </geometry>
                     <origin rpy="0 0 3.14159" xyz="0.120 0.15 0.1" />
         </xacro:unless>
-        <material name="black"/>
+                <material name="body_color" />
       </visual>
       <collision>
         <geometry>
@@ -193,7 +193,7 @@
                     <mesh filename="package://stl/foot.stl" scale="0.001 0.001 0.001" />
         </geometry>
                 <origin rpy="0 -0.40010 3.14159" xyz="0.00 0.01 0.015" />
-        <material name="blue"/>
+                <material name="foot_color" />
       </visual>
       <collision>
         <geometry>
@@ -236,7 +236,7 @@
       <geometry>
                 <mesh filename="package://stl/mainbody.stl" scale="0.001 0.001 0.001" />
       </geometry>
-      <material name="black"/> 
+            <material name="body_color" /> 
       <origin rpy="0 0 0" xyz="-0.042 -0.055 -0.010"/>
     </visual>
     <collision>
@@ -258,7 +258,7 @@
                 <mesh filename="package://stl/backpart.stl" scale="0.001 0.001 0.001" />
       </geometry>
             <origin rpy="0 0 3.14159" xyz="0.04 0.055 -0.010" />
-      <material name="white"/>
+            <material name="shell_color" />
     </visual>
     <collision>
       <geometry>
@@ -282,7 +282,7 @@
                 <mesh filename="package://stl/frontpart.stl" scale="0.001 0.001 0.001" />
       </geometry>
             <origin rpy="0 0 3.14159" xyz="0.040 0.055 -0.010" />
-      <material name="white"/>
+            <material name="shell_color" />
     </visual>
     <collision>
       <geometry>

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 { footColor, isEmbeddedApp, location, toeWorldPositions } from '$lib/utilities';
-	import { location } 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 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 })
+			.addGroundPlane()
-			.addGridHelper({ size: 250, divisions: 125, y: -2 })
+			.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(
+        renderTraceLines(toes)
-			modelAngles.map((ang) => degToRad(ang)) as number[]
+
-		);
+		robot.position.y = robot.position.y - Math.min(...toes.map(toe => toe.y));
-		robot.position.y = Math.max(...points.map((coord) => coord[0] / 100)) - 2.7;
 		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();
+				model_xml = xml;
-
-				urdfLoader.workingPath = LoaderUtils.extractUrlBase(url);
-
 				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})`);
+};