✨ Refactors and adds new mode concept

.vscode/settings.json

@@ -11,6 +11,7 @@
       "Leika",
       "lerp",
       "MDNS",
+      "nipplejs",
       "Psram",
       "smnc",
       "ssid",

app/src/App.svelte

@@ -6,15 +6,16 @@
 	import Controller from './routes/Controller.svelte';
 	import { fileService } from '$lib/services';
 	import Settings from './routes/Settings.svelte';
-	import { jointNames, model, outControllerData } from '$lib/store';
+	import { jointNames, model, outControllerData, mode } from '$lib/stores';
-	import { loadModelAsync } from '$lib/utilities';
+	import { loadModelAsync, socketLocation } from '$lib/utilities';
-	import { socketLocation } from '$lib/utilities';
 	import type { Result } from '$lib/utilities/result';
 
 	export let url = window.location.pathname;
 	onMount(async () => {
 		socketService.connect(socketLocation);
 		socketService.addPublisher(outControllerData);
+		socketService.addPublisher(mode, 'mode');
+
 		registerFetchIntercept();
 		const modelRes = await loadModelAsync('/spot_micro.urdf.xacro');
 

app/src/components/Controls.svelte

@@ -1,18 +1,15 @@
 <script lang="ts">
 	import nipplejs from 'nipplejs';
 	import { onMount } from 'svelte';
-	import { throttler, toInt8 } from '$lib/utilities';
+	import { capitalize, throttler, toInt8 } from '$lib/utilities';
-	import socketService from '$lib/services/socket-service';
+	import { input, outControllerData, mode, modes } from '$lib/stores';
-	import { emulateModel, input, outControllerData } from '$lib/store';
+	import type { vector } from '$lib/models';
 
 	let throttle = new throttler();
 	let left: nipplejs.JoystickManager;
 	let right: nipplejs.JoystickManager;
 
 	let throttle_timing = 40;
-
-	let mode = 'rest'; // 'rest' | 'stand' | 'stand+' | 'walk'
-
 	let data = new Int8Array(6);
 
 	onMount(() => {
@@ -24,22 +21,6 @@
 			restOpacity: 0.3
 		});
 
-		left.on('move', (evt, data) => {
-			input.update((o) => {
-				o.left = data.vector;
-				return o;
-			});
-			throttle.throttle(updateData, throttle_timing);
-		});
-
-		left.on('end', (evt, data) => {
-			input.update((o) => {
-				o.left = { x: 0, y: 0 };
-				return o;
-			});
-			throttle.throttle(updateData, throttle_timing);
-		});
-
 		right = nipplejs.create({
 			zone: document.getElementById('right') as HTMLElement,
 			color: 'grey',
@@ -48,22 +29,19 @@
 			restOpacity: 0.3
 		});
 
-		right.on('move', (evt, data) => {
+		left.on('move', (_, data) => handleJoyMove('left', data.vector));
-			input.update((o) => {
+		left.on('end', (_, __) => handleJoyMove('left', { x: 0, y: 0 }));
-				o.right = data.vector;
+		right.on('move', (_, data) => handleJoyMove('right', data.vector));
-				return o;
+		right.on('end', (_, __) => handleJoyMove('right', { x: 0, y: 0 }));
-			});
-			throttle.throttle(updateData, throttle_timing);
 	});
 
-		right.on('end', (evt, data) => {
+	const handleJoyMove = (key: 'left' | 'right', data: vector) => {
-			input.update((o) => {
+		input.update((inputData) => {
-				o.right = { x: 0, y: 0 };
+			inputData[key] = data;
-				return o;
+			return inputData;
 		});
 		throttle.throttle(updateData, throttle_timing);
-		});
+	};
-	});
 
 	const updateData = () => {
 		data[0] = 0;
@@ -75,8 +53,22 @@
 		data[6] = toInt8($input.speed, 0, 100);
 
 		outControllerData.set(data);
+	};
 
-        if (!$emulateModel) socketService.send(data);
+	const handleKeyup = (event: KeyboardEvent) => {
+		const down = event.type === 'keydown';
+		input.update((data) => {
+			if (event.key === 'w') data.left.y = down ? -1 : 0;
+			if (event.key === 'a') data.left.x = down ? -1 : 0;
+			if (event.key === 's') data.left.y = down ? 1 : 0;
+			if (event.key === 'd') data.left.x = down ? 1 : 0;
+			return data;
+		});
+		throttle.throttle(updateData, throttle_timing);
+	};
+
+	const changeMode = (modeValue: Modes) => {
+		mode.set(modeValue);
 	};
 </script>
 
@@ -86,4 +78,15 @@
 		<div class="flex-1" />
 		<div id="right" class="flex w-60 items-center" />
 	</div>
+	<div class="absolute bottom-0 z-10 p-4 gap-4 flex">
+		{#each modes as modeValue}
+			<button
+				on:click={() => changeMode(modeValue)}
+				class="rounded-md outline outline-2 text-zinc-200 outline-zinc-600 p-2">
+                {capitalize(modeValue)}
+            </button>
+		{/each}
 	</div>
+</div>
+
+<svelte:window on:keyup={handleKeyup} on:keydown={handleKeyup} />

app/src/components/Topbar.svelte

@@ -1,8 +1,9 @@
 <script lang="ts">
 	import socketService from '$lib/services/socket-service';
 	import { Icon, Bars3, XMark, Power, Battery100, Signal, SignalSlash } from 'svelte-hero-icons';
-	import { emulateModel } from '$lib/store';
+	import { emulateModel } from '$lib/stores';
 	import { Link, useLocation } from 'svelte-routing';
+	import { isConnected } from '$lib/stores';
 
 	const views = ['Virtual environment', 'Robot camera'];
 	const modes = ['Drive', 'Choreography'];
@@ -12,7 +13,6 @@
 	let selected_view = views[0];
 	let selected_modes = modes[0];
 	let settingOpen = window.location.pathname.includes('/settings');
-	let isConnected = socketService.isConnected;
 
 	$: emulateModel.set(selected_view === views[0]);
 	$: settingOpen = $location.pathname.includes('/settings');

app/src/components/Views/Model.svelte

@@ -3,11 +3,11 @@
 	import { CanvasTexture, CircleGeometry, Mesh, MeshBasicMaterial } from 'three';
 	import socketService from '$lib/services/socket-service';
 	import uzip from 'uzip';
-	import { model } from '$lib/store';
+	import { model } from '$lib/stores';
 	import { ForwardKinematics } from '$lib/kinematic';
 	import { location } from '$lib/utilities';
 	import { fileService } from '$lib/services';
-	import { servoAngles, mpu } from '$lib/stores';
+	import { servoAngles, mpu, jointNames } from '$lib/stores';
 	import SceneBuilder from '$lib/sceneBuilder';
 	import { lerp, degToRad } from 'three/src/math/MathUtils';
 
@@ -18,34 +18,9 @@
 	let modelAngles: number[] | Int16Array = new Array(12).fill(0);
 	let modelTargetAngles: number[] | Int16Array = new Array(12).fill(0);
 
-	let modelBodyAngles: EulerAngle = { omega: 0, phi: 0, psi: 0 };
-	let modelTargeBodyAngles: EulerAngle = { omega: 0, phi: 0, psi: 0 };
-
 	const videoStream = `//${location}/api/stream`;
 
-	let showModel = true,
+	let showStream = false;
-		showStream = false;
-
-	const servoNames = [
-		'front_left_shoulder',
-		'front_left_leg',
-		'front_left_foot',
-		'front_right_shoulder',
-		'front_right_leg',
-		'front_right_foot',
-		'rear_left_shoulder',
-		'rear_left_leg',
-		'rear_left_foot',
-		'rear_right_shoulder',
-		'rear_right_leg',
-		'rear_right_foot'
-	];
-
-	interface EulerAngle {
-		omega: number;
-		phi: number;
-		psi: number;
-	}
 
 	onMount(async () => {
 		await cacheModelFiles();
@@ -68,12 +43,12 @@
 	};
 
 	const updateAngles = (name: string, angle: number) => {
-		modelTargetAngles[servoNames.indexOf(name)] = angle * (180 / Math.PI);
+		modelTargetAngles[$jointNames.indexOf(name)] = angle * (180 / Math.PI);
 		socketService.send(
 			JSON.stringify({
 				type: 'kinematic/angle',
 				angle: angle * (180 / Math.PI),
-				id: servoNames.indexOf(name)
+				id: $jointNames.indexOf(name)
 			})
 		);
 	};
@@ -100,7 +75,7 @@
 	};
 
 	const addVideoStream = () => {
-		context = streamCanvas.getContext('2d');
+		context = streamCanvas.getContext('2d')!;
 		texture = new CanvasTexture(stream);
 		const liveStream = new Mesh(
 			new CircleGeometry(35, 32),
@@ -132,26 +107,14 @@
 
 		handleVideoStream();
 
-		for (let i = 0; i < servoNames.length; i++) {
+		for (let i = 0; i < $jointNames.length; i++) {
 			modelAngles[i] = lerp(
-				robot.joints[servoNames[i]].angle * (180 / Math.PI),
+				(robot.joints[$jointNames[i]].angle as number) * (180 / Math.PI),
 				modelTargetAngles[i],
 				0.1
 			);
-			robot.joints[servoNames[i]].setJointValue(degToRad(modelAngles[i]));
+			robot.joints[$jointNames[i]].setJointValue(degToRad(modelAngles[i]));
 		}
-
-		modelBodyAngles.omega = lerp(
-			robot.rotation.x * (180 / Math.PI),
-			modelTargeBodyAngles.omega - 90,
-			0.1
-		);
-		modelBodyAngles.phi = lerp(robot.rotation.y * (180 / Math.PI), modelTargeBodyAngles.phi, 0.1);
-		modelBodyAngles.psi = lerp(
-			robot.rotation.z * (180 / Math.PI),
-			modelTargeBodyAngles.psi + 90,
-			0.1
-		);
 	};
 </script>
 

app/src/components/settings/Calibration.svelte

@@ -1,6 +1,6 @@
 <script lang="ts">
 	import { onMount } from 'svelte';
-	import { jointNames } from '../../lib/store';
+	import { jointNames } from '../../lib/stores';
 
 	type Servo = {
 		id: number;
@@ -10,7 +10,7 @@
 		pwmFor180: number;
 	};
 
-	let servos: any[] = [];
+	let servos: Servo[] = [];
 
 	onMount(() => {
 		jointNames.subscribe((data) => {
@@ -58,7 +58,8 @@
 				id="minPWM"
 				class="bg-zinc-800"
 				value={servos[selectedServo].minPWM}
-				on:blur={(event) => updateServoValue(selectedServo, 'minPWM', Number(event.target.value))}
+				on:blur={(event) =>
+					updateServoValue(selectedServo ?? 0, 'minPWM', Number(event.target?.value))}
 			/>
 
 			<label for="maxPWM">Max PWM:</label>
@@ -67,7 +68,8 @@
 				id="maxPWM"
 				class="bg-zinc-800"
 				value={servos[selectedServo].maxPWM}
-				on:blur={(event) => updateServoValue(selectedServo, 'maxPWM', Number(event.target.value))}
+				on:blur={(event) =>
+					updateServoValue(selectedServo ?? 0, 'maxPWM', Number(event.target?.value))}
 			/>
 
 			<label for="pwmFor180">PWM for 180°:</label>
@@ -77,7 +79,7 @@
 				class="bg-zinc-800"
 				value={servos[selectedServo].pwmFor180}
 				on:blur={(event) =>
-					updateServoValue(selectedServo, 'pwmFor180', Number(event.target.value))}
+					updateServoValue(selectedServo ?? 0, 'pwmFor180', Number(event.target?.value))}
 			/>
 		</div>
 	{/if}

app/src/components/settings/Configuration.svelte

@@ -1,10 +1,8 @@
 <script lang="ts">
-	import socketService from '$lib/services/socket-service';
+	import { socketService } from '$lib/services';
+	import { isConnected, settings } from '$lib/stores';
 	import { onMount } from 'svelte';
 
-	let isConnected = socketService.isConnected;
-	let settings = socketService.settings;
-
 	onMount(() => {
 		if ($isConnected) {
 			const message = JSON.stringify({ type: 'system/settings' });

app/src/components/settings/Info.svelte

@@ -2,10 +2,7 @@
 	import { onMount } from 'svelte';
 	import { humanFileSize } from '$lib/utilities';
 	import socketService from '$lib/services/socket-service';
-
+	import { isConnected, systemInfo } from '$lib/stores';
-	let isConnected = socketService.isConnected;
-	let settings = socketService.settings;
-	let systemInfo = socketService.systemInfo;
 
 	onMount(() => {
 		if ($isConnected) {

app/src/components/settings/Log.svelte

@@ -1,10 +1,8 @@
 <script lang="ts">
 	import socketService from '$lib/services/socket-service';
+	import { isConnected, logs } from '$lib/stores';
 	import { onMount } from 'svelte';
 
-	let isConnected = socketService.isConnected;
-	let log = socketService.log;
-
 	onMount(() => {
 		if ($isConnected) {
 			const message = JSON.stringify({ type: 'system/logs' });
@@ -14,7 +12,7 @@
 </script>
 
 <div class="w-full h-full">
-	{#each $log as entry}
+	{#each $logs as entry}
 		<div>{entry}</div>
 	{/each}
 </div>

app/src/lib/models.ts

@@ -1,11 +1,20 @@
+export type vector = { x: number; y: number };
+
+export interface ControllerInput {
+	left: vector;
+	right: vector;
+	height: number;
+	speed: number;
+}
+
 export type angles = number[] | Int16Array;
 
-type AnglesData = {
+export type AnglesData = {
 	type: 'angles';
 	data: angles;
 };
 
-type LogData = {
+export type LogData = {
 	type: 'log';
 	data: string;
 };

app/src/lib/services/socket-service.ts

@@ -39,7 +39,9 @@ class SocketService {
 	}
 
 	public addPublisher(store: Writable<WebsocketOutData>, type?: string) {
-		store.subscribe((data) => this.send(type ? JSON.stringify({ type, data }) : data));
+		const publish = (data: WebsocketOutData) =>
+			this.send(type ? JSON.stringify({ type, data }) : data);
+		store.subscribe(publish);
 	}
 
 	private handleConnected(): void {

app/src/lib/stores/index.ts

@@ -1,2 +1,3 @@
 export * from './socket-store';
 export * from './logging-store';
+export * from './model-store';

app/src/lib/stores/model-store.ts

@@ -1,17 +1,24 @@
-import { writable } from 'svelte/store';
+import type { ControllerInput } from '$lib/models';
 import { persistentStore } from '$lib/utilities';
+import { writable, type Writable } from 'svelte/store';
 
 export const emulateModel = writable(true);
 
-export const input = writable({
+export const jointNames = persistentStore('joint_names', []);
+
+export const model = writable();
+
+export const modes = ['idle', 'stand', 'walk'] as const;
+
+export type Modes = (typeof modes)[number];
+
+export const mode: Writable<Modes> = writable('idle');
+
+export const outControllerData = writable(new Int8Array([0, 0, 0, 0, 0, 70, 0]));
+
+export const input: Writable<ControllerInput> = writable({
 	left: { x: 0, y: 0 },
 	right: { x: 0, y: 0 },
 	height: 70,
 	speed: 0
 });
-
-export const outControllerData = writable(new Int8Array([0, 0, 0, 0, 0, 70, 0]));
-
-export const jointNames = persistentStore('joint_names', []);
-
-export const model = writable();

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

@@ -3,3 +3,7 @@ export const humanFileSize = (size: number): string => {
 	var i = size == 0 ? 0 : Math.floor(Math.log(size) / Math.log(1024));
 	return Number((size / Math.pow(1024, i)).toFixed(2)) * 1 + units[i];
 };
+
+export const capitalize = (str: string): string => {
+	return str.charAt(0).toUpperCase() + str.slice(1).toLowerCase();
+};

app/src/main.ts

@@ -3,7 +3,7 @@ import './index.css';
 import App from './App.svelte';
 
 const app = new App({
-	target: document.getElementById('app')
+	target: document.getElementById('app') as HTMLElement
 });
 
 export default app;

app/src/routes/Controller.svelte

@@ -2,7 +2,7 @@
 	import Stream from '$components/Views/Stream.svelte';
 	import Model from '$components/Views/Model.svelte';
 	import Controls from '$components/Controls.svelte';
-	import { emulateModel } from '$lib/store';
+	import { emulateModel } from '$lib/stores';
 </script>
 
 <div class="flex justify-center items-center w-full h-full">

app/src/routes/Settings.svelte

@@ -5,13 +5,10 @@
 	import Configuration from '../components/settings/Configuration.svelte';
 	import {
 		Icon,
-		Wifi,
-		CommandLine,
 		InformationCircle,
 		BookOpen,
 		AdjustmentsVertical,
-		Cog6Tooth,
+		Cog6Tooth
-		Newspaper
 	} from 'svelte-hero-icons';
 	import Calibration from '../components/settings/Calibration.svelte';
 

app/test/specs/number-convert.spec.ts

@@ -42,5 +42,3 @@ describe('toInt8', () => {
 		expect(toInt8(2, -1, 1)).toBe(127);
 	});
 });
-
-

mock/server.js

@@ -19,22 +19,29 @@ const randomFloatFromInterval = (min, max) =>
 const radToDeg = (val) => val * (180 / Math.PI);
 const degToRad = (val) => val * (Math.PI / 180);
 
+const lerp = (start, end, amt) => {
+  return (1 - amt) * start + amt * end;
+};
+
 function createNewClientState() {
   return {
     model: JSON.parse(JSON.stringify(model)),
     settings: JSON.parse(JSON.stringify(settings)),
     logs: JSON.parse(JSON.stringify(logs)),
     subscriptions: {},
+    mode: "idle",
+    controller: {
+      stop: 0,
+      lx: 0,
+      ly: 0,
+      rx: 0,
+      ry: 0,
+      h: 70,
+      s: 0,
+    },
   };
 }
 
-function subscribeClientToCategory(ws, category) {
-  if (!subscriptions[category]) {
-    subscriptions[category] = new Set();
-  }
-  subscriptions[category].add(ws);
-}
-
 const unsubscribeClientFromCategory = (ws, category) => {
   if (!subscriptions[category]) return;
   subscriptions[category].delete(ws);
@@ -56,7 +63,7 @@ if (!Array.prototype.last){
   Array.prototype.last = function () {
     return this[this.length - 1];
   };
-};
+}
 
 const model = {
   battery: {
@@ -90,6 +97,8 @@ const model = {
   },
   running: true,
   mode: "stand",
+  rotation: [0, 0, 0],
+  position: [0, 0, 0],
 };
 
 const settings = {
@@ -217,28 +226,111 @@ const unpackMessageBuffer = (data) => {
   };
 };
 
-const updateStanding = (ws, controller) => {
+const rest = {
-  if (!ws.clientState.model.running) return;
+  rotation: [0, 0, 0],
-  const data = unpackMessageBuffer(controller);
+  position: [0, 10, 0],
-  ws.send(
+};
+
+const idle = (client) => {
+  for (let i = 0; i < 3; i++) {
+    client.clientState.model.position[i] = lerp(
+      client.clientState.model.position[i],
+      rest.position[i],
+      0.01
+    );
+    client.clientState.model.rotation[i] = lerp(
+      client.clientState.model.rotation[i],
+      rest.rotation[i],
+      0.01
+    );
+  }
+  client.send(
     JSON.stringify({
       type: "angles",
-      data: updateBodyState(ws.clientState.model, data.angles, data.position),
+      data: updateBodyState(
+        client.clientState.model,
+        client.clientState.model.rotation,
+        client.clientState.model.position
+      ),
     })
   );
 };
 
+const stand = (client) => {
+  if (!client.clientState.model.running) return;
+  const data = unpackMessageBuffer(client.clientState.controller);
+  client.send(
+    JSON.stringify({
+      type: "angles",
+      data: updateBodyState(
+        client.clientState.model,
+        data.angles,
+        data.position
+      ),
+    })
+  );
+};
+
+// https://www.hindawi.com/journals/cin/2016/9853070/
+
+const step = (model, controller, tick) => {
+  const y1 = -100 * Math.sin(-0.05 * tick) - 150;
+  const y2 = -100 * Math.sin(-0.05 * tick + Math.PI) - 150;
+  const x1 = Math.abs((tick % 120) - 60) - 60;
+  const Lp = [
+    // -50  is minimum
+    [100, y1, 100, 1],
+    [100, y2, -100, 1],
+    [-100, y2, 100, 1],
+    [-100, y1, -100, 1],
+  ];
+
+  model.servos.angles = kinematic
+    .calcIK(
+      Lp,
+      model.rotation.map((x) => degToRad(x)),
+      model.position
+    )
+    .flat()
+    .map((x, i) => radToDeg(x * model.servos.dir[i]));
+  return model.servos.angles;
+};
+
+const walk = (client) => {
+  const angles = step(
+    client.clientState.model,
+    client.clientState.controller,
+    client.tick
+  );
+  client.send(JSON.stringify({ type: "angles", data: angles }));
+};
+
+const start_dynamics = (client) => {
+  client.tick = 0;
+  client.clientState.mode = "idle";
+  client.clientState.next_mode = "walk";
+  const modes = { idle, stand, walk };
+  client.id = setInterval(() => {
+    client.tick += 1;
+
+    if (client.clientState.mode !== client.clientState.next_mode) {
+      // Transition
+      client.clientState.mode = client.clientState.next_mode;
+    } else {
+      modes[client.clientState.mode](client);
+    }
+  }, 10);
+};
+
 const handelController = (ws, buffer) => {
   const controllerData = bufferToController(new Int8Array(buffer));
+  ws.clientState.controller = controllerData;
   if (controllerData.stop) {
     ws.clientState.model.running = false;
     ws.clientState.logs.push("[2024-02-05 19:10:00] [Warning] STOPPING SERVOS");
     ws.send(JSON.stringify({ type: "log", data: ws.clientState.logs.last() }));
     return;
   }
-  if (ws.clientState.model.mode === "stand") {
-    updateStanding(ws, controllerData);
-  }
 };
 
 const handleBufferMessage = (ws, buffer) => {
@@ -247,19 +339,11 @@ const handleBufferMessage = (ws, buffer) => {
   }
 };
 
-const handleJsonMessage = (ws, message) => {
+const handleJsonMessage = (ws, data) => {
-  let data = message;
-  try {
-    data = JSON.parse(message);
-  } catch (error) {
-    return;
-  }
   switch (data.type) {
-    case "subscribe":
+    case "mode":
-      subscribeClientToCategory(ws, data.category);
+      ws.clientState.next_mode = data.data;
-      break;
+      //   ws.send({ type: "battery", data: JSON.stringify(updateBattery()) });
-    case "unsubscribe":
-      unsubscribeClientFromCategory(ws, data.category);
       break;
     case "sensor/battery":
       ws.send({ type: "battery", data: JSON.stringify(updateBattery()) });
@@ -353,15 +437,17 @@ const handleJsonMessage = (ws, message) => {
 wss.on("connection", (ws) => {
   const clientState = createNewClientState();
   ws.clientState = clientState;
+  start_dynamics(ws);
   ws.on("error", console.error);
 
   ws.on("message", (message) => {
-    if (typeof message === "object") {
+    let data = message;
+    try {
+      data = JSON.parse(message);
+      handleJsonMessage(ws, data);
+    } catch (error) {
       handleBufferMessage(ws, message);
-      return;
     }
-
-    handleJsonMessage(ws, message);
   });
 
   ws.on("close", () => {