diff --git a/app/src/lib/components/Visualization.svelte b/app/src/lib/components/Visualization.svelte index bc66811..d93d584 100644 --- a/app/src/lib/components/Visualization.svelte +++ b/app/src/lib/components/Visualization.svelte @@ -2,7 +2,7 @@ import { onDestroy, onMount } from 'svelte'; import { BufferGeometry, Line, LineBasicMaterial, Vector3, type NormalBufferAttributes } from 'three'; import uzip from 'uzip'; - import { model, outControllerData, servoAnglesOut } from '$lib/stores'; + import { kinematicData, model, outControllerData, servoAnglesOut } from '$lib/stores'; import { footColor, isEmbeddedApp, throttler, toeWorldPositions } from '$lib/utilities'; import { fileService } from '$lib/services'; import { servoAngles, mpu, jointNames } from '$lib/stores'; @@ -38,9 +38,8 @@ [-100, -100, -100, 1], ]; - let settings = { - 'Internal kinematic':true, + 'Internal kinematic':false, 'Trace feet':debug, 'Trace points': 30, 'Fix camera on robot': true, @@ -99,18 +98,29 @@ general.add(settings, 'Internal kinematic') const kinematic = gui_panel.addFolder('Kinematics'); - kinematic.add(settings, 'omega', -20, 20) - kinematic.add(settings, 'phi', -30, 30) - kinematic.add(settings, 'psi', -20, 15) - kinematic.add(settings, 'x', -90, 90) - kinematic.add(settings, 'y', 0, 200) - kinematic.add(settings, 'z', -130, 130) + kinematic.add(settings, 'omega', -20, 20).onChange(updateKinematicPosition) + kinematic.add(settings, 'phi', -30, 30).onChange(updateKinematicPosition) + kinematic.add(settings, 'psi', -20, 15).onChange(updateKinematicPosition) + kinematic.add(settings, 'x', -90, 90).onChange(updateKinematicPosition) + kinematic.add(settings, 'y', 0, 200).onChange(updateKinematicPosition) + kinematic.add(settings, 'z', -130, 130).onChange(updateKinematicPosition) const visibility = gui_panel.addFolder('Visualization'); visibility.add(settings, 'Trace feet') visibility.add(settings, 'Trace points', 1, 1000, 1) } + const updateKinematicPosition = () => { + kinematicData.set([ + settings.omega, + settings.phi, + settings.psi, + settings.x, + settings.y, + settings.z + ]) + } + const cacheModelFiles = async () => { let data = await fetch('/stl.zip').then((data) => data.arrayBuffer()); @@ -201,7 +211,11 @@ if (settings['Internal kinematic']) calculate_kinematics() robot.position.y = robot.position.y - Math.min(...toes.map(toe => toe.y)); - robot.rotation.z = lerp(robot.rotation.z, degToRad($mpu.heading + 90), 0.1); + robot.position.z = lerp(robot.position.z, -settings.x / 100, 0.1); + + robot.rotation.z = lerp(robot.rotation.z, degToRad(settings.phi + $mpu.heading + 90), 0.1); + robot.rotation.y = lerp(robot.rotation.y, degToRad(settings.omega - settings.z / 2.5), 0.1); + robot.rotation.x = lerp(robot.rotation.x, degToRad(settings.psi - 90), 0.1); for (let i = 0; i < $jointNames.length; i++) { currentModelAngles[i] = lerp( diff --git a/app/src/lib/stores/model-store.ts b/app/src/lib/stores/model-store.ts index 362e593..74e90de 100644 --- a/app/src/lib/stores/model-store.ts +++ b/app/src/lib/stores/model-store.ts @@ -23,6 +23,8 @@ export const mode: Writable = writable(ModesEnum.Idle); export const outControllerData = writable([0, 0, 0, 0, 0, 70, 0]); +export const kinematicData = writable([0, 0, 0, 0, 100, 0]); + export const input: Writable = writable({ left: { x: 0, y: 0 }, right: { x: 0, y: 0 }, diff --git a/app/src/routes/+layout.svelte b/app/src/routes/+layout.svelte index bece194..c11d8e3 100644 --- a/app/src/routes/+layout.svelte +++ b/app/src/routes/+layout.svelte @@ -13,7 +13,7 @@ import Menu from './menu.svelte'; import Statusbar from './statusbar.svelte'; import Login from './login.svelte'; - import { ModesEnum, mode, outControllerData, servoAngles, servoAnglesOut, socket } from '$lib/stores'; + import { ModesEnum, kinematicData, mode, outControllerData, servoAngles, servoAnglesOut, socket } from '$lib/stores'; import type { Analytics, Battery, DownloadOTA } from '$lib/types/models'; import { api } from '$lib/api'; @@ -29,6 +29,7 @@ outControllerData.subscribe((data) => socket.sendEvent("input", {data})); mode.subscribe((data) => socket.sendEvent("mode", {data})); servoAnglesOut.subscribe((data) => socket.sendEvent("angles", {data})); + kinematicData.subscribe((data) => socket.sendEvent("position", {data})); }); onDestroy(() => { diff --git a/esp32/lib/ESP32-sveltekit/MotionService.h b/esp32/lib/ESP32-sveltekit/MotionService.h index 900dcaf..096704b 100644 --- a/esp32/lib/ESP32-sveltekit/MotionService.h +++ b/esp32/lib/ESP32-sveltekit/MotionService.h @@ -9,6 +9,7 @@ #define LIGHT_SETTINGS_ENDPOINT_PATH "/api/input" #define ANGLES_EVENT "angles" #define INPUT_EVENT "input" +#define POSITION_EVENT "position" #define MODE_EVENT "mode" enum class MOTION_STATE @@ -32,12 +33,16 @@ class MotionService _socket->registerEvent(INPUT_EVENT); _socket->registerEvent(ANGLES_EVENT); _socket->registerEvent(MODE_EVENT); + _socket->registerEvent(POSITION_EVENT); _socket->onEvent(INPUT_EVENT, [&](JsonObject &root, int originId) { handleInput(root, originId); }); _socket->onEvent(MODE_EVENT, [&](JsonObject &root, int originId) { handleMode(root, originId); }); _socket->onEvent(ANGLES_EVENT, [&](JsonObject &root, int originId) { anglesEvent(root, originId); }); + + _socket->onEvent(POSITION_EVENT, [&](JsonObject &root, int originId) { positionEvent(root, originId); }); + _socket->onSubscribe(ANGLES_EVENT, std::bind(&MotionService::syncAngles, this, std::placeholders::_1, std::placeholders::_2)); } @@ -52,6 +57,18 @@ class MotionService syncAngles(String(originId)); } + void positionEvent(JsonObject &root, int originId) + { + JsonArray array = root["data"].as(); + position.omega = array[0]; + position.phi = array[1]; + position.psi = array[2]; + position.xm = array[3]; + position.ym = array[4]; + position.zm = array[5]; + // syncAngles(String(originId)); + } + void handleInput(JsonObject &root, int originId) { JsonArray array = root["data"].as(); @@ -59,6 +76,20 @@ class MotionService { input[i] = array[i]; } + float lx = input[1]; + float ly = input[2]; + float rx = input[3]; + float ry = input[4]; + float h = input[5]; + float s = input[6]; + position = { + 0, + rx / 4, + ry / 4, + ly / 2, + (h + 128) * (float)0.7, + lx / 2 + }; ESP_LOGI("MotionService", "Input: %.0f %.0f %.0f %.0f %.0f %.0f %.0f", input[0], input[1], input[2], input[3], input[4], input[5], input[6]); } @@ -76,7 +107,6 @@ class MotionService sprintf(output, "[%0.f,%0.f,%0.f,%0.f,%0.f,%0.f,%0.f,%0.f,%0.f,%0.f,%0.f,%0.f]", angles[0], angles[1], angles[2], angles[3], angles[4], angles[5], angles[6], angles[7], angles[8], angles[9], angles[10], angles[11]); _socket->emit(ANGLES_EVENT, output, String(originId).c_str()); - } float lerp(float start, float end, float t) { @@ -99,31 +129,8 @@ class MotionService break; case MOTION_STATE::STAND: { - - float lp[4][4] = { - { 100, -100, 100, 1}, - { 100, -100, -100, 1}, - {-100, -100, 100, 1}, - {-100, -100, -100, 1} - }; - float lx = input[1]; - float ly = input[2]; - float rx = input[3]; - float ry = input[4]; - float h = input[5]; - float s = input[6]; - position_t p = { - 0, - rx / 4, - ry / 4, - ly / 2, - (h+128)*0.7, - lx / 2 - }; float new_angles[12] = {0,}; - float dir[12] = {-1, -1, -1, 1, -1, -1, -1, -1, -1, 1, -1, -1}; - - kinematics.calculate_inverse_kinematics(lp, p, new_angles); + kinematics.calculate_inverse_kinematics(lp, position, new_angles); for (int i = 0; i < 12; i++) { float new_angle = lerp(angles[i], new_angles[i] * dir[i], 0.3); @@ -138,10 +145,20 @@ class MotionService break; } case MOTION_STATE::WALK: - angles[1] += dir; - if (angles[1] >= 90) dir = -1; - if (angles[1] <= 0) dir = 1; - updated = true; + float new_angles[12] = {0,}; + lp[0][1] += walk_dir; + if (lp[0][1] >= 200) walk_dir = -1; + if (lp[0][1] <= 100) walk_dir = 1; + + kinematics.calculate_inverse_kinematics(lp, position, new_angles); + + for (int i = 0; i < 12; i++) { + float new_angle = lerp(angles[i], new_angles[i] * dir[i], 0.3); + if (new_angle != angles[i]) { + angles[i] = new_angle; + updated = true; + } + } break; } return updated; @@ -163,13 +180,22 @@ class MotionService constexpr static int MotionInterval = 100; + position_t position = {0, 0, 0, 0, 0, 0}; + float dir[12] = {-1, -1, -1, 1, -1, -1, -1, -1, -1, 1, -1, -1}; + float lp[4][4] = { + { 100, -100, 100, 1}, + { 100, -100, -100, 1}, + {-100, -100, 100, 1}, + {-100, -100, -100, 1} + }; + float input[7] = {0, 0, 0, 0, 0, 0, 0}; float angles[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; float rest_angles[12] = {0, 90, -145, 0, 90, -145, 0, 90, -145, 0, 90, -145}; float stand_angles[12] = {0, 45, -90, 0, 45, -90, 0, 45, -90, 0, 45, -90}; MOTION_STATE motionState = MOTION_STATE::IDLE; unsigned long _lastUpdate; - int dir = 2; + int walk_dir = 2; }; #endif