✨ 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/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/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/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

@@ -14,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 }
@@ -248,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
+}

esp32/include/kinematics.h

@@ -38,7 +38,6 @@ 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_pitch = 15 * DEG2RAD_F;
 
@@ -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/src/main.cpp

@@ -144,6 +144,9 @@ 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() {
@@ -182,6 +185,27 @@ void setupEventSocket() {
                    [&](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 *) {
@@ -235,6 +259,14 @@ void IRAM_ATTR serviceLoopEntry(void *) {
             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);