🤖 Adds 3D model view

app/src/App.svelte

@@ -12,13 +12,23 @@
 	export let url = window.location.pathname;
 	onMount(() => {
 		connect(`ws://${location}`);
+        registerFetchIntercept()
 	});
+
+    const registerFetchIntercept = () => {
+        const { fetch: originalFetch } = window;
+        window.fetch = async (...args) => {
+            const cache = await caches.open("files")
+            const [resource, config] = args;
+            return await cache.match(resource) ?? originalFetch(resource, config)
+        };
+    }
 </script>
 
 <Router {url}>
 	<TopBar />
 	<Sidebar />
-	<div class="absolute w-full h-full">
+	<div class="absolute w-full h-full bg-background text-on-background">
 		<Route path="/" component={Controller} />
 		<Route path="/config" component={Config} />
 		<Route path="/health" component={Health} />

app/src/components/Model/ModelView.svelte

@@ -0,0 +1,136 @@
+<script lang="ts">
+import { onMount } from 'svelte';
+import {
+    WebGLRenderer,
+    PerspectiveCamera,
+    Scene,
+    Mesh,
+    PlaneGeometry,
+    ShadowMaterial,
+    DirectionalLight,
+    PCFSoftShadowMap,
+    sRGBEncoding,
+    AmbientLight,
+    MathUtils,
+    LoaderUtils
+} from 'three';
+import { XacroLoader } from 'xacro-parser';
+import { OrbitControls } from 'three/examples/jsm/controls/OrbitControls.js';
+import URDFLoader from 'urdf-loader';
+import { servoBuffer } from '../../lib/socket'
+import { lerp } from '../../lib/utils';
+import uzip from 'uzip';
+	import { outControllerData } from '../../lib/store';
+	import { solve } from '../../lib/kinematics';
+
+let el: HTMLCanvasElement;
+let scene, camera, renderer, robot, controls;
+let angles = new Int8Array(12)
+
+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"]
+
+onMount(async () => {
+    await cacheModelFiles()
+    createScene()
+});
+
+const cacheModelFiles = async () => {
+    const cacheKey = "files"
+    const cache = await caches.open(cacheKey)
+
+    let data = await fetch("/stl.zip").then(data => data.arrayBuffer())
+    
+    var files = uzip.parse(data);
+    
+    for(const [path, data] of Object.entries(files) as [path:string, data:Uint8Array][]){
+        const url = new URL(path, window.location.href)
+        cache.put(url, new Response(data));   
+    }    
+}
+
+const loadModel = () => {
+    const url = '/spot_micro.urdf.xacro';
+    const xacroLoader = new XacroLoader();
+    xacroLoader.load( url, xml => {
+        const urdfLoader = new URDFLoader();
+        urdfLoader.workingPath = LoaderUtils.extractUrlBase( url );
+
+        robot = urdfLoader.parse( xml );
+        robot.rotation.x = Math.PI / 0.6666;
+        robot.rotation.z = Math.PI / 2;
+        robot.traverse(c => c.castShadow = true);
+        robot.updateMatrixWorld(true);
+
+        scene.add( robot );
+
+    },  (error) =>  console.log(error));
+}
+
+const createScene = () => {
+    scene = new Scene();
+    camera = new PerspectiveCamera();
+    camera.position.set(-0.5, 0.5, 1);
+
+    renderer = new WebGLRenderer({ antialias: true, canvas: el });
+    renderer.outputEncoding = sRGBEncoding;
+    renderer.shadowMap.enabled = true;
+    renderer.shadowMap.type = PCFSoftShadowMap;
+    document.body.appendChild(renderer.domElement);
+
+    const directionalLight = new DirectionalLight(0xffffff, 1.0);
+    directionalLight.castShadow = true;
+    directionalLight.shadow.mapSize.setScalar(1024);
+    directionalLight.position.set(5, 30, 5);
+    scene.add(directionalLight);
+
+    const ambientLight = new AmbientLight(0xffffff, 0.2);
+    scene.add(ambientLight);
+
+    const ground = new Mesh(new PlaneGeometry(), new ShadowMaterial({ opacity: 0.25 }));
+    ground.rotation.x = -Math.PI / 2;
+    ground.scale.setScalar(30);
+    ground.receiveShadow = true;
+    scene.add(ground);
+
+    controls = new OrbitControls(camera, renderer.domElement);
+    controls.minDistance = 0;
+    controls.maxDistance = 4;
+    controls.update();
+
+    loadModel()
+    handleResize()
+    render()
+}
+
+
+const handleResize = () => {
+    renderer.setSize(window.innerWidth, window.innerHeight);
+    renderer.setPixelRatio(window.devicePixelRatio);
+
+    camera.aspect = window.innerWidth / window.innerHeight;
+    camera.updateProjectionMatrix();
+}
+
+const render = () => {
+    requestAnimationFrame(render);
+    renderer.render(scene, camera);
+
+    if(!robot) return
+    
+    const newAngles = $servoBuffer
+    if(JSON.stringify(angles) === JSON.stringify(newAngles)) return
+
+    for (let i = 0; i < servoNames.length; i++) {
+        angles[i] = lerp(robot.joints[servoNames[i]].angle * (180/Math.PI), newAngles[i], 0.2)
+        robot.joints[servoNames[i]].setJointValue(MathUtils.degToRad(angles[i]));
+    }
+}
+</script>
+  
+<svelte:window on:resize={handleResize}></svelte:window>
+
+<canvas bind:this={el} class="absolute"></canvas>

app/src/lib/socket.ts

@@ -4,7 +4,11 @@ export type WebSocketStatus = 'OPEN' | 'CONNECTING' | 'CLOSED'
 
 export const isConnected = writable(false)
 
-export const data = writable(new Float32Array(13))
+export const dataBuffer = writable(new Float32Array(13))
+
+export const servoBuffer = writable(new Int8Array(12))
+
+export const data = writable();
 
 export const status:Writable<WebSocketStatus> = writable('CLOSED')
 
@@ -32,7 +36,7 @@ const _disconnected = () => {
 
 const _message = (event) => {    
     if (event.data instanceof ArrayBuffer) {
-        let buffer = new Uint8Array(event.data);
-        data.set(new Float32Array(buffer.buffer));
-    }
+        let buffer = new Int8Array(event.data);
+        servoBuffer.set(buffer);
+    } else dataBuffer.set(JSON.parse(event.data));
 }

app/src/lib/utils.ts

@@ -1,4 +1,8 @@
 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 + ['B', 'kB', 'MB', 'GB', 'TB'][i];
-}
+}
+
+export const lerp = (start: number, end: number, amt: number) => {
+    return (1 - amt) * start + amt * end;
+};

app/src/routes/Controller.svelte

@@ -1,9 +1,11 @@
 <script lang="ts">
 	import Stream from '../components/Views/Stream.svelte';
 	import Controls from '../components/Controls.svelte';
+    import ModelView from '../components/Model/ModelView.svelte';
 </script>
 
 <div class="flex justify-center items-center w-full h-full">
 	<Stream />
+    <ModelView />
 	<Controls />
 </div>