✨ Adds yertle model visulization

app/src/lib/components/Visualization.svelte

@@ -1,5 +1,5 @@
 <script lang="ts">
-  import { onDestroy, onMount } from 'svelte';
+  import { onDestroy, onMount } from 'svelte'
   import {
     BufferGeometry,
     Line,
@@ -10,8 +10,8 @@
     SphereGeometry,
     Vector3,
     type NormalBufferAttributes,
-    type Object3DEventMap,
-  } from 'three';
+    type Object3DEventMap
+  } from 'three'
   import {
     ModesEnum,
     kinematicData,
@@ -22,54 +22,55 @@
     servoAngles,
     mpu,
     jointNames,
-  } from '$lib/stores';
+    currentKinematic
+  } from '$lib/stores'
   import {
     extractFootColor,
     populateModelCache,
     throttler,
-    getToeWorldPositions,
-  } from '$lib/utilities';
-  import SceneBuilder from '$lib/sceneBuilder';
-  import { lerp, degToRad } from 'three/src/math/MathUtils';
-  import { GUI } from 'three/addons/libs/lil-gui.module.min.js';
-  import Kinematic, { type body_state_t } from '$lib/kinematic';
+    getToeWorldPositions
+  } from '$lib/utilities'
+  import SceneBuilder from '$lib/sceneBuilder'
+  import { lerp, degToRad } from 'three/src/math/MathUtils'
+  import { GUI } from 'three/addons/libs/lil-gui.module.min.js'
+  import Kinematic, { type body_state_t } from '$lib/kinematic'
   import {
     BezierState,
     CalibrationState,
     EightPhaseWalkState,
     IdleState,
     RestState,
-    StandState,
-  } from '$lib/gait';
-  import { radToDeg } from 'three/src/math/MathUtils.js';
-  import type { URDFRobot } from 'urdf-loader';
-  import { get } from 'svelte/store';
+    StandState
+  } from '$lib/gait'
+  import { radToDeg } from 'three/src/math/MathUtils.js'
+  import type { URDFRobot } from 'urdf-loader'
+  import { get } from 'svelte/store'
 
   interface Props {
-    sky?: boolean;
-    orbit?: boolean;
-    panel?: boolean;
-    debug?: boolean;
-    ground?: boolean;
+    sky?: boolean
+    orbit?: boolean
+    panel?: boolean
+    debug?: boolean
+    ground?: boolean
   }
 
-  let { sky = true, orbit = false, panel = true, debug = false, ground = true }: Props = $props();
+  let { sky = true, orbit = false, panel = true, debug = false, ground = true }: Props = $props()
 
-  let sceneManager = $state(new SceneBuilder());
-  let canvas: HTMLCanvasElement;
+  let sceneManager = $state(new SceneBuilder())
+  let canvas: HTMLCanvasElement
 
-  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 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 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 target_position = { x: 0, z: 0, yaw: 0 }
 
-  let kinematic = new Kinematic();
+  let kinematic = get(currentKinematic)
 
   let planners = {
     [ModesEnum.Deactivated]: new IdleState(),
@@ -78,11 +79,11 @@
     [ModesEnum.Rest]: new RestState(),
     [ModesEnum.Stand]: new StandState(),
     [ModesEnum.Crawl]: new EightPhaseWalkState(),
-    [ModesEnum.Walk]: new BezierState(),
-  };
-  let lastTick = performance.now();
+    [ModesEnum.Walk]: new BezierState()
+  }
+  let lastTick = performance.now()
 
-  const dir = [1, -1, -1, -1, -1, -1, 1, -1, -1, -1, -1, -1];
+  const dir = [1, -1, -1, -1, -1, -1, 1, -1, -1, -1, -1, -1]
 
   let body_state = {
     omega: 0,
@@ -91,8 +92,8 @@
     xm: 0,
     ym: 0.5,
     zm: 0,
-    feet: kinematic.getDefaultFeetPos(),
-  };
+    feet: kinematic.getDefaultFeetPos()
+  }
 
   let settings = {
     'Internal kinematic': true,
@@ -109,52 +110,52 @@
     xm: 0,
     ym: 0.7,
     zm: 0,
-    Background: 'black',
-  };
+    Background: 'black'
+  }
 
   onMount(async () => {
-    await populateModelCache();
-    await createScene();
-    servoAngles.subscribe(updateAnglesFromStore);
-    if (panel) createPanel();
-  });
+    await populateModelCache()
+    await createScene()
+    servoAngles.subscribe(updateAnglesFromStore)
+    if (panel) createPanel()
+  })
 
   onDestroy(() => {
-    canvas.remove();
-    gui_panel?.destroy();
-  });
+    canvas.remove()
+    gui_panel?.destroy()
+  })
 
   const updateAnglesFromStore = (angles: number[]) => {
-    if (sceneManager.isDragging) return;
-    if (settings['Internal kinematic']) return;
-    modelTargetAngles = angles;
-  };
+    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';
+    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 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 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 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([
@@ -163,17 +164,14 @@
       settings.psi,
       settings.xm,
       settings.ym,
-      settings.zm,
-    ]);
-  };
+      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
-    );
-  };
+    modelTargetAngles[$jointNames.indexOf(name)] = angle * (180 / Math.PI)
+    Throttler.throttle(() => servoAnglesOut.set(modelTargetAngles.map(num => Math.round(num))), 100)
+  }
 
   const createScene = async () => {
     sceneManager
@@ -187,46 +185,46 @@
       .addTransformControls(sceneManager.model)
       .fillParent()
       .addRenderCb(render)
-      .startRenderLoop();
+      .startRenderLoop()
 
-    if (ground) sceneManager.addGroundPlane();
+    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);
+    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);
+      sceneManager.addDragControl(updateAngles)
     }
-    if (sky) sceneManager.addSky();
+    if (sky) sceneManager.addSky()
 
     for (let i = 0; i < 4; i++) {
-      const geometry = new BufferGeometry();
-      const material = new LineBasicMaterial({ color: extractFootColor() });
-      const line = new Line(geometry, material);
-      trace_lines.push(geometry);
-      sceneManager.scene.add(line);
+      const geometry = new BufferGeometry()
+      const material = new LineBasicMaterial({ color: extractFootColor() })
+      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;
+      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]);
-    });
-  };
+      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;
+    if (sceneManager.isDragging || !settings['Internal kinematic']) return
     const position: body_state_t = {
       omega: settings.omega,
       phi: settings.phi,
@@ -234,37 +232,37 @@
       xm: settings.xm,
       ym: settings.ym,
       zm: settings.zm,
-      feet: body_state.feet,
-    };
+      feet: body_state.feet
+    }
 
-    let new_angles = kinematic.calcIK(position).map((x, i) => radToDeg(x * dir[i]));
-    modelTargetAngles = new_angles;
-  };
+    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));
+    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.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);
-  };
+    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();
-  };
+    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;
-  };
+    return settings['Smooth motion'] ? lerp(start, end, amount) : end
+  }
 
   const update_gait = () => {
-    if (sceneManager.isDragging || !settings['Internal kinematic']) return;
-    const controlData = get(outControllerData);
+    if (sceneManager.isDragging || !settings['Internal kinematic']) return
+    const controlData = get(outControllerData)
     const data = {
       stop: controlData[0],
       lx: controlData[1],
@@ -273,67 +271,67 @@
       ry: controlData[4],
       h: controlData[5],
       s: controlData[6],
-      s1: controlData[7],
-    };
-    body_state.ym = ((data.h + 127) * 0.35) / 100;
+      s1: controlData[7]
+    }
+    body_state.ym = ((data.h + 127) * 0.35) / 100
 
-    let planner = planners[get(mode)];
-    const delta = performance.now() - lastTick;
-    lastTick = performance.now();
+    let planner = planners[get(mode)]
+    const delta = performance.now() - lastTick
+    lastTick = performance.now()
 
-    body_state = planner.step(body_state, data, delta);
+    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;
-  };
+    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;
-  };
+    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);
-  };
+    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 robot = sceneManager.model
+    if (!robot) return
 
-    const toes = getToeWorldPositions(robot);
+    const toes = getToeWorldPositions(robot)
 
-    renderTraceLines(toes);
-    update_camera(robot);
-    update_gait();
-    calculate_kinematics();
-    update_robot_position(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'];
+    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]));
+      )
+      robot.joints[$jointNames[i]].setJointValue(degToRad(currentModelAngles[i]))
     }
 
-    orient_robot(robot, toes);
-    updateTargetPosition();
-  };
+    orient_robot(robot, toes)
+    updateTargetPosition()
+  }
 </script>
 
 <svelte:window onresize={sceneManager.fillParent} />

app/src/lib/gait.ts

@@ -1,6 +1,6 @@
+import { get } from 'svelte/store'
 import type { body_state_t } from './kinematic'
-import Kinematic from './kinematic'
-import { fromInt8 } from './utilities'
+import { currentKinematic } from './stores/featureFlags'
 
 const { sin } = Math
 
@@ -39,7 +39,7 @@ export abstract class GaitState {
   }
 
   public get default_feet_pos() {
-    return new Kinematic().getDefaultFeetPos()
+    return get(currentKinematic).getDefaultFeetPos()
   }
 
   protected get default_height() {

app/src/lib/kinematic.ts

@@ -20,6 +20,15 @@ export interface target_position {
   yaw: number
 }
 
+export interface KinematicParams {
+  l1: number
+  l2: number
+  l3: number
+  l4: number
+  L: number
+  W: number
+}
+
 const { cos, sin, atan2, acos, sqrt, max, min } = Math
 
 const DEG2RAD = 0.017453292519943
@@ -43,14 +52,13 @@ export default class Kinematic {
     [1, 0, 0]
   ]
 
-  constructor() {
-    this.l1 = 60.5 / 100
-    this.l2 = 10 / 100
-    this.l3 = 111.7 / 100
-    this.l4 = 118.5 / 100
-
-    this.L = 207.5 / 100
-    this.W = 78 / 100
+  constructor(params: KinematicParams) {
+    this.l1 = params.l1
+    this.l2 = params.l2
+    this.l3 = params.l3
+    this.l4 = params.l4
+    this.L = params.L
+    this.W = params.W
 
     this.mountOffsets = [
       [this.L / 2, 0, this.W / 2],

app/src/lib/stores/featureFlags.ts

@@ -1,7 +1,8 @@
 import { api } from '$lib/api'
 import { notifications } from '$lib/components/toasts/notifications'
+import Kinematic from '$lib/kinematic'
 import { persistentStore } from '$lib/utilities'
-import { type Writable } from 'svelte/store'
+import { derived, type Writable } from 'svelte/store'
 
 let featureFlagsStore: Writable<Record<string, boolean | string>>
 
@@ -19,3 +20,42 @@ export function useFeatureFlags() {
 
   return featureFlagsStore
 }
+
+export const variants = {
+  SPOTMICRO_ESP32: {
+    model: '/spot_micro.urdf.xacro',
+    stl: '/stl.zip',
+    kinematics: {
+      l1: 60.5 / 100,
+      l2: 10 / 100,
+      l3: 111.7 / 100,
+      l4: 118.5 / 100,
+      L: 207.5 / 100,
+      W: 78 / 100
+    }
+  },
+  SPOTMICRO_YERTLE: {
+    model: '/yertle.URDF',
+    stl: '/URDF.zip',
+    kinematics: {
+      l1: 35 / 100,
+      l2: 0 / 100,
+      l3: 130 / 100,
+      l4: 130 / 100,
+      L: 240 / 100,
+      W: 78 / 100
+    }
+  }
+}
+
+export const currentVariant = derived(useFeatureFlags(), $flagStore => {
+  const variantFlag = $flagStore['variant'] as string
+  return variantFlag && variants[variantFlag as keyof typeof variants] ?
+      variants[variantFlag as keyof typeof variants]
+    : variants.SPOTMICRO_ESP32
+})
+
+export const currentKinematic = derived(
+  currentVariant,
+  $variant => new Kinematic($variant.kinematics)
+)

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

@@ -2,15 +2,16 @@ import { Color, LoaderUtils, Vector3 } from 'three'
 import URDFLoader, { type URDFRobot } from 'urdf-loader'
 import { XacroLoader } from 'xacro-parser'
 import { Result } from '$lib/utilities'
-import { jointNames, model } from '$lib/stores'
+import { currentVariant, jointNames, model } from '$lib/stores'
 import uzip from 'uzip'
 import { fileService } from '$lib/services'
+import { get } from 'svelte/store'
 
 let model_xml: XMLDocument
 
 export const populateModelCache = async () => {
   await cacheModelFiles()
-  const modelRes = await loadModel('/spot_micro.urdf.xacro')
+  const modelRes = await loadModel(get(currentVariant).model)
   if (modelRes.isOk()) {
     const [urdf, JOINT_NAME] = modelRes.inner
     jointNames.set(JOINT_NAME)
@@ -21,7 +22,7 @@ export const populateModelCache = async () => {
 }
 
 export const cacheModelFiles = async () => {
-  const data = await fetch('/stl.zip')
+  const data = await fetch(get(currentVariant).stl)
 
   const files = uzip.parse(await data.arrayBuffer())