🎨 Expand animation with looping

app/src/lib/components/Visualization.svelte

@@ -1,13 +1,16 @@
 <script lang="ts">
     import { onDestroy, onMount } from 'svelte'
     import {
+        BufferGeometry,
+        Line,
+        LineBasicMaterial,
         Mesh,
         MeshBasicMaterial,
         type Object3D,
         SphereGeometry,
         Vector3,
-        type Object3DEventMap,
-        Color
+        type NormalBufferAttributes,
+        type Object3DEventMap
     } from 'three'
     import {
         ModesEnum,
@@ -23,31 +26,37 @@
         walkGait,
         walkGaitToMode
     } from '$lib/stores'
-    import { populateModelCache, throttler, getToeWorldPositions } from '$lib/utilities'
+    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 { type body_state_t } from '$lib/kinematic'
-    import { BezierState, CalibrationState, IdleState, RestState, StandState } from '$lib/gait'
+    import {
+        Animater,
+        BezierState,
+        CalibrationState,
+        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'
 
     interface Props {
-        defaultColor?: string | null
+        sky?: boolean
         orbit?: boolean
         panel?: boolean
         debug?: boolean
         ground?: boolean
     }
 
-    let {
-        defaultColor = '#0091ff',
-        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
@@ -69,7 +78,8 @@
         [ModesEnum.Calibration]: new CalibrationState(),
         [ModesEnum.Rest]: new RestState(),
         [ModesEnum.Stand]: new StandState(),
-        [ModesEnum.Walk]: new BezierState()
+        [ModesEnum.Walk]: new BezierState(),
+        [ModesEnum.Animate]: new Animater()
     }
     let lastTick = performance.now()
 
@@ -106,7 +116,7 @@
         xm: 0,
         ym: 0.7,
         zm: 0,
-        Background: defaultColor
+        Background: 'black'
     }
 
     onMount(async () => {
@@ -151,7 +161,7 @@
         visibility.add(settings, 'Trace points', 1, 1000, 1)
         visibility.add(settings, 'Target position')
         visibility.add(settings, 'Smooth motion')
-        visibility.addColor(settings, 'Background').onChange(setSceneBackground).listen()
+        visibility.addColor(settings, 'Background')
     }
 
     const updateKinematicPosition = () => {
@@ -165,8 +175,6 @@
         ])
     }
 
-    const setSceneBackground = (c: string | null) => (sceneManager.scene.background = new Color(c!))
-
     const updateAngles = (name: string, angle: number) => {
         modelTargetAngles[$jointNames.indexOf(name)] = angle * (180 / Math.PI)
         Throttler.throttle(
@@ -197,13 +205,13 @@
         sceneManager.scene.add(target)
 
         if (debug) {
-            sceneManager.addDragControl(angles => {
+            sceneManager.addDragControl((angles: Record<string, number>) => {
                 Object.entries(angles).forEach(([name, angle]) => {
                     updateAngles(name, angle)
                 })
             })
         }
-        if (defaultColor) setSceneBackground(settings['Background'] || defaultColor)
+        if (sky) sceneManager.addSky()
     }
 
     const calculate_kinematics = () => {

app/src/lib/gait.ts

@@ -490,4 +490,465 @@ const comb = (n: number, k: number): number => {
     let c = 1
     for (let i = 0; i < k; i++) c = (c * (n - i)) / (i + 1)
     return c
+};
+
+/*
+Units:  meters, radians, seconds / beats
+*/
+
+// interface Options {
+//     controls: 'body' | 'legs' | 'both';
+
+//     extendable?: boolean; // if true, the animation can loop
+//     description?: string; // a description of the animation
+// }
+
+interface Frame {
+  time: number
+  position: number[]
+  orientation: number[]
+  feet?: number[][]
+}
+
+type Parameter = {
+  // name: string;
+  min: number
+  max: number
+  default: number
+}
+
+type Parameters = Record<string, Parameter>
+
+interface Animation {
+  // options: Options = {};
+  parameters: Parameters
+  frames: Frame[]
+  loop?: boolean
+}
+
+const generateCircleAnimation = (
+  radius: number,
+  y: number,
+  duration: number,
+  segments: number
+): Animation => {
+  const frames: Frame[] = []
+  const deltaTime = duration / segments
+
+  for (let i = 0; i <= segments; i++) {
+    const angle = (2 * Math.PI * i) / segments // Angle in radians
+    const x = radius * Math.cos(angle)
+    const z = radius * Math.sin(angle)
+
+    frames.push({
+      time: i * deltaTime,
+      position: [x, y, z],
+      orientation: [0, 0, 0]
+    })
+  }
+
+  return {
+    parameters: {
+      speed: { min: 0.1, max: 2, default: 1 },
+      x_offset: { min: -0.1, max: 0.1, default: 0 }
+    },
+    frames
+  }
+}
+
+const kinematicShowCaseGen = generateCircleAnimation(0.5, 0.7, 4000, 32)
+
+const kinematicShowCase: Animation = {
+  loop: true,
+  parameters: {
+    speed: { min: 0.1, max: 2, default: 1 },
+    x_offset: { min: -0.1, max: 0.1, default: 0 }
+  },
+  frames: [
+    {
+      time: 0,
+      position: [0.5, 0.7, 0],
+      orientation: [0, 0, 0],
+      feet: [
+        [1, -1, 1, 1],
+        [1, -1, -1, 1],
+        [-1, -1, 1, 1],
+        [-1, -1, -1, 1]
+      ]
+    },
+    {
+      time: 500,
+      position: [0.3, 0.7, 0.3],
+      orientation: [0, 0, 0]
+    },
+    {
+      time: 1000,
+      position: [0, 0.7, 0.5],
+      orientation: [0, 0, 0]
+    },
+    {
+      time: 1500,
+      position: [-0.3, 0.7, 0.3],
+      orientation: [0, 0, 0]
+    },
+    {
+      time: 2000,
+      position: [-0.5, 0.7, 0],
+      orientation: [0, 0, 0]
+    },
+    {
+      time: 2500,
+      position: [-0.3, 0.7, -0.3],
+      orientation: [0, 0, 0]
+    },
+    {
+      time: 3000,
+      position: [0, 0.7, -0.5],
+      orientation: [0, 0, 0]
+    },
+    {
+      time: 3500,
+      position: [0.3, 0.7, -0.3],
+      orientation: [0, 0, 0]
+    },
+    {
+      time: 4000,
+      position: [0.5, 0.7, 0],
+      orientation: [0, 0, 0]
+    }
+  ]
+}
+
+const stretch: Animation = {
+  loop: false,
+  parameters: {
+    speed: { min: 0.1, max: 2, default: 1 },
+    x_offset: { min: -0.1, max: 0.1, default: 0 }
+  },
+  frames: [
+    // Step forward
+    {
+      time: 0,
+      position: [0, 0.7, 0],
+      orientation: [0, 0, 0],
+      feet: [
+        [1, -1, 1, 1],
+        [1, -1, -1, 1],
+        [-1, -1, 1, 1],
+        [-1, -1, -1, 1]
+      ]
+    },
+    {
+      time: 250,
+      position: [0, 0.7, -0.2],
+      orientation: [0, 0, 0],
+      feet: [
+        [1.5, -0.5, 1, 1],
+        [1, -1, -1, 1],
+        [-1, -1, 1, 1],
+        [-1, -1, -1, 1]
+      ]
+    },
+    {
+      time: 500,
+      position: [0, 0.7, -0.2],
+      orientation: [0, 0, 0],
+      feet: [
+        [2, -1, 1, 1],
+        [1, -1, -1, 1],
+        [-1, -1, 1, 1],
+        [-1, -1, -1, 1]
+      ]
+    },
+    {
+      time: 750,
+      position: [0, 0.7, 0.2],
+      orientation: [0, 0, 0],
+      feet: [
+        [2, -1, 1, 1],
+        [1, -1, -1, 1],
+        [-1, -1, 1, 1],
+        [-1, -1, -1, 1]
+      ]
+    },
+    {
+      time: 1000,
+      position: [0, 0.7, 0.2],
+      orientation: [0, 0, 0],
+      feet: [
+        [2, -1, 1, 1],
+        [1.5, -0.5, -1, 1],
+        [-1, -1, 1, 1],
+        [-1, -1, -1, 1]
+      ]
+    },
+    {
+      time: 1250,
+      position: [0, 0.7, 0.2],
+      orientation: [0, 0, 0],
+      feet: [
+        [2, -1, 1, 1],
+        [2, -1, -1, 1],
+        [-1, -1, 1, 1],
+        [-1, -1, -1, 1]
+      ]
+    },
+    {
+      time: 2500,
+      position: [0.5, 0.7, 0],
+      orientation: [0, 0, 25]
+    },
+    {
+      time: 4000,
+      position: [-0.7, 0.7, 0],
+      orientation: [0, 0, -20]
+    },
+    {
+      time: 5000,
+      position: [-0.7, 0.7, 0],
+      orientation: [0, 0, -20]
+    },
+    {
+      time: 6000,
+      position: [0, 0.7, 0],
+      orientation: [0, 0, 0]
+    },
+    {
+      time: 6000,
+      position: [-0.2, 0.7, -0.2],
+      orientation: [0, 0, 0],
+      feet: [
+        [2, -1, 1, 1],
+        [2, -1, -1, 1],
+        [-1, -1, 1, 1],
+        [-1, -1, -1, 1]
+      ]
+    },
+    {
+      time: 6500,
+      position: [-0.2, 0.7, -0.2],
+      orientation: [0, 0, 0],
+      feet: [
+        [0.5, -0.5, 1, 1],
+        [2, -1, -1, 1],
+        [-1, -1, 1, 1],
+        [-1, -1, -1, 1]
+      ]
+    },
+    {
+      time: 7000,
+      position: [-0.2, 0.7, 0.2],
+      orientation: [0, 0, 0],
+      feet: [
+        [1, -1, 1, 1],
+        [2, -1, -1, 1],
+        [-1, -1, 1, 1],
+        [-1, -1, -1, 1]
+      ]
+    },
+    {
+      time: 7500,
+      position: [-0.2, 0.7, 0.2],
+      orientation: [0, 0, 0],
+      feet: [
+        [1, -1, 1, 1],
+        [0.5, -0.5, -1, 1],
+        [-1, -1, 1, 1],
+        [-1, -1, -1, 1]
+      ]
+    },
+    {
+      time: 8000,
+      position: [0, 0.7, 0],
+      orientation: [0, 0, 0],
+      feet: [
+        [1, -1, 1, 1],
+        [1, -1, -1, 1],
+        [-1, -1, 1, 1],
+        [-1, -1, -1, 1]
+      ]
+    }
+  ]
+}
+
+const pee: Animation = {
+  loop: false,
+
+  parameters: {
+    speed: { min: 0.1, max: 2, default: 1 },
+    x_offset: { min: -0.1, max: 0.1, default: 0 }
+  },
+  frames: [
+    {
+      time: 0,
+      position: [0, 0.7, 0],
+      orientation: [0, 0, 0],
+      feet: [
+        [1, -1, 1, 1],
+        [1, -1, -1, 1],
+        [-1, -1, 1, 1],
+        [-1, -1, -1, 1]
+      ]
+    },
+    {
+      time: 1000,
+      position: [0, 0.7, 0],
+      orientation: [0, 0, 0],
+      feet: [
+        [1, -1, 1, 1],
+        [1, -1, -1, 1],
+        [-1, -1, 1, 1],
+        [-1, -1, -1, 1]
+      ]
+    },
+    {
+      time: 2000,
+      position: [0.2, 0.7, 0.45],
+      orientation: [0, 0, 0],
+      feet: [
+        [1, -1, 1, 1],
+        [1, -1, -1, 1],
+        [-1, -1, 1, 1],
+        [-1, -1, -1, 1]
+      ]
+    },
+    {
+      time: 3000,
+      position: [0.2, 0.7, 0.45],
+      orientation: [0, 0, 0],
+      feet: [
+        [1, -1, 1, 1],
+        [1, -1, -1, 1],
+        [-1, -1, 1, 1],
+        [-1, -1, -1, 1]
+      ]
+    },
+    {
+      time: 4000,
+      position: [0.2, 0.7, 0.45],
+      orientation: [0, 0, 0],
+      feet: [
+        [1, -1, 1, 1],
+        [1, -1, -1, 1],
+        [-1, -1, 1, 1],
+        [-1, 0, -1, 1]
+      ]
+    },
+    {
+      time: 5000,
+      position: [0.2, 0.7, 0.45],
+      orientation: [0, 0, 0],
+      feet: [
+        [1, -1, 1, 1],
+        [1, -1, -1, 1],
+        [-1, -1, 1, 1],
+        [-1, -1, -1, 1]
+      ]
+    },
+    {
+      time: 6000,
+      position: [0, 0.7, 0],
+      orientation: [0, 0, 0],
+      feet: [
+        [1, -1, 1, 1],
+        [1, -1, -1, 1],
+        [-1, -1, 1, 1],
+        [-1, -1, -1, 1]
+      ]
+    }
+  ]
+}
+
+export class Animater extends GaitState {
+  protected name = 'Bezier'
+  time = 0
+  animation = [stretch, pee, kinematicShowCase][0]
+  speed = 1
+  xOffset = 0
+
+  begin() {
+    this.reset()
+    super.begin()
+  }
+
+  end() {
+    this.reset()
+    super.end()
+  }
+
+  reset() {
+    this.time = 0
+  }
+
+  step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
+    return this.step_animation(body_state, dt)
+  }
+
+  setAnimation(animation: Animation) {
+    this.animation = animation
+    this.reset()
+  }
+
+  step_animation(body_state: body_state_t, dt: number = 0.02) {
+    this.dt = dt / 1000
+    const frames = this.animation.frames
+    const duration = frames[frames.length - 1].time
+
+    this.time += dt * this.speed
+    if (this.animation.loop !== false && this.time > duration) {
+      this.time = this.time % duration
+    } else if (this.time > duration) {
+      this.time = duration
+    }
+
+    const { prevFrame, nextFrame } = this.getBoundingFrames()
+
+    const t = this.getInterpolationFactor(prevFrame, nextFrame)
+    const position = this.interpolatePosition(prevFrame.position, nextFrame.position, t)
+    const orientation = this.interpolatePosition(prevFrame.orientation, nextFrame.orientation, t)
+
+    // Apply x_offset
+    // position[0] += this.xOffset;
+
+    body_state.xm = position[0]
+    body_state.ym = position[1]
+    body_state.zm = position[2]
+    body_state.omega = orientation[0]
+    body_state.phi = orientation[1]
+    body_state.psi = orientation[2]
+
+    if (prevFrame.feet && nextFrame.feet) {
+      for (let i = 0; i < 4; i++) {
+        body_state.feet[i] = this.interpolatePosition(prevFrame.feet[i], nextFrame.feet[i], t)
+      }
+    }
+
+    return body_state
+  }
+
+  private getBoundingFrames(): { prevFrame: Frame; nextFrame: Frame } {
+    const frames = this.animation.frames
+
+    for (let i = 0; i < frames.length - 1; i++) {
+      const prevFrame = frames[i]
+      const nextFrame = frames[i + 1]
+
+      if (this.time >= prevFrame.time && this.time <= nextFrame.time) {
+        return { prevFrame, nextFrame }
+      }
+    }
+
+    // Fallback (should not be reached if looping correctly)
+    return { prevFrame: frames[frames.length - 1], nextFrame: frames[0] }
+  }
+
+  private getInterpolationFactor(prevFrame: Frame, nextFrame: Frame): number {
+    const timeRange = nextFrame.time - prevFrame.time
+    const elapsed = this.time - prevFrame.time
+    return elapsed / timeRange
+  }
+
+  private interpolatePosition(pos1: number[], pos2: number[], t: number): number[] {
+    return pos1.map((val, index) => val + t * (pos2[index] - val))
+  }
 }

app/src/lib/sceneBuilder.ts

@@ -17,16 +17,18 @@ import {
     MeshPhongMaterial,
     EquirectangularReflectionMapping,
     ACESFilmicToneMapping,
+    MathUtils,
     Group,
     MeshBasicMaterial,
-    RepeatWrapping,
-    Object3D
+    RepeatWrapping
 } from 'three'
+import { Sky } from 'three/addons/objects/Sky.js'
 import { OrbitControls } from 'three/examples/jsm/controls/OrbitControls'
 import { TransformControls } from 'three/examples/jsm/controls/TransformControls'
 import { Reflector } from 'three/examples/jsm/objects/Reflector.js'
 import { type URDFJoint, type URDFMimicJoint, type URDFRobot } from 'urdf-loader'
 import { PointerURDFDragControls } from 'urdf-loader/src/URDFDragControls'
+import { sunCalculator } from './utilities/position-utilities'
 
 export const addScene = () => new Scene()
 
@@ -63,6 +65,8 @@ export default class SceneBuilder {
     private fog!: FogExp2
     private isLoaded: boolean = false
     public isDragging: boolean = false
+    highlightMaterial: MeshPhongMaterial
+    sky!: Sky
     transformControl: TransformControls
     public modelGroup!: Group
 
@@ -85,6 +89,34 @@ export default class SceneBuilder {
         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)
@@ -302,7 +334,7 @@ export default class SceneBuilder {
         )
         dragControls.updateJoint = (joint: URDFMimicJoint, angle: number) => {
             this.setJointValue(joint.name, angle)
-            updateAngle({ [joint.name]: angle })
+            updateAngle(joint.name, angle)
         }
         dragControls.onDragStart = () => {
             this.orbit.enabled = false

app/src/lib/stores/imu.ts

@@ -1,5 +1,5 @@
 import { writable } from 'svelte/store'
-import type { IMUMsg } from '$lib/types/models'
+import type { IMU } from '$lib/types/models'
 
 const maxIMUData = 100
 
@@ -14,24 +14,11 @@ export const imu = (() => {
         bmp_temp: [] as number[]
     })
 
-    const addData = (content: IMUMsg) => {
+    const addData = (content: IMU) => {
         update(data => {
-            if (content.imu && content.imu[4]) {
-                data.x = [...data.x, content.imu[0]].slice(-maxIMUData)
-                data.y = [...data.y, content.imu[1]].slice(-maxIMUData)
-                data.z = [...data.z, content.imu[2]].slice(-maxIMUData)
-            }
-
-            if (content.mag && content.mag[4]) {
-                data.heading = [...data.heading, content.mag[3]].slice(-maxIMUData)
-            }
-
-            if (content.bmp && content.bmp[3]) {
-                data.pressure = [...data.pressure, content.bmp[0]].slice(-maxIMUData)
-                data.altitude = [...data.altitude, content.bmp[1]].slice(-maxIMUData)
-                data.bmp_temp = [...data.bmp_temp, content.bmp[2]].slice(-maxIMUData)
-            }
-
+            ;(Object.keys(content) as (keyof IMU)[]).forEach(key => {
+                data[key] = [...data[key], content[key]].slice(-maxIMUData)
+            })
             return data
         })
     }

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

@@ -8,17 +8,26 @@ export const jointNames = persistentStore('joint_names', <string[]>[])
 
 export const model = writable()
 
-export const modes = ['deactivated', 'idle', 'calibration', 'rest', 'stand', 'walk'] as const
+export const modes = [
+  'deactivated',
+  'idle',
+  'calibration',
+  'rest',
+  'stand',
+  'walk',
+  'animate'
+] as const
 
 export type Modes = (typeof modes)[number]
 
 export enum ModesEnum {
-    Deactivated = 0,
-    Idle = 1,
-    Calibration = 2,
-    Rest = 3,
-    Stand = 4,
-    Walk = 5
+  Deactivated = 0,
+  Idle = 1,
+  Calibration = 2,
+  Rest = 3,
+  Stand = 4,
+  Walk = 5,
+  Animate = 6
 }
 
 export enum WalkGaits {

app/src/lib/types/models.ts

@@ -154,12 +154,6 @@ export type IMU = {
     pressure: number
 }
 
-export type IMUMsg = {
-    imu: [number, number, number, number, boolean]
-    mag: [number, number, number, number, boolean]
-    bmp: [number, number, number, boolean]
-}
-
 export interface I2CDevice {
     address: number
     part_number: string

app/src/lib/utilities/index.ts

@@ -4,5 +4,6 @@ export * from './svelte-utilities'
 export * from './math-utilities'
 export * from './buffer-utilities'
 export * from './model-utilities'
+export * from './position-utilities'
 export * from './string-utilities'
 export * from './color-utilities'

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

@@ -0,0 +1,86 @@
+class SunCalculator {
+    calculateSunElevation(lat: number = 55, lon: number = 12) {
+        const now = new Date()
+        const JD = this.getJulianDate(now)
+        const solarDec = this.getSolarDeclination(JD)
+        const solarTime = this.getSolarTime(now, lon)
+
+        const hourAngle = (solarTime - 12) * 15
+        const elevation = Math.asin(
+            Math.sin(this.degToRad(lat)) * Math.sin(solarDec) +
+                Math.cos(this.degToRad(lat)) *
+                    Math.cos(solarDec) *
+                    Math.cos(this.degToRad(hourAngle))
+        )
+
+        return this.radToDeg(elevation)
+    }
+
+    getJulianDate(date: Date) {
+        const Y = date.getUTCFullYear()
+        const M = date.getUTCMonth() + 1
+        const D =
+            date.getUTCDate() +
+            date.getUTCHours() / 24 +
+            date.getUTCMinutes() / 1440 +
+            date.getUTCSeconds() / 86400
+        const A = Math.floor((14 - M) / 12)
+        const Y1 = Y + 4800 - A
+        const M1 = M + 12 * A - 3
+        return (
+            D +
+            Math.floor((153 * M1 + 2) / 5) +
+            365 * Y1 +
+            Math.floor(Y1 / 4) -
+            Math.floor(Y1 / 100) +
+            Math.floor(Y1 / 400) -
+            32045
+        )
+    }
+
+    getSolarDeclination(JulianDate: number) {
+        const n = JulianDate - 2451545
+        const L = (280.46 + 0.9856474 * n) % 360
+        const g = this.degToRad((357.528 + 0.9856003 * n) % 360)
+        const lambda = this.degToRad(L + 1.915 * Math.sin(g) + 0.02 * Math.sin(2 * g))
+        return Math.asin(Math.sin(lambda) * Math.sin(this.degToRad(23.44)))
+    }
+
+    getSolarTime(date: Date, lon: number) {
+        const EoT = this.getEquationOfTime(date)
+        const offset = date.getTimezoneOffset() / 60
+        const standardMeridian = Math.round(lon / 15) * 15
+        const solarTime =
+            date.getUTCHours() +
+            (date.getUTCMinutes() + (4 * (standardMeridian - lon) + EoT)) / 60 -
+            offset
+        return (solarTime + 24) % 24
+    }
+
+    getEquationOfTime(date: Date) {
+        const JD = this.getJulianDate(date)
+        const n = JD - 2451545
+        const g = this.degToRad((357.528 + 0.9856003 * n) % 360)
+        const q = this.degToRad((280.46 + 0.9856474 * n) % 360)
+        return (
+            4 *
+            this.radToDeg(
+                0.000075 +
+                    0.001868 * Math.cos(q) -
+                    0.032077 * Math.sin(g) -
+                    0.014615 * Math.cos(2 * q) -
+                    0.040849 * Math.sin(2 * g)
+            )
+        )
+    }
+
+    degToRad(deg: number) {
+        return deg * (Math.PI / 180)
+    }
+
+    radToDeg(rad: number) {
+        return rad * (180 / Math.PI)
+    }
+}
+
+export const sunCalculator = new SunCalculator()

app/src/routes/+page.svelte

@@ -17,7 +17,7 @@
 <div class="hero bg-base-100 h-screen">
     <div class="card md:card-side bg-base-200 shadow-2xl flex justify-center items-center">
         <div class="w-64 h-64">
-            <Visualization defaultColor={null} orbit panel={false} ground={false} />
+            <Visualization sky={false} orbit panel={false} ground={false} />
         </div>
         <div class="card-body w-80">
             <h2 class="card-title text-center text-2xl">Begin you journey</h2>

app/src/routes/peripherals/i2c/i2c.svelte

@@ -16,11 +16,6 @@
             part_number: 'MPU6050',
             name: 'Six-Axis (Gyro + Accelerometer) MEMS MotionTracking™ Devices'
         },
-        {
-            address: 105,
-            part_number: 'ICM20948',
-            name: 'Nine-Axis (Gyro + Accelerometer + Magnetometer) MEMS MotionTracking™ Device'
-        },
         { address: 115, part_number: 'PAJ7620U2', name: 'Gesture sensor' },
         { address: 119, part_number: 'BMP085', name: 'Temp/Barometric' }
     ]

app/src/routes/peripherals/imu/imu.svelte

@@ -6,7 +6,7 @@
     import { slide } from 'svelte/transition'
     import { onDestroy, onMount } from 'svelte'
     import { socket } from '$lib/stores'
-    import { MessageTopic, type IMUMsg } from '$lib/types/models'
+    import { MessageTopic, type IMU } from '$lib/types/models'
     import { useFeatureFlags } from '$lib/stores/featureFlags'
     import { Rotate3d } from '$lib/components/icons'
 
@@ -18,12 +18,10 @@
     let angleChartElement: HTMLCanvasElement
     let tempChartElement: HTMLCanvasElement
     let altitudeChartElement: HTMLCanvasElement
-    let magnetometerChartElement: HTMLCanvasElement
 
     let angleChart: Chart
     let tempChart: Chart
     let altitudeChart: Chart
-    let magnetometerChart: Chart
 
     const getChartColors = () => {
         const style = getComputedStyle(document.body)
@@ -173,37 +171,6 @@
                 }
             }
         })
-
-        magnetometerChart = new Chart(magnetometerChartElement, {
-            type: 'line',
-            data: {
-                datasets: [
-                    {
-                        label: 'Heading',
-                        borderColor: colors.primary,
-                        backgroundColor: colors.primary,
-                        borderWidth: 2,
-                        data: $imu.heading,
-                        yAxisID: 'y'
-                    }
-                ]
-            },
-            options: {
-                ...baseConfig,
-                scales: {
-                    ...baseConfig.scales,
-                    y: {
-                        ...baseConfig.scales.y,
-                        title: {
-                            display: true,
-                            text: 'Heading [°]',
-                            color: colors.background,
-                            font: { size: 16, weight: 'bold' }
-                        }
-                    }
-                }
-            }
-        })
     }
 
     const updateChartData = (chart: Chart, data: number[]) => {
@@ -227,10 +194,6 @@
             angleChart.update('none')
         }
 
-        if ($features.mag) {
-            updateChartData(magnetometerChart, $imu.heading)
-        }
-
         if ($features.bmp) {
             updateChartData(tempChart, $imu.bmp_temp)
             updateChartData(altitudeChart, $imu.altitude)
@@ -238,7 +201,7 @@
     }
 
     onMount(() => {
-        socket.on(MessageTopic.imu, (data: IMUMsg) => {
+        socket.on(MessageTopic.imu, (data: IMU) => {
             console.log(data)
             imu.addData(data)
         })
@@ -272,17 +235,6 @@
         </div>
     {/if}
 
-    {#if $features.mag}
-        <div class="w-full overflow-x-auto">
-            <div
-                class="flex w-full flex-col space-y-1 h-60"
-                transition:slide|local={{ duration: 300, easing: cubicOut }}
-            >
-                <canvas bind:this={magnetometerChartElement}></canvas>
-            </div>
-        </div>
-    {/if}
-
     {#if $features.bmp}
         <div class="w-full overflow-x-auto">
             <div

docs/5_running.md

@@ -1,22 +1,5 @@
 # Running the spot
 
-### Start the Development Server
-
-Use the following commands to launch the development server with Vite, enabling instant updates:
-
-```sh
-cd app
-pnpm run dev
-```
-
-Please note that this example uses the `pnpm` package manager. For any other package manager such as `npm` or `yarn`, please adjust the command accordingly to run the dev command.
-
-[Download the pnpm package manager from pnpm.io](https://pnpm.io/installation)
-### Access the Vite server
-
-Access the frontend via the browser link provided by Vite.<br>
-Vite typically runs on port 5173, and can be accessed locally at [localhost:5173](http://localhost:5173/)
-
 > *Prerequsition*: You have successfully built, flashed, and configured your robot.
 
 Navigate to `/controller`

docs/6_developing.md

@@ -22,4 +22,13 @@ server: {
 
 > Changes require a restart of the development server.
 
+### Start the Development Server
 
+Use the following commands to launch the development server with Vite, enabling instant updates:
+
+```sh
+cd app
+pnpm run dev
+```
+
+Access the frontend via the provided browser link.

esp32/features.ini

@@ -13,8 +13,6 @@ build_flags =
   -D USE_HMC5883=0
   -D USE_BMP180=0
   -D USE_MPU6050=0
-  -D USE_ICM20948=1
-  -D USE_ICM20948_SPIMODE=1
   -D USE_WS2812=1
   -D USE_BNO055=0
   -D USE_USS=0

esp32/include/communication/comm_base.hpp

@@ -38,16 +38,16 @@ class CommAdapterBase {
         array.add(event);
         array.add(payload);
 
+        // TODO: Only send to subscribed
+
 #if USE_MSGPACK
         std::string bin;
         serializeMsgPack(doc, bin);
-        xSemaphoreGive(mutex_);
-        send(reinterpret_cast<const uint8_t *>(bin.data()), bin.size(), -1);
+        send(reinterpret_cast<const uint8_t *>(bin.data()), bin.size(), -1); // TODO: Make CID dynamic
 #else
         String out;
         serializeJson(doc, out);
-        xSemaphoreGive(mutex_);
-        send(out.c_str(), -1);
+        send(out.c_str(), cid);
 #endif
     }
 

esp32/include/communication/websocket_adapter.h

@@ -10,7 +10,7 @@
 
 #include <communication/comm_base.hpp>
 
-class Websocket : public CommAdapterBase {
+class Websocket : CommAdapterBase {
   public:
     Websocket(PsychicHttpServer &server, const char *route = "/api/ws");
 

esp32/include/features.h

@@ -12,7 +12,7 @@
 #define USE_CAMERA 0
 #endif
 
-// ESP32 IMU off by default
+// ESP32 IMU on by default
 #ifndef USE_MPU6050
 #define USE_MPU6050 0
 #endif
@@ -22,14 +22,6 @@
 #define USE_BNO055 1
 #endif
 
-// ESP32 IMU off by default
-#ifndef USE_ICM20948
-#define USE_ICM20948 0
-#endif
-#ifndef USE_ICM20948_SPIMODE // I2C on by default
-#define USE_ICM20948_SPIMODE 0
-#endif
-
 // ESP32 magnetometer on by default
 #ifndef USE_HMC5883
 #define USE_HMC5883 0

esp32/include/peripherals/barometer.h

@@ -36,33 +36,37 @@ struct BarometerMsg : public SensorMessageBase {
     friend void toJson(JsonVariant v, BarometerMsg const& a) { a.toJson(v); }
 };
 
-class Barometer : public SensorBase<BarometerMsg> {
+class Barometer {
   public:
-    bool initialize(void* _) override {
-        _msg.success = _bmp.begin();
-        return _msg.success;
+    bool initialize() {
+        bmp_success = _bmp.begin();
+        return bmp_success;
     }
 
-    bool update() override {
-        if (!_msg.success) return false;
-        _bmp.getTemperature(&_msg.temperature);
+    bool readBarometer() {
+        if (!bmp_success) return false;
+        _bmp.getTemperature(&temperature);
         sensors_event_t event;
         _bmp.getEvent(&event);
-        _msg.pressure = event.pressure;
-        _msg.altitude = _bmp.pressureToAltitude(seaLevelPressure, _msg.pressure);
+        pressure = event.pressure;
+        altitude = _bmp.pressureToAltitude(seaLevelPressure, pressure);
         return true;
     }
 
-    float getPressure() { return _msg.pressure; }
+    float getPressure() { return pressure; }
 
-    float getAltitude() { return _msg.altitude; }
+    float getAltitude() { return altitude; }
 
-    float getTemperature() { return _msg.temperature; }
+    float getTemperature() { return temperature; }
 
-    bool active() { return _msg.success; }
+    bool active() { return bmp_success; }
 
   private:
     Adafruit_BMP085_Unified _bmp {10085};
+    bool bmp_success {false};
+    float pressure {0};
+    float altitude {0};
+    float temperature {0};
 
     const float seaLevelPressure = SENSORS_PRESSURE_SEALEVELHPA;
 };

esp32/include/peripherals/imu.h

@@ -6,10 +6,6 @@
 #include <ArduinoJson.h>
 #include <utils/math_utils.h>
 
-#if FT_ENABLED(USE_ICM20948)
-#include "ICM_20948.h"
-#endif
-
 #if FT_ENABLED(USE_MPU6050)
 #include <MPU6050_6Axis_MotionApps612.h>
 #endif
@@ -48,112 +44,46 @@ struct IMUAnglesMsg : public SensorMessageBase {
 
 class IMU : public SensorBase<IMUAnglesMsg> {
   public:
-    bool initialize(void* _arg = nullptr) override {
+    bool initialize() override {
 #if FT_ENABLED(USE_MPU6050)
         _imu.initialize();
         _msg.success = _imu.testConnection();
-        if (!_msg.success) {
-            ESP_LOGE("IMU", "MPU6050 connection test failed");
-            return false;
-        }
+        if (!_msg.success) return false;
         devStatus = _imu.dmpInitialize();
         if (devStatus == 0) {
-            _imu.setXGyroOffset(0);
-            _imu.setYGyroOffset(0);
-            _imu.setZGyroOffset(0);
-            _imu.setXAccelOffset(0);
-            _imu.setYAccelOffset(0);
-            _imu.setZAccelOffset(0);
-
+            _imu.setDMPEnabled(false);
+            _imu.setDMPConfig1(0x03);
+            _imu.setDMPEnabled(true);
             _imu.setI2CMasterModeEnabled(false);
             _imu.setI2CBypassEnabled(true);
             _imu.setSleepEnabled(false);
-            _imu.setRate(1);
-            _imu.resetFIFO();
-            _imu.setDMPEnabled(true);
-
-            ESP_LOGI("IMU", "MPU6050 DMP initialized successfully");
         } else {
-            ESP_LOGE("IMU", "DMP initialization failed (code %d)", devStatus);
-            _msg.success = false;
+            return false;
         }
 #endif
 #if FT_ENABLED(USE_BNO055)
         _msg.success = _imu.begin();
         if (!_msg.success) {
-            ESP_LOGE("IMU", "BNO055 connection test failed");
             return false;
         }
         _imu.setExtCrystalUse(true);
 #endif
-#if FT_ENABLED(USE_ICM20948)
-    #if FT_ENABLED(USE_ICM20948_SPIMODE) > 0
-        #define ICM_20948_USE_DMP // TODO: Move to features.ini
-        SPI_PORT.begin(SPI_SCK, SPI_MISO, SPI_MOSI, -1); // TODO: Move to global spi start
-        _imu = (ICM_20948_SPI*)_arg;
-        #ifndef ICM20948_ALIVE
-        #define ICM20948_ALIVE
-        _imu->begin(ICM20948_SPI_CS, SPI_PORT); ESP_LOGI("IMU", "Beginning ICM20948 in SPI mode");
-        _imu->initializeDMP();
-        #endif
-    #else
-        _imu = (ICM_20948_I2C*)_arg;
-        #ifndef ICM20948_ALIVE
-        #define ICM20948_ALIVE
-        _imu->begin(Wire, 1, 0xFF); ESP_LOGI("IMU", "Beginning ICM20948 in I2C mode");
-        #endif
-        
-    #endif
-    if (_imu->status != ICM_20948_Stat_Ok){ ESP_LOGW("IMU", "Failed to start ICM20948: begin failed"); return false; }
-    
-    _imu->setSampleMode((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), ICM_20948_Sample_Mode_Continuous);
-    if (_imu->status != ICM_20948_Stat_Ok){ ESP_LOGW("IMU", "Failed to start ICM20948: set sample failed"); return false; }
-    
-    ICM_20948_fss_t myFSS;
-    myFSS.a = gpm2;
-    myFSS.g = dps250;
-    _imu->setFullScale((ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), myFSS);
-    if (_imu->status != ICM_20948_Stat_Ok){ ESP_LOGW("IMU", "Failed to start ICM20948: set full scale failed"); return false; }
-    // TODO: Setup low pass filter config
-    _msg.success = true;
-#endif
-        return _msg.success;
+        return true;
     }
 
     bool update() override {
+        if (!_msg.success) return false;
 #if FT_ENABLED(USE_MPU6050)
-        uint16_t fifoCount = _imu.getFIFOCount();
-        uint8_t intStatus = _imu.getIntStatus();
-
-        if (intStatus & 0x10) {
-            _imu.resetFIFO();
-            ESP_LOGW("IMU", "FIFO overflow, resetting");
-            return false;
-        }
-
-        if (_imu.dmpGetCurrentFIFOPacket(fifoBuffer)) {
-            _imu.dmpGetQuaternion(&q, fifoBuffer);
-            _imu.dmpGetGravity(&gravity, &q);
-            _imu.dmpGetYawPitchRoll(_msg.rpy, &q, &gravity);
-            return true;
+        if (_imu.dmpPacketAvailable()) {
+            if (_imu.dmpGetCurrentFIFOPacket(fifoBuffer)) {
+                _imu.dmpGetQuaternion(&q, fifoBuffer);
+                _imu.dmpGetGravity(&gravity, &q);
+                _imu.dmpGetYawPitchRoll(_msg.rpy, &q, &gravity);
+                return true;
+            }
         }
         return false;
 #endif
-#if FT_ENABLED(USE_ICM20948)
-          
-        #ifndef ICM20948_GET_AGMT_UPDATED_ONCE_PER_LOOP
-        #define ICM20948_GET_AGMT_UPDATED_ONCE_PER_LOOP
-        if (_imu->dataReady())
-        {
-            _imu->getAGMT();
-        } else {
-            return false;
-        }
-        #endif
-        _msg.rpy[0] = _imu->accX();
-        _msg.rpy[1] = _imu->accY();
-        _msg.rpy[2] = _imu->accZ();
-#endif
 #if FT_ENABLED(USE_BNO055)
         sensors_event_t event;
         _imu.getEvent(&event);
@@ -175,7 +105,7 @@ class IMU : public SensorBase<IMUAnglesMsg> {
   private:
 #if FT_ENABLED(USE_MPU6050)
     MPU6050 _imu;
-    uint8_t devStatus {0};
+    uint8_t devStatus {false};
     Quaternion q;
     uint8_t fifoBuffer[64];
     VectorFloat gravity;
@@ -183,12 +113,4 @@ class IMU : public SensorBase<IMUAnglesMsg> {
 #if FT_ENABLED(USE_BNO055)
     Adafruit_BNO055 _imu {55, 0x29};
 #endif
-#if FT_ENABLED(USE_ICM20948)
-    #if FT_ENABLED(USE_ICM20948_SPIMODE) > 0
-        ICM_20948_SPI* _imu;
-    #else
-        //#define WIRE_PORT Wire 
-        ICM_20948_I2C* _imu;
-    #endif
-#endif
 };

esp32/include/peripherals/magnetometer.h

@@ -11,7 +11,6 @@
 
 #include <peripherals/sensor.hpp>
 
-
 struct MagnetometerMsg : public SensorMessageBase {
     float rpy[3] {0, 0, 0};
     float heading {-1};
@@ -39,87 +38,37 @@ struct MagnetometerMsg : public SensorMessageBase {
 
 class Magnetometer : public SensorBase<MagnetometerMsg> {
   public:
-    bool initialize(void* _arg) override {
-        #if FT_ENABLED(USE_ICM20948)
-            #if FT_ENABLED(USE_ICM20948_SPIMODE) > 0
-                SPI_PORT.begin(SPI_SCK, SPI_MISO, SPI_MOSI, -1);
-                _mag = (ICM_20948_SPI*)_arg;
-                #ifndef ICM20948_ALIVE
-                #define ICM20948_ALIVE
-                _imu->begin(ICM20948_SPI_CS, SPI_PORT); ESP_LOGI("Magnetometer", "Beginning ICM20948 in SPI mode");
-                #endif
-            #else
-                _mag = (ICM_20948_I2C*)_arg;
-                #ifndef ICM20948_ALIVE
-                #define ICM20948_ALIVE
-                if (!_mag->isConnected()) { _mag->begin(Wire, 1, 0xFF); ESP_LOGI("Magnetometer", "Beginning ICM20948 in I2C mode"); }
-                #endif
-                
-            #endif
-            if (_mag->status != ICM_20948_Stat_Ok){ return false; }
-            
-            _mag->startupMagnetometer();
-            if (_mag->status != ICM_20948_Stat_Ok){ return false; }
-            _msg.success = true;
-        #elif FT_ENABLED(USE_HMC5883)
-            _msg.success = _mag.begin();
-        #endif
-        return _msg.success;
+    bool initialize() {
+        msg.success = _mag.begin();
+        return msg.success;
     }
 
-    bool update() override {
-        if (!_msg.success) return false;
-        #if FT_ENABLED(USE_ICM20948)
-        #ifndef ICM20948_GET_AGMT_UPDATED_ONCE_PER_LOOP
-        #define ICM20948_GET_AGMT_UPDATED_ONCE_PER_LOOP
-        if (_imu->dataReady())
-        { 
-            _imu->getAGMT();
-        } else {
-            return false;
-        }
-        #endif
-        _msg.rpy[0] = _mag->magX();
-        _msg.rpy[1] = _mag->magY();
-        _msg.rpy[2] = _mag->magZ();
-
-        #elif FT_ENABLED(USE_HMC5883)
-            sensors_event_t event;
-            bool updated = _mag.getEvent(&event);
-            if (!updated) return false;
-            _msg.rpy[0] = event.magnetic.x;
-            _msg.rpy[1] = event.magnetic.y;
-            _msg.rpy[2] = event.magnetic.z;
-        #endif
-        _msg.heading = atan2(_msg.rpy[1], _msg.rpy[0]); // atan2(y, x)
-        _msg.heading += declinationAngle;
-        if (_msg.heading < 0) _msg.heading += 2 * PI;
-        if (_msg.heading > 2 * PI) _msg.heading -= 2 * PI;
-        _msg.heading *= 180 / M_PI;
+    bool update() {
+        if (!msg.success) return false;
+        sensors_event_t event;
+        bool updated = _mag.getEvent(&event);
+        if (!updated) return false;
+        msg.rpy[0] = event.magnetic.x;
+        msg.rpy[1] = event.magnetic.y;
+        msg.rpy[2] = event.magnetic.z;
+        msg.heading = atan2(event.magnetic.y, event.magnetic.x);
+        msg.heading += declinationAngle;
+        if (msg.heading < 0) msg.heading += 2 * PI;
+        if (msg.heading > 2 * PI) msg.heading -= 2 * PI;
+        msg.heading *= 180 / M_PI;
         return true;
     }
 
-    float getMagX() { return _msg.rpy[0]; }
+    float getMagX() { return msg.rpy[0]; }
 
-    float getMagY() { return _msg.rpy[1]; }
+    float getMagY() { return msg.rpy[1]; }
 
-    float getMagZ() { return _msg.rpy[2]; }
+    float getMagZ() { return msg.rpy[2]; }
 
-    float getHeading() { return _msg.heading; }
+    float getHeading() { return msg.heading; }
 
   private:
-    
-    #if FT_ENABLED(USE_ICM20948)
-        #if FT_ENABLED(USE_ICM20948_SPIMODE) > 0
-            #define SPI_PORT SPI // TODO in periphearals_seetings.h
-            #define CS_PIN 2 
-            ICM_20948_SPI* _mag;
-        #else
-            //#define WIRE_PORT Wire 
-            ICM_20948_I2C* _mag;
-        #endif
-    #elif FT_ENABLED(USE_HMC5883)
-        Adafruit_HMC5883_Unified _mag {12345};
-    #endif
+    Adafruit_HMC5883_Unified _mag {12345};
+    MagnetometerMsg msg;
     const float declinationAngle = 0.22;
 };

esp32/include/peripherals/peripherals.h

@@ -87,10 +87,10 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
 
     JsonDocument doc;
     char message[MAX_ESP_IMU_SIZE];
-#if FT_ENABLED(USE_MPU6050 || USE_BNO055 || USE_ICM20948)
+#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
     IMU _imu;
 #endif
-#if FT_ENABLED(USE_HMC5883) || FT_ENABLED(USE_ICM20948)
+#if FT_ENABLED(USE_HMC5883)
     Magnetometer _mag;
 #endif
 #if FT_ENABLED(USE_BMP180)

esp32/include/peripherals/sensor.hpp

@@ -17,7 +17,7 @@ class SensorBase {
   public:
     SensorBase() {}
 
-    virtual bool initialize(void* _arg) = 0;
+    virtual bool initialize() = 0;
 
     virtual bool update() = 0;
 

esp32/include/settings/peripherals_settings.h

@@ -15,7 +15,7 @@
 #define SCL_PIN SCL
 #endif
 #ifndef I2C_FREQUENCY
-#define I2C_FREQUENCY 400000UL
+#define I2C_FREQUENCY 1000000UL
 #endif
 
 class PinConfig {

esp32/include/system_service.h

@@ -3,7 +3,7 @@
 #include <ESPmDNS.h>
 #include <PsychicHttp.h>
 #include <WiFi.h>
-#include <communication/websocket_adapter.h>
+// #include <communication/websocket_adapter.h>
 #include <filesystem.h>
 #include <global.h>
 #include "esp_timer.h"
@@ -25,7 +25,7 @@ void sleep();
 void status(JsonObject &root);
 void metrics(JsonObject &root);
 
-void emitMetrics(Websocket &socket);
+void emitMetrics();
 
 const char *resetReason(esp_reset_reason_t reason);
 } // namespace system_service

esp32/include/utils/timing.h

@@ -39,49 +39,3 @@
         name##_count = 0;                                                    \
         last_time = esp_timer_get_time() / 1000;                             \
     }
-
-#define CALLS_PER_SECOND_TIMED_START_TICK(name, function)                        \
-    static uint64_t name##_##function##_start = 0;                           \
-    static uint64_t name##_##function##_total_time = 0;                      \
-    static uint64_t name##_##function##_call_count = 0;                      \
-    name##_##function##_start = esp_timer_get_time();
-
-#define CALLS_PER_SECOND_TIMED_END_TICK(name, function)                          \
-    name##_##function##_total_time += (esp_timer_get_time() - name##_##function##_start); \
-    name##_##function##_call_count++;
-
-#define CALLS_PER_SECOND_TIMED_CALL(name, function, call)                        \
-    static uint64_t name##_##function##_total_time = 0;                          \
-    static uint64_t name##_##function##_call_count = 0;                          \
-    do {                                                                         \
-        uint64_t name##_##function##_start = esp_timer_get_time();               \
-        call;                                                                    \
-        name##_##function##_total_time += (esp_timer_get_time() - name##_##function##_start); \
-        name##_##function##_call_count++;                                        \
-    } while (0)
-
-#define CALLS_PER_SECOND_TIMED_FUNC_PRINT(name, function)                        \
-    if (name##_##function##_call_count > 0) {                                    \
-        uint64_t avg = name##_##function##_total_time / name##_##function##_call_count; \
-        if (avg < 1000) {                                                        \
-            ESP_LOGI("Timing", "  %s: %llu us (avg over %llu calls)",            \
-                     #function, avg, name##_##function##_call_count);            \
-        } else {                                                                 \
-            ESP_LOGI("Timing", "  %s: %llu ms (avg over %llu calls)",            \
-                     #function, avg / 1000, name##_##function##_call_count);     \
-        }                                                                        \
-        name##_##function##_total_time = 0;                                      \
-        name##_##function##_call_count = 0;                                      \
-    }
-
-#define CALLS_PER_SECOND_TIMED(name, ...)                                        \
-    do {                                                                         \
-        static uint64_t name##_last_print = 0;                                   \
-        uint64_t name##_current_time = esp_timer_get_time() / 1000;              \
-        if (name##_current_time - name##_last_print >= 1000) {                   \
-            ESP_LOGI("Timing", "=== %s Average Timings ===", #name);             \
-            __VA_ARGS__                                                          \
-            name##_last_print = name##_current_time;                             \
-        }                                                                        \
-    } while (0)
-

esp32/scripts/pre_build.py

@@ -2,6 +2,6 @@ from pathlib import Path
 
 Import("env")
 
-filesystem_dir = env["PROJECT_DIR"] + "/esp32/data"
+filesystem_dir = env["PROJECT_DIR"] + "/data"
 
 Path(filesystem_dir).mkdir(exist_ok=True)

esp32/src/features.cpp

@@ -12,14 +12,11 @@ void printFeatureConfiguration() {
     ESP_LOGI("Features", "USE_MOTION: %s", USE_MOTION ? "enabled" : "disabled");
 
     // Sensors
-    ESP_LOGI("Features", "USE_ICM20948: %s", USE_ICM20948 ? "enabled" : "disabled");
     ESP_LOGI("Features", "USE_BNO055: %s", USE_BNO055 ? "enabled" : "disabled");
     ESP_LOGI("Features", "USE_MPU6050: %s", USE_MPU6050 ? "enabled" : "disabled");
     ESP_LOGI("Features", "USE_HMC5883: %s", USE_HMC5883 ? "enabled" : "disabled");
     ESP_LOGI("Features", "USE_BMP180: %s", USE_BMP180 ? "enabled" : "disabled");
     ESP_LOGI("Features", "USE_USS: %s", USE_USS ? "enabled" : "disabled");
-    ESP_LOGI("Features", "USE_PAJ7620U2: %s", USE_PAJ7620U2 ? "enabled" : "disabled");
-    
 
     // Peripherals
     ESP_LOGI("Features", "USE_PCA9685: %s", USE_PCA9685 ? "enabled" : "disabled");
@@ -34,8 +31,8 @@ void printFeatureConfiguration() {
 
 void features(JsonObject &root) {
     root["camera"] = USE_CAMERA ? true : false;
-    root["imu"] = (USE_MPU6050 || USE_BNO055 || USE_ICM20948) ? true : false;
-    root["mag"] = (USE_HMC5883 || USE_BNO055 || USE_ICM20948) ? true : false;
+    root["imu"] = (USE_MPU6050 || USE_BNO055) ? true : false;
+    root["mag"] = (USE_HMC5883 || USE_BNO055) ? true : false;
     root["bmp"] = USE_BMP180 ? true : false;
     root["sonar"] = USE_USS ? true : false;
     root["servo"] = USE_PCA9685 ? true : false;

esp32/src/main.cpp

@@ -124,20 +124,15 @@ void IRAM_ATTR SpotControlLoopEntry(void *) {
     motionService.begin();
 
     for (;;) {
-        CALLS_PER_SECOND_TIMED_CALL(SpotControlLoopEntry, peripherals_update, peripherals.update());
-        CALLS_PER_SECOND_TIMED_CALL(SpotControlLoopEntry, motionService_update, motionService.update(&peripherals));
-        CALLS_PER_SECOND_TIMED_CALL(SpotControlLoopEntry, servoController_setAngles, servoController.setAngles(motionService.getAngles()));
-        CALLS_PER_SECOND_TIMED_CALL(SpotControlLoopEntry, servoController_update, servoController.update());
+        CALLS_PER_SECOND(SpotControlLoopEntry);
+        peripherals.update();
+        motionService.update(&peripherals);
+        servoController.setAngles(motionService.getAngles());
+        servoController.update();
 #if FT_ENABLED(USE_WS2812)
         ledService.loop();
 #endif
-        // CALLS_PER_SECOND_TIMED(SpotControlLoopEntry,
-        //     CALLS_PER_SECOND_TIMED_FUNC_PRINT(SpotControlLoopEntry, peripherals_update)
-        //     CALLS_PER_SECOND_TIMED_FUNC_PRINT(SpotControlLoopEntry, motionService_update)
-        //     CALLS_PER_SECOND_TIMED_FUNC_PRINT(SpotControlLoopEntry, servoController_setAngles)
-        //     CALLS_PER_SECOND_TIMED_FUNC_PRINT(SpotControlLoopEntry, servoController_update)
-        // );
-        // vTaskDelayUntil(&xLastWakeTime, xFrequency);
+        vTaskDelayUntil(&xLastWakeTime, xFrequency);
     }
 }
 
@@ -149,10 +144,6 @@ void IRAM_ATTR serviceLoopEntry(void *) {
     MDNS.setInstanceName(APP_NAME);
     apService.begin();
 
-#if FT_ENABLED(USE_CAMERA)
-    cameraService.begin();
-#endif
-
     setupServer();
 
     socket.begin();
@@ -162,13 +153,7 @@ void IRAM_ATTR serviceLoopEntry(void *) {
     for (;;) {
         wifiService.loop();
         apService.loop();
-        EXECUTE_EVERY_N_MS(2000, system_service::emitMetrics(socket));
-        EXECUTE_EVERY_N_MS(500, {
-            JsonDocument doc;
-            JsonVariant results = doc.to<JsonVariant>();
-            peripherals.getIMUResult(results);
-            socket.emit(EVENT_IMU, results);
-        });
+        EXECUTE_EVERY_N_MS(2000, system_service::emitMetrics());
 
         vTaskDelay(100 / portTICK_PERIOD_MS);
     }

esp32/src/peripherals/peripherals.cpp

@@ -15,40 +15,18 @@ void Peripherals::begin() {
 
     updatePins();
 
-#if FT_ENABLED(USE_ICM20948)
-    #if FT_ENABLED(USE_ICM20948_SPIMODE) > 0
-        ICM_20948_SPI* icm20948 = new ICM_20948_SPI;
-    #else
-        ICM_20948_I2C* icm20948 = new ICM_20948_I2C;
-    #endif
-#endif
-
-// --- IMU ---
 #if FT_ENABLED(USE_MPU6050 || USE_BNO055)
-    if (!_imu.initialize(nullptr)) ESP_LOGE("Peripherals", "IMU initialize failed");
-#elif FT_ENABLED(USE_ICM20948)
-    if (!_imu.initialize(icm20948)) ESP_LOGE("Peripherals", "IMU initialize failed (ICM20948)");
+    if (!_imu.initialize()) ESP_LOGE("IMUService", "IMU initialize failed");
 #endif
-
-// --- MAGNETOMETER ---
 #if FT_ENABLED(USE_HMC5883)
-    if (!_mag.initialize(nullptr)) ESP_LOGE("Peripherals", "MAG initialize failed");
-#elif FT_ENABLED(USE_ICM20948)
-    if (!_mag.initialize(icm20948)) ESP_LOGE("Peripherals", "MAG initialize failed (ICM20948)");
+    if (!_mag.initialize()) ESP_LOGE("IMUService", "MAG initialize failed");
 #endif
-
-// --- BMP ---
 #if FT_ENABLED(USE_BMP180)
-    if (!_bmp.initialize(nullptr)) ESP_LOGE("Peripherals", "BMP initialize failed");
+    if (!_bmp.initialize()) ESP_LOGE("IMUService", "BMP initialize failed");
 #endif
-
-
-// --- GESTURE ---
 #if FT_ENABLED(USE_PAJ7620U2)
-    if (!_gesture.initialize(nullptr)) ESP_LOGE("Peripherals", "Gesture initialize failed");
+    if (!_gesture.initialize()) ESP_LOGE("IMUService", "Gesture initialize failed");
 #endif
-
-// --- SONAR ---
 #if FT_ENABLED(USE_USS)
     _left_sonar = std::make_unique<NewPing>(USS_LEFT_PIN, USS_LEFT_PIN, MAX_DISTANCE);
     _right_sonar = std::make_unique<NewPing>(USS_RIGHT_PIN, USS_RIGHT_PIN, MAX_DISTANCE);
@@ -56,25 +34,11 @@ void Peripherals::begin() {
 };
 
 void Peripherals::update() {
-    bool res = true;
-    CALLS_PER_SECOND_TIMED_CALL(Peripherals_update, read_imu, res = readImu());
-    #ifdef FT_ENABLED(USE_ICM20948)
-        // IF ICM_20948 fails to get IMU, it means that mag also does not have new data
-        CALLS_PER_SECOND_TIMED_CALL(Peripherals_update, read_mag, if (res) { res = readMag(); } );
-    #else
-        CALLS_PER_SECOND_TIMED_CALL(Peripherals_update, read_mag, res = readMag());
-    #endif
-    CALLS_PER_SECOND_TIMED_CALL(Peripherals_update, read_gesture, EXECUTE_EVERY_N_MS(100, { readGesture(); }) );
-    CALLS_PER_SECOND_TIMED_CALL(Peripherals_update, read_bmp, EXECUTE_EVERY_N_MS(500, { readBMP(); }) );
-    CALLS_PER_SECOND_TIMED_CALL(Peripherals_update, read_sonar, EXECUTE_EVERY_N_MS(500, { readSonar(); }) );
-
-    CALLS_PER_SECOND_TIMED(Peripherals_update,
-        CALLS_PER_SECOND_TIMED_FUNC_PRINT(Peripherals_update, read_imu)
-        CALLS_PER_SECOND_TIMED_FUNC_PRINT(Peripherals_update, read_mag)
-        CALLS_PER_SECOND_TIMED_FUNC_PRINT(Peripherals_update, read_gesture)
-        CALLS_PER_SECOND_TIMED_FUNC_PRINT(Peripherals_update, read_bmp)
-        CALLS_PER_SECOND_TIMED_FUNC_PRINT(Peripherals_update, read_sonar)
-    );
+    readImu();
+    readMag();
+    EXECUTE_EVERY_N_MS(100, { readGesture(); });
+    EXECUTE_EVERY_N_MS(500, { readBMP(); });
+    EXECUTE_EVERY_N_MS(500, { readSonar(); });
 }
 
 void Peripherals::updatePins() {
@@ -84,7 +48,6 @@ void Peripherals::updatePins() {
 
     if (state().sda != -1 && state().scl != -1) {
         Wire.begin(state().sda, state().scl, state().frequency);
-        ESP_LOGI("Peripherals", "Starting Wire with SDA=%d, SCL=%d, FREQ=%d", state().sda, state().scl, state().frequency);
         i2c_active = true;
     }
 }
@@ -116,7 +79,7 @@ void Peripherals::scanI2C(uint8_t lower, uint8_t higher) {
 /* IMU FUNCTIONS */
 bool Peripherals::readImu() {
     bool updated = false;
-#if FT_ENABLED(USE_MPU6050 || USE_BNO055 || USE_ICM20948)
+#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
     beginTransaction();
     updated = _imu.update();
     endTransaction();
@@ -126,7 +89,7 @@ bool Peripherals::readImu() {
 
 bool Peripherals::readMag() {
     bool updated = false;
-#if FT_ENABLED(USE_HMC5883 || USE_ICM20948)
+#if FT_ENABLED(USE_HMC5883)
     beginTransaction();
     updated = _mag.update();
     endTransaction();
@@ -138,7 +101,7 @@ bool Peripherals::readBMP() {
     bool updated = false;
 #if FT_ENABLED(USE_BMP180)
     beginTransaction();
-    updated = _bmp.update();
+    updated = _bmp.readBarometer();
     endTransaction();
 #endif
     return updated;
@@ -164,7 +127,7 @@ void Peripherals::readSonar() {
 
 float Peripherals::angleX() {
     return
-#if FT_ENABLED(USE_MPU6050 || USE_BNO055 || USE_ICM20948)
+#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
         _imu.getAngleX();
 #else
         0;
@@ -173,7 +136,7 @@ float Peripherals::angleX() {
 
 float Peripherals::angleY() {
     return
-#if FT_ENABLED(USE_MPU6050 || USE_BNO055 || USE_ICM20948)
+#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
         _imu.getAngleY();
 #else
         0;
@@ -182,7 +145,7 @@ float Peripherals::angleY() {
 
 float Peripherals::angleZ() {
     return
-#if FT_ENABLED(USE_MPU6050 || USE_BNO055 || USE_ICM20948)
+#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
         _imu.getAngleZ();
 #else
         0;
@@ -202,17 +165,14 @@ float Peripherals::leftDistance() { return _left_distance; }
 float Peripherals::rightDistance() { return _right_distance; }
 
 void Peripherals::getIMUResult(JsonVariant &root) {
-#if FT_ENABLED(USE_MPU6050 || USE_BNO055 || USE_ICM20948)
-    JsonVariant imu = root["imu"].to<JsonVariant>();
-    _imu.getResults(imu);
+#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
+    _imu.getResults(root);
 #endif
-#if FT_ENABLED(USE_HMC5883 || USE_ICM20948) // TODO:
-    JsonVariant mag = root["mag"].to<JsonVariant>();
-    _mag.getResults(mag);
+#if FT_ENABLED(USE_HMC5883)
+    _mag.getResults(root);
 #endif
 #if FT_ENABLED(USE_BMP180)
-    JsonVariant bmp = root["bmp"].to<JsonVariant>();
-    _bmp.getResults(bmp);
+    _bmp.getResults(root);
 #endif
 }
 

esp32/src/system_service.cpp

@@ -121,14 +121,13 @@ void metrics(JsonObject &root) {
     root["core_temp"] = temperatureRead();
 }
 
-void emitMetrics(Websocket &socket) {
-    if (!socket.hasSubscribers(EVENT_ANALYTICS)) return;
-
-    JsonDocument doc;
-    JsonObject root = doc.to<JsonObject>();
-    system_service::metrics(root);
-    JsonVariant data = doc.as<JsonVariant>();
-    socket.emit(EVENT_ANALYTICS, data);
+void emitMetrics() {
+    // if (!socket.hasSubscribers(EVENT_ANALYTICS)) return;
+    // analyticsDoc.clear();
+    // JsonObject root = analyticsDoc.to<JsonObject>();
+    // system_service::metrics(root);
+    // JsonVariant data = analyticsDoc.as<JsonVariant>();
+    // socket.emit(EVENT_ANALYTICS, data);
 }
 
 const char *resetReason(esp_reset_reason_t reason) {

platformio.ini

@@ -55,10 +55,6 @@ build_flags =
 	-D USS_RIGHT_PIN=14
 	-D SDA_PIN=47
 	-D SCL_PIN=21
-    -D SPI_SCK=41
-    -D SPI_MISO=19
-    -D SPI_MOSI=20
-    -D ICM20948_SPI_CS=2 # Only needed if ICM20948 is in SPI mode
 
 [env:seeed-xiao-esp32s3]
 platform = espressif32
@@ -94,7 +90,6 @@ build_flags =
 	${factory_settings.build_flags}
     ${features.build_flags}
 	${build_settings.build_flags}
-    -D SPI_PORT=SPI # Define which spi port to use for external components
 	-D CORE_DEBUG_LEVEL=4
 	-D register=
 	-std=gnu++2a
@@ -121,7 +116,6 @@ lib_deps =
 	adafruit/Adafruit Unified Sensor@^1.1.14
     adafruit/Adafruit BNO055@^1.6.4
 	FastLED@3.5.0
-    sparkfun/SparkFun 9DoF IMU Breakout - ICM 20948 - Arduino Library@^1.3.2
 extra_scripts = 
 	pre:esp32/scripts/pre_build.py
     pre:esp32/scripts/build_app.py

simulation/play.py

@@ -15,11 +15,7 @@ from src.controllers import Controller, GUIController, WebSocketController
 
 class SpotMicroSimulation:
     def __init__(
-        self,
-        controller: Controller,
-        env: Optional[QuadrupedEnv] = None,
-        terrain_type: TerrainType = TerrainType.FLAT,
-        dt: float = 1.0 / 240,
+        self, controller: Controller, env: Optional[QuadrupedEnv] = None, terrain_type: TerrainType = TerrainType.FLAT
     ):
         print("Initializing Spot Micro simulation...")
         try:
@@ -27,7 +23,7 @@ class SpotMicroSimulation:
                 self.env = env
                 print("Using existing environment")
             else:
-                self.env = QuadrupedEnv(terrain_type=terrain_type, dt=dt)
+                self.env = QuadrupedEnv(terrain_type=terrain_type)
                 print("Environment created successfully")
 
             print(f"Robot ID: {self.env.robot.robot_id}")
@@ -82,7 +78,7 @@ class SpotMicroSimulation:
         )
 
         self.gait = GaitController(standby)
-        self.dt = dt
+        self.dt = 1.0 / 240
 
     def step(self):
         self.controller.update(self.body_state, self.gait_state, self.dt)
@@ -91,24 +87,10 @@ class SpotMicroSimulation:
         joints = self.kinematics.inverse_kinematics(self.body_state)
         joints = joints * self.joint_directions
 
-        obs, _, done, truncated, _ = self.env.step(joints)
-
-        self._print_mpu6050_data(obs)
+        _, _, done, truncated, _ = self.env.step(joints)
 
         return joints, done, truncated
 
-    def _print_mpu6050_data(self, obs):
-        accel = obs[0:3]
-        gyro = obs[3:6]
-        heading = obs[6]
-        altitude = obs[7]
-
-        print(
-            f"MPU6050: Accel({accel[0]:8.3f}, {accel[1]:8.3f}, {accel[2]:8.3f}) "
-            f"Gyro({gyro[0]:8.3f}, {gyro[1]:8.3f}, {gyro[2]:8.3f}) "
-            f"Mag({heading:8.3f}) Baro({altitude:8.3f})"
-        )
-
     def run_sync(self):
         try:
             while self.controller.is_running():

simulation/src/envs/quadruped_env.py

@@ -38,31 +38,22 @@ class QuadrupedRobot:
         return [p.getJointInfo(self.robot_id, idx)[1].decode("utf-8") for idx in self.movable_joint_indices]
 
     def get_observation(self):
-        pos_w, quat_wb = p.getBasePositionAndOrientation(self.robot_id)
-        v_w, w_w = p.getBaseVelocity(self.robot_id)
-
-        R = np.array(p.getMatrixFromQuaternion(quat_wb), dtype=np.float32).reshape(3, 3)
-
-        if hasattr(self, "prev_velocity") and self.prev_velocity is not None:
-            dt = 1.0 / 240.0
-            accel_world = (v_w - self.prev_velocity) / dt
-        else:
-            accel_world = np.array([0.0, 0.0, 0.0])
-
-        accel_body = R.T @ np.asarray(accel_world, dtype=np.float32)
-        gravity_body = R.T @ np.array([0, 0, -9.81], dtype=np.float32)
-        accel_body += gravity_body
-
-        gyro_body = np.degrees(R.T @ np.asarray(w_w, dtype=np.float32))
-
-        euler = p.getEulerFromQuaternion(quat_wb)
-        heading = np.degrees(euler[2])
-
-        altitude = np.array([pos_w[2]], dtype=np.float32)
-
-        self.prev_velocity = np.array(v_w)
-
-        return np.concatenate([accel_body, gyro_body, [heading], altitude]).astype(np.float32)
+        position, orientation = p.getBasePositionAndOrientation(self.robot_id)
+        orientation = p.getEulerFromQuaternion(orientation)
+        velocity, angular_velocity = p.getBaseVelocity(self.robot_id)
+        joint_states = p.getJointStates(self.robot_id, self.movable_joint_indices)
+        joint_positions = [state[0] for state in joint_states]
+        joint_velocities = [state[1] for state in joint_states]
+        return np.concatenate(
+            [
+                position,
+                orientation,
+                velocity,
+                angular_velocity,
+                joint_positions,
+                joint_velocities,
+            ]
+        ).astype(np.float32)
 
     def apply_action(self, action):
         for i, position in enumerate(action):
@@ -87,7 +78,6 @@ class QuadrupedEnv(gym.Env):
         target_velocity: float = 0.5,
         max_steps: int = 1000,
         distance_limit: float = 10.0,
-        dt: float = 1.0 / 240,
     ):
         super().__init__()
         if render_mode == "human":
@@ -95,7 +85,7 @@ class QuadrupedEnv(gym.Env):
         else:
             p.connect(p.DIRECT)
 
-        self.observation_space = gym.spaces.Box(low=-np.inf, high=np.inf, shape=(8,), dtype=np.float32)
+        self.observation_space = gym.spaces.Box(low=-np.inf, high=np.inf, shape=(36,), dtype=np.float32)
         self.action_space = gym.spaces.Box(low=-1, high=1, shape=(12,), dtype=np.float32)
         p.setAdditionalSearchPath(pybullet_data.getDataPath())
 
@@ -103,10 +93,9 @@ class QuadrupedEnv(gym.Env):
         self.render_mode = render_mode
         self.target_velocity = target_velocity
         self.max_steps = max_steps
-        self.prev_accel = np.zeros(3)
-        self.estimated_velocity = np.zeros(3)
         self.current_step = 0
-        self.dt = dt
+
+        self.prev_velocity = None
 
         self._setup_world()
         if render_mode == "human":
@@ -118,7 +107,7 @@ class QuadrupedEnv(gym.Env):
         self.robot = QuadrupedRobot("src/resources/spot.urdf")
         self._load_terrain(self.terrain_type)
         p.setGravity(0, 0, -9.8)
-        p.setTimeStep(self.dt)
+        p.setTimeStep(1 / 240)
         if self.render_mode == "human":
             self.gui = GUI(self.robot.robot_id)
         else:
@@ -168,7 +157,7 @@ class QuadrupedEnv(gym.Env):
 
         obs = self.robot.get_observation()
         reward = self.calculate_reward(obs)
-        done = self.is_done()
+        done = self.is_done(obs)
         truncated = self.current_step >= self.max_steps
 
         return obs, reward, done, truncated, {}
@@ -178,42 +167,48 @@ class QuadrupedEnv(gym.Env):
         # p.disconnect()
 
     def calculate_reward(self, obs):
-        accel = obs[0:3]
-        gyro = obs[3:6]
-        heading = obs[6]
-        altitude = obs[7]
+        position = obs[:3]
+        orientation = obs[3:6]
+        velocity = obs[6:9]
+        angular_velocity = obs[9:12]
 
-        self.estimated_velocity = self.estimated_velocity + self.prev_accel * self.dt
-
-        self.prev_accel = accel.copy()
-
-        forward_velocity = self.estimated_velocity[0]
+        forward_velocity = velocity[0]
         velocity_reward = -abs(forward_velocity - self.target_velocity)
 
-        height_penalty = -abs(altitude - 0.3) * 0.5
+        height_penalty = -abs(position[2] - 0.3) * 0.5
 
-        orientation_penalty = -(abs(gyro[0]) + abs(gyro[1])) * 0.1
+        roll, pitch, yaw = orientation
+        orientation_penalty = -(abs(roll) + abs(pitch)) * 1.0
 
-        angular_penalty = -np.sum(np.square(gyro)) * 0.01
+        angular_penalty = -np.sum(np.square(angular_velocity)) * 0.05
 
-        lateral_acc_penalty = -abs(accel[1]) * 0.01
+        sideways_velocity_penalty = -abs(velocity[1]) * 0.3
 
-        vertical_acc_penalty = -abs(accel[2] + 9.81) * 0.01
+        if self.prev_velocity is not None:
+            dt = 1.0 / 240.0
+            acceleration = (velocity - self.prev_velocity) / dt
+            lateral_acc_penalty = -abs(acceleration[1]) * 0.01
+            vertical_acc_penalty = -abs(acceleration[2]) * 0.01
+        else:
+            lateral_acc_penalty = 0
+            vertical_acc_penalty = 0
+
+        self.prev_velocity = velocity.copy()
 
         total_reward = (
             velocity_reward
             + height_penalty
             + orientation_penalty
             + angular_penalty
+            + sideways_velocity_penalty
             + lateral_acc_penalty
             + vertical_acc_penalty
         )
         return total_reward
 
-    def is_done(self):
-        position, orientation = p.getBasePositionAndOrientation(self.robot.robot_id)
-        orientation = p.getEulerFromQuaternion(orientation)
-
+    def is_done(self, obs):
+        position = obs[:3]
+        orientation = obs[3:6]
         return self._is_fallen(orientation) or self._is_distance_limit_exceeded(position)
 
     def _is_distance_limit_exceeded(self, position):
@@ -221,4 +216,9 @@ class QuadrupedEnv(gym.Env):
         return distance > self._distance_limit
 
     def _is_fallen(self, orientation):
+        # orientation = self.spot.GetBaseOrientation()
+        # rot_mat = self._pybullet_client.getMatrixFromQuaternion(orientation)
+        # local_up = rot_mat[6:]
+        # pos = self.spot.GetBasePosition()
+        # return (np.dot(np.asarray([0, 0, 1]), np.asarray(local_up)) < 0.55)
         return abs(orientation[0]) > 0.85 or abs(orientation[1]) > 0.85