💅 Updates the frontpage

app/src/app.css

@@ -2,39 +2,39 @@
 @plugin "daisyui";
 
 @plugin "daisyui" {
-    themes:
+  themes:
-        light --default,
+    light --default,
-        dark --prefersdark;
+    dark --prefersdark;
 }
 
 @plugin "daisyui/theme" {
-    name: 'light';
+  name: 'light';
-    default: true;
+  default: true;
-    --color-primary: #00bfff;
+  --color-primary: #00bfff;
-    --color-secondary: #3c00ff;
+  --color-secondary: #3c00ff;
-    --base-content: white;
+  --base-content: white;
 }
 
 @plugin "daisyui/theme" {
-    name: 'dark';
+  name: 'dark';
-    prefersdark: true;
+  prefersdark: true;
-    --color-primary: #00bfff;
+  --color-primary: #00bfff;
-    --color-secondary: #3c00ff;
+  --color-secondary: #3c00ff;
-    --base-content: oklch(0.3 0.012 256);
+  --base-content: oklch(0.3 0.012 256);
 }
 
 #nipple_0_0,
 #nipple_1_1 {
-    z-index: 10 !important;
+  z-index: 10 !important;
 }
 
 #three-gui-panel {
-    top: 64px;
+  top: 64px;
-    right: 0px;
+  right: 0px;
 }
 
 @media (max-width: 1023px) {
-    #three-gui-panel {
+  #three-gui-panel {
-        top: 48px;
+    top: 48px;
-    }
+  }
 }

app/src/lib/components/Visualization.svelte

@@ -1,345 +1,340 @@
 <script lang="ts">
-    import { onDestroy, onMount } from 'svelte';
+  import { onDestroy, onMount } from 'svelte'
-    import {
+  import {
-        BufferGeometry,
+    BufferGeometry,
-        Line,
+    Line,
-        LineBasicMaterial,
+    LineBasicMaterial,
-        Mesh,
+    Mesh,
-        MeshBasicMaterial,
+    MeshBasicMaterial,
-        Object3D,
+    Object3D,
-        SphereGeometry,
+    SphereGeometry,
-        Vector3,
+    Vector3,
-        type NormalBufferAttributes,
+    type NormalBufferAttributes,
-        type Object3DEventMap
+    type Object3DEventMap
-    } from 'three';
+  } from 'three'
-    import {
+  import {
-        ModesEnum,
+    ModesEnum,
-        kinematicData,
+    kinematicData,
-        mode,
+    mode,
-        model,
+    model,
-        outControllerData,
+    outControllerData,
-        servoAnglesOut,
+    servoAnglesOut,
-        servoAngles,
+    servoAngles,
-        mpu,
+    mpu,
-        jointNames
+    jointNames
-    } from '$lib/stores';
+  } from '$lib/stores'
-    import { footColor, populateModelCache, throttler, toeWorldPositions } from '$lib/utilities';
+  import { footColor, populateModelCache, throttler, toeWorldPositions } from '$lib/utilities'
-    import SceneBuilder from '$lib/sceneBuilder';
+  import SceneBuilder from '$lib/sceneBuilder'
-    import { lerp, degToRad } from 'three/src/math/MathUtils';
+  import { lerp, degToRad } from 'three/src/math/MathUtils'
-    import { GUI } from 'three/addons/libs/lil-gui.module.min.js';
+  import { GUI } from 'three/addons/libs/lil-gui.module.min.js'
-    import Kinematic, { type body_state_t } from '$lib/kinematic';
+  import Kinematic, { type body_state_t } from '$lib/kinematic'
-    import {
+  import {
-        BezierState,
+    BezierState,
-        CalibrationState,
+    CalibrationState,
-        EightPhaseWalkState,
+    EightPhaseWalkState,
-        FourPhaseWalkState,
+    FourPhaseWalkState,
-        IdleState,
+    IdleState,
-        RestState,
+    RestState,
-        StandState
+    StandState
-    } from '$lib/gait';
+  } from '$lib/gait'
-    import { radToDeg } from 'three/src/math/MathUtils.js';
+  import { radToDeg } from 'three/src/math/MathUtils.js'
-    import type { URDFRobot } from 'urdf-loader';
+  import type { URDFRobot } from 'urdf-loader'
-    import { get } from 'svelte/store';
+  import { get } from 'svelte/store'
 
-    interface Props {
+  interface Props {
-        sky?: boolean;
+    sky?: boolean
-        orbit?: boolean;
+    orbit?: boolean
-        panel?: boolean;
+    panel?: boolean
-        debug?: boolean;
+    debug?: boolean
-        ground?: boolean;
+    ground?: boolean
+    zoom?: number
+  }
+
+  let {
+    sky = true,
+    orbit = false,
+    panel = true,
+    debug = false,
+    ground = true,
+    zoom = 8
+  }: Props = $props()
+
+  let sceneManager = $state(new SceneBuilder())
+  let canvas: HTMLCanvasElement = $state()
+
+  let currentModelAngles: number[] = new Array(12).fill(0)
+  let modelTargetAngles: number[] = new Array(12).fill(0)
+  let gui_panel: GUI
+  let Throttler = new throttler()
+
+  let feet_trace = new Array(4).fill([])
+  let trace_lines: BufferGeometry<NormalBufferAttributes>[] = []
+  let target: Object3D<Object3DEventMap>
+
+  let target_position = { x: 0, z: 0, yaw: 0 }
+
+  let kinematic = new Kinematic()
+
+  let planners = {
+    [ModesEnum.Deactivated]: new IdleState(),
+    [ModesEnum.Idle]: new IdleState(),
+    [ModesEnum.Calibration]: new CalibrationState(),
+    [ModesEnum.Rest]: new RestState(),
+    [ModesEnum.Stand]: new StandState(),
+    [ModesEnum.Crawl]: new EightPhaseWalkState(),
+    [ModesEnum.Walk]: new BezierState()
+  }
+  let lastTick = performance.now()
+
+  const dir = [1, -1, -1, -1, -1, -1, 1, -1, -1, -1, -1, -1]
+
+  let body_state = {
+    omega: 0,
+    phi: 0,
+    psi: 0,
+    xm: 0,
+    ym: 0.5,
+    zm: 0,
+    feet: planners[ModesEnum.Idle].default_feet_pos
+  }
+
+  let settings = {
+    'Internal kinematic': true,
+    'Robot transform controls': false,
+    'Auto orient robot': true,
+    'Trace feet': debug,
+    'Target position': false,
+    'Trace points': 30,
+    'Fix camera on robot': true,
+    'Smooth motion': true,
+    omega: 0,
+    phi: 0,
+    psi: 0,
+    xm: 0,
+    ym: 0.7,
+    zm: 0,
+    Background: 'black'
+  }
+
+  onMount(async () => {
+    await populateModelCache()
+    await createScene()
+    servoAngles.subscribe(updateAnglesFromStore)
+    if (panel) createPanel()
+  })
+
+  onDestroy(() => {
+    canvas.remove()
+    gui_panel?.destroy()
+  })
+
+  const updateAnglesFromStore = (angles: number[]) => {
+    if (sceneManager.isDragging) return
+    if (settings['Internal kinematic']) return
+    modelTargetAngles = angles
+  }
+
+  const createPanel = () => {
+    gui_panel = new GUI({ width: 310 })
+    gui_panel.close()
+    gui_panel.domElement.id = 'three-gui-panel'
+
+    const general = gui_panel.addFolder('General')
+    general.add(settings, 'Internal kinematic')
+    general.add(settings, 'Robot transform controls')
+    general.add(settings, 'Auto orient robot')
+
+    const kinematic = gui_panel.addFolder('Kinematics')
+    kinematic.add(settings, 'omega', -20, 20).onChange(updateKinematicPosition).listen()
+    kinematic.add(settings, 'phi', -30, 30).onChange(updateKinematicPosition).listen()
+    kinematic.add(settings, 'psi', -20, 15).onChange(updateKinematicPosition).listen()
+    kinematic.add(settings, 'xm', -1, 1).onChange(updateKinematicPosition).listen()
+    kinematic.add(settings, 'ym', 0, 1).onChange(updateKinematicPosition).listen()
+    kinematic.add(settings, 'zm', -1.3, 1.3).onChange(updateKinematicPosition).listen()
+
+    const visibility = gui_panel.addFolder('Visualization')
+    visibility.add(settings, 'Trace feet')
+    visibility.add(settings, 'Trace points', 1, 1000, 1)
+    visibility.add(settings, 'Target position')
+    visibility.add(settings, 'Smooth motion')
+    visibility.addColor(settings, 'Background')
+  }
+
+  const updateKinematicPosition = () => {
+    kinematicData.set([
+      settings.omega,
+      settings.phi,
+      settings.psi,
+      settings.xm,
+      settings.ym,
+      settings.zm
+    ])
+  }
+
+  const updateAngles = (name: string, angle: number) => {
+    modelTargetAngles[$jointNames.indexOf(name)] = angle * (180 / Math.PI)
+    Throttler.throttle(() => servoAnglesOut.set(modelTargetAngles.map(num => Math.round(num))), 100)
+  }
+
+  const createScene = async () => {
+    sceneManager
+      .addRenderer({ antialias: true, canvas, alpha: true })
+      .addPerspectiveCamera({ x: -0.5, y: 0.5, z: 1 })
+      .addOrbitControls(Math.min(zoom, 8), 30, orbit)
+      .addDirectionalLight({ x: 10, y: 20, z: 10, color: 0xffffff, intensity: 3 })
+      .addAmbientLight({ color: 0xffffff, intensity: 0.5 })
+      .addFogExp2(0xcccccc, 0.015)
+      .addModel($model)
+      .addTransformControls(sceneManager.model)
+      .fillParent()
+      .addRenderCb(render)
+      .startRenderLoop()
+
+    if (ground) sceneManager.addGroundPlane()
+
+    const geometry = new SphereGeometry(0.1, 32, 16)
+    const material = new MeshBasicMaterial({ color: 0xffff00 })
+    target = new Mesh(geometry, material)
+    sceneManager.scene.add(target)
+
+    if (debug) {
+      sceneManager.addDragControl(updateAngles)
+    }
+    if (sky) sceneManager.addSky()
+
+    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 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
     }
 
-    let {
+    trace_lines.forEach((line, i) => {
-        sky = true,
+      feet_trace[i].push(foot_positions[i])
-        orbit = false,
+      feet_trace[i] = feet_trace[i].slice(-settings['Trace points'])
-        panel = true,
+      line.setFromPoints(feet_trace[i])
-        debug = false,
+    })
-        ground = true
+  }
-    }: Props = $props();
 
-    let sceneManager = $state(new SceneBuilder());
+  const calculate_kinematics = () => {
-    let canvas: HTMLCanvasElement = $state();
+    if (sceneManager.isDragging || !settings['Internal kinematic']) return
+    const position: body_state_t = {
+      omega: settings.omega,
+      phi: settings.phi,
+      psi: settings.psi,
+      xm: settings.xm,
+      ym: settings.ym,
+      zm: settings.zm,
+      feet: body_state.feet
+    }
 
-    let currentModelAngles: number[] = new Array(12).fill(0);
+    let new_angles = kinematic.calcIK(position).map((x, i) => radToDeg(x * dir[i]))
-    let modelTargetAngles: number[] = new Array(12).fill(0);
+    modelTargetAngles = new_angles
-    let gui_panel: GUI;
+  }
-    let Throttler = new throttler();
 
-    let feet_trace = new Array(4).fill([]);
+  const orient_robot = (robot: URDFRobot, toes: Vector3[]) => {
-    let trace_lines: BufferGeometry<NormalBufferAttributes>[] = [];
+    if (settings['Robot transform controls'] || !settings['Auto orient robot']) return
-    let target: Object3D<Object3DEventMap>;
+    robot.position.y = robot.position.y - Math.min(...toes.map(toe => toe.y))
 
-    let target_position = { x: 0, z: 0, yaw: 0 };
+    robot.position.z = smooth(robot.position.z, -settings.xm, 0.1)
+    robot.position.x = smooth(robot.position.x, -settings.zm, 0.1)
 
-    let kinematic = new Kinematic();
+    robot.rotation.z = smooth(robot.rotation.z, degToRad(-settings.phi + $mpu.heading + 90), 0.1)
+    robot.rotation.y = smooth(robot.rotation.y, degToRad(settings.omega), 0.1)
+    robot.rotation.x = smooth(robot.rotation.x, degToRad(settings.psi - 90), 0.1)
+  }
 
-    let planners = {
+  const update_camera = (robot: URDFRobot) => {
-        [ModesEnum.Deactivated]: new IdleState(),
+    if (!settings['Fix camera on robot']) return
-        [ModesEnum.Idle]: new IdleState(),
+    sceneManager.orbit.target = robot.position.clone()
-        [ModesEnum.Calibration]: new CalibrationState(),
+  }
-        [ModesEnum.Rest]: new RestState(),
-        [ModesEnum.Stand]: new StandState(),
-        [ModesEnum.Crawl]: new EightPhaseWalkState(),
-        [ModesEnum.Walk]: new BezierState()
-    };
-    let lastTick = performance.now();
 
-    const dir = [1, -1, -1, -1, -1, -1, 1, -1, -1, -1, -1, -1];
+  const smooth = (start: number, end: number, amount: number) => {
+    return settings['Smooth motion'] ? lerp(start, end, amount) : end
+  }
 
-    let body_state = {
+  const update_gait = () => {
-        omega: 0,
+    if (sceneManager.isDragging || !settings['Internal kinematic']) return
-        phi: 0,
+    const controlData = get(outControllerData)
-        psi: 0,
+    const data = {
-        xm: 0,
+      stop: controlData[0],
-        ym: 0.5,
+      lx: controlData[1],
-        zm: 0,
+      ly: controlData[2],
-        feet: planners[ModesEnum.Idle].default_feet_pos
+      rx: controlData[3],
-    };
+      ry: controlData[4],
+      h: controlData[5],
+      s: controlData[6],
+      s1: controlData[7]
+    }
+    body_state.ym = ((data.h + 127) * 0.75) / 100
 
-    let settings = {
+    let planner = planners[get(mode)]
-        'Internal kinematic': true,
+    const delta = performance.now() - lastTick
-        'Robot transform controls': false,
+    lastTick = performance.now()
-        'Auto orient robot': true,
-        'Trace feet': debug,
-        'Target position': false,
-        'Trace points': 30,
-        'Fix camera on robot': true,
-        'Smooth motion': true,
-        omega: 0,
-        phi: 0,
-        psi: 0,
-        xm: 0,
-        ym: 0.7,
-        zm: 0,
-        Background: 'black'
-    };
 
-    onMount(async () => {
+    body_state = planner.step(body_state, data, delta)
-        await populateModelCache();
-        await createScene();
-        servoAngles.subscribe(updateAnglesFromStore);
-        if (panel) createPanel();
-    });
 
-    onDestroy(() => {
+    settings.omega = body_state.omega
-        canvas.remove();
+    settings.phi = body_state.phi
-        gui_panel?.destroy();
+    settings.psi = body_state.psi
-    });
+    settings.xm = body_state.xm
+    settings.ym = body_state.ym
+    settings.zm = body_state.zm
+  }
 
-    const updateAnglesFromStore = (angles: number[]) => {
+  const update_robot_position = (robot: URDFRobot) => {
-        if (sceneManager.isDragging) return;
+    if (!settings['Robot transform controls']) return
-        if (settings['Internal kinematic']) return;
+    settings.omega = radToDeg(robot.rotation.y)
-        modelTargetAngles = angles;
+    settings.phi = radToDeg(robot.rotation.z) + $mpu.heading - 90
-    };
+    settings.psi = radToDeg(robot.rotation.x) + 90
+    settings.xm = robot.position.z * 100
+    settings.zm = -robot.position.x * 100
+  }
 
-    const createPanel = () => {
+  const updateTargetPosition = () => {
-        gui_panel = new GUI({ width: 310 });
+    target.visible = settings['Target position']
-        gui_panel.close();
+    target.position.x = smooth(target.position.x, target_position.x, 0.5)
-        gui_panel.domElement.id = 'three-gui-panel';
+    target.position.z = smooth(target.position.z, target_position.z, 0.5)
+  }
 
-        const general = gui_panel.addFolder('General');
+  const render = () => {
-        general.add(settings, 'Internal kinematic');
+    const robot = sceneManager.model
-        general.add(settings, 'Robot transform controls');
+    if (!robot) return
-        general.add(settings, 'Auto orient robot');
 
-        const kinematic = gui_panel.addFolder('Kinematics');
+    const toes = toeWorldPositions(robot)
-        kinematic.add(settings, 'omega', -20, 20).onChange(updateKinematicPosition).listen();
-        kinematic.add(settings, 'phi', -30, 30).onChange(updateKinematicPosition).listen();
-        kinematic.add(settings, 'psi', -20, 15).onChange(updateKinematicPosition).listen();
-        kinematic.add(settings, 'xm', -1, 1).onChange(updateKinematicPosition).listen();
-        kinematic.add(settings, 'ym', 0, 1).onChange(updateKinematicPosition).listen();
-        kinematic.add(settings, 'zm', -1.3, 1.3).onChange(updateKinematicPosition).listen();
 
-        const visibility = gui_panel.addFolder('Visualization');
+    renderTraceLines(toes)
-        visibility.add(settings, 'Trace feet');
+    update_camera(robot)
-        visibility.add(settings, 'Trace points', 1, 1000, 1);
+    update_gait()
-        visibility.add(settings, 'Target position');
+    calculate_kinematics()
-        visibility.add(settings, 'Smooth motion');
+    update_robot_position(robot)
-        visibility.addColor(settings, 'Background');
-    };
 
-    const updateKinematicPosition = () => {
+    sceneManager.transformControl.showX = settings['Robot transform controls']
-        kinematicData.set([
+    sceneManager.transformControl.showY = settings['Robot transform controls']
-            settings.omega,
+    sceneManager.transformControl.showZ = settings['Robot transform controls']
-            settings.phi,
-            settings.psi,
-            settings.xm,
-            settings.ym,
-            settings.zm
-        ]);
-    };
 
-    const updateAngles = (name: string, angle: number) => {
+    for (let i = 0; i < $jointNames.length; i++) {
-        modelTargetAngles[$jointNames.indexOf(name)] = angle * (180 / Math.PI);
+      currentModelAngles[i] = smooth(
-        Throttler.throttle(
+        (robot.joints[$jointNames[i]].angle as number) * (180 / Math.PI),
-            () => servoAnglesOut.set(modelTargetAngles.map(num => Math.round(num))),
+        modelTargetAngles[i],
-            100
+        0.1
-        );
+      )
-    };
+      robot.joints[$jointNames[i]].setJointValue(degToRad(currentModelAngles[i]))
+    }
 
-    const createScene = async () => {
+    orient_robot(robot, toes)
-        sceneManager
+    updateTargetPosition()
-            .addRenderer({ antialias: true, canvas, alpha: true })
+  }
-            .addPerspectiveCamera({ x: -0.5, y: 0.5, z: 1 })
-            .addOrbitControls(8, 30, orbit)
-            .addDirectionalLight({ x: 10, y: 20, z: 10, color: 0xffffff, intensity: 3 })
-            .addAmbientLight({ color: 0xffffff, intensity: 0.5 })
-            .addFogExp2(0xcccccc, 0.015)
-            .addModel($model)
-            .addTransformControls(sceneManager.model)
-            .fillParent()
-            .addRenderCb(render)
-            .startRenderLoop();
-
-        if (ground) sceneManager.addGroundPlane();
-
-        const geometry = new SphereGeometry(0.1, 32, 16);
-        const material = new MeshBasicMaterial({ color: 0xffff00 });
-        target = new Mesh(geometry, material);
-        sceneManager.scene.add(target);
-
-        if (debug) {
-            sceneManager.addDragControl(updateAngles);
-        }
-        if (sky) sceneManager.addSky();
-
-        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 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 calculate_kinematics = () => {
-        if (sceneManager.isDragging || !settings['Internal kinematic']) return;
-        const position: body_state_t = {
-            omega: settings.omega,
-            phi: settings.phi,
-            psi: settings.psi,
-            xm: settings.xm,
-            ym: settings.ym,
-            zm: settings.zm,
-            feet: body_state.feet
-        };
-
-        let new_angles = kinematic.calcIK(position).map((x, i) => radToDeg(x * dir[i]));
-        modelTargetAngles = new_angles;
-    };
-
-    const orient_robot = (robot: URDFRobot, toes: Vector3[]) => {
-        if (settings['Robot transform controls'] || !settings['Auto orient robot']) return;
-        robot.position.y = robot.position.y - Math.min(...toes.map(toe => toe.y));
-
-        robot.position.z = smooth(robot.position.z, -settings.xm, 0.1);
-        robot.position.x = smooth(robot.position.x, -settings.zm, 0.1);
-
-        robot.rotation.z = smooth(
-            robot.rotation.z,
-            degToRad(-settings.phi + $mpu.heading + 90),
-            0.1
-        );
-        robot.rotation.y = smooth(robot.rotation.y, degToRad(settings.omega), 0.1);
-        robot.rotation.x = smooth(robot.rotation.x, degToRad(settings.psi - 90), 0.1);
-    };
-
-    const update_camera = (robot: URDFRobot) => {
-        if (!settings['Fix camera on robot']) return;
-        sceneManager.orbit.target = robot.position.clone();
-    };
-
-    const smooth = (start: number, end: number, amount: number) => {
-        return settings['Smooth motion'] ? lerp(start, end, amount) : end;
-    };
-
-    const update_gait = () => {
-        if (sceneManager.isDragging || !settings['Internal kinematic']) return;
-        const controlData = get(outControllerData);
-        const data = {
-            stop: controlData[0],
-            lx: controlData[1],
-            ly: controlData[2],
-            rx: controlData[3],
-            ry: controlData[4],
-            h: controlData[5],
-            s: controlData[6],
-            s1: controlData[7]
-        };
-        body_state.ym = ((data.h + 127) * 0.75) / 100;
-
-        let planner = planners[get(mode)];
-        const delta = performance.now() - lastTick;
-        lastTick = performance.now();
-
-        body_state = planner.step(body_state, data, delta);
-
-        settings.omega = body_state.omega;
-        settings.phi = body_state.phi;
-        settings.psi = body_state.psi;
-        settings.xm = body_state.xm;
-        settings.ym = body_state.ym;
-        settings.zm = body_state.zm;
-    };
-
-    const update_robot_position = (robot: URDFRobot) => {
-        if (!settings['Robot transform controls']) return;
-        settings.omega = radToDeg(robot.rotation.y);
-        settings.phi = radToDeg(robot.rotation.z) + $mpu.heading - 90;
-        settings.psi = radToDeg(robot.rotation.x) + 90;
-        settings.xm = robot.position.z * 100;
-        settings.zm = -robot.position.x * 100;
-    };
-
-    const updateTargetPosition = () => {
-        target.visible = settings['Target position'];
-        target.position.x = smooth(target.position.x, target_position.x, 0.5);
-        target.position.z = smooth(target.position.z, target_position.z, 0.5);
-    };
-
-    const render = () => {
-        const robot = sceneManager.model;
-        if (!robot) return;
-
-        const toes = toeWorldPositions(robot);
-
-        renderTraceLines(toes);
-        update_camera(robot);
-        update_gait();
-        calculate_kinematics();
-        update_robot_position(robot);
-
-        sceneManager.transformControl.showX = settings['Robot transform controls'];
-        sceneManager.transformControl.showY = settings['Robot transform controls'];
-        sceneManager.transformControl.showZ = settings['Robot transform controls'];
-
-        for (let i = 0; i < $jointNames.length; i++) {
-            currentModelAngles[i] = smooth(
-                (robot.joints[$jointNames[i]].angle as number) * (180 / Math.PI),
-                modelTargetAngles[i],
-                0.1
-            );
-            robot.joints[$jointNames[i]].setJointValue(degToRad(currentModelAngles[i]));
-        }
-
-        orient_robot(robot, toes);
-        updateTargetPosition();
-    };
 </script>
 
 <svelte:window onresize={sceneManager.fillParent} />

app/src/lib/sceneBuilder.ts

@@ -1,379 +1,379 @@
 import {
-    Mesh,
+  Mesh,
-    PerspectiveCamera,
+  PerspectiveCamera,
-    PlaneGeometry,
+  PlaneGeometry,
-    Scene,
+  Scene,
-    WebGLRenderer,
+  WebGLRenderer,
-    AmbientLight,
+  AmbientLight,
-    DirectionalLight,
+  DirectionalLight,
-    PCFSoftShadowMap,
+  PCFSoftShadowMap,
-    type GridHelper,
+  type GridHelper,
-    ArrowHelper,
+  ArrowHelper,
-    Vector3,
+  Vector3,
-    FogExp2,
+  FogExp2,
-    CanvasTexture,
+  CanvasTexture,
-    type ColorRepresentation,
+  type ColorRepresentation,
-    type WebGLRendererParameters,
+  type WebGLRendererParameters,
-    MeshPhongMaterial,
+  MeshPhongMaterial,
-    EquirectangularReflectionMapping,
+  EquirectangularReflectionMapping,
-    ACESFilmicToneMapping,
+  ACESFilmicToneMapping,
-    MathUtils,
+  MathUtils,
-    Group,
+  Group,
-    MeshBasicMaterial,
+  MeshBasicMaterial,
-    RepeatWrapping
+  RepeatWrapping
-} from 'three';
+} from 'three'
-import { Sky } from 'three/addons/objects/Sky.js';
+import { Sky } from 'three/addons/objects/Sky.js'
-import { OrbitControls } from 'three/examples/jsm/controls/OrbitControls';
+import { OrbitControls } from 'three/examples/jsm/controls/OrbitControls'
-import { TransformControls } from 'three/examples/jsm/controls/TransformControls';
+import { TransformControls } from 'three/examples/jsm/controls/TransformControls'
-import { Reflector } from 'three/examples/jsm/objects/Reflector.js';
+import { Reflector } from 'three/examples/jsm/objects/Reflector.js'
-import { type URDFJoint, type URDFMimicJoint, type URDFRobot } from 'urdf-loader';
+import { type URDFJoint, type URDFMimicJoint, type URDFRobot } from 'urdf-loader'
-import { PointerURDFDragControls } from 'urdf-loader/src/URDFDragControls';
+import { PointerURDFDragControls } from 'urdf-loader/src/URDFDragControls'
-import { sunCalculator } from './utilities/position-utilities';
+import { sunCalculator } from './utilities/position-utilities'
 
-export const addScene = () => new Scene();
+export const addScene = () => new Scene()
 
 interface position {
-    x?: number;
+  x?: number
-    y?: number;
+  y?: number
-    z?: number;
+  z?: number
 }
 
 interface light {
-    color?: ColorRepresentation;
+  color?: ColorRepresentation
-    intensity?: number;
+  intensity?: number
 }
 
 interface arrowOptions {
-    origin: position;
+  origin: position
-    direction: position;
+  direction: position
-    length?: number;
+  length?: number
-    color?: ColorRepresentation;
+  color?: ColorRepresentation
 }
 
-type directionalLight = position & light;
+type directionalLight = position & light
 
 export default class SceneBuilder {
-    public scene: Scene;
+  public scene: Scene
-    public camera!: PerspectiveCamera;
+  public camera!: PerspectiveCamera
-    public ground!: Mesh;
+  public ground!: Mesh
-    public renderer!: WebGLRenderer;
+  public renderer!: WebGLRenderer
-    public orbit: OrbitControls;
+  public orbit: OrbitControls
-    public callback: Function | undefined;
+  public callback: Function | undefined
-    public gridHelper!: GridHelper;
+  public gridHelper!: GridHelper
-    public model!: URDFRobot;
+  public model!: URDFRobot
-    public liveStreamTexture!: CanvasTexture;
+  public liveStreamTexture!: CanvasTexture
-    private fog!: FogExp2;
+  private fog!: FogExp2
-    private isLoaded: boolean = false;
+  private isLoaded: boolean = false
-    public isDragging: boolean = false;
+  public isDragging: boolean = false
-    highlightMaterial: any;
+  highlightMaterial: any
-    sky!: Sky;
+  sky!: Sky
-    transformControl: TransformControls;
+  transformControl: TransformControls
-    public modelGroup!: Group;
+  public modelGroup!: Group
 
-    constructor() {
+  constructor() {
-        this.scene = new Scene();
+    this.scene = new Scene()
-        if (this.scene.environment?.mapping) {
+    if (this.scene.environment?.mapping) {
-            this.scene.environment.mapping = EquirectangularReflectionMapping;
+      this.scene.environment.mapping = EquirectangularReflectionMapping
-        }
+    }
-        return this;
+    return this
+  }
+
+  public addRenderer = (parameters?: WebGLRendererParameters) => {
+    this.renderer = new WebGLRenderer(parameters)
+    this.renderer.outputColorSpace = 'srgb'
+    this.renderer.shadowMap.enabled = true
+    this.renderer.shadowMap.type = PCFSoftShadowMap
+    this.renderer.toneMapping = ACESFilmicToneMapping
+    this.renderer.toneMappingExposure = 0.85
+    if (!parameters?.canvas) document.body.appendChild(this.renderer.domElement)
+    return this
+  }
+
+  public addSky = () => {
+    this.sky = new Sky()
+    this.sky.scale.setScalar(450000)
+    this.scene.add(this.sky)
+    const effectController = {
+      turbidity: 10,
+      rayleigh: 3,
+      mieCoefficient: 0.005,
+      mieDirectionalG: 0.7,
+      elevation: sunCalculator.calculateSunElevation(),
+      azimuth: 200,
+      exposure: this.renderer.toneMappingExposure
+    }
+    const uniforms = this.sky.material.uniforms
+    uniforms['turbidity'].value = effectController.turbidity
+    uniforms['rayleigh'].value = effectController.rayleigh
+    uniforms['mieCoefficient'].value = effectController.mieCoefficient
+    uniforms['mieDirectionalG'].value = effectController.mieDirectionalG
+    this.renderer.toneMappingExposure = 0.5
+    const phi = MathUtils.degToRad(90 - effectController.elevation)
+    const theta = MathUtils.degToRad(effectController.azimuth)
+    const sun = new Vector3()
+
+    sun.setFromSphericalCoords(1, phi, theta)
+    uniforms['sunPosition'].value.copy(sun)
+    return this
+  }
+
+  public addPerspectiveCamera = (options: position) => {
+    this.camera = new PerspectiveCamera()
+    this.camera.position.set(options.x ?? 0, options.y ?? 2.7, options.z ?? 0)
+    this.scene.add(this.camera)
+    return this
+  }
+
+  public addGroundPlane = (options?: position) => {
+    const checkerboardTexture = this.createCheckerboardTexture(1024, 2)
+    checkerboardTexture.wrapS = RepeatWrapping
+    checkerboardTexture.wrapT = RepeatWrapping
+    checkerboardTexture.repeat.set(100, 100)
+    const checkerboardMat = new MeshBasicMaterial({
+      map: checkerboardTexture,
+      opacity: 0.1,
+      transparent: true
+    })
+
+    const plane = new PlaneGeometry(400, 400)
+
+    this.ground = new Mesh(plane, checkerboardMat)
+    this.ground.rotation.x = -Math.PI / 2
+    this.ground.position.set(options?.x ?? 0, options?.y ?? 0.01, options?.z ?? 0)
+    this.ground.receiveShadow = true
+    this.scene.add(this.ground)
+
+    const mirror = new Reflector(plane, {
+      clipBias: 0.003,
+      textureWidth: window.innerWidth * window.devicePixelRatio,
+      textureHeight: window.innerHeight * window.devicePixelRatio,
+      color: 0x00bfff
+    })
+    mirror.rotateX(-Math.PI / 2)
+    this.scene.add(mirror)
+
+    return this
+  }
+
+  public addOrbitControls = (minDistance: number, maxDistance: number, autoRotate = true) => {
+    this.orbit = new OrbitControls(this.camera, this.renderer.domElement)
+    this.orbit.minDistance = 5
+    this.orbit.maxDistance = maxDistance
+    this.orbit.autoRotate = autoRotate
+    this.orbit.update()
+    return this
+  }
+
+  public addAmbientLight = (options: light) => {
+    const ambientLight = new AmbientLight(options.color, options.intensity)
+    this.scene.add(ambientLight)
+    return this
+  }
+
+  public addDirectionalLight = (options: directionalLight) => {
+    const directionalLight = new DirectionalLight(options.color, options.intensity)
+    directionalLight.castShadow = true
+    directionalLight.shadow.camera.top = 10
+    directionalLight.shadow.camera.bottom = -10
+    directionalLight.shadow.camera.right = 10
+    directionalLight.shadow.camera.left = -10
+    directionalLight.shadow.mapSize.set(4096, 4096)
+
+    directionalLight.position.set(options.x ?? 0, options.y ?? 0, options.z ?? 0)
+    this.scene.add(directionalLight)
+    return this
+  }
+
+  private createCheckerboardTexture = (size: number, squares: number) => {
+    const canvas = document.createElement('canvas')
+    canvas.width = size
+    canvas.height = size
+    const context = canvas.getContext('2d')
+
+    const squareSize = size / squares
+
+    for (let y = 0; y < squares; y++) {
+      for (let x = 0; x < squares; x++) {
+        context!.fillStyle = (x + y) % 2 === 0 ? '#ffffff' : '#000000'
+        context!.fillRect(x * squareSize, y * squareSize, squareSize, squareSize)
+      }
     }
 
-    public addRenderer = (parameters?: WebGLRendererParameters) => {
+    const texture = new CanvasTexture(canvas)
-        this.renderer = new WebGLRenderer(parameters);
+    texture.wrapS = texture.wrapT = RepeatWrapping
-        this.renderer.outputColorSpace = 'srgb';
+    texture.anisotropy = 16
-        this.renderer.shadowMap.enabled = true;
+    return texture
-        this.renderer.shadowMap.type = PCFSoftShadowMap;
+  }
-        this.renderer.toneMapping = ACESFilmicToneMapping;
-        this.renderer.toneMappingExposure = 0.85;
-        if (!parameters?.canvas) document.body.appendChild(this.renderer.domElement);
-        return this;
-    };
 
-    public addSky = () => {
+  public addFogExp2 = (color: ColorRepresentation, density?: number) => {
-        this.sky = new Sky();
+    this.scene.fog = new FogExp2(color, density)
-        this.sky.scale.setScalar(450000);
+    return this
-        this.scene.add(this.sky);
+  }
-        const effectController = {
-            turbidity: 10,
-            rayleigh: 3,
-            mieCoefficient: 0.005,
-            mieDirectionalG: 0.7,
-            elevation: sunCalculator.calculateSunElevation(),
-            azimuth: 200,
-            exposure: this.renderer.toneMappingExposure
-        };
-        const uniforms = this.sky.material.uniforms;
-        uniforms['turbidity'].value = effectController.turbidity;
-        uniforms['rayleigh'].value = effectController.rayleigh;
-        uniforms['mieCoefficient'].value = effectController.mieCoefficient;
-        uniforms['mieDirectionalG'].value = effectController.mieDirectionalG;
-        this.renderer.toneMappingExposure = 0.5;
-        const phi = MathUtils.degToRad(90 - effectController.elevation);
-        const theta = MathUtils.degToRad(effectController.azimuth);
-        const sun = new Vector3();
 
-        sun.setFromSphericalCoords(1, phi, theta);
+  public fillParent = () => {
-        uniforms['sunPosition'].value.copy(sun);
+    const parentElement = this.renderer.domElement.parentElement
-        return this;
+    if (parentElement) {
-    };
+      const width = parentElement.clientWidth
-
+      const height = parentElement.clientHeight
-    public addPerspectiveCamera = (options: position) => {
+      this.handleResize(width, height)
-        this.camera = new PerspectiveCamera();
-        this.camera.position.set(options.x ?? 0, options.y ?? 2.7, options.z ?? 0);
-        this.scene.add(this.camera);
-        return this;
-    };
-
-    public addGroundPlane = (options?: position) => {
-        const checkerboardTexture = this.createCheckerboardTexture(1024, 2);
-        checkerboardTexture.wrapS = RepeatWrapping;
-        checkerboardTexture.wrapT = RepeatWrapping;
-        checkerboardTexture.repeat.set(100, 100);
-        const checkerboardMat = new MeshBasicMaterial({
-            map: checkerboardTexture,
-            opacity: 0.1,
-            transparent: true
-        });
-
-        const plane = new PlaneGeometry(400, 400);
-
-        this.ground = new Mesh(plane, checkerboardMat);
-        this.ground.rotation.x = -Math.PI / 2;
-        this.ground.position.set(options?.x ?? 0, options?.y ?? 0.01, options?.z ?? 0);
-        this.ground.receiveShadow = true;
-        this.scene.add(this.ground);
-
-        const mirror = new Reflector(plane, {
-            clipBias: 0.003,
-            textureWidth: window.innerWidth * window.devicePixelRatio,
-            textureHeight: window.innerHeight * window.devicePixelRatio,
-            color: 0x00bfff
-        });
-        mirror.rotateX(-Math.PI / 2);
-        this.scene.add(mirror);
-
-        return this;
-    };
-
-    public addOrbitControls = (minDistance: number, maxDistance: number, autoRotate = true) => {
-        this.orbit = new OrbitControls(this.camera, this.renderer.domElement);
-        this.orbit.minDistance = minDistance;
-        this.orbit.maxDistance = maxDistance;
-        this.orbit.autoRotate = autoRotate;
-        this.orbit.update();
-        return this;
-    };
-
-    public addAmbientLight = (options: light) => {
-        const ambientLight = new AmbientLight(options.color, options.intensity);
-        this.scene.add(ambientLight);
-        return this;
-    };
-
-    public addDirectionalLight = (options: directionalLight) => {
-        const directionalLight = new DirectionalLight(options.color, options.intensity);
-        directionalLight.castShadow = true;
-        directionalLight.shadow.camera.top = 10;
-        directionalLight.shadow.camera.bottom = -10;
-        directionalLight.shadow.camera.right = 10;
-        directionalLight.shadow.camera.left = -10;
-        directionalLight.shadow.mapSize.set(4096, 4096);
-
-        directionalLight.position.set(options.x ?? 0, options.y ?? 0, options.z ?? 0);
-        this.scene.add(directionalLight);
-        return this;
-    };
-
-    private createCheckerboardTexture = (size: number, squares: number) => {
-        const canvas = document.createElement('canvas');
-        canvas.width = size;
-        canvas.height = size;
-        const context = canvas.getContext('2d');
-
-        const squareSize = size / squares;
-
-        for (let y = 0; y < squares; y++) {
-            for (let x = 0; x < squares; x++) {
-                context!.fillStyle = (x + y) % 2 === 0 ? '#ffffff' : '#000000';
-                context!.fillRect(x * squareSize, y * squareSize, squareSize, squareSize);
-            }
-        }
-
-        const texture = new CanvasTexture(canvas);
-        texture.wrapS = texture.wrapT = RepeatWrapping;
-        texture.anisotropy = 16;
-        return texture;
-    };
-
-    public addFogExp2 = (color: ColorRepresentation, density?: number) => {
-        this.scene.fog = new FogExp2(color, density);
-        return this;
-    };
-
-    public fillParent = () => {
-        const parentElement = this.renderer.domElement.parentElement;
-        if (parentElement) {
-            const width = parentElement.clientWidth;
-            const height = parentElement.clientHeight;
-            this.handleResize(width, height);
-        }
-        return this;
-    };
-
-    public handleResize = (width = window.innerWidth, height = window.innerHeight) => {
-        this.renderer.setSize(width, height);
-        this.renderer.setPixelRatio(window.devicePixelRatio);
-        this.camera.aspect = width / height;
-        this.camera.updateProjectionMatrix();
-        return this;
-    };
-
-    public addRenderCb = (callback: Function) => {
-        this.callback = callback;
-        return this;
-    };
-
-    public startRenderLoop = () => {
-        this.renderer.setAnimationLoop(() => {
-            this.renderer.render(this.scene, this.camera);
-            this.orbit.update();
-            this.handleRobotShadow();
-            if (this.callback) this.callback();
-            if (!this.liveStreamTexture) return;
-        });
-        return this;
-    };
-
-    public addArrowHelper = (options?: arrowOptions) => {
-        const dir = new Vector3(
-            options?.direction.x ?? 0,
-            options?.direction.y ?? 0,
-            options?.direction.z ?? 0
-        );
-        const origin = new Vector3(
-            options?.origin.x ?? 0,
-            options?.origin.y ?? 0,
-            options?.origin.z ?? 0
-        );
-        const arrowHelper = new ArrowHelper(
-            dir,
-            origin,
-            options?.length ?? 1.5,
-            options?.color ?? 0xff0000
-        );
-        this.scene.add(arrowHelper);
-        return this;
-    };
-
-    private setJointValue(jointName: string, angle: number) {
-        if (!this.model) return;
-        if (!this.model.joints[jointName]) return;
-        this.model.joints[jointName].setJointValue(angle);
     }
+    return this
+  }
 
-    isJoint = (j: URDFJoint) => j.isURDFJoint && j.jointType !== 'fixed';
+  public handleResize = (width = window.innerWidth, height = window.innerHeight) => {
+    this.renderer.setSize(width, height)
+    this.renderer.setPixelRatio(window.devicePixelRatio)
+    this.camera.aspect = width / height
+    this.camera.updateProjectionMatrix()
+    return this
+  }
 
-    highlightLinkGeometry = (m: URDFMimicJoint, revert: boolean, material: MeshPhongMaterial) => {
+  public addRenderCb = (callback: Function) => {
-        const traverse = (c: any) => {
+    this.callback = callback
-            if (c.type === 'Mesh') {
+    return this
-                if (revert) {
+  }
-                    c.material = c.__origMaterial;
-                    delete c.__origMaterial;
-                } else {
-                    c.__origMaterial = c.material;
-                    c.material = material;
-                }
-            }
 
-            if (c === m || !this.isJoint(c)) {
+  public startRenderLoop = () => {
-                for (let i = 0; i < c.children.length; i++) {
+    this.renderer.setAnimationLoop(() => {
-                    const child = c.children[i];
+      this.renderer.render(this.scene, this.camera)
-                    if (!child.isURDFCollider) {
+      this.orbit.update()
-                        traverse(c.children[i]);
+      this.handleRobotShadow()
-                    }
+      if (this.callback) this.callback()
-                }
+      if (!this.liveStreamTexture) return
-            }
+    })
-        };
+    return this
-        traverse(m);
+  }
-    };
 
-    public addTransformControls = (model: any) => {
+  public addArrowHelper = (options?: arrowOptions) => {
-        this.transformControl = new TransformControls(this.camera, this.renderer.domElement);
+    const dir = new Vector3(
-        this.transformControl.addEventListener('dragging-changed', (event: any) => {
+      options?.direction.x ?? 0,
-            this.orbit.enabled = !event.value;
+      options?.direction.y ?? 0,
-            this.isDragging = !event.value;
+      options?.direction.z ?? 0
-        });
+    )
-        this.transformControl.attach(model);
+    const origin = new Vector3(
-        this.scene.add(this.transformControl);
+      options?.origin.x ?? 0,
-        this.transformControl.setMode('rotate');
+      options?.origin.y ?? 0,
-        return this;
+      options?.origin.z ?? 0
-    };
+    )
+    const arrowHelper = new ArrowHelper(
+      dir,
+      origin,
+      options?.length ?? 1.5,
+      options?.color ?? 0xff0000
+    )
+    this.scene.add(arrowHelper)
+    return this
+  }
 
-    public addModel = (model: any) => {
+  private setJointValue(jointName: string, angle: number) {
-        this.modelGroup = new Group();
+    if (!this.model) return
-        this.modelGroup.add(model);
+    if (!this.model.joints[jointName]) return
-        this.model = model;
+    this.model.joints[jointName].setJointValue(angle)
-        this.scene.add(this.modelGroup);
+  }
-        return this;
-    };
 
-    public addDragControl = (updateAngle: any) => {
+  isJoint = (j: URDFJoint) => j.isURDFJoint && j.jointType !== 'fixed'
-        const highlightColor = '#FFFFFF';
-        const highlightMaterial = new MeshPhongMaterial({
-            shininess: 10,
-            color: highlightColor,
-            emissive: highlightColor,
-            emissiveIntensity: 0.9
-        });
 
-        const dragControls = new PointerURDFDragControls(
+  highlightLinkGeometry = (m: URDFMimicJoint, revert: boolean, material: MeshPhongMaterial) => {
-            this.scene,
+    const traverse = (c: any) => {
-            this.camera,
+      if (c.type === 'Mesh') {
-            this.renderer.domElement
+        if (revert) {
-        );
+          c.material = c.__origMaterial
-        dragControls.updateJoint = (joint: URDFMimicJoint, angle: number) => {
+          delete c.__origMaterial
-            this.setJointValue(joint.name, angle);
+        } else {
-            updateAngle(joint.name, angle);
+          c.__origMaterial = c.material
-        };
+          c.material = material
-        dragControls.onDragStart = () => {
+        }
-            this.orbit.enabled = false;
+      }
-            this.isDragging = true;
-        };
-        dragControls.onDragEnd = () => {
-            this.orbit.enabled = true;
-            this.isDragging = false;
-        };
-        dragControls.onHover = (joint: URDFMimicJoint) =>
-            this.highlightLinkGeometry(joint, false, highlightMaterial);
-        dragControls.onUnhover = (joint: URDFMimicJoint) =>
-            this.highlightLinkGeometry(joint, true, highlightMaterial);
 
-        this.renderer.domElement.addEventListener(
+      if (c === m || !this.isJoint(c)) {
-            'touchstart',
+        for (let i = 0; i < c.children.length; i++) {
-            data => dragControls._mouseDown(data.touches[0]),
+          const child = c.children[i]
-            { passive: true }
+          if (!child.isURDFCollider) {
-        );
+            traverse(c.children[i])
-        this.renderer.domElement.addEventListener(
+          }
-            'touchmove',
+        }
-            data => dragControls._mouseMove(data.touches[0]),
+      }
-            { passive: true }
+    }
-        );
+    traverse(m)
-        this.renderer.domElement.addEventListener(
+  }
-            'touchend',
-            data => dragControls._mouseUp(data.touches[0]),
-            { passive: true }
-        );
-        return this;
-    };
 
-    public toggleFog = () => {
+  public addTransformControls = (model: any) => {
-        this.scene.fog = this.scene.fog ? null : this.fog;
+    this.transformControl = new TransformControls(this.camera, this.renderer.domElement)
-    };
+    this.transformControl.addEventListener('dragging-changed', (event: any) => {
+      this.orbit.enabled = !event.value
+      this.isDragging = !event.value
+    })
+    this.transformControl.attach(model)
+    this.scene.add(this.transformControl)
+    this.transformControl.setMode('rotate')
+    return this
+  }
 
-    private handleRobotShadow = () => {
+  public addModel = (model: any) => {
-        if (this.isLoaded) return;
+    this.modelGroup = new Group()
-        const intervalId = setInterval(() => this.model?.traverse(c => (c.castShadow = true)), 10);
+    this.modelGroup.add(model)
-        setTimeout(() => clearInterval(intervalId), 1000);
+    this.model = model
-        this.isLoaded = true;
+    this.scene.add(this.modelGroup)
-    };
+    return this
+  }
+
+  public addDragControl = (updateAngle: any) => {
+    const highlightColor = '#FFFFFF'
+    const highlightMaterial = new MeshPhongMaterial({
+      shininess: 10,
+      color: highlightColor,
+      emissive: highlightColor,
+      emissiveIntensity: 0.9
+    })
+
+    const dragControls = new PointerURDFDragControls(
+      this.scene,
+      this.camera,
+      this.renderer.domElement
+    )
+    dragControls.updateJoint = (joint: URDFMimicJoint, angle: number) => {
+      this.setJointValue(joint.name, angle)
+      updateAngle(joint.name, angle)
+    }
+    dragControls.onDragStart = () => {
+      this.orbit.enabled = false
+      this.isDragging = true
+    }
+    dragControls.onDragEnd = () => {
+      this.orbit.enabled = true
+      this.isDragging = false
+    }
+    dragControls.onHover = (joint: URDFMimicJoint) =>
+      this.highlightLinkGeometry(joint, false, highlightMaterial)
+    dragControls.onUnhover = (joint: URDFMimicJoint) =>
+      this.highlightLinkGeometry(joint, true, highlightMaterial)
+
+    this.renderer.domElement.addEventListener(
+      'touchstart',
+      data => dragControls._mouseDown(data.touches[0]),
+      { passive: true }
+    )
+    this.renderer.domElement.addEventListener(
+      'touchmove',
+      data => dragControls._mouseMove(data.touches[0]),
+      { passive: true }
+    )
+    this.renderer.domElement.addEventListener(
+      'touchend',
+      data => dragControls._mouseUp(data.touches[0]),
+      { passive: true }
+    )
+    return this
+  }
+
+  public toggleFog = () => {
+    this.scene.fog = this.scene.fog ? null : this.fog
+  }
+
+  private handleRobotShadow = () => {
+    if (this.isLoaded) return
+    const intervalId = setInterval(() => this.model?.traverse(c => (c.castShadow = true)), 10)
+    setTimeout(() => clearInterval(intervalId), 1000)
+    this.isLoaded = true
+  }
 }

app/src/routes/+layout.svelte

@@ -1,125 +1,125 @@
 <script lang="ts">
-    import { onDestroy, onMount } from 'svelte';
+  import { onDestroy, onMount } from 'svelte'
-    import { page } from '$app/state';
+  import { page } from '$app/state'
-    import { Modals, modals } from 'svelte-modals';
+  import { Modals, modals } from 'svelte-modals'
-    import Toast from '$lib/components/toasts/Toast.svelte';
+  import Toast from '$lib/components/toasts/Toast.svelte'
-    import { notifications } from '$lib/components/toasts/notifications';
+  import { notifications } from '$lib/components/toasts/notifications'
-    import { fade } from 'svelte/transition';
+  import { fade } from 'svelte/transition'
-    import '../app.css';
+  import '../app.css'
-    import Menu from '../lib/components/menu/Menu.svelte';
+  import Menu from '../lib/components/menu/Menu.svelte'
-    import Statusbar from '../lib/components/statusbar/statusbar.svelte';
+  import Statusbar from '../lib/components/statusbar/statusbar.svelte'
-    import {
+  import {
-        telemetry,
+    telemetry,
-        analytics,
+    analytics,
-        ModesEnum,
+    ModesEnum,
-        kinematicData,
+    kinematicData,
-        mode,
+    mode,
-        outControllerData,
+    outControllerData,
-        servoAngles,
+    servoAngles,
-        servoAnglesOut,
+    servoAnglesOut,
-        socket,
+    socket,
-        location,
+    location,
-        useFeatureFlags
+    useFeatureFlags
-    } from '$lib/stores';
+  } from '$lib/stores'
-    import type { Analytics, DownloadOTA } from '$lib/types/models';
+  import type { Analytics, DownloadOTA } from '$lib/types/models'
-    interface Props {
+  interface Props {
-        children?: import('svelte').Snippet;
+    children?: import('svelte').Snippet
-    }
+  }
 
-    let { children }: Props = $props();
+  let { children }: Props = $props()
 
-    const features = useFeatureFlags();
+  const features = useFeatureFlags()
 
-    onMount(async () => {
+  onMount(async () => {
-        const ws = $location ? $location : window.location.host;
+    const ws = $location ? $location : window.location.host
-        socket.init(`ws://${ws}/api/ws/events`);
+    socket.init(`ws://${ws}/api/ws/events`)
 
-        addEventListeners();
+    addEventListeners()
 
-        outControllerData.subscribe(data => socket.sendEvent('input', { data }));
+    outControllerData.subscribe(data => socket.sendEvent('input', { data }))
-        mode.subscribe(data => socket.sendEvent('mode', { data }));
+    mode.subscribe(data => socket.sendEvent('mode', { data }))
-        servoAnglesOut.subscribe(data => socket.sendEvent('angles', { data }));
+    servoAnglesOut.subscribe(data => socket.sendEvent('angles', { data }))
-        kinematicData.subscribe(data => socket.sendEvent('position', { data }));
+    kinematicData.subscribe(data => socket.sendEvent('position', { data }))
-    });
+  })
 
-    onDestroy(() => {
+  onDestroy(() => {
-        removeEventListeners();
+    removeEventListeners()
-    });
+  })
 
-    const addEventListeners = () => {
+  const addEventListeners = () => {
-        socket.on('open', handleOpen);
+    socket.on('open', handleOpen)
-        socket.on('close', handleClose);
+    socket.on('close', handleClose)
-        socket.on('error', handleError);
+    socket.on('error', handleError)
-        socket.on('rssi', handleNetworkStatus);
+    socket.on('rssi', handleNetworkStatus)
-        socket.on('mode', (data: ModesEnum) => mode.set(data));
+    socket.on('mode', (data: ModesEnum) => mode.set(data))
-        socket.on('analytics', handleAnalytics);
+    socket.on('analytics', handleAnalytics)
-        socket.on('angles', (angles: number[]) => {
+    socket.on('angles', (angles: number[]) => {
-            if (angles.length) servoAngles.set(angles);
+      if (angles.length) servoAngles.set(angles)
-        });
+    })
-        features.subscribe(data => {
+    features.subscribe(data => {
-            if (data?.download_firmware) socket.on('otastatus', handleOAT);
+      if (data?.download_firmware) socket.on('otastatus', handleOAT)
-            if (data?.sonar) socket.on('sonar', data => console.log(data));
+      if (data?.sonar) socket.on('sonar', data => console.log(data))
-        });
+    })
-    };
+  }
 
-    const removeEventListeners = () => {
+  const removeEventListeners = () => {
-        socket.off('analytics', handleAnalytics);
+    socket.off('analytics', handleAnalytics)
-        socket.off('open', handleOpen);
+    socket.off('open', handleOpen)
-        socket.off('close', handleClose);
+    socket.off('close', handleClose)
-        socket.off('rssi', handleNetworkStatus);
+    socket.off('rssi', handleNetworkStatus)
-        socket.off('otastatus', handleOAT);
+    socket.off('otastatus', handleOAT)
-    };
+  }
 
-    const handleOpen = () => {
+  const handleOpen = () => {
-        notifications.success('Connection to device established', 5000);
+    notifications.success('Connection to device established', 5000)
-    };
+  }
 
-    const handleClose = () => {
+  const handleClose = () => {
-        notifications.error('Connection to device lost', 5000);
+    notifications.error('Connection to device lost', 5000)
-        telemetry.setRSSI(0);
+    telemetry.setRSSI(0)
-    };
+  }
 
-    const handleError = (data: any) => console.error(data);
+  const handleError = (data: any) => console.error(data)
 
-    const handleAnalytics = (data: Analytics) => analytics.addData(data);
+  const handleAnalytics = (data: Analytics) => analytics.addData(data)
 
-    const handleNetworkStatus = (data: number) => telemetry.setRSSI(data);
+  const handleNetworkStatus = (data: number) => telemetry.setRSSI(data)
 
-    const handleOAT = (data: DownloadOTA) => telemetry.setDownloadOTA(data);
+  const handleOAT = (data: DownloadOTA) => telemetry.setDownloadOTA(data)
 
-    let menuOpen = $state(false);
+  let menuOpen = $state(false)
 </script>
 
 <svelte:head>
-    <title>{page.data.title}</title>
+  <title>{page.data.title}</title>
 </svelte:head>
 
-<div class="drawer">
+<div class="drawer h-screen">
-    <input id="main-menu" type="checkbox" class="drawer-toggle" bind:checked={menuOpen} />
+  <input id="main-menu" type="checkbox" class="drawer-toggle" bind:checked={menuOpen} />
-    <div class="drawer-content flex flex-col">
+  <div class="drawer-content flex flex-col">
-        <!-- Status bar content here -->
+    <!-- Status bar content here -->
-        <Statusbar />
+    <Statusbar />
 
-        <!-- Main page content here -->
+    <!-- Main page content here -->
-        {@render children?.()}
+    {@render children?.()}
-    </div>
+  </div>
-    <!-- Side Navigation -->
+  <!-- Side Navigation -->
-    <div class="drawer-side z-30 shadow-lg">
+  <div class="drawer-side z-30 shadow-lg">
-        <label for="main-menu" class="drawer-overlay"></label>
+    <label for="main-menu" class="drawer-overlay"></label>
-        <Menu menuClicked={() => (menuOpen = false)} />
+    <Menu menuClicked={() => (menuOpen = false)} />
-    </div>
+  </div>
 </div>
 
 <Modals>
-    <!-- svelte-ignore a11y_click_events_have_key_events -->
+  <!-- svelte-ignore a11y_click_events_have_key_events -->
-    <!-- svelte-ignore a11y_no_static_element_interactions -->
+  <!-- svelte-ignore a11y_no_static_element_interactions -->
-    {#snippet backdrop()}
+  {#snippet backdrop()}
-        <div
+    <div
-            class="fixed inset-0 z-40 max-h-full max-w-full bg-black/20 backdrop-blur-sm"
+      class="fixed inset-0 z-40 max-h-full max-w-full bg-black/20 backdrop-blur-sm"
-            transition:fade
+      transition:fade
-            onclick={modals.closeAll}
+      onclick={modals.closeAll}>
-        ></div>
+    </div>
-    {/snippet}
+  {/snippet}
 </Modals>
 
 <Toast />

app/src/routes/+page.svelte

@@ -1,28 +1,29 @@
 <script lang="ts">
-	import type { PageData } from './$types';
+  import { goto } from '$app/navigation'
-	import { notifications } from '$lib/components/toasts/notifications';
+  import Visualization from '$lib/components/Visualization.svelte'
-	import Visualization from '$lib/components/Visualization.svelte';
+  import { socket } from '$lib/stores'
+  import { onMount } from 'svelte'
 
-	interface Props {
+  onMount(() => {
-		data: PageData;
+    socket.subscribe(isConnected => {
-	}
+      if (isConnected) {
-
+        goto('/controller')
-	let { data }: Props = $props();
+      }
+    })
+  })
 </script>
 
-<div class="hero bg-base-100 h-screen">
+<div class="w-full h-full flex justify-center items-center">
-	<div class="card md:card-side bg-base-200 shadow-2xl flex justify-center items-center">
+  <div class="h-full flex flex-col">
-        <div class="w-64 h-64">
+    <div class="grow-3 w-80 relative">
-            <Visualization sky={false} orbit panel={false} ground={false}/>
+      <Visualization sky={false} orbit panel={false} ground={false} zoom={8} />
-        </div>
+      <div class="absolute bottom-0 w-full h-40 bg-gradient-to-t from-base-100 to-transparent">
-		<div class="card-body w-80">
+      </div>
-			<h2 class="card-title text-center text-2xl">Welcome to {data.app_name}</h2>
+    </div>
-			<p class="py-6 text-center"></p>
+    <div class="grow-3 flex justify-center">
-			<a
+      <a class="btn btn-primary rounded-full" href={$socket ? '/controller' : '/connection'}>
-				class="btn btn-primary"
+        Add Robot Dog
-				href="/controller"
+      </a>
-				onclick={() => notifications.success('You did it!', 1000)}>Begin</a
+    </div>
-			>
+  </div>
-		</div>
-	</div>
 </div>