✨ Adds skill system

app/src/lib/components/SkillPanel.svelte

@@ -0,0 +1,156 @@
+<script lang="ts">
+    import { skill } from '$lib/stores'
+    import { onMount, onDestroy } from 'svelte'
+
+    let targetX = $state(0.5)
+    let targetZ = $state(0)
+    let targetYaw = $state(0)
+    let speed = $state(0.5)
+
+    const status = skill.status
+    const isActive = skill.isActive
+    const progress = skill.progress
+
+    const presets = [
+        { name: 'Forward 0.5m', x: 0.5, z: 0, yaw: 0 },
+        { name: 'Forward 1m', x: 1, z: 0, yaw: 0 },
+        { name: 'Back 0.5m', x: -0.5, z: 0, yaw: 0 },
+        { name: 'Left 0.5m', x: 0, z: 0.5, yaw: 0 },
+        { name: 'Right 0.5m', x: 0, z: -0.5, yaw: 0 },
+        { name: 'Turn Left 90°', x: 0, z: 0, yaw: 1.57 },
+        { name: 'Turn Right 90°', x: 0, z: 0, yaw: -1.57 },
+        { name: 'Turn 180°', x: 0, z: 0, yaw: 3.14 }
+    ]
+
+    onMount(() => skill.init())
+    onDestroy(() => skill.destroy())
+
+    function executeSkill() {
+        skill.walk(targetX, targetZ, targetYaw, speed)
+    }
+
+    function runPreset(preset: (typeof presets)[0]) {
+        skill.walk(preset.x, preset.z, preset.yaw, speed)
+    }
+
+    function formatMeters(val: number): string {
+        return val.toFixed(3) + 'm'
+    }
+
+    function formatDegrees(rad: number): string {
+        return ((rad * 180) / Math.PI).toFixed(1) + '°'
+    }
+</script>
+
+<div class="card bg-base-200 shadow-xl">
+    <div class="card-body p-4">
+        <h2 class="card-title text-sm flex justify-between">
+            Skill Control
+            <span class="badge" class:badge-success={$isActive} class:badge-ghost={!$isActive}>
+                {$isActive ? 'Active' : 'Idle'}
+            </span>
+        </h2>
+
+        <div class="grid grid-cols-2 gap-2 text-xs mb-2">
+            <div class="stat bg-base-300 rounded-lg p-2">
+                <div class="stat-title text-xs">Position</div>
+                <div class="stat-value text-sm">
+                    {formatMeters($status.x)}, {formatMeters($status.z)}
+                </div>
+                <div class="stat-desc">Yaw: {formatDegrees($status.yaw)}</div>
+            </div>
+            <div class="stat bg-base-300 rounded-lg p-2">
+                <div class="stat-title text-xs">Distance</div>
+                <div class="stat-value text-sm">{formatMeters($status.distance)}</div>
+                <div class="stat-desc">Total traveled</div>
+            </div>
+        </div>
+
+        {#if $isActive}
+            <div class="mb-2">
+                <div class="flex justify-between text-xs mb-1">
+                    <span>Progress</span>
+                    <span>{($progress * 100).toFixed(0)}%</span>
+                </div>
+                <progress class="progress progress-primary w-full" value={$progress} max="1"></progress>
+                <div class="text-xs text-base-content/60 mt-1">
+                    Target: ({$status.skill_target_x.toFixed(2)}, {$status.skill_target_z.toFixed(2)}, {formatDegrees(
+                        $status.skill_target_yaw
+                    )})
+                </div>
+            </div>
+        {/if}
+
+        <div class="divider my-1 text-xs">Presets</div>
+
+        <div class="grid grid-cols-4 gap-1">
+            {#each presets as preset}
+                <button class="btn btn-xs btn-outline" onclick={() => runPreset(preset)}>
+                    {preset.name}
+                </button>
+            {/each}
+        </div>
+
+        <div class="divider my-1 text-xs">Custom</div>
+
+        <div class="grid grid-cols-3 gap-2">
+            <div class="form-control">
+                <label class="label py-0" for="skill-x">
+                    <span class="label-text text-xs">X (m)</span>
+                </label>
+                <input
+                    id="skill-x"
+                    type="number"
+                    step="0.1"
+                    bind:value={targetX}
+                    class="input input-bordered input-xs w-full"
+                />
+            </div>
+            <div class="form-control">
+                <label class="label py-0" for="skill-z">
+                    <span class="label-text text-xs">Z (m)</span>
+                </label>
+                <input
+                    id="skill-z"
+                    type="number"
+                    step="0.1"
+                    bind:value={targetZ}
+                    class="input input-bordered input-xs w-full"
+                />
+            </div>
+            <div class="form-control">
+                <label class="label py-0" for="skill-yaw">
+                    <span class="label-text text-xs">Yaw (rad)</span>
+                </label>
+                <input
+                    id="skill-yaw"
+                    type="number"
+                    step="0.1"
+                    bind:value={targetYaw}
+                    class="input input-bordered input-xs w-full"
+                />
+            </div>
+        </div>
+
+        <div class="form-control mt-2">
+            <label class="label py-0" for="skill-speed">
+                <span class="label-text text-xs">Speed: {speed.toFixed(2)}</span>
+            </label>
+            <input id="skill-speed" type="range" min="0.1" max="1" step="0.05" bind:value={speed} class="range range-xs range-primary" />
+        </div>
+
+        <div class="card-actions justify-between mt-2">
+            <div class="flex gap-1">
+                <button class="btn btn-xs btn-ghost" onclick={() => skill.resetPosition()}>Reset Pos</button>
+            </div>
+            <div class="flex gap-1">
+                <button class="btn btn-xs btn-error" onclick={() => skill.stop()} disabled={!$isActive}>
+                    Stop
+                </button>
+                <button class="btn btn-xs btn-primary" onclick={executeSkill} disabled={$isActive}>
+                    Execute
+                </button>
+            </div>
+        </div>
+    </div>
+</div>

app/src/lib/components/Visualization.svelte

@@ -1,16 +1,13 @@
 <script lang="ts">
     import { onDestroy, onMount } from 'svelte'
     import {
-        BufferGeometry,
-        Line,
-        LineBasicMaterial,
         Mesh,
         MeshBasicMaterial,
         type Object3D,
         SphereGeometry,
         Vector3,
-        type NormalBufferAttributes,
+        type Object3DEventMap,
-        type Object3DEventMap
+        Color
     } from 'three'
     import {
         ModesEnum,
@@ -26,12 +23,7 @@
         walkGait,
         walkGaitToMode
     } from '$lib/stores'
-    import {
+    import { populateModelCache, throttler, getToeWorldPositions } from '$lib/utilities'
-        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'
@@ -42,14 +34,20 @@
     import { get } from 'svelte/store'
 
     interface Props {
-        sky?: boolean
+        defaultColor?: string | null
         orbit?: boolean
         panel?: boolean
         debug?: boolean
         ground?: boolean
     }
 
-    let { sky = true, orbit = false, panel = true, debug = false, ground = true }: Props = $props()
+    let {
+        defaultColor = '#0091ff',
+        orbit = false,
+        panel = true,
+        debug = false,
+        ground = true
+    }: Props = $props()
 
     let sceneManager = $state(new SceneBuilder())
     let canvas: HTMLCanvasElement
@@ -76,13 +74,14 @@
     let lastTick = performance.now()
 
     const dir = [1, -1, -1, -1, -1, -1, 1, -1, -1, -1, -1, -1]
+    const THREEJS_SCALE = 10
 
     let body_state = {
         omega: 0,
         phi: 0,
         psi: 0,
         xm: 0,
-        ym: 0.5,
+        ym: 0.15,
         zm: 0,
         feet: kinematic.getDefaultFeetPos(),
         cumulative_x: 0,
@@ -106,9 +105,9 @@
         phi: 0,
         psi: 0,
         xm: 0,
-        ym: 0.7,
+        ym: 0.15,
         zm: 0,
-        Background: 'black'
+        Background: defaultColor
     }
 
     onMount(async () => {
@@ -153,7 +152,7 @@
         visibility.add(settings, 'Trace points', 1, 1000, 1)
         visibility.add(settings, 'Target position')
         visibility.add(settings, 'Smooth motion')
-        visibility.addColor(settings, 'Background')
+        visibility.addColor(settings, 'Background').onChange(setSceneBackground).listen()
     }
 
     const updateKinematicPosition = () => {
@@ -167,6 +166,8 @@
         ])
     }
 
+    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 +198,13 @@
         sceneManager.scene.add(target)
 
         if (debug) {
-            sceneManager.addDragControl((angles: Record<string, number>) => {
+            sceneManager.addDragControl(angles => {
                 Object.entries(angles).forEach(([name, angle]) => {
                     updateAngles(name, angle)
                 })
             })
         }
-        if (sky) sceneManager.addSky()
+        if (defaultColor) setSceneBackground(settings['Background'] || defaultColor)
     }
 
     const calculate_kinematics = () => {
@@ -239,8 +240,16 @@
         const rotatedXm = settings.xm * cosYaw - settings.zm * sinYaw
         const rotatedZm = settings.xm * sinYaw + settings.zm * cosYaw
 
-        robot.position.x = smooth(robot.position.x, -rotatedZm - body_state.cumulative_z * 1.2, 0.1)
+        robot.position.x = smooth(
-        robot.position.z = smooth(robot.position.z, -rotatedXm - body_state.cumulative_x * 1.2, 0.1)
+            robot.position.x,
+            (-rotatedZm - body_state.cumulative_z) * THREEJS_SCALE,
+            0.1
+        )
+        robot.position.z = smooth(
+            robot.position.z,
+            (-rotatedXm - body_state.cumulative_x) * THREEJS_SCALE,
+            0.1
+        )
 
         const pitch = degToRad(settings.psi - 90) + body_state.cumulative_pitch
         const roll = degToRad(settings.omega) + body_state.cumulative_roll
@@ -275,7 +284,6 @@
             s: controlData[5],
             s1: controlData[6]
         }
-        body_state.ym = data.h
 
         let planner = planners[get(mode)]
         const delta = performance.now() - lastTick
@@ -296,8 +304,8 @@
         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.xm = robot.position.z / THREEJS_SCALE
-        settings.zm = -robot.position.x * 100
+        settings.zm = -robot.position.x / THREEJS_SCALE
     }
 
     const updateTargetPosition = () => {

app/src/lib/gait.ts

@@ -26,21 +26,34 @@ export abstract class GaitState {
 
     protected dt = 0.02
     protected body_state!: body_state_t
+
+    protected get kinematic() {
+        return get(currentKinematic)
+    }
+
     protected gait_state: gait_state_t = {
-        step_height: 0.4,
+        step_height: 0,
         step_x: 0,
         step_z: 0,
         step_angle: 0,
         step_velocity: 1,
-        step_depth: 0.002
+        step_depth: 0
     }
 
     public get default_feet_pos() {
-        return get(currentKinematic).getDefaultFeetPos()
+        return this.kinematic.getDefaultFeetPos()
     }
 
     protected get default_height() {
-        return 0.5
+        return this.kinematic.default_body_height
+    }
+
+    protected get default_step_depth() {
+        return this.kinematic.default_step_depth
+    }
+
+    protected get default_step_height() {
+        return this.kinematic.default_step_height
     }
 
     begin() {
@@ -67,16 +80,15 @@ export abstract class GaitState {
     }
 
     map_command(command: ControllerCommand) {
-        const newCommand = {
+        const kin = this.kinematic
-            step_height: 0.4 + (command.s1 + 1) / 2,
+        this.gait_state = {
-            step_x: command.ly,
+            step_height: command.s1 * kin.max_step_height,
-            step_z: -command.lx,
+            step_x: command.ly * kin.max_step_length,
+            step_z: -command.lx * kin.max_step_length,
             step_velocity: command.s,
             step_angle: command.rx,
-            step_depth: 0.002
+            step_depth: kin.default_step_depth
         }
-
-        this.gait_state = newCommand
     }
 }
 
@@ -92,14 +104,13 @@ export class IdleState extends GaitState {
 export class CalibrationState extends GaitState {
     protected name = 'Calibration'
 
-    // eslint-disable-next-line @typescript-eslint/no-unused-vars
     step(body_state: body_state_t, _command: ControllerCommand) {
         super.step(body_state, _command)
         body_state.omega = 0
         body_state.phi = 0
         body_state.psi = 0
         body_state.xm = 0
-        body_state.ym = this.default_height * 10
+        body_state.ym = this.kinematic.max_body_height
         body_state.zm = 0
         body_state.feet = this.default_feet_pos
         return body_state
@@ -109,14 +120,13 @@ export class CalibrationState extends GaitState {
 export class RestState extends GaitState {
     protected name = 'Rest'
 
-    // eslint-disable-next-line @typescript-eslint/no-unused-vars
     step(body_state: body_state_t, _command: ControllerCommand) {
         super.step(body_state, _command)
         body_state.omega = 0
         body_state.phi = 0
         body_state.psi = 0
         body_state.xm = 0
-        body_state.ym = this.default_height / 2
+        body_state.ym = this.kinematic.min_body_height
         body_state.zm = 0
         body_state.feet = this.default_feet_pos
         return body_state
@@ -128,11 +138,13 @@ export class StandState extends GaitState {
 
     step(body_state: body_state_t, command: ControllerCommand) {
         super.step(body_state, command)
+        const kin = this.kinematic
         body_state.omega = 0
-        body_state.phi = command.rx * 10 * (Math.PI / 2)
+        body_state.ym = kin.min_body_height + command.h * kin.body_height_range
-        body_state.psi = command.ry * 10 * (Math.PI / 2)
+        body_state.psi = command.ry * kin.max_pitch
-        body_state.xm = command.ly / 4
+        body_state.phi = command.rx * kin.max_roll
-        body_state.zm = command.lx / 4
+        body_state.xm = command.ly * kin.max_body_shift_x
+        body_state.zm = command.lx * kin.max_body_shift_z
         body_state.feet = this.default_feet_pos
         return body_state
     }
@@ -191,6 +203,8 @@ export class BezierState extends GaitState {
 
     step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
         super.step(body_state, command, dt)
+        const kin = this.kinematic
+        this.body_state.ym = kin.min_body_height + command.h * kin.body_height_range
         this.step_length = Math.sqrt(this.gait_state.step_x ** 2 + this.gait_state.step_z ** 2)
         if (this.gait_state.step_x < 0) this.step_length = -this.step_length
         this.update_phase()
@@ -339,7 +353,8 @@ export class BezierState extends GaitState {
         let angle = Math.atan2(this.gait_state.step_z, this.step_length) * 2
         const delta_pos = controller(length, angle, ...args, phase)
 
-        length = this.gait_state.step_angle * 2
+        const kin = this.kinematic
+        length = this.gait_state.step_angle * kin.max_step_length
         angle = yawArc(this.default_feet_pos[index], this.body_state.feet[index])
 
         const delta_rot = controller(length, angle, ...args, phase)

app/src/lib/kinematic.ts

@@ -50,7 +50,22 @@ export default class Kinematic {
 
     DEG2RAD = DEG2RAD
 
+    max_roll: number
+    max_pitch: number
+    max_body_shift_x: number
+    max_body_shift_z: number
+    max_leg_reach: number
+    min_body_height: number
+    max_body_height: number
+    body_height_range: number
+    max_step_length: number
+    max_step_height: number
+    default_step_depth: number
+    default_body_height: number
+    default_step_height: number
+
     mountOffsets: number[][]
+    default_feet_positions: number[][]
 
     invMountRot = [
         [0, 0, -1],
@@ -66,18 +81,34 @@ export default class Kinematic {
         this.L = params.L
         this.W = params.W
 
+        this.max_roll = 15 * (Math.PI / 2)
+        this.max_pitch = 15 * (Math.PI / 2)
+        this.max_body_shift_x = this.W / 3
+        this.max_body_shift_z = this.W / 3
+        this.max_leg_reach = this.femur + this.tibia - this.coxa_offset
+        this.min_body_height = this.max_leg_reach * 0.45
+        this.max_body_height = this.max_leg_reach * 1
+        this.body_height_range = this.max_body_height - this.min_body_height
+        this.max_step_length = this.max_leg_reach * 0.8
+        this.max_step_height = this.max_leg_reach / 2
+        this.default_step_depth = 0.002
+        this.default_body_height = this.min_body_height + this.body_height_range / 2
+        this.default_step_height = this.default_body_height / 2
+
         this.mountOffsets = [
             [this.L / 2, 0, this.W / 2],
             [this.L / 2, 0, -this.W / 2],
             [-this.L / 2, 0, this.W / 2],
             [-this.L / 2, 0, -this.W / 2]
         ]
+
+        this.default_feet_positions = this.mountOffsets.map((offset, i) => {
+            return [offset[0], 0, offset[2] + (i % 2 === 1 ? -this.coxa : this.coxa)]
+        })
     }
 
     getDefaultFeetPos(): number[][] {
-        return this.mountOffsets.map((offset, i) => {
+        return this.default_feet_positions.map(pos => [...pos])
-            return [offset[0], -1, offset[2] + (i % 2 === 1 ? -this.coxa : this.coxa)]
-        })
     }
 
     calcIK(p: body_state_t): number[] {

app/src/lib/sceneBuilder.ts

@@ -17,18 +17,16 @@ import {
     MeshPhongMaterial,
     EquirectangularReflectionMapping,
     ACESFilmicToneMapping,
-    MathUtils,
     Group,
     MeshBasicMaterial,
-    RepeatWrapping
+    RepeatWrapping,
+    type Object3D
 } 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()
 
@@ -65,8 +63,6 @@ export default class SceneBuilder {
     private fog!: FogExp2
     private isLoaded: boolean = false
     public isDragging: boolean = false
-    highlightMaterial: MeshPhongMaterial
-    sky!: Sky
     transformControl: TransformControls
     public modelGroup!: Group
 
@@ -89,34 +85,6 @@ 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)
@@ -334,7 +302,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/application.ts

@@ -4,6 +4,7 @@ import { get, type Writable } from 'svelte/store'
 import Visualization from '$lib/components/Visualization.svelte'
 import Stream from '$lib/components/Stream.svelte'
 import ChartWidget from '$lib/components/widget/ChartWidget.svelte'
+import SkillPanel from '$lib/components/SkillPanel.svelte'
 
 export interface WidgetConfig {
     id: string | number
@@ -25,7 +26,8 @@ export const isWidgetConfig = (
 export const WidgetComponents = {
     Visualization,
     Stream,
-    ChartWidget
+    ChartWidget,
+    SkillPanel
 }
 
 interface View {
@@ -59,6 +61,16 @@ const defaultViews: View[] = [
                 { id: 2, component: 'Visualization', props: { debug: true } }
             ]
         }
+    },
+    {
+        name: 'Skills',
+        content: {
+            id: 'root',
+            widgets: [
+                { id: 1, component: 'Visualization', props: { debug: true } },
+                { id: 2, component: 'SkillPanel' }
+            ]
+        }
     }
 ]
 

app/src/lib/stores/featureFlags.ts

@@ -29,24 +29,24 @@ export const variants = {
         model: `${base}spot_micro.urdf.xacro`,
         stl: `${base}stl.zip`,
         kinematics: {
-            coxa: 60.5 / 100,
+            coxa: 0.0605,
-            coxa_offset: 10 / 100,
+            coxa_offset: 0.01,
-            femur: 111.7 / 100,
+            femur: 0.1112,
-            tibia: 118.5 / 100,
+            tibia: 0.1185,
-            L: 207.5 / 100,
+            L: 0.2075,
-            W: 78 / 100
+            W: 0.078
         }
     },
     SPOTMICRO_YERTLE: {
         model: `${base}yertle.URDF`,
         stl: `${base}URDF.zip`,
         kinematics: {
-            coxa: 35 / 100,
+            coxa: 0.035,
-            coxa_offset: 0 / 100,
+            coxa_offset: 0.0,
-            femur: 130 / 100,
+            femur: 0.13,
-            tibia: 130 / 100,
+            tibia: 0.13,
-            L: 240 / 100,
+            L: 0.24,
-            W: 78 / 100
+            W: 0.078
         }
     }
 }

app/src/lib/stores/imu.ts

@@ -1,5 +1,5 @@
 import { writable } from 'svelte/store'
-import type { IMU } from '$lib/types/models'
+import type { IMUMsg } from '$lib/types/models'
 
 const maxIMUData = 100
 
@@ -14,11 +14,24 @@ export const imu = (() => {
         bmp_temp: [] as number[]
     })
 
-    const addData = (content: IMU) => {
+    const addData = (content: IMUMsg) => {
         update(data => {
-            ;(Object.keys(content) as (keyof IMU)[]).forEach(key => {
+            if (content.imu && content.imu[4]) {
-                data[key] = [...data[key], content[key]].slice(-maxIMUData)
+                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)
+            }
+
             return data
         })
     }

app/src/lib/stores/index.ts

@@ -7,3 +7,4 @@ export * from './telemetry'
 export * from './analytics'
 export * from './featureFlags'
 export * from './location-store'
+export * from './skill'

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

@@ -50,5 +50,5 @@ export const input: Writable<ControllerInput> = writable({
     right: { x: 0, y: 0 },
     height: 0.5,
     speed: 0.5,
-    s1: 0.05
+    s1: 0.5
 })

app/src/lib/stores/skill.ts

@@ -0,0 +1,85 @@
+import { writable, derived } from 'svelte/store'
+import { socket } from './socket'
+import { MessageTopic, type SkillStatus, type SkillCommand } from '$lib/types/models'
+
+const defaultStatus: SkillStatus = {
+    x: 0,
+    y: 0,
+    z: 0,
+    yaw: 0,
+    distance: 0,
+    skill_active: false,
+    skill_target_x: 0,
+    skill_target_z: 0,
+    skill_target_yaw: 0,
+    skill_traveled_x: 0,
+    skill_traveled_z: 0,
+    skill_rotated: 0,
+    skill_progress: 0,
+    skill_complete: false
+}
+
+function createSkillStore() {
+    const status = writable<SkillStatus>(defaultStatus)
+    const history = writable<SkillCommand[]>([])
+    let unsubscribe: (() => void) | null = null
+
+    function init() {
+        if (unsubscribe) return
+        unsubscribe = socket.on<SkillStatus>(MessageTopic.skillStatus, data => {
+            status.set(data)
+            if (data.event === 'complete') {
+                history.update(h => [...h.slice(-9), getCurrentTarget(data)])
+            }
+        })
+    }
+
+    function getCurrentTarget(s: SkillStatus): SkillCommand {
+        return { x: s.skill_target_x, z: s.skill_target_z, yaw: s.skill_target_yaw }
+    }
+
+    function execute(cmd: SkillCommand) {
+        socket.sendEvent(MessageTopic.skill, cmd)
+    }
+
+    function walk(x: number, z: number = 0, yaw: number = 0, speed: number = 0.5) {
+        execute({ x, z, yaw, speed })
+    }
+
+    function turn(yaw: number, speed: number = 0.5) {
+        execute({ x: 0, z: 0, yaw, speed })
+    }
+
+    function stop() {
+        socket.sendEvent(MessageTopic.displacement, { action: 'clear' })
+    }
+
+    function resetPosition() {
+        socket.sendEvent(MessageTopic.displacement, { action: 'reset' })
+    }
+
+    function destroy() {
+        if (unsubscribe) {
+            unsubscribe()
+            unsubscribe = null
+        }
+    }
+
+    return {
+        status,
+        history,
+        init,
+        destroy,
+        execute,
+        walk,
+        turn,
+        stop,
+        resetPosition,
+        isActive: derived(status, $s => $s.skill_active),
+        progress: derived(status, $s => $s.skill_progress),
+        position: derived(status, $s => ({ x: $s.x, y: $s.y, z: $s.z, yaw: $s.yaw }))
+    }
+}
+
+export const skill = createSkillStore()
+

app/src/lib/types/models.ts

@@ -1,5 +1,6 @@
 export enum MessageTopic {
     imu = 'imu',
+    imuCalibrate = 'imuCalibrate',
     mode = 'mode',
     input = 'input',
     analytics = 'analytics',
@@ -13,7 +14,10 @@ export enum MessageTopic {
     servoPWM = 'servoPWM',
     WiFiSettings = 'WiFiSettings',
     sonar = 'sonar',
-    rssi = 'rssi'
+    rssi = 'rssi',
+    skill = 'skill',
+    skillStatus = 'skill_status',
+    displacement = 'displacement'
 }
 
 export type vector = { x: number; y: number }
@@ -154,6 +158,16 @@ 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 type IMUCalibrationResult = {
+    success: boolean
+}
+
 export interface I2CDevice {
     address: number
     part_number: string
@@ -237,3 +251,28 @@ export interface MDNSStatus {
     services: MDNSService[]
     global_txt_records: MDNSTxtRecord[]
 }
+
+export interface SkillCommand {
+    x: number
+    z: number
+    yaw: number
+    speed?: number
+}
+
+export interface SkillStatus {
+    x: number
+    y: number
+    z: number
+    yaw: number
+    distance: number
+    skill_active: boolean
+    skill_target_x: number
+    skill_target_z: number
+    skill_target_yaw: number
+    skill_traveled_x: number
+    skill_traveled_z: number
+    skill_rotated: number
+    skill_progress: number
+    skill_complete: boolean
+    event?: string
+}

app/src/lib/utilities/index.ts

@@ -4,6 +4,5 @@ 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/model-utilities.ts

@@ -1,4 +1,4 @@
-import { Color, LoaderUtils, Vector3 } from 'three'
+import { Color, Vector3 } from 'three'
 import URDFLoader, { type URDFRobot } from 'urdf-loader'
 import { XacroLoader } from 'xacro-parser'
 import { Result } from '$lib/utilities'

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

@@ -1,86 +0,0 @@
-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 sky={false} orbit panel={false} ground={false} />
+            <Visualization defaultColor={null} 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/connection/Connection.svelte

@@ -5,7 +5,7 @@
 
     const update = () => {
         const ws = $apiLocation ? $apiLocation : window.location.host
-        socket.init(`ws://${ws}/api/ws/events`)
+        socket.init(`ws://${ws}/api/ws`)
     }
 </script>
 

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 IMU } from '$lib/types/models'
+    import { MessageTopic, type IMUMsg, type IMUCalibrationResult } from '$lib/types/models'
     import { useFeatureFlags } from '$lib/stores/featureFlags'
     import { Rotate3d } from '$lib/components/icons'
 
@@ -14,6 +14,8 @@
 
     const features = useFeatureFlags()
     let intervalId: ReturnType<typeof setInterval> | number
+    let isCalibrating = $state(false)
+    let calibrationResult = $state<IMUCalibrationResult | null>(null)
 
     let angleChartElement: HTMLCanvasElement
     let tempChartElement: HTMLCanvasElement
@@ -201,19 +203,31 @@
     }
 
     onMount(() => {
-        socket.on(MessageTopic.imu, (data: IMU) => {
+        socket.on(MessageTopic.imu, (data: IMUMsg) => {
             console.log(data)
             imu.addData(data)
         })
 
+        socket.on(MessageTopic.imuCalibrate, (data: IMUCalibrationResult) => {
+            isCalibrating = false
+            calibrationResult = data
+        })
+
         initializeCharts()
         intervalId = setInterval(updateData, 200)
     })
 
     onDestroy(() => {
         socket.off(MessageTopic.imu)
+        socket.off(MessageTopic.imuCalibrate)
         clearInterval(intervalId)
     })
+
+    function startCalibration() {
+        isCalibrating = true
+        calibrationResult = null
+        socket.sendEvent(MessageTopic.imuCalibrate, {})
+    }
 </script>
 
 <SettingsCard collapsible={false}>
@@ -224,6 +238,26 @@
         <span>IMU</span>
     {/snippet}
 
+    <div class="flex items-center gap-2 mb-4">
+        <button
+            class="btn btn-sm btn-primary"
+            onclick={startCalibration}
+            disabled={isCalibrating || !$features.imu}
+        >
+            {#if isCalibrating}
+                <span class="loading loading-spinner loading-xs"></span>
+                Calibrating...
+            {:else}
+                Calibrate IMU
+            {/if}
+        </button>
+        {#if calibrationResult}
+            <span class="badge" class:badge-success={calibrationResult.success} class:badge-error={!calibrationResult.success}>
+                {calibrationResult.success ? 'Calibrated' : 'Failed'}
+            </span>
+        {/if}
+    </div>
+
     {#if $features.imu}
         <div class="w-full overflow-x-auto">
             <div

docs/5_running.md

@@ -1,5 +1,22 @@
 # 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,13 +22,4 @@ 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/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);
-        send(reinterpret_cast<const uint8_t *>(bin.data()), bin.size(), -1); // TODO: Make CID dynamic
+        xSemaphoreGive(mutex_);
+        send(reinterpret_cast<const uint8_t *>(bin.data()), bin.size(), -1);
 #else
         String out;
         serializeJson(doc, out);
-        send(out.c_str(), cid);
+        xSemaphoreGive(mutex_);
+        send(out.c_str(), -1);
 #endif
     }
 
@@ -55,8 +55,16 @@ class CommAdapterBase {
     void send(const char *data, int cid = -1) { send(reinterpret_cast<const uint8_t *>(data), strlen(data), cid); }
     virtual void send(const uint8_t *data, size_t len, int cid = -1) = 0;
 
-    void subscribe(const char *event, int cid = 0) { client_subscriptions[event].push_back(cid); }
+    void subscribe(const char *event, int cid = 0) {
-    void unsubscribe(const char *event, int cid = 0) { client_subscriptions[event].push_back(cid); }
+        xSemaphoreTake(mutex_, portMAX_DELAY);
+        client_subscriptions[event].push_back(cid);
+        xSemaphoreGive(mutex_);
+    }
+    void unsubscribe(const char *event, int cid = 0) {
+        xSemaphoreTake(mutex_, portMAX_DELAY);
+        client_subscriptions[event].remove(cid);
+        xSemaphoreGive(mutex_);
+    }
 
     void handleEventCallbacks(std::string event, JsonVariant &jsonObject, int originId) {
         for (auto &callback : event_callbacks[event]) {
@@ -102,35 +110,21 @@ class CommAdapterBase {
             }
             case message_type_t::PING: {
                 ESP_LOGI("Comm Base", "PING (cid=%d)", cid);
-#if USE_MSGPACK
+                ping(cid);
-                static const uint8_t pong[] = {0x91, 0x04};
-                send(pong, sizeof(pong), cid);
-#else
-                send("[4]", cid);
-#endif
                 break;
             }
+
             case message_type_t::PONG: ESP_LOGI("Comm Base", "PONG (cid=%d)", cid); break;
             default: ESP_LOGW("Comm Base", "Unknown message type: %d", static_cast<int>(type)); break;
         }
-
-        if (type == PONG) {
-            ESP_LOGV("EventSocket", "Pong");
-            return;
-        } else if (type == PING) {
-            ESP_LOGV("EventSocket", "Ping");
-            ping(cid);
-            return;
-        }
     }
 
     void ping(int cid) {
 #if USE_MSGPACK
-        const uint8_t out[] = {0x91, 0x04};
+        static const uint8_t pong[] = {0x91, 0x04};
-        send(out, sizeof(out), cid);
+        send(pong, sizeof(pong), cid);
 #else
-        const char *out = "[4]";
+        send("[4]", cid);
-        send(out, strlen(out), cid);
 #endif
     }
 

esp32/include/communication/websocket_adapter.h

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

esp32/include/features.h

@@ -42,6 +42,11 @@
 #define USE_PCA9685 1
 #endif
 
+// WS2812 LED strip off by default
+#ifndef USE_WS2812
+#define USE_WS2812 0
+#endif
+
 // ESP32 MDNS on by default
 #ifndef USE_MDNS
 #define USE_MDNS 1

esp32/include/filesystem.h

@@ -25,6 +25,7 @@ esp_err_t uploadFile(PsychicRequest *request, const std::string &filename, uint6
                      bool last);
 
 esp_err_t getFiles(PsychicRequest *request);
+esp_err_t getConfigFile(PsychicRequest *request);
 esp_err_t handleDelete(PsychicRequest *request, JsonVariant &json);
 esp_err_t handleEdit(PsychicRequest *request, JsonVariant &json);
 

esp32/include/kinematics.h

@@ -6,26 +6,26 @@
 class KinConfig {
   public:
 #if defined(SPOTMICRO_ESP32)
-    static constexpr float coxa = 60.5f / 100.0f;
+    static constexpr float coxa = 0.0605f;
-    static constexpr float coxa_offset = 10.0f / 100.0f;
+    static constexpr float coxa_offset = 0.010f;
-    static constexpr float femur = 111.2f / 100.0f;
+    static constexpr float femur = 0.1112f;
-    static constexpr float tibia = 118.5f / 100.0f;
+    static constexpr float tibia = 0.1185f;
-    static constexpr float L = 207.5f / 100.0f;
+    static constexpr float L = 0.2075f;
-    static constexpr float W = 78.0f / 100.0f;
+    static constexpr float W = 0.078f;
 #elif defined(SPOTMICRO_ESP32_MINI)
-    static constexpr float coxa = 35.0f / 100.0f;
+    static constexpr float coxa = 0.035f;
-    static constexpr float coxa_offset = 0.0f / 100.0f;
-    static constexpr float femur = 60.0f / 100.0f;
-    static constexpr float tibia = 60.0f / 100.0f;
-    static constexpr float L = 160.0f / 100.0f;
-    static constexpr float W = 80.0f / 100.0f;
-#elif defined(SPOTMICRO_YERTLE)
-    static constexpr float coxa = 35.0f / 100.0f;
     static constexpr float coxa_offset = 0.0f;
-    static constexpr float femur = 130.0f / 100.0f;
+    static constexpr float femur = 0.060f;
-    static constexpr float tibia = 130.0f / 100.0f;
+    static constexpr float tibia = 0.060f;
-    static constexpr float L = 240.0f / 100.0f;
+    static constexpr float L = 0.160f;
-    static constexpr float W = 78.0f / 100.0f;
+    static constexpr float W = 0.080f;
+#elif defined(SPOTMICRO_YERTLE)
+    static constexpr float coxa = 0.035f;
+    static constexpr float coxa_offset = 0.0f;
+    static constexpr float femur = 0.130f;
+    static constexpr float tibia = 0.130f;
+    static constexpr float L = 0.240f;
+    static constexpr float W = 0.078f;
 #endif
 
     static constexpr float mountOffsets[4][3] = {
@@ -38,9 +38,8 @@ class KinConfig {
         {mountOffsets[3][0], 0, mountOffsets[3][2] - coxa, 1},
     };
 
-    // Max constants
+    static constexpr float max_roll = 15 * DEG2RAD_F;
-    static constexpr float max_roll = 15 * (float)M_PI_2;
+    static constexpr float max_pitch = 15 * DEG2RAD_F;
-    static constexpr float max_pitch = 15 * (float)M_PI_2;
 
     static constexpr float max_body_shift_x = W / 3;
     static constexpr float max_body_shift_z = W / 3;
@@ -60,12 +59,85 @@ class KinConfig {
     static constexpr float default_step_height = default_body_height / 2;
 };
 
+struct displacement_state_t {
+    float x {0};
+    float y {0};
+    float z {0};
+    float roll {0};
+    float pitch {0};
+    float yaw {0};
+
+    void reset() { x = y = z = roll = pitch = yaw = 0; }
+    float distance() const { return std::sqrt(x * x + z * z); }
+};
+
+struct skill_target_t {
+    float target_x {0};
+    float target_z {0};
+    float target_yaw {0};
+
+    float traveled_x {0};
+    float traveled_z {0};
+    float rotated {0};
+
+    bool active {false};
+
+    void set(float x, float z, float yaw) {
+        target_x = x;
+        target_z = z;
+        target_yaw = yaw;
+        traveled_x = traveled_z = rotated = 0;
+        active = true;
+    }
+
+    void reset() {
+        target_x = target_z = target_yaw = 0;
+        traveled_x = traveled_z = rotated = 0;
+        active = false;
+    }
+
+    void accumulate(float dx, float dz, float dyaw) {
+        traveled_x += dx;
+        traveled_z += dz;
+        rotated += dyaw;
+    }
+
+    bool isComplete() const {
+        if (!active) return false;
+        bool x_ok = (target_x == 0) || (target_x > 0 ? traveled_x >= target_x : traveled_x <= target_x);
+        bool z_ok = (target_z == 0) || (target_z > 0 ? traveled_z >= target_z : traveled_z <= target_z);
+        bool yaw_ok = (target_yaw == 0) || (target_yaw > 0 ? rotated >= target_yaw : rotated <= target_yaw);
+        return x_ok && z_ok && yaw_ok;
+    }
+
+    float progress() const {
+        if (!active) return 0;
+        float total_target = std::fabs(target_x) + std::fabs(target_z) + std::fabs(target_yaw);
+        if (total_target == 0) return 1;
+
+        auto clampProgress = [](float traveled, float target) -> float {
+            if (target == 0) return 0;
+            float p = traveled / target;
+            return std::clamp(p, 0.0f, 1.0f) * std::fabs(target);
+        };
+
+        float total_progress = clampProgress(traveled_x, target_x) + clampProgress(traveled_z, target_z) +
+                               clampProgress(rotated, target_yaw);
+        return total_progress / total_target;
+    }
+};
+
 struct alignas(16) body_state_t {
     float omega {0}, phi {0}, psi {0}, xm {0}, ym {KinConfig::default_body_height}, zm {0};
     float feet[4][4];
 
+    displacement_state_t cumulative;
+    skill_target_t skill;
+
     void updateFeet(const float newFeet[4][4]) { COPY_2D_ARRAY_4x4(feet, newFeet); }
 
+    void resetDisplacement() { cumulative.reset(); }
+
     bool operator==(const body_state_t &other) const {
         if (!IS_ALMOST_EQUAL(omega, other.omega) || !IS_ALMOST_EQUAL(phi, other.phi) ||
             !IS_ALMOST_EQUAL(psi, other.psi) || !IS_ALMOST_EQUAL(xm, other.xm) || !IS_ALMOST_EQUAL(ym, other.ym) ||

esp32/include/motion.h

@@ -3,6 +3,7 @@
 
 #include <ArduinoJson.h>
 #include "esp_timer.h"
+#include <functional>
 
 #include <kinematics.h>
 #include <peripherals/gesture.h>
@@ -18,6 +19,8 @@
 
 enum class MOTION_STATE { DEACTIVATED, IDLE, CALIBRATION, REST, STAND, WALK };
 
+using SkillCompleteCallback = std::function<void()>;
+
 class MotionService {
   public:
     void begin();
@@ -30,6 +33,10 @@ class MotionService {
 
     void handleMode(JsonVariant &root, int originId);
 
+    void handleDisplacement(JsonVariant &root, int originId);
+
+    void handleSkill(JsonVariant &root, int originId);
+
     void setState(MotionState *newState);
 
     void handleGestures(const gesture_t ges);
@@ -42,6 +49,39 @@ class MotionService {
 
     inline bool isActive() { return state != nullptr; }
 
+    void resetDisplacement() { body_state.resetDisplacement(); }
+
+    void setSkillTarget(float x, float z, float yaw) { body_state.skill.set(x, z, yaw); }
+
+    void clearSkill() { body_state.skill.reset(); }
+
+    bool isSkillActive() const { return body_state.skill.active; }
+
+    bool isSkillComplete() const { return body_state.skill.isComplete(); }
+
+    const displacement_state_t &getDisplacement() const { return body_state.cumulative; }
+
+    const skill_target_t &getSkill() const { return body_state.skill; }
+
+    void getDisplacementResult(JsonVariant &root) const {
+        root["x"] = body_state.cumulative.x;
+        root["y"] = body_state.cumulative.y;
+        root["z"] = body_state.cumulative.z;
+        root["yaw"] = body_state.cumulative.yaw;
+        root["distance"] = body_state.cumulative.distance();
+        root["skill_active"] = body_state.skill.active;
+        root["skill_target_x"] = body_state.skill.target_x;
+        root["skill_target_z"] = body_state.skill.target_z;
+        root["skill_target_yaw"] = body_state.skill.target_yaw;
+        root["skill_traveled_x"] = body_state.skill.traveled_x;
+        root["skill_traveled_z"] = body_state.skill.traveled_z;
+        root["skill_rotated"] = body_state.skill.rotated;
+        root["skill_progress"] = body_state.skill.progress();
+        root["skill_complete"] = body_state.skill.isComplete();
+    }
+
+    void setSkillCompleteCallback(SkillCompleteCallback callback) { skillCompleteCallback = callback; }
+
   private:
     Kinematics kinematics;
 
@@ -63,6 +103,11 @@ class MotionService {
     float dir[12] = {1, -1, -1, -1, -1, -1, 1, -1, -1, -1, -1, -1};
 
     int64_t lastUpdate = esp_timer_get_time();
+
+    SkillCompleteCallback skillCompleteCallback = nullptr;
+    bool skillWasComplete = false;
+
+    void checkSkillComplete();
 };
 
 #endif

esp32/include/motion_states/walk_state.h

@@ -98,11 +98,33 @@ class WalkState : public MotionState {
 
         step_length = std::hypot(gait_state.step_x, gait_state.step_z);
         if (gait_state.step_x < 0.0f) step_length = -step_length;
+
+        const bool moving = !isZero(gait_state.step_x) || !isZero(gait_state.step_z) || !isZero(gait_state.step_angle);
+        updateDisplacement(body_state, dt, moving);
+
         updatePhase(dt);
         updateBodyPosition(body_state, dt);
         updateFeetPositions(body_state);
     }
 
+    void updateDisplacement(body_state_t &body_state, float dt, bool moving) {
+        if (!moving) return;
+
+        float dx_local = gait_state.step_x * gait_state.step_velocity * dt * speed_factor;
+        float dz_local = gait_state.step_z * gait_state.step_velocity * dt * speed_factor;
+        float dyaw = gait_state.step_angle * gait_state.step_velocity * dt * speed_factor;
+
+        if (body_state.skill.active) {
+            body_state.skill.accumulate(dx_local, dz_local, dyaw);
+        }
+
+        float cos_yaw = std::cos(body_state.cumulative.yaw);
+        float sin_yaw = std::sin(body_state.cumulative.yaw);
+        body_state.cumulative.x += dx_local * cos_yaw - dz_local * sin_yaw;
+        body_state.cumulative.z += dx_local * sin_yaw + dz_local * cos_yaw;
+        body_state.cumulative.yaw += dyaw;
+    }
+
   protected:
     void handleCommand(const CommandMsg &cmd) override {
         target_body_state.ym = KinConfig::min_body_height + cmd.h * KinConfig::body_height_range;

esp32/include/peripherals/barometer.h

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

@@ -48,39 +48,60 @@ class IMU : public SensorBase<IMUAnglesMsg> {
 #if FT_ENABLED(USE_MPU6050)
         _imu.initialize();
         _msg.success = _imu.testConnection();
-        if (!_msg.success) return false;
+        if (!_msg.success) {
+            ESP_LOGE("IMU", "MPU6050 connection test failed");
+            return false;
+        }
         devStatus = _imu.dmpInitialize();
         if (devStatus == 0) {
-            _imu.setDMPEnabled(false);
+            _imu.setXGyroOffset(0);
-            _imu.setDMPConfig1(0x03);
+            _imu.setYGyroOffset(0);
-            _imu.setDMPEnabled(true);
+            _imu.setZGyroOffset(0);
+            _imu.setXAccelOffset(0);
+            _imu.setYAccelOffset(0);
+            _imu.setZAccelOffset(0);
+
             _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 {
-            return false;
+            ESP_LOGE("IMU", "DMP initialization failed (code %d)", devStatus);
+            _msg.success = 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
-        return true;
+        return _msg.success;
     }
 
     bool update() override {
         if (!_msg.success) return false;
 #if FT_ENABLED(USE_MPU6050)
-        if (_imu.dmpPacketAvailable()) {
+        uint16_t fifoCount = _imu.getFIFOCount();
-            if (_imu.dmpGetCurrentFIFOPacket(fifoBuffer)) {
+        uint8_t intStatus = _imu.getIntStatus();
-                _imu.dmpGetQuaternion(&q, fifoBuffer);
+
-                _imu.dmpGetGravity(&gravity, &q);
+        if (intStatus & 0x10) {
-                _imu.dmpGetYawPitchRoll(_msg.rpy, &q, &gravity);
+            _imu.resetFIFO();
-                return true;
+            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;
         }
         return false;
 #endif
@@ -102,10 +123,30 @@ class IMU : public SensorBase<IMUAnglesMsg> {
 
     float getAngleZ() { return _msg.rpy[0]; }
 
+    bool calibrate() {
+#if FT_ENABLED(USE_MPU6050)
+        if (!_msg.success) return false;
+        ESP_LOGI("IMU", "Starting calibration...");
+        _imu.CalibrateGyro(6);
+        _imu.CalibrateAccel(6);
+        ESP_LOGI("IMU", "Calibration complete");
+        return true;
+#elif FT_ENABLED(USE_BNO055)
+        if (!_msg.success) return false;
+        ESP_LOGI("IMU", "Starting calibration...");
+        adafruit_bno055_offsets_t offsets;
+        bool result = _imu.getSensorOffsets(offsets);
+        ESP_LOGI("IMU", "Calibration complete");
+        return result;
+#else
+        return false;
+#endif
+    }
+
   private:
 #if FT_ENABLED(USE_MPU6050)
     MPU6050 _imu;
-    uint8_t devStatus {false};
+    uint8_t devStatus {0};
     Quaternion q;
     uint8_t fifoBuffer[64];
     VectorFloat gravity;

esp32/include/peripherals/magnetometer.h

@@ -38,37 +38,36 @@ struct MagnetometerMsg : public SensorMessageBase {
 
 class Magnetometer : public SensorBase<MagnetometerMsg> {
   public:
-    bool initialize() {
+    bool initialize() override {
-        msg.success = _mag.begin();
+        _msg.success = _mag.begin();
-        return msg.success;
+        return _msg.success;
     }
 
-    bool update() {
+    bool update() override {
-        if (!msg.success) return false;
+        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[0] = event.magnetic.x;
-        msg.rpy[1] = event.magnetic.y;
+        _msg.rpy[1] = event.magnetic.y;
-        msg.rpy[2] = event.magnetic.z;
+        _msg.rpy[2] = event.magnetic.z;
-        msg.heading = atan2(event.magnetic.y, event.magnetic.x);
+        _msg.heading = atan2(event.magnetic.y, event.magnetic.x);
-        msg.heading += declinationAngle;
+        _msg.heading += declinationAngle;
-        if (msg.heading < 0) msg.heading += 2 * PI;
+        if (_msg.heading < 0) _msg.heading += 2 * PI;
-        if (msg.heading > 2 * PI) msg.heading -= 2 * PI;
+        if (_msg.heading > 2 * PI) _msg.heading -= 2 * PI;
-        msg.heading *= 180 / M_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:
     Adafruit_HMC5883_Unified _mag {12345};
-    MagnetometerMsg msg;
     const float declinationAngle = 0.22;
 };

esp32/include/peripherals/peripherals.h

@@ -74,6 +74,8 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
     float leftDistance();
     float rightDistance();
 
+    bool calibrateIMU();
+
     StatefulHttpEndpoint<PeripheralsConfiguration> endpoint;
 
   private:

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();
+void emitMetrics(Websocket &socket);
 
 const char *resetReason(esp_reset_reason_t reason);
 } // namespace system_service

esp32/scripts/pre_build.py

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

esp32/src/filesystem.cpp

@@ -22,6 +22,20 @@ static Initializer initializer;
 
 esp_err_t getFiles(PsychicRequest *request) { return request->reply(200, "application/json", listFiles("/").c_str()); }
 
+esp_err_t getConfigFile(PsychicRequest *request) {
+    String path = "/config" + request->uri().substring(11);
+    if (!ESP_FS.exists(path)) {
+        return request->reply(404, "text/plain", "File not found");
+    }
+    File file = ESP_FS.open(path, "r");
+    if (!file) {
+        return request->reply(500, "text/plain", "Failed to open file");
+    }
+    String content = file.readString();
+    file.close();
+    return request->reply(200, "application/json", content.c_str());
+}
+
 esp_err_t handleDelete(PsychicRequest *request, JsonVariant &json) {
     if (json.is<JsonObject>()) {
         const char *filename = json["file"].as<const char *>();

esp32/src/main.cpp

@@ -33,17 +33,29 @@ LEDService ledService;
 #if FT_ENABLED(USE_CAMERA)
 Camera::CameraService cameraService;
 #endif
+#if FT_ENABLED(USE_MDNS)
+MDNSService mdnsService;
+#endif
 
 // Service
 WiFiService wifiService;
 APService apService;
 
 void setupServer() {
-    server.config.max_uri_handlers = 10 + WWW_ASSETS_COUNT;
+    server.config.max_uri_handlers = 32 + WWW_ASSETS_COUNT;
     server.maxUploadSize = 1000000; // 1 MB;
     server.listen(80);
-    server.serveStatic("/api/config/", ESP_FS, "/config/");
     server.on("/api/features", feature_service::getFeatures);
+    server.on("/api/system/status", HTTP_GET,
+              [&](PsychicRequest *request) { return system_service::getStatus(request); });
+    server.on("/api/system/reset", HTTP_POST,
+              [&](PsychicRequest *request, JsonVariant &json) { return system_service::handleReset(request); });
+    server.on("/api/system/restart", HTTP_POST,
+              [&](PsychicRequest *request, JsonVariant &json) { return system_service::handleRestart(request); });
+    server.on("/api/system/sleep", HTTP_POST,
+              [&](PsychicRequest *request, JsonVariant &json) { return system_service::handleSleep(request); });
+    server.on("/api/system/metrics", HTTP_GET,
+              [&](PsychicRequest *request) { return system_service::getMetrics(request); });
 #if USE_CAMERA
     server.on("/api/camera/still", HTTP_GET,
               [&](PsychicRequest *request) { return cameraService.cameraStill(request); });
@@ -60,6 +72,56 @@ void setupServer() {
     server.on("/api/servo/config", HTTP_POST, [&](PsychicRequest *request, JsonVariant &json) {
         return servoController.endpoint.handleStateUpdate(request, json);
     });
+
+    // WiFi
+    server.on("/api/wifi/sta/settings", HTTP_GET,
+              [&](PsychicRequest *request) { return wifiService.endpoint.getState(request); });
+    server.on("/api/wifi/sta/settings", HTTP_POST, [&](PsychicRequest *request, JsonVariant &json) {
+        return wifiService.endpoint.handleStateUpdate(request, json);
+    });
+    server.on("/api/wifi/scan", HTTP_GET, [&](PsychicRequest *request) { return wifiService.handleScan(request); });
+    server.on("/api/wifi/networks", HTTP_GET,
+              [&](PsychicRequest *request) { return wifiService.getNetworks(request); });
+    server.on("/api/wifi/sta/status", HTTP_GET,
+              [&](PsychicRequest *request) { return wifiService.getNetworkStatus(request); });
+
+    // AP
+    server.on("/api/ap/status", HTTP_GET, [&](PsychicRequest *request) { return apService.getStatus(request); });
+    server.on("/api/ap/settings", HTTP_GET,
+              [&](PsychicRequest *request) { return apService.endpoint.getState(request); });
+    server.on("/api/ap/settings", HTTP_POST, [&](PsychicRequest *request, JsonVariant &json) {
+        return apService.endpoint.handleStateUpdate(request, json);
+    });
+
+    // Peripherals
+    server.on("/api/peripherals", HTTP_GET,
+              [&](PsychicRequest *request) { return peripherals.endpoint.getState(request); });
+    server.on("/api/peripherals", HTTP_POST, [&](PsychicRequest *request, JsonVariant &json) {
+        return peripherals.endpoint.handleStateUpdate(request, json);
+    });
+
+    // MDNS
+#if FT_ENABLED(USE_MDNS)
+    server.on("/api/mdns", HTTP_GET, [&](PsychicRequest *request) { return mdnsService.endpoint.getState(request); });
+    server.on("/api/mdns", HTTP_POST, [&](PsychicRequest *request, JsonVariant &json) {
+        return mdnsService.endpoint.handleStateUpdate(request, json);
+    });
+    server.on("/api/mdns/status", HTTP_GET, [&](PsychicRequest *request) { return mdnsService.getStatus(request); });
+    server.on("/api/mdns/query", HTTP_POST,
+              [&](PsychicRequest *request, JsonVariant &json) { return mdnsService.queryServices(request, json); });
+#endif
+
+
+    // Filesystem
+    server.on("/api/config/*", HTTP_GET, [](PsychicRequest *request) { return FileSystem::getConfigFile(request); });
+    server.on("/api/files", HTTP_GET, [&](PsychicRequest *request) { return FileSystem::getFiles(request); });
+    server.on("/api/files", HTTP_POST, FileSystem::uploadHandler);
+    server.on("/api/files/delete", HTTP_POST,
+              [&](PsychicRequest *request, JsonVariant &json) { return FileSystem::handleDelete(request, json); });
+    server.on("/api/files/edit", HTTP_POST,
+              [&](PsychicRequest *request, JsonVariant &json) { return FileSystem::handleEdit(request, json); });
+    server.on("/api/files/mkdir", HTTP_POST,
+              [&](PsychicRequest *request, JsonVariant &json) { return FileSystem::mkdir(request, json); });
 #if EMBED_WEBAPP
     mountStaticAssets(server);
 #endif
@@ -82,6 +144,10 @@ void setupServer() {
 #define EVENT_I2C_SCAN "i2cScan"
 #define EVENT_SERVO_CONFIGURATION_SETTINGS "servoPWM"
 #define EVENT_SERVO_STATE "servoState"
+#define EVENT_DISPLACEMENT "displacement"
+#define EVENT_SKILL "skill"
+#define EVENT_SKILL_STATUS "skill_status"
+#define EVENT_IMU_CALIBRATE "imuCalibrate"
 
 void setupEventSocket() {
     // Motion events
@@ -107,11 +173,39 @@ void setupEventSocket() {
         socket.emit(EVENT_I2C_SCAN, results);
     });
 
+    socket.onEvent(EVENT_IMU_CALIBRATE, [&](JsonVariant &root, int originId) {
+        JsonDocument doc;
+        JsonVariant results = doc.to<JsonVariant>();
+        results["success"] = peripherals.calibrateIMU();
+        socket.emit(EVENT_IMU_CALIBRATE, results);
+    });
+
     // Servo controller events
     socket.onEvent(EVENT_SERVO_CONFIGURATION_SETTINGS,
                    [&](JsonVariant &root, int originId) { servoController.servoEvent(root, originId); });
     socket.onEvent(EVENT_SERVO_STATE,
                    [&](JsonVariant &root, int originId) { servoController.stateUpdate(root, originId); });
+
+    // Skill events
+    socket.onEvent(EVENT_DISPLACEMENT,
+                   [&](JsonVariant &root, int originId) { motionService.handleDisplacement(root, originId); });
+
+    socket.onEvent(EVENT_SKILL, [&](JsonVariant &root, int originId) { motionService.handleSkill(root, originId); });
+
+    socket.onEvent(EVENT_SKILL_STATUS, [&](JsonVariant &root, int originId) {
+        JsonDocument doc;
+        JsonVariant results = doc.to<JsonVariant>();
+        motionService.getDisplacementResult(results);
+        socket.emit(EVENT_SKILL_STATUS, results);
+    });
+
+    motionService.setSkillCompleteCallback([&]() {
+        JsonDocument doc;
+        JsonVariant results = doc.to<JsonVariant>();
+        motionService.getDisplacementResult(results);
+        results["event"] = "complete";
+        socket.emit(EVENT_SKILL_STATUS, results);
+    });
 }
 
 void IRAM_ATTR SpotControlLoopEntry(void *) {
@@ -122,6 +216,7 @@ void IRAM_ATTR SpotControlLoopEntry(void *) {
     peripherals.begin();
     servoController.begin();
     motionService.begin();
+    peripherals.calibrateIMU();
 
     for (;;) {
         CALLS_PER_SECOND(SpotControlLoopEntry);
@@ -144,6 +239,10 @@ void IRAM_ATTR serviceLoopEntry(void *) {
     MDNS.setInstanceName(APP_NAME);
     apService.begin();
 
+#if FT_ENABLED(USE_CAMERA)
+    cameraService.begin();
+#endif
+
     setupServer();
 
     socket.begin();
@@ -153,7 +252,21 @@ void IRAM_ATTR serviceLoopEntry(void *) {
     for (;;) {
         wifiService.loop();
         apService.loop();
-        EXECUTE_EVERY_N_MS(2000, system_service::emitMetrics());
+        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(200, {
+            if (motionService.isSkillActive()) {
+                JsonDocument doc;
+                JsonVariant results = doc.to<JsonVariant>();
+                motionService.getDisplacementResult(results);
+                socket.emit(EVENT_SKILL_STATUS, results);
+            }
+        });
 
         vTaskDelay(100 / portTICK_PERIOD_MS);
     }

esp32/src/motion.cpp

@@ -46,6 +46,65 @@ void MotionService::handleMode(JsonVariant &root, int originId) {
     }
 }
 
+void MotionService::handleDisplacement(JsonVariant &root, int originId) {
+    std::string action = root["action"] | "";
+    if (action == "reset") {
+        resetDisplacement();
+        ESP_LOGI("MotionService", "Displacement reset");
+    } else if (action == "clear") {
+        clearSkill();
+        skillWasComplete = false;
+        ESP_LOGI("MotionService", "Skill cleared");
+    }
+}
+
+void MotionService::handleSkill(JsonVariant &root, int originId) {
+    float x = root["x"] | 0.0f;
+    float z = root["z"] | 0.0f;
+    float yaw = root["yaw"] | 0.0f;
+    float speed = root["speed"] | 0.5f;
+
+    setSkillTarget(x, z, yaw);
+    skillWasComplete = false;
+
+    float linear_mag = std::hypot(x, z);
+    bool has_linear = linear_mag > 0.001f;
+    bool has_yaw = std::fabs(yaw) > 0.001f;
+
+    if (has_linear || has_yaw) {
+        if (has_linear) {
+            float norm_x = x / linear_mag;
+            float norm_z = z / linear_mag;
+            command.ly = norm_x;
+            command.lx = -norm_z;
+        } else {
+            command.ly = 0;
+            command.lx = 0;
+        }
+        command.rx = has_yaw ? (yaw > 0 ? 1.0f : -1.0f) : 0;
+        command.s = speed;
+        if (state) state->handleCommand(command);
+    }
+
+    ESP_LOGI("MotionService", "Skill set: Walk(%.3f, %.3f, %.3f) speed=%.2f", x, z, yaw, speed);
+}
+
+void MotionService::checkSkillComplete() {
+    if (!body_state.skill.active) return;
+    if (skillWasComplete) return;
+
+    if (body_state.skill.isComplete()) {
+        skillWasComplete = true;
+        command = {0, 0, 0, 0, command.h, 0, command.s1};
+        if (state) state->handleCommand(command);
+        ESP_LOGI("MotionService", "Skill complete! Traveled: (%.3f, %.3f), Rotated: %.3f", body_state.skill.traveled_x,
+                 body_state.skill.traveled_z, body_state.skill.rotated);
+        if (skillCompleteCallback) {
+            skillCompleteCallback();
+        }
+    }
+}
+
 void MotionService::handleGestures(const gesture_t ges) {
     if (ges != gesture_t::eGestureNone) {
         ESP_LOGI("Motion", "Gesture: %d", ges);
@@ -64,10 +123,12 @@ bool MotionService::update(Peripherals *peripherals) {
     handleGestures(peripherals->takeGesture());
     if (!state) return false;
     int64_t now = esp_timer_get_time();
-    float dt = (now - lastUpdate) / 1000000.0f; // Convert microseconds to seconds
+    float dt = (now - lastUpdate) / 1000000.0f;
     lastUpdate = now;
     state->updateImuOffsets(peripherals->angleY(), peripherals->angleX());
+    ESP_LOGI("MotionService", "IMU Offsets: %.3f, %.3f", peripherals->angleY(), peripherals->angleX());
     state->step(body_state, dt);
+    checkSkillComplete();
     kinematics.calculate_inverse_kinematics(body_state, new_angles);
 
     return update_angles(new_angles, angles);

esp32/src/peripherals/peripherals.cpp

@@ -34,8 +34,8 @@ void Peripherals::begin() {
 };
 
 void Peripherals::update() {
-    readImu();
+    EXECUTE_EVERY_N_MS(20, { readImu(); });
-    readMag();
+    EXECUTE_EVERY_N_MS(100, { readMag(); });
     EXECUTE_EVERY_N_MS(100, { readGesture(); });
     EXECUTE_EVERY_N_MS(500, { readBMP(); });
     EXECUTE_EVERY_N_MS(500, { readSonar(); });
@@ -101,7 +101,7 @@ bool Peripherals::readBMP() {
     bool updated = false;
 #if FT_ENABLED(USE_BMP180)
     beginTransaction();
-    updated = _bmp.readBarometer();
+    updated = _bmp.update();
     endTransaction();
 #endif
     return updated;
@@ -166,13 +166,16 @@ float Peripherals::rightDistance() { return _right_distance; }
 
 void Peripherals::getIMUResult(JsonVariant &root) {
 #if FT_ENABLED(USE_MPU6050 || USE_BNO055)
-    _imu.getResults(root);
+    JsonVariant imu = root["imu"].to<JsonVariant>();
+    _imu.getResults(imu);
 #endif
 #if FT_ENABLED(USE_HMC5883)
-    _mag.getResults(root);
+    JsonVariant mag = root["mag"].to<JsonVariant>();
+    _mag.getResults(mag);
 #endif
 #if FT_ENABLED(USE_BMP180)
-    _bmp.getResults(root);
+    JsonVariant bmp = root["bmp"].to<JsonVariant>();
+    _bmp.getResults(bmp);
 #endif
 }
 
@@ -182,4 +185,15 @@ void Peripherals::getSonarResult(JsonVariant &root) {
     array[0] = _left_distance;
     array[1] = _right_distance;
 #endif
+}
+
+bool Peripherals::calibrateIMU() {
+#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
+    beginTransaction();
+    bool result = _imu.calibrate();
+    endTransaction();
+    return result;
+#else
+    return false;
+#endif
 }

esp32/src/system_service.cpp

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

esp32/src/www_mount.cpp

@@ -23,12 +23,8 @@ void mountStaticAssets(PsychicHttpServer& server) {
         auto* handle = new (&buf[i * sizeof(PsychicWebHandler)]) PsychicWebHandler();
         handle->onRequest([a](PsychicRequest* req) { return web_send(req, *a); });
         server.on(a->uri, HTTP_GET, handle);
-    }
+        if (strcmp(a->uri, WWW_OPT.default_uri) == 0) {
-    for (size_t i = 0; i < WWW_ASSETS_COUNT; i++) {
+            server.defaultEndpoint->setHandler(handle);
-        if (strcmp(WWW_ASSETS[i].uri, WWW_OPT.default_uri) == 0) {
-            server.defaultEndpoint->setHandler(
-                reinterpret_cast<PsychicWebHandler*>(&buf[i * sizeof(PsychicWebHandler)]));
-            break;
         }
     }
 }

simulation/play.py

@@ -15,7 +15,11 @@ from src.controllers import Controller, GUIController, WebSocketController
 
 class SpotMicroSimulation:
     def __init__(
-        self, controller: Controller, env: Optional[QuadrupedEnv] = None, terrain_type: TerrainType = TerrainType.FLAT
+        self,
+        controller: Controller,
+        env: Optional[QuadrupedEnv] = None,
+        terrain_type: TerrainType = TerrainType.FLAT,
+        dt: float = 1.0 / 240,
     ):
         print("Initializing Spot Micro simulation...")
         try:
@@ -23,7 +27,7 @@ class SpotMicroSimulation:
                 self.env = env
                 print("Using existing environment")
             else:
-                self.env = QuadrupedEnv(terrain_type=terrain_type)
+                self.env = QuadrupedEnv(terrain_type=terrain_type, dt=dt)
                 print("Environment created successfully")
 
             print(f"Robot ID: {self.env.robot.robot_id}")
@@ -78,7 +82,7 @@ class SpotMicroSimulation:
         )
 
         self.gait = GaitController(standby)
-        self.dt = 1.0 / 240
+        self.dt = dt
 
     def step(self):
         self.controller.update(self.body_state, self.gait_state, self.dt)
@@ -87,10 +91,24 @@ class SpotMicroSimulation:
         joints = self.kinematics.inverse_kinematics(self.body_state)
         joints = joints * self.joint_directions
 
-        _, _, done, truncated, _ = self.env.step(joints)
+        obs, _, done, truncated, _ = self.env.step(joints)
+
+        self._print_mpu6050_data(obs)
 
         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,22 +38,31 @@ class QuadrupedRobot:
         return [p.getJointInfo(self.robot_id, idx)[1].decode("utf-8") for idx in self.movable_joint_indices]
 
     def get_observation(self):
-        position, orientation = p.getBasePositionAndOrientation(self.robot_id)
+        pos_w, quat_wb = p.getBasePositionAndOrientation(self.robot_id)
-        orientation = p.getEulerFromQuaternion(orientation)
+        v_w, w_w = p.getBaseVelocity(self.robot_id)
-        velocity, angular_velocity = p.getBaseVelocity(self.robot_id)
+
-        joint_states = p.getJointStates(self.robot_id, self.movable_joint_indices)
+        R = np.array(p.getMatrixFromQuaternion(quat_wb), dtype=np.float32).reshape(3, 3)
-        joint_positions = [state[0] for state in joint_states]
+
-        joint_velocities = [state[1] for state in joint_states]
+        if hasattr(self, "prev_velocity") and self.prev_velocity is not None:
-        return np.concatenate(
+            dt = 1.0 / 240.0
-            [
+            accel_world = (v_w - self.prev_velocity) / dt
-                position,
+        else:
-                orientation,
+            accel_world = np.array([0.0, 0.0, 0.0])
-                velocity,
+
-                angular_velocity,
+        accel_body = R.T @ np.asarray(accel_world, dtype=np.float32)
-                joint_positions,
+        gravity_body = R.T @ np.array([0, 0, -9.81], dtype=np.float32)
-                joint_velocities,
+        accel_body += gravity_body
-            ]
+
-        ).astype(np.float32)
+        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)
 
     def apply_action(self, action):
         for i, position in enumerate(action):
@@ -78,6 +87,7 @@ 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":
@@ -85,7 +95,7 @@ class QuadrupedEnv(gym.Env):
         else:
             p.connect(p.DIRECT)
 
-        self.observation_space = gym.spaces.Box(low=-np.inf, high=np.inf, shape=(36,), dtype=np.float32)
+        self.observation_space = gym.spaces.Box(low=-np.inf, high=np.inf, shape=(8,), dtype=np.float32)
         self.action_space = gym.spaces.Box(low=-1, high=1, shape=(12,), dtype=np.float32)
         p.setAdditionalSearchPath(pybullet_data.getDataPath())
 
@@ -93,9 +103,10 @@ 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":
@@ -107,7 +118,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(1 / 240)
+        p.setTimeStep(self.dt)
         if self.render_mode == "human":
             self.gui = GUI(self.robot.robot_id)
         else:
@@ -157,7 +168,7 @@ class QuadrupedEnv(gym.Env):
 
         obs = self.robot.get_observation()
         reward = self.calculate_reward(obs)
-        done = self.is_done(obs)
+        done = self.is_done()
         truncated = self.current_step >= self.max_steps
 
         return obs, reward, done, truncated, {}
@@ -167,48 +178,42 @@ class QuadrupedEnv(gym.Env):
         # p.disconnect()
 
     def calculate_reward(self, obs):
-        position = obs[:3]
+        accel = obs[0:3]
-        orientation = obs[3:6]
+        gyro = obs[3:6]
-        velocity = obs[6:9]
+        heading = obs[6]
-        angular_velocity = obs[9:12]
+        altitude = obs[7]
 
-        forward_velocity = velocity[0]
+        self.estimated_velocity = self.estimated_velocity + self.prev_accel * self.dt
+
+        self.prev_accel = accel.copy()
+
+        forward_velocity = self.estimated_velocity[0]
         velocity_reward = -abs(forward_velocity - self.target_velocity)
 
-        height_penalty = -abs(position[2] - 0.3) * 0.5
+        height_penalty = -abs(altitude - 0.3) * 0.5
 
-        roll, pitch, yaw = orientation
+        orientation_penalty = -(abs(gyro[0]) + abs(gyro[1])) * 0.1
-        orientation_penalty = -(abs(roll) + abs(pitch)) * 1.0
 
-        angular_penalty = -np.sum(np.square(angular_velocity)) * 0.05
+        angular_penalty = -np.sum(np.square(gyro)) * 0.01
 
-        sideways_velocity_penalty = -abs(velocity[1]) * 0.3
+        lateral_acc_penalty = -abs(accel[1]) * 0.01
 
-        if self.prev_velocity is not None:
+        vertical_acc_penalty = -abs(accel[2] + 9.81) * 0.01
-            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, obs):
+    def is_done(self):
-        position = obs[:3]
+        position, orientation = p.getBasePositionAndOrientation(self.robot.robot_id)
-        orientation = obs[3:6]
+        orientation = p.getEulerFromQuaternion(orientation)
+
         return self._is_fallen(orientation) or self._is_distance_limit_exceeded(position)
 
     def _is_distance_limit_exceeded(self, position):
@@ -216,9 +221,4 @@ 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