diff --git a/app/src/lib/components/SkillPanel.svelte b/app/src/lib/components/SkillPanel.svelte new file mode 100644 index 0000000..3b27da7 --- /dev/null +++ b/app/src/lib/components/SkillPanel.svelte @@ -0,0 +1,156 @@ + + +
+
+

+ Skill Control + + {$isActive ? 'Active' : 'Idle'} + +

+ +
+
+
Position
+
+ {formatMeters($status.x)}, {formatMeters($status.z)} +
+
Yaw: {formatDegrees($status.yaw)}
+
+
+
Distance
+
{formatMeters($status.distance)}
+
Total traveled
+
+
+ + {#if $isActive} +
+
+ Progress + {($progress * 100).toFixed(0)}% +
+ +
+ Target: ({$status.skill_target_x.toFixed(2)}, {$status.skill_target_z.toFixed(2)}, {formatDegrees( + $status.skill_target_yaw + )}) +
+
+ {/if} + +
Presets
+ +
+ {#each presets as preset} + + {/each} +
+ +
Custom
+ +
+
+ + +
+
+ + +
+
+ + +
+
+ +
+ + +
+ +
+
+ +
+
+ + +
+
+
+
diff --git a/app/src/lib/stores/application.ts b/app/src/lib/stores/application.ts index d381b80..e64ab5b 100644 --- a/app/src/lib/stores/application.ts +++ b/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' } + ] + } } ] diff --git a/app/src/lib/stores/index.ts b/app/src/lib/stores/index.ts index 8ab16e5..9e16548 100644 --- a/app/src/lib/stores/index.ts +++ b/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' diff --git a/app/src/lib/stores/skill.ts b/app/src/lib/stores/skill.ts new file mode 100644 index 0000000..05d32b0 --- /dev/null +++ b/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(defaultStatus) + const history = writable([]) + let unsubscribe: (() => void) | null = null + + function init() { + if (unsubscribe) return + unsubscribe = socket.on(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() + diff --git a/app/src/lib/types/models.ts b/app/src/lib/types/models.ts index ceb4785..403f3af 100644 --- a/app/src/lib/types/models.ts +++ b/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 +} diff --git a/esp32/include/kinematics.h b/esp32/include/kinematics.h index c3f032d..4c46c55 100644 --- a/esp32/include/kinematics.h +++ b/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) || diff --git a/esp32/include/motion.h b/esp32/include/motion.h index 3cf2c31..d37d8aa 100644 --- a/esp32/include/motion.h +++ b/esp32/include/motion.h @@ -3,6 +3,7 @@ #include #include "esp_timer.h" +#include #include #include @@ -18,6 +19,8 @@ enum class MOTION_STATE { DEACTIVATED, IDLE, CALIBRATION, REST, STAND, WALK }; +using SkillCompleteCallback = std::function; + 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 diff --git a/esp32/include/motion_states/walk_state.h b/esp32/include/motion_states/walk_state.h index 86e8d59..2d20f46 100644 --- a/esp32/include/motion_states/walk_state.h +++ b/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; diff --git a/esp32/src/main.cpp b/esp32/src/main.cpp index 0976fa3..5e69cd4 100644 --- a/esp32/src/main.cpp +++ b/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(); + motionService.getDisplacementResult(results); + socket.emit(EVENT_SKILL_STATUS, results); + }); + + motionService.setSkillCompleteCallback([&]() { + JsonDocument doc; + JsonVariant results = doc.to(); + 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(); + motionService.getDisplacementResult(results); + socket.emit(EVENT_SKILL_STATUS, results); + } + }); vTaskDelay(100 / portTICK_PERIOD_MS); } diff --git a/esp32/src/motion.cpp b/esp32/src/motion.cpp index dfef554..04a635f 100644 --- a/esp32/src/motion.cpp +++ b/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);