From f41d5a7949aba35173a03511fb8c8886b8cd8cd4 Mon Sep 17 00:00:00 2001
From: Rune Harlyk <rune@harlyk.dk>
Date: Sun, 25 Feb 2024 02:04:50 +0100
Subject: [PATCH] =?UTF-8?q?=F0=9F=90=BE=20Adds=20foot=20tracing=20and=20gu?=
 =?UTF-8?q?i=20panel?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 app/public/spot_micro.urdf.xacro            | 236 ++++++++++----------
 app/src/app.css                             |   5 +
 app/src/components/Views/Model.svelte       |  71 ++++--
 app/src/lib/sceneBuilder.ts                 |   4 +-
 app/src/lib/utilities/location-utilities.ts |   1 -
 app/src/lib/utilities/model-utilities.ts    |  23 +-
 app/src/routes/Controller.svelte            |   4 +-
 7 files changed, 201 insertions(+), 143 deletions(-)

diff --git a/app/public/spot_micro.urdf.xacro b/app/public/spot_micro.urdf.xacro
index 7d997bf..e6b2dae 100644
--- a/app/public/spot_micro.urdf.xacro
+++ b/app/public/spot_micro.urdf.xacro
@@ -1,119 +1,119 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="spot_micro_rviz">
 
-  <material name="white">
-    <color rgba="1 1 1 1"/>
-  </material>
-  <material name="black">
-    <color rgba="0.1 0.1 0.1 1"/>
-  </material>
-  <material name="blue">
-    <color rgba="0 0.75 1 1"/>
-  </material>
+    <material name="shell_color">
+        <color rgba="1 1 1 1" />
+    </material>
+    <material name="body_color">
+        <color rgba="0.1 0.1 0.1 1" />
+    </material>
+    <material name="foot_color">
+        <color rgba="0 0.75 1 1" />
+    </material>
 
-  <!-- Params -->
+    <!-- Params -->
 
-  <xacro:property name="body_length" value="0.140" />
-  <xacro:property name="body_width" value="0.110" />
-  <xacro:property name="body_height" value="0.070" />
+    <xacro:property name="body_length" value="0.140" />
+    <xacro:property name="body_width" value="0.110" />
+    <xacro:property name="body_height" value="0.070" />
 
-  <xacro:property name="front_length" value="0.058" />
-  <xacro:property name="rear_length" value="0.040" />
+    <xacro:property name="front_length" value="0.058" />
+    <xacro:property name="rear_length" value="0.040" />
 
-  <xacro:property name="shoulder_length" value="0.044" />
-  <xacro:property name="shoulder_width" value="0.038" />
+    <xacro:property name="shoulder_length" value="0.044" />
+    <xacro:property name="shoulder_width" value="0.038" />
 
-  <xacro:property name="leg_length" value="0.1075" />
-  <xacro:property name="foot_length" value="0.130" />
+    <xacro:property name="leg_length" value="0.1075" />
+    <xacro:property name="foot_length" value="0.130" />
 
-  <xacro:property name="toe_radius" value="0.020" />
-  <!--  <xacro:property name="toe_radius" value="0.014" /> -->
-  <xacro:property name="toe_width" value="0.020" />
-  <xacro:property name="shift" value="0.055" />
-  <xacro:property name="shiftx" value="0.093" />
-  <xacro:property name="shifty" value="0.039" />
+    <xacro:property name="toe_radius" value="0.020" />
+    <!--  <xacro:property name="toe_radius" value="0.014" /> -->
+    <xacro:property name="toe_width" value="0.020" />
+    <xacro:property name="shift" value="0.055" />
+    <xacro:property name="shiftx" value="0.093" />
+    <xacro:property name="shifty" value="0.039" />
 
-  <!-- Macros -->
+    <!-- Macros -->
 
-  <xacro:macro name="gen_shoulder" params="name left">
-    <link name="${name}">
-      <visual>
-        <xacro:if value="${left}">
-        <geometry>
-          <mesh filename="package://stl/lshoulder.stl" scale="0.001 0.001 0.001"/>
-        </geometry>
-        <origin rpy="0 0 3.14159" xyz="0.135 0.015 -0.01"/>
-        </xacro:if>
-        <xacro:unless value="${left}">
-        <geometry>
-          <mesh filename="package://stl/rshoulder.stl" scale="0.001 0.001 0.001"/>
-        </geometry>
-        <origin rpy="0 0 3.14159" xyz="0.135 0.095 -0.01"/>
-        </xacro:unless>
-        <material name="black"/>
-      </visual>
-      <collision>
-        <geometry>
-          <box size="${shoulder_length} ${shoulder_width} ${body_height}"/>
-        </geometry>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-      </collision>
-      <inertial>
-        <mass value="0.10"/>
-        <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
-      </inertial>
-    </link>
-  </xacro:macro>
+    <xacro:macro name="gen_shoulder" params="name left">
+        <link name="${name}">
+            <visual>
+                <xacro:if value="${left}">
+                    <geometry>
+                        <mesh filename="package://stl/lshoulder.stl" scale="0.001 0.001 0.001" />
+                    </geometry>
+                    <origin rpy="0 0 3.14159" xyz="0.135 0.015 -0.01" />
+                </xacro:if>
+                <xacro:unless value="${left}">
+                    <geometry>
+                        <mesh filename="package://stl/rshoulder.stl" scale="0.001 0.001 0.001" />
+                    </geometry>
+                    <origin rpy="0 0 3.14159" xyz="0.135 0.095 -0.01" />
+                </xacro:unless>
+                <material name="body_color" />
+            </visual>
+            <collision>
+                <geometry>
+                    <box size="${shoulder_length} ${shoulder_width} ${body_height}" />
+                </geometry>
+                <origin rpy="0 0 0" xyz="0 0 0" />
+            </collision>
+            <inertial>
+                <mass value="0.10" />
+                <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
+            </inertial>
+        </link>
+    </xacro:macro>
 
-  <xacro:macro name="gen_shoulder_joint" params="pos shiftx shifty">
-    <joint name="${pos}_shoulder" type="revolute">
-      <parent link="base_link"/>
-      <child link="${pos}_shoulder_link"/>
-      <axis xyz="1 0 0"/>
-      <origin rpy="0 0 0" xyz="${shiftx} ${shifty} 0"/>
-      <limit effort="1000.0" lower="-0.548" upper="0.548" velocity="0.7"/>
-      <dynamics damping="0.0" friction="0.5"/>
-    </joint>
-  </xacro:macro>
+    <xacro:macro name="gen_shoulder_joint" params="pos shiftx shifty">
+        <joint name="${pos}_shoulder" type="revolute">
+            <parent link="base_link" />
+            <child link="${pos}_shoulder_link" />
+            <axis xyz="1 0 0" />
+            <origin rpy="0 0 0" xyz="${shiftx} ${shifty} 0" />
+            <limit effort="1000.0" lower="-0.548" upper="0.548" velocity="0.7" />
+            <dynamics damping="0.0" friction="0.5" />
+        </joint>
+    </xacro:macro>
 
-  <xacro:macro name="gen_leg" params="name left">
-    <link name="${name}_cover">
-      <visual>
-        <xacro:if value="${left}">
-        <geometry>
-          <mesh filename="package://stl/larm_cover.stl" scale="0.001 0.001 0.001"/>
-        </geometry>
-        <origin rpy="0 -0.139 3.14159" xyz="0.130 -0.040 -0.025"/>
-        </xacro:if>
-        <xacro:unless value="${left}">
-        <geometry>
-          <mesh filename="package://stl/rarm_cover.stl" scale="0.001 0.001 0.001"/>
-        </geometry>
-        <origin rpy="0 -0.139 3.14159" xyz="0.130 0.15 -0.025"/>
-        </xacro:unless>
-        <material name="white"/>
-      </visual>
-    </link>
-    <link name="${name}">
-      <visual>
-        <xacro:if value="${left}">
-        <geometry>
-          <mesh filename="package://stl/larm.stl" scale="0.001 0.001 0.001"/>
-        </geometry>
-        <origin rpy="0 -0.139 3.14159" xyz="0.130 -0.040 -0.025"/>
-        </xacro:if>
-        <xacro:unless value="${left}">
-        <geometry>
-          <mesh filename="package://stl/rarm.stl" scale="0.001 0.001 0.001"/>
-        </geometry>
-        <origin rpy="0 -0.139 3.14159" xyz="0.130 0.15 -0.025"/>
-        </xacro:unless>
-        <material name="black"/>
-<!--        <geometry>
+    <xacro:macro name="gen_leg" params="name left">
+        <link name="${name}_cover">
+            <visual>
+                <xacro:if value="${left}">
+                    <geometry>
+                        <mesh filename="package://stl/larm_cover.stl" scale="0.001 0.001 0.001" />
+                    </geometry>
+                    <origin rpy="0 -0.139 3.14159" xyz="0.130 -0.040 -0.025" />
+                </xacro:if>
+                <xacro:unless value="${left}">
+                    <geometry>
+                        <mesh filename="package://stl/rarm_cover.stl" scale="0.001 0.001 0.001" />
+                    </geometry>
+                    <origin rpy="0 -0.139 3.14159" xyz="0.130 0.15 -0.025" />
+                </xacro:unless>
+                <material name="shell_color" />
+            </visual>
+        </link>
+        <link name="${name}">
+            <visual>
+                <xacro:if value="${left}">
+                    <geometry>
+                        <mesh filename="package://stl/larm.stl" scale="0.001 0.001 0.001" />
+                    </geometry>
+                    <origin rpy="0 -0.139 3.14159" xyz="0.130 -0.040 -0.025" />
+                </xacro:if>
+                <xacro:unless value="${left}">
+                    <geometry>
+                        <mesh filename="package://stl/rarm.stl" scale="0.001 0.001 0.001" />
+                    </geometry>
+                    <origin rpy="0 -0.139 3.14159" xyz="0.130 0.15 -0.025" />
+                </xacro:unless>
+                <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"/>
@@ -150,17 +150,17 @@
       <visual>
         <xacro:if value="${left}">
         <geometry>
-          <mesh filename="package://stl/lfoot.stl" scale="0.001 0.001 0.001"/>
+                        <mesh filename="package://stl/lfoot.stl" scale="0.001 0.001 0.001" />
         </geometry>
-        <origin rpy="0 0 3.14159" xyz="0.120 -0.04 0.1"/>
+                    <origin rpy="0 0 3.14159" xyz="0.120 -0.04 0.1" />
         </xacro:if>
         <xacro:unless value="${left}">
         <geometry>
-          <mesh filename="package://stl/rfoot.stl" scale="0.001 0.001 0.001"/>
+                        <mesh filename="package://stl/rfoot.stl" scale="0.001 0.001 0.001" />
         </geometry>
-        <origin rpy="0 0 3.14159" xyz="0.120 0.15 0.1"/>
+                    <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>
@@ -190,10 +190,10 @@
     <link name="${name}">
       <visual>
         <geometry>
-        <mesh filename="package://stl/foot.stl" scale="0.001 0.001 0.001"/>
+                    <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"/>
+                <origin rpy="0 -0.40010 3.14159" xyz="0.00 0.01 0.015" />
+                <material name="foot_color" />
       </visual>
       <collision>
         <geometry>
@@ -229,14 +229,14 @@
     <xacro:gen_toe_joint pos="${pos}"/>
   </xacro:macro>
 
-  <!-- Robot Body -->
+    <!-- Robot Body -->
 
   <link name="base_link">
     <visual>
       <geometry>
-        <mesh filename="package://stl/mainbody.stl" scale="0.001 0.001 0.001"/>
+                <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>
@@ -255,10 +255,10 @@
   <link name="rear_link">
     <visual>
       <geometry>
-        <mesh filename="package://stl/backpart.stl" scale="0.001 0.001 0.001"/>
+                <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"/>
+            <origin rpy="0 0 3.14159" xyz="0.04 0.055 -0.010" />
+            <material name="shell_color" />
     </visual>
     <collision>
       <geometry>
@@ -279,10 +279,10 @@
   <link name="front_link">
     <visual>
       <geometry>
-        <mesh filename="package://stl/frontpart.stl" scale="0.001 0.001 0.001"/>
+                <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"/>
+            <origin rpy="0 0 3.14159" xyz="0.040 0.055 -0.010" />
+            <material name="shell_color" />
     </visual>
     <collision>
       <geometry>
@@ -300,7 +300,7 @@
     <child link="front_link"/>
   </joint>
 
-  <!-- create Legs -->
+    <!-- create Legs -->
 
   <xacro:gen_full_leg_joint pos="front_left" shiftx="${shiftx}" shifty="${shifty}" shift="${shift}" left="true"/>
   <xacro:gen_full_leg_joint pos="front_right" shiftx="${shiftx}" shifty="-${shifty}" shift="-${shift}" left="false"/>
diff --git a/app/src/app.css b/app/src/app.css
index 31f8945..c21217b 100644
--- a/app/src/app.css
+++ b/app/src/app.css
@@ -13,3 +13,8 @@
 body {
 	margin: 0;
 }
+
+#three-gui-panel {
+    top: 50px;
+    right:0px
+}
\ No newline at end of file
diff --git a/app/src/components/Views/Model.svelte b/app/src/components/Views/Model.svelte
index b68f5ae..cb43d6f 100644
--- a/app/src/components/Views/Model.svelte
+++ b/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;
 
diff --git a/app/src/lib/sceneBuilder.ts b/app/src/lib/sceneBuilder.ts
index 1ef64d1..8e1ee51 100644
--- a/app/src/lib/sceneBuilder.ts
+++ b/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;
diff --git a/app/src/lib/utilities/location-utilities.ts b/app/src/lib/utilities/location-utilities.ts
index 880196d..2ec372a 100644
--- a/app/src/lib/utilities/location-utilities.ts
+++ b/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:';
diff --git a/app/src/lib/utilities/model-utilities.ts b/app/src/lib/utilities/model-utilities.ts
index 235c46d..a5dd4e3 100644
--- a/app/src/lib/utilities/model-utilities.ts
+++ b/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})`);
+};
diff --git a/app/src/routes/Controller.svelte b/app/src/routes/Controller.svelte
index 88db930..ac332ad 100644
--- a/app/src/routes/Controller.svelte
+++ b/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>